3. 逆运动学解析解实现
任务目标
针对标准6R关节型机械臂（后三轴交于一点，满足Pieper准则），实现逆运动学解析解，给定末端位姿矩阵，求解全部8组关节角候选解，并验证正逆解互逆性。
构型与坐标系约定
采用改进DH参数，典型构型如下（单位：mm，角度：°）：
| i | alpha(i-1) | a(i-1) | d(i) | theta(i) |
|---|------------|--------|------|----------|
| 1 | 0 | 0 | 150 | theta1 |
| 2 | -90 | 0 | 0 | theta2 |
| 3 | 0 | 400 | 0 | theta3 |
| 4 | -90 | 0 | 430 | theta4 |
| 5 | 90 | 0 | 0 | theta5 |
| 6 | -90 | 0 | 80 | theta6 |末端位姿矩阵T06为4×4齐次变换矩阵，由正运动学计算得到。
解析解推导要点
利用腕心分离法：
- 求theta1：由腕心位置
p_w = p_06 - d6 * R_06 * [0,0,1]^T，得theta1 = atan2(p_w.y, p_w.x)，存在两个解（±180°偏移）。 - 求theta2、theta3：在theta1确定后，将问题转化为平面二连杆求角，利用余弦定理得theta3两个解，再由几何关系求theta2。
- 求theta4、theta5、theta6：由
R36 = R30^T * R06，根据欧拉角（ZYZ）分解得theta4、theta5、theta6，其中theta4和theta6各有两个解，theta5由atan2(sqrt(r31^2+r32^2), r33)确定。
组合后共8组候选解（部分构型可能退化）。
完整实现代码
import numpy as np
from numpy import sin, cos, arctan2, sqrt, pi
# 改进DH参数
DH = [
{'alpha': 0, 'a': 0, 'd': 150, 'theta_offset': 0},
{'alpha': -pi/2,'a': 0, 'd': 0, 'theta_offset': 0},
{'alpha': 0, 'a': 400, 'd': 0, 'theta_offset': 0},
{'alpha': -pi/2,'a': 0, 'd': 430, 'theta_offset': 0},
{'alpha': pi/2, 'a': 0, 'd': 0, 'theta_offset': 0},
{'alpha': -pi/2,'a': 0, 'd': 80, 'theta_offset': 0},
]
def dh_transform(theta, dh_param):
"""计算单个DH变换矩阵"""
alpha = dh_param['alpha']
a = dh_param['a']
d = dh_param['d']
ct, st = cos(theta), sin(theta)
ca, sa = cos(alpha), sin(alpha)
return np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1]
])
def forward_kinematics(q):
"""正运动学：关节角列表 -> T06"""
T = np.eye(4)
for i in range(6):
T = T @ dh_transform(q[i], DH[i])
return T
def inverse_kinematics(T06):
"""逆运动学解析解，返回所有候选解列表（每个解为6个关节角）"""
solutions = []
# 提取末端位置和姿态
p = T06[:3, 3]
R = T06[:3, :3]
d6 = DH[5]['d']
# 腕心位置
pw = p - d6 * R @ np.array([0, 0, 1.0])
px, py, pz = pw
# ---- theta1 ----
r = sqrt(px**2 + py**2)
if r < 1e-6:
return [] # 奇异：腕心在轴1轴线上
theta1_candidates = [arctan2(py, px), arctan2(-py, -px)]
for theta1 in theta1_candidates:
c1, s1 = cos(theta1), sin(theta1)
# ---- theta3 ----
# 在theta1确定后，计算theta2/theta3平面的几何参数
# 从基座到腕心在平面内的投影
x_plane = c1*px + s1*py # 沿x1方向
z_plane = pz # 沿z0方向
# 考虑DH偏移：a1=0, d1=150, a2=0, d2=0
# 腕心相对关节2坐标系的位置
xw = x_plane
zw = z_plane - DH[0]['d']
# 二连杆参数：L1=a2=400, L2=a3=400(实际a3=0但d4=430偏移)
# 注意：关节3后d4=430沿z3方向，需在平面中处理
# 简化：将d4视为连杆长度的一部分（Pieper准则）
L1 = DH[2]['a'] # 400
L2 = DH[3]['d'] # 430
D = (xw**2 + zw**2 - L1**2 - L2**2) / (2*L1*L2)
D = np.clip(D, -1.0, 1.0)
theta3_1 = arctan2(sqrt(1-D**2), D)
theta3_2 = arctan2(-sqrt(1-D**2), D)
for theta3 in [theta3_1, theta3_2]:
# ---- theta2 ----
phi = arctan2(zw, xw)
psi = arctan2(L2*sin(theta3), L1 + L2*cos(theta3))
theta2 = phi - psi
# ---- theta4, theta5, theta6 ----
# 计算R30（前三个关节的旋转矩阵）
T01 = dh_transform(theta1, DH[0])
T12 = dh_transform(theta2, DH[1])
T23 = dh_transform(theta3, DH[2])
R30 = (T01 @ T12 @ T23)[:3, :3]
# R36 = R30^T * R06
R36 = R30.T @ R
# ZYZ欧拉角分解
r31 = R36[2, 0]
r32 = R36[2, 1]
r33 = R36[2, 2]
theta5 = arctan2(sqrt(r31**2 + r32**2), r33)
if abs(theta5) < 1e-6:
# 奇异：theta5=0，theta4和theta6退化
theta4 = 0.0
theta6 = arctan2(-R36[0, 1], R36[0, 0])
solutions.append([theta1, theta2, theta3, theta4, theta5, theta6])
elif abs(theta5 - pi) < 1e-6:
theta4 = 0.0
theta6 = arctan2(R36[0, 1], -R36[0, 0])
solutions.append([theta1, theta2, theta3, theta4, theta5, theta6])
else:
for theta5_sign in [1.0, -1.0]:
theta5_cur = theta5_sign * theta5
theta4 = arctan2(r32/sin(theta5_cur), r31/sin(theta5_cur))
theta6 = arctan2(R36[1, 2]/sin(theta5_cur), -R36[0, 2]/sin(theta5_cur))
solutions.append([theta1, theta2, theta3, theta4, theta5_cur, theta6])
# 去重（数值容差）
unique_solutions = []
for sol in solutions:
if not any(np.allclose(sol, existing, atol=1e-6) for existing in unique_solutions):
unique_solutions.append(sol)
return unique_solutions
def verify_solution(q, T06_target):
"""验证单组解的正逆互逆性"""
T_calc = forward_kinematics(q)
return np.allclose(T_calc, T06_target, atol=1e-6)
# ============ 测试与验证 ============
if __name__ == "__main__":
# 测试1：随机生成关节角 -> 正解 -> 逆解 -> 验证
np.random.seed(42)
q_test = np.random.uniform(-pi, pi, 6)
T_target = forward_kinematics(q_test)
print("目标关节角:", np.round(q_test, 4))
print("目标位姿矩阵:\n", np.round(T_target, 4))
all_solutions = inverse_kinematics(T_target)
print(f"\n找到 {len(all_solutions)} 组候选解")
# 验证所有解
valid_count = 0
for i, q_sol in enumerate(all_solutions):
is_valid = verify_solution(q_sol, T_target)
if is_valid:
valid_count += 1
print(f"解{i+1}: {np.round(q_sol, 4)} 正逆互验: {'通过' if is_valid else '失败'}")
print(f"\n验证结果: {valid_count}/{len(all_solutions)} 组解通过正逆互验")
# 测试2：检查是否包含原始关节角（或等效解）
found_original = any(np.allclose(q_test, sol, atol=1e-4) for sol in all_solutions)
print(f"原始关节角是否在解集中: {'是' if found_original else '否（可能为等效解）'}")运行与验证
运行方式：
python ik_solver.py验证方法：
- 程序随机生成一组关节角，通过正运动学得到目标位姿矩阵；
- 调用逆运动学求解器，输出全部候选解；
- 对每组候选解重新执行正运动学，与目标位姿矩阵比较，误差小于
1e-6即判定通过； - 检查原始关节角是否出现在解集中（由于多解性，可能以等效解形式出现）。
通过条件：所有输出的候选解均通过正逆互验，且解的数量在1~8之间（非奇异情况下通常为8组）。
多解情况说明
解析解天然产生多组候选解：theta1有2个候选，theta3有2个候选，theta4/theta6组合有2种（theta5非奇异时），共2×2×2=8组。实际使用时需根据关节限位、避障等约束筛选最优解。本实现已包含数值去重逻辑，避免重复输出相同解。