鄭為湊，于振中，惠 晶

(江南大學(xué)輕工過程先進控制教育部重點實驗室，江蘇無錫214122)

路徑規(guī)劃技術(shù)是智能移動機器人研究的核心內(nèi)容之一，其中動態(tài)環(huán)境下移動機器人的路徑規(guī)劃問題是移動機器人研究領(lǐng)域的一個熱點與難點。路徑規(guī)劃算法和環(huán)境感知手段是兩個重要的研究方面。常用的路徑規(guī)劃算法主要有模板匹配路徑規(guī)劃技術(shù)、地圖構(gòu)建規(guī)劃技術(shù)和人工智能路徑規(guī)劃技術(shù)［1-2］。模板匹配方法是將機器人當(dāng)前狀態(tài)與過去經(jīng)歷相比較，比較依賴機器人過去的經(jīng)驗;人工勢場路徑規(guī)劃方法是將機器人在環(huán)境中的運動視為一種機器人在虛擬的人工受力場中的運動，存在局部極小點和計算量較大等特點;地圖構(gòu)建又分為路標(biāo)法和柵格法，存在路徑規(guī)劃的實時性的問題。人工智能路徑規(guī)劃是將現(xiàn)代人工智能技術(shù)應(yīng)用于移動機器人的路徑規(guī)劃中，包括人工神經(jīng)網(wǎng)絡(luò)、進化算法、模糊邏輯與信息融合等。但是人工智能存在學(xué)習(xí)樣本難獲取與學(xué)習(xí)滯后問題;傳統(tǒng)的人工勢場路徑規(guī)劃方法是將機器人在環(huán)境中的運動視為一種機器人在虛擬的人工受力場中的運動，存在局部極小點和目標(biāo)不可達等缺陷［3-5］。文中針對人工勢場在機器人中的應(yīng)用特點，利用改進人工勢場法并使用遺傳信賴域算法求解改進人工勢場的子目標(biāo)點問題，由多個最小點構(gòu)成全局最優(yōu)路徑，實現(xiàn)規(guī)劃最優(yōu)化路徑。

Ravari等人研究了基于視頻攝像機的移動機器人路徑規(guī)劃與導(dǎo)航［6］，但是需要將二維視頻圖像轉(zhuǎn)換為3D模型，計算量大且誤差也較大?；跓o線傳感網(wǎng)絡(luò)的移動機器人定位導(dǎo)航技術(shù)，無線傳感網(wǎng)與移動機器人結(jié)合可以擴展其感知范圍，提供定位及路徑規(guī)劃，拓展機器人的導(dǎo)航能力［7-10］。Kinect是以影像辨識為核心技術(shù)的一種3D體感攝影機，可以每秒捕捉30次RGB圖像，并且還有傳感器負(fù)責(zé)探測力度和深度，結(jié)合2D平面影像攝影與3D深度影像攝影技術(shù)，具備即時動態(tài)捕捉、影像辨識、三維測量、顏色識別等多項功能。微軟公司最近針對Kinect體感器提供了W indows平臺的SDK，采用VC++編程工具可以方便地延伸Kinect的強大功能，為移動機器人系統(tǒng)基于Kinect實時創(chuàng)建三維地形模型提供了良好的技術(shù)支持。Benavidez等人研究基于Kinect的移動機器人的導(dǎo)航和目標(biāo)跟蹤系統(tǒng)，并利用實驗驗證了該系統(tǒng)的可靠性［11］。Smisek等人研究基于Kinect的實時3D建模和目標(biāo)物體定位方法，并對建模和定位的精度進行了定量分析［12］。

文中采用Kinect體感器實時獲取動態(tài)環(huán)境中的3D深度影像與RGB影像信息，為移動機器人實時探測收集局部環(huán)境信息，提高系統(tǒng)對復(fù)雜環(huán)境的感知能力。機器人根據(jù)實時環(huán)境信息的變化重新規(guī)劃和調(diào)整路徑，有效利用局部規(guī)劃，得出更加優(yōu)化的路徑，及時處理所遇到的隨機障礙信息，從而提高移動機器人整體的路徑規(guī)劃性能。其中，路徑規(guī)劃器采用基于遺傳信賴域算法改進的人工勢場法，可以有效提高移動機器人的路徑規(guī)劃效率。

