# A Waypoint Tracking Controller for Autonomous Road Vehicles Using ROS Framework

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Controller Design

#### 2.1. Vehicle Model and Limitations

#### 2.2. Waypoints Interpolation

_{i}(i ∈ [0,n]) to join (see Figure 4a,b). To simplify segments handling, it is usual to normalize U into u ∈ [0,1] for each segment, obtaining the spline definition Q(u) = (X

_{i}(u), Y

_{i}(u)) ∀ i ∈ [0,n−1] that is shown in Figure 4c, with cubic polynomials for each segment:

_{i}(u) and Y

_{i}(u) of the segment i are calculated to fulfill continuity conditions in first and second derivatives at waypoints P

_{i}and P

_{i+1}. For the initial and final segments, a certain orientation in the x-y plane is also imposed at the initial and final waypoints of the spline: θ

_{0}and θ

_{n}, respectively (see Figure 4a).

_{i}(u) polynomial of segment i, and calling D

_{iy}and D

_{(i+1)y}to its first derivatives at points P

_{i}and P

_{i+1}, respectively (so D

_{iy}= Y

_{i}′(0) and D

_{(i+1)y}= Y

_{i}′(1)), we obtain the following expressions for its coefficients:

_{0}and θ

_{n}) can be translated into slope conditions for Y(u) and X(u) polynomials in P

_{0}and P

_{n}in the following way:

_{i}:

_{i}(u) polynomial of segment i, replacing sine with cosine functions in Equation (7).

#### 2.3. Linear Velocity Profiler and Curvature Calculation

_{MAX}is the maximum allowed linear velocity for the vehicle and RC

_{MAX}is the maximum value of the radius of curvature that reduces the linear velocity below V

_{MAX}.

_{i}assigned to each segment of the spline is calculated by averaging the N subsequent segments velocities, using a vector λ of normalized weights:

#### 2.4. Lateral Control

_{e}θ

_{e}]′, defined as the distance from the control point to the closest point of the path (that we call reference pose χ

_{d}= (x

_{d}, y

_{d}, θ

_{d})) and the difference between the vehicle heading and the tangent to the path in the reference point (see Figure 5), respectively.

#### 2.4.1. Reference Pose and Tracking Error Calculation

_{i}(u) and X

_{i}(u) coefficients are always known. To calculate the reference point (x

_{d}, y

_{d}), the value of u = u

_{m}that minimizes the Euclidean distance between the control point (x, y) and the trajectory must be calculated. Setting to zero the first derivative of the distance function, we obtain:

_{m}of the polynomial closest to the last value of u must be chosen, and when this solution overtakes 1, i must be incremented to change to a new spline segment. Therefore, the reference position is given by:

#### 2.4.2. LQR Optimal Controller

_{s}is:

_{11}, q

_{22}and r allow an intuitive adjustment of the relative importance of minimizing the lateral error, the orientation error and the control effort, respectively. A good initial value for these parameters is the inverse of the quadratic maximum allowed deviation. The value of the feedback matrix K can be obtained from the Ricatti equation in stationary regime [52].

_{11}, q

_{22}and r parameters) in the trajectory, the tracking errors and the actuation signal. Figure 6a shows the reference spline to follow in black color, and the resulting trajectories using: (1) the same value for all the parameters (blue line); (2) a higher value for q

_{11}(red line); (3) a higher value for q

_{22}(green line) and (4) a higher value for r (magenta line). As can be seen in Figure 6b, q

_{11}and q

_{22}minimize the lateral and orientation errors, respectively while a higher value of r results in a smoother signal control but with higher tracking errors. It can also be observed the linear velocity command provided by the linear velocity profiler along the path.

#### 2.5. Delay Compensation

- It is assumed that at the time k in which the control signal is calculated, the available position reading χ
_{r}corresponds to the vehicle position n_{p}sampling periods before. If the vehicle travels at high speeds, this delay introduces a large error in the position used to calculate the control signal. - It is assumed that the control signal calculated at time k will have effect on the vehicle after n
_{c}sampling periods. Obviously, at this time the vehicle will also have modified its position considerably if the speed is high.

