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

摘要

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

主权项

1.一种基于开关约束的水下航行器定位鲁棒优化方法，其特征在于步骤如下：步骤1：通过水下航行器实验来构建因子图，并使用模型的条件概率分布P(X|U)来解决SLAM姿态图问题：其中：X表示节点姿态，U表示节点约束；给定一组里程计测距和闭环约束ui,uij∈U，其中ui表示任何类型的控制输入，与xi相关，xi表示节点i；uij表示与xi和xj之间的闭环有关的位移；最优的机器人姿态表示为X*；步骤2：将模型的条件概率分布P(X|U)分解成：其中：xi+1～N(f(xi,ui),∑i)，xj～N(f(xi,uij),∧ij)，f(xi,ui)表示通过运动模型连接两个连续状态xi和xi+1的里程计约束条件，∑i表示里程计约束协方差矩阵，∧ij表示闭环约束协方差矩阵；求解最优的机器人姿态X*：步骤3：对最优的机器人姿态X*引入二进制加权因子ωij：所述权重ωij＝Ψ(sij):R→{0,1}其中：Ψ(sij)为将开关变量映射为权重因子的开关函数；步骤4：对Ψ使用S形函数：并将新的开关变量S＝{sij}引入到成本函数中：步骤5：将sij的初始值设置为γij，并将开关变量sij建模为正态分布的高斯变量：sij～N(γij,Ξij)其中：Ξij为sij的切换先验约束的协方差矩阵；基于以上，得出下式：为导航控制的最优机器人姿态X*。