《Python机器人基础原理》第七十九篇：机械臂仿真——路点冗余测试与末端路径变化观察

 

1. 实际工业应用场景

 

周三上午，3C电子装配产线。一台4自由度SCARA机器人执行手机中框螺丝锁附任务：从供料位吸取螺丝，依次移动到4个螺孔位锁附，再返回供料位。轨迹规划器在关节空间用三次多项式插值生成各关节角度曲线，经正运动学映射到末端笛卡尔坐标。

 

调试工程师小郑把新标定的螺孔坐标输入系统，点了"生成轨迹"。仿真窗口里，末端路径从供料位拉出一条平滑曲线，依次穿过4个螺孔点，最后回到起点。看起来没问题。

 

控制工程师老孟拉过键盘，在轨迹编辑器里螺孔2和螺孔3之间手动插入了两个额外的路点——坐标几乎就在原来直线路径上，X方向偏移不到5 mm。

 

"你干嘛？这两个点多余啊，直接走直线不是更顺？"

 

老孟按下"重新规划"："你看看路径变成什么样了。"

 

仿真窗口刷新。新路径在螺孔2到螺孔3之间拐了一个明显的弯——末端先往左偏了约8 mm，再绕回来穿过螺孔3。关节角曲线也变了：关节2的角速度峰值从1.2 rad/s跳到了2.1 rad/s。

 

"这……为什么会拐弯？"

 

"因为你在笛卡尔空间加了冗余路点，但轨迹是在关节空间插值的。关节空间到笛卡尔空间是非线性映射——大臂和小臂的长度比决定了末端路径不是直线。插入冗余路点后，每个路点处IK求解给出的关节角组合不同，逐段多项式经过这些新的关节角中间点，末端自然走出弧线。"

 

小郑皱眉："那如果我在产线上这么干，电机扭矩够吗？"

 

"这就是问题。冗余路点改变了关节速度曲线，峰值上升，加速度更大。你必须在离线仿真里先验证——插入不同数量的冗余路点，观察末端路径怎么变、关节速度峰值怎么涨。"

 

构型约定说明：本篇采用4自由度SCARA构型（基座旋转关节θ₁/8 kg，大臂长0.45 m/3.5 kg，小臂长0.35 m/2.0 kg，腕部旋转关节θ₄/1.5 kg，基座偏距d₁=0.32 m，垂直行程d₄∈[0, 0.30] m，末端负载8 kg）。轨迹规划在关节空间进行，路点冗余测试观察末端路径变化与关节速度峰值。

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于路点冗余测试算法的教学验证。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。

 

2. 痛点分析

 

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

第6章 轨迹规划与控制 → 6.1 轨迹规划基础 关节空间逐段多项式插值中，插入冗余路点改变每段边界条件，末端路径形状随之变化

第6章 轨迹规划与控制 → 6.3 关节空间与笛卡尔空间轨迹 关节空间插值经非线性FK映射到笛卡尔空间，冗余路点导致末端偏离原直线

第3章 连杆参数及运动学求解 → 3.5 工作空间与奇异性 冗余路点可能将IK解推近奇异构型，关节速度峰值急剧上升

第10章 仿真实验 → 10.1 轨迹仿真验证 离线仿真中验证不同路点密度下的路径可行性与关节负载

 

本篇代码解决 L2 离线仿真校核层。输入：一组基础关节角路点（起点→途经点1→途经点2→终点）+ 冗余路点插入策略。处理：①生成无冗余路点的基准关节轨迹；②在指定区间插入N个冗余关节角路点；③逐段三次多项式插值生成各关节角度曲线；④通过正运动学映射到末端笛卡尔路径；⑤对比基准与冗余方案的末端路径形状、关节速度峰值、路径长度。输出：4张分析图表 + CSV数据文件。

 

不覆盖：笛卡尔空间B样条插值、在线轨迹修正、动力学约束下的时间最优规划、力控装配策略。

 

3. 核心逻辑讲解

 

物理直觉

 

想象你控制一个人的手画一条线。你只告诉他"从A点画到B点"，他会走最短路径——直线。但如果你在中间加一个"必须经过C点"的约束，即使C几乎在AB直线上，他的手也会微微绕一下——因为人手臂的关节不是直接沿直线推的，肩关节和肘关节的配合天然产生弧线。

 

