無人機PX4狀態(tài)估計

1、1.1 組合導(dǎo)航從上章介紹飛行器需要獲得自己的十五個狀態(tài)信息，需要通過傳感器測量并推算。單獨的慣性測量元件由于誤差累積，導(dǎo)致隨時間增長發(fā)發(fā)散。因此需要通過額外的傳感器信息來進行互補修正。組合導(dǎo)航技術(shù)結(jié)合GNSS慣性測量元件、磁力計、氣壓計和超聲波等傳感設(shè)備，利用各自的有點互補，采用數(shù)字信號處理技術(shù)融合多傳感器的測量值，從而獲得較為準確的飛行器的十五個狀 態(tài)空間量，通過一定的算法和規(guī)律盡可能的估計出與真實狀態(tài)量接近的量。慣性測量元件容易發(fā)散，從而這個發(fā)散可以通過GNS驗球衛(wèi)星定位系統(tǒng)來獲取修正和抑制，然而 GNSSR有在室外無遮擋的環(huán)境下才能得到較好的信號強 度和定位信息，當(dāng)信號質(zhì)量較差或者位于

2、室內(nèi)導(dǎo)致無法定位的時候，系統(tǒng)則需要由慣性元件來短時間抑制和修正。 GNSS勺高度信息通常誤差較大，這時候可 以通過超聲波傳感器和氣壓傳感器進行一定程度的修正。組合導(dǎo)航技術(shù)中傳感器互補的思想原理是由克勞德-香農(nóng)在1948年的信息論中提出。信息論的線性估計理論讓數(shù)字處理工程師在系統(tǒng)設(shè)計時能夠把多個具有誤差的傳感器通過數(shù)學(xué)方程組合矩陣運算融合起來，利用傳感器信息估計特定的狀態(tài)量，并且理論上提出了更多的傳感器狀態(tài)信息參與互補運算，就能 夠得到更好的狀態(tài)估計（更接近真值，并且方差更?。?。1.2 飛行器的坐標系三維笛卡爾支教坐標系? 三維笛卡爾直角坐標系是指三條相互垂直的坐標軸（正交）相交形成的坐標系，其

3、中相交的點稱為原點O（ origin ）。? 每個軸都指向特定的方向，兩個不同的坐標軸決定一個平面，也叫笛卡 爾平面。? 通常 3 個軸采用右手系標定，即右手的拇指方向指向X 軸的正方向，食指則指向Y軸方向，中指指向的是Z軸方向。? 此外對于圍繞每個軸的正旋轉(zhuǎn)方向，則是用右手大拇指對應(yīng)每個軸的正方向，則其他四指自然彎曲的方向為圍繞該軸旋轉(zhuǎn)的正方向。本文所介紹的軟件和算法均采用右手定則剛體剛體（Rigid Body ）的意思是指物體在任何外力的作用下，它的體積和形 狀都不會發(fā)生改變。這是力學(xué)范疇的一個理想抽象模型概念，實際情況任何物 體在受外力作用的時候都會或多或少的發(fā)生形變，并不存在真正意義上

4、的剛體。 但如果形變本身相對于物體的運動微乎其微，可以忽略，或者并不影響我們分 析物體的運動結(jié)果，則可以在特定的情況下有條件的忽略這種形變，即將物體 假設(shè)成剛體。其中質(zhì)心運動的分析范疇屬于牛頓第二定律定義的質(zhì)點運動與受力之間的 關(guān)系，即主要研究飛行器質(zhì)心的位置和速度信息。而轉(zhuǎn)動的建模方程則屬于歐 拉方程所分析的模型運動關(guān)系，即主要研究飛行器的姿態(tài)信息。要研究以上兩種運動的模型和信息，需要定義兩個三維笛卡爾直角坐標系:大地坐標系-（或慣性坐標系，用e或者G表示）用于研究飛行器相對大 地的運動狀態(tài)以及空間位置坐標。通常飛控都是把地面起飛點選取為該坐標系 的坐標原點，即初始化x,y,z坐標為0o通常

5、大地坐標系的X-Y-Z方向選取為 大地的北-東-地。機體坐標系-（用b或B表示）坐標原點取機體的重心,用于研究飛行器 相對于重心的旋轉(zhuǎn)運動。通常機體坐標系的X-Y-Z方向選取為機身的前-右-下。機體坐標系是固定在機體上隨著機體的運動而運動。1.3方向余弦矩陣和歐拉角63.1方向余雄陣及啊般翻住個靴沃1就際蜥觸耳對刊地嘛Me,三恤胞fi嬋瞅1邠提,業(yè)也嗎于機體金標系b,三個軸對應(yīng)的單位矢1分別是|i|cos（A（川。呵匕01心。舊）CO$(/.i) cos(/，0 cowKi)co$(/J)co$U/)co$(K./)CQSil.k) cosy.*) cos(K, k)瞰蝸拗胭嬋岐1瓢酸理樹如t

6、,帆醞成|i|/|co$(/J), m,i)=川 IMcos(K“三融融脆-這僥獻Ml體坐屆收劇嘛歌班嫣標63.1方向余蜩大地坐赧的3郵位矢也可以初體坐標系沖標出來,MHcoXU) cosi/i) cw(L力 co«/J) co$(/,k) cosfj.k)cos(Ki) CO5(K,/) cos(KJt)I i Ji Ki K*= l j I I K I Ik Ik K k這兩使陣定義了從Y坐標系的靴廨螺到另-世標系的轉(zhuǎn)換法:,或都用防式.這兩餞 陣即稱為方向余魏陣（Direction Cosine Matrix）632姿態(tài)與歐拉角利用艙公式來研究洲WB統(tǒng)質(zhì)情鬧的時悅主即究的對象就

7、題體的戢.無人機fi啜杰（或者翻角）指的是飛行黜端仙 椒/!詞的你見是用瀛斯黝固連機體坐標系與大地幽新系之間的角位1版飛謂需要了解這個當(dāng)前的姿態(tài)角位1關(guān)系, 從而蝴鹿預(yù)娜搦惻睞計Utt®繳蒯L從而實觀一定蹴作.對于三堆空間里的Y豺系，任碉體摘披魏頤用三個鼬角果標,鹿的射系是mt不動的，即前酬謎的大嬲歌 顆耐腕挫頒囑翻懶喻鐮,四確粉刷蒯陋ima岫詢機偏航角1.3.2姿態(tài)與歐拉角在定義歐拉角的時候，相同的姿態(tài)不同的取法會得到不同的歐拉角。本書 采用的取法順序為Z-Y-X,即繞三個軸的旋轉(zhuǎn)順序是先繞 Z軸，再繞Y軸，再 繞X軸，最終達到一定的姿態(tài)。而對于同樣的姿態(tài)，如果繞軸的順序不同，則得

