CN116758153A - 用于机器人精准位姿获取的基于多因子图的后端优化方法 - Google Patents

用于机器人精准位姿获取的基于多因子图的后端优化方法 Download PDF

Info

Publication number
CN116758153A
CN116758153A CN202310632057.2A CN202310632057A CN116758153A CN 116758153 A CN116758153 A CN 116758153A CN 202310632057 A CN202310632057 A CN 202310632057A CN 116758153 A CN116758153 A CN 116758153A
Authority
CN
China
Prior art keywords
imu
frame
point cloud
pose
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
Application number
CN202310632057.2A
Other languages
English (en)
Inventor
沈志熙
胡琴
韦佳汛
向军
李涛
付鹏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing University
Original Assignee
Chongqing University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Chongqing University filed Critical Chongqing University
Priority to CN202310632057.2A priority Critical patent/CN116758153A/zh
Publication of CN116758153A publication Critical patent/CN116758153A/zh
Pending legal-status Critical Current

Links

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/88—Lidar systems specially adapted for specific applications
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明涉及一种用于机器人精准位姿获取的基于多因子图的后端优化方法，构建改进的多因子图模型，首先对IMU数据进行预积分得到IMU传感器的观测模型误差并构建IMU因子，同时利用前端激光里程计输出的位姿变化关系计算激光雷达传感器的观测模型误差，构建激光里程计因子，并在检测回环成功时构建闭环因子，然后将这三种约束因子与相关联的状态变量用边相连，添加到因子图基本模型中。该方法提高了回环检测的准确率和效率，成功检测到回环后计算形成回环的两帧点云之间的位姿变换，并构建闭环因子加入到多因子图模型中进行优化，进而提高了后端优化中位姿估计的精度。

Description

