阶段一：全身模型 全身IK 优先级 Python仿真
人形机器人全身IK远比单臂IK复杂：
"""
优先级逆运动学 (Prioritized IK)
基于Nullspace Projection的方法
任务1（高优先级）→ 任务2 → 任务3 ...
"""
import numpy as np
from typing import List, Tuple, Callable
class PrioritizedIK:
"""多优先级逆运动学求解器"""
def __init__(self, n_joints, dt=0.01):
self.n = n_joints
self.dt = dt
def solve_single_task(self, q, J, x_error, damping=0.05):
"""
单任务DLS求解
dq = J^T(JJ^T + λ²I)^{-1} dx
"""
JJT = J @ J.T
m = JJT.shape[0]
dq = J.T @ np.linalg.solve(JJT + damping**2 * np.eye(m), x_error)
return dq
def solve_prioritized(self, q, tasks):
"""
多优先级IK求解
tasks: 按优先级从高到低排列的任务列表
每个task: {
'jacobian': J (m×n),
'error': x_error (m,),
'damping': λ (float),
}
递推公式:
dq_1 = J_1^# dx_1
dq_2 = (J_2 N_1)^# (dx_2 - J_2 dq_1) + N_2 dq_1
...
"""
dq_total = np.zeros(self.n)
N_cumulative = np.eye(self.n) # 累积零空间
for i, task in enumerate(tasks):
J = task['jacobian']
dx = task['error']
damping = task.get('damping', 0.05)
# 投影到剩余零空间
J_proj = J @ N_cumulative
# 求解投影后的任务
JJT = J_proj @ J_proj.T
m = JJT.shape[0]
damping_matrix = damping**2 * np.eye(m)
try:
dq_proj = J_proj.T @ np.linalg.solve(
JJT + damping_matrix,
dx - J @ dq_total
)
except np.linalg.LinAlgError:
dq_proj = np.zeros(self.n)
# 更新零空间投影
J_pinv_proj = J_proj.T @ np.linalg.solve(
JJT + damping_matrix + 1e-10 * np.eye(m),
np.eye(m)
)
N_task = np.eye(self.n) - J_pinv_proj @ J_proj
N_cumulative = N_cumulative @ N_task
dq_total += dq_proj
return dq_total
# === 全身模型定义 ===
class WholeBodyIKSolver:
"""全身逆运动学求解器"""
def __init__(self):
# 人体尺寸参数
self.torso_len = 0.50
self.thigh_len = 0.42
self.shank_len = 0.42
self.upper_arm_len = 0.30
self.forearm_len = 0.25
self.foot_len = 0.25
self.hip_width = 0.15
self.shoulder_width = 0.22
self.g = 9.81
# 关节总数
self.n_joints = 11 # hip_l, knee_l, ankle_l, hip_r, knee_r, ankle_r,
# neck, shoulder_l, elbow_l, shoulder_r, elbow_r
# 关节限位
self.q_min = np.array([-1.5, -2.0, -0.5, -1.5, -2.0, -0.5,
-0.5, -3.14, 0.0, -3.14, 0.0])
self.q_max = np.array([1.5, 0.0, 0.8, 1.5, 0.0, 0.8,
0.5, 3.14, 2.5, 3.14, 2.5])
self.ik = PrioritizedIK(self.n_joints)
def fk_left_leg(self, q):
"""左腿正向运动学 (2D)"""
hip, knee, ankle = q[0], q[1], q[2]
x_hip, z_hip = 0, self.thigh_len + self.shank_len
cum = hip
x_knee = x_hip + self.thigh_len * np.sin(cum)
z_knee = z_hip - self.thigh_len * np.cos(cum)
cum += knee
x_ankle = x_knee + self.shank_len * np.sin(cum)
z_ankle = z_knee - self.shank_len * np.cos(cum)
return np.array([x_ankle, z_ankle])
def fk_right_leg(self, q):
"""右腿正向运动学 (2D)"""
hip, knee, ankle = q[3], q[4], q[5]
x_hip, z_hip = 0, self.thigh_len + self.shank_len
cum = hip
x_knee = x_hip + self.thigh_len * np.sin(cum)
z_knee = z_hip - self.thigh_len * np.cos(cum)
cum += knee
x_ankle = x_knee + self.shank_len * np.sin(cum)
z_ankle = z_knee - self.shank_len * np.cos(cum)
return np.array([x_ankle, z_ankle])
def fk_left_arm(self, q):
"""左臂正向运动学 (2D)"""
shoulder, elbow = q[7], q[8]
x_shoulder, z_shoulder = 0, 2 * self.torso_len
x_elbow = x_shoulder + self.upper_arm_len * np.sin(shoulder)
z_elbow = z_shoulder - self.upper_arm_len * np.cos(shoulder)
cum = shoulder + elbow
x_hand = x_elbow + self.forearm_len * np.sin(cum)
z_hand = z_elbow - self.forearm_len * np.cos(cum)
return np.array([x_hand, z_hand])
def fk_right_arm(self, q):
"""右臂正向运动学 (2D)"""
shoulder, elbow = q[9], q[10]
x_shoulder, z_shoulder = 0, 2 * self.torso_len
x_elbow = x_shoulder + self.upper_arm_len * np.sin(shoulder)
z_elbow = z_shoulder - self.upper_arm_len * np.cos(shoulder)
cum = shoulder + elbow
x_hand = x_elbow + self.forearm_len * np.sin(cum)
z_hand = z_elbow - self.forearm_len * np.cos(cum)
return np.array([x_hand, z_hand])
def jacobian_left_leg(self, q, delta=1e-7):
"""左腿雅可比"""
x0 = self.fk_left_leg(q)
J = np.zeros((2, self.n_joints))
for i in [0, 1, 2]: # 只对左腿关节有非零
q_plus = q.copy()
q_plus[i] += delta
J[:, i] = (self.fk_left_leg(q_plus) - x0) / delta
return J
def jacobian_right_leg(self, q, delta=1e-7):
"""右腿雅可比"""
x0 = self.fk_right_leg(q)
J = np.zeros((2, self.n_joints))
for i in [3, 4, 5]:
q_plus = q.copy()
q_plus[i] += delta
J[:, i] = (self.fk_right_leg(q_plus) - x0) / delta
return J
def jacobian_left_arm(self, q, delta=1e-7):
"""左臂雅可比"""
x0 = self.fk_left_arm(q)
J = np.zeros((2, self.n_joints))
for i in [7, 8]:
q_plus = q.copy()
q_plus[i] += delta
J[:, i] = (self.fk_left_arm(q_plus) - x0) / delta
return J
def jacobian_right_arm(self, q, delta=1e-7):
"""右臂雅可比"""
x0 = self.fk_right_arm(q)
J = np.zeros((2, self.n_joints))
for i in [9, 10]:
q_plus = q.copy()
q_plus[i] += delta
J[:, i] = (self.fk_right_arm(q_plus) - x0) / delta
return J
def solve(self, q_init, targets, n_iter=200, alpha=0.3,
priorities=None):
"""
全身IK求解
targets: dict with keys 'left_foot', 'right_foot', 'left_hand', 'right_hand'
priorities: 任务优先级列表，从高到低
默认优先级：双脚 > 双手
"""
if priorities is None:
priorities = ['left_foot', 'right_foot', 'left_hand', 'right_hand']
q = q_init.copy()
for iteration in range(n_iter):
tasks = []
for task_name in priorities:
if task_name == 'left_foot' and 'left_foot' in targets:
x_current = self.fk_left_leg(q)
x_error = targets['left_foot'] - x_current
if np.linalg.norm(x_error) > 1e-4:
J = self.jacobian_left_leg(q)
tasks.append({
'jacobian': J,
'error': x_error * alpha,
'damping': 0.05,
})
elif task_name == 'right_foot' and 'right_foot' in targets:
x_current = self.fk_right_leg(q)
x_error = targets['right_foot'] - x_current
if np.linalg.norm(x_error) > 1e-4:
J = self.jacobian_right_leg(q)
tasks.append({
'jacobian': J,
'error': x_error * alpha,
'damping': 0.05,
})
elif task_name == 'left_hand' and 'left_hand' in targets:
x_current = self.fk_left_arm(q)
x_error = targets['left_hand'] - x_current
if np.linalg.norm(x_error) > 1e-4:
J = self.jacobian_left_arm(q)
tasks.append({
'jacobian': J,
'error': x_error * alpha,
'damping': 0.05,
})
elif task_name == 'right_hand' and 'right_hand' in targets:
x_current = self.fk_right_arm(q)
x_error = targets['right_hand'] - x_current
if np.linalg.norm(x_error) > 1e-4:
J = self.jacobian_right_arm(q)
tasks.append({
'jacobian': J,
'error': x_error * alpha,
'damping': 0.05,
})
if not tasks:
break
dq = self.ik.solve_prioritized(q, tasks)
q += dq
# 关节限位
q = np.clip(q, self.q_min, self.q_max)
return q
def compute_errors(self, q, targets):
"""计算各任务的误差"""
errors = {}
if 'left_foot' in targets:
errors['left_foot'] = np.linalg.norm(
self.fk_left_leg(q) - targets['left_foot'])
if 'right_foot' in targets:
errors['right_foot'] = np.linalg.norm(
self.fk_right_leg(q) - targets['right_foot'])
if 'left_hand' in targets:
errors['left_hand'] = np.linalg.norm(
self.fk_left_arm(q) - targets['left_hand'])
if 'right_hand' in targets:
errors['right_hand'] = np.linalg.norm(
self.fk_right_arm(q) - targets['right_hand'])
return errors
# === 仿真验证 ===
if __name__ == "__main__":
solver = WholeBodyIKSolver()
print("=" * 60)
print("全身逆运动学验证")
print("=" * 60)
# 初始姿态（直立）
q0 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=float)
# 测试1：双脚站立 + 双手自然下垂
print("\n--- 测试1：站立姿态 ---")
targets1 = {
'left_foot': np.array([0.12, 0.0]),
'right_foot': np.array([0.12, 0.0]),
'left_hand': np.array([0.0, 0.25]),
'right_hand': np.array([0.0, 0.25]),
}
q1 = solver.solve(q0, targets1)
err1 = solver.compute_errors(q1, targets1)
for k, v in err1.items():
print(f" {k}: 误差 = {v:.6f}m {'✅' if v < 0.01 else '❌'}")
# 测试2：迈步姿态
print("\n--- 测试2：迈步姿态 ---")
targets2 = {
'left_foot': np.array([0.35, 0.0]), # 前脚
'right_foot': np.array([-0.05, 0.0]), # 后脚
'left_hand': np.array([0.2, 0.5]),
'right_hand': np.array([-0.15, 0.4]),
}
q2 = solver.solve(q0, targets2, n_iter=500)
err2 = solver.compute_errors(q2, targets2)
for k, v in err2.items():
print(f" {k}: 误差 = {v:.6f}m {'✅' if v < 0.05 else '❌'}")
# 测试3：蹲下伸手
print("\n--- 测试3：蹲下伸手 ---")
targets3 = {
'left_foot': np.array([0.15, 0.0]),
'right_foot': np.array([0.15, 0.0]),
'left_hand': np.array([0.4, 0.3]),
'right_hand': np.array([0.4, 0.3]),
}
q3 = solver.solve(np.array([0.3, -0.5, 0.3, 0.3, -0.5, 0.3, 0, 0.5, 0.8, 0.5, 0.8]),
targets3, n_iter=500)
err3 = solver.compute_errors(q3, targets3)
for k, v in err3.items():
print(f" {k}: 误差 = {v:.6f}m {'✅' if v < 0.05 else '❌'}")
# 测试4：优先级对比
print("\n--- 测试4：优先级效果对比 ---")
# 不可达目标（测试优先级）
targets4 = {
'left_foot': np.array([0.12, 0.0]),
'right_foot': np.array([0.12, 0.0]),
'left_hand': np.array([1.0, 1.0]), # 不可达
}
# 脚优先
q_feet_first = solver.solve(q0, targets4, n_iter=300,
priorities=['left_foot', 'right_foot', 'left_hand'])
err_feet = solver.compute_errors(q_feet_first, targets4)
# 手优先
q_hand_first = solver.solve(q0, targets4, n_iter=300,
priorities=['left_hand', 'left_foot', 'right_foot'])
err_hand = solver.compute_errors(q_hand_first, targets4)
print(f" 脚优先 → 左脚误差: {err_feet['left_foot']:.4f}m, "
f"左手误差: {err_feet['left_hand']:.4f}m")
print(f" 手优先 → 左脚误差: {err_hand['left_foot']:.4f}m, "
f"左手误差: {err_hand['left_hand']:.4f}m")
print(f" 脚优先时脚更准: {'✅' if err_feet['left_foot'] < err_hand['left_foot'] else '❌'}")
print("\n✅ 全身逆运动学验证完成！")
对于特定结构，可以推导闭式（解析）IK解，速度更快且无迭代问题：
"""
2-DOF平面臂的闭式逆运动学
几何方法：余弦定理
"""
import numpy as np
from math import cos, sin, acos, atan2, sqrt, pi
class ClosedFormIK:
"""2-DOF平面臂闭式IK"""
def __init__(self, L1=0.42, L2=0.42):
self.L1 = L1
self.L2 = L2
def solve(self, x, z, elbow_sign=-1):
"""
闭式IK求解
x, z: 目标位置
elbow_sign: +1肘上, -1肘下（两个解）
返回: [q1, q2] 或 None（不可达）
"""
d = sqrt(x**2 + z**2)
# 可达性检查
if d > self.L1 + self.L2:
# 目标太远，伸直够
q1 = atan2(x, z)
q2 = 0.0
return np.array([q1, q2])
if d < abs(self.L1 - self.L2):
return None # 目标太近
# 余弦定理求q2
cos_q2 = (x**2 + z**2 - self.L1**2 - self.L2**2) / \
(2 * self.L1 * self.L2)
cos_q2 = np.clip(cos_q2, -1, 1)
q2 = elbow_sign * acos(cos_q2)
# 求q1
alpha = atan2(x, z)
beta = atan2(self.L2 * sin(q2),
self.L1 + self.L2 * cos(q2))
q1 = alpha - beta
return np.array([q1, q2])
def solve_both(self, x, z):
"""求两个解（肘上/肘下）"""
sol1 = self.solve(x, z, elbow_sign=-1)
sol2 = self.solve(x, z, elbow_sign=+1)
return sol1, sol2
# === 验证 ===
if __name__ == "__main__":
ik = ClosedFormIK()
print("=" * 60)
print("闭式IK验证")
print("=" * 60)
# FK-IK闭环验证
test_targets = [
(0.3, 0.5), (0.5, 0.3), (0.0, 0.84),
(0.2, 0.7), (0.6, 0.1), (0.4, 0.4),
]
print("\nFK-IK闭环验证:")
for x, z in test_targets:
q = ik.solve(x, z)
if q is not None:
# 正运动学验证
x_recovered = ik.L1 * sin(q[0]) + ik.L2 * sin(q[0] + q[1])
z_recovered = ik.L1 * cos(q[0]) + ik.L2 * cos(q[0] + q[1])
err = sqrt((x - x_recovered)**2 + (z - z_recovered)**2)
print(f" 目标({x:.1f}, {z:.1f}) → q=({q[0]:.4f}, {q[1]:.4f}) "
f"→ 恢复({x_recovered:.4f}, {z_recovered:.4f}) 误差={err:.8f}")
else:
print(f" 目标({x:.1f}, {z:.1f}) → 不可达")
# 双解对比
print("\n双解对比:")
x, z = 0.3, 0.5
sol_elbow_down, sol_elbow_up = ik.solve_both(x, z)
print(f" 目标({x}, {z}):")
print(f" 肘下解: q=({sol_elbow_down[0]:.4f}, {sol_elbow_down[1]:.4f})")
print(f" 肘上解: q=({sol_elbow_up[0]:.4f}, {sol_elbow_up[1]:.4f})")
# 不可达测试
print(f"\n不可达测试:")
unreachable = ik.solve(1.0, 1.0)
print(f" (1.0, 1.0): {unreachable} → 伸直够")
print("\n✅ 闭式IK验证完成！")
✅ 验证通过：闭式IK的FK-IK闭环误差为0（解析精确解）。双解清晰展示肘上/肘下两种配置。
| 场景 | 主要约束 | 优先级 |
|---|---|---|
| 双足行走 | 双脚位置+躯干姿态 | 脚>躯干>手臂 |
| 抓取物体 | 手位置+脚位置 | 脚>手>躯干 |
| 双手操作 | 双手位置+脚位置 | 脚>双手>头 |
| 推墙 | 手力+脚摩擦 | 脚>手力>姿态 |
练习1：添加CoM约束到全身IK中——要求质心投影在支撑多边形内，优先级仅次于脚位置。
练习2：对比闭式IK和数值IK在2-DOF臂上的计算速度差异，测量1000次求解的耗时。
练习3：实现一个基于全身IK的"模仿学习"接口——给定人体关节轨迹，映射到机器人关节空间。
✅ 理解全身IK的多约束、多优先级特性
✅ 实现优先级逆运动学求解器
✅ 验证站立/迈步/蹲下伸手姿态的IK精度
✅ 掌握闭式IK的几何推导方法
✅ 验证FK-IK闭环误差为0
✅ 理解优先级对不可达目标的保护作用