_{s}= 0.1 s, and 5 sampling periods of delay have been assumed in position reading and control actuation (n

_{p}= n

_{c}= 5). Matrices Q and R have been experimentally adjusted to provide the best result, obtaining the trajectory and signals shown in red dot-dash line in Figure 8. As can be seen, the delays greatly destabilize the control system, with the lateral error approaching 6 m in the curve sections, which is not admissible for an urban autonomous vehicle.

_{p}+ n

_{c}) the last speed control signals sent to the actuators. In this way, at the sampling time k, the last known position χ(k − n

_{p}) will be integrated until the sampling instant (k + n

_{c}) using the vehicle model and the speeds stored in the buffer:

_{s}the sampling period of the controller.

_{p}= n

_{c}= 5. Results have been obtained with three different configurations, using the best adjustment of Q and R in each case: (1) without delay compensation (shown in red dot-dashed line), (2) compensating only part of the delay (shown in magenta dashed line), specifically n

_{p}= n

_{c}= 3 and (3) compensating the complete delay (shown in continuous blue line). Figure 8a shows the followed trajectories and Figure 8b the temporal evolution of the controller signals. As can be observed, lateral error is reduced from 6 m to less than 1 m using a compensation in which the number of real delays is known and compensated. Even if the number of delays cannot be known in practice and only part of them are compensated, the obtained improvement is relevant. In conclusion, for the proposed control algorithm to be viable in a vehicle traveling at high speeds in cities, it is essential to make a good estimation of the delays introduced by the measurement and actuation systems and compensate them conveniently.

## 3. ROS Implementation

## 4. Experimental Results and Discussion

#### 4.1. Test Bench

#### 4.2. Ablation Tests

_{p}= 10 and n

_{p}= 8 (using T

_{s}= 0.1 s) were estimated for localization measurements and actuation, respectively. As can be seen, improvement due to the delay compensator at low speeds is not very significant, although a faster actuation is observed in curves, which slightly reduces tracking errors and allows the vehicle to complete the route in a shorter time.

_{MAX}= 13.5 m/s, RC

_{MAX}=20 m and λ = [0.5 0.3 0.1 0.1] (see Section 2.2). Results are shown with magenta dashed line in Figure 11. In this case the average speed along the route increases from 6 m/s to 8.65 m/s, but it decreases to values of up to 3 m/s in the areas with greater curvature. This allows the route to be completed in a shorter time, reducing the tracking errors (specially the lateral one) in curves. In the A–B section, the lateral error is reduced from 0.5 m to 0.35 m.

#### 4.3. Comparison with Other Proposals

_{MAX}= 13.5 m/s. Figure 12 and Table 3 show the temporal evolution and quantitative results, respectively.

## 5. Conclusions

