《Python机器人基础原理》第五十五篇：奇异区轨迹测试——在靠近奇异位形处生成笛卡尔轨迹，观察逆运动学求解波动

 

1. 实际工业应用场景

 

周三下午，某3C电子元件精密装配产线。一台4-DOF SCARA机器人（大臂长0.40 m，小臂长0.30 m，额定负载3 kg，重复定位精度±0.01 mm）正在执行PCB板上微型连接器插装任务。工艺要求末端执行器沿一条近似直线的下插轨迹运动，轨迹末端恰好经过工作空间的内圈区域——即大臂和小臂几乎完全折叠的位置。

 

调试工程师小李盯着上位机监控画面，眉头紧锁："师傅，你看这个。轨迹规划没问题，笛卡尔空间路径是直线，速度也匀速。但一到靠近内圈这个区域，关节2的角速度曲线就出现剧烈尖峰，从0.3 rad/s瞬间飙到3.7 rad/s，然后又掉回来。示教器上关节扭矩也跟着飙升。"

 

旁边的主任工程师老刘拉过椅子坐下："你把轨迹末端的笛卡尔坐标报一下。"

 

"X=0.108 m，Y=0.025 m，Z=0.080 m。差不多就在基座中心附近偏一点的位置。"

 

老刘点了点头："0.108和0.025，到基座中心的距离大约0.111 m。你的大臂0.40 m，小臂0.30 m，两者之差是0.10 m。末端跑到离基座中心刚过0.10 m的地方——这就是肘部奇异区。两臂几乎折叠，雅可比矩阵在这个位置接近奇异，条件数飙升。逆运动学求解时，末端微小的笛卡尔位置误差会被放大成巨大的关节角度变化。你看到的速度尖峰就是IK求解器对末端直线插补微小量化误差的放大效应。"

 

"那怎么测？"

 

"离线仿真里，你在肘部奇异区附近生成一条笛卡尔直线轨迹，逐点做逆运动学，记录每个点的关节角、关节角速度，再算条件数。你会看到条件数从正常区域的个位数飙升到几百的时候，关节角速度就开始出现尖锐峰值。这就是奇异位形对轨迹规划的影响——不是你的轨迹有问题，是工作空间选错了。"

 

"那产线上怎么办？"

 

"两个思路。一是轨迹规划阶段就避开奇异区，让末端路径离内圈保持最小安全距离。二是用阻尼最小二乘法做逆运动学，给雅可比伪逆加阻尼项，牺牲一点末端精度换取关节运动的平滑性。今天你先在仿真里把现象复现出来，下周我们再看DLS的实现。"

 

构型约定说明：本篇采用4-DOF SCARA简化模型（2个水平旋转关节+1个垂直移动关节+1个末端旋转关节），用于奇异区逆运动学波动的算法验证。连杆参数：大臂长0.40 m、质量1.5 kg、质心位于0.20 m处、绕Z轴惯量0.032 kg·m²；小臂长0.30 m、质量1.0 kg、质心位于0.15 m处、绕Z轴惯量0.015 kg·m²；升降关节行程0.25 m。全部参数内置，自包含可复现。通用DH正运动学复用公共模块（

"robot/common/kinematics.py"）。

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于奇异位形附近逆运动学求解波动的算法验证。不可直接用于真实机器人控制器或产线轨迹规划。真实工业机器人的逆运动学求解在控制器中以固件方式实时执行，本仿真未考虑关节限位保护、碰撞检测等安全机制。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第3章 连杆参数及运动学求解 → 3.3 逆运动学 解析逆运动学在肘部奇异区求解不稳定，微小位置误差放大为关节速度尖峰

第3章 连杆参数及运动学求解 → 3.2 正运动学 雅可比矩阵条件数计算、奇异判定

第6章 轨迹规划与控制 → 6.2 笛卡尔空间轨迹规划 直线插补轨迹经过内圈折叠区导致关节速度发散

第2章 位姿描述与坐标变换 笛卡尔空间位置误差→关节空间角度放大的几何解释

 

本篇代码解决 L2 离线奇异区探测层。输入：笛卡尔空间直线轨迹（起点→终点，N个插值点）。处理：逐点做解析逆运动学（固定elbow-up单分支）→计算雅可比条件数→数值差分求关节速度→统计波动幅度。输出：奇异区判定报告+可视化曲线。

 

