邱朋，汪光，趙理，趙博陽

（1.北京信息科技大學(xué)機(jī)電工程學(xué)院，北京 100192；2.河北工業(yè)大學(xué)人工智能與數(shù)據(jù)科學(xué)學(xué)院，天津 300401）

1 前言

隨著智能駕駛技術(shù)的發(fā)展，無人駕駛車輛越來越多的運(yùn)用到人們的生產(chǎn)生活中，其相關(guān)技術(shù)可分為：定位導(dǎo)航、環(huán)境感知、路徑規(guī)劃以及自動(dòng)控制技術(shù)，其中無人駕駛車輛路徑規(guī)劃與行駛避障一直是研究的重點(diǎn)[1]。路徑規(guī)劃是指在已知或未知的環(huán)境中，由車載雷達(dá)、視覺傳感器等獲取外界信息后，利用車載控制器規(guī)劃出一條由起點(diǎn)到目標(biāo)點(diǎn)的無碰撞軌跡。由于實(shí)際車輛行駛過程中更多時(shí)候處于動(dòng)態(tài)環(huán)境，目前針對(duì)路徑規(guī)劃相關(guān)算法各國(guó)學(xué)者研究廣泛，但對(duì)于動(dòng)態(tài)避障問題仍有待進(jìn)一步研究[2?4]。

人工勢(shì)場(chǎng)法是路徑規(guī)劃算法中一種常用的方法，該算法原理簡(jiǎn)單易理解，是一種準(zhǔn)確性與實(shí)時(shí)性較高的一種方法[5]，因此被廣泛應(yīng)用于無人車以及機(jī)器人等領(lǐng)域的路徑規(guī)劃求解當(dāng)中，但對(duì)于動(dòng)態(tài)環(huán)境的適應(yīng)性較低，無法滿足無人車處于動(dòng)態(tài)環(huán)境中所需路徑準(zhǔn)確穩(wěn)定的要求。文獻(xiàn)[6]在動(dòng)態(tài)路徑規(guī)劃過程中，考慮了速度因素的影響，解決了路徑抖動(dòng)不穩(wěn)定的問題，但未考慮行駛道路邊界的限制，在實(shí)際行駛中可能駛出道路。文獻(xiàn)[7]中提出一種斥力偏轉(zhuǎn)模型，修正斥力方向，降低了規(guī)劃路徑的長(zhǎng)度及曲度，但并未對(duì)考慮實(shí)際行駛環(huán)境中的速度因素，因而無法準(zhǔn)確描述實(shí)際行駛場(chǎng)景。文獻(xiàn)[8]中提出一種高斯人工勢(shì)場(chǎng)算法，該算法針對(duì)結(jié)構(gòu)化道路環(huán)境，增加車道中心線對(duì)行駛車輛的引力，增加了汽車避障過程的穩(wěn)定性，但當(dāng)汽車離車輛中心線較遠(yuǎn)時(shí)，該方法會(huì)面臨路徑抖動(dòng)的問題。

因此，結(jié)合實(shí)際汽車行駛環(huán)境及車輛外形特征，借鑒人工勢(shì)場(chǎng)法的基本思想，改進(jìn)斥力勢(shì)場(chǎng)函數(shù)，應(yīng)用橢圓化距離代替斥力勢(shì)場(chǎng)的中的實(shí)際距離，構(gòu)造道路邊界勢(shì)場(chǎng)、障礙物速度勢(shì)場(chǎng)，引力勢(shì)場(chǎng)多重約束下的改進(jìn)人工勢(shì)場(chǎng)模型。同時(shí)，基于純追蹤算法理論，建立以轉(zhuǎn)角為控制變量的路徑跟蹤模型，并通過Matlab驗(yàn)證改進(jìn)算法的路徑規(guī)劃及跟蹤效果。

