朱欣華，王 健，郭民環(huán)，姚速瑞，蘇 巖

（南京理工大學(xué)，南京 210094）

目前，兩輪驅(qū)動機(jī)器人廣泛應(yīng)用于工業(yè)生產(chǎn)、倉儲轉(zhuǎn)運(yùn)以及智能家居等多個(gè)領(lǐng)域，在節(jié)約人力成本的同時(shí)，能夠有效提高工作質(zhì)量[1-3]。兩輪機(jī)器人的運(yùn)動控制對其性能有著很大的影響[4]。運(yùn)動控制主要包括路徑規(guī)劃和路徑跟蹤兩方面內(nèi)容。路徑規(guī)劃是根據(jù)機(jī)器人的任務(wù)要求以及所處的環(huán)境信息通過特定的算法計(jì)算搜索出最優(yōu)路徑，然后再對計(jì)算得到的最優(yōu)路徑進(jìn)行跟蹤，而準(zhǔn)確地進(jìn)行路徑跟蹤，是兩輪驅(qū)動機(jī)器人順利完成任務(wù)的基礎(chǔ)[5]。由于兩輪驅(qū)動機(jī)器人屬于欠驅(qū)動系統(tǒng)，較難控制，目前大部分實(shí)際應(yīng)用的兩輪驅(qū)動機(jī)器人的路徑跟蹤控制都是基于雙閉環(huán)的電機(jī)控制，內(nèi)環(huán)是電流環(huán)，外環(huán)是速度環(huán)，每個(gè)電機(jī)單獨(dú)控制[6]。這類方法通常有一定的局限性，由于沒有引入位置信息，若兩輪機(jī)器人的初始位置與期望路徑的初始位置存在較大偏差，或者兩輪機(jī)器人在行駛過程中受外力影響發(fā)生橫向偏移時(shí)，兩輪機(jī)器人將無法對期望路徑進(jìn)行跟蹤。同時(shí)，目前對于兩輪驅(qū)動機(jī)器人運(yùn)動控制的研究較多地停留在理論研究階段，對于控制方法的實(shí)際驗(yàn)證較少[7]。

本文針對兩輪驅(qū)動機(jī)器人的路徑跟蹤問題，提出一種新的控制方法。通過左右輪的雙閉環(huán)控制來約束兩輪機(jī)器人的速度和位姿，在此基礎(chǔ)上，在期望路徑附近建立矢量場，引入位置約束，促使兩輪機(jī)器人在偏離期望路徑時(shí)能夠快速回歸，完成路徑跟蹤任務(wù)。同時(shí)搭建了兩輪驅(qū)動機(jī)器人硬件實(shí)驗(yàn)平臺，分別對直線路徑和曲線路徑進(jìn)行跟蹤，驗(yàn)證雙閉環(huán)控制的有效性。在此基礎(chǔ)上通過外力使兩輪機(jī)器人偏離期望路徑后再進(jìn)行路徑跟蹤，以驗(yàn)證在存在位置誤差時(shí)矢量場法的有效性。最后對通過路徑規(guī)劃算法計(jì)算出的路徑進(jìn)行跟蹤，驗(yàn)證本文提出的路徑跟蹤控制方法的實(shí)用性和有效性。

1 兩輪驅(qū)動機(jī)器人運(yùn)動學(xué)模型

兩輪驅(qū)動機(jī)器人在二維平面的運(yùn)動可以簡化為平面剛體運(yùn)動。其運(yùn)動完全靠兩個(gè)驅(qū)動輪的轉(zhuǎn)速來控制[8]。在路徑跟蹤控制之前，先要對兩輪驅(qū)動機(jī)器人的線速度v和角速度ω的信息進(jìn)行融合，分別求出加在閉環(huán)驅(qū)動器上左右輪的期望速度，然后再對兩輪的速度進(jìn)行閉環(huán)控制。

兩輪驅(qū)動機(jī)器人的機(jī)械結(jié)構(gòu)由兩個(gè)驅(qū)動輪、一個(gè)隨動輪以及機(jī)器人主體組成。隨動輪僅在運(yùn)動失衡時(shí)起支撐作用，在運(yùn)動學(xué)方程中可以忽略不計(jì)，其簡化模型如圖1所示。在全局坐標(biāo)系（O, X, Y）中，兩輪機(jī)器人的位置（圖中的點(diǎn) M）可由向量表示，機(jī)器人的運(yùn)動學(xué)方程表示為：

式中，ωl表示左輪角速度；ωr表示右輪角速度；R表示機(jī)器人驅(qū)動輪半徑；D表示兩驅(qū)動輪之間的距離。

參考機(jī)器人位置（圖中的點(diǎn) N）由向量 pr=表示，如圖1所示，以兩輪機(jī)器人M為參考建立局部坐標(biāo)系 (o, i, j)，其中局部坐標(biāo)系原點(diǎn)o為機(jī)器人兩驅(qū)動輪輪重心連線的中點(diǎn)，i方向?yàn)闄C(jī)器人速度正方向。機(jī)器人運(yùn)動的誤差矢量可表示為：

兩輪驅(qū)動機(jī)器人的位置可以由以下方程表示：

式中，ΔΦr和ΔΦl分別為采樣時(shí)間內(nèi)兩輪驅(qū)動機(jī)器人左右編碼的角位移；Δs為采樣時(shí)間內(nèi)機(jī)器人的移動位移；Δθ為采樣時(shí)間內(nèi)機(jī)器人的移動角度。

圖1 兩輪驅(qū)動機(jī)器人運(yùn)動模型示意圖Fig.1 Schematic diagram of motion model of two-wheel-drive robot

基于兩輪驅(qū)動機(jī)器人運(yùn)動模型的路徑跟蹤控制問題主要是使機(jī)器人系統(tǒng)在任意初始誤差下，確定兩輪驅(qū)動機(jī)器人的輸入控制vc和ωc，從而跟蹤參考位姿和參考輸入

2 路徑跟蹤控制方法

針對兩輪驅(qū)動機(jī)器人的路徑跟蹤問題，通過給定驅(qū)動電機(jī)不同的速度參數(shù)來控制兩輪差速驅(qū)動機(jī)器人的位姿和速度，從而實(shí)現(xiàn)兩輪驅(qū)動機(jī)器人的直線、轉(zhuǎn)彎或旋轉(zhuǎn)等不同的運(yùn)動方式。首先建立左右輪的雙閉環(huán)PID控制器，通過約束兩輪驅(qū)動機(jī)器人的速度和位姿的方式來跟蹤期望軌跡，在此基礎(chǔ)上，在期望路徑附近建立矢量場，引入位置約束，促使兩輪驅(qū)動機(jī)器人在偏離期望路徑時(shí)能夠快速回歸。

2.1 雙閉環(huán)PID控制器

PID控制是一種最通用的控制方法[9]。大多數(shù)反饋回路用該方法或其較小的變形來實(shí)現(xiàn)控制。常規(guī)的PID控制系統(tǒng)主要由被控對象以及控制器兩個(gè)部分構(gòu)成[10]。本文所設(shè)計(jì)的控制算法的核心思想是通過雙閉環(huán)PID控制器來調(diào)節(jié)兩輪驅(qū)動機(jī)器人左右兩輪的轉(zhuǎn)速，從而對期望路徑進(jìn)行跟蹤，其原理如圖2所示。

圖2 兩輪驅(qū)動機(jī)器人運(yùn)動控制算法原理圖Fig.2 Motion control algorithm schematic of two-wheel-drive robot

控制算法包含兩個(gè)PID控制器。其中，θd和vd分別為兩輪驅(qū)動機(jī)器人的期望航向角和期望線速度；θa和 va分別為兩輪驅(qū)動機(jī)器人的實(shí)際航向角和實(shí)際線速度。在角度環(huán)PID控制器中，輸入值為航向角誤差eθ，輸出值為uθ，方程如下：

式中，Kpθ為角度環(huán)比例項(xiàng)系數(shù)，Kiθ為角度環(huán)積分項(xiàng)系數(shù)，Kdθ為角度環(huán)微分項(xiàng)系數(shù)。求解出的 uθ（t)用來修正兩輪驅(qū)動機(jī)器人在運(yùn)行過程中的航向誤差。

在速度環(huán)PID控制器中，輸入值為兩輪驅(qū)動機(jī)器人兩驅(qū)動輪連線中點(diǎn)線速度誤差 ev，輸出值為 uv，方程如下：

