[发明专利]基于位姿链模型的SLAM后端轨迹优化方法

权利要求书

1.基于位姿链模型的SLAM后端轨迹优化方法，其特征在于，包括以下步骤：

S1. 通过前端获得的初始估计和概率约束建立完整的位姿链模型；

S2. 对位姿链模型的闭环部分做轨迹优化，位姿链中的节点有严格的时序先后之分，针对每个闭环中的最后一个节点，计算其与闭环初始节点之间的位姿变换，将其作为整个闭环中轨迹的总误差，先使用插值函数将此误差做局部分解，分解成为与闭环内边的条数相同份数的局部更新量，再将此局部更新量使用李群上的乘法运算分配到闭环中对应的边上；

S3. 再针对相邻的两个位姿点，用算法分配得到的局部更新量去更新原来两位姿点的位姿，计算出更加准确的相对位姿，在闭环内部完成优化，由于闭环部分和非闭环部分中有一个节点是公共的，运用此节点优化前后的位姿变换作为非闭环部分的累计总误差并将其使用相同的方式分配到非闭环部分的各节点上，完成整条轨迹的优化。

2.根据权利要求1所述的基于位姿链模型的SLAM后端轨迹优化方法，其特征在于：所述的步骤S1中，所述的位姿链模型为稀疏的位姿链模型。