封閉園區內無人駕駛洗掃車路徑規劃及控制

作者簡介：葉明（1976－），男，重慶人，教授，碩導，博士，主要研究方向為新能源與智能汽車；周俊充（1995－），男（通信作者），四川達州人，碩士研究生，主要研究方向為智能汽車（zhoujunchong@2020.cqut.edu，cn）；鄭毅（1983－），男，實驗師，主要研究方向為汽車服務工程；盧祥偉（1997－），男，碩士研究生，主要研究方向為智能汽車；劉永剛（1982－），男，重慶人，教授，博導，博士，主要研究方向為新能源汽車動力系統優化與控制、智能汽車決策與控制技術.

摘 要：為實現封閉園區自動洗掃車的無人駕駛功能，設計了一種基于封閉園區場景的路徑規劃與控制算法。采用改進的A－Star算法并融合三階貝塞爾曲線構建全局路徑規劃層；基于狀態空間采樣設計局部路徑規劃層，考慮洗掃車轉向約束、安全碰撞距離、道路邊界距離等約束設計代價函數得到最優局部路徑；使用Pure－Pursuit跟蹤算法對局部路徑規劃層輸出的最優路徑進行追蹤；搭建Simulink－ROS聯仿平臺，在Simulink中編寫規劃控制算法并編譯植入到ROS平臺下進行仿真驗證；最后采用MDC300F車載計算機對算法進行多工況實車實驗。結果表明，該規劃控制算法能夠控制自動環衛洗掃車在多工況場景下完成跟車、避障、會車等無人駕駛功能。

關鍵詞：無人駕駛；路徑規劃；A-Star算法；貝塞爾曲線；狀態空間采樣；純追蹤算法

中圖分類號：TP323.19 文獻標志碼：A 文章編號：1001－3695（2023）04－007－1000－06doi： 10.19734/j.issn.1001－3695.2022.09.0456

Abstract：To realize the unmanned function of automatic washing and sweeping vehicles in closed parks， this paper designed a path planning and control algorithm based on a closed park scenario. It constructed the global path planning layer by using the improved A-Star algorithm combined with the third-order Bezier curve. It designed the local path planning layer based on the state space sampling， and obtained the optimal local path by considering the constraints such as the steering constraints of the sweeper， the safe collision distance， and the road boundary distance. It used the Pure-Pursuit tracking algorithm to track the optimal path output by the local path planning layer. Then， it built the Simulink-ROS joint simulation platform， compiled the planning control algorithm in Simulink， and embedded it into the ROS platform for simulation verification. Finally， it used the MDC300F vehicle-mounted computer to carry out the multi-condition real-vehicle experiment of the algorithm. The results show that the planning control algorithm can control the automatic sanitation washing and sweeping vehicle to complete unmanned functions such as following cars， avoiding obstacles， and meeting cars in multiple working conditions.

Key words：unmanned driving; path planning; A-Star algorithm; Bezier curve; state space sampling; pure-pursuit algorithm

0 引言

近年來，無人駕駛汽車成為了社會的熱點，也廣泛商用于封閉園區，如物流配送車、環衛車、園區觀光車等，而路徑規劃是無人駕駛的核心技術之一［1］。無人駕駛車輛路徑規劃是指根據周圍環境信息，規劃出一條從起始點到終止點滿足無人駕駛需求的一系列最優性能指標的路徑的過程［2］。無人駕駛中的路徑規劃分為全局路徑規劃層和局部路徑規劃層［3］。全局路徑規劃是在已知行駛環境先驗信息后，結合主車的GPS位置信息，規劃生成一條滿足車輛運動學約束且兼顧車輛安全性、高效性以及穩定性的全局整體無碰撞路徑［4］。局部路徑規劃是根據全局參考路徑信息和車載智能傳感器獲取實時環境信息，從而生成一條最優路徑反饋給底層跟蹤控制器［4，5］。

