[发明专利]水下机器人的多线地形匹配导航方法

摘要

本发明的目的在于提供水下机器人的多线地形匹配导航方法，包括如下步骤：接收多普勒速度计和光纤罗经信息，得到当前水下机器人所处的推算位置，接收多普勒速度计四个波束的返回数据，得到水下机器人距离海底的四个斜距，计算垂直距离并融合深度计的值，得到四个水深点值。重复若干次，得到四组可以用于匹配的线地形数据。利用概率相关的方法，得到四个地形匹配后修正的线地形，并对其进行融合，获得精确匹配结果。本发明可有效地适应长航程水下机器人在水下长时间可靠的精确导航的要求，无需采用昂贵的惯性导航装置，可避免水下机器人频繁上浮修正导航数据所带来的种种弊端，保证水下机器人导航的精确度。

主权项

水下机器人的多线地形匹配导航方法，其特征是：（1）在t=t0初始时刻，获取水下机器人的初始位置(x0,y0)；（2）采集水下机器人上的光纤罗经的初始方向角α0；（3）在t1=t0+Δt时刻，采集水下机器人上的深度计和多普勒声纳速度计数据，获得当前位置水下机器人的速度v1和四条波束的返回时间Tn(n=1,2,3,4)，从而得到四条波束的长度（l11,l21,l31,l41），结合多普勒声纳速度计的开角大小和深度计得到当前机器人正下方的水深值，从而得到四个深度值（h11,h21,h31,h41）；（4）在t2=t0+2Δt，……，tn=t0+nΔt时刻，分别获取多普勒声纳速度计四条波束所对应时刻的四个水深数据，结合步骤（3）中得到的四个深度值（h11,h21,h31,h41），每条波束可得到一条线地形深度数据序列，四条波束共可得到四条线地形数据(h11，h12，……，h1n)，(h21，h22，……，h2n)，(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,4对应位置的精确坐标序列（(xk1,yk1)(xk2,yk2)……(xkn,ykn)）；（8）分别对(xk1,yk1)、(xk2,yk2)……(xkn,ykn)(k=1,2,3,4)中四个坐标值进行融合计算，得到（t1=t0+Δt，t2=t0+2Δt，……，tn=t0+nΔt）时刻水下机器人所处的精确位置序列（(X1,Y1)(X2,Y2)……(Xn,Yn)），(Xn,Yn)为当前时刻水下机器人所处位置的精确位置坐标；（9）输出当前水下机器人所处的精确位置坐标到水下机器人主控制计算机；（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)；（11）采集计算得到tn+1时刻的深度值数据和水下机器人的大致位置坐标并赋值给hkn（k=1,2,3,4）和(xn,yn)，得到四条更新的四条线地形数据和匹配区域数据，重复步骤（7）～（9），实现对水下机器人位置坐标连续不间断的精确导航定位；（12）在水下机器人完成预定任务或者到达指定区域后，完成导航。