Patricia Mellodge Chapter 2. Project Background 10 2.3 Current FLASH Development While the cars currently being developed in the FLASH lab are similar to the previous versions in concept, the implementation is very different. Because the cars will be part of a museum display, the emphasis is now on reliability. The cars must be capable of running most of the day with little intervention from the museum staff. Also, the cars must be robust to little kids throwing stuff at them. Current development is done with these issues in mind. These concerns are addressed in the following ways: • Low power components are used wherever p ossible to minimize power consumption. • The cars are capable of automatic recharging and the display itself houses recharging bays. • Manual driving is diabled to prevent museum visitors from controlling the vehicle. • All control processing is done on board the car (rather than sent via wireless link to a PC) to preserve the integrity of the data. • The display is enclosed and the cars have bodies for asthetics and to prevent access to the circuitry. While this section provides an overview and highlights of the current FLASH car develop- ment, complete details of the hardware and software are given in Chapter 6. Chapter 3 Mathematical Modeling and Control Algorithm In this chapter, the kinematic model for the rear wheel drive, front wheel steered robotic car is derived. Using this model, a controller is given to perform path following. 3.1 Mathematical Modeling The model used throughout this work is a kinematic model. This type of model allows for the decoupling of vehicle dynamics from its movement. Therefore, the vehicle’s dynamic properties, such as mass, center of gravity, etc. do not enter into the equations. To derive this model, the nonholonomic constraints of the system are utilized. 3.1.1 Nonholonomic Contraints If a system has restrictions in its velocity, but those restrictions do not cause restrictions in its positioning, the system is said to be nonholonomically constrained. Viewed another way, the system’s local movement is restricted, but not its global movement. Mathematically, this means that the velocity constraints cannot be integrated to position constraints. The most familiar example of a nonholonomic system is demonstrated by a parallel parking maneuver. When a driver arrives next to a parking space, he cannot simply slide his car sideways into the spot. The car is not capable of sliding sideways and this is the velocity restriction. However, by moving the car forwards and backwards and turning the wheels, the car can be placed in the parking space. Ignoring the restrictions caused by external objects, the car can be located at any position with any orientation, despite lack of sideways movement. 11 Patricia Mellodge Chapter 3. Mathematical Modeling and Control Algorithm 12 Figure 3.1: The velocity constraints on a rolling wheel with no slippage. The nonholonomic constraints of each wheel of the mobile robot are shown in Fig. 3.1. The wheel’s velocity is in the direction of rolling. There is no velocity in the perpendicular direction. This model assumes that there is no wheel slippage. 3.1.2 Global Coordinate Model The exact position and orientation of the car in some global coordinate system can be described by four variables. Fig. 3.2 shows each of the variables. The (x, y) coordinates give the location of the center of the rear axle. The car’s angle with respect to the x-axis is given by θ. The steering wheel’s angle with respect to the car’s longitudinal axis is given by φ. From the constraints shown in Fig. 3.1, the velocity of the car in the x and y directions is given as ˙x = v 1 cosθ (3.1) ˙y = v 1 sinθ (3.2) where v 1 is the linear velocity of the rear wheels. The location of the center of the front axle (x 1 , y 1 ) is given by x 1 = x + lcosθ (3.3) y 1 = y + l sinθ (3.4) Patricia Mellodge Chapter 3. Mathematical Modeling and Control Algorithm 13 Figure 3.2: The global coordinate system for the car. and the velocity is given by ˙x 1 = ˙x − l ˙ θsinθ (3.5) ˙y 1 = ˙y + l ˙ θcosθ (3.6) Applying the no-slippage constraint to the front wheels gives ˙x 1 sin(θ + φ) = ˙y 1 cos(θ + φ) (3.7) Inserting (3.5) and (3.6) into (3.7) and solving for ˙ θ yields ˙ θ = tanφ l v 1 (3.8) The complete kinematic model is then given as       ˙x ˙y ˙ θ ˙ φ       =      cosθ sinθ tanφ l 0      v 1 +      0 0 0 1      v 2 (3.9) where v 1 is the linear velocity of the rear wheels and v 2 is the angular velocity of the steering wheels. Patricia Mellodge Chapter 3. Mathematical Modeling and Control Algorithm 14 Figure 3.3: The path coordinates for the car. 3.1.3 Path Coordinate Model The global model is useful for performing simulations and its use is described in Chapter 5. However, on the hardware implementation, the sensors cannot detect the car’s location with respect to some global coordinates. The sensors can only detect the car’s location with respect to the desired path. Therefore, a more useful model is one that describes the car’s behavior in terms of the path coordinates. The path coordinates are shown in Fig. 3.3. The perpendicular distance between the rear axle and the path is given by d. The angle between the car and the tangent to the path is θ p = θ − θ t . The distance traveled along the path starting at some arbitrary initial position is given by s, the arc lengh. The car’s kinematic model in terms of the path coordinates is given by [9]       ˙s ˙ d ˙ θ p ˙ φ       =       cosθ p 1−dc(s) sinθ p tanφ l − c(s)cosθ p 1−dc(s) 0       v 1 +      0 0 0 1      v 2 (3.10) where c(s) is the path’s curvature and is defined as c(s) = dθ t ds Patricia Mellodge Chapter 3. Mathematical Modeling and Control Algorithm 15 3.2 Control Law 3.2.1 Path Following There are three possible tasks that the car could perform: point-to-point stabilization, path following, and trajectory tracking. Point-to-point stabilization requires that the car move from point A to p oint B with no restrictions on its movement between those two points. With path following, the car must move along a geometric path. Trajectory tracking is similar to path following, except the car must follow a path at a given speed. In this project, the goal for the car is path following. The car must sense its position with respect to the path and return to the path if it is off course. The track in the lab contains a white line on a black surface which the car is to follow. In addition, there are magnets beneath the track. The car can sense both of these types of lines and both provide a path for following. A higher level planner, independent of the controller discussed here, is responsible for determining which type of line to follow. The hardware and software that performs the sensing and planning are discussed in Chapter 6. 3.2.2 Chained Form Before developing the controller for the model given in (3.10), the system must be converted into chained form. The (2,n) single-chain form has the following structure [10]: ˙x 1 = u 1 ˙x 2 = u 2 ˙x 3 = x 2 u 1 . . . ˙x n = x n−1 u 1 (3.11) Although the system has two inputs, u 1 and u 2 , this model can be considered single input if u 1 is known a priori. For the car model with four states, the (2,4) chained form becomes ˙x 1 = u 1 ˙x 2 = u 2 ˙x 3 = x 2 u 1 ˙x 4 = x 3 u 1 (3.12) The states are given as x 1 = s (3.13) Patricia Mellodge Chapter 3. Mathematical Modeling and Control Algorithm 16 x 2 = −c  (s)dtanθ p − c(s)(1 − dc(s)) 1 + sin 2 θ p cos 2 θ p + (1 − dc(s)) 2 tanφ lcos 3 θ p (3.14) x 3 = (1 − dc(s))tanθ p (3.15) x 4 = d (3.16) where the variables are defined in Fig. 3.3, c(s) is the path’s curvature, and c  (s) denotes the derivative of c with respect to s. The inputs are defined as follows v 1 = 1 − dc(s) cosθ p u 1 (3.17) v 2 = α 2 (u 2 − α 1 u 1 ) (3.18) where v 1 is the linear velocity of the rear wheels, v 2 is the angular velocity of the steering wheels, and α 1 = ∂x 2 ∂s + ∂x 2 ∂d (1 − dc(s))tanθ p + ∂x 2 ∂θ p  tanφ(1 − dc(s)) lcosθ p − c(s)  α 2 = lcos 3 θ p cos 2 φ (1 − dc(s)) 2 3.2.3 Input-Scaling Controller With the system in chained form, the controller to perform path following can be developed. In this form, path following equates to stabilizing x 2 , x 3 , x 4 from (3.12) to zero. The input scaling controller from [9] is given here. First the variables are redefined as follows χ = (χ 1 , χ 2 , χ 3 , χ 4 ) = (x 1 , x 4 , x 3 , x 2 ) so the chained form system is then ˙χ 1 = u 1 ˙χ 2 = χ 3 u 1 ˙χ 3 = χ 4 u 1 ˙χ 4 = u 2 (3.19) As stated in [9], the system (3.19) is controllable if u 1 (t) is a ”piecewise continuous, bounded, and strictly positive (or negative) function”. With u 1 known a priori, u 2 is left as the only input to the system. The controller for u 2 (with the appropriate restrictions on u 1 ) becomes u 2 = −k 1 |u 1 (t)|χ 2 − k 2 u 1 (t)χ 3 − k 3 |u 1 (t)|χ 4 (3.20) Chapter 4 Curvature Estimation The model for the car and the resulting controller given in the previous chapter require knowledge of the path’s curvature. This chapter describes several methods for estimating the path’s curvature. Except for c(s), all of the variables in (3.10) are known or can be measured by sensors on the car. The feedback control algorithm based on this model must know the curvature to calculate the desired inputs v 1 and v 2 . The problem then is to determine the curvature of the path based on the known or measured variables. In the FLASH lab, a two-foot wide track circuit has been built for prototype development. Several constraints have been placed on the path configuration. One is that the path be continuous. Another is that the path be either straight or a curve of known constant radius. A sample path showing these constraints is shown in Fig. 4.1. This sample path is made up of straight sections and curves of two different radii. The resulting curvature profile is shown in Fig 4.2. From the previous chapter, c(s) is defined as c(s) = dθ t ds Therefore, if the path is turning left, c(s) is positive and if the path is turning right, c(s) is negative. The magnitude of c(s) is 1 R , where R is the radius of the circle describing the curve. As a result the constraints, the curvature of the path as a function of distance is discontinuous and piecewise constant. The derivative of c(s) with respect to distance is zero except for those locations where the curvature changes. There, the derivative is infinite. Therefore, the following assuption is made c  (s) = 0 17 Patricia Mellodge Chapter 4. Curvature Estimation 18 Figure 4.1: A sample path showing the constraints. Figure 4.2: The curvature of the path in Fig. 4.1 with respect to the path length, s. Patricia Mellodge Chapter 4. Curvature Estimation 19 c  (s) = 0 . . . with c  (s) denoting the derivative of c with respect to s. The derivatives are taken to be zero with disturbances where the curvature changes. The curvature of the path is known a priori; the track is built using pieces with known radii. Therefore, the estimation result can be used to select the actual value. In other words, the calculated curvature need not be used for c(s) in the state equations and controller. Rather, the actual curvature value can be selected based on the outcome of the estimation. With the path configuration constraints defined, three methods of curvature estimation are now presented. 4.1 Estimation Methods 4.1.1 Estimation Based on the Steering Angle φ The first method of estimating c(s) is based solely on the steering angle, φ. At steady state, the car’s steering wheels turn with the curves of the path. This method simply estimates the curvature using the steering wheels’ angle. If the front wheels are fixed at a certain angle, the car will describe a circle of a certain radius. Using (3.9), a MATLAB simulation was used to find the radius, R, described for several values of φ. It was found that the relationship between the circle’s curvature, c(s) = 1 R , and φ was nearly a straight line. So the relationship between c(s) and φ was approximated to be c(s) = α + βφ (4.1) where α and β were determined using the method of least squares to fit a line to the data. The sign of c ( s ) is the same as φ . To make this method more robust to noise, the value of φ used in (4.1) can be averaged over several sample periods. By averaging φ, this method provides a good estimate even if the car is oscillating about the desired path. However, (4.1) will work only if the car is generally following the desired path. 4.1.2 Estimation Based on the Vehicle Kinematics The second method of estimating the curvature is based on the vehicle kinematics. If all the variables in (3.10) are known or can be measured, the equation can be solved for c(s). . Chapter 3. Mathematical Modeling and Control Algorithm 14 Figure 3. 3: The path coordinates for the car. 3. 1 .3 Path Coordinate Model The global model is useful for performing simulations and its. [9]       ˙s ˙ d ˙ θ p ˙ φ       =       cosθ p 1−dc(s) sinθ p tanφ l − c(s)cosθ p 1−dc(s) 0       v 1 +      0 0 0 1      v 2 (3. 10) where c(s) is the path s curvature and is defined as c(s) = dθ t ds Patricia Mellodge Chapter 3. Mathematical Modeling and Control Algorithm 15 3. 2 Control Law 3. 2.1 Path Following There. the rear axle and the path is given by d. The angle between the car and the tangent to the path is θ p = θ − θ t . The distance traveled along the path starting at some arbitrary initial position is