專利名稱：采用同時定位與地圖構(gòu)建方法的自主式水下機器人的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于一種水下航行與運載工具，具體地說是一種采用同時定位與地圖構(gòu)建方法的 自主式水下機器人。
背景技術(shù)：
慣性導(dǎo)航是自主式水下機器人(Autonomous Underwater Vehicles ，簡稱AUV)在水下 自主航行時采用的主要定位手段，但是其主要問題在于精度能滿足要求時，往往價格過于昂 貴。隨著聲納技術(shù)的進步，目前較先進的AUV基本不采用加速度計而是采用多普勒測速儀,可 以直接測出AUV相對于海底的3D速度而無需積分。采用多普勒測速儀與光纖陀螺相結(jié)合的 導(dǎo)航方法，仍然不可避免的產(chǎn)生累積誤差，為消除這種偏差，目前主要采用的方法有(1)利 用GPS修正，即讓AUV在其路途中的一系列位置上浮，接受GPS信號以修正當前位置；(2) 聲學(xué)輔助導(dǎo)航，在作業(yè)區(qū)域布置長基線、短基線等；(3)地形輔助導(dǎo)航。但是上述方法均以 不同形式對AUV的工作范圍加以限制，其中GPS多次上浮修正偏差，能源消耗很大，限制了 工作時間和工作范圍；而聲學(xué)基線方法限制AUV只能在短距離內(nèi)有效；地形輔助導(dǎo)航利用地 形信息來定位和導(dǎo)航，對AUV而言，這要求事先必須有精確的海底地形地圖。由于海底地形 和陸地不同，因此獲得精確的海底地圖仍然是各國正在探索的問題，目前而言，只有在近海 的部分區(qū)域才具有比較精確的海底地形地貌數(shù)據(jù)，因此基于海圖的地形輔助導(dǎo)航的應(yīng)用也有 很大局限。
隨著AUV技術(shù)的不斷發(fā)展，其作為深海水下運載技術(shù)的重要執(zhí)行載體，將向更長程、更 廣闊工作海域發(fā)展。如果要求AUV在更廣闊的區(qū)域以更低廉的運行費用工作， 一個可行性思 路是盡量減少對母船和基陣等外部導(dǎo)航框架的依賴，使AUV實現(xiàn)自主導(dǎo)航定位。"同時定位 與地圖構(gòu)建方法" (Simultaneous Localization and mapping,簡稱SLAM方法)被認為是 機器人在未知環(huán)境中真正實現(xiàn)自主導(dǎo)航的關(guān)鍵。SLAM方法可描述為在未知環(huán)境下，移動機 器人從未知的起點開始，經(jīng)過一系列位置，利用機載傳感器信息構(gòu)建增量式環(huán)境地圖同時對 自身位置定位，該方法自1990年提出以來得到了快速發(fā)展。同時定位與地圖創(chuàng)建方法與地圖 表達形式密切相關(guān)，如柵格地圖、特征地圖、拓撲地圖等，基于環(huán)境特征的SLAM方法應(yīng)用最 為廣泛，尤其適用于沒有規(guī)則幾何形狀的戶外環(huán)境、障礙物等。它將機器人的位姿和環(huán)境特 征坐標表達在一個狀態(tài)向量之中，在機器人的運動過程中通過對環(huán)境特征的觀測作最優(yōu)準則 的估計。該方法分為三個步驟(l)基于外部感知的環(huán)境特征提取；(2)遞推形式的預(yù)測和更 新算法；(3)相應(yīng)的數(shù)據(jù)關(guān)聯(lián)技術(shù)。AUV作為機器人學(xué)科的一個重要分支， 一直以來都在不斷融入陸路機器人的最新技術(shù)和 研究熱點。目前國內(nèi)對AUV的研究僅局限于陸路機器人的導(dǎo)航定位方面，在AUV上采用SLAM 方法的研究很少。國際上，美國伍茲霍爾海洋研究所(冊O(shè)I)的研究者采用六自由度視覺系 統(tǒng)基于SLAM方法開展AUV探測的研究，對"泰坦尼克"沉船進行勘探，取得很好效果；悉尼 大學(xué)現(xiàn)場機器人研究中心用水下機器人進行珊瑚礁海底進行勘査，也取得初步效果。但是上 述基于SLAM方法的AUV導(dǎo)航定位范圍均很小， 一般僅在幾百米甚至幾十米以內(nèi)。

