劉公偉，趙 強

改進(jìn)人工勢場法的無人車換道避障路徑規(guī)劃

劉公偉，趙 強

（東北林業(yè)大學(xué) 交通學(xué)院，哈爾濱 150040）

無人車輛；換道避障；路徑規(guī)劃；人工勢場法（APF）；B樣條曲線

0 引言

路徑規(guī)劃是無人駕駛的核心技術(shù)之一，當(dāng)前較為常用的無人駕駛路徑規(guī)劃算法主要有人工勢場法（artificial potential field，APF）、A*算法、遺傳算法（genetic algorithm，GA）和柵格法等[1]。人工勢場法能夠規(guī)劃出較為平滑的路徑且具有良好的實時性，從而得到了廣泛的應(yīng)用[2]。文獻(xiàn)[3]最先提出了人工勢場方法，其原理是目標(biāo)點對機器人產(chǎn)生吸引力，障礙物對機器人產(chǎn)生排斥力，機器人的運動由合力勢場控制，最終繞過障礙物抵達(dá)目標(biāo)點。文獻(xiàn)[4]分析了傳統(tǒng)人工勢場法的缺點，如局部最優(yōu)問題和目標(biāo)不可達(dá)問題。

近年來，針對傳統(tǒng)人工勢場法的不足，研究者們提出了很多改進(jìn)方法。文獻(xiàn)[5]通過將機器人與目標(biāo)點間的距離引入斥力勢場函數(shù)，避免目標(biāo)不可達(dá)問題。文獻(xiàn)[6]通過建立斥力旋轉(zhuǎn)角，避免局部最優(yōu)問題。文獻(xiàn)[7]引入與角度和距離相關(guān)的附加引力，避免局部極小值問題。文獻(xiàn)[8]通過構(gòu)建彎道邊界斥力勢能場、同車道行駛較慢車輛斥力勢能場和彎道引力勢能場，實現(xiàn)在彎道超車的路徑規(guī)劃。文獻(xiàn)[9]通過將強化學(xué)習(xí)（reinforcement learning，RL）和黑洞人工勢場相結(jié)合來解決局部穩(wěn)定點問題，但路徑會出現(xiàn)震蕩。文獻(xiàn)[10]引入灰狼算法（grey wolf optimizer，GWO），將灰狼算法生成的路徑規(guī)劃節(jié)點作為人工勢場算法的臨時目標(biāo)點，解決了傳統(tǒng)人工勢場法生成路徑震蕩的問題。

本文對傳統(tǒng)人工勢場法進(jìn)行優(yōu)化改進(jìn)，更改障礙物斥力勢場函數(shù)與斥力勢場作用范圍，并建立相對速度斥力勢場與新型道路勢場，利用B樣條（B-spline）曲線生成換道路徑簇，篩選合適路徑。最后在多種動態(tài)障礙物環(huán)境下通過仿真實驗來驗證改進(jìn)算法生成路徑的平順性、安全性與可行性。

1 傳統(tǒng)人工勢場法

1.1 引力勢場

引力勢場隨著被控車與目標(biāo)位置之間的距離成正比例變化。引力勢場的勢場函數(shù)為

引力為

式中為被控車所受引力，矢量方向為被控車位置指向目標(biāo)位置。

1.2 斥力勢場

障礙物斥力勢場受被控車和障礙物間的距離影響，當(dāng)被控車沒有進(jìn)入障礙物影響范圍時，其受到的勢場值為零；當(dāng)被控車進(jìn)入障礙物影響范圍后，其受到的勢場值隨二者之間的距離成反比例變化。斥力勢場的勢場函數(shù)為

假設(shè)障礙物位置為道路坐標(biāo)系下點（5 m，5 m），根據(jù)式（3）繪制出障礙物斥力勢場三維示意如圖2所示。

圖2 障礙物斥力勢場三維圖

斥力為

2 改進(jìn)人工勢場法

2.1 改進(jìn)障礙物斥力勢場

為解決目標(biāo)不可達(dá)問題，引入被控車與目標(biāo)點之間的距離關(guān)系，對原斥力勢場函數(shù)進(jìn)行改變，使被控車受到的斥力與引力同時減小，且只有在其達(dá)到目標(biāo)點時，斥力和引力才同時變?yōu)榱?。修改后的斥力勢場的勢場函?shù)為

改進(jìn)后被控車受力如圖3所示。

圖3 被控車受力圖

圖4 改進(jìn)后障礙物斥力勢場三維圖

速度斥力勢場函數(shù)為

2.2 道路勢場

