尚 帥，戴興安，王錦博，宋 偉，盛守照

(1.南京航空航天大學(xué)自動(dòng)化學(xué)院,江蘇 南京 210016；2.中國(guó)航天科工南京晨光集團(tuán)，江蘇 南京 210006)

0 引言

隨著工業(yè)自動(dòng)化的發(fā)展，無(wú)人機(jī)器人在工業(yè)制造、農(nóng)業(yè)種植、牧業(yè)養(yǎng)殖等場(chǎng)景應(yīng)用得越來(lái)越廣泛。如今為了提高畜牧業(yè)的生產(chǎn)效率，房舍集體飼養(yǎng)取代了以往的自然牧養(yǎng)。以奶牛為例主要生活在牛舍中，滯留在牛舍地面上的污物成為奶牛和人類(lèi)的潛在致病源，從而極大地影響人類(lèi)生命財(cái)產(chǎn)安全及奶牛健康和產(chǎn)奶量。為了適應(yīng)多變的應(yīng)用場(chǎng)景，移動(dòng)機(jī)器人導(dǎo)航控制技術(shù)顯得尤為主要。

目前，移動(dòng)機(jī)器人導(dǎo)航的主要方法有磁導(dǎo)航、光學(xué)導(dǎo)航、激光導(dǎo)航和慣性導(dǎo)航等。許多學(xué)者也對(duì)此進(jìn)行了深入的研究，季冉鳴等[1]為解決無(wú)人機(jī)器人導(dǎo)航問(wèn)題，提出了基于磁釘及慣導(dǎo)系統(tǒng)的機(jī)器人導(dǎo)航控制算法；李鳳娥等[2]為解決自動(dòng)引導(dǎo)車(chē)的定位精度，系統(tǒng)地分析了磁釘導(dǎo)航產(chǎn)生誤差的原因及解決方法；李毅等[3]為實(shí)現(xiàn)機(jī)器人精確定位，設(shè)計(jì)了基于機(jī)器人模型的定位導(dǎo)航系統(tǒng)。

基于此，本文以牛舍自動(dòng)清潔機(jī)器人為工程研究背景，采用一種改進(jìn)的機(jī)器人定位算法模型與磁釘導(dǎo)航技術(shù)和純跟蹤算法相結(jié)合，希望能夠有效地解決機(jī)器人導(dǎo)航過(guò)程中的定位、累計(jì)誤差等問(wèn)題，使無(wú)人機(jī)器人導(dǎo)航在工程實(shí)踐中易于實(shí)現(xiàn)。

1 機(jī)器人定位算法模型

本文中的清潔機(jī)器人采用三輪結(jié)構(gòu)，前面1個(gè)輪子為萬(wàn)向輪，后面2個(gè)輪為驅(qū)動(dòng)輪，采用差速轉(zhuǎn)向方式。清潔機(jī)器人定位算法模型如圖1所示，定義XOY為清潔機(jī)器人全局坐標(biāo)系CG，定義XaOaYa為清潔機(jī)器人局部坐標(biāo)系CR，機(jī)器人的實(shí)時(shí)定位信息用[x,y,ψ]來(lái)表示，其中x和y分別表示清潔機(jī)器人在全局坐標(biāo)系CG下的橫、縱坐標(biāo)，ψ表示機(jī)器人當(dāng)前的航向角。

圖1 清潔機(jī)器人定位算法模型

(1)

vl=k·Ml，vr=k·Mr。L為機(jī)器人2個(gè)后驅(qū)動(dòng)輪的中心輪距。清潔機(jī)器人在局部坐標(biāo)系CR中的x和y方向的變化量可通過(guò)前面計(jì)算出的dψ值推算出來(lái)：

(2)

(3)

機(jī)器人在局部坐標(biāo)系中變化量分別為dy、dx、dψ，現(xiàn)把這些局部坐標(biāo)系中的變化量轉(zhuǎn)換為全局坐標(biāo)系CG中的位置和角度信息[5]，需要進(jìn)行如下變換：

ψn+1=ψn+dψ

(4)

xn+1=xn+(dx×cosψ-dysinψ)

(5)

yn+1=yn+(dx×sinψ+dycosψ)

(6)

[xn+1，yn+1，ψn+1]表示清潔機(jī)器人當(dāng)前時(shí)刻的位置和角度信息，它可以由前一時(shí)刻的[xn，yn，ψn]定位信息推算，這樣根據(jù)前一時(shí)刻的定位信息不斷地累加就能夠得到清潔機(jī)器人在清潔牛舍排泄物過(guò)程中的實(shí)時(shí)定位信息。

