CN105928515A - 一种无人机导航系统 - Google Patents

Abstract

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

Description

一种无人机导航系统

技术领域

本发明涉及导航技术领域，尤其是一种无人机导航系统。

背景技术

在导航技术方面，目前应用得最多，最成熟的导航方式有INS惯性导航和卫星导航。GPS卫星导航的优点是具有全球性、全天候、长时间定位精度高的特点，但缺点是信号易受干扰和遮挡，在强电磁环境下和有高楼遮挡时，信号质量变差，并且其输出频率有限，一般为1—10Hz，输出不连续，在需要快速更新信息的场合，如机动性和实时性要求较高的无人机系统上，GPS卫星导航的缺点便凸显出来。而INS惯性导航系统是一种全自主式的导航方式，因此具有很强的隐蔽性和抗干扰的能力，并且输出信息连续，短时间内定位精度高。但由于MEMS-INS器件自身的特点，陀螺仪和加速度计有初始零偏、随机漂移等误差，随着时间的累计作用，其误差越来越大，长时间定位精度较差，最终无法准确反映无人机的姿态和位置信息。

发明内容

本发明所要解决的技术问题是提供一种能够准确反映无人机的姿态和位置信息的无人机导航系统。

本发明解决其技术问题所采用的技术方案为：该无人机导航系统，包括INS惯性导航系统模块、磁力计、GPS导航系统模块、控制中心，所述INS惯性导航系统模块包括陀螺仪和加速度计，所述陀螺仪、加速度计、磁力计、GPS导航系统模块分别与控制中心信号连接，所述陀螺仪用于测量无人机的角速度参数并将其测得的角速度参数传递给控制中心，所述磁力计用于测量无人机的偏航角并将其测得的偏航角参数传递给控制中心，所述加速度计用于测量无人机的加速度并将其测得的加速度参数传递给控制中心，所述GPS导航系统模块用于测量无人机在GPS导航系统中的位置速度信息，所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算出无人机在INS惯性导航系统中的位置速度信息，控制中心将无人机在GPS导航系统中的位置速度信息与无人机在GPS导航系统中的位置速度信息进行Kalman滤波并进行导航误差估计，然后利用导航误差估计修正无人机在INS惯性导航系统中的位置速度信息从而得到最终的导航信息。

进一步的是，所述陀螺仪、加速度计、磁力计、分别与控制中心通过I2C总线连接。

进一步的是，所述GPS导航系统模块与控制中心通过USART串口连接。

进一步的是，所述控制中心采用如下方法计算得到最终的导航信息，其具体计算方法如下所述：

A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得到四元数q₀，q₁，q₂，q₃；其中为陀螺仪在无人机自身坐标系下的测得的三个轴的角速度信息；

所述四元数微分方程为：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=0.5\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right];$

B、将步骤A中求解的q₀，q₁，q₂，q₃代入下式求解得到姿态矩阵

$C_n^b=\left[\begin{array}{ccc}{q}_0+q_1+q_2+q_3& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0-q_1+q_2-q_3& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0-q_1-q_2+q_3\end{array}\right]$

根据下述与方向余弦的关系式

计算得出无人机的INS惯性导航模块姿态角θ、γ、

C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角

D、将加速度计测得的加速度参数f^b和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息v_N v_Ev_U，所述微分方程为：

${\stackrel{\·}{v}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n$

其中，vⁿ＝[v_N v_E v_U]'分别为INS惯性导航坐标系中东、北、天方向上的速度，为地球自转角速度，为无人机绕INS惯性导航坐标系各轴向的转动角速率，gⁿ为重力加速度；

E、将步骤D计算得出的v_N v_E v_U分别代入下式求解得出无人机在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)，X_I(t)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示：δv_x,δv_y,δv_z为INS惯性导航系统沿东、北、天方向上的速度误差；φ_x,φ_y,φ_z为无人机的姿态角误差；δL,δλ,δh分别代表无人机所在纬度、经度和高度误差；ε_x,ε_y,ε_z分别代表陀螺仪的随机漂移；分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中F_N(t)对应于δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数的INS惯性导航系统矩阵，其非零元素如下：

$\begin{array}{cc}F(1,1)=\frac{v_y\tan L}{R_N+h}-\frac{v_z}{R_N+h}& F(1,2)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_x\tan L}{R_N+h}\end{array}$

