自动驾驶基于几何横向控制算法原理解析和实现（c++）_自动驾驶c++

自动驾驶横向控制算法

最近抽空学习了下自动驾驶中基于几何的横向控制算法， 分别是pure pursuit, stanley control, rear wheel feedback control，还实现了vehicle with trailer的横向控制，代码都是基于cmake工程，方便移植到自己的项目中，对代码有需求的同学可以私聊。

这里直接贴出自己的笔记图了，懒得再输入一次公式啦

先贴出阿克曼小车的运动学模型，要注意是以后轴为中心

阿克曼运动学模型

 

pure pursuit 

算法很容易实现，控制精度不是很高， 前进后退都适用，调参个人经验是根据曲率来决定速度和前视距离。

 stanley control

 

 斯坦利算法好像不适用于倒车，倒车的控制精度完全比不上前进的精度， 这里说一下我个人对上面公式的理解，第一个是角度差，代表的是当前heading和最近的一个target point的方向偏差，这个公式的目的是让小车的heading和曲线最近点切线方向一致，如果把第二个公式去掉的话，小车和曲线的偏差就没法变小，但是车行走路线是和轨迹一致的，为了把横向误差缩小，第二个公式就起到作用了， 这个公式直接对误差乘一个k当做方向盘的转角，横向误差越大， 转角就越大，这个时候角度误差又会制约这个方向盘的打角，避免跟踪的时候出现震荡， 这里在贴一下实现的一些细节

 rear wheel feedback control参考文献

stanley control是基于前轮的控制，不适用于倒车，后轮反馈控制是前进倒车都适用。