1、姿態(tài)的描述方

姿態(tài)解算理前幾天在里偶爾看到一個帖子，帖子的內(nèi)容是問的為什么不用傾斜角表示姿態(tài)，我源于我們?nèi)粘Ｉ畹乃季S。1比一個立方體，我們放在水平面上的時候它的底面是和水平面平行的，但是當我們把立方體的一個腳墊起一個角度時，這樣一來，立方體的一條棱與水平面的垂線就有了一定的夾角了。我們所說傾斜了多少多少度就是指的這個夾角，這是我們直觀的反應(yīng)。我認為這樣直觀的反應(yīng)甚至比歐拉角還要來的直觀，因為歐拉角是基于旋轉(zhuǎn)的，肯定不會說這個立方體X、Y軸各旋轉(zhuǎn)了多少度（假設(shè)Z旋轉(zhuǎn)無效，我們可能也沒那個概念，我們直觀的反應(yīng)就是它傾斜了一定度數(shù)。表達但不適合數(shù)學上的計算，還有就是我們僅僅知道這個傾斜角我們怎么施加控制量呢？高中物理學習物體運動的時候我們知道，物體的運動是合運動，我們可以把它的運動矢量正交分解為幾個運動的合成（不正交也是可以的，但那不是在自找麻煩嗎，同樣道理，我們可以把剛體的旋轉(zhuǎn)分解為三個軸上的旋轉(zhuǎn)，這個旋轉(zhuǎn)的角度就是歐拉角，如圖2。

