craic/docs/arm.md

机械臂运动学推导

6-DOF 机械臂逆运动学和正运动学完整推导。

机械臂结构

关节配置

J1: 线性滑轨（垂直运动，高度 d1）
J2: 基座旋转（绕 Z 轴，XY 平面）
J3: 肘关节（绕 Z 轴，XY 平面）
J4: 腕关节（绕 Z 轴，XY 平面）
J5: 末端俯仰（0° = 水平）
J6: 夹爪旋转

几何参数

参数	值 (mm)	说明
L1	125	J2-J3 连杆长度
L2	125	J3-J4 连杆长度
x4	110	J4-TCP 水平偏移
z4	80	J4-TCP 垂直偏移（夹爪状态相关：张开 55mm，闭合 -100mm）

坐标系

基坐标系（右手系，Z 轴朝上）：

        Z↑ (高度)
        |
        |
        o----→ X (前)
       /
      /
     Y (左)

• 原点：J1 滑轨底部
• d1 范围：[-290, 0] mm（负值表示在基准面以下）

正运动学（FK）

已知关节状态 → 计算 TCP 位姿

输入

• d1: 高度（J1 线性位移）
• θ2, θ3, θ4: J2/J3/J4 角度（度）

输出

• (x, y, z): TCP 位置（mm）
• φ: TCP 偏航角（度）

推导

步骤 1：计算 J4 中心位置（XY 平面）

J2 和 J3 形成二连杆机构：

x_{j4} = L_1 \cos\theta_2 + L_2 \cos(\theta_2 + \theta_3)

y_{j4} = L_1 \sin\theta_2 + L_2 \sin(\theta_2 + \theta_3)

步骤 2：计算 TCP 偏航角

\phi = \theta_2 + \theta_3 + \theta_4

步骤 3：计算 TCP 位置

从 J4 中心沿 φ 方向偏移 x4，垂直偏移 z4：

x = x_{j4} + x_4 \cos\phi

y = y_{j4} + x_4 \sin\phi

z = d_1 - z_4

Python 实现

def forward_kinematics(d1, theta2_deg, theta3_deg, theta4_deg, L1=125, L2=125, x4=110, z4=80):
    """正运动学"""
    theta2 = math.radians(theta2_deg)
    theta3 = math.radians(theta3_deg)
    theta4 = math.radians(theta4_deg)
    phi = theta2 + theta3 + theta4
    
    # J4 中心
    x_j4 = L1 * math.cos(theta2) + L2 * math.cos(theta2 + theta3)
    y_j4 = L1 * math.sin(theta2) + L2 * math.sin(theta2 + theta3)
    
    # TCP 位置
    x = x_j4 + x4 * math.cos(phi)
    y = y_j4 + x4 * math.sin(phi)
    z = d1 - z4
    
    return x, y, z, math.degrees(phi)

逆运动学（IK）

已知 TCP 目标位姿 → 计算关节角度

输入

• (x, y, z): 目标位置（mm）
• φ: 目标偏航角（度）

输出

• d1: 高度
• θ2, θ3, θ4: 关节角度（度）

推导

步骤 1：计算 J4 中心目标位置

从 TCP 位置反向计算 J4 位置：

x_{j4} = x - x_4 \cos\phi

y_{j4} = y - x_4 \sin\phi

z_{j4} = z + z_4

d_1 = z_{j4}

步骤 2：计算平面距离

r = \sqrt{x_{j4}^2 + y_{j4}^2}

步骤 3：求解 θ3（余弦定理）

二连杆机构的标准解法：

\cos\theta_3 = \frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}

检查工作空间：

-1 \leq \cos\theta_3 \leq 1 \quad \Rightarrow \quad |L_1 - L_2| \leq r \leq L_1 + L_2

否则目标超出工作空间。

有两个解（肘部朝上 / 朝下）：

\theta_3 = \pm \arccos\left(\frac{r^2 - L_1^2 - L_2^2}{2L_1L_2}\right)

步骤 4：求解 θ2

\theta_2 = \arctan2(y_{j4}, x_{j4}) - \arctan2\left(L_2\sin\theta_3, L_1 + L_2\cos\theta_3\right)

步骤 5：求解 θ4

\theta_4 = \phi - \theta_2 - \theta_3

Python 实现

def inverse_kinematics(x, y, z, phi_deg, elbow_up=False, L1=125, L2=125, x4=110, z4=80):
    """逆运动学"""
    phi = math.radians(phi_deg)
    
    # 步骤 1: 计算 J4 中心
    x_j4 = x - x4 * math.cos(phi)
    y_j4 = y - x4 * math.sin(phi)
    z_j4 = z + z4
    d1 = z_j4
    
    # 步骤 2: 平面距离
    r2 = x_j4**2 + y_j4**2
    r = math.sqrt(r2)
    
    # 步骤 3: 求解 theta3
    cos_theta3 = (r2 - L1**2 - L2**2) / (2 * L1 * L2)
    
    if cos_theta3 < -1 or cos_theta3 > 1:
        raise ValueError(f"目标超出工作空间，r={r:.1f}mm")
    
    # 肘部朝上：负角度，肘部朝下：正角度
    sin_theta3 = -math.sqrt(1 - cos_theta3**2) if elbow_up else math.sqrt(1 - cos_theta3**2)
    theta3 = math.atan2(sin_theta3, cos_theta3)
    
    # 步骤 4: 求解 theta2
    theta2 = math.atan2(y_j4, x_j4) - math.atan2(
        L2 * sin_theta3,
        L1 + L2 * cos_theta3
    )
    
    # 步骤 5: 求解 theta4
    theta4 = phi - theta2 - theta3
    
    # 角度归一化到 [-180, 180)
    def normalize(angle):
        a = (angle + math.pi) % (2 * math.pi) - math.pi
        return a if a != -math.pi or angle <= 0 else math.pi
    
    return (
        d1,
        math.degrees(normalize(theta2)),
        math.degrees(normalize(theta3)),
        math.degrees(normalize(theta4))
    )

