基于距离与视差信息的水下机器人避障方法
【技术领域】
[0001] 本发明涉及水下机器人智能控制技术领域，尤其是一种融合距离与视差信息的水 下机器人避障方法。
【背景技术】
[0002] 水下机器人在海难搜救、水下防入侵技术、物种跟踪探测等领域具有广泛的应用 价值。在水下环境中，沉船等沉积物以及礁石等使得水下环境非常复杂，如果水下机器人不 能主动、有效地躲避这些障碍，将有可能导致水下机器人执行任务的失败甚至导致水下机 器人损坏。
[0003] 在现有技术中检索发现，中国专利申请号为201310488141. 8,名称为：基于前视 声纳的水下机器人避碰方法，该方法前视声纳图像数据引入机器人避障策略中，可以减小 机器人避碰盲区。但上述方法在设计无碰撞轨迹规划算法时，并没有考虑水下机器人的控 制输入约束。在实际应用中，水下机器人一般都是由电机驱动，而电机驱动器都有功率限 制，当输入信号超过这一限制，饱和发生，进而使系统的动态性能降低（例如，超调量、调节 时间以及震荡次数等），甚至导致闭环系统不稳定。
[0004] 再有，中国专利申请号为201210539473.X，名称为：一种智能水下机器人的自主 避障方法，该方法将水下机器人运动目标、障碍物和水下机器人控制性能进行统一考虑来 实现避障任务。上述方法是基于距离信息设计的，但仅考虑距离信息是不够的，因为实时有 效的避障行为不仅需要距离信息，还需要视角信息，例如人们在规避障碍的过程中，有效 的避障路线是通过距离与视差调整来共同确立的。在水下三维环境中，方向性显得更为重 要。因此，如何在水下复杂障碍物环境下，利用距离和视差信息对水下机器人设计无碰撞轨 迹优化策略，以提高水下机器人轨迹规划的有效性，显得尤为重要。
【发明内容】
[0005] 本发明目的在于提供一种可动态调整壁障策略、路径更优、躲避效果好、可靠性高 的基于距离与视差信息的水下机器人避障方法。
[0006] 为实现上述目的，采用了以下技术方案：
[0007] 本发明主要包括水下机器人、探测声呐、前向红外摄像头、侧向红外摄像头以及尾 部红外摄像头，在水下机器人上安装探测声呐，在水下机器人的前端对称安装两个前向红 外摄像头，在水下机器人顶部两侧位置分别安装侧向红外摄像头，在水下机器人尾部对称 安装尾部红外摄像头，所述避障方法包括以下步骤：
[0008] (1)根据水下机器人与目标点的位置，以及海图数据库信息离线规划出水下机器 人的全局参考轨迹，设定如下代价函数使得水下机器人在无障碍情况下能够移动到目标点
[0009]
【主权项】
1. 一种基于距离与视差信息的水下机器人避障方法，包括水下机器人、探测声呐、前向 红外摄像头、侧向红外摄像头以及尾部红外摄像头，在水下机器人上安装探测声呐，在水下 机器人的前端对称安装两个前向红外摄像头，在水下机器人顶部两侧位置分别安装侧向红 外摄像头，在水下机器人尾部对称安装两个尾部红外摄像头，其特征在于，所述避障方法包 括以下步骤： (1) 根据水下机器人与目标点的位置，以及海图数据库信息离线规划出水下机器人的 全局参考轨迹，设定如下代价函数使得水下机器人在无障碍情况下能够移动到目标点
式中，《>0与U>0为比例因子；
为参考轨迹，qQ(t)GR3与
分别表示参考轨迹的位置与速度向量； 所述水下机器人视为一个质量为m的质点，质点位置为水下机器人中心点，质点在 空间坐标系中有三个自由度，即q(t) = [x(t)，y(t)，z(t)]T;通过反馈线性化，水下机 器人的运动学方程可以用二阶积分器模型表示，其中速度与控制输入向量分别表示为
与u(t) =[ux(t)，uy(t)，uz(t) ]T;控制输入约束表示为-Uxmin<ux(t )<Uxmax、_Uymin< U y(t) < 1^眶与_U zmin< U z (t) < Uzmax，其中Uxmin>0、Uxmax>0、Uymin>0、Uymax>0、 uzmin>〇与uzniax>0为输入饱和常数； (2) 探测声呐作为全局障碍测量仪器，监测水下机器人周边远距离环境信息，获取水下 机器人与障碍物的相对距离信息； 红外摄像头作为局部障碍测量仪器，监测水下机器人周边近距离环境信息，获取近距 离障碍物的视角信息； (3) 判断是否需要避障，如果需要避障，根据相对距离信息，利用预测控制调整避障策 略，构造基于预测控制的优化算法；如果不需要避障则按照原路线行驶； (4) 若水下机器人到达目标点，则轨迹规划结束，完成任务；若水下机器人未到达目标 点，则重新执行步骤（2)。
2. 根据权利要求1所述的基于距离与视差信息的水下机器人避障方法，其特征在于， 所述步骤（3)中的避障策略如下： 首先，定义障碍物集合；设定障碍物的空间范围可通过多个球的交集/并集来表示；利 用球的叠加，最终描述出来的障碍物区域为不规则形状或规则形状；因此，第m(m= 1，…） 个障碍物包含在一个二维圆内，用符号Bm(Om,pm)表不，其中Om(OmGR3)与pm(pm>〇)分 别表示此球的中心与半径，进而水下机器人与障碍物m的避碰约束为||q-〇m| |>Pm+ds; 其次，划分障碍物所处的区域；设定d为障碍物与水下机器人表面最近距离，dsaft为避 障警戒距离，4为避障远距离，ds为避障近距离，d ; 当(1>4^时，障碍物处于警戒线之外，不需要躲避障碍； 当dsafe>心屯时，障碍物处于远距离避障区域，水下机器人利用探测声呐反馈的距离 信息进行避障，构造基于距离信息的势能函数
其中时，水下机器人可在不与障碍物发生碰撞前提下沿着障碍物影响域半径 移动； 当士多心屯时，障碍物处于近距离避障区域，水下机器人利用红外摄像头反馈的视差 信息进行避障，构造视差函数作为预测控制算法的终点状态控制器，前向红外摄像头 对方形障碍物的视角差为0i，侧向红外摄像头对不规则形障碍物的视角差为0 2，水下机 器人运动方向角度为a，考虑水下机器人的运动方向，对视差进行校正，得出新的视差^与 4，
构造类似于
的视差函数以作为终点状态控制器，其中k^X)为 加权值； 融合距离与视差信息构造避障函数，
，作为预测 控制算法的终点状态控制器；其中dsaft多d>ds。
3.根据权利要求1所述的基于距离与视差信息的水下机器人避障方法，其特征在于： 所述的预测控制的优化算法，在任意采样时刻tk，求解涉及状态约束和控制约束的有限时 域开环优化问题得到一组控制序列u%得出预测时域内的假定输出，并将当前的控制输入 作用于被控对象；在下一采样时刻tk+1，基于新的初始状态重复上一时刻的优化过程，不断 滚动优化，从而形成闭环控制，完成无碰撞轨迹优化任务。
【专利摘要】一种基于距离与视差信息的水下机器人避障方法，包括以下步骤：控制装置根据作业任务和海图数据库信息对水下机器人进行全局轨迹规划；探测声呐与摄像头分别作为全局与局部障碍测量仪器，以获取水下机器人与障碍物的相对距离与视差信息；控制装置进而判断是否需要避障，利用预测控制调整避障策略；若水下机器人达到目标点，则轨迹规划结束。本发明具有可动态调整壁障策略、路径更优、躲避效果好、可靠性高等优点，能够应对复杂障碍，提高了水下机器人执行任务的可靠性。
【IPC分类】G05D1-10
【公开号】CN104571128
【申请号】CN201410827493
【发明人】闫敬, 杨晛, 关新平, 罗小元, 华长春
【申请人】燕山大学
【公开日】2015年4月29日
【申请日】2014年12月26日