一种车辆航向角确定方法、系统、车辆以及存储介质与流程

本发明涉及自动驾驶，具体涉及一种车辆航向角确定方法、系统、车辆以及存储介质。

背景技术：

1、目前，具有自动驾驶系统(autonomous driving system，ads)的车辆已开始逐步推向市场，极大地促进了智能交通的发展。而航向角是保障自动驾驶车辆安全稳定运行的一个重要参数。

2、申请公布号为cn116295457a公开了一种基于二维语义地图的车辆视觉定位方法及系统，其通过获取当前的传感器位姿数据以及相机图像数据；对相机图像数据进行图像感知，得到视觉语义感知要素；获取当前车辆所在环境的二维语义地图，得到地图要素；根据视觉语义感知要素，获取二维语义地图中地图要素的高度，构建地图要素采样点的三维坐标；将地图要素采样点的三维坐标和视觉语义感知要素进行混合数据关联；基于混合数据关联结果进行非线性优化迭代，得到最优的当前位姿，当前位姿中包括航向角。但其存在如下技术问题：二维语义地图的精度较低，导致地图要素采样点与视觉语义感知要素之间的匹配精度不高，进而导致航向角确定不准确，且通过非线性优化迭代获得航向角，运算量较大，导致航向角的确定速度较慢。

3、因此，急需提出一种车辆航向角确定方法、系统、车辆以及存储介质，解决上述技术问题。

技术实现思路

1、有鉴于此，有必要提供一种车辆航向角确定方法、系统、车辆以及存储介质，用以解决现有技术中存在的确定出的航向角不准确，且确定效率较低的技术问题。

2、一方面，本发明提供了一种车辆航向角确定方法，包括：

3、基于设置在车辆上的视觉传感器确定第一车道边线的第一形点数据，并基于设置在车辆上的高精度定位系统和高精度地图系统确定第二车道边线的第二形点数据；所述第一形点数据包括各第一车道边线形点在目标坐标系下的第一x轴坐标值和第一y轴坐标值，所述第二形点数据包括各第二车道边线形点在所述目标坐标系下的第二x轴坐标值和第二y轴坐标值；

4、确定所述车辆到所述第一车道边线的第一投影点以及所述车辆到所述第二车道边线的第二投影点；

5、确定各所述第一车道边线形点与所述第一投影点之间的第一形点距离，以及各所述第二车道边线形点与所述第二投影点之间的第二形点距离；

6、以所述第一形点距离为自变量，所述第一x轴坐标值和所述第一y轴坐标值为因变量，分别构建第一五次拟合方程以及第二五次拟合方程，并以所述第二形点距离为自变量，所述第二x轴坐标值和所述第二y轴坐标值为因变量，分别构建第三五次拟合方程以及第四五次拟合方程；

7、基于所述第一五次拟合方程以及第二五次拟合方程确定所述车辆的第一夹角，基于所述第三五次拟合方程以及第四五次拟合方程确定所述车辆的第二夹角，并基于所述第二夹角和所述第二夹角确定车辆航向角。

8、在一些可能的实现方式中，所述基于设置在车辆上的视觉传感器确定第一车道边线的第一形点数据，包括：

9、基于设置在车辆上的视觉传感器获取所述第一车道边线的第一车道边线信息；

10、以所述车辆为目标坐标原点，构建所述目标坐标系，并确定所述第一车道边线信息在所述目标坐标系中的多个第一车道边线坐标点；

11、基于所述多个第一车道边线坐标点确定所述第一形点数据。

12、在一些可能的实现方式中，所述基于所述多个坐标点确定所述第一形点数据，包括：

13、以目标拟合方程对所述多个坐标点进行拟合，获得车道边线拟合方程；

14、基于预设的提取范围从所述车道边线拟合方程中提取出所述第一形点数据。

15、在一些可能的实现方式中，所述基于设置在车辆上的高精度定位系统和高精度地图系统确定第二车道边线的第二形点数据，包括：

16、基于设置在车辆上的高精度定位系统和高精度地图系统获取所述第二车道边线的第二车道边线信息；所述第二车道边线信息包括在预设坐标系下的多个初始第二车道边线坐标点；

17、基于所述高精度定位系统确定所述预设坐标系和所述目标坐标系的坐标变换矩阵，并基于所述坐标变换矩阵和所述多个第二初始车道边线坐标点确定多个第二车道边线坐标点；

18、对所述多个第二车道边线坐标点进行下采样处理，获得所述第二形点数据。

19、在一些可能的实现方式中，所述对所述多个第二车道边线坐标点进行下采样处理，获得所述第二形点数据，包括：

20、确定各所述第二车道边线坐标点与所述第二投影点之间的待筛选形点距离；

21、获取预设形点距离范围，并基于所述待筛选形点距离和所述预设形点距离范围确定所述第二形点数据。

22、在一些可能的实现方式中，所述预设形点距离范围包括所述车辆已经行走的第一预设距离范围和所述车辆行进方向的第二预设距离范围，且所述第一预设距离范围和所述第二预设距离范围相等。

23、在一些可能的实现方式中，所述第一五次拟合方程为：

24、x1'＝c0+c1*s1+c2*s12+c3*s13+c4*s14+c5*s15

25、所述第二五次拟合方程为：

26、y1'＝c'0+c'1*s1+c'2*s12+c'3*s13+c'4*s14+c'5*s15

27、所述第三五次拟合方程为：

28、x2'＝d0+d1*s2+d2*s22+d3*s23+d4*s24+d5*s25

29、所述第四五次拟合方程为：

30、y2'＝d'0+d'1*s2+d'2*s22+d'3*s23+d'4*s24+d'5*s25

31、所述车辆航向角为：

32、θ＝α+β

33、

34、

35、式中，x1’为第一x轴坐标值；s1为第一形点距离；y1’为第一y轴坐标值；x2’为第二x轴坐标值；s2为第二形点距离；y2’为第二y轴坐标值；θ为车辆航向角；α为第一夹角；β为第二夹角；c0为第一参数；c1为第二参数；c2为第三参数；c3为第四参数；c4为第五参数；c5为第六参数；c'0为第七参数；c'1为第八参数；c'2为第九参数；c'3为第十参数；c'4为第十一参数；c'5为第十二参数；d0为第十三参数；d1为第十四参数；d2为第十五参数；d3为第十六参数；d4为第十七参数；d5为第十八参数；d'0为第十九参数；d'1为第二十参数；d'2为第二十一参数；d'3为第二十二参数；d'4为第二十三参数；d'5为第二十四参数。

36、另一方面，本发明还提供了一种车辆航向角确定系统，包括：

37、形点数据确定单元，用于基于设置在车辆上的视觉传感器确定第一车道边线的第一形点数据，并基于设置在车辆上的高精度定位系统和高精度地图系统确定第二车道边线的第二形点数据；所述第一形点数据包括各第一车道边线形点在目标坐标系下的第一x轴坐标值和第一y轴坐标值，所述第二形点数据包括各第二车道边线形点在所述目标坐标系下的第二x轴坐标值和第二y轴坐标值；

38、投影点确定单元，用于确定所述车辆到所述第一车道边线的第一投影点以及所述车辆到所述第二车道边线的第二投影点；

39、形点距离确定单元，用于确定各所述第一车道边线形点与所述第一投影点之间的第一形点距离，以及各所述第二车道边线形点与所述第二投影点之间的第二形点距离；

40、五次拟合方程构建单元，用于以所述第一形点距离为自变量，所述第一x轴坐标值和所述第一y轴坐标值为因变量，分别构建第一五次拟合方程以及第二五次拟合方程，并以所述第二形点距离为自变量，所述第二x轴坐标值和所述第二y轴坐标值为因变量，分别构建第三五次拟合方程以及第四五次拟合方程；

41、车辆航向角确定单元，用于基于所述第一五次拟合方程以及第二五次拟合方程确定所述车辆的第一夹角，基于所述第三五次拟合方程以及第四五次拟合方程确定所述车辆的第二夹角，并基于所述第二夹角和所述第二夹角确定车辆航向角。

42、另一方面，本发明还提供了一种车辆，包括存储器和处理器，其中，

43、所述存储器，用于存储程序；

44、所述处理器，与所述存储器耦合，用于执行所述存储器中存储的所述程序，以实现上述任意一种可能的实现方式中所述的车辆航向角确定方法中的步骤。

45、另一方面，本发明还提供了一种计算机可读存储介质，用于存储计算机可读取的程序或指令，所述程序或指令被处理器执行时能够实现上述任意一种可能的实现方式中所述的车辆航向角确定方法中的步骤。

46、采用上述实施例的有益效果是：本发明提供的车辆航向角确定方法，基于设置在车辆上的高精度定位系统和高精度地图系统确定第二车道边线的第二形点数据，相比于二维语义地图，高精度地图系统中的高精度地图精度更高且稳定，且本发明通过确定车辆到第一车道边线的第一投影点以及车辆到第二车道边线的第二投影点，以车辆这个统一参考物分别构建五次拟合方程，可确保第一形点数据和第二形点数据的匹配精度，从而提高确定出的车辆航向角的准确性。

47、进一步地，本发明通过以第一形点距离为自变量，第一x轴坐标值和第一y轴坐标值为因变量，分别构建第一五次拟合方程以及第二五次拟合方程，并以第二形点距离为自变量，第二x轴坐标值和第二y轴坐标值为因变量，分别构建第三五次拟合方程以及第四五次拟合方程，基于四个五次拟合方程即可确定出车辆航向角，无需进行非线性迭代，计算过程简单且计算量小，从而提高了车辆航向角的确定效率。

48、更进一步地，本发明以第一形点距离为自变量，第一x轴坐标值和第一y轴坐标值为因变量，分别构建第一五次拟合方程以及第二五次拟合方程，并以第二形点距离为自变量，第二x轴坐标值和第二y轴坐标值为因变量，分别构建第三五次拟合方程以及第四五次拟合方程，而并非是直接以第一x轴坐标值和第二x轴坐标值为自变量，第一y轴坐标值和第二y轴坐标值为因变量构建拟合方程，确保拟合出的曲线不失真，且降低了第一车道边线形点和第二车道边线形点之间的误差，使确定车辆航向角，稳定可靠，不会出现跳变的情况。