## Supplementary Materials

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Barea, R.; Bergasa, L.M.; Romera, E.; López-Guillén, E.; Pérez, O.; Tradacete, M.; López, J. Integrating State-of-the art CNNs for Multi-Sensor 3D Vehicle Detection in Real Autonomous Driving Enritonments. In Proceedings of the 2019 IEEE Intelligent Transportation Systems Conference (ITSC), Auckland, New Zealand, 27–30 October 2019; pp. 1425–1431. [Google Scholar] [CrossRef]
- Barea, R.; Pérez, C.; Bergasa, L.M.; Romera, E.; Molinos, E.; Ocaña, M.; López, J. Vehicle Detection and Localization using 3D Lidar Point Cloud and Image Semantic Segmentation. In Proceedings of the 2018 21st International Conference on Intelligent Transportation Systems (ITSC), Maui, HI, USA, 4–7 November 2018; pp. 3481–3486. [Google Scholar] [CrossRef]
- Kendall, A.; Hawke, J.; Janz, D.; Mazur, P.; Reda, D.; Allen, J.-M.; Lam, V.-D.; Bewley, A.; Shah, A. Learning to drive in a day. In Proceedings of the 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 20–24 May 2019; pp. 8248–8254. [Google Scholar] [CrossRef]
- Ort, L.; Paull, L.; Rus, D. Autonomous Vehicle Navigation in Rural Environments without Detailed Prior Maps. In Proceedings of the 2018 IEEE International Conference on Robotics and Automation (ICRA), Brisbane, Australia, 21–25 May 2018; pp. 2040–2047. [Google Scholar] [CrossRef]
- Zheng, L.; Li, B.; Zhang, H.; Shan, Y.; Zhou, J. A High-Definition Road-Network Model for Self-Driving Vehicles. Int. J. Geo-Inf.
**2018**, 7, 417. [Google Scholar] [CrossRef] [Green Version] - Bétaill, D.; Toledo-Moreo, R. Creating Enhanced Maps for Lane-Level Vehicle Navigation. Trans. Intell. Transp. Syst.
**2010**, 11, 786–798. [Google Scholar] [CrossRef] - Liu, R.; Wang, J.; Zhang, B. High Definition Map for Automated Driving: Overview and Analysis. J. Navig.
**2019**, 1–18. [Google Scholar] [CrossRef] - Tomtom. Available online: https://www.tomtom.com (accessed on 23 April 2020).
- Comma.ai. Available online: https://comma.ai/ (accessed on 23 April 2020).
- OpenStreetMaps. Available online: https://www.openstreetmap.org (accessed on 23 April 2020).
- OpenDRIVE. Available online: http://www.opendrive.org/ (accessed on 23 April 2020).
- Camacho, F.; Pérez-Zuriaga, A.; Campoy-Ungría, J.; García, A.; Tarko, A. Use of Heading Direction for Recreating the Horizontal Alignment on an Existing Road. Comput. Aided Civ. Infrastruct. Eng.
**2015**. [Google Scholar] [CrossRef] - González, D.; Pérez, J.; Milanés, V.; Nashashibi, F. A Review of Motion Planning Technique for Automated Vehicles. IEEE Trans. Intell. Transp. Syst.
**2018**, 17, 1135–1145. [Google Scholar] [CrossRef] - Robot Operating System (ROS). Available online: https://www.ros.org/ (accessed on 24 April 2020).
- LaValle, S.M. Planning Algorithms; Cambridge University Press: New York, NY, USA, 2006. [Google Scholar]
- Kunchev, V.; Jain, L.; Ivancevic, V.; Finn, A. Path Planning and Obstacle Avoidance for Autonomous Mobile Robots: A Review. In Knowledge-Based Intelligent Information and Engineering Systems; Springer: Berlin, Germany, 2006; pp. 537–544. [Google Scholar]
- Reeds, J.; Shepp, L. Optimal paths for a car that goes both forwards and backwards. Pac. J. Math.
**1990**, 145, 367–393. [Google Scholar] [CrossRef] [Green Version] - Hsieh, M.F.; Ozguner, U. A parking algorithm for an autonomous vehicle. In Proceedings of the IEEE Intellignet Vehicles Symposium, Eindhoven, The Netherlands, 4–6 June 2008; pp. 1155–1160. [Google Scholar] [CrossRef]
- Vorovieba, H.; Glaser, S.; Minoiu-Enache, N.; Mammar, S. Automatic parallel parcking with geometric continuous-curvature path planning. In Proceedings of the 2013 10 IEEE International Conference on Networking, Sensing and Control (ICNSC), Evry, Fance, 8–11 June 2013; pp. 418–423. [Google Scholar] [CrossRef] [Green Version]
- Brezak, M.; Petrovic, I. Real-time approximation of clothoids with bounded error for path planning applications. IEEE Trans. Robot.
**2014**, 30, 507–515. [Google Scholar] [CrossRef] - McNaughton, M.; Urmson, C.; Dolan, J.; Lee, J.-W. Motion planning for autonomous driving with a conformal spatiotemporal lattice. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 4889–4895. [Google Scholar] [CrossRef]
- Lee, J.W.; Litkouhi, B. A unified framework of the automated lane centering/changing control for motion smoothness adaptation. In Proceedings of the IEEE Conference on Intelligent Transportation Systems, Anchorage, AK, USA, 16–19 September 2012; pp. 282–287. [Google Scholar] [CrossRef]
- Han, L.; Yashiro, H.; Nejad, H.T.N.; Do, Q.H.; Mita, S. Bezier curve based path planning for autonomous vehicle in urban environment. In Proceedings of the IEEE Intelligent Vehicle Symposium, San Diego, CA, USA, 21–24 June 2010; pp. 1036–1042. [Google Scholar] [CrossRef]
- Liang, Z.; Zheng, G.; Li, J. Automatic parking path optimization based on bezier curve fitting. In Proceedings of the IEEE International Conference on Automation and Logistics, Zhengzhou, China, 15–17 August 2012; pp. 583–587. [Google Scholar] [CrossRef]
- Elbanhawi, M.; Simic, M.; Jazar, R.N. Solutions for path planning using spline parametrization. In Nonlinear Approaches in Engineering Applications; Springer: Cham, Switzerland, 2018. [Google Scholar] [CrossRef]
- Götte, C.; Keller, M.; Nattermann, T.; Haß, C.; Glander, K.-H.; Bertram, T. Spline-based motion planning for automated driving. IFAC-PapersOnLine
**2017**, 50, 9114–9119. [Google Scholar] [CrossRef] - Magid, E.; Lavrenon, R.; Khasianov, A. Modified spline-based path planning for autonomous ground vehicle. In Proceedings of the 14th International Conference on Informatics in Control, Automation and Robotics, Paris, France, 26–28 July 2017; pp. 132–141. [Google Scholar] [CrossRef] [Green Version]
- Kano, H.; Fujioka, H.B. Spline trajectory planning with curvature constraint. In Proceedings of the 2108 Annual American Control Conference (ACC), Milwaukee, WI, USA, 27–29 June 2018; pp. 1963–1968. [Google Scholar] [CrossRef]
- Barton, M.J. Controller Development and Implementation for Path Planning and Following in an Autonomous Urban Vehicle. Bachelor’s Thesis, The University of Sydney, Sydney, Australia, 2001. [Google Scholar]
- Smuel, M.; Hussein, M.; Binti, M. A review of some pure-pursuit based path tracking techniques for control of autonomous vehicle. Int. J. Comput. Appl.
**2016**, 135, 35–38. [Google Scholar] [CrossRef] - Hoffman, G.M.; Tomlin, C.J.; Montemerlo, M.; Thrun, S. Autonomous Automobile Trajectory Tracking for Off-Road Drriving: Controller Design, Experimental Validation and Racing. In Proceedings of the 2007 American Control Conference, New York, NY, USA, 9–13 July 2007; pp. 2296–2301. [Google Scholar] [CrossRef]
- Girbés, V.; Armesto, L.; Tornero, J.; Solanes, J. Smooth kinematic controller vs. Pure-pursuit for non-holonomic vehicles. Conf. Auton. Robot. Syst.
**2011**. [Google Scholar] [CrossRef] - Kissai, M.; Monsuez, B.; Tapus, A. Review of integrated vehicle dynamics control architectures. In Proceedings of the 2017 European Conference on Mobile Robots (ECMR), Paris, France, 6–8 September 2017; pp. 1–8. [Google Scholar] [CrossRef]
- Ni, J.; Hu, J. Dynamics control of autonomous vvehicle at driving limits and experiment on an autonomous formula racing car. Mech. Syst. Signal Process.
**2017**, 90, 154–174. [Google Scholar] [CrossRef] - Mousavinejad, E.; Han, Q.-L.; Yang, F.; Zhu, Y.; Vlacic, L. Integrated control of ground vehicles dynamics via advanced terminal sliding mode control, Vehicle System Dynamics. Int. J. Veh. Mech. Mobil.
**2016**, 55, 268–294. [Google Scholar] [CrossRef] - Han, G.; Fu, W.; Wang, W.; Wu, Z. The Lateral Tracking Control for the Intelligent Vehicle Based on Adaptive PID Neural Network. Sensors
**2017**, 17, 1244. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Hu, B.; Li, J.; Yang, J.; Bai, H.; Li, S.; Sun, Y.; Yang, X. Reinforcement Learning Approach to Design Practical Adaptive Control for a Small-Scale Intelligent Vehicle. Symmetry
**2019**, 11, 1139. [Google Scholar] [CrossRef] [Green Version] - Hima, S.; Glaser, S.; Vanholme, B. Controller design for trajectory tracking of autonomous passenger vehicles. In Proceedings of the 14th International IEEE Conference on Intelligent Transportations Systems (ITSC), Washington, DC, USA, 5–7 October 2011; pp. 1459–1464. [Google Scholar] [CrossRef]
- Park, M.; Lee, S.; Han, W. Development of Steering Control System for Autonomous Vehicle using geometry-based path tracking algorithm. ETRI J.
**2015**, 37, 617–625. [Google Scholar] [CrossRef] - Liu, Y.; Cui, D. Application of Optimal Control Method to Path Tracking Problem of Vehicle. SAE Int. J. Veh. Dyn. Stab. NVH
**2019**, 3, 209–219. [Google Scholar] [CrossRef] - Makantasis, K.; Papageorgiou, M. Motorway Path Planning for Automated Road Vehicles Based on Optimal Control Methods. Transp. Res. Record
**2018**, 112–123. [Google Scholar] [CrossRef] - ROS Navigation Stack. Available online: http://wiki.ros.org/navigation (accessed on 24 April 2020).
- ROS Follow_Waypoints Package. Available online: http://wiki.ros.org/follow_waypoints (accessed on 24 April 2020).
- ROS Yocs_Waypoints_Navi Package. Available online: http://wiki.ros.org/yocs_waypoints_navi (accessed on 24 April 2020).
- Waypoint_Tracking_Controller for ROS. Available online: https://github.com/RobeSafe-UAH/Waypoint_Tracking_Controller (accessed on 10 May 2020).
- CARLA Simulator. Available online: https://carla.org/ (accessed on 10 May 2020).
- López, J.; Otero, C.; Sanz, R.; Paz, E.; Molinos, E.; Barea, R. A new approach to local navigation for autonomous driving vehicles based on the curvature velocity method. In Proceedings of the 2019 International Conference on Robotics and Automation (ICRA), Montreal, QC, Canada, 20–24 May 2019; pp. 1751–1757. [Google Scholar] [CrossRef]
- O’Connor, M.; Bell, T.; Elkaim, G.; Parkinson, B. Automatic Steering of Farm Vehicles Using GPS. In Proceedings of the 3rd International Conference on Precision Agriculture, Minneapolis, MN, USA, 23–26 June 1996. [Google Scholar]
- Knott, G.D. Interpolating Cubic Splines; Progress in Computer Science and Applied Logic; Springer: Berlin, Germany, 2000. [Google Scholar]
- Floater, M.S. On the deviation of a parametric cubic spline interpolant from its data polygon. Comput. Aided Geom. Design
**2008**, 25, 148–156. [Google Scholar] [CrossRef] - Lepetic, M.; Klancar, G.; Skrjanc, I.; Matko, D.; Potocnik, B. Time optimal path planning considering acceleration limits. Robot. Auton. Syst.
**2003**, 45, 199–210. [Google Scholar] [CrossRef] - Ogata, K. Discrete-Time Control Systems; Prentice-Hall, Inc.: Upper Saddle River, NJ, USA, 1987. [Google Scholar]
- Quigley, M.; Conley, K.; Gerkey, B.; Faust, J.; Foote, T.; Leibs, J.; Wheeler, R.; Ng, A. ROS: An open-source Robot Operating System. In Proceedings of the ICRA Workshop on Open Source Software, Kobe, Japan, 12–17 May 2009. [Google Scholar]
- Robot Operating System (ROS). The Complete Reference; Koubaa, A., Ed.; Springer: Cham, Switzerland, 2017. [Google Scholar]
- Cousins, S. Exponential growth of ROS. IEEE Robot. Autom. Mag.
**2011**, 18, 19–20. [Google Scholar] [CrossRef] - Guimaraes, R.L.; Schneider, A.; Fabro, J.; Becjer, T.; Brenner, V.A. ROS Navigation: Concepts and tutorial. Stud. Comput. Intell.
**2016**. [Google Scholar] [CrossRef] - Move-Base Package of ROS. Available online: http://wiki.ros.org/move_base (accessed on 30 April 2020).
- Rviz Package of ROS. Available online: http://wiki.ros.org/rviz (accessed on 9 May 2020).
- Dosovitskiy, A.; Ros, G.; Codevilla, F.; López, A.; Koltun, V. CARLA: An Open Urban Driving Simulator. In Proceedings of the 1st Annual Conference on Robot Learning, Mountain View, CA, USA, 13–15 November 2017. [Google Scholar]
- Sánchez Juanola, M. Speed Traffic Sign Detection on the CARLA Simulator Using YOLO. Master’s Thesis, University Pompeu Fabra, Barcelona, Spain, 2019. [Google Scholar]
- Jaâfra, Y.; Laurent, J.L.; Deruyver, A.; Naceur, M.S. Seeking for Robustness in Reinforcement Learning: Application on Carla Simulator. In Proceedings of the 2019 International Conference on Machine Learning (ICML), Long Beach, CA, USA, 9–15 June 2019. [Google Scholar]
- Dworak, D.; Ciepiela, F.; Derbisz, J.; Izzat, I.; Komorkiewicz, M.; Wojcik, M. Performance of LiDAR object detectiion deep learning architectures based on artificially generated point cloud data from CARLA simulator. In Proceedings of the 2019 24 International Conference on Methods and Models in Automation and Robotics (MMAR), Międzyzdroje, Poland, 26–29 August 2019. [Google Scholar]
- ROS Bridge from CARLA Simulator. Available online: https://carla.readthedocs.io/en/latest/ros_installation/ (accessed on 13 May 2020).
- Gómez-Huélamo, C.; Bergasa, L.M.; Barea, R.; López-Guillén, E.; Arango, F.; Sánchez, P. Simulating use cases for the UAH Autonomous Electric Car. In Proceedings of the 2019 IEEE Intelligent Transportation Systems Conference (ITSC), Auckland, New Zealand, 27–30 October 2019; pp. 2305–2311. [Google Scholar] [CrossRef]