當前對車輛的路徑規劃方法研究大致分為基于搜索的規劃方法、人工勢場法、基于隨機采樣的規劃方法和基于離散優化的規劃方法四類［6］。基于圖搜索的路徑規劃方法通過使周圍行車道路離散化，生成柵格環境電子地圖，然后根據代價值最小快速搜索出一條最優路徑，常見的圖搜索算法有Dijkstra、A-Star和D-Star［7］。其中A-Star算法被認為是綜合性最好的圖搜索算法。A-Star算法通過啟發函數引導搜索方向，通過估計函數使靠近目標的節點優先擴展，從而計算出一條代價最小的最優路徑，但在復雜應用環境中，由于狀態格子較多，存在搜索效率低、路徑不平滑的問題。Xu等人［8］基于A-Star算法，引入多個啟發函數，提出了BB-Star算法，提高了搜索效率。Zhang等人［9］通過增加A-Star算法搜索方向的數量，減少路徑拐點和長度。人工勢場法是由Khatib［10］首次提出，該算法計算簡單且路徑光滑，但容易陷入局部最優?；陔S機采樣的規劃方法有概率道路圖（probabilistic road map，PRM）和快速搜索隨機樹（rapidly-exploring random tree，RRT）［7］，該算法搜索能力強，但得不到最優解?；陔x散優化的規劃方法［11］通過對結構化道路進行序列性采樣，提高了采樣效率，減小了計算難度。

對于橫向控制，常用研究方法包括PID （proportional-integral-derivative，）控制、預瞄控制、模糊控制、純追蹤控制、滑?？刂坪妥顑灴刂频龋?2］。譚寶成等人［13］提出了一種新型增量式PID控制算法，使無人車跟蹤調整時間減少，超調量減小，車輛實現快速穩定的路徑跟蹤。李宏剛等人［14］基于預瞄跟隨理論對橫向控制和縱向控制進行解耦，并通過反饋機制實時進行偏差修正，實現精準的橫縱向控制。張蘇才等人［15］在無人駕駛方程式賽中提出了一種基于純追蹤和模型預測控制的橫向混合控制策略。

針對純電動環衛洗掃車在封閉園區內自動駕駛的行車環境要求，并提高車輛的行駛安全性和平順性，使其能夠應對復雜的行車環境和突發狀況，本文設計了一種針對封閉園區自動駕駛洗掃車的路徑規劃及控制算法。

1 全局路徑規劃

針對封閉園區內無人駕駛洗掃車全局路徑規劃的問題，以A-Star算法作為基礎算法，重新構建啟發函數，確定搜索方向以提高搜索效率。由于車身尺寸和轉彎半徑較大，利用三階貝塞爾曲線對全局路徑進行平滑處理，并通過仿真實驗數據對比得出：改進的A-Star算法在路徑規劃時，與傳統A-Star算法相比，平均路徑長度縮短6.3%，路徑角度縮小65.3%，危險碰撞次數減少100%，新的全局路徑規劃算法提高了自動駕駛車輛的安全性、平順性和工作效率。

1.1 A-Star算法簡介

A-Star算法是一種靜態路網中求解最短路徑最有效、運用最廣泛的啟發式搜索算法。該算法通過評價函數進行引導搜索與決定方向。評價函數為

f（k）=g（k）+h（k）（1）

其中：f（k）為從起始點經由節點k到目標點的估計代價；g（k）為從起始點到節點k的實際代價；h（k）為從當前節點k到目標點的啟發式代價函數。

1.2 A-Star算法改進

如圖1所示，為解決傳統A-Star算法搜索效率低、拐點頻繁、路徑優化不足等問題，對A-Star算法進行了以下優化。

1）提高搜索效率

將父節點的節點信息引入到子節點的啟發函數中，節點的方向角為算法提供了方向性，利用距離信息重新構造啟發函數，從而加快搜索速度，則改進后的A-Star算法評價函數為

f（k）=g（k）+（h（k）+h（f）－Wcos σ）edng/dsg（2）

其中：h（f）為當前父節點啟發函數；σ為節點方向角；W為權重值；dng、dsg分別為當前節點、起點與目標點之間的距離。

