1.如何确定自身位置？

无人驾驶车辆安装GPS（全球定位系统）和IMU（惯导）可以获取自身的位姿、偏航角、经纬度以及加速度等参数。

GPFPD:标准标准标准标准 GI 定位定姿消息集

GTIMU：时间、IMU 数据显示消息集

2.如何获得需要循迹的路径？

可以通过串口助手或者GPS上位机软件来录制车辆行驶过的GPS数据，将 .txt 格式的数据转成 .kml 格式的数据，导入Google Earth里就可以看到录制的路径。然后筛选路径点，可根据车速每隔一段距离选取一个GPS坐标点。

录制的GPS路径信息（未经过筛选）

3.如何寻找起始点或者下一个点？

无人驾驶车辆自身的偏航角以及经纬度是已知的，录制的GPS路径点的经纬度也是已知的。

我认为的偏航角是以大地为坐标系，正北方向为0°，以顺时针方向增加，正东方向为90°，正南方向为180°，正西方向为270°，直到回到正北方向为360°。

但是，现在就会遇到一个问题。

因此，经过思考发现了一个办法来确定角度。

这张图如果不够清楚，来看下一张图。

坐标系原点为小车的经纬度坐标，根据小车的经纬度来划分象限，根据目标点在不同的象限来确定使用不同的航向角计算办法。

计算所得出来的航向角再与小车的航向角相减，得出需要进行转向的角度，以此来趋近于起始目标点。