基于動態(tài)窗口法的移動機(jī)器人自動避障導(dǎo)航研究

彭育強(qiáng),黃澤龍,李少偉

(廣東交通職業(yè)技術(shù)學(xué)院軌道交通學(xué)院，廣東 廣州 510650)

0 引言

隨著人工智能技術(shù)的不斷發(fā)展，機(jī)器人技術(shù)及其應(yīng)用已從局部的工業(yè)場景延伸至人們的日常生活之中。從機(jī)器人技術(shù)的發(fā)展趨勢可以看出，機(jī)器人顯得越來越智能化，將來能夠更好地為人類社會發(fā)展提供服務(wù)。移動機(jī)器人的自動避障技術(shù)是機(jī)器人技術(shù)及應(yīng)用的研究重點內(nèi)容之一。其目的是在障礙物環(huán)境下，機(jī)器人能夠自主避開障礙物并規(guī)劃最佳運(yùn)動路徑運(yùn)行至目標(biāo)點，主要涉及機(jī)器人自身定位、路徑規(guī)劃以及移動控制等[1-3]。

本文研究的主要內(nèi)容是在樹莓派平臺的基礎(chǔ)上,采用STM32控制器以及通過動態(tài)窗口法，結(jié)合機(jī)器人操作系統(tǒng)(robot operating system,ROS)平臺,實現(xiàn)對機(jī)器人的在線自動避障控制;通過MATLAB仿真以及搭建移動機(jī)器人系統(tǒng)平臺進(jìn)行試驗測試，驗證了該移動機(jī)器人自動避障方案的正確性、有效性等。

1 機(jī)器人系統(tǒng)總體方案設(shè)計

控制方案采用目前熱門的ROS作為機(jī)器人開發(fā)平臺。該平臺可以方便、快速地為機(jī)器人控制系統(tǒng)提供相應(yīng)的控制服務(wù)，包括硬件抽象描述、底層設(shè)備應(yīng)用控制、公共功能的操作以及發(fā)包管理和程序傳遞等[4]。本文所設(shè)計的機(jī)器人控制系統(tǒng)總體框圖如圖1所示。

圖1 機(jī)器人控制系統(tǒng)總體框圖Fig.1 Block diagram of robot control system

系統(tǒng)可以通過應(yīng)用層，包括手機(jī)應(yīng)用(application,APP)、PS3手柄或者機(jī)器人可視化(robot visnalization,RVIZ)等，輸入控制信息；在導(dǎo)航層，通過自適應(yīng)蒙特卡羅定位(adaptive Monte Coarlo location,AMCL)定位技術(shù)實現(xiàn)對機(jī)器人在地圖中的位置跟蹤，采用動態(tài)窗口法實現(xiàn)局部路徑中的避障規(guī)劃，控制機(jī)器人硬件平臺中的差速電機(jī)實現(xiàn)避障控制。

2 機(jī)器人平臺搭建

本機(jī)器人系統(tǒng)主要由上位機(jī)和下位機(jī)組成。上位機(jī)由工控機(jī)組成，負(fù)責(zé)機(jī)器人的所有控制。下位機(jī)由STM32作為主控芯片，負(fù)責(zé)機(jī)器人差速電機(jī)的速度和角度控制，涉及機(jī)器人運(yùn)動控制、定位導(dǎo)航等功能。除此之外，系統(tǒng)還包括激光雷達(dá)、陀螺儀等傳感器，用于機(jī)器人周圍環(huán)境檢測。為保證上位機(jī)與下位機(jī)之間數(shù)據(jù)傳輸?shù)臏?zhǔn)確性與實時性，采用RS-485總線方式進(jìn)行通信連接。

2.1 上位機(jī)

