四軸飛行器1.4 姿態(tài)解算和Matlab實(shí)時(shí)姿態(tài)顯示

MPU6050數(shù)據(jù)讀取出來(lái)后，經(jīng)過(guò)一個(gè)星期的努力，姿態(tài)解算和在matlab上的實(shí)時(shí)顯示姿態(tài)終于完成了。

1：完成matlab的串口，并且實(shí)時(shí)通過(guò)波形顯示數(shù)據(jù)

2：添加RTT查看CPU使用率的擴(kuò)展功能，MPU6050讀取數(shù)據(jù)的優(yōu)化

3：四元素表示的坐標(biāo)變化，四元素與歐拉角的關(guān)系和Madgwick的IMUupdate算法

4：飛控?cái)?shù)據(jù)采集線程和數(shù)據(jù)處理線程的安排，類似于生產(chǎn)者與消費(fèi)者的關(guān)系。

?先放個(gè)效果視頻。。。如果看不了視頻，請(qǐng)打開(kāi)視屏網(wǎng)址：http://v.youku.com/v_show/id_XNzU3MTk0MTAw.html

1：matlab串口初始化還是比較簡(jiǎn)單的，網(wǎng)上的資料也很多，這里就直接貼初始化代碼了。

1 % --- Executes on button press in pb_OpenSerialPort. 2 function pb_OpenSerialPort_Callback(hObject, eventdata, handles) 3 % hObject handle to pb_OpenSerialPort (see GCBO) 4 % eventdata reserved - to be defined in a future version of MATLAB 5 % handles structure with handles and user data (see GUIDATA) 6 % 7 global o_SerialPort; 8 %______________________________________________ 9 %GUI全局變量 10 11 12 %---------------------串口初始化----------------------- 13 %%%COM端口初始化 14 int_Index_COM=get(handles.pop_SerialPort,'Value'); 15 string_COM=get(handles.pop_SerialPort,'String'); 16 string_Select_COM=string_COM{int_Index_COM}; 17 o_SerialPort=serial(string_Select_COM); 18 %%%Baud初始化 19 int_Index_Baud=get(handles.pop_BaudRate,'Value'); 20 string_Baud=get(handles.pop_BaudRate,'String'); 21 string_Select_Baud=string_Baud{int_Index_Baud}; 22 double_Baud=str2double(string_Select_Baud); 23 set(o_SerialPort,'BaudRate',double_Baud); 24 %%%設(shè)置數(shù)據(jù)長(zhǎng)度 25 int_Index_DataBit=get(handles.pop_DataBit,'Value'); 26 string_DataBit=get(handles.pop_DataBit,'String'); 27 string_Select_DataBit=string_DataBit(int_Index_DataBit); 28 double_DataBit=str2double(string_Select_DataBit); 29 set(o_SerialPort,'DataBits',double_DataBit); 30 %%%設(shè)置停止位長(zhǎng)度 31 int_Index_StopBits=get(handles.pop_StopBits,'Value'); 32 string_StopBits=get(handles.pop_StopBits,'String'); 33 string_Select_StopBits=string_StopBits(int_Index_StopBits); 34 double_StopBits=str2double(string_Select_StopBits); 35 set(o_SerialPort,'StopBits',double_StopBits); 36 %%%設(shè)置輸入緩沖區(qū)大小為1M 37 set(o_SerialPort,'InputBufferSize',1024000); 38 %%%串口事件回調(diào)設(shè)置 39 40 set(o_SerialPort,'BytesAvailableFcnMode','terminator'); 41 set(o_SerialPort,'terminator','!'); %!標(biāo)識(shí)結(jié)束符結(jié)束，方便處理和讀取數(shù)據(jù) 42 43 o_SerialPort.BytesAvailableFcn={@EveBytesAvailableFcn,handles}; 44 % ----------------------打開(kāi)串口----------------------- 45 fopen(o_SerialPort);

