Stochastically Optimized Monocular Vision-Based ... - CiteSeerX

AIAA 2007-6865

AIAA Guidance, Navigation and Control Conference and Exhibit 20 - 23 August 2007, Hilton Head, South Carolina

Stochastically Optimized Monocular Vision-Based Guidance Design Yoko Watanabe∗, Eric N. Johnson† and Anthony J. Calise‡ Georgia Institute of Technology, Atlanta, GA, 30332 This paper designs a relative navigation and guidance system for unmanned aerial vehicles for monocular vision-based control applications. Since 2-D vision-based measurement is nonlinear with respect to the 3-D relative state, an extended Kalman filter (EKF) is applied in the navigation system design. It is well-known that the vision-based estimation performance highly depends on the relative motion of the vehicle to the target. Therefore, a main objective of this paper is to derive an optimal guidance law to achieve given missions under the condition that the EKF-based relative navigation outputs are fed back to the guidance system. This paper suggests a stochastic optimal guidance design that minimizes the expected value of a cost function of the guidance error and control effort subject to the EKF prediction and update procedures. A one-step-ahead suboptimal optimization technique is implemented to avoid iterative computations. The approach is applied to vision-based target tracking and obstacle avoidance, and simulation results are illustrated.

I.

Introduction

Unmanned aerial vehicles, or UAVs, are expected to play an important role in both military and commercial applications. Two main advantages of using UAVs instead of manned aircraft for these operations are the following: they are considered to be cost-effective and there is no risk in the loss of human pilot life.1 The autonomous flight system of UAVs has been progressively developed in recent years. Autonomy is distinguished from automation as it requires the capability of making decisions as well as executing them.2 One of the most challenging problems for autonomous UAV flight is situation awareness. The system requires active or passive sensors to provide information about uncertain environments, and also requires algorithms that extract objects of interest from that information. For example, laser range finders can provide very accurate environmental data.3 However, as seen in nature among birds and insects, a passive sensor can provide sufficient information as an exclusive sensor to detect objects. Furthermore, it is efficient to use a vision sensor since it is compact, light-weight and low cost. Therefore, this paper considers a navigation and guidance design for UAVs for monocular vision-based control applications. Monocular vision-based navigation and control is one of the most focused research topics for the automation of UAVs. Some studies focus on vision-based vehicle localization, in which vision information is utilized to determine the vehicle states such as position and attitude in case of GPS failures.4,5 Others have developed algorithms to recover the 3-D environment from 2-D vision measurements, assuming all the vehicle states are known through its own-ship navigation system. For example, vision-based target tracking has been intensively investigated with applications of autonomous landing,6 ground object tracking,7 formation flight8 and aerial refueling.9 To achieve vision-based UAV autonomous operations, realtime image processing is a requirement. The image processing itself is a very challenging topic, and in particular the processing time is critical for such applications. This paper does not focus on development of image processing algorithms. Assuming the image processor detects a target in each image frame, a relative navigation system is designed to estimate the relative state to the target from the image processor outputs. Since the vision-based measurement is nonlinear with respect to the state, an extended Kalman filter (EKF)10,11 is applied, and the EKF-based estimates are fed back to the guidance system. ∗ Graduate

Research Assistant, Email: yoko [email protected] Martin Assistant Professor of Avionics Integration, Email: [email protected] ‡ Professor, Email: [email protected] † Lockheed

1 of 16 American Institute of Aeronautics and Astronautics

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

A main objective of this paper is to derive an optimal or approximately optimal guidance law that achieves a given mission by using vision-based navigation. Generally, the guidance design is formulated as a vehicle trajectory optimization problem. There is a large body of research on this subject.12−14 Most consider off-line trajectory generation and aimed to solve the optimization problem efficiently under the assumption of full state information. However, in real applications, the optimal vehicle trajectory needs to be calculated by using the state estimate which is updated at each time step by available measurements. A common way to determine a guidance law for such a case is to replace the true states by their estimates in the optimal solution that is obtained by assuming full state information. However, this approach is not optimal and can even cause poor guidance performance due to the estimation errors. Blackmore suggested a guidance design which considers estimation and modeling uncertainties.15 This work designs a guidance law for obstacle avoidance by using the estimated vehicle states. The guidance design utilizes particles to measure the probabilistic estimation uncertainties and the vehicle is guided to a further distance from the obstacle when the estimate includes a larger uncertainty. For vision-based relative navigation, it is well-known that estimation performance highly depends on sensor motion relative to a target.16 At the same time, guidance performance directly depends on the estimation accuracy since the estimates are fed back to the guidance law. In other words, the separation principle does not hold between estimation and control. Therefore, this paper derives a vehicle motion which achieves given missions while minimizing the estimation errors. The observer trajectory optimization for estimation improvement was first treated by Hammel et al.17 Similar studies have been performed in bearing-only localization and target tracking18−20 and in vision-based estimation.21,23 In these papers, an optimal observer trajectory is calculated so that a certain estimation performance cost is minimized. Singh et al. formulated a more generalized problem called sensory scheduling.22 They introduced the concept of sensor actions which could be not only a sensor trajectory but also a choice of sensor to be used and a tunable parameter. The estimation error is minimized over available sensor actions. However only sensor trajectory optimization is treated in their paper. The main focus of those work was cost function selection so that the resulting optimization problem can be efficiently solved. For example, Hammel et al. and Oshman et al. maximize the determinant of the Fisher information matrix over the observer trajectory by using a direct gradient method.17,18 Frew et al. minimize the determinant of the predicted estimation error covariance matrix over the discretized observer motion.21 There are two main issues associated with those works. One is that there has not been established a systematic way to choose the cost function. The other is that all of their algorithms require iterative computations to obtain the optimal solution which is not suitable for realtime application. Since the estimation error is assumed to be white Gaussian noise with zero mean and its estimated covariance matrix is obtained in the EKF process, stochastic optimization can be performed for the original vehicle trajectory optimization problem which has been set up to achieve a given mission. In this approach, the cost function will be systematically obtained. Kim and Rock also suggested a stochastic feedback controller design for bearing-only tracking.24 Since the EKF update law is nonlinear with regard to the relative motion dynamics, a solution of the resulting stochastic optimization problem can be only obtained numerically. To reduce the computational cost, this paper derives an approximately optimal or suboptimal solution. In Kim and Rock’s paper, the steady state solution is assumed and the optimal guidance law is derived by solving the algebraic Ricatti equation. However, this assumption is not appropriate for many applications such as target tracking, in which a finite terminal time is given. Logothetis et al. compared several different suboptimal techniques for observer trajectory optimization for bearing-only tracking problem.25 The idea of one-step-ahead (OSA) optimization was introduced as a suboptimal strategy, in which the optimization problem is solved under the assumption that there will be only one more final measurement at the next time step. Under this assumption, the observer trajectory optimization needs to be considered only for a guidance input at the current time step since there will be no chance to improve the estimation accuracy after the final measurement is obtained. Therefore, the optimization will be performed over a single vector representing a guidance input at the current time step, and it can be solved by an algebraic equation. This paper suggests combining the stochastic optimization formulation and the OSA suboptimal optimization approach to establish a guidance design for the monocular vision-based control problem. The suggested approach eliminated the need for heuristic strategies in guidance design, and significantly reduces the computational burden. This paper is structured as follows: Section II formulates the monocular vision-based vehicle guidance problem, Section III designs the EKF-based relative navigation system, Section IV discusses our OSA suboptimal guidance design, Section V presents the simulation results and Section VI contains the concluding


