Autoware 开源框架车辆运动学建模推导

学习Autoware开源框架的资料整理，侵删！

开源自动驾驶框架Autoware介绍
Autoware包含以下模块：
1.定位：通过结合GNSS和IMU传感器的3D地图和SLAM算法来实现定位
2.检测：使用具有传感器融合算法和深度神经网络的摄像机以及LiDAR
3.预测和规划：基于概率机器人技术和基于规则的系统，部分还使用深度神经网络
4.控制：Autoware向车辆输出的速度和角速度的扭曲量

以上四个模块覆盖了自动驾驶的技术流程：
感知层 – 预测及规划层 – 控制层

Autoware框架：浅析Apollo和Autoware的异同

主要包括感知（preception）和规划（planning）两部分：
(参考Autoware主要模块的解释及构架框图)
1.Localization

• lidar_localizar：计算车辆在全局坐标的当前位置（x,y,z,roll,pitch,yaw），使用Lidar的扫描数据和预先构建的地图信息。Autoware推荐使用NDT算法来匹配激光雷达当前帧和3D Map。

pitch是围绕X轴旋转，也叫做俯仰角；yaw是围绕Y轴旋转，也叫偏航角；roll是围绕Z轴旋转，也叫翻滚角。

• gnss_localizer：转换GNSS接收器发来的NEMA消息到位置信息（x,y,z,roll,pitch,yaw），结果可以被单独使用为车辆的当前位置，也可以作为lidar_localizar的初始参考位置。
• dead_reckoner：主要使用IMU传感器预测车辆的下一帧位置，也可以用来对lidar_localizar和gnss_localizar的结果进行插值。

2.Detection

• lidar_detector：从激光雷达单帧扫描读取点云信息，提供基于激光雷达的目标检测。主要使用欧几里得聚类算法，从地面以上的点云得到聚类结果，除此之外还可以使用卷积神经网络的算法进行分类（VoxelNet，LMNet）。
• image_detector：读取来自摄像头的图片，提供基于图像的目标检测。主要的算法包括R-CNN，SSD和Yolo，可以进行多类别(汽车，行人等)实时目标检测。
• image_tracker：使用image_detector的检测结果完成目标跟踪功能。算法基于Beyond Pixels，图像上的目标跟踪结果被投影到3D空间，结合lidar_detector的检测结果输出最终的目标跟踪结果。
• fusion_detector：输入激光雷达的单帧扫描点云和摄像头的图片信息，进行在3D空间的更准确的目标检测。激光雷达的位置和摄像头的位置需要提前进行联合标定，现在主要是基MV3D算法来实现。
• fusion_tools： 将lidar_detector和image_detector的检测结果进行融合，image_detector 的识别类别被添加到lidar_detector的聚类结果上。
• object_tracter： 预测检测目标的下一步位置，跟踪的结果可以被进一步用于目标行为分析和目标速度分析。跟踪算法主要是基于卡尔曼滤波器。

3.Prediction

• moving_predictor： 使用目标跟踪的结果来预测临近物体的未来行动轨迹，例如汽车或者行人。
• collision_predictor： 使用moving_predictor的结果来进一步预测未来是否会与跟踪目标发生碰撞。输入的信息包括车辆的跟踪轨迹，车辆的速度信息和目标跟踪信息。

---

以下内容学习《无人驾驶车辆模型预测控制》（针对低速）

路径跟踪与轨迹规划：
路径点 + 时间约束 = 轨迹点
路径规划 + 车辆的横、纵向动力学约束 = 轨迹规划

对于无人汽车来说，全局路径点只要包含空间信息，或者包含姿态信息，而不需要和时间相关，但是对于局部规划需要考虑时间信息。

对于触须生成时，采用圆弧曲线，忽略了车辆转弯半径的变换，但是在高速环境下容易跟踪失败！

假设车辆在运动过程中没有滑移，那么就是非完整性动力学约束，在低速行驶过程中可以考虑忽略横向动力学。（纯跟踪算法Pure Pursuit）

在参考书中，通过2自由度和14自由度模型的跟踪结果对比表明：
复杂的预测模型并不是最好的选择，对车辆的动力学模型进行合理简化并且选择满足行驶工况的约束条件才是研究的对象。也就是说，在模型预测控制中，模型约束条件能够表达控制对象的真实物理限制即可。

车辆运动学建模推导

在后轴的行驶轴心上，$\acute{X_r}$和$\acute{Y_r}$分别代表在XY方向上的导数，即速度。将其分解到车辆的行驶方向上，即:
$$v_r=\acute{X_r}cos\phi +\acute{Y_r}sin\phi$$且在前后轴上，横向速度应当为0，即满足运动学约束：
$$\acute{X_f}cos(\phi +{\delta }_f)-\acute{Y_f}sin(\phi +{\delta }_f)=0$$$$\acute{X_r}sin\phi -\acute{Y_r}cos\phi =0$$
联立上式：
$$\acute{X_r}=\acute{Y_r}\frac{cos\phi }{sin\phi }$$$$v_r=\acute{Y_r}\frac{cos\phi }{sin\phi }cos\phi +\acute{Y_r}sin\phi =\frac{\acute{Y_r}cos{\phi }^2+\acute{Y_r}sin{\phi }^2}{sin\phi }=\frac{\acute{Y_r}}{sin\phi }$$
因此可得：$$v_{r}sin\phi =\acute{Y_r}$$同理：$$v_{r}cos\phi =\acute{X_r}$$
根据前后轮的几何关系：$$X_f=X_r+lcos\phi$$$$Y_f=Y_r+lsin\phi$$
根据轮速差估算横摆角速度：
$$Ya{w}_{rate}=\frac{(外侧轮速-内侧轮速)}{轮距}\times cos(车轮转角)$$
使用后轮计算横摆角，因为后轮没有偏移角，即$cos0=1$，但是一般使用驱动轮因此：
$$\omega =\frac{v_r}{l}tan{\delta }_f$$
同时可得到：
$$R=\frac{v_r}{\omega }$$$${\delta }_f=arctan(\frac{\omega l}{v_r})=arctan(\frac{l}{R})$$
综上可得到运动学模型：
$$\left[\begin{array}{c}\acute{X_r}\\ \acute{Y_r}\\ \acute{\phi }\end{array}\right]=\left[\begin{array}{c}cos\phi \\ sin\phi \\ \frac{tan{\delta }_f}{l}\end{array}\right]v_r=\left[\begin{array}{c}cos\phi \\ sin\phi \\ 0\end{array}\right]v_r+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\omega$$