[发明专利]一种分布式多机器人协同SLAM的位姿优化方法

说明书

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

技术领域

本发明涉及定位与地图构建技术领域，具体涉及一种分布式多机器人协同SLAM的位姿优化方法。

背景技术

分布式协同定位与地图构建系统包括多个可以独立完成定位与地图构建的机器人。在一个多机器人协同即时定位与地图构建(Simultaneous Localization andMapping，SLAM)系统中，每个机器人的目标是利用可用的测量值来估计自身的运动轨迹，并利用与其他机器人会合时的相互通信传输相对位姿信息对自身的位姿进行位姿图优化，因此机器人将得到两种类型的观测：机器人内的观测和机器人间的观测。

将所有机器人的运动轨迹整合到一个(待估计的)位姿集，这个位姿集的最大似然估计值为测量似然值的最大值。由于求解最大似然估计值的过程中存在非凸约束，因此分布式协同定位与地图构建的位姿优化过程具有相当大的计算量并且对位姿初始值的准确度要求较高。

因此，目前亟需一种分布式协同SLAM的位姿优化方法，以解决非凸约束导致的求解难度大、对初始值的准确度要求高的问题。

发明内容

有鉴于此，本发明实施例提供了一种分布式多机器人协同SLAM的位姿优化方法，以解决现有技术中多机器人协同SLAM系统中由非凸约束导致的求解难度大、对初始值的准确度要求高的问题。

本发明实施例提供了一种分布式多机器人协同SLAM的位姿优化方法，包括：

通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化；

通过高斯牛顿迭代实现完全版本位姿估计进行恢复；

根据完全版本位姿估计建立图优化方程；

采用分布式雅可比超松弛迭代对位姿图进行优化。

可选地，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，包括：

对机器人位姿优化残差式中的旋转子部分进行去非凸约束松弛化处理，得到机器人位姿优化残差式中的旋转子部分的无非凸约束形式；

将机器人位姿优化残差式中旋转子部分转换成第一线性方程；

通过最小二乘原理将第一线性方程转化为正规方程并进行求解。

可选地，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，还包括：

将正规方程求解出来的向量，按一个列向量对应矩阵一行的排列规则，重新写成旋转矩阵；

通过特殊正交化投影将旋转矩阵投影到特殊正交群上。

可选地，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，包括：

设置未知扰动参数；

将每个未知旋转矩阵重写成已知旋转估计矩阵乘以未知扰动参数在特殊正交群上映射的形式，从而实现对机器人位姿优化残差式中旋转矩阵的新参数化。

可选地，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，还包括：