基于最大后验估计水下运动漂移和噪声的SLAM方法技术

本发明专利技术公开了基于最大后验估计水下运动漂移和噪声的SLAM方法，包括：1）基于EKF

【技术实现步骤摘要】
基于最大后验估计水下运动漂移和噪声的SLAM方法


[0001]本专利技术涉及水下机器人
，特别涉及一种基于最大后验估计水下运动漂移和噪声的SLAM方法。

技术介绍

[0002]随着无人驾驶技术的发展，水下移动机器人实时定位和导航(Localization and Mapping)研究越来越受到重视。但是针对复杂的水下环境带来的挑战：(1)水下环境光的变化使得特征点提取不足，导致定位误差较大；(2)机器人运动受到水流干扰不稳定，导致水下运动漂移较大；(3)水下的动态噪声会造成较大噪声误差，导致观测数据误差较大。以上问题的存在制约着无人驾驶技术在水下的定位与导航精度，不能满足水下机器人自适应巡航的轨迹精度与安全性的要求。
[0003]2019年哈尔滨工业大学的白成超等人，提出了一种基于垂足点线特征结合的EKF
‑
SLAM机器人定位与建图方法(授权公告号：CN 110866927B)，将机器人坐标系下原点到直线的垂足点、世界坐标系下原点到直线的垂足点作为线段的地标特征点，并采用端点迭代方法对连续的点进行多线段拟合。该方法主要适用于空气环境，如果运用于水下环境中，则存在水下环境对机器人运动影响、每一帧迭代计算协方差矩阵都会累积增加水下环境的地标误差的问题。但该方法对水下移动机器人实时定位和导航研究提供了一个很好的思路。
[0004]针对在水下环境中机器人定位与建图误差过大的问题，2019年中国人民解放军战略支援部队信息工程大学的李万里等人提出一种水流速度估算方法、组合导航方法及装置(授权公告号：CN 110873813B)，依据组合导航系统和多普勒测速仪获取的速度来建立水流速度的系统和观测方程。该方法考虑水流对状态预测的误差，提高了导航结果的准确性；但是该方法利用卡尔曼滤波器计算水流速度，并不适用于水下非高斯白噪声影响较大的系统。2021年，东北电力大学的夏琳琳等人针对水下环境的定位与建图精度问题，提出一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法(申请公布号CN113739795A)，该方法利用偏振光测量水下潜航器的位姿信息，并利用自适应无迹卡尔曼滤波融合，通过调节偏振传感器的定向获取更精确的观测状态，从而提高导航的定位的精度。该方法是基于硬件方法进行的改进，一方面，采用偏振传感器等硬件设备增大了成本，另一方面，每帧的误差累积仍会导致较大的误差。
[0005]综上所述：现有的扩展卡尔曼滤波的同步定位与建图技术(Extended Kalman Filter Simultaneous Localization And Mapping，EKF
‑
SLAM)方法前提假设环境噪声服从均值为0的正态分布，但在机器人运动过程中，水下环境噪声具有不确定性。如果能够对未知的水下运动漂移和噪声进行估计，并加以补偿，则依据所构建环境地图，将能够实现水下机器人自定位精度的提高。

技术实现思路

