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

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

文章目录

  • 一、姿态解算原理相关
    • 1、简介
    • 2、坐标变换和旋转矩阵
    • 3、四元数
      • (1)四元数和欧拉角的关系
      • (2)四元数的求解
        • 1、建立微分方程
        • 2、一阶龙格库塔法求解微分方程
    • 4、旋转矩阵中的一列
  • 二、匿名四轴姿态解算分析
    • 1、坐标系定义
    • 2、坐标系转换
      • (1)匿名旋转坐标系
      • (2)相关代码分析
    • 3、IMU_update函数分析
      • (1)整体分析
      • (2)互补滤波原理相关
      • (3)互补滤波相关代码分析
    • 4、姿态角输出函数函数calculate_RPY()
  • 三、参考

一、姿态解算原理相关

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_nOn​Xn​Yn​Zn​ 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_bOb​Xb​Yb​Zb​ 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_AOA​XA​YA​ZA​ 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_BOB​XB​YB​ZB​ OBXBYBZBOBXBYBZBOBXBYBZB
pid控制器实现互补滤波原理_第1张图片
对于原系 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_AOA​XA​YA​ZA​ 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_BOB​XB​YB​ZB​ 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​​=rxA​​ryB​​=cos(θ)ryA​​+sin(θ)rzA​​rzB​​=−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 ]} ⎣⎡​rxB​​ryB​​rzB​​​⎦⎤​=⎣⎡​100​0cos(θ)−sin(θ)​0sin(θ)cos(θ)​⎦⎤​⎣⎡​rxA​​ryA​​rzA​​​⎦⎤​ [rxBryBrzB]=[1000cos(θ)sin(θ)0sin(θ)cos(θ)][rxAryArzA]rxBryBrzB=100amp;0amp;cos(θ)amp;sin(θ)amp;0amp;sin(θ)amp;cos(θ)rxAryArzArxBryBrzB=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(​θ)​=⎣⎡​100​0cos(θ)−sin(θ)​0sin(θ)cos(θ)​⎦⎤​ Cx(θ)=[1000cos(θ)sin(θ)0sin(θ)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(γ)0sin(γ)010sin(γ)0cos(γ)]Cy(γ)=cos(γ)0sin(γ)amp;0amp;1amp;0amp;sin(γ)amp;0amp;cos(γ)Cy(γ)=cos(γ)0sin(γ)010sin(γ)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(ψ)0​001​⎦⎤​ Cz(ψ)=[cos(ψ)sin(ψ)0sin(ψ)cos(ψ)0001]Cz(ψ)=cos(ψ)sin(ψ)0amp;sin(ψ)amp;cos(ψ)amp;0amp;0amp;0amp;1Cz(ψ)=cos(ψ)sin(ψ)0sin(ψ)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_nOn​Xn​Yn​Zn​ 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_bOb​Xb​Yb​Zb​ 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}^TCnb​T 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​∗Cnb​T=E CnbCnbT=ECnbCnbT=ECnbCnbT=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​+q1​i+q2​i+q3​k 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轴转角,和前文有所不同
  1. 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_eOe​Xe​Ye​Ze​ 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_bOb​Xb​Yb​Zb​ ObXbYbZbObXbYbZbObXbYbZb,整体旋转矩阵为
    在这里插入图片描述
  2. 对应的四元数表示法为:
    在这里插入图片描述
  3. 解算出的欧拉角为
    pid控制器实现互补滤波原理_第2张图片

(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​​−ωx​0−ωz​ωy​​−ωy​ωz​0−ωx​​−ωx​−ωy​ωx​0​⎦⎥⎥⎤​⋅⎣⎢⎢⎡​q0​q1​q2​q3​​⎦⎥⎥⎤​ dQdt=1/2[0ωxωyωxωx0ωzωyωyωz0ωxωzωyωx0][q0q1q2q3]dtdQ=1/20ωxωyωzamp;ωxamp;0amp;ωzamp;ωyamp;ωyamp;ωzamp;0amp;ωxamp;ωxamp;ωyamp;ωxamp;0q0q1q2q3dtdQ=1/20ωxωyωzωx0ωzωyωyωz0ωxωxωyωx0q0q1q2q3 记 为 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=ΦQdtdQ=ΦQdtdQ=Φ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 ]} ⎣⎢⎢⎡​q0​q1​q2​q3​​⎦⎥⎥⎤​t+Δt​=⎣⎢⎢⎡​q0​q1​q2​q3​​⎦⎥⎥⎤​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[ωxq1ωyq2ωzq3ωxq0ωyq3+ωzq2ωxq3+ωyq0ωzq1ωxq2+ωyq1+ωzq0]q0q1q2q3t+Δt=q0q1q2q3t+21Δtωxq1ωyq2ωzq3ωxq0ωyq3+ωzq2ωxq3+ωyq0ωzq1ωxq2+ωyq1+ωzq0q0q1q2q3t+Δt=q0q1q2q3t+21Δtωxq1ωyq2ωzq3ωxq0ωyq3+ωzq2ωxq3+ωyq0ωzq1ωxq2+ωyq1+ωzq0
    这里 ω ω ω ω\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
  • 匿名四轴程序中一共有三个坐标系:
  1. 地理坐标系,标记为w(官方定义如下)
  2. 机体坐标系,标记为a(官方定义如下)
    pid控制器实现互补滤波原理_第3张图片
  3. 航向坐标系,标记为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)相关代码分析

#

你可能感兴趣的:(四轴飞控)