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

摘要：為了加速自主代客泊車系統(tǒng)落地，基于改進快速行進樹算法提出了一種自主代客泊車路徑規(guī)劃方法.首先，采用類廣度優(yōu)先搜索策略建立環(huán)境地圖的“路徑場”，并提出一種高計算效率的避障檢測策略，基于環(huán)境地圖“路徑場”提出一種符合汽車非完整約束的遠端參考點和近端參考點選取原則與路徑節(jié)點更新原則，通過使路徑節(jié)點逐漸靠近目標節(jié)點來完成自主代客泊車引導路徑規(guī)劃任務.其次，基于Dubins曲線規(guī)劃滿足初始泊車方位角任意性要求和泊車位方位角的非唯一性要求的泊車路徑，引導汽車安全駛入泊車位.最后，仿真驗證所提方法的可行性.結果表明：相對于傳統(tǒng)的快速行進算法，所提方法規(guī)劃的自主代客泊車路徑滿足汽車非完整約束要求，可以安全引導汽車完成自主代客泊車任務.

關鍵詞：自主代客泊車;路徑規(guī)劃;改進快速行進樹算法;避障檢測策略;Dubins曲線

中圖分類號：U461.1文獻標志碼：A

Autonomous Valet Parking Path Planning Based on Modified Fast Marching Tree

ZHANG Jiaxu" ZHOU Shiying LIU Ye GUO Chong ZHAO Jian1

（1. State Key Laboratory7 of Automotive Simulation and Control，Jilin University，Changchun 13002 China;

2. Intelligent Network Ramp;D Institute，China FAW Group Co.，Ltd.，Changchun 13001 China）

Abstract ：In order to accelerate the landing of the autonomous valet parking system，a novel autonomous valet parking path planning method is proposed based on the improved fast marching tree algorithm. Firstly，a breadth- first-search-like strategy is used to establish the “path field”of the environmental map，and an obstacle avoidance detection strategy with high computational efficiency is proposed. A selection principle of the far and near reference points and an update principle of the path node based on the \"path field\" of environmental map are proposed to conform to the vehicle non-holonomic constraint. According to the above proposed strategies and principles，the path node is gradually close to the target node to complete the autonomous valet parking guidance path planning task . Then，on the basis of the Dubins curve，the parking path which meets the arbitrary requirement of the initial vehicle parking azimuth and the non-uniqueness requirement of the parking space azimuth angle is planned to guide the vehicle to enter the parking space safely. Finally，the feasibility of the proposed method is verified by simulation，and the results show that when compared with the traditional fast marching algorithm ，the planned autonomous valet parking path based on the proposed method meets the requirement of the vehicle non-holonomic constraint and can guide the vehicle to complete the task of autonomous valet parking.

Key words ：autonomous valet parking;path planning;improved fast marching tree algorithm;obstacle avoidance detection strategy;Dubins curve

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

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

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

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

1引導路徑規(guī)劃

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

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

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

式中：L_max為點M₁、M₂、N₁和N₂之間的最大距離.

圖3（b）～（4）中線段M₁M₂與線段N₁N₂不相交的充分條件為：

式中：×表示向量積運算;//表示或運算.

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

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

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

2泊車路徑規(guī)劃

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

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

3仿真分析

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

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

4結論

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

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

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

參考文獻

[1] DOLGOV D，THRUN S，MONTEMERLO M，et al. Path planning for autonomous vehicles in unknown semi-structured environments [J] .The International Journal of Robotics Research ，2010，29（5）：485-501.

[2]余卓平，李奕姍，熊璐.無人車運動規(guī)劃算法綜述[J].同濟大學學報（自然科學版），2017，45（8）：1150-1159.

YU Z P，LI Y S，XIONG L.A review of the motion planning problem of autonomous vehicle [J].Journal of Tongji University （Natural Science），2017，45（8）：1150-1159.（In Chinese）

[3] LI B，ZHANG Y M，SHAO Z J，et al. Simultaneous versus joint computing：a case study of multi-vehicle parking motion planning [J] .Journal of Computational Science，2017，20：30-40.

[4] KHALID M，CAO Y，ZHANG X，et al. Towards autonomy ：cost-effective scheduling for long-range autonomous valet parking （LAVP）[C]//2018 IEEE Wireless Communications and Networking Conference. Barcelona，Spain ：IEEE，2018 ：1-6.

[5] KHALID M，CAO Y，ASLAM N ，et al. Optimized pricing amp; scheduling model for long range autonomous valet parking [C]//2018 International Conference on Frontiers of Information Technology （FIT）. Islamabad，Pakistan：IEEE，2018：65-70.

[6]KHALID M ，CAO Y，ASLAM N，et al. AV Park：reservation andcost optimization-based cyber-physical system for long-range autonomous valet parking （L-AVP）[J]. IEEE Access，2019，7：114141-114153.

[7]徐開放.基于D*Lite算法的移動機器人路徑規(guī)劃研究[D].哈爾濱：哈爾濱工業(yè)大學，2017.

XU K F. Research on mobile robot path-planning based on D*Lite algorithm[D] . Harbin：Harbin Institute of Technology，2017.（In Chinese）

[8]KLAUDT S，ZLOCKI A，ECKSTEIN L.A-priori map information

and path planning for automated valet-parking[ C]//2017 IEEE Intelligent Vehicles Symposium. Los Angeles，CA，USA：IEEE，2017：1770-1775.

[9]JEONG Y，KIM S，JO B R，et al. Sampling based vehicle motion planning for autonomous valet parking with moving obstacles[J] . International Journal of Automotive Engineering，2018，9（4）：215-222.

[10]朱冰，韓嘉懿，趙健，等.基于安全場改進RRT*算法的智能汽車路徑規(guī)劃方法J].汽車工程，2020，42（9）：1145-1150.

ZHU B，HAN J Y，ZHAO J，et al. Safety field-based improved RRT* algorithm for path planning of intelligent vehicle [J]. Automotive Engineering，2020，42（9）：1145-1150（. In Chinese）

[11] JANSON L，SCHMERLING E，CLARK A，et al. Fast marching tree：a fast marching sampling-based method for optimal motion planning in many dimensions[J] . The International Journal of Robotics Research，2015，34（7）：883-921.

[12] HAN D，NIE H，CHEN J B，et al. Dynamic obstacle avoidance for manipulators using distance calculation and discrete detection[J] . Robotics and Computer-Integrated Manufacturing，2018，49：98-104.

[13]張家旭，趙健，施正堂，等.基于回旋曲線的垂直泊車軌跡規(guī)劃與跟蹤控制[J].東南大學學報（自然科學版），2020，50（1）：182-191.

ZHANG J X，ZHAO J，SHI Z T，et al. Trajectory planning and tracking control for perpendicular parking based on clothoid curve [J]. Journal of Southeast University （Natural Science Edition），2020，50（1）：182-191（. In Chinese）

[14] SHKEL A M，LUMELSKY V. Classification of the dubins set[J] . Robotics and Autonomous Systems，200 34（4）：179-202.