两连杆机器鱼的简单建模以及MATLAB仿真(2)
上一篇文章中，写过了关于两连杆机器鱼建模的方法。实际上，有一个细节值得注意，那就是在联立(1)和(2)方程，求解鱼头加速度，这一步中，是如何联立求解的。一般有两种方式：
- (1) 假设当前加速度已知，因此，鱼尾的力可以根据(2)式求出，从而把求出的F代入到(1)式中去，求出 V b V_b Vb。这也是我们上一篇文章中，使用到的联立求解办法。
- (2) 假设当前加速度未知，那么需要联立(1)和(2)式共同求解出 V b V_b Vb。这是我们这篇文章会着重介绍的，现在看不明白不要紧。
通过之后的仿真我们可以看到，这两种联立求解方式的不同，最终仿真的结果是类似的。但是，非常重要的一点，上一篇文章的求解方式在多连杆机器鱼中，极易造成计算结果发散，而本文介绍的求解方法的数值性能要稳定得多！！！
1 两连杆机器鱼的建模
本文延续上一篇文章的记号和公式，这里只是着重介绍计算 V ˙ b \dot{V}_b V˙b和 Ω ˙ b \dot{\Omega}_b Ω˙b的方式。
上一篇文章说到，对于鱼头的受力分析可以表达为：
F
+
F
d
r
a
g
=
m
b
V
˙
b
+
m
b
Ω
b
×
V
b
M
+
M
d
r
a
g
=
J
b
Ω
˙
b
+
Ω
b
×
J
b
Ω
b
(1)
\begin{aligned} F+F_{d r a g} &=m_{b} \dot{V}_{b}+m_{b} \Omega_{b} \times V_{b} \\ M+M_{d r a g} &=J_{b} \dot{\Omega}_{b}+\Omega_{b} \times J_{b} \Omega_{b} \end{aligned}\tag{1}
F+FdragM+Mdrag=mbV˙b+mbΩb×Vb=JbΩ˙b+Ωb×JbΩb(1)
对于鱼尾的受力分析则表达为：
F
=
−
m
t
V
˙
t
M
=
−
J
t
Ω
˙
t
−
Ω
t
×
J
t
Ω
t
+
r
b
t
×
F
(2)
\begin{aligned} F &=-m_{t} \dot{V}_{t} \\ M &=-J_{t} \dot{\Omega}_{t}-\Omega_{t} \times J_{t} \Omega_{t}+r_{b t} \times F \end{aligned}\tag{2}
FM=−mtV˙t=−JtΩ˙t−Ωt×JtΩt+rbt×F(2)
另外，
V
˙
t
\dot{V}_t
V˙t和
Ω
˙
t
\dot{\Omega}_t
Ω˙t与
V
˙
b
\dot{V}_b
V˙b和
Ω
˙
b
\dot{\Omega}_b
Ω˙b的关系为：
V
˙
t
=
V
˙
b
+
Ω
t
×
V
b
+
Ω
˙
t
×
r
b
t
+
Ω
t
×
Ω
t
×
r
b
t
Ω
˙
t
=
Ω
˙
b
+
θ
¨
e
3
(3)
\begin{array}{l}{\dot{V}_{t}=\dot{V}_{b}+\Omega_{t} \times V_{b}+\dot{\Omega}_{t} \times r_{b t}+\Omega_{t} \times \Omega_{t} \times r_{b t}} \\ {\dot{\Omega}_{t}=\dot{\Omega}_{b}+\ddot{\theta} \mathbf{e}_{3}}\end{array}\tag{3}
V˙t=V˙b+Ωt×Vb+Ω˙t×rbt+Ωt×Ωt×rbtΩ˙t=Ω˙b+θ¨e3(3)
由于我们假设了，鱼头的加速度
V
˙
b
\dot{V}_b
V˙b和
Ω
˙
b
\dot{\Omega}_b
Ω˙b未知，所以我们不能直接求出
F
F
F和
M
M
M，而是要联立(1)(2)(3)式求解
V
˙
b
\dot{V}_b
V˙b和
Ω
˙
b
\dot{\Omega}_b
Ω˙b。求解过程如下：
由(2)和(3)得出
F
F
F和
M
M
M与
V
˙
b
\dot{V}_b
V˙b和
Ω
˙
b
\dot{\Omega}_b
Ω˙b的关系 :
F
=
−
m
t
V
˙
b
+
m
t
r
^
b
t
Ω
˙
b
+
m
t
(
r
b
t
×
θ
¨
e
3
−
Ω
t
×
V
b
−
Ω
t
×
Ω
t
×
r
b
t
)
M
=
−
m
t
r
^
b
t
V
˙
b
−
(
J
t
−
m
t
r
^
b
t
r
^
b
t
)
Ω
˙
b
−
J
t
θ
¨
e
3
−
Ω
t
×
J
t
Ω
t
+
r
b
t
×
m
t
(
r
b
t
×
θ
¨
e
3
−
Ω
t
×
V
b
−
Ω
t
×
Ω
t
×
r
b
t
)
(4)
\begin{aligned} F &=-m_{t} \dot{V}_{b}+m_{t}\hat{r}_{bt}\dot{\Omega}_{b}+m_{t}(r_{bt}\times\ddot\theta\mathbf{e}_3-{\Omega_{t}} \times V_{b}-\Omega_{t} \times \Omega_{t} \times r_{b t}) \\ M &=-m_{t}\hat{r}_{b t} \dot{V}_{b} -(J_{t} - m_{t}\hat{r}_{b t} \hat{r}_{bt})\dot{\Omega}_{b} -J_{t}\ddot{\theta} \mathbf{e}_{3}-\Omega_{t} \times J_{t} \Omega_{t}\\ &+ r_{b t} \times m_{t}(r_{bt}\times\ddot\theta\mathbf{e}_3-{\Omega_{t}} \times V_{b}-\Omega_{t} \times \Omega_{t} \times r_{b t}) \end{aligned}\tag{4}
FM=−mtV˙b+mtr^btΩ˙b+mt(rbt×θ¨e3−Ωt×Vb−Ωt×Ωt×rbt)=−mtr^btV˙b−(Jt−mtr^btr^bt)Ω˙b−Jtθ¨e3−Ωt×JtΩt+rbt×mt(rbt×θ¨e3−Ωt×Vb−Ωt×Ωt×rbt)(4)
把上式简化记为:
F
=
−
m
t
V
˙
b
+
m
t
r
^
b
t
Ω
˙
b
+
F
1
M
=
−
m
t
r
^
b
t
V
˙
b
−
(
J
t
−
m
t
r
^
b
t
r
^
b
t
)
Ω
˙
b
+
M
1
(5)
\begin{aligned} F &=-m_{t} \dot{V}_{b}+m_{t}\hat{r}_{bt}\dot{\Omega}_{b}+F_1 \\ M &=-m_{t}\hat{r}_{b t} \dot{V}_{b} -(J_{t} - m_{t}\hat{r}_{b t} \hat{r}_{bt})\dot{\Omega}_{b} +M_1 \end{aligned}\tag{5}
FM=−mtV˙b+mtr^btΩ˙b+F1=−mtr^btV˙b−(Jt−mtr^btr^bt)Ω˙b+M1(5)
其中:
F
1
=
m
t
(
r
b
t
×
θ
¨
e
3
−
Ω
t
×
V
b
−
Ω
t
×
Ω
t
×
r
b
t
)
M
1
=
−
J
t
θ
¨
e
3
−
Ω
t
×
J
t
Ω
t
+
r
b
t
×
m
t
(
r
b
t
×
θ
¨
e
3
−
Ω
t
×
V
b
−
Ω
t
×
Ω
t
×
r
b
t
)
\begin{aligned} F_1 &=m_{t}(r_{bt}\times\ddot\theta\mathbf{e}_3-{\Omega_{t}} \times V_{b}-\Omega_{t} \times \Omega_{t} \times r_{b t}) \\ M_1 &=-J_{t}\ddot{\theta} \mathbf{e}_{3}-\Omega_{t} \times J_{t} \Omega_{t} + r_{b t} \times m_{t}(r_{bt}\times\ddot\theta\mathbf{e}_3-{\Omega_{t}} \times V_{b}-\Omega_{t} \times \Omega_{t} \times r_{b t}) \end{aligned}
F1M1=mt(rbt×θ¨e3−Ωt×Vb−Ωt×Ωt×rbt)=−Jtθ¨e3−Ωt×JtΩt+rbt×mt(rbt×θ¨e3−Ωt×Vb−Ωt×Ωt×rbt)
再把(5)和(1)联立 :
−
(
m
t
+
m
b
)
V
˙
b
+
m
t
r
^
b
t
Ω
˙
b
=
m
b
Ω
b
×
V
b
−
F
1
−
F
d
r
a
g
−
m
t
r
^
b
t
V
˙
b
−
(
J
t
+
J
b
−
m
t
r
^
b
t
r
^
b
t
)
Ω
˙
b
=
Ω
b
×
J
b
Ω
b
−
M
1
−
M
d
r
a
g
(6)
\begin{aligned} -(m_{t}+m_{b}) \dot{V}_{b}+m_{t}\hat{r}_{bt}\dot{\Omega}_{b}&=m_{b} \Omega_{b} \times V_{b}-F_1-F_{d r a g} \\ -m_{t}\hat{r}_{b t} \dot{V}_{b} -(J_{t}+J_{b}- m_{t}\hat{r}_{b t} \hat{r}_{bt})\dot{\Omega}_{b}&=\Omega_{b} \times J_{b} \Omega_{b} -M_1-M_{d r a g} \end{aligned}\tag{6}
−(mt+mb)V˙b+mtr^btΩ˙b−mtr^btV˙b−(Jt+Jb−mtr^btr^bt)Ω˙b=mbΩb×Vb−F1−Fdrag=Ωb×JbΩb−M1−Mdrag(6)
记：
H
=
[
−
(
m
t
+
m
b
)
m
t
r
^
b
t
−
m
t
r
^
b
t
−
(
J
t
+
J
b
−
m
t
r
^
b
t
r
^
b
t
)
]
H= \left[\begin{matrix} -(m_{t}+m_{b}) & m_{t}\hat{r}_{bt}\\ -m_{t}\hat{r}_{b t} & -(J_{t}+J_{b}- m_{t}\hat{r}_{b t} \hat{r}_{bt}) \end{matrix}\right]
H=[−(mt+mb)−mtr^btmtr^bt−(Jt+Jb−mtr^btr^bt)]
则有：
[
V
˙
b
Ω
˙
b
]
=
H
−
1
[
m
b
Ω
b
×
V
b
−
F
1
−
F
d
r
a
g
Ω
b
×
J
b
Ω
b
−
M
1
−
M
d
r
a
g
]
(7)
\left[\begin{matrix} \dot{V}_{b}\\ \dot{\Omega}_{b} \end{matrix}\right] =H^{-1} \left[\begin{matrix} m_{b} \Omega_{b} \times V_{b}-F_1-F_{d r a g}\\ \Omega_{b} \times J_{b} \Omega_{b} -M_1-M_{d r a g} \end{matrix}\right]\tag{7}
[V˙bΩ˙b]=H−1[mbΩb×Vb−F1−FdragΩb×JbΩb−M1−Mdrag](7)
这样我们得到了最终的计算表达式(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 仿真结果
和上一篇文章中4.3部分的仿真结果对比，几乎是完全相同的，当然仿真参数也没有改动。
扫一扫
1950
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
