基于固态激光雷达的雷达惯性紧耦合定位建图方法技术

本发明专利技术属于激光雷达、无人机及定位建图领域，为提出一种基于固态激光雷达的定位建图方法，实现在线获得准确地结构还原，且在长走廊、室内密闭环境等易出现严重退化的场景下可以得到较好的精度，本发明专利技术，基于固态激光雷达的雷达惯性紧耦合定位建图方法，采用固态激光雷达获取点云，对获取到的点云和惯性测量单元IMU数据进行预处理后，提取几何角点、几何平面点和强度角点；将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合；利用校正后的雷达点云强度信息，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。本发明专利技术主要应用于无人机设计制造场合。人机设计制造场合。人机设计制造场合。

【技术实现步骤摘要】
基于固态激光雷达的雷达惯性紧耦合定位建图方法


[0001]本专利技术属于定位建图领域，具体涉及基于固态激光雷达惯性紧耦合的自主定位与建图技术、基于雷达点云强度增强的雷达惯性里程计以及基于二维强度图和三维空间的地图维护方法。

技术介绍

[0002]近年来，四旋翼无人机的研发技术不断成熟，制作成本逐渐降低，已经在军事和民用领域展现出巨大的应用价值。通常，全球导航卫星系统(Global Navigation Satellite System,GNSS)用于在室外空旷场景下提供定位信息，然而这大大限制了无人机的应用场景，应用无人机搭载的传感器，如激光雷达、相机、毫米波雷达，惯性测量单元(Inertial Measurement Unit,IMU)等，以实时获取当前的位置信息以及周围的环境信息的同步定位与建图(SLAM)技术应运而生。
[0003]固态激光雷达有着价格便宜、重量轻、体积小、量程远、点云稠密等优良特性。然而由于该类型雷达具有非重复式扫描以及有限视场角的特性，传统的LiDAR SLAM方法并不适用，因此需要设计新的算法实现定位与建图的功能。对于LiDAR SLAM系统，IMU是不可或缺的，IMU预测得到的位置姿态以及速度等信息对于雷达点云畸变处理和输出高频平滑的里程计信息十分重要。
[0004]针对激光雷达的定位建图问题，国内外学者都进行了大量的研究。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，该系统具备一套新颖的异常点剔除方法，用于剔除位于边缘或在图像视图中被遮挡的不稳定映射点。
[0005]综上所述，国内外学者对激光雷达的定位建图问题进行了卓有成效的研究，并取得了丰硕的研究成果。然而，针对固态激光雷达的特殊非重复式扫描特性，需要设计相应的特征提取、位姿估计以及地图管理模块。同时，针对于长走廊、狭窄的办公室等室内场景导致的里程计退化问题广泛存在于激光雷达定位建图方法中。因此，研究基于固态激光雷达的雷达惯性紧耦合的自主定位与建图技术至关重要。

技术实现思路

