Frenet坐標框架下的自適應避障算法

匡軍，尤杰，鐘曉婉，萬仁卓，韓冬桂，燕怒*

Frenet坐標框架下的自適應避障算法

匡軍1a，尤杰1b，鐘曉婉2，萬仁卓1b，韓冬桂1a，燕怒1a*

（1.武漢紡織大學 a.機械工程與自動化學院 b.電子與電氣工程學院，武漢 430200； 2.襄陽汽車職業技術學院 智能制造學院，湖北 襄陽 441021）

解決包裝車間無人運輸車輛在沿全局路徑行駛過程中，難以同時保證跟隨軌跡平滑及規避障礙物的問題。構建Frenet坐標框架，描述車輛位姿與全局路徑的相對關系，利用五次多項式生成多條待選局部路徑；采用障礙物勢場法模糊處理包裝車間障礙的外形輪廓，并設置膨脹區間；結合待選路徑采樣點經過障礙物膨脹區間得到的碰撞值，以及采樣點與全局規劃路徑的偏差值，綜合評估路徑的避障能力，篩選出最優的避障路徑。該算法能根據障礙物的大小，動態解算出與全局路徑偏移量小且運動連貫的局部避障路徑。所提算法使得車輛完成運輸任務時的效果更好，且效率更高。

局部路徑規劃；避障算法；最優路徑；Frenet坐標系；自適應

隨著人工智能、計算機技術的迅猛發展，路徑規劃作為無人運輸車輛的核心決策部分，是否具備避障功能是車輛能否安全行駛的關鍵，也是無人運輸技術能否落地實現的前提[1]。局部路徑規劃能根據車體所處運動狀態進行實時動態規劃，在面對障礙時車體會及時做出決策響應，因此車輛避障時的局部路徑規劃是無人運輸車輛必須解決的問題之一[2]。

近年來，國內外已有大量學者研究了局部路徑規劃的相關算法[3–9]。趙明等[4]通過改進的人工勢場法，幫助移動機器人快速到達目標點，避免機器人停滯在疊加勢場的局部穩定點上而無法移動。針對無軌膠輪車井下路徑規劃問題，鮑久圣等[10]利用改進的A*算法，提高了節點搜索效率，加快了路徑規劃時間。雖然上述算法能合理地求解最優的局部路徑，并且優化算法模型，減少計算量，但此類算法的求解路線均以離散點的形式連接而成，對車輛實時運動狀態連貫性的反應較模糊。魏民祥等[11]以自動駕駛汽車為研究對象，基于Frenet坐標系算法模型，以核心安全指標構建損失函數，求解出更加平滑的運動軌跡。Eshtehardian等[12]提出了一種基于RRT*和b樣條相結合的方法，可以平滑快速隨機采樣點路徑，使局部路徑規劃連貫。Durakl等[13]針對路徑規劃中過多節點和尖點問題，利用貝塞爾曲線修剪，丟棄多余的節點，并對峰值進行處理，使得軌跡連續平滑。在有障礙物的情況下，面對不同的障礙，針對車輛動態作出運動決策變更的局部路徑規劃算法的研究較少，其主要原因在于障礙物的空間位置不同且形狀、大小各異，極大地影響了車輛跟隨全局參考路徑行駛軌跡的連貫性，會產生停滯或者轉動角加速度過大等情況[2,14]。由此可見，亟須一種能夠使車輛平滑跟隨全局路徑且具備避障能力的算法，以控制車輛按照規劃路徑行駛。

文中基于Frenet坐標框架，明確車輛位姿與全局路徑的相對位置關系，并將控制量引入局部路徑規劃中，結合路徑采樣點經過障礙物膨脹區間得到的碰撞總值和全局路徑離散點偏差總值，綜合評估路徑的避障質量，從而篩選出最優局部路徑，并反饋到運動規劃中，控制車輛的行駛，使無人運輸車輛按照規劃路徑行駛且具備自主避障能力。

1 靜態地圖信息

