一种多无人艇协同导航条件下陀螺漂移的滤波方法技术

本发明专利技术公开了一种多无人艇协同导航条件下陀螺漂移的滤波方法，该滤波方法为：装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，使用EKF算法对从艇的位置进行更新和修正，估计并补偿陀螺漂移。本发明专利技术采用扩展卡尔曼滤波方法对自身的航迹推位进行修正，估计并补偿MEMS陀螺初始航向偏差和漂移，提高了定位精度；为了提高可观测性，考虑两艘主艇交替向从艇发送测距信息，采用同一个状态先估计初始航向偏差，再彻底消除航向偏差并估计陀螺漂移，收到了很好滤波效果。

【技术实现步骤摘要】
一种多无人艇协同导航条件下陀螺漂移的滤波方法
本专利技术属于多无人艇协同导航定位
，尤其涉及一种多无人艇协同导航条件下陀螺漂移的滤波方法。
技术介绍
多无人艇的协同导航是利用系统中其他艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，装备低精度导航设备的艇可以提高自身的导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时，协同导航可以在一定程度上恢复这些平台的导航能力。因此研究无人水面艇的协同导航具有重要的理论价值和实际意义。水声通信可以实现对各跟随艇的定位和编队航行部署，其依赖性小。水声测距方法测量范围能够达到2km-6km，且测距精度在1m以内。对于载体的运动状态以及时统精度方面均没有苛刻的要求。因此，水声装置广播方式进行通信，采用水声设备辅以无线电时统的方法进行测距。但是，从艇的MEMS陀螺精度较低，难以同时估计初始航向偏差和陀螺漂移，使得滤波状态只有三维的问题。
技术实现思路
本专利技术实施例的目的在于提供一种多无人艇协同导航条件下陀螺漂移的滤波方法，旨在解决从艇的MEMS陀螺精度较低，难以同时估计初始航向偏差和陀螺漂移的难题，使得滤波状态只有三维的问题。本专利技术实施例是这样实现的，一种多无人艇协同导航条件下陀螺漂移的滤波方法，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：步骤一，装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号；步骤二，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；步骤三，估计初始航向偏差在采用卡尔曼滤波方法估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态Xk=[xkykX(3)]T，xk、yk表示从艇在k时刻的位置，先在t0秒内准确估计出初始航向偏差t0秒后转而估计陀螺漂移ε，即步骤四，补偿初始航向偏差在MEMS陀螺测得的航向中完全消除估计出的初始航向偏差，即式中表示MEMS陀螺测得的航向，在以后的航向测量值中不再包含初始航向偏差，只有陀螺漂移，在t0后，令X(3)=0，并重新设置系统噪声方差阵，为估计陀螺漂移做好准备；步骤五，估计并补偿陀螺漂移式中vk为从艇速度，t表示推位时间间隔，ε表示MEMS陀螺漂移，k表示算法执行次数，注意到项表明算法在状态一步预测中用陀螺漂移的估计值补偿了航向测量值的误差，表示成一般形式得：Xk+1=f(Xk，uk，t)+wk线性化后得，Xk+1=FkXk+Bkuk+wk式中，Xk=[xkykε]T，系统噪声wk～N(0，Qk)，步骤六，建立量测方程，式中，观测量Zk表示主从艇的距离r，xa、ya表示主艇的位置，xb、yb表示从艇位置，Vk～N(0，Rk)为测量噪声；步骤七，用扩展卡尔曼滤波修正从艇推位误差，Pk=(I-KkHk)Pk/k-1式中，为滤波输出的状态估计，为状态和及其方差一步预测，Kk为滤波增益。进一步，在步骤一中，两辆主艇上分别装有高精度惯导设备和水声通信模块，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒。进一步，在步骤二中，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，利用测量的速度和航向进行航迹推算，并利用主从艇的距离作为观测量修正位置信息并估计陀螺漂移。本专利技术提供的多无人艇协同导航条件下陀螺漂移的滤波方法，通过采用扩展卡尔曼滤波方法对自身的航迹推位进行修正，估计并补偿MEMS陀螺初始航向偏差和漂移，提高了定位精度；为了提高可观测性，考虑了两艘主艇交替向从艇发送测距信息，将航向偏差和陀螺漂移列为系统方程的两个状态难以用EKF同时估计和补偿，采用同一个状态先估计初始航向偏差，再彻底消除航向偏差并估计陀螺漂移，收到了很好的滤波效果。附图说明图1是本专利技术实施例提供的多无人艇协同导航条件下陀螺漂移的滤波方法流程图。具体实施方式为了使本专利技术的目的、技术方案及优点更加清楚明白，以下结合实施例，对本专利技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本专利技术，并不用于限定本专利技术。下面结合附图及具体实施例对本专利技术的应用原理作进一步描述。如图1所示，本专利技术实施例的多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：S101：装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号；S102：从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；S103：使用卡尔曼滤波方法对从艇的位置进行更新和修正，估计并补偿陀螺漂移。本专利技术的具体步骤为：方案为：两条主艇广播水声信号步骤一，系统安装与配置两辆主艇上分别装有高精度惯导设备和水声通信模块，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒，从艇上的多普勒测速仪给出速度大小，MEMS陀螺给出航向，从艇上的水声通信模块接收主艇发来的测距信息，利用艇上自带的电脑进行航迹推算和滤波，计算所在经纬度位置并估计陀螺漂移；步骤二，实验具体实施方法将所有相关设备安装在从艇上，读取MEMS陀螺输出航向作为静态数据并保存，一小时后，主艇设备安装完毕，开始实验，三艇在水面上行驶时保持三角队形，提高从艇测距时的可观测性，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒，从艇上的水声通信模块接收主艇发来的测距信息，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，利用测量的速度和航向进行航迹推算，并利用主从艇的距离作为观测量修正位置信息并估计陀螺漂移；步骤三，估计初始航向偏差在采用EKF估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态Xk=[xkykX(3)]T，xk、yk表示从艇在k时刻的位置，先在t0秒内准确估计出初始航向偏差t0秒后转而估计陀螺漂移ε，即步骤四，补偿初始航向偏差在MEMS陀螺测得的航向中完全消除估计出的初始航向偏差，即式中表示MEMS陀螺测得的航向，这样，在以后的航向测量值中不再包含初始航向偏差，只有陀螺漂移，在t0后，令X(3)=0，并重新设置系统噪声方差阵为合适的值，为估计陀螺漂移做好准备；步骤五，估计并补偿陀螺漂移式中vk为从艇速度，t表示推位时间间隔，ε表示MEMS陀螺漂移，k表示算法执行次数，注意到项表明算法在状态一步预测中用陀螺漂移的估计值补偿了航向测量值的误差，表示成一般形式得：Xk+1=f(Xk，u本文档来自技高网...

【技术保护点】
一种多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：步骤一，装有惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号；步骤二，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；步骤三，估计初始航向偏差：在采用卡尔曼滤波方法估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态Xk=[xk?yk?X(3)]T,xk、yk表示从艇在k时刻的位置，先在t0秒内准确估计出初始航向偏差t0秒后转而估计陀螺漂移ε，即步骤四，补偿初始航向偏差：在MEMS陀螺测得的航向中完全消除估计出的初始航向偏差，即式中表示MEMS陀螺测得的航向，在以后的航向测量值中不再包含初始航向偏差，只有陀螺漂移，在t0后，今X(3)=0，并重新设置系统噪声方差阵，为估计陀螺漂移做好准备；步骤五，估计并补偿陀螺漂移：式中vk为从艇速度，t表示推位时间间隔，ε表示MEMS陀螺漂移，k表示算法执行次数，注意到项表明算法在状态一步预测中用陀螺漂移的估 计值补偿了航向测量值的误差，表示成一般形式得：Xk+1=f(Xk，uk，t)+wk线性化后得，Xk+1=FkXk+Bkuk+wk式中，Xk=[xk?yk?ε]T，系统噪声wk～N(0，Qk)，步骤六，建立量测方程：Zk=r=h(Xk)=(xa-xb)2+(ya-yb)2+Vk=HX+Vk式中，观测量Zk表示主从艇的距离r，xa、ya表示主艇的位置，xb、yb表示从艇位置，H=∂h∂X=(xb-xa)/r(yb-ya)/r0,Vk～N(0，Rk)为测量噪声；步骤七，用扩展卡尔曼滤波修正从艇推位误差：X^k/k-1=FkX^k-1Pk/k-1=FkPk-1FkT+Bk-1Qk-1Bk-1TKk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)Pk=(I?KkHk)Pk/k?1式中，Pk=E[(Xk-X^k)(Xk-X^k)T],为滤波输出的状态估计，为状态和方差一步预测，Kk为滤波增益。FSA0000099483670000011.tif,FSA0000099483670000012.tif,FSA0000099483670000013.tif,FSA0000099483670000014.tif,FSA0000099483670000015.tif,FSA0000099483670000016.tif,FSA0000099483670000017.tif,FSA0000099483670000021.tif,FSA0000099483670000022.tif,FSA0000099483670000023.tif,FSA00000994836700000211.tif,FSA00000994836700000212.tif...