為避免被控車左右擺動震蕩以及避免被控車在車道線附近壓線行駛，建立新型道路勢場，在保留邊界斥力勢場的基礎(chǔ)上，加入車道線斥力勢場，其作用是引導(dǎo)被控車在車道中心線附近行駛。新型道路勢場需滿足以下條件：最低勢場值在車道中心線處，最高勢場值在道路邊界線處，車道線處的勢場值處于二者之間；且最大值應(yīng)該小于障礙物斥力勢場最大值，以確保被控車可以順利跨越車道線。

以同向三車道為例，共有2條車道線與2條邊界線，所設(shè)計的道路勢場函數(shù)為

在邊界線附近時，道路勢場力的方向為垂直邊界線并指向車道線方向；在車道線附近時，道路勢場力的方向垂直車道線并根據(jù)被控車與車道線的相對位置更改。根據(jù)式（15）繪制出的道路勢場三維示意如圖5所示。

圖5 道路勢場三維圖

2.3 曲率優(yōu)化

為解決傳統(tǒng)人工勢場法在繞過障礙物時曲率過大，破壞被控車側(cè)向安全行駛的情況，使路徑平順且曲率在合理范圍內(nèi)，采用B樣條曲線形成換道路徑簇，然后應(yīng)用多目標(biāo)優(yōu)化的方法篩選出最合適的路徑。

根據(jù)節(jié)點向量中節(jié)點的分布，將B樣條曲線劃分為均勻B樣條曲線、準(zhǔn)均勻B樣條曲線、分段貝塞爾（Bezier）曲線和非均勻B樣條曲線，其中，準(zhǔn)均勻B樣條曲線保證了曲線經(jīng)過控制多邊形的首尾端點[14]。在路徑規(guī)劃時通常采用3次或3次以上的準(zhǔn)均勻B樣條曲線，可以保證經(jīng)過確定始末點，且使生成的路徑具有連續(xù)的曲率。考慮車輛側(cè)向動力學(xué)模型傳遞函數(shù)中的最高次數(shù)為3，將采用四階3次準(zhǔn)均勻B樣條曲線進(jìn)行路徑曲率突變處的路徑簇生成。

以同向三車道為例，在（22.5 m，-1.75 m）處設(shè)置靜止障礙物，生成的換道路徑簇如圖6所示。

注：虛線表示生成的換道路徑簇；實線方框為仿真結(jié)束時的障礙物位置。

多目標(biāo)優(yōu)化問題是在給定區(qū)域內(nèi)多于一個的數(shù)值目標(biāo)的最優(yōu)化問題，在進(jìn)行多目標(biāo)優(yōu)化時對特定子目標(biāo)的優(yōu)化不能影響到對其他子目標(biāo)的優(yōu)化，常用的多目標(biāo)優(yōu)化方法有線型加權(quán)和法、最小-最大法、非支配排序遺傳算法、帕累托（Pareto）等級法等[15-17]。

最優(yōu)路徑的篩選僅與路徑本身有關(guān)，最重要的2個路徑評價指標(biāo)為路徑平均曲率與路徑長度，其中，路徑平均曲率反映換道過程的側(cè)向安全性，路徑長度反映換道時間。因此，選擇線性加權(quán)和法進(jìn)行最優(yōu)路徑的篩選，構(gòu)建優(yōu)化函數(shù)為

注：實線表示經(jīng)過線性加權(quán)和法優(yōu)化篩選出的最優(yōu)路徑；實線方框為仿真結(jié)束時障礙物位置。

圖7 篩選的最優(yōu)路徑

算法流程如圖8所示。

圖8 算法流程

3 實驗與結(jié)果分析

為證明改進(jìn)算法的合理性，在矩陣實驗室MATLAB軟件中搭建單向三車道場景，車道寬度為3.5 m，方向由左至右，車道由下至上分別稱為第一車道、第二車道、第三車道，在第一車道下方建立非機動車道，并建立4種障礙物場景。

3.1 靜止障礙物工況

在第一車道設(shè)置障礙物1，質(zhì)心坐標(biāo)為（22.5 m，-1.75 m）；在第二車道設(shè)置障礙物2，質(zhì)心坐標(biāo)為（55 m，1.75 m）；在第三車道設(shè)置障礙物3，質(zhì)心坐標(biāo)為（72.5 m，5.25 m）。使被控車在（0，-1.75 m）處以45 km/h初始速度行駛，駛向目標(biāo)點（100 m，1.75 m）。由本文所提出的方法生成的換道路徑和由傳統(tǒng)人工勢場法生成的換道路徑對比如圖9所示。

注：實線表示由本文所提出的方法生成的換道路徑；虛線表示由傳統(tǒng)人工勢場法生成的換道路徑；實線方框為仿真結(jié)束時障礙物位置。

