劉 路 楊 冰 韋 東 宋 宇 陳黎卿 孫 燕,3

(1.安徽農(nóng)業(yè)大學(xué)工學(xué)院， 合肥 230036； 2.安徽省智能農(nóng)機(jī)裝備工程實(shí)驗(yàn)室， 合肥 230036；3.安徽省工業(yè)節(jié)電與電能質(zhì)量控制協(xié)同創(chuàng)新中心， 合肥 230601)

0 引言

近年來，小型履帶機(jī)器人廣泛應(yīng)用于田間作業(yè)中，由于田間路面崎嶇不平[1-3]，易造成航向偏差增大。因此，車身側(cè)傾、俯仰對(duì)小型履帶式機(jī)器人的影響成為農(nóng)田環(huán)境下農(nóng)業(yè)導(dǎo)航控制領(lǐng)域不可忽視的因素[4-8]。為解決路面崎嶇不平對(duì)小型履帶機(jī)器人的影響問題，國內(nèi)外諸多學(xué)者多采用二維平面滑?？刂?SMC)作為位姿控制系統(tǒng)[9-14]，解決了二維平整路面路徑跟蹤的問題。機(jī)器人在三維崎嶇路面上的姿態(tài)控制誤差與機(jī)器人行駛過程中的姿態(tài)變化有關(guān)，需要探索合適的姿態(tài)檢測及控制修正方法，以提升機(jī)器人實(shí)時(shí)控制精度[15-20]。

針對(duì)上述問題，本文以小型履帶機(jī)器人作為研究對(duì)象，設(shè)計(jì)一種基于降維變系數(shù)的滑?？刂葡到y(tǒng)，該系統(tǒng)采用慣導(dǎo)傳感器與RTK北斗定位系統(tǒng)，實(shí)時(shí)檢測車身位姿信息，微控制器將理想位姿信息作為降維變系數(shù)系統(tǒng)的控制輸入，實(shí)時(shí)匹配兩側(cè)履帶運(yùn)行速度，以達(dá)到在崎嶇路面修正車身航向、提高行駛穩(wěn)定性的目的。

1 履帶機(jī)器人系統(tǒng)組成與原理

1.1 履帶機(jī)器人結(jié)構(gòu)及主要參數(shù)

為了實(shí)現(xiàn)行距800 mm的壟間精確導(dǎo)航和精準(zhǔn)控制，履帶機(jī)器人的整機(jī)結(jié)構(gòu)如圖1所示，主要功能包括機(jī)器人運(yùn)行過程中通信、電機(jī)驅(qū)動(dòng)和車身控制，相關(guān)技術(shù)參數(shù)如表1所示。

圖1 小型履帶機(jī)器人整機(jī)結(jié)構(gòu)簡圖

表1 小型履帶植保機(jī)器人主要技術(shù)參數(shù)

1.2 降維變系數(shù)底盤建模方法

機(jī)器人在路面上行駛時(shí)，地面坐標(biāo)系為XOY，機(jī)器人的局部坐標(biāo)系xoy中令機(jī)器人質(zhì)心為原點(diǎn)，局部坐標(biāo)系相對(duì)于地面坐標(biāo)系的位姿可表示為T1，如圖2所示。

圖2 二維平整路面運(yùn)動(dòng)簡圖

圖2機(jī)器人的二維運(yùn)動(dòng)學(xué)方程與誤差方程為

T1(x,y,α)=N1(α)Q1(v,ω)

(1)

K1(xe,ye,αe)=R1(α)A1(Δ)

(2)

式中T1(x,y,α)——二維機(jī)器人位姿

x、y——機(jī)器人運(yùn)動(dòng)時(shí)，x、y軸方向移動(dòng)距離

α——機(jī)器人運(yùn)動(dòng)時(shí)，局部坐標(biāo)系x軸與地面坐標(biāo)系X軸間的夾角，亦稱為偏航角

N1(α)——二維實(shí)際和理想位姿坐標(biāo)系轉(zhuǎn)換矩陣

