转载自：https://blog.csdn.net/wxc971231/article/details/97449026
https://blog.csdn.net/luoshi006/article/details/51513580

一、姿态解算原理相关

1、简介

对于四旋翼无人机来说，有两个相关坐标系，即地理坐标系 O n X n Y n Z n O n X n Y n Z n O n X n Y n Z n OnXnYnZnO_nX_nY_nZ_nOnXnYnZn OnXnYnZnOnXnYnZnOnXnYnZn和机体坐标系 O b X b Y b Z b O b X b Y b Z b O b X b Y b Z b ObXbYbZbO_bX_bY_bZ_bObXbYbZb ObXbYbZbObXbYbZbObXbYbZb。
姿态角的检测对于四旋翼控制是至关重要的。比如在做飞行器姿态控制的时候，通常使用串级pid的方法，需要得到飞行器姿态角（即欧拉角）来做角度位置环反馈。再如利用装置在机腹垂直向下拍摄的摄像头，进行机体视觉定位时，也需要靠姿态角进行视觉反馈补偿。
要解算姿态角，就要研究从地理坐标系到机体坐标系的转换过程。这个转换过程不是唯一的，比如可以先绕X轴旋转 θ θ θ θ\thetaθ θθθ，再绕Y轴旋转 γ γ γ γ\gammaγ γγγ，最后绕Z轴旋转 ψ ψ ψ ψ\psiψ ψψψ，这样得到的一组姿态角称为卡尔丹角；也可以按ZYX的顺序旋转，这样得到的一组姿态角称为欧拉角（这是最常用的，pitch,roll,yaw）。不管按照什么顺序，得到的角度都可以称作广义欧拉角。实际理论分析时，旋转顺序不是很重要，这个顺序会影响四元数与欧拉角的关系，但是都可以进行解算。

2、坐标变换和旋转矩阵

下面从最基本的坐标变换入手开始讲解，请看如下坐标系旋转： O A X A Y A Z A O A X A Y A Z A O A X A Y A Z A OAXAYAZAO_AX_AY_AZ_AOAXAYAZA OAXAYAZAOAXAYAZAOAXAYAZA绕OX旋转 θ θ θ θ\thetaθ θθθ到 O B X B Y B Z B O B X B Y B Z B O B X B Y B Z B OBXBYBZBO_BX_BY_BZ_BOBXBYBZB OBXBYBZBOBXBYBZBOBXBYBZB

对于原系 O A X A Y A Z A O A X A Y A Z A O A X A Y A Z A OAXAYAZAO_AX_AY_AZ_AOAXAYAZA OAXAYAZAOAXAYAZAOAXAYAZA中的一个向量 [ r x A , r y A , r z A ] [ r x A , r y A , r z A ] [ r x A , r y A , r z A ] [rxA,ryA,rzA][r_{x_A},r_{y_A},r_{z_A}][rxA,ryA,rzA] [rxA,ryA,rzA][rxA,ryA,rzA][rxA,ryA,rzA]，转换到新系 O B X B Y B Z B O B X B Y B Z B O B X B Y B Z B OBXBYBZBO_BX_BY_BZ_BOBXBYBZB OBXBYBZBOBXBYBZBOBXBYBZB中的向量 [ r x B , r y B , r z B ] [ r x B , r y B , r z B ] [ r x B , r y B , r z B ] [rxB,ryB,rzB][r_{x_B},r_{y_B},r_{z_B}][rxB,ryB,rzB] [rxB,ryB,rzB][rxB,ryB,rzB][rxB,ryB,rzB]，有：

r x B = r x A r y B = c o s ( θ ) r y A + s i n ( θ ) r z A r z B = − s i n ( θ ) r y A + c o s ( θ ) r z A r x B = r x A r y B = c o s ( θ ) r y A + s i n ( θ ) r z A r z B = − s i n ( θ ) r y A + c o s ( θ ) r z A r x B = r x A r y B = c o s ( θ ) r y A + s i n ( θ ) r z A r z B = − s i n ( θ ) r y A + c o s ( θ ) r z A rxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzAr_{x_B}=r_{x_A} \\ r_{y_B}=cos(\theta)r_{y_A}+sin(\theta)r_{z_A} \\ r_{z_B}=-sin(\theta)r_{y_A}+cos(\theta)r_{z_A}rxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzA rxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzArxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzArxB=rxAryB=cos(θ)ryA+sin(θ)rzArzB=−sin(θ)ryA+cos(θ)rzA

可以用旋转矩阵表示

