一种估计初始航向偏差滤波的方法技术

技术编号:9906248 阅读:160 留言:0更新日期:2014-04-11 02:35
本发明专利技术公开了一种估计初始航向偏差滤波的方法,该估计初始航向偏差滤波的方法包括以下步骤:准备装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号;从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算,并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离;使用卡尔曼滤波算法对从艇的位置进行更新和修正,估计并补偿陀螺的初始航向偏差。本发明专利技术通过主艇无线电广播位置信息,从艇被动接受主艇发射的无线电信号,提高了扩展性,用两主艇和一从艇可以模拟多从艇的情形;从艇安装低精度惯导设备进行推位,并通过测量和主艇的距离利用主艇的高精度惯导设备,提高了定位精度,大大降低了设备的成本。

【技术实现步骤摘要】
【专利摘要】本专利技术公开了,该估计初始航向偏差滤波的方法包括以下步骤:准备装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号;从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算,并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离;使用卡尔曼滤波算法对从艇的位置进行更新和修正,估计并补偿陀螺的初始航向偏差。本专利技术通过主艇无线电广播位置信息,从艇被动接受主艇发射的无线电信号,提高了扩展性,用两主艇和一从艇可以模拟多从艇的情形;从艇安装低精度惯导设备进行推位,并通过测量和主艇的距离利用主艇的高精度惯导设备,提高了定位精度,大大降低了设备的成本。【专利说明】
本专利技术属于多无人艇协同导航定位
,尤其涉及。
技术介绍
多无人艇的协同导航是利用系统中其他艇的高精度导航信息,通过一定的信息交换,实现艇间导航资源的共享,装备低精度导航设备的艇可以提高自身的导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时,协同导航可以在一定程度上恢复这些平台的导航能力。因此研究无人水面艇的协同导航具有重要的理论价值和实际意义。水声通信可以实现对各跟随艇的定位和编队航行部署,其依赖性小。水声测距方法测量范围能够达到2km~6km,且测距精度在Im以内。对于载体的运动状态以及时统精度方面均没有苛刻的要求。因此,水声装置广播方式进行通信,采用水声设备辅以无线电时统的方法进行测距。从艇的惯导设备精度较低,通过测量和装备有高精度惯导设备的主艇的距离,采用扩展卡尔曼滤波方法对自身的航迹推位进行修正,估计并补偿初始航向偏差,提高定位精度。由于MEMS陀螺无法进行初始对准,初始航向偏差是随机的。为了提高可观测性,考虑两艘主艇交替向从艇发送测距信息。到目前为止,还没有将航向偏差作为状态并将主从艇距离作为观测量进行卡尔曼滤波的先例。
技术实现思路
本专利技术实施例的目的在于提供,旨在解决目前还没有将航向偏差作为状态并将主从艇距离作为观测量进行卡尔曼滤波的问题。本专利技术实施例是这样实现的,,该估计初始航向偏差滤波的方法包括以下步骤:步骤一、将所有相关设备安装在从艇上,读取MEMS陀螺输出航向作为静态数据并保存,一小时后,主艇设备安装完毕,开始实验,三艇在水面上行驶时保持三角队形,两主艇将水声通信信号和发送时间戳交替发送给从艇,间隔10秒,从艇上的水声通信模块接收主艇发来的测距信息,通过水声信号发送和接收的时间差乘以声速计算主从艇的距离,利用测量的速度和航向进行航迹推算,并利用主从艇的距离作为观测量修正位置信息并估计初始航向偏差;步骤二、建立系统方程【权利要求】1.,其特征在于,该估计初始航向偏差滤波的方法包括以下步骤: 步骤一、将所有相关设备安装在从艇上,读取MEMS陀螺输出航向作为静态数据并保存,一小时后,主艇设备安装完毕,三艇在水面上行驶时保持三角队形,两主艇将水声通信信号和发送时间戳交替发送给从艇,间隔10秒,从艇上的水声通信模块接收主艇发来的测距信息,通过水声信号发送和接收的时间差乘以声速计算主从艇的距离,利用测量的速度和航向进行航迹推算,并利用主从艇的距离作为观测量修正位置信息并估计初始航向偏差; 步骤二、建立系统方程: 2.如权利要求1所述的估计初始航向偏差滤波的方法,其特征在于,在步骤四中,由于GPS容易受到干扰,当收不到GPS信号或GPS信号出现错误时,只计算时间更新部分,即航迹推算,不计算量测更新部分,当收到正确的GPS信号后,再进行时间更新和量测更新,用主从艇的距离对从艇的位置进行更新修正。【文档编号】G01C21/20GK103712625SQ201310713137【公开日】2014年4月9日 申请日期:2013年12月23日 优先权日:2013年12月23日 【专利技术者】徐博, 金辰, 刘杨, 单为, 邱立民, 董海波, 白金磊, 陶冠时, 易楚伟 申请人:哈尔滨工程大学本文档来自技高网
...

【技术保护点】
一种估计初始航向偏差滤波的方法,其特征在于,该估计初始航向偏差滤波的方法包括以下步骤:步骤一、将所有相关设备安装在从艇上,读取MEMS陀螺输出航向作为静态数据并保存,一小时后,主艇设备安装完毕,三艇在水面上行驶时保持三角队形,两主艇将水声通信信号和发送时间戳交替发送给从艇,间隔10秒,从艇上的水声通信模块接收主艇发来的测距信息,通过水声信号发送和接收的时间差乘以声速计算主从艇的距离,利用测量的速度和航向进行航迹推算,并利用主从艇的距离作为观测量修正位置信息并估计初始航向偏差;步骤二、建立系统方程:式中,xk、yk表示从艇在k时刻的位置,vk为从艇速度,t表示推位时间间隔,表示MEMS陀螺测得的航向,表示初始航向偏差,在方程中估计值修正了航向测量值的偏差,表示成:Xk+1=f(Xk,uk,t)+wk线性化后得,Xk+1=FkXk+Bkuk+wk式中,系统噪声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)为测量噪声;步骤四、用扩展卡尔曼滤波修正从艇推位误差;(1)时间更新:X^k/k-1=FkX^k-1Pk/k-1=FkPk-1FkT+Bk-1Qk-1Bk-1T(2)量测更新:Kk=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],为滤波输出的状态估计,Pk/k?1为状态和及方差一步预测,Kk为滤波增益。FSA0000099317850000011.tif,FSA0000099317850000012.tif,FSA0000099317850000013.tif,FSA0000099317850000014.tif,FSA0000099317850000015.tif,FSA0000099317850000016.tif,FSA0000099317850000025.tif,FSA0000099317850000026.tif...

【技术特征摘要】

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

网友询问留言 已有0条评论
  • 还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。

1