CN106599108B - 一种三维环境中多模态环境地图构建方法 - Google Patents

Abstract

本发明公开了一种三维环境中多模态环境地图构建方法。以建图模式多次运行无人车，每一次运行得到不同的关键帧坐标序列。基于欧式距离计算不同关键帧坐标间的地理接近性，以此识别不同次运行关键帧序列的交叉点，这些交叉点作为拓扑地图的节点，将多条轨迹联结在一起，构成一整个地图。在此拓扑地图上执行基于启发式图搜索算法的路径规划，即可获得路径交叉点序列，用这个路径交叉点序列可以索引得到原始的稠密关键帧坐标，这些坐标作为轨迹规划的结果，输入到无人车控制执行系统，实现无人车的位置跟踪。

Description

一种三维环境中多模态环境地图构建方法

技术领域

本发明属于移动机器人自主导航技术领域，特别是针对三维环境的多模态环境地图构建方法。

背景技术

地图构建技术是移动机器人导航系统的核心功能，构建地图是为了后续的重用，如果仅仅执行单次作业，那么构建地图的意义并不明显。地图是对环境的描述，是一种建模行为，其意义在于为机器人的行为决策提供必要的描述。传统的地图形式有度量地图，拓扑地图及混合地图。度量地图能精确用于导航，但表达不够紧凑，会占用计算机系统大量的内存资源，拓扑地图形式简单，易于存储，但是不足以用于导航。混合地图的计算则较为复杂。

发明内容

本发明所要解决的技术问题是提供一种三维环境中多模态环境地图构建方法，以提供同时支持自定位、路径规划和控制的地图数据系统。为此，本发明提供以下技术方案：

一种三维环境中多模态环境地图构建方法，包括以下步骤：

步骤1，无人车为随机探索环境的自动运行模式，无人车的双目相机采集环境图像，建立稀疏视觉特征地图，该地图的参考坐标系称为特征地图坐标系，坐标原点是无人车启动时相机的中心，正前方为z轴正方向，正右方为x轴正方向，并以二进制文件形式将特征地图的数据存储至车载计算机本地磁盘；

步骤2，从车载计算机的本地磁盘读取地图数据文件至内存，无人车设定为定位模式，定位成功后，接收用户设置的目标位置坐标信息；有位置关系变化后，反复尝试重定位；

步骤3，步骤2中每次成功定位后，稠密立体匹配双目相机的左右图像，获得三维点云序列；

步骤4，从三维点云中拟合得到无人车的底盘的平面模型；

从三维点云序列中随机选取3个三维点；检测3个三维点是否共线，若共线，则重新选取；估计出一个平面方程其中，是平面方程的参数，x、y、z是三维点云的坐标；

统计点云序列中有多少三维点符合这一平面方程，记这个数量为K；重复执行这个统计过程L次，考察选择K值最大的一个平面方程，若其K值大于某一事先设定的阈值K_thresh，那么，这个方程就是地盘平面模型Π：ax+by+cz+d＝0，否则，认为本次计算失败，沿用上一次计算的结果作为当前的平面模型Π；

步骤5，将三维点云序列变换至特征地图坐标系，并根据点云序列的高度属性，利用八叉树栅格地图(Octomap)方法，构建三维栅格地图；

5-1，点云变换：

采用如下齐次变换公式，将点云坐标从相机坐标系变换至至特征地图坐标系(如附图中图2)；

其中，^AP表示点云P在特征地图坐标系下的坐标，^BP表示点云P在相机坐标系下的坐标，表示特征地图坐标系到相机坐标系的变换矩阵；

5-2，基于步骤4中的平面模型Π划分稠密点云为地面部分和高于地面部分，高于地面部分视为障碍物点云，操作如下：

获取三维点云序列中的一个点云P，计算P到平面Π的距离其中，P_x、P_y、P_z是点P的坐标；

若d≤d_thresh，将P加入可通行区域点云集合P_free，其中，d_thresh是事先设定的距离阈值。

5-3，基于5-2中点云高度属性的判断，即可通行区域点云集合P_free对应地面部分，其余点云对应高于地面的障碍部分，利用八叉树栅格地图方法(如附图中图3)，以点云集合{^AP}构建三维栅格地图；

步骤6，对步骤5中的三维栅格地图进行降维处理，之后进行离散化，得到二维栅格地图，具体步骤如下：

6-1，根据双目相机的高度，将高于双目相机的点云在三维栅格地图中对应的节点剔除，与此同时，将三维栅格单元的坐标值投影到特征地图坐标系的x-z平面；

