CN116758153B 用于机器人精准位姿获取的基于多因子图的后端优化方法 （重庆大学）

用于机器人精准位姿获取的基于多因子图本发明涉及一种用于机器人精准位姿获取传感器的观测模型误差并构建IMU因子，同时利用前端激光里程计输出的位姿变化关系计算激将这三种约束因子与相关联的状态变量用边相2式中与en分别表示IMU因子、激光里程计因子与闭环因子观测模型的误2.如权利要求1所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方IMU的过程如下：⃞=q,+"+n"(42)v=v,+gA+R(a,-b"-n:)Ar(44)3tAr,=(v⃞-v-gA4)(47)iin"表示第i时刻原始角速度的白噪声和第i时刻加速度的白噪声；Ar;=R;(55)i,fIMU表示IMU传感器在i帧与i+1帧之间的观测模型误差将预积分得到的ar,"代入公式(41)可以得到IMU传感器在i帧与j帧之间的观测模型误差e;,按同样的方法可得到43.如权利要求2所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方Lidar的过程如下：rm(57)fLidar表示激光雷达传感器在i帧与i+1帧之间的观测模型误差efn,将△Ti,i+1代入公式4.如权利要求3所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方Loop的过程如下：利用传统的迭代最近点特征匹配方法得到两个点集之间准确的位姿变换△T，设当前fLoop表示回环检测在当前帧第i+1帧与回环帧第c帧的两帧之间的观测模型误差eu,5.如权利要求4所述的一种用于机器人精准位姿获取的基于多因子图的后端优化方S201：激光雷达获取环境信息得到的点云包括距离信息将笛卡尔坐标系下的3D点云投影到由XY平面构成的极坐标坐标系下，变换后的点云极坐S202：然后以传感器为中心，通过环形角度和径向方向将点云等分切分Ns×Nr个子区5个子区域Sab内的点云按垂直方向分为8个单元，并从下到上标其中z(·)表示将Sab的二进制编码转换为十进制值的函数，将该式子中的几何信息与的几何编码值分别为与计算与的余弦距离并归一化处理，则得到几何相似度函云全局描述符Ωc中对应的几何信息；假设某个点云的全局描述符为2;,这里的n表示点云的全局描述符在水平方向上平移设和分别为ηq和ηc索引值相同的列，取和计算强度相似度得分如q和ηc分别表示两个点云描述符之间的强度特征，分别表示强度相似通过比较ηc和ηq平移n*列之后的强度6使用当前帧的过去N帧点云描述符和K个候选帧的过去N帧点云描述符的相似度的平均ηq-ic-i分别表示当前帧与候选帧点云全局描述符中的几何在移动机器人反方向访问时，即传感器观测的视角发生变化的情况下，Ωq-i相应地改7化地图的连贯性和一致性。目前后端优化算法主要包括基于滤波器的方法和基于非线性[0005](1)单一的传感器用于定位和建图时会面临场景变化导致特征退化的弊端，为了[0006](2)回环检测是后端优化的重要环节，关于回环检测现有的工作通常利用点云局ii+18[0014]式中与分别表示IMU因子、激光里程计因子与闭环因子观测模型由三种因子计算的两个变量节点之间的相对位姿变换关系，△Ti,j包括和[0020]⃞=q,+"+n"(42)[0021]⃞,=R"(a,-g)+b"+n"(43)[0022]式中R,"表示地图坐标系W到传感器坐标系B的旋转矩阵，g是W中的重力加速度，[0030]Ar,=(v⃞-v-gA)(47)9bi′[0040]通过三阶段预积分计算得到确定的两个时刻之间的相对位姿变化，进而得到a,",计算公式如下：[0043]Ar;=Rr;(ss[0046]fIMU表示IMU传感器在i帧与i+1帧之间的观测模型误差将预积分得到的代入公式(41)可以得到IMU传感器在i帧与j帧之间的观测模型误差ef,按同样的方法r(57)[0050]fLidar表示激光雷达传感器在i帧与i+1帧之间的观测模型误差eion,将△Ti,i+1代入[0052]利用传统的迭代最近点特征匹配方法得到两个点集之间准确的位姿变换△T，设[0055]fLoop表示回环检测在当前帧第i+1帧与回环帧第c帧的两帧之间的观测模型误差将代入公式(41)可以得到[0057]S201：激光雷达获取环境信息得到的点云包括距离信息和强度信息。通过式子将每个子区域Sab内的点云按垂直方向分为8个单元，并从下到上标记为zk，其中k∈(1,,配点云全局描述符Ωc中对应的几何信息。[0075]假设某个点云的全局描述符为2;,这里的n表示点云的全局描述符在水平方向上ηc分别表示两个点云描述符之间的强度特征，分别表示强度[0086]使用当前帧的过去N帧点云描述符和K个候选帧的过去N帧点云描述符的相似度的ηc-i分别表示当前帧与候选帧点云全局描述[0093]本发明在现有的因子图优化模型中加入IMU因子，除了SLAM框架前端激光里程计动态目标质心状态的最优估计值其中，表示t时刻潜在动态目标的对上一帧的位姿变换关系和上一帧激光雷达的位姿就可以计算出激光雷达当前帧的位姿[0124]对无序点云进行序列化后，基于时序找到与tk时刻最近的两帧IMU数据并进行预理可得到tk+1时刻IMU姿态基于此即可得到tk+1时刻相对于tk时刻差和距离以及单元格是否被占用的二进制表示，这些特征信息被用于生成FCNN网络的输动态目标对应的激光点坐标计算潜在动态目标的质心",",表示t时刻和t-1时[0155]通过公式(9)将t时刻图片中潜在动态目标与将t-1时刻图片中的潜在动态目标进次激光雷达扫描的所有静态目标对应的所有激光点表示为Pk′，将每一个激光点表示为[0159]将p""所在线束在水平方向上的连续点集用表示，则对于p""相邻点构成p""的曲率的计算公式为：[0161]式中Ns表示s""中激光点的个数，p""表示s[0162]然后根据c值的大小对s""中的所有激光点进行分类，c值大于设定阈值的作为提取两个边缘特征点和四个平面特征点，四个子区域中所有边缘特征点的集[0170]式中表示当前第k帧的静态目标对应的所有激光点重投影到当前帧开始时刻的[0171]假设εk中边缘特征点pε的局部平滑度是cε,sk中平面特征点ps的局部平滑度是cs，p"的曲率。局边缘特征集合即一帧图像中含有的所有εk的集合，如果协局平面特征集合即一帧图像中含有的所有sk的集合，如果协方差矩阵包含一个最小特征pp[0184]其中[Tp]×即[Tkpk]×,表示将4D点表达式{x,y,z,1}转换为3D点表达式{x,y,z}并表示一个4*4矩阵和4*1的向量得到一个4*1的向量，边缘残差的雅可比矩阵Jε通过以下公帧激光点云的投影图，先维护一个栈作为滑动窗口。接着在时序上提取i个最靠近的关键帧，并将这些关键帧表示为一个子关键帧集{Fk-i,...,Fk}，通过与W之间的变换矩阵[0193]其中，分别代表子关键帧集中第k个关键帧和k-i个关键帧的边缘特征ire是△Tk,k+1的李代数表示，通过公式(19)就可以计更新后的最优定位估计，△Tk,k+1表示相邻两关键帧间位姿变换关系，pi表示更新的静态[0201]第k+1次激光雷达扫描的静态目标对应的所有激光点表示为P/k+1，P/k+1中一个激光点表示为pk+1，为两个连续帧k和k+1之间的6自由度位姿变换，根据第k+1帧的△Tk,k+1计算得到，pi表示更新的静态目标对应的激光点的最[0202]输出pi和相邻两关键帧间位姿变换关系△Tk,k+1。作用下决定观测数据z1。位姿x1根据预测方程g和控制输入量u2共同决定了下一次的位姿fLidar[0238]式中与e:mn分别表示IMU因子、激光里程计因子与闭环因子观测模型由三种因子计算的两个变量节点之间的相对位姿变换关系，△Ti,j包括和并纠正IMU数据中的误差；第三步利用更新的IMU零偏重新计算积分，并输出IMU因子相关[0245]⃞=q,+"+n"(42)式子中可以看出和a,受IMU零偏bt和白噪声nt的影响，ωt和at分别表示t时刻激光雷达实[0248]和分别表示原始角速度的IMU零偏与加速度的IMU零偏，b和"统称为IMU零[0249]然后进行第一阶段计算，通过整合IMU的多帧角速度与加速度来估计机器人的运结果输入之前，GTSAM优化器会输出更新的位置变换、速度变换和旋转变换以及零偏矩阵bi′[0265]通过三阶段预积分计算得到确定的两个时刻之间的相对位姿变化，进而得到A,",计算公式如下：Ar;=RR;(55)[0271]fIMU表示IMU传感器在i帧与i+1帧之间的观测模型误差eimn:将预积分得到的dr,"代入公式(41)可以得到IMU传感器在i帧与j帧之间的观测模型误差ef,按同样的方法可得到Tim(57)[0275]fLidar表示激光雷达传感器在i帧与i+1帧之间的观测模型误差eion,将△Ti,i+1代入公式(41)可以得到误差将代入公式(41)可以得到[0282]S201：激光雷达获取环境信息得到的点云包括距离信息和强度信息。通过式子配点云全局描述符Ωc中对应的几何信息。[0303]假设某个点云的全局描述符为s2;,这里的n表示点云的全局描述符在水平方向上准确的匹配结果。其核心是通过逐列计算两个点云描述符之间的强度特征ηq和ηc的相似性来实现的。设和分别为ηq和ηc索引值相同的列，取艰和计算强度相似度得分如下：ηc分别表示两个点云描述符之间的强度特征，分别表示强度ηc-i分别表示当前帧与候选帧点云全局描述架上加入本发明提出的基于多因子图的后端优化算法，针对提出的多因子图后端优化算pp[0335]评价后端优化算法性能的相关指标包括算法输出轨迹的绝对位姿误差(ATE)和相[0340]相对位姿误差(RPE)通过计算等时间间隔的两帧位姿差值来描述测量里程计的误闭环检测实验在KITTI数据集这几个序列上进行。一方面通过对比本发明提出的后端优化算法在开启闭环检测功能与关闭闭环检测功能的情况下，全局轨迹的RPE指标来评估闭环于描述符的LidarIris回环检测方法。同时将本发明提出的后端优化算法与LOAM和LeGO-LOAM在KITTI数据集的07序列以及08序列下输出的轨迹与地面真值作对比，且计算每个开启回环检测时在02和05序列下的全局定位结果，右侧为关闭回环检测时的全局定位结采用的回环检测方法在05序列上的RPE结果有明显差异，闭环功能关闭后的全局定位误差功能后的位姿误差相较于关闭时减少了71证明本发明采用的回环检测方法在SLAM后端SLAM中的DBoW3方法，基于点云全局描述符SC的回环检测方法以及基于描述符的Lidar[0360]表4分别展示了LOAM、LeGO-LOAM与本发明提出的后端优化算