全海深水下机器人及其定位方法与流程

[0001]
本发明涉及水下机器人的技术领域。


背景技术：

[0002]
水下机器人作为海洋开发的重要工具，可有效克服水下环境恶劣危险、人的潜水深度有限的问题，完成水下极限作业。
[0003]
水下机器人的精准定位是对其进行自主调控或遥控的重要前提。现有技术中，多通过传感器、声呐等获得水下机器人的位置信息。其中传感器可根据当下环境获得水下机器人的一些实时位置信息，声呐可通过声波的传导和反射等获得水下机器人与其他目标或障碍物之间的距离信息。在这两种信息中，通过声呐等声波测量手段得到的信息数据是离散的，而通过一般传感器得到的数据可以是连续的，现有技术中仍然缺乏将这两类信息进行合理高效融合的手段，无法基于融合信息同时得到水下机器人的自我位置和相对位置。
[0004]
而现有的可对声波测量数据与其他信息数据进行融合的手段，如通过粒子滤波进行信息融合或通过极大似然估计进行融合，在样本数量多的体系，如复杂的海洋环境下应用时，均需要进行大量的数据处理，复杂程度极高，且存在数据延迟高，适用性差。
[0005]
此外，因声波在水中的传播存在能量损耗，在极限量程之外，声波无法抵达或返回，因此直接依靠声波测量，无法获得水下机器人的全海深数据。


技术实现要素：

[0006]
本发明的目的在于提出一种可克服声波测量数据的离散、量程有限的问题，并根据简单、有效的融合后数据，对水下机器人进行准确的、全海深的定位的方法。
[0007]
本发明的目的还在于提出一种通过上述定位方法进行定位的水下机器人，其可实现高效准确的自主控制。
[0008]
本发明首先提供了如下的技术方案：
[0009]
全海深水下机器人的定位方法，其包括：通过卡尔曼滤波法对所述机器人的声波测量数据及实时深度数据进行融合，根据融合模型对所述机器人进行定位。
[0010]
根据本发明的一些优选实施方式：所述声波测量数据通过单波束高度计获得。
[0011]
该优选实施方式中，单波束高度计可获得良好的垂向距离数据，在目前水下机器人测量距底高度方面应用广泛。其工作中需要发射和接收声波，因此在每次发射声波的间隔时间里，所得数据是空白的。
[0012]
根据本发明的一些优选实施方式：所述深度数据通过压力传感器获得。
[0013]
根据本发明的一些优选实施方式：所述声波测量数据为通过声波测量得到的所述水下机器人距离海底的高度数据。
[0014]
根据本发明的一些优选实施方式：以所述声波测量数据作为所述卡尔曼滤波法中的观测值。
[0015]
根据本发明的一些优选实施方式：以根据所述实时深度数据获得的机器人距离海
底的高度数据作为所述卡尔曼滤波法中的估计值。
[0016]
根据本发明的一些优选实施方式：对所述声波测量数据进行去噪后再构建所述融合模型。
[0017]
根据本发明的一些优选实施方式：所述融合模型的状态方程设置如下：
[0018][0019]
其中，表示k时刻的先验状态估计值；表示k-1时刻的后验状态估计值；u
k-1
表示k-1时刻的输入参数，a、b表示状态转换矩阵；
[0020]
且，所述融合模型的观测方程设置如下：
[0021][0022]
其中，表示基于先验状态估计值作出的预测观测值，h表示卡尔曼增益中的状态转换矩阵；
[0023]
且，所述融合模型的误差相关矩阵设置如下：
[0024][0025]
其中，表示k时刻的先验估计值的协方差；p
k-1
表示k-1时刻的后验估计值的协方差；q表示过程激励噪声协方差；
[0026]
且，所述融合模型的卡尔曼增益设置如下：
[0027][0028]
其中，r表示测量噪声协方差。
[0029]
根据本发明的一些优选实施方式：设置所述融合模型的更新方程如下：
[0030][0031][0032]
其中，i表示单位矩阵。
[0033]
本发明进一步提出了一种全海深水下机器人，其含有单波束高度计、通过压力传感的深度计、pid控制器及控制元件，其中，所述pid控制器根据上述任一种定位方法获得所述水下机器人的当前位置信息。
[0034]
本发明具备以下有益效果：
[0035]
本发明通过卡尔曼滤波法对单波束高度计和压力深度计的信号数据进行融合，所用观测数据中包括系统中的噪声和干扰的影响，可直接通过滤波过程实现最优估计。
[0036]
本发明以连续且更精确的数据作为估计值，以离散且误差较大的数据作为预测值，通过信息融合，可得到参考两种数据后更贴近实际数据的最优估计值。
[0037]
本发明通过信息融合的方式，可使水下机器人的距底高度数据更加完整，在超出高度计量程的深度中可根据单一传感器数据，得到水下机器人的多个高度数据。
[0038]
本发明的定位方法可克服单波束高度计容易受到外部环境的噪声干扰及障碍物
影响，导致测得数据和实际数据偏差较大的缺陷，得到高准确度的定位数据。
[0039]
本发明可实现水下机器人的自主、全海深作业。
附图说明
[0040]
图1为具体实施方式所述通过单波束高度计获得的信号数据统计图。
[0041]
图2为具体实施方式所述本发明的系统组成图。
[0042]
图3为实施例所述信息融合前后距底高度数据对比图。
[0043]
图4为实施例所述超量程单波束高度计数据反馈图。
[0044]
图5为实施例所述单波束高度计超量程后所得融合信息图。
具体实施方式
[0045]
以下结合实施例和附图对本发明进行详细描述，但需要理解的是，所述实施例和附图仅用于对本发明进行示例性的描述，而并不能对本发明的保护范围构成任何限制。所有包含在本发明的发明宗旨范围内的合理的变换和组合均落入本发明的保护范围。
[0046]
以下实施方式使用如附图2所示的水下机器人，其设置有单波束高度计、压力深度计、pid控制器及控制元件。
[0047]
其中单波束高度计是一种通过单束短脉冲声波测量水下机器人距水底距离的传感器。其通过换能器垂直向下发射单束短脉冲声波，声波在抵达海底后发生反射，反射波回到传感器并被接收，根据单束短脉冲声波发射到海底再反射回机器人的时间可确定机器人距离海底的高度。由于声波在水中传播需要时间，且存在能量损失，因此单波束高度计获得的数据具有一定延时性，且在发射间隔的时间里缺少数据，即所得数据是由一组组不连续的离散测点组成。单波束高度计的测试量程约为2000米左右。
[0048]
压力深度计是一种通过压敏元件获得水下机器人当前位置处压力，并根据压力获得当前位置深度的压力传感器，本发明使用压力深度计可选用常用的水下机器人压力传感器，如压阻式压力传感器。
[0049]
本发明的水下机器人的工作过程如下：单波束高度计将获得的机器人距离海底的高度的相关数据、压力深度计将其获得的机器人当前位置深度(即其距离海面的距离)的相关数据分别传输至pid控制器，由控制器根据本发明的定位方法获得水下机器人的当前位置，再根据控制方程获得其移动至目标位置所需的推力或力矩，并将这些信息处理为控制指令，输入至水下机器人搭载的控制元件如舵或推力器等处，由控制元件执行指令，实现水下机器人的定向移动。
[0050]
其中，通过单波束高度计获得机器人距离海底高度的过程如下：
[0051][0052]
其中，d
tr
表示机器人距底高度，c表示声波在海水介质中平均速度，t表示声波从发出到接收到的传播时间。
[0053]
即，由机器人控制器获得单波束高度计发出的声波从发出到接收的反射时间，如附图1所示，并根据式(1)计算得到机器人距底高度d
tr
。
[0054]
通过压力深度计获得机器人当前位置深度的过程如下：
[0055]
p＝ρgh
ꢀꢀ
(2)，
[0056]
其中，ρ表示海水密度，g表示重力加速度，h表示当前位置深度，p表示通过压力深度计获得的当前位置压强。
[0057]
在获得上述基于单波束高度计的距底高度数据和基于压力深度计的当前位置深度数据后，通过以下过程建立融合模型：
[0058]
s1根据当前位置深度数据，获得基于压力深度计的距底高度数据，如下：
[0059]
h＝d-d
ꢀꢀ
(3)，
[0060]
其中，h表示距底高度，d表示当前海域深度，d表示根据压力深度计获得的当前位置深度。
[0061]
s2通过基于单波束高度计的距底高度数据，及基于压力深度计的距底高度数据，建立数据融合的卡尔曼滤波模型。
[0062]
具体如下：
[0063]
s21设置基于单波束高度计的距底高度数据作为卡尔曼滤波的观测值，基于压力深度计的距底高度数据作为卡尔曼滤波的估计值；
[0064]
s22根据所述观测值及所述估计值，建立如下的卡尔曼滤波模型。
[0065]
s221设置模型的状态方程，如下：
[0066][0067]
其中，表示k-1时刻的后验状态估计值(对应的，表示k时刻的后验状态估计值)；表示k时刻的先验状态估计值；u
k-1
表示k-1时刻的输入参数，其可直接代入通过压力深度计数据、由h＝d-d得到的距底高度h；a、b表示状态转换矩阵。
[0068]
s222设置模型的观测方程，如下：
[0069][0070]
其中，表示基于先验状态估计值作出的预测观测值，其可直接代入通过单波束高度计获得的距底高度h，h表示卡尔曼增益中的状态转换矩阵。
[0071]
s223设置模型的误差相关矩阵，如下：
[0072][0073]
其中，表示k时刻的先验估计值的协方差，即的协方差；p
k-1
表示k-1时刻的后验估计值的协方差，即的协方差(对应的，p
k
表示k时刻的后验估计值协方差，即的协方差)，q表示过程激励噪声协方差，其可通过经验进行设置。
[0074]
s223设置模型的卡尔曼增益，如下：
[0075]
[0076]
其中，k
k
表示卡尔曼增益或卡尔曼系数矩阵；r表示测量噪声协方差，其可通过一般观测得到。
[0077]
s224设置模型的反馈误差，如下：
[0078][0079]
其中，z
k
表示观测值，e表示观测值和预测观测值的残差。
[0080]
s224对模型进行迭代更新，获得最终的融合模型，包括：
[0081]
对误差相关矩阵进行如下更新：
[0082][0083]
其中，i表示单位矩阵。
[0084]
对模型的状态变量进行如下更新：
[0085][0086]
实施例1
[0087]
为验证水下机器人垂向运动的控制感知与融合，本实施例通过机器人的水动力参数构建运动学模型，并通过添加高斯白噪声模拟海洋环境。
[0088]
其中，为了分别研究控制系统的控制效果和对于传感器数据的去噪和信息融合效果，构建了两个不完全相同的状态模型，并采用同一个水下机器人运动模型。通过分别对研究目标进行验证，可以更直观地观察到系统的去噪效果、信息融合后卡尔曼滤波器对单波束高度计数据的修正效果。水下机器人的控制系统对机器人起到的控制作用的精确程度和系统响应时间，可以根据设定不同的期望深度，并对数据进行对比得到。
[0089]
具体的，通过simulink模拟海洋环境中的噪声和水下机器人的传感器数据，对单波束高度计和压力深度计进行信息融合，验证水下机器人在信息融合后的距底高度和深度数据。
[0090]
其过程为：
[0091]
为了更加显著直观地观察去噪部分的效果，首先建立搭载单波束高度计和压力深度计的水下机器人运动模型，设定该水下机器人为一个具有恒定垂向速度的水下机器人，根据传感器数学模型在机器人运动模型中搭载该两种传感器。并设定该水下机器人垂向推力器的正车最大推力为20kgf。
[0092]
考虑到现有技术中自主式水下机器人的最大下潜深度约为6300m，且全世界深度超过6000m的海域只占极少一部分，在初始参数调整中，设定机器人下潜至深度为2025m的海洋环境中。
[0093]
对两种传感器分别添加高斯白噪声。其瞬时值概率密度函数服从高斯分布，同时，基于两种传感器的自身特性不同，对二者添加不同功率的白噪声。其中，对于受到干扰较小，误差幅度较小的压力深度计，添加功率较低的高斯白噪声；而对于容易受到干扰，有可能产生较大误差的单波束高度计添加功率较高的高斯白噪声。
[0094]
基于以上设定获得传感器对特征量测量得到的数据曲线，如添加噪声后传感器数据形成的含噪曲线，可以看出，数据曲线与预期相同，其中单波束高度计受到环境干扰导致的误差明显大于压力深度的误差。为了验证去噪效果需在系统中构建去噪系统，分别对两
种传感器加入低通滤波器。把含噪信号f
noise
作为输入信号，如下：
[0095]
f
noise
(t)＝f(t)+f
whitenoise
(t)。
[0096]
设置低通滤波器的比例因数：
[0097][0098]
其中dt表示本仿真实施例中对传感器和噪声设置的步长，t表示时间常数。根据压力深度计和单波束高度计的数据特性设定步长，计算得到比例因数k，作为常数输入到低通滤波器中。把比例因数k和含噪信号u(t)代入低通滤波器基本公式，进行低通滤波，如下：
[0099]
y(t)＝k*u(t)+(1-k)*y(t-1)。
[0100]
为了对水下机器人的控制系统进行验证，并在验证时有更加直观的效果，本实施例的仿真中选择忽略环境噪声的干扰，使用原始信号f(t)进行模拟。
[0101]
首先在机器人运动模型中添加pid控制系统，把对水下机器人垂向方向的期望位移作为指令输入到pid控制系统中，回代压力深度计的数据形成闭环，进行迭代计算得到如下的偏差函数e(t):
[0102]
e(t)＝r(t)-c(t)，
[0103]
其中，r(t)代表系统期望位移，c(t)代表压力深度计的数据。
[0104]
将得到的期望函数代入到如下的pid控制器基本方程中：
[0105][0106]
其中，控制系统的比例参数k
p
、积分参数k
i
和微分参数k
d
在初始设定时先根据参数在控制系统中起到的作用选择较合理的参数，并可在后续实验中进行调整。u(t)作为控制系统的输出，可作为指令输入到水下机器人的水平舵、螺旋桨等设备中，实现对转速、舵角等进行控制，进而调整水下机器人的位移。
[0107]
本实施例中水下机器人搭载正车最大推力为20kgf的推力器，u(t)作为控制系统的指令函数对推力器发出指令控制水下机器人推力大小。
[0108]
根据水下机器人的水动力系数计算机器人受到的浮力b大小：
[0109]
f＝t-b-ω，
[0110]
其中，f表示水下机器人垂直方向上运动时受到的合力，t表示k时刻推力器对机器人的推力，推力方向沿z轴正方向即深度增加方向为正；ω表示水下机器人受到的由于流体粘性产生的摩擦阻力；b表示水下机器人在海水中受到的浮力大小，方向始终指向z轴负方向，且为根据水下机器人水动力参数计算得到的常数，如下：
[0111]
b＝v*p，
[0112]
其中，v表示水下机器人的排水体积；ρ表示海水密度。
[0113]
本实施例中设置ρ＝1.02g/cm3[0114]
通过在模型中代入海水浮力和控制系统控制推力器输出的推力，可以得到水下机器人在垂向运动方向上所受到的合力。
[0115]
由于推力器正车推力最大为20kgf，应用saturation对输入信号范围进行限制，使输入值在上下限范围内直接输出，而超出上下限时输出20kgf。
[0116]
进一步的，根据牛顿第三定律可知：
[0117]
f＝ma，
[0118]
设定水下机器人初始速度设置为零，由此得到推力器提供给水下机器人的加速度随时间变化的函数，经过两次积分运算就可以得到机器人深度位置。为了验证水下机器人的控制系统，并进行必要的参数整合，运行仿真模型观察水下机器人垂向坐标变化。
[0119]
经过对控制效果的观测，可以发现此时水下机器人不仅超调偏大，而且速度变化较慢，响应时间很长，期望深度16米的情况下在20秒的时间长度内不能完成控制，同时根据图像后半部分发现控制系统仍存在一定的静态误差，必须对pid参数尤其是积分参数进行整合。经过多次调整，可以得到比较合理的参数。
[0120]
在所得参数的基础上，构建本发明的融合模型。并设定水下机器人位于水深为10500m的海洋中，且逐渐下潜至9000m深度，其间经历单波束高度计从无数据到有数据的过程，以单波束高度计数据为0的部分表示超出量程、高度计无数据的情况。
[0121]
海洋噪声协方差可通过经验公式直接得出，由传感器精度导致的噪声协方差设置为厂家数据，均可作为已知项，因此在仿真过程中，可直接根据添加的高斯白噪声单点噪声的概率密度函数服从高斯分布，得到噪声信号协方差矩阵q，如本实施例中设定协方差q＝0.10004，设定估计值初始方差p为1，其值在融合过程中会随时间发生变化，即信息融合产生的对应数据不断与q值叠加。
[0122]
本实施在获得模型的卡尔曼增益时，由于水下机器人垂向运动是连续的，因此可认为基于深度计获得的估计值在k时刻，即作为后验估计的数据与前一时刻即k-1时刻经过卡尔曼滤波融合处理后的数据相对。
[0123]
本实施例中基于已知的高度传感器的基础信息，设置其测量方差为7.5，进一步通过估计值方差和测量值方差根据式(7)获得卡尔曼增益函数。
[0124]
实施例中通过作为观测值的高度数据和作为估计值的基于压力深度计获得的距底深度数据做差，得到噪声数据。
[0125]
本实施例针对垂向的信息融合，因此不需进行矩阵转换，即设置转换矩阵为常数1。获得卡尔曼增益后即能够通过式(8)、(9)最终得到估计方差和高度数据。
[0126]
在有高度计数据的情况下，对水下机器人在每一时刻k获得的两种传感器数据的概率密度函数的方差进行比较，判断真实数据更倾向于那种传感器数据，基于更倾向于真实数据的传感器数据，获得该时刻的机器人距底高度，即融合前信息。
[0127]
同时，对获得的两种传感器数据通过上述具体实施方式所述的数据融合的卡尔曼滤波模型进行融合，获得融合后的机器人距底高度，即融合后信息。
[0128]
融合前信息与融合后信息的对比如附图3所示，可以看出，在高度计量程范围内，此时经过信息融合，数据因外部噪声或传感器精度所造成的误差明显得到缩小，并且数据曲线在信息融合后由离散的单个数据点变为随时间连续的曲线，说明在高度计量程内时，通过卡尔曼滤波技术进行信息融合，不仅可以减小原始数据中的大部分误差(即图中信息融合前数据点振荡程度和信息融合后曲线振荡程度对比后，信息融合后的数据振荡明显较小)，而且可以得到精准的连续评测曲线。
[0129]
当垂向位置超出单波束高度计量程时，此时高度计数据曲线如图4所示，在超量程部分表示为0，在实际应用中，此时不存在高度计数据。此时的高度数据通过压力深度计数
据代入h＝d-d获得。经过本发明的信息融合处理后，得到距底高度数据如附图5。可以看出，在未经信息融合处理的图4中，不仅在超出量程处不存在高度数据的情况，同时由于外部影响或传感器精度影响，所得数据存在明显误差，数据曲线不断振荡而不平滑，与现实中水下机器人进行垂向深度调整时的运动状态显然不符；而在经过信息融合处理的图5中，高度数据在高度计超量程后通过深度计数据计算得到，即此时只需要通过压力深度计进行垂向位置感知即可。
[0130]
可以看出，本发明的方法可使距底高度数据变成连续数据，且在超出单波束高度计量程的情况下依然可以测得水下机器人的距底高度数据。
[0131]
以上实施例仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例。凡属于本发明思路下的技术方案均属于本发明的保护范围。应该指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下的改进和润饰，这些改进和润饰也应视为本发明的保护范围。