CN116202509A - 一种面向室内多层建筑的可通行地图生成方法 - Google Patents

Abstract

本发明公开了一种面向室内多层建筑的可通行地图生成方法，包括以下步骤：将激光雷达、惯性传感器和轮速里程计安装到无人车上；基于激光雷达、惯性传感器和轮速里程计建立坐标系，并基于所述坐标系获得误差状态；构建测量模型，通过测量模型对多层建筑进行测量，获得点云数据和无人车位姿数据；通过获得的误差状态对无人车位姿数据进行数据更新，获得更新位姿数据；根据所述点云数据和更新位姿数据生成可通行地图。本方法采用紧耦合多传感器融合的方式进行定位，可以有效提高系统的鲁棒性与定位精度，针对如长廊，隧道等的结构退化环境，利用紧耦合多传感器融合的方式可以有效解决环境退化问题。

Description

一种面向室内多层建筑的可通行地图生成方法

技术领域

本发明属于路径规划技术领域，尤其涉及一种面向室内多层建筑的可通行地图生成方法。

背景技术

传统三维点云地图通过定位模块给出的位姿将实时点云或者关键帧投影至全局坐标系，这样构建出来的室内场景下点云地图可读性较差，地图使用者难以从点云地图中获取关键的信息。二维地图虽然结构清晰，可读性强，但是当无人车运行在有坡度地面或者多楼层环境时，无法使用。而且传统激光雷达与IMU/轮速里程计融合首先通过激光雷达的匹配过程获取一个初步位姿，然后将获得的位姿与IMU、里程计进行融合。此种方式为松耦合融合，当激光雷达处于退化环境时，此时点云匹配错误，松耦合融合会继续使用错误的匹配结果进行融合，此时定位精度会不可避免的降低。

发明内容

为解决上述技术问题，本发明提出了一种面向室内多层建筑的可通行地图生成方法，以解决退化环境下定位失效的问题。

为实现上述目的，本发明提供了一种面向室内多层建筑的可通行地图生成方法，包括以下步骤：

将激光雷达、惯性传感器和轮速里程计安装到无人车上；

基于激光雷达、惯性传感器和轮速里程计建立坐标系，并基于所述坐标系获得误差状态；

构建测量模型，通过测量模型对多层建筑进行测量，获得点云数据和无人车位姿数据；

通过获得的误差状态对无人车位姿数据进行数据更新，获得更新位姿数据；

根据所述点云数据和更新位姿数据生成可通行地图。

优选地，所述建立坐标系的方法包括：

建立激光雷达坐标系、惯性传感器坐标系、轮速里程计坐标系和全局坐标系，并将所述激光雷达坐标系和轮速里程计坐标系转换到惯性传感器坐标系中；

激光雷达坐标系的坐标系原点位于激光雷达扫描中心，X轴指向激光雷达正前向扫描方向，Z轴沿着激光雷达旋转轴竖直向上，Y轴指向激光雷达左向；

惯性传感器坐标系的坐标系原点位于惯性传感器中心，X轴指向无人车正前方向，Z轴竖直向上，Y轴指向无人车左向；

轮速里程计坐标系的坐标系原点位于无人车中心，X轴指向无人车正前方向，Z轴竖直向上，Y轴指向无人车左向；

全局坐标系为开始测量时刻的惯性传感器坐标系。

优选地，所述获得误差状态的方法包括：

通过陀螺仪的输出和加速度计的输出获得系统输入；

通过陀螺仪与加速度计的量测噪声和陀螺仪与加速度计的随机游走过程获得系统噪声；

基于所述系统输入和系统噪声获取位姿变换关系；

通过位姿变换关系推导得出误差量的关系，并基于误差量的关系得到误差状态。

优选地，所述构建测量模型的方法包括构建轮速里程计量测模型和构建激光雷达量测模型；

所述构建轮速里程计量测模型的方法包括：通过惯性传感器坐标系在全局坐标系下的速度得到轮速里程计的量测信息，根据所述量测信息获得观测值，基于所述观测值获得轮速里程计的观测矩阵，所述观测矩阵结合量测噪声得到轮速里程计的观测方程，基于轮速里程计的观测方程构建轮速里程计量测模型。

优选地，所述构建激光雷达量测模型的方法包括：

通过激光雷达对多层建筑进行扫描，获得三维点云，通过三维点云获取特征点集，所述特征点集结合激光雷达的观测值获得激光雷达的观测方程，基于激光雷达的观测方程构建激光雷达量测模型。

优选地，所述生成可通行地图的过程包括：

根据点云数据进行可通行区域检测获得可通行区域，可通行区域结合更新位姿数据构建可通行地图，可通行区域检测包括地面可通行区域检测和楼梯检测；

所述地面可通行区域检测的过程包括：根据激光雷达的安装高度对点云数据进行裁剪处理，获得裁剪后的点云数据，以俯视图的视角将点云数据进行扇形分割，分割角度为激光雷达水平分辨率，将区域内的点由直角坐标系转换成柱坐标系描述的数据结构，对同一分割角的射线按照半径大小进行排序得到顺序数据集，比较射线相邻三点获得相邻三点的坡度，根据坡度判断检测区域是否为地面可通行区域。

优选地，所述楼梯检测的过程包括：

筛选出非地面可通行区域数据集，将非地面可通行区域数据集由柱坐标系转换为直角坐标系得到点云集合，基于楼梯的踢面、踏面和楼梯宽度对点云集合进行楼梯平面查找，得到初始楼梯平面，再通过直线拟合对初始楼梯平面进行筛选，得到楼梯区域。

优选地，所述生成可通行地图的过程包括：

基于octomap构建栅格地图，对可通行区域点云进行八叉树地图光线追踪，得到全局可通行区域栅格地图中新增的栅格与消失的栅格，新增栅格与消失栅格结合无人车在全局系下的位姿生成可通行地图。

与现有技术相比，本发明具有如下优点和技术效果：

本发明所述的面向室内多层建筑的可通行地图生成方法，本方法采用紧耦合的方式通过三种传感器的组合测量有效的解决了环境退化后精确度降低的问题，并且通过八叉树地图光线追踪方法进行地图更新，有效的减少了数据地图的数据存储量。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明实施例的可通行地图生成方法流程图；

图2为本发明实施例的传感器的坐标系与位置图；

图3为本发明实施例的扇形划分示意图；

图4为本发明实施例的楼梯特征示意图；

图5为本发明实施例的楼梯点云投影示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

实施例1

如图1所示，本发明提出了一种面向室内多层建筑的可通行地图生成方法，包括以下步骤：

将激光雷达、惯性传感器和轮速里程计安装到无人车上；

基于激光雷达、惯性传感器和轮速里程计建立坐标系，并基于所述坐标系获得误差状态；

构建测量模型，通过测量模型对多层建筑进行测量，获得点云数据和无人车位姿数据；

通过获得的误差状态对无人车位姿数据进行数据更新，获得更新位姿数据；

根据所述点云数据和更新位姿数据生成可通行地图。

进一步的优化方案，构建的无人车坐标系如图2所示，

其中t时刻激光雷达坐标系F_L,t定义如下：坐标系原点位于激光雷达扫描中心，X轴指向激光雷达正前向扫描方向，Z轴沿着激光雷达旋转轴竖直向上，Y轴指向激光雷达左向，X、Y、Z轴符合右手定则，惯性传感器(IMU)输出当前时刻其自身坐标系的角速度(陀螺仪)和比力(加速度计输出)，设在t时刻陀螺仪输出ω_m,t，加速度计输出为a_m,t，所在坐标系为t时刻IMU系F_I,t，轮速里程计输出当前车体的前向运动速度，一般坐标系F_O,t位于车体的中心位置，t时刻轮速里程计的输出为v_odo,t，t时刻无人车的前向速度通常表示为s_ov_odo,t，其中s_o为轮速里程计的标度因数，全局坐标系F_G定义为0时刻的IMU坐标系F_I,0。

进一步的优化方案，各传感器之间的关系如下：

由于各种传感器安装位置不同，其所在坐标系之间并不完全重合，设t时刻各种传感器的坐标系与位置如图2所示，本文所有的数据处理均在IMU坐标系下。

设轮速里程计和激光雷达与IMU之间的外参分别为

与

分别表示两种传感器所在坐标系到IMU系下的转换矩阵。

其中

分别表示轮速里程计系到IMU系的旋转矩阵与平移向量，

分别表示激光雷达系到IMU系的旋转矩阵与平移向量。

当在t时刻获得轮速里程计的速度量测s_ov_odo,t时，此时转换至IMU系下的速度为

可分为平动速度

和转动速度

ω_t表示t时刻的角速度，

表示三维向量ω_t的反对称矩阵若设ω_t＝[ω_tx ω_tyω_tz]^T，则

所以

当在t时刻获得激光雷达特征点

时，将其转换至IMU坐标系下得到

进一步的优化方案，对运算符号进行如下定义：

设M为SO(3)流形空间，设其对应的正切空间为T，则T即为三维反对称阵(或三维向量)构成的空间，设三维向量空间为R³。定义

和

操作符如下：

其中，R∈M，r∈T，

Exp是由罗德里格斯公式定义的从正切空间T到SO(3)的指数映射，

ln(·)为Exp(·)的逆映射。

进一步的优化方案，误差状态的计算方法如下：

定义有如下状态量

与

为IMU系相对于全局坐标系的旋转矩阵、平移向量与三维速度。b_ω、b_a分别为陀螺仪和加速度计的零偏。

设系统的输入为u，则

其中ω_m为陀螺仪的输出，a_m为加速度计的输出。

设系统的噪声均为高斯过程的白噪声，则系统的噪声建模如下：

其中n_ω、n_a为陀螺仪与加速度计的量测噪声，n_bω、n_ba为陀螺仪与加速度计的随机游走过程。

由IMU的运动模型，设i时刻的系统输入与噪声分别为u_i与w_i，则从i时刻到i+1时刻的位姿变换关系为：

其中，Δt_i＝t_i+1-t_i为i时刻到i+1时刻的时间步长，

本文选取状态的误差量

作为卡尔曼滤波的估计量，则

定义如下：

其中

表示IMU坐标系的角度误差量，

分别表示各状态量的误差量，其值为状态量减去真值。

根据i时刻与i+1时刻x的关系，推导两个时刻误差量的关系，有如下步骤：

首先将过程噪声w_i设为0，利用i时刻的预测值

预测i+1时刻的预测值

之后根据i时刻的状态最佳估计值x_i，推导出i+1时刻的状态最佳估计值x_i+1

则i+1时刻的误差状态可表示为：

其中F_x与F_w的推导过程如下：

首先根据式18可知

同理可得：

将上面两式非线性部分进行线性化处理得到：

其中，J_r为右乘BCH近似雅可比矩阵，

此时，

可表示为

此时即可得到F_x与F_w分别为：

设Q＝ww^T为噪声w的协方差矩阵，则误差状态的协方差矩阵P_i+1为：

P_i+1＝F_xP_iF_x ^T+F_wQF_wT 31

进一步的优化方案，轮速里程计量测模型构造方法如下：”

设i+1时刻轮速里程计的输出的量测信息为s_ov_odo,i+1，由式可知，该时刻IMU坐标系在全局坐标系下的速度

为:

则s_ov_odo,i+1用如下状态量表示：

用误差状态量表示各个状态量，

所以s_ov_odo,i+1可表示为：

将s_ov_odo,i+1线性化可得：

令观测值

为：

则轮速里程计的观测矩阵

为：

设轮速里程计的量测噪声为n_O，则观测方程表示如下：

基于观测方程构建轮速里程计量测模型。

激光雷达量测模型构造过程如下

根据三维激光雷达扫描的方式，得到的特征点集

在地图点云中对应的特征集合为

当

为平面特征时，v_j表示为其对应平面的法向量，平面中心坐标为

当

为边缘特征时，v_j表示为其对应边缘的边缘方向，边缘中心坐标为

对于任一特征点

其与地图特征之间的残差为：

用状态量表示残差并线性化得到：

理想情况下残差的值应该为0，但实际情况考虑到量测噪声n_f,j，故特征残差可表示如下：

设观测值

为：

则观测方程为：

其中

为观测矩阵，其表示如下，

当取所有特征点的量测时，令

因此激光雷达的观测方程表示如下，

基于激光雷达的观测方程构建激光雷达量测模型。

进一步的优化方案，对无人车状态更新

根据已经获得IMU的误差状态方程与轮速历程计/激光雷达误差量测方程，因此根据卡尔曼滤波的增益方程i+1时刻的滤波增益K为

其中P_i+1为i+1时刻误差状态的预测协方差矩阵，

(

或

)为观测矩阵，R(R＝n_Ln_L ^T或者R＝n_On_O ^T)为量测噪声的协方差矩阵。

根据误差状态卡尔曼滤波流程，认为i+1时刻的误差状态预测值为0，因此i+1时刻的误差状态估计值

为：

i+1时刻的状态估值

更新为：

i+1时刻的状态估值

的协方差矩阵更新为：

进一步的优化方案，生成可通行地图的过程包括：

根据点云数据进行可通行区域检测获得可通行区域，可通行区域结合更新位姿数据构建可通行地图，可通行区域检测包括地面可通行区域检测和楼梯检测；

本申请基于射线几何的方法进行地面可通行区域检测：

设

为i时刻激光雷达坐标系下的原始点云，由于需要分割地面点与非地面的障碍物点云，所以需要先根据激光雷达的安装高度对点云进行裁剪处理，得到裁剪后的点云

可表达为：

其中h₁与h₂是设定的裁剪高度阈值。

获得裁剪后的点云

之后，以俯视图的视角将点云进行扇形划分，如图3所示。

将

分别划分给不同区域S₀,S₁...S_j...S_2π/α，α为分割角度通常为激光雷达水平分辨率。现以扇形区域S_j为例进行可通行区域检测。首先将该区域内的点p_Sj(x_Sj,y_Sj,z_Sj)由直角坐标系转换成柱坐标系描述的数据结构g_col(r_col,θ_col,z_Sj)，其中r_col与θ_col分别为：

由于所设的分割角度α较小，因此可以将S_j内的点看成一条直线，对同一夹角的射线按照半径大小进行排序得到数据集G_col。

此时G_col可表示为

G_col＝{g_col,0,g_col,1...g_col,j...g_col,m|r_col,0＜r_col,1＜...r_col,j＜...r_col,m} 57

设p_col,j，p_col,j-1，p_col,j-2为射线相邻三点，n_j-1为p_col,j-2与p_col,j-1两点之间的向量，n_j为p_col,j-1与p_col,j两点之间的向量，则p_col,j与p_col,j-1的坡度β_j为：

此时判断射线前后两点的坡度是否大于一定的角度阈值ε即可判断改点是否为地面可通行区域点，故S_j区域里的地面点可表示为：

因此可得当前扫描点云的地面可通行区域P_ground为：

首先由于激光雷达的扫描范围较广，对一帧内所有点云进行处理时数据量较大，对激光雷达正前方面向楼梯的区域进行筛选得到数据集合

可表示为：

将

由柱坐标系转换为直角坐标系得到点云集合P_up，之后对P_up进行楼梯平面查找。在查找过程中注意以下特征。一阶楼梯分为踢面P_r与踏面P_t两部分，而踢面和踏板平面的法线分别与激光雷达X轴的方向平行和垂直，且通常楼梯的宽度小于w，P_r与P_t的高度延申小于h_r或者深度延申小于d_r，如图4所示。

针对上述特征P_up中筛选出合适的平面集合P_plane＝{P_plane,1,P_plane,2,...P_plane,j,...P_plane,l}，其中l为平面数量。此时P_plane会包含一些干扰平面的点云，因此需要对楼梯点云进行二次筛选。

首先需要将第一次查找的楼梯平面点云P_plane投影至XZ轴平面得到点云集合P_line＝{p_line(x_line,y_line,z_line)|y_line＝0,p_line∈P_plane}，如图5所示。

由图5可以看出，投影至XZ轴平面得到点云P_line近似为一条直线，因此利用最小二乘法对点云P_line进行直线拟合得到直线的表达式为：

A_linex+B_linez+C_line＝0 62

其中A_line，B_line，C_line为拟合直线的参数，利用拟合的直线对楼梯平面进行二次筛选。

首先获取P_plane每个平面的中心点坐标n_plane,j＝(x_plane,j,y_plane,j,z_plane,j),j＝1,2,...l，计算n_plane,j到拟合直线的距离d_line,j为：

d_line,j小于设定的距离阈值d_th为楼梯点云P_stair，P_stair可表示为：

P_stair＝{P_plane,j|d_line,j＜d_th，P_plane,j∈P_plane} 64

其中距离阈值d_th与楼梯的特征有关，通常小于1m。

根据i时刻多楼层复杂环境内的可通行区域点云

和i时刻IMU坐标系在全局坐标系下的位姿

将实时扫描的可通行区域转换至全局系下得到

可表示为：

为激光雷达与IMU的外参矩阵。得到i时刻全局可通行区域点云

后，将

投影至栅格地图之中，利用光线追踪法(Ray-Casting)对新增和消失的点云进行检测。

将每一帧可通行区域点云投影至八叉树中形成一个全局八叉树地图On，当得到i时刻扫描到的全局可通行区域

将其与O_n进行处理，得到新增的栅格区域与减少的栅格区域。

假设

为

中的一点，

为i时刻激光雷达在全局坐标系的位置，设{v_j}为

与

之间激光经过的j个栅格集合。根据octomap栅格概率更新原理，对栅格的不确定度进行更新，得到：

其中

为本次观测对应的不确定度。由上式可知当

较大时，

对环境变化的灵敏度较高，此时环境变化检测较快，但是容易收到噪声干扰；当

较小时，对环境的变化灵敏度较低，此时环境变化检测较为稳定，但是检测速度较慢。对当前时刻所有可通行区域点云进行光线追踪，可以得到全局可通行区域八叉树地图O_n中新增的栅格{V_inc,i}与消失的栅格{V_dis,i}：

{V_inc,i}＝{v_i|l(o_v,i)＞0.5,l(o_v,i-1)＜0.5} 67

{V_dis,i}＝{v_i|l(o_v,i)＜0.5,l(o_v,i-1)＞0.5} 68

此时全局可通行区域八叉树地图O_n每个栅格的状态也得到了更新，当需要将地图数据从无人车传输至其他终端时，每次地图传输只需要传输O_n中变化的部分{V_change}与无人车在全局系下的位姿

其中{V_change}可表示为：

{V_change,i}＝{{V_inc},{V_dis}} 69

根据第一次的传输数据和每次变化的部分结合无人车在全局系下的位姿构建可通行区域地图。

综上可见，本发明与现有技术相比的显著优势概括如下：

1)本发明通过多传感器融合的方式可以有效解决环境退化问题；

2)本发明利用八叉树进行地图更新可是实现增量式地图更新，有效减少数据地图数据的存储量；

3)本发明采取紧耦合的融合方式，将IMU/轮速里程计与原始特征点云数据进行融合，在点云匹配过程中考虑IMU与轮速里程计提供的量测信息，构建关于三者的综合目标函数，获取更精确的位姿估计结果。

以上，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (8)

1.一种面向室内多层建筑的可通行地图生成方法，其特征在于，包括以下步骤：

将激光雷达、惯性传感器和轮速里程计安装到无人车上；

基于激光雷达、惯性传感器和轮速里程计建立坐标系，并基于所述坐标系获得误差状态；

构建测量模型，通过测量模型对多层建筑进行测量，获得点云数据和无人车位姿数据；

通过获得的误差状态对无人车位姿数据进行数据更新，获得更新位姿数据；

根据所述点云数据和更新位姿数据生成可通行地图。

2.根据权利要求1所述的面向室内多层建筑的可通行地图生成方法，其特征在于，所述建立坐标系的方法包括：

建立激光雷达坐标系、惯性传感器坐标系、轮速里程计坐标系和全局坐标系，并将所述激光雷达坐标系和轮速里程计坐标系转换到惯性传感器坐标系中；

激光雷达坐标系的坐标系原点位于激光雷达扫描中心，X轴指向激光雷达正前向扫描方向，Z轴沿着激光雷达旋转轴竖直向上，Y轴指向激光雷达左向；

惯性传感器坐标系的坐标系原点位于惯性传感器中心，X轴指向无人车正前方向，Z轴竖直向上，Y轴指向无人车左向；

轮速里程计坐标系的坐标系原点位于无人车中心，X轴指向无人车正前方向，Z轴竖直向上，Y轴指向无人车左向；

全局坐标系为无人车开始运行时刻的惯性传感器坐标系。

3.根据权利要求1所述的面向室内多层建筑的可通行地图生成方法，其特征在于，所述获得误差状态的方法包括：

通过陀螺仪的输出和加速度计的输出获得系统输入；

通过陀螺仪与加速度计的量测噪声和陀螺仪与加速度计的随机游走过程获得系统噪声；

基于所述系统输入和系统噪声获取位姿变换关系；

通过位姿变换关系推导得出误差量的关系，并基于误差量的关系得到误差状态。

4.根据权利要求1所述的面向室内多层建筑的可通行地图生成方法，其特征在于，所述构建测量模型的方法包括构建轮速里程计量测模型和构建激光雷达量测模型；

所述构建轮速里程计量测模型的方法包括：通过惯性传感器坐标系在全局坐标系下的速度得到轮速里程计的量测信息，根据所述量测信息获得观测值，基于所述观测值获得轮速里程计的观测矩阵，所述观测矩阵结合量测噪声得到轮速里程计的观测方程，基于轮速里程计的观测方程构建轮速里程计量测模型。

5.根据权利要求4所述的面向室内多层建筑的可通行地图生成方法，其特征在于，所述构建激光雷达量测模型的方法包括：

通过激光雷达对多层建筑进行扫描，获得三维点云，通过三维点云获取特征点集，所述特征点集结合激光雷达的观测值获得激光雷达的观测方程，基于激光雷达的观测方程构建激光雷达量测模型。

6.根据权利要求1所述的面向室内多层建筑的可通行地图生成方法，其特征在于，所述生成可通行地图的过程包括：

根据点云数据进行可通行区域检测获得可通行区域，可通行区域结合更新位姿数据构建可通行地图，可通行区域检测包括地面可通行区域检测和楼梯检测；

所述地面可通行区域检测的过程包括：根据激光雷达的安装高度对点云数据进行裁剪处理，获得裁剪后的点云数据，以俯视图的视角将点云数据进行扇形分割，分割角度为激光雷达水平分辨率，将区域内的点由直角坐标系转换成柱坐标系描述的数据结构，对同一分割角的射线按照半径大小进行排序得到顺序数据集，比较射线相邻三点获得相邻三点的坡度，根据坡度判断检测区域是否为地面可通行区域。

7.根据权利要求6所述的面向室内多层建筑的可通行地图生成方法，其特征在于，所述楼梯检测的过程包括：

筛选出非地面可通行区域数据集，将非地面可通行区域数据集由柱坐标系转换为直角坐标系得到点云集合，基于楼梯的踢面、踏面和楼梯宽度对点云集合进行楼梯平面查找，得到初始楼梯平面，再通过直线拟合对初始楼梯平面进行筛选，得到楼梯区域。

8.根据权利要求1所述的面向室内多层建筑的可通行地图生成方法，其特征在于，所述生成可通行地图的过程包括：

基于octomap构建栅格地图，对可通行区域点云进行八叉树地图光线追踪，得到全局可通行区域栅格地图中新增的栅格与消失的栅格，新增栅格与消失栅格结合无人车在全局系下的位姿生成可通行地图。

Images