CN112578394A - 附有几何约束的LiDAR/INS融合定位与制图方法 - Google Patents

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

Info

Publication number
CN112578394A
CN112578394A CN202011340993.9A CN202011340993A CN112578394A CN 112578394 A CN112578394 A CN 112578394A CN 202011340993 A CN202011340993 A CN 202011340993A CN 112578394 A CN112578394 A CN 112578394A
Authority
CN
China
Prior art keywords
ins
lidar
error
equation
kalman filtering
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011340993.9A
Other languages
English (en)
Other versions
CN112578394B (zh
Inventor
张秋昭
蔚科
王方田
孙澳
郑南山
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China University of Mining and Technology CUMT
Original Assignee
China University of Mining and Technology CUMT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China University of Mining and Technology CUMT filed Critical China University of Mining and Technology CUMT
Priority to CN202011340993.9A priority Critical patent/CN112578394B/zh
Publication of CN112578394A publication Critical patent/CN112578394A/zh
Application granted granted Critical
Publication of CN112578394B publication Critical patent/CN112578394B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种附有几何约束的LiDAR/INS融合定位与制图方法，获取矿井下的移动载体的INS数据以及同步的LiDAR数据；以INS的预测载体位姿作为LiDAR的初始位姿进行扫描匹配，并对得到的LiDAR数据采用基于主成分分析的附有几何约束的方法提取线特征和角点特征；在水平通道和垂直通道上，分别构建Kalman滤波状态方程和量测方程；最终通过Kalman滤波融合解算并进行最优平滑处理，得到整个导航过程中移动载体在当地地理坐标系下的位置、姿态信息并建图，实时性高，计算量小，导航精度高。

Description

附有几何约束的LiDAR/INS融合定位与制图方法
技术领域
本发明涉及一种定位与制图方法，具体是一种附有几何约束的LiDAR/INS融合定位与制图方法，属于特殊地下空间的开发利用技术领域。
背景技术
世界上有很多的关停矿井或即将关停矿井，包括煤矿、金属非金属矿、盐矿等，如果对于这些特殊地下空间加以综合利用，可以减缓现有土地资源缺乏、减少对现有资源的浪费并节约建设地下空间的费用，而复杂环境下的室内定位技术是实现未知环境探知的关键技术。
现有的室内定位技术包括基于WiFi的室内定位技术、基于射频识别(Radiofrequency identification,RFID)的室内定位技术、超宽带技术(Ultra Wideband,UWB)、LiDAR等无线传感器网络技术，其中，基于WiFi的室内定位技术、基于射频识别(Radiofrequency identification,RFID)的室内定位技术、超宽带技术(Ultra Wideband,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)
Figure BDA0002798598250000041
式中，ε0、
Figure BDA0002798598250000042
为随机常值；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滤波状态方程，具体方法如下：
首先得到方向余弦矩阵
Figure BDA0002798598250000043
与INS的输出角速度之间的关系：
Figure BDA0002798598250000044
通过公式(1)得到
Figure BDA0002798598250000045
之后，通过捷联惯性导航机械编排求得速度误状态量与位置误差状态量计算公式：
Figure BDA0002798598250000046
Figure BDA0002798598250000047
其中：
Figure BDA0002798598250000048
表示导航坐标系到载体坐标系转换的方向余弦矩阵，δfb表示加表残余零偏误差，
Figure BDA0002798598250000049
表示INS的姿态角误差，
Figure BDA00027985982500000410
表示地球自转角速度在导航坐标系下的投影，
Figure BDA00027985982500000411
表示牵连角速度在导航坐标系下的投影，δvn表示速度误差在导航坐标系下的投影，
Figure BDA00027985982500000412
和
Figure BDA00027985982500000413
表示角速度误差，
Figure BDA00027985982500000414
表示重力误差。
所述导航坐标系是以当地的北东地方向来建立的，所以在INS和LiDAR中都是一个坐标系。
Kalman滤波融合以INS的解算结果误差
Figure BDA0002798598250000051
作为状态量，构建状态方程：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1
式中：
Figure BDA0002798598250000052
为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预测位姿
Figure BDA0002798598250000061
进行初始化迭代，公式如下：
Figure BDA0002798598250000062
记k-1时刻的位姿为
Figure BDA0002798598250000063
其中(px-1,k,py-1,k)为移动载体在地球坐标系中的坐标，
Figure BDA0002798598250000064
为航偏角；
Figure BDA0002798598250000065
为利用INS信息预测的移动载体从k-1至k时刻的水平x方向、y方向以及航偏角的变化量。
利用LiDAR数据对INS输出的位置和速度信息进行辅助修正，以LiDAR和INS输出的位置和速度信息之差为观测模型构建Kalman滤波量测方程，具体方法如下：
a)由于INS的误差随时间累积的特性，以LiDAR输出位置和速度信息对INS输出位置和速度信息进行辅助修正，量测方程中的观测量分为速度观测量和位置观测量，在LiDAR/INS的组合方式中，以两系统得到的位置量和速度量的差值作为观测量，由于INS和LiDAR得到的位置、速度信息分别在导航坐标系和相对参考坐标系下，因此，需要对两者信息进行转换，使其在统一坐标系下，利用位姿转换矩阵将LiDAR输出的位置和速度信息
Figure BDA0002798598250000066
转换到INS导航坐标系下的位置和速度信息
Figure BDA0002798598250000067
Figure BDA0002798598250000068
式中：
Figure BDA0002798598250000069
表示相对参考坐标系下的位姿到传感器测量得到的载体系下位姿的转换矩阵，
Figure BDA00027985982500000610
表示载体系下的位姿到导航坐标系下的位姿转换矩阵。
b)通过转换后的位置和速度信息得到位置误差量测方程Zr和速度误差量测方程Zv
Figure BDA00027985982500000611
其中：
Figure BDA0002798598250000071
Figure BDA0002798598250000072
式中：rx1、ry1表示INS在导航坐标系下的位置推算值，rx2、ry2表示LiDAR的位置输出值，δrx、δry表示INS的位置误差，Ax、Ay为LiDAR的位置误差；
Figure BDA0002798598250000073
其中：
Figure BDA0002798598250000074
Figure BDA0002798598250000075
式中：vx1、vy1表示INS在导航坐标系下水平面内的速度信息，vx2、vy2表示LiDAR的速度输出值，δrx、δry表示INS的速度误差，Bx、By为LiDAR的速度误差。
进一步，将Kalman滤波量测方程Zr、Zv合并表示为矩阵形式：
令
Figure BDA0002798598250000076
则：
Figure BDA0002798598250000077
对Zk进行Kalman滤波解算之后得到：
Figure BDA0002798598250000078
Pk＝(I-KkHk)Pk/k-1
其中，
Figure BDA0002798598250000079
Figure BDA00027985982500000710
Figure BDA00027985982500000711
其中，
Figure BDA0002798598250000081
为
Figure BDA0002798598250000082
前一时刻的状态估值；
Figure BDA0002798598250000083
为Xk的Kalman滤波估值，即移动载体的位姿信息；Φk/k-1为状态一步转移矩阵；Kk为滤波增益矩阵，Pk/k-1为预测的状态向量协方差阵；
Figure BDA0002798598250000084
为当前时刻的量测方程系数矩阵，
Figure BDA0002798598250000085
为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和状态方程
Figure BDA0002798598250000086
为：
Figure BDA0002798598250000087
Figure BDA0002798598250000088
式中：FD(t)表示INS系统动态矩阵；GD(t)表示INS系统噪声驱动矩阵；WD(t)表示INS系统激励噪声矩阵。
将INS系统激励噪声WD(t)均假设为高斯白噪声：
WD(t)＝[0 Wa,D Wg,E Wgb,E Wab,D]
Figure BDA0002798598250000091
Figure BDA0002798598250000092
其中：δhINS为INS机械编排推算高程的实际误差；
δhINS(t)＝hINS(t)-hreal(t)
式中：hINS(t)表示INS导航推算值，hreal(t)表示INS导航的实际值；δvINS,D为INS导航推算的垂向误差；bg,N为北向陀螺零偏误差；
Figure BDA0002798598250000098
为来自于东向陀螺零偏误差bg,E和东向陀螺测量白噪声Wg,E的俯仰角误差；ba,D为加速度计垂直零偏误差；Wa,D为垂向加速度计误差，并假设其仅为高斯白噪声过程，I表示单位矩阵；t表示载体移动的时间。
将INS参与零偏误差建模为一阶高斯马尔科夫过程：
Figure BDA0002798598250000093
Figure BDA0002798598250000094
式中：bg,E为东向陀螺零偏误差，Tgb为相关时间，Wgb,E为驱动白噪声；ba,D为加速度计垂直零偏误差，Tab为相关时间，驱动白噪声为Wab,D为驱动白噪声。
为方便使用离散时间卡尔曼滤波方程，对状态方程
Figure BDA0002798598250000095
进行线性化得：
Xk＝Ψk/k-1Xk-1+Wk-1
其中：
Figure BDA0002798598250000096
Figure BDA0002798598250000097
式中：Ψ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表示移动载体沿北向的速度；
Figure BDA0002798598250000101
表示俯仰角误差；nv,D表示零速约束观测值的观测噪声。
将量测方程zh、zv,D合并表示为矩阵的形式：
Zk＝MkXk+Nk
Figure BDA0002798598250000102
Figure BDA0002798598250000103
Figure BDA0002798598250000104
对矩阵Zk、Mk、Nk进行Kalman滤波解算之后得到：
Figure BDA0002798598250000105
Pk＝(I-KkMk)Pk/k-1
其中，
Figure BDA0002798598250000111
Figure BDA0002798598250000112
Figure BDA0002798598250000113
其中，
Figure BDA0002798598250000114
为
Figure BDA0002798598250000115
前一时刻的状态估值；
Figure BDA0002798598250000116
为Xk的Kalman滤波估值，即移动载体的位姿信息；Ψk/k-1为状态一步转移矩阵；Kk为滤波增益矩阵；Pk/k-1为预测的状态向量协方差阵；
Figure BDA0002798598250000117
为k时刻的量测方程系数矩阵，
Figure BDA0002798598250000118
为Mk的转置；Rk为量测噪声Nk的对称正定方差阵；Pk-1为当前时刻状态向量的协方差阵；
5)如图1所示，对步骤3)和步骤4)Kalman滤波解算后的结果进行最优平滑处理，将解算得到的状态向量
Figure BDA0002798598250000119
及其协方差矩阵Pk作为初值输入到最优平滑算法中，利用正反向滤波算法实现固定区间平滑，得到移动载体平滑后在当地地理坐标系中的精确位置和姿态信息并建图，实现移动载体的精确导航。
在整个过程中，采用滑轨式的测量方法，将INS导航测量单元、LiDAR装置直接固定在移动载体上，移动载体贴于矿井运输巷行驶，垂向上与运输巷顶面保持刚性连接接触，还可获得矿井巷道三维坐标序列，可用于后期的应用，如巷道的变形监测等。