remarks. The suggested guidance design has been applied to vision-based target tracking and vision-based obstacle avoidance problems.

II.

Problem Formulation

Let X v , V v and av be a vehicle’s position, velocity and acceleration vectors in an inertial frame respectively. Consider the following simple linear dynamics for the vehicle. ˙ v (t) = V v (t), X

V˙ v (t) = av (t)

(1)

It is assumed that all the vehicle’s states are available through the own-ship navigation system. The target dynamics are similarly given by ˙ t (t) = V t (t), V˙ t (t) = at (t) X (2) where X t , V t and at are the target’s position, velocity and acceleration in the inertial frame. Nonaccelerating target, i.e., at (t) = 0 is assumed for simplicity. Then the relative motion dynamics of the target with respect to the vehicle are ˙ X(t) = V (t),

V˙ (t) = −av (t)

(3)

where X and V are the relative position and velocity in the inertial frame defined by X(t) = X t (t) − X v (t),

V (t) = V t (t) − V v (t)

Suppose that a 2-D passive vision sensor is fixed at the center of gravity of the vehicle, and that the image processor, which is able to detect the target’s location in each frame of images, is available. Let Lc (t) denote a known camera attitude represented by a rotation matrix from the inertial frame to a camera frame. The camera frame is taken so that the camera’s optical axis aligned with its Xc axis. Then the relative position vector in the camera frame will be X c (t) = Lc (t)X(t) = [ Xc

Yc

Zc ]

T

(4)

Assuming a pin-hole camera model as shown in Figure 1, the target position in the image frame at a time step tk is given by · ¸ · ¸ f Yc (tk ) y(tk ) h (X c (tk )) = = (5) z(tk ) Xc (tk ) Zc (tk ) where f is the focal length of the camera. In this paper, f = 1 is used without loss of generality. The image processor outputs the target’s position in the image frame with measurement noise. z k = h (X c (tk )) + ν k

(6)

where ν k is a zero mean white Gaussian noise with covariance matrix Rk . Since the vision-based measurement error depends on a range to the target in the camera’s optical axis direction, we will suppose Rk =

σ2 I Xc2 (tk )

A goal of the vision-based control problem is to guide the vehicle to achieve given missions by using the image processor output (6) and the known vehicle’s states. Suppose that the mission given to the vehicle is represented as the following minimization problem. Z i 1 tf h 1 T T (x(t) − xd (t)) A (x(t) − xd (t)) + aTv (t)Bav (t) dt (7) min J = (x(tf ) − xf ) Sf (x(tf ) − xf ) + av 2 2 t0 where A ≥ 0, B > 0 and Sf ≥ 0. x is the relative state vector defined by · ¸ X(t) x(t) = V (t) xd (t) is the desired relative motion and xf is the desired relative state at the terminal time tf . 3 of 16 American Institute of Aeronautics and Astronautics

Figure 1. Pin-Hole Camera Model

III.

Relative Navigation System

In this section, the vision-based relative navigation system is designed to estimate the relative state x(tk ) from the image processor output z k given in (6). Since the vision-based measurement z k is nonlinear with respect to the relative state x(tk ), an extended Kalman filter (EKF) is applied. The Kalman filter is a recursive solution to the least-square method for a linear filtering problem,26 and an EKF is an extension of the standard Kalman filter so that it can be applied to nonlinear systems by linearizing the system about the predicted estimate at each time step.10,11 The relative dynamics (3) are rewritten in terms of x. · ¸ · ¸ O I O ˙ x(t) = x(t) + av (t) = F x(t) + Gav (t) (8) O O −I This linear system is discretized as follows. xk+1 = Φk xk + Gk avk where

·

Φk = e

F (tk+1 −tk )

I = O

¸ (tk+1 − tk )I , I

Z

tk+1

Gk =

F (tk+1 −t)

e

(9) ·1 Gdt = −

tk

