《Python机器人基础原理》第四十篇：产线调试——增加关节最大加速度限制，调整轨迹时间，防止机械臂过载冲击

 

1. 实际工业应用场景

 

周二下午，某汽车零部件焊接产线周检修后的首次热机调试。一台3-DOF平面SCARA机器人（大臂长400 mm、质量2.1 kg、质心距基座210 mm、绕质心转动惯量0.18 kg·m²；小臂长300 mm、质量1.4 kg、质心距肘关节150 mm、绕质心转动惯量0.06 kg·m²；基座旋转关节额定扭矩8.0 N·m、最大角速度2.8 rad/s、最大角加速度8.0 rad/s²；末端额定负载0.8 kg；节拍要求3.5 s/次，重复定位精度±0.08 mm）正在执行金属垫片搬运任务。

 

这是第39篇工位仿真之后的跟进调试。上篇的时间缩放把速度压到了约束线以下（k=1.22），但没管加速度。小赵把优化后的轨迹文件导入调试终端，按下启动。机器人动了0.3秒，在第二段加速段急停，驱动器面板跳出"Overload Protection — Joint 1 Torque Limit"报警。

 

"又超了。"老刘放下咖啡杯，走到示教器前调出加速度曲线，"你看，取料位到抬升段那0.3秒的加速窗口里，关节1的角加速度峰值14.2 rad/s²，超了额定8.0的两倍还不止。扭矩跟加速度成正比——基座等效惯量大约0.72 kg·m²，14.2乘以0.72，峰值扭矩10.2 N·m，超了8.0的额定值。上篇k=1.22只能把加速度压到9.54，还是超。Z轴也一样——z从0.05升到0.18 m只给了0.3秒，线加速度2.89 m/s²，超了0.8的约束三倍多。"

 

小赵翻出上篇的报告。"那时间缩放继续拉？"

 

"对，但这次约束条件变了。加速度按1/k²缩小，不是1/k。k=1.22只够管速度，加速度需要k=√(14.2/8.0)×1.05≈1.40。取速度和加速度约束的较大值——k_final=1.40。缩放后节拍3.2×1.40=4.48秒，超了3.5秒的产线要求。"

 

"超了近一秒。"

 

"所以得改路径。把加速段的时间窗口拉长——要么把抬升段从0.3秒扩到0.5秒，要么把取料到抬升的z向距离缩短。缩放是兜底，改路径才是根本。"

 

本篇解决的问题：在已有速度约束时间缩放的基础上，增加关节最大角加速度约束校验。对越限段进行二次时间缩放（全局拉伸），量化加速度越限的严重程度和节拍代价。用简化动力学模型估算扭矩，验证加速度约束与过载报警的因果关系。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于产线调试阶段加速度约束校核算法验证。不可直接用于真实机器人控制器或产线设备。真实工业现场需结合驱动器扭矩前馈、在线加速度规划、伺服增益整定。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

课程模块 本篇对应的工程痛点

第6章 轨迹规划与控制 → 6.4 轨迹约束与优化 时间缩放仅约束速度，未约束加速度，导致驱动器扭矩超限触发过载保护

第5章 机器人关节驱动与控制 → 5.3 扭矩-加速度关系 电机扭矩与关节加速度成正比（τ = J·α），加速度越限直接导致扭矩越限

第4章 机器人动力学建模 → 4.2 牛顿-欧拉递推 加速度约束需要动力学模型支撑，本篇用简化惯量模型估算扭矩，验证约束有效性

 

本篇代码解决 L2 离线产线调试仿真校核层。输入：工位轨迹点位序列 + 时间戳 + 关节最大角速度/角加速度约束 + 简化动力学参数。处理：IK映射→速度计算→加速度计算→双约束检测→时间缩放（取速度/加速度约束的较大k）→扭矩估算→节拍量化→验证。输出：4张对比图表 + 缩放报告。

 

不覆盖：完整牛顿-欧拉动力学递推、摩擦力模型、柔性关节振动、在线自适应加速度限制。

 

3. 核心逻辑讲解

 

物理直觉

 

你开一辆载重卡车走山路。上坡时你不能一脚油门踩到底——发动机扭矩有限，加速度太大就会熄火。机械臂同理：每个关节电机有最大输出扭矩，扭矩等于等效转动惯量乘以角加速度加上摩擦。角加速度越大，需要的扭矩越大。如果轨迹在拐点处要求瞬间从静止加速到高速，加速度极大，电机根本跟不上，驱动器只能报过载。

 