? ? ? matlab串口我們采用回調(diào)函數(shù)，類似于中斷方式哈，但是mtalb的串口十分的不好用哈，沒(méi)有多線程，而我們?cè)谥袛嗬锩嫘枰M(jìn)行波形顯示，四元素旋轉(zhuǎn)等各種數(shù)據(jù)操作，是需要花費(fèi)點(diǎn)時(shí)間的，這就導(dǎo)致我們的數(shù)據(jù)平率不能很高。。當(dāng)上傳的速率達(dá)到100hz以后，就會(huì)出錯(cuò)了。。50hz也不穩(wěn)定。。這個(gè)實(shí)在是有點(diǎn)。。。擔(dān)心以后的系統(tǒng)辨識(shí)和慣性導(dǎo)航的數(shù)據(jù)處理了。。頭疼。。。

? ? ? matlab采用符號(hào)‘！’為結(jié)束符，碰到這個(gè)符號(hào)matlab就會(huì)調(diào)用回調(diào)函數(shù)，中間的數(shù)據(jù)都是逗號(hào)隔開(kāi)的，數(shù)據(jù)順序一次為accex,accey,accez,temp,gyrox,gyroy,gyroz,cpu_major,q0,q1,q2,q3發(fā)送，數(shù)據(jù)通過(guò)sprintf進(jìn)行格式化，然后通過(guò)rt_kprintf函數(shù)發(fā)送，

1 sprintf(buffer,"\n%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7d,%7.2f,%7.2f,%7.2f,%7.2f!", \ 2 mpu6050_data_tf->acce_x,mpu6050_data_tf->acce_y,mpu6050_data_tf->acce_z,\ 3 mpu6050_data_tf->temp,mpu6050_data_tf->gyro_x,mpu6050_data_tf->gyro_y,\ 4 mpu6050_data_tf->gyro_z,(s8)cpu_major,q0,q1,q2,q3); 5 rt_kprintf("%s",buffer);

?temp是MPU6050讀出的溫度數(shù)，cpu_major是CPU使用率，q0,q1,q2,q3分別對(duì)應(yīng)四元素的四個(gè)參數(shù)，q0是實(shí)數(shù)，其他分別對(duì)應(yīng)i,j,k的參數(shù)。

? matlab數(shù)據(jù)處理:收到數(shù)據(jù)后，其實(shí)標(biāo)準(zhǔn)的處理方式是用matlab的regexp函數(shù)，用正則表達(dá)式將數(shù)據(jù)讀取出來(lái)，我們沒(méi)有用這個(gè)，上傳數(shù)據(jù)格式我們自己可以控制，所以處理起來(lái)很簡(jiǎn)單，沒(méi)必要用到復(fù)雜的正則表達(dá)式，而且正則表達(dá)式處理時(shí)間應(yīng)該比我們自己簡(jiǎn)單的處理方法的時(shí)間要長(zhǎng)，所以采用簡(jiǎn)單的處理方法。處理方法是先將數(shù)據(jù)中的空格去掉，然后去掉結(jié)束符感嘆號(hào)，最后把數(shù)據(jù)中的間隔福逗號(hào)去掉，去掉后調(diào)用str2num函數(shù)將字符串轉(zhuǎn)換為數(shù)字就行了。

1 StringIn(StringIn==' ')=[]; %先去掉空格 2 StringIn(StringIn=='!')=[]; %去掉感嘆號(hào) 3 StringIn(StringIn==',')=' '; %逗號(hào)編程空格 4 SourceData=str2num(StringIn);

調(diào)用plot函數(shù)就可以繪制波形了。。這個(gè)比較簡(jiǎn)單，不過(guò)還是解釋下四元素在這里的用處。

話說(shuō)我們最開(kāi)始的時(shí)候?qū)懥艘粋€(gè)通過(guò)yaw pitch roll現(xiàn)實(shí)姿態(tài)的函數(shù)，寫(xiě)著寫(xiě)著我們發(fā)現(xiàn)了用方向余弦的方法，而現(xiàn)在我們直接使用四元素進(jìn)行坐標(biāo)變化，簡(jiǎn)單暴力，幾行代碼搞定。具體代碼如下：

