CN111880191B

CN111880191B - 基于多智能体激光雷达和视觉信息融合的地图生成方法

Info

Publication number: CN111880191B
Application number: CN202010548671.7A
Authority: CN
Inventors: 孙嘉航; 李瑜茹; 陈晨; 程翔
Original assignee: Peking University
Current assignee: Peking University
Priority date: 2020-06-16
Filing date: 2020-06-16
Publication date: 2023-03-28
Anticipated expiration: 2040-06-16
Also published as: CN111880191A

Abstract

本发明公布了基于多智能体激光雷达和视觉信息融合的地图生成方法，通过分类识别静态目标和动态目标，每个单智能体分别联合标定单智能体的激光雷达和视觉传感器，并识别单智能体的视觉传感器时间一致下的静态和动态目标，生成栅格地图，再根据单智能体GPS信息，将多智能体的检测结果进行融合。采用本发明提供的技术方案，可为多智能体提供安全可靠、可预测的局部道路区域信息。

Description

基于多智能体激光雷达和视觉信息融合的地图生成方法

技术领域

本发明属于多智能体传感器技术领域，涉及智能体激光雷达和视觉信息融合方法，尤其涉及一种基于多智能体激光雷达和视觉信息融合构建栅格地图的方法。

背景技术

近些年来，由于生物学、计算机科学、人工智能、控制科学、社会学等多个学科交叉和渗透发展，多智能体系统越来越受到众多学者的广泛关注，已成为当前控制学科以及人工智能领域的研究热点。多智能体(multi-agent system)的目标是让若干个具备简单智能却便于管理控制的系统能通过相互协作实现复杂智能，使得在降低系统建模复杂性的同时，提高系统的鲁棒性、可靠性、灵活性。目前多智能体已在智能机器人、传感器网络、数据融合、多机器人合作控制、交通车辆控制、网络的资源分配等领域广泛应用。

随着信息技术和智能机器人的发展，集视觉设计、人工智能、体系结构、自动控制等众多技术于一体的智能体应运而生。通过智能体传感系统感知道路环境,为达到预定目标自动规划行驶路线并控制行驶姿态，实现安全的行驶。

在智能体的各项技术中，环境感知是实现车辆自动化、智能化运行的前提条件，环境感知作为第一环节，处于智能体与外界环境信息交互的关键位置，其关键在于使智能体更好地模拟人类的感知能力，从而理解自身和周边的环境态势。环境感知主要指对智能体周围的三类物体进行感知：路面、静态物体和动态物体。感知系统不仅能确定周边障碍物的位置、运动轨迹等信息，还会识别周围的路况信息、天气状况等，好的感知系统能为智能体的规划和控制提供准确的参考信息，使之做出正确的决策。

智能体上安装的每类传感器也都有自身的感知盲区。实际上，这些区域仅相对于特定时刻而言，随着智能体的行进在下一时刻即会产生新的盲区。行驶过程中，通过组合使用多类传感器和运用时序关联的感知技术，可以缩小感知盲区的范围，将各种数据合理地进行互补与融合，才能够保证整个系统的可靠运行。如何选择合适的技术将智能体多传感器数据融合起来，以达到良好的效果，是目前智能体环境感知研究的重要方向。

申请号为“201410252993.1”，申请公布号为“CN104002747A”，名称为《一种基于无人驾驶汽车的多激光雷达栅格地图融合系统》的中国发明申请，公开了一种基于无人驾驶汽车的多激光雷达栅格地图融合系统，包括安装在无人驾驶汽车前方的一线激光雷达和四线激光雷达，安装在车顶的八线激光雷达，安装在车后的一线激光雷达；上述激光雷达通过交换机并利用以太网船只负责数据融合的第一中控机，再利用栅格地图融合方法进行数据融合并对数据进行栅格化编码，最后通过以太网将编码值传至第二工控机。其解决无人驾驶汽车和辅助驾驶汽车与障碍物放生碰撞的问题，提高车辆行驶过程中的安全性。该智能车辆中多激光雷达数据融合系统，虽然可以提供障碍物信息，但是其数据信息单一，结构复杂，成本较高，实用性受限制。

