CN116182837A - 基于视觉激光雷达惯性紧耦合的定位建图方法 - Google Patents
基于视觉激光雷达惯性紧耦合的定位建图方法 Download PDFInfo
- Publication number
- CN116182837A CN116182837A CN202310253115.0A CN202310253115A CN116182837A CN 116182837 A CN116182837 A CN 116182837A CN 202310253115 A CN202310253115 A CN 202310253115A CN 116182837 A CN116182837 A CN 116182837A
- Authority
- CN
- China
- Prior art keywords
- radar
- imu
- point
- visual
- representing
- 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
- 230000000007 visual effect Effects 0.000 title claims abstract description 70
- 238000000034 method Methods 0.000 title claims abstract description 69
- 238000013507 mapping Methods 0.000 title claims abstract description 26
- 230000008878 coupling Effects 0.000 title claims abstract description 18
- 238000010168 coupling process Methods 0.000 title claims abstract description 18
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 18
- 230000010354 integration Effects 0.000 claims abstract description 37
- 238000006731 degradation reaction Methods 0.000 claims abstract description 33
- 230000015556 catabolic process Effects 0.000 claims abstract description 30
- 238000005457 optimization Methods 0.000 claims abstract description 19
- 230000008569 process Effects 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 42
- 230000001133 acceleration Effects 0.000 claims description 32
- 238000013519 translation Methods 0.000 claims description 24
- 238000005259 measurement Methods 0.000 claims description 16
- 238000000605 extraction Methods 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 12
- 238000007781 pre-processing Methods 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 9
- 238000000354 decomposition reaction Methods 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 5
- 238000004458 analytical method Methods 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 5
- 125000000205 L-threonino group Chemical group [H]OC(=O)[C@@]([H])(N([H])[*])[C@](C([H])([H])[H])([H])O[H] 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 238000009472 formulation Methods 0.000 claims description 2
- 239000000203 mixture Substances 0.000 claims description 2
- 238000013461 design Methods 0.000 abstract description 3
- 238000004519 manufacturing process Methods 0.000 abstract 1
- 230000006870 function Effects 0.000 description 10
- 230000000694 effects Effects 0.000 description 4
- 230000004927 fusion Effects 0.000 description 4
- 238000012423 maintenance Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 238000005286 illumination Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 241000287196 Asthenes Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- HOWHQWFXSLOJEF-MGZLOUMQSA-N systemin Chemical compound NCCCC[C@H](N)C(=O)N[C@@H](CCSC)C(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H]([C@@H](C)O)C(=O)N[C@@H](CC(O)=O)C(=O)OC(=O)[C@@H]1CCCN1C(=O)[C@H]1N(C(=O)[C@H](CC(O)=O)NC(=O)[C@H](CCCN=C(N)N)NC(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H]2N(CCC2)C(=O)[C@H]2N(CCC2)C(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H](CCC(N)=O)NC(=O)[C@@H](NC(=O)[C@H](C)N)C(C)C)CCC1 HOWHQWFXSLOJEF-MGZLOUMQSA-N 0.000 description 1
- 108010050014 systemin Proteins 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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)
- Navigation (AREA)
Abstract
本发明涉及无人机、视觉激光雷达、里程计和定位建图技术领域，为提出一种基于视觉激光雷达惯性紧耦合的定位建图方法,以得到高精度的定位轨迹和周围环境的几何结构信息，且在典型激光雷达退化场景下可以保持较好的鲁棒性与精度，本发明，基于视觉激光雷达惯性紧耦合的定位建图方法，首先分别对自主定位与建图、激光雷达自主定位与建图、视觉与惯性测量单元IMU三种传感器数据进行处理；其次，构建视觉重投影、IMU预积分和雷达几何约束，联合三种约束建立非线性优化问题；最后，利用优化完成后的位姿与当前帧雷达点云更新体素地图，为后续过程提供几何结构信息。本发明主要应用于无人机设计制造场合。
Description
技术领域
本发明属于定位建图领域，具体涉及基于视觉激光雷达惯性紧耦合的自主定位与建图技术、基于自适应权值调整的视觉雷达惯性里程计以及基于自适应体素的地图维护方法。
背景技术
近年来，微小型无人机在情报搜集、搜索救援、侦察预警、精确打击等方面正发挥着越来越重要的作用，已经成为未来军事战场中不可或缺的力量。传统无人机在执行任务时，往往需要依赖全球导航卫星系统进行定位，存在诸多局限，如在导航信号杂乱的环境或低空飞行时，会受到障碍物的影响，严重影响定位精度，且无法在卫星导航拒止复杂环境下进行应用，同时全球导航卫星系统无法提供周围的环境信息，使其无法应用在混乱环境或室内密闭环境中的快速飞行。因此微小型无人机在各种任务的执行中主要通过自身机载传感器实现实时的自主定位与建图(Simultaneous Localization And Mapping,SLAM)。
针对视觉SLAM和激光雷达SLAM方法，国内外学者进行了大量的研究。视觉SLAM方面，2015年萨拉戈萨大学Mur-Artal等人提出了著名的单目视觉SLAM系统ORB-SLAM，其采用的ORB特征点加速了局部地图与当前帧中特征点的匹配，该方案还加入了回环检测功能，存在轨迹闭环时可以通过合理分配累计误差来获得更高的位姿估计精度。在ORB-SLAM的基础上，其后续又提出了适用单目、双目、RGBD相机的ORB-SLAM2和适用于视觉、视觉惯性融合的ORB-SLAM3，将视觉与惯性测量单元(Inertial Measurement Unit，IMU)进行传感器融合以提升定位方法的性能。2017年香港科技大学Aerial Robotics Group提出了VINS-Mono，其使用基于优化的滑动窗口来提供高精度的视觉惯性定位，具有高效的IMU预积分、偏差校正、在线外参校准、故障检测和恢复、闭环检测、全局姿态图优化、地图合并、姿态图重用等功能，随后该团队又将其改进为多源传感器紧耦合优化框架VINS-Fusion、GVINS和VINS-Fisheye。激光雷达SLAM方面，卡耐基梅隆大学学者Ji Zhang等人于2014年提出了LOAM，是目前使用最广泛的基于特征的激光雷达SLAM算法之一。该方法从点云中提取分布在边缘线与平面上的特征点，通过计算点云间点线、点面距离实现点云匹配。通过帧间匹配与地图层匹配相结合得到了兼具实时性与精度的位姿估计。2019年MIT学者T.Shan等人基于LOAM提出了LeGO-LOAM，在点云匹配环节使用点云聚类标记特征，提高了匹配的精度，同时在后端加入了因子图优化实现回环检测。2020年T.Shan等人又提出了一个基于因子图优化的激光雷达惯性自主导航方法LIO-SAM，该方法融合了IMU预积分约束与LOAM中提出的地图层匹配约束，结合关键帧策略实现实时状态估计。
发明内容
为克服现有技术的不足，本发明旨在提出一种基于视觉激光雷达惯性紧耦合的定位建图方法。使用本方法可以得到高精度的定位轨迹和周围环境的几何结构信息，且在典型激光雷达退化场景(如长走廊)场景下可以保持较好的鲁棒性与精度。为此，本发明采取的技术方案是，基于视觉激光雷达惯性紧耦合的定位建图方法，首先分别对自主定位与建图、激光雷达自主定位与建图、视觉与惯性测量单元IMU三种传感器数据进行处理，对IMU测量数据进行积分得到高频位姿估计和预积分值；对自主定位与建图中的视觉图像提取特征点并进行匹配与跟踪，接着利用投影到相机坐标系下的激光雷达点云，恢复视觉特征点的深度信息；对采集到的激光雷达点云进行运动畸变校正，采用几何特征提取算法，对点云数据中的线面特征进行提取，与局部地图中的点云进行匹配，构建雷达几何因子；其次，构建视觉重投影、IMU预积分和雷达几何约束，联合三种约束建立非线性优化问题，对雷达几何约束的Hessian矩阵进行退化分析，调整各约束权值，利用列文伯格-马夸尔特方法进行位姿求解；最后，利用优化完成后的位姿与当前帧雷达点云更新体素地图，根据输入点云的几何性质自适应调整体素大小，为后续过程提供几何结构信息。
具体步骤如下：
第一部分，传感器数据预处理：
(1)IMU数据预处理
接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，IMU的测量模型表示为:
其中，
和/>
分别为tk时刻IMU测量得到的三轴加速度和三轴角速度，/>
和/>
为tk时刻加速度与角速度的实际值，/>
和/>
分别为tk时刻加速度计偏置和陀螺仪偏置，/>
表示tk时刻从世界系到机体系的旋转矩阵，g表示世界系下的重力矢量，na与ng为IMU测量噪声；
然后根据IMU的输入积分获得高频状态预测，tk与tk+1时刻之间的离散运动方程表示为：
其中，
分别表示tk+1时刻IMU测量得到的三轴加速度和三轴角速度，/>
为tk时刻从机体系到世界系的旋转矩阵，/>
为tk+1时刻从机体系到世界系的旋转矩阵，将离散运动方程中的状态量与IMU的测量数据分离，将原本世界系下的积分变为相邻两帧间相对运动的积分，在离散运动方程两边同时乘以从世界系到机体系的旋转矩阵/>
或者其对应的旋转四元数/>
得下式：
其中，
即为tk到tk+1时刻之间的IMU预积分增量信息，分别表示以tk时刻机体系为参考坐标系到tk+1时刻的平移增量、速度增量和tk+1时刻到tk时刻的旋转四元数，具体离散形式表达式为：
其中，
分别表示从tk时刻到ti+1时刻的平移、速度和旋转预积分量，
分别表示从tk时刻到ti时刻的平移、速度和旋转预积分量，/>
为利用中值法将当前机体系下的测量加速度和角速度转换至tk时刻机体系的加速度和角速度，通过上式不断迭代即可得到从tk时刻到tk+1时刻的平移、速度和旋转预积分量；
(2)图像数据处理
使用角点检测来获取图像特征，针对相机获取的图像信息，对图像提取特征点：图像上有一个像素矩形向多个方向滑动，如果像素矩形灰度没有变化，则认为窗口内不存在角点；如果像素矩形沿某一方向移动灰度变化很大，沿另一方向灰度变化不大，则认为像素矩形内图像为一条边；若像素矩形图像沿任何方向移动，灰度值均产生变化，则认为图像中存在角点；
(3)雷达几何特征点提取
对于单帧雷达点云数据的一点Pi＝(xi,yi,zi)，xi,yi,zi为点Pi在雷达坐标系中的坐标，按照平滑度ci将其定义为线特征与平面特征，其计算公式如下：
其中，S为Pi的邻域点集合，xj为Pi的一个邻域点，|S|为邻域点数量，||Pi||为Pi到雷达坐标系原点的距离，当ci小于某一阈值的时候，认为该点是平面特征点；当ci大于某一阈值的时候，认为该点是线特征点；
第二部分，约束建立与状态求解：对于一个视觉激光雷达惯性紧耦合系统而言，系统的状态估计问题建模为一个最大后验估计问题：
其中，XLVIO表示视觉雷达惯性里程计的状态向量，
表示视觉雷达惯性里程计状态的最大后验估计，u表示IMU采集的加速度和角速度信息，zL表示雷达观测信息，zV表示视觉观测，arg max(·)表示求使得括号内的值最大的参数的函数，p(·)表示概率密度函数，该最大后验概率估计问题可通过贝叶斯公式转化为一个非线性最小二乘问题，如下式所示：
其中，arg min(·)表示求使得括号内的值最大的参数的函数，rB为IMU预积分残差，k为关键帧编号，
为IMU预积分协方差，rC为视觉重投影残差，(l,j)表示第l个特征点在第j个相机帧中被观测组成的视觉观测，C表示视觉观测的集合，/>
表示视觉观测协方差，μ为雷达退化权值，rL为雷达几何残差，s表示第s个雷达特征点，/>
表示雷达特征的集合，σs为雷达观测的协方差，利用L-M法对该非线性最小二乘问题进行迭代求解即可得到位姿状态；
(1)IMU预积分残差
IMU预积分约束rB为通过现有状态求得的相机两帧之间包括位置、姿态、速度、偏置的状态变量和通过IMU预积分求得的两帧之间的状态变量之间的残差，如下式所示：
其中，
为通过IMU预积分求得的两帧之间的位置、速度和姿态的增量残差，/>
为两帧之间IMU加速度计偏置和陀螺仪偏置残差，[·]xyz表示取四元数的虚部，/>
表示IMU预积分姿态增量四元数的逆，/>
和/>
分别为tk+1时刻加速度计偏置和陀螺仪偏置；
(2)视觉重投影残差
由于估计值与实际位置的误差，实际位置下投影的特征点与原来保存的特征点重投影会产生误差rC，该误差被定义为视觉特征的重投影误差，将特征点Pl从第i帧相机坐标系Ci通过两帧之间的位姿转换关系，转换到第j帧相机坐标系Cj下的像素坐标，与实际观察到的第j帧相机坐标系下的特征点的像素坐标之间的像素值的差的马氏距离，即为视觉特征重投影误差，定义视觉误差项为：
其中，
为特征点Pl在第j帧相机坐标系Cj下的归一化坐标，/>
是特征点Pl从第i帧相机坐标系通过位姿关系投影到第j帧相机坐标系的归一化坐标；
(3)雷达几何残差
设点
为平面点特征集合中的一点，先将其投影至世界坐标系，记作/>
然后在局部地图中搜索与/>
最近的三个点Pa,Pb,Pc，进而可以构建点到面的几何残差为：
(4)雷达退化处理
对于非线性的雷达残差rL，进行一阶泰勒展开，如下式所示：
其中，
为rL在线性化工作点的值，JL为雷达残差的雅可比矩阵，ΔXLVIO表示状态增量，则雷达残差部分对应的Hessian矩阵为/>
，对其进行奇异值分解找到其最小特征值λmin，设定经验阈值thres，当Hessian矩阵的特征值大于这个阈值时，认为未发生退化，赋予雷达残差权重μ1；当Hessian矩阵的特征值大于这个阈值时，认为发生退化，赋予雷达残差权重μ2，即：
自适应体素地图管理：针对每一帧特征点云，首先按其世界坐标系中的坐标位置将其分配到固定大小的体素中，然后针对每个体素中的点
按照以下方式计算其均值/>
与协方差cov；
其中，N为体素中的雷达特征点数量，求得体素中的雷达特征点云协方差后，对其进行特征值分解，得到按照小到达依次排列的三个特征值λ1,λ2,λ3，若λ2大于λ1一定的倍数，则停止继续拆分体素；若λ2小于或者等于λ1一定的倍数，但体素大小未达到最小，则也停止拆分体素，若体素大小未达到最小，则继续对子体素内的特征点云进行均值和协方差的计算并进行体素的拆分，直至其特征值比例不满足要求或者体素大小已达到最小。
本发明的特点及有益效果是：
1、本发明提出了视觉激光雷达惯性紧耦合的定位建图方法，首先在前端对IMU数据处理后得到高频IMU预测位姿和帧间预积分，对视觉图像进行角点提取与匹配得到帧间特征对应关系，对雷达原始点云基于几何特征提取得到雷达平面特征点；在后端位姿解算利用L-M法迭代优化求解得到低频视觉激光雷达惯性里程计位姿，最后与IMU预测位姿紧耦合输出高频位姿里程计位姿。在不同的场景下均具有较稳定的效果，能够很好的平衡时间效率和精度之间的要求，针对室外大环境以及室内密闭环境可以在保证实时性的同时建立一致性良好的地图。
2、本发明设计了基于自适应权值调整的视觉雷达惯性里程计，利用视觉重投影约束、雷达几何约束和IMU预积分约束构建非线性优化问题，然后针对雷达约束对应的雅可比矩阵构建Hessian矩阵，对其进行特征值分解，以最小特征值作为退化判据，设置经验阈值调整约束权重，采取滑动窗口策略优化求解无人机位姿状态，实现不依赖外部传感器的快速精确定位，在约束充足的场景下，该方法与最先进的框架定位精度相当，在约束不足易导致退化的场景下，仍可以表现出高鲁棒性。
3、本发明设计了一个基于自适应体素的地图维护方法，根据雷达点云在空间中的三维分布，对雷达特征点云进行体素化处理，构建基于八叉树数据结构自适应体素地图。通过对每个体素中的雷达点云的均值和协方差计算，分析其三维空间几何性质，并作为体素细分的依据，直到每个体素中存储的均值与协方差均能很好地反应雷达特征点云的空间几何性质，在提供良好特征约束的同时，减少维护地图的计算资源与内存消耗。
附图说明：
图1为无人机硬件实物平台；
图2为基于视觉激光雷达惯性紧耦合系统结构图；
图3为走廊场景下视觉角点示意图；
图4为走廊场景下的雷达特征点示意图；
图5为本发明与主流方法在SubT仿真环境的定位轨迹误差指标对比图；
图6为本发明与主流方法在走廊退化场景下的定位轨迹对比图；
图7为本发明在丛林飞行中的定位与建图效果示意图。
具体实施方式
视觉SLAM方法无需具备环境先验信息即可完成定位工作，具备成本低、通用性强等优点。但视觉自主定位也有显著的缺陷，比如受到光照、纹理的限制，长时间运行容易产生估计漂移现象，很大程度上影响了无人机在复杂环境下运行的可靠性。激光雷达SLAM方法具有更宽的视场范围且不受光照变化影响，能直接测得环境的距离信息，适用场景比视觉SLAM方法更为广泛，精度也更为准确。但在缺乏几何特征的退化场景下难以进行有效的局部和全局点云匹配，造成定位和建图精度下降。因此，通过三种传感器的融合充分利用视觉采集稠密的环境信息、IMU测量高频运动信息和激光雷达精确测量环境距离信息的优势，研制一套鲁棒高精度的多传感器融合定位系统，对无人机在导航拒止环境下的自主飞行至关重要。
本发明技术方案是，基于视觉激光雷达惯性紧耦合的定位建图方法，
首先分别对三种传感器数据进行处理，对IMU测量数据进行积分得到高频位姿估计和预积分值；对视觉图像提取特征点并进行匹配与跟踪，接着利用投影到相机坐标系下的激光雷达点云，恢复视觉特征点的深度信息；对采集到的激光雷达点云进行运动畸变校正，采用几何特征提取算法，对点云数据中的线面特征进行提取，与局部地图中的点云进行匹配，构建雷达几何因子。其次，构建视觉重投影、IMU预积分和雷达几何约束，联合三种约束建立非线性优化问题，对雷达几何约束的Hessian矩阵进行退化分析，调整各约束权值，利用列文伯格-马夸尔特(Levenberg-Marquardt，L-M)方法进行位姿求解。最后，利用优化完成后的位姿与当前帧雷达点云更新体素地图，根据输入点云的几何性质自适应调整体素大小，在节约内存资源的同时，为后续过程提供有效几何结构信息。
本发明提出的方法，旨在解决单一传感器退化场景下的鲁棒准确定位建图问题，为此，本发明采用的技术方案是基于视觉激光雷达惯性紧耦合的定位建图技术，具体内容分为以下三部分：
第一部分，传感器数据预处理：
(1)IMU数据预处理
接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，IMU的测量模型可以表示为:
其中，
和/>
分别为tk时刻IMU测量得到的三轴加速度和三轴角速度，/>
和/>
为tk时刻加速度与角速度的实际值，/>
和/>
分别为tk时刻加速度计偏置和陀螺仪偏置，/>
表示tk时刻从世界系到机体系的旋转矩阵，g表示世界系下的重力矢量，na与ng为IMU测量噪声。
然后根据IMU的输入积分获得高频状态预测，tk与tk+1时刻之间的离散运动方程可以表示为：
其中，
分别表示tk+1时刻IMU测量得到的三轴加速度和三轴角速度，/>
为tk时刻从机体系到世界系的旋转矩阵，/>
为tk+1时刻从机体系到世界系的旋转矩阵。为了节约计算量，将离散运动方程中的状态量与IMU的测量数据分离，将原本世界系下的积分变为相邻两帧间相对运动的积分，使得这个积分量只与IMU测量值有关，而与积分起始点无关。因此，在离散运动方程两边同时乘以从世界系到机体系的旋转矩阵/>
(或者其对应的旋转四元数/>
)可得下式：
其中，
即为tk到tk+1时刻之间的IMU预积分增量信息，分别表示以tk时刻机体系为参考坐标系到tk+1时刻的平移增量、速度增量和tk+1时刻到tk时刻的旋转四元数，具体离散形式表达式为：
其中，
分别表示从tk时刻到ti+1时刻的平移、速度和旋转预积分量，
分别表示从tk时刻到ti时刻的平移、速度和旋转预积分量，/>
为利用中值法将当前机体系下的测量加速度和角速度转换至tk时刻机体系的加速度和角速度，通过上式不断迭代即可得到从tk时刻到tk+1时刻的平移、速度和旋转预积分量。
(2)图像数据处理
为充分利用图像中的环境信息，需要提取图像上区分度较明显的特征，通常对图像中的角点信息或边缘信息进行提取，本发明提出的方法主要使用角点检测来获取图像特征，针对相机获取的图像信息，对图像提取特征点。特征点选取Harris角点，其相对于ORB、SIFT、SURF等特征点具有速度更快、计算量适中的优势，相对于FAST特征点具有精度更高、具备旋转不变性等优势，是一种计算时间及提取精度适中的特征点。
Harris角点的基本思想是图像上有一个像素矩形向多个方向滑动，如果像素矩形灰度没有变化，则认为窗口内不存在角点；如果像素矩形沿某一方向移动灰度变化很大，沿另一方向灰度变化不大，则认为像素矩形内图像为一条边；若像素矩形图像沿任何方向移动，灰度值均产生变化，则认为图像中存在角点。
(3)雷达几何特征点提取
不同于相机传感器采集的丰富的像素信息，激光雷达只能采集到周围环境中稀疏的物体信息，因此本发明提出的方法从点云的空间几何特征寻找可靠的雷达特征提取方法。
对于单帧雷达点云数据的一点Pi＝(xi,yi,zi)，xi,yi,zi为点Pi在雷达坐标系中的坐标，可按照平滑度ci将其定义为线特征与平面特征，其计算公式如下：
其中，S为Pi的邻域点集合，xj为Pi的一个邻域点，|S|为邻域点数量，||Pi||为Pi到雷达坐标系原点的距离。当ci小于某一阈值的时候，认为该点是平面特征点；当ci大于某一阈值的时候，认为该点是线特征点。本方法中只采用了平面特征点作为雷达几何特征点。
第二部分，约束建立与状态求解：对于一个视觉激光雷达惯性紧耦合系统而言，系统的状态估计问题可以建模为一个最大后验估计问题：
其中，XLVIO表示视觉雷达惯性里程计的状态向量，
表示视觉雷达惯性里程计状态的最大后验估计，u表示IMU采集的加速度和角速度信息，zL表示雷达观测信息，zV表示视觉观测，arg max(·)表示求使得括号内的值最大的参数的函数，p(·)表示概率密度函数。该最大后验概率估计问题可通过贝叶斯公式转化为一个非线性最小二乘问题，如下式所示：
其中，arg min(·)表示求使得括号内的值最大的参数的函数，rB为IMU预积分残差，k为关键帧编号，
为IMU预积分协方差，rC为视觉重投影残差，(l,j)表示第l个特征点在第j个相机帧中被观测组成的视觉观测，C表示视觉观测的集合，/>
表示视觉观测协方差，μ为雷达退化权值，rL为雷达几何残差，s表示第s个雷达特征点，/>
表示雷达特征的集合，σs为雷达观测的协方差，利用L-M法对该非线性最小二乘问题进行迭代求解即可得到位姿状态。
(1)IMU预积分残差
IMU预积分约束rB为通过现有状态求得的相机两帧之间位置、姿态、速度、偏置等状态变量和通过IMU预积分求得的两帧之间的状态变量之间的残差，如下式所示：
其中，
为通过IMU预积分求得的两帧之间的位置、速度和姿态的增量残差，/>
为两帧之间IMU加速度计偏置和陀螺仪偏置残差，[·]xyz表示取四元数的虚部，/>
表示IMU预积分姿态增量四元数的逆，/>
和/>
分别为tk+1时刻加速度计偏置和陀螺仪偏置。
(2)视觉重投影残差
通过视觉特征点提取与视觉重投影，可以有效地感知外部环境并利用与环境的相对位置实现长期鲁棒的位置估计。由于估计值与实际位置的误差，实际位置下投影的特征点与原来保存的特征点重投影会产生误差rC，该误差可被定义为视觉特征的重投影误差。将特征点Pl从第i帧相机坐标系Ci通过两帧之间的位姿转换关系，转换到第j帧相机坐标系Cj下的像素坐标，与实际观察到的第j帧相机坐标系下的特征点的像素坐标之间的像素值的差的马氏距离，即为视觉特征重投影误差。可定义视觉误差项为：
其中，
为特征点Pl在第j帧相机坐标系Cj下的归一化坐标，/>
是特征点Pl从第i帧相机坐标系通过位姿关系投影到第j帧相机坐标系的归一化坐标。
(3)雷达几何残差
设点
为平面点特征集合中的一点，先将其投影至世界坐标系，记作/>
然后在局部地图中搜索与/>
最近的三个点Pa,Pb,Pc，进而可以构建点到面的几何残差为：
(4)雷达退化处理
针对激光雷达退化场景，本质上是雷达观测所提供的某个自由度上的约束缺失或者较为稀少。在构建无人机状态优化问题时，每个雷达观测提供的约束对应优化方程系数矩阵中的若干行，因此通过对雷达约束构成的优化问题系数矩阵进行退化分析，作为雷达残差权值调整的退化依据。
对于非线性的雷达残差rL，进行一阶泰勒展开，如下式所示：
其中，
为rL在线性化工作点的值，JL为雷达残差的雅可比矩阵，ΔXLVIO表示状态增量，则雷达残差部分对应的Hessian矩阵为/>
对其进行奇异值分解找到其最小特征值λmin，设定经验阈值thres，当Hessian矩阵的特征值大于这个阈值时，认为未发生退化，赋予雷达残差权重μ1；当Hessian矩阵的特征值大于这个阈值时，认为发生退化，赋予雷达残差权重μ2，即：
自适应体素地图管理：本发明使用基于八叉树结构的自适应体素地图实现高效的地图存储，具体针对每一帧特征点云，首先按其世界坐标系中的坐标位置将其分配到固定大小的体素中，然后针对每个体素中的点
按照以下方式计算其均值/>
与协方差cov。
其中，N为体素中的雷达特征点数量，求得体素中的雷达特征点云协方差后，对其进行特征值分解，得到按照小到达依次排列的三个特征值λ1,λ2,λ3，若λ2大于λ1一定的倍数，则停止继续拆分体素；若λ2小于或者等于λ1一定的倍数，但体素大小未达到最小，则也停止拆分体素，若体素大小未达到最小，则继续对子体素内的特征点云进行均值和协方差的计算并进行体素的拆分，直至其特征值比例不满足要求或者体素大小已达到最小。
以下结合附图对本发明的技术方案作进一步说明。
本发明提出了一种基于视觉激光雷达惯性紧耦合的定位建图方法。使用本方法可以得到高精度的定位轨迹和周围环境的几何结构信息，且在典型激光雷达退化场景(如长走廊)场景下可以保持较好的鲁棒性与精度。本方法选用的传感器为Velodyne vlp-16机械式激光雷达，Intel Realsense D455相机和pixhawk飞控内置的IMU，无人机硬件平台如图1所示。视觉激光雷达惯性紧耦合SLAM算法，分为三个部分：传感器数据预处理、约束建立与状态求解以及自适应体素地图管理。结构如图2所示：传感器数据预处理部分对IMU数据处理后得到高频IMU预测位姿和帧间预积分；对视觉图像进行角点提取与匹配得到帧间特征对应关系，走廊场景视觉特征如图3所示；对雷达原始点云基于几何特征提取得到雷达平面特征点，走廊场景雷达平面特征点如图4所示。约束建立与状态求解部分利用视觉重投影约束、雷达几何约束和IMU预积分约束构建非线性优化问题，并针对传感器退化情况调整约束权重，采取滑动窗口策略优化求解无人机位姿状态，实现不依赖外部传感器的快速精确定位。自适应体素地图管理部分根据点云地图在空间中的三维分布，构建基于八叉树数据结构的自适应体素栅格地图，对雷达特征进行体素化处理，减小维护地图的计算资源与内存消耗。
具体实现方法如下：
接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，IMU的测量模型可以表示为:
其中，
和/>
分别为tk时刻IMU测量得到的三轴加速度和三轴角速度，/>
和/>
为tk时刻加速度与角速度的实际值，/>
和/>
分别为tk时刻加速度计偏置和陀螺仪偏置，/>
表示tk时刻从世界系到机体系的旋转矩阵，g表示世界系下的重力矢量，na与ng为IMU测量噪声。
然后根据IMU的输入积分获得高频状态预测，tk与tk+1时刻之间的离散运动方程可以表示为：
其中，
分别表示tk+1时刻IMU测量得到的三轴加速度和三轴角速度，/>
为tk时刻从机体系到世界系的旋转矩阵，/>
为tk+1时刻从机体系到世界系的旋转矩阵。为了节约计算量，将离散运动方程中的状态量与IMU的测量数据分离，将原本世界系下的积分变为相邻两帧间相对运动的积分，使得这个积分量只与IMU测量值有关，而与积分起始点无关。因此，在离散运动方程两边同时乘以从世界系到机体系的旋转矩阵/>
(或者其对应的旋转四元数/>
)可得下式：
其中，
即为tk到tk+1时刻之间的IMU预积分增量信息，分别表示以tk时刻机体系为参考坐标系到tk+1时刻的平移增量、速度增量和tk+1时刻到tk时刻的旋转四元数，具体离散形式表达式为：
其中，
分别表示从tk时刻到ti+1时刻的平移、速度和旋转预积分量，
分别表示从tk时刻到ti时刻的平移、速度和旋转预积分量，/>
为利用中值法将当前机体系下的测量加速度和角速度转换至tk时刻机体系的加速度和角速度，通过上式不断迭代即可得到从tk时刻到tk+1时刻的平移、速度和旋转预积分量。
为充分利用图像中的环境信息，需要提取图像上区分度较明显的特征，通常对图像中的角点信息或边缘信息进行提取，本发明提出的方法主要使用角点检测来获取图像特征，针对相机获取的图像信息，对图像提取特征点。特征点选取Harris角点，其相对于ORB、SIFT、SURF等特征点具有速度更快、计算量适中的优势，相对于FAST特征点具有精度更高、具备旋转不变性等优势，是一种计算时间及提取精度适中的特征点。
Harris角点的基本思想是图像上有一个像素矩形向多个方向滑动，如果像素矩形灰度没有变化，则认为窗口内不存在角点；如果像素矩形沿某一方向移动灰度变化很大，沿另一方向灰度变化不大，则认为像素矩形内图像为一条边；若像素矩形图像沿任何方向移动，灰度值均产生变化，则认为图像中存在角点。
不同于相机传感器采集的丰富的像素信息，激光雷达只能采集到周围环境中稀疏的物体信息，因此本发明提出的方法从点云的空间几何特征寻找可靠的雷达特征提取方法。
对于单帧雷达点云数据的一点Pi＝(xi,yi,zi)，xi,yi,zi为点Pi在雷达坐标系中的坐标，可按照平滑度ci将其定义为线特征与平面特征，其计算公式如下：
其中，S为Pi的邻域点集合，xj为Pi的一个邻域点，|S|为邻域点数量，||Pi||为Pi到雷达坐标系原点的距离。当ci小于某一阈值的时候，认为该点是平面特征点；当ci大于某一阈值的时候，认为该点是线特征点。本方法中只采用了平面特征点作为雷达几何特征点。
对于一个视觉激光雷达惯性紧耦合系统而言，系统的状态估计问题可以建模为一个最大后验估计问题：
其中，XLVIO表示视觉雷达惯性里程计的状态向量，
表示视觉雷达惯性里程计状态的最大后验估计，u表示IMU采集的加速度和角速度信息，zL表示雷达观测信息，zV表示视觉观测，arg max(·)表示求使得括号内的值最大的参数的函数，p(·)表示概率密度函数。该最大后验概率估计问题可通过贝叶斯公式转化为一个非线性最小二乘问题，如下式所示：
其中，arg min(·)表示求使得括号内的值最大的参数的函数，rB为IMU预积分残差，k为关键帧编号，
为IMU预积分协方差，rC为视觉重投影残差，(l,j)表示第l个特征点在第j个相机帧中被观测组成的视觉观测，C表示视觉观测的集合，/>
表示视觉观测协方差，μ为雷达退化权值，rL为雷达几何残差，s表示第s个雷达特征点，/>
表示雷达特征的集合，σs为雷达观测的协方差，利用L-M法对该非线性最小二乘问题进行迭代求解即可得到位姿状态。
IMU预积分约束rB为通过现有状态求得的相机两帧之间位置、姿态、速度、偏置等状态变量和通过IMU预积分求得的两帧之间的状态变量之间的残差，如下式所示：
其中，
为通过IMU预积分求得的两帧之间的位置、速度和姿态的增量残差，/>
为两帧之间IMU加速度计偏置和陀螺仪偏置残差，[·]xyz表示取四元数的虚部，/>
表示IMU预积分姿态增量四元数的逆，/>
和/>
分别为tk+1时刻加速度计偏置和陀螺仪偏置。
通过视觉特征点提取与视觉重投影，可以有效地感知外部环境并利用与环境的相对位置实现长期鲁棒的位置估计。由于估计值与实际位置的误差，实际位置下投影的特征点与原来保存的特征点重投影会产生误差rC，该误差可被定义为视觉特征的重投影误差。将特征点Pl从第i帧相机坐标系Ci通过两帧之间的位姿转换关系，转换到第j帧相机坐标系Cj下的像素坐标，与实际观察到的第j帧相机坐标系下的特征点的像素坐标之间的像素值的差的马氏距离，即为视觉特征重投影误差。可定义视觉误差项为：
其中，
为特征点Pl在第j帧相机坐标系Cj下的归一化坐标，/>
是特征点Pl从第i帧相机坐标系通过位姿关系投影到第j帧相机坐标系的归一化坐标。
设点
为雷达平面点特征集合中的一点，先将其投影至世界坐标系，记作
然后在局部地图中搜索与/>
最近的三个点Pa,Pb,Pc，进而可以构建点到面的几何残差为：/>
针对激光雷达退化场景，本质上是雷达观测所提供的某个自由度上的约束缺失或者较为稀少。在构建无人机状态优化问题时，每个雷达观测提供的约束对应优化方程系数矩阵中的若干行，因此通过对雷达约束构成的优化问题系数矩阵进行退化分析，作为雷达残差权值调整的退化依据。
对于非线性的雷达残差rL，进行一阶泰勒展开，如下式所示：
其中，
为rL在线性化工作点的值，JL为雷达残差的雅可比矩阵，ΔXLVIO表示状态增量,则雷达残差部分对应的Hessian矩阵为/>
对其进行奇异值分解找到其最小特征值λmin，设定经验阈值thres，当Hessian矩阵的特征值大于这个阈值时，认为未发生退化，赋予雷达残差权重μ1；当Hessian矩阵的特征值大于这个阈值时，认为发生退化，赋予雷达残差权重μ2，即：
使用基于八叉树结构的自适应体素地图实现高效的地图存储，具体针对每一帧特征点云，首先按其世界坐标系中的坐标位置将其分配到固定大小的体素中，然后针对每个体素中的点
按照以下方式计算其均值/>
与协方差cov。
其中，N为体素中的雷达特征点数量，求得体素中的雷达特征点云协方差后，对其进行特征值分解，得到按照小到达依次排列的三个特征值λ1,λ2,λ3，若λ2大于λ1一定的倍数，则停止继续拆分体素；若λ2小于或者等于λ1一定的倍数，但体素大小未达到最小，则也停止拆分体素，若体素大小未达到最小，则继续对子体素内的特征点云进行均值和协方差的计算并进行体素的拆分，直至其特征值比例不满足要求或者体素大小已达到最小。
图5是本发明与主流方法在SubT仿真场景下定位轨迹误差指标对比图。图6是本发明在长走廊退化环境下与主流视觉和雷达雷达里程计的定位轨迹对比，视觉和雷达方法均无法完成闭环轨迹的定位，而本方法可以在保证不退化的同时得到很好的定位精度。图7是丛林环境无人机飞行过程中的定位轨迹与建图效果示意图，可见本方法在实际飞行中获得了良好的定位精度与建图效果。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
Claims (2)
1.一种基于视觉激光雷达惯性紧耦合的定位建图方法，其特征是，首先分别对自主定位与建图、激光雷达自主定位与建图、视觉与惯性测量单元IMU三种传感器数据进行处理，对IMU测量数据进行积分得到高频位姿估计和预积分值；对自主定位与建图中的视觉图像提取特征点并进行匹配与跟踪，接着利用投影到相机坐标系下的激光雷达点云，恢复视觉特征点的深度信息；对采集到的激光雷达点云进行运动畸变校正，采用几何特征提取算法，对点云数据中的线面特征进行提取，与局部地图中的点云进行匹配，构建雷达几何因子；其次，构建视觉重投影、IMU预积分和雷达几何约束，联合三种约束建立非线性优化问题，对雷达几何约束的Hessian矩阵进行退化分析，调整各约束权值，利用列文伯格-马夸尔特方法进行位姿求解；最后，利用优化完成后的位姿与当前帧雷达点云更新体素地图，根据输入点云的几何性质自适应调整体素大小，为后续过程提供几何结构信息。
2.如权利要求1所述的基于视觉激光雷达惯性紧耦合的定位建图方法，其特征是，具体步骤如下：
第一部分，传感器数据预处理：
(1)IMU数据预处理
接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，IMU的测量模型表示为:
其中，
和/>
分别为tk时刻IMU测量得到的三轴加速度和三轴角速度，/>
和/>
为tk时刻加速度与角速度的实际值，/>
和/>
分别为tk时刻加速度计偏置和陀螺仪偏置，/>
表示tk时刻从世界系到机体系的旋转矩阵，g表示世界系下的重力矢量，na与ng为IMU测量噪声；
然后根据IMU的输入积分获得高频状态预测，tk与tk+1时刻之间的离散运动方程表示为：
其中，
分别表示tk+1时刻IMU测量得到的三轴加速度和三轴角速度，/>
为tk时刻从机体系到世界系的旋转矩阵，/>
为tk+1时刻从机体系到世界系的旋转矩阵，将离散运动方程中的状态量与IMU的测量数据分离，将原本世界系下的积分变为相邻两帧间相对运动的积分，在离散运动方程两边同时乘以从世界系到机体系的旋转矩阵/>
或者其对应的旋转四元数/>
得下式：
其中，
即为tk到tk+1时刻之间的IMU预积分增量信息，分别表示以tk时刻机体系为参考坐标系到tk+1时刻的平移增量、速度增量和tk+1时刻到tk时刻的旋转四元数，具体离散形式表达式为：
其中，
分别表示从tk时刻到ti+1时刻的平移、速度和旋转预积分量，/>
分别表示从tk时刻到ti时刻的平移、速度和旋转预积分量，/>
为利用中值法将当前机体系下的测量加速度和角速度转换至tk时刻机体系的加速度和角速度，通过上式不断迭代即可得到从tk时刻到tk+1时刻的平移、速度和旋转预积分量；
(2)图像数据处理
使用角点检测来获取图像特征，针对相机获取的图像信息，对图像提取特征点：图像上有一个像素矩形向多个方向滑动，如果像素矩形灰度没有变化，则认为窗口内不存在角点；如果像素矩形沿某一方向移动灰度变化很大，沿另一方向灰度变化不大，则认为像素矩形内图像为一条边；若像素矩形图像沿任何方向移动，灰度值均产生变化，则认为图像中存在角点；
(3)雷达几何特征点提取
对于单帧雷达点云数据的一点Pi＝(xi,yi,zi)，xi,yi,zi为点Pi在雷达坐标系中的坐标，按照平滑度ci将其定义为线特征与平面特征，其计算公式如下：
其中，S为Pi的邻域点集合，xj为Pi的一个邻域点，|S|为邻域点数量，||Pi||为Pi到雷达坐标系原点的距离，当ci小于某一阈值的时候，认为该点是平面特征点；当ci大于某一阈值的时候，认为该点是线特征点；
第二部分，约束建立与状态求解：对于一个视觉激光雷达惯性紧耦合系统而言，系统的状态估计问题建模为一个最大后验估计问题：
其中，XLVIO表示视觉雷达惯性里程计的状态向量，
表示视觉雷达惯性里程计状态的最大后验估计，u表示IMU采集的加速度和角速度信息，zL表示雷达观测信息，zV表示视觉观测，arg max(·)表示求使得括号内的值最大的参数的函数，p(·)表示概率密度函数，该最大后验概率估计问题可通过贝叶斯公式转化为一个非线性最小二乘问题，如下式所示：
其中，arg min(·)表示求使得括号内的值最大的参数的函数，rB为IMU预积分残差，k为关键帧编号，
为IMU预积分协方差，rC为视觉重投影残差，(l,j)表示第l个特征点在第j个相机帧中被观测组成的视觉观测，C表示视觉观测的集合，/>
表示视觉观测协方差，μ为雷达退化权值，rL为雷达几何残差，s表示第s个雷达特征点，/>
表示雷达特征的集合，σs为雷达观测的协方差，利用L-M法对该非线性最小二乘问题进行迭代求解即可得到位姿状态；
(1)IMU预积分残差
IMU预积分约束rB为通过现有状态求得的相机两帧之间包括位置、姿态、速度、偏置的状态变量和通过IMU预积分求得的两帧之间的状态变量之间的残差，如下式所示：
其中，
为通过IMU预积分求得的两帧之间的位置、速度和姿态的增量残差，/>
为两帧之间IMU加速度计偏置和陀螺仪偏置残差，[]xyz表示取四元数的虚部，/>
表示IMU预积分姿态增量四元数的逆，/>
和/>
分别为tk+1时刻加速度计偏置和陀螺仪偏置；
(2)视觉重投影残差
由于估计值与实际位置的误差，实际位置下投影的特征点与原来保存的特征点重投影会产生误差rC，该误差被定义为视觉特征的重投影误差，将特征点Pl从第i帧相机坐标系Ci通过两帧之间的位姿转换关系，转换到第j帧相机坐标系Cj下的像素坐标，与实际观察到的第j帧相机坐标系下的特征点的像素坐标之间的像素值的差的马氏距离，即为视觉特征重投影误差，定义视觉误差项为：
其中，CjPl为特征点Pl在第j帧相机坐标系Cj下的归一化坐标，
是特征点Pl从第i帧相机坐标系通过位姿关系投影到第j帧相机坐标系的归一化坐标；
(3)雷达几何残差
设点
为平面点特征集合中的一点，先将其投影至世界坐标系，记作/>
然后在局部地图中搜索与/>
最近的三个点Pa,Pb,Pc，进而可以构建点到面的几何残差为：
(4)雷达退化处理
对于非线性的雷达残差rL，进行一阶泰勒展开，如下式所示：
其中，
为rL在线性化工作点的值，JL为雷达残差的雅可比矩阵，ΔXLVIO表示状态增量，则雷达残差部分对应的Hessian矩阵为/>
对其进行奇异值分解找到其最小特征值λmin，设定经验阈值thres，当Hessian矩阵的特征值大于这个阈值时，认为未发生退化，赋予雷达残差权重μ1；当Hessian矩阵的特征值大于这个阈值时，认为发生退化，赋予雷达残差权重μ2，即：
自适应体素地图管理：针对每一帧特征点云，首先按其世界坐标系中的坐标位置将其分配到固定大小的体素中，然后针对每个体素中的点
按照以下方式计算其均值/>
与协方差cov；/>
其中，N为体素中的雷达特征点数量，求得体素中的雷达特征点云协方差后，对其进行特征值分解，得到按照小到达依次排列的三个特征值λ1,λ2,λ3，若λ2大于λ1一定的倍数，则停止继续拆分体素；若λ2小于或者等于λ1一定的倍数，但体素大小未达到最小，则也停止拆分体素，若体素大小未达到最小，则继续对子体素内的特征点云进行均值和协方差的计算并进行体素的拆分，直至其特征值比例不满足要求或者体素大小已达到最小。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310253115.0A CN116182837A (zh) | 2023-03-16 | 2023-03-16 | 基于视觉激光雷达惯性紧耦合的定位建图方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310253115.0A CN116182837A (zh) | 2023-03-16 | 2023-03-16 | 基于视觉激光雷达惯性紧耦合的定位建图方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116182837A true CN116182837A (zh) | 2023-05-30 |
Family
ID=86448835
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310253115.0A Pending CN116182837A (zh) | 2023-03-16 | 2023-03-16 | 基于视觉激光雷达惯性紧耦合的定位建图方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116182837A (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116698046A (zh) * | 2023-08-04 | 2023-09-05 | 苏州观瑞汽车技术有限公司 | 一种物业室内服务机器人建图定位和回环检测方法及系统 |
| CN116758161A (zh) * | 2023-06-26 | 2023-09-15 | 北京道仪数慧科技有限公司 | 移动端空间数据生成方法及空间感知移动端 |
| CN117073690A (zh) * | 2023-10-17 | 2023-11-17 | 山东大学 | 一种基于多地图策略的导航方法及系统 |
| CN117109568A (zh) * | 2023-08-24 | 2023-11-24 | 北京自动化控制设备研究所 | 惯性/多维视觉联合定位方法 |
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117437290A (zh) * | 2023-12-20 | 2024-01-23 | 深圳市森歌数据技术有限公司 | 一种多传感器融合的自然保护区无人机三维空间定位方法 |
| CN117824625A (zh) * | 2024-03-05 | 2024-04-05 | 河海大学 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
-
2023
- 2023-03-16 CN CN202310253115.0A patent/CN116182837A/zh active Pending
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116758161A (zh) * | 2023-06-26 | 2023-09-15 | 北京道仪数慧科技有限公司 | 移动端空间数据生成方法及空间感知移动端 |
| CN116698046A (zh) * | 2023-08-04 | 2023-09-05 | 苏州观瑞汽车技术有限公司 | 一种物业室内服务机器人建图定位和回环检测方法及系统 |
| CN116698046B (zh) * | 2023-08-04 | 2023-12-01 | 苏州观瑞汽车技术有限公司 | 一种物业室内服务机器人建图定位和回环检测方法及系统 |
| CN117109568A (zh) * | 2023-08-24 | 2023-11-24 | 北京自动化控制设备研究所 | 惯性/多维视觉联合定位方法 |
| CN117073690A (zh) * | 2023-10-17 | 2023-11-17 | 山东大学 | 一种基于多地图策略的导航方法及系统 |
| CN117073690B (zh) * | 2023-10-17 | 2024-03-15 | 山东大学 | 一种基于多地图策略的导航方法及系统 |
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117367412B (zh) * | 2023-12-07 | 2024-03-29 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117437290A (zh) * | 2023-12-20 | 2024-01-23 | 深圳市森歌数据技术有限公司 | 一种多传感器融合的自然保护区无人机三维空间定位方法 |
| CN117437290B (zh) * | 2023-12-20 | 2024-02-23 | 深圳市森歌数据技术有限公司 | 一种多传感器融合的自然保护区无人机三维空间定位方法 |
| CN117824625A (zh) * | 2024-03-05 | 2024-04-05 | 河海大学 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
| CN117824625B (zh) * | 2024-03-05 | 2024-05-14 | 河海大学 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN116182837A (zh) | 基于视觉激光雷达惯性紧耦合的定位建图方法 | |
| CN109709801B (zh) | 一种基于激光雷达的室内无人机定位系统及方法 | |
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| CN113625774B (zh) | 局部地图匹配与端到端测距多无人机协同定位系统和方法 | |
| CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN103413352A (zh) | 基于rgbd多传感器融合的场景三维重建方法 | |
| CN114419147A (zh) | 一种救援机器人智能化远程人机交互控制方法及系统 | |
| Jia et al. | A Survey of simultaneous localization and mapping for robot | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN111812978B (zh) | 一种多无人机协作slam方法与系统 | |
| Liu | A robust and efficient lidar-inertial-visual fused simultaneous localization and mapping system with loop closure | |
| CN115355904A (zh) | 一种针对地面移动机器人的Lidar-IMU融合的slam方法 | |
| CN115218889A (zh) | 一种基于点线特征融合的多传感器室内定位方法 | |
| CN112945233B (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| Wang et al. | Micro aerial vehicle navigation with visual-inertial integration aided by structured light | |
| Zhang et al. | Hybrid iteration and optimization-based three-dimensional reconstruction for space non-cooperative targets with monocular vision and sparse lidar fusion | |
| CN116295342A (zh) | 一种用于飞行器勘测的多传感状态估计器 | |
| Zuo et al. | Overview of obstacle avoidance algorithms for UAV environment awareness | |
| Liu et al. | A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM | |
| Zhao et al. | 3D indoor map building with monte carlo localization in 2D map | |
| CN115113170A (zh) | 一种基于室内特征退化环境的激光雷达边缘特征预测方法 | |
| CN114459474A (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
| Liu et al. | 6-DOF motion estimation using optical flow based on dual cameras |
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 |