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

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


背景技术：

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


技术实现要素：

6.为克服现有技术的不足，本发明旨在提出一种基于固态激光雷达的定位建图方法，使用本方法进行定位建图，可以在线获得准确地结构还原，且在长走廊、室内密闭环境
等易出现严重退化的场景下可以得到较好的精度。为此，本发明采取的技术方案是，基于固态激光雷达的雷达惯性紧耦合定位建图方法，采用固态激光雷达获取点云，对获取到的点云和惯性测量单元imu数据进行预处理后，提取几何角点、几何平面点和强度角点；将imu预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计；利用校正后的雷达点云强度信息，在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息；针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。
7.具体步骤如下：
8.数据预处理：接收到imu输入后，对imu数据进行预处理用以实现实时预测机器人状态，t时刻时，imu的加速度为a
t
，角速度为w
t
，imu的输入表示为:
[0009][0010][0011]
其中为t时刻imu测量的原始加速度，为t时刻imu测量的原始角速度，ba和bg是满足高斯噪声随机游动过程的imu偏置，na和ng分别指的是测量加速度和角速度过程中存在的白噪声，wg表示的是世界系下的重力矢量，
irw
指的是从世界坐标系到imu坐标系下的旋转矩阵，
wri
指的是从imu坐标系到世界坐标系下的旋转矩阵，两者有以下关系
irw
＝(
wri
)
t
，其中x
t
为转置符号；
[0012]
于是，根据imu的输入可以实时推断出机器人的状态，所述状态包含机器人的姿态r，速度v以及位置p，tk时刻与t
k+1
时刻之间的离散运动方程表示为：
[0013][0014][0015][0016]
其中表示t
k+1
时刻在世界坐标系下机器人位置的预测值，表示t
k+1
时刻在世界坐标系下机器人速度的预测值，表示t
k+1
时刻在世界坐标系下机器人位姿的预测值，表示tk时刻在世界坐标系下机器人的实际位置，表示tk时刻在世界坐标系下机器人的实际速度，表示tk时刻在世界坐标系下机器人的实际位姿，为tk时刻imu测量的加速度，为tk时刻imu测量的角速度，δt＝t
k+1-tk表示两个相邻imu的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
[0017]
[0018][0019]
由此可以看出，每次由imu推断得到的状态信息包含了tk时刻的imu坐标系下的姿态、速度以及位置信息；
[0020]
考虑到雷达点云中存在的噪声以及avia特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤，对于点云去畸变部分，采用高频imu里程计来对点云进行优化；
[0021]
在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点，利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正，对于一根线束上的前后两个点pi和p
i+1
，其强度差δii校正为：
[0022][0023]
其中αi、α
i+1
分别表示第i个点和第i+1个点的入射角角度，ii和i
i+1
分别表示第i个点和第i+1个点的原始强度值；
[0024]
前端特征点提取：对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向，在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点；
[0025]
在确定了对于局部区块的分配以及扩展方式之后，接下来的工作就是判断此区块内的点是否非平面点或者角点：
[0026]
首先，针对点云中的一个局部区块p∈p，p表示一帧点云的集合，p是该帧点云下一个特定的局部区块，假设该区块内的所有点云对应着存在一个平面方程π(x,y,z)，式中的x,y,z表示点的三维坐标，在此局部区块内，首先找到距离雷达原点最远的点pa，接着寻找离pa最远的点pb，最后利用如下公式寻找第三个pc：
[0027][0028]
表示在点云p中寻找使得x最大的点，通过寻找到的三个点可以描述该区块内的几何性质，从而拟合出平面方程π(x,y,z)：
[0029][0030]
其中n
x
,ny,nz和表示的平面方程的参数通过以下方式计算：
[0031][0032][0033][0034]
其中表示的平面所代表的法向量，为局部区块里面点云p的质心，n为点云的数
量。如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说那么这个局部区块中的所有点都被认为是平面点；
[0035]
对于判断不满足平面方程的点，则继续判断是否满足角点的性质，如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，使用基于曲率的方式判断，如果曲率较大的话，该点即为角点；此外，对于已经经过扩展后仍不满足平面点的点集，使用基于pca(principal component analysis，主成分分析)分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点；
[0036]
加入强度角点以增加后端位姿解算时的约束，点云的强度值取值范围是0-255，在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点；在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值；
[0037]
在构建局部地图过程中，应用自适应体素地图进行构建；为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波，首先利用平面区块的法向量对不同区块进行聚类：
[0038]
cosθ＝n1·
n2[0039]
n1和n2分别为两个平面区块的法向量，如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面，对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m；
[0040]
后端状态估计：对于一个lidar slam系统而言，无人机的状态的估计问题建模为一个极大似然估计问题：
[0041][0042]
其中fk表示的是第k个关键帧下的特征点点集，表示状态的最大后验概率估计，表示目前观测到的六自由度位姿状态，p(
·
|
·
)表示状态与观测所满足的条件概率密度函数，f(
·
)为目标函数。在假设观测模型为高斯函数的情况下，最大后验概率问题转换为一个求解一个非线性最小二乘问题：
[0043][0044]
其中表示优化得到的旋转矩阵，表示优化得到的位置，fi(
·
)表示第i个观测得到的残差，n为所有观测数量之和；
[0045]
地图管理：本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：
[0046]
(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的；
[0047]
(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一定阈值，则判定当前帧为关键帧；
[0048]
(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧；
[0049]
对于构建局部地图的过程，选择基于kd-tree(k-dimensional tree)快速索引的方式，首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点；
[0050]
采用基于二维强度图与三维点云的外点剔除地图管理方法，针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云；
[0051]
使用累计5秒内的点云pj(j＝1,2,...,50)以及相应的位姿构建出局部地图m(l)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度。为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差，对于一个具有原始强度值i的点p而言，其在二维强度图像坐标系下的像素值可以定义为vi(u,v)：
[0052][0053]
其中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)可以表示为：
[0054][0055]
针对长走廊等约束少的场景，完善雷达slam系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性，首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置，剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环；其次，使用基于强度的扫描上下文判断回环。得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠；在判断满足回环的条件后，使用icp(iterative closest point)进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧可以触发回环，进而触发基于因子图的位姿图优化。
[0056]
本发明的特点及有益效果是：
[0057]
1、本发明提出了基于固态激光雷达的惯性紧耦合定位建图方法，首先对imu数据
处理后得到高频imu预测位姿，对雷达原始点云进行外点剔除以及去畸变后，在前端提取得到平面点和角点；在后端位姿解算时，使用基于点线点面的匹配方式，利用高斯牛顿迭代优化求解低频雷达里程计位姿，最后与imu预测位姿紧耦合，使用isam(incremental smoothing and mapping)求解得到高频精准的无人机位姿。在不同的场景下均具有较稳定的效果，能够很好的平衡时间效率和精度之间的要求，针对室外大环境以及室内密闭环境可以在保证实时性的同时建立一致性良好的地图。
[0058]
2、本发明设计了基于雷达点云强度增强的鲁棒雷达惯性里程计，利用点云强度值在数据预处理阶段进一步剔除外点。在特征提取阶段，不仅考虑了具有几何信息的角点以及平面点，还在校正后的强度基础上提取了强度角点，为后端提供更为的位姿约束。对于后端，分别对角点以及平面点设计了相应的权重，考虑到了几何、强度、配准效果等信息，调节了不同质量的特征点对于位姿解算的贡献。在约束充足的场景下，该方法与最先进的框架定位精度相当，在约束不足易导致退化的场景下，仍可以表现出高鲁棒性。
[0059]
3、本发明设计了一个基于二维强度图与三维空间的外点剔除地图维护方法，在前端对于平面点使用自适应体素滤波的策略，实现对聚类后不同点云的均匀采样。首先，将三维点云投影成二维强度图，使用基于lsd的方法提取线段并剔除不具有“线”的属性的点。然后，使用基于ransac(random sample consensus)的方法对局部地图中的角点进行直线拟合并剔除外点。最后，新增基于因子图的位姿图优化环节，在检测到发生回环时，对全局关键帧位姿进行优化，从而更新全局地图，增加地图的一致性。该算法相较于其他主流方法在正常场景和部分退化场景下可以实现实时、精准的定位与建图，并且在部分极端场景下能保持长时间较为稳定地运行。
附图说明：
[0060]
图1为搭载了livox avia固态激光雷达的无人机平台；
[0061]
图2为基于固态激光雷达的雷达惯性紧耦合设计结构图；
[0062]
图3为走廊场景下和室内场景的强度角点图；
[0063]
图4为极端退化环境下构建的局部地图。
具体实施方式
[0064]
通过imu惯性测量单元解析出无人机的位姿信息、基于雷达点云强度增强的鲁棒雷达惯性里程计和基于二维强度图和三维点云的外点剔除地图管理方法，实现无人机在严重退化场景下的鲁棒定位建图。结构如下：采用固态激光雷达获取点云。在前端中，对获取到的点云和imu数据进行预处理后，设计特征提取方法提取几何角点、几何平面点和强度角点；在后端中，将imu预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计。考虑到室内场景可能存在的退化问题，利用校正后的雷达点云强度信息，在几何面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息。针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。针对设计方案，将本方法的核心部分分为前端和后端两个模块，其中前端主要对预处理后的数据进行特征点选取，后端基于前端提取出的特
征点进行状态估计。
[0065]
基于固态激光雷达的雷达惯性紧耦合里程计选用的是livox avia固态激光雷达，为了适配该雷达的点云结构，设计了新的雷达惯性slam算法，分为四个部分：数据预处理、前端特征点提取、后端状态估计以及地图管理。
[0066]
数据预处理：接收到imu输入后，对imu数据进行预处理用以实现实时预测机器人状态。假设t时刻时，imu的加速度为a
t
，角速度为w
t
，imu的输入可以表示为:
[0067][0068][0069]
其中为t时刻imu测量的原始加速度，为t时刻imu测量的原始角速度，ba和bg是满足高斯噪声随机游动过程的imu偏置，na和ng分别指的是测量加速度和角速度过程中存在的白噪声。wg表示的是世界系下的重力矢量，
irw
指的是从世界坐标系到imu坐标系下的旋转矩阵，
wri
指的是从imu坐标系到世界坐标系下的旋转矩阵，两者有以下关系
irw
＝(
wri
)
t
，其中x
t
为转置符号。
[0070]
于是，根据imu的输入可以实时推断出机器人的状态，本文的状态包含机器人的姿态r，速度v以及位置p。因此，tk时刻与t
k+1
时刻之间的离散运动方程可以表示为：
[0071][0072][0073][0074]
其中表示t
k+1
时刻在世界坐标系下机器人位置的预测值，表示t
k+1
时刻在世界坐标系下机器人速度的预测值，表示t
k+1
时刻在世界坐标系下机器人位姿的预测值，表示tk时刻在世界坐标系下机器人的实际位置，表示tk时刻在世界坐标系下机器人的实际速度，表示tk时刻在世界坐标系下机器人的实际位姿，为tk时刻imu测量的加速度，为tk时刻imu测量的角速度，δt＝t
k+1-tk表示两个相邻imu的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
[0075][0076][0077]
由此可以看出，每次由imu推断得到的状态信息包含了tk时刻的imu坐标系下的姿态、速度以及位置信息。
[0078]
考虑到雷达点云中存在的噪声以及avia特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤。对于点
云去畸变部分，本发明采用高频imu里程计来对点云进行优化。
[0079]
在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点。利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正。对于一根线束上的前后两个点pi和p
i+1
，其强度差δii可以校正为：
[0080][0081]
其中αi、α
i+1
分别表示第i个点和第i+1个点的入射角角度，ii和i
i+1
分别表示第i个点和第i+1个点的原始强度值。
[0082]
前端特征点提取：对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向。在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点。
[0083]
在确定了对于局部区块的分配以及扩展方式之后，接下来的工作就是判断此区块内的点是否非平面点或者角点。
[0084]
首先，针对点云中的一个局部区块p∈p，p表示一帧点云的集合，p是该帧点云下一个特定的局部区块。假设该区块内的所有点云对应着存在一个平面方程π(x,y,z)，式中的x,y,z表示点的三维坐标。在此局部区块内，首先找到距离雷达原点最远的点pa，接着寻找离pa最远的点pb，最后利用如下公式寻找第三个pc：
[0085][0086]
表示在点云p中寻找使得x最大的点，通过寻找到的三个点可以描述该区块内的几何性质，从而拟合出平面方程π(x,y,z)：
[0087][0088]
其中n
x
,ny,nz和表示的平面方程的参数通过以下方式计算：
[0089][0090][0091][0092]
其中表示的平面所代表的法向量，为局部区块里面点云p的质心，n为点云的数量。如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说说那么这个局部区块中的所有点都被认为是平面点。
[0093]
对于判断不满足平面方程的点，则继续判断是否满足角点的性质。如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，于是可以使用基于曲率的方式判断，如果曲率较大的话，该点即为角点。此外，对于已经经过扩展后仍不满足平面点的点集，使用基于pca分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大
特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点。
[0094]
除了传统的几何角点和几何平面点，本发明加入了强度角点以增加后端位姿解算时的约束。点云的强度值取值范围是0-255，本发明在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点。在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值。
[0095]
在构建局部地图过程中，为了减小地图所占用的空间，加快地图索引的效率，应用自适应体素地图进行构建。为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波。首先利用平面区块的法向量对不同区块进行聚类：
[0096]
cosθ＝n1·
n2[0097]
n1和n2分别为两个平面区块的法向量，如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面。对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m。
[0098]
后端状态估计：对于一个lidar slam系统而言，无人机的状态的估计问题可以建模为一个极大似然估计问题：
[0099][0100]
其中fk表示的是第k个关键帧下的特征点点集，表示状态的最大后验概率估计，表示目前观测到的六自由度位姿状态，p(
·
|
·
)表示状态与观测所满足的条件概率密度函数，f(
·
)为目标函数。在假设观测模型为高斯函数的情况下，最大后验概率问题可以转换为一个求解一个非线性最小二乘问题：
[0101][0102]
其中表示优化得到的旋转矩阵，表示优化得到的位置，fi(
·
)表示第i个观测得到的残差，n为所有观测数量之和。
[0103]
地图管理：本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：
[0104]
(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的。本发明中选择每隔2帧进行一次关键帧判定的策略。
[0105]
(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一定阈值，则判定当前帧为关键帧。
[0106]
(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧。
[0107]
对于构建局部地图的过程，本发明选择的是基于kd-tree快速索引的方式。首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，可以快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点。以此种方式构建的地图实现了每帧点云与位姿的解耦，这使得在改变位姿的时候可以同时改变地图，这为回环检测、离线
构建高质量地图提供了接口。
[0108]
由于系统长时间运行下回出现外点累积，导致地图中点云一致性下降，本发明针对此问题设计了一个基于二维强度图与三维点云的外点剔除地图管理方法。针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云。
[0109]
使用累计5秒内的点云pj(j＝1,2,...,50)以及相应的位姿构建出局部地图m(l)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度。为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差。对于一个具有原始强度值i的点p而言，其在二维强度图像坐标系下的像素值可以定义为vi(u,v)：
[0110][0111]
其中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)可以表示为：
[0112][0113]
针对长走廊等约束少的场景，完善雷达slam系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性。首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置。剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环。其次，使用基于强度的扫描上下文判断回环。得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠。在判断满足回环的条件后，使用icp进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧可以触发回环，进而触发基于因子图的位姿图优化。
[0114]
以下结合附图对本发明的技术方案作进一步说明。
[0115]
本发明提出了一种基于固态激光雷达的定位建图方法，解决室内场景下雷达里程计退化的问题。本方法选用的是livox avia固态激光雷达，搭载了固态激光雷达的无人机如图1所示。为了适配该雷达的点云结构，设计了新的雷达惯性slam算法，分为四个部分：数据预处理、前端特征点提取、后端状态估计以及地图管理。结构如图2所示：数据预处理部分采用固态激光雷达获取点云，对获取到的点云和imu数据进行预处理。特征提取部分设计特征提取方法提取几何角点和几何平面点，并对几何平面点进行了自适应体素滤波。在几何
面特征的基础上提取强度角点，以几何信息和强度信息对平面点和角点分别设计权重，进而求解无人机的位姿信息。后端状态估计部分，将imu预测得到的位姿与雷达观测信息以紧耦合的方式进行融合，求解得到六自由度的位姿估计并对因子图进行优化。在地图管理部分，使用基于关键帧的策略实现高效的地图存储，针对系统长时间运行下强度角点由于外点积累的影响退化成面特征的情况，将三维点云投影为二维图像，使用基于图像以及基于三维空间相辅的方法实现对地图中外点的剔除。
[0116]
具体实现方法如下：
[0117]
系统在接收到imu输入后，对imu进行预处理以实现实时预测机器人状态。假设t时刻时，imu的加速度为a
t
，角速度为w
t
，imu的输入可以表示为:
[0118][0119]
其中为t时刻imu测量的原始加速度，为t时刻imu测量的原始角速度，ba和bg是满足高斯噪声随机游动过程的imu偏置，na和ng分别指的是测量加速度和角速度过程中存在的白噪声。wg表示的是世界系下的重力矢量，
irw
指的是从世界坐标系到imu坐标系下的旋转矩阵，
wri
指的是从imu坐标系到世界坐标系下的旋转矩阵，两者有以下关系
irw
＝(
wri
)
t
，其中x
t
为转置符号。
[0120]
于是，根据imu的输入可以实时推断出机器人的状态，本文的状态包含机器人的姿态r，速度v以及位置p。因此，tk时刻与t
k+1
时刻之间的离散运动方程可以表示为：
[0121][0122]
其中表示t
k+1
时刻在世界坐标系下机器人位置的预测值，表示t
k+1
时刻在世界坐标系下机器人速度的预测值，表示t
k+1
时刻在世界坐标系下机器人位姿的预测值，表示tk时刻在世界坐标系下机器人的实际位置，表示tk时刻在世界坐标系下机器人的实际速度，表示tk时刻在世界坐标系下机器人的实际位姿，为tk时刻imu测量的加速度，为tk时刻imu测量的角速度，δt＝t
k+1-tk表示两个相邻imu的时间差，φ^∈so(3)表示的是向量φ经过反对称操作后满足李代数的性质，李代数与李群满足如下指数映射：
[0123][0124]
由此可以看出，每次由imu推断得到的状态信息包含了tk时刻的imu坐标系下的姿态、速度以及位置信息。但对于每一时刻的状态变化都需要重新计算运动方程，于是采用预
积分的方法减少计算量。将参考系从世界系转换到机体系下后，可以使用imu原始加速度和原始角速度得到imu预积分部分：
[0125][0126]
其中分别表示世界坐标系下，在tk与t
k+1
两时刻之间关于位置、速度以及姿态的预积分量，表示t
k+1
时刻在世界坐标系下机器人的实际位置。由于该预积分量计算的是相邻两帧的与状态无关的积分量，这使得在每一次状态重新传播的过程中无需再次计算此部分的量，大大减少了计算量。
[0127]
考虑到雷达点云p中存在的噪声以及avia特殊的非重复式扫描形式，在进行特征提取前需要进行雷达点云的预处理，分为点云外点剔除和点云去畸变两个处理步骤。对于点云去畸变部分，本发明采用高频imu里程计来对点云进行优化。
[0128]
为了实现去畸变，将采样时间为ti∈[tk,t
k+1
]的所有点投影到扫描结束时间t
k+1
中，对于时域中的每一点，搜索时域上与imu预测位姿时间戳最近的位姿变换矩阵于是关于雷达坐标系l下第i个点的三维坐标即可转换为
[0129][0130]
其中it
l
为雷达与imu之间的内参矩阵，该矩阵在avia出厂时已经标定好。
[0131]
在外点剔除步骤中，会剔除盲区、视角边缘、入射角过大以及被遮挡物遮挡的点。利用强度信息进一步剔除一些不可靠的点，针对同一线束上前后两个点进行强度差值校正。对于一根线束上的前后两个点pi和p
i+1
，其强度差δii可以校正为：
[0132][0133]
其中αi、α
i+1
分别表示第i个点和第i+1个点的入射角角度，ii和i
i+1
分别表示第i个点和第i+1个点的原始强度值。
[0134]
对于一帧的雷达点，利用其时域内的有序性决定扩展局部区块的方向。在判断使用最近邻策略搜索得到的邻域点数量不足时，会在每一根线束以时域的方向去搜索直至得到足够多数量的点。在确定了对于局部区块的分配以及扩展方式之后，需要判断此区块内的点是否为非平面点或者角点。
[0135]
针对每一个局部区块p∈p，假设该区块内的所有点云对应着存在一个平面方程π(x,y,z)。在此局部区块内，首先找到距离雷达原点最远的点pa，接着寻找距离pa最远的点pb，最后利用如下公式寻找第三个pc：
[0136]
[0137]
表示在点云p中寻找使得x最大的点，通过寻找到的三个点可以描述该区块内的几何性质，从而拟合出平面方程π(x,y,z)：
[0138][0139]
其中n
x
,ny,nz和表示的平面方程的参数通过以下方式计算：
[0140][0141]
其中表示的平面所代表的法向量，为局部区块里面点云p的质心，n为点云的数量。如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说量。如果没有一个点与拟合平面的距离大于平均局部区块大小(1m)的十分之一，也就是说那么这个局部区块中的所有点都被认为是平面点。
[0142]
对于判断不满足平面方程的点，则继续判断是否满足角点的性质。如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，于是可以使用基于曲率的方式判断，如果曲率较大的话，该点即为角点。此外，对于已经经过扩展后仍不满足平面点的点集，使用基于pca分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点。
[0143]
除了传统的几何角点和几何平面点，本发明加入了强度角点以增加后端位姿解算时的约束。点云的强度值取值范围是0-255，本发明在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点。在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值。图3是在走廊场景下和室内场景下存在退化环境的强度角点图，可以看到在环境中存在的用其他主流算法无法提取出的如展板边缘，天花板缝隙等肉眼可分辨的边缘，通过强度角点提取可以成功得到约束。
[0144]
在构建局部地图过程中，为了减小地图所占用的空间，加快地图索引的效率，应用自适应体素地图进行构建。为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波。首先利用平面区块的法向量对不同区块进行聚类：
[0145]
cosθ＝n1·
n2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0146]
如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面。对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m。
[0147]
对于一个lidar slam系统而言，无人机的状态的估计问题可以建模为一个极大似然估计问题：
[0148][0149]
其中fk表示的是第k个关键帧下的特征点点集，表示待优化求解的六自由度
位姿，p(
·
)表示状态与观测所满足的概率密度函数，f(
·
)为目标函数。在假设观测模型为高斯的情况下，最大后验概率问题可以转换为一个求解一个非线性最小二乘问题：
[0150][0151]
其中表示优化得到的旋转矩阵，表示优化得到的位置，fi(
·
)表示第i个观测得到的残差，n为所有观测数量之和。
[0152]
本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：
[0153]
(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的。本发明中选择每隔2帧进行一次关键帧判定的策略。
[0154]
(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一定阈值，则判定当前帧为关键帧。
[0155]
(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧。
[0156]
对于局部地图的构建，本发明选择的是基于kd-tree快速索引的方式。首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，可以快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点。以此种方式构建的地图实现了每帧点云与位姿的解耦，这使得在改变位姿的时候可以同时改变地图，这为回环检测、离线构建高质量地图提供了接口。
[0157]
针对系统长时间运行下由于外点累积导致地图中点云一致性下降的问题，设计了一个基于二维强度图与三维点云的外点剔除地图管理方法。针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云。针对长走廊等约束少的场景，并完善雷达slam系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性。
[0158]
使用累计5秒内的点云pj(j＝1,2,...,50)以及相应的位姿构建出局部地图m(l)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度。
[0159]
对于局部地图m(l)中的任意一点p＝(x,y,z)，其在图像坐标系下的像素坐标(u,v)为：
[0160][0161]
其中round(
·
)表示一个取整函数，fu，fv，cu，cv四个参数表示的是与相机内参类似的参数，调整这些参数可以改变构建出来的图像的分辨率以及偏置的大小。
[0162]
对于深度通道而言，其像素值vi(u,v)存储的是每个点的x轴的坐标值x：
[0163]vi
(u,v)＝x
ꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0164]
本发明所设计的基于图像的外点剔除的主要目的是利用目前研究较为成熟的基
于图像处理的方式，在一幅二维图像上找到具有“线”的性质的点，并剔除掉其余的外点。为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差。对于一个具有原始强度值i的点p而言，其在二维强度图像坐标系下的像素值可以定义为vi(u,v)：
[0165][0166]
其中log(
·
)表示一个对数函数，clamp(min,x,max)表示将一个变量x约束至下限min到上限max之间的函数。将vi(u,v)的像素值限定为(0,255)之间，构建的强度图可以视作为一个灰度图。
[0167]
于是将问题转变为在二维灰度图像中寻找线段，采用基于区域生长的线段扩张lsd方法进行线段的生成。lsd方法从随机寻找一个像素开始，将该像素所在区域的角度表示为该像素所处的水平线的方向，不断地判断于该区域邻接的像素与夹角，若夹角在一定阈值内，则将该像素添加到区域内，重复该过程直到不能添加新的像素。
[0168]
应用lsd对二维强度图进行线段提取后可以得到线段的两个端点，用深度通道的深度信息，可以将端点反映射为两个3d点ps＝(x1,y1,z1)和pe＝(x2,y2,z2)。于是利用恢复后的两个3d点，可以得到一个唯一的直线，其方程ξ(x,y,z)可以表示为：
[0169][0170]
经过长时间的累计，外点数量过多可能导致基于二维图像的方式无法提取到线特征，于是进一步使用基于随机抽样一致性的方式提取三维线特征，剔除不符合线特征的强度角点以及几何角点。步骤方法如下：
[0171]
1.对于一个点集，首先随机选择两个点，并使用公式(16)表示经过该两个点的三维直线方程。
[0172]
2.接下来，选择一定阈值(如0.1m),点集内与当前拟合直线距离小于阈值的点为内点，大于阈值的为外点，计算内点外点的数量以及点线距离总和，用以衡量当前拟合直线效果好坏。
[0173]
3.重复步骤1和步骤2，记录每一次拟合直线的直线方程以及拟合效果。
[0174]
4.最后寻找出所有直线方程中内点数量最多，距离总和最小的线段方程表示最终的结果并将对应的外点剔除。
[0175]
最后加入回环检测模块，首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置。剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环。其次，使用基于强度的扫描上下文判断回环。得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠。在判断满足回环的条件后，使用icp进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧可以触发回环，进而触发基
于因子图的位姿图优化。
[0176]
表1展示了在室内部分退化环境下本方法与主流方法的轨迹误差分析表。表2是本发明与主流方法在不同场景下建图偏差分析对比表。表3是本发明在退化环境下出现退化的时间以及距离、角度误差分析对比表，其他方法均无法完整的进行建图，而本方法可以在保证不退化的同时得到很好的精度。
[0177]
表1
[0178][0179][0180]
表2
[0181][0182]
表3
[0183][0184]
图4是在室内极端退化环境下构建的局部地图，在此场景下，传统的基于几何的建图方法并不能提取到足够的特征点进行约束。
[0185]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

技术特征：
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)的十分之一，也就是说那么这个局部区块中的所有点都被认为是平面点；对于判断不满足平面方程的点，则继续判断是否满足角点的性质，如果该局部区块内的点并未经过区块扩展，这意味着该区域内点云足够稠密，使用基于曲率的方式判断，如果曲率较大的话，该点即为角点；此外，对于已经经过扩展后仍不满足平面点的点集，使用基于pca(principal component analysis，主成分分析)分解的方式去判断点集内的点是否具有“线”的性质，如果得到的最大特征值大于第二大特征值的三倍，则认为该局部区块内的点为角点；加入强度角点以增加后端位姿解算时的约束，点云的强度值取值范围是0-255，在数据预处理步骤中已经获取了同一线束上前后两点的强度差，选取25作为阈值，大于该阈值的点视作强度角点；在获取了几何角点、几何面特征以及强度角点后，根据几何信息、强度信息和配准过程的信息，分别为角点和平面点设计相应权重，区分各个点对位姿解算的贡献值；在构建局部地图过程中，应用自适应体素地图进行构建；为了保证体素内点属性的一致性，只针对平面点进行自适应体素滤波，首先利用平面区块的法向量对不同区块进行聚类：cosθ＝n1·
n2n1和n2分别为两个平面区块的法向量，如果两个法向量n1和n2的夹角θ小于一定阈值，则判定当前两个平面为同一类型的平面，对聚类后得到的点云进行数量上的判断，对于不同数量的点云采取不同的滤波参数，如大于500个的聚类后点云采用0.4m的参数，大于200的采用0.2m，小于100的采用0.1m；后端状态估计：对于一个lidar slam系统而言，无人机的状态的估计问题建模为一个极大似然估计问题：其中f
k
表示的是第k个关键帧下的特征点点集，表示状态的最大后验概率估计，表示目前观测到的六自由度位姿状态，p(
·
|
·
)表示状态与观测所满足的条件概率密度函数，f(
·
)为目标函数，在假设观测模型为高斯函数的情况下，最大后验概率问题转换为一个求解一个非线性最小二乘问题：其中表示优化得到的旋转矩阵，表示优化得到的位置，f
i
(
·
)表示第i个观测得到的残差，n为所有观测数量之和；地图管理：本发明使用基于关键帧的策略实现高效的地图存储，选择一定的策略，若当前帧点云满足一定的条件，则将当前帧点云加入地图：(1)由于配准的过程是使用是局部地图，局部地图是被判断为关键帧的地图所构建的；(2)如果当前无人机位姿与前一次加入地图帧的无人机位姿在旋转上和平移都大于一
定阈值，则判定当前帧为关键帧；(3)在检测到无人机静止或者当前发生纯旋转时，会在时域上进行滤波，每间隔0.5秒就会创建一个新的关键帧；对于构建局部地图的过程，选择基于kd-tree(k-dimensional tree)快速索引的方式，首先利用每个关键帧的位置信息构建出一个kd-tree，在kd-tree的数据结构中，快速索引得到当前位置邻域内的点或者索引与当前位置最近的k个点；采用基于二维强度图与三维点云的外点剔除地图管理方法，针对角点相较于平面点鲁棒性差的问题，首先将累积的局部点云图投影成二维强度图，使用基于图像的方法提取出线段并剔除不符合线特征的点云；使用累计5秒内的点云p
j
(j＝1,2,...,50)以及相应的位姿构建出局部地图m(l)，随后把三维点云投影成二维图像，图像分别有两个通道，分别为强度通道和深度通道，强度通道的像素表示的点云强度值大小，深度通道为每个三维点的深度，为了将角点的特征凸显出来方便进行基于图像的外点剔除，设计如下的映射函数来增强强度图的视差，对于一个具有原始强度值i的点p而言，其在二维强度图像坐标系下的像素值定义为v
i
(u,v)：其中log(
·
)表示一个对数函数，clamp(min,x,max)表示将一个变量x约束至下限min到上限max之间的函数，将v
i
(u,v)的像素值限定为(0,255)之间，构建的强度图视作为一个灰度图，于是将问题转变为在二维图像中寻找线段，采用基于区域生长的线段扩张lsd(line segment detector)方法进行线段的生成,得到线段的两个端点，用深度通道的深度信息，将两个端点反映射为两个3d点p
s
＝(x1,y1,z1)和p
e
＝(x2,y2,z2)，于是利用恢复后的两个3d点，得到一个唯一的直线，其方程ξ(x,y,z)表示为：针对长走廊等约束少的场景，完善雷达slam系统，添加了回环检测模块，保障同一个场景下长时间运行后构建地图的一致性，首先，在系统运动时会保存每个关键帧的位姿以及点云信息，使用位姿中的位置信息构建一个kd-tree，寻找与当前时刻最近邻的k个位置，剔除掉与当前帧时间接近的关键帧后，验证剩余的关键帧与当前帧的位置是否满足一定阈值，如果小于阈值，则判定为可能的回环；其次，使用基于强度的扫描上下文判断回环，得到局部累计地图投影得到的二维图像，将二维强度图应用于基于视觉词袋模型的方法，在第一步的基础上进一步校验回环的触发是否可靠；在判断满足回环的条件后，使用icp(iterative closest point)进行最后的校验，如果最后得到的匹配分数超过设定的阈值，则判定当前帧触发回环，进而触发基于因子图的位姿图优化。

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


技术研发人员：田栢苓 孙华清 李海松
受保护的技术使用者：天津大学
技术研发日：2023.03.17
技术公布日：2023/7/22