申请号为“201811236936.9”，申请公布号为“CN109443369A”，名称为《利用激光雷达和视觉传感器融合构建动静态栅格地图的方法》的中国发明申请，公开了一种利用激光雷达和视觉传感器融合构建动静态栅格地图的方法，联合标定行驶车辆的激光雷达和视觉传感器，通过分类标识静态目标和动态目标，并确保传感器信息时间同步；在车辆行进过程中，构建多维度动静态栅格地图。本发明构建的动静态栅格地图可以表示当前车辆周围的道路状态，例如可行驶区域、周围的障碍物、障碍物的运动状态等等，为车辆在当前局部区域安全行驶提供信息，为无人驾驶汽车提供安全可靠、可预测的局部道路区域信息。该方法虽然融合了多传感器数据，但是随着车辆行驶状态的改变，仍然存在着出现盲区的可能，存在一定安全隐患。

综上，现有技术难以实现结构简单、低成本且安全性高的基于多智能体激光雷达和视觉信息融合生成栅格地图的技术，无法满足实际情况的需要。

发明内容

本发明的目的在于克服上述技术的不足，提供一种基于多智能体利用激光雷达和视觉传感器融合构建栅格地图的方法，通过分类识别静态目标和动态目标，生成栅格地图，为多智能体提供安全可靠、可预测的局部道路区域信息。

本发明为实现上述目的，采用以下技术方案：

一种基于多智能体利用激光雷达和视觉传感器融合构建栅格地图的方法，每个单智能体分别联合标定单智能体的激光雷达和视觉传感器，并识别单智能体的视觉传感器时间一致下的静态和动态目标；根据单智能体GPS(Global Positioning System，全球定位系统)信息，将多智能体的检测结果融合起来，在单智能体行进过程中，由于视觉传感器采集频率低，因此每个单智能体的视觉传感器采集数据周期，都执行以下步骤构建栅格地图，具体步骤如下：

步骤一、采集单智能体的激光雷达生成的点云数据，利用点云数据高程信息进行地面区域分割，去除地面反射点及不感兴趣区域，得到静态目标，所述静态目标即静态障碍物，无需区分其类型，只需要得到它们的位置和形状；

步骤二、采集单智能体的视觉传感器拍摄的图像数据，使用图像检测算法，检测出图像中的行人、机动车及感兴趣区域等动态目标的位置及尺寸信息。

步骤三、将单智能体视觉传感器得到的动态目标数据投射到单智能体激光雷达得到的静态目标数据中；融合形成动静态目标数据；同样方法对同一场景下另一单智能体传感器数据进行融合，并分别构建局部栅格地图。

步骤四、根据多智能体的位置信息，将局部栅格地图扩展成全局栅格地图。

所述激光雷达和视觉传感器对得到的数据进行融合具体方法包括如下步骤：

1)获取激光雷达坐标系和视觉传感器相机坐标系之间的变换关系；

根据摄像机成像模型，首先空间中的一点由世界坐标系转换到摄像机坐标系，然后再将其投影到成像平面(摄像机的CCD)，最后再将成像平面上的数据转换到图像平面(最后生成的图像)。使用标定板对激光雷达及视觉传感器进行标定，可以得到激光雷达坐标系和视觉传感器相机坐标系之间的变换关系：

/>

其中，(x_c，y_c，z_c)表示摄像机坐标系的三维坐标轴,X_c，Y_c为光心图像坐标，Z_c为物体离光心的距离；点云数据为含有3列(x_w，y_w，z_w)的N行数组，(x_w，y_w，z_w)是世界坐标系的三维坐标轴，X_W、Y_W、Z_W代表扫描物体表面点的三维空间位置；R为旋转矩阵，T为平移向量，根据视觉雷达传感器成像原理，以像素为单位的相机坐标系(U,V)和以毫米为单位的图像坐标系(x,y)之间的变换关系为：

