輪式移動(dòng)機(jī)器人路徑跟蹤控制方法研究

肖獻(xiàn)強(qiáng)，程亞兵，王家恩，王鼎用

（1.合肥工業(yè)大學(xué)機(jī)械工程學(xué)院，安徽 合肥 230009；2.合肥工業(yè)大學(xué) 汽車與交通工程學(xué)院，安徽 合肥 230009）

1 引言

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

2 路徑跟蹤控制方法

移動(dòng)機(jī)器人的循跡控制主要包括路徑規(guī)劃和路徑跟蹤控制兩個(gè)方面內(nèi)容。此處研究的重點(diǎn)為路徑跟蹤控制方法，即在通過(guò)路徑規(guī)劃方法獲得期望運(yùn)動(dòng)路徑后，控制移動(dòng)機(jī)器人實(shí)現(xiàn)對(duì)該期望運(yùn)動(dòng)路徑的跟蹤。

移動(dòng)機(jī)器人控制系統(tǒng)由地圖構(gòu)建與定位、路徑規(guī)劃、路徑跟蹤以及底層控制四個(gè)模塊組成。地圖構(gòu)建與定位模塊構(gòu)建移動(dòng)機(jī)器人運(yùn)行的全局坐標(biāo)系，并完成實(shí)時(shí)定位；根據(jù)構(gòu)建的地圖進(jìn)行路徑規(guī)劃，獲取期望運(yùn)動(dòng)路徑；綜合移動(dòng)機(jī)器人的實(shí)時(shí)位置信息以及期望運(yùn)動(dòng)路徑，進(jìn)行路徑跟蹤控制，計(jì)算在當(dāng)前位置跟蹤期望運(yùn)動(dòng)路徑所需的期望橫擺角速度，將期望橫擺角速度轉(zhuǎn)換為電機(jī)脈沖輸出，最終實(shí)現(xiàn)移動(dòng)機(jī)器人的路徑跟蹤控制。

2.1 差速輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

假定在理想條件下差速輪式移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中的位姿可表示為P=［x，y，φ］T，其中x 表示移動(dòng)機(jī)器人質(zhì)心相對(duì)于絕對(duì)坐標(biāo)系原點(diǎn)X 軸方向的距離，y 表示相對(duì)于絕對(duì)坐標(biāo)系原點(diǎn)Y 軸方向的距離，φ 表示左右輪中垂線與Y 軸的夾角。

圖1 運(yùn)動(dòng)學(xué)模型示意圖Fig.1 Schematic Diagram of Kinematic Model

差速輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型，如圖1 所示。右輪速度vr，左輪速度vl，移動(dòng)機(jī)器人在移動(dòng)過(guò)程中的運(yùn)動(dòng)狀態(tài)矩陣［vx，vy，ω］T，如式（1）所示。

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

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

2.2 路徑跟蹤偏差模型

移動(dòng)機(jī)器人在路徑跟蹤控制中的偏差模型包括四個(gè)部分：路徑跟蹤直線方程求解、預(yù)備目標(biāo)點(diǎn)計(jì)算、預(yù)瞄點(diǎn)選取以及偏差計(jì)算。

移動(dòng)機(jī)器人由當(dāng)前點(diǎn)到達(dá)目標(biāo)點(diǎn)，當(dāng)前點(diǎn)用帶方向的藍(lán)色圓點(diǎn)表示，目標(biāo)點(diǎn)用帶方向的紅色表示，黃色圓點(diǎn)表示預(yù)備目標(biāo)點(diǎn)，綠色圓點(diǎn)表示預(yù)瞄點(diǎn)，如圖2 所示。根據(jù)目標(biāo)點(diǎn)坐標(biāo)及方向角度求解路徑跟蹤直線方程，并根據(jù)預(yù)備目標(biāo)點(diǎn)到目標(biāo)點(diǎn)距離Δl和預(yù)瞄距離Δd 確定路徑跟蹤預(yù)備目標(biāo)點(diǎn)和預(yù)瞄點(diǎn)。最后計(jì)算移動(dòng)機(jī)器人當(dāng)前位置相對(duì)于預(yù)瞄點(diǎn)的縱向、側(cè)向以及方向偏差。

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

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

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

移動(dòng)機(jī)器人當(dāng)前位置相對(duì)于預(yù)瞄點(diǎn)的偏差即為預(yù)瞄點(diǎn)在以當(dāng)前位置為原點(diǎn)的局部坐標(biāo)系下的坐標(biāo)，如式（4）所示。

2.3 路徑跟蹤控制方法設(shè)計(jì)

在OcXcYc局部坐標(biāo)系中移動(dòng)機(jī)器人質(zhì)心與預(yù)瞄點(diǎn)之間實(shí)時(shí)構(gòu)造虛擬路徑[9]，如圖3 所示。

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

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

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

局部坐標(biāo)系OcXcYc是以移動(dòng)機(jī)器人當(dāng)前位置為原點(diǎn)，當(dāng)前方向?yàn)閄 軸正方向建立的，如圖3 所示。在局部坐標(biāo)系下虛擬路徑的構(gòu)造條件為：起始點(diǎn)坐標(biāo)為（0，0），斜率為 0，曲率為 ρ=ω/v；預(yù)瞄點(diǎn)坐標(biāo)為（xe，ye），斜率為tanφe，曲率為0。根據(jù)構(gòu)造條件即可求解得虛擬路徑方程為：

