【文章內(nèi)容簡介】 控制量等功能。系統(tǒng)程序流程如圖41所示。圖41 系統(tǒng)程序流程 System process 無線通信模塊無線通信模塊實現(xiàn)上位機(jī)對四旋翼飛行器的飛行控制和跟蹤定位，實時與上位機(jī)控制系統(tǒng)交換信息，接收上位機(jī)控制系統(tǒng)傳輸?shù)娘w行控制指令信號、向上位機(jī)控制系統(tǒng)發(fā)送當(dāng)前飛行器實時飛行和姿態(tài)數(shù)據(jù)等相應(yīng)信息，以完成指定的飛行控制任務(wù)。STM32通過SPI接口與NRF24L01通信，將ROM中的數(shù)據(jù)發(fā)送出去。STM32控制NRF24L01發(fā)送數(shù)據(jù)，主要分為兩個步驟：一是STM32先向NRF24L01中寫入數(shù)據(jù)，二是控制NRF24L01發(fā)送數(shù)據(jù)。在執(zhí)行過程中，本設(shè)計先寫入發(fā)送數(shù)據(jù)的目標(biāo)地址再寫入數(shù)據(jù)，然后再控制NRF24L01發(fā)送地址和數(shù)據(jù)。NRF24L01發(fā)送模式會自動產(chǎn)生字頭和CRC校驗碼，當(dāng)發(fā)送過程完成后，數(shù)據(jù)準(zhǔn)備好引腳通知STM32數(shù)據(jù)發(fā)送完畢。NRF24L01的發(fā)送和接收流程如圖4圖43。圖42 發(fā)送數(shù)據(jù)流程圖 圖43 接收數(shù)據(jù)流程圖 The flow diagram of send data The flow diagram of receive data 姿態(tài)的表示方法 坐標(biāo)系統(tǒng)飛行器涉及兩個空間直角坐標(biāo)系統(tǒng)：地理坐標(biāo)系和機(jī)體坐標(biāo)系。地理坐標(biāo)系是固連在地面的坐標(biāo)系，機(jī)體坐標(biāo)系是固連在飛行器上的坐標(biāo)系。四軸飛行器運動范圍小，可以不考慮地面曲率，且假設(shè)地面為慣性系。地理坐標(biāo)系采用“東北天坐標(biāo)系”，X軸指向東，為方便羅盤的使用，Y軸指向地磁北，Z軸指向天頂。機(jī)體坐標(biāo)系原點在飛行器中心，xy平面為電機(jī)所在平面，電機(jī)分布在{|x|=|y|,z=0}的直線上，第一象限的電機(jī)帶正槳，z軸指向飛行器上方。如圖44所示。圖44 四軸的模型圖 The model of four axis 姿態(tài)的運算飛行器的姿態(tài)，是指飛行器的指向，一般用三個姿態(tài)角表示，包括偏航角(yaw)、俯仰角(pitch)和滾轉(zhuǎn)角(roll)。更深一層，姿態(tài)其實是一個旋轉(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ù)等。在本文中，矩陣用加粗大寫字母表示，如，左上標(biāo)和左下標(biāo)表示從機(jī)體坐標(biāo)系(Aircraft)變換到地理坐標(biāo)系(Earth)；四元數(shù)用加粗小寫字母表示，如，上下標(biāo)意義與變換矩陣一樣；向量用帶箭頭加粗小寫字母表示，如，左上標(biāo)A表示向量的值是在機(jī)體坐標(biāo)系的坐標(biāo)值。因為姿態(tài)實質(zhì)是一個旋轉(zhuǎn)變換，根據(jù)剛體有限轉(zhuǎn)動的歐拉定理，旋轉(zhuǎn)變換是可以串聯(lián)的，所以一個姿態(tài)可以經(jīng)過一個旋轉(zhuǎn)變換，變成另一個姿態(tài)。類比點和向量的概念，姿態(tài)相當(dāng)于點，旋轉(zhuǎn)相當(dāng)于向量，點可以通過加向量，變成另一個點。如果用矩陣表示旋轉(zhuǎn)，旋轉(zhuǎn)的串聯(lián)由矩陣乘法來實現(xiàn)。如果用四元數(shù)表示旋轉(zhuǎn)，則由四元數(shù)的乘法來實現(xiàn)旋轉(zhuǎn)串聯(lián)。用四元數(shù)來表示旋轉(zhuǎn)，組合旋轉(zhuǎn)時比用其他方法運算量更少，所以無論在計算機(jī)圖形學(xué)、飛行器控制等涉及剛體旋轉(zhuǎn)的領(lǐng)域，四元數(shù)都有舉足輕重的地位[3]。飛行器的姿態(tài)計算是圍繞姿態(tài)四元數(shù)進(jìn)行的，下面簡要介紹一下四元數(shù)的運算。1個四元數(shù)由4個實數(shù)組成： （41）規(guī)范化的四元數(shù)可以表示旋轉(zhuǎn)，見下式（42），θ為旋轉(zhuǎn)的轉(zhuǎn)角，單位向量為旋轉(zhuǎn)的轉(zhuǎn)軸。 （42）記四元數(shù)乘法的符號為。四元數(shù)乘法跟矩陣一樣，有結(jié)合律，但沒有交換律。運算過程見（43）式。 （43）四元數(shù)轉(zhuǎn)換成矩陣的函數(shù)記為R( )，具體過程見（44）式。 （44） 姿態(tài)測量獲取當(dāng)前姿態(tài)是控制飛行器平穩(wěn)飛行的基礎(chǔ)，姿態(tài)的測量要求低噪聲、高輸出頻率，當(dāng)采用陀螺儀等需要積分的傳感器時，還需要考慮積分誤差等問題。近年來MEMS傳感器越來越成熟、應(yīng)用越來越廣泛，稱為低成本姿態(tài)測量的首選器件[4]，因此本設(shè)計使用的傳感器全部都是MEMS傳感器。在使用傳感器的值進(jìn)行測量姿態(tài)之前，有必要校正傳感器。由于實驗條件限制，傳感器的校正有兩項，分別對兩種類型的傳感器：陀螺儀靜止時0輸出的傳感器、加速度計測量某向量場強(qiáng)度的傳感器。 傳感器的校正（一） 陀螺儀對于陀螺儀等靜止時0輸出的傳感器，可以很方便地校正零偏。把傳感器固定好，這時對輸出值Xf求平均值，得到的A即為零偏，如（45）式。實際使用時，把測量得到的值減去零偏，得到的值就是校正值。實際應(yīng)用的公式如（46），A為零偏值，31矩陣，單位：LSB；Yi為校正好的值，31矩陣，單位：rad/s；Xi為測量原始值，單位：LSB；gain為轉(zhuǎn)換系數(shù)，單位：(rad/s)/LSB，由傳感器的數(shù)據(jù)手冊給出。 （45） （46）（二） 加速度計加速度計是測量所在點的重力場的值的傳感器，靜態(tài)時加速度計測的是等效重力加速度場。下面介紹加速度計的校正方法。 加速度計測量的對象是比力，也就是等效重力加速度和運動加速度的和，當(dāng)靜止時，運動加速度為0，加速度計的測量值為等效重力加速度，可以利用這一點校正加速度計。加速度計的校正的思路為：對測量值平移和縮放，把測量值擬合到重力加速度。因此校正的任務(wù)為：尋找最佳的平移和縮放參數(shù)，使總體測量的數(shù)據(jù)值更接近重力加速度。記測量值為，校正后的值為，平移參數(shù)為，縮放參數(shù)為，他們之間的關(guān)系為（47）式。 （47）定義誤差u為測量值長度與重力加速度常數(shù)G的平方差 （48）把公式（47）代入（48），得： （49）記 （410） （411） （412）則u可以表示成 （413）設(shè)目標(biāo)函數(shù)U，用來衡量整體誤差，這里用單個誤差的平方和。 （414）校正的具體任務(wù)為：尋找參數(shù){a,b,c,d,e,f,g}，使U最小。因為U是{a,b,c,d,e,f,g}的多項式函數(shù)，使U最小的點必為極值點，一階偏導(dǎo)為0，得（415）式。 （415）記 （416）由式（410）、（411）和（415）： （417）為了求出P，需要解式（417）的齊次線性方程組。由于數(shù)據(jù)噪聲、運算精度限制等原因，方程（417）極少會出現(xiàn)非零解，所以轉(zhuǎn)向求近似解，解法為經(jīng)典的高斯消元法。為了減小誤差，行的處理順序有講究，先處理待消變量系數(shù)最大的行。當(dāng)處理到最后一個變量時，系數(shù)已經(jīng)變得很小了，強(qiáng)制令為0，以釋放一個自由度，這樣就有非零解了。解出的解帶一個任意常數(shù)，設(shè)為C，Pb為某基礎(chǔ)解系，見（418）式。 （418）由（411）、（412）和（418）得到6個校正參數(shù)： （419）所以校正加速度計的整體流程為：測量一批靜態(tài)數(shù)據(jù)，然后用這批數(shù)據(jù)根據(jù)（416）式生成方陣B，然后求方程（417）的近似解P，再代入式（419），得到校正參數(shù)。 數(shù)據(jù)融合有了傳感器的數(shù)據(jù)，就可以用來計算姿態(tài)了。加速度計測量對象為比力，受運動加速度的影響很大，特別是受飛行器機(jī)架振動的影響。而陀螺儀受外部影響弱，穩(wěn)定性好，但輸出量為角速度，需要積分才能得到姿態(tài)，無法避免誤差的累積。為了得到穩(wěn)定的、近實時的姿態(tài)，對各傳感器的數(shù)據(jù)取長補(bǔ)短，需要研究各種數(shù)據(jù)融合算法。（一） 姿態(tài)插值法首先介紹“姿態(tài)插值法”，過程分兩部分：一、如果已知初始姿態(tài)，可以利用陀螺儀積分，不斷推算出下一個姿態(tài)，這部分動態(tài)性能好，但誤差會累積；二、利用加速度計，可以直接計算出一個姿態(tài)，這個姿態(tài)的期望是正確的，但正如上文所述，這個姿態(tài)噪聲大，不穩(wěn)定。為了把兩個系統(tǒng)的優(yōu)點結(jié)合起來，對兩個系統(tǒng)的結(jié)果進(jìn)行插值，得到的值作為當(dāng)前的姿態(tài)。下面介紹“姿態(tài)插值法”的計算過程。首先是第一個部分陀螺儀積分算姿態(tài)。陀螺的輸出為時間離散的角速度，要對時間積分才得到角度。記陀螺輸出的角速度為，單位為rad/s，采樣間隔為。假定采樣間隔足夠短，在一個采樣時間間隔里，角速度不變，且轉(zhuǎn)角足夠小，忽略三角函數(shù)高階項，各軸間相互的影響忽略不計，根據(jù)式(42)，該間隔里的旋轉(zhuǎn)可以用四元數(shù)表示成 （420）更新前的姿態(tài)為，為在的基礎(chǔ)上的旋轉(zhuǎn)，則把他們相乘，即得到新的當(dāng)前的姿態(tài)。 （421）然后是第二個部分。姿態(tài)的定義為兩個坐標(biāo)系統(tǒng)的旋轉(zhuǎn)變換，因此只要知道兩對在兩個坐標(biāo)系對應(yīng)的向量，就可以把姿態(tài)求出來。兩對變量為加速度和磁場強(qiáng)度，測量出的加速度和磁場強(qiáng)度是在機(jī)體坐標(biāo)系的，而地理坐標(biāo)系的加速度和磁場是常量，存在一個旋轉(zhuǎn)，可以把機(jī)體坐標(biāo)系的加速度和磁場強(qiáng)度轉(zhuǎn)換到與地理坐標(biāo)系的對應(yīng)的常量重合，這個旋轉(zhuǎn)就是所求的姿態(tài)。由于傳感器系統(tǒng)誤差、噪聲等影響，測量出的加速度和磁場的夾角不是恒定值，不可能精確旋轉(zhuǎn)到與常量一樣，只能求最接近的旋轉(zhuǎn)。最接近的原則可定為：一、旋轉(zhuǎn)后四個向量共面；二、加速度和磁場成一定角度，旋轉(zhuǎn)后的角平分線與常量的角平分線重合。為了達(dá)到最接近原則，把處理的量由加速度和磁場強(qiáng)度，轉(zhuǎn)成它們的平面法線和角平分線。記加速度計輸出為，羅盤輸出為，規(guī)范化的等效重力加速度為，規(guī)范化的地磁場強(qiáng)度為 測量的加速度和磁場強(qiáng)度的平面法線向量和對角線向量為： （422） （423）等效重力加速度敞亮和地磁場強(qiáng)度常量的平面法線向量和對角線為： （424） （425）定義一個函數(shù)，用于把旋轉(zhuǎn)軸和轉(zhuǎn)角轉(zhuǎn)換成四元數(shù)： （426）再定義一個函數(shù)，用于獲取把一個向量旋轉(zhuǎn)到另一個向量的旋轉(zhuǎn)四元數(shù)： （427）第一次旋轉(zhuǎn)，把平面法線旋轉(zhuǎn)到重合： （428）經(jīng)過第一次旋轉(zhuǎn)，資格向量已經(jīng)共面，測量的加速度和磁場強(qiáng)度的對角線向量變成： （429）再進(jìn)行第二次旋轉(zhuǎn)，把對角線重疊在一起： （430）把兩次旋轉(zhuǎn)組合起來，就是第二部分所求的姿態(tài)了。 （431）把兩個部分算出的姿態(tài)進(jìn)行插值，就可以得到當(dāng)前的姿態(tài)。因為兩個部分算出的姿態(tài)相差小，用普通線性插值即可。α為插值系數(shù)，范圍在[0,1]，越接近0，第一部分的姿態(tài)占的權(quán)重就越大，一般取接近0的值。 （432）即為輸出的姿態(tài)。姿態(tài)插值法的流程如圖45。圖45 姿態(tài)插值法數(shù)據(jù)流程圖 Data flowchart of interpolation（二） 梯度下降法從實際的使用效果來說，姿態(tài)插值法已經(jīng)滿足要求，但用了三角函數(shù)，即使經(jīng)過優(yōu)化，運算量還是很大，不方便應(yīng)用到低性能MCU或提高運算頻率。Sebastian . Madgwick提出了更加有效的方法——用梯度下降法計算姿態(tài)[5]。其思路為：陀螺儀不斷積分姿態(tài)；同時把加速度和磁場強(qiáng)度的誤差表示成姿態(tài)的函數(shù)，令誤差最小的點為極值點，然后用梯度下降法逼近誤差最小的點，即糾正了陀螺的積分誤差。這個方法，陀螺計算的姿態(tài)占主要，加速度計和羅盤只是輔助糾正。 用陀螺積分姿態(tài)的方法跟“姿態(tài)插值法”的第一部分一樣，關(guān)鍵是如何用加速度和磁場強(qiáng)度糾正誤差。首先要定義誤差，最直觀就是定義為測量值與常量值之差，這樣加速度和磁場強(qiáng)度就分別有一個誤差了。因為只考慮方向，相減前要規(guī)范化。為了減少運算量、利用常量值里的 0和1分量，相減之前統(tǒng)一到機(jī)體坐標(biāo)系，而不是地理坐標(biāo)系。旋轉(zhuǎn)矩陣的逆等于其轉(zhuǎn)置矩陣。 （433）（434）加速度和磁場的誤差都是向量，需要把他們合并，而且要符合誤差最小的點在極小點。最終的誤差定義為它們模的平方和。 （435）有了誤差F，就可以對姿態(tài)求偏導(dǎo)。 （436）計算出了梯度，然后要確定步長，這里步長定位梯度的模的β倍。最后把梯度乘以步長，融合到陀螺運算的姿態(tài)里。 （437）梯度下降法的流程圖如圖46。圖46 梯度下降法數(shù)據(jù)流程圖 Data flowchart of gradient descent（三） 互補(bǔ)濾波算法Mark Euston提出了運算量比梯度下降法更小的互補(bǔ)濾波法[6]。相比梯度下降法用加速度和磁場強(qiáng)度計算姿態(tài)的梯度，互補(bǔ)濾波法是把加速度和磁場強(qiáng)度的誤差構(gòu)造成糾正旋轉(zhuǎn)，疊加到陀螺測出的角增量上，實現(xiàn)高效的數(shù)據(jù)融合。圖47 互補(bǔ)濾波法數(shù)據(jù)流程圖 Data flowchart of plementary filter統(tǒng)一到同一坐標(biāo)系后，規(guī)范化的加速度和磁場強(qiáng)度的測量值和常量值可以作一個叉積，叉積的模為角度誤差的正弦，小角情況下認(rèn)為正比于角度，方向根據(jù)右手法則，可以作為糾正的旋轉(zhuǎn)軸，乘上一個系數(shù)后，可以與陀螺算出的角增量疊加。首先算出兩個叉積： （438） （439）然后