ESMF Based Multiple UAVs Active Cooperative Observation Method in Relative Velocity Coordinates

Transcription

1 Joint 48th IEEE Conference on Decision and Control and 8th Chinese Control Conference Shanghai, P.R. China, December 6-8, 009 WeCIn5.4 ESMF Based Multiple UAVs Active Cooperative Observation Method in Relative Velocit Coordinates Feng Gu, Yuqing He, Jianda Han and Yuechao Wang Abstract based on etended set-membership filter (ESMF and the path planning method in relative velocit coordinates (RVCs, a new 3D multiple Unmanned Aerial Vehicle (UAV sstems active cooperative observation method, high precision cooperative observing a moving target b proper planning the behavior of each member vehicle, is proposed. he new method combines the ESMF based cooperative observation method and the LP-based trajector generation method in RVCs, where the ESMF based cooperative method is used to obtain the moving target s motion state which is further used as part of the cost function to plan the UAV s behavior b using the planning method in RVCs. he contribution of this paper is: the computational burden of the proposed cooperative algorithm is comparable to single ESMF algorithm; the high precision observation of moving target is alwas obtainable b considering the optimal observation condition during trajector planning; 3 the planning algorithm in RVCs, where the trajector planning can be modeled as a LP problem, is used to optimize the UAV s behavior, thus, in compan with, the fast application of the new proposed active cooperative observation method is desirable. Finall, the simulations in 3D environments are conducted to verif the feasibilit and validit of the method. I. INRODUCION Localization/identification is one of the basic enabling techniques of mobile robotics []. hus, it is also one of the top important research tass in the field of robotics. However, single mobile robot sstem ma present powerlessness due to the limited visibilit and observation accurac of equipped sensors, especiall when facing complicated surroundings and terrible observation conditions. An effective method, which has been etensivel researched in some applications, to overcome these drawbacs is to mae observation b fusing the observations of a common moving target from multiple mobile robot sstems [-5]. Cooperative observation is usuall on the basis of estimation technique. Man estimation methods, including traditional stochastic theor based methods, and ESMF strateg, have all been considered to construct the cooperative observation algorithm. For eample, wor [5] presents a method that uses a simple re-parameterization of two dimensional Gaussian distributions to obtain more Manuscript received March, 009. F.Gu is with the Graduate School of the Chinese Academ of Sciences and State Ke Laborator of Robotics, Shenang Institute of Automation, Chinese Academ of Sciences, Shenang 006, China ( fenggu@sia.cn. Y.Q.He, J.D.Han, and Y.C.Wang are all with the State Ke Laborator of Robotics, Shenang Institute of Automation, Chinese Academ of Sciences, Shenang 006, China ( heuqing, jdhan, cwang@sia.cn. accurate target position estimation from two or more robot agents. In [], an optimal cooperative position and velocit estimation, based on the Cramer-Rao bound, of the ground moving target (GM is investigated aiming at obtaining the optimal cooperative observation. All of these methods present the same disadvantages, i.e., the process and measurement noise should both be stochastic variables with a prior nown mean and covariance, which often mismatches the realit. ESMF is another estimation method that can be used in cooperative observation. Different from the stochastical estimation methods, ESMF assumes that the noise is Unnown But Bounded (UBB [6], and it can obtain an uncertain estimation set which the real sstem state are ensured in [7]. With these characteristics, ESMF are ver useful for the observation problem and the use of ESMF in cooperative observation is discussed in [9] where the cooperative observation method is a simple combination of two ESMF algorithm. However, the computational burden of this method is heav due to the process of computing the intersection and its outer bounding ellipsoid, especiall in 3D surroundings. In all of the preceding researches, the influence of robot motion on the observation is not considered, while this will absolutel deteriorate the optimal observation results. hus, in this paper, a new on-line cooperative observation method is introduced, where a new ESMF based cooperative observation method with reduced computational burden is first proposed, and then, this method is considered into the trajector planning method in RVCs to obtain better observations b relative motion of robot sstems, i.e., the active cooperative observation method. II. PROBLEM FORMULAION he problem of two UAVs cooperativel detecting and tracing a moving target is researched as shown in Fig., is a moving target to be traced; A and A denote two UAV sstems, where A, named main UAV, is responsible for accuratel detecting, planning path of two UAVs and pursuit the states of the target b fusing the observation data from both the main UAV A and the accessorial UAV A. he angle between the tie line between UAV sstems A, A and target, i.e. φ is defined as cooperative observation angle. In general, the dnamic model is necessar in order to estimate/predict the state of a dnamic sstem. However, the inematics or dnamics of the moving target vehicle is usuall unnown or too complicated to be used. hus, in this paper, the following Newton s laws of motion based inematic equation is used to denote the action of the moving /09/$ IEEE 3008

2 target, X I v X w, 3,,, ( 0 0 where X is the position of the target, v,, is the velocit vectors, Δ is the sampling time, w, is the process noise bounded b [ ] w Q [ w ], Q,,,, is a smmetric and positive definite matri. z A, (,, z,,, A, (,, z O,,, Fig. Cooperative tracing with two UAVs he observation of the moving target b UAV sstems is realized b 3D radar sensor. he theor of the observation can be shown in Fig. and the following equations, (,, z,,, r ( ( ( z z n i,, i,, i,, i, r, i tan, i, i,, i ( (, i,, i, tan, i, i,, i, i, z n z n where r i,, θ i,, α i, are the observations of the moving target denoted as polar coordination. n, =(n r,i, n θ,i, n α,i is the observation noises and satisfies [ ] n R [ n ].R,,,, is a smmetric and positive definite matri. z i, i, i, A i, (,, z i, i, r i, (,, z,,, Fig. Description of the parameters of radar III. COOPERAIVE OBSERVAION BASED ON ESMF A. Etended Set-Membership Filter ESMF delivers an ellipsoid set given b the following equation, n E(, P R ( P ( ( where is the center of the ellipsoid; P is an envelope matri satisfing smmetric and positive definite conditions. Considering a discrete non-linear sstem written as: f( w (3 h( n (4 w R n and n + R m are respectivel process and measurement noise which satisf the following inequalities, [ ] w Q [ w ], [ ] n R [ n ] Q and R + are both smmetric and positive definite matri. Linearizing Eq. (3 near current state ields, f f O w (5 ( ( / ( ( he ESMF method considers the higher order terms (H.O.. as a part of the process noise and computes its envelope matri with interval analsis method [7]. he linearized Eq. (3 can be rewritten as, f( f ( / ( w (6 Similarl, the output equation can be rewritten as, h( h ( / ( n (7 he new noise bounds can be denoted as, [ w ] Q [ w ], [ n ] R [ n ] With the linearized sstem, ESMF algorithm can be used to estimate the state of sstem (3 to (4 b the following steps [9], Step I: ESMF Prediction Step f (,, (8 P A P A /( Q /,, (9 Step II: ESMF Update Step K ( h(,,, (0 P ( ( P / (,, ( ( P /( C W C ( P /(,, ( A f ( / C h( /,,, [ ( /( / ],, [ h( ] W [ h( ],, W C P C R K P C W β (0, and ρ + (0, are used in the process of determining outer bounding of summation of ellipsoids. he can be calculated using the methods in [6]. hen estimation result is E(,, P,. he main idea of ESMF is to obtain the final estimation b intersecting prediction uncertain set and the measurement set [8]. hus, ESMF is inherent an algorithm to compute the intersection of two special sets, which is a ver useful idea for cooperative observation. B. Cooperative Observation Based on ESMF he observation result obtained b using ESMF is an 3009

3 uncertaint ellipsoid set denoted as Eq. (, which the target is ensured to lie within. hus, it is obvious that better observation result can be obtained b intersecting two observation uncertaint sets. he main idea of the new proposed method is to embed the process of the intersecting into the ESMF algorithm itself, i.e. to displace the process of computing the intersection of the prediction uncertaint set and the measurement one with the process of computing the intersection of the following three sets: prediction uncertaint set and two measurement uncertaint sets, as shown in Fig. 3. K h,, +, + P ( ( (6 ( P /(,, ( P /( C W C P /(,, C h( /,,, K ( P / ( C W, W C ( P / ( C R / (7 [ ( ] [ ( ] h W h,, R R / R /,, R, R It is well nown that ESMF update step, i.e., Eq. (0 and (, is itself an algorithm to find the intersection of the prediction ellipsoid set and the observation set. hen, we mae full use of this theor to intersect the three sets as shown in Fig. 4. his process can be divided as two steps: Fig. 3 Main idea of cooperative observation method he detailed steps of the cooperative observation algorithm, which is eecuted in the computer sstem of main UAV sstem, are as follows. he superscript i (i=, on the left of the variable denotes the variable is computed using the ith UAV s observation data at each sub-step. Step : Single prediction: f( (,, P A P A /( Q / (3,, A f ( /, Step : Cooperative Update: Sub-step Fusing the Accessorial UAV Measurement: K ( h( (4,,, P P /(,, P C W C P,, ( /( /( where C h( /, W C P C /( R /,, K ( P /( C W, h W h,, [ ( ] [ ( ] R R / R /,, R R Sub-step Fusing the Main UAV Measurement: (5 Fig. 4 Details of intersecting the three sets he first sub-step is to intersect the prediction ellipsoid set E(, P and the accessorial UAV s observation,, set S.Based on the ESMF algorithm, the result E(, P,, will be an ellipsoid set that satisfies Eq. (, denoted as E temp ; he second sub-step is to intersect the E temp and the main UAV s observation set S. his sub-step taes E temp as the prediction ellipsoid set and updates it using the main UAV s observation, i.e. calculates the intersection of E temp and the main UAV s observation. However, in some special situations, the two UAVs observation sets ma not intersect, i.e. E temp S =, thus, in order to continue the algorithm recursivel, we can directl tae the E temp = E(, P as the final result to,, continue the algorithm recursivel. IV. ACIVE COOPERAIVE OBSERVAION MEHOD In this section, based on the cooperative observation method formulated above, a new active cooperative observation method is proposed. he main idea of this method is to embed path planning between the single 300

4 prediction step and cooperative update step so as to achieve optimal observation at each time step. Fig. 5 arget tracing in RVCs Here the LP-based path planning method in RVCs [0] is used for optimal observation. With this method the path planning problem in the dnamic environment can be described as minimizing an objective function subject to a set of linear inequalities that are easil embedded into LP planner [0]. he main idea is as shown in Fig. 5. he arbitrar contour with a point denotes the target with velocit v. A i is a moving UAV with velocit v i. L Ai is the ra that originates from A i to which is the center of the target. L Di is the ra that originates from A i in the direction of v Ai v v v Ai i is the UAV-target relative velocit. γ Ai is the angle between v and L A A. he pursuit theor in RVCs is to mae v Ai lie within the cone area denoted b AM N in Fig. 5 and towards to the target b adjusting the v [0, ], i.e., i vai vi { vai LDi } (8 It should be noted that the target is supposed to eep constant speed during sampling time Δ, therefore the equation v v Ai Ai is alwas satisfied. In our method, the UAV-target relative states can be calculated according to the target states predicted in single prediction step. hen the optimization criterion based on path planning method in RVCs is designed and analzed in the following. Optimization with Respect to Pursuit Velocit From the discussion above we can see clearl that if the UAV wants to trac the target with shortest time, the component of v in L Ai Ai direction, i.e. v is maimal [0] as ti shown in Fig. 5. v can be denoted as, ti v v L (9 ti Ai Ai herefore we propose to design the pursuit velocit criterion as, J L( wl, w L ( v, v (0 A, A,,, L ( L, L ( v, v, w A, A,,, 0 and w 0 are weight values. Optimization with Respect to Cooperative Observation When the cooperative observation angle φ is 90 degree, the two UAVs can achieve optimal observation [],[9]. he relationship between φ and the position of the UAVs can be denoted as, ( X, X,, ( X, X,, cos ( X, X,, X, X,, he position state of UAV X, can be denoted b the, v i, as following: I I 3 3 I 3 X X i, i, v ( i, 0 I 3 I3 Substituting Eq. ( into Eq. ( and linearizing it ields, cos c ( c, c ( v, v (3 0,, c X X / X X 0 X X ( X X c ( X 3 X X X X X X X X X ( X X c ( X X 3 X X X X X X X X v X,,,, X X v X,,,, hus, the optimal observation criterion can be designed as, J c ( c, c ( v, v (4 0,, hen the optimization of cooperative observation angle is to minimize J b adjusting the v i., B defining a positive variable z as [0], z c ( c, c ( v, v z (5 0,, hen, minimizing z subjected to the inequalit of (5 is equivalent to minimizing J. he objective function of (4 can be rewritten as a linear one J =w 3 z (6 where w 3 0 is the weight value of this objective. 30

5 3 Optimization with Respect to Collision Avoidance For multi-uav cooperation, one of the most important problems is collision avoidance. In our method, eeping the UAVs apart from the target can avoid two collisions: the collision between each UAV and the target; the collision between the two UAVs. he distance between each UAV and the target can be denoted as, d X X (7,,,, d, X, X,, (8 Substituting Eq. ( into Eq. (7 and Eq. (8 and linearizing them, then this criterion can be designed as wd vm, J d Dc wd v (9 5 A, 0, d X X, d / X X, d / X X Dc Dc, Dc is the required distance between each UAV and the target. w 4 0 and w 5 0 are the weight values. Using the same method as the Eq. (4, minimizing J 3 is equivalent to minimizing z subjected to the following inequalit, wd v, z d0 Dc z ( wd v 5, he objective function (9 can be written as, J 3 =z (3 he constraints due to UAV inematics and dnamics also have to be considered: v lowlimi v i, v uplimi (3 vuplimi ( j min( vima ( j, vima ( j vi, ( j vlowlimi ( j ma( vimin ( j, vimin ( j vi, ( j v ima and v imin are the maimum and minimum velocit change; v mai and v mini are maimum and minimum of velocities; j=,, 3 denotes the corresponding component of the vector. supposed that there is a sudden change in the target s trajector. But from Fig. 6 we can see that the two UAVs can still trac the target cooperativel with the proper trajector and the convergence of the cooperative observation method can be ensured, i.e., the observation result can converge to a stead uncertaint set quicl. Fig. 6 Whole process of cooperative tracing he size of an ellipsoid can be indicated b its length of three aes i.e. the trace of envelope matri P, in Eq. (. Fig.7 shows changing trend of the trace of matri P,, which illustrates the uncertain ellipsoid quantitativel. he dashed line denotes the envelope matri trace estimated b the ESMF algorithm based on onl a group of measurement data; the solid line denotes the case when two groups of measurement data are available. It is obvious from Fig. 7 that the uncertain ellipsoid set cooperativel estimated b two UAVs can converge to a smaller stead value ( vs , which indicates much higher observation precision. Besides that there is an increase near Step 5 because of the sudden change of target trajector. But after that the uncertain ellipsoid can still converge, indicating that the proposed method has better stead performance. V. SIMULAION RESULS he target sstem model is as Eq. ( he envelop matri of sstem noise and observation noise are Q=diag(0.000, 0.000, and R=diag(0.00, 0.00, 0.00 respectivel. he initial envelope matri of uncertain area of target is P 0 =diag (0., 0., 0..he weight values are w =0.0, w =0.0005, w 3 =.7, w 4 =, w 5 =.. Fig. 6 shows the process of cooperative tracing. he solid line in the middle is the trajector of target and other two solid lines are trajector of UAVs. he dashed lines are the observation lines connected with each UAV and the target. Each ellipsoid is the cooperative observation result that gets smaller and smaller with the tracing and observing going on. For verifing the ecellence performance our method we Fig. 7 race of envelope matri of target position Fig. 8 shows the change of cooperative observation angle during the tracing process. he cooperative observation angle can alwas eep around 90 degree during the tracing process ecept near step 5 where the target trajector has a 30

6 sudden change. From Fig. 8 the conclusion is that our method can achieve near-optimal cooperative observation activel with the moving of the target b path planning the path properl. Fig. 8 Change of cooperative observation angle For discussing the speediness of our method more specificall, the cooperative observation method proposed in [9] is compared with our method. able I is the computational cost of different algorithms []. From the table we can see clearl the new proposed algorithm in this paper taes averagel onl 0.33s to complete one iteration computation, which is almost equivalent to that of the single ESMF algorithm. While the algorithm proposed in [9] taes averagel 0.39s, which is about 0% more than that of the new proposed method in this paper. hus, we can conclude that the new cooperative observation algorithm is more suitable to be used in fast applications. More difference between these two methods can be found in []. able I he comparisons of computational cost Algorithms ime cost (s Single ESMF 0.3 New Proposed Algorithm 0.33 Algorithm in [9] 0.39 REFERENCES [] G. Gu, P. R. Chandler, C. J. Schumacher, A. Spars, and M. Pachter, Optimum cooperative UAV sensing based on Cramer-Rao bound, IEEE ransactions on Aerospace and Electronic Sstems, vol. 4, no.4, 006. [] A. Sanfeliu, J. M. M. ur and A. C. Murtra;, Efficient active global localization for mobile robots operating in large and cooperative environments, Proceedings of IEEE International Conference on Robotics and Automation, pp [3] U. Zengin and A. Dogan. Cooperative target tracing for autonomous UAVs in an adversarial environment, Proceedings of AIAA Guidance, Navigation, and Control Conference and Ehibit, 006 [4] D. H. Shim, H. J. Kim and S. Sastr. Decntralized nonlinear model predictive control of multiple fling robots, Proceedings of IEEE Conference on Decision and Control, pp , 003 [5] A. W. Stroupe, M. C. Martin and. Balch, Distributed Sensor Fusion for Object Position Estimation b Multi-Robot Sstems, Proceedings of IEEE International Conference on Robot and Automation, pp , 00. [6] B. Zhou, J. Han and G. Liu. A UD factorization-based nonlinear adaptive set-membership filter for ellipsoidal estimation, International Journal of Robust and Nonlinear Control, 007. [7] E. Scholte and M. E. Campbell. A nonlinear set-membership filter for on-line applications, International Journal of Robust and Nonlinear Control, 3, pp , 003. [8] E. Scholte and M. E. Campbell. On-line nonlinear guaranteed estimation with application to a high performance aircraft, Proceedings of American Control Conference, pp [9] M. E. Campbell and J. Ousingsawat. On-line estimation and path planning for multiple vehicles in an uncertain environment, International Journal of Robust and Nonlinear Control, 4, pp , 004. [0] D. Zu, J. Han and D. an. LP-based optimal path planning in acceleration space, Proceedings of IEEE International Conference on Robotics and Biomimetics, pp , 006 [] D. Zu, J. Han and D. an. MILP-based trajector generation in relative velocit coordinates, Proceedings of IEEE Conference on Decision and Control, pp , 007 [] F. Gu, Y. He, J. Han and Y. Wang. On-line cooperative observation based on ESMF in three dimensional environments, Proceedings of AIAA Guidance, Navigation, and Control Conference and Ehibit. VI. CONCLUSION An active cooperative observation method, which combines the LP based path planning method in RVCs with cooperative observation based on ESMF, was presented in this paper. With this method the UAVs first plan the path according to the prediction of target and then updated the predicted states of the target cooperativel for obtain the more accurate result. he following advantages can be obtained with the new proposed algorithm: this method can observe the target optimall b planning the path of the UAVs dnamicall; fusing the observation result without etra computation burden apart from ESMF and planning the path with LP method can ensure the speediness of algorithm; 3 Besides that, this method avoids introducing the more approimations which can ensure the accurate observation result. Finall, the simulation results verif the feasibilit and stead performances of the new proposed method. 303