Q1(v,ω)——二維機(jī)器人線速度、角速度矩陣

v——線速度

ω——角速度

K1(xe,ye,αe)——二維機(jī)器人位姿誤差

xe、ye——x、y方向誤差

αe——偏航角誤差

R1(α)——二維實(shí)際位姿和理想位姿誤差坐標(biāo)系轉(zhuǎn)換矩陣

A1(Δ)——二維實(shí)際位姿與理想位姿誤差矩陣

Δ——二維實(shí)際位姿與理想位姿誤差

機(jī)器人在非平整路面運(yùn)動(dòng)時(shí)，出現(xiàn)隨機(jī)的姿態(tài)側(cè)向或俯仰變化，二維運(yùn)動(dòng)模型的路徑跟蹤會(huì)有較大跟蹤誤差。為保證機(jī)器人的路徑跟蹤精度，需要對(duì)車身上述姿態(tài)變化導(dǎo)致的誤差進(jìn)行補(bǔ)償以實(shí)現(xiàn)姿態(tài)的修正與位置糾偏。

圖3 三維崎嶇路面運(yùn)動(dòng)簡圖

三維運(yùn)動(dòng)模型與二維運(yùn)動(dòng)模型相比，增添垂直于地平面方向的Z軸，如圖3所示。降維模型利用機(jī)器人實(shí)際三維空間位姿與理想三維空間旋轉(zhuǎn)映射關(guān)系，使機(jī)器人運(yùn)動(dòng)過程中通過對(duì)映射后的輪距、航向的投影參數(shù)修正將控制模型始終保持在地面坐標(biāo)系XOY平面內(nèi)，從而去除控制模型中z軸方向變量，θ、γ為機(jī)器人運(yùn)動(dòng)時(shí)自身所處的局部坐標(biāo)系中z、y軸分別與地面坐標(biāo)系中Z、Y軸的夾角，即橫滾角與俯仰角。其自身所處的局部坐標(biāo)系原點(diǎn)相對(duì)于地面坐標(biāo)系的位姿表示為(x,y,z,α,θ,γ)，因此其降維變系數(shù)模型、運(yùn)動(dòng)學(xué)方程與誤差方程為

U(xe,ye,αe)=f(θ,γ)

(3)

T2(x+xe,y+ye,α+αe)=N1(α)f(θ,γ)Q1(v,ω)

(4)

K2(xe,ye,αe)=R2(α)f(θ,γ)A2(Δ)

(5)

式中U(xe,ye,αe)——投影參數(shù)修正函數(shù)

f(θ,γ)——空間旋轉(zhuǎn)映射函數(shù)

T2(x+xe,y+ye,α+αe)——三維機(jī)器人位姿

K2(xe,ye,αe)——三維車身位姿誤差

R2(α)——三維實(shí)際位姿和理想位姿誤差坐標(biāo)系轉(zhuǎn)換矩陣

A2(Δ)——三維實(shí)際位姿與理想位姿誤差矩陣

上述降維變系數(shù)中，將圖3所示三維狀態(tài)下機(jī)器人位姿通過映射投影的方法獲得理想位姿。在映射投影過程中，姿態(tài)發(fā)生變化產(chǎn)生二維平面位姿誤差，因此，采用投影參數(shù)修正函數(shù)U(xe,ye,αe)通過映射函數(shù)f(θ,γ)對(duì)產(chǎn)生的誤差進(jìn)行補(bǔ)償，進(jìn)而消除z軸方向與θ、γ波動(dòng)對(duì)跟蹤精度的影響。三維運(yùn)動(dòng)學(xué)模型T2在映射函數(shù)f(θ,γ)的補(bǔ)償下進(jìn)行二維控制，最終達(dá)到降維控制的目的。

2 降維變系數(shù)控制方法