Claims (7)

1.一种附有几何约束的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装置直接固定在移动载体上，移动载体贴于矿井运输巷行驶，垂向上与运输巷顶面保持刚性连接接触。
2.根据权利要求1所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，在LiDAR装置中采用重复位置识别技术。
3.根据权利要求1所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，步骤(3)中采用基于主成分分析的附有几何约束的方法对LiDAR原始数据进行特征提取，具体方法如下：
在矿井环境下LiDAR数据具有集聚类的特点，采用基于主成分分析的附有几何约束的方法进行直线和角点特征的提取，具体方法如下：
a)将区域的两端LiDAR数据点连成一条直线l；
b)求区域之间其他所有点到此直线l的距离d；
c)找出最大的距离值dmax，判断dmax与阈值D1的大小：
若dmax<D1，则认为该直线之间的点符合直线特征，归入直线特征集Ω1；
若dmax≥D1，则将该点看作角点特征，并以该点到直线l的垂线或垂线延长线为界划分区域，连接划分后区域两端的点构成直线l；
d)重复b)-步骤c)，直到所有区域都寻找完成。
4.根据权利要求3所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，将矿井巷道本身的线特征作为约束对LiDAR提取的直线特征和角点特征进行纠正，纠正方法如下：
判断相邻两条直线特征的斜率之差是否大于设定阈值D2，若是，则将这两条直线特征提取至修正后的直线特征集Ω2，若否，删除该角点特征并合并这两条直线特征，提取至直线特征集Ω1。
5.根据权利要求1所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，步骤(3)中以INS数据状态预测位姿作为初始位姿进行水平特征修正与扫描匹配，具体方法如下：
在k时刻扫描匹配过程中，利用k-1时刻的INS预测位姿
Figure FDA0002798598240000021
进行初始化迭代，公式如下：
Figure FDA0002798598240000022
记k-1时刻的位姿为
Figure FDA0002798598240000023
其中(px-1,k,py-1,k)为移动载体在地球坐标系中的坐标，
Figure FDA0002798598240000024
为航偏角；
Figure FDA0002798598240000025
为利用INS信息预测的移动载体从k-1至k时刻的位姿变化量。
6.根据权利要求1所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，步骤3)中通过下述公式构建Kalman滤波状态方程和量测方程：
Kalman滤波状态方程：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1
式中：
Figure FDA0002798598240000031
为INS的姿态角误差，δv为在INS下的位置误差，δr为在INS下的位置误差；Φk/k-1为状态一步转移矩阵，Γk/k-1为INS系统噪声分配矩阵，Wk-1是INS系统噪声向量，Xk表示k时刻的状态向量，Xk-1表示k-1时刻的状态向量，k和k-1分别表示k时刻和k-1时刻；
Kalman滤波量测方程包括位置误差量测方程Zr和速度误差量测方程Zv：
Figure FDA0002798598240000032
其中：
Figure FDA0002798598240000033
Figure FDA0002798598240000034
式中：rx1、ry1表示INS在导航坐标系下的位置推算值，rx2、ry2表示LiDAR的位置输出值，δrx、δry表示INS的位置误差，Ax、Ay为LiDAR的位置误差；
Figure FDA0002798598240000035
其中：
Figure FDA0002798598240000036
Figure FDA0002798598240000037
式中：vx1、vy1表示INS数据在导航坐标系下水平面内的速度信息，vx2、vy2表示LiDAR的速度输出值，δrx、δry表示INS的速度误差，Bx、By为LiDAR的速度误差。
7.根据权利要求1所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，步骤4)中通过下述公式构建Kalman滤波状态方程和量测方程：
构建Kalman滤波状态方程：
高程方向Kalman滤波状态向量xD和状态方程
Figure FDA0002798598240000041
为：
Figure FDA0002798598240000042
Figure FDA0002798598240000043
式中：FD(t)表示INS系统动态矩阵；GD(t)表示INS系统噪声驱动矩阵；WD(t)表示INS系统激励噪声矩阵；
将INS系统激励噪声WD(t)均假设为高斯白噪声：
WD(t)＝[0 Wa,D Wg,E Wgb,E Wab,D]
Figure FDA0002798598240000044
Figure FDA0002798598240000045
其中：δhINS为INS导航机械编排推算高程的实际误差；
δhINS(t)＝hINS(t)-hreal(t)
式中：hINS(t)表示INS导航推算值，hreal(t)表示INS导航的实际值；δvINS,D为INS导航推算的垂向误差；bg,N为北向陀螺零偏误差；
Figure FDA0002798598240000048
为来自于东向陀螺零偏误差bg,E和东向陀螺测量白噪声Wg,E的俯仰角误差；ba,D为加速度计垂直零偏误差；Wa,D为垂向加速度计误差，并假设其仅为高斯白噪声过程；I表示单位矩阵；t表示载体移动的时间；
将INS参与零偏误差建模为一阶高斯马尔科夫过程：
Figure FDA0002798598240000046
Figure FDA0002798598240000047
式中：bg,E为东向陀螺零偏误差，Tgb为相关时间，Wgb,E为驱动白噪声；ba,D为加速度计垂直零偏误差，Tab为相关时间，驱动白噪声为Wab,D为驱动白噪声；
对状态方程
Figure FDA0002798598240000051
进行线性化得：
Xk＝Ψk/k-1Xk-1+Wk-1
其中：
Figure FDA0002798598240000052
Figure FDA0002798598240000053
式中：Ψk/k-1表示状态一步转移矩阵；Wk-1是INS系统噪声向量；F(t)表示INS系统动态矩阵；G(t)表示INS系统噪声驱动矩阵；W(t)表示INS系统激励噪声矩阵；k和k-1分别表示k时刻和k-1时刻；
构建Kalman滤波量测方程：
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表示移动载体沿北向的速度；
Figure FDA0002798598240000054
表示俯仰角误差；nv,D表示零速约束观测值的观测噪声。
CN202011340993.9A 2020-11-25 2020-11-25 附有几何约束的LiDAR/INS融合定位与制图方法 Active CN112578394B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011340993.9A CN112578394B (zh) 2020-11-25 2020-11-25 附有几何约束的LiDAR/INS融合定位与制图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011340993.9A CN112578394B (zh) 2020-11-25 2020-11-25 附有几何约束的LiDAR/INS融合定位与制图方法

