技术领域
[0001] 本
发明涉及一种新型的捷联惯性
导航系统(SINS)和全球
定位系统(GPS)的组合导航方法。此部分属于航空应用领域中无人机自驾仪上的导航计算模
块，可应用于各种
无人机导航系统中。
背景技术
[0002] 捷联惯导系统(SINS)能够连续实时地提供
位置、速度、
姿态等导航信息，且不受任何外界信息的干扰，是一种完全自主式的导航系统。但导航误差随时间积累的缺点使捷联惯导系统不能够单独完成长时间高
精度的导航任务，需要其他辅助系统从旁协助。全球定位系统(GPS)则具有定位精度高，导航误差不随时间积累等优点，但是数据更新
频率低、易受外界环境的干扰。因而SINS/GPS组合导航系统应运而生，目前已广泛应用于军事民用等的航空领域中。
[0003] 组合导航系统中，实际系统总是存在不同程度的非线性，虽然有些系统可以近似看作线性系统，但是大部分系统难以用线性微分或差分方程描述。此外，实际系统中通常还存在高斯或非高斯随机噪声干扰。非线性随机动态系统广泛存在于工程实践，如飞机和舰船的惯性导航系统、组合导航系统等都属于这类系统。在这类非线性随机动态系统中，如何进行有效甚至最优状态估计尤为重要，这就需要采用非线性滤波技术。然而，每种非线性滤波方法都有其各自的局限性，因此仅依靠滤波技术在恶劣的飞行条件下不能得到准确的导航参数估计。
[0004] 对于惯性导航系统，建立准确的误差方程是采用滤波技术进行对准和状态误差估计的
基础，若系统模型存在误差，则会影响滤波精度，甚至使滤波发散。通常采用的线性误差方程都是在假定平台失准
角(即姿态误差角)为小量的条件下导出的，当失准角较大时，系统误差呈现较严重的非线性特性，这些线性方程已不能准确描述捷联惯导系统的误差传播特性，如果继续使用可能致使滤波误差较大甚至发散，因此，需要寻找更为精确的非线性误差方程来代替。
发明内容
[0005] 在方位误差角为大失准角时，组合导航系统的线性模型已经无法描述实际系统的特性，不能满足导航精度的要求。
扩展卡尔曼滤波在运用过程中往往采用对非线性模型进行泰勒级数展开近似真实系统，这样做不可避免地会产生截断误差，加之计算过程中积分误差的积累，很容易造成扩展卡尔曼滤波的不稳定甚至发散。基于以上考虑，本发明首先在方位误差角为大失准角的情况下推导了SINS/GPS组合导航系统的非线性模型，然后应用PI
控制器和扩展卡尔曼滤波相结合的方法进行数据融合，得到组合导航参数的最优估计。
[0006] 本发明的目的是通过下述技术方案实现的。
[0007] 步骤一、使用PI(比例积分)控制器补偿角速率ω。
[0009] 第一种情况：
[0010] 飞行器的航向角误差为：
[0011] yawerror＝yaw_SINS×yaw_GPS
[0012] 其中，yaw_SINS＝[C11 C21 0]T；yaw_GPS＝[cog_x cog_y0]T，cog为GPS给出的航向角。
[0013] 重
力加速度：g＝acc-ω×V_GPS
[0014] 其中，VGPS为GPS给出的飞行器速度；ω为角速率，acc为加速度。
[0016] rollpitcherror＝g_SINS×g
[0017] 其中，g_SINS＝[C31 C32 C33]T
[0018] 总误差：totalerror＝ωYyawerror+ωRProllpitcherror
[0019] 其中，ωY和ωRP分别为航向角误差，横滚俯仰角误差的权重系数。
[0020] 第二种情况：
[0021] 当GPS数据没有给出的时刻，yawerror＝0
[0022] 则总误差totalerror＝ωRProllpitcherror
[0023] 比例(P)补偿：ωProportion＝KPtotalerror
[0024] 积分(I)补偿：ωIntegral＝ωIntegral+KItotalerrorΔt
[0025] 其中，KP为比例系数；KI为积分系数；Δt为
采样间隔。
[0026] 对角速率ω进行PI补偿，得到补偿后的角速率为
[0027] ωcorrection＝ωProportion+ωIntegral
[0028] 将补偿后的角速率ωcorrection作为SINS的输入进行运算，得到姿态角(航向角ψ，俯仰角θ和横滚角γ)，速度(东向Vx，北向Vt，天向Vz)，位置(纬度L，经度λ，高度h)。
[0029] 步骤二、在方位误差角为大失准角时，推导出姿态非线性误差方程、速度非线性误差方程以及位置误差方程。从中选取状态变量，建立组合导航系统的非线性误差模型。
[0030] 姿态非线性误差方程为：
[0031]
[0032]
[0033]
[0034]
[0035]
[0036]
[0037] 速度非线性误差方程为：
[0038]
[0039]
[0040]
[0041]
[0042]
[0043]
[0044] 位置误差方程为：
[0045]
[0046]
[0047]
[0049]
[0050] 其中， 为位置速率， 为地球自转速率；φx，φy，φz为东向，北向，方位失准角；Vx，Vy，Vz为东向，北向，天向的速度；δVx，δVy，δVz为东向、北向、天向速度误差；L，λ，h为纬度，经度，高度；δL，δλ，δh为纬度、经度、高度误差；fx，fy，fz为东向，北向，天向的比力； 为东向，北向，天向的陀螺常值漂移；
为东向，北向，天向的加速度计零偏；Rm为地球子午圈上的
曲率半径，Rn为地球卯酉圈上的
曲率半径；陀螺常值漂移为 加速度计零偏为
[0051] 从以上的误差方程中，选取以下15个状态变量：
[0052]
[0053] 建立连续多阶的
状态方程和观测方程，形式如下：
[0054]
[0055] Z(t)＝HX(t)+V(t)
[0056] 以上即为SINS/GPS组合导航系统的非线性误差模型。
[0057] 其中，f(·)为X(t)的非线性函数；H(t)为
观测矩阵；W(t)为过程噪声序列；V(t)为观测噪声序列。Z(t)的形式如下：
[0058]
[0059] Vx_GPS，Vy_GPS，Vz_GPS为GPS给出的东向，北向，天向的速度；L_GPS，λ_GPS，h_GPS为GPS给出的纬度，经度，高度信息。
[0060] 步骤三、使用二阶泰勒级数展开对步骤二得到的非线性误差模型进行线性化并离散化。
[0061]
[0062] 记X(k)＝X(t)，X(k+1)＝X(t+Δt)，则离散化后的状态方程和观测方程为：
[0063]
[0064] Z(k)＝HX(k)+V(k)
[0065] 其中， Δt为采样间隔。
[0066] 步骤四、对步骤三离散化后的模型使用扩展卡尔曼滤波。
[0067] 步骤五、使用步骤四得出的扩展卡尔曼滤波结果修正步骤一中SINS输出的姿态矩阵 速度(Vx，Vy，Vz)、位置(L，λ，h)以及陀螺的输出值ω′、加速度计的输出值acc′，最后得到导航参数的最优估计即飞行器的姿态角、速度、位置的信息。
[0068] 修正过程如下：
[0069] 姿态矩阵修正：
[0070] 其中， 由姿态失准角得出； 为SINS计算出的姿态矩阵； 为修正后的姿态矩阵。
[0071] 速度修正
[0072]
[0073] 其中，V′x，V′y，V′z为修正后的东向，北向和天向的速度。
[0074] 位置修正
[0075]
[0076] 其中，L′，λ′，h′为修正后的纬度，经度和高度。
[0077] 陀螺角速率补偿
[0078] ω＝ω′-ε
[0079] 其中，ω′为陀螺输出的角速率，ω为修正后的陀螺角速率， 为陀螺常值漂移。
[0080] 加速度补偿
[0081]
[0082] 其中，acc′为加速度计输出的加速度，acc为修正后的加速度，为加速度计零偏。
[0083] 步骤六、整个导航解算过程中，PI控制器对角速率的补偿频率与SINS的更新频率是一样的，并且要大于扩展卡尔曼滤波对角速率的修正频率。在GPS与SINS的组合时刻，按照上述步骤一至步骤五的方法，对飞行器的姿态角、速度和位置进行更新；在非组合时刻，姿态角、位置和速度按照步骤一中的情况二由SINS直接给出。
[0084] 有益效果
[0085] 在假定方位误差角为大失准角的情况下，建立了多阶的SINS/GPS组合导航系统的非线性模型。
[0086] 本发明克服了传统组合导航方法只有在组合时刻进行陀螺和加速度计的误差补偿，而在非组合时刻不对陀螺和加速度计的误差进行修正的
缺陷。本发明在捷联惯导系统(SINS)解算之前引入PI控制器保证了每一时刻进行姿态计算的角速率都是当前最优值。在SINS/GPS组合时刻，角速率的修正是在扩展卡尔曼滤波
算法对陀螺进行了一定的误差补偿的基础上加入PI控制器对角速率进行再次精确补偿；而在非组合时刻，陀螺输出的角速率值仍可以由PI控制器进行修正。算法计算量没有明显增加，却大大改进了传统非线性滤波的精度。
附图说明
[0087] 图1为本发明的SINS/GPS组合导航系统方法原理示意图；
[0088] 图2为具体实施方式中理想导航
坐标系oxnynzn与实际导航坐标系oxn′yn′zn′之间的关系示意图；
[0089] 图3为本发明的SINS/GPS组合导航系统方法
流程图。
具体实施方式
[0090] 下面结合实例及附图，对本发明作进一步详细说明。
[0091] 本实例坐标及符号说明：
[0092] 坐标系符号：理想导航坐标系n，实际导航坐标系n′，
机体坐标系b坐标系选取：选取地理坐标系(东北天)作为导航坐标系
[0093] 第一步：设计PI(比例积分)控制器补偿角速率。
[0094] 将飞行器的姿态矩阵(从机体坐标系b到导航坐标系n的转换矩阵) 记为[0095]
[0096] 第一种情况：
[0097] 飞行器的航向角误差：
[0098] yawerror＝yaw_SINS×yaw_GPS
[0099] 其中，yaw_SINS＝[C11 C21 0]T；yaw_GPS＝[cog_x cog_y0]T，cog为GPS给出的航向角。
[0100]
重力加速度：g＝acc-ω×V_GPS
[0101] 其中，VGPS为GPS给出的速度；acc为从加速度计读出的加速度值；g为重力加速度。
[0102] 飞行器的横滚角和俯仰角误差：
[0103] rollpitcherror＝g_SINS×g
[0104] 其中，g_SINS＝[C31 C32 C33]T
[0105] 总误差：totalerror＝ωYyawerror+ωRProllpitcherror
[0106] 其中，ωY和ωRP分别为航向角误差，横滚俯仰角误差的权重系数。
[0107] 第二种情况：
[0108] 当GPS数据没有给出的时刻，yawerror＝0
[0109] 则总误差totalerror＝ωRProllpitcherror
[0110] 比例(P)补偿：ωProportion＝KPtotalerror
[0111] 积分(I)补偿：ωIntegral＝ωIntegral+KItotalerrorΔt
[0112] 其中，KP为比例系数；KI为积分系数；Δt为采样间隔。
[0113] 对角速率ω进行PI补偿，得到补偿后的角速率为：
[0114] ωcorrection＝ωProportion+ωIntegral
[0115] 将补偿后的角速率ωcorrection作为SINS的输入进行运算，得到姿态角(航向角ψ，俯仰角θ和横滚角γ)，速度(东向Vx，北向Vy，天向Vz)，位置(纬度L，经度λ，高度h)。
[0116] 第二步：考虑
水平方向上的姿态误差角基本不会出现大角度，而方位误差角在实际过程中往往会出现大角度的情况，从而假定水平误差角φx和φy为小角度，φz为大失准角。基于此，推导SINS/GPS组合导航的非线性模型。
[0117] ①实际导航坐标系n′偏离理想导航坐标系n的失准角在东北天三个方向上分别为φx，φy，φz，则
[0118]
[0119]
[0120] 当方位误差为大失准角时，φz不能看作小角度进行近似计算。而水平方向上基本不会出现大角度偏差的情况，所以水平失准角φx和φy可看作小角度，即sinφx＝φx，cosφx＝1，sinφy＝φy，cosφy＝1，则
[0121]
[0122] ②姿态非线性误差方程为：
[0123]
[0124]
[0125]
[0126] 可得：
[0127]
[0128]
[0129]
[0130]
[0131]
[0132]
[0133] ③速度非线性误差方程为：
[0134]
[0135]
[0136]
[0137] 可得：
[0138]
[0139]
[0140]
[0141]
[0142]
[0143]
[0144] ④位置误差方程为：
[0145]
[0146]
[0147]
[0148] ⑤陀螺、加速度计误差方程为：
[0149]
[0150] 其中， 为位置速率， 为地球自转速率；φx，φy，φz为东向，北向，方位失准角；Vx，Vy，Vz为东向，北向，天向的速度；δVx，δVy，δVz为东向、北向、天向速度误差；L，λ，h为纬度，经度，高度；δL，δλ，δh为纬度、经度、高度误差；fx，fy，fz为东向，北向，天向的比力； 为东向，北向，天向的陀螺常值漂移；
为东向，北向，天向的加速度计零偏；Rm为地球子午圈上的曲率半径，Rn为地球卯酉圈上的曲率半径；陀螺常值漂移为 加速度计零偏为
[0151] ⑥基于以上的误差方程，本
实施例中选取15个状态变量如下：
[0152]
[0153] 建立连续多阶的状态方程和观测方程，即SINS/GPS组合导航系统的非线性模型：
[0154]
[0155] Z(t)＝HX(t)+V(t)
[0156] 其中，f(·)为X(t)的非线性函数；H(t)为观测矩阵；W(t)为过程噪声序列；V(t)为观测噪声序列。Z(t)的形式如下：
[0157]
[0158] Vx_GPS，Vy_GPS，Vz_GPS为GPS给出的东向，北向，天向的速度；L_GPS，λ_GPS，h_GPS为GPS给出的纬度，经度，高度信息。
[0159] 第三步：使用二阶泰勒级数展开对第二步得到的非线性误差模型进行线性化并离散化。
[0160] 设采样间隔为Δt
[0161]
[0162] 记X(k)＝X(t)，X(k+1)＝X(t+Δt)，则离散化后的系统方程为：
[0163]
[0164] Z(k)＝HX(k)+V(k)
[0165] 其中，
[0166] 第四步：对第三步离散化后的模型进行扩展卡尔曼滤波：
[0167] 状态一步预测：
[0168] 一步预测误差方差矩阵：P(k+1，k)＝Φ(k)P(k)ΦT(k)+Γ(k)Q(k)ΓT(k)[0169] 滤波增益矩阵：K(k+1)＝P(k+1，k)HT[HP(k+1，k)HT+R(k)]-1
[0170] 状态估计：
[0171] 估计误差方差矩阵：P(k+1)＝[I-K(k+1)H]P(k+1，k)
[0172] 其中，
[0173]
[0174] 即Q(k)是系统过程噪声W(k)的方差矩阵；R(k)是系统观测噪声V(k)的方差矩阵；δkj是Kronecker-δ函数。
[0175] 第五步：使用第四步得出的扩展卡尔曼滤波结果修正第一步中SINS输出的姿态矩阵 速度(Vx，Vy，Vz)、位置(L，λ，h)以及陀螺的输出值ω′、加速度计的输出值acc′，最后得到导航参数的最优估计即飞行器的姿态角、速度、位置的信息。
[0176] 修正过程如下：
[0177] ①姿态矩阵修正
[0178]
[0179] 其中， 为计算出的姿态矩阵， 为修正后的姿态矩阵。
[0180] θ主＝arcsin(C32)
[0181]
[0182]
[0183] 若航向角ψ，俯仰角θ和横滚角γ的取值范围分别定义为[0，2π]，[-π，+π]。
[0184] θ＝θ主
[0185]
[0186]
[0187] 其中，ψ，γ，θ为经过修正后的航向角，横滚角和俯仰角。
[0188] ②速度修正
[0189]
[0190] 其中，V′x，V′v，V′z为修正后的东向，北向和天向的速度。
[0191] ③位置修正
[0192]
[0193] 其中，L′，λ′，h′为修正后的纬度，经度和高度。
[0194] ④陀螺角速率补偿
[0195] ω＝ω′-ε
[0196] 其中，ω′为陀螺输出的角速率，ω为修正后的陀螺角速率， 为陀螺常值漂移。
[0197] ⑤加速度补偿
[0198]
[0199] 其中，acc′为加速度计输出的加速度，acc为修正后的加速度，为加速度计零偏。
[0200] 第六步：整个导航解算过程中，PI控制器对角速率的补偿频率与SINS的更新频率是一样的，并且要大于扩展卡尔曼滤波对角速率的修正频率。在GPS与SINS的组合时刻，按照上述第一步至第五步的方法，对飞行器的姿态角、速度和位置进行更新；在非组合时刻，姿态角、位置和速度按照第一步中的情况二由SINS直接给出。
[0201] 基于方位误差角为大失准角的情况，建立SINS/GPS组合导航的非线性误差模型，使模型更加逼近真实系统反映实际组合系统的特性。每种非线性滤波方法都有自身的局限性，不能仅仅依靠滤波技术完成组合导航的导航参数的最优估计，因而构造一种基于PI控制器和扩展卡尔曼滤波相结合的方法进行导航参数的误差估计来获得更加精确的导航参数。既解决了扩展卡尔曼滤波在模型线性化时产生的截断误差和固定步长的积分运算造成的误差积累问题，又解决了陀螺漂移导致数学平台漂移直接影响导航参数计算不准确的问题。本发明克服了传统组合导航方法只有在组合时刻进行陀螺和加速度计的误差补偿，而在非组合时刻不对陀螺和加速度计的误差进行修正的缺陷。本发明在捷联惯导系统(SINS)解算之前引入PI控制器保证了每一时刻进行姿态计算的角速率都是当前最优值。在SINS/GPS组合时刻，角速率的修正是在扩展卡尔曼滤波算法对陀螺进行了一定的误差补偿的基础上加入PI控制器对角速率进行再次精确补偿；而在非组合时刻，陀螺输出的角速率值仍可以由PI控制器进行修正。算法计算量没有明显增加，却大大改进了传统非线性滤波的精度。
