[发明专利]一种基于开关约束的水下航行器定位鲁棒优化方法

说明书

本发明涉及一种基于开关约束的水下航行器定位鲁棒优化方法，针对传统水下航行器导航控制方法未能对错误闭环实现鲁棒性的缺点，基于General Graph Optimization(g2o)和开关约束的导航控制方法，采用一种开关约束后端优化方法，利用节点因子图，设计了一种消除地图构建过程中产生的错误闭环的方法。本发明解决现有水下航行器在同步定位与地图构建过程中产生错误闭环而导致其所构建地图出现错误的局限，使得航行器在地图构建过程中对错误闭环进行处理并加权为零，并且添加开关先验约束，以消除错误闭环带来的不利影响。理论证明了该发明的鲁棒性，实验验证了该发明的有效性，对减小水下航行器地图构建错误率有良好的效果。

技术领域

本发明属于水下航行器导航控制领域涉及一种基于开关约束的水下航行器定位鲁棒优化方法。

背景技术

水下航行器由于没有GPS信号的支持，需要通过自身路径不断形成闭环来消除里程计误差并进行地图构建与自身定位的优化。而传统的优化方法是利用最小二乘优化方法，包括增量式平滑和映射方法(iSAM)和一致姿态估计方法，即水下航行器通过将视觉树方法与贝叶斯概率框架相结合来解决这个问题，由此将位置识别化为递归贝叶斯估计问题，或者通过立体相机来检测潜在的闭合环路，并基于几何一致性检查来过滤错误闭环。

从上述分析过程可以看出，传统的水下航行器导航控制技术系统在理想的环境下能够保证其鲁棒性，但无法实现对错误闭环的处理能力，因而极易造成地图构建出错。鉴于此，本文发明了一种基于开关约束的水下航行器定位鲁棒优化方法，用以处理错误闭环。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于开关约束的水下航行器定位鲁棒优化方法，针对传统水下航行器导航控制方法未能对错误闭环实现鲁棒性的缺点，基于General Graph Optimization(g2o)和开关约束的导航控制方法，使得航行器在地图构建过程中对错误闭环进行处理并加权为零，并且添加开关先验约束，以消除错误闭环带来的不利影响。

技术方案

一种基于开关约束的水下航行器定位鲁棒优化方法，其特征在于步骤如下：

步骤1：通过水下航行器实验来构建因子图，并使用模型的条件概率分布P(X|U)来解决SLAM姿态图问题：

其中：X表示节点姿态，U表示节点约束；给定一组里程计测距和闭环约束u_i,u_ij∈U，其中u_i表示任何类型的控制输入，与x_i相关，x_i表示节点i；u_ij表示与x_i和x_j之间的闭环有关的位移；

最优的机器人姿态表示为X^*；

步骤2：将模型的条件概率分布P(X|U)分解成：

其中：x_i+1～N(f(x_i,u_i),∑_i)，x_j～N(f(x_i,u_ij),∧_ij)，f(x_i,u_i)表示通过运动模型连接两个连续状态x_i和x_i+1的里程计约束条件，∑_i表示里程计约束协方差矩阵，∧_ij表示闭环约束协方差矩阵；

求解最优的机器人姿态X^*：