四軸姿態(tài)檢測(cè)和控制理論

1、四軸飛行器的姿態(tài)測(cè)量四軸飛行器的姿態(tài)測(cè)量與控制與控制1二二四軸飛行器的姿態(tài)測(cè)量與四軸飛行器的姿態(tài)測(cè)量與控制控制首先簡(jiǎn)單介紹一下四軸飛行器，飛行器由機(jī)架、電機(jī)、螺旋槳和控制電路構(gòu)成。 機(jī)架呈十字狀，是固定其他部件的平臺(tái)。 電機(jī)采用直流空心杯電機(jī)。 四個(gè)螺旋槳中有兩個(gè)正槳，兩個(gè)反槳。 控制部分主要由STM32處理器和mpu6050數(shù)字姿態(tài)處理器（集成三軸陀螺儀和三軸加速度計(jì)）組成。2二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制 結(jié)構(gòu)框圖結(jié)構(gòu)框圖3二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制飛行器涉及兩個(gè)空間直角坐標(biāo)系統(tǒng)：地理坐標(biāo)系和機(jī)體坐標(biāo)系。地理坐標(biāo)系是固連在地面的坐標(biāo)系

2、，機(jī)體坐標(biāo)系是固連在飛行器上的坐標(biāo)系。地理坐標(biāo)系采用“東北天坐標(biāo)系”，X 軸指向東，為方便羅盤的使用，Y 軸指向地磁北，Z 軸指向天頂。機(jī)體坐標(biāo)系原點(diǎn)在飛行器中心，xy 平面為電機(jī)所在平面。4二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制5二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制飛行器的姿態(tài)，是指飛行器的指向，一般用三個(gè)姿態(tài)角表示，包括偏航角、俯仰角和滾轉(zhuǎn)角。更深一層，姿態(tài)其實(shí)是一個(gè)旋轉(zhuǎn)變換，表示機(jī)體坐標(biāo)系與地理坐標(biāo)系的旋轉(zhuǎn)關(guān)系，這里定義姿態(tài)為機(jī)體坐標(biāo)系向地理坐標(biāo)系的轉(zhuǎn)換。旋轉(zhuǎn)變換有多種表示方式，包括變換矩陣、姿態(tài)角、轉(zhuǎn)軸轉(zhuǎn)角、四元數(shù)等。6二二四軸飛行器的姿態(tài)測(cè)量與控

3、制四軸飛行器的姿態(tài)測(cè)量與控制因?yàn)樽藨B(tài)實(shí)質(zhì)是一個(gè)旋轉(zhuǎn)變換，根據(jù)剛體有限轉(zhuǎn)動(dòng)的歐拉定理，旋轉(zhuǎn)變換是可以串聯(lián)的，所以一個(gè)姿態(tài)可以經(jīng)過(guò)一個(gè)旋轉(zhuǎn)變換，變成另一個(gè)姿態(tài)。類比點(diǎn)和向量的概念，姿態(tài)相當(dāng)于點(diǎn)，旋轉(zhuǎn)相當(dāng)于向量，點(diǎn)可以通過(guò)加向量，變成另一個(gè)點(diǎn)。如果用四元數(shù)表示旋轉(zhuǎn)，則由四元數(shù)的乘法來(lái)實(shí)現(xiàn)旋轉(zhuǎn)串聯(lián)。7二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制22221qqqqwxyz8二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制rpqpqpqpqrpqpqpqpqrrqpqpqpqrpqpqpqpqww wx xy yz zxw xx wy zz yrpqyw xx zy wz xzw zx

4、 yy xz w9二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制 2222221 222222221 222222221 22qqq qq qq qq qq qq qqqq qq qq qq qq qq qqqyzx ywzxzwyR qx ywzxzyzwxxzwyyzwxxy10二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制 動(dòng)力學(xué)動(dòng)力學(xué)原理原理螺旋槳旋轉(zhuǎn)時(shí)，把空氣對(duì)螺旋槳的壓力在軸向和側(cè)向兩個(gè)方向分解，得到兩種力學(xué)效應(yīng)：推力和轉(zhuǎn)矩。當(dāng)四軸飛行器懸停時(shí)，合外力為0，螺旋槳的推力用于抵消重力，轉(zhuǎn)矩則由成對(duì)的正槳反槳抵消。當(dāng)飛行器運(yùn)動(dòng)時(shí)，因?yàn)橥屏χ荒苎剌S向，所以只能通過(guò)傾斜

5、姿態(tài)來(lái)提供水平的動(dòng)力，控制運(yùn)動(dòng)由控制姿態(tài)來(lái)間接實(shí)現(xiàn)。11二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制假設(shè)四軸為剛體，根據(jù)質(zhì)點(diǎn)系動(dòng)量矩定理，角速度和角加速度由外力矩決定，通過(guò)控制四個(gè)螺旋槳，可以產(chǎn)生需要的力矩。首先對(duì)螺旋槳編號(hào)：第一象限的為0 號(hào)，然后逆時(shí)針依次遞增。同步增加0 號(hào)和1 號(hào)、減小2 號(hào)和3 號(hào)槳的功率，可以在不改變推力的情況下，提供x 軸的力矩；同步增加1 號(hào)和2 號(hào)、減小0 號(hào)和3 號(hào)槳的功率，可以在不改變推力的情況下，提供y 軸的力矩；同步增加1 號(hào)和3 號(hào)、減小0 號(hào)和2 號(hào)槳的功率，可以在不改變推力的情況下，提供z 軸的力矩。12二二四軸飛行器的姿態(tài)測(cè)量與控制

6、四軸飛行器的姿態(tài)測(cè)量與控制13二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制 姿態(tài)測(cè)量姿態(tài)測(cè)量計(jì)算姿態(tài)主要用到2 個(gè)傳感器：陀螺儀、加速度計(jì)。其中加速度計(jì)測(cè)量結(jié)果受運(yùn)動(dòng)加速度影響較大，特別是飛行器的震動(dòng)；陀螺儀受外部影響較小穩(wěn)定性較好，但是輸出量為角速度，需要積分后才能得到姿態(tài)，在積分過(guò)程中不可避免的使誤差累積。為了得到準(zhǔn)確的姿態(tài)就要綜合考慮兩種傳感器的利弊，取長(zhǎng)補(bǔ)短通過(guò)數(shù)據(jù)融合得到理想的姿態(tài)。141522221sinsinsinsin2222t gt gt gt gxyzxyz 21 sinsinsinsin2222t gt gt gt gxyz 二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制1222214222nnTgggAAngggt xt yt ztrxyz 11nnnnAEEAAAnqqr16二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制11nAEmEacAAmacRqaa17二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制agtc 1222111114222nnAAnrxyzxyz 11nnnnAEEAAAnqqr18二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制19二二四軸飛行器的姿態(tài)測(cè)量與控制四軸飛行器的姿態(tài)測(cè)量與控制假設(shè)姿態(tài)差為小量，所以正弦值近似等于角度值，但注意此處四元數(shù)內(nèi)的角度值僅為實(shí)際姿態(tài)差角度的一半，將