互補(bǔ)濾波算法姿態(tài)解算PPT學(xué)習(xí)教案

1、會計(jì)學(xué)1互補(bǔ)濾波算法姿態(tài)解算互補(bǔ)濾波算法姿態(tài)解算Mini INS/GPS姿態(tài)儀姿態(tài)儀第1頁/共39頁第2頁/共39頁第3頁/共39頁第4頁/共39頁介紹內(nèi)容：介紹內(nèi)容： 1、四元數(shù)、四元數(shù) 2、姿態(tài)表示的方法、姿態(tài)表示的方法 3、姿態(tài)解算原理、姿態(tài)解算原理第5頁/共39頁一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j則稱數(shù)則稱數(shù)q為四元數(shù)，而為四元數(shù)，而q0稱為四元數(shù)稱為四元數(shù)q的實(shí)部，稱的實(shí)部，稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 1.2 四元數(shù)的表示方式四

2、元數(shù)的表示方式第6頁/共39頁一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j則稱數(shù)則稱數(shù)q為四元數(shù)，而為四元數(shù)，而q0稱為四元數(shù)稱為四元數(shù)q的實(shí)部，稱的實(shí)部，稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q 1.2 四元數(shù)的表示方式四元數(shù)的表示方式第7頁/共39頁一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=

3、i, ki=-ik=j則稱數(shù)則稱數(shù)q為四元數(shù)，而為四元數(shù)，而q0稱為四元數(shù)稱為四元數(shù)q的實(shí)部，稱的實(shí)部，稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q 1.2 四元數(shù)的表示方式四元數(shù)的表示方式第8頁/共39頁一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 則稱數(shù)則稱數(shù)q為四元數(shù)，為四元數(shù)， 而而q0稱為四元數(shù)稱為四元數(shù)q的實(shí)部，的實(shí)部， 稱稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qi

4、jkqqqq0123,Rq q q q 1.2 四元數(shù)的表示方式四元數(shù)的表示方式第9頁/共39頁一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè) , 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 則稱數(shù)則稱數(shù)q為四元數(shù)，為四元數(shù)， 而而q0稱為四元數(shù)稱為四元數(shù)q的實(shí)部，的實(shí)部， 稱稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q123ijkq qq 1.2 四元數(shù)的表示方式四元數(shù)的表示方式第10頁/共39頁一、四元數(shù)一、四元數(shù)1.1 四元數(shù)定義四元數(shù)定義 設(shè)設(shè)

5、, 其中，其中，i，j，k滿足滿足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 則稱數(shù)則稱數(shù)q為四元數(shù)，為四元數(shù)， 而而q0稱為四元數(shù)稱為四元數(shù)q的實(shí)部，的實(shí)部， 稱稱 為為q的虛部。的虛部。 四元數(shù)的共軛為四元數(shù)的共軛為 0123qijkqqqq0123,Rq q q q123ijkq qq0123ijkqqqqq 1.2 四元數(shù)的示方式四元數(shù)的示方式第11頁/共39頁第12頁/共39頁1.3 四元數(shù)運(yùn)算四元數(shù)運(yùn)算第13頁/共39頁二、東北天坐標(biāo)系二、東北天坐標(biāo)系 東北天坐標(biāo)系（東北天坐標(biāo)系（表示為表示為n系系）是一種當(dāng)?shù)氐乩碜鴺?biāo)系，原點(diǎn)位于導(dǎo)航系統(tǒng)）是

6、一種當(dāng)?shù)氐乩碜鴺?biāo)系，原點(diǎn)位于導(dǎo)航系統(tǒng)所處的位置所處的位置P點(diǎn)，坐標(biāo)軸指向北、東和當(dāng)?shù)卮咕€方向（向下），也有稱為點(diǎn)，坐標(biāo)軸指向北、東和當(dāng)?shù)卮咕€方向（向下），也有稱為北東地坐標(biāo)系北東地坐標(biāo)系第14頁/共39頁三、姿態(tài)表示方法三、姿態(tài)表示方法 第15頁/共39頁三、姿態(tài)表示方法三、姿態(tài)表示方法 3.1 方向余弦矩陣方向余弦矩陣 方向余弦矩陣用符號方向余弦矩陣用符號 表示，是一個(gè)表示，是一個(gè)3*3階的矩陣，矩陣的列表示階的矩陣，矩陣的列表示載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。 nbC第16頁/共39頁三、姿態(tài)表示方法三、姿態(tài)表示方法 3.1 方向余弦

