適用于復雜動態(tài)環(huán)境的智能車運動規(guī)劃方法

李 卓，姚立澤，王麗霞，劉福聚

（1.北京交通運輸職業(yè)學院汽車工程系，北京102618；2.長春職業(yè)技術(shù)學院汽車學院，吉林 長春 130011；3.中國汽車技術(shù)研究中心有限公司業(yè)務發(fā)展部，北京100070）

1 引言

汽車持有量的增加帶來了道路擁擠、環(huán)境污染、交通事故等諸多問題，智能車自動駕駛技術(shù)為這些問題的解決帶來了契機[1]。運動規(guī)劃是自動駕駛技術(shù)的重要研究方向，是實現(xiàn)自動駕駛的前提。

根據(jù)對工作環(huán)境的掌握程度，運動規(guī)劃分為全局規(guī)劃和局部規(guī)劃；根據(jù)工作環(huán)境障內(nèi)礙物運動情況，運動規(guī)劃分為動態(tài)規(guī)劃和靜態(tài)規(guī)劃。運動規(guī)劃常用方法有可視圖法、自由空間法、Dijkstra 算法、人工勢場法、智能算法等，可視圖法要求工作環(huán)境簡單，當障礙物較多時搜索時間過長[2]；Dijkstra 算法對目標點啟發(fā)信息使用較少，導致算法規(guī)劃路徑時間較長[3]；人工勢場法能夠較好躲避障礙物，但是存在目標點不可達和局部極值問題[4]；智能算法應用于運動規(guī)劃是當前主要發(fā)展方向，應用智能算法規(guī)劃的路徑較優(yōu)，存在的問題主要集中在算法自身缺陷上。

目前運動規(guī)劃成果大都集中在靜態(tài)環(huán)境中，動態(tài)環(huán)境下運動規(guī)劃方法相對較少且不成熟，以人工勢場法為基礎(chǔ)，針對算法自身缺陷和動態(tài)環(huán)境適應性進行了改進，使智能車能夠躲避動態(tài)障礙物、跟蹤動態(tài)目標點，得到了適用于動態(tài)環(huán)境的動態(tài)人工勢場法，經(jīng)驗證此方法能夠使智能車在動態(tài)環(huán)境中安全行駛。

2 傳統(tǒng)人工勢場法

在人工勢場中，目標點對智能車產(chǎn)生引力勢場，障礙物對智能車產(chǎn)生斥力勢場，工作環(huán)境中任意點的勢場為引力勢場和斥力勢場的矢量和，勢場力為勢力場的負梯度值，智能車在勢場力的牽引下，到達目標點。

2.1 引力勢場與引力

記智能車位置為XR，目標點位置為XG，障礙物位置為XO，智能車受到的斥力場記為Urep（XR），引力場為Uatt（XR），綜合勢場為U（XR）。

目標點對智能車的引力勢場大小與兩者距離有關(guān)，距離越大則勢場越強，引力勢場Uatt（XR）構(gòu)造方式如下[5]：

式中：Ka—引力勢場增益系數(shù)；d（XR，XG）—智能車與目標點間的歐式距離。

則根據(jù)力與勢場的換算關(guān)系可知，目標點對智能車的吸引力為：

式中：grad（）—梯度計算。引力方向為智能車指向目標點。

2.2 斥力勢場與斥力

當障礙物與智能車距離越近時，斥力勢場越強，斥力也就越大，當兩者距離越遠時，斥力勢場越弱，斥力也就越小，當兩者距離超過一定范圍時，斥力勢場消失，則斥力勢場構(gòu)造為[6]：

式中：η—斥力勢場增益系數(shù)；ρ0—斥力勢場作用范圍臨界值。根據(jù)斥力勢場給出斥力為：

其中負號表示斥力方向與引力方向相反。

2.3 綜合勢場與合力

智能車在任意一點受到的綜合勢場作用為引力勢場和斥力勢場的矢量和，即：

智能車在任意一點受到的合力既可以通過綜合勢場求得，也可以通過引力與斥力的矢量和求得，合力方向使用平行四邊形準則確定，如圖1 所示。

圖1 多個力求合力的平行四邊形規(guī)則Fig.1 Composition Method of Multiple Forces

3 算法自身缺陷的改進