【技术特征摘要】
1.一种多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：步骤一，装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号；步骤二，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；步骤三、估计初始航向偏差在采用卡尔曼滤波方法估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态Xk＝[xkykX(3)]T，xk、yk表示从艇在k时刻的位置，先在t0秒内准确估计出初始航向偏差t0秒后转而估计陀螺漂移ε，即步骤四、补偿初始航向偏差在MEMS陀螺测得的航向中完全消除估计出的初始航向偏差，即式中表示MEMS陀螺测得的航向，在以后的航向测量值中不再包含初始航向偏差，只有陀螺漂移，在t0后，令X(3)＝0，并重新设置系统噪声方差阵，为估计陀螺漂移做好准备；步骤五、估计并补偿陀螺漂移式中vk为从艇速度，t表示推位时间间隔，ε表示MEMS陀螺漂移，n表示算法执行次数，注意到项表明算法在状态一步预测中用陀螺漂移的估计值补偿了航向测量值的误差，表示成一般形式得：Xk+1＝f(Xk，uk，t)+wk线性化后得，Xk+1＝FkXk+Bkuk+wk式中，Xk＝[xkykε]T，其中系统噪声...

【专利技术属性】
技术研发人员：徐博，金辰，刘杨，董海波，单为，邱立民，白金磊，易楚伟，张广拓，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：