﻿ 改进扩展卡尔曼滤波的四旋翼姿态估计算法
 计算机应用   2017, Vol. 37 Issue (4): 1122-1128  DOI: 10.11772/j.issn.1001-9081.2017.04.1122 0

### 引用本文

WANG Long, ZHANG Zheng, WANG Li. Improved extended Kalman filter for attitude estimation of quadrotor[J]. Journal of Computer Applications, 2017, 37(4): 1122-1128. DOI: 10.11772/j.issn.1001-9081.2017.04.1122.

### 文章历史

Improved extended Kalman filter for attitude estimation of quadrotor
WANG Long, ZHANG Zheng, WANG Li
School of Information Science and Engineering, Wuhan University of Science and Technology, Wuhan Hubei 430081, China
Key words: quadrotor    Extended Kalman Filter (EKF)    motion acceleration restraint    dynamic step    gradient descent algorithm
0 引言

1 姿态参考系统 1.1 姿态坐标系

 图 1 载体坐标系和导航坐标系 Figure 1 Body coordinate system and navigation coordinate system
1.2 姿态角与四元数的关系

 $\boldsymbol{q}={{q}_{0}}+{{q}_{1}}{\rm i}+{{q}_{2}}{\rm j}+{{q}_{3}}{\rm k}={{\left[ {{q}_{0}}\;\;\boldsymbol{ }{{q}_{1}}\;\;\boldsymbol{ }{{q}_{2}}\;\;\boldsymbol{ }{{q}_{3}} \right]}^{\text{T}}}$ (1)

 \boldsymbol{C}_{b}^{n}=\left[ \begin{align} & 1-2{{q}_{2}^{2}}-2{{q}_{3}^{2}}\boldsymbol{ }2({{q}_{1}}{{q}_{2}}-{{q}_{0}}{{q}_{3}})\boldsymbol{ }2({{q}_{1}}{{q}_{3}}+{{q}_{0}}{{q}_{2}}) \\ & 2({{q}_{1}}{{q}_{2}}+{{q}_{0}}{{q}_{3}})\boldsymbol{ }1-2{{q}_{1}^{2}}-2{{q}_{3}^{2}}\boldsymbol{ }2({{q}_{2}}{{q}_{3}}-{{q}_{0}}{{q}_{1}}) \\ & 2({{q}_{1}}{{q}_{3}}-{{q}_{0}}{{q}_{2}})\boldsymbol{ }2({{q}_{2}}{{q}_{3}}-{{q}_{0}}{{q}_{1}})\boldsymbol{ }1-2{{q}_{1}^{2}}-2{{q}_{2}^{2}} \\ \end{align} \right]\text{ } (2)

 $\left[ \begin{array}{l} \phi \\ \theta \\ \psi \end{array} \right] = \left[ \begin{array}{c} a\tan 2(2{q_2}{q_3} + 2{q_0}{q_1},{q_3^2} - {q_2^2} - {q_1^2} + {q_0^2})\\ - a\sin (2{q_1}{q_3} - 2{q_0}{q_2})\\ a\tan 2(2{q_1}{q_2} + 2{q_0}{q_3},{q_1^2} + {q_0^2} - {q_3^2} - {q_2^2}) \end{array} \right]$ (3)
1.3 改进EKF姿态估计算法结构

 图 2 改进EKF姿态估计算法原理 Figure 2 Principle of improved EKF attitude estimation algorithm
2 改进EKF姿态估计算法 2.1 陀螺仪姿态更新算法

 ${}_{b}^{n}\boldsymbol{\dot{q}}=\frac{1}{2}{}_{b}^{n}\boldsymbol{q}\otimes \boldsymbol{\omega }$ (4)

 $\left[ \begin{array}{l} {{\dot q}_0}\\ {{\dot q}_1}\\ {{\dot q}_2}\\ {{\dot q}_3} \end{array} \right] = \frac{1}{2}\left[ \begin{array}{l} 0{\boldsymbol{ }}\;\; - {\omega _x}{\boldsymbol{ }}\;\; - {\omega _y}{\boldsymbol{ }}\;\; - {\omega _z}\\ {\omega _x}{\boldsymbol{ }}\;\;\;\;0{\boldsymbol{ }}\;\;\;\;\;\;{\omega _z}{\boldsymbol{ }}\;\; \;\;- {\omega _y}\\ {\omega _z}{\boldsymbol{ }}\;\;\;\;{\omega _y}{\boldsymbol{ }} \;\;- {\omega _x}{\boldsymbol{ }}\;\;\;\;0 \end{array} \right]\left[ \begin{array}{l} {q_0}\\ {q_1}\\ {q_2}\\ {q_3} \end{array} \right]$ (5)
 $_{b}^{n}{{\boldsymbol{\dot{q}}}_{k}}=\frac{1}{2}{}_{b}^{n}{{\boldsymbol{q}}_{\left( k-1 \right)}}\otimes \boldsymbol{\omega }_{k}^{b}$ (6)
 $_{b}^{n}{{\boldsymbol{q}}_{k}}=_{b}^{n}{{\boldsymbol{q}}_{\left( k-1 \right)}}+_{b}^{n}{{\boldsymbol{\dot{q}}}_{k}}{{T}_{s}}\text{ }$ (7)

