pid控制器實現互補濾波原理

轉載自：https://blog.csdn.net/wxc971231/article/details/97449026

一、姿態解算原理相關

1、簡介

• 對於四旋翼無人機來說，有兩個相關座標系，即地理座標系$OnXnYnZnO_nX_nY_nZ_nOn​Xn​Yn​Zn​$和機體座標系$ObXbYbZbO_bX_bY_bZ_bOb​Xb​Yb​Zb​$。
• 姿態角的檢測對於四旋翼控制是至關重要的。比如在做飛行器姿態控制的時候，通常使用串級pid的方法，需要得到飛行器姿態角（即歐拉角）來做角度位置環反饋。再如利用裝置在機腹垂直向下拍攝的攝像頭，進行機體視覺定位時，也需要靠姿態角進行視覺反饋補償。
• 要解算姿態角，就要研究從地理座標系到機體座標系的轉換過程。這個轉換過程不是唯一的，比如可以先繞X軸旋轉$θ\thetaθ$，再繞Y軸旋轉$γ\gammaγ$，最後繞Z軸旋轉$ψ\psiψ$，這樣得到的一組姿態角稱爲卡爾丹角；也可以按ZYX的順序旋轉，這樣得到的一組姿態角稱爲歐拉角（這是最常用的，pitch,roll,yaw）。不管按照什麼順序，得到的角度都可以稱作廣義歐拉角。實際理論分析時，旋轉順序不是很重要，這個順序會影響四元數與歐拉角的關係，但是都可以進行解算。

2、座標變換和旋轉矩陣

下面從最基本的座標變換入手開始講解，請看如下座標系旋轉：$OAXAYAZAO_AX_AY_AZ_AOA​XA​YA​ZA​$繞OX旋轉$θ\thetaθ$到$OBXBYBZBO_BX_BY_BZ_BOB​XB​YB​ZB​$

對於原系$OAXAYAZAO_AX_AY_AZ_AOA​XA​YA​ZA​$中的一個向量$[rxA,ryA,rzA][r_{x_A},r_{y_A},r_{z_A}][rxA​​,ryA​​,rzA​​]$，轉換到新系$OBXBYBZBO_BX_BY_BZ_BOB​XB​YB​ZB​$中的向量$[rxB,ryB,rzB][r_{x_B},r_{y_B},r_{z_B}][rxB​​,ryB​​,rzB​​]$，有：

$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​​$

可以用旋轉矩陣表示

