A Centralized Control Algorithm for Target Tracking with UAVs

Transcription

1 A Centralized Control Algorithm for Tracing with UAVs Pengcheng Zhan, David W. Casbeer, A. Lee Swindlehurst Dept. of Elec. & Comp. Engineering, Brigham Young University, Provo, UT, USA, 8462 Telephone: (81) , Fax: (81) Abstract Due to their wide range of practical applications, Unmanned Air Vehicles (UAVs) have recently attracted considerable attention in the research community. In this paper, we focus on their use in a simple target tracing application, and tae advantage of their maneuverability to improve tracing performance. A centralized control point is used to command the headings of the UAVs in order to minimize a target localization criterion based on the Kalman filter. A practical model for the system s architecture is presented, along with simulation results that show the algorithm significantly improves estimation of the target s parameters compared with the uncontrolled system. I. INTRODUCTION The use of Unmanned Air Vehicles (UAVs) has gained considerable momentum given their success in recent military operations, and their promise for important domestic applications such as border and coast patrol, fire perimeter monitoring, search and rescue, etc. The cooperation of teams of UAVs can be used to accomplish tass that are dangerous for human operators, and they can reduce operational costs by implementing tass using smaller and cheaper UAVs rather than larger more expensive ones. In this paper, we consider the use of a team of UAVs for tracing a maneuvering target, a problem that has recently received some attention in the research literature. In [1], the optimal set of UAV locations is derived based on the wellnown Cramer-Rao bound. However, the results are derived under two assumptions that may not be realistic in practice: (1) the UAVs are able to form local estimates of the target position themselves, and (2) the UAVs are equi-distant to the target. In [2], [3], [4] the trajectories of a team of mobile sensors are optimized assuming a bearings-only measurement model, and [5] extends this wor by adding range measurements to the problem. However, small UAVs may have a difficult time maing accurate bearing measurements given their small aperture and the fact that they are much more sensitive to wind-blown disturbances. In our wor, we assume a multistatic scenario in which a basestation, either airborne or on the ground, illuminates the target with a radar signal, and the UAVs are able to mae time-delay and Doppler measurements of the target s reflected signal. The UAVs feed these measurements to the base, which is running an Extended Kalman Filter (EKF) to trac the This wor was supported by the U. S. National Science Foundation under Information Technology Research Grant CCR target s position and velocity. The base then gives heading commands to each UAV in order to minimize some function of the EKF error, and hence more accurately trac the target. In effect, the maneuverability of the UAVs provide the base with an EKF whose parameters are tunable to minimize the target state estimation error. Our approach can be considered to be a special but more tractable case of the sensor management problem posed in [6]. The format of the paper will be as follows: in Section II we set up the problem and describe the EKF and the assumed dynamic models. Section III describes two algorithms for determining optimal UAV heading angles based on the EKF updates. Section IV presents the results of a simple simulation, and some conclusions are drawn in Section V. II. SYSTEM ARCHITECTURE A. Problem Statement As mentioned above, we consider a multistatic radar problem in which a set of N UAVs receives the signal reflected by a mobile target illuminated by a base transmitter. We assume that the UAVs are able to form time delay (τ i )and Doppler (f i ) estimates based on these signals, and transmit these measurements bac to the base. With an auto-pilot such as that described in [7], the UAVs are capable of velocity and altitude hold. Therefore, for simplicity, we consider only a two-dimensional scenario here. We define the position of the base station as the origin for our (x, y) coordinate system. Let x, y, V x,andv y represent respectively the position of the target and the components of the target velocity vector in the (x, y) directions. Similarly, let x i and y i denote the position of the i th UAV, and assume that each UAV is flying at the same constant speed V, with heading angle ψ i measured counterclocwise from the x-axis. According to [8] and after appropriate scaling, we have: where τ i = x 2 + y 2 + (x x i ) 2 +(y y i ) 2 (1) f i = V xx + V y y x2 + y + V p 2 (x xi ) 2 +(y y i ) 2 (2) V p =(V x V cos ψ i )(x x i )+(V y V sin ψ i )(y y i ). Using all the time-delay and Doppler information sent from the UAVs, the base station runs an Extended Kalman Filter (EKF) to trac the target, and at the same time sends /5/$2. 25 IEEE 1148 Authorized licensed use limited to: Univ of Calif Irvine. Downloaded on September 3, 29 at 2:33 from IEEE Xplore. Restrictions apply.

