输入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