《Python机器人基础原理》第九十四篇（修正版）：搬运机械臂——多目标点逆运动学批量求解与无解点位统计
声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。
1. 实际工业应用场景
周五夜班，汽车零部件焊接产线做新批次切换。一台3-DOF平面搬运机械臂（臂展R=0.55 m，额定负载5.0 kg，单轴最大角速度ω_max=2.5 rad/s，重复定位精度±0.05 mm，末端最大线速度0.8 m/s）需要执行48个取放料点位。工艺工程师小王把CAD导出的目标笛卡尔坐标批量导入上位机，点击"自动规划"——系统报出6个点位逆解失败。
小王盯着屏幕皱眉："这6个点位在CAD里明明在臂展圆内，为什么解不出来？是算法bug还是这些点确实超出工作空间？"
旁边的调试工程师老刘凑过来看："你的目标点Z坐标不是全零——CAD导出时带了0.1 mm量级的噪声，有些点Z到了0.02 m。这台是纯2D平面搬运臂，第三关节是腕部旋转，没有Z方向的伸缩自由度。Z≠0的点它永远到不了。另外，就算XY在平面内，如果距离基座超过0.55 m，或者刚好在臂展边界上数值奇异，逆解也会失败。"
小王："那我需要提前离线筛一遍——把所有目标点批量跑一遍逆运动学，统计哪些能解、哪些不能解、无解的原因是什么。别等到了产线上才发现某个点够不到，耽误整条线节拍。"
老刘点头："写个离线仿真，用解析逆解，3-DOF平面臂，批量跑48个点，输出成功率、失败点坐标、失败原因分类。全部在PC上跑，不动真实设备。"
构型约定：本篇采用3-DOF平面搬运机械臂（2R+1R腕转），基座J1回转，大臂J2俯仰，腕部J3旋转（不改变末端位置，仅改变末端朝向）。正运动学使用标准DH（连杆1 a=0.25 m, α=0°；连杆2 a=0.30 m, α=0°；连杆3 a=0.0 m, d=0.0 m, α=0°）。逆运动学为解析解（几何法），仅求解位置（X, Y），Z固定为0。目标点Z≠0时直接判定为超出工作空间。技术栈限定numpy + matplotlib。
⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于逆运动学批量求解验证。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。
2. 痛点分析
课程模块 本篇对应的工程痛点
第3章 连杆参数及运动学求解 → 3.4 逆运动学 解析逆解的几何法推导与多解选择
第3章 → 3.3 齐次变换矩阵 正运动学验证逆解正确性
第6章 轨迹规划与控制 → 6.1 路径点生成 批量目标点的可达性预筛选
第9章 仿真实验 → 离线验证 上位机离线校验点位可行性
本篇代码解决 L2 离线仿真校核层。输入：DH参数集、目标笛卡尔点位列表（N个，含X/Y/Z坐标）。处理：①对每个点位检查Z是否为0（工作空间约束）；②用几何法求逆解（余弦定理），判断是否在臂展范围内；③对可达点计算双解并按关节角变化量选优；④正运动学校验，误差>1e-6 m的判定为校验失败；⑤统计求解成功率、失败原因分类。输出：成功率饼图 + 失败点散点图 + 可达点双解分布图 + 工作空间边界图 + 4张分析图表。
不覆盖：数值迭代逆解、动力学、力控制、实时控制、多臂协同、带姿态约束的θ₃求解。
3. 核心逻辑讲解
物理直觉
逆运动学就是"已知手在哪里，反推每个关节该转多少度"。对于2R平面臂（两个旋转关节决定末端位置），这本质上是一个"已知三角形三边长度求角度"的几何问题。
想象你的手臂：大臂长L1，小臂长L2，末端要够到距离肩膀为R的一个点。只要R ≤ L1+L2（够得到）且R ≥ |L1-L2|（不会太近导致折叠），就能用余弦定理算出肘关节和肩关节的角度。如果R > L1+L2，点太远，够不到；如果R < |L1-L2|，点太近，小臂完全折叠也碰不到——这两种情况逆解都不存在。
本篇的第三个关节是腕转，不改变位置只改变朝向，所以逆解时先不管它，位置解出来后腕转角由末端朝向需求决定（本篇简化为θ₃=0，预留扩展接口）。
业务逻辑 → 代码映射
输入：目标点位列表 targets = [(x₁,y₁,z₁), (x₂,y₂,z₂), ..., (xₙ,yₙ,zₙ)]
↓
【步骤1：逐点过滤】
for each (x, y, z):
if |z| > ε: 标记"Z超界"，continue
R = √(x² + y²)
if R > L1+L2: 标记"超出臂展"
elif R < |L1-L2|: 标记"进入奇异内圈"
else: 进入逆解
↓
【步骤2：几何法逆解】
用余弦定理求肘角θ₂
cos(θ₂) = (R² - L1² - L2²) / (2·L1·L2)
θ₂>0 → elbow-up, θ₂<0 → elbow-down
肩部角θ₁ = atan2(y,x) - atan2(L2·sin|θ₂|, L1+L2·cosθ₂)
（注意：θ₁ = φ - ψ，补偿肘部抬升带来的偏移）
↓
【步骤3：选优】
若提供当前关节角，选变化量小的构型；否则默认elbow-up
↓
【步骤4：正运动学校验】
将逆解代入正运动学，验证末端位置与目标的偏差
若偏差 > 1e-6 m → 标记为"校验失败"
↓
【步骤5：统计与可视化】
成功率、失败分类计数、4张图表
关键公式文字翻译
距离计算：
"R = √(x² + y²)"。目标点到基座的水平距离。如果R超过两臂之和，点在工作空间外。
余弦定理求肘角：
"cos(θ₂) = (R² - L1² - L2²) / (2·L1·L2)"。这是三角形三边求内角的标准公式。θ₂是肘关节的弯曲角（正值表示小臂向上弯，即elbow-up）。如果|cos(θ₂)| > 1，说明三角形不存在——目标点不可达。
肩部角：
"θ₁ = φ - ψ"。φ = atan2(y, x)是目标点的极角，ψ是三角形内角修正量 = atan2(L2·sinθ₂, L1 + L2·cosθ₂)。减号是关键：当肘部向上弯时，末端相对于肩部连线向外偏了ψ角，肩部必须向内收ψ才能把末端拉回目标点。如果写成加号，末端会偏到目标点外侧去——正运动学校验会暴露这个错误。
选优策略：计算两个解对应的关节角变化量（从当前位置到目标位），选变化量小的那个。这减少关节运动幅度和碰撞风险。若未提供当前角，默认选elbow-up（工程惯例，避免小臂向下碰撞工作台）。
4. OOP代码实现
技术栈
限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。全部自实现。
项目结构树
ik_batch_simulation/
├── run_ik_batch_simulation.py
├── requirements.txt
├── robot/
│ ├── __init__.py
│ ├── models.py
│ ├── dh_kinematics.py
│ ├── inverse_kinematics.py
│ ├── batch_processor.py
│ └── visualize.py
├── tests/
│ ├── __init__.py
│ └── test_ik_batch.py
├── results/
└── output/
核心源码
"robot/models.py" — 数据模型
"""
数据模型：DH参数、机器人配置、目标点位
全部参数内置，自包含可复现
"""
import numpy as np
import math
from dataclasses import dataclass, field
from typing import List, Optional
@dataclass
class DHTwistParams:
"""
DH参数集（标准DH）
theta_offset: θ偏移 (rad)
d: 连杆偏距 (m)
a: 连杆长度 (m)
alpha: 扭角 (rad)
name: 连杆名称
"""
theta_offset: float
d: float
a: float
alpha: float
name: str = ""
def __post_init__(self):
for name, val in [("theta", self.theta_offset),
("d", self.d), ("a", self.a),
("alpha", self.alpha)]:
if not math.isfinite(val):
raise ValueError(
f"{self.name} {name}必须为有限数值")
@dataclass
class RobotConfig:
"""
3-DOF平面搬运机械臂配置（2R+1R腕转）
连杆1: J1回转, a=0.25m, α=0°
连杆2: J2俯仰, a=0.30m, α=0°
连杆3: J3腕转, a=0.0m, α=0°
工作空间：Z=0平面，半径|L1-L2|到L1+L2
注意：逆解仅对theta_offset全为0的构型有效
"""
dh_params: List[DHTwistParams] = field(default_factory=list)
z_tolerance: float = 1e-4 # Z方向容差 (m)
dist_tolerance: float = 1e-9 # 距离边界容差 (m)
def __post_init__(self):
if not self.dh_params:
self.dh_params = [
DHTwistParams(0.0, 0.0, 0.25, 0.0, "L1_J1"),
DHTwistParams(0.0, 0.0, 0.30, 0.0, "L2_J2"),
DHTwistParams(0.0, 0.0, 0.0, 0.0, "L3_J3"),
]
# 检查theta_offset是否全为0（逆解前提条件）
for dh in self.dh_params:
if abs(dh.theta_offset) > 1e-12:
raise ValueError(
"逆解仅支持theta_offset=0的构型，"
"非零offset需修改逆解公式")
@property
def l1(self) -> float:
return self.dh_params[0].a
@property
def l2(self) -> float:
return self.dh_params[1].a
@property
def max_reach(self) -> float:
return self.l1 + self.l2
@property
def min_reach(self) -> float:
return abs(self.l1 - self.l2)
@dataclass
class TargetPoint:
"""单个目标点位"""
x: float # m
y: float # m
z: float # m
label: str = ""
def __post_init__(self):
for name, val in [("x", self.x), ("y", self.y), ("z", self.z)]:
if not math.isfinite(val):
raise ValueError(f"TargetPoint {name}必须为有限数值")
@dataclass
class IKResult:
"""单点逆解结果"""
target: TargetPoint
success: bool
reason: str = ""
theta1: Optional[float] = None
theta2: Optional[float] = None
theta3: Optional[float] = None
solution_type: str = "" # "elbow_up" / "elbow_down" / ""
pos_error: float = 0.0
verify_passed: bool = False
index: int = 0
"robot/dh_kinematics.py" — 正运动学
"""
标准DH正运动学：3-DOF平面搬运臂
用于逆解后的正确性校验
"""
import numpy as np
import math
from robot.models import RobotConfig
class DHKinematics:
def __init__(self, config: RobotConfig):
self.config = config
def dh_transform(self, theta, d, a, alpha):
"""单个连杆的标准DH齐次变换矩阵 (4×4)"""
c_t, s_t = math.cos(theta), math.sin(theta)
c_a, s_a = math.cos(alpha), math.sin(alpha)
return np.array([
[c_t, -s_t * c_a, s_t * s_a, a * c_t],
[s_t, c_t * c_a, -c_t * s_a, a * s_t],
[0, s_a, c_a, d],
[0, 0, 0, 1]
], dtype=float)
def forward_kinematics(self, joint_angles: list):
"""
正运动学：链式乘法
joint_angles: [θ1, θ2, θ3]
"""
if len(joint_angles) != 3:
raise ValueError("需要3个关节角[θ1, θ2, θ3]")
T_total = np.eye(4)
for i, dh in enumerate(self.config.dh_params):
theta = joint_angles[i] + dh.theta_offset
T_i = self.dh_transform(theta, dh.d, dh.a, dh.alpha)
T_total = T_total @ T_i
return T_total
def get_end_position(self, joint_angles: list):
"""返回末端位置 [x, y, z]"""
T = self.forward_kinematics(joint_angles)
return np.array([T[0, 3], T[1, 3], T[2, 3]], dtype=float)
"robot/inverse_kinematics.py" — 解析逆运动学
"""
3-DOF平面搬运臂解析逆运动学（几何法）
仅求解位置（X,Y），Z固定为0
第三关节为腕转，不改变末端位置
⚠ 逆解公式仅对theta_offset=0的构型有效
"""
import numpy as np
import math
from robot.models import RobotConfig, TargetPoint, IKResult
class GeometricInverseKinematics:
def __init__(self, config: RobotConfig):
self.config = config
self.l1 = config.l1
self.l2 = config.l2
self.max_reach = config.max_reach
self.min_reach = config.min_reach
self.z_tol = config.z_tolerance
self.dist_tol = config.dist_tolerance
def _angle_dist(self, a, b):
"""计算两个角之间的最短距离（考虑±π缠绕）"""
d = a - b
while d > math.pi:
d -= 2 * math.pi
while d < -math.pi:
d += 2 * math.pi
return abs(d)
def solve(self, target: TargetPoint, current_angles=None):
"""
对单个目标点求解逆运动学
current_angles: 当前关节角 [θ1, θ2, θ3]，用于选优
返回 IKResult
"""
# 1. Z方向检查
if abs(target.z) > self.z_tol:
return IKResult(
target=target,
success=False,
reason="Z超界：本臂无Z自由度，仅支持Z=0平面"
)
# 2. 距离检查
r = math.sqrt(target.x**2 + target.y**2)
if r > self.max_reach + self.dist_tol:
return IKResult(
target=target,
success=False,
reason=f"超出臂展：R={r:.4f}m > {self.max_reach:.4f}m"
)
if r < self.min_reach - self.dist_tol:
return IKResult(
target=target,
success=False,
reason=f"进入奇异内圈：R={r:.4f}m < {self.min_reach:.4f}m"
)
# 3. 余弦定理求肘角θ₂
# cos(θ₂) = (r² - l1² - l2²) / (2·l1·l2)
cos_theta2 = (r**2 - self.l1**2 - self.l2**2) / (2 * self.l1 * self.l2)
# 数值安全：clamp到[-1, 1]
cos_theta2 = max(-1.0, min(1.0, cos_theta2))
# θ₂ > 0 为 elbow-up（小臂向上弯）
theta2_up = math.acos(cos_theta2)
theta2_down = -theta2_up
# 4. 肩部角θ₁
phi = math.atan2(target.y, target.x)
# ψ = atan2(l2*sin(θ₂), l1 + l2*cos(θ₂))
psi = math.atan2(self.l2 * math.sin(theta2_up),
self.l1 + self.l2 * math.cos(theta2_up))
# 关键：θ₁ = φ - ψ（减号，补偿肘部抬升偏移）
theta1_up = phi - psi # elbow-up 对应的肩部角
theta1_down = phi + psi # elbow-down 对应的肩部角
# 5. 选优策略
if current_angles is not None:
dist_up = (self._angle_dist(theta1_up, current_angles[0])
+ self._angle_dist(theta2_up, current_angles[1]))
dist_down = (self._angle_dist(theta1_down, current_angles[0])
+ self._angle_dist(theta2_down, current_angles[1]))
if dist_up <= dist_down:
chosen_t1, chosen_t2 = theta1_up, theta2_up
sol_type = "elbow_up"
else:
chosen_t1, chosen_t2 = theta1_down, theta2_down
sol_type = "elbow_down"
else:
# 无当前角时默认elbow-up（工程惯例）
chosen_t1, chosen_t2 = theta1_up, theta2_up
sol_type = "elbow_up"
# 6. 腕转角θ₃（本篇简化为0，预留扩展）
theta3 = 0.0
return IKResult(
target=target,
success=True,
reason="OK",
theta1=chosen_t1,
theta2=chosen_t2,
theta3=theta3,
solution_type=sol_type
)
def verify(self, ik_result, kinematics,
pos_error_threshold=1e-6):
"""
用正运动学校验逆解正确性
若误差超过阈值，将verify_passed置为False
"""
if not ik_result.success:
return ik_result
joint_angles = [ik_result.theta1, ik_result.theta2,
ik_result.theta3]
pos = kinematics.get_end_position(joint_angles)
target_pos = np.array([ik_result.target.x,
ik_result.target.y,
ik_result.target.z])
error = np.linalg.norm(pos - target_pos)
ik_result.pos_error = error
ik_result.verify_passed = error <= pos_error_threshold
return ik_result
"robot/batch_processor.py" — 批量处理
"""
批量逆运动学求解与统计
含正运动学校验二次分类：校验失败单独统计
"""
import numpy as np
from robot.models import RobotConfig, TargetPoint, IKResult
from robot.inverse_kinematics import GeometricInverseKinematics
from robot.dh_kinematics import DHKinematics
class BatchIKProcessor:
def __init__(self, config: RobotConfig):
self.config = config
self.ik = GeometricInverseKinematics(config)
self.kinematics = DHKinematics(config)
def process(self, targets: list, current_angles=None,
pos_error_threshold=1e-6):
"""
批量处理目标点位
返回：results列表, 统计字典
"""
results = []
stats = {
'total': len(targets),
'success': 0,
'failed': 0,
'verify_failed': 0,
'z_out_of_range': 0,
'beyond_reach': 0,
'below_min_reach': 0,
'elbow_up_count': 0,
'elbow_down_count': 0,
'max_pos_error': 0.0,
}
for i, target in enumerate(targets):
result = self.ik.solve(target, current_angles)
result.index = i
if result.success:
# 正运动学校验
result = self.ik.verify(
result, self.kinematics, pos_error_threshold)
if result.verify_passed:
stats['success'] += 1
if result.solution_type == 'elbow_up':
stats['elbow_up_count'] += 1
else:
stats['elbow_down_count'] += 1
else:
# 校验失败：逆解数学有误
stats['verify_failed'] += 1
result.success = False
result.reason = (
f"校验失败：pos_error="
f"{result.pos_error:.2e} > {pos_error_threshold}"
)
stats['max_pos_error'] = max(
stats['max_pos_error'], result.pos_error)
else:
stats['failed'] += 1
reason = result.reason
if 'Z超界' in reason:
stats['z_out_of_range'] += 1
elif '超出臂展' in reason:
stats['beyond_reach'] += 1
elif '奇异内圈' in reason:
stats['below_min_reach'] += 1
results.append(result)
stats['success_rate'] = stats['success'] / stats['total'] * 100
return results, stats
"robot/visualize.py" — 可视化
"""
逆运动学批量求解可视化：4张图表
① 成功率饼图（含校验失败）
② 失败点散点图（按失败原因着色）
③ 可达点双解分布（elbow-up vs elbow-down）
④ 工作空间边界图 + 全部目标点叠加
"""
import os
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans',
'Noto Sans CJK SC']
plt.rcParams['axes.unicode_minus'] = False
class IKVisualizer:
def __init__(self, results_dir="results"):
self.results_dir = results_dir
os.makedirs(results_dir, exist_ok=True)
def plot_success_pie(self, stats):
"""① 成功率饼图（含校验失败子分类）"""
fig, ax = plt.subplots(figsize=(6, 6))
labels = ['成功', '失败（含校验失败）']
sizes = [stats['success'],
stats['failed'] + stats['verify_failed']]
colors = ['#4CAF50', '#F44336']
ax.pie(sizes, labels=labels, colors=colors,
autopct='%1.1f%%', shadow=False, startangle=90)
ax.set_title(f'逆解成功率\n'
f'({stats["success"]}/{stats["total"]} 点)')
plt.tight_layout()
path = os.path.join(self.results_dir, "01_success_pie.png")
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_failed_points(self, results):
"""② 失败点散点图"""
fig, ax = plt.subplots(figsize=(8, 8))
success_x, success_y = [], []
z_fail_x, z_fail_y = [], []
reach_fail_x, reach_fail_y = [], []
min_fail_x, min_fail_y = [], []
for r in results:
if r.success:
success_x.append(r.target.x)
success_y.append(r.target.y)
else:
reason = r.reason
if 'Z超界' in reason:
z_fail_x.append(r.target.x)
z_fail_y.append(r.target.y)
elif '超出臂展' in reason:
reach_fail_x.append(r.target.x)
reach_fail_y.append(r.target.y)
elif '奇异内圈' in reason:
min_fail_x.append(r.target.x)
min_fail_y.append(r.target.y)
ax.scatter(success_x, success_y, c='lightgray', s=30,
label='成功', alpha=0.6, edgecolors='gray')
if z_fail_x:
ax.scatter(z_fail_x, z_fail_y, c='red', s=50,
label='Z超界', marker='x')
if reach_fail_x:
ax.scatter(reach_fail_x, reach_fail_y, c='orange', s=50,
label='超出臂展', marker='s')
if min_fail_x:
ax.scatter(min_fail_x, min_fail_y, c='purple', s=50,
label='奇异内圈', marker='^')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_title('目标点位可达性分布')
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
ax.legend()
plt.tight_layout()
path = os.path.join(self.results_dir, "02_failed_points.png")
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_solution_distribution(self, results):
"""③ 可达点双解分布"""
fig, ax = plt.subplots(figsize=(8, 8))
up_x, up_y = [], []
down_x, down_y = [], []
for r in results:
if r.success:
if r.solution_type == 'elbow_up':
up_x.append(r.target.x)
up_y.append(r.target.y)
else:
down_x.append(r.target.x)
down_y.append(r.target.y)
if up_x:
ax.scatter(up_x, up_y, c='blue', s=40,
label=f'Elbow-up ({len(up_x)}点)', alpha=0.7)
if down_x:
ax.scatter(down_x, down_y, c='green', s=40,
label=f'Elbow-down ({len(down_x)}点)', alpha=0.7)
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_title('逆解构型分布 (Elbow-up vs Elbow-down)')
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
ax.legend()
plt.tight_layout()
path = os.path.join(self.results_dir,
"03_solution_distribution.png")
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_workspace_boundary(self, config, results=None):
"""④ 臂展边界与工作空间图 + 可选叠加目标点"""
fig, ax = plt.subplots(figsize=(8, 8))
l1 = config.l1
l2 = config.l2
max_r = l1 + l2
min_r = abs(l1 - l2)
theta = np.linspace(0, 2 * np.pi, 200)
ax.plot(max_r * np.cos(theta), max_r * np.sin(theta),
'r-', linewidth=2, label=f'最大臂展 {max_r:.2f} m')
if min_r > 1e-6:
ax.plot(min_r * np.cos(theta), min_r * np.sin(theta),
'r--', linewidth=1.5,
label=f'最小臂展 {min_r:.2f} m')
# 叠加所有目标点
if results is not None:
ok_x, ok_y = [], []
fail_x, fail_y = [], []
for r in results:
if r.success:
ok_x.append(r.target.x)
ok_y.append(r.target.y)
else:
fail_x.append(r.target.x)
fail_y.append(r.target.y)
if ok_x:
ax.scatter(ok_x, ok_y, c='blue', s=15, alpha=0.4,
label='可达点', edgecolors='none')
if fail_x:
ax.scatter(fail_x, fail_y, c='red', s=20, alpha=0.5,
label='不可达点', marker='x')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_title('机械臂工作空间边界 + 目标点叠加')
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
ax.legend(fontsize=8)
plt.tight_layout()
path = os.path.join(self.results_dir,
"04_workspace_boundary.png")
plt.savefig(path, dpi=150)
plt.close()
return path
5. 可视化要求
序号 图表文件 服务结论
①
"01_success_pie.png" 逆解成功率（含校验失败子分类），直观展示批量点位的可达比例
②
"02_failed_points.png" 失败点空间分布，识别不可达区域（Z超界/臂展外/内圈奇异）
③
"03_solution_distribution.png" 可达点中elbow-up/down构型分布，评估选优策略效果
④
"04_workspace_boundary.png" 工作空间边界（最大/最小臂展圆）+ 全部目标点叠加，理论可达域与实测对比
6. README使用说明 + 核心知识点卡片
快速开始
pip install -r requirements.txt
python -m pytest tests/test_ik_batch.py -v # 3/
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！