CN101219708B - 仿生机器鱼无半径转弯的控制方法 - Google Patents

Abstract

本发明请求保护一种仿生机器鱼无半径转弯的控制方法，涉及自动控制领域。本发明以机器鱼质心为原点，建立机器鱼的运动学模型；将机器鱼关节化，根据运动学模型计算出每个关节的旋转角度；控制电路发出控制信号，驱动机器鱼从鱼尾关节开始摆动，通过水的推力F形成力偶，实现仿生机器鱼以质心为原点的无半径转弯。通过水对机器鱼的反作用力，实现机器鱼绕质心的无半径旋转。采用本发明的仿生机器鱼无半径旋转方法，可以使仿生机器鱼实现原地无半径转弯，在改变方向时不影响位置的改变，机器鱼更加灵活。这种可以很好控制某个位置上方向的改变的方法，能实现水下原地探测或水下考古中的原地摄影。

Description

仿生机器鱼无半径转弯的控制方法

技术领域

本发明属于自动控制领域，具体涉及仿生机器鱼运动控制方法。

背景技术

水下推进器在海洋生物观察、海底探险、海洋救捞、检测油罐漏油、管道检测、以及军事方面的应用日益增加。传统的螺旋桨在水中游动时，在它的侧面会产生涡流，降低物体的运动速度，增加动力的消耗，且有噪声。与水下推进器相比，鱼有惊人的游动能力。观察发现，鱼的游动具有无噪声，高推进效率，机动性强等优点，因此仿鱼推进器的研究成为一个重要的研究课题。

20世纪90年代以来，出现了模仿鱼运动的仿生机器鱼。从1994年美国麻省工学院成功研制的世界上第一条真正意义上的仿生金枪鱼到2005年英国Essex大学研制的世界第一条具有自主控制能力的机器鱼，机器鱼已经具备有阻力小的外形、实现小半径转弯，自主蔽障等特点，因此，以鱼的推进模式运动的机器鱼被认为是未来水下运载器的设计目标，日益引起各国的重视，以美国、日本和英国为代表的许多国家积极开展对机器鱼的研究，使得机器鱼飞速发展。但是机器鱼要达到实用还存在一些问题：目前还没有柔软而有力的驱动器，要实现s-形转弯，快速启动，快速制动，在下潜过程中快速稳定的悬浮在水中，加之不知鱼是怎样控制涡流推进，不能预测给定一个命令，机器鱼将会做出什么样动作等等，并且在控制机器鱼进行转弯运动时存在转弯半径，机器鱼的转弯半径是影响其机动性的重要因素，有半径转弯的机器鱼改变方向的同时客观的改变了位置，不能单独改变方向。很难在水下探测或水下考古中实现原位旋转观察。

发明内容

本发明所要解决的技术问题是，针对现有技术中，控制机器鱼转弯时存在转弯半径，很难在水下实现原位旋转的上述缺陷，提出一种控制仿生机器鱼实现无半径转弯，在水下实现原位旋转的方法。

本发明实现仿生机器鱼的无半径转弯控制方法所采用的技术方案是，以机器鱼质心点为原点，通过水的反作用力形成力偶，实现仿生机器鱼以质心为原点的无半径转弯。具体包括如下步骤，确定机器鱼质心点，建立机器鱼的运动学模型；将机器鱼关节化，根据运动学模型计算出每个关节每时刻的旋转角度；在存储器中建立鱼尾关节与旋转角度的关系列表；控制电路通过神经网络发出控制信号，通过修改神经元控制器的加权系数，使加权系数的修正沿着旋转角度误差的平方的减小方向，从而实现自适应的最优控制，驱动机器鱼从鱼尾关节开始摆动，通过水的推力F形成力偶，实现仿生机器鱼以质心为原点的无半径转弯。水的推力F的方向垂直于鱼体，幅度正比于被带动的流体的附加质量m_a(l)和其机器鱼在垂直方向的加速度a(l，t)。建立机器鱼的运动学模型的步骤为，在二维平面以机器鱼体上的质心点为圆点O建立一个坐标系，建立以鱼尾的弯曲半径Rc为半径，正切于x轴的圆，其方程为：x²+(y-R_c)²＝R_c ²。根据水的密度ρ_ω、鱼的高度h确定附加质量m_a(l)。根据机器鱼旋转的角速度，有效阻力面积，确定机器鱼所受到的水的阻力。

