組合導(dǎo)航姿態(tài)解算學(xué)習(xí)筆記

1、學(xué)習(xí)必備歡迎下載 2015-3-9 1參考丁君AHRS航姿解算中的兩種濾波方法的比較研究，發(fā)現(xiàn)使用加速度的數(shù)據(jù)可以 解算橫滾角（roll）和俯仰角（pitch）. 2因開發(fā)板上單片機(jī)無ADC，無法對購買模塊進(jìn)行處理，故僅僅參考附帶的程序。希望可 以將adxl335模塊的示例程序轉(zhuǎn)移到mpu6050中，陀螺儀的數(shù)據(jù)暫時不用，僅僅使用加速度 計(jì)的數(shù)據(jù)進(jìn)行兩個角度的解算。 3老師想讓我研究載體做圓周運(yùn)動時加速度的解算，但是我想先從靜態(tài)的開始，我覺得靜態(tài) 下的測試是基礎(chǔ)，上來就研究最難的我接受不了。所以我想先用三軸的加速度數(shù)據(jù)先把靜態(tài) 下的姿態(tài)解算出來。 4我發(fā)現(xiàn)如果我僅僅可以解算靜態(tài)下的姿態(tài)，無法解

2、決載體做回轉(zhuǎn)運(yùn)動下的姿態(tài)我還是完不 成本科生的任務(wù)。因?yàn)楸究粕娜蝿?wù)是汽車姿態(tài)測量，所以光靜止是不夠的。 2015-3-11 5校正這一環(huán)節(jié)是我所沒有考慮到的，因?yàn)橛衅钸€有靈敏度不匹配。但前提是我要先解算 出來。 6論文不應(yīng)該是最后完成的，論文是邊做邊寫的，最后應(yīng)該是是復(fù)制粘貼修改格式和布局而 已。 7. 三軸加速度計(jì)和三輛磁傳感器的姿態(tài)原理 選擇北東地坐標(biāo)摹柞為導(dǎo)竝坐標(biāo)系5系人戦體的縱軸 0X,、橫軸0Y,和豎軸0Z.構(gòu)成的右手坐標(biāo)系作為載休坐標(biāo) 系（占系人則樣爲(wèi)到卜系的方向余弦矩陣為： sinrycosfl cosinf ainysinsinf + co$ysin#sin - siiry

3、co (1) 其中疔陽 分別為載體的橫滾角（R胡）、俯仰角（Pitch）和 航向角（九聞人 在載體坐標(biāo)系b系下，重力場分城為# = g： g； g：l 當(dāng)載體處于非加速運(yùn)動狀態(tài)時|導(dǎo)航坐標(biāo)系 -gsinB gsinycos (2) (4) nST的重力場分量為昭” =C0 0 g】S則有: 從而得到橫滾角和俯伸角分別為: y - arctanfgf/J) gcflsycos-1 arcsin 8可以嘗試將adxl335的示例程序（淘寶模塊）移植到 MPU6050中去解算橫滾角和俯仰角。 2015-3-12 1為什么示例程序產(chǎn)生的六軸數(shù)據(jù)跟我想象的不一樣那呢？加速度計(jì)的數(shù)據(jù)不是98,沒有 小數(shù)點(diǎn)

4、。但是我發(fā)現(xiàn)買的arm模塊數(shù)據(jù)也不是98，而且我用手機(jī)里的磁鐵去干擾磁強(qiáng)計(jì)時， 發(fā)現(xiàn)磁強(qiáng)計(jì)的數(shù)據(jù)發(fā)生了很大的變化，如果真的要使用磁強(qiáng)計(jì)一定要注意周圍的磁場干擾。 看來加速度計(jì)的數(shù)據(jù)是可以用的，因?yàn)閯e人都可以做到。 為什么示例程序中減去偏移量，而我卻不能減去偏移量，比如+X的加速度最大值是 16000,減去之后，在+x該等于零時，又出現(xiàn)了 -16000,這是我更不想看到的。 2. extern float atan2 (float y, float x);程序格式又搞錯了，人家不是atan2 (float y/float x)， 否則很容易出現(xiàn)錯誤too many actual paramete

