基于多傳感融合的丘陵山地農(nóng)機姿態(tài)檢測單元

蒿晟昆，霍靜琦，張 宇，候 璞，張 琛，李志偉

（山西農(nóng)業(yè)大學(xué)農(nóng)業(yè)工程學(xué)院，山西晉中 030801）

國內(nèi)對于丘陵山地農(nóng)機的研究主要集中在其車身調(diào)節(jié)機構(gòu)及調(diào)整算法[1]，對于農(nóng)機作業(yè)姿態(tài)檢測研究較少，多數(shù)研究只采用單軸或雙軸傾角傳感器采集農(nóng)機車身姿態(tài)信號，但是拖拉機在作業(yè)時抖動噪音干擾極大，僅依靠傾角傳感器很難獲得農(nóng)機真實的姿態(tài)信息，發(fā)動機工作噪音、路面噪音、車身部件抖動噪音均會影響姿態(tài)檢測值[2]。該文通過多傳感器融合，利用雙軸傾角傳感器及三軸加速度計實時對丘陵作業(yè)時的農(nóng)機進行姿態(tài)檢測，分別采用不同的濾波算法對采樣信號進行處理得到兩組傾角信號，通過信號融合算法將兩組傾角信號融合，進而獲得更加精確、穩(wěn)定、全面的農(nóng)機姿態(tài)信息。

1 丘陵山地拖拉機作業(yè)安全性分析

丘陵山地農(nóng)機的安全性主要從橫向穩(wěn)定性和縱向穩(wěn)定性兩個方面考慮[3]；提高橫縱向穩(wěn)定性是為防止農(nóng)機在丘陵山地作業(yè)時出現(xiàn)傾翻或者滑移等危險情況。爬坡作業(yè)時的受力分析如圖1 所示，通常農(nóng)機緩慢勻速爬坡作業(yè)，空氣阻力對農(nóng)機縱向穩(wěn)定性的影響忽略不計，其中，農(nóng)機受力情況如下：自身重力G、地面給予的垂直支撐力N、履帶驅(qū)動輪的牽引力P1、履帶所受到的地面阻力Pf。設(shè)此時斜坡角度為α1，力矩平衡公式[4]如式（1）所示：

圖1 農(nóng)機爬坡作業(yè)的受力圖

根據(jù)式（1）可知，隨著坡度加大，地面對履帶的垂直支撐力N逐漸沿著履帶從接地中點向接地后緣點A移動，所以農(nóng)機爬坡作業(yè)時不發(fā)生縱向傾翻的臨界條件為：

當(dāng)農(nóng)機處于另外一種作業(yè)狀態(tài)，設(shè)農(nóng)機以較低勻速進行橫坡作業(yè)，其整機受力如圖2 所示，其中，農(nóng)機質(zhì)心高度為h、質(zhì)心偏移為e、N1和N2分別為地面對左右履帶的支撐合力、Z1和Z2分別為地面對履帶的橫向摩擦力、Fi為繞M點的慣性力、B和b分別為履帶軌距和接地寬度。農(nóng)機勻速行駛時慣性力忽略不計。根據(jù)受力平衡方程，當(dāng)右側(cè)履帶支撐合力N2的作用點移至M點的橫向傾翻臨界條件為[5]：

圖2 農(nóng)機橫坡作業(yè)的受力圖

2 姿態(tài)檢測傳感器

該文采用三軸加速度傳感器和雙軸傾角傳感器，傳統(tǒng)研究利用單軸傾角傳感器檢測農(nóng)機姿態(tài)不夠全面，僅能檢測農(nóng)機坡地作業(yè)時的橫向傾角[6]；將單軸傾角傳感器置于農(nóng)機車身質(zhì)心處雖然可以檢測出傾角，但是實際作業(yè)中很難將農(nóng)機的運動理想化為一個質(zhì)點[7]，因此在車身前后安裝傾角傳感器和加速度傳感器測出對應(yīng)的傾角信號，結(jié)合傳感器各自的特點，采用不同的濾波算法最大化消除農(nóng)機自身抖動和路面引入的噪音信號[8]，通過信號融合算法將兩者融合后便能更準(zhǔn)確地呈現(xiàn)農(nóng)機姿態(tài)。

傾角傳感器選用邁科傳感公司的LVG726T 型動態(tài)傾角傳感器，該款傳感器工作溫度范圍為-40～85 ℃，IP67 防護等級，可用于工業(yè)、農(nóng)業(yè)等領(lǐng)域。

其工作原理為電容微型擺錘原理[9]，將傾角傳感器置于傾斜面時，地球重力在電容微型擺錘上產(chǎn)生分量，電容值也會發(fā)生變化；擺錘與左極板之間以及右極板之間各自產(chǎn)生一個電壓，當(dāng)發(fā)生傾斜時，極板電壓會按照一定規(guī)律變化；通過解算得到傾角α關(guān)于極板電壓的函數(shù)[10]。

加速度傳感器選用邁科傳感公司的MK926 型三軸加速度計，全量程范圍內(nèi)偏差典型值和比例因子穩(wěn)定性小于0.1%。

其工作原理是利用硅的機械特性設(shè)計可移動機構(gòu)，該機構(gòu)由固定和運動的兩片硅梳齒組成（Siliconfingers）；前者視為固定的電極，后者視為可移動電極。當(dāng)硅梳齒發(fā)生相對運動后，電容值和運動的相對位移存在線性關(guān)系；ASIC 接口芯片將電容差值轉(zhuǎn)換為電壓信號值，內(nèi)置微處理器將電壓信號轉(zhuǎn)換為重力加速度g在X、Y、Z軸的加速度分量Ax、Ay和Az[11]。如圖3 所示，根據(jù)三角函數(shù)關(guān)系可以解得其縱向傾斜角β2和橫向傾斜角α2，如式（4）所示：

圖3 三軸加速度轉(zhuǎn)傾角示意圖

3 丘陵山地農(nóng)機姿態(tài)信號檢測單元的設(shè)計

丘陵山地農(nóng)機姿態(tài)信號檢測單元的微控制芯片使用STM32F103ZET6，其外設(shè)拓展能力強、低功耗、總線時鐘頻率高，可用于數(shù)據(jù)處理能力要求不高、外界環(huán)境復(fù)雜、制作成本較低的農(nóng)機自動化設(shè)計中。由于農(nóng)機的駕駛臺通常只能提供12 V 或者24 V 的電壓，利用12 V 多路DC-DC 模塊將12 V 降壓至3.3 V后向單片機供電；12 V 降壓至5 V 后向三軸加速度計和動態(tài)傾角傳感器供電。通過拓展JLINK-08 模塊使其只需要USB 線即可修改程序，便于現(xiàn)場調(diào)試。此外還搭載一塊0.96 寸的OLED 數(shù)顯模塊，實時顯示姿態(tài)檢測數(shù)據(jù)。

MK926 加速度計和LVG726T 動態(tài)傾角傳感器的信號傳輸方式均為232 通信，該文通過兩組RS232-TTL 轉(zhuǎn)換模塊連接兩個傳感器的數(shù)據(jù)接口并通過串口通信方式讀入單片機內(nèi)，其中，傾角傳感器通過PA9、PA10 引腳的串口1 進行數(shù)據(jù)傳輸，加速度傳感器通過PA2、PA3 引腳的串口2 進行數(shù)據(jù)傳輸。

兩個傳感器的讀取數(shù)據(jù)包指令均相同，因此單片機在讀取傾角和加速度信號時，只需通過串口1、串口2 同步發(fā)送讀取全部數(shù)據(jù)的命令，其讀取全部數(shù)據(jù)命令設(shè)置如下：

void tx_stack_init(void)//讀取全部數(shù)據(jù)發(fā)送命令：68 04 00 04 08

