一种用于自动驾驶的目标航向角的信息融合方法及存储介质与流程

1.本发明涉及自动驾驶技术领域，具体涉及一种用于自动驾驶的目标航向角的信息融合方法及存储介质。


背景技术：

2.当前自动驾驶已经成为社会的热点，不仅是学术研究的热点，而且各个企业也在加大投入，以期将更高级的自动驾驶技术落地量产。在功能实现中，越高级的自动驾驶功能，其对于感知的精细度要求越高。而对于目标的感知，准确感知目标的航向角就是一个关键技术点。只有准确的感知到目标的航向角，才能准确判断目标的运动趋势以及与目标的碰撞风险，最终才能做出正确的决策规划。
3.在现实中，对于目标航向角的检测虽然视觉传感器能给出一个较准确的检测，但是精度和稳定性远远不够，比如，当目标处于传感fov边界时根本无法完整识别。而毫米波雷达虽然能检测目标的部分角点特征，例如申请号为cn202011606638.1，名称为目标位姿计算方法及计算模块记载的技术方案，但是采用毫米波雷达的精度不够，无法实现目标的准确感知，因此，需要对多传感的信息进行融合从而得到更准确的目标航向角。


技术实现要素：

4.针对现有技术存在的上述不足，本发明的目的在于解决现有的目标航向角的检测准确性和稳定性差的问题，提供一种用于自动驾驶的目标航向角的信息融合方法及存储介质，能够提高目标航向角检测的准确性和稳定性。
5.为了解决上述技术问题，本发明采用如下技术方案：
6.一种用于自动驾驶的目标航向角的信息融合方法，包括以下步骤，
7.获取待测目标关联的视觉freespace观测数据、雷达点云观测数据、视觉目标观测数据以及视觉目标的横向速度及纵向速度；
8.基于视觉freespace观测数据中所有freespace点，采用pca算法进行计算，将得出的主方向作为目标的航向角h1；
9.基于雷达点云观测数据中所有雷达点云，基于pca算法直接计算，将得出的主方向作为目标的航向角h2；
10.基于视觉目标观测数据，直接输出目标航向角h3；
11.基于视觉目标的横向速度及纵向速度，视觉输出的目标的横纵向速度比，即为目标的航向角h4；
12.将航向角h1、h2、h3和h4进行加权求和，再进行低通滤波得到目标的最终航向角。
13.进一步，所述视觉ffeespace观测数据中包含多个观测点，每个观测点的信息里包含横纵向位置信息(fsxn，fsyn)，基于所述(fsxn，fsyn)建立一个二维的pca，得到所有点的位置分布的主方向v1，v1的方向即是航向角h1。
14.进一步，所述雷达点云观测数据中包含多个观测点，每个观测点的信息里包含横
纵向位置信息(pxn，pyn)，基于所述(pxn，pyn)建立一个二维的pca，得到所有点的位置分布的主方向v2，v2的方向即是航向角h2。
15.进一步，所述h4的计算方法为，h4＝arctan(fc_vy
÷
fc_vc)，其中，fc_vy，fc_vc分别为关联的视觉目标的横向速度及纵向速度。
16.进一步，将航向角h1、h2、h3和h4进行加权求和，再进行低通滤波得到目标的最终航向角具体为，首先通过加权得到融合的结果：h＝h1*w1+h2*w2+h3*w3+h4*w4；再通过低通滤波得到目标的最终航向角h*，h
*
＝h*wf+h
p
*(1-wf)；
17.其中w1、w2、w3、w4为信息权重，在工程中根据目标数据进行动态标定调整，wf为滤波参数，也需要进行标定。
18.本发明还提供一种存储介质，所述存储介质存储有一个或者多个程序，所述一个或者多个程序被处理器运行时，执行所述的用于自动驾驶的目标航向角的信息融合方法的步骤。
19.与现有技术相比，本发明具有如下有益效果：
20.本发明基于目标关联的视觉ffeespace观测数据、雷达点云观测数据、视觉目标观测数据以及视觉目标的横向速度及纵向速度等多源信息实现对于目标航向角的检测，然后基于场景调节各信息的权重进行信息融合，再经过一个滤波得出结果。这样，提高了目标航向角检测的准确性和稳定性。有利于自动驾驶过程中准确判断目标的运动趋势以及与目标的碰撞风险。
附图说明
21.图1为本发明一种用于自动驾驶的目标航向角的信息融合方法的流程图。
22.图2为本发明中ffeespace点集及雷达点云观测点集pca主方向与航向角的关系图。
具体实施方式
23.下面将结合附图及实施例对本发明作进一步说明。
24.实施例一
25.本实施例公开一种用于自动驾驶的目标航向角的信息融合方法。
26.一种用于自动驾驶的目标航向角的信息融合方法，参见图1，包括以下步骤，
27.获取待测目标关联的视觉freespace观测数据、雷达点云观测数据、视觉目标观测数据以及视觉目标的横向速度及纵向速度；
28.基于视觉freespace观测数据中所有freespace点，采用pca(主成分分析，下同)算法进行计算，将得出的主方向作为目标的航向角h1；
29.基于雷达点云观测数据中所有雷达点云，基于pca算法直接计算，将得出的主方向作为目标的航向角h2；
30.基于视觉目标观测数据，直接输出目标航向角h3；
31.基于视觉目标的横向速度及纵向速度，视觉输出的目标的横纵向速度比，即为目标的航向角h4；
32.将航向角h1、h2、h3和h4进行加权求和，再进行低通滤波得到目标的最终航向角。
33.具体实施时，参见图2，所述视觉freespace观测数据中包含多个观测点，每个观测点的信息里包含横纵向位置信息(fsxn，fsyn)，基于所述(fsxn，fsyn)建立一个二维的pca，得到所有点的位置分布的主方向v1，v1的方向即是航向角h1。
34.再参见图2，所述雷达点云观测数据中包含多个观测点，每个观测点的信息里包含横纵向位置信息(pxn，pyn)，基于所述(pxn，pyn)建立一个二维的pca，得到所有点的位置分布的主方向v2，v2的方向即是航向角h2。
35.航向角h3从视觉目标观测数据中直接提取。
36.所述h4的计算方法为，h4＝arctan(fc_vy
÷
fc_vc)，其中，fc_vy，fc_vc分别为关联的视觉目标的横向速度及纵向速度。
37.在获得h1、h2、h3和h4后，将航向角h1、h2、h3和h4进行加权求和得到融合的结果，其公式为h＝h1*w1+h2*w2+h3*w3+h4*w4；再进行低通滤波得到目标的最终航向角，所述低通滤波得公式为：h
*
＝h*wf+h
p
*(1-wf)；其中w1、w2、w3、w4为信息权重，在工程中根据目标数据进行动态标定调整，wf为滤波参数，也需要进行标定。
38.这样，基于目标关联的视觉freespace观测数据、雷达点云观测数据、视觉目标观测数据以及视觉目标的横向速度及纵向速度等多源信息实现对于目标航向角的检测，然后基于场景调节各信息的权重进行信息融合，再经过一个滤波得出结果。这样，提高了目标航向角检测的准确性和稳定性。有利于自动驾驶过程中准确判断目标的运动趋势以及与目标的碰撞风险。
39.实施例二
40.为了进一步说明本发明一种用于自动驾驶的目标航向角的信息融合方法的实际效果，本实施例公开了一种基于实施例一实现的目标航向角的信息融合方法及应用场景。
41.在自动驾驶场景中，自动驾驶车辆感知到一目标，并获取了该目标关联的视觉freespace观测数据包括大致能描述目标轮廓的多个点，每个点包含了其横纵向坐标fsxn，fsyn、雷达点云观测数据包括大致能描述目标轮廓的多个点，每个点包含了其横纵向坐标pxn，pyn、视觉目标观测数据包括该目标的横纵向位置和航向角h3，以及视觉目标的横向速度fc_vy及纵向速度fc_vx。
42.基于视觉freespace观测数据中所有freespace点，采用pca算法进行计算，将得出的主方向作为目标的航向角h1，计算如下：
[0043][0044]
其中：x＝[fsx1，fsx2，fsx3...fsxn]
t
；y＝[fsy1，fsy2，fsy3...fsyn]
t
，n为视觉freespace点个数；
[0045]
基于雷达点云观测数据中所有雷达点云，基于pca算法直接计算，将得出的主方向作为目标的航向角h2，计算如下：
[0046][0047]
其中：x＝[px1，px2，px3...pxn]
t
；y＝[py1，py2，py3...pyn]
t
，n为雷达点云个数；
[0048]
基于视觉目标观测数据，直接得到目标航向角h3；
[0049]
基于视觉目标的横向速度及纵向速度，视觉输出的目标的横纵向速度比，即为目
标的航向角h4，计算如下：
[0050]
h4＝arctan(fc_vy
÷
fc_vc).。
[0051]
将航向角h1、h2、h3和h4进行加权求和，再进行低通滤波得到目标的最终航向角，计算如下：
[0052]
h＝h1*w1+h2*w2+h3*w3+h4*w4[0053]h*
＝h*wf+h
p
*(1-wf)
[0054]
其中，w1＝0.2，w2＝0.2，w3＝0.5，w4＝0.1，wf＝0.25。
[0055]
这样，即可根据目标关联的视觉freespace观测数据、雷达点云观测数据、视觉目标观测数据以及视觉目标的横向速度及纵向速度等多源信息获得目标的最终航向角。提高了目标航向角检测的准确性和稳定性。有利于自动驾驶过程中准确判断目标的运动趋势以及与目标的碰撞风险。
[0056]
实施例三
[0057]
本实施例在实施例一的基础上，公开了一种存储介质。
[0058]
一种存储介质，所述存储介质存储有一个或者多个程序，所述一个或者多个程序被处理器运行时，执行用于自动驾驶的目标航向角的信息融合方法的步骤。计算机可读存储介质可以是u盘、硬盘或其他具有存储功能的设备。
[0059]
如上所述，本发明的提醒系统不限于所述配置，其他可以实现本发明的实施例的系统均可落入本发明所保护的范围内。
[0060]
最后需要说明的是，以上实施例仅用以说明本发明的技术方案而非限制技术方案，本领域的普通技术人员应当理解，那些对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，均应涵盖在本发明的权利要求范围当中。