$[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​​​⎦⎤​$

• 中間的就是 繞x軸旋轉$θ\thetaθ$ 的旋轉矩陣，記作
  $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(θ)​⎦⎤​$

• 同樣的，還有 繞y軸旋轉$γ\gammaγ$ 的旋轉矩陣，記作
  $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(γ)​⎦⎤​$

• 同樣的，還有 繞z軸旋轉$ψ\psiψ$ 的旋轉矩陣，記作
  $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​⎦⎤​$

• 假如現在我們按ZXY的順序，從地理座標系$OnXnYnZnO_nX_nY_nZ_nOn​Xn​Yn​Zn​$旋轉到機體座標系$ObXbYbZbO_bX_bY_bZ_bOb​Xb​Yb​Zb​$，整體旋轉矩陣爲
  $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(θ)​⎦⎤​$

• 注意一點，如果從n繫到b系的旋轉矩陣爲$CnbC_n^bCnb​$，則從b繫到n系的旋轉矩陣爲$CnbT{C_n^b}^TCnb​T$，顯然有$Cnb∗CnbT=EC_n^b*{C_n^b}^T=ECnb​∗Cnb​T=E$，所以旋轉矩陣是正交矩陣

• 這個結論不能直接使用，一方面這個直接是不可解的，另一方面大量三角函數運算會導致大量資源佔用。

3、四元數

• 四元數是一種超複數，由四個元構成：$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$
• 關於四元數更詳細的介紹可以參考四元數
• 利用四元數我們可以提出另一種描述空間矩陣的方法，具體可以參考秦永元的《慣性導航》和【教程】四旋翼飛行器姿態解算算法入門學習-Rick Grimes

（1）四元數和歐拉角的關係

• 注意這三個式子中，$θ\thetaθ$是繞Y軸轉角，$ϕ\phiϕ$是繞X軸轉角，$ψ\psiψ$是繞Z軸轉角，和前文有所不同

1. 按ZYX的順序從地理座標系$OeXeYeZeO_eX_eY_eZ_eOe​Xe​Ye​Ze​$旋轉到機體座標系$ObXbYbZbO_bX_bY_bZ_bOb​Xb​Yb​Zb​$，整體旋轉矩陣爲
   
2. 對應的四元數表示法爲：
   
3. 解算出的歐拉角爲
   

（2）四元數的求解

• 飛行器姿態的改變，可以對應到旋轉矩陣的改變，進一步對應到四元數的改變。實時姿態計算，實際上也就是實時更新四元數。
• 我們可以構建四元數關於時間的微分方程，來研究四元數關於時間的變化規律，求解該微分方程，即可解出四元數

1、建立微分方程

• 從四元數的三角表示法入手推到微分方程，具體推到過程可以參考秦永元的《慣性導航》
  $Q=cos(θ)2+λsin(θ)2Q=\frac{cos(\theta)}{2}+\frac{\lambda sin(\theta)}{2} Q=2cos(θ)​+2λsin(θ)​$$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=Φ⋅Q 記爲 \frac{dQ}{dt}=\Phi·Q 記爲dtdQ​=Φ⋅Q$

2、一階龍格庫塔法求解微分方程

一階龍格庫塔法是求解微分方程常用的工程方法，原理是把微分轉化爲微元增量，利用遞推迭代的方法求解

• 設有微分方程$dydx=f(x,y)\frac{dy}{dx}=f(x,y)dxdy​=f(x,y)$

  求解y的迭代公式爲$y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))y(\lambda+\Delta\lambda)=y(\lambda)+\Delta\lambda·f(x(\lambda),y(\lambda))y(λ+Δλ)=y(λ)+Δλ⋅f(x(λ),y(λ))$

• 對應到四元數微分方程：
  $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)$
  整理得
  $[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​​⎦⎥⎥⎤​$
  這裏$ω\omegaω$就是三軸角速度，可以用陀螺儀直接測得

4、旋轉矩陣中的一列

• 旋轉矩陣中的一列有特殊意義，它代表原座標系中一個方向的單位向量（X/Y/Z），在新座標系中對應的向量值
• 用方向向量（如X方向$[1,0,0]T[1,0,0]^T[1,0,0]T$）左乘旋轉矩陣可以輕易看出，這裏不寫了

二、匿名四軸姿態解算分析

按Ano_Imu.c文件順序進行分析

1、座標系定義

• 匿名四軸姿態解算部分主要寫在Ano_Imu.c中
• 匿名四軸程序中一共有三個座標系：

1. 地理座標系，標記爲w（官方定義如下）
2. 機體座標系，標記爲a（官方定義如下）
   
3. 航向座標系，標記爲h（專門解釋一下，四旋翼飛行時，姿態角pitch和roll一般很小，在任意時刻，如果我們粗略地認爲飛行器始終與底面平行（把飛機擺正，使pitch=roll=0），此時的機體座標系即爲航向座標系）

2、座標系轉換

（1）匿名旋轉座標系

• 匿名的座標系變換，是按是按ZYX順序從地理座標系轉到機體座標系的，這個旋轉矩陣是前面提過的 (注意這三個式子中，$θ\thetaθ$是繞Y軸轉角，$ϕ\phiϕ$是繞X軸轉角，$ψ\psiψ$是繞Z軸轉角)

$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(θ)​⎦⎤​$

• 程序中用二維數組att_matrix表示從機體座標系轉到地理座標系的旋轉矩陣，是上面那個的轉置
  $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(θ)​⎦⎤​$
• 注意這裏繞Y軸轉角$θ\thetaθ$ (pitch)，繞X軸轉角$ϕ\phiϕ$ (roll)都是很小的角

（2）相關代碼分析

#