本发明的有益效果：本发明是机器鱼运动控制中急转弯技术的研究，与c形和s形急转弯不同之处在于提出了一种机器鱼无半径转弯的控制方法。控制仿生机器鱼原地无半径转弯，在改变方向时不影响位置的改变，使机器鱼更加灵活，可以很好地控制某个位置上方向的改变，能实现水下原地探测或水下考古中的原地摄影。

附图说明

图1机器鱼的质心确定的正视示意图

图2机器鱼的运动控制流程图

图3a 4关节机器鱼曲线拟和理论鱼体波动曲线示意图

图3b机器鱼关节运动曲线示意图

图4两层神经网络构造控制器

图5仿生机器鱼的无半径转弯运动模型

图6虚拟机器鱼原地转弯的序列

图7仿生机器鱼无半径转弯旋转轨迹

具体实施方式

鱼的游动方式分为两类：周期性的运动是以不同的速度循环运动，适合长距离巡航；非周期的运动则包括跳、钻孔、飞、喷气式推动、急转弯、启动、制动、滑行等。通常将机器鱼的运动分为多种基本行为：

1.巡航：机器鱼沿着一条直线以恒速，加速或减速运动。

2.巡航中转弯：在直航速度不为零的情况下，转弯。

3.急转弯：突然有个角加速度。

4.快速启动：以很大的加速度启动。

5.快速制动：以很大的加速度快速制动。

6.滑行：机器鱼身体成为直线不用动力向前滑行。

7.上浮下潜

有半径转弯的仿生机器鱼不能实现在原位改变方向，即要改变方向，位置就改变了，而无半径转弯则可以单独改变机器鱼的方向而不改变机器鱼的位置。以下结合附图和具体实施例对本发明的实现进行具体描述。本发明仿生机器鱼无半径转弯的控制以机器鱼质心点为原点，通过水的反作用力形成力偶，实现仿生机器鱼以质心为原点的无半径转弯。

首先确定机器鱼质心点：对于上下对称，前后对称的机器鱼。那么确定质心点就是确定左右方向的质心在哪。对于地球上体积不太大的物体，重心与质心的位置是重合的。可采用牵引法寻找仿生机器鱼的重心，当物体受三个力作用处于平衡状态时，这三个力的作用线必相交于一点。如图1所示为确定机器鱼质心的正视示意图，将机器鱼的一端用细绳AB悬挂起来，另一端用弹性细绳CD缓慢拉起到一适当位置，分别画出AB、CD的延长线，并相交于E点，E点的正上方O点就是机器鱼的重心。

如图2所示为机器鱼无半径转弯的控制流程图。首先将机器鱼尾关节化，关节越少越容易控制，同时刚性越强，柔性越差；关节越多，柔性越强，但控制越复杂，应同时考虑两者以达到所要求的条件。每个关节的长度，主要依靠电机的尺寸。

给定机器鱼每个关节的最大、最小旋转角度、旋转速度{θ_{j_max}，speed，θ_{j_min}}，通过旋转角度控制计算模块计算出每个关节每时刻要旋转的角度θ，经过A/D转换器将其转换为数字量，控制处理器采用PWM脉冲信号控制伺服电机，使鱼尾旋转，通过位置反馈信号判断是否达到要控制的位置，达到了进入下一个控制环节，未达到，经基于神经网络的PID控制器进行控制，使其旋转达到预定位置。

根据理论鱼体波动曲线拟和关节机器鱼曲线确定机器鱼的各关节位置。