2 2 (tk+1 − tk ) I (tk+1 − tk )I

¸

Then the EKF is formulated to estimate the relative state at the time step tk , xk from the vision-based measurement (6). The EKF includes the following two procedures: prediction and update. Prediction: ˆ− x k+1 − Pk+1

ˆ k + Gk avk = Φk x =

Φk Pk ΦTk

(10) (11)

Update: ˆ k+1 x Pk+1 Kk+1

³ ´ ˆ− ) ˆ− = x + K z − h( X k+1 k+1 ck+1 k+1 − = (I − Kk+1 Hk+1 ) Pk+1 ¡ ¢−1 − − T T = Pk+1 Hk+1 Hk+1 Pk+1 Hk+1 + Rk+1


(12) (13) (14)

ˆ− ˆ k denote the predicted and updated estimates of the relative state x at the time step tk , and Pk− x k and x and Pk are their error covariance matrices. Kk+1 given by (14) is called Kalman gain matrix. Hk+1 is called a measurement matrix and it is defined by Hk+1 =

h 1 £ i ¤ ∂h(X c (tk+1 )) ¯¯ ˆ − ) I Lc (tk+1 ) O −h( X − = ¯ ˆ ck+1 Xck+1 ˆ− ∂x(tk+1 ) x(tk+1 )=x k+1

(15)

ˆ − = Lc (tk+1 )X ˆ k+1 . Rk+1 is a covariance matrix of the measurement error ν k+1 and we will use where X ck+1 Rk+1 =

σ2 I 2 ˆ c−k+1 X

(16)

Since the camera’s field of view is limited and the image processor may sometimes fail to capture the target, the vision-based measurement is not always available. When this happens, only the EKF prediction procedure (10-11) is performed. The absence of a measurement corresponds to having a measurement having an infinitely large noise. When Rk+1 = ∞ in (14), the Kalman gain becomes zero and it results in − ˆ k+1 = x ˆ− x k+1 and Pk+1 = Pk+1 . Once the estimated relative position and velocity are obtained, the estimated target’s state can be calculated by adding the known vehicle’s state. ˆ t (tk ) = X ˆ k + X v (tk ), X

Vˆ t (tk ) = Vˆ k + V v (tk )

(17)

These estimates will be fed back to the guidance system.

IV.

Guidance Design

This section discusses a stochastically optimized guidance design for vision-based mission achievement. The navigation filter designed in the previous section is used for relative state estimation, and the estimates are fed back to the guidance system. For monocular vision-based relative navigation, the estimation performance highly depends on sensor motion relative to the target.16 Therefore, this paper suggests a guidance design which includes a sensor trajectory optimization to improve estimation accuracy, and hence improve overall mission achievement accuracy. A.

Stochastic Optimization

A mission given to the vehicle is formulated as a minimization of (7) subject to the linear dynamics (3). Consider the problem of trying to determine the acceleration input at the time step tk , given the current ˆ k and its error covariance matrix Pk . Suppose the true state xk is available, an optimal updated estimates x solution of (7) can be obtained by solving the Hamilton-Jacobi-Bellman (HJB) equation.27 To simplify we consider the special case of terminal tracking, i.e., A = O in this paper. Then a closed form of the optimal solution and the optimal cost can be derived as follows. ³ ´ T −1 a∗v (tk ) = −B −1 GT eF (tf −tk ) Sf (I + Gk Sf ) eF (tf −tk ) xk − xf (18) ³ ´ ³ ´ T 1 F (tf −tk ) −T J ∗ (tk ) = e xk − xf (I + Gk Sf ) Sf eF (tf −tk ) xk − xf (19) 2 where

Z

tf

Gk =

eF (tf −s) GB −1 GT eF

T

(tf −s)

ds

tk

Note that Gk > 0 when (F, G) in (8) is controllable and B > 0.28 The optimal solution (18) is called LQR controller.27 Since the true state is not accessible in reality, the optimal guidance input (18) cannot be realized. When the measurement model is linear, an optimal estimator and controller can be designed independently and it results in the standard linear Kalman filter and LQG controller.28,29 However, the vision-based measurement given in (6) is nonlinear function of the state. For such a case the separation principle does not hold between estimation and control, and closed form solutions are not available. A common way to determine a guidance 5 of 16 American Institute of Aeronautics and Astronautics

input is to replace the true state by its estimate in (18). We will refer to this approach as an estimated optimal guidance design in this paper. ³ ´ T ˆ ∗v (tk ) = −B −1 GT eF (tf −tk ) Sf (I + Gk Sf )−1 eF (tf −tk ) x ˆ k − xf a (20) = a∗v (tk ) + B −1 GT eF

T

(tf −tk )

−1 F (tf −tk )

Sf (I + Gk Sf )

e

˜k x

˜ k = xk − x ˆ k is the estimation error at tk . The expected cost associated with the estimated optimal where x guidance design is i T 1 h −T −1 (21) Jˆ∗ (tk ) = E [J(tk )] = J ∗ (tk ) + tr Pk eF (tf −tk ) (I + Gk Sf ) Sf (I + Gk Sf ) eF (tf −tk ) 2 £ ¤ ˜kx ˜ Tk = Pk . The second term in (21) corresponds to an increase in cost due assuming E [˜ xk ] = 0 and E x ˜ k . Since the optimization is performed by assuming zero estimation error, this to the estimation error x guidance policy can cause poor guidance performance when the estimation error is large. As stated before, the vision-based estimation performance significantly depends on the relative motion of the camera (which is fixed to the vehicle in our problem) to the target that is created by the guidance policy. Hence, the overall guidance performance can be improved by adding some relative motion. This paper suggests designing a guidance law by stochastically minimizing J under the condition of using the EKF-based navigation system designed in Section III. £ ¤ ˜ (t)˜ Assuming E [˜ x(t)] = 0 and E x xT (t) = P (t) for ∀ t ≥ tk , the stochastic optimized guidance design can be formulated as follows. Z 1 1 tf T 1 T min E [J] = (ˆ x(tf ) − xf ) Sf (ˆ x(tf ) − xf ) + av (t)Bav (t)dt + tr [P (tf )Sf ] (22) av 2 2 tk 2 subject to the EKF prediction and update procedures (10-14). This problem is highly nonlinear and its analytical solution can not be obtained. There are numerous numerical optimization algorithms, such as dynamic programming. However, these algorithms require iterative computations and thus they are not suitable for realtime implementation. The next subsection discusses a suboptimal optimization strategy which is applied to derive an approximately optimal solution without iterative computations. B.

One-Step-Ahead Suboptimal Optimization

This subsection focuses on deriving a realtime applicable algorithm to solve the stochastic optimization problem given in (22). The one-step-ahead (OSA) suboptimal optimization technique is introduced by Logothetis et al.25 In this technique, at the time step tk , the optimization is performed under the assumption that the observer anticipates only one more final measurement at one time step ahead tk+1 . They applied the OSA-based suboptimal strategy to minimize several different estimation performance costs chosen in a heuristic way for a bearing-only tracking problem. This paper applies it to solve the suboptimal solution for (22). ˆ k and Pk at the time step tk , let Given x ˆ ∗v (tk ) + ∆a av (tk ) = a

(23)

ˆ ∗v (tk ) denotes the estimated optimal guidance law given be the vehicle acceleration input at tk . The input a in (20) and ∆a is an additional input which is for the estimation improvement. Under the OSA optimization assumption, there will be only one more measurement available at the next time step. Therefore, the estimation accuracy will not be improved after tk+1 . It means that what can be done after tk+1 is only to ˆ k+1 . apply the estimated optimal guidance law which is recalculated at tk+1 using the updated estimate x ³ ´ −1 −1 T F T (tf −t) ˆ k+1 − xf ˆ ∗∗ G e Sf (I + Gk+1 Sf ) eF (tf −tk+1 ) x (24) av (t) = a v (t) = −B ˆ k , its for tk+1 ≤ t < tf . Then the expected cost E [J] at tk becomes a function of the current estimate x error covariance matrix Pk and the additional acceleration input ∆a. # "Z tf i 1 1 h T ˆ ∗∗ ˆ ∗∗T E [J] (tk ) = E (x(tf ) − xf ) Sf (x(tf ) − xf ) + E a v (t)dt v (t)B a 2 2 tk+1 6 of 16 American Institute of Aeronautics and Astronautics

1 + E 2 '

·Z

tk+1 tk

¸ (ˆ a∗v (tk )

T

+ ∆a) B

(ˆ a∗v (tk )

+ ∆a) dt

³ ´ T 1 −T Jˆ∗ (tk ) + ∆aT GTk eF (tf −tk+1 ) (I + Gk+1 Sf ) Sf eF (tf −tk+1 ) Gk + Bk ∆a 2 i 1 h F T (tf −tk+1 ) −1 − − tr e Sf Gk+1 Sf (I + Gk+1 Sf ) eF (tf −tk+1 ) (Pk+1 − Pk+1 ) 2

(25)

where Bk = B(tk+1 − tk ). The second term in (25) represents an increase in control cost due to the additional input ∆a and the third term represents a decrease in terminal tracking error due to the estimation improvement by the measurement update at tk+1 . Now, our guidance policy is to find ∆a which maximizes the decrease in cost. Hence, the solution for ∆a can be obtained by solving the following algebraic equation. ´ ∂ ³ ˆ∗ J (tk ) − E [J] (tk ) = 0 ∂∆a

(26)

From the EKF update law of the error covariance matrix (13), we have − Pk+1 − Pk+1

= =

− Kk+1 Hk+1 Pk+1 ¡ ¢−1 − − − T T Pk+1 Hk+1 Hk+1 Pk+1 Hk+1 + Rk+1 Hk+1 Pk+1

(27)

where Pk− = Φk+1 Pk ΦTk from the EKF prediction law (11). Define Bk+1 Sk+1

T

(tf −tk+1 )

−T

=

GTk eF

=

T − Pk+1 eF (tf −tk+1 ) Sf Gk+1 Sf

(I + Gk+1 Sf )

Sf eF (tf −tk+1 ) Gk + Bk −1 F (tf −tk+1 )

(I + Gk+1 Sf )

e

(28) − Pk+1

(29)

Then the equation (26) can be rewritten as follows. i ´ ¡ ¢−1 ∂ ³ h − T T tr Sk+1 Hk+1 Hk+1 Pk+1 Hk+1 + Rk+1 Hk+1 − ∆aT Bk+1 ∆a = 0 ∂∆a · i¸T h ¢−1 ¡ ∂ 1 −T − T T + Rk+1 Hk+1 tr Sk+1 Hk+1 Hk+1 Pk+1 Hk+1 ∴ ∆a = Bk+1 2 ∂∆a

(30)

Hk+1 is the measurement matrix defined by (15), which is a function of the predicted estimate at tk+1 . ˆ− x k+1

=

ˆ k + Gk av (tk ) = Φk x ˆ k + Gk (ˆ Φk x a∗v (tk ) + ∆a)

=

ˆ ∗− x k+1 + Gk ∆a

(31)

Hence, the right hand side of (30) becomes a function of ∆a. To simplify the calculation, we will approximate the solution for (30) by · h i¸T ¡ ¢−1 1 −T ∂ − T T ∆a ' Bk+1 tr Sk+1 Hk+1 Hk+1 Pk+1 Hk+1 + Rk+1 Hk+1 2 ∂∆a

¯ ¯ ¯

ˆ− ˆ ∗− x k+1 =xk+1

(32)

From the form of (32), it can be said that the Sk+1 matrix plays the role of weighting the additional input ∆a. Since Sk+1 is a quadratic function of the current estimation error covariance Pk by definition (29), the resulting ∆a is small when having accurate estimation and it is large when having poor estimation. This is reasonable because the vehicle does not need to create the extra maneuver to improve the estimation when the estimate is already sufficiently accurate.

V.

Applications

The vision-based navigation and guidance systems designed above have been applied to two applications. The first application is a vision-based rendezvous problem with a stationary target. The second application is a combined mission of waypoint tracking and vision-bsaed obstacle avoidance. Simulation results for each application are shown in this section.


A. 1.

Vision-Based Rendezvous with Stationary Target Mission

A mission of the rendezvous with a stationary target at a given terminal time tf can be formulated as a minimization problem given in (7) with the following parameters. · ¸ s I O A = O, B = I, Sf = x (sx > 0, sv ≥ 0), xf = 0 (33) O sv I Since it is known that the target is stationary in this application, the EKF is formulated to estimate the relative position of the target with respect to the vehicle. Therefore, the estimation error covariance matrix Pk and the measurement matrix Hk+1 in the EKF are 3 × 3 and 2 × 3 matrices respectively, and we need to T replace the Pk+1 and Hk+1 matrices in (30) by · − ¸ Pk+1 O − Pk+1 = , Hk+1 = [ Hk+1 O ] O O 2.

Guidance Design

The estimated optimal guidance law with the parameters given in (33) results in the following linear feedback controller. ´ ³ ´ i 1 h³ sx sv ˆ k − sx (tf − tk )2 + sx sv (tf − tk )3 + sv V v (tk ) ˆ ∗v (tk ) = (tf − tk )2 X sx (tf − tk ) + (34) a Λk 2 3 where

sx sx sv (tf − tk )3 + (tf − tk )4 + sv (tf − tk ) (35) 3 12 ˆ ∗v (tk ) in order to create extra In the OSA suboptimal guidance design, an additional input ∆a is added to a motion which improves the estimation performance. Bk+1 and Sk+1 are expressed as · ¸ βk+1 sk+1 Pk2 O Bk+1 = I, Sk+1 = (36) Λk+1 Λk+1 O O Λk = 1 +

where βk+1

=

sk+1

=

µ ¶ tk+1 − tk Λk+1 (tk+1 − tk ) + sx (tk+1 − tk )2 (tf − tk+1 ) + 2 µ ¶ 2 (tf − tk+1 ) (tf − tk+1 )(tk+1 − tk ) (tk+1 − tk )2 +sx sv (tk+1 − tk )2 (tf − tk+1 ) + + 3 2 4 ³ ´ 2 sx sv (tf − tk+1 )3 1 + (tf − tk+1 ) 3 4

By substituting (15) and (16) and defining Pck+1 = Lc (tk+1 )Pk LTc (tk+1 ),

£ ¤ ˆ− ) I , Hk+1 = −h(X ck+1

the equation (32) can be rewritten by · ´¸ ¯ ¢ sk+1 ∂ ³ T ¡ ¯ 2 T 2 −1 ∆ai ' tr Pck+1 Hk+1 Hk+1 Pck+1 Hk+1 + σ I Hk+1 ¯ ˆ − ˆ ∗− 2βk+1 ∂∆ai X k+1 =X k+1 · µ ¶¸ ¯ ¡ ¢ ∂H sk+1 −1 ¯ k+1 T T tr Pc2k+1 Hk+1 Hk+1 Pck+1 Hk+1 + σ2 I = ¯ˆ− ˆ ∗− βk+1 ∂∆ai X k+1 =X k+1 " # ¢ ¡ ¯ 2 −1 T ∂ Hk+1 Pck+1 Hk+1 + σ I sk+1 ¯ T tr Pc2k+1 Hk+1 Hk+1 ¯ ˆ − i = 1, 2, 3 + ˆ ∗− , 2βk+1 ∂∆ai X k+1 =X k+1

(37)

(38)

The derivative in the right hand side of (38) can be computed as follows. (tk+1 − tk )2 ∂Hk+1 = [ Hk+1 ei ∂∆ai 2

O]


(39)

Ã ! ¡ ¢−1 · ¸µ ¶−1 µ ¶ T ∂ Hk+1 Pck+1 Hk+1 + σ2 I 1 ∂Γk+1 ∂Γk+1 ∂ detΓk+1 −1 = det − Γk+1 ∂∆ai detΓk+1 ∂∆ai ∂∆ai ∂∆ai ¡ ¢ T where Γk+1 = Hk+1 Pck+1 Hk+1 + σ 2 I and ∂Γk+1 ∂∆ai ∂ detΓk+1 ∂∆ai

µ

(40)

µ ¶T ∂Hk+1 T Pck+1 Hk+1 + Hk+1 Pck+1 ∂∆ai ¸ · µ ¶¸ · µ ¶ ∂Γ ∂Hk+1 k+1 −1 T = (detΓk+1 ) tr Γ−1 = 2 (detΓ ) tr Γ P H k+1 c k+1 k+1 k+1 k+1 ∂∆ai ∂∆ai =

∂Hk+1 ∂∆ai

¶

ˆ ∗− , the additional input ∆a can be calculated by By evaluating the matrices Hk+1 and Hk+1 in (38) at X k+1 using the current estimates, its error covariance and known vehicle states. 3.

Simulation Results

The simulation results for the vision-based rendezvous are compared between two guidance policies: the OSA suboptimal guidance and the estimated optimal guidance. The vehicle is initially located at the origin T T with its velocity as V v (0) = [ 10 2 0 ] (ft/sec), and the target is fixed at X t = [ 100 20 20 ] (ft). The vehicle’s mission is to make a rendezvous with the target at time tf = 20 (sec). For the rendezvous mission, sx = 100 and sv = 10 are given. Since the target is known to be stationary, the EKF-based navigation system is designed to estimate only the relative position. The initial estimation error is −20 (ft) in each axis, and the initial error covariance is taken as P0 = 202 I (ft2 ). For the measurement noise covariance matrix, σ = 1 is used. Figure 2 shows the vehicle trajectory, and Figure 3 and 4 are the vehicle velocity and acceleration input which are generated by those two guidance laws. Figure 5 and 6 present the target’s position estimation error and its standard deviation. Figure 7 summarizes the total cost which consists of the terminal tracking error and the control cost for the entire mission. When using the estimated optimal guidance policy, the vehicle approaches almost straight to the target. Range observability is lost in such a case, and a large bias in the position estimation error remains. Due to the large bias, the vehicle fails to rendezvous with the target and its final miss distance is about 14.8 (ft). On the other hand, the OSA suboptimal guidance law creates lateral motions to maintain the observability and hence to improve the estimation performance, and it enables the vehicle to achieve the rendezvous mission with high accuracy (within 0.03 (ft)). Even though the control cost increases due to the additional input ∆a, the terminal tracking error and the total cost are significantly reduced by using the OSA suboptimal guidance policy suggested in this paper.

OSA Suboptimal Estimated Optimal Target

100 90 20

80 70

Z

15

60

10

50 40

5

X

30 20

0 20

10

Y

10 0

0

Figure 2. Vehicle Trajectory and Target Location


Vx

2

OSA Suboptimal Estimated Optimal

10

0

ax

5

0 −2

0

5

10

15

20

−4

2

0

5

10

15

20

0

5

10

15

20

0

5

10

1

ay

Vy

0 1

−1 −2

0

0

5

10

15

−3

20

2

1

az

Vz

0 1

−1 OSA Suboptimal Estimated Optimal

−2 0

0

5

10

15

−3

20

time

Figure 3. Vehicle Velocity

20

Figure 4. Vehicle Acceleration

10


σ

x

0

ex

15

time

5

−10 0 −20

0

5

10

15

0

5

10

15

20

0

5

10

15

20

0

5

10

15

20

20 1

σy

ey

0 −2

0.5

−4 0

10

5

10

15

0

20

σz

1

−2 0

15

20

0.5 0

time

time

Figure 5. Position Estimation Error

Figure 6. Error Standard Deviation

6 Terminal Tracking Error OSA Suboptimal: 0.0872 Estimated Optimal: 10968

5

C

−4

J : Control Cost

ez

0

5 OSA Suboptimal Estimated Optimal

Total Cost OSA Suboptimal: 5.2442 Estimated Optimal: 10971

4

3

2

1 OSA Suboptimal Estimatd Optimal 0

0

5

10

15

20

time Figure 7. Control Cost, Terminal Tracking Error and Total Cost


B. 1.

Waypoint Tracking and Vision-Based Obstacle Avoidance Mission

In the second application, a combined mission of waypoint tracking and vision-based obstacle avoidance is given to a vehicle. The vehicle is required to visit a given waypoint while avoiding unforeseen obstacles on its way by using a 2-D vision sensor. In this example, a constant known speed u in the inertial X-direction T is assumed. Since the waypoint position X wp = [ Xwp Ywp Xwp ] is known, the terminal time is given by Xwp − Xv (0) Xwp − Xv (tk ) tf = = tk + (41) u u Let xv (t) = [ X Tv (t)

T

V Tv (t) ] , then the waypoint tracking problem is formulated as follows.

1 1 T min Jwp = (xv (tf ) − xf ) Sf (xv (tf ) − xf ) + av 2 2

Z

tf 0

aTv (t)Bav (t)dt

(42)

where  ∞ 0 0 B =  0 1 0, 0 0 1 

·

s I Sf = x O

O sv I

¸

·

(sx > 0, sv ≥ 0),

¸ X wp xf = , Vf

Vf

  u = 0 0

Since the vehicle’s states and the waypoint position are completely known, the optimal guidance can be realized if there is no obstacle on its way. However, if there is an obstacle which is critical to the vehicle, the vehicle needs to take some avoiding maneuvers. Stationary point obstacles are assumed in this problem, and the vehicle is always required to maintain a minimum separation distance d from every obstacles in order to avoid a collision with them. Same as in the first application, the navigation system is designed to estimate the obstacles’ position from the vision-based measurements of their position in each image frame. 2.

Guidance Design

Suppose there is no obstacle, then the optimal solution for the waypoint tracking problem given by (42) is realized. The solution is given by a∗vwp (tk ) =

1 Λk 1 − Λk −

³

´ sx sv (tf − tk )2 (X v (tk ) − X wp ) 2 ³ ´ s x sv sx (tf − tk )2 + (tf − tk )3 + sv (V v (tk ) − V f ) 3 sx (tf − tk ) +

(43)

where Λk is the same as defined by (35). For obstacle avoidance, first we need to determine if there is any critical obstacle from the vision-based estimated obstacle positions. For collision criteria, a collision cone approach introduced by Chakravarthy and Ghose30 is applied. The original collision cone approach is restricted in a 2-D plane and it is extended to the 3-D case in this problem.31 Let X obs denote an obstacle position. To apply the 2-D collision cone approach to the 3-D case, we only look at a 2-D plane created by a relative position vector (X obs − X v (tk )) and the vehicle velocity vector V v (tk ). Since the obstacle is a stationary point and the minimum separation distance is constant (d), the obstacle’s collision safety boundary is defined by a circle with its center at the obstacle’s position and a radius d on the 2-D plane. Then, the collision cone is defined as shown in Figure 8. The obstacle is considered to be critical if the vehicle velocity vector lies within its collision cone. In such a case, the vehicle will collide with the obstacle if no control is applied. If the collision cone criteria is satisfied, an aiming point X ap is specified at a tangential point of the collision cone and the obstacle’s safety boundary (Figure 8). Once the aiming point is specified, the obstacle avoidance mission coincides with the aiming point tracking problem. Therefore, a similar guidance policy from the waypoint tracking problem in the previous subsection can be applied. Define the relative state as · ¸ X obs − X v (t) x(t) = −V v (t)


Figure 8. Collision Cone and Aiming Point

Then the aiming point tracking problem will be formulated as follows. Z 1 1 tap T T min Jap = (x(tap ) − xap ) Sap (x(tap ) − xap ) + av (t)Bav (t)dt av 2 2 tk

(44)

where tap

Xap − Xv (tk ) = tk + , u

·

Sap

s I = xap O

O svap I

¸

·

(sxap > 0, svap ≥ 0),

xap

X obs − X v (tk ) = −V f

¸

The estimated optimal guidance and the OSA suboptimal guidance laws can be derived in the same manner as done in the previous example of the vision-based target tracking. When there are more than two critical obstacles, the vehicle will take avoiding maneuvers for the closest one. 3.

Simulation Results

Simulation results for the two different guidance policies are compared. The vehicle is initially at the origin T with its velocity V v (0) = [ 10 2 2 ] (ft/sec), and a constant speed in the X-direction is u = 10 (ft/sec). T A waypoint is given at X wp = [ 100 20 20 ] (ft). There exist two unforeseen obstacles on the way to T T the waypoint: Obstacle 1 at X obs1 = [ 30 6 6 ] (ft) and Obstacle 2 at X obs2 = [ 60 20 15 ] (ft). The minimum separation distance from obstacles is d = 10 (ft). The initial estimation error is +50% of the true relative position for each obstacle, and its initial error covariance matrix is P0 = 202 I (ft2 ). For the tracking missions, sx = sxap = 100 and sv = svap = 10 are used. Figure 9 shows the vehicle trajectory, obstacle locations and their safety boundaries, and the waypoint location. Figure 10 and 11 are the vehicle velocity and acceleration. Figure 12 presents a time profile of distances between the vehicle and each obstacle. Figure 13 shows the control cost. From Figure 12, we can see that there is a violation of Obstacle 1’s safety boundary when the estimated optimal guidance law is used, and that the violation does not occur when using the OSA suboptimal guidance. Figure 14 and 15 show the estimation error of each obstacle position and its standard deviation. From these figures, it is clear that the convergence of the estimation error is significantly improved by introducing the additional guidance input ∆a in the suggested guidance design. The improvement in estimation results in the improvement in the guidance performance of obstacle avoidance.


OSA Suboptimal Estimated Optimal Waypoint Obstacle Position Obstacle Safety Boundary

Obstacle2 20

Z

100 10

80 60

Obstacle1

0 40

40 20

X

20 0

Y

0

Figure 9. Vehicle Trajectory and Obstacles

10


ay

Vy 0

−5

0 −10

0

2

4

6

8

−20

10

8

0

2

4

0

2

4

6

8

10

6

8

10

20

6

10

4

az

Vz


10

5

0

2 −10

0 −2

0

2

4

6

8

−20

10

time

time

Figure 10. Vehicle Velocity

Figure 11. Vehicle Acceleration


80 OSA Suboptimal: Obstacle1 OSA Suboptimal: Obstacle2 Estimated Optimal: Obstacle1 Estimated Optimal: Obstacle2

Distance to Obstacles

70 60 50 40 30 20 10

Minimum Distance 0

0

2

4

6

8

10

time Figure 12. Distance from Obstacles


80

Jc: Control Cost

70 60 50 40 30 20 10 0

0

2

4

6

8

time Figure 13. Control Cost


10

OSA Suboptimal: Obstacle1 OSA Suboptimal: Obstacle2 Estimated Optimal: Obstacle1 Estimated Optimal: Obstacle2

x

20

OSA Suboptimal: Obstacle1 OSA Suboptimal: Obstacle2 Estimated Optimal: Obstacle1 Estimated Optimal: Obstacle2

20

σ

ex

40

10

0 0

2

4

6

8

0

10

20

0

2

4

6

8

10

0

2

4

6

8

10

0

2

4

6

8

10

8

σy

ey

6 10

0

2

4

6

8

0

10

15

6

σz

ez

10 5

4 2

0 −5

4 2

0

0

2

4

6

8

0

10

time

time

Figure 14. Position Estimation Error

VI.

Figure 15. Error Standard Deviation

Conclusion

This paper proposes a suboptimal guidance design for UAVs for vision-based control applications. The suggested approach approximately maximizes the expected guidance performance stochastically, under the condition that the EKF is used to estimate relative states to target from vision-based measurement. To reduce the computation in optimization, the idea of one-step-ahead suboptimal optimization is implemented. The overall vision-based navigation and guidance system has been applied to the vision-based target tracking problem and vision-based obstacle avoidance problem. By comparing the simulation results with those of the conventional guidance design, a significant improvement in the estimation and guidance performance was observed. For future work, we would like to compare the results with the numerically obtained optimal solution of (22). In addition, we would like to investigate deriving the derivative in (32) numerically. Even in the simple case we have seen in the target tracking problem, obtaining the derivative is computationally intensive.

VII.

Acknowledgement

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

References 1 E. Bone and C. Bolkcom. ”Unmanned Aerial Vehicles: Background and Issues for Congress” Technical Report RL31872, Congressional Research Service. The Library of Congress. 2003. 2 B.T. Clough. ”Unmanned Aerial Vehicles: Autonomous Control Challenges, a Researcher’s Prospective” AIAA Journal of Aerospace Computing, Information and Communication, 2(8). 2005. 3 J.R. Miller and O. Amidi. ”3-D Site Mapping with the CMU Autonomous Helicopter” The 5th International Conference on Inteligent Autonomous Systems. 1998. 4 A. Wu, E.N. Johnson and A.A. Proctor. ”Vision-Aided Inertial Navigation for Flight Control” Journal of Aerospace Computing, Information, and Communication, 2(9). 2005. 5 A. Koch, H. Wittch and F. Thielecke. ”A Vision-Based Navigation Algorithm for a VTOL-UAV” AIAA Guidance, Navigation and Control Conference. 2006. 6 B. Sinopoli, M. Micheli, G. Donato and T.J. Koo. ”Vision-Based Navigation for an Unmanned Aerial Vehicle” IEEE International Conference on Robotics and Automation. 2001. 7 A.A. Proctor and E.N. Johnson. ”Vision-Only Aircraft Flight Control Methods and Test Results” AIAA Guidance, Navigation and Control Conference. 2004. 8 E.N. Johnson, A.J. Calise, R. Sattigeri and Y.Watanabe. ”Approaches to Vision-Based Formation Flight” IEEE Conference on Decision and Control. 2004. 9 J. Valasek, J. Kimmett, D. Hughes, K. Gunnam and J. Junkins. ”Vision Based Sensor and Navigation System for Autonomous Aerial Refueling” AIAA the 1st UAV Conference. 2002. 10 R.G. Brown and P.Y.C. Hwang. ”Introduction to Random Signals and Applied Kalman Filtering” John WIley & SOns. 1997.


11 P.

Zarchan and H. Musoff. ”Fundamentals of Kalman Filtering: A Practical Approach” AIAA. 2004. Rao, N.L. Phillips, S.J. Fu and N.M. Conrardy. ”Horizontal Plane Trajectory Optimization for Threat Avoidance and Waypoint Randezvous” IEEE Aerospace and Electoronics Conference. 1990. 13 J.T. Betts. ”Survey of Numerical Methods for Trajectory Optimization” AIAA Journal of Guidance, Control and Dynamics, 21(2). 1998. 14 S. Belkhous, A. Azzouz, C. Nerquizian, M. Saad and V. Nerquizian. ”Trajectory Optimization in Both Static and Dynamic Environments” IEEE International Conference on Industrial Technology. 2004. 15 L. Blackmore. ”Probablistic Particle Control Approach to Optimal, Robust Predictive Control” AIAA Guidance, Navigation and Control Conference. 2006. 16 L. Metthies and T. Kanade. ”Kalman Filter-based Algorithms for Estimating Depth from Image Sequences” International Journal of Computer Vision. 1989. 17 S.E. Hammel, P.T. Liu, E.J. Hillard and K.F. Gong. ”Optimal Observer Motion for Localization with Bearing Measurements” Computers and Mathmatics with Applications, 18(1-3). 1989. 18 Y. Oshman and P. Davidson. ”Optimal Observer Trajectories for Passive Target Localization using Bearing-Only Measurement” AIAA Guidance, Navigation and Control Conference. 1996. 19 Y. Oshman and P. Davidson. ”Optimization of Observer Trajectories for Bearings-Only Target Localization” IEEE Transactions on Aerospace and Electronic Systems, 35(3). 1999. 20 A. Logothetis, A. Isaksson and R.J. Evans. ”An Information Theoretic Approach to Observer Path Design” IEEE Conference on Decision and Control. 1997. 21 E.W. Frew and S.M. Rock. ”Trajectory Generation for Constant Velocity Target Motion Estimating using Monocular Vision” AIAA Guidance, Navigation and Control Conference. 2003. 22 S.S. Singh, N.Kantas, B. Vo, A. Doucet and R.J. Evans. ”Simulation-Based Optimal Sensor Scheduling with Application to Observer Trajectory Planning” Automatica. 2005. 23 Y. Watanabe, E.J. Johnson and A.J. Calise. ”Vision-Based Guidance Design from Sensor Trajectory Optimization” AIAA Guidance, Navigation and Control Conference. 2006. 24 J. Kim and S. Rock. ”Stochastic Feedback Controller Design Considering the Dual Effect” AIAA Guidance, Navigation and Control Conference. 2006. 25 A. Logothetis, A. Isaksson and R.J. Evan. ”Comparison of Suboptimal Strategies for Optimal Own-Ship Maneuvers in Bearings-Only Tracking” American Control Conference. 1998. 26 R.E. Kalman. ”A New Appraoch to Linear Filtering and Prediction Problems” Transaction of the ASME - Journal of Basic Engineering, 82. 1962. 27 E. Bryson and Y. Ho. ”Applied Optimal Control” Taylor & Francis. 1975. 28 D.S. Bernstein and W.M. Haddad. ”Control-System Synthesis: The Fixed-Structure Approach” Georgia Institute of Technology. 1995. 29 K. Zhou, J.C. Doyle and K. Glover. ”Robust and Optimal Control” Prentice Hall. 1996. 30 A. 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, 28(5). 1998. 31 Y. Watanabe, A.J. Calise and E.N. Johnson. ”Minimum-Effort Guidance for Vision-Based Collision Avoidance” AIAA Atmospheric Flight Mechanics Conference. 2006. 12 N.S.