由于整個(gè)算法的定位信息都是根據(jù)編碼器的值作為原始值來(lái)計(jì)算得到，即使選擇分辨率大的編碼器，長(zhǎng)時(shí)間工作之后，累計(jì)誤差還是存在的，當(dāng)累計(jì)誤差足夠大時(shí)會(huì)導(dǎo)致清潔機(jī)器人無(wú)法正常定位清潔牛舍。為了保證清潔機(jī)器人能夠長(zhǎng)時(shí)間工作保證牛舍的清潔，本文采用磁釘導(dǎo)航修正算法來(lái)修正模型位置的累計(jì)誤差。

2 清潔機(jī)器人導(dǎo)航控制算法

2.1 磁釘傳感器位置修正算法

由于系統(tǒng)的所有定位信息都是由編碼器的值來(lái)確定的，雖然對(duì)編碼器的參數(shù)進(jìn)行了矯正，但是誤差仍然是存在的，在長(zhǎng)時(shí)間的工作之后，累計(jì)誤差會(huì)導(dǎo)致機(jī)器人無(wú)法正常定位[6]。為了對(duì)牛舍清潔機(jī)器人的航向、位置量測(cè)值的累計(jì)誤差進(jìn)行修正，采用在設(shè)定的移動(dòng)路徑上每隔一段距離鋪設(shè)1個(gè)磁釘?shù)姆椒?，由安裝在機(jī)器人上的磁傳感器進(jìn)行檢測(cè)。為了降低后輪驅(qū)動(dòng)電機(jī)對(duì)此磁傳感器的影響，將磁傳感器安裝在機(jī)器人前端清潔鏟的中間位置，垂直于車(chē)體軸線(xiàn)，磁傳感器的中點(diǎn)位于機(jī)器人的中軸線(xiàn)上。

選用一款具有16個(gè)霍爾檢測(cè)點(diǎn)的磁傳感器[7]，檢測(cè)點(diǎn)的間距為l=10 mm，朝清潔機(jī)器人前方看去，檢測(cè)點(diǎn)從左到右的編號(hào)依次為1～16 。這款磁傳感器的檢測(cè)精度為(30±10) mm范圍內(nèi)N、S磁極，當(dāng)傳感器有m個(gè)連續(xù)點(diǎn)有輸出時(shí)，設(shè)檢測(cè)到的最左邊點(diǎn)的編號(hào)為n，則軌道偏離中心線(xiàn)的偏差d的計(jì)算公式為

(7)

該算法的示意如圖2所示。根據(jù)式(7)的計(jì)算結(jié)果可知：當(dāng)磁釘位置偏向車(chē)體右側(cè)的時(shí)候d為正，偏向左側(cè)的時(shí)候d為負(fù)。

圖2 磁釘導(dǎo)航側(cè)偏距計(jì)算方法示意

在設(shè)定的清潔路徑上每一個(gè)磁釘?shù)娜肿鴺?biāo)都可根據(jù)設(shè)定的磁釘間距確定，假設(shè)機(jī)器人當(dāng)前狀態(tài)為n，清潔機(jī)器人磁傳感器檢測(cè)到磁釘A時(shí)，可根據(jù)式(7)計(jì)算得到的車(chē)輛偏差d、磁傳感器中心至雙后輪軸中心軸線(xiàn)距離Lw，機(jī)器人航向ψn和磁釘A的坐標(biāo)[xA,yA]對(duì)當(dāng)前車(chē)輛位置進(jìn)行修正，并得到當(dāng)前車(chē)輛實(shí)際的位置坐標(biāo)：

xn=xA-Lw×cosψn+d×sinψn

(8)

yn=yA-Lw×sinψn+d×cosψn

(9)

2.2 純跟蹤算法

全局的規(guī)劃路徑由一系列的路徑點(diǎn)構(gòu)成，這些路徑點(diǎn)必須包含空間位置信息，也可以包含姿態(tài)信息，但不需要與時(shí)間有關(guān)，這些路徑點(diǎn)被稱(chēng)為全局路徑點(diǎn)。

