淺議四軸飛行器的Matlab實時姿態顯示

謝兆清

【摘要】 近年四軸飛行器發展相當迅速.越來越多的人開始關注并研究四軸飛行器。本文將分析一種常見的四軸飛行器姿態解算方法并用matlab實現姿態顯示。希望能給學習四軸飛行器的朋友們帶來幫助。

【關鍵字】 四軸飛行器 姿態顯示

一、matlab串口初始化

matlab串口采用回調函數，類似于中斷方式。但是mtalb的串口沒有多線程，而我們在中斷方式里面需要進行波形顯示，四元素旋轉等各種數據操作，這就導致我們的數據平率不能很高。matlab采用符號‘！為結束符，碰到這個符號matlab就會調用回調函數，中間的數據都是逗號隔開的，數據順序一次為accex，accey，accez，temp，gyrox，gyroy，gyroz，c pu_major，q0，q1，q2，q3發送，數據通過sprintf進行格式化，然后通過rt_kprintf函數發送。temp是MPU6050讀出的溫度數，cpu_major是CPU使用率，q0，q1，q2，q3分別對應四元素的四個參數，q0是實數，其他分別對應i，j，k的參數。

matlab數據處理：收到數據后，標準的處理方式是用matlab的regexp函數，用正則表達式將數據讀取出來.將數據中的空格、結束符感嘆號、間隔符逗號，去掉后調用str2num函數將字符串轉換為數字就行。

二、四元素表示的坐標變化和四元素與歐拉角的關系

當獲取MPU6050的數據后，一是加速度，一是角速度。有了加速度我們就可以算出pitch和roll。因為飛機是動態的，飛機動的過程中自己本身機體也會產生加速度，所以我們要分辨地球重力產生的重力加速度和機體的加速度才能算出集體的pitch和roll。加速度在震動情況下輸出的值是波動很大的，而陀螺在動態下輸出就好很多。陀螺輸出的是角速度，我們用時間乘以角速度就可以的得到角度，每次積分的角度和上次的角度相加，就可以達到集體的xyz三個軸的角度。雖然積分的時候可以使用龍格-庫塔積分方法，但是角速度積分仍然存在誤差，這時我們要短期相信陀螺儀，然后長期相信加速度.其中w，x，y，z就是四元素的四個元素，W為實數部分，xyz對應ijk的三個變量。四元素的微分方程：

可以寫為

三、matlab界面姿態顯示

添加控制四個電機的pwm數值現實和pid控制器中yaw pitch roll目標值的顯示， PID的控制效果和對齊就可以顯示出來了。如圖1。

左邊中間兩個方框，左邊那個33.76是mpu6050讀出來的溫度數值，右邊的7是代表CPU使用率為7%。

四 、結束語

飛控數據采集線程和數據處理線程的安排，類似于生產者與消費者的關系。生產者只管采集數據，消費者消費數據.我們有兩個變量互斥量和計數來同步兩個任務。生產者只管統計數據.但是需要做些處理，拿到數據后對數據均值濾波，就是除以統計的次數，同時將原來的數值清0。互斥量的存在，可以保證數據操作的同步和安全性。

參 考 文 獻

[1] 王俊； 魯曉天.對四軸飛行器基于姿態算法的分析與實現 [J].河南科技，2015-01-25

[2] 徐云川.四軸飛行器姿態解算算法設計與仿真 [J].科技視界，2016-08-15.