發(fā)明內(nèi)容
本發(fā)明的目的在于在更大范圍內(nèi)實現(xiàn)基于同時定位與地圖構(gòu)建方法的自主式水下機器人 自主導(dǎo)航，提出了一種采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其以掃描成像聲 納為主傳感器、以同時定位與地圖構(gòu)建為主要方法，能夠完成在較復(fù)雜的海底環(huán)境下的自主 導(dǎo)航，特別適用于深海未知的復(fù)雜環(huán)境，制造成本低，通過該發(fā)明的出海作業(yè)費用大大降低， 且整個裝置的可靠性好。
本發(fā)明是采用以下的技術(shù)方案實現(xiàn)的 一種采用同時定位與地圖構(gòu)建方法的自主式水下 機器人，包括主計算機子系統(tǒng)、數(shù)據(jù)采集子系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、艙內(nèi)參數(shù)檢測子系統(tǒng)、 推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)和電源子系統(tǒng)，其中所述的電源子系統(tǒng)包括主電源子 系統(tǒng)和備用電源子系統(tǒng)，主電源子系統(tǒng)提供機器人系統(tǒng)的電力需求，備用電源子系統(tǒng)獨立于 主電源子系統(tǒng)，提供機器人系統(tǒng)在自救狀態(tài)下的電力需求，所述的機器人還包括冗余自救子 系統(tǒng)和求救報警子系統(tǒng)，當主計算機子系統(tǒng)無法正常工作時冗余自救系統(tǒng)接手對機器人系統(tǒng)
的控制權(quán)，所述的數(shù)據(jù)采集子系統(tǒng)包括USB-CAN適配器，數(shù)據(jù)采集子系統(tǒng)向上與主計算機子 系統(tǒng)之間通過以太網(wǎng)通信，數(shù)據(jù)采集子系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、主電源子系統(tǒng)、艙內(nèi)參數(shù)檢 測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)、冗余自救子系統(tǒng)、備用電源子系統(tǒng)和求 救報警子系統(tǒng)分別通過CAN適配器接入C緒總線，數(shù)據(jù)采集子系統(tǒng)通過CAN總線與慣性導(dǎo)航 子系統(tǒng)、主電源子系統(tǒng)、艙內(nèi)參數(shù)檢測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)、冗 余自救子系統(tǒng)、備用電源子系統(tǒng)和求救報警子系統(tǒng)通信，所述的艙外傳感器子系統(tǒng)包括聲納 傳感器，其由安裝在外框架前方頂部的窄波束掃描成像聲納和在外框架前方底部的具有較寬 波束角的普通避碰聲納組成，其中窄波束掃描成像聲納用于探測周圍環(huán)境目標，普通避碰聲 納用于測量機器人距離水底的高度和探測前下方障礙物以緊急避碰。
本發(fā)明中，所述的主計算機子系統(tǒng)、數(shù)據(jù)采集子系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、電源子系統(tǒng)、 艙內(nèi)參數(shù)檢測子系統(tǒng)、冗余自救子系統(tǒng)和求救報警子系統(tǒng)設(shè)在壓力艙內(nèi)，所述的壓力艙為呈 上下設(shè)置的雙壓力艙結(jié)構(gòu)，壓力艙固定在外框架上，即本發(fā)明采用開架式機械結(jié)構(gòu)。
所述的慣性導(dǎo)航子系統(tǒng)中的傳感器包括光纖陀螺儀、加速度計和數(shù)字羅盤，上述傳感器采用中等精度儀器即可滿足要求，分別通過CAN適配器接入CAN總線，與數(shù)據(jù)采集子系統(tǒng)交 互數(shù)據(jù)。
所述的電源子系統(tǒng)由鋰電子動力電池組和電池監(jiān)測板組成，電池的容量由水下機器人的 續(xù)航時間以及推進器功率等因素確定，其中電池監(jiān)測板通過CAN適配器接入CAN總線，所述 的備用電源子系統(tǒng)通過CAN總線與冗余自救子系統(tǒng)通信。
所述的艙內(nèi)檢測子系統(tǒng)用于時刻檢測壓力艙內(nèi)的各種狀態(tài)參數(shù)等，如溫度、濕度、電源 電壓、電源電流、艙密封狀態(tài)，其中溫度、濕度和艙密封狀態(tài)采用多點檢測，該子系統(tǒng)由艙 內(nèi)參數(shù)檢測板和前端檢測電路組成，其中艙內(nèi)參數(shù)檢測板通過CAN適配器接入CAN總線。
所述的推進器控制子系統(tǒng)包括五個水下推進器和分別控制各推進器的驅(qū)動控制板，可實 現(xiàn)五自由度運動，其中沿壓力艙的垂直方向設(shè)有三個推進器，水平方向設(shè)有兩個推進器，垂 直方向推進器分別固定于外框架前部的對稱兩側(cè)和外框架的尾部，水平方向推進器分別固定 于外框架中部的對稱兩側(cè)，所述的驅(qū)動控制板通過CAN適配器接入CAN總線。
所述的艙外傳感器子系統(tǒng)還包括壓力傳感器和GPS天線，壓力傳感器固定于框架的尾部 且垂直方向推進器的上方，用于測量水壓從而計算該處距離水面的高度，GPS天線安裝在外 框架的后部上方，其分別通過CAN適配器接入CAN總線。
所述的冗余自救子系統(tǒng)為雙冗余自救子系統(tǒng)，其中第一套冗余自救子系統(tǒng)由數(shù)據(jù)采集子 系統(tǒng)兼任，在主計算機子系統(tǒng)不能正常工作時接手控制，第二套冗余自救子系統(tǒng)為完全獨立 的嵌入式單板控制器，可以采用ARM嵌入式控制板，專門用于自救處理，在主計算機子系統(tǒng) 和數(shù)據(jù)采集子系統(tǒng)都不能正常工作時接手控制，其與主計算機子系統(tǒng)和數(shù)據(jù)采集子系統(tǒng)之間 通過RS232接口通信，并通過CAN適配器接入CAN總線與其它子系統(tǒng)通信。
所述的求救報警子系統(tǒng)在冗余自救子系統(tǒng)的控制下工作，包括聲學(xué)求救報警系統(tǒng)和無線 求救報警系統(tǒng)，即當水下機器人出現(xiàn)故障時，可以通過兩種方式發(fā)出求救報警，該子系統(tǒng)通 過CAN總線與冗余自救子系統(tǒng)交互數(shù)據(jù)。
本發(fā)明還包括一種采用同時定位與地圖構(gòu)建方法的自主式水下機器人的聲納信號特征提 取方法，該方法包括以下步驟-
(1) 在聲納每個Ping的所有Bin中找出主回波，定義為點特征。
(2) 如果回波中較高強度的Bin連成片，形成了點簇，則將聚集在一定范圍內(nèi)，比如0.5米 的點簇用一個點特征表達。
(3) —周期的掃描結(jié)束后，對于距水下機器人相同距離上存在的連續(xù)點特征進行合并處理。 本發(fā)明的有益效果是該自主式水下機器人導(dǎo)航方法與傳統(tǒng)方法利用各種外部輔助導(dǎo)航
手段不同，其不使用聲學(xué)基陣如長基線LBL、短基線SBL、超短基線USBL等，也不使用昂貴
6的高精度慣性導(dǎo)航系統(tǒng)，而主要依賴主動聲納傳感器配合中等精度的慣性導(dǎo)航傳感器，并采 用同時定位與地圖構(gòu)建方法來完成自主導(dǎo)航定位，因此(1)使用和操作的費用低傳統(tǒng)方法 每次出海都需要功能齊全的母船，勢必會造成出?；ㄙM巨大，而本發(fā)明脫離了外部導(dǎo)航支持， 比如母船支持和布放聲學(xué)基陣，因此使得出海作業(yè)費用大大降低；(2)制造成本低該裝置 采用了具有較好成像能力的主動掃描聲納作為主要的環(huán)境探測傳感器，且配合使用中等精度 的慣性導(dǎo)航傳感器即可滿足要求，無需采用昂貴的高精度慣性系統(tǒng)，因此大大降低了制造成 本；(3)可工作的區(qū)域更廣闊由于本發(fā)明采用同時定位與地圖構(gòu)建方法的導(dǎo)航方法，減小 了誤差積累，因此在相同導(dǎo)航誤差要求下可航行更遠，可以實現(xiàn)10公里左右的自主導(dǎo)航；(5) 由于本發(fā)明采用兩套冗余自救子系統(tǒng)，當機器人發(fā)生意外或故障時起作用，保障機器人安全 擺脫險境并浮出水面，通過聲學(xué)方式或無線方式報警，因此整個機器人系統(tǒng)具有很好的可靠 性；(4)未知性和復(fù)雜性是深海的特點，而本發(fā)明原理上是利用障礙物目標之間的統(tǒng)計相關(guān) 性消除或減小航位計算過程中產(chǎn)生的誤差積累，并且在深海環(huán)境下聲納的成像效果更優(yōu)，因 此本發(fā)明非常適用于深海環(huán)境。


