四旋翼姿態解算原理

姿態結算原理

旋轉矩陣的建立

如果要進行四旋翼姿態分析，那麼需要進行座標系與座標系之間的轉換，地理座標系與四旋翼座標系之間的聯繫如圖2-6所示。四旋翼(Quad-rotor)分別繞三個軸轉動的角度變化如圖2-7 所示。

圖2-6 機體座標與地面座標的關係

                                                

圖2-7 滾轉角、俯仰角、偏航角示意圖

根據圖2-6、圖2-7中的關係，建立如下矩陣：

                 

                  

如向量P 依次繞Z、Y、X旋轉，那麼旋轉後的向量與之前的向量的關係爲：

                     

其中，就是地理與機體座標之間的旋轉關係，如下： 

    

四元數計算法

四元數在四旋翼姿態角計算中用的比較多，利用四元數計算姿態角較簡單，同時，利用四元數解算姿態角可以避免萬向節死鎖。四元數的表示方法如下：

              

    其中，theta/2是旋轉的角度，n是四元數表示的旋轉軸的方向，q₀是實部，其餘的是不同方向的虛部。且表示的三個方向都相互垂直。在四元數中，可以用兩個向量的乘積表示一個四元數向量繞另一個向量旋轉。

設四元數A和B，如果要表示A以B爲轉軸旋轉後的結果，則可以表示爲四元數A和B的乘積。表示方法如下：

    設一個四元數Q沒有實部,且是單位四元數。則有|Q|=1，。如果四元數R(x,y,z)以Q爲轉軸旋轉，那麼旋轉後的向量可表示爲，,也就是。旋轉後的向量的計算過程如下：

       

    設四元數中的旋轉矩陣是R_q ，那麼R_q就可以表示爲：

  又由

則有

 

                

可得

   由公式（2-16）、（2-17）和（2-18）可得姿態角通過四元數進行計算的公式。對四元數關於時間微分，可以得到四元數與四旋翼角速度相關的更新公式，四元數更新後，進行姿態角計算。在這裏本文不進行推導直接給出計算公式如下：

      

.3 互補誤差融合

由於陀螺儀長時時間工作存在較大的漂移，但是陀螺儀短時間內測得的數據比較準確，而加速度計長時間內測得的數據比較準確，所以利用四旋翼加速度與四旋翼的重力加速度的差作爲陀螺儀測得的角速度的補償量,進行姿態誤差補償。設四旋翼座標系上的重力加速度爲，加速度計測得的加速度爲，計算過程如下：

 

則可得：

             

融合一定的PI參數，將融合後的誤差對角速度進行補償，如下：