附有几何约束的LiDAR/INS融合定位与制图方法与流程

本发明涉及一种定位与制图方法，具体是一种附有几何约束的lidar/ins融合定位与制图方法，属于特殊地下空间的开发利用技术领域。

背景技术：

世界上有很多的关停矿井或即将关停矿井，包括煤矿、金属非金属矿、盐矿等，如果对于这些特殊地下空间加以综合利用，可以减缓现有土地资源缺乏、减少对现有资源的浪费并节约建设地下空间的费用，而复杂环境下的室内定位技术是实现未知环境探知的关键技术。

现有的室内定位技术包括基于wifi的室内定位技术、基于射频识别(radiofrequencyidentification,rfid)的室内定位技术、超宽带技术(ultrawideband,uwb)、lidar等无线传感器网络技术，其中，基于wifi的室内定位技术、基于射频识别(radiofrequencyidentification,rfid)的室内定位技术、超宽带技术(ultrawideband,uwb)虽能在室内环境下实现定位，但其信号十分容易受到室内环境的影响，且需要预先在室内布设参考节点，不能实现自主导航。lidar(激光雷达)具有定位精度高，不像视觉传感器受环境光照等影响，比较稳定的优点，广泛应用于室内定位与制图中，但lidar匹配高度依赖于周围环境的特征，在低特征环境下表现较差。为弥补lidar的缺陷，引入一种ins导航(惯性导航)技术，惯性导航技术不依赖任何外界信息即可提供导航参数的自主式导航系统，但因其是通过前一时刻已知信息推算当前时刻位姿信息，所以解算误差会随时间的累积，只能短时间保证高精度。因此，将两种传感器进行融合，取长补短得到高精度的室内地图成为国内外学者研究的一个重要方向。

在lidar/ins(激光雷达/惯性导航系统)组合系统的实时应用中，由于lidar扫描匹配的时间过长会带来观测数据时间延迟的问题，导致当前观测数据无法实时得到和处理，直接影响lidar/ins组合的实时性能。针对该问题，卢晓、李林林提出基于新息重组来建立最优滤波器，但该方法由于建立多个观测方程不利于kalman滤波的扩展；王光辉提出采用增广矩阵的方法解决时间延迟，但其计算量较大。

技术实现要素：

针对上述现有技术存在的问题，本发明提供一种附有几何约束的lidar/ins融合定位与制图方法，实时性高，计算量小，导航精度高。

为实现上述目的，本发明提供一种附有几何约束的lidar/ins融合定位与制图方法，包括以下步骤：

1)获取矿井下的移动载体的ins原始数据以及同步的lidar原始数据；

2)对获取的ins进行零偏不稳定性和陀螺漂移的误差建模与补偿；

3)在水平通道上，构建kalman滤波状态方程和量测方程：

利用补偿后的ins数据进行状态预测并构建kalman滤波状态方程，通过该状态方程得到状态预测载体位姿；同时采用基于主成分分析的附有几何约束的方法对lidar原始数据进行特征提取，以ins数据状态预测位姿作为lidar初始位姿进行水平特征修正与扫描匹配，通过巷道约束特征对提取特征进行修正；同时利用lidar数据对ins输出的位置和速度信息进行辅助修正，以lidar和ins输出的位置和速度信息之差为观测模型构建kalman滤波量测方程，解算kalman滤波状态方程和量测方程得出状态向量及其协方差矩阵；

4)在垂直通道上，将相对于运输巷道的垂向和侧向的零速作为约束引入到lidar/ins组合的观测量中，并将俯仰角误差和东向陀螺零偏误差增广至kalman滤波状态向量中，构建kalman滤波状态方程和量测方程，解算kalman滤波状态方程和量测方程得出状态向量及其协方差矩阵；

5)对步骤(3)和步骤(4)kalman滤波解算后的结果进行最优平滑处理：将解算得到的状态向量及其协方差矩阵作为初值输入到最优平滑算法中，利用正反向滤波算法实现固定区间平滑，得到移动载体平滑后在当地地理坐标系中的精确位置和姿态信息并建图；

在整个过程中，采用滑轨式的测量方法，将ins测量单元、lidar装置直接固定在移动载体上，移动载体贴于矿井运输巷行驶，垂向上与运输巷顶面保持刚性连接接触。

与现有技术相比，本发明具有以下优点：

1.本发明采用ins数据状态预测位姿作为lidar初始位姿进行扫描匹配，有效地减少了匹配时间，从而减少了计算量，提高了计算速度，同时提高了系统数据的实时性。

