CN104881045A 说明书 5/9页 其中0Y轴垂直于水面向下，0Z轴沿着仿生机器鱼起始时刻的尾头轴向前，0X轴由右手定则 确定。整个追踪控制分为视觉定位、定深控制、定向控制。首先，通过嵌入式视觉系统计算 出目标（图示为Entrance)的三维信息：然后，在视觉指引下，仿生机器鱼游动向目标所在 的深度（到达A点）并保持：最后，机器鱼调整航向完成定向控制（到达B点），使得鱼体 对准入口目标中心，快速游动向目标处。 [0065]仿生机器鱼通过下潜运动到达目标中心所在深度，需要胸鳍产生攻角，并且鱼体 达到一定游速才能实现。因此，在定深控制的过程中，无法采集到稳定的图像，实时的图像 数据反馈是不可靠的。为了解决这一问题，采用视觉给定深度信息，利用压力传感器反馈深 度信息，在这个过程中不进行基于视觉的深度反馈。另一方面，到达指定深度后，机器鱼调 整航向完成定向控制需要较高的精度，这需要基于视觉的多阶段航向给定：同时，需要兼顾 机器鱼游动向入口的快速性。但是，保证采集图像的稳定与机器鱼游动的灵活快速是矛盾 的。因此，均衡控制精度和运动的灵活快速需要多阶段定向控制的支持。 [0066]基于以上的分析设计了本发明基于嵌入式视觉的三维追踪的控制系统，如图2所 示，图中，设计了定向控制的启动开关，开关的闭合受定深控制器的控制。整个控制过程，定 深控制持续执行，以保持机器鱼在入口中心所在深度：定向控制则受开关控制在深度保持 较好的情况下进入。 [0067]图中，e是深度误差，可以表示为式(1)：同样，偏航误差e2可以表示为式(2)。 [0068] e(k)=Aa(k)-A.(k) (1) [0069]e2(k)=Da(k)-D.(k) (2) [0070] 其中，A(k)、Da(k)分别表示k时刻视觉给定的偏航角与深度，A。(k)、D。(k)分别表 示k时刻测量得到偏航角与深度。另外，u1、u2分别表示定深控制器与定向控制器的输出。 [007]本发明中机器鱼的运动模型选用鱼体波模型。传统的鱼体波模型基本形式如式 (3)所示： [0072]yhody (x,t)=(c:x+c2x2)sin(kx-0t) (3) [0073] 其中，y,表示鱼体的横向位移，X表示鱼体沿头尾轴的位置，c代表线性鱼体波 的振幅包络线系数，℃代表了鱼体波的二次项系数，k是波数，⊙是鱼体波的频率，t代表 时间。 [0074] 对传统的鱼体波模型进行改进，以保证头部的平稳性为目的，改进后的鱼体波模 型如式(4)所示： (4) [0075] c小=儿s（-Gr =(cx+cx2)sin(kx-wt)+c;cxsin(wt) [0076] 其中，℃是比例因子，用于调整鱼体波包络线的幅度，使其满足机器鱼游动时的振 幅特点。其它参数同式(3)。离散化输入到各个关节的鱼体波信号是一种简谐运动的振荡 信号，可以表示如式（⑤）。 [0077] 0=4sin(ot+9)+5i=l,2,,6 (5) [0078] 其中，0：表示t时刻输入到第i个舵机的角位移：A,表示第i个振荡器的振幅： 9