[发明专利]一种基于五阶CKF的GPS/SINS/CNS组合导航方法

摘要

本发明公开一种基于五阶CKF的GPS/SINS/CNS组合导航方法，其特征在于，包括如下步骤：步骤1：建立以SINS误差方程为基础的组合导航系统非线性误差方程和线性量测方程；步骤2：利用五阶球面径向容积规则准则构建五阶容积卡尔曼滤波器；步骤3：利用五阶CKF对SINS与GPS、CNS输出的信息进行滤波、融合，得到导航参数的最优估计。

主权项

1.一种基于五阶CKF的GPS/SINS/CNS组合导航方法，其特征在于，包括如下步骤：步骤1：建立以SINS误差方程为基础的组合导航系统非线性误差方程和线性量测方程；步骤1.1：选取“东北天”地理坐标系为导航坐标系n系；选取载体“右前上”坐标系为载体坐标系b系；n系先后经过3次欧拉角转动至b系，三个欧拉角记为航向角ψ∈(‑π,π]，纵摇角横摇角γ∈(‑π,π]；n系与b系之间的姿态矩阵记为真实姿态角记为真实速度记为真实地理坐标系为P＝[L λ H]^T，其中，L为纬度，λ为经度，H为高度；SINS解算出的姿态角记为速度记为地理坐标系为SINS解算出的数学平台记为n′系，n′系与b系之间的姿态矩阵记为记姿态角误差为速度误差为：位置误差为：则非线性误差模型如下：其中，为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差为零均值白噪声过程，为b系下三轴加速度常值误差，为n系下三轴加速度的随机误差为零均值白噪声过程，为加速度计的实际输出，是b系下数学平台旋转角速度，为解算出的数学平台旋转角速度，为地球旋转角速度，为解算出的数学平台相对于地球的旋转角速度，为对应的计算误差，R_M、R_N分别为当地子午圈卯西圈曲率半径，为欧拉角微分方程系数矩阵的逆矩阵；为n′系与n系之间的姿态矩阵，C_ω和具体为：步骤1.2：所述非线性误差方程和线性量测方程建立过程如下：选取速度误差欧拉角平台误差角φ_e、φ_n、φ_u；位置误差δL、δλ；b系下的加速度计常值误差以及陀螺常值误差以及构成12维状态向量，H近似为零：和P只取前两维状态，和为零均值白噪声过程；该非线性状态方程可简记为：以采样周期T作为滤波周期，可以使用四阶龙格‑库塔积分方法，以T为步长将其离散化，具体步骤如下：假设选取的状态向量在初始时刻的值x₀已知，且记t₁＝t+T/2，则可以通过以下迭代公式将离散：Δx1＝[f(x,t)+ω(t)]TΔx2＝[f(x,t1)+Δx1/2+ω(t1)]TΔx3＝[f(x,t1)+Δx2/2+ω(t1)]TΔx4＝[f(x,t+T)+Δx3+ω(t+T)]Txk+1＝xk+(Δx1+2Δx2+2Δx3+Δx4)/6记离散后状态滤波方程为：xk＝f(xk‑1)+ωk‑1线性量测方程建立过程如下：选取GPS速度误差M_e、M_n分别为GPS接收机的测速误差项在东向北向坐标轴上的分量；速度量测量矢量定义如下：式中，Hv(t)＝[diag[1,1]02×10]在捷联工作模式下，由CNS输出的姿态信息可以得到载体的三轴姿态信息而SINS通过惯导解算也会给出载体的三轴姿态信息为因此将两者相减可得到载体的三轴姿态误差角Δφ：Δφ'＝M×Δφ式中M为姿态误差角转换矩阵：在组合导航系统中将Δφ'作为观测值建立系统的量测模型，通过最优估计的方法实时估计导航系统中惯性器件的误差，并以此对SINS进行修正；量测方程如下：式中，Hφ＝[03×2 I3×3 03×7]由以上分析可知，全组合的量测方程为：同样以T作为滤波周期，并以T作为步长进行简单离散化，由状态方程和量测方程组成如下滤波方程：步骤2：利用五阶球面径向容积规则准则构建五阶容积卡尔曼滤波器；步骤3：利用五阶CKF对SINS与GPS、CNS输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息。