四軸姿態(tài)解算

1、又花了將近一個(gè)星期，終于把姿態(tài)解算的框架完成了。僅僅是把陀螺儀、加速度計(jì)、羅盤融合在一起，得出旋轉(zhuǎn)姿態(tài)，沒(méi)有對(duì)加速度積分，沒(méi)有用到氣壓計(jì)，幾乎沒(méi)有濾波。算是階段性的工作吧，把框架設(shè)計(jì)得合理一點(diǎn)，以后添加/修改就很簡(jiǎn)單了。從傳感器的讀取，到四元數(shù)的學(xué)習(xí)，到空間旋轉(zhuǎn)的處理方法，循序漸進(jìn)，逐個(gè)擊破。主要參考了以下資料（按閱讀的時(shí)間循序）：計(jì)算機(jī)圖形學(xué)幾何工具算法詳解（四元數(shù)轉(zhuǎn)矩陣的公式是錯(cuò)的?。┙换ナ接?jì)算機(jī)圖形學(xué)基于OpenGL的自頂向下方法維基百科四元數(shù)框框的日記四元數(shù)青衫湮痕四元數(shù)Heath's blog四元數(shù)與歐拉角之間的轉(zhuǎn)換阿莫電子論壇【原創(chuàng)】姿態(tài)估計(jì)下面總結(jié)一下“姿態(tài)解算”的過(guò)程，

2、分為“傳感器”、“四元數(shù)與旋轉(zhuǎn)”、“姿態(tài)解算框架”、“長(zhǎng)期融合”、“快速融合”四部分。1.傳感器我用的是10軸姿態(tài)傳感器模塊，其中陀螺儀是L3G4200D，加速度計(jì)是ADXL345，羅盤是HMC5883L，氣壓計(jì)是BMP085。全部都通過(guò)一條共用的I2C總線訪問(wèn)，速度都支持400kHz。先講講I2C庫(kù)。要配置、讀取傳感器，首先把通信做好，這里就是I2C庫(kù)了?，F(xiàn)在大部分單片機(jī)都有支持中斷的硬件I2C了，可以寫個(gè)高效的I2C庫(kù)。我只實(shí)現(xiàn)了主機(jī)發(fā)送和主機(jī)接收模式，這里簡(jiǎn)單介紹一下接口。接口函數(shù)主要有2個(gè)：uint8_t I2C_transmit    

3、0;      (uint8_t which,I2C_transmitCallback cb);void I2C_setNextCallback       (uint8_t which,I2C_transmitCallback cb);I2C_transmit()用于觸發(fā)一次傳輸（發(fā)送或接收），異步執(zhí)行，調(diào)用后馬上返回。其中有一個(gè)I2C_transmitCallback類型的參數(shù)，就是決定發(fā)送或接收、如何處理數(shù)據(jù)的回調(diào)函數(shù)了，其定義如下：/* 數(shù)據(jù)傳輸回

4、調(diào)函數(shù)。 * 每（準(zhǔn)備）傳輸一個(gè)字節(jié)都調(diào)用一次。 * 參數(shù)： *      seq  => 序號(hào)，第一次調(diào)用時(shí)為0，以后每次調(diào)用遞增。 *      data => seq=0時(shí)寫(從機(jī)地址+W/R)到data。 *              seq!=0時(shí)

5、data是數(shù)據(jù)。發(fā)送就寫data，接收就讀data。 * 返回值表示下一步的行為： *   I2C_RT_START => 發(fā)送開始信號(hào)。 *   I2C_RT_STOP  => 發(fā)送停止信號(hào)。 *   I2C_RT_REPEAT_START_OR_STOP => 如果有下一次傳輸，就發(fā)送RepeatStart，否則發(fā)送Stop。 *   I2C_RT_ACK  

6、60;=> 繼續(xù)傳送，回應(yīng)ACK。 *   I2C_RT_NACK  => 繼續(xù)傳送，回應(yīng)NACK。  */typedef uint8_t (* I2C_transmitCallback)(uint8_t seq,uint8_t * data);當(dāng)用I2C_transmit()成功觸發(fā)一次傳輸后，I2C庫(kù)會(huì)根據(jù)需要調(diào)用回調(diào)函數(shù)，所以使用這個(gè)I2C庫(kù)就是寫回調(diào)函數(shù)了。而I2C_setNextCallback()則是用來(lái)設(shè)置緊接著的一次傳輸?shù)?。?dāng)本次傳輸結(jié)束時(shí)，不發(fā)送“Stop”信號(hào)，而是發(fā)送“Repeat