3.1 局部極值陷阱及虛擬水流法改進

人工勢場法的本質(zhì)是，智能車在合力作用下，沿著勢場減弱的方向運動到目標點。但是在特定環(huán)境中，由于障礙物數(shù)量及排列方式的不同，會使某些點位的合力為0，此時智能車陷入局部極值陷阱，從勢場角度講，此時智能車位于勢場的局部極小值點[7-8]。

使用虛擬水流的思想解決這一問題，主要使用水流的兩個特性：（1）當水流至凹處時，就開水注水，直至水流高度超過周圍高度則進行流動；（2）水往低處流。使用類比法，智能車起始位置為人工水源，將工作環(huán)境中智能車受到的綜合勢場比作地勢，局部極值陷阱即為凹處，當水流至凹處時，向此處注水就相當于增加此處的勢場，當凹處勢場足夠大時，繼續(xù)向勢場減小的方向前進，直至到達勢場最小位置（也即目標點）。

根據(jù)以上分析，虛擬水流法對人工勢場法的改進分三個步驟實現(xiàn)：

（1）判斷智能車是否陷入局部極值陷阱。

智能車陷入局部極值時，表現(xiàn)為在某處振蕩或轉(zhuǎn)圈。為了給出局部極值陷阱的判定方法，首先給出鄰域位置集的概念。記為智能車在單位時間內(nèi)能夠到達的位置，則所有的集合稱為智能車當前位置XR的鄰域位置集，記為那么當：

成立且此處不是目標點時，說明智能車陷入了局部極值陷阱；若上式不成立，說明智能車未陷入局部極值陷阱。

（2）模擬注水過程，增大局部極值處的勢場。

智能車在局部極值陷阱處受到的總勢場調(diào)整為：

式中：要求v-1>0，代表“注水”過程。

（3）選擇鄰域位置集最小勢能點作為前進方向。

經(jīng)過第2 步注水過程，局部極值陷阱處的勢能不再是局部極值，按照“水往低處流”的原則，智能車選擇鄰域位置集的最小勢能點作為前進方向，這樣就跳出了局部極值陷阱，而后按照人工勢場法繼續(xù)前進，直至到達目標點。

3.2 目標不可達及斥力勢場改進

按照傳統(tǒng)人工勢場法中對引力和斥力的定義方法，當智能車靠近目標點時。目標點對智能車的引力作用會減小，若目標點附近存在障礙物，障礙物對智能車的斥力就會占主導地位，使智能車在目標點附近徘徊而無法到達目標點[9-10]。

為了解決這一問題，將智能車到目標點的距離引入到斥力勢場中，使得智能車接近目標點時，目標點對智能車的引力占主導地位。具體改進方法為：

式中：要求n∈（0，1）。對斥力勢場求負梯度，得斥力為：

其中，將斥力Frep（XR）分為是因為兩個力的形成原因和方向不同。其中由斥力勢場對d（XR，XO）求偏導所得，為：

此力的方向由障礙物指向智能車。

此力的方向為由智能車指向目標點。

分析Frep1（XR）和Frep2（XR）可知，當智能車靠近目標點時，d（XR，XO）逐漸減小并趨于0，則Frep1（XR）也趨于0，而Frep2（XR）趨于無窮大，從力的角度講，當智能車靠近目標點時，障礙物斥力Frep1（XR）不起主導作用，而力的分量Frep2（XR）起主導作用，吸引著智能車逐漸向目標點靠近，直至到達目標點后，無窮大的吸力將智能車牢牢吸附在目標點。

3.3 改進方法的算法流程圖

基于3.1 節(jié)和3.2 節(jié)對局部極值和目標不可達問題的改進，給出改進算法流程圖，如圖2 所示。

圖2 改進人工勢場法流程圖Fig.2 Flow Chart of Improved Artificial Potential Field

4 適用于動態(tài)環(huán)境的動態(tài)勢場法

上節(jié)對算法的改進克服了傳統(tǒng)算法自身缺陷，但是改進算法中目標點引力和障礙物斥力只考慮了位置因素，引力和斥力的構(gòu)造中沒有考慮對目標點和障礙物速度、加速度的趨向或避讓，因此上節(jié)的改進算法只適用于靜態(tài)環(huán)境。智能車的運行環(huán)境大都是動態(tài)環(huán)境，因此在上節(jié)改進的基礎(chǔ)上，在引力和斥力中引入速度、加速度因素，從而提出適用于動態(tài)環(huán)境的動態(tài)人工勢場法。

4.1 引力勢場與引力函數(shù)

在動態(tài)環(huán)境中，引力勢場不僅要考慮智能車與目標點的相對位置因素，還要考慮相對速度因素，得改進式為：

式中：Kv—引力勢場速度增益參數(shù)；VR—智能車速度；VG—目標點速度。則引力的合力為：

4.2 斥力勢場與斥力函數(shù)

在動態(tài)環(huán)境中，障礙物對智能車的斥力作用，不僅要考慮位置因素，同時還要考慮速度因素和加速度因素。在式（3）對斥力勢場改進的基礎(chǔ)上，適用于動態(tài)環(huán)境的斥力勢場為：

4.3 綜合勢場與合力

適用于動態(tài)環(huán)境的綜合勢場為動態(tài)引力勢場和動態(tài)斥力市場的矢量和，即：

當智能車陷入局部極值陷阱時，依然使用虛擬水流法解決，注水過程及表達式與3.1 節(jié)中完全一致。

適用于動態(tài)環(huán)境的合力為引力和斥力的矢量和，為：

合力的方向即為智能車前進的方向。

4.4 基于動態(tài)人工勢場法的運動規(guī)劃流程圖

第3 節(jié)針對算法自身缺陷進行了改進，本節(jié)在第3 節(jié)改進基礎(chǔ)上，進行了動態(tài)環(huán)境適應性改進。結(jié)合兩節(jié)的改進內(nèi)容，給出基于動態(tài)人工勢場法的運動規(guī)劃流程圖，如圖3 所示。

圖3 基于動態(tài)人工勢場法的運動規(guī)劃Fig.3 Flow Chart of Motion Planning Based on Dynamic Artificial Potential Field

5 仿真驗證

仿真驗證分為三個部分，一是驗證虛擬水流法能夠使人工勢場法逃出局部極值陷阱；二是驗證斥力勢場函數(shù)的改進可以解決目標不可達問題；三是驗證動態(tài)人工勢場法適用于動態(tài)環(huán)境，能夠規(guī)劃出無碰路徑。

5.1 仿真一：局部極值陷阱問題驗證

此部分構(gòu)造出一個含有局部極值陷阱的工作環(huán)境，如圖4（a）所示。智能車起始位置為（2，0），目標位置為（4，5），智能車步長設(shè)置為0.4，斥力影響范圍閾值設(shè)置為dn=2，引力勢場增益系數(shù)Ka=2.5，斥力勢場增益系數(shù)η=5.4。為了構(gòu)造局部極值陷阱，在工作環(huán)境中安放了33 個障礙物，每隔0.05 排列成拱形，如圖4 所示。圖中矩形表示智能車在不同時刻留下的軌跡，圓形為障礙物，“*”為目標點。

則使用傳統(tǒng)人工勢場法和虛擬水流法改進的人工勢場法分別進行運動規(guī)劃，結(jié)果如圖4 所示。

分析圖4 可知，圖4（a）中，智能車最后一個位置為局部極值陷阱，到達此位置后智能車受力為0，無法繼續(xù)前進。此時改進算法模擬虛擬水流法注水過程，使極值陷阱處勢場增大，而后向勢場最低處前進，以此來擺脫局部極值缺陷。

圖4 局部極值缺陷問題驗證Fig.4 Local Extremum Trap Problem Clarification

5.2 仿真二：目標不可達問題驗證

圖5 目標不可達問題驗證Fig.5 Goal Unreachable Problem Clarification

目標不可達問題是因為目標點附近存在障礙物，為了驗證對目標不可達問題的改進，此部分超構(gòu)造的工作環(huán)境，如圖5 所示。圖中：圓圈—障礙物；方形—智能車；星號—目標點，智能車起始點為（2，0），目標點為（5，3），智能車步長設(shè)置為0.1，斥力影響范圍閾值設(shè)置為dn=2，引力勢場增益系數(shù)Ka=2.5，斥力勢場增益系數(shù)η=5.4。為了構(gòu)造出目標不可達問題，在目標點附近安置了6個障礙物。分別使用傳統(tǒng)人工勢場法和改進人工勢場法在此環(huán)境中進行運動規(guī)劃，結(jié)果如圖5 所示。

從圖5（a）可以看出，由于目標點附近存在障礙物，當智能車接近目標點時，目標點的引力減小，而障礙物的斥力增加，最終合力方向使智能車遠離目標點；智能車遠離目標點后，引力作用增加，而斥力減小，合力方向使智能車向目標點靠近，如此反復形成了智能車在目標點附近徘徊的現(xiàn)象。

而斥力勢場改進的人工勢場法，當智能車接近目標點時，雖然目標點的引力作用減小了，但是斥力勢場產(chǎn)生的力的分量Frep2（XR）逐漸占據(jù)主導地位，使得智能車逐漸靠近目標點，并最終到達目標點。

5.3 仿真三：動態(tài)環(huán)境下運動規(guī)劃

構(gòu)造一個復雜動態(tài)環(huán)境，如圖6 所示。圖中：方塊—智能車；圓點—目標點；物體—障礙物，其中C1、C2為動態(tài)障礙物，C1運動方向為-150°，一步步長為0.1（與機器人步長單位相同），C2運動方向為0°，步長為0.05。智能車起始位置為（4，10），目標位置為（15，3），智能車步長設(shè)置為0.2，斥力影響范圍閾值設(shè)置為dn=2，引力勢場增益系數(shù)Ka=2.5，斥力勢場增益系數(shù)η=5.4，水流速v=2。

圖6 復雜動態(tài)環(huán)境中智能車運動軌跡Fig.6 Intelligent Vehicle Motion Path under Complex Dynamic Environment

在此復雜動態(tài)環(huán)境中，智能車的運動過程，如圖6 所示。圖6給出了某幾個關(guān)鍵時刻智能車的運動狀態(tài)，圖中黑色虛線表示智能車的行駛軌跡。智能車在運動過程中，邊運動邊檢測障礙物，對于靜態(tài)障礙物，使用式（6）給出的斥力勢場函數(shù)，對于動態(tài)障礙物則使用式（13）給出的動態(tài)斥力勢場函數(shù)；當檢測到目標時，靜態(tài)目標點使用式（1）給出的引力勢場，動態(tài)障礙物使用式（11）給出的動態(tài)引力勢場函數(shù)。智能車在運動過程中，邊進行規(guī)劃邊檢測是否陷入局部極值陷阱，陷入局部極值陷阱時，使用虛擬水流法進行注水從而逃出陷阱位置。最終智能車運動軌跡，如圖6 所示。在t=8.7s，智能車在目標點引力和障礙物斥力作用下，到達所示位置；t=16.8s 時，智能車在避開靜態(tài)障礙物的同時，檢測到動態(tài)障礙物C1，使用動態(tài)斥力勢場函數(shù)用于躲避動態(tài)障礙物；t=20.9時，智能車成功避開動態(tài)障礙物C1，繼續(xù)前進；t=24.5s 時，智能車檢測到動態(tài)障礙物C2，使用動態(tài)斥力勢場函數(shù)進行避障，由于C2不斷靠近障礙物，所以C2對智能車的斥力包括位置因素、速度因素、加速度因素；t=30.1s 時，智能車成功避開動態(tài)障礙物C2，同時檢測到目標點，目標點為靜止狀態(tài)，使用傳統(tǒng)引力勢場函數(shù)；t=40.8s 時，智能車成功到達目標點。

6 結(jié)論

從兩個方面對傳統(tǒng)人工勢場法進行了改進：（1）針對算法自身存在的缺陷，使用虛擬水流法和改進斥力勢場的方法，解決了局部極值和目標不可達問題；（2）對算法進行了動態(tài)環(huán)境適應性改進，使算法能夠躲避動態(tài)障礙物、跟蹤動態(tài)目標點。經(jīng)驗證，基于動態(tài)人工勢場的運動規(guī)劃方法能夠在復雜動態(tài)環(huán)境中規(guī)劃出安全路徑，保證智能車安全行駛。下一步的研究方向為路徑的精確跟蹤，此部分內(nèi)容在這里未做研究。