嵌入式视觉引导下仿生机器鱼三维追踪控制方法
【技术领域】
[0001] 本发明设及视觉控制领域，尤其是一种嵌入式视觉引导下仿生机器鱼=维追踪控 制方法。
【背景技术】
[0002] 水下自主追踪，是水下机器人（Autonomousunderwatervehicles,AUVs)的重要 任务，具有广泛的应用前景，包括海洋资源勘探、海洋地图构建W及海洋救援等。虽然基于 视觉的追踪容易受到水中光线不均、成像质量低的影响，但是基于视觉的追踪具有成本低 廉、精度较高的优点，仍然引起广泛的关注。
[0003] 基于视觉的水下追踪任务，包括构建地图的水下追踪与无地图的水下追踪。构 建地图的方法，适用于精度要求高、环境未知的场合，但是地图构建是计算复杂、资源消耗 大的方案。相关技术人员构建了一种实时的视觉SLAM(simultaneouslocalizationand mapping)地图，克服了水下成像视野受限、成像质量不高的缺点，并且利用视觉显著性方 法实现舰船的检测；技术人员利用SC-P皿(Singleclusterprob油ilityhypothesis density)滤波方法进行视觉SLAM地图的构建，为水下机器人的导航奠定了基础。无地图的 视觉追踪，适用于环境已知的场合，具有简单、高效的特点。Ro化'iguez-Telles等利用改 进的化IC(SimpleLinearIterativeClustering)方法进行图像分割，并采用最小邻域分 类器获取目标位置，然后设计鲁椿性的控制策略指引水下机器人的运动；并计了按一定规 律分布的水下光斑W指引水下机器人的入仓。
[0004] 仿生机器鱼是自治水下机器人高机动性运动的典范。通过对真实鱼类游动的模 仿，仿生机器鱼具有高超的游动性能和高效的效率；在机器鱼上增加视觉追踪，推动了自治 水下机器人应对复杂环境、精准控制的任务要求。但是，目前仿生机器鱼上的视觉应用还处 于初级阶段。例如，在一条仿斑点箱鈍（Spottedboxfish)的机器鱼上实现基于视觉的平 面跟随；又如，在一条多控制面的仿生机器鱼上实现了平面内的视觉追踪控制；再如设计 了单关节的小型机器鱼，实现了基于视觉信息的位置预测。
【发明内容】
[0005] 本发明的目的是设计一种嵌入式视觉引导下仿生机器鱼=维追踪控制方法，在已 知环境中实现仿生机器鱼对目标的自主识别与追踪。
[0006] 本发明采用的技术方案为；一种嵌入式视觉引导下仿生机器鱼=维追踪控制方 法，包括W下步骤：
[0007] 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中屯、位置的深度 f目息；
[000引步骤2,依据深度信息，基于模糊滑模控制的方法控制仿生机器鱼游动到目标中屯、 所在深度，并保持仿生机器鱼在目标中屯、所在的深度游动；
[0009] 步骤3,在仿生机器鱼保持在目标中屯、位置所在的深度游动时，通过仿生机器鱼的 嵌入式视觉系统计算偏航角，并采用多阶段的定向控制方法进行目标的追踪。
[0010] 进一步地，所述的目标为具有按照一定规律排列的色块的圆形的人工地标；所述 人工地标的中屯、位置通过单一颜色的识别及不同颜色的严格拓扑关系进行确定。
[0011] 进一步地，所述人工地标中色块为扇形区域，该扇形区域W人工地标的圆屯、为圆 屯、、W人工地标的半径为半径，色块的圆屯、角之和为360°，不同颜色的色块均与间隔分布。
[0012] 进一步地，W仿生机器鱼起始时刻嵌入式视觉系统的摄像机的镜头光屯、的位置作 为坐标原点建立惯性坐标系OXYZ，其中OY轴垂直于水面向下，OZ轴为摄像机的镜头光轴沿 仿生机器鱼头部向前，OX轴由右手定则确定；当仿生机器鱼与人工地标的中屯、位置不在同 一深度时，仿生机器鱼通过嵌入式视觉系统采集到的人工地标为楠圆形图像，根据色块的 拓扑关系识别出所述楠圆形图像的长轴和短轴；计算长轴两个端点的水平位置坐标分别为 (X。yi)、（X2,y2)，并计算长轴的长度1 ;
[0013] 根据小孔成像模型按照如下步骤进行人工地标的中屯、位置所在深度的计算：
[0014] 步骤1. 1，计算视觉给定的偏航角Ad:
[0015]
[0016] 步骤1. 2,计算视觉给定的俯仰角Pd:
[0017]
[0018] 其中，Ad为视觉给定的偏航角，Pd为视觉给定的俯仰角，（U，V)为人工地标的中屯、 位置坐标，（u"，v。）为嵌入式视觉系统的摄像机镜头光屯、的图像坐标，为成像平面到 相机坐标系的放大系数；
[0019] 步骤1. 3,计算人工地标的中屯、位置的深度Dd:
[0020]
[002U Dd=d*tan(Pd)
[002引其中，d为目标离光屯、的距离，f为焦距，R为圆形人工地标的直径，Dd为目标中屯、 所在深度。
[0023] 进一步地，所述基于模糊滑模控制的方法为：每个采样周期计算人工地标的中屯、 位置的深度Dd与当前的测量深度D。的差值作为深度误差e1，依据深度误差ei计算切换函 数值S、切换函数的变化量i，将切换函数值S、切换函数的变化量i输入模糊控制器得到胸 罐攻角的变化率，进而通过鱼体波模型调整仿真机器鱼的下潜动作。
[0024] 进一步地，切换函数值S和切换函数的变化量i的计算步骤为：
[0025] 步骤2. 1. 1，计算滑模面的输入：
[0026]
[0027]其中，Ts是采样周期，^为深度误差ei的一阶导数，g为深度误差ei的二阶导数； [002引步骤2. 1. 2,计算切换函数值S和切换函数的变化量占：
[0029]
[0030] 其中，Cl、C2与C3为常量调节因子。
[0031] 进一步地，所述模糊控制器的设计及胸罐攻角的变化率的计算方法包括如下步 骤：
[0032] 步骤2. 2. 1，模糊控制器隶属度函数设计：切换函数值S、切换函数的变化量、胸 罐攻角的变化率早分别使用五个、走个、走个模糊语言值，分别用模糊语言值NB、醒、NS、ZE、 PS、PM化及PB代表负大、负中、负小、零、正小、正中化及正大，隶属度函数选用正态分布的 函数；
[0033] 步骤2. 2. 2,模糊控制器规则库设计；规则库的设计W满足柏<0的条件，且当S、 討莫糊语言值均为NB或PB时，til的模糊语言值与S、i的模糊语言值相同，W加快?yi变化效 率.
[0034] 步骤2. 2. 3,利用重屯、解模糊方法解算模糊输出；
[003引步骤2. 2. 4,计算胸罐的攻角Ui似：
[0036]
[0037] 进一步地，所述多阶段定向控制方法包括：
[003引定向运动控制中模糊控制器的设计；分别包括隶属度设计、规则库的设计W及解 模糊方法，其中，隶属度函数采用标准=角函数；解模糊方法采用重屯、解模糊方法；
[0039] 采用巧螺仪测量的偏航角度作为负反馈，用W保证控制系统的稳定性；偏航误差 62的变化为ec，其公式如下；
[0040]
〇
[0041] 进一步地，所述步骤3中的多阶段定向控制过程，包括偏航阶段和直游阶段的控 制过程；
[0042] 所述偏航阶段控制过程，包括如下步骤：
[0043] 步骤3. 1. 1，设计鱼体波模型，进而从所述鱼体波模型上增强仿生机器鱼头部的稳 定性；
[0044] 步骤3. 1. 2,基于牛顿-欧拉方法对仿生机器鱼的水动力学进行建模；
[0045] 步骤3. 1. 3,基于水动力学模型，采用遗传算法进行参数优化W满足仿生机器鱼的 头部最小摆动；
[0046] 所述直游阶段的控制过程为；W正常游动参数控制鱼体游动，保证直游的快速性。
[0047] 进一步地，所述鱼体波模型为：
[0048]
[0049] 其中；ybody为鱼体的横向位移，X为鱼体沿头尾轴的位置，C1为线性鱼体波的振幅 包络线系数，C2为鱼体波的二次项系数，k为波数，《为鱼体波的频率，C3为比例因子； [0化0] 离散化输入到各个关节的鱼体波信号为：
[0化1]
[005引其中，0康示t时刻输入到第i个驼机的角位移，A康示第i个振荡器的振幅， 巧是第i个振荡器相对于基准谐波的相位差，i= 1，2, ..，4表示尾部驼机，i= 5, 6代表 胸罐驼机，并且，写1=I2= ^ 3= ^ 4=。2，写5= ^ 6=Ui。
[0化3] 采用上述技术方案，产生的技术效果有：本发明针对基于嵌入式视觉的仿生机器 鱼的=维追踪控制，在机器鱼运动机动灵活的基础上实现准确的控制任务。提出了基于视 觉的=维追踪控制系统，将=维追踪划分为定深控制与定向控制W尽量减小控制量的禪 合，保证控制的平滑稳定。利用了模糊滑模控制方法，实现无精确模型指导、干扰较大环境 下的深度控制；多阶段的定向控制中，将连续的基于图像伺服控制转化为多阶段的偏航反 馈控制，降低控制的复杂度。并且，通过对深度控制的评估控制定向控制的引入，实现深度 控制与定向控制的切换。
【附图说明】
[0054] 图1是基于嵌入式视觉的仿生机器鱼S维追踪示意图；
[0化5]图2是基于嵌入式视觉的S维追踪的控制系统框图；
[0化6] 图3是S维定位的人工地标示意图；
[0化7]图4是基于视觉的S维定位示意图；
[005引图5是基于模糊滑模的定深运动控制框图；
[0化9]图6a是模糊控制的