2）消除多余拐點

考慮封閉園區內純電動智能環衛洗掃車實際應用情況，針對車身較大、轉彎半徑需求較高這一特點，進行拉直路徑消除多余拐點處理。在全局路徑上利用二階導數檢測拐點，然后由起始點出發，每隔一個拐點連接起來。若連線上有障礙物，則保留原始路徑；否則刪除原始路徑，保留連線，直至判斷到終點。優化前后對比如圖2所示。

3）路徑平滑處理

本文使用三階貝塞曲線自適應分段擬合方法對規劃的路徑進行平滑處理。如圖3所示，三階貝塞爾曲線是由四個控制點P0、P1、P2、P3唯一確定的。

如圖4所示，改進后的A-Star算法規劃出的路徑折角更小，且路徑長度更短，在拐角處的碰撞概率減小，更加符合純電動智能環衛洗掃車在封閉園區內智能駕駛的路徑要求，其路徑長度等相關數據如表1所示。

2 局部路徑規劃

本文根據純電動自動駕駛環衛洗掃車行駛特點建立車輛運動學模型。采用基于狀態空間采樣的局部路徑生成方法，生成符合運動學約束的路徑，設計代價函數從而得到最優候選路徑。

2.1 車輛運動學模型

本文基于阿克曼轉向幾何（Ackerman steering geometry）理論［16］建立運動學模型，如圖5所示。

2.2 局部路徑生成

針對動態局部避障路徑，以全局路徑作為參考路徑，設計了一種基于采樣的局部路徑生成方法。該算法的基本思想為：基于車道中心線的離散路徑點，向橫向與縱向生成多個采樣點，同時，為了使最優路徑具有避障趨勢，將各個采樣點的起始狀態與期望的末端狀態依次擬合為多個可行駛軌跡，最后根據評價函數選擇出代價最小的軌跡作為控制器的輸入。

2.2.1 狀態空間采樣

本文在2.1節中建立了車輛運動學模型，用狀態Z來描述，如式（9）所示。在狀態空間中對車輛行駛下一時刻的期望狀態進行采樣，能夠有效減少無效路徑生成，同時保證在空間位姿上車輛末端狀態的完備性。

如圖6所示，將生成路徑的末端狀態和參考路徑對齊可以防止車輛進入不安全的末端姿勢，并提供有效的采樣方案。此外，由于候選路徑的計算是完全并行的，所以使用并行計算技術來增加沿縱向和橫向的采樣密度，可以潛在地提高最優路徑的質量。

在路徑生成之后，為提高車輛行駛安全性和平順性［19］，避免不規則的加速/減速縱向運動，并緩解橫向側滑效應，考慮了以下約束條件，如表2所示。

3 基于Pure-Pursuit算法的橫向控制設計

純追蹤算法以車后軸為切點、車輛縱向車身為切線，通過控制前輪轉向角，使車輛可以沿著一條經過目標路徑點的圓弧行駛，如圖8所示。

本文在Pure-Pursuit算法的基礎上還使用了Stanly、LQR（linear quadratic regulator）和MPC（model predictive control）控制算法進行了橫向控制實驗對比。圖9（a）是不同跟蹤算法下對換道路徑的跟蹤效果，可以看出，Pure-Pursuit算法相比于其他控制算法更快地跟蹤上換道路徑；從圖9（b）～（e）的數據可以看出，Pure-Pursuit算法的控制效果收斂性更好，車輛的橫向偏移量更小，前輪轉角變化量及其變化增量的變化幅值更低，因此橫向控制效果更加穩定；同時從圖9（f）可以看出，使用Pure-Pursuit算法進行橫向跟蹤的迭代時間相比于其他控制算法相差不大。綜上所述，自動洗掃車使用Pure-Pursuit算法進行橫向控制的效果優于其他控制算法。

4 實車實驗