6-2，对经过降维的三维栅格地图，进行栅格单元中心点的坐标的离散化处理，得到二维栅格地图，包括如下步骤：

确定二维栅格地图中坐标x的取值范围：x_min，x_max；

确定二维栅格地图中坐标z的取值范围：z_min，z_max；

输入二维栅格地图节点中心的连续坐标值：x_c，z_c；

圆整至邻近的整数值：x_r，z_r；

利用坐标范围映射数组索引：x_d＝x_min+x_r，z_d＝z_min+z_r。

x_d、z_d即是离散化的二维栅格地图节点的坐标值。

在上述技术方案的基础上，本发明还可以采用一下进一步的技术方案：

将可用于无人车自定位的特征地图与可用于无人车全局路径规划的栅格地图进行关联，基于双目相机图像构建稀疏视觉特征地图，利用双目相机获取的三维点云构建栅格地图。

通过判断无人车的底盘所在地平面模型确定点云的可通行性，以利用八叉树栅格地图方法快速构建和更新三维栅格地图。

由于采用本发明的技术方案，本发明的有益效果为：本发明提出的多模态环境地图本质上是一种松耦合的基于映射关系的地图数据结构。既能用于精确导航，又易于表达，实质上是用稀疏图映射稠密度量图。这种多模态的地图可用于无人车自定位、路径规划和控制。

附图说明

图1是本发明车辆、底盘、地面示意图。

图2是本发明坐标变换示意图。

图3是本发明三维空间结构示意图。

图4是本发明八叉树数据结构示意图。

图5是本发明目相机立体成像过程示意图。

图6本发明降维后的二维栅格地图示意图。

图7是本发明二维栅格地图的离散化流程示意图。

图8是本发明二维栅格地图的离散化结果示意图。

具体实施方式

为了更好地理解本发明的技术方案，以下结合附图作进一步描述。一种三维环境中多模态环境地图构建方法，包括以下步骤：

步骤1，无人车为随机探索环境的自动运行模式，无人车的双目相机采集环境图像，建立稀疏视觉特征地图，该地图的参考坐标系称为特征地图坐标系，坐标原点是无人车启动时相机的中心，正前方为z轴正方向，正右方为x轴正方向，并以二进制文件形式将特征地图的数据存储至车载计算机本地磁盘。

为使无人车尽可能采集到尽可能丰富的环境信息，采用一种随机游走的运动策略，即任意时刻，使无人车尽可能朝向空旷的地带运动，这一方面保证了车辆自动运行的安全，同时，也能采集到足够多的环境特征点。

采取如下步骤计算随机游走的运动方向：

1)激光雷达获取点云

从水平安装的2D激光雷达获取点云序列{(θ_i,d_i)|i＝1,2,…,M}。

激光雷达以预设的角度步长每次扫描周围一定角度范围、一定距离范围，当光线照射至物体时，即可返回该处的角度和距离信息，这种角度和距离信息即构成一个点云。如此持续扫描，就可获得连续的二维点云信息。

2)定义安全行驶方向矢量

其中，r_thresh是预设的避障半径。

3)计算安全行驶方向

计算x方向的矢量和：

计算y放向的矢量和：

x方向矢量和归一化：

y方向矢量和归一化：

计算行驶方向角：

从而，得到安全行驶方向θ_steer，以此作为角度操纵控制量发送给执行器，即可驱动无人车驶向安全方向。

建立稀疏视觉特征地图的过程。

1)特征点的计算和位姿跟踪

从图像中抽取ORB特征点：

首先，按如下步骤提取FAST角点；

然后，利用灰度质心法为FAST角点添加方向信息；

在提取了有方向信息的FAST角点后，对每个特征点计算ORB描述子，采用BRIEF算法进行描述；

匹配前后帧的特征点，在图像I_t中提取到特征点在图像I_t+1中提取到特征点采用快速近似最近邻(FLANN)算法进行匹配；根据这些匹配对计算两帧间的位姿变换矩阵；

2)基于图优化方法优化全局误差

3)基于词包模型实现闭环检测

要做闭环检测，以校正无人车位姿和地图的累积误差，这里，采用基于表面特征的场景匹配算法，即词包模型。词包模型的原理是视觉词典和词汇统计直方图向量的余弦距离比较，余弦距离公式为

4)地图数据的序列化和反序列化。

为实现地图重用，以便每次重新运行无人车时不必重新建图，或者，为了在原有地图的基础上扩充新区域的地图，必须对内存中的地图数据进行序列化和反序列化。采用boost函数库的serialization类库进行序列化和反序列化操作。序列化的对象是关键帧数据及关键帧对应的特征点云数据。