F(1,5)＝-f_z F(1,6)＝f_y

$F(1,7)=(2{\mathrm{\ω}}_{ie}{v}_z\sin L+2{\mathrm{\ω}}_{ie}{v}_y\cos L+\frac{v_x{v}_y{\sec }^{2}L}{R_N+h})$

$\begin{array}{cc}F(2,1)=-(\frac{2v_x\tan L}{R_N+h}+2{\mathrm{\ω}}_{ie}\sin L)& F(2,2)=-\frac{v_z}{R_M+h}\end{array}$

F(2,4)＝f_z F(2,6)＝-f_x

$\begin{array}{cc}F(2,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_x{\sec }^{2}L}{R_N+h})v_x& F(3,1)=(2{\mathrm{\ω}}_{ie}\cos L+\frac{2v_x}{R_M+h})\end{array}$

F(3,4)＝-f_y F(3,5)＝f_x F(3,7)＝-2ω_iev_xsinL

$\begin{array}{ccc}F(3,9)=\frac{2g_0}{R_M}& F(4,2)=-\frac{1}{R_M+h}& F(4,5)={\mathrm{\ω}}_{ie}\sin L+\frac{v_x}{R_N+h}\tan L\end{array}$

$\begin{array}{cc}F(4,6)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_x}{R_N+h}& F(5,1)=\frac{1}{R_N+h}\end{array}$

F(5,7)＝-ω_ie sinL

$\begin{array}{ccc}F(6,1)=\frac{tgL}{R_N+h}& F(6,4)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}& F(6,5)=\frac{v_y}{R_M+h}\end{array}$

$\begin{array}{ccc}F(6,7)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L& F(7,2)=\frac{1}{R_M+h}& F(8,1)=\frac{\sec L}{R_N+h}\end{array}$

F(9,3)＝1

F_S(t)为δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是9×6，

F_M(t)为ε_x,ε_y,ε_z，与陀螺仪及加速度计漂移对应的INS惯性导航系统矩阵，是一个维数为6×6的对角线矩阵，表示如下：

F_M(t)＝diag[-1/T_gx -1/T_gy -1/T_gz -1/T_ax -1/T_ay -1/T_az]；其中，T_gx表示陀螺仪x轴的误差模型的时间常数，T_gy表示陀螺仪y轴的误差模型的时间常数，T_gz表示陀螺仪z轴的误差模型的时间常数，T_ax表示加速度计x轴误差模型的时间常数，T_ay表示加速度计y轴误差模型的时间常数，T_az表示加速度计z轴误差模型的时间常数；

G_I(t)＝diag[1 1 1......1 1]_15×15；

W_I(t)是一个15维的向量，如下所示：

W_I(t)＝[a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅]，

a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅表示系统过程噪声序

列；

Z(t)为无人机在INS惯性导航系统中的位置速度信息与无人机在GPS导航系统中的位置速度信息的差值，是一个6维向量，

Z(t)＝[δv_x+N_vx δv_y+N_vy δv_z+N_vz (R_M+h)δL+N_y (R_M+h)cosLδλ+N_x δh+N_h]^T，其中，N_vx表示GPS导航系统在x方向上的速度误差，N_vy表示GPS导航系统在y方向上的速度误差，N_vz表示GPS导航系统在z方向上的速度误差，N_x表示GPS导航系统在x方向上的位置误差，N_y表示GPS导航系统在y方向上的位置误差，N_h表示GPS导航系统在z方向上的位置误差；

其中

V_v(t)＝[N_vx N_vy N_vz]^T

V_p(t)＝[N_x N_y N_z]^T

G、将上述得到的连续状态方程离散化后得到X_k＝Φ_k,k-1X_k-1+W_k-1，其中

将上述得到的连续观测方程Z(t)＝H(t)X_I(t)+V(t)离散化后得到Z_k＝H_kX_k+V_k；

其中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，得到连续的无人机导航信息。

本发明的有益效果：该无人机导航系统包括INS惯性导航系统模块、磁力计、GPS导航系统模块、控制中心，通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；同时也解决了单一的INS惯性导航参数累计误差越来越大，长时间定位精度发散的缺点，利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点；利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂移较大的情况，本系统利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏航角，另外，此系统成本较低，系统稳定性较强，能够输出比较满意的导航定位信息，从而能够准确反映无人机的姿态和位置信息。