為實現自動駕駛洗掃車在封閉園區內的無人行駛功能，實驗為洗掃車搭載了華測CGI-610GPS、AXIS攝像頭、北科天繪32線激光雷達等智能傳感器，采用華為MDC300F車載計算機，在某車企的封閉園區內進行多工況實車實驗，主要包括行人橫穿、前車制動、讓行同向車輛、讓行對向車輛、前方兩輪車交互、跟隨前方同車道車輛行駛的工況場景，以驗證洗掃車的行人避讓、前車制動、同車道車車交互等智能行駛功能，并對洗掃車在各工況下的行駛數據進行分析，整個實驗的流程框架如圖10所示，車輛參數如表4所示。

為使車輛滿足封閉園區內行駛功能，依據《重慶市自動駕駛開放道路準入測試方案V1.0》對車輛進行以下工況測試。

工況1 自動緊急制動

1）行人橫穿

圖11（a）是主車在行人橫穿場景下的避障讓行實驗，該實驗用于驗證車輛對行人的避障讓行功能，其分析結果如圖11（b）所示。從圖中可以看出，交互初始時刻，行人在車道外靜止等待橫穿，車輛速度為40 km/h，隨著時間推移，當車輛行駛到距離行人15 m處，車輛開始減速（T=12 s），車輛距離行人的縱向距離減小梯度變緩；同時，行人觀察到車輛開始減速之后便決定爭取交互主動權，開始橫穿道路，并在T=31 s時完成橫穿，隨后車輛再次啟動行駛。

2）前車制動

圖12（a）是主車的前向制動實驗，該實驗用于驗證主車對前方車輛的制動安全，其分析結果如圖12（b）所示。從圖中可以看出，主車檢測到與前車的縱向距離逐漸減小至15 m處時，開始減速（T=31 s），直至主車停車，即主車能夠保證與前車的安全距離，完成制動。

工況2 主車讓行

1）讓行同向車輛

圖13（a）是主車與同向車輛駛入讓行工況實驗。從圖13（b）中可以看出，主車以40 km/h的速度行駛，在T=10 s時，前方有一車輛減速匯入同一車道。此刻，主車觀察到前方有車匯入，從而開始減速并與前車保持安全距離在10 m以上，當前方車輛匯入后同一車道后，前車再加速駛離，隨后主車再恢復初始行駛狀態。

2）讓行對向車輛

圖14（a）是主車與對向車輛駛入讓行工況實驗，從圖14（b）中可以看出，主車以40 km/h的速度行駛，對向車輛初始時刻駛入本車道，在相距15 m時，主車開始減速避讓，在此期間，對向車輛也在減速，直至主車停車（T=20 s）后，對向車輛再加速從相鄰車道與主車錯車駛出，待對向車輛駛出本車道時，主車再啟動行駛。

工況3 車道內行駛

1）兩輪車沿道路行駛

圖15（a）是主車對前方兩輪車的交互工況實驗。從圖15（b）中可以看出，主車以40 km/h的速度行駛，于初始時刻檢測到前方同車道有兩輪車占道行駛，隨后主車在T=15 s時開始減速，直至T=46 s時，兩輪車開始減速駛離本車道，當主車未檢測到前方有障礙物時，再恢復至初始工況行駛。

2）跟隨前方同車道車輛行駛

圖16（a）是主車跟隨前方同車道車輛行駛工況實驗。從圖16（b）中可以看出，主車會根據前方車輛的狀態而實時調整自車的行駛策略，例如，在T∈［0，50］s，主車檢測到前方車輛不斷減速，從而實時調整自車的行駛速度，并且縱觀該工況的整個實驗過程，主車都與前車至少保持10 m以上的安全距離。

5 結束語

本文針對封閉園區內無人駕駛洗掃車路徑規劃及控制問題，基于車輛運動學模型，采用A-Star算法和三階貝塞爾曲線融合的全局路徑規劃算法，將其作為參考路徑，采用狀態空間采樣的局部路徑規劃算法生成一條最優路徑，基于車輛動力學模型，使用純追蹤控制算法進行跟蹤控制。

