楊明輝，吳垚，張勇，肖曉暉

(武漢大學(xué)動力與機械學(xué)院，湖北武漢，430072)

在未知工作環(huán)境中，移動機器人應(yīng)該具備感知環(huán)境的能力以避開障礙物、實現(xiàn)自動導(dǎo)航[1]，其中障礙物包括靜態(tài)障礙物和動態(tài)障礙物。目前，基于多傳感器信息融合的避障方法已經(jīng)被提出，可以分為傳統(tǒng)路徑規(guī)劃方法和智能控制方法。在傳統(tǒng)方法中，人工勢場法和向量場直方圖法(VFH)應(yīng)用最為廣泛。人工勢場法原理簡單，易于實現(xiàn)，但其在路徑規(guī)劃時存在目標不可達和局部極小值問題。GUO 等[2-3]通過改進勢場函數(shù)以消除局部極小點，但在動態(tài)環(huán)境下，仍會產(chǎn)生大量無謂的避碰運動[4]。ULRICH等[5-6]在VFH算法的基礎(chǔ)上提出了VFH+算法和VFH*算法。ZHANG等[7]提出了VFH#算法，該算法考慮動態(tài)障礙物的速度信息以適應(yīng)動態(tài)環(huán)境。ZHU 等[8]在VFH 法基礎(chǔ)上提出速度判斷規(guī)則，在面對動態(tài)障礙物時選出最合適的速度方向。但該系列方法環(huán)境信息存儲量大，且容易陷入循環(huán)震蕩。智能控制方法中主要有蟻群算法、遺傳算法、神經(jīng)網(wǎng)絡(luò)和模糊控制。CAI 等[9]提出一種蟻群算法與模糊控制技術(shù)結(jié)合的方式，柳長安等[10]提出一種基于改進蟻群算法的移動機器人動態(tài)路徑規(guī)劃方法。周蘭鳳等[11]提出了一種基于知識的遺傳算法，利用專門的遺傳算子對參數(shù)進行自適應(yīng)調(diào)整，并且把環(huán)境信息整合到種群初始化中。劉旭紅等[12]提出了一種可變長染色體編碼方式，并且把地圖網(wǎng)格化，在實驗中算法能夠在靜態(tài)和動態(tài)環(huán)境下搜索到優(yōu)秀的無碰撞路徑。YANG 等[13]開發(fā)了一種分層模糊控制策略，實現(xiàn)了機器人在未知環(huán)境導(dǎo)航。喬俊飛等[14]設(shè)計了基于動態(tài)神經(jīng)網(wǎng)絡(luò)的移動機器人導(dǎo)航算法并在Pioneer3-DX機器人上驗證。ZHU等[15]提出了一種基于神經(jīng)網(wǎng)絡(luò)的學(xué)習(xí)算法來調(diào)節(jié)模糊邏輯系統(tǒng)的隸屬度函數(shù)參數(shù)，提高了移動機器人的軌跡平滑度。錢夔等[16]采用模糊神經(jīng)網(wǎng)絡(luò)控制，實現(xiàn)了移動機器人在未知環(huán)境中的自主避障，并采用虛目標法逃離局部陷阱?？琢钗牡萚17]采用閉環(huán)模糊神經(jīng)網(wǎng)絡(luò)有效縮短了移動機器人的行進路徑并提高行進速度。其中蟻群算法和遺傳算法都能實現(xiàn)動態(tài)環(huán)境自主避障，但此類方法涉及到進化迭代，計算量大，實時性較差。模糊神經(jīng)網(wǎng)絡(luò)方法融合了模糊控制和神經(jīng)網(wǎng)絡(luò)的優(yōu)點，在靜態(tài)障礙物環(huán)境中可實現(xiàn)無碰避障，但面對動態(tài)障礙物時會產(chǎn)生冗余路徑甚至發(fā)生碰撞[18]。因此，本文作者在模糊神經(jīng)網(wǎng)絡(luò)的基礎(chǔ)上提出一種動態(tài)障礙物避障方法，以期實現(xiàn)在動態(tài)環(huán)境中的無碰避障，提升機器人行進速度。

1 機器人避障模型

本文采用兩輪差動式移動平臺，機器人的避障模型如圖1所示。

設(shè)機器人在k-1時刻的坐標為Xk-1=[xk-1,yk-1]T，在k時刻的機器人坐標為Xk=[xk,yk]T，目標點坐標為[x,y]T。k時刻機器人坐標與目標點坐標的夾角用目標航向角θE(k)表示：

