Analysis of a Geometric Path Tracking Method for a Nonholonomic Mobile Robots Based on

- 2926 - Analysis of a Geometric Path Tracking Method for a Nonholonomic Mobile Robots Based on Vector Pursuit DongHyung Kim1, ByungGab Yu1, JiYeong Lee2 and ChangSoo Han2 1Department of Mechanical Engineering, University of Hanyang, Seoul, Korea (Tel : +82-31-400-4062; E-mail: fileman@hanyang.ac.kr) 2Department of Mechanical and Information Management Engineering, University of Hanyang, Ansan, Korea (Tel : +82-31-400-5253; E-mail: jiyeongl@hanyang.ac.kr) Abstract: The path tracking is important method in the autonomous navigation. The path tracking controller should be robust with the large tracking error, and the geometric path tracking methods already have an ability to overcome that problem. In this paper, we deal with the vector pursuit which is the one of the geometric path tracking methods. We proposed the new path tracking control system which can determines the look-ahead distance with the given constraints, the reference velocity and the limit on the angular velocity. The simulation and the experiment result show that the mobile robot satisfies the constraints while it follows the path. 1. INTRODUCTION Many path tracking approaches for unmanned ground vehicles are developed but most of those works focus on the stability and the convergence of the tracking controller [5] so they are unable to give the intuitive sense in tuning the controller. The geometric path tracking method[1][2][3] use the look-ahead point which is similar to a driver’s looking point, so the look-ahead distance should be chosen properly. For example, as the vehicle speed increases, the look-ahead distance should increase also. Wit, J. S. [1][2] compare the performances of the geometric path tracking control methods, follow the carrot, pure pursuit and vector pursuit. The results show that vector pursuit is the most robust with large tracking error that occurs when the mobile robot strays away from the path, for example, to avoid the obstacle. Therefore vector pursuit has an advantage for applying to the mobile robot. This research focus on the Vector Pursuit[1] which is one of the geometric path tracking methods. This method is based on screw theory that describes the instantaneous motion of the rigid body. If the mobile robot is considered as a rigid body, screw theory is suitable to generate the desired motion of the mobile robot. This paper presents a method that determine the look-ahead distance for a given motion constraints. With given the reference velocity and the limit on the angular velocity, the look-ahead distance is determined analytically. With this work, the new path tracking control system is developed. The proposed geometric path tracking is tested through the simulation and the experiment. 2. VECTOR PURSUIT PATH TRACKING Vector pursuit is different from other geometric path tracking method in that it is based on a screw theory, which is a way to express the rigid body motion combining both rotational and translational parts. If we consider a mobile robot as a rigid body, it is natural to Figure 1. The coordinate systems and the description of the desired motion describe the instantaneous motion of a mobile robot by using screw theory. 2.1 Model Before we consider the path tracking method, the specific model for the mobile robot should be defined. In this paper, we use a simple unicycle model, which is shown in the figure 1, that (x,y) and Rθ are the position and the orientation of the mobile robot. Then a kinematic model is given as cos 0 sin 0 0 1 R R R x y v θ θ ω θ ª º ª º ª º « » « » « »= +« » « » « » « » « » « »¬ ¼ ¬ ¼ ¬ ¼ � � � (1) where v is the linear velocity, and ω is the angular velocity. Because the model of the mobile robot is unicycle, the nonholonomic constraints are exists. That is, the motion orthogonal to the robot’s forward is impossible W Y W X R X RY L X LY L Lθ Rθ 1iw − iw d L P 'P Pθ R ( ),x y ( )' , 'x yp p ICROS-SICE International Joint Conference 2009 August 18-21, 2009, Fukuoka International Congress Center, Japan PR0002/09/0000-2926 ¥400 © 2009 SICE - 2927 - Figure 2. Instantaneous motion a and this constraint can be expresse equation. sin( ) cos( )R Rx yθ θ− =� � 2.2 Screw theory For describing the motion of the rigid in the figure 2, a screw can be thoug about an axis and a translation along tha In the figure 2, a rigid body is rotate velocity, ω about a screw, $ that ha centerline of a screw is defined by usin coordinates. If we define a unit vector, direction with the centerline and a mom the line about the origin, the vectors Plucker�� line coordinates of this li coordinates, the velocity of the rigi expressed by 0$ ( ; )hS Sω ω ω= where 0 0hS S hS r S hS= + = × + and r is any vector from the origin to the screw. 2.3 Vector Pursuit The global planner produces a seri and the path is represented as a series connecting these waypoints. For exam shows a mobile robot that following between the waypoints 1iw − and iw . At first, we consider the coordinat mobile robot. The coordinate systems c figure 1. The world coordinate s coordinate system and each axis is superscript, W. The robot coordinate sy a superscript R, is a moving coordinate to the robot, with x-axis aligned with th robot. Finally, the look-ahead coord defined using the look-ahead point. point is defined to be a point on the pl the distance L, the look-ahead dist orthogonal projection of the robot’s p planned path. The look-ahead coordina origin at the look-ahead point, with direction of the path. To calculate the screws, there ar methods in [1] but in this paper, we based on the first one of these. This ignores the nonholonomic constraints an about a screw ed as following 0 (2) d body, as shown ght as a rotation at same axis. e with an angular as a pitch, h. The ng Plucker�� line S , that has same ment vector, So ,of (S ; So) are the ine. Using this id body can be (3) S (4) the centerline of ies of waypoints, of line segments mple, the figure 1 a path segment te systems for a can be seen in the ystem is fixed s denoted by a stem, denoted by e system attached he heading of the dinate system is The look-ahead lanned path, with tance, from the position onto the ate system has its its x-axis in the re two different e used a method method initially nd considers it Figure 3. Vehicle motion screw is continua later. Finally, like the other methods, vector pursuit me turning radius. The calculati four steps. 1) Calculate the two translation screw $ $r for the look-ahea The translation screw can b an infinite pitch. Using the e pitch, the translation screw is $ (0v = The translation screw that m WyR), to the look-ahaed point ( $ 0, 0, 0 ; W W L t xv d § −= ©¨ Next, the rotation screw has the equation (3) with a zer equation $ (ω ω= Using this result, the rotatio ($ 0, 0, 1W r ω= 2) Calculate the desire that does not con constraints. The desired instantaneous $ $W Wd t= 0,0, ; W W W L R x xy v d ω ω§ § −= +¨ ¨¨ ©© The equation (9) describes rotate at the same time. If th translation screw defined b velocity v, then the time look-ahead point is tt = Similarly, if the mobile rob R X RY $d R y n if desired instantaneous ally executed. r geometric path tracking ethods give us a desired ion processes consist of a instantaneous screw, the $t and the rotation screw ad point. be thought as a screw with equation (3) with a infinite defined by 0; )vS (5) moves the robot from (WxR, (WxL, WyL) is , , 0 W W W R L Rx y y d d ·− − ¹¸ (6) s a zero pitch. Again, using ro pitch, we can get the 0; )S Sω ω (7) on screw is ); , , 0W WR Ry x− (8) ed instantaneous screw $d nsider the nonholonomic screw is defined by $W r+ (9) , , 0 W W WR L R R x y yx v d ω ·· § ·−− + ¸¸ ¨ ¸ ¸¹ © ¹ ¹ a motion to translate and he mobile robot has a the by $t with the constant e required to reach the d v = (10) bot follows the rotation $R dω $d R x - 2928 - Figure 4. The desired screw and the look-ahead point on the circle screw defined by $r with the constant angular velocity ω , the time required to rotate from the current orientation to the orientation at the look-ahead point is L R rt θ θ ω −= (11) where Lθ is the angle from the x-axis of the world coordinate system to the x-axis of the look-ahead coordinate system and Rθ is the orientation of the mobile robot with respect to the world coordinate. Those angles are described in the figure 1. And we assume that the relation between tt and tr be r tt kt= (12) where k is positive constant. Using these results, the position of the centerline for the desired instantaneous screw can be calculated with respect to the world coordinate system $ $ , d d W W W W W W WL R L R R V L R W W W W W W WL R L R R V L R y y y yvx x x k d x x x xvy y y k d ω θ θ ω θ θ § ·§ ·− −= − = − ¨ ¸¨ ¸ −© ¹ © ¹ § ·§ ·− −= + = + ¨ ¸¨ ¸ −© ¹ © ¹ (13) It can be rewritten with respect to the robot coordinate ( ) ( ) $ $ $ $ $ $ cos( ) sin( ) cos( ) sin( ) sin( ) cos( ) sin( ) cos( ) d d d d d d R W W R R R R W W R R R W W R R R R W W R R x x y x y y x y x y θ θ θ θ θ θ θ θ = + − + = − + − − + (14) 3) Calculate a new desired instantaneous screw '$d with a new look-ahead point for considering the nonholonomic constraint which is given by the equation (2). The desired instantaneous screw, which is given by the equation (9), represents a motion as shown in the figure 3 which is impossible due to nonholonomic constraint. So we need to calculate the new desired screw, $d ′ based on the desired instantaneous screw, $d . In the figure 5, the motion of the new desired Figure 5. New desired screw and new look-ahead point instantaneous screw can be described as a rotation along a circle centered at the centerline of desired screw with radius R, tangent to the robot’s x-axis. At the figure 4, to find the location of the look-ahead point with respect to the robot coordinate system, the angle from the x-axis of the robot coordinate system to the centerline of the desired screw is ( )$ $atan2 ,d dR Ry xα = (15) And the angle between the line from the robot position to the look-ahead point, p is calculated by 1 2 2 $ $ cos 2 d d R R L x y β −= + (16) Let the angle from the robot coordinate x-axis to the look-ahead point be γ . If the direction of the desired screw’s centerline is positive which is already described in the figure 4 and 5, γ is determined by γ α β= − (17) But if the direction of the desired screw’s centerline is negative, then γ α β= + (18) From the figure 5, the new look-ahead point with respect to the robot coordinate can be calculated by cos( ) sin( ) R L R L x L y L γ γ = = (15) 4) Calculate the desired turning radius, R. To find the desired turning radius, the calculation process is as followed. At first, we can fine the three equations from the geometrical relations in the figure 5. 2 2 2 2 2 2 R L R L V R L L a x R R a y x y L + = = + + = (16) Therefore, the desired turning radius is 2 . 2R L LR y = (17) In this equation, the sign of the desired turning radius determines the direction of the rotation. And the desired turning radius can be calculated with given current configuration of the mobile robot and the current waypoints using the look-ahead distance. R X RY L $R dω p α β ( )$ $,d dR Rx y R X RY R a R Lx R Ly L '$ R dω $R dω p - 2929 - Figure 6. Trajectory when the mobile robot follow a path A, B to B, A 3. THE LOOK-AHEAD DISTANCE WITH THE ANGULAR VELOCITY CONSTRAINT The determination of the look-ahead distance given motion constraints of robot is an important problem in the geometric path tracking methods. In this research, we consider the case when there is a limit maxω on the angular velocity to find the relationship between maxω and the look-ahead distance. The given path consists of a series of straight lines and the lines are the connection between the waypoints. The angles between any two lines imply a turning motion to the mobile robot. And the largest angular velocity occurs with the largest angle difference, 180 degree. Therefore, to get the look-ahead distance with given maxω , the situation for 180 degree turn which is shown in the figure 6 is considered. In this figure, the path is given by the waypoints A䜮B䜮A and the trajectory of the mobile robot have a continuous curvature because it has a constant linear velocity. The equation (11) can be rewritten with the equations (10), (12) ( )L R L R L R r t v t kt kd θ θ θ θ θ θω − − −= = = (18) The robot start turn at the point B, so at that time L Rθ θ π− = (19) is satisfied. And also the distance d become L. Substituting this result to the equation (18), the desired look-ahead distance for satisfying the maximum angular velocity limit is max ref des v L k π ω= (20) with the inputs, vref and maxω . The above equation (20) means that if we select the look-ahead desL L≥ , the mobile robot do not exceed the angular velocity maxω . The look-ahead distance is proportional to the velocity and inversely proportional to the angular velocity. Figure 7. The block diagram for the path tracking control system 4. APPLICATION OF VECTOR PURSUIT PATH TRACKING METHOD 4.1 Following the waypoints The look-ahead distance is used as a way to determine whether the mobile robot clear the waypoint or not. If the waypoint is on the inner side of the circle that the origin is same with the mobile robot’s position and the radius is the look-ahead distance, then the waypoint is cleared. And then the mobile robot will follow the next waypoints. Therefore we can easily know that small look-ahead distance make a sharp turn while the long look-ahead distance make a smooth turn. It is very intuitive. For instance, if the driver wants to make a large turn, then the driver needs to look a far-distance during a turning motion. The other side, the driver need to look a short-distance. 4.2 Making use of the desired turning radius In application problem, the mobile robot should be able to making the desired turning radius. If we ignore the lateral direction slippage, we can fine the desired velocity for each wheel in terms of the desired turning radius and the reference velocity. For deal with that mobile robot which has two differential wheels, the simple equations are used as the desired velocity for each wheel in terms of the desired turning radius, R. 1 2 1 2 R ref L ref lv v R lv v R § ·= +¨ ¸© ¹ § ·= +¨ ¸© ¹ (21) In here, vref is the reference linear velocity and l is the distance between two wheels. 4.3 The block diagram of the path tracking control system The path tracking is sort of the control method. Therefore the whole process of this research can be described as the diagram in the figure 7. As shown in the figure 7, the path tracking controller has a four input and an one output. From the sensor of the mobile robot, the controller obtains the configuration, the position and the orientation. The waypoints are fixed data. But the reference velocity and the angular velocity constraint can be constant or variable. The only output of the path tracking controller A B wˆ›G{™ˆŠ’•ŽG j–•›™–““Œ™ t–‰“ŒG y–‰–› Mobile Robot Configuration, [ ]TRx y θ Waypoints, w0,w1,…,wn Desired Turning Radius, R Reference Velocity, vref max Limit on the angular velocity (constraint), ω - 2930 - Figure 8. Simulation result, trajectory of the mobile robot is the desired turning radius. In the previous geometric path tracking methods, the look-ahead distance is the input of the path tracking controller. But by making the look-ahead distance as the function of the linear velocity and the angular velocity, the desired turning radius can be determined with those other parameters. 5. RESULTS In this research, the ability to follow the path while the mobile robot satisfies the given constraints is the key point. And the mobile robot should clear the waypoints in other. To make it sure the path tracking method which is proposed in this paper, the result of the simulation and the experiment are performed. Of course, in the simulation the desired angular velocity and the angular velocity of the mobile robot is same. But in the experiment, as shown in the figure 12, the error between the desired turning radius and the measured angular velocity exist. 5.1 Simulation result At first, a simulation is performed. The initial position is (0,0) and the initial orientation is 0 degree. This is corresponding with the initial configuration [0 0 0]T. The reference velocity is 0.5m/s and the limit on the angular velocity is 45degree/sec and the path is given by the 6-waypoints, w0=(0,0), w1=(6,0), w2=(6,5), w3=(2,7), w4=(8,8), w5=(10,6). The figure 8, 9, 10 shows the result of the simulation. The waypoints are denoted by a small circle in the figure 8. And in the figure 9, the desired turning radius in the equation (17) is changed by the desired angular velocity using the equation ref des v R ω = (22) 5.2 Experiment result The popular mobile robot, The Pioneer 2 is used as Figure 9. Simulation result, time versus angular velocity the test platform. The test condition is same with the previous simulation. The result of the trajectory is similar with the simulation. But in the figure 12, during small interval, there are some noises on the measurement. 6. CONCLUTION In this paper, the vector pursuit is used as a path tracking method for the mobile robot. And using the vector pursuit with the look-ahead distance which is suggested by the function of the linear velocity and the limit on the angular velocity, the new path tracking control system is developed. The simple path that contain the 6-waypoints are used as an example for testing the path tracking with the proposed path tracking method. The simulation and the experiment are performed to that path. Results show that the mobile robot does not violate the limit on the angular velocity. In the future, this proposed path tracking method will be combined with the path planning problem. By perceiving the environment, the location of the planned waypoints can be changed to avoid the obstacle. Figure 10. Simulation result, time versus position 0 1 2 3 4 5 6 7 8 9 10 11 -2 -1 0 1 2 3 4 5 6 7 8 9 x position(m) y po si tio n( m ) 0 5 10 15 20 25 30 35 40 45 50 -50 -40 -30 -20 -10 0 10 20 30 40 Time(sec) A ng ul ar V el oc ity (d eg /s ec ) 0 5 10 15 20 25 30 35 40 45 50 0 1 2 3 4 5 6 7 8 9 10 Time(sec) P os iti on (m ) x position y position - 2931 - Figure 11. Experiment result, trajectory of the mobile robot Figure 12. Experiment result, time versus angular velocity Figure 13. Experiment result, time versus position ACKNOWLEDGMENT This research was supported by a grant from Construction Technology Innovation Program(CTIP) funded by Ministry of Land, Transportation and Maritime Affairs(MLTM) of Korean government. And this research was supported by SRC/ERC program of MOST (grant # R11-2005-056-03003-0) REFERENCES [1] Wit, J. S., “Vector Pursuit