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

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

2、的姿態(tài)我還是完不成本科生的任務(wù)。因為本科生的任務(wù)是汽車姿態(tài)測量，所以光靜止是不夠的。2015-3-11 5. 校正這一環(huán)節(jié)是我所沒有考慮到的，因為有偏差還有靈敏度不匹配。但前提是我要先解算出來。6. 論文不應(yīng)該是最后完成的，論文是邊做邊寫的，最后應(yīng)該是是復(fù)制粘貼修改格式和布局而已。7.8. 可以嘗試將adxl335的示例程序（淘寶模塊）移植到MPU6050中去解算橫滾角和俯仰角。2015-3-12 1. 為什么示例程序產(chǎn)生的六軸數(shù)據(jù)跟我想象的不一樣那呢？加速度計的數(shù)據(jù)不是9.8，沒有小數(shù)點。但是我發(fā)現(xiàn)買的arm模塊數(shù)據(jù)也不是9.8，而且我用手機里的磁鐵去干擾磁強計時，發(fā)現(xiàn)磁強計的數(shù)據(jù)發(fā)生了很大

3、的變化，如果真的要使用磁強計一定要注意周圍的磁場干擾?？磥砑铀俣扔嫷臄?shù)據(jù)是可以用的，因為別人都可以做到。為什么示例程序中減去偏移量，而我卻不能減去偏移量，比如+x的加速度最大值是16000，減去之后，在+x該等于零時，又出現(xiàn)了-16000，這是我更不想看到的。2. extern float atan2 (float y, float x);程序格式又搞錯了，人家不是atan2 (float y/float x)，否則很容易出現(xiàn)錯誤too many actual parameters.3.現(xiàn)在的狀態(tài)是不能進行全姿態(tài)解算，x軸的顯示范圍是（90º270º），和我想要的范圍（-9

4、0º+90º）正好差了180º，但是減去還不行，減去后串口上只顯示一個負(fù)號。還好汽車達不到那個角度±90º，哪怕是在汽車測試中，但是飛機能達到啊。所以這個問題最終還是要解決的。我想先把一個角度解算出來，然后去推廣。我在主函數(shù)里改動pitch=(int)(atan(ratio)*180)/3.1415926)+180);這一句不行，后來我改動void lcd_printf(char *s,int temp_data)函數(shù)里面，在第一句我加上了temp_data-=180;然后俯仰角就輸出正常了，也不知道為什么。4. uchar是一個8位無符號數(shù),

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

6、那一關(guān)，害怕，害怕就去學(xué)。2. 老師可能需要我做一個松耦合組合導(dǎo)航，先讓我去研究一下算法。然后再去用硬件實現(xiàn)。該整理資料了，整理完給老師一份，然后再說設(shè)計硬件編程的事。等我把航向角結(jié)算出來后后立馬去研究組合導(dǎo)航算法。方向錯了，停止就是前進，否則以后都不能和老師交流了。這是很危險的。我是否應(yīng)該聽老師的，先搞算法，然后再去考慮接下來的實現(xiàn)。3. 網(wǎng)上的GPS模塊沒見有遵守I2C通信協(xié)議的，都是一個收一個發(fā)送，這樣我還真得考慮數(shù)據(jù)同步的問題。2015年3月16日1.我終于知道為什么用示例程序在串口上顯示的數(shù)據(jù)為什么和我想象的那么不一樣了，因為你在初始化MPU6050的時候會對陀螺儀和加速度進行一些配

7、置，其中包括一個叫做full scale range的配置，就拿陀螺儀來說如果你將這個范圍配置在，那么這個范圍所對應(yīng)的sensitivity scale factor就是16.4。感謝唐朔飛老師的計算機組成原理，感謝日本Asahi Kasei的數(shù)據(jù)手冊，讓我在看補數(shù)、補碼的過程中，讓我在看到日本磁強計的測量數(shù)據(jù)與磁通密度的對比中讓我想到了陀螺儀和加速度計也是這樣的。謝謝你們。一個好的數(shù)據(jù)手冊就應(yīng)該讓user看明白。2.怎樣才能在陀螺儀的寄存器中的16bit數(shù)據(jù)中看出那個表示小數(shù)點？都不表示小數(shù)點，只有通過sensitivity scale factor之后才會產(chǎn)生小數(shù)點，這時的數(shù)據(jù)才是精確地。

8、好了，這下可以全身心地投入到算法研究中去了，傳感器輸出的就是，加速度數(shù)據(jù)和角速度數(shù)據(jù)，接下來你要做的就是研究一個適合車輛檢測的算法了。3.MPU6050的程序遷移到9150中沒有發(fā)現(xiàn)問題，接下來采集磁強計的數(shù)據(jù)。僅僅修改了取數(shù)據(jù)的順序，加入了磁強計各個軸數(shù)據(jù)寄存器地址，沒有使用WIA(Device ID)。采集成功后開始解算航向角，接下來需要考慮的是磁強計的三軸正方向與GYRO&ACC是不同的，所以在解算時提前要考慮好，GYRO&ACC的俯仰角和橫滾角是什么，方向是怎么定義的，然后考慮相對于磁強計這些角度和方向又是如何。4.定義繞x軸旋轉(zhuǎn)為橫滾角，繞y軸為俯仰角,繞z軸為航向角

9、。5.Mpu9150顯示9軸原始數(shù)據(jù)時出現(xiàn)問題，磁強計的數(shù)據(jù)不發(fā)生改變。我懷疑是磁強計的CNTL寄存器沒有配置，導(dǎo)致傳感器處于掉電模式。問題依然沒有解決。6.現(xiàn)在的問題是和陀螺儀加速計在一起采集時，數(shù)據(jù)能采集出來，但是值一直保持不變，單獨被采集時壓根采集不出來。顯示-00001，這個值哪怕不連接傳感器都能做到。2015年3月17日1. 磁強計與GYRO&ACC使用的slaveaddress好像是一樣的，這樣讀出的數(shù)據(jù)雖然不變但是不是-00001了，三軸讀出的數(shù)據(jù)分別是24354，-01549,00336。數(shù)據(jù)是一直往上傳的。難道是現(xiàn)在磁強計現(xiàn)在運行在單次測量模式？2. 我覺得自己得好好

10、處理一下磁強計的數(shù)據(jù)，因為磁強計的數(shù)據(jù)擁有四個符號位。H=(Single_ReadI2C(REG_Address)&0x8f;與上0x8f后4位符號位只保留一位，值依然沒有改變，處理后的數(shù)據(jù)為03874，-30221,00336。但是這不能保證都是正數(shù)的補碼，所以我決定建立一個數(shù)組，長度為13.3.如何舍棄一個二進制數(shù)的前三位？不過我又發(fā)現(xiàn)多個符號位不影響從二進制轉(zhuǎn)換為十進制。4. IICwriteByte(0xd0,0x37,0x02);         /0xd0是mpu9150的slaveaddress,0x3

11、7是旁路使能配置寄存器55，配置為0x02說明處理器能夠直接讀取輔助I2C的數(shù)據(jù)。     IICwriteByte(0xD0,0x6A,0x00);/0x6a是用戶配置寄存器106,配置為輔助總線的邏輯由主I2C總線決定。        IICwriteByte(0x18,0x0A,0x01);          /0x18是磁強計的slaveaddress,0x0a是磁強計的控制寄存器CNTL,配置為單次測量模式。阿莫電子論壇上網(wǎng)友出的招數(shù)。5.針對這一句話，每

12、次讀完數(shù)據(jù)我再配置為單次讀寫模式。int GetData(uchar REG_Address)uchar H,L;H=(Single_ReadI2C(MAGADD,REG_Address);Single_WriteI2C(MAGADD,CNTL, 0x01);L=Single_ReadI2C(MAGADD,REG_Address-1);Single_WriteI2C(MAGADD,CNTL, 0x01);return (H<<8)+L; /合成數(shù)據(jù)6. 現(xiàn)在求解出來的航向角波動很大，非常不穩(wěn)定，變化范圍在-180º180º之間。也許航向角的解算與俯仰角和橫滾角的

13、取值范圍也有關(guān)系。二者的取值范圍均為-90º90º。7. 現(xiàn)在航向角還是沒有求出來，我想這可能是磁強計的數(shù)據(jù)有問題2015年3月18日1. 現(xiàn)在的數(shù)據(jù)還是沒有換算成真實數(shù)據(jù)，而且?guī)?shù)點的數(shù)據(jù)通過串口往上傳我還沒試過。2. 另外提一點，我的俯仰角解算和橫滾角解算用的方法和上面論文提到的有差異。AN1057而且解出來的值也不是我想要的航向角，而且我懷疑上面兩個角度也不是我想要的航向角和橫滾角。3. 到現(xiàn)在我才明白采樣時間間隔內(nèi)的角增量就是角速度，速度增量就是加速度，當(dāng)時我竟然還給老師說，用的數(shù)據(jù)不是加速度和角速度，太愚蠢了，沒聽老師的話真是太失敗了。4. ？秦永元慣性導(dǎo)航第二