式中：θE(k)∈( -180°,180°]。

圖1 移動機器人避障模型Fig.1 Mobile robot obstacle avoidance model

k時刻移動機器人的航向角設(shè)為θ(k)，目標角度ta(k)為k時刻機器人目標航向角θE(k)與當前航向角的夾角：

式中：θE(k)可由超寬帶系統(tǒng)(UWB)得的定位信息計算得到，θ(k)由高精度轉(zhuǎn)角儀測得。

2 動態(tài)障礙物避障策略

在面對動態(tài)障礙物時，當機器人不能正確判斷障礙物運動信息時，可能產(chǎn)生冗余路徑，甚至與障礙物發(fā)生碰撞。動態(tài)障礙物碰撞場景示意圖如圖2所示。若機器人采用優(yōu)先右轉(zhuǎn)原則，則在面對向右運動的動態(tài)障礙物時不能及時避開障礙物，與障礙物發(fā)生碰撞，避障失敗。

為適應(yīng)動態(tài)障礙物環(huán)境，本文提出一種動態(tài)障礙物避障策略，包括3個部分：動態(tài)障礙物運動方向判斷策略以判斷障礙物的運動軌跡、子目標點更新策略以優(yōu)化機器人避障路徑以及模糊神經(jīng)網(wǎng)絡(luò)輸出機器人轉(zhuǎn)角控制量和速度控制量。

圖2 動態(tài)障礙物碰撞場景示意圖Fig.2 Diagram of dynamic obstacle collision scene

2.1 動態(tài)障礙物方向判斷策略

在運動過程中，動態(tài)障礙物的運動方向與機器人的運動方向存在相交和平行2 種情況。圖3所示為2種情況下移動機器人在運動過程中和動態(tài)障礙物可能發(fā)生碰撞場景中的位置信息，其中r為機器人與障礙物的碰撞區(qū)域半徑。本文對動態(tài)障礙物假設(shè)如下：障礙物的運動方向不變且在機器人探測范圍內(nèi)，障礙物運動速度恒定；障礙物的大小已知。

移動機器人保存連續(xù)2個規(guī)劃周期的動態(tài)障礙物與機器人相對位置信息。定義d′x為當前周期障礙物與機器人直線運動軌跡的距離，dx為上一周期障礙物與機器人直線運動軌跡的距離，其計算公式為

式中：θ1,θ2,θ3,θ4和θ5分別對應(yīng)5個超聲波傳感器的掃描角度；d1,d2,d3,d4和d5分別對應(yīng)5個超聲波傳感器檢測到的障礙物距離。同時采用5個超聲波數(shù)據(jù)對障礙物方向與距離進行判斷，可以減少單個超聲波傳感器因為廣角帶來的誤差。

由式(3)可得

式中：td為障礙物運動方向，v為移動機器人運動速度，dt為檢測周期，δ為設(shè)定的距離閾值。td=0 表示障礙物正在遠離機器人，不做避障處理；td=-1 表示障礙物相對機器人向左運動；td=1表示障礙物相對機器人向右運動。在2個規(guī)劃周期內(nèi)，同一個障礙物到移動機器人運動軌跡的距離發(fā)生變化，即判斷為動態(tài)障礙物并標記運動方向。

2.2 子目標點更新策略

傳統(tǒng)的模糊神經(jīng)網(wǎng)絡(luò)避障策略在檢測到障礙物時會對機器人的運行速度進行調(diào)整，距離障礙物越近，運行速度越慢[17]。本文引入子目標點更新策略，在機器人運動過程中，檢測到障礙物后實時更新子目標點，暫時隱藏終目標點，到達子目標點之后再將目標點更新為終目標點，最終得到的機器人的運動軌跡將會通過多個子目標點和最終目標點。更新子目標點之后，控制器的輸入目標角度ta獲得更新，控制器輸出機器人速度控制量不變，機器人不減速到達子目標點，縮短機器人運動時間。

移動機器人在檢測到障礙物之后控制器可以根據(jù)2個周期的障礙物位置信息估計障礙物的運動方向與運動速度(見圖3)，系統(tǒng)可以預(yù)測機器人和障礙物在下幾個周期的位置信息，若預(yù)期到障礙物的位置會進入碰撞區(qū)域，則認為碰撞可能發(fā)生，更新子目標點。碰撞區(qū)域半徑為機器人半徑和障礙物半徑之和。碰撞區(qū)域的圓心是預(yù)計碰撞發(fā)生時障礙物的位置。子目標點可以通過式(5)確定，偏轉(zhuǎn)方向由障礙物的運動方向確定。