圖1為本發(fā)明的機械結(jié)構(gòu)示意圖2為本發(fā)明的系統(tǒng)結(jié)構(gòu)示意圖3為本發(fā)明的電控系統(tǒng)結(jié)構(gòu)圖4為本發(fā)明的同時定位與地圖構(gòu)建方法處理流程示意圖； 圖5為水平方向控制器工作示意圖； 圖6為垂直方向控制器工作示意圖； 圖7為本發(fā)明工作原理圖。
具體實施例方式
如1所示，本發(fā)明采用開架式機械結(jié)構(gòu)，外框架1上固定上下雙層壓力艙2,并設(shè)置五 個水下推進器，包括三個垂直方向推進器5和兩個水平方向推進器6，其中在外框架1前部 的左右對稱兩側(cè)分別設(shè)有垂直方向推進器5，同時在外框架1的前部上方設(shè)有窄波束掃描成 像聲納3，前部下方設(shè)有具有較寬波束的普通避碰聲納4;外框架1中部的左右對稱兩側(cè)分別 固定水平方向推進器6;外框架l的后部上方設(shè)有GPS天線8，其后部中間位置設(shè)有一個垂直 方向推進器5，該推進器的上方安裝有壓力傳感器7。
如圖2所示，本發(fā)明的自主式水下機器人系統(tǒng)主要由艙內(nèi)主計算機子系統(tǒng)、數(shù)據(jù)采集子 系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、主電源子系統(tǒng)、艙內(nèi)參數(shù)監(jiān)測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳 感器子系統(tǒng)、冗余自救子系統(tǒng)、備用電源子系統(tǒng)和求救報警子系統(tǒng)組成，以下結(jié)合圖2和圖3對各個子系統(tǒng)模塊的功能和工作原理進行詳細說明。
1、 主計算機子系統(tǒng)
主計算機子系統(tǒng)是水下機器人的計算平臺和上層決策中心，可以采用一臺高性能工控機， 比如配置IEI公司四核CPU—Q6600主板、8G內(nèi)存和SATA2高速硬盤。主計算機擔負機器人 的絕大部分計算任務(wù)，包括傳感器信號處理、姿態(tài)估計與導(dǎo)航計算、控制計算、任務(wù)決策、 路徑規(guī)劃等計算。它發(fā)送指令給數(shù)據(jù)采集子系統(tǒng)，并從數(shù)據(jù)采集子系統(tǒng)獲取各種傳感器數(shù)據(jù) 和底層參數(shù)數(shù)據(jù)。另外，它和第二套冗余自救子系統(tǒng)也有數(shù)據(jù)交互，以相互確認工作狀態(tài)。
2、 數(shù)據(jù)采集子系統(tǒng)
數(shù)據(jù)采集子系統(tǒng)負責采集來自各個子系統(tǒng)的數(shù)據(jù)，如慣性導(dǎo)航子系統(tǒng)、電源子系統(tǒng)、艙 內(nèi)參數(shù)監(jiān)測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)，并和第二套冗余自救子系統(tǒng)有 數(shù)據(jù)交互，以相互確認工作狀態(tài)。數(shù)據(jù)采集子系統(tǒng)由PC104工控機和USB-CAN適配器組成。 數(shù)據(jù)采集子系統(tǒng)向上與主計算機子系統(tǒng)通過以太網(wǎng)通信，接收主計算機子系統(tǒng)的指令數(shù)據(jù)， 并將采集處理的各類數(shù)據(jù)發(fā)送給主計算機，高速網(wǎng)絡(luò)通信方式能夠確保大容量數(shù)據(jù)的可靠傳 輸，向下通過CAN總線與其它子系統(tǒng)通信。另外，數(shù)據(jù)采集子系統(tǒng)還作為第套冗余自救了 系統(tǒng)，當主計算機子系統(tǒng)不能正常工作時取得整個系統(tǒng)的控制權(quán)。
3、 慣性導(dǎo)航子系統(tǒng)
該子系統(tǒng)中的慣性導(dǎo)航傳感器包括光纖陀螺儀、加速度計，并配合一個二軸數(shù)字羅盤。 陀螺儀采用中等精度的產(chǎn)品，比如俄羅斯的VG951D，其作用是獲得水下機器人的偏航角速率， 數(shù)字量輸出并積分后即可得到水下機器人的方向角。加速度計也采用中等精度的產(chǎn)品，比如 Jewell的LCF2350，分辨率luG。數(shù)字羅盤可選用HMR3000，角度精度可達O. 5度，器件自 帶的校正功能使得該數(shù)字羅盤在距離推進器較近的情況下也可以可靠地工作。慣性導(dǎo)航子系 統(tǒng)中的上述各個傳感器通過CAN適配器接入CAN總線，和數(shù)據(jù)采集子系統(tǒng)交互數(shù)據(jù)。
4、 主電源子系統(tǒng)
主電源由鋰離子動力電池組和電池監(jiān)測板組成，提供機器人系統(tǒng)的電力需求，電池的容 量與水下機器人的續(xù)航時間以及推進器功率等因素密切相關(guān)，可按如卜公式推算
電池容量=(推進器總功率乂70% +工控機功率+高功率傳感器的功率)X續(xù)航時間。 電池監(jiān)測板通過CAN適配器接入CAN總線，和數(shù)據(jù)采集子系統(tǒng)交互數(shù)據(jù)。
5、 艙內(nèi)參數(shù)監(jiān)測子系統(tǒng)
要保證水下機器人的正常工作，必須時刻監(jiān)測壓力艙內(nèi)的各種狀態(tài)參數(shù)包括溫度、濕度、 電源電壓、電源電流、艙密封狀態(tài)等。其中溫度、濕度和艙密封監(jiān)測采用多點檢測。艙內(nèi)參 數(shù)監(jiān)測子系統(tǒng)通過CAN適配器接入CAN總線，和數(shù)據(jù)采集子系統(tǒng)交互數(shù)據(jù)。6、 推進器控制子系統(tǒng)
本發(fā)明采用五推進器方式，垂直方向有三個推進器，水平方向有兩個推進器，可實現(xiàn)除 水平移動外的五自由度運動。推進器采用分布式控制方案，每個推進器分別對應(yīng)地由一個驅(qū) 動控制板來控制，該控制板以單片機為核心，五個控制板分別通過CAN適配器接入CAN總線， 和數(shù)據(jù)采集子系統(tǒng)交互數(shù)據(jù)。推進器由直流永磁電動機或直流無刷電動機配合多葉片螺旋槳 組成。
7、 艙外傳感器子系統(tǒng)
本發(fā)明除了在艙內(nèi)安裝慣性導(dǎo)航傳感器之外，還在艙外安裝聲納傳感器、壓力傳感器和 GPS天線。其中聲納傳感器有兩個 一個安裝在機器人的前方頂部，是窄波束的掃描成像聲 納，用于探測周圍環(huán)境目標；另外一個安裝在前方底部，是避碰聲納即普通的掃描成像聲納， 其具有較寬的波束角，用于測量機器人距離水底的高度和探測前下方障礙物以緊急避碰。壓 力傳感器用來測量水壓從而換算出機器人距離水面的距離。由于工作在水下環(huán)境，對艙外傳 感器的水密性和耐壓性的要求較高，因此聲納和壓力傳感器本身需要足夠的水密設(shè)計，目前 市場上出售的GPS天線一般都不具有水密性，需要單獨做水密處理，本發(fā)明選擇聚四氟乙烯 材料的密封殼體封閉GPS天線，具有較好的水密性，而且基本不影響GSP在水面接收無線信 號。GPS天線和聲納傳感器通過CAN適配器接入CAN總線，壓力傳感器連接AD采集卡并通過 CAN適配器接入CAN總線。
8、 冗余自救子系統(tǒng)
盡管機器人系統(tǒng)的設(shè)計相對比較可靠，但是不能保證其在工作過程中完全不會出現(xiàn)故障， 尤其是當電池和上層計算機系統(tǒng)出現(xiàn)問題后，將導(dǎo)致水下機器人完全失控，因此冗余自救子 系統(tǒng)用于在主計算機子系統(tǒng)或數(shù)據(jù)采集子系統(tǒng)無法正常工作時接手整個系統(tǒng)的控制權(quán)。冗余 自救子系統(tǒng)有兩個，構(gòu)成雙冗佘自救子系統(tǒng)。其中第一套冗余自救子系統(tǒng)由數(shù)據(jù)采集子系統(tǒng) 兼任，在主計算機不能正常工作時接手控制，第二套冗余自救子系統(tǒng)是完全獨立的一套嵌入 式單板控制器，可以采用ARM嵌入式控制板，其專門用于自救處理，當主計算機子系統(tǒng)和數(shù) 據(jù)采集子系統(tǒng)都不能正常工作時，第二套冗余自救子系統(tǒng)接手控制。
9、 備用電源子系統(tǒng)
備用電源獨立于主電源子系統(tǒng)，由鋰離子動力電池組和電池監(jiān)測板組成，提供給機器人 系統(tǒng)在自救狀態(tài)下的電力需求。 一般備用電池組的容量比主電源電池組容量小得多，設(shè)計時 一般考慮至少能夠使機器人浮上海面所需要的能量。備用電源子系統(tǒng)的電池監(jiān)測板通過CAN 適配器接入CAN總線與冗余自救子系統(tǒng)交互數(shù)據(jù)。
10、 求救報警子系統(tǒng)
9在水下機器人出現(xiàn)故障時，求救報警子系統(tǒng)在冗余自救子系統(tǒng)的控制下工作，通過兩種 方式發(fā)出求救報警， 一種是在水下通過聲學(xué)Modem方式，另一種是浮出水面后通過無線通信 方式。求救報警子系統(tǒng)通過CAN適配器接入CAN總線，與第一套冗余自救子系統(tǒng)即數(shù)據(jù)采集 子系統(tǒng)以及第二套冗余自救子系統(tǒng)交互數(shù)據(jù)。
本發(fā)明的水下機器人采用同時定位與地圖構(gòu)建方法即SLAM方法進行導(dǎo)航定位，從而實 現(xiàn)自主航行。該方法的工作原理描述如下利用該方法進行導(dǎo)航定位的過程中涉及到兩個坐 標系，即靜坐標系和動坐標系。靜坐標系即絕對坐標系，動坐標系是以當前載體的位置為中 心，以當前航向方向為X軸。SLAM方法的執(zhí)行過程大體可以分為三個階段預(yù)測，觀測和更 新。首先將載體姿態(tài)和地圖特征存儲在一個獨立的狀態(tài)向量中，然后通過先預(yù)測再觀測的迭 代遞推過程來估計系統(tǒng)狀態(tài)，實現(xiàn)對載體的定位和特征地圖的創(chuàng)建。預(yù)測通常是利用自身攜 帶的陀螺儀等儀器對當前時刻載體的姿態(tài)進行航位推算，由于傳統(tǒng)航位推算的精度一般均很 低，所以在預(yù)測階段載體姿態(tài)的估計誤差會有所增加。在觀測階段，利用聲納感知周圍環(huán)境， 對載體周圍環(huán)境中的特征進行再次觀測，得到載體坐標系下表示特征與載體相對位置的量測 值，再利用載體當前的估計位置將地圖中的特征與量測值轉(zhuǎn)換到同一坐標系下，通過數(shù)據(jù)關(guān) 聯(lián)過程確定量測與特征的一一對應(yīng)關(guān)系，將兩者的位置偏差作為觀測值，經(jīng)過擴展卡爾曼濾 波處理后，可以同時改善載體和特征的狀態(tài)估計。本發(fā)明的水下機器人同時定位與地圖構(gòu)建 處理算法的流程如圖4所示。
通過對周圍環(huán)境感知來進行基于SLAM的自主導(dǎo)航主要依賴于安裝在AUV上部的掃描成像 聲納。而對于地圖在線構(gòu)建和AUV定位方法而言，只有環(huán)境特征是唯一可用的，因此特征的 提取是至關(guān)重要的一環(huán)。在特征提取之前先要進行聲納回波信號的濾波，其中閾值的選擇對 目標檢測有重要的影響，如果閾值選擇過低，輸出背景信號比重加大，不穩(wěn)定的潛目標較多， 目標信號不明顯；選擇過高可能會保留過多的噪聲，并且丟失部分目標信息。水聽設(shè)備可以 接收到強度在0dB 80dB的聲納回波信號， 一般把最高采樣分貝T設(shè)定在50dB 60dB之間， 則采樣范圍為
，既可以很好地保留目標信息也可以去除部分高強度噪聲的干擾。濾波之 后即可從聲納數(shù)據(jù)中提取特征，提取特征主要采取如下三個步驟
(1) 在聲納每個Ping中的所有Bin中找出主回波，把它定義為點特征。主回波是那些相對 Ping中強度幅值的平均值而言明顯較高的回波Bin，該Bin對應(yīng)的距離值代表了聲納和目標 之間的距離；
(2) 如果回波中較高強度的Bin連成片，形成了點簇，則將聚集在一定距離，如0.5米之內(nèi) 的點簇用一個點特征表達。這樣，那些由海底成片礁石形成的成片點簇就被合并標記成等間 距的若干點特征，這些點特征間距為0. 5米；(3) —周期的掃描結(jié)束后，對于距水下機器人相同距離，即相同編號的Bin上存在的連續(xù)點
特征進行合并處理，將0.5米內(nèi)的所有點特征合并為一個點特征，但是由上述主回波形成的
點特征不參與合并處理。
按上述方法獲得點特征后，很容易可知每個點特征的方位角和距離。穩(wěn)定的點特征都將
被加入到特征地圖中。上述步驟(2)和(3)中都有合并處理，這主要是考慮到海底復(fù)雜環(huán)境很
容易使得聲納數(shù)據(jù)處理產(chǎn)生過于密集的點特征，不利于水下機器人導(dǎo)航定位算法的實時在線
計算。上述步驟中以縱向和橫向兩種方式進行合并處理，類似于一種針對密集點特征的低通
濾波效果，并且由于我們規(guī)定主回波形成的點特征不參與此濾波，因此既能形成對大片礁巖
穩(wěn)定而有規(guī)律的點特征標記，又不會丟失海底中明顯突起礁石的可靠的點特征標記。
本發(fā)明的控制系統(tǒng)采用了兩層控制架構(gòu)，即底層的推進器驅(qū)動控制和高層的控制行為決
策系統(tǒng)。底層控制系統(tǒng)采用解耦方式將水下機器人的運動分解為水平方向和垂直方向，因此
通過如圖5所示的水平方向控制器和如圖6所示的垂直方向控制器分別完成水平方向控制和
垂直方向控制，其中PID參數(shù)以及其它比例系數(shù)根據(jù)實際工作情況進行整定，高層決策系統(tǒng)
的輸出量成為底層驅(qū)動系統(tǒng)的輸入期望值，其采用的行為決策結(jié)構(gòu)如圖7中所示。綜上所述，
本發(fā)明中的水下機器人實現(xiàn)自主導(dǎo)航的工作過程如下所示首先，航行過程中底層數(shù)據(jù)采集
系統(tǒng)對所述的未知海底環(huán)境進行數(shù)據(jù)采集，這個過程由慣性導(dǎo)航子系統(tǒng)和艙外傳感器子系統(tǒng)
的傳感器完成，其中加速度計、光纖陀螺配合數(shù)字羅盤在SLAM方法的預(yù)測階段中對當前時刻
載體的姿態(tài)進行航行推算，其中光纖陀螺獲得水下機器人的偏航角速率，數(shù)字羅盤輸出機器
人三軸姿態(tài)角；窄波束掃描成像聲納用于探測環(huán)境目標，可以較精確地獲得周圍環(huán)境中的目
標反射波，成像聲納的觀測主要用于SLAM方法的觀測階段，對水下機器人周圍環(huán)境中的特征
進行觀測進而通過特征提取獲得目標的距離和方位角；普通避碰聲納用于探測前方障礙和測
量機器人與海底之間的距離；壓力傳感器用于測量機器人所述位置與海平面之間的距離。上
述傳感器的原始數(shù)據(jù)經(jīng)數(shù)據(jù)采集子系統(tǒng)采集并將相關(guān)數(shù)據(jù)傳輸給主計算機子系統(tǒng)。主計算機
子系統(tǒng)通過對SLAM處理算法的計算可獲得水下機器人姿態(tài)最優(yōu)估計以及環(huán)境特征地圖的最
優(yōu)估計，這些信息和前述已獲得的距離/方位信息、障礙物距離信息、海拔信息和深度信息分
別輸入到預(yù)先定義的各種行為模式中，行為模式是指水下機器人在一定輸入條件下經(jīng)過預(yù)先
設(shè)計的行為計算方法輸出機器人下一時刻的期望執(zhí)行量，比如偏航角、前移量或深度。本發(fā)
明中共定義了七種行為模式，分別是設(shè)定路線航行行為、區(qū)域探測行為、避障行為、設(shè)定海
拔航行行為、最小海拔航行行為、設(shè)定深度航行行為和最小深度航行行為。各個行為模式之
間是相互獨立的，因此都會產(chǎn)生各自的期望執(zhí)行量，這些執(zhí)行量被直接送往偏航角/航速模糊
邏輯決策器和深度模糊邏輯決策器中，偏航角/航速模糊邏輯決策器和深度模糊邏輯決策器在工作時也同時接收任務(wù)規(guī)劃與路徑規(guī)劃模塊的指令，它們采用一套事先設(shè)計整定好的模糊決 策算法來最終確定下一時刻的期望執(zhí)行量，即期望的偏航角/航速和期望的深度，并發(fā)送到底 層的推進器控制子系統(tǒng)。推進器控制子系統(tǒng)利用陀螺儀數(shù)據(jù)和壓力傳感器數(shù)據(jù)并使用了兩個 解耦的PTD控制器，圖5和圖6只是給出了解耦的控制器方案，實際工作中還需要根據(jù)水下 機器人的機械動力學(xué)模型計算出兩個水平推進器和三個垂直推進器各自的推力分配值。最后， 由PID的輸出控制功率驅(qū)動電路從而驅(qū)動水平方向推進器和垂直方向推進器。
本發(fā)明實現(xiàn)了 一種以掃描成像聲納為主傳感器、以同時定位與地圖構(gòu)建方法為主要導(dǎo)航 定位方法的自主式水下機器人，該機器人的技術(shù)指標是
外形尺寸應(yīng)按具體使用要求，特別是搭載負荷要求，比如外框架可以是長1.5米、寬 1. 2米、高1. 1米，由304不銹鋼管材制成；壓力艙直徑0. 3米，長1米，由硬鋁合金材料 制成。
工作水深不限，主要取決于壓力艙的材質(zhì)、結(jié)構(gòu)和水密能力，以及外部傳感器的水密
指標等。
航速對于開架式的水下機器人，航速一般最高4節(jié)； 水平推力為匹配上述外形和結(jié)構(gòu)，30 40公斤力； 垂直推力25公斤力；
續(xù)航能力配4000Wh電池， 一般可續(xù)航5 8小時；
AUV導(dǎo)航定位精度在比較復(fù)雜的海底環(huán)境屮以巡航10公里計，定位誤差可小于100米。
在已知起始點絕對坐標的情況下，該機器人能夠完全借助聲納與其它機載傳感器在較復(fù)
雜海底環(huán)境下進行自主導(dǎo)航，巡航過程中不上浮，完全采用無纜方式；能夠?qū)崿F(xiàn)特征地圖閉
環(huán)，即在環(huán)航返回到出發(fā)點時能夠識別出起始點海域。
本發(fā)明通過搭載多種傳感器和作業(yè)設(shè)備，可以開展在江河湖泊、水電大壩、海洋等水下 環(huán)境中的監(jiān)測任務(wù)以及維護、維修等工作。
權(quán)利要求
1、一種采用同時定位與地圖構(gòu)建方法的自主式水下機器人，包括主計算機子系統(tǒng)、數(shù)據(jù)采集子系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、艙內(nèi)參數(shù)檢測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)和電源子系統(tǒng)，其特征在于所述的電源子系統(tǒng)包括主電源子系統(tǒng)和備用電源子系統(tǒng)，所述的機器人還包括冗余自救子系統(tǒng)和求救報警子系統(tǒng)，所述的數(shù)據(jù)采集子系統(tǒng)與主計算機子系統(tǒng)之間通過以太網(wǎng)通信，數(shù)據(jù)采集子系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、主電源子系統(tǒng)、艙內(nèi)參數(shù)檢測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)、冗余自救子系統(tǒng)、備用電源子系統(tǒng)和求救報警子系統(tǒng)分別通過CAN適配器接入CAN總線，數(shù)據(jù)采集子系統(tǒng)通過CAN總線與慣性導(dǎo)航子系統(tǒng)、主電源子系統(tǒng)、艙內(nèi)參數(shù)檢測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)、冗余自救子系統(tǒng)、備用電源子系統(tǒng)和求救報警子系統(tǒng)通信，所述的艙外傳感器子系統(tǒng)包括聲納傳感器，其由安裝在外框架前方頂部的窄波束掃描成像聲納和安裝在外框架前方底部的普通避碰聲納組成。
2、 根據(jù)權(quán)利要求1所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在于所述的主計算機子系統(tǒng)、數(shù)據(jù)采集子系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、電源子系統(tǒng)、艙內(nèi)參數(shù)檢測子系統(tǒng)、冗余自救子系統(tǒng)和求救報警子系統(tǒng)設(shè)在壓力艙內(nèi)，所述的壓力艙為呈上下設(shè)置的 雙壓力艙，其固定在外框架上。
3、 根據(jù)權(quán)利要求l所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在 于所述的慣性導(dǎo)航子系統(tǒng)中的傳感器包括光纖陀螺儀、加速度計和數(shù)字羅盤，其分別通過 CAN適配器接入CAN總線，與數(shù)據(jù)采集子系統(tǒng)通信。
4、 根據(jù)權(quán)利要求l所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在 于所述的電源子系統(tǒng)由鋰離子動力電池組和電池監(jiān)測板組成，其中電池監(jiān)測板通過CAN適 配器接入CAN總線，所述的備用電源子系統(tǒng)通過CAN總線與冗余自救子系統(tǒng)通信。
5、 根據(jù)權(quán)利要求l所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在 于所述的艙內(nèi)檢測子系統(tǒng)由艙內(nèi)參數(shù)檢測板和檢測電路組成，其中艙內(nèi)參數(shù)檢測板通過CAN 適配器接入CAN總線。
6、 根據(jù)權(quán)利要求l所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在 于所述的推進器控制子系統(tǒng)包括五個水下推進器和分別控制各推進器的驅(qū)動控制板，其中 沿壓力艙的垂直方向設(shè)有三個推進器，水平方向設(shè)有兩個推進器，垂直方向推進器分別固定 于外框架前部的對稱兩側(cè)和外框架的尾部，水平方向推進器分別固定于外框架中部的對稱兩 側(cè)，所述的驅(qū)動控制板通過CAN適配器接入CAN總線。
7、 根據(jù)權(quán)利要求l所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在于所述的艙外傳感器子系統(tǒng)還包括壓力傳感器和GPS天線，壓力傳感器固定于框架的尾部 且垂直方向推進器的上方，GPS天線安裝在外框架的后部上方，其分別通過CAN適配器接入 CAN總線。
8、 根據(jù)權(quán)利要求l所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在于所述的冗余自救子系統(tǒng)為雙冗余自救子系統(tǒng)，其中第一套冗余自救子系統(tǒng)即為數(shù)據(jù)采集子系統(tǒng)，第二套冗余自救子系統(tǒng)與主計算機子系統(tǒng)和數(shù)據(jù)采集子系統(tǒng)之間通過RS232接口通 信，并通過CAN適配器接入CAN總線與其它子系統(tǒng)通信。
9、 根據(jù)權(quán)利要求l所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人，其特征在 于所述的求救報警子系統(tǒng)由聲學(xué)求救報警系統(tǒng)和無線求救報警系統(tǒng)組成，該子系統(tǒng)通過CAN 適配器接入CAN總線與冗余自救子系統(tǒng)通信。
10、 一種采用權(quán)利要求1所述的采用同時定位與地圖構(gòu)建方法的自主式水下機器人的聲 納信號特征提取方法，其特征在于該方法包括以下步驟(1) 在聲納每個Ping的所有Bin中找出主回波，定義為點特征。(2) 如果回波中較高強度的Bin連成片，形成了點簇，則將聚集在一定范圍內(nèi)的點簇用一個 點特征表達。(3) —周期的掃描結(jié)束后，對于距水下機器人相同距離上存在的連續(xù)點特征進行合并處理。
全文摘要
本發(fā)明屬于一種水下航行與運載工具，具體地說是一種采用同時定位與地圖構(gòu)建方法的自主式水下機器人。其以掃描成像聲納為主傳感器、以同時定位與地圖構(gòu)建為主要方法，能夠完成在較復(fù)雜的海底環(huán)境下的自主導(dǎo)航，本發(fā)明包括主計算機子系統(tǒng)、數(shù)據(jù)采集子系統(tǒng)、電源子系統(tǒng)、慣性導(dǎo)航子系統(tǒng)、艙內(nèi)參數(shù)檢測子系統(tǒng)、推進器控制子系統(tǒng)、艙外傳感器子系統(tǒng)、冗余自救子系統(tǒng)和求救報警子系統(tǒng)，其中所述的電源子系統(tǒng)包括主電源子系統(tǒng)和備用電源子系統(tǒng)，數(shù)據(jù)采集子系統(tǒng)向上通過以太網(wǎng)與主計算機子系統(tǒng)通信，向下通過CAN總線與其它子系統(tǒng)通信。其特別適用于深海未知的復(fù)雜環(huán)境，制造成本和出海作業(yè)費用低，且可靠性好。
文檔編號B63G8/00GK101436074SQ20081023786
公開日2009年5月20日 申請日期2008年12月6日 優(yōu)先權(quán)日2008年12月6日
發(fā)明者波 何, 劉貴杰, 崔國霖, 王國宇, 程軍娜 申請人:中國海洋大學(xué)