純跟蹤算法是一種利用幾何關(guān)系確定曲率的方法，該曲率將驅(qū)動(dòng)車(chē)輛到達(dá)選定的路徑點(diǎn)。牛舍清潔機(jī)器人的清潔路徑比較固定，根據(jù)磁釘?shù)匿佋O(shè)軌跡進(jìn)行規(guī)劃。由2.1節(jié)可知，磁釘?shù)奈恢貌蛔?，有固定的空間位置坐標(biāo)，所以在此可以把磁釘作為全局路徑點(diǎn)來(lái)進(jìn)行清潔機(jī)器人導(dǎo)航算法研究。

因?yàn)樵趯?shí)際工程應(yīng)用中，為了節(jié)約成本和便于安裝，磁釘?shù)匿佋O(shè)密度不會(huì)很大，正常情況下2～3 m鋪設(shè)1個(gè)磁釘，若該算法僅僅以磁釘作為全局路徑點(diǎn)，由于磁釘鋪設(shè)密度低，會(huì)導(dǎo)致控制效果較差，不能達(dá)到精度要求[8]。所以,在此提出一種虛擬路徑點(diǎn)的方法來(lái)保證在不增大磁釘鋪設(shè)密度的基礎(chǔ)上保證控制精度。由于磁釘?shù)淖鴺?biāo)都已知，所以能夠每2個(gè)磁釘之間等間距取一些點(diǎn)的坐標(biāo)作為虛擬路徑點(diǎn)，其原理如圖3所示，把這些虛擬路徑點(diǎn)和磁釘共同作為全局路徑點(diǎn)使用。

圖3 全局路徑點(diǎn)分布示意

從雙輪差速機(jī)器人模型出發(fā)，純跟蹤算法以機(jī)器人后軸為切入點(diǎn)，機(jī)器人的縱向車(chē)身為切線(xiàn)，通過(guò)控制后輪差速控制機(jī)器人轉(zhuǎn)向，使得清潔機(jī)器人沿著一條可以經(jīng)過(guò)全局路徑點(diǎn)的圓弧行駛，其原理如圖4所示。

圖4 清潔機(jī)器人純跟蹤算法原理

假設(shè)圖中的磁釘A[xA,yA]為下一個(gè)要追蹤的路徑點(diǎn)，它位于已經(jīng)規(guī)劃好的全局清潔路徑上，現(xiàn)在需要控制機(jī)器人的后軸經(jīng)過(guò)該路徑點(diǎn)，ld為機(jī)器人的當(dāng)前位置到目標(biāo)路徑點(diǎn)的距離即純跟蹤的前視距離，α為車(chē)身姿態(tài)到目標(biāo)路徑點(diǎn)的夾角，根據(jù)正弦定理可以推導(dǎo)出機(jī)器人運(yùn)動(dòng)所需的曲率γ。

(10)

(11)

(12)

其中，α可根據(jù)磁釘A坐標(biāo)[xA,yA]與機(jī)器人當(dāng)前位置坐標(biāo)[xn,yn]計(jì)算得到，為

(13)

前視距離ld與機(jī)器人當(dāng)前的線(xiàn)速度vn有關(guān)，在這里設(shè)置1個(gè)常量前視距離Lfc和1個(gè)視距系數(shù)Kd，則視距與速度的關(guān)系式為

ld=Kdvn+Lfc

(14)

雙輪差速結(jié)構(gòu)的車(chē)輛的速度vn與左、右輪的速度vl、vr有關(guān)，為

(15)

由式(12)得到的曲率，由式(15)得到的速度vn，和式(1)得到的角速度dψ，可以推算出當(dāng)前的左、右輪轉(zhuǎn)速分別為：

(16)

(17)

利用增量PID的方式對(duì)左右車(chē)輪轉(zhuǎn)速進(jìn)行控制[9]，控制值公式為

vn=P(en-en-1)+I·en+D(en-2en-1+en-2)

(18)

P為比例系數(shù)；I為積分系數(shù)；D為微分系數(shù)；en為當(dāng)前轉(zhuǎn)速誤差；en-1為上一次減速誤差；en-2為上上次轉(zhuǎn)速誤差。

根據(jù)計(jì)算得到的左右輪控制量驅(qū)動(dòng)左、右2輪進(jìn)行雙輪差速控制，從而達(dá)到清潔機(jī)器人根據(jù)目標(biāo)路徑進(jìn)行跟蹤作業(yè)的效果。

3 機(jī)器人導(dǎo)航系統(tǒng)驗(yàn)證

3.1 模型驗(yàn)證實(shí)驗(yàn)

為驗(yàn)證清潔機(jī)器人模型準(zhǔn)確度，如圖5所示進(jìn)行實(shí)物平臺(tái)測(cè)試驗(yàn)證。

