基于多傳感融合的丘陵山地農機姿態檢測單元

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

（山西農業大學農業工程學院，山西晉中 030801）

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

1 丘陵山地拖拉機作業安全性分析

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

圖1 農機爬坡作業的受力圖

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

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

圖2 農機橫坡作業的受力圖

2 姿態檢測傳感器

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

傾角傳感器選用邁科傳感公司的LVG726T 型動態傾角傳感器，該款傳感器工作溫度范圍為-40～85 ℃，IP67 防護等級，可用于工業、農業等領域。

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

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

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

圖3 三軸加速度轉傾角示意圖

3 丘陵山地農機姿態信號檢測單元的設計

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

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

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

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

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

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

4 動態傾角傳感器標準中值濾波算法

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

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

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

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

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

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

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

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

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

三軸加速度傳感器的數據包為X、Y、Z方向的加速度值Ax、Ay、Az，根據式（4）的反正切關系可求得農機姿態的橫向傾角α2和縱向傾角β2。加速度傳感器轉化后的角度仍存在噪音干擾，導致信號波動性大、平穩性差，需要對其進行數字濾波，該文對加速度傳感器的數據進行數字卡爾曼濾波[13]。

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

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

1）狀態預測如式（11）所示：

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

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

3）對系統進行狀態估計；

4）修正k時刻該系統的最優解協方差，如下：

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

6 多傳感器平均加權融合算法

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

圖4 加權數據融合算法模型

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

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

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

雙傳感器經過MVFA 算法融合后的姿態估計值可以表示為：

結合上文描述的傾角濾波及傾角融合算法，圖5所示為農機姿態檢測的主要步驟。

7 試驗與結果分析

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

圖6 橫向傾角數據圖

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

圖7 縱向傾角數據圖

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

圖8 橫向傾角多傳感融合結果圖

圖9 縱向傾角多傳感融合結果圖

8 結論

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