Hybrid Controller for an Unmanned Ground Vehicle with Non-linear/Hybrid Dynamics

Transcription

1 Hybrid Controller for an Unmanned Ground Vehicle with Non-linear/Hybrid Dynamics James Goppert School of Aeronautics and Astronautics Engineering Purdue University West Lafayette, Indiana Brandon Wampler School of Aeronautics and Astronautics Engineering Purdue University West Lafayette, Indiana Abstract This report presents a method of controlling an unmanned ground vehicel (UGV) by employing hybrid control theory. The linear dynamics of the UGV near a straight path conform to modern linear quadratic regulator theory. Two different methods for regulation of the UGV on a straight track are analyzed. The first method employs the steering angle as an input, while the second employs the derivative of the steering angle as the input. While steering angle feedback offers a solution of lower state dimension, it was found that the noise in the steering angle command was too large for driving typical servo motors. When the UGV is turning the linear dynamics are violated. At the accelerations required for our UGV during cornering, side slip cannot be neglected. As the vehicle skids while cornering, linear assumptions are violated. Various surfaces can also change the cornering accelerations at which significant tire slip occurs. A simple yaw rate feedback PID controller is employed in an attempt to maintain a constant acceleration during the turn and ensure system robustness to environemental variations. Due to the continuous motion of the vehicle, the discrete modes of the controller, and the discrete properties of the UGV s transmission, the system must be controlled considering hybrid automaton theory. A switching scheme is therefore developed to effectively switch between the straight mode s linear quadratic regulator control and the turning modes yaw rate feedback PID control. Fig. 1. Modelled Unmanned Ground Vehicle I. INTRODUCTION Controlling a vehicle to follow a desired path through an environment can be approached in several different ways. The first is to have a path planning algorithm decide the trip and then use a tracker controller to follow the trajectory. This can be complex when considering the computation cost to calculate a continuous trajectory. We implemented a hybrid approach to controlling a car along a desired path in order to simplify the problem and offer several advantages that will be mentioned later. A simpler navigation method would be to assign key waypoints and then navigate between them while rounding the vertices for a smooth driving. This system is hybrid due to the discrete modes used to accomplish the goal and model the system. Straight line segment tracks allow us to place our control coordinate system on the line and make the problem simpler by using a Linear Quadratic Regulator instead of a Linear Quadratic Tracker. We then used a classical PID controller to try and maintain constant lateral acceleration when transitioning to different straight tracks. A. Radius of Curvature Tracking Tracking predefined smooth trajectories is a well developed field in robotics. These trajectories require computation of splines of various orders to connect waypoints along a desired path. The radius of curvature of the path is used to track the curvature of the road and maintain zero cross track. An example of this system is developed in [2], [3], [4], and [5] B. Steering Law Tracking Recent development for the DARPA Grand Challenge for unmanned ground vehicles has resulted in signficant progress. Instead of tracking a desired trajectory directly by minimizing crosstrack, several researchs have chosen to track a desired steering command. The steering command is chosen to be globally stable. This method offers a fast robust solution that has been proven in offroad and racing vehicles. The disadvantage is that global optimality of the path is sacrificed. The steering law depends primarily upon the current state of

