專利名稱：自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及水下機(jī)器人導(dǎo)航系統(tǒng)，尤其是一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)。
技術(shù)背景
慣性導(dǎo)航是自主式水下機(jī)器人(Autonomous Underwater Vehicles，簡稱AUV)在 水下自主航行時采用的主要定位手段，但是其主要問題在于精度能滿足要求時，往往價格 過于昂貴。由于聲納技術(shù)的進(jìn)步，目前較先進(jìn)的AUV基本不采用加速度計而是采用多普勒 測速儀，可以直接測出AUV相對于海底的3D速度而無需積分。采用多普勒測速儀與光纖陀 螺相結(jié)合的導(dǎo)航方法，仍然不可避免的產(chǎn)生累積誤差，其導(dǎo)航精度為航程的0. 2%。為消除 這種偏差，目前主要采用的方法有(1)利用GPS修正，即讓AUV在其路途中的一系列位置 上浮，接受GPS信號以修正當(dāng)前位置；(2)聲學(xué)輔助導(dǎo)航，在作業(yè)區(qū)域布置如長基線、短基線 等；(3)地形輔助導(dǎo)航。但是目前上述主要方法均以不同形式對AUV工作范圍加以限制，其 中GPS多次上浮修正偏差，能源消耗很大，限制了工作時間和工作范圍；而聲學(xué)基線方法限 制AUV只能在短距離內(nèi)有效；地形輔助導(dǎo)航類似于巡航導(dǎo)彈所采用的方法，利用地形信息 來定位和導(dǎo)航，對AUV而言，這要求事先必須有精確的海底地形地圖。由于海底地形和陸地 不同，因此獲得精確的海底地圖仍然是各國正在探索的問題，目前而言，只有在近海的部分 區(qū)域才具有比較精確的海底地形地貌數(shù)據(jù)，因此基于海圖的地形輔助導(dǎo)航的應(yīng)用也有很大 局限。
隨著AUV技術(shù)的不斷發(fā)展，其作為深海水下運(yùn)載技術(shù)的重要執(zhí)行載體，將向更長 程、更廣闊工作海域發(fā)展。如果要求AUV在更廣闊的區(qū)域以更低廉的運(yùn)行費(fèi)用工作，一個可 行性思路是盡量減少對母船和基陣等外部導(dǎo)航框架的依賴，使AUV實(shí)現(xiàn)自主導(dǎo)航定位?！巴?時定位與地圖構(gòu)建方法” (Simultaneous Localization and mapping，簡稱SLAM方法)被 認(rèn)為是機(jī)器人在未知環(huán)境中真正實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵。SLAM方法可描述為在未知環(huán)境 下，移動機(jī)器人從未知的起點(diǎn)開始，經(jīng)過一系列位置，利用機(jī)載傳感器信息構(gòu)建增量式環(huán)境 地圖同時對自身位置定位。該方法自1990年提出以來得到很快的發(fā)展。同時定位與地圖 創(chuàng)建方法與地圖表達(dá)形式密切相關(guān)，如柵格地圖、特征地圖、拓?fù)涞貓D等，基于環(huán)境特征的 SLAM方法應(yīng)用最為廣泛，特別是對于戶外環(huán)境，障礙物沒有規(guī)則幾何形狀。它將機(jī)器人的位 姿和環(huán)境特征坐標(biāo)表達(dá)在一個狀態(tài)向量之中，在機(jī)器人的運(yùn)動過程中通過對環(huán)境特征的觀 測作最優(yōu)準(zhǔn)則的估計。分為三個步驟(1)基于外部感知的環(huán)境特征提??；(2)遞推形式的 預(yù)測和更新算法；⑶相應(yīng)的數(shù)據(jù)關(guān)聯(lián)技術(shù)。
為此，聲納技術(shù)應(yīng)用到AUV中，掃描聲納具有較好的成像能力，掃描聲納配合使用 中等精度的慣性導(dǎo)航傳感器即可滿足自主導(dǎo)航定位，并且無需采用昂貴的高精度慣性系 統(tǒng)，大大降低了 AUV的制造成本，針對大范圍的水下環(huán)境，掃描成像聲納和內(nèi)部傳感器可以 實(shí)現(xiàn)大尺度全局自主導(dǎo)航，但是其不適用于在局部海域內(nèi)精確自主導(dǎo)航，尤其是當(dāng)局部海 域的地形地貌比較復(fù)雜時，掃描成像聲納可能無法實(shí)現(xiàn)自主導(dǎo)航的功能。4發(fā)明內(nèi)容
本發(fā)明的目的在于提出了一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)，其同時實(shí)現(xiàn)了在 大尺度、中尺度和小尺度精確自主導(dǎo)航。
本發(fā)明是采用以下的技術(shù)方案實(shí)現(xiàn)的一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)，包 括慣性基礎(chǔ)導(dǎo)航裝置和外部傳感器導(dǎo)航裝置，其中慣性基礎(chǔ)導(dǎo)航裝置包括多普勒測速儀、 光纖陀螺、壓力傳感器、電子羅盤和深度計，外部傳感器導(dǎo)航裝置包括聲納，其中，該組合導(dǎo) 航系統(tǒng)還包括水下結(jié)構(gòu)光傳感器和水下雙目視覺平臺，其中水下結(jié)構(gòu)光傳感器包括位于自 主式水下機(jī)器人外框架前部的前視結(jié)構(gòu)光傳感器和位于外框架底部的下視結(jié)構(gòu)光傳感器， 水下雙目視覺平臺包括位于外框架前部的前視雙目視覺平臺和位于外框架底部的下視雙 目視覺平臺，前視結(jié)構(gòu)光傳感器和前視雙目視覺平臺組成了位于外框架前部的前視結(jié)構(gòu)光 與視覺系統(tǒng)模塊，下視結(jié)構(gòu)光傳感器和下視雙目視覺平臺組成了位于外框架底部的下視結(jié) 構(gòu)光與視覺系統(tǒng)模塊。
本發(fā)明中，所述的聲納包括設(shè)置在外框架前部上方的窄波束掃描成像聲納和設(shè)置 在外框架前部下方的具有較寬波束的普通避碰聲納。所述的水下結(jié)構(gòu)光傳感器包括攝像 機(jī)、激光投射器、振鏡以及振鏡控制板。所述的水下雙目視覺平臺包括兩臺水下高清攝像機(jī) 和對應(yīng)的圖像采集卡。
本發(fā)明還涉及一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)的聲納信號斑特征提取方法， 其中該方法包括以下步驟首先，利用融合脈沖耦合神經(jīng)網(wǎng)絡(luò)、中值濾波和維納濾波優(yōu)勢的 混合濾波進(jìn)行圖像噪聲濾除；然后，利用廣義形態(tài)學(xué)復(fù)合運(yùn)算進(jìn)行邊緣檢測和特征提取，獲 取障礙物信息，并提取障礙物的質(zhì)心應(yīng)用于AUV導(dǎo)航。
本發(fā)明還涉及一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)中水下結(jié)構(gòu)光傳感器的三維 測量方法，其中該方法包括以下步驟首先，對水下攝像機(jī)進(jìn)行標(biāo)定標(biāo)定時可以利用實(shí)驗(yàn) 室的水槽實(shí)現(xiàn)攝像機(jī)標(biāo)定，利用平面網(wǎng)格靶標(biāo)作為標(biāo)定用靶標(biāo)，標(biāo)定過程在水中進(jìn)行，由靶 標(biāo)上的網(wǎng)格點(diǎn)和對應(yīng)的像面坐標(biāo)建立若干共軛對，利用這些共軛對采用改進(jìn)的徑向約束排 列方法來求解攝像機(jī)模型；第二步，結(jié)構(gòu)光光平面標(biāo)定以及測量點(diǎn)坐標(biāo)的確定標(biāo)定時先 在水上根據(jù)振鏡不同轉(zhuǎn)角下在平面網(wǎng)格靶標(biāo)上的交線確定振鏡的初始角，以及振鏡在不同 角度下對應(yīng)的光平面在世界坐標(biāo)系下的方程，從而求出光平面上任一點(diǎn)即水中物體的三維 坐標(biāo)。
本發(fā)明還涉及一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)中水下雙目視覺平臺的三維 測量方法，該方法包括以下步驟
第一步，特征提取采用基于Harris角點(diǎn)的特征提取方法，實(shí)現(xiàn)水下雙目視覺圖 像的特征提取，同時對特征點(diǎn)周圍的灰度分布進(jìn)行了二次曲面擬合，在實(shí)際圖像中提取角 點(diǎn)時，首先確定要提取的特征點(diǎn)數(shù)目N，然后找出角點(diǎn)函數(shù)R(x，y)值最大的N個角點(diǎn)，尋找 局部極值時，先將整個圖像平面分為幾塊小區(qū)域，然后在每個小區(qū)域內(nèi)各提取一定數(shù)目的 特征點(diǎn)以用于匹配；
第二步，立體匹配采用基于Harris角點(diǎn)匹配和基于準(zhǔn)稠密的方法實(shí)現(xiàn)特征的立 體匹配；
第三步，視差圖的計算通過對同一被測物體的不同圖像進(jìn)行Harris角點(diǎn)檢測和 零均值歸一化互相關(guān)最優(yōu)策略匹配擴(kuò)散得到更多特征對應(yīng)點(diǎn)，對各特征點(diǎn)計算視差后得到視差第四步，三維重建采用分格顯示地貌的方法實(shí)現(xiàn)由視差圖重建三維場景。
其中分格顯示地貌的方法主要包括以下步驟(1)計算視差圖里各點(diǎn)在攝像機(jī)坐 標(biāo)系下的三維坐標(biāo)；(2)建立攝像機(jī)坐標(biāo)系與車體坐標(biāo)系的轉(zhuǎn)換關(guān)系，將攝像機(jī)坐標(biāo)系下 的三維坐標(biāo)轉(zhuǎn)換為車體坐標(biāo)系下各點(diǎn)的三維坐標(biāo)；(3)先將視差圖分成4X4塊，共十六個 方格，因?yàn)楫?dāng)攝像頭被固定在機(jī)器人上后有一個俯仰角，所以經(jīng)過第二步坐標(biāo)變換后，每一 個網(wǎng)格對應(yīng)著一個實(shí)際的區(qū)域，這塊區(qū)域與機(jī)器人之前有固定的距離和角度，然后遍歷整 個視差圖，選取適當(dāng)?shù)恼系K物高度閾值T，高于T的物體認(rèn)為是障礙物，機(jī)器人不可通行，對 含有障礙物的方格區(qū)域，選取障礙物的最高點(diǎn)作為該區(qū)域的高度；對于不含障礙物的方格 區(qū)域，取所有落入點(diǎn)高度的平均值作為該區(qū)域的高度，至此上述十六個分格均為帶有高度 信息的方格，而且可以知道它們是否為障礙物，及其位置和距離。
本發(fā)明的有益效果是本發(fā)明中的組合導(dǎo)航系統(tǒng)包括慣性基礎(chǔ)導(dǎo)航裝置和外部傳 感器裝置，其中外部傳感器裝置中的掃描成像聲納作為該機(jī)器人的大尺度探測傳感器，與 慣性基礎(chǔ)導(dǎo)航裝置實(shí)現(xiàn)大尺度全局自主導(dǎo)航；外部傳感器裝置中的水下結(jié)構(gòu)光傳感器可以 實(shí)現(xiàn)三維測量，主要針對地形地貌變化相對平緩的局部復(fù)雜海域，其與掃描成像聲納結(jié)合 可以實(shí)現(xiàn)中尺度的精確自主導(dǎo)航；水下雙目視覺平臺采用雙攝像機(jī)從不同角度獲取被測 物體的兩幅數(shù)字圖像，并基于視差原理恢復(fù)出物體的三維幾何信息，重建物體的三維輪廓 和位置，為水下環(huán)境的三維重建和AUV自主導(dǎo)航提供環(huán)境信息，針對地形地貌變化劇烈和 海況復(fù)雜的局部海域，水下雙目視覺平臺結(jié)合掃描成像聲納可以實(shí)現(xiàn)小尺度的精確自主導(dǎo) 航。因此該組合導(dǎo)航系統(tǒng)可以實(shí)現(xiàn)自主式水下機(jī)器人的精確導(dǎo)航。


