基于慣導技術的水田拖拉機下陷深度檢測方法

孫恒輝，宿寧，張耀文，張雷，黃偉

（1.230031 安徽省 合肥市 中國科學院合肥物質科學研究院；2.230031 安徽省 合肥市 中科合肥智慧農業(yè)谷有限責任公司）

0 引言

下陷深度是反映拖拉機通過性能的重要指標。拖拉機行駛過程中出現(xiàn)下陷時，其行駛阻力隨下陷深度增加而增加[1-3]。水田環(huán)境下的土壤水分過大、土壤堅實度小呈爛泥狀，拖拉機在此類土壤上行駛時容易發(fā)生過度滑轉和嚴重下沉，輕則增加能耗和操作難度，重則導致拖拉機無法行駛。

對下陷深度的實時監(jiān)測不僅有助于提高水田拖拉機的實時操作性，以降低油耗、提高作業(yè)效率，同時也可以為研究水田行走機具與水田泥土之間的作用關系提供必要的實驗數(shù)據，有助于設計通過性更好、作業(yè)效率更高的水田拖拉機[4-8]。

水田環(huán)境中一直有水的存在，受到拖拉機運動的干擾，水面會產生大幅度波動，此時傳統(tǒng)的光、超聲波、視覺等測距方法不能應用到水田環(huán)境。此外，拖拉機在爛泥狀水田土壤上行駛時容易出現(xiàn)車體向一側傾斜的現(xiàn)象，這使得適用于旱地等硬質地面的接觸式檢測方式如五輪儀由于輪面不能貼合地面而產生很大的檢測誤差。水田環(huán)境具有的獨特特征，增加了水田拖拉機下陷深度檢測的難度。

本文基于慣導技術，實時檢測了水田拖拉機行駛過程中的三維加速度和三維姿態(tài)角，基于歐拉角坐標系的變換，計算出某一時刻水田拖拉機實時的下陷深度。該方法是一種非接觸式的測量與計算方法，不受水面擾動、水體潑濺、車體隨意傾斜的影響，實驗與計算結果顯示，是一種穩(wěn)定、有效、實時檢測水田拖拉機下陷深度的方法。

1 檢測方法

1.1 坐標系的建立

建立如圖1 所示的坐標系。圖中，在慣導安裝的位置上設置慣導坐標系O-XYZ，為右手坐標系。其原點O位于慣導裝置的幾何中心；Y軸正向指前，即指向拖拉機向前運動方向；Z軸垂直地面正向指天；X軸正向垂直于YZ平面并指向拖拉機外側。建立大地坐標系G-XYZ，同樣為右手坐標系。大地坐標系G-XYZ的X、Y、Z軸分別與拖拉機靜止時慣導坐標系O-XYZ的X、Y、Z軸方向平行；坐標系G-XYZ的Z軸垂直于大地表面。大地坐標系G-XYZ原點G位于地面上，大地坐標系G-XYZ原點G與慣導坐標系O-XYZ原點O之間的距離在大地坐標系G-XYZ的Z軸上的投影即為慣導相對地面的高度。

圖1 慣導與下陷深度檢測點的坐標系示意圖Fig.1 Schematic diagram of coordinate system of inertial navigation and subsidence depth points

在拖拉機前進方向的右側的行走系履帶下部設置下陷深度檢測點坐標系O'-X'Y'Z'，為便于測量和人眼直觀辨識，將坐標系O'-X'Y'Z'原點O'設置在履帶行走系中間的承重輪軸線中心上，坐標系O'-X'Y'Z'原點即下陷深度檢測點。同理，在拖拉機前進方向的左側的行走系履帶下部設置下陷深度檢測點坐標系O"-X"Y"Z"。拖拉機兩側運動方向即為拖拉機運動方向，因此坐標系O'-X'Y'Z'和坐標系O"-X"Y"Z"的X、Y、Z軸的正向均與坐標系O-XYZ的一致。

坐標系O'-X'Y'Z'原點O'在坐標系O-XYZ中的位置可表示為：=[x'，y'，z']T，坐標系O"-X"Y"Z"原點O"在坐標系O-XYZ中的位置可表示為：=[x"，y"，z"]。當拖拉機組裝完畢后，拖拉機兩側履帶相對主體的位置是固定不變的，即坐標系O'-X'Y'Z'原點O'在坐標系O-XYZ中的位置和坐標系O"-X"Y"Z"原點O"在坐標系O-XYZ中的位置是2 組和拖拉機幾何尺寸相關的常數(shù)，其空間尺寸精度通過加工和裝配公差來保證，且該位置參數(shù)值并不隨拖拉機的運動而發(fā)生改變。

1.2 水田拖拉機三維姿態(tài)的表示

由于水田土壤爛滑的獨特性，水田拖拉機行駛過程中時常會發(fā)生前后傾斜、左右傾斜，即其三維姿態(tài)時刻在發(fā)生變化，此時需要借助三位姿態(tài)角和坐標系變換的方法，將拖拉機行駛過程中其姿態(tài)相對大地坐標系的變化值表示出來。

