(19)中华人民共和国国家知识产权局 (12)发明专利申请 ▣ (10)申请公布号CN104881045A (43)申请公布日2015.09.02 (21)申请号201510337549.4 (22)申请日2015.06.17 (71)申请人中国科学院自动化研究所 地址100080北京市海淀区中关村东路95 勿 (72)发明人喻俊志孙飞虎徐德赵鹏 (74)专利代理机构北京博维知识产权代理事务 所（特殊普通合伙）11486 代理人方振昌 (51)nt.Cl. 605D1/12(2006.01) 权利要求书3页说明书9页附图4页 (54)发明名称 嵌入式视觉引导下仿生机器鱼三维追踪控制 方法 (57)摘要 本发明公开了一种嵌入式视觉引导下仿生机 角度测量 器鱼三维追踪控制方法，首先，步骤1，设置人工 定向 控制器 地标，通过嵌入式视觉系统自主识别人工地标，结 佰高被 开关 合人工地标的尺寸信息，获取目标所在的三维位 模 定深 控制器 置信息：步骤2，获取所述目标所在三维位置信息 的深度，利用基于模糊滑模的控制方法，引导仿生 深度量 机器鱼游动到目标深度并保持在目标深度游动： 视觉翡最 步骤3，所述仿生机器鱼在目标深度保持游动时， 通过多阶段的定向控制，保证仿生机器鱼对目标 的追踪。本发明在机器鱼运动机动灵活的基础上 实现了准确的控制任务。 S50188501
CN104881045A 权利要求书 1/3页 1.一种嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在于，包括以下步 骤： 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中心位置的深度信 息： 步骤2，依据深度信息，基于模糊滑模控制的方法控制仿生机器鱼游动到目标中心所在 深度，并保持仿生机器鱼在目标中心所在的深度游动： 步骤3，在仿生机器鱼保持在目标中心位置所在的深度游动时，通过仿生机器鱼的嵌入 式视觉系统计算偏航角，并采用多阶段的定向控制方法进行目标的追踪。 2.根据权利要求1所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述的目标为具有按照一定规律排列的色块的圆形的人工地标：所述人工地标的中心 位置通过单一颜色的识别及不同颜色的严格拓扑关系进行确定。 3.根据权利要求2所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述人工地标中色块为扇形区域，该扇形区域以人工地标的圆心为圆心、以人工地标的 半径为半径，色块的圆心角之和为360°，不同颜色的色块均与间隔分布。 4.根据权利要求3所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征 在于，以仿生机器鱼起始时刻嵌入式视觉系统的摄像机的镜头光心的位置作为坐标原点建 立惯性坐标系0XYZ,其中0Y轴垂直于水面向下，0Z轴为摄像机的镜头光轴沿仿生机器鱼 头部向前，OX轴由右手定则确定：当仿生机器鱼与人工地标的中心位置不在同一深度时， 仿生机器鱼通过嵌入式视觉系统采集到的人工地标为椭圆形图像，根据色块的拓扑关系 识别出所述椭圆形图像的长轴和短轴：计算长轴两个端点的水平位置坐标分别为(x1,y1)、 (x2,y2),并计算长轴的长度1； 根据小孔成像模型按照如下步骤进行人工地标的中心位置所在深度的计算： 步骤1.1，计算视觉给定的偏航角A: tan(4)=4-4 步骤1.2，计算视觉给定的俯仰角P: tan(p)=-va 其中，A为视觉给定的偏航角，P为视觉给定的俯仰角，(u,v)为人工地标的中心位置 坐标，(u,ⅴ)为嵌入式视觉系统的摄像机镜头光心的图像坐标，(f,f,)为成像平面到相机 坐标系的放大系数： 步骤1.3，计算人工地标的中心位置的深度D: d=1.R= R -x)/2+(0-)f Da=d·tan(P) 其中，d为目标离光心的距离，f为焦距，R为圆形人工地标的直径，D为目标中心所在 深度。 5.根据权利要求4所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 2
CN104881045A 权利要求书 2/3页 于，所述基于模糊滑模控制的方法为：每个采样周期计算人工地标的中心位置的深度D,与 当前的测量深度D.的差值作为深度误差e,依据深度误差e,计算切换函数值s、切换函数 的变化量$，将切换函数值S、切换函数的变化量5输入模糊控制器得到胸鳍攻角的变化率 ,进而通过鱼体波模型调整仿真机器鱼的下潜动作。 6.根据权利要求5所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，切换函数值s和切换函数的变化量5的计算步骤为： 步骤2.1.1，计算滑模面的输入： e(k)=D(k)-D(k) e()=S()e(-1) Ts (6)=9)-ek-1 Ts 其中，Ts是采样周期，￠为深度误差e,的一阶导数，已为深度误差e,的二阶导数： 步骤2.1.2，计算切换函数值s和切换函数的变化量： [s(k)=ce(k)+c(k)+c(k) (k)=s(k)-s(k-1) 其中，c1、c2与c3为常量调节因子。 7.根据权利要求6所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述模糊控制器的设计及胸鳍攻角的变化率“，的计算方法包括如下步骤： 步骤2.2.1，模糊控制器隶属度函数设计：切换函数值s、切换函数的变化量，5、胸鳍攻 角的变化率i分别使用五个、七个、七个模糊语言值，分别用模糊语言值NB、NM、NS、ZE、PS、 PM以及PB代表负大、负中、负小、零、正小、正中以及正大，隶属度函数选用正态分布的函 数： 步骤2.2.2，模糊控制器规则库设计：规则库的设计以满足s对<0的条件，且当s、5模 糊语言值均为NB或PB时，的模糊语言值与s、S的模糊语言值相同，以加快：s心变化效率； 步骤2.2.3，利用重心解模糊方法解算模糊输出： 步骤2.2.4，计算胸鳍的攻角u1(k): 4(k)=4(k-1)+4(k)T。 8.根据权利要求7所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述多阶段定向控制方法包括： 定向运动控制中模糊控制器的设计：分别包括隶属度设计、规则库的设计以及解模糊 方法，其中，隶属度函数采用标准三角函数：解模糊方法采用重心解模糊方法： 采用陀螺仪测量的偏航角度作为负反馈，用以保证控制系统的稳定性；偏航误差的 变化为ec,其公式如下： 3
CN104881045A 权利要求书 3/3页 ec(k)=(k)-e(k-1) Ts 9.根据权利要求1-8任一项所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法， 其特征在于，所述步骤3中的多阶段定向控制过程，包括偏航阶段和直游阶段的控制过程： 所述偏航阶段控制过程，包括如下步骤： 步骤3.1.1，设计鱼体波模型，进而从所述鱼体波模型上增强仿生机器鱼头部的稳定 性； 步骤3.1.2，基于牛顿-欧拉方法对仿生机器鱼的水动力学进行建模： 步骤3.1.3，基于水动力学模型，采用遗传算法进行参数优化以满足机器鱼的头部最小 摆动； 所述直游阶段的控制过程为：以正常游动参数控制鱼体游动，保证直游的快速性。 10.根据权利要求9所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征 在于，所述鱼体波模型为： ax）=时1）-gs =(cx+cax2)sin(kx-wt)+c c xsin(wt) 其中：y为鱼体的横向位移，x为鱼体沿头尾轴的位置，c,为线性鱼体波的振幅包络 线系数，C2为鱼体波的二次项系数，k为波数，o为鱼体波的频率，c3为比例因子： 离散化输入到各个关节的鱼体波信号为： 0=4sin(o+g)+i=1,2,,6 其中，0：表示t时刻输入到第i个舵机的角位移，A:表示第1个振荡器的振幅，9：是 第i个振荡器相对于基准谐波的相位差，i=1,2,.·,4表示尾部舵机，1=5,6代表胸鳍舵 机，并且，51=52=53=ξ4=u2,55=56=u1
CN104881045A 说明书 1/9页 嵌入式视觉引导下仿生机器鱼三维追踪控制方法 技术领域 [0001] 本发明涉及视觉控制领域，尤其是一种嵌入式视觉引导下仿生机器鱼三维追踪控 制方法。 背景技术 [oo02]水下自主追踪，是水下机器人(Autonomous underwater vehicles,AUVs)的重要 任务，具有广泛的应用前景，包括海洋资源勘探、海洋地图构建以及海洋救援等。虽然基于 视觉的追踪容易受到水中光线不均、成像质量低的影响，但是基于视觉的追踪具有成本低 廉、精度较高的优点，仍然引起广泛的关注。 [0003]基于视觉的水下追踪任务，包括构建地图的水下追踪与无地图的水下追踪。构 建地图的方法，适用于精度要求高、环境未知的场合，但是地图构建是计算复杂、资源消耗 大的方案。相关技术人员构建了一种实时的视觉SLAM(simultaneous localization and mapping)地图，克服了水下成像视野受限、成像质量不高的缺点，并且利用视觉显著性方 法实现舰船的检测；技术人员利用SC-PHD(Single cluster probability hypothesis density)滤波方法进行视觉SLAM地图的构建，为水下机器人的导航奠定了基础。无地图的 视觉追踪，适用于环境已知的场合，具有简单、高效的特点。Rodr'iguez-Telles等利用改 进的SLIC(Simple Linear Iterative Clustering)方法进行图像分割，并采用最小邻域分 类器获取目标位置，然后设计鲁棒性的控制策略指引水下机器人的运动：并计了按一定规 律分布的水下光斑以指引水下机器人的入仓。 [0004] 仿生机器鱼是自治水下机器人高机动性运动的典范。通过对真实鱼类游动的模 仿，仿生机器鱼具有高超的游动性能和高效的效率；在机器鱼上增加视觉追踪，推动了自治 水下机器人应对复杂环境、精准控制的任务要求。但是，目前仿生机器鱼上的视觉应用还处 于初级阶段。例如，在一条仿斑点箱鲀(Spotted boxfish)的机器鱼上实现基于视觉的平 面跟随：又如，在一条多控制面的仿生机器鱼上实现了平面内的视觉追踪控制：再如设计 了单关节的小型机器鱼，实现了基于视觉信息的位置预测。 发明内容 [0005]本发明的目的是设计一种嵌入式视觉引导下仿生机器鱼三维追踪控制方法，在己 知环境中实现仿生机器鱼对目标的自主识别与追踪。 [0006] 本发明采用的技术方案为：一种嵌入式视觉引导下仿生机器鱼三维追踪控制方 法，包括以下步骤： [0007] 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中心位置的深度 信息： [0008] 步骤2，依据深度信息，基于模糊滑模控制的方法控制仿生机器鱼游动到目标中心 所在深度，并保持仿生机器鱼在目标中心所在的深度游动： [0009] 步骤3，在仿生机器鱼保持在目标中心位置所在的深度游动时，通过仿生机器鱼的 5