基于雙目視覺定位技術(shù)的無人船航跡跟蹤控制室內(nèi)模型實(shí)驗(yàn)

雷 潔，徐文華，許國冬，李明爵

（1.重慶前衛(wèi)科技集團(tuán)有限公司，重慶 401121；2.哈爾濱工程大學(xué)船舶工程學(xué)院，哈爾濱 150001）

1 引言

海洋的開發(fā)利用與貨物運(yùn)輸一般依靠水面航行器。得益于傳感器技術(shù)與無人技術(shù)的發(fā)展，船舶逐漸向智能化方向發(fā)展。通過集成運(yùn)動(dòng)控制系統(tǒng)自動(dòng)跟蹤預(yù)設(shè)航跡，實(shí)現(xiàn)無人駕駛是智能化的首要目標(biāo)。室內(nèi)的模型實(shí)驗(yàn)是研究船舶航跡跟蹤控制系統(tǒng)的重要手段，但船舶的運(yùn)動(dòng)控制需要定位系統(tǒng)進(jìn)行輔助。實(shí)船上通常使用的GPS 定位系統(tǒng)與水聲定位系統(tǒng)在室內(nèi)不能有效地實(shí)現(xiàn)定位與跟蹤。因此，船舶航跡跟蹤控制的室內(nèi)模型實(shí)驗(yàn)需要采用合適的室內(nèi)定位技術(shù)。

當(dāng)前已有的室內(nèi)定位技術(shù)多通過硬件設(shè)備實(shí)現(xiàn)，包括紅外線定位技術(shù)、Wi-Fi 定位技術(shù)以及超聲波定位技術(shù)等。這些定位技術(shù)由信號(hào)發(fā)射器和信號(hào)接收器配合實(shí)現(xiàn)[1-6]。提前將多個(gè)信號(hào)接收器布置在室內(nèi)場景下，信號(hào)發(fā)射器安裝在目標(biāo)物上隨物體運(yùn)動(dòng)并定時(shí)向外發(fā)射固定信號(hào)，在多個(gè)接收器接收到信號(hào)后根據(jù)多邊測量原理計(jì)算目標(biāo)物的實(shí)時(shí)坐標(biāo)。由于布置多個(gè)信號(hào)接收器，造價(jià)以及功耗導(dǎo)致這些定位技術(shù)的成本較高。此外，信號(hào)容易發(fā)生遮擋、折射與衰減，信號(hào)傳播極易受到場地環(huán)境的影響。

雙目視覺定位技術(shù)是近年來比較熱門的定位技術(shù)，已經(jīng)被應(yīng)用到多個(gè)領(lǐng)域，如火星探測器[7]、無人駕駛汽車[8]等，同時(shí)它在室內(nèi)場景下有很強(qiáng)的適用性，比如餐廳的服務(wù)機(jī)器人可以通過雙目視覺技術(shù)，將餐品準(zhǔn)確送到目標(biāo)桌號(hào)[9]。雙目視覺定位技術(shù)是一種基于三角測距原理的定位技術(shù)，通過一組固定且各項(xiàng)參數(shù)信息已知的攝像機(jī)拍攝物體圖像，配合電腦程序?qū)崿F(xiàn)目標(biāo)的定位[10]。相較于常規(guī)的室內(nèi)定位技術(shù)，雙目視覺定位技術(shù)成本較低，定位精度高。雙目視覺技術(shù)的處理對(duì)象是攝像機(jī)拍攝的圖像，信息保留較好，位置信息反饋快，定位結(jié)果的可信度高。本文采用雙目視覺定位技術(shù)，應(yīng)用到船模室內(nèi)航跡跟蹤控制實(shí)驗(yàn)中。實(shí)驗(yàn)表明，該方法實(shí)用可靠、穩(wěn)定性好。

2 雙目視覺定位技術(shù)原理與室內(nèi)船模定位系統(tǒng)

2.1 雙目視覺技術(shù)原理

一般情況下，雙目視覺定位系統(tǒng)使用平行雙目模式，兩臺(tái)攝像機(jī)的光軸平行，成像平面共面并且水平高度一致，光心之間的連線平行于成像平面[11]，具體模型如圖1所示。圖中，lO和rO是左右攝像機(jī)的光心，Z l和Zr軸是兩個(gè)攝像機(jī)的光軸，A和B是兩個(gè)空間點(diǎn)，點(diǎn)A距離成像平面較遠(yuǎn)，它們在兩個(gè)成像平面上的投影是lA和rA以及l(fā)B和rB。由圖可知，空間點(diǎn)在左右成像平面上的投影點(diǎn)的橫坐標(biāo)存在偏差，在雙目視覺系統(tǒng)中，這個(gè)偏差叫做視差，通常用d表示[11-14]?？梢园l(fā)現(xiàn)，點(diǎn)A的視差A(yù)lAr比點(diǎn)B的視差Bl Br要大。