加速度约束的本质：限制速度变化的快慢。速度约束管"最高能跑多快"，加速度约束管"从静止到最高速至少要用多长时间"。两者共同决定轨迹时间下界。

 

业务逻辑 → 代码映射

 

输入：工位轨迹 P[N][3] + 时间戳 t[N] + ω_max + α_max + 等效惯量 J

  ↓

【步骤1：逆运动学映射】

  q[i] = IK(P[i]) → [θ₁, θ₂, θ₃, d₄]

  ↓

【步骤2：前向差分算速度】

  ω[i] = (q[i+1] - q[i]) / (t[i+1] - t[i])

  ↓

【步骤3：差分之差分算加速度】

  α[i] = (ω[i+1] - ω[i]) / (t[i+1] - t[i])

  （假设等间隔采样，dt恒定）

  ↓

【步骤4：双约束计算缩放因子】

  k_vel = max(|ω|) / ω_max × safety_margin

  k_acc = sqrt(max(|α|) / α_max) × safety_margin

  （加速度按1/k²缩小，所以k ∝ sqrt(α_actual/α_max)）

  k_final = max(k_vel, k_acc)

  ↓

【步骤5：时间缩放】

  t_scaled = t × k_final，位置序列不变

  ↓

【步骤6：扭矩估算】

  τ_est[i] = J × α_scaled[i]

  ↓

【步骤7：验证 + 节拍量化】

  重算缩放后速度和加速度，确认满足约束

  若 scaled_duration > 节拍要求 → 给出路径优化建议

  ↓

输出：对比图表 + 缩放报告

 

关键公式文字翻译

 

加速度缩放关系：位置P(t)，速度v = dP/dt，加速度a = d²P/dt²。时间缩放 t' = k·t，则 v' = (1/k)·v，a' = (1/k²)·a。加速度按1/k²缩小——k需要比速度约束要求的更大。例如实际加速度14.2 rad/s²，约束8.0，比值1.775，开方得1.332，安全系数1.05 → k_acc = 1.40。

 

扭矩估算：τ ≈ J·α（忽略摩擦和重力项）。基座等效惯量约0.72 kg·m²，加速度14.2 rad/s²时估算扭矩10.2 N·m，超额定8.0 N·m。缩放后加速度7.24 rad/s²，扭矩5.2 N·m，满足约束。

 

为什么缩放不会引入额外放大：时间缩放只改变时间参数化，路径不变。IK映射的非线性已经包含在原始加速度 α_raw 中，缩放后精确按 α_scaled(t) = α_raw(t/k) / k² 缩小。不存在"缩放后额外冲高"的效应。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。

 

项目结构树

 

debug_sim/

├── run_debug_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── inverse_kinematics.py

│ ├── trajectory_generator.py

│ ├── time_scaler.py

│ ├── constraint_checker.py

│ ├── dynamics_model.py

│ └── visualize_debug.py

├── tests/

│ ├── __init__.py

│ └── test_debug_sim.py

└── results/

 

核心源码

 

"robot/models.py" — 数据模型

 

"""

数据模型：DH参数、连杆动力学参数、轨迹点位、调试配置

全部参数内置，自包含可复现

"""

 

 

from dataclasses import dataclass, field

from typing import List

import numpy as np

 

 

@dataclass

class DHLink:

    """单个连杆的D-H参数（标准D-H约定）"""

    a: float # 连杆长度 (m)

    alpha: float # 连杆扭转角 (rad)

    d: float # 连杆偏移 (m)

    theta_offset: float = 0.0 # 关节角偏置 (rad)

 

 

@dataclass

class LinkDynamics:

    """连杆动力学参数"""

    mass: float # 质量 (kg)

    com_x: float # 质心x坐标 (m，沿连杆方向)

    com_y: float # 质心y坐标 (m)

    com_z: float # 质心z坐标 (m)

    inertia_zz: float # 绕Z轴（旋转轴）转动惯量 (kg·m²)

 

 

@dataclass