此算法模型中的靜態地圖信息包含全局路徑規劃和障礙物信息預設。全局路徑規劃指在包含障礙物的區域內，找到一條從初始點到指定目標點的路徑[11]。為了更易表達車體的實時位置及運動狀態，通常在笛卡爾坐標系中構建地圖場景。在實際規劃過程中，由于存在系統誤差和躲避障礙物的需求，局部路徑與全局路徑之間必然存在偏差。由此，這里提出在Frenet坐標框架下，依據全局路徑將車體運動狀態分解為橫向距離和縱向偏差，2個方向相互垂直，更易表達局部路徑與全局規劃路徑的相對關系，從而方便規劃局部路徑，縮短與全局路徑的縱向偏差[15]。為了較好地處理車輛與障礙物的位置關系，解決障礙形狀輪廓在局部路徑規劃中難以表達的難題，將針對地圖場景中的障礙物進行膨脹預處理。

文獻[2]構建了障礙物的代價勢場，模糊處理了障礙物的形狀輪廓，并證明了它在局部路徑規劃中的有效性。基于此，針對包裝車間車輛運輸的環境，車輛需要繞行的障礙多為陣列的方形，但大小和輪廓不一，因此這里以方形障礙末端位置為圓心，根據輪廓大小設置半徑向四周膨脹，建立膨脹區域。障礙物經膨脹處理前后車輛運行軌跡的變更如圖1所示。

圖1 軌跡變更示意圖

在圖1中，虛線為障礙物膨脹處理前車輛的運行軌跡；實線為障礙物膨脹處理后行駛的軌跡；虛線顯示車輛繞行方形障礙時出現了極值，導致車輛運行不平滑，出現速度和轉動變化過大的情況。確定障礙物膨脹區間有利于求解局部路徑的碰撞值。

2 車體位姿坐標系轉化模型

在Frenent框架下規劃局部路徑，需將在笛卡爾坐標系中的車輛位姿轉化到Frenet坐標框架中。構建轉化模型，描述車體位姿運動狀態，如圖2所示。

圖2 車體位姿相對關系的描述

如圖2所示，由起始點s和終止點t規劃全局路徑，在平面笛卡爾坐標下，車輛實時位姿數學變量的計算見式（1）。

式中：x、x為車輛在笛卡爾坐標系中的橫、縱坐標;v為車輛行駛的速度；為車輛行駛的加速度；θ為行駛偏航角；為角加速度。

在Frenet坐標框架下設置車輛的位姿f狀態，見式（2）。

建立數學轉換模型，車輛載體坐標系對應的笛卡爾坐標為(x,x)。為車輛對全局路徑的投影點，對應的笛卡爾坐標為(r,r)。令向量、向量為平面全局坐標系下正交單位向量；向量、向量為處相互正交的單位向量；向量r、向量r為r處相互正交的單位向量；θ為向量與軸正向的夾角，表示車輛實時位姿角度；r為向量r與軸正向的夾角，表示車輛規劃位姿角度；?為車輛實時位姿角度與規劃位姿角度的偏差。通過公式推導可得笛卡爾坐標與Frenet坐標的數學轉化模型，見式（3）—（5）。

3 最優軌跡篩選策略

最優軌跡篩選策略由2個部分組成。第1部分，將獲取車輛的實時位姿，由式（3）—（5）轉化至Frenet坐標框架中，獲得與全局規劃路徑縱向偏差和橫向位移的相對關系，并以此作為起始狀態點。然后根據需求在縱向（偏差）和橫向（位移）上，分別規劃多個終止狀態點，以五次多項式的形式，求解多條隨時間變化的曲線。針對車輛的運動狀態，構建損失函數，以相應權重來計算2個方向耦合得到的軌跡質量，并設置軌跡質量閾值，初步篩選出滿足規劃要求的局部路徑集合。第2部分，將Frenet框架下耦合得到的局部規劃路徑集合，反饋到笛卡爾坐標系中，得到多條笛卡爾坐標系中的局部規劃路徑，并從2個維度對生成的軌跡進行評估。第1個維度從安全性角度出發，根據地圖中的障礙信息，計算每條待選軌跡的碰撞總值。第2個維度從跟隨效果出發，計算軌跡采樣點與全局規劃路徑的偏差，得到每條待選軌跡的偏差總值。權衡求解出偏差較小且安全性較高的最優跟隨軌跡。最后，依據該路徑曲線求解出采樣間隔時間?后的車輛位姿，進而控制車輛平滑行駛至該規劃點，實現最優局部跟隨。為了保證路徑的平滑，將以此規劃點作為下次規劃點的初始狀態點，循環往復，直至車輛到達目標點，從而完成全局路徑跟隨。