圖3 動態(tài)障礙物檢測及方向判斷Fig.3 Dynamic obstacle detection and direction judgment

式中：Rr為機器人半徑，Ro為障礙物半徑。

2.3 模糊神經(jīng)網(wǎng)絡(luò)

本文采用T-S型模糊神經(jīng)網(wǎng)絡(luò)控制器來完成輸入/輸出關(guān)系的映射，并采用反向傳播(back propagation)算法和最小二乘法來完成對輸入/輸出數(shù)據(jù)對的建模，使得設(shè)計出來的模糊神經(jīng)網(wǎng)絡(luò)系統(tǒng)能夠更好地模擬出希望的或是實際的輸入/輸出關(guān)系[19]。本文設(shè)計一個7輸入2輸出的模糊神經(jīng)網(wǎng)絡(luò)，分為5層，其簡化結(jié)構(gòu)示意圖如圖4所示。

圖4 模糊神經(jīng)網(wǎng)絡(luò)簡化結(jié)構(gòu)示意圖Fig.4 Simplified structural diagram of fuzzy neural network

模糊神經(jīng)網(wǎng)絡(luò)的第1 層為輸入層。它的每個節(jié)點直接與輸入向量的各分量連接，將輸入值傳送到下一層。本文該層的節(jié)點數(shù)n=7，輸入狀態(tài)空間向量I為

第2 層每個節(jié)點代表1個語言變量。本文將輸入障礙物距離信息d1，d2，d3，d4和d5分 為N(近)和F(遠)2個等級；將目標角度ta分為5個等級(RB，RS，Z，LS，LB)，這5 個等級分別代表機器人目標角度信息(偏左大、偏左小、不偏、偏右小、偏右大)；將障礙物運動方向td分為2個等級即L(向左)和R(向右)。該層的作用是計算各輸入分量屬于各語言變量模糊集合的隸屬函數(shù)μji：

式中：xi是輸入狀態(tài)空間向量元素，i= 1,2,…,n;j=1,2,…,mi；n為輸入量的位數(shù)；mi為xi的模糊分割數(shù)。本文采用矩形函數(shù)rect(·)作為隸屬度函數(shù)。

第3 層的每個節(jié)點代表1條模糊規(guī)則，用來匹配模糊規(guī)則的前件，計算出每條規(guī)則的適應(yīng)度αj：

式 中：i1∈{1,2,…,m1};i2∈{1,2,…,m2};in∈{1,2,…,mn};j= 1,2,…,m,

第4 層的節(jié)點數(shù)與第3 層的相同，實現(xiàn)歸一化計算，αj歸一化結(jié)果為

式中：j= 1,2,…,m。

第5 層為輸出層，本文選用加權(quán)平均法去模糊化。輸出量為機器人的轉(zhuǎn)角控制量w和速度控制量v，即：

根據(jù)上述避障策略，融合障礙物運動方向判斷策略和子目標點更新策略的避障算法流程如圖5所示。

圖5 動態(tài)環(huán)境避障算法流程Fig.5 Algorithm of obstacle avoidance in dynamic environment

3 試驗與分析

3.1 試驗平臺與試驗環(huán)境

本文使用Turtlebot3 waffle作為移動平臺，配置5路超聲波傳感器檢測障礙物距離，超聲波傳感器測量角度為15°。5 路超聲波可以探測移動機器人行進方向跨度135°范圍內(nèi)的障礙物邊界信息，探測精度為0.3 cm±1%。為避免超聲波傳感器相互干擾，在探測障礙物距離信息時，超聲波傳感器依次循環(huán)啟動，任何緊鄰的傳感器不會同時工作。

為讓移動機器人具備位置信息的感知能力，差速移動平臺采用UWB和高精度轉(zhuǎn)角儀作為航跡推算定位的傳感器來獲取自身的位置與姿態(tài)。移動機器人如圖6所示。

圖6 移動機器人試驗平臺Fig.6 Mobile robot test platform

基于所提的動態(tài)障礙物避障控制算法，為試驗平臺設(shè)計移動機器人自主避障控制系統(tǒng)，系統(tǒng)結(jié)構(gòu)如圖7所示。該控制系統(tǒng)主要包括信息采集、避障策略和運動控制3個部分。