7、矩陣方向余弦矩陣 方向余弦矩陣用符號方向余弦矩陣用符號 表示，是一個(gè)表示，是一個(gè)3*3階的矩陣，矩陣的列表示階的矩陣，矩陣的列表示載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。 nbC111213212223313233nbcccCcccccc第17頁/共39頁三、姿態(tài)表示方法三、姿態(tài)表示方法 3.1 方向余弦矩陣方向余弦矩陣 方向余弦矩陣用符號方向余弦矩陣用符號 表示，是一個(gè)表示，是一個(gè)3*3階的矩陣，矩陣的列表示階的矩陣，矩陣的列表示載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影。 第第i行、行、j列的元素表

8、示參考坐標(biāo)系列的元素表示參考坐標(biāo)系i軸和載體坐標(biāo)系軸和載體坐標(biāo)系j軸夾角的余弦。軸夾角的余弦。 在載體坐標(biāo)系中定義的矢量在載體坐標(biāo)系中定義的矢量 ，可以通過該矢量左乘方向余弦矩，可以通過該矢量左乘方向余弦矩陣陣 ，即，即 nbC111213212223313233nbcccCccccccbrnbCnnbbCrr第18頁/共39頁3.2 歐拉角歐拉角 一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換，可以通過繞不同坐標(biāo)軸的一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換，可以通過繞不同坐標(biāo)軸的3次次連續(xù)轉(zhuǎn)動來實(shí)現(xiàn)。從參考系到一個(gè)新的坐標(biāo)系的變換可以表示：連續(xù)轉(zhuǎn)動來實(shí)現(xiàn)。從參考系到一個(gè)新的坐標(biāo)系的變換可以表示： 繞參考坐標(biāo)系的繞參考

9、坐標(biāo)系的z軸轉(zhuǎn)動軸轉(zhuǎn)動 角角 繞新坐標(biāo)系的繞新坐標(biāo)系的y軸轉(zhuǎn)動軸轉(zhuǎn)動 角角 繞新坐標(biāo)系的繞新坐標(biāo)系的x軸轉(zhuǎn)動軸轉(zhuǎn)動 角角 稱為歐拉轉(zhuǎn)動角稱為歐拉轉(zhuǎn)動角第19頁/共39頁3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實(shí)現(xiàn)。次轉(zhuǎn)動來實(shí)現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個(gè)具有表示，它是一個(gè)具有4個(gè)元素的矢量，這些元素是該矢個(gè)元素的矢量，這些元素是該矢量方向和轉(zhuǎn)動大

10、小的函數(shù)。量方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個(gè)角度轉(zhuǎn)動一個(gè)角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。第20頁/共39頁3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實(shí)現(xiàn)。次轉(zhuǎn)動來實(shí)現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個(gè)具有表示，它是一個(gè)具有4個(gè)元素的矢量，這些元素是該矢個(gè)元素的矢量，這些元素是

11、該矢量方向和轉(zhuǎn)動大小的函數(shù)。量方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個(gè)角度轉(zhuǎn)動一個(gè)角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。第21頁/共39頁3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實(shí)現(xiàn)。次轉(zhuǎn)動來實(shí)現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個(gè)具有表示，它是一個(gè)具有4個(gè)元素的矢量，這些元素是該矢個(gè)元素

12、的矢量，這些元素是該矢量方向和轉(zhuǎn)動大小的函數(shù)。量方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個(gè)角度轉(zhuǎn)動一個(gè)角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。第22頁/共39頁3.3 四元數(shù)四元數(shù) 四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)四元數(shù)姿態(tài)表達(dá)式是一個(gè)四參數(shù)的表達(dá)式。它的基本思路是：一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考系中的矢量 的單的單次轉(zhuǎn)動來實(shí)現(xiàn)。次轉(zhuǎn)動來實(shí)現(xiàn)。 四元數(shù)用符號四元數(shù)用符號q表示，它是一個(gè)具有表示，它是一個(gè)具有4個(gè)元素的矢量，這

