馬婭婕 劉國(guó)慶 胡 軼 張 磊

(武漢科技大學(xué)冶金自動(dòng)化與檢測(cè)技術(shù)教育部工程研究中心 武漢 430081)

0 引 言

隨著傳感技術(shù)和嵌入式系統(tǒng)的快速發(fā)展，無(wú)人機(jī)自主性日益提高，在實(shí)際應(yīng)用中，無(wú)人機(jī)難免遇到障礙物，因此避障功能一直是研究熱點(diǎn)。環(huán)境信息的獲取手段多種多樣，例如相機(jī)[1]、激光[2]、熱成像[3]和超聲波等；實(shí)時(shí)效果好的避障算法有A*算法、爬行蟲算法及人工勢(shì)場(chǎng)法等。其中人工勢(shì)場(chǎng)法由于其計(jì)算量小，實(shí)時(shí)性好，避障軌跡平滑，被廣泛應(yīng)用。

傳統(tǒng)的人工勢(shì)場(chǎng)法只考慮能否安全到達(dá)目標(biāo)點(diǎn)，目前有很多的改進(jìn)算法致力于提高避障平滑性、改善避障軌跡、逃離極小點(diǎn)。例如文獻(xiàn)[4]通過對(duì)人工勢(shì)場(chǎng)法增設(shè)額外控制力，能夠幫助機(jī)器人主動(dòng)逃離極小點(diǎn)。文獻(xiàn)[5]采用混合經(jīng)驗(yàn)的改進(jìn)人工勢(shì)場(chǎng)法使得機(jī)器人能夠適應(yīng)多種形狀的障礙物。文獻(xiàn)[6]結(jié)合最優(yōu)控制和人工勢(shì)場(chǎng)法實(shí)現(xiàn)了無(wú)人機(jī)的自主飛行控制。文獻(xiàn)[7]在人工勢(shì)場(chǎng)法中引入距離因子實(shí)現(xiàn)了無(wú)人機(jī)編隊(duì)之間的距離保持。文獻(xiàn)[8]對(duì)勢(shì)場(chǎng)引入局部約束函數(shù)，實(shí)現(xiàn)了機(jī)器人的動(dòng)態(tài)避障。此外，人工勢(shì)場(chǎng)法在機(jī)器人避障上廣泛應(yīng)用，如衛(wèi)星編隊(duì)避障[9]、伴隨機(jī)器人避障[10]。但是上述改進(jìn)算法無(wú)法完成基于預(yù)設(shè)航線的無(wú)人機(jī)避障。文獻(xiàn)[11]提出了線勢(shì)場(chǎng)的概念。文獻(xiàn)[12]實(shí)現(xiàn)了基于直線航線的避障。但是該算法仍然存在不足之處，避障過程不夠平滑，無(wú)法跟蹤曲線航線。因此本文針對(duì)曲線航線跟蹤對(duì)人工勢(shì)場(chǎng)法進(jìn)行改進(jìn)，并應(yīng)用Adams[13]構(gòu)建仿真場(chǎng)景，進(jìn)行仿真驗(yàn)證。

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

1.1 傳統(tǒng)人工勢(shì)場(chǎng)法的局限性

人工勢(shì)場(chǎng)法是由Khatib[14]提出的一種虛擬力法。它的基本思想是將移動(dòng)機(jī)器人在周圍環(huán)境中的運(yùn)動(dòng)，設(shè)計(jì)成一種抽象的人造引力場(chǎng)中的運(yùn)動(dòng)，目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人產(chǎn)生“引力”，障礙物對(duì)其產(chǎn)生“斥力”，最后通過求合力來控制移動(dòng)機(jī)器人的運(yùn)動(dòng)。將人工勢(shì)場(chǎng)法運(yùn)用到無(wú)人機(jī)的避障中，如圖1所示，用小球表示無(wú)人機(jī)，較大球體表示障礙物。

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

人工勢(shì)場(chǎng)法是通過計(jì)算當(dāng)前位置勢(shì)函數(shù)的負(fù)梯度方向作為無(wú)人機(jī)下一步的運(yùn)動(dòng)方向，引力勢(shì)函數(shù)和斥力勢(shì)函數(shù)分別為Uatt(q)和Urep(q)：

(1)