1 機器人運動學(xué)模型

首先建立移動機器人的運動模型，如圖1所示。機器人位置向量由一個三維向量來表示X=［x y θ］T，其中(x，y)為機器人在二維空間的位置坐標(biāo)，θ為機器人相對于X軸的朝向，即速度方向與x軸的夾角。控制移動機器人的參數(shù)主要有2個:速度V與導(dǎo)向角ω。

圖1 機器人運動學(xué)模型Fig.1 Robot kinematic model

由所建立的機器人運動學(xué)模型可得其運動學(xué)方程:

2 Kinect與路徑規(guī)劃

為實現(xiàn)移動機器人在未知動態(tài)環(huán)境中的安全移動，需完成以下任務(wù):地形檢測、障礙物識別、可移動區(qū)定義、路徑規(guī)劃與導(dǎo)航等。Kinect體感器通過內(nèi)置的CMOS紅外攝像機，采用光編碼技術(shù)，無論周圍環(huán)境的光照條件如何，均以每秒30幀速度生成景深圖像流，實時3D地再現(xiàn)周圍環(huán)境，可以為移動機器人完成地形檢測與地圖構(gòu)建，障礙物與目標(biāo)識別，并將這些信息傳遞給計算機，為移動機器人路徑規(guī)劃打下基礎(chǔ)。Kinect與路徑規(guī)劃如圖2所示。

圖2 K inect與路徑規(guī)劃Fig.2 Kinect and path planning

計算機根據(jù)構(gòu)建的地圖與障礙物位置信息，計算出移動機器人、目標(biāo)、障礙物空間點的坐標(biāo)。根據(jù)坐標(biāo)結(jié)果，建立目標(biāo)和障礙物的勢場模型，利用遺傳信賴域算法求解出子目標(biāo)點，由多個子目標(biāo)點最終構(gòu)成全局的優(yōu)化路徑。

3 移動機器人的路徑規(guī)劃算法

3.1 人工勢場法的改進措施

針對傳統(tǒng)人工勢場中的局部極小點和機器人與目標(biāo)障礙物在一條直線上時反復(fù)運動，到達不了目標(biāo)點的缺陷，做出以下改進:

1)將目標(biāo)對機器人的吸引力以及障礙物對機器人的排斥力轉(zhuǎn)化為一種勢場強度，采用計算勢場強度的方法來代替?zhèn)鹘y(tǒng)的矢量力控制。

2)在障礙物的斥力勢中添加系數(shù)項‖XXg‖2，這樣當(dāng)機器人靠近目標(biāo)點時，引力勢減小的同時斥力勢也在減小，直到達到目標(biāo)點;引力勢、斥力勢同時減為0，從而解決了障礙物與目標(biāo)點過近而導(dǎo)致的目標(biāo)不可達問題。

3)對于局部極小點導(dǎo)致的“死鎖”問題，引入“填平勢場”來引導(dǎo)機器人走出局部極小點，即在局部極小點處增加一個附加勢場Uadd。

3.2 改進人工勢場的模型

根據(jù)以上措施可建立人工勢場模型:

1)目標(biāo)點對全方位移動車體的吸引力勢模型如式(2)所示:

式中:ρ(X，Xg)為移動車體中心點當(dāng)前位置與目標(biāo)點之間的距離;k為正比例位置增益系數(shù);X為機器人中心點在運動空間的位置［x，y］T;Xg為目標(biāo)點位置［xgyg］T。

2)第i個靜態(tài)障礙物對全方位移動車體的排斥力勢模型如式(3)所示:

式中:i∈ (1，2，…，n)，n 為靜態(tài)障礙物的總數(shù);ρ(X，Xi)為移動車體中心當(dāng)前位置與第i個障礙物間的最短距離;ρ0為障礙物的有效影響距離;η為正比例位置增益系數(shù)。