由于目前系統(tǒng)只支持在Linux系統(tǒng)上安裝部署，在此選用具有體積小、功能豐富、成本低等特點的樹莓派3B作為主控器。在平臺上安裝Ubuntu MATE系統(tǒng)。Ubuntu是一個以桌面應(yīng)用為主的開源GNU/Linux操作系統(tǒng)，可實現(xiàn)ROS在樹莓派上的穩(wěn)定運(yùn)行，并通過串口和硬件層連接實現(xiàn)數(shù)據(jù)采集和通信[5-6]。

2.2 STM32控制器

選用STM32F103RCT6作為機(jī)器人底層硬件系統(tǒng)的控制器。該芯片擁有的資源非常豐富，如72 MHz速度、256 KB程序存儲器容量以及豐富的外圍設(shè)備接口和通信方式。該控制器除了對機(jī)器人進(jìn)行運(yùn)動控制之外，還對陀螺儀傳感器數(shù)據(jù)進(jìn)行解算，得出機(jī)器人運(yùn)行角速度；對編碼器進(jìn)行解算，得出機(jī)器人運(yùn)行線速度等。通過對傳感器數(shù)據(jù)采集和角速度、線速度的積分運(yùn)算處理，得到機(jī)器人的里程計信息，并將該里程信息通過串口發(fā)送至ROS。STM32控制系統(tǒng)結(jié)構(gòu)如圖2所示。ROS發(fā)送相關(guān)控制指令至STM32，從而控制機(jī)器人驅(qū)動電機(jī)，實現(xiàn)對機(jī)器人的運(yùn)動控制。

圖2 STM32控制系統(tǒng)結(jié)構(gòu)圖Fig.2 STM32 control system structure diagram

2.3 激光雷達(dá)

考慮到機(jī)器人運(yùn)行的現(xiàn)場環(huán)境、性價比以及系統(tǒng)集成度等因素，本系統(tǒng)選用Lidar x2系列激光雷達(dá)C0602作為機(jī)器人感知環(huán)境的傳感器。該雷達(dá)系統(tǒng)采用三角測距原理，激光測距采樣能力能夠達(dá)到2 000次/s，可以實現(xiàn)在二維平面的8 m半徑范圍內(nèi)進(jìn)行360°全方位的激光測距掃描，并產(chǎn)生所在空間的平面點云地圖信息；可以方便地用于地圖測繪、機(jī)器人定位導(dǎo)航、物體/環(huán)境建模等。STM32通過串口提取C0602激光雷達(dá)發(fā)送過來有效數(shù)據(jù)，并解析得到激光雷達(dá)的實測信息，即機(jī)器人的狀態(tài)信息。激光傳感器C0602通信協(xié)議如表1所示[7]。

表1 激光傳感器C0602通信協(xié)議Tab.1 Laser sensor C0602 communication protocol

3 路徑導(dǎo)航算法

路徑導(dǎo)航技術(shù)是移動機(jī)器人技術(shù)的核心關(guān)鍵技術(shù)。其目的是使機(jī)器人按照預(yù)定的路徑導(dǎo)航信息自主運(yùn)動控制，或者依據(jù)傳感器獲取外部環(huán)境進(jìn)行相應(yīng)的引導(dǎo)，從而規(guī)劃出一條機(jī)器人能夠順利達(dá)到目標(biāo)點的路徑。在本系統(tǒng)中，研究機(jī)器人在全局路徑規(guī)劃導(dǎo)航中需要具備避障能力，即局部路徑規(guī)劃能力。導(dǎo)航過程首先通過激光雷達(dá)傳感器獲得相關(guān)的地圖信息，然后進(jìn)行路徑規(guī)劃計算，最后發(fā)送路徑信息給機(jī)器人系統(tǒng)，通過相應(yīng)的決策層使其實現(xiàn)自主導(dǎo)航控制功能[8]。移動機(jī)器人導(dǎo)航框架結(jié)構(gòu)如圖3所示。

圖3 移動機(jī)器人導(dǎo)航框架結(jié)構(gòu)圖Fig.3 Mobile robot navigation frame structure diagram

本系統(tǒng)中，機(jī)器人系統(tǒng)采用差速輪作為移動控制電機(jī)，研究采用動態(tài)窗口法進(jìn)行機(jī)器人路徑規(guī)劃導(dǎo)航控制。動態(tài)窗口法由D.Fox提出，是一種直接在控制指令空間內(nèi)搜索使得目標(biāo)函數(shù)取得最大值的最佳控制指令自主壁障算法[9]。動態(tài)窗口法研究中需要著重考慮機(jī)器人的動力學(xué)性能，其核心是在機(jī)器人的速度(v，w)空間采樣多組速度，并模擬機(jī)器人在這些速度下一定時間(sim_period)內(nèi)的移動軌跡。得到多組軌跡后，使用評價函數(shù)選出最優(yōu)的一組速度(含角速度、線速度)來驅(qū)動機(jī)器人運(yùn)動[10]。因此，如需模擬機(jī)器人的運(yùn)動軌跡，則先要建立機(jī)器人的運(yùn)動模型。根據(jù)兩輪差速機(jī)器人運(yùn)動學(xué)模型結(jié)構(gòu)，假設(shè)機(jī)器人線速度為v、角速度為ω、某一時刻(k)在世界坐標(biāo)系中的姿態(tài)角為θ(k)、在時間間隔Δk內(nèi)機(jī)器人作直線運(yùn)動。機(jī)器人的軌跡在世界坐標(biāo)系的位置可以推算為：

(1)

根據(jù)機(jī)器人運(yùn)動軌跡模型采樣機(jī)器人運(yùn)動速度，即可推算出機(jī)器人的運(yùn)動軌跡。在二維空間中，機(jī)器人速度(v、ω)存在著無窮多組速度。但機(jī)器人自身運(yùn)動又受到相關(guān)條件的制約[11]，如電機(jī)性能、周圍環(huán)境、機(jī)器人自身結(jié)構(gòu)等。因此，需要依靠機(jī)器人的自身條件和環(huán)境等客觀因素，將采樣速度控制在一定范圍內(nèi)。以下為對機(jī)器人運(yùn)動控制影響較大的約束因素。

①機(jī)器人自身最大、最小速度約束。

vm={v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(2)

機(jī)器人運(yùn)動控制中的速度受到自身線速度、角速度的最小、最大速度約束，因此需要將機(jī)器人的速度空間vm限制在約束范圍內(nèi)。

②機(jī)器人驅(qū)動機(jī)電性能約束。

(3)

③安全制度距離約束。

為了保證機(jī)器人在碰到障礙物之前能夠及時停止，機(jī)器人的速度必須控制在一定范圍內(nèi)。假設(shè)機(jī)器人在速度(v、ω)時刻遇到障礙物的距離為dist(v、ω)，則機(jī)器人的速度范圍為：

(4)

④評價函數(shù)。

通過式(2)～式(4)可知，在約束條件下，機(jī)器人速度空間中的不同速度(v，ω)有著不同運(yùn)行軌跡。因此，需要建立評價函數(shù)選出最優(yōu)軌跡。最優(yōu)運(yùn)行軌跡對應(yīng)的速度組(v,ω)則選定為機(jī)器人控制指令。其評價函數(shù)如下：

G(v,ω)=σ](αheading(v,ω)+βdist(v,ω)+

γvelocity(v,ω)]

(5)

heading為方位角評價函數(shù)，表示機(jī)器人在速度(v,ω)時，運(yùn)行至模擬軌跡終端時機(jī)器人與目標(biāo)點之間的角度差距?？刂七^程中，要使機(jī)器人的運(yùn)行方向與目標(biāo)點之間的角度盡量小，朝著目標(biāo)點運(yùn)動。dist表示機(jī)器人在速度(v,ω)時，與障礙物之間的最近距離。如果該軌跡下無障礙物，則將其設(shè)為一常數(shù)。velocity為機(jī)器人在速度(v,ω)時的速度大小評價函數(shù)，使機(jī)器人速度盡量高速運(yùn)動。為了解決各評價函數(shù)的評價標(biāo)準(zhǔn)不同導(dǎo)致評價函數(shù)不連續(xù)的問題，需要對各評價函數(shù)進(jìn)行歸一化處理，σ為平滑函數(shù)α、β、γ為權(quán)重系數(shù)。其最終目的是使機(jī)器人朝著目標(biāo)點避開障礙物,從而以最快的速度運(yùn)行[12]。