2 傳統(tǒng)人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法應(yīng)用與無人車路徑規(guī)劃的基本思想是將無人車在周圍環(huán)境中的運(yùn)動(dòng)虛擬為在受力場(chǎng)中的運(yùn)動(dòng)，障礙物對(duì)無人車產(chǎn)生斥力，目標(biāo)點(diǎn)對(duì)無人車產(chǎn)生引力，無人車受引力與斥力的共同作用下，由起點(diǎn)沿著勢(shì)場(chǎng)強(qiáng)度下降最快的方向朝著目標(biāo)點(diǎn)運(yùn)動(dòng)，其受力分析，如圖1所示。

圖1 無人車受力分析Fig.1 Stress Analysis of Unmanned Vehicles

傳統(tǒng)人工勢(shì)場(chǎng)法中將受控對(duì)象的運(yùn)動(dòng)簡(jiǎn)化為質(zhì)點(diǎn)運(yùn)動(dòng)，運(yùn)動(dòng)空間為二維歐式空間，即平面運(yùn)動(dòng)，受控對(duì)象的位置坐標(biāo)為X=(x，y)，目標(biāo)點(diǎn)坐標(biāo)為Xgoal=(xgoal，ygoal)，目標(biāo)點(diǎn)對(duì)受控對(duì)象的引力勢(shì)場(chǎng)函數(shù)為二次函數(shù)，即：

式中：k—引力勢(shì)場(chǎng)系數(shù)；Xgoal?X—受控車輛到目標(biāo)點(diǎn)的距離ρgoal為：

定義環(huán)境模型中障礙點(diǎn)位置坐標(biāo)為Xobject=(xobject，yobject)，則該處斥力勢(shì)場(chǎng)函數(shù)為：

式中：η—斥力勢(shì)場(chǎng)系數(shù)；ρ0—斥力勢(shì)場(chǎng)影響距離；ρobject—受控車輛到障礙物的距離，當(dāng)ρobject>ρ0時(shí)，車輛不受斥力勢(shì)場(chǎng)的作用，其中：

受控對(duì)象所受總勢(shì)場(chǎng)為：

式中：n—障礙物的個(gè)數(shù)。

3 改進(jìn)人工勢(shì)場(chǎng)法

3.1 速度斥力勢(shì)場(chǎng)

為適應(yīng)實(shí)際無人車行駛環(huán)境，改善傳統(tǒng)人工勢(shì)場(chǎng)法中動(dòng)態(tài)規(guī)劃能力差的問題，在改進(jìn)人工勢(shì)場(chǎng)模型中加入速度斥力場(chǎng)，其函數(shù)為：

3.2 引力勢(shì)能場(chǎng)

無人車在避障前后都是以主車道線為主要行駛路徑，因此以車道中心線為參考，構(gòu)建車道中心線引力勢(shì)能場(chǎng)，無人車向車道中心線兩側(cè)運(yùn)動(dòng)時(shí)，引力勢(shì)能逐漸增加，且增加速度越來越快，其表達(dá)式為：

式中：λ—道路中心線引力勢(shì)能場(chǎng)因子；x0—無人車起點(diǎn)橫坐標(biāo)；ymax—無人車縱向最大位移。

由式（6）和式（1）得出無人車在行駛過程中所受總引力勢(shì)能為：

無人車所受引力勢(shì)能的三維分布圖，如圖2 所示。距離目標(biāo)點(diǎn)距離越遠(yuǎn)，引力勢(shì)能越小，距離車道中心線距離越遠(yuǎn)，引力勢(shì)能越大。

圖2 引力勢(shì)能場(chǎng)三維分布圖Fig.2 Three?Dimensional Distribution of Gravitational Potential Energy Field

3.3 道路邊界斥力場(chǎng)