在降維運(yùn)動(dòng)學(xué)幾何模型基礎(chǔ)上，設(shè)計(jì)基于降維變系數(shù)的位置控制外環(huán)、偏航角控制內(nèi)環(huán)、滿足機(jī)器人運(yùn)動(dòng)約束條件的控制策略，實(shí)現(xiàn)機(jī)器人對(duì)于位置、姿態(tài)的精確控制，同時(shí)實(shí)現(xiàn)在崎嶇非平整路面上平穩(wěn)行駛的功能。

2.1 糾偏系統(tǒng)工作原理

本文所研究的雙輪差速轉(zhuǎn)向履帶機(jī)器人，通過控制兩驅(qū)動(dòng)輪的轉(zhuǎn)速對(duì)車身姿態(tài)與理想軌跡進(jìn)行調(diào)節(jié)與跟蹤。機(jī)器人實(shí)際位置信息作為控制輸入，其線速度與角速度作為控制輸出，通過理想路徑與實(shí)際路徑的誤差反饋，由糾偏控制系統(tǒng)消除理想位置與實(shí)際位置的誤差，最終實(shí)現(xiàn)路徑跟蹤。

上述閉環(huán)系統(tǒng)屬于雙環(huán)滑模控制系統(tǒng)，其中姿態(tài)控制系統(tǒng)為內(nèi)環(huán)，位置控制系統(tǒng)為外環(huán)。位姿信息在控制律p1、p2、p3和p4的控制下通過降維變系數(shù)模型傳遞給內(nèi)外環(huán)控制，完成對(duì)線速度、角速度的控制，最終達(dá)到路徑跟蹤的目的。其控制流程圖如圖4所示。

圖4 降維變系數(shù)控制系統(tǒng)流程圖

2.2 降維變系數(shù)控制系統(tǒng)組成

降維變系數(shù)控制系統(tǒng)由STM32F407ZGT6型微處理器、MTI-G-710型慣導(dǎo)傳感器、ZM-6615型直流無刷電機(jī)驅(qū)動(dòng)器、直流無刷電機(jī)等組成，如圖5所示。其中，慣導(dǎo)傳感器提供姿態(tài)信息，RTK北斗定位系統(tǒng)提供位置信息，上位機(jī)根據(jù)控制模型輸出控制變量，輸出的控制變量通過運(yùn)動(dòng)控制器將線速度、角速度信息轉(zhuǎn)換成左右履帶輪電機(jī)轉(zhuǎn)速，最終兩輪差速運(yùn)動(dòng)實(shí)現(xiàn)路徑跟蹤。

圖5 控制系統(tǒng)組成與結(jié)構(gòu)

2.3 降維變系數(shù)控制系統(tǒng)設(shè)計(jì)

2.3.1位置外環(huán)控制律設(shè)計(jì)

(6)

上述位置控制系統(tǒng)中p1在x方向的子系統(tǒng)控制律為p1x

式中q1——波動(dòng)量λ1、a1、η1——控制參數(shù)

由此可以得到

(7)

若系統(tǒng)趨于穩(wěn)定，則x方向線速度為

同理，針對(duì)y控制系統(tǒng)，設(shè)計(jì)控制律為

(8)

式中λ2、a2、η2——控制參數(shù)

p1y——在y方向的位置子系統(tǒng)控制律

s2——在y方向的滑模函數(shù)

2.3.2偏航角內(nèi)環(huán)控制律設(shè)計(jì)

(9)

(λ4>0,η4>0)

式中q4——波動(dòng)量

得到

若系統(tǒng)趨于穩(wěn)定，則需要x方向線速度為

同理，針對(duì)θ、γ控制系統(tǒng)，其控制律為

(10)

(11)

從而實(shí)現(xiàn)s5、s6指數(shù)收斂，θ、γ收斂于理想姿態(tài)θd、γd。最終實(shí)現(xiàn)履帶機(jī)器人對(duì)理想姿態(tài)曲線的角度跟蹤。

由于獨(dú)立的外環(huán)控制在位置跟蹤過程中易受到姿態(tài)波動(dòng)的影響，產(chǎn)生跟蹤誤差大、穩(wěn)定性差的情況，因此引入偏航角內(nèi)環(huán)控制，通過內(nèi)環(huán)收斂速度大于外環(huán)收斂速度的方法，確保整個(gè)降維變系數(shù)控制系統(tǒng)的穩(wěn)定。