1 q0=SourceData(9); 2 q1=SourceData(10); 3 q2=SourceData(11); 4 q3=SourceData(12); 5 6 %創(chuàng)建四元素矩陣 7 q = [1-2*(q2^2+q3^2) 2*(q1*q2-q0*q3) 2*(q0*q2+q1*q3); 8 2*(q1*q2+q0*q3) 1-2*(q1^2+q3^2) 2*(q2*q3-q0*q1); 9 2*(q1*q3-q0*q2) 2*(q2*q3+q0*q1) 1-2*(q1^2+q2^2)]; 10 11 %c初始化三角形的三個(gè)坐標(biāo)點(diǎn) 12 xd=[3 -1.2735;3 -1.2735]; 13 yd=[0 1.3474;0 -1.3474]; 14 zd=[0 0;0 0]; 15 16 %坐標(biāo)變換 17 temp = [xd(1,1) yd(1,1) zd(1,1); 18 xd(1,2) yd(1,2) zd(1,2); 19 xd(2,2) yd(2,2) zd(2,2)]; 20 temp = temp*q; 21 xd = [temp(1:2,1)';temp(1,1),temp(3,1)]; 22 yd = [temp(1:2,2)';temp(1,2),temp(3,2)]; 23 zd = [temp(1:2,3)';temp(1,3),temp(3,3)]; 24

? ?首先成功數(shù)據(jù)中提取四元素的四個(gè)參數(shù)，然后創(chuàng)建四元素的旋轉(zhuǎn)矩陣，最后對(duì)三角形的三個(gè)坐標(biāo)旋轉(zhuǎn)下就可以了。。真是暴汗。。之前那程序?qū)懥艘惶?。。。。。?/span>

matlab界面如下，后期我們還需要添加控制四個(gè)電機(jī)的pwm數(shù)值現(xiàn)實(shí)和pid控制器中yaw pitch roll目標(biāo)值的顯示，這樣就可以看到PID的控制效果和對(duì)齊進(jìn)行調(diào)整了。

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

?

2：為了觀察cpu的使用情況，我想找個(gè)像UCOS里面的一個(gè)變量，可以查看CPU使用率的參數(shù)，可是RTT并沒(méi)有包涵在標(biāo)準(zhǔn)的系統(tǒng)中，而需要單獨(dú)添加，在RTT系統(tǒng)目錄下的examples\kernel中，我們可以找到一個(gè)叫做cpuusage.c文件，將這個(gè)文件添加到我們自己的工程中，然后調(diào)用void cpu_usage_init()函數(shù)，這個(gè)函數(shù)是初始化RTT IDLE線程的一個(gè)鉤子函數(shù)，在空閑期間統(tǒng)計(jì)CPU的使用率。。函數(shù)代碼如下：

1 void cpu_usage_init() 2 { 3 /* set idle thread hook */ 4 rt_thread_idle_sethook(cpu_usage_idle_hook); 5 }

具體統(tǒng)計(jì)方式我們就不多說(shuō)了哈。。這一說(shuō)又要說(shuō)一大段話。。初始化后怎么獲取CPU的使用率，我們調(diào)用void cpu_usage_get(rt_uint8_t *major, rt_uint8_t *minor)函數(shù)就可以獲取CPU的使用率，分別為整數(shù)部分和小數(shù)部分，我們值用了整數(shù)部分。。哈。。懶得處理了。。整數(shù)部分已經(jīng)可以反映CPU的使用率了。