5、rs. 3. 現(xiàn)在的狀態(tài)是不能進(jìn)行全姿態(tài)解算，x軸的顯示范圍是(90(270o)，和我想要的范 圍(-905+90。)正好差了180o,但是減去還不行，減去后串口上只顯示一個負(fù)號。還好汽 車達(dá)不到那個角度土 90o,哪怕是在汽車測試中，但是飛機(jī)能達(dá)到啊。所以這個問題最終還 是要解決的。我想先把一個角度解算出來，然后去推廣。 我在主函數(shù)里改動 pitch=(i nt)(ata n(ratio)*180)/3.1415926)+180); 這一句不行，后來我改 動 void lcd_printf(char *s,int temp_data)函數(shù)里面，在第一句我加上了temp_data-=180;然

6、后俯 仰角就輸出正常了，也不知道為什么。 4. uchar是一個8位無符號數(shù)，表示范圍0到255,而uint是十六位無符號數(shù)，表示范圍0到 65535.但是要注意的是8位單片機(jī)。(摘自網(wǎng)絡(luò)) 5. 現(xiàn)在能解一個俯仰角，下一個是橫滾角。我想這兩個角的性質(zhì)應(yīng)該是比較接近的。但 是論文不是這么寫的。橫滾角也解算出來了，但是航向角好像不能通過加速度計(jì)解算。 2015-3-13 1. 因?yàn)楹较蚪墙馑悴怀?，所以找出MPU9150,希望采用其中的磁強(qiáng)計(jì)來解算磁航向角。 接下來下載相關(guān)datasheet并閱讀。 2. 現(xiàn)在的解算方法還不涉及迭代，所以現(xiàn)在還沒出現(xiàn)那種隨著時間的推移，誤差累積越 來越嚴(yán)重的情況。

7、當(dāng)前的解算與值與當(dāng)前的采集數(shù)據(jù)有關(guān)。 20XX年3月15日 1. 為什么不直接搞 DSP直接跳過ARM那一關(guān)，害怕，害怕就去學(xué)。 2. 老師可能需要我做一個松耦合組合導(dǎo)航，先讓我去研究一下算法。然后再去用硬件實(shí) 現(xiàn)。該整理資料了，整理完給老師一份，然后再說設(shè)計(jì)硬件編程的事。等我把航向角結(jié)算出 來后后立馬去研究組合導(dǎo)航算法。方向錯了，停止就是前進(jìn)，否則以后都不能和老師交流了。 這是很危險(xiǎn)的。我是否應(yīng)該聽老師的，先搞算法，然后再去考慮接下來的實(shí)現(xiàn)。 3. 網(wǎng)上的GPS模塊沒見有遵守I2C通信協(xié)議的，都是一個收一個發(fā)送，這樣我還真得 考慮數(shù)據(jù)同步的問題。 20XX年3月16日 1. 我終于知道為什么

8、用示例程序在串口上顯示的數(shù)據(jù)為什么和我想象的那么不一樣了， 因?yàn)槟阍诔跏蓟疢PU6050的時候會對陀螺儀和加速度進(jìn)行一些配置，其中包括一個叫做full scale range的配置，就拿陀螺儀來說如果你將這個范圍配置在一 2000 s，那么這個范圍所 對應(yīng)的sensitivity scale factor就是16.4 LSB s。感謝唐朔飛老師的計(jì)算機(jī)組成原理， 感謝日本Asahi Kasei的數(shù)據(jù)手冊，讓我在看補(bǔ)數(shù)、補(bǔ)碼的過程中，讓我在看到日本磁強(qiáng)計(jì) 的測量數(shù)據(jù)與磁通密度的對比中讓我想到了陀螺儀和加速度計(jì)也是這樣的。謝謝你們。一個 好的數(shù)據(jù)手冊就應(yīng)該讓 user看明白。 2. 怎樣才能在陀螺

9、儀的寄存器中的16bit數(shù)據(jù)中看出那個表示小數(shù)點(diǎn)？都不表示小數(shù) 點(diǎn)，只有通過sensitivity scale factor之后才會產(chǎn)生小數(shù)點(diǎn)，這時的數(shù)據(jù)才是精確地。好了， 這下可以全身心地投入到算法研究中去了，傳感器輸出的就是，加速度數(shù)據(jù)和角速度數(shù)據(jù)， 接下來你要做的就是研究一個適合車輛檢測的算法了。 學(xué)習(xí)必備歡迎下載 3. MPU6050的程序遷移到9150中沒有發(fā)現(xiàn)問題，接下來采集磁強(qiáng)計(jì)的數(shù)據(jù)。僅僅修改 了取數(shù)據(jù)的順序，加入了磁強(qiáng)計(jì)各個軸數(shù)據(jù)寄存器地址，沒有使用WIA(Device ID)。采集成 功后開始解算航向角，接下來需要考慮的是磁強(qiáng)計(jì)的三軸正方向與GYRO與上 0 x8f 后 4

10、 位符號位只保留一位，值依然 沒有改變，處理后的數(shù)據(jù)為03874，-30221,00336。但是這不能保證都是正數(shù)的補(bǔ)碼，所以 我決定建立一個數(shù)組，長度為13. 3如何舍棄一個二進(jìn)制數(shù)的前三位？不過我又發(fā)現(xiàn)多個符號位不影響從二進(jìn)制轉(zhuǎn)換為 十進(jìn)制。 4. IICwriteByte(0 xd0,0 x37,0 x02);0 xd0 是 mpu9150 的 slaveaddress,0 x37 是旁路使 能配置寄存器55，配置為0 x02說明處理器能夠直接讀取輔助I2C的數(shù)據(jù)。 IICwriteByte(0 xD0,0 x6A,0 x00); /0 x6a是用戶配置寄存器106,配置為輔助總線的邏輯

11、由主I2C總線決定。 IICwriteByte(0 x18,0 x0A,0 x01);0 x18 是磁強(qiáng)計(jì)的 slaveaddress,0 x0a 是磁強(qiáng)計(jì) 的控制寄存器CNTL,配置為單次測量模式。 阿莫電子論壇上網(wǎng)友出的招數(shù)。 SIme 日select CADO VSS VSS 0 0 0 1 1 0 0 R1W VSS VDD 0 001101 RA/V VDD VSS Q001110 RW VDD VDD 0 D 01 1 1* R/W CADO 14 open TST2 OS- open I 9 AK8975 13 5 學(xué)習(xí)必備歡迎下載 6,4.2. Single Measureme

12、nt Mode When single measurcniciU mode (X4ODE3:0“0 H=(Si ngle_Readl2C(MAGADD,REG_Address); Sin gle_Writel2C(MAGADD,CNTL, 0 x01); L=Si ngle_Readl2C(MAGADD,REG_Address-1); Sin gle_WriteI2C(MAGADD,CNTL, 0 x01); return (H8)+L;/ 合成數(shù)據(jù) 6現(xiàn)在求解出來的航向角波動很大，非常不穩(wěn)定，變化范圍在-180o180o之間。也許航 向角的解算與俯仰角和橫滾角的取值范圍也有關(guān)系。二者的取值范

13、圍均為-90o90a 7現(xiàn)在航向角還是沒有求出來，我想這可能是磁強(qiáng)計(jì)的數(shù)據(jù)有問題 20XX年3月18日 1現(xiàn)在的數(shù)據(jù)還是沒有換算成真實(shí)數(shù)據(jù)，而且?guī)?shù)點(diǎn)的數(shù)據(jù)通過串口往上傳我還沒試 過。 2另外提一點(diǎn)，我的俯仰角解算和橫滾角解算用的方法和上面論文提到的有差異。 9 = tan L Agrr +Az,out X.OLT + A2 Y.OUT LCg 心W (11) (12) (13) AN1057 而且解出來的值也不是我想要的航向角, 而且我懷疑上面兩個角度也不是我想要的航向角 和橫滾角。 3到現(xiàn)在我才明白采樣時間間隔內(nèi)的角增量就是角速度，速度增量就是加速度，當(dāng)時我 竟然還給老師說，用的數(shù)據(jù)不是

14、加速度和角速度，太愚蠢了，沒聽老師的話真是太失敗了。 秦永元慣性導(dǎo)航第二版 P253 答：（X J表示的各分量構(gòu)造成的叉乘斜對稱矩陣， = a= axiayjazk b 二 bxi by j bzk c = Cxi Cy j Czk c = a b =aybz - azby iazbx -axbz j axby - aybxk = = i jk =ax ayaz bxbybz Cx0-azaybx Cy |=|az0_a*|by cj ayax0bj c = a b =a b i0-azayI （a漢）=|az0-ax ayax0 一 5汽車測試中存在劃槳效應(yīng)嗎？需要劃槳效應(yīng)補(bǔ)償量 ；：Vsg

15、im嗎？劃槳效應(yīng)補(bǔ)償項(xiàng)的 優(yōu)化算法以劃槳運(yùn)動為環(huán)境條件， 那么在汽車測試中，尤其是做穩(wěn)態(tài)回轉(zhuǎn)時的環(huán)境條件又是 什么，這里可以分析一下，這里可以具體環(huán)境具體分析， 這里可以出現(xiàn)創(chuàng)新點(diǎn)，就是將工作 條件修改，這里就是研究生工作量的體現(xiàn)之處。 6汽車在做等半徑穩(wěn)態(tài)回轉(zhuǎn)時，我覺得俯仰角和橫滾角是不變的，只有航向角在發(fā)生變 化。我覺得汽車在做穩(wěn)態(tài)回轉(zhuǎn)運(yùn)動中角速度和比力分別是 t i；= j B l cost f t i； = kC sin t 因?yàn)槲矣X得在車輛做穩(wěn)態(tài)回轉(zhuǎn)運(yùn)動時，橫滾角存在擺動，z軸上加速度波動較大。但只 是感性的猜測，沒有理性的數(shù)據(jù)支持。 還有就是我希望自己的程序能夠做到自適應(yīng)，根據(jù)測試

16、環(huán)境自動切換姿態(tài)解算算法。因 為不同的環(huán)境條件下假設(shè)是不一樣的，不能一概而論。 7JQIN2P296又出了一個惡心的名詞直流分量，細(xì)節(jié)，但就是卡著你讓你過不去，想了 半天了，擱置一下吧。 20XX 年 3 月 24 日 10:46 1將坐標(biāo)系理解成為剛體，弓I入四元數(shù)描述剛體的轉(zhuǎn)動，并建立四元數(shù)q。、q、q2、q? 與坐標(biāo)變換矩陣C,也就是姿態(tài)矩陣C；之間的關(guān)系。一旦有了姿態(tài)矩陣就可以解算三個角 度。四元數(shù)Q包含了所有的姿態(tài)信息，捷聯(lián)慣導(dǎo)中的姿態(tài)更新實(shí)質(zhì)是如何計(jì)算四元數(shù)Q。 2我想嘗試編寫采用定時采樣增量法解四元數(shù)微分方程解算姿態(tài)的程序。我覺得通過編 程可以更加深入的了解四元數(shù)算法。 3直在困

17、惑 GPS在姿態(tài)解算中到底起什么作用，今天終于釋然了，在（ QIN2P255 ） 其中心的獲取按下式進(jìn)行 必=述 一 + 必 2* 50） 式中，応是捷聯(lián)陀螺的輸岀（對機(jī)械轉(zhuǎn)子陀螺必須經(jīng)過動、靜態(tài)誤差的補(bǔ)償）匚由 姿態(tài)更新的最新值確定；胡“和就分別是位置速率和地球自轉(zhuǎn)速率對于導(dǎo)航坐標(biāo) 系取地理坐標(biāo)系的悄況 嚨十必= twifcosL l sinL 十 Ve Rn 式中為導(dǎo)航計(jì)算所得的最新值。 這里雖然沒有提到 GPS,但是GPS的信息可以從這里引入，引入到位置速率和地球自轉(zhuǎn)速 率，但是只能用用緯度信息 L,但是GPS輸出的速度信息，而不是東北天方向的各個分量 所以V和Vn還是得不到。 證據(jù)：（

