基于位姿链模型的SLAM后端轨迹优化方法与流程

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

背景技术：

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

技术实现要素：

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

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

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

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

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

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

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

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

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

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

附图说明

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

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

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

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

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

如图1所示，本发明采用了位姿链slam后端优化模型，模型的特点在于：节点之间有严格的时序关系、图模型比较稀疏。

三角形表示机器人的pose，边表示相连pose之间的相对pose，模型中有两种边：连续边，连接t、t+1时刻pose；闭环边，连接t、t+k(k>1)时刻pose，图中虚线部分即为闭环边。

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

本算法优化主要分为两部分，首先对闭环部分进行优化，优化完成后针对非闭环部分与闭环部分公共点优化前后的位姿的改变对非闭环部分进行优化，两部分优化采用相同的方式，主要分三步进行：

(1)计算局部更新量：如图3中an与dn之间的五个黑色三角形分别为左边五条边的局部更新量。

(2)计算分布更新量：如图4中an与dn之间的五个黑色三角形用虚线对应到左边的五条边上的量为左边五条边的分布更新量。

(3)重新计算待优化节点间的相对位姿，使得an收敛到dn；

本算法的主要特点在于：每检测到一个闭环就开始优化，这一特点保证了在线优化的可能，图模型中闭环边的减少使得图模型更加稀疏，使得计算速度得到大幅度提升，闭环部分优化完成后对非闭环部分再进行优化的反馈机制保持了整体的优化精度。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

技术特征：

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

技术研发人员：陈龙;杨俊;黎丹
受保护的技术使用者：中山大学
技术研发日：2017.05.18
技术公布日：2017.10.20