附图说明

图1为本发明所述无人机导航系统计算得出的纬度误差值；

图2为本发明所述无人机导航系统计算得出的纬度误差方差值；

图3为本发明所述无人机导航系统计算得出的经度误差值；

图4为本发明所述无人机导航系统计算得出的经度误差方差值；

图5为本发明所述无人机导航系统计算得出的高度误差值；

图6为本发明所述无人机导航系统计算得出的高度误差方差值；

图7为本发明所述无人机导航系统计算得出的东向速度误差值；

图8为本发明所述无人机导航系统计算得出的东向速度误差方差值；

图9为本发明所述无人机导航系统计算得出的北向速度误差值；

图10为本发明所述无人机导航系统计算得出的北向速度误差方差值；

图11为本发明所述无人机导航系统计算得出的天向速度误差值；

图12为本发明所述无人机导航系统计算得出的天向速度误差方差值。

具体实施方式

本发明所述的无人机导航系统，包括INS惯性导航系统模块、磁力计、GPS导航系统模块、控制中心，所述INS惯性导航系统模块包括陀螺仪和加速度计，所述陀螺仪、加速度计、磁力计、GPS导航系统模块分别与控制中心信号连接，所述陀螺仪用于测量无人机的角速度参数并将其测得的角速度参数传递给控制中心，所述磁力计用于测量无人机的偏航角并将其测得的偏航角参数传递给控制中心，所述加速度计用于测量无人机的加速度并将其测得的加速度参数传递给控制中心，所述GPS导航系统模块用于测量无人机在GPS导航系统中的位置速度信息，所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算出无人机在INS惯性导航系统中的位置速度信息，控制中心将无人机在GPS导航系统中的位置速度信息与无人机在GPS导航系统中的位置速度信息进行Kalman滤波并进行导航误差估计，然后利用导航误差估计修正无人机在INS惯性导航系统中的位置速度信息从而得到最终的导航信息。该无人机导航系统包括INS惯性导航系统模块、磁力计、GPS导航系统模块、控制中心，通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；同时也解决了单一的INS惯性导航参数累计误差越来越大，长时间定位精度发散的缺点，利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点；利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂移较大的情况，本系统利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏航角，另外，此系统成本较低，能够输出比较满意的导航定位信息，从而能够准确反映无人机的姿态和位置信息。

在上述实施方式中，为了保证信号传递的时效性与准确性，所述陀螺仪、加速度计、磁力计、分别与控制中心通过I2C总线连接。所述GPS导航系统模块与控制中心通过USART串口连接。

所述控制中心采用如下方法计算得到最终的导航信息，其具体计算方法如下所述：

A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得到四元数q₀，q₁，q₂，q₃；其中为陀螺仪在无人机自身坐标系下的测得的三个轴的角速度信息；

所述四元数微分方程为：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=0.5\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right];$

B、将步骤A中求解的q₀，q₁，q₂，q₃代入下式求解得到姿态矩阵

$C_n^b=\left[\begin{array}{ccc}{q}_0+q_1+q_2+q_3& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0-q_1+q_2-q_3& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0-q_1-q_2+q_3\end{array}\right]$

根据下述与方向余弦的关系式

计算得出无人机的INS惯性导航模块姿态角θ、γ、

C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角

D、将加速度计测得的加速度参数f^b和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息v_N v_Ev_U，所述微分方程为：

${\stackrel{\·}{v}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n$

其中，vⁿ＝[v_N v_E v_U]'分别为INS惯性导航坐标系中东、北、天方向上的速度，为地球自转角速度，为无人机绕INS惯性导航坐标系各轴向的转动角速率，gⁿ为重力加速度；

E、将步骤D计算得出的v_N v_E v_U分别代入下式求解得出无人机在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)，X_I(t)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示：δv_x,δv_y,δv_z为INS惯性导航系统沿东、北、天方向上的速度误差；φ_x,φ_y,φ_z为无人机的姿态角误差；δL,δλ,δh分别代表无人机所在纬度、经度和高度误差；ε_x,ε_y,ε_z分别代表陀螺仪的随机漂移；分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中F_N(t)对应于δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数的INS惯性导航系统矩阵，其非零元素如下：

