Multi-Robot Tracking of a Moving Object Using Directional Sensors

Transcription

1 Mult-Robot Trackng of a Movng Object Usng Drectonal Sensors Xaomng Hu, Karl H. Johansson, Manuel Mazo Jr., Alberto Speranzon Dept. of Sgnals, Sensors & Systems Royal Insttute of Technology, SE- 44 Stockholm, Sweden Optmzaton and Systems Theory Royal Insttute of Technology, SE- 44 Stockholm, Sweden Abstract The problem of estmatng and trackng the moton of a movng target by a team of moble robots s studed n ths paper. Each robot s assumed to have a drectonal sensor wth lmted range, thus more than one robot sensor s needed for solvng the problem. A sensor fuson scheme based on nter-robot communcaton s proposed n order to obtan accurate real-tme nformaton of the poston anoton of the target. Accordngly a herarchcal control scheme s developed, n whch a consecutve set of desred formatons s planned through a dscrete model and low-level contnuous-tme controls are executed to track the resultng references. The algorthm s llustrated through smulatons and experments I. INTRODUCTION Mult-robot systems are used n many stuatons n order to mprove performance, sensng ablty and relablty, n comparson to sngle-robot solutons. For example, n applcatons lke exploraton, survellance and trackng, we want to control a team of robots to keep specfc formatons n order to acheve better overall performance. Formaton control s a partcularly actve area of mult-robot systems, e.g., [1], [2], [3], [4], [5]. Most of the work n the lterature, however, s focused on the problem of desgnng a controller for mantanng a preassgned formaton. In ths paper we consder the problem of localzng and trackng a movng object usng drectonal sensors that are mounted on moble robots. When the range s far and the resoluton becomes low, many vsual sensors are n effect reduced to only drectonal sensors, snce the depth nformaton s then hard to recover. Snce we need to have suffcent separaton for the sensor channels, t s reasonable that we mount the sensors on dfferent robots. Snce one sensor s obvously not enough to localze the target, a network of sensors s needed. Thus the goal of the control desgn s not only that the robots should track the target, but also that the robots should coordnate ther moton so that the sensng and localzng of the target s not lost. In stuatons lke ths, sensng and estmaton become central and an ntegrated soluton of control for sensng, or actve sensng, s a must. The man contrbuton of our work s a herarchcal algorthm for localzng and trackng a movng target. We study a prototype of a dstrbuteoble sensng network, namely, a team of nonholonomc robots wth exteroceptve sensors. These trackng robots have hard sensor constrants, as they can just obtan relatve angular poston of the target wthn a lmted feld of vson, and relatve postons of each other wthn short dstance. Our soluton provdes a cooperatve scheme n whch the hgher level of the algorthm plans a formaton for the robots to follow n order to track the target. The robots exchange sensor nformaton to estmate the poston of the target by trangulaton. In partcular, snce the moton of the target s unknown and thus can not be planned, the moton plannng for the formaton must be done on-the-fly and based on the actual sensor readngs, whch s qute dfferent from many formaton control algorthms n the lterature where all agents moton can be planned. In the lower level of our algorthm, for each robot we use a trackng controller that s based on the so-called vrtual vehcle approach [6], whch turns to be qute robust wth respect to uncertantes and dsturbances. We should emphasze that n order to mplement our control algorthms, only local nformaton s needed. The outlne of the paper s as follows. The problem formulaton for collaboratve trackng s presented n Secton II. The herarchcal soluton s descrbed n Secton III. Supportng smulaton results are shown n Secton IV, and some prelmnary expermental work s presented n Secton V. The conclusons are gven n Secton VI. II. PROBLEM FORMULATION Consder N robots trackng a movng target as shown n Fgure 1 where N = 2. The robots are postoned at x,y = 1,,N, respectvely, whle the target s at x T,y T. The moton of the target s not a pror gven. Each robot has a drectonal sensor, whch provdes an estmate of the drecton α = β θ, = 1,,N, to the target from robot, where β = arctan y T y x T x From the estmates of α and the relatve postons of the robots, estmates of the target poston and velocty relatve to the robots can be derved. Note that α = corresponds to the target beng n the headng drecton θ of robot. The sensors are assumed to have a constant lmted angular range of α max,π, so that the estmated angle to the target from

