基于视觉信息丢失的机器鱼运动控制算法

2020-11-10 02:09谢景鹏李宗刚杜亚江葛立明

水下无人系统学报 2020年5期

谢景鹏，李宗刚，杜亚江，葛立明

(1.兰州交通大学机电工程学院，甘肃兰州，730070；2.兰州交通大学机器人研究所，甘肃兰州，730070)

0 引言

随着海洋经济在世界经济的占比不断提高，水下仿生机器人成为研究的热点之一。在游动机理、仿生机构设计，运动控制方法以及与环境的交互技术等方面取得了显著进展[1]。为实现水下仿生机器人的自主运动，水下运动目标识别也成为重要研究内容之一[2-3]。

目前，水下机器人自主作业面临着巨大挑战，相较陆地，水下环境光线较弱，因此，水下环境中机器人目标识别问题就显得尤为重要。目标识别主要包含以下的几个阶段：一是数据预处理[4]，二是图像特征提取[5]，三是分类特征选择[6]。现阶段的目标检测和图像识别主要使用卷积神经网络来进行[7]。

为了实现水下机器人的自主控制[8]，需要解决水中目标的识别与跟踪问题，很多学者针对这一问题做了大量研究。文献[9]实现主动视觉跟踪，使得机器鱼能够在二维平面上自由游动。文献[10]主要研究面向机器鱼的多目标跟踪系统，可以即时跟踪多个目标，是多机器鱼实现群体协作、队形控制的基础。

文中针对一个领导者机器鱼引导多个跟随者机器鱼向期望位置移动过程中存在的视觉信息丢失问题进行研究。通过对机器鱼图像状态识别采用二次滤波法进行判定，使用基于半闭环控制的直线追踪法和圆弧切线法实现对机器鱼的运动控制。

1 问题描述

设系统由N(N≥ 2)个机器鱼组成，其中1 个机器鱼为领航者，其余机器鱼作为跟随者。领航者通过图像识别判断跟随者机器鱼的位置和运动状态从而对跟随者进行运动控制。文中主要研究当领航者机器鱼由于外在因素丢失图像信息时，跟随者根据之前的控制信息自主预测完成领航者机器鱼发布的任务。文中所使用的机器鱼为三关节仿生机器鱼，如图1 所示。由于实验条件限制，摄像头看作领航者，三关节仿生机器鱼看作跟随者。

图1 机器鱼Fig.1 Robotic fish

2 视觉信息丢失时目标机器鱼状态判别

2.1 基于二次滤波法的机器鱼状态判定

领航者机器鱼识别到执行者机器鱼有正反2种状态。机器鱼状态识别正反判断如图2 所示。

图2 机器鱼状态识别判定Fig.2 Judgment of robotic fish state recognition

图中：φ1为上一通信模块发送周期机器鱼中心位置到当前周期机器鱼中心位置向量的角度；φ2为当前周期机器鱼的方向角度，σ=φ2-φ1为角度差。通信模块发送周期ΔT取50 ms。在正常的游动中，机器鱼处于反选状态时，。然而，实验中对实时数据分析后，发现在正常的游动中，存在数据的突变情况，故需将这种情况的信息滤掉。文中采用基于事件频率的二次滤波法对数据进行实时滤波，图3 为数据处理流程。

图3 数据处理流程图Fig.3 Flow chart of data processing

2.2 视觉信息丢失时机器鱼的运动模型

视觉信息丢失时机器鱼的轨迹模型见图4。

图4 机器鱼轨迹模型Fig.4 Trajectory model of robotic fish

将曲线轨迹微分为多边形，由图4 可知，存在

式中：α′为微分后的其中一段轨迹对应的圆心角；α′′为机器鱼当前周期方向与上一周期的偏差。设当前周期数为i，θi表示当前周期机器鱼预测方向与目标轨迹方向的理论夹角，ωk和vk分别表示当前周期内机器鱼的角速度和线速度，其中k表示档位，k=1，2，3，…，15；Yi为机器鱼与目标轨迹径向偏差；Xi表示机器鱼在目标轨迹上的投影位置。则

a，b，c，d，e，f分别表示1 个动作周期内，机器鱼方向档位4，5，7，9，10，7 对应的执行周期个数。也就是说1 个动作周期是由多个方向档位共同作用完成，每个档位的作用时间除以ΔT就是执行周期个数。则动作指令矩阵可表示为

2.2.1 基于半闭环控制的直线追踪法

当平台状态识别丢失瞬间，鱼头方向与追踪轨迹方向夹角-20°＜β0＜20°时，采用半闭环控制的直线追踪法实现对机器鱼的运动控制。控制框图如图5 所示。

图5 直线追踪法控制框图Fig.5 The control block diagram of straight-line tracking method