圖1為本發(fā)明的結(jié)構(gòu)示意圖2為慣性基礎(chǔ)導(dǎo)航裝置的軟件模塊；
圖3為分布式聯(lián)邦擴(kuò)展卡爾曼濾波方法示意圖4為基于分布式聯(lián)邦擴(kuò)展卡爾曼濾波SLAM方法示意圖5為本發(fā)明中自主式水下機(jī)器人的硬件系統(tǒng)結(jié)構(gòu)示意圖6為本發(fā)明中自主式水下機(jī)器人的軟件系統(tǒng)結(jié)構(gòu)示意圖。
具體實(shí)施方式
下面結(jié)合附圖對本發(fā)明做進(jìn)一步說明。
如圖1所示為本發(fā)明所述的自主式水下機(jī)器人。該機(jī)器人采用開架式機(jī)械結(jié)構(gòu)， 外框架1上固定上下雙層壓力艙2，并設(shè)置五個水下推進(jìn)器，包括三個垂直方向推進(jìn)器和兩 個水平方向推進(jìn)器8，其中在外框架1前部的左右對稱兩側(cè)分別設(shè)有垂直方向推進(jìn)器6，外 框架1中部的左右對稱兩側(cè)分別固定水平方向推進(jìn)器8 ；外框架1的后部上方設(shè)有GPS天 線，后部的中間位置設(shè)有一個垂直方向推進(jìn)器9，該推進(jìn)器的上方安裝有壓力傳感器。
該自主式水下機(jī)器人的導(dǎo)航系統(tǒng)包括慣性基礎(chǔ)導(dǎo)航裝置和外部傳感器導(dǎo)航裝置， 其中慣性基礎(chǔ)導(dǎo)航裝置包括多普勒測速儀、光纖陀螺、壓力傳感器、電子羅盤、深度計等。外 部傳感器導(dǎo)航裝置包括聲納、水下結(jié)構(gòu)光傳感器和水下雙目視覺平臺。聲納包括設(shè)置在外框架1前部上方的窄波束掃描成像聲納3和設(shè)置在外框架1前部下方的具有較寬波束的普 通避碰聲納5，其中掃描成像聲納作為該機(jī)器人的大尺度探測傳感器，與慣性基礎(chǔ)導(dǎo)航裝置 實(shí)現(xiàn)大尺度全局自主導(dǎo)航。水下結(jié)構(gòu)光傳感器包括位于外框架1前部的前視結(jié)構(gòu)光傳感器 和位于外框架1底部的下視結(jié)構(gòu)光傳感器，水下雙目視覺平臺包括位于外框架1前部的前 視雙目視覺平臺和位于外框架1底部的下視雙目視覺平臺。其中，前視結(jié)構(gòu)光傳感器和前 視雙目視覺平臺組成了位于外框架1前部的前視結(jié)構(gòu)光與視覺系統(tǒng)模塊4，下視結(jié)構(gòu)光傳 感器和下視雙目視覺平臺組成了位于外框架1底部的下視結(jié)構(gòu)光與視覺系統(tǒng)模塊7。
慣性基礎(chǔ)導(dǎo)航裝置中，多普勒測速儀用于測量AUV的行進(jìn)速度；光纖陀螺配合電 子羅盤對當(dāng)前時刻載體的姿態(tài)進(jìn)行航行推算，其中光纖陀螺用于獲得AUV的偏航角速率， 電子羅盤用于輸出AUV的三軸姿態(tài)角；壓力傳感器用于測量AUV所處位置與海平面之間的 距離。上述傳感器均設(shè)置在AUV內(nèi)部，所以又稱為內(nèi)部傳感器。慣性基礎(chǔ)導(dǎo)航裝置內(nèi)的各 傳感器通過初始對準(zhǔn)、捷連解算和組合導(dǎo)航等步驟對AUV的速度、位置和姿態(tài)角等狀態(tài)信 息進(jìn)行計算。慣性基礎(chǔ)導(dǎo)航裝置的軟件模塊如圖2所示，結(jié)合附圖對利用慣性基礎(chǔ)導(dǎo)航裝 置獲得AUV狀態(tài)信息的方法具體介紹
第一步，初始對準(zhǔn)由于慣導(dǎo)系統(tǒng)的初值誤差在以后的導(dǎo)航解算中有積累作用，因 此必須控制初始對準(zhǔn)誤差在一定范圍內(nèi)，特別是初始姿態(tài)誤差。采用卡爾曼濾波算法實(shí)現(xiàn) 慣導(dǎo)的精對準(zhǔn)，首先建立速度誤差方程和姿態(tài)誤差方程，然后采用離散卡爾曼濾波算法對 誤差作最優(yōu)估計。
第二步，慣導(dǎo)數(shù)據(jù)捷聯(lián)解算初始對準(zhǔn)以后，首先利用慣導(dǎo)系統(tǒng)輸出的角速率和上 一時刻四元數(shù)，根據(jù)四階龍格-庫塔法更新四元數(shù)，進(jìn)而更新捷聯(lián)矩陣，再根據(jù)捷聯(lián)矩陣與 姿態(tài)角的關(guān)系更新姿態(tài)角；然后，利用慣導(dǎo)系統(tǒng)輸出的比力加速度和上一時刻的速度，根據(jù) 四階龍格-庫塔法更新速度；最后，利用上一時刻的位置，根據(jù)龍格-庫塔法更新位置。
第三步，組合導(dǎo)航首先建立捷聯(lián)慣導(dǎo)系統(tǒng)的誤差方程，包括速度誤差方程、位置 誤差方程和姿態(tài)誤差方程，并用其作為系統(tǒng)狀態(tài)方程；然后分別將電子羅盤、深度計和多普 勒測速儀的觀測作為觀測方程；最后，利用聯(lián)邦擴(kuò)展卡爾曼濾波算法估計速度、位置和姿態(tài) 角等狀態(tài)信息。
自主式水下機(jī)器人對其周圍大尺度環(huán)境的感知主要通過掃描成像聲納3。由于機(jī) 械掃描式聲納的掃描速度很慢，使得AUV在運(yùn)動中得到的完整聲納掃描圖像會出現(xiàn)失真， 從這種聲納圖像中提取環(huán)境特征就會產(chǎn)生錯誤的結(jié)果。因此，在利用聲納束捕捉圖像的同 時，對航行器的運(yùn)動軌跡進(jìn)行預(yù)測，利用軌跡預(yù)測對運(yùn)動產(chǎn)生的圖像失真進(jìn)行補(bǔ)償，降低失 真的影響。經(jīng)過預(yù)處理后即可從聲納數(shù)據(jù)中提取特征，特征提取方法主要有點(diǎn)特征提取方 法和斑狀特征提取方法。
從聲納數(shù)據(jù)中提取點(diǎn)特征分為如下三個步驟(a)對聲納所有的單個Ping找出和 主返回之間的距離，這個距離實(shí)際上代表了和目標(biāo)之間的距離；(b)把這些主返回分成若 干簇，其中那些規(guī)模小的、明顯的簇可以認(rèn)為是點(diǎn)特征，進(jìn)而做距離/方位的估計處理；(C) 將距離/方位信息與地圖中已有的特征進(jìn)行匹配。
在水下環(huán)境中，礁石的突出部分和海底凸起的巖礁在聲納數(shù)據(jù)中都可以歸類為點(diǎn) 特征，大面積巖礁的返回相對平緩得多，不視為點(diǎn)特征，只作為障礙。從單個Ping找到那些 主返回以后，再找出當(dāng)前掃描數(shù)據(jù)中與主返回等深的區(qū)塊，就可以作為點(diǎn)特征。處理過程7中，檢查掃描數(shù)據(jù)的連續(xù)Pings看是否存在連續(xù)的主返回，如果多個主返回形成的簇比較 小，也可以視為一個點(diǎn)特征。進(jìn)一步，再計算點(diǎn)特征的方位角，方法是找到點(diǎn)特征的主返回 簇的分布中心，以此點(diǎn)來計算；計算距離時，取簇中的主返回距離的均值。
從聲納數(shù)據(jù)中提取斑狀特征包括以下步驟首先，利用融合脈沖耦合神經(jīng)網(wǎng)絡(luò)、中 值濾波和維納濾波優(yōu)勢的混合濾波進(jìn)行圖像噪聲濾除；然后，利用廣義形態(tài)學(xué)復(fù)合運(yùn)算進(jìn) 行邊緣檢測和特征提取，獲取障礙物信息，并提取障礙物的質(zhì)心應(yīng)用于AUV導(dǎo)航。
水下結(jié)構(gòu)光傳感器包括攝像機(jī)、激光投射器、振鏡以及振鏡控制板，該傳感器可以 實(shí)現(xiàn)三維測量，主要針對地形地貌變化相對平緩的局部復(fù)雜海域，其與掃描成像聲納結(jié)合， 可以實(shí)現(xiàn)中尺度的精確自主導(dǎo)航。由于被測目標(biāo)在水中，攝像機(jī)設(shè)置在具有玻璃窗口的密 封的殼體中，從被測目標(biāo)所在的世界坐標(biāo)系到像面坐標(biāo)系要由水到玻璃，再由玻璃到空氣， 發(fā)生兩次折射，常規(guī)的攝像機(jī)模型不再適用，可以通過以下步驟實(shí)現(xiàn)水下結(jié)構(gòu)光傳感器的 三維測量
首先，對水下攝像機(jī)進(jìn)行標(biāo)定標(biāo)定時可以利用實(shí)驗(yàn)室的水槽實(shí)現(xiàn)攝像機(jī)標(biāo)定，利 用平面網(wǎng)格靶標(biāo)作為標(biāo)定用靶標(biāo)，標(biāo)定過程在水中進(jìn)行，由靶標(biāo)上的網(wǎng)格點(diǎn)和對應(yīng)的像面 坐標(biāo)建立若干共軛對，利用這些共軛對采用改進(jìn)的徑向約束排列方法來求解攝像機(jī)模型， 該方法可直接解出攝像機(jī)模型中的未知參數(shù)。為了實(shí)現(xiàn)利用結(jié)構(gòu)光來對水下大尺度空間進(jìn) 行測量，采用將水上標(biāo)定結(jié)果的參數(shù)轉(zhuǎn)換為水下測量結(jié)果來實(shí)現(xiàn)攝像機(jī)的標(biāo)定，在水上可 以利用平面網(wǎng)格靶標(biāo)進(jìn)行較大尺度的標(biāo)定，在實(shí)驗(yàn)室內(nèi)可利用3*3米靶標(biāo)進(jìn)行水上標(biāo)定， 由于系統(tǒng)的測量景深較大，該標(biāo)定結(jié)果可用于整個測量范圍，將水上標(biāo)定結(jié)果代入?yún)?shù)轉(zhuǎn) 換模型可用于水下大尺度的測量；
第二步，結(jié)構(gòu)光光平面標(biāo)定以及測量點(diǎn)坐標(biāo)的確定結(jié)構(gòu)光標(biāo)定是確定結(jié)構(gòu)光光 平面和攝像機(jī)像面的映射關(guān)系。在本系統(tǒng)中，激光器投射出的激光平面經(jīng)振鏡反射后穿過 空氣和玻璃進(jìn)入水中，發(fā)生了兩次折射。該部分的標(biāo)定方法同上一部分的標(biāo)定過程，先在水 上根據(jù)振鏡不同轉(zhuǎn)角下在平面網(wǎng)格靶標(biāo)上的交線確定振鏡的初始角，以及振鏡在不同角度 下對應(yīng)的光平面在世界坐標(biāo)系下的方程，從而求出光平面上任一點(diǎn)即水下物體點(diǎn)的三維坐 標(biāo)。
本發(fā)明利用水下結(jié)構(gòu)光傳感器作為一個相對獨(dú)立的模塊，分別用于水下地形地貌 的測量和AUV的前視導(dǎo)航，結(jié)構(gòu)光的標(biāo)定可獨(dú)立進(jìn)行，即在安裝到AUV上之前進(jìn)行標(biāo)定，將 結(jié)構(gòu)光的測量過程做成單獨(dú)的動態(tài)庫供AUV主機(jī)調(diào)用，該庫中主要包含三個函數(shù)。AUV主機(jī) 通過調(diào)用其中的測量函數(shù)可返回被測目標(biāo)的三維點(diǎn)云數(shù)據(jù)。根據(jù)這些數(shù)據(jù)可繪制海底地形 地貌，也可以獲得前方障礙物的距離形狀、體積等信息，為實(shí)現(xiàn)準(zhǔn)確導(dǎo)航提供豐富的信息。
水下雙目視覺平臺包括兩臺水下高清攝像機(jī)和圖像采集卡。該平臺采用雙攝像機(jī) 從不同角度獲取被測物體的兩幅數(shù)字圖像，并基于視差原理恢復(fù)出物體的三維幾何信息， 重建物體的三維輪廓和位置，為水下環(huán)境的三維重建和AUV自主導(dǎo)航提供環(huán)境信息。針對 地形地貌變化劇烈和海況復(fù)雜的局部海域，水下雙目視覺平臺結(jié)合掃描成像聲納可以實(shí)現(xiàn) 小尺度的精確自主導(dǎo)航。水下雙目視覺平臺采用基于Harris角點(diǎn)的圖像特征提取、特征匹 配和視差圖計算等步驟，并在此基礎(chǔ)上利用分格顯示地貌實(shí)現(xiàn)三維重建。下面對利用水下 雙目視覺平臺實(shí)現(xiàn)三維重建的方法詳細(xì)的說明
第一步，特征提取由于水下場景的隨意性，同時提取的特征點(diǎn)應(yīng)滿足一致性，即兩幅圖像中的特征點(diǎn)應(yīng)該盡量一一對應(yīng)，并且它的周圍點(diǎn)應(yīng)該包含足夠的信息以便于匹 配，因此采用基于Harris角點(diǎn)的特征提取方法，實(shí)現(xiàn)水下雙目視覺圖像的特征提取。同時， 對特征點(diǎn)周圍的灰度分布進(jìn)行了二次曲面擬合，通過求解該二次曲面的極值可以將精度提 高到亞像素級而Harris角點(diǎn)提取方法只能精確到像素級，因此進(jìn)一步提高了精度；其次， 在實(shí)際圖像中提取角點(diǎn)時，首先確定要提取的特征點(diǎn)數(shù)目N，然后找出角點(diǎn)函數(shù)R(x，y)值 最大的N個角點(diǎn)，克服了閾值選擇較為困難的問題；另外，由于角點(diǎn)可能集中在圖像的個別 區(qū)域，而另外的區(qū)域可能沒有足夠的角點(diǎn)用于匹配和重建。因此在尋找局部極值時，先將整 個圖像平面分為幾塊小區(qū)域，然后在每個小區(qū)域內(nèi)各提取一定數(shù)目的特征點(diǎn)以用于匹配。
第二步，立體匹配主要采用基于Harris角點(diǎn)匹配和基于準(zhǔn)稠密的方法實(shí)現(xiàn)特征 的立體匹配。
第三步，視差圖的計算通過對左右兩幅圖像進(jìn)行Harris角點(diǎn)檢測和零均值歸一 化互相關(guān)(Zero-mean normalized cross-correlation，簡稱ZNCC)最優(yōu)策略匹配擴(kuò)散得到 更多特征對應(yīng)點(diǎn)，對各特征點(diǎn)計算視差后得到視差圖。
第四步，三維重建根據(jù)立體視覺在導(dǎo)航應(yīng)用中的特點(diǎn)，采用一種分格顯示地貌的 方法實(shí)現(xiàn)由視差圖重建三維場景1)計算視差圖里各點(diǎn)在攝像機(jī)坐標(biāo)系下的三維坐標(biāo)；2) 建立攝像機(jī)坐標(biāo)系與車體坐標(biāo)系的轉(zhuǎn)換關(guān)系，將攝像機(jī)坐標(biāo)系下的三維坐標(biāo)轉(zhuǎn)換為車體坐 標(biāo)系下各點(diǎn)的三維坐標(biāo)；幻先將視差圖分成4X4塊，共十六個方格，因?yàn)楫?dāng)攝像頭被固定 在機(jī)器人上后有一個俯仰角，所以經(jīng)過第二步坐標(biāo)變換后，每一個網(wǎng)格對應(yīng)著一個實(shí)際的 區(qū)域，這塊區(qū)域與機(jī)器人之前有固定的距離和角度，然后遍歷整個視差圖，選取適當(dāng)?shù)恼系K 物高度閾值T，高于T的物體認(rèn)為是障礙物，機(jī)器人不可通行，對含有障礙物的方格區(qū)域，選 取障礙物的最高點(diǎn)作為該區(qū)域的高度；對于不含障礙物的方格區(qū)域，取所有落入點(diǎn)高度的 平均值作為該區(qū)域的高度，這樣就得到了十六個有高度信息的方格，而且可以知道它們是 否為障礙物，以及它們的位置和距離等信息。通過分格顯示地貌的算法，可以減少數(shù)據(jù)量。 實(shí)驗(yàn)結(jié)果表明，該算法計算量小、魯棒性強(qiáng)，完全滿足視覺導(dǎo)航中的實(shí)時性、準(zhǔn)確性要求。
由于本發(fā)明中的水下機(jī)器人采用包括慣性基礎(chǔ)導(dǎo)航裝置和外部傳感器導(dǎo)航裝置 的組合導(dǎo)航系統(tǒng)，為了有效地融合聲納、水下結(jié)構(gòu)光傳感器、水下雙目立體視覺平臺以及慣 性基礎(chǔ)導(dǎo)航裝置的傳感器信息，因此如圖3所示，本發(fā)明利用分布式聯(lián)邦擴(kuò)展卡爾曼濾波 方法實(shí)現(xiàn)了慣性基礎(chǔ)導(dǎo)航裝置、掃描成像聲納、水下雙目視覺系統(tǒng)和結(jié)構(gòu)光傳感器的數(shù)據(jù) 融合，從而實(shí)現(xiàn)了以掃描成像聲納作為大尺度探測傳感器、以結(jié)構(gòu)光為中尺度探測傳感器， 以雙目視覺系統(tǒng)為小尺度探測傳感器的多傳感器AUV精確組合導(dǎo)航裝置。如圖4所示，SLAM 子系統(tǒng)的執(zhí)行過程大體可以分為三個階段預(yù)測，觀測和更新。預(yù)測階段，慣性基礎(chǔ)導(dǎo)航裝 置將檢測到的姿態(tài)、速度和初始位置等數(shù)據(jù)輸入SLAM子系統(tǒng)的AUV運(yùn)動方程中，通過AUV 運(yùn)動方程計算出的位置預(yù)測和誤差協(xié)方差預(yù)測輸入觀測方程中得到預(yù)測的觀測值；觀測階 段，外部傳感器裝置對周圍環(huán)境中的特征再次觀測，將實(shí)際觀測的距離和方位角等數(shù)據(jù)輸 入SLAM子系統(tǒng)中；更新過程中，通過數(shù)據(jù)關(guān)聯(lián)過程確定預(yù)測的觀測值與實(shí)際觀測值的一一 對應(yīng)關(guān)系，并計算殘差1、民、W…更新階段，將兩者的殘差最為觀測值，通過擴(kuò)展示卡爾曼濾 波處理后，對載體和特征的狀態(tài)估計進(jìn)行改善和更新。
在深海復(fù)雜環(huán)境下應(yīng)用時，傳統(tǒng)的擴(kuò)展示卡爾曼濾波算法時間復(fù)雜度較高，隨著 特征點(diǎn)的個數(shù)增加，運(yùn)算時間急劇增長，最消耗時間的部分是環(huán)境特征協(xié)方差矩陣的求逆9運(yùn)算，由于本系統(tǒng)環(huán)境特征協(xié)方差矩陣具有正定性和對稱性，對該矩陣進(jìn)行LDL分解，這樣 只需對其中一部分進(jìn)行求逆就能得到原來矩陣的逆矩陣，提高了矩陣求逆計算的效率，時 間大約為原來的三分之一；另外在實(shí)際應(yīng)用中，AUV自身的更新和環(huán)境特征的更新一般是 不同步的，因此采取更新AUV位置多次，更新一次環(huán)境特征的方法，使AUV的運(yùn)動軌跡更加 精確。在數(shù)據(jù)關(guān)聯(lián)方面，采用基于最近鄰數(shù)據(jù)關(guān)聯(lián)算法的數(shù)據(jù)關(guān)聯(lián)算法，并輔以緩沖技術(shù)即 開辟一個臨時特征數(shù)組，將新的特征先放入臨時數(shù)組中，只有當(dāng)滿足一定條件時再加入到 實(shí)際的地圖中去，有效的避免了雜波以及多重匹配的錯誤，提高了系統(tǒng)的穩(wěn)定性。通過在算 法復(fù)雜度和數(shù)據(jù)關(guān)聯(lián)兩方面進(jìn)行改進(jìn)，使得在環(huán)境特征點(diǎn)較少的情況下能夠達(dá)到很好的精 度和實(shí)時性。
如圖5所示，本發(fā)明所述的自主式水下機(jī)器人主要包括信息采集系統(tǒng)、嵌入式核 心控制系統(tǒng)、應(yīng)急狀態(tài)檢測系統(tǒng)、運(yùn)動驅(qū)動系統(tǒng)和水面計算機(jī)工作站，系統(tǒng)各硬件之間主要 采用PCI總線、RS-232串行通信端口和USB接口進(jìn)行通信。其中嵌入式核心控制系統(tǒng)采用的 是研華UN0-3074嵌入式工控主機(jī)，集成有Pentium Ml. 8GHZ處理器，IGB DDR SDRRAM, 160G 硬盤，通信端口包括四個RS-232接口，四個USB接口，四個PCI接口和八個DIO數(shù)據(jù)輸入輸 出接口，大大提高了嵌入式系統(tǒng)的數(shù)據(jù)處理和存儲能力以及系統(tǒng)穩(wěn)定性。信息采集系統(tǒng)中 包含組合導(dǎo)航系統(tǒng)，其中水下結(jié)構(gòu)光傳感器通過PCI總線與嵌入式核心控制系統(tǒng)連接，實(shí) 現(xiàn)了測量數(shù)據(jù)的采集；水下雙目視覺平臺通過USB接口與嵌入式核心控制系統(tǒng)連接，實(shí)現(xiàn) 了圖像信息的采集；聲納、電子羅盤和多普勒測速儀等慣性基礎(chǔ)導(dǎo)航傳感器通過RS-232串 行通信端口與嵌入式核心控制系統(tǒng)連接，實(shí)現(xiàn)數(shù)據(jù)采集。電源管理模塊可以提供5V，12V和 24V多種電源接口。應(yīng)急狀態(tài)檢測系統(tǒng)中，通過A/D數(shù)據(jù)采集卡獲取AUV深度信息、電池電 壓和漏水等模擬信息，A/D數(shù)據(jù)采集卡通過PCI總線與嵌入式工控機(jī)連接。運(yùn)動驅(qū)動系統(tǒng) 中，通過D/A數(shù)據(jù)采集卡向AUV的無刷電機(jī)推進(jìn)器發(fā)送模擬電壓信號，使其發(fā)出AUV運(yùn)動所 需的推力，從而實(shí)現(xiàn)位置或速度跟蹤控制，D/A數(shù)據(jù)采集卡通過PCI總線與嵌入式工控機(jī)連 接。同時，嵌入式工控機(jī)采用TCP協(xié)議與水面計算機(jī)工作站通信以獲取任務(wù)和目標(biāo)指令。
如圖6所示，AUV的控制系統(tǒng)軟件需要有較好的實(shí)時性和可靠性，本發(fā)明的AUV控 制系統(tǒng)采用采用基于優(yōu)先級的搶占式多任務(wù)調(diào)度的嵌入式實(shí)時操作系統(tǒng)vxWorks，如圖11 所示，系統(tǒng)任務(wù)按功能主要分為四類
(1)數(shù)據(jù)采集子系統(tǒng)該部分根據(jù)核心控制系統(tǒng)的目標(biāo)指令，獲取導(dǎo)航系統(tǒng)的導(dǎo) 航信息以及獲取基礎(chǔ)運(yùn)動控制系統(tǒng)底層傳感器數(shù)據(jù)，包括結(jié)構(gòu)光傳感器、雙目視覺、聲納、 慣導(dǎo)等提供的導(dǎo)航信息以及深度、漏水和電池電壓等信息，其數(shù)據(jù)處理主要是對獲取的數(shù) 據(jù)進(jìn)行預(yù)處理和數(shù)據(jù)融合，根據(jù)感知結(jié)果生成下一時刻的目標(biāo)信息。
(2)核心控制子系統(tǒng)完成核心控制算法的運(yùn)行，根據(jù)感知層提供的觀測數(shù)據(jù)，確 定AUV當(dāng)前的運(yùn)動狀態(tài)和下一步的運(yùn)動目標(biāo)，并利用控制算法獲取各種運(yùn)動控制參數(shù)，計 算得到AUV為了達(dá)到目標(biāo)狀態(tài)所需的各自由度的推力。
(3)運(yùn)動驅(qū)動子系統(tǒng)根據(jù)核心控制系統(tǒng)計算的推力結(jié)果，將推力分配到各推進(jìn)器上。
(4)應(yīng)急情況處理子系統(tǒng)實(shí)現(xiàn)傳感器、推進(jìn)器故障、漏電、漏水和電壓檢測，并根 據(jù)故障的等級對AUV的作業(yè)情況進(jìn)行處理。為了保證任務(wù)的快速響應(yīng)，同時考慮AUV的智 能要求，建立了專門的任務(wù)來處理緊急情況，同時在某些緊急情況下控制系統(tǒng)的信息將不10通過核心控制系統(tǒng)，直接由數(shù)據(jù)采集系統(tǒng)傳遞給運(yùn)動驅(qū)動系統(tǒng)，以提高響應(yīng)時間。
本發(fā)明采用兩組48V/300AH的鋰電池組為AUV提供能源，其參數(shù)為電壓范圍 43. 2V-53. 2V，正常工作電流70A，工作溫度-20°C _50°C。同時開發(fā)了電源管理模塊，可以 為AUV提供5V、12V和24V的電源接口。
權(quán)利要求
1.一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)，包括慣性基礎(chǔ)導(dǎo)航裝置和外部傳感器導(dǎo)航裝 置，其中慣性基礎(chǔ)導(dǎo)航裝置包括多普勒測速儀、光纖陀螺、壓力傳感器、電子羅盤和深度計， 外部傳感器導(dǎo)航裝置包括聲納，其特征在于該組合導(dǎo)航系統(tǒng)還包括水下結(jié)構(gòu)光傳感器和 水下雙目視覺平臺，其中水下結(jié)構(gòu)光傳感器包括位于自主式水下機(jī)器人外框架(1)前部的 前視結(jié)構(gòu)光傳感器和位于外框架(1)底部的下視結(jié)構(gòu)光傳感器，水下雙目視覺平臺包括位 于外框架(1)前部的前視雙目視覺平臺和位于外框架(1)底部的下視雙目視覺平臺，前視 結(jié)構(gòu)光傳感器和前視雙目視覺平臺組成了位于外框架(1)前部的前視結(jié)構(gòu)光與視覺系統(tǒng) 模塊G)，下視結(jié)構(gòu)光傳感器和下視雙目視覺平臺組成了位于外框架(1)底部的下視結(jié)構(gòu) 光與視覺系統(tǒng)模塊(7)。
2.根據(jù)權(quán)利要求1所述的自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)，其特征在于所述的聲納 包括設(shè)置在外框架(1)前部上方的窄波束掃描成像聲納C3)和設(shè)置在外框架(1)前部下 方 的具有較寬波束的普通避碰聲納(5)。
3.根據(jù)權(quán)利要求1所述的自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)，其特征在于所述的水下 結(jié)構(gòu)光傳感器包括攝像機(jī)、激光投射器、振鏡以及振鏡控制板。
4.根據(jù)權(quán)利要求1所述的自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)，其特征在于水下雙目視 覺平臺包括兩臺水下高清攝像機(jī)和對應(yīng)的圖像采集卡。
5.一種權(quán)利要求1所述自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)的聲納信號斑特征提取方法， 其特征在于該方法包括以下步驟首先，利用融合脈沖耦合神經(jīng)網(wǎng)絡(luò)、中值濾波和維納濾波 優(yōu)勢的混合濾波進(jìn)行圖像噪聲濾除；然后，利用廣義形態(tài)學(xué)復(fù)合運(yùn)算進(jìn)行邊緣檢測和特征 提取，獲取障礙物信息，并提取障礙物的質(zhì)心應(yīng)用于AUV導(dǎo)航。
6.一種權(quán)利要求1所述自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)中水下結(jié)構(gòu)光傳感器的三維 測量方法，其特征在于該方法包括以下步驟第一步，對水下攝像機(jī)進(jìn)行標(biāo)定標(biāo)定時利用實(shí)驗(yàn)室的水槽實(shí)現(xiàn)攝像機(jī)標(biāo)定，利用平面 網(wǎng)格靶標(biāo)作為標(biāo)定用靶標(biāo)，標(biāo)定過程在水中進(jìn)行，由靶標(biāo)上的網(wǎng)格點(diǎn)和對應(yīng)的像面坐標(biāo)建 立若干共軛對，利用這些共軛對采用改進(jìn)的徑向約束排列方法來求解攝像機(jī)模型；第二步，結(jié)構(gòu)光光平面標(biāo)定以及測量點(diǎn)坐標(biāo)的確定標(biāo)定時先在水上根據(jù)振鏡不同轉(zhuǎn) 角下在平面網(wǎng)格靶標(biāo)上的交線確定振鏡的初始角，以及振鏡在不同角度下對應(yīng)的光平面在 世界坐標(biāo)系下的方程，從而求出光平面上任一點(diǎn)即水中物體的三維坐標(biāo)。
7.—種權(quán)利要求1所述自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)中水下雙目視覺平臺的三維 測量方法，其特征在于該方法包括以下步驟第一步，特征提取采用基于Harris角點(diǎn)的特征提取方法，實(shí)現(xiàn)水下雙目視覺圖像的 特征提取，同時對特征點(diǎn)周圍的灰度分布進(jìn)行了二次曲面擬合，在實(shí)際圖像中提取角點(diǎn)時， 首先確定要提取的特征點(diǎn)數(shù)目N，然后找出角點(diǎn)函數(shù)R(x，y)值最大的N個角點(diǎn)，尋找局部 極值時，先將整個圖像平面分為幾塊小區(qū)域，然后在每個小區(qū)域內(nèi)各提取一定數(shù)目的特征 點(diǎn)以用于匹配；第二步，立體匹配采用基于Harris角點(diǎn)匹配和基于準(zhǔn)稠密的方法實(shí)現(xiàn)特征的立體匹配；第三步，視差圖的計算通過對同一被測物體的不同圖像進(jìn)行Harris角點(diǎn)檢測和零均 值歸一化互相關(guān)最優(yōu)策略匹配擴(kuò)散得到更多特征對應(yīng)點(diǎn)，對各特征點(diǎn)計算視差后得到視差圖；第四步，三維重建采用分格顯示地貌的方法實(shí)現(xiàn)由視差圖重建三維場景。
8.根據(jù)權(quán)利要求7所述的自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)水下雙目視覺平臺的三維 測量方法，其特征在于所述的分格顯示地貌的方法主要包括以下步驟(1)計算視差圖里各點(diǎn)在攝像機(jī)坐標(biāo)系下的三維坐標(biāo)；(2)建立攝像機(jī)坐標(biāo)系與車體坐標(biāo)系的轉(zhuǎn)換關(guān)系，將攝像機(jī)坐標(biāo)系下的三維坐標(biāo)轉(zhuǎn)換 為車體坐標(biāo)系下各點(diǎn)的三維坐標(biāo)；(3)先將視差圖分成4X4塊，共十六個方格，經(jīng)過第二步坐標(biāo)變換后，每一個網(wǎng)格對應(yīng) 著一個實(shí)際的區(qū)域，這塊區(qū)域與機(jī)器人之前有固定的距離和角度，然后遍歷整個視差圖，選 取適當(dāng)?shù)恼系K物高度閾值T，高于T的物體認(rèn)為是障礙物，機(jī)器人不可通行，對含有障礙物 的方格區(qū)域，選取障礙物的最高點(diǎn)作為該區(qū)域的高度；對于不含障礙物的方格區(qū)域，取所有 落入點(diǎn)高度的平均值作為該區(qū)域的高度，至此上述十六個分格均為帶有高度信息的方格， 而且可以知道它們是否為障礙物，及其位置和距離。
全文摘要
本發(fā)明涉及水下機(jī)器人導(dǎo)航系統(tǒng)，尤其是一種自主式水下機(jī)器人組合導(dǎo)航系統(tǒng)。該系統(tǒng)包括慣性基礎(chǔ)導(dǎo)航裝置和外部傳感器導(dǎo)航裝置，其中慣性基礎(chǔ)導(dǎo)航裝置包括多普勒測速儀、光纖陀螺、壓力傳感器、電子羅盤和深度計，外部傳感器導(dǎo)航裝置包括聲納，其中，該組合導(dǎo)航系統(tǒng)還包括水下結(jié)構(gòu)光傳感器和水下雙目視覺平臺，其中水下結(jié)構(gòu)光傳感器包括位于自主式水下機(jī)器人外框架前部的前視結(jié)構(gòu)光傳感器和位于外框架底部的下視結(jié)構(gòu)光傳感器，水下雙目視覺平臺包括位于外框架前部的前視雙目視覺平臺和位于外框架底部的下視雙目視覺平臺，前視結(jié)構(gòu)光傳感器和前視雙目視覺平臺組成了位于外框架前部的前視結(jié)構(gòu)光與視覺系統(tǒng)模塊，下視結(jié)構(gòu)光傳感器和下視雙目視覺平臺組成了位于外框架底部的下視結(jié)構(gòu)光與視覺系統(tǒng)模塊。
文檔編號G01C21/16GK102042835SQ201010534188
公開日2011年5月4日 申請日期2010年11月5日 優(yōu)先權(quán)日2010年11月5日
發(fā)明者叢艷平, 劉兵, 殷波, 潘學(xué)松, 賈東寧, 魏志強(qiáng) 申請人:中國海洋大學(xué)