[ r x B r y B r z B ] = [ 1000 c o s ( θ ) s i n ( θ ) 0 − s i n ( θ ) c o s ( θ ) ] [ r x A r y A r z A ] [ r x B r y B r z B ] = [ 1 a m p ; 0 a m p ; 0 0 a m p ; c o s ( θ ) a m p ; s i n ( θ ) 0 a m p ; − s i n ( θ ) a m p ; c o s ( θ ) ] [ r x A r y A r z A ] ⎣ ⎡ r x B r y B r z B ⎦ ⎤ = ⎣ ⎡ 100 0 c o s ( θ ) − s i n ( θ ) 0 s i n ( θ ) c o s ( θ ) ⎦ ⎤ ⎣ ⎡ r x A r y A r z A ⎦ ⎤ [rxBryBrzB]=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)][rxAryArzA] {\left[ \begin{array}{c} r_{x_B}\\ r_{y_B}\\ r_{z_B} \end{array} \right ]}= {\left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos(\theta) & sin(\theta)\\ 0 & -sin(\theta) & cos(\theta) \end{array} \right ]} {\left[ \begin{array}{c} r_{x_A}\\ r_{y_A}\\ r_{z_A} \end{array} \right ]} ⎣⎡rxBryBrzB⎦⎤=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤⎣⎡rxAryArzA⎦⎤ [rxBryBrzB]=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)][rxAryArzA]⎣⎡rxBryBrzB⎦⎤=⎣⎡100amp;0amp;cos(θ)amp;−sin(θ)amp;0amp;sin(θ)amp;cos(θ)⎦⎤⎣⎡rxAryArzA⎦⎤⎣⎡rxBryBrzB⎦⎤=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤⎣⎡rxAryArzA⎦⎤