圖1 雙目視覺模型Fig.1 Binocular vision model

為了方便推導(dǎo)空間點(diǎn)坐標(biāo)與視差之間的具體數(shù)學(xué)關(guān)系，現(xiàn)給出了雙目視覺模型的俯視圖[15]，如圖2所示。其中，和是左右攝像機(jī)的光心，線段叫做雙目視覺系統(tǒng)的基線，長度為B。和代表兩個(gè)成像平面，和表示兩個(gè)主點(diǎn)，f是焦距。點(diǎn)P是空間中的一個(gè)任意點(diǎn)，與光心平面的距離是Z，在左右成像平面上的投影分別為和，d是點(diǎn)P的視差。很顯然，在圖2 中，三角形相似于三角形，所以有：

圖2 雙目視覺模型俯視圖Fig.2 Top view of binocular vision model

上式可簡化為：

根據(jù)方程（2）可知，如果已知攝像機(jī)的焦距f、基線B以及空間點(diǎn)的視差d，則可以計(jì)算出該點(diǎn)的空間坐標(biāo)信息。

2.2 雙目視覺攝像機(jī)圖像標(biāo)定與畸變校正

由雙目視覺定位的原理可知，空間點(diǎn)的坐標(biāo)計(jì)算需要知道攝像機(jī)的焦距、基線等參數(shù)，攝像機(jī)的出廠銘牌中一般不完全包含。此外，由于生產(chǎn)裝配過程中的誤差，攝像機(jī)拍攝的圖像存在一定的畸變，而實(shí)際的雙目視覺模型通常如圖3所示。和是左右攝像機(jī)的光心，和為點(diǎn)P在不同攝像機(jī)成像平面上的位置。通過攝像機(jī)標(biāo)定可以得到攝像機(jī)的內(nèi)、外參數(shù)，以此可將實(shí)際模型校正為雙目平行模式，并得到焦距f和基線B。同時(shí)，攝像機(jī)標(biāo)定還能得到畸變參數(shù)，對(duì)圖像畸變進(jìn)行校正。

圖3 雙目視覺實(shí)際模型Fig.3 Practical model of binocular vision

本文基于張正友標(biāo)定法[16]對(duì)攝像機(jī)進(jìn)行標(biāo)定。在標(biāo)定過程中，雙目攝像機(jī)保持不動(dòng)，拍攝不同位置下的棋盤格標(biāo)定板圖像，為了保證標(biāo)定結(jié)果精確，采集了70 組共140 張圖像。根據(jù)標(biāo)定結(jié)果對(duì)左右圖像位置和畸變進(jìn)行校正，結(jié)果如圖4所示。

圖4 攝像機(jī)標(biāo)定結(jié)果Fig.4 Camera calibration result

2.3 室內(nèi)船模運(yùn)動(dòng)定位系統(tǒng)搭建

實(shí)驗(yàn)在室內(nèi)的水池內(nèi)進(jìn)行，主要包含船模、計(jì)算機(jī)以及雙目攝像頭等實(shí)驗(yàn)設(shè)備，各設(shè)備的布置如圖5所示。

圖5 實(shí)驗(yàn)示意圖Fig.5 Schematic diagram of experiment

實(shí)驗(yàn)選用的攝像機(jī)為USB 免驅(qū)雙目攝像機(jī)，如圖6所示。攝像機(jī)通過一條USB3.0 接口的數(shù)據(jù)線與控制電腦主機(jī)相連實(shí)現(xiàn)圖像傳輸，幀率最高可達(dá)60 fps，單個(gè)攝像頭的分辨率為1280×720像素。雙目攝像機(jī)通過鋼材架設(shè)在水池上方，成像平面與水面平行，在經(jīng)過圖像處理后可得出特征點(diǎn)的三維坐標(biāo)信息，成像平面與水面平行可以保證相機(jī)有最大的視野范圍。由于場地限制，攝像機(jī)與水面距離為2 m，攝像機(jī)視野中的水池面積約為2.4 m×1.8 m。

圖6 雙目攝像機(jī)Fig.6 Binocular camera