式中，q為無(wú)人機(jī)當(dāng)前位置，ρ(g)為無(wú)人機(jī)距離目標(biāo)點(diǎn)距離，ρ(q)為無(wú)人機(jī)距離障礙物表面距離，ρ0為障礙物有效影響范圍，ξ為引力場(chǎng)系數(shù)，η為斥力場(chǎng)系數(shù)。

(2)

式中，▽?duì)?g)為單位矢量，方向由目標(biāo)點(diǎn)指向無(wú)人機(jī)，▽?duì)?q)也是單位矢量，方向?yàn)檎系K物指向無(wú)人機(jī)。此時(shí)無(wú)人機(jī)所受合力為

(3)

傳統(tǒng)的人工勢(shì)場(chǎng)法可能使無(wú)人機(jī)陷入局部最小點(diǎn)，在局部最小點(diǎn)時(shí)，無(wú)人機(jī)所受到的目標(biāo)點(diǎn)的引力和障礙物的斥力大小相等，方向相反，即所受合力為0，而且速度也為0，無(wú)人機(jī)在該點(diǎn)靜止，避障失敗，如圖2所示。

圖2 傳統(tǒng)人工勢(shì)場(chǎng)法局部最小點(diǎn)

1.2 線勢(shì)場(chǎng)的引進(jìn)和改進(jìn)

本課題組避障研究基于預(yù)設(shè)航線跟蹤，此時(shí)引力場(chǎng)不再是由目標(biāo)點(diǎn)產(chǎn)生，而是由目標(biāo)航線產(chǎn)生,引入線勢(shì)場(chǎng)的概念[9]。

引力場(chǎng)由預(yù)設(shè)航線產(chǎn)生：

(4)

式中，ρline(g)為無(wú)人機(jī)距離預(yù)設(shè)航線的距離，▽?duì)?g)為預(yù)設(shè)航線指向無(wú)人機(jī)的單位矢量。線勢(shì)場(chǎng)的作用是產(chǎn)生使無(wú)人機(jī)靠近預(yù)設(shè)航線的力，這實(shí)質(zhì)上是反饋控制，最終目的是為了消除位置誤差。

圖3是預(yù)設(shè)航線為直線時(shí)的情況，在無(wú)人機(jī)執(zhí)行不同的任務(wù)時(shí)，預(yù)設(shè)航線也有可能為曲線，下面以正弦航線為例給出當(dāng)預(yù)設(shè)航線為曲線時(shí)的情形。當(dāng)預(yù)設(shè)航線為曲線時(shí)，基于預(yù)設(shè)航線的人工勢(shì)場(chǎng)避障方法要求當(dāng)無(wú)人機(jī)在障礙物影響范圍外時(shí)，無(wú)人機(jī)能夠跟蹤預(yù)設(shè)航線。

圖3 線勢(shì)場(chǎng)引入

單純的位置反饋控制無(wú)法消除曲線跟蹤的誤差，在這里引進(jìn)前饋和速度控制，令

(5)

式中，vX為無(wú)人機(jī)沿X軸方向的速度，ρcurve(g)為無(wú)人機(jī)距離預(yù)設(shè)航線距離，ΔT為控制周期，ΔVY為預(yù)設(shè)軌跡和實(shí)際軌跡的Y向速度差，θ為速度差系數(shù)，兩者的存在是為了消除航線跟蹤誤差，將在2.3節(jié)證明。

1.3 斥力場(chǎng)的改進(jìn)

由文獻(xiàn)[11]可知，基于線勢(shì)場(chǎng)和傳統(tǒng)斥力場(chǎng)的無(wú)人機(jī)避障軌跡不理想，本文進(jìn)行斥力場(chǎng)改進(jìn)，將斥力場(chǎng)的方向改為平行于Y軸，如圖4所示。改進(jìn)后的斥力為

(6)

式中除▽?duì)?q)發(fā)生變化外，其余參數(shù)均與式(2)相同。▽?duì)?q)為單位矢量，具體方向?yàn)?/p>

(7)

式中，yq為無(wú)人機(jī)位置的縱坐標(biāo)，y0為障礙物中心的縱坐標(biāo)。

圖4 斥力場(chǎng)改進(jìn)

采用改進(jìn)后的人工勢(shì)場(chǎng)法，無(wú)人機(jī)在X軸上所受勢(shì)場(chǎng)合力為0，為完成基于預(yù)設(shè)航線的避障任務(wù)，給定無(wú)人機(jī)X軸方向一個(gè)合適的速度。