以下以4关节机器鱼为例对机器鱼的关节化过程进行具体说明，如图3a所示为4关节机器鱼曲线拟和理论鱼体波动曲线。为使实际机器鱼的关节曲线与理论鱼体波动曲线误差最小，应使其两曲线之间的面积最小。

将机器鱼关节化，根据运动学模型计算出每个关节的旋转角度；在存储器中建立鱼关节与旋转角度的关系列表。

图3b为机器鱼关节运动曲线示意图。

控制处理器中算法模块计算某一时刻两曲线之间面积的误差为：

$\mathrm{sum}\left(i\right)=\underset{j=1}{\overset{4}{\mathrm{\Σ}}}{e}_{\mathrm{ij}},(i=\mathrm{0,1,2}.....17)---\left(1\right)$

其中i为离散的时刻，j＝1，2，3，4为关节数。

根据误差e_ij最小求出关节的坐标值。

误差： $e_{\mathrm{ij}}=\left|\underset{b\_x_{\mathrm{ij}}}{\overset{e\_x_{\mathrm{ij}}}{\∫}}\right[g\left(x\right)-f\left(x\right)\left]\mathrm{dx}\right|$ 最小          (2)

其中：

$f\left(x\right)=R_c\±\sqrt{{R_c}^2-x^2}---\left(3\right)$

g_ij(x)＝k_ijx+b_ij， $k_{\mathrm{ij}}=\frac{e\_y_{\mathrm{ij}}-b\_y_{\mathrm{ij}}}{e\_x_{\mathrm{ij}}-b\_x_{\mathrm{ij}}},$ b_ij＝b_y_ij-k_ijb_y_ij    (4)

式中：f(x)为机器鱼转弯的理论曲线；g_ij(x)为每一时刻每一关节的直线表达式；(b_x_ij，b_y_ij)为每一关节的起点坐标；(e_x_ij，e_y_ij)为每一关节的终点坐标。

约束条件：

