涂 柯，侯宏錄，蘇 煒

(西安工業(yè)大學(xué) 光電工程學(xué)院，西安 710021)

隨著智能技術(shù)的發(fā)展，無人機(jī)被廣泛應(yīng)用于軍事、農(nóng)業(yè)、物流和其他任務(wù)中[1]。無人機(jī)作為代表，在眾多領(lǐng)域發(fā)揮著重要作用，但在實(shí)際應(yīng)用中值得關(guān)注的是安全問題，而避障路徑規(guī)劃是解決安全問題的重要手段[2-4]。無人機(jī)自主避障能力對(duì)無人機(jī)任務(wù)完成產(chǎn)生重要影響。

文獻(xiàn)[5-7]提出的人工勢(shì)場(chǎng)法，滿足無人機(jī)實(shí)時(shí)避障路徑規(guī)劃要求，可規(guī)劃出較平滑的路徑躲避障礙物，且算法所需時(shí)間短、滿足實(shí)時(shí)性。但該方法應(yīng)用在無人機(jī)工作場(chǎng)景中，進(jìn)行無人機(jī)的在線路徑規(guī)劃躲避障礙物時(shí)，存在以下問題：目標(biāo)附近存在障礙物時(shí)，無人機(jī)所受引力小于斥力，在目標(biāo)附近震蕩，無法到達(dá)目標(biāo)位置[8]；目標(biāo)引力與障礙物斥力大小相等，方向相反，無人機(jī)所受合力為零，陷入局部極小值陷阱[9]；由于障礙物的影響，使得無人機(jī)在躲避障礙物時(shí)偏離原直線路徑的避障路線，路徑長(zhǎng)度大幅度增加，導(dǎo)致路徑過長(zhǎng)。文獻(xiàn)[10]在無人機(jī)陷入局部極值陷阱時(shí)，引入懲罰勢(shì)函數(shù)代替?zhèn)鹘y(tǒng)斥力勢(shì)函數(shù)，并采用模擬退火法來搜索路徑，有效避免了局部最小點(diǎn)和震蕩點(diǎn)，但引入太多信息導(dǎo)致計(jì)算量及路徑長(zhǎng)度的增加；文獻(xiàn)[11]針對(duì)路徑震蕩問題，設(shè)置探測(cè)窗口判斷是否激活衰減函數(shù)，消除狹窄通道處軌跡振蕩現(xiàn)象，針對(duì)目標(biāo)不可達(dá)問題，設(shè)置無人機(jī)與目標(biāo)點(diǎn)距離因子，調(diào)節(jié)斥力場(chǎng)系數(shù)，有效解決目標(biāo)附近存在障礙物而致使無法到達(dá)情況，但該法存在部分較陡峭的彎角路徑；文獻(xiàn)[12]針對(duì)局部極小值陷阱問題，在斥力勢(shì)場(chǎng)函數(shù)中加入航跡點(diǎn)和目標(biāo)點(diǎn)之間的距離，同時(shí)引入了協(xié)調(diào)力幫助無人機(jī)逃離局部極小值陷阱；文獻(xiàn)[13]針對(duì)局部極小值問題，設(shè)計(jì)避障預(yù)防檢測(cè)策略，對(duì)無人機(jī)飛行方向進(jìn)行實(shí)時(shí)校正，并通過對(duì)無人機(jī)在局部極小值點(diǎn)附近的飛行方向和步長(zhǎng)調(diào)整，實(shí)現(xiàn)無人機(jī)自主避障功能，但驗(yàn)證方案簡(jiǎn)單且只在二維環(huán)境中驗(yàn)證；文獻(xiàn)[14]引入斥力偏轉(zhuǎn)模型，解決局部極小值問題，并引入斥力增益系數(shù)，優(yōu)化航向過大問題；文獻(xiàn)[15]用正六邊形引導(dǎo)方法來改善局部極小值問題，并在排斥勢(shì)場(chǎng)函數(shù)中添加距離校正因子，有效改善局部極小值問題，但仍然存在路徑震蕩及轉(zhuǎn)彎角度過大情況；文獻(xiàn)[16]將機(jī)器人與目標(biāo)點(diǎn)之間的距離引入斥力場(chǎng)函數(shù)中，改變排斥力方向，避免機(jī)器人陷入局部極小點(diǎn)而引起的停滯不前問題，但仍存在路徑震蕩及路徑長(zhǎng)度增加問題。目前大多避障路徑規(guī)劃都是基于二維空間中，而無人機(jī)工作環(huán)境為三維工作環(huán)境，且復(fù)雜程度、不確定程度更高，對(duì)避障路徑要求更高。

