基于速度矢量合成的AUV 在線路徑規(guī)劃研究*

徐煒翔 朱志宇

（江蘇科技大學(xué)電子信息學(xué)院 鎮(zhèn)江 212003）

1 引言

AUV（Autonomous Underwater Vehicle）作為一類表現(xiàn)優(yōu)異的無人水下作業(yè)載具逐漸成為了研究的熱點［1］。其自主性很大程度上體現(xiàn)在AUV 具有根據(jù)已有的環(huán)境地圖信息和未知的傳感器環(huán)境信息來自主規(guī)劃任務(wù)路徑和自主識別并躲避障礙物的能力。而AUV在線路徑規(guī)劃就是AUV路徑規(guī)劃任務(wù)中不可或缺的一環(huán)，作為對全局路徑規(guī)劃的補充，針對AUV 前進路徑上出現(xiàn)的未知移動障礙物進行碰撞預(yù)測并提前采取改變航向航速等策略做出避碰動作，確認(rèn)安全之后再回到已規(guī)劃好的主路徑上來［2］。因此，AUV 的在線路徑規(guī)劃對整體任務(wù)的安全性至關(guān)重要，研究AUV 的實時自主避碰策略具有重要意義［3］。

本文研究AUV 在水下未知環(huán)境中自主避碰問題，即根據(jù)如多波束聲吶、雙目攝像機等傳感器實時數(shù)據(jù)，獲取障礙物位置信息實時預(yù)測障礙物軌跡，提前采取適當(dāng)策略進行自主避碰［4］。該問題的難點在于傳感器探測范圍有限導(dǎo)致的信息處理時間短和AUV 避碰動作受自身性能約束等因素限制無法實現(xiàn)快速制動與轉(zhuǎn)向。國內(nèi)AUV 路徑規(guī)劃技術(shù)在十幾年的發(fā)展過程中，主要研究集中在靜態(tài)環(huán)境或單一動態(tài)障礙物環(huán)境，對于復(fù)雜動態(tài)障礙物環(huán)境下的自主避碰研究較少［5］。

現(xiàn)有的AUV 動態(tài)避障方法主要有人工勢場法、遺傳算法、滾動?xùn)鸥穹ā⒛：窠?jīng)網(wǎng)絡(luò)、速度矢量法［6~9］等。例如，毛宇鋒［10］等利用水下航行器與動態(tài)障礙物的相對位置與速度關(guān)系建立速度矢量坐標(biāo)系，根據(jù)相對速度判斷碰撞風(fēng)險，對有碰撞風(fēng)險的目標(biāo)采取最小轉(zhuǎn)向原則求得AUV 得安全避碰路徑。嚴(yán)浙平［11］等將動態(tài)障礙物位置速度、AUV位置速度等信息進行二進制編碼簡化將二位信息簡化為一維信息，將AUV 的下一步航向及速度作為輸出采用遺傳算法求得無碰路徑。王秀芳［12］首先采用滾動?xùn)鸥駥崟r更新障礙物信息構(gòu)建環(huán)境模型，然后采用生物啟發(fā)模型將柵格地圖與神經(jīng)網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)相結(jié)合，通過計算神經(jīng)元活性值尋找安全路徑。這些方法往往有一些缺陷，如人工勢場法容易陷入局部最優(yōu)，遺傳算法等啟發(fā)式算法搜索時間長無法滿足實時性，滾動?xùn)鸥穹o法應(yīng)用于復(fù)雜環(huán)境等。如何做到簡化環(huán)境模型，實時處理信息及時反應(yīng)做出避碰動作就成為一直以來研究的難點。

2 環(huán)境模型構(gòu)建

環(huán)境模型包括前視聲吶模型、AUV 模型、障礙物模型及相對坐標(biāo)系模型，對這些因素分別進行精準(zhǔn)的建模是完成路徑規(guī)劃的基礎(chǔ)。

2.1 聲吶模型

本文采用二維虛擬多波束聲吶進行研究［13］，設(shè)定聲吶的探測距離為150m，多波束數(shù)量為80 條，水平開角為120°，工作頻率為2Hz。當(dāng)障礙物進入聲吶探測范圍時，求出其與相應(yīng)波束之間的交點坐標(biāo)與AUV 坐標(biāo)得到障礙物距離信息，若無交點則返回最大數(shù)值表示未探測到障礙物，之后將得到的信息傳遞到規(guī)劃程序進行實時避碰。

