謝兆清

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

【關鍵字】 四軸飛行器 姿態(tài)顯示

一、matlab串口初始化

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

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

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

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

可以寫為

三、matlab界面姿態(tài)顯示

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

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

四 、結束語

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

參 考 文 獻

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

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