如圖2 所示，坐標系G-XYZ表示大地坐標系，坐標系O-XYZ表示拖拉機的初始靜止位置0，坐標系O1-X1Y1Z1表示拖拉機行駛一段時間t后的位置1。

坐標系O-XYZ原點O在大地坐標系G-XYZ的Z軸上的投影點為OGZ，坐標系O1-X1Y1Z1原點O1在大地坐標系G-XYZ的Z軸上的投影點為OGZ1。坐標系O-XYZ的X、Y、Z軸與大地坐標系G-XYZ的X、Y、Z軸方向平行。平移坐標系O1-X1Y1Z1到其原點和大地坐標系G-XYZ原點重合的位置、且保持X、Y、Z三軸的方向不變后，得到坐標系O1'-X1'Y1'Z1'。

采用歐拉角表示轉換矩陣的方法[9]，坐標系O1'-X1'Y1'Z1'的X1'、Y1'、Z1'三軸方向距大地坐標系G-XYZ的X、Y、Z軸的角度分別為：φ、θ、ψ，坐標系O1-X1Y1Z1的X1、Y1、Z1三軸的方向距大地坐標系G-XYZ的X、Y、Z軸的角度也分別為：φ、θ、ψ。

車體發(fā)生前后傾斜可視為車體繞大地坐標系G-XYZ的X軸旋轉角度φ，該向余弦矩陣Rot（x1，φ）可表示為

而車體發(fā)生左右傾斜可視為車體繞大地坐標系G-XYZ的Y軸旋轉角度φ，該向余弦矩陣Rot（y1，φ）可表示為

按先繞X軸再繞Y軸再繞Z軸的旋轉順序，此時水田拖拉機位置1 相對位置0 的坐標轉換矩陣Rot（0，1）可表示為

1.3 下限深度的計算

由于水田土壤爛滑的獨特性，從實際操作中發(fā)現(xiàn)，水田拖拉機的下陷可分為兩部分：一部分是車體整體呈直線上下的下陷；另一部分是車體發(fā)生前后傾斜或左右傾斜時，水田拖拉機行走系不同局部位置的不同深度的下陷。

如圖2 所示，拖拉機行駛一段時間t后，車體整體的下陷深度可表示為水田拖拉機位置0 與位置1 的距離在大地坐標系G-XYZ的Z軸上的投影距離，即線段的長度，其可以通過與大地坐標系Z方向上垂直加速度的關系式求出。該關系式可表示為

然而由于慣導裝置是固定在拖拉機上，在行駛過程中，慣導裝置和水田拖拉機的三維姿態(tài)一樣時刻在發(fā)生變化。從慣導讀取的加速度值是以其自身坐標系為基準，需要經過轉換矩陣Rot（y1，φ）轉化后，才能表示出大地坐標系Z方向上垂直加速度。垂直加速度的計算公式為

式中：gtx——沿慣導坐標系x軸測量的加速度分量；gty——沿慣導坐標系y軸測量的加速度分量；gtz——沿慣導坐標系z軸測量的加速度分量。

為具體地說明水田拖拉機車體發(fā)生前后傾斜或左右傾斜時，不同局部位置具有不同下陷深度的情況，對圖1 中水田拖拉機行走系兩側下陷深度檢測點的位置變化進行以下計算公式的推導。

此時處于位置1 的水田拖拉機的右側的下陷深度檢測點在坐標系O-XYZ下可表示為

處于位置1 的水田拖拉機左側的下陷深度檢測點在坐標系O-XYZ下可表示為

則水田拖拉機行走系的右側下陷深度為

水田拖拉機行走系的左側下陷深度為

1.3 PLC 程序

PLC 具有可靠性高、抗干擾能力強、易于擴展、寄存器和計數(shù)器數(shù)量巨大等特點，適用于構建采集、通訊、計算、存儲下陷深度檢測數(shù)據的主要電氣系統(tǒng)[10-11]。

系統(tǒng)程序采用ispsoft-DELTA IA-PLC 語言進行編寫[10-11]，在梯形圖程序中通過 PLC 功能函數(shù)接收慣導發(fā)出的測量值信號，實現(xiàn)下陷深度測量值的自動采集、通訊、計算和存儲，PLC 控制的技術特點保證了本檢測系統(tǒng)的可控制性能、抗干擾能力及自動化程度等。

圖3 為在某一時刻t采集慣導發(fā)送的在其自身坐標系下X方向上加速度值的PLC 梯形圖，采集Y方向上加速度值和Z方向上加速度值的PLC 梯形圖與之類似。圖4 為采集慣導發(fā)送的三維姿態(tài)角的PLC 梯形圖。其中，D 腳標700 表示航向角數(shù)據的采集，D 腳標702 表示俯仰角數(shù)據的采集，D腳標704 表示橫滾角數(shù)據的采集。航向角表示車體繞坐標系O-XYZ的Z軸旋轉角度ψ，俯仰角表示車體繞坐標系O-XYZ的X軸旋轉角度φ，橫滾角表示車體繞坐標系O-XYZ的Y軸旋轉角度θ。