其中，f_x＝f/d_x以及f_y＝f/d_y,称为相机在U轴和V轴方向上的尺度因子,U₀、V₀是像平面(image plane)的中心点，又称相机主点坐标；f为焦距。

综合上述公式，激光雷达到图像坐标系的变换关系为：

其中，M为3×4矩阵，称为投影矩阵；M₁完全由f_x、f_y、u₀、v₀决定，这些参数称为摄像机内部参数，内参是相机固有的属性，实际上就是焦距，像元尺寸；M₂完全由摄像机相对于世界坐标系的方位决定，称为摄像机的外部参数，外参表示相机相对于世界坐标系的旋转和平移变换关系；X_W为空间点在世界坐标系下的齐次坐标，与激光雷达点云坐标系一致。

2)对于激光雷达点云数据，利用高程信息，进行地面区域分割，根据车辆位置，去除道路内动态目标，得到静态目标的点云集合。

3)对于摄像机图像数据，利用上述激光雷达到图像坐标系变换关系的逆变换，将检测到的图像中的动态目标及动态目标区域内的像素点投影到激光雷达点云数据上，得到动态目标的点云集合。

4)对上述静态目标的点云集合和动态目标的点云集合进行聚类及归一化处理，计算得到点云概率密度图，生成局部栅格地图。

5)对另一智能体激光雷达及摄像机数据进行同样处理，执行步骤2)-4)，再根据多智能体位置关系，拼接两张局部栅格地图成为全局栅格地图。

所述动态目标主要指道路上的机动车、非机动车和行人，静态目标主要指道路两边楼房、树木及可能存在的路中央隔离带。

有益效果：

与现有技术相比，本发明为多智能体多角度多传感器数据生成全局栅格地图，组合使用多类传感器和运用时序关联的感知技术，可以缩小感知盲区的范围，充分发挥了传感器各自优势，利用视觉传感器丰富了栅格地图的信息，去除激光雷达数据中大量与驾行驶无关的信息，能够给决策端计算机提供信息量小且有效的数据信息。本发明构建的全局栅格地图可以表示当前智能体交通环境信息，例如马路边缘、路口、障碍物类型及障碍物轮廓点，为智能体在当前局部区域安全行驶提供信息，为智能体提供安全可靠、可预测的局部道路区域信息；其为智能体安全行驶提供了一种技术保障。

附图说明

图1是本发明提供的多智能体激光雷达和视觉信息融合构建栅格地图方法的流程框图。

图2是点云数据映射到图像数据。

图3是本发明具体实施时利用图像检测算法得到动态目标位置区域的截图。

图4是本发明具体实施时将动态目标区域映射到点云的示意图。

图5是本发明具体实施时生成的二维连续概率栅格图。

图6是本发明具体实施时生成的概率密度图。

图7是本发明具体实施时生成的二维全局栅格地图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

详见附图1、2，本实施例提供了一种多智能体激光雷达和视觉信息融合构建栅格地图的方法，在确保传感器信息时间同步下，联合标定智能体的激光雷达和视觉传感器，分别对点云数据和图像数据进行处理，融合处理后的信息，得到单智能体静态目标和动态目标，在智能体行进过程中，每个视觉传感器采集图像周期，执行以下步骤构建局部栅格地图，同时，对另一智能体进行同样处理，再根据多智能体位置信息，将局部栅格地图拼接成全局栅格地图，具体步骤如下：

步骤一、利用激光雷达采集激光雷达点云数据，利用点云数据高程信息进行地面区域分割，去除道路区域内的点云数据，得到静态目标，所述静态目标无需区分类型，只需要得到它们的位置和形状，同时该数据还将用于步骤三的数据融合；

