两连杆机器鱼的简单建模以及MATLAB仿真(2)

341
0
2020年9月8日 10时55分

上一篇文章中,写过了关于两连杆机器鱼建模的方法。实际上,有一个细节值得注意,那就是在联立(1)和(2)方程,求解鱼头加速度,这一步中,是如何联立求解的。一般有两种方式:

 

  • (1) 假设当前加速度已知,因此,鱼尾的力可以根据(2)式求出,从而把求出的F代入到(1)式中去,求出VbV_b。这也是我们上一篇文章中,使用到的联立求解办法。
  • (2) 假设当前加速度未知,那么需要联立(1)和(2)式共同求解出VbV_b。这是我们这篇文章会着重介绍的,现在看不明白不要紧。

 

通过之后的仿真我们可以看到,这两种联立求解方式的不同,最终仿真的结果是类似的。但是,非常重要的一点,上一篇文章的求解方式在多连杆机器鱼中,极易造成计算结果发散,而本文介绍的求解方法的数值性能要稳定得多!!!


1 两连杆机器鱼的建模

本文延续上一篇文章的记号和公式,这里只是着重介绍计算 两连杆机器鱼的简单建模以及MATLAB仿真(2)插图和 两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(1) 的方式。

 

上一篇文章说到,对于鱼头的受力分析可以表达为:

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(2)

 

对于鱼尾的受力分析则表达为:

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(3)

 

另外,两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(4)两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(5)两连杆机器鱼的简单建模以及MATLAB仿真(2)插图 两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(1) 的关系为:

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(6)

 

由于我们假设了,鱼头的加速度 两连杆机器鱼的简单建模以及MATLAB仿真(2)插图两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(1) 未知,所以我们不能直接求出 F M ,而是要联立(1)(2)(3)式求解 两连杆机器鱼的简单建模以及MATLAB仿真(2)插图两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(1) 。求解过程如下:

 

由(2)和(3)得出 F M 两连杆机器鱼的简单建模以及MATLAB仿真(2)插图两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(1) 的关系 :

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(7)

 

把上式简化记为:

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(8)

 

其中:

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(9)

 

再把(5)和(1)联立 :

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(10)

 

记:

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(11)

 

则有:

 

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(12)

 

这样我们得到了最终的计算表达式(7)。

2 MATLAB代码实现

clc;
clear all;
close all;

% 物理参数
mb = 1.0;
Jb = 0.01;

% 物理参数
mt = 0.2;
Jt = 0.001;
r = 0.1;

% 关节运动
theta = 0;
dtheta = 0;
ddtheta = 0;
a = pi*2;
b = pi/4;

% 运动状态
Vb = zeros(3,1);
dVb = zeros(3,1);
Wb = zeros(3,1);
dWb = zeros(3,1);
Vt = zeros(3,1);
dVt = zeros(3,1);
Wt = zeros(3,1);
dWt = zeros(3,1);
Yaw = 0;
Pos = zeros(3,1);

% 阻力系数
CFb = 10*[0.1; 0.01; 0];
CMb = [0; 0; 1];
CFt = [0.1; 0.1; 0.1];

% 力
F = zeros(3,1);
M = zeros(3,1);
Flist = [];
Mlist = [];

