python的机器人基础原理工业场景模拟第一百三十一篇:产线仿真，对比高，低控制采样频率，绘图展示采样频率对末端跟踪精度影响。
《Python机器人基础原理》第一百三十一篇：产线仿真——对比高、低控制采样频率，绘图展示采样频率对末端跟踪精度影响
声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。
1. 实际工业应用场景
周五夜班，车身点焊线。六轴搬运机械臂沿直线轨迹移送电池包，HMI上末端位置误差曲线像锯齿一样跳动。质检员拿激光跟踪仪一测：最大偏差1.8 mm，刚好卡在工艺窗口边缘。
"把控制频率从250 Hz提到1000 Hz，误差能降多少？"工艺工程师小赵盯着趋势图问。
控制工程师老周摇头："现在不敢拍胸脯。250 Hz下每个控制周期4 ms，力矩是零阶保持的——两个控制周期之间，机械臂靠上一拍的力矩自由飞。频率越低，'飞'得越久，轨迹偏差越大。但频率上去之后，CPU负载、通信抖动、编码器量化噪声全都会变。"
"那怎么定量验证？"
"搭个离线仿真。2R平面构型，固定PD增益，对比250 Hz、500 Hz、1000 Hz三种控制采样频率下的末端跟踪误差。普通PC跑几分钟就出结果。"
"参数呢？"
"竖直平面内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。末端沿直线从(0.45 m, 0.30 m)走到(0.70 m, 0.10 m)，焊缝长度约0.320 m，速度0.15 m/s，理论焊接时间约2.14 s。PD用Kp=[800, 600] N·m/rad、Kd=[80, 60] N·m·s/rad。物理积分步长固定0.1 ms，控制周期分别取4 ms（250 Hz）、2 ms（500 Hz）、1 ms（1000 Hz）。"
⚠️ 声明：本代码仅用于课程离线仿真，运行在普通PC上。不包含任何真实机器人控制器的实时通信栈或硬件驱动。不可直接用于产线设备。
2. 痛点分析
课程模块 本篇对应的工程痛点
第5章 机器人关节驱动与控制 → 离散时间控制 控制采样频率不足导致零阶保持力矩在两个周期之间产生轨迹偏差
第6章 轨迹规划与控制 → 跟踪误差量化 不同采样频率下的末端位置误差需要离线定量对比，而非凭经验估算
第4章 机器人动力学建模 → 闭式动力学方程 高频控制下离散化误差与连续动力学模型的偏差需要数值验证
本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、笛卡尔路点、焊接速度、PD增益、控制采样频率列表。处理：①笛卡尔直线插值生成连续期望轨迹；②以固定微步长（0.1 ms）做RK4物理积分；③在每个控制周期边界重新计算力矩并零阶保持；④每拍计算正运动学，统计末端位置误差。输出：4张分析图表 + 单元测试。
不覆盖：六轴逆解数值迭代、实时操作系统调度、通信抖动、编码器量化噪声。
3. 核心逻辑讲解
物理直觉
控制采样频率就像拍照的帧率。你每4 ms拍一张照，决定这一拍用多大力气；中间4 ms内，机械臂就靠这一张"照片"决定的力矩往前飞。帧率越低，两帧之间"瞎飞"的时间越长，轨迹就越容易飘。提高帧率相当于让控制器更频繁地"看一眼"实际位置并修正——偏差自然更小。
但帧率不是越高越好。每拍都要算逆解、算PD、算动力学，CPU时间有限；而且传感器读数本身有噪声，拍得太频繁会把噪声当真信号去修正，反而抖。
业务逻辑 → 代码映射
输入：连杆参数 + 笛卡尔路点 + 焊接速度 + PD增益 + 控制频率列表
↓
【步骤1：生成连续期望轨迹】
笛卡尔直线插值 → 密集路点 → 解析逆解 → 线性关节插值（恒速工况）
物理时间轴以 dt_phys=0.1 ms 均匀铺开
↓
【步骤2：对每个控制频率运行仿真】
控制周期 Ts = 1/fs
每 phys_steps_per_ctrl = Ts / dt_phys 个物理微步共享同一力矩
RK4积分，控制周期边界处重新计算PD力矩
↓
【步骤3：末端误差统计（每拍）】
正运动算实际末端位置 → 与期望末端比较 → 计算误差(mm)
剔除首尾瞬态后统计均值、最大值、RMSE
↓
【步骤4：可视化对比】
4张图：误差时程、精度对比、频率-误差曲线、稳态段放大
关键公式文字翻译
零阶保持离散控制：在每个控制周期[k·Ts, (k+1)·Ts)内，控制力矩τ(k)保持不变，动力学方程以连续方式积分。这是工业驱动器最常见的模式——电流环输出在下一个控制周期到来前不会更新。
末端位置误差：e_pos(t) = || FK(q_act(t)) - FK(q_des(t)) ||。FK是正运动学，把关节角映射回笛卡尔坐标。误差单位用毫米，方便与工艺窗口对比。
RMSE：√( Σ e_pos² / N )。相比最大值，RMSE对持续小偏差更敏感，适合评价整段轨迹的跟踪品质。
4. OOP代码实现
技术栈
限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等第三方重型库。
项目结构树
sampling_frequency_sim/
├── run_sim.py # 主入口（多控制频率对比）
├── requirements.txt
├── robot/
│ ├── __init__.py
│ ├── models.py # dataclass数据模型
│ ├── inverse_kinematics.py # 解析逆解
│ ├── trajectory.py # 笛卡尔插值+线性关节轨迹
│ ├── dynamics.py # 2R闭式动力学
│ ├── 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 sampling frequency comparison 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}")
@dataclass
class SimConfig:
"""仿真全局配置。"""
links: list
gravity: float = 9.81
dt_phys: float = 0.0001 # 物理积分步长（固定0.1 ms）
t_sim: float = 2.14
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.t_sim <= 0:
raise ValueError("SimConfig.t_sim 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 CartesianLinearInterpolator:
"""笛卡尔空间直线插值，按总弧长等比例密集采样。"""
def __init__(self, waypoints, total_time):
if len(waypoints) < 2:
raise ValueError("Need at least 2 waypoints")
self.waypoints = [np.asarray(wp, dtype=float) for wp in waypoints]
self.total_time = total_time
self.segment_lengths = []
self.cumulative_lengths = [0.0]
total_length = 0.0
for i in range(len(self.waypoints) - 1):
seg_len = np.linalg.norm(self.waypoints[i+1]
- self.waypoints[i])
self.segment_lengths.append(seg_len)
total_length += seg_len
self.cumulative_lengths.append(total_length)
self.total_length = total_length
if total_length <= 0:
raise ValueError("Total path length must be positive")
def interpolate(self, n_samples):
"""返回n_samples个等间距笛卡尔点。"""
s_values = np.linspace(0.0, self.total_length, n_samples)
points = np.empty((n_samples, 2))
for i, s in enumerate(s_values):
seg_idx = np.searchsorted(self.cumulative_lengths, s,
side='right') - 1
seg_idx = max(0, min(seg_idx, len(self.segment_lengths) - 1))
seg_start = self.cumulative_lengths[seg_idx]
seg_len = self.segment_lengths[seg_idx]
t_local = 0.0 if seg_len < 1e-12 else (s - seg_start) / seg_len
p = (self.waypoints[seg_idx]
+ t_local * (self.waypoints[seg_idx+1]
- self.waypoints[seg_idx]))
points[i] = p
return points
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.
# 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 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
def forward_kinematics_2r(q):
"""2R正运动学：返回末端(x,y)。"""
l1, l2 = 0.55, 0.45
x = l1 * np.cos(q[0]) + l2 * np.cos(q[0] + q[1])
y = l1 * np.sin(q[0]) + l2 * np.sin(q[0] + q[1])
return np.array([x, y])
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.n_phys_steps = int(round(config.t_sim / dt_phys))
self.config = config
def run(self, q_start, control_period, payload_mass=2.0):
"""
运行仿真。
control_period: 控制采样周期（秒），如0.004表示250 Hz。
返回字典包含关节量、期望关节量、末端位置误差(mm)。
"""
# 每个控制周期包含的物理微步数
phys_steps_per_ctrl = int(round(control_period / self.dt_phys))
if phys_steps_per_ctrl < 1:
raise ValueError("control_period must be >= dt_phys")
n_ctrl_steps = self.n_phys_steps // phys_steps_per_ctrl
remainder_steps = self.n_phys_steps % phys_steps_per_ctrl
n_total = n_ctrl_steps * phys_steps_per_ctrl + remainder_steps
t_arr = np.arange(n_total) * self.dt_phys
q_hist = np.zeros((n_total, 2))
qd_hist = np.zeros((n_total, 2))
tau_hist = np.zeros((n_total, 2))
q_des_hist = np.zeros((n_total, 2))
qd_des_hist = np.zeros((n_total, 2))
pos_errors_mm = np.zeros(n_total)
self.dyn.set_payload(Payload(payload_mass))
x = np.concatenate([q_start, np.zeros(2)])
current_tau = np.zeros(2)
step_idx = 0
for ctrl_i in range(n_ctrl_steps + (1 if remainder_steps > 0 else 0)):
steps_this_ctrl = phys_steps_per_ctrl
if ctrl_i == n_ctrl_steps and remainder_steps > 0:
steps_this_ctrl = remainder_steps
elif ctrl_i > n_ctrl_steps:
break
t_ctrl = step_idx * self.dt_phys
# 在控制周期边界处重新计算力矩（零阶保持）
q_des, qd_des = self.traj.get_at(t_ctrl)
grav_ff = self.dyn.gravity_vector(q_des)
current_tau = self.ctrl.compute_torque(
x[:2], x[2:4], q_des, qd_des, gravity_torque=grav_ff)
# 当前控制周期内用同一力矩积分
for _ in range(steps_this_ctrl):
t = t_arr[step_idx]
q_des, qd_des = self.traj.get_at(t)
# 记录期望
q_des_hist[step_idx] = q_des
qd_des_hist[step_idx] = qd_des
# 正运动学计算末端位置误差
p_act = forward_kinematics_2r(x[:2])
p_des = forward_kinematics_2r(q_des)
pos_errors_mm[step_idx] = np.linalg.norm(
p_act - p_des) * 1000.0
# 记录实际状态
q_hist[step_idx] = x[:2]
qd_hist[step_idx] = x[2:4]
tau_hist[step_idx] = current_tau
def deriv(xx, tau_fixed=current_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)
step_idx += 1
return {
't': t_arr,
'q1': q_hist[:, 0], 'q2': q_hist[:, 1],
'qd1': qd_hist[:, 0], 'qd2': qd_hist[:, 1],
'tau1': tau_hist[:, 0], 'tau2': tau_hist[:, 1],
'q1_des': q_des_hist[:, 0], 'q2_des': q_des_hist[:, 1],
'qd1_des': qd_des_hist[:, 0], 'qd2_des': qd_des_hist[:, 1],
'pos_errors_mm': pos_errors_mm,
}
"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, results_dict):
"""图1：不同控制频率下的末端位置误差时程。"""
fig, ax = plt.subplots(figsize=(11, 5))
for label, data in results_dict.items():
t = data['t']
err = data['pos_errors_mm']
ax.plot(t, err, linewidth=0.6, alpha=0.8, label=f'{label}')
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_accuracy_comparison(self, labels, metrics_list):
"""图2：各频率下误差统计指标对比（均值、最大值、RMSE）。"""
fig, axes = plt.subplots(1, 3, figsize=(13, 4))
means = [m['mean'] for m in metrics_list]
maxs = [m['max'] for m in metrics_list]
rmses = [m['rmse'] for m in metrics_list]
x = np.arange(len(labels))
width = 0.25
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 + width, 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 + 2*width, 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_accuracy_comparison.png")
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_frequency_vs_error(self, frequencies, metrics_list):
"""图3：控制频率与RMSE的关系曲线（半对数）。"""
fig, ax = plt.subplots(figsize=(9, 5))
rmses = [m['rmse'] for m in metrics_list]
means = [m['mean'] for m in metrics_list]
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
2
0- 0
已为社区贡献185条内容
所有评论(0)