用于机器人精准位姿获取的基于多因子图的后端优化方法
技术领域
本发明涉及激光SLAM技术领域，特别涉及用于机器人精准位姿获取的基于多因子图的优化方法。
背景技术
传统SLAM方法获得机器人的激光里程计和的位姿后，其得到的定位和地图结果还是会存在较大的误差。尤其对于机器人运动里程很长的情况下，即便激光里程计算法的精度较高，也会受到累计误差的影响，从而导致定位与建图精确度下降。所以SLAM方法还需要在后端进一步优化结果，得到机器人更加准确的位姿定位结果。
在SLAM中，后端优化不仅可以对传感器数据进行滤波和校正，减少噪声和误差，提高***的稳定性和鲁棒性；还可以将不同时间和位置的地图信息进行融合和校正，从而优化地图的连贯性和一致性。目前后端优化算法主要包括基于滤波器的方法和基于非线性(图优化)两种方法。滤波器算法虽然计算速度快，但估计精度相对较低，只适用于小规模、线性或近似线性***。非线性优化算法中最具代表性的为基于因子图的优化算法。
但是，传统的基于因子图后端优化方法仍然存在如下问题：
(1)单一的传感器用于定位和建图时会面临场景变化导致特征退化的弊端，为了解决这个问题，可以通过将多个传感器融合起来，例如利用IMU、GPS等数据进行后端优化，实现更加准确的定位和建图。而激光雷达与IMU的松耦合方案，虽然计算效率高，但是位姿估计累积误差大。
(2)回环检测是后端优化的重要环节，关于回环检测现有的工作通常利用点云局部或全局的几何特征进行编码，生成点云的局部描述符或全局描述符，并对点云描述符进行匹配识别。其中局部描述符容易受到噪声的影响，且重识别需要消耗大量的几何计算。全局描述子因为能够有效地捕捉到物体的整体形态信息，具有更好的鲁棒性，但是不当的特征编码方式会严重影响位置识别的计算效率，进而影响后端优化的性能。
发明内容
针对现有技术存在的上述问题，本发明要解决的技术问题是：如何获取机器人精准的位姿。
为解决上述技术问题，本发明采用如下技术方案：一种用于机器人精准位姿获取的基于多因子图的后端优化方法，包括如下步骤：
S100：构建改进的多因子图模型F(X)：
式中f(i,j)(Xi,Xj)代表第i帧位姿节点与第j帧位姿节点间的关系因子，实线方框Ci，虚线方框Fi，黑色节点分为IMU因子fIMU、激光里程计因子fLidar和闭环因子fLoop，fIMU＝fIMU(Xi,Xi+1)、fLidar＝fLidar(Xi,Xi+1)，fLoop＝fLoop(Xc,Xi+1)，E表示F(X)中与节点相关联的全部边。
后端优化的目标函数转换为求解最小二乘问题，表示为：
式中与/>分别表示IMU因子、激光里程计因子与闭环因子观测模型的误差，/> 与/>分别为对应的测量噪声协方差。其中，误差项可以统一表示为：
式中Ti与Tj分别为机器人第i个和第j个关键帧在世界坐标系下的位姿，△Ti,j表示由三种因子计算的两个变量节点之间的相对位姿变换关系，△Ti,j包括和
S200：将上述内容输出的fIMU和fLidar增量式地添加到F(X)中，当移动机器人在运动过程中访问到相同地方，即回环检测成功后在F(X)中加入fLoop，然后相关变量节点根据因子节点的更新进行位姿优化，并基于优化后的状态集X*，将前端里程计时所建立的投影图Mk中的相应点云，重新投影至世界坐标系实现机器人精准位姿的更新。
作为优选，所述S100中获得fIMU的过程如下：
首先，假设t时刻IMU输出的原始角速度与加速度用与/>表示，由IMU测量模型可知：
式中表示地图坐标系W到传感器坐标系B的旋转矩阵，g是W中的重力加速度，ωt和at分别表示t时刻激光雷达实际角速度和加速度；
和/>分别表示原始角速度的IMU零偏与加速度的IMU零偏，/>和/>统称为IMU零偏bt，/>和/>分别表示原始角速度的白噪声与加速度的白噪声，/>和/>统称为nt；
在时间t+△t处，机器人的速度vt+Δt、机器人的位置pt+Δt和旋转变换Rt+Δt分别由以下公式计算：
其中，Rt，pt，vt分别表示t时刻激光雷达的旋转变换，位置和速度，△t表示短时间。
第二步计算机器人i与j时刻之间的相对运动变化，这段时间内的速度变化△vij、位置变换△pij和旋转变换△Rij分别由以下公式计算：
其中，vi，pi，Ri分别表示激光雷达在i时刻的速度，位置和旋转变换，vj，pj，Rj分别表示激光雷达在j时刻的速度，位置和旋转变换，△tij表示i时刻与j时刻的时间差。
然后，利用开源的GTSAM优化库，优化IMU的速度变化△vij、位置变换△pij和旋转变换△Rij三个状态变量。在下一次预积分结果输入之前，GTSAM优化器会输出更新的位置变换、速度变换和旋转变换以及零偏矩阵bi′。
利用更新的零偏bi′，通过下式重新计算预积分，获得优化后的IMU在β时刻的速度位置/>和旋转变换/>
其中，bi′a和bi′ω分别表示更新后原始角速度的IMU零偏和更新后加速度的IMU零偏，和/>表示第i时刻原始角速度的白噪声和第i时刻加速度的白噪声。
通过三阶段预积分计算得到确定的两个时刻之间的相对位姿变化，进而得到计算公式如下：
分别表示更新后的i与j时刻之间速度变换、位置变换、和旋转变换。
fIMU表示IMU传感器在i帧与i+1帧之间的观测模型误差将预积分得到的代入公式(41)可以得到IMU传感器在i帧与j帧之间的观测模型误差/>按同样的方法可得到/>
作为优选，所述S100中获得fLidar的过程如下：
机器人激光里程计输出相邻两个关键帧i与i+1之间的位姿变换关系△Ti,i+1，将△Ti,i+1用来计算激光里程计因子中误差项，表示为：
fLidar表示激光雷达传感器在i帧与i+1帧之间的观测模型误差将△Ti,i+1代入公式(41)可以得到/>
作为优选，所述S100中获得fLoop的过程如下：
利用传统的迭代最近点特征匹配方法得到两个点集之间准确的位姿变换△T，设当前点云位姿为Ti+1，回环帧的位姿为Tc，则得到两者之间的相对位姿变化，即闭环因子表示为：
其中，
fLoop表示回环检测在当前帧第i+1帧与回环帧第c帧的两帧之间的观测模型误差将/>代入公式(41)可以得到/>
作为优选，所述S200中回环检测的过程如下：
S201：激光雷达获取环境信息得到的点云包括距离信息和强度信息。通过式子(59)对强度信息进行归一化和信息校准后得到ηk′，假设第k帧中每个点云表示为[xk,yk,zk,ηk′]，将笛卡尔坐标系下的3D点云投影到由X Y平面构成的极坐标坐标系下，变换后的点云极坐标表示为：
S202：然后以传感器为中心，通过环形角度和径向方向将点云等分切分Ns×Nr个子区域，每个分段表示为：
其中a∈[1,Ns]，b∈[1,Nr]，Lmax表示激光雷达依次扫描最大的感应范围。因为同一物体返回的强度值相同，对于每个小区域Sab里的点云，用子区域中的最大强度信息代替该空间的强度值ηab：
S203：提取了每个子区域Sab的强度信息后，还需要对该区域里的点集继续编码，在将每个子区域Sab内的点云按垂直方向分为8个单元，并从下到上标记为zk，其中k∈(1,2,...,8)，z坐标相同的点在同一个单元。若某个单元中没有激光点，则将zk置为零，否则，若一个单元内至少有一个激光点，则将zk置为1。这样，每个子区域Sab都会得到一个8位的二进制编码，然后根据zk值将二进制转换为十进制数值，则每个区域的几何信息dab可以表示为：
其中z(·)表示将Sab的二进制编码转换为十进制值的函数。将该式子中的几何信息与式子(64)中的强度信息结合，就得到点云全局描述符Ω(a,b)：
S204：采用两步分层的快速检测方法，分别计算几何相似度和强度相似度来加速匹配，然后再基于一致性验证匹配的有效性。
S204-1：几何相似度计算
将当前点云的全局描述符表示为Ωq，待匹配点云的全局描述符为Ωc，两者相同索引处的几何编码值分别为与/>计算/>与/>的余弦距离并归一化处理，则得到几何相似度函数：
其中，NS表示前面提到的NS个扇区，dq，dc分别表示当前点云全局描述符Ωq和待匹配点云全局描述符Ωc中对应的几何信息。
假设某个点云的全局描述符为这里的n表示点云的全局描述符在水平方向上平移了n个单位。列位移数n*和移动后的几何相似度得分D(dq,dc)分别定义为：
S204-2：强度相似度计算
设和/>分别为ηq和ηc索引值相同的列，取/>和/>计算强度相似度得分/>如下：
其中，ηq和ηc分别表示两个点云描述符之间的强度特征，分别表示强度相似度得分。
通过比较ηc和ηq平移n*列之后的强度特征最终计算强度相似度计算结果为：
n*表示视角变化。
S204-3：一致性验证
使用当前帧的过去N帧点云描述符和K个候选帧的过去N帧点云描述符的相似度的平均值作为最终相似度，将其作为一致性的验证标准，计算公式如下所示：
其中Θ(pq,pc)表示当前帧的过去N帧点云描述符和K个候选帧中某一帧的过去N帧点云描述符的相似度的平均值，pq表示当前帧，pc表示K个候选帧中的某一帧，dq-i，dc-i，ηq-i，ηc-i分别表示当前帧与候选帧点云全局描述符中的几何信息和强度信息。
在移动机器人反方向访问时，即传感器观测的视角发生变化的情况下，Ωq-i相应地改变为Ωq+i。利用得到的K个待匹配对象构建一个K近邻搜索树，将目标点云的描述符与搜索树中的进行比较，采用式(72)度量它们之间的相似度。最后，通过设置一个阈值选择最相似的候选点，将其作为重新访问的位置，表示为：
其中C表示搜索树中K个待匹配对象索引值，是设置的阈值。c*表示最终得到的闭环位置索引值，从历史关键帧中获取回环帧的位姿Tc，并将其用于后续构建闭环因子。
相对于现有技术，本发明至少具有如下优点：
本发明在现有的因子图优化模型中加入IMU因子，除了SLAM框架前端激光里程计构建的位姿约束外，利用惯性传感器的观测数据构架IMU因子，实现IMU与激光雷达传感器的紧耦合，同时引入点云关键帧和滑窗策略满足SLAM的实时性。
本发明方法中融合了一种新的基于点云全局描述符的回环检测方法，提高了回环检测的准确率和效率，成功检测到回环后计算形成回环的两帧点云之间的位姿变换，并构建闭环因子加入到多因子图模型中进行优化，进而提高SLAM方法后端优化中位姿估计的精度。
附图说明
图1为一种激光里程计估计方法的流程框架图。
图2为基于FCNN的分割网络模型的框架图。
图3为多目标跟踪方法的框架图。
图4为本发明方法的框架图。
图5为改进的多因子图优化模型。
图6为回环检测方法框架图。
图7为点云切分示意图。
图8为为几何信息编码示意图。
图9为贝叶斯网络模型
图10为因子图模型。
图11为开启回环检功能前后全局轨迹对比，其中(a)为02序列开启闭环，(b)为02序列关闭闭环，(c)为05序列开启闭环，(d)为05序列关闭闭环。
具体实施方式
下面对本发明作进一步详细说明。
本发明中一种激光里程计的估计方法如下：
一种基于动态目标跟踪的激光里程计估计方法，包括如下步骤：
S1：对激光雷达采集的每一帧激光雷达点云中的所有激光点进行预处理，预处理后的点云采用基于FCNN的点云分割网络检测出潜在动态目标；
S2：利用卡尔曼滤波方法对检测出的潜在动态目标进行状态估计，得到t时刻潜在动态目标质心状态的最优估计值其中，/>表示t时刻潜在动态目标的平面坐标，/>表示t时刻潜在动态目标的速度；
S3：计算潜在动态目标的几何信息和强度信息；
S4：融合潜在动态目标的几何信息和强度信息，利用关联方法检测出所有潜在动态目标，根据S2计算的潜在动态目标的速度，如果潜在动态目标的速度t时刻的速度为0则判定该潜在动态目标为静态目标，否则为动态目标
S5：通过曲率对静态目标进行特征点提取；
S6：对特征点通过曲率赋予不同的权重，然后基于加权函数构造位姿约束函数并求解得到静态目标的位姿估计；
S7：基于滑动窗口和关键帧以及S6得到的位姿估计对静态目标对应的激光点进行畸变补偿更新，得到最新激光里程计即得到相邻关键帧间位姿变换关系△Tk,k+1以及更新的静态目标对应的激光点的最新坐标。由相邻两帧静态目标点云的位姿变换可以得到相邻两帧激光雷达的位姿变换，通过当前帧相对上一帧的位姿变换关系和上一帧激光雷达的位姿就可以计算出激光雷达当前帧的位姿估计。激光雷达的初始位姿由IMU预积分得到，然后由相邻两帧静态目标点云的位姿变换可以得到相邻两帧激光雷达的位姿变换，通过当前帧相对上一帧的位姿变换关系和上一帧激光雷达的位姿就可以计算出激光雷达当前帧的位姿估计。
具体的，所述S1中对激光雷达采集到的点云中的所有激光点进行预处理的步骤如下：
当线机械式激光雷达自转获取环境三维信息时，频率为10HZ，即扫描一圈只需要0.1s，这就导致机器人在移动过程中，因为自身的旋转运动或平移运动，一帧激光雷达点云内的所有点可能不是在同一个位置采集的。载体在向前运动的过程中，同一帧采集到的点云的坐标系不一样。对于产生的运动畸变，必须通过数据补偿以消除激光点云的失真，即将某一帧的点云重投影到同一时刻坐标系下。
设第k次扫描采集到的激光点云用Pk表示，点云失真补偿就是将Pk中所有激光点重投影到tk时刻结束的雷达坐标系下。利用激光雷达坐标系与IMU坐标系之间的变换关系即可将激光点云映射到IMU坐标系下：
式中pI与pL分别表示IMU坐标系下的激光点和雷达坐标系下的激光点。
由于激光雷达采集到的点云只包含几何信息和强度信息，不包含采集顺序，所以对点云数据进行插值处理之前，需要对点云进行序列化操作。激光雷达以恒定的角速度匀速旋转，可基于扫描一圈点云的时序，根据角度差值对点云进行编号，θe表示结束扫描线相对于起始线的角度，θn为当前扫描线相对于起始线的角度，当前点所在的时刻表示为：
其中，ts表示激光雷达起始扫描时间，f表示激光雷达旋转频率。
对无序点云进行序列化后，基于时序找到与tk时刻最近的两帧IMU数据并进行预积分计算，然后将计算结果投影到世界坐标系W下，从而获得tk时刻在W下的IMU姿态同理可得到tk+1时刻IMU姿态/>基于此即可得到tk+1时刻相对于tk时刻的位姿：
最后通过进行变换计算，获得在tn时刻每个激光点点的运动补偿变换矩阵/>
实现运动失真的补偿，得到补偿后的激光点
具体的，所述S1中基于FCNN的点云分割网络检测出潜在动态目标的过程如下：
S1.1：对预处理后的点云进行降维处理，然后根据鸟瞰投影对预处理后的点云的3D空间划分区域并建立其2D索引，然后提取投影图内各网格内点柱的特征信息，些特征信息被用于生成FCNN网络的输入，即二维特征图；
具体的提取投影图内各网格内点柱的8个特征信息，该8个特征信息包括单元格中激光点的个数、平均强度、平均高度、最高点的强度、最大高度、单元格中心与原点的角度偏差和距离以及单元格是否被占用的二进制表示，这些特征信息被用于生成FCNN网络的输入，即二维特征图。采用二维网格对鸟瞰投影点云数据进行切割，每个激光点落在一个网格中，从而变为有序的二维矩阵，一个网格中可能有几个或者0个激光点。
FCNN网络包括特征编码器、特征解码器和属性预测器；
S1.2：特征编码器通过卷积层的卷积方式对二维特征图进行语义特征提取，并连续下采样二维特征图的空间分辨率，最后输出一个特征向量，所述卷积层包括经过训练的VGG网络和ResNet网络；
S1.3：所述特征向量输入特征解码器，采用反卷积层，进行上采样，得到与二维特征图相同尺寸的预测图；
S1.4：属性预测器计算预测图中每个网格的四个属性值，所述四个属性值是指中心偏移值、中心偏移方向值、属性概率值和物体高度值；
S1.5：根据每个网格的中心偏移值和中心偏移方向构建每个网格的偏向指针，每个多个指针指向的单元格作为候选的分类对象。
对于候选的分类对象，根据分类对象的属性概率值不小于概率阈值则认为其是可靠聚类对象
筛选出的可靠聚类对象就是潜在的动态目标。
具体的，所述S3中计算潜在动态目标的几何信息和强度信息的计算过程如下：
S3.1：计算潜在动态目标的几何信息即计算潜在动态目标的质心位置，利用潜在动态目标对应的激光点坐标计算潜在动态目标的质心位置如公式(6)所示：
式中n表示潜在动态目标对应的激光点的数量，xi,yi,zi表示动态目标对应的第i个激光点的坐标，表示动态目标质心的坐标；
S3.2：将激光雷达输出的原始激光点数据转换为十进制表示，并作归一化处理，得到每个激光点的强度信息。
S3.3：激光雷达的强度通道是有噪声的，这是由于目标表面特性，例如粗糙度、表面反射率以及采集几何距离等影响所致。因此，为了减少这些因素的干扰，通过映射函数校准激光点的强度信息：
其中，d为原始激光点的距离信息，ηk表示第k个原始激光点的强度信息，ηk′表示第k个激光点校准后的强度信息。
具体的，所述S4中利用关联方法检测出所有潜在动态目标的过程如下：
S4.1：计算潜在动态目标对应的激光点的表面反射系数ρ′：
其中，M为常量，该常量与激光雷达发射功率、光学传输***、大气传输系数等有关，α为激光入射角；
S4.2：判断不同帧中潜在动态目标是否为同一目标，满足公式(9)则认为不同帧中潜在动态目标是同一目标，否则不是：
式中dthr和ρthr分别表示设置的距离阈值和强度阈值，表示t时刻和t-1时刻潜在动态目标a的质心位置，/>表示在t时刻和t-1时刻a的强度平均值，对潜在动态目标对应的每个激光点的反射率ρ′取平均值即可得到该潜在动态目标表面的反射率ρ。
通过公式(9)将t时刻图片中潜在动态目标与将t-1时刻图片中的潜在动态目标进行关联，关联出被障碍物遮挡的潜在动态目标，从而检测出所有潜在动态目标。
关联方法结束后可以将当前帧的目标与前一帧的目标关联起来，如图3所示，多目标跟踪方法结合关联结果和卡尔曼滤波器对目标的状态预测得到跟踪结果，即跟踪结果包括目标的运动轨迹和位置、速度等状态，然后根据判断目标的速度是否为0将目标分为静态目标和动态目标。
具体的，所述S5中通过曲率对静态目标进行特征点提取的过程如下：
S5.1：激光雷达扫描具有M个平行读数的垂直平面，每一平行线上有N个点，将第k次激光雷达扫描的所有静态目标对应的所有激光点表示为Pk′，将每一个激光点表示为其中m∈[1,M],n∈[1,N]，xk,yk,zk为第k个激光点的三维坐标；
将所在线束在水平方向上的连续点集用/>表示，则对于/>相邻点构成的平面，/>的曲率/>的计算公式为：
式中Ns表示中激光点的个数，/>表示/>中第m行中第j个激光点。
然后根据c值的大小对中的所有激光点进行分类，c值大于设定阈值的作为角点或边点，c值小于或等于设定阈值的被视为平面特征点。
S5.2：通过设置的阈值σ将点标记为待选边缘点和待选平面特征点。为了获得均匀分布的特征点，将每条扫描线采集到的/>平均分为四个子区域，每个子区域中最多提取两个边缘特征点和四个平面特征点，四个子区域中所有边缘特征点的集合记为εk，四个子区域中所有平面特征点的集合集记为sk。
具体的，所述S6中求解得到静态目标位姿估计的过程如下：
S6.1：将第k次激光雷达扫描的所有静态目标对应的所有激光点表示为Pk′，Pk′中一个激光点表示为pk＝{xk,yk,zk,1}T，设激光雷达在第k帧中的位姿表示Tk，则两个连续帧k-1和k之间的6自由度位姿变换为：
其中，是/>的李代数表示。函数log(·)是从齐次变换矩阵到李代数的映射，第k帧间到短时间δt内的位姿变换表示为：
式中表示当前第k帧的静态目标对应的所有激光点重投影到当前帧开始时刻的位置。
假设εk中边缘特征点pε的局部平滑度是cε，sk中平面特征点ps的局部平滑度是cs，它们的权重定义如公式(14)所示：
/>
其中，W(pε)、W(ps)分别表示pε和ps的权重，表示εk中第i行第j个边缘特征点的曲率。
S6.2：计算每个边缘特征点pε在全局边缘特征集合中的临近点的协方差矩阵，全局边缘特征集合即一帧图像中含有的所有εk的集合，如果协方差矩阵包含最大特征值，说明pε在全局边缘特征中的临近点分布在一条直线上，这个最大特征值对应的特征向量看作直线的方向，临近点的几何中心/>看作直线的位置，此时边缘特征点pε到全局边缘特征的距离是：
式中符号·是点积，pn是单位向量；
S6.3：计算每个平面特征点pS，在全局平面特征集合中与临近点的协方差矩阵，全局平面特征集合即一帧图像中含有的所有sk的集合，如果协方差矩阵包含一个最小特征值，说明这些临近点分布在一个平面上，这个最小特征值对应的特征向量看作曲面法线的方向，临近点的几何中心/>可以看作平面的位置，此时平面特征点pS到全局平面特征集合的距离表示为：
S6.4：优化函数定义为：
S6.5：利用高斯牛顿法对公式(12)求解非线性方程，得到最优姿态估计，应用具有左扰动的模型来估计雅可比矩阵Jp：
其中[Tp]×即[Tkpk]×，表示将4D点表达式{x,y,z,1}转换为3D点表达式{x,y,z}并计算其斜对称矩阵，δ、ξ分别表示短时间，δt和两个连续帧之间的6自由度位姿变换Tkpk表示一个4*4矩阵和4*1的向量得到一个4*1的向量，边缘残差的雅可比矩阵Jε通过以下公式计算：
平面残差的雅可比矩阵Js：
通过雅可比矩阵表示迭代增量△Tk，使用△Tk更新激光里程计估计结果，反复迭代优化姿态，直到△Tk收敛，即可计算出当前激光雷达的位姿估计Tk。
具体的，所述S7中得到最新激光里程计输出的过程如下：
S7.1：通过设置一个位姿变换的阈值选取关键帧，平移和旋转阈值分别设置为1m和10°，即当机器人在第k时刻与k+1时刻之间的位姿变换超过位姿变换的阈值时，就会选择当前帧为关键帧Fk+1并加入滑动窗口，该Fk+1与pk的位姿相匹配；为了存储固定数量的最近帧激光点云的投影图，先维护一个栈作为滑动窗口。接着在时序上提取i个最靠近的关键帧，并将这些关键帧表示为一个子关键帧集{Fk-i,...,Fk}，通过与W之间的变换矩阵{T'k-i,...,T'k}，将子关键帧集投影到W下得到投影图Mk。
Mk由边缘特征点云投影图和平面特征点云投影图/>组成。每个关键帧与对应的投影图Mk相互关联，关联关系定义如下所示：
其中，分别代表子关键帧集中第k个关键帧和k-i个关键帧的边缘特征点，/>分别代表子关键帧集合中第k个关键帧和k-i个关键帧的平面特征点。
S7.2：将最新获得的k+1关键帧个关键帧Fk+1转换到W下并与所述投影图Mk关联，获得第k+1帧的增量式位姿变换△Tk,k+1：
其中，表示Tk的逆矩阵，Tk+1表示k+1帧激光雷达的位姿估计。
利用6自由度位姿变换重新计算失真，得到：
式中是△Tk,k+1的李代数表示，通过公式(19)就可以计算出激光雷达更新后的最优定位估计，△Tk,k+1表示相邻两关键帧间位姿变换关系，/>表示更新的静态目标对应的激光点的最新坐标；
第k+1次激光雷达扫描的静态目标对应的所有激光点表示为P′k+1，P′k+1中一个激光点表示为pk+1，为两个连续帧k和k+1之间的6自由度位姿变换，根据第k+1帧的△Tk,k+1计算得到，/>表示更新的静态目标对应的激光点的最新坐标。
输出和相邻两关键帧间位姿变换关系△Tk,k+1。
因子图(Factor Graph)后端优化方法是一种常见的图优化方法，它使用因子图中的变量和因子来表示机器人位姿和环境特征之间的关系。其本质上是一个贝叶斯网络如图9所示，贝叶斯网络是一种有向无环图，假设ti时刻机器人的状态量表示为XK＝{xi}，i∈(0...K)表示，传感器检测到的观察物的位置，即路标点为LN＝{lj}，其中j∈(1...N)，接收到的激光传感器观测为ZK＝{zi}，机器人的控制输入值用UK＝{ui}表示，其中i∈(0...K)。图中细线和粗线分别表示预测方程g和测量方程h。实线框中路标l1和位姿x1在测量方程h的作用下决定观测数据z1。位姿x1根据预测方程g和控制输入量u2共同决定了下一次的位姿x2，并通过这个基本过程逐步递进。
由观测量估计状态量的过程可以转换成后验概率问题，该后验概率表示为：
P(XK|ZK) (26)
根据贝叶斯定律，有：
式中P(ZK|XK)和P(XK)通常称为似然和先验概率，根据马尔可夫假设，每个状态对应的观测量都是彼此独立的，当前状态只与上一时刻有关，则有：
在每个时刻，考虑机器人的控制输入值作为状态量的参数值，式(27)右侧运动模型可进一步改写为：
其中P(x0)为初始状态的先验值。
然后求解最优状态估计问题就变成求解各变量与测量值联合概率密度函数问题：
假设预测模型是高斯测量模型，则预测模型可以表示为：
xi＝g(xi-1,ui)+ωi (31)
其中ωi表示运动过程中ti时刻输入的预测噪声，且ωi～N(0,Λi)，Λi表示噪声协方差矩阵。
同理，假设观测模型也是高斯测量模型，则观测模型可表示为：
zi＝h(xi,lj)+vi (33)
其中vi为测量噪声，且vi～N(0,Γi)，Γi表示其对应的协方差矩阵。
然后结合式子(32)和(34)，对联合概率密度函数P(XK,LK,UK,ZK)取负对数，可以将最大后验概率问题转换为最小二乘问题，表示为：
因子图模型{X,F,E}在贝叶斯网络模型上进行简化，如图10所示，变量节点间概率联系用因子节点fi∈F表示，即状态变量节点xi∈X间的因子fi(xi-1,xi)对应概率P(xi|xi-1,ui)，状态变量与路标变量节点间的因子对应概率P(zi|xi,lj)。eij∈E为连接因子节点与变量节点的边，表示节点之间的约束关系。则式(30)联合概率密度函数P(XK,LK,UK,ZK)可以转换为多因子的乘积：
式中Xi是与因子节点fi相邻的xi集合，则图10的因子图模型优化方程为：
F(X,L)＝f0(x0)f1(x0,x1)f2(x1,x2)f3(x2,x3)f4(l1,x1)f5(l2,x2) (37)
式中f0(x0)对应先验因子p(x0)，然后同样通过负对数的形式得到非线性最小二乘问题，根据非线性最小问题求解方法LM可以优化待求解变量，从而得到全局的最优位姿。
一种用于机器人精准位姿获取的基于多因子图的后端优化方法，包括如下步骤：
S100：构建改进的多因子图模型F(X)：
式中f(i,j)(Xi,Xj)代表第i帧位姿节点与第j帧位姿节点间的关系因子，实线方框Ci，虚线方框Fi，黑色节点分为IMU因子fIMU、激光里程计因子fLidar和闭环因子fLoop，fIMU＝fIMU(Xi,Xi+1)、fLidar＝fLidar(Xi,Xi+1)，fLoop＝fLoop(Xc,Xi+1)，E表示F(X)中与节点相关联的全部边。
基于IMU因子、激光里程计因子和闭环因子构建多因子图模型进行后端优化，首先对IMU数据进行预积分得到IMU传感器的观测模型误差并构建IMU因子fIMU，同时利用前端激光里程计输出的位姿变化关系计算激光雷达传感器的观测模型误差，构建激光里程计因子fLidar，并在检测回环成功时构建闭环因子fLoop，然后将这三种约束因子与相关联的状态变量Xi用边相连，添加到因子图基本模型中，图10的因子图优化模型可进一步表示为多因子图优化模型，如图5所示。
后端优化的目标函数转换为求解最小二乘问题，表示为：
式中与/>分别表示IMU因子、激光里程计因子与闭环因子观测模型的误差，/> 与/>分别为对应的测量噪声协方差。其中，误差项可以统一表示为：
式中Ti与Tj分别为机器人第i个和第j个关键帧在世界坐标系下的位姿，△Ti,j表示由三种因子计算的两个变量节点之间的相对位姿变换关系，△Ti,j包括和
S200：将上述内容输出的fIMU和fLidar增量式地添加到F(X)中，当移动机器人在运动过程中访问到相同地方，即回环检测成功后在F(X)中加入fLoop，然后相关变量节点根据因子节点的更新进行位姿优化，并基于优化后的状态集X*，将前端里程计时所建立的投影图Mk中的相应点云，重新投影至世界坐标系实现机器人精准位姿的更新。
具体的，所述S100中获得fIMU的过程如下：
为了更准确地估计机器人的位置和方向，将IMU因子计算分为三步完成。第一步进行初次预积分计算，第二步利用开源的因子图优化框架GTSAM对初次预积分结果进行优化，并纠正IMU数据中的误差；第三步利用更新的IMU零偏重新计算积分，并输出IMU因子相关项。具体计算过程如下：
首先，假设t时刻IMU输出的原始角速度与加速度用与/>表示，由IMU测量模型可知：
式中表示地图坐标系W到传感器坐标系B的旋转矩阵，g是W中的重力加速度，从式子中可以看出/>和/>受IMU零偏bt和白噪声nt的影响，ωt和at分别表示t时刻激光雷达实际角速度和加速度；
和/>分别表示原始角速度的IMU零偏与加速度的IMU零偏，/>和/>统称为IMU零偏bt，/>和/>分别表示原始角速度的白噪声与加速度的白噪声，/>和/>统称为nt；
然后进行第一阶段计算，通过整合IMU的多帧角速度与加速度来估计机器人的运动。在时间t+△t处，机器人的速度vt+Δt、机器人的位置pt+Δt和旋转变换Rt+Δt分别由以下公式计算：
其中，Rt，pt，vt分别表示t时刻激光雷达的旋转变4换，位置和速度，△t表示短时间。
第二步计算机器人i与j时刻之间的相对运动变化，这段时间内的速度变化△vij、位置变换△pij和旋转变换△Rij分别由以下公式计算：
其中，vi，pi，Ri分别表示激光雷达在i时刻的速度，位置和旋转变换，vj，pj，Rj分别表示激光雷达在j时刻的速度，位置和旋转变换，△tij表示i时刻与j时刻的时间差。
然后，利用开源的GTSAM优化库，优化IMU的速度变化△vij、位置变换△pij和旋转变换△Rij三个状态变量。在GTSAM优化库中，先验因子为IMU在i时刻的速度和位置，IMU的零偏矩阵作为因子节点，优化的变量节点为j时刻的速度、位置和变换矩阵。在下一次预积分结果输入之前，GTSAM优化器会输出更新的位置变换、速度变换和旋转变换以及零偏矩阵bi′。
利用更新的零偏bi′，通过下式重新计算预积分，获得优化后的IMU在β时刻的速度位置/>和旋转变换/>
其中，bi′a和bi′ω分别表示更新后原始角速度的IMU零偏和更新后加速度的IMU零偏，和/>表示第i时刻原始角速度的白噪声和第i时刻加速度的白噪声。
通过三阶段预积分计算得到确定的两个时刻之间的相对位姿变化，进而得到计算公式如下：
分别表示更新后的i与j时刻之间速度变换、位置变换、和旋转变换。
fIMU表示IMU传感器在i帧与i+1帧之间的观测模型误差将预积分得到的代入公式(41)可以得到IMU传感器在i帧与j帧之间的观测模型误差/>按同样的方法可得到/>
具体的，所述S100中获得fLidar的过程如下：
机器人激光里程计输出相邻两个关键帧i与i+1之间的位姿变换关系△Ti,i+1，将△Ti,i+1用来计算激光里程计因子中误差项，表示为：
fLidar表示激光雷达传感器在i帧与i+1帧之间的观测模型误差将△Ti,i+1代入公式(41)可以得到/>
具体的，所述S100中获得fLoop的过程如下：
当闭合回路检测成功后，通过形成闭合的两个关键帧之间的相对位姿变化关系构建闭环因子。利用传统的迭代最近点特征匹配方法得到两个点集之间准确的位姿变换△T，设当前点云位姿为Ti+1，回环帧的位姿为Tc，则得到两者之间的相对位姿变化，即闭环因子表示为：
其中，fLoop表示回环检测在当前帧第i+1帧与回环帧第c帧的两帧之间的观测模型误差将/>代入公式(41)可以得到/>
具体的，所述S200中回环检测的过程如下：
回环检测主要通过不同的判断策略对机器人进行位置识别，检测机器人在运行过程中是否与历史位姿或路线形成了闭环；如果形成了闭环，则构建闭环因子加入因子图模型中并对机器人位姿进行优化，从而有效地矫正漂移较大的轨迹。
S201：激光雷达获取环境信息得到的点云包括距离信息和强度信息。通过式子(59)对强度信息进行归一化和信息校准后得到ηk′，假设第k帧中每个点云表示为[xk,yk,zk,ηk′]，将笛卡尔坐标系下的3D点云投影到由X Y平面构成的极坐标坐标系下，变换后的点云极坐标表示为：
S202：然后以传感器为中心，通过环形角度和径向方向将点云等分切分Ns×Nr个子区域，如图7为点云切分示意图，一帧点云被切分为Ns个扇区与Nr个环。每个分段表示为：
其中a∈[1,Ns]，b∈[1,Nr]，Lmax表示激光雷达依次扫描最大的感应范围。因为同一物体返回的强度值相同，对于每个小区域Sab里的点云，用子区域中的最大强度信息代替该空间的强度值ηab：
S203：提取了每个子区域Sab的强度信息后，还需要对该区域里的点集继续编码。编码示意图如图8所示，在将每个子区域Sab内的点云按垂直方向分为8个单元，并从下到上标记为zk，其中k∈(1,2,...,8)，z坐标相同的点在同一个单元。若某个单元中没有激光点，则将zk置为零，否则，若一个单元内至少有一个激光点，则将zk置为1。这样，每个子区域Sab都会得到一个8位的二进制编码，然后根据zk值将二进制转换为十进制数值，则每个区域的几何信息dab可以表示为：
其中z(·)表示将Sab的二进制编码转换为十进制值的函数。将该式子中的几何信息与式子(64)中的强度信息结合，就得到点云全局描述符Ω(a,b)：
位置识别是通过计算当前位置和历史位置之间的欧氏距离，找到与当前位置最接近的历史位置的过程。即当前位置的描述符和历史位置的描述符进行匹配的过程。但随着历史位置的增加，搜索匹配的计算成本也会增加。
S204：采用两步分层的快速检测方法，分别计算几何相似度和强度相似度来加速匹配，然后再基于一致性验证匹配的有效性。
S204-1：几何相似度计算
对于一对待匹配全局描述符，首先计算两个全局描述符的几何相似度得分。将当前点云的全局描述符表示为Ωq，待匹配点云的全局描述符为Ωc，两者相同索引处的几何编码值分别为与/>计算/>与/>的余弦距离并归一化处理，则得到几何相似度函数：
其中，NS表示前面提到的NS个扇区，dq，dc分别表示当前点云全局描述符Ωq和待匹配点云全局描述符Ωc中对应的几何信息。
然而，计算的点云全局扫描符的表示形式会受到传感器位置的影响，即激光雷达的扫描位置不同，计算出来的同一索引处的描述符也会有所不同。主要体现在按照环向顺序编码的特征，当点云旋转时，构造的点云描述符在索引上可能会发生水平位移。
为了解决点云描述符的旋转不变性问题，可以先计算每对点云的全局描述符之间在水平方向上所有可能的位移距离，得到一组距离值。最后，从这组距离值中选择最小值作为该点云描述符对之间的距离，然后再解算几何相似性，计算过程如下：
假设某个点云的全局描述符为这里的n表示点云的全局描述符在水平方向上平移了n个单位。列位移数n*和移动后的几何相似度得分D(dq,dc)分别定义为：
S204-2：强度相似度计算
强度相似度匹配的目的在于减少几何相似度匹配中的误差和噪声，从而获得更加准确的匹配结果。其核心是通过逐列计算两个点云描述符之间的强度特征ηq和ηc的相似性来实现的。设和/>分别为ηq和ηc索引值相同的列，取/>和/>计算强度相似度得分如下：
其中，ηq和ηc分别表示两个点云描述符之间的强度特征，分别表示强度相似度得分。
通过比较ηc和ηq平移n*列之后的强度特征最终计算强度相似度计算结果为：/>
n*表示视角变化。
选取与目标点云最相似的K个历史描述符，并将对应的K个候选帧的历史描述符输入到一致性验证部分进行验证。
S204-3：一致性验证
在SLAM***中，回环帧的相邻帧点云相似度较高，且传感器反馈在时间上是连续的，为了减少错误匹配的情况，使用当前帧的过去N帧点云描述符和K个候选帧的过去N帧点云描述符的相似度的平均值作为最终相似度，将其作为一致性的验证标准，计算公式如下所示：
其中Θ(pq,pc)表示当前帧的过去N帧点云描述符和K个候选帧中某一帧的过去N帧点云描述符的相似度的平均值，pq表示当前帧，pc表示K个候选帧中的某一帧，dq-i，dc-i，ηq-i，ηc-i分别表示当前帧与候选帧点云全局描述符中的几何信息和强度信息。
在移动机器人反方向访问时，即传感器观测的视角发生变化的情况下，Ωq-i相应地改变为Ωq+i。利用得到的K个待匹配对象构建一个K近邻搜索树，将目标点云的描述符与搜索树中的进行比较，采用式(72)度量它们之间的相似度。最后，通过设置一个阈值选择最相似的候选点，将其作为重新访问的位置，表示为：
其中C表示搜索树中K个待匹配对象索引值，是设置的阈值。c*表示最终得到的闭环位置索引值，从历史关键帧中获取回环帧的位姿Tc，并将其用于后续构建闭环因子。
实验验证：
采用经典的基于激光雷达SLAM框架LOAM算法作为baseline模型框架，并在LOAM框架上加入本发明提出的基于多因子图的后端优化算法，针对提出的多因子图后端优化算法，分别做了以下几组对比实验：第一组是本发明提出的后端优化算法在开启回环检测方法前后的对比实验，第二组是本发明采用的回环检测方法和其他基于点云描述符的回环检测算法的对比实验，第三组是不同激光SLAM框架中后端优化算法的对比实验。
1.实验数据集
实验使用KITTI数据集中的RawData数据集，该数据集共有22组数据，其中前11个
序列(0-10)带有地面真实轨迹，剩下11个序列(11-21)没有地面真实轨迹，各序列的具体信息可见表1。
表1KITTI数据集各序列信息表
/>
2.评价标准
验证回环检测方法有效性，本质上考察一个二分类问题的分类效果，考察指标为准确率(Precision)与召回率(Recall Rate)，这两个指标通过真阳性(TP)、真阴性(TN)、假阳性(FP)、假阴性(FN)这四个基础指标计算得到。
闭环检测的精确率Kp表示为正确预测的闭环在所有被预测为有回环的占比，计算公式为：
闭环检测实验的召回率Kr表示为正确预测的闭环占全部有闭环环境的占比，计算公式为：
评价后端优化算法性能的相关指标包括算法输出轨迹的绝对位姿误差(ATE)和相对位姿误差(RPE)，绝对位姿误差(ATE)通过计算SLAM算法输出的轨迹与真实轨迹之间的误差来得到的，可以评价算法的精度和轨迹的一致性。设i时刻算法输出位姿为pi，此时位姿的地面真值为qi，且输出位姿到真实位姿的变换矩阵为T，则i时刻的绝对位姿差值Ei为：
通常使用Ei的均方根误差(RMSE)作为ATE指标，来评估全局定位一致性，根据各时刻的绝对位姿误差可计算出全局轨迹的RMSE：
式中m表示轨迹中的位姿数，trans(Ei)表示绝对误差的平移部分。
相对位姿误差(RPE)通过计算等时间间隔的两帧位姿差值来描述测量里程计的误差，它可以帮助我们了解相邻帧之间的姿态变化情况，同时也可以反映定位算法在全局轨迹中局部位姿估计的准确性和鲁棒性。同样的，通常使用均方根误差(RMSE)作为RPE指标，先计算各时刻的相对位姿误差，假设等时间间隔为△，则i时刻的位姿差值Fi为：
各时刻的相对位姿差值取RMSE为：
/>
其中n表示轨迹中的位姿个数，trans(Fi)表示相对误差的平移部分。
3.实验设置
由表1可知，数据集中前11个序列里只有00序列、02序列、05-08序列具有回环，故闭环检测实验在KITTI数据集这几个序列上进行。一方面通过对比本发明提出的后端优化算法在开启闭环检测功能与关闭闭环检测功能的情况下，全局轨迹的RPE指标来评估闭环检测方法的性能。另一方面将本文回环检测方法继续与其他SLAM算法中前沿的闭环检测方法进行对比，包括视觉SLAM中的DBoW3方法，基于点云全局描述符SC的回环检测方法以及基于描述符的Lidar Iris回环检测方法。同时将本发明提出的后端优化算法与LOAM和LeGO-LOAM在KITTI数据集的07序列以及08序列下输出的轨迹与地面真值作对比，且计算每个SLAM***输出轨迹的ATE与RPE相关数值，从而比较整个算法位姿估计的精度。
4.实验结果分析
图11为本发明方法在开启和关闭回环检测功能两种情况下的全局轨迹对比，各序列的RPE指标见表2所示。
表2开启闭环检测前后RPE结果
Table 2RPE results before and after opening closed-loop detection
图11中圈出来的地方表示该序列的闭环场景，左侧为本发明提出的后端优化算法开启回环检测时在02和05序列下的全局定位结果，右侧为关闭回环检测时的全局定位结果，可以直观地看到，当关闭了回环检测功能后，误差将会随着时间的推移而累计，导致位姿估计出现明显偏差，算法输出的运动轨迹在回环处引入没有完全实现闭合；反之开启闭环检测之后，本文方法在02序列下具有反向访问闭环场景的复杂环境下都能较好地实现了闭环匹配，并由于成功构建闭环因子加入到多因子图优化模型中消除累积的漂移，因而提高了全局位姿估计精度。
由表2的相对位姿误差(RPE)结果可知，开启闭环与关闭闭环两种模式下，本发明采用的回环检测方法在05序列上的RPE结果有明显差异，闭环功能关闭后的全局定位误差明显较大；且在回环场景更复杂的02序列，两种模式的位姿估计RPE差别更显著，开启闭环功能后的位姿误差相较于关闭时减少了71％，证明本发明采用的回环检测方法在SLAM后端优化算法中的重要性。
将回环检测方法继续与其他SLAM算法中前沿的闭环检测方法进行对比，包括视觉SLAM中的DBoW3方法，基于点云全局描述符SC的回环检测方法以及基于描述符的LidarIris回环检测方法，结果如表3所示。
表3KITTI数据集下闭环检测方法结果对比
由表3可见，对于具有反向访问闭环场景的02序列和08序列，基于视觉的方法召回率明显降低，因为基于视觉的方法无法实现反向的闭环匹配，使得检测到的回环减少，而本文方法和Lidar Iris方法在构建全局描述符时考虑了多角度访问历史位置的影响，因此表现出更好的召回率。但在02序列非结构特征较多的场景下，道路狭窄且道路两边都是树木的非建筑物路段，回环检测的精度受到几何形状的影响，通过几何信息和强度信息构建的全局描述符差异性较小，从而使得SC和本文回环检测方法的性能有所损失，精度降低。在00序列和05序列下，本文方法比其他方法都具有更高的精度和召回率，充分展示了该方法识别场景能力的稳定性。
表4 07序列下各算法实验结果
表4分别展示了LOAM、LeGO-LOAM与本发明提出的后端优化算法对于KITTI数据集07序列的全局定位结果和水平方向与旋转方向上的误差。由表的数据可知，三种算法在07序列下的性能差别不太大，但可以看出，本文算法在水平方向的误差明显比其他两种算法低，对于距离为694米的里程计，本文方法的绝对轨迹误差与相对轨迹误差的均方根误差和标准差都表现得更好，其中绝对轨迹误差最大值控制在0.451米以内，相对位姿误差控制在0.081米以内，说明本文的位姿估计的精确性与鲁棒性都优于另外两种方法。
表5 08序列下各算法实验结果
表5分别展示了LOAM、LeGO-LOAM与本发明提出的后端优化算法对于KITTI数据集08序列的全局定位结果和水平方向与旋转方向上的误差。
由表的数据可知，在后端优化后，三种算法都有效校正了前端累积的漂移，且对于距离长达3222米的车程，三种算法在08序列下的性能差异比较明显，根据后端优化后的定位结果，本文算法的精确度都要优于另外两种算法。结合表5中位姿误差数据，可以看出相比于LOAM后端优化没有进行回环检测，LeGO-LOAM后端优化环节直接结合帧到地图(scanto map)的匹配方式，本文后端优化融合IMU因子矫正长期累积的漂移的效果最显著，其次是LeGO-LOAM方法。
综上所述，通过2种不同序列的KITTI数据集，分别验证了LOAM、LeGO-LOAM和本发明提出的基于多因子图后端优化算法在不同场景下的表现以及性能。可以得出，在短距离的道路场景下本算法优势不明显；在长距离的动态复杂环境下本算法在精度和稳定性上都有较好的提升。
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (5)

