無人水下航行器未知環(huán)境輪廓構(gòu)建方法

李 楊, 于浩淼, 郭 晨, 賈兆巖

無人水下航行器未知環(huán)境輪廓構(gòu)建方法

李 楊1, 于浩淼1, 郭 晨1, 賈兆巖2

(1. 大連海事大學(xué) 船舶電氣工程學(xué)院, 遼寧 大連, 116026; 2. 中國兵器工業(yè)集團(tuán) 航空彈藥研究院有限公司, 黑龍江 哈爾濱, 150001)

無人水下航行器(UUV)運(yùn)行時(shí)所攜帶的多元測距聲吶在水下環(huán)境獲取到的數(shù)據(jù)置信度較低。因此, 采用均值漂移聚類算法對所獲取到的數(shù)據(jù)進(jìn)行預(yù)處理, 將異常數(shù)據(jù)與正常數(shù)據(jù)分為不同的數(shù)據(jù)類, 得到邊界點(diǎn)的集合; 然后利用Alpha-Shapes算法將邊界點(diǎn)連接成輪廓線; 并且為了滿足UUV跟蹤的理想路徑需平滑的要求, 利用貝塞爾曲線對構(gòu)建的輪廓線進(jìn)行擬合; 最后, 利用控制方法來實(shí)現(xiàn)對所構(gòu)建地圖的跟蹤, 從而解決了UUV在未知海洋環(huán)境下的地圖構(gòu)建問題。

無人水下航行器; 未知環(huán)境; 輪廓構(gòu)建; 均值漂移算法; Alpha-Shapes算法; 貝塞爾曲線

0 引言

受自然環(huán)境影響, 水下航行器在復(fù)雜的海洋環(huán)境中執(zhí)行指定任務(wù)的難度系數(shù)很高[1]。無人水下航行器(unmanned undersea vehicle, UUV)的靈活性、安全性及自主性都使其相較于其他水下航行器具備一定的優(yōu)勢, 因此, 其工作范圍得到大大提升[2]。UUV的這些特性使其能夠更好地完成水下環(huán)境偵察、資源勘探、緊急救援等難度較大、危險(xiǎn)系數(shù)較高的任務(wù)[3]。強(qiáng)大的自主性使其能夠在水下同步實(shí)現(xiàn)未知環(huán)境探測與巡岸跟蹤。但由于工作過程中沒有人的參與, UUV只能通過自身攜帶多元測距聲吶以及其他傳感器對水下周圍環(huán)境進(jìn)行感知[4]。目前, 聲吶是水下測距應(yīng)用最廣泛的傳感器[5]。但在探測過程中, 聲吶會受到外界環(huán)境的干擾使其獲取到的數(shù)據(jù)具有弱觀性[6]?

國內(nèi)外對UUV環(huán)境構(gòu)建的研究主要集中于圖像特征提取及定位導(dǎo)航。Kimball等[7]針對冰川的定深航行問題, 收集水下多波束聲吶圖像, 利用估計(jì)器不僅可以延續(xù)之前獲取的冰川位置, 又能對冰川的平移與旋轉(zhuǎn)進(jìn)行精確估計(jì)。張潔等[8]在UUV地圖構(gòu)建問題中引入了數(shù)字圖像處理方法, 用于處理在水下獲取到的聲吶數(shù)據(jù), 并采用最近鄰數(shù)據(jù)關(guān)聯(lián)(nearest neighbor filter, NNF)算法將所采用的數(shù)據(jù)關(guān)聯(lián), 再利用擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)同時(shí)定位與建圖(simultaneous localization and mapping, SLAM)估計(jì)UUV姿態(tài), 并同步構(gòu)建基于障礙物特征的水下環(huán)境地圖。王宏健等[9]針對EKF在各種不確定情況下的精度降低問題, 提出了通過利用Saga- Husa自適應(yīng)EKF與強(qiáng)跟蹤EKF相結(jié)合的算法來提高UUV在水下進(jìn)行地圖構(gòu)建的精準(zhǔn)度。遲東南[10]針對UUV巡岸控制問題, 提出了利用小波變換將獲取到的聲吶數(shù)據(jù)中的高頻數(shù)據(jù)進(jìn)行放大, 再利用支持向量聚類算法將異常點(diǎn)與正常點(diǎn)分為不同的數(shù)據(jù)類, 進(jìn)而通過貝塞爾曲線對輪廓線進(jìn)行擬合, 最后對擬合后的輪廓線進(jìn)行巡岸跟蹤控制。李海豐等[11]針對視覺SLAM誤差較大的問題, 提出了一種多特征融合的點(diǎn)、線、面SLAM(point line plane SLAM, PLP-SLAM)算法。該算法利用點(diǎn)特征估計(jì)機(jī)器人位姿, 構(gòu)建觀測模型, 并利用線段和平面特征來描述環(huán)境信息。黃靖偉[12]和王富民[13]等都針對視覺SLAM在水下應(yīng)用過程中面對的問題進(jìn)行了解決與優(yōu)化。

