一、前置条件

1. 机器人型号：Kawasaki RS080N

2. 坐标系建系规则：右手定则（正前方为X+,左边为Y+，向上为Z+）

3. 旋转顺序规定：J1和J2顺时针为正方向，J3,J4,J5,J6逆时针为正方向

二、标准DH法

1. 机器人建系

机器人建系如下

2. 标准DH表

根据上图列出标准DH表如下：

关节	d_i（mm）	θ_i 初始值	a_i（mm）	α_i（rad）
J1	680	θ1	150	π/2
J2	0	θ2 + π/2	870	0
J3	0	θ3 + π/2	0	π/2
J4	1080	θ4	0	-π/2
J5	0	θ5	0	π/2
J6	165	θ6	0	0

三、正运动学推导

使用标准 DH 方法，单关节齐次变换矩阵为：

python代码如下：

def dh_transform(i, theta):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha[i]), np.sin(alpha[i])
    return np.array([
        [ct, -st * ca, st * sa, a[i] * ct],
        [st,  ct * ca, -ct * sa, a[i] * st],
        [0,     sa,     ca,     d[i]],
        [0,     0,      0,      1]
    ])

整体正解：

def forward_kinematics(q_deg):
    T = np.eye(4)
    for i in range(6):
        theta = joint_sign[i] * np.radians(q_deg[i]) + theta_offset[i]
        T = T @ dh_transform(i, theta)
    return T

四、逆运动学

1. 求腕部中心 P_w

末端位置 P06 = (px, py, pz)
末端 z 轴方向 a06 = (ax, ay, az)

腕部中心 P_wc = P06 - d6 * z_6：

Wx = px - d6 * ax
Wy = py - d6 * ay
Wz = pz - d6 * az

2. 求解 θ1

3. 求解 θ2, θ3

平面三角形形成的两边与角：

定义中间变量：

使用余弦定理：

求 θ3：

再根据法向向量投影：

4. 求解 R03、R36

分别通过前三个关节计算 R03，再用：

5. θ4 ~ θ6 姿态求解

若非奇异（sin(θ5) ≠ 0）：

若奇异（sin(θ5) ≈ 0）：

6. 逆运动学完整实现

• 先计算腕部中心

• 解出 θ1, θ2, θ3（共两组）

• 计算 R36 → 解 θ4~θ6（2 种）

• 每组组合拓展 ±360° 候选项

• 筛除不在关节限制范围内的解