嵌入式视觉引导下仿生机器鱼三维追踪控制方法

嵌入式视觉引导下仿生机器鱼三维追踪控制方法
【技术领域】
[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是模糊控制的 输入S的隶属度函数；
[0060] 图化是模糊控制的输入i的隶属度函数；
[0061] 图6c是模糊控制的输出^^1的隶属度函数；
[0062] 图7是基于模糊的定向运动控制框图。
【具体实施方式】
[0063] 为使本发明的目的、技术方案和优点更加清楚明白，W下结合具体实施例，并参照 附图，对本发明进一步详细说明。
[0064] 如图1所示，W仿生机器鱼起始时刻的位置作为坐标原点建立惯性坐标系0XYZ， 其中OY轴垂直于水面向下，OZ轴沿着仿生机器鱼起始时刻的尾头轴向前，ox轴由右手定则 确定。整个追踪控制分为视觉定位、定深控制、定向控制。首先，通过嵌入式视觉系统计算 出目标（图示为化trance)的S维信息；然后，在视觉指引下，仿生机器鱼游动向目标所在 的深度（到达A点）并保持；最后，机器鱼调整航向完成定向控制（到达B点），使得鱼体 对准入口目标中屯、，快速游动向目标处。
[00化]仿生机器鱼通过下潜运动到达目标中屯、所在深度，需要胸罐产生攻角，并且鱼体 达到一定游速才能实现。因此，在定深控制的过程中，无法采集到稳定的图像，实时的图像 数据反馈是不可靠的。为了解决该一问题，采用视觉给定深度信息，利用压力传感器反馈深 度信息，在该个过程中不进行基于视觉的深度反馈。另一方面，到达指定深度后，机器鱼调 整航向完成定向控制需要较高的精度，该需要基于视觉的多阶段航向给定；同时，需要兼顾 机器鱼游动向入口的快速性。但是，保证采集图像的稳定与机器鱼游动的灵活快速是矛盾 的。因此，均衡控制精度和运动的灵活快速需要多阶段定向控制的支持。
[0066] 基于W上的分析设计了本发明基于嵌入式视觉的=维追踪的控制系统，如图2所 示，图中，设计了定向控制的启动开关，开关的闭合受定深控制器的控制。整个控制过程，定 深控制持续执行，W保持机器鱼在入口中屯、所在深度；定向控制则受开关控制在深度保持 较好的情况下进入。
[0067] 图中，ei是深度误差，可W表示为式（1);同样，偏航误差6 2可W表示为式（2)。
[0068] 化）=Ad化)-Ac化）(1)
[0069]62化）=Dd(k)-Dc(k) (2)
[0070] 其中，Ad似、Dd似分别表示k时刻视觉给定的偏航角与深度，A。似、D。似分别表 示k时刻测量得到偏航角与深度。另外，Ui、U2分别表示定深控制器与定向控制器的输出。
[0071] 本发明中机器鱼的运动模型选用鱼体波模型。传统的鱼体波模型基本形式如式 (3)所示：
[0072] yb〇dy(x，t)=(CiX+C2x2)sin化x-?t) 做
[0073]其中，ybDdy表示鱼体的横向位移，X表示鱼体沿头尾轴的位置，c1代表线性鱼体波 的振幅包络线系数，C,代表了鱼体波的二次项系数，k是波数，《是鱼体波的频率，t代表 时间。
[0074] 对传统的鱼体波模型进行改进，W保证头部的平稳性为目的，改进后的鱼体波模 型如式（4)所示：
(4)
[0075]
[0076] 其中，C3是比例因子，用于调整鱼体波包络线的幅度，使其满足机器鱼游动时的振 幅特点。其它参数同式（3)。离散化输入到各个关节的鱼体波信号是一种简谐运动的振荡 信号，可W表不如式巧）。
[0077] (5)
[007引其中，0i表示t时刻输入到第i个驼机的角位移；Ai表示第i个振荡器的振幅； 巧是第i个振荡器相对于基准谐波的相位差，i= 1，2, ..，4表示尾部驼机；i= 5, 6代表胸 罐驼机，并且，写 1=C2= ^ 3= ^ 4=U2;C5= ^ 6=Ui。
[0079] 本发明所提出的嵌入式视觉引导下仿生机器鱼=维追踪控制方法，具体分为基于 嵌入式视觉的自主识别与定位、基于模糊滑模的定深运动控制W及多阶段的定向运动控 制。
[0080] 所述追踪控制方法具体包括W下步骤：
[0081] 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中屯、位置的深度 信息；
[0082] 水下目标的识别与连续定位，是具有挑战的任务；在嵌入式系统资源有限的前提 下，基于人工地标的视觉识别与定位是可靠、有效的方法。在保证实时性的同时，为提高算 法的鲁椿性，设计了色块按照一定规律排列的人工地标。并且，结合地标的已知尺寸信息， 可W解算出目标的=维位置信息。
[0083] 如图3所示，本发明的目标为具有按照一定规律排列的色块的圆形的人工地标； 所述人工地标的中屯、位置通过单一颜色的识别及不同颜色的严格拓扑关系进行确定。图 中，R区域代表红色区域，B区域代表藍色区域。本发明的人工地标中色块为扇形区域，该扇 形区域W人工地标的圆屯、为圆屯、、W人工地标的半径为半径，色块的圆屯、角之和为360°， 不同颜色的色块均与间隔分布。对于单一颜色的识别，关注于其主分量，使其满足宽松的阔 值识别（弱识别）；通过不同颜色的严格拓扑关系（强识别），最终确定人工地标的中屯、位 置。
[0084] W仿生机器鱼起始时刻嵌入式视觉系统的摄像机的镜头光屯、的位置作为坐标原 点建立惯性坐标系OXYZ，其中OY轴垂直于水面向下，OZ轴为摄像机的镜头光轴沿仿生机 器鱼头部向前，OX轴由右手定则确定；当仿生机器鱼与人工地标的中屯、位置不在同一深度 时，仿生机器鱼通过嵌入式视觉系统采集到的人工地标为楠圆形图像，根据几何关系，楠圆 的长轴与短轴相互垂直且相交于确定的中屯、。因此，分别向垂直中屯、位置的方向出发，根据 色块的拓扑关系识别出所述楠圆形图像的长轴和短轴；计算长轴两个端点的水平位置坐标 分别为（XI，yi)、咕，y2)，并计算长轴的长度1 ;
[0085] 根据小孔成像模型按照如下步骤进行人工地标的中屯、位置所在深度的计算：
[0086] 图4描述了小孔成像的进行S维定位的基本原理。其中摄像机坐标系OXYZ的坐 标原点设为摄像机的镜头光屯、，Z轴为光轴。图像坐标系〇iUV的坐标原点位于光轴中屯、。
[0087] 忽略摄像头的镜头崎变，摄像机内参数矩阵可W表述为：
[008引
巧）
[0089] 其中，（U。，V。）是摄像机镜头光屯、的图像坐标；也fy)表示成像平面到相机坐标系 的放大系数，可W由式（7)表示：
(7)
[0090]
[0091]其中，f代表焦距，（Sy，Sy)分别表示表示图像中X方向、y方向上单位象素的长度。
[0093]
[009引根据式做可W计算出给定的偏航角。
(8)
[0094] 其中，（u，v)是目标中屯、的图像坐标。[0095] 同理，视觉给定的俯仰角Pd可W表示为式巧）。
[0096] 9、
[0097] 根据小孔成像的几何关系可得： 闺
(10)
[0099] 其中，R表示圆形人工地标的直径。
[0100] 最终，人工地标标示中屯、的深度Dd可W表示为：
[01Dd=d*tan(Pd) (11)
[010引其中，d代表目标离光屯、的距离。
[0103] 步骤2,依据目标深度信息，基于模糊滑模控制的方法控制仿生机器鱼游动到目标 中屯、所在深度，并保持仿生机器鱼在目标中屯、所在的深度游动；
[0104] 胸罐产生攻角实现深度控制是一个=阶系统，并且深度的获取与机器鱼的游速密 切相关。因此，深度控制过程中实时的图像数据反馈不可靠。简化的模型分析不能直接用 于系统的控制，只能具有指导意义；并且，机器鱼在下潜过程中柔性身体产生形变导致较大 扰动。因此，深度控制器的设计基于模糊滑模控制，一方面克服无精确模型的问题，另一方 面抑制机器鱼俯仰运动中干扰。
[01化]如图5所示，基于模糊滑模控制的方法为；每个采样周期计算人工地标的中屯、位 置的深度Dd与当前的测量深度D。的差值作为深度误差e1，依据深度误差ei计算切换函数 值S、切换函数的变化量i，将切换函数值S、切换函数的变化量i输入模糊控制器得到胸罐 攻角的变化率，进而通过鱼体波模型调整仿真机器鱼的下潜动作。
[0106] 切换函数值S和切换函数的变化量i的计算步骤为：
[0107] 首先，获取滑模面的输入：
[010 引 (12)
[0109] 其中，Ts是采样周期，^为深度误差ei的一阶导数，Si为深度误差ei的二阶导数；
[0110] 然后，按照式（13)计算切换函数值S和切换函数的变化量立： (13)[0111]
[011引其中，Cl、C2与C3为常量调节因子。
[0113] 所述模糊控制器的设计及胸罐攻角的变化率^的计算方法包括如下步骤：
[0114] 模糊控制器隶属度设计
[0115] 输入切换函数值3、切换函数的变化量古、输出分别使用五个、^;：个、^;：个模糊 语言值。语言值NB、NM、NS、ZE、PS、PMW及PB分别代表负大、负中、负小、零、正小、正中W 及正大。模糊控制器的隶属度函数选用正态分布的函数，如图6所示。
[0116] 模糊控制器规则库设计
[0117] 规则库的设计W满足滑模面存在条件为前提，即满足<0的条件。且当S、i均为 NB或PB时，2^1的模糊语言值与S、5的模糊语言值相同，^加快^变化效率；
[0118] 解模糊方法
[0119] 利用重屯、解模糊方法解算模糊输出；
[0120] 计算胸罐的攻角Ui化)，可W表示为式（14)。
[0121 ]
(14)
[012引同时，为了保证机器鱼具有足够的速度，W产生足够大小的俯仰力，给定输入到机 器鱼尾部驼机的鱼体波信号。
[0123] 步骤3,在仿生机器鱼保持在目标中屯、位置所在的深度游动时，通过仿生机器鱼的 嵌入式视觉系统计算偏航角，并采用多阶段的定向控制方法进行目标的追踪。
[0124] 所提出基于模糊的定向控制框图如图7所示，定向运动控制中模糊控制器的设 计，分别包括隶属度设计、规则库的设计W及解模糊方法。其中，隶属度函数采用标准=角 函数；规则库设计根据相关经验；解模糊方法采用重屯、解模糊方法。
[0125] 图7中巧螺仪测量的偏航角度作为负反馈，用W保证控制系统的稳定性。图中，偏 航误差62的变化表示成ec，ec由式（15)确定。
(15)
[0126]
[0127] 具体设计方法如步骤2中模糊控制器的设计，所不同的是62与ec分别模糊化为 走个、五个模糊语言值。
[0128] 机器鱼的灵活运动是对鱼类高超的运动能力的仿生。但是，其灵活运动必然导致 成像质量低下，甚至模糊，该必然会降低基于视觉的机器鱼的控制精度。为了协调具有嵌入 式视觉的机器鱼的运动性能与控制精度，必须精细化多阶段运动控制，在保证控制精度的 前提下尽量满足机器鱼灵活的运动。多阶段的运动控制过程设计如下；
[0129] 多阶段定向控制过程，包括偏航阶段和直游阶段的控制过程；首先，通过视觉给定 偏航角度，机器鱼转向到给定角度；然后，机器鱼在头部对准目标时直游，快速到达目标。
[0130] 偏航阶段，为保证采集图像的稳定可靠，控制机器鱼头部摆动的平稳性。偏航阶段 控制过程，包括如下步骤：
[0131] 设计鱼体波模型，进而从所述鱼体波模型（如式5)上增强仿生机器鱼头部的稳定 性；
[0132] 基于牛顿-欧拉方法对仿生机器鱼的水动力学进行建模；
[0133] 基于水动力学模型，采用遗传算法进行参数优化W满足机器鱼的头部最小摆动；
[0134] 所述直游阶段的控制过程为；W正常游动参数控制鱼体游动，保证直游的快速性。 一方面，定向控制通过鱼体的偏航实现，偏航时头部平稳性的保持减少了成像的抖动，能够 确保目标定位的可靠性，为定向控制的精确性奠定基础；另一方面，直游阶段，W快速追踪 到目标为目的，W正常的游动参数控制鱼体快速游动。
[01巧]W上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详 细说明，所应理解的是，W上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡 在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保 护范围之内。
【主权项】
1. 一种嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在于，包括以下步 骤： 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中心位置的深度信 息； 步骤2,依据深度信息，基于模糊滑模控制的方法控制仿生机器鱼游动到目标中心所在 深度，并保持仿生机器鱼在目标中心所在的深度游动； 步骤3,在仿生机器鱼保持在目标中心位置所在的深度游动时，通过仿生机器鱼的嵌入 式视觉系统计算偏航角，并采用多阶段的定向控制方法进行目标的追踪。2. 根据权利要求1所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述的目标为具有按照一定规律排列的色块的圆形的人工地标；所述人工地标的中心 位置通过单一颜色的识别及不同颜色的严格拓扑关系进行确定。3. 根据权利要求2所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述人工地标中色块为扇形区域，该扇形区域以人工地标的圆心为圆心、以人工地标的 半径为半径，色块的圆心角之和为360°，不同颜色的色块均与间隔分布。4. 根据权利要求3所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征 在于，以仿生机器鱼起始时刻嵌入式视觉系统的摄像机的镜头光心的位置作为坐标原点建 立惯性坐标系OXYZ，其中OY轴垂直于水面向下，OZ轴为摄像机的镜头光轴沿仿生机器鱼 头部向前，OX轴由右手定则确定；当仿生机器鱼与人工地标的中心位置不在同一深度时， 仿生机器鱼通过嵌入式视觉系统采集到的人工地标为椭圆形图像，根据色块的拓扑关系 识别出所述椭圆形图像的长轴和短轴；计算长轴两个端点的水平位置坐标分别为（ Xl，yi)、 (x2, y2)，并计算长轴的长度1 ; 根据小孔成像模型按照如下步骤进行人工地标的中心位置所在深度的计算： 步骤1. 1，计算视觉给定的偏航角Ad:步骤1. 2,计算视觉给定的俯仰角Pd:其中，~为视觉给定的偏航角，PdS视觉给定的俯仰角，（u，v)为人工地标的中心位置 坐标，（?%)为嵌入式视觉系统的摄像机镜头光心的图像坐标，（fx，fy)为成像平面到相机 坐标系的放大系数； 步骤1. 3,计算人工地标的中心位置的深度Df1:Dd= d · tan(P d) 其中，d为目标离光心的距离，f为焦距，R为圆形人工地标的直径，％为目标中心所在 深度。5. 根据权利要求4所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述基于模糊滑模控制的方法为：每个采样周期计算人工地标的中心位置的深度〇4与 当前的测量深度D。的差值作为深度误差e i，依据深度误差^计算切换函数值s、切换函数 的变化量〗，将切换函数值s、切换函数的变化量:i 1输入模糊控制器得到胸鳍攻角的变化率 七，进而通过鱼体波模型调整仿真机器鱼的下潜动作。6. 根据权利要求5所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，切换函数值s和切换函数的变化量:i的计算步骤为： 步骤2. 1.1，计算滑模面的输入：其中，Ts是采样周期，^为深度误差ei的一阶导数，&为深度误差ei的二阶导数； 步骤2. 1. 2,计算切换函数值s和切换函数的变化量i :其中，Cl、(32与c 3为常量调节因子。7. 根据权利要求6所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述模糊控制器的设计及胸鳍攻角的变化率4的计算方法包括如下步骤： 步骤2. 2. 1，模糊控制器隶属度函数设计：切换函数值s、切换函数的变化量:i、胸鳍攻 角的变化率A分别使用五个、七个、七个模糊语言值，分别用模糊语言值NB、匪、NS、ZE、PS、 PM以及PB代表负大、负中、负小、零、正小、正中以及正大，隶属度函数选用正态分布的函 数； 步骤2. 2. 2,模糊控制器规则库设计：规则库的设计以满足SUI的条件，且当s、i模 糊语言值均为NB或PB时，七的模糊语言值与s、i的模糊语言值相同，以加快:W变化效率； 步骤2. 2. 3,利用重心解模糊方法解算模糊输出； 步骤2. 2. 4,计算胸鳍的攻角U1 (k): Zil (/c) = w, (A' - I) + ?] [k ) · Ts 〇8. 根据权利要求7所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述多阶段定向控制方法包括： 定向运动控制中模糊控制器的设计：分别包括隶属度设计、规则库的设计以及解模糊 方法，其中，隶属度函数采用标准三角函数；解模糊方法采用重心解模糊方法； 采用陀螺仪测量的偏航角度作为负反馈，用以保证控制系统的稳定性；偏航误差％的 变化为ec，其公式如下：y. TRiWt乂个」安I i-β 1士一项/jTDditWiK人式视觉引导下仿生机器鱼三维追踪控制方法， 其特征在于，所述步骤3中的多阶段定向控制过程，包括偏航阶段和直游阶段的控制过程； 所述偏航阶段控制过程，包括如下步骤： 步骤3. 1. 1，设计鱼体波模型，进而从所述鱼体波模型上增强仿生机器鱼头部的稳定 性； 步骤3. 1. 2,基于牛顿-欧拉方法对仿生机器鱼的水动力学进行建模； 步骤3. 1. 3,基于水动力学模型，采用遗传算法进行参数优化以满足机器鱼的头部最小 摆动； 所述直游阶段的控制过程为：以正常游动参数控制鱼体游动，保证直游的快速性。10.根据权利要求9所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征 在于，所述鱼体波模型为：其中：yb()dy为鱼体的横向位移，X为鱼体沿头尾轴的位置，C1为线性鱼体波的振幅包络 线系数，C2为鱼体波的二次项系数，k为波数，ω为鱼体波的频率，C 3为比例因子； 离散化输入到各个关节的鱼体波信号为： Oi = Ai ?η(ω? + Pi) + ξ? i = 1,2,..,6 其中，Si表示t时刻输入到第i个舵机的角位移，Ai表示第i个振荡器的振幅，仍是 第i个振荡器相对于基准谐波的相位差，i = 1，2, ..，4表示尾部舵机，i = 5, 6代表胸鳍舵 机，并且，I1= ξ 2= € 3= € 4=U2，ξ5= ξ 6=Ult>
【专利摘要】本发明公开了一种嵌入式视觉引导下仿生机器鱼三维追踪控制方法，首先，步骤1，设置人工地标，通过嵌入式视觉系统自主识别人工地标，结合人工地标的尺寸信息，获取目标所在的三维位置信息；步骤2，获取所述目标所在三维位置信息的深度，利用基于模糊滑模的控制方法，引导仿生机器鱼游动到目标深度并保持在目标深度游动；步骤3，所述仿生机器鱼在目标深度保持游动时，通过多阶段的定向控制，保证仿生机器鱼对目标的追踪。本发明在机器鱼运动机动灵活的基础上实现了准确的控制任务。
【IPC分类】G05D1/12
【公开号】CN104881045
【申请号】CN201510337549
【发明人】喻俊志, 孙飞虎, 徐德, 赵鹏
【申请人】中国科学院自动化研究所
【公开日】2015年9月2日
【申请日】2015年6月17日