CN104002948A - 二自由度仿生机器鱼携带目标的控制方法 - Google Patents

Abstract

本发明公开了一种二自由度仿生机器鱼携带目标的控制方法，用于解决现有机器鱼控制方法精确度差的技术问题。技术方案是该方法通过采集机器鱼及其所携带目标的状态参数，建立极坐标系。构建力学模型，建立机器鱼携带目标的运动学方程和动力学方程，确立控制参量与机器鱼速度和角速度的关系，根据理想的携带目标的状态，生成优化控制方法。并根据实验得到最终的速度和角速度的最优输出值。该方法采用客观精确的运动学方程，动力学方程确定约束条件，综合考虑了运动快速性、稳定性和精确性的要求，实时性好，精确程度高。

Description

二自由度仿生机器鱼携带目标的控制方法

技术领域

本发明涉及一种机器鱼控制方法，特别是涉及一种二自由度仿生机器鱼携带目标的控制方法。

背景技术

仿生机器鱼是通过模仿鱼类的游动机理，实现水中作业的一种机器鱼。它广泛应用于军事侦察，海洋勘探，管道检测，寻找污染源等方面，因此对仿生机器鱼的研究有重要的意义。

文献“基于坐标变换的机器鱼顶球算法，兵工自动化，2012，Vol.31(11),p73-79”在分析典型顶球算法的基础上，考虑影响进球的客观因素，提出了一种基于坐标变换的机器鱼控制方法。该方法秉承以头顶球为主的进攻模式，最大限度地利用机器鱼尾巴和身体击球。根据机器鱼、球以及对方球门的坐标几何位置关系设计动作决策机制，来选择机器鱼动作。大大提高了机器鱼带球进攻的效率。基于此思路，该文提出的新型顶球算法。部分解决了基本顶球算法运用于实际机器人水球比赛中存在的问题。文献所述方法仅针对机器鱼，球以及球门坐标几何关系进行了动作决策设计，并没有考虑实际情况下，鱼与目标之间的动力学问题，也无法保证机器鱼携带目标的精确性。

综上所述，现有的机器鱼携带目标方法尚未建立机器鱼与目标之间的动力学关系，未采取有效手段保证携带目标的精确性，具有一定的局限性。

发明内容

为了克服现有机器鱼控制方法精确度差的不足，本发明提供一种二自由度仿生机器鱼携带目标的控制方法。该方法通过采集机器鱼及其所携带目标的状态参数，建立极坐标系。构建力学模型，建立机器鱼携带目标的运动学方程和动力学方程，确立控制参量与机器鱼速度和角速度的关系，根据理想的携带目标的状态，生成优化控制方法。并根据实验得到最终的速度和角速度的最优输出值。该方法采用客观精确的运动学方程，动力学方程确定约束条件，综合考虑了运动快速性、稳定性和精确性的要求，实时性好，精确程度高。

本发明解决其技术问题所采用的技术方案是：一种二自由度仿生机器鱼携带目标的控制方法，其特点是采用以下步骤：

步骤一、以二自由度仿生机器鱼鱼体质心为坐标原点，以二自由度仿生机器鱼鱼体正方向为极轴正方向建立极坐标系，确定平面极坐标运动方程。

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}=-\mathrm{\ω}+\mathrm{\υ}\·\sin \mathrm{\θ}/\mathrm{\ρ}\\ \stackrel{\·}{\mathrm{\ρ}}=-\mathrm{\υ}\·\cos \mathrm{\θ}\end{array}\right.---\left(1\right)$

式中，(ρ,θ)为目标点的坐标，υ为二自由度仿生机器鱼质心的线速度大小，ω为二自由度仿生机器鱼绕质心角速度大小。

步骤二、以目标中心O为极点，以由携带目标的终点Destination指向O的方向为极轴，建立极坐标系。计算二自由度仿生机器鱼鱼头坐标H(ρ_H,α_H)，体心坐标B(ρ_B,α_B)，O到Destination得距离D_OD，以及体轴方向角α_F。所有角度都转换到(-π,π]区间内。

步骤三、构建力学模型，根据平面极坐标运动学方程，得

$\frac{d\left|{\mathrm{\α}}_F\right|}{\mathrm{dt}}={\mathrm{\ω}}_o-\frac{v\sin \left|{\mathrm{\α}}_F\right|}{D_{\mathrm{OD}}}---\left(2\right)$