2.本发明利用矿井巷道的特征对提取的lidar特征进行修正，优化了特征提取质量，实现了特征的准确匹配。

3.本发明kalman滤波以两系统(lidar和ins)的位置量和速度量的差值作为观测量，用lidar数据对ins输出的位置和速度信息进行辅助修正，有效地克服了单一导航技术由于自身技术的不足对导航精度的影响。

4.本发明对ins/lidar组合解算后的数据进行最优平滑处理，进一步有效提高了定位解算精度。

附图说明

图1是本发明整体结构流程图；

图2是本发明水平通道数据处理流程图；

图3是本发明lidar原始数据基于主成分分析的附有几何约束的方法提取特征流程图；

图4是本发明垂直通道数据处理流程图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，本发明一种附有几何约束的lidar/ins融合定位与制图方法，包括以下步骤：

1)获取矿井下移动载体的ins原始数据以及同步的lidar原始数据；

2)对获取的ins数据进行零偏不稳定性和陀螺漂移的误差建模与补偿

某一固定时间段内让ins的随机零偏常值、角度随机游走和一阶马尔科夫过程保持某一固定特性，当超过该固定时间段后，重新分配惯性器件的随机零偏常值、角度随机游走和一阶马尔科夫过程的特性，使得零偏不稳定性误差的描述更加接近于ins真实的误差特性，陀螺仪和加速度计的误差建模与补偿模型如下：

ε＝ε0+bg(t/tb)+εg(t/tg)+arw(t)

式中，ε0、为随机常值；t表示载体运动的时间；bg(t/tb)、ba(t/tb)分别为陀螺和加速度计零偏不稳定性的零偏常值均方差；εg(t/tg)、εa(t/tg)分别表示陀螺仪和加速度计的一阶马尔科夫过程均方差；arw(t)、vrw(t)分别表示陀螺仪和加速度计的随机游走噪声；tb表示零偏不稳定性的作用时间；tg表示一阶马尔科夫的作用时间。

3)在水平通道上，构建kalman滤波状态方程和量测方程，如图2所示：

利用补偿后的ins数据进行状态预测并构建kalman滤波状态方程，通过该状态方程得到状态预测载体位姿(位置和姿态)；同时采用基于主成分分析的附有几何约束的方法对lidar原始数据进行特征提取，以ins数据状态预测位姿作为lidar初始位姿进行水平特征修正与扫描匹配，通过巷道约束特征对提取特征进行修正；同时利用lidar数据对ins输出的位置和速度信息进行辅助修正，以lidar和ins输出的位置和速度信息之差为观测模型构建kalman滤波量测方程，解算kalman滤波状态方程和量测方程得出状态向量及其协方差矩阵，从而得到水平通道上移动载体在当地地理坐标系下的位置和姿态信息。

其中，利用补偿后的ins数据进行状态预测并构建kalman滤波状态方程，具体方法如下：

首先得到方向余弦矩阵与ins的输出角速度之间的关系：

通过公式(1)得到之后，通过捷联惯性导航机械编排求得速度误状态量与位置误差状态量计算公式：

其中：表示导航坐标系到载体坐标系转换的方向余弦矩阵，δf^b表示加表残余零偏误差，表示ins的姿态角误差，表示地球自转角速度在导航坐标系下的投影，表示牵连角速度在导航坐标系下的投影，δvⁿ表示速度误差在导航坐标系下的投影，和表示角速度误差，表示重力误差。

所述导航坐标系是以当地的北东地方向来建立的，所以在ins和lidar中都是一个坐标系。

kalman滤波融合以ins的解算结果误差作为状态量，构建状态方程：

xk＝φk/k-1xk-1+γk/k-1wk-1

式中：为ins的姿态角误差，δv为在导航坐标系下的位置误差，δr为在导航坐标系下的位置误差；φk/k-1为状态一步转移矩阵，γk/k-1为ins系统噪声分配矩阵，wk-1是ins系统噪声向量，k和k-1分别表示k时刻和k-1时刻。

如图3所示，采用基于主成分分析的附有几何约束的方法对lidar原始数据进行特征提取，具体方法如下：

在矿井环境下lidar数据具有集聚类的特点，采用基于主成分分析的附有几何约束的方法进行直线和角点特征的提取，具体方法如下：

a)将区域的两端lidar数据点连成一条直线l；

b)求区域之间其他所有点到此直线l的距离d；