3.1 Frenet框架下局部路徑規劃

為了保證運動狀態連貫且平滑，依據Frenet框架中的Jerk模型，可將車輛位姿隨時間變化的橫向距離和縱向偏差用五次多項式擬合為曲線，表示時刻，見式（6）。

通過規劃Frenet坐標橫向距離f和縱向偏差f的初始狀態和終止狀態，可求解a與a各系數的值，見式（7）—（9）。

針對縱向偏差f，根據車輛大小和全局路徑寬度，可將終止狀態點與全局路徑偏差(e)控制在[?, +]距離區間內，負號表示規劃局部軌跡穿過規劃全局路徑，正號表示未穿過，并通過設置?為等間距偏差，來控制待選路徑的數量密度。終止點運動狀態與全局規劃路徑切向同向，因此l(e)和l(e)為0，由式（9）可規劃得到種縱向偏差f隨時間變化的曲線。

同樣需要規劃橫向距離f的終止點，在一定范圍內動態設置s(e)和s(e)，滿足車輛行駛過程中的速度需求。將式（7）—（8）代入式（6），得到規劃曲線集合。

由于車輛局部運動速度和加速度規劃往往會影響車輛運動狀態的連貫性，并且面對載人車輛，該規劃直接影響乘坐人員的舒適程度[11]。基于此，將2個方向規劃的曲線，以時間間隔?離散為個采樣點。根據曲線加速度、加速度變化率等信息構建損失函數，并以相應權重計算路徑質量，見式（10）—（12）。

式中：(l)、(s)分別為縱向損失函數和橫向損失函數；j、d為比例系數；k、k為2個方向上的加權系數，它們分別控制橫向維度和縱向維度對局部軌跡規劃的影響；為該局部軌跡的綜合得分，以此來評估該軌跡的優劣情況，通過設置閾值，篩選出多條符合預期的待選局部軌跡曲線。

3.2 求解最優局部避障軌跡

如圖3所示，無箭頭實線表示車輛需跟隨的全局規劃路徑，有箭頭的實線為局部待選軌跡集合，并將其中的軌跡4用虛線表示，便于描述算法的處理方式，曲線上實心圓為采樣點。為單個軌跡采樣點的偏差距離，為單個采樣點到附近障礙物的距離，為附近障礙物的膨脹半徑。如圖3中軌跡4所示，將局部軌跡離散為多個采樣點坐標(li,li)，與之對應的全局路徑離散點坐標為(gi,gi)，障礙物的膨脹區域圓心坐標為(d,d)，并根據式（13）—（17）分別計算路徑的評估質量。

圖3 碰撞總值和偏差總值的計算模型

將式（13）代入式（15）中，求得該路徑的碰撞總值c，根據前面障礙物地圖設置中所述，表示處于障礙物膨脹區域內的離散點數量，障礙物膨脹區域外離散點碰撞值為0。將式（14）代入式（16）中，可以求得路徑的偏差總值d，表示該局部路徑離散采樣點的總數量。最后由式（17）計算出從2個維度綜合得到的路徑評估值V。1、2為比例系數。不難發現，當c較小時，車輛局部軌跡遠離障礙物，安全性較高。當d較小時，車輛局部運動軌跡與全局規劃路徑偏差較小。由此可見，V最小的曲線為最優避障曲線，能使局部路徑在規避障礙的同時，與全局規劃路徑的偏移量最小。

4 實驗分析

4.1 系統實驗平臺搭建