2 Robot 1 x 1,y 1 x m,y m Target x T,y T d 1 α 1 d 2 θ 1 β m α 2 θ m β 2 θ 2 s the lnear vdeo sensor of the Khepera II robot as llustrated n Fgure 9 and further descrbed n Secton V. Gven that the robots follow the target on a certan dstance, there s a trade-off between the robustness of trackng on the target for both sensors or robots and the robustness of estmates. When the robots are very close to each other, the target wll be safely wthn the sensor vew feld but the estmate wll be very senstve to measurement naccuraces snce the two drectons toward the target are almost parallel; When the robots are very far away from each other, any moton of the target can lead to no angular measurements snce t wll be outsde the vew feld. However, f the sensor readngs are avalable n ths case, the target poston estmate by trangulaton wll be qute robust to measurement errors. Fg. 1. Robot 2 x 2,y 2 Two robots trackng a target under constraned angular sensng. robot s gven by ˆα = { α, α α max, otherwse where ˆα = denotes that the target s out of range. We ntroduce the dstance d = x T,y T x,y, = 1,,N. Suppose that the angles α, = 1,,N, are wthn the sensor range, then the target poston s related to the global coordnates of the robot as ˆxT ŷ T = x y + d cosβ snβ, = 1,,N 1 From 1 one can derve: x x j cosβ cosβj + d y y d j snβ j =,, j = 1,,N. snβ j 2 Multplyng 2 by cosβ snβ we obtan d = d j cosβ β j 1 cosβ j ψ j, 3 p j where, p j = x x j 2 +y y j 2 and ψ j = arctan y y j x x j, both of whch can be measured locally wthout knowng the global coordnates. Clearly when N > 2, there s redundant nformaton n 3 and ths can be used, for example, to mprove the accuracy and robustness of the estmaton. In the rest of the paper, we only consder the case N = 2, where the dstance estmates are gven by d1 d 2 = 1 cosβ1 cosβ 2 x1 x 2 snβ 1 snβ 2 y 1 y 2 provded that the nverse exsts. The estmaton problem s thus how to obtan a good estmate dˆ 1, dˆ 2, under the constrants on the drectonal sensors. An example of a drectonal sensor III. HIERARCHICAL TRACKING ALGORITHM The trade-off between havng guaranteed poston estmates of the target and havng accurate estmates leads to mposng a desred formaton for the two robots followng the target. The formaton s chosen such that the target s wthn the angular lmts of the robot sensors, and the sensor readngs gve a well-condtoned estmate of x T,y T. The evoluton of the formaton s defned n a dscrete set of ponts, whle lowerlevel contnuous-tme controls make the robots trackng the formaton. The resultng herarchcal control structure has an upper level n whch the evoluton of the formaton s updated at dscrete events and a lower level dedcated to the trackng by the robots of the wayponts defned by the formaton. Ths secton descrbes both the hgh-level formaton plannng and the low-level trackng control n detal. In ths paper we assume that each robot s modeled as uncycle ẋ = v cosθ ẏ = v snθ = 1,2 4 θ = ω where v and ω are the controls, and the drectonal sensor s mounted along the orentaton axs. A. Formaton Plannng The desred robot formaton s shown n Fgure 1. The dstance between the robots at x 1,y 1 and x 2,y 2 s denoted p, and the dstances to the target are d 1 and d 2. The desred orentaton of the two robots s fxed and corresponds to that the robot headngs should be perpendcular to the axs that connects the two robots. Thus the formaton s mantaned as long as ṗ = d 1 = d 2 = and θ 1 = θ 2. Let x m,y m denote the pont half-way between the robots, x m = x 1 + x 2 2, y m = y 1 + y 2. 2 Under the deal condton that the formaton s mantaned, the poston of ths pont together wth the headng of the axs θ m decdes the state of the formaton for the two robots, where

3 θ m = ψ 12 π 2. The evoluton of the deal formaton for the two robots s descrbed by ẋ m = v m cosθ m ẏ m = v m snθ m 5 θ m = ω m, where v m s the speed of x m,y m, and ω m the rotatonal speed of the axs. Accordngly we have to have v 1 = v m ω m p/2 v 2 = v m + ω m p/2 6 ω 1 = ω m ω 2 = ω m, n order to keep ṗ = and the headng perpendcular to the axs. Now let v m = ẋt cosβ m + ẏ T snβ m cosα m ω m = v m snα m + ẏ T cosβ m ẋ T snβ m wth β m = arctan y T y m, α m = β m θ m. x T x m It s easy to show that wth the above defned v m and ω m ṗ = d 1 = d 2 = and α 1 = α 2 =,.e., the desred formaton s mantaned. Equaton 5 wll be used to plan the reference path, where no dynamcs of the actual robots s consdered. The evoluton of the reference formaton s updated at dscrete tme nstances t k, k =,1,... Gven an estmate of the target poston at tme t k, the target poston at t k+1 s estmated. We suppose that no model of the target s avalable, so a smple estmate s lnear extrapolaton where x T t k+1 = ˆx T t k +t k+1 t k ˆv T t k cos ˆθ T t k ỹ T t k+1 =ŷ T t k +t k+1 t k ˆv T t k sn ˆθ T t k ˆv T t k = ˆx T t k,ŷ T t k ˆx T t k 1,ŷ T t k 1 t k t k 1 ˆθ T t k =arctan ŷt t k ŷ T t k 1 ˆx T t k ˆx T t k 1 The reference path provded to the low-level moton control s gven by trajectores generated from the controls v,ω, = 1,2 n 6, where v m and ω m are contnuous-tme controls defned over t k,t k+1, such that the correspondng way pont for x m t k+1,y m t k+1 s reached. If v,ω, = 1,2, are constant over an nterval, they generate the followng reference trajectores: x ref t=x f t k + v t k ω t k y ref t=y f t k v t k ω t k [ ] snθ f t k +ω t k t snθ f t k ] [ cosθ f t k+ω t k t cosθ f t k Now the queston s how to choose the ntal reference ponts at each step x f t k,y f t k,θ f t k, = 1,2, such that the desred formaton s fulflled. Those ponts are calculated based on the estmatons of the movng target from sensors readngs. Two dfferent strateges are proposed for choosng those ponts. In the frst, shown n Fgure 2a, we compute the new ntal reference ponts as θ f t k=arctan ŷt t k y m t k ˆx T t k x m t k x f t k =x f mt k ± p f cosθ f t k y f t k=y f m t k p f snθ f t k, where x m t k = 1 2 x 1t k +x 2 t k, y m t k = 1 2 y 1t k +y 2 t k, and p f specfes the desred dstance between the robots. In the second one, see Fgure 2b, we use the followng equatons where θ f t k =ˆθ T t k x f t k=x f m t k ± p f cosθ f t k y f t k=y f m t k p f snθ f t k, x f m t k y f m t = k ˆxT t k d ŷ T t k m f cosθ f t k snθ f t k and dm f specfes the desred dstance to the target. formaton. Remark: Note that the reference trajectory x f,yf can easly be expressed n the robot-fxed coordnate systems. Durng each updatng nterval t k, t k+1, the odometry can be used for localzaton wth respect to the ntal frame at t k. In the followng we use only the frst proposed strategy to compute the ntal reference ponts. B. Trackng Control The reference trajectores x ref,y ref, = 1,2, generated by the hgh-level formaton plannng are tracked by the robots usng the vrtual vehcle approach [6], [7]. The dea s brefly summarzed as follows. When one uses drectonal sensors, t s mportant that the relatve orentaton of the robot s known. Ths naturally could be acheved f we could specfy at what relatve orentaton the look-ahead dstance should be kept. Here we want the moble robot at all nstants to be orented towards the target and therefore we smply choose the reference pont x L,y L to be on the robot s axs of orentaton at a dstance L from the center of the robot. Then x L = x + Lcosθ y L = y + Lsnθ. 7 When choosng L n mplementaton, one has to make a compromse between performance and computaton. Proposton Let v r and θ r be the speed and orentaton of the pont x r,y r to be tracked by the robot. Then for each robot

4 x T t k,y T t k x T t k,y T t k x f 1 t k,y f 1 t k θ f x f 2 t k,y f 2 t k x 1 t k,y 1 t k x 2 t k,y 2 t k a Frst correcton strategy. x f 1 t k,y f 1 t k θ f x f 2 t k,y f 2 t k x T t k 1,y T t k 1 x 2 t k,y 2 t k x 1 t k,y 1 t k b Second correcton strategy. Fg. 2. Correcton strateges for the ntal ponts at each teraton n the Formaton Control level. x ref t k,y ref t k x ref t k+1,y ref t k+1 x t k+1,y t k x t k,y t k 15 Fg. 3. Trackng of a trajectory generated between t k and t k+1. n 4, as t, x L t,y L t converges to x rt,y r t wth the followng control 5 Robot 1 v = kl ρ cos φ+v r cosθ r θ ω = kρ L sn φ + v r L snθ r θ where φ = arctan y r y x r x θ s the relatve angle to the target measured by the robot, ρ s the dstance of the robot to the reference pont and k s any postve constant. In our case x r,y r for each robot s specfed by a reference trajectory x ref t,y ref t, = 1,2, on an nterval t k,t k+1.parameterze the trajectores as p s := x ref = 1,2. The parameter, s, s defned n order to adjust the speed of the pont accordng to the trackng error so that the robot can keep up wth t see Fgure 3. Here we choose to defne s as ṡ = v p 2 + q 2 e αρ s,q s := y ref s, where v s the desred speed at whch one wants robot to track ts path a natural choce s v = v t k, and a s an approprate postve constant. We then get { v = kρ cos φ L+v e αρ cosθ T θ ω = kρ L sn φ 8 + v L e αρ snθ T θ. In mplementaton, we can let θ T = ˆθ T t k. IV. SIMULATION RESULTS The herarchcal control algorthms developed n prevous secton are now evaluated through a smulated example. In Robot 2 }{{} Fg. 4. The target s shown as a thck lne and wth thn lnes the trackers trajectores. Despte the errors n the ntal postons of the robots, they converge to the desred formaton after a few events note the dfference between the ntal state formaton trangle and the posteror four ones. terms of formaton plannng, only the frst strategy s tested. Let the target follow a crcular trajectory and the trackers have an error n the measurement of ther β angles wth dstrbuton U.2,.2. Fgure 4 shows the target as a thck lne and wth thn lnes the trackers trajectores. Note that despte errors n the ntal postons of the robots, they converge to the desred formaton after a few events, see Fgure 6. The formatons at the ntal state and four other nstants are ndcated wth dashed trangles. Fgure 5 shows a zoomed vew of a part of Robot 2 s trajectory, taken from Fgure 4, wth added nformaton. The zoom s taken n the begnnng of the trajectory and thus the robot s stll tryng to converge to the hgh-level planner reference, as also s shown n Fgure 6. In Fgure 5 wth crcles are marked the corrected ntal ponts at each step of the hgh-level plannng algorthm, t s x f t k,y f t k. The stars denote the fnal poston of the tracker robots after an step, namely x t k,y t k. And the arrows are pontng at the postons desgned as fnal ponts of

5 x T e t vs x T t 9.65 x 2 f t k+1,y 2 f t k x 2 ref t k+1,y 2 ref t k x 2 t k+1,y 2 t k+1 3 y T e t vs y T t x 2 f t k,y 2 f t k x 2 t k,y 2 t k x 2 ref t k,y 2 ref t k measured and estmated samples Fg. 5. A closer look to the Robot 2 s trajectory n the regon marked wth a bg under-brace n Fgure 4. Some extra nformaton has been added about the planned path thck dashed lne.the crcles denote x f t k,y f t k, astersks x t k,y t k and the arrows pont to x ref t k,y ref t k. Note that the x-axs and y-axs scale are dfferent. Fg. 7. Comparson of the poston of the target estmated by the robots based on ther sensor measurements x e T,ye T thck dashed lne vs the real poston of the target x T,y T thn lne α 2 [rad] t [s] d 2 [mm] t [s] Fg. 6. Evoluton of d 2 and α 2 Robot 2 correspondng to the frst quarter of crcle n Fgure 4. The dashed thck lne shows the desred values of d 2,α 2 to fulfll the desred formaton. the trajectory at the prevous step,.e. x ref t k,y ref t k. The estmated poston of the target compared to the real poston of the target n ths smulaton s shown n Fgure 7. V. EXPERIMENTAL RESULTS Currently we are mplementng the algorthms on a team of two Khepera II robots [8] trackng a target robot. The setup s llustrated n Fgure 8. Khepera II s a small self-contaned wheeled robot wth mcro-processor and basc sensors nfrared proxmty sensors and encoders. Its dameter s about 7 mm, t has a precse odometry and a lnear speed n the range of.2 1. m/s. Our Khepera II robots are provded wth a lnear vdeo sensor, see Fgure 9. It gves a drectonal measurement up to a dstance of about 25 cm. The sensor conssts of a lnear lght senstve array of 64 1 pxels, whch gves an mage wth 256 gray levels. A major constrant of the lnear vdeo sensor s a lmted horzontal feld of vew of Fg. 8. Two Khepera II robots trackng a target. 36 degrees,.e., the angular range s lmted to α max = 18. For data exchange, the two robots use an on-board rado communcaton system. In ths testng benchmark some experments have been performed as the one presented n Fgure 1 wth a LEGO robot actng as target anovng n a slow and smooth curved trajectory. As can be seen n that fgure the startng formaton s not the desred one, and after one step of the hgh-level planner the desred formaton s reached. Moreover, n Fgure 11 s shown n a closer look how the formaton s kept by drvng the robots followng the trajectores generated by the path-plannng level. Fnally, Fgure 12 presents a zoom of one of the Khepera II robots followng the desred planned trajectores usng the controller presented n Secton III.B.

6 2 15 y coord. [mm] 5 Fg. 9. Khepera II lnear vson sensor. The angular range s lmted to α max = 18. Illustraton from [8] x coord. [mm] 4 Fg. 11. Zoom from Fgure 1 showng how the formaton evolves and s kept between two samplng ponts. y coord. [mm] 3 2 ACKNOWLEDGMENT Ths work was supported by the European Commsson through the RECSYS IST project, the Swedsh Research Councl and the Swedsh Foundaton for Strategc Research through ts Centre for Autonomous Systems at the Royal Insttute of Technology x coord. [mm] Fg. 1. An experment showng two Khepera II robots trackng a LEGO Mndstorms robot movng on a smooth curvng trajectory. The thck lne represents the estmated trajectory of the LEGO robot, and the thcker lnes sequence of astersks the trackers trajectores. Wth thck dashed trangles s marked the formaton evoluton. VI. CONCLUSIONS Mult-robot estmaton and trackng of a movng target was dscussed n the paper. An ntegrated approach to sensng and control of two robots was presented, when each robot s equpped wth a drectonal sensor wth lmted angular range. The sensor readngs were fused n order to get an estmate of the targets moton. A herarchcal control strategy was developed and tested, n whch hgh-level commands were ssued to plan a seres of desred formatons for the robots. Low-level trackng of paths connectng wayponts defned by the formatons was specfed accordng to the recent vrtual vehcle method. Ongong work ncludes a systematc treatment of communcaton lmtatons n the system. REFERENCES [1] T. Balch and R. Arkn, Behavor-based formaton control for mult-robot teams, IEEE Transacton on Robotcs and Automaton, vol. 14, no. 6, pp , [2] B. Dunbar and R. Murray, Model predctve control of coordnateultvehcle formatons, n IEEE Conference on Decson and Control, 22. [3] P. Ögren, E. Forell, and N. Leonard, Formatons wth a msson: Stable coordnaton of vehcle group maneuvers, n Internatonal Symposum on Mathematcal Theory of Networks and Systems, 22. [4] N. Leonard and E. Forell, Vrtual leaders, artfcal potentals and coordnated control of groups, n IEEE Conference on Decson and Control, 21, pp [5] H. G. Tanner, G. J. Pappas, and V. Kumar, Leader-to-formaton stablty, n IEEE Transactons on Robotcs and Automaton, 24,to appear. [6] M. Egerstedt, X. Hu, and A. Stotsky, Control of moble platforms usng a vrtual vehcle approach, IEEE Transacton on Automatc Control, vol. 46, no. 11, pp , 21. [7] T. Gustav and X. hu, Formaton control for moble robots wth lmted sensor nformaton, n IEEE Conference on Robotcs and automaton, 25. [8] K-Team S.A. [Onlne]. Avalable:

7 x 1,y 1 18 y coord. [mm] x 1 t 1,y 1 t 1 x 1 ref t 1,y 1 ref t 1 x 1 t 2,y 1 t 2 x 1 ref t 2,y 1 ref t 2 x 1 f t 2,y 1 f t x coord. [mm] Fg. 12. Bg zoom from Fgure 1, n a smlar manner to Fgure 5, showng how one Khepera II robot follows the trajectores desgned at the path plannng level. The fgure presents wth a thck dash lne the path planner trajectory and wth a thn lne the robot moton. Crcles mark the desred endng ponts from the path plannng, and wth astersks the poston of the robot at the samplng nstants. Also ponted by an arrow a corrected pont s remarked.