2.2 加速度补偿算法

 $\left[ \begin{array}{l} a_{gx}^b\\ a_{gy}^b\\ a_{gz}^b \end{array} \right] = {\boldsymbol{C}}_n^b\left[ \begin{array}{l} a_{gx}^n\\ a_{gy}^n\\ a_{gz}^n \end{array} \right]$ (8)

 $\left[ \begin{array}{l} a_x^b\\ a_y^b\\ a_z^b \end{array} \right] = \left[ \begin{array}{l} a_{ax}^b\\ a_{ay}^b\\ a_{az}^b \end{array} \right] - {\boldsymbol{C}}_n^b\left[ \begin{array}{l} a_{gx}^n\\ a_{gy}^n\\ a_{gz}^n \end{array} \right]$ (9)
2.3 动态步长梯度下降算法

 ${{\boldsymbol{g}}^{b}}={}_{n}^{b}\boldsymbol{q}\otimes {{\boldsymbol{g}}^{n}}\otimes {}_{b}^{n}{{\boldsymbol{q}}^{\boldsymbol{*}}}$ (10)

 {{\boldsymbol{f}}_{g}}\left( {}_{n}^{b}\boldsymbol{q,a} \right)=\left[ \begin{align} & \;\;\;\;2\left( {{q}_{1}}{{q}_{3}}-{{q}_{0}}{{q}_{2}} \right)-a_{x}^{b} \\ & \;\;\;\;2\left( {{q}_{0}}{{q}_{1}}+{{q}_{2}}{{q}_{3}} \right)-a_{y}^{b} \\ & 2\left( {1}/{2}\;-{{q}_{1}^{2}}-{{q}_{2}^{2}} \right)-a_{z}^{b} \\ \end{align} \right] (11)

 ${{\boldsymbol{J}}_g}\left( {{}_n^b{\boldsymbol{q}}} \right) = \frac{{d{\boldsymbol{f}_g}}}{{d{}_n^b\boldsymbol{q}}} = \left[ \begin{array}{l} \frac{{\partial {\boldsymbol{f}_{gx}}}}{{d{}_n^b\boldsymbol{q}}}\\ \frac{{\partial {\boldsymbol{f}_{gy}}}}{{d{}_n^b\boldsymbol{q}}}\\ \frac{{\partial {\boldsymbol{f}_{gz}}}}{{d{}_n^b\boldsymbol{q}}} \end{array} \right]{\rm{ = }}\left[ \begin{array}{l} - 2{q_0}\;{\rm{ }}2{q_3}\;{\rm{ }} - 2{q_0}\;{\rm{ }}2{q_1}\\ {\rm{ }}\;\;2{q_1}\;\;{\rm{ }}2{q_0}\;\;\;\;{\rm{ }}2{q_3}\;\;{\rm{ }}2{q_2}\\ {\rm{ }}\;\;\;0{\rm{ }} \;- 4{q_1}\;{\rm{ }} - 4{q_2}\;\;{\rm{ }}0 \end{array} \right]$ (12)

 ${}_{n}^{b}{{\boldsymbol{q}}_{\nabla }}\left( k \right)={}_{n}^{b}{{\boldsymbol{q}}_{\nabla }}\left( k-1 \right)-\boldsymbol{\mu} \frac{\nabla {{\boldsymbol{f}}_{g}}\left( {}_{n}^{b}\boldsymbol{q,}{{\boldsymbol{a}}^{b}} \right)}{\left\| \left. \nabla {{\boldsymbol{f}}_{g}}\left( {}_{n}^{b}\boldsymbol{q,}{{\boldsymbol{a}}^{b}} \right) \right\| \right.}$ (13)

 $\boldsymbol{\mu} \sim \alpha \left\| \boldsymbol{\omega} \right\|{{T}_{s}}$ (14)

 $\boldsymbol{\omega }=\left[ {{\omega }_{x}}\text{ }{{\omega }_{y}}\text{ }{{\omega }_{z}} \right]$

 $\left\| \boldsymbol{\omega} \right\|=\sqrt{(\omega {{_{x}^{b}})^{2}}+(\omega {{_{y}^{b}})^{2}}+(\omega {{_{z}^{b}})^{2}}}\text{ }$ (15)

 $\mu \text{=}{{\mu }_{0}}\text{+}\alpha {{T}_{s}}\sqrt{(\omega {{_{x}^{b}})^{2}}+(\omega {{_{y}^{b}})^{2}}+(\omega {{_{z}^{b}})^{2}}}\text{ }$ (16)
