A new line tracking method for nonholonomic vehicles

We investigate the problem of finding an algorithm for the movement of a vehicle under the nonholonomic constraint to track a given directed straight line without allowing any spinning motion. We propose a new principle of computing the derivative of path curvature as a linear combination of the current vehicle path curvature, vehicle orientation error, and positional error. We call this function the steering function. By linearization we find an optimal selection of parameters for critically damped motions and obtain a single parameter, /spl sigma/, for tracking, which we call smoothness. The uniform asymptotic stability of the feedback rule is proved through a Lyapunov function. Numerous simulation results as well as experimental results obtained on the autonomous robot Yamabico at the Naval Postgraduate School are included to show the effectiveness of this method.


Introduction
The motion planning/control problems for autonomous vehicles with the nonholonomic constraint have given rise to a vast body of literature in recent years. Due to a celebrated result by Brocket [l], it has been known that nonholonomic wheeled mobile robots can not be stabilized to a given configuration by a smooth feedback control. Therefore, to handle the "configuration tracking problem", some researchers such as Canudas and Sordalen in [2] have proposed piecewise smooth feedback laws for exponentially stabilizing a mobile robot. Samson in 13, 41 has proposed a smooth time varying feedback control law for stabilizing a robot, or even a chained system of wheeled robots to a given final configuration.
While these approaches are mathematically elegant and general enough to be applied to a variety of situations, for the specific problem of "straight-line tracking" the feedback laws given by these procedures do not necessarily yield the simplest and most efficient feedback controllers. The problem considered in this paper is to find an algorithm for the movement of the Fariba Fahroo Department of Mathematics Naval Postgraduate School Monterey, CA 93943 fahrooamat h.nps.navy.mil vehicle to track a given directed line without allowing any spinning motion. Our approach to this problem is based on a geometrical notion leading to a feedback design that is easy to use and implement, and also guarantees the exponential convergence of the mobile vehicle's path from an arbitrary initial configuration to a given directed straight line.
This problem was first studied by one of the authors. In [5] a motion planning algorithm using a sequence of straight lines was proposed. After this preliminary investigation, he found a stable tracking rule using a Lyapunov function [6]. The problem in this paper was to find a suitable pair ( v , w ) , the linear and angular velocity of the vehicle so that the vehicle would track a target moving on a straight directed line. A condition for the parameter in the feedback control rule for a critically damped response was obtained through linearization. The results from both papers were successfully implemented and tested on the Yamabico autonomous vehicle at the University of Tsukuba and at the University of California at Santa Barbara.
Recently, DeSantis, [7] has developed a control rule based on the geometric path tracking principle for a tractor-trailer-like robots to track a straight line or a circular arc. In a related effort, Thuilot and others, [SI have developed a theory based on linearized dynamic feedback as well as time-varying feedbacks for the moving reference trajectory tracking problem for vehicles with several steering wheels.
The new method proposed in this paper is based on the fundamental observation on the role of path curvature in vehicle motion control. In this method, the derivative dK:/ds of curvature is computed rather than the curvature K: itself, where s is path length. A rule to compute the derivative of path curvature is to express it as a linear combination of the vehicle path curvature, vehicle heading errot, and signed positional error Ad. This function is called a steering function in this paper.
The main results of this theory are summarized as follows: (1) The uniform asymptotic stability of the feedback rule is proved through linearization and a Lyapunov function. (2) Assuming the critical damping condition. the steering function contains only one parameter that controls the smoothness (or equivalently. sharpness) of vehicle motions. (3) As the condition I d~/ d s l < IX is satisfied, this scheme ensures the continuity of curvature K that is needed for rigid body vehicle motion [9]. (4) This theory is independent of vehicle architecture.
To demonstrate the effectiveness of this method numerous simulation results are included. This smooth motion control algorithm has been successfully working on the Yamabico autonomous mobile vehicle at Naval Postgraduate School. Because of the simplicity of the steering function, the implementation is easy and the processing time is short. The real trajectories of Yamabico generated by this algorithm are also shown. From a comparison of these results with the simulation results, we observe a good agreement between the two types of results.

Motion Control by Path Curvature
A two dimensional nonholonomic vehicle's status is described by a vehzcle configuration where p and 6 denote its position and heading orientqtion respectively, and K is the path curvature in a glbbal Cartesian coordinate system. All these variables are functions of time or path length. We include the path curvature K in the configuration, partially because a vehicle under the nonholonomic constraint can execute only a path that has curvature continuity [9, 10, 111.
Although a two dimensional vehicle motion has generally three degrees of freedom [9], a nonholonomic motion has only two degrees of freedom. Such a motion can be described as follows: There is one and only one line C fixed on the vehicle so that there is a rotational center c on C with a rotational speed of w (Fig. 1). We call this line C the major axis of the vehicle. An arbitrary point on the major axis can be taken as a vehzcle reference poznt OV. Since the position of c is specified by the distance r from O V , an instantaneous motion Q is described by only two varihbles ( r , w ) or ( r , T I ) , where TI is the linear speed at Ov. A better representation is where K = 1/r is curvature. If the center c of rotation is at infinity, the vehicle is moving on a straight 2909 line and K = 0. This representation clearly shows the two degrees of freedom possessed by nonholonomic vehicle motions. The path curvature K is the best variable as a tool to describe vehicle's motions, because (1) K is more directly related to vehicle control, and (2) the curvature is independent of how a global coordinate system is placed. As we notice every day, the path curvature of an automobile is positively controlled through its steering wheel. In this paper, we will mainly discuss designing the optimal curvature K , but not of speed v. With this background, the configuration q and motion Q are represented as functions of length s as the independent variable rather than time t. By the definition of curvature, and by the fact that the tangential direction of the trajectory is equal to the vehicle heading O(s) (Fig. l), we obtain

Steering Function
Now our original problem becomes the one of finding the "optimal" curvature K by feedback at each moment in order to track a given line L. Actually, rather than computing the curvature itself, we propose a method which computes the derivative dKIds of curvature K , because of the constraint that the curvature must be continuous. Since the magnitude 1x1 is finite, the curvature is always continuous. Let a vehicle q = ( p , 6, K ) X-axis in an exponential rate. In order to study the stability properties of the zero solution and their dependence on the choice of a , b, c , we refer to Lyapunov stability theories. First, we use the indirect method which gives us information about asymptotic stability of the nonlinear equation by studying the related linearized first order system of equations. So we write Equation (9) as a system of first order equations by using the following variables: Figure 2: Principle of Path Tracking be supposed to track a directed line L (Fig. 2). We propose a specific form of a control rule (7) dK:

Linearization
Choosing the "optimal" combination of the three constants ( a , b, c ) is the next important issue to tackle. If we select an inappropriate combination, the vehicle may not converge to the line L or may oscillate around the directed line and takes longer tracking for convergence. We first perform an appropriate coordinate transformation so that the given directed line L becomes the X-axis of the global coordinate system in the positive direction [12]. After this transformation, Thus, the steering function (7) becomes dK: -= -a K -bBcy. ds (9) Notice that this is a nonlinear homogeneous differential equation and there is no closed form solution. We impose the condition, 181 < $, so that the trajectory is a single-valued function y( 2).
We can formulate the problem as achieving asymptotic stability of the zero equilibrium solution of the nonlinear dynamic equation (9) by a proper choice of a, b, c. In this case, uniform asymptotic stabilaty of the zero solution means that the trajectories of the vehicle with certain initial configurations converge to the We would like to have the form where the dot denotes differentiation with respect to 2. We represent all the variables in Equation (9) Now we linearize the system by using the Jacobian of For our problem the eigenvalues X of A satisfy the equation, To obtain the proper conditions on the coefficients for achieving asymptotic stability, we refer to the Routh-Hurwita criterion which says a , b, and c must be all positive and abc > 0. Therefore, we have the following statement. If the eigenvalues are three negative numbers, -kl, -K2, -k3, the parameters a , b, c must satisfy a = k l + 1 2 + k3, Furthermore, if we take all the negative real eigenvalues equal, then the relationship among the constants becomes simpler: We call k the gain of the steering function. Therefore, we can conclude A steering function has only one parameter U .

, Global Stability
The stability analysis presented in the previous section gives us asymptotic stability of the solutions in a local qense. This means that only for the initial configurakions that are sufficiently close to the origin, the subsequent trajectories converge to the X-axis. To study the stability properties of the highly nonlinear system (9), we need to refer to the direct Lyapunov method which gives us more information about the subset of of the real plane called the domain of attraction or the set of initial conditions that result in uniform asymptotic convergence to a desired path. Upon some numerical experiments, we have encountered examples where for some initial conditions the trajectories do not converge to the X-axis. Therefore, we already know that the system lacks global exponential stability. But we can indeed prove a stability result that is stronger than the one obtained by linearization. Under a restriction only on the magnitude of the direction 0, we will prove that all the resulting paths converge to the X-axis, regardless of the initial values of the other variables, y, and n. This result is obtained by the direct method which relies on constructing a positive definite Lyapunov function whose path derivative along the trajectory is negative. We refer to the following theorem  We can now summarize the results on the estimate of the region of asymptotic stability in the following proposition.

Effect of Smoothness c
The only parameter in the steering function is the smoothness a. Selecting an appropriate smoothness in a given situation is fundamentally important. Generally speaking, using a larger smoothness is better for obtaining a smoother motion so that a faster motion is possible. However, in a tighter space, we need to use a smaller smoothness in order to make sharper turns. In this section we show the effect of the value of smoothness on the resultant vehicle motions.  This line tracking algorithm is already implemented as a part of the Yamabico autonomous vehicle software system. Figure 5 shows the difference between the results obtained by simulation and by real experiments. Both results are pretty close and the performance by Yamabico is satisfactory.

Wall Tracking
The wall tracking problem is one of planning a motion for a vehicle to track non-flat parallel walls with a given constant clearance of do, where the vehicle is equipped with side-looking range finders. The vehicle obtains the current clearance Ad to the wall by the sensor in real time in order to keep the clearance A d equal to the given desired clearance do by steering the vehicle. Unless the vehicle is in a transition status, the vehicle's orientation must be equal to the wall direction 81 and its curvature must be 0. This problem can be easily solved using the steering function method. We use a term c(Addo) instead of c a d in Equation (7). One simulation result is given in Figure 6, Part (I). This behavior shows how the vehicle faithfully follows the exact profile of the walls.  Namely, the clearance has some allowance. By this method, the vehicle executes steering so that slight wall-position changes do not cause vehicle's lateral motions (Fig. 6, Part (11)). Only when B significant change occurs, the vehicle steers.

Conclusion
In this paper, the new principle of controlling the derivative of path curvature for smooth nonholonomic motion planning is proposed and is specifically applied to line tracking. Through linearization and the Lyapupov stability theory, we have shown exponential cohvergence of the vehicle's path from an initial condition to a given straight line. This algorithm workslwell not only in simulation, but also in real vehiclq navigation on the Yamabico autonomous mobile robot.
We will report on the application of this principle to other path tracking problems such as tracking a circle, or a more general C1 curve in future work.