生成路徑的曲率變化對比與航向角變化對比如圖10、圖11所示。

圖10 靜止障礙物工況下參考路徑曲率變化對比

圖11 靜止障礙物工況下被控車航向角變化對比

3.2 勻速移動障礙物工況

在第一車道設(shè)置以10.8 km/h的速度勻速行駛的移動障礙物1，質(zhì)心坐標(biāo)為（15 m，-1.75 m）；在第二車道設(shè)置以16.2 km/h的速度勻速行駛的移動障礙物2，質(zhì)心坐標(biāo)為（37.5 m，1.75 m）；在第三車道設(shè)置以16.2 km/h的速度勻速行駛的移動障礙物3，質(zhì)心坐標(biāo)為（57.5 m，5.25 m）。使被控車在（0，-1.75 m）處以54 km/h初始速度行駛，駛向目標(biāo)點（100 m，1.75 m）。由本文所提出的方法生成的換道路徑和由傳統(tǒng)人工勢場法生成的換道路徑對比如圖12所示。

注：實線表示由本文所提出的方法生成的換道路徑；虛線表示由傳統(tǒng)人工勢場法生成的換道路徑；實線方框為仿真結(jié)束時障礙物位置；虛線方框為換道開始時障礙物位置。

生成路徑的曲率變化對比與航向角變化對比如圖13、圖14所示。

圖13 勻速移動障礙物工況下參考路徑曲率變化對比

圖14 勻速移動障礙物工況下被控車航向角變化對比

3.3 減速障礙物工況

在第一車道設(shè)置靜止障礙物1，質(zhì)心坐標(biāo)為（22.5 m，-1.75 m）；在第二車道設(shè)置以27 km/h的初速度行駛，以1.1 m/s2的減速度減速至靜止的移動障礙物2，質(zhì)心坐標(biāo)為（42.5 m，1.75 m）。使被控車在（0，-1.75 m）處以54 km/h初始速度行駛，駛向目標(biāo)點（100 m，5.25 m）。由本文所提出的方法生成的換道路徑和由傳統(tǒng)人工勢場法生成的換道路徑對比如圖15所示。

注：實線表示由本文所提出的方法生成的換道路徑；虛線表示由傳統(tǒng)人工勢場法生成的換道路徑；實線方框為仿真結(jié)束時障礙物位置；虛線方框為換道開始時障礙物位置。

生成路徑的曲率變化對比與航向角變化對比如圖16、圖17所示。

圖16 減速障礙物工況下參考路徑曲率變化對比

圖17 減速障礙物工況下被控車航向角變化對比

3.4 加速障礙物工況

在第一車道設(shè)置靜止障礙物1，質(zhì)心坐標(biāo)為（22.5 m，-1.75 m）；在第二車道設(shè)置以10 km/h的初速度行駛，以1 m/s2的加速度做勻加速運動的移動障礙物2，質(zhì)心坐標(biāo)為（35 m，1.75 m）。使被控車在（0，-1.75 m）處以50.4 km/h初始速度行駛，駛向目標(biāo)點（100 m，5.25 m）。由本文所提出的方法生成的換道路徑和由傳統(tǒng)人工勢場法生成的換道路徑對比如圖18所示。

注：實線表示由本文所提出的方法生成的換道路徑；虛線表示由傳統(tǒng)人工勢場法生成的換道路徑；實線方框為仿真結(jié)束時障礙物位置；虛線方框為換道開始時障礙物位置。

生成路徑的曲率變化對比與航向角變化對比如圖19、圖20所示。

圖19 加速障礙物工況下參考路徑曲率變化對比

圖20 加速障礙物工況下被控車航向角變化對比

4 結(jié)束語

針對無人駕駛車輛換道避障路徑規(guī)劃問題，本文提出了一種結(jié)合B樣條曲線的改進(jìn)人工勢場法，更改了障礙物斥力勢場，并增加了相對速度斥力勢場與新型道路勢場，應(yīng)用B樣條曲線生成換道路徑簇并篩選合適路徑，進(jìn)行多種工況下的仿真實驗。結(jié)果表明，改進(jìn)后的人工勢場法可以生成平滑且符合道路行駛規(guī)范的路徑，具有更好的平順性與安全性。

[1] 袁師召, 李軍. 無人駕駛汽車路徑規(guī)劃研究綜述[J]. 汽車工程師, 2019(5): 11-13.

[2] 霍鳳財, 遲金, 黃梓健, 等. 移動機器人路徑規(guī)劃算法綜述[J]. 吉林大學(xué)學(xué)報(信息科學(xué)版), 2018, 36(6): 639-647.