無人車在實(shí)際行駛過程中，在沒有其它障礙物的影響時(shí)，將受到道路邊界的約束，且距離邊界越近，所受斥力越大，道路中心線所受邊界斥力最小。因此，根據(jù)無人車在道路上行駛到任意位置危險(xiǎn)系數(shù)不同，選用變化程度不同的函數(shù)來構(gòu)建道路邊界斥力場(chǎng)。當(dāng)無人車行駛到兩車道中心線之間時(shí)，此時(shí)距離道路邊界較遠(yuǎn)，危險(xiǎn)系數(shù)較小，無人車較為安全，選用變化較為平緩的正弦函數(shù)構(gòu)建道路邊界斥力場(chǎng)；當(dāng)無人車行駛到兩車道中心線兩邊時(shí)，所受斥力較大，且距離越近危險(xiǎn)系數(shù)越高，所受斥力變化較快，選用增長(zhǎng)較快指數(shù)函數(shù)構(gòu)建道路邊界斥力場(chǎng)，其表達(dá)式如式（8）所示：

由式（8）得到無人車所處行駛于不同位置時(shí)所受道路邊界勢(shì)能場(chǎng)的三維分布，如圖3所示。無人車距離道路邊界越近，斥力勢(shì)場(chǎng)越大，且變化越快。無人車處于左右車道中心線之間時(shí)，勢(shì)場(chǎng)最小，且變化緩慢。

圖3 道路邊界勢(shì)能場(chǎng)三維分布圖Fig.3 Three?Dimensional Distribution Map of Potential Energy Field on the Road Boundary

式中：γ1，γ2—道路邊界斥力場(chǎng)因子；L—道路寬度；xl，xr—左右車道中心線的橫坐標(biāo)。

3.4 障礙物斥力勢(shì)場(chǎng)

無人車外輪廓近似長(zhǎng)方形，在進(jìn)行路徑規(guī)劃時(shí)，考慮路徑平滑且無突變的要求，采用橢圓化斥力場(chǎng)邊界的概念，若障礙物某一時(shí)刻坐標(biāo)為(xobject，yobject)，將其斥力勢(shì)場(chǎng)抽象為橢圓函數(shù)為：

將式（9）二維橢圓方程通過正態(tài)分布函數(shù)拓展到三維斥力勢(shì)能場(chǎng)，其表達(dá)式為：

為了簡(jiǎn)化式（10），取ρ=0，令β=(2πσ1σ2)?1，其表達(dá)式變?yōu)椋?/p>

式中：β—障礙物斥力場(chǎng)因子；ρ—橫縱向相關(guān)系數(shù)；σ1，σ2—調(diào)節(jié)無人車避障路徑形狀的參數(shù)；UP—一個(gè)極小的正數(shù)。

由式（11）得出不同位置斥力勢(shì)場(chǎng)能場(chǎng)的三維分布圖，如圖4所示。

圖4 障礙物斥力勢(shì)場(chǎng)分布圖Fig.4 Distribution Map of Obstacle Repulsive Potential Field

因此，無人車在不同位置所受勢(shì)能場(chǎng)為車道中心線引力場(chǎng)，道路邊界線斥力場(chǎng)，障礙物斥力場(chǎng)，速度斥力場(chǎng)以及目標(biāo)點(diǎn)引力場(chǎng)，其函數(shù)表達(dá)式如式（12）所示：

4 純追蹤控制器設(shè)計(jì)

4.1 車輛簡(jiǎn)化模型

將車輛模型簡(jiǎn)化為二輪自行車模型[9]，其優(yōu)勢(shì)在于簡(jiǎn)化了前輪轉(zhuǎn)角與后軸所遵循的曲率之間的關(guān)系，如圖5所示。根據(jù)二自由度自行車模型幾何關(guān)系可得：

圖5 車輛二自由度自行車模型Fig.5 Bicycle Model With Two Degrees of Freedom

式中：δ—前輪轉(zhuǎn)角；L—軸距。

4.2 無人車轉(zhuǎn)向控制

為完成對(duì)期望軌跡的追蹤，從車輛二自由度自行車模型出發(fā)，以后軸為切點(diǎn)，無人車縱向車身為切線，通過控制無人車轉(zhuǎn)角，使得無人車可以按照經(jīng)過目標(biāo)點(diǎn)的圓弧行駛[10]，如圖6所示。