以激光雷达安装位置作为雷达坐标系原点O_W，智能体前进方向为Z_W轴，Y_W轴指向智能体上方，X_W-Y_W-Z_W轴满足右手螺旋定则，x_w、y_w、z_w代表扫描物体表面点的三维空间位置，则得到的点云数据的每个点w的坐标在激光雷达坐标系下可表示为(x_w，y_w，z_w)。根据高度信息y_w可分割出地面区域；删除智能体行驶正前方一定范围内的点云，可将动态目标去除，此时的激光点云数据能够体现出智能体周边的静态障碍物信息。具体实施时，当智能体在沿市区道路向正前方行驶，范围可定为50m；沿高速道路正前方行驶，范围可定为100m；智能体拐弯时，范围可定为10m。

步骤二、详见附图3，利用彩色相机采集视觉传感器图像数据，利用图像检测算法得到动态目标，检测出图像中存在的行人、机动车和非机动车动态目标的位置及尺寸信息，并对每一个动态目标标记其类型；图像坐标系的坐标原点为图像左上角，即可得到图像坐标系下如下表的动态目标信息。

表1动态目标信息

步骤三、将步骤二所得动态目标投影到步骤一静态目标点云数据中，得到融合后的动静态目标。由激光雷达到图像坐标系的变换关系看出，主要包含两步关键环节：计算得到动态目标距光心的距离z_c和将动态目标检测框映射到静态环境点云。

动态目标距离光心的距离z_c的计算方法如下：

1)充分发挥激光雷达测距优势，利用点云深度信息求光心到障碍物的距离z_c，根据激光雷达坐标系和相机坐标系转换关系，由点云求出并保存(x_c，y_c，z_c)及其索引。

2)将点云映射到图像坐标系，并将障碍物检测框分割成n×n个分框，得到障碍物检测框每个分框内点云索引值，对检测框每个分框内点云z_c求均值得到n个

值。以障碍物检测框中心所在框z_c为例，得到如下表的信息。

样本编号	1	2	3
				中心点z<sub>c</sub>值	18.65	9.721	12.96
检测框中心所在框z<sub>c</sub>值	18.8027	9.5013	12.54

动态目标检测框映射到静态环境点云方法：

1)根据激光雷达到图像坐标系变换关系的逆运算及上述求出动态目标检测框分框的z_c值，将动态目标投影到激光雷达静态目标点云。

2)由于点云较稀疏，将动态目标检测框内的像素点及对应RGB值投影到激光雷达静态目标点云，此时动态目标信息变为(x,y,z,R,G,B)。

项目	动态目标图像位置坐标	动态目标激光点云坐标系下坐标	对应RGB值
				1	(1114.52，263.72)	(12.1654，-6.1665，-0.1588)	149 164 169
2	(1114.52，384.84)	(11.6880，-5.9759，-0.1528)	165 169 181
				3	(1281.63，263.72)	(39.6648，-19.6380，-1.0299)	158 168 170
4	(1281.63，384.84)	(12.1277，-6.1235，-0.2371)	162 164 161

步骤四、详见附图5，忽略单智能体的垂向运动，YW轴既指向智能体上方，也垂直于路面，故将(x_w，y_w，z_w)投影到二维的路面平面上为(x_w，z_w)。栅格图的范围为以O_W为中心向前后左右各25米的区域，将三维点云数据投影到二维平面上，形成二维连续概率栅格图。

步骤五、详见附图6，将融合后的动静态点云归一化处理，并计算生成概率密度图。

连续型随机变量的概率密度函数是描述这个随机变量的输出值，在某个确定的取值点附近的可能性的函数，用核函数拟合数据点,画出相应的核函数概率密度图，核密度估计 (Kernel density estimation)，是一种用于估计概率密度函数的非参数方法，就是采用平滑的峰值函数(“核”)来拟合观察到的数据点，从而对真实的概率分布曲线进行模拟。

本发明具体使用k-means算法对数据进行聚类，聚类的目标是使同一类对象的相似度尽可能地大；不同类对象之间的相似度尽可能地小。由于点云集合的疏密程度不同，数据的差异性，需要进行归一化处理，归一化的具体作用是归纳统一样本的统计分布性，归一化在 0～1之间是统计的概率分布，使得点云集合均值接近于0或与其均方差相比很小。