不覆盖：阻尼最小二乘法（DLS）实时求解、elbow-up/down分支切换导致的跳变、冗余自由度自运动优化、动力学约束下的奇异规避。

 

关于"振荡"与"尖峰"的区分：本篇使用固定分支解析IK，复现的是奇异区附近的关节速度尖锐峰值（确定性函数对末端量化误差的放大），而非控制器在分支间跳变或数值迭代不收敛导致的振荡。振荡现象需要分支切换或数值IK才能复现，留待下一篇（DLS+分支切换）正面处理。

 

3. 核心逻辑讲解

 

物理直觉

 

奇异位形是机器人工作空间中一类特殊位形，在这个位置上机器人的某些自由度"丢失"了——末端在某个方向上无法产生速度，即使关节在快速转动。SCARA机器人有两类典型奇异：

 

- 肘部奇异（内圈折叠）：大臂和小臂几乎完全折叠，r≈|a1-a2|，此时θ2≈π，雅可比接近降秩。末端在内圈附近沿径向的微小移动需要关节2产生巨大角速度。

- 伸直奇异（外圈伸直）：大臂和小臂几乎完全伸直，r≈a1+a2，此时θ2≈0，同样导致雅可比接近降秩。

 

数学上，末端速度v和关节速度θ̇之间的关系是 v = J·θ̇，其中J是雅可比矩阵。正常位形下J满秩，可以从θ̇ = J⁻¹·v稳定求解。但在奇异区，J的条件数（最大奇异值/最小奇异值）飙升到几百甚至上千。此时J⁻¹的微小数值误差会被放大成巨大的θ̇——末端移动1 mm，关节可能要转几度。这就是"速度尖峰"的本质。

 

业务逻辑 → 代码映射

 

输入：笛卡尔直线轨迹（起点X0,Y0,Z0 → 终点X1,Y1,Z1，N个插值点）

      机器人DH参数（大臂长a1=0.40 m, 小臂长a2=0.30 m）

  ↓

【步骤1：逐点生成笛卡尔位姿】

  直线插值：P_i = P_start + i/(N-1) * (P_end - P_start)

  ↓

【步骤2：对每个点做解析逆运动学】

  余弦定理求θ2：cos(θ2) = (r²-a1²-a2²)/(2·a1·a2)

  θ1 = atan2(y,x) - atan2(a2·sinθ2, a1+a2·cosθ2)

  固定elbow-up分支（θ2>0）

  记录θ1_i, θ2_i；不可达点标记

  ↓

【步骤3：计算雅可比条件数】

  J = ∂(x,y)/∂(θ1,θ2) 的2×2雅可比

  cond(J) = σ_max / σ_min（SVD分解）

  标记 cond > 阈值（如100）的点为"接近奇异"

  不可达点单独统计，不与奇异混淆

  ↓

【步骤4：分段数值差分求关节速度】

  对可达点区间做中心差分：ω_i = (θ_{i+1} - θ_{i-1}) / (2·Δt)

  不可达区间断开，不填0

  ↓

【步骤5：统计波动】

  分别计算关节1/关节2的速度标准差和峰值

  条件数 vs 关节速度散点图

  ↓

输出：奇异区报告 + 可视化

 

关键公式文字翻译

 

2R平面逆运动学（解析解）：已知末端位置(x,y)，求θ1和θ2。先算末端到基座的距离r=√(x²+y²)。用余弦定理求θ2：cos(θ2) = (r²-a1²-a2²)/(2·a1·a2)。当r接近|a1-a2|时（肘部奇异），cos(θ2)接近-1，θ2接近π，分母接近零，θ1对(x,y)极度敏感。当r接近a1+a2时（伸直奇异），cos(θ2)接近1，θ2接近0。θ1 = atan2(y,x) - atan2(a2·sinθ2, a1+a2·cosθ2)。

 

雅可比矩阵（2×2）：第一行是末端X位置对θ1和θ2的偏导：∂x/∂θ1 = -a1·sin(θ1) - a2·sin(θ1+θ2)，∂x/∂θ2 = -a2·sin(θ1+θ2)。第二行是Y位置偏导：∂y/∂θ1 = a1·cos(θ1) + a2·cos(θ1+θ2)，∂y/∂θ2 = a2·cos(θ1+θ2)。本篇仅用旋转关节子块（2×2），各列均为长度量纲(m)，条件数无量纲化问题不显著。

 

