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

張家旭，周時瑩，劉曄，郭崇，趙健

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式中：×表示向量積運算；∥表示或運算.

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

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

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

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

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

2 泊車路徑規(guī)劃

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

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

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

3 仿真分析

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

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

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

4 結(jié)論

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

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

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