2.2 AUV與障礙物模型

水下環(huán)境中的障礙物多種多樣，大多形狀難以具體描述。本文以障礙物最大邊界為直徑d作外接圓來表示障礙物的實際大小并對其邊界作膨化處理，即在外接圓外增加一個安全閾值m，閾值m的大小由AUV 的最大直徑確定。其構(gòu)造如圖所示。當(dāng)存在多個障礙物距離較近時，將其視作一個大型障礙物處理。同理，AUV 也以最大直徑作外接圓表示［14］。

2.3 相對坐標(biāo)系

為了統(tǒng)一描述AUV 運動狀態(tài)和障礙物運動狀態(tài)，分別建立固定坐標(biāo)系（即大地坐標(biāo)系）與運動坐標(biāo)系。固定坐標(biāo)系取環(huán)境中任意一定點，兩坐標(biāo)軸互相垂直。運動坐標(biāo)系取AUV 質(zhì)心作為原點，橫軸沿AUV 中線指向艇艏，縱軸垂直與橫軸垂直。兩坐標(biāo)系平移原點重合后的旋轉(zhuǎn)角為θ，則運動坐標(biāo)系與大地坐標(biāo)系的變換矩陣可表示為

3 障礙物軌跡預(yù)測

根據(jù)不同時段收集到的位置信息做到對不同障礙物精確可靠的運動軌跡預(yù)測是AUV 動態(tài)避碰的基礎(chǔ)［15］。對于動態(tài)障礙物，需要預(yù)測目標(biāo)的位置坐標(biāo)、速度、航向、加速度等關(guān)鍵信息。在復(fù)雜海洋環(huán)境中，動態(tài)障礙物運動狀態(tài)多種多樣，可預(yù)測的障礙物運動軌跡一般為勻速直線運動或勻變速直線運動。因為卡爾曼濾波相較于普通低通濾波方法具有無延時和信號滯后的優(yōu)點可以對目標(biāo)狀態(tài)進行實時狀態(tài)估計而廣泛運用于運動估計，且只需要輸入當(dāng)前由傳感器采集到的測量值和上一個周期的估計值就能估計當(dāng)前的狀態(tài)，所以本文采用擴展卡爾曼濾波算法對移動障礙物運動狀態(tài)作預(yù)測與估計。

在二維平面中，移動障礙物的運動軌跡模型可表示為

在建立障礙物的數(shù)學(xué)模型之后，就可以通過卡爾曼濾波對障礙物進行精確的估計和預(yù)測。標(biāo)準(zhǔn)卡爾曼濾波算法的適用前提必須是線性系統(tǒng)，當(dāng)障礙物做非線性運動時，標(biāo)準(zhǔn)卡爾曼濾波算法不能將障礙物的加速運動看作隨機噪聲，所以當(dāng)障礙物做變速運動時標(biāo)準(zhǔn)卡爾曼濾波算法會失效。此時采用擴展卡爾曼濾波算法進行狀態(tài)估計［16］，擴展卡爾曼濾波算法通過雅克比矩陣將期望和方差線性化，使卡爾曼濾波適用于非線性系統(tǒng)問題的估計。

4 基于速度合成算法的動目標(biāo)避碰

面對移動障礙物的威脅，通常有三種避碰策略［17］。第一種是保持AUV 航向不變通過改變AUV航速，即AUV 的加速或減速動作實現(xiàn)避碰。這種方法需要AUV 能夠短時間內(nèi)實現(xiàn)增速或減速，對于中小型AUV 其推進器推力往往不足以靈活調(diào)整航速，而且一旦減速之后再加速回到巡航速度能耗巨大。第二種是保持AUV 航速不變通過改變AUV航向，即調(diào)整AUV 前進方向的艏搖角度實現(xiàn)避碰。這種方法受限于AUV 自身轉(zhuǎn)向性能影響難以實際運用。第三種綜合了改變航向和改變航速來及時調(diào)整AUV 姿態(tài)實現(xiàn)避碰，這種方法能耗最小且更加靈活。本文采用第三種避碰策略實現(xiàn)AUV動態(tài)避碰。

4.1 障礙物威脅預(yù)測

