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

说明书

本发明提出了基于水下环境特征的AUV鲁棒VBHIAKF‑SLAM导航方法，包括步骤：1、构建水下三维空间环境下的EKF‑SLAM系统模型；2、利用变分贝叶斯参数对量测噪声协方差进行估计；3、采用Huber鲁棒估计器对加权残差向量的每一个维度进行检测，并针对不同阈值范围内的维度赋予不同的权重值；4、多次迭代测量更新环节，根据迭代后的量测值对状态估计误差值进行逐步修正。该方法可以适用于量测噪声时变且量测信息中出现连续野值点时的环境，有效提高水下航行器的定位精度，改善对环境干扰的适用性和鲁棒性。

技术领域

本发明属于基于特征的水下SLAM导航技术领域，具体为基于水下环境特征的AUV鲁棒VBHIAKF-SLAM导航方法。

背景技术

高精度导航定位技术是水下航行器顺利完成航行探测及作业任务的重要保障。地球物理场导航方法因具有隐蔽性好、自主性强、误差不随时间累积等优点，近年来已成为水下导航领域的研究热点。但是，物理场辅助导航实现高精度导航定位的前提是获取高精度的信息丰富的先验环境信息图，而先验物理场图的获取在有些场景下是很难实现的。SLAM定位技术无需先验地图，仅通过自身携带的传感器不断感知外界环境信息便可实现对机器人自身定位及环境特征位置的实时更新。而标准EKF-SLAM算法的滤波精度依赖于精确的系统模型和准确的噪声统计先验知识，AUV在水下作业时，量测噪声的统计特性很难事先准确获取，即使先验统计已知，也很容易因强时变特性的内部和外部的不确定性而改变，从而导致其滤波精度的下降，严重时甚至可能会引起滤波的发散。同时，根据已有的文献，目前应用于AUV的基于特征的水下SLAM算法大多数是基于二维平面空间环境下的。

发明内容

为解决上述技术问题，本发明提出了基于水下环境特征的AUV鲁棒VBHIAKF-SLAM导航方法，该方法首先基于二维平面模型构建一种水下三维空间环境下的EKF-SLAM系统模型，并针对传统EKF-SLAM模型因噪声统计特性未知或时变且量测信息中出现连续野值点时滤波精度下降甚至发散的问题，提出一种鲁棒VBHIAKF-SLAM自适应滤波算法。

基于水下环境特征的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