2 the vehicle and deviation from the current path. It is not well suited for a look ahead control strategy which would allow higher vehicle performance. C. Contribution The contribution of this paper is that a computationally efficient, yet robust method is developed that can handle nonlinearities associated with tire slip. This method has look ahead capability through the use of a dynamic switching guard used to initialize turns. The simplicity of the waypoint tracking scheme also makes the behaviour predictable for intefacing with human controllers. When applied to the field of obstacle avoidance this method can also be easily interfaced with a locally and globally optimal path finding algorithm. II. HYBRID NONLINEAR DYNAMIC MODEL Fig. 2. UGV Forces and Moments A. Vehicle Paramters The vehicle modelled in this paper has the following parameters: m 3.kg (mass) Iz.6kg m 2 (moment of inertia) C α.16/deg (cornering stiffness) (Smith,23) f d.31/s (drive train drag coefficient) The mass is high due to the onboard processing and sensors. The moment of inertia was estimated given the vehicle mass and geometry. The drive train drag coefficient was assumed to be proportional to velocity. This was done to match the energy dissipation of the drive train when throttle was not applied. The cornering stiffness was referenced from [1] for a standard tire. B. Forces and Moments The state space model used to simulate and control the car is a linearized version of a nonlinear dynamic model. The nonlinear model comes from a simple force model shown in Fig. 2. It is assumed that the forces acting on the tires are proportional to the side slip angles represented by β. The forces on the tires of the same axle are assumed to be equal to each other as well. The forces on the front and rear tires are given by equations eqns. The tangents of the sideslip angles can then be defined by Eq. 2. The deviation system about the reference state of a straight path is then found by linearization about this state. The first order taylor series expansion provides a linear equation for side slip. The longitudinal forces are modelled as the difference from the drive train force and the product of the drag coefficient and the longitudinal velocity. As seen in Eq. 2. ΣF x = F d f d V x F f = 2 C αf (δ β f ) (1) F r = 2 C αr ( β r ) (2) where δ - steering angle C αf/r - cornering stiffness β f/r - side slip The sideslip kinematic equations govern the lateral forces of the tire. In order for the model to be robust, it is important that the side slip angles be well approximated by a linear function within typical steering angles (for our vehicle +/- 3 degrees. tanβ f = (V y + l f ψ)/v x tanβ r = (V y + l r ψ)/v x using small angle approximations β f = (ẏ + l f ψ)/v x β r = (ẏ + l r ψ)/v x where V x - longitudinal velocity V y - lateral velocity ψ - heading angle rate Since tangent can be well approximated (within 1%) as linear over the range +/- 3 degrees, the controller should be robust near a zero side slip reference trajectory. C. Hybrid Transmission Model The transmission of the car was modeled as a hybrid system since it has three discrete modes of operation, forward, reverse, and neutral. To model the response of the speed controller in our RC test car, we used a transfer function shown by Eq. 3 to give the drive train a more realistic behavior. Also, for our RC car the speed control must remain in neutral for 1 second before switching from forward to reverse or visa versa. This delay can be seen in the neutral mode block in Fig. 3, a certain time constraint has to be met before the transmission will switch to forward or reverse. The delay in the neutral mode is a design feature of the speed controller of the RC car. This feature prevents zeno switching executions which could lead to burn out of the DC brush motor.

3 B. Speed Control Fig. 3. D. Hybrid Automaton Definition Transmission Stateflow Diagram Using the previous information, we develop a hybrid automaton defined by the following variables. H = (Q, X, σ, V, Init, f, Inv, R, G) where: Q - discrete modes: throttle (forward, neutral, reverse) X - continous states: position and velocity of UGV σ - discrte input: steering (left, right, straight) waypoint(next, current, intermediate) V - continuous input variables: throttle command, yaw rate command (during turn mode) Init - initial state: vehicle must be on ground for dynamic model assumptions to be valid f - vector field for evolution of continuous dynamics: the vehicle nonlinear dynamic model Inv - invariant set: combination of states and inputs for which continous evolution is allowed R - reset relations: as described in statflow schematics G - guards for transitions on discrete states Q and σ III. PID DESIGN OF SPEED CONTROL AND TURN MODES A PID controller was used to control the velocity of the car and also to sustain the turns when transitioning between waypoint tracks. The velocity is controlled by feeding back the velocity error and tuning the PID until we reached a rise time of 1 second without excessive overshoot or oscillation. A similar procedure was used for tuning the PID of the turn mode. In this case yaw rate error was fed back. The desired yaw rate was calculated based on turn radius and velocity discussed in the next section. The PID controller on yaw rate allows us to react to slipping and sliding of the car around corners while still trying to maintain a desired yaw rate without excessive oscillation. A. Servo Plant Model Equation 3 is the model of the servo dynamics for both the throttle servo and the steering servo. The transfer function essentially acts a low pass filter reducing noise significantly that is over 1 Hz. The initial and final value theorem ensure that the transfer function tracks the input command proportionally. The speed control loop uses velocity error feedback and a PID controller. The gains of the PID controller are set to have a fast rise time to ensure turning initiates at the point a turn is commanded. This rise time is important to the hybrid system because it effects the switching behaviour of the system. Where higher precision is needed this rise time could be modelled as a transient state in the hybrid system between the straight and turn mode. C. Turn Modes 1 s + 1 When in either the left or the right turn mode the vehicle attempts to maintain a constant turn acceleration. The acceleration is maintained by yaw rate feedback routed to the steering angle input. Again, the rise time is important to ensure that the switching can be approximated as instantaneous. As in the speed control loop the transient response of the controller can be modelled in an intermediate mode between the straight and turn mode. IV. LQR DESIGN OF STRAIGHT MODE The straight mode consists of a LQR with full state feedback to control heading and cross track. The LQR requires a linear system so we convert our system to a linear deviation system, linearized about zero heading and zero cross track. The linear deviation system is given by Eq. 4. A. Available Feedback A simulated INS system is used to provide position, velocity, acceleration, heading, and roll rate estimates to the system. The linear deviation system about the reference trajectory requires several specific states. First, for the longitudinal control the longitudinal velocity V x is required. This velocity is fedback for the feedback to the speed controller. Also V x can be used to gain schedule the LQR for various straight away velocities. For this paper only one straight away velocity was used (5m/s). The lateral system requires the cross track, the cross track velocity, the yaw angle, and the yaw rate angle. These states are all available from the INS unit but there is inherent noise in the system. You will notice in Fig. 17 that the cross track is not zero. This deviation is not a result of steady state error in the control scheme, but it is a result of INS error propagation in position. (3)

4 B. Linearization of UGV Dynamics 1 2C αf +2C αr 2C αf +2C αr mv x m 1 2C αf l f +2C αrl r 2C αf l f +2C αrl r I zv x I z ẋ = Ax + Bu x = e1 e1 e2 e2 2C αf l f +2C αrl r mv x 2C αf l 2 f +2Cαrl2 r I zv x 2C alphaf m 2C αf l f Iz A = B = where: u - delta, steering angle (input) e1 - error in crosstrack e2 - error in heading C αf/r - cornering stiffness for front/rear tire l f /r - distance to front/rear axel from c.g. I z - polar moment of inertia m - mass of vehicle V x - vehicle longitudinal velocity C. Continous Algebraic Ricatti Equation Formulation The LQR is formulated as follows for a continuous time system (x) = Ax + Bu with cost function J = (x Qx + u Ru)dt the cost minimizing feedback law is u = Kx where K is K = R 1 B P and P is found by solving the continuous time ARE A P + P A P BR 1 B P + Q = D. Performance Index Design Where K is the steady state Kalman gain. The LQR optimizes the response to minimize the cost given by the designed cost function. Our initial LQR design was as follows: Q=diag([1,1,1,1]) R=1 K1 = [ ] These gains gave us good path following but resulted in poor yaw damping. In order to remedy this, another LQR was designed with the following parameters: Q=diag([1,,1,8]) R=.5 K2=[ ] This design penalizes the yaw rate gives the system more control input freedom while neglecting the cross track velocity. This resulted in a better behavior when exiting turns with less yaw oscillation in the straight mode operation. The yaw rate plots for the two LQR designs in Fig. 4 and Fig. 5 show the improvement in yaw rate behavior. E. Steering Rate Feedback vs. Steering Angle Feedback Although behaviour of the system with steering angle feedback performed well, observation of the steering command revealed that the command exceeded the physical constraints of the system. Although the servo plant was filtering out high frequency inputs, the robustness of the LQR was violated due to the false assumption that the steering angle could be instantaneously commanded. It was therefore necessary to add the steering angle to the state and treat the derivative of the steering angle as a command to the system. Although the actual system accepts a steering angle command to the servo motor, the LQR optimized gain may be applied by integrating the LQR feedback (steering angle rate) to get the steering angle. The previous LQR derivation requires a slight modificatoin given below where A and B are the same as those defined previously: Ap = [A, B;, ]Bp = [; ; ; ; 1] For an initial stable design point Q and R were chosen as the identity matrix. The solution of the continuous algebraic ricatti equation yields the following gain: K = [ ] closed loop poles i i yaw poles i i cross track poles -1. delta poles (steering) As can be observed from the steering history and yaw rate history for the initial stable design, the yaw rate is too high. This can also be observed by the slow yaw poles. By penalizing the yaw rate and yaw position more heavily we can drive the closed loop poles of the yaw further negative. A design point was chosen where the cross track and yaw poles were approximately at the same position along the negative real axis. This ensures the system has a higher time constant for input decay. K = [ ] closed loop poles i i yaw poles delta poles (steering) i i cross track poles Note that the steering angle is less stable, but the overall performance improves due to the yaw and cross track poles since they dominate the system. The yaw poles also have a higher frequency that the cross track poles. It is important that the turn does not excite the resonant frequency of these modes.

5 1 Heading Rate, undesigned 3 Tire Side Slip, designed ψ dot, deg/s β, deg Front Rear Fig. 4. Undesigned Heading Rate Response Fig. 6. Undesigned Tire Slip Angle (β) Response Heading Rate, designed 3 2 Tire Side Slip, designed Front Rear ψ dot, deg/s β, deg Fig. 5. Designed Heading Rate Response Fig. 7. Designed Tire Slip Angle (β) Response This can be avoided through choice of centripetal acceleration. The designed heading rate and tire slip oscillations are both quickly dampled using the modified LQR gains obtained through penalization of the yaw and yaw rate. V. WAYPOINT AND STEERING SWITCHING SCHEMES A. Waypoint Switching In order to make a smooth transition between desired paths, we designed a dynamic waypoint guard that changes based the vehicles current state, direction of the next desired track, and the lateral acceleration desire for the turn. More specifically the guard is a function of the vehicles velocity, angle between current heading and the heading of the next desired track, and the lateral acceleration deemed appropriate for the turn. When the car crosses the longitudinal distance x from the waypoint it is currently tracking, it causes a discrete switch to the next waypoint and a path to follow is drawn. The faster the car is going and the sharper the turn, the further out it will switch to allow more room to turn at the desired rate and still maintain a desired lateral accelleration The equations that govern the guard position x shown in Fig. 8 are: v = R ψ R = v2 a c tan(α/2) = x R x = v2 a c tan(α/2) where: a c - centripetal acceleration

6 Fig. 8. State Dependent Waypoint Guard v - velocity R - radius of curvature α - change in heading angle x - waypoint transition guard The waypoint dynamic guard calculations assume that if there is no sideslip present, the car will transition between the two tracks perfectly along the radius R if the yaw rate ψ is maintained throughout the whole turn. Since there is sideslip allowed in our model, and a PID is used to control the yaw rate of the turn, we usually end up with a little overshoot by sliding beyond the track before the heading of the car matches the track and the LQR takes over. The rise time of the PID controller for yaw rate and velocity also contribute to this overshoot. The waypoint switching diagram is shown in Fig. 9. The guards are along the switching lines and inside of each mode bubble is the set of instructions to execute. In addition to the dynamic longitudinal guard discussed earlier, a lateral guard condition must be met to ensure the car is close to the current track before executing a turn to the next. The intermediate waypoint mode in the bottom of the Fig. 9 is a safety measure to ensure that the desired path can be tracked even if there are large deviations. If the car switches to a new waypoint and is further away than a set minimum, the guidance system will create an intermediate waypoint to drive the car perpendicular to the track until it is close enough to resume normal tracking. B. Steering Mode Control The guard that controls the steering modes is set up in an overlapping fashion as shown in Fig 1. If the heading of the car is more than 2 deg. from the desired path, the car will switch into left or right turning mode to realign itself. When the heading eventually matches that of the desired track Fig. 9. Waypoint Control Stateflow Diagram within 1 deg., the straight mode takes over and the crosstrack LQR quickly corrects the cars position and heading to the desired track. This overlapping guard gives the LQR enough room to operate without switching out immediately after a control is applied as a non overlapping guard would do. The reason for switching to a turning mode is to correct large heading deviations where the linearized model, linearized about zero cross-track and zero heading error, is no longer a valid assumption. The corresponding state flow diagram for the turning modes is shown in Fig. 11. The guard conditions are seen along the transition lines between each of the modes. VI. SIMULATION RESULTS A position drifting behavior is present when running our simulations. This is due to the errors modeled into the inertial navigation system (INS). These errors integrated over time cause a drift in the estimated position away from the actual position. This error propagation is shown in Fig. 15. All three axes exhibit an unstable estimation behavior. This is due to the position information being taken directly from the double integration of the accelerations and moments in the inertial measurement unit including errors. The errors are also visible in the path history shown in Fig. 17. The vehicle is at the end of the loop which is also the start. Notice it thinks it is in the same position that is started in when in fact it is slightly off. The position error starts low and gradually grows. If allowed to continue uncorrected the error will continue to grow without bound. Usually another system is used to correct errors in INS systems such as GPS or another external measurement. In our case, our future work involves using imaging and optical flow to identify and localize landmarks. These landmark points

7 6 Measured Acceleration, designed 4 2 A, m/s Fig. 12. Measured Acceleration Fig. 1. Steering Guard 3 2 Steering Angle, designed 1 delta, deg 1 2 Fig. 11. Steering Control Stateflow Diagram will be grouped into plane landmarks to represent the sides of buildings. We plan on using a Kalman filter to integrate the INS and visual measurements since both are stochastic in nature. There is definitely noise in the INS as discussed earlier. There is also uncertainty in the precision of the measurements coming into the system. These uncertainties in the INS and measurements are represented by covariance matrices. Kalman filters handle these types of problems well, especially when the noise can be assumed to be Gaussian in nature. VII. INS MODEL AND FUTURE WORK The INS model used for the state estimation in this paper assumed the following parameters that were thought to be indicative of medium grade INS systems: scale factor - 1 (uncorrellated) bias - Fig. 13. Steering Angle noise level - IMU : 1e 4m/s 2, GY ROS : 1e 4rad/s In future work we hope to integrate the current INS based estimation and control test bed with another external position estimate for the system. This external position estimate will make the system detectable. Current instruments that are being considered are vision cameras and LIDAR (light detection and ranging) scanners. In an environment where GPS is denied, such as a battlefield or large cities, the ability to localize with respect to the local environment will make unamnned systems more robust. REFERENCES [1] Smith, Nicholas; Understanding Parameters Influencing Tire Modelling, Colorado State University, 24 Formula SAE Platform

8 INS roll, pitch, yaw error propagation, designed error, m Fig. 14. Virtual Reality Test-bed INS position error propagation, designed.5.5 deg Fig. 16. roll pitch yaw Roll, Pitch, Yaw Error 1 N E D Fig. 15. Position Error [2] Rajamani, r.; Vehicle Dynamics and Control, Springer, 26, ISBN: [3] Freunc, Eckhard; Mayr, Rober; Nonlinear Path Control in Automated Vehicle Guidance, IEEE Transactions on Robotics and Automation, vol. 13, no. 1, February 1997, 49. [4] Ben Amar, Faiz, Steering behaviour and control of fast wheeled robots, Laboratoire de Robotique de Paris [5] Hoffmann, Gabriel M.; Autonomous Automobile Trajectory Tracking for Off-Road Driving Controller Design, Experimental Validation and Racing, 27. Fig. 17. Trajectory for LQR with Designed Performance Indices