Multiple Vehicle Team Tasking for Cooperative Estimation Jarurat Ousingsawat and Mark E. Campbell Sibley School of Mechanical and Aerospace Engineering Cornell University, Ithaca, NY 14853
[email protected],
[email protected]
Abstract
the possibility of using fewer sensors or lower quality, less expensive sensors. This results in better and more flexible reconnaissance. Cooperative estimation, while potentially beneficial, also depends significantly on observer trajectories. Without appropriate paths (and therefore task assignments), cooperation can waste computation and bandwidth. It is important to understand the interactions between path planning and cooperative estimation. In this work, the environment consists of multiple vehicles, multiple goals/targets, and ellipsoidal obstacles. Two important components of cooperative reconnaissance are:
A methodology for cooperating multiple vehicles in a reconnaissance problem is proposed. A team of vehicles is used in surveying multiple targets potentially at different priority levels. The overall objective is to minimize the uncertainty on locations of targets using cooperative estimation in minimum time. Because the performance of the estimation highly depends on trajectories of observers, optimal path planning trajectories are naturally integrated in the problem. Uncertainties include planning in the presence of ellipsoidal obstacles. The relationship between locations of vehicles and estimator performance is investigated. A fast algorithm for team selection is proposed to make it applicable for an on-line application.
1
• Multiple task assignment: the assignment of goals to vehicles has a direct effect on system performance. The trajectories used to complete the assignment also play a major role in the problem.
Introduction
Research on cooperating multiple vehicles has been widely explored in recent years ([2],[3],[7]). By using all resources through cooperation among vehicles in a team, the overall performance can be improved. Multiple vehicle usage is applicable for a variety tasks, i.e. survey/mapping in an unknown environment, battlefield attack, and defense. Cooperative control of multiple vehicle systems can be a challenging task because the size and numerical complexity of the problem become significantly larger as a number of vehicles grows. In Ref. [2], the cooperative control of multiple UAVs performing series of tasks on targets is investigated. The environment assumes no obstacles. A network flow and auction algorithm is used in the multiple assignment problem. In Ref. [7], the task assignment for multiple vehicles in the presence of polygon obstacles is explored. This work focuses on cooperative reconnaissance. The information from different vehicles is combined through cooperative estimation. Not only can the estimator performance be improved, it also allows
• Cooperative estimation: it is used to indicate locations of vehicles where the team gains the most information. The approach proposed here separates the assignment problem into two sub-problems: optimal trajectory primitives and task assignment. This allows the algorithm to work much faster than solving the whole problem at once. This paper investigates the relationship between locations of observers and estimator performance using an information approach. The knowledge of multiple task assignment and cooperative estimation are then integrated into the cooperative reconnaissance problem. The paper is organized as follow. First, multiple task assignment using optimal trajectory primitives is presented. Then cooperative estimation is investigated. Finally, the cooperative reconnaissance problem is set up and solved. Simulation results are presented. 1
2
Multiple Task Assignment using Optimal Trajectory Primitives
provided by an estimator in the form of an ellipsoid (see Ref. [8]). The problem with polygon obstacles, which is a simpler problem, is discussed at the end of the section. The shortest path between two points, pa and pb , is determined using a graph search technique. The A* search [1] is chosen for its computational efficiency. In A* search, the cost function consists of a function to be minimized and a heuristics function. The heuristics function is used to evaluate a node if it is worthwhile to expand. This allows the search to terminate a path once the algorithm realizes that it has no possibility of becoming the optimal path. With an appropriate heuristics function, the optimality can still be guaranteed. Each path Pk is constructed using breadth first search. Path Pk consists of points pk (1 · · · NPk ), where NPk is a number of nodes in Pk . Therefore pk (1) is always pa . The cost function is defined as:
Given a set of goals, a team of vehicles is given a task to visit all goals in the set with presence of ellipsoidal obstacles in the environment as shown in Figure 1. The system consists of Nw goals, Nv vehicles and Nobs obstacles. Each vehicle assumes to be equipped with radar that is used to detect obstacles/targets. The vehicles move at the maximum speed, unless are instructed by the planner.
goal A
Vehicle 1 Vehicle 2 Vehicle 3
goal B
JA∗ (Pk ) = D(k) + DH (k)
goal C goal D
(1)
N Pk
where D(k) =
X
kpk (j) − pk (j − 1)k
j=2
Figure 1: Multiple Vehicle Task Assignment Scenario
DH (k) = kpk (NPk ) − pb k D is the path distance of Pk , and DH is the heuristic function which is the minimum predicted distance to point pb (no obstacles). Unlike polygon obstacles, ellipsoidal obstacles are not represented by a finite number of points, which makes it more difficult. While ellipsoids can be approximated using a large number of polyhedral around an ellipse, the computation slows immensely in higher dimensional problems. This paper proposes using a tangent search technique in order to define nodes and edges. It is obvious that the shortest path from one point to another around an ellipse is the one through tangent lines between the ellipse and points (see also Ref. [6]). Using this fact, nodes and edges are defined dynamically based on a current position. Unless there is a direct path to a goal, next available nodes are as shown in Figure 2. Figure 2(a) shows an example where next nodes from a point along a possible shortest path around an ellipse are simply two tangent points on the ellipse. However, if a current point is on an ellipse as shown in Figure 2(b), next available nodes to another ellipse become tangent points between two ellipses. The other technique is used to reduce unnecessary search. Because the shortest path with no obstacles is obviously a straight line, searching
The objective is to visit all goals in minimum time. Each vehicle must be assigned a set of goals that it has to visit along with the trajectory that provides the minimum-time. There are two major issues to be addressed in this problem: 1)minimumtime trajectory problem, 2) task assignment problem. Instead of solving both problems at the same time which is computationally expensive, they are solved separately. Firstly, the minimum-time trajectory primitives are solved from each vehicle’s starting point to every goal and from each goal to other goals. Secondly, all the possible combinations are listed. Finally, the task assignment problem is put in a Mixed Integer Linear Programming (MILP) form and solved for the optimal solution.
2.1
Minimum-Time Trajectory Primitives in the Presence of Ellipsoidal Obstacles
Because vehicles travel at the maximum speed, the minimum-time problem is equivalent to shortest path problem. The ellipses are considered as obstacles because tracking information on an obstacle, especially in a dynamic environment, is often 2
Current node
A
Next available nodes
T1
original path
(a)
(b)
T2 C
Figure 2: Next Available Nodes around an Ellipse
Figure 3: Path with Turning Radius Restriction
B and the bisector point of line AC.
through obstacles that are not in the way of this direct path is unnecessary. The technique works recursively. First, the straight line path is considered, then new nodes are defined on the last obstacle that collides with the direct path. The direct paths to those nodes are processed as if the nodes are new destination. The process is iterated until the direct paths to nodes do not collide. If obstacles are described as polygons, the problem becomes even easier to solve. Instead of using the tangent view technique to find next available nodes, the extreme points of obstacles that gives collision-free edges become ones.
2.2
B
Cr x
new path
Cr
=
Pm
=
α1 Pm − (1 − α1 ) PB 1 (PA + α2 PC + (1 − α2 ) PB ) 2
where PA , PB , PC are coordinates of A, B, C and α1 =
PA − P B r , α2 = kPB − Cr k kPB − PC k
The new path is then constructed from point A to the tangent point T1 , around and leave the circle at the tangent point T2 to point C. The new optimal path from A → T1 and T2 → C is computed if necessary (if collision occurs).
Listing Permutations 2.3
All possible trajectories (star to goal, goal to goal) for each vehicle are listed in this step. Each trajectory is called “permutation”. For Nw goals and Nv vehicles. A number of permutations (Np ) is: Np
=
Nv
ÃN
w −Nv +1
X
k=1
µ
Nw k
¶!
Solving for the Optimal Solution
In the final step, the permutations that completes the mission with the minimum team time are chosen. The cost function is the total team time, which is the maximum time among all vehicles. The problem is in a Mixed Integer Linear Programming form as shown in Ref.[7]. The difference here is that the MILP problem solved involves only the task assignment, not the path itself which is constructed from the primitives. Thus, the approach is quite fast. The decision variable is X, a Np × 1 vector. The element Xi is either 1, if the ith permutation is chosen, or 0, otherwise. The other decision variable is T which is the final team time.
(2)
Notice that Np significantly grows as numbers of goals and vehicles increase. For the ith permutation, variables are assigned as follow: • ui : vehicle corresponding to the permutation. • vi : Nw × 1 vector. The k th element is either 1, if the permutation visits the goal k th , or 0, otherwise.
min T,X
subject to
• ci : Cost corresponding to the permutation
J =T +²
c i Xi
i=1
Xi ∈ {0, 1} Np X
The cost ci is the time to complete the permutation. Due to vehicle’s turning radius restriction (r), paths must be put together under the constraint. Figure 3 illustrates gluing of the path AB and BC. In order to obtain the new path, the circle of radius r is drawn such that point B is on the circle. The center, Cr , is on the line that connects point
Np X
c i Xi ≤ T
i=1
Np X
Vij = 1
∀i = 1 . . . Nw
Xj = 1
∀i = 1 . . . Nv
j=1
X
j∈Pi
3
(3)
Consider a pair of the ith vehicle and the pth in a target tracking problem with a full radar measurement. The radar measurement is zip (k) = {Rip (k) θRi p (k)} + vi . Rip (k) is a range measurement and θR,ip (k) is an azimuth angle. Both are measured relative to the observer. The measurement noise, vi , is bounded by an ellipsoid:
© ª where Pi = {k | uk = i} and V = v1 · · · vNp
2.4
Simulation Result
This section shows a result for the multiple vehicle task assignment problem as described in the previous section. The Figure 4 shows the optimal solution for the example in Figure 1.
viT
2 σR,i 0
0 σθ2R ,i
¸−1
vi ≤ 1
FIM of the ith vehicle observing the pth target at time step k is shown in Equation 5. In cooperative estimation, Nv vehicles observe the same target. The combined information for the pth target at time step k, Icp (k) , is simply the determinant of the summation of FIMs. ¯N ¯ v ¯X ¯ ¯ ¯ Icp (k) = ¯ F IMip (k)¯ (6) ¯ ¯
Figure 4: Multiple Vehicle Assignment Result
3
·
i=1
The composite information for the overall system is defined as the summation of the combined information of each target.
Cooperative Estimation
Ntarget
Icomp (k) =
The objective of cooperative estimation is to use information from two or more vehicles to improve tracking information. Each vehicle estimates and shares the result through location estimates and uncertainty ellipsoids. The intersection of two ellipsoids is then determined in order to produce a better result. Details of cooperative estimation using Set Membership Filter can be found in Ref. [4]. Cooperative estimation, while intuitively appealing, is highly reliant on the chosen vehicle trajectories. For example, two vehicles very close to each other would receive similar information, producing uncertainty ellipsoids that are very similar in shape and orientation. Intersection of these ellipsoids would not produce much better (smaller volume) result. It is proposed here to use the Fisher Information Matrix (FIM) as a guide to choosing goal locations for the cooperative team. In Ref. [5], Oshman determines the optimal observer trajectory that maximizes determinant of FIM and minimizes threat. The determinant of FIM represents amount of information from the measurement that is inverselyproportional to uncertainty. For a discrete system with the batch measurement up to time step k, Z k , and state x, FIM is defined as: £ ¤ (4) F IM = E ∇x ln p(Z k |x) · ∇Tx ln p(Z k |x )
X
Ici (k)
(7)
i=1
Based on what mentioned earlier, the volume and shape of uncertainty ellipsoids depend on locations of observers. Because of the nature of radar measurement, uncertainty in Line Of Sight (LOS) direction is smaller compare to others while uncertainty in the direction perpendicular to LOS produces is the largest. It can be seen that the intersecting ellipse of two ellipses has smallest volume if major axes are perpendicular. It is found in Ref. [4] that the volume of the intersecting ellipsoid (VI ) is as following: p λ1,min λ2,min (8) VI = 2π sin(∆β) where λ1,2 are maximum eigenvalues (length of major axes) of two original ellipses and ∆β is difference in orientation between ellipses. Note that although the minimum volume occurs where ∆β = 90◦ , it can be seen in Figure 5 that as the angle approaches 90◦ , the volume also approaches the minimum. This allows the algorithm to be more flexible on locations of an observer pair as they try to triangulate. Figure 6 shows the composite information map of two vehicles and two targets. The prohibited 4
F IMip (k) =
k X j=1
1 2 σR,i
1 2 σR,i
∆x2ip (j) 2 (j) Rip
∆xip (j)∆yip (j) 2 (j) Rip
+
1 σθ2
R ,i
−
1 σθ2
R ,i
2 ∆yip (j) 4 (j) Rip
1 2 σR,i
∆xip (j)∆yip (j) 4 (j) Rip
∆xip (j)∆yip (j) 2 (j) Rip 2 1 ∆yip (j) 2 2 (j) σR,i Rip
−
1 σθ2
R ,i
+
1 σθ2
R ,i
∆xip (j)∆yip (j) 4 (j) Rip ∆x2ip (j) 4 (j) Rip
th where (xi (j), yi (j)), (xp (j), yp (j)) are locations of the ith vehicle and the p p target at time step j. ∆xip (j) = xi (j) − xp (j), ∆yip (j) = yi (j) − yp (j), and Rip (j) = ∆xip (j)2 + ∆yip (j)2
4 V I
0
10
20
30
40
50
60
70
80
Cooperative sance
(5)
Reconnais-
Cooperative reconnaissance is the problem where cooperative estimation and multiple vehicle tasking are integrated. A team of vehicles is sent out to survey a group of targets. Each target has a zone around it that is prohibited for vehicles to be in as shown in Figure 7.
90
Db (degree)
Figure 5: Volume of Intersecting Ellipsoid y finish obstacle
zone is defined around each target. The location of the first vehicle is fixed. The figure shows the information that the team gains in the function of the location of the other vehicle. It can be seen that the more information is gained significantly by moving closer to the targets. This shows the effect of locations of observers on the information.
prohibited zone
x
start
Figure 7: Reconnaissance Scenario
This problem involves many sub-problems: task assignment, optimal primitives, cooperative estimation. The longer observers travel, the more information they can gather. Therefore the optimal path must represent a trade-off between time used to visit all targets and information gathered on those targets.
I
comp
8000
10
6000 4000
5
vehicle 1 ( x1, x2)
2000 0
0
10
y2
J = T + PN T
5
i=1 Icomp (i)
−5
0
x2
−5 −10
α
where Icomp is defined in Equation 7. T is the final team time and NT is the time step at T . α is a weighting factor of uncertainty in the cost function. Solving the full optimization problem can be an extreme computational burden. The approach shown in section 2 is applied instead. It can be seen from Equation 5 and Figure 6 that the more
−10
Figure 6: Composite Information Map
5
information gains as the vehicles move closer the targets. Therefore, the circle defining prohibited zone around each target is discretized. Waypoints are placed around each prohibited zone. In order to investigate the effect of locations of observers on the prohibited zone, a simple example with one target and two vehicles as shown in Figure 8 is considered. The normalized composite ³ ´ I information comp is calculated for different ∆β. T The result from Figure 9 shows that the normalized information approaches the maximum as ∆β is close to 90◦ . Note that it is consistent with the result from Equation 8.
cles travel at the maximum speed. The true speed of the vehicle that travels shorter distance between the pair can then be calculated such that both vehicles arrive at the boundary simultaneously.
5
Simulation Results
The approach described earlier is applied on problems. Four waypoints are place around each prohibited zone. It shows in Figure 10 and 12 that vehicles try to triangulate on each target before moving to the next one. A pair of waypoints on each prohibited zone and the order of target are chosen so that the overall team time is minimized. The Figure 11 and 13 show the increasing in Information over time. Notice that the sharply increasing of the composite information happens when the vehicles are near a target.
Waypoint target
Db
vehicle 1 vehicle 2
y
Figure 8: An One Target Example
1.5
x 105
1.4
Icomp T
1.3
x
1.2 1.1 1 20
30
40
50
60 Db (degree)
70
80
Figure 10: Result of a Reconnaissance Scenario shown in Figure 7
90
Figure 9: The relation between Normalized Composite Information and ∆β
2.5
x 106
2
Using this result, pairs of waypoints on each zone that are approximately 90◦ apart are selected and considered as one single point The problem is then solved similarly as previously described in Section 2. However, each permutation involves a pair of vehicles instead of a single vehicle and the pair(s) of waypoints instead of a set of goals because of the cooperation. Another issue to be addressed is that both vehicles must reach the boundary of the prohibited zone simultaneously. Because the cost is defined by the final completion time, the problem can be solved by initially assuming that all vehi-
1.5
Icomp 1 0.5 0 0
10
20
Time
30
40
50
Figure 11: Composite Information over Time from Figure 10
6
y
AFRL at Wright Patterson AFB, with Dr. Helen Gill as the DARPA program manager.
References [1] Nilsson NJ, “Artificial Intelligence: A New Synthesis ”, Morgan Kaufmann, 1998 [2] Chandler PR, et.al. “Complexity in UAV Cooperative Control”, American Control Conference, Anchorage, AK, 2002 x
[3] Hajjawi MK, Shirkhodaie A, “Cooperative Visual Team Working and Target Tracking of Mobile Robots”, System Theory., March, 2002
Figure 12: Example of Cooperative Reconnaissance 2.5
[4] Ousingsawat J and Campbell ME, “On-line Estimation and Path Planning for Multiple Vehicles in an Uncertain Environment”, submitted to International Journal of Robust Nonlinear Control , 2003
x 106
2 1.5
Icomp
[5] Oshman Y, Davidson P, “Optimization of Observer Trajectories for Bearings-Only Target Localization”, IEEE Transactions on Aerospace and Electronics Systems, Vol.35 No.3 July 1999 pp. 892–902
1 0.5 0 0
10
20
30
Time
40
50
60
[6] Isaacs R, “Differential Games”, John Wiley and Sons, 1965
Figure 13: Composite Information over Time from Figure 12
6
[7] Richards A, Bellingham J, Tillerson M, and How J, “Coordination and Control of Multiple UAVs”, Guidance Navigation and Control Conference, August 2002
Conclusion
A methodology for cooperating multiple vehicles is developed. First, it is used in the task assignment of multiple vehicles. The task assignment problem is set up to use optimal trajectory primitives in the presence of ellipsoidal obstacles. The primitives are determined using A* search and glued together under vehicle’s turning constraint. Then the assignment problem is set up as a MILP problem. The same technique is used in the reconnaissance problem. By discritizing around prohibited zones, the same approach can be applied similarly. The waypoints are chosen such that the performance of cooperative estimation is maximized. The simulation results show trajectories of vehicles as they try to gather information on targets.
7
[8] Scholte E and Campbell ME, “On-line Nonlinear Guaranteed Estimation with Application to a High Performance Aircraft”, American Control Conference, 2002
Acknowledgments
This work was supported under a DARPA ITO Grant (Contract # F33615-99-C-3612), through 7
