基于人工勢場算法的智能車輛路徑規(guī)劃仿真?

安林芳，陳 濤，成艾國，方 威

(湖南大學(xué)，汽車車身先進(jìn)設(shè)計(jì)制造國家重點(diǎn)實(shí)驗(yàn)室，長沙 410082)

前言

近年來，自動(dòng)駕駛逐步走進(jìn)人們的生活，其中智能車輛路徑規(guī)劃和障礙物規(guī)避一直是研究的重點(diǎn)。智能車輛的局部路徑規(guī)劃是指基于傳感器等設(shè)備感知行駛遇到的空間障礙物，在滿足動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)約束和穩(wěn)定性、舒適性等評價(jià)指標(biāo)的條件下預(yù)先或?qū)崟r(shí)地為智能車輛規(guī)劃出從出發(fā)點(diǎn)到目標(biāo)終點(diǎn)的最優(yōu)路徑[1]。

目前常用的方法包括遺傳算法[2]、神經(jīng)網(wǎng)絡(luò)方法和人工勢場算法等。其中，人工勢場算法無須對全局進(jìn)行搜索，規(guī)劃時(shí)間短，執(zhí)行效率高，非常適合實(shí)時(shí)性和安全性要求較高的路徑規(guī)劃，用該算法規(guī)劃的路徑最平滑和安全[3]。文獻(xiàn)[4]中提出利用高斯組合隸屬函數(shù)建立引力目標(biāo)點(diǎn)函數(shù)，在引力點(diǎn)函數(shù)中考慮障礙物和車輛約束的方法，消除了抖動(dòng)問題，實(shí)現(xiàn)了無人駕駛車輛局部路徑最優(yōu)。文獻(xiàn)[5]中根據(jù)車輛行駛避障的實(shí)際情況，通過優(yōu)化安全避障中人工勢場圓形虛擬力場作用域模型，生成動(dòng)態(tài)障礙物與虛擬引力勢場，解決了虛擬勢場作用域固定、避障角度過大的問題，實(shí)現(xiàn)了車輛小角度、大半徑避障。文獻(xiàn)[6]中提出隨機(jī)變動(dòng)目標(biāo)點(diǎn)的方法，跳出勢場局部最小點(diǎn)，并采用連鎖網(wǎng)絡(luò)方法，解決了在障礙物密集區(qū)域軌跡振蕩的問題。

將人工勢場算法運(yùn)用到智能車輛局部路徑規(guī)劃中，不僅要考慮到算法中障礙點(diǎn)和目標(biāo)點(diǎn)的建模，還要考慮到智能車輛本身的運(yùn)動(dòng)復(fù)雜性。智能車輛行駛一方面受到道路邊界的約束，另一方面車輛本身受到運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的約束。

為保證車輛行駛平穩(wěn)舒適并能準(zhǔn)確躲避障礙點(diǎn)，提出了一種新的障礙點(diǎn)構(gòu)建方式。根據(jù)車輛行駛中道路邊界障礙物、實(shí)際障礙物和車輛間最小安全距離，按照避障換道方式構(gòu)建障礙點(diǎn)模型，最后考慮車輛動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束，通過對車輛轉(zhuǎn)向角進(jìn)行控制實(shí)現(xiàn)車輛預(yù)期運(yùn)動(dòng)的執(zhí)行。

1 總體框架

為保證智能車輛能規(guī)劃最優(yōu)路徑并實(shí)現(xiàn)準(zhǔn)確平穩(wěn)的追蹤，按照環(huán)境感知、行為決策、路徑規(guī)劃和車輛運(yùn)動(dòng)執(zhí)行4部分來對車輛進(jìn)行控制，如圖1所示。

環(huán)境感知部分由車載傳感器對道路、障礙點(diǎn)和車輛行駛狀態(tài)的相關(guān)信息進(jìn)行采集和處理。行為決策部分根據(jù)

采集的信息按照駕駛行為決策進(jìn)行障礙物分析判斷，并對障礙點(diǎn)、目標(biāo)點(diǎn)進(jìn)行建模。路徑規(guī)劃部分根據(jù)已建立的障礙點(diǎn)、目標(biāo)點(diǎn)模型和車輛行駛狀態(tài)采用改進(jìn)人工勢場算法進(jìn)行避障路徑規(guī)劃。運(yùn)動(dòng)執(zhí)行部分根據(jù)上述命令，通過車輛轉(zhuǎn)向機(jī)構(gòu)和驅(qū)動(dòng)/制動(dòng)機(jī)構(gòu)等實(shí)現(xiàn)車輛對所規(guī)劃路徑的實(shí)時(shí)穩(wěn)定跟蹤。