中间的就是绕x轴旋转 θ θ θ θ\thetaθ θθθ 的旋转矩阵，记作
C x ( θ ) = [ 1000 c o s ( θ ) s i n ( θ ) 0 − s i n ( θ ) c o s ( θ ) ] C x ( θ ) = [ 1 a m p ; 0 a m p ; 0 0 a m p ; c o s ( θ ) a m p ; s i n ( θ ) 0 a m p ; − s i n ( θ ) a m p ; c o s ( θ ) ] C x ( θ ) = ⎣ ⎡ 100 0 c o s ( θ ) − s i n ( θ ) 0 s i n ( θ ) c o s ( θ ) ⎦ ⎤ Cx(θ)=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)] C_{x_(\theta)}= {\left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos(\theta) & sin(\theta)\\ 0 & -sin(\theta) & cos(\theta) \end{array} \right ]} Cx(θ)=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤ Cx(θ)=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)]Cx(θ)=⎣⎡100amp;0amp;cos(θ)amp;−sin(θ)amp;0amp;sin(θ)amp;cos(θ)⎦⎤Cx(θ)=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤
同样的，还有绕y轴旋转 γ γ γ γ\gammaγ γγγ 的旋转矩阵，记作
C y ( γ ) = [ c o s ( γ ) 0 − s i n ( γ ) 010 s i n ( γ ) 0 c o s ( γ ) ] C y ( γ ) = [ c o s ( γ ) a m p ; 0 a m p ; − s i n ( γ ) 0 a m p ; 1 a m p ; 0 s i n ( γ ) a m p ; 0 a m p ; c o s ( γ ) ] C y ( γ ) = ⎣ ⎡ c o s ( γ ) 0 s i n ( γ ) 010 − s i n ( γ ) 0 c o s ( γ ) ⎦ ⎤ Cy(γ)=[cos(γ)0−sin(γ)010sin(γ)0cos(γ)] C_{y_(\gamma)}= {\left[ \begin{array}{ccc} cos(\gamma) & 0 & -sin(\gamma)\\ 0 & 1 & 0\\ sin(\gamma) & 0 & cos(\gamma) \end{array} \right ]} Cy(γ)=⎣⎡cos(γ)0sin(γ)010−sin(γ)0cos(γ)⎦⎤ Cy(γ)=[cos(γ)0−sin(γ)010sin(γ)0cos(γ)]Cy(γ)=⎣⎡cos(γ)0sin(γ)amp;0amp;1amp;0amp;−sin(γ)amp;0amp;cos(γ)⎦⎤Cy(γ)=⎣⎡cos(γ)0sin(γ)010−sin(γ)0cos(γ)⎦⎤
同样的，还有绕z轴旋转 ψ ψ ψ ψ\psiψ ψψψ 的旋转矩阵，记作
C z ( ψ ) = [ c o s ( ψ ) − s i n ( ψ ) 0 s i n ( ψ ) c o s ( ψ ) 0001 ] C z ( ψ ) = [ c o s ( ψ ) a m p ; − s i n ( ψ ) a m p ; 0 s i n ( ψ ) a m p ; c o s ( ψ ) a m p ; 0 0 a m p ; 0 a m p ; 1 ] C z ( ψ ) = ⎣ ⎡ c o s ( ψ ) s i n ( ψ ) 0 − s i n ( ψ ) c o s ( ψ ) 0 001 ⎦ ⎤ Cz(ψ)=[cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001] C_{z_(\psi)}= {\left[ \begin{array}{ccc} cos(\psi) & -sin(\psi) & 0 \\ sin(\psi) & cos(\psi) & 0 \\ 0 & 0 & 1 \end{array} \right ]} Cz(ψ)=⎣⎡cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎦⎤ Cz(ψ)=[cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001]Cz(ψ)=⎣⎡cos(ψ)sin(ψ)0amp;−sin(ψ)amp;cos(ψ)amp;0amp;0amp;0amp;1⎦⎤Cz(ψ)=⎣⎡cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎦⎤
假如现在我们按ZXY的顺序，从地理坐标系 O n X n Y n Z n O n X n Y n Z n O n X n Y n Z n OnXnYnZnO_nX_nY_nZ_nOnXnYnZn OnXnYnZnOnXnYnZnOnXnYnZn旋转到机体坐标系 O b X b Y b Z b O b X b Y b Z b O b X b Y b Z b ObXbYbZbO_bX_bY_bZ_bObXbYbZb ObXbYbZbObXbYbZbObXbYbZb，整体旋转矩阵为
C n b = C y ( γ ) C x ( θ ) C z ( ψ ) = [ c o s ( γ ) c o s ( ψ ) + s i n ( γ ) s i n ( ψ ) s i n ( θ ) − c o s ( γ ) s i n ( ψ ) + s i n ( γ ) c o s ( ψ ) c o s ( θ ) − s i n ( ψ ) c o s ( θ ) s i n ( ψ ) c o s ( θ ) c o s ( ψ ) c o s ( θ ) s i n ( θ ) s i n ( γ ) c o s ( ψ ) − c o s ( γ ) s i n ( ψ ) s i n ( θ ) − s i n ( γ ) s i n ( ψ ) − c o s ( γ ) c o s ( ψ ) s i n ( θ ) c o s ( γ ) c o s ( θ ) ] C n b = C y ( γ ) C x ( θ ) C z ( ψ ) = [ c o s ( γ ) c o s ( ψ ) + s i n ( γ ) s i n ( ψ ) s i n ( θ ) a m p ; − c o s ( γ ) s i n ( ψ ) + s i n ( γ ) c o s ( ψ ) c o s ( θ ) a m p ; − s i n ( ψ ) c o s ( θ ) s i n ( ψ ) c o s ( θ ) a m p ; c o s ( ψ ) c o s ( θ ) a m p ; s i n ( θ ) s i n ( γ ) c o s ( ψ ) − c o s ( γ ) s i n ( ψ ) s i n ( θ ) a m p ; − s i n ( γ ) s i n ( ψ ) − c o s ( γ ) c o s ( ψ ) s i n ( θ ) a m p ; c o s ( γ ) c o s ( θ ) ] C n b = C y ( γ ) C x ( θ ) C z ( ψ ) = ⎣ ⎡ c o s ( γ ) c o s ( ψ ) + s i n ( γ ) s i n ( ψ ) s i n ( θ ) s i n ( ψ ) c o s ( θ ) s i n ( γ ) c o s ( ψ ) − c o s ( γ ) s i n ( ψ ) s i n ( θ ) − c o s ( γ ) s i n ( ψ ) + s i n ( γ ) c o s ( ψ ) c o s ( θ ) c o s ( ψ ) c o s ( θ ) − s i n ( γ ) s i n ( ψ ) − c o s ( γ ) c o s ( ψ ) s i n ( θ ) − s i n ( ψ ) c o s ( θ ) s i n ( θ ) c o s ( γ ) c o s ( θ ) ⎦ ⎤ Cnb=Cy(γ)Cx(θ)Cz(ψ)=[cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)−sin(ψ)cos(θ)sin(ψ)cos(θ)cos(ψ)cos(θ)sin(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)cos(γ)cos(θ)] C_n^b=C_{y_(\gamma)}C_{x_(\theta)}C_{z_(\psi)}= {\left[ \begin{array}{ccc} cos(\gamma)cos(\psi)+ sin(\gamma)sin(\psi)sin(\theta) & -cos(\gamma)sin(\psi)+sin(\gamma)cos(\psi)cos(\theta) & -sin(\psi)cos(\theta) \\ sin(\psi)cos(\theta)& cos(\psi)cos(\theta) & sin(\theta) \\ sin(\gamma)cos(\psi)- cos(\gamma)sin(\psi)sin(\theta) & -sin(\gamma)s in(\psi)-cos(\gamma)cos(\psi)sin(\theta) & cos(\gamma)cos(\theta) \end{array} \right ]} Cnb=Cy(γ)Cx(θ)Cz(ψ)=⎣⎡cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)sin(ψ)cos(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)cos(ψ)cos(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)−sin(ψ)cos(θ)sin(θ)cos(γ)cos(θ)⎦⎤ Cnb=Cy(γ)Cx(θ)Cz(ψ)=[cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)−sin(ψ)cos(θ)sin(ψ)cos(θ)cos(ψ)cos(θ)sin(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)cos(γ)cos(θ)]Cnb=Cy(γ)Cx(θ)Cz(ψ)=⎣⎡cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)sin(ψ)cos(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)amp;−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)amp;cos(ψ)cos(θ)amp;−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)amp;−sin(ψ)cos(θ)amp;sin(θ)amp;cos(γ)cos(θ)⎦⎤Cnb=Cy(γ)Cx(θ)Cz(ψ)=⎣⎡cos(γ)cos(ψ)+sin(γ)sin(ψ)sin(θ)sin(ψ)cos(θ)sin(γ)cos(ψ)−cos(γ)sin(ψ)sin(θ)−cos(γ)sin(ψ)+sin(γ)cos(ψ)cos(θ)cos(ψ)cos(θ)−sin(γ)sin(ψ)−cos(γ)cos(ψ)sin(θ)−sin(ψ)cos(θ)sin(θ)cos(γ)cos(θ)⎦⎤
注意一点，如果从n系到b系的旋转矩阵为 C n b C n b C n b CnbC_n^bCnb CnbCnbCnb，则从b系到n系的旋转矩阵为 C n b T C n b T C n b T CnbT{C_n^b}^TCnbT CnbTCnbTCnbT，显然有 C n b ∗ C n b T = E C n b ∗ C n b T = E C n b ∗ C n b T = E Cnb∗CnbT=EC_n^b*{C_n^b}^T=ECnb∗CnbT=E Cnb∗CnbT=ECnb∗CnbT=ECnb∗CnbT=E，所以旋转矩阵是正交矩阵
这个结论不能直接使用，一方面这个直接是不可解的，另一方面大量三角函数运算会导致大量资源占用。