船模定位系統(tǒng)采用無線控制模式。由Arduino 2560 開發(fā)板、HC-05 藍(lán)牙模塊、直流電機(jī)、L298N電機(jī)驅(qū)動(dòng)器、舵機(jī)、干電池以及船模實(shí)現(xiàn)。HC-05藍(lán)牙模塊包含兩塊，分別連接在電腦主機(jī)和Arduino 2560 開發(fā)板上，實(shí)現(xiàn)信號(hào)的無線傳輸；Arduino 2560 開發(fā)板根據(jù)HC-05 藍(lán)牙模塊接收的信號(hào)控制船模的運(yùn)動(dòng)，一方面直接控制舵機(jī)角度，另一方面采用PWM 技術(shù)通過L298N 電機(jī)驅(qū)動(dòng)器控制直流電機(jī)與船模螺旋槳轉(zhuǎn)速，進(jìn)而控制船模速度；船模中所有硬件設(shè)備均由干電池供電。

室內(nèi)定位系統(tǒng)基于Qt Creator 開發(fā)船模運(yùn)動(dòng)控制程序。采用視線導(dǎo)向法（Line of Sight Guidance，LOS）[17-19]作為船模的航跡跟蹤算法，采用PID 控制器[20-21]對(duì)船模的推進(jìn)系統(tǒng)進(jìn)行控制，采用幀差法[22]根據(jù)船模在前后兩幀上的位置與艏向角計(jì)算縱向速度、橫向速度和艏向角速度，依據(jù)多幀的位置和艏向角計(jì)算縱向加速度、橫向加速度和艏向角加速度。程序中關(guān)于雙目視覺定位的代碼均基于OpenCV 2.4 計(jì)算機(jī)視覺庫，程序界面及工作模塊如圖7所示。

圖7 船模運(yùn)動(dòng)控制程序Fig.7 Ship model motion control program

2.4 圖像預(yù)處理與定位

確定空間點(diǎn)三維坐標(biāo)的前提是準(zhǔn)確找到該點(diǎn)在左右圖像上的投影坐標(biāo)，獲得視差，這在雙目視覺技術(shù)中叫做左右圖像匹配。視覺定位部分的流程圖如圖8所示。實(shí)驗(yàn)中使用船模重心和縱剖面上一點(diǎn)即可反映船模的位置和朝向。為準(zhǔn)確提取上述兩點(diǎn)的投影坐標(biāo)，實(shí)現(xiàn)左右圖像的快速匹配，根據(jù)實(shí)驗(yàn)水池背景為藍(lán)色這一特征，在船模的重心以及縱剖面上粘貼大小不同的白色剪紙，作為圖像匹配的特征物，如圖9所示。

圖8 視覺定位部分流程圖Fig.8 Flow chart of visual positioning

圖9 船模特征物布置示意圖Fig.9 Layout of ship model features

特征物僅占據(jù)了攝像機(jī)采集圖像的極小部分，其余圖像均為干擾。為快速從圖中提取特征物，提前對(duì)圖像進(jìn)行了預(yù)處理，將圖像中的干擾區(qū)域完全剔除掉。由于特征物是依據(jù)顏色選擇的，圖像預(yù)處理的主要方法和步驟均圍繞顏色特征設(shè)計(jì)，具體的處理過程為：（1）圖像灰度化；（2）圖像濾波去噪；（3）圖像二值化；（4）形態(tài)學(xué)處理。具體處理結(jié)果如圖10所示，從圖10（a）與圖10（e）可以看出，圖中的干擾區(qū)域已經(jīng)完全剔除，設(shè)置在船模上的特征物被準(zhǔn)確地保留下來。

圖10 圖像預(yù)處理Fig.10 Image preprocessing

以兩個(gè)特征物區(qū)域的面積大小作為判別依據(jù)，可區(qū)分出不同的點(diǎn)，并實(shí)現(xiàn)左右圖像的匹配。設(shè)正方形區(qū)域形心點(diǎn)為P1，三角形區(qū)域形心點(diǎn)為P2，程序的匹配結(jié)果如圖11所示。從最終的匹配結(jié)果來看，程序成功提取了船模特征物，并根據(jù)船模特征物大小對(duì)其進(jìn)行識(shí)別，實(shí)現(xiàn)了左右船模圖像中同一像素點(diǎn)之間的匹配。在本系統(tǒng)中，方形中心P1 為船體重心位置，視為船模位置坐標(biāo)；三角形中心P2 為船艏位置，通過P1、P2 點(diǎn)坐標(biāo)，即可算出艏向角，從而得到船舶模型相應(yīng)的位置與艏向角信息。

圖11 左右圖像匹配結(jié)果Fig.11 Image matching results

3 室內(nèi)船模航跡跟蹤控制實(shí)驗(yàn)

3.1 雙目視覺定位驗(yàn)證與誤差分析

在開展船模運(yùn)動(dòng)與定位之前，測試所設(shè)計(jì)的雙目視覺定位系統(tǒng)的精度。

雙目視覺定位的基礎(chǔ)原理是提取特征物的形心坐標(biāo)，通過計(jì)算得出目標(biāo)的相關(guān)姿態(tài)信息。上節(jié)中所使用的三角形及方形特征物，本質(zhì)上是為了依靠不同形狀物體的特征面積區(qū)分不同點(diǎn)的坐標(biāo)信息，得到模型的位置與艏向。因此，為了測試定位精度，本節(jié)使用圓形（形心即圓心）進(jìn)行精度的測量。

使用3D 打印技術(shù)制作兩個(gè)大小不同的白色圓形薄片，較小圓的圓心固定在直尺0 刻度位置，較大圓的圓心固定在600mm 刻度位置，即兩圓心之間的距離為600mm，如圖12所示。為保證誤差分析結(jié)果有效可靠，將帶有兩圓的標(biāo)尺擺放在攝像頭視線范圍內(nèi)的不同位置進(jìn)行測量，測得兩個(gè)圓心的平面坐標(biāo)并計(jì)算圓心之間的距離，對(duì)比分析定位系統(tǒng)的視覺圖像測量精度。誤差分析以及后續(xù)的定位工作中需要根據(jù)坐標(biāo)系統(tǒng)確定關(guān)鍵點(diǎn)的具體坐標(biāo)值。在本實(shí)驗(yàn)中，以圖像的正中心為坐標(biāo)原點(diǎn)，水平向右為X軸正向，豎直向上為Y軸正向。此外，在后續(xù)計(jì)算船模的艏向角時(shí)，以Y軸正向?yàn)?°，順時(shí)針遞增至360°。

圖12 測量物體Fig.12 Measuring object

攝像機(jī)捕捉圖像與圓心提取結(jié)果如圖13所示，具體測得的圓心平面坐標(biāo)和誤差值見表1。由表1 可知，當(dāng)前設(shè)計(jì)的雙目視覺定位系統(tǒng)測量的長度最小誤差為0.68%，最大誤差為2.29%。

表1 圓心空間坐標(biāo)與誤差值Table 1 Spatial coordinates and error values of circle center

圖13 不同位置的拍攝圖像及其圓心提取結(jié)果（左側(cè)圖像為攝像機(jī)拍攝原圖，右側(cè)圖像為實(shí)現(xiàn)圖像匹配后的圖像）Fig.13 Captured images at different positions and their circle center extraction results(The left image is the original image taken by the camera,and the right image is the image after image matching)

實(shí)際測量數(shù)據(jù)表明，雙目視覺獲得的位置精度高，完全可以滿足室內(nèi)實(shí)驗(yàn)的精度要求。分析發(fā)現(xiàn)標(biāo)尺在圖像中心區(qū)域的誤差最小，在視線邊緣區(qū)域的誤差較大。

3.2 船模定艏向控制實(shí)驗(yàn)

對(duì)船模的航速與艏向角速度不做限制，進(jìn)行船模的定艏向控制實(shí)驗(yàn)。實(shí)驗(yàn)中圖像豎直向上方向?yàn)?°方向，角度順時(shí)針增加。船模的航行軌跡如圖14所示。為保證足夠的運(yùn)動(dòng)時(shí)長，初始時(shí)刻船模均靜置在靠近攝像機(jī)視野邊界的位置。圖14（a）為目標(biāo)艏向角為60°的船模運(yùn)動(dòng)軌跡圖，初始時(shí)刻船模的艏向角為90°；圖14（b）為目標(biāo)艏向角為270°的船模運(yùn)動(dòng)軌跡圖，初始時(shí)刻船模的艏向角為0°。從船模的實(shí)際航行軌跡來看，船艏最終都轉(zhuǎn)向了設(shè)計(jì)航向，達(dá)到了控制目標(biāo)。

圖14 定艏向控制實(shí)驗(yàn)Fig.14 Fixed heading control experiment

3.3 船模航跡跟蹤控制實(shí)驗(yàn)