圖6 無人車轉(zhuǎn)向控制原理Fig.6 The Principle of Steering Control for Unmanned Vehicles

圖6中(gx，gy)表示無人車要追蹤的下一個(gè)目標(biāo)點(diǎn)，α表示當(dāng)前車身姿態(tài)與目標(biāo)點(diǎn)的夾角，ld表示后軸到目標(biāo)點(diǎn)的距離，R為轉(zhuǎn)向半徑，根據(jù)正弦定理得：

前視距離被定義為車速的線性函數(shù)，即：ld=εvy(t)，其中：ε—前視系數(shù)，vy(t)—無人車縱向速度，則式（15）轉(zhuǎn)化為：

5 仿真實(shí)驗(yàn)及結(jié)果分析

在無人車二自由度模型中，考慮無人車的當(dāng)前位置、偏航角度以及無人車的速度，其中速度控制應(yīng)用簡(jiǎn)單的P控制器，橫向控制采用純追蹤控制器，基于以上原理在Simulink中搭建仿真模型，如圖7所示。

圖7 純追蹤控制器模型Fig.7 Pure Tracking Controller Model

仿真過程改進(jìn)人工勢(shì)場(chǎng)法主要參數(shù)，如表1所示。純追蹤控制模型主要參數(shù)，如表2所示。

表1 改進(jìn)人工勢(shì)場(chǎng)法參數(shù)Tab.1 Parameters of Improved Artificial Potential Field Method

表2 純追蹤控制模型主要參數(shù)Tab.2 Main Parameters of Pure Tracking Control Model

5.1 靜態(tài)環(huán)境

仿真實(shí)驗(yàn)中，設(shè)定道路寬度為8m，無人車尺寸為邊長(zhǎng)1m的正方形，仿真時(shí)間間隔為0.1s，無人車起始坐標(biāo)點(diǎn)為（6，0），終點(diǎn)坐標(biāo)點(diǎn)為（7，150），障礙物坐標(biāo)為（6.5，60），設(shè)置無人車速度為36km/h，設(shè)定移動(dòng)步長(zhǎng)為0.1m，仿真結(jié)果，如圖8～圖9所示。

圖8 靜態(tài)環(huán)境無人車規(guī)劃路徑和實(shí)際路徑曲線Fig.8 Static and Unmanned Vehicle Planned and Actual Path

圖9 靜態(tài)環(huán)境無人車規(guī)劃路徑和實(shí)際路徑橫向誤差曲線Fig.9 Lateral Error Curve of Planned Path and Actual Path of Unmanned Vehicle in Static Environment

由圖8、圖9可知改進(jìn)后的算法能夠準(zhǔn)確避開障礙物，并且規(guī)劃路徑與實(shí)際路徑吻合度較高，橫向誤差最大值為0.015m，橫向位置距離右側(cè)道路邊界近2m，遠(yuǎn)大于實(shí)際車輛車身寬度一半，說明改進(jìn)算法能夠準(zhǔn)確避開靜態(tài)障礙物，所建立的純追蹤控制模型能夠?qū)崿F(xiàn)安全平穩(wěn)的跟蹤參考軌跡。由圖11、圖12可以知轉(zhuǎn)角范圍控制在±3°內(nèi)，整體變化平緩，沒有突變及震蕩情況的出現(xiàn)。無人車實(shí)際橫擺角曲線與參考線趨勢(shì)相同，整體偏差較小，說明無人車在路徑跟蹤過程中穩(wěn)定性較好，如圖12所示。

圖10 無人車規(guī)劃轉(zhuǎn)角與實(shí)際轉(zhuǎn)角變化曲線Fig.10 Variation Curve of Unmanned Vehicle’s Planned Rotation Angle And Actual Rotation Angle

