輪式移動機器人路徑跟蹤控制方法研究

肖獻強，程亞兵，王家恩，王鼎用

（1.合肥工業大學機械工程學院，安徽 合肥 230009；2.合肥工業大學 汽車與交通工程學院，安徽 合肥 230009）

1 引言

隨著工業制造自動化和智能化水平的不斷提高，移動機器人已經成為工業智能制造領域不可或缺的智能化設備，以取代傳統工業制造中繁重的人工勞動[1-3]。移動機器人的路徑規劃和路徑跟蹤是實現自主移動的基礎[4-5]。路徑跟蹤控制是通過控制模型及方法使移動機器人不斷逼近并最終到達規劃的目標位置。路徑跟蹤控制作為移動機器人研究的一個關鍵技術，是在滿足一定的安全性和穩定性的前提下，通過控制移動機器人的速度和移動方向，不斷縮小運動過程中的偏差，最終實現路徑跟蹤。近年來，許多學者對路徑跟蹤控制方法進行了深入的研究[6-10]。早期研究人員在經典PID 控制的基礎上，提出了前饋加PID 反饋的非線性PID 控制的方法[6-7]，為了提高PID 控制的魯棒性，具有學習功能的自適應[8]、預瞄控制[9]、模糊控制[10]等優化PID 算法被提出。PID控制雖能解決移動機器人的路徑跟蹤問題，但是PID 控制參數的獲取需要通過大量標定，難以實現最優控制。針對以上問題，同時為了減少道路曲率、預瞄距離等對路徑跟蹤控制精度的影響，提出一種基于動態預瞄的移動機器人路徑跟蹤控制方法，根據目標點坐標與方向角度求解路徑跟蹤直線方程，并在該直線上選取合適的預瞄點，分析移動機器人當前位置與預瞄點在局部坐標系下的位置關系，建立路徑跟蹤偏差模型，結合路徑跟蹤偏差模型在移動機器人與預瞄點之間實時動態規劃虛擬路徑，從而獲取由當前位置到達目標點的期望橫擺角速度，最終實現精確的路徑跟蹤控制。

2 路徑跟蹤控制方法

移動機器人的循跡控制主要包括路徑規劃和路徑跟蹤控制兩個方面內容。此處研究的重點為路徑跟蹤控制方法，即在通過路徑規劃方法獲得期望運動路徑后，控制移動機器人實現對該期望運動路徑的跟蹤。

移動機器人控制系統由地圖構建與定位、路徑規劃、路徑跟蹤以及底層控制四個模塊組成。地圖構建與定位模塊構建移動機器人運行的全局坐標系，并完成實時定位；根據構建的地圖進行路徑規劃，獲取期望運動路徑；綜合移動機器人的實時位置信息以及期望運動路徑，進行路徑跟蹤控制，計算在當前位置跟蹤期望運動路徑所需的期望橫擺角速度，將期望橫擺角速度轉換為電機脈沖輸出，最終實現移動機器人的路徑跟蹤控制。

2.1 差速輪式移動機器人運動學模型

假定在理想條件下差速輪式移動機器人在運動過程中的位姿可表示為P=［x，y，φ］T，其中x 表示移動機器人質心相對于絕對坐標系原點X 軸方向的距離，y 表示相對于絕對坐標系原點Y 軸方向的距離，φ 表示左右輪中垂線與Y 軸的夾角。

圖1 運動學模型示意圖Fig.1 Schematic Diagram of Kinematic Model

差速輪式移動機器人運動學模型，如圖1 所示。右輪速度vr，左輪速度vl，移動機器人在移動過程中的運動狀態矩陣［vx，vy，ω］T，如式（1）所示。

式中：vx、vy—速度在X 軸和Y 軸方向上的分量；ω—橫擺角速度；D—兩輪間距。

規定初始位姿 P=［x，y，φ］T=［x0，y0，φ0］T，得到任意時刻位姿矩陣與左右輪速度之間的變換式（2）。

2.2 路徑跟蹤偏差模型

移動機器人在路徑跟蹤控制中的偏差模型包括四個部分：路徑跟蹤直線方程求解、預備目標點計算、預瞄點選取以及偏差計算。

移動機器人由當前點到達目標點，當前點用帶方向的藍色圓點表示，目標點用帶方向的紅色表示，黃色圓點表示預備目標點，綠色圓點表示預瞄點，如圖2 所示。根據目標點坐標及方向角度求解路徑跟蹤直線方程，并根據預備目標點到目標點距離Δl和預瞄距離Δd 確定路徑跟蹤預備目標點和預瞄點。最后計算移動機器人當前位置相對于預瞄點的縱向、側向以及方向偏差。

圖2 路徑跟蹤偏差模型Fig.2 Deviation Model of Path Tracking