航跡跟蹤控制主要使用PID 控制器結(jié)合LOS算法對(duì)船模的航向角進(jìn)行控制，對(duì)船模的航行速度不做要求。航跡的設(shè)計(jì)主要通過設(shè)置航跡點(diǎn)坐標(biāo)實(shí)現(xiàn)。設(shè)置若干數(shù)量的航跡點(diǎn)，再使用直線將航跡點(diǎn)依次連接即可得到設(shè)計(jì)航跡。本文設(shè)計(jì)了航跡點(diǎn)數(shù)分別為3 點(diǎn)和6 點(diǎn)的兩種航跡，控制船模對(duì)其進(jìn)行跟蹤，具體的實(shí)驗(yàn)結(jié)果如圖15所示。圖中黑色虛線為設(shè)計(jì)航跡，黑色實(shí)線為船模的實(shí)際航跡，紅色虛線為路徑點(diǎn)的切換圓。切換圓以航跡點(diǎn)為圓心，一定數(shù)值為半徑，當(dāng)船模跟蹤航跡點(diǎn)行駛至切換圓內(nèi)，船模自動(dòng)按順序跟蹤下一個(gè)航跡點(diǎn)。在本實(shí)驗(yàn)中，3 航跡點(diǎn)跟蹤實(shí)驗(yàn)的切換圓半徑為0.2 m，6 航跡點(diǎn)跟蹤實(shí)驗(yàn)的切換圓半徑為0.1 m。

圖15 航跡跟蹤控制實(shí)驗(yàn)Fig.15 Track tracking control experiment

從圖15 可以看出，船模實(shí)現(xiàn)了對(duì)預(yù)設(shè)航跡的穩(wěn)定跟蹤，證明本文設(shè)計(jì)的雙目視覺定位系統(tǒng)在響應(yīng)速度和定位精度上都能夠滿足室內(nèi)船舶航跡跟蹤控制系統(tǒng)設(shè)計(jì)的要求。

4 結(jié)論

室內(nèi)模型實(shí)驗(yàn)是研究船舶航跡跟蹤控制系統(tǒng)的重要手段，針對(duì)實(shí)船常用定位技術(shù)無法在室內(nèi)環(huán)境下使用的難點(diǎn)，本文提出將雙目視覺技術(shù)作為定位工具，向控制系統(tǒng)實(shí)時(shí)反饋船模的位置信息，輔助船舶航跡跟蹤控制的室內(nèi)模型研究。從船模的位置跟蹤與航跡控制實(shí)驗(yàn)結(jié)果看，設(shè)計(jì)的控制系統(tǒng)對(duì)船模的控制效果達(dá)到了預(yù)期目標(biāo)，雙目視覺定位系統(tǒng)在船模航跡跟蹤控制的室內(nèi)模型研究中可行、可靠。

在實(shí)驗(yàn)過程中，圖像的預(yù)處理方法及流程僅考慮了顏色特征，處理結(jié)果受環(huán)境影響較大，進(jìn)一步研究還可以考慮在不預(yù)設(shè)目標(biāo)顏色和形狀等特征的室外更加復(fù)雜的情況，以滿足更多應(yīng)用。當(dāng)前采用雙目攝像機(jī)分辨率較低，采用高分辨率相機(jī)可提供更高精度的位置信息。

5 展望

本文所使用的雙目視覺定位技術(shù)，其基本原理是通過提取特征圖形的三維坐標(biāo)信息，計(jì)算并得出相關(guān)的姿態(tài)信息，以進(jìn)行后續(xù)操作。實(shí)驗(yàn)水池背景色較為單一，因此該方法可以用于水池實(shí)驗(yàn)環(huán)境中。目前，雙目視覺技術(shù)還處于發(fā)展階段，通過規(guī)則標(biāo)記物定位所得出的坐標(biāo)精度更高。對(duì)于復(fù)雜環(huán)境，該雙目視覺定位技術(shù)在今后的發(fā)展中可進(jìn)行相應(yīng)的改進(jìn)，如可通過與深度學(xué)習(xí)結(jié)合，使用卷積神經(jīng)網(wǎng)絡(luò)與圖像處理相結(jié)合的方式進(jìn)行定位，便可不需通過特征物直接將被測物體識(shí)別出來，實(shí)現(xiàn)對(duì)無明顯標(biāo)記點(diǎn)的無人船進(jìn)行姿態(tài)的提取，這將大大提高該雙目視覺定位技術(shù)的應(yīng)用范圍。