[发明专利]一种基于滤波方法和曲线拟合方法相结合的协同导航方法

摘要

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

主权项

一种基于滤波方法和曲线拟合方法相结合的协同导航方法，其特征在于：（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，即覆盖滤波估计值，再进行新的量测更新。