14、版P253答：表示的各分量構(gòu)造成的叉乘斜對稱矩陣，。5. 汽車測試中存在劃槳效應(yīng)嗎？需要劃槳效應(yīng)補償量嗎？劃槳效應(yīng)補償項的優(yōu)化算法以劃槳運動為環(huán)境條件，那么在汽車測試中，尤其是做穩(wěn)態(tài)回轉(zhuǎn)時的環(huán)境條件又是什么，這里可以分析一下，這里可以具體環(huán)境具體分析，這里可以出現(xiàn)創(chuàng)新點，就是將工作條件修改，這里就是研究生工作量的體現(xiàn)之處。6. 汽車在做等半徑穩(wěn)態(tài)回轉(zhuǎn)時，我覺得俯仰角和橫滾角是不變的，只有航向角在發(fā)生變化。我覺得汽車在做穩(wěn)態(tài)回轉(zhuǎn)運動中角速度和比力分別是因為我覺得在車輛做穩(wěn)態(tài)回轉(zhuǎn)運動時，橫滾角存在擺動，z軸上加速度波動較大。但只是感性的猜測，沒有理性的數(shù)據(jù)支持。還有就是我希望自己的程序能夠做到自適

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

16、今天終于釋然了，在（QIN2P255）這里雖然沒有提到GPS，但是GPS的信息可以從這里引入，引入到位置速率和地球自轉(zhuǎn)速率，但是只能用用緯度信息,但是GPS輸出的速度信息，而不是東北天方向的各個分量，所以和還是得不到。證據(jù)：（1）（2）（3）（4）（5）（6）我只想說我缺少完整的速度信息（即三個方向），但是低成本的傳感器不提供。而且如果真要做組合導(dǎo)航就得需要經(jīng)度、緯度、高度三個信息都需要。還有就是我以前不知道所謂位置解算，結(jié)果就是經(jīng)度、緯度和高度，得到的就是地球上的位置。從上圖最后一句話知道，要想解算姿態(tài)必須把速度和位置解算出來。也許下圖中可能有缺陷，但是他說明姿態(tài)速度位置解算是一個整體。3. 在慣性導(dǎo)航和GPS組合導(dǎo)航系統(tǒng)中各子系統(tǒng)的誤差源和量測中引入的誤差都是隨機的，而卡爾曼濾波從概率統(tǒng)計最優(yōu)的角度估算出系統(tǒng)誤差并消除之。當(dāng)然也可以使用經(jīng)典的回路反饋法。這句話有力的說明了使用卡爾曼濾波的原因，就是為了消除誤差。4. 想問問老師GPS提供的信息是位置信息（經(jīng)度、緯度和高度）和速度（東向、北向和天向）嗎？5. 因為我處理的信號都是隨機信號，沒有確定的頻譜，所以就不能使用處理確定性信號的常規(guī)的濾波了，比如低通、高通、帶通等。2015年3月25日16:54:231. 想買原子的mini開發(fā)板，自己板子上的arm不能用。按照光盤的資料提示操作串口打不開。