条件数：cond(J) = σ_max/σ_min，通过SVD分解得到。cond=1表示最优（各向同性），cond>100表示严重各向异性（接近奇异），cond→∞表示完全奇异。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。通用DH正运动学复用公共模块。

 

项目结构树

 

singularity_trajectory_sim/

├── run_singularity_test.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── common/

│ │ ├── __init__.py

│ │ └── kinematics.py

│ ├── models.py

│ ├── scara_ik.py

│ ├── jacobian.py

│ ├── singularity_checker.py

│ └── visualize_singularity.py

├── tests/

│ ├── __init__.py

│ └── test_singularity_trajectory.py

└── results/

 

核心源码

 

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

 

"""

数据模型：4-DOF SCARA机械臂连杆参数、笛卡尔轨迹配置

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

本篇聚焦运动学奇异分析，mass/com/inertia为后续动力学篇预留

"""

 

 

from dataclasses import dataclass, field

from typing import List, Tuple

 

 

@dataclass

class LinkConfig:

    """单连杆物理参数"""

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

    mass: float = 0.0 # 连杆质量 (kg)，本篇未使用，为动力学篇预留

    com_position: float = 0.0 # 质心位置 (m)，本篇未使用

    inertia_zz: float = 0.0 # 绕Z轴惯量 (kg·m²)，本篇未使用

    joint_limits: Tuple[float, float] = (-3.14, 3.14)

 

 

@dataclass

class CartesianWaypoint:

    """单个笛卡尔路径点"""

    x: float = 0.0 # X坐标 (m)

    y: float = 0.0 # Y坐标 (m)

    z: float = 0.0 # Z坐标 (m)

    theta: float = 0.0 # 末端旋转角 (rad)

 

 

@dataclass

class CartesianTrajectoryConfig:

    """笛卡尔空间直线轨迹配置"""

    start_point: CartesianWaypoint = field(default_factory=CartesianWaypoint)

    end_point: CartesianWaypoint = field(default_factory=CartesianWaypoint)

    num_points: int = 50 # 插值点数量

    duration: float = 5.0 # 轨迹总时长 (s)

 

    def __post_init__(self):

        if self.num_points < 2:

            raise ValueError("插值点数量必须≥2")

 

 

@dataclass

class ScaraArmConfig:

    """

    4-DOF SCARA机械臂配置

    关节1：大臂旋转（肩），关节2：小臂旋转（肘）

    关节3：升降移动，关节4：末端旋转

    """

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

    base_height: float = 0.30 # 基座高度 (m)

 

    def __post_init__(self):

        if not self.links:

            self.links = [

                LinkConfig(length=0.40, mass=1.5, com_position=0.20,

                          inertia_zz=0.032, joint_limits=(-3.14, 3.14)),

                LinkConfig(length=0.30, mass=1.0, com_position=0.15,

                          inertia_zz=0.015, joint_limits=(-3.14, 3.14)),

                LinkConfig(length=0.25, mass=0.5, com_position=0.125,

                          inertia_zz=0.005, joint_limits=(0.0, 0.25)),

                LinkConfig(length=0.0, mass=0.2, com_position=0.0,

                          inertia_zz=0.001, joint_limits=(-3.14, 3.14)),

            ]

 

"robot/scara_ik.py" — SCARA逆运动学（解析解）

 

"""

4-DOF SCARA逆运动学求解

平面2R部分解析求解（余弦定理），升降和末端旋转直接映射

固定elbow-up分支（θ2>0），复现奇异区速度尖峰

"""

 

 

import numpy as np

 

 

