[发明专利]基于INS惯性导航与GPS导航以及磁力计的导航算法

摘要

本发明公开了一种能够准确反映无人机的姿态和位置信息的基于INS惯性导航与GPS导航以及磁力计的导航算法。该导航算法利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点；利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂移较大的情况，本算法利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏航角，另外，此算法稳定性较强，能够输出比较满意的导航定位信息，从而能够准确反映载体的姿态和位置信息。适合在导航技术领域推广应用。

主权项

1.基于INS惯性导航与GPS导航以及磁力计的导航算法，其特征在于包括以下步骤：A、将INS惯性导航系统的陀螺仪测得的载体角速度参数代入四元数微分方程求解得到四元数q₀，q₁，q₂，q₃；其中为陀螺仪在载体自身坐标系下的测得的三个轴的角速度信息；所述四元数微分方程为：B、将步骤A中求解的q₀，q₁，q₂，q₃代入下式求解得到姿态矩阵根据下述与方向余弦的关系式计算得出载体的INS惯性导航模块姿态角θ、γ、C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角D、将INS惯性导航系统的加速度计测得的加速度参数f^b和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到载体在INS惯性导航坐标系下的东、北、天三个方向上的速度信息v_N v_E v_U，所述微分方程为：其中，vⁿ＝[v_N v_E v_U]^T分别为INS惯性导航坐标系中东、北、天方向上的速度投影到惯性导航坐标系中的速度，为地球自转角速度投影到惯性导航坐标系中的地球自转角速度，为无人机绕INS惯性导航坐标系各轴向的转动角速率投影到惯性导航坐标系中的转动角速率，gⁿ为重力加速度投影到惯性导航坐标系中的重力加速度；E、将步骤D计算得出的vN vE vU分别代入下式求解得出载体在INS惯性导航系统中的位置信息，其中L为纬度，λ为经度，h为高度，h＝h(0)+∫v_Udt，其中L(0)表示载体初始位置的纬度值，λ(0)表示载体初始位置的经度值，h(0)表示载体距离地球表面的初始高度，R_M表示地球子午圈上的曲率半径，R_N表示纬度圈上的曲率半径；F、建立状态方程和观测方程Z(t)＝H(t)X_I(t)+V(t)，XI(t)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示：X_I(t)＝[δv_x δv_y δv_z φ_x φ_y φ_z δL δλ δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]，δv_x,δv_y,δv_z为INS惯性导航系统沿东、北、天方向上的速度误差；φ_x,φ_y,φ_z为载体的姿态角误差；δL,δλ,δh分别代表载体所在纬度、经度和高度误差；ε_x,ε_y,ε_z分别代表陀螺仪的随机漂移；▽_x,▽_y,▽_z分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中F_N(t)对应于δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数的INS惯性导航系统矩阵，其非零元素如下：F(3,4)＝‑fy F(3,5)＝fx F(3,7)＝‑2ωievxsin LF_S(t)为δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是9×6，FM(t)为εx,εy,εz，▽x,▽y,▽z与陀螺仪及加速度计漂移对应的INS惯性导航系统矩阵，是一个维数为6×6的对角线矩阵，表示如下：FM(t)＝diag[‑1/Tgx ‑1/Tgy ‑1/Tgz ‑1/Tax ‑1/Tay ‑1/Taz]；其中，Tgx表示陀螺仪x轴的误差模型的时间常数，Tgy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的误差模型的时间常数，Tax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差模型的时间常数，Taz表示加速度计z轴误差模型的时间常数；GI(t)＝diag[1 1 1......1 1]15×15；WI(t)是一个15维的向量，如下所示：WI(t)＝[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15]，a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15表示系统过程噪声序列；Z(t)为载体在INS惯性导航中的位置速度信息与载体在GPS导航系统中的位置速度信息的差值，是一个6维向量，Z(t)＝[δvx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cos Lδλ+Nx δh+Nh]T，其中，Nvx表示GPS导航系统在x方向上的速度误差，Nvy表示GPS导航系统在y方向上的速度误差，Nvz表示GPS导航系统在z方向上的速度误差，Nx表示GPS导航系统在x方向上的位置误差，Ny表示GPS导航系统在y方向上的位置误差，Nh表示GPS导航系统在z方向上的位置误差；其中Vv(t)＝[Nvx Nvy Nvz]TVp(t)＝[Nx Ny Nz]TG、将上述得到的连续状态方程离散化后得到X_k＝Φ_k,k‑1X_k‑1+W_k‑1，其中将上述得到的连续观测方程Z(t)＝H(t)XI(t)+V(t)离散化后得到Zk＝HkXk+Vk；其中I是单位矩阵，F是INS惯性导航系统的状态转移矩阵，Δt是离散化后INS惯性导航系统的采样时间；H、将载体在INS惯性导航系统中的位置速度信息与载体在GPS导航系统中的位置速度信息作差得到Z(t)在k时刻的观测信息z；I、计算k时刻INS惯性导航系统状态的最优估计值其中，为在k‑1时刻INS惯性导航系统状态的最优估计值，Q_k‑1是INS惯性导航系统的噪声矩阵，其大小是由INS惯性导航元件的性能决定，R_k是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定；J、将计算得到的值与载体在INS惯性导航系统中的位置速度信息作差得到最优的导航参数；K、重复步骤H‑J，得到连续的载体的导航信息参数。