Implementation of Deconfliction in Multivehicle Autonomous Systems

IMPLEMENTATION OF DECONFLICTION IN MULTIVEHICLE AUTONOMOUS SYSTEMS

A. P. Melander*, N. D. Powel*, E. Lalish*, K. A. Morgansen*, J. S. Jang** and J. Vian** *Department of Aeronautics and Astronautics, University of Washington, Seattle, WA, 98195-2400; **The Boeing Company, Seattle, WA, 98124

Keywords: Deconﬂiction, multivehicle control, autonomous systems

Abstract be guaranteed for them to be accepted by their manned counterparts. The focus of the work in this paper is the im- Many conflict resolution strategies have been plementation of a distributed deconfliction algo- proposed using varying degrees of automation. rithm for vehicles with constraints on velocity Some of these strategies are designed specifically and acceleration. The algorithm was tested on for Air Traffic Control (ATC) applications, while two testbeds: a quadrotor system and an under- others are more general in nature. An overview water vehicle system. The quadrotor system has and classification of many collision avoidance al- vehicles that can hover in place and move in arbi- gorithms can be found in [8]. The Distributed trary directions, while the vehicles in the under- Reactive Collision Avoidance (DRCA) algorithm water system move with a constant forward ve- developed in [9] is a conflict resolution scheme locity and can only change direction by changing guaranteeing collision avoidance, either in the heading. Results are demonstrated for a variety plane or in 3D, for an arbitrary number, n, of of numbers of vehicles as well as types of colli- nonholonomic vehicles. The DRCA algorithm is sion directions. designed to work with vehicles that have limited control authority and complex dynamics (such as 1 Introduction aircraft, which have low acceleration compared to speed and must bank to turn). According to the As multi-vehicle autonomous systems are stud- collision avoidance classification in [8], DRCA ied and implemented, the issue of conflict res- is a nominal (projects current states into the fu- olution becomes increasingly important. From ture along a single trajectory), horizontal plane, mobile robots performing a cooperative search global algorithm that combines turning and speed to air traffic control for unmanned aerial vehi- changes and detects conflict. It is most closely cles, collision avoidance is of utmost importance associated with a force field approach, although for safety. Much of the work so far on colli- it does not strictly meet this definition because sion avoidance has been sponsored by the FAA DRCA does not treat vehicles as charged parti- to support a potential move to free-flight air traf- cles. fic control [13], [7], whereby aircraft can avoid An imperative feature for any avoidance algo- each other in a decentralized manner rather than rithm used for automating air or shipping traffic relying on a land-based controller. Similar con- must be a guarantee of collision avoidance. For a cepts have been discussed regarding autonomous collision avoidance guarantee to be valid for real harbor control for ships [11], [6]. These scenarios vehicles, it must also restrict the maximum ac- will become more important as unmanned vehi- celeration. Any collision avoidance scheme that cles are introduced, because safety will need to does not meet this criteria can be ruled out for

1 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN** such applications. Many of the approaches pre- tors, hats over unit-vectors, script capital letters viously proposed guarantee avoidance, but only for sets, standard capital letters for matrices and for a limited number of vehicles [15], [2]. Be- functions, and everything else is assumed scalar. cause traffic in the air and on the sea is rapidly Quantities subscripted with t, n, or b refer to the increasing, a higher likelihood exists for a colli- tangent, normal, or binormal direction, respec- sion (or at least a conflict) involving multiple ve- tively. hicles. The DRCA algorithm is ideal for such ap- Each vehicle has a nominal desired control plications because it guarantees collision avoid- input, ud(t), which comes from an arbitrary ance (including a restriction on maximum accel- outer-loop controller. This controller is designed eration) for an arbitrary number of vehicles si- for the vehicle to perform a desired task, which multaneously. could range from target tracking to waypoint nav- Furthermore, a centralized avoidance scheme igation, area searching, etc. The goal of the should be avoided because of the high compu- DRCA algorithm is to adjust the control input tational load required by the central node, lack on each vehicle to guarantee collision avoidance of robustness (what happens if it breaks?), and while simultaneously staying close to the desired inability to respond quickly to emergency situa- control input (keeping in mind that this desired tions. The DRCA algorithm distributes compu- control can change with time). tation among the entire group because each vehi- For this approach to collision avoidance, the cle accounts only for its own interactions. This only vehicle states that matter are position and distribution makes for O(n) calculations on each velocity. Orientations affect performance, as they vehicle, which should be reasonable in most ap- often have bearing on the magnitude of acceler- plications. It is not a centralized algorithm, but ation available in a particular direction, but they it is not strictly decentralized since states of all do not directly affect the underlying features of other vehicles are required (not just the nearest conflict and collision. In this way, many differ- neighbors). Since it is not centralized or decen- ent vehicle models work equivalently with this tralized, the term “distributed” is used in naming approach. To simplify the math, a simple vehi- the algorithm. cle model will be used for most of the following The focus of the work in this paper is to pro- analysis: a 3D double integrator, which for the ith ceed beyond the theoretical and simulation work vehicle is given by developed in [9] by implementing the DRCA al- d r v gorithm on actual hardware. The remainder of i = i dt v u the paper is organized as follows to present the i i (1) d implementation. In Section 2, system modeling Θi = ΩiΘi, is discussed, and the deconfliction algorithm is dt presented. The two testbeds being utilized for im- where r,v ∈ R3 are the position and velocity of plementation are discussed in Section 3. Results the vehicle center of mass, and u ∈ R3 is the con- of the deconfliction implementation are given in trol input. The matrix Θ = [tˆ,nˆ,bˆ] defines the ori- Section 4, and conclusions are given in Section 5. entation of a body-fixed coordinate frame located at the vehicle center of mass relative to an inertial 2 Decentralized Reactive Collision Avoid- coordinate frame, and Ω is the cross product ma- T ance trix of the body rotation vector ω = [ωt,ωn,ωb] . Note that the orientation of the vehicle (defined ˆ 2.1 System Model by the tˆ, nˆ, and b vectors) is only used as a local coordinate frame for the DRCA algorithm. The The work here presents implementation of a orientation does not directly affect the dynamics method for deconflicting n vehicles. The notation (r and v), and as such can be arbitrary. How- throughout this paper will use bold face for vec- ever, many vehicle’s input constraints are related

2 Implementation of Deconﬂiction in Multivehicle Autonomous Systems to their orientation, and so it can be useful to tie equivalent representation of the system is (1) with this local coordinate frame to the actual body co- u = uatˆ+kvkωbnˆ −kvkωnbˆ. The tangent vector ordinates of the vehicle. must be initialized to the same direction as the We constrain the input by use of an arbitrarily velocity vector, but the dynamics will keep the varying constraint set, ui ∈ Ci. The only require- two vectors aligned from then on. In this case, R ment is that Ci always contain the origin. A sim- can be deﬁned by ple example of an input constraint set that limits maximum acceleration and velocity is utmax = −utmin = uamax

unmax = −unmin = kvkωbmax n 3 i = ui ∈ kuik ≤ umax, C R ubmax = −ubmin = kvkωnmax , T o kvik ≥ vmax =⇒ ui vi ≤ 0 . (2) and the accompanying saturation function is

For the DRCA algorithm, one must choose a ( vuTv T u − , kvk ≥ smax,u v ≥ 0 set of rectangular constraints R (which can also S = smax vary with time, state, etc.) for each vehicle that u, otherwise. encloses its C, as well as a corresponding saturation function, S : R → C. The function S must Normally one would not equate a holonomic be continuous, must become the identity map for model to a nonholonomic one, largely because any u ∈ C, and must preserve the sign of each of differences in controllability. However, full component of u when decomposed in the tˆ, nˆ, controllability is not essential to the DRCA algo- and bˆ directions. In this example, one can choose rithm since only position and velocity are essential. The DRCA algorithm is designed to use any 3 Ri = ui ∈ R − umaxi ≤ uti ≤ umaxi ,... , (3) control authority available, assuming controllability in the position and velocity states. Note and that full controllability is generally required for

 umax the nominal control, ud(t). ui , kuik > umax  kuik The relative position vector from vehicle i to  T viui vi T Si = u − , kv k ≥ v ,u v ≥ 0 (4) vehicle j is denoted r˜i j ≡ r j − ri, while the rel- i vmax i max i i  ative velocity vector is defined in the opposite ui, otherwise. sense: v˜i j ≡ vi − v j. Note that these definitions An example of how more complex vehi- imply that r˜˙i j = −v˜i j, and v˜˙i j = ui − u j. cle dynamics can be represented by this simple A useful quantity to define is the dimension- model with an appropriate choice of input con- less Deconfliction Difficulty Factor, η, to com- straint set follows. Let us model a vehicle which pare different systems in which collision avoid- can move forward with variable speed and turn ance is to be implemented. This factor is defined in two axes (a 3D unicycle model) with limits on as its turn rate, forward acceleration, and maximum v2 η = max . (5) speed. One way to describe the model mathemat- umaxdsep ically is by Conceptually, this factor is the ratio of the worst d r stˆ case turning radius to the required separation dis- = dt s u tance. It can also be thought of in terms of stop- a , d ping distance. Θ = ΩΘ, The DRCA algorithm was designed primarily dt for systems with large η (greater than unity) such where |ua| ≤ uamax , |ωn| ≤ ωnmax , |ωb| ≤ ωbmax , as aircraft and ships, where the collision avoid- and |s| ≥ smax =⇒ uas ≤ 0. Alternatively, an ance task is difficult because of the dominance

3 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN** of vehicle inertia. Vehicles with small η (sig- vi pt,i jti niﬁcantly less than unity) such as mobile robots v˜ e v j can often be modeled as having a direct veloc- si pn,i jni dsep ity command, since the inertia becomes insignif- icant. In such cases, potential function methods α r˜ β for collision avoidance may be preferable to the c dsep DRCA algorithm because of their simplicity and α ability to closely pack vehicles. The downside of potential function methods is their general lack of provable guarantees, especially when inertia is Fig. 1 An augmented depiction of the collision considered. cone used in DRCA.

2.2 Deconfliction Algorithm the rest of this paper, the i j subscripts will generally be suppressed (for example, r˜ will be writ- The vehicles considered here are modeled as i j ten as r˜). point masses, however physical vehicles have fi- nite size. Therefore, to account for physical con- Lemma 2.1 Let β = ∠v˜ − ∠r˜0, α = straints in the theoretical model, the condition for dsep arcsin , and r˜0 be the relative posi- conflict is not to attain the same position in space kr˜0k tion vector at the time conflict is being checked. at the same time, but rather to come within a A necessary and sufficient condition for no minimum allowed distance of each other at some conflict to occur is point in time. This minimum distance could be, for example, the five nautical mile separation be- |β| ≥ α. tween aircraft required by the FAA or the sum of the radii of two vehicles. The angle α represents the half-width of the collision cone ([3], [4], [2]), which is depicted in Fig. Definition 1 (Collision) A collision occurs be- 1. tween vehicles i and j when A proof using this notation is given in [10],

r˜i j < dsep,i j, but is conceptually the same as the original collision cone proofs from [3]. where dsep,i j is the minimum allowed separation If the system initially contains conflicts, a de- distance between the vehicles’ geometric centers. confliction maneuver must be performed to bring the system into a conflict-free state. One such For two vehicles not actively in a collision, maneuver is for all vehicles to turn left until the the next question is whether they will collide if system is conflict-free. Once the deconfliction they remain on their present headings. This situ- maneuver has been performed and the system ation will be called a conflict. is in a conflict-free state, then the deconfliction Definition 2 (Conflict) A conflict occurs be- maintenance controller can be used to keep the tween vehicles i and j if they are not currently in system conflict-free. This controller allows each a collision, but with zero control input (i.e. con- vehicle to use its desired control input unless that stant velocity), at some future point in time they input would cause the vehicle to come into con- will enter a collision: flict with another vehicle. A basic block diagram of this process is shown in Fig. 2.

dmin,i j ≡ min r˜i j < dsep,i j. (6) In order to smoothly transition from the de- t>0 sired control to the avoidance control, each vehi- The following lemma provides a useful way cle needs a way to measure how close its velocity to check for conflicts. To simplify the notation in vector is to causing a conflict. The first step is to

4 Implementation of Deconﬂiction in Multivehicle Autonomous Systems

Enter plane enclosing the collision cone (defined by the normal vector e) in each of the tˆ, nˆ, and bˆ directions. These signed distances are Yes Desired Separated? Control 2 2 2 ei j ei j ei j No p = , p = , and p = , t,i j Tˆ n,i j T b,i j T ˆ Yes ei jti ei jnˆ i ei jbi Deconfliction Conflict−Free? Maintenance No which are described graphically in Fig. 1. Define εt,εn,εb > 0 as thresholds such that Deconfliction Maneuver when |pt| > εt, the conflict is far enough away that it can be ignored (and likewise for pn and Fig. 2 DRCA algorithm flow chart. pb). The n-vehicle deconfliction maintenance controller running on vehicle i computes pt, pn, and pb to each of the other vehicles and then finds construct a unit-vector, cˆ, representing the side of the closest conflict in each direction, i.e. the collision cone nearest v˜. The vector cˆ is found + by rotating r˜ by α around a vector q = r˜ × v˜ and pt = min pt,i j > 0,εti i j normalizing: − (9) pti = −max pt,i j < 0,−εti , r˜ q × r˜ j cˆ = cosα + sinα. (7) krk kqkkrk and likewise for pn and pb. Note that by definition 0 < p± ≤ ε. To simplify notation, in any case Next, construct a normal vector, e, from the where a relation holds in all of the tangent, nor- collision cone to the relative velocity vector, v˜ mal, and binormal directions, the subscript will (see Fig. 1). If cˆTv˜ > 0, then e = (I − cˆcˆT)v˜, but be suppressed. if cˆTv˜ ≤ 0 (the vehicles are headed away from The input is constructed using a function, F, each other), then no normal exists, and the near- such that in each direction u = F(p+, p−,u ) est point on the collision cone is the tip, so e = v˜. d (meaning the control choice is does not produce Therefore: a conflict-generating velocity). The control func- ( v˜, cˆTv˜ ≤ 0 tion chosen for the implementation of the DRCA e = (8) algorithm here is (I − cˆcˆT)v˜, cˆTv˜ > 0. u u F(p+, p−,u ) = min p+ + max p− In order to combine the effects of multiple d ε ε collision cones, a useful approach is to decom- u − u − u + d max min p+ p−, pose the system into three component directions ε2 and analyze those directions separately. Let the (10) coordinate system be defined by the orthonormal ˆ ˆ because it is a bilinear interpolation of the follow- vectors t, nˆ, and b. The orientation of this coor- + − dinate system is arbitrary, but the convention of ing ordered triples of the form (p , p ,u): using tangent, normal, and binormal notation is P = (0,0,0) P = (ε,0,u ) chosen since fixing the coordinates to the body 1 2 min P = (0,ε,u ) P = (ε,ε,u ). of the vehicle often simplifies analysis. 3 max 4 d The next step is to determine how much con- An example of this control function is shown in trol (change in velocity) can be applied in each of Fig. 3. Because F depends on the desired control, these directions before a conflict forms. For sim- ud must be saturated such that plicity, a conservative approach is taken whereby the signed distance is found fromv ˜ to the tangent umin ≤ ud ≤ umax. (11)

5 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN**

(see [9] for full theorem statement and proof). Each time a new vehicle is added to the deconfliction group, D, it performs its deconfliction maneuver, broadcasting a conflict-free velocity to the group. Since this broadcast velocity is the quantity used by the deconfliction maintenance algorithm until the new vehicle is truly conflict- free, the deconfliction group never sees any new conflicts.

3 Testbeds Fig. 3 Example of the control function, F. Two testbeds were utilized to evaluate the DRCA Note that P4 moves increases and decreases with algorithm for different vehicle and environment changing ud. types. The first testbed is composed of autonomous air vehicles which can hover in place This choice of control function means that once and move in arbitrary directions. The second u is constructed from its three components, then testbed is composed of autonomous underwater u ∈ R . Then the saturation function S will give vehicles which are constrained to move at a con- the final resultant control vector, which will be in stant velocity and can only change direction via C. The octant-preserving nature of S will ensure heading changes. this final saturation step does not violate the re- quirements for the proof of collision avoidance. A more intuitive way to choose a value for ε 3.1 Boeing Quadrotor System is to relate it to a gain-like parameter, The Boeing Vehicle Swarm Technology Lab umax − umin (VSTL) was developed to provide rapid prototyp- k = . ε ing capabilities for mission algorithms, vehicle Note that k has units of inverse seconds. Also, hardware, and health management ([14],[1],[5]). . × . × . 3 one can see that the magnitude of the gradient of The indoor testbed is a 30 5 15 2 6 1m vol- the control function will always be less than or ume that allows for testing a heterogeneous mix- equal to k, regardless of the desired control. ture of autonomous vehicles. The testbed has full-state feedback capability through a VICON Assuming the deconfliction group, D, has n vehicles, this algorithm’s computation time on position reference system, pulsing visible light each vehicle scales as O(n), because it only re- that bounces off reflective markers attached to the quires the computation of each other vehicle’s vehicles and using a system of cameras to tri- collision cone and then substitutes these results angulate the vehicles’ position, velocity and at- into the control function. Technically O(n2) titude. Position accuracy is sub-millimeter and computations happen in the entire group, but angular accuracy is sub-degree. The overall lab- since these computations are independent of each oratory is shown in Fig. 4. Data for multiple ve- other (only linked by the sensed or communi- hicles is provided to the central data processing cated states), they can happen in parallel in a dis- hub at 100 frames per second with approximately tributed fashion, so only the per-vehicle scaling 10 milliseconds of latency [14]. The vehicles affects computation time. used for testing the DRCA algorithm are heavily- The system is guaranteed to maintain its modified Draganflyer quadrotors as shown in Fig. conflict-free state despite arbitrary control au- 5. thority restrictions with the following algorithm

6 Implementation of Deconﬂiction in Multivehicle Autonomous Systems

Fig. 4 Vehicle Swarm Technology Laboratory Fig. 6 University of Washington Fin-Actuated (VSTL) developed by the Boeing Research and Autonomous Underwater Vehicle system. Technology group. 3.3 Testbed Constraints and DRCA

3.2 University of Washington Fin-Actuated Although these testbeds are adequate for testing Autonomous Underwater Vehicles the DRCA algorithm, they do not entirely match the assumed operational criteria of DRCA. The The UW testbed is composed of a set of DRCA algorithm was designed using “velocity three ﬁn-actuated autonomous underwater vehi- space.” Because rotorcraft are continually chang- cles (Fig. 6) operating in a freshwater tank 6.1 × ing velocity while hovering, the quadrotors are 2.4 × 2.4m3 in size. Four grey-scale underwater constantly going in and out of conﬂict even if cameras in the upper corners of the tank are con- they are commanded to be stationary a safe dis- nected to a computer with a quad-core processor tance apart. Additionally, the algorithm is de- and 4GB RAM to perform real-time 3D tracking signed with the assumption of existence of space of objects in the tank. Position and velocity infor- that is free of collision cones. This assumption mation is broadcast from this tracking computer guarantees collision avoidance in free operational to the underwater vehicles at 5 Hz. All control space, but it can present problems in an enclosed calculations are done onboard the vehicles them- environment. In certain scenarios, vehicles may selves. Additional details on this system can be be presented with the choice of a collision or go- found in [12]. ing out of bounds (assuming that the boundary is a choice rather than a hard physical constraint), so that collision avoidance can no longer be guaranteed.

4 Experimental Results

Four experiments were run with the quadrotor system: one moving vehicle with a static obstacle, one moving vehicle and one vehicle with a fixed waypoint, two vehicles flying directly at one another and four vehicles flying at one an- Fig. 5 Quadrotor vehicle equipped with refer- other. Experimental results were compared to ence markers. predicted results from numerical simulation using Matlab. Four experiments were run with

7 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN** the UW system: two vehicles without colli- Table 1 Parameters used in DRCA simulation sion avoidance performing a collision, two vehi- Parameter dsep smax umax cles with DRCA avoiding the collision from the Value 1.0 1.25 5.6638 ﬁrst experiment, two vehicles colliding obliquely Parameter ε dt bound without deconﬂiction, and two vehicles with an Value 1.0 0.4 × smax 6.0 oblique interaction using DRCA. umax

4.1.1 Static obstacle with one flying vehicle 4.1 Boeing Quadrotor System Flight testing for the static obstacle case (with the The DRCA algorithm was implemented in C and static obstacle being a quadrotor with no colli- C++ and is called at 50 Hz. For the purposes sion avoidance running) gives results similar to of this demonstration, the prescribed deconflic- those found in simulation (Figs. 7(a)-7(d)). A bit tion maneuver for the vehicles was “all-turn- of delay is present in the flight test that shows left”. Several parameters can be adjusted within up in the flight path (Fig. 7(a)) and in the con- the DRCA algorithm to produce desired behav- trols (Fig. 7(b)). Other than this delay, the flight ior: dsep, smax, umax, ε, dt, and bound. The first path and controls looked similar in flight test and three parameters were defined in the discussion simulation, as does the algorithm performance. of the DRCA. The term bound is the distance Observing Fig. 7(c) shows that the vehicles in between vehicles at which deconfliction begins. flight test and in simulation had similar separa- DRCA expects to operate at the control (acceler- tion distances. Upon closer inspection, the vehi- ation) level, modifying the desired control input cles came about 0.2 m closer in simulation than to produce a new control that guarantees collision in flight test. Fig. 7(d) shows the speed of the avoidance. However, the Boeing programming moving vehicle and verifies the delay in the hard- architecture outputs desired velocities instead of ware environment. It also shows that the max- controls, so dt was a gain used to convert desired imum speed in simulation was greater than the control to velocity. 1.25 m/s specified. This difference in speed prob- The algorithm was tested both in experiment ably resulted in the vehicles moving closer to- and in a Boeing simulation that uses Matlab gether in simulation (notice that the time of maxi- Simulink to interact with the simulation environ- mum speed corresponded to the time of minimum ment SwarmView. The parameters above were separation distance). adjusted to produce desirable collision avoidance performance. Since dsep is defined for safety 4.1.2 Fixed waypoint with one flying vehicle and smax and umax are properties of the vehicle, the three parameters actually determined were ε, The results for the case of one flying vehicle bound, and dt. The parameters were determined and one vehicle with a fixed waypoint (with by an initial set from simulation results before DRCA activated on both vehicles) are shown in flight test and then refinement during flight test. Figs. 8(a)-8(d). Fig. 8(a) shows that the flight The final values used to produce the results below paths of the vehicles were virtually identical with are given in Table 1. The algorithm was tested simulation. Likewise, Fig. 8(c) shows that the ex- in four different scenarios described below. The perimental controls and conflict resolution were flight path and vehicle separation data came from extremely close to the simulated results. The the VICON position reference system. The con- speed (Fig. 8(d)) had some minor variations, but flict and control information used data from in- was quite similar overall between experiment and ternal logging within the DRCA algorithm itself simulation. As specified, the speed of vehicle two and is shown for vehicle 1 only. had a maximum at 1.25 m/s. Fig. 8(b) shows that the vehicles maintained similar separation over time in simulation and in flight test. It also shows

8 Implementation of Deconﬂiction in Multivehicle Autonomous Systems

1 flight test simulation 0 In Bound? 1

Vehicle 2 flight test Conflict 0 Vehicle 2 simulation 5 ) 10 Vehicle 1 flight test and simulation 2 0 (m/s Veh 2 x u −5 8 5 ) 2 0 (m/s Veh 2 y u −5 6 0 5 10 15 20 25 Time (s) (b) 4 10 flight test 9 simulation

8 2 7

(m) 5 0 sep 4 ||r|| − d y position (m) 3

2 −2 1

−1 −4 0 5 10 15 20 25 Time (s) (c)

−6 1.5 flight test simulation

−8 1

−4 −2 0 2 4 x position (m) Veh 2 Speed (m/s) 0.5 (a)

0 0 5 10 15 20 25 (d)

Fig. 7 Static obstacle with one vehicle. Fig. 7(a) shows the trajectories of the vehicles, and Fig. 7(b) gives the controls of the vehicles and the times that the vehicles were in bound and in conflict. Fig. 7(c) gives the difference between vehicle separation and minimum separation distance. Note that a collision occurs if the curve becomes negative. Fig. 7(d) shows the velocity of the moving vehicle. that the vehicles did not collide. Overall, the 4.1.3 Two vehicles flying at one another DRCA algorithm performed as designed in this case. In the case of two vehicles flying directly at one another, flight test results were quite similar to simulation (Figs. 9(a)-9(d)). Fig. 9(a) shows that the paths in simulation and in experiment were

9 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN**

10 Vehicle 1 flight test Vehicle 2 flight test Vehicle 1 simulation Vehicle 2 simulation 8

1 flight test simulation 6

In Bound? 0

1 4

Conflict 0 5 ) 2 2 0 (m/s Veh 1 x u −5 0 5 ) 2

y position (m) 0 (m/s Veh 1 y u −2 −5 50 5 10 15 20 25

) Time (s) 2 0 (m/s Veh 2 x

−4 u −5 5 ) 2 −6 0 (m/s Veh 2 y u −5 0 5 10 15 20 25 −8 Time (s) (c)

0.5 flight test −4 −2 0 2 4 0.4 x position (m) simulation 0.3 (a) 0.2

10 0.1

Veh 1 Speed (m/s) flight test 0 9 simulation

8 1.5

7 1 6

(m) 5 0.5 sep

4 Veh 2 Speed (m/s) 0 ||r|| − d 0 5 10 15 20 25 3 Time (s) 2 (d) 1

−1 0 5 10 15 20 25 Time (s) (b)

Fig. 8 Fixed waypoint with one flying vehicle. Fig. 8(a) shows the trajectories of the vehicles, and Fig. 8(c) gives the controls for the vehicles and the times that the vehicles were in bound and in conflict. Fig. 8(b) gives the difference between vehicle separation and minimum separation distance. Note that a collision occurs if the curve becomes negative. Fig. 8(d) shows the vehicle velocities. similar, but that they did not line up perfectly. In maneuver earlier than in simulation and seemed flight test, the vehicles began their deconfliction to have a little bit of a lag in the controls when

10 Implementation of Deconﬂiction in Multivehicle Autonomous Systems

Vehicle10 1 flight test

Vehicle 2 flight test 1 flight test Vehicle 1 simulation 8 simulation Vehicle 2 simulation

In Bound? 0

6 1

Conflict 0 4 5 ) 2 2 0 (m/s Veh 1 x u −5 0 5 ) 2 0 y position (m) (m/s Veh 1 −2 y u −5 50 2 4 6 8 10 12 14 16 18 20 22

−4 ) Time (s) 2 0 (m/s Veh 2 x −6 u −5 5 ) −8 2 0 (m/s Veh 2 y u −10 −5 −4 −2 0 2 4 0 2 4 6 8 10 12 14 16 18 20 22 x position (m) Time (s) (a) (c)

20 1.5 flight test flight test 18 simulation simulation 1 16 0.5 14 Veh 1 Speed (m/s) 12 0 (m) 10 sep 1.5 8 ||r|| − d 1 6

4 0.5

2 Veh 2 Speed (m/s) 0 0 0 2 4 6 8 10 12 14 16 18 20 22 Time (s) 0 2 4 6 8 10 12 14 16 18 20 22 Time (s) (d) (b)

Fig. 9 Two vehicles flying at one another. Fig. 9(a) shows the trajectories of the vehicles, and Fig. 9(c) gives the controls for the vehicles and the times that the vehicles were in bound and in conflict. Fig. 9(b) gives the difference between vehicle separation and minimum separation distance. Note that a collision occurs if the curve becomes negative. Fig. 9(d) shows the vehicle velocities. compared to simulation. Fig. 9(c) shows that curve did not become negative indicates that the controls in flight test were delayed compared to vehicles succeeded in avoiding collision. simulation, which likely was the cause of the different trajectories. Other than this delay, the con- 4.1.4 Four vehicles flying at one another trols were quite similar. Fig. 9(c) also shows that the vehicles deconflicted a bit slower in flight The results from the four-vehicle head-on flight test compared to simulation. Fig. 9(d) shows the test are shown in Figs. 10(a)-10(b). Fig. 10(a) speed of the vehicles, which further confirms the shows the trajectories of all four vehicles in flight delay in the system. Fig. 9(b) shows that the ve- test and compares them with simulation. All hicles had similar separation with respect to time flight paths correspond well between simulation in simulation and flight test. The fact that the and experiment. Fig. 10(b) portrays the similar separation distances in flight test and in sim-

11 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN**

20 Veh 1 & 2 Veh 1 & 3 10 18 Veh 1 & 4 Vehicle 1 flight test Veh 2 & 3 Vehicle 2 flight test 16 Veh 2 & 4 Vehicle 3 flight test Veh 3 & 4 Vehicle 4 flight test 14 8 Veh 1 & 2 sim Vehicle 1 simulation 12 Veh 1 & 3 sim Vehicle 2 simulation Veh 1 & 4 sim Vehicle 3 simulation (m) 10 Veh 2 & 3 sim Vehicle 4 simulation sep 6 Veh 2 & 4 sim 8 Veh 3 & 4 sim ||r|| − d

4 4

0 2

0 5 10 15 20 25 Time (s)

0 (b)

y position (m) 1.5 flight test 1 simulation −2 Veh 1 0.5 Speed (m/s) 0 1.5 Time (s) −4 1

Veh 2 0.5 Speed (m/s) 0 1.5 Time (s) −6 1

Veh 3 0.5 Speed (m/s) 0 −8 1.5 Time (s) 1

Veh 4 0.5 Speed (m/s) −10 0 −4 −2 0 2 4 0 5 10 15 20 25 x position (m) Time (s) (a) (c)

Fig. 10 Four vehicles flying at one another. Fig. 10(a) shows the trajectories of the vehicles, and Fig. 10(b) gives the difference between vehicle separation and minimum separation distance. Note that a collision occurs if the curve becomes negative. Fig. 10(c) shows the vehicle velocities. ulation, and confirms that no collisions occurred 4.2.1 Two vehicles with intentional collision between any vehicles. Fig. 10(c) shows similar speeds in simulation and in flight test, although To demonstrate that the UW underwater vehi- speeds in the flight test spike a bit higher at the cles will actually collide without use of a decon- time when the vehicles are closest together. fliction algorithm when commanded to move directly at one another, two vehicles were set at op- 4.2 UW Underwater Vehicle System posite ends of the tank and commanded to seek a waypoint behind the opposite vehicle. DRCA The DRCA algorithm on the UW underwater ve- was not used in this experiment. hicles used the “all-turn-left” deconfliction ma- Fig. 11(a) shows the trajectories followed by neuver. The parameter bound used in the quad- the vehicles during the experiment. Because the rotors was not used on the underwater vehi- waypoint sought by each vehicle was directly be- cles because the DRCA algorithm was run at all hind the opposing vehicle, the waypoint algo- times, regardless of vehicle separation. The ve- rithm caused the vehicles to engage in a head-on hicles were treated as having a constant com- collision. Fig. 11(b) shows the distance between manded speed of 1.3 ft/s. The commanded sepa- the vehicles. The distance between the vehicles ration distance dsep was 1.5 ft. decreases zero as the experiment progresses, indicating that the vehicles collided.

12 Implementation of Deconﬂiction in Multivehicle Autonomous Systems

(a) (b)

Fig. 11 Two vehicles with intentional collision. Fig. 11(a) shows the trajectory of the underwater vehicles, and Fig. 11(b) shows the vehicle separation distance. Note that the vehicles collided while seeking their waypoints. The vehicles were not running the DRCA algorithm.

4.2.2 Two vehicles with DRCA reach their targets.

To demonstrate the effectiveness of the DRCA algorithm, the same waypoint controller and initial 4.2.3 Two vehicles with intentional oblique col- conditions were used as in §4.2.1, but with the lision DRCA algorithm running as an additional layer of control. The DRCA algorithm allowed the To demonstrate the deconfliction algorithm in a vehicles to reach their waypoints while avoiding different situation, two vehicles were placed in collision. adjacent corners of the tank and given waypoints Fig. 12(a) shows the trajectories of the two along the long edge of the tank opposite their vehicles running the DRCA algorithm. The ve- starting positions. Without the use of a decon- hicles started out in conflict and made an im- fliction algorithm these conditions caused the ve- mediate turn to the left to reach a conflict-free hicles to collide obliquely. For this experiment, state. They then passed each other outside the one of the two fin-actuated vehicles was replaced minimum separation distance and turned back to with a remote control toy shark controlled by a the right toward their waypoints. After they were human operator. The human operator drove the completely past each other, they were able to go toy shark directly to its waypoint at a constant to their commanded waypoints, completing their velocity. mission. Fig. 12(b) shows the separation between Fig. 13(a) shows the trajectories of the two the two vehicles during the run. Note that al- vehicles operating without deconfliction. Both though the vehicles passed close to the minimum vehicles move diagonally towards the opposing separation distance, they always remained in a wall and meet in the center of the tank. Fig. 13(b) collision-free state. shows the distance between the two vehicles, The only difference between the runs in this which goes to zero, indicating a collision. Note experiment and the runs in the experiment that that this collision is intentional and demonstrates intentionally caused a collision was the addition that a collision will take place with these initial of DRCA control on top of the desired controller. conditions if deconfliction is not employed. The DRCA algorithm successfully prevented collision while permitting the vehicles to efficiently

13 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN**

(a) (b)

Fig. 12 Two vehicles with DRCA. Fig. 11(a) shows the trajectory of the underwater vehicles, and Fig. 11(b) shows the difference between vehicle separation and minimum separation distance. Note that the vehicles reached their waypoints without colliding.

(a) (b)

Fig. 13 Two vehicles with intentional oblique collision. Fig. 13(a) shows the trajectory of the underwater vehicles, and Fig. 13(b) shows the vehicle separation distance. The vehicles were not running the DRCA algorithm.

4.2.4 Two vehicles with oblique interaction and It then passes behind the red vehicle until it can DRCA follow its desired controller to the waypoint. Fig. 14(b) shows the distance between the In this experiment, the conditions of §4.2.3 were two vehicles in excess of the commanded sepa- repeated, but DRCA was added as an additional ration distance. Note that the DRCA algorithm layer of control on the green vehicle. The red ve- permits the vehicles to avoid collision even when hicle (the human controlled toy shark) was con- one of the vehicles is not participating in decon- trolled directly to its waypoint and did not use the ﬂiction. At no point in the run did the vehicles DRCA algorithm. Fig. 14(a) shows the trajecto- come within the commanded separation distance ries of the two vehicles during the run. Because of each other. The red vehicle did not inten- the vehicles begin in conﬂict, the green vehicle tionally behave antagonistically, but ignored the makes an immediate left turn to a safe heading. green vehicle during its run.

14 Implementation of Deconﬂiction in Multivehicle Autonomous Systems

(a) (b)

Fig. 14 Two vehicles with oblique interaction and DRCA. Fig. 14(a) shows the trajectory of the underwater vehicles, and Fig. 14(b) shows the difference between vehicle separation and minimum separation distance. Note that the vehicles reached their waypoints without colliding.

5 Conclusion FA-9550-07-1-0528.

The work in this paper has focused on the imple- Copyright Statement mentation of a deconfliction algorithm on two autonomous multivehicle platforms, one composed The authors confirm that they, and/or their company or of air vehicles and one composed of underwa- organization, hold copyright on all of the original ma- ter vehicles. For both testbeds, the DRCA algo- terial included in this paper. The authors also confirm rithm was used to prevent collisions in a number that they have obtained permission, from the copy- of typical scenarios. Some particular points to right holder of any third party material included in this note in these scenarios are that the DRCA algo- paper, to publish it as part of their paper. The authors rithm was designed for systems with open bound- confirm that they give permission, or have obtained aries with an assumption that conflict-free con- permission from the copyright holder of this paper, for trol choices exist and in which the only conflicts the publication and distribution of this paper as part of being considered are between vehicles using the the ICAS2010 proceedings or as individual off-prints algorithm. In fact, the results in the work here from the proceedings. show that these assumptions can be relaxed, at least to a certain extent, and deconfliction can still References be maintained. Results between experiment and simulation compared well indicating that the sim- [1] Bieniawski S, Pigg P, Vian J, Bethke B, and ulator for the air vehicles is accurately reflecting How J. Exploring health-enabled mission con- experimental performance. Current work is di- cepts in the vehicle swarm technology lab. Proc rected at exploring the effects of limited sensing Proceedings of 2009 Infotech@Aerospace Con- (range and directionality) on the deconfliction al- ference, 2009. gorithm. [2] Carbone C, Ciniglio U, Corraro F, and Luongo S. A novel 3d geometric algorithm for aircraft autonomous collision avoidance. Proc Proceed- Acknowledgment ings of the 45th IEEE Conference on Decision & Control, December 2006. The work in this paper was funded in part by the [3] Chakravarthy A and Ghose D. Obstacle avoid- Boeing Company and in part by AFOSR grant ance in a dynamic environment: A collision

15 A. P. MELANDER*, N. D. POWEL*, E. LALISH*, K. A. MORGANSEN*, J. S. JANG** AND J. VIAN**

cone approach. IEEE Transactions on Systems, [15] Tomlin C, Pappas G, and Sastry S. Conflict Man, and Cybernetics, Vol. 28, No 5, pp 562– resolution for air traffic management: a study 574, September 1998. in multi-agent hybrid systems. IEEE Transac- [4] Frazzoli E, Mao Z, Oh J, and Feron E. Res- tions on Automatic Control, Vol. 43, pp 509– olution of conflicts involving many aircraft via 521, 1998. semidefinite programming. AIAA J. Guidance, Contr. and Dyn., Vol. 24, No 1, pp 79–86, 2001. [5] Halaas D, Bieniawski S, Pigg P, and Vian J. Control and management of an indoor, health enabled, heterogeneous fleet. Proc Proceedings of 2009 Infotech@Aerospace Conference, 2009. [6] Hwang C. N. The integrated design of fuzzy collision-avoidance and h-infinity autopilots on ships. Journal of Navigation, Vol. 55, pp 117– 136, 2002. [7] Kirk D. B, Heagy W. S, and Yablonski M. J. Problem resolution support for free flight oper- ations. IEEE Transactions on Intelligent Trans- portation Systems, Vol. 2, pp 72–80, 2001. [8] Kuchar J and Yang L. A review of conflict de- tection and resolution modeling methods. IEEE Transactions on Intelligent Transportation Sys- tems, Vol. 1, pp 179–189, 2000. [9] Lalish E. Distributed Reactive Collision Avoid- ance for Multivehicle Systems. PhD thesis, Uni- versity of Washington, 2009. [10] Lalish E and Morgansen K. Distributed reactive collision avoidance for multivehicle systems. Proc Proc. IEEE Conf. Dec. Control, 2008. [11] Le M. D. An automatic control system for ship harbour manoeuvers using decoupling control. Proc Proceedings of the IEEE Conference on Robotics & Automation, 2001. [12] Morgansen K, Triplett B, and Klein D. Ge- ometric methods for modeling and control of free-swimming fin-actuated underwater vehicles. IEEE Transactions on Robotics, Vol. 23, No 6, pp 1184–1199, December 2007. [13] Paielli R. A and Erzberger H. Conflict probabil- ity estimation for free flight. Journal of Guid- ance, Control, and Dynamics, Vol. 20, pp 558– 596, 1997. [14] Saad E, Vian J, Clark G, and Bieniawski S. Vehicle swarm rapid prototyphing testbed. Proc AIAA Infotech@Aerospace Conference and AIAA Unmanned...Unlimited Conference, April 2009.