式中，ω_o＝ω×sign(α_F)，ω为二自由度仿生机器鱼的角速度。

步骤四、将二自由度仿生机器鱼鱼头的速度做矢量分解：

$\left\{\begin{array}{c}{v}_r=v\cos {\mathrm{\α}}_{\mathrm{crio}}+{\mathrm{\ω}}_0{D}_{\mathrm{hb}}\sin {\mathrm{\α}}_{\mathrm{crio}}\\ v_t=v\sin {\mathrm{\α}}_{\mathrm{crio}}-{\mathrm{\ω}}_0{D}_{\mathrm{hb}}\cos {\mathrm{\α}}_{\mathrm{crio}}\end{array}\right.---\left(3\right)$

式中，v_r为二自由度仿生机器鱼鱼头的速度指向O的法向速度分量，v_t为二自由度仿生机器鱼鱼头的法向速度分量，α_crio＝α_cri×sign(α_F)。

在二自由度仿生机器鱼鱼头相对目标没有滑移的情况下，令圆形目标上的接触点受到鱼头的静摩擦力F_f，F_f与v_r、v_t正相关，并且与v_t同向，因此得到：F_f∝v_r,F_f∝v_t，即：

$F_f={\mathrm{\μv}}_r{v}_t=\mathrm{\μ}[\frac{v^2-{{\mathrm{\ω}}_o}^2{D_{\mathrm{hb}}}^2}{2}\sin \left({2\mathrm{\α}}_{\mathrm{crio}}\right)-{\mathrm{v\ω}}_o{D}_{\mathrm{hb}}\cos \left({2\mathrm{\α}}_{\mathrm{crio}}\right)]---\left(4\right)$

经过实物测试，ω_maxD_hb≈0.1v_max，ω_max ²D_hb ²≈0.01v_max ²，忽略ω_max ²D_hb ²项，得：

$F_f={\mathrm{\μv}}_r{v}_t=\mathrm{\μ}[\frac{v^2}{2}\sin \left({2\mathrm{\α}}_{\mathrm{crio}}\right)-{\mathrm{v\ω}}_o{D}_{\mathrm{hb}}\cos \left({2\mathrm{\α}}_{\mathrm{crio}}\right)]---\left(5\right)$

式中，μ为水介质下的摩擦系数，μ>0。

接触点受到相对O的力矩：

M_Touch＝F_fR               (6)

式中，R为圆形目标的半径。

根据运动学方程，α_H绝对值得变化速率：

$\frac{d\left|{\mathrm{\α}}_H\right|}{\mathrm{dt}}=-\frac{M_{\mathrm{Touch}}}{I_O}=-\frac{F_{f}R}{I_O}---\left(7\right)$

式中，I_O为目标绕O点的转动惯量。

步骤五、生成控制方法，二自由度仿生机器鱼携带目标理想的状态是α_H＝0,α_F＝π，为了尽可能的让二自由度仿生机器鱼向理想状态靠拢，当|α_H|≥π/2或者二自由度仿生机器鱼鱼头未触及目标时，令二自由度仿生机器鱼游向Rear点，尽快进入携带目标状态；

进入携带目标状态后，保证与为了保证运动和进入理想状态的快速性，根据实验选取最优值，取：

$v_1=0.95\frac{{\mathrm{\ω}}_o{D}_{\mathrm{OD}}}{\sin \left|{\mathrm{\α}}_F\right|},v_2=\max \{v_1,0\},v_{\mathrm{out}}=\min \{v_2,v_{\max }\};---\left(8\right)$

同时，还要保证

①如果0≤|α_crio|<π/4，得为了尽量消除转角运动的震荡，保证运动的稳定性，根据实验选取最优值，取：

${\mathrm{\&omega;}}_{o1}=0.95\frac{v}{{2D}_{\mathrm{hb}}}\tan \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right),{\mathrm{\&omega;}}_{o2}=\max \{{\mathrm{\&omega;}}_{o1},-{\mathrm{\&omega;}}_{\max }\},{\mathrm{\&omega;}}_o=\min \{{\mathrm{\&omega;}}_{o2},{\mathrm{\&omega;}}_{\max }\};---\left(9\right)$

②如果π/4≤|α_crio|≤π/2，取ω₀＝ω_max。

ω_out＝ω_o×sign(α_F)             (10)将(v_out,ω_out)作为二自由度仿生机器鱼当前目标状态进行控制。

本发明的有益效果是：该方法通过采集二自由度仿生机器鱼及其所携带目标的状态参数，建立极坐标系。构建力学模型，建立二自由度仿生机器鱼携带目标的运动学方程和动力学方程，确立控制参量与二自由度仿生机器鱼速度和角速度的关系，根据理想的携带目标的状态，生成优化控制方法。并根据实验得到最终的速度和角速度的最优输出值。该方法采用客观精确的运动学方程，动力学方程确定约束条件，综合考虑了运动快速性、稳定性和精确性的要求，实时性好，精确程度高。

下面结合附图和具体实施方式对本发明作详细说明。

附图说明

图1是本发明二自由度仿生机器鱼携带目标的控制方法的流程图。

图2是本发明二自由度仿生机器鱼携带目标的控制方法中环境变量以及控制变量示意图。

图3是本发明二自由度仿生机器鱼携带目标的控制方法中平面极坐标运动方程几何关系参照图。

具体实施方式

参照图1-3。

1、本发明所应用的数学理论和控制原理推导。

以二自由度仿生机器鱼鱼体质心为坐标原点，以二自由度仿生机器鱼鱼体正方向为极轴正方向建立极坐标系。运动方程是刻画系统运动的物理参量所满足的方程或方程组，以参量对于时间的微分方程形式出现。由于在已建坐标系中二自由度仿生机器鱼是静止的，故目标点始终以速度υ朝极轴的负方向运动，同时，绕原点以角速度-ω做圆周运动，可得如下平面极坐标运动方程：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}=-\mathrm{\&omega;}+\mathrm{\&upsi;}\&CenterDot;\sin \mathrm{\&theta;}/\mathrm{\&rho;}\\ \stackrel{\&CenterDot;}{\mathrm{\&rho;}}=-\mathrm{\&upsi;}\&CenterDot;\cos \mathrm{\&theta;}\end{array}\right.---\left(1\right)$

式中，(ρ,θ)为目标点的坐标，υ为二自由度仿生机器鱼质心的线速度大小，ω为二自由度仿生机器鱼绕质心角速度大小。

2、建立坐标系，确立环境变量。

以目标中心O为极点，以由携带目标的终点Destination指向O的方向为极轴，建立极坐标系。计算二自由度仿生机器鱼鱼头坐标H(ρ_H,α_H),体心坐标B(ρ_B,α_B)，O到Destination得距离D_OD，以及体轴方向角α_F。所有角度都转换到(-π,π]区间内。

3、构建力学模型。

根据平面极坐标运动学方程，可得：

$\frac{d\left|{\mathrm{\&alpha;}}_F\right|}{\mathrm{dt}}={\mathrm{\&omega;}}_o-\frac{v\sin \left|{\mathrm{\&alpha;}}_F\right|}{D_{\mathrm{OD}}}---\left(2\right)$

式中，ω_o＝ω×sign(α_F)，ω为二自由度仿生机器鱼的角速度。

二自由度仿生机器鱼通过鱼头与圆形目标接触，推动目标前进。将二自由度仿生机器鱼鱼头的速度做矢量分解：

$\left\{\begin{array}{c}{v}_r=v\cos {\mathrm{\&alpha;}}_{\mathrm{crio}}+{\mathrm{\&omega;}}_0{D}_{\mathrm{hb}}\sin {\mathrm{\&alpha;}}_{\mathrm{crio}}\\ v_t=v\sin {\mathrm{\&alpha;}}_{\mathrm{crio}}-{\mathrm{\&omega;}}_0{D}_{\mathrm{hb}}\cos {\mathrm{\&alpha;}}_{\mathrm{crio}}\end{array}\right.---\left(3\right)$

式中，v_r为二自由度仿生机器鱼鱼头的速度指向O的法向速度分量，v_t为二自由度仿生机器鱼鱼头的法向速度分量，α_crio＝α_cri×sign(α_F)。

在二自由度仿生机器鱼鱼头相对目标没有滑移的情况下，令圆形目标上的接触点受到鱼头的静摩擦力F_f，F_f与v_r、v_t正相关，并且与v_t同向，因此得到：F_f∝v_r,F_f∝v_t，即：

$F_f={\mathrm{\&mu;v}}_r{v}_t=\mathrm{\&mu;}[\frac{v^2-{{\mathrm{\&omega;}}_o}^2{D_{\mathrm{hb}}}^2}{2}\sin \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)-{\mathrm{v\&omega;}}_o{D}_{\mathrm{hb}}\cos \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)]---\left(4\right)$