為了驗證該算法的可行性，這里搭建了一輛以STM32F103芯片為底層驅動板的四輪小車，以STM32F407芯片為核心控制板，用以寫入算法。在實驗初始過程，由起始出發點和終止目標點生成全局路徑，將障礙物的位置、大小、形狀通過膨脹算法處理，生成靜態地圖，并賦值到芯片處理系統中，以校準小車起始點位姿。在實驗過程中，通過搭載MEMS-INS/GNSS北斗導航系統獲取車體的實時位姿數據，用以觀測車輛的行駛情況[16]，并結合全局路徑和障礙物信息，解算出最優跟隨軌跡，并轉化為小車驅動控制量，以控制小車行駛。搭建的系統如圖4所示，實驗場景如圖5所示。

圖4 車體系統實物

圖5 實驗場景

4.2 結果及分析

實驗著重關注車輛在規避不同障礙的同時跟隨全局路徑的運動狀態，設計了2組實驗。第1組實驗，在全局規劃路徑且車輛初始狀態相同的情況下，記錄車輛在存在障礙物和不存在障礙物的情況下跟隨全局規劃路徑的運動狀態。第2組實驗，在障礙物位置相同但因大小、形狀的不同，通過膨脹處理后膨脹區域半徑不同的情況下，記錄車輛跟隨全局規劃路徑的狀態。將3種不同障礙物預設寫入算法系統中，觀測能否解算出偏移量較小的避障跟隨軌跡。由此，保持實驗車體的初始狀態一致，初始速度為2 m/s，初始位置與參考軌跡的偏差為1 m，最大加速度為2 m/s2，生成跟隨待選軌跡的曲率，并限制在1 m?1，糾偏時間采樣間隔為0.2 s，利用python工具包matplotlib繪制仿真效果圖。

如圖6—8所示，粗實線為全局參考路徑，虛線為車輛最終的跟隨軌跡，呈扇形的多條較細實線表示文中算法在規劃采樣點處求解得到的多條待選軌跡，五角星與虛線圓圈共同表示障礙物的位置與相應的膨脹區域，三角形表示車體在行駛過程中的規劃采樣點。在參考軌跡一致時，第1組實驗中車體的跟隨情況如圖6～7所示。根據車輛跟隨全局規劃路徑行駛過程中生成的行駛軌跡可知，車體在行駛過程中有效地繞過了障礙物，并有能力平滑跟隨全局參考路徑。在第2組實驗中（圖7～8），在相同參考路徑和障礙物位置的情況下，將障礙物的膨脹半徑分別設置為1 m和2 m，車輛的局部運動決策分別為從障礙物上方通過及從障礙物下方通過。通過分析可知，車輛在面對較大的障礙物時，繼續選擇上方通過會導致跟隨軌跡與全局規劃路徑的偏差較大，從而求解出向下方通過的運動決策，滿足了避障要求的同時，選擇了偏移量更小的局部路徑。由此可知，求解的最優跟隨軌跡隨著膨脹區域的變化而變化，車輛行駛能有效繞過障礙物，且平滑地跟隨全局路徑。

通過上述實驗，驗證了此避障算法在車輛相對于全局路徑存在偏差時，能夠有效求解平滑的跟隨軌跡，并可根據障礙物的位置從待選軌跡中篩選出更合理的跟隨軌跡，來決策控制車輛，能較好地滿足路徑規劃中的避障要求。

圖6 無障礙物時車輛跟蹤軌跡

圖7 障礙物較小時車輛跟蹤軌跡

圖8 障礙物較大時車輛跟蹤軌跡

5 結語

基于Frenet坐標框架數學模型，利用Frenet坐標系對車輛位姿和參考全局路徑的高表達性，在Frenet框架下求解多條待選軌跡，且映射到笛卡爾坐標系中，通過障礙物膨脹來優化算法，篩選出具有避障能力的最優軌跡，進而控制車輛行駛，并進行了實測。

1）算法使得車輛平滑安全地跟隨全局參考路徑，并自主從起始點行駛到終止點。

2）算法求解的局部跟隨軌跡平滑且兼顧避障，解決了車輛遇到障礙物行駛不連貫的問題。