結合目標點坐標及方向角度（xt，yt，φt）解得路徑跟蹤直線方程，以目標點為圓心Δl 為半徑畫圓弧，圓弧與路徑跟蹤直線存在兩個交點，如圖2 所示。根據目標點的方向角度選取的黃色圓點作為預備目標點（x′t，y′t，φ′t），預備目標點的坐標（x′t，y′t）可根據式（3）進行求解，且有，φ′t=φt。

式中：a、b、c—直線方程的參數；移動機器人在路徑跟蹤控制過程中始終以當前位置為圓心Δd 為半徑畫圓弧，如圖2 中所示綠色圓弧。當所畫圓弧與路徑跟蹤直線相交（交點坐標的計算可參考式（3）），且交點與目標點間距離小于Δl 時，選取該交點作為預瞄點，否則始終以預備目標點作為路徑跟蹤控制的預瞄點（xp，yp，φp）。

移動機器人當前位置相對于預瞄點的偏差即為預瞄點在以當前位置為原點的局部坐標系下的坐標，如式（4）所示。

2.3 路徑跟蹤控制方法設計

在OcXcYc局部坐標系中移動機器人質心與預瞄點之間實時構造虛擬路徑[9]，如圖3 所示。

圖3 路徑跟蹤控制方法示意圖Fig.3 Schematic Diagram of Path Tracking Control Method

假設該虛擬路徑方程，如式（5）所示。

式中：A、B、C、D、E、F—虛擬路徑方程的參數。

局部坐標系OcXcYc是以移動機器人當前位置為原點，當前方向為X 軸正方向建立的，如圖3 所示。在局部坐標系下虛擬路徑的構造條件為：起始點坐標為（0，0），斜率為 0，曲率為 ρ=ω/v；預瞄點坐標為（xe，ye），斜率為tanφe，曲率為0。根據構造條件即可求解得虛擬路徑方程為：

已知虛擬路徑的曲線方程為y（x），移動機器人質心在曲線上的坐標為（x，y），速度為v，速度方向及運動曲率與移動機器人當前所在曲線上點一致。定義ωd為運動軌跡為曲線方程時的理想橫擺角速度，其計算公式，如式（7）所示。則理想橫擺角速度變化率的計算公式，如式（8）所示。

期望橫擺角速度ωc為當前橫擺角速度加上理想橫擺變化率與采樣時間的乘積，如式（10）所示。

3 實驗驗證

3.1 實驗平臺介紹

實驗平臺采用自主設計的差速輪式移動機器人，其基本參數，如表1 所示。

表1 差速輪式移動機器人基本參數Tab.1 Basic Parameters of Differential Wheeled Mobile Robots

3.2 實驗數據分析

移動機器人的實驗運動路徑為8 字形，移動機器人需要進行連續的圓弧轉向以充分驗證所設計的移動機器人路徑跟蹤控制方法的可靠性、穩定性和精確性，以左下角站點為起始點，運動軌跡為①-②-③-④最終到達終點，如圖4 所示。

移動機器人實驗過程中的實際運動軌跡曲線圖，如圖5 所示。從軌跡曲線圖中可以看出移動機器人能很好的跟蹤規劃的運動軌跡。對比圖4 與圖5，實際運動軌跡與期望運動軌跡非常接近，表明了設計的路徑跟蹤控制方法的精確性和可靠性。

圖4 期望運動軌跡曲線Fig.4 Desired Trajectory Curve

圖5 實際運動軌跡曲線Fig.5 Actual Trajectory Curve

移動機器人在運動過程中側向偏差和橫擺角速度的變化曲線，如圖6 所示。進行圓弧轉向時橫擺角速度變大，結束轉向時橫擺角速度恢復至正常值。從橫擺角速度變化曲線可知，橫擺角速度始終保持在±0.6rad/之間，并且在整個運動過程中橫擺角速度的變化曲線非常光滑，表明移動機器人在路徑跟蹤控制過程中運動平緩，可靠。當移動機器人運動至地標位置時，利用地標對慣性導航過程中的累積誤差進行消除，故在圖6 中會出現側向偏差發生突變的現象。從圖中可以看到在運動過程中慣導的累積誤差能夠保證在±6mm，表明該路徑跟蹤控制方法能夠實現高精度的路徑跟蹤控制。

圖6 側向偏差及橫擺角速度變化曲線Fig.6 Variation Curve of Lateral Deviation and Yaw Rate

4 結論

（1）根據移動機器人的運動狀態及其路徑跟蹤預瞄點在局部坐標系中的相對位置，在移動機器人質心和預瞄點之間實時規劃出一條逼近目標軌跡的動態虛擬路徑。

（2）針對傳統的基于PID 的路徑跟蹤控制方法參數標定困難的問題，研究設計了一種基于預瞄點的移動機器人路徑跟蹤控制方法，并進行了實驗測試，結果表明本方法能夠實現高精度路徑跟蹤控制，可實現6mm 控制精度。

（3）移動機器人在運動過程中仍然存在一定的誤差，其中誤差的主要來源包括傳感器誤差，機械結構導致的誤差等，在之后的研究中應采取措施避免此類誤差，以更好的驗證算法本身的精確性。