圖1 智能車輛總體結(jié)構(gòu)框架

2 改進(jìn)的人工勢場算法路徑規(guī)劃

2.1 傳統(tǒng)人工勢場算法原理

1986年，Khatib首次提出人工勢場算法，并將其引入到路徑規(guī)劃領(lǐng)域[7]。傳統(tǒng)人工勢場算法的基本思想是將被控對象在空間中的運(yùn)動(dòng)虛擬成一個(gè)質(zhì)點(diǎn)在虛擬力場中的受力運(yùn)動(dòng)，目標(biāo)點(diǎn)會(huì)對被控對象產(chǎn)生吸引力，而由障礙物或威脅區(qū)域產(chǎn)生的障礙點(diǎn)會(huì)對被控對象產(chǎn)生排斥力。最終，被控對象在引力勢場和斥力勢場的共同作用下朝著目標(biāo)點(diǎn)運(yùn)動(dòng)，如圖2所示。

圖2 人工勢場算法模型

將被控對象簡化為一質(zhì)點(diǎn)，其運(yùn)動(dòng)空間為一個(gè)二維歐式空間，實(shí)際上即平面，被控對象在其中的坐標(biāo)為 X＝(x，y)，目標(biāo)點(diǎn)坐標(biāo)為 Xg＝(xg，yg)，目標(biāo)點(diǎn)對被控對象產(chǎn)生的引力勢場為Uatt＝f(X)，該函數(shù)定義為與被控對象位置、目標(biāo)點(diǎn)位置相關(guān)的二次函數(shù)，即

式中：k為引力勢場系數(shù)；X-Xg為被控對象到目標(biāo)點(diǎn)的距離ρg。

由此可得，被控對象所受引力為引力勢場的負(fù)梯度，即

式中Vg是由被控對象指向目標(biāo)點(diǎn)的單位向量。

當(dāng)障礙點(diǎn)坐標(biāo)位置為Xob＝(xob，yob)，被控對象受到該處的斥力勢場函數(shù)為

式中：η為斥力勢場系數(shù)；ρo為斥力勢場作用距離；ρob為被控對象到障礙點(diǎn)的距離。當(dāng)被控對象在作用范圍之外，即ρob大于ρo時(shí)，被控對象不受斥力作用。

式中Vob是由被控對象指向障礙點(diǎn)的單位向量。因此，被控對象在其運(yùn)動(dòng)空間所受斥力為

綜上所述，當(dāng)被控對象在其運(yùn)動(dòng)空間受n個(gè)障礙點(diǎn)作用時(shí)，其所受合力為

傳統(tǒng)人工勢場算法存在的問題如下。

(1)目標(biāo)不可達(dá) 當(dāng)目標(biāo)點(diǎn)附近存在障礙點(diǎn)時(shí)，被控對象在目標(biāo)點(diǎn)附近所受到的斥力大于引力，可能會(huì)導(dǎo)致被控對象一直在目標(biāo)點(diǎn)附近徘徊，無法到達(dá)目標(biāo)點(diǎn)。

(2)局部極小值 當(dāng)被控對象在某一點(diǎn)所受到的斥力和引力的大小相等且方向相反時(shí)，由于合力為零，被控對象無法確定下一步的前進(jìn)方向，可能會(huì)出現(xiàn)停止或者徘徊的情況。

2.2 改進(jìn)人工勢場模型

文獻(xiàn)[8]中通過在斥力勢場函數(shù)中加入目標(biāo)點(diǎn)與被控對象之間的距離作為引入調(diào)節(jié)因子來解決目標(biāo)不可達(dá)問題，實(shí)現(xiàn)了目標(biāo)點(diǎn)附近存在障礙點(diǎn)時(shí)，被控對象在向目標(biāo)點(diǎn)運(yùn)動(dòng)的同時(shí)所受斥力相應(yīng)減小，改進(jìn)后斥力勢場為

2.3 基于人工勢場算法的目標(biāo)點(diǎn)和障礙點(diǎn)建模