本文針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法避障路徑規(guī)劃時(shí)會(huì)出現(xiàn)路徑過長(zhǎng)、目標(biāo)不可達(dá)以及易局部極小值陷阱問題做出改進(jìn)，并將其推廣至三維空間中。

1 人工勢(shì)場(chǎng)法分析

人工勢(shì)場(chǎng)法將無人機(jī)飛行環(huán)境虛擬為力場(chǎng)環(huán)境，不受障礙物數(shù)量影響，且算法所需時(shí)間短、滿足實(shí)時(shí)性，其原理是視目標(biāo)位置為負(fù)電荷，對(duì)無人機(jī)產(chǎn)生引力;視障礙物為正電荷，產(chǎn)生斥力；各障礙物斥力及目標(biāo)引力之和為無人機(jī)所受合力[17-19]，圖1為無人機(jī)受力示意圖。

圖1 無人機(jī)受力示意圖

人工勢(shì)場(chǎng)法勢(shì)場(chǎng)函數(shù)為

(1)

Urep(Xp)=

(2)

式中：Xp=(x,y,z)為無人機(jī)當(dāng)前位置，Xob=(xob,yob,zob)為障礙物位置坐標(biāo)；Xgoal=(xgoal,ygoal,zgoal)為目標(biāo)位置；katt、krep為引力、斥力勢(shì)場(chǎng)系數(shù)；Uatt(Xp)、Urep(Xp)為引力、斥力勢(shì)場(chǎng)函數(shù)；ρ(Xp,Xgoal)=‖Xp-Xgoal‖為無人機(jī)與目標(biāo)點(diǎn)相對(duì)距離；ρ(Xp,Xob)=‖Xp-Xob‖為無人機(jī)與障礙物相對(duì)距離；ρ0為障礙物最大作用范圍，超過該范圍時(shí)不產(chǎn)生影響。

對(duì)勢(shì)場(chǎng)求取負(fù)梯度可得

Fatt(Xp)=-(Uatt(Xp))

(3)

Frep(Xp)=

(4)

引力與斥力的疊加為無人機(jī)所受合力，則合力為

(5)

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

2.1 求解目標(biāo)不可達(dá)

如果目標(biāo)附近存在障礙物，無人機(jī)向目標(biāo)飛行，則會(huì)造成無人機(jī)接近目標(biāo)時(shí)，其自身所受引力小、斥力大情況，合力方向背離目標(biāo)位置，無人機(jī)在合力驅(qū)使下遠(yuǎn)離目標(biāo)行駛，無人機(jī)會(huì)駛出一定的距離，所受引力增大，斥力減小，又逐漸接近目標(biāo)位置，反復(fù)在目標(biāo)位置附近徘徊震蕩但卻不能到達(dá)目標(biāo)位置。

為解決該問題，將無人機(jī)與目標(biāo)位置相對(duì)距離引入到斥力勢(shì)場(chǎng)中[20-21]，斥力勢(shì)場(chǎng)函數(shù)如下：

Urep(Xp)=

(6)

式中:t為調(diào)節(jié)因數(shù)，可變化的正常數(shù)。求取改進(jìn)斥力勢(shì)場(chǎng)負(fù)梯度，可求得斥力為

Frep(Xp)=-Urep(Xp)

(7)

式中：

(8)

(9)

