【泡泡图灵智库】基于实时无迹卡尔曼滤波器的自主式水下航行器

泡泡图灵智库，带你精读机器人顶级会议文章

标题：A real-time unscented Kalman fifilter on manifolds for challenging AUV navigation

来源：IROS 2020

编译：万鑫

审核：管培育

这是泡泡图灵智库推送的第579篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

摘要

大家好，今天为大家带来的文章是A real-time unscented Kalman fifilter on manifolds for challenging AUV navigation

我们在深海探测任务的背景下，考虑了自主式水下航行器（AUV）的定位和导航问题。我们提出了一种基于无迹卡尔曼滤波器的解决方案用于车载导航。该方法使用精确的地球模型估算机器人的位置，姿态，速度。我们的算法能够快速的处理非线性问题，并且比在导航中广泛使用的扩展卡尔曼滤波器实施起来简单得多。这种无迹变换能够有效避免对雅可比阵的计算，并且非常适合在多传感器数据融合的情况下进行快速处理。真实的广义蒙特卡罗模拟证明了滤波器能够精确估计不确定性，并展现了其收敛能力。我们在 Marseille（法国马赛）900米深试验场通过真实实验验证了该方法的适用性。

贡献

1）开发基于UKF-M的完整惯性导航系统（INS），以实现工业深水AUV所需的高精度，一致性和快速性。

2）展示了精确的地球运动学模型与UKF-M结合用以确保航位估算的鲁棒性。  

3）使用ROS 2开发端到端导航仿真框架，以验证滤波器在仿真中的性能，并使用AUV平台收集实验数据。

4）提供了ROS 2代码，提供了与机载程序相同的UKF-M代码用以加快开发改进以及测试的效率。

算法流程

1.动机

扩展卡尔曼滤波器(EKF)已经有半个世纪的发展历史了，但若要将其部署在高精度惯性导航的框架下仍然需要专门知识，而这些知识却只存在于高端IMU的制造公司内部。我们认为基于无迹卡尔曼滤波器(UKF)的惯性导航系统能够克服这一障碍，并使其能够适用于欠缺设计经验的机器人系统上。实际上，相比较EKF方法（以及近期基于平滑处理的方法[2]），采用UKF可以使设计人员免于计算雅可比矩阵，并且能够避免出错以及提高通用性。

精确的导航模型

连续时间系统框架下状态演化方程[11]：

模型离散化

鉴于滤波器的实现，我们从上面的连续时间模型中推导出该系统的离散更新方程：

传感器误差的处理

上述模型中的许多不确定性源于IMU信号中存在噪声。然而，IMU传感器的噪声建模十分困难。因此，我们的框架中仅考虑后验噪声模型，详情参阅[12]，[13]。

我们对该系统中使用的传感器均做了类似的简化，例如全球定位系统(GPS)、Doppler Velocity Log (DVL)、深度传感器和位于底部基于声学的Ultra Short Base Line (USBL)定位系统。

无迹卡尔曼滤波算法

现在，我们介绍UKF-M算法以及其如何以无导数的方式处理模型的非线性特性。  UKF-M背后的主要思想是使用UKF方法[15]中的sigma-points来捕获模型中的非线性。这些sigma-points将统计模型进行线性化处理，代替了EKF使用的雅可比解析表达式。  UKF-M在每个采样时刻重复执行以下两个步骤：

（a）传播（通过动力学方程）

（b）更新（根据测量结果重新估计分布）

a.传播部分算法

b.更新部分算法

主要结果

1.仿真实验

图2：校准阶段的经度误差（m）。尽管是在校准过程，不确定性也得到了很好的估计，并且是稳定的。

图3：潜水阶段的经度误差（m）。估计过程中的不确定性可以很好被预测。

图4：在潜水阶段中积累了大的漂移之后的经度误差（m）。该图说明了航位推算过程中滤波器的收敛特性。

2.真实实验

图7：根据Schlum berger的AUV采集的实验数据估算的轨迹。 

如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。