机械臂同理。关节空间轨迹规划意味着你告诉每个关节"在t₀时刻转到θ₀，在t₁时刻转到θ₁"。关节按三次多项式平滑转动。末端位置是各关节角的非线性函数（三角函数组合）。当两个路点之间的关节角变化被拆成多段（插入冗余路点），每段多项式经过的中间关节角不同，末端走出的弧线自然不同。

 

冗余路点越多，关节角曲线被"拉"向更多中间点，末端路径越偏离原始直线。这是关节空间插值的固有特性——不是bug，是数学本质。

 

业务逻辑 → 代码映射

 

输入：基础关节角路点列表 q_waypoints_base = [q0, q1, q2, q3]

     冗余路点插入数 n_redundant

  ↓

【步骤1：生成冗余关节角路点】

  在q1和q2之间线性插值生成n_redundant个中间关节角

  拼接为 q_waypoints_redundant = [q0, q1, rq1, rq2, ..., q2, q3]

  ↓

【步骤2：时间分配】

  按各段关节角变化量的平均幅度比例分配时间

  t[i] = t[i-1] + ||q_i - q_{i-1}||₁ / v_nominal_joint

  ↓

【步骤3：逐段三次多项式插值（每个关节独立）】

  对关节j：q_j(t) = a0 + a1·τ + a2·τ² + a3·τ³

  边界条件：位置匹配路点值，速度在路点处连续（内部路点速度非零）

  ↓

【步骤4：正运动学映射】

  在密集采样时刻计算末端笛卡尔坐标

  p_end(t) = FK(q(t))

  ↓

【步骤5：对比分析】

  基准方案 vs 冗余方案：

  - XY平面末端路径形状

  - 各关节速度峰值

  - 末端路径总长度

  - IK可达性（路点是否在工作空间内）

  ↓

【步骤6：可视化】

  输出4张分析图表

 

关键公式文字翻译

 

三次多项式插值（归一化时间）：q(τ) = a₀ + a₁·τ + a₂·τ² + a₃·τ³，其中τ = (t-t₀)/T ∈ [0,1]。4个系数由4个边界条件确定：起点位置q(t₀)=q₀、终点位置q(t₁)=q₁、起点速度q'(t₀)=v₀、终点速度q'(t₁)=v₁。闭式解：a₀=q₀, a₁=v₀·T, a₂=3(q₁-q₀)-(2v₀+v₁)T, a₃=2(q₀-q₁)+(v₀+v₁)T。

 

关节速度：q'(t) = (a₁ + 2a₂τ + 3a₃τ²) / T。直接从多项式系数求导获得，不需要额外差分。

 

末端位置（正运动学）：x = a₁·cosθ₁ + a₂·cos(θ₁+θ₂)，y = a₁·sinθ₁ + a₂·sin(θ₁+θ₂)，z = d₁ + d₄。关节角的非线性组合决定了末端路径是弧线而非直线。

 

路径长度：L = Σ‖p(t_{k+1}) - p(t_k)‖。逐段累加相邻采样点的欧氏距离。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, pandas, matplotlib, math（标准库）。不引入pybullet、roboticstoolbox等任何第三方重型库。轨迹插值用numpy数组运算，正运动学用解析公式。

 

项目结构树

 

waypoint_redundancy_test/

├── run_waypoint_test.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── forward_kinematics.py

│ ├── inverse_kinematics.py

│ ├── trajectory_generator.py

│ ├── redundancy_tester.py

│ └── visualize_trajectory.py

├── tests/

│ ├── __init__.py

│ └── test_waypoint_redundancy.py

├── results/

└── output/

 

核心源码

 

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

 

"""

数据模型：SCARA D-H参数、连杆物理属性、关节角路点

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

"""

 

 

import numpy as np

from dataclasses import dataclass, field

from typing import List

 

 

@dataclass

class LinkDHParams:

    """单连杆 D-H 参数"""

    name: str = ""

    a: float = 0.0 # 连杆长度 (m)

    alpha: float = 0.0 # 连杆扭角 (rad)

    d: float = 0.0 # 连杆偏距 (m)

    theta_offset: float = 0.0

    theta_min: float = -np.pi

    theta_max: float = np.pi

 

 

@dataclass