通過聲吶等傳感器采集到障礙物信息時并對障礙物軌跡進行狀態(tài)估計之后，首先要對障礙物威脅進行評估，判斷障礙物與AUV 前進路徑的最小距離是否小于安全距離從而決策是否采取避碰行為。

在大地坐標(biāo)系中，假設(shè)AUV 的安全半徑為m，障礙物的半徑為n。AUV 的位置坐標(biāo)和速度分別為Pauv(x1，y1) 、vauv(vx1，vy1) ，障礙物的位置坐標(biāo)和速度分別為Pobs(x2，y2)，vobs(vx2，vy2)可 得AUV 與障礙物在水平方向與垂直方向距離差分別為(xd，yd)，水平方向與垂直方向速度差分別為(vxd，vyd)，即得到二者的相對距離與相對速度。通過式（3）可得AUV 與障礙物路徑相交時所需時間t。

將t帶入式（4）可得AUV與障礙物得最小距離Dt。

將Dt與安全距離m比較，若Dt小于m則進入下一步避碰程序，反之則AUV 按原速度與航向繼續(xù)行進。

4.2 期望航向與航速求解

通過障礙物威脅預(yù)測模型可知，AUV 的避碰目的就是使AUV 與障礙物的相對速度在交會時間內(nèi)所駛過的相對距離大于安全距離從而解除碰撞威脅。本文避碰策略為綜合避碰策略，所以，AUV的避碰行為由航向更新機制和航速更新機制兩部分共同組成。

1）航向更新機制

已知AUV 在無障礙環(huán)境中，水平坐標(biāo)系中航向角角為θ，AUV 的速度矩陣vauv和障礙物的速度矩陣vobs，可得AUV 和障礙物相對速度vre=vauvvobs，其模值為 ||vre。根據(jù)相對速度可導(dǎo)出其與AUV速度夾角α的關(guān)系如下。

在水平坐標(biāo)系中，AUV 與障礙物相對位置的改變可以用AUV 質(zhì)點與障礙物質(zhì)點的連線，AUV與障礙物膨化模型過連線的切線，兩條線之間的夾角表示，交點的位置坐標(biāo)可由式（6）推導(dǎo)。

在得到交點坐標(biāo)后，AUV 與障礙物邊界同側(cè)下切線的夾角β可以由障礙物與交點連線角tan(Pobs，Pi)減去上切線與質(zhì)點連線角tanδ=得到。此時，

而航向角更新系數(shù)即障礙物同側(cè)切線角與相對速度角差的絕對值，即

根據(jù)AUV 速度正切值tanvauv和相對速度正切值tanvre的大小關(guān)系判斷AUV航向變化的方向，并得到AUV的期望航向。

2）航速更新機制

根據(jù)式（5）可知，AUV 與障礙物相對速度的變化量為 ||vre×sinα，則更新后的航速為

AUV 在判定與障礙物存在碰撞風(fēng)險之后即進入自主避碰程序，通過預(yù)測障礙物運動軌跡，實時調(diào)整航向、航速。在與障礙物距離到威脅范圍之外后，重新根據(jù)目標(biāo)點位置距離最短原則調(diào)整航向，并回復(fù)到巡航航速以節(jié)約能量消耗。

5 仿真實驗與結(jié)果分析

為驗證本文算法的可行性，在Matlab2016b 環(huán)境下進行仿真試驗。AUV 的基本參數(shù)如下，聲吶探測范圍為150m，AUV 安全直徑3m，巡航速度為2.5m/s，最大航速為3.5m/s。安全距離為50m，威脅距離為5m。轉(zhuǎn)向角最大/最小控制輸入為aθmax=2°/s，aθmin=-2°/s。

5.1 障礙物軌跡估計試驗

首先以二維平面空間中勻變速直線運動為例，采用擴展卡爾曼濾波算法進行預(yù)測，預(yù)測結(jié)果如圖2所示。

圖2 軌跡測量位置曲線與預(yù)測位置曲線

從仿真結(jié)果可以看出，擴展卡爾曼濾波算法很好的剔除了噪聲，預(yù)測曲線十分理想。試驗證明，擴展卡爾曼濾波算法能夠很好地對變速目標(biāo)進行狀態(tài)估計和預(yù)測。

5.2 單個動目標(biāo)避碰試驗

第二項是在二維平面中對AUV 進行動態(tài)避碰仿真試驗。AUV 的起點位置坐標(biāo)為( 0 ，0 )，終點位置坐標(biāo)為(1 00，100 )。障礙物直徑5m ，從左上方往右下方運動，沿x軸速度為0.5m/s，沿y軸速度為-2m/s。規(guī)劃結(jié)果如圖3所示。

圖3 單個動礙物避碰路徑示意圖

從圖3 可以看出AUV 在動礙物在探測到進入威脅距離后改變航向開始進行自主避碰，在確認(rèn)離開威脅距離后再次改變航向以最短路徑向目標(biāo)點前進。圖4 是AUV 與障礙物距離曲線示意圖。從圖中可以看出，AUV 與障礙物最近點約為10m，滿足AUV避碰要求。

圖4 AUV與障礙物距離曲線

圖5 是AUV 速度變化曲線，可以看出AUV 在進入自主避碰決策中，不僅改變了航向且同時調(diào)整了航速。經(jīng)過一段時間的加速航行確認(rèn)駛出威脅距離后降速回到正常巡航航速，整個加速過程在AUV 最大航速范圍內(nèi)。圖6 反應(yīng)的是AUV 與初始點到目標(biāo)點連線的角度變化，可以看出兩次調(diào)整航向都在AUV轉(zhuǎn)向角輸出控制范圍內(nèi)。

圖5 AUV速度變化曲線

圖6 AUV航向變化圖

5.3 復(fù)雜動目標(biāo)避碰試驗

第三項是針對復(fù)雜環(huán)境下多個動態(tài)障礙物威脅時的AUV 自主避碰仿真試驗，包括勻速運動障礙物和勻變速運動障礙物。AUV 起點位置坐標(biāo)( 0 ，0 )，終點位置坐標(biāo)( 2 50，250 )，共有4 個移動障礙物。第一個障礙物直徑3m，從左上往右下方向移動，x 軸速度為0.5m/s，y 軸速度為-2m/s。第二個障礙物直徑7m，從正下往正上方向移動，y 軸初速度為0，加速度為0.1m/s，y軸速度為-2m/s。第三個障礙物直徑5m，從右下往左上方向移動，x 軸速度為-0.7m/s，y 軸速度為0.7m/s。第四個障礙物直徑4m，從左往右移動，x 軸初速度為0，加速度為0.1m/s，y軸速度為0m/s。規(guī)劃結(jié)果如圖5所示。

從圖7 可以看出AUV 在前進過程中多次改變航向，從整體視角來看，AUV 連續(xù)避碰效果良好。圖8反映了AUV與各個障礙物的距離關(guān)系，可以看出AUV 通過避碰行為全程與障礙物保持在安全距離之內(nèi)。

圖7 多個動礙物避碰路徑

圖8 AUV與障礙物距離曲線

圖9 和圖10 分別反映了AUV 速度與航向的變化情況。從圖中可以看出，航速最高為3.5m/s，并在離開威脅距離后迅速回到正常巡航狀態(tài)。AUV航速與航向的調(diào)整是相對應(yīng)的，通過混合調(diào)整的策略達(dá)到降低能耗提高靈活性的目的，仿真可知此方法是有效的。

圖1 障礙物模型示意圖

圖9 AUV速度變化曲線

圖10 AUV航向變化圖

6 結(jié)語

本文研究了未知環(huán)境下針對動態(tài)障礙物的AUV 在線路徑規(guī)劃問題。首先，通過AUV 搭載的前視聲吶等傳感器收集到的障礙物信息進行建模，采用擴展卡爾曼濾波算法對移動障礙物進行狀態(tài)估計，預(yù)測障礙物軌跡并判斷碰撞威脅程度；然后，通過AUV 與運動障礙物之間的相對運動關(guān)系，基于速度合成法采用改變航速和改變航向的綜合避碰策略，及時做出避障動作實現(xiàn)實時避碰。最后通過一系列仿真試驗驗證了該方法的有效性。本文存在兩方面不足，一是沒有拓展到復(fù)雜三維空間環(huán)境中，二是對于動態(tài)障礙物的模型沒有考慮更多的可能性，如做弧形運動的障礙物等，后續(xù)還需要繼續(xù)研究。