搭建基于ROS/Simulink構架的自動駕駛系統，進行實車多工況實驗。實驗結果表明，在自主駕駛車輛前方縱向距離20 m處檢測到行人/周車，在縱向距離為15 m處主車開始減速行駛，并保持10 m以上安全距離。采用的路徑規劃算法和控制算法能夠在動/靜障礙物場景下實時規劃出局部避障路徑并跟蹤行駛，實現車輛安全穩定行駛。接下來將對駕駛員駕駛行為進行研究，從而依據不同駕駛風格來調整局部規劃的評價函數，實現封閉園區內個性化自動駕駛。

參考文獻：

［1］李克強，戴一凡，李升波，等. 智能網聯汽車 （ICV） 技術的發展現狀及趨勢 ［J］. 汽車安全與節能學報，2017，8（1）： 1－14. （Li Keqiang，Dai Yifan，Li Shengbo，et al. Development status and trend of intelligent connected vehicle （ICV） technology ［J］. Journal of Automotive Safety and Energy，2017，8（1）： 1－14.）

［2］余伶俐，周開軍，陳白帆. 智能駕駛技術： 路徑規劃與導航控制 ［M］. 北京： 機械工業出版社，2020： 3－4. （Yu Lingli，Zhou Kaijun，Chen Baifan. Intelligent driving technology： path planning and navigation control［M］.Beijing：Machinery Industry Press，2020：3－4.）

［3］陳慧巖，熊光明，龔建偉，等. 無人駕駛汽車概論 ［M］. 北京： 北京理工大學出版社，2014： 6－7. （Chen Huiyan，Xiong Guangming，Gong Jianwei，et al. Introduction to driverless cars ［M］. Beijing： Beijing University of Technology Press，2014： 6－7.）

［4］馮酉南. 某無人駕駛車輛路徑規劃算法設計與實驗研究 ［D］. 吉林： 吉林大學，2019. （Feng Younan. Design and experimental research of a driverless vehicle path planning algorithm ［D］. Jilin： Jilin University，2019.）

［5］張鑫辰，張軍，劉元盛，等. 智能車決策路徑規劃的研究進展 ［C］// 中國計算機用戶協會網絡應用分會2019年第二十三屆網絡新技術與應用年會論文集. 2019： 23－27. （Zhang Xinchen，Zhang Jun，Liu Yuansheng，et al. Research progress of intelligent vehicle decision path planning［C］// Proc of the 23rd Annual Confe-rence on New Network Technologies and Applications，Network Application Branch，China Computer Users Association. 2019： 23－27.）

［6］唐斌，許占祥，江浩斌，等. 基于分段優化的車輛換道避障軌跡規劃 ［J］. 汽車工程，2022，44（6）： 831－841. （Tang Bin，Xu Zhan-xiang，Jiang Haobin，et al. Trajectory planning of intelligent vehicles in lane change for collision avoidance based on segmented optimization ［J］. Automotive Engineering，2022，44（6）： 831－841.）

［7］余卓平，李奕姍，熊璐. 無人車運動規劃算法綜述 ［J］. 同濟大學學報：自然科學版，2017，45（8）： 1150－1159. （Yu Zhuoping，Li Yishan，Xiong Lu. A review of the motion planning problem of autonomous vehicle ［J］. Journal of Tongji University：Natural Science，2017，45（8）： 1150－1159.）

［8］Xu Zhe，Liu Xin，Chen Qianglong. Application of improved A-Star algorithm in global path planning of unmanned vehicles ［C］// Proc of Chinese Automation Congress.Piscataway，NJ：IEEE Press，2019： 2075－2080.

［9］Zhang Zhi，Wang Su，Zhou Jie. A-Star algorithm for expanding the number of search directions in path planning ［C］// Proc of the 2nd International Seminar on Artificial Intelligence，Networking and Information Technology. Piscataway，NJ： IEEE Press，2021： 208－211.

［10］Khatib O. Real-time obstacle avoidance for manipulators and mobile robots ［C］// Proc of IEEE International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press，1985： 500－505.