3、四元数

四元数是一种超复数，由四个元构成： Q ( q 0 , q 1 , q 2 , q 3 ) = q 0 + q 1 i + q 2 i + q 3 k Q ( q 0 , q 1 , q 2 , q 3 ) = q 0 + q 1 i + q 2 i + q 3 k Q ( q 0 , q 1 , q 2 , q 3 ) = q 0 + q 1 i + q 2 i + q 3 k Q(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q_0,q_1,q_2,q_3)=q_0+q_1i+q_2i+q_3kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3k Q(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3k
关于四元数更详细的介绍可以参考四元数
利用四元数我们可以提出另一种描述空间矩阵的方法，具体可以参考秦永元的《惯性导航》和【教程】四旋翼飞行器姿态解算算法入门学习-Rick Grimes

（1）四元数和欧拉角的关系

注意这三个式子中， θ θ θ θ\thetaθ θθθ是绕Y轴转角， ϕ ϕ ϕ ϕ\phiϕ ϕϕϕ是绕X轴转角， ψ ψ ψ ψ\psiψ ψψψ是绕Z轴转角，和前文有所不同

按ZYX的顺序从地理坐标系 O e X e Y e Z e O e X e Y e Z e O e X e Y e Z e OeXeYeZeO_eX_eY_eZ_eOeXeYeZe OeXeYeZeOeXeYeZeOeXeYeZe旋转到机体坐标系 O b X b Y b Z b O b X b Y b Z b O b X b Y b Z b ObXbYbZbO_bX_bY_bZ_bObXbYbZb ObXbYbZbObXbYbZbObXbYbZb，整体旋转矩阵为
对应的四元数表示法为：
解算出的欧拉角为

（2）四元数的求解

飞行器姿态的改变，可以对应到旋转矩阵的改变，进一步对应到四元数的改变。实时姿态计算，实际上也就是实时更新四元数。
我们可以构建四元数关于时间的微分方程，来研究四元数关于时间的变化规律，求解该微分方程，即可解出四元数

1、建立微分方程

从四元数的三角表示法入手推到微分方程，具体推到过程可以参考秦永元的《惯性导航》
Q = c o s ( θ ) 2 + λ s i n ( θ ) 2 Q = c o s ( θ ) 2 + λ s i n ( θ ) 2 Q = 2 c o s ( θ ) + 2 λ s i n ( θ ) Q=cos(θ)2+λsin(θ)2Q=\frac{cos(\theta)}{2}+\frac{\lambda sin(\theta)}{2} Q=2cos(θ)+2λsin(θ) Q=cos(θ)2+λsin(θ)2Q=2cos(θ)+2λsin(θ)Q=2cos(θ)+2λsin(θ) d Q d t = 1 / 2 [ 0 − ω x − ω y − ω x ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 ] ⋅ [ q 0 q 1 q 2 q 3 ] d Q d t = 1 / 2 [ 0 a m p ; − ω x a m p ; − ω y a m p ; − ω x ω x a m p ; 0 a m p ; ω z a m p ; − ω y ω y a m p ; − ω z a m p ; 0 a m p ; ω x ω z a m p ; ω y a m p ; − ω x a m p ; 0 ] ⋅ [ q 0 q 1 q 2 q 3 ] d t d Q = 1 / 2 ⎣ ⎢ ⎢ ⎡ 0 ω x ω y ω z − ω x 0 − ω z ω y − ω y ω z 0 − ω x − ω x − ω y ω x 0 ⎦ ⎥ ⎥ ⎤ ⋅ ⎣ ⎢ ⎢ ⎡ q 0 q 1 q 2 q 3 ⎦ ⎥ ⎥ ⎤ dQdt=1/2[0−ωx−ωy−ωxωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]⋅[q0q1q2q3] \frac{dQ}{dt}=1/2 {\left[ \begin{array}{cccc} 0 & -\omega_x & -\omega_y & -\omega_x\\ \omega_x & 0 & \omega_z & -\omega_y\\ \omega_y & -\omega_z & 0 & \omega_x\\ \omega_z & \omega_y & -\omega_x & 0 \end{array} \right ]}· {\left[ \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3 \end{array} \right ]} dtdQ=1/2⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωx−ωyωx0⎦⎥⎥⎤⋅⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤ dQdt=1/2[0−ωx−ωy−ωxωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]⋅[q0q1q2q3]dtdQ=1/2⎣⎢⎢⎡0ωxωyωzamp;−ωxamp;0amp;−ωzamp;ωyamp;−ωyamp;ωzamp;0amp;−ωxamp;−ωxamp;−ωyamp;ωxamp;0⎦⎥⎥⎤⋅⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤dtdQ=1/2⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωx−ωyωx0⎦⎥⎥⎤⋅⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤ 记为 d Q d t = Φ ⋅ Q 记为 d Q d t = Φ ⋅ Q 记为 d t d Q = Φ ⋅ Q 记为dQdt=Φ⋅Q 记为 \frac{dQ}{dt}=\Phi·Q 记为dtdQ=Φ⋅Q 记为dQdt=Φ⋅Q记为dtdQ=Φ⋅Q记为dtdQ=Φ⋅Q

2、一阶龙格库塔法求解微分方程

一阶龙格库塔法是求解微分方程常用的工程方法，原理是把微分转化为微元增量，利用递推迭代的方法求解

设有微分方程 d y d x = f ( x , y ) d y d x = f ( x , y ) d x d y = f ( x , y ) dydx=f(x,y)\frac{dy}{dx}=f(x,y)dxdy=f(x,y) dydx=f(x,y)dxdy=f(x,y)dxdy=f(x,y)

求解y的迭代公式为 y ( λ + Δ λ ) = y ( λ ) + Δ λ ⋅ f ( x ( λ ) , y ( λ ) ) y ( λ + Δ λ ) = y ( λ ) + Δ λ ⋅ f ( x ( λ ) , y ( λ ) ) y ( λ + Δ λ ) = y ( λ ) + Δ λ ⋅ f ( x ( λ ) , y ( λ ) ) y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(\lambda+\Delta\lambda)=y(\lambda)+\Delta\lambda·f(x(\lambda),y(\lambda))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ)) y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))
对应到四元数微分方程：
Q ( t − Δ t ) = Q ( t ) + Δ t ⋅ Φ ( t ) ⋅ Q ( t ) Q ( t − Δ t ) = Q ( t ) + Δ t ⋅ Φ ( t ) ⋅ Q ( t ) Q ( t − Δ t ) = Q ( t ) + Δ t ⋅ Φ ( t ) ⋅ Q ( t ) Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t) Q(t-\Delta t)=Q(t)+\Delta t·\Phi(t)·Q(t) Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t) Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)Q(t−Δt)=Q(t)+Δt⋅Φ(t)⋅Q(t)
整理得
[ q 0 q 1 q 2 q 3 ] t + Δ t = [ q 0 q 1 q 2 q 3 ] t + 12 ⋅ Δ t ⋅ [ − ω x ⋅ q 1 − ω y ⋅ q 2 − ω z ⋅ q 3 ω x ⋅ q 0 − ω y ⋅ q 3 + ω z ⋅ q 2 ω x ⋅ q 3 + ω y ⋅ q 0 − ω z ⋅ q 1 − ω x ⋅ q 2 + ω y ⋅ q 1 + ω z ⋅ q 0 ] [ q 0 q 1 q 2 q 3 ] t + Δ t = [ q 0 q 1 q 2 q 3 ] t + 1 2 ⋅ Δ t ⋅ [ − ω x ⋅ q 1 − ω y ⋅ q 2 − ω z ⋅ q 3 ω x ⋅ q 0 − ω y ⋅ q 3 + ω z ⋅ q 2 ω x ⋅ q 3 + ω y ⋅ q 0 − ω z ⋅ q 1 − ω x ⋅ q 2 + ω y ⋅ q 1 + ω z ⋅ q 0 ] ⎣ ⎢ ⎢ ⎡ q 0 q 1 q 2 q 3 ⎦ ⎥ ⎥ ⎤ t + Δ t = ⎣ ⎢ ⎢ ⎡ q 0 q 1 q 2 q 3 ⎦ ⎥ ⎥ ⎤ t + 21 ⋅ Δ t ⋅ ⎣ ⎢ ⎢ ⎡ − ω x ⋅ q 1 − ω y ⋅ q 2 − ω z ⋅ q 3 ω x ⋅ q 0 − ω y ⋅ q 3 + ω z ⋅ q 2 ω x ⋅ q 3 + ω y ⋅ q 0 − ω z ⋅ q 1 − ω x ⋅ q 2 + ω y ⋅ q 1 + ω z ⋅ q 0 ⎦ ⎥ ⎥ ⎤ [q0q1q2q3]t+Δt=[q0q1q2q3]t+12⋅Δt⋅[−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0] {\left[ \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3 \end{array} \right ]}_{t+\Delta t}= {\left[ \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3 \end{array} \right ]}_{t}+ \frac{1}{2}· \Delta t· {\left[ \begin{array}{c} -\omega_x·q_1 -\omega_y·q_2 -\omega_z ·q_3\\ \omega_x·q_0 -\omega_y·q_3 +\omega_z ·q_2\\ \omega_x·q_3 +\omega_y·q_0 -\omega_z ·q_1\\ -\omega_x·q_2 +\omega_y·q_1 +\omega_z ·q_0 \end{array} \right ]} ⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21⋅Δt⋅⎣⎢⎢⎡−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0⎦⎥⎥⎤ [q0q1q2q3]t+Δt=[q0q1q2q3]t+12⋅Δt⋅[−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0]⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21⋅Δt⋅⎣⎢⎢⎡−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+Δt=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21⋅Δt⋅⎣⎢⎢⎡−ωx⋅q1−ωy⋅q2−ωz⋅q3ωx⋅q0−ωy⋅q3+ωz⋅q2ωx⋅q3+ωy⋅q0−ωz⋅q1−ωx⋅q2+ωy⋅q1+ωz⋅q0⎦⎥⎥⎤
这里 ω ω ω ω\omegaω ωωω就是三轴角速度，可以用陀螺仪直接测得

