《Python机器人基础原理》第十六篇：零部件装配——修改连杆质量参数，仿真连杆重量变化，观察各关节力矩变化趋势
1. 实际工业应用场景
周二上午，某汽车零部件装配线例行维护后重启。一台4-DOF垂直关节装配机械臂（3旋转+1垂直平移，构型与第十四、十五篇同系但增加第三旋转轴）负责将变速箱齿轮组从托盘抓取并压装到壳体中。机器人构型：大臂长600 mm，小臂长450 mm，第三旋转关节（腕部俯仰）偏置距0 mm，z轴垂直行程±200 mm。关节硬件限位：θ₁ ±170°、θ₂ ±135°、θ₃ ±150°、d₄ ±200 mm。连杆质量原始设计值：大臂4.2 kg（质心距基座0.28 m），小臂2.8 kg（质心距肘部0.20 m），腕部1.2 kg（质心距肘部0.40 m），末端执行器+齿轮组负载5.5 kg。额定负载8 kg。
维护工把大臂拆下来更换了内部谐波减速器。装回去的时候，在大臂末端多缠了两圈线缆护套——为了防磨损，多用了约0.6 kg的尼龙编织带。没人把这个当回事。
下午两点，第一件产品压装偏位0.4 mm，报废。
小王接到报警赶到现场。他检查了视觉定位系统——正常。检查了末端夹具气压——正常。检查了轨迹程序——和上个班次一模一样。
"低速手动模式下没问题，"他对旁边的新员工说，"但自动模式下速度一上去，第二轴就出现跟踪误差，末端放不到位。"
新员工翻了翻维护记录："上午换过减速器。会不会是装配问题？"
"减速器本身没问题，回零精度正常。但我注意到一件事——"小王调出伺服驱动器的实时力矩曲线，"第二轴的峰值力矩从原来的19.7 N·m跳到了22.8 N·m。大臂换了减速器，多缠了0.6 kg线缆，质心还往后移了3 cm。"
"质量变化会影响第二轴？"
"第二轴直接承受大臂传来的重力矩。大臂质量增加0.6 kg、力臂增加0.03 m，重力力矩增量约0.6×9.81×0.31≈1.8 N·m。加上惯性项也跟着变大，第二轴总峰值多了约3.1 N·m。控制器里的动力学前馈模型还是旧参数——它以为大臂只有4.2 kg，实际已经4.8 kg了。前馈补偿少算了这部分，伺服在高速换向时要靠反馈PID硬追，跟踪误差就冒出来了。"
"那第三轴呢？"
"第三轴是腕部俯仰，它的重力项里含有(m₂+m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)——大臂质量变化不改变第三轴的重力矩（大臂力臂通过L₁，不经过L₂）。但在质点模型下，第三轴惯性项M₃₃只含腕部和末端质量，不受大臂质量影响。所以第三轴力矩不会直接变大——真正出问题的是第二轴跟踪误差传递到末端，导致整体定位偏。"
小王打开笔记本电脑："我需要做一个参数敏感性仿真：把大臂质量从4.2 kg逐步改到5.5 kg，观察各关节力矩的变化趋势。验证第二轴力矩的线性增长，同时确认第三轴在质点模型下确实不受大臂质量影响——这样就能确定故障根源在第二轴前馈失准，而不是第三轴过载。"
本篇解决的问题：基于拉格朗日动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q)，在固定轨迹下，系统性地修改连杆质量参数（大臂、小臂、腕部、末端负载），仿真各关节力矩随质量变化的趋势。量化"连杆重量变化对关节负载的影响程度"，为产线维护后的参数更新提供依据。本篇将连杆视为集中质量（质点模型），明确声明此简化下第三轴惯性项与θ₁、θ₂质量解耦。
⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于连杆质量参数变化对关节力矩影响的敏感性分析。不可直接用于真实机器人控制器、伺服驱动器或装配产线。真实工业现场需结合在线参数辨识、关节扭矩传感器反馈与安全监控。代码作者不对直接使用本代码于真实设备造成的任何后果负责。
2. 痛点分析
课程模块 本篇对应的工程痛点
第4章 机器人动力学建模 → 4.2 连杆坐标系与D-H参数 连杆质量/质心位置是动力学计算的前提输入，参数错误直接导致力矩计算偏差
第4章 → 4.4 关节空间动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q) 重力项G(q)和质量参数线性相关，惯性项M(q)也含质量项
第5章 机器人关节驱动与控制 → 5.3 前馈补偿 控制器内动力学模型参数与实际不符时，前馈补偿失准→跟踪误差
第7章 机器人非线性控制 → 7.2 自适应控制 质量参数变化属于"慢时变不确定性"，需要在线辨识或自适应律
本篇代码解决 L3 离线参数敏感性分析层。输入不同连杆质量参数组合，输出各关节力矩随质量变化的曲线。代码不是设备实时控制器，不做伺服闭环，不做在线参数辨识。它做的事情是：在离线环境中量化"质量变化→力矩变化"的映射关系，告诉维护工程师"换了个减速器多缠了线缆，力矩会偏多少，需不需要重新标定"。
不覆盖：完整6轴动力学、摩擦力辨识、实时自适应控制律、力控制。
3. 核心逻辑讲解
物理直觉
你背过书包吗？书包里多放两本书，你肩膀就多承受几斤。机械臂也一样——大臂上多缠了0.6 kg线缆，大臂关节（第一轴）要多花力气抬起来，小臂关节（第二轴）要花力气支撑大臂传过来的力矩。第三轴（腕部俯仰）在质点模型下不直接承受大臂质量变化——它的重力臂走的是L₂（小臂长度），不经过L₁（大臂长度）。但第二轴力矩变了，控制器前馈补偿如果还是旧参数，伺服就要靠PID硬追，跟踪误差就冒出来，最终体现在末端定位偏位上。
关键是：质量变化对不同关节的影响不是均匀的，也不是每个关节都受影响。第一轴直接承受大臂自重，影响最大。第二轴承受大臂+小臂的重力矩，大臂质量变化会线性传递。第三轴在质点模型下与大臂质量解耦——这是本篇要验证的一个重要结论。
业务逻辑 → 代码映射
输入：基准质量参数 + 质量变化范围（如大臂+0~2.0 kg）
↓
步骤1：生成N组质量参数组合（单变量扫描 or 全组合）
↓
步骤2：对每组参数，在固定梯形轨迹下运行动力学求解
↓
步骤3：记录各关节的峰值力矩（τ_M峰值、τ_C峰值、τ_G峰值、τ_total峰值）
↓
步骤4：绘制力矩 vs 质量变化曲线
↓
步骤5：计算灵敏度系数（每kg质量变化引起的力矩变化量，单位N·m/kg）
↓
步骤6：输出对比图表
关键公式文字翻译
重力项 G(q) 与质量的关系：τ_G1 = -g·[m₁·com₁·cosθ₁ + m₂·(L₁·cosθ₁ + com₂·cos(θ₁+θ₂)) + (m₃+m₄)·(L₁·cosθ₁ + L₂·cos(θ₁+θ₂+θ₃))]。每一项都是"质量×力臂×重力×三角函数"。质量增加Δm，力矩直接增加Δm·g·力臂。线性关系。
惯性矩阵 M(q) 与质量的关系：M₁₁中 m₁·com₁² 项随m₁线性变化。M₃₃ = m₃·com₃² + m₄·(com₃+L₂)²——只含腕部和末端质量，不含m₁、m₂。这是质点模型下第三轴与大臂/小臂质量解耦的数学体现。
科氏力项与质量的关系：τ_C ∝ h·sinθ₂·(2θ₁˙θ₂˙+θ₂˙²)，其中 h = m₂·L₁·com₂ + (m₃+m₄)·L₁·L₂。h中不含m₁——大臂质量变化不影响科氏力项。
总结：在质点模型下，大臂质量m₁只影响τ_G1、τ_G2（通过L₁力臂项）和τ_M中的M₁₁项。对第三轴完全无直接影响。所有受影响项与质量都是线性关系——扫几个点就能外推。
单位：力矩 N·m，质量 kg，力臂 m，角速度 rad/s。
4. OOP代码实现
技术栈
严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。
项目结构树
robot_mass_sensitivity_sim/
├── run_mass_sensitivity_sim.py
├── requirements.txt
├── dynamics/
│ ├── __init__.py
│ ├── models.py
│ ├── trajectory.py
│ ├── dynamics_solver.py
│ └── visualize.py
├── tests/
│ ├── __init__.py
│ └── test_mass_sensitivity_sim.py
└── results/
核心源码
"dynamics/models.py" — dataclass数据模型
"""
4-DOF垂直关节装配机械臂数据模型
连杆参数、质量、质心位置、关节限位、重力常量
构型说明：3旋转+1垂直平移，与第十四/十五篇同系
θ1: 基座偏航, θ2: 肘部偏航, θ3: 腕部俯仰, d4: z轴升降
"""
from dataclasses import dataclass, field
from typing import List, Tuple
import numpy as np
@dataclass
class JointLimit:
"""单关节硬件限位"""
min_val: float
max_val: float
joint_name: str = ""
def __post_init__(self):
if self.min_val >= self.max_val:
raise ValueError(
f"JointLimit: min_val({self.min_val})必须严格小于"
f"max_val({self.max_val})")
if not np.isfinite(self.min_val) or not np.isfinite(self.max_val):
raise ValueError("JointLimit: 限位值必须为有限数值")
def contains(self, value: float) -> bool:
return (self.min_val - 1e-10) <= value <= (self.max_val + 1e-10)
@dataclass
class JointLimitSet:
"""4-DOF垂直关节装配机械臂关节限位集合"""
limits: List[JointLimit]
def __post_init__(self):
if len(self.limits) != 4:
raise ValueError(f"需要4个关节限位, 实际{len(self.limits)}个")
@classmethod
def default_4dof_assembly(cls):
"""
默认4-DOF垂直关节装配机械臂限位
θ1: ±170° θ2: ±135° θ3: ±150° d4: ±200 mm
"""
pi = 3.141592653589793
return cls(limits=[
JointLimit(-170 * pi / 180, 170 * pi / 180, 'joint1_base_yaw'),
JointLimit(-135 * pi / 180, 135 * pi / 180, 'joint2_elbow_yaw'),
JointLimit(-150 * pi / 180, 150 * pi / 180, 'joint3_wrist_pitch'),
JointLimit(-0.200, 0.200, 'joint4_z_lift'),
])
def check_all_with_indices(self, joint_values: List[float]) -> Tuple[bool, List[int]]:
if len(joint_values) != 4:
raise ValueError(f"需要4个关节值, 实际{len(joint_values)}个")
all_pass = True
violation_indices = []
for i, (val, limit) in enumerate(zip(joint_values, self.limits)):
if not limit.contains(val):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
@dataclass
class LinkMassProps:
"""
连杆质量与质心属性
mass: 质量 (kg)
com_x, com_y, com_z: 质心在连杆坐标系中的位置 (m)
本篇将连杆视为集中质量（质点模型），忽略绕质心转动惯量
"""
mass: float
com_x: float
com_y: float
com_z: float
link_name: str = ""
def __post_init__(self):
if self.mass < 0:
raise ValueError(f"LinkMassProps: 质量不能为负, 实际{self.mass}")
for name, val in [('com_x', self.com_x), ('com_y', self.com_y),
('com_z', self.com_z)]:
if not np.isfinite(val):
raise ValueError(f"LinkMassProps.{name}必须为有限数值")
@dataclass
class MassPropertySet:
"""完整连杆质量属性集合"""
link_props: List[LinkMassProps]
def __post_init__(self):
if len(self.link_props) != 4:
raise ValueError(f"需要4个连杆质量属性, 实际{len(self.link_props)}个")
@classmethod
def default_assembly_arm(cls):
"""
4-DOF垂直关节装配机械臂质量参数
大臂: 4.2 kg, 质心距基座0.28 m
小臂: 2.8 kg, 质心距肘部0.20 m
腕部: 1.2 kg, 质心距肘部0.40 m（在L2=0.45m范围内，合理）
末端执行器+齿轮组: 吸盘0.8 kg + 齿轮组4.7 kg = 5.5 kg
"""
return cls(link_props=[
LinkMassProps(4.2, 0.28, 0.0, 0.0, 'link1_upper_arm'),
LinkMassProps(2.8, 0.20, 0.0, 0.0, 'link2_forearm'),
LinkMassProps(1.2, 0.40, 0.0, 0.0, 'link3_wrist'),
LinkMassProps(5.5, 0.0, 0.0, 0.0, 'link4_end_effector'),
])
def copy_with_modified_mass(self, link_index: int, new_mass: float):
"""
返回修改后的质量属性副本（不修改原对象）
link_index: 0=大臂, 1=小臂, 2=腕部, 3=末端
使用deepcopy语义，保证多次调用不互相污染
"""
new_props = []
for i, prop in enumerate(self.link_props):
if i == link_index:
new_props.append(LinkMassProps(
new_mass, prop.com_x, prop.com_y, prop.com_z, prop.link_name))
else:
new_props.append(LinkMassProps(
prop.mass, prop.com_x, prop.com_y, prop.com_z, prop.link_name))
return MassPropertySet(link_props=new_props)
@dataclass
class DHParam:
"""标准D-H参数"""
theta: float
d: float
a: float
alpha: float
def __post_init__(self):
for name, val in [('theta', self.theta), ('d', self.d),
('a', self.a), ('alpha', self.alpha)]:
if not np.isfinite(val):
raise ValueError(f"DHParam.{name}必须为有限数值")
@dataclass
class RobotParams:
"""
4-DOF垂直关节装配机械臂完整参数
大臂600mm, 小臂450mm, 第三轴偏置0mm, d4=300mm基座高
"""
links: List[DHParam]
joint_names: List[str] = field(default_factory=lambda: [
'joint1_base_yaw', 'joint2_elbow_yaw',
'joint3_wrist_pitch', 'joint4_z_lift'
])
g: float = 9.81
@classmethod
def default_assembly_arm(cls):
pi = 3.141592653589793
links = [
DHParam(0.0, 0.300, 0.600, 0.0),
DHParam(0.0, 0.0, 0.450, 0.0),
DHParam(0.0, 0.0, 0.0, 0.0),
DHParam(0.0, 0.0, 0.0, 0.0),
]
return cls(links=links)
"dynamics/trajectory.py" — 独立梯形速度轨迹生成器（修复减速段bug）
"""
独立梯形速度轨迹生成器：各关节按位移比例独立计算加速/匀速/减速时长
修复：减速段时长 = v_max / |a_max|，不再复用t_acc
命名：IndependentTrapezoidalTrajectory（各关节参数独立）
"""
import numpy as np
class IndependentTrapezoidalTrajectory:
"""
独立梯形速度轨迹：每个关节根据自身位移独立计算梯形参数
保证所有关节在各自时间内精确到达目标，无超调、无速度反向
"""
def __init__(self, q_start, q_end, total_time, max_velocity, max_acceleration):
"""
q_start: 起始关节角度 [θ1, θ2, θ3, d4]
q_end: 终止关节角度
total_time: 总运动时间 (s)，各关节共享（时间同步）
max_velocity: 峰值关节角速度上限 (rad/s)
max_acceleration: 峰值关节角加速度上限 (rad/s²)
"""
self.q_start = np.array(q_start, dtype=float)
self.q_end = np.array(q_end, dtype=float)
self.total_time = total_time
self.max_velocity = max_velocity
self.max_acceleration = max_acceleration
self.delta_q = self.q_end - self.q_start
# 为每个关节计算独立的梯形参数
# 核心修复：减速段时长 t_dec = v_max / |a_max|，独立于 t_acc
self.joint_params = []
for i in range(4):
dq = self.delta_q[i]
if abs(dq) < 1e-10:
self.joint_params.append({
'v_max': 0.0, 'a_max': 0.0,
't_acc': 0.0, 't_const': total_time,
't_dec': 0.0, 'direction': 1.0
})
continue
direction = 1.0 if dq > 0 else -1.0
abs_dq = abs(dq)
# 理论加速段时长
t_acc_theory = max_velocity / max_acceleration
if 2 * t_acc_theory < total_time:
# 尝试使用全局速度上限
v_candidate = max_velocity
t_acc = t_acc_theory
t_dec = t_acc_theory # 减速段同样时长
t_const = total_time - t_acc - t_dec
# 校验：加速段位移 + 匀速段位移 + 减速段位移 >= 总位移
# 加速段位移 = 0.5 * a * t_acc² = 0.5 * v_candidate * t_acc
# 减速段位移 = 0.5 * v_candidate * t_dec
accel_dist = 0.5 * v_candidate * t_acc
decel_dist = 0.5 * v_candidate * t_dec
const_dist = v_candidate * t_const if t_const > 0 else 0.0
if accel_dist + const_dist + decel_dist < abs_dq - 1e-6:
# 位移太大，需要降低速度
# 三角形轨迹：加速到v_peak后立即减速
# dq = 0.5 * a * t_acc² + 0.5 * v_peak * t_dec
# 且 v_peak = a * t_acc = |a| * t_dec
# 设 t_acc = t_dec = t_half, 则 dq = a * t_half²
t_half = np.sqrt(abs_dq / max_acceleration)
if 2 * t_half > total_time:
t_half = total_time / 2.0
v_candidate = max_acceleration * t_half
t_acc = t_half
t_dec = t_half
t_const = total_time - 2 * t_half
if t_const < 0:
t_const = 0.0
elif t_const < 0:
t_const = 0.0
# 退化为三角形
t_half = total_time / 2.0
v_candidate = max_acceleration * t_half
t_acc = t_half
t_dec = t_half
else:
# 总时间不够完整梯形，退化为三角形
t_half = total_time / 2.0
v_candidate = max_acceleration * t_half
t_acc = t_half
t_dec = t_half
t_const = 0.0
self.joint_params.append({
'v_max': direction * v_candidate,
'a_max': direction * max_acceleration,
't_acc': t_acc,
't_const': t_const,
't_dec': t_dec,
'direction': direction
})
def evaluate(self, t):
"""
在时刻t计算关节位置、速度、加速度
所有关节在 t>=total_time 时精确停在 q_end，无跳变、无速度反向
"""
if t <= 0:
return self.q_start.copy(), np.zeros(4), np.zeros(4)
if t >= self.total_time:
return self.q_end.copy(), np.zeros(4), np.zeros(4)
q = np.zeros(4)
q_dot = np.zeros(4)
q_ddot = np.zeros(4)
for i in range(4):
dq = self.delta_q[i]
if abs(dq) < 1e-10:
q[i] = self.q_start[i]
continue
params = self.joint_params[i]
v_max = params['v_max']
a_max = params['a_max']
t_acc = params['t_acc']
t_const = params['t_const']
t_dec = params['t_dec']
if t <= t_acc:
# 加速段
q_ddot[i] = a_max
q_dot[i] = a_max * t
q[i] = self.q_start[i] + 0.5 * a_max * t**2
elif t <= t_acc + t_const:
# 匀速段
q_ddot[i] = 0.0
q_dot[i] = v_max
q[i] = (self.q_start[i]
+ 0.5 * a_max * t_acc**2
+ v_max * (t - t_acc))
elif t <= t_acc + t_const + t_dec:
# 减速段：时长 = t_dec, 减速度 = -|a_max|*direction
t_dec_elapsed = t - t_acc - t_const
a_dec = -abs(a_max) * params['direction']
q_ddot[i] = a_dec
q_dot[i] = v_max + a_dec * t_dec_elapsed
# 速度在减速段结束时精确归零
q[i] = (self.q_start[i]
+ 0.5 * a_max * t_acc**2
+ v_max * t_const
+ v_max * t_dec_elapsed
+ 0.5 * a_dec * t_dec_elapsed**2)
else:
# 安全兜底：应在 t_acc+t_const+t_dec 时已到达终点
q[i] = self.q_end[i]
q_dot[i] = 0.0
q_ddot[i] = 0.0
return q, q_dot, q_ddot
def generate_trajectory_array(self, dt=0.01):
"""生成完整轨迹数组"""
times = np.arange(0, self.total_time + dt, dt)
n = len(times)
q_array = np.zeros((n, 4))
q_dot_array = np.zeros((n, 4))
q_ddot_array = np.zeros((n, 4))
for i, t in enumerate(times):
q, qd, qdd = self.evaluate(t)
q_array[i, :] = q
q_dot_array[i, :] = qd
q_ddot_array[i, :] = qdd
return times, q_array, q_dot_array, q_ddot_array
"dynamics/dynamics_solver.py" — 动力学求解器（核心业务类）
"""
动力学求解器：计算惯性项、科氏力/离心力项、重力项
支持修改连杆质量参数，仿真质量变化对各关节力矩的影响
τ = M(q)q¨ + C(q,q˙)q˙ + G(q)
本篇将连杆视为集中质量（质点模型），忽略绕质心转动惯量。
4-DOF垂直关节臂：前3个旋转关节参与平面内动力学，第4关节为z平移。
物理声明：
- 第三轴（腕部俯仰）在质点模型下与大臂质量m₁解耦
- M₃₃只含m₃、m₄，不含m₁、m₂
- τ_G3含(m₃+m₄)·L₂项，不含m₁
- τ_C3 = 0（第三轴与前两轴无速度耦合）
"""
import numpy as np
from dynamics.models import RobotParams, MassPropertySet
class DynamicsSolver:
"""
基于拉格朗日方程的3旋转+1平移垂直关节臂动力学求解器
输入4关节状态，前3个旋转关节参与M/C/G计算
z平移关节（关节4）仅产生轴向力，不影响旋转关节力矩
"""
def __init__(self, params: RobotParams, mass_props: MassPropertySet):
self.params = params
self.mass_props = mass_props
self.g = params.g
# 提取参数（前3个旋转关节）
self.L1 = params.links[0].a
self.L2 = params.links[1].a
self.m1 = mass_props.link_props[0].mass
self.m2 = mass_props.link_props[1].mass
self.m3 = mass_props.link_props[2].mass
self.m4 = mass_props.link_props[3].mass
self.com1 = mass_props.link_props[0].com_x
self.com2 = mass_props.link_props[1].com_x
self.com3 = mass_props.link_props[2].com_x
# 科氏力耦合系数 h = m₂·L₁·com₂ + (m₃+m₄)·L₁·L₂
# 不含m₁——大臂质量不影响科氏力项
self.h = (self.m2 * self.L1 * self.com2
+ (self.m3 + self.m4) * self.L1 * self.L2)
def inertia_matrix(self, theta2, theta3):
"""
计算3×3惯性矩阵 M(q)（前3个旋转关节）
本篇将连杆视为集中质量，忽略绕质心转动惯量
物理声明：M₃₃不含m₁、m₂，第三轴与大臂/小臂质量解耦
"""
c2 = np.cos(theta2)
c23 = np.cos(theta2 + theta3)
# M₁₁ = m₁·com₁² + m₂·(L₁²+com₂²+2L₁·com₂·cosθ₂)
# + (m₃+m₄)·(L₁²+L₂²+2L₁·L₂·cos(θ₂+θ₃))
M11 = (self.m1 * self.com1**2
+ self.m2 * (self.L1**2 + self.com2**2 + 2*self.L1*self.com2*c2)
+ (self.m3 + self.m4) * (self.L1**2 + self.L2**2
+ 2*self.L1*self.L2*c23))
# M₁₂ = M₂₁ = m₂·(com₂²+L₁·com₂·cosθ₂)
# + (m₃+m₄)·(L₂²+L₁·L₂·cos(θ₂+θ₃))
M12 = (self.m2 * (self.com2**2 + self.L1*self.com2*c2)
+ (self.m3 + self.m4) * (self.L2**2 + self.L1*self.L2*c23))
M13 = 0.0 # 第三轴与前两轴在质点模型下无惯性耦合
# M₂₂ = m₂·com₂² + (m₃+m₄)·L₂²
M22 = self.m2 * self.com2**2 + (self.m3 + self.m4) * self.L2**2
M23 = 0.0
# M₃₃ = m₃·com₃² + m₄·(com₃+L₂)²
# 只含腕部和末端质量，不含m₁、m₂
M33 = self.m3 * self.com3**2 + self.m4 * (self.com3 + self.L2)**2
return np.array([[M11, M12, M13],
[M12, M22, M23],
[M13, M23, M33]], dtype=float)
def coriolis_vector(self, theta2, theta3, theta1_dot, theta2_dot, theta3_dot):
"""
计算科氏力/离心力向量 τ_C = C(q,q˙)·q˙（前3关节）
质点模型下第三轴与前两轴无速度耦合，τ_C3 = 0
"""
s2 = np.sin(theta2)
s23 = np.sin(theta2 + theta3)
# τ_C1 = -h·sinθ₂·(2·θ₁˙·θ₂˙ + θ₂˙²)
tau_C1 = -self.h * s2 * (2 * theta1_dot * theta2_dot + theta2_dot**2)
# τ_C2 = h·sinθ₂·θ₁˙²
tau_C2 = self.h * s2 * theta1_dot**2
# τ_C3 = 0（质点模型下腕部俯仰与前两轴无科氏耦合）
tau_C3 = 0.0
return np.array([tau_C1, tau_C2, tau_C3], dtype=float)
def gravity_vector(self, theta1, theta2, theta3):
"""
计算重力项向量 G(q)（前3关节）
τ_G1含m₁·com₁项（大臂自重直接作用）
τ_G2含m₂·com₂ + (m₃+m₄)·L₂项
τ_G3含(m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)项——不含m₁
"""
c1 = np.cos(theta1)
c12 = np.cos(theta1 + theta2)
c123 = np.cos(theta1 + theta2 + theta3)
# τ_G1 = -g·[m₁·com₁·cosθ₁ + m₂·(L₁·cosθ₁+com₂·cos(θ₁+θ₂))
# + (m₃+m₄)·(L₁·cosθ₁+L₂·cos(θ₁+θ₂+θ₃))]
tau_G1 = -self.g * (
self.m1 * self.com1 * c1
+ self.m2 * (self.L1 * c1 + self.com2 * c12)
+ (self.m3 + self.m4) * (self.L1 * c1 + self.L2 * c123)
)
# τ_G2 = -g·[m₂·com₂·cos(θ₁+θ₂) + (m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)]
tau_G2 = -self.g * (
self.m2 * self.com2 * c12
+ (self.m3 + self.m4) * self.L2 * c123
)
# τ_G3 = -g·[(m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)]
# 不含m₁——大臂质量不直接影响第三轴重力矩
tau_G3 = -self.g * (
(self.m3 + self.m4) * self.L2 * c123
)
return np.array([tau_G1, tau_G2, tau_G3], dtype=float)
def compute_all_terms(self, q, q_dot, q_ddot):
"""
计算完整动力学方程的各项分解
输入4关节状态，前3个旋转关节参与计算
第4关节（z平移）不影响旋转关节力矩
返回：(tau_M, tau_C, tau_G, tau_total)
全部为3元素数组，单位 N·m
"""
if len(q) < 3 or len(q_dot) < 3 or len(q_ddot) < 3:
raise ValueError("需要至少3个关节的角度、速度、加速度")
theta1 = q[0]
theta2 = q[1]
theta3 = q[2]
theta1_dot = q_dot[0]
theta2_dot = q_dot[1]
theta3_dot = q_dot[2]
theta1_ddot = q_ddot[0]
theta2_ddot = q_ddot[1]
theta3_ddot = q_ddot[2]
# 惯性项
M = self.inertia_matrix(theta2, theta3)
tau_M = M @ np.array([theta1_ddot, theta2_ddot, theta3_ddot])
# 科氏力/离心力项
tau_C = self.coriolis_vector(theta2, theta3, theta1_dot, theta2_dot, theta3_dot)
# 重力项
tau_G = self.gravity_vector(theta1, theta2, theta3)
# 总力矩
tau_t
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
