CN104034328B - 一种基于滤波方法和曲线拟合方法相结合的协同导航方法 - Google Patents

一种基于滤波方法和曲线拟合方法相结合的协同导航方法 Download PDF

Info

Publication number
CN104034328B
CN104034328B CN201410216152.5A CN201410216152A CN104034328B CN 104034328 B CN104034328 B CN 104034328B CN 201410216152 A CN201410216152 A CN 201410216152A CN 104034328 B CN104034328 B CN 104034328B
Authority
CN
China
Prior art keywords
ship
fitting
filtering
curve
seconds
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201410216152.5A
Other languages
English (en)
Other versions
CN104034328A (zh
Inventor
徐博
金辰
刘杨
董海波
邱立民
贺浩
高伟
白金磊
单为
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201410216152.5A priority Critical patent/CN104034328B/zh
Publication of CN104034328A publication Critical patent/CN104034328A/zh
Application granted granted Critical
Publication of CN104034328B publication Critical patent/CN104034328B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

本发明属于应用曲线拟合方法的协同导航领域，具体为一种基于滤波方法和曲线拟合方法相结合的协同导航方法。本发明包括：建立从艇运动的系统方程和量测方程；滤波估计从艇轨迹；拟合数据保存；选取拟合向量；曲线拟合；利用拟合曲线预测下一个4秒从艇的位置。当从艇的轨迹近似为直线或机动性较弱时，由于MEMS陀螺误差较大且难以建立准确误差模型，这种方法相比滤波估计能提高定位精度。由于采用从艇当前位置之前的5个数据点进行拟合，充分利用了从艇的惯性，避免了使用之前全部数据点进行拟合多项式阶数高，难以准确估计从艇位置的缺点。

Description

一种基于滤波方法和曲线拟合方法相结合的协同导航方法
技术领域
本发明属于应用曲线拟合方法的协同导航领域，具体为一种基于滤波方法和曲线拟合方法相结合的协同导航方法。
背景技术
随着对海洋开发认识的加深，无人水面艇以其机动性强、成本低等优势成为研究热点。多无人艇的协同导航是利用系统中其他艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，装备低精度导航设备的艇可以提高自身的导航精度。
当从艇上装备的微机电系统(micro-electro-mechanical system,MEMS)陀螺误差较大且难以准确建模时，可以利用曲线拟合结合滤波方法估计从艇的位置。所谓曲线拟合是指设法找出某条光滑的曲线，使它尽可能接近所有数据点。其思想是使它能反映这些离散数据的变化趋势，使数据点的误差平方和最小。也就是用已知一组测定的数据去求得自变量和因变量的一个近似解析表达式，使误差的平方和最小，这就是常用的最小二乘法原理。由于人为或其他因素的干扰，主从艇的测距量测更新信息不能在每一个时刻都得到，因此在两次量测更新之间可以用曲线拟合的方法减小累积推位误差。此外，由于MEMS陀螺误差较大导致推位误差大从而影响滤波精度，所以采用曲线拟合和滤波组合的方法可以提高从艇直线航行时的定位精度。
发明内容
本发明的目的在于提供一种为在无人艇协同导航条件下，使用滤波与曲线拟合相结合的方法来估计从艇位置，从而提高定位精度的基于滤波方法和曲线拟合方法相结合的协同导航方法。
本发明的目的是这样实现的：
（1）建立从艇运动的系统方程和量测方程
xk、yk表示从艇在k时刻的位置，vk为从艇速度，t表示推位时间间隔，表示MEMS陀螺测得的航向，表示MEMS陀螺误差，用估计值修正航向测量值的偏差：
Xk+1=f(Xk,uk,t)+wk
线性化后得：
Xk+1=FkXk+Bkuk+wk
式中，
系统噪声wk～N(0,Qk)；
根据主从艇的距离建立量测方程：
观测量Zk表示主从艇的距离，xa、ya表示主艇的位置，xb、yb表示从艇位置，Vk～N(0,Rk)为测量噪声；
（2）滤波估计从艇轨迹
时间更新：
量测更新：
式中， 为滤波输出的状态估计，为状态和及其方差一步预测，Kk为滤波增益，滤波和曲线拟合可以同步并行运行，通过曲线拟合覆盖滤波估计值提高定位精度；
（3）拟合数据保存
在5秒时间内进行滤波，并保存每一个时刻的状态，将保存的滤波数据用多项式拟合方法拟合4个数据点，得到曲线的方程，并由曲线方程预测出5秒后从艇的位置，并将拟合数据保存；
（4）选取拟合向量
当每5秒进行量测更新时，从艇k时刻的位置坐标xk、yk都是时刻k的函数，把xk、yk分别分成p组，每组包含5个时刻的xk、yk，当组数p为偶数且k是5的整数倍时，拟合xk和yk关于时间k的多项式，设位置坐标xk、yk对应的拟合向量为ul、vl，令ul=xk-5+l,vl=yk-5+l,l=1,2,3,4；
（5）曲线拟合
当从艇每5秒测量和主艇的距离进行量测更新，把向量ul,vl关于l=1,2,3,4按照3阶多项式拟合方法拟合成多项式a(l),b(l)，以下具体说明三阶多项式a(l)的拟合，设a(l)=a+bl+cl2+dl3,l=1,2,3,4，用最小二乘原理求三次拟合曲线的系数，拟合方差为：
求a(l)的系数使拟合方差最小，令
代入ul(l=1,2,3,4)四个点，得到四个方程分别解出a(l)的系数a,b,c,d；
（6）利用拟合曲线预测下一个4秒从艇的位置
每5秒进行量测更新，当时间k%5≠0时，令xk=a(5+k%5),yk=b(5+k%5)，利用拟合多项式a(l),b(l)预测5秒后的4秒内从艇的位置，并将预测值赋给xk,yk，即覆盖滤波估计值，再进行新的量测更新。
本发明的有益效果在于：
当从艇的轨迹近似为直线或机动性较弱时，由于MEMS陀螺误差较大且难以建立准确误差模型，这种方法相比滤波估计能提高定位精度。由于采用从艇当前位置之前的5个数据点进行拟合，充分利用了从艇的惯性，避免了使用之前全部数据点进行拟合多项式阶数高，难以准确估计从艇位置的缺点。
附图说明
图1为每5秒进行量测更新拟合示意图。
图2为每5秒量测更新时算法流程图。
图3为曲线拟合与滤波位置误差比较仿真图。
图4为曲线拟合轨迹与真实轨迹比较仿真图。
图5为跟随艇轨迹比较图。
具体实施方式
下面结合附图对本发明做进一步描述。
步骤一、建立从艇运动的系统方程和量测方程
式中，xk、yk表示从艇在k时刻的位置，vk为从艇速度，t表示推位时间间隔。表示MEMS陀螺测得的航向，表示MEMS陀螺误差，在方程中用其估计值修正了航向测量值的偏差。表示成一般形式得：
Xk+1=f(Xk,uk,t)+wk (2)
线性化后得：
Xk+1=FkXk+Bkuk+wk (3)
式中 系统噪声wk～N(0,Qk)。
根据主从艇的距离建立量测方程如下：
式中，观测量Zk表示主从艇的距离，xa、ya表示主艇的位置，xb、yb表示从艇位置，Vk～N(0,Rk)为测量噪声。
步骤二、滤波估计从艇轨迹
(1)时间更新：
(2)量测更新：
式中， 为滤波输出的状态估计，为状态和及其方差一步预测，Kk为滤波增益。在从艇航行过程中滤波和曲线拟合可以同步并行运行，通过曲线拟合覆盖相应滤波估计值可以提高定位精度。
步骤三、拟合数据保存
在5秒时间内进行滤波，并保存每一个时刻的状态。将保存的滤波数据用多项式拟合方法拟合4个数据点，得到曲线的方程，并由曲线方程预测出5秒后从艇的位置，并将拟合数据保存。
步骤四、选取拟合向量
当每5秒进行量测更新时，假设从艇k时刻的位置坐标xk、yk都是时刻k的函数。把xk、yk分别分成p组，每组包含5个时刻的xk、yk。当组数p为偶数且k是5的整数倍时，拟合xk和yk关于时间k的多项式。设位置坐标xk、yk对应的拟合向量为ul、vl。令ul=xk-5+l,vl=yk-5+l,l=1,2,3,4。由于量测更新点相比前4个点差别较大，拟合时采用量测更新点会使得拟合曲线产生较大偏差甚至发散，所以拟合时不采用量测更新点。虽然不采用量测更新点，但拟合使用的4个数据点是在上一次量测更新的基础上一步预测得到的，所以仍能保证算法的精度。
步骤五、曲线拟合
当从艇每5秒测量和主艇的距离进行量测更新时，把向量ul,vl分别关于l=1,2,3,4按照3阶多项式拟合方法拟合成多项式a(l),b(l)，它们是关于变量l的函数（如图1所示）。曲线拟合操作不影响量测更新的进行。
以下具体说明三阶多项式a(l)的拟合。设a(l)=a+bl+cl2+dl3,l=1,2,3,4。用最小二乘原理求三次拟合曲线的系数。拟合方差为：
求a(l)的系数使拟合方差最小。即令
将ul(l=1,2,3,4)四个点代入上式，得到四个方程分别解出a(l)的系数a,b,c,d。
步骤六、利用拟合曲线预测下一个4秒从艇的位置
假设每5秒进行量测更新，当时间k%5≠0时(%表示整除取余)，令xk=a(5+k%5),yk=b(5+k%5)，利用拟合多项式a(l),b(l)预测5秒后的4秒内从艇的位置，并将预测值赋给xk,yk，即覆盖滤波估计值。之后再进行新的量测更新，算法流程图如图2所示。这样曲线预测也不影响量测更新的进行。当每秒进行量测更新时操作同上。
从图3可以看出，由于仿真时从艇轨迹是直线，曲线拟合(实线)相比单纯滤波（星号线）误差要小。从图4可以看出，在从艇直线航行的条件下，曲线拟合和滤波组合得到的轨迹（虚线）相比单纯滤波（星号线）来说，与真实轨迹（实线）更为接近。进而说明曲线拟合解决了两次量测更新之间使用MEMS陀螺推位误差大的问题，而且不影响量测更新的进行，和传统滤波估计方法完美地结合在一起，对定位精度的提高起了重要作用。

Claims (1)

1.一种基于滤波方法和曲线拟合方法相结合的协同导航方法，其特征在于：
（1）建立从艇运动的系统方程和量测方程
xk、yk表示从艇在k时刻的位置，vk为从艇速度，t表示推位时间间隔，表示MEMS陀螺测得的航向，表示MEMS陀螺误差，用估计值修正航向测量值的偏差：
Xk+1＝f(Xk,uk,t)+wk
线性化后得：
Xk+1＝FkXk+Bkuk+wk
式中，
系统噪声wk～N(0,Qk)；
根据主从艇的距离建立量测方程：
观测量Zk表示主从艇的距离，xa、ya表示主艇的位置，xb、yb表示从艇位置，Vk～N(0,Rk)为测量噪声；
（2）滤波估计从艇轨迹
时间更新：
量测更新：
式中， 为滤波输出的状态估计，为状态和及其方差一步预测，Kk为滤波增益，滤波和曲线拟合可以同步并行运行，通过曲线拟合覆盖滤波估计值提高定位精度；
（3）拟合数据保存
在5秒时间内进行滤波，并保存每一个时刻的状态，将保存的滤波数据用多项式拟合方法拟合4个数据点，得到曲线的方程，并由曲线方程预测出5秒后从艇的位置，并将拟合数据保存；
（4）选取拟合向量
当每5秒进行量测更新时，从艇k时刻的位置坐标xk、yk都是时刻k的函数，把xk、yk分别分成p组，每组包含5个时刻的xk、yk，当组数p为偶数且k是5的整数倍时，拟合xk和yk关于时间k的多项式，设位置坐标xk、yk对应的拟合向量为ul、vl，令ul＝xk-5+l,vl＝yk-5+l,l＝1,2,3,4；
（5）曲线拟合
当从艇每5秒测量和主艇的距离进行量测更新，把向量ul,vl关于l＝1,2,3,4按照3阶多项式拟合方法拟合成多项式a(l),b(l)，以下具体说明三阶多项式a(l)的拟合，设a(l)＝a+bl+cl2+dl3,l＝1,2,3,4，用最小二乘原理求三次拟合曲线的系数，拟合方差为：
求a(l)的系数使拟合方差最小，令
代入ul(l＝1,2,3,4)四个点，得到四个方程分别解出a(l)的系数a,b,c,d；
（6）利用拟合曲线预测下一个4秒从艇的位置
每5秒进行量测更新，当时间k%5≠0时，令xk＝a(5+k%5),yk＝b(5+k%5)，利用拟合多项式a(l),b(l)预测5秒后的4秒内从艇的位置，并将预测值赋给xk,yk，即覆盖滤波估计值，再进行新的量测更新。
CN201410216152.5A 2014-05-21 2014-05-21 一种基于滤波方法和曲线拟合方法相结合的协同导航方法 Active CN104034328B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410216152.5A CN104034328B (zh) 2014-05-21 2014-05-21 一种基于滤波方法和曲线拟合方法相结合的协同导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410216152.5A CN104034328B (zh) 2014-05-21 2014-05-21 一种基于滤波方法和曲线拟合方法相结合的协同导航方法

Publications (2)

Publication Number Publication Date
CN104034328A CN104034328A (zh) 2014-09-10
CN104034328B true CN104034328B (zh) 2017-03-29

Family

ID=51465193

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410216152.5A Active CN104034328B (zh) 2014-05-21 2014-05-21 一种基于滤波方法和曲线拟合方法相结合的协同导航方法

Country Status (1)

Country Link
CN (1) CN104034328B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104866705B (zh) * 2015-03-26 2017-08-08 休斯网络技术有限公司 机载导航数据的截断式卡尔曼滤波方法
CN107300710A (zh) * 2016-04-15 2017-10-27 大唐半导体设计有限公司 一种实现定位的方法及装置
CN111950123A (zh) * 2020-07-08 2020-11-17 北京航天控制仪器研究所 一种陀螺仪误差系数曲线拟合预测方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103292813A (zh) * 2013-05-24 2013-09-11 哈尔滨工程大学 一种提高水面艇编队导航精度的信息滤波方法
CN103322999A (zh) * 2013-05-24 2013-09-25 哈尔滨工程大学 一种适用于多艇导航的保留历史状态的信息滤波算法
CN103335646A (zh) * 2013-06-20 2013-10-02 哈尔滨工程大学 一种基于分散式增广信息滤波的多艇协同导航方法
CN103398712A (zh) * 2013-08-02 2013-11-20 中国人民解放军63983部队 交替领航的协同导航方法
CN103697892A (zh) * 2013-12-26 2014-04-02 哈尔滨工程大学 一种多无人艇协同导航条件下陀螺漂移的滤波方法
CN103712625A (zh) * 2013-12-23 2014-04-09 哈尔滨工程大学 一种估计初始航向偏差滤波的方法
CN103759730A (zh) * 2014-01-16 2014-04-30 南京师范大学 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103292813A (zh) * 2013-05-24 2013-09-11 哈尔滨工程大学 一种提高水面艇编队导航精度的信息滤波方法
CN103322999A (zh) * 2013-05-24 2013-09-25 哈尔滨工程大学 一种适用于多艇导航的保留历史状态的信息滤波算法
CN103335646A (zh) * 2013-06-20 2013-10-02 哈尔滨工程大学 一种基于分散式增广信息滤波的多艇协同导航方法
CN103398712A (zh) * 2013-08-02 2013-11-20 中国人民解放军63983部队 交替领航的协同导航方法
CN103712625A (zh) * 2013-12-23 2014-04-09 哈尔滨工程大学 一种估计初始航向偏差滤波的方法
CN103697892A (zh) * 2013-12-26 2014-04-02 哈尔滨工程大学 一种多无人艇协同导航条件下陀螺漂移的滤波方法
CN103759730A (zh) * 2014-01-16 2014-04-30 南京师范大学 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于微惯性网络的多水面无人艇协同导航定位技术研究;徐博等;《惯性技术发展动态发展方向研讨会》;20111231;191-195 *
基于水声通信延迟的多UUV协同定位算法;高伟等;《系统工程与电子技术》;20140331;第36卷(第3期);539-545 *
考虑通信延迟的多水面无人艇协同定位算法;高伟等;《哈尔滨工程大学学报》;20131231;第34卷(第12期);1490-1496 *

Also Published As

Publication number Publication date
CN104034328A (zh) 2014-09-10

Similar Documents

Publication Publication Date Title
CN104809326B (zh) 一种异步传感器空间配准算法
CN108362281B (zh) 一种长基线水下潜艇匹配导航方法及系统
CN103389506B (zh) 一种用于捷联惯性/北斗卫星组合导航系统的自适应滤波方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN107643082B (zh) 基于多波束的多路径并行iccp水下地形匹配方法
CN109459019A (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN103389094B (zh) 一种改进的粒子滤波方法
Merckelbach et al. Current measurements from autonomous underwater gliders
CN105953799B (zh) 基于熵值法的重力场适配区内水下运载体航迹规划方法
CN107797125B (zh) 一种减小深海探测型auv导航定位误差的方法
CN104034328B (zh) 一种基于滤波方法和曲线拟合方法相结合的协同导航方法
CN104807479A (zh) 一种基于主惯导姿态变化量辅助的惯导对准性能评估方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN107884800A (zh) 观测时滞系统的组合导航数据解算方法、装置及导航设备
CN106525042A (zh) 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法
CN103292812A (zh) 一种微惯性sins/gps组合导航系统的自适应滤波方法
CN107193023A (zh) 一种具有闭式解的高精度北斗卫星系统单点定位方法
CN104280024B (zh) 一种深水机器人组合导航装置和方法
KR100868849B1 (ko) 선박의 속력 시운전 최적코스 선정 방법
CN105737850B (zh) 基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法
CN108121890A (zh) 一种基于线性卡尔曼滤波的航姿信息融合方法
Liu et al. Improved ICCP algorithm and its application in gravity matching aided inertial navigation system
CN103487056A (zh) 一种基于人工蜂群算法和平均Hausdorff距离的重力匹配方法
CN104949687A (zh) 一种长时间导航系统全参数精度评估方法
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant