基于組合導航的整平機定位系統的設計與試驗

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

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

0 引言

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

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

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

1 系統設計

1.1 硬件設計

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

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

1.2 姿態解算算法設計

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

圖1 數據融合濾波過程Fig.1 The process of data fusion filtering

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

圖2 姿態解算軟件流程圖Fig.2 Flow chart the attitude algorithm

1.3 組合導航算法設計

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

由于姿態解算和組合導航的狀態方程都是高階時變的，在主控制器中進行卡爾曼濾波時包含了大量的矩陣計算。為了保證運行速度，本文調用了函數名為"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 試驗結果及分析

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

2.1 姿態解算試驗結果分析

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

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

圖5 姿態解算實驗結果圖Fig.5 Experimental result diagram of attitude algorithm

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

2.2 組合導航試驗結果分析

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

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

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

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

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

3 結論

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