基于動(dòng)態(tài)虛擬障礙物的智能車(chē)輛局部路徑規(guī)劃方法＊

吳乙萬(wàn)，黃 智

（湖南大學(xué) 汽車(chē)車(chē)身先進(jìn)設(shè)計(jì)制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南 長(zhǎng)沙 410082）

路徑規(guī)劃是智能車(chē)輛的一個(gè)重要研究領(lǐng)域，其目標(biāo)是在動(dòng)態(tài)環(huán)境下，尋找一條從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑.

目前常用到的路徑規(guī)劃方法有人工勢(shì)場(chǎng)法、格柵法、可視圖法以及其他各種智能規(guī)劃算法等，其中人工勢(shì)場(chǎng)法以其數(shù)學(xué)計(jì)算的簡(jiǎn)單明了被廣泛應(yīng)用.當(dāng)前研究主要集中在解決如局部極小問(wèn)題[1－2]、目標(biāo)不可達(dá)問(wèn)題[3－4]和抖動(dòng)消除問(wèn)題[5－6]等各種理論問(wèn)題，為方便研究往往將移動(dòng)體看作無(wú)約束的質(zhì)點(diǎn)或剛體.由于未考慮車(chē)輛運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)響應(yīng)的約束，對(duì)于智能車(chē)輛，連通的路徑不一定可跟蹤.

文獻(xiàn)[7]提出虛擬障礙物的概念，當(dāng)移動(dòng)體遇到障礙物需要進(jìn)行避障時(shí)，通過(guò)改變有效虛擬障礙物的布局，采用改進(jìn)的向量直方圖方法進(jìn)行避障路徑規(guī)劃.該方法的避障觸發(fā)距離未考慮車(chē)速因素，有效虛擬障礙物的分布無(wú)法進(jìn)行數(shù)值解析且未考慮動(dòng)態(tài)障礙物的影響，只適用于低速工況下以最小轉(zhuǎn)彎半徑進(jìn)行避障.

為解決智能車(chē)的局部路徑規(guī)劃問(wèn)題，針對(duì)結(jié)構(gòu)化道路環(huán)境，本文提出一種動(dòng)態(tài)虛擬障礙物布局方法，并將其應(yīng)用于智能車(chē)的局部路徑規(guī)劃.通過(guò)車(chē)載傳感器實(shí)時(shí)獲取動(dòng)態(tài)環(huán)境信息，進(jìn)行駕駛行為決策并生成受約束的動(dòng)態(tài)虛擬障礙物，再采用引入動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束的改進(jìn)人工勢(shì)場(chǎng)法規(guī)劃智能車(chē)無(wú)碰撞局部路徑.

1 系統(tǒng)框架

智能車(chē)采用分層控制結(jié)構(gòu)，由環(huán)境感知、行為決策、局部路徑規(guī)劃和執(zhí)行控制等4層構(gòu)成，如圖1所示.環(huán)境感知層根據(jù)車(chē)載傳感器采集到的道路及障礙物信息進(jìn)行多源信息融合，并實(shí)時(shí)采集本車(chē)狀態(tài)信息.行為決策層根據(jù)道路環(huán)境、車(chē)輛動(dòng)力學(xué)響應(yīng)和道路交通規(guī)則等進(jìn)行車(chē)輛駕駛行為實(shí)時(shí)決策并生成虛擬障礙物.局部路徑規(guī)劃層根據(jù)虛擬障礙物信息進(jìn)行無(wú)碰撞路徑規(guī)劃.執(zhí)行控制層控制轉(zhuǎn)向及驅(qū)／制動(dòng)操縱機(jī)構(gòu)，使車(chē)輛跟蹤所規(guī)劃的路徑.

圖1 智能車(chē)整體框架Fig.1 Architecture of intelligent vehicle

2 虛擬障礙物的生成

本文的核心思想是依據(jù)行為決策結(jié)果動(dòng)態(tài)生成受道路和真實(shí)障礙物約束的虛擬障礙物，并將其應(yīng)用到智能車(chē)局部路徑規(guī)劃中.虛擬障礙物分為基于車(chē)道線(xiàn)的虛擬障礙物和基于真實(shí)障礙物的虛擬障礙物，根據(jù)駕駛行為決策的結(jié)果可分為有效虛擬障礙物和無(wú)效虛擬障礙物.

2.1 基于車(chē)道線(xiàn)的虛擬障礙物生成

基于車(chē)道線(xiàn)的虛擬障礙物依據(jù)相機(jī)采集的道路信息（車(chē)道寬度、道路寬度、車(chē)道數(shù)）生成.具體方法為：沿車(chē)輛行駛方向，在檢測(cè)到的所有車(chē)道線(xiàn)上進(jìn)行等間距平移所截取的點(diǎn).在車(chē)輛目標(biāo)行駛車(chē)道上的虛擬障礙物為有效，否則為無(wú)效障礙.圖2為在無(wú)真實(shí)障礙物時(shí)依據(jù)車(chē)道線(xiàn)生成虛擬障礙物的布局.

圖2 基于車(chē)道線(xiàn)的虛擬障礙物布局Fig.2 The layout of virtual obstacle based on the lane line

2.2 基于真實(shí)障礙物的虛擬障礙物生成

如圖3所示，A為目標(biāo)車(chē)，在A的行駛車(chē)道上存在障礙物B，B為當(dāng)前車(chē)道上的真實(shí)障礙物.若B速度為零即為靜止障礙物，否則為移動(dòng)障礙物.當(dāng)相鄰車(chē)道上有足夠的換道空間，行為決策層將實(shí)時(shí)根據(jù)目標(biāo)車(chē)與障礙物的距離、相對(duì)速度及相鄰車(chē)道是否允許換道等進(jìn)行避障換道行為的決策.當(dāng)安全距離太小，在換道初期，A與B將發(fā)生追尾碰撞或角碰[8].假設(shè)換道過(guò)程中目標(biāo)車(chē)與障礙物的速度均保持不變，t0時(shí)刻，A向左側(cè)轉(zhuǎn)向，在tp時(shí)刻到達(dá)臨界碰撞位置（圖3中Ax，Bx分別表示tx時(shí)刻A，B車(chē)位置）.

圖3 車(chē)輛臨界碰撞工況Fig.3 Vehicle critical position of collision

為避免與真實(shí)障礙物發(fā)生碰撞，采用雙圓弧換道路徑初步估算到達(dá)臨界碰撞點(diǎn)的時(shí)刻tp，即圖4中A0點(diǎn)到A1點(diǎn)的時(shí)間.O為圓心，R為圓弧的曲率半徑.

式中：VAx為目標(biāo)車(chē)的縱向速度；aAy為目標(biāo)車(chē)的側(cè)向加速度.圖4中w為車(chē)道寬；q為目標(biāo)車(chē)質(zhì)心與車(chē)道線(xiàn)的距離.

圖4 圓弧換道的幾何解析Fig.4 The geometry resolution of arc lane－changing path

為了保證換道過(guò)程中車(chē)輛的乘坐舒適性并提高車(chē)輛的行駛穩(wěn)定性，通常取aAy≤0.2g.

由幾何關(guān)系可得：

目標(biāo)車(chē)與障礙物的相對(duì)速度Vr為：

式中：di為第i采樣時(shí)刻目標(biāo)車(chē)與障礙物的相對(duì)距離；di－1為第i－1采樣時(shí)刻目標(biāo)車(chē)與障礙物的相對(duì)距離；T為采樣周期.為避免發(fā)生換道碰撞的最小安全距離Dr[8]為：

當(dāng)Vr≤0時(shí)，隨著時(shí)間增加，目標(biāo)車(chē)與障礙物的安全距離保持不變或增大，換道行為也會(huì)越安全，Dr取零.

當(dāng)Vr＞0時(shí)，隨著時(shí)間增加，目標(biāo)車(chē)與障礙物的安全距離減小，為避免發(fā)生碰撞須預(yù)留一定的安全距離來(lái)補(bǔ)償?shù)竭_(dá)碰撞點(diǎn)目標(biāo)車(chē)所經(jīng)過(guò)的距離，因此式（5）可改寫(xiě)為：

式中：ξ為安全系數(shù)，取ξ＝1.0～1.3.

考慮本車(chē)速度并預(yù)留一定安全余量的換道安全距離D為：

式中：Th為車(chē)間時(shí)距，Th通常取2～3s.

當(dāng)di＞D且di＜D＋Δ，同時(shí)左前方安全區(qū)域內(nèi)沒(méi)有來(lái)車(chē)時(shí)，則允許換道，反之減速等待合適的時(shí)機(jī)再進(jìn)行換道.其中Δ＝VAxT.

允許換道時(shí)，根據(jù)臨界碰撞時(shí)間內(nèi)目標(biāo)車(chē)輛走過(guò)的距離（VAxtpξ）與車(chē)道寬度w構(gòu)建虛擬車(chē)道線(xiàn)與當(dāng)前車(chē)道的兩邊緣相交，在兩交點(diǎn)內(nèi)進(jìn)行等間距虛擬障礙物的選取，同時(shí)將本車(chē)道左右邊沿的部分虛擬障礙物置為無(wú)效障礙物.目標(biāo)車(chē)質(zhì)心坐標(biāo)為（xa，ya），質(zhì)心距車(chē)輛前端距離為lf，質(zhì)心距車(chē)道距離為q，虛擬車(chē)道線(xiàn)的解析式為：

圖5為存在真實(shí)障礙物且具備安全換道的條件時(shí)，虛擬障礙物的布局.

圖5 基于真實(shí)障礙物和車(chē)道線(xiàn)的虛擬障礙物布局Fig.5 The layout of virtual obstacles based on the real obstacles and lane line

在車(chē)輛行駛過(guò)程中，環(huán)境感知層實(shí)時(shí)更新真實(shí)障礙物和車(chē)道線(xiàn)的信息，由行為決策層實(shí)時(shí)決策并動(dòng)態(tài)調(diào)整虛擬障礙物的布局.

2.3 局部目標(biāo)點(diǎn)

根據(jù)感知范圍和決策的目標(biāo)車(chē)道確定局部目標(biāo)點(diǎn).如圖2和圖5所示，L為目標(biāo)車(chē)的車(chē)身長(zhǎng)度，h為目標(biāo)車(chē)輛的縱向感知距離，H為局部目標(biāo)點(diǎn)與目標(biāo)車(chē)的縱向距離，位于目標(biāo)車(chē)道中心，其中H＝h＋L.

3 考慮動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束的局部路徑規(guī)劃

3.1 車(chē)輛的動(dòng)力學(xué)及運(yùn)動(dòng)學(xué)約束

在車(chē)輛行駛過(guò)程中，考慮車(chē)輛的操縱性和舒適性，引入橫向動(dòng)力學(xué)約束：

由文獻(xiàn)[9－10]可知，通過(guò)加在連通路徑點(diǎn)上的方向約束限制，可使生成的路徑滿(mǎn)足車(chē)輛非完整約束.

不同車(chē)速下車(chē)輛的最小轉(zhuǎn)彎半徑rvmin為：

基于阿克曼轉(zhuǎn)向原理[11]可知，前輪轉(zhuǎn)角與轉(zhuǎn)彎半徑r和車(chē)輛軸距l(xiāng)應(yīng)滿(mǎn)足：

不同車(chē)速下的最小轉(zhuǎn)彎半徑?jīng)Q定的前輪轉(zhuǎn)角極限為δvmax，同時(shí)受轉(zhuǎn)向機(jī)構(gòu)物理約束的前輪轉(zhuǎn)角極限為δmmax.為使車(chē)輛能夠跟蹤規(guī)劃的路徑，在路徑規(guī)劃中應(yīng)使車(chē)輛前輪轉(zhuǎn)角滿(mǎn)足方向約束（｜δ｜≤min（δvmax，δmmax））.

3.2 改進(jìn)人工勢(shì)場(chǎng)法的局部路徑規(guī)劃

采用文獻(xiàn)[12]所提出的勢(shì)場(chǎng)函數(shù)：

相應(yīng)的吸引力Fatt為：

相應(yīng)的斥力Frep為：

式中：katt為吸引系數(shù)；krep為排斥系數(shù)；dCG為智能車(chē)道目標(biāo)點(diǎn)的距離；X為智能車(chē)當(dāng)前位置；XG為目標(biāo)位置矢量；ρ0為單個(gè)障礙物影響的最大距離范圍；ρ為機(jī)器人與障礙物的最短距離.

3.3 局部最小點(diǎn)處理

當(dāng)智能車(chē)所受引力和斥力在一條直線(xiàn)上，如果智能車(chē)只依靠自身所受的勢(shì)場(chǎng)力是無(wú)法離開(kāi)障礙物的.根據(jù)車(chē)輛的避障方向，對(duì)智能車(chē)施加垂直車(chē)道線(xiàn)的附加控制力Fadd來(lái)解決這個(gè)問(wèn)題.

式中：kadd為附加力系數(shù).

4 局部路徑規(guī)劃仿真

為了驗(yàn)證算法的有效性和可行性，進(jìn)行多組智能車(chē)路徑規(guī)劃仿真實(shí)驗(yàn).目標(biāo)車(chē)軸距l(xiāng)＝2.48m，最小轉(zhuǎn)彎半徑Rmin＝5m，車(chē)道寬度3.75m.取最大側(cè)向加速度aymax＝0.1g進(jìn)行路徑規(guī)劃.

第1組仿真實(shí)驗(yàn)：如圖6所示，目標(biāo)車(chē)從原點(diǎn)（0，0）分別以5，10，15和20m／s等不同初始速度向前行駛，并超越靜止于（100，0）處的障礙車(chē)，對(duì)比分析速度對(duì)避障路徑規(guī)劃的影響并分析路徑的曲率半徑.圖中縱向位置用X表示，橫向位置用Y表示（下同）.圖7為曲率半徑對(duì)比圖.

由圖6和圖7可知，為避免發(fā)生碰撞并確保車(chē)輛的操縱穩(wěn)定性，隨著目標(biāo)車(chē)車(chē)速的提高，避障換道所需的空間也增加，同時(shí)規(guī)劃路徑的曲率半徑均大于車(chē)輛最小轉(zhuǎn)彎半徑.

圖6 目標(biāo)車(chē)以不同速度避讓靜止的障礙物Fig.6 The target vehicle avoid static obstacle at different speed

圖7 曲率半徑對(duì)比圖Fig.7 Comparison of road radius

第2組仿真實(shí)驗(yàn)：為分析跟蹤避障路徑時(shí)車(chē)輛動(dòng)力學(xué)響應(yīng)，在Carsim中建立無(wú)人駕駛研究平臺(tái)的整車(chē)動(dòng)力學(xué)模型，采用單點(diǎn)預(yù)瞄控制策略進(jìn)行轉(zhuǎn)向控制，預(yù)瞄時(shí)間為1s.實(shí)驗(yàn)結(jié)果如圖8所示.

由圖8可知，側(cè)向加速度的超調(diào)量為14.1%，最大橫向跟蹤誤差為0.11m，各種車(chē)速下車(chē)輛能較好地跟隨道路，規(guī)劃路徑的可跟蹤性較好.

第3組仿真實(shí)驗(yàn)：目標(biāo)車(chē)以10m／s的初始速度從（0，0）點(diǎn)向前行駛，同時(shí)在（60，0）處有一慢速運(yùn)動(dòng)障礙車(chē)，根據(jù)相對(duì)速度、相對(duì)距離及本車(chē)速度確定觸發(fā)避障換道的時(shí)機(jī)，對(duì)比分析障礙車(chē)分別以1m／s和5m／s的速度前進(jìn)時(shí)的避障路徑規(guī)劃.如圖9所示，避障路徑由目標(biāo)車(chē)速度決定，與障礙車(chē)的速度無(wú)關(guān)；目標(biāo)車(chē)與障礙車(chē)的相對(duì)距離與相對(duì)運(yùn)動(dòng)速度決定觸發(fā)避障換道的時(shí)機(jī).

圖8 路徑跟蹤響應(yīng)曲線(xiàn)Fig.8 The response of path tracking

圖9 目標(biāo)車(chē)以10m／s的速度避讓不同速度的障礙物Fig.9 The target vehicle avoid the obstacles of different speed at the speed of 10m／s

5 結(jié) 論

本文提出了基于動(dòng)態(tài)虛擬障礙物的智能車(chē)輛局部路徑規(guī)劃方法，通過(guò)分析換道時(shí)目標(biāo)車(chē)與障礙車(chē)的運(yùn)動(dòng)關(guān)系，并考慮側(cè)向加速度的約束獲得了虛擬車(chē)道線(xiàn)的解析式，根據(jù)行為決策的結(jié)果實(shí)現(xiàn)虛擬障礙物的動(dòng)態(tài)布局，在路徑規(guī)劃中加入動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束解決了人工勢(shì)場(chǎng)法無(wú)法直接應(yīng)用于智能車(chē)路徑規(guī)劃的問(wèn)題.

[1] 朱毅，張濤，宋靖雁.未知環(huán)境下勢(shì)場(chǎng)法路徑規(guī)劃的局部極小問(wèn)題研究[J].自動(dòng)化學(xué)報(bào)，2010，36（8）：1122－1129.ZHU Yi，ZHANG Tao，SONG Jing－yan.Study on the local minima problem of path planning using potential filed method in unknown environments[J].Acta Automatica Sinica，2010，36（8）：1122－1129.（In Chinese）

[2] LEE J，NAM Y Y，HONG S J.Random force based algorithm for local minima escape of potential filed method[C]／／11th International Conference on Control Automation Robotics ＆ Vision.Singapore：Nanyang Technological University，2010：827－832.

[3] CEN Y W，WANG L H，ZHANG H D.Real－time obstacle avoidance strategy for mobile robot based on improved coordinating potential field with genetic algorithm[C]／／16th IEEE International Conference on Control Applications：Part of IEEE Multi－conference on Systems and Control.Singapore：IEEE Control Systems Society，2007：415－419.

[4] JIA Q，WANG X S.An improved potential field method for path planning[C]／／2010Chinese Control and Decision Conference.Xuzhou：Northeastern University，2010：2265－2270.

[5] 高云峰，黃海.復(fù)雜環(huán)境下基于市場(chǎng)原理的路徑規(guī)劃方法[J].機(jī)器人，2004，26（2）：114－118.GAO Yun－feng，HUANG Hai.A path planning algorithm based on potential field for complex enviroment[J].Robot，2004，26（2）：114－118.（In Chinese）

[6] LI S R，YU G J，LI N.Path planning research for mobile robot based on the circle scanning method[C]／／2008Chinese Control and Decision Conference.Yantai：Northeastern University，2008：1698－1703.

[7] 葉煒垚，王春香，楊明，等.基于虛擬障礙物的移動(dòng)機(jī)器人路徑規(guī)劃方法[J].機(jī)器人，2011，33（3）：273－278.YE Wei－yao，WANG Chun－xiang，YANG Ming，etal.Virtual obstacles based path planning for mobile robots[J].Robot，2011，33（3）：273－278.（In Chinese）

[8] 王榮本，游峰，崔高健，等.車(chē)輛安全換道分析[J].吉林大學(xué)學(xué)報(bào)：工學(xué)版，2005，35（2）：179－182.WANG Rong－ben，YOU Feng，CUI Gao－jian，etal.Analysis on lane－changing safety of vehicle[J].Journal of Jilin University：Engineering and Technology Edition，2005，35（2）：179－182.（In Chinese）

[9] WANG Y J，LINNETT J A，ROBERTS J.Kinematics，kinematic constraints and path planning for wheeled mobile robots[J].Robotica，1994（12）：391－400.

[10] 宋金澤，戴斌，單恩忠，等.一種改進(jìn)的RRT路徑規(guī)劃算法[J].電子學(xué)報(bào)，2010，38（增刊2）：225－228.SONG Jin－ze，DAI Bin，SHAN En－zhong，etal.An improved RRT path planning algorithm[J].Acta Electronica Sinica，2010，38（Sup2）：225－228.（In Chinese）

[11] RAJAMANI R.Vehicle dynamics and control[M].2nd ed.New York：Springer，2006：24－25.

[12] 王萌，王曉榮，李春貴，等.改進(jìn)人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人路徑規(guī)劃研究[J].計(jì)算機(jī)工程與設(shè)計(jì)，2008，29（6）：1504－1506.WANG Meng，WANG Xiao－rong，LI Chun－gui，etal.Study of local path planning of mobile robot based on improved artificial potential field method[J].Computer Engineering and Design，2008，29（6）：1504－1506.（In Chinese）