改進(jìn)后斥力矢量分量Frep1(Xp)方向由障礙物遠(yuǎn)離方向指向無人機(jī)；改進(jìn)后斥力矢量分量Frep2(Xp)方向由無人機(jī)指向目標(biāo)位置方向，與引力同向。無人機(jī)駛向目標(biāo)位置時(shí)，無人機(jī)與目標(biāo)位置之間距離逐漸減小。當(dāng)n∈(0,1)時(shí)，ρ(Xp,Xgoal)→0,斥力分量Frep1(Xp)→0,斥力分量Frep2(Xp)→∞，在引力Fatt(Xp)和斥力分量Frep2(Xp)作用下，即使目標(biāo)位置周圍存在障礙物，無人機(jī)仍然能朝目標(biāo)位置飛行，并且方向背離障礙物，在人工勢(shì)場(chǎng)總和力的作用下無人機(jī)可以安全的到達(dá)目標(biāo)位置，完成飛行任務(wù)。

2.2 基于調(diào)控力的局部極小值抑制算法

分析無人機(jī)受力情況可知，當(dāng)斥力與引力大小相等方向相反時(shí)，會(huì)出現(xiàn)局部極小。本文利用調(diào)控力以解決無人機(jī)局部陷入極小值引起的受力平衡狀態(tài)。

若在所有位置引入調(diào)控力，則會(huì)導(dǎo)致避障路徑的增加，與最優(yōu)路徑要求不符；故只需在陷入局部極小時(shí)引入調(diào)控力，使無人機(jī)逃離局部極小陷阱。因此需檢測(cè)到無人機(jī)陷入局部極小值點(diǎn)，引入調(diào)控力，使無人機(jī)逃離陷阱。

定義夾角θF，為各障礙物產(chǎn)生的合斥力與引力的夾角，極小值檢測(cè)條件為

(10)

式(10)成立時(shí)，判定無人機(jī)陷入極小值陷阱，設(shè)定的ε≥0值越小，判定越靈敏，誤判率越低。經(jīng)過多次在Matlab中仿真實(shí)驗(yàn)，本文將ε設(shè)定為π/90時(shí)，可實(shí)現(xiàn)算法最優(yōu)實(shí)現(xiàn)。若式(10)成立，將引入調(diào)控力進(jìn)行逃離極小值陷阱。

在無人機(jī)陷入局部極小值陷阱時(shí)，引入調(diào)控力

(11)

(12)

μ為調(diào)控系數(shù)，調(diào)控系數(shù)定義為

(13)

在調(diào)控力的作用下，無人機(jī)將打破局部極小狀態(tài)，逃離局部極小值陷阱后，取消調(diào)控力，在引力和斥力作用下，無人機(jī)會(huì)繼續(xù)朝目標(biāo)位置飛行。

2.3 基于檢測(cè)因子的路徑優(yōu)化策略

在無人機(jī)的人工勢(shì)場(chǎng)法避障路徑規(guī)劃中，若無障礙物，無人機(jī)沿直線路徑前進(jìn)，快速安全到達(dá)目標(biāo)點(diǎn)。但由于存在障礙物的影響，使得無人機(jī)在躲避障礙物時(shí)難免規(guī)劃出偏離原直線路徑的避障路線，路徑長(zhǎng)度大幅度增加，從而導(dǎo)致路徑過長(zhǎng)。無人機(jī)受無人機(jī)自身及載重影響，續(xù)航能力有限，因此路徑的長(zhǎng)度必須小于或等于無人機(jī)可飛行的最大距離。

針對(duì)路徑過長(zhǎng)的問題，躲避障礙物時(shí)，避免無效斥力,檢測(cè)因子如圖2所示。

圖2 檢測(cè)因子示意圖

在障礙物躲避過程計(jì)算障礙物斥力時(shí)，首先引入檢測(cè)因子Si，Si定義如下：

Si=|ρi(X,X0)sinθi|,

(14)

式中：ρi(X,X0)為無人機(jī)與第i個(gè)障礙物之間距離；sinθi為無人機(jī)與第i個(gè)障礙物之間連線和無人機(jī)與目標(biāo)位置連線夾角正弦值。

當(dāng)Si大于無人機(jī)行進(jìn)安全距離R，無人機(jī)依當(dāng)前行進(jìn)路線前進(jìn)不會(huì)與該障礙物發(fā)生碰撞，判定該障礙物為無效障礙物，不考慮該障礙物所產(chǎn)生斥力，當(dāng)Si小于無人機(jī)行進(jìn)安全距離R，無人機(jī)依當(dāng)前行進(jìn)路線前進(jìn)會(huì)與該障礙物發(fā)生碰撞，判定該障礙物為有效障礙物，計(jì)算斥力時(shí)需考慮該障礙物所產(chǎn)生斥力。則引入距離因子后的斥力公式為