Publications (2)

Publication Number Publication Date
CN112578394A true CN112578394A (zh) 2021-03-30
CN112578394B CN112578394B (zh) 2022-09-27

Family

ID=75124313

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011340993.9A Active CN112578394B (zh) 2020-11-25 2020-11-25 附有几何约束的LiDAR/INS融合定位与制图方法

Country Status (1)

Country Link
CN (1) CN112578394B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113843788A (zh) * 2021-06-03 2021-12-28 安徽师范大学 一种基于毫米波与惯性融合导航的安防机器人
CN114858124A (zh) * 2022-04-14 2022-08-05 广东省水利水电科学研究院 一种大坝表面变形监测系统及方法
CN116541732A (zh) * 2023-07-05 2023-08-04 山东金叶智能设备有限公司 基于超声波数据与最优化算法的气象监测系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20130102873A (ko) * 2012-03-08 2013-09-23 현대엠엔소프트 주식회사 컬러 지상 라이다 측량 데이터를 이용하여 건물의 3차원 모델링 데이터를 자동으로 생성하기 위한 방법
CN109708632A (zh) * 2019-01-31 2019-05-03 济南大学 一种面向移动机器人的激光雷达/ins/地标松组合导航系统及方法
CN109737957A (zh) * 2019-01-31 2019-05-10 济南大学 一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统
CN110849363A (zh) * 2019-12-03 2020-02-28 深圳市镭神智能系统有限公司 激光雷达与组合惯导的位姿标定方法、系统及介质
CN111665512A (zh) * 2019-02-21 2020-09-15 香港科技大学 基于3d激光雷达和惯性测量单元的融合的测距和绘图

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20130102873A (ko) * 2012-03-08 2013-09-23 현대엠엔소프트 주식회사 컬러 지상 라이다 측량 데이터를 이용하여 건물의 3차원 모델링 데이터를 자동으로 생성하기 위한 방법
CN109708632A (zh) * 2019-01-31 2019-05-03 济南大学 一种面向移动机器人的激光雷达/ins/地标松组合导航系统及方法
CN109737957A (zh) * 2019-01-31 2019-05-10 济南大学 一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统
CN111665512A (zh) * 2019-02-21 2020-09-15 香港科技大学 基于3d激光雷达和惯性测量单元的融合的测距和绘图
CN110849363A (zh) * 2019-12-03 2020-02-28 深圳市镭神智能系统有限公司 激光雷达与组合惯导的位姿标定方法、系统及介质

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113843788A (zh) * 2021-06-03 2021-12-28 安徽师范大学 一种基于毫米波与惯性融合导航的安防机器人
CN113843788B (zh) * 2021-06-03 2024-04-19 安徽师范大学 一种基于毫米波与惯性融合导航的安防机器人
CN114858124A (zh) * 2022-04-14 2022-08-05 广东省水利水电科学研究院 一种大坝表面变形监测系统及方法
CN114858124B (zh) * 2022-04-14 2023-04-07 广东省水利水电科学研究院 一种大坝表面变形监测系统及方法
CN116541732A (zh) * 2023-07-05 2023-08-04 山东金叶智能设备有限公司 基于超声波数据与最优化算法的气象监测系统
CN116541732B (zh) * 2023-07-05 2023-09-01 山东金叶智能设备有限公司 基于超声波数据与最优化算法的气象监测系统

