[发明专利]二自由度仿生机器鱼携带目标的控制方法

摘要

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

主权项

一种二自由度仿生机器鱼携带目标的控制方法，其特征在于包括以下步骤：步骤一、以二自由度仿生机器鱼鱼体质心为坐标原点，以二自由度仿生机器鱼鱼体正方向为极轴正方向建立极坐标系，确定平面极坐标运动方程；$\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)作为二自由度仿生机器鱼当前目标状态进行控制。