CN104571128A - 基于距离与视差信息的水下机器人避障方法 - Google Patents

Abstract

一种基于距离与视差信息的水下机器人避障方法，包括以下步骤：控制装置根据作业任务和海图数据库信息对水下机器人进行全局轨迹规划；探测声呐与摄像头分别作为全局与局部障碍测量仪器，以获取水下机器人与障碍物的相对距离与视差信息；控制装置进而判断是否需要避障，利用预测控制调整避障策略；若水下机器人达到目标点，则轨迹规划结束。本发明具有可动态调整壁障策略、路径更优、躲避效果好、可靠性高等优点，能够应对复杂障碍，提高了水下机器人执行任务的可靠性。

Description

基于距离与视差信息的水下机器人避障方法

技术领域

本发明涉及水下机器人智能控制技术领域，尤其是一种融合距离与视差信息的水下机器人避障方法。

背景技术

水下机器人在海难搜救、水下防入侵技术、物种跟踪探测等领域具有广泛的应用价值。在水下环境中，沉船等沉积物以及礁石等使得水下环境非常复杂，如果水下机器人不能主动、有效地躲避这些障碍，将有可能导致水下机器人执行任务的失败甚至导致水下机器人损坏。

在现有技术中检索发现，中国专利申请号为201310488141.8，名称为：基于前视声纳的水下机器人避碰方法，该方法前视声纳图像数据引入机器人避障策略中，可以减小机器人避碰盲区。但上述方法在设计无碰撞轨迹规划算法时，并没有考虑水下机器人的控制输入约束。在实际应用中，水下机器人一般都是由电机驱动，而电机驱动器都有功率限制，当输入信号超过这一限制，饱和发生，进而使***的动态性能降低(例如，超调量、调节时间以及震荡次数等)，甚至导致闭环***不稳定。

再有，中国专利申请号为201210539473.X，名称为：一种智能水下机器人的自主避障方法，该方法将水下机器人运动目标、障碍物和水下机器人控制性能进行统一考虑来实现避障任务。上述方法是基于距离信息设计的，但仅考虑距离信息是不够的，因为实时有效的避障行为不仅需要距离信息，还需要视角 信息，例如人们在规避障碍的过程中，有效的避障路线是通过距离与视差调整来共同确立的。在水下三维环境中，方向性显得更为重要。因此，如何在水下复杂障碍物环境下，利用距离和视差信息对水下机器人设计无碰撞轨迹优化策略，以提高水下机器人轨迹规划的有效性，显得尤为重要。

发明内容

本发明目的在于提供一种可动态调整壁障策略、路径更优、躲避效果好、可靠性高的基于距离与视差信息的水下机器人避障方法。

为实现上述目的，采用了以下技术方案：

本发明主要包括水下机器人、探测声呐、前向红外摄像头、侧向红外摄像头以及尾部红外摄像头，在水下机器人上安装探测声呐，在水下机器人的前端对称安装两个前向红外摄像头，在水下机器人顶部两侧位置分别安装侧向红外摄像头，在水下机器人尾部对称安装尾部红外摄像头，所述避障方法包括以下步骤：

(1)根据水下机器人与目标点的位置，以及海图数据库信息离线规划出水下机器人的全局参考轨迹，设定如下代价函数使得水下机器人在无障碍情况下能够移动到目标点

$E\left(t\right)=\mathrm{\ω}{\left|\right|q\left(t\right)-q_0\left(t\right)\left|\right|}^2+\mathrm{\υ}{\left|\right|\stackrel{\·}{q}\left(t\right)-{\stackrel{\·}{q}}_0\left(t\right)\left|\right|}^2,$

式中，ω>0与υ>0为比例因子；为参考轨迹，q₀(t)∈R³与分别表示参考轨迹的位置与速度向量；

所述水下机器人视为一个质量为m的质点，质点位置为水下机器人中心点， 质点在空间坐标系中有三个自由度，即q(t)＝[x(t),y(t),z(t)]^T；通过反馈线性化，水下机器人的运动学方程可以用二阶积分器模型表示，其中速度与控制输入向量分别表示为与u(t)＝[u_x(t),u_y(t),u_z(t)]^T；控制输入约束表示为-u_xmin≤u_x(t)≤u_xmax、-u_ymin≤u_y(t)≤u_ymax与-u_zmin≤u_z(t)≤u_zmax，其中u_xmin>0、u_xmax>0、u_ymin>0、u_ymax>0、u_zmin>0与u_zmax>0为输入饱和常数；