使用率一出來(lái)，嚇了一跳，在1000hz采樣率和100hz姿態(tài)解算下，使用率高達(dá)43%，而且我們還沒(méi)有進(jìn)行姿態(tài)解算。。指示簡(jiǎn)單的上傳數(shù)據(jù)到matlab。。。先分析原因，我們?cè)谧x取MPU6050數(shù)據(jù)的時(shí)候，因?yàn)镮2C是可能可以重入的函數(shù)，使用了RTT零界區(qū)的管理函數(shù)，rt_enter_critical(void)和rt_exit_critical(void) 進(jìn)行處理，I2C讀取數(shù)據(jù)的時(shí)間也是相當(dāng)可觀的，可能是因?yàn)檫@個(gè)導(dǎo)致CPU使用率很高，當(dāng)時(shí)用零界區(qū)也是想偷懶，I2C雖然是一個(gè)可重入的函數(shù)，但是我們使用的是模擬I2C，也就是說(shuō)I2C是可以被打斷的，所以其實(shí)我們是可以試用互斥量來(lái)處理的，本來(lái)共享資源就是應(yīng)該使用互斥量來(lái)處理的哈。。額。。用互斥量，可以保證I2C不被重入，但是可以被打斷，這樣不回影響更高優(yōu)先級(jí)任務(wù)的運(yùn)行。。。

MPU6050讀數(shù)據(jù)還有一個(gè)需要處理，上一章我們也說(shuō)了，為了偷懶，避免大端小端的轉(zhuǎn)換，我們通過(guò)共用體的方式讀取處理從而避免轉(zhuǎn)換，之前我們也說(shuō)了，這個(gè)會(huì)帶來(lái)讀取數(shù)據(jù)的時(shí)候效率底下的問(wèn)題，我們來(lái)算算有多底下。。。我們需要讀取accex,accey,accez,temp,gyrox,gyroy,gyroz，7個(gè)變量，都是2字節(jié)的，需要讀取14次，每次通過(guò)I2C讀取的過(guò)程是寫(xiě)MPU6050的地址，然后的到ack，然后寫(xiě)寄存器地址，然后寫(xiě)MPU6050讀取的命令，然后讀數(shù)據(jù)，然后返回noack命令，返回stop命令，中間通訊過(guò)程中的應(yīng)答幀需要的事件我們忽略掉，單看讀一個(gè)字節(jié)，我們需要寫(xiě)三個(gè)字節(jié)讀取一個(gè)字節(jié)，中間傳輸了4個(gè)字節(jié)，14*4=56個(gè)字節(jié)，也就是讀取14個(gè)字節(jié)的數(shù)據(jù)我們中間傳輸了56個(gè)字節(jié)。。相當(dāng)?shù)睦速M(fèi)，所以還是改成連讀吧。。如果我們使用硬件I2C或者使用DMA方式，這還不那么明顯，讀取的時(shí)候CPU可以做其他事情，指示讀取獲得數(shù)據(jù)的時(shí)間長(zhǎng)了點(diǎn)。。哎。。模擬的生不起啊。。看看連讀，連讀下我們只需要寫(xiě)入MPU6050地址，開(kāi)始寄存器，然后寫(xiě)讀命令，然后讀啊讀，讀完14個(gè)字節(jié)就可以了（這幾個(gè)寄存器在MPU6050里面內(nèi)部地址是連起來(lái)的哈），也就是3+14=17，也就是說(shuō)讀14個(gè)字節(jié)，我們中間只傳了17個(gè)字節(jié)，效率相當(dāng)?shù)目陀^。經(jīng)過(guò)這樣處理,CPU的使用率可以降低15%左右。。。。

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

? ? 說(shuō)起四元素，不懂的時(shí)候感覺(jué)那是相當(dāng)?shù)母呱?#xff0c;這是個(gè)姿態(tài)表達(dá)式而已，就和歐拉角一樣的，換個(gè)表達(dá)方式。。