步骤2，从车载计算机的本地磁盘读取地图数据文件至内存，无人车设定为定位模式，定位成功后，接收用户设置的目标位置坐标信息；有位置关系变化后，反复尝试重定位；

步骤3，步骤2中每次成功定位后，稠密立体匹配双目相机的左右图像，获得三维点云序列。双目立体视觉三维测量基于视差原理。如图5所示，基线长度B是左右相机投影中心连线的距离。双目相机的焦距为f。左右相机同一时刻拍摄空间物体同一点P(x_p,y_p,z_p)，并分别在左右图像上生成像点P_l(X_l,Y_l)和P_r(X_r,Y_r)。

从而，点P的图像坐标Y相同，即有Y_l＝Y_r＝Y。则由三角形相似原理有如下关系：

从而，可以计算视差：

Disp＝X_l-X_r

由此，可计算点P在相机坐标系下的三维坐标为：

因此，只要能得到左右相机图像平面上的匹配点对，即可计算出匹配点对应的空间点的三维坐标。

为了保证实时控制系统的计算性能，采用快速的绝对差相关算法(SSD)计算图相对的匹配点，计算公式如下：

其中，

d_max和d_min分别时最小和最大视差；

m是方形模板的尺寸，单位是像素；

I_left和I_right分别是双目相机的左右图像。

从而，只要计算出满足视差范围容许值的绝对差最小的匹配对，就可以认为匹配成功。

步骤4，从三维点云中拟合得到无人车的底盘的平面模型；

从三维点云序列中随机选取3个三维点；检测3个三维点是否共线，若共线，则重新选取；估计出一个平面方程其中，是平面方程的参数，x、y、z是三维点云的坐标；

统计点云序列中有多少三维点符合这一平面方程，记这个数量为K；重复执行这个统计过程L次，考察选择K值最大的一个平面方程，若其K值大于某一事先设定的阈值K_thresh，那么，这个方程就是地盘平面模型Π：ax+by+cz+d＝0，否则，认为本次计算失败，沿用上一次计算的结果作为当前的平面模型Π；

步骤5，将三维点云序列变换至特征地图坐标系，并根据点云序列的高度属性，利用八叉树栅格地图(Octomap)方法，构建三维栅格地图；

5-1，点云变换：

采用如下齐次变换公式，将点云坐标从相机坐标系变换至至特征地图坐标系如图2所示；

其中，^AP表示点云P在特征地图坐标系下的坐标，^BP表示点云P在相机坐标系下的坐标，表示特征地图坐标系到相机坐标系的变换矩阵；

5-2，基于步骤4中的平面模型Π划分稠密点云为地面部分和高于地面部分，高于地面部分视为障碍物点云，操作如下：

获取三维点云序列中的一个点云P，计算P到平面Π的距离其中，P_x、P_y、P_z是点P的坐标；

若d≤d_thresh，将P加入可通行区域点云集合P_free，其中，d_thresh是事先设定的距离阈值。

5-3，基于5-2中点云高度属性的判断，即可通行区域点云集合P_free对应地面部分，其余点云对应高于地面的障碍部分，利用八叉树栅格地图方法，如图3和图4所示，以点云集合{^AP}构建三维栅格地图。

从点云中拟合地面；计算每个点到拟合的平面距离；若计算的距离小于某个阈值，则判定为非占据，否则，判定为占据；

步骤6，对步骤5中的三维栅格地图进行降维处理，之后进行离散化，得到二维栅格地图，具体步骤如下：

6-1，根据双目相机的高度，将高于双目相机的点云在三维栅格地图中对应的节点剔除，与此同时，将三维栅格单元的坐标值投影到特征地图坐标系的x-z平面；

6-2，对经过降维的三维栅格地图，如图6所示，进行栅格单元中心点的坐标的离散化处理，如图7所示，得到二维栅格地图，如图8所示，包括如下步骤：

确定二维栅格地图中坐标x的取值范围：x_min，x_max；

确定二维栅格地图中坐标z的取值范围：z_min，z_max；

输入二维栅格地图节点中心的连续坐标值：x_c，z_c；

圆整至邻近的整数值：x_r，z_r；

利用坐标范围映射数组索引：x_d＝x_min+x_r，z_d＝z_min+z_r。

x_d、z_d即是离散化的二维栅格地图节点的坐标值。

在上述技术方案的基础上，本发明还可以采用一下进一步的技术方案：