提出一種新的障礙點(diǎn)構(gòu)建方式，即根據(jù)車輛行駛中道路邊界障礙物、實(shí)際障礙物和車輛間最小安全距離，按照避障換道方式構(gòu)建障礙點(diǎn)模型，對稱分布障礙點(diǎn)，使目標(biāo)點(diǎn)始終位于障礙點(diǎn)對稱軸線上，保證被控智能車輛能在合力作用下沿著規(guī)劃的目標(biāo)路徑運(yùn)動(dòng)。車輛行駛過程中障礙點(diǎn)建模按照實(shí)際情況可劃分為：行駛道路內(nèi)不存在障礙物和行駛道路中存在實(shí)際障礙物兩種情況。行駛道路內(nèi)不存在障礙物時(shí)，選取當(dāng)前行駛道路邊界構(gòu)建障礙點(diǎn)；行駛道路中存在實(shí)際障礙物時(shí)，根據(jù)實(shí)際障礙物信息和車輛行駛狀態(tài)構(gòu)建車輛行駛的障礙點(diǎn)。

2.3.1 道路內(nèi)無障礙物

由智能車輛環(huán)境感知部分的傳感器和雷達(dá)等裝置對當(dāng)前行駛道路進(jìn)行實(shí)時(shí)掃描，得到道路邊界信息。當(dāng)智能車輛當(dāng)前行駛道路中不存在有效障礙點(diǎn)時(shí)，設(shè)定障礙點(diǎn)由道路邊界線構(gòu)成，目標(biāo)點(diǎn)由當(dāng)前行駛道路中心線構(gòu)成，如圖3所示。

圖3 道路邊界障礙點(diǎn)模型

2.3.2 行駛道路存在實(shí)際障礙物

當(dāng)智能車輛當(dāng)前行駛道路內(nèi)遇到實(shí)際障礙物時(shí)，按照駕駛行為習(xí)慣分析智能車輛避障的過程?？珊唵蔚乩斫鉃?，智能車輛環(huán)境感知部分通過傳感器得到障礙物的信息，由行為決策對障礙物情況進(jìn)行判斷處理，再基于人工勢場算法進(jìn)行障礙點(diǎn)建模。

假設(shè)智能車輛避障路徑符合正弦函數(shù)特性。在相同條件下，與圓弧換道等避障路徑相比，正弦換道具有軌跡曲率極大值最小、換道平滑、計(jì)算簡單等優(yōu)點(diǎn)[9]。如圖4所示，結(jié)合車輛實(shí)際行駛情況，對車輛進(jìn)行運(yùn)動(dòng)學(xué)分析。被控車輛在檢測到障礙點(diǎn)后經(jīng)決策判斷在t0時(shí)刻開始進(jìn)行換道動(dòng)作；行駛至t0+t1時(shí)，達(dá)到與障礙點(diǎn)發(fā)生碰撞的臨界點(diǎn)；行駛至t0+t3時(shí)，車輛完成避障變道，車體擺正到達(dá)局部目標(biāo)點(diǎn)G。

圖4 車輛避障換道運(yùn)動(dòng)學(xué)模型

為保證t0到t0+t3時(shí)間內(nèi)X軸方向不發(fā)生碰撞，車輛和障礙點(diǎn)之間應(yīng)滿足的最小安全距離為

式中：vP(0)，vQ(0)，aP和 aQ分別為被控車輛、障礙物的初始速度和加速度。

車輛避障路徑符合正弦函數(shù)特性，故車輛避障換道過程中的側(cè)向加速度ay滿足

對側(cè)向加速度做時(shí)間積分可得側(cè)向速度vy，再對速度進(jìn)行積分，可得避障變道過程中車輛的側(cè)向位移y。

設(shè)等速偏移換道過程中，車輛沿X軸勻速運(yùn)動(dòng)，保證乘客的舒適性，速度為vx。

綜上，為保證被控車輛準(zhǔn)確按照換道軌跡行駛，將車輛避障換道軌跡平移最小安全距離得到障礙點(diǎn)軌跡為

通過建立對稱分布障礙點(diǎn)模型可使目標(biāo)點(diǎn)始終處于對稱軸線上，對稱方向斥力分力相互抵消，保證斥力引力始終在一條直線上。解決了振蕩和局部最小值問題，使智能車輛能夠準(zhǔn)確按照目標(biāo)路徑運(yùn)動(dòng)，如圖5所示。通過對障礙物的變換建模，既解決了目標(biāo)不可達(dá)問題，又保證了所規(guī)劃的路徑平滑，實(shí)現(xiàn)了被控車輛平穩(wěn)順暢換道。

圖5 路徑規(guī)劃實(shí)際障礙點(diǎn)模型

3 車輛動(dòng)力學(xué)約束

將人工勢場模型引入智能車輛路徑規(guī)劃中，目標(biāo)點(diǎn)所產(chǎn)生的引力勢場函數(shù)受到道路邊界障礙物和實(shí)際障礙物的約束，同時(shí)還要考慮車輛本身運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)約束。