7、 Start”信號(hào)，然后開始下一個(gè)傳輸。利用這個(gè)函數(shù)可以發(fā)起連續(xù)多次傳輸。有了I2C庫(kù)就可以操作傳感器了，以L3G4200D為例講解。先看看DataSheet，操作L3G4200D有兩個(gè)步驟，首先配置好寄存器，然后不斷獲取數(shù)據(jù)。配置寄存器比較簡(jiǎn)單，就是發(fā)送一段數(shù)據(jù)，用到兩個(gè)函數(shù)：/*  * 初始化芯片。 * 返回值 ： L3G4200D_RT_NORMAL,L3G4200D_RT_BUS_BUSY */uint8_t l3g4200d_init(void)    if(I2C_transmit(L3G4200D_WHICH_

8、I2C,l3g4200d_init_callback) = I2C_RT_TRANSMIT_NORMAL)        return L3G4200D_RT_NORMAL;    return L3G4200D_RT_BUS_BUSY;/* * 初始化使用的I2C回調(diào)函數(shù)。 */uint8_t l3g4200d_init_callback(uint8_t seq,uint8_t * data)    const static

9、 uint8_t value =         I2C_addressToByte_write(L3G4200D_ADDRESS), /* 總線地址。 */        (0x80 | 0x20), /* 寄存器地址?；?x80表示連續(xù)寫。*/        0xEF, /* CTRL_REG1 */  

10、      0x00, /* CTRL_REG2 */        0x00, /* CTRL_REG3 */        0x00, /* CTRL_REG4，250dps量程。 */        /* 其它默認(rèn)。 */   

11、60;    /    if(seq = sizeof(value)        return I2C_RT_STOP;    /    *data = valueseq;    return I2C_RT_ACK;讀取數(shù)據(jù)就有點(diǎn)麻煩，因?yàn)樽x之前要設(shè)置寄存器指針，所以其實(shí)包含兩次傳輸：第一次是發(fā)送，設(shè)置當(dāng)前寄存器指針

