专利名称：水下机器人的多线地形匹配导航方法
技术领域：
本发明涉及的是ー种导航方法，具体地说是水下导航方法。
背景技术：
由于水下机器人具有广泛的应用前景，世界各国高度重视水下机器人的研究并取得了长足的发展。在水下机器人的研究中，水下机器人是否能实现精确的导航定位是其能否顺利完成任务并安全返航的关键。常用于水下机器人的水下导航定位方法包括水声学导航、航位推算导航、惯性导航等。水声学导航需要布置声学基阵，不利于水下机器人的独立作业任务，且导航范围受限。航位推算导航和惯性导航系统在短期内精度较高，但其定位误差随时间积累，长时间水下航行会产生较大的偏差，需要定期上浮接收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日
发明者李晔, 陈鹏云, 陈小龙, 姜言清, 吴琪, 苏清磊, 李一鸣 申请人:哈尔滨工程大学