python的机器人基础原理工业场景模拟第一百三十三篇:仿真工况切换，机械臂从高速切换低速，评估PD控制器之同速度段跟踪性能。
《Python机器人基础原理》第一百三十三篇：仿真工况切换——机械臂从高速切换低速，评估PD控制器不同速度段跟踪性能
声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。
1. 实际工业应用场景
周四下午，弧焊工位。六轴焊接机械臂正在做工艺切换测试：前3秒以0.25 m/s高速扫掠逼近焊缝起点，后4秒降为0.10 m/s恒速焊接。HMI曲线上，速度切换瞬间关节1的位置误差从0.8 mm跳到3.2 mm，持续震荡近1秒才收敛。
"切换瞬间力矩饱和了。"调试工程师小刘指着驱动器监控，"高速段Kp=600够用，低速段轨迹平缓、惯性力小，同样的增益显得太冲。现场只能手动把整段Kp降到400，结果高速段又软了，末端晃得厉害。"
"这是典型的多工况增益失配。"控制工程师老周调出离线仿真，"2R平面构型，L1=0.55 m / L2=0.45 m，连杆质量8.0 kg / 5.0 kg，转动惯量0.60 kg·m² / 0.18 kg·m²，质心0.28 m / 0.22 m，末端负载2.0 kg。前3秒末端以0.25 m/s沿直线从(0.45 m, 0.30 m)走到(0.60 m, 0.20 m)，3秒时切换为0.10 m/s走到(0.70 m, 0.10 m)。PD用Kp=[600, 450] N·m/rad、Kd=[60, 45] N·m·s/rad。对比一下切换前后两个速度段的跟踪误差，看固定增益在不同工况下的表现。"
⚠️ 声明：本代码仅用于课程离线仿真，运行在普通PC上。不包含任何真实机器人控制器的实时通信栈或硬件驱动。不可直接用于产线设备。
2. 痛点分析
课程模块 本篇对应的工程痛点
第5章 机器人关节驱动与控制 → 离散时间控制 固定PD增益在多速度工况下无法同时兼顾动态响应与稳态精度
第6章 轨迹规划与控制 → 跟踪误差量化 速度切换瞬间的瞬态误差尖峰需要离线定量评估
第4章 机器人动力学建模 → 闭式动力学方程 高速段惯性力/科氏力显著，低速段以重力+粘性摩擦为主，动力学项贡献随速度变化
本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、分段直线路点、分段速度、PD增益。处理：①笛卡尔直线插值生成连续期望轨迹；②以固定微步长（0.1 ms）做RK4物理积分；③PD+重力前馈控制；④分段统计跟踪误差。输出：4张分析图表 + 单元测试。
不覆盖：六轴逆解数值迭代、实时操作系统调度、通信抖动、编码器量化噪声。
3. 核心逻辑讲解
物理直觉
机械臂控制就像开车过弯——高速时需要硬悬挂（高Kp）稳住车身，低速时硬悬挂反而把路面细碎震动全传到方向盘上，让人觉得颠。PD增益本质上是在"响应速度"和"阻尼平稳"之间找平衡。高速段轨迹变化快，需要大Kp跟上；低速段轨迹平缓，同样的Kp会让控制器对微小误差过度反应，产生抖动。速度切换瞬间，期望加速度突变，PD输出跳变，如果力矩饱和，就会产生瞬态尖峰。
业务逻辑 → 代码映射
输入：连杆参数 + 分段路点 + 分段速度 + PD增益
↓
【步骤1：生成连续期望轨迹】
分段笛卡尔直线插值 → 按各段实际时长密集采样 → 解析逆解 → 线性关节插值
时间轴由轨迹对象自动派生：t_switch = cumulative_time[1]，t_total = total_time + 余量
↓
【步骤2：RK4仿真】
每步计算期望关节角/速度 → PD+重力前馈 → 力矩限幅 → 正向动力学 → 积分
↓
【步骤3：分段误差统计】
以真实切换时刻 t_switch 为界，分别统计高速段和低速段的
均值、最大值、RMSE（剔除起跳瞬态 t<0.3 s）
↓
【步骤4：可视化对比】
4张图：误差时程、分段指标、切换瞬态放大、速度-误差关系
关键公式文字翻译
PD控制律：τ = Kp·(q_des - q) + Kd·(qd_des - qd) + G(q_des)。比例项盯位置误差，微分项盯速度误差，重力前馈用期望位形的重力项补偿静态下垂。
末端位置误差：e_pos(t) = || FK(q_act(t)) - FK(q_des(t)) ||。单位毫米，方便与工艺窗口对比。
RMSE：√(Σ e_pos² / N)。对持续小偏差敏感，适合评价整段轨迹的跟踪品质；最大值反映瞬态尖峰。
4. OOP代码实现
技术栈
限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等第三方重型库。
项目结构树
speed_switch_sim/
├── run_sim.py # 主入口（高速/低速段对比）
├── requirements.txt
├── robot/
│ ├── __init__.py
│ ├── models.py # dataclass数据模型
│ ├── inverse_kinematics.py # 解析逆解
│ ├── trajectory.py # 分段直线插值+线性关节轨迹
│ ├── dynamics.py # 2R闭式动力学（含参数化FK）
│ ├── controller.py # PD+重力前馈
│ ├── simulator.py # RK4仿真器
│ └── visualize.py # 4张图表
├── tests/
│ ├── __init__.py
│ └── test_integration.py # 3类10个测试
├── results/ # 4张图表
└── README.md
核心源码
"robot/models.py"
"""
Data models for speed-switching PD simulation.
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
from dataclasses import dataclass
@dataclass
class LinkInertia:
"""连杆惯性参数结构体。"""
name: str
mass: float # kg
length: float # m
inertia: float # kg·m^2
com_offset: float # m
def __post_init__(self):
if self.mass <= 0:
raise ValueError(f"{self.name}: mass must be positive")
if self.length <= 0:
raise ValueError(f"{self.name}: length must be positive")
if self.inertia <= 0:
raise ValueError(f"{self.name}: inertia must be positive")
if not 0 < self.com_offset <= self.length:
raise ValueError(
f"{self.name}: com_offset must be in (0, length]")
@dataclass
class Payload:
"""末端负载。"""
mass: float = 0.0 # kg
def __post_init__(self):
if self.mass < 0:
raise ValueError("Payload mass cannot be negative")
@dataclass
class PDGains:
"""PD增益。"""
Kp: np.ndarray
Kd: np.ndarray
def __post_init__(self):
self.Kp = np.asarray(self.Kp, dtype=float)
self.Kd = np.asarray(self.Kd, dtype=float)
if self.Kp.shape != (2,):
raise ValueError(f"Kp must be length-2, got {self.Kp.shape}")
if self.Kd.shape != (2,):
raise ValueError(f"Kd must be length-2, got {self.Kd.shape}")
if np.any(self.Kp <= 0) or np.any(self.Kd < 0):
raise ValueError("Kp must be positive, Kd non-negative")
@dataclass
class SimConfig:
"""
仿真全局配置。
时间轴由 SegmentedCartesianTrajectory 自动派生，
此处只存用户指定的切换后余量，禁止手写与轨迹矛盾的 t_switch/t_total。
"""
links: list
gravity: float = 9.81
dt_phys: float = 0.0001 # 物理积分步长（固定0.1 ms）
tail_duration: float = 2.0 # 末段冻结余量（s）
torque_limit_high: float = 100.0 # 关节1力矩限幅 (N·m)
torque_limit_low: float = 80.0 # 关节2力矩限幅 (N·m)
def __post_init__(self):
if not self.links or len(self.links) < 2:
raise ValueError("SimConfig.links must contain at least 2 links")
if self.dt_phys <= 0:
raise ValueError("SimConfig.dt_phys must be positive")
if self.tail_duration < 0:
raise ValueError("SimConfig.tail_duration must be non-negative")
if self.torque_limit_high <= 0 or self.torque_limit_low <= 0:
raise ValueError("torque_limit must be positive")
"robot/inverse_kinematics.py"
"""
解析逆运动学：平面2R机械臂。
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
import math
from robot.models import SimConfig
class InverseKinematics2R:
"""平面2R解析逆解，多解按与上一拍最近分支筛选。"""
def __init__(self, config: SimConfig):
if len(config.links) < 2:
raise ValueError("InverseKinematics2R requires at least 2 links")
self.l1 = config.links[0].length
self.l2 = config.links[1].length
def solve_single(self, x, y, q_target=None):
"""返回最优逆解。不可达时抛出ValueError。"""
x, y = float(x), float(y)
r2 = x * x + y * y
r = math.sqrt(r2)
max_reach = self.l1 + self.l2
min_reach = abs(self.l1 - self.l2)
if r > max_reach + 1e-9 or r < min_reach - 1e-9:
raise ValueError(f"Position ({x:.4f}, {y:.4f}) unreachable")
cos_q2 = (r2 - self.l1**2 - self.l2**2) / (2.0 * self.l1 * self.l2)
cos_q2 = max(-1.0, min(1.0, cos_q2))
q2_a = math.acos(cos_q2)
q2_b = -q2_a
solutions = []
for q2 in [q2_a, q2_b]:
phi = math.atan2(self.l2 * math.sin(q2),
self.l1 + self.l2 * math.cos(q2))
q1 = math.atan2(y, x) - phi
solutions.append(np.array([q1, q2]))
if q_target is not None:
q_target = np.asarray(q_target, dtype=float)
solutions.sort(key=lambda q: np.linalg.norm(q - q_target))
return solutions[0].copy()
"robot/trajectory.py"
"""
分段笛卡尔直线插值 + 线性关节轨迹。
时间轴由轨迹对象自动派生，禁止手写与路点/速度矛盾的切换时刻。
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
class SegmentedCartesianTrajectory:
"""
分段直线轨迹：每段有独立速度，按弧长等比例密集采样。
"""
def __init__(self, segments):
"""
segments: list of dicts, each with keys:
'start': (x, y), 'end': (x, y), 'speed': m/s
"""
self.segments = segments
self.segment_lengths = []
self.segment_times = []
self.cumulative_time = [0.0]
total_time = 0.0
for seg in segments:
start = np.asarray(seg['start'], dtype=float)
end = np.asarray(seg['end'], dtype=float)
length = np.linalg.norm(end - start)
speed = float(seg['speed'])
if speed <= 0:
raise ValueError(f"Segment speed must be positive, got {speed}")
seg_time = length / speed
self.segment_lengths.append(length)
self.segment_times.append(seg_time)
total_time += seg_time
self.cumulative_time.append(total_time)
self.total_time = total_time
# 真实切换时刻由路点/速度自动派生，不依赖手写常量
self.t_switch = self.cumulative_time[1] if len(segments) >= 2 else 0.0
def get_position_at_time(self, t):
"""返回t时刻的笛卡尔坐标。"""
if t <= 0:
return np.asarray(self.segments[0]['start'], dtype=float)
if t >= self.total_time:
return np.asarray(self.segments[-1]['end'], dtype=float)
idx = np.searchsorted(self.cumulative_time, t, side='right') - 1
idx = max(0, min(idx, len(self.segments) - 1))
t_seg_start = self.cumulative_time[idx]
seg_time = self.segment_times[idx]
start = np.asarray(self.segments[idx]['start'], dtype=float)
end = np.asarray(self.segments[idx]['end'], dtype=float)
if seg_time < 1e-12:
return start.copy()
alpha = (t - t_seg_start) / seg_time
return (1.0 - alpha) * start + alpha * end
class LinearJointTrajectory:
"""
线性关节轨迹，通过密集逆解点线性插值。
时间栅格按各段实际时长生成，不丢失低速段信息。
"""
def __init__(self, time_grid, joint_positions):
self.time_grid = np.asarray(time_grid, dtype=float)
self.joint_positions = np.asarray(joint_positions, dtype=float)
if len(self.time_grid) != len(self.joint_positions):
raise ValueError("time_grid and joint_positions length mismatch")
self.t_start = self.time_grid[0]
self.t_end = self.time_grid[-1]
self.n_points = len(self.time_grid)
def get_at(self, t):
"""返回t时刻的关节角和速度。"""
if t <= self.t_start:
return self.joint_positions[0].copy(), np.zeros(2)
if t >= self.t_end:
return self.joint_positions[-1].copy(), np.zeros(2)
idx = np.searchsorted(self.time_grid, t, side='right') - 1
idx = max(0, min(idx, self.n_points - 2))
t0 = self.time_grid[idx]
t1 = self.time_grid[idx + 1]
dt = t1 - t0
if dt < 1e-12:
return self.joint_positions[idx].copy(), np.zeros(2)
alpha = (t - t0) / dt
q = (1.0 - alpha) * self.joint_positions[idx] \
+ alpha * self.joint_positions[idx + 1]
qd = (self.joint_positions[idx + 1]
- self.joint_positions[idx]) / dt
return q, qd
"robot/dynamics.py"
"""
2R planar rigid-body dynamics with payload support.
FK参数化，从config读取连杆长度，避免硬编码。
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import warnings
import numpy as np
import math
from robot.models import SimConfig, Payload
class Dynamics2R:
def __init__(self, config: SimConfig):
self.cfg = config
self.links = config.links
self.g = config.gravity
self.payload = Payload(0.0)
self.l1 = self.links[0].length
self.m1 = self.links[0].mass
self.r1 = self.links[0].com_offset
self.I1 = self.links[0].inertia
self.l2 = self.links[1].length
self.m2 = self.links[1].mass
self.r2 = self.links[1].com_offset
self.I2 = self.links[1].inertia
def set_payload(self, p: Payload):
self.payload = p
def forward_kinematics(self, q):
"""参数化正运动学，从config读取连杆长度。"""
x = self.l1 * math.cos(q[0]) + self.l2 * math.cos(q[0] + q[1])
y = self.l1 * math.sin(q[0]) + self.l2 * math.sin(q[0] + q[1])
return np.array([x, y], dtype=float)
def mass_matrix(self, q):
"""2R质量矩阵M(q)。"""
q2 = q[1]
c2 = math.cos(q2)
mp = self.payload.mass
M11 = (self.I1 + self.I2 + mp * self.l2**2
+ self.m1 * self.r1**2
+ self.m2 * (self.l1**2 + self.r2**2
+ 2 * self.l1 * self.r2 * c2)
+ mp * (self.l1**2 + self.l2**2
+ 2 * self.l1 * self.l2 * c2))
M12 = (self.I2
+ self.m2 * self.r2 * (self.r2 + self.l1 * c2)
+ mp * self.l2 * (self.l2 + self.l1 * c2))
M22 = self.I2 + mp * self.l2**2 + self.m2 * self.r2**2
return np.array([[M11, M12], [M12, M22]], dtype=float)
def coriolis_vector(self, q, qd):
"""科氏力/离心力向量C(q,qd)·qd。"""
q2 = q[1]
s2 = math.sin(q2)
mp = self.payload.mass
h = (self.m2 * self.l1 * self.r2
+ mp * self.l1 * self.l2) * s2
C1 = h * qd[1] * (2.0 * qd[0] + qd[1])
C2 = -h * qd[0]**2
return np.array([C1, C2], dtype=float)
def gravity_vector(self, q):
"""重力向量 G(q)。"""
q1, q2 = q[0], q[1]
c1 = math.cos(q1)
c12 = math.cos(q1 + q2)
mp = self.payload.mass
G1 = (self.m1 * self.g * self.r1 * c1
+ self.m2 * self.g * (self.l1 * c1 + self.r2 * c12)
+ mp * self.g * (self.l1 * c1 + self.l2 * c12))
G2 = (self.m2 * self.g * self.r2 * c12
+ mp * self.g * self.l2 * c12)
return np.array([G1, G2], dtype=float)
def forward_dynamics(self, q, qd, tau):
"""
正向动力学：τ - C(q,qd) - G(q) = M(q)·qdd
解析2×2求逆加速计算；行列式接近零时警告。
"""
M = self.mass_matrix(q)
Cqd = self.coriolis_vector(q, qd)
G = self.gravity_vector(q)
rhs = tau - Cqd - G
det = M[0, 0] * M[1, 1] - M[0, 1] * M[1, 0]
if abs(det) < 1e-12:
warnings.warn(f"Mass matrix near-singular, det={det:.2e}",
RuntimeWarning)
return np.zeros(2)
inv_M = np.array([[M[1, 1], -M[0, 1]],
[-M[1, 0], M[0, 0]]]) / det
return inv_M @ rhs
"robot/controller.py"
"""
PD控制器 + 重力前馈。
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
from robot.models import PDGains
class PDController:
def __init__(self, gains: PDGains, gravity_compensation=True):
self.Kp = gains.Kp
self.Kd = gains.Kd
self.gravity_compensation = gravity_compensation
def compute_torque(self, q, qd, q_des, qd_des, gravity_torque=None):
"""
PD + 重力前馈。
重力前馈使用期望位形下的重力向量 G(q_des)。
"""
e = q_des - q
ed = qd_des - qd
tau_pd = self.Kp * e + self.Kd * ed
if self.gravity_compensation and gravity_torque is not None:
tau = tau_pd + gravity_torque
else:
tau = tau_pd
return tau
"robot/simulator.py"
"""
RK4仿真器：速度切换工况。
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
from robot.dynamics import Dynamics2R
from robot.controller import PDController
from robot.models import SimConfig, Payload
class Simulator:
def __init__(self, dynamics: Dynamics2R, controller: PDController,
trajectory, dt_phys, config: SimConfig):
self.dyn = dynamics
self.ctrl = controller
self.traj = trajectory
self.dt_phys = dt_phys
self.config = config
self.n_phys_steps = int(round(config.t_total / dt_phys))
def run(self, q_start, payload_mass=2.0):
"""
运行仿真，返回历史字典。
每步记录积分后的最终状态，避免丢失最后一拍。
"""
n = self.n_phys_steps
t_arr = np.arange(n) * self.dt_phys
# 预分配
hist = {
't': t_arr,
'q1': np.zeros(n), 'q2': np.zeros(n),
'qd1': np.zeros(n), 'qd2': np.zeros(n),
'tau1': np.zeros(n), 'tau2': np.zeros(n),
'q1_des': np.zeros(n), 'q2_des': np.zeros(n),
'qd1_des': np.zeros(n), 'qd2_des': np.zeros(n),
'pos_errors_mm': np.zeros(n),
}
self.dyn.set_payload(Payload(payload_mass))
x = np.concatenate([q_start, np.zeros(2)])
for i in range(n):
t = t_arr[i]
# 期望轨迹
q_des, qd_des = self.traj.get_at(t)
# 记录期望
hist['q1_des'][i] = q_des[0]
hist['q2_des'][i] = q_des[1]
hist['qd1_des'][i] = qd_des[0]
hist['qd2_des'][i] = qd_des[1]
# 正运动学误差（参数化FK，与连杆参数同步）
p_act = self.dyn.forward_kinematics(x[:2])
p_des = self.dyn.forward_kinematics(q_des)
hist['pos_errors_mm'][i] = np.linalg.norm(
p_act - p_des) * 1000.0
# 记录实际状态
hist['q1'][i] = x[0]
hist['q2'][i] = x[1]
hist['qd1'][i] = x[2]
hist['qd2'][i] = x[3]
# 控制力矩
grav_ff = self.dyn.gravity_vector(q_des)
tau = self.ctrl.compute_torque(
x[:2], x[2:4], q_des, qd_des, gravity_torque=grav_ff)
# 按关节分级力矩限幅（对应驱动器饱和）
tau[0] = np.clip(tau[0],
-self.config.torque_limit_high,
self.config.torque_limit_high)
tau[1] = np.clip(tau[1],
-self.config.torque_limit_low,
self.config.torque_limit_low)
hist['tau1'][i] = tau[0]
hist['tau2'][i] = tau[1]
# RK4积分
def deriv(xx, tau_fixed=tau):
qdd = self.dyn.forward_dynamics(
xx[:2], xx[2:4], tau_fixed)
return np.concatenate([xx[2:4], qdd])
k1 = deriv(x)
k2 = deriv(x + 0.5 * self.dt_phys * k1)
k3 = deriv(x + 0.5 * self.dt_phys * k2)
k4 = deriv(x + self.dt_phys * k3)
x = x + (self.dt_phys / 6.0) * (k1 + 2*k2 + 2*k3 + k4)
return hist
"robot/visualize.py"
"""
4张速度切换仿真可视化图表。
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
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 Visualizer:
def __init__(self, results_dir="results"):
self.rd = results_dir
os.makedirs(results_dir, exist_ok=True)
def plot_error_time_series(self, hist, t_switch):
"""图1：末端位置误差时程，标注切换时刻。"""
fig, ax = plt.subplots(figsize=(11, 5))
ax.plot(hist['t'], hist['pos_errors_mm'], 'b-',
linewidth=0.6, alpha=0.8)
ax.axvline(x=t_switch, color='r', linestyle='--', linewidth=1.5,
label=f'速度切换 t={t_switch:.2f}s')
ax.set_xlabel('时间 (s)')
ax.set_ylabel('末端位置误差 (mm)')
ax.set_title('速度切换工况下末端位置误差时程')
ax.legend(fontsize=8)
ax.grid(True, alpha=0.3)
plt.tight_layout()
path = os.path.join(self.rd, "01_error_time_series.png")
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_segment_comparison(self, metrics_high, metrics_low):
"""图2：高速段与低速段误差指标对比。"""
fig, axes = plt.subplots(1, 3, figsize=(13, 4))
labels = ['高速段', '低速段']
means = [metrics_high['mean'], metrics_low['mean']]
maxs = [metrics_high['max'], metrics_low['max']]
rmses = [metrics_high['rmse'], metrics_low['rmse']]
x = np.arange(2)
width = 0.35
axes[0].bar(x, means, width, color='steelblue', alpha=0.8)
axes[0].set_ylabel('均值 (mm)')
axes[0].set_title('误差均值')
axes[0].set_xticks(x)
axes[0].set_xticklabels(labels)
axes[0].grid(True, alpha=0.3, axis='y')
axes[1].bar(x, maxs, width, color='orange', alpha=0.8)
axes[1].set_ylabel('最大值 (mm)')
axes[1].set_title('误差最大值')
axes[1].set_xticks(x)
axes[1].set_xticklabels(labels)
axes[1].grid(True, alpha=0.3, axis='y')
axes[2].bar(x, rmses, width, color='green', alpha=0.8)
axes[2].set_ylabel('RMSE (mm)')
axes[2].set_title('RMSE')
axes[2].set_xticks(x)
axes[2].set_xticklabels(labels)
axes[2].grid(True, alpha=0.3, axis='y')
plt.tight_layout()
path = os.path.join(self.rd, "02_segment_comparison.png")
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_switch_transient(self, hist, t_switch, window=0.5):
"""图3：切换瞬间瞬态放大（±window秒）。"""
fig, ax = plt.subplots(figsize=(11, 5))
t = hist['t']
err = hist['pos_errors_mm']
mask = (t >= t_switch - window) & (t <= t_switch + window)
ax.plot(t[mask], err[mask], 'b-', linewidth=1.0, alpha=0.9)
ax.axvline(x=t_switch, color='r', linestyle='--', linewidth
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
2
0- 0
已为社区贡献192条内容
所有评论(0)