專利名稱：水下機器人的多線地形匹配導航方法
技術領域：
本發明涉及的是ー種導航方法，具體地說是水下導航方法。
背景技術：
由于水下機器人具有廣泛的應用前景，世界各國高度重視水下機器人的研究并取得了長足的發展。在水下機器人的研究中，水下機器人是否能實現精確的導航定位是其能否順利完成任務并安全返航的關鍵。常用于水下機器人的水下導航定位方法包括水聲學導航、航位推算導航、慣性導航等。水聲學導航需要布置聲學基陣，不利于水下機器人的獨立作業任務，且導航范圍受限。航位推算導航和慣性導航系統在短期內精度較高，但其定位誤差隨時間積累，長時間水下航行會產生較大的偏差，需要定期上浮接收GPS進行修正。水下機器人頻繁上浮不利于長時間深水作業，且海上風浪較大時對水下機器人相對較小的載體也是ー個巨大的威脅。
發明內容
本發明的目的在于提供ー種不使用GPS等外部傳感器的水下機器人的多線地形匹配導航方法。本發明的目的是這樣實現的:本發明水下機器人的多線地形匹配導航方法，其特征是:( 1)在t=t0初始時刻，獲取水下機器人的初始位置(X0, y0)；(2)采集水下機器人上的光纖羅經的初始方向角α0；(3)在t1=t0+Δt時刻，采集水下機器人上的深度計和多普勒聲納速度計數據，獲得當前位置水下機器人的速度V1和四條波束的返回時間Tn(n=l，2，3，4)，從而得到四條波束的長度(I11, I21, I31, I41)，結合多普勒聲納速度計的開角大小和深度計得到當前機器人正下方的水深值，從而得到四個深度值(hn，h21, h31, h41)；(4)在t2=tQ+2 Δ t,......，tn=t0+n Δ t時刻,分別獲取多普勒聲納速度計四條波束
所對應時刻的四個水深數據，結合步驟(3)中得到的四個深度值(hn，h21，h31，h41)，每條波束可得到一條線地形深度數據序列，四條波束共可得到四條線地形數據(hn，h12，……，hln)，(h21，h22，......，，(h41，h42，......，h4n)；(5)利用多普勒聲納速度計輸出的相應時刻水下機器人速度(V1, V2,……，vn)和光纖羅經輸出的水下機器人的首向角(a 1, a2,……，a n)，對其積分可以得到水下機器人所處的大致位置坐標序列((Xl，Y1) (x2, y2)……(xn，yn))，計算位置誤差值的方差σ;(6)由步驟(5)中水下機器人所處的大致位置處利用6σ原則在海圖中選取匹配區域，并提取匹配區域的水深值數據；(7)利用MSD算子對獲取的四組線地形水深數據和匹配區域中的水深值數據進行匹配運算，得到四組線地形(hkl，hk2，……，hkn)，k=l，2，3，4對應位置的精確坐標序列((xkl，yki) (xk2, yk2)......(xkn，ykn )；
(8)分別對(xkl, ykl)、(xk2, yk2)......(Xkn, ykn) (k=l, 2，3，4)中四個坐標值進行融合
計算，得到(tfVH A t, t2=t0+2 A t,......, tn=t0+n A t)時刻水下機器人所處的精確位置序列
((X1, Y1) (X2, Y2)……(Xn, Yn) )，(Xn, Yn)為當前時刻水下機器人所處位置的精確位置坐標；(9)輸出當前水下機器人所處的精確位置坐標到水下機器人主控制計算機；(10)在 tn+1=tn+ A t 時亥丨J，令 hkl=hk2, hk2=hk3,......，hn)=、，k=l, 2, 3, 4 ； (X1, Y1)
= (x2，y2), (x2, j2) = (x3, y3),......, (Xn-Dylri) = (XnJn)；(11)采集計算得到tn+1時刻的深度值數據和水下機器人的大致位置坐標并賦值給hkn (k=l，2，3，4)和(xn，yn)，得到四條更新的四條線地形數據和匹配區域數據，重復步驟
(7) (9)，實現對水下機器人位置坐標連續不間斷的精確導航定位；(12)在水下機器人完成預定任務或者到達指定區域后，完成導航。本發明還可以包括:1、所述的四條波束的長度(ln，I21, I31, I41)由海水中聲速傳播的經驗公式分別對(0, Tn)積分計算得到。本發明的優勢在于:本發明可有效地適應長航程水下機器人在水下長時間可靠的精確導航的要求，無需采用昂貴的慣性導航裝置，可避免水下機器人頻繁上浮修正導航數據所帶來的種種弊端，保證水下機器人導航的精確度。
圖1為水下機器人的多線地形匹配導航裝置結構示意圖；圖2地形匹配計算機的密封結構示意圖；圖3為本發明的流程圖。
具體實施例方式下面結合附圖舉例對本發明做更詳細地描述:結合圖1 3，水下機器人的多線地形匹配導航裝置的組成包括多普勒速度計1、水壓深度計2、PC/104計算機3、PC/104總線AD采集卡4、PC/104總線串ロ板5、光纖羅經
6、主控計算機7。同時結合圖2，PC/104計算機3、PC/104總線AD采集卡4和PC/104總線串ロ板5通過總線插接，與光纖羅經6 —起密封于PC/104計算機水密外殼8中，PC/104計算機水密外殼上設置三個水密插頭，ー個水密插頭9將PC/104總線AD采集卡4和深度計連接，第二個水密插頭10將PC/104總線串ロ板5和多普勒速度計連接，第三個水密插頭11引出連線三根，將PC/104計算機3與控制艙主控計算機7通過RS-232串行通信聯結。將多線地形匹配系統與主控計算機聯接。多線地形匹配系統上電，程序自啟動。啟動3個任務。任務1:匹配程序初始化，多普勒速度計、光纖羅經和深度計初始化，處于待機狀態。任務2:建立串口數據檢測線程，與主控計算機形成通信。任務3:任務3為主任務，機制為自動應答，依照預先程序設定的控制節拍，啟動多普勒速度計和深度計采集數據，同時采集光纖羅經信息，由光纖羅經數據和多普勒速度計數據進行航位計算得到水下機器人的大致位置，由深度計和多普勒速度計測得的數據計算水下機器人周圍的四個深度值。航行過程中，采集點位形成四條線地形。通過由航位推算的結果確定海圖的匹配范圍，利用概率相關的方法確定水下機器人周圍的四條線地形的修正值，并將其進行融合得到精確位置。將計算出位置結果，傳輸給主控計算機，完成ー個指令循環。在完成第一次匹配后，以后僅需要用最新的數據替代最老的數據進行匹配，即可實現水下機器人長時精確的水下地形匹配導航。本發明的方法流程如下，如圖3:一、初始化:1、在初始時刻t=、時刻，由水下機器人控制艙主控制計算機或人工輸入水下機器人的初始位置(Xtl, y0)。2、采集光纖羅經的初始方向角a。。ニ、初次匹配3、在WAt時刻，采集深度計和多普勒聲納速度計數據。多普勒聲納速度計直接輸出當前位置水下機器人的速度V1和四條波束的返回時間Tn(n=l，2，3，4)，利用海水中聲速傳播的經驗公式分別對(0，Tn)積分可計算得到四條波束的長度(ln，I21, I31, 141)，結合多普勒聲納速度計的開角大小和深度計得到當前機器人正下方的水深值，利用勾股定理計算得到四個深度值(hn, h21, h31, h41)。4、在t2=t0+2 A t,......，tn=tQ+n A t時亥Ij,利用步驟3中的方法采集計算可得到
多普勒聲納速度計四條波束所對應時刻的四個水深數據，結合步驟3中得到的四個深度值，每條波束可得到一條線地形深度數據序列，四條波束共可得到四條線地形數據(hn，
hi2，......，hlnノ，(h21，h22，......，h2n)，(h41，h42，......，h4n)。
5、利用多普勒聲納速度計輸出的相應時刻水下機器人速度(V1，V2，……，vn)和光纖羅經輸出的水下機器人的首向角(a p a2,……，a n)，對其積分可以得到水下機器人所處的大致位置坐標序列((Xl，Y1) (x2, y2)……(xn，yn))，計算位置誤差值的方差O。6、由步驟5中水下機器人所處的大致位置處利用6o原則在海圖中選取匹配區域，并提取匹配區域的水深值數據。7、利用MSD算子對獲取的四組線地形水深數據和匹配區域中的水深值數據進行匹配運算，得到四組線地形(hkl，hk2，……，hkn)，k=l，2，3，4對應位置的精確坐標序列((xkl，Ykl) (Xk2，yk2)......(Xkn，Ykn) )，k—1，2, 3, 4。8、分別對(xkl, ykl)，k=l, 2, 3, 4 ； (xk2, yk2)，k=l, 2, 3, 4 ；......； (Xkn, yj，k=l, 2，3，4
中四個坐標值進行融合計算，得到(tft。+At, t2=t0+2 A t,......, tn=tQ+n A t)時刻水下機
器人所處的精確位置序列((X1, Y1) (X2, Y2)……(Xn, Yn))，其中(Xn，Yn)為當前時刻水下機器人所處位置的精確位置坐標。9、輸出當前水下機器人所處的精確位置坐標到水下機器人主控制計算機。三、第二次以后的匹配10、在 tn+1=tn+ A t 時亥Ij,令 hkl=hk2, hk2=hk3,......，K(^1)=Kkn, k=l, 2, 3, 4 ； (X1, Y1)
_、x2，y2),、x2，y2)_、x3，y3),......，、Xn-l，yn-l ノ _(Xn，yn ノ11、采集計算得到tn+1時刻的深度值數據和水下機器人的大致位置坐標并賦值給hto(k=l，2，3，4)和(xn，yn)，可以得到四條更新的四條線地形數據和匹配區域數據。重復步驟7、8、9，即可以實現對水下機器人位置坐標連續不間斷的精確導航定位。
四、終止在水下機器人完成預定任務或者到達指定區域，主控制計算機輸出導航終止指令，跳出流程。
權利要求
1.下機器人的多線地形匹配導航方法，其特征是: (1)在t=、初始時刻，獲取水下機器人的初始位置(Xtl,Y0)； (2)采集水下機器人上的光纖羅經的初始方向角a^ ; (3)在At時刻，采集水下機器人上的深度計和多普勒聲納速度計數據，獲得當前位置水下機器人的速度V1和四條波束的返回時間Tn(n=l，2，3，4)，從而得到四條波束的長度(し，I2I, I3I, 141)，結合多普勒聲納速度計的開角大小和深度計得到當前機器人正下方的水深值，從而得到四個深度值(hn，h21, h31, h41)； (4)在t2=tQ+2A t,......, tn=tQ+n A t時刻,分別獲取多普勒聲納速度計四條波束所對應時刻的四個水深數據，結合步驟(3)中得到的四個深度值(hn，h21, h31, h41)，每條波束可得到一條線地形深度數據序列，四條波束共可得到四條線地形數據(hn，h12，……，hln)，(h21，ヒ22，......，h2n/*，(h41，h42，......，h4n)； (5)利用多普勒聲納速度計輸出的相應時刻水下機器人速度(V1,V2,……，vn)和光纖羅經輸出的水下機器人的首向角(a p a2,……，a n)，對其積分可以得到水下機器人所處的大致位置坐標序列((Xl，Y1) (x2, y2)……(xn，yn))，計算位置誤差值的方差0 ; (6)由步驟(5)中水下機器人所處 的大致位置處利用60原則在海圖中選取匹配區域，并提取匹配區域的水深值數據； (7)利用MSD算子對獲取的四組線地形水深數據和匹配區域中的水深值數據進行匹配運算，得到四組線地形(hkl，hk2，……，hkn)，k=l, 2，3，4對應位置的精確坐標序列((xkl，ykl)(xk2，yk2)......(xkn, yj )； (8)分別對(xkl,ykl)、(xk2, yk2)......(xkn, ykn) (k=l, 2，3，4)中四個坐標值進行融合計算，得到(tftd+A t, t2=t0+2 A t,......，tn=tQ+n A t)時刻水下機器人所處的精確位置序列((X1, Y1) (X2, Y2)……(Xn, Yn) )，(Xn, Yn)為當前時刻水下機器人所處位置的精確位置坐標； (9)輸出當前水下機器人所處的精確位置坐標到水下機器人主控制計算機；(10)在tn+1=tn+At 時亥Ij,令 hkl=hk2, hk2=hk3,......，K(^1)=Kkn, k=l, 2, 3, 4 ; (X1J1)= (x2，y2)，U2, J2) = (x3, y3；,......，Un-1, Yn-1) = (xn, Yn)；(11)采集計算得到tn+1時刻的深度值數據和水下機器人的大致位置坐標并賦值給hkn(k=l, 2，3，4)和(xn，yn)，得到四條更新的四條線地形數據和匹配區域數據，重復步驟(7) (9)，實現對水下機器人位置坐標連續不間斷的精確導航定位； (12)在水下機器人完成預定任務或者到達指定區域后，完成導航。
2.根據權利要求1所述的水下機器人的多線地形匹配導航方法，其特征是:所述的四條波束的長度(I11, I21, I31, I41)由海水中聲速傳播的經驗公式分別對(0，Tn)積分計算得到。
全文摘要
本發明的目的在于提供水下機器人的多線地形匹配導航方法，包括如下步驟接收多普勒速度計和光纖羅經信息，得到當前水下機器人所處的推算位置，接收多普勒速度計四個波束的返回數據，得到水下機器人距離海底的四個斜距，計算垂直距離并融合深度計的值，得到四個水深點值。重復若干次，得到四組可以用于匹配的線地形數據。利用概率相關的方法，得到四個地形匹配后修正的線地形，并對其進行融合，獲得精確匹配結果。本發明可有效地適應長航程水下機器人在水下長時間可靠的精確導航的要求，無需采用昂貴的慣性導航裝置，可避免水下機器人頻繁上浮修正導航數據所帶來的種種弊端，保證水下機器人導航的精確度。
文檔編號G01S15/88GK103090861SQ201310012789
公開日2013年5月8日 申請日期2013年1月14日 優先權日2013年1月14日
發明者李曄, 陳鵬云, 陳小龍, 姜言清, 吳琪, 蘇清磊, 李一鳴 申請人:哈爾濱工程大學