4、旋转矩阵中的一列

旋转矩阵中的一列有特殊意义，它代表原坐标系中一个方向的单位向量（X/Y/Z），在新坐标系中对应的向量值
用方向向量（如X方向 [ 1 , 0 , 0 ] T [ 1 , 0 , 0 ] T [ 1 , 0 , 0 ] T [1,0,0]T[1,0,0]^T[1,0,0]T [1,0,0]T[1,0,0]T[1,0,0]T）左乘旋转矩阵可以轻易看出，这里不写了

二、匿名四轴姿态解算分析

按Ano_Imu.c文件顺序进行分析

1、坐标系定义

匿名四轴姿态解算部分主要写在Ano_Imu.c中
匿名四轴程序中一共有三个坐标系：

地理坐标系，标记为w（官方定义如下）
机体坐标系，标记为a（官方定义如下）
航向坐标系，标记为h（专门解释一下，四旋翼飞行时，姿态角pitch和roll一般很小，在任意时刻，如果我们粗略地认为飞行器始终与底面平行（把飞机摆正，使pitch=roll=0），此时的机体坐标系即为航向坐标系）

2、坐标系转换

（1）匿名旋转坐标系

匿名的坐标系变换，是按是按ZYX顺序从地理坐标系转到机体坐标系的，这个旋转矩阵是前面提过的 (注意这三个式子中， θ θ θ θ\thetaθ θθθ是绕Y轴转角， ϕ ϕ ϕ ϕ\phiϕ ϕϕϕ是绕X轴转角， ψ ψ ψ ψ\psiψ ψψψ是绕Z轴转角)