文中UUV對海中某小島進(jìn)行輪廓構(gòu)建與同步巡岸, 目的是對海島水下情況進(jìn)行偵察, 及時(shí)發(fā)現(xiàn)可疑問題。首先, 由于UUV攜帶的聲吶采集到的數(shù)據(jù)受環(huán)境干擾具有弱觀性, 因此, 采用均值漂移聚類算法對數(shù)據(jù)進(jìn)行預(yù)處理, 剔除誤差較大的異常數(shù)據(jù), 進(jìn)而得到數(shù)據(jù)點(diǎn)云; 然后利用Alpha-Shapes算法根據(jù)數(shù)據(jù)點(diǎn)云中的邊界點(diǎn)構(gòu)建出島嶼的地圖輪廓; 此時(shí)得到的輪廓線無法滿足UUV本身航行特性對理想路徑的平滑要求, 因此利用貝塞爾曲線對構(gòu)建的輪廓線進(jìn)行擬合。與文獻(xiàn)[14]采用的3次樣條函數(shù)相比, 曲線的擬合效果更好, 更加平滑, 便于后續(xù)跟蹤。最后, UUV將擬合后的輪廓線作為理想路徑進(jìn)行跟蹤。

1 數(shù)據(jù)預(yù)處理

UUV所攜帶聲吶采用聲波返回原理獲取到的數(shù)據(jù)置信度低, 因此采用均值漂移聚類算法[15]對此類數(shù)據(jù)進(jìn)行預(yù)處理, 能夠減少后續(xù)構(gòu)建輪廓的誤差。

均值漂移聚類根據(jù)概率密度函數(shù)內(nèi)的采樣數(shù)據(jù)來確定其局部最大值的位置, 是一種根據(jù)概率求解局部極值的算法。其采用的是一種基于核密度的無參估計(jì), 只要參數(shù)確定, 概率分布即可確定。

無參估計(jì)在多維空間中具有下述形式

其中

將式(3)變?yōu)檩喞瘮?shù)的形式

式(4)即為均值漂移聚類中對應(yīng)于特征值的概率密度函數(shù)計(jì)算公式。

因?yàn)榫灯扑惴ㄐ枰蠼獾氖蔷植繕O值, 且核函數(shù)是可微的, 所以求式(4)的梯度可以得到

(6)

式中, 等號右側(cè)第1項(xiàng)為常量, 第2項(xiàng)為漂移向量

依照上述內(nèi)容可總結(jié)為如圖1所示的聚類算法流程圖。

圖1 聚類算法流程圖

經(jīng)過判斷就可以從最初位置“漂移”至極值點(diǎn)位置, 完成聚類。

2 地圖輪廓構(gòu)建

2.1 邊界點(diǎn)網(wǎng)格篩選