已知虛擬路徑的曲線方程為y（x），移動(dòng)機(jī)器人質(zhì)心在曲線上的坐標(biāo)為（x，y），速度為v，速度方向及運(yùn)動(dòng)曲率與移動(dòng)機(jī)器人當(dāng)前所在曲線上點(diǎn)一致。定義ωd為運(yùn)動(dòng)軌跡為曲線方程時(shí)的理想橫擺角速度，其計(jì)算公式，如式（7）所示。則理想橫擺角速度變化率的計(jì)算公式，如式（8）所示。

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

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)平臺(tái)介紹

實(shí)驗(yàn)平臺(tái)采用自主設(shè)計(jì)的差速輪式移動(dòng)機(jī)器人，其基本參數(shù)，如表1 所示。

表1 差速輪式移動(dòng)機(jī)器人基本參數(shù)Tab.1 Basic Parameters of Differential Wheeled Mobile Robots

3.2 實(shí)驗(yàn)數(shù)據(jù)分析

移動(dòng)機(jī)器人的實(shí)驗(yàn)運(yùn)動(dòng)路徑為8 字形，移動(dòng)機(jī)器人需要進(jìn)行連續(xù)的圓弧轉(zhuǎn)向以充分驗(yàn)證所設(shè)計(jì)的移動(dòng)機(jī)器人路徑跟蹤控制方法的可靠性、穩(wěn)定性和精確性，以左下角站點(diǎn)為起始點(diǎn)，運(yùn)動(dòng)軌跡為①-②-③-④最終到達(dá)終點(diǎn)，如圖4 所示。

移動(dòng)機(jī)器人實(shí)驗(yàn)過(guò)程中的實(shí)際運(yùn)動(dòng)軌跡曲線圖，如圖5 所示。從軌跡曲線圖中可以看出移動(dòng)機(jī)器人能很好的跟蹤規(guī)劃的運(yùn)動(dòng)軌跡。對(duì)比圖4 與圖5，實(shí)際運(yùn)動(dòng)軌跡與期望運(yùn)動(dòng)軌跡非常接近，表明了設(shè)計(jì)的路徑跟蹤控制方法的精確性和可靠性。

圖4 期望運(yùn)動(dòng)軌跡曲線Fig.4 Desired Trajectory Curve

圖5 實(shí)際運(yùn)動(dòng)軌跡曲線Fig.5 Actual Trajectory Curve

移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中側(cè)向偏差和橫擺角速度的變化曲線，如圖6 所示。進(jìn)行圓弧轉(zhuǎn)向時(shí)橫擺角速度變大，結(jié)束轉(zhuǎn)向時(shí)橫擺角速度恢復(fù)至正常值。從橫擺角速度變化曲線可知，橫擺角速度始終保持在±0.6rad/之間，并且在整個(gè)運(yùn)動(dòng)過(guò)程中橫擺角速度的變化曲線非常光滑，表明移動(dòng)機(jī)器人在路徑跟蹤控制過(guò)程中運(yùn)動(dòng)平緩，可靠。當(dāng)移動(dòng)機(jī)器人運(yùn)動(dòng)至地標(biāo)位置時(shí)，利用地標(biāo)對(duì)慣性導(dǎo)航過(guò)程中的累積誤差進(jìn)行消除，故在圖6 中會(huì)出現(xiàn)側(cè)向偏差發(fā)生突變的現(xiàn)象。從圖中可以看到在運(yùn)動(dòng)過(guò)程中慣導(dǎo)的累積誤差能夠保證在±6mm，表明該路徑跟蹤控制方法能夠?qū)崿F(xiàn)高精度的路徑跟蹤控制。

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

4 結(jié)論

（1）根據(jù)移動(dòng)機(jī)器人的運(yùn)動(dòng)狀態(tài)及其路徑跟蹤預(yù)瞄點(diǎn)在局部坐標(biāo)系中的相對(duì)位置，在移動(dòng)機(jī)器人質(zhì)心和預(yù)瞄點(diǎn)之間實(shí)時(shí)規(guī)劃出一條逼近目標(biāo)軌跡的動(dòng)態(tài)虛擬路徑。

（2）針對(duì)傳統(tǒng)的基于PID 的路徑跟蹤控制方法參數(shù)標(biāo)定困難的問(wèn)題，研究設(shè)計(jì)了一種基于預(yù)瞄點(diǎn)的移動(dòng)機(jī)器人路徑跟蹤控制方法，并進(jìn)行了實(shí)驗(yàn)測(cè)試，結(jié)果表明本方法能夠?qū)崿F(xiàn)高精度路徑跟蹤控制，可實(shí)現(xiàn)6mm 控制精度。

（3）移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中仍然存在一定的誤差，其中誤差的主要來(lái)源包括傳感器誤差，機(jī)械結(jié)構(gòu)導(dǎo)致的誤差等，在之后的研究中應(yīng)采取措施避免此類誤差，以更好的驗(yàn)證算法本身的精確性。