8、到的三個歐拉角也不相同。 因此按Z-Y-X的定義進一步來說，本書所定義的歐拉角 舶角（觸）0 -機的水平面般也抬頭為正 犒航角（方位角耀仁f -機頭方向在他平面的嬲與林M軸加缺配北偏東為正 源角（雕角-搟顆體斕覷樹植姐 極為正姿態(tài)解算(attitude algorithm )也叫姿態(tài)分析或者姿態(tài)估計，即指飛控 系統(tǒng)或者慣導(dǎo)系統(tǒng)根據(jù)慣性測量單元(IMUU陀螺儀，加速度計，磁力計等傳感 器的實時信息求解飛行器的歐拉角的算法過程，因此也叫 IMU數(shù)據(jù)融合(IMU Data fusing )。633歐拉南的定軸轉(zhuǎn)動表示矩陣物物一切中.瞋歐檢由的定此可中粗的分制就次性系的jmx三地旃轉(zhuǎn)也*.明爵到機體坐

9、標率上但文三次腐輛的的第 為。兒幾兀力JT津津i.小兒匕& 0片九時】.第一次國境旋轉(zhuǎn)雹軸旋您5變成顯然此次旋轉(zhuǎn)新的坐標系與老坐標系之間的關(guān)系(投影)cos(W) sin(W)-sin(W) cos(W)00-cos(W) sin(W) 0庵=-sin(“)cos(W) 0-001第二次旋轉(zhuǎn)圍繞匕軸旋轉(zhuǎn)仇旋轉(zhuǎn)到OXzPzZz顯然此次旋轉(zhuǎn)新的坐標系與老坐標系之間的關(guān)系（投影）sin(6)0cos(0)匕zcos(8) 001sin(») 0Fcos(0) 0 sin(0)' 偌=010sin(J) 0 cos(6) 一第三次旋轉(zhuǎn)圍繞汗2軸旋轉(zhuǎn)巾，旋轉(zhuǎn)到OX/&Z

10、b顯然此次旋轉(zhuǎn)新的坐標系與老坐標系之間的關(guān)系（投影）1100Rg = 0 cos ） sing01 sin（p） cos（p）1d-疝間Q期陰雖然飛行器的姿態(tài)是隨機轉(zhuǎn)動，而并不是依次沿三軸旋轉(zhuǎn)而成，我們?nèi)匀?可以依靠歐拉角的三次旋轉(zhuǎn)來還原出飛行器的姿態(tài)信息。而且如果旋轉(zhuǎn)的三軸 順序不同，得到的旋轉(zhuǎn)矩陣也不同，并且歐拉角也不相等，三次旋轉(zhuǎn)先后組合 在一起即可以形成從慣性大地坐標系 e到機體坐標系b的旋轉(zhuǎn)矩陣：1 Q 。眄。用"即牌=Q哪的$瞰）0 I。-$i曬 cm（啾-M*期仲）網(wǎng)0卜疝喇-疝劭sin®*如口）»o（f） - ch（旬州（昉 如仲）如砌+如仲）（財

11、同 珈剛 ）CO心”疝的* ODE砌+ 5X1（。卜疝響 酬（*）*酬砌疝砌網(wǎng)+詞的 期明*儂伯T11 r12 r13, 房觸阻哪魏隔D.R” %hl儂，裁沱喻旗硯儺IMMRI r31 r32 r33,巾=arctan p = - arcsin(r13)仲=arctan(rn/r33)方向余弦矩陣的幾大缺陷抑S7閥，會注直蝴歷向親密電螭蝴算眥t因為朋余弦屬愚個統(tǒng)取部個元爵腰酬漉新勺曲奇異點這眄庭間上萩為萬向節(jié)滕佝曲皿叫隨域是當(dāng)三個雕中的中間這個雌達到641四元數(shù)的定義醞觸XI用于頓縊8 (及郵圖像分析)上標E蝴的踞引他糊于另懿嘯 O,器誦，娥)，四元嬲靛魴面的優(yōu)虹 強更快，提供平滑雕、布姆免萬

12、向蛹題、存網(wǎng)西畋小”.四無數(shù)(Quaternions)是由愛爾蘭數(shù)學(xué)家漢韻強 (WilliamRowan Hamilton, 18052865)在1843年提出來的數(shù)學(xué)枇念它將復(fù)數(shù)所湎幽三鯉間拓展到間，可以郭蝗酸更睚斷他然也可以以腳助.強尊捌螃!, < 聘運算醞fi主要包含四個元索qoqqq，其中%均為蝮四元數(shù)的表達網(wǎng):Q(flo-?r?2»?3)= % + ?ii + + ?3fc四元數(shù)艇算題J:i*i = j*j = k*k = -li*j = k,j*k = t,k*i = jy*i = -k,k*/ = -i,i*k = -j6.4.1四元數(shù)的定義醞州眄OMf”.典也是

