朱 楠，黃 歡，金 鑫，秦海鵬

(江蘇師范大學 電氣工程及自動化學院，江蘇 徐州 221000)

0 引言

我國是農(nóng)業(yè)大國，水資源嚴重短缺，平整度較低的農(nóng)田會導致農(nóng)業(yè)灌溉用水的浪費。近年來，我國田間機械化技術發(fā)展迅速，不斷將小面積的農(nóng)田進行整合，提高了水資源和土地的利用率。對插播前的水田進行整平，可保證肥料或除草劑能夠均勻地分布在水田底部，提高肥料的利用率，有效抑制雜草的生長，從而提高水稻的產(chǎn)量。整平機是保證農(nóng)田在插播前平整的一類設備，傳統(tǒng)的人工粗整平技術已經(jīng)無法滿足大面積水田的整平需求[1]，而整平機可大大提高平整精度和工作效率，因此得到了廣泛應用。

航向角控制、運動軌跡監(jiān)測及無人駕駛是整平機工作過程中的關鍵技術。獲取整平機在工作時的偏航信息不僅能保證整平機始終處于直線運動狀態(tài)，也可提供整平機驅(qū)動輪的轉向角度，從而根據(jù)差速控制技術分配不同的轉速。華南農(nóng)業(yè)大學的楊偉偉等人利用卡爾曼濾波算法將陀螺儀與加速度計的數(shù)據(jù)進行融合，解算出的俯仰角可實現(xiàn)平地鏟的姿態(tài)控制，代替高成本的傾角傳感器[2]，但無法同時提供整平機的偏航信息。在無人駕駛的過程中，保證整平機以規(guī)定的速度沿規(guī)劃好的路徑行走尤為重要。三一重工路面機械研究院的胡宏秋和張中堯等人采用GPS系統(tǒng)實現(xiàn)了整平機的遠程控制與定位[3]。然而，GPS在遇到室外復雜地形時信號易失鎖，無法滿足整平機在復雜工作環(huán)境下的定位要求。慣性導航是基于慣性技術的一種導航方法，能夠?qū)崿F(xiàn)運動體的自主控制和測量，提供運動體實時的姿態(tài)信息。但是，慣性導航主要依賴于對陀螺儀和加速度計輸出的角速度和加速度進行積分來獲得導航參數(shù)，隨著時間的累積，陀螺儀的積分誤差會越來越大。因此，提高慣性導航的定位精度主要是提高陀螺儀的精度。鑒于慣性導航和GPS各自的優(yōu)缺點可以互補，它們之間的組合已成為組合導航中的“黃金搭檔”，一般將卡爾曼濾波作為核心算法。目前，國內(nèi)外研究較為深入的有松組合模式和緊組合模式。松組合模式是一種組合深度較淺的模式，也是GPS/INS最早使用的組合方式，在工程上易于實現(xiàn)[4]。

綜上，本文將慣性導航使用的慣性傳感器輸出的數(shù)據(jù)與磁力計輸出的數(shù)據(jù)進行融合，實現(xiàn)姿態(tài)解算，解算出的姿態(tài)角可同時提供整平機的航向角及實現(xiàn)平地鏟的姿態(tài)控制。選擇GPS/INS組合導航的定位方式對整平機進行定位，將慣性傳感器輸出的數(shù)據(jù)進行慣導解算后與GPS輸出的導航參數(shù)進行融合，估計整平機的速度和位置信息。

1 系統(tǒng)設計

1.1 硬件設計

本文分為姿態(tài)解算算法設計和組合導航算法設計兩個模塊。磁力計選擇HMC5983L，慣性傳感器選擇北微傳感的BW-AH200高精度九軸航姿參考系統(tǒng)，其內(nèi)部集成了高精度的MEMS陀螺儀和加速度計，能夠提供用于進行慣導解算和姿態(tài)解算所需角速度和加速度的信息。GPS模塊選擇高精度的瑞士u-blox MAX-6Q GPS，利用C語言編程將姿態(tài)解算算法和組合導航算法在嵌入式系統(tǒng)中實現(xiàn)，主控制器選擇STM32F103TB，具有數(shù)據(jù)處理能力強、資源豐富、性價比高等優(yōu)點，是32位市場上功耗較低的產(chǎn)品，含有兩個I2C接口以及USART接口[5]。磁力計輸出的數(shù)據(jù)通過I2C接口發(fā)送給主控制器，慣性傳感器和GPS的數(shù)據(jù)通過串口發(fā)送給主控制器。編譯環(huán)境為MDK-Lite Version4.23。