首先對數(shù)據(jù)預(yù)處理后得到的平面點(diǎn)云進(jìn)行數(shù)據(jù)網(wǎng)格化, 這樣可以減少Alpha-Shapes算法的判斷時(shí)間。將點(diǎn)云劃為如圖2所示的圖像網(wǎng)格。網(wǎng)格大小會影響后續(xù)邊界點(diǎn)的提取, 因此, 為了削弱網(wǎng)格邊長選取對提取邊界點(diǎn)的影響, 給出尺寸計(jì)算公式

圖2 網(wǎng)格圖例

Fig. 2 Grid legend

經(jīng)過網(wǎng)格化后, 若圖像內(nèi)有數(shù)據(jù)點(diǎn)則像素為1, 否則為0。依次對每個(gè)網(wǎng)格進(jìn)行判斷, 若像素為1, 且相鄰網(wǎng)格也為1, 即可判斷此網(wǎng)格為內(nèi)部點(diǎn)網(wǎng)格, 不必用Alpha-Shapes算法進(jìn)行判斷; 否則即為邊界點(diǎn)網(wǎng)格, 可用Alpha-Shapes算法來判斷。因此使用網(wǎng)格法可減少Alpha-Shapes算法的判斷時(shí)間。

2.2 邊界點(diǎn)查找

任取邊界點(diǎn)網(wǎng)格內(nèi)點(diǎn)與其鄰域內(nèi)個(gè)近鄰點(diǎn), 取這個(gè)點(diǎn)歐氏距離的平均值, 即

Alpha-Shapes輪廓提取參數(shù)

3 曲線擬合

為了滿足UUV在水下未知環(huán)境探測時(shí)的慣性要求, 需要對已得到的輪廓線進(jìn)行平滑處理, 將得到的平滑曲線作為理想路徑, 便于UUV進(jìn)行跟蹤。

3.1 貝塞爾曲線擬合

貝塞爾曲線[17]是應(yīng)用于二維圖像的數(shù)學(xué)曲線, 是計(jì)算機(jī)圖形學(xué)中相當(dāng)重要的參數(shù)曲線。貝塞爾曲線主要包括起始點(diǎn)、終止點(diǎn)以及控制點(diǎn), 其中, 起始點(diǎn)和終止點(diǎn)也被稱為數(shù)據(jù)點(diǎn)。在操作過程中, 通過改變和調(diào)整控制點(diǎn)的位置來實(shí)現(xiàn)貝塞爾曲線的變化。因此, 控制點(diǎn)決定了曲線的彎曲程度。

3.2 2階貝塞爾曲線

2階貝塞爾曲線由1個(gè)控制點(diǎn)和2個(gè)數(shù)據(jù)點(diǎn)組成, 如圖3所示。在和線段上分別找到和兩點(diǎn)。連接, 找到上一點(diǎn)。將點(diǎn)從點(diǎn)移向點(diǎn),點(diǎn)從點(diǎn)移向點(diǎn), 這段時(shí)間內(nèi)點(diǎn)的移動軌跡即為2階貝塞爾曲線。

2階貝塞爾曲線可以理解為是2個(gè)數(shù)據(jù)點(diǎn)不斷變化的1階貝塞爾曲線。

由文獻(xiàn)[17]可得1階貝塞爾曲線公式為

圖3 2階貝塞爾曲線圖

由1階貝塞爾曲線可知

同理可得3階、4階等高階公式。

3.3 曲線擬合

UUV在跟蹤目標(biāo)路徑的過程中會遇到2種情況: 轉(zhuǎn)彎路段和非轉(zhuǎn)彎路段。

轉(zhuǎn)彎路段的情況如圖3所示, 在這樣的環(huán)境下選擇2階貝塞爾曲線進(jìn)行擬合。非轉(zhuǎn)彎路段的情況是由于UUV在水下探測過程中無法一次性獲得全局?jǐn)?shù)據(jù), 而局部數(shù)據(jù)無法代表整段航程的輪廓線特點(diǎn), 因此, 將這部分局部數(shù)據(jù)暫定為非轉(zhuǎn)彎路段。

