CN116449384A - 基于固态激光雷达的雷达惯性紧耦合定位建图方法 - Google Patents
基于固态激光雷达的雷达惯性紧耦合定位建图方法 Download PDFInfo
- Publication number
- CN116449384A CN116449384A CN202310263473.XA CN202310263473A CN116449384A CN 116449384 A CN116449384 A CN 116449384A CN 202310263473 A CN202310263473 A CN 202310263473A CN 116449384 A CN116449384 A CN 116449384A
- Authority
- CN
- China
- Prior art keywords
- points
- point
- intensity
- map
- point cloud
- 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 100
- 238000013507 mapping Methods 0.000 title claims abstract description 32
- 230000008878 coupling Effects 0.000 title claims description 10
- 238000010168 coupling process Methods 0.000 title claims description 10
- 238000005859 coupling reaction Methods 0.000 title claims description 10
- 238000007781 pre-processing Methods 0.000 claims description 23
- 230000008569 process Effects 0.000 claims description 23
- 239000013598 vector Substances 0.000 claims description 22
- 230000001133 acceleration Effects 0.000 claims description 17
- 238000000605 extraction Methods 0.000 claims description 16
- 238000001914 filtration Methods 0.000 claims description 14
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000007726 management method Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 9
- 238000005457 optimization Methods 0.000 claims description 8
- 238000009825 accumulation Methods 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 7
- 238000000513 principal component analysis Methods 0.000 claims description 6
- 238000010586 diagram Methods 0.000 claims description 5
- 230000008030 elimination Effects 0.000 claims description 5
- 238000003379 elimination reaction Methods 0.000 claims description 5
- 238000003860 storage Methods 0.000 claims description 5
- 238000007476 Maximum Likelihood Methods 0.000 claims description 4
- 230000003044 adaptive effect Effects 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 4
- 230000001186 cumulative effect Effects 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 238000003384 imaging method Methods 0.000 claims description 4
- 238000005295 random walk Methods 0.000 claims description 4
- 238000013519 translation Methods 0.000 claims description 4
- 238000012795 verification Methods 0.000 claims description 4
- 230000000007 visual effect Effects 0.000 claims description 4
- 238000013461 design Methods 0.000 abstract description 8
- 238000004519 manufacturing process Methods 0.000 abstract description 3
- 230000015556 catabolic process Effects 0.000 description 11
- 238000006731 degradation reaction Methods 0.000 description 11
- 238000004422 calculation algorithm Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 5
- 238000011160 research Methods 0.000 description 5
- 238000010276 construction Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000012423 maintenance Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 230000000903 blocking effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- IIBYAHWJQTYFKB-UHFFFAOYSA-N bezafibrate Chemical compound C1=CC(OC(C)(C)C(O)=O)=CC=C1CCNC(=O)C1=CC=C(Cl)C=C1 IIBYAHWJQTYFKB-UHFFFAOYSA-N 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- WHWDWIHXSPCOKZ-UHFFFAOYSA-N hexahydrofarnesyl acetone Chemical compound CC(C)CCCC(C)CCCC(C)CCCC(C)=O WHWDWIHXSPCOKZ-UHFFFAOYSA-N 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000033772 system development Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- 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
- 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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
- G01S13/865—Combination of radar systems with lidar systems
-
- 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
- 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
-
- 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)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
本发明属于激光雷达、无人机及定位建图领域，为提出一种基于固态激光雷达的定位建图方法，实现在线获得准确地结构还原，且在长走廊、室内密闭环境等易出现严重退化的场景下可以得到较好的精度，本发明，基于固态激光雷达的雷达惯性紧耦合定位建图方法，采用固态激光雷达获取点云，对获取到的点云和惯性测量单元IMU数据进行预处理后，提取几何角点、几何平面点和强度角点；将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合；利用校正后的雷达点云强度信息，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。本发明主要应用于无人机设计制造场合。
Description
技术领域
本发明属于定位建图领域，具体涉及基于固态激光雷达惯性紧耦合的自主定位与建图技术、基于雷达点云强度增强的雷达惯性里程计以及基于二维强度图和三维空间的地图维护方法。
背景技术
近年来，四旋翼无人机的研发技术不断成熟，制作成本逐渐降低，已经在军事和民用领域展现出巨大的应用价值。通常，全球导航卫星系统(Global Navigation SatelliteSystem,GNSS)用于在室外空旷场景下提供定位信息，然而这大大限制了无人机的应用场景，应用无人机搭载的传感器，如激光雷达、相机、毫米波雷达，惯性测量单元(InertialMeasurement Unit,IMU)等，以实时获取当前的位置信息以及周围的环境信息的同步定位与建图(SLAM)技术应运而生。
固态激光雷达有着价格便宜、重量轻、体积小、量程远、点云稠密等优良特性。然而由于该类型雷达具有非重复式扫描以及有限视场角的特性，传统的LiDAR SLAM方法并不适用，因此需要设计新的算法实现定位与建图的功能。对于LiDAR SLAM系统，IMU是不可或缺的，IMU预测得到的位置姿态以及速度等信息对于雷达点云畸变处理和输出高频平滑的里程计信息十分重要。
针对激光雷达的定位建图问题，国内外学者都进行了大量的研究。2014年，卡耐基梅隆大学的Ji Zhang提出了LOAM算法，对雷达定位建图算法产生了深远影响。2019年，香港大学的Fu Zhang提出了针对Livox固态雷达的纯LiDAR SLAM系统Loam-Livox，该系统依据特有的非重复式扫描特性设计了相应的运动补偿策略、特征点提取、外点剔除方法以及点云匹配方法，为后续的固态雷达SLAM系统发展奠定了基础。2022年，该团队在之前的基础上提出了更为高效、鲁棒和通用的雷达惯性里程计框架FAST-LIO2，该框架将前端的特征提取转为直接法，使用基于ikd-tree的增量式kd-tree的地图维护方法使得后端的配准计算量大大减少以至于FAST-LIO2可以实现100Hz的里程计。2022年，香港大学的Chunran Zheng等人提出了一个快速的雷达惯性视觉里程计(LIO)系统FAST-LIVO，该系统具备一套新颖的异常点剔除方法，用于剔除位于边缘或在图像视图中被遮挡的不稳定映射点。
综上所述，国内外学者对激光雷达的定位建图问题进行了卓有成效的研究，并取得了丰硕的研究成果。然而，针对固态激光雷达的特殊非重复式扫描特性，需要设计相应的特征提取、位姿估计以及地图管理模块。同时，针对于长走廊、狭窄的办公室等室内场景导致的里程计退化问题广泛存在于激光雷达定位建图方法中。因此，研究基于固态激光雷达的雷达惯性紧耦合的自主定位与建图技术至关重要。
发明内容
为克服现有技术的不足，本发明旨在提出一种基于固态激光雷达的定位建图方法，使用本方法进行定位建图，可以在线获得准确地结构还原，且在长走廊、室内密闭环境等易出现严重退化的场景下可以得到较好的精度。为此，本发明采取的技术方案是，基于固态激光雷达的雷达惯性紧耦合定位建图方法，采用固态激光雷达获取点云，对获取到的点云和惯性测量单元IMU数据进行预处理后，提取几何角点、几何平面点和强度角点；将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计；利用校正后的雷达点云强度信息，在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息；针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。
具体步骤如下：
数据预处理：接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，t时刻时，IMU的加速度为at，角速度为wt，IMU的输入表示为:
其中为t时刻IMU测量的原始加速度，/>为t时刻IMU测量的原始角速度，ba和bg是满足高斯噪声随机游动过程的IMU偏置，na和ng分别指的是测量加速度和角速度过程中存在的白噪声，Wg表示的是世界系下的重力矢量，IRW指的是从世界坐标系到IMU坐标系下的旋转矩阵，WRI指的是从IMU坐标系到世界坐标系下的旋转矩阵，两者有以下关系IRW＝(WRI)T，其中xT为转置符号；
于是，根据IMU的输入可以实时推断出机器人的状态，所述状态包含机器人的姿态R，速度v以及位置p，tk时刻与tk+1时刻之间的离散运动方程表示为：
其中表示tk+1时刻在世界坐标系下机器人位置的预测值，/>表示tk+1时刻在世界坐标系下机器人速度的预测值，/>表示tk+1时刻在世界坐标系下机器人位姿的预测值，/>表示tk时刻在世界坐标系下机器人的实际位置，/>表示tk时刻在世界坐标系下机器人的实际速度，/>表示tk时刻在世界坐标系下机器人的实际位姿，/>为tk时刻IMU测量的加速度，/>为tk时刻IMU测量的角速度，Δt＝tk+1-tk表示两个相邻IMU的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
由此可以看出，每次由IMU推断得到的状态信息包含了tk时刻的IMU坐标系下的姿态、速度以及位置信息；
考虑到雷达点云中存在的噪声以及AVIA特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤，对于点云去畸变部分，采用高频IMU里程计来对点云进行优化；
在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点，利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正，对于一根线束上的前后两个点pi和pi+1，其强度差ΔIi校正为：
其中αi、αi+1分别表示第i个点和第i+1个点的入射角角度，Ii和Ii+1分别表示第i个点和第i+1个点的原始强度值；
前端特征点提取：对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向，在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点；
在确定了对于局部区块的分配以及扩展方式之后，接下来的工作就是判断此区块内的点是否非平面点或者角点：
首先，针对点云中的一个局部区块P∈P，P表示一帧点云的集合，P是该帧点云下一个特定的局部区块，假设该区块内的所有点云对应着存在一个平面方程Π(x,y,z)，式中的x,y,z表示点的三维坐标，在此局部区块内，首先找到距离雷达原点最远的点pa，接着寻找离pa最远的点pb，最后利用如下公式寻找第三个pc：
表示在点云P中寻找使得X最大的点，通过寻找到的三个点可以描述该区块内的几何性质，从而拟合出平面方程Π(x,y,z)：
其中nx,ny,nz和表示的平面方程的参数通过以下方式计算：
其中表示的平面所代表的法向量，/>为局部区块里面点云P的质心，N为点云的数量。如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说那么这个局部区块中的所有点都被认为是平面点；
对于判断不满足平面方程的点，则继续判断是否满足角点的性质，如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，使用基于曲率的方式判断，如果曲率较大的话，该点即为角点；此外，对于已经经过扩展后仍不满足平面点的点集，使用基于PCA(Principal Component Analysis，主成分分析)分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点；
加入强度角点以增加后端位姿解算时的约束，点云的强度值取值范围是0-255，在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点；在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值；
在构建局部地图过程中，应用自适应体素地图进行构建；为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波，首先利用平面区块的法向量对不同区块进行聚类：
cosθ＝n1·n2
n1和n2分别为两个平面区块的法向量，如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面，对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m；
后端状态估计：对于一个LiDAR SLAM系统而言，无人机的状态的估计问题建模为一个极大似然估计问题：
其中Fk表示的是第k个关键帧下的特征点点集，表示状态的最大后验概率估计，/>表示目前观测到的六自由度位姿状态，p(·|·)表示状态与观测所满足的条件概率密度函数，f(·)为目标函数。在假设观测模型为高斯函数的情况下，最大后验概率问题转换为一个求解一个非线性最小二乘问题：
其中表示优化得到的旋转矩阵，/>表示优化得到的位置，fi(·)表示第i个观测得到的残差，N为所有观测数量之和；
地图管理：本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：
(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的；
(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一定阈值，则判定当前帧为关键帧；
(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧；
对于构建局部地图的过程，选择基于kd-tree(k-dimensional tree)快速索引的方式，首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点；
采用基于二维强度图与三维点云的外点剔除地图管理方法，针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云；
使用累计5秒内的点云Pj(j＝1,2,...,50)以及相应的位姿构建出局部地图M(L)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度。为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差，对于一个具有原始强度值I的点p而言，其在二维强度图像坐标系下的像素值可以定义为VI(u,v)：
其中log(·)表示一个对数函数，clamp(MIN,x,MAX)表示将一个变量x约束至下限MIN到上限MAX之间的函数。将VI(u,v)的像素值限定为(0,255)之间，构建的强度图视作为一个灰度图，于是将问题转变为在二维图像中寻找线段，采用基于区域生长的线段扩张LSD(Line Segment Detector)方法进行线段的生成。得到线段的两个端点，用深度通道的深度信息，将两个端点反映射为两个3D点ps＝(x1,y1,z1)和pe＝(x2,y2,z2)。于是利用恢复后的两个3D点，得到一个唯一的直线，其方程Ξ(x,y,z)可以表示为：
针对长走廊等约束少的场景，完善雷达SLAM系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性，首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置，剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环；其次，使用基于强度的扫描上下文判断回环。得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠；在判断满足回环的条件后，使用ICP(Iterative Closest Point)进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧可以触发回环，进而触发基于因子图的位姿图优化。
本发明的特点及有益效果是：
1、本发明提出了基于固态激光雷达的惯性紧耦合定位建图方法，首先对IMU数据处理后得到高频IMU预测位姿，对雷达原始点云进行外点剔除以及去畸变后，在前端提取得到平面点和角点；在后端位姿解算时，使用基于点线点面的匹配方式，利用高斯牛顿迭代优化求解低频雷达里程计位姿，最后与IMU预测位姿紧耦合，使用ISAM(IncrementalSmoothing And Mapping)求解得到高频精准的无人机位姿。在不同的场景下均具有较稳定的效果，能够很好的平衡时间效率和精度之间的要求，针对室外大环境以及室内密闭环境可以在保证实时性的同时建立一致性良好的地图。
2、本发明设计了基于雷达点云强度增强的鲁棒雷达惯性里程计，利用点云强度值在数据预处理阶段进一步剔除外点。在特征提取阶段，不仅考虑了具有几何信息的角点以及平面点，还在校正后的强度基础上提取了强度角点，为后端提供更为的位姿约束。对于后端，分别对角点以及平面点设计了相应的权重，考虑到了几何、强度、配准效果等信息，调节了不同质量的特征点对于位姿解算的贡献。在约束充足的场景下，该方法与最先进的框架定位精度相当，在约束不足易导致退化的场景下，仍可以表现出高鲁棒性。
3、本发明设计了一个基于二维强度图与三维空间的外点剔除地图维护方法，在前端对于平面点使用自适应体素滤波的策略，实现对聚类后不同点云的均匀采样。首先，将三维点云投影成二维强度图，使用基于LSD的方法提取线段并剔除不具有“线”的属性的点。然后，使用基于RANSAC(Random Sample Consensus)的方法对局部地图中的角点进行直线拟合并剔除外点。最后，新增基于因子图的位姿图优化环节，在检测到发生回环时，对全局关键帧位姿进行优化，从而更新全局地图，增加地图的一致性。该算法相较于其他主流方法在正常场景和部分退化场景下可以实现实时、精准的定位与建图，并且在部分极端场景下能保持长时间较为稳定地运行。
附图说明：
图1为搭载了Livox AVIA固态激光雷达的无人机平台；
图2为基于固态激光雷达的雷达惯性紧耦合设计结构图；
图3为走廊场景下和室内场景的强度角点图；
图4为极端退化环境下构建的局部地图。
具体实施方式
通过IMU惯性测量单元解析出无人机的位姿信息、基于雷达点云强度增强的鲁棒雷达惯性里程计和基于二维强度图和三维点云的外点剔除地图管理方法，实现无人机在严重退化场景下的鲁棒定位建图。结构如下：采用固态激光雷达获取点云。在前端中，对获取到的点云和IMU数据进行预处理后，设计特征提取方法提取几何角点、几何平面点和强度角点；在后端中，将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计。考虑到室内场景可能存在的退化问题，利用校正后的雷达点云强度信息，在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息。针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。针对设计方案，将本方法的核心部分分为前端和后端两个模块，其中前端主要对预处理后的数据进行特征点选取，后端基于前端提取出的特征点进行状态估计。
基于固态激光雷达的雷达惯性紧耦合里程计选用的是Livox AVIA固态激光雷达，为了适配该雷达的点云结构，设计了新的雷达惯性SLAM算法，分为四个部分：数据预处理、前端特征点提取、后端状态估计以及地图管理。
数据预处理：接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态。假设t时刻时，IMU的加速度为at，角速度为wt，IMU的输入可以表示为:
其中为t时刻IMU测量的原始加速度，/>为t时刻IMU测量的原始角速度，ba和bg是满足高斯噪声随机游动过程的IMU偏置，na和ng分别指的是测量加速度和角速度过程中存在的白噪声。Wg表示的是世界系下的重力矢量，IRW指的是从世界坐标系到IMU坐标系下的旋转矩阵，WRI指的是从IMU坐标系到世界坐标系下的旋转矩阵，两者有以下关系IRW＝(WRI)T，其中xT为转置符号。
于是，根据IMU的输入可以实时推断出机器人的状态，本文的状态包含机器人的姿态R，速度v以及位置p。因此，tk时刻与tk+1时刻之间的离散运动方程可以表示为：
其中表示tk+1时刻在世界坐标系下机器人位置的预测值，/>表示tk+1时刻在世界坐标系下机器人速度的预测值，/>表示tk+1时刻在世界坐标系下机器人位姿的预测值，/>表示tk时刻在世界坐标系下机器人的实际位置，/>表示tk时刻在世界坐标系下机器人的实际速度，/>表示tk时刻在世界坐标系下机器人的实际位姿，/>为tk时刻IMU测量的加速度，/>为tk时刻IMU测量的角速度，Δt＝tk+1-tk表示两个相邻IMU的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
由此可以看出，每次由IMU推断得到的状态信息包含了tk时刻的IMU坐标系下的姿态、速度以及位置信息。
考虑到雷达点云中存在的噪声以及AVIA特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤。对于点云去畸变部分，本发明采用高频IMU里程计来对点云进行优化。
在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点。利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正。对于一根线束上的前后两个点pi和pi+1，其强度差ΔIi可以校正为：
其中αi、αi+1分别表示第i个点和第i+1个点的入射角角度，Ii和Ii+1分别表示第i个点和第i+1个点的原始强度值。
前端特征点提取：对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向。在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点。
在确定了对于局部区块的分配以及扩展方式之后，接下来的工作就是判断此区块内的点是否非平面点或者角点。
首先，针对点云中的一个局部区块P∈P，P表示一帧点云的集合，P是该帧点云下一个特定的局部区块。假设该区块内的所有点云对应着存在一个平面方程Π(x,y,z)，式中的x,y,z表示点的三维坐标。在此局部区块内，首先找到距离雷达原点最远的点pa，接着寻找离pa最远的点pb，最后利用如下公式寻找第三个pc：
表示在点云P中寻找使得X最大的点，通过寻找到的三个点可以描述该区块内的几何性质，从而拟合出平面方程Π(x,y,z)：
其中nx,ny,nz和表示的平面方程的参数通过以下方式计算：
其中表示的平面所代表的法向量，/>为局部区块里面点云P的质心，N为点云的数量。如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说/> 那么这个局部区块中的所有点都被认为是平面点。
对于判断不满足平面方程的点，则继续判断是否满足角点的性质。如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，于是可以使用基于曲率的方式判断，如果曲率较大的话，该点即为角点。此外，对于已经经过扩展后仍不满足平面点的点集，使用基于PCA分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点。
除了传统的几何角点和几何平面点，本发明加入了强度角点以增加后端位姿解算时的约束。点云的强度值取值范围是0-255，本发明在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点。在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值。
在构建局部地图过程中，为了减小地图所占用的空间，加快地图索引的效率，应用自适应体素地图进行构建。为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波。首先利用平面区块的法向量对不同区块进行聚类：
cosθ＝n1·n2
n1和n2分别为两个平面区块的法向量，如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面。对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m。
后端状态估计：对于一个LiDAR SLAM系统而言，无人机的状态的估计问题可以建模为一个极大似然估计问题：
其中Fk表示的是第k个关键帧下的特征点点集，表示状态的最大后验概率估计，/>表示目前观测到的六自由度位姿状态，p(·|·)表示状态与观测所满足的条件概率密度函数，f(·)为目标函数。在假设观测模型为高斯函数的情况下，最大后验概率问题可以转换为一个求解一个非线性最小二乘问题：
其中表示优化得到的旋转矩阵，/>表示优化得到的位置，fi(·)表示第i个观测得到的残差，N为所有观测数量之和。
地图管理：本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：
(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的。本发明中选择每隔2帧进行一次关键帧判定的策略。
(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一定阈值，则判定当前帧为关键帧。
(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧。
对于构建局部地图的过程，本发明选择的是基于kd-tree快速索引的方式。首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，可以快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点。以此种方式构建的地图实现了每帧点云与位姿的解耦，这使得在改变位姿的时候可以同时改变地图，这为回环检测、离线构建高质量地图提供了接口。
由于系统长时间运行下回出现外点累积，导致地图中点云一致性下降，本发明针对此问题设计了一个基于二维强度图与三维点云的外点剔除地图管理方法。针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云。
使用累计5秒内的点云Pj(j＝1,2,...,50)以及相应的位姿构建出局部地图M(L)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度。为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差。对于一个具有原始强度值I的点p而言，其在二维强度图像坐标系下的像素值可以定义为VI(u,v)：
其中log(·)表示一个对数函数，clamp(MIN,x,MAX)表示将一个变量x约束至下限MIN到上限MAX之间的函数。将VI(u,v)的像素值限定为(0,255)之间，构建的强度图可以视作为一个灰度图。于是将问题转变为在二维图像中寻找线段，采用基于区域生长的线段扩张LSD方法进行线段的生成。得到线段的两个端点，用深度通道的深度信息，将两个端点反映射为两个3D点ps＝(x1,y1,z1)和pe＝(x2,y2,z2)。于是利用恢复后的两个3D点，可以得到一个唯一的直线，其方程Ξ(x,y,z)可以表示为：
针对长走廊等约束少的场景，完善雷达SLAM系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性。首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置。剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环。其次，使用基于强度的扫描上下文判断回环。得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠。在判断满足回环的条件后，使用ICP进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧可以触发回环，进而触发基于因子图的位姿图优化。
以下结合附图对本发明的技术方案作进一步说明。
本发明提出了一种基于固态激光雷达的定位建图方法，解决室内场景下雷达里程计退化的问题。本方法选用的是Livox AVIA固态激光雷达，搭载了固态激光雷达的无人机如图1所示。为了适配该雷达的点云结构，设计了新的雷达惯性SLAM算法，分为四个部分：数据预处理、前端特征点提取、后端状态估计以及地图管理。结构如图2所示：数据预处理部分采用固态激光雷达获取点云，对获取到的点云和IMU数据进行预处理。特征提取部分设计特征提取方法提取几何角点和几何平面点，并对几何平面点进行了自适应体素滤波。在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息。后端状态估计部分，将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计并对因子图进行优化。在地图管理部分，使用基于关键帧的策略实现高效的地图存储，针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。
具体实现方法如下：
系统在接收到IMU输入后，对IMU进行预处理以实现实时预测机器人状态。假设t时刻时，IMU的加速度为at，角速度为wt，IMU的输入可以表示为:
其中为t时刻IMU测量的原始加速度，/>为t时刻IMU测量的原始角速度，ba和bg是满足高斯噪声随机游动过程的IMU偏置，na和ng分别指的是测量加速度和角速度过程中存在的白噪声。Wg表示的是世界系下的重力矢量，IRW指的是从世界坐标系到IMU坐标系下的旋转矩阵，WRI指的是从IMU坐标系到世界坐标系下的旋转矩阵，两者有以下关系IRW＝(WRI)T，其中xT为转置符号。
于是，根据IMU的输入可以实时推断出机器人的状态，本文的状态包含机器人的姿态R，速度v以及位置p。因此，tk时刻与tk+1时刻之间的离散运动方程可以表示为：
其中表示tk+1时刻在世界坐标系下机器人位置的预测值，/>表示tk+1时刻在世界坐标系下机器人速度的预测值，/>表示tk+1时刻在世界坐标系下机器人位姿的预测值，/>表示tk时刻在世界坐标系下机器人的实际位置，/>表示tk时刻在世界坐标系下机器人的实际速度，/>表示tk时刻在世界坐标系下机器人的实际位姿，/>为tk时刻IMU测量的加速度，/>为tk时刻IMU测量的角速度，Δt＝tk+1-tk表示两个相邻IMU的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
由此可以看出，每次由IMU推断得到的状态信息包含了tk时刻的IMU坐标系下的姿态、速度以及位置信息。但对于每一时刻的状态变化都需要重新计算运动方程，于是采用预积分的方法减少计算量。将参考系从世界系转换到机体系下后，可以使用IMU原始加速度和原始角速度/>得到IMU预积分部分：
其中分别表示世界坐标系下，在tk与tk+1两时刻之间关于位置、速度以及姿态的预积分量，/>表示tk+1时刻在世界坐标系下机器人的实际位置。由于该预积分量计算的是相邻两帧的与状态无关的积分量，这使得在每一次状态重新传播的过程中无需再次计算此部分的量，大大减少了计算量。
考虑到雷达点云P中存在的噪声以及AVIA特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤。对于点云去畸变部分，本发明采用高频IMU里程计来对点云进行优化。
为了实现去畸变，将采样时间为ti∈[tk,tk+1]的所有点投影到扫描结束时间tk+1中，对于时域中的每一点，搜索时域上与IMU预测位姿时间戳最近的位姿变换矩阵于是关于雷达坐标系L下第i个点的三维坐标/>即可转换为/>
其中ITL为雷达与IMU之间的内参矩阵，该矩阵在AVIA出厂时已经标定好。
在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点。利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正。对于一根线束上的前后两个点pi和pi+1，其强度差ΔIi可以校正为：
其中αi、αi+1分别表示第i个点和第i+1个点的入射角角度，Ii和Ii+1分别表示第i个点和第i+1个点的原始强度值。
对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向。在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点。在确定了对于局部区块的分配以及扩展方式之后，需要判断此区块内的点是否为非平面点或者角点。
针对每一个局部区块P∈P，假设该区块内的所有点云对应着存在一个平面方程Π(x,y,z)。在此局部区块内，首先找到距离雷达原点最远的点pa，接着寻找距离pa最远的点pb，最后利用如下公式寻找第三个pc：
/>
表示在点云P中寻找使得X最大的点，通过寻找到的三个点可以描述该区块内的几何性质，从而拟合出平面方程Π(x,y,z)：
其中nx,ny,nz和表示的平面方程的参数通过以下方式计算：
其中表示的平面所代表的法向量，/>为局部区块里面点云P的质心，N为点云的数量。如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说 那么这个局部区块中的所有点都被认为是平面点。
对于判断不满足平面方程的点，则继续判断是否满足角点的性质。如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，于是可以使用基于曲率的方式判断，如果曲率较大的话，该点即为角点。此外，对于已经经过扩展后仍不满足平面点的点集，使用基于PCA分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点。
除了传统的几何角点和几何平面点，本发明加入了强度角点以增加后端位姿解算时的约束。点云的强度值取值范围是0-255，本发明在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点。在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值。图3是在走廊场景下和室内场景下存在退化环境的强度角点图，可以看到在环境中存在的用其他主流算法无法提取出的如展板边缘，天花板缝隙等肉眼可分辨的边缘，通过强度角点提取可以成功得到约束。
在构建局部地图过程中，为了减小地图所占用的空间，加快地图索引的效率，应用自适应体素地图进行构建。为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波。首先利用平面区块的法向量对不同区块进行聚类：
cosθ＝n1·n2 (10)
如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面。对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m。
对于一个LiDAR SLAM系统而言，无人机的状态的估计问题可以建模为一个极大似然估计问题：
其中Fk表示的是第k个关键帧下的特征点点集，表示待优化求解的六自由度位姿，p(·)表示状态与观测所满足的概率密度函数，f(·)为目标函数。在假设观测模型为高斯的情况下，最大后验概率问题可以转换为一个求解一个非线性最小二乘问题：
其中表示优化得到的旋转矩阵，/>表示优化得到的位置，fi(·)表示第i个观测得到的残差，N为所有观测数量之和。
本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：
(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的。本发明中选择每隔2帧进行一次关键帧判定的策略。
(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一定阈值，则判定当前帧为关键帧。
(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧。
对于局部地图的构建，本发明选择的是基于kd-tree快速索引的方式。首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，可以快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点。以此种方式构建的地图实现了每帧点云与位姿的解耦，这使得在改变位姿的时候可以同时改变地图，这为回环检测、离线构建高质量地图提供了接口。
针对系统长时间运行下由于外点累积导致地图中点云一致性下降的问题，设计了一个基于二维强度图与三维点云的外点剔除地图管理方法。针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云。针对长走廊等约束少的场景，并完善雷达SLAM系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性。
使用累计5秒内的点云Pj(j＝1,2,...,50)以及相应的位姿构建出局部地图M(L)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度。
对于局部地图M(L)中的任意一点p＝(x,y,z)，其在图像坐标系下的像素坐标(u,v)为：
其中round(·)表示一个取整函数，fu，fv，cu，cv四个参数表示的是与相机内参类似的参数，调整这些参数可以改变构建出来的图像的分辨率以及偏置的大小。
对于深度通道而言，其像素值VI(u,v)存储的是每个点的x轴的坐标值x：
VI(u,v)＝x (14)
本发明所设计的基于图像的外点剔除的主要目的是利用目前研究较为成熟的基于图像处理的方式，在一幅二维图像上找到具有“线”的性质的点，并剔除掉其余的外点。为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差。对于一个具有原始强度值I的点p而言，其在二维强度图像坐标系下的像素值可以定义为VI(u,v)：
其中log(·)表示一个对数函数，clamp(MIN,x,MAX)表示将一个变量x约束至下限MIN到上限MAX之间的函数。将VI(u,v)的像素值限定为(0,255)之间，构建的强度图可以视作为一个灰度图。
于是将问题转变为在二维灰度图像中寻找线段，采用基于区域生长的线段扩张LSD方法进行线段的生成。LSD方法从随机寻找一个像素开始，将该像素所在区域的角度表示为该像素所处的水平线的方向，不断地判断于该区域邻接的像素与夹角，若夹角在一定阈值内，则将该像素添加到区域内，重复该过程直到不能添加新的像素。
应用LSD对二维强度图进行线段提取后可以得到线段的两个端点，用深度通道的深度信息，可以将端点反映射为两个3D点ps＝(x1,y1,z1)和pe＝(x2,y2,z2)。于是利用恢复后的两个3D点，可以得到一个唯一的直线，其方程Ξ(x,y,z)可以表示为：
经过长时间的累计，外点数量过多可能导致基于二维图像的方式无法提取到线特征，于是进一步使用基于随机抽样一致性的方式提取三维线特征，剔除不符合线特征的强度角点以及几何角点。步骤方法如下：
1.对于一个点集，首先随机选择两个点，并使用公式(16)表示经过该两个点的三维直线方程。
2.接下来，选择一定阈值(如0.1m),点集内与当前拟合直线距离小于阈值的点为内点，大于阈值的为外点，计算内点外点的数量以及点线距离总和，用以衡量当前拟合直线效果好坏。
3.重复步骤1和步骤2，记录每一次拟合直线的直线方程以及拟合效果。
4.最后寻找出所有直线方程中内点数量最多，距离总和最小的线段方程表示最终的结果并将对应的外点剔除。
最后加入回环检测模块，首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置。剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环。其次，使用基于强度的扫描上下文判断回环。得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠。在判断满足回环的条件后，使用ICP进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧可以触发回环，进而触发基于因子图的位姿图优化。
表1展示了在室内部分退化环境下本方法与主流方法的轨迹误差分析表。表2是本发明与主流方法在不同场景下建图偏差分析对比表。表3是本发明在退化环境下出现退化的时间以及距离、角度误差分析对比表，其他方法均无法完整的进行建图，而本方法可以在保证不退化的同时得到很好的精度。
表1
表2
表3
图4是在室内极端退化环境下构建的局部地图，在此场景下，传统的基于几何的建图方法并不能提取到足够的特征点进行约束。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
Claims (2)
1.一种基于固态激光雷达的雷达惯性紧耦合定位建图方法，其特征是，采用固态激光雷达获取点云，对获取到的点云和惯性测量单元IMU数据进行预处理后，提取几何角点、几何平面点和强度角点；将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计；利用校正后的雷达点云强度信息，在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息；针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。
2.如权利要求1所述的基于固态激光雷达的雷达惯性紧耦合定位建图方法，其特征是，具体步骤如下：
数据预处理：接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，t时刻时，IMU的加速度为at，角速度为wt，IMU的输入表示为：
其中为t时刻IMU测量的原始加速度，/>为t时刻IMU测量的原始角速度，ba和bg是满足高斯噪声随机游动过程的IMU偏置，na和ng分别指的是测量加速度和角速度过程中存在的白噪声，Wg表示的是世界系下的重力矢量，IRW指的是从世界坐标系到IMU坐标系下的旋转矩阵，WRI指的是从IMU坐标系到世界坐标系下的旋转矩阵，两者有以下关系IRW＝(WRI)T，其中xT为转置符号；
于是，根据IMU的输入实时推断出机器人的状态，所述状态包含机器人的姿态R，速度v以及位置p，tk时刻与tk+1时刻之间的离散运动方程表示为：
其中表示tk+1时刻在世界坐标系下机器人位置的预测值，/>表示tk+1时刻在世界坐标系下机器人速度的预测值，/>表示tk+1时刻在世界坐标系下机器人位姿的预测值，/>表示tk时刻在世界坐标系下机器人的实际位置，/>表示tk时刻在世界坐标系下机器人的实际速度，/>表示tk时刻在世界坐标系下机器人的实际位姿，/>为tk时刻IMU测量的加速度，/>为tk时刻IMU测量的角速度，Δt＝tk+1-tk表示两个相邻IMU的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
由此，每次由IMU推断得到的状态信息包含了tk时刻的IMU坐标系下的姿态、速度以及位置信息；
考虑到雷达点云中存在的噪声以及AVIA特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤，对于点云去畸变部分，采用高频IMU里程计来对点云进行优化；
在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点，利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正，对于一根线束上的前后两个点pi和pi+1，其强度差ΔIi校正为：
其中αi、αi+1分别表示第i个点和第i+1个点的入射角角度，Ii和Ii+1分别表示第i个点和第i+1个点的原始强度值；
前端特征点提取：对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向，在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点；
在确定了对于局部区块的分配以及扩展方式之后，接下来的工作就是判断此区块内的点是否非平面点或者角点：
首先，针对点云中的一个局部区块P∈P，P表示一帧点云的集合，P是该帧点云下一个特定的局部区块，假设该区块内的所有点云对应着存在一个平面方程Π(x,y,z)，式中的x,y,z表示点的三维坐标，在此局部区块内，首先找到距离雷达原点最远的点pa，接着寻找离pa最远的点pb，最后利用如下公式寻找第三个pc：
表示在点云P中寻找使得X最大的点，通过寻找到的三个点描述该区块内的几何性质，从而拟合出平面方程Π(x,y,z)：
其中nx,ny,nz和表示的平面方程的参数通过以下方式计算：
其中表示的平面所代表的法向量，/>为局部区块里面点云P的质心，N为点云的数量，如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说那么这个局部区块中的所有点都被认为是平面点；
对于判断不满足平面方程的点，则继续判断是否满足角点的性质，如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，使用基于曲率的方式判断，如果曲率较大的话，该点即为角点；此外，对于已经经过扩展后仍不满足平面点的点集，使用基于PCA(Principal Component Analysis，主成分分析)分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点；
加入强度角点以增加后端位姿解算时的约束，点云的强度值取值范围是0-255，在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点；在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值；
在构建局部地图过程中，应用自适应体素地图进行构建；为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波，首先利用平面区块的法向量对不同区块进行聚类：
cosθ＝n1·n2
n1和n2分别为两个平面区块的法向量，如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面，对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m；
后端状态估计：对于一个LiDAR SLAM系统而言，无人机的状态的估计问题建模为一个极大似然估计问题：
其中Fk表示的是第k个关键帧下的特征点点集，表示状态的最大后验概率估计，表示目前观测到的六自由度位姿状态，p(·|·)表示状态与观测所满足的条件概率密度函数，f(·)为目标函数，在假设观测模型为高斯函数的情况下，最大后验概率问题转换为一个求解一个非线性最小二乘问题：
其中表示优化得到的旋转矩阵，/>表示优化得到的位置，fi(·)表示第i个观测得到的残差，N为所有观测数量之和；
地图管理：本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：
(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的；
(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一定阈值，则判定当前帧为关键帧；
(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧；
对于构建局部地图的过程，选择基于kd-tree(k-dimensional tree)快速索引的方式，首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点；
采用基于二维强度图与三维点云的外点剔除地图管理方法，针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云；
使用累计5秒内的点云Pj(j＝1,2,...,50)以及相应的位姿构建出局部地图M(L)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度，为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差，对于一个具有原始强度值I的点p而言，其在二维强度图像坐标系下的像素值定义为VI(u,v)：
其中log(·)表示一个对数函数，clamp(MIN,x,MAX)表示将一个变量x约束至下限MIN到上限MAX之间的函数，将VI(u,v)的像素值限定为(0,255)之间，构建的强度图视作为一个灰度图，于是将问题转变为在二维图像中寻找线段，采用基于区域生长的线段扩张LSD(Line Segment Detector)方法进行线段的生成,得到线段的两个端点，用深度通道的深度信息，将两个端点反映射为两个3D点ps＝(x1,y1,z1)和pe＝(x2,y2,z2)，于是利用恢复后的两个3D点，得到一个唯一的直线，其方程Ξ(x,y,z)表示为：
针对长走廊等约束少的场景，完善雷达SLAM系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性，首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置，剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环；其次，使用基于强度的扫描上下文判断回环，得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠；在判断满足回环的条件后，使用ICP(Iterative Closest Point)进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧触发回环，进而触发基于因子图的位姿图优化。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310263473.XA CN116449384A (zh) | 2023-03-17 | 2023-03-17 | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310263473.XA CN116449384A (zh) | 2023-03-17 | 2023-03-17 | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116449384A true CN116449384A (zh) | 2023-07-18 |
Family
ID=87126484
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310263473.XA Pending CN116449384A (zh) | 2023-03-17 | 2023-03-17 | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116449384A (zh) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116908810A (zh) * | 2023-09-12 | 2023-10-20 | 天津大学四川创新研究院 | 一种无人机搭载激光雷达测量建筑土方的方法和系统 |
| CN116958452A (zh) * | 2023-09-18 | 2023-10-27 | 北京格镭信息科技有限公司 | 三维重建方法和系统 |
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117387598A (zh) * | 2023-10-08 | 2024-01-12 | 北京理工大学 | 一种紧耦合轻量级的实时定位与建图方法 |
| CN117968680A (zh) * | 2024-03-29 | 2024-05-03 | 西安现代控制技术研究所 | 一种惯性-雷达组合导航有限帧量测变权重更新方法 |
-
2023
- 2023-03-17 CN CN202310263473.XA patent/CN116449384A/zh active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116908810A (zh) * | 2023-09-12 | 2023-10-20 | 天津大学四川创新研究院 | 一种无人机搭载激光雷达测量建筑土方的方法和系统 |
| CN116908810B (zh) * | 2023-09-12 | 2023-12-12 | 天津大学四川创新研究院 | 一种无人机搭载激光雷达测量建筑土方的方法和系统 |
| CN116958452A (zh) * | 2023-09-18 | 2023-10-27 | 北京格镭信息科技有限公司 | 三维重建方法和系统 |
| CN117387598A (zh) * | 2023-10-08 | 2024-01-12 | 北京理工大学 | 一种紧耦合轻量级的实时定位与建图方法 |
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117367412B (zh) * | 2023-12-07 | 2024-03-29 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117968680A (zh) * | 2024-03-29 | 2024-05-03 | 西安现代控制技术研究所 | 一种惯性-雷达组合导航有限帧量测变权重更新方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112435325B (zh) | 基于vi-slam和深度估计网络的无人机场景稠密重建方法 | |
| CN110223348B (zh) | 基于rgb-d相机的机器人场景自适应位姿估计方法 | |
| CN108986037B (zh) | 基于半直接法的单目视觉里程计定位方法及定位系统 | |
| CN116449384A (zh) | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 | |
| CN113436260B (zh) | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 | |
| CN114862949B (zh) | 一种基于点线面特征的结构化场景视觉slam方法 | |
| CN112233177B (zh) | 一种无人机位姿估计方法及系统 | |
| CN110688905B (zh) | 一种基于关键帧的三维物体检测与跟踪方法 | |
| CN113985445A (zh) | 一种基于相机与激光雷达数据融合的3d目标检测算法 | |
| CN113223045B (zh) | 基于动态物体语义分割的视觉与imu传感器融合定位系统 | |
| EP3293700B1 (en) | 3d reconstruction for vehicle | |
| CN112115980A (zh) | 基于光流跟踪和点线特征匹配的双目视觉里程计设计方法 | |
| CN105160649A (zh) | 基于核函数非监督聚类的多目标跟踪方法及系统 | |
| CN112418288A (zh) | 一种基于gms和运动检测的动态视觉slam方法 | |
| Ye et al. | Integrated image matching and segmentation for 3D surface reconstruction in urban areas | |
| WO2024114119A1 (zh) | 一种基于双目相机引导的传感器融合方法 | |
| CN110766782A (zh) | 基于多无人机视觉协同的大型施工场景实时重构方法 | |
| CN114549549B (zh) | 一种动态环境下基于实例分割的动态目标建模跟踪方法 | |
| CN115222884A (zh) | 一种基于人工智能的空间对象分析及建模优化方法 | |
| CN115355904A (zh) | 一种针对地面移动机器人的Lidar-IMU融合的slam方法 | |
| CN107610216B (zh) | 基于粒子群优化多视角立体点云生成方法及应用的摄像机 | |
| Rothermel et al. | Fast and robust generation of semantic urban terrain models from UAV video streams | |
| CN113536959A (zh) | 一种基于立体视觉的动态障碍物检测方法 | |
| CN116468786B (zh) | 一种面向动态环境的基于点线联合的语义slam方法 | |
| CN116147618B (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 |