[0006]本专利技术的目的是克服现有技术缺陷，提供基于最大后验估计水下运动漂移和噪声
的SLAM方法，通过对现有的EKF
‑
SLAM方法中的状态预测部分进行改进，结合系统模型中运动噪声和观测噪声的估计，对系统模型的高斯分布噪声方差进行自适应滤波，然后进行SLAM估计，来提高移动机器人定位估计和建图精度。
[0007]本专利技术的目的是这样实现的：一种基于最大后验估计水下运动漂移和噪声的SLAM方法，包括以下步骤：
[0008]步骤1)基于EKF
‑
SLAM估计水下机器人的运动漂移；
[0009]步骤1.1)对机器人SLAM系统状态初始化；
[0010]步骤1.2)构建基于EKF
‑
SLAM状态估计采样点；
[0011]步骤1.3)基于状态估计误差更新协方差矩阵；
[0012]步骤2)基于最大后验概率更新水下机器人的运动漂移；
[0013]步骤3)基于EKF
‑
SLAM估计水下噪声参数；
[0014]步骤3.1)进行水下噪声参数初始化；
[0015]步骤3.2)水下噪声参数协方差矩阵估计；
[0016]步骤3.3)基于状态估计更新协方差矩阵；
[0017]步骤4)基于最大后验概率更新水下噪声参数。
[0018]为了适用于水下复杂噪音场景，所述步骤1.2)具体包括：通过基于EKF的非线性卡尔曼滤波器实现基于方程构建的状态空间模型的参数估计，通过泰勒公式将非线性输入与输出方程线性化，对状态向量的均值与方差进行估计和优化。
[0019]在基于机器人位姿状态方程中加入水下噪声输入误差作为补偿，减小了复杂水下环境的噪声干扰导致的轨迹误差问题，改善了非线性系统下的卡尔曼滤波效果，所述步骤1.3)具体包括：依据步骤1.1)和步骤1.2)，水下机器人在采样点ζ
i,k
状态k
‑
1时刻对k时刻的状态估计值为：
[0020][0021]f为k时刻的非线性系统状态方程，f(ζ
i,k
,θ
i
)为k时刻的系统状态估计值，θ
i
为采样点i状态的水下不确定噪声参数；k时刻的漂移误差参数Δx
k
是k时刻实际系统状态矩阵与k
‑
1时刻对k时刻的状态估计值之差，记为：
[0022]水下机器人在采样点ζ
i,k
状态k
‑
1时刻对k时刻的状态的协方差矩阵为：
[0023][0024]其中θ
k|k
‑1为k
‑
1至k时刻的过程中的输入误差，记为：θ
k|k
‑1＝θ
k
‑1+e
k
‑1，θ
k
‑1为k
‑
1时刻的水下不确定噪声参数，e
k
‑1为k
‑
1时刻均值为0的高斯白噪声向量，依据公式(2)获得k
‑
1时刻对k时刻的状态的协方差矩阵，从而准确的得到状态估计的采样点；
[0025]基于k时刻估计的观测向量将公式(1)和(2)类比推导得公式(3)和(4)，计算观测估计量和协方差
[0026][0027][0028]h为k时刻的观测函数，h(ζ
i,k
,θ
k|k
‑1)为k时刻的系统观测量，ω
i,k|k
‑1为k
‑
1时刻对k时刻估计的权重，依据观测估计量的精度差动态赋值。
[0029]进一步的，所述步骤2)具体包括：
[0030]基于贝叶斯滤波理论，估计运动漂移参数Δx
k
是基于对后验概率密度的估计，此时的损失函数表示为：
[0031][0032]基于最大后验概率对公式(5)对参数θ
k|k
‑1求导，求解最精确的运动漂移参数Δx
k
，再校正k...

【技术保护点】

【技术特征摘要】
1.一种基于最大后验估计水下运动漂移和噪声的SLAM方法，其特征在于，包括以下步骤：步骤1)基于EKF
‑
SLAM估计水下机器人的运动漂移；步骤1.1)对机器人SLAM系统状态初始化；步骤1.2)构建基于EKF
‑
SLAM状态估计采样点；步骤1.3)基于状态估计误差更新协方差矩阵；步骤2)基于最大后验概率更新水下机器人的运动漂移；步骤3)基于EKF
‑
SLAM估计水下噪声参数；步骤3.1)进行水下噪声参数初始化；步骤3.2)水下噪声参数协方差矩阵估计；步骤3.3)基于状态估计更新协方差矩阵；步骤4)基于最大后验概率更新水下噪声参数。2.根据权利要求1所述的基于最大后验估计水下运动漂移和噪声的SLAM方法，其特征在于，所述步骤1.2)具体包括：通过基于EKF的非线性卡尔曼滤波器实现基于方程构建的状态空间模型的参数估计，通过泰勒公式将非线性输入与输出方程线性化，对状态向量的均值与方差进行估计和优化。3.根据权利要求1所述的基于最大后验估计水下运动漂移和噪声的SLAM方法，其特征在于，所述步骤1.3)具体包括：依据步骤1.1)和步骤1.2)，水下机器人在采样点ζ
i,k
状态k
‑
1时刻对k时刻的状态估计值为：f为k时刻的非线性系统状态方程，f(ζ
i,k
,θ
i
)为k时刻的系统状态估计值，θ
i
为采样点i状态的水下不确定噪声参数；k时刻的漂移误差参数Δx
k
是k时刻实际系统状态矩阵与k
‑
1时刻对k时刻的状态估计值之差，记为：水下机器人在采样点ζ
i,k
状态k
‑
1时刻对k时刻的状态的协方差矩阵为：其中θ
k|k
‑1为k
‑
1至k时刻的过程中的输入误差，记为：θ
k|k
‑1＝θ
k
‑1+...

【专利技术属性】
技术研发人员：孙进，汪和平，姜金，
申请(专利权)人：扬州大学，
类型：发明
国别省市：