差速輪式移動(dòng)機(jī)器人的導(dǎo)航算法研究及仿真實(shí)驗(yàn)

0 引言

輪式移動(dòng)機(jī)器人主要有4種結(jié)構(gòu)形式被廣泛的熟知，即前輪驅(qū)動(dòng)并可轉(zhuǎn)向移動(dòng)機(jī)器人、兩輪差動(dòng)驅(qū)動(dòng)的移動(dòng)機(jī)器人、汽車式移動(dòng)機(jī)器人、帶拖車的輪式移動(dòng)機(jī)器人、三輪全向移動(dòng)機(jī)器人[1]。移動(dòng)機(jī)器人因具有多種機(jī)構(gòu)形式，使得它的應(yīng)用也滲入到各個(gè)領(lǐng)域，包括深海無人潛水器、無人飛行器、服務(wù)機(jī)器人和工業(yè)機(jī)器人等。移動(dòng)機(jī)器人的廣泛應(yīng)用使得人們的生活和生產(chǎn)發(fā)生了很大的變化，提高了生產(chǎn)效率和生活質(zhì)量。因此移動(dòng)機(jī)器人的協(xié)調(diào)和協(xié)作、導(dǎo)航定位、路徑規(guī)劃、控制等問題的研究引起了廣大科研工作者的關(guān)注。其中自主導(dǎo)航技術(shù)是其研究的核心[2,3]，而定位是完成導(dǎo)航首先要解決的問題[4]。

本文主要對(duì)輪式移動(dòng)的差速底盤的機(jī)器人的定位導(dǎo)航問題展開研究，著重分析導(dǎo)航算法中直線導(dǎo)航和圓弧導(dǎo)航，針對(duì)這兩種導(dǎo)航算法只是基于單一的角度變量進(jìn)行糾偏的問題進(jìn)行分析和改進(jìn)，并提出新的改進(jìn)算法，為移動(dòng)機(jī)器人的更深入的研究奠定堅(jiān)實(shí)的基礎(chǔ)。

如圖1所示為移動(dòng)機(jī)器人路徑規(guī)劃導(dǎo)航定位結(jié)構(gòu)圖。

1 輪式機(jī)器人差速定位算法

1.1 差速輪式機(jī)器人的系統(tǒng)介紹

圖1 移動(dòng)機(jī)器人導(dǎo)航結(jié)構(gòu)圖

差速輪式移動(dòng)機(jī)器人，主要依靠后邊的驅(qū)動(dòng)輪驅(qū)動(dòng)前方的輪子進(jìn)行控制，使機(jī)器人按照預(yù)定的軌跡行走。這種結(jié)構(gòu)底盤的機(jī)器人靈活轉(zhuǎn)向，依靠速率和方向的不同，實(shí)現(xiàn)直線行走和轉(zhuǎn)彎的目的[5]。

本系統(tǒng)機(jī)器人采用光纖陀螺與光電碼盤實(shí)現(xiàn)機(jī)器人的定位，使用光纖陀螺的信號(hào)可以讀取相對(duì)于起始點(diǎn)的角度數(shù)據(jù)，這樣就可以通過相對(duì)位置、角度信息解算得到機(jī)器人相對(duì)于起始坐標(biāo)系的坐標(biāo)信息，從而實(shí)現(xiàn)機(jī)器人的定位。

1.2 輪式機(jī)器人差速定位

如圖2所示為系統(tǒng)定位的示意圖，[xyθ]表示機(jī)器人的系統(tǒng)定位信息。

圖2 系統(tǒng)定位示意圖

圖中：x,y為機(jī)器人全局坐標(biāo)系下的坐標(biāo)；θ為機(jī)器人的方位角；Cg，CR為全局、局部坐標(biāo)系。

