无人车采用纯跟踪算法跟踪离线路径（ROS，C++）航向角处理问题

一：序言

采用纯跟踪跟踪离线路径过程中航向角的处理尤为重要，需要结合自身传感器所获取的坐标系进行处理，不然后面再采用纯跟踪算法进行路径跟踪的时候，就会因为计算车辆位置与跟踪点方位角和航向角坐标不同导致获取的控制转角出错，所以我这里对航向角的整体处理思路进行一个梳理，有感兴趣的同学可以去看我前面几篇写这个项目具体实现流程的文章
无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分
无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第二部分 实车验证
无人车采用纯跟踪算法跟踪离线路径（ROS，C++）坐标系转换选取问题

二：整体思路

上一篇说了，车辆以起始点为原点建立东北天坐标系，为了符合后续的控制功能实现，通过航向角将这个东北天坐标旋转，让车头所对的方向和这个东北天方法的东方向一致也就是组成了X为东，北为Y的二维坐标系，但是这个位置上由于传感器定义的不同，所以你获取的航向角是不一定以这个东为坐标轴获取。
就我自己的传感器而言：

这个传感器是以北轴以获取到的“北”为起始点，由北向顺时针到南向为0到正180变化，逆时针为0到-180度变换，这个如何在移植的过程中需要重点注意下，所以在进行坐标系旋转以及后续操作的时候，就需要将这个航向角的坐标轴转变为以东为轴的航向角

当完成航向角转换和坐标系转换后，航向角是以东轴进行计算，而车辆整体运动也是在这个东北天，以东为X，路径点同样是以东北天坐标轴，这样再进行纯跟踪算法进行角度计算的时候就不存在出现坐标不一致的问题

三：算法实现思路

纯跟踪算法实际上是通过当前到跟踪路径点的角度偏差来计算车辆需要进行的移动，所以在进行计算角度时非常重要：

自己手画了一个示意图，因为航向角做转换所以是以东进行处理 ，比如现在a1为-30度，而我又知道跟踪点和车辆自身位置信息，所以我可以得出a2，两者相减就可以完成车辆需要偏航的角度，后续就就可以通过纯跟踪算法完成控制，可能会出现角度和单位的问题，后续文章会进行更详细补充

总结

在这个过程中需要考虑坐标准换和旋转坐标系代码的处理，同时还需要对最终的角度范围进行设定，所以C++，ROS的使用很重要，我自己也边在优化代码边学习这两个课程，有需要一块学习的同学可以联系我，另外文章也是自己的一个总结，可能会存在一些错误，有问题欢迎评论区指出，或者私信我。