python的机器人基础原理工业场景模拟第九十四篇:搬运机械臂，设置多组目标笛卡尔点位，批量执行逆运动学，统计求解失败，无解的点位数量。

《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解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！