王 瀲，陳 霄，完顏娟

（1.解放軍91054 部隊(duì)，北京 102442；2.軍事科學(xué)院，北京 100091）

0 引言

無(wú)人艇作為一種小型的海上無(wú)人智能平臺(tái)，具有高速機(jī)動(dòng)、靈活隱蔽的特點(diǎn)，可潛入敵區(qū)，展開情報(bào)獲取工作，實(shí)現(xiàn)對(duì)關(guān)切海域的持續(xù)監(jiān)視。此外，USV 還可用于反潛、掃雷、電子戰(zhàn)、海上搜救、海洋科考、執(zhí)法取證、后勤支援等軍用和民用領(lǐng)域，具有廣泛的應(yīng)用前景［1-3］。在USV 自主執(zhí)行各項(xiàng)任務(wù)過程中，需要實(shí)時(shí)對(duì)其附近海洋環(huán)境進(jìn)行態(tài)勢(shì)感知，收集靜態(tài)和動(dòng)態(tài)障礙物等信息，來(lái)實(shí)現(xiàn)對(duì)各類障礙物的安全躲避，以此確保所肩負(fù)任務(wù)的順利完成［4-5］。

無(wú)人艇避障算法種類很多，包括Dijkstra、D*、A*、人工勢(shì)場(chǎng)法、快速隨機(jī)搜索樹、速度障礙法等［6-9］。周則興等提出了一種基于改進(jìn)人工勢(shì)場(chǎng)法的USV 避障算法，通過引入自衰減斥力場(chǎng)解決了期望航向角突變和抖動(dòng)的問題，仿真對(duì)比結(jié)果表明了算法的先進(jìn)性［10］。張金澤等提出一種密集障礙物海況下的USV 自主避障算法，該算法基于模糊推理的改進(jìn)型雙窗口動(dòng)態(tài)窗口法設(shè)計(jì)實(shí)現(xiàn)，仿真實(shí)驗(yàn)表明該算法能有效避免USV 從密集障礙物群外繞行，避障路徑更加合理［11］。竇強(qiáng)為解決無(wú)人作戰(zhàn)艇在未知海域避碰規(guī)劃問題，借鑒柵格法和人工勢(shì)場(chǎng)法，設(shè)計(jì)實(shí)現(xiàn)了一種基于導(dǎo)航雷達(dá)回波視頻的實(shí)時(shí)避碰規(guī)劃算法，并通過仿真和實(shí)際試驗(yàn)證明了算法是可行的［12］。劉漸道等提出一種融合避碰規(guī)則的動(dòng)態(tài)窗口法來(lái)解決USV 在海上航行時(shí)的自主避障問題，充分考慮開始避障的時(shí)機(jī)、避讓的轉(zhuǎn)向幅度以及何時(shí)進(jìn)行復(fù)航等因素，仿真實(shí)驗(yàn)驗(yàn)證算法的可行性［13］。

無(wú)人艇等水面航行器在實(shí)際海洋環(huán)境中航行時(shí)，主要會(huì)受到風(fēng)、浪、流等不確定因素的影響。通常情況下，為了確保無(wú)人艇自身能夠按照預(yù)定的航線和設(shè)定的航速航行，通常分兩種情況考慮海洋環(huán)境的動(dòng)態(tài)不確定性：一方面無(wú)人艇通過裝備的風(fēng)向、風(fēng)速、海流等傳感器，實(shí)時(shí)感知海洋環(huán)境的動(dòng)態(tài)變化，通過航跡跟蹤控制算法實(shí)時(shí)補(bǔ)償校準(zhǔn)，確保無(wú)人艇更準(zhǔn)確地跟蹤航線，主要需要考慮風(fēng)向、風(fēng)速、海流等傳感器的測(cè)量誤差；另一方面無(wú)人艇通過搭載的雷達(dá)等傳感器，實(shí)時(shí)感知周圍障礙物的情況，預(yù)測(cè)動(dòng)態(tài)障礙物的運(yùn)動(dòng)軌跡，以在合適的時(shí)間啟動(dòng)恰當(dāng)?shù)谋苷纤惴ǎ瑢?shí)現(xiàn)對(duì)障礙物的準(zhǔn)確躲避，主要需要考慮雷達(dá)等傳感器測(cè)量誤差、觀測(cè)漂移和動(dòng)態(tài)障礙物隨機(jī)運(yùn)動(dòng)不確定性。本文主要研究USV態(tài)勢(shì)自主感知及避障算法，因此，考慮采用已經(jīng)對(duì)風(fēng)向、風(fēng)速、海流等傳感器的測(cè)量誤差進(jìn)行了補(bǔ)償校準(zhǔn)的航跡跟蹤控制算法，例如文獻(xiàn)［14］中的算法，主要考慮傳感器測(cè)量誤差、觀測(cè)漂移和動(dòng)態(tài)障礙物隨機(jī)運(yùn)動(dòng)不確定性這一類因素。

本文針對(duì)USV 在動(dòng)態(tài)不確定海洋環(huán)境下的態(tài)勢(shì)自主感知和實(shí)時(shí)避障問題，充分考慮傳感器測(cè)量誤差、觀測(cè)漂移和動(dòng)態(tài)障礙物隨機(jī)運(yùn)動(dòng)不確定性，對(duì)相關(guān)誤差進(jìn)行不確定性建模，設(shè)計(jì)了一種融合COLREGs 海事規(guī)則的USV 態(tài)勢(shì)自主感知及避障算法，給出了算法詳細(xì)的流程設(shè)計(jì)圖，并通過正面相遇、追越、左右交叉相遇4 種典型的場(chǎng)景，對(duì)本文提出的算法進(jìn)行了仿真實(shí)驗(yàn)驗(yàn)證，仿真結(jié)果表明了本文所提出的考慮COLREGs 規(guī)則和動(dòng)態(tài)不確定海洋環(huán)境下的USV 態(tài)勢(shì)自主感知及避障算法的有效性。

1 三類海事規(guī)則

如圖1 所示，以USV 航向?yàn)榛鶞?zhǔn)，定義追越、交叉相遇和正面相遇3 個(gè)場(chǎng)景。追越：若USV 與障礙物的航向差在［0°，45°）和［315°，360°）區(qū)間，若有碰撞威脅，USV 須從障礙物左側(cè)通過。交叉相遇：若USV 與障礙物航向差在［45°，165°］區(qū)間，若有碰撞威脅，USV 須左側(cè)航行躲避；若差值在（195°，315°）區(qū)間，若有碰撞威脅，USV 須右側(cè)航行躲避。正面相遇：若USV 與障礙物的航向角度差值在（165°，195°］區(qū)間，若有碰撞威脅，USV 須右側(cè)航行躲避。

圖1 沖突場(chǎng)景定義Fig.1 The definitions of conflict scenes

2 融合COLREGs 規(guī)則的速度障礙法

定義使用COLREGs 規(guī)則的時(shí)機(jī)是十分困難的，當(dāng)前學(xué)術(shù)界和工業(yè)界均沒有精確標(biāo)準(zhǔn)，本文采用判定距離L0，以此來(lái)觸發(fā)COLREGs 規(guī)則的啟用時(shí)機(jī)。具體如下：若USV 與障礙物之間的距離大于L0，則不進(jìn)行避讓；若USV 與障礙物之間的距離小于L0，當(dāng)存在碰撞危險(xiǎn)時(shí)，則遵循COLREGs 規(guī)則啟用局部動(dòng)態(tài)路徑規(guī)劃，USV 局部動(dòng)態(tài)路徑規(guī)劃采用融入COLREGs 規(guī)則的速度避障法。

如下頁(yè)圖2 所示，假設(shè)障礙物為圓形并進(jìn)行適度膨脹（非圓形可采用其外接圓表示），USV 相對(duì)于障礙物的速度為V=VA-VB，以USV 的位置為頂點(diǎn)，向障礙物作切線l1，l2，定義兩切線間為錐形碰撞區(qū)并記為VO，即V 處于兩切線之間時(shí)，必會(huì)碰撞。對(duì)于多個(gè)障礙物，VO 域記為多個(gè)障礙物計(jì)算后VOi的集合，即，m 為障礙物的個(gè)數(shù)。當(dāng)USV 的速度矢量與VO 域有重疊部分且距離小于等于L0時(shí)，USV 立即改變航向或速度以脫離碰撞區(qū)。

圖2 速度障礙法Fig.2 Velocity obstacle avoidance algrithm

以障礙物在USV 的左側(cè)為例，假定t=i 時(shí)刻，已知障礙物的速度為VB，航向?yàn)棣菳，USV 的速度為VA，航向?yàn)棣華，障礙物的半徑為r，其與USV 之間的距離為d，且與USV 的連線OO'與X 軸之間的夾角為α，則，。根據(jù)余弦定理可得：

1）對(duì)于執(zhí)行精確循跡任務(wù)的USV 和轉(zhuǎn)向困難或轉(zhuǎn)向代價(jià)較大的大型USV，在探測(cè)到未知障礙物時(shí)，可只改變航速，不改變航向。首先，求解相撞時(shí)的速度，其中，d 為USV 與障礙物間的距離，可通過傳感器獲取，T 為時(shí)間周期。之后，根據(jù)避障要求確定速度的改變量。若要求以避障最短時(shí)間為首要目標(biāo)，則，dmin最小安全距離；若以安全為首要保證，則，dmax為最大安全距離。

2）根據(jù)COLREGs 規(guī)則，改變USV 的航向，可分為USV 航向、航速均改變和僅改變USV 航向兩種情況，需要根據(jù)其與障礙物的相遇場(chǎng)景來(lái)選定，具體設(shè)計(jì)如下：當(dāng)USV 與障礙物相遇場(chǎng)景為“追越”時(shí)，在根據(jù)COLREGs 規(guī)則改變USV 航向的同時(shí)，USV 的期望速度設(shè)計(jì)為原期望航速的1.5 倍；當(dāng)USV 與障礙物相遇場(chǎng)景為“交叉”時(shí)，在根據(jù)COLREGs 規(guī)則改變USV 航向的同時(shí)，USV 的期望速度設(shè)計(jì)為原期望航速的0.5 倍；當(dāng)USV 與障礙物相遇場(chǎng)景為“正面相遇”時(shí)，USV 的期望速度設(shè)計(jì)為原期望航速的0.5 倍。

避障結(jié)束后，USV 根據(jù)自身所在的位置和原全局路徑規(guī)劃的航線信息，在恰當(dāng)?shù)母櫩刂扑惴ǖ淖饔孟拢謴?fù)到原航線上，繼續(xù)自主航行。

3 信息的不確定性建模

由于USV 航行在復(fù)雜時(shí)變的海洋環(huán)境中，障礙物運(yùn)動(dòng)具有一定的不確定性，且USV 航行中對(duì)障礙物位置和運(yùn)動(dòng)的探測(cè)都依靠各類傳感器，如雷達(dá)、測(cè)速儀等，無(wú)法避免地存在測(cè)量誤差或漂移等問題。因此，在USV 態(tài)勢(shì)自主感知及避障過程中，須對(duì)傳感器測(cè)量誤差和障礙物隨機(jī)運(yùn)動(dòng)導(dǎo)致的不確定性進(jìn)行建模。

圖3 不同時(shí)刻的障礙物位置Fig.3 The obstacle location at different times

式中，θ 為待估計(jì)的參數(shù)，Θ 為其取值范圍。假設(shè)X的樣本值為，則的聯(lián)合分布率為。設(shè)定的樣本值為，則事件發(fā)生的概率為：

極大似然函數(shù)為：

由式（8）解得：

由此可得μ，σ2的極大似然估計(jì)量分別為：

同理，應(yīng)用極大似然法估計(jì)Δv 和Δθ 的期望和方差：

在正態(tài)分布中，根據(jù)“3σ”法則約有99.7%的值分布在均值μ 附近3 個(gè)標(biāo)準(zhǔn)差σ 的區(qū)域內(nèi)。

4 考慮不確定信息的USV 態(tài)勢(shì)自主感知及避障算法

如下頁(yè)圖4 所示，在t=i 時(shí)，根據(jù)t=1，2，…，i-1時(shí)刻障礙物的狀態(tài)，對(duì)t=i 時(shí)刻障礙物的速度矢量建模，并利用第3 章中的方法估計(jì)出t=i 時(shí)刻的由此求得t=i 時(shí)的和。根據(jù)“3σ”法則，t=i 時(shí)和的大小分別在區(qū)間內(nèi)。障礙物速度矢量變化最大時(shí)，不確定性最高，據(jù)此確定其二次膨脹半徑R，從而將不確定性轉(zhuǎn)化為確定性。此外，由圖4 中幾何關(guān)系可得：

圖4 考慮障礙物不確定性的情況Fig.4 The situation considering uncertain information about obstacles

圖5 考慮COLREGs 規(guī)則和不確定性信息的USV 局部路徑規(guī)劃流程圖Fig.5 The local path planing flow chart of USV considering COLREGs rules and uncertain information

5 仿真實(shí)驗(yàn)與分析

考慮USV 在沿全局路徑航行時(shí)對(duì)未知?jiǎng)討B(tài)和靜態(tài)障礙物的避障問題，如圖6 所示，紅色區(qū)域?yàn)橐阎撵o態(tài)障礙物，為模擬動(dòng)態(tài)障礙物運(yùn)動(dòng)的不確定性，其速度變化量為Δv=0.2 kn，變化范圍為［-2，2］kn；角度變化量設(shè)置為Δθ=2°，變化范圍為［-15°，15°］，在t=i 時(shí)，根據(jù)t=1，2，…，i-1 時(shí)刻障礙物的運(yùn)動(dòng)狀態(tài)，利用第3 章中的方法估計(jì)出t=i 時(shí)刻的，由此求得t=i 時(shí)的和。根據(jù)“3σ”法則確定障礙物的二次膨脹半徑。取判定距離L0=80 m，最小和最大安全距離為dmin=20 m，dmax=80 m。本章分4 種典型的相遇場(chǎng)景進(jìn)行仿真實(shí)驗(yàn)。

圖6 USV 與障礙物正面相遇Fig.6 USV encountering the obstacle face to face

5.1 正面相遇和追越場(chǎng)景

障礙物設(shè)置如下：

障礙物1：起始點(diǎn)位置坐標(biāo)（350，210），航速為10 kn，航向?yàn)?20°，t=10 s 時(shí)，障礙物開始運(yùn)動(dòng)；

障礙物2：起始點(diǎn)位置坐標(biāo)（850，500），航速為6 kn，航向?yàn)?5°，t=85 s 時(shí)，障礙物開始運(yùn)動(dòng)。

USV 的初始航速仍設(shè)定為20 kn，全局路徑起始點(diǎn)為A，終止點(diǎn)為B，從圖6 可以看出，USV 通過對(duì)障礙物一段時(shí)間內(nèi)（本節(jié)仿真中從障礙物開始運(yùn)動(dòng)時(shí)刻開始計(jì)算）運(yùn)動(dòng)不確定性航向、航速數(shù)據(jù)進(jìn)行不確定性建模，得到膨脹半徑，據(jù)此對(duì)障礙物進(jìn)行二次膨脹，如圖6 中紅色虛線圓所示。

當(dāng)USV 距離障礙物1 80 m 時(shí)，USV 判定會(huì)遇場(chǎng)景為正面相遇，并立即開始進(jìn)行局部路徑規(guī)劃進(jìn)行自主避障，依據(jù)COLREGs 規(guī)則，USV 從被超越障礙物的右舷處航行通過。如下頁(yè)圖7 所示，可以看到USV 在障礙物的右側(cè)規(guī)劃出了一條局部避障路徑，并在航跡跟蹤控制算法的作用下，通過跟蹤規(guī)劃好的局部路徑，實(shí)現(xiàn)了對(duì)障礙物的右舷超越，之后USV 能夠繼續(xù)沿任務(wù)之初規(guī)劃好的全局路徑航行。

圖7 正面相遇下USV 自主避障Fig.7 The autonomous obstacle avoidance of USV in head-on encounter

USV 在沿全局路徑航行過程中，實(shí)時(shí)檢測(cè)未知障礙物，如圖8 所示，當(dāng)USV 距離障礙物2 80 m時(shí)，USV 判定會(huì)遇場(chǎng)景為追越并開始進(jìn)行局部路徑規(guī)劃，依據(jù)COLREGs 規(guī)則，當(dāng)存在碰撞危險(xiǎn)時(shí)，要求USV 從被超越障礙物的左舷處安全通過，如圖9所示，可以看到USV 在障礙物的左舷規(guī)劃出了一條局部避障路徑，并在航跡跟蹤控制算法的作用下，通過跟蹤規(guī)劃好的局部路徑，實(shí)現(xiàn)了對(duì)障礙物的左舷超越，之后USV 能夠繼續(xù)沿任務(wù)之初規(guī)劃好的全局路徑航行。

圖8 USV 與障礙物追越場(chǎng)景仿真圖Fig.8 The overtaking scene simulation diagraph between USV and the obstacle

圖9 追越場(chǎng)景下USV 自主避障Fig.9 The autonomous obstacle avoidance of USV under overtaking scene

5.2 左右交叉相遇場(chǎng)景

障礙物設(shè)置如下：

障礙物3：起始點(diǎn)位置坐標(biāo)（295，375），航速為15 kn，航向?yàn)?45°，t=20 s 時(shí)，障礙物開始運(yùn)動(dòng)；

障礙物4：起始點(diǎn)位置坐標(biāo)（750，305），航速為12 kn，航向?yàn)?25°，t=60 s 時(shí)，障礙物開始運(yùn)動(dòng)。

同樣地，USV 全局路徑的起始點(diǎn)為A，終止點(diǎn)為B，初始航速仍設(shè)定為20 kn，從圖10 可以看出，USV 通過對(duì)障礙物一段時(shí)間內(nèi)運(yùn)動(dòng)不確定性航向、航速數(shù)據(jù)進(jìn)行不確定性建模，得到膨脹半徑，據(jù)此對(duì)障礙物進(jìn)行二次膨脹，如圖10 中紅色虛線圓所示。

圖10 USV 與障礙物左交叉相遇場(chǎng)景仿真圖Fig.10 The left cross encounter scene simulation diagraph between USV and obstacle

當(dāng)USV 距離障礙物3 80 m 時(shí)，USV 判定會(huì)遇場(chǎng)景為左交叉相遇并開始進(jìn)行局部路徑規(guī)劃，依據(jù)COLREGs 規(guī)則，當(dāng)存在碰撞危險(xiǎn)時(shí)，要求USV 左側(cè)航行躲避。如圖11 所示，可以看到USV 在原航行路線的左側(cè)規(guī)劃出了一條局部避障路徑，并在航跡跟蹤控制算法的作用下，通過實(shí)時(shí)跟蹤規(guī)劃好的局部路徑，實(shí)現(xiàn)了對(duì)障礙物的有效躲避，之后USV 能夠繼續(xù)沿任務(wù)之初規(guī)劃好的全局路徑航行，并實(shí)時(shí)檢測(cè)未知障礙物。

圖11 左交叉場(chǎng)景下USV 自主避障Fig.11 The autonomous obstacle avoidance of USV under left cross scene

如圖12 所示，當(dāng)USV 距離障礙物4 80 m 時(shí)，USV 判定會(huì)遇場(chǎng)景為右交叉相遇并開始進(jìn)行局部路徑規(guī)劃，依據(jù)COLREGs 規(guī)則，當(dāng)存在碰撞危險(xiǎn)時(shí)，要求USV 右側(cè)航行躲避，如圖13 所示，可以看到USV 在原航行路線的右側(cè)規(guī)劃出了一條局部避障路徑，并在航跡跟蹤控制算法的作用下，通過實(shí)時(shí)跟蹤規(guī)劃好的局部路徑，實(shí)現(xiàn)了對(duì)障礙物的有效躲避，之后USV 能夠繼續(xù)沿任務(wù)之初規(guī)劃好的全局路徑航行，并實(shí)時(shí)檢測(cè)未知障礙物，直至安全到達(dá)全局路徑的終點(diǎn)。仿真結(jié)果表明了本文所提出的考慮COLREGs 規(guī)則和動(dòng)態(tài)不確定海洋環(huán)境下的USV態(tài)勢(shì)自主感知及避障算法的有效性。

圖12 USV 與障礙物右交叉相遇場(chǎng)景仿真圖Fig.12 The right cross encounter scene simulation diagraph between USV and the obstacle

圖13 右交叉場(chǎng)景下USV 自主避障Fig.13 The autonomous obstacle avoidance of USV under right cross scene

6 結(jié)論

本文針對(duì)USV 在動(dòng)態(tài)不確定海洋環(huán)境下的態(tài)勢(shì)自主感知和實(shí)時(shí)避障問題，設(shè)計(jì)了一種融合COLREGs 海事規(guī)則的USV 態(tài)勢(shì)自主感知及避障算法，該算法充分考慮了傳感器測(cè)量誤差和障礙物隨機(jī)運(yùn)動(dòng)不確定性，對(duì)相關(guān)誤差進(jìn)行不確定性建模，并通過正面相遇、追越、左右交叉相遇4 種典型的場(chǎng)景進(jìn)行仿真實(shí)驗(yàn)證明了算法的先進(jìn)性和有效性。

本文提出的算法融合了真實(shí)的海上航行規(guī)則，且擁有較強(qiáng)的穩(wěn)定性和較低的計(jì)算量，在工程實(shí)踐中具備很好的指導(dǎo)意義。與此同時(shí)，對(duì)我國(guó)USV 相關(guān)航行避障技術(shù)和相關(guān)避障系統(tǒng)等裝備的研制具有一定的理論指導(dǎo)和現(xiàn)實(shí)意義。