园区自动驾驶实车平台决策规划控制系统（一）——基于纯追踪算法的横向控制（C++实现）

总结一下自己这一年的项目经验，由于本人技术水平有限，写作内容难免有错，如果发现还请不吝赐教~

在项目中本人负责的是自动驾驶的决策规划控制算法部分，在研发过程中负责对接上层感知定位模块和下层线控系统，项目实现了在园区场景下的自动驾驶。实现的场景包括单障碍物换道避障，多障碍物换道避障，跟车行驶，停车等待行人等。

接下来准备把项目中用到的横向控制、纵向控制、换道轨迹规划和决策模型分别记录，并附上核心代码，本文是第一篇。先放几个测试场景的视频作为预告。

园区自动驾驶（一）——基于Pure Pursuit的横向控制

由于本文是针对园区的自动驾驶，车速较低，且循迹路径是一条固定的路线，测试路径如下图所示，图中：绿色路径是录制的离线路径，录制方法是手动开车时记录下组合惯导系统的实时定位，并保存；紫色的路径是实际行驶路径；蓝色长条不是车辆模型，长度也不是车身长度，蓝条后端是车辆当前位置（后轴），蓝条前端是纯追踪算法当前的目标点。

在讲纯追踪算法之前，先陈述下使用纯追踪算法对车辆运动学和动力学模型的简化。

车辆运动学和动力学模型简化

无人驾驶-控制-自行车模型_0->oo的博客-CSDN博客

二自由度车辆运动学模型和动力学模型这位大佬讲的很清楚，总结一下结论，简化包括：

• 不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动，如下图所示；

• 左右侧车轮转角一致，这样可将左右侧轮胎合并为一个轮胎，以便于搭建单车模型，如图2所示；

• 车辆行驶速度变化缓慢，忽略前后轴载荷的转移；

• 车身及悬架系统是刚性的；

• 

简化结论有 ：

1、一般条件下：，其中 航向角为yaw，横摆角为；

但低速条件下，认为车不会发生侧向滑动（漂移），因此质心侧偏角 β ≈ 0，可得： 航向角yaw=横摆角 。

2、一般条件下，轮胎由于是胶体，会在压力下发生形变，导致轮胎侧偏；

但低速条件下，将轮胎视为完全刚体，不会发生变形，不考虑轮胎侧偏，且后轮一般不转向，因此后轮转角  。

bicycle模型

在以上简化假设都成立的情况下，才能将四轮akerman转向模型简化为二轮bicycle模型。

如上图所示，bicycle模型中，L是车的轴距，R是当前转向角下，车辆遵循的转弯半径，可以得到前轮转角δ 为：