選擇卡爾曼濾波作為本系統(tǒng)的核心算法，進行信息融合。在姿態(tài)解算中，將慣性傳感器和磁力計各自的輸出進行融合，狀態(tài)變量為四元數(shù)和陀螺儀的零點漂移，觀測量為加速度計和磁力計的輸出值[6]。在組合導航中，將慣性傳感器與GPS各自輸出的數(shù)據(jù)進行融合，狀態(tài)方程為慣導誤差方程，觀測方程為兩種導航子系統(tǒng)各自輸出導航參數(shù)的差值。

1.2 姿態(tài)解算算法設計

姿態(tài)解算的狀態(tài)方程為四元數(shù)微分方程，是一個非線性方程，需要對其求偏導以實現(xiàn)線性化，這種濾波方法稱為擴展卡爾曼濾波(EKF)[7]。將各傳感器的原始數(shù)據(jù)經(jīng)濾波處理后進行融合。由于共有九軸的數(shù)據(jù)，需要進行大量維數(shù)較大的矩陣運算，主控制器的性能不能完全保證高精度的運算，這就導致每次運算都會出現(xiàn)相應的線性化誤差。因此，協(xié)方差逐漸失去正定性，導致濾波發(fā)散，影響了姿態(tài)角的精度。因此，本文使用UD分解濾波算法與擴展卡爾曼濾波相結合，對卡爾曼濾波中的誤差協(xié)方差進行UD分解，運算數(shù)量級的減少降低了線性化舍入誤差的影響，增加了穩(wěn)定性，使輸出的姿態(tài)角更加精確[8]。用姿態(tài)誤差的最優(yōu)估計補償陀螺儀提供的姿態(tài)參數(shù)，得到姿態(tài)的最優(yōu)估計。濾波過程如圖1所示。

圖1 數(shù)據(jù)融合濾波過程Fig.1 The process of data fusion filtering

將算法在嵌入式系統(tǒng)中實現(xiàn)。首先設置初始值，對狀態(tài)轉移矩陣和測量雅克比矩陣賦值，利用加速度計測量的重力和磁力計測量的磁場對陀螺儀進行雙矢量補償；經(jīng)卡爾曼濾波后，得出姿態(tài)誤差的最優(yōu)估計，從而計算出3個姿態(tài)角。數(shù)據(jù)通過串口發(fā)送給整平機主控制器。軟件流程圖如圖2所示。

圖2 姿態(tài)解算軟件流程圖Fig.2 Flow chart the attitude algorithm

1.3 組合導航算法設計

本文選擇GPS/INS組合導航的定位方式，利用卡爾曼濾波估計整平機的速度及位置信息，選擇慣導系統(tǒng)的誤差方程作為卡爾曼濾波的狀態(tài)方程，狀態(tài)變量為速度、位置及姿態(tài)角在3個方向的誤差，以及加速度計和陀螺儀各自的零點漂移。量測方程的觀測量選擇GPS和慣性導航分別輸出的位置、速度信息之差。校正方式分為輸出校正和反饋校正。由于輸出校正在工程上易于實現(xiàn)，且濾波器發(fā)生故障后不會對系統(tǒng)造成影響，因此本系統(tǒng)選擇輸出校正的方式，即將濾波后的導航參數(shù)誤差最優(yōu)估計反饋給慣性導航系統(tǒng)，得到導航參數(shù)的最優(yōu)值。數(shù)據(jù)融合之前必須進行初始化，即確定相關導航參數(shù)的初始值，包括初始姿態(tài)角的給定、初始速度的給定、位置的給定、初始對準時濾波所用的狀態(tài)誤差方差陣、量測噪聲方差陣、初始誤差方差陣的給定，以及GPS信息校正時濾波所用的狀態(tài)誤差方差陣、量測噪聲方差陣和初始誤差方差陣的給定[9]。慣性導航的導航參數(shù)由BW-AH200輸出的角速度和加速度經(jīng)慣導解算后得出。狀態(tài)方程和量測方程的具體公式見文獻[4]。算法流程圖如圖3所示。