13、o3ToMUk到3B,財哂崢*酚，任中勿易的幾何廉義可以3瞟成為一種直轉(zhuǎn).而四方由懶義為Qy,q 旗陣形式可以寫為Q =此外定義四旅的+ q/+q/ + qj. aftCHta四元,這表示單位四元數(shù).四酒的逆QT = Lt對于單位四元數(shù)Q-l = Q運n為相應(yīng)部位的加派，例如假設(shè)Q = q0+qj+qj+q3kP = P0+Pj WP31fQ±P = (%±Po)+(Qt ± P” + (/ 土 P2)J + 83 土 p3 M而p Q =(Po+p j+p j+/)&”+q?" q同=OVb - PNi - PW J +(Po% + PN( +

14、 Pz/ -23奧)'(Pi/h +%q。+PW| -%)/ +(Po +PWo +P1% -P2*)k= r0 + rj r“ r5t641四元數(shù)的我研形確以=0 1 I J q q q q. 3 1 -P%; 2 3-P-P&A pl詼巴-P2 1 4 p p p p>=m 八 nlnI=Q p或者IB 4 1J p p p p 3 CS3 0 1 2 3 H q q qb642四元數(shù)與旋轉(zhuǎn)的關(guān)系四元我是可以描述三間中腑的旋轉(zhuǎn)醫(yī)中三維空間(R3)中削咻上的任意一點Pk以)點要圍境通過原點的矢用抽y旋舞帷度,轉(zhuǎn)期(匕)點上.將P點用齊次坐標來表示為醞 數(shù):®M

15、.定義Q為任意單位四元數(shù)，則(月"“6/2)o5(8/2)代表的旋轉(zhuǎn)可以將喋期%y/M5w.首先，對于單位四元數(shù)Q如釉行標乘法的話，并不影響旋轉(zhuǎn)本身.圍境Q與$00為標卻所對應(yīng)的旋轉(zhuǎn)是一樣的，顯然(sQ)P(sQ)XQPQ«4s 1QAQ :=QPQ :=QPQ耿,任何四元數(shù)的實數(shù)郃分可以寫成% = (Q + Q.)/2,因此QPQ,的標實酬(QPQ，)="二等匚=也誓2二絲產(chǎn)=QR(P)Q=R(P).因此對于四元數(shù)P二p，M的實部在及過旋轉(zhuǎn)Q后不會改變，此外旋轉(zhuǎn)本身不會改變虛部矢部分的大小，因此|P| = |P'|.642四元數(shù)與旋轉(zhuǎn)的關(guān)系旋轉(zhuǎn),為事直嚙

16、與，點的共面目垂直于旋轉(zhuǎn)軸V.定義砒四元數(shù)唯反量部分底旋相畫上的投影矢曲0是四元即的矢量部分喈旋鼾面I影燈L 蝴當(dāng)我量圍頗電唱專渝度到2 謹餞設(shè)yl是該碓統(tǒng)姮能言獻1.譴歸蛛量均為單位 #矢艮這洋魏vOvl = es1由于應(yīng)喻垂直于旋轉(zhuǎn)投影面.低特柏0位矢量為v =窗孑現(xiàn)在我們定義用于描述該底喻四元BlQ = vl Z|EV1|vOa = (vOx vl,vO vl| = |vsuij.cos此外令v2 = QvOQ只要證明與“的夾角是JW裊用Oft從到，的俺裴四元ft ”到以的旋轉(zhuǎn)EjcRvZ vl, =QvO(?r = (vl vOJ>r = vOvO(vrvr),由于單25dtvO

17、vO = T, vlvl= -1,因itv2*v=Q：卜叫叫M，vO二這a聊2與八的相椒匕與幽平面重合，并映角醐,均筆 網(wǎng)以與幽夾角為e.酬3雅義的v2 = (MQ ,即是P知;傳平面上的投影，是VCS過Q所通述的貨轉(zhuǎn)四元數(shù)旋轉(zhuǎn)后的矢！(vO X vl)II VO X vlj|既然四元數(shù)可用來描述旋轉(zhuǎn),那么就可以用它來描述從大地坐標系e到機體坐標系b之間的轉(zhuǎn)換，也就是用它來描述6.3.1所介紹的方向余龍炬陣.在參考文獻2臬永元的慣性導(dǎo)航）一文中922節(jié)給出了四元數(shù)與方向余弦矩陣的對應(yīng)關(guān)系和詳細的推導(dǎo)過也這里簿要的給出一下結(jié)論機體以卬=3J+叼+恤A,旋轉(zhuǎn)運動.從QA/到（MW; u為垂直于臼的

18、單位眼時軸.自小二m經(jīng)運算可胤r，= （/ + UsE& +而嗎UU）d = DJ,從而得到網(wǎng)為機體坐標系b到大地坐標系的方向余弦矩陣,即:ggg=/+ l/2sin-cos- + 2sin*-47y2220 -nsln-msin j,， i0-(m2 +rr)sinx-0 1 0 + 2cos-0 Rsin-e-msin j/msinInsin/msin2 y-(f2 + n2)sin2 j* mnsm -Insin -一mnsm -» a 2°-(m2 + r)sin2 -RBUn n 0燈醐 勘Y叫 ybi* q產(chǎn)而叫 %=對Q 二 “ + 仇.。*.外心=3

19、；.(%.町.+ 成0)疝-二61. /血； 代A幅用會費鉗瑞城+q”4一諳2值曲-命%)2®%+q。4)=28兇2+用3)2%-研1)2(如也-qoq2) 2(和外 + QoQi) % -d-d + 4醞數(shù)與懶劃砌應(yīng)關(guān)系,因此采用醞期飛行圖姿態(tài)進行估計，值需要每個時鐘采樣硼腌環(huán)迭代運H出新的四元 數(shù)為444,就可以獲得方向余弦矩陣，從祓得實時的歐拉角信義如何在每個時鐘采樣周期通過新的傳感器信息送代出新的四元教信息呢？這就是采用K格庫塔法對四元嬲行粉運鼠由于匹元數(shù)Q = cos g + (如 + m/0 +*)sin % cos ； + u( sin 鵬期邊進行酚可以得到dQdti

20、9 de ,1 e de due e-sin - - + ue - - cos - - + sin 22dt22dtdt21/06= -6)e-l cos-4- ue-sin-j乙乙乙/11h= Q = 3Q 3b 乙乙1 , 、=5 (qO + qli + q2j + q3k) (a)xi + a)yj 4- a)zk)=。匕 Q 乙Jt'p因此采用一階龍格庫塔法,q(t + r)= q+ T *=q(E)+ 孫。Q展開后可以得到四元數(shù)每部分的更新(一山彳91 R尸 3蕓93)丁2(3,9。一 qa + 破)丁2(q3 + a)yqa -/qj T03= 93 +卡爾曼濾波卡爾曼濾波

