python的机器人基础原理工业场景模拟第八十篇:重载搬运，轨迹不变增大负载质量，统计动力学输出力矩峰值增长幅度。
《Python机器人基础原理》第八十篇：机械臂仿真——重载搬运中轨迹不变增大负载的动力学力矩峰值分析
1. 实际工业应用场景
周五下午，汽车零部件焊接产线。一台4自由度SCARA机器人负责将变速箱壳体从输送辊道搬运到焊接夹具上。原工艺用一台额定负载50 kg的机器人，末端执行器自重12 kg，工件毛坯38 kg——总负载50 kg，恰好在额定边界。电气工程师老周在调试时发现，机器人在第2段轨迹（从辊道提升后水平横移）关节2的伺服驱动器偶发过载报警。
"报警代码E-07，过载。你看关节2的力矩曲线。"老周指着屏幕，"横移段峰值力矩62 N·m，额定是85 N·m，占到了73%。余量只剩27%。"
控制工程师小郑翻出机器人选型手册："额定负载50 kg，末端执行器12 kg，工件38 kg——刚好50 kg。但SCARA的关节轴都是竖直的，重力不加载到旋转关节上对吧？"
"对。竖直轴旋转，重力与轴线平行，力矩为零。负载全靠Z轴丝杠扛着。旋转关节的力矩纯粹来自加速时的惯性——质量越大，加速越猛，力矩越大。"
"那我想知道的是——如果把工件质量从38 kg降到25 kg（换气动轻量化夹爪），力矩峰值能降多少？轨迹不变，还是这条路径。"
小郑打开仿真："我跑一下。固定轨迹，分别设负载质量15 kg、25 kg、38 kg、50 kg，用逆动力学算各关节力矩曲线，看峰值怎么涨。"
构型约定说明：本篇采用4自由度SCARA构型。基座旋转关节θ₁（额定扭矩85 N·m）、大臂长0.45 m/3.5 kg（质心位于距关节2轴0.18 m处，绕自身质心转动惯量0.04 kg·m²）、小臂长0.35 m/2.0 kg（质心位于距关节3轴0.15 m处，绕自身质心转动惯量0.02 kg·m²）、腕部旋转关节θ₄（额定扭矩15 N·m）。基座偏距d₁=0.32 m，垂直行程d₄∈[0, 0.30] m（由Z轴丝杠独立驱动，不计入旋转关节动力学）。末端负载质量可变（15~50 kg），负载绕Z轴转动惯量0.05 kg·m²。轨迹固定为关节空间三次多项式插值，路点序列不变。
⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于重载搬运动力学力矩分析的算法教学验证。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。
2. 痛点分析
课程模块 本篇对应的工程痛点
第4章 机器人动力学建模 → 4.1 牛顿-欧拉递推动力学 SCARA竖直轴旋转关节不承受重力力矩，重载搬运时力矩纯由惯性+科氏/离心项贡献
第4章 机器人动力学建模 → 4.3 关节驱动力矩计算 轨迹固定时增大负载质量，量化惯性力矩峰值增长幅度，判断是否会逼近驱动器额定扭矩
第5章 机器人关节驱动与控制 → 5.2 伺服驱动匹配 力矩峰值直接对应伺服电机电流需求，需留足安全余量
第10章 仿真实验 → 10.2 动力学仿真验证 离线仿真中对比不同负载下的力矩曲线，为夹具轻量化设计提供数据支撑
本篇代码解决 L2 离线仿真校核层。输入：固定关节空间轨迹（路点+时间分配+三次多项式系数）、不同负载质量列表（15/25/38/50 kg）。处理：①对每种负载质量计算各关节角加速度（多项式二次求导）；②用牛顿-欧拉法计算各旋转关节驱动力矩（仅惯性项+科氏/离心项，不含重力项）；③统计各关节力矩峰值及增长幅度。输出：4张分析图表 + CSV数据文件。
不覆盖：完整拉格朗日方程推导、摩擦力模型（库仑+粘滞）、Z轴丝杠动力学、实时力矩前馈控制。
3. 核心逻辑讲解
物理直觉
SCARA机器人的四个关节中，θ₁/θ₂/θ₃/θ₄的轴线全部竖直向上。重力竖直向下。根据力矩定义 τ = r × F，当力矢量与旋转轴平行时，叉乘结果为零。所以——重力不产生绕这些轴的旋转力矩。这是SCARA构型的核心优势：竖直轴只管旋转定位，重力全部由立柱和Z轴丝杠的结构强度承担。
那重载搬运时旋转关节的力矩从哪来？纯来自加速。
想象你站在一块能水平旋转的转盘上，手里举着一个重物。转盘匀速转动时你几乎不费力。但转盘启动（加速）的瞬间，你会感到手臂被向外拉——这就是惯性力矩。质量越大、启动越猛（角加速度越大），你需要施加的力矩越大。
具体到SCARA：
- 关节1（基座）：要加速整个大臂+小臂+负载绕Z轴旋转。等效转动惯量 = 大臂自身 + 小臂等效 + 负载等效。负载质量越大，等效惯量越大，加速需要的力矩越大。
- 关节2（大臂）：要加速小臂+负载绕关节2轴旋转。等效惯量 = 小臂自身 + 负载相对关节2的转动惯量。
- 科氏/离心项：当大臂和小臂同时运动时，小臂的速度叠加在大臂旋转上，产生额外的离心惯性力，投影到关节轴上形成力矩。
所以力矩峰值增长不是简单的"质量翻倍→力矩翻倍"。惯性项让增长接近线性（τ ∝ m·α），科氏项跟速度平方有关（τ ∝ m·ω²）。本篇用牛顿-欧拉递推算法逐项算出来。
业务逻辑 → 代码映射
输入：固定关节轨迹 q(t), q'(t), q''(t)
负载质量列表 m_load_list = [15, 25, 38, 50] kg
↓
【步骤1：运动学准备】
轨迹已由三次多项式生成，含位置/速度/加速度
↓
【步骤2：对每种负载质量，逐采样点计算关节力矩】
对每个时刻t：
├─ τ1 = I₁₁(q2)·q̈1 + 科氏项(含q̇1,q̇2,q2)
│ [不含重力：轴竖直，重力∥轴，力矩=0]
├─ τ2 = I₂₂·q̈2 + 科氏项(q̇1,q2)
│ [不含重力：同上]
├─ τ3 = I₃₃·q̈3
│ [不含重力：同上]
└─ τ4 = (I_wrist + I_load_z)·q̈4
↓
【步骤3：统计各关节力矩峰值】
τ_peak(joint, load) = max(|τ(t)|)
增长幅度 = (τ_peak(m_new) - τ_peak(m_base)) / |τ_peak(m_base)| × 100%
↓
【步骤4：可视化】
输出4张分析图表
关键公式文字翻译
关节1力矩（基座旋转）：
- 惯性项：τ₁_inertia = [大臂等效惯量 + 小臂等效惯量(q₂) + 负载等效惯量(q₂)] × q̈₁。其中负载等效惯量 = m_load × (a₁² + a₂² + 2a₁a₂·cosθ₂)。这是末端到基座轴的水平距离平方乘以质量。
- 科氏/离心项：τ₁_coriolis = -m_load·a₁·a₂·sinθ₂·(2q̇₁q̇₂ + q̇₂²) - m₃·a₁·com₃ₓ·sinθ₂·(2q̇₁q̇₂ + q̇₂²)。来自速度平方产生的离心惯性力在关节1轴上的投影。
关节2力矩（大臂）：
- 惯性项：τ₂_inertia = [小臂等效惯量 + 负载惯量] × q̈₂ = (m₃·com₃ₓ² + I₃ + m_load·a₂²) × q̈₂。
- 科氏项：τ₂_coriolis = m₃·a₁·com₃ₓ·sinθ₂·q̇₁² + m_load·a₁·a₂·sinθ₂·q̇₁²。大臂转动时小臂/负载被"甩"出的离心力投影。
关节3/4力矩：τ₃ = I_load_eff·q̈₃（负载绕Z轴惯量×角加速度）；τ₄ = (I_wrist + I_load_z)·q̈₄。
⚠️ 量纲审计：每个力矩项的力方向（水平面内离心惯性力）、关节轴方向（竖直Z）、叉乘结果（绕Z轴的力矩），全部自洽。
4. OOP代码实现
技术栈
严格限定：numpy, pandas, matplotlib, math（标准库）。不引入pybullet、roboticstoolbox等任何第三方重型库。动力学计算用牛顿-欧拉递推，加速度从多项式系数解析求导。
项目结构树
heavy_load_dynamics/
├── run_load_sweep.py
├── requirements.txt
├── robot/
│ ├── __init__.py
│ ├── models.py
│ ├── trajectory_generator.py
│ ├── dynamics.py
│ └── visualize_dynamics.py
├── tests/
│ ├── __init__.py
│ └── test_heavy_load_dynamics.py
├── results/
└── output/
核心源码
"robot/models.py" — 数据模型
"""
数据模型：SCARA连杆物理参数、负载参数
全部参数内置，自包含可复现
"""
import numpy as np
from dataclasses import dataclass, field
from typing import List
@dataclass
class LinkPhysicalParams:
"""单连杆物理参数"""
name: str = ""
mass: float = 0.0 # 连杆质量 (kg)
com_dist: float = 0.0 # 质心到本连杆关节轴的水平距离 (m)
inertia_about_joint: float = 0.0 # 绕本连杆关节轴的转动惯量 (kg·m²)
length: float = 0.0 # 连杆长度 (m)
inertia_about_com: float = 0.0 # 绕质心的转动惯量 (kg·m²)
theta_min: float = -np.pi
theta_max: float = np.pi
@dataclass
class LoadParams:
"""末端负载参数"""
mass: float = 15.0 # 负载质量 (kg)
inertia_z: float = 0.05 # 负载绕Z轴（旋转轴）的转动惯量 (kg·m²)
# 注：SCARA竖直轴旋转关节不承受重力力矩
# 负载重力由Z轴丝杠独立承担，此处不定义重力相关参数
@dataclass
class RobotArmConfig:
"""SCARA机械臂构型配置（含物理参数）"""
links: List[LinkPhysicalParams] = field(default_factory=list)
d1: float = 0.32 # 基座偏距 (m)
d4_min: float = 0.0
d4_max: float = 0.30
d4_nominal: float = 0.15
gravity: float = 9.81 # 重力加速度 (m/s²)
def __post_init__(self):
if not self.links:
self.links = [
LinkPhysicalParams(
name="基座旋转", mass=8.0, com_dist=0.0,
inertia_about_joint=0.85, length=0.0,
inertia_about_com=0.85,
theta_min=np.deg2rad(-135),
theta_max=np.deg2rad(135)),
LinkPhysicalParams(
name="大臂", mass=3.5, com_dist=0.18,
inertia_about_joint=0.12, length=0.45,
inertia_about_com=0.04,
theta_min=np.deg2rad(-130),
theta_max=np.deg2rad(130)),
LinkPhysicalParams(
name="小臂", mass=2.0, com_dist=0.15,
inertia_about_joint=0.06, length=0.35,
inertia_about_com=0.02,
theta_min=np.deg2rad(-150),
theta_max=np.deg2rad(150)),
LinkPhysicalParams(
name="腕部旋转", mass=1.5, com_dist=0.0,
inertia_about_joint=0.12, length=0.0,
inertia_about_com=0.12,
theta_min=np.deg2rad(-180),
theta_max=np.deg2rad(180)),
]
@property
def n_joints(self):
return len(self.links)
@property
def a1(self):
return self.links[1].length
@property
def a2(self):
return self.links[2].length
@property
def r_max(self):
return self.a1 + self.a2
@property
def r_min(self):
theta1_limit = abs(self.links[1].theta_min)
return np.sqrt(self.a1**2 + self.a2**2 -
2 * self.a1 * self.a2 * np.cos(theta1_limit))
"robot/trajectory_generator.py" — 关节空间轨迹生成
"""
关节空间轨迹生成：逐段三次多项式插值
固定轨迹用于不同负载下的动力学分析
"""
import numpy as np
import math
from robot.models import RobotArmConfig
class JointTrajectoryGenerator:
def __init__(self, config: RobotArmConfig, nominal_joint_speed=1.2):
self.cfg = config
self.nominal_joint_speed = nominal_joint_speed
def allocate_time(self, q_waypoints):
"""按关节角变化量L1范数比例分配时间"""
n = len(q_waypoints)
t = np.zeros(n)
t[0] = 0.0
for i in range(1, n):
delta = np.sum(np.abs(q_waypoints[i] - q_waypoints[i-1]))
dt = delta / self.nominal_joint_speed
t[i] = t[i-1] + dt
return t
def cubic_coefficients(self, q0, q1, t0, t1, v0=0.0, v1=0.0):
"""闭式求解三次多项式系数（归一化时间τ∈[0,1]）"""
T = t1 - t0
if T <= 1e-12:
return [q0, 0.0, 0.0, 0.0]
a0 = q0
a1 = v0 * T
a2 = 3 * (q1 - q0) - (2 * v0 + v1) * T
a3 = 2 * (q0 - q1) + (v0 + v1) * T
return [a0, a1, a2, a3]
def evaluate_position(self, coeffs, tau):
a0, a1, a2, a3 = coeffs
return a0 + a1 * tau + a2 * tau**2 + a3 * tau**3
def evaluate_velocity(self, coeffs, tau, T):
if T <= 1e-12:
return 0.0
a0, a1, a2, a3 = coeffs
return (a1 + 2 * a2 * tau + 3 * a3 * tau**2) / T
def evaluate_acceleration(self, coeffs, tau, T):
"""评估加速度（rad/s²），从多项式系数解析求导"""
if T <= 1e-12:
return 0.0
a0, a1, a2, a3 = coeffs
return (2 * a2 + 6 * a3 * tau) / (T * T)
def compute_internal_velocities(self, q_waypoints, t_waypoints):
"""计算内部路点处的连续速度（中心差分）"""
n = len(q_waypoints)
v_waypoints = np.zeros_like(q_waypoints)
for j in range(1, n-1):
dt_prev = t_waypoints[j] - t_waypoints[j-1]
dt_next = t_waypoints[j+1] - t_waypoints[j]
if dt_prev > 1e-12 and dt_next > 1e-12:
v_waypoints[j] = (q_waypoints[j+1] -
q_waypoints[j-1]) / (dt_prev + dt_next)
return v_waypoints
def generate_trajectory(self, q_waypoints, n_samples_per_segment=60):
"""生成完整轨迹：位置、速度、加速度"""
t_waypoints = self.allocate_time(q_waypoints)
n_segments = len(q_waypoints) - 1
n_joints = q_waypoints.shape[1]
v_waypoints = self.compute_internal_velocities(
q_waypoints, t_waypoints)
coeffs_all = [[] for _ in range(n_joints)]
for j in range(n_joints):
for i in range(n_segments):
t0 = t_waypoints[i]
t1 = t_waypoints[i + 1]
q0 = q_waypoints[i, j]
q1 = q_waypoints[i + 1, j]
v0 = v_waypoints[i, j]
v1 = v_waypoints[i + 1, j]
coeffs = self.cubic_coefficients(q0, q1, t0, t1, v0, v1)
coeffs_all[j].append(coeffs)
total_time = t_waypoints[-1]
t_samples = np.linspace(0, total_time,
n_samples_per_segment * n_segments)
n_samples = len(t_samples)
q_samples = np.zeros((n_samples, n_joints))
qd_samples = np.zeros((n_samples, n_joints))
qdd_samples = np.zeros((n_samples, n_joints))
for k, t in enumerate(t_samples):
segment_idx = 0
for i in range(n_segments):
if t_waypoints[i] <= t <= t_waypoints[i + 1]:
segment_idx = i
break
if t > t_waypoints[-1]:
segment_idx = n_segments - 1
t0 = t_waypoints[segment_idx]
t1 = t_waypoints[segment_idx + 1]
T = t1 - t0
tau = (t - t0) / T if T > 1e-12 else 0.0
for j in range(n_joints):
q_samples[k, j] = self.evaluate_position(
coeffs_all[j][segment_idx], tau)
qd_samples[k, j] = self.evaluate_velocity(
coeffs_all[j][segment_idx], tau, T)
qdd_samples[k, j] = self.evaluate_acceleration(
coeffs_all[j][segment_idx], tau, T)
return {
't': t_samples,
'q': q_samples,
'qd': qd_samples,
'qdd': qdd_samples,
't_waypoints': t_waypoints
}
"robot/dynamics.py" — 牛顿-欧拉动力学计算（修正版）
"""
SCARA机械臂牛顿-欧拉动力学递推（修正版）
关键物理约束：关节1/2/3/4轴线均竖直向上，重力沿-Z方向
→ 重力与旋转轴平行，对旋转关节的力矩恒为零
→ 旋转关节力矩仅含惯性项 + 科氏/离心项
Z轴升降由独立丝杠驱动，不计入旋转关节动力学
"""
import numpy as np
import math
from robot.models import RobotArmConfig, LoadParams
class SCARADynamics:
def __init__(self, config: RobotArmConfig):
self.cfg = config
self.g = config.gravity
def compute_joint_torques(self, q, qd, qdd, load: LoadParams):
"""
计算单个采样时刻的各关节驱动力矩（仅旋转关节）
量纲审计：
- 力方向：水平面内离心惯性力（m·r·ω², m·r·α）
- 关节轴方向：竖直Z
- 叉乘结果：绕Z轴的力矩（N·m）
"""
theta1, theta2, theta3, theta4 = q
dtheta1, dtheta2, dtheta3, dtheta4 = qd
ddtheta1, ddtheta2, ddtheta3, ddtheta4 = qdd
a1 = self.cfg.a1
a2 = self.cfg.a2
# 连杆参数（索引对应关节编号）
# link[0]: 基座（不参与旋转动力学，仅结构）
# link[1]: 大臂
m_arm2 = self.cfg.links[1].mass
com2_dist = self.cfg.links[1].com_dist # 0.18 m
I2_about_joint = self.cfg.links[1].inertia_about_joint # 0.12
# link[2]: 小臂
m_arm3 = self.cfg.links[2].mass
com3_dist = self.cfg.links[2].com_dist # 0.15 m
I3_about_joint = self.cfg.links[2].inertia_about_joint # 0.06
# link[3]: 腕部
I_wrist = self.cfg.links[3].inertia_about_joint # 0.12
m_load = load.mass
I_load_z = load.inertia_z # 负载绕Z轴转动惯量
# ========== 关节1（基座旋转）力矩 ==========
# 力方向：水平面内离心惯性力
# 关节轴方向：竖直Z
# 叉乘结果：绕Z轴的力矩
# 1a. 大臂惯性力矩
# 大臂质心绕关节1旋转的等效惯量 = m2*com2_dist^2 + I2_about_joint
I_arm2_about_j1 = m_arm2 * com2_dist**2 + I2_about_joint
tau1_inertia_arm2 = I_arm2_about_j1 * ddtheta1
# 1b. 小臂惯性力矩（通过基座）
# 小臂质心到关节1轴的水平距离平方
r3_sq = a1**2 + com3_dist**2 + 2 * a1 * com3_dist * math.cos(theta2)
I_arm3_about_j1 = m_arm3 * r3_sq + I3_about_joint
tau1_inertia_arm3 = I_arm3_about_j1 * ddtheta1
# 1c. 末端负载惯性力矩
# 末端到关节1轴的水平距离平方
I_load_about_j1 = m_load * (a1**2 + a2**2 +
2 * a1 * a2 * math.cos(theta2))
tau1_inertia_load = I_load_about_j1 * ddtheta1
# 1d. 科氏/离心项（力方向：水平离心力，轴：Z）
# 小臂质心离心项对关节1的投影
tau1_coriolis_arm3 = (-m_arm3 * a1 * com3_dist *
math.sin(theta2) *
(2 * dtheta1 * dtheta2 + dtheta2**2))
# 末端负载离心项对关节1的投影
tau1_coriolis_load = (-m_load * a1 * a2 *
math.sin(theta2) *
(2 * dtheta1 * dtheta2 + dtheta2**2))
tau1 = (tau1_inertia_arm2 + tau1_inertia_arm3 +
tau1_inertia_load + tau1_coriolis_arm3 +
tau1_coriolis_load)
# 注：无重力项。重力∥Z轴，力矩=0。
# ========== 关节2（大臂）力矩 ==========
# 力方向：水平面内离心惯性力
# 关节轴方向：竖直Z
# 2a. 小臂惯性力矩
I_arm3_about_j2 = m_arm3 * com3_dist**2 + I3_about_joint
tau2_inertia_arm3 = I_arm3_about_j2 * ddtheta2
# 2b. 末端负载惯性力矩
I_load_about_j2 = m_load * a2**2
tau2_inertia_load = I_load_about_j2 * ddtheta2
# 2c. 科氏项（大臂旋转时小臂/负载的离心力投影到关节2）
tau2_coriolis = ((m_arm3 * a1 * com3_dist +
m_load * a1 * a2) *
math.sin(theta2) * dtheta1**2)
tau2 = (tau2_inertia_arm3 + tau2_inertia_load +
tau2_coriolis)
# 注：无重力项。关节2轴竖直，重力力矩=0。
# ========== 关节3（小臂末端旋转）力矩 ==========
# 负载绕Z轴的惯量 × 角加速度
# 注：无重力项。轴竖直，重力∥轴。
I_load_eff_j3 = I_load_z
tau3 = I_load_eff_j3 * ddtheta3
# ========== 关节4（腕部旋转）力矩 ==========
# 腕部自身惯量 + 负载绕Z轴惯量
tau4 = (I_wrist + I_load_z) * ddtheta4
return np.array([tau1, tau2, tau3, tau4], dtype=np.float64)
def compute_trajectory_torques(self, traj, load: LoadParams):
"""
对整条轨迹计算各关节力矩
返回 (n_samples × 4) 数组
"""
n = len(traj['t'])
torques = np.zeros((n, 4))
for k in range(n):
q = traj['q'][k]
qd = traj['qd'][k]
qdd = traj['qdd'][k]
torques[k] = self.compute_joint_torques(q, qd, qdd, load)
return torques
"robot/visualize_dynamics.py" — 可视化
"""
动力学分析可视化：4张图表
① 各关节力矩峰值随负载质量变化
② 关节2力矩曲线对比（基准负载 vs 最大负载）
③ 力矩组成分解（惯性/科氏）
④ 力矩峰值增长幅度柱状图
"""
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
class DynamicsVisualizer:
def __init__(self, results_dir="results"):
self.results_dir = results_dir
import os
os.makedirs(results_dir, exist_ok=True)
def plot_torque_peak_vs_load(self, load_masses, peak_torques_all):
"""① 各关节力矩峰值随负载质量变化"""
fig, ax = plt.subplots(figsize=(8, 6))
joint_names = ['关节1(基座)', '关节2(大臂)', '关节3(小臂)',
'关节4(腕部)']
colors = ['steelblue', 'orange', 'green', 'red']
for j in range(4):
peaks = [peak_torques_all[i][j] for i in range(len(load_masses))]
ax.plot(load_masses, peaks, 'o-', linewidth=2, markersize=8,
color=colors[j], label=joint_names[j])
ax.set_xlabel('负载质量 (kg)')
ax.set_ylabel('力矩峰值 (N·m)')
ax.set_title('各关节力矩峰值 vs 负载质量（仅惯性+科氏项）')
ax.legend(fontsize=8)
ax.grid(True, alpha=0.3)
plt.tight_layout()
path = f"{self.results_dir}/01_torque_peak_vs_load.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_joint2_torque_curves(self, t, torques_dict, load_masses):
"""② 关节2力矩曲线对比"""
fig, ax = plt.subplots(figsize=(10, 6))
min_load = min(load_masses)
max_load = max(load_masses)
ax.plot(t, torques_dict[min_load][:, 1], '-', linewidth=1.5,
color='steelblue', label=f'{min_load} kg')
ax.plot(t, torques_dict[max_load][:, 1], '-', linewidth=1.5,
color='red', label=f'{max_load} kg')
ax.set_xlabel('时间 (s)')
ax.set_ylabel('关节2力矩 (N·m)')
ax.set_title('关节2（大臂）力矩曲线对比')
ax.legend(fontsize=8)
ax.grid(True, alpha=0.3)
plt.tight_layout()
path = f"{self.results_dir}/02_joint2_torque_curves.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_torque_decomposition(self, traj, dynamics_engine, load_masses):
"""③ 力矩组成分解（关节2，最大负载）"""
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 6))
max_load_mass = max(load_masses)
from robot.models import LoadParams
load = LoadParams(mass=max_load_mass)
n = len(traj['t'])
inertia_contrib = np.zeros(n)
coriolis_contrib = np.zeros(n)
a1 = dynamics_engine.cfg.a1
a2 = dynamics_engine.cfg.a2
m_load = load.mass
for k in range(n):
q = traj['q'][k]
qd = traj['qd'][k]
qdd = traj['qdd'][k]
theta1, theta2 = q[0], q[1]
dtheta1, dtheta2 = qd[0], qd[1]
ddtheta2 = qdd[1]
# 关节2惯性项
inertia_contrib[k] = m_load * a2**2 * ddtheta2
# 关节2科氏项
coriolis_contrib[k] = m_load * a1 * a2 * math.sin(
theta2) * dtheta1**2
total = inertia_contrib + coriolis_contrib
ax1.plot(traj['t'], inertia_contrib, '-', linewidth=1.5,
label='惯性项', color='steelblue')
ax1.plot(traj['t'], coriolis_contrib, '-', linewidth=1.5,
label='科氏项', color='orange')
ax1.plot(traj['t'], total, '--', linewidth=1.5,
label='合计', color='red')
ax1.set_xlabel('时间 (s)')
ax1.set_ylabel('力矩贡献 (N·m)')
ax1.set_title(f'关节2力矩组成分解（{max_load_mass} kg负载）')
ax1.legend(fontsize=7)
ax1.grid(True, alpha=0.3)
# 右图：峰值占比
peak_inertia = np.max(np.abs(inertia_contrib))
peak_coriolis = np.max(np.abs(coriolis_contrib))
total_peak = peak_inertia + peak_coriolis
if total_peak > 0:
sizes = [peak_inertia/total_peak*100,
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献132条内容
所有评论(0)