CN108415445B - 一种基于开关约束的水下航行器定位鲁棒优化方法 - Google Patents

Abstract

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

Description

一种基于开关约束的水下航行器定位鲁棒优化方法

技术领域

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

背景技术

水下航行器由于没有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^*：

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

有益效果

本发明提出的一种基于开关约束的水下航行器定位鲁棒优化方法，针对传统水下航行器导航控制方法未能对错误闭环实现鲁棒性的缺点，基于General GraphOptimization(g2o)和开关约束的导航控制方法，采用一种开关约束后端优化方法，利用节点因子图，设计了一种消除地图构建过程中产生的错误闭环的方法。

本发明解决现有水下航行器在同步定位与地图构建过程中产生错误闭环而导致其所构建地图出现错误的局限，使得航行器在地图构建过程中对错误闭环进行处理并加权为零，并且添加开关先验约束，以消除错误闭环带来的不利影响。理论证明了该发明的鲁棒性，实验验证了该发明的有效性，对减小水下航行器地图构建错误率有良好的效果。

附图说明

图1：SLAM过程中产生的闭环因子图；

图2：航行器实际路径；

图3：路径优化结果；

具体实施方式

现结合实施例、附图对本发明作进一步描述：

首先进行水下环境布置。在水下布置五个标记点并将其中任意两个设置为相同点。

确定水下航行器初始位置(x,z)＝(0,0)；航行器速度v＝1m/s，内部里程计的精度为方差为1的高斯白噪声，以预设附图2路径航行。

步骤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：将节点概率分布分解成：

其中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：引入二进制加权因子ω_ij

其中ω_ij∈{0,1}

并为每个权重ω_ij引入一个连续变量s_ij∈R，即x_i和x_j之间的闭环约束的开关变量

ω_ij＝Ψ(s_ij):R→{0,1} (5)

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

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

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

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

s_ij～N(γ_ij,Ξ_ij) (8)

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

基于以上，得出

水下航行器在本发明的导航控制方法下允许优化器改变由前端进行的数据关联。因此，它可以消除一些环路闭合约束的影响，从而即使存在异常值的情况下也能收敛到正确的解。最后在步骤(5)获得的控制方法的作用下对航行器进行实验分析，在存在相似区域的水下环境中进行实验，完成水下航行器导航控制方法验证。

实验中对水下航行器进行10次数据采集，对所得出的因子图，在g2o的基础上，通过本发明方法处理后进行结果分析。航行器根据本发明设计的控制律式(9)，达到所需的消除错误的闭环约束效果。实验表明，该发明对处理水下航行器地图构建错误闭环具有良好的效果。

Claims (1)

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^*。

Images