圖11 無人車參考橫擺角和實(shí)際橫擺角曲線Fig.11 Unmanned Vehicle Reference Yaw Angle and Actual Yaw Angle Curve

5.2 動(dòng)態(tài)環(huán)境

為驗(yàn)證改進(jìn)算法在動(dòng)態(tài)環(huán)境中避障的準(zhǔn)確性，在實(shí)驗(yàn)中設(shè)定移動(dòng)障礙物初始坐標(biāo)為（6.5，60），運(yùn)動(dòng)方向與y軸正方向夾角135°，速度為36km/h，仿真時(shí)間間隔0.1s，設(shè)置無人車速度為36km/h，移動(dòng)步長(zhǎng)為0.1m，仿真結(jié)果，如圖12所示。

圖12 動(dòng)態(tài)環(huán)境無人車規(guī)劃路徑與實(shí)際路徑曲線Fig.12 Unmanned Vehicle Planning Path and Actual Path Curve in Dynamic Environment

由圖12及圖13可知改進(jìn)算法在動(dòng)態(tài)環(huán)境中能夠準(zhǔn)確避開障礙物，期望路徑與規(guī)劃路徑吻合度較高，橫向誤差最大值為0.022m，所建立的純追蹤控制器在動(dòng)態(tài)路徑規(guī)劃中能夠?qū)崿F(xiàn)準(zhǔn)確追蹤。動(dòng)態(tài)環(huán)境下無人車轉(zhuǎn)角及橫擺角變化曲線，如圖14、圖15所示。

圖13 動(dòng)態(tài)環(huán)境無人車規(guī)劃路徑與實(shí)際路徑橫向誤差曲線Fig.13 Lateral Error Curve of Planned Path and Actual Path of Unmanned Vehicle in Dynamic Environment

圖14 無人車規(guī)劃轉(zhuǎn)角與實(shí)際轉(zhuǎn)角變化曲線Fig.14 Variation Curve of Unmanned Vehicle’s Planned Rotation Angle and Actual Rotation Angle

從圖中可以看出在出現(xiàn)動(dòng)態(tài)障礙物后無人車開始轉(zhuǎn)向，轉(zhuǎn)角在3°以下，整體變化平穩(wěn)，無較大范圍內(nèi)突變及震蕩。無人車實(shí)際橫擺角與參考橫擺角趨勢(shì)相同，整體偏差較小，無人車在路徑跟蹤過程中穩(wěn)定性較好，如圖15所示。

圖15 無人車參考橫擺角與實(shí)際橫擺角變化曲線Fig.15 Change Curve of Reference Yaw Angle and Actual Yaw Angle of Unmanned Vehicle

5.3 對(duì)比傳統(tǒng)人工勢(shì)場(chǎng)法

基于5.2中的動(dòng)態(tài)環(huán)境信息，設(shè)置動(dòng)態(tài)障礙物的移動(dòng)速度分別為18km/h、36km/h、54km/h，在Matlab 中應(yīng)用傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行仿真結(jié)果，如圖16、圖17所示。

圖16 動(dòng)態(tài)環(huán)境下傳統(tǒng)人工勢(shì)場(chǎng)法路徑曲線Fig.16 Path Curve of Traditional Artificial Potential Field Method Under Dynamic Environment

圖17 動(dòng)態(tài)障礙物不同速度下改進(jìn)人工勢(shì)場(chǎng)法路徑曲線Fig.17 Improved Artificial Potential Field Path Curves for Different Speeds of Dynamic Obstacles