C w a = [ c o s ( θ ) c o s ( ψ ) s i n ( ψ ) c o s ( θ ) − s i n ( θ ) s i n ( ϕ ) s i n ( θ ) c o s ( ψ ) − c o s ( ϕ ) s i n ( ψ ) s i n ( ϕ ) s i n ( θ ) s i n ( ψ ) + c o s ( ϕ ) c o s ( ψ ) s i n ( ϕ ) c o s ( θ ) c o s ( ϕ ) s i n ( θ ) c o s ( ψ ) + s i n ( ϕ ) s i n ( ψ ) c o s ( ϕ ) s i n ( θ ) s i n ( ψ ) − s i n ( ϕ ) c o s ( ψ ) c o s ( ϕ ) c o s ( θ ) ] C w a = [ c o s ( θ ) c o s ( ψ ) a m p ; s i n ( ψ ) c o s ( θ ) a m p ; − s i n ( θ ) s i n ( ϕ ) s i n ( θ ) c o s ( ψ ) − c o s ( ϕ ) s i n ( ψ ) a m p ; s i n ( ϕ ) s i n ( θ ) s i n ( ψ ) + c o s ( ϕ ) c o s ( ψ ) a m p ; s i n ( ϕ ) c o s ( θ ) c o s ( ϕ ) s i n ( θ ) c o s ( ψ ) + s i n ( ϕ ) s i n ( ψ ) a m p ; c o s ( ϕ ) s i n ( θ ) s i n ( ψ ) − s i n ( ϕ ) c o s ( ψ ) a m p ; c o s ( ϕ ) c o s ( θ ) ] C w a = ⎣ ⎡ c o s ( θ ) c o s ( ψ ) s i n ( ϕ ) s i n ( θ ) c o s ( ψ ) − c o s ( ϕ ) s i n ( ψ ) c o s ( ϕ ) s i n ( θ ) c o s ( ψ ) + s i n ( ϕ ) s i n ( ψ ) s i n ( ψ ) c o s ( θ ) s i n ( ϕ ) s i n ( θ ) s i n ( ψ ) + c o s ( ϕ ) c o s ( ψ ) c o s ( ϕ ) s i n ( θ ) s i n ( ψ ) − s i n ( ϕ ) c o s ( ψ ) − s i n ( θ ) s i n ( ϕ ) c o s ( θ ) c o s ( ϕ ) c o s ( θ ) ⎦ ⎤ Cwa=[cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)] C_w^a= {\left[ \begin{array}{ccc} cos(\theta)cos(\psi) & sin(\psi)cos(\theta) & -sin(\theta) \\ sin(\phi)sin(\theta)cos(\psi)- cos(\phi)sin(\psi) & sin(\phi)sin(\theta)sin(\psi)+cos(\phi)cos(\psi) & sin(\phi)cos(\theta) \\ cos(\phi)sin(\theta)cos(\psi)+ sin(\phi)sin(\psi) & cos(\phi)sin(\theta)sin(\psi)-sin(\phi)cos(\psi) & cos(\phi)cos(\theta) \end{array} \right ]} Cwa=⎣⎡cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)⎦⎤ Cwa=[cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)]Cwa=⎣⎡cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)amp;sin(ψ)cos(θ)amp;sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)amp;cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)amp;−sin(θ)amp;sin(ϕ)cos(θ)amp;cos(ϕ)cos(θ)⎦⎤Cwa=⎣⎡cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)⎦⎤