由于姿態(tài)解算和組合導航的狀態(tài)方程都是高階時變的，在主控制器中進行卡爾曼濾波時包含了大量的矩陣計算。為了保證運行速度，本文調(diào)用了函數(shù)名為"Matrix.h"的頭文件，它包含了矩陣加、減、乘、轉置、求逆、計算行列式的值及生成單位矩陣等功能。部分程序如下：

void MatrixAdd( float* fMatrixA,float* fMatrixB,float* Result,

unsigned int m,unsigned int n )

{

unsigned int index_i = 0;

unsigned int index_j = 0;

unsigned int itemp = 0;

for (index_i=0;index_i

for (index_j=0;index_j

{

itemp = index_i*n+index_j;

*(Result+itemp) = *(fMatrixA+itemp) + *(fMatrixB+itemp);

}

}

圖3 組合導航算法軟件流程圖Fig.3 Flow chart of the integrated navigation algorithm

2 試驗結果及分析

本課題組設計了一套混凝土激光整平機的可移動實驗平臺，用來完成姿態(tài)解算實驗，如圖4所示。該實驗平臺由底座、刮平板、直流24V穩(wěn)壓電源和5個電推桿組成。其中，1用來調(diào)節(jié)前后水平，2用來模擬路況，3為24V直流穩(wěn)壓電源，4用來調(diào)節(jié)高程，5用來調(diào)節(jié)左右水平。

2.1 姿態(tài)解算試驗結果分析

將慣性傳感器和磁力計放置在刮平板上方，每隔5s將刮平板旋轉一定的角度，多次記錄實驗數(shù)據(jù)，結果如圖5所示。

圖4 整平機實驗平臺Fig.4 Experimental platform of the leveling machine

圖5 姿態(tài)解算實驗結果圖Fig.5 Experimental result diagram of attitude algorithm

由圖5可知：解算出的航向角可精確提供整平機的偏航信息，俯仰角可實現(xiàn)平地鏟的姿態(tài)控制。航向角與實際角度的偏差最大為4.8°，其余時間都在2°以內(nèi)。俯仰角與實際角度的偏差不超過2°，滿足實際要求。

2.2 組合導航試驗結果分析

為了驗證組合導航算法的可靠性，本文在整平機上進行了30s的實測，實測數(shù)據(jù)通過MatLab進行后處理。處理后的速度和位置的誤差曲線如圖6所示。

圖6 誤差曲線圖Fig.6 Error curve

在實測的30s中，人為設置一段GPS失效的時間，誤差曲線如圖7所示。

圖7 GPS失效時的誤差曲線圖Fig.7 Error curve when GPS failure

由圖6、圖7可知：本系統(tǒng)在實際應用中能夠?qū)φ綑C實現(xiàn)速度控制和精確定位。其速度和位置誤差在30s的時間內(nèi)始終趨近于“0”，且位置的最大誤差不超過5m，相較于GPS10～15m的定位精度有了很大提高，同時滿足組合導航0～5m定位精度的要求。當GPS信號失效時，僅由慣性導航提供的導航參數(shù)誤差會在很短的時間內(nèi)變得很大，與理論相符合。

3 結論

選擇GPS/INS組合導航系統(tǒng)給出整平機實時的位置和速度信息，慣導解算中沒有考慮初始對準和誤差補償。利用慣性傳感器提供的數(shù)據(jù)與磁力計進行融合，解算出的姿態(tài)角可同時給出整平機的偏航信息及實現(xiàn)平地鏟的姿態(tài)控制。本文提出的組合導航的方法不僅可以應用在整平機的水平定位上，也可以用在垂直定位(即高程測量)中，為今后提高整平機平地精度的研究打下了基礎。