基于高精度地图的车道线数据融合方法与流程
本发明涉及无人驾驶技术领域，尤其涉及一种基于高精度地图的车道线数据融合方法。
背景技术：
作为无人驾驶汽车感知、路径规划及控制所高度依赖的数据源，车道线数据的准确性及稳定性对于汽车行驶的安全稳定至关重要。然而传统的基于单一数据源的车道线数据处理由于没有额外数据进行交叉验证，其准确性、可靠性难以得到保证。如何将感知识别的车道线与先验的高精度地图数据进行融合并监测车道线数据的可靠性，从而得到更加准确稳定的车道线输出结果对于无人驾驶汽车安全平稳地行驶有着重要意义。
技术实现要素：
本发明的一个目的是解决至少上述问题，并提供至少后面将说明的优点。
本发明还有一个目的是提供一种基于高精度地图的车道线数据融合方法，通过将感知识别的车道线、高精度地图以及定位数据进行融合，得到了更加准确、稳定的车道线输出，同时实现了监测车道线数据本身的可靠性的技术效果。
为实现上述目的和一些其他的目的，本发明采用如下技术方案：
一种基于高精度地图的车道线数据融合方法，主要包括以下步骤：
s1、将车辆的定位数据与高精度地图和在所述定位数据对应位置感知的车道线数据分别结合以构建第一局部道路帧和第二局部道路帧；
s2、根据所述定位数据和高精度地图获得最优估计局部道路帧；其中，所述最优估计局部道路帧指对当前局部道路帧的最优估计值；
s3、将所述最优估计局部道路帧与所述定位数据在时间上进行同步；
s4、依据s1得到的第一局部道路帧对与所述定位数据时间同步后的最优估计局部道路帧进行更新；
s5、将s4得到的更新后的最优估计局部道路帧与所述车道线数据在时间上进行同步；
s6、依据s1得到的第二局部道路帧对与所述车道线数据时间同步后的最优估计局部道路帧进行修正。
优选的是，所述的基于高精度地图的车道线数据融合方法中，所述定位数据包括：位置、姿态、线速度、角速度、位置精度、姿态精度以及定位时间戳；
所述车道线数据包括：精度、曲线段以及车道线时间戳；以及
所述第一局部道路帧和第二局部道路帧均包括：帧的时间戳、帧的位置、帧的姿态、车道线投影精度以及车道线的曲线段。
优选的是，所述的基于高精度地图的车道线数据融合方法中，s1中构建所述第一局部道路帧的方法为：
s1-1、根据所述定位数据由所述高精度地图中抽取对应于所述定位数据的位置的车道线数据；
s1-2、经s1-1获得的车道线数据转换到获得所述定位数据的车辆的坐标系下，得到车道线的曲线段；
s1-3、将所述定位数据中的位置精度和姿态精度转换为所述第一局部道路帧的车道线投影精度；
s1-4、将由所述定位数据获取的定位时间戳、位置和姿态作为所述第一局部道路帧的时间戳、帧的位置以及帧的姿态。
优选的是，所述的基于高精度地图的车道线数据融合方法中，s1中构建所述第二局部道路帧的方法为：
s2-1、将所述定位数据的时间与所述车道线数据的时间同步，采用公式1和公式2分别计算得到所述定位数据与所述车道线数据时间同步后的位置和姿态；
p′l＝vl(tm-tl)+pl公式1；
r′l＝ωl(tm-tl)+rl公式2；
其中，p′l为所述定位数据与所述车道线数据同步后的位置；r′l为所述定位数据与所述车道线数据同步后的姿态；vl为所述定位数据的线速度；tm为所述车道线数据的车道线时间戳；tl为所述定位数据的定位时间戳；pl为所述定位数据的位置；ωl为所述定位数据的角速度；rl为所述定位数据的姿态；
s2-2、将tm、p′l、r′l以及所述车道线数据的曲线段作为所述第二局部道路帧的时间戳、帧的位置、帧的姿态以及车道线的曲线段。
优选的是，所述的基于高精度地图的车道线数据融合方法中，s3中将最优估计局部道路帧与定位数据在时间上进行同步的方法为：
s3-1、对所述最优估计局部道路帧的每条车道线均进行采样，得到点集；
s3-2、计算所述最优估计局部道路帧与所述定位数据的相对位置和相对姿态；
s3-3、依据s3-2得到的相对位置和相对姿态将s3-1得到的点集的坐标系转换到所述定位数据对应的车辆的车辆坐标系，形成新的点集；
s3-4、对由s3-3得到的新的点集做曲线拟合，得到新的曲线段，并利用所述新的曲线段更新所述最优估计局部道路帧的车道线；
s3-5、利用所述定位数据的将所述最优估计局部道路帧的位置、姿态和定位时间戳分别更新所述最优估计局部道路帧的位置、姿态以及时间戳。
优选的是，所述的基于高精度地图的车道线数据融合方法中，s4中依据s1得到的第一局部道路帧对与所述定位数据时间同步后的最优估计局部道路帧进行更新的方法为：
s4-1、获取与所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中相匹配的车道线；
s4-2、根据s4-1得到的车道线的投影精度分别设定由所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中得到的车道线融合时的权重函数；
s4-3、依据s4-2得到的权重函数将相匹配的车道线进行拟合，得到新车道线，利用所述新车道线更新与所述定位数据时间同步后的最优估计局部道路帧中与所述第一局部道路帧中相匹配的车道线，并同时更新所述投影精度。
优选的是，所述的基于高精度地图的车道线数据融合方法中，s4-1中获取所述相匹配的车道线的方法为：
s5-1、对所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中包含的车道线分别按照相同的系数采样，得到对应于所述第一局部道路帧的采样点集a和对应于与所述定位数据时间同步后的最优估计局部道路帧的采样点集b；
s5-2、将所述采样点集a和采样点集b相对应的参数相减后分别计算均值和均方差；
s5-3、分别比较所述均值和均方差是否小于预设的阈值；是，则判定车道线匹配；否，则判定车道线不匹配；
s5-4、检测所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中所有相匹配的车道线的车道线序号是否符合序号偏移约束；是，则判定车道线匹配合理；否，则判定车道线不匹配。
优选的是，所述的基于高精度地图的车道线数据融合方法中，s4-3中依据权重函数将相匹配的车道线进行拟合得到新车道线的方法为：
设待拟合得到新车道线的相匹配的两条车道线分别为l1和l2，相应的曲线段分别为y＝c1(x)，
s6-1、将两条车道线及权重分别按照x采样，得到m个样本，其中m>n+1，从而得到带权重的采样点集{xi,yi,wi}，i＝1,2,3…,m；其中xi和yi为采样点的坐标；wi为所述采样点对应的权重；
s6-2、对所述采样点集进行n次曲线拟合，设目标曲线为
优选的是，所述的基于高精度地图的车道线数据融合方法中，s1前还需对感知的所述车道线数据中的异常车道线进行滤除，所述异常车道线通过判断车道线的平行性判定，具体判断方法为：
s7-1、对两条车道线沿车道线的纵向延伸方向等间距的采样，得到两组分别对应于两条车道线的采样点集；
s7-2、计算两组所述采样点集中对应的点对间的横向距离的均方差；
s7-3、判断得到的横向距离的均方差是否小于预设的最大阈值；是，则判定两条车道线平行；否，则判定两条车道线不平行。
优选的是，所述的基于高精度地图的车道线数据融合方法中，s6中依据s1得到的第二局部道路帧对与所述车道线数据时间同步后的最优估计局部道路帧进行修正的方法为：
s8-1、将所述第二局部道路帧的时间和与所述车道线数据时间同步后的最优估计局部道路帧的时间同步，采用公式3和公式4分别计算得到时间同步后的位置和姿态；；
p″l＝vl(tf-tl)+pl公式3；
r″l＝ωl(tf-tl)+rl公式4；
其中，p″l为所述第二局部道路帧的时间和与所述车道线数据时间同步后的最优估计局部道路帧的时间同步后的位置；r″l为所述第二局部道路帧的时间和与所述车道线数据时间同步后的最优估计局部道路帧的时间同步后的姿态；vl为所述定位数据的线速度；tf为所述第二局部道路帧的时间戳；tl为所述定位数据的定位时间戳；pl为所述定位数据的位置；ωl为所述定位数据的角速度；rl为所述定位数据的姿态；
s8-2、将tf、p″l、r″l、所述第二局部道路帧的车道线投影精度以及第二局部道路帧的车道线的曲线段分别作为与所述车道线数据时间同步后的最优估计局部道路帧的时间戳、帧的位置、帧的姿态、车道线投影精度以及车道线的曲线段。
本发明至少包括以下有益效果：
本发明的基于高精度地图的车道线数据融合方法中，通过融合感知识别的车道线与高精度地图数据，实现了对车道线识别的交叉验证，避免了单点故障，提高了车道线输出结果的准确性和可靠性。
通过使用车辆的定位数据与高精度地图结合对车道线进行预测，然后通过车辆的定位数据与感知的车道线数据对预测的车道线进行修正，即通过预测和修正两步滤波，使得车道线输出结果更加稳定和平滑。
本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。
附图说明
图1是本发明提供的基于高精度地图的车道线数据融合方法的流程图。
具体实施方式
下面结合附图对本发明做详细说明，以令本领域普通技术人员参阅本说明书后能够据以实施。
如图1所示，一种基于高精度地图的车道线数据融合方法，主要包括以下步骤：
s1、将车辆的定位数据与高精度地图和在所述定位数据对应位置感知的车道线数据分别结合以构建第一局部道路帧和第二局部道路帧；
s2、根据所述定位数据和高精度地图获得最优估计局部道路帧；其中，所述最优估计局部道路帧指对当前局部道路帧的最优估计值；
s3、将所述最优估计局部道路帧与所述定位数据在时间上进行同步；
s4、依据s1得到的第一局部道路帧对与所述定位数据时间同步后的最优估计局部道路帧进行更新；
s5、将s4得到的更新后的最优估计局部道路帧与所述车道线数据在时间上进行同步；
s6、依据s1得到的第二局部道路帧对与所述车道线数据时间同步后的最优估计局部道路帧进行修正。
在上述方案中，车辆定位数据和高精度地图结合构建第一局部道路帧，通过第一局部道路帧对最优估计局部道路帧进行更新，实现通过定位数据和高精度地图对车道线的预测，然后通过定位数据和感知的车道线数据结合构建第二局部道路帧，通过第二局部道路帧对预测的车道线进行修正，通过反复的预测和修正有效的提高了车道线输出结果的准确性和可靠性。
通过融合感知识别的车道线与高精度地图数据，实现了对车道线识别的交叉验证，避免了单点故障，提高了车道线输出结果的准确性和可靠性。
通过使用车辆的定位数据与高精度地图结合对车道线进行预测，然后通过车辆的定位数据与感知的车道线数据对预测的车道线进行修正，即通过预测和修正两步滤波，使得车道线输出结果更加稳定和平滑。
另外，最优估计局部道路帧是基于定位数据和高精度地图组合来构建的，具体步骤与构建局部道路帧类似。若定位并未处于高精度地图范围之内，则以定位和车道线数据组合来构建。
一个优选方案中，所述定位数据包括：位置、姿态、线速度、角速度、位置精度、姿态精度以及定位时间戳。
所述车道线数据包括：精度、曲线段以及车道线时间戳。
所述第一局部道路帧和第二局部道路帧均包括：帧的时间戳、帧的位置、帧的姿态、车道线投影精度以及车道线的曲线段。
一个优选方案中，s1中构建所述第一局部道路帧的方法为：
s1-1、根据所述定位数据由所述高精度地图中抽取对应于所述定位数据的位置的车道线数据；
s1-2、经s1-1获得的车道线数据转换到获得所述定位数据的车辆的坐标系下，得到车道线的曲线段；
s1-3、将所述定位数据中的位置精度和姿态精度转换为所述第一局部道路帧的车道线投影精度；
s1-4、将由所述定位数据获取的定位时间戳、位置和姿态作为所述第一局部道路帧的时间戳、帧的位置以及帧的姿态。
在上述方案中，根据定位数据从高精度地图中抽取在该定位下的车道线数据并转换到车体坐标系，赋给局部道路帧的车道线li；由于高精度地图数据可被认为是不带误差的真值，因此误差可认为仅仅来源于定位，所以将定位数据的位置精度和姿态精度转换为车道线投影精度并赋值；该局部道路帧的时间戳、位置及姿态均完全拷贝自定位数据的相应字段。
一个优选方案中，s1中构建所述第二局部道路帧的方法为：
s2-1、将所述定位数据的时间与所述车道线数据的时间同步，采用公式1和公式2分别计算得到所述定位数据与所述车道线数据时间同步后的位置和姿态；
p′l＝vl(tm-tl)+pl公式1；
r′l＝ωl(tm-tl)+rl公式2；
其中，p′l为所述定位数据与所述车道线数据同步后的位置；r′l为所述定位数据与所述车道线数据同步后的姿态；vl为所述定位数据的线速度；tm为所述车道线数据的车道线时间戳；tl为所述定位数据的定位时间戳；pl为所述定位数据的位置；ωl为所述定位数据的角速度；rl为所述定位数据的姿态；
s2-2、将tm、p′l、r′l以及所述车道线数据的曲线段作为所述第二局部道路帧的时间戳、帧的位置、帧的姿态以及车道线的曲线段；其中，由于无需进行坐标系转换，因此车道线投影精度直接来源于车道线精度。
一个优选方案中，s3中将最优估计局部道路帧与定位数据在时间上进行同步的方法为：
s3-1、对所述最优估计局部道路帧的每条车道线均进行采样，得到点集；
s3-2、计算所述最优估计局部道路帧与所述定位数据的相对位置和相对姿态；
s3-3、依据s3-2得到的相对位置和相对姿态将s3-1得到的点集的坐标系转换到所述定位数据对应的车辆的车辆坐标系，形成新的点集；
s3-4、对由s3-3得到的新的点集做曲线拟合，得到新的曲线段，并利用所述新的曲线段更新所述最优估计局部道路帧的车道线；
s3-5、利用所述定位数据的将所述最优估计局部道路帧的位置、姿态和定位时间戳分别更新所述最优估计局部道路帧的位置、姿态以及时间戳。
一个优选方案中，s4中依据s1得到的第一局部道路帧对与所述定位数据时间同步后的最优估计局部道路帧进行更新的方法为：
s4-1、获取与所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中相匹配的车道线；
s4-2、根据s4-1得到的车道线的投影精度分别设定由所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中得到的车道线融合时的权重函数；
s4-3、依据s4-2得到的权重函数将相匹配的车道线进行拟合，得到新车道线，利用所述新车道线更新与所述定位数据时间同步后的最优估计局部道路帧中与所述第一局部道路帧中相匹配的车道线，并同时更新所述投影精度。
在上述方案中，若车道线不匹配，则标记更新失败，并在更新失败次数超出设定的次数后，重新初始化最优估计局部道路帧。
一个优选方案中，s4-1中获取所述相匹配的车道线的方法为：
s5-1、对所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中包含的车道线分别按照相同的系数采样，得到对应于所述第一局部道路帧的采样点集a和对应于与所述定位数据时间同步后的最优估计局部道路帧的采样点集b；
s5-2、将所述采样点集a和采样点集b相对应的参数相减后分别计算均值和均方差；
s5-3、分别比较所述均值和均方差是否小于预设的阈值；是，则判定车道线匹配；否，则判定车道线不匹配；
s5-4、检测所述第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧中所有相匹配的车道线的车道线序号是否符合序号偏移约束；是，则判定车道线匹配合理；否，则判定车道线不匹配。
在上述方案中，s5-4中检测匹配合理性的操作为：对于第一局部道路帧以及与所述定位数据时间同步后的最优估计局部道路帧中所有匹配的车道线，应当符合序号偏移约束-1，+1或者0。例如第一局部道路帧的车道线序号1,2,3分别匹配与所述定位数据时间同步后的最优估计局部道路帧的车道线序号2,3,4，相匹配的所有序号均偏移+1，则认为为合理匹配，若匹配不合理，则认为第一局部道路帧和与所述定位数据时间同步后的最优估计局部道路帧不匹配。
一个优选方案中，s4-3中依据权重函数将相匹配的车道线进行拟合得到新车道线的方法为：
设待拟合得到新车道线的相匹配的两条车道线分别为l1和l2，相应的曲线段分别为y＝c1(x)，
s6-1、将两条车道线及权重分别按照x采样，得到m个样本，其中m>n+1，从而得到带权重的采样点集{xi,yi,wi}，i＝1,2,3…,m；其中xi和yi为采样点的坐标；wi为所述采样点对应的权重；
s6-2、对所述采样点集进行n次曲线拟合，设目标曲线为
在上述方案中，矩阵方程1可以使用qr分解、最小二乘法，以及svd等多种方法进行求解。
一个优选方案中，s1前还需对感知的所述车道线数据中的异常车道线进行滤除，所述异常车道线通过判断车道线的平行性判定，具体判断方法为：
s7-1、对两条车道线沿车道线的纵向延伸方向等间距的采样，得到两组分别对应于两条车道线的采样点集；
s7-2、计算两组所述采样点集中对应的点对间的横向距离的均方差；
s7-3、判断得到的横向距离的均方差是否小于预设的最大阈值；是，则判定两条车道线平行；否，则判定两条车道线不平行。
在上述方案中，现有技术中一般感知识别的车道线有4条，如果某条车道线与其余3条车道线均不平行，则将其标记为异常车道线。而后即可采用上述方法进行两条车道线间平行性的判断。
一个优选方案中，s6中依据s1得到的第二局部道路帧对与所述车道线数据时间同步后的最优估计局部道路帧进行修正的方法为：
s8-1、将所述第二局部道路帧的时间和与所述车道线数据时间同步后的最优估计局部道路帧的时间同步，采用公式3和公式4分别计算得到时间同步后的位置和姿态；；
p″l＝vl(tf-tl)+pl公式3；
r″l＝ωl(tf-tl)+rl公式4；
其中，p″l为所述第二局部道路帧的时间和与所述车道线数据时间同步后的最优估计局部道路帧的时间同步后的位置；r″l为所述第二局部道路帧的时间和与所述车道线数据时间同步后的最优估计局部道路帧的时间同步后的姿态；vl为所述定位数据的线速度；tf为所述第二局部道路帧的时间戳；tl为所述定位数据的定位时间戳；pl为所述定位数据的位置；ωl为所述定位数据的角速度；rl为所述定位数据的姿态；
s8-2、将tf、p″l、r″l、所述第二局部道路帧的车道线投影精度以及第二局部道路帧的车道线的曲线段分别作为与所述车道线数据时间同步后的最优估计局部道路帧的时间戳、帧的位置、帧的姿态、车道线投影精度以及车道线的曲线段。
尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里所示出与描述的图例。
- 还没有人留言评论。精彩留言会获得点赞！