假設(shè)機(jī)器人由第m個(gè)狀態(tài)到達(dá)第m+1個(gè)狀態(tài)時(shí)，即經(jīng)實(shí)線到達(dá)虛線時(shí)，產(chǎn)生很小的位移量。從圖中可以看出從動(dòng)輪會(huì)行走一段很短的圓弧。假定機(jī)器人左側(cè)編碼器的變化量為L1，右邊的變化量為L2。N表示編碼器的分辨率，r表示從動(dòng)輪半徑，Len為兩邊編碼器從動(dòng)輪的中心輪距[6]。則有以下公式：

式中：dθ表示機(jī)器人旋轉(zhuǎn)的角度，k1=2πr1/N和k2=2πr2/N表示兩側(cè)編碼器的矯正系數(shù)，再由dθ值能夠求出在CR中x,y方向的變化量：

進(jìn)行以下公式的變換，可以將dy，dx，dθ轉(zhuǎn)換為在全局坐標(biāo)系中的角度與位置信息：

其中[xnynθn]表示前一時(shí)刻的定位信息，[xn+1yn+1θn+1]表示當(dāng)前時(shí)刻的位置信息。得到的坐標(biāo)信息和陀螺得到的坐標(biāo)信息做數(shù)據(jù)融合后經(jīng)過累加得到機(jī)器人的實(shí)時(shí)坐標(biāo)信息。

由以上的運(yùn)算可知，這個(gè)過程存在累積誤差，經(jīng)過長時(shí)間的作業(yè)之后，累計(jì)誤差使機(jī)器人的定位信息出現(xiàn)偏差。以下使用累計(jì)誤差修正算法對(duì)誤差進(jìn)行消除[7]。圓形的中心與從動(dòng)輪中心的距離為l，圓的坐標(biāo)為則機(jī)器人的當(dāng)前的位置信息如下公式所示：

實(shí)驗(yàn)表明，通過以上的累計(jì)誤差修正算法的機(jī)器人，在沒有受到嚴(yán)重的破壞的條件下能夠很好的實(shí)現(xiàn)系統(tǒng)的定位信息，文獻(xiàn)[7]也通過具體的實(shí)例驗(yàn)證了這種累計(jì)誤差修正法的有效性和正確性。

2 直線導(dǎo)航算法

將差速底盤機(jī)器人底盤的定位信息通過串口傳送到以ARM和FPGA為控制核心的主機(jī)上，在主機(jī)上采用直線、圓弧導(dǎo)航算法實(shí)現(xiàn)機(jī)器人的導(dǎo)航。通過分析和對(duì)比，發(fā)現(xiàn)了這兩種導(dǎo)航算法中存在的問題，以下分別對(duì)這倆種導(dǎo)航算法進(jìn)行了改進(jìn)。提高了機(jī)器人導(dǎo)航的精度。

2.1 直線導(dǎo)航算法

如圖3所示為機(jī)器人直線導(dǎo)航算法的示意圖，根據(jù)系統(tǒng)的位置與角度來實(shí)現(xiàn)機(jī)器人的反饋。利用目標(biāo)點(diǎn)和編碼器中心點(diǎn)的連線和當(dāng)前機(jī)器人行走方向的夾角作為反饋參數(shù)，使用機(jī)器人與目標(biāo)點(diǎn)的距離作為判斷是否到達(dá)的條件[8]。

圖3 直線導(dǎo)航算法示意圖

如圖3所示，機(jī)器人車頭方向與目標(biāo)點(diǎn)的夾角為α，目標(biāo)點(diǎn)的坐標(biāo)為[disx，disy]，利用下面的公式可以把目標(biāo)點(diǎn)的坐標(biāo)轉(zhuǎn)化為機(jī)器人的局部坐標(biāo)值：

利用得到α值（當(dāng)Xlocal＜0時(shí)取負(fù)號(hào)），由α的值使用PID反饋調(diào)節(jié)，通過下面的公式對(duì)車頭方向進(jìn)行調(diào)整。

式中，PIDresult為調(diào)整量，αn與αn-1為當(dāng)前時(shí)刻和上一時(shí)刻的α值。

