计算四连杆机构的运动学

一、 理论基础：闭环向量方程法

对于最常见的平面铰链四连杆机构（如下图所示），我们可以建立其闭环向量方程。
过程
输出
"位置: θ₃, θ₄"
"速度: ω₃, ω₄

（对时间求一阶导）"
"加速度: α₃, α₄

（对时间求二阶导）"
输入
"主动杆（如L2）

已知：θ₂, ω₂, α₂"
"建立闭环向量方程

L₁ + L₂ = L₃ + L₄"
"分解为X、Y方向

得到位移方程组"
"求解非线性方程组

得从动杆角度 θ₃, θ₄"

将所有连杆用向量表示，其向量和为零：
L₂ + L₃ = L₁ + L₄

将这个向量方程分解到直角坐标系的X和Y轴上，就得到了位移方程组：

X方向 ：
L₂ * cos(θ₂) + L₃ * cos(θ₃) - L₄ * cos(θ₄) - L₁ * cos(θ₁) = 0

Y方向 ：
L₂ * sin(θ₂) + L₃ * sin(θ₃) - L₄ * sin(θ₄) - L₁ * sin(θ₁) = 0

已知条件：

• 连杆长度：L₁, L₂, L₃, L₄
• 机架（固定杆）与X轴夹角：θ₁（通常为0，即水平）
• 主动杆（假设为L₂）的瞬时角度：θ₂（通常作为输入变量）

未知量：

• 从动杆的角度：θ₃, θ₄

这是一个包含两个未知数的非线性方程组。针对这一问题，主要有两种数值求解方法：

二、 两种核心计算方法对比与选择

方法	核心思想	优点	缺点	MATLAB实现推荐
1. 数值迭代法 (如牛顿-拉夫森法)	基于初始猜测值，通过迭代使函数值逼近零来求解。	通用性强，适用于任何满足杆长条件的四连杆机构。	需要良好的初始猜测值以保证收敛；对于奇异位置附近可能不稳定。	fsolve 函数。
2. 几何/解析法 (如使用半角正切公式)	利用三角恒等式消元，得到一个关于tan(θ₃/2)或tan(θ₄/2)的一元二次方程。	计算速度快，不存在迭代不收敛的问题；概念清晰。	推导过程稍复杂；可能需要对解进行象限判断。	直接套用求根公式，利用 atan2 函数确定象限。

选择建议 ：对于大多数应用，特别是需要连续分析机构整个运动周期时，几何/解析法 因其稳定高效 而成为首选。数值迭代法则更适用于复杂机构或作为通用求解器。

三、 MATLAB实现方案（推荐几何法）

以下是采用几何解析法求解位置，并通过求导进行速度、加速度分析的MATLAB实现框架。

function [theta3, theta4, omega3, omega4, alpha3, alpha4] = fourbar_kinematics(L1, L2, L3, L4, theta1, theta2, omega2, alpha2)
% 输入:
%   L1, L2, L3, L4: 连杆长度（L1为机架）
%   theta1: 机架角度(弧度)，通常为0
%   theta2: 主动杆(杆2)角度(弧度)
%   omega2: 主动杆角速度(rad/s)
%   alpha2: 主动杆角加速度(rad/s^2)
% 输出:
%   theta3, theta4: 从动杆3和4的角度(弧度)
%   omega3, omega4: 从动杆角速度
%   alpha3, alpha4: 从动杆角加速度

    % 1. 位置分析（求解theta4）
    K1 = L1 / L2;
    K2 = L1 / L4;
    K3 = (L2^2 - L3^2 + L4^2 + L1^2) / (2 * L2 * L4);
    
    A = cos(theta2 - theta1) - K1 * cos(theta1) + K2 * cos(theta1) + K3;
    B = -2 * sin(theta2 - theta1);
    C = K1 * cos(theta1) - K2 * cos(theta1) + K3 - cos(theta2 - theta1);
    
    % 解一元二次方程求 t = tan(theta4/2)
    discriminant = B.^2 - 4 .* A .* C;
    if any(discriminant < 0)
        error('当前杆长组合或输入角度下，机构无法装配。');
    end
    
    t4 = (-B - sqrt(discriminant)) ./ (2 * A); % 通常取此模式分支
    % t4 = (-B + sqrt(discriminant)) ./ (2 * A); % 另一种装配模式
    
    theta4 = 2 * atan(t4);
    
    % 2. 求解theta3 (利用向量方程)
    K4 = L1 / L3;
    K5 = (L4^2 - L1^2 - L2^2 - L3^2) / (2 * L2 * L3);
    D = cos(theta2 - theta1) - K4 * cos(theta1) + K5;
    E = -2 * sin(theta2 - theta1);
    F = K4 * cos(theta1) + K5 + cos(theta2 - theta1);
    
    t3 = (-E - sqrt(E.^2 - 4 .* D .* F)) ./ (2 * D);
    theta3 = 2 * atan(t3);
    
    % 3. 速度分析（对位移方程求导）
    A_mat = [L3*sin(theta3), -L4*sin(theta4); 
             -L3*cos(theta3), L4*cos(theta4)];
    B_vec = [L2*omega2*sin(theta2); 
             -L2*omega2*cos(theta2)];
    
    omega_vec = A_mat \ B_vec; % 解线性方程组
    omega3 = omega_vec(1);
    omega4 = omega_vec(2);
    
    % 4. 加速度分析（对速度方程求导）
    C_vec = [L2*alpha2*sin(theta2) + L2*omega2^2*cos(theta2) + L3*omega3^2*cos(theta3) - L4*omega4^2*cos(theta4);
             -L2*alpha2*cos(theta2) + L2*omega2^2*sin(theta2) + L3*omega3^2*sin(theta3) - L4*omega4^2*sin(theta4)];
    
    alpha_vec = A_mat \ C_vec;
    alpha3 = alpha_vec(1);
    alpha4 = alpha_vec(2);
end

使用示例：

% 定义连杆参数 (长度单位一致即可，如mm)
L = [100, 50, 80, 120]; % [L1, L2, L3, L4]
theta1 = 0; % 机架水平
theta2 = deg2rad(45); % 主动杆在45度位置
omega2 = deg2rad(30); % 假设主动杆以30度/秒匀速转动
alpha2 = 0; % 角加速度为0

% 调用函数进行计算
[th3, th4, om3, om4, al3, al4] = fourbar_kinematics(L(1), L(2), L(3), L(4), theta1, theta2, omega2, alpha2);

% 输出结果
fprintf('杆3角度: %.2f°, 角速度: %.2f rad/s, 角加速度: %.2f rad/s^2\n', rad2deg(th3), om3, al3);
fprintf('杆4角度: %.2f°, 角速度: %.2f rad/s, 角加速度: %.2f rad/s^2\n', rad2deg(th4), om4, al4);

参考代码 计算四连杆机构运动学 www.youwenfan.com/contentcsp/96225.html

四、 关键点与进阶提示

1. 杆长条件（Grashof准则） ：并非任意四根杆都能构成可连续运动的铰链四连杆。最常见的曲柄摇杆机构需要满足"最短杆与最长杆长度之和 ≤ 其余两杆长度之和"，且最短杆为机架的对边（连架杆）。
2. 两个装配模式 ：对于同一组输入（θ₂），机构通常有两种可能的构型（对应上述代码中一元二次方程的"±"两个解），分别称为开式分支 和交叉分支。实际机构只能处于其中一种，需要根据初始装配情况选择正确的解。
3. 可视化 ：计算后强烈建议使用MATLAB的plot或animatedline函数绘制机构运动动画，直观验证计算结果的正确性。
4. 扩展 ：对于更复杂的滑块-曲柄机构 或多杆机构，核心思想依然是建立闭环向量方程并求解，只是方程形式会有所不同。