差速輪式移動機器人的導航算法研究及仿真實驗

0 引言

輪式移動機器人主要有4種結構形式被廣泛的熟知，即前輪驅動并可轉向移動機器人、兩輪差動驅動的移動機器人、汽車式移動機器人、帶拖車的輪式移動機器人、三輪全向移動機器人[1]。移動機器人因具有多種機構形式，使得它的應用也滲入到各個領域，包括深海無人潛水器、無人飛行器、服務機器人和工業機器人等。移動機器人的廣泛應用使得人們的生活和生產發生了很大的變化，提高了生產效率和生活質量。因此移動機器人的協調和協作、導航定位、路徑規劃、控制等問題的研究引起了廣大科研工作者的關注。其中自主導航技術是其研究的核心[2,3]，而定位是完成導航首先要解決的問題[4]。

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

如圖1所示為移動機器人路徑規劃導航定位結構圖。

1 輪式機器人差速定位算法

1.1 差速輪式機器人的系統介紹

圖1 移動機器人導航結構圖

差速輪式移動機器人，主要依靠后邊的驅動輪驅動前方的輪子進行控制，使機器人按照預定的軌跡行走。這種結構底盤的機器人靈活轉向，依靠速率和方向的不同，實現直線行走和轉彎的目的[5]。

本系統機器人采用光纖陀螺與光電碼盤實現機器人的定位，使用光纖陀螺的信號可以讀取相對于起始點的角度數據，這樣就可以通過相對位置、角度信息解算得到機器人相對于起始坐標系的坐標信息，從而實現機器人的定位。

1.2 輪式機器人差速定位

如圖2所示為系統定位的示意圖，[xyθ]表示機器人的系統定位信息。

圖2 系統定位示意圖

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

假設機器人由第m個狀態到達第m+1個狀態時，即經實線到達虛線時，產生很小的位移量。從圖中可以看出從動輪會行走一段很短的圓弧。假定機器人左側編碼器的變化量為L1，右邊的變化量為L2。N表示編碼器的分辨率，r表示從動輪半徑，Len為兩邊編碼器從動輪的中心輪距[6]。則有以下公式：

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

進行以下公式的變換，可以將dy，dx，dθ轉換為在全局坐標系中的角度與位置信息：

其中[xnynθn]表示前一時刻的定位信息，[xn+1yn+1θn+1]表示當前時刻的位置信息。得到的坐標信息和陀螺得到的坐標信息做數據融合后經過累加得到機器人的實時坐標信息。

由以上的運算可知，這個過程存在累積誤差，經過長時間的作業之后，累計誤差使機器人的定位信息出現偏差。以下使用累計誤差修正算法對誤差進行消除[7]。圓形的中心與從動輪中心的距離為l，圓的坐標為則機器人的當前的位置信息如下公式所示：

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

2 直線導航算法

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

2.1 直線導航算法

如圖3所示為機器人直線導航算法的示意圖，根據系統的位置與角度來實現機器人的反饋。利用目標點和編碼器中心點的連線和當前機器人行走方向的夾角作為反饋參數，使用機器人與目標點的距離作為判斷是否到達的條件[8]。

圖3 直線導航算法示意圖

如圖3所示，機器人車頭方向與目標點的夾角為α，目標點的坐標為[disx，disy]，利用下面的公式可以把目標點的坐標轉化為機器人的局部坐標值：

利用得到α值（當Xlocal＜0時取負號），由α的值使用PID反饋調節，通過下面的公式對車頭方向進行調整。

式中，PIDresult為調整量，αn與αn-1為當前時刻和上一時刻的α值。

2.2 改進的直線導航算法

上述的導航算法是針對角度單一變量進行糾偏，越接近終點時糾偏效果越劇烈，當PID參數很大時，會導致很大超調，因而該算法需要做一定改進，設計直線導航算法：

圖4 改進的直線導航示意圖

在平面坐標系中，機器人位置為（x，y）朝向角度為q。設定機器人半徑為R0，目標直線為由點（x1，y1）和（x2，y2）確定。有以下關系：

H為點到直線距離：

又有：

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

3 圓弧導航

圓弧的導航可以參照圖3。只不過目標點改為了圓心，我們所要PID的角度變成了機器人現有角度與圓切線間的夾角。利用下面的公式可以把目標點的坐標轉化為機器人的局部坐標：

之后使用來求出α值。

調整公式依然是：

3.1 改進的圓弧導航算法

由于原有導航算法是針對角度單一量進行糾偏，越接目標時糾偏效果越劇烈，當PID參數很大時，會導致很大超調，因而該算法需要做一定改進，設計直線導航算法圓弧導航算法，設計算法圖示如圖5所示。

圖5 圓弧導航示意圖

首先將目標圓心進行坐標系轉化，得到其在機器人的坐標系（機器人所在位置為原點，機器人運動方向為x軸方向）內的坐標（x0，y0）。以直線與目標圓交點處切線方向進行改進的直線導航即可。有：

可以求出角度γ：

對角度γ進行PID之后，對于軌跡是直線和圓弧相切時，導航的目標點也是一個連續變化的點，不會出現導航的突變，機器人的運動會更平滑精確。

3.2 驅動輪速分配

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

式中：R為圓弧半徑（向量），規定圓心在路徑右側為正，左側為負；L為驅動輪間的距離；U為由偏差產生的控制量；

將U定義為向量，基于不同的情況進行考慮，總結控制量與偏差量符號變化情況，將其歸納為一個統一公式。

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

4 Simulink仿真驗證

針對以上提出的改進算法，對圓弧導航算法的改進前后進行仿真驗證，仿真數學模型如式(24)所示：

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

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

1）由于本文使用的是單軸陀螺，即使旋轉一圈是2π，Δα會出現從0逐步增大，再逐不減小到0的過程。

圖6 改進前與改進后結果對比圖

2）理想中的陀螺的精度可以滿足實際的情況，但是現實這一定會有誤差。

5 結論

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

參考文獻：

[1]沈猛.輪式移動機器人導航控制與路徑規劃研究[D].陜西:西北工業大學,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]貝旭穎,平雪良,高文研.輪式移動機器人里程計系統誤差校核[J].計算機應用與研究,2017,35(9).

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

[6]王鳴.機器人實時定位與導航系統仿真設計研究[J].計算機仿真,2012,12(03):216-218.

[7]李毅,吳焱岷,宏英.機器人實時定位與導航系統的設計研究[J].制造業自動化2012,34(4):132-134.

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