2.2 改進(jìn)的直線導(dǎo)航算法

上述的導(dǎo)航算法是針對(duì)角度單一變量進(jìn)行糾偏，越接近終點(diǎn)時(shí)糾偏效果越劇烈，當(dāng)PID參數(shù)很大時(shí)，會(huì)導(dǎo)致很大超調(diào)，因而該算法需要做一定改進(jìn)，設(shè)計(jì)直線導(dǎo)航算法：

圖4 改進(jìn)的直線導(dǎo)航示意圖

在平面坐標(biāo)系中，機(jī)器人位置為（x，y）朝向角度為q。設(shè)定機(jī)器人半徑為R0，目標(biāo)直線為由點(diǎn)（x1，y1）和（x2，y2）確定。有以下關(guān)系：

H為點(diǎn)到直線距離：

又有：

可以求出角度γ，然后對(duì)角度αn進(jìn)行PID控制就行了。

3 圓弧導(dǎo)航

圓弧的導(dǎo)航可以參照?qǐng)D3。只不過目標(biāo)點(diǎn)改為了圓心，我們所要PID的角度變成了機(jī)器人現(xiàn)有角度與圓切線間的夾角。利用下面的公式可以把目標(biāo)點(diǎn)的坐標(biāo)轉(zhuǎn)化為機(jī)器人的局部坐標(biāo)：

之后使用來求出α值。

調(diào)整公式依然是：

3.1 改進(jìn)的圓弧導(dǎo)航算法

由于原有導(dǎo)航算法是針對(duì)角度單一量進(jìn)行糾偏，越接目標(biāo)時(shí)糾偏效果越劇烈，當(dāng)PID參數(shù)很大時(shí)，會(huì)導(dǎo)致很大超調(diào)，因而該算法需要做一定改進(jìn)，設(shè)計(jì)直線導(dǎo)航算法圓弧導(dǎo)航算法，設(shè)計(jì)算法圖示如圖5所示。

圖5 圓弧導(dǎo)航示意圖

首先將目標(biāo)圓心進(jìn)行坐標(biāo)系轉(zhuǎn)化，得到其在機(jī)器人的坐標(biāo)系（機(jī)器人所在位置為原點(diǎn)，機(jī)器人運(yùn)動(dòng)方向?yàn)閤軸方向）內(nèi)的坐標(biāo)（x0，y0）。以直線與目標(biāo)圓交點(diǎn)處切線方向進(jìn)行改進(jìn)的直線導(dǎo)航即可。有：

可以求出角度γ：

對(duì)角度γ進(jìn)行PID之后，對(duì)于軌跡是直線和圓弧相切時(shí)，導(dǎo)航的目標(biāo)點(diǎn)也是一個(gè)連續(xù)變化的點(diǎn)，不會(huì)出現(xiàn)導(dǎo)航的突變，機(jī)器人的運(yùn)動(dòng)會(huì)更平滑精確。

3.2 驅(qū)動(dòng)輪速分配

基于對(duì)多種情況的考慮，可以將速度分配寫成一個(gè)通式，V,V1,V2表示機(jī)器人實(shí)時(shí)的平均、右輪、左輪速度。

式中：R為圓弧半徑（向量），規(guī)定圓心在路徑右側(cè)為正，左側(cè)為負(fù)；L為驅(qū)動(dòng)輪間的距離；U為由偏差產(chǎn)生的控制量；

將U定義為向量，基于不同的情況進(jìn)行考慮，總結(jié)控制量與偏差量符號(hào)變化情況，將其歸納為一個(gè)統(tǒng)一公式。

之后再將偏差E經(jīng)過PID控制，然后使用控制量U分配左右輪的速度從而達(dá)到糾偏的用途。

4 Simulink仿真驗(yàn)證

針對(duì)以上提出的改進(jìn)算法，對(duì)圓弧導(dǎo)航算法的改進(jìn)前后進(jìn)行仿真驗(yàn)證，仿真數(shù)學(xué)模型如式(24)所示：

