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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

8 結(jié)論

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