Application and Analysis of a Robust Trajectory Tracking Controller for Under-Characterized Autonomous Vehicles

Transcription

1 Application and Analysis of a Robust Trajectory Tracking Controller for Under-Characterized Autonoous Vehicles Melonee Wise and John Hsu Abstract When developing path tracking controllers for autonoous vehicles the dynaic constraints of the vehicle are a critical factor. It is therefore necessary to ensure that all tracking trajectories produced by the controller are sooth and continuous. In this paper, a path tracking controller is proposed and ipleented on an experiental autonoous vehicle. This tracking ethod decouples the low-level heading and steering control of the vehicle fro the ain tracking controller, therefore requiring less vehicle characterization. The results of this paper will show this ethod yields a RMS.25 cross-track error with little to no vehicle characterization. I. INTRODUCTION With the recent activity in autonoous vehicle developent at the DARPA Urban Challenge, any researchers ([], [], [2]) are focusing on developing robust path tracking controllers. These controllers either are part of the path planning or incorporate the low-level steering and speed controllers. Both approaches require extensive vehicle characterization, [2], and repeated calibration runs to ensure stability and accuracy. Additionally ost of the control odels for wheeled obile robots and car-like vehicles are based around the bicycle odel which does not account for tire slippage, suspension stiffness, engine throttle delay, etc. In this paper the tracking controller is treated separately fro the heading and speed controller of the vehicle. This pushes the vehicle characterization into the low level controller so that the vehicle dynaics can be odeled using the bicycle odel. This allows for siplified ipleentation and testing of the tracking algorith. For exaple, adopting the current tracking algorith for driving the vehicle in reverse requires inial changes to the algorith itself; whereas careful characterization of the reverse steering characteristics are required for previously cited ethods. II. THE PATH TRACKING PROBLEM A. Proble Description Given a planar two diensional trajectory coposed of discrete GPS waypoints, the path follower is defined as a odule which coands the vehicle to follow the specified path with soe predefined tracking accuracy and passenger cofort. Given the uncertainties exhibited by the environent (uneven paveents, slippage, etc.) and nonlinear response behaviors of an under-characterized autonoous vehicle, a robust path follower ust be able to track soothly This work was supported by Willow Garage M. Wise is a Senior Engineer with Willow Garage, Menlo Park, CA 9425, USA wise@willowgarage.co J. Hsu is a Senior Engineer with Willow Garage, Menlo Park, CA 9425, USA johnhsu@willowgarage.co and consistently to the target trajectory. Through robust path tracking, the gap between high-level path planning and lowlevel hardware control of an autonoous vehicle is bridged. ) Coordinate Syste: The body frae coordinates, shown in Fig., is a right handed coordinate syste with y-axis pointing forward and z-axis pointing upwards. The rotational degrees of freedos adheres to the right-hand rule with the exception of the yaw angle. Where yaw is lefthanded with respect to the z-axis so it has the sae rotational direction as the heading convention where corresponds to north and 9 corresponds to east. Fig.. Body Frae Axis Syste: x, y and z axis of the vehicle s body frae, with rotational degrees of freedo, pitch, roll and yaw. A. Governing Equations III. CONTROL DESIGN In order to track soothly to a given trajectory, a path ust be coputed fro a given initial location and heading to soe target point and heading on the desired trajectory, Fig. 2. The in-line, cross-track, and heading errors, (e x,e y,e θ ) are given by e x = (x t x c )cos(θ t ) + (y t y c )sin(θ t ) () e y = (x t x c )sin(θ t ) + (y t y c )cos(θ t ) (2) e θ = θ t θ c. (3) Once the path errors are deterined a cubic polynoial, (4), can be used to satisfy dynaic constraints iposed by

