一种基于五阶CKF的GPS/SINS/CNS组合导航方法技术

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

【技术实现步骤摘要】
一种基于五阶CKF的GPS/SINS/CNS组合导航方法
本专利技术涉及捷联惯性导航和组合导航
，尤其是一种基于五阶CKF的SINS/GPS/CNS组合导航方法。
技术介绍
在SINS/GPS/CNS组合导航系统中，GPS可随时提供精确的速度信息，因此经组合系统可将捷联惯导的速度误差控制在GPS的精度范围。CNS系统可提供精确的姿态角信息，经组合后可提高姿态角的测量精度，但由于客观条件的限制，只能间断提供航向信息，这就要求在无航向信息的时间间隔内，航向误差的估计必须稳定。基于上述原因，有必要在组合导航系统中引入一种更为先进的滤波算法，克服系统模型参数不准或随环境变化的影响，以提高系统模型对于外部环境的适应性，进而提供高精度的导航信息。因此，研究CKF滤波方法，进一步提高其滤波性能，并将其应用于SINS/GPS/CNS组合导航系统中的非线性滤波问题，无论是在理论上，还是在工程实践上均具有重要意义。
技术实现思路
专利技术目的：为克服现有技术中存在的不足，本专利技术提供了一种提高滤波精度和滤波效率的基于5阶CKF的GPS/SINS/CNS组合导航方法。技术方案：为解决上述技术问题，本专利技术提供了基于五阶CKF的GPS/SINS/CNS组合导航方法，其特征在于，包括如下步骤：步骤1：建立以SINS误差方程为基础的组合导航系统非线性误差方程和线性量测方程；步骤2：利用五阶球面径向容积规则准则构建五阶容积卡尔曼滤波器；步骤3：利用五阶CKF对SINS与GPS、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系下数学平台旋转角速度，为解算出的数学平台旋转角速度，为地球旋转角速度，为解算出的数学平台相对于地球的旋转角速度，为对应的计算误差。RM、RN分别为当地子午圈卯西圈曲率半径，为欧拉角微分方程系数矩阵的逆矩阵；为n′系与n系之间的姿态矩阵，Cω和具体为：步骤1.2：所述非线性误差方程和线性量测方程建立过程如下：选取速度误差欧拉角平台误差角φe、φn、φu；位置误差δL、δλ；b系下的加速度计常值误差以及陀螺常值误差以及构成12维状态向量，H近似为零：和P只取前两维状态，和为零均值白噪声过程；。该非线性状态方程可简记为：以采样周期T作为滤波周期，可以使用四阶龙格-库塔积分方法，以T为步长将其离散化，具体步骤如下：假设选取的状态向量在初始时刻的值x0已知，且记t1＝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速度误差Me、Mn分别为GPS接收机的测速误差项在东向北向坐标轴上的分量；速度量测量矢量定义如下：式中，Hv(t)＝[diag[1,1]02×10]在捷联工作模式下，由CNS输出的姿态信息可以得到载体的三轴姿态信息而SINS通过惯导解算也会给出载体的三轴姿态信息为因此将两者相减可得到载体的三轴姿态误差角Δφ：Δφ'＝M×Δφ式中M为姿态误差角转换矩阵：在组合导航系统中将Δφ'作为观测值建立系统的量测模型，通过最优估计的方法实时估计导航系统中惯性器件的误差，并以此对SINS进行修正；量测方程如下：式中，Hφ＝[03×2I3×303×7]由以上分析可知，全组合的量测方程为：同样以T作为滤波周期，并以T作为步长进行简单离散化。由状态方程和量测方程组成如下滤波方程：进一步地，所述步骤2)利用五阶球面径向容积规则准则构建五阶容积卡尔曼滤波器具体为：步骤2.1：五阶球面径向容积准则如下：f(x)为任意函数，Rn为积分区域，用近似的方法获得其解析值的近似解。取x＝ry且yTy＝1,r∈[0,∞)，因此上述积分在球面径向坐标系可写成：Un为n维单位球面，σ(·)为Un上的元素。积分I(f)可分离为Spherical积分和Radial积分：假设Spherica积分S(r)的Ns采样逼近：Radial积分Rr的Nr采样逼近：获得I(f)的Nr×Ns采样逼近为：Spherical准则：S(r)的计算公式如下：其中[s]n为[e]n的中点集，记为分别为：[e]n和[s]n内向量个数分别为2n和2n(n-1)，[e]i和[s]i分别[e]n和[s]n的第i列；解得：Radial规则：利用广义Gauss-Laguerre积分规则来求解该方程组步骤2.2：五阶容积卡尔曼滤波器的构建方法如下：对于建立的滤波方程：式中xk为12维状态向量，且各分量独立，服从高斯分布；zk为5维观测向量；ωk-1和νk分别为过程噪声和量测噪声，满足：式中，δij为δ函数；Q为系统噪声的方差阵；R为量测噪声的方差阵。五阶CKF算法如下：步骤2.2.1：时间更新：①假设k-1时刻的后验概率分布已知，Cholesky分解：Pk-1＝Sk-1Sk-1T①计算容积点：式中，i＝1,2,…,2n；j＝1,2,…,2n(n-1)①通过状态方程传播容积点：x1,i*＝f(x1,i)；x2,i*＝f(x2,i)x3,j*＝f(x3,j)；x4,j*＝f(x4,j)①估计k时刻状态及误差阵的预测值：K＝3-α；步骤2.2.2：量测更新①Cholesky分解Pxx：①计算容积点：①通过观测方程传播容积点：z1,i＝h(y1,i)，z2,i＝h(y2,i)z3,i＝h(y3,i)，z4,i＝h(y4,i)①估计k时刻的观测预测值：①计算自相关和互相关协方差阵：①利用Guass-Bayes滤波框架完成滤波：由于所述的量测方程为线性方程，所以简化量测更新：①Cholesky分解Pxx：①计算一步预测量测值①计算自相关和互相关协方差阵：①利用Guass-Bayes滤波框架完成滤波：进一步地，所述步骤3)利用五阶CKF对SINS与GPS、CNS输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息具体为：步骤3.1：当五阶CKF滤波器完成一次滤波后，根据滤波器输出的12维状态量本文档来自技高网...

