一种多无人艇协同导航条件下陀螺漂移的滤波方法
【专利摘要】本发明公开了一种多无人艇协同导航条件下陀螺漂移的滤波方法，该滤波方法为：装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，使用EKF算法对从艇的位置进行更新和修正，估计并补偿陀螺漂移。本发明采用扩展卡尔曼滤波方法对自身的航迹推位进行修正，估计并补偿MEMS陀螺初始航向偏差和漂移，提高了定位精度；为了提高可观测性，考虑两艘主艇交替向从艇发送测距信息，采用同一个状态先估计初始航向偏差，再彻底消除航向偏差并估计陀螺漂移，收到了很好滤波效果。
【专利说明】一种多无人艇协同导航条件下陀螺漂移的滤波方法
【技术领域】
[0001]本发明属于多无人艇协同导航定位【技术领域】，尤其涉及一种多无人艇协同导航条件下陀螺漂移的滤波方法。
【背景技术】
[0002]多无人艇的协同导航是利用系统中其他艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，装备低精度导航设备的艇可以提高自身的导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时，协同导航可以在一定程度上恢复这些平台的导航能力。因此研究无人水面艇的协同导航具有重要的理论价值和实际意义。水声通信可以实现对各跟随艇的定位和编队航行部署，其依赖性小。水声测距方法测量范围能够达到2km-6km，且测距精度在Im以内。对于载体的运动状态以及时统精度方面均没有苛刻的要求。因此，水声装置广播方式进行通信，采用水声设备辅以无线电时统的方法进行测距。
[0003]但是，从艇的MEMS陀螺精度较低，难以同时估计初始航向偏差和陀螺漂移，使得滤波状态只有三维的问题。
【发明内容】
[0004]本发明实施例的目的在于提供一种多无人艇协同导航条件下陀螺漂移的滤波方法，旨在解决从艇的MEMS陀螺精度较低，难以同时估计初始航向偏差和陀螺漂移的难题，使得滤波状态只有三维的问题。
[0005]本发明实施例是这样实现的，一种多无人艇协同导航条件下陀螺漂移的滤波方法，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤:`[0006]步骤一，装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号;
[0007]步骤二，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；
[0008]步骤二，估计初始航向偏差
[0009]在采用卡尔曼滤波方法估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态Xk=[Xk ykX(3)]T, xk、yk表示从艇在k时刻的位置，先在h秒内准确估计出初始航向偏差Δφ，t0秒后转而估计陀螺漂移ε，即
[0010]
Δφ (? <?0)
^(3) = { Ψ
[0011]步骤四，补偿初始航向偏差
[0012]在MEMS陀螺测得的航向中完全消除估计出的初始航向偏差，即
[0013]
【权利要求】
1.一种多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤: 步骤一，装有惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号； 步骤二，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离； 步骤二，估计初始航向偏差: 在采用卡尔曼滤波方法估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态Xk=[xk yk X (3)]T, xk、yk表示从艇在k时刻的位置，先在h秒内准确估计出初始航向偏差Δφ，t0秒后转而估计陀螺漂移ε，即
2.如权利要求1所述的多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，在步骤一中，两辆主艇上分别装有惯导设备和水声通信模块，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒。
3.如权利要求1所述的多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，在步骤二中，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，利用测量的速度和航向进行航迹推算，并利用主从艇的距离作为观测量修正位置信息并估计陀螺漂移。
【文档编号】G01C21/20GK103697892SQ201310725949
【公开日】2014年4月2日 申请日期:2013年12月26日 优先权日:2013年12月26日
【发明者】徐博, 金辰, 刘杨, 董海波, 单为, 邱立民, 白金磊, 易楚伟, 张广拓 申请人:哈尔滨工程大学