一种分布式多机器人协同SLAM的位姿优化方法技术

本发明专利技术公开了一种分布式多机器人协同SLAM的位姿优化方法，包括：通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化；通过高斯牛顿迭代实现完全版本位姿估计进行恢复；根据完全版本位姿估计建立图优化方程；采用分布式雅可比超松弛迭代对位姿图进行优化。在合理的测量噪声水平下，通过以上方法，能够得到非常接近最大似然估计的解决方案。本实施例提供的优化方法只需要求解三个线性方程，计算效率得到了提高，此外，即使在初始运动轨迹估计不准确的情况下，本优化方法也仍然能够收敛，进而机器人完成对自身位姿的优化。完成对自身位姿的优化。完成对自身位姿的优化。

【技术实现步骤摘要】
一种分布式多机器人协同SLAM的位姿优化方法


[0001]本专利技术涉及定位与地图构建
，具体涉及一种分布式多机器人协同SLAM的位姿优化方法。

技术介绍

[0002]分布式协同定位与地图构建系统包括多个可以独立完成定位与地图构建的机器人。在一个多机器人协同即时定位与地图构建(Simultaneous Localization andMapping，SLAM)系统中，每个机器人的目标是利用可用的测量值来估计自身的运动轨迹，并利用与其他机器人会合时的相互通信传输相对位姿信息对自身的位姿进行位姿图优化，因此机器人将得到两种类型的观测：机器人内的观测和机器人间的观测。
[0003]将所有机器人的运动轨迹整合到一个(待估计的)位姿集，这个位姿集的最大似然估计值为测量似然值的最大值。由于求解最大似然估计值的过程中存在非凸约束，因此分布式协同定位与地图构建的位姿优化过程具有相当大的计算量并且对位姿初始值的准确度要求较高。
[0004]因此，目前亟需一种分布式协同SLAM的位姿优化方法，以解决非凸约束导致的求解难度大、对初始值的准确度要求高的问题。

技术实现思路

