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

（1.230031 安徽省 合肥市 中國(guó)科學(xué)院合肥物質(zhì)科學(xué)研究院；2.230031 安徽省 合肥市 中科合肥智慧農(nóng)業(yè)谷有限責(zé)任公司）

0 引言

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

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

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

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

1 檢測(cè)方法

1.1 坐標(biāo)系的建立

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

圖1 慣導(dǎo)與下陷深度檢測(cè)點(diǎn)的坐標(biāo)系示意圖Fig.1 Schematic diagram of coordinate system of inertial navigation and subsidence depth points

在拖拉機(jī)前進(jìn)方向的右側(cè)的行走系履帶下部設(shè)置下陷深度檢測(cè)點(diǎn)坐標(biāo)系O'-X'Y'Z'，為便于測(cè)量和人眼直觀辨識(shí)，將坐標(biāo)系O'-X'Y'Z'原點(diǎn)O'設(shè)置在履帶行走系中間的承重輪軸線中心上，坐標(biāo)系O'-X'Y'Z'原點(diǎn)即下陷深度檢測(cè)點(diǎn)。同理，在拖拉機(jī)前進(jìn)方向的左側(cè)的行走系履帶下部設(shè)置下陷深度檢測(cè)點(diǎn)坐標(biāo)系O"-X"Y"Z"。拖拉機(jī)兩側(cè)運(yùn)動(dòng)方向即為拖拉機(jī)運(yùn)動(dòng)方向，因此坐標(biāo)系O'-X'Y'Z'和坐標(biāo)系O"-X"Y"Z"的X、Y、Z軸的正向均與坐標(biāo)系O-XYZ的一致。

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

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

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

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

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

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

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

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

按先繞X軸再繞Y軸再繞Z軸的旋轉(zhuǎn)順序，此時(shí)水田拖拉機(jī)位置1 相對(duì)位置0 的坐標(biāo)轉(zhuǎn)換矩陣Rot（0，1）可表示為

1.3 下限深度的計(jì)算

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

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

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

式中：gtx——沿慣導(dǎo)坐標(biāo)系x軸測(cè)量的加速度分量；gty——沿慣導(dǎo)坐標(biāo)系y軸測(cè)量的加速度分量；gtz——沿慣導(dǎo)坐標(biāo)系z(mì)軸測(cè)量的加速度分量。

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

此時(shí)處于位置1 的水田拖拉機(jī)的右側(cè)的下陷深度檢測(cè)點(diǎn)在坐標(biāo)系O-XYZ下可表示為

處于位置1 的水田拖拉機(jī)左側(cè)的下陷深度檢測(cè)點(diǎn)在坐標(biāo)系O-XYZ下可表示為

則水田拖拉機(jī)行走系的右側(cè)下陷深度為

水田拖拉機(jī)行走系的左側(cè)下陷深度為

1.3 PLC 程序

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

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

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

圖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ù)據(jù)。采用開環(huán)控制方式最大的優(yōu)點(diǎn)是無反饋環(huán)節(jié)，即輸出量C（s）受控于輸入量R（s），且不能對(duì)控制量反施影響。

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

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

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

2 實(shí)驗(yàn)與分析

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

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

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

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

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

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

此外，在經(jīng)過臺(tái)階式下沉或上升明顯的地面，由于此時(shí)沿大地坐標(biāo)系Z軸的加速度作用時(shí)間相對(duì)較長(zhǎng)，垂直方向的位置落差較明顯，采用增加屏蔽段和未加屏蔽段的計(jì)算方法計(jì)算出車體高度值均在有效范圍內(nèi)。

3 結(jié)論

本文基于慣導(dǎo)技術(shù)提出了一種間接的、不接觸水面實(shí)時(shí)檢測(cè)拖拉機(jī)行駛過程中下陷深度的方法，避免了水體和爛泥對(duì)檢測(cè)精度的影響。

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

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

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