# A Hierarchical Control System for Autonomous Driving towards Urban Challenges
## 2 Hierarchical Control System
### Vehicle Control Strategy
#### 2.3.2 Adaptive-Pure Pursuit Algorithm Based-Lateral Controller
\right]=\left[ \begin{array}{cc}w\\ W_{C}^{-1}&-^{W}R_{C}^{-1W}pc\\ 0&1\end{array} \right]\left[ \begin{array}{c}w\\ w\\ y_{w}\\ 1\end{array} \right]\.\] (A2)

Equation (A3) can be written as

\[\left\{ \begin{array}{c}\Delta x={}^{W}x_{w}-{}^{W}x_{C}\\ \Delta y={}^{W}y_{w}-{}^{W}y_{C}\\ c_{xw}=\Delta x*\cos({}^{W}\theta_{C})+\Delta y*sin({}^{W}\theta_{C})\\ c_{yw}=\Delta y*\cos({}^{W}\theta_{C})-\Delta x*sin({}^{W}\theta_{C})\end{array}  \right.\.\] (A4)

### Smoother Trajectory

The smoother trajectory algorithm performs the 3-order polynomial by solving a least-square problem with QR decomposition. The polynomial function can also estimate the curve level by comparing the error of heading angle and tangent at the closest point to the vehicle. The problem is to find parameter \(c\) with \(Ic=y\) as shown in Figure A1. It can convert to a least square problem \(c^{*}=\underset{c}{\arg\min}\left\|Ic-y\right\|^{2}\). This problem has several techniques to solve such as QR, SVD,Cholesky factorization with the computational cost \(2 mn^{2}-\frac{2n^{3}}{3}\), \(14 mn^{2}+8n^{3}\), and \(n^{2}(2m+\frac{n}{3})\) flops, respectively. In general, QR factorization is selected due to lowest complex, so \(I=Q^{mxm}R^{nxn}\). The use of QR is shown as

\[ \begin{array}{l}I^{T}I=(QR)^{T}QR=R^{T}R\\ R^{T}Rc=I^{T}y=R^{T}Q^{T}y\\ Rc=Q^{T}y.\end{array}  \tag{10}\]

We use C++ Eigen-library to serve QR factorization. ## References

* [1] [PERSON] Critical Reasons for Crashes Investigated in the National Motor Vehicle Crash Causation Survey. March 2018. Available online: [[https://crashstats.nhtsa.dot.gov/Api/Public/ViewPublication/812506](https://crashstats.nhtsa.dot.gov/Api/Public/ViewPublication/812506)]([https://crashstats.nhtsa.dot.gov/Api/Public/ViewPublication/812506](https://crashstats.nhtsa.dot.gov/Api/Public/ViewPublication/812506)) (accessed on 10 March 2020). * [2] SAE International. _Taxonomy and Definitions for Terms Related to Driving Automation Systems for On-Road Motor Vehicles_; SAE International: Warrendale, PA, USA, 2019. * [3] [PERSON] Waymo is first to put fully self-driving cars on US roads without a safety driver. _The Verge_, 27 August 2019. * [4] Available online: [[https://en.wikipedia.org/wiki/DARPA_Grand_Challenge](https://en.wikipedia.org/wiki/DARPA_Grand_Challenge)]([https://en.wikipedia.org/wiki/DARPA_Grand_Challenge](https://en.wikipedia.org/wiki/DARPA_Grand_Challenge)) (accessed on 10 March 2020). * [5] [PERSON]; [PERSON]; [PERSON] _The 2005 DARPA Grand Challenge: The Great Robot race_; Springer: New York, NY, USA, **2007**; Volume 36. [CrossRef]
* [6] ELROB, The European Land Robot Trial (ELROB). Available online: [[http://www.elrob.org/](http://www.elrob.org/)]([http://www.elrob.org/](http://www.elrob.org/)) (accessed on 10 March 2020). * [7] [PERSON]; [PERSON]; [PERSON]; [PERSON] China Future Challenge: Beyond the Intelligent Vehicle. _IEEE Intell. Transp. Syst. Soc. Newslett._**2014**, 16, 8-10. * [8] Available online: [[http://www.hyundai-ngv.com/en/tech_coop/sub05.do](http://www.hyundai-ngv.com/en/tech_coop/sub05.do)]([http://www.hyundai-ngv.com/en/tech_coop/sub05.do](http://www.hyundai-ngv.com/en/tech_coop/sub05.do)) (accessed on 10 March 2020). * [9] Available online: [[https://selfdrivingcarx.com/google-waymo/](https://selfdrivingcarx.com/google-waymo/)]([https://selfdrivingcarx.com/google-waymo/](https://selfdrivingcarx.com/google-waymo/)) (accessed on 10 March 2020). * [10] Tesla Motors: Model S Press Kit. Available online: [[https://www.tesla.com/presskit/autopilot](https://www.tesla.com/presskit/autopilot)]([https://www.tesla.com/presskit/autopilot](https://www.tesla.com/presskit/autopilot)) (accessed on 10 March 2020). * [11] [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON] An open approach to autonomous vehicles. _IEEE Micro_**2015**, 35, 60-68. [CrossRef]* [PERSON] et al. (2018) [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON]; [PERSON] Baidu Apollo em motion planner. _arXiv_**2018**, arXiv:1807.08048. * NVIDIA (2020) NVIDIA.