結(jié)合2自由度車輛模型，如圖6所示，建立運(yùn)動(dòng)微分方程：

式中：k1和k2分別為前后輪的側(cè)偏剛度；m為車輛總質(zhì)量；Iz為車體轉(zhuǎn)動(dòng)慣量。

圖6 車輛線性2自由度模型

在車輛行駛過程中，轉(zhuǎn)向靈敏度是檢驗(yàn)車輛行駛方向的準(zhǔn)確性和穩(wěn)定性的重要指標(biāo)。一般采用橫擺角速度與前輪轉(zhuǎn)角的比值來表示車輛轉(zhuǎn)向靈敏度[10]，即

式中：v為車輛行駛速度；L為車輛軸距；K為穩(wěn)定性系數(shù)。為了保證行駛的安全性，要求轉(zhuǎn)向靈敏度與車速呈線性關(guān)系，即中性轉(zhuǎn)向K＝0。

基于車輛坐標(biāo)系下的運(yùn)動(dòng)，可得車輛質(zhì)心加速度沿Y軸分量即車輛側(cè)向加速度為

車輛進(jìn)行轉(zhuǎn)向避障時(shí)滿足穩(wěn)態(tài)轉(zhuǎn)向避障且車輛轉(zhuǎn)向時(shí)質(zhì)心側(cè)偏角較小。速度恒定情況下可認(rèn)為質(zhì)心速度約等于車輛沿X軸的速度分量，因此可求得

通過控制轉(zhuǎn)向盤的轉(zhuǎn)角實(shí)現(xiàn)安全避障時(shí)，角度過大可能會(huì)造成車輛側(cè)滑的危險(xiǎn)，為保證汽車安全行駛，實(shí)際控制中的避障角度和行駛速度要滿足一定動(dòng)力學(xué)條件。在改變轉(zhuǎn)向盤轉(zhuǎn)角時(shí)要求車輛側(cè)向加速度不大于0.4g，即車輛行駛安全角和行駛速度之間滿足動(dòng)力學(xué)條件[11]?？傻们拜嗈D(zhuǎn)角應(yīng)滿足

根據(jù)上述分析，智能車輛前輪轉(zhuǎn)向角同時(shí)受到行駛速度和轉(zhuǎn)彎半徑的影響。因此，為保證平穩(wěn)轉(zhuǎn)向，前輪轉(zhuǎn)角取值應(yīng)滿足：

4 仿真分析

為驗(yàn)證所構(gòu)建的局部路徑規(guī)劃方法的可行性和有效性，利用Prescan軟件搭建測試環(huán)境，并用Matlab/simulink軟件進(jìn)行基于人工勢場和車輛避障換道模型改進(jìn)的算法編程控制，對智能車輛行駛過程進(jìn)行模擬仿真，驗(yàn)證路徑規(guī)劃的準(zhǔn)確性。

借助Prescan平臺(tái)搭建預(yù)瞄跟隨模型。以Audi A8為被控智能車輛，選擇單向車道寬度為3.5m的雙車道為車輛行駛道路，同時(shí)為被控智能車輛添加傳感器，并設(shè)定其動(dòng)力學(xué)模型為Prescan自帶的線性2自由度模型，隨后將模型連接至Matlab/simulink進(jìn)行仿真如。

對改進(jìn)的人工勢場算法進(jìn)行Matlab編程仿真，算法控制流程如圖7所示。

設(shè)定智能車輛在36，54和72km/h 3種速度下勻速行駛，檢測遇到靜止障礙物并開始避障換道，設(shè)定換道時(shí)間為8s，預(yù)瞄時(shí)間為1.5s[12]，得到路徑跟隨仿真結(jié)果，如圖8～圖11所示。

圖7 改進(jìn)人工勢場算法流程

圖8 不同速度下實(shí)際運(yùn)動(dòng)路徑

由圖8可知，通過改進(jìn)人工勢場算法對目標(biāo)點(diǎn)和障礙點(diǎn)建模，得到的智能車輛實(shí)際運(yùn)動(dòng)路徑與避障換道模型下構(gòu)建的目標(biāo)路徑基本重合，算法能規(guī)劃出滿足要求的平滑路徑。

由圖9可知，仿真中智能車輛目標(biāo)路徑和實(shí)際運(yùn)動(dòng)路徑的橫向誤差呈現(xiàn)出先增大再減小的趨勢，且速度越大，橫向誤差越小。在智能車輛速度36km/h時(shí)，仿真中出現(xiàn)的最大橫向誤差為0.07m，滿足誤差要求，能實(shí)現(xiàn)較準(zhǔn)確的實(shí)時(shí)跟蹤。