2 bac the command heading for each UAV such that the next measurement received will produce an improved estimate at the base station. B. Extended Kalman Filter The need for the EKF is apparent from the nonlinear measurement equations in (1) and (2). We represent the target state vector at time as x =[xyv x V y ] T, and arrange the measurements into the observation vector h (x ) = [τ 1... τ N f 1... f N ] T. The discrete-time system and observation models are then written as: x = Ax 1 + ω 1 (3) z = h (x )+ν, (4) where ω 1 N(, Q 1 ) and ν N(, R ) are respectively the process and measurement noise, which are assumed to be uncorrelated and Gaussian with covariances: Eω ω T j = Q δ,j (5) Eν ν T j = R δ,j (6) Eω ν T j =. (7) As with the standard Kalman filter, the EKF can be divided into two stages, the time update and measurement update steps. With the time update, we propagate the target s state estimate and prediction covariance matrix as ˆx - = Aˆx 1 (8) P - = AP 1 A T + Q 1 (9) Once we have the measurements from the sensors, we update our estimate as: K = P - H T (H P - H T + R ) 1 (1) ˆx = ˆx - + K (z h (ˆx - )) (11) P =(I K H )P - (12) In the above equations, H =( h x )T is a function of the UAVs position and heading. The basic idea is to reduce the size of the estimate covariance in equation (12) by updating the heading of the UAVs, and hence H. In essence we are applying an outer control loop around the Kalman filter. C. and UAV Dynamic Models In the simulation studies whose results we present later, we will assume for simplicity a target with a constant velocity model, which can be described by 1 t A = 1 t 1, (13) 1 where t is the time interval between samples. We will also assume white process and measurement noise terms, although this is not necessary either: ) Q =diag (σx,σ 2 y,σ 2 2 Vx,σ 2 Vy (14) R =diag ( στ 2,...,στ 2,σf 2,...,σf 2 ) (15) Modeling errors can be accounted for by tuning the parameters in Q and R. In a similar way, we assume that we have control over each UAV s heading at different time slots. Assuming an instantaneous response to heading commands, the UAVs will fly in a straight line with a velocity V and heading ψ i during each time interval. The inertial position of the UAV i at time is given by: x i, = x i, 1 + Vcos(ψ i, ) t (16) y i, = y i, 1 + Vsin(ψ i, ) t, (17) where ψ i, is its heading at the same time. III. ALGORITHMS We present two algorithms for commanding the UAV headings. In the first, we attempt to minimize the trace of the one-step ahead prediction error covariance of the EKF. In the second, we attempt to maximize the information that the new measurements would provide. These two approaches are outlined in the sections below. A. One-Step Ahead Approach In this approach, we command the UAVs at time t = such that at t = +1 the trace of the prediction covariance P - +1 is minimized. Mathematically, the idea is to find the vector of heading commands Ψ =[ψ 1,...ψ N, ] T such that, Ψ =argmintr(p - +1). (18) Ψ If we recall Equations (9) and (12), and mae appropriate substitutions, our cost function becomes: tr ( P - ) ( +1 = tr A[(P - ) 1 + H T R 1 H ] 1 A T ) + Q. (19) We employ a simple first-order gradient search to minimize (19). The gradient with respect to the heading of the i th UAV at time is given by where, tr(p - +1 ) ( = tr B = P - 1 AB 1 + H T R 1 H. B ) B 1 A T (2) The problem boils down to calculating H. The matrix H is 2N-by-4 matrix. When 1 i N, H (i, j) = τi x (j), otherwise H (i, j) = fi N x (j). Therefore, when 1 i N, we have: x H (i, 1) = x2 + y + 2 H (i, 2) = x x i, (xi, x) 2 +(y i, y) 2 y x2 + y + y y i, 2 (xi, x) 2 +(y i, y) 2 H (i, 3) = H (i, 4) =. (21) When N +1 i 2N, we have the expression for H given in equation (22) Authorized licensed use limited to: Univ of Calif Irvine. Downloaded on September 3, 29 at 2:33 from IEEE Xplore. Restrictions apply.

3 V x H (i, 1) = x2 + y (V x x + V y y) x 2 ( V x V cos ψ + i N, x 2 + y 2 ) 3 (xi N, x) 2 +(y i N, y) (x x i N,) 2 (V x V cos ψ i N, )(x x i N, )+(V y V sin ψ i N, )(y y i N, ) H (i, 2) = ( (x x i N, ) 2 +(y y i N, ) 2 ) 3 V y x2 + y (V x x + V y y) y 2 ( V y V sin ψ + i N, x 2 + y 2 ) 3 (xi N, x) 2 +(y i N, y) (y y i N,) 2 (V x V cos ψ i N, )(x x i N, )+(V y V sin ψ i N, )(y y i N, ) H (i, 3) = H (i N,1) H (i, 4) = H (i N,2). ( (x x i N, ) 2 +(y y i N, ) 2 ) 3 (22) In the expression for H, x, y, V x,v y are the position and velocity of the target at time t =, andx i,,y i,,ψ i, are the position and heading for the i th UAV at time t =. After plugging (16) and (17) in, we tae the derivative of H with respect to ψ i,.when1 i N, we have equation (23), otherwise we have equation (24), where d i = x x i, 1 V cos ψ i, t d i = y y i, 1 V sin ψ i, t R i = (d i )2 +(d i )2 s i = V x V cos ψ i, s i = V y V sin ψ i,. In practical scenarios, the UAV dynamics limit the rate at which its heading may change. Therefore, for each step we restrict the UAVs position to an arc confined by the previous heading. In particular, we append the following constraint to the to optimization problem: ψ i, ψ i, 1 C i =1...N, (25) where C is the upper bound on the turning rate of the UAV. B. Measurement Entropy Maximization Approach In this section, we present an alternative approach based on maximizing the information or entropy introduced at each UAV measurement update. If we recall equation (4) and linearize around the approximate state and measurement vectors, we have: z z + H (x x )+ν, (26) where x = Aˆx 1 and z = h ( x ).Ifwedefine ẽ x x x and ẽ z z z, equation (26) can be simplified as: ẽ z = H ẽ x + ν (27) where ẽ z represents the new information our EKF can use to improve the estimate. We now: E(ẽ z ẽ H z )=H E(ẽ x ẽ H x )H H + R = H P - H T + R, (28) Fig. 1. No Control Case : UAV Trajectory so the entropy of the new measurement is therefore: h(ẽ z )= 1 2 ln ( (2πe) 2N E(ẽ z ẽ H z ) ) (29) After getting rid of the constant term in (29), we simply need to maximize E(ẽ z ẽ H z ). We refer to this approach as Measurement Entropy Maximization (MEM): Ψ =argmax Ψ H P - H T + R. (3) In our implementation, constraint (25) is taen into account, and the simple gradient search approach is used to find the optimum. While the global optimum is not always guaranteed, it yields excellent results in our simulations. IV. SIMULATION RESULTS In this section, we present simulation results for the One- Step-Ahead and MEM algorithms proposed above. Two UAVs are assumed to be tracing a target that moves according to the perturbed constant velocity model of (3). The nominal velocity of the target in the x and y directions is V x = 15m/s and 115 Authorized licensed use limited to: Univ of Calif Irvine. Downloaded on September 3, 29 at 2:33 from IEEE Xplore. Restrictions apply.

4 H (i,1) = d i (di V sin ψ i, t d i V cos ψ i, t) + V sin ψ i, t (R i )3 R i H (i,2) = d i (di V sin ψ i, t d i V cos ψ i, t) V cos ψ i, t (R i )3 R i H (i,3) = H (i,4) = (23) +3 (si N +3 (si N di N di N (di N (di N H (i,1) = V sin ψ i N, R i N V sin ψ i N,+s i N )di N H (i,2) si N V sin ψ i N, t d i N (di N V sin ψ i N, t d i N (di N V sin ψ i N, t d i N s ) 5 = V cos ψ i N, R i N V sin ψ i N,+s i N ) si N V sin ψ i N, t d i N V cos ψ i N, s i N d i N i N di N (di N V sin ψ i N, t d i N (di N V sin ψ i N, t d i N + s ) 5 H (i,3) H (i,4) V sin ψ i N, t V cos ψ i N, s i N d i N = H (i N,1) i N di N V cos ψ i N, t = H (i N,2) (24) Fig. 2. No Control Case : Real Estimation Error Fig. 3. One Step Ahead Case : UAV Trajectory V y =15m/s, and the initial target position is assumed to be at (x, y) = (, ) meters. The two UAVs are initially positioned at (25, 25) and (2, 2) with headings 5π 18 and π 6, respectively. We also assume that each UAV flies at a constant speed of V =1(m/s). In the simulation, the process covariance matrix Q issettobediag(1, 1, 3, 3) and the measurement covariance matrix R to be diag(8, 8, 1, 1). Simulations are run for 9 seconds. In Figure (1) and (2), we first show the case where the UAVs are not controlled. Figure (1) shows the paths of the target and uncontrolled UAVs, and Figure (2) shows the estimation error x ˆx 2. Clearly, the tracing performance in this case is rapidly deteriorating. The performance of the constrained One-Step-Ahead approach is shown in Figure (3) and (4), and that of the MEM algorithm in Figure (5) and (6). Both show 1151 Authorized licensed use limited to: Univ of Calif Irvine. Downloaded on September 3, 29 at 2:33 from IEEE Xplore. Restrictions apply.

5 Fig. 4. One Step Ahead Case : Real Estimation Error Fig. 6. Maximize Entropy Case : Real Estimation Error Fig. 5. Maximize Entropy Case : UAV Trajectory essentially identical results that are significantly better than the uncontrolled EKF. Similar performance improvements were observed in many other cases as well, although the results show wide variations depending on the relative speed of the target and UAVs as well as their initial positions and headings. REFERENCES [1] G. Gu, P.R. Chandler, C.J. Schumacher, A. Spars, and M. Prachter. Optimum cooperative uav sensing based on cramer-rao bound. In Proceedings of the American Control Conference, pages , Portland, Oregon, June 25. [2] B. Grocholsy, A. Maareno, and H. Durrant-Whyte. Informationtheoretic coordinated control of multiple sensor platforms. In Proceedings of the IEEE International Conference on Robotics and Automation, volume 1, pages , 23. [3] Marcel L. Hernandez. Optimal sensor trajectories in bearings-only tracing. In Per Svensson and Johan Schubert, editors, Proceedings of the Seventh International Conference on Information Fusion, volume II, pages 893 9, Mountain View, CA, Jun 24. International Society of Information Fusion. [4] Y. Oshman and P. Davidson. Optimization of observer trajectories for bearings-only target localization. IEEE Trans. Aerosp. Electron. Syst., 35(3):892 92, July [5] J. Ousingsawat and M. Campbell. Establishing trajectories for multivehicle reconnaissance. In Proceedings of the AIAA Guidance, Navigation, and Control Conference, pages , Aug. 24. [6] Ying He and K.P. Chong. Sensor scheduling for target tracing in sensor networs. In Proceedings of the IEEE Conference on Decision and Control, pages , December 24. [7] Randal et al. Beard. Autonomous vehicle technologies for small fixed wing uavs. AIAA Journal of Aerospace Computing, Information, and Communication, 2(1):92 18, 25. [8] Nicholas J. Willis. Bistatic Radar. Artech House, Boston, V. CONCLUSIONS In this paper, we presented two centralized control algorithms for optimally guiding the headings of UAVs employed in a target tracing application. The UAV headings are chosen either to maximize the measurement entropy or to minimize the trace of the prediction error covariance associated with an EKF tracer. By introducing an outer loop control over the EKF in this way, dramatically improved tracing results are achieved Authorized licensed use limited to: Univ of Calif Irvine. Downloaded on September 3, 29 at 2:33 from IEEE Xplore. Restrictions apply.