c)找出最大的距离值dmax，判断dmax与阈值d1的大小：

若dmax<d1，则认为该直线之间的点符合直线特征，归入直线特征集ω1；

若dmax≥d1，则将该点看作角点特征，并以该点到直线l的垂线或垂线延长线为界划分区域，连接划分后区域两端的点构成直线l；

d)重复b)-步骤c)，直到所有区域都寻找完成。

由于在明显呈凹形状的区域中，角点特征的提取会出现错误，这时利用矿井巷道本身的线特征作为约束对lidar提取的直线特征和角点特征进行纠正，纠正方法如下：

判断相邻两条直线特征的斜率之差是否大于设定阈值d2，若是，则将这两条直线特征提取至修正后的直线特征集ω2，若否，删除该角点特征并合并这两条直线特征，提取至直线特征集ω1。

以ins的预测载体位姿作为lidar的初始位姿进行扫描匹配，具体方法如下：

要使不同时刻的点运用同一坐标系表示，必须借助ins获取移动载体的位姿变化，从而解算出不同时刻坐标系的转换矩阵，通过坐标值和转换矩阵，实现当前点云和历史点云的统一表示，实现多帧点云融合。通过捷联惯导的输出结果，将历史点云数据投影到当前相对坐标系下，与当前点云数据构成融合。四元数求解位置坐标转换矩阵。lidar在ins信息预测载体位姿的基础上进行扫描匹配，实现高精度的定位，匹配方法如下：

在k时刻扫描匹配过程中，利用k-1时刻的ins预测位姿进行初始化迭代，公式如下：

记k-1时刻的位姿为其中(px-1,k,py-1,k)为移动载体在地球坐标系中的坐标，为航偏角；为利用ins信息预测的移动载体从k-1至k时刻的水平x方向、y方向以及航偏角的变化量。

利用lidar数据对ins输出的位置和速度信息进行辅助修正，以lidar和ins输出的位置和速度信息之差为观测模型构建kalman滤波量测方程，具体方法如下：

a)由于ins的误差随时间累积的特性，以lidar输出位置和速度信息对ins输出位置和速度信息进行辅助修正，量测方程中的观测量分为速度观测量和位置观测量，在lidar/ins的组合方式中，以两系统得到的位置量和速度量的差值作为观测量，由于ins和lidar得到的位置、速度信息分别在导航坐标系和相对参考坐标系下，因此，需要对两者信息进行转换，使其在统一坐标系下，利用位姿转换矩阵将lidar输出的位置和速度信息转换到ins导航坐标系下的位置和速度信息

式中：表示相对参考坐标系下的位姿到传感器测量得到的载体系下位姿的转换矩阵，表示载体系下的位姿到导航坐标系下的位姿转换矩阵。

b)通过转换后的位置和速度信息得到位置误差量测方程zr和速度误差量测方程zv

其中：

式中：rx1、ry1表示ins在导航坐标系下的位置推算值，rx2、ry2表示lidar的位置输出值，δrx、δry表示ins的位置误差，ax、ay为lidar的位置误差；

其中：

式中：vx1、vy1表示ins在导航坐标系下水平面内的速度信息，vx2、vy2表示lidar的速度输出值，δrx、δry表示ins的速度误差，bx、by为lidar的速度误差。

进一步，将kalman滤波量测方程zr、zv合并表示为矩阵形式：

令则：

对zk进行kalman滤波解算之后得到：

pk＝(i-kkhk)pk/k-1

其中，

其中，为前一时刻的状态估值；为xk的kalman滤波估值，即移动载体的位姿信息；φk/k-1为状态一步转移矩阵；kk为滤波增益矩阵，pk/k-1为预测的状态向量协方差阵；为当前时刻的量测方程系数矩阵，为hk的转置；rk为量测噪声vk的对称正定方差阵；pk-1为当前时刻状态向量的协方差阵；γk-1表示量测矩阵；qk-1表示状态噪声；i表示单位矩阵。

4)在垂直通道上，构建kalman滤波状态方程和量测方程，如图4所示：将相对于运输巷道的垂向和侧向的零速作为约束引入到lidar/ins组合的观测量中，并将俯仰角误差和东向陀螺零偏误差增广至kalman滤波状态向量中，以提升矿井巷道垂向测量精度，构建kalman滤波状态方程和量测方程，解算kalman滤波状态方程和量测方程得出状态向量及其协方差矩阵，从而得到垂直通道上移动载体在当地地理坐标系下的位置和姿态信息