经过实物测试，得到ω_maxD_hb≈0.1v_max，ω_max ²D_hb ²≈0.01v_max ²，忽略ω_max ²D_hb ²项，得：

$F_f={\mathrm{\&mu;v}}_r{v}_t=\mathrm{\&mu;}[\frac{v^2}{2}\sin \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)-{\mathrm{v\&omega;}}_o{D}_{\mathrm{hb}}\cos \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)]---\left(5\right)$

式中，μ为水介质下的摩擦系数，μ>0。由于μ不影响控制输出，故不需给出具体值。

接触点受到相对O的力矩：

M_Touch＝F_fR                  (6)

式中，R为圆形目标的半径。

假设二自由度仿生机器鱼鱼头相对目标没有滑移，根据运动学方程，α_H绝对值得变化速率：

$\frac{d\left|{\mathrm{\&alpha;}}_H\right|}{\mathrm{dt}}=-\frac{M_{\mathrm{Touch}}}{I_O}=-\frac{F_{f}R}{I_O}---\left(7\right)$

式中，I_O为目标绕O点的转动惯量。

4、生成控制方法。

二自由度仿生机器鱼依靠头部推动目标前进，最理想的状态是α_H＝0,α_F＝π，二自由度仿生机器鱼全力加速前进。优化程度高的控制方法，总是向理想状态靠拢，并且控制过程的大部分时间在理想状态附近徘徊。

基于上述思想，当|α_H|≥π/2或者二自由度仿生机器鱼鱼头未触及目标时，令二自由度仿生机器鱼游向Rear点，尽快进入携带目标状态；

进入携带目标状态后，要保证即为了保证运动和进入理想状态的快速性，根据实验选取最优值，取：

$v_1=0.95\frac{{\mathrm{\&omega;}}_o{D}_{\mathrm{OD}}}{\sin \left|{\mathrm{\&alpha;}}_F\right|},v_2=\max \{v_1,0\},v_{\mathrm{out}}=\min \{v_2,v_{\max }\};---\left(8\right)$

同时，还要保证

①如果0≤|α_crio|<π/4，得为了尽量消除转角运动的震荡，保证运动的稳定性，根据实验选取最优值，取：

${\mathrm{\&omega;}}_{o1}=0.95\frac{v}{{2D}_{\mathrm{hb}}}\tan \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right),{\mathrm{\&omega;}}_{o2}=\max \{{\mathrm{\&omega;}}_{o1},-{\mathrm{\&omega;}}_{\max }\},{\mathrm{\&omega;}}_o=\min \{{\mathrm{\&omega;}}_{o2},{\mathrm{\&omega;}}_{\max }\};---\left(9\right)$

②如果π/4≤|α_crio|≤π/2，取ω₀＝ω_max。

ω_out＝ω_o×sign(α_F)             (10)

将(v_out,ω_out)作为二自由度仿生机器鱼当前目标状态进行控制。

Claims (1)

1.一种二自由度仿生机器鱼携带目标的控制方法，其特征在于包括以下步骤：

步骤一、以二自由度仿生机器鱼鱼体质心为坐标原点，以二自由度仿生机器鱼鱼体正方向为极轴正方向建立极坐标系，确定平面极坐标运动方程；

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}=-\mathrm{\&omega;}+\mathrm{\&upsi;}\&CenterDot;\sin \mathrm{\&theta;}/\mathrm{\&rho;}\\ \stackrel{\&CenterDot;}{\mathrm{\&rho;}}=-\mathrm{\&upsi;}\&CenterDot;\cos \mathrm{\&theta;}\end{array}\right.---\left(1\right)$

式中，(ρ,θ)为目标点的坐标，υ为二自由度仿生机器鱼质心的线速度大小，ω为二自由度仿生机器鱼绕质心角速度大小；

步骤二、以目标中心O为极点，以由携带目标的终点Destination指向O的方向为极轴，建立极坐标系；计算二自由度仿生机器鱼鱼头坐标H(ρ_H,α_H)，体心坐标B(ρ_B,α_B)，O到Destination得距离D_OD，以及体轴方向角α_F；所有角度都转换到(-π,π]区间内；

步骤三、构建力学模型，根据平面极坐标运动学方程，得

$\frac{d\left|{\mathrm{\&alpha;}}_F\right|}{\mathrm{dt}}={\mathrm{\&omega;}}_o-\frac{v\sin \left|{\mathrm{\&alpha;}}_F\right|}{D_{\mathrm{OD}}}---\left(2\right)$