将可用于无人车自定位的特征地图与可用于无人车全局路径规划的栅格地图进行关联，基于双目相机图像构建稀疏视觉特征地图，利用双目相机获取的三维点云构建栅格地图。

通过判断无人车的底盘所在地平面模型确定点云的可通行性，以利用八叉树栅格地图方法快速构建和更新三维栅格地图。

Claims (3)

1.一种三维环境中多模态环境地图构建方法，其特征在于，包括以下步骤：

步骤1，设置无人车为随机探索环境的自动运行模式，打开无人车的双目相机，采集环境图像，建立稀疏视觉特征地图，该地图的参考坐标系称为特征地图坐标系，坐标原点是无人车启动时相机的中心，正前方为z轴正方向，正右方为x轴正方向，并以二进制文件形式将特征地图的数据存储至车载计算机本地磁盘；

步骤2，从车载计算机的本地磁盘读取地图数据文件至内存，启动无人车，设置为定位模式，定位成功后，接收用户设置的目标位置坐标信息；有位置关系变化后，反复尝试重定位；

步骤3，成功定位后，稠密立体匹配双目相机的左右图像，获得三维点云序列；

步骤4，从三维点云中拟合得到无人车的底盘平面模型；

从三维点云序列中随机选取3个三维点；检测3个三维点是否共线，若共线，则重新选取；估计出一个平面方程 其中，是平面方程的参数，x、y、z是三维点云的坐标；

统计点云序列中有多少三维点符合这一平面方程，记这个数量为K；重复执行这个统计过程L次，考察选择K值最大的一个平面方程，若其K值大于某一事先设定的阈值K_thresh，那么，这个方程就是底盘平面模型Π：ax+by+cz+d＝0，否则，认为本次计算失败，沿用上一次计算的结果作为当前的底盘平面模型Π；

步骤5，将三维点云序列变换至特征地图坐标系，并根据点云序列的高度属性，利用八叉树栅格地图方法，构建三维栅格地图；

5-1，点云变换：

采用如下齐次变换公式，将点云坐标从相机坐标系变换至特征地图坐标系，其中，相机坐标系是指坐标原点定义在相机光心的右手坐标系，特征地图坐标系是指坐标原点定义在无人车启动时的相机光心的右手坐标系；

其中，^AP表示点云P在特征地图坐标系下的坐标，^BP表示点云P在相机坐标系下的坐标，表示特征地图坐标系到相机坐标系的变换矩阵；

5-2，基于步骤4中的底盘平面模型Π划分稠密点云为地面部分和高于地面部分，高于地面部分视为障碍物点云，操作如下：

获取三维点云序列中的一个点云P，计算P到底盘平面模型Π的距离其中，P_x、P_y、P_z是点P的坐标；

若d≤d_thresh，将P加入可通行区域点云集合P_free，其中，d_thresh是事先设定的距离阈值；

5-3，基于5-2中求得的点云P到底盘平面模型Π的距离的判断，即可通行区域点云集合P_free对应地面部分，其余点云对应高于地面的障碍部分，利用八叉树栅格地图方法，以点云集合{^AP}构建三维栅格地图；

步骤6，对步骤5中的三维栅格地图进行降维处理，之后进行离散化，得到二维栅格地图，具体步骤如下：

6-1，根据双目相机的高度，将高于双目相机的点云在三维栅格地图中对应的节点剔除，与此同时，将三维栅格单元的坐标值投影到特征地图坐标系的x-z平面；

6-2，对经过降维的三维栅格地图，进行栅格单元中心点的坐标的离散化处理，得到二维栅格地图，包括如下步骤：

确定二维栅格地图中坐标x的取值范围：x_min，x_max；

确定二维栅格地图中坐标z的取值范围：z_min，z_max；

输入二维栅格地图节点中心的连续坐标值：x_c，z_c；

圆整至邻近的整数值：x_r，z_r；

利用坐标范围映射数组索引：x_d＝x_min+x_r，z_d＝z_min+z_r；x_d、z_d即是离散化的二维栅格地图节点的坐标值。

2.如权利要求 1所述的一种三维环境中多模态环境地图构建方法，其特征在于：将可用于无人车自定位的特征地图与可用于无人车全局路径规划的栅格地图进行关联，基于双目相机图像构建稀疏视觉特征地图，利用双目相机获取的三维点云构建栅格地图。

3.如权利要求 1所述的一种三维环境中多模态环境地图构建方法，其特征在于：通过判断无人车的底盘所在地平面模型确定点云的可通行性，以利用八叉树栅格地图方法快速构建和更新三维栅格地图。