對于傳感器傳回的數(shù)據(jù)還需要進行解算，MK926 加速度計測量X、Y、Z軸的加速度值由壓縮BCD 碼表示，每個軸的數(shù)據(jù)由三個字節(jié)組成，格式為XAA.BBB；A 為符號標(biāo)志，1 表示-，0 表示+；AA 為兩位加速度整數(shù)值；BBB 為三位加速度小數(shù)值。LVG726T-60 傾角傳感器測量的X、Y軸傾角值由壓縮BCD 碼表示，每個軸的數(shù)據(jù)由三個字節(jié)組成，格式為XA.AA.BB；X為符號標(biāo)志，1 表示-，0 表示+；AAA為三位角度整數(shù)值；BB 為三位角度小數(shù)值。

通常需要采集傳感器檢測的姿態(tài)數(shù)據(jù)，因此還設(shè)計了可供實時開啟/關(guān)閉的數(shù)據(jù)采集模塊，通過加裝3.3 V 電源開關(guān)向單片機PA4 引腳發(fā)送高低電平，默認(rèn)開關(guān)關(guān)閉時PA4 引腳保持低電平信號，不進行數(shù)據(jù)采集；當(dāng)手動開啟開關(guān)后，PA4 引腳置為高電平，單片機軟件觸發(fā)存儲中斷服務(wù)子函數(shù)，將此后采集的傾角、加速度數(shù)據(jù)通過SDIO 方式存入SD 卡內(nèi)，便于實驗數(shù)據(jù)分析及處理。

4 動態(tài)傾角傳感器標(biāo)準(zhǔn)中值濾波算法

利用傾角傳感器對農(nóng)機姿態(tài)進行檢測時，其采集的原始數(shù)據(jù)存在噪音干擾，影響對農(nóng)機姿態(tài)的判斷，由于LVG726 動態(tài)傾角傳感器內(nèi)部MCU 自帶卡爾曼濾波算法，因此對原始數(shù)據(jù)采用標(biāo)準(zhǔn)中值濾波算法，提高數(shù)據(jù)的可讀性；中值濾波算法是以離散數(shù)據(jù)統(tǒng)計理論為基礎(chǔ)的非線性濾波算法，其特點為最大化抑制噪音干擾的同時還能保護信號邊緣完整性。

標(biāo)準(zhǔn)中值濾波器（SMF）本質(zhì)是將絕對誤差最小估計[12]，作為非線性信號濾波技術(shù)，該算法的原理和運算對比其他濾波算法更簡單，且有良好的噪音抑制效果。假設(shè)一組離散的時間序列T，設(shè)置一定寬度的滑動窗口后，按次序從數(shù)據(jù)序列取奇數(shù)長度的數(shù)據(jù)作為樣本，對應(yīng)該時刻x的樣本為{T(x)}，如式（5）所示：

式中，ZR和ZL為非負(fù)整數(shù)，{T(x)}的取值需要對稱分布，即ZR=ZL=Z1，則有：

式（6）中Z=2×Z1+1，Ti(x)=T(x-Z1+i-1)，i={1,2,3,4,…,Z}；利用Med 函數(shù)對采樣數(shù)據(jù)取中值，即對序列T(x)進行大小排序處理，并選取中值賦給L(x)視為此次中值濾波輸出值[13]。

如果對采樣序列{T(x)}進行大小排序處理得到T1(x),T2(x),T3(x),…,TZ(x)，那么經(jīng)過中值濾波后的輸出值L(x)可用式（7）表示：

對于有限長度的離散序列{T1(x),T2(x),T3(x),…,TQ(x)}，中值濾波器的滑動窗口寬度為1+2×Z1，在濾波前還要對輸入信號兩端拓展Z1個額外數(shù)據(jù)，以保證信號不丟失，通常采用如下方法：

對稱拓展，拓展的額外數(shù)據(jù)等于滑動濾波窗口兩端信號值，如式（8）所示：

動態(tài)傾角傳感器的標(biāo)準(zhǔn)中值濾波算法步驟如下：①輸入采樣的傾角信號；②對采樣數(shù)據(jù)采用對稱拓展；③設(shè)置中值濾波器的濾波窗口大小，以達到較好的濾波效果；④對處于濾波窗口中的序列T(x)進行大小排序；⑤通過輸出序列中值L(x)作為本次傾角信號濾波值。