2 dv b dt = Av b + (B v b ) ax(,e x ) (D + v b ) ax(, e x ) (7) This results in an acceleration control law of the following for a ax if v s /dt > a ax a control = a ax if v s /dt < a ax. (8) v s /dt otherwise The result of this control law can be used to control the speed of the vehicle and therefore close the loop around the in-line path error. Fig. 2. Heading and error definitions. vehicle dynaics and waypoint path planning (i.e. position, heading, and turning)[]. The cubic polynoial is a function of the cross track error, e y, and the constant c which dictates the steepness of the approach, as seen in Fig. 3. B. Ipleentation The vehicle testing platfor is an autonoized 26 Ford Escape Hybrid. The basic software architecture of this vehicle is presented in Fig. 4; whereas the details of the controller hardware ipleentations are described in section IV-B.. P(x p ) = c(x p ) 3 sign(e y ) (4) Fro (4) the approach path heading angle θ p, can be deterined, where θ t is the target heading on the desired path. Once the approach path heading angle is calculated, the approach path heading angle can be directly used to steer the vehicle via a heading controller. ( ey ) 2/3 θ p = θ t arctan(3c sign(ey )) (5) c Fig. 4. Message Passing Architecture (MPA) construct. Fig. 3. The effect of the paraeter c on the approach path. Since the current approach needs to track to a specific GPS waypoint at every given tie step, a controller for inline position tracking is needed as well as for cross-track position tracking. To extend the controller for in-line error tracking, a odified bang-bang controller (7) is used. The estiated state velocity of the vehicle is given by v s = ė x + (2a ax e x ) /2 v b. (6) where a ax is the axiu acceleration of the vehicle, and v b is scalar value. By odifying the sign function of the bang-bang controller, the estiated state velocity changes soothly satisfying the dynaic constraints of the vehicle. The ain software coponents of our autonoous vehicle syste are coposed of a path planner, a path follower, Message Passing Architecture (MPA), several independent sensor data processors and a few low-level control interfaces. The backbone of the software architecture is the MPA which allows hardware and software driven processes to counicate between each other by passing essages robustly and efficiently through the use of shared eory. The basic structure of MPA is a ring-buffer queue, where all software processes can independently retrieve data chronologically or can insert new data to the end of the queue. The input to the path follower coes fro the path planner odule, where a target path is specified by a list of waypoints in the global frae in the for of (9). x i = x i y i v i t i θ i ;i =,n (9)

3 The velocity profile (v i ) is assigned to the list of waypoints based on hardware and environental constraints such as available torque, path curvature, terrain roughness and posted speed liits. Given that the velocity profile has been deterined, a tie stap (t i ) is assigned to each eleent of the waypoint list along the target trajectory. The path planner odule sends each waypoint of the target path to the path follower based on the corresponding tie stap. Since the target waypoints are a set of discrete path locations, siple two-diensional linear interpolation based on tie is used to generate each target waypoint sent to the path follower. The in-line and cross-track tracking errors, (e x,e y ), are defined by the distance fro the vehicle s current position to the target waypoint at every tie step. Given soe finite tracking error, a robust path follower ust be able to generate a sooth, stable and convergent landing curve to guide the vehicle back towards the intended path. Based on the tracking errors, the path follower coputes the corresponding landing curve, then outputs velocity (v c ) and heading (θ c ) coands to the MPA structure. The velocity and steering coands are picked up by the vehicle s low-level PID controllers. Separate PID loops for speed and heading control are ipleented in the current vehicle platfor. The steering PID controller deterines the steering angle based on input heading angle. While two separate PID controllers for the accelerator and the brake works together to aintain the desired velocity. Additionally, all low-level hardware controller PID s are written in C/C++ languages with an average update rate of around Hz. IV. SIMULATIONS AND EXPERIMENTS A. Siulation ) Siulator Overview: The siulation environent is created using the open source Gazebo Project ( A snap shot of the siulator in action is shown in Fig. 5. A Gazebo vehicle odel has been created with siilar ass properties and acceleration/braking/steering characteristics as the actual vehicle. 2) Siulator Results: Given a test path shown in Fig. 6, the speed profile and the resulting ground tracks and crosstrack errors of the siulated runs are plotted in Figs.7(a) 7(c). The speed profile in Fig. 7(c) has been generated by liiting the overall acceleration and ultiplying the result by a weighting function proportional to the inverse of the path curvature; thus the lateral acceleration at corners are liited by predefined constants. Fro Fig.7(b) it is evident that the cross-track error has axiu agnitude of.2, while in-line tracking error spans ±.5. The reason that the in-line errors are extreely large in coparison to crosstrack errors is due to the fact that the vehicle was tuned in favor of passenger cofort rather than tracking accuracy. By increasing paraeter a ax in (8), the tracking accuracy can be iproved draatically. Unfortunately, increasing in-line tracking accuracy results in noticeably ore aggressive acceleration and braking behavior of the vehicle. In particular, Fig. 5. A Snapshot of the Siulator Fig. 6. Saple Test Path. the speed control odule tends to alternate rapidly between accelerating and braking odes. B. Experients ) Control Hardware Overview: The experiental platfor is a Ford Escape Hybrid shown in Fig. 8, which has been reverse engineered for autonoous control. The existing core systes (steering, gear shift, accelerator, brakes, etc.) are actuated electronically which allows for easy interface with and control of these systes. The vehicle is controlled by four custo 6Bit dspic Microcontroller boards, shown in Fig. 9, which interface with the existing Ford Escape coputer hardware. The controllers are inserted in line, using standard Ford parts, to interface with the existing systes for easy installation, repair, or reoval. Four odules are daisy chained together via a CAN bus and control the gear shift, accelerator, brakes, and steering.