[0005]有鉴于此，本专利技术实施例提供了一种分布式多机器人协同SLAM的位姿优化方法，以解决现有技术中多机器人协同SLAM系统中由非凸约束导致的求解难度大、对初始值的准确度要求高的问题。
[0006]本专利技术实施例提供了一种分布式多机器人协同SLAM的位姿优化方法，包括：
[0007]通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化；
[0008]通过高斯牛顿迭代实现完全版本位姿估计进行恢复；
[0009]根据完全版本位姿估计建立图优化方程；
[0010]采用分布式雅可比超松弛迭代对位姿图进行优化。
[0011]可选地，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，包括：
[0012]对机器人位姿优化残差式中的旋转子部分进行去非凸约束松弛化处理，得到机器人位姿优化残差式中的旋转子部分的无非凸约束形式；
[0013]将机器人位姿优化残差式中旋转子部分转换成第一线性方程；
[0014]通过最小二乘原理将第一线性方程转化为正规方程并进行求解。
[0015]可选地，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，还包括：
[0016]将正规方程求解出来的向量，按一个列向量对应矩阵一行的排列规则，重新写成
旋转矩阵；
[0017]通过特殊正交化投影将旋转矩阵投影到特殊正交群上。
[0018]可选地，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，包括：
[0019]设置未知扰动参数；
[0020]将每个未知旋转矩阵重写成已知旋转估计矩阵乘以未知扰动参数在特殊正交群上映射的形式，从而实现对机器人位姿优化残差式中旋转矩阵的新参数化。
[0021]可选地，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，还包括：
[0022]使用一阶近似指数映射对新参数化后的机器人位姿优化残差式进行一阶近似，得到第二机器人位姿优化残差式；第二机器人位姿优化残差式为一阶近似的新参数化机器人位姿优化残差式。
[0023]可选地，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，还包括：
[0024]将所有机器人位姿中的未知量重新排列为一个向量P，以将第二机器人位姿优化残差式转换为第二线性方程；未知量包括位移姿态和旋转修正量；
[0025]通过最小二乘原理对第二线性方程转换为正规方程，从而对向量P求解，得到旋转修正参数；
[0026]通过旋转修正参数获取机器人自身旋转位姿和机器人自身位移位姿。
[0027]可选地，根据完全版本位姿估计建立图优化方程，包括：
[0028]将完全版本位姿估计转换为李群T的形式；
[0029]根据其他机器人在任意时刻对自身机器人的观测数据设置位姿向量。
[0030]可选地，采用分布式雅可比超松弛迭代对位姿图进行优化，包括：
[0031]获取自身位姿信息；
[0032]固定相对位姿，对自身位姿进行位姿图优化，得到自身位姿优化估计；
[0033]以自身位姿优化估计为基础，固定自身位姿，对相对位姿进行图位姿优化估计。
[0034]可选地，采用分布式雅可比超松弛迭代对位姿图进行优化，还包括：
[0035]当估计精度符合预设条件时，通过迭代求出机器人位姿优化残差，从而求解机器人自身运动轨迹位姿集的最大似然估计值。
[0036]本专利技术实施例的有益效果：
[0037]本专利技术实施例提供了一种分布式多机器人协同SLAM的位姿优化方法，应用于多机器人分布式协同定位与地图构建系统，在合理的测量噪声水平下，能够得到非常接近最大似然估计的解决方案。本专利技术实施例提供的分布式多机器人协同SLAM的位姿优化方法，只需要求解三个线性方程，计算效率得到了提高。此外，本专利技术实施例提供的分布式多机器人协同SLAM的位姿优化方法不需要初始预测，即使在初始运动轨迹估计不准确的情况下，也仍然能够收敛，进而机器人完成对自身位姿的优化。
附图说明
[0038]通过参考附图会更加清楚的理解本专利技术的特征和优点，附图是示意性的而不应理解为对本专利技术进行任何限制，在附图中：
[0039]图1示出了本专利技术实施例中一种分布式多机器人协同SLAM的位姿优化方法的流程图；
[0040]图2示出了本专利技术实施例中另一种分布式多机器人协同SLAM的位姿优化方法的流程图；
[0041]图3示出了本专利技术实施例中一种分布式协同SLAM中机器人内测量和机器人间测量示意图；
[0042]图4示出了本专利技术实施例中一种分布式协同SLAM位姿图优化中H矩阵的块结构示意图；
[0043]图5示出了本专利技术实施例中一种分布式协同SLAM中机器人运动轨迹估计示意图。
具体实施方式
[0044]为使本专利技术实施例的目的、技术方案和优点更加清楚，下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。
[0045]本专利技术实施例提供了一种分布式多机器人协同SLAM的位姿优化方法，如图1和图2所示，包括：
[0046]步骤S10，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化。
[0047]在多机器人系统中，每个机器人的目标是利用可用的观测来估计自身的轨迹，每个机器人有机器人内观测和机器人间观测，再通过与其他机器人偶尔会合通信时，其他机器人传输来的相对位姿信息来对自身运动轨迹进行位姿图优化。在对自身位姿优化的过程中，由本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种分布式多机器人协同SLAM的位姿优化方法，其特征在于，包括：通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化；通过高斯牛顿迭代实现完全版本位姿估计进行恢复；根据所述完全版本位姿估计建立图优化方程；采用分布式雅可比超松弛迭代对位姿图进行优化。2.根据权利要求1所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，包括：对机器人位姿优化残差式中的旋转子部分进行去非凸约束松弛化处理，得到所述机器人位姿优化残差式中的旋转子部分的无非凸约束形式；将所述机器人位姿优化残差式中旋转子部分转换成第一线性方程；通过最小二乘原理将所述第一线性方程转化为正规方程并进行求解。3.根据权利要求2所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，还包括：将所述正规方程求解出来的向量，按一个列向量对应矩阵一行的排列规则，重新写成旋转矩阵；通过特殊正交化投影将所述旋转矩阵投影到特殊正交群上。4.根据权利要求3所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，包括：设置未知扰动参数；将每个未知旋转矩阵重写成已知旋转估计矩阵乘以所述未知扰动参数在特殊正交群上映射的形式，从而实现对所述机器人位姿优化残差式中旋转矩阵的新参数化。5.根据权利要求4所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过高斯牛顿迭代实现完全版本位姿估计...

【专利技术属性】
技术研发人员：邱海洋，张旭，王慧，智鹏飞，朱志宇，葛慧林，
申请(专利权)人：江苏科技大学，
类型：发明
国别省市：