試驗環(huán)境為寬7.5 m，長9.0 m的室內(nèi)大廳，設(shè)置移動機器人起始點坐標為(4 m，1 m)，起始航向角為0°，目標點坐標為(4 m，6 m)，控制器輸入為超聲波傳感器測得的障礙物距離信息[d1,d2,d3,d4,d5]T、目標角度ta以及障礙物的運動方向判斷td，以機器人獲取的UWB實時坐標為機器人坐標，每個運動周期輸出1次機器人坐標得到機器人的運動軌跡。

動態(tài)障礙物為大廳中移動的行人，在機器人運行過程中，控制系統(tǒng)根據(jù)式(3)和式(4)對障礙物運動方向進行判斷。

圖7 控制系統(tǒng)結(jié)構(gòu)示意圖Fig.7 Structural diagram of control system

3.2 試驗結(jié)果分析

面對動態(tài)障礙物與移動機器人運動軌跡相交時，機器人運動軌跡和判斷結(jié)果如圖8所示。

圖8 動態(tài)障礙物與機器人運動軌跡相交判斷結(jié)果Fig.8 Predicting result of dynamic obstacles and robot trajectories in condition of intersection

由圖8可以看出：當動態(tài)障礙物向右運動時，機器人對向右運動的障礙物做出了正確判斷并成功繞開了移動障礙物。當動態(tài)障礙物向左運動時，盡管機器人在對動態(tài)障礙物做方向判斷時出現(xiàn)了誤判，但機器人對連續(xù)2個周期的判斷結(jié)果進行對比，若連續(xù)2個周期的判斷結(jié)果相同，控制器才會做出相應(yīng)決策，所以只在1 個周期出現(xiàn)誤判不會影響機器人的運動軌跡，機器人成功繞開了障礙物。動態(tài)障礙物與機器人運動軌跡相交試驗結(jié)果如圖9所示。

圖9 動態(tài)障礙物與機器人運動軌跡相交試驗結(jié)果Fig.9 Experimental result of dynamic obstacles and robot trajectories in condition of intersection

由圖9可知：加入動態(tài)障礙物運動方向判斷之后，機器人可以準確判斷動態(tài)障礙物的運動方向，并對比連續(xù)2個周期的判斷結(jié)果，糾正可能出現(xiàn)的個別周期的運動方向誤判，有效繞開障礙物，實現(xiàn)無碰撞避障，最終到達指定目標點。

面對與移動機器人平行運動動態(tài)障礙物時，根據(jù)式(5)以及是否更新子目標點，機器人運動軌跡與速度曲線如圖10所示。從圖10可以看出：有子目標點更新策略時，機器人避障路徑更短，且行進速度始終保持高速，不會因為避障行為的發(fā)生而減速。其試驗結(jié)果如圖11所示。

圖10 機器人運動軌跡與速度曲線Fig.10 Robot trajectories and velocity curves

圖11 動態(tài)障礙物與機器人運動軌跡平行試驗結(jié)果Fig.11 Experimental result of dynamic obstacles and robot trajectories in condition of parallel

機器人平均行進速度如表1所示。由表1可以看出：當機器人左偏時，在有子目標點策略下，機器人平均行進速度為47.36 cm/s，而在無子目標點策略時，機器人平均行進速度為44.61 cm/s，可見：在有子目標點策略下，機器人平均行進速度提升6.19%。當機器人右偏時，在有子目標點策略下，機器人平均行進速度為47.16 cm/s，而在無子目標點策略時，機器人平均行進速度是42.20 cm/s，可見：在有子目標點策略下，機器人平均行進速度提升11.75%。

表1 機器人平均行進速度Table1 Average moving speed of robot cm/s

4 結(jié)論

1)提出一種動態(tài)障礙物避障算法，該算法包括動態(tài)障礙物運動方向判斷策略、子目標更新策略以及模糊神經(jīng)網(wǎng)絡(luò)3個部分。

2)根據(jù)所提算法設(shè)計的控制器可以在未知動態(tài)環(huán)境中判斷動態(tài)障礙物的運動方向并更新子目標點以完成無碰避障；與無子目標點更新策略相比，在有子目標點策略下，機器人平均行進速度提升了11.75%，驗證了所設(shè)計控制器的有效性和可行性。