［11］魏民祥，滕德成，吳樹凡. 基于Frenet坐標系的自動駕駛軌跡規劃與優化算法 ［J］. 控制與決策，2021，36（4）： 815－824. （Wei Minxiang，Teng Decheng，Wu Shufan. Trajectory planning and optimization algorithm for automated driving based on Frenet coordinate system ［J］. Control and Decision，2021，36（4）： 815－824.）

［12］謝輝，劉爽爽. 基于模型預測控制的無人駕駛汽車橫縱向運動控制 ［J］. 汽車安全與節能學報，2019，10（3）： 326－333. （Xie Hui，Liu Shuangshuang. Lateral and longitudinal motion control of unmanned vehicles using model predictive control ［J］. Automotive Safety and Energy，2019，10（3）： 326－333.）

［13］譚寶成，王賓. 無人駕駛車輛路徑跟蹤的增量式PID控制 ［J］. 西安工業大學學報，2016，36（12）： 996－1001. （Tan Baocheng，Wang Bin. Path following incremental PID control for unmanned vehicle ［J］. Journal of Xi’an Technological University，2016，36（12）： 996－1001.）

［14］李宏剛，王云鵬，廖亞萍，等. 無人駕駛礦用運輸車輛感知及控制方法 ［J］. 北京航空航天大學學報，2019，45（11）： 2335－2344. （Li Honggang，Wang Yunpeng，Liao Yaping，et al. Perception and control method of driverless mine transport vehicle ［J］. Journal of Beijing University of Aeronautics and Astronautics，2019，45（11）： 2335－2344.）

［15］張蘇才，李剛，王立勇，等. 無人駕駛方程式賽車橫向運動控制研究 ［J］. 濟南大學學報：自然科學版，2022，36（3）： 292－300. （Zhang Sucai，Li Gang，Wang Liyong，et al. Research on lateral motion control of driverless racing cars ［J］. Journal of University of Jinan：Science and Technology，2022，36（3）： 292－300.）

［16］張旭東，徐?？?，鄒淵，等. 融合TangentBug與Dubins曲線的智能輪式車輛局部路徑規劃算法 ［J］. 汽車工程，2021，43（6）： 833－841，869. （Zhang Xudong，Xu Fukang，Zou Yuan，et al. A local path planning algorithm for intelligent wheeled vehicle combining TangentBug and Dubins path ［J］. Automotive Engineering，2021，43（6）： 833－841，869.）

［17］單云霄，郭曉旻，龍江云，等. 漸優隨機采樣算法在結構化道路無人駕駛中的應用 ［J］. 中國公路學報，2018，31（4）： 192－201. （Shan Yunxiao，Guo Xiaomin，Long Jiangyun，et al. Asymptotically sampling-based algorithm with applications to autonomous urban dri-ving on structured road ［J］. China Journal of Highway and Transport，2018，31（4）： 192－201.）

［18］張垚，彭育輝. 無人駕駛汽車離散優化的軌跡規劃算法 ［J］. 福州大學學報：自然科學版，2021，49（4）： 508－515. （Zhang Yao，Peng Yuhui. Development of autonomous vehicle trajectory planning used discrete optimization ［J］. Journal of Fuzhou University：Na-tural Science Edition，2021，49（4）： 508－515.）

［19］袁靜妮，楊林，唐曉峰，等. 基于改進RRT*與行駛軌跡優化的智能汽車運動規劃 ［J］. 自動化學報，2022，48（12）：2941－2950. （Yuan Jingni，Yang Lin，Tang Xiaofeng，et al. Autonomous vehicle motion planning based on improved RRT* algorithm and trajectory optimization ［J］. Acta Automatica Sinica，2022，48（12）：2941－2950.）

［20］彭曉燕，謝浩，黃晶. 無人駕駛汽車局部路徑規劃算法研究 ［J］. 汽車工程，2020，42（1）： 1－10. （Peng Xiaoyan，Xie Hao，Huang Jing. Research on local path planning algorithm for unmanned vehicles ［J］. Automotive Engineering，2020，42（1）： 1－10.