21、是一種系統(tǒng)狀態(tài)觀測估計器，用來描述一個系統(tǒng)的輸入、輸出 和內(nèi)部遞歸迭代及重構(gòu)的方式?？柭鼮V波作為一個估計算法，主要基于系統(tǒng) 狀態(tài)空間的方法，因此也屬于第七章所談到的現(xiàn)代控制理論?？柭鼮V波使用 系統(tǒng)模型的迭代方程(或者微分方程)計算出理論輸出模型在于實際觀測相比 較，乘上比例因子形成狀態(tài)反饋，將系統(tǒng)狀態(tài)重構(gòu)偏差迭代消除，濾波器不但 對系統(tǒng)進行模型抽象，同時建立量測噪聲和系統(tǒng)噪聲模型，并通過測量與理論 的比較結(jié)果估計出噪聲能量?？柭鼮V波器是一種遞推線性最小方差估計的算法。對于解決大部分的問 題，是最優(yōu)，效率最高，也是最有用的?？柭鼮V波的廣泛應(yīng)用超過30年，包括機器人控制、導(dǎo)航，傳感器數(shù)據(jù)融

22、合，信號處理，計算機圖像處理等等。它 的特點主要是 無儲性EG)二心 即系統(tǒng)狀態(tài)的怙計值的數(shù)學(xué)期望等真徒 估計方差。(£) =以尸)一雙為工最小，即任何一個其他的估計.所對應(yīng)的 方差都大于濾波器的估計值的方差 實時性，采用遞推方式，減小運尊量，實時輸出運克濾波結(jié)果卡爾曼濾波公式:系統(tǒng)狀態(tài)方程.X# =巾*#7*4 +限-1 + Wr-iWjcT觀測公式：入="*見+/燈上7為索線狀態(tài)空間轉(zhuǎn)尊矩陣.為上時刻的凝統(tǒng)輸入量*為索統(tǒng)哄自嘉ft力系統(tǒng)啜聲，乩內(nèi)說刈翅陣，也為堀網(wǎng)噬聲-索統(tǒng)噪聲與羯嚏聲的統(tǒng)計將性為高斯白喉聲.= G,且些聲方壁姐 降為系統(tǒng)喚聲節(jié)差短庫： 丸 =QAi =