在占据栅格地图中，对于一个点，我们用p(s＝1)来表示它是栅格未被占有状态的概率，用p(s＝0)来表示它是栅格被占有状态的概率，栅格未被占有状态的概率与栅格被占有状态的概率两者的和为1：p(s＝1)+p(s＝0)＝1。将两者的比值来作为栅格地图中点的状态：

对于一个点，对新增的一个测量值Odd(s)记为z～{0,1}，之后我们需要更新它的状态。假设测量值来之前，该点的状态为Odd(s)，我们要更新它为:

这种表达方式类似于条件概率，表示在z发生的条件下s的状态。

根据贝叶斯公式，我们有：

其中，p(s＝1|z)表示在z发生的条件下s＝1的状态(栅格未被占有状态)的概率；p(s＝0|z)表示z发生的条件下s＝0的状态(栅格被占有状态)的概率，p(z|s＝1)表示栅格未被占有条件下z的概率，p(z|s＝0)表示栅格被占有条件下z的概率，p(z)表示z的概率，p(s＝1)表示栅格未被占有的概率，p(s＝0)表示栅格被占有的概率；

代入之后，我们得

我们对两边取对数得：

仅剩测量量：

我们称这个比值为测量值的模型(Measurement Model)，标记为/>

测量值的模型只有两种，栅格未被占有的标记为lofree，栅格被占有的标记为looccu：

而且，测量值都是定值。

这样，如果我们用logOdd(s)来表示位置s的状态S的话，我们的更新规则就进一步简化成了：s⁺＝s^-+lomeas。其中s⁺和s^-分别表示测量值之后和之前s的状态。

另外，在没有任何测量值的初始状态下，一个点的初始状态S_init。

根据p(s＝1)和p(s＝0)的值，即得到点云概率密度图。

步骤七、详见附图7，两单智能体均检测出障碍物区域为红色，两单智能体均没有障碍物用白色，1号智能体单独检测出障碍物为黄色，2号智能体单独检测出障碍物为蓝色。根据概率密度图，构建智能体局部栅格地图；并根据智能体定位信息(如GPS)，找出多智能体位置关系，将局部栅格地图拼接成全局栅格地图。

上述参照实施例对该一种多智能体激光雷达和视觉信息融合构建栅格地图的方法进行的详细描述，是说明性的而不是限定性的，可按照所限定范围列举出若干个实施例，因此在不脱离本发明总体构思下的变化和修改，应属本发明的保护范围之内。

Claims

1.一种基于多智能体激光雷达和视觉信息融合的地图生成方法，每个单智能体分别联合标定单智能体的激光雷达和视觉传感器，并识别单智能体的视觉传感器时间一致下的静态和动态目标；根据单智能体的GPS信息，将多智能体的检测结果进行融合；

在单智能体行进过程中，每个单智能体的视觉传感器在采集数据周期中，均执行以下步骤构建栅格地图：

步骤一、采集单智能体的激光雷达生成的点云数据，利用点云数据高程信息进行地面区域分割，去除地面反射点及不感兴趣区域，得到静态目标数据，包括静态目标的位置和形状，所述静态目标即静态障碍物；

步骤二、采集单智能体的视觉传感器拍摄的图像数据，使用图像检测算法，检测出图像中的动态目标数据，包括动态目标的位置及尺寸信息；

步骤三、将单智能体视觉传感器得到的动态目标数据投射到单智能体激光雷达得到的静态目标数据中，融合形成动静态目标数据；

采用同样方法对同一场景下另一单智能体传感器数据进行融合，并分别构建得到局部栅格地图；

对得到的数据进行融合的方法具体包括如下步骤：

1)获取激光雷达坐标系和视觉传感器相机坐标系之间的变换关系：首先将空间中的一点由世界坐标系转换到摄像机坐标系；然后再投影到成像平面，最后再将成像平面上的数据转换到图像平面；

2)利用激光雷达点云数据的高程信息进行地面区域分割；根据车辆位置，去除道路内的动态目标，得到静态目标的点云集合；