程序中用二维数组att_matrix表示从机体坐标系转到地理坐标系的旋转矩阵，是上面那个的转置
a t t m a t r i x = C a w = ( C w a ) T = [ c o s ( θ ) c o s ( ψ ) s i n ( ϕ ) s i n ( θ ) c o s ( ψ ) − c o s ( ϕ ) s i n ( ψ ) c o s ( ϕ ) s i n ( θ ) c o s ( ψ ) + s i n ( ϕ ) s i n ( ψ ) s i n ( ψ ) c o s ( θ ) s i n ( ϕ ) s i n ( θ ) s i n ( ψ ) + c o s ( ϕ ) c o s ( ψ ) c o s ( ϕ ) s i n ( θ ) s i n ( ψ ) − s i n ( ϕ ) c o s ( ψ ) − s i n ( θ ) s i n ( ϕ ) c o s ( θ ) c o s ( ϕ ) c o s ( θ ) ] a t t _ m a t r i x = C a w = ( C w a ) T = [ c o s ( θ ) c o s ( ψ ) a m p ; s i n ( ϕ ) s i n ( θ ) c o s ( ψ ) − c o s ( ϕ ) s i n ( ψ ) a m p ; c o s ( ϕ ) s i n ( θ ) c o s ( ψ ) + s i n ( ϕ ) s i n ( ψ ) s i n ( ψ ) c o s ( θ ) a m p ; s i n ( ϕ ) s i n ( θ ) s i n ( ψ ) + c o s ( ϕ ) c o s ( ψ ) a m p ; c o s ( ϕ ) s i n ( θ ) s i n ( ψ ) − s i n ( ϕ ) c o s ( ψ ) − s i n ( θ ) a m p ; s i n ( ϕ ) c o s ( θ ) a m p ; c o s ( ϕ ) c o s ( θ ) ] a t t m a t r i x = C a w = ( C w a ) T = ⎣ ⎡ c o s ( θ ) c o s ( ψ ) s i n ( ψ ) c o s ( θ ) − s i n ( θ ) s i n ( ϕ ) s i n ( θ ) c o s ( ψ ) − c o s ( ϕ ) s i n ( ψ ) s i n ( ϕ ) s i n ( θ ) s i n ( ψ ) + c o s ( ϕ ) c o s ( ψ ) s i n ( ϕ ) c o s ( θ ) c o s ( ϕ ) s i n ( θ ) c o s ( ψ ) + s i n ( ϕ ) s i n ( ψ ) c o s ( ϕ ) s i n ( θ ) s i n ( ψ ) − s i n ( ϕ ) c o s ( ψ ) c o s ( ϕ ) c o s ( θ ) ⎦ ⎤ att_matrix=Caw=(Cwa)T=[cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)] att\_matrix=C_a^w=(C_w^a)^T= {\left[ \begin{array}{ccc} cos(\theta)cos(\psi) & sin(\phi)sin(\theta)cos(\psi)- cos(\phi)sin(\psi) &cos(\phi)sin(\theta)cos(\psi)+ sin(\phi)sin(\psi) \\ sin(\psi)cos(\theta) &sin(\phi)sin(\theta)sin(\psi)+cos(\phi)cos(\psi) & cos(\phi)sin(\theta)sin(\psi)-sin(\phi)cos(\psi) \\ -sin(\theta) &sin(\phi)cos(\theta) & cos(\phi)cos(\theta) \end{array} \right ]} att_matrix=Caw=(Cwa)T=⎣⎡cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)⎦⎤ attmatrix=Caw=(Cwa)T=[cos(θ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)sin(ψ)cos(θ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)−sin(θ)sin(ϕ)cos(θ)cos(ϕ)cos(θ)]att_matrix=Caw=(Cwa)T=⎣⎡cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)amp;sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)amp;sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)amp;sin(ϕ)cos(θ)amp;cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)amp;cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)amp;cos(ϕ)cos(θ)⎦⎤attmatrix=Caw=(Cwa)T=⎣⎡cos(θ)cos(ψ)sin(ψ)cos(θ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)+cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)⎦⎤
注意这里绕Y轴转角 θ θ θ θ\thetaθ θθθ (pitch)，绕X轴转角 ϕ ϕ ϕ ϕ\phiϕ ϕϕϕ (roll)都是很小的角

