水下机器人二维变速圆周运动的SBL定位EKF滤波仿真分析

        水下机器人在水下想要获得定位信息目前主要是基于声学方式的长基线(LBL)、短基线(SBL)、超短基线(USBL)获得局部定位数据，还有通过地磁场或者重力场匹配导航的方式，这类方式目前仅限于军事领域。当然还有最近热门的水下SLAM定位导航的方式，需要借助图像声呐或者光学摄像头获取周围环境的特征进行同时建图与导航，这种方式不需要借助外部的信息即可实现定位，是目前的研究热点，但此类方式需要进行大量的计算，同时要求周围的水域有明显的特征（也称为landmark）才能获取更加准确的定位结果，技术难度较大。本博客主要介绍基于短基线(SBL)定位的水下机器人扩展卡尔曼(EKF)滤波matlab仿真分析。SBL在各类港口、近海养殖场、河流、湖泊等浅水环境复杂的区域具有很好的定位性能，在融合了机器人自身的压力计、数字罗盘、加速度计这些信息后得到的比纯SBL解算结果更好的定位结果。

仿真环境简介

本文仿真了ROV在水下一定深度下水平二维变速圆周运动的情况，机器人的运动方程式如下：

                                     

则在大地坐标系下(G系)的速度和加速度表达式如下：

               

               

               

             

机器人的机头（机载坐标系X轴）沿着轨迹的切线方向运动，则在机载坐标系(V系)下的速度和加速度分别为：

                                 

                                 

                                 

                                

具体情况如下图所示：

则信标与水听器1、2、3的斜距表达式为：

                        

                        

                        

利用上述信息即可计算出仿真需要的机器人的深度、加速度、方位角以及斜距这些信息，再按照实际的需求添加一定方差的高斯白噪声，即可模拟出实际仪器的测量数据。

机器人的状态预测模型

本文选取机器人坐标(x,y,z)，机器人航向角（注意其与相差了90度，即），机器人载体坐标系下的速度，信标的坐标