一种实时感知信息与自动驾驶地图的融合方法及系统与流程

本发明涉及一种自动驾驶技术领域，特别是关于一种实时感知信息与自动驾驶地图的融合方法及系统。

背景技术：

自动驾驶是一个高度复杂的集成系统，高级别自动驾驶需要完整而鲁棒的环境感知、决策规划和车辆控制。环境感知技术是整个系统的基础，目前技术的检测率、准确度等还远不能达到安全性要求，因此利用地图提供先验信息辅助实时感知，已经逐渐成为业界共识。自动驾驶地图提供大量的高精度先验静态信息，如道路边界、路面形状、车道线、交通标志、固定障碍物等，而实时感知则主要提供动态信息。

实际上，地图信息和实时感知信息并不是简单的静态和动态的叠加关系，实时感知可以得到很多静态信息，自动驾驶地图也隐含了许多动态信息的运动规律，同时，静态信息和动态信息的关系也是环境理解的重要内容。现有研究中，一些是简单地合并地图信息和实时感知信息，也有一些从某一个方面实现两者信息的融合，如定位、可行驶区域、参考路径等，但并未全面彻底地揭示并利用各种元素之间的联系。

技术实现要素：

针对现有研究中对自动驾驶地图与实时感知信息的联系考虑的不足，本发明的目的是提供一种实时感知信息与自动驾驶地图的融合方法及系统，其能实现各种环境元素在地图平台上的联系和整合。

为实现上述目的，本发明采取以下技术方案：一种实时感知信息与自动驾驶地图的融合方法，其包括以下步骤：1）进行地图匹配定位，给出自车在地图坐标系中的位置，并明确自车坐标系与地图坐标系间的关系；2）确定障碍物与地图中道路的关系；实时感知到的障碍物信息，根据与道路的关系分为三类：道路内目标、道路外目标和道路边界目标；其中，道路内目标，应确认其所位于的车道；根据地图匹配定位时给出的自车坐标系与地图坐标系的关系，将障碍物信息与地图信息进行比对，得到障碍物与道路关系；障碍物与地图中道路的关系分为两种情况：2.1）三维传感器检测到的障碍物：这类传感器能直接检测得到障碍物在自车坐标系内的运动状态，利用自车坐标系与地图坐标系之间的关系，将障碍物位置转移到地图上，从而与地图进行比对；2.2）单目视觉传感器在图像平面内检测到的障碍物：这种传感器得到的是地图坐标系在图像坐标系上的平面投影，通过标定矩阵，将地图中的道路边界和车道线投影到图像中，再与图像中的障碍物进行对比，判断障碍物与道路关系，并得到障碍物所在的车道；3）可行驶区域融合与边界状态分析；4）将步骤2）、步骤3）与地图本身的静态环境信息相结合，实现感知结果的集成输出。

进一步，所述步骤2）中，利用地图匹配定位结果，将实时感知到的障碍物转到地图坐标系中，以障碍物为目标，根据道路边界对目标进行筛选，对道路边界内部的目标，判断其位于哪个车道，并明确其与该车道的关系，得到融合地图信息后的动静态目标。

进一步，所述障碍物与道路的关系按下式计算：

（1）

式中，为道路左边界；为道路右边界；o为障碍物；dist为距离函数。

进一步，若未观测到障碍物形状，则按照其位置，计算到车道线的距离；若观测到障碍物形状，则针对包围盒各顶点计算到车道线距离，若符号全部相同，则取数值上最小者；若符号不完全相同，则取为0，如下式所示：

（2）

式中，d为点到直线距离函数，l为车道线，p为障碍物位置，vi为障碍物顶点，i的取值范围为1至n的整数，n为障碍物顶点数。

进一步，对道路内目标判断其所位于的车道时，逐一检查各车道，以给定车道的左右边界代替公式（1）中的道路左右边界，即可判断目标是否位于该车道，直至找到某条车道，使该目标位于此车道边界内或边界上；对于道路内目标，进一步计算出其与该车道的关系：根据障碍物朝向与车道线朝向，判断障碍物朝向相对于车道中心线朝向的偏转角；根据障碍物位置、形状与车道线，利用公式（2）计算其到左右车道线的距离。