【技术保护点】
一种基于五阶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系下数学平台旋转角速度，为解算出的数学平台旋转角速度，为地球旋转角速度，为解算出的数学平台相对于地球的旋转角速度，为对应的计算误差，RM、RN分别为当地子午圈卯西圈曲率半径，为欧拉角微分方程系数矩阵的逆矩阵；为n′系与n系之间的姿态矩阵，Cω和具体为：步骤1.2：所述非线性误差方程和线性量测方程建立过程如下：选取速度误差欧拉角平台误差角φe、φn、φu；位置误差δL、δλ；b系下的加速度计常值误差以及陀螺常值误差以及构成12维状态向量，H近似为零：和P只取前两维状态，和为零均值白噪声过程；该非线性状态方程可简记为：以采样周期T作为滤波周期，可以使用四阶龙格-库塔积分方法，以T为步长将其离散化，具体步骤如下：假设选取的状态向量在初始时刻的值x0已知，且记t1＝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速度误差Me、Mn分别为GPS接收机的测速误差项在东向北向坐标轴上的分量；速度量测量矢量定义如下：式中，Hv(t)＝[diag[1,1]02×10]在捷联工作模式下，由CNS输出的姿态信息可以得到载体的三轴姿态信息而SINS通过惯导解算也会给出载体的三轴姿态信息为因此将两者相减可得到载体的三轴姿态误差角Δφ：Δφ'＝M×Δφ式中M为姿态误差角转换矩阵：在组合导航系统中将Δφ'作为观测值建立系统的量测模型，通过最优估计的方法实时估计导航系统中惯性器件的误差，并以此对SINS进行修正；量测方程如下：式中，Hφ＝[03×2I3×303×7]由以上分析可知，全组合的量测方程为：同样以T作为滤波周期，并以T作为步长进行简单离散化，由状态方程和量测方程组成如下滤波方程：步骤2：利用五阶球面径向容积规则准则构建五阶容积卡尔曼滤波器；步骤3：利用五阶CKF对SINS与GPS、CNS输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息。2.如权利要求1所述的基于五阶CKF的GPS/SINS/CNS组合导...

【专利技术属性】
技术研发人员：徐晓苏，刘心雨，孙进，杨博，王捍兵，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32