3）通過實驗驗證，該算法能夠使車輛在面對障礙物大小、形狀不同的情況下做出相應的決策改變，使車輛面對不同環境具有較好的適應能力。

[1] 袁春, 龔城, 何成誠, 等. Frenet坐標系及凸近似避障原理的無人車局部路徑規劃[J]. 重慶理工大學學報(自然科學), 2022, 36(4): 59-67.

YUAN Chun, GONG Cheng, HE Cheng-cheng, et al. Local Path Planning for Unmanned Vehicles Based on Frenet Coordinate System and Convex Approximate Obstacle Avoidance Principle[J]. Journal of Chongqing University of Technology (Natural Science), 2022, 36(4): 59-67.

[2] 遲勝凱, 謝永芳, 陳曉方, 等. 基于障礙物代價勢場的移動機器人避障算法[J]. 北京航空航天大學學報, 2022, 48(11): 2289-2303.

CHI Sheng-kai, XIE Yong-fang, CHEN Xiao-fang, et al. Obstacle Avoidance Algorithm of Mobile Robot Based on Obstacle Cost Potential Field[J]. Journal of Beijing University of Aeronautics and Astronautics, 2022, 48(11): 2289-2303.

[3] 錢東海, 孫林林, 趙偉. 基于三次B樣條曲線的叉車型AGV路徑規劃研究[J]. 計算機測量與控制, 2022, 30(4): 177-181.

QIAN Dong-hai, SUN Lin-lin, ZHAO Wei. Study on Forklift AGV Path Planning Based on Cubic B-Spline Curve[J]. Computer Measurement & Control, 2022, 30(4): 177-181.

[4] 趙明, 鄭澤宇, 么慶豐, 等. 基于改進人工勢場法的移動機器人路徑規劃方法[J]. 計算機應用研究, 2020, 37(S2): 66-68.

ZHAO Ming, ZHENG Ze-yu, YAO Qing-feng, et al. Path Planning Method of Mobile Robot Based on Improved Artificial Potential Field Method[J]. Application Research of Computers, 2020, 37(S2): 66-68.

[5] 朱佳瑩, 高茂庭. 融合粒子群與改進蟻群算法的AUV路徑規劃算法[J]. 計算機工程與應用, 2021, 57(6): 267-273.

ZHU Jia-ying, GAO Mao-ting. AUV Path Planning Based on Particle Swarm Optimization and Improved Ant Colony Optimization[J]. Computer Engineering and Applications, 2021, 57(6): 267-273.

[6] 楊明亮, 李寧. 改進A*算法的移動機器人路徑規劃[J]. 機械科學與技術, 2022, 41(5): 795-800.

YANG Ming-liang, LI Ning. Study on Mobile Robot Path Planning Based on Improved A*Algorithm[J]. Mechanical Science and Technology for Aerospace Engineering, 2022, 41(5): 795-800.

[7] ATIYAH A N, ADZHAR N, JAINI N I. An Overview: On Path Planning Optimization Criteria and Mobile Robot Navigation[J]. Journal of Physics: Conference Series, 2021, 1988(1): 012036.

[8] SATHIYA V, CHINNADURAI M, RAMABALAN S. Mobile Robot Path Planning Using Fuzzy Enhanced Improved Multi-Objective Particle Swarm Optimization (FIMOPSO)[J]. Expert Systems With Applications, 2022, 198: 116875.

[9] 王沙晶, 陳建業. 基于Frenet坐標系的智能車運動規劃研究[J]. 移動電源與車輛, 2019(1): 22-29.

WANG Sha-jing, CHEN Jian-ye. Motion Planning of Autonomous Vehicle Based on Frenet Coordinate System[J]. Movable Power Station & Vehicle, 2019(1): 22-29.

[10] 鮑久圣, 張牧野, 葛世榮, 等. 基于改進A*和人工勢場算法的無軌膠輪車井下無人駕駛路徑規劃[J]. 煤炭學報, 2022, 47(3): 1347-1360.