(2)探测声呐作为全局障碍测量仪器，监测水下机器人周边远距离环境信息，获取水下机器人与障碍物的相对距离信息；

红外摄像头作为局部障碍测量仪器，监测水下机器人周边近距离环境信息，获取近距离障碍物的视角信息；

(3)判断是否需要避障，如果需要避障，根据相对距离信息，利用预测控制调整避障策略，构造基于预测控制的优化算法；如果不需要避障则按照原路线行驶；

(4)若水下机器人到达目标点，则轨迹规划结束，完成任务；若水下机器人未到达目标点，则重新执行步骤(2)。

进一步的，步骤(3)中的避障策略如下：

首先，定义障碍物集合；设定障碍物的空间范围可通过多个球的交集/并集来表示；利用球的叠加，最终描述出来的障碍物区域为不规则形状或规则形状；因此，第m(m＝1,…)个障碍物包含在一个二维圆内，用符号B_m(O_m,ρ_m)表示，其中O_m(O_m∈R³)与ρ_m(ρ_m>0)分别表示此球的中心与半径，进而水下机器人与障碍物m的避碰约束为||q-O_m||>ρ_m+d_s；

其次，划分障碍物所处的区域；设定d为障碍物与水下机器人表面最近距离，d_safe为避障警戒距离，d_l为避障远距离，d_s为避障近距离，d_safe>d_l>d_s>0；

当d>d_safe时，障碍物处于警戒线之外，不需要躲避障碍；

当d_safe≥d>d_l时，障碍物处于远距离避障区域，水下机器人利用探测声呐反馈的距离信息进行避障，构造基于距离信息的势能函数

$E_{\mathrm{obs}}^d=\frac{{\left(\right||q-O_m||-d_l-{\mathrm{\ρ}}_m)}^2}{\left|\right|q-O_m\left|\right|-{\mathrm{\ρ}}_m},$

其中时，水下机器人可在不与障碍物发生碰撞前提下沿着障碍物影响域半径移动；

当d_l≥d>d_s时，障碍物处于近距离避障区域，水下机器人利用红外摄像头反馈的视差信息进行避障，构造视差函数作为预测控制算法的终点状态控制器，前向红外摄像头对方形障碍物的视角差为θ₁，侧向红外摄像头对不规则形障碍物的视角差为θ₂，水下机器人运动方向角度为α，考虑水下机器人的运动方向，对视差进行校正，得出新的视差与

构造类似于的视差函数以作为终点状态控制器，其中k_obs>0为加权值；最小化视角函数，最终理想的角度为水下障碍物最终偏离红外摄像头的中心点，实现对障碍物的主动规避；

融合距离与视差信息构造避障函数，作为预测控制算法的终点状态控制器；其中d_safe≥d>d_s；在避障区域内，随着障 碍物趋近水下机器人，基于视差的避障居主导作用，相反地，随着障碍物远离水下机器人，基于距离的避障居主导作用。

进一步的，所述的预测控制的优化算法，在任意采样时刻t_k，求解涉及状态约束和控制约束的有限时域开环优化问题得到一组控制序列u^*，得出预测时域内的假定输出，并将当前的控制输入作用于被控对象；在下一采样时刻t_k+1，基于新的初始状态重复上一时刻的优化过程，不断滚动优化，从而形成闭环控制，完成无碰撞轨迹优化任务。

与现有技术相比，本发明具有如下优点：

1、水下机器人可以根据与障碍物的相对距离和视角信息，动态调整避障策略，能够更好地反映水下障碍物的信息，路径更优、躲避障碍效果更好；

2、在控制算法中采用了预测控制，可以方便地将控制输入约束和状态约束结合到控制器的设计中，能有效避免水下机器人陷入输入饱和状态，实现了水下机器人控制输入约束与避障策略的无缝融合。

附图说明

图1是本发明方法的避障流程图。

图2是本发明方法中水下机器人的避障区域划分效果图。

图3是本发明方法中水下机器人对水下障碍物的视角构造俯视图。

附图标号：1-水下机器人、2-探测声呐、3-前向红外摄像头、4-侧向红外摄像头、5-尾部红外摄像头、6-方形障碍物、7-不规则障碍物。

具体实施方式

下面结合附图对本发明做进一步说明：

本发明主要包括水下机器人1、探测声呐2、前向红外摄像头3、侧向红外摄像头4，以及尾部红外摄像头5，在水下机器人上安装探测声呐，在水下机器人的前端对称安装两个前向红外摄像头，在水下机器人顶部两侧位置分别安装侧向红外摄像头，尾部对称安装两个尾部红外摄像头，所述避障方法包括以下步骤：

如图1所示的本发明方法的避障流程图中；

(1)根据水下机器人与目标点的位置，以及海图数据库信息离线规划出水下机器人的全局参考轨迹，很容易得出，最理想的参考轨迹为水下机器人到目标点之间的线段。但考虑到水下环境的复杂性，例如固定礁石等阻挡，需要综合考虑海图数据库信息，进而设计一条从水下机器人起始点到目标点最近的一条线路作为参考轨迹，并设定如下代价函数使得水下机器人在无障碍情况下能够移动到目标点

$E\left(t\right)=\mathrm{\ω}{\left|\right|q\left(t\right)-q_0\left(t\right)\left|\right|}^2+\mathrm{\υ}{\left|\right|\stackrel{\·}{q}\left(t\right)-{\stackrel{\·}{q}}_0\left(t\right)\left|\right|}^2,$

式中，ω>0与υ>0为比例因子；为参考轨迹，q₀(t)∈R³与分别表示参考轨迹的位置与速度向量；

所述水下机器人视为一个质量为m的质点，质点位置为水下机器人中心点，质点在空间坐标系中有三个自由度，即q(t)＝[x(t),y(t),z(t)]^T；通过反馈线性化，水下机器人的运动学方程可以用二阶积分器模型表示，其中速度与控制输入(即加速度)向量分别表示为与u(t)＝[u_x(t),u_y(t),u_z(t)]^T；控制输入约束表示为-u_x min≤u_x(t)≤u_x max、-u_y min≤u_y(t)≤u_y max与-u_z min≤u_z(t)≤u_z max，其中 u_x min>0、u_x max>0、u_y min>0、u_y max>0、u_z min>0与u_z max>0为输入饱和常数；

(2)上述全局轨迹规划是在已知海图数据库信息基础上离线得出的，但是在复杂动态变化的水下环境中，海图信息有可能发生了很大的变化，例如沉船沉积以及礁石移动等，导致全局路径规划方法计算出来的路径很有可能是被堵死。因此，水下机器人需要在全局轨迹规划基础上根据水下环境的变化动态调整轨迹。进而，探测声呐作为全局障碍测量仪器，监测水下机器人周边远距离环境信息，获取水下机器人与障碍物的相对距离信息；红外摄像头作为局部障碍测量仪器，监测水下机器人周边近距离环境信息，获取近距离障碍物的视角信息；

(3)判断是否需要避障，如果需要避障，根据相对距离信息，利用预测控制调整避障策略，构造基于预测控制的优化算法；如果不需要避障则按照原路线行驶；

首先，定义障碍物集合；设定障碍物的空间范围可通过多个球的交集/并集来表示；利用球的叠加，最终描述出来的障碍物区域为不规则形状或规则形状；因此，第m(m＝1,…)个障碍物包含在一个二维圆内，用符号B_m(O_m,ρ_m)表示，其中O_m(O_m∈R³)与ρ_m(ρ_m>0)分别表示此球的中心与半径，进而水下机器人与障碍物m的避碰约束为||q-O_m||>ρ_m+d_s；

其次，当水下机器人感知到障碍物存在时，需要判断障碍物所处的区域；设定d为障碍物与水下机器人表面最近距离，d_safe为避障警戒距离，d_l为避障远距离，d_s为避障近距离，d_safe>d_l>d_s>0；

