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

权利要求书

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

步骤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^*：

步骤3：对最优的机器人姿态X^*引入二进制加权因子ω_ij：

所述权重ω_ij＝Ψ(s_ij):R→{0,1}

其中：Ψ(s_ij)为将开关变量映射为权重因子的开关函数；

步骤4：对Ψ使用S形函数：

并将新的开关变量S＝{s_ij}引入到成本函数中：

步骤5：将s_ij的初始值设置为γ_ij，并将开关变量s_ij建模为正态分布的高斯变量：

s_ij～N(γ_ij,Ξ_ij)

其中：Ξ_ij为s_ij的切换先验约束的协方差矩阵；

基于以上，得出下式：

为导航控制的最优机器人姿态X^*。