$\left\{\begin{array}{c}b\_x_{\mathrm{ij}}=b\_y_{\mathrm{ij}}=0,j=1\\ b\_x_{\mathrm{ij}}=e\_x_{\mathrm{ij}-1},j=\mathrm{2,3,4}\end{array}\right.---\left(5\right)$

${(e\_x_{\mathrm{ij}}-b\_x_{\mathrm{ij}})}^2+{(e\_y_{\mathrm{ij}}-b\_y_{\mathrm{ij}})}^2=L_j^2---\left(6\right)$

L_j为每一关节的长度。

通过上式计算出最大时刻和最小时刻每个关节的坐标(x，y)。

根据每个关节的坐标计算出每个关节的最大、最小旋转角度{θ_max，θ_min}，中心控制处理器首先输出{θ_max，speed，θ_min}给旋转角度控制计算器，旋转角度控制计算器精确的计算机器鱼中间节点旋转的角度θ的值，在存储器中存储关节与旋转角度之间的关系，控制处理器根据旋转角度θ的值控制伺服电机驱动机器鱼体转动。

$\mathrm{\θ}=\mathrm{speed}*t=k\left|\tan \mathrm{\α}\right|*t=k\left|\frac{{\mathrm{\θ}}_{\max }-{\mathrm{\θ}}_{\min }}{t1}\right|*t---\left(7\right)$

其中k为角度的修正值，通过实验得到；tl机器鱼尾关节摆动到最大角度的时间；θ_max为机器鱼尾关节摆动的最大角度；θ_min为机器鱼尾关节摆动的最小角度，也就是摆动的开始角度。这样的话，4关节的机器鱼仅仅需要控制15个参数，{θ_{i_max}，speed_i，θ_{i_min}}，i＝1，2，3，4，ψ_j j＝1，2，3为伺服电机之间的相位差。

通过精确的拟合θ(t)曲线，确定各关节的坐标位置，计算关节的旋转角度，根据关节的旋转角度控制伺服电机驱动机器鱼尾摆动。通过基于神经网络的数字PID控制，根据每时刻的旋转角度控制伺服电机驱动机器鱼从鱼尾关节开始摆动，机器鱼尾弯曲产生推力。鱼尾摆动产生的推力和水的反作用力推动机器鱼旋转实现无半径转弯。

数字PID控制算法为：

$U\left(k\right)=K_{P}e\left(k\right)+K_I\underset{j=0}{\overset{k}{\mathrm{\Σ}}}e\left(j\right)+K_D[e\left(k\right)-e(k-1)]---\left(8\right)$

PID调节的原理就是通过设置合适的PID参数，使实际输出值快速平稳地收敛或者逼近于期望输出值。为了实现在线修正PID参数，采用神经网络的计算方法，PID参数可由网络本身根据现场环境的改变而在线地修正，其具体方法如下：

根据式(8)，可用一个如图4所示的两层神经网络构造控制器，对神经网络的PID参数进行自校正。

             X₁＝e(k)

网络输入层为： $X_2=\underset{j=0}{\overset{k}{\mathrm{\Σ}}}e\left(j\right)$

             X₃＝e(k)-e(k-1)

网络输出层为：U＝W₁X₁+W₂X₂+W₃X₃   (9)

式中：e(k)为输入量与反馈量代数之和，即误差量；X₁，X₂，X₃为误差量的代数和；W₁，W₂，W₃为神经网络权系数，表示PID的三个参数。其中，{W_i}为权系数，从表达式可看出神经网络控制器具有PID控制器结构，其权系数就等于PID控制器的三个参数，控制器的任务就是调整控制量u(k)(关于角度误差量的代数)，使得输出y(k)(被控对象旋转角度输出量)等于给定的y_r(k)(机器鱼关节的期望输出旋转角度)。因此，在此引入二次型性能指标，即取实际输出与期望输出的差值的平方作为误差，为了计算方便再乘以系数，即得角度误差E为：

E＝(1/2)·[y_r(k+1)-y(k+1)]²

$\frac{\∂E}{\∂W_i\left(k\right)}=[y_r(k+1)-y(k+1)]\·\frac{-\∂y(k+1)}{\∂W_i\left(k\right)}$

在单神经元控制中引入二次型性能指标，通过修改神经元控制器的加权系数W_i(k)，使性能指标趋于最小，从而实现自适应的最优控制。使加权系数W_i(k)的修正沿着角度误差E的平方的减小方向，即对W_i(k)的负梯度方向搜索调整。

根据BP神经网络的误差反向传播和加权系数的调整，则有

${\mathrm{\ΔW}}_i\left(k\right)=W_i\left(k\right)-W_i(k-1)=-l_i\frac{\∂E}{\∂W_i\left(k\right)}=l_i[y_r(k+1)-y(k+1)]\·\frac{\∂y(k+1)}{\∂W_i\left(k\right)}$

${=l}_i[y_r(k+1)-y(k+1)]\·\frac{\∂y(k+1)}{\∂u\left(k\right)}\frac{\∂u\left(k\right)}{\∂W_i\left(k\right)}$

又由(9)式可得

$\frac{\∂u\left(k\right)}{\∂W_i\left(k\right)}=X_i$

所以可得神经网络PID控制器权系数的修正式为

$\mathrm{\Δ}{W}_i\left(k\right)=l_i[y_r(k+1)-y(k+1)]\·X_i\·\frac{\∂y(k+1)}{\∂u\left(k\right)}$

其中，0＜l_i＜1学习步长， $\frac{\∂y(k+1)}{\∂u\left(k\right)}=\frac{y(k+1)-y\left(k\right)}{u\left(k\right)-u(k-1)}$

ΔW_i(k)是权重的变化量，由此，神经网络权系数{W_i}便可在线修正。

建立仿真机器鱼的运动模型，对鱼体上每一点的运动轨迹进行描述。旋转角度控制计算器计算机器鱼中间节点旋转的角度θ，控制处理器根据旋转角度θ的值控制伺服电机驱动机器鱼尾转动。水产生的推力作用于机器鱼的鱼尾关节，保持质心的运动轨迹为一条直线，使其质心的位置不随时间变化。

如图5所示为仿生机器鱼的无半径转弯运动模型。以鲹科类机器鱼为研究对象建立仿生机器鱼的运动模型，在二维平面以机器鱼体上的质心点为圆点O建立一个坐标系，a为鱼头位置、b为鱼尾位置，质心在鱼体上。鱼头为刚体，控制电路电源装在鱼头处，鱼尾是柔性的，主要产生推力，如鱼嘴到质心的距离为l1，鱼尾到质心的位置为l2，Rc为鱼尾向一边弯曲的圆的圆半径，在鱼尾弯曲的过程中，Rc从无穷大变化到如图位置，Rc决定机器鱼旋转的角度。假设机器鱼尾从起始位置1变化到以(O，R_c)为圆心，以R_c为半径的圆弧上位置2，即通过摆动鱼尾获得无半径转弯的动力。建立正切于x轴，以R_c为半径的圆，圆方程为：

x²+(y-R_c)²＝R_c ²         (10)

其中，R_c决定转弯的特点，是影响机器鱼旋转的速度，以及最大转角的主要因素。

鱼尾圆心的轨迹：y_c＝R_c＝e^-(t-c)    (11)

其中：t为时间，c为常量。

同时鱼体上的任何关节点坐标(x，y)绕质心旋转，即

$x^{\′2}+y^{\′2}=\sqrt{x^2+y^2}---\left(12\right)$

其中：

是鱼体上任关节点到质心的距离，x′，y′为鱼体任一点的轨迹坐标。

通过上述方法建立机器鱼的运动学模型，即对鱼体上每一点的运动轨迹进行描述。

机器鱼在弯曲时带动了周围的一部分水改变动量，这部分水对机器鱼产生推力F，因为机器鱼的关节连接的是硬臂，可以忽略其弹性系数，所以忽略鱼肌肉间的相互作用。水的推力F的方向垂直于鱼体，幅度正比于被带动的流体的附加质量m_a(l)和其机器鱼在垂直方向的加速度a(l，t)。当单位长度的一段鱼体在其横截面内左右摆动时，也带动了鱼体周围的一部分流体一起改变动量，因此流体对鱼体的反作用力，除了鱼体本身的动量变化引起的反作用力以外，还要考虑被带动的流体的这部分附加质量的动量的变化引起的侧向力F，鱼体带动周围流体，这个被带动的周围流体对鱼产生了附加质量。因为水的阻力D(l，t)仅仅因为旋转引起，因此，忽略在切线方向水的反作用力。

针对上述模型对仿生机器鱼无半径转弯运动进行具体分析，确定其运动速度、加速度的情况。假如质心O(0，0)在机器鱼体的1/3处，a点为鱼头，b点为鱼尾。鱼转弯的主要能量来自水产生的推力F。根据水的密度ρ_ω、鱼的高度h确定附加质量m_a(l)。

附加质量：m_a(l)＝0.25πρ_ωh²         (13)

从而得到水的推力：F(l，t)＝ηm_a(l)a(l，t)    (14)

其中η是常数。

根据机器鱼旋转的角速度ω(t)，有效阻力面积A(l，t)，确定机器鱼所受到的水的阻力：D(l，t)＝0.5ρ_ωC_d[ω(t)r(l，t)]²A(l，t)         (15)

它们的力矩分别为：T_D(l，t)＝D(l，t)·r_i(l，t)

T_F(l，t)＝F(l，t)·K              (16)

总力矩为： $T\left(t\right)=T_F(l,t)-T_D(l,t)={\∫}_{-r_1}^{r_2}{T}_F(l,t)\mathrm{dl}-{\∫}_{-r_1}^{r_2}{T}_D(l,t)\mathrm{dl}---\left(17\right)$

鱼体对于质心的转动惯量：

$I\left(t\right)={\∫}_{-r_1}^0\mathrm{\ρ}\left(l\right)l^2\mathrm{dl}+{\∫}_0^{r_2}\mathrm{\ρ}\left(l\right){(x(t,\mathrm{\θ})-x_{\mathrm{\ω}})}^2+y^2(t,\mathrm{\θ}))\mathrm{dl}---\left(18\right)$

