CN112578389B - 一种多源融合的rov实时路径规划方法 - Google Patents

Abstract

本发明提出了一种多源融合的ROV实时路径规划方法，基于水下定位系统和环扫声呐信息，包括以下步骤：S1.针对水下运动目标的运动特征，建立目标运动状态模型，通过该模型描述动态障碍物的动态变化，探测动态障碍物实时的绝对位置；S2.利用扩展卡尔曼滤波对动态障碍物的绝对位置进行估计，预测动态障碍物的位置信息；S3.建立模糊神经网络控制器，实现ROV的实时路径规划，对动态障碍物的精准避障。本发明融合了水下定位系统和环扫声呐的信息，能探测到动态障碍物实时的绝对位置，通过扩展卡尔曼滤波进行位置估计，可精准预测动态障碍物的位置信息，再通过模糊神经网络的避障算法，实现对动态障碍物的精准避障。

Description

一种多源融合的ROV实时路径规划方法

【技术领域】

本发明涉及水下机器人控制的技术领域，特别是一种基于水下定位系统和环扫声呐信息融合的实时路径规划方法。

【背景技术】

21世纪是一个人类探测、开发以及可持续发展海洋资源的世纪，海洋资源中包涵了丰富的石油资源、天然气资源、生物资源、化学资源等。随着开发海洋资源的力度不断加大，人类越来越意识到智能水下设备的重要性，能够精准探测水下复杂环境以及自主完成水下作业的水下机器人成为世界各国企业及教育机构重点研究的课题。

ROV(Remote Operated Vehicle遥控无人潜水器)的实时路径规划是安全、高效完成水下作业的重要保证。在ROV航行的过程中，可能会遇到动态的障碍物，这就要求ROV能够迅速的对障碍物的精确位置以及未来状态做出准确判断，并且实时规划好安全合理路径，做出有效的避障反应。国内外众多学者对水下机器人实时路径规划方法展开了研究，提出了很多有效的方法，例如人工势场法、A*算法、神经网络、模糊逻辑等。它们对局部的环境有较强的的感知能力，但是对于动态障碍物而言，分析其绝对位置信息和潜在碰撞风险区能力较差，不能保证避开动态障碍物的准确性。

【发明内容】

本发明的目的就是解决现有技术中的问题，提出一种多源融合的ROV实时路径规划方法，可精准预测动态障碍物的位置信息，实现对动态障碍物的精准避障。

为实现上述目的，本发明提出了一种多源融合的ROV实时路径规划方法，基于水下定位系统和环扫声呐信息，包括以下步骤：

S1.针对水下运动目标的运动特征，建立目标运动状态模型，通过该模型描述动态障碍物的动态变化，探测动态障碍物实时的绝对位置；

S2.利用扩展卡尔曼滤波对动态障碍物的绝对位置进行估计，预测动态障碍物的位置信息；

S3.建立模糊神经网络控制器，实现ROV的实时路径规划，对动态障碍物的精准避障。

作为优选，S1具体包括以下步骤：

S1.1：采用随机运动模型描述障碍物的动态变化，用概率分布函数来估算：

a(t)＝β*w(t) (1)

其中w(t)是一个随机向量，a(t)为加速度；

S1.2：把概率分布函数假设为高斯分布函数或者联合分布函数，则分布函数可以表示为：

prob(w(t))＝f_p(μ(w(t)),σ²(w(t))) (2)

其中μ(w(t)),σ²(w(t))分别为均值和方差向量；

S1.3：障碍物的速度计算公式：

v(t)＝v(t-Δt)+∫_-Δtw(τ)dτ (3)

S1.4：随机运动模型可描述为：

a(t)＝α*a(t-Δt)+β*w(t) (4)

其中α为保障原有加速度的权值，β为随机向量w(t)的权值；

S1.5：由此，速度公式可表示为：

v(t)＝v(t-Δt)+∫_-Δt(α*a(τ-Δt)+β*w(τ))d(τ) (5)

S1.6：建立直角坐标系，将速度分解为x方向和y方向：