式中，Kpv為速度環(huán)比例項(xiàng)系數(shù)，Kiv為速度環(huán)積分項(xiàng)系數(shù)，Kdv為速度環(huán)微分項(xiàng)系數(shù)。求解出的 uv（t)用來控制兩輪驅(qū)動機(jī)器人在運(yùn)行過程中的行駛速度。

傳統(tǒng)以輪距中點(diǎn)為參考對象的兩輪驅(qū)動機(jī)器人的控制通常是通過維持作為參考對象驅(qū)動輪角速度不變，而通過調(diào)整另一個(gè)輪的角速度來調(diào)節(jié)航向角，這種控制方式調(diào)節(jié)不夠靈活，在轉(zhuǎn)角比較大的環(huán)境中具有局限性[11]。而本文設(shè)計(jì)的控制方法，是通過速度環(huán)PID控制器求解出控制量 uv(t)來調(diào)節(jié)左右輪驅(qū)動電機(jī)的基礎(chǔ)轉(zhuǎn)速，然后通過角度環(huán)PID控制器求解出控制量uθ(t)來調(diào)節(jié)兩輪驅(qū)動機(jī)器人的姿態(tài)。方法如下：

式中，Tr和 Tl分別為輸入到右輪和左輪驅(qū)動電機(jī)的值，K為控制量到電機(jī)驅(qū)動模塊的PWM值的轉(zhuǎn)換系數(shù)。最終，通過對兩輪驅(qū)動機(jī)器人左右輪的分別控制，實(shí)現(xiàn)兩輪驅(qū)動機(jī)器人對預(yù)期軌跡的跟蹤。

2.2 矢量場的建立

通過雙閉環(huán)的PID控制器可以很好地對期望軌跡進(jìn)行跟蹤，由于沒有引入位置約束，若兩輪驅(qū)動機(jī)器人的初始位置與期望位置存在較大偏差或兩輪驅(qū)動機(jī)器人在行駛過程中受外力因素位置發(fā)生偏離時(shí)，僅僅通過雙閉環(huán)的PID控制器，無法使兩輪驅(qū)動機(jī)器人回到預(yù)期的行駛路線。也就是說只依靠雙閉環(huán)PID控制器只能保證機(jī)器人按照期望路徑的形狀行駛，無法保證兩輪驅(qū)動機(jī)器人軌跡的絕對位置與期望路徑重合。

針對以上問題，本文通過在期望路徑附近建立矢量場的方法，引入位置信息，促使兩輪驅(qū)動機(jī)器人在偏離期望路徑時(shí)能夠快速回歸[12]。具體方法如下：

圖3為兩輪驅(qū)動機(jī)器人當(dāng)前位置與預(yù)設(shè)軌跡示意圖，P點(diǎn)為機(jī)器人的當(dāng)前位置，P1為預(yù)設(shè)起點(diǎn)，P2為預(yù)設(shè)目標(biāo)點(diǎn)，的長度為機(jī)器人的步長，機(jī)器人的期望路徑可以看作由很多條該類型的線段組成。虛線y表示機(jī)器人當(dāng)前位置P距離期望軌跡的距離，y有正負(fù)之分。建立矢量場是根據(jù)構(gòu)建一個(gè)只考慮行駛方向的速度場，根據(jù)任一P點(diǎn)位置求解出行駛方向目標(biāo)值。構(gòu)建方法如下：

圖3 兩輪驅(qū)動機(jī)器人預(yù)設(shè)軌跡示意圖Fig.3 Presupposition trajectory schematic of two-wheel-drive robot

其中，χd為行駛方向目標(biāo)值，其為關(guān)于y的函數(shù)。χ∞是y很大時(shí)的行駛方向目標(biāo)值，垂直并指向是值為正數(shù)的常量，用來調(diào)節(jié)arctan()ky·隨y收斂到0的速度。由公式可以看出，當(dāng)y很大，即兩輪驅(qū)動機(jī)器人當(dāng)前位置偏離期望路徑很多時(shí)，求解得出的行駛方向目標(biāo)值垂直指向期望路徑當(dāng) y為 0，即機(jī)器人當(dāng)前位置與期望路徑重合時(shí)，求解得出的行駛方向目標(biāo)值與期望路徑平行。建立矢量場的仿真結(jié)果如圖4所示，圖中，點(diǎn)劃線代表期望軌跡，箭頭代表機(jī)器人在不同位置時(shí)求解出的期望行駛方向。