class RobotArmConfig:

    """SCARA机械臂构型配置"""

    links: List[LinkDHParams] = field(default_factory=list)

    masses: List[float] = field(default_factory=list)

    d4_max: float = 0.30

    d4_min: float = 0.0

    d4_nominal: float = 0.15 # 仿真用固定垂直位置

 

    def __post_init__(self):

        if not self.links:

            self.links = [

                LinkDHParams(name="基座旋转", a=0.0, alpha=0.0, d=0.32,

                             theta_min=np.deg2rad(-135),

                             theta_max=np.deg2rad(135)),

                LinkDHParams(name="大臂", a=0.45, alpha=0.0, d=0.0,

                             theta_min=np.deg2rad(-130),

                             theta_max=np.deg2rad(130)),

                LinkDHParams(name="小臂", a=0.35, alpha=0.0, d=0.0,

                             theta_min=np.deg2rad(-150),

                             theta_max=np.deg2rad(150)),

                LinkDHParams(name="腕部旋转", a=0.0, alpha=0.0, d=0.0,

                             theta_min=np.deg2rad(-180),

                             theta_max=np.deg2rad(180)),

            ]

            self.masses = [8.0, 3.5, 2.0, 1.5]

 

    @property

    def n_joints(self):

        return len(self.links)

 

    @property

    def a1(self):

        return self.links[1].a

 

    @property

    def a2(self):

        return self.links[2].a

 

    @property

    def d1(self):

        return self.links[0].d

 

    @property

    def r_max(self):

        return self.a1 + self.a2

 

    @property

    def r_min(self):

        # r_min由大臂关节限位决定（links[1]，±130°）

        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/forward_kinematics.py" — 正运动学

 

"""

4自由度SCARA正运动学

将关节角映射为末端笛卡尔坐标

"""

 

 

import numpy as np

import math

from robot.models import RobotArmConfig

 

 

class ForwardKinematicsSCARA:

    def __init__(self, config: RobotArmConfig):

        self.cfg = config

        self.d4 = config.d4_nominal # 垂直位置从配置读取

 

    def compute_pose(self, q):

        """

        q = [θ1, θ2, θ3, θ4]

        返回 [x, y, z, θ_end]

        """

        theta1, theta2, theta3, theta4 = q

 

        # 末端XY坐标：大臂+小臂的连杆几何叠加

        x = self.cfg.a1 * math.cos(theta1) + self.cfg.a2 * math.cos(

            theta1 + theta2)

        y = self.cfg.a1 * math.sin(theta1) + self.cfg.a2 * math.sin(

            theta1 + theta2)

        z = self.cfg.d1 + self.d4

 

        # 末端姿态角

        theta_end = theta1 + theta2 + theta3 + theta4

 

        return np.array([x, y, z, theta_end], dtype=np.float64)

 

    def compute_all_poses(self, q_trajectory):

        """

        对关节角轨迹（n_samples × 4）逐点计算末端位姿

        返回 (n_samples × 4) 数组

        """

        n = q_trajectory.shape[0]

        poses = np.zeros((n, 4))

        for k in range(n):

            poses[k, :] = self.compute_pose(q_trajectory[k, :])

        return poses

 

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

 

"""

4自由度SCARA逆运动学（解析解）

用于验证路点是否在工作空间内

"""

 

 

import numpy as np

import math

from robot.models import RobotArmConfig

 

 

class InverseKinematicsSCARA:

    def __init__(self, config: RobotArmConfig):

        self.cfg = config

 

    def solve(self, x, y, z, theta_end, elbow_config="elbow_up"):

        """

        解析逆运动学求解

        elbow_config: "elbow_up" 或 "elbow_down"

        返回：q = [θ1, θ2, θ3, θ4] 或 None（不可达）

        """

        a1 = self.cfg.a1

        a2 = self.cfg.a2

        d1 = self.cfg.d1

 

        # 检查工作空间

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

        if r < self.cfg.r_min or r > self.cfg.r_max:

            return None

 

        # 检查垂直行程

        if z < d1 + self.cfg.d4_min or z > d1 + self.cfg.d4_max:

            return None

 

        # 余弦定理求肘关节角θ2

        cos_theta2 = (x**2 + y**2 - a1**2 - a2**2) / (2 * a1 * a2)

        if abs(cos_theta2) > 1.0:

            return None

 

        theta2 = math.acos(cos_theta2)

        if elbow_config == "elbow_down":

            theta2 = -theta2

 

        # 肩关节角θ1

        gamma = math.atan2(a2 * math.sin(theta2),

                           a1 + a2 * math.cos(theta2))

        theta1 = math.atan2(y, x) - gamma

 

        # 腕部旋转分配：θ3=0，全部旋转由θ4承担

        theta3 = 0.0

        theta4 = theta_end - theta1 - theta2 - theta3

 

        # 限位检查

        if (theta1 < self.cfg.links[0].theta_min or

            theta1 > self.cfg.links[0].theta_max):

            return None

        if (theta2 < self.cfg.links[1].theta_min or

            theta2 > self.cfg.links[1].theta_max):

            return None

        if (theta4 < self.cfg.links[3].theta_min or

            theta4 > self.cfg.links[3].theta_max):

            return None

 

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

 