(15)

當(dāng)障礙物不構(gòu)成當(dāng)前路徑威脅時(shí)，忽略該障礙物影響，繼續(xù)沿原路徑前進(jìn)，引入Si后避障路徑相較于傳統(tǒng)人工勢(shì)場(chǎng)法更加平滑且路徑較短。

3 仿真驗(yàn)證與分析

為驗(yàn)證改進(jìn)人工勢(shì)場(chǎng)法的可行性，分別設(shè)置造成目標(biāo)不可達(dá)、局部極小值、影響路徑長(zhǎng)度的標(biāo)志性障礙物，并對(duì)傳統(tǒng)算法和改進(jìn)算法做對(duì)比，并進(jìn)行模擬仿真實(shí)驗(yàn)。主要參數(shù)設(shè)定見表1。

表1 改進(jìn)算法驗(yàn)證基本參數(shù)

3.1 相對(duì)距離求解目標(biāo)不可達(dá)仿真驗(yàn)證

驗(yàn)證加入目標(biāo)與無人機(jī)距離改進(jìn)斥力函數(shù)解決目標(biāo)不可達(dá)問題，并與傳統(tǒng)算法進(jìn)行對(duì)比。在目標(biāo)位置附近設(shè)置障礙物，且離目標(biāo)位置非常相近，障礙物位置見表2，目標(biāo)位置為(10,10,10)，距離目標(biāo)最近的障礙物為5號(hào)障礙物，該障礙物位置為(10,9.5,10)，其與目標(biāo)位置之間距離為0.5 m，與目標(biāo)點(diǎn)距離小于障礙物影響距離，會(huì)導(dǎo)致無人機(jī)飛行至目標(biāo)點(diǎn)附近時(shí)，障礙物產(chǎn)生斥力大于目標(biāo)點(diǎn)所產(chǎn)生引力造成目標(biāo)不可達(dá)現(xiàn)象。

表2 驗(yàn)證目標(biāo)不可達(dá)問題障礙物參數(shù)

由圖3可看出，傳統(tǒng)算法下無人機(jī)飛行至目標(biāo)點(diǎn)附近時(shí)受障礙物5影響，在目標(biāo)附近震蕩，但無法到達(dá)目標(biāo)位置；而在圖4改進(jìn)算法下，無人機(jī)可順利抵達(dá)目標(biāo)位置。表明改進(jìn)算法可以有效解決目標(biāo)位置無法到達(dá)問題。

圖3 改進(jìn)前無法抵達(dá)目標(biāo)點(diǎn)

圖4 改進(jìn)后抵達(dá)目標(biāo)點(diǎn)

3.2 調(diào)控力極值抑制算法仿真驗(yàn)證

針對(duì)局部極小值驗(yàn)證，本文引入調(diào)控力逃離局部極小值陷阱問題做出驗(yàn)證，將加入調(diào)控力算法與傳統(tǒng)人工勢(shì)場(chǎng)算法進(jìn)行對(duì)比。無人機(jī)起始位置為(0，0，0)，目標(biāo)位置為(10，10，10)。障礙物位置見表3。1號(hào)障礙物位置為(4，4，4)，無人機(jī)行進(jìn)到該障礙物附近時(shí)，目標(biāo)點(diǎn)引力與障礙物1所產(chǎn)生斥力大小相等方向相反，無人機(jī)此時(shí)合力近乎為零，無人機(jī)陷入局部極小值陷阱。

表3 驗(yàn)證局部極小值問題障礙物參數(shù)

由仿真結(jié)果看出，圖5傳統(tǒng)人工勢(shì)場(chǎng)法算法下，在行進(jìn)到(3.5，3.5，3.5)位置附近時(shí)，陷入局部極小值陷阱中，無法逃離，無人機(jī)陷入停滯不前狀態(tài)導(dǎo)致無人機(jī)避障失敗。

圖5 改進(jìn)前陷入極小值陷阱無法逃離