上述模型能夠表示出機(jī)器人在行進(jìn)時(shí)輪速與偏差量隨時(shí)間變化的關(guān)系，H,為距離與角度偏差初值；H,,V1,V2,V,KH,K如上述介紹；為了讓仿真變得簡單，設(shè)定KH的值，只讓K的值發(fā)生改變，如圖6所示的就是仿真得到的改進(jìn)前后的圓弧導(dǎo)航算法的路徑。

圖中實(shí)線為改進(jìn)后的圓弧導(dǎo)航算法的路徑，虛線改進(jìn)前的圓弧導(dǎo)航算法的路徑，橫縱坐標(biāo)軸的都是以mm計(jì)量的。由圖6知，改進(jìn)前與改進(jìn)后在初始時(shí)基本沒有差異，但是在最后只有大約15mm的誤差。從導(dǎo)航數(shù)據(jù)可以分析出，此偏差主要是由陀螺引起的。由陀螺傳送的與真實(shí)角度存在一個(gè)誤差Δα，機(jī)器人就會(huì)糾偏，Δα主要有以下的兩點(diǎn)產(chǎn)生：

1）由于本文使用的是單軸陀螺，即使旋轉(zhuǎn)一圈是2π，Δα?xí)霈F(xiàn)從0逐步增大，再逐不減小到0的過程。

圖6 改進(jìn)前與改進(jìn)后結(jié)果對(duì)比圖

2）理想中的陀螺的精度可以滿足實(shí)際的情況，但是現(xiàn)實(shí)這一定會(huì)有誤差。

5 結(jié)論

輪式的差速底盤移動(dòng)機(jī)器人的導(dǎo)航算法中的直線和圓弧導(dǎo)航只是針對(duì)單一角度變量進(jìn)行糾偏，且越接近終點(diǎn)時(shí)糾偏效果越劇烈，當(dāng)PID參數(shù)很大時(shí)，會(huì)導(dǎo)致超調(diào)很大，因此對(duì)該算法進(jìn)行了改進(jìn)，提出了新的導(dǎo)航算法。最后建立Simulink模型進(jìn)行仿真。仿真結(jié)果表明新的導(dǎo)航方法取得了良好的效果。

參考文獻(xiàn)：

[1]沈猛.輪式移動(dòng)機(jī)器人導(dǎo)航控制與路徑規(guī)劃研究[D].陜西:西北工業(yè)大學(xué),2006:3-4.

[2]Bashiri M,Vatankhah H, Ghidary S. Hybrid adaptive Differential Evolution for Mobile Robot Localization[J].Intelligent Service Robotics.2012,5(2):99-107.

[3]Cheng P Y,Chen P J.Navigation of mobile robot by using D+algorithm[J].Intelligent Service Robotics.2012,5(2):229-243.

[4]貝旭穎,平雪良,高文研.輪式移動(dòng)機(jī)器人里程計(jì)系統(tǒng)誤差校核[J].計(jì)算機(jī)應(yīng)用與研究,2017,35(9).

[5]亢海龍,徐婭萍,王濤,等.基于卡爾曼濾波器的輪式機(jī)器人定位方法[J].機(jī)械與電子,2009,72-74.

[6]王鳴.機(jī)器人實(shí)時(shí)定位與導(dǎo)航系統(tǒng)仿真設(shè)計(jì)研究[J].計(jì)算機(jī)仿真,2012,12(03):216-218.

[7]李毅,吳焱岷,宏英.機(jī)器人實(shí)時(shí)定位與導(dǎo)航系統(tǒng)的設(shè)計(jì)研究[J].制造業(yè)自動(dòng)化2012,34(4):132-134.

[8]朱磊磊,陳軍.輪式移動(dòng)機(jī)器人研究綜述[J].機(jī)床與液壓,2009,37(8):242-247.