所以机器鱼相对于质心的角加速度为：

$a_{\mathrm{\ω}}\left(t\right)=\frac{T\left(t\right)}{I\left(t\right)}---\left(19\right)$

机器鱼旋转过程的效果示意如图6所示。0位为初始位置，装在鱼头处的控制电路发出控制信号，驱动机器鱼的关节从鱼尾开始摆动。图中位置1到3表示鱼尾从以半径无穷到R_c的弯曲过程，这个过程是产生推力的主要阶段，鱼尾向一边弯曲，通过水的反作用力来产生推力矩，同时由于水的阻力产生阻力矩，两个力矩的合力矩使鱼顺时针旋转起来，此时加速度从最大慢慢减小，此过程为角速度增大的过程，位置4到位置12为只受到阻力的过程，此时加速度为负，此时处于角速度减小的过程，同时阻力也在减小。位置13到位置18为鱼尾展开的过程。其中，鱼尾的弯曲半径R_c是影响机器鱼转角的重要因子，决定机器鱼的转角。

从图6、7可以看出，机器鱼先从初始开始，逆时针弯曲鱼尾，然后顺时针绕质心旋转。实线为鱼体在不同时刻旋转的位置。5条虚线分别为鱼头a点、鱼尾关节1，关节2，关节3，关节4旋转的路径，由图看出它们是圆弧形的。从图中看出质心在一原点处，说明质心的位置是固定的不随时间变化。从图中看出在逆时针弯曲鱼尾的过程中和旋转完毕时展开鱼尾过程中鱼头位置不变；鱼尾关节的位置随时间变化而变化，鱼尾关节一开始逆时针弯曲，然后顺时针旋转，最后展开。