斥力場(chǎng)改進(jìn)之后對(duì)避障和無(wú)人機(jī)控制進(jìn)行了優(yōu)化： (1) 增強(qiáng)了無(wú)人機(jī)的可控性； (2) 可自主逃離局部最小點(diǎn)。

由于無(wú)人機(jī)X軸速度可根據(jù)任務(wù)要求和環(huán)境給定，如果環(huán)境復(fù)雜且探查半徑較大，可將速度設(shè)定為較小值；如果環(huán)境簡(jiǎn)單且探查半徑較小，可將速度設(shè)定為較大值。該方法提高了任務(wù)速度，極大地增強(qiáng)了無(wú)人機(jī)的可控性。

局部最小點(diǎn)是無(wú)人機(jī)所受引力和斥力合力為0的點(diǎn)，在該點(diǎn)處，無(wú)人機(jī)有沿X軸的速度，在下一周期，無(wú)人機(jī)X軸的位置發(fā)生變化，所受合力不再為0，即無(wú)人機(jī)自主逃離局部最小點(diǎn)。

2 Adams仿真

2.1 Adams簡(jiǎn)介及環(huán)境搭建

Adams即機(jī)械系統(tǒng)動(dòng)力學(xué)自動(dòng)分析，可用于預(yù)測(cè)機(jī)械系統(tǒng)的性能、運(yùn)動(dòng)范圍、碰撞檢測(cè)、峰值載荷以及計(jì)算有限元的輸入載荷等。相比于其他無(wú)人機(jī)避障仿真軟件如Matlab和VC等，Adams不僅可以更加直觀地觀察無(wú)人機(jī)的運(yùn)動(dòng)過程，而且還可以對(duì)無(wú)人機(jī)所受的力、加速度、速度等信息進(jìn)行更加細(xì)致的分析，此外Adams更是避免了大量的矢量編程。

用Adams中View分別搭建基于直線航線和曲線航線的2種避障場(chǎng)景，本課題組完成了無(wú)人機(jī)的基本控制和高度控制[15]，在這里將無(wú)人機(jī)等效為單位質(zhì)量、單位直徑的小球(以下稱無(wú)人機(jī))，障礙物分別為半徑不同的2種無(wú)窮質(zhì)量的大球，為了檢驗(yàn)無(wú)人機(jī)的航線跟蹤能力，建立一個(gè)輔助球并給它合適的初始X方向速度和Y方向位移，使其產(chǎn)生高度為0(方便計(jì)算)、航線為直線和正弦曲線的2種軌跡。本文無(wú)人機(jī)應(yīng)用場(chǎng)景是地表近距離，所以研究目標(biāo)為實(shí)現(xiàn)三維避障算法。給定無(wú)人機(jī)一個(gè)3向合力，分別在Y軸和Z軸輸入引力和斥力的合力，進(jìn)行避障仿真。

2.2 線引力為定值和變量的對(duì)比驗(yàn)證

文獻(xiàn)[7]在其改進(jìn)算法中提出為了提高無(wú)人機(jī)的可控性將引力改為定值，在此采用二維避障場(chǎng)景驗(yàn)證2種方法的優(yōu)劣性，場(chǎng)景設(shè)計(jì)如圖5所示，參數(shù)分別為ρ0=5 m，ξ=100，η=100，常量引力為800。

由圖6和圖7可知，2種方法均可完成基于預(yù)設(shè)航線的無(wú)人機(jī)避障，且定引力的軌跡確實(shí)相對(duì)變引力的軌跡能更好地跟隨預(yù)設(shè)航線。但是定引力曲線有微小抖動(dòng)，平穩(wěn)性不好，分析2種方式在避障過程中的速度，由圖8可知引力為定值時(shí)Y軸速度抖動(dòng)太大，而引力隨距離變化時(shí)速度較為平緩，綜合分析可以看出引力隨距離變化時(shí)避障效果更為明顯。

圖5 仿真場(chǎng)景設(shè)置

圖6 引力為定值和變量時(shí)的軌跡對(duì)比

圖7 引力為定值和變量時(shí)的Y向位移對(duì)比

圖8 引力為定值和變量時(shí)的Y軸速度對(duì)比

