四軸飛行器及PID控制算法的介紹

　　從陀螺儀器的三軸角速度通過四元數(shù)法得到俯仰，航偏，滾轉(zhuǎn)角，這是快速解算，結(jié)合三軸地磁和三軸加速度得到漂移補償和深度解算。

　　姿態(tài)的數(shù)學模型坐標系

　　姿態(tài)解算需要解決的是四軸飛行器和地球的相對姿態(tài)問題。地理坐標系是固定不變的，正北，正東，正上構(gòu)成了坐標系的X，Y，Z軸用坐標系R表示，飛行器上固定一個坐標系用r表示，那么我們就可以適用歐拉角，四元數(shù)等來描述r和R的角位置關(guān)系。

　　姿態(tài)的數(shù)學表示

　　姿態(tài)有多種數(shù)學表示方式，常見的是四元數(shù)，歐拉角，矩陣和軸角。在四軸飛行器中使用到了四元數(shù)和歐拉角，姿態(tài)解算的核心在于旋轉(zhuǎn)。姿態(tài)解算中使用四元數(shù)來保存飛行器的姿態(tài)，包括旋轉(zhuǎn)和方位。在獲得四元數(shù)之后，會將其轉(zhuǎn)化為歐拉角，然后輸入到姿態(tài)控制算法中。姿態(tài)控制算法的輸入參數(shù)必須要是歐拉角。AD值是指MPU6050的陀螺儀和加速度值，3個維度的陀螺儀值和3個維度的加速度值，每個值為16位精度。AD值必須先轉(zhuǎn)化為四元數(shù)，然后通過四元數(shù)轉(zhuǎn)化為歐拉角。在四軸上控制流程如下圖：

　　下面是用四元數(shù)表示飛行姿態(tài)的數(shù)學公式，從MPU6050中采集的數(shù)據(jù)經(jīng)過下面的公式計算就可以轉(zhuǎn)換成歐拉角，傳給姿態(tài)PID控制器中進行姿態(tài)控制。