BAO Jiu-sheng, ZHANG Mu-ye, GE Shi-rong, et al. Underground Driverless Path Planning of Trackless Rubber Tyred Vehicle Based on Improved A*and Artificial Potential Field Algorithm[J]. Journal of China Coal Society, 2022, 47(3): 1347-1360.

[11] 魏民祥, 滕德成, 吳樹凡. 基于Frenet坐標系的自動駕駛軌跡規劃與優化算法[J]. 控制與決策, 2021, 36(4): 815-824.

WEI Min-xiang, TENG De-cheng, WU Shu-fan. Trajectory Planning and Optimization Algorithm for Automated Driving Based on Frenet Coordinate System[J]. Control and Decision, 2021, 36(4): 815-824.

[12] ESHTEHARDIAN S A, KHODAYGAN S. A Continuous RRT*-Based Path Planning Method for Non- Holonomic Mobile Robots Using B-Spline Curves[J]. Journal of Ambient Intelligence and Humanized Computing, 2023, 14(7): 8693-8702.

[13] DURAKL? Z, NABIYEV V. A New Approach Based on Bezier Curves to Solve Path Planning Problems for Mobile Robots[J]. Journal of Computational Science, 2022, 58: 101540.

[14] XU Guo-qiang, JIANG Wei-lai, WANG Zhao-lei, et al. Autonomous Obstacle Avoidance and Target Tracking of UAV Based on Deep Reinforcement Learning[J]. Journal of Intelligent & Robotic Systems, 2022, 104(4): 60.

[15] 聶光明, 謝波, 田彥濤. 基于Frenet框架的協同自適應巡航控制算法設計[J]. 吉林大學學報(工學版), 2022, 52(7): 1687-1695.

NIE Guang-ming, XIE Bo, TIAN Yan-tao. Design of Cooperative Adaptive Cruise Control Algorithm Based on Frenet Framework[J]. Journal of Jilin University (Engineering and Technology Edition), 2022, 52(7): 1687-1695.

[16] 高揚, 徐永貴, 劉江. 基于GPS和視覺同步定位與建圖的無人駕駛車輛定位方法研究[J]. 汽車技術, 2021(6): 19-24.

GAO Yang, XU Yong-gui, LIU Jiang. Research on Localization Method for Unmanned Vehicle Based on GPS and VSLAM[J]. Automobile Technology, 2021(6): 19-24.

Adaptive Obstacle Avoidance Algorithm Based on Frenet Coordinate Framework

KUANG Jun1a, YOU Jie1b, ZHONG Xiao-wan2, WAN Ren-zhuo1b, HAN Dong-gui1a, YAN Nu1a*

(1. a. School of Mechanical Engineering and Automation, b. School of Electronic and Electrical Engineering, Wuhan Textile University, Wuhan 430200, China; 2. School of Intelligent Manufacturing, Xiangyang Auto Vocational Technical College, Hubei Xiangyang 441021, China)

The work aims to address the difficulty of ensuring smooth following trajectories and obstacle avoidance at the same time when unmanned transport vehicles are travelling along a global path in the packaging workshop. The Frenet coordinate frame was constructed to describe the relative relationship between the vehicle pose and the global path. A quintic polynomial was adopted to generate multiple local paths to be selected.The obstacle profile of the packaging workshop was fuzzy processed by the obstacle potential field method, and the expansion interval was set. Combined with the collision values calculated for the sampling points of the path to be selected in the obstacle expansion interval, and the deviation values of the sampling points from the globally planned paths, the obstacle avoidance quality of the local paths was comprehensively evaluated to filter out the optimal obstacle avoidance path. According to the size of obstacles, the algorithm could dynamically calculate the local obstacle avoidance path with small offset from the global path and coherent motion. The proposed algorithm makes the vehicle complete the transport task with better effect and higher efficiency.

local path planning; obstacle avoidance algorithm; optimal path; Frenet coordinate system; adaptive

TP242

A

1001-3563(2023)19-0165-06

10.19554/j.cnki.1001-3563.2023.19.021

2022-11-28

國家自然科學基金（51775388）

責任編輯：彭颋