式中，ω_o＝ω×sign(α_F)，ω为二自由度仿生机器鱼的角速度；

步骤四、将二自由度仿生机器鱼鱼头的速度做矢量分解：

$\left\{\begin{array}{c}{v}_r=v\cos {\mathrm{\&alpha;}}_{\mathrm{crio}}+{\mathrm{\&omega;}}_0{D}_{\mathrm{hb}}\sin {\mathrm{\&alpha;}}_{\mathrm{crio}}\\ v_t=v\sin {\mathrm{\&alpha;}}_{\mathrm{crio}}-{\mathrm{\&omega;}}_0{D}_{\mathrm{hb}}\cos {\mathrm{\&alpha;}}_{\mathrm{crio}}\end{array}\right.---\left(3\right)$

式中，v_r为二自由度仿生机器鱼鱼头的速度指向O的法向速度分量，v_t为二自由度仿生机器鱼鱼头的法向速度分量，α_crio＝α_cri×sign(α_F)；

在二自由度仿生机器鱼鱼头相对目标没有滑移的情况下，令圆形目标上的接触点受到鱼头的静摩擦力F_f，F_f与v_r、v_t正相关，并且与v_t同向，因此得到：F_f∝v_r,F_f∝v_t，即：

$F_f={\mathrm{\&mu;v}}_r{v}_t=\mathrm{\&mu;}[\frac{v^2-{{\mathrm{\&omega;}}_o}^2{D_{\mathrm{hb}}}^2}{2}\sin \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)-{\mathrm{v\&omega;}}_o{D}_{\mathrm{hb}}\cos \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)]---\left(4\right)$

经过实物测试，ω_maxD_hb≈0.1v_max，ω_max ²D_hb ²≈0.01v_max ²，忽略ω_max ²D_hb ²项，得：

$F_f={\mathrm{\&mu;v}}_r{v}_t=\mathrm{\&mu;}[\frac{v^2}{2}\sin \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)-{\mathrm{v\&omega;}}_o{D}_{\mathrm{hb}}\cos \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right)]---\left(5\right)$

式中，μ为水介质下的摩擦系数，μ>0；

接触点受到相对O的力矩：

M_Touch＝F_fR             (6)

式中，R为圆形目标的半径；

根据运动学方程，α_H绝对值得变化速率：

$\frac{d\left|{\mathrm{\&alpha;}}_H\right|}{\mathrm{dt}}=-\frac{M_{\mathrm{Touch}}}{I_O}=-\frac{F_{f}R}{I_O}---\left(7\right)$

式中，I_O为目标绕O点的转动惯量；

步骤五、生成控制方法，二自由度仿生机器鱼携带目标理想的状态是α_H＝0,α_F＝π，为了尽可能的让二自由度仿生机器鱼向理想状态靠拢，当|α_H|≥π/2或者二自由度仿生机器鱼鱼头未触及目标时，令二自由度仿生机器鱼游向Rear点，尽快进入携带目标状态；

进入携带目标状态后，保证与为了保证运动和进入理想状态的快速性，根据实验选取最优值，取：

$v_1=0.95\frac{{\mathrm{\&omega;}}_o{D}_{\mathrm{OD}}}{\sin \left|{\mathrm{\&alpha;}}_F\right|},v_2=\max \{v_1,0\},v_{\mathrm{out}}=\min \{v_2,v_{\max }\};---\left(8\right)$

同时，还要保证

①如果0≤|α_crio|<π/4，得为了尽量消除转角运动的震荡，保证运动的稳定性，根据实验选取最优值，取：

${\mathrm{\&omega;}}_{o1}=0.95\frac{v}{{2D}_{\mathrm{hb}}}\tan \left({2\mathrm{\&alpha;}}_{\mathrm{crio}}\right),{\mathrm{\&omega;}}_{o2}=\max \{{\mathrm{\&omega;}}_{o1},-{\mathrm{\&omega;}}_{\max }\},{\mathrm{\&omega;}}_o=\min \{{\mathrm{\&omega;}}_{o2},{\mathrm{\&omega;}}_{\max }\};---\left(9\right)$

②如果π/4≤|α_crio|≤π/2，取ω₀＝ω_max；

ω_out＝ω_o×sign(α_F)            (10)

将(v_out,ω_out)作为二自由度仿生机器鱼当前目标状态进行控制。