进一步，将地图中的道路边界和车道线投影到图像中，公式为：

式中，，，为自车坐标系内坐标，mqj为相机参数矩阵的元素，q=0、1、2，j=1、2、3、4；u，v分别为图像像素横、纵坐标，z为相机光心高度。

进一步，所述步骤3）中，针对可行驶区域，将实时感知得到的可行驶区域与地图提供的可行驶区域进行融合，并根据实时感知障碍物信息，分段判断可行驶区域边界的状态，得到边界语义以及速度信息。

进一步，通过激光雷达或视觉传感器的地面连通区域分割，实现可行驶区域的实时感知；利用地图提供的道路边界，以及停止线、实线这些交通规则不可跨越的边界，缩减可行驶区域的范围；同时，结合障碍物信息，明确可行驶区域边界的语义以及速度。

一种实时感知信息与自动驾驶地图的融合系统，其包括地图匹配定位模块、关系确定模块、分析模块和输出模块；所述地图匹配定位模块用于进行地图匹配定位，给出自车在地图坐标系中的位置，并明确自车坐标系与地图坐标系间的关系；所述关系确定模块用于确定障碍物与地图中道路的关系；实时感知到的障碍物信息，根据与道路的关系分为三类：道路内目标、道路外目标和道路边界目标；其中，道路内目标，应确认其所位于的车道；根据地图匹配定位时给出的自车坐标系与地图坐标系的关系，将障碍物信息与地图信息进行比对，得到障碍物与道路关系；障碍物与地图中道路的关系分为两种情况：（1）三维传感器检测到的障碍物：这类传感器能直接检测得到障碍物在自车坐标系内的运动状态，利用自车坐标系与地图坐标系之间的关系，将障碍物位置转移到地图上，从而与地图进行比对；（2）单目视觉传感器在图像平面内检测到的障碍物：这种传感器得到的是地图坐标系在图像坐标系上的平面投影，通过标定矩阵，将地图中的道路边界和车道线投影到图像中，再与图像中的障碍物进行对比，判断障碍物与道路关系，并得到障碍物所在的车道；所述分析模块对可行驶区域融合与边界状态进行分析；所述输出模块将所述关系确定模块、分析模块的结果与地图本身的静态环境信息相结合，实现感知结果的集成输出。

进一步，所述分析模块中，针对可行驶区域，将实时感知得到的可行驶区域与地图提供的可行驶区域进行融合，并根据实时感知障碍物信息，分段判断可行驶区域边界的状态，得到边界语义以及速度信息。

本发明由于采取以上技术方案，其具有以下优点：本发明过滤掉了道路外的障碍物，并明确了道路内的障碍物所在的车道以及其与该车道的位置角度关系，为运动预测提供了依据；根据地图信息剔除了感知得到的可行驶区域中不合理的部分，并得到了边界的语义类别与速度。至此，各种环境元素在地图平台上得到了联系和整合。

附图说明

图1是本发明的整体方法流程示意图。

图2是地图坐标下障碍物与道路关系判断示意图。

图3是地图坐标下障碍物与所在车道关系判断示意图。

图4是图像坐标下障碍物与道路及车道关系判断示意图。

图5是可行驶区域融合与边界状态分析示意图。

具体实施方式

本发明包括障碍物与地图中道路的关系、可行驶区域融合与边界状态分析两个部分。地图匹配定位技术是本发明的基础，该技术可以通过实时感知静态信息与地图比对，得到更为精确的自车状态，明确自车与道路的关系并统一坐标系。下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明提供一种实时感知信息与自动驾驶地图的融合方法，其包括以下步骤：

1）进行地图匹配定位，给出自车在地图坐标系中的位置，并明确自车坐标系与地图坐标系间的关系；

地图匹配定位已有较多现有成熟技术，在此不再赘述，采用现有方法即可。