3)利用Kinect對動態(tài)環(huán)境的感知能力，由每秒30幀的景深圖像流可以獲得動態(tài)障礙物位置和運動的方向，計算機根據(jù)其位置與運動方向的解算，可以獲得動態(tài)障礙物與機器人的相對運動速度。動態(tài)障礙物是運動的，僅考慮位置并不能完全反應(yīng)環(huán)境信息，把動態(tài)障礙物與機器人的相對速度引入到勢場函數(shù)中，得到第r個動態(tài)障礙物對全方位移動車體的排斥力勢模型如式(4)所示:

式中:r∈ (1，2，…，m)，m 為移動障礙物的總數(shù);ζ為正比例系數(shù);V為移動車體的當(dāng)前運動速度，V∈(2Vmax/3，Vmax);Vr為第r個動態(tài)障礙物的當(dāng)前運動速度;φ為移動車體的當(dāng)前運動方向;φ為第r個動態(tài)障礙物的當(dāng)前運動方向。

4)當(dāng)機器人處于局部極小點時，引入填平勢解決局部極小點問題，填平勢模型如式(5)所示:

式中:ρa為移動車體是否到達目標(biāo)點的評判距離;s為正比例系數(shù)。

全方位移動車體總的勢場強度如式(6)所示;當(dāng)機器人處于局部極小點時由式(6)加上填平勢作為總的勢場強度值如式(7)所示:

基于以上模型，機器人運動過程中每個采樣周期都以機器人可達范圍內(nèi)的勢場強度之和最小的點作為子目標(biāo)點，多個子目標(biāo)點構(gòu)成了全局優(yōu)化路徑。勢場強度之和由式(6)表示。為了避免子目標(biāo)點在局部極小點附近震蕩的情況，采用矢量合成的方法判斷機器人是否處于局部極小點，如果處于局部極小點，勢場強度之和包含填平勢，如式(7)表示。設(shè)機器人的最大速度為Vmax，采樣周期為t0，則機器人在每個采樣周期的可達范圍就是以當(dāng)前位置為中心，Vmaxt0為半徑的一個圓。為了保證機器人運動的平穩(wěn)性和執(zhí)行的效率，機器人運動的速度不宜過大或過小，因此可以在R∈(2Vmaxt0/3，Vmaxt0)，θ∈(0，2π)的環(huán)形區(qū)域內(nèi)選擇子目標(biāo)點。

如圖3所示，圖中的環(huán)形陰影部分即為子目標(biāo)點的可選區(qū)域。圖3中陰影部分的點可以表示為x′=x+R cosθ，y′=y+R sinθ。因此，式(6)和式(7)是關(guān)于變量R和θ的函數(shù)，R∈(2Vmaxt0/3，Vmaxt0)，θ∈(0，2π)。設(shè) z1=x+R cosθ，z2=y+R sinθ，z=(z1，z2)。由式(5)和式(6)可以得出，使用遺傳信賴域算法求解子目標(biāo)點的目標(biāo)函數(shù)如式(8)所示，即求解一類線性約束最優(yōu)化問題，當(dāng)用矢量合成方法判斷出機器人處于局部極小點時式(8)加上填平勢Uadd(z)作為目標(biāo)函數(shù):

圖3 子目標(biāo)點的選擇范圍Fig.3 Choice of the sub-target point

使用二次逼近，構(gòu)造約束信賴域子問題:

式中:gk=▽U(zk);Δk為信賴域半徑;Gk=▽2U(zk)，求解Gk非常復(fù)雜，用擬牛頓法的BFGS公式構(gòu)造Hession矩陣Bk來近似Gk;dk為下降試探步;Ω為R和θ的取值范圍。

算法所涉及符號說明:z∈ R2，zk=(zk1，zk2)，gk+1－ gk，:= 表示賦值BkdkdTkBk/dTkBkdk，實際下降量與預(yù)估下降量的比值為其中在求解用min U)時，應(yīng)用遺傳算法快速求解min U()得到一個優(yōu)于當(dāng)前點的迭代點。從而最終求解出子目標(biāo)點得到變量R和θ。計算機根據(jù)變量R和θ計算出最終的移動機器人的線速度V和導(dǎo)向角ω，從而達到移動機器人的路徑規(guī)劃。

4 實驗與分析

4.1 基于K inect的移動機器人路徑規(guī)劃體系結(jié)構(gòu)