**Figure 1.**General navigation architecture of an autonomous vehicle. This work proposes a solution for the Waypoint Tracking Controller that receives as inputs a set of path waypoints and speed specifications along with the estimated vehicle state and generates as outputs the vehicle control signals.

**Figure 2.**Global architecture of the proposed Waypoint Tracking Controller. Inputs are: a set of path waypoints, an external speed specification and the estimated vehicle pose. Outputs are: linear speed V and steering angle ρ. The main subsystems are a Spline Interpolator, a Linear Velocity Profiler, a Linear Quadratic Regulator (LQR) Lateral Controller and a Delay Compensation System.

**Figure 3.**Kinematic model of an automobile with Ackerman steering. The control point is the central point of the front wheel’s axle. The kinematic model obtains the evolution of the location of the control point (x, y, θ) as a function of the speed V(t) and the angle ρ(t) of a virtual central front wheel at the control point.

**Figure 4.**Cubic spline definition: (

**a**) Cubic spline in cartesian space x-y, with five waypoints (orientation is imposed at start and goal points) and four segments, (

**b**) parametric cubic splines for each coordinate of Cartesian space, X(U) and Y(U), (

**c**) polynomials of each segment of the spline with normalized parameter u.

**Figure 5.**Variables involved in lateral control: reference pose χ