采用该方法的机器鱼轨迹如图6 所示。该控制方法实质上是控制每个动作周期βj_1 和βj_2在15°左右，且抑制轨迹振幅在一定范围内。在第j个动作周期内，机器鱼半周期出射角用βj_1 表示，全周期出射角用βj_2 表示。Ⅰ、Ⅲ阶段结束时，机器鱼与追踪轨迹的径向偏差用Yj_chu1、Yj_chu2 表示。Ⅱ、Ⅳ阶段结束时，机器鱼与追踪轨迹的径向偏差用Yj_ru1、Yj_ru2 表示。βj_1′、βj_2′、Yj_ru1′、Yj_ru2′分别表示各量的期望值。上述变量均为理论计算值。以第j个动作周期为例，动作指令矩阵为

针对前半周期，设ZG1 为Ⅰ阶段10 档位作用时间的理论值。Fix()为向下取整函数，Mod()为取余函数，round(fj,0)表示将fj的值四舍五入到整数位。d j，e j，fj的计算过程如式(12)，具体取值如表1 所示。

图6 直线追踪法路径规划Fig.6 Path planning of straight-line tracking method

表1 dj，ej，fj 取值Table 1 The value of dj,ej,fj

计算Ⅱ阶段结束时的机器鱼位姿状态

针对后半周期，设ZG2 为Ⅲ阶段5 档位作用时间的理论值。aj，b j，cj的计算如下，具体取值如表2 所示。

表2 aj, bj, cj 取值Table 2 The value of aj,bj,cj

根据上述模型，可以推导出对于n个动作周期，相应的每半个周期的角度变化量矩阵为

对于第j个动作周期，结合式(15)，则半周期入射角βj_1，全周期出射角βj_2 为

2.2.2 圆弧切线法

当平台状态识别丢失瞬间，鱼头方向与追踪轨迹的方向夹角β0＜-20°或β0＞20°时，采用圆弧切线法实现对机器鱼的转弯控制。机器鱼路径规划如图7 所示。其中，F(x f，yf)和φ分别为机器鱼识别状态丢失瞬间鱼头点坐标和鱼头方向。B(xb，yb)为目标点坐标，O(x0，y0)为单一档位作用下机器鱼所绕圆心，D(x d，yd)表示直线BD与圆轨迹的切点，R表示圆轨迹的半径，由β0的大小决定，α为机器鱼实走圆弧所对应的圆心角。

O(x0，y0)坐标如下，其中φ＜0时，h=1；φ＞ 0时，h=2。

图7 圆弧切线法路径规划Fig.7 Path planning of circle tangent method

设过点B(xb，yb)的直线BD表达式如下，根据点到直线距离公式，存在以下关系

式中：φ＜0 时，k取最大值；φ＞0 时，k取最小值。设Tk表示单一档位作用下，机器鱼游动一周的时间，则圆心角α和时间tn可表示为

机器鱼到达D点后，采用上述直线追踪法实现对轨迹2 的追踪，从而实现机器鱼的直游控制。

3 仿真验证

3.1 二次滤波法仿真

现将实验中采集到的σ数据作为模型输入。其中，σ为φ2与φ1的角度差，φ1为上一通信模块发送周期的机器鱼中心位置到当前周期的机器鱼中心位置的向量角度，φ2为当前周期机器鱼的方向角度。利用MATLAB 进行输入输出波形仿真。如图8 所示，图中0 为反选，1 为正选。其中蓝色波形表示输入的原数据，红色波形表示采用二次滤波法滤波后的输出数据。由图可知，机器鱼在第3 s 之前处于正选状态，在3 s 之后处于反选状态。

3.2 基于半闭环控制的直线追踪法仿真

图8 二次滤波法仿真Fig.8 Simulation of secondary filtering method

当视觉信息丢失发生，且鱼头方向与追踪轨迹方向夹角-20°＜β0＜20°时，机器鱼的速度取v4=7.35 px/ms，v5=7.53 px/ms，v7=9.09 px/ms，v9=8.43px/ms，v10=9.06 px/ms；角速度取ω4=0.094 rad/ms，ω5=0.066 rad/ms，ω9=0.039 rad/ms，ω10=0.058 rad/ΔT。则随着β0的变化，机器鱼运动过程中各曲线如图9 所示。图9(a)和(b)分别表示β0=-5°、-15°、-20°、5°、15°和20°时，机器鱼预测实时方向与追踪轨迹的角度偏差变化曲线。由图可知，曲线振幅维持在-15°～15°之间，曲线周期维持在1 s 左右。由于输入偏角β0的不同，导致各曲线初值不同，且存在一定相位差，但曲线最终都趋于稳态。图9(c)、(d)分别为β0=-5 °、-15°、-2 0°、5°、15°和20°时，机器鱼预测实时位置与追踪轨迹的径向偏差曲线。由图可知，曲线振幅维持在-8～8 px 之间，曲线周期维持在1 s 左右。由于输入偏角β0的不同，导致各曲线初值不同，且存在一定相位差，但曲线均能在1 s 左右都趋于稳态。