基于Kinect的移動機器人控制體系結(jié)構(gòu)主要由Kinect體感器、計算機、移動機器人3部分組成。路徑規(guī)劃體系結(jié)構(gòu)如圖4所示。

圖4 基于Kinect的移動機器人控制框架Fig.4 Framework of K inect-based mobile robot control

利用Kinect體感器產(chǎn)生的RGB圖像和3D圖像實時探測獲取移動機器人的周邊動態(tài)環(huán)境，實現(xiàn)對動態(tài)環(huán)境中障礙物和地形的感知。計算機將Kinect傳遞來的3D深度影像、距離傳感器、RGB圖像的原始數(shù)據(jù)流進行分析處理，實現(xiàn)對目標(biāo)和障礙物的識別與定位。RGB圖像與3D深度圖像提供了移動機器人周邊動態(tài)環(huán)境的特征，包括障礙物的大小、遠近、顏色等特征。利用SLAM算法或者VFH/VFH+算法［5］實現(xiàn)機器人在未知環(huán)境下的自我定位和目標(biāo)定位。通過對目標(biāo)與機器人的自身定位，獲得目標(biāo)、障礙物及機器人的位置坐標(biāo)，構(gòu)建改進的人工勢場，并利用遺傳信賴域算法來求解子目標(biāo)點，通過多個子目標(biāo)點構(gòu)成全局的最優(yōu)路徑。移動機器人從計算機中獲得運動控制量:運動速度V和導(dǎo)向角速度ω，來調(diào)整自己的運動狀態(tài)及運動姿勢，實現(xiàn)在動態(tài)環(huán)境下的無碰運動。

4.2 實驗驗證

為了驗證該體系結(jié)構(gòu)的有效性，利用現(xiàn)有條件:IRobot機器人，Microsoft Kinect體感器，計算機，動態(tài)障礙物，靜態(tài)障礙物，構(gòu)建有效的實驗系統(tǒng)，設(shè)計路徑規(guī)劃實驗。實驗場景如圖5所示。

選取算法的初始參數(shù)為:Δ0=0.5U(z0)，ε1=0.1，ε2=0.03，ε3=0.05，a=b=0.5，M=1.5，η1=0.15，η2=0.3，B0=I2×2，β1=0.35，β2=0.75，β3=1.25，N=20，Pc=0.99，Pm=0.05，Tmax=100，κ =0.1，編 碼 長 度 l=32。Vmax=0.3 m/s，t0=3 s，z0=(5Vmaxt0/6，0)，k=1，η =2，ζ =0.1，n=2，m=2，s=0.3，φ = π/2，ρ0=1 m，ρa=0.15 m，機器人起始點(1.00，0.25)，目標(biāo)點(1.35，3.65)，單位為 m。根據(jù)建立的坐標(biāo)系，計算機記錄移動機器人在運動過程中的多個典型點，多個點擬合的路徑曲線如圖6所示。運動軌跡上典型點的勢場強度和算法執(zhí)行時間如表1所示。

圖5 實驗場景Fig.5 Experimental scene

圖6 移動機器人的運動軌跡Fig.6 Trajectory of themobile robot

在實驗中從移動機器人的移動軌跡分析可見，移動機器人的行為具備了良好的穩(wěn)定性、一致性與連續(xù)性。改進人工勢場模型與遺傳算法的結(jié)合應(yīng)用可以克服傳統(tǒng)人工勢場法的缺點，并且在求解子目標(biāo)點時具有較快的收斂速度。從實驗中可以看出，移動機器人可以快速避開靜態(tài)與動態(tài)的障礙物，無碰撞地快速到達目標(biāo)位置。該體系結(jié)構(gòu)能夠較好地實現(xiàn)移動機器人在動態(tài)環(huán)境下的路徑規(guī)劃任務(wù)。

5 結(jié)語