2.4 运动加速度抑制处理算法

 ${{\delta }_{k}}={\varepsilon }/{\left( \varepsilon \text{+}\left\| {{\boldsymbol{a}_{s}}^{b}}_{k} \right\| \right)}\;$ (17)

 ${{\boldsymbol{a}_{s}}^{b}}_{k}={{\boldsymbol{a}}^{b}}_{k}-\boldsymbol{g}{{\left[ C{{_{b}^{n}}_{31}}\text{ }C{{_{b}^{n}}_{32}}\text{ }C{{_{b}^{n}}_{33}} \right]}^{\text{T}}_{\left. k \right|k-1}}$ (18)

 ${}_{n}^{b}{{\boldsymbol{q}}_{\nabla }}\left( k \right)={}_{n}^{b}{{\boldsymbol{q}}_{\nabla }}\left( k-1 \right)-{{\delta }_{k}}*\mu \frac{\nabla {{\boldsymbol{f}}_{g}}\left( {}_{n}^{b}\boldsymbol{q,}{{\boldsymbol{a}}^{b}} \right)}{\left\| \left. \nabla {{\boldsymbol{f}}_{g}}\left( {}_{n}^{b}\boldsymbol{q,}{{\boldsymbol{a}}^{b}} \right) \right\| \right.}$ (19)

