Tracking Control and Adaptive Local Navigation for Nonholonomic Mobile Robots

Transcription

1 Tracking Control and Adaptive Local Navigation for Nonholonomic Mobile Robot Alexander Mojaev Andrea Zell Univerity of Tuebingen, Computer Science Dept., Computer Architecture, Sand, D Tuebingen, Germany Abtract. A reactive tracking control law and a flexible local navigation method for non-holonomic mobile vehicle are preented. The tracking controller enable noie tolerant trajectory following and colliion avoidance with a feedback control loop through raw enor data (laer canner, onar, IR). The controller need no knowledge about abolute robot coordinate. Baed on the tracking control a local navigation method i decribed, which allow colliion avoidance and flexible path earch in a changing environment. Introduction A reliable motion control i indipenable for developing an autonomou mobile ytem. When looking at biological ytem one can find out that they can move quickly and without rik of colliion by perception of relative ditance data. Thi property a well a the fact that pecial enor or time-conuming algorithm for elf-localiation are needed in order to determine abolute coordinate and the heading angle of a mobile ytem make it neceary to develop a quick and reactive motion control excluively by up-to-date enor information. In order to olve thi problem a reactive and noie tolerant motion controller wa deigned which make it poible to cope with dynamic obtacle, and which i robut to noiy enor data at the ame time. The principle of the deigned law i to control motion along a given trajectory. One of the advantage of the control i the minimum of input information: only the current, relative deviation from the trajectory (lateral ditance) i needed a input value. For that reaon it i not neceary to plan the whole motion equence in advance. The path i able to adapt itelf reactively to the dynamic environment. In order to make it poible to apply noiy enor data a filtering of the input data wa implemented which reduce the influence of the diturbance on the control. Laer canner, ultraonic enor and infrared enor have turned out to be ucceful in experiment on a reactive motion control. Even in cae of very noiy enor data uch a ultraonic or infrared enor data a colliion-free trajectory i achieved. Previou Work A relative tracking along a given trajectory uing liding-mode technique i decribed in Balluchi et al []. The variable-tructure control law for vehicle orientation tabilize a vehicle moving with approximately contant velocity on the given path. The controller need

2 *, + a lateral ditance from the path, heading angle error and ign of the curvature of the reference path. However, the lat two input value are difficult to extract from enor data when a tracking trajectory i unknown. Fox, Burgard and Thrun preented a dynamic window approach in []. A pace of tranlational and rotational velocitie for a robot equipped with a ynchro-drive i built depending on the environment. A colliion free path i generated by earching for admiible velocitie (dynamic window). A time varying control law for olving the tracking problem i decribed in ([5], Tarín et al). The poition control method allow to drive the mobile ytem from any initial to any final tate configuration while minimizing the input energy. However, the control need global robot coordinate and thu a time conuming localization. Furthermore, no dynamic colliion avoidance i implemented. Toulari and Kambhampati preented a geometric olution for obtacle avoidance [6]. In conideration of bound of robot parameter a colliion-free path i generated uing admiible acceleration and deceleration. Behavior-baed navigation uing hierarchical neural network i decribed in [4] (Pauly). A complex robot tak i divided into everal behavior (colliion avoidance, wall following, earch for free pace) that are realized by different neural network. Synchronou drive kinematic Our mobile robot of type RWI B-, named Robin and Colin, are equipped with a four wheel ynchronou drive. Thee robot are controlled by mean of an input of tranlational and rotational velocitie, and the correponding acceleration,. The non-holonomic kinematic of uch a ytem can be decribed by the differential equation (). () We define the complete tate vector a % &')( "!. The initial tate $#mut be known in order to calculate the dynamic behaviour of the ytem. Figure : The robot in the abolute Carteian coordinate ytem. 4 Differential drive kinematic /. Our mall robot of type Active Media Pioneer DX are equipped with a differential drive kinematic where the wheel are driven by different motor : where. i the radiu of a wheel and i the ditance between the wheel : ()

3 : () L < ? By mean of the following equation the wheel velocitie ;6and 7can be converted into the robot-centered 45 =6 tranlational and rotational velocitie () and vice vera (4): The further. 8> BA 76? :. C DA : (4) calculation are developed in general form for the ynchronou drive kinematic (). For the differential drive the reult can be tranformed by (4). 5 Control of the robot along a given trajectory A non-holonomou vehicle cannot be controlled by a direct input of goal coordinate. For that reaon variou control method are ued in order to make it poible for the robot to reach it target. Many poition control method olve thi problem by uing the odometry information. Due to incorrect odometry data thee procee can only be applied to a limited extent in the real environment. Therefore a poition control law wa developed which need only the relative deviation from a given trajectory a a control input. The cycle i cloed by the loop actuator-environment-enor which make reactive control poible []. EGF 5. Tracking with relative coordinate A continuou a` KF H)I ofhî a` #cbedfhgtrki`x KFONKF LQPSRUTGVXWZYtrk[F]\ aymptotic convergence to the trajectory can achieved by the following exponential tracking function compenation (5) o that the curvature and thu the convergence peed can be modified with the coefficientgtrk. A coordinate ytemkj `mln`pon` u` vgtrk` `trkxwzyozw { MF _Ftrk Figure : The tracking function and convergence of the robot to the trajectory. aligned with the given trajecoryq (Fig. ) i defined. The ytem move along the trajectory o that the abcialr`j àlway u` reult in a tangent. In thi coordinate ytem a tracking trajectory`ttrkì defined which make a convergence with the given trajectoryq poible. By differentiation of (5) the derivation u` (6) and the tangent are calculated: inclination`trk uù} xwzyozwvgtrka`m (7) P JF

