当前位置： 首页 > article >正文

【小项目】四连杆机构的Python运动学求解和MATLAB图形仿真

article 2025/3/18 12:19:53

任务

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

运行结果：


http://www.kler.cn/a/589734.html

相关文章：

  • 机器学习--卷积神经网络原理及MATLAB回归实现
  • Docker 使用指南
  • C# WPF编程-画刷（Brush）填充图形对象的颜色或图案
  • SpringBoot3和企业版Splunk集成（附加docker配置）
  • CoreData 调试警告：多个 NSEntityDescriptions 声明冲突的解决
  • 数模AI使用教程(新) 2025.3.17
  • Windows安全日志Defender 的配置被修改5007
  • Python数据可视化——生成数据（一）
  • Python基于Django和协同过滤算法实现电影推荐系统功能丰富版
  • 跟着AI复习一下pytorch原理和操作
  • OpenCV计算摄影学（22）将输入的彩色图像转换为两种风格的铅笔素描效果函数pencilSketch()
  • 嵌入式硬件篇---龙芯GPIO控制
  • CTF WEB题
  • 每日一题--进程与协程的区别
  • 【在校课堂笔记】Python 第5节课 总结
  • axios 和 fetch异同点
  • Java继承与反思，单例模式与静态的思考
  • 【redis】Jedis 操作 Redis 基础指令（下）
  • Mysql中创建表时的约束条件
  • 力扣刷题——2181.合并零之间的节点