在垂直通道上，移动载体运动过程中与矿井运输巷密贴，垂向上与运输巷顶面保持刚性连接接触，可以准确反映运输巷的几何状态，移动载体只有在运输巷的纵向里程方向上有运动速度，而在与之垂直的侧向和垂向速度均为零，因此，移动载体可以利用这两种约束来修正ins。将该约束引入到lidar与lidar/ins组合的观测量中，此时，需要考虑俯仰角误差的影响，即将相对于运输巷道的垂向和侧向零速作为约束引入到lidar/ins组合的观测量中，并将俯仰角误差和东向陀螺零偏误差增广至卡kalman波状态向量中，提升矿井巷道垂向测量精度；

将相对于运输巷道的垂向和侧向的零速作为约束，构建kalman滤波状态方程和量测方程：

高程方向kalman滤波状态向量xd和状态方程为：

式中：fd(t)表示ins系统动态矩阵；gd(t)表示ins系统噪声驱动矩阵；wd(t)表示ins系统激励噪声矩阵。

将ins系统激励噪声wd(t)均假设为高斯白噪声：

wd(t)＝[0wa,dwg,ewgb,ewab,d]

其中：δhins为ins机械编排推算高程的实际误差；

δhins(t)＝hins(t)-hreal(t)

式中：hins(t)表示ins导航推算值，hreal(t)表示ins导航的实际值；δvins,d为ins导航推算的垂向误差；bg,n为北向陀螺零偏误差；为来自于东向陀螺零偏误差bg,e和东向陀螺测量白噪声wg,e的俯仰角误差；ba,d为加速度计垂直零偏误差；wa,d为垂向加速度计误差，并假设其仅为高斯白噪声过程，i表示单位矩阵；t表示载体移动的时间。

将ins参与零偏误差建模为一阶高斯马尔科夫过程：

式中：bg,e为东向陀螺零偏误差，tgb为相关时间，wgb,e为驱动白噪声；ba,d为加速度计垂直零偏误差，tab为相关时间，驱动白噪声为wab,d为驱动白噪声。

为方便使用离散时间卡尔曼滤波方程，对状态方程进行线性化得：

xk＝ψk/k-1xk-1+wk-1

其中：

式中：ψk/k-1表示状态一步转移矩阵；wk-1是ins系统噪声向量；f(t)表示ins系统动态矩阵；g(t)表示ins系统噪声驱动矩阵；w(t)表示ins系统激励噪声矩阵；k和k-1分别表示k时刻和k-1时刻。

kalman滤波器的观测量为ins导航推算的高程与lidar解算高程之差和零速约束误差：

ins导航推算的高程与lidar解算高程之差的量测方程为：

zh＝hins-hlidar＝δhins+nr,h

式中：hins表示ins导航推算值，hlidar表示lidar输出值，δhins为ins导航机械编排推算高程的实际误差；nr,h(t)表示lidar的高程误差；

零速约束的速度误差量测方程为：

zv,d＝δvd-vnφpitch+nv,d

式中：zv,d表示零速约束在v系下的垂向速度误差；vn表示移动载体沿北向的速度；表示俯仰角误差；nv,d表示零速约束观测值的观测噪声。

将量测方程zh、zv,d合并表示为矩阵的形式：

zk＝mkxk+nk

对矩阵zk、mk、nk进行kalman滤波解算之后得到：

pk＝(i-kkmk)pk/k-1

其中，

其中，为前一时刻的状态估值；为xk的kalman滤波估值，即移动载体的位姿信息；ψk/k-1为状态一步转移矩阵；kk为滤波增益矩阵；pk/k-1为预测的状态向量协方差阵；为k时刻的量测方程系数矩阵，为mk的转置；rk为量测噪声nk的对称正定方差阵；pk-1为当前时刻状态向量的协方差阵；

5)如图1所示，对步骤3)和步骤4)kalman滤波解算后的结果进行最优平滑处理，将解算得到的状态向量及其协方差矩阵pk作为初值输入到最优平滑算法中，利用正反向滤波算法实现固定区间平滑，得到移动载体平滑后在当地地理坐标系中的精确位置和姿态信息并建图，实现移动载体的精确导航。

在整个过程中，采用滑轨式的测量方法，将ins导航测量单元、lidar装置直接固定在移动载体上，移动载体贴于矿井运输巷行驶，垂向上与运输巷顶面保持刚性连接接触，还可获得矿井巷道三维坐标序列，可用于后期的应用，如巷道的变形监测等。