% 其他辅助变量
e3 = [0;0;1];
time = [];
The = [];
Vel = [];
WVel = [];
Poslist = [];
ta = 0;
TA = [];
%% 主要仿真过程
for t = 0:0.01:40
	% 关节运动规律
    theta = b*cos(a*t);
    dtheta = -a*b*sin(a*t);
    ddtheta = -a*a*b*cos(a*t);
    r_bt = r*[cos(theta);sin(theta);0];
    
    % 速度
    Wt = Wb + dtheta*e3;
    Vt = Vb + cross(Wt,r_bt);
    
    % 阻力
    Fdb = -0.5*CFb.*[sign(Vb(1))*Vb(1)*Vb(1); sign(Vb(2))*Vb(2)*Vb(2); sign(Vb(3))*Vb(3)*Vb(3)];
    Mdb = -0.5*CMb.*[sign(Wb(1))*Wb(1)*Wb(1); sign(Wb(2))*Wb(2)*Wb(2); sign(Wb(3))*Wb(3)*Wb(3)];
    Fdt = -0.5*CFt.*[sign(Vt(1))*Vt(1)*Vt(1); sign(Vt(2))*Vt(2)*Vt(2); sign(Vt(3))*Vt(3)*Vt(3)];
 
    % 计算力
    F1 = mt*(cross(r_bt,ddtheta*e3)-cross(Wt,Vb)-cross(Wt,cross(Wt,r_bt)));
    M1 = -Jt*ddtheta*e3 - cross(Wt, Jt*Wt) + cross(r_bt, F1);
    K = [mb*cross(Wb,Vb) - F1 - Fdb;
            cross(Wb, Jb*Wb) - M1 - Mdb];
        
    % 计算H矩阵
    r_bt_crossmat = zeros(3,3);
    r_bt_crossmat(1,2) = -r_bt(3);
    r_bt_crossmat(1,3) = r_bt(2);
    r_bt_crossmat(2,1) = r_bt(3);
    r_bt_crossmat(2,3) = -r_bt(1);
    r_bt_crossmat(3,1) = -r_bt(2);
    r_bt_crossmat(3,2) = r_bt(1);
    H = [-(mt+mb)*eye(3), mt*r_bt_crossmat;
            -mt*r_bt_crossmat, -(Jt+Jb)*eye(3)+mt*r_bt_crossmat*r_bt_crossmat];
    Hinv = inv(H);
    
    % 分析头部连杆
    U = Hinv*K;
    dVb = U(1:3);
    dWb = U(4:6);
    Vb = Vb + dVb*0.01;
    Wb = Wb + dWb*0.01;

    % 位置更新
    Yaw = Yaw + Wb(3)*0.01;
    R = [cos(Yaw), -sin(Yaw), 0;
            sin(Yaw), cos(Yaw), 0;
            0, 0, 1];

    Vw = R*Vb;
    Pos = Pos + Vw*0.01;
    Poslist = [Poslist, Pos];

    % 收集数据
    time = [time, t];
    Flist = [Flist, F];
    Mlist = [Mlist, M];
    Vel = [Vel, Vb];
    WVel = [WVel, Wb];
    ta = ta + F/mb*0.01;
    TA = [TA, ta];
end
%% 绘图
figure(1)
subplot(3,2,1)
title('力');
hold on
plot(time, Flist(1,:),'r')
ylabel('X')
grid on
subplot(3,2,3)
plot(time, Flist(2,:),'g')
ylabel('Y')
grid on
subplot(3,2,5)
plot(time, Flist(3,:),'b')
ylabel('Z')
grid on
subplot(3,2,2)
title('力矩');
hold on
plot(time, Mlist(1,:),'r')
ylabel('X')
grid on
subplot(3,2,4)
plot(time, Mlist(2,:),'g')
ylabel('Y')
grid on
subplot(3,2,6)
plot(time, Mlist(3,:),'b')
ylabel('Z')
grid on

figure(2)
subplot(3,2,1)
title('速度');
hold on
plot(time, Vel(1,:),'r')
ylabel('X')
grid on
subplot(3,2,3)
plot(time, Vel(2,:),'g')
ylabel('Y')
grid on
subplot(3,2,5)
plot(time, Vel(3,:),'b')
ylabel('Z')
grid on
subplot(3,2,2)
title('角速度');
hold on
plot(time, WVel(1,:),'r')
ylabel('X')
grid on
subplot(3,2,4)
plot(time, WVel(2,:),'g')
ylabel('Y')
grid on
subplot(3,2,6)
plot(time, WVel(3,:),'b')
ylabel('Z')
grid on

figure(4)
plot(Poslist(1,:),Poslist(2,:))
grid on
axis equal

 

3 仿真结果

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(13)

两连杆机器鱼的简单建模以及MATLAB仿真(2)插图(14)

和上一篇文章中4.3部分的仿真结果对比,几乎是完全相同的,当然仿真参数也没有改动。

发表评论

后才能评论