文中提出了一種基于最新體視覺傳感器Kinect的移動機器人動態(tài)環(huán)境下的路徑規(guī)劃方法。利用Kinect體感器實時精準(zhǔn)獲取當(dāng)前的三維地形信息，可以高效感知移動機器人周邊的動態(tài)環(huán)境信息，采用基于遺傳信賴域的改進人工勢場法，克服傳統(tǒng)人工勢場法的缺陷，實現(xiàn)優(yōu)化的路徑規(guī)劃。由實驗驗證了基于Kinect的移動機器人系統(tǒng)具有良好的穩(wěn)定性與實用性，能夠較好地滿足實時控制，完成移動機器人在動態(tài)環(huán)境下的路徑規(guī)劃任務(wù)，為移動機器人的路徑規(guī)劃提出了一條新的思路。

表1 運動軌跡上典型點執(zhí)行時間和勢場強度Tab.1 Typical point on the trajectory execution time and potential field strength

［1］朱大奇，顏明重.移動機器人路徑規(guī)劃技術(shù)綜述［J］.控制與決策，2010，25(7):961-967.ZHU Daqi，YAN Mingzhong.Mobile robot path planning technology review［J］.Control and Decision，2010，25(7):961-967.(in Chinese)

［2］周蘭鳳，徐芳.一種考慮不確定性的移動機器人路徑規(guī)劃方法［J］.微電子學(xué)與計算機，2010，27(7):86-89.ZHOU Lanfang，XU Fang.Account of uncertaintymobile robot path planning［J］.Microelectronics and Computer，2010，27(7):86-89.(in Chinese)

［3］陳立彬，尤波.基于改進人工勢場法的機器人動態(tài)追蹤與避障［J］.控制理論與應(yīng)用，2007，26(4):8-10.CHEN Libin，YOU Bo.Dynamic tracking and obstacle avoidance robot based improved artificial potential field method［J］.Control Theory and Applications，2007，26(4):8-10.(in Chinese)

［4］況菲，王耀南.基于混合人工勢場－遺傳算法的移動機器人路徑規(guī)劃仿真研究［J］.系統(tǒng)仿真學(xué)報，2006，18(3):774-777.KUANG Fei，WANG Yaonan.Hybrid artificial potential field-simulation of mobile robot path planning GA［J］.System Simulation，2006，18(3):774-777.(in Chinese)

［5］Park M，Jeon J，Lee M.Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing［C］//Proceedings of the 2001 IEEE International Symposium on Industrial Electronics.Pusan:Institute of Electrical and Electronics Engineers，2001:1530-1535.

［6］Ravari A R N，Taghirad H D，Tam jidi A H.vision-based fuzzy navigation of mobile robots in grassland environments［C］//Proceeding of International Coference on Advanced IntelligentMechatronics.［S.l.］:IEEE/ASME Press，2009:1441-1446.

［7］薛晗，陶溢，馬宏緒.基于無線傳感器網(wǎng)絡(luò)的未知環(huán)境下移動機器人實時路徑規(guī)劃［J］.計算機應(yīng)用研究，2008，25(7):2029-2032.XUE Han，TAO Yi，MA Hongxu.Real-time path planning formobile robotbased on wireless sensor networks in unknown environment［J］.Application Research of Computers，2008，25(7):2029-2032.(in Chinese)

［8］朱齊丹，吳葉斌.基于無線傳感器網(wǎng)絡(luò)的移動機器人路徑規(guī)劃［J］.華中科技大學(xué)學(xué)報:自然科學(xué)版，2010，38(12):113-116.ZHU Qidan，WU Yebin.Mobile robot path planning based on wireless sensor networks［J］.Journal of Huazhong University of Science and Technology:Natural Science Edition，2010，38(12):113-116.(in Chinese)

［9］李文鋒.無線傳感器網(wǎng)絡(luò)與移動機器人控制［M］.北京:科學(xué)出版社，2009.

［10］Jang-Ping Sheu，Prasan Kumar Sahoo.Efficient path planning and data gathering protocols for the wireless sensor network［J］.Computer Communication，2010，33(5):398-408.

［11］Patrick Benavidez.Mobile robot navigation and target tracking system［C］//International Conference on Systems Engineering.Albuquerque，New Mexico:［s.n.］，2011:299-304.

［12］Jan Smisek，Michal Jancosek，Tomas Pajdla.3D with kinect［C］//IEEE International Conference on Computer Vision Workshops.Barcelona，Spain:［s.n.］，2011:1154-1160.