? ? 其實(shí)回到最開(kāi)始的地方，就是我們獲取MPU6050的數(shù)據(jù)后怎么處理。一個(gè)是加速度，一個(gè)是角速度。。有了加速度我們就可以算出pitch和roll了，可是可是。。。。?？墒腔覚C(jī)是動(dòng)態(tài)的，會(huì)動(dòng)的，灰機(jī)動(dòng)的過(guò)程中自己本身機(jī)體也會(huì)產(chǎn)生加速度，那么我們就需要分辨因?yàn)榈厍蛑亓Ξa(chǎn)生的重力加速度和機(jī)體的加速度才能算出集體的pitch和roll了，這個(gè)分辨的過(guò)程會(huì)比較麻煩，不過(guò)后面用慣性導(dǎo)航算法的時(shí)候，這個(gè)是接觸，否則沒(méi)有分辨出機(jī)體的加速度怎么可以計(jì)算出灰機(jī)的軌跡呢。。對(duì)吧。。不過(guò)這里有個(gè)問(wèn)題哈，加速度在震動(dòng)情況下輸出的值是波動(dòng)很大的，而陀螺在動(dòng)態(tài)下輸出就要好很多。。?？墒强墒窃倏墒恰?。。陀螺輸出的是角速度，我們用時(shí)間乘以這個(gè)角速度就可以的到角度，每次積分的角度和上次的角度相加，就可以達(dá)到集體的xyz三個(gè)軸的角度了，可是可是再可是。。。。你采樣頻率足夠高嗎？解算頻率足夠高嗎？在你解算的過(guò)程中，角速度是恒定的嗎？保證不了吧？那就意味著長(zhǎng)時(shí)間的對(duì)陀螺儀積分出來(lái)的角度誤差會(huì)越來(lái)越大。。。雖然積分的時(shí)候可以使用龍格-庫(kù)塔積分方法，這個(gè)方法還是比較簡(jiǎn)單的哈，但是角速度積分誤差還是無(wú)可避免的。。。這個(gè)時(shí)候加速度就派上用場(chǎng)了。。。用加速度校正陀螺儀角度積分的誤差啊。。。。對(duì)，沒(méi)錯(cuò)，就是短期相信陀螺儀，然后長(zhǎng)期相信加速度。Madgwick的IMUupdate算法的總體思路就是這樣的哈。。。

? ?返回來(lái)說(shuō)四元素。。。四元素其實(shí)不難的哈，我參照 鄧正隆的慣性技術(shù)的四元素部分學(xué)習(xí)了下，講的還是比較詳細(xì)的，看不懂？對(duì)，開(kāi)始是看不怎么太懂，，哈。怎么辦？做題啊。。。按照書(shū)上講的，按照他的推導(dǎo)過(guò)程，自己推倒一遍。。瞬間明白了有木有？雖然理解深度可能還不深，但是我直到怎么用了。。哈哈。。

推到過(guò)程自己用筆推下就懂了。。

四元素還可以參考這個(gè)網(wǎng)址http://www.cnblogs.com/Mrt-02/archive/2011/10/15/2213656.html

四元素的一些公式：

坐標(biāo)轉(zhuǎn)換公式，我們matlab里面畫(huà)圖的坐標(biāo)轉(zhuǎn)化公式就是這個(gè)哈。。

其中w,x,y,z就是四元素的四個(gè)元素，W為實(shí)數(shù)部分，xyz對(duì)應(yīng)ijk的三個(gè)變量。

四元素的微分方程：

?

這個(gè)公式在IMUupdate有用到哈，看了這個(gè)公式應(yīng)該直到halfT是怎么來(lái)的了吧。。四元素其實(shí)知識(shí)還挺多的哈。。還是要自己算算才會(huì)哈。。看沒(méi)用的。。

4：飛控?cái)?shù)據(jù)采集線程和數(shù)據(jù)處理線程的安排，類似于生產(chǎn)者與消費(fèi)者的關(guān)系。

? ? 采集數(shù)據(jù)的頻率可以很高，采集足夠多的數(shù)據(jù)才好處理嗎。哈。?？墒荂PU的性能考慮，姿態(tài)解算頻率太高并不劃算。。。

? ? 我們有兩個(gè)線程，一個(gè)采集數(shù)據(jù)線程，采集頻率500hz，一個(gè)姿態(tài)解算線程，將采集過(guò)來(lái)的數(shù)據(jù)進(jìn)行解算，通過(guò)解算的結(jié)果再控制電機(jī)。這其中數(shù)據(jù)采集線程就是生產(chǎn)者，姿態(tài)解算線程就是消費(fèi)者。。。。他們之間需要通過(guò)怎樣的協(xié)調(diào)工作才能是效率高并且數(shù)據(jù)安全呢？這里我們有三鐘方法：