class RobotConfig:

    """

    机器人配置：3-DOF平面SCARA + Z轴移动

    dyn[0]=基座, dyn[1]=大臂, dyn[2]=小臂, dyn[3]=末端负载

    """

    links: List[DHLink]

    dynamics: List[LinkDynamics]

    joint_limits: List[tuple] = None

    max_joint_velocities: List[float] = None

    max_joint_accelerations: List[float] = None

    rated_torques: List[float] = None

 

    def __post_init__(self):

        if self.joint_limits is None:

            self.joint_limits = [

                (-170 * np.pi / 180, 170 * np.pi / 180),

                (-150 * np.pi / 180, 150 * np.pi / 180),

                (-150 * np.pi / 180, 150 * np.pi / 180),

                (-0.12, 0.12),

            ]

        if self.max_joint_velocities is None:

            self.max_joint_velocities = [2.8, 2.8, 3.2, 0.12]

        if self.max_joint_accelerations is None:

            self.max_joint_accelerations = [8.0, 8.0, 10.0, 0.8]

        if self.rated_torques is None:

            self.rated_torques = [8.0, 5.0, 3.0, 0.0]

 

 

# DH参数（与第39篇同一工位）

CALIBRATED_LINKS = [

    DHLink(a=0.400, alpha=0.0, d=0.150, theta_offset=0.0),

    DHLink(a=0.300, alpha=0.0, d=0.0, theta_offset=0.0),

    DHLink(a=0.0, alpha=0.0, d=0.0, theta_offset=0.0),

    DHLink(a=0.0, alpha=0.0, d=-0.10, theta_offset=0.0),

]

 

# 动力学参数：与场景描述对齐

# dyn[0]=基座, dyn[1]=大臂(inertia_zz=0.18), dyn[2]=小臂(inertia_zz=0.06), dyn[3]=末端负载

CALIBRATED_DYNAMICS = [

    LinkDynamics(mass=1.5, com_x=0.0, com_y=0.0, com_z=0.05, inertia_zz=0.05),

    LinkDynamics(mass=2.1, com_x=0.21, com_y=0.0, com_z=0.0, inertia_zz=0.18),

    LinkDynamics(mass=1.4, com_x=0.15, com_y=0.0, com_z=0.0, inertia_zz=0.06),

    LinkDynamics(mass=0.8, com_x=0.0, com_y=0.0, com_z=0.0, inertia_zz=0.01),

]

 

 

@dataclass

class TrajectoryPoint:

    """轨迹点位"""

    time: float

    position: np.ndarray

 

 

@dataclass

class TimeScalingConfig:

    """时间缩放配置"""

    safety_margin: float = 1.05

    max_scale_factor: float = 5.0

    cycle_time_requirement: float = 3.5

 

 

@dataclass

class ScalingReport:

    """缩放报告"""

    original_duration: float

    scaled_duration: float

    scale_factor: float

    max_vel_raw: float

    max_vel_scaled: float

    max_accel_raw: float

    max_accel_scaled: float

    violating_joint_vel: int

    violating_joint_accel: int

    constraint_satisfied: bool

    cycle_time_violated: bool

    cycle_time_excess: float

    k_from_velocity: float

    k_from_acceleration: float

 

"robot/inverse_kinematics.py" — 逆运动学

 

"""

逆运动学模块

3-DOF平面SCARA + Z轴移动，解析解

不可达时raise ValueError

Z轴d4 = z - d_base，纳入限位检查

"""

 

 

import numpy as np

from robot.models import RobotConfig

 

 

