譚寶成,崔佳超
(西安工業(yè)大學 電子信息工程學院,西安710021)
目前,無人駕駛技術在智能車領域中是國內外的研究的重點課題,其中避障算法是研究對象中的熱點和難點,并且在國內外都取得一定的成果,常用的避障算法有:人工勢場法[1-3]、遺傳算法[4]、柵格法、蟻群算法[5-6]等.遺傳算法的收斂速度和收斂效率不但決定于種群的數(shù)量,而且也受到交叉概率和變異概率的影響;蟻群算法的搜素最優(yōu)路徑的時間長短受到蟻群數(shù)量和外界環(huán)境障礙物的數(shù)量影響.人工勢場法是由Khatb提出的一種虛擬力法,與遺傳算法和蟻群算法相比,其優(yōu)點為算法簡明,實時性良好,規(guī)劃路徑軌跡圓滑,適合無人駕駛智能車領域.但存在容易進入局部最小點,導致不能達到目標點問題[7].為了解決這一問題,大量學者展開研究,于振中[8]等人提出了添加填平勢場法.石為人[9]等人研究了連接區(qū)域障礙物法來避免局部最小點給系統(tǒng)造成的影響.但在結構化高速公路環(huán)境下,這些方法存在以下問題:① 斥力場范圍過大,無人車在非碰撞路徑上會受到其它路徑上的斥力場作用;② 斥力場范圍過小,無人車避障時存在角度跳變大.這些問題限制了以上方法在結構化道路環(huán)境中的應用.
針對人工勢場法在結構化高速公路環(huán)境應用的難題,本文通過建立汽車拐彎模型,分析人工勢場模型,提出改變勢場作用范圍建立新的人工勢場模型.通過對障礙物進行分類,改造障礙物虛擬排斥力的作用條件,生成動態(tài)障礙物與虛擬引力勢場,提出了改進人工勢場避障算法,以期實現(xiàn)智能車在結構化高速公路環(huán)境中小角度、大半徑避障.
無人駕駛車輛拐彎模型如圖1所示,給定車體前輪一個角度,則車體以速度v圍繞圓心O以一定半徑d做勻速圓弧運動,時間t內車的質心走過的弧長記為S,圓心角為β,車輛的輪胎與中心軸的夾角問為a,中心軸長度為L,懸臂長為l,則車體避障時滿足關系式為
圖1 無人駕駛車輛拐彎模型Fig.1 The mode of unmanned vehicle in turning conditions
無人駕駛智能車車體模型如圖2所示,INF1、INF2、INF3為紅外測距傳感器,主要測量車輛側向和后向的的障礙物距離,GPS通過衛(wèi)星可獲得車輛的地球坐標和航向,在沒有障礙物的情況下,車輛可以以按照預先規(guī)劃好的路徑進行行駛,LADAR為激光雷達,掃描角度為180°,掃描距離為200m,通過與圖像傳感器CCD的數(shù)據(jù)融合,可獲得前方障礙物的極坐標(ρ,θ),在忽略障礙物的高度情況下,以激光雷達為原點的局部直角坐標系中,障礙物坐標為(xo,yo),其中xo=ρ×cosθ,yo=ρ×sinθ,將障礙物的局部坐標轉換為大地坐標(Xo,Yo)為
式中:Xc,Yc為無人車在地球坐標系中的坐標;ψ為無人車輛的航向.
圖2 無人車車體模型Fig.2 Unmanned vehicle body model
文中在傳統(tǒng)人工勢場上進行改進,將傳統(tǒng)的障礙物圓形作用區(qū)域改造成高速公路環(huán)境下的長方形作用區(qū)域,然后給出虛擬動態(tài)目標點,在障礙物和虛擬目標點的共同作用下規(guī)劃車輛的運動方向.
傳統(tǒng)的障礙物的虛擬力場如圖3所示,以障礙物的的中心為原點,半徑為r的圓所包含的范圍為虛擬力場,不同的虛擬力場半徑r在結構化高速公路中影響車輛的避障路徑的規(guī)劃,障礙物OBSTACLE的虛擬力場作用半徑r取的過小,車輛沒有足夠的避障距離,從而導致車輛在小距離的危險避障環(huán)境中出現(xiàn)大角度,快速的降低速度進行避障;如果障礙物虛擬力場作用半徑r取的過大可以讓無人駕駛車輛提前進行避障區(qū)域,但是大范圍的斥力場覆蓋高速公路中的無碰路徑點,在無碰路徑點無人車會進行沒有必要的方向調節(jié),這不符合人類的駕駛習慣,也會給其他車輛的安全行駛帶來隱患.
圖3 傳統(tǒng)人工勢場模型Fig.3 Traditional artificial potential field model
如果將勢場作用區(qū)域進行劃份,讓無人車行駛方向上的勢場作用距離較大,使無人車提前進行修正方向,進行避障;讓無人車行駛方向垂直方向上的勢場作用距離較小,使無人車快速逃出避障區(qū)域.改進后的人工勢場可以使無人車進行小角度平穩(wěn)進行路徑規(guī)劃,在無碰區(qū)域也不會出現(xiàn)沒必要的方向調節(jié).改造后的勢場作用域模型如圖4所示,在Y軸方向上,無人車受到障礙物的虛擬勢場的最小距離為D;在X軸方向上,無人車多受到的障礙物的虛擬勢場距離為d.
圖4 改進后的人工勢場模型Fig.4 The improved artificial potential field model
根據(jù)無人車行駛時,無人車與障礙物的距離,將障礙物分為需緊急避障的障礙物和安全避障的障礙物.需緊急避障的障礙物是指無人車突然發(fā)現(xiàn)前方車輛正從其它車道側向駛向智能車所在的無障礙車道時,并且滿足無人車和駛入車輛實際距離小于經(jīng)過計算處理安全避障距離D,此時應該進行緊急避障,緊急避障的方法是減速或者剎車.如果智能人車的實際距離大于經(jīng)過計算處理安全避障距離D,將其視為安全避障車輛.安全避障的方法采取改進后的人工勢場.最終確保智能車安全脫離障礙物.根據(jù)無人車行駛時遇到的兩類的障礙物決策無人車采取變向避障還是減速避障.
遇到前方障礙物時,無人駕駛車輛通過調節(jié)前輪與車軸的夾角a進行避障,最大夾角a決定了無人車的最小拐彎半徑和Y軸方向的安全避障距離Do[10],Do和角度a的關系為
其中Dx為無人車避障時在X方向上的安全距離.
無人車輛正常進行路徑規(guī)劃時,非緊急情況下,方向轉角不能總是以最大的角度進行避障,這也符合人類駕駛習慣,所以避障據(jù)離Do應該考慮到無人車速度vc和障礙物與無人車的相對速度的vR的約束,則避障距離D的修正公式為
式中:ζ為修正因子,取1~1.5.
當vR>0時,說明無人車的速度小于障礙物的速度,無人車和障礙物的距離在逐漸變大,此時避障距離D=0,當vR<0時,無人車速度大于障礙物速度,此時無人車和障礙物之間的距離正在縮小,因此可得
無人車在預設路經(jīng)上行駛時,遇到障礙物,脫離預設路徑,進行避障,此時系統(tǒng)在安全車道上生成局部虛擬目標點,牽引無人車輛脫駛向安全區(qū)域,如圖4所示,局部虛擬目標點在無障礙物安全區(qū)域的路徑中央,目標點相對于無人車的Y軸方向距離為D+H/2.
當確定了人工勢場作用域后,進而可以對人工勢場在結構化道路環(huán)境下的模型進行改造,當無人車遇到障礙物時,虛擬目標點是系統(tǒng)根據(jù)安全需要自動生成,虛擬目標點的作用范圍是全局的,屬于靜態(tài)目標點,因此,目標點的引力勢函數(shù)為
我感覺,梁實秋先生這段接地氣的話,把“自控力”這個原本抽象的概念,很通俗易懂地表達了出來:克制,是為了更好的自己。
式中:Ka為引力勢場常量;Qc為無人車地球坐標系位置向量;Qg為地球坐標系中目標點的位置向量.
該引力場對無人車產生的引力為引力勢能的負梯度,得到引力公式為
在行駛過程中,無人車的行駛方向為無人車局部坐標的Y軸,無人車通過自身傳感器獲得障礙物的位置信息和速度信息,在第k時刻,無人車行駛方向道路上無障礙車輛,在k+1時刻,無人車發(fā)現(xiàn)前方障礙物車輛,即障礙物在局部坐標中的橫坐標滿足|x|<d,無人車檢檢測到行駛方向的車輛的實際Y軸距離大于計算出的安全距離D,無人車行駛到距障礙物距離為D處開始進行避障,則安全避障的斥力函數(shù)的最大范圍為D,這種情況下的斥力場函數(shù)改進為
對斥力場函數(shù)求負梯度得到安全避障時無人車所受到的斥力函數(shù)為
在目標點的虛擬引力場和障礙車輛的虛擬斥力場的共同作用條件下,智能車避開障礙物,趨向目標點,最中達到避障.F的向量大小控制車的速度,F(xiàn)的方向控制真能車的避障方向盤角度.
如果在行駛過程中,遇到緊急障礙物,在k+1時刻,無人車發(fā)現(xiàn)前方障礙物車輛,并且計算獲得安全避障距離大于無人車與障礙物的實際距離,不足夠勢場法避障條件,應該立即進行減速或者制動剎車等控制決策.
在地球坐標系中,假設無人車的坐標Qc(xc,yc),虛擬目標點坐標Qg(xg,yg),第i個障礙物的坐標為Qci(xci,yci),則無人車與目標點的夾角θg,與第i個障礙物的夾角θoi計算為
由引力勢場與斥力場合力所決定的無人車的航向角為
其中n為障礙物總數(shù).
人工勢場的避障模型仿真采用VB6.0,圖5為傳統(tǒng)人工勢場的避障模型,用紅色圓圈區(qū)域分別為5個動態(tài)障礙物obs的斥力范圍,綠色線條為無人車避障路徑,紅色點為目標點,在傳統(tǒng)模型中,無碰路徑上A車道也存在障礙物的斥力場,不必要的障礙物斥力范圍相互疊加,作用于無人車,讓無人車進行不必要的避障策略;圖6為改進后的人工勢場避障模型,智能車根據(jù)自身速度結合與障礙物的相對速度計算出不同障礙物的最大安全避障距離D,延無人車行駛方向的X軸上,斥力場的作用距離為D=50m,垂直與車道的Y軸上的斥力腸作用距離為d=20m,智能車在B道上受到收到障礙物與目標點的勢力場的和的作用駛入A車道,d>20m時,只受目標點的引力場作用,從而避免了不必要的障礙物的斥力場的作用.
圖5 傳統(tǒng)人工勢場避障模型輸出Fig.5 Avoidance model simulation of traditional artificial potential field
為了對比傳統(tǒng)方法與改進后的勢場法在無人車避障中的角度的變得化率大小,文中在VB仿真中將避障中無人車調節(jié)航向的直線斜率記錄在text文件中,再運用Matlab的繪圖函數(shù)將數(shù)據(jù)繪制成避障時刻T與斜率的二維圖,如圖7所示,其中橫坐標步數(shù)表示第幾個時間T,縱坐標表示在時間T時刻智能車的調節(jié)方向直線的斜率.綠色線條為傳統(tǒng)避障的數(shù)據(jù),黑色線條為改進后的避障數(shù)據(jù),通過途中數(shù)據(jù)可知,傳統(tǒng)避障模式中角度調節(jié)變化較大,調節(jié)時間較長;改進后的避障模型中,角度變化緩慢,跳變較小,無人車脫離斥力場快.
圖6 改進后的人工勢場避障模型輸出Fig.6 Avoidance model simulation of improved artificial potential field
圖7 斜率對照圖Fig.7 Comparison of slope
文中通過改造傳統(tǒng)障礙物斥力場的圓形作用域,調節(jié)障礙物勢場作用范圍,對傳統(tǒng)人工勢場法進行了改進.通過仿真得出結論為
1)采用改進人工勢場法避障算法,可以使無人車在斥力場的作用下提前進行小角度避障控制調節(jié),在無碰路徑上不受其他障礙物的斥力場的作用.避障過程角度變化緩慢,跳變較小,無人車脫離斥力場作用域快,可以用于實時性要求較高的結構化道路環(huán)境中.
2)通過改進人工勢場法避障算法仿真,結果表明改進人工勢場避障算法運行時間短,實時性好,可以適應結構化道路高速避障環(huán)境.
[1] KHATIB O.Real Time Obstacle Avoidance for Manipulators and Mobile Robots[J].International Journal of Robotics and Research,1986,5(1):90.
[2] PARK M,JEON J,LEE M.Obstacle Avoidance for Mobile Robots Using Artificial Potential Field Approach with Simulated Annealing[C]//Proceedings of the 2001IEEE International Symposium on Industrial Electronics.Pusan:Institute of Electrical and E-lectronics Engineers,2001:1530.
[3] KITAMURA Y,TANAKA T,KISHINO F,et al.3-D Path Planning in a Dynamic Environment Using an Octree and an Artificial Potential Field[C]//Proceedings of the 1955IEEE International Conference on Intelligent Robots and Systems.Piscataway:IEEE,1995:2474.
[4] 李晉.基于蟻群算法和遺傳算法的機器人路徑規(guī)劃研究[D].哈爾濱:哈爾濱工業(yè)大學,2012.LI Jin.Robot Path Planning Research Based on Ant Colony and Genetic Algorithm[D].Harbin:Harbin Institute of Technology,2012.(in Chinese)
[5] 劉世聰.機器人算法研究[D].大慶:東北石油大學,2011.LIU Shi-cong.Research on Robot Obstacle Avoidance Algorithm[D].Daqing:Northeast Petroleum University,2011.(in Chinese)
[6] 賴智銘,郭躬德.基于自適應閾值蟻群算法的路徑規(guī)劃算法[J].計算機系統(tǒng)應用,2014,23(2):113.LAN Zhi-ming,GUO Gong-de.Ant Colony Optimization Based on Self-Adption Threshold for Path Planning[J].Computer Systems & Applications,2014,23(2):113.(in Chinese)
[7] LEE J,NAM Y Y,HONG S J.Random Force Based Algorithm for Local Minima Secape of Potential Filed Method[C]//11th Internationl Conferece on Control Automation Robotics & Vision.Singapore:Nanyang Technological University,2010:827.
[8] 于振中,閆繼宏.改進人工勢場法的移動機器人路徑規(guī)劃[J].哈爾濱工業(yè)大學學報,2011,43(1):50.ference on Systems,Wasington:Man and Cybernetics,2003,3:2231.
[11] 張劍明,田魁岳.基于 Matlab/Simulink的液壓伺服系統(tǒng)動態(tài)性能仿真研究[J].工程機械,2007,44(4):57.ZHANG Jian-ming,TIAN Kui-yue.Simulation Research on Dynamic Performances of a Hydraulic Servo System Based on Matlab/Simulink[J].Construction Machinery and Equipment,2007,44(4):57.(in Chinese)
[12] 韓京清,王學軍.非線性PID控制器在機械手控制中的應用[C]//1993中國控制與決策學術年會論文集,沈陽:東北大學,1993:12.HAN Jing-qing,WANG Xue-jun.The Application of Nonlinear PID Controller in Manipulator Control[C]//1993Chinese Control and Decision Making Academic Essays, Shengyang: Northeastern University,1993:12.(in Chinese)