【小项目】四连杆机构的Python运动学求解和MATLAB图形仿真
任务
Python运动学解算
from math import cos, sin, pi, sqrt, atan2
from matplotlib import pyplot as plt
import matplotlib
import pandas as pd
"""
变量说明:
位置s
速度v
加速度a
角度phi
角速度omega
角加速度alpha
杆长l
其中位置、速度、加速度均为双元素列表,索引0为x方向,索引1为y方向
"""
class RR:
"""
RR杆组
参数:
A点位置、速度、加速度
杆组绕A点旋转初始角度、角速度、角加速度、已转过角度
杆件长度
属性:
B点位置、速度、加速度
"""
def __init__(self, sA, vA, aA, phi_i, omega_i, alpha_i, l_i, delta_i):
self.sB = [0, 0]
self.vB = [0, 0]
self.aB = [0, 0]
theta_i = phi_i + delta_i
self.sB[0] = sA[0] + l_i * cos(theta_i)
self.sB[1] = sA[1] + l_i * sin(theta_i)
self.vB[0] = vA[0] - l_i * sin(theta_i) * omega_i
self.vB[1] = vA[1] + l_i * cos(theta_i) * omega_i
self.aB[0] = aA[0] - l_i * cos(theta_i) * omega_i ** 2 - l_i * sin(theta_i) * alpha_i
self.aB[1] = aA[1] - l_i * sin(theta_i) * omega_i ** 2 + l_i * cos(theta_i) * alpha_i
def __str__(self):
return (f'xB={self.sB[0]:.2f}\n'
f'yB={self.sB[1]:.2f}\n'
f'vxB={self.vB[0]:.2f}\n'
f'vyB={self.vB[1]:.2f}\n'
f'axB={self.aB[0]:.2f}\n'
f'ayB={self.aB[1]:.2f}\n')
class RRR:
"""
RRR杆组
参数:
外副B、D两点的位置、速度、加速度
BC、CD杆长l_i、l_j
属性:
内副C点位置、速度、加速度
BC、CD杆件与x轴夹角、角速度、角加速度
"""
def __init__(self, sB, vB, aB, sD, vD, aD, l_i, l_j, DIR_flag):
self.sC = [0, 0]
self.vC = [0, 0]
self.aC = [0, 0]
self.phi_i = 0
self.omega_i = 0
self.alpha_i = 0
#location
l_BD = sqrt((sD[0] - sB[0]) ** 2 + (sD[1] - sB[1]) ** 2)
A0 = 2 * l_i * (sD[0] - sB[0])
B0 = 2 * l_i * (sD[1] - sB[1])
C0 = l_i ** 2 + l_BD ** 2 - l_j ** 2
self.phi_i = 2 * atan2(B0 + DIR_flag * sqrt(A0 ** 2 + B0 ** 2 - C0 ** 2), A0 + C0)
self.sC[0] = sB[0] + l_i * cos(self.phi_i)
self.sC[1] = sB[1] + l_i * sin(self.phi_i)
phi_j = atan2(self.sC[1] - sD[1], self.sC[0] - sD[0])
#speed
C_i = l_i * cos(self.phi_i)
C_j = l_j * cos(phi_j)
S_i = l_i * sin(self.phi_i)
S_j = l_j * sin(phi_j)
G1 = C_i * S_j - C_j * S_i
self.omega_i = (C_j * (vD[0] - vB[0]) + S_j * (vD[1] - vB[1])) / G1
omega_j = (C_i * (vD[0] - vB[0]) + S_i * (vD[1] - vB[1])) / G1
self.vC[0] = vB[0] - self.omega_i * l_i * sin(self.phi_i)
self.vC[1] = vB[1] + self.omega_i * l_i * cos(self.phi_i)
#acceleration
G2 = aD[0] - aB[0] + self.omega_i ** 2 * C_i - omega_j ** 2 * C_j
G3 = aD[1] - aB[1] + self.omega_i ** 2 * S_i - omega_j ** 2 * S_j
self.alpha_i = (G2 * C_j + G3 * S_j) / G1
alpha_j = (G2 * C_i + G3 * S_i) / G1
self.aC[0] = aB[0] - self.alpha_i * l_i * sin(self.phi_i) - self.omega_i ** 2 * l_i * cos(self.phi_i)
self.aC[1] = aB[1] + self.alpha_i * l_i * cos(self.phi_i) - self.omega_i ** 2 * l_i * sin(self.phi_i)
def __str__(self):
return (f'xC={self.sC[0]:.2f}\n'
f'yC={self.sC[1]:.2f}\n'
f'vxC={self.vC[0]:.2f}\n'
f'vyC={self.vC[1]:.2f}\n'
f'axC={self.aC[0]:.2f}\n'
f'ayC={self.aC[1]:.2f}\n')
#固定点A、D状态
sA = [0, 0]
vA = [0, 0]
aA = [0, 0]
sD = [170, 0]
vD = [0, 0]
aD = [0, 0]
#存储旋转过程中F点状态的列表
xF = []
yF = []
vxF = []
vyF = []
axF = []
ayF = []
vF = []
aF = []
#用于图表绘制的角度坐标
phi = [item for item in range(0, 360)]
#∠BEF
theta_EBF = atan2(45, 60)
#BF杆长
l_BF = sqrt(45 ** 2 + 60 ** 2)
#以1为角度步长旋转
for i in range(360):
#转换为弧度制
delta_i = pi / 180 * i
#解算B点状态
AB = RR(sA, vA, aA, 0, 10, 0, 75, delta_i)
#解算C点状态和BC杆旋转状态
BCD = RRR(AB.sB, AB.vB, AB.aB, sD, vD, aD, 120, 135, 1)
#解算F点状态
BF = RR(AB.sB, AB.vB, AB.aB, BCD.phi_i, BCD.omega_i, BCD.alpha_i, l_BF, theta_EBF)
#将F点状态存储进列表并将单位由毫米转换为米
xF.append(BF.sB[0] / 100)
yF.append(BF.sB[1] / 100)
vxF.append(BF.vB[0] / 100)
vyF.append(BF.vB[1] / 100)
axF.append(BF.aB[0] / 100)
ayF.append(BF.aB[1] / 100)
vF.append(sqrt(vxF[i] ** 2 + vyF[i] ** 2))
aF.append(sqrt(axF[i] ** 2 + ayF[i] ** 2))
#绘制图表
font = {'family': 'MicroSoft YaHei',
'weight': 'bold',
'size': 10}
matplotlib.rc('font', **font)
fig = plt.figure(figsize=(20, 20), dpi=300)
plt.grid(alpha=1, linestyle="--")
plt.xlabel('x')
plt.ylabel('y')
plt.title("F点轨迹曲线")
plt.scatter(xF, yF)
plt.show()
plt.scatter(phi, vxF)
plt.xlabel('φ')
plt.ylabel('v_Fx')
plt.title("F点水平速度-角度曲线")
plt.show()
plt.scatter(phi, vyF)
plt.xlabel('φ')
plt.ylabel('v_Fy')
plt.title("F点竖直速度-角度曲线")
plt.show()
plt.scatter(phi, axF)
plt.xlabel('φ')
plt.ylabel('a_Fx')
plt.title("F点水平加速度-角度曲线")
plt.show()
plt.scatter(phi, ayF)
plt.xlabel('φ')
plt.ylabel('a_Fy')
plt.title("F点竖直加速度-角度曲线")
plt.show()
plt.xlabel('x')
plt.ylabel('y')
plt.title("F点轨迹-速度曲线")
plt.scatter(xF, yF, c=vF, cmap='coolwarm')
plt.colorbar()
plt.show()
plt.xlabel('x')
plt.ylabel('y')
plt.title("F点轨迹-加速度曲线")
plt.scatter(xF, yF, c=aF, cmap='coolwarm')
plt.colorbar()
plt.show()
#导出表格
data = {
'degree': range(0, 360),
'sFx': xF,
'sFy': yF,
'vFx': vxF,
'vFy': vyF,
'axF': axF,
'ayF': ayF,
}
df = pd.DataFrame(data)
df.to_excel('output.xlsx', index=False)
运行结果:
MATLAB图形仿真
RR.m(RR杆组位置解算函数)
function [sBx, sBy] = RR(sAx, sAy, phi_i, l_i, delta_i)
theta_i = phi_i + delta_i;
sBx = sAx + l_i * cos(theta_i);
sBy = sAy + l_i * sin(theta_i);
RRR.m(RRR杆组位置解算函数)
function [sCx, sCy, phi_i] = RRR(sBx, sBy, sDx, sDy, l_i, l_j)
BD = sqrt((sDx - sBx) ^ 2 + (sDy - sBy) ^ 2);
A0 = 2 * l_i * (sDx - sBx);
B0 = 2 * l_i * (sDy - sBy);
C0 = l_i ^ 2 + BD ^ 2 - l_j ^ 2;
phi_i = 2 * atan2(B0 + sqrt(A0 ^ 2 + B0 ^ 2 - C0 ^ 2), A0 + C0);
sCx = sBx + l_i * cos(phi_i);
sCy = sBy + l_i * sin(phi_i);
main.m
clear;
AB = 75;
BC = 120;
CD = 135;
AD = 170;
BE = 60;
EF = 45;
BF = sqrt(BE ^ 2 + EF ^ 2);
EBF = atan2(45, 60);
Bx = [];
Cx = [];
Dx = [];
Ex = [];
Fx = [];
By = [];
Cy = [];
Dy = [];
Ey = [];
Fy = [];
figure;
grid on;
filename = 'animation.gif';
% 位置解算
for i = 1: 1: 361
delta_i = i * (pi / 180);
[sBx, sBy] = RR(0, 0, 0, AB, delta_i);
[sCx, sCy, phi_i] = RRR(sBx, sBy, AD, 0, BC, CD);
[sFx, sFy] = RR(sBx, sBy, phi_i, BF, EBF);
sEx = (sBx + sCx) / 2;
sEy = (sBy + sCy) / 2;
Bx(end + 1) = sBx;
Cx(end + 1) = sCx;
Ex(end + 1) = sEx;
Fx(end + 1) = sFx;
By(end + 1) = sBy;
Cy(end + 1) = sCy;
Ey(end + 1) = sEy;
Fy(end + 1) = sFy;
end
for i = 1: 1: 360
clf;
hold on;
% 绘制杆件
plot([0, Bx(i)], [0, By(i)], 'b');
plot([Bx(i), Cx(i)], [By(i), Cy(i)], 'b');
plot([Cx(i), AD], [Cy(i), 0], 'b');
plot([Ex(i), Fx(i)], [Ey(i), Fy(i)], 'b');
% 绘制运动副
plot(0, 0, 'ro');
plot(Bx(i), By(i), 'ro');
plot(Cx(i), Cy(i), 'ro');
plot(AD, 0, 'ro');
plot(Ex(i), Ey(i), 'ro');
plot(Fx(i), Fy(i), 'ro');
plot(Fx, Fy)
hold off;
axis([-100 200 -100 200]);
% 生成gif
frame = getframe(gcf);
im = frame2im(frame);
[imind, cm] = rgb2ind(im, 256)
if i == 1
imwrite(imind, cm, filename, 'gif', 'LoopCount', inf, 'DelayTime', 0.01);
else
imwrite(imind, cm, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.01);
end
pause(0.01);
end
运行结果: