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

说明书

技术领域

本发明涉及全自主移动机器人的技术领域，更具体地，涉及基于位姿链模型的SLAM后端轨迹优化方法。

背景技术

SLAM(同时定位与建图)被认为是实现真正全自主移动机器人的关键，主要分为前端预处理和后端优化两部分，前端预处理通过对各种传感器数据进行分析综合，提供初始估计地图和位置信息，后端优化是利用图模型描述初始估计和概率约束，运用最优化的方法对初始估计进行优化，实现更高精度的建图与定位。一般的SLAM后端轨迹优化算法都是将前端获取的数据全部获取之后根据建立的因子图或位姿图提炼出一个待优化的目标函数，利用最小二乘算法将非线性方程线性化求得更新步长，这类算法都属于离线优化，而真正的SLAM系统与一般的三维重建和定位技术的区别就在于能够在运动的时候得到实时的周围环境的三维建图和准确位置，所以离线的优化只是事后的处理方案，对于真正的应用来说没有太大价值。

发明内容

本发明为克服上述现有技术所述的至少一种缺陷，提供基于位姿链模型的SLAM后端轨迹优化方法，实现在线实时优化。

本发明的技术方案是：采用了节点间有严格时序关系的位姿链模型，利用时序关系每检测到一个闭环，就对闭环内的轨迹进行一次优化，这样就避免了像其他优算法一样必须等到采集完所有数据之后才进行优化的处理滞后问题，可以实现在线优化。

具体的，基于位姿链模型的SLAM后端轨迹优化方法，其中，包括以下步骤：

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

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

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

本算法引进了节点间有严格时序关系的位姿链模型，认为前端的闭环检测模块所获得的精度比初始估计更值得信赖。

本算法与一般的后端优化算法的共同特征在于：首先建立表示初始估计和概率约束的图模型，利用图模型计算出初始估计与按照其他方式推算出来的更为准确的估计值之差，使用一定的数学方法将总误差合理分散到轨迹的各部分，而本发明区别于一般后端轨迹优化算法的特征在于：采用的不是其他算法用到的位姿图模型，而选择更为稀疏的位姿链模型，位姿链模型的运用使得本算法能够解决后端优化的庞大计算量导致的优化速度慢等问题，同时，一般的轨迹优化算法需要在采集完所有的数据之后才能计算总误差，都是离线算法，而由于位姿链模型中的节点之间有严格的时序关系，只要遇到闭环就可以开始优化，这使得在线优化成为可能。因此能够满足一般应用的在线优化需求。

进一步的，采用了更为稀疏的位姿链模型。算法建立在当今的传感器精度达到一定标准的基础之上，因此不需要做过多的闭环检测减少轨迹漂移，相比于传统的用于后端轨迹优化的位姿图模型来说要稀疏很多，解决了一般的SLAM后端优化技术不能实现的在线实时优化的问题，同时也能满足应用的精度要求，为解决SLAM后端优化问题提供了一种快速有效的解决方法。

与现有技术相比，有益效果是：本发明的有益效果是采用了节点间有严格时序关系的位姿链模型，利用时序关系实现了在线优化，同时利用图模型的稀疏性使得计算速度相对于一般优化算法有了大幅度提升。一般来说速度和精度是很难兼顾，而本算法速度提升之后精度上的牺牲很小，可以能够满足应用需求。

附图说明

图1是本发明中使用的位姿链模型。

图2是本发明里程计的初始估计及真实轨迹示意图。

图3是本发明根据初始估计及真实轨迹计算局部更新量示意图。

图4是本发明根据局部更新量计算分布更新量示意图。

具体实施方式