技术领域
[0001] 本
发明涉及一种机器鱼控制方法,特别是涉及一种二自由度仿生机器鱼携带目标的控制方法。
背景技术
[0002] 仿生机器鱼是通过模仿鱼类的游动机理,实现
水中作业的一种机器鱼。它广泛应用于军事侦察,海洋勘探,管道检测,寻找污染源等方面,因此对仿生机器鱼的研究有重要的意义。
[0003] 文献“基于坐标变换的机器鱼顶球
算法,兵工自动化,2012,Vol.31(11),p73-79”在分析典型顶球算法的
基础上,考虑影响进球的客观因素,提出了一种基于坐标变换的机器鱼控制方法。该方法秉承以头顶球为主的进攻模式,最大限度地利用机器鱼尾巴和身体击球。根据机器鱼、球以及对方球
门的坐标几何
位置关系设计动作决策机制,来选择机器鱼动作。大大提高了机器鱼带球进攻的效率。基于此思路,该文提出的新型顶球算法。部分解决了基本顶球算法运用于实际
机器人水球比赛中存在的问题。文献所述方法仅针对机器鱼,球以及球门坐标几何关系进行了动作决策设计,并没有考虑实际情况下,鱼与目标之间的动
力学问题,也无法保证机器鱼携带目标的精确性。
[0004] 综上所述,现有的机器鱼携带目标方法尚未建立机器鱼与目标之间的动力学关系,未采取有效手段保证携带目标的精确性,具有一定的局限性。
发明内容
[0005] 为了克服现有机器鱼控制方法精确度差的不足,本发明提供一种二自由度仿生机器鱼携带目标的控制方法。该方法通过采集机器鱼及其所携带目标的状态参数,建立极
坐标系。构建力学模型,建立机器鱼携带目标的运动学方程和动力学方程,确立控制参量与机器鱼速度和
角速度的关系,根据理想的携带目标的状态,生成优化控制方法。并根据实验得到最终的速度和角速度的最优输出值。该方法采用客观精确的运动学方程,动力学方程确定约束条件,综合考虑了运动快速性、
稳定性和精确性的要求,实时性好,精确程度高。
[0006] 本发明解决其技术问题所采用的技术方案是:一种二自由度仿生机器鱼携带目标的控制方法,其特点是采用以下步骤:
[0007] 步骤一、以二自由度仿生机器鱼鱼体质心为坐标原点,以二自由度仿生机器鱼鱼体正方向为极轴正方向建立极坐标系,确定平面极坐标运动方程。
[0008]
[0009] 式中,(ρ,θ)为目标点的坐标,υ为二自由度仿生机器鱼质心的线速度大小,ω为二自由度仿生机器鱼绕质心角速度大小。
[0010] 步骤二、以目标中心O为极点,以由携带目标的终点Destination指向O的方向为极轴,建立极坐标系。计算二自由度仿生机器鱼鱼头坐标H(ρH,αH),体心坐标B(ρB,αB),O到Destination得距离DOD,以及体轴方向角αF。所有角度都转换到(-π,π]区间内。
[0011] 步骤三、构建力学模型,根据平面极坐标运动学方程,得
[0012]
[0013] 式中,ωo=ω×sign(αF),ω为二自由度仿生机器鱼的角速度。
[0014] 步骤四、将二自由度仿生机器鱼鱼头的速度做矢量分解:
[0015]
[0016] 式中,vr为二自由度仿生机器鱼鱼头的速度指向O的法向速度分量,vt为二自由度仿生机器鱼鱼头的法向速度分量,αcrio=αcri×sign(αF)。
[0017] 在二自由度仿生机器鱼鱼头相对目标没有滑移的情况下,令圆形目标上的
接触点受到鱼头的静
摩擦力Ff,Ff与vr、vt正相关,并且与vt同向,因此得到:Ff∝vr,Ff∝vt,即:
[0018]
[0019] 经过实物测试,ωmaxDhb≈0.1vmax,ωmax2Dhb2≈0.01vmax2,忽略ωmax2Dhb2项,得:
[0020]
[0021] 式中,μ为水介质下的
摩擦系数,μ>0。
[0022] 接触点受到相对O的力矩:
[0023] MTouch=FfR (6)
[0024] 式中,R为圆形目标的半径。
[0025] 根据运动学方程,αH绝对值得变化速率:
[0026]
[0028] 步骤五、生成控制方法,二自由度仿生机器鱼携带目标理想的状态是αH=0,αF=π,为了尽可能的让二自由度仿生机器鱼向理想状态靠拢,当|αH|≥π/2或者二自由度仿生机器鱼鱼头未触及目标时,令二自由度仿生机器鱼游向Rear点,尽快进入携带目标状态;
[0029] 进入携带目标状态后,保证 与 为了保证运动和进入理想状态的快速性,根据实验选取最优值,取:
[0030]
[0031] 同时,还要保证
[0032] ①如果0≤|αcrio|<π/4,得 为了尽量消除转角运动的震荡,保证运动的稳定性,根据实验选取最优值,取:
[0033]
[0034] ②如果π/4≤|αcrio|≤π/2,取ω0=ωmax。
[0035] ωout=ωo×sign(αF) (10)将(vout,ωout)作为二自由度仿生机器鱼当前目标状态进行控制。
[0036] 本发明的有益效果是:该方法通过采集二自由度仿生机器鱼及其所携带目标的状态参数,建立极坐标系。构建力学模型,建立二自由度仿生机器鱼携带目标的运动学方程和动力学方程,确立控制参量与二自由度仿生机器鱼速度和角速度的关系,根据理想的携带目标的状态,生成优化控制方法。并根据实验得到最终的速度和角速度的最优输出值。该方法采用客观精确的运动学方程,动力学方程确定约束条件,综合考虑了运动快速性、稳定性和精确性的要求,实时性好,精确程度高。
[0037] 下面结合
附图和具体实施方式对本发明作详细说明。
附图说明
[0038] 图1是本发明二自由度仿生机器鱼携带目标的控制方法的
流程图。
[0039] 图2是本发明二自由度仿生机器鱼携带目标的控制方法中环境变量以及控制变量示意图。
[0040] 图3是本发明二自由度仿生机器鱼携带目标的控制方法中平面极坐标运动方程几何关系参照图。
具体实施方式
[0041] 参照图1-3。
[0042] 1、本发明所应用的数学理论和控制原理推导。
[0043] 以二自由度仿生机器鱼鱼体质心为坐标原点,以二自由度仿生机器鱼鱼体正方向为极轴正方向建立极坐标系。运动方程是刻画系统运动的物理参量所满足的方程或方程组,以参量对于时间的微分方程形式出现。由于在已建坐标系中二自由度仿生机器鱼是静止的,故目标点始终以速度υ朝极轴的负方向运动,同时,绕原点以角速度-ω做圆周运动,可得如下平面极坐标运动方程:
[0044]
[0045] 式中,(ρ,θ)为目标点的坐标,υ为二自由度仿生机器鱼质心的线速度大小,ω为二自由度仿生机器鱼绕质心角速度大小。
[0046] 2、建立坐标系,确立环境变量。
[0047] 以目标中心O为极点,以由携带目标的终点Destination指向O的方向为极轴,建立极坐标系。计算二自由度仿生机器鱼鱼头坐标H(ρH,αH),体心坐标B(ρB,αB),O到Destination得距离DOD,以及体轴方向角αF。所有角度都转换到(-π,π]区间内。
[0048] 3、构建力学模型。
[0049] 根据平面极坐标运动学方程,可得:
[0050]
[0051] 式中,ωo=ω×sign(αF),ω为二自由度仿生机器鱼的角速度。
[0052] 二自由度仿生机器鱼通过鱼头与圆形目标接触,推动目标前进。将二自由度仿生机器鱼鱼头的速度做矢量分解:
[0053]
[0054] 式中,vr为二自由度仿生机器鱼鱼头的速度指向O的法向速度分量,vt为二自由度仿生机器鱼鱼头的法向速度分量,αcrio=αcri×sign(αF)。
[0055] 在二自由度仿生机器鱼鱼头相对目标没有滑移的情况下,令圆形目标上的接触点受到鱼头的静摩擦力Ff,Ff与vr、vt正相关,并且与vt同向,因此得到:Ff∝vr,Ff∝vt,即:
[0056]
[0057] 经过实物测试,得到ωmaxDhb≈0.1vmax,ωmax2Dhb2≈0.01vmax2,忽略ωmax2Dhb2项,得:
[0058]
[0059] 式中,μ为水介质下的摩擦系数,μ>0。由于μ不影响控制输出,故不需给出具体值。
[0060] 接触点受到相对O的力矩:
[0061] MTouch=FfR (6)
[0062] 式中,R为圆形目标的半径。
[0063] 假设二自由度仿生机器鱼鱼头相对目标没有滑移,根据运动学方程,αH绝对值得变化速率:
[0064]
[0065] 式中,IO为目标绕O点的转动惯量。
[0066] 4、生成控制方法。
[0067] 二自由度仿生机器鱼依靠头部推动目标前进,最理想的状态是αH=0,αF=π,二自由度仿生机器鱼全力
加速前进。优化程度高的控制方法,总是向理想状态靠拢,并且控制过程的大部分时间在理想状态附近徘徊。
[0068] 基于上述思想,当|αH|≥π/2或者二自由度仿生机器鱼鱼头未触及目标时,令二自由度仿生机器鱼游向Rear点,尽快进入携带目标状态;
[0069] 进入携带目标状态后,要保证 即 为了保证运动和进入理想状态的快速性,根据实验选取最优值,取:
[0070]
[0071] 同时,还要保证
[0072] ①如果0≤|αcrio|<π/4,得 为了尽量消除转角运动的震荡,保证运动的稳定性,根据实验选取最优值,取:
[0073]
[0074] ②如果π/4≤|αcrio|≤π/2,取ω0=ωmax。
[0075] ωout=ωo×sign(αF) (10)
[0076] 将(vout,ωout)作为二自由度仿生机器鱼当前目标状态进行控制。