通過仿真結(jié)果可以看出，矢量場法在目標(biāo)路徑附近構(gòu)建了一個(gè)全平面的矢量場，當(dāng)兩輪驅(qū)動機(jī)器人處于任何位置的時(shí)候，均可在矢量場的引導(dǎo)下回歸到目標(biāo)路徑?？梢酝ㄟ^調(diào)節(jié)式(14)中的 k值來改變回歸的速度。由圖4可以看出，當(dāng)兩輪驅(qū)動機(jī)器人距離目標(biāo)路徑較遠(yuǎn)時(shí)，求解出的期望行駛方向幾乎垂直于目標(biāo)路徑，這樣可以保證兩輪驅(qū)動機(jī)器人快速回歸。而當(dāng)兩輪驅(qū)動機(jī)器人接近目標(biāo)路徑時(shí)，求解出的期望行駛方向與目標(biāo)路徑之間的偏角逐漸變小，直至期望行駛方向與目標(biāo)路徑重合，這樣可以保證兩輪驅(qū)動機(jī)器人在回歸過程中行駛軌跡圓滑。建立的矢量場很好地補(bǔ)充了雙閉環(huán)PID控制器缺少位置信息的不足。

圖4 矢量場法仿真結(jié)果Fig.4 Simulation results of vector field method

3 實(shí)驗(yàn)研究

3.1 實(shí)驗(yàn)平臺及實(shí)驗(yàn)環(huán)境

圖5為研究時(shí)所用的兩輪驅(qū)動機(jī)器人平臺，其主體由三個(gè)車輪和一個(gè)硬件平臺組成。三個(gè)車輪中有兩個(gè)為驅(qū)動輪，分別由兩個(gè)PWM電機(jī)控制，另一個(gè)為萬向輪，主要起支撐作用。硬件平臺由一塊Arduino uno R3開發(fā)板以及藍(lán)牙模塊等傳感器組成，其中藍(lán)牙模塊主要用來實(shí)現(xiàn)機(jī)器人與上位機(jī)之間的無線通信。作為實(shí)驗(yàn)驗(yàn)證，由上位機(jī)將計(jì)算的結(jié)果發(fā)送給控制平臺，通過控制兩輪驅(qū)動機(jī)器人左右輪的轉(zhuǎn)速來控制機(jī)器人運(yùn)動。

在實(shí)際應(yīng)用中，首先，根據(jù)實(shí)驗(yàn)用兩輪驅(qū)動機(jī)器人平臺的模型確定參考位姿，以兩輪重心連線中點(diǎn)作為局部坐標(biāo)系原點(diǎn)，從原點(diǎn)指向機(jī)器人支撐輪的方向?yàn)樗俣日较?，右轉(zhuǎn)為角速度正方向。然后將機(jī)器人驅(qū)動輪半徑和兩驅(qū)動輪距離代入公式(2)中計(jì)算出運(yùn)動學(xué)模型，最后根據(jù)運(yùn)動模型設(shè)計(jì)實(shí)驗(yàn)。

定位信息獲取技術(shù)用來提供兩輪驅(qū)動機(jī)器人的位置、姿態(tài)等信息，它是能否對機(jī)器人實(shí)現(xiàn)控制的基礎(chǔ)[13-14]。為實(shí)現(xiàn)實(shí)驗(yàn)驗(yàn)證，本文選用視覺定位技術(shù)來獲取機(jī)器人的位置信息。視覺定位技術(shù)是一種能夠得到較為精準(zhǔn)位置信息的定位技術(shù)，通過在環(huán)境中設(shè)置的攝像機(jī)來采集機(jī)器人的位置信息[15]。

圖5 兩輪驅(qū)動機(jī)器人實(shí)驗(yàn)平臺Fig.5 Two-wheel-drive robot experimental platform

實(shí)驗(yàn)在Vicon實(shí)驗(yàn)室環(huán)境下完成，如圖6所示。

圖6 Vicon實(shí)驗(yàn)室Fig.6 Vicon Laboratory