4 仿真驗證及機(jī)器人調(diào)試

4.1 仿真驗證

通過MATLAB仿真驗證上述算法的有效性。設(shè)置機(jī)器人的初期狀態(tài)x(m)、y(m)、ayaw(Rad)、v(m/s)、w(rad/s)，分別為0、0、0.5pi、0、0。目標(biāo)點位置為：ggoal=[10,9]。隨機(jī)設(shè)置障礙物位置點：oobstacle=[0,2;1,3;2,4;5,4;5,5;5,6;5,9;8,8;8,9;7,9;6,5;6,3;6,8;6,7;7,4;9,8;9,11;9,6] 沖突判定用的障礙物半徑oobstacleR=0.2。機(jī)器人運(yùn)動學(xué)模型參數(shù)為：最高速度，m/s；最高旋轉(zhuǎn)速度，rad/s；加速度，m/ss；旋轉(zhuǎn)加速度，rad/ss；速度分辨率，m/s；轉(zhuǎn)速分辨率，rad/s。轉(zhuǎn)速分辨率為：KKinematic= [1.0,toRadian(20.0),0.2,toRadian(50.0), 0.01,toRadian(1)]。評價函數(shù)參數(shù)[heading,dist,velocity,predictDT]為：eevalParam= [0.05,0.2,0.1,3.0]。MATLAB仿真運(yùn)行結(jié)果如圖4所示。

圖4 MATLAB仿真運(yùn)行結(jié)果圖Fig.4 MATLAB simulation running result

通過仿真驗證，該方法可以控制機(jī)器人順利地避開各障礙物到達(dá)目標(biāo)點，且生成的路徑平滑。由此證明，該機(jī)器人路徑規(guī)劃方法是有效的。

4.2 機(jī)器人調(diào)試

機(jī)器人平臺由兩個差速電機(jī)和萬向輪組成，控制模塊主要由上位機(jī)、STM32運(yùn)動控制模塊以及激光傳感器等組成，采用鋰電池直流電源供電。STM32運(yùn)動控制模塊主要實現(xiàn)對機(jī)器人電機(jī)的調(diào)速控制、里程計算等；激光傳感器用于機(jī)器人感知現(xiàn)在周圍環(huán)境信息；上位機(jī)采用在Ubuntu中的安裝ROS作為軟件開發(fā)平臺，采用AMCL技術(shù)進(jìn)行即時定位與地圖構(gòu)建，并通過動態(tài)窗口算法實現(xiàn)局部區(qū)域?qū)Ш?。試驗過程在有多個障礙物的實驗室內(nèi)進(jìn)行。在RVIZ界面中，可隨機(jī)設(shè)定機(jī)器人目的點。

5 結(jié)論

本文通過動態(tài)窗口算法模擬仿真驗證算法的有效性，并采用上位機(jī)系統(tǒng)、STM32運(yùn)動控制模塊以及激光傳感器等搭建機(jī)器人系統(tǒng)，實現(xiàn)了該算法在實際機(jī)器人硬件平臺上的運(yùn)用。試驗結(jié)果表明，該方案能夠?qū)崿F(xiàn)機(jī)器人在未知環(huán)境中的地圖構(gòu)建，以及自動壁障自主導(dǎo)航等功能。但在機(jī)器人靠近障礙物時，速度角容易出現(xiàn)較大波動的現(xiàn)象。因此，對于該算法在機(jī)器人控制系統(tǒng)中的實際應(yīng)用還需作進(jìn)一步改進(jìn)研究。