class ScaraInverseKinematics:

    """

    SCARA逆运动学求解器

    输入：末端笛卡尔位置 (x, y, z, theta)

    输出：关节角度 [θ1, θ2, d3, θ4]

    注意：本篇使用固定elbow-up单分支解析解，用于复现奇异区速度尖峰。

    分支切换导致的振荡现象需DLS+分支管理，留待下一篇。

    """

 

    def __init__(self, a1: float, a2: float):

        """

        a1: 大臂长度 (m)

        a2: 小臂长度 (m)

        """

        if a1 <= 0 or a2 <= 0:

            raise ValueError("连杆长度必须为正数")

        self.a1 = a1

        self.a2 = a2

        self.reach_max = a1 + a2

        self.reach_min = abs(a1 - a2)

 

    def solve_2r_analytical(self, x: float, y: float, elbow_up: bool = True):

        """

        2R平面解析逆解

        elbow_up=True时θ2>0（SCARA俯视平面中肘部向一侧弯曲）

        返回 (theta1, theta2) 或 None（不可达）

        """

        r_sq = x*x + y*y

        r = np.sqrt(r_sq)

 

        # 可达性检查：末端必须在工作空间圆环内

        if r > self.reach_max or r < self.reach_min:

            return None

 

        # 余弦定理求θ2

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

 

        # 数值稳定性：裁剪到[-1, 1]

        cos_theta2 = np.clip(cos_theta2, -1.0, 1.0)

 

        if elbow_up:

            theta2 = np.arccos(cos_theta2) # θ2 > 0

        else:

            theta2 = -np.arccos(cos_theta2) # θ2 < 0

 

        # 求θ1

        alpha = np.arctan2(y, x)

        beta = np.arctan2(self.a2 * np.sin(theta2),

                          self.a1 + self.a2 * np.cos(theta2))

        theta1 = alpha - beta

 

        return theta1, theta2

 

    def solve_full(self, x: float, y: float, z: float,

                   theta: float, elbow_up: bool = True):

        """

        完整4-DOF逆解

        返回 [θ1, θ2, d3, θ4]

        """

        result_2r = self.solve_2r_analytical(x, y, elbow_up)

        if result_2r is None:

            return None

 

        theta1, theta2 = result_2r

 

        # 升降关节直接映射

        d3 = z

 

        # 末端旋转直接映射

        theta4 = theta

 

        return [theta1, theta2, d3, theta4]

 

    def solve_trajectory(self, waypoints, elbow_up: bool = True):

        """

        对整条轨迹逐点求解

        返回 list of (joint_angles, reachable)

        """

        results = []

        for wp in waypoints:

            if len(wp) == 4:

                x, y, z, theta = wp

            else:

                x, y = wp[0], wp[1]

                z = wp[2] if len(wp) > 2 else 0.0

                theta = wp[3] if len(wp) > 3 else 0.0

 

            angles = self.solve_full(x, y, z, theta, elbow_up)

            reachable = angles is not None

            results.append((angles, reachable))

 

        return results

 

"robot/jacobian.py" — 雅可比矩阵与条件数

 

"""

SCARA 2R平面雅可比矩阵计算与条件数分析

用于判定奇异位形（肘部奇异/伸直奇异）

"""

 

 

import numpy as np

 

 

class ScaraJacobian:

    """

    2R平面雅可比矩阵（仅XY平面旋转关节，忽略升降和末端旋转）

    J = ∂(x,y)/∂(θ1,θ2)

    本篇仅用2×2旋转子块，各列均为长度量纲(m)，条件数无需额外尺度归一化

    """

 

    def __init__(self, a1: float, a2: float):

        if a1 <= 0 or a2 <= 0:

            raise ValueError("连杆长度必须为正数")

        self.a1 = a1

        self.a2 = a2

 

    def compute_jacobian(self, theta1: float, theta2: float):

        """

        计算2×2雅可比矩阵

        J[0,0] = ∂x/∂θ1, J[0,1] = ∂x/∂θ2

        J[1,0] = ∂y/∂θ1, J[1,1] = ∂y/∂θ2

        """

        J = np.zeros((2, 2))

 

        J[0, 0] = (-self.a1 * np.sin(theta1)

                   - self.a2 * np.sin(theta1 + theta2))

        J[0, 1] = -self.a2 * np.sin(theta1 + theta2)

        J[1, 0] = (self.a1 * np.cos(theta1)

                   + self.a2 * np.cos(theta1 + theta2))

        J[1, 1] = self.a2 * np.cos(theta1 + theta2)

 

        return J

 

    def condition_number(self, theta1: float, theta2: float,

                         cond_threshold: float = 100.0):

        """

        计算雅可比矩阵条件数（2-范数）

        cond = σ_max / σ_min

        返回 (cond, singular_flag, sigma_min, sigma_max)

        """

        J = self.compute_jacobian(theta1, theta2)

 

        try:

            singular_values = np.linalg.svd(J, compute_uv=False)

            sigma_max = singular_values[0]

            sigma_min = singular_values[1]

 

            if sigma_min < 1e-12:

                return float('inf'), True, sigma_min, sigma_max

 

            cond = sigma_max / sigma_min

            singular_flag = cond > cond_threshold

            return cond, singular_flag, sigma_min, sigma_max

 

        except np.linalg.LinAlgError:

            return float('inf'), True, 0.0, 0.0

 

    def manipulability_index(self, theta1: float, theta2: float):

        """

        可操作度指标 w = sqrt(det(J·J^T))

        值越小越接近奇异

        """

        J = self.compute_jacobian(theta1, theta2)

        w = np.sqrt(abs(np.linalg.det(J @ J.T)))

        return w

 