_{d}= (x

_{d}, y

_{d}, θ

_{d}) and tracking errors ξ = [d

_{e}θ

_{e}]′.

**Figure 6.**Influence of LQR controller parameters in the trajectory, the tracking errors and the control signal: (

**a**) Resulting trajectories using (1) the same value for all the parameters (blue line), (2) a higher value for q

_{11}(red line), (3) a higher value for q

_{22}(green line) and (4) a higher value for r (magenta line); (

**b**) tracking errors, control signal (steering angle) and linear velocity command provided by the linear velocity profiler.

**Figure 8.**Effect of delays and their compensation: (

**a**) resulting trajectories and (

**b**) tracking errors and control signals. Delays of n

_{p}= n

_{c}= 5 have been simulated in the sensors and actuators of the vehicle, and results are shown in the following cases: without compensation of delays (red dot-dashed line), with a partial compensation of the delays using n

_{p}= n

_{c}= 3 in the compensator (magenta dashed line) and with a complete compensation of the delays (continuous blue line).

**Figure 9.**“Controller_node” operation: state machine is shown in (

**a**), and the work flows of “Stop_State”, “Trajectory_State” and “Control_State” are shown in (

**b**), (

**c**) and (

**d**), respectively.

**Figure 10.**Test bench in the CARLA Simulator: (

**a**) 610-m route in Town_03 used for the tests (AB segment is used to compare results in curves), (

**b**) view of the route in Rviz Robot Operating System (ROS) visualizer, with the input waypoints shown as red points, (

**c**) a closer view of vehicle model making the first turn in CARLA, (

**d**) Rviz view of the same turn, with trajectory and reference pose markers for visualization.

