馬婭婕 劉國慶 胡 軼 張 磊

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

0 引 言

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

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

1 人工勢場法改進

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

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

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

人工勢場法是通過計算當前位置勢函數(shù)的負梯度方向作為無人機下一步的運動方向，引力勢函數(shù)和斥力勢函數(shù)分別為Uatt(q)和Urep(q)：

(1)

式中，q為無人機當前位置，ρ(g)為無人機距離目標點距離，ρ(q)為無人機距離障礙物表面距離，ρ0為障礙物有效影響范圍，ξ為引力場系數(shù)，η為斥力場系數(shù)。

(2)

式中，▽ρ(g)為單位矢量，方向由目標點指向無人機，▽ρ(q)也是單位矢量，方向為障礙物指向無人機。此時無人機所受合力為

(3)

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

圖2 傳統(tǒng)人工勢場法局部最小點

1.2 線勢場的引進和改進

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

引力場由預設航線產(chǎn)生：

(4)

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

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

圖3 線勢場引入

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

(5)

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

1.3 斥力場的改進

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

(6)

式中除▽ρ(q)發(fā)生變化外，其余參數(shù)均與式(2)相同。▽ρ(q)為單位矢量，具體方向為

(7)

式中，yq為無人機位置的縱坐標，y0為障礙物中心的縱坐標。

圖4 斥力場改進

采用改進后的人工勢場法，無人機在X軸上所受勢場合力為0，為完成基于預設航線的避障任務，給定無人機X軸方向一個合適的速度。

斥力場改進之后對避障和無人機控制進行了優(yōu)化： (1) 增強了無人機的可控性； (2) 可自主逃離局部最小點。

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

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

2 Adams仿真

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

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

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

2.2 線引力為定值和變量的對比驗證

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

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

圖5 仿真場景設置

圖6 引力為定值和變量時的軌跡對比

圖7 引力為定值和變量時的Y向位移對比

圖8 引力為定值和變量時的Y軸速度對比

2.3 引入前饋和速度的線勢場對比驗證

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

(8)

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

圖9 線勢場引入前饋和速度控制前后曲線跟蹤Y向位移

2.4 基于改進人工勢場法的三維避障實驗

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

預設航線為航點(-30, 0, 0)到航點(30, 0, 0)之間的正弦曲線，軌跡方程為

(9)

給定無人機在XY平面所受勢場引力為

·▽ρ(g)

(10)

在XZ平面所受勢場引力為

(11)

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

(12)

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

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

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

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

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

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

圖14 無人機避障過程3向速度

3 結(jié) 論

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