一种用于低成本小型无人机的姿态航向参考系统

摘要

本发明涉及一种用于低成本小型无人机的姿态航向参考系统，包括角速率陀螺、加速度计、GPS、角速率陀螺运算模块、校正模块和卡尔曼滤波器，其中角速率陀螺测量飞机的滚转角速率、俯仰角速率和偏航角速率；加速度计测量重力在机体坐标轴系下的分量；GPS测量飞机的航迹方位角；角速率陀螺运算模块得到带偏移的滚转角φ₁、俯仰角θ₁和偏航角ψ₁；校正模块得到估计的滚转角φ₂、俯仰角θ₂，并将GPS测量得到的航迹方位角作为估计的偏航角ψ₂；卡尔曼滤波器将角速率陀螺运算模块和校正模块产生的数据进行融合，得到最终的姿态角[φ θ ψ]^T。本发明降低了小型无人机系统的成本；减小了航向参考系统的复杂程度；提高了航姿估计的精度。

主权项

一种用于低成本小型无人机的姿态航向参考系统，其特征在于，包括角速率陀螺、加速度计、GPS、角速率陀螺运算模块、校正模块和卡尔曼滤波器，其中：角速率陀螺测量飞机的滚转角速率、俯仰角速率和偏航角速率；加速度计测量重力在机体坐标轴系下的分量；GPS测量飞机的航迹方位角；角速率陀螺运算模块通过角速率陀螺测量的滚转角速率、俯仰角速率和偏航角速率，得到带偏移的滚转角φ₁、俯仰角θ₁和偏航角ψ₁；校正模块通过加速度计测量的重力在机体坐标轴系下的分量得到校正模块估计的滚转角φ₂、俯仰角θ₂，并将GPS测量得到的航迹方位角作为估计的偏航角ψ₂；卡尔曼滤波器将角速率陀螺运算模块和校正模块产生的数据进行融合，得到最终的姿态角[φ θ ψ]^T；所述角速率陀螺运算模块使用四元数法建模，首先定义姿态四元数q：$q=\left[\begin{array}{c}{q}_0\\ \overrightarrow{q}\end{array}\right]$其中q₀称为四元数q的标量部分，$\overrightarrow{q}={\left[\begin{array}{ccc}{q}_1& q_2& q_3\end{array}\right]}^T$称为四元数q的矢量部分，通过角速率陀螺的测量值实时更新四元数：$\stackrel{·}{q}=\frac{1}{2}\Omega ·q=\frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$其中，p为滚转角速率，q俯仰角速率，r为偏航角速率；然后通过以下公式得到载体姿态角$\begin{array}{c}{\phi }_1=\arctan \frac{2(q_1{q}_3+q_0{q}_1)}{1-2(q_1^2+q_2^2)}\\ {\theta }_1=\arcsin 2(-q_1{q}_3+q_2{q}_0)\\ {\psi }_1=\arctan \frac{2(q_1{q}_2+q_3{q}_0)}{1-2(q_2^2+q_3^2)}\end{array};$所述卡尔曼滤波器工作步骤为：在确定初始条件后，就利用角速率陀螺的测量数据积分，进行姿态估计，这一步骤称为“时间更新”，提供了高带宽的姿态信息；在时间更新阶段，由于陀螺数据存在噪声，使得估计姿态的误差随时间累积，因而不能靠单纯的陀螺数据积分确定姿态，为了抑制陀螺误差，需要引入校正系统对陀螺数据进行周期性的修正，这一步骤称为“量测更新”，状态误差的协方差矩阵以及陀螺漂移估计也在该步骤校正；接下来，进行新一轮的时间更新，如此周期性重复；所述卡尔曼滤波器进行数据融合的具体方法为：第一步：利用角速率陀螺测量数据计算姿态四元数${\stackrel{\stackrel{·}{^}}{q}}^{-}=\frac{1}{2}\Omega ·{\hat{q}}^{-}$第二步：预测状态误差协方差矩阵P$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T$式中，Q_k‑1为过程噪声协方差矩阵，F和G为雅克比矩阵；第三步：在校正系统中，利用加速度计及GPS，得到由校正系统估计的姿态角信息[φ₂ θ₂ ψ₂]^T；第四步：利用上一时间步长估计的系统姿态角信息${\left[\begin{array}{ccc}\hat{\phi }& \hat{\theta }& \hat{\Psi }\end{array}\right]}^T$同校正系统得到的姿态角信息作差，得到卡尔曼滤波器的观测量$Z=\left[\begin{array}{c}\mathrm{\delta \phi }\\ \mathrm{\delta \theta }\\ \mathrm{\delta \psi }\end{array}\right]=\left[\begin{array}{c}{\phi }_2\\ {\theta }_2\\ {\psi }_2\end{array}\right]-\left[\begin{array}{c}\hat{\phi }\\ \hat{\theta }\\ \hat{\psi }\end{array}\right]$第五步：计算卡尔曼增益矩阵$K_k=P_{k-1}{H}_k^T{(H_k{P}_{k-1}{H}_k^T+R_k)}^{-1}$式中，R_k为量测噪声协方差矩阵，H为雅克比矩阵；第六步：更新状态误差协方差矩阵$P_k^{+}=(I-K_k{H}_k)P_k^{-}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$第七步：更新状态向量${\hat{X}}_k^{+}={\left[\begin{array}{cccc}{\stackrel{\hat{\to }}{q}}_{\mathrm{ek}}^{+}& {\delta \hat{b}}_{\mathrm{pk}}^{+}& {\delta \hat{b}}_{\mathrm{qk}}^{+}& {\delta \hat{b}}_{\mathrm{rk}}^{+}\end{array}\right]}^T=K_k{Z}_k$第八步：使用估计的误差四元数及陀螺漂移和修正时间更新步骤中所得姿态四元数及角速率陀螺测量值${\hat{q}}_k^{+}={\hat{q}}_k^{-}\otimes {\left[\begin{array}{cc}1& {\stackrel{\hat{\to }}{q}}_{\mathrm{ek}}^{+}\end{array}\right]}^T$${\hat{b}}_k^{+}={\hat{b}}_k^{-}{+\delta \hat{b}}_k^{+}$${\hat{\omega }}_k^{+}={\hat{\omega }}_k^{-}-{\hat{b}}_k^{+}$第九步：由姿态四元数即可计算出姿态角[φ θ ψ]^T。