v_x(t)＝v(t)*cosθ (6)

v_y(t)＝v(t)*sinθ (7)

其中θ为运动方向与x方向的夹角；

S1.7：将速度在x方向和y方向上积分，可分别求出x方向和y方向上的位移：

作为优选，S2具体包括以下步骤：

S2.1：将系统的状态转移方程表示为：

θ(k)＝f(θ_k-1)+S_k (10)

S2.2：量测方程表示为：

Z(k)＝h(θ_k)+V_k (11)

S2.3：用泰勒展开式对(10)式在上一次的估值<θ_k-1>处展开得：

θ_k＝f(θ_k-1)+S_k＝f(<θ_k-1>)+F_k-1(θ_k-1-<θ_k-1>)+S_k (12)

其中F_k-1表示函数θ_k在<θ_k-1>处的雅克比矩阵；

S2.4：用泰勒展开式对(11)式在本轮的状态预测值θ_k'处展开得：

Z_k＝h(θ_k)+V_k＝h(θ_k')+H_k(θ_k-θ_k')+V_k (13)

其中H_k表示函数h(θ)在θ_k'处的雅克比矩阵；

S2.5：给出扩展卡尔曼滤波的预测方程：

θ_k'＝f(<θ_k-1>) (14)

其中雅克比矩阵F_k-1为

S2.6：给出扩展卡尔曼滤波的更新方程：

<θ_k>＝θ'_k+K'_k(Z_k-h(θ'_k)) (18)

∑_k＝(I-K'_kH_k)∑'_k (19)

其中雅克比矩阵H_k为

S2.7：通过扩展卡尔曼滤波分别预测出x方向和y方向上的位移Sx和Sy。

作为优选，S3具体包括以下步骤：

S3.1：输入输出变量：输入变量为Sx，Sy和艇首指向

输出变量为ROV下一步的速度V和转向角增量Δθ；

S3.2：模糊化输入输出变量，并选取对应的隶属度函数；

S3.3：根据环境分类，建立完整的模糊推理规则；

S3.4：按照“if-then”的形式进行模糊推理：

其中

为模糊系统的模糊集，

为模糊参数，y_i为根据模糊规则得到的输出，k为输入参数个数；

S3.5：计算各输入变量的隶属度：

其中

分别为隶属度函数的中心和宽度；

S3.6：将各隶属度进行模糊计算，采用模糊算子为连乘算子：

其中n为模糊子集数；

S3.7：根据模糊计算结果计算模糊型的输出值：

S3.8：采用重心法进行反模糊化：

S3.9：设计一个三输入、两输出的网络结构形式，共有五层，分别为输入层、前提层、规则层、结论层、输出层；

S3.10：模糊神经网络的学习，需学习神经网络系数

以及第二层隶属度函数中心值

和宽度

S3.11：系数和参数修正：

其中e为期望输出和实际输出的误差，y_d为网络期望输出，y_e为网络实际输出，

为神经网络的系数，α为网络学习率，x_j为网络输入参数。

作为优选，S3.2具体包括：Sx和Sy按论域被划分为很近、近、中等、远、很远，隶属度函数为高斯函数；

按论域被划分为负大、负小、零、正小、正大，隶属度函数为高斯函数；V按论域被划分为慢、中、快，隶属度函数为高斯函数；Δθ按论域被划分为负大、负小、零、正小、正大，隶属度函数为高斯函数。

本发明融合了水下定位系统和环扫声呐的信息，能探测到动态障碍物实时的绝对位置，通过扩展卡尔曼滤波进行位置估计，可精准预测动态障碍物的位置信息，再通过模糊神经网络的避障算法，实现对动态障碍物的精准避障。

本发明的有益效果：

1.利用动态障碍物的绝对位置信息做估测，可提高对障碍物位置描述的精度；

2.模糊神经网络输入和输出参数选择更优化，提高了实时路径规划的有效性。

本发明的特征及优点将通过实施例结合附图进行详细说明。

【附图说明】

图1为水下机器人在某一时刻的状态图；

图2为模糊神经网络的结构图；

图3是实施例1中X,Y方向轨迹预测结果的仿真图；

图4是实施例1中X,Y方向轨迹预测误差的仿真图；

图5是实施例1中真实轨迹和预测轨迹对比仿真图；

图6是实施例2中X,Y方向轨迹预测结果的仿真图；

图7是实施例2中X,Y方向轨迹预测误差的仿真图；

图8是实施例2中轨迹预测真实轨迹和预测轨迹对比仿真图；

图9为障碍物水平向右移动状态的避障仿真图，其中，1为本算法，2为传统算法；

图10为障碍物水平向左移动状态的避障仿真图，其中，1为本算法，2为传统算法。

【具体实施方式】

参阅图1至图2，本实施例的方法针对某一深度水面内的ROV实时路径规划问题做出解答。

步骤1:针对水下运动目标的运动特征，建立适用的目标运动状态模型。本方法采用随机运动模型来描述障碍物的动态变化，可用一些概率分布的函数来估算:

a(t)＝β*w(t) (1)

其中w(t)是一个随机向量，a(t)为加速度；

步骤2:把概率分布函数假设为高斯分布函数或者联合分布函数，则分布函数可以表示为:

prob(w(t))＝f_p(μ(w(t)),σ²(w(t))) (2)

其中μ(w(t)),σ²(w(t))分别为均值和方差向量；

步骤3:障碍物的速度计算公式:

v(t)＝v(t-Δt)+∫_-Δtw(τ)dτ (3)

步骤4:随机运动模型可描述为:

a(t)＝α*a(t-Δt)+β*w(t) (4)

其中α为保障原有加速度的权值，β为随机向量w(t)的权值；

步骤5:由此，速度公式可表示为:

v(t)＝v(t-Δt)+∫_-Δt(α*a(τ-Δt)+β*w(τ))d(τ) (5)

步骤6:建立直角坐标系，将速度分解为x方向和y方向:

v_x(t)＝v(t)*cosθ (6)

v_y(t)＝v(t)*sinθ (7)

其中θ为运动方向与x方向的夹角；

步骤7:将速度在x方向和y方向上积分，可分别求出x方向和y方向上的位移:

步骤8:利用扩展卡尔曼滤波(EKF)对动态障碍物的绝对位置进行估计，将系统的状态转移方程表示为:

θ(k)＝f(θ_k-1)+S_k (10)

步骤9:量测方程表示为:

Z(k)＝h(θ_k)+V_k (11)

步骤10:用泰勒展开式对(10)式在上一次的估值<θ_k-1>处展开得:

θ_k＝f(θ_k-1)+S_k＝f(<θ_k-1>)+F_k-1(θ_k-1-<θ_k-1>)+S_k (12)

其中F_k-1表示函数θ_k在<θ_k-1>处的雅克比矩阵；

步骤11:用泰勒展开式对(11)式在本轮的状态预测值θ_k'处展开得:

Z_k＝h(θ_k)+V_k＝h(θ_k')+H_k(θ_k-θ_k')+V_k (13)

其中H_k表示函数h(θ)在θ_k'处的雅克比矩阵；

步骤12:给出EKF的预测方程:

θ_k'＝f(<θ_k-1>) (14)

其中雅克比矩阵F_k-1为

步骤13:给出EKF的更新方程:

<θ_k>＝θ'_k+K'_k(Z_k-h(θ'_k)) (18)

∑_k＝(I-K'_kH_k)∑'_k (19)

其中雅克比矩阵H_k为

步骤14:通过EKF分别预测出x方向和y方向上的位移Sx和Sy；

步骤15:建立模糊神经网络控制器，实现ROV的实时路径规划；首先输入输出变量:输入变量为Sx，Sy和艇首指向

输出变量为ROV下一步的速度V和转向角增量Δθ；

步骤16:模糊化输入输出变量，并选取对应的隶属度函数:Sx和Sy按论域被划分为很近、近、中等、远、很远，隶属度函数为高斯函数；

按论域被划分为负大、负小、零、正小、正大，隶属度函数为高斯函数；V按论域被划分为慢、中、快，隶属度函数为高斯函数；Δθ按论域被划分为负大、负小、零、正小、正大，隶属度函数为高斯函数；

步骤17:根据环境分类，建立完整的模糊推理规则；

步骤18:按照“if-then”的形式进行模糊推理:

其中

为模糊系统的模糊集，

为模糊参数，y_i为根据模糊规则得到的输出，k为输入参数个数；

步骤19:计算各输入变量的隶属度:

其中

分别为隶属度函数的中心和宽度；

步骤20:将各隶属度进行模糊计算，采用模糊算子为连乘算子:

其中n为模糊子集数；

步骤21:根据模糊计算结果计算模糊型的输出值:

步骤22:采用重心法进行反模糊化:

步骤23:设计一个三输入、两输出的网络结构形式，共有五层，分别为输入层、前提层、规则层、结论层、输出层；

步骤24:模糊神经网络的学习，需学习神经网络系数

以及第二层隶属度函数中心值

和宽度

步骤25:系数和参数修正:

其中e为期望输出和实际输出的误差，y_d为网络期望输出，y_e为网络实际输出，

为神经网络的系数，α为网络学习率，x_j为网络输入参数。

实施例1

参阅图3-图5，目标初始坐标为(0，0)，0-20个节拍上在x方向上速度为0m/s，y方向上速度为0.1m/s做匀速运动，20-40个节拍上在x方向上以0.01m/s^2做匀加速运动。

在y方向上以0.01m/s^2做匀加速运动，40-60个节拍上在x方向上以0.015m/s^2做匀加速运动，在y方向上以-0.01m/s^2做匀加速运动，其余时间做匀速直线运动。

采样时间为2S，使用多次仿真(如N取为10)时，横、纵轴的估计方差分别为0.0054、0.0050。

实施例2

参阅图6-图8，目标初始坐标为(0，0)，在x方向上做速度为Vx＝0.2sin(pi*t/60)的正弦运动，在y方向上做速度为Vy＝0.2cos(pi*t/60)的余弦运动。

采样周期为2S，使用多次仿真(如N取为10)时，横、纵轴的估计方差分别为0.0053、0.0047。

传统算法采用模糊神经网络避障算法，是通过将声呐前方180°范围分成5个方向，左，左前，中，右前，右5个方向，分别将5个方向的测得障碍物的距离做为模糊神经网络的输入，还有一个输入参数即为障碍物的运动方向分为向左和向右，一共6输入，2输出参数和本方法一样。参阅图9和图10，可以看出采用本发明的方法避障的精准度明显高于传统算法的精准度。

上述实施例是对本发明的说明，不是对本发明的限定，任何对本发明简单变换后的方案均属于本发明的保护范围。

Claims (3)

1.一种多源融合的ROV实时路径规划方法，基于水下定位系统和环扫声呐信息，其特征在于：包括以下步骤：

S1.针对水下运动目标的运动特征，建立目标运动状态模型，通过该模型描述动态障碍物的动态变化，探测动态障碍物实时的绝对位置；

S2.利用扩展卡尔曼滤波对动态障碍物的绝对位置进行估计，预测动态障碍物的位置信息；

S3.建立模糊神经网络控制器，实现ROV的实时路径规划，对动态障碍物的精准避障；

S1具体包括以下步骤：

S1.1：采用随机运动模型描述障碍物的动态变化，用概率分布函数来估算：

a(t)＝β*w(t) (1)

其中w(t)是一个随机向量，a(t)为加速度；

S1.2：把概率分布函数假设为高斯分布函数或者联合分布函数，则分布函数可以表示为：

prob(w(t))＝f_p(μ(w(t)),σ²(w(t))) (2)

其中μ(w(t)),σ²(w(t))分别为均值和方差向量；

S1.3：障碍物的速度计算公式：

v(t)＝v(t-Δt)+∫_-Δtw(τ)dτ (3)

S1.4：随机运动模型可描述为：

a(t)＝α*a(t-Δt)+β*w(t) (4)

其中α为保障原有加速度的权值，β为随机向量w(t)的权值；

S1.5：由此，速度公式可表示为：

v(t)＝v(t-Δt)+∫_-Δt(α*a(τ-Δt)+β*w(τ))d(τ) (5)

S1.6：建立直角坐标系，将速度分解为x方向和y方向：

v_x(t)＝v(t)*cosθ (6)

v_y(t)＝v(t)*sinθ (7)

其中θ为运动方向与x方向的夹角；

S1.7：将速度在x方向和y方向上积分，可分别求出x方向和y方向上的位移：

S2具体包括以下步骤：

S2.1：将系统的状态转移方程表示为：

θ(k)＝f(θ_k-1)+S_k (10)

S2.2：量测方程表示为：

Z(k)＝h(θ_k)+V_k (11)

S2.3：用泰勒展开式对(10)式在上一次的估值<θ_k-1>处展开得：

θ_k＝f(θ_k-1)+S_k＝f(<θ_k-1>)+F_k-1(θ_k-1-<θ_k-1>)+S_k (12)

其中F_k-1表示函数θ_k在<θ_k-1>处的雅克比矩阵；

S2.4：用泰勒展开式对(11)式在本轮的状态预测值θ_k'处展开得：

Z_k＝h(θ_k)+V_k＝h(θ_k')+H_k(θ_k-θ_k')+V_k (13)

其中H_k表示函数h(θ)在θ_k'处的雅克比矩阵；

S2.5：给出扩展卡尔曼滤波的预测方程：

θ_k'＝f(<θ_k-1>) (14)

其中雅克比矩阵F_k-1为

S2.6：给出扩展卡尔曼滤波的更新方程：

<θ_k〉＝θ′_k+K′_k(Z_k-h(θ′_k)) (18)

Σ_k＝(I-K′_kH_k)Σ′_k (19)

其中雅克比矩阵H_k为

S2.7：通过扩展卡尔曼滤波分别预测出x方向和y方向上的位移Sx和Sy。

2.如权利要求1所述的一种多源融合的ROV实时路径规划方法，其特征在于：

S3具体包括以下步骤：

S3.1：输入输出变量：输入变量为Sx，Sy和艇首指向

输出变量为ROV下一步的速度V和转向角增量Δθ；

S3.2：模糊化输入输出变量，并选取对应的隶属度函数；

S3.3：根据环境分类，建立完整的模糊推理规则；

S3.4：按照“if-then”的形式进行模糊推理：

其中

为模糊系统的模糊集，

为模糊参数，y_i为根据模糊规则得到的输出，k为输入参数个数；

S3.5：计算各输入变量的隶属度：

其中

分别为隶属度函数的中心和宽度；

S3.6：将各隶属度进行模糊计算，采用模糊算子为连乘算子：

其中n为模糊子集数；

S3.7：根据模糊计算结果计算模糊型的输出值：

S3.8：采用重心法进行反模糊化：

S3.9：设计一个三输入、两输出的网络结构形式，共有五层，分别为输入层、前提层、规则层、结论层、输出层；

S3.10：模糊神经网络的学习，需学习神经网络系数

以及第二层隶属度函数中心值

和宽度

S3.11：系数和参数修正：

其中e为期望输出和实际输出的误差，y_d为网络期望输出，y_e为网络实际输出，

为神经网络的系数，α为网络学习率，x_j为网络输入参数。

3.如权利要求2所述的一种多源融合的ROV实时路径规划方法，其特征在于：

S3.2具体包括：Sx和Sy按论域被划分为很近、近、中等、远、很远，隶属度函数为高斯函数；

按论域被划分为负大、负小、零、正小、正大，隶属度函数为高斯函数；V按论域被划分为慢、中、快，隶属度函数为高斯函数；Δθ按论域被划分为负大、负小、零、正小、正大，隶属度函数为高斯函数。

Images