滿足此條件控制點(diǎn)個(gè)數(shù)的最小值即為貝塞爾曲線的階次。

出于安全考慮, UUV在跟蹤環(huán)境輪廓線的同時(shí)也要與其保持一定的安全距離。安全距離與UUV自身的屬性如質(zhì)量、重心、回轉(zhuǎn)半徑以及水動力系數(shù)等因素有關(guān), 即

4 仿真結(jié)果及分析

根據(jù)圖4所示流程圖, 對圖5的小島進(jìn)行地圖構(gòu)建并跟蹤。

圖4 仿真流程圖

UUV在水下利用聲吶探測到數(shù)據(jù)點(diǎn)后, 利用均值漂移聚類算法對數(shù)據(jù)進(jìn)行預(yù)處理, 得到如圖6的島嶼輪廓數(shù)據(jù)點(diǎn)云。得到島嶼數(shù)據(jù)云點(diǎn)集后, 利用Alpha-Shapes算法在無序的點(diǎn)集中提取出輪廓, 如圖7所示。

上述操作得到的是封閉的多邊形, 無法滿足UUV在水下探測時(shí)的跟蹤條件。因此, 利用貝塞爾曲線對島嶼邊緣輪廓進(jìn)行擬合, 在保證安全距離的條件下, 得到如圖8所示的平滑輪廓線。圖9為邊緣輪廓線與平滑后的輪廓線的對比圖, 可以看出, 貝塞爾曲線使原本輪廓線不平滑的彎折處更加平滑, 利于后續(xù)UUV進(jìn)行巡岸跟蹤。圖10將文中采用的貝塞爾曲線與3次樣條擬合算法得到的輪廓線與邊緣輪廓進(jìn)行對比, 從中可以看出, 獲取點(diǎn)集數(shù)量較多時(shí), 貝塞爾曲線的擬合效果要優(yōu)于3次樣條法[14], 得到的曲線更加平滑, 更加貼合真實(shí)路徑。該平滑曲線即為UUV水下探測跟蹤島嶼的理想路徑。

圖5 島礁環(huán)境衛(wèi)星圖

圖6 島嶼數(shù)據(jù)點(diǎn)以及噪聲點(diǎn)分布

圖7 島嶼邊緣輪廓

圖8 邊緣平滑曲線

圖9 邊緣輪廓線與平滑后輪廓線對比

圖10 3次樣條法與貝塞爾曲線擬合輪廓線對比

構(gòu)建島嶼輪廓是為了便于實(shí)現(xiàn)UUV在水下的路徑跟蹤。利用欠驅(qū)動UUV路徑跟蹤控制方法[18]對平滑后的島嶼輪廓邊緣路徑進(jìn)行跟蹤。由于巡岸跟蹤主要關(guān)注的是水平面運(yùn)動, 因此, 只需建立3個(gè)自由度的2階UUV水平面動力學(xué)和運(yùn)動學(xué)模型。然后通過引入Serret-Frenet坐標(biāo)系來改進(jìn)傳統(tǒng)的視線法(Line-of-sight, LOS)制導(dǎo)方法。進(jìn)而將航跡跟蹤位置誤差轉(zhuǎn)化為航行器的艏向角誤差。最后利用非奇異終端滑?？刂品椒ㄔO(shè)計(jì)控制器來鎮(zhèn)定艏向角誤差。圖11為在此控制方法下欠驅(qū)動UUV部分路徑跟蹤示意圖。仿真結(jié)果如圖12～13所示, 給出了某一段時(shí)間內(nèi)變量的響應(yīng)曲線。

圖11 UUV部分路徑跟蹤效果

圖12 UUV水平面跟蹤艏向角響應(yīng)曲線

圖13 UUV水平面跟蹤誤差曲線

5 結(jié)束語

