基于磁釘陣列的牛舍清潔機器人導航系統(tǒng)研究

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

(1.南京航空航天大學自動化學院,江蘇 南京 210016；2.中國航天科工南京晨光集團，江蘇 南京 210006)

0 引言

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

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

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

1 機器人定位算法模型

本文中的清潔機器人采用三輪結構，前面1個輪子為萬向輪，后面2個輪為驅動輪，采用差速轉向方式。清潔機器人定位算法模型如圖1所示，定義XOY為清潔機器人全局坐標系CG，定義XaOaYa為清潔機器人局部坐標系CR，機器人的實時定位信息用[x,y,ψ]來表示，其中x和y分別表示清潔機器人在全局坐標系CG下的橫、縱坐標，ψ表示機器人當前的航向角。

圖1 清潔機器人定位算法模型

(1)

vl=k·Ml，vr=k·Mr。L為機器人2個后驅動輪的中心輪距。清潔機器人在局部坐標系CR中的x和y方向的變化量可通過前面計算出的dψ值推算出來：

(2)

(3)

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

ψ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]表示清潔機器人當前時刻的位置和角度信息，它可以由前一時刻的[xn，yn，ψn]定位信息推算，這樣根據(jù)前一時刻的定位信息不斷地累加就能夠得到清潔機器人在清潔牛舍排泄物過程中的實時定位信息。

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

2 清潔機器人導航控制算法

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

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

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

(7)

該算法的示意如圖2所示。根據(jù)式(7)的計算結果可知：當磁釘位置偏向車體右側的時候d為正，偏向左側的時候d為負。

圖2 磁釘導航側偏距計算方法示意

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

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

(8)

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

(9)

2.2 純跟蹤算法

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

純跟蹤算法是一種利用幾何關系確定曲率的方法，該曲率將驅動車輛到達選定的路徑點。牛舍清潔機器人的清潔路徑比較固定，根據(jù)磁釘?shù)匿佋O軌跡進行規(guī)劃。由2.1節(jié)可知，磁釘?shù)奈恢貌蛔儯泄潭ǖ目臻g位置坐標，所以在此可以把磁釘作為全局路徑點來進行清潔機器人導航算法研究。

因為在實際工程應用中，為了節(jié)約成本和便于安裝，磁釘?shù)匿佋O密度不會很大，正常情況下2～3 m鋪設1個磁釘，若該算法僅僅以磁釘作為全局路徑點，由于磁釘鋪設密度低，會導致控制效果較差，不能達到精度要求[8]。所以,在此提出一種虛擬路徑點的方法來保證在不增大磁釘鋪設密度的基礎上保證控制精度。由于磁釘?shù)淖鴺硕家阎阅軌蛎?個磁釘之間等間距取一些點的坐標作為虛擬路徑點，其原理如圖3所示，把這些虛擬路徑點和磁釘共同作為全局路徑點使用。

圖3 全局路徑點分布示意

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

圖4 清潔機器人純跟蹤算法原理

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

(10)

(11)

(12)

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

(13)

前視距離ld與機器人當前的線速度vn有關，在這里設置1個常量前視距離Lfc和1個視距系數(shù)Kd，則視距與速度的關系式為

ld=Kdvn+Lfc

(14)

雙輪差速結構的車輛的速度vn與左、右輪的速度vl、vr有關，為

(15)

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

(16)

(17)

利用增量PID的方式對左右車輪轉速進行控制[9]，控制值公式為

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

(18)

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

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

3 機器人導航系統(tǒng)驗證

3.1 模型驗證實驗

為驗證清潔機器人模型準確度，如圖5所示進行實物平臺測試驗證。

圖5 清潔機器人實物驗證

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

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

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

表2 機器人模型轉彎弧度和x軸驗證數(shù)據(jù)

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

3.2 仿真實驗

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

圖6 清潔機器人直線跟蹤仿真

圖7 清潔機器人曲線跟蹤仿真

圖6和圖7中，圓點表示為磁釘，實線為機器人行駛過的路徑，三角標為清潔機器人當前的目標路徑點。

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

4 結束語

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