淺議四軸飛行器的Matlab實(shí)時(shí)姿態(tài)顯示

謝兆清

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

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

一、matlab串口初始化

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

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

二、四元素表示的坐標(biāo)變化和四元素與歐拉角的關(guān)系

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

可以寫為

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

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

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

四 、結(jié)束語

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

參 考 文 獻(xiàn)

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

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