4 2 Car Ground Track Coand Path 3 Fig. 8. Experiental Ford Escape Hybrid (a) Siulation Ground Tracks.5 e x e y tracking errors ().5.5 Fig. 9. Low Level Control Boards speed (/s) tie (sec) (b) Siulation Tracking Errors GPS Speed Path Velocity Coand tie (sec) (c) Siulation Speed Profile Fig. 7. Siulation results tracking path in Fig. 6. i. Gear Shift Module: The gear shift odule not only electronically selects the drive gear of the vehicle; the odule also dictates whether the vehicle is in driver or autonoous ode. This is a design feature built into the syste to quickly switch the vehicle fro huan driver ode to coputer controlled ode. The gear shift odule listens on the vehicle s CAN bus to deterine the shifter position of the vehicle When the vehicle is in low gear the coputer is able to send coands, over the coputer CAN bus, controlling the gear position and other vehicle systes. ii. Accelerator Module: The accelerator odule controls the speed of the vehicle and the turn signals. A RPM sensor in the transission deterines the vehicle speed and broadcasts the speed on the vehicle CAN bus. The accelerator onitors the speed and uses a PID controller to aintain velocity set points dictated by the vehicle coputer. The turn signals are turned on using a siple MOSFET switch that is activated when the odule receives turn signal coand fro the coputer. iii. Brake Module: The brake odule is responsible for sending brake control signals, and turning the brake lights on and off. The Ford Escape brakes are controlled using PWM pulses that increent and decreent an internal counter to increase and decrease the braking force. A PID controller in the brake odule receives set point coands fro the vehicle s coputer and sends

5 pulses to the vehicle accordingly. The brake lights are also turned on using a siple MOSFET switch that is activated when the odule receives a brake signal. iv. Steering Module: The steering odule uses the power assist otor in the Ford Escape to control the steering wheel position. A string potentioeter was added to the steering colun to obtain accurate steering angles. The Ford Escape power assist otor relies on a torque sensor in the steering syste to deterine the aount of assist (torque) required to ove the steering wheel. Siilar to the brakes, a PID controller in the steering odule receives set point coands fro the vehicle s coputer and sends torque values to the vehicle s power assist otor accordingly Car Ground Track Coand Path 2) Sensor Hardware Overview: The vehicle is localized using an integrated senor network that utilizes the vehicle on board diagnostics and the NovAtel SPAN (Synchronized Position Attitude and Navigation) syste. i. On Board Diagnostics: The Ford Escape Hybrid coes equipped with Hall effect sensors on all four wheels and a transission RPM sensor transit data to the vehicle CAN bus. The on board diagnostic port on the vehicle can be used to read the CAN bus which transits vehicle sensor data at a rate of 2Hz. This data is used for siple odoetry and verifying the current position of the vehicle. ii. NovAtel SPAN: The NovAtel SPAN syste integrates a GPS (NovAtel GPS-72L) and IMU (HG7 SPAN62). The GPS-72L receives L-Band frequencies fro the OniSTAR correction service and receives updates at a rate of Hz. The HG7 SPAN62 IMU is a cobined laser ring gyro and acceleroeter with an update rate of Hz. Cobining these two coponents the NovAtel SPAN syste has a published accuracy of. and a second outage accuracy of.39. 3) Experiental Trials & Results: Fig. shows the results of tracking to the test path in Fig.6 using the current algorith in our actual test vehicle. As a result of discrepancies between vehicle dynaics and siulator odel dynaics, the axiu cross-track errors have increases fro.2 to.4 for the actual test vehicle runs while the in-line tracking errors on the actual test vehicle have reained near the sae levels as the siulation runs. An additional test case was perfored to exaine the perforance of the path tracking algorith under higher lateral acceleration loads. A slalo path as shown in Fig. was given to the path follower to track, the results of tracking a ore aggressive path are deonstrated in Fig.2. It is evident in Fig.2(b) that cross-track errors are increased while in-line tracking errors reain unchanged. The increase in cross-track error is due to the fact that the current path tracking algorith is unable to handle lateral sliding otions caused by excessive lateral steering otions. tracking errors () speed (/s) (a) Test Path Ground Tracks tie (sec) (b) Test Path Tracking Errors GPS Speed Path Velocity Coand tie (sec) Fig.. (c) Test Path Speed Profile Test Path Tracking Perforance Results e x e y

