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

摘要

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

主权项

一种基于滤波方法和曲线拟合方法相结合的协同导航方法，其特征在于：（1）建立从艇运动的系统方程和量测方程x_k、y_k表示从艇在k时刻的位置，v_k为从艇速度，t表示推位时间间隔，表示MEMS陀螺测得的航向，表示MEMS陀螺误差，用估计值修正航向测量值的偏差：X_k+1＝f(X_k,u_k,t)+w_k线性化后得：X_k+1＝F_kX_k+B_ku_k+w_k式中，系统噪声w_k～N(0,Q_k)；根据主从艇的距离建立量测方程：$Z_k=h\left(X_k\right)\sqrt{{(x_a-x_b)}^2+{(y_a-y_a)}^2+V_k=\mathrm{HX}+V_k}$观测量Z_k表示主从艇的距离，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置，$H=\frac{\partial h}{\partial X}=[(x_b-x_a)/r(y_b-y_a)/r0],$V_k～N(0,R_k)为测量噪声；（2）滤波估计从艇轨迹时间更新：$\begin{array}{c}{\hat{X}}_{k/k-1}=F_k{\hat{X}}_{k-1}\\ P_{k/k-1}=F_k{P}_{k-1}{F}_k^t=B_{k-1}{Q}_{k-1}{B}_{k-1}^T\end{array};$量测更新：$\begin{array}{c}{K}_k=P_{k/k-1}{H}_k^T{(H_k-P_{k/k-1}{H}_k^T+R_k)}^{-1}\\ {\hat{X}}_h={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\\ P_k=(I-K_k{H}_k)P_{k/k-1}\end{array};$式中，$P_k=E\left[(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right],$为滤波输出的状态估计，为状态和及其方差一步预测，K_k为滤波增益，滤波和曲线拟合可以同步并行运行，通过曲线拟合覆盖滤波估计值提高定位精度；（3）拟合数据保存在5秒时间内进行滤波，并保存每一个时刻的状态，将保存的滤波数据用多项式拟合方法拟合4个数据点，得到曲线的方程，并由曲线方程预测出5秒后从艇的位置，并将拟合数据保存；（4）选取拟合向量当每5秒进行量测更新时，从艇k时刻的位置坐标x_k、y_k都是时刻k的函数，把x_k、y_k分别分成p组，每组包含5个时刻的x_k、y_k，当组数p为偶数且k是5的整数倍时，拟合x_k和y_k关于时间k的多项式，设位置坐标x_k、y_k对应的拟合向量为u_l、v_l，令u_l＝x_k‑5+l,v_l＝y_k‑5+l,l＝1,2,3,4；（5）曲线拟合当从艇每5秒测量和主艇的距离进行量测更新，把向量u_l,v_l关于l＝1,2,3,4按照3阶多项式拟合方法拟合成多项式a(l),b(l，以下具体说明三阶多项式a(l)的拟合，设a(l)＝a+bl+cl²+dl³,l＝1,2,3,4，用最小二乘原理求三次拟合曲线的系数，拟合方差为：$S^2\text{=}\underset{l=1}{\overset{4}{\Sigma }}{(a\left(l\right)-u_l)}^2,$求a(l)的系数使拟合方差最小，令$\frac{{\partial S}^2}{\partial a}=\frac{{\partial S}^2}{\partial b}=\frac{{\partial S}^2}{\partial c}=\frac{{\partial S}^2}{\partial d}=0,$代入u_l(l＝1,2,3,4)四个点，得到四个方程分别解出a(l)的系数a,b,c,d；（6）利用拟合曲线预测下一个4秒从艇的位置每5秒进行量测更新，当时间k%5≠0时，令x_k＝a(5+k%5),y_k＝b(5+k%5)，利用拟合多项式a(l),b(l)预测5秒后的4秒内从艇的位置，并将预测值赋给x_k,y_k，即覆盖滤波估计值，再进行新的量测更新。