4 no reactive mooth u~ wzyozwhgtrk (8) Note that the derivation (6) and the tangent inclination (7) do not directly depend on the time u but only on the lateral ditance of the tracking path and thu the tracking curve i independent on the tranlational velocity. The firt relative control equation now reult from (7): Here denote the input parameter of the control. It i the deviation of the robot from the trajectory `and i determined by enor in real time. One condition of the control law (8) i that the orientation of the robot (heading angle`) mut be identical with the tangent. Becaue thi condition can not be atified, we introduce a compenation of inclination`trk the robot heading angle error (ee Fig. ): ƒ t`trk ` (9) Thi error can be eliminated by the additional rotation with the angular velocity: comp gcompƒ () u ` the compenation parameter. The lowergcomp i, the longer the angle compenation take. The whole control equation arie from (8) in conideration of () a follow: () An etimation of robot relative orientation `with reference to the moving coordinate ytemj `ŽwZy { } `mln` on`can be obtained either indirectly through of` () or directly from enor data. In Fig. the tracking trajectorie and error with variou initial orientation angle are hown. The decribed tracking () work even without the etimation of`(fig., bottom). In cae the equation () correpond to a PI control in which the etimation of the orientation error i realized by the integration of`trk. The developed control poee the following advantage: knowledge about abolute robot coordinate i needed. The input of the control i the actual lateral ditance from the trajectory. tracking. We can continuoly adjut the trajectory to the environment. exponential convergence to the trajectory. where ˆ xgcomp?i u~ wzy zwvgtrk 8ŠgcompwZyOzw9gtrk Œ 5. Corridor following and colliion avoidance We applied thi tracking technique to a corridor following problem. Solving thi tak by our : reactive motion control we can automatically avoid all obtruction if we define the given trajectory a the middle path. The input of the control in the corridor following tak i.left ().right

5 y, [m] traject. 45 degree degree -45 degree error, [m] degree degree -45 degree y, [m] traject. 45 degree degree -45 degree error, [m] degree degree -45 degree Figure : The tracking along a inuoidal function for three different initial robot orientation (-45, and 45 deg.) Top: with% etimation by () ( comp š œ ), bottom: without% etimation (% ' comp pžÿ). Left are hown the tracking trajectorie, right the correponding control error. Other parameter: trk=7, =.7 m/; limit of the maximal rotation acceleration ( max = 5 rad/ ) and velocity ( max =.9 rad/) were alo taken into account. F 4 left left left left F right right right right Figure 4: Corridor following: trajectory definition (left) and imulated tracking (right) =.5 m/. ` right left where.right.left the left and right ditance to the obtacle regarding the robot orientation (Fig. : 4, left). Motion control i realized by (). The heading angle error can either be neglected (if no angle meaurement exit) or calculated from enor data: ( ` ª #«(4) The imulation reult with no knowledge about heading angle ) are hown in Fig. 4, right. 5. Data filtering The control input i determined from enor data and contain noie. Thi noie component i intenified by the differential equation part () (becaue it pectrum contain motly high frequencie) and the mobile ytem can poibly loe control. We ue two filtering method: a imple non-linear filtering baed on the obervation that the ditance derivative.right and.left are very rarely greater than the robot tranlational velocity.right.left. Larger value

6 control input filter ±X²G³with µÿ' c š. time, [] control input pectr frequency, rad/ filter gain filter frequency, rad/ Figure 5: The control input (example in Fig. 4), it Fourier pectrum and the frequency characteritic of the are interpreted a erroneu and ignored:.right~.right~9 ~8 min.right~ ¹ ~.left~.left~ ~8 min.left~ ; ~ (5) Additionally we ue a linear filtering: if we analye the Fourier pectra of the control º^» º#^»Aham»½¼ ¾ ªÀ ÁÃÂ (6) input, we can oberve that the ignal ha motly low frequency component in contrat to noie (Fig. 5). So we proce the input control by a low pa filter (FIR) with minimal time delay to minimize enor noie. The impule characteritic i º#)» c)» Ä)»?? Aham)»tÅ 8? Å«ÆÄ» ÀÈÇ Here (7) are the impule characteritic of an ideal low pa filter withà coefficient and the hamming window with a parameteråé i the filter cut frequency. We do not ue the ÂpÊÌË, c coefficient with» Ž to avoid a ignal delay. Although it implicate a degradation in filter efficiency, the noie damping in the Ï i ufficient to achieve a relevant improvement of the control with very noiy enor data. range DÍÎ c Ð The filtering i realized a a convolution of the control input with impule characteritic: ÑÒ Ó#º)» ^Ô» (8) Fig. 6 demontrate the control improvement in a real environment uing laer canner data: unneceary rotation fluctuation are eliminated. 5.4 Control of tranlational velocity The robot tranlational velocity i determined from the robot free front and ide. min.right.leftditance: min8 max9 min...min min (9) where.min andmin are the afety radiu and the frontal afety ditance.

7 Õ Õ Ö Ö õ ææåå control input control input time, [] time, [] control input pectr control input pectr frequency, rad/ frequency, rad/ Figure 6: Experiment in a real indoor environment: control quality improvement dealing with noiy enor data by filtering, from left to right: the control input, it Fourier pectrum =Ø, and robot path without filtering. Right: the control input proceed with the filter decribed. 5.5 The virtual corridor The virtual corridor i a et of point building a cloed pace within which the robot can move freely. Thi corridor connect the tart poition of the robot! with the local navigationtt"!target (Fig. 7). The virtual corridor i modified during the robot motion: if the target ditance decreae, the rear line of the virtual corridor move forward, leaving ufficient pace for poible alternative target earching, however. The wider the corridor i, the more pace the robot ha for colliion avoidance, although the navigation can take longer. The width { # # } and length { çkç èkè ëòñãíóîôñ (varying.b and (.f) of the virtual corridor are choen depending on the environment propertie. The node coordinate are calculated a follow: { ÛÚ GÚ} { ë"ð"íxîïð { ÝÜ dynamic obtacle ZÜ } f { { ÝÞ ZÞ } ëlíóîl ëtíxît { ãkãë"ìuímîïì ëíxî b.@ù { cö> vö øùö Zcö }ö ŽwZy zw {t t } utœú t Ü8 t ÜÂ where Figure 7: Point of the virtual corridor. and The corridor i firt built in the robot centered coordinate ytem and then rotated in the target direction. The virtual ditance.vc right and.vc left which are needed for motion control, are calculated by well known trigonometry rule. 5.6 Dead end coverage } 8.@Ù { h ÂpÊ } 8.@Ù { ut8 h ÂpÊ } } 8.@Ù { ut8 ÂpÊ.f}.b } 8.@Ù { Â Ê.f}.b} () àßßàááââ ãkãää Uing a radial enor data we can determine maximal colliion-free ditance for each direction. If thee ditance in theû9 deg. ector are lower than the frontal afety ditance, the èkèééêê

8 Figure 8: Adaptive local navigation by the virtual corridor technique, left: ome imulation example, right: experimental navigation in a real world environment. The robot mut reach the target point S and E. robot interpret the ituation a dead end and rotate 8 degree. 5.7 Fuion of virtual and real environment A virtual corridor i alway built in relation to a real environment, which i repreented by a grid map model. Thi environment model and the robot coordinate are continuoly updated by a global localization algorithm. We integrate the real obtacle into our virtual corridor, modifying left and right robot ide ditance ued for tracking control (). The example of flexible local navigation in a changing environment are hown in Fig. 8. Ðenor.right ü Ðenor.left üø 6 Experimental reult ÏÓ#.rightý ÏÓ#.leftý The decribed approache to tracking control and local navigation with onar and a laercanner in indoor environment have been implemented on two mobile robot Robin and Colin of type RWI-B. At an exploration velocity up to the maximal robot velocity ( m/ uing laer canner,.8 m/ uing only onar data) the colliion avoidance, navigation and mapping are executed in real-time. The experiment how high flexibility of the decribed navigation and colliion avoidance technique (Fig. 8, right) in a complex indoor environment. Reference [] A. Balluchi, A. Bicchi, A. Baletrino, G. Caalino, Path Tracking Control for Dubin Car, Proc. IEEE Int. Conf. on Robotic and Automation, (996). [] D. Fox, W. Burgard, S. Thrun, The Dynamic Window Approach to Colliion Avoidance, IEEE Robotic & Automation Magazine, Vol. 4, Num.,, (997). [] A. Mojaev, Umgebungwahrnehmung, Selbtlokaliation und Navigation mit einem mobilen Roboter, Ph.D. thei, Univerity of Tbingen, WSI, Shaker Verlag ISBN , (). [4] M. Pauly, Exploration und Navigation mit hierarchichen Neuronalen Netzen, Autonome Mobile Syteme, 96, G. Schmidt, U. Hanebeck and F. Freyberger (ed.), 8 89, (996). [5] C. Tarín, H. Brugger, B. Tibken, E.P. Hofer, Optimal Control for a Synchronou Driven Unicycle-Like Autonomou mobile Robot, Autonome Mobile Syteme, 99, G. Schmidt, U. Hanebeck and F. Freyberger (ed.),, (999). [6] A. Toulari, C. Kambhampati, Avoiding Moving Obtacle by Deviation from a Mobile Robot Nominal Path, The International Journal of Robotic Reearch, Num. 5, Vol. 8, , May, (999).