5 三軸加速度傳感器卡爾曼濾波算法

三軸加速度傳感器的數(shù)據(jù)包為X、Y、Z方向的加速度值A(chǔ)x、Ay、Az，根據(jù)式（4）的反正切關(guān)系可求得農(nóng)機姿態(tài)的橫向傾角α2和縱向傾角β2。加速度傳感器轉(zhuǎn)化后的角度仍存在噪音干擾，導(dǎo)致信號波動性大、平穩(wěn)性差，需要對其進行數(shù)字濾波，該文對加速度傳感器的數(shù)據(jù)進行數(shù)字卡爾曼濾波[13]。

卡爾曼濾波算法基本原理：利用系統(tǒng)中的信號及干擾噪音建立狀態(tài)空間模型，融合上一時刻的最優(yōu)估計值和當(dāng)前時刻測量值，進而解算出當(dāng)前時刻最優(yōu)估計值。該算法依照“預(yù)測值—測量值—估計值”的順序反復(fù)運算[14]，從而達到實時濾波的效果。設(shè)隨機線性離散系統(tǒng)的狀態(tài)方程及測量方程分別為：

式中，Q(k)為k時刻系統(tǒng)狀態(tài)陣；P(k)為k時刻的測量值；E(k)為系統(tǒng)噪音陣；R(k)為觀測噪音陣；H為系統(tǒng)測量參數(shù)陣；A、B為參數(shù)陣。其遞推過程如下：

1）狀態(tài)預(yù)測如式（11）所示：

2）k時刻協(xié)方差估計過程如式（12）、（13）所示：

其中，S為該系統(tǒng)噪音的協(xié)方差，V為測量噪音協(xié)方差，δ(k)為濾波增益矩陣。

3）對系統(tǒng)進行狀態(tài)估計；

4）修正k時刻該系統(tǒng)的最優(yōu)解協(xié)方差，如下：

通過上述公式，設(shè)定任意初值和k時刻的測量值P(k)，通過卡爾曼濾波算法便可遞推估計出下一時刻狀態(tài)。該文需要對加速度計轉(zhuǎn)換后的橫向傾角α2和縱向傾角β2分別進行卡爾曼濾波；輸入的傾角信號為農(nóng)機作業(yè)時的姿態(tài)信號，該系統(tǒng)真值無法確定，因此進行卡爾曼濾波處理時應(yīng)該調(diào)整系數(shù)盡可能抑制噪音對真實傾角信號的干擾，在信號不失真的情況下減小信號波動。

6 多傳感器平均加權(quán)融合算法

圖4 所示為加權(quán)數(shù)據(jù)融合算法的多傳感器測量值融合模型[15]；加權(quán)融合算法模型如下：同時對一個目標(biāo)進行測量時，n個傳感器相對應(yīng)的測量數(shù)據(jù)為X1,X2,X3,…,Xn。則第k個傳感器的測量值為Xk且每個傳感器的測量值互相獨立，互不干擾。每個傳感器的加權(quán)系數(shù)表示為W1,W2,W3,…,Wn，先將Xn進行加權(quán)融合[16]。平均加權(quán)、最優(yōu)加權(quán)和自適應(yīng)加權(quán)三種融合算法對于每個傳感器測量值的加權(quán)系數(shù)W的求解方法存在區(qū)別。式（15）表示加權(quán)融合后的取值。

圖4 加權(quán)數(shù)據(jù)融合算法模型

加權(quán)融合算法通常利用測量值Xn和真實值X計算出總均方差，當(dāng)均方差取最小值時，每個傳感器的加權(quán)系數(shù)W為最優(yōu)值，但是農(nóng)機在丘陵山地坡地作業(yè)時，無法獲取姿態(tài)的真實值X，僅能通過傳感器的噪音濾波和真實信號融合盡可能描述農(nóng)機的作業(yè)姿態(tài)；因此該文對三軸加速度計轉(zhuǎn)換后的橫向傾角α2、縱向傾角β2和傾角傳感器采集的橫向傾角α1和縱向傾角β1進行加權(quán)平均濾波處理。

