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

摘要

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

主权项

一种三维环境中多模态环境地图构建方法，其特征在于，包括以下步骤：步骤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)；${}^{A}P={{}_B{}^{A}T}^{B}P$其中，^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即是离散化的二维栅格地图节点的坐标值。