圖3 采集X 方向上加速度值的PLC 梯形圖Fig.3 PLC ladder diagram for collecting acceleration value in X direction

圖4 采集三維姿態(tài)角的PLC 梯形圖Fig.4 PLC ladder diagram of collecting 3D attitude angle

本系統(tǒng)采用開環(huán)控制的方式讀取各傳感器采集的數(shù)據。采用開環(huán)控制方式最大的優(yōu)點是無反饋環(huán)節(jié)，即輸出量C（s）受控于輸入量R（s），且不能對控制量反施影響。

采集到慣導發(fā)送的加速度值和旋轉角度φ和旋轉角度θ，利用PLC 內嵌的st 語言編寫功能塊，計算沿大地坐標系Z方向上垂直加速度，并計算水田拖拉機車體發(fā)生傾斜時拖拉機行走系各不同局部檢測點的下陷深度。部分程序代碼如圖5 所示。

圖5 計算下陷深度的st 語言程序Fig.5 st language programs for calculating subsidence depth

程序中設置屏蔽段的原因是由于受到地球運轉和慣導加工工藝與檢測原理的影響，即使放置在人眼視為靜止的地面，慣導輸出的讀數(shù)仍然是持續(xù)小范圍變化的值，而不是一個靜止的常數(shù)值。數(shù)值波動會被錯誤地認為是合理的沿大地坐標系Z軸的下陷引起的加速度變化，從而會對整體下陷深度的測量產生誤差。本文采用設置屏蔽段上下區(qū)間值的方法，將有可能產生檢測誤差的細小波動值舍去。

2 實驗與分析

將慣導安裝在某型拖拉機上開展實際測試，車身高度2.556 m。設置3 種地形特征：（1）臺階式下沉地面；（2）臺階式上升地面，臺階高度2 mm；（3）斜坡式上升地面，斜坡角度19.35°，高度0.27 m。根據慣導輸出的三維加速度值和三位姿態(tài)角，按式（4）—式（11）依次計算和記錄在3 種地形上行駛時車體高度的實時變化曲線，如圖6—圖8 所示。曲線是計算左右側下限深度變化曲線的重要中間參數(shù)。為了便于與地面高度形成直觀的比較，圖中實際計算數(shù)據已減去車體高度。實驗中，車速設為8 km/h，慣導數(shù)據采樣間隔0.1 s。

如圖6—圖8 所示，細實線表示實際地面值，帶星號的細實線為未加屏蔽段計算的車體高度值，帶圓圈的細實線表示在程序中增加屏蔽段和合理設置屏蔽段上下區(qū)間值后計算的車體高度值。圖6 中細實線表示一半為地面一半為下沉2 mm 地面的臺階式下沉地面，下沉在拖拉機前進方向1 m 處。圖7 中細實線表示一半為地面一半為上升2 mm 地面的臺階式上升地面，上升在拖拉機前進方向1 m 處。圖8 中細實線表示角度為斜角19.35°高度0.7 m 的斜坡式上升地面。

圖6 臺階式下沉地面行駛時車體高度的變化曲線示意圖Fig.6 Variation curve of tractor body height when driving on stepped sinking ground

圖7 臺階式上升地面行駛時車體高度的變化曲線示意圖Fig.7 Variation curve of tractor body height when driving on stepped up ground

圖8 斜坡式上升地面行駛時車體高度的變化曲線示意圖Fig.8 Variation curve of tractor body height when driving on sloping rising ground

如圖6—圖8 所示，在地面情況變化不大的情況下，如水平路面和斜角19.35°高度0.7 m 緩慢上升的斜坡式上升地面，未加屏蔽段計算的車體高度值偏離細實線所標識的實際地面值較大，增加了累計誤差，甚至會出現(xiàn)嚴重失真的情況。增加屏蔽段計算的車體高度值偏離細實線所標識的實際地面值較小，同時也較為穩(wěn)定。

此外，在經過臺階式下沉或上升明顯的地面，由于此時沿大地坐標系Z軸的加速度作用時間相對較長，垂直方向的位置落差較明顯，采用增加屏蔽段和未加屏蔽段的計算方法計算出車體高度值均在有效范圍內。

3 結論

本文基于慣導技術提出了一種間接的、不接觸水面實時檢測拖拉機行駛過程中下陷深度的方法，避免了水體和爛泥對檢測精度的影響。

（1）計算下陷深度的關鍵參數(shù)是計算垂直方向的加速度，基于歐拉角推導的拖拉機靜止和移動位置之間相互表示的轉換矩陣，是將從慣導讀取的基于其自身坐標系的加速度轉化為計算垂直方向的加速度的關鍵。

（2）PLC 作為構建采集、通訊、計算、存儲下陷深度檢測數(shù)據的主要電氣系統(tǒng)，具有結構簡單、運行穩(wěn)定可靠等優(yōu)點，大幅度降低了檢測過程所需的時間和人力。

（3）實驗結果表明，通過設置屏蔽段上下區(qū)間的值，有助于穩(wěn)定檢測。此外，拖拉機行走系不同位置的下陷深度是不一樣的。