由圖16可知，在傳統(tǒng)人工勢(shì)場(chǎng)法路徑規(guī)劃中，動(dòng)態(tài)障礙物出現(xiàn)之前，路徑平直，效果較好。但當(dāng)動(dòng)態(tài)障礙物出現(xiàn)后，所規(guī)劃路徑波動(dòng)較大，并且避開障礙物后，到達(dá)目標(biāo)點(diǎn)之前出現(xiàn)小范圍的波動(dòng)，隨著動(dòng)態(tài)障礙物速度的增加，避開動(dòng)態(tài)障礙物的路徑抖動(dòng)幅度越來越大，路徑連續(xù)波動(dòng)較明顯。由圖17可知，改進(jìn)算法所規(guī)劃的路徑平直，無連續(xù)波動(dòng)，穩(wěn)定性較高，且隨著動(dòng)態(tài)障礙物速度的增加，改進(jìn)算法規(guī)劃的路徑無大范圍波動(dòng)。對(duì)比分析圖16、17可知，改進(jìn)算法在動(dòng)態(tài)避障中路徑規(guī)劃效果較好，且在障礙物不同速度下，路徑規(guī)劃效果較為穩(wěn)定。

對(duì)比分析圖12、圖16可知，在傳統(tǒng)人工勢(shì)場(chǎng)法路徑規(guī)劃中，動(dòng)態(tài)障礙物出現(xiàn)之前，路徑平直，效果較好，但當(dāng)動(dòng)態(tài)障礙物出現(xiàn)后，所規(guī)劃路徑波動(dòng)較大，并且避開障礙物后，到達(dá)目標(biāo)點(diǎn)之前出現(xiàn)小范圍的波動(dòng)。改進(jìn)算法所規(guī)劃的路徑平直，無連續(xù)波動(dòng)，穩(wěn)定性較高。

5.4 實(shí)驗(yàn)平臺(tái)

擬實(shí)驗(yàn)車，該車包括激光雷達(dá)、攝像頭、毫米波雷達(dá)、GPS等傳感器以及主流的ADAS控制器，如圖18所示。智能小車參數(shù)，如表3所示。

圖18 實(shí)驗(yàn)平臺(tái)Fig.18 Experimental Platform

表3 智能小車參數(shù)Tab.3 Smart Car Parameters

設(shè)定智能小車以最大速度行駛，應(yīng)用改進(jìn)人工勢(shì)場(chǎng)法得，如圖19所示。智能小車以最大速度行駛時(shí)，檢測(cè)到動(dòng)態(tài)障礙物信息后，能夠及時(shí)躲避障礙物，準(zhǔn)確到達(dá)目標(biāo)點(diǎn)。

圖19 智能小車路徑Fig.19 Smart Car Path

6 結(jié)論

針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法無法適應(yīng)于無人車動(dòng)態(tài)路徑規(guī)劃的問題，基于結(jié)構(gòu)化道路改進(jìn)人工勢(shì)場(chǎng)法，考慮實(shí)際車輛外形的特征，應(yīng)用橢圓化距離代替斥力勢(shì)場(chǎng)的中的實(shí)際距離，引入道路邊界斥力勢(shì)場(chǎng)模型及車道中心線引力勢(shì)場(chǎng)模型，從而在較小車道空間內(nèi)獲得局部避障路徑，為了更加準(zhǔn)確的描述無人車行駛環(huán)境及動(dòng)態(tài)障礙物信息，引入障礙物速度斥力勢(shì)場(chǎng)。在算法的驗(yàn)證上，應(yīng)用Matlab編程，將車輛模型簡(jiǎn)化為邊長(zhǎng)為1m的矩形，添加靜態(tài)及動(dòng)態(tài)障礙物，建立以轉(zhuǎn)角為控制變量的純追蹤控制模型對(duì)所規(guī)劃的路徑進(jìn)行跟隨控制。Matlab仿真及智能小車驗(yàn)證結(jié)果表明，在靜態(tài)及動(dòng)態(tài)環(huán)境中，改進(jìn)算法能夠準(zhǔn)確避開靜態(tài)及動(dòng)態(tài)障礙物，與傳統(tǒng)人工勢(shì)場(chǎng)法對(duì)比，所規(guī)劃路徑平直，穩(wěn)定性較高，所建立的純追蹤控制模型能夠進(jìn)行平穩(wěn)的跟蹤控制。