[0006]为克服现有技术的不足，本专利技术旨在提出一种基于固态激光雷达的定位建图方法，使用本方法进行定位建图，可以在线获得准确地结构还原，且在长走廊、室内密闭环境
等易出现严重退化的场景下可以得到较好的精度。为此，本专利技术采取的技术方案是，基于固态激光雷达的雷达惯性紧耦合定位建图方法，采用固态激光雷达获取点云，对获取到的点云和惯性测量单元IMU数据进行预处理后，提取几何角点、几何平面点和强度角点；将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计；利用校正后的雷达点云强度信息，在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息；针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。
[0007]具体步骤如下：
[0008]数据预处理：接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，t时刻时，IMU的加速度为a
t
，角速度为w
t
，IMU的输入表示为:
[0009][0010][0011]其中为t时刻IMU测量的原始加速度，为t时刻IMU测量的原始角速度，b
a
和b
g
是满足高斯噪声随机游动过程的IMU偏置，n
a
和n
g
分别指的是测量加速度和角速度过程中存在的白噪声，
W
g表示的是世界系下的重力矢量，
I
R
W
指的是从世界坐标系到IMU坐标系下的旋转矩阵，
W
R
I
指的是从IMU坐标系到世界坐标系下的旋转矩阵，两者有以下关系
I
R
W
＝(
W
R
I
)
T
，其中x
T
为转置符号；
[0012]于是，根据IMU的输入可以实时推断出机器人的状态，所述状态包含机器人的姿态R，速度v以及位置p，t
k
时刻与t
k+1
时刻之间的离散运动方程表示为：
[0013][0014][0015][0016]其中表示t
k+1
时刻在世界坐标系下机器人位置的预测值，表示t
k+1
时刻在世界坐标系下机器人速度的预测值，表示t
k+1
时刻在世界坐标系下机器人位姿的预测值，表示t
k
时刻在世界坐标系下机器人的实际位置，表示t
k
时刻在世界坐标系下机器人的实际速度，表示t
k
时刻在世界坐标系下机器人的实际位姿，为t
k
时刻IMU测量的加速度，为t
k
时刻IMU测量的角速度，Δt＝t
k+1
‑
t
k
表示两个相邻IMU的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
[0017][0018][0019]由此可以看出，每次由IMU推断得到的状态信息包含了t
k
时刻的IMU坐标系下的姿态、速度以及位置信息；
[0020]考虑到雷达点云中存在的噪声以及AVIA特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤，对于点云去畸变部分，采用高频IMU里程计来对点云进行优化；
[0021]在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点，利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正，对于一根线束上的前后两个点p
i
和p
i+1
，其强度差ΔI
i
校正为：
[0本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于固态激光雷达的雷达惯性紧耦合定位建图方法，其特征是，采用固态激光雷达获取点云，对获取到的点云和惯性测量单元IMU数据进行预处理后，提取几何角点、几何平面点和强度角点；将IMU预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计；利用校正后的雷达点云强度信息，在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息；针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。2.如权利要求1所述的基于固态激光雷达的雷达惯性紧耦合定位建图方法，其特征是，具体步骤如下：数据预处理：接收到IMU输入后，对IMU数据进行预处理用以实现实时预测机器人状态，t时刻时，IMU的加速度为a
t
，角速度为w
t
，IMU的输入表示为：，IMU的输入表示为：其中为t时刻IMU测量的原始加速度，为t时刻IMU测量的原始角速度，b
a
和b
g
是满足高斯噪声随机游动过程的IMU偏置，n
a
和n
g
分别指的是测量加速度和角速度过程中存在的白噪声，
W
g表示的是世界系下的重力矢量，
I
R
W
指的是从世界坐标系到IMU坐标系下的旋转矩阵，
W
R
I
指的是从IMU坐标系到世界坐标系下的旋转矩阵，两者有以下关系
I
R
W
＝(
W
R
I
)
T
，其中x
T
为转置符号；于是，根据IMU的输入实时推断出机器人的状态，所述状态包含机器人的姿态R，速度v以及位置p，t
k
时刻与t
k+1
时刻之间的离散运动方程表示为：时刻之间的离散运动方程表示为：时刻之间的离散运动方程表示为：其中表示t
k+1
时刻在世界坐标系下机器人位置的预测值，表示t
k+1
时刻在世界坐标系下机器人速度的预测值，表示t
k+1
时刻在世界坐标系下机器人位姿的预测值，表示t
k
时刻在世界坐标系下机器人的实际位置，表示t
k
时刻在世界坐标系下机器人的实际速度，表示t
k
时刻在世界坐标系下机器人的实际位姿，为t
k
时刻IMU测量的加速度，为t
k
时刻IMU测量的角速度，Δt＝t
k+1
‑
t
k
表示两个相邻IMU的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
由此，每次由IMU推断得到的状态信息包含了t
k
时刻的IMU坐标系下的姿态、速度以及位置信息；考虑到雷达点云中存在的噪声以及AVIA特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤，对于点云去畸变部分，采用高频IMU里程计来对点云进行优化；在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点，利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正，对于一根线束上的前后两个点p
i
和p
i+1
，其强度差ΔI
i
校正为：其中α
i
、α
i+1
分别表示第i个点和第i+1个点的入射角角度，I
i
和I
i+1
分别表示第i个点和第i+1个点的原始强度值；前端特征点提取：对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向，在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点；在确定了对于局部区块的分配以及扩展方式之后，接下来的工作就是判断此区块内的点是否非平面点或者角点：首先，针对点云中的一个局部区块P∈P，P表示一帧点云的集合，P是该帧点云下一个特定的局部区块，假设该区块内的所有点云对应着存在一个平面方程Π(x,y,z)，式中的x,y,z表示点的三维坐标，在此局部区块内，首先找到距离雷达原点最远的点p
a
，接着寻找离p
a
最远的点p
b
，最后利用如下公式寻找第三个p
c
：：表示在点云P中寻找使得X最大的点，通过寻找到的三个点描述该区块内的几何性质，从而拟合出平面方程Π(x,y,z)：其中n
x
,n
y
,n
z
和表示的平面方程的参数通过以下方式计算：表示的平面方程的参数通过以下方式计算：表示的平面方程的参数通过以下方式计算：其中表示的平面所代表的法向量，为局部区块里面点云P的质心，N为点云的数量，
如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说那么这个局部区块中...

【专利技术属性】
技术研发人员：田栢苓，孙华清，李海松，
申请(专利权)人：天津大学，
类型：发明
国别省市：