class InverseKinematics:

    """SCARA逆运动学解析求解器"""

 

    def __init__(self, config: RobotConfig, elbow_mode: str = "up"):

        self.config = config

        self.a1 = config.links[0].a

        self.a2 = config.links[1].a

        self.d_base = config.links[0].d

        self.elbow_mode = elbow_mode

 

    def solve(self, position: np.ndarray) -> np.ndarray:

        x, y, z = position[0], position[1], position[2]

        r = np.sqrt(x**2 + y**2)

 

        r_min = abs(self.a1 - self.a2)

        r_max = self.a1 + self.a2

        if r < r_min - 1e-6 or r > r_max + 1e-6:

            raise ValueError(

                f"点位 ({x:.3f}, {y:.3f}, {z:.3f}) 不可达："

                f"r={r:.4f} m 超出 [{r_min:.4f}, {r_max:.4f}] m"

            )

 

        cos_theta2 = (r**2 - self.a1**2 - self.a2**2) / (2 * self.a1 * self.a2)

        if abs(cos_theta2) > 1.0:

            raise ValueError(f"arccos参数越界: {cos_theta2:.6f}")

        theta2 = np.arccos(cos_theta2)

 

        phi = np.arctan2(y, x)

        cos_alpha = (r**2 + self.a1**2 - self.a2**2) / (2 * self.a1 * r)

        if abs(cos_alpha) > 1.0:

            raise ValueError(f"α的arccos参数越界: {cos_alpha:.6f}")

        alpha = np.arccos(cos_alpha)

 

        if self.elbow_mode == "up":

            theta1 = phi - alpha

        else:

            theta1 = phi + alpha

 

        theta3 = -theta2

        d4 = z - self.d_base

 

        # 限位检查（4关节全部校验）

        limits = self.config.joint_limits

        joint_vals = [theta1, theta2, theta3, d4]

        joint_names = ["基座", "大臂", "小臂", "Z轴"]

        for j in range(4):

            if not (limits[j][0] <= joint_vals[j] <= limits[j][1]):

                if j < 3:

                    raise ValueError(

                        f"{joint_names[j]}关节角 {joint_vals[j]*180/np.pi:.1f}° "

                        f"超出限位 [{limits[j][0]*180/np.pi:.1f}°, "

                        f"{limits[j][1]*180/np.pi:.1f}°]"

                    )

                else:

                    raise ValueError(

                        f"{joint_names[j]}位移 {joint_vals[j]*1000:.1f} mm "

                        f"超出限位 [{limits[j][0]*1000:.1f}, "

                        f"{limits[j][1]*1000:.1f}] mm"

                    )

 

        return np.array([theta1, theta2, theta3, d4], dtype=np.float64)

 

"robot/trajectory_generator.py" — 轨迹生成

 

"""

轨迹生成模块

四段式路径：取料→抬升→平移→下降放料

原始节拍3.2 s，加速窗口0.3 s，故意制造加速度越限

与第39篇为同一工位的不同阶段

"""

 

 

import numpy as np

from robot.models import TrajectoryPoint

 

 

class TrajectoryGenerator:

    """产线调试轨迹生成器"""

 

    def __init__(self, total_time: float = 3.2, n_points: int = 140):

        self.total_time = total_time

        self.n_points = n_points

 

    def generate_debug_trajectory(self) -> list:

        """

        四段式路径，模拟焊接产线搬运

        0: 取料 (0.25, -0.10, 0.05)

        1: 抬升 (0.25, -0.10, 0.18)

        2: 平移 (0.45, 0.10, 0.18)

        3: 放料 (0.45, 0.10, 0.05)

 

        时间分配：0→0.3→0.8→3.2

        第一段加速窗口仅0.3 s，制造加速度越限

        Z轴0.3 s内从0.05升到0.18 m，线加速度约2.89 m/s²

        """

        dt = self.total_time / (self.n_points - 1)

        trajectory = []

 

        waypoints = np.array([

            [0.25, -0.10, 0.05],

            [0.25, -0.10, 0.18],

            [0.45, 0.10, 0.18],

            [0.45, 0.10, 0.05],

        ])

 

        t_waypoints = np.array([0.0, 0.3, 0.8, 3.2])

 

        for i in range(self.n_points):

            t = i * dt

            seg = 0

            for j in range(len(t_waypoints) - 1):

                if t_waypoints[j] <= t <= t_waypoints[j + 1]:

                    seg = j

                    break

            if t >= t_waypoints[-1]:

                seg = len(t_waypoints) - 2

 

            t0 = t_waypoints[seg]

            t1 = t_waypoints[seg + 1]

            p0 = waypoints[seg]

            p1 = waypoints[seg + 1]

 

            if t1 - t0 > 1e-6:

                s = (t - t0) / (t1 - t0)

            else:

                s = 1.0

 

            pos = p0 + s * (p1 - p0)

            trajectory.append(TrajectoryPoint(time=t, position=pos))

 

        return trajectory

 

"robot/time_scaler.py" — 时间缩放

 

"""

时间缩放模块

支持速度和加速度双约束，取较大的k

加速度按1/k²缩放，k_acc = sqrt(α_ratio) × margin

"""

 

 

from typing import List

import numpy as np

from robot.models import TimeScalingConfig, ScalingReport

 

 

