一种行车轨迹平滑方法、系统、电子设备及存储介质与流程

本发明涉及自动驾驶，更具体地，涉及一种行车轨迹平滑方法、系统、电子设备及存储介质。

背景技术：

1、自动驾驶汽车又称无人驾驶汽车、电脑驾驶汽车、或轮式移动机器人，是一种通过电脑系统实现无人驾驶的智能汽车。在20世纪已有数十年的历史，21世纪初呈现出接近实用化的趋势。自动驾驶汽车依靠人工智能、视觉计算、雷达、监控装置和全球定位系统协同合作，让电脑可以在没有任何人类主动的操作下，自动安全地操作机动车辆。

2、在自动驾驶汽车的应用领域中，记忆泊车功能亦是其核心功能，记忆泊车功能需要输入平滑的行车轨迹从而使得车辆平稳的驶入目标位置，通常情况下，可以通过slam建图的定位信息可获取低速行车参考轨迹。在循迹的时候直接将参考轨迹下发给控制模块即可。然而，由于系统不稳定性，slam建图的定位信息往往是不平滑而且是不等间距的。因此，如何对不平滑的行车轨迹进行平滑是亟待解决的问题。

技术实现思路

1、本发明针对现有技术中存在的技术问题，提供一种行车轨迹平滑方法、系统、电子设备及存储介质，用以解决如何对不平滑的行车轨迹进行平滑的问题。

2、本发明的第一方面，提供了一种行车轨迹平滑方法，包括：

3、基于预设采样间距对获取到的行车轨迹点序列进行采样，得到第一行车轨迹点序列；

4、基于线性插值法对所述第一行车轨迹点序列进行插值，得到第二行车轨迹点序列；

5、基于滑动窗口均值滤波算法对所述第二行车轨迹点序列进行均值滤波处理，得到第三行车轨迹点序列；

6、计算所述第三行车轨迹点序列的轨迹曲率集合，基于所述第三行车轨迹点序列和所述轨迹曲率集合输出平滑后的行车轨迹。

7、在上述技术方案的基础上，本发明还可以作出如下改进。

8、优选的，所述基于预设采样间距对获取到的初始车轨迹点序列进行采样，得到第一行车轨迹点序列的步骤之前，包括：

9、对获取到的初始轨迹点序列进行去重，得到去重后的行车轨迹点序列。

10、优选的，所述基于线性插值法对所述第一行车轨迹点序列进行插值，得到第二行车轨迹点序列的步骤，包括：

11、获取所述第一行车轨迹点序列中相邻两个点的坐标，所述坐标包括x轴坐标、y轴坐标和yaw航向角；

12、根据线性关系计算所述相邻两个点的中间点坐标，将所述中间点坐标插入至所述第一行车轨迹点序列，得到第二行车轨迹点序列。

13、优选的，所述基于滑动窗口均值滤波算法对所述第二行车轨迹点序列进行均值滤波处理，得到第三行车轨迹点序列的步骤，包括：

14、基于滑动窗口在所述第二行车轨迹点序列上滑动，依次计算每一个滑动窗口内的多个点的x坐标均值、y坐标均值和yaw航向角均值，完成对所述第二行车轨迹点序列的均值滤波，得到第三行车轨迹点序列。

15、优选的，所述基于滑动窗口在所述第二行车轨迹点序列上滑动，依次计算每一个滑动窗口内的多个点的x坐标均值、y坐标均值和yaw航向角均值，完成对所述第二行车轨迹点序列的均值滤波，得到第三行车轨迹点序列的步骤，包括：

16、获取所述第二行车轨迹点序列中每一个轨迹点的x坐标，以当前轨迹点作为滑动窗口的中心，对滑动窗口中的多个轨迹点的x坐标取均值作为当前轨迹点均值滤波后的x坐标值，遍历所述第二行车轨迹点序列中每一个轨迹点，得到所述第二行车轨迹点序列中每一个轨迹点均值滤波后的x坐标值；

17、获取所述第二行车轨迹点序列中每一个轨迹点的y坐标，以当前轨迹点作为滑动窗口的中心，对滑动窗口中的多个轨迹点的y坐标取均值作为当前轨迹点均值滤波后的y坐标值，遍历所述第二行车轨迹点序列中每一个轨迹点，得到所述第二行车轨迹点序列中每一个轨迹点均值滤波后的y坐标值；

18、获取所述第二行车轨迹点序列中每一个轨迹点的yaw航向角，构成yaw航向角序列；

19、依次对所述yaw航向角序列中的每相邻两个轨迹点的yaw航向角进行差值计算，得到航向角差值序列；

20、将所述yaw航向角序列与所述航向角差值序列相加，得到待滤波航向角序列；

21、获取所述待滤波航向角序列中每一个轨迹点的待滤波航向角，以当前轨迹点作为滑动窗口的中心，对滑动窗口中的多个轨迹点的待滤波航向角取均值作为当前轨迹点均值滤波后的待滤波航向角，遍历所述第二行车轨迹点序列中每一个轨迹点，得到所述第二行车轨迹点序列中每一个轨迹点均值滤波后的待滤波航向角；

22、对所述第二行车轨迹点序列中每一个轨迹点均值滤波后的待滤波航向角基于预设标准区间进行标准化，得到每一个轨迹点标准化后的航向角。

23、优选的，所述计算所述第三行车轨迹点序列的轨迹曲率集合的步骤，包括：

24、依次获取所述第三行车轨迹点序列中每相邻三个点的坐标，基于每相邻三个点的坐标构建圆心为0的圆，基于每相邻三个点和所述圆心0计算每相邻三个点的中心点的曲率；

25、所述依次对所述yaw航向角序列中的每相邻两个轨迹点的yaw航向角进行差值计算，得到航向角差值序列的步骤，包括：

26、在每个相邻的两个轨迹点中，当前一个轨迹点与后一个轨迹点的差值大于π时，将前一个轨迹点与后一个轨迹点的差值减去2π，得到前一个轨迹点的航向角差值；

27、当前一个轨迹点与后一个轨迹点的差值小于-π时，将前一个轨迹点与后一个轨迹点的差值增加2π，得到前一个轨迹点的航向角差值；

28、遍历所有相邻两个轨迹点，得到航向角差值序列。

29、遍历所有相邻三个点，得到对应的中心点的曲率，获取所述第三行车轨迹点序列的轨迹曲率集合。

30、本发明的第二方面，提供一种行车轨迹平滑系统，包括：

31、轨迹采样模块，用于基于预设采样间距对获取到的行车轨迹点序列进行采样，得到第一行车轨迹点序列；

32、轨迹插值模块，用于基于线性插值法对所述第一行车轨迹点序列进行插值，得到第二行车轨迹点序列；

33、轨迹均值模块，用于基于滑动窗口均值滤波算法对所述第二行车轨迹点序列进行均值滤波处理，得到第三行车轨迹点序列；

34、轨迹平滑模块，用于计算所述第三行车轨迹点序列的轨迹曲率集合，基于所述第三行车轨迹点序列和所述轨迹曲率集合输出平滑后的行车轨迹。

35、本发明的第三方面，提供了一种电子设备，包括存储器、处理器，所述处理器用于执行存储器中存储的计算机管理类程序时实现上述第一方面中任一行车轨迹平滑方法的步骤。

36、本发明的第四方面，提供了一种计算机可读存储介质，其上存储有计算机管理类程序，所述计算机管理类程序被处理器执行时实现上述第一方面中任一行车轨迹平滑方法的步骤。

37、本发明提供的一种行车轨迹平滑方法、系统、电子设备及存储介质，方法包括：基于预设采样间距对获取到的行车轨迹点序列进行采样，得到第一行车轨迹点序列；基于线性插值法对上述第一行车轨迹点序列进行插值，得到第二行车轨迹点序列；基于滑动窗口均值滤波算法对上述第二行车轨迹点序列进行均值处理，得到第三行车轨迹点序列；计算上述第三行车轨迹点序列的轨迹曲率集合，基于上述第三行车轨迹点序列和上述轨迹曲率集合输出平滑后的行车轨迹。本发明通过对初始行车轨迹进行重采样、线性插值、均值滤波以及曲率重算，从而实现了对行车轨迹的二维坐标、航向角、曲率进行平滑，得到等间距且易于控制的行车轨迹，大大地提升了车辆低速行驶的平稳性。