6 Fig.. Slalo Path. V. CONCLUSIONS AND FUTURE WORKS A. Conclusions The derivation, ipleentation and test results of a robust and stable path tracking algorith are presented in this paper. The current path tracking schee is well behaved and has sub-eter accuracy without the need for detailed characterization of vehicle dynaics. Even when pushed to the liits, as deonstrated in the slalo test case, the current path tracking algorith is able to track the target path, with soe sacrifice in accuracy, but without exhibiting any undesirable instabilities. B. Future Works As discussed in the conclusion, the in-line tracking controller had a significant negative ipact on the accuracy of the trajectory controller. In light of this fact, future work will be done investigating and using other in-line tracking or velocity control ethods to increase the perforance of the existing path tracking controller. VI. ACKNOWLEDGMENTS We would like to thank our tea leader, Jonathan Stark, without his long hours of hard work and dedication to this project, our research would not have been possible. REFERENCES [] K. C. Koh and H. S. Cho, A Sooth Path Tracking Algorith for Wheeled Mobile Robots with Dynaic Constraints, Journal of Intelligent and Robotic Systes, vol. 24, 999, pp [2] Gabriel Hoffann, Claire Tolin, et. al., Autonoous Autoobile Trajectory Tracking for Off-Road Driving: Controller Design, Experiental Validation and Racing, in Aerican Control Conference, 27, pp [3] Luca Consolini, Aurelio Piazzi, and Mario Tosques, Path Following of Car-Like Vehicles Using Dynaic Inversion, International Journal of Control, vol. 76, 23, pp [4] ChangBoon Low and Danwei Wang, Robust Path Following of Car- Like WMR in the Presence of Skidding Effects in International Conference on Mechatronics, Taipei, Taiwan 25 pp tracking errors () speed (/s) Car Ground Track Coand Path 5 5 (a) Slalo Run Ground Tracks tie (sec) (b) Slalo Run Tracking Errors GPS Speed Path Velocity Coand 5 5 tie (sec) Fig. 2. (c) Slalo Run Speed Profile Slalo Tracking Perforance Results e x e y

7 [5] Margan Davidson and Vikas Bahl, The Scalar ǫ-controller: A Spatial Path tracking Approach for ODV, Ackeran, and Differentially Steered Autonoous Mobile Robots, in International Conference on Robotics and Autoation, Seoul, Korea, 2, pp [6] F. Diaz del Rio, G. Jienez, et al., A Generalization of Path Following for Mobile Robots, in International Conference on Robotics and Autoation, Detroit, MI, 999, pp [7] Cripps Donald, Spatially-Robust Vehicle Path Tracking Using Noral Error Feedback, in Proceedings of SPIE, vol. 4364, 2, pp [8] A. M. Bloch and N. H. McClaroch, Control and Stabilization of Nonholonoic Dynaic Systes, in IEEE Trans. Autoatic Control, vol. 37, 992, pp [9] Yutaka J. Kanayaa and Fariba Fahroo, A New Continuous-Curvature Line/Path-Tracking Method for Car-Like Vehicles, Advanced Robotics, vol. 3, 2, pp [] Willia Travis, Robert Daily, et al., SciAutonics-Auburn Engineering s Low-Cost High-Speed ATV for the 25 DARPA Grand Challenge, Journal of Field Robotics, vol. 23, 26, pp [] Isaac Miller, Sergei Lupashin, et al., Cornell University s 25 DARPA Grand Challenge Entry, Journal of Field Robotics, vol. 23, 26, pp [2] Chris Urson, Charlie Ragusa, et al., A Robust Approach to High- Speed Navigation for Unrehearsed Desert Terrain, Journal of Field Robotics, vol. 23, 26, pp