加權(quán)平均融合算法（MVFA）基于數(shù)學(xué)平均式求取的思想對各傳感器的加權(quán)系數(shù)Wi取相等的值；n個傳感器在同一時刻對同一對象進行測量，各傳感器方差分別為

由于該文采用雙傳感器數(shù)據(jù)融合，其總均方差可以簡化為式（16）：

雙傳感器經(jīng)過MVFA 算法融合后的姿態(tài)估計值可以表示為：

結(jié)合上文描述的傾角濾波及傾角融合算法，圖5所示為農(nóng)機姿態(tài)檢測的主要步驟。

7 試驗與結(jié)果分析

試驗選用一臺小型履帶柴油動力農(nóng)機進行丘陵山地作業(yè)。如圖6（a）、（b）所示為農(nóng)機在橫坡上作業(yè)時采集的數(shù)據(jù)，此時農(nóng)機存在橫向側(cè)翻的隱患，分別繪制加速度計采集的加速度信號轉(zhuǎn)化為傾角信號后進行卡爾曼濾波、傾角傳感器采集的傾角信號進行標(biāo)準(zhǔn)中值濾波的曲線圖。根據(jù)對比分析可知，三軸加速度計所測得的傾角信號波動范圍為0～20.4°，傾角信號傳感器所測得的傾角信號波動范圍為0～51.2°。進行濾波處理的曲線均表現(xiàn)平滑，排除意外噪音干擾，波動范圍均為0～3°，符合濾波預(yù)期效果。

圖6 橫向傾角數(shù)據(jù)圖

在三軸加速度計所測得的傾角信號波動范圍為0～6.3°，傾角信號傳感器所測得的傾角信號波動范圍為0～6.7°的基礎(chǔ)上，圖7 所示為農(nóng)機在縱坡上作業(yè)時采集的數(shù)據(jù)，此時農(nóng)機存在縱向側(cè)翻的隱患，分別繪制加速度計采集的加速度信號轉(zhuǎn)化為傾角信號后進行卡爾曼濾波、傾角傳感器采集的傾角信號進行標(biāo)準(zhǔn)中值濾波的曲線圖。進行濾波處理的曲線均表現(xiàn)平滑，排除意外噪音干擾，波動范圍均為0～2.5°，符合濾波預(yù)期效果。

圖7 縱向傾角數(shù)據(jù)圖

多傳感融合結(jié)果如圖8、9 所示，由于三軸加速度計傳感器和傾角傳感器安裝在農(nóng)機中心不同位置處，因此農(nóng)機車體前后測量值存在差異，為了更好地描述農(nóng)機整體的作業(yè)姿態(tài)，將兩種傳感器測量值分別濾波后再采用平均加權(quán)融合算法處理。處理后的曲線更加平滑，數(shù)值波動范圍為0～2.2°，幾乎無噪音干擾，對于整車姿態(tài)變化過程描述更為準(zhǔn)確。

圖8 橫向傾角多傳感融合結(jié)果圖

圖9 縱向傾角多傳感融合結(jié)果圖

8 結(jié)論

該文通過設(shè)計一種基于多傳感融合的丘陵山地農(nóng)機姿態(tài)檢測單元，優(yōu)化現(xiàn)有的單軸傾角傳感器測量的缺陷，選擇三軸加速度與雙軸傾角相融合的姿態(tài)檢測策略，將農(nóng)機不再理想化為一個質(zhì)點，而是分別測量農(nóng)機車體前后位置的橫向、縱向傾角信號，經(jīng)過不同的濾波算法對其達到最優(yōu)處理后再次進行多傳感器融合，保證盡可能排除農(nóng)機自身抖動和外界干擾，最大程度還原農(nóng)機真實作業(yè)時姿態(tài)的變化，為后續(xù)研發(fā)丘陵山地農(nóng)機坡地自適應(yīng)機構(gòu)和控制系統(tǒng)提供了思路與參考。