$\begin{array}{cc}F(1,1)=\frac{v_y\tan L}{R_N+h}-\frac{v_z}{R_N+h}& F(1,2)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_x\tan L}{R_N+h}\end{array}$

F(1,5)＝-f_z F(1,6)＝f_y

$F(1,7)=(2{\mathrm{\ω}}_{ie}{v}_z\sin L+2{\mathrm{\ω}}_{ie}{v}_y\cos L+\frac{v_x{v}_y{\sec }^{2}L}{R_N+h})$

$\begin{array}{cc}F(2,1)=-(\frac{2v_x\tan L}{R_N+h}+2{\mathrm{\ω}}_{ie}\sin L)& F(2,2)=-\frac{v_z}{R_M+h}\end{array}$

F(2,4)＝f_z F(2,6)＝-f_x

$\begin{array}{cc}F(2,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_x{\sec }^{2}L}{R_N+h})v_x& F(3,1)=(2{\mathrm{\ω}}_{ie}\cos L+\frac{2v_x}{R_M+h})\end{array}$

F(3,4)＝-f_y F(3,5)＝f_x F(3,7)＝-2ω_iev_x sinL

$\begin{array}{ccc}F(3,9)=\frac{2g_0}{R_M}& F(4,2)=-\frac{1}{R_M+h}& F(4,5)={\mathrm{\ω}}_{ie}\sin L+\frac{v_x}{R_N+h}\tan L\end{array}$

$\begin{array}{cc}F(4,6)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_x}{R_N+h}& F(5,1)=\frac{1}{R_N+h}\end{array}$

F(5,7)＝-ω_ie sinL

$\begin{array}{ccc}F(6,1)=\frac{tgL}{R_N+h}& F(6,4)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}& F(6,5)=\frac{v_y}{R_M+h}\end{array}$

$\begin{array}{ccc}F(6,7)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L& F(7,2)=\frac{1}{R_M+h}& F(8,1)=\frac{\sec L}{R_N+h}\end{array}$

F(9,3)＝1

F_S(t)为δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是96，

F_M(t)为ε_x,ε_y,ε_z，与陀螺仪及加速度计漂移对应的INS惯性导航系统矩阵，是一个维数为66的对角线矩阵，表示如下：

F_M(t)＝diag[-1/T_gx -1/T_gy -1/T_gz -1/T_ax -1/T_ay -1/T_az]；其中，T_gx表示陀螺仪x轴的误差模型的时间常数，T_gy表示陀螺仪y轴的误差模型的时间常数，T_gz表示陀螺仪z轴的误差模型的时间常数，T_ax表示加速度计x轴误差模型的时间常数，T_ay表示加速度计y轴误差模型的时间常数，T_az表示加速度计z轴误差模型的时间常数；

G_I(t)＝diag[1 1 1......1 1]₁₅₁₅；

W_I(t)是一个15维的向量，如下所示：

W_I(t)＝[a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅]，

a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅表示系统过程噪声序

列；

Z(t)为无人机在INS惯性导航系统中的位置速度信息与无人机在GPS导航系统中的位置速度信息的差值，是一个6维向量，

Z(t)＝[δv_x+N_vx δv_y+N_vy δv_z+N_vz (R_M+h)δL+N_y (R_M+h)cosLδλ+N_x δh+N_h]^T，其中，N_vx表示GPS导航系统在x方向上的速度误差，N_vy表示GPS导航系统在y方向上的速度误差，N_vz表示GPS导航系统在z方向上的速度误差，N_x表示GPS导航系统在x方向上的位置误差，N_y表示GPS导航系统在y方向上的位置误差，N_h表示GPS导航系统在z方向上的位置误差；

其中

V_v(t)＝[N_vx N_vy N_vz]^T

V_p(t)＝[N_x N_y N_z]^T

G、将上述得到的连续状态方程离散化后得到X_k＝Φ_k,k-1X_k-1+W_k-1，其中

将上述得到的连续观测方程Z(t)＝H(t)X_I(t)+V(t)离散化后得到Z_k＝H_kX_k+V_k；

其中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，得到连续的无人机导航信息。