**Figure 11.**Ablation tests: using only the basic LQR lateral controller with 6 m/s constant speed (red dot-dashed line), using LQR lateral control + delay compensation (blue dotted line), using LQR lateral control + velocity profiler (with V

_{MAX}= 13.5 m/s) (magenta dashed line), using the complete controller with delay compensation and velocity profiler (blue line).

**Figure 12.**Comparison with other tracking controllers: Pure Pursuit (magenta dashed line), Beam Curvature Method that deals with dynamic obstacles (BCM-DO) (red dot-dashed line), proposed Waypoint Tracking Controller (blue continuous line).

**Table 1.**List of subscribed topics, published topics and parameters for the “controller_node” node of the “Waypoint_Tracking_Controller” package.

Subscribed Topics | |||

Topic Name | Message Type | Description | |

waypoints_input | nav_msgs/Path | Sequence of input waypoints. | |

absolute_pose | nav_msgs/Odometry | Absolute position of the vehicle. | |

external_speed | std_msgs/Float64 | External speed specification. | |

Published Topics | |||

Topic Name | Message Type | Description | |

Spline | nav_msgs/Path | Interpolated path for display purposes. | |

points_spline | visualization_msgs/Marker | Decimated waypoints for spline calculation, with minimum distance between points given by min_dist parameter. Useful for visualization purposes. | |