"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=0.5):

        """

        nominal_joint_speed: 名义关节角速度 (rad/s)，用于时间分配

        """

        self.cfg = config

        self.nominal_joint_speed = nominal_joint_speed

 

    def allocate_time(self, q_waypoints):

        """

        按相邻路点间关节角变化量的L1范数比例分配时间

        返回时间数组 t[i]

        """

        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]）

        q(τ) = a0 + a1·τ + a2·τ² + a3·τ³

        返回：[a0, a1, a2, a3]

        """

        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):

        """在归一化时间τ处评估速度（rad/s）"""

        if T <= 1e-12:

            return 0.0

        a0, a1, a2, a3 = coeffs

        return (a1 + 2 * a2 * tau + 3 * a3 * tau**2) / T

 

    def compute_internal_velocities(self, q_waypoints, t_waypoints):

        """

        计算内部路点处的连续速度（有限差分）

        端点速度设为0

        """

        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=50):

        """

        生成完整关节空间轨迹

        输入：q_waypoints (n_waypoints × 4)

        返回：采样时刻、关节角数组、关节速度数组、路点时间

        """

        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)

 

        # 为4个关节分别求解多项式系数

        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))

 

        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)

 

        return {

            't_samples': t_samples,

            'q_samples': q_samples,

            'qd_samples': qd_samples,

            't_waypoints': t_waypoints,

            'q_waypoints': q_waypoints,

            'coeffs_all': coeffs_all

        }

 

"robot/redundancy_tester.py" — 冗余路点测试

 

"""

冗余路点测试：在基准关节角路点之间插入多余中间路点

对比基准轨迹与冗余轨迹的末端路径、关节速度、路径长度

