CN112578394A - 附有几何约束的LiDAR/INS融合定位与制图方法 - Google Patents
附有几何约束的LiDAR/INS融合定位与制图方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 230000004927 fusion Effects 0.000 title claims abstract description 13
- 238000001914 filtration Methods 0.000 claims abstract description 49
- 238000005259 measurement Methods 0.000 claims abstract description 41
- 238000009499 grossing Methods 0.000 claims abstract description 14
- 230000008569 process Effects 0.000 claims abstract description 14
- 238000004364 calculation method Methods 0.000 claims abstract description 11
- 238000000513 principal component analysis Methods 0.000 claims abstract description 9
- 230000001360 synchronised effect Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 52
- 238000012937 correction Methods 0.000 claims description 11
- 238000013507 mapping Methods 0.000 claims description 9
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- 230000033001 locomotion Effects 0.000 claims description 5
- 230000005284 excitation Effects 0.000 claims description 4
- 230000004807 localization Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 15
- 230000009466 transformation Effects 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000000605 extraction Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000005295 random walk Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000003321 amplification Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000006798 recombination Effects 0.000 description 1
- 238000005215 recombination Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
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融合定位与制图方法，属于特殊地下空间的开发利用技术领域。
背景技术
世界上有很多的关停矿井或即将关停矿井，包括煤矿、金属非金属矿、盐矿等，如果对于这些特殊地下空间加以综合利用，可以减缓现有土地资源缺乏、减少对现有资源的浪费并节约建设地下空间的费用，而复杂环境下的室内定位技术是实现未知环境探知的关键技术。
现有的室内定位技术包括基于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)
式中，ε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)得到
之后，通过捷联惯性导航机械编排求得速度误状态量与位置误差状态量计算公式：
其中：
表示导航坐标系到载体坐标系转换的方向余弦矩阵，δfb表示加表残余零偏误差，
表示INS的姿态角误差，
表示地球自转角速度在导航坐标系下的投影，
表示牵连角速度在导航坐标系下的投影，δvn表示速度误差在导航坐标系下的投影，
和
表示角速度误差，
表示重力误差。
所述导航坐标系是以当地的北东地方向来建立的，所以在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)＝[0 Wa,D Wg,E Wgb,E Wab,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装置直接固定在移动载体上，移动载体贴于矿井运输巷行驶，垂向上与运输巷顶面保持刚性连接接触，还可获得矿井巷道三维坐标序列，可用于后期的应用，如巷道的变形监测等。
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预测位姿
进行初始化迭代，公式如下：
记k-1时刻的位姿为
其中(px-1,k,py-1,k)为移动载体在地球坐标系中的坐标，
为航偏角；
为利用INS信息预测的移动载体从k-1至k时刻的位姿变化量。
6.根据权利要求1所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，步骤3)中通过下述公式构建Kalman滤波状态方程和量测方程：
Kalman滤波状态方程：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1
式中：
为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：
其中：
式中：rx1、ry1表示INS在导航坐标系下的位置推算值，rx2、ry2表示LiDAR的位置输出值，δrx、δry表示INS的位置误差，Ax、Ay为LiDAR的位置误差；
其中：
式中：vx1、vy1表示INS数据在导航坐标系下水平面内的速度信息，vx2、vy2表示LiDAR的速度输出值，δrx、δry表示INS的速度误差，Bx、By为LiDAR的速度误差。
7.根据权利要求1所述的一种附有几何约束的LiDAR/INS融合定位与制图方法，其特征在于，步骤4)中通过下述公式构建Kalman滤波状态方程和量测方程：
构建Kalman滤波状态方程：
高程方向Kalman滤波状态向量xD和状态方程
为：
式中：FD(t)表示INS系统动态矩阵；GD(t)表示INS系统噪声驱动矩阵；WD(t)表示INS系统激励噪声矩阵；
将INS系统激励噪声WD(t)均假设为高斯白噪声：
WD(t)＝[0 Wa,D Wg,E Wgb,E Wab,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解算高程之差的量测方程为：
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表示零速约束观测值的观测噪声。
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)
| 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)
| 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激光雷达和惯性测量单元的融合的测距和绘图 |
-
2020
- 2020-11-25 CN CN202011340993.9A patent/CN112578394B/zh active Active
Patent Citations (5)
| 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)
| 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) | 一种基于激光雷达的室内无人机定位系统及方法 | |
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN105628026B (zh) | 一种移动物体的定位定姿方法和系统 | |
| WO2018184108A1 (en) | Location-based services system and method therefor | |
| CN106052691B (zh) | 激光测距移动制图中闭合环误差纠正方法 | |
| CN105823480A (zh) | 基于单信标的水下移动目标定位算法 | |
| Zheng et al. | Low-cost gps-aided lidar state estimation and map building | |
| CN109781098B (zh) | 一种列车定位的方法和系统 | |
| CN103674021A (zh) | 基于捷联惯导与星敏感器的组合导航系统及方法 | |
| CN114485643B (zh) | 一种煤矿井下移动机器人环境感知与高精度定位方法 | |
| Zhou et al. | Kinematic measurement of the railway track centerline position by GNSS/INS/odometer integration | |
| CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN114777812B (zh) | 一种水下组合导航系统行进间对准与姿态估计方法 | |
| CN115479598A (zh) | 基于多传感器融合的定位与建图方法及紧耦合系统 | |
| CN115984463A (zh) | 一种适用于狭窄巷道的三维重建方法和系统 | |
| CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
| Srinara et al. | Performance analysis of 3D NDT scan matching for autonomous vehicles using INS/GNSS/3D LiDAR-SLAM integration scheme | |
| Chen et al. | Knowledge-based indoor positioning based on LiDAR aided multiple sensors system for UGVs | |
| Wang et al. | Barometer assisted smartphone localization for vehicle navigation in multilayer road networks | |
| CN111256708A (zh) | 一种基于射频识别的车载组合导航方法 | |
| CN102288175B (zh) | 一种适用于星敏感器的自主预报方法 |
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 |