利用上述方法计算得出的导航信息，能够准确反映无人机的姿态和位置信息，可以实现较好的定位导航精度，图1为本发明所述无人机导航系统计算得出的纬度误差值；图2为本发明所述无人机导航系统计算得出的纬度误差方差值；图3为本发明所述无人机导航系统计算得出的经度误差值；图4为本发明所述无人机导航系统计算得出的经度误差方差值；图5为本发明所述无人机导航系统计算得出的高度误差值；图6为本发明所述无人机导航系统计算得出的高度误差方差值；图7为本发明所述无人机导航系统计算得出的东向速度误差值；图8为本发明所述无人机导航系统计算得出的东向速度误差方差值；图9为本发明所述无人机导航系统计算得出的北向速度误差值；图10为本发明所述无人机导航系统计算得出的北向速度误差方差值；图11为本发明所述无人机导航系统计算得出的天向速度误差值；图12为本发明所述无人机导航系统计算得出的天向速度误差方差值；

从上述测试结果图可以看出，本发明所述无人机导航系统得出的经度、纬度、高度的误差方差均能快速收敛至比较小的数值；对位置、速度等导航信息也能实现滤平滑作用，不会产生大的跳变，系统的稳定性较强。

Claims (4)

1.一种无人机导航系统，其特征在于：包括INS惯性导航系统模块、磁力计、GPS导航系统模块、控制中心，所述INS惯性导航系统模块包括陀螺仪和加速度计，所述陀螺仪、加速度计、磁力计、GPS导航系统模块分别与控制中心信号连接，所述陀螺仪用于测量无人机的角速度参数并将其测得的角速度参数传递给控制中心，所述磁力计用于测量无人机的偏航角并将其测得的偏航角参数传递给控制中心，所述加速度计用于测量无人机的加速度并将其测得的加速度参数传递给控制中心，所述GPS导航系统模块用于测量无人机在GPS导航系统中的位置速度信息，所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算出无人机在INS惯性导航系统中的位置速度信息，控制中心将无人机在GPS导航系统中的位置速度信息与无人机在GPS导航系统中的位置速度信息进行Kalman滤波并进行导航误差估计，然后利用导航误差估计修正无人机在INS惯性导航系统中的位置速度信息从而得到最终的导航信息。

2.如权利要求1所述的无人机导航系统，其特征在于：所述陀螺仪、加速度计、磁力计、分别与控制中心通过I2C总线连接。

3.如权利要求2所述的无人机导航系统，其特征在于：所述GPS导航系统模块与控制中心通过USART串口连接。

4.如权利要求1所述的无人机导航系统，其特征在于：所述控制中心采用如下方法计算得到最终的导航信息，其具体计算方法如下所述：

A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得到四元数q₀，q₁，q₂，q₃；其中为陀螺仪在无人机自身坐标系下的测得的三个轴的角速度信息；

所述四元数微分方程为：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]\mathrm{=0}\mathrm{.5}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right];$

B、将步骤A中求解的q₀，q₁，q₂，q₃代入下式求解得到姿态矩阵

$C_n^b=\left[\begin{array}{ccc}{q}_0+q_1+q_2+q_3& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0-q_1+q_2-q_3& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0-q_1-q_2+q_3\end{array}\right]$

根据下述与方向余弦的关系式

计算得出无人机的INS惯性导航模块姿态角θ、γ、

C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角

D、将加速度计测得的加速度参数f^b和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息v_N v_E v_U，所述微分方程为：

${\stackrel{\·}{v}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n$

其中，vⁿ＝[v_N v_E v_U]'分别为INS惯性导航坐标系中东、北、天方向上的速度，为地球自转角速度，为无人机绕INS惯性导航坐标系各轴向的转动角速率，gⁿ为重力加速度；

E、将步骤D计算得出的v_N v_E v_U分别代入下式求解得出无人机在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)，

X_I(t)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示：

δv_x,δv_y,δv_z为INS惯性导航系统沿东、北、天方向上的速度误差；φ_x,φ_y,φ_z为无人机的姿态角误差；δL,δλ,δh分别代表无人机所在纬度、经度和高度误差；ε_x,ε_y,ε_z分别代表陀螺仪的随机漂移；分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中F_N(t)对应于δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数的INS惯性导航系统矩阵，其非零元素如下：

