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

摘要

本发明涉及全自主移动机器人的技术领域，更具体地，涉及基于位姿链模型的SLAM后端轨迹优化方法。SLAM被认为是实现真正全自主移动机器人的关键，主要分为前端预处理和后端优化两部分，前端预处理通过对各种传感器数据进行分析综合，提供初始估计地图和位置信息，后端优化是利用图模型描述初始估计和概率约束，运用最优化的方法对初始估计进行优化，实现更高精度的建图与定位。本算法引进了节点间有严格时序关系的位姿链模型，利用位姿链模型的稀疏特性解决了一般的SLAM后端优化技术不能实现的在线实时优化的问题，同时也能满足应用的精度要求，为解决SLAM后端优化问题提供了一种快速有效的解决方法。

主权项

基于位姿链模型的SLAM后端轨迹优化方法，其特征在于，包括以下步骤：S1. 通过前端获得的初始估计和概率约束建立完整的位姿链模型；S2. 对位姿链模型的闭环部分做轨迹优化，位姿链中的节点有严格的时序先后之分，针对每个闭环中的最后一个节点，计算其与闭环初始节点之间的位姿变换，将其作为整个闭环中轨迹的总误差，先使用插值函数将此误差做局部分解，分解成为与闭环内边的条数相同份数的局部更新量，再将此局部更新量使用李群上的乘法运算分配到闭环中对应的边上；S3. 再针对相邻的两个位姿点，用算法分配得到的局部更新量去更新原来两位姿点的位姿，计算出更加准确的相对位姿，在闭环内部完成优化，由于闭环部分和非闭环部分中有一个节点是公共的，运用此节点优化前后的位姿变换作为非闭环部分的累计总误差并将其使用相同的方式分配到非闭环部分的各节点上，完成整条轨迹的优化。