飛行姿態解算（一）喻時耕宇·4個月前大概六個月前研究的東西，貼出來給大家分享下，不知到有無同樣想搞四軸飛控的朋友，可能會感愛好。如果有業內算法工程師能一起討論就更加好拉。近來又來研究研究有關飛行姿態解算的算法，看看當時沒搞懂的四元數算法。重要也是學了網上幾位大神的資源，才干小有所得。在這先附上參考某些大神心得的網址：四元數維基百科向量維基百科【Unity技巧】四元數（Quaternion）和旋轉四元數和旋轉學習筆記—四元數與歐拉角之間的轉換四元數和歐拉角之間的轉換[小應用]GD32F103+MPU9150四軸飛行器第一步：9DOF姿態融合-GigaDeviceGD32MCU論壇9DOF姿態融合（需要登錄）尚有某些沒有統計。。忘記地址了。。諸多解釋上面帖子里都有了。我想從另一種方向來寫這篇文章。先來說說我理解這個東西的通過吧。說起對飛行姿態的解算，其實是相稱復雜的一種過程，并不像我們普通認為的那么簡樸。首先，一開始（涉及我自己）普通可能有個誤區：姿態不就一種傾角嘛（類比斜面）？因此感覺只需規定一種傾斜角而已，感覺挺簡樸的。認真一想，發現其實有姿態有2個角度，能夠想象一種滑梯再像側面翻滾一下，這樣其實對于空間思維不太好的人已有一點難度了。然后發現還需要再加一種是描述物體本身自旋的姿態。那么最后的結論為，姿態總共有3個軸。那么以我本身的知識來說，如何能夠檢測出物體姿態的這3個變量呢？總共需要3個傳感器。第一步：+加速度傳感器。說起測量傾斜角，普通能看見的都使用水泡，鉛垂線之類的東西。有傳感器使用經驗的第一感覺就會覺得使用重力傳感器就OK啦。首先說說重力傳感器的原理，這里說的重力傳感器又叫加速度傳感器。加速度計和陀螺儀指南(很具體的介紹)理解了這個東西后來，那么問題來了，僅僅靠加速度計能否完畢角度的測量？答案是在靜態狀況下能夠，在動態狀況下不能夠。加速度傳感器，從這個名字（以及上面的原理）也能夠看出，其測不是重力，而是重力引發的類似加速度帶來的效應。因此對于其它的加速度同樣會有讀數（運動狀態的變化），特別在震動的時候（震動狀態），該傳感器會有非常大的數據變化，此時的數據難以反映重力的實際值，因此結論是單靠加速度傳感器無法完畢姿態解算。第二步：+陀螺儀既然單靠加速度傳感器無法完畢姿態解算，那還需要添加哪些傳感器？通過上面資料我們能夠找到最少還需要的一種傳感器，陀螺儀。陀螺儀測量出的數據為繞各個軸的旋轉角速度。通過高等數學的知識能夠得出，對角速度進行積分，能夠得到旋轉角度。把旋轉角度加到之前測出的姿態上，會得到一種新的姿態，設為姿態A，通過加速度傳感器能夠算出來一種姿態B，這樣將這兩個姿態一定融合，就能夠得到一種比較精確的姿態，這也就是之后我們能夠進行姿態融合的基礎。第三步：+地磁場傳感器其實我們還缺一種傳感器，地磁場傳感器，其實他有個通俗的名字：電子羅盤。說到這，可能有人就已經懂得為什么需要這個傳感器了。當加速度傳感器完全水平的時候，能夠預料，重力傳感器無法分辨出在水平面旋轉的角度即繞Z軸的旋轉無法顯示出來，此時只有陀螺儀能夠檢測。于是又回到第一步時的問題。陀螺儀即使動態十分快速，但由于其工作原理是積分，因此在靜態會有累計誤差，體現為角度會始終增加或者始終減少。。于是我們會需要一種在水平位置能確認朝向的傳感器，這就是如今IMU（慣性導航單元）必備的第三個傳感器，地磁場傳感器，通過這3個傳感器的互相校正，我們終于在大的理論上能夠得到比較精確的姿態參數了?？偨Y一下前三步：1、擬定姿態我們需要三個軸上旋轉的數據。2、我們需要三個傳感器，分別為3軸加速度傳感器，3軸陀螺儀，以及3軸地磁場傳感器。完畢這三步，能夠說在大致姿態解算的框架上我已有了概念，但具體怎么做還是兩眼一抹黑啊。于是本著站在巨人的肩膀上做事的原則，我又開始漫長的資料搜索以及篩選。于是有了本文。第四步理解什么叫姿態請各位自帶筆和紙，復習或者預習大學高等數學，線性代數，復變函數，等數學知識，聽我慢慢回想的學習的過程。。。既然我始終說姿態解算，姿態解算，那究竟什么是姿態。上圖，我們想象一種飛機在天空飛行，他可能側傾，可能仰俯，也可能旋轉，這就是我們平時所說的姿態，那么這個姿態抽象出來意味著什么呢？這里是重點啦：姿態，就是一種坐標系與另一種坐標系的轉換關系。這個可能比較抽象哈。首先在飛機上建立一種坐標系，X軸為機翼的方向，Y軸為機頭的方向，Z軸垂直于飛機，這個坐標系是隨著飛機姿態變化而變動的，此時就規定飛機的姿態，就等價于求出這個坐標系，那么如何得到這個坐標系？要得到一種新的坐標系，首先要有一種參考坐標系，普通選用Y軸正向為正北，x正向為正東，z軸垂直于地平面了，給他取個名字，就叫地理坐標系吧。因此此時，我們所說的姿態其實就是飛機坐標系和地理坐標系（是固定的）的一種關系?，F在我們終于能夠進入第五步啦。第五步：如何表達這種關系。找到了表達這種關系的辦法，就能夠運用這個辦法隨意轉換這2個坐標系。先上結論吧。有三種關系表達方式：1、歐拉角2、余弦矩陣3、四元數1、歐拉角歐拉角是很直觀的，一種物體的旋轉，能夠分解到三個坐標軸上的旋轉。這三個旋轉角度就是歐拉角。在慣性系統中普通把這三個角度分別稱為roll，pitch，yaw。上個直觀的圖，很容易理解。用歐拉角來描述物體的旋轉不光需要有角度，還需要有旋轉次序，普通旋轉次序是先yaw后pitch，再roll反映到坐標軸上就是先繞Z軸旋轉，再繞X軸旋轉，最后繞Y軸旋轉。為什么有次序呢？與否能夠沒次序？如果身邊有東西能夠轉轉看，這個問題之后在理論上會進行闡明。需要注意的是yawpitchroll都是對應的固定的參考系也就是上面說的地理坐標系而言，每次新的姿態坐標系都是由地理坐標系通過歐拉角旋轉得到的。這樣我們就用歐拉角表達了物體的姿態。歐拉角是有諸多優點的。但是也有致命的缺點，那就是GimbalLock（萬向節死鎖），要理解GimbalLock所說的狀況（可能有點難）讓我們看個現實中的場景。假如我們有一種望遠鏡和一種用來放望遠鏡的三腳架，（我們將）三腳架放在地面上，使支撐望遠鏡的三腳架的頂部是平行于地平面（參考平面）的，方便使得豎向的旋轉軸（記為x軸）是完全地垂直于地平面的。現在，我們就能夠將望遠鏡饒x軸旋轉360度，從而觀察（以望遠鏡為中心的）水平包圍圈的全部方向。普通將正北朝向方位角度記為0度方位角。第二個坐標軸，即平行于地平面的橫向的坐標軸（記為y軸）使得望遠鏡能夠饒著它上下旋轉，普通將地平面朝向的仰角記為0度，這樣，望遠鏡能夠向上仰+90度指向天頂，或者向下-90度指向腳底。好了，萬事俱備。現在，天空中（涉及地面上）的每個點只需要唯一的一對x和y度數就能夠擬定。例如x=90度,y=45度指向的點是位于正東方向的半天空上?，F在，看看萬向節死鎖是怎么發生的。一次，我們探測到有一種飛行器貼地飛行，位于望遠鏡的正東方向（x=90度，y=10度），朝著我們直飛過來，我們跟蹤它。飛行器飛行方向是保持x軸角度90度不變，而y向的角度在慢慢增大。隨著飛行器的臨近，y軸角增加的越來越快且當y向的角度達成90度時（即將超越），忽然它急轉彎朝南飛去。這時，我們發現我們不能將望遠鏡朝向南方（在只繞一種軸旋轉的狀況下），由于此時y向已經是90度！造成我們失去跟蹤目的。這就是萬向節死鎖！為什么說不能將望遠鏡朝向南方呢，讓我們看看坐標變化，從開始的（x=90度，y=10度）到（x=90度，y=90度），這個過程沒有問題，望遠鏡慢慢轉動跟蹤飛行器。當飛行器達成（x=90度，y=90度）后，坐標忽然變成（x=180度，y=90度）（由于朝南），x由90突變成180度,因此望遠鏡需要饒垂直軸向x軸旋轉180-90=90度方便追上飛行器,但此時，望遠鏡已經是平行于x軸，我們懂得饒平行于本身的中軸線的的旋轉變化不了朝向，就象擰螺絲同樣，螺絲頭的指向不變。因此望遠鏡的指向還是天頂。而后由于飛行器飛遠，坐標變成（x=180度，y<90度）時，y向角減小，望遠鏡只能又轉回到正東指向，望'器'興嘆。這闡明用x,y旋轉角（又稱歐拉角）來定向物體有時并不能按照你想像的那樣工作，象上面的例子中從（x=90度，y=10度）到（x=90度，y=90度），按照歐拉角旋轉確實能夠對的地定向，但從（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y<90度）,按照歐拉角旋轉后的定向并非對的。我的理解是坐標值的變化和飛行器空間的位置變化一一對應，但是從（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y<90度）這個變化，飛行器位置是持續的變化，但坐標值的變化卻不是持續的（從90突變到180），其因素在于（x=90度，y=90度）和（x=180度，y=90度）甚至和（x=任意度，y=90度）這些不同的坐標值對應空間同一種位置，這種多個坐標值對應同一種位置的不一致性是造成死鎖的本源。上面是2維坐標系中的例子，同樣，對于3維的也同樣。例如有一種平行于x軸的向量，我們先將它饒y旋轉直到它平行于z軸，這時，我們會發現任何饒z的旋轉都變化不了向量的方向，即萬向節死鎖，因此說傳統的歐拉角是不能做到全姿態解析的。2.方向余弦矩陣。說方向余弦矩陣之前，先討論方向余弦。一種向量的方向（姿態）我們能夠用他在參考坐標系（地理坐標系）各個軸向的夾角的余弦來表達（及在各個軸的投影）。類似的一種坐標系能夠當作是3個向量構成，因此三個向量分別在坐標軸上的投影能夠用來表達一種坐標系與參考坐標系的關系。這總共9個方向余弦構成了一種三階矩陣，其對應方式以下圖。第i行、j列的元素表達參考坐標系i軸和姿態坐標系j軸夾角的余弦。事實上方向余弦和歐拉角沒有本質區別，由于方向余弦事實上就是用歐拉角表達的。下面附上推倒具體體現式的辦法先從二維坐標系轉換開始。推廣到三軸的單次旋轉，我們用矩陣表達為（繞Z軸旋轉）：這里要說一下矩陣的含義，C21表達坐標系1到坐標系2的變換矩陣，那么有這樣我們能夠得到3個變換矩陣分別為單獨繞Z軸旋轉，繞X軸旋轉，繞Y軸旋轉。事實上，兩坐標系任何復雜的角位置關系都能夠看做有限次基本旋轉的組合，變換矩陣等于基本旋轉擬定的變換矩陣的連乘（線性代數），連乘的基本次序根據基本旋轉的次序向右排列。之因此有次序是由于矩陣有“左乘”和“右乘”之分（還是線性代數）。那么我們得到：最后的矩陣就是完整的余弦矩陣。γ、θ、ψ就是歐拉角啦。至此我們解釋了為什么歐拉角會有旋轉的次序之分。從以上數學計算能夠看出不同的旋轉次序會帶來不同的成果。四元數：四元數要介紹的太多了。。由于他優點有諸多，運用起來也很方便，但是理解起來太抽象了。百度四元數，一開始看到的就是四元數來歷，尚有就是四元數的基本計算。對于來歷，還是想說一下，四元數（Quaternions）是由威廉·盧云·哈密爾頓(WilliamRowanHamilton,1805-1865）在1843年愛爾蘭發現的數學概念（百度百科）。將實數域擴充到復數域，并用復數來表達平面對量，用復數的加、乘運算表達平面對量的合成、伸縮和旋，這就是我們熟知的復數的二維空間含義，因此人們會繼續猜想，運用三維復數不就能夠體現三維空間的變換了嗎，歷史上有諸多數學家試圖尋找過三維的復數，但后來證明這樣的三維復數是不存在的。有關這個結論的證明，我沒有查到更明確的版本，據《古今數學思想》中的一種理由，三維空間中的伸縮旋轉變換需要四個變量來決定：兩個變量決定軸的方向，一種變量決定旋轉角度，一種變量決定伸縮比例。這樣，只有三個變量的三維復數無法滿足這樣的規定。但是歷史上得到的應當是比這個更強的結論，即使不考慮空間旋轉，只從代數角度來說，三維的復數域作為普通復數域的擴張域是不存在的。并且，據《古今數學思想》敘述，即使像哈密爾頓后來引入四元數那樣，犧牲乘法交換律，這樣的三維復數也得不到。通過某些年的努力之后，Hamilton發現自己被迫應作兩個讓步，第一種是他的新數包含四個分量，而第二個是他必須犧牲乘法交換律。（《古今數學思想》第三冊177頁）但是四元數用作旋轉的作用明顯，簡化了運算，并且避免了GimbalLock。四元數是最簡樸的超復數，我們不能把四元數簡樸的理解為3D空間的矢量，它是4維空間中的的矢量，也是非常不容易想像的。那什么是四元數呢？在維基百科中截取一段：復數是由實數加上虛數單位i構成，其中相似地，四元數都是由實數加上三個元素i、j、k構成，并且它們有以下的關系：每個四元數都是1、i、j和k的線性組合，即是四元數普通可表達為要把兩個四元數相加只需將同類的系數加起來就能夠，就像復數同樣。那么四元數如何表達旋轉呢？再截取維基百科上的一段話群旋轉主條目：四元數與空間旋轉非零四元數的乘法群在R3的實部為零的部分上的共軛作用能夠實現轉動。單位四元數（絕對值為1的四元數）若實部為cos(t)，它的共軛作用是一種角度為2t的轉動，轉軸為虛部的方向。提取幾個核心詞：四元數表達轉動，首先其模應當為1，以下這個值應當為1。此時實部，也就是a若等于cos(t)則他的共軛作用為一種2t的轉動，轉軸方向為向量(b,c,d)的方向。那什么叫共軛作用？事實上就是四元數表達旋轉的方式。那么運用四元數代表旋轉是如何實現的，在載體系定義的一種矢量rb能夠直接運用四元數將其在參考系中表達為rn。首先定義一種四元數rb'，它的虛部等于rb的對應分量，標量分量為零：參考系中的rn’表達為這也就是共軛作用。，其中q=a+bi+cj+dk，q*為q復共軛，q=a-bi-cj-dk及因此有：寫成矩陣式：由于都表達旋轉，因此這個矩陣理論上應當和余弦矩陣是等效的，從而就能計算歐拉角了。上圖顯示了四元數和余弦矩陣的關系。這樣反求出歐拉角以下圖：此時就能夠根據四元數反求出歐拉角了。飛行姿態解算（二）喻時耕宇·4個月前繼之前研究了某些飛行姿態理論方面的問題后，又找到了之前很流行的一段外國大神寫的代碼，來分析分析。先貼上代碼：#include"AHRS.h"http://----------------------------------------------------------------------------------------------------//Definitions#defineKp2.0f#defineKi0.005f#definehalfT0.5f//----------------------------------------------------------------------------------------------------//Variabledefinitionsfloatq0=1,q1=0,q2=0,q3=0;floatexInt=0,eyInt=0,ezInt=0;//----------------------------------------------------------------------------------------------------//FunctionvoidAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz){floatnorm;floathx,hy,hz,bx,bz;floatvx,vy,vz,wx,wy,wz;floatex,ey,ez;floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3;norm=sqrt(ax*ax+ay*ay+az*az);ax=ax/norm;ay=ay/norm;az=az/norm;norm=sqrt(mx*mx+my*my+mz*mz);mx=mx/norm;my=my/norm;mz=mz/norm;hx=2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);hy=2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1);hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2);bx=sqrt((hx*hx)+(hy*hy));bz=hz;vx=2*(q1q3-q0q2);vy=2*(q0q1+q2q3);vz=q0q0-q1q1-q2q2+q3q3;wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);ex=(ay*vz-az*vy)+(my*wz-mz*wy);ey=(az*vx-ax*vz)+(mz*wx-mx*wz);ez=(ax*vy-ay*vx)+(mx*wy-my*wx);exInt=exInt+ex*Ki;eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;gx=gx+Kp*ex+exInt;gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt;q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0=q0/norm;q1=q1/norm;q2=q2/norm;q3=q3/norm;}代碼不長，但是數學理論基礎卻是十分扎實的，必須要要有之前（一）的鋪墊。一段段分析代碼：#defineKp2.0f#defineKi0.005f#definehalfT0.5ffloatq0=1,q1=0,q2=0,q3=0;floatexInt=0,eyInt=0,ezInt=0;前三行是某些常數的定義，涉及Kp參數，Ki參數，采樣時間。后兩行是某些變量的聲明，q0，q1，q2，q3分別代表四元數的4個參數a，b，c，d，v=q0+q1i+q2j+q3kexInt，eyInt，ezInt代表誤差的積分項。這個背面會有闡明。voidAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz)floatnorm;floathx,hy,hz,bx,bz;floatvx,vy,vz,wx,wy,wz;floatex,ey,ez;來說說這些變量代表的意義，再注意各個參數是相對于哪個參考系的。這是算法理解的重點。先說形參的gxgygz表達陀螺儀讀出的數據，分別為繞飛行器參考系x軸，y軸，z軸正方向旋轉的角速度。axayaz表達加速度計讀出的數據，分別為重力加速度在飛行器參考系x軸，y軸，z軸的分量。mxmymz表達磁場傳感器讀出的數據，分別為地磁場在飛行器參考系x軸，y軸，z軸的分量。norm是做歸一化的中間變量。hx,hy,hz表達將飛行器參考系上的地磁矢量轉換到地理坐標系（參考坐標系）后的矢量。bx，bz與上面同樣，（注意）區別是bx在算法里為sqrt（hx*hx+hy*hy）這事實上是建立了一種誤差函數，bx越靠近hx，則整個預計姿態與電子羅盤測得的姿態越重疊。vx,vy,vz為將原則單位重力轉換到飛行器參考系后各個坐標軸上的分量。（重力加速度）wx,wy,wz為將bx與bz又重新轉換到飛行器參考系后各個坐標軸上的分量。（地磁場）這個地方是比較難理解的，為什么轉換過來又轉換回去，背面會說。ex,ey,ez為向量a和向量m，與向量v和向量w的外積的在飛行器參考系的向量。該向量描述了a和m與v和w的偏差程度。floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3;這是些預先計算的數據，方便背面計算使用。norm=sqrt(ax*ax+ay*ay+az*az);ax=ax/norm;ay=ay/norm;az=az/norm;norm=sqrt(mx*mx+my*my+mz*mz);mx=mx/norm;my=my/norm;mz=mz/norm;對加速度傳感器和磁場傳感器得到的數值進行歸一化，方便計算。hx=2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);hy=2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1);hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2);運用四元數變換坐標系。具體為什么這樣變換上一張圖，筆記一里有具體過程。上圖為從飛行器坐標系轉換到地理坐標系的余弦矩陣的四元數體現。bx=sqrt((hx*hx)+(hy*hy));bz=hz;bx在當由磁場得到的飛行器參考系和由全部參數融合得到的飛行器參考系重疊的時候理論上是等于hx的，此時hy應當等于0vx=2*(q1q3-q0q2);vy=2*(q0q1+q2q3);vz=q0q0-q1q1-q2q2+q3q3;反向使用上圖余弦矩陣的四元數體現。及把a替代成-a，原理為四元數體現旋轉時，a為旋轉的角度，因此想旋轉回去只需要把a變成-a。(vx,vy,vz)為一原則重力在飛行器參考系上的矢量。wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);同理，反向使用余弦矩陣的四元數體現。這里可能大家已經發現，怎么wxwywz是由傳感器得到的。ex=(ay*vz-az*vy)+(my*wz-mz*wy);ey=(az*vx-ax*vz)+(mz*wx-mx*wz);ez=(ax*vy-ay*vx)+(mx*wy-my*wx);e為誤差向量他是向量a和v的外積加上m和w的外積。exInt=exInt+ex*Ki;eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;對誤差進行積分；gx=gx+Kp*ex+exInt;gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt;用誤差的積分和誤差本身與Kp的乘積的和對角速度進行賠償。q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;運用龍格-庫格法求出四元數的值，此值由賠償后的角速度求出。norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0=q0/norm;q1=q1/norm;q2=q2/norm;q3=q3/norm;對四元數進行歸一化。至此算法結束。剩余的事情就是把四元數轉化為歐拉角，即可求出我們能夠理解的飛行器的姿態yaw，roll，pitch另外稍微分析下這個算法的實現原理。a與v的外積成果為sinθ|a||v|nn為垂直于a，v構成平面的單位向量，方向恪守右手定理。由于都是歸一化的，因此，這個成果的大小與θ有關，這個θ為a與v的夾角，由于a與v在halfT時間內的區別應當非常小，因此，此時θ=sinθ，這時候能夠說這個外積的每個參數與θ成正比。這個誤差是怎么來的？就是由于飛行器在這短時間旋轉了一定角度。而從另一種角度，如果單純從陀螺儀，同樣能夠得到飛行器旋轉的一種角度。這個時候就有個問題？我們選擇相信誰的？我們誰也不相信，而是選擇按一定比例相信了3個參數，e（誤差），g（陀螺儀角速度）和eInt（誤差的積分），這樣我們就融合了這三個參數，最后得到我們的姿態。PS：此方式在數學上在算法上沒有任何問題，但是由于地磁傳感器極易受到多種干擾（想想百度地圖中的指南針精確度），而此算法又將地磁傳感器所批示的方向過分的融入到了姿態當中，造成實際使用中數據會非常不穩定。我也是在此之后才發現。因此之后我使用了另一種融合算法，這個留到下一次再說。飛行姿態解算（三）喻時耕宇·4個月前繼之前研究了某些飛行姿態理論方面的問題后，又找到了之前很流行的一段外國大神寫的代碼，來分析分析。第二篇文章的最后，講到了文章中的算法在實際使用中有重大缺點。大家都懂得，分析算法理論的時候諸多狀況下我們沒有考慮太多外界干擾的狀況，因素是諸多狀況下，傳感器的精度以及受到的干擾并不會特別大，而明顯的影響到算法。但是在IMU系統中，有點不同。由于地磁場十分微弱，而我們生活中有大量使用電子設備，使得磁場非常的混亂，以至于地磁傳感器非常容易受到干擾。由于以上算法把地磁傳感器一同加入到姿態的測定中，并基本予以了地磁傳感器與加速度傳感器同樣的加權，造成地磁傳感器一旦被干擾，會對姿態產生地球重力忽然被干擾同樣的成果。。。對姿態的測量是消滅性的。綜上，考慮到磁場的不穩定性，必須對地磁傳感器進行降權解決，使得他對姿態的影響變小。于是設計了下列的算法。將磁場傳感器的數據在姿態角度中剔除，更新姿態的俯仰角（PITCH）以及橫滾角（ROLL）的時候只使用加速度傳感器以及陀螺儀(角速度傳感器)。只在計算偏航角的（YAW）的時候使用磁場傳感器。也就是只使用磁場傳感器作為一種電子指南針，定位整個姿態在水平面旋轉的角度。這樣設計，讓磁場傳感器只影響姿態中的一種數值，減少了磁場的權重，即使磁場收到干擾，也不會造成姿態驟變，使得四軸墜機。在對YAW進行計算的時候使用了以下函數。eulerAngleRaw.yaw=0.9*(eulerAngleRaw.yaw-gzF*2*halfT)+0.1*angleMagYaw;此處的0.9和0.1是能夠變動的但他們相加應當為1，此處為一種最簡樸的1階低通濾波器，增加0.1則是增大截止頻率。算法的流程圖是這樣的：程序流程圖新的姿態更新算法是這樣的voidAHRSupdate(floatgxf,floatgyf,floatgzf,floataxf,floatayf,floatazf,floatmxf,floatmyf,floatmzf){doublenorm;floatvx,vy,vz;floatex,ey,ez;floathalfT;//采樣周期的二分之一//輔助變量，以減少重復操作數floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3;//測量歸一化norm=invSqrt(axf*axf+ayf*ayf+azf*azf);axf=axf*norm;//向量a為傳感器重力飛行器分量ayf=ayf*norm;azf=azf*norm;//norm=invSqrt(mxf*mxf+myf*myf+mzf*mzf);//向量m為傳感器磁場飛行器分量//mxf=mxf*norm;//myf=myf*norm;//mzf=mzf*norm;//計算參考磁通方向//hx=2*mxf*(0.5-q2q2-q3q3)+2*myf*(q1q2-q0q3)+2*mzf*(q1q3+q0q2);//向量h為磁場通過旋轉后來參考系分量//hy=2*mxf*(q1q2+q0q3)+2*myf*(0.5-q1q1-q3q3)+2*mzf*(q2q3-q0q1);//hz=2*mxf*(q1q3-q0q2)+2*myf*(q2q3+q0q1)+2*mzf*(0.5-q1q1-q2q2);//bx=1.0f/invSqrt((hx*hx)+(hy*hy));//原則上應當只有X向的分量ex的磁場傳感器部分