2.3 引入前饋和速度的線勢(shì)場(chǎng)對(duì)比驗(yàn)證

仿真場(chǎng)景設(shè)置3個(gè)單位質(zhì)量的小球，分別為預(yù)設(shè)軌跡球、前饋+反饋的線勢(shì)場(chǎng)跟蹤球和單純反饋的跟蹤球。3球均給定X向的恒定速度，預(yù)設(shè)軌跡球給定Y方向的正弦位移(以正弦曲線為例)，2跟蹤球受到線勢(shì)場(chǎng)作用力，分別為

(8)

仿真結(jié)果如圖9所示，單純位置反饋控制的線勢(shì)場(chǎng)無(wú)法消除跟蹤曲線航線的誤差，而引入前饋和速度控制的線勢(shì)場(chǎng)能夠極大地削弱甚至消除跟蹤曲線航線的誤差，跟蹤效果良好。

圖9 線勢(shì)場(chǎng)引入前饋和速度控制前后曲線跟蹤Y向位移

2.4 基于改進(jìn)人工勢(shì)場(chǎng)法的三維避障實(shí)驗(yàn)

課題組已經(jīng)完成無(wú)人機(jī)的基本控制和定高算法，避障原則采用較低障礙物越過、較高障礙物繞過的思路，即水平和豎直方向分別采用改進(jìn)后的人工勢(shì)場(chǎng)法。預(yù)設(shè)航線為正弦曲線，Z坐標(biāo)為0，即不進(jìn)行高度控制驗(yàn)證。避障場(chǎng)景如圖10所示。無(wú)人機(jī)用半徑0.5 m的單位質(zhì)量球代替，出發(fā)點(diǎn)(-30, 0, 0)，低矮障礙物半徑2 m，球心位于(-15, -7, 0)，3個(gè)較高障礙物半徑依次為5 m、5 m、6 m，位置依次為(-15, 1, 0)、(1, 3, 0)、(15, 7, 0)，單位為m。

預(yù)設(shè)航線為航點(diǎn)(-30, 0, 0)到航點(diǎn)(30, 0, 0)之間的正弦曲線，軌跡方程為

(9)

給定無(wú)人機(jī)在XY平面所受勢(shì)場(chǎng)引力為

·▽?duì)?g)

(10)

在XZ平面所受勢(shì)場(chǎng)引力為

(11)

單個(gè)障礙物產(chǎn)生的斥力為

(12)

無(wú)人機(jī)初始速度為vx=1 m/s。進(jìn)行60 s三維避障仿真。仿真結(jié)果如圖11～圖14所示。

由圖11～圖13可以看到，改進(jìn)后的人工勢(shì)場(chǎng)法可以在完成避障的同時(shí)盡量追蹤預(yù)設(shè)航線，滿足避障要求。下面分析無(wú)人機(jī)避障過程中的速度，判斷避障軌跡是否平滑。通過圖14可知整個(gè)避障過程中無(wú)人機(jī)各向速度均小于6 m/s，速度處于可控范圍內(nèi)，并且沒有較大抖動(dòng)，軌跡較為平滑。軌跡圖和速度曲線表明該算法能夠平滑完成基于預(yù)設(shè)航線的無(wú)人機(jī)三維避障。

圖10 三維仿真場(chǎng)景構(gòu)造

圖11 三維避障仿真結(jié)果

圖12 三維避障仿真結(jié)果XY投影

圖13 三維避障仿真結(jié)果XZ投影

圖14 無(wú)人機(jī)避障過程3向速度

3 結(jié) 論

針對(duì)目前無(wú)人機(jī)許多場(chǎng)合需要用到基于預(yù)設(shè)航線的三維避障，對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行了改進(jìn)，將引力場(chǎng)改為航線勢(shì)場(chǎng)，并加入速度勢(shì)場(chǎng)和前饋控制，將斥力場(chǎng)方向改為垂直于航線，消除了局部最小點(diǎn)?；贏dams構(gòu)建三維避障場(chǎng)景，運(yùn)用改進(jìn)引力線勢(shì)場(chǎng)進(jìn)行航線跟蹤，跟蹤曲線更加精確和平滑；運(yùn)用改進(jìn)人工勢(shì)場(chǎng)法能夠完成基于預(yù)設(shè)航線的無(wú)人機(jī)三維避障，而且避障軌跡較為平滑。