2）确定障碍物与地图中道路的关系：利用地图匹配定位结果，将实时感知到的障碍物转到地图坐标系中，以障碍物为目标，根据道路边界对目标进行筛选，对道路边界内部的目标，判断其位于哪个车道，并明确其与该车道的关系，得到融合地图信息后的动静态目标。

其中，实时感知到的障碍物信息，根据与道路的关系可以分为三类：道路内目标、道路外目标和道路边界目标：

道路外目标与驾驶无关，可以删除，道路外目标则被滤除，不再出现在最终的集成感知结果中。

道路内目标，还应进一步确认其所位于的车道；由于在地图匹配定位时给出了自车坐标系与地图坐标系的关系（即两个坐标系的三维旋转平移矩阵），可以据此将障碍物信息与地图信息进行比对，得到障碍物与道路关系。

障碍物与地图中道路的关系具体确定可以分为两种情况：

2.1）毫米波雷达、激光雷达、多目立体视觉等三维传感器检测到的障碍物。

这类传感器能直接检测得到障碍物在自车坐标系内的位置、朝向、速度等运动状态，利用自车坐标系与地图坐标系之间的关系，即可将障碍物位置转移到地图上，从而与地图进行比对。

例如，如图2所示，左侧道路边界为护栏，则护栏左侧的对向车道上的障碍物不会影响到本车驾驶，属于道路外目标，将其滤除。右侧道路边界和人行道路沿都会产生毫米波雷达回波并输出为目标，结合地图即可确认它们属于道路边界目标。余下的点位于车行道或人行道上，则为道路内目标，根据其位置即可判断出其位于哪条车道。

障碍物与道路的关系按下式计算：

（1）

式中，为道路左边界；为道路右边界；o为障碍物；dist为距离函数，规定左侧为正，右侧为负，若到左右边界距离乘积小于零则位于道路内部，等于零则位于边界上，大于零则位于道路外部。

若未观测到障碍物形状，则按照其位置，计算到车道线的距离；若观测到障碍物形状，则针对包围盒各顶点计算到车道线距离，若符号全部相同，则取数值上最小者；若符号不完全相同，则取为0，如下式所示：

（2）

式中，d为点到线距离函数，l为车道线，p为障碍物位置，vi为障碍物顶点，i的取值范围为1至n的整数，n为障碍物顶点数。

对道路内目标判断其所位于的车道时，逐一检查各车道，以给定车道的左右边界代替公式（1）中的道路左右边界，即可判断目标是否位于该车道，直至找到某条车道，使该目标位于此车道边界内或边界上。

对于道路内目标，进一步计算出其与该车道的关系。如图3所示，根据障碍物朝向与车道线朝向，判断障碍物朝向相对于车道中心线朝向的偏转角。根据障碍物位置、形状与车道线，利用公式（2）计算障碍物到左车道线的距离、到右车道线的距离。这些信息对目标未来的运动状态预测提供了依据。

2.2）单目视觉传感器在图像平面内检测到的障碍物。

这种传感器得到的是地图坐标系在图像坐标系上的平面投影，因此不具有三维尺度信息，不能通过图像平面内的障碍物还原出其在地图坐标系中的位置和形状。但是，通过标定矩阵，可以将地图中的道路边界和车道线投影到图像中，再与图像中的障碍物进行对比，判断障碍物与道路关系，并得到障碍物所在的车道。

将地图中的道路边界和车道线投影到图像中，公式为：

（3）

式中，，，为自车坐标系下的坐标，mqj为相机参数矩阵的元素，其中q=0、1、2，j=1、2、3、4。u，v分别为图像像素横、纵坐标，z为参数（相机光心高度），可由（3）矩阵的第三行求出。将道路边界和车道线的采样点通过公式（3）转换到图像坐标，即可得到道路边界和车道线在图像坐标中的投影。

如图4所示，给出了一个根据图像平面内地图投影与图像平面内障碍物信息判断障碍物与道路车道关系的实施例。基于图像的障碍物检测算法若只能给出2d包围框（如障碍物1、2、3），则可以根据2d包围框底边（即接地线）判断其是否位于道路边界内，针对道路边界内的目标还可判断其所在车道，以底边两个顶点作为公式（2）中的顶点，在像素坐标内应用公式（1）（2），即可判断道路边界内目标所在的车道以及其与车道的相对位置关系。