reference_pose | geometry_msgs/PoseStamped | Reference pose for control χ_{d} (see Figure 2). | |

predicted_pose | geometry_msgs/PoseStamped | Pose predicted by the delay compensator to calculate the reference pose. | |

steer_cmd | std_msgs/Float64 | Front wheel steer angle command. | |

speed_cmd | std_msgs/Float64 | Front wheel speed command. | |

cmd_vel | geometry_msgs/Twist | Linear and angular velocity command. | |

Parameters | |||

Parameter Name | Type | Subsystem | Description |

n_max | Int | Spline Interpolator | Maximum number of input waypoints. |

min_dist | double | Minimum distance between waypoints. | |

rc_max | double | Linear Velocity Profiler | Maximum value of the radius of curvature that reduces the linear velocity below v_max. |

v_max | double | Maximum allowed linear velocity. | |

lambda_vector | List | Vector λ of normalized weights of the velocity profiler. | |

speed_mode | Int | Mode of combination of external speed specification and internal velocity profiler speed specification. | |

np | Int | Delay Compensator | Number of sampling periods estimated for the delay of the localization system. |

nc | Int | Number of sampling periods estimated for the delay of the actuation system. | |

q11 | double | LQR Lateral Controller | q11 parameter of the LQR lateral controller. |

q22 | double | q22 parameter of the LQR lateral controller. | |

r | double | r parameter of the LQR lateral controller. | |

Ts | double | All | Sampling period of the control system. |

Basic LQR Lateral Control | Lateral Control with Delay Compensation | Lateral Control with Speed Profiler | Lateral Control with Delay Compensation and Speed Profiler | |
---|---|---|---|---|

Total lateral error (m) ^{1} | 0.1954 | 0.1680 | 0.2041 | 0.1733 |

AB Curve Lateral error (m) ^{1} | 0.5019 | 0.4395 | 0.3524 | 0.2924 |

Total Orientation error (rad) ^{1} | 0.1087 | 0.1074 | 0.1509 | 0.1126 |

AB Curve Orientation error (rad) ^{1} | 0.2285 | 0.2068 | 0.2268 | 0.2035 |

Average speed (m/s) | 6 | 6 | 8.65 | 8.70 |

Maximum speed (m/s) | 6 | 6 | 13.5 | 13.5 |

Time to complete the route (s) | 103 | 102 | 71 | 70 |

^{1}RMS Value.

Pure Pursuit | BCM-DO | Proposed Waypoint Tracking Controller | |
---|---|---|---|

Total lateral error (m) ^{1} | 0.2755 | 0.2842 | 0.1733 |

AB Curve Lateral error (m) ^{1} | 0.3179 | 0.3125 | 0.2924 |

Total Orientation error (rad) ^{1} | 0.1174 | 0.1055 | 0.1126 |

AB Curve Orientation error (rad) ^{1} | 0.1520 | 0.1471 | 0.2035 |

Average speed (m/s) | 6.1 | 6.2 | 8.7 |

Maximum speed (m/s) | 13.5 | 13.5 | 13.5 |

Time to complete the route (s) | 100 | 98 | 70 |

^{1}RMS Value.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Gutiérrez, R.; López-Guillén, E.; Bergasa, L.M.; Barea, R.; Pérez, Ó.; Gómez-Huélamo, C.; Arango, F.; del Egido, J.; López-Fernández, J.
A Waypoint Tracking Controller for Autonomous Road Vehicles Using ROS Framework. *Sensors* **2020**, *20*, 4062.
https://doi.org/10.3390/s20144062

**AMA Style**

Gutiérrez R, López-Guillén E, Bergasa LM, Barea R, Pérez Ó, Gómez-Huélamo C, Arango F, del Egido J, López-Fernández J.
A Waypoint Tracking Controller for Autonomous Road Vehicles Using ROS Framework. *Sensors*. 2020; 20(14):4062.
https://doi.org/10.3390/s20144062

**Chicago/Turabian Style**

Gutiérrez, Rodrigo, Elena López-Guillén, Luis M. Bergasa, Rafael Barea, Óscar Pérez, Carlos Gómez-Huélamo, Felipe Arango, Javier del Egido, and Joaquín López-Fernández.
2020. "A Waypoint Tracking Controller for Autonomous Road Vehicles Using ROS Framework" *Sensors* 20, no. 14: 4062.
https://doi.org/10.3390/s20144062