《水下机器人的多线地形匹配导航方法.pdf》由会员分享，可在线阅读，更多相关《水下机器人的多线地形匹配导航方法.pdf（8页珍藏版）》请在专利查询网上搜索。
1、(10)申请公布号 CN 103090861 A (43)申请公布日 2013.05.08 CN 103090861 A *CN103090861A* (21)申请号 201310012789.8 (22)申请日 2013.01.14 G01C 21/00(2006.01) G01S 15/88(2006.01) (71)申请人 哈尔滨工程大学 地址 150001 黑龙江省哈尔滨市南岗区南通 大街 145 号哈尔滨工程大学科技处知 识产权办公室 (72)发明人 李晔 陈鹏云 陈小龙 姜言清 吴琪 苏清磊 李一鸣 (54) 发明名称 水下机器人的多线地形匹配导航方法 (57) 摘要 本发明的目的。
2、在于提供水下机器人的多线地 形匹配导航方法， 包括如下步骤 ： 接收多普勒速 度计和光纤罗经信息， 得到当前水下机器人所处 的推算位置， 接收多普勒速度计四个波束的返回 数据， 得到水下机器人距离海底的四个斜距， 计算 垂直距离并融合深度计的值， 得到四个水深点值。 重复若干次， 得到四组可以用于匹配的线地形数 据。利用概率相关的方法， 得到四个地形匹配后 修正的线地形， 并对其进行融合， 获得精确匹配结 果。本发明可有效地适应长航程水下机器人在水 下长时间可靠的精确导航的要求， 无需采用昂贵 的惯性导航装置， 可避免水下机器人频繁上浮修 正导航数据所带来的种种弊端， 保证水下机器人 导航的精。
3、确度。 (51)Int.Cl. 权利要求书 1 页 说明书 4 页 附图 2 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请 权利要求书1页 说明书4页 附图2页 (10)申请公布号 CN 103090861 A CN 103090861 A *CN103090861A* 1/1 页 2 1. 水下机器人的多线地形匹配导航方法， 其特征是 ： （1） 在 t=t0初始时刻， 获取水下机器人的初始位置 (x0,y0) ； （2） 采集水下机器人上的光纤罗经的初始方向角 0； （3） 在 t1=t0+t 时刻， 采集水下机器人上的深度计和多普勒声纳速度计数据， 获得当 前位置水下机。
4、器人的速度 v1和四条波束的返回时间 Tn(n=1,2,3,4)， 从而得到四条波束的 长度 （l11,l21,l31,l41） ， 结合多普勒声纳速度计的开角大小和深度计得到当前机器人正下方 的水深值， 从而得到四个深度值 （h11,h21,h31,h41） ； （4） 在 t2=t0+2t， tn=t0+nt 时刻， 分别获取多普勒声纳速度计四条波束所对 应时刻的四个水深数据， 结合步骤 （3） 中得到的四个深度值 （h11,h21,h31,h41） ， 每条波束可得 到一条线地形深度数据序列， 四条波束共可得到四条线地形数据 (h11， h12， h1n)， (h21， h22， h2n。
5、)， (h41， h42， h4n) ； （5） 利用多普勒声纳速度计输出的相应时刻水下机器人速度 （v1， v2， vn） 和光纤 罗经输出的水下机器人的首向角 （1， 2， n） ， 对其积分可以得到水下机器人所处 的大致位置坐标序列 （(x1,y1)(x2,y2)(xn,yn)） ， 计算位置误差值的方差 ； （6） 由步骤 （5） 中水下机器人所处的大致位置处利用 6 原则在海图中选取匹配区域， 并提取匹配区域的水深值数据 ； （7） 利用 MSD 算子对获取的四组线地形水深数据和匹配区域中的水深值数据进行匹配 运算， 得到四组线地形 (hk1， hk2， hkn)， k=1,2,3,。
6、4 对应位置的精确坐标序列 （(xk1,yk1) (xk2,yk2)(xkn,ykn)） ； （8）分别对 (xk1,yk1)、 (xk2,yk2)(xkn,ykn)(k=1,2,3,4) 中四个坐标值进行融合计 算， 得到 （t1=t0+t， t2=t0+2t， tn=t0+nt） 时刻水下机器人所处的精确位置序列 （(X1,Y1)(X2,Y2)(Xn,Yn)） ， (Xn,Yn) 为当前时刻水下机器人所处位置的精确位置坐标 ； （9） 输出当前水下机器人所处的精确位置坐标到水下机器人主控制计算机 ； （10）在 tn+1=tn+t 时 刻，令 hk1=hk2， hk2=hk3， ， hk(。
7、n-1)=hkn， k=1,2,3,4 ；（x1,y1） =(x2,y2)， (x2,y2)=（x3,y3） ， (xn-1,yn-1)=(xn,yn) ； （11） 采集计算得到 tn+1时刻的深度值数据和水下机器人的大致位置坐标并赋值给 hkn （k=1,2,3,4） 和 (xn,yn)， 得到四条更新的四条线地形数据和匹配区域数据， 重复步骤 （7） （9） ， 实现对水下机器人位置坐标连续不间断的精确导航定位 ； （12） 在水下机器人完成预定任务或者到达指定区域后， 完成导航。 2. 根据权利要求 1 所述的水下机器人的多线地形匹配导航方法， 其特征是 ： 所述的四 条波束的长度 （。
8、l11,l21,l31,l41） 由海水中声速传播的经验公式分别对 (0,Tn) 积分计算得到。 权 利 要 求 书 CN 103090861 A 2 1/4 页 3 水下机器人的多线地形匹配导航方法 技术领域 0001 本发明涉及的是一种导航方法， 具体地说是水下导航方法。 背景技术 0002 由于水下机器人具有广泛的应用前景， 世界各国高度重视水下机器人的研究并取 得了长足的发展。在水下机器人的研究中， 水下机器人是否能实现精确的导航定位是其能 否顺利完成任务并安全返航的关键。 0003 常用于水下机器人的水下导航定位方法包括水声学导航、 航位推算导航、 惯性导 航等。水声学导航需要布置声。
9、学基阵， 不利于水下机器人的独立作业任务， 且导航范围受 限。 航位推算导航和惯性导航系统在短期内精度较高， 但其定位误差随时间积累， 长时间水 下航行会产生较大的偏差， 需要定期上浮接收 GPS 进行修正。水下机器人频繁上浮不利于 长时间深水作业， 且海上风浪较大时对水下机器人相对较小的载体也是一个巨大的威胁。 发明内容 0004 本发明的目的在于提供一种不使用 GPS 等外部传感器的水下机器人的多线地形 匹配导航方法。 0005 本发明的目的是这样实现的 ： 0006 本发明水下机器人的多线地形匹配导航方法， 其特征是 ： 0007 （1） 在 t=t0初始时刻， 获取水下机器人的初始位置。
10、 (x0,y0) ； 0008 （2） 采集水下机器人上的光纤罗经的初始方向角 0； 0009 （3） 在 t1=t0+t 时刻， 采集水下机器人上的深度计和多普勒声纳速度计数据， 获 得当前位置水下机器人的速度 v1和四条波束的返回时间 Tn(n=1,2,3,4)， 从而得到四条波 束的长度 （l11,l21,l31,l41） ， 结合多普勒声纳速度计的开角大小和深度计得到当前机器人正 下方的水深值， 从而得到四个深度值 （h11,h21,h31,h41） ； 0010 （4） 在 t2=t0+2t， tn=t0+nt 时刻， 分别获取多普勒声纳速度计四条波束 所对应时刻的四个水深数据， 结。
11、合步骤 （3） 中得到的四个深度值 （h11,h21,h31,h41） ， 每条波束 可得到一条线地形深度数据序列， 四条波束共可得到四条线地形数据 (h11， h12， h1n)， (h21， h22， h2n)， (h41， h42， h4n) ； 0011 （5） 利用多普勒声纳速度计输出的相应时刻水下机器人速度 （v1， v2， vn） 和 光纤罗经输出的水下机器人的首向角 （1， 2， n） ， 对其积分可以得到水下机器人 所处的大致位置坐标序列 （(x1,y1)(x2,y2)(xn,yn)） ， 计算位置误差值的方差 ； 0012 （6） 由步骤 （5） 中水下机器人所处的大致位置。
12、处利用 6 原则在海图中选取匹配 区域， 并提取匹配区域的水深值数据 ； 0013 （7） 利用 MSD 算子对获取的四组线地形水深数据和匹配区域中的水深值数据进 行匹配运算， 得到四组线地形 (hk1， hk2， hkn)， k=1,2,3,4 对应位置的精确坐标序列 （(xk1,yk1)(xk2,yk2)(xkn,ykn)） ； 说 明 书 CN 103090861 A 3 2/4 页 4 0014 （8） 分别对 (xk1,yk1)、 (xk2,yk2)(xkn,ykn)(k=1,2,3,4) 中四个坐标值进行融合 计算， 得到 （t1=t0+t， t2=t0+2t， ， tn=t0+n。
13、t） 时刻水下机器人所处的精确位置序列 （(X1,Y1)(X2,Y2)(Xn,Yn)） ， (Xn,Yn) 为当前时刻水下机器人所处位置的精确位置坐标 ； 0015 （9） 输出当前水下机器人所处的精确位置坐标到水下机器人主控制计算机 ； 0016 （10） 在 tn+1=tn+t 时刻， 令 hk1=hk2， hk2=hk3， hk(n-1)=hkn， k=1,2,3,4 ；（x1,y1） =(x2,y2)， (x2,y2)=（x3,y3） ， (xn-1,yn-1)=(xn,yn) ； 0017 （11） 采集计算得到 tn+1时刻的深度值数据和水下机器人的大致位置坐标并赋值 给 hkn（。
14、k=1,2,3,4） 和 (xn,yn)， 得到四条更新的四条线地形数据和匹配区域数据， 重复步骤 （7） （9） ， 实现对水下机器人位置坐标连续不间断的精确导航定位 ； 0018 （12） 在水下机器人完成预定任务或者到达指定区域后， 完成导航。 0019 本发明还可以包括 ： 0020 1、 所述的四条波束的长度 （l11,l21,l31,l41） 由海水中声速传播的经验公式分别对 (0,Tn) 积分计算得到。 0021 本发明的优势在于 ： 本发明可有效地适应长航程水下机器人在水下长时间可靠的 精确导航的要求， 无需采用昂贵的惯性导航装置， 可避免水下机器人频繁上浮修正导航数 据所带来。
15、的种种弊端， 保证水下机器人导航的精确度。 附图说明 0022 图 1 为水下机器人的多线地形匹配导航装置结构示意图 ； 0023 图 2 地形匹配计算机的密封结构示意图 ； 0024 图 3 为本发明的流程图。 具体实施方式 0025 下面结合附图举例对本发明做更详细地描述 ： 0026 结合图 1 3， 水下机器人的多线地形匹配导航装置的组成包括多普勒速度计 1、 水压深度计 2、 PC/104 计算机 3、 PC/104 总线 AD 采集卡 4、 PC/104 总线串口板 5、 光纤罗经 6、 主控计算机 7。同时结合图 2， PC/104 计算机 3、 PC/104 总线 AD 采集卡。
16、 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 串行通信联结。 0027 将多线地形匹配系统与主控计算机联接。 0028 多线地形匹配系统上电， 程序自启动。启动 3 个任务。 0029 任务 1 ： 匹配程序初始化。
17、， 多普勒速度计、 光纤罗经和深度计初始化， 处于待机状 态。 0030 任务 2 ： 建立串口数据检测线程， 与主控计算机形成通信。 0031 任务 3 ： 任务 3 为主任务， 机制为自动应答， 依照预先程序设定的控制节拍， 启动多 普勒速度计和深度计采集数据， 同时采集光纤罗经信息， 由光纤罗经数据和多普勒速度计 数据进行航位计算得到水下机器人的大致位置， 由深度计和多普勒速度计测得的数据计算 说 明 书 CN 103090861 A 4 3/4 页 5 水下机器人周围的四个深度值。航行过程中， 采集点位形成四条线地形。通过由航位推算 的结果确定海图的匹配范围， 利用概率相关的方法确定水。
18、下机器人周围的四条线地形的修 正值， 并将其进行融合得到精确位置。将计算出位置结果， 传输给主控计算机， 完成一个指 令循环。 在完成第一次匹配后， 以后仅需要用最新的数据替代最老的数据进行匹配， 即可实 现水下机器人长时精确的水下地形匹配导航。 0032 本发明的方法流程如下， 如图 3 ： 0033 一、 初始化 ： 0034 1、 在初始时刻 t=t0时刻， 由水下机器人控制舱主控制计算机或人工输入水下机器 人的初始位置 (x0,y0)。 0035 2、 采集光纤罗经的初始方向角 0。 0036 二、 初次匹配 0037 3、 在t1=t0+t时刻， 采集深度计和多普勒声纳速度计数据。 。
19、多普勒声纳速度计直 接输出当前位置水下机器人的速度 v1和四条波束的返回时间 Tn(n=1,2,3,4), 利用海水中 声速传播的经验公式分别对(0,Tn)积分可计算得到四条波束的长度 （l11,l21,l31,l41） ,结合 多普勒声纳速度计的开角大小和深度计得到当前机器人正下方的水深值， 利用勾股定理计 算得到四个深度值 （h11,h21,h31,h41） 。 0038 4、 在 t2=t0+2t， tn=t0+nt 时刻， 利用步骤 3 中的方法采集计算可得到 多普勒声纳速度计四条波束所对应时刻的四个水深数据， 结合步骤 3 中得到的四个深度 值， 每条波束可得到一条线地形深度数据序列。
20、， 四条波束共可得到四条线地形数据 (h11， h12， h1n)， (h21， h22， h2n)， (h41， h42， h4n)。 0039 5、 利用多普勒声纳速度计输出的相应时刻水下机器人速度 （v1， v2， vn） 和光 纤罗经输出的水下机器人的首向角 （1， 2， n） ， 对其积分可以得到水下机器人所 处的大致位置坐标序列 （(x1,y1)(x2,y2)(xn,yn)） ， 计算位置误差值的方差 。 0040 6、 由步骤 5 中水下机器人所处的大致位置处利用 6 原则在海图中选取匹配区 域， 并提取匹配区域的水深值数据。 0041 7、 利用 MSD 算子对获取的四组线地形。
21、水深数据和匹配区域中的水深值数据进 行匹配运算， 得到四组线地形 (hk1， hk2， hkn)， k=1,2,3,4 对应位置的精确坐标序列 （(xk1,yk1)(xk2,yk2)(xkn,ykn)） ， k=1,2,3,4。 0042 8、 分别对 (xk1,yk1)， k=1,2,3,4 ； (xk2,yk2)， k=1,2,3,4 ； ； (xkn,ykn)， k=1,2,3,4 中四个坐标值进行融合计算， 得到 （t1=t0+t， t2=t0+2t， tn=t0+nt） 时刻水下机 器人所处的精确位置序列 （(X1,Y1)(X2,Y2)(Xn,Yn)） ， 其中 (Xn,Yn) 为当。
22、前时刻水下机器 人所处位置的精确位置坐标。 0043 9、 输出当前水下机器人所处的精确位置坐标到水下机器人主控制计算机。 0044 三、 第二次以后的匹配 0045 10、 在 tn+1=tn+t 时刻， 令 hk1=hk2， hk2=hk3， hk(n-1)=hkn， k=1,2,3,4 ；（x1,y1） =(x2,y2)， (x2,y2)=（x3,y3） ， (xn-1,yn-1)=(xn,yn) 0046 11、 采集计算得到 tn+1时刻的深度值数据和水下机器人的大致位置坐标并赋值给 hkn（k=1,2,3,4） 和 (xn,yn)， 可以得到四条更新的四条线地形数据和匹配区域数据。重复步 骤 7、 8、 9， 即可以实现对水下机器人位置坐标连续不间断的精确导航定位。 说 明 书 CN 103090861 A 5 4/4 页 6 0047 四、 终止 0048 在水下机器人完成预定任务或者到达指定区域， 主控制计算机输出导航终止指 令， 跳出流程。 说 明 书 CN 103090861 A 6 1/2 页 7 图 1 图 2 说 明 书 附 图 CN 103090861 A 7 2/2 页 8 图 3 说 明 书 附 图 CN 103090861 A 8 。