針對欠驅(qū)動UUV巡岸地圖構(gòu)建問題, 首先采用均值漂移聚類算法對水下探測過程中聲吶獲取到的數(shù)據(jù)進(jìn)行預(yù)處理, 剔除噪聲點(diǎn), 得到包含島嶼信息的云點(diǎn)集; 再利用Alpha-Shapes算法從云點(diǎn)集中提取出島嶼輪廓點(diǎn), 順次連接得到島嶼輪廓線; 然后, 基于UUV水下探測的屬性, 利用貝塞爾曲線對島嶼輪廓線進(jìn)行擬合, 得到平滑曲線; 最后將該平滑曲線作為理想路徑, 在保證一定安全距離的情況下, 實(shí)現(xiàn)了欠驅(qū)動UUV基于非奇異終端滑模控制方法對規(guī)劃好的路徑進(jìn)行跟蹤; 仿真試驗(yàn)證實(shí)了文中方法可以實(shí)現(xiàn)欠驅(qū)動UUV在未知環(huán)境的地圖構(gòu)建, 并能夠通過設(shè)計(jì)路徑跟蹤控制器執(zhí)行巡岸任務(wù)。

[1] 陳強(qiáng). 水下無人航行器[M]. 北京: 國防工業(yè)出版社, 2014.

[2] 陳子印. 欠驅(qū)動無人水下航行器三維路徑跟蹤反步控制方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2013.

[3] 韓亞楠. 復(fù)雜海洋環(huán)境下的欠驅(qū)動AUV路徑跟蹤控制 [D]. 大連: 大連海事大學(xué), 2020.

[4] Liang X, Qu X R, Hou Y H, et al. Three-dimensional Path Following Control of Underactuated Autonomous Underwater Vehicle Based on Damping Backstepping[J]. International Journal of Advanced Robotic Systems, 2017, 14(4): 1-9.

[5] Chen L, Yang A L, Hu H S, et al. RBPF-MSIS: Toward Rao-Blackwellized Particle Filter SLAM for Autonomous Underwater Vehicle with Slow Mechanical Scanning Imaging Sonar[J]. IEEE Systems Journal, 2020, 14(3): 3301- 3312.

[6] 嚴(yán)浙平, 遲冬南, 趙智, 等. 支持向量聚類的UUV障礙物孤立點(diǎn)惰性檢測[J]. 哈爾濱工程大學(xué)學(xué)報(bào), 2012, 33(11): 1377-1383.

Yan Zhe-ping, Chi Dong-nan, Zhao Zhi, et al. Outlier Inertia Detection of UUV Obstacles Based on Support Vector Clustering[J]. Journal of Harbin Engineering University, 2012, 33(11): 1377-1383.

[7] Kimball P, Rock S. Sonar-based Iceberg-relative Navigation for Autonomous Underwater Vehicles[J]. Deep-Sea Research II, 2011, 58(11-12): 1301-1310.

[8] 張潔, 何波, 姚科, 等. 基于前視聲納的AUV同時(shí)定位與地圖構(gòu)建[J]. 微計(jì)算機(jī)信息(測控自動化), 2009, 36(4): 255-257.

Zhang Jie, He Bo, Yao Ke, et al. Simultaneous Localization and Mapping for an Autonomous Underwater Vehicle Using Forward-looking Sonar[J]. Microcomputer Information, 2009, 36(4): 255-257.

[9] 王宏健, 王晶, 邊信黔, 等. 基于組合EKF的自主水下航行器SLAM [J]. 機(jī)器人, 2012, 34(1): 56-64.

Wang Hong-jian, Wang Jing, Bian Xin-qian, et al. SLAM of AUV Based on the Combined EKF[J]. Robot, 2012, 34(1): 56-64.

[10] 遲冬南. 未知環(huán)境UUV多元聲測距與同步巡岸控制方法[D]. 哈爾濱: 哈爾濱工程大學(xué), 2013.

[11] 李海豐, 胡遵河, 陳新偉. PLP-SLAM: 基于點(diǎn)、線、面特征融合的視覺SLAM方法[J]. 機(jī)器人, 2017, 39(2): 214-220.

