编队车辆的轨迹跟踪方法、系统、车辆及设备与流程

本申请涉及车辆，尤其涉及一种编队车辆的轨迹跟踪方法、系统、车辆及设备。

背景技术：

1、现有车辆编队行驶技术，高度依赖rtk卫星定位、公网通信及v2v车车通信，在隧道、高楼遮挡等无网、弱定位路段，卫星定位失效、车车通信断连，仅能依靠单车imu+轮速里程计进行短时航位推算，实现短时虚拟领航。长时间行驶时，imu惯导会出现明显的航向零偏偏移、横向位置发散，导致跟随车轨迹偏离前车、编队队形畸变。卫星定位失效、车车通信断连，同时无法实现队列姿态协同，难以抑制惯导累积误差，导致虚拟领航无法长时间稳定运行。

技术实现思路

1、基于此，有必要针对上述技术问题，提供一种编队车辆的轨迹跟踪方法、系统、车辆及设备，跟随车依托自身激光雷达感知车车相对姿态，在无网络、弱卫星定位、无v2v通信场景下的编队轨迹跟踪中，保障虚拟领航长时稳定运行，脱离编队对rtk卫星定位、公网、v2v车车通信的依赖，适配无网、弱定位、长距离信号差等极端场景的编队车辆行驶。

2、第一方面，提供一种编队车辆的轨迹跟踪方法，包括：

3、当进入长时虚拟领航模式的情况下，根据自车捕捉的前车车尾点云数据，获得前车的等效航向；

4、根据自车的惯导数据和预先确定出的航向基准，得到自车的航向角；

5、根据所述前车的等效航向和自车的航向角，得到自车和前车的航向偏差；

6、根据所述航向偏差的变化判断是否满足校正条件；

7、如果满足校正条件，则根据所述航向偏差校正所述自车的航向。

8、在一些示例中，所述根据自车捕捉的前车车位点云数据，获得前车的等效航向，包括：

9、利用前激光雷达捕捉前车车尾点云数据；

10、根据所述前车车尾点云数据，拟合所述前车的车尾中轴线及车身朝向；

11、根据所述前车的车尾中轴线及车身朝向，得到所述前车的等效航向。

12、在一些示例中，在根据自车的惯导数据和预先确定出的航向基准，得到自车的航向角之前，还包括：

13、当卫星定位正常、车车通信正常的情况下，根据选择的大地坐标系，确定出所述航向基准。

14、在一些示例中，所述根据所述前车的等效航向和自车的航向角，得到自车和前车的航向偏差，包括：

15、以车头向左横摆为航向角的正方向、向右横摆为航向角的负方向，获得所述前车的等效航向和自车的航向角之间的差值，并将所述差值作为所述自车和前车的航向偏差。

16、在一些示例中，所述根据所述航向偏差的变化判断是否满足校正条件，包括：

17、如果所述航向偏差呈连续、顺滑、缓慢变化，且与领航车的道路曲率匹配，判定为不满足所述校正条件，否则，判定为满足所述校正条件。

18、在一些示例中，所述如果满足校正条件，则根据所述航向偏差校正所述自车的航向，包括：

19、如果所述航向偏差大于预设值，则控制所述自车车头向左拉回；

20、如果所述航向偏差小于所述预设值，则控制所述自车车头向右拉回。

21、第二方面，提供了一种编队车辆的轨迹跟踪系统，包括：

22、等效航向确定模块，用于当进入长时虚拟领航模式的情况下，根据自车捕捉的前车车尾点云数据，获得前车的等效航向；

23、航向角计算模块，用于根据自车的惯导数据和预先确定出的航向基准，得到自车的航向角；

24、偏差计算模块，用于根据所述前车的等效航向和自车的航向角，得到自车和前车的航向偏差；

25、控制模块，用于根据所述航向偏差的变化判断是否满足校正条件，并在满足校正条件时，根据所述航向偏差校正所述自车的航向。

26、第三方面，提供了一种车辆，包括：根据上述的第二方面所述的编队车辆的轨迹跟踪系统。

27、第四方面，提供了一种计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时，实现上述第一方面以及第一方面任意一种可能的实现方式的编队车辆的轨迹跟踪方法的步骤。

28、第五方面，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时，实现上述第一方面以及第一方面任意一种可能的实现方式的编队车辆的轨迹跟踪方法的步骤。

29、第六方面，提供一种计算机程序产品，其上存储有计算机程序，该程序被处理器执行时，实现上述第一方面以及第一方面任意一种可能的实现方式的编队车辆的轨迹跟踪方法的步骤。

30、采用本申请的实施例，在定位、车车通信等异常的情况下，可以根据自车捕捉的前车车尾点云数据，获得前车的等效航向，并根据自车的惯导数据和预先确定出的航向基准，得到自车的航向角，然后，根据前车的等效航向和自车的航向角，得到自车和前车的航向偏差，最后，根据所述航向偏差的变化判断满足校正条件的情况下，可以根据航向偏差校正自车的航向，跟随车依托自身激光雷达感知车车相对姿态，在无网络、弱卫星定位、无v2v通信场景下的编队轨迹跟踪中，保障虚拟领航长时稳定运行，脱离编队对rtk卫星定位、公网、v2v车车通信的依赖，适配无网、弱定位、长距离信号差等极端场景的编队车辆行驶。

1.一种编队车辆的轨迹跟踪方法，其特征在于，包括：

2.根据权利要求1所述的编队车辆的轨迹跟踪方法，其特征在于，所述根据自车捕捉的前车车位点云数据，获得前车的等效航向，包括：

3.根据权利要求1所述的编队车辆的轨迹跟踪方法，其特征在于，在根据自车的惯导数据和预先确定出的航向基准，得到自车的航向角之前，还包括：

4.根据权利要求1所述的编队车辆的轨迹跟踪方法，其特征在于，所述根据所述前车的等效航向和自车的航向角，得到自车和前车的航向偏差，包括：

5.根据权利要求1所述的编队车辆的轨迹跟踪方法，其特征在于，根据所述航向偏差的变化判断是否满足校正条件，包括：

6.根据权利要求1-5任一项所述的编队车辆的轨迹跟踪方法，其特征在于，所述如果满足校正条件，则根据所述航向偏差校正所述自车的航向，包括：

7.一种编队车辆的轨迹跟踪系统，其特征在于，包括：

8.一种车辆，其特征在于，包括：根据权利要求7所述的编队车辆的轨迹跟踪系统。

9.一种计算机设备，包括存储器、处理器以及存储在存储器上的计算机程序，其特征在于，所述处理器执行所述计算机程序时，实现根据权利要求1-6任一项所述的编队车辆的轨迹跟踪方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现根据权利要求1-6任一项所述的编队车辆的轨迹跟踪方法。