基于陀螺儀和加速度計(jì)的飛行機(jī)器人自平衡算法研究-曹珂杰-南京

1、江蘇省高等學(xué)校大學(xué)生實(shí)踐創(chuàng)新訓(xùn)練計(jì)劃項(xiàng)目申報(bào)表推薦學(xué)校：南京工程學(xué)院項(xiàng)目名稱：基于陀螺儀和加速度計(jì)的飛行器自平衡算法的研究項(xiàng)目類型：重點(diǎn)項(xiàng)目一般項(xiàng)目 指導(dǎo)項(xiàng)目所屬一級(jí)學(xué)科名稱：電子信息類項(xiàng)目負(fù)責(zé)人：曹珂杰聯(lián)系電話：指導(dǎo)教師：聯(lián)系電話：申報(bào)日期：2013年4月24日江蘇省教育廳 制二一三年三月項(xiàng)目名稱基于陀螺儀和加速度計(jì)的飛行機(jī)器人自平衡算法的研究項(xiàng)目所屬一級(jí)學(xué)科 08工學(xué)項(xiàng)目所屬二級(jí)學(xué)科 電子信息類項(xiàng)目類型（）重點(diǎn)項(xiàng)目 （）一般項(xiàng)目 （ ）指導(dǎo)項(xiàng)目項(xiàng)目實(shí)施時(shí)間起始時(shí)間： 2013 年 6 月 完成時(shí)間： 2014年 5 月項(xiàng)目簡(jiǎn)介(100字以內(nèi)）鑒于融合機(jī)械臂與四軸飛行器于一體的飛行機(jī)器人高空

2、作業(yè)時(shí)穩(wěn)定性差的缺點(diǎn)，利用本套基于陀螺儀和加速度計(jì)的自平衡算法，可以消除機(jī)械臂運(yùn)動(dòng)對(duì)飛行器位姿產(chǎn)生的影響，實(shí)現(xiàn)系統(tǒng)的自平衡。主要成果第二指導(dǎo)教師姓名單位年齡專業(yè)技術(shù)職務(wù)主要成果一、申請(qǐng)理由（包括自身具備的知識(shí)條件、自己的特長(zhǎng)、興趣、已有的實(shí)踐創(chuàng)新成果等）熟練應(yīng)用51單片機(jī)，并且開(kāi)始學(xué)習(xí)arm，以及熟練掌握Protel 99SE，DXP，Multisim，Altium designer等電路制作以及電路仿真軟件的使用，VC+6.0，VS2010，IAR，Keil，OPENCVCodeWarrior等多種編譯環(huán)境的使用，Matlab等多種數(shù)字處理軟件的使用?，F(xiàn)正在學(xué)習(xí)LabView的使用及技巧，基

3、于Linux環(huán)境下的軟件的編寫，Matlab的深入研究。本組組員已有實(shí)踐創(chuàng)新成果：（1） 基于51單片機(jī)的智能小車（2） 基于OPENCV雙目測(cè)距（3） 亞洲水下機(jī)器人亞軍（4） 對(duì)四軸飛行器的研究與制作（5） 對(duì)各種算法深入研究（6） 對(duì)機(jī)械臂精細(xì)研究二、項(xiàng)目方案1、 研究現(xiàn)狀：四軸飛行機(jī)器人已廣泛應(yīng)用到軍事、消防、安檢、娛樂(lè)等場(chǎng)所；對(duì)機(jī)械臂的研究和制作頗具規(guī)模；利用搭載機(jī)械臂的四軸飛行器高空作業(yè)技術(shù)尚未成熟。2項(xiàng)目研究目標(biāo)及主要內(nèi)容研究基于陀螺儀和加速度計(jì)的自平衡算法，消除機(jī)械臂運(yùn)動(dòng)對(duì)飛行器位姿產(chǎn)生的影響，實(shí)現(xiàn)系統(tǒng)的自平衡，從而完成減小機(jī)械臂對(duì)物品的操作誤差的目標(biāo)。3.項(xiàng)目主要技術(shù)方向：方

4、案一：姿態(tài)角度測(cè)控系統(tǒng)主要由加速度計(jì)、陀螺儀、微控制器、濾波電路、電機(jī)調(diào)速器、無(wú)刷電機(jī)等部分組成.姿態(tài)檢測(cè)系統(tǒng)的硬件平臺(tái)如圖,由微處理器對(duì)陀螺儀、濾波電路和加速度計(jì)構(gòu)成的傳感器組進(jìn)行高速A/D采樣后,通過(guò)卡爾曼濾波器對(duì)傳感器數(shù)據(jù)進(jìn)行補(bǔ)償和信息融合,得到準(zhǔn)確的姿態(tài)角度信號(hào),此角度信號(hào)再通過(guò)PID控制器運(yùn)算,輸出給電子調(diào)速器轉(zhuǎn)換成PWM信號(hào),進(jìn)而對(duì)電機(jī)進(jìn)行控制.加速度計(jì)用于測(cè)量物體的線性加速度,加速度計(jì)的輸出值與傾角呈非線性關(guān)系,隨著傾角的增加而表現(xiàn)為正弦函數(shù)變化.因此對(duì)加速度計(jì)的輸出 進(jìn) 行 反 正 弦 函 數(shù) 處 理,才 能 得 到 其 傾角值5-6.測(cè)量數(shù)據(jù)噪聲與帶寬的平方根成正比,即噪聲會(huì)

5、隨帶寬的增加而增加.VNoise=350ug Bw ×1.5式中:Bw為傳感器帶寬(單位為Hz.因此在設(shè)計(jì)卡爾曼濾波器時(shí),首先要確定被測(cè)加速度的頻率范圍,然后再設(shè)計(jì)濾波器的參數(shù),盡量使濾波器帶寬略高于被測(cè)頻率,這樣不僅有助于濾除高頻干擾,而且也有利于降低系統(tǒng)噪聲干擾.但是如果要得到精確的傾角值,帶寬就需要設(shè)置得比較小,而這時(shí)加速度計(jì)動(dòng)態(tài)響應(yīng)慢,不適合跟蹤動(dòng)態(tài)角度運(yùn)動(dòng),如果期望快速的響應(yīng),又會(huì)引入較大的噪聲.再加上其測(cè)量范圍的限制,使得單獨(dú)應(yīng)用加速度計(jì)檢測(cè)飛行器傾角并不合適,需要與其他傳感器共同使用.陀螺儀的作用是用來(lái)測(cè)量角速度信號(hào),通過(guò)對(duì)角速度積分,便能得到角度值.陀螺儀本身極易受噪

6、聲干擾,微機(jī)械陀螺不能承受較大的震動(dòng),同時(shí)由于溫度變化、不穩(wěn)定力矩等因素,陀螺儀會(huì)產(chǎn)生漂移誤差,并會(huì)隨著時(shí)間的推移而累加變大,通過(guò)積分會(huì)使得誤差變得很大.因此,也不能單獨(dú)使用陀螺儀作為本系統(tǒng)的傾角傳感器.2 卡爾曼濾波融合過(guò)程首先建立系統(tǒng)的狀態(tài)方程和測(cè)量方程.由于傾角和傾角角速度存在導(dǎo)數(shù)關(guān)系,系統(tǒng)傾斜真實(shí)角度可以用來(lái)做一個(gè)狀態(tài)向量.利用機(jī)械臂角度，估計(jì)機(jī)械臂下一個(gè)位置，對(duì)機(jī)械臂產(chǎn)生何種影響。在該系統(tǒng)中,采用加速度計(jì)估計(jì)出陀螺儀常值偏差b,以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測(cè)方程:式中,gyro為包含固定偏差的陀螺儀輸出角速度,acce為加速度計(jì)經(jīng)處理后得到的角度值,wg為陀螺儀測(cè)量噪聲

7、,wa為加速度計(jì)測(cè)量噪聲,b為陀螺儀漂移誤差,wg與wa相互獨(dú)立,此處假設(shè)二者為滿足正態(tài)分布的白色噪聲,令Ts為系統(tǒng)采樣周期,得到離散系統(tǒng)的狀態(tài)方程和測(cè)量方程:同時(shí),要估算k時(shí)刻的實(shí)際角度,就必須根據(jù)k-1時(shí)刻的角度值,再根據(jù)預(yù)測(cè)得到的k時(shí)刻的角度值得到k時(shí)刻的高斯噪聲的方差,在此基礎(chǔ)之上卡爾曼濾波器進(jìn)行遞歸運(yùn)算直至估算出最優(yōu)的角度值.在此,須知道系統(tǒng)過(guò)程噪聲協(xié)方差陣Q以及測(cè)量誤差的協(xié)方差矩陣R,對(duì)卡爾曼濾波器進(jìn)行校正.Q與R矩陣的形式如下:式中,q_acce和q_gyro分別是加速度計(jì)和陀螺儀測(cè)量的協(xié)方差,其數(shù)值代表卡爾曼濾波器對(duì)其傳感器數(shù)據(jù)的信任程度,值越小,表明信任程度越高.在該系統(tǒng)中陀

8、螺儀的值更為接近準(zhǔn)確值,因此取q_gyro的值小于q_acce的值.當(dāng)前狀態(tài):式中X(k|k-1是利用k預(yù)測(cè)的結(jié)果,X(k-1|k-1是k-1時(shí)刻的最優(yōu)結(jié)果.則有對(duì)應(yīng)于X(k|k-1的協(xié)方差為:P(k|k-1=AP(k-1|k-1AT+Q (2式中,P(k-1|k-1是X(k-1|k-1對(duì)應(yīng)的協(xié)方差,AT表示A的轉(zhuǎn)置矩陣,Q是系統(tǒng)過(guò)程的協(xié)方差.式子(1、(2即對(duì)系統(tǒng)的狀態(tài)更新.則狀態(tài)k的最優(yōu)化估算值X(k|k:X(k|k=X(k|k-1+K(k(Z(k-HX(k|k-1 (3其中H =1 0K為卡爾曼增益(KalmanGain:K(k=P(k|k-1HT/(HP(k|k-1HT+R (4此時(shí),

9、我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k.但是為了使卡爾曼濾波器不斷的運(yùn)行下去直到找到最優(yōu)的角度值,我們還要更新k狀態(tài)下X(k|k的協(xié)方差:P(k|k=(I-Kg(kHP(k|k-1 (5其中,I為單位陣,對(duì)于本系統(tǒng)則有當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí),P(k|k就是式子(2的P(k-1|k-1.(3、(4、(5式為卡爾曼濾波器狀態(tài)更新方程.計(jì)算完時(shí)間更新方程和測(cè)量更新方程后,再次重復(fù)上一次計(jì)算得到的后驗(yàn)估計(jì),作為下一次計(jì)算的先驗(yàn)估計(jì),這樣,周而復(fù)始、循環(huán)反復(fù)地運(yùn)算下去直至找到最優(yōu)的結(jié)果.方案二：在姿態(tài)控制算法里，只用標(biāo)準(zhǔn)的PI和加權(quán)移動(dòng)平均這2種算法，沒(méi)有采用卡爾曼濾波或別的“高深”算法。具體流程

10、如下：  首先利用處理器的AD功能，將陀螺儀、加速度計(jì)的數(shù)值讀取近來(lái)。由于加速度計(jì)對(duì)振動(dòng)非常敏感，所以我這里對(duì)加速度計(jì)做了一個(gè)移動(dòng)期為10的加權(quán)移動(dòng)平均，用來(lái)過(guò)濾掉部分振動(dòng)。由于我的四軸對(duì)這些數(shù)據(jù)的采集是工作在300Hz的頻率下，所以移動(dòng)期為10對(duì)我的四軸來(lái)說(shuō)只相當(dāng)于延遲33ms，再加上適當(dāng)?shù)募訖?quán)，基本可以消除由于加權(quán)移動(dòng)平均帶來(lái)的響應(yīng)滯后問(wèn)題。  對(duì)于陀螺儀，由于這個(gè)比較重要，所以我沒(méi)有做多余的處理，就把AD的數(shù)據(jù)直接拿來(lái)用了。以前也測(cè)試過(guò)移動(dòng)期為2的情況下，也沒(méi)有問(wèn)題。  下面對(duì)每個(gè)軸的陀螺儀做PI，就可以得到馬達(dá)修正量。將這個(gè)量

11、加上油門后輸出給馬達(dá)，就完成姿態(tài)調(diào)整任務(wù)了。  不過(guò)這里就有一個(gè)“隱患”。由于陀螺儀有漂移，也就是以恒定速度右轉(zhuǎn)90度，然后再左轉(zhuǎn)90度后，陀螺儀積分不會(huì)為0。如下圖所示：  可以看到在四軸最后回到平衡位置時(shí)，陀螺儀積分并不為0。這個(gè)偏差在所有陀螺儀里都存在，在ENC03上更加厲害一些，所以需要對(duì)陀螺儀的積分?jǐn)?shù)據(jù)進(jìn)行適當(dāng)處理后才能使用，否則積分會(huì)越積越大。這就是下面要討論的。如何將遙控器的控制信號(hào)融合進(jìn)來(lái)：在姿態(tài)控制算法里，我只用到了標(biāo)準(zhǔn)的PI和加權(quán)移動(dòng)平均這2種算法，沒(méi)有采用卡爾曼濾波或別的“高深”算法。具體流程如下：  首先利用處

12、理器的AD功能，將陀螺儀、加速度計(jì)的數(shù)值讀取近來(lái)。由于加速度計(jì)對(duì)振動(dòng)非常敏感，所以我這里對(duì)加速度計(jì)做了一個(gè)移動(dòng)期為10的加權(quán)移動(dòng)平均，用來(lái)過(guò)濾掉部分振動(dòng)。由于我的四軸對(duì)這些數(shù)據(jù)的采集是工作在300Hz的頻率下，所以移動(dòng)期為10對(duì)我的四軸來(lái)說(shuō)只相當(dāng)于延遲33ms，再加上適當(dāng)?shù)募訖?quán)，基本可以消除由于加權(quán)移動(dòng)平均帶來(lái)的響應(yīng)滯后問(wèn)題。  對(duì)于陀螺儀，由于這個(gè)比較重要，所以我沒(méi)有做多余的處理，就把AD的數(shù)據(jù)直接拿來(lái)用了。以前也測(cè)試過(guò)移動(dòng)期為2的情況下，也沒(méi)有問(wèn)題。  下面對(duì)每個(gè)軸的陀螺儀做PI，就可以得到馬達(dá)修正量。將這個(gè)量加上油門后輸出給馬達(dá)，就完成姿態(tài)調(diào)整任務(wù)

13、了。  不過(guò)這里就有一個(gè)“隱患”。由于陀螺儀有漂移，也就是以恒定速度右轉(zhuǎn)90度，然后再左轉(zhuǎn)90度后，陀螺儀積分不會(huì)為0。如下圖所示：  可以看到在四軸最后回到平衡位置時(shí)，陀螺儀積分并不為0。這個(gè)偏差在所有陀螺儀里都存在，在ENC03上更加厲害一些，所以需要對(duì)陀螺儀的積分?jǐn)?shù)據(jù)進(jìn)行適當(dāng)處理后才能使用，否則積分會(huì)越積越大。這就是下面要討論的。如何調(diào)整PI參數(shù)：實(shí)時(shí)融合： 1.將陀螺儀積分和加表濾波后的值做差； 2.按照情況對(duì)差值進(jìn)行衰減，并作限幅處理； 3.將衰減值加入到角度中。 長(zhǎng)期融合： 長(zhǎng)期融合主要包括兩個(gè)部分，一是對(duì)機(jī)械臂下一個(gè)位置的估計(jì)，二是對(duì)角速度

14、的漂移進(jìn)行估計(jì)（估計(jì)值是要在每一個(gè)控制周期都耦合到角度中的），三是對(duì)陀螺儀的常值誤差（也就是陀螺儀中立點(diǎn)）進(jìn)行實(shí)時(shí)的修正。 1.將陀螺儀積分的積分和加速度積分做差（PS：為什么這里要使用加表積分和陀螺儀積分的積分，因?yàn)樵?56個(gè)檢測(cè)周期內(nèi)，有一些加速度計(jì)的值含有有害的加速度分量，如果只使用一個(gè)時(shí)刻的加表值對(duì)陀螺儀漂移進(jìn)行估計(jì)，顯然估計(jì)值不會(huì)準(zhǔn)確，使用多個(gè)周期的值進(jìn)行疊加后做座平均處理，可以減小隨機(jī)的有害加速度對(duì)估計(jì)陀螺儀漂移過(guò)程中所鎖產(chǎn)生的影響） 2.將上面兩個(gè)值進(jìn)行衰減，得到估計(jì)的陀螺儀漂移 3.對(duì)使考慮了陀螺儀漂移和不考慮陀螺儀漂移得到的角度做差，如果這兩個(gè)值較大，說(shuō)明陀螺儀在前段時(shí)間內(nèi)測(cè)

15、到的角速率不夠準(zhǔn)確，需要對(duì)差值誤差（也就是陀螺儀中立點(diǎn)）進(jìn)行修正，修正幅度和差值有關(guān) 長(zhǎng)期融合十分關(guān)鍵，如果不能對(duì)陀螺儀漂移做修正，則系統(tǒng)運(yùn)行一段時(shí)間后，速率環(huán)的穩(wěn)定性會(huì)降低。德國(guó)人的控制算法的核心是對(duì)角速度做PI計(jì)算，P的作用是使四軸能夠產(chǎn)生對(duì)于外界干擾的抵抗力矩，I的作用是讓四軸產(chǎn)生一個(gè)與角度成正比的抵抗力。 如果只有P的作用，將四軸拿在手上就會(huì)發(fā)現(xiàn)，四軸能夠抵抗外界的干擾力矩的作用，而且這個(gè)抵抗力非常快速，只要手妄圖改變四軸的轉(zhuǎn)速，四軸就會(huì)產(chǎn)生一個(gè)抵抗力矩，但是，如果用手將四軸扳過(guò)一個(gè)角度，則四軸無(wú)法自己回到水平的角度位置，這就需要I的調(diào)節(jié)作用。 對(duì)角速度做I（積分）預(yù)算實(shí)際得到的就是角

16、度，德國(guó)人四軸里面用的也是角度值，如果四軸有一個(gè)傾斜角度，那么四軸就會(huì)自己進(jìn)行調(diào)整，直到四軸的傾角為零，它所產(chǎn)生的抵抗力是與角度成正比的，但是，如果只有I的作用，會(huì)使四軸迅速產(chǎn)生振蕩，因此，必須將P和I結(jié)合起來(lái)一起使用，這時(shí)候基本上就會(huì)得到德國(guó)四軸的效果了。如何將加速度計(jì)的數(shù)據(jù)融合進(jìn)陀螺儀積分里：根據(jù)上面的說(shuō)明，已經(jīng)可以大概了解為什么要對(duì)陀螺儀積分進(jìn)行處理了。但這里的陀螺儀積分還有另一個(gè)重要的物理意義，那就是這個(gè)積分記錄了四軸偏轉(zhuǎn)的絕對(duì)角度（雖然不是直接的角度，但是只要乘一個(gè)系數(shù)就可以變角度了），而不再是角速度。為了能做到四軸的穩(wěn)定懸停，這個(gè)角度的重要性不可小看。通常的做法，是利用加速度計(jì)一個(gè)

17、軸的偏轉(zhuǎn)和陀螺儀積分建立對(duì)照關(guān)系，通過(guò)試驗(yàn)測(cè)出一個(gè)系數(shù)。然后程序不斷通過(guò)加速度計(jì)的偏轉(zhuǎn)計(jì)算得出陀螺儀應(yīng)該的積分大小，根據(jù)這個(gè)數(shù)值對(duì)陀螺儀積分進(jìn)行修正。當(dāng)然這個(gè)修正也有講究，決不能直接替換掉陀螺儀的積分。通常都是將通過(guò)加速度計(jì)得出的積分“猜測(cè)值”縮小，限幅后加入陀螺儀的積分里。過(guò)大或過(guò)小都可能造成問(wèn)題，須要反復(fù)試驗(yàn)才能得出合理值。在我的四軸里，我測(cè)出的陀螺儀和加速度計(jì)的對(duì)照關(guān)系是：當(dāng)加速度計(jì)偏差=94 時(shí)，陀螺儀積分 = 696。這是 100Hz 積分頻率，加速度計(jì)數(shù)據(jù)放大 6 倍，3.3V 參考電壓下得出的。這個(gè)系數(shù)決定了加速度計(jì)積分猜測(cè)值的“斜率”大小。然后我把這個(gè)計(jì)算值

18、縮小 22 倍，然后限制幅度為正負(fù) 5，最后才將數(shù)據(jù)加入陀螺儀積分里。在加入了積分修正代碼后，陀螺儀積分就可以正確收斂了。更高級(jí)的做法是利用卡爾曼濾波將加速度計(jì)和陀螺儀數(shù)據(jù)進(jìn)行融合，得出絕對(duì)角度，以次來(lái)進(jìn)行姿態(tài)糾正。不過(guò)卡爾曼濾波比較復(fù)雜，數(shù)據(jù)延遲的問(wèn)題我一直沒(méi)能很好地解決，所以我的四軸里沒(méi)有使用這種方法。攝像頭部分：由于攝像頭本身存在誤差，增加透鏡中心之間的距離，可以提高深度信息的精度，但減小了 2 個(gè)攝像頭共同影響的區(qū)域。此外，為保證精度，還要對(duì)透鏡進(jìn)行標(biāo)定。機(jī)械手臂的運(yùn)動(dòng)學(xué)分析對(duì)機(jī)械手臂進(jìn)行控制，首先要進(jìn)行數(shù)學(xué)建模，并分析其運(yùn)動(dòng)學(xué)規(guī)律，機(jī)械手臂可以簡(jiǎn)化為連桿和關(guān)節(jié)組成的模型，如圖 O 為

19、基座原點(diǎn)，d1、d2分別是連桿 1、2 的長(zhǎng)度，d3為連桿 3 和 4 的總長(zhǎng)，1、2和 3分別為關(guān)節(jié) 1、2 和 3 的關(guān)節(jié)變量，關(guān)節(jié) 4 相當(dāng)人的腕關(guān)節(jié)，方便抓物。關(guān)節(jié) 1 的旋轉(zhuǎn)軸平行于連桿 1，關(guān)節(jié) 2 和 3的旋轉(zhuǎn)軸垂直于平面。對(duì)于平面上一點(diǎn)(x,y，要使機(jī)械手臂末端達(dá)到此坐標(biāo)點(diǎn)，須滿足：通過(guò)雙目視覺(jué)得到物體坐標(biāo)，首先旋轉(zhuǎn)關(guān)節(jié) 1，使機(jī)械手臂和物體在同一個(gè)平面上，然后控制關(guān)節(jié) 2 和 3，使機(jī)械手臂末端達(dá)到指定位置。四軸飛行器是一個(gè)具有 6 個(gè)自由度和 4 個(gè)輸入的欠驅(qū)動(dòng)系統(tǒng), 具有不穩(wěn)定和強(qiáng)耦合等特點(diǎn), 除了受自身機(jī)械結(jié)構(gòu)和旋翼空氣動(dòng)力學(xué)影響外, 也很容易受到外界的干擾。飛行著的高空作業(yè)機(jī)器人的姿態(tài)最終通過(guò)調(diào)節(jié) 4個(gè)電機(jī)的轉(zhuǎn)速進(jìn)行調(diào)整, 飛行控制系統(tǒng)通過(guò)各，傳感器獲得飛行機(jī)器人的姿態(tài)信息, 經(jīng)過(guò)一定的控制算法解算出 4個(gè)電機(jī)的轉(zhuǎn)速, 通過(guò) I2C 接口發(fā)送給電機(jī)調(diào)速器調(diào)整 4 個(gè)電機(jī)的轉(zhuǎn)速, 以實(shí)現(xiàn)對(duì)其姿態(tài)的控制。預(yù)計(jì)成果圖：5.進(jìn)度安排：具體的時(shí)間進(jìn)度和任務(wù)節(jié)點(diǎn)安排如下：2013.052013.6 項(xiàng)目申報(bào)與審批，資料收集和技術(shù)調(diào)研2013.072013.9 技術(shù)的總體方案設(shè)計(jì)2013.92013.11 各個(gè)子模塊的搭建和完善2013.1220141 各模塊的調(diào)試2014.220143 整體編程調(diào)試2014.420145 產(chǎn)品完善，資料