【小项目】四连杆机构的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

运行结果：