五杆机构（Five-bar linkage）-两个圆的交点问题

用户11770632

发布于 2025-11-15 10:58:35

1130

📜 历史背景

五连杆机构（five-bar linkage）属于平面闭环机构的典型代表，是经典连杆机构的一种，发展历史可以追溯到19世纪中后期的机构学兴起阶段。

🕰️ 发展里程碑：

18~19世纪：基础连杆理论发展
- 机构学的基础由欧拉、伽利略和库利奥利等人打下。
- 四连杆机构成为研究基础。
- 五连杆作为拓展结构被提出，解决更复杂运动需求。
20世纪初：图解法与解析法
- 出现了运动链分析方法。
- 五连杆被用于研究复杂路径合成问题，特别是双输入控制。
20世纪后期：并联机构兴起
- 工业自动化对高刚度、高速度机构需求增加。
- 五连杆作为平面并联结构，被大量用于高速切割、描绘等任务。
- 应用于机器手、双臂协调控制等场景。
21世纪至今：机器人中的广泛应用
- 用于仿生机械臂、康复机器人、微操作平台。
- 加入电机控制系统，实现运动学、动力学实时解算。
- 与 Delta、Stewart 等并联结构共同构建“并联机器人族谱”。

五连杆运动学模型分析

🧩 基本结构简图（字母代表点，Lx代表连杆）：

固定点：A 和 B（地面）
主动连杆：L1（AC）、L2（BC）
从动连杆：L3（CD）、L4（CE）
末端平台或末端点：C
驱动关节：A、B 处各有一个电机控制角度 θ1, θ2

📐 几何参数定义：

符号	含义
A, B	两个固定点，作为驱动点（基础）
d	AB 的长度，也就是基座宽度
L1	AC，从点 A 到动点 C 的连杆长度
L2	BC，从点 B 到动点 C 的连杆长度
L3	CD，点 C 到 D 的从动连杆（可选扩展）
L4	CE，点 C 到 E 的从动连杆（可选扩展）
θ1	电机1（点 A 处）的转角
θ2	电机2（点 B 处）的转角
(x, y)	末端点 C 的平面坐标

正向与逆向运动学推导

📌 正向运动学（给定 θ1 和 θ2，求 x, y）

点 A 固定在原点 (0, 0)，点 B 固定在 (d, 0)
点 C 在两个驱动连杆端点分别为：
- 点 D = A + L1 × [cos(θ1), sin(θ1)]
- 点 E = B + L2 × [cos(θ2), sin(θ2)]
点 C 是点 D 和 E 的连杆末端交点，满足：
||C - D|| = L3 ||C - E|| = L4
这就是求两个圆的交点问题，中心分别为 D 和 E，半径分别为 L3 和 L4，可以使用几何法或解析法解出 (x, y)。

📌 逆向运动学（给定末端 C 坐标，求 θ1, θ2）

点 A 和 B 是已知的固定点

求出向量：

AC = C - A
BC = C - B

利用余弦定理求出 θ1 和 θ2：

θ1 = atan2(yC, xC) ± acos(Δ)
θ2 = atan2(yC, xC - d) ± acos(Δ)

其中 Δ 表示根据三角函数算出的夹角部分。

Python 与 MATLAB 模拟代码

🐍 Python 示例代码（正向运动学）

import numpy as np
import matplotlib.pyplot as plt

# 参数
L1 = L2 = 2
L3 = L4 = 2
d = 3  # AB距离

theta1 = np.deg2rad(60)
theta2 = np.deg2rad(120)

# 点 A 和 B
A = np.array([0, 0])
B = np.array([d, 0])

# 驱动杆末端
D = A + L1 * np.array([np.cos(theta1), np.sin(theta1)])
E = B + L2 * np.array([np.cos(theta2), np.sin(theta2)])

# 圆交点求解（两圆求交）
def circle_intersection(p1, r1, p2, r2):
    d = np.linalg.norm(p2 - p1)
    if d > r1 + r2 or d < abs(r1 - r2):
        return None  # no solution

    a = (r1**2 - r2**2 + d**2) / (2 * d)
    h = np.sqrt(r1**2 - a**2)
    mid = p1 + a * (p2 - p1) / d
    offset = h * np.array([-(p2[1] - p1[1]), p2[0] - p1[0]]) / d
    return mid + offset, mid - offset

result = circle_intersection(D, L3, E, L4)
if result:
    C1, C2 = result
    print("末端位置候选点：", C1, C2)

    # 绘图
    for C in [C1, C2]:
        plt.figure()
        plt.plot([A[0], D[0]], [A[1], D[1]], 'b-')
        plt.plot([B[0], E[0]], [B[1], E[1]], 'g-')
        plt.plot([D[0], C[0]], [D[1], C[1]], 'r-')
        plt.plot([E[0], C[0]], [E[1], C[1]], 'r-')
        plt.scatter([A[0], B[0], C[0]], [A[1], B[1], C[1]], c='k')
        plt.axis('equal')
        plt.title('五杆机构末端点')
        plt.grid(True)
        plt.show()

🧮 MATLAB 示例代码（正向运动学）

L1 = 2; L2 = 2; L3 = 2; L4 = 2;
d = 3;

theta1 = deg2rad(60);
theta2 = deg2rad(120);

A = [0, 0];
B = [d, 0];

D = A + L1 * [cos(theta1), sin(theta1)];
E = B + L2 * [cos(theta2), sin(theta2)];

[C1, C2] = circle_intersection(D, L3, E, L4);

disp('末端位置候选点:');
disp(C1); disp(C2);

% 绘图函数
figure;
hold on; axis equal; grid on;

plot([A(1), D(1)], [A(2), D(2)], 'b');
plot([B(1), E(1)], [B(2), E(2)], 'g');
plot([D(1), C1(1)], [D(2), C1(2)], 'r');
plot([E(1), C1(1)], [E(2), C1(2)], 'r');
plot(A(1), A(2), 'ko'); plot(B(1), B(2), 'ko'); plot(C1(1), C1(2), 'ko');
title('五杆机构运动图');

function [p3a, p3b] = circle_intersection(p1, r1, p2, r2)
    d = norm(p2 - p1);
    a = (r1^2 - r2^2 + d^2) / (2 * d);
    h = sqrt(r1^2 - a^2);
    p3m = p1 + a * (p2 - p1) / d;
    offset = h * [-1*(p2(2) - p1(2)), (p2(1) - p1(1))] / d;
    p3a = p3m + offset;
    p3b = p3m - offset;
end

图示