CN116105729A - 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 - Google Patents
一种针对野外洞穴森林环境侦察的多传感器融合定位方法 Download PDFInfo
- Publication number
- CN116105729A CN116105729A CN202310162326.3A CN202310162326A CN116105729A CN 116105729 A CN116105729 A CN 116105729A CN 202310162326 A CN202310162326 A CN 202310162326A CN 116105729 A CN116105729 A CN 116105729A
- Authority
- CN
- China
- Prior art keywords
- information
- coordinate system
- imu
- residual
- gnss
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 230000004927 fusion Effects 0.000 title claims abstract description 26
- 230000000007 visual effect Effects 0.000 claims abstract description 57
- 238000005457 optimization Methods 0.000 claims abstract description 19
- 230000008878 coupling Effects 0.000 claims abstract description 9
- 238000010168 coupling process Methods 0.000 claims abstract description 9
- 238000005859 coupling reaction Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000005259 measurement Methods 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 13
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 7
- 230000015556 catabolic process Effects 0.000 claims description 6
- 238000006731 degradation reaction Methods 0.000 claims description 6
- 238000012216 screening Methods 0.000 claims description 6
- 230000008054 signal transmission Effects 0.000 claims description 6
- 230000000593 degrading effect Effects 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 5
- 230000033001 locomotion Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 4
- 238000007781 pre-processing Methods 0.000 claims description 4
- 230000011218 segmentation Effects 0.000 claims description 4
- 239000005436 troposphere Substances 0.000 claims description 4
- 230000009471 action Effects 0.000 abstract description 2
- 238000005516 engineering process Methods 0.000 description 7
- 230000010354 integration Effects 0.000 description 5
- 235000019994 cava Nutrition 0.000 description 3
- 238000005286 illumination Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 239000005433 ionosphere Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- 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
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- 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
- G01C21/1656—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 with passive imaging devices, e.g. cameras
-
- 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/20—Instruments for performing navigational calculations
-
- 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
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明涉及一种针对野外洞穴森林环境侦察的多传感器融合定位方法，属于目标定位技术领域。通过激光惯性GNSS四传感器紧耦合的方式，将激光雷达点云残差、视觉重投影误差、IMU残差、GNSS的伪距、多普勒频移和时钟钟差以因子的形式加入到优化中，充分利用了激光雷达作用范围大、误差小、不受光线影响的优点，视觉充分利用纹理、像素特征信息，IMU高频的优点以及GNSS提供的绝对位置信息和无累积误差的优点，利用激光雷达信息、视觉、IMU信息和GNSS信息优化计算得到可靠精确的高频位置信息。
Description
技术领域
本发明属于目标定位技术领域，具体涉及一种针对野外洞穴森林环境侦察的多传感器融合定位方法。
背景技术
同时定位与建图(Simultaneous Localization and Mapping,SLAM)技术是智能机器人领域的基础技术，能够准确的定位自身位置和感知周围环境是进行导航与探索的前提。
近年来，SLAM技术被广泛研究，出现了使用相机的视觉SLAM技术和使用激光雷达的激光SLAM技术。其中视觉SLAM技术依赖图像信息，因此在纹理弱、亮度暗、快速运动的情况下极易失效，而激光雷达因其可适应各种环境、可探测范围广、且在黑暗或弱纹理环境下仍然能有效获取点云信息的优点，在定位与建图上更稳定；但即便高性能的激光雷达，输出频率也只有10hz，在长走廊、平坦的空地以及墙角处也会出现定位发散的问题，且随之定位时间推移，会产生逐渐增大的累计误差；GNSS卫星导航系统能得到绝对位置，但是易受到屏蔽与干扰；IMU能输出高频的线加速度和角速度，但仅由IMU积分获得的相对位置误差会快速累积。本发明考虑野外侦察的实际场景，针对任务中常见的山洞、森林等低光照、弱纹理且卫星信号易受干扰的环境中卫星导航信息和视觉信息退化的问题，将激光雷达、视觉信息、GNSS卫星导航信息和IMU信息紧耦合，利用一个因子图进行全局优化，提出一种能够自适应切换系统融合状态，获取无缝高频绝对位置精确定位方法，可被应用于战术侦察、洞穴探测等领域。
发明内容
要解决的技术问题
针对野外侦察中的山洞、森林等低光照、弱纹理且卫星信号易受干扰的环境中卫星导航信息和视觉信息退化条件下，视觉定位与卫星定位极易发散的问题，本发明提出一种针对野外洞穴森林环境侦察的多传感器融合定位方法。
技术方案
一种针对野外洞穴森林环境侦察的多传感器融合定位方法，实现多传感器紧耦合定位系统在卫星导航信息和视觉信息退化条件下，能够自适应切换系统融合状态，获取无缝的高频的绝对位置；其特征在于步骤如下：
步骤1：对激光雷达输入的原始点云信息进行预处理，把点云信息进行点云分割并打上标签，剔除其中不可靠的点云信息，对余下的可靠的点云进行特征提取；对视觉图像信息进行特征点提取，通过激光雷达深度信息辅助视觉特征点跟踪；对IMU信息进行预积分处理；使用星历信息对GNSS信息进行可靠性筛选，得到稳定可靠的传感器信息；
步骤2：进行线性插值把激光雷达帧和视觉图像帧通过时间戳对齐；通过视觉的三维运动重建获得载体的相对于初始位置的初始相对位姿；将前几帧的激光雷达点云合并构成局部点云地图，将一帧的点云与局部点云地图进行匹配，根据匹配关系构建残差函数，求解获得载体的初始相对位姿；将视觉初始化相对位姿和激光雷达初始化位姿进行加权融合得到初始相对位姿；
将获得的初始相对位姿与IMU联合初始化得到IMU加速度计与陀螺仪的偏置以及重力矢量的对齐，利用SPP算法通过至少四颗以上的卫星的伪距信息计算出载体粗略的绝对位置，并将齐与相对位置对齐；将GNSS信息的多普勒频移与激光视觉惯性系统得到的相对速度对齐，计算求解激光视觉惯性系统相对位姿的local局部坐标系、ENU东北天坐标系与GNSS绝对位置所在的ECEF地心地固坐标系之间的转换矩阵；
步骤3：计算激光雷达点云残差、视觉重投影误差、IMU预积分残差和GNSS信息的伪距残差、多普勒频移残差、接收机时钟残差并分别作为因子构建因子图优化模型，求解载体的位置信息；选取关键帧并构建滑动窗口对其中所有的状态量
进行优化求解，其中n为滑动窗口中的帧数，m为滑动窗口中所有特征点的数量，
分别为载体的位置、速度和姿态，ba、bω为加速度计和陀螺仪的偏置，δt为接收机时钟钟差，
为接收机时钟钟差变化率，
为相机外参，
为激光雷达外参，ρk,k∈[0,m]为特征点的逆深度，ψ为载体坐标系和ENU坐标系之间的偏航角；
使用视觉特征点可用性状态切换策略，在视觉特征点匹配小于设定阈值或者初始化IMU的陀螺仪的偏置大于设定阈值时，锁定受视觉特征影响的状态量并在因子图中隔离视觉重投影误差因子，系统退化为激光惯性卫星耦合的定位方法；
通过星历信息对卫星GNSS信息进行筛选，当无可用的GNSS信息时，锁定仅受GNSS信息影响的状态量并在因子图中隔离GNSS信息的伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，此时因子图中剩激光雷达点云残差、视觉重投影误差和IMU残差，系统自动切换到激光视觉惯性定位系统，由此解算出的载体相对位姿联合有GNSS信息时解算出的转换阵，依旧可以不间断得获得ENU坐标系下的绝对位姿，待重新接收到可用得GNSS信息时，再对仅受GNSS信息影响的状态量进行解锁，并加入伪距残差因子、多普勒频移残差因子和接收机时钟残差因子进行联合优化，对偏航角ψ、接收机时钟钟差δt、接收机时钟钟差变化率
继续进行优化迭代更新；
步骤4：将步骤3获得的和激光雷达频率相同的位置信息与帧间获取的IMU积分信息统一到同一坐标系下进行加和，获得与IMU信息同频的高频定位信息。
本发明进一步的技术方案：所述的视觉重投影误差残差因子：
其中C表示滑窗中至少被观察到两次的特征集，
为特征点l在第j帧中图像平面上的特征坐标，
为第i帧中特征点在第j帧中的重投影位置，
为第i帧中特征点在第j帧中的图像平面上的重投影特征坐标，
为世界坐标系和第j帧IMU坐标系之间的转换矩阵，
为第j帧IMU坐标系和世界坐标系之间的转换矩阵，
为相机坐标系和IMU坐标系之间的转换矩阵，
为IMU坐标系和世界坐标系之间的转换矩阵，ρl为特征点l的逆深度。
本发明进一步的技术方案：所述的伪距残差因子：
其中ωE为地球自转角速度，tf为卫星信号传输时间，Rz(ωEtf)为地球在卫星信号传输过程中的旋转，Rz(ωEtf)ps为接收机接收到卫星信号时卫星在ECEF坐标系下的实际位置，
表示接收机的位置，c代表光速，δtk为接收机时钟钟差，Δts为卫星时钟误差，
和
分别代表对流层和电流层带来的延迟误差，
为伪距的量测值。
本发明进一步的技术方案：所述的多普勒频移残差因子：
其中λ表示载波信号的波长，
表示从接收机到卫星的单位向量，vs和
分别表示卫星和接收机的速度，
表示接收机时钟钟差变化率，
表示卫星时钟误差漂移率，
为多普勒频移量测值。
本发明进一步的技术方案：所述的接收机时钟残差因子：
其中δtk和δtk-1分别为第k时刻和第k-1时刻的接收机时钟钟差，
为第k-1时刻的接收机时钟钟差变化率，
为第k时刻和第k-1时刻的时间差。
有益效果
本发明提供的一种针对野外洞穴森林环境侦察的多传感器融合定位方法，通过激光惯性GNSS四传感器紧耦合的方式，将激光雷达点云残差、视觉重投影误差、IMU残差、GNSS的伪距、多普勒频移和时钟钟差以因子的形式加入到优化中，充分利用了激光雷达作用范围大、误差小、不受光线影响的优点，视觉充分利用纹理、像素特征信息，IMU高频的优点以及GNSS提供的绝对位置信息和无累积误差的优点，利用激光雷达信息、视觉、IMU信息和GNSS信息优化计算得到可靠精确的高频位置信息。
本发明提出的一种针对野外洞穴森林环境侦察的多传感器融合定位方法，考虑野外侦察的实际场景，针对任务中常见的山洞、森林等低光照、弱纹理且卫星信号易受干扰的环境中卫星导航信息和视觉信息退化的问题，对进出GNSS拒止环境时的状态切换进行研究，在进入拒止环境无可用的GNSS信息时，锁定仅受GNSS信息影响的状态量偏航角ψ、接收机时钟钟差δt和接收机时钟钟差变化率
并在因子图中隔离GNSS信息的伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，系统退化为激光视觉惯性融合的定位方法并利用偏航角推算载体的绝对位置，解决了在GNSS卫星信号少甚至完全拒止的环境下的定位问题。
本发明提出了一种视觉特征点可用性状态切换策略，在视觉特征点匹配小于设定阈值或者初始化IMU的陀螺仪的偏置大于设定阈值时，锁定受视觉特征影响的状态量并在因子图中隔离视觉重投影误差因子，系统退化为激光惯性卫星耦合的定位方法。克服了卫星导航信息和视觉信息退化条件下的定位发散问题，能够自适应切换系统融合状态，进出拒止环境及视觉失真环境仍能获取无缝的高频的绝对位置。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1针对野外洞穴森林环境侦察的多传感器融合定位方法流程图；
图2ECEF坐标系、ENU坐标系和local坐标系关系图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
本发明由传感器信息预处理、多传感器联合初始化、多传感器紧耦合和高频定位信息输出四大部分组成。首先对激光雷达点云信息进行点云分割、提取特征，对视觉图像信息进行特征点提取，通过激光雷达深度信息辅助视觉特征点跟踪，对IMU信息进行预积分处理，并使用星历信息对GNSS信息进行可靠性筛选，得到可用的传感器信息。
视觉的三维运动重建获得载体的初始相对位姿；激光雷达点云信息中提取的特征进行匹配并由此建立残差模型计算载体的初始相对位姿；激光雷达初始位姿和视觉初始位姿进行加权融合。
进行激光视觉IMU联合初始化确定IMU加速度计与陀螺仪的偏置以及重力矢量的对齐，进行激光惯性视觉系统与GNSS信息的联合初始化得到载体的绝对位姿；通过传感器信息可靠性判定确定多传感器融合策略选择融合激光雷达、视觉和IMU三者的量测信息；激光雷达和IMU二者量测信息或者激光雷达、视觉、IMU和GNSS四者的量测信息实现GNSS拒止环境和非拒止环境下的无缝定位，获取载体的绝对位姿信息；最后将IMU信息进行积分并与多传感器融合获得的载体位姿信息相加获得高频的位姿信息。
图1为本发明的流程图：首先对激光雷达、视觉、IMU和GNSS获得的信息进行预处理，对激光雷达获得的点云信息进行点云分割和特征提取，视觉进行特征提取并使用激光雷达深度数据进行特征跟踪，对惯性IMU数据进行预积分，对GNSS信息进行可靠性筛选；然后使用点云提取出的边缘点和面点与局部点云地图进行匹配，构建残差函数计算相对位姿，后与IMU信息进行联合初始化；使用单点定位算法SPP方法计算初始绝对位置并使用多普勒频移与激光惯性系统获得的速度信息对齐，完成四传感器联合初始化；在有可靠的视觉特征信息时，进行联合优化求解载体位姿，在无可用视觉特征信息时，系统退化为激光惯性GNSS定位进行解算。在有可靠的GNSS信息时，进行四传感器联合优化求解载体位姿，在无可靠的GNSS信息时，系统退化为激光视觉惯性定位进行解算。
具体步骤如下：
1、对IMU信息进行预积分处理；从激光雷达获取原始点云信息后，先将点云信息转化为深度图，遍历所有点，检查其相邻的点，满足两点间的角度阈值则继续搜索，将联通的点打上相同的标签，剔除同标签数小于30个的点，由此剔除了不可靠的点云信息，对处理后的点云进行特征提取，提取边缘点和面点；
对视觉图像信息进行特征点提取，通过激光雷达所得到的深度信息来标定视觉系统三角化恢复的深度信息从而辅助视觉特征点跟踪。
从GNSS接收机获取GNSS量测信息后，会对其进行筛选，首先会剔除还未收到其对应星历信息的量测信息，剔除伪距标准差和多普勒频移标准差较大(大于2.0)的不可靠的GNSS信息，最后利用接收机与卫星的仰角大小，去掉仰角较小(小于30度)即受到地球电离层和对流层影响较大的量测信息，剩余的可靠的量测信息用于初始化和优化。
2、进行初始化
进行线性插值把激光雷达帧和视觉图像帧通过时间戳对齐。
通过视觉的三维运动重建获得载体相对于初始位置的初始相对位姿Pc。
将前几帧的激光雷达点云合并构成局部点云地图，将提取到的边缘点和面点与局部点云地图进行匹配，根据匹配关系构建残差函数，边缘点残差函数为：
其中
表示第k+1时刻第i个边缘点匹配点的坐标，
和
表示第k时刻局部地图
点所在线上任意两点。面点残差函数为：
其中
表示第k+1时刻第j个面点匹配点的坐标，
和
表示第k时刻局部地图
点所在面上任意两点。将所有边缘点残差和面点残差相加得到总的激光雷达点云残差模型：
其中a为边缘点的数量，b为面点的数量。最小化ΔL求解获得载体的初始相对位姿Pl；根据视觉特征点的匹配情况将视觉初始化相对位姿Pc和激光雷达初始化位姿Pl进行加权融合得到初始相对位姿P。融合公式如下：
P＝εPc+(1-ε)Pl
其中ε代表视觉特征信息的置信度，n为视觉特征匹配成功数量，nth为所设定阈值，nth一般设置为200。再将获得的初始相对位姿P与IMU联合初始化得到IMU加速度计与陀螺仪的偏置以及重力矢量的对齐。
使用SPP(单点定位)算法，利用伪距和三角测距原理，计算获得接收机的粗略坐标，由于存在时钟钟差且不同GNSS系统的时钟钟差不同，因此需要至少N+3颗卫星的伪距信息才能计算出载体的绝对位置和时钟偏差，其中N为GNSS系统(GPS、北斗、GLONASS、Gialleo等)的数量，利用多普勒频移与激光视觉惯性系统得到的相对速度对齐，获得ENU坐标系与local坐标系系的偏航角ψ；利用计算获得的绝对坐标和偏航角ψ获得ECEF坐标系、ENU坐标系以及local坐标系的转换关系，如图2所示，ENU(东北天)系到ECEF(地心地固)系的转换为：
其中lat为纬度，lon为经度，local(局部)坐标系到ENU(东北天)坐标系的转换为：
3、计算激光雷达点云残差、重投影误差、IMU预积分残差和GNSS信息的伪距残差、多普勒频移残差、接收机时钟残差并分别作为因子构建因子图，选取关键帧并构建滑动窗口对其中所有的状态量
进行优化求解，其中n为滑动窗口中的帧数，
分别为载体的位置、速度和姿态，ba、bω为加速度计和陀螺仪的偏置，δt为接收机时钟钟差，
为接收机时钟钟差变化率，
为激光雷达外参，ψ为载体坐标系和ENU坐标系之间的偏航角。
计算IMU预积分误差作为IMU残差ΔI，步骤2中已计算激光雷达点云残差ΔL。
建立重投影误差残差因子：
其中C表示滑窗中至少被观察到两次的特征集，
为特征点l在第j帧中图像平面上的特征坐标，
为第i帧中特征点在第j帧中的重投影位置，
为第i帧中特征点在第j帧中的图像平面上的重投影特征坐标，
为世界坐标系和第j帧IMU坐标系之间的转换矩阵，
为第j帧IMU坐标系和世界坐标系之间的转换矩阵，
为相机坐标系和IMU坐标系之间的转换矩阵，
为IMU坐标系和世界坐标系之间的转换矩阵，ρl为特征点l的逆深度。
建立伪距残差因子：
其中ωE为地球自转角速度，tf为卫星信号传输时间，Rz(ωEtf)为地球在卫星信号传输过程中的旋转，Rz(ωEtf)ps为接收机接收到卫星信号时卫星在ECEF坐标系下的实际位置，
表示接收机的位置，c代表光速，δtk为接收机时钟钟差，Δts为卫星时钟误差，
和
分别代表对流层和电流层带来的延迟误差，
为伪距的量测值。
建立多普勒频移残差因子：
其中λ表示载波信号的波长，
表示从接收机到卫星的单位向量，vs和
分别表示卫星和接收机的速度，
表示接收机时钟钟差变化率，
表示卫星时钟误差漂移率，
为多普勒频移量测值。
建立接收机时钟残差因子：
其中vtk和δtk-1分别为第k时刻和第k-1时刻的接收机时钟钟差，
为第k-1时刻的接收机时钟钟差变化率，
为第k时刻和第k-1时刻的时间差。
将这六个因子加入到优化项中，建立优化模型：
问题被建模成非线性最小二乘问题，使用梯度下降法对该优化模型进行求解，获得载体的位姿信息。
当无可用的GNSS信息时，锁定仅受GNSS信息影响的状态量偏航角ψ、接收机时钟钟差δt和接收机时钟钟差变化率
并在因子图中隔离GNSS信息的伪距残差因子
多普勒频移残差因子
和接收机时钟残差因子
因子图剩余IMU残差因子、激光雷达点云残差因子和重投影误差残差因子，系统自动退化为激光视觉惯性融合的定位方法并利用GNSS丢失前的偏航角ψ推算载体的绝对位置，待重新接收到可用的GNSS信息时，再对仅受GNSS信息影响的状态量进行解锁并加入伪距残差因子
多普勒频移残差因子
和接收机时钟残差因子
进行联合优化，实现了载体进出拒止环境绝对定位的无缝解算输出。
使用视觉特征点可用性状态切换策略，在视觉特征点匹配小于设定阈值或者初始化IMU的陀螺仪的偏置大于设定阈值时，锁定受视觉特征影响的状态量并在因子图中隔离视觉重投影误差因子，系统退化为激光惯性卫星耦合的定位方法。
4、根据上述里程计获得的与激光雷达频率相同的位置信息计算出local坐标系与ENU坐标系之间的转换矩阵
将帧间获取的IMU积分信息与转换矩阵
进行加和获得与IMU信息同频的高频定位信息。
其中Imu_propagate为帧间的IMU积分信息，odometry_enu_high为高频定位信息。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。
Claims (5)
1.一种针对野外洞穴森林环境侦察的多传感器融合定位方法，实现多传感器紧耦合定位系统在卫星导航信息和视觉信息退化条件下，能够自适应切换系统融合状态，获取无缝的高频的绝对位置；其特征在于步骤如下：
步骤1：对激光雷达输入的原始点云信息进行预处理，把点云信息进行点云分割并打上标签，剔除其中不可靠的点云信息，对余下的可靠的点云进行特征提取；对视觉图像信息进行特征点提取，通过激光雷达深度信息辅助视觉特征点跟踪；对IMU信息进行预积分处理；使用星历信息对GNSS信息进行可靠性筛选，得到稳定可靠的传感器信息；
步骤2：进行线性插值把激光雷达帧和视觉图像帧通过时间戳对齐；通过视觉的三维运动重建获得载体的相对于初始位置的初始相对位姿；将前几帧的激光雷达点云合并构成局部点云地图，将一帧的点云与局部点云地图进行匹配，根据匹配关系构建残差函数，求解获得载体的初始相对位姿；将视觉初始化相对位姿和激光雷达初始化位姿进行加权融合得到初始相对位姿；
将获得的初始相对位姿与IMU联合初始化得到IMU加速度计与陀螺仪的偏置以及重力矢量的对齐，利用SPP算法通过至少四颗以上的卫星的伪距信息计算出载体粗略的绝对位置，并将齐与相对位置对齐；将GNSS信息的多普勒频移与激光视觉惯性系统得到的相对速度对齐，计算求解激光视觉惯性系统相对位姿的local局部坐标系、ENU东北天坐标系与GNSS绝对位置所在的ECEF地心地固坐标系之间的转换矩阵；
步骤3：计算激光雷达点云残差、视觉重投影误差、IMU预积分残差和GNSS信息的伪距残差、多普勒频移残差、接收机时钟残差并分别作为因子构建因子图优化模型，求解载体的位置信息；选取关键帧并构建滑动窗口对其中所有的状态量
进行优化求解，其中n为滑动窗口中的帧数，m为滑动窗口中所有特征点的数量，
分别为载体的位置、速度和姿态，ba、bω为加速度计和陀螺仪的偏置，δt为接收机时钟钟差，
为接收机时钟钟差变化率，
为相机外参，
为激光雷达外参，ρk,k∈[0,m]为特征点的逆深度，ψ为载体坐标系和ENU坐标系之间的偏航角；
使用视觉特征点可用性状态切换策略，在视觉特征点匹配小于设定阈值或者初始化IMU的陀螺仪的偏置大于设定阈值时，锁定受视觉特征影响的状态量并在因子图中隔离视觉重投影误差因子，系统退化为激光惯性卫星耦合的定位方法；
通过星历信息对卫星GNSS信息进行筛选，当无可用的GNSS信息时，锁定仅受GNSS信息影响的状态量并在因子图中隔离GNSS信息的伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，此时因子图中剩激光雷达点云残差、视觉重投影误差和IMU残差，系统自动切换到激光视觉惯性定位系统，由此解算出的载体相对位姿联合有GNSS信息时解算出的转换阵，依旧可以不间断得获得ENU坐标系下的绝对位姿，待重新接收到可用得GNSS信息时，再对仅受GNSS信息影响的状态量进行解锁，并加入伪距残差因子、多普勒频移残差因子和接收机时钟残差因子进行联合优化，对偏航角ψ、接收机时钟钟差δt、接收机时钟钟差变化率
继续进行优化迭代更新；
步骤4：将步骤3获得的和激光雷达频率相同的位置信息与帧间获取的IMU积分信息统一到同一坐标系下进行加和，获得与IMU信息同频的高频定位信息。
2.根据权利要求1所述的针对野外洞穴森林环境侦察的多传感器融合定位方法，其特征在于所述的视觉重投影误差残差因子：
其中C表示滑窗中至少被观察到两次的特征集，
为特征点l在第j帧中图像平面上的特征坐标，
为第i帧中特征点在第j帧中的重投影位置，
为第i帧中特征点在第j帧中的图像平面上的重投影特征坐标，
为世界坐标系和第j帧IMU坐标系之间的转换矩阵，
为第j帧IMU坐标系和世界坐标系之间的转换矩阵，
为相机坐标系和IMU坐标系之间的转换矩阵，
为IMU坐标系和世界坐标系之间的转换矩阵，ρl为特征点l的逆深度。
3.根据权利要求1所述的针对野外洞穴森林环境侦察的多传感器融合定位方法，其特征在于所述的伪距残差因子：
其中ωE为地球自转角速度，tf为卫星信号传输时间，Rz(ωEtf)为地球在卫星信号传输过程中的旋转，Rz(ωEtf)ps为接收机接收到卫星信号时卫星在ECEF坐标系下的实际位置，
表示接收机的位置，c代表光速，δtk为接收机时钟钟差，Δts为卫星时钟误差，
和
分别代表对流层和电流层带来的延迟误差，
为伪距的量测值。
4.根据权利要求1所述的针对野外洞穴森林环境侦察的多传感器融合定位方法，其特征在于所述的多普勒频移残差因子：
其中λ表示载波信号的波长，
表示从接收机到卫星的单位向量，vs和
分别表示卫星和接收机的速度，
表示接收机时钟钟差变化率，
表示卫星时钟误差漂移率，
为多普勒频移量测值。
5.根据权利要求1所述的针对野外洞穴森林环境侦察的多传感器融合定位方法，其特征在于所述的接收机时钟残差因子：
其中δtk和δtk-1分别为第k时刻和第k-1时刻的接收机时钟钟差，
为第k-1时刻的接收机时钟钟差变化率，
为第k时刻和第k-1时刻的时间差。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310162326.3A CN116105729A (zh) | 2023-02-24 | 2023-02-24 | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310162326.3A CN116105729A (zh) | 2023-02-24 | 2023-02-24 | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116105729A true CN116105729A (zh) | 2023-05-12 |
Family
ID=86258012
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310162326.3A Pending CN116105729A (zh) | 2023-02-24 | 2023-02-24 | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116105729A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116817928A (zh) * | 2023-08-28 | 2023-09-29 | 北京交通大学 | 基于因子图优化的卫导/惯导列车多源融合定位的方法 |
-
2023
- 2023-02-24 CN CN202310162326.3A patent/CN116105729A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116817928A (zh) * | 2023-08-28 | 2023-09-29 | 北京交通大学 | 基于因子图优化的卫导/惯导列车多源融合定位的方法 |
| CN116817928B (zh) * | 2023-08-28 | 2023-12-01 | 北京交通大学 | 基于因子图优化的卫导/惯导列车多源融合定位的方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110412635B (zh) | 一种环境信标支持下的gnss/sins/视觉紧组合方法 | |
| WO2021248636A1 (zh) | 一种自动驾驶对象探测和定位系统及方法 | |
| WO2016182964A1 (en) | Adaptive positioning system | |
| US11875519B2 (en) | Method and system for positioning using optical sensor and motion sensors | |
| CN114199259A (zh) | 一种基于运动状态与环境感知的多源融合导航定位方法 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
| CN115435779A (zh) | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 | |
| CN116105729A (zh) | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 | |
| Wang et al. | UGV‐UAV robust cooperative positioning algorithm with object detection | |
| Wen et al. | 3D LiDAR aided GNSS real-time kinematic positioning | |
| US20240094415A1 (en) | System and method for detecting outliers in gnss observations | |
| Wang et al. | GIVE: A tightly coupled RTK-inertial–visual state estimator for robust and precise positioning | |
| CN115523920B (zh) | 一种基于视觉惯性gnss紧耦合的无缝定位方法 | |
| Ćwian et al. | GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments | |
| CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及系统 | |
| CN116482735A (zh) | 一种受限空间内外高精度定位方法 | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| Liu et al. | 3D LiDAR aided GNSS NLOS mitigation for reliable GNSS-RTK positioning in urban canyons | |
| Volden et al. | Development and experimental evaluation of visual-acoustic navigation for safe maneuvering of unmanned surface vehicles in harbor and waterway areas | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| Li et al. | Multi-target multi-platform sensor registration in geodetic coordinates | |
| Amami | The Integration of Stand-Alone GPS Code Positioning, Carrier Phase Delta Positioning & MEMS-Based INS | |
| CN114646993A (zh) | 一种基于gnss、视觉以及imu进行精确定位的数据融合算法 |
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 |