[发明专利]基于水下环境特征的AUV鲁棒VBHIAKF-SLAM导航方法

权利要求书

1.基于水下环境特征的AUV鲁棒VBHIAKF-SLAM导航方法，包括，其特征在于：

(1)构建水下三维空间环境下的EKF-SLAM系统模型；

步骤(1)的具体方法如下：

(1-1)预测阶段；

根据AUV的运动学特性，基于EKF-SLAM系统的状态方程表示为：

式中，为系统的状态向量，为预测阶段的载体状态，为地图向量，为载体在当前时刻相对于前一时刻的位姿变化量，V_v是时间不相关的过程噪声向量，其中，表示为：

式中，分别是AUV在全局坐标系下的位置和航向角；

若用代表第i个特征在全局坐标系下的位置，则地图向量表示为：

因此，状态方程被扩展为：

式中，分别为载体在当前时刻相对于前一时刻的位姿变化量；

(1-2)量测更新阶段

设当前时刻传感器测量到一个已经存储在地图中估计值为的特征点，则由坐标系间的变量关系可构建出其量测方程为：

若量测z与地图中的第i个特征点正确关联，则SLAM的Kalman滤波估计为：

S＝HP_a,k/k-1H^T+R

W＝P_a,k/k-1H^TS^-1

P_a,k＝P_a,k/k-1-WSW^T

其中，量测信息的雅可比矩阵H可以由下式获得：

在这里，

(1-3)状态扩充阶段

当某个特征被第一次观测到时，需要通过状态扩充的过程将其加入到地图中，使地图随载体航行区域的扩大实现自动扩展，其实现过程为：

首先将观测值z转换成全局直角坐标系下的特征点位置，其转换函数为：

因此，扩充后的状态函数由下式给出：

其中，其对应的协方差矩阵为：

式中，P_v、P_m、P_vm分别是载体的协方差矩阵，地图的协方差矩阵和载体与地图间的协方差矩阵，而雅可比矩阵表述为：

其中，雅可比矩阵和由下式求得：

故，协方差矩阵进一步表示为：

最终，通过上述公式将新观测到的特征点扩充到状态向量中，从而实现地图的扩充和构建；(2)利用变分贝叶斯参数对量测噪声方差进行估计；

步骤(2)的具体方法如下：

利用变分贝叶斯参数，估计量测噪声协方差矩阵为：

其中，μ和η是Inverse-Gamma概率密度分布参数，其具体表示为：

μ_k,i＝μ_k-1,i+0.5

式中，i＝1,…,d，d为量测向量的维数；

(3)采用Huber鲁棒估计器对加权残差向量的每一个维度进行检测，并针对不同阈值范围内的维度赋予不同的权重值；

步骤(3)的具体方法如下：

若假定：

则系统模型表述为：

m_k＝G_kX_k+ξ_k

根据广义极大似然估计方法进行求解，可以得到相应的迭代收敛解为：

式中，上标j表示迭代次数，迭代初始化

根据最终收敛的状态所对应的ψ值得相应的状态误差协方差矩阵为：

这里，Ψ为权重函数，通过如下公式求出：

式中，γ为调节因子，e_k,i表示第i维的归一化残差；

由于矩阵G_k特殊的结构形式，通过应用矩阵求逆引理，进一步将状态估计过程转换为更一般的形式；

首先将权值矩阵Ψ作分块化处理，用Ψ_x和Ψ_y分别代表状态预测残差和量测预测残差：

其中，0_m×n表示m×n维零值矩阵；

经过推导整理，预测协方差和量测协方差矩阵表示为：

(4)多次迭代测量更新环节，根据迭代后的量测值对估计误差逐步修正。

2.根据权利要求1所述的基于水下环境特征的AUV鲁棒VBHIAKF-SLAM导航方法，其特征在于：步骤(4)的具体方法如下：

由上述推导可得，系统状态的估计值和对应的协方差矩阵表示为：

式中，K_k为滤波增益，

迭代结束后的即为系统最终的状态估计值和协方差矩阵，这里，n＝1,2…N为迭代次数，设置为5。