圖5 清潔機(jī)器人實(shí)物驗(yàn)證

磁釘沿全局坐標(biāo)系y軸方向排列，清潔機(jī)器人沿y軸方向走直線(xiàn)，設(shè)定磁釘間距分別為1 300.000 mm、2 000.000 mm，當(dāng)經(jīng)過(guò)1個(gè)磁釘，對(duì)y軸數(shù)據(jù)清零，將模型計(jì)算出的y軸值與實(shí)際磁釘間距做比較，機(jī)器人模型y軸驗(yàn)證數(shù)據(jù)如表1所示。

表1 機(jī)器人模型y軸驗(yàn)證數(shù)據(jù)

清潔機(jī)器人沿y軸方向做180.000°轉(zhuǎn)彎，由一個(gè)磁釘轉(zhuǎn)向另一個(gè)x方向平行間距為1 400.000 mm的磁釘，將模型計(jì)算出的轉(zhuǎn)彎弧度及x軸行駛距離與實(shí)際的轉(zhuǎn)彎弧度及x軸行駛距離做比較，機(jī)器人模型轉(zhuǎn)彎弧度和x軸驗(yàn)證數(shù)據(jù)如表2所示。

表2 機(jī)器人模型轉(zhuǎn)彎弧度和x軸驗(yàn)證數(shù)據(jù)

由表1和表2可以看出，由模型計(jì)算得到的航向ψ變化量、x軸方向變化量、y軸方向變化量與實(shí)際測(cè)量參數(shù)對(duì)比結(jié)果顯示，模型計(jì)算誤差較小，而且數(shù)據(jù)穩(wěn)定，模型比較可靠。

3.2 仿真實(shí)驗(yàn)

實(shí)驗(yàn)對(duì)象為雙輪差速式牛舍清潔機(jī)器人。本算法利用編碼器、磁傳感器和機(jī)器人車(chē)輛參數(shù)進(jìn)行導(dǎo)航控制，清潔機(jī)器人的路徑有的是直線(xiàn)有的是曲線(xiàn)，根據(jù)2種不同工作路徑分別仿真，直線(xiàn)跟蹤仿真如圖6所示，曲線(xiàn)跟蹤仿真如圖7所示。

圖6 清潔機(jī)器人直線(xiàn)跟蹤仿真

圖7 清潔機(jī)器人曲線(xiàn)跟蹤仿真

圖6和圖7中，圓點(diǎn)表示為磁釘，實(shí)線(xiàn)為機(jī)器人行駛過(guò)的路徑，三角標(biāo)為清潔機(jī)器人當(dāng)前的目標(biāo)路徑點(diǎn)。

根據(jù)仿真結(jié)果可知，本文提出的純跟蹤磁釘修正算法在直線(xiàn)段的跟蹤效果較好，在轉(zhuǎn)彎的時(shí)候會(huì)存在一定的超調(diào)，但也能在短時(shí)間內(nèi)回到預(yù)定軌跡，最終都能實(shí)現(xiàn)按照預(yù)定軌跡進(jìn)行作業(yè)。

4 結(jié)束語(yǔ)

本文針對(duì)牛舍自動(dòng)清潔機(jī)器人導(dǎo)航系統(tǒng)展開(kāi)研究，首先，建立了機(jī)器人定位模型并對(duì)其進(jìn)行了分析，然后在機(jī)器人模型的基礎(chǔ)上，對(duì)磁釘導(dǎo)航策略進(jìn)行研究，并引入純跟蹤(Pure Pursuit)算法完成目標(biāo)跟蹤及修正。最后，先將該清潔機(jī)器人在實(shí)物平臺(tái)上進(jìn)行模型驗(yàn)證，驗(yàn)證結(jié)果顯示，與實(shí)際測(cè)量參數(shù)相比較，該模型計(jì)算誤差較小，數(shù)據(jù)穩(wěn)定，模型比較可靠；再對(duì)該模型進(jìn)行目標(biāo)路徑跟蹤仿真實(shí)驗(yàn)，仿真結(jié)果表明，所提出的純跟蹤磁釘修正算法能夠?qū)崿F(xiàn)按照預(yù)訂軌跡進(jìn)行作業(yè)，驗(yàn)證了該導(dǎo)航系統(tǒng)能引導(dǎo)機(jī)器人進(jìn)行目標(biāo)路徑跟蹤行駛，可以完成牛舍清潔工作。