class TimeScaler:

    """轨迹时间缩放器"""

 

    def __init__(self, config: TimeScalingConfig):

        self.config = config

 

    def compute_velocities(self, times: np.ndarray,

                           joint_vars: np.ndarray) -> np.ndarray:

        """

        前向差分计算关节速度

        假设等间隔采样（dt恒定），边界处dt取相邻值

        """

        n = len(times)

        n_joints = joint_vars.shape[1]

        velocities = np.zeros((n - 1, n_joints))

        for i in range(n - 1):

            dt = times[i + 1] - times[i]

            if dt > 1e-10:

                velocities[i] = (joint_vars[i + 1] - joint_vars[i]) / dt

        return velocities

 

    def compute_accelerations(self, times: np.ndarray,

                              joint_vars: np.ndarray) -> np.ndarray:

        """

        前向差分的差分计算关节加速度

        假设等间隔采样：统一使用 times[1]-times[0] 作为dt

        边界两行加速度恒为0（无足够邻点）

        加速度按1/k²缩放

        """

        velocities = self.compute_velocities(times, joint_vars)

        n = len(velocities)

        n_joints = velocities.shape[1]

        accelerations = np.zeros((n - 1, n_joints))

 

        # 等间隔假设下，统一dt

        dt = times[1] - times[0] if len(times) > 1 else 1.0

 

        for i in range(n - 1):

            if dt > 1e-10:

                accelerations[i] = (velocities[i + 1] - velocities[i]) / dt

 

        return accelerations

 

    def compute_scale_factors(self, joint_vars: np.ndarray,

                              times: np.ndarray,

                              max_velocities: List[float],

                              max_accelerations: List[float]) -> tuple:

        """

        分别计算速度约束和加速度约束要求的缩放因子

        返回 (k_vel, k_acc)

        """

        velocities = self.compute_velocities(times, joint_vars)

        accelerations = self.compute_accelerations(times, joint_vars)

 

        n_joints = velocities.shape[1]

 

        # 速度约束

        max_vel_ratio = 0.0

        for j in range(n_joints):

            if max_velocities[j] > 0:

                ratio = np.max(np.abs(velocities[:, j])) / max_velocities[j]

                max_vel_ratio = max(max_vel_ratio, ratio)

 

        k_vel = max_vel_ratio * self.config.safety_margin

        k_vel = max(1.0, min(k_vel, self.config.max_scale_factor))

 

        # 加速度约束：k ∝ sqrt(α_actual / α_max)

        max_accel_ratio = 0.0

        for j in range(n_joints):

            if max_accelerations[j] > 0:

                ratio = np.max(np.abs(accelerations[:, j])) / max_accelerations[j]

                max_accel_ratio = max(max_accel_ratio, ratio)

 

        k_acc = np.sqrt(max_accel_ratio) * self.config.safety_margin

        k_acc = max(1.0, min(k_acc, self.config.max_scale_factor))

 

        return k_vel, k_acc

 

    def scale_timestamps(self, times: np.ndarray,

                         scale_factor: float) -> np.ndarray:

        """仅拉伸时间戳，位置序列不变——不做反向插值"""

        return times * scale_factor

 

    def generate_report(self, original_duration: float,

                        scaled_duration: float,

                        scale_factor: float,

                        max_vel_raw: float, max_vel_scaled: float,

                        max_accel_raw: float, max_accel_scaled: float,

                        violating_joint_vel: int,

                        violating_joint_accel: int,

                        constraint_satisfied: bool,

                        cycle_time_requirement: float,

                        k_vel: float, k_acc: float) -> ScalingReport:

        cycle_time_violated = scaled_duration > cycle_time_requirement

        cycle_time_excess = max(0.0, scaled_duration - cycle_time_requirement)

 

        return ScalingReport(

            original_duration=original_duration,

            scaled_duration=scaled_duration,

            scale_factor=scale_factor,

            max_vel_raw=max_vel_raw,

            max_vel_scaled=max_vel_scaled,

            max_accel_raw=max_accel_raw,

            max_accel_scaled=max_accel_scaled,

            violating_joint_vel=violating_joint_vel,

            violating_joint_accel=violating_joint_accel,

            constraint_satisfied=constraint_satisfied,

            cycle_time_violated=cycle_time_violated,

            cycle_time_excess=cycle_time_excess,

            k_from_velocity=k_vel,

            k_from_acceleration=k_acc

        )

 

"robot/constraint_checker.py" — 约束校验

 

"""

约束校验模块

同时校验速度和加速度约束

复用TimeScaler的差分方法

"""

 

 

from typing import List

import numpy as np

from robot.models import RobotConfig

 

 