"""

 

 

import numpy as np

import math

from robot.models import RobotArmConfig

from robot.trajectory_generator import JointTrajectoryGenerator

from robot.forward_kinematics import ForwardKinematicsSCARA

from robot.inverse_kinematics import InverseKinematicsSCARA

 

 

class RedundancyTester:

    def __init__(self, config: RobotArmConfig):

        self.cfg = config

        self.traj_gen = JointTrajectoryGenerator(

            config, nominal_joint_speed=0.5)

        self.fk = ForwardKinematicsSCARA(config)

        self.ik = InverseKinematicsSCARA(config)

 

    def insert_redundant_waypoints(self, base_q_waypoints, n_redundant,

                                   segment_index=1):

        """

        在base_q_waypoints[segment_index]和segment_index+1之间

        线性插值插入n_redundant个冗余关节角路点

        """

        q_start = base_q_waypoints[segment_index]

        q_end = base_q_waypoints[segment_index + 1]

 

        redundant_qs = []

        for i in range(1, n_redundant + 1):

            alpha = i / (n_redundant + 1)

            q_red = q_start + alpha * (q_end - q_start)

            redundant_qs.append(q_red)

 

        # 拼接新路点列表

        new_q_waypoints = np.vstack([

            base_q_waypoints[:segment_index + 1],

            np.array(redundant_qs),

            base_q_waypoints[segment_index + 1:]

        ])

 

        return new_q_waypoints

 

    def compute_cartesian_trajectory(self, joint_traj):

        """

        将关节空间轨迹映射到笛卡尔空间

        """

        q_samples = joint_traj['q_samples']

        poses = self.fk.compute_all_poses(q_samples)

 

        return {

            't_samples': joint_traj['t_samples'],

            'pos_samples': poses,

            'q_samples': q_samples,

            'qd_samples': joint_traj['qd_samples']

        }

 

    def compute_path_length(self, pos_samples):

        """

        计算末端路径总长度

        L = Σ ||p_{k+1} - p_k||

        """

        length = 0.0

        n = len(pos_samples)

        for k in range(1, n):

            dx = pos_samples[k, 0] - pos_samples[k-1, 0]

            dy = pos_samples[k, 1] - pos_samples[k-1, 1]

            dz = pos_samples[k, 2] - pos_samples[k-1, 2]

            length += math.sqrt(dx**2 + dy**2 + dz**2)

        return length

 

    def check_waypoint_reachability(self, q_waypoints):

        """

        检查每个路点是否可达（通过IK反解验证）

        返回可达性数组

        """

        n = len(q_waypoints)

        reachable = np.ones(n, dtype=bool)

 

        for i in range(n):

            q = q_waypoints[i]

            pose = self.fk.compute_pose(q)

            q_back = self.ik.solve(pose[0], pose[1], pose[2], pose[3],

                                   elbow_config="elbow_up")

            if q_back is None:

                reachable[i] = False

 

        return reachable

 

    def run_comparison(self, base_q_waypoints, n_redundant_list,

                       segment_index=1):

        """

        运行对比测试

        """

        results = {}

 

        # 基准轨迹（0个冗余路点）

        base_traj = self.traj_gen.generate_trajectory(base_q_waypoints)

        base_cartesian = self.compute_cartesian_trajectory(base_traj)

        base_path_length = self.compute_path_length(

            base_cartesian['pos_samples'])

        base_reachable = self.check_waypoint_reachability(base_q_waypoints)

 

        results[0] = {

            'q_waypoints': base_q_waypoints,

            'joint_traj': base_traj,

            'cartesian': base_cartesian,

            'path_length': base_path_length,

            'reachable': base_reachable,

            'n_redundant': 0

        }

 

        # 不同冗余路点数量的轨迹

        for n_red in n_redundant_list:

            if n_red == 0:

                continue

 

            redundant_q_wps = self.insert_redundant_waypoints(

                base_q_waypoints, n_red, segment_index)

            traj = self.traj_gen.generate_trajectory(redundant_q_wps)

            cartesian = self.compute_cartesian_trajectory(traj)

            path_length = self.compute_path_length(

                cartesian['pos_samples'])

            reachable = self.check_waypoint_reachability(redundant_q_wps)

 

            results[n_red] = {

                'q_waypoints': redundant_q_wps,

                'joint_traj': traj,

                'cartesian': cartesian,

                'path_length': path_length,

                'reachable': reachable,

                'n_redundant': n_red

            }

 

        return results

 

"robot/visualize_trajectory.py" — 可视化

 

"""

轨迹可视化：冗余路点测试分析图表

4张图表：XY平面路径对比、关节速度峰值对比、路径长度变化、路点可达性

"""

 

 

import numpy as np

import matplotlib.pyplot as plt

 

plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']

plt.rcParams['axes.unicode_minus'] = False

 

 

class TrajectoryVisualizer:

    def __init__(self, results_dir="results"):

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_xy_path_comparison(self, results):

        """① XY平面路径对比"""

        fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 6))

 

        colors = {0: 'steelblue', 1: 'orange', 2: 'green', 4: 'red'}

 

        # 左图：完整路径

        for n_red, data in results.items():

            color = colors.get(n_red, 'gray')

            pos = data['cartesian']['pos_samples']

            ax1.plot(pos[:, 0], pos[:, 1], '-', linewidth=1.5,

                     color=color, label=f'{n_red}个冗余路点')

 

        # 标注基准路点

        base_q_wps = results[0]['q_waypoints']

        base_pos = results[0]['cartesian']['pos_samples']

        # 路点位置（从关节角反算）

        for i in range(len(base_q_wps)):

            # 找到最近的采样点

            ax1.plot(base_pos[i*50, 0], base_pos[i*50, 1],

                     'ks', markersize=6)

            ax1.annotate(f'P{i}', (base_pos[i*50, 0],

                                   base_pos[i*50, 1]),

                         fontsize=8, ha='right')

 

        ax1.set_xlabel('X (m)')

        ax1.set_ylabel('Y (m)')

        ax1.set_title('XY平面末端路径对比')

        ax1.legend(fontsize=8)

        ax1.grid(True, alpha=0.3)

        ax1.set_aspect('equal', adjustable='box')

 

        # 右图：局部放大（第2段）

        for n_red, data in results.i

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