一种基于超短基线的导航定位方法
【技术领域】
[0001] 本发明涉及水下机器人技术领域，尤其涉及一种基于超短基线扣SBL)的导航定 位的方法。
【背景技术】
[0002] 水下机器人在复杂的海底区域进行长时间作业时，其自身具备较高的导航定位精 度是至关重要的。自主导航方法由于有时间累积误差，因此需要外部的导航信息进行实时 修正。长基线基阵可W为水下机器人提供高精度的定位信息，但缺点是布阵和回收过程繁 琐，而且受作业现场环境的影响较大。超短基线安装在母船上，可W实时监控水下水下机器 人位置，它既不需要布阵，同时可随母船跟踪水下机器人，缺点是现有的超短基线没有水声 通讯功能，因而定位信息无法提供给水下机器人。利用声通讯机可W将US化的定位信息发 给水下机器人，再由水下机器人进行数据融合，进而可W实现US化组合导航功能，送既避 免了布阵和回收过程的繁琐，又能使水下机器人的作业区域更广，使用更加灵活。
【发明内容】
[0003] 为了克服现有方法的不足，本发明要解决的技术问题是提供一种基于超短基线的 导航定位方法，利用US化跟踪定位水下机器人并通过声通讯机将定位信息回发给水下机 器人使其实现组合导航，同时具备对声通讯时延进行补偿的能力。
[0004] 本发明为实现上述目的所采用的技术方案是；一种基于超短基线的导航定位方 法，包括W下步骤：
[0005] 利用超短基线对水下机器人进行定位；
[0006] 通过声通讯机将母船位置信息和超短基线定位到的水下机器人位置信息发给水 下机器人；
[0007] 水下机器人接收声通讯机发出的定位信息；
[0008] 结合运动补偿算法计算水下机器人收到声通讯机信息时自身的位置信息；
[0009] 将经过运动补偿后的位置信息作为观测量对水下机器人的实际位置进行更新。
[0010] 所述的利用超短基线对水下机器人进行定位具体为；通过超短基线基阵与水下机 器人搭载的应答器通信得到水下机器人的位置信息。
[0011] 所述运动补偿包括W下步骤：
[0012] 第1步；获得母船和水下机器人位置信息；
[0013] 第2步：获得水下机器人收到声通讯机发出信息时刻的深度数据；
[0014] 第3步；解算水下机器人当前时刻的位置：
[0015]
[0016] 其中，di是k时刻母船与水下机器人之间的水平距离；d2是k-1时刻母船与水下 机器人间的水平距离，该变量为已知量油是Δ t时刻里水下机器人所走过的水平距离； Δ t是从声通讯机发送数据到水下机器人接收数据所经历的时间；hk是k时刻水下机器人 所在的深度；Xk 1是k-1时刻超短基线获得的水下机器人位置信息；f( ·)是水声信号在水 下的传播距离和传播时间的关系函数；^是水下机器人在接收声通讯机数据过程中的速度 矢量，为已知量；k-1时刻为超短基线得到水下机器人位置信息的时刻；k时刻为水下机器 人收到其位置信息的时刻；
[0017] 解上面方程组得到Δ t，则k时刻水下机器人所在的位置Xk为：
[0018]
[0019] 所述第2步中的深度数据由水下机器人搭载的深度计提供。
[0020] 所述对水下机器人的实际位置进行更新，包括W下步骤：
[0021] 计算观测新息：
[0022]
[0023] 其中，V为观测新息，Xk为k时刻水下机器人所在的位置巧为观测矩阵，由系统的 观测模型得来，是已知量；为基于k-1时刻得到的k时刻水下机器人状态的预测；
[0024] 计算系统增益：
[00 巧]
[0026] 其中，Kk为系统增益，Pk|k 1为系统的状态协方差预测矩阵，Rk为观测噪声协方差矩 阵；
[0027] 更新：
[0028]
[0029] 本发明具有W下优点及有益效果：
[0030] 1.利用US化跟踪定位水下机器人并通过声通讯机将定位信息回发给水下机器人 使其实现组合导航
[0031] 2.操作简单，能准确有效地利用US化定位信息实现水下机器人组合导航，进而提 高水下机器人在水下的导航定位精度。
[0032] 3.具备对声通讯时延进行补偿的能力。
【附图说明】
[0033] 图1是本发明的组成示意图；
[0034] 图2是本发明的运动补偿算法流程图。
【具体实施方式】
[0035] 下面结合附图及实施例对本发明做进一步的详细说明。
[0036] 本发明由的水下机器人、母船、US化系统、水面监控计算机、声通讯机组成，其中水 下机器人需要搭载深度计、WL (多普勒测速仪）W及角速度巧螺，如图1所示。
[0037] US化基阵与水下机器人上搭载的应答器通信，再由US化-BOX计算出水下机器人 位置，然后通过声通讯机将该位置及母船位置信息发给水下机器人。
[0038] US化定位数据的判别方法如图2所示，水下机器人收到母船声通讯机发来的信息 并解算它与上一时刻位置数据之间距离（如果是第一个数据则不计算），当该距离小于转， 则计数器i加1，当i大于等于N时，则判别该位置信息是有效的，可W用来更新水下机器人 的位置。
[0039] 当得到有效的位置数据W后，水下机器人控制系统通过运动补偿算法算出水下机 器人当前的位置。
[0040] 第1步：获得母船和水下机器人的位置信息。
[0041] 第2步；获得水下机器人收到声通讯机信息时当前的深度数据。
[0042] 水下机器人当前的深度数据由水下机器人上搭载的深度计设备提供。
[0043] 第3步；解算水下机器人当前时刻的位置。
[0044]
[0045] 假设US化在k-1时刻得到水下机器人位置信息，并通过声通讯机发送出去，水下 机器人在k时刻收到该信息。则W上方程组中，hk是k时刻水下机器人所在的深度，可W由 深度计得到，Xk 1是k-1时刻US化获得的水下机器人位置，f (·)是水声信号在水下的传播 距离和传播时间的关系函数。di是k时刻母船与水下机器人之间的水平距离，d2是k-1时 刻母船与水下机器人间的水平距离，该变量为已知量，ds是Δ t时段里水下机器人所走过的 水平距离。
[0046] 解上述方程组可W得到从声通讯机发送数据到水下机器人接收数据所经历的时 间Δ t，则k时刻水下机器人所在的位置Xk为
[0047]
[0048] 按照本发明的方法将补偿得到的水下机器人当前位置用于更新导航算法的状态 量。
[004引第1步；计算观测新息
[0050]
[0051] 第2步；计算系统增益
[005引式中，Η为观测矩阵，Xk为k时刻水下机器人的状态量，Pk为系统的状态协方差矩 阵，Rk为观测噪声协方差矩阵。
【主权项】
1. 一种基于超短基线的导航定位方法，其特征在于，包括以下步骤： 利用超短基线对水下机器人进行定位； 通过声通讯机将母船位置信息和超短基线定位到的水下机器人位置信息发给水下机 器人； 水下机器人接收声通讯机发出的定位信息； 结合运动补偿算法计算水下机器人收到声通讯机信息时自身的位置信息； 将经过运动补偿后的位置信息作为观测量对水下机器人的实际位置进行更新。2. 根据权利要求1所述的一种基于超短基线的导航定位方法，其特征在于，所述的利 用超短基线对水下机器人进行定位具体为：通过超短基线基阵与水下机器人搭载的应答器 通信得到水下机器人的位置信息。3. 根据权利要求1所述的一种基于超短基线的导航定位方法，其特征在于，所述运动 补偿包括以下步骤： 第1步：获得母船和水下机器人位置信息； 第2步：获得水下机器人收到声通讯机发出信息时刻的深度数据； 第3步：解算水下机器人当前时刻的位置：其中，山是k时刻母船与水下机器人之间的水平距离；d2是k-Ι时刻母船与水下机器 人间的水平距离，该变量为已知量；d3是At时刻里水下机器人所走过的水平距离；At是 从声通讯机发送数据到水下机器人接收数据所经历的时间；h k是k时刻水下机器人所在的 深度；Xk i是k-Ι时刻超短基线获得的水下机器人位置信息；f( ·)是水声信号在水下的传 播距离和传播时间的关系函数；^是水下机器人在接收声通讯机数据过程中的速度矢量， 为已知量；k-Ι时刻为超短基线得到水下机器人位置信息的时刻；k时刻为水下机器人收到 其位置信息的时刻； 解上面方程组得到Λ t，则k时刻水下机器人所在的位置Xk为：4. 根据权利要求3所述的一种基于超短基线的导航定位方法，其特征在于，所述第2步 中的深度数据由水下机器人搭载的深度计提供。5. 根据权利要求1所述的一种基于超短基线的导航定位方法，其特征在于，所述对水 下机器人的实际位置进行更新，包括以下步骤： 计算观测新息：其中，v为观测新息，Xk为k时刻水下机器人所在的位置；Η为观测矩阵，由系统的观测 模型得来，是已知量；为基于k_l时刻得到的k时刻水下机器人状态的预测； 计算系统增益： Kk = Pk,k !HT(HPk|k1 其中，Kk为系统增益，Pk|k i为系统的状态协方差预测矩阵，Rk为观测噪声协方差矩阵； 更新：
【专利摘要】本发明公开一种基于超短基线(USBL)的导航定位方法，该发明将水下无人无缆水下机器人(水下机器人)的超短基线定位信息通过声通讯机发给水下机器人，以实现水下机器人的组合导航功能。该发明分成2个阶段，第一阶段是利用USBL对水下机器人进行跟踪定位，并将定位信息通过声通讯机发给水下机器人，第二阶段是水下机器人接收到定位信息，再经过运动补偿对导航算法进行状态更新。本发明操作简单，能准确有效地利用USBL定位信息实现水下机器人组合导航，进而提高水下机器人在水下的导航定位精度。
【IPC分类】G01C21/00, G01C25/00, G01C21/20
【公开号】CN105628016
【申请号】CN201410605690
【发明人】王飞, 徐春晖, 王轶群, 刘健, 许以军
【申请人】中国科学院沈阳自动化研究所
【公开日】2016年6月1日
【申请日】2014年10月30日