圖 歐拉角的定義不僅僅和旋轉(zhuǎn)角度有關(guān)系，還和旋轉(zhuǎn)軸的旋轉(zhuǎn)順序有關(guān)系，任何一種旋轉(zhuǎn)順序是合法的根據(jù)定義，拉角有12種旋轉(zhuǎn)序（，一個物體通過任意一個旋轉(zhuǎn)順序都可以達到同樣的姿態(tài)，在各個學科里所以為了統(tǒng)一，航空航天領(lǐng)域規(guī)定XYZ為歐拉角的旋轉(zhuǎn)順序。上面已經(jīng)說了歐拉角的定義。歐拉角的定義也是很直觀而且容易理解的，也利于我們的計算，因為我們用的慣件也是按照單個軸向運動來測量的。定義上的歐拉角還和我們所說的aw、Pitch、Roll不是一回事。因為定義上的歐拉角就是剛體繞三個軸的旋轉(zhuǎn)角度，旋轉(zhuǎn)和外界的東西（參考系）是沒有關(guān)系的。awPitch、Roll就是載體對于參考系來（這也是產(chǎn)生GimbalLock的原因，下面會講到。我們現(xiàn)在說的飛機都是在近地表附近飛行，所以我們習慣是拿地球作為參考系，我們的飛Z軸旋轉(zhuǎn)30°，這個旋轉(zhuǎn)的角就是我們所說的aw，同樣，繞飛機的X軸旋轉(zhuǎn)30°，我們得到Pich，繞飛機Y軸旋轉(zhuǎn)得到Roll，如圖3。這和規(guī)定角的旋轉(zhuǎn)順序是一樣的，所以說是和角是一一對應(yīng)的，要注意角是也沒有實際的意義，來個更直觀的：理解GimbalLock所說的情況，這可能有點不好理解，讓我們看個現(xiàn)實中的場景。假我們有一望遠鏡和一用來放望鏡的三腳，（）三腳架放在地面上，使支撐望遠鏡的三腳架的頂部是平行于地平面（參考平面（記為x軸是完全地垂直于地平面的?，F(xiàn)在，我們就可以將望遠鏡饒x軸旋轉(zhuǎn)360（以望遠鏡為中心的0度方位角。第二個坐（記為y軸使得望遠鏡可以饒著它上下旋0度，這樣，望遠鏡可以向上仰+90-90度指向腳底。好了，萬事俱備。現(xiàn)在，天空中（包括地面上）的每個點只需要唯一的一對x和y度數(shù)就可以確定。比如x=90度,y=45度指向的點是位于正東方向的半天空上?，F(xiàn)在，看看萬向節(jié)死鎖是怎么發(fā)生的。一次，我們探測到有一個飛行器貼地飛行，位于望遠鏡的正東方向（x=90度，y=10度，朝著我們直飛過來，我們它。飛行器飛行方向是保持x軸角度90度不變，而y向的角度在慢慢增大。隨著飛行器的，y軸角增長的越來越快且當y向的角度達到90（即將，它轉(zhuǎn)南。時們我們不能將望遠鏡朝向南方，因為此時y向已經(jīng)是90度，造成我們失去目標。這就是萬向節(jié)死鎖！為什么說不能將望遠鏡朝向南方呢，讓我們看看坐標變化，從開始的（x=90度，y=10度）到（x=90度，y=90度，這個過程沒有問題，望遠鏡慢慢轉(zhuǎn)動飛行器。當飛行器到達（x=90度，y=90度）后，坐標突然變成（x=180度，y=90度（因為朝南，x由90突變成180度,所以望遠鏡需要饒垂直軸向x軸旋轉(zhuǎn)180-90=90度以便追上飛行器,但此時，望遠鏡已經(jīng)是平行于x絲一樣，螺絲頭的指向不變。所以望遠鏡的指向還是天頂。而后由于飛行器飛遠，坐標變成（x=180度，y<90度）時，y向角減小，望遠鏡只能又轉(zhuǎn)回到正東指向，望'器'興嘆。這說明用x,y旋轉(zhuǎn)角（又稱角）來定向物體有時并不能按照你想像的那樣工作，象上面的例子中從（x=90度，y=10度）到（x=90度，y=90度，按照角旋轉(zhuǎn)確實可以正確地定向，但從（x=90度，y=90度）到（x=180度，y=90度，再到（x=180度，y<90度）,按照角旋轉(zhuǎn)后的定向并非正確。我的理解是坐標值的變化和飛行器空間的位置變化一一對應(yīng)，但是從（x=90度，y=90度）到（x=180度，y=90度（x=180度，y<90度）這個變化，飛行器位置是連續(xù)的變化，但坐標值的變化卻不是連續(xù)的（從90突變到180，其原因在于（x=90y=90度）（x=180y=90度）（x=任意度，y=90度）這些不同的坐標值對應(yīng)空間同一個位置，這種多個坐標值對應(yīng)同一個位置的不一方向余一個向量在坐標系中的位置也可以用方向余弦表示，也就是這個向量分別到三個坐標軸的夾角余弦值，實際上就是這個向量到各個坐標軸的投影啦，角度范圍是0~π（以推廣到載體坐標系和參考坐標系當中，我們就有了載體坐標軸xyz分別與參考軸XYZ的方向余弦，這里就是所說的方向余弦矩陣了，它是由兩組不同的標準正交基的基底向量之間的方向余弦所形成的3x3:ijij特別是《慣性導航》有推導的過程推廣到三軸的單次旋轉(zhuǎn)，我們用矩陣表示為這里要說一下矩陣的含義，C21表示坐標系1到坐標系2的變換矩陣，那么這是Z軸的基本旋轉(zhuǎn)，我X2、Y2投影0，Z2投影1，這是為什么呢？自。121軸。imu.c里面就有直接的計算。那么單獨旋轉(zhuǎn)各個 么我們得到：按上面推論，此矩陣的下面三個就是三軸對應(yīng)的方向余弦，γ、θ、ψ在明白了角和方向余弦矩陣的關(guān)系了吧。了。我們四元數(shù)，一開始看到的就是四元數(shù)來歷，還有就是四元數(shù)的基本計算。對于來歷，還是想說一下，四元數(shù)（Quaternions）是由威廉··哈密爾頓(WilliamRowanHamilton,1805-1865）在1843年愛爾蘭發(fā)現(xiàn)的數(shù)學概念（百科。合成、伸縮和旋，這就是我們熟知的復數(shù)的二含義，所以人們會繼續(xù)猜想，利用三維復數(shù)不就可以表達三的變換了嗎，歷史上有很多數(shù)學家試圖尋找過三維的復數(shù)，但后來證明這樣的三維復數(shù)是不存在的。有關(guān)這個結(jié)論的證明，我沒有查到更明確的版本，據(jù)《古今數(shù)學思想》中的一個理由，三中的伸縮旋轉(zhuǎn)變換需要四個變量來決定：兩個變量復數(shù)這樣的要求。但是歷史上得到的應(yīng)該是比這個更強的結(jié)論，即使不考慮空間旋轉(zhuǎn)，復數(shù)也得不到。經(jīng)過一些年的努力之后，Hamilton發(fā)現(xiàn)自己被迫應(yīng)作兩個讓步，第一個是他的新數(shù)包含四個分量，而第二個是他必須犧牲乘法交換律（《古今數(shù)學思想》第三冊177頁）但是四元數(shù)用作旋轉(zhuǎn)的作用明顯，簡化了運算，而且避免了GimbalLock，四元數(shù)是最簡單的超復數(shù)，我們不能把四元數(shù)簡單的理解為3D空間的矢量，它是4中的的矢量，1、四元數(shù)的表示方式2、四元數(shù)的計算關(guān)于四元數(shù)的基本運算，加減乘除，大家從別的地方看看吧，百科要注意的是，四元數(shù)的元的平方定義為-1的性質(zhì)決定的。四元數(shù)由一個實數(shù)和三個虛數(shù)構(gòu)成，所以是一個空間的向量，但是它的三個虛數(shù)又有三的性質(zhì)，因此，三中的一個矢量，可以看作一個實部為0的四元數(shù)，這個四元數(shù)是這個三的一個矢量在空間里的“映像”，或者叫這個矢量的“四元數(shù)映像”。這樣，我們就把三和一個空間聯(lián)系起來，用空間中四元數(shù)的性質(zhì)和運算規(guī)律來研究三中剛體定點轉(zhuǎn)動的問題。四元數(shù)的乘法不可逆性，這里說的是四元數(shù)的外積，定義兩個四元數(shù)它們相乘得到

得到內(nèi)機，而且還得到外積！這是由四元數(shù)運算性質(zhì)決定的。qp乘積的向量部分是:另一個坐標系的變換可以通過繞一個定義在參考坐標系中的矢量μ的單次轉(zhuǎn)Qabicj+dk[w，vw=a，v=bicj+dk那么，v是矢量，表示三里的旋轉(zhuǎn)軸。w標量，表示旋轉(zhuǎn)角度。所以，定義μμμ體坐標系重合。這可能不好理解，為什么說繞矢量μ還轉(zhuǎn)動μ，(μx/μ)就相當于 是繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度，為旋轉(zhuǎn)軸在x,y,z方？那么利用四元數(shù)代表旋轉(zhuǎn)是如何實現(xiàn)的，rb可以直接rnrb'rb的相應(yīng)參考系中的rn’表示為，其中q=a+bi+cj+dk，q*的復共軛。因此有寫成矩陣式這個矩陣就對應(yīng)方向余弦矩陣了，從而對應(yīng)角了肯定還有沒回過神的上面僅僅說明了一“結(jié)果要直觀說清楚四元如何工作確實有點抽。我們把里點用部為0的四元數(shù)表示成q=bi+cj+dk，我們可以理解為虛部就是表示我們生活的三，那么我們把q 為了更清楚地看到兩個四元數(shù)乘積到底是什么樣子，我們把上節(jié)用到的向量空間的觀拿過來，四元數(shù)的全體構(gòu)成的集合F是實數(shù)域上的向量空間，可以把四元數(shù)q=a+bi++dk看成實數(shù)元組（a，b，c，d。然后用另外一個四元數(shù)Q左乘q，這個運算就相當于q在空間F上的線性變換，這就如同兩個虛數(shù)相乘（1+i）*（2+i）=1+3i（2+i）在復數(shù)坐標系上的一個線性變換——大小和方向都發(fā)送了變化。如果Q是個純實數(shù)（虛部為0的四元數(shù)，那么Q*q就是q簡單的伸縮比例，方向什么的沒有發(fā)生變化；如果Q是個虛部不為0的四元數(shù)，同樣道理q而，通過比較上述等式的各個元素，四元數(shù)可以直接用角或方向余弦表示。同樣地，角也可以用方向余弦或四元數(shù)表示。用方向余弦表示四元用角直接表示四元數(shù)用方向余弦表示角可以按下述方法直接由方向余弦推導出角。當0！=90。時，角可由下關(guān)于傳感器，價格貴的肯定性能就好點。還有就是傳感器的安裝方向，個人喜好X模6、數(shù)數(shù)據(jù)融合也算是姿態(tài)解算的了，好的解算算法可以更好的補償傳感器誤差，目前就研究了里的imu.c濾波思想，感覺很巧妙。

////Quaternionimplementationofthe'DCMfilter'[Mayhonyet//Usermustdefine'halfT'asthe(sampleperiod/2),andthefiltergains'Kp'and//Globalvariables'q0','q1','q2','q3'arethequaternionelementsrepresentingthe// Seemyreportforanoverviewoftheuseofquaternionsinthis//Usermustcall'IMUupdate()'everysampleperiodandparsecalibratedgyroscope('gx',//andaccelerometer('ax','ay','ay') //unitsareirrelevantasthevectoris #defineKi0.008f //integralgainernsrateofconvergenceofgyroscopebiases#definehalfT0.001f //halfthesampleperiod采樣周期的一半floatq0=1,q1=0,q2=0,q3=0; //quaternionelementsrepresentingtheestimatedfloatexInt=0,eyInt=0,ezInt= //scaledintegralvoidIMUupdate(floatgx,floatgy,floatgz,floatax,floatay,float{floatfloatvx,vy,vz;//wx,wy,wz;floatex,ey,ez;floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq1q1=q1*q1;floatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=//normalisethenorm=sqrt(ax*ax+ay*ay+az*az); 為單位向量。因為是單位矢量到參考性的投影，所以要把acc單位化ax=ax/norm; ay=ay/norm; azaz //四元數(shù)對應(yīng)//這是把四元數(shù)換算成方向余弦中的第三行的三個元素，根據(jù)余弦矩陣和角的定義，地里坐標系的Z軸重力向量，//轉(zhuǎn)到機體坐標系，正好是這三個元素，所以這里的 其實就是上一次的（四元數(shù)）的機體坐標參考//換算出來的重力的單位向量。(是帶有誤差的姿態(tài)。vx=2*(q1q3-q0q2); //參考系Z軸與載體系x軸之間vy=2*(q0q1+q2q3); //Z軸與載體系y軸vz=q0q0-q1q1-q2q2+q3q3; //Z軸與載體系z軸之間//errorissumofcrossproductbetweenreferencedirectionoffieldsanddirectionmeasuredbysensorsax，ay，az 是機體坐標參考系上，加計測出來的重力向量，也就是實際測出來的重力向量（這明顯就是重力在三個軸//上的“分。那他們之間的誤差向量就是陀螺積分后的姿態(tài)和加計測出//的姿態(tài)之間的誤差。向量間的誤差可以用向量的叉積（外積）來表示，ex，ey，ez就叉積向量仍是在機體坐標系上的，而陀螺的積分誤差也是在機體坐標系上的，而且叉積的大小和陀螺的積分誤差成正比，正好用來修正陀螺，這是因為，由于陀螺是對機體直接積分的，所以對陀螺的誤差修正會直接會直接體現(xiàn)在對機//體坐標系的修正，就是對機體修正。說白了，就是用加計測量標定陀螺積分這里誤差沒說清楚，不是指向量差。這個叉積誤差是指將帶有誤差的加計向量轉(zhuǎn)動到與重力向量重合，需要繞什么軸，轉(zhuǎn)多少角度。逆向推理一下，這個叉積在機體三軸上的投影，就是加計和重力之間的角度誤差。也就是說，如果陀螺按這個叉積誤差的軸，轉(zhuǎn)動叉積誤差的角度（也就是轉(zhuǎn)動三軸投影的角度）那就能把加計和重力向量的誤差消除掉。//（具體可看向量叉積的定義）如果完全按叉積誤差轉(zhuǎn)過去，那就是完全信任加計。如果一點也不轉(zhuǎn)，那就是完全信任//陀螺。那么把這個叉積的三軸乘以X%，加到陀螺的積分角度上去，就是這個x%互系數(shù)的互補算//軸夾角，按角的轉(zhuǎn)動順序分解到三軸上去，會很麻煩。這里的叉積算法把sin誤差當成角度誤差，并無視角的//為了解釋方便，所以上面就把叉積等同于角度誤差了exay*vzaz*vy //上面說了，vx,vy,vz

ey=az*vx-ax*vz;ez=ax*vy-ay*vx//對誤差進行積exInt=exInt+ex*Ki;eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;//adjustedgyroscopegx=gx+Kp*ex+exInt; gygyKp*ey //修正陀螺輸gz=gz+Kp*ez+ezInt;