零点偏移

物理零点（机械对齐）与数学零点（直线构型）存在偏差：

关节	零点偏移
J2	+3°
J3	+7°
J4	+25°

转换关系

命令角度 ↔ 数学角度：

\theta_{\text{command}} = \theta_{\text{math}} + \text{offset}

\theta_{\text{math}} = \theta_{\text{command}} - \text{offset}

使用：

• 正运动学：先减去偏移（命令 → 数学），再计算
• 逆运动学：先计算（数学），再加上偏移（数学 → 命令）

工作空间

高度范围

z \in [-290 - z_4, 0 - z_4] = [-370, -80] \text{ mm}

水平范围

r_{\min} = |L_1 - L_2| + x_4 = 0 + 110 = 110 \text{ mm}

r_{\max} = L_1 + L_2 + x_4 = 125 + 125 + 110 = 360 \text{ mm}

可达圆环：半径 110mm 到 360mm

关节限位

关节	最小值	最大值	单位
height	-290	0	mm
J2	-110	115	°
J3	-120	145	°
J4	-90	130	°
J5	-180	180	°
J6	-180	180	°

奇异点

1. 肩部奇异点

当 J4 中心在原点正上方：

x_{j4} = y_{j4} = 0 \quad \Rightarrow \quad r = 0

此时 θ2 无定义（可取任意值）。

避免：保持 r > 10mm

2. 肘部奇异点

当机械臂完全伸直或完全折叠：

\theta_3 = 0° \text{ 或 } \pm 180°

此时运动控制不稳定。

避免：保持 |θ3| > 5°

运动插值

关节空间线性插值，避免笛卡尔空间的复杂路径规划。

算法

给定起点 q_start 和终点 q_end，生成 N 个中间点：

q_i = q_{\text{start}} + \frac{i}{N}(q_{\text{end}} - q_{\text{start}}), \quad i = 1, 2, \ldots, N

参数

• duration: 运动时长（秒）
• rate: 插值频率（Hz）
• steps = ceil(duration × rate): 插值点数

示例：duration=2.0s, rate=20Hz → steps=40

Python 实现

def interpolate_joints(start, end, duration=2.0, rate=20.0):
    """关节空间插值"""
    steps = max(1, int(math.ceil(duration * rate)))
    trajectory = []
    
    for i in range(1, steps + 1):
        t = i / steps
        state = {
            key: int(round(start[key] + t * (end[key] - start[key])))
            for key in start.keys()
        }
        trajectory.append(state)
    
    return trajectory

完整示例

示例 1：前方抓取

目标：抓取前方 250mm，右侧 50mm，高度 -150mm 的物体

# 逆运动学
d1, theta2, theta3, theta4 = inverse_kinematics(
    x=250, y=50, z=-150, phi_deg=0
)
# 输出: d1=-70, theta2=11.3°, theta3=-48.6°, theta4=37.3°

# 验证：正运动学
x, y, z, phi = forward_kinematics(d1, theta2, theta3, theta4)
# 输出: (250.0, 50.0, -150.0, 0.0°) ✓

示例 2：多段轨迹

# 初始位置
start = {'height': 0, 'j2': 0, 'j3': 0, 'j4': 0, 'j5': 81, 'j6': 30}

# 目标 1: 上方
_, theta2, theta3, theta4 = inverse_kinematics(200, 100, -50, 45)
waypoint1 = {'height': -50, 'j2': theta2, 'j3': theta3, 'j4': theta4, 'j5': 81, 'j6': 30}

# 目标 2: 抓取位置
_, theta2, theta3, theta4 = inverse_kinematics(200, 100, -150, 45)
waypoint2 = {'height': -150, 'j2': theta2, 'j3': theta3, 'j4': theta4, 'j5': -100, 'j6': -5}

# 生成轨迹
traj1 = interpolate_joints(start, waypoint1, duration=2.0)      # 2秒到上方
traj2 = interpolate_joints(waypoint1, waypoint2, duration=1.0)  # 1秒下降抓取

参考

UDP 命令格式

JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n

• 所有角度值已包含零点偏移
• 直接发送到 ESP32 的 UDP 端口 8888

相关工具

• tools/udp_control.py - 命令行控制（支持 joints 和 pose 模式）
• ros2/src/udp_teleop/udp_teleop/arm_control.py - ROS 节点
• docs/vision_calibration_horizontal.md - 相机坐标变换

测试

# 测试逆运动学
python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --dry-run

# 发送命令
python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --duration 2.0