1.一种用于机器人精准位姿获取的基于多因子图的后端优化方法，其特征在于：包括如下步骤：
S100：构建改进的多因子图模型F(X)：
式中f(i,j)(Xi,Xj)代表第i帧位姿节点与第j帧位姿节点间的关系因子，实线方框Ci，虚线方框Fi，黑色节点分为IMU因子fIMU、激光里程计因子fLidar和闭环因子fLoop，fIMU＝fIMU(Xi,Xi+1)、fLidar＝fLidar(Xi,Xi+1)，fLoop＝fLoop(Xc,Xi+1)，E表示F(X)中与节点相关联的全部边；
后端优化的目标函数转换为求解最小二乘问题，表示为：
式中与/>分别表示IMU因子、激光里程计因子与闭环因子观测模型的误差，/> 与/>分别为对应的测量噪声协方差，其中，误差项可以统一表示为：
式中Ti与Tj分别为机器人第i个和第j个关键帧在世界坐标系下的位姿，△Ti,j表示由三种因子计算的两个变量节点之间的相对位姿变换关系，△Ti,j包括和/>
S200：将上述内容输出的fIMU和fLidar增量式地添加到F(X)中，当移动机器人在运动过程中访问到相同地方，即回环检测成功后在F(X)中加入fLoop，然后相关变量节点根据因子节点的更新进行位姿优化，并基于优化后的状态集X*，将前端里程计时所建立的投影图Mk中的相应点云，重新投影至世界坐标系实现机器人精准位姿的更新。
2.如权利要求1所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方法，其特征在于：所述S100中获得fIMU的过程如下：
首先，假设t时刻IMU输出的原始角速度与加速度用与/>表示，由IMU测量模型可知：
式中表示地图坐标系W到传感器坐标系B的旋转矩阵，g是W中的重力加速度，ωt和at分别表示t时刻激光雷达实际角速度和加速度；
和/>分别表示原始角速度的IMU零偏与加速度的IMU零偏，/>和/>统称为IMU零偏bt，和/>分别表示原始角速度的白噪声与加速度的白噪声，/>和/>统称为nt；
在时间t+△t处，机器人的速度vt+Δt、机器人的位置pt+Δt和旋转变换Rt+Δt分别由以下公式计算：
其中，Rt，pt，vt分别表示t时刻激光雷达的旋转变换，位置和速度，△t表示短时间；
第二步计算机器人i与j时刻之间的相对运动变化，这段时间内的速度变化△vij、位置变换△pij和旋转变换△Rij分别由以下公式计算：
其中，vi，pi，Ri分别表示激光雷达在i时刻的速度，位置和旋转变换，vj，pj，Rj分别表示激光雷达在j时刻的速度，位置和旋转变换，△tij表示i时刻与j时刻的时间差；
然后，利用开源的GTSAM优化库，优化IMU的速度变化△vij、位置变换△pij和旋转变换△Rij三个状态变量，在下一次预积分结果输入之前，GTSAM优化器会输出更新的位置变换、速度变换和旋转变换以及零偏矩阵b′i；
利用更新的零偏b′i，通过下式重新计算预积分，获得优化后的IMU在β时刻的速度位置/>和旋转变换/>
其中，和/>分别表示更新后原始角速度的IMU零偏和更新后加速度的IMU零偏，/>和表示第i时刻原始角速度的白噪声和第i时刻加速度的白噪声；
通过三阶段预积分计算得到确定的两个时刻之间的相对位姿变化，进而得到计算公式如下：
分别表示更新后的i与j时刻之间速度变换、位置变换、和旋转变换；
fIMU表示IMU传感器在i帧与i+1帧之间的观测模型误差将预积分得到的/>代入公式(41)可以得到IMU传感器在i帧与j帧之间的观测模型误差/>按同样的方法可得到
3.如权利要求2所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方法，其特征在于：所述S100中获得fLidar的过程如下：
机器人激光里程计输出相邻两个关键帧i与i+1之间的位姿变换关系△Ti,i+1，将△Ti,i+1用来计算激光里程计因子中误差项，表示为：
fLidar表示激光雷达传感器在i帧与i+1帧之间的观测模型误差将△Ti,i+1代入公式(41)可以得到/>
4.如权利要求3所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方法，其特征在于：所述S100中获得fLoop的过程如下：
利用传统的迭代最近点特征匹配方法得到两个点集之间准确的位姿变换△T，设当前点云位姿为Ti+1，回环帧的位姿为Tc，则得到两者之间的相对位姿变化，即闭环因子表示为：
其中，
fLoop表示回环检测在当前帧第i+1帧与回环帧第c帧的两帧之间的观测模型误差将/>代入公式(41)可以得到/>
5.如权利要求4所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方法，其特征在于：所述S200中回环检测的过程如下：
S201：激光雷达获取环境信息得到的点云包括距离信息和强度信息，通过式子(59)对强度信息进行归一化和信息校准后得到η′k，假设第k帧中每个点云表示为[xk,yk,zk,η′k]，将笛卡尔坐标系下的3D点云投影到由X Y平面构成的极坐标坐标系下，变换后的点云极坐标表示为：
S202：然后以传感器为中心，通过环形角度和径向方向将点云等分切分Ns×Nr个子区域，每个分段表示为：
其中a∈[1,Ns]，b∈[1,Nr]，Lmax表示激光雷达依次扫描最大的感应范围；因为同一物体返回的强度值相同，对于每个小区域Sab里的点云，用子区域中的最大强度信息代替该空间的强度值ηab：
S203：提取了每个子区域Sab的强度信息后，还需要对该区域里的点集继续编码，在将每个子区域Sab内的点云按垂直方向分为8个单元，并从下到上标记为zk，其中k∈(1,2,...,8)，z坐标相同的点在同一个单元；若某个单元中没有激光点，则将zk置为零，否则，若一个单元内至少有一个激光点，则将zk置为1，每个子区域Sab都会得到一个8位的二进制编码，然后根据zk值将二进制转换为十进制数值，则每个区域的几何信息dab可以表示为：
其中z(·)表示将Sab的二进制编码转换为十进制值的函数，将该式子中的几何信息与式子(64)中的强度信息结合，就得到点云全局描述符Ω(a,b)：
S204：采用两步分层的快速检测方法，分别计算几何相似度和强度相似度来加速匹配，然后再基于一致性验证匹配的有效性；
S204-1：几何相似度计算
将当前点云的全局描述符表示为Ωq，待匹配点云的全局描述符为Ωc，两者相同索引处的几何编码值分别为与/>计算/>与/>的余弦距离并归一化处理，则得到几何相似度函数：
其中，NS表示前面提到的NS个扇区，dq，dc分别表示当前点云全局描述符Ωq和待匹配点云全局描述符Ωc中对应的几何信息；
假设某个点云的全局描述符为这里的n表示点云的全局描述符在水平方向上平移了n个单位，列位移数n*和移动后的几何相似度得分D(dq,dc)分别定义为：
S204-2：强度相似度计算
设和/>分别为ηq和ηc索引值相同的列，取/>和/>计算强度相似度得分/>如下：
其中，ηq和ηc分别表示两个点云描述符之间的强度特征，分别表示强度相似度得分；
通过比较ηc和ηq平移n*列之后的强度特征最终计算强度相似度计算结果为：
n*表示视角变化；
S204-3：一致性验证
使用当前帧的过去N帧点云描述符和K个候选帧的过去N帧点云描述符的相似度的平均值作为最终相似度，将其作为一致性的验证标准，计算公式如下所示：
其中Θ(pq,pc)表示当前帧的过去N帧点云描述符和K个候选帧中某一帧的过去N帧点云描述符的相似度的平均值，pq表示当前帧，pc表示K个候选帧中的某一帧，dq-i，dc-i，ηq-i，ηc-i分别表示当前帧与候选帧点云全局描述符中的几何信息和强度信息；
在移动机器人反方向访问时，即传感器观测的视角发生变化的情况下，Ωq-i相应地改变为Ωq+i，利用得到的K个待匹配对象构建一个K近邻搜索树，将目标点云的描述符与搜索树中的进行比较，采用式(72)度量它们之间的相似度；最后，通过设置一个阈值选择最相似的候选点，将其作为重新访问的位置，表示为：
其中C表示搜索树中K个待匹配对象索引值，是设置的阈值；c*表示最终得到的闭环位置索引值，从历史关键帧中获取回环帧的位姿Tc，并将其用于后续构建闭环因子。
CN202310632057.2A 2023-05-31 2023-05-31 用于机器人精准位姿获取的基于多因子图的后端优化方法 Pending CN116758153A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310632057.2A CN116758153A (zh) 2023-05-31 2023-05-31 用于机器人精准位姿获取的基于多因子图的后端优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310632057.2A CN116758153A (zh) 2023-05-31 2023-05-31 用于机器人精准位姿获取的基于多因子图的后端优化方法