但在看到障碍物侧、后两个面时，2d包围框不能将其区分，因此利用2d包围框底边将造成误差。若检测算法能给出3d障碍物投影信息（如障碍物4），则可以根据其侧边与底边的3个顶点（即图中a、b、c点），应用公式（1）（2）判断目标所在的车道以及其与车道的相对位置关系。

3）可行驶区域融合与边界状态分析：针对可行驶区域，将实时感知得到的可行驶区域与地图提供的可行驶区域进行融合，并根据实时感知障碍物信息，分段判断可行驶区域边界的状态，得到边界语义以及速度信息。

具体融合方法为：通过激光雷达或视觉传感器的地面连通区域分割，可以实现可行驶区域的实时感知。并利用地图提供的道路边界，以及停止线、实线等交通规则约束所形成的不可跨越的虚拟边界，可以缩减可行驶区域的范围；同时，结合障碍物信息，明确可行驶区域边界的语义（指边界的类型）以及速度。

如图5所示，考虑交通规则约束的虚拟边界，将感知得到的可行驶区域的范围收窄。同时给分段边界的部分边界段赋予了语义信息，即路沿护栏等形成的道路物理边界、交通规则边界，其边界速度为0。同时，与实时感知障碍物融合，明确哪些边界段由动态障碍物形成，同时以动态障碍物速度为可行驶区域边界速度。余下的边界段则为传感器探测边界形成，边界速度为0，至此完成了全部边界段的语义和速度划分。

4）将步骤2）、步骤3）与地图本身的静态环境信息相结合，实现感知结果的集成输出：

利用环境中各种元素之间的关联，经过障碍物与道路关系、可行驶区域融合与边界状态分析，实时感知与地图信息实现了融合。过滤掉了道路外的障碍物，并明确了道路内的障碍物所在的车道以及其与该车道的位置角度关系，为运动预测提供了依据；根据地图信息剔除了感知得到的可行驶区域中不合理的部分，并得到了边界的语义类别与速度。至此，各种环境元素在地图平台上得到了联系和整合。

本发明还提供一种实时感知信息与自动驾驶地图的融合系统，其包括地图匹配定位模块、关系确定模块、分析模块和输出模块；

地图匹配定位模块用于进行地图匹配定位，给出自车在地图坐标系中的位置，并明确自车坐标系与地图坐标系间的关系；

关系确定模块用于确定障碍物与地图中道路的关系；实时感知到的障碍物信息，根据与道路的关系分为三类：道路内目标、道路外目标和道路边界目标；其中，道路内目标，应确认其所位于的车道；根据地图匹配定位时给出的自车坐标系与地图坐标系的关系，将障碍物信息与地图信息进行比对，得到障碍物与道路关系；

障碍物与地图中道路的关系分为两种情况：

（1）三维传感器检测到的障碍物：这类传感器能直接检测得到障碍物在自车坐标系内的运动状态，利用自车坐标系与地图坐标系之间的关系，将障碍物位置转移到地图上，从而与地图进行比对；

（2）单目视觉传感器在图像平面内检测到的障碍物：这种传感器得到的是地图坐标系在图像坐标系上的平面投影，通过标定矩阵，将地图中的道路边界和车道线投影到图像中，再与图像中的障碍物进行对比，判断障碍物与道路关系，并得到障碍物所在的车道；

分析模块对可行驶区域融合与边界状态进行分析；

输出模块将所述关系确定模块、分析模块的结果与地图本身的静态环境信息相结合，实现感知结果的集成输出。

上述实施例中，在分析模块中，针对可行驶区域，将实时感知得到的可行驶区域与地图提供的可行驶区域进行融合，并根据实时感知障碍物信息，分段判断可行驶区域边界的状态，得到边界语义以及速度信息。

上述各实施例仅用于说明本发明，各个步骤都是可以有所变化的，在本发明技术方案的基础上，凡根据本发明原理对个别步骤进行的改进和等同变换，均不应排除在本发明的保护范围之外。