Vicon即光學(xué)運(yùn)動捕捉系統(tǒng)，是通過視覺的方式來采集實(shí)驗(yàn)環(huán)境中標(biāo)記物的位置。在該實(shí)驗(yàn)環(huán)境中，上位機(jī)接收到攝像頭提供的兩輪驅(qū)動機(jī)器人位置信息，通過建模計(jì)算出兩輪驅(qū)動機(jī)器人當(dāng)前所處的位置坐標(biāo)以及航向角度，最后通過計(jì)算得到兩輪驅(qū)動機(jī)器人當(dāng)前的速度，從而形成閉環(huán)反饋[16]。在得到機(jī)器人的基本信息后，由上位機(jī)通過藍(lán)牙模塊向機(jī)器人發(fā)送指令，進(jìn)行路徑跟蹤實(shí)驗(yàn)。

3.2 實(shí)驗(yàn)結(jié)果及分析

首先為了驗(yàn)證雙閉環(huán)控制方法的有效性，分別對直線路徑和較復(fù)雜的曲線路徑進(jìn)行跟蹤。在此基礎(chǔ)上通過外力使兩輪驅(qū)動機(jī)器人偏離期望路徑后再進(jìn)行路徑跟蹤，以驗(yàn)證矢量場法的有效性。最后通過模擬兩輪驅(qū)動機(jī)器人的真實(shí)工作環(huán)境，先利用文獻(xiàn)[17]中提出的局部路徑規(guī)劃算法進(jìn)行路徑規(guī)劃，再對規(guī)劃出的路徑進(jìn)行跟蹤，驗(yàn)證本文提出的路徑跟蹤控制方法的實(shí)用性和有效性。由上位機(jī)采集實(shí)驗(yàn)過程中兩輪驅(qū)動機(jī)器人的位置、速度以及航向等信息，為方便對比分析，采用Matlab對數(shù)據(jù)進(jìn)行處理，并將實(shí)驗(yàn)時(shí)兩輪驅(qū)動機(jī)器人運(yùn)動軌跡繪制在二維平面上。

首先，進(jìn)行了對于直線路徑的跟蹤，圖7所示為引入控制算法前后兩輪驅(qū)動機(jī)器人行走軌跡的對比圖。直線路徑比較簡單，兩輪驅(qū)動機(jī)器人只需保持恒定的速度大小和方向行駛即可。但從圖中可以看出，由于無人小車的固有誤差以及實(shí)驗(yàn)環(huán)境所存在的誤差，在引入控制算法前，兩輪驅(qū)動機(jī)器人并沒有跟蹤實(shí)驗(yàn)所期望的直線路徑，而是沿著一條逐漸向左偏離的曲線行駛。在引入控制算法之后，控制算法對誤差進(jìn)行了有效的修正，兩輪驅(qū)動機(jī)器人基本可以沿著一條直線行駛（如圖中虛線所示）。

圖7 引入控制算法前后軌跡對比圖Fig.7 The trajectory contrast map before and after the introduction of the control algorithm

考慮到直線路徑跟蹤較為簡單，設(shè)計(jì)了對較復(fù)雜的路徑進(jìn)行跟蹤的實(shí)驗(yàn)。路徑由以下函數(shù)表示：

該函數(shù)中t為變量，代表時(shí)間，函數(shù)圖形為從原點(diǎn)出發(fā)，經(jīng)過一個(gè)曲率隨時(shí)間變化的曲線，最終回到原點(diǎn)。圖8所示為實(shí)驗(yàn)結(jié)果，通過兩輪驅(qū)動機(jī)器人軌跡可以看出，兩輪驅(qū)動機(jī)器人成功完成了軌跡跟蹤任務(wù)并最終回到原點(diǎn)，圖中，軌跡X方向的范圍為3.00 m，Y方向的范圍為1.70 m，用矢量表示即為3.45 m。把圖中每個(gè)實(shí)際跟蹤點(diǎn)的誤差矢量與軌跡范圍矢量的比值定義為每個(gè)實(shí)際跟蹤點(diǎn)的相對誤差，求取各跟蹤點(diǎn)的相對誤差的均方差為0.86%，最大相對誤差為3.64%。

圖8 較復(fù)雜路徑跟蹤實(shí)驗(yàn)結(jié)果Fig.8 Experimental results of complex path tracking

本文所設(shè)計(jì)的兩輪驅(qū)動機(jī)器人路徑跟蹤控制算法中引入了矢量場，即當(dāng)兩輪驅(qū)動機(jī)器人初始位置或在行駛過程中由于意外偏離期望路徑時(shí)，兩輪驅(qū)動機(jī)器人能夠在矢量場的引導(dǎo)下快速回歸期望路徑。為驗(yàn)證矢量場法的有效性，在上述兩個(gè)實(shí)驗(yàn)的基礎(chǔ)上，通過外力將兩輪驅(qū)動機(jī)器人的初始位置放置在偏離期望路徑0.5 m左右的地方，期望路徑為直線路徑。圖9為兩輪驅(qū)動機(jī)器人偏離期望路徑時(shí)的實(shí)驗(yàn)結(jié)果。

圖9 兩輪驅(qū)動機(jī)器人偏離期望路徑時(shí)實(shí)驗(yàn)結(jié)果Fig.9 Experimental results of two-wheel-drive robot deviating from the expected path

通過兩輪驅(qū)動機(jī)器人的軌跡可以看出，與傳統(tǒng)的PID控制相比，當(dāng)機(jī)器人的初始位置偏離期望路徑時(shí)，本文設(shè)計(jì)的控制方法能夠使機(jī)器人在矢量場的作用下，快速回歸期望路徑。并且，在回歸過程中，前期回歸速度較快，幾乎沿著垂直于期望路徑的軌跡行駛，當(dāng)逐漸靠近期望路徑的時(shí)候，機(jī)器人航向與期望路徑之間的夾角逐漸變小，直至與期望軌跡重合，這種回歸方式也更加符合工程的實(shí)際需求。

在實(shí)際應(yīng)用中，兩輪驅(qū)動機(jī)器人通常會預(yù)先根據(jù)環(huán)境信息進(jìn)行路徑規(guī)劃，再對規(guī)劃的路徑進(jìn)行跟蹤，圖10為避障路徑跟蹤實(shí)驗(yàn)結(jié)果。在路徑上設(shè)置三個(gè)隨機(jī)的障礙物，檢測到障礙物后，采用局部路徑規(guī)劃算法計(jì)算出避障路徑[15]，再由兩輪驅(qū)動機(jī)器人對避障路徑進(jìn)行跟蹤。通過實(shí)驗(yàn)結(jié)果可以看出，本文設(shè)計(jì)的兩輪驅(qū)動機(jī)器人路徑跟蹤控制方法可以對避障路徑進(jìn)行跟蹤，完成避障任務(wù)。

圖10 避障路徑跟蹤實(shí)驗(yàn)結(jié)果Fig.10 Experimental results of obstacle avoidance path tracking

4 結(jié) 語

兩輪驅(qū)動機(jī)器人的路徑跟蹤是其運(yùn)動控制的基礎(chǔ)。本文提出了一種兩輪機(jī)器人路徑跟蹤控制方法，通過左右輪的雙閉環(huán)PID控制來約束兩輪驅(qū)動機(jī)器人的速度和位姿。在此基礎(chǔ)上，在期望路徑附近建立矢量場，引入位置約束，由此可以促使兩輪驅(qū)動機(jī)器人在偏離期望路徑時(shí)能夠快速回歸，完成路徑跟蹤任務(wù)，增強(qiáng)兩輪驅(qū)動機(jī)器人的魯棒性。在完成控制方法研究后，搭建了兩輪驅(qū)動機(jī)器人硬件實(shí)驗(yàn)平臺，設(shè)計(jì)了能夠體現(xiàn)控制算法特點(diǎn)的實(shí)驗(yàn)方案，具體實(shí)現(xiàn)了兩輪驅(qū)動機(jī)器人的路徑跟蹤控制，實(shí)驗(yàn)結(jié)果表明，被控兩輪驅(qū)動機(jī)器人能夠準(zhǔn)確跟蹤期望路徑，由較復(fù)雜路徑跟蹤實(shí)驗(yàn)結(jié)果得出的各跟蹤點(diǎn)的相對誤差的均方差為 0.86%，最大相對誤差為 3.64%。通過實(shí)驗(yàn)驗(yàn)證了提出的兩輪驅(qū)動機(jī)器人路徑跟蹤控制方法的實(shí)用性和有效性。