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

权利要求书

1.一种分布式多机器人协同SLAM的位姿优化方法，其特征在于，包括：

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

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

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

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

2.根据权利要求1所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，包括：

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

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

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

3.根据权利要求2所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过去非凸约束松弛化和特殊正交化投影得到全部机器人的旋转位姿估计，完成旋转初始化，还包括：

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

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

4.根据权利要求3所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，包括：

设置未知扰动参数；

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

5.根据权利要求4所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，还包括：

使用一阶近似指数映射对新参数化后的所述机器人位姿优化残差式进行一阶近似，得到第二机器人位姿优化残差式；所述第二机器人位姿优化残差式为一阶近似的新参数化机器人位姿优化残差式。

6.根据权利要求5所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，通过高斯牛顿迭代实现完全版本位姿估计进行恢复，还包括：

将所有机器人位姿中的未知量重新排列为一个向量P，以将所述第二机器人位姿优化残差式转换为第二线性方程；所述未知量包括位移姿态和旋转修正量；

通过最小二乘原理对所述第二线性方程转换为正规方程，从而对所述向量P求解，得到旋转修正参数；

通过所述旋转修正参数获取机器人自身旋转位姿和机器人自身位移位姿。

7.根据权利要求1所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，根据所述完全版本位姿估计建立图优化方程，包括：

将所述完全版本位姿估计转换为李群T的形式；

根据其他机器人在任意时刻对自身机器人的观测数据设置位姿向量。

8.根据权利要求1所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，采用分布式雅可比超松弛迭代对位姿图进行优化，包括：

获取自身位姿信息；

固定相对位姿，对自身位姿进行位姿图优化，得到自身位姿优化估计；

以所述自身位姿优化估计为基础，固定所述自身位姿，对所述相对位姿进行图位姿优化估计。

9.根据权利要求8所述的分布式多机器人协同SLAM的位姿优化方法，其特征在于，采用分布式雅可比超松弛迭代对位姿图进行优化，还包括：

当估计精度符合预设条件时，通过迭代求出机器人位姿优化残差，从而求解机器人自身运动轨迹位姿集的最大似然估计值。