（1） 生產(chǎn)者只管采集數(shù)據(jù)，消費(fèi)者只管消費(fèi)數(shù)據(jù)，這就有個(gè)麻煩，生產(chǎn)者不知道消費(fèi)者什么時(shí)候會(huì)消費(fèi)數(shù)據(jù)，所以他要時(shí)刻將數(shù)據(jù)準(zhǔn)備好，讓消費(fèi)者隨時(shí)可以消費(fèi)數(shù)據(jù)，并且數(shù)據(jù)是要最新的。那么濾波怎么辦？每個(gè)數(shù)據(jù)都要是最新的且濾波好的，那就要用平滑濾波了，應(yīng)該是這么叫，然后窗口多大呢？不知道，額。。希望是低耦合高內(nèi)聚的代碼哈。如果需要解決窗口大大小，有兩種方法，實(shí)現(xiàn)預(yù)定好，或者消費(fèi)者消費(fèi)的時(shí)候告知生產(chǎn)者，我已經(jīng)消費(fèi)，那么生產(chǎn)者可以重新設(shè)定窗口。那么，這中間不可避免的消費(fèi)者和生產(chǎn)者是要交流的，而且消費(fèi)數(shù)據(jù)的時(shí)候數(shù)據(jù)是共享資源，需要互斥量。。也就是說(shuō)，要實(shí)現(xiàn)我們需要互斥量，然后要平滑濾波，生產(chǎn)者每次生產(chǎn)數(shù)據(jù)都要進(jìn)行濾波運(yùn)算。。。效率并不高? ? ? ? ?

（2） 生產(chǎn)者采集N個(gè)數(shù)據(jù)，通知消費(fèi)者消費(fèi)。這里有個(gè)問(wèn)題，消費(fèi)者處在阻塞狀態(tài)，等生產(chǎn)者生產(chǎn)好了通知消費(fèi)者，消費(fèi)者才消費(fèi)，可是，系統(tǒng)運(yùn)行會(huì)出現(xiàn)各種各樣的問(wèn)題，無(wú)法保證100%生產(chǎn)者的task不出問(wèn)題，所以這樣的系統(tǒng)怎么說(shuō)呢，安全不好保證，同是也不利于我們到時(shí)候統(tǒng)計(jì)各個(gè)任務(wù)的狀態(tài)，通過(guò)統(tǒng)計(jì)各個(gè)任務(wù)的狀態(tài)我們可以實(shí)現(xiàn)類似于硬件看門狗的功能，任務(wù)出問(wèn)題了，可以對(duì)任務(wù)進(jìn)行刪除再重啟的操作等。。

（3） 生產(chǎn)者只管采集數(shù)據(jù)，消費(fèi)者消費(fèi)數(shù)據(jù)，但是我們這中間增加一個(gè)協(xié)調(diào)員。我們有兩個(gè)變量來(lái)同步這兩個(gè)任務(wù)。?；コ饬亢陀?jì)數(shù)。。。生產(chǎn)者只管統(tǒng)計(jì)數(shù)據(jù)，將數(shù)據(jù)進(jìn)行累加，同時(shí)技術(shù)，消費(fèi)者消費(fèi)數(shù)據(jù)，但是需要做些處理，拿到數(shù)據(jù)后對(duì)數(shù)據(jù)均值濾波，就是除以統(tǒng)計(jì)的次數(shù)，同時(shí)將原來(lái)的數(shù)值請(qǐng)0。。。。互斥量的存在，可以保證數(shù)據(jù)操作的同步和安全性。最后，我們是采用第三種方法。

總結(jié)

以上是生活随笔為你收集整理的四轴飞行器1.4 姿态解算和Matlab实时姿态显示的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。