[3] KHATIB O. Real-Time obstacle avoidance for manipulators and mobile robots[J]. The International Journal of Robotics Research, 1986, 5(1): 90-98.

[4] 郜輝, 呂志剛. 人工勢場法目標(biāo)不可達(dá)的研究[J]. 國外電子測量技術(shù), 2018, 37(1): 29-33.

[5] 趙東輝, 李偉莉. 改進(jìn)人工勢場的機器人路徑規(guī)劃[J]. 機械設(shè)計與制造, 2017(7): 252-255.

[6] 余政. 基于改進(jìn)人工勢場法的智能汽車超車軌跡規(guī)劃策略[J]. 農(nóng)業(yè)裝備與車輛工程, 2021, 59(9): 64-68.

[7] 張三川, 明珠. 基于主動安全的改進(jìn)人工勢場局部路徑規(guī)劃研究[J]. 鄭州大學(xué)學(xué)報(工學(xué)版), 2021, 42(5): 32-36.

[8] 張家旭, 王晨, 趙健. 基于改進(jìn)人工勢場法的汽車彎道超車路徑規(guī)劃與跟蹤控制[J]. 汽車工程, 2021, 43(4): 546-552.

[9] YAO Q F, ZHENG Z Y, QI L, et al. Path planning method with improved artificial potential field-a reinforcement learning perspective[C]// The Institute of Electrical and Electronic Engineers (IEEE). IEEE Access. New York: IEEE, 2020: 135513-135523[2022-06-20].

[10] 音凌一, 向鳳紅. 融合改進(jìn)灰狼優(yōu)化算法和人工勢場法的路徑規(guī)劃[J]. 電子測量技術(shù), 2022, 45(3): 43-53.

[11] 胡中華, 許昕, 陳中. 無人機三維航跡非均勻三次B樣條平滑算法[J]. 控制工程, 2020, 27(7): 1259-1266.

[12] 李傳軍, 張世輝, 謝久明, 等. 基于3次B樣條曲線的快速直接插補技術(shù)研究[J]. 制造技術(shù)與機床, 2021(7): 159-164.

[13] 汪佳興, 莊繼暉, 程曉鳴, 等. B樣條曲線下的MPC軌跡重規(guī)劃算法[J]. 重慶理工大學(xué)學(xué)報(自然科學(xué)), 2020, 34(4): 27-35.

[14] 何旭光. 基于B樣條曲線模型的車道線檢測方法研究[D]. 上海: 上海工程技術(shù)大學(xué), 2020.

[15] 馬小姝, 李宇龍, 嚴(yán)浪. 傳統(tǒng)多目標(biāo)優(yōu)化方法和多目標(biāo)遺傳算法的比較綜述[J]. 電氣傳動自動化, 2010, 32(3): 48-50.

[16] 劉漢武, 雷雨龍, 付堯, 等. 基于多目標(biāo)優(yōu)化的增程式電動汽車自適應(yīng)制動回饋控制策略[J]. 華南理工大學(xué)學(xué)報(自然科學(xué)版), 2021, 49(7): 42-50.

[17] 王麗萍, 任宇, 邱啟倉, 等. 多目標(biāo)進(jìn)化算法性能評價指標(biāo)研究綜述[J]. 計算機學(xué)報, 2021, 44(8): 1590-1619.

Lane-changing obstacle avoidance path planning of unmanned vehicles based on improved artificial potential field method

LIU Gongwei, ZHAO Qiang

(School of Traffic and Transportation, Northeast Forestry University, Harbin 150040, China)

unmanned vehicle; lane-changing obstacle avoidance; path planning; artificial potential field method (APF); B-spline curve

劉公偉, 趙強. 改進(jìn)人工勢場法的無人車換道避障路徑規(guī)劃[J]. 導(dǎo)航定位學(xué)報, 2023, 11(3): 112-118 .（LIU Gongwei, ZHAO Qiang. Lane-changing obstacle avoidance path planning of unmanned vehicles based on improved artificial potential field method[J]. Journal of Navigation and Positioning, 2023, 11(3): 112-118.）

10.16547/j.cnki.10-1096.20230315.

U463.6；P228

A

2095-4999(2023)03-0112-08

2022-07-14

黑龍江省重點研發(fā)計劃項目（JD22A014）；黑龍江省留學(xué)歸國人員科學(xué)基金項目（LC2015019）。

劉公偉（1997—），男，黑龍江哈爾濱人，碩士研究生，研究方向為無人駕駛車輛路徑規(guī)劃與跟蹤。

趙強（1971—），男，黑龍江富錦人，博士，教授，研究方向為無人駕駛車輛跟蹤與控制。