13、些元素是該矢個(gè)元素的矢量，這些元素是該矢量方向和轉(zhuǎn)動大小的函數(shù)。量方向和轉(zhuǎn)動大小的函數(shù)。定義定義 的大小和方向是使參考系繞的大小和方向是使參考系繞 轉(zhuǎn)動一個(gè)角度轉(zhuǎn)動一個(gè)角度 ，就能與載體坐，就能與載體坐標(biāo)系重合。標(biāo)系重合。第23頁/共39頁 利用四元數(shù)進(jìn)行矢量變換利用四元數(shù)進(jìn)行矢量變換 首先定義一個(gè)四元數(shù)首先定義一個(gè)四元數(shù)rb rb=ix+jy+kz rb=0+ix+jy+kz有：有： rn=q*rb*q rn=(q0+iq1+jq2+kq3)(0+ix+jy+kz)(q0-iq1-jq2-kq3) =0+(q02+q12-q22-q32)x+2(q1q2-q0q3)y+2(q1q3+q0q2

14、)zi +2(q1q2+q0q3)x+(q02-q12+q22-q32)y+2(q2q3-q0q1)zj +2(q1q3-q0q2)x+2(q2q3+q0q1)y+(q02-q12-q22+q32)zkrn也可以表示成矩陣形式也可以表示成矩陣形式nbrc r0 00 cc2222012312031302222212030123230122221302230101232()2()2()2()2()2()Cq q q qqq q qqq q qqq q qq q q qq q q qqq q qq q q qq q q q第24頁/共39頁3.4 方向余弦、歐拉角和四元數(shù)的關(guān)系方向余弦、歐拉角和四

15、元數(shù)的關(guān)系 這樣，四元數(shù)可以用方向余弦、歐拉角表示，同樣，歐拉角也可以用這樣，四元數(shù)可以用方向余弦、歐拉角表示，同樣，歐拉角也可以用方向余弦和四元數(shù)表示。方向余弦和四元數(shù)表示。2222012312031302222212030123230122221302230101232()2()2()2()2()2()q q q qqq q qqq q qqq q qq q q qq q q qqq q qq q q qq q q q第25頁/共39頁用方向余弦表示四元數(shù)用方向余弦表示四元數(shù) 1203 22 3101 33 1202 11 23011 12 23 321()41()41()4(1)qccc