$\begin{array}{cc}F(1,1)=\frac{v_y\tan L}{R_N+h}-\frac{v_z}{R_N+h}& F(1,2)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_x\tan L}{R_N+h}\end{array}$

F(1,5)＝-f_z F(1,6)＝f_y

$F(1,7)=(2{\mathrm{\ω}}_{ie}{v}_z\sin L+2{\mathrm{\ω}}_{ie}{v}_y\cos L+\frac{v_x{v}_y{\sec }^{2}L}{R_N+h})$

$\begin{array}{cc}F(2,1)=-(\frac{2v_x\tan L}{R_N+h}+2{\mathrm{\ω}}_{ie}\sin L)& F(2,2)=-\frac{v_z}{R_M+h}\end{array}$

F(2,4)＝f_z F(2,6)＝-f_x

$\begin{array}{cc}F(2,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_x{\sec }^{2}L}{R_N+h})v_x& F(3,1)=(2{\mathrm{\ω}}_{ie}\cos L+\frac{2v_x}{R_M+h})\end{array}$

F(3,4)＝-f_y F(3,5)＝f_x F(3,7)＝-2ω_iev_x sin L

$\begin{array}{ccc}F(3,9)=\frac{2g_0}{R_M}& F(4,2)=-\frac{1}{R_M+h}& F(4,5)={\mathrm{\ω}}_{ie}\sin L+\frac{v_x}{R_N+h}\tan L\end{array}$

$\begin{array}{cc}F(4,6)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_x}{R_N+h}& F(5,1)=\frac{1}{R_N+h}\end{array}$

F(5,7)＝-ω_ie sin L

$\begin{array}{ccc}F(6,1)=\frac{tgL}{R_N+h}& F(6,4)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}& F(6,5)=\frac{v_y}{R_M+h}\end{array}$

$\begin{array}{ccc}F(6,7)={\mathrm{\ω}}_{re}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L& F(7,2)=\frac{1}{R_M+h}& F(8,1)=\frac{\sec L}{R_N+h}\end{array}$

F(9,3)＝1

F_S(t)为δv_x,δv_y,δv_z，φ_x,φ_y,φ_z，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是9×6，

F_M(t)为ε_x,ε_y,ε_z，与陀螺仪及加速度计漂移对应的INS惯性导航系统矩阵，是一个维数为6×6的对角线矩阵，表示如下：

F_M(t)＝diag[-1/T_gx -1/T_gy -1/T_gz -1/T_ax -1/T_ay -1/T_az]；其中，T_gx表示陀螺仪x轴的误差模型的时间常数，T_gy表示陀螺仪y轴的误差模型的时间常数，T_gz表示陀螺仪z轴的误差模型的时间常数，T_ax表示加速度计x轴误差模型的时间常数，T_ay表示加速度计y轴误差模型的时间常数，T_az表示加速度计z轴误差模型的时间常数；

G_I(t)＝diag[1 1 1......1 1]_15×15；

W_I(t)是一个15维的向量，如下所示：

W_I(t)＝[a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅]，

a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ a₁₀ a₁₁ a₁₂ a₁₃ a₁₄ a₁₅表示系统过程噪声序列；

Z(t)为无人机在INS惯性导航系统中的位置速度信息与无人机在GPS导航系统中的位置速度信息的差值，是一个6维向量，

Z(t)＝[δv_x+N_vx δv_y+N_vy δv_z+N_vz (R_M+h)δL+N_y (R_M+h)cosLδλ+N_x δh+N_h]^T，其中，N_vx表示GPS导航系统在x方向上的速度误差，N_vy表示GPS导航系统在y方向上的速度误差，N_vz表示GPS导航系统在z方向上的速度误差，N_x表示GPS导航系统在x方向上的位置误差，N_y表示GPS导航系统在y方向上的位置误差，N_h表示GPS导航系统在z方向上的位置误差；

其中

V_v(t)＝[N_vx N_vy N_vz]^T

V_p(t)＝[N_x N_y N_z]^T

G、将上述得到的连续状态方程离散化后得到X_k＝Φ_k,k-1X_k-1+W_k-1，其中

将上述得到的连续观测方程Z(t)＝H(t)X_I(t)+V(t)离散化后得到Z_k＝H_kX_k+V_k；

其中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，得到连续的无人机导航信息。