Li Hai-feng, Hu Zun-he, Chen Xin-wei. PLP-SLAM: A Visual SLAM Method Based on Point-Line-Plane Feature Fusion[J]. Robot, 2017, 39(2): 214-220.

[12] 黃靖偉. 基于水下圖像信息的同步定位與地圖構(gòu)建方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2019.

[13] 王富民. 自主水下機(jī)器人同時(shí)定位和地圖構(gòu)建方法研究[D]. 沈陽: 沈陽工業(yè)大學(xué), 2020.

[14] 李航. 基于激光測距的UUV地圖輪廓構(gòu)建與同步巡岸控制方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2018.

[15] 王健. 基于均值漂移聚類和優(yōu)化的粒子濾波跟蹤算法研究[D]. 天津: 河北工業(yè)大學(xué), 2014.

[16] 廖中平, 陳立, 白慧鵬, 等. 自適應(yīng)-shapes平面點(diǎn)云邊界提取方法[J]. 長沙理工大學(xué)學(xué)報(bào)(自然科學(xué)版), 2019, 16(2): 15-21.

Liao Zhong-ping, Chen Li, Bai Hui-peng, et al. Adaptive Alpha-shapes Plane Point Cloud Boundary Extraction Method[J]. Journal of Changsha University of Science and Technology(Natural Science), 2019, 16(2): 15-21.

[17] Song B Y, Wang Z D, Zou L. An Improved PSO Algorithm for Smooth Path Planning of Mobile Robots Using Continuous High-degree Bezier Curve[J]. Applied Soft Computing Journal, 2021, 100: 106960.

[18] 李明, 李銅橋. 基于非奇異終端滑模的欠驅(qū)動UUV航跡跟蹤控制[J]. 應(yīng)用科技, 2018, 45(2): 11-16.

Li Ming, Li Tong-qiao. Path-following Control for an Underactuated UUV Based on Non-singular Terminal Sliding Mode[J]. Applied Science and Technology, 2018, 45(2): 11-16.

Construction Method of Unknown Environment Contour for Unmanned Undersea Vehicle

LI Yang1, YU Hao-miao1, GUO Chen1, JIA Zhao-yan2

(1. Dalian Maritime University, College of Marine Electrical Engineering, Dalian 116026, China; 2. NORINCO Group Air Ammunition Research Institute Co., Ltd, Harbin 150001, China)

The confidence level of data measured by multivariate ranging sonar in an unmanned undersea vehicle(UUV) in an underwater environment is generally low. Therefore, the mean shift clustering algorithm was used to preprocess the obtained data. First, the abnormal and normal data are divided into different data categories to obtain the set of boundary points. Second, the boundary points are connected with contour lines using the Alpha-Shapes algorithm. Furthermore, the Bézier curve is used to fit the constructed contour lines to obtain an ideal, smooth tracking path of the UUV. Finally, a control method was used to track the constructed map. Thus, the problem of UUV map construction in an unknown marine environment was effectively addressed.

unmanned undersea vehicle; unknown environment; contour construction; mean shift algorithm; Alpha- Shapes algorithm; Bézier curve

李楊, 于浩淼, 郭晨, 等. 無人水下航行器未知環(huán)境輪廓構(gòu)建方法[J]. 水下無人系統(tǒng)學(xué)報(bào), 2022, 30(2): 197-203.

U674.91; TJ630.34

A

2096-3920(2022)02-0197-07

10.11993/j.issn.2096-3920.2022.02.009

2021-05-20;

2021-07-26.

國家自然科學(xué)基金項(xiàng)目(51809028, 51879027); 中國博士后科學(xué)基金面上項(xiàng)目(2020M670733); 遼寧省博士科研啟動基金計(jì)劃項(xiàng)目(2019-BS-022); 中央高校基礎(chǔ)研究基金項(xiàng)目(3132019318).

李 楊(1997-), 女, 在讀碩士, 主要研究方向?yàn)闊o人水下航行器運(yùn)動控制技術(shù).

(責(zé)任編輯: 許 妍)