16、qccqqccqqccq用歐拉角表示四元數(shù)用歐拉角表示四元數(shù) q0 q1 q2 q3對于小角度位移，四元數(shù)參數(shù)可以用表示為對于小角度位移，四元數(shù)參數(shù)可以用表示為第26頁/共39頁用方向余弦表示歐拉角用方向余弦表示歐拉角四元數(shù)表示歐拉角四元數(shù)表示歐拉角 23012222012323011203222201232()arctan()arcsin(2()2()arctan()q qq qqqqqq qq qq qq qqqqq90o第27頁/共39頁四、互補(bǔ)濾波的姿態(tài)解算算法四、互補(bǔ)濾波的姿態(tài)解算算法 姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元

17、數(shù)法。歐拉角法歐拉角法在求解姿態(tài)時(shí)存在奇點(diǎn)（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；在求解姿態(tài)時(shí)存在奇點(diǎn)（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦方向余弦可用于全姿態(tài)的解算但計(jì)算量大，不能滿足實(shí)時(shí)性要求?？捎糜谌藨B(tài)的解算但計(jì)算量大，不能滿足實(shí)時(shí)性要求。四元數(shù)法四元數(shù)法，其計(jì)算量小，無奇點(diǎn)且可以滿足飛行器運(yùn)動過程中姿態(tài)的實(shí)時(shí)解算。，其計(jì)算量小，無奇點(diǎn)且可以滿足飛行器運(yùn)動過程中姿態(tài)的實(shí)時(shí)解算。第28頁/共39頁四、互補(bǔ)濾波的姿態(tài)解算算法四、互補(bǔ)濾波的姿態(tài)解算算法 姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在求解姿態(tài)時(shí)

18、存在奇點(diǎn)（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可求解姿態(tài)時(shí)存在奇點(diǎn)（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可用于全姿態(tài)的解算但計(jì)算量大，不能滿足實(shí)時(shí)性要求。四元數(shù)法，其計(jì)算量用于全姿態(tài)的解算但計(jì)算量大，不能滿足實(shí)時(shí)性要求。四元數(shù)法，其計(jì)算量小，無奇點(diǎn)且可以滿足飛行器運(yùn)動過程中姿態(tài)的實(shí)時(shí)解算。小，無奇點(diǎn)且可以滿足飛行器運(yùn)動過程中姿態(tài)的實(shí)時(shí)解算。 姿態(tài)解算的原理姿態(tài)解算的原理：對于一個(gè)確定的向量，用不同的坐標(biāo)系表示時(shí)，他們：對于一個(gè)確定的向量，用不同的坐標(biāo)系表示時(shí)，他們所表示的大小和方向一定是相同的。但是由于這兩個(gè)坐標(biāo)系的旋轉(zhuǎn)矩陣存在所表示的大小和方向一定是相同的。但是由于這兩個(gè)坐標(biāo)

19、系的旋轉(zhuǎn)矩陣存在誤差，那么當(dāng)一個(gè)向量經(jīng)過這么一個(gè)有誤差存在的旋轉(zhuǎn)矩陣后，在另一個(gè)坐誤差，那么當(dāng)一個(gè)向量經(jīng)過這么一個(gè)有誤差存在的旋轉(zhuǎn)矩陣后，在另一個(gè)坐標(biāo)系中肯定和理論值是有偏差的，我們通過這個(gè)偏差來修正這個(gè)旋轉(zhuǎn)矩陣。標(biāo)系中肯定和理論值是有偏差的，我們通過這個(gè)偏差來修正這個(gè)旋轉(zhuǎn)矩陣。這個(gè)旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正這個(gè)旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正了。了。 第29頁/共39頁四、互補(bǔ)濾波的姿態(tài)解算算法四、互補(bǔ)濾波的姿態(tài)解算算法 姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四

20、元數(shù)法。歐拉角法在求解姿態(tài)時(shí)存在奇點(diǎn)（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可求解姿態(tài)時(shí)存在奇點(diǎn)（萬向節(jié)死鎖），不能用于全姿態(tài)的解算；方向余弦可用于全姿態(tài)的解算但計(jì)算量大，不能滿足實(shí)時(shí)性要求。四元數(shù)法，其計(jì)算量用于全姿態(tài)的解算但計(jì)算量大，不能滿足實(shí)時(shí)性要求。四元數(shù)法，其計(jì)算量小，無奇點(diǎn)且可以滿足飛行器運(yùn)動過程中姿態(tài)的實(shí)時(shí)解算。小，無奇點(diǎn)且可以滿足飛行器運(yùn)動過程中姿態(tài)的實(shí)時(shí)解算。 姿態(tài)解算的原理：對于一個(gè)確定的向量，用不同的坐標(biāo)系表示時(shí)，他們姿態(tài)解算的原理：對于一個(gè)確定的向量，用不同的坐標(biāo)系表示時(shí)，他們所表示的大小和方向一定是相同的。但是由于這兩個(gè)坐標(biāo)系的旋轉(zhuǎn)矩陣存在所表示的大小和方向一定

21、是相同的。但是由于這兩個(gè)坐標(biāo)系的旋轉(zhuǎn)矩陣存在誤差，那么當(dāng)一個(gè)向量經(jīng)過這么一個(gè)有誤差存在的旋轉(zhuǎn)矩陣后，在另一個(gè)坐誤差，那么當(dāng)一個(gè)向量經(jīng)過這么一個(gè)有誤差存在的旋轉(zhuǎn)矩陣后，在另一個(gè)坐標(biāo)系中肯定和理論值是有偏差的，我們通過這個(gè)偏差來修正這個(gè)旋轉(zhuǎn)矩陣。標(biāo)系中肯定和理論值是有偏差的，我們通過這個(gè)偏差來修正這個(gè)旋轉(zhuǎn)矩陣。這個(gè)旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正這個(gè)旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，這樣姿態(tài)就被修正了。了。4.1 互補(bǔ)濾波器解算原理互補(bǔ)濾波器解算原理 陀螺儀陀螺儀動態(tài)響應(yīng)特性良好，但計(jì)算姿態(tài)時(shí)會產(chǎn)生累積誤差。動態(tài)響應(yīng)特性良好，但計(jì)算姿態(tài)時(shí)會產(chǎn)生累積誤差。磁

22、力計(jì)和加磁力計(jì)和加速度計(jì)速度計(jì)測量姿態(tài)沒有累積誤差，但動態(tài)響應(yīng)較差。因此他們在頻域上特性互測量姿態(tài)沒有累積誤差，但動態(tài)響應(yīng)較差。因此他們在頻域上特性互補(bǔ)，可以采用互補(bǔ)濾波器融合這三種傳感器的數(shù)據(jù)，提高測量精度和系統(tǒng)的補(bǔ)，可以采用互補(bǔ)濾波器融合這三種傳感器的數(shù)據(jù)，提高測量精度和系統(tǒng)的動態(tài)性能。動態(tài)性能。 第30頁/共39頁互補(bǔ)濾波器的傳遞函數(shù)互補(bǔ)濾波器的傳遞函數(shù) 12( )( ),( )( )( )C sssssC ssC sGG因此，通過互補(bǔ)濾波器能夠消除高頻噪聲和低頻誤差的累積，能夠很好的融合各傳感器的數(shù)據(jù)用用C（s）表示飛行器運(yùn)動過程中真實(shí)的姿態(tài)矩陣，表示飛行器運(yùn)動過程中真實(shí)的姿態(tài)矩陣，互

23、補(bǔ)濾波器估計(jì)的姿態(tài)矩陣互補(bǔ)濾波器估計(jì)的姿態(tài)矩陣加速度計(jì)和磁力計(jì)觀測到的姿態(tài)矩陣加速度計(jì)和磁力計(jì)觀測到的姿態(tài)矩陣0HCC觀測到高頻噪聲觀測到高頻噪聲陀螺儀測量得到的數(shù)據(jù)計(jì)算得到的姿態(tài)矩陣陀螺儀測量得到的數(shù)據(jù)計(jì)算得到的姿態(tài)矩陣LCC低頻誤差累積低頻誤差累積 G1具有一階低通濾波特性，具有一階低通濾波特性，G2具有一階高通濾波特性具有一階高通濾波特性C（s）=kp+ki/s第31頁/共39頁（1）初始化四元數(shù)）初始化四元數(shù) 姿態(tài)計(jì)算初始，將已知載體初始姿態(tài)角帶入下式，求出初始時(shí)刻的四姿態(tài)計(jì)算初始，將已知載體初始姿態(tài)角帶入下式，求出初始時(shí)刻的四元數(shù)。元數(shù)。 q0 q1 q2 q34.2 四元數(shù)姿態(tài)解算

24、步驟四元數(shù)姿態(tài)解算步驟第32頁/共39頁（2）獲取角速度、加速度、磁力計(jì)值）獲取角速度、加速度、磁力計(jì)值 讀取到的讀取到的加速度加速度測量值測量值ax,ay,az;陀螺儀陀螺儀測量值測量值wx,wy,wz；磁力計(jì)磁力計(jì)測量值測量值 mx,my,mz (3) 將加速度計(jì)測量值、磁力計(jì)測量值化為單位向量將加速度計(jì)測量值、磁力計(jì)測量值化為單位向量（4）從四元數(shù)里獲得重力向量和磁場向量）從四元數(shù)里獲得重力向量和磁場向量 重力向量重力向量 1302230122201232222012312031302022220120301232301122221302230101232()2()2()2()2()2(

25、)2()2()Txyzqqqqq qq qq qq qvq qq qqqqqq qq qvvq qq qq qq qqqqqq qq qq qq qqqq 2qn系中，加速度計(jì)輸出（重力向量）系中，加速度計(jì)輸出（重力向量）bCn（表示從（表示從n系到系到b系轉(zhuǎn)換矩陣）系轉(zhuǎn)換矩陣）乘以加速度計(jì)的實(shí)際測量值為加速度計(jì)的實(shí)際測量值為 (ax,ay,az)T，(vx,vy,vz)T和和(ax,ay,az)T 均表示在均表示在b系中系中的豎直向下的向量的豎直向下的向量(ex,ey,ez)T = (vx,vy,vz)T 叉乘叉乘(ax,ay,az)T 第33頁/共39頁地磁場向量地磁場向量可不可以采用和加

26、速度計(jì)一樣的修正方法來修正？可不可以采用和加速度計(jì)一樣的修正方法來修正？加速度計(jì)在靜止時(shí)測量的是重力加速度加速度計(jì)在靜止時(shí)測量的是重力加速度(0,0,1) ，是有大小和方向的；，是有大小和方向的；而是與而是與x軸（或者軸（或者y軸）呈一個(gè)角度，與軸）呈一個(gè)角度，與z軸呈一個(gè)角度。記作軸呈一個(gè)角度。記作(bx,by,bz)T ，這里我們讓，這里我們讓x軸對準(zhǔn)北邊，所以軸對準(zhǔn)北邊，所以by=0，即，即 (bx,0,bz)T同理，地磁計(jì)同樣測量的是地球磁場的大小和方向，只不過這個(gè)方向不再是豎直向下同理，地磁計(jì)同樣測量的是地球磁場的大小和方向，只不過這個(gè)方向不再是豎直向下倘若我們知道倘若我們知道bx和和bz的精確值，那么我們就可以采用和加速度計(jì)一樣的修正方法來修正。的精確值，那么我們就可以采用和加速度計(jì)一樣的修正方法來修正。只不過在加速度計(jì)中，我們在只不過在加速度計(jì)中，我們在n系中的參考向量是（系中的參考向量是（0,0,1) ，變成了地磁計(jì)的，變成了地磁計(jì)的 (bx,0,bz)T但實(shí)際上，當(dāng)?shù)氐卮艌鱿鄬τ跂|北天坐標(biāo)的夾角無法測量但實(shí)際上，當(dāng)?shù)氐卮艌鱿鄬τ跂|北天坐標(biāo)的夾角無法測量我們的做法：反過來從我們的做法：反過來從b系推往系推往n系系第34頁/共39頁22231 20 31 30 2221 2