class ConstraintChecker:

    """双约束校验器"""

 

    def __init__(self, robot_config: RobotConfig, time_scaler):

        self.robot_config = robot_config

        self.max_velocities = robot_config.max_joint_velocities

        self.max_accelerations = robot_config.max_joint_accelerations

        self.time_scaler = time_scaler

 

    def check_all_constraints(self, times: np.ndarray,

                              joint_vars: np.ndarray) -> dict:

        """同时校验速度和加速度约束"""

        velocities = self.time_scaler.compute_velocities(times, joint_vars)

        accelerations = self.time_scaler.compute_accelerations(times, joint_vars)

 

        n_joints = velocities.shape[1]

 

        # 速度校验

        vel_violations = np.zeros(n_joints, dtype=bool)

        max_vel_per_joint = np.zeros(n_joints)

        for j in range(n_joints):

            max_vel_per_joint[j] = np.max(np.abs(velocities[:, j]))

            if max_vel_per_joint[j] > self.max_velocities[j]:

                vel_violations[j] = True

 

        # 加速度校验

        accel_violations = np.zeros(n_joints, dtype=bool)

        max_accel_per_joint = np.zeros(n_joints)

        for j in range(n_joints):

            max_accel_per_joint[j] = np.max(np.abs(accelerations[:, j]))

            if max_accel_per_joint[j] > self.max_accelerations[j]:

                accel_violations[j] = True

 

        all_satisfied = (not np.any(vel_violations) and

                         not np.any(accel_violations))

 

        return {

            'all_satisfied': all_satisfied,

            'velocity': {

                'satisfied': not np.any(vel_violations),

                'max_per_joint': max_vel_per_joint,

                'violations': vel_violations,

            },

            'acceleration': {

                'satisfied': not np.any(accel_violations),

                'max_per_joint': max_accel_per_joint,

                'violations': accel_violations,

            }

        }

 

"robot/dynamics_model.py" — 简化动力学

 

"""

简化动力学模型

正确计算等效转动惯量（含平行轴定理）

用 τ ≈ J·α 估算关节扭矩，验证加速度约束与过载报警的因果关系

"""

 

 

import numpy as np

from robot.models import RobotConfig

 

 

class SimplifiedDynamics:

    """

    简化动力学模型

    dyn[0]=基座, dyn[1]=大臂(inertia_zz=0.18), dyn[2]=小臂(0.06), dyn[3]=末端负载

    """

 

    def __init__(self, config: RobotConfig):

        self.config = config

        self.J_equivalent = self._compute_equivalent_inertia()

 

    def _compute_equivalent_inertia(self) -> list:

        """

        计算每个关节的等效转动惯量（含平行轴项）

 

        基座关节（关节0）：

          J = 基座自身 + 大臂(m·r² + I_zz) + 小臂(m·r² + I_zz) + 末端(m·r²)

        大臂关节（关节1）：

          J = 大臂自身I_zz + 小臂(m·r² + I_zz) + 末端(m·r²)

        小臂关节（关节2）：

          J = 小臂自身I_zz + 末端(m·r²)

        Z轴（关节3）：

          J = 等效质量

        """

        dyn = self.config.dynamics

        links = self.config.links

        J = [0.0, 0.0, 0.0, 0.0]

 

        # 小臂关节（关节2）

        J[2] = dyn[2].inertia_zz + dyn[3].mass * links[1].a**2

 

        # 大臂关节（关节1）

        r_small = links[0].a + links[1].com_x

        J[1] = dyn[1].inertia_zz + dyn[2].mass * r_small**2 + dyn[2].inertia_zz

        J[1] += dyn[3].mass * links[0].a**2

 

        # 基座关节（关节0）

        r_big = links[0].com_x

        J[0] = dyn[0].inertia_zz + dyn[1].mass * r_big**2 + dyn[1].inertia_zz

        r_small_total = links[0].a + links[1].a

        J[0] += dyn[2].mass * r_small_total**2 + dyn[2].inertia_zz

        J[0] += dyn[3].mass * r_small_total**2

 

        # Z轴：等效质量

        J[3] = dyn[2].mass + dyn[3].mass

 

        return J

 

    def estimate_torques(self, accelerations: np.ndarray) -> np.ndarray:

        """

        估算各关节扭矩：τ_j ≈ J_j · α_j

        前3轴角加速度×惯量，第4轴线加速度×质量

        """

        n = accelerations.shape[0]

        n_joints = accelerations.shape[1]

        torques = np.zeros((n, n_joints))

 

        for i in range(n):

            for j in range(n_joints):

                torques[i, j] = self.J_equivalent[j] * acceler

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！