（2）相关代码分析

pid控制器实现互补滤波原理相关推荐

姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）
互补滤波原理: 在四轴入门理论知识那节我们说,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系.陀 ...
PID控制器开发笔记之一：PID算法原理及基本实现
在自动控制中,PID及其衍生出来的算法是应用最广的算法之一.各个做自动控制的厂家基本都有会实现这一经典算法.我们在做项目的过程中,也时常会遇到类似的需求,所以就想实现这一算法以适用于 ...
PID控制器原理详解
简介: 什么是 PID? 它是一种在编程中使用的基本方法,如果正确调整,可以令人难以置信的有效和准确,PID代表比例积分微分,3个单独的部分连接在一起,虽然有时你不需要三个都使用.例如,您可以改为有P ...
四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
对比直立车模控制中的互补滤波、Karlman滤波和参考滤波方案
前几天,有参加智能车竞赛的同学在公众号上询问一个关于直立车模控制中角度融合算法的问题.感觉这是所有初次准备智能车模竞赛的同学都会碰到的问题. 卓大大,请问您可以针对卡尔曼滤波.互补滤波.清华滤波的原理 ...
Pixhawk-姿态解算-互补滤波
版权声明:本文为博主原创文章,未经博主允许不得转载. 目录(?)[+] 位置型PID的C语言实现增量型PID的C语言实现积分分离的PID控制算法C语言实现变积分的PID控制算法C语言实现 ...
PID控制器笔记（附上简单的demo）
PID控制器简介在过程控制中,按偏差的比例(P).积分(I)和微分(D)进行控制的PID控制器(亦称PID调节器)是应用最为广泛的一种自动控制器.它具有原理简单,易于实现,适用面广,控制参数相互独立 ...
实验三、数字PID控制器的设计
实验三.数字PID控制器的设计 --- 直流闭环调速实验一.实验目的 1．理解晶闸管直流单闭环调速系统的数学模型和工作原理;. 2. 掌握PID控制器参数对控制系统性能的影响; 3. 能够运用MAT ...
PID控制器算法-Simulink仿真
PID控制算法概述上图是一个闭环控制系统地框图: 假设是调试一个电机的速度,上图的r(t)是目标速度,y(t)是速度输出量,e(t)是速度误差,u(t)是PID计算后发送给电机的输出值,被控对象是 ...

pid控制器实现互补滤波原理