Also Published As

Publication number Publication date
CN112578394B (zh) 2022-09-27

Similar Documents

Publication Publication Date Title
CN112578394B (zh) 附有几何约束的LiDAR/INS融合定位与制图方法
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN110514225B (zh) 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN109709801B (zh) 一种基于激光雷达的室内无人机定位系统及方法
CN105628026B (zh) 一种移动物体的定位定姿方法和系统
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN110412635B (zh) 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN104635251B (zh) 一种ins/gps组合定位定姿新方法
WO2018184108A1 (en) Location-based services system and method therefor
CN105823480A (zh) 基于单信标的水下移动目标定位算法
CN109781098B (zh) 一种列车定位的方法和系统
Zheng et al. Low-cost gps-aided lidar state estimation and map building
CN103674021A (zh) 基于捷联惯导与星敏感器的组合导航系统及方法
CN115407357A (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN102519470A (zh) 多级嵌入式组合导航系统及导航方法
Zhou et al. Kinematic measurement of the railway track centerline position by GNSS/INS/odometer integration
CN114485643B (zh) 一种煤矿井下移动机器人环境感知与高精度定位方法
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合系统
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN111288984A (zh) 一种基于车联网的多车联合绝对定位的方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
Srinara et al. Performance analysis of 3D NDT scan matching for autonomous vehicles using INS/GNSS/3D LiDAR-SLAM integration scheme
CN116429116A (zh) 一种机器人定位方法及设备
Chen et al. Knowledge-based indoor positioning based on LiDAR aided multiple sensors system for UGVs

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant