基于改進(jìn)快速行進(jìn)樹(shù)的自主代客泊車(chē)路徑規(guī)劃

張家旭，周時(shí)瑩，劉曄，郭崇，趙健

（1.吉林大學(xué)汽車(chē)仿真與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，吉林長(zhǎng)春 130022；2.中國(guó)第一汽車(chē)集團(tuán)有限公司智能網(wǎng)聯(lián)研發(fā)院，吉林長(zhǎng)春 130011）

自主代客泊車(chē)系統(tǒng)被公認(rèn)為是解決“最后一公里”自動(dòng)駕駛問(wèn)題的有效手段，其核心組件包括感知、定位、規(guī)劃和控制，開(kāi)發(fā)高計(jì)算效率且滿足汽車(chē)非完整約束的路徑規(guī)劃方法是自主代客泊車(chē)系統(tǒng)落地的必要條件之一［1-2］.為了加速自主代客泊車(chē)系統(tǒng)落地，國(guó)內(nèi)外眾多學(xué)者對(duì)自主代客泊車(chē)路徑規(guī)劃問(wèn)題展開(kāi)了廣泛研究.

目前，自主代客泊車(chē)路徑規(guī)劃問(wèn)題研究可劃分為基于優(yōu)化的規(guī)劃方法［3-6］、基于柵格的規(guī)劃方法［7-8］和基于隨機(jī)采樣的規(guī)劃方法［9-10］.文獻(xiàn)［3］基于龍格-庫(kù)塔法建立了包含汽車(chē)運(yùn)動(dòng)學(xué)約束和避障約束的自主代客泊車(chē)路徑離散優(yōu)化模型，并采用內(nèi)點(diǎn)法求解該離散優(yōu)化問(wèn)題來(lái)獲得最優(yōu)的自主代客泊車(chē)路徑.文獻(xiàn)［4-6］以能量消耗最小和時(shí)間消耗最小為優(yōu)化目標(biāo)，基于優(yōu)化理論獲得自主代客泊車(chē)路徑.盡管上述基于優(yōu)化的規(guī)劃方法可以得到最優(yōu)的自主代客泊車(chē)路徑，但優(yōu)化問(wèn)題求解過(guò)程所需的較大計(jì)算量限制了其工程應(yīng)用.

相對(duì)于基于優(yōu)化的規(guī)劃方法，基于柵格的規(guī)劃方法和基于隨機(jī)采樣的規(guī)劃方法是通過(guò)離散化自主代客泊車(chē)路徑搜索空間來(lái)提高計(jì)算效率，并分別通過(guò)精細(xì)化離散空間和增加采樣點(diǎn)數(shù)量來(lái)提高算法的完備性.文獻(xiàn)［7］和文獻(xiàn)［8］采用柵格地圖離散化自主代客泊車(chē)路徑搜索空間，并分別采用D*Lite 算法和混合A*算法規(guī)劃自主代客泊車(chē)路徑，但規(guī)劃的自主代客泊車(chē)路徑精度受限于柵格地圖的細(xì)化程度，而過(guò)于精細(xì)的柵格地圖所需的計(jì)算量較大.文獻(xiàn)［9］和文獻(xiàn)［10］采用隨機(jī)采樣點(diǎn)離散化自主代客泊車(chē)路徑搜索空間，并分別采用快速隨機(jī)擴(kuò)展樹(shù)算法（Rap?idly Random Tree，RRT）和RRT*算法規(guī)劃自主代客泊車(chē)路徑，但規(guī)劃的自主代客泊車(chē)路徑難以滿足汽車(chē)非完整約束.

鑒于此，本文將自主代客泊車(chē)路徑細(xì)分為用于引導(dǎo)汽車(chē)安全行駛到泊車(chē)位附近的引導(dǎo)路徑和用于引導(dǎo)汽車(chē)安全駛?cè)氩窜?chē)位的泊車(chē)路徑，并分別基于改進(jìn)快速行進(jìn)樹(shù)算法和Dubins曲線規(guī)劃引導(dǎo)路徑和泊車(chē)路徑.首先，采用類廣度優(yōu)先搜索策略建立環(huán)境地圖的“路徑場(chǎng)”，并提出一種高計(jì)算效率的避障檢測(cè)策略，基于環(huán)境地圖“路徑場(chǎng)”提出符合汽車(chē)非完整約束的遠(yuǎn)端參考點(diǎn)和近端參考點(diǎn)選取原則與路徑節(jié)點(diǎn)更新原則，通過(guò)使路徑節(jié)點(diǎn)逐漸靠近目標(biāo)節(jié)點(diǎn)來(lái)完成自主代客泊車(chē)引導(dǎo)路徑規(guī)劃任務(wù).隨后，基于Dubins 曲線規(guī)劃滿足初始泊車(chē)方位角任意性要求和泊車(chē)位方位角的非唯一性要求的泊車(chē)路徑，引導(dǎo)汽車(chē)安全駛?cè)氩窜?chē)位.最后，仿真驗(yàn)證所提自主代客泊車(chē)路徑規(guī)劃方法的可行性，結(jié)果表明：相對(duì)于基于傳統(tǒng)快速行進(jìn)算法的自主代客泊車(chē)路徑規(guī)劃方法，所提方法規(guī)劃的自主代客泊車(chē)路徑滿足汽車(chē)非完整約束要求，可以安全引導(dǎo)汽車(chē)完成自主代客泊車(chē)任務(wù).

1 引導(dǎo)路徑規(guī)劃

快速行進(jìn)樹(shù)算法采用類廣度優(yōu)先搜索策略建立環(huán)境地圖的“路徑場(chǎng)”，并沿著環(huán)境地圖“路徑場(chǎng)”梯度下降方向快速規(guī)劃出成本最小的無(wú)碰撞路徑.但環(huán)境地圖“路徑場(chǎng)”梯度下降方向變化率不滿足汽車(chē)的非完整約束要求，使規(guī)劃的無(wú)碰撞路徑不可執(zhí)行［11］.為此，本節(jié)在類廣度優(yōu)先搜索策略建立的環(huán)境地圖“路徑場(chǎng)”基礎(chǔ)上改進(jìn)快速行進(jìn)樹(shù)算法，并利用改進(jìn)的快速行進(jìn)樹(shù)算法規(guī)劃自主代客泊車(chē)引導(dǎo)路徑，使規(guī)劃的路徑滿足汽車(chē)非完整約束要求.因此，改進(jìn)的快速行進(jìn)樹(shù)算法包括：環(huán)境地圖“路徑場(chǎng)”建立模塊和路徑規(guī)劃模塊.其中，環(huán)境地圖“路徑場(chǎng)”建立模塊的流程如圖1 所示，采用候選列表保存成本值未修正的節(jié)點(diǎn)，采用開(kāi)放列表保存成本值已修正但未擴(kuò)展的節(jié)點(diǎn)，以及采用封閉列表保存成本值已修正且已擴(kuò)展的節(jié)點(diǎn).并且從目標(biāo)節(jié)點(diǎn)開(kāi)始，采用類廣度優(yōu)先搜索策略維護(hù)候選列表、開(kāi)放列表和封閉列表來(lái)建立環(huán)境地圖“路徑場(chǎng)”.

基于圖2 所示的環(huán)境地圖“路徑場(chǎng)”建立實(shí)例，闡述圖1 所示的改進(jìn)快速行進(jìn)樹(shù)算法“路徑場(chǎng)”建立模塊流程.如圖2 所示，成本值已修正且已擴(kuò)展的目標(biāo)節(jié)點(diǎn)A和節(jié)點(diǎn)B保存在封閉列表中，成本值已修正但未擴(kuò)展的節(jié)點(diǎn)C、節(jié)點(diǎn)D和節(jié)點(diǎn)E保存在開(kāi)放列表中，成本值未修正的節(jié)點(diǎn)F和節(jié)點(diǎn)G保存在候選列表中.從開(kāi)放列表中取出成本值最小的節(jié)點(diǎn)D作為待擴(kuò)展節(jié)點(diǎn)，將待擴(kuò)展節(jié)點(diǎn)D圓鄰域與候選列表交集作為待修正集合{節(jié)點(diǎn)F，節(jié)點(diǎn)G}.順序取出待修正集合中的節(jié)點(diǎn)，將節(jié)點(diǎn)F圓鄰域與開(kāi)放列表交集中使其成本值最小的節(jié)點(diǎn)D作為其候選父節(jié)點(diǎn)，將節(jié)點(diǎn)G圓鄰域與開(kāi)放列表交集中使其成本值最小的節(jié)點(diǎn)D作為其候選父節(jié)點(diǎn).若待修正集合中節(jié)點(diǎn)與其候選父節(jié)點(diǎn)構(gòu)成的路徑為無(wú)碰撞路徑，則將此節(jié)點(diǎn)從候選列表中刪除，并且將此節(jié)點(diǎn)標(biāo)記為已修正節(jié)點(diǎn)，其候選父節(jié)點(diǎn)作為其真實(shí)父節(jié)點(diǎn).當(dāng)待修正集合為空時(shí)，更新開(kāi)放列表和封閉列表，即將已修正節(jié)點(diǎn)插入開(kāi)放列表中，將待擴(kuò)展節(jié)點(diǎn)D從開(kāi)放列表中刪除，將待擴(kuò)展節(jié)點(diǎn)D插入封閉列表中.

圖1 環(huán)境地圖“路徑場(chǎng)”建立模塊流程Fig.1 The building process of the“path field”of environmental map

圖2 環(huán)境地圖“路徑場(chǎng)”建立實(shí)例Fig.2 The building instance of the“path field”of environmental map

在圖1 所示的改進(jìn)快速行進(jìn)樹(shù)算法環(huán)境地圖“路徑場(chǎng)”建立模塊流程中，判斷待修正集合中節(jié)點(diǎn)與其候選父節(jié)點(diǎn)構(gòu)成的路徑是否為無(wú)碰撞路徑是一項(xiàng)需要較大計(jì)算量的工作［12］.為了提高算法的計(jì)算效率，本節(jié)采用多邊形描述障礙物，并且將路徑與多邊形不相交問(wèn)題轉(zhuǎn)化為路徑與多邊形的邊不相交問(wèn)題，即兩條線段不相交問(wèn)題.如圖3 所示，基于向量積運(yùn)算性質(zhì)，可得圖3（a）中線段M1M2與線段N1N2不相交的充分條件為

圖3 無(wú)碰撞路徑判斷策略Fig.3 The judgment strategy of collision-free path

式中：Lmax為點(diǎn)M1、M2、N1和N2之間的最大距離.

圖3（b）～（d）中線段M1M2與線段N1N2不相交的充分條件為：

式中：×表示向量積運(yùn)算；∥表示或運(yùn)算.

在圖1 所示的環(huán)境地圖“路徑場(chǎng)”建立模塊流程的基礎(chǔ)上，改進(jìn)快速行進(jìn)樹(shù)算法.基于圖4 所示的路徑規(guī)劃流程，規(guī)劃滿足汽車(chē)非完整約束的自主代客泊車(chē)引導(dǎo)路徑.

圖4 路徑規(guī)劃模塊流程Fig.4 The process of the path planning module

如圖4 所示，改進(jìn)快速行進(jìn)樹(shù)算法路徑規(guī)劃模塊主要依據(jù)選取的遠(yuǎn)端參考點(diǎn)和近端參考點(diǎn)來(lái)更新路徑節(jié)點(diǎn)，使路徑節(jié)點(diǎn)從起始節(jié)點(diǎn)開(kāi)始逐漸靠近目標(biāo)節(jié)點(diǎn)，進(jìn)而規(guī)劃出滿足汽車(chē)非完整約束的自主代客泊車(chē)引導(dǎo)路徑.因此，遠(yuǎn)端參考點(diǎn)和近端參考點(diǎn)的選取原則與路徑節(jié)點(diǎn)的更新原則是路徑規(guī)劃模塊的核心.如圖5（a）所示，在當(dāng)前路徑節(jié)點(diǎn)P1的大圓鄰域內(nèi)選取成本值最小的節(jié)點(diǎn)Pfar作為圓心點(diǎn)，將以節(jié)點(diǎn)Pfar為圓心點(diǎn)的圓鄰域內(nèi)所有節(jié)點(diǎn)的橫、縱坐標(biāo)平均值構(gòu)成的節(jié)點(diǎn)作為當(dāng)前路徑節(jié)點(diǎn)P1的遠(yuǎn)端參考點(diǎn)Pfar，以避免選取的遠(yuǎn)端參考點(diǎn)靠近障礙物.為了提高計(jì)算效率，將當(dāng)前路徑節(jié)點(diǎn)P1與遠(yuǎn)端參考點(diǎn)Pfar之間的路徑作為汽車(chē)輪廓內(nèi)邊緣行駛軌跡，若當(dāng)前路徑節(jié)點(diǎn)P1與遠(yuǎn)端參考點(diǎn)Pfar之間的路徑與障礙物相交，則采用同樣的方式選取當(dāng)前路徑節(jié)點(diǎn)的近端參考點(diǎn)Pnear，并且以近端參考點(diǎn)Pnear作為當(dāng)前路徑節(jié)點(diǎn)P1更新的參考點(diǎn).若當(dāng)前路徑節(jié)點(diǎn)P1與近端參考點(diǎn)Pnear之間的路徑方向滿足汽車(chē)非完整約束要求，則沿著當(dāng)前路徑節(jié)點(diǎn)P1與近端參考點(diǎn)Pnear之間的路徑方向更新當(dāng)前路徑節(jié)點(diǎn)P1；否則，按照更新后的路徑節(jié)點(diǎn)能夠最大限度靠近近端參考點(diǎn)Pnear原則，基于汽車(chē)最小轉(zhuǎn)彎半徑更新當(dāng)前路徑節(jié)點(diǎn)P1.

圖5 參考點(diǎn)選取原則和路徑節(jié)點(diǎn)更新原則Fig.5 The principles of selecting reference point and updating path node

如圖5（b）所示，若當(dāng)前路徑節(jié)點(diǎn)P1與遠(yuǎn)端參考點(diǎn)Pfar之間的路徑與障礙物不相交，則遠(yuǎn)端參考點(diǎn)Pfar作為當(dāng)前路徑節(jié)點(diǎn)P1更新的參考點(diǎn).若當(dāng)前路徑節(jié)點(diǎn)P1與遠(yuǎn)端參考點(diǎn)Pfar之間的路徑方向滿足汽車(chē)非完整約束要求，則沿著當(dāng)前路徑節(jié)點(diǎn)P1與遠(yuǎn)端參考點(diǎn)Pfar之間的路徑方向更新當(dāng)前路徑節(jié)點(diǎn)P1；否則，按照更新后的路徑節(jié)點(diǎn)能夠最大限度靠近遠(yuǎn)端參考點(diǎn)Pfar原則，基于汽車(chē)最小轉(zhuǎn)彎半徑更新當(dāng)前路徑節(jié)點(diǎn)P1.

2 泊車(chē)路徑規(guī)劃

當(dāng)汽車(chē)沿著自主代客泊車(chē)引導(dǎo)路徑行駛到斜向泊車(chē)位附近時(shí)，泊車(chē)路徑用于將汽車(chē)安全地引導(dǎo)到斜向泊車(chē)位，進(jìn)而完成自主代客泊車(chē)任務(wù)［13］.考慮初始泊車(chē)方位角的任意性，以及斜向泊車(chē)位方位角的非唯一性，基于Dubins曲線，本節(jié)規(guī)劃針對(duì)斜向泊車(chē)位的泊車(chē)路徑.

Dubins 曲線是在滿足汽車(chē)非完整約束和邊界約束條件下，連接泊車(chē)起始點(diǎn)和目標(biāo)點(diǎn)的最短路徑.采用R表示汽車(chē)以最小轉(zhuǎn)彎半徑右轉(zhuǎn)，采用S表示汽車(chē)沿著直線行駛，采用L 表示汽車(chē)以最小轉(zhuǎn)彎半徑左轉(zhuǎn)，則連接泊車(chē)起始點(diǎn)和目標(biāo)點(diǎn)的最短路徑是圖6所示的四種Dubins 曲線之一，即RSR，RSL，LSR 和LSL.本節(jié)基于文獻(xiàn)［14］提出的方法，計(jì)算連接泊車(chē)起始點(diǎn)和目標(biāo)點(diǎn)的四種Dubins 曲線，并將無(wú)碰撞且最短的Dubins曲線作為泊車(chē)路徑.

圖6 基于Dubins曲線的泊車(chē)路徑Fig.6 The parking path based on Dubins curves

3 仿真分析

在VC++6.0 環(huán)境中，仿真驗(yàn)證本文提出的基于改進(jìn)快速行進(jìn)樹(shù)算法的自主代客泊車(chē)路徑規(guī)劃方法.仿真驗(yàn)證過(guò)程采用的汽車(chē)參數(shù)為：汽車(chē)軸距L=2.405 m、汽車(chē)寬度W=1.523 m、汽車(chē)前懸長(zhǎng)度Lf=0.800 m、汽車(chē)后懸長(zhǎng)度Lr=0.950 m 和汽車(chē)最小轉(zhuǎn)彎半徑Rmin=4.200 m，仿真結(jié)果如圖7 所示.考慮汽車(chē)輪廓的影響和后續(xù)路徑跟蹤控制模塊容差要求，對(duì)障礙物進(jìn)行了膨脹處理，圖7 中的虛線封閉區(qū)域?yàn)檎系K物膨脹區(qū)域.

圖7 自主代客泊車(chē)路徑規(guī)劃結(jié)果Fig.7 The planning results of autonomous valet parking path

圖7（a）為采用類廣度優(yōu)先搜索策略建立的環(huán)境地圖“路徑場(chǎng)”，圓圈和五角星分別表示自主代客泊車(chē)起始點(diǎn)和目標(biāo)點(diǎn)；圖7（b）為快速行進(jìn)樹(shù)算法沿著圖7（a）所示的環(huán)境地圖“路徑場(chǎng)”梯度下降方向規(guī)劃出的引導(dǎo)路徑，粗實(shí)線表示引導(dǎo)路徑，細(xì)實(shí)線表示當(dāng)前節(jié)點(diǎn)的梯度下降方向；圖7（c）、圖7（d）為本文提出的改進(jìn)快速行進(jìn)樹(shù)算法規(guī)劃出的引導(dǎo)路徑，粗實(shí)線表示改進(jìn)快速行進(jìn)樹(shù)算法規(guī)劃出的引導(dǎo)路徑，虛線表示快速行進(jìn)樹(shù)算法規(guī)劃出的引導(dǎo)路徑；圖7（e）為汽車(chē)沿自主代客泊車(chē)路徑行駛產(chǎn)生的包絡(luò).由圖7（b）～（e）可知，快速行進(jìn)樹(shù)算法和本文提出的改進(jìn)快速行進(jìn)樹(shù)算法規(guī)劃出的引導(dǎo)路徑均為無(wú)碰撞路徑.并且兩種算法規(guī)劃的路徑長(zhǎng)度相當(dāng)，但快速行進(jìn)樹(shù)算法規(guī)劃出的引導(dǎo)路徑存在方向變化率較大的折點(diǎn)A和B，而本文提出的改進(jìn)快速行進(jìn)樹(shù)算法規(guī)劃的引導(dǎo)路徑更平滑，滿足汽車(chē)非完整約束要求.同時(shí)，由圖7（e）可知，在初始泊車(chē)方位角不為零時(shí)，本文基于Dubins曲線規(guī)劃出的針對(duì)斜向泊車(chē)位的泊車(chē)路徑可以安全引導(dǎo)汽車(chē)駛?cè)氩窜?chē)位.

4 結(jié)論

1）基于改進(jìn)快速行進(jìn)樹(shù)算法規(guī)劃出自主代客泊車(chē)系統(tǒng)的引導(dǎo)路徑.采用類廣度優(yōu)先搜索策略建立了環(huán)境地圖的“路徑場(chǎng)”，并提出了一種高計(jì)算效率的避障檢測(cè)策略，以及基于環(huán)境地圖“路徑場(chǎng)”提出了一種符合汽車(chē)非完整約束的遠(yuǎn)端參考點(diǎn)和近端參考點(diǎn)選取原則與路徑節(jié)點(diǎn)更新原則.在上述策略和原則的加持下，通過(guò)使路徑節(jié)點(diǎn)逐漸靠近目標(biāo)節(jié)點(diǎn)來(lái)完成自主代客泊車(chē)引導(dǎo)路徑規(guī)劃任務(wù).

2）基于Dubins曲線規(guī)劃出自主代客泊車(chē)系統(tǒng)的泊車(chē)路徑.充分考慮初始泊車(chē)方位角任意性要求和泊車(chē)位方位角的非唯一性要求，計(jì)算連接泊車(chē)起始點(diǎn)和目標(biāo)點(diǎn)的四種Dubins曲線，即RSR，RSL，LSR和LSL，將無(wú)碰撞且最短的Dubins 曲線作為泊車(chē)路徑，進(jìn)而完成泊車(chē)路徑規(guī)劃任務(wù).

3）在VC++6.0 環(huán)境中，仿真驗(yàn)證本文提出的自主代客泊車(chē)路徑規(guī)劃方法的可行性.結(jié)果表明：相對(duì)于傳統(tǒng)的快速行進(jìn)算法，所提方法規(guī)劃的自主代客泊車(chē)路徑滿足汽車(chē)非完整約束要求，可以安全引導(dǎo)汽車(chē)完成自主代客泊車(chē)任務(wù).