从图7机器鱼运动轨迹示意图可看出，在改变方向时不影响位置的改变，使机器鱼更加灵活，可以很好的控制某个位置上方向的改变，能实现水下原地探测或水下考古中的原地摄影。

Claims (6)

1.一种仿生机器鱼无半径转弯的控制方法，其特征在于，建立机器鱼的运动学模型；将机器鱼关节化，给定机器鱼每个关节的最大、最小旋转角度及旋转速度；根据运动学模型计算机器鱼各关节节点每时刻的旋转角度；控制处理器通过基于神经网络的控制，根据每时刻中间节点旋转的角度控制伺服电机驱动机器鱼从鱼尾关节开始摆动；通过水产生的推力形成力偶，实现机器鱼以质心为原点的无半径转弯。

2.根据权利要求1所述的控制方法，其特征在于，所述建立机器鱼的运动学模型的步骤为，在二维平面以机器鱼体上的质心点为圆点建立一个坐标系，建立以鱼尾的弯曲半径Rc，正切于x轴的圆，其方程为：x²+(y-R_c)²＝R_c ²，其中，Rc决定机器鱼旋转的角度。

3.根据权利要求1所述的控制方法，其特征在于，所述机器鱼关节化的步骤具体包括，根据理论鱼体波动曲线拟和关节机器鱼曲线，使两曲线之间的面积最小，确定机器鱼的各关节位置。

4.根据权利要求1所述的控制方法，其特征在于，所述神经网络的控制包括，通过修改神经元控制器的加权系数，使加权系数的修正沿着旋转角度误差的平方的减小方向，从而实现自适应的最优控制。

5.根据权利要求1或2所述的控制方法，其特征在于，水的推力F的方向垂直于鱼体，幅度正比于被带动的水的附加质量和机器鱼在垂直方向的加速度。

6.根据权利要求5所述的控制方法，其特征在于，根据水的密度、机器鱼的高度确定所述附加质量。

Images