3)对于摄像机图像数据，利用激光雷达到图像坐标系变换关系的逆变换，将检测到的图像中的动态目标及动态目标区域内的像素点投影到激光雷达点云数据上，得到动态目标的点云集合；

4)对静态目标的点云集合和动态目标的点云集合进行聚类和归一化处理，计算得到点云概率密度图，生成局部栅格地图；

计算得到点云概率密度图，具体包括如下过程：

对于栅格地图中的一个点，p(s＝1)表示该点是栅格未被占有状态的概率，p(s＝0)表示该点是栅格被占有状态的概率；

栅格未被占有状态的概率与栅格被占有状态的概率两者的和为1：p(s＝1)+p(s＝0)＝1；

将两者的比值作为栅格地图中点的状态，表示为：

对于栅格地图中的一个点，该点的状态为Odd(s)；新增一个测量值z～{0,1}，则更新该点的状态为:

表示在z发生的条件下s的状态；

根据贝叶斯公式，得到：

对两边取对数得：

为测量值的模型，记/>

测量值为定值；

用logOdd(s)表示位置s的状态S，则：s⁺＝s^-+lomeas；其中，s⁺和s^-分别表示测量值之后和之前s的状态；

在没有任何测量值的初始状态下，一个点的初始状态S_init表示为：

即得到点云概率密度图；

5)对另一智能体激光雷达及摄像机数据进行同样处理，即执行步骤2)～4)；再根据多智能体位置关系，将两张局部栅格地图拼接成为全局栅格地图；

步骤四、根据多智能体的位置信息，将局部栅格地图扩展成全局栅格地图；

通过上述步骤，即实现基于多智能体激光雷达和视觉信息融合的栅格地图生成。

2.如权利要求1所述基于多智能体激光雷达和视觉信息融合的地图生成方法，其特征是，步骤1)获取激光雷达坐标系和视觉传感器相机坐标系之间的变换关系，具体包括如下步骤：

使用标定板对激光雷达及视觉传感器进行标定，得到激光雷达坐标系和视觉传感器相机坐标系之间的变换关系，表示为：

其中，(x_c，y_c，z_c)表示摄像机坐标系的三维坐标轴；X_c，Y_c为光心图像坐标，Z_c为物体离光心的距离；点云数据为含有3列(x_w，y_w，z_w)的N行数组；(x_w，y_w，z_w)是世界坐标系的三维坐标轴；X_W、Y_W、Z_W代表扫描物体表面点的三维空间位置；R为旋转矩阵，T为平移向量；

以像素为单位的相机坐标系(U,V)和以毫米为单位的图像坐标系(x,y)之间的变换关系表示为：

其中，f_x＝f/dx以及f_y＝f/dy,称为相机在U轴和V轴方向上的尺度因子,U₀、V₀是像平面的中心点，又称相机主点坐标；f为焦距；

激光雷达到图像坐标系的变换关系表示为：

其中，M为投影矩阵，为3×4矩阵；M₁完全由f_x、f_y、u₀、v₀决定；参数f_x、f_y、u₀、v₀均为摄像机内部参数；M₂为摄像机的外部参数，完全由摄像机相对于世界坐标系的方位决定，表示相机相对于世界坐标系的旋转和平移变换关系；X_W为空间点在世界坐标系下的齐次坐标，与激光雷达点云坐标系一致。

3.如权利要求1所述基于多智能体激光雷达和视觉信息融合的地图生成方法，其特征是，所述动态目标包括道路上的机动车、非机动车、行人和感兴趣区域；和/或，所述静态目标包括道路两边的楼房、树木及路中央隔离带。

4.如权利要求1所述基于多智能体激光雷达和视觉信息融合的地图生成方法，其特征是，步骤4)具体采用k-means算法进行聚类。

5.如权利要求1所述基于多智能体激光雷达和视觉信息融合的地图生成方法，其特征是，测量值的模型有两种：栅格未被占有的测量值的模型和栅格被占有的的测量值的模型，表示为：

其中，lofree为栅格未被占有的测量值的模型；looccu为栅格被占有的的测量值的模型。