"robot/singularity_checker.py" — 奇异区检测与波动统计

 

"""

奇异区轨迹检测与逆解波动统计

不可达点与奇异点分开统计，分段差分避免虚假尖峰

"""

 

 

import numpy as np

 

 

class SingularityChecker:

    """

    奇异区检测器

    不可达（工作空间之外）与接近奇异（条件数>阈值）独立判定

    """

 

    def __init__(self, a1: float, a2: float,

                 cond_threshold: float = 100.0):

        self.jacobian = ScaraJacobian(a1, a2)

        self.cond_threshold = cond_threshold

 

    def analyze_trajectory(self, waypoints, joint_angles_list,

                           reachable_flags, dt: float):

        """

        分析整条轨迹

        waypoints: [(x,y,z,theta), ...]

        joint_angles_list: [[θ1,θ2,d3,θ4], ...]

        reachable_flags: [bool, ...]

        dt: 时间步长 (s)

        """

        n = len(waypoints)

        condition_numbers = []

        singular_flags = []

        manipulability = []

        sigma_mins = []

        sigma_maxs = []

 

        # 逐点计算条件数（仅对可达点）

        for i, (angles, reachable) in enumerate(

            zip(joint_angles_list, reachable_flags)

        ):

            if not reachable or angles is None:

                condition_numbers.append(float('inf'))

                singular_flags.append(False) # 不可达≠奇异

                manipulability.append(0.0)

                sigma_mins.append(0.0)

                sigma_maxs.append(0.0)

                continue

 

            theta1, theta2 = angles[0], angles[1]

            cond, is_singular, sig_min, sig_max = (

                self.jacobian.condition_number(

                    theta1, theta2, self.cond_threshold

                )

            )

            w = self.jacobian.manipulability_index(theta1, theta2)

 

            condition_numbers.append(cond)

            singular_flags.append(is_singular)

            manipulability.append(w)

            sigma_mins.append(sig_min)

            sigma_maxs.append(sig_max)

 

        # 分段数值差分求关节速度（中心差分，跳过不可达点）

        joint_velocities = [[], []]

        joint_accelerations = [[], []]

 

        for j in range(2): # θ1, θ2

            vel_j = [float('nan')] * n

            angles_j = [angles[j] if (angles and reachable_flags[i])

                        else float('nan')

                        for i, angles in enumerate(joint_angles_list)]

 

            # 找可达区间，在区间内做差分

            i = 0

            while i < n:

                if not reachable_flags[i]:

                    i += 1

                    continue

                # 找到当前可达区间的起始

                start = i

                while i < n and reachable_flags[i]:

                    i += 1

                end = i - 1

 

                # 区间内有至少2个点才做差分

                if end - start >= 1:

                    # 首点：前向差分

                    vel_j[start] = (

                        (angles_j[start+1] - angles_j[start]) / dt

                    )

                    # 中间点：中心差分

                    for k in range(start+1, end):

                        vel_j[k] = (

                            (angles_j[k+1] - angles_j[k-1]) / (2.0 * dt)

                        )

                    # 末点：后向差分

                    if end > start:

                        vel_j[end] = (

                            (angles_j[end] - angles_j[end-1]) / dt

                        )

 

            joint_velocities[j] = vel_j

 

        # 加速度（对速度差分，同样分段）

        for j in range(2):

            acc_j = [float('nan')] * n

            vel_j = joint_velocities[j]

 

            i = 0

            while i < n:

                if not reachable_flags[i]:

                    i += 1

                    continue

                start = i

                while i < n and reachable_flags[i]:

                    i += 1

                end = i - 1

 

                if end - start >= 2:

                    acc_j[start] = (vel_j[start+1] - vel_j[start]) / dt

                    for k in range(start+1, end):

                        acc_j[k] = (

                            (vel_j[k+1] - vel_j[k-1]) / (2.0 * dt)

                        )

                    acc_j[end] = (vel_j[end] - vel_j[end-1]) / dt

 

            joint_accelerations[j] = acc_j

 

        # 波动统计（分别统计两关节，排除nan）

        vel_std_j1 = float('nan')

        vel_std_j2 = float('nan')

        vel_peak_j1 = 0.0

        vel_peak_j2 = 0.0

 

        valid_vels_j1 = [v for v in joint_velocities[0]

                         if not np.isnan(v)]

        valid_vels_j2 = [v for v in joint_velocities[1]

                         if not np.isnan(v)]

 

        if valid_vels_j1:

            vel_std_j1 = float(np.std(valid_vels_j1))

            vel_peak_j1 = float(max(abs(v) for v in valid_vels_j1))

        if valid_vels_j2:

            vel_std_j2 = float(np.std(valid_vels_j2))

            vel_peak_j2 = float(max(abs(v) for v in valid_vels_j2))

 

        valid_conds = [c for c in condition_numbers

                       if c != float('inf')]

        cond_peak = max(valid_conds) if valid_conds else float('inf')

 

        singular_count = sum(1 for s in singular_flags if s)

        unreachable_count = sum(1 for r in reachable_flags if not r)

 

        return {

            'condition_numbers': condition_numbers,

            'singular_flags': singular_flags,

            'manipulability': manipulability,

            'sigma_mins': sigma_mins,

            'sigma_maxs': sigma_maxs,

            'joint_velocities': joint_velocities,

            'joint_accelerations': joint_accelerations,

            'vel_std_j1': vel_std_j1,

            'vel_std_j2': vel_std_j2,

            'vel_peak_j1': vel_peak_j1,

            'vel_peak_j2': vel_peak_j2,

            'cond_peak': cond_peak,

            'singular_count': singular_count,

            'unreachable_count': unreachable_count,

            'total_points': n

        }

 

    def generate_report(self, analysis_result):

        """生成奇异区检测报告（不可达与奇异分开统计）"""

        lines = ["=" * 60, "奇异区轨迹检测报告", "=" * 60]

        lines.append(f"总路径点: {analysis_result['total_points']}")

        lines.append(

            f"不可达点数: {analysis_result['unreachable_count']}"

        )

        lines.append(

            f"接近奇异点数: {analysis_result['singular_count']}"

        )

        if analysis_result['total_points'] > 0:

            pct = (100.0 * analysis_result['singular_count']

                   / analysis_result['total_points'])

            lines.append(f" ({pct:.1f}%的路径点接近奇异)")

        lines.append(

            f"峰值条件数: {analysis_result['cond_peak']:.1f}"

        )

        if not np.isnan(analysis_result['vel_std_j1']):

            lines.append(

                f"关节1速度标准差: "

                f"{analysis_result['vel_std_j1']:.4f} rad/s"

            )

            lines.append(

                f"关节1速度峰值: "

                f"{analysis_result['vel_peak_j1']:.4f} rad/s"

            )

        if not np.isnan(analysis_result['vel_std_j2']):

            lines.append(

                f"关节2速度标准差: "

                f"{analysis_result['vel_std_j2']:.4f} rad/s"

            )

            lines.append(

                f"关节2速度峰值: "

                f"{analysis_result['vel_peak_j2']:.4f} rad/s"

            )

        lines.append("-" * 60)

 

        if analysis_result['unreachable_count'] > 0:

            lines.append("✗ 存在不可达点（末端超出工作空间圆环）")

        if analysis_result['singular_count'] > 0:

            lines.append(

                "⚠ 轨迹经过肘部/伸直奇异区，"

                "逆运动学求解存在速度尖峰风险"

            )

            lines.append(

                " 建议：调整路径远离内圈（r≈|a1-a2|）或外圈（r≈a1+a2），"

                "或采用阻尼最小二乘法"

            )

        if (analysis_result['unreachable_count'] == 0

                and analysis_result['singular_count'] == 0):

            lines.append("✓ 轨迹全程远离奇异区，IK求解稳定")

 

        return "\n".join(lines)

 

"robot/visualize_singularity.py" — 可视化

 

"""

奇异区轨迹可视化

4张图表：工作空间+路径/条件数曲线/关节速度波动/可操作度

内圈（肘部奇异）和外圈（伸直奇异）分别标注

"""

 

 

import numpy as np

import matplotlib.pyplot as plt

 

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

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

 

 

class SingularityVisualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(resul

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