Publications (1)

Publication Number Publication Date
CN116758153A true CN116758153A (zh) 2023-09-15

Family

ID=87946943

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310632057.2A Pending CN116758153A (zh) 2023-05-31 2023-05-31 用于机器人精准位姿获取的基于多因子图的后端优化方法

Country Status (1)

Country Link
CN (1) CN116758153A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116958452A (zh) * 2023-09-18 2023-10-27 北京格镭信息科技有限公司 三维重建方法和***
CN117761704A (zh) * 2023-12-07 2024-03-26 上海交通大学 多机器人相对位置的估计方法及***

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116958452A (zh) * 2023-09-18 2023-10-27 北京格镭信息科技有限公司 三维重建方法和***
CN117761704A (zh) * 2023-12-07 2024-03-26 上海交通大学 多机器人相对位置的估计方法及***

Similar Documents

Publication Publication Date Title
Chen et al. OverlapNet: Loop closing for LiDAR-based SLAM
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及***
Wang et al. Intensity scan context: Coding intensity and geometry relations for loop closure detection
CN110645974B (zh) 一种融合多传感器的移动机器人室内地图构建方法
CN111693047B (zh) 一种高动态场景下的微小型无人机视觉导航方法
CN110675418B (zh) 一种基于ds证据理论的目标轨迹优化方法
CA2982044C (en) Method and device for real-time mapping and localization
Lenac et al. Fast planar surface 3D SLAM using LIDAR
Cieslewski et al. Point cloud descriptors for place recognition using sparse visual information
CN111127513A (zh) 一种多目标跟踪方法
CN116758153A (zh) 用于机器人精准位姿获取的基于多因子图的后端优化方法
CN106780631B (zh) 一种基于深度学习的机器人闭环检测方法
CN110717927A (zh) 基于深度学习和视惯融合的室内机器人运动估计方法
CN113432600A (zh) 基于多信息源的机器人即时定位与地图构建方法及***
CN114998276B (zh) 一种基于三维点云的机器人动态障碍物实时检测方法
CN113092807B (zh) 基于多目标跟踪算法的城市高架道路车辆测速方法
CN114119659A (zh) 一种多传感器融合的目标跟踪方法
He et al. Online semantic-assisted topological map building with LiDAR in large-scale outdoor environments: Toward robust place recognition
Wu et al. A robust and precise LiDAR-inertial-GPS odometry and mapping method for large-scale environment
Cui et al. Online multi-target tracking for pedestrian by fusion of millimeter wave radar and vision
CN113781563B (zh) 一种基于深度学习的移动机器人回环检测方法
Nielsen et al. Survey on 2d lidar feature extraction for underground mine usage
Zhang et al. A LiDAR-intensity SLAM and loop closure detection method using an intensity cylindrical-projection shape context descriptor
CN113160280A (zh) 一种基于激光雷达的动态多目标跟踪方法
Leung et al. Evaluating set measurement likelihoods in random-finite-set slam

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