如图2所示，当d>d_safe时，障碍物处于警戒线之外，不需要躲避障碍；

当d_safe≥d>d_l时，障碍物处于远距离避障区域，水下机器人利用探测声呐反馈的距离信息进行避障，构造基于距离信息的势能函数

$E_{\mathrm{obs}}^d=\frac{{\left(\right||q-O_m||-d_l-{\mathrm{\ρ}}_m)}^2}{\left|\right|q-O_m\left|\right|-{\mathrm{\ρ}}_m},$

其中时，水下机器人可在不与障碍物发生碰撞前提下沿着障碍物影响域半径移动；

当d_l≥d>d_s时，障碍物处于近距离避障区域，水下机器人利用红外摄像头反馈的视差信息进行避障，构造视差函数作为预测控制算法的终点状态控制器，前向红外摄像头(安装在水下机器人前方位置)对方形障碍物6的视角差为θ₁，侧向红外摄像头(安装在水下机器人顶部两侧位置)对不规则形障碍物7的视角差为θ₂，水下机器人运动方向角度为α，考虑水下机器人的运动方向，对视差进行校正，得出新的视差与

构造类似于的视差函数以作为终点状态控制器，其中k_obs>0为加权值；可以看出，如果最小化视角函数，最终理想的角度为水下障碍物最终偏离红外摄像头的中心点，相对应地水下机器人可以实现对障碍物的主动规避；

融合距离与视差信息构造避障函数，作为预测控制算法的终点状态控制器；其中d_safe≥d>d_s；在避障区域内，随着障 碍物趋近水下机器人，基于视差的避障居主导作用，相反地，随着障碍物远离水下机器人，基于距离的避障居主导作用。

当d_s≥d>0时，为极度危险区域，此时需要在上述避障策略驱使下，避免障碍物进入区域。

最后构造基于预测控制的优化算法，在任意采样时刻t_k，求解涉及状态约束(避障控制约束)和控制约束(输入饱和约束)的有限时域开环优化问题得到一组控制序列u^*，得出预测时域内的假定输出，并将当前的控制输入作用于被控对象；在下一采样时刻t_k+1，基于新的初始状态重复上一时刻的优化过程，不断滚动优化，从而形成闭环控制，完成无碰撞轨迹优化任务。

实施例一：采样周期为δ>0，预测步长为预测时域为T＝N_rδ，预测时刻为t_k＝t₀+kδ，其中预测控制问题构建如下：

在更新时刻t_k，对于水下机器人：给定当前状态z(t_k)与预测时域内的控制输入其中τ∈[t_k,t_k+T]，求解类似如下最优控制问题

$J^{*}(t_k,z^{*}\left(t_k\right),u^{*}(\·))=\underset{u(\·)}{\min }J(z\left(t_k\right),u(\·))$

s.t.***模型，避碰约束与输入饱和约束

进而，可得出水下机器人的最优控制输入，进而得出最优输出状态。

(4)若水下机器人到达目标点，则轨迹规划结束，完成任务；若水下机器人未到达目标点，则重新执行步骤(2)，继续动态优化轨迹。

本发明不局限于上述的优选实施方式，任何人应该得知在本发明的启示下做出的结构变化，凡是与本发明具有相同或者相近似的技术方案，均属于本发 明的保护范围。

Claims (3)

1.一种基于距离与视差信息的水下机器人避障方法，包括水下机器人、探测声呐、前向红外摄像头、侧向红外摄像头以及尾部红外摄像头，在水下机器人上安装探测声呐，在水下机器人的前端对称安装两个前向红外摄像头，在水下机器人顶部两侧位置分别安装侧向红外摄像头，在水下机器人尾部对称安装两个尾部红外摄像头，其特征在于，所述避障方法包括以下步骤：

(1)根据水下机器人与目标点的位置，以及海图数据库信息离线规划出水下机器人的全局参考轨迹，设定如下代价函数使得水下机器人在无障碍情况下能够移动到目标点

$E\left(t\right)=\mathrm{\ω}{\left|\right|q\left(t\right)-q_0\left(t\right)\left|\right|}^2+\mathrm{\υ}{\left|\right|\stackrel{.}{q}\left(t\right)-{\stackrel{.}{q}}_0\left(t\right)\left|\right|}^2,$

式中，ω>0与υ>0为比例因子；为参考轨迹，q₀(t)∈R³与分别表示参考轨迹的位置与速度向量；

所述水下机器人视为一个质量为m的质点，质点位置为水下机器人中心点，质点在空间坐标系中有三个自由度，即q(t)＝[x(t),y(t),z(t)]^T；通过反馈线性化，水下机器人的运动学方程可以用二阶积分器模型表示，其中速度与控制输入向量分别表示为与u(t)＝[u_x(t),u_y(t),u_z(t)]^T；控制输入约束表示为-u_xmin≤u_x(t)≤u_xmax、-u_ymin≤u_y(t)≤u_ymax与-u_zmin≤u_z(t)≤u_zmax，其中u_xmin>0、u_xmax>0、u_ymin>0、u_ymax>0、u_zmin>0与u_zmax>0为输入饱和常数；

(2)探测声呐作为全局障碍测量仪器，监测水下机器人周边远距离环境信息，获取水下机器人与障碍物的相对距离信息；

红外摄像头作为局部障碍测量仪器，监测水下机器人周边近距离环境信息，获取近距离障碍物的视角信息；

(3)判断是否需要避障，如果需要避障，根据相对距离信息，利用预测控制调整避障策略，构造基于预测控制的优化算法；如果不需要避障则按照原路线行驶；

(4)若水下机器人到达目标点，则轨迹规划结束，完成任务；若水下机器人未到达目标点，则重新执行步骤(2)。

2.根据权利要求1所述的基于距离与视差信息的水下机器人避障方法，其特征在于，所述步骤(3)中的避障策略如下：

首先，定义障碍物集合；设定障碍物的空间范围可通过多个球的交集/并集来表示；利用球的叠加，最终描述出来的障碍物区域为不规则形状或规则形状；因此，第m(m＝1,…)个障碍物包含在一个二维圆内，用符号B_m(O_m,ρ_m)表示，其中O_m(O_m∈R³)与ρ_m(ρ_m>0)分别表示此球的中心与半径，进而水下机器人与障碍物m的避碰约束为||q-O_m||>ρ_m+d_s；

其次，划分障碍物所处的区域；设定d为障碍物与水下机器人表面最近距离，d_safe为避障警戒距离，d_l为避障远距离，d_s为避障近距离，d_safe>d_l>d_s>0；

当d>d_safe时，障碍物处于警戒线之外，不需要躲避障碍；

当d_safe≥d>d_l时，障碍物处于远距离避障区域，水下机器人利用探测声呐反馈的距离信息进行避障，构造基于距离信息的势能函数

$E_{\mathrm{obs}}^d=\frac{{\left(\right||q-O_m||-d_l-{\mathrm{\ρ}}_m)}^2}{\left|\right|q-O_m\left|\right|-{\mathrm{\ρ}}_m},$

其中时，水下机器人可在不与障碍物发生碰撞前提下沿着障碍物影响域半径移动；

当d_l≥d>d_s时，障碍物处于近距离避障区域，水下机器人利用红外摄像头反馈的视差信息进行避障，构造视差函数作为预测控制算法的终点状态控制器，前向红外摄像头对方形障碍物的视角差为θ₁，侧向红外摄像头对不规则形障碍物的视角差为θ₂，水下机器人运动方向角度为α，考虑水下机器人的运动方向，对视差进行校正，得出新的视差与

构造类似于的视差函数以作为终点状态控制器，其中k_obs>0为加权值；

融合距离与视差信息构造避障函数，作为预测控制算法的终点状态控制器；其中d_safe≥d>d_s。

3.根据权利要求1所述的基于距离与视差信息的水下机器人避障方法，其特征在于：所述的预测控制的优化算法，在任意采样时刻t_k，求解涉及状态约束和控制约束的有限时域开环优化问题得到一组控制序列u^*，得出预测时域内的假定输出，并将当前的控制输入作用于被控对象；在下一采样时刻t_k+1，基于新的初始状态重复上一时刻的优化过程，不断滚动优化，从而形成闭环控制，完成无碰撞轨迹优化任务。