18、1） NE0-6M GPS TEST AT0MALIENTEK KETO：Upload MEA Data SW NMEA Data Up Load: OFF Longitude：113. 32866 E Latitude:23 30203 N Altitude:81. 3m Speed:0. 232kni/h Fix Mode： 3D Valid satellite:12 Visible satellite:15 iitc n+H7ni4/n?/is (2) 5f SGPVTG (地面速度信息，Track Made Good and Ground Speed) SGPVTG語句的基本格式如2

19、SGPVTG,(1 )ST,(2),M,N,(4),K, *hh(CR)(LF) (1) 以真北為參考基準(zhǔn)的地面航向(000359度，前面的0也將被傳輸) (2) 以磁北為參考基準(zhǔn)的地面航向(000T59度，前面的0也將被傳輸) (3) 地而速率(000.0999.9節(jié)，前面的0也將被傳輸) (4) 地面速率(0000.01851.8公里/小時，前面的0也將被傳輸 (5) 模式指示(Q自主定位，D二差分，E二估算，2數(shù)據(jù)無效) 舉例如下工 $G PVTG195,N,0,361,K,A*2A (3) (4) 學(xué)習(xí)必備歡迎下載 尿蚯.其施度和位制課罟相關(guān)性用亠階馬爾可走過程表示: 網(wǎng)r.r =民h

20、+論 (5) 低成本SINS/GPS組合導(dǎo)航系統(tǒng)的結(jié)構(gòu)設(shè)計(jì)如圖4.3J所示。 水 平 對 準(zhǔn) zlgjgfQl GFS信息 性 fl 航向?qū)?(6) 圖4.31低成本IMU/GPS組合導(dǎo)航系統(tǒng)結(jié)構(gòu)設(shè)計(jì) 學(xué)習(xí)必備歡迎下載 fflfK :卅和g 電月砒置JI* 鼻*V唯 我只想說我缺少完整的速度信息（即三個方向），但是低成本的傳感器不提供。 而且如果真要 做組合導(dǎo)航就得需要經(jīng)度、緯度、高度三個信息都需要。 還有就是我以前不知道所謂位置解算，結(jié)果就是經(jīng)度、緯度和高度，得到的就是地球上 的位置。 從上圖最后一句話知道，要想解算姿態(tài)必須把速度和位置解算出來。也許下圖中可能有 缺陷，但是他說明姿態(tài)速度位置

21、解算是一個整體。 3在慣性導(dǎo)航和 GPS組合導(dǎo)航系統(tǒng)中各子系統(tǒng)的誤差源和量測中引入的誤差都是隨機(jī) 的，而卡爾曼濾波從概率統(tǒng)計(jì)最優(yōu)的角度估算出系統(tǒng)誤差并消除之。當(dāng)然也可以使用經(jīng)典的 回路反饋法。 這句話有力的說明了使用卡爾曼濾波的原因，就是為了消除誤差。 加速度計(jì) 組合 比力坐標(biāo) 變換 位速度 機(jī)體坐標(biāo)系b 容態(tài)陣r 翌態(tài)陣 航向，竇 初值 解算 態(tài)解算 航向*姿態(tài) 組合 指令解算 學(xué)習(xí)必備歡迎下載 4想問問老師 GPS提供的信息是位置信息（經(jīng)度、緯度和高度）和速度（東向、 北向和天向）嗎？ 5因?yàn)槲姨幚淼男盘柖际请S機(jī)信號，沒有確定的頻譜，所以就不能使用處理確定性 信號的常規(guī)的濾波了，比如低通、高通、帶通等。 20XX 年 3 月 25 日 16:54:23 1想買原子的 mini開發(fā)板，