3 一种改进EKF姿态估计建模

 \left\{ \begin{align} & {{{\boldsymbol{\hat{X}}}}_{k}}={{\boldsymbol{A}}_{k}}{{\boldsymbol{X}}_{k-1}}+{{\boldsymbol{W}}_{k}} \\ & {{\boldsymbol{Z}}_{k}}={{\boldsymbol{H}}_{k}}{{{\boldsymbol{\hat{X}}}}_{k}}+{{\boldsymbol{V}}_{k}} \\ \end{align} \right. (20)

 \left\{ \begin{align} & E[{{\boldsymbol{W}}_{k}}]=0\text{ };\;\;E[{{\boldsymbol{W}}_{k}}\boldsymbol{W}_{k}^{\text{T}}]={{\boldsymbol{Q}}_{k}} \\ & E[{{\boldsymbol{V}}_{k}}]=0\text{ };\;\;E[{{\boldsymbol{V}}_{k}}\boldsymbol{V}_{k}^{\text{T}}]={{\boldsymbol{R}}_{k}} \\ \end{align} \right. (21)

3.1 过程模型

 $\left[ \begin{array}{l} {{\dot x}_1}\\ {{\dot x}_2}\\ {{\dot x}_3}\\ {{\dot x}_4} \end{array} \right] = \frac{1}{2}\left[ \begin{array}{l} {x_1}\\ {x_2}\\ {x_3}\\ {x_4} \end{array} \right] \otimes \left[ \begin{array}{c} 0\\ \omega _x^b\\ \omega _y^b\\ \omega _z^b \end{array} \right]$ (22)

 \boldsymbol{\dot{q}}={{\mathit{\pmb{\Omega}}}^{b}}\boldsymbol{q}=\left[ \begin{align} & 0\text{ }-\frac{\omega _{x}^{b}}{2}\; -\frac{\omega _{y}^{b}}{2}\text{ -}\frac{\omega _{z}^{b}}{2} \\ & \frac{\omega _{x}^{b}}{2}\;\; 0\;\;-\frac{\omega _{z}^{b}}{2}\;\;\frac{\omega _{y}^{b}}{2}\text{ } \\ & \frac{\omega _{y}^{b}}{2}\;\frac{\omega _{z}^{b}}{2}\;\; 0 \;\;-\frac{\omega _{x}^{b}}{2} \\ & \frac{\omega _{z}^{b}}{2}\;-\frac{\omega _{y}^{b}}{2}\text{ }\frac{\omega _{x}^{b}}{2}\;\; 0\;\; \\ \end{align} \right]\boldsymbol{q} (23)

 ${{\boldsymbol{q}}_{k}}=\exp (\mathit{\pmb{\Omega}}_{k-1}^{b}{{T}_{s}}){{\boldsymbol{q}}_{k-1}}\text{ }k=1,2,...$ (24)

 ${{\boldsymbol{q}}_{k}}=\left[ {{I}_{3\times 3}}\left( 1-\frac{\Delta {{\theta ^{2}}}}{8} \right)+\frac{1}{2}\mathit{\pmb{\Omega}}_{k-1}^{b}{{T}_{s}} \right]{{\boldsymbol{q}}_{k-1}}\text{ }$ (25)

 ${{\boldsymbol{\omega }}^{b}}\text{=}{{\boldsymbol{\omega }}_{\text{real}}}+{}^{g}\boldsymbol{b}+{}^{g}\boldsymbol{w}$ (26)

 ${{\boldsymbol{q}}_{k}}=A\left( \mathit{\pmb{\Omega}}_{k-1}^{b},{{T}_{s}} \right){{\boldsymbol{q}}_{k-1}}+{}^{q}{{\boldsymbol{w}}_{k-1}}=\\ \;\;\;\;\;\;\;\left[ {{\boldsymbol{I}}_{3\times 3}}\left( 1-\frac{\Delta {{\theta ^{2}}}}{8} \right)+\frac{1}{2}\boldsymbol{\Omega }_{k-1}^{b}{{T}_{s}} \right]{{\boldsymbol{q}}_{k-1}}\text{+}{}^{q}{{\boldsymbol{w}}_{k-1}}$ (27)

 ${}^{q}{{\boldsymbol{w}}_{k-1}}=-\frac{{{T}_{s}}}{2}{{\boldsymbol{\Xi} }_{k-1}}{}^{q}{{\boldsymbol{w}}_{k-1}}=-\frac{{{T}_{s}}}{2}\left[ \begin{matrix} \left[ {{\boldsymbol{e}}_{k-1}}\times \right]+{{q}_{0}}{{\boldsymbol{I}}_{3\times 3}} \\ -\boldsymbol{e}_{k-1}^{\text{T}} \\ \end{matrix} \right]{}^{g}\boldsymbol{w}$ (28)

 ${{\boldsymbol{Q}}_{k}}={{\left( \frac{{{T}_{s}}}{2} \right)^{2}}}{{\boldsymbol{\Xi} }_{k}}\sigma _{g^{2}}\boldsymbol{I}\boldsymbol{\Xi} _{k}^{T}\text{ }$ (29)
3.2 观测模型

 ${{\boldsymbol{z}}_{i}}={{\boldsymbol{q}}_{\nabla ,i-1}}={{\boldsymbol{x}}_{i}}+{{\boldsymbol{v}}_{i}},\text{ }i=1,2,3.4$ (30)

 ${{\boldsymbol{H}}_{k}}={{\boldsymbol{I}}_{4\times 4}}$ (31)

 图 3 改进EKF姿态估计算法原理框图 Figure 3 Principle diagram of improved EKF attitude estimation
3.3 组建改进EKF四旋翼姿态估计算法

 图 4 磁力计球形拟合 Figure 4 Magnetometer spherical fitting

 $\left[ \begin{array}{l} m_{cx}^b\\ m_{cy}^b\\ m_{cz}^b \end{array} \right] = \left[ \begin{array}{l} m_x^b\cos \phi - m_z^b\sin \phi \\ m_x^b\sin \theta \sin \phi + m_y^b\cos \theta + m_z^b\sin \theta \cos \phi \\ m_x^b\cos \theta \sin \phi - m_y^b\sin \theta + m_z^b\cos \theta \cos \phi \end{array} \right]$ (32)

 $\psi =\arctan \left( \frac{m_{cy}^{b}}{m_{cx}^{b}} \right)$ (33)

4 实验测试与结果分析 4.1 实验测试平台

 图 5 四旋翼飞行器实验平台系统结构 Figure 5 System structure of experimental platform for quadrotor

4.2 实验结果及分析

 图 6 静态测试下两种姿态算法的结果 Figure 6 Static test results of two attitude algorithms

 图 7 抗磁干扰实验结果 Figure 7 Anti-magnetic interference test results

 图 8 水平滑动测试两种算法的结果 Figure 8 Horizontal slip test results of two attitude algorithms

 图 9 动态绕轴测试下两种算法结果 Figure 9 Rotation test results of two attitude algorithms

 图 10 两种算法偏航角输出结果 Figure 10 yaw results of two attitude algorithms
5 结语