23、汽，測量嚶聲方差矩陣：/ =網(wǎng)品產(chǎn)離散卡爾曼濾波器的目的就是要通過系統(tǒng)上一時刻的狀態(tài)和輸入，以及當(dāng) 前時刻的觀測來估計出當(dāng)前時刻的狀態(tài)，并且它是最小方差的無偏估計。對于 線性系統(tǒng)來說，它也是這幾部分的線性疊加，即式父 = K潑kfk-i + K向t + Kk uk-i 杵曼源波式中曲幽,巾4崛魏獨騎趟趟醐酎蝴1帆蛔B.跚嗨茄城1加田I.下夙aii端晶地蟠tX.小,社局切械章空肥計.掰工憫的倒耨靛導(dǎo)出三個孤蜥”.人 更嘉晟九戰(zhàn)觸捺比幽Mb 卸3 -“ = %An =加-加=穴.*-1麻=| + %-1 +- 6L-1|*-L & %(心卜“內(nèi)T + Mt-1 + 口上-例1k-J + “

24、) -=隗上7 -此為中土b1 - £；%T + K；依=1 -八=斤1)+ Q -即%)%.1嗨=1 + 口 -兒加-端Uhl - &嶺要求E(品，二0.EPE/G =*N*Th J- Mi 凡H也上T -3F7)+人聞h.滴帆t) +(»凡瓦- (；)的-)卜口因為時111)=。. EWi);。，E(Vjt) = 0 且£(*1)¥0, Ed-1)中 o所助須足除卜1 -及必軋山7 - K =。設(shè)二外/T 一&此,心】.ft即f -KkHk-Kk = 0K； = 1- KkHk 將式口泗代同式中.得到均上二(外/一1 一 K春見/慮_

25、)鼠_情_i + KkZk +。一 K/Quri=巾*品-1他-1 + *k 體1 %他/一國-ill + uk-l) + ufc-l= fc|fc-l +雁f -之桃7)(式子4 )其中幾41 =中門-1心71 +見.它農(nóng)小的占山日葉制的系統(tǒng)狀態(tài)例通過條統(tǒng)建華來的汁3的勺前時刻向狀 志化iHk %1 = &輸1，它仍油當(dāng)前時刻估計值腦過觀瀾鹿也矩陣估計出來當(dāng)前時刻的雙窩量，它號力 到實際也割哂之間的差即為當(dāng)前時見的晚洲浙皂系統(tǒng)狀態(tài)估計言預(yù)測狀態(tài)+卡爾曼濾波系數(shù)*觀測新息這個卡爾曼澹被系數(shù)均是年次法代根指系統(tǒng)咕計和煤由佑計動態(tài)調(diào)整.根心堪中 T計算它，使得岫*是打法的最小方趨估計，即使將先席的方差矩陣生夬的值最小將式（式子4） ftUu中.得到:=£埠即丫廣備（小后十 口廣冊/* J=U- 4見&* -%或市一 i - v=i-小用 乂上中一 %n j -=（，一即跖）%tT - & ,為了計?？诒痊F(xiàn)幾出的方氈飾/加的