3 仿真試驗(yàn)

為了驗(yàn)證降維變系數(shù)控制方法可在二維路面上進(jìn)行精確跟蹤，以雙輪差速轉(zhuǎn)向模型作為仿真對(duì)象，在Matlab/Simulink環(huán)境下進(jìn)行二維平整路面路徑跟蹤仿真試驗(yàn)。二維路徑追蹤試驗(yàn)中，初始位置與姿態(tài)設(shè)為(0,0,0,0,0,0)，設(shè)置理想路徑為

(12)

表2 二維仿真相關(guān)參數(shù)

在此平面路徑下，進(jìn)行二維路徑仿真跟蹤時(shí)，由于不存在z軸數(shù)據(jù)，故而僅需仿真x、y軸誤差與追蹤α即可。仿真結(jié)果如圖6所示。

圖6 二維路徑跟蹤仿真結(jié)果

二維路徑仿真結(jié)果表明，二維平整路面路徑跟蹤過程中，x軸誤差在初始的0～2 s內(nèi)便逐漸減少至0。y軸誤差始終控制在±0.2 m之間，偏航角最初的1 s內(nèi)精確跟蹤了理想角度。

為了驗(yàn)證降維變系數(shù)控制方法在三維崎嶇路面跟蹤的精確性，進(jìn)行三維路徑跟蹤仿真試驗(yàn)，具體仿真參數(shù)如表3所示。

考慮到路面起伏，取理想路徑函數(shù)為初始位置與姿態(tài)設(shè)為(0,0,0,0,0,0)。

(13)

表3 三維仿真相關(guān)參數(shù)

三維路徑跟蹤仿真結(jié)果如圖7所示。圖7a表明，履帶機(jī)器人可精確跟蹤二維平整路面理想軌跡，結(jié)合圖7b歐拉角追蹤，表明機(jī)器人在跟蹤上理想偏航角αd后，便無較大范圍的波動(dòng)。在三維路面的頂點(diǎn)或坑底等角度瞬間變化時(shí)，降維變系數(shù)控制方法能在1 s內(nèi)快速地跟蹤理想的偏航角、滾動(dòng)角和俯仰角。

結(jié)合圖7a與圖7c可以看出，由于起始點(diǎn)位置設(shè)為原點(diǎn)，故而在0～6 s的初始階段x、y、z軸存在較大的誤差。6 s后，隨著時(shí)間的增加，三軸位置誤差逐漸減小，直至誤差在±0.1 m內(nèi)波動(dòng)。

圖7 三維路徑跟蹤仿真結(jié)果

二維平整路面與三維崎嶇路面相比，二維平整路面仿真時(shí)將a4、a5、a6、λ4、λ5、λ6設(shè)置為0，僅改變剩余的控制參數(shù)，記錄機(jī)器人的路徑跟蹤結(jié)果，仿真結(jié)果表明二維路面和三維路面誤差均滿足實(shí)際需要。

4 降維變系數(shù)控制試驗(yàn)

為了驗(yàn)證本文方法的可行性，以安徽農(nóng)業(yè)大學(xué)安徽省智能農(nóng)機(jī)裝備工程實(shí)驗(yàn)室研制的履帶機(jī)器人作為試驗(yàn)平臺(tái)，試驗(yàn)環(huán)境為非硬化崎嶇路面和硬化平整路面兩種，設(shè)計(jì)車速為1.5 m/s。對(duì)理想的二維平整路面與三維崎嶇路面進(jìn)行路徑跟蹤田間試驗(yàn)，如圖8所示。

圖9 二維平整路面路徑跟蹤試驗(yàn)結(jié)果

圖8 田間試驗(yàn)

二維平整路面試驗(yàn)結(jié)果如圖9所示。圖9a中，在理想軌跡與實(shí)際軌跡第2次相交時(shí)可認(rèn)為機(jī)器人由初始波動(dòng)階段進(jìn)入穩(wěn)定跟蹤階段。如圖9b所示，在平面上跟蹤時(shí)，其初始波動(dòng)階段的橫向誤差在-6.2～18.7 cm之間，在路徑點(diǎn)為360時(shí)，機(jī)器人處于穩(wěn)定跟蹤，其誤差在-2.9～8.8 cm內(nèi)波動(dòng)。偏航角跟蹤如圖9c所示，其跟蹤誤差在±2°之間，符合控制要求。

崎嶇路面試驗(yàn)結(jié)果如圖10與圖11所示。其中，圖10a為本文崎嶇路面下行駛軌跡與路譜圖；圖10b與圖10d分別為圖10a在xy平面與z軸方向投影的跟蹤圖。如圖10b與圖10d所示，本文所提出的降維變系數(shù)控制系統(tǒng)能夠較好地追蹤目標(biāo)路徑；在實(shí)際軌跡與理想軌跡第4次相交時(shí)，機(jī)器人由初始波動(dòng)階段進(jìn)入穩(wěn)定跟蹤階段，其跟蹤誤差如圖10c所示，橫向誤差在初始波動(dòng)階段的誤差范圍為-23.6～52.7 cm，在路徑點(diǎn)序號(hào)為381時(shí)，由初始波動(dòng)階段進(jìn)入穩(wěn)定跟蹤階段，機(jī)器人在穩(wěn)定跟蹤階段的誤差范圍為-14.3～21.5 cm，都處于合理的控制范圍內(nèi)。俯仰角、橫滾角與偏航角分別如圖11所示，其控制波動(dòng)皆在±5°以內(nèi)。

圖10 三維崎嶇路面位置跟蹤試驗(yàn)結(jié)果

圖11 三維崎嶇路面角度跟蹤試驗(yàn)結(jié)果

5 結(jié)論

(1)采用數(shù)學(xué)建模方法建立了三維運(yùn)動(dòng)學(xué)模型與誤差方程，基于此設(shè)計(jì)了一種崎嶇山地環(huán)境下的履帶機(jī)器人降維變系數(shù)控制系統(tǒng)。

(2)Matlab環(huán)境下的仿真結(jié)果表明，所提出的控制方法能夠?qū)β膸C(jī)器人進(jìn)行有效的控制，并且能夠?qū)硐肼窂竭M(jìn)行有效的跟蹤。其二維平整路面仿真結(jié)果表明，x軸誤差逐漸減少為0，y軸誤差控制在±0.2 m范圍內(nèi)，航偏角在1 s內(nèi)完成跟蹤理想角度；三維崎嶇路面仿真結(jié)果表明，三軸姿態(tài)角在1 s內(nèi)完成對(duì)理想姿態(tài)角的跟蹤，同時(shí)三軸誤差都控制在±0.1 m內(nèi)。因此，在仿真階段能夠滿足履帶機(jī)器人保持在800 mm寬作物行內(nèi)的行駛要求。

(3)履帶機(jī)器人在二維平整路面上進(jìn)行路徑跟蹤時(shí)，其橫向誤差在初始波動(dòng)階段波動(dòng)范圍在-6.2～18.7 cm之間，機(jī)器人處于穩(wěn)定跟蹤時(shí)，其誤差在-2.9～8.8 cm內(nèi)波動(dòng)，姿態(tài)角在±2°范圍內(nèi)波動(dòng)。在三維崎嶇路面上，采用降維變系數(shù)控制方法能夠?qū)硐肼窂竭M(jìn)行有效跟蹤，機(jī)器人橫向誤差在初始波動(dòng)階段范圍為-23.6～52.7 cm，在穩(wěn)定跟蹤階段范圍為-14.3～21.5 cm，姿態(tài)角控制在±5°范圍內(nèi)。說明履帶機(jī)器人能夠保持在800 mm寬作物行內(nèi)的正常行駛。