3.3 圆弧切线法仿真

当视觉信息丢失发生且鱼头方向与追踪轨迹方向夹角β0＜-2 0°或β0＞-20°时，机器鱼的速度v4，v5，v7，v9，v10取值以及机器鱼角速度ω4，ω5，ω7，ω9，ω10取值与同3.2 节。目标点B取xb=650 px，yb=300 px，视觉信息丢失时，图10(a)中的鱼头点F取xf=200 px，yf=350 px，图10(b)中的鱼头点F取xf=200 px，yf=100 px，则仿生机器鱼随着β0的变化，运动过程中各变化曲线如图 10(a)和(b)所示。图 10(a)表示β0=-5 0°、-100°、-150°时，机器鱼预测实时方向与追踪轨迹2 的角度偏差变化曲线，图10(b)表示β0=50 °、100°、150°时，机器鱼预测实时方向与追踪轨迹2 的角度偏差变化曲线。

由图可知，不同的输入偏角β0，会导致各曲线初值不同，每条曲线前期均有一段斜率稳定的直线，表示机器鱼正在以恒角速度进行转弯，转弯时间随着的增加依次增加。转弯完成后，机器鱼进入直线运动状态，且该部分曲线最终趋于稳态，曲线振幅维持在-15°～15°之间，曲线周期维持在1 s 左右。

图9 直线追踪法仿真曲线Fig.9 Simulation curves of straight-line tracking method

图10 圆弧切线法仿真曲线Fig.10 Simulation curves of circle tangent method

4 实物实验

实验环境为3 m×2 m×0.3 m 的室内水池，水池左上角为坐标原点，仿生机器鱼根据不同实验需要布置在相应位置。红色立柱表示每个实验中设立的目标点位置。实验环境如图11 所示。

4.1 正反状态判定策略实验

图11 实验环境Fig.11 Experiment environment

实验中，仿生机器鱼初始位置在水池左上角随机布置。实验中机器鱼的目标点B为xb=645 px，yb=380 px 。图12(a)中，机器鱼刚开始处于正选状态，向目标点运动，6 s 后被人为标定为反选状态。图12(b)中，机器鱼0 s 后一直标定为反选状态。图12(c)给出了2 次实验中，机器鱼各自的轨迹图像。由图可知，相对于未采用判定策略前，采用判定策略的机器鱼能够识别自身反选状态并做出补偿，进而不影响其向目标点的运动。

图12 判定策略验证实验Fig.12 Verification experiment of judgment strategy

4.2 基于半闭环控制的直线追踪法实验

实验中，仿生机器鱼初始位置在水池左侧随机布置。设定机器鱼的目标点B为xb=645 px，yb=235 px。实体环境中机器鱼的各变化曲线如图13(b)和(c)所示。实验程序设定第2 s 后机器鱼处于视觉信息丢失阶段，测得β0=-7.65°。

图13(b)为机器鱼实时方向与追踪轨迹的角度偏差变化，曲线的振幅为 -1 5°～15 °，曲线周期为1 s 左右，且曲线围绕着纵坐标为0 的直线上下振荡，趋于稳态，与图9(a)仿真结果一致。图13(c)为机器鱼实时位置与追踪轨迹的径向偏差变化，曲线的振幅基本为-8～8 px，曲线周期为1 s 左右，且曲线围绕着纵坐标为0 的直线上下振荡，趋于稳态，与图9(c)仿真结果一致。

4.3 圆弧切线法实验

图13 直线追踪法实验Fig.13 Experiment of straight-line tracking method

实验中，仿生机器鱼初始位置在水池右侧上部随机布置。设定机器鱼的目标点B为xb=420 px，yb=315 px。第0 s 开始，机器鱼处于视觉信息丢失阶段，测得β0=149.8°。实验截图与曲线如图14 所示。图14(b)为机器鱼实时方向与追踪轨迹2 的角度偏差变化，机器鱼在0～3.5 s 阶段，处于恒角速度转弯阶段，从图中可以看出机器鱼在3.5 s 以后进入直线运动状态，该阶段曲线振幅为-1 5 °～ 15°，曲线周期为1 s 左右，且趋于稳态，与图10(b)仿真结果一致。

图14 圆弧切线法实验Fig.14 Experiment of circle tangent method

5 结束语

文中提出了机器鱼在视觉信息丢失情况下的运动控制问题，针对这个问题，提出了一种运动补偿控制算法。该算法由3 个部分组成，分别是基于二次滤波法的状态判定，基于半闭环控制的直线追踪法和圆弧切线法。所提算法经过仿真、实验的论证，均表明可以解决机器鱼在视觉丢失的情况下的运动控制问题。需要说明的是，该算法在实体实验中还存在一定的不足。在后续工作中会对这3 种算法合成一种算法并且在户外环境中对所提算法进行试验验证和改进。