由圖10和圖11仿真結(jié)果可知，被控智能車輛能按照所規(guī)劃的目標(biāo)路徑實(shí)現(xiàn)平順避障換道。換道過程中的前輪轉(zhuǎn)角隨著車輛行駛過程呈現(xiàn)先正向增大再減小至零，再反向增大，最后減小至零的趨勢，且最大轉(zhuǎn)向角與智能車輛行駛速度成反比。智能車輛Audi A8軸距L＝2.94m，設(shè)定最小轉(zhuǎn)彎半徑為5m，得到前輪轉(zhuǎn)角最大值為6.6°。當(dāng)智能車輛速度為36km/h時(shí)，避障換道過程出現(xiàn)的最大前輪轉(zhuǎn)角為 5.52°，滿足要求。

圖9 不同車速下智能車輛橫向誤差

圖10 不同車速下智能車輛橫擺角速度

圖11 不同車速下智能車輛前輪轉(zhuǎn)角

5 結(jié)論

將人工勢場算法運(yùn)用到智能車輛局部路徑規(guī)劃中，結(jié)合車輛行駛實(shí)際情況，按照正弦避障路徑以最小安全距離對稱構(gòu)建障礙點(diǎn)模型，保證局部目標(biāo)點(diǎn)處于對稱軸線上，以智能車輛運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)約束進(jìn)行仿真。結(jié)果表明，算法能規(guī)劃出平滑的目標(biāo)路徑，車輛行駛運(yùn)動(dòng)平穩(wěn)安全。解決了目標(biāo)不可達(dá)和局部最小值問題，同時(shí)考慮了智能車輛實(shí)際約束，解決了智能車輛局部路徑規(guī)劃問題。下一步研究將考慮不同路況下智能車輛的復(fù)雜約束，同時(shí)通過實(shí)車試驗(yàn)進(jìn)行驗(yàn)證。

[1] 張殿富，劉福.基于人工勢場法的路徑規(guī)劃方法研究及展望[J].計(jì)算機(jī)工程與科學(xué)，2013，35(6)：88-95.

[2] 石鐵峰.改進(jìn)遺傳算法在移動(dòng)機(jī)器人路徑規(guī)劃中的應(yīng)用[J].計(jì)算機(jī)仿真，2011，28(4)：193-195，303.

[3] YAO Peng， WANG Honglun， SU Zikang.Cooperative path planning with applications to target tracking and obstacle avoidance for multi-UAVs[J].Aerospace Science and Technology，2016(4)：2.

[4] 修彩靖，陳慧.基于改進(jìn)人工勢場法的無人駕駛車輛局部路徑規(guī)劃的研究[J].汽車工程，2013，35(9)：808-811.

[5] 譚寶成，崔佳超.改進(jìn)人工勢場法在無人車避障中的應(yīng)用[J].西安工業(yè)大學(xué)學(xué)報(bào)，2014，34(12)：1007-1011，1017.

[6] 霍鳳財(cái)，任偉建，劉東輝.基于改進(jìn)的人工勢場法的路徑規(guī)劃方法研究[J].自動(dòng)化技術(shù)與應(yīng)用，2016，35(3)：63-67.

[7] KHATIB O.Real time obstacle avoidance formanipulators and mobile robots[J].The International Journal of Robotics Research，2003，2(5)：500-505.

[8] 楊一波，王朝立.基于改進(jìn)的人工勢場法的機(jī)器人避障控制及其MATLAB實(shí)現(xiàn)[J].上海理工大學(xué)學(xué)報(bào)，2013，35(5)：496-500.

[9] 李瑋.智能車輛自由換道模型研究[J].公路交通科技，2010，27(2)：119-123.

[10] 崔佳超.無人駕駛智能車在動(dòng)態(tài)環(huán)境中的避障方法[D].西安：西安工業(yè)大學(xué)，2015.

[11] 徐梁征，肖成永，張建武.汽車列車系統(tǒng)穩(wěn)定性分析及控制系統(tǒng)仿真[J].計(jì)算機(jī)仿真，2003(12)：107-110，40.

[12] 高振海，管欣，李謙，等.基于預(yù)瞄跟隨理論的駕駛員跟隨汽車目標(biāo)速度的控制模型[J].吉林工業(yè)大學(xué)學(xué)報(bào)(工學(xué)版)，2002(1)：1-5.