12、；第二次是接收，獲取測(cè)量值。第一次傳輸結(jié)束前要用I2C_setNextCallback()設(shè)置第二次傳輸?shù)幕卣{(diào)函數(shù)。見(jiàn)代碼：/* * 獲取角速度。 * 異步，讀取成功后，l3g4200d_measureCompleted()會(huì)被調(diào)用。 * 返回值 ： L3G4200D_RT_NORMAL,L3G4200D_RT_BUS_BUSY */uint8_t l3g4200d_measureOmega(void)    if(I2C_transmit(L3G4200D_WHICH_I2C,l3g4200d_measureOmega_c

13、allback_reg) = I2C_RT_TRANSMIT_NORMAL)        return L3G4200D_RT_NORMAL;    return L3G4200D_RT_BUS_BUSY;/* * 讀角速度的I2C回調(diào)函數(shù)，設(shè)置當(dāng)前寄存器地址。 */uint8_t l3g4200d_measureOmega_callback_reg(uint8_t seq,uint8_t * data)    if(seq

14、 = 0)            *data = I2C_addressToByte_write(L3G4200D_ADDRESS);        return I2C_RT_ACK;        if(seq = 1)        

15、    *data = L3G4200D_REG_OMEGA; /* 角速度數(shù)據(jù)的寄存器地址。 */        return I2C_RT_ACK;        /    I2C_setNextCallback(L3G4200D_WHICH_I2C,l3g4200d_measureOmega_callback_read); 

16、0;  return I2C_RT_REPEAT_START_OR_STOP;/* * 讀角速度的I2C回調(diào)函數(shù)，讀取數(shù)據(jù)。 */uint8_t l3g4200d_measureOmega_callback_read(uint8_t seq,uint8_t * data)    if(seq = 0)            *data = I2C_addressToByte_read(L3G4200D_AD

17、DRESS);        return I2C_RT_ACK;        if(seq > L3G4200D_BUFFER_SIZE)        return I2C_RT_STOP;    /    l3g4200d_var.buffer_8useq-1 =

18、*data;    if(seq = L3G4200D_BUFFER_SIZE)            l3g4200d_measureCompleted();        return I2C_RT_NACK;        return I2C_RT_ACK;這樣就可以讀取

19、角速度了。所有數(shù)據(jù)都是在I2C中斷里處理，所以要注意數(shù)據(jù)安全性，合理利用volatile。其他傳感器的工作方式幾乎一樣，Crtl+C，Ctrl+V，然后改改名字，改改參數(shù)就可以用了。2.四元數(shù)與旋轉(zhuǎn)姿態(tài)解算的核心在于旋轉(zhuǎn)，一般旋轉(zhuǎn)有4種表示方式：矩陣表示、歐拉角表示、軸角表示和四元數(shù)表示。矩陣表示適合變換向量，歐拉角最直觀，軸角表示則適合幾何推導(dǎo)，而在組合旋轉(zhuǎn)方面，四元數(shù)表示最佳。因?yàn)樽藨B(tài)解算需要頻繁組合旋轉(zhuǎn)和用旋轉(zhuǎn)變換向量，所以采用四元數(shù)保存組合姿態(tài)、輔以矩陣來(lái)變換向量的方案。下面介紹一下四元數(shù)，然后給出幾種旋轉(zhuǎn)表示的轉(zhuǎn)換。四元數(shù)可以理解為一個(gè)實(shí)數(shù)和一個(gè)向量的組合，也可以理解為四維的向量。這

20、里用一個(gè)圈表示q是一個(gè)四元數(shù)（很可能不是規(guī)范的表示方式）。四元數(shù)的長(zhǎng)度（模）與普通向量相似。下面是對(duì)四元數(shù)的單位化，單位化的四元數(shù)可以表示一個(gè)旋轉(zhuǎn)。四元數(shù)相乘，旋轉(zhuǎn)的組合就靠它了。旋轉(zhuǎn)的“軸角表示”轉(zhuǎn)“四元數(shù)表示”。這里創(chuàng)造一個(gè)運(yùn)算q(w,)，用于把繞單位向量w轉(zhuǎn)角的旋轉(zhuǎn)表示為四元數(shù)。通過(guò)q(w,)，引伸出一個(gè)更方便的運(yùn)算q(f,t)。有時(shí)需要把向量f的方向轉(zhuǎn)到向量t的方向，這個(gè)運(yùn)算就是生成表示對(duì)應(yīng)旋轉(zhuǎn)的四元數(shù)的（后面會(huì)用到）。然后是“四元數(shù)表示”轉(zhuǎn)“矩陣表示”。再次創(chuàng)造運(yùn)算，用R(q)表示四元數(shù)q對(duì)應(yīng)的矩陣（后面用到）。多個(gè)旋轉(zhuǎn)的組合可以用四元數(shù)的乘法來(lái)實(shí)現(xiàn)?！八脑獢?shù)表示”轉(zhuǎn)“歐拉角表示”。用于顯示。3.姿態(tài)解算框架姿態(tài)解算框架其實(shí)就是程序框架了。設(shè)計(jì)框架既要準(zhǔn)確，又要高效。總體設(shè)計(jì)是這樣的：用一個(gè)計(jì)時(shí)器定時(shí)觸發(fā)測(cè)量；所有測(cè)量過(guò)程都靠中斷推進(jìn)；在main函數(shù)里不斷檢查測(cè)量是否完成，完成就進(jìn)行解算。測(cè)量過(guò)程比較耗時(shí)間，而這樣設(shè)計(jì)，測(cè)量和解算可以同時(shí)進(jìn)行，不會(huì)浪費(fèi)CPU時(shí)間在（等待）測(cè)量上。而通過(guò)計(jì)時(shí)器觸發(fā)測(cè)量，最大限度保證積分間隔的準(zhǔn)確。4.長(zhǎng)期