而在圖6，在行進(jìn)到(3.5，3.5，3.5)位置，無人機(jī)陷入局部極小值陷阱后，由于調(diào)控力的作用，無人機(jī)能逃離極小值陷阱，到達(dá)目標(biāo)位置。在引入調(diào)控力后，無人機(jī)能夠檢測(cè)到局部極小值陷阱，并可逃離該陷阱，仿真結(jié)果表明改進(jìn)后算法可解決局部極小值陷阱問題。

圖6 改進(jìn)后可逃離極小值陷阱

3.3 檢測(cè)因子路徑優(yōu)化策略仿真驗(yàn)證

針對(duì)路徑過長(zhǎng)問題，驗(yàn)證加入檢測(cè)因子后優(yōu)化路徑長(zhǎng)度方法的有效性，將傳統(tǒng)法與引入距離因子后的人工勢(shì)場(chǎng)法躲避障礙物時(shí)避障路徑規(guī)劃情況做對(duì)比。無障礙物時(shí)，無人機(jī)沿直線向目標(biāo)位置飛行，當(dāng)Si>R時(shí)，所產(chǎn)生斥力為無效斥力，反之，為有效斥力。障礙物參數(shù)見表4。障礙物1、2、3、4相應(yīng)的距離因子Si均大于R,障礙物5距離因子Si小于R，則在改進(jìn)后的算法中，無人機(jī)應(yīng)只受障礙物5的影響。

表4 驗(yàn)證路徑過長(zhǎng)問題障礙物參數(shù)

圖7和圖8為兩種算法對(duì)比結(jié)果，圖7為傳統(tǒng)法路徑，可以看出，在遇到障礙物時(shí)，傳統(tǒng)法由于在障礙物斥力影響下，路徑發(fā)生變化，“繞路”躲避障礙物；圖8為改進(jìn)法路徑，1～4號(hào)障礙物檢測(cè)因子都大于R,無人機(jī)可安全通過，改進(jìn)法將其視為無效障礙物，不考慮其影響，而障礙物5檢測(cè)因子Si小于R，視為威脅障礙物，考慮障礙斥力影響躲避障礙物，仿真結(jié)果表明改進(jìn)算法可明顯減小路徑長(zhǎng)度，可降低優(yōu)化避障路徑。經(jīng)分析多次仿真實(shí)驗(yàn)數(shù)據(jù)可得，改進(jìn)后算法可減少路徑長(zhǎng)度9%～15%。

圖7 改進(jìn)前路徑

圖8 改進(jìn)后路徑

4 結(jié) 論

針對(duì)人工勢(shì)場(chǎng)法在無人機(jī)避障路徑規(guī)劃時(shí)的路徑過長(zhǎng)問題、目標(biāo)不可達(dá)問題以及局部極小值陷阱問題做出改進(jìn)。針對(duì)目標(biāo)不可達(dá)問題，引入無人機(jī)與目標(biāo)之間相對(duì)距離到斥力勢(shì)場(chǎng)函數(shù)中，減小目標(biāo)附近障礙物斥力解決目標(biāo)不可達(dá)問題；針對(duì)極小值陷阱問題，在無人機(jī)陷入局部極小值陷阱后，引入調(diào)控力幫助無人機(jī)逃離，以該方法解決局部極小值陷阱問題；針對(duì)路徑長(zhǎng)度問題，將檢測(cè)因子引入到斥力函數(shù)中，檢測(cè)障礙物斥力是否為無效斥力，排除無效斥力影響，優(yōu)化無人機(jī)避障路徑減小路徑長(zhǎng)度，減小路徑長(zhǎng)度9%～15%。通過理論分析與傳統(tǒng)人工勢(shì)場(chǎng)法仿真對(duì)比，得出結(jié)論，改進(jìn)算法有效解決了上述傳統(tǒng)人工勢(shì)場(chǎng)法存在的目標(biāo)不可達(dá)、局部極小值陷阱、路徑長(zhǎng)度長(zhǎng)問題，提升了無人機(jī)工作安全性的同時(shí)提高了適應(yīng)性，使其在無人機(jī)避障路徑規(guī)劃中具有應(yīng)用價(jià)值。