Minimum-Effort Guidance for

Vision-Based Collision Avoidance

Yoko Watanabe∗, Anthony J. Calise† and Eric N. Johnson‡

Georgia Institute of Technology, Atlanta, GA, 30332

Johnny H. Evers§

AFRL/MNGN, Eglin AFB, FL, 32542

This paper describes a vehicle guidance strategy for waypoint tracking as well as col-lision avoidance with unforeseen obstacles using a 2D passive vision sensor mounted onthe vehicle. An extended Kalman filter is applied to estimate the position of each ob-stacle relative to the vehicle from image-based measurements. A collision cone approachis utilized to determine a critical obstacle, and an aiming point for the critical obstacleis set on a boundary of the cone. A minimum-effort guidance law for multiple targetstracking is applied to guide the vehicle to a given waypoint via the aiming point to avoidthe critical obstacle. Simulation results illustrate that the suggested minimum-effort guid-ance achieves a waypoint tracking mission while avoiding obstacles with less control effortwhen compared to a previously developed sequential proportional navigation approach.Moreover, minimum-effort guidance improves convergence of vision-based obstacle posi-tion estimation, and hence enhances obstacle avoidance performance.

I. Introduction

Unmanned vehicles have been developed for practical applications in various fields. Especially, unmannedaerial vehicles (UAVs) play an important role in military operations. For some missions UAVs have tooperate in congested environments that include both fixed and moving obstacles. For such missions, obstacleavoidance is an anticipated requirement.

In this paper, we develop a minimum-effort guidance design which enables a vehicle to reach a givendestination while avoiding collisions with obstacles. When there is no obstacle, it will result in proportionalnavigation (PN) which is considered a simple and very effective strategy in target interception.1 However,unforeseen obstacles must be avoided on a way to the destination. Kumar and Ghose proposed a guidancelaw that achieves both waypoint tracking and collision avoidance.2 However, this algorithm assumes rangeinformation is available from a radar. A method described in a paper by Kwag and Kang also assumes aradar sensor system for collision avoidance.3 In this paper, we develop an extended Kalman filter (EKF)that processes the information available on a camera’s image plane, to estimate the variables needed by theguidance law. As seen in nature among bird and insects, a vision sensor can provide sufficient information asan exclusive sensor for the purpose of obstacle avoidance. Furthermore, it is efficient to use a vision sensorsince it is low cost, light weight and compact.

A collision cone approach suggested by Chakravarthy and Ghose is applied to establish a collision criteria.4

A collision cone is defined for each obstacle by two lines tangential to the obstacle’s collision-safety boundary.An obstacle is considered to be critical if the vehicle’s velocity vector lies within its collision cone. If there ismore than one critical obstacle, the closest is chosen as the most critical and an obstacle avoidance maneuveris executed.

∗Graduate Research Assistant, Email: yoko watanabe@ae.gatech.edu†Professor, Email: anthony.calise@aerospace.gatech.edu‡Lockheed Martin Assistant Professor of Avionics Integration, Email: eric.johnson@aerospace.gatech.edu§Chief, Autonomous Control Team, Email: johnny.evers@eglin.af.mil

1 of 9

American Institute of Aeronautics and Astronautics

AIAA Atmospheric Flight Mechanics Conference and Exhibit21 - 24 August 2006, Keystone, Colorado

AIAA 2006-6641

Copyright © 2006 by the Authors. Published by the American Institute of Aeronautics and Astronautics, Inc., with permission.

A guidance strategy for collision avoidance is designed by applying a minimum-effort guidance (MEG)method for multiple targets tracking.5,6 Han and Bang, and the present authors suggested a PN-basedcollision avoidance method.7,8 After the most critical obstacle is identified, an aiming point is given at theintersection of the collision cone and the collision-safety boundary. In order to avoid the obstacle, the vehicleis guided towards the aiming point by using PN guidance. The MEG-based approach minimizes the controleffort required for the vehicle to reach the destination via the aiming point, while the PN-based approachminimizes the effort required to reach the aiming point, and then to reach the destination separately. TheMEG law is derived by minimizing the control effort, given an interior point condition for the aiming pointand a terminal condition for the destination. Therefore, the MEG-based approach can achieve the missionwith less control effort compared to the PN-based guidance. This is particularly important when maneuveringin congested environments with limited maneuver capability. Moreover, MEG-based guidance creates morelateral motion which helps convergence of the vision-based estimation errors, and hence improves overallperformance in collision avoidance.

In Section II the vehicle and obstacle motion dynamics are presented. In Section III, the formulationof the EKF is presented. Section IV introduces a collision criteria based on the collision cone. Section Vexplains the MEG-based guidance design for a waypoint tracking and obstacle avoidance. In Section VI,we discuss an example of collision avoidance for stationary obstacles and in Section VII, we present theconcluding remarks.

II. Problem Formulation

Figure 1 summarizes the problem geometry. Let (X, Y, Z) denotes a local fixed frame. Let Xv and Vbe a vehicle’s position and velocity vector, respectively. Let a be a vehicle’s acceleration input vector. Thenthe vehicle motion dynamics is given by

Xv =

xv

yv

zv

=

uvw

= V (1)

V =

uvw

=

ax

ay

az

= a (2)

In this problem, we assume that the UAV flies with a constant speed in the X direction, which meansu = const. and ax = 0. The UAV is controlled by commanding a lateral acceleration ay and a verticalacceleration az.

Figure 1. Problem Geometry

2 of 9

American Institute of Aeronautics and Astronautics

Let Xd = [ xd yd zd ]T be a given destination position in the local frame. Then the problem of reachinga destination point is satisfied if

y(tf ) = yd, z(tf ) = zd (3)

where tf is a terminal time defined by

tf = t +xd − xv(t)

u(4)

Let Xobs be obstacle’s position in the local frame and assume Xobs = 0, i.e., stationary obstacles. Thenthe obstacle’s relative motion dynamics to the vehicle is written by

X = Xobs − Xv = −V (5)

where X = Xobs − Xv = [ x y z ]T is a relative position vector. For collision avoidance, the vehicle isrequired to keep a minimum separation distance d from every obstacle. Therefore, a collision-safety boundarybecomes a spherical surface with a radius d and a center at the obstacle’s position.

III. Vision-Based Estimator Design

A camera is mounted on the UAV, and its optical axis is controlled so that it remains aligned withthe X axis in an inertial frame. This gimbal camera assumption is not necessary, and the method can beeasily extended to the case where this pointing axis occasionally changes if the camera’s attitude is known.Assuming a pin-hole camera model, the measurement of the obstacle position in an image plane at a k-thtime step is given by

zk =1xk

[yk

zk

]+ νk = h(Xk) + νk (6)

where ν is a measurement noise with zero mean and a variance R = σ2I.Since (6) is nonlinear with respect to the relative state, an extended Kalman filter (EKF) is applied to

estimate the relative position of each obstacle. All the own-ship vehicle states are assumed to be known.The EKF for the process model (5) and the measurement model (6) is formulated as follows.9,10

Update:

Xk = X−k + Kk(zk − z−k ) (7)

Pk = P−k −KkHkP−k (8)Kk = P−k Hk(HkP−k HT

k + R)−1 (9)

where Xk is an updated estimate of X at a k-th time step and Pk is its error covariance matrix. Kk

is a Kalman gain. X−k and P−k are a predicted estimate and its error covariance matrix. A predicted

measurement is obtained by z−k = h(X−k ) and a measurement matrix Hk is calculated by

Hk =∂h(X)

∂X

∣∣∣X=

ˆX−k

=1

x−k

−

y−k

x−k

1 0

− z−k

x−k

0 1

=

1x−k

[−h(X

−k ) I

]

Prediction:

X−k+1 = Xk − V k∆t− 1

2a∆t2 (10)

P−k+1 = ΦkPkΦTk (11)

where ∆t is a sampling time. Φk is a state transition matrix and which can be approximated by

Φk ' I

for stationary obstacles when ∆t is sufficiently small. The camera is modeled as having a limited field ofview, and only the prediction steps (10, 11) are executed for obstacles lying temporarily outside the field ofview.

3 of 9

American Institute of Aeronautics and Astronautics

Figure 2. Collision Cone

IV. Collision Criteria

A collision cone approach is applied to determine if the obstacle is critical to the vehicle or not.4 A collisioncone is defined by a set of tangential lines from the vehicle to the obstacle’s collision-safety boundary. Whenthe vehicle’s velocity vector lies in the collision cone, the obstacle is considered to be critical. Consider a 2Dplane including the relative position vector X and the vehicle velocity vector V . Then the collision-safetyboundary appears as a circle and the collision cone is specified by two vectors (r1, r2) which are from thevehicle position and are tangential to the boundary circle, as shown in Figure 2. r1 and r2 are defined asfollows.

ri = X + dui, i = 1, 2 (12)

where

u1 = − 1‖X‖2 (c(X · V ) + d)X + cV

u2 =1

‖X‖2 (c(X · V )− d) X − cV , c =

√‖X‖2 − d2

‖X‖2‖V ‖2 − (X · V )2

The vehicle velocity vector can be written in terms of r1 and r2.

V = ar1 + br2 (13)

where a and b are coefficients calculated as follows.

a =12

(X · V

‖X‖2 − d2+

1cd

)(14)

b =12

(X · V

‖X‖2 − d2− 1

cd

)(15)

Then, the collision cone criterion is given by

a > 0 AND b > 0 (16)

When the collision cone criterion (16) is satisfied, the vehicle is considered to be in danger of collisionwith the obstacle and it should take some avoiding maneuver. The aiming point Xap is specified to be usedfor collision avoidance, as shown in Figure 2.

Xap =

xap

yap

zap

=

{r1, 0 < a ≤ br2, 0 < b < a

(17)

4 of 9

American Institute of Aeronautics and Astronautics

A time-to-go to the aiming point is defined as follows.

tgo = t +xap − xv(t)

u(18)

When (tgo − t) is large, there is no urgency for the vehicle to take an avoidance maneuver. Also, if (tgo − t)is negative or larger than the terminal time tf , there is no chance of collision. Therefore, in addition to thecollision cone criterion (16), we add the following time-to-go criteria.

tgo − t < T, 0 < tgo < tf (19)

An obstacle is considered critical only if both collision criteria (16) and (19) are satisfied. If there is morethan one critical obstacle, the one closest to the vehicle (hasving the smallest value of tgo) is chosen as themost critical.

V. Guidance Law

A sequential proportional navigation (PN)-based approach was suggested by Han and Bang.7 Thisapproach is derived by solving the following optimization problem.

mina

Joa =12

∫ tgo

t0

aT (t)a(t)dt =12

∫ tgo

t0

(a2

y(t) + a2z(t)

)dt (20)

subject to the vehicle motion dynamics (1-2) with a terminal condition

yv(tgo) = yap, zv(tgo) = zap (21)

The resulting optimal guidance law by using the relative state estimates at t = t0 is given by

aoa(t0) = −3

1

(tgo − t0)2

0yv(t0)− yap

zv(t0)− zap

+

1tgo − t0

0v(t0)w(t0)

(22)

Since the obstacle positions are unknown and we can only obtain estimated values, Xap and tgo are used in(22) instead of their true values. If there is no critical obstacle, a nominal control input for a destinationtracking can be derived in a similar way. By solving

mina

Jnom =12

∫ tf

t0

aT (t)a(t)dt =12

∫ tf

t0

(a2

y(t) + a2z(t)

)dt (23)

with a terminal condition (3), we will obtain

anom(t0) = −3

1

(tf − t0)2

0yv(t0)− yd

zv(t0)− zd

+

1tf − t0

0v(t0)w(t0)

(24)

As an alternative, the minimum-effort guidance (MEG) for multiple targets tracking developed by Ben-Asher5,6 can be applied. In this problem, the aiming point and the destination point are considered as twotargets to be tracked. The minimum-effort guidance law is derived by solving

mina

J =12

∫ tf

t0

aT (t)a(t)dt =12

∫ tf

t0

(a2

y(t) + a2z(t)

)dt (25)

subject to the vehicle motion dynamics (1-2), with an interior point constraint for the aiming point and aterminal constraint for the destination.

{yv(tgo) = yap, zv(tgo) = zap

yv(tf ) = yd, zv(tf ) = zd(26)

5 of 9

American Institute of Aeronautics and Astronautics

From Euler-Lagrange equation,11 this problem can be solved analytically. The optimal guidance law usingthe estimates is obtained as follows.

a(t0) = aoa(t0)− 33(tgo − t0) + 4(tf − tgo)

0v(t0)w(t0)

+

3tgo − t0

0yv(t0)− yap

zv(t0)− zap

− 2

tf − tgo

0yap − yd

zap − zd

(27)where aoa(t0) is given in (22).

VI. Simulation Results

In this section, simulation results are presented comparing the PN and MEG guidance approaches for thecase of two obstacles and a single destination point. Two cases are studied. In the first case true states, whichare not available in reality, are fed back to the guidance laws. Both guidance laws succeeded to guide thevehicle to the destination while avoiding collisions with two obstacles, although the trajectories are slightlydifferent (Figure 3). Figure 4 and Figure 5 depict the corresponding time histories of the lateral controlinput and the control cost. Even though there is not much difference in the resulting vehicle trajectories,the control cost is significantly decreased using MEG instead of PN-based guidance. Figures 6-8 show theresults for the case where the obstacle relative positions are estimated from vision-based measurements andthe estimates are fed back to the guidance law. Figures 9 and 10 show position estimation errors for the twoobstacles. From 10, it is observed that the range estimation error converges faster when using the MEG-based guidance, which is due to the property that MEG creates larger lateral motion earlier in the maneuverin comparison to PN guidance. In this example, the range estimation error results in a slight violation ofthe boundary when using PN guidance. This violation increases the control input for collision avoidance,and increases the measure of control effort. From these results we can conclude that MEG-based guidanceachieves the mission objective with significantly less control effort.

0

10

20

30

40

50

60

70

80

−100

10

−10

−5

0

5

10

YX

Z

MEG: Vehicle TrajectoryPN: Vehicle TrajectoryObstacle Safety BoundaryObstacleStart Point and Destination

Obstacle 2 Obstacle 1

Start Point

Destination

Figure 3. Vehicle Trajectories: Both PN and MEG avoid the obstacles and arrive at the desired destination,the MEG flies a smoother trajectory

6 of 9

American Institute of Aeronautics and Astronautics

0 1 2 3 4 5 6 7 8−10

−5

0

5

10

t

a y

0 1 2 3 4 5 6 7 8−10

−5

0

5

10

t

a z

MEGPN

Figure 4. Lateral Acceleration Inputs

0 1 2 3 4 5 6 7 80

5

10

15

20

25

30

35

40

45

t

J

MEGPN

Figure 5. Control Costs: MEG achieves the missionwith less control cost than PN.

0

10

20

30

40

50

60

70

80

−100

10

−10

−5

0

5

10

YX

Z

MEG: Vehicle TrajectoryPN: Vehicle Trajectory Obstacle Safety BoundaryObstacleStart Point and Destination

Obstacle 2 Obstacle 1

Start Point

Destination

Figure 6. Vehicle Trajectories: MEG avoids both obstacles while PN violates the second obstacle’s collision-safety boudary.

7 of 9

American Institute of Aeronautics and Astronautics

0 1 2 3 4 5 6 7 8−10

−5

0

5

10

t

a y

0 1 2 3 4 5 6 7 8−10

−5

0

5

10

t

a z

MEGPN

Figure 7. Lateral Acceleration Inputs

0 1 2 3 4 5 6 7 80

5

10

15

20

25

30

35

40

45

t

J

MEGPN

Figure 8. Control Costs: MEG has less control costthan PN.

0 1 2 3 4 5 6 7 8−6−4−2

02

t

e x

Obstacle 1

0 1 2 3 4 5 6 7 8−6−4−2

02

t

e y

0 1 2 3 4 5 6 7 8−6−4−2

02

t

e z

MEGPN

Figure 9. Position Estimation Error (Obstacle 1)

0 1 2 3 4 5 6 7 8−2

0246

t

e x

Obstacle 2

0 1 2 3 4 5 6 7 8−2

0246

t

e y

0 1 2 3 4 5 6 7 8−2

0246

t

e z

MEGPN

Figure 10. Position Estimation Error (Obstacle 2):MEG improves a convergence of the range estimationerror.

8 of 9

American Institute of Aeronautics and Astronautics

VII. Conclusion

This paper applies a minimum-effort guidance law for multiple targets tracking to the problem of vision-based obstacle avoidance. Simulation results verify that using the MEG approach reduces the requiredcontrol effort in comparison to a previously implemented PN-based guidance approach. In addition to that,MEG guidance improves estimation accuracy and overall obstacle avoidance performance. In this paper,only one destination point was considered. However, it has been previously shown that the optimal solutioncan be also derived for the case of n target points.5,6 Therefore, the approach can be easily extended to aproblem of multiple waypoint tracking while avoiding obstacles. For future work, we would like to implementMEG guidance in a more realistic 6 DOF simulation, and to validate the overall approach in a flight test.

VIII. Acknowledgement

This work was supported in part by AFOSR MURI, #F49620-03-1-0401: Active Vision Control Systemsfor Complex Adversarial 3-D Environments.

References

1P.Zarchan. ”Tactical and Strategic Missile Guidance” AIAA. 1994.2B.A.Kumar and D.Ghose. ”Radar-Assisted Collision Avoidance/Guidance Strategy for Planar Flight” IEEE Transactions

on Aerospace and Electronic Systems. Vol.37, No.1. January 2001.3Y.K.Kwag and J.W.Kang. ”Obstacle Awareness and Collision Avoidance Radar Sensor System for Low-Altitude Flying

Smart UAV” Digital Avionics Systems Conference. October 2004.4A.Chakravarthy and D.Ghose. ”Obstacle Avoidance in a Dynamic Environment: A Collision Cone Approach” IEEE

Transactions on Systems, Man and Cybernetics-Part A: Systems and Humans. Vol.28, No.5. 1998.5J.Z.Ben-Asher. ”Minimum-Effort Interception of Multiple Targets” AIAA Journal of Guidance, Control, and Dynamics.

Vol.16, No.3. 1993.6J.Z.Ben-Asher and I.Yaesh. ”Advances in Missile Guidance Theory” AIAA. 1998.7S.C.Han and H.Bang. ”Proportional Navigation-Based Optimal Collision Avoidance for UAVs” 2nd International Con-

ference on Autonomous Robots and Agents. 2004.8Y.Watanabe, E.N.Johnson and A.J.Calise. ”Vision-Based Approaches to UAV Formation Flight and Obstacle Avoidance”

Second International Symposium on Innovative Aerial/Space Flyer Systems. December 2005.9R.G.Brown and P.Y.C.Hwang. ”Introduction to Random Signals and Applied Kalman Filtering” John Wiley & Sons.

1997.10P.Zarchan and H.Musoff. ”Fundamentals of Kalman Filtering: A Practical Approach” AIAA. 2004.11E.Bryson and Y.Ho. ”Applied Optimal Control” Taylor & Francis. 1975.

9 of 9

American Institute of Aeronautics and Astronautics