《Python机器人基础原理》第九篇：装配机器人——给定末端目标位姿，调用工具箱求解逆运动学，输出全部数学解暂不筛选

 

1. 实际工业应用场景

 

周三下午，某3C产品装配车间，手机中框与玻璃后盖的自动贴合工位。

 

机械臂从传送带取料，运动到贴合工装上方的预设位姿，吸盘下降、释放、完成贴合。看起来行云流水——直到产线换型，新款手机的中框尺寸变了2 mm，工装夹具重新设计，原来的示教点位全部作废。

 

调试员小陈站在机器人控制柜前，手里攥着一张CAD图纸，上面标着新工装上贴合面的精确位姿：x=285.3 mm, y=42.7 mm, z=380 mm，绕z轴旋转-15°。他知道该让机器人末端到这个位姿，但关节该转到多少度？

 

"以前都是示教器上手动挪过去，一个点一个点试。"小陈跟旁边负责工艺的李工说。

 

"这款中框有4个贴合工位，每个工位要取料、中转、贴合、退出四个位姿。你手动示教十六个点，每个点调半小时，两天过去了。而且换下一个型号又得重来。"

 

"那怎么办？"

 

"你给我写一个逆运动学求解器。我把目标位姿输进去，你给我算出关节该转多少度——全部可能的角度组合都给我，我自己在示教器上挑一个不撞机的。"

 

这就是本篇要解决的问题：给定一台4-DOF SCARA装配机器人的结构参数和末端目标位姿（x, y, z, 绕z轴姿态角φ），调用符号推导工具箱求解逆运动学，输出全部数学解（多个关节角度组合），暂不筛选最优解。不调用任何第三方机器人商业库，用sympy做符号推导获取解析表达式，numpy做数值代入。让调试工程师在换型时不再靠手感试点，而是用数学拿到全部可能的关节构型。

 

机器人构型参数：4-DOF SCARA装配机器人，3个旋转关节（基座回转、大臂水平旋转、小臂水平旋转，三轴竖直平行）+1个垂直平移关节（z轴升降）。连杆参数：大臂长350 mm，小臂长280 mm，z轴行程±75 mm（相对基座面），基座安装高度350 mm（编入D-H表d₁=0.350 m）。末端工具为真空吸盘，质量0.8 kg，吸盘直径120 mm。目标位姿精度要求±0.1 mm。单节拍周期4 s。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于逆运动学解析解推导与验证。不可直接用于真实机器人控制器、PLC或装配产线。真实装配现场的逆解筛选需结合关节限位、奇异规避、碰撞检测与动力学约束。

 

2. 痛点分析

 

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

第3章 连杆参数及运动学求解 → 3.4 逆运动学 给定末端目标位姿，求解全部可能的关节角度组合，SCARA构型存在肘部上/下翻两个解分支

第3章 连杆参数及运动学求解 → 3.2 D-H参数法 基于标准D-H参数表建立正运动学方程，作为逆解推导的起点

第3章 连杆参数及运动学求解 → 3.3 正运动学 逆解自洽校验：将求出的关节角代回正运动学，验证末端是否回到目标位姿

 

本篇代码解决 L1 离线仿真校核层。输入末端目标位姿+机器人D-H参数，输出全部逆解关节角度。代码不是设备实时控制器，不做解筛选（不判断哪组解更优），不做轨迹规划，不做动力学校验。它只做一件事：把笛卡尔空间的目标位姿"翻译"回关节空间的所有可能姿态——让工艺工程师拿到全部候选构型，在示教器上排除撞机的解后直接选用。

 

不覆盖：逆解最优筛选、关节限位检查、碰撞检测、轨迹插补、动力学、5轴以上冗余构型、数值迭代逆解（本篇用解析法）。

 

3. 核心逻辑讲解

 

物理直觉

 

逆运动学就是正运动学的反问题。正运动学问的是"我知道每节脊椎转了多少度，指尖在哪"；逆运动学问的是"我知道指尖要在哪个位置，每节脊椎该转多少度"。

 

听起来只是把方程反着解，实际上难度天差地别。正运动学是"往前算"——逐级矩阵乘法，结果唯一确定。逆运动学是"倒推"——一个末端位置可能对应多种关节组合，就像你伸手拿桌上的杯子，可以从左边伸过去，也可以从右边绕过去，甚至手肘朝上翻着够。

 

SCARA机器人是逆解最简单的构型之一。前两个旋转关节决定末端在XY平面上的位置（纯平面几何，余弦定理直接解），第三个旋转关节决定末端姿态角（由目标姿态唯一确定），第四个平移关节决定z高度（直接减法）。所以SCARA的逆解有解析表达式，不需要迭代猜，用笔和纸就能推导出全部解。

 

业务逻辑 → 代码映射

 

输入：末端目标位姿 (x, y, z, φ) + SCARA几何参数 (L₁, L₂, d₁)

  ↓

步骤1：数据校验——目标位姿是否在可达空间内

       水平距离 r = √(x²+y²)，检查 |L₁-L₂| ≤ r ≤ L₁+L₂

  ↓

步骤2：平面几何求解关节2角度（肘角）

   - 余弦定理：r² = L₁² + L₂² + 2·L₁·L₂·cos(θ₂)

   - 两个解分支：肘部"下翻"θ₂=-acos(...) 与"上翻"θ₂=+acos(...)

  ↓

步骤3：求解关节1角度

   - θ₁ = atan2(y, x) - atan2(L₂·sin(θ₂), L₁+L₂·cos(θ₂))

   - 每个θ₂分支对应唯一θ₁

  ↓

步骤4：求解关节3角度（姿态约束）

   - 目标姿态 φ = θ₁ + θ₂ + θ₃ → θ₃ = φ - θ₁ - θ₂

   - 姿态角唯一确定，无±π二义性（加π会改变末端朝向）

  ↓

步骤5：求解关节4（z轴平移）

   - d₄ = z_target - d₁（基座高度编入D-H表d₁，保证FK自洽）

  ↓

步骤6：组合全部分支 → 2组数学解（肘下翻/肘上翻）

  ↓

步骤7：正运动学自洽校验（将每组解代回正运动学验证误差）

  ↓

输出：全部数学解（2组关节角度）+ 校验结果

 

为什么适合离线仿真

 

- 逆解解析表达式一旦推导出来就是纯代数运算，零迭代、零微分方程

- sympy符号推导确保公式正确性，避免手算抄错符号

- 全部解输出后工程师可以人工筛选——排除超出关节限位的解、排除导致奇异的解

- 普通PC毫秒级完成，换型时批量求解多个目标位姿毫无压力

 

关键公式文字翻译

 

平面距离约束：末端到基座中心的水平距离 r = √(x²+y²) 必须介于 |L₁-L₂| 和 L₁+L₂ 之间，否则够不着。这是可达性的充要条件（圆环域）。

 

余弦定理求肘角：已知三角形三边（大臂L₁、小臂L₂、水平距离r），用余弦定理 r² = L₁²+L₂²+2·L₁·L₂·cos(θ₂) 直接算出肘关节角θ₂。θ₂=0时大小臂完全伸直，θ₂=π时完全折叠。存在两个方向——肘部向上翻（+acos）和向下弯（-acos），对应逆解的两个分支。

 

基座角求解：θ₁ = 目标方位角atan2(y,x) 减去 小臂末端方位角atan2(L₂·sinθ₂, L₁+L₂·cosθ₂)。几何意义是从基座到末端的总方位角中扣除小臂带来的偏转角。

 

姿态角叠加：SCARA末端绕z轴的姿态角φ等于三个旋转关节角之和（φ = θ₁+θ₂+θ₃）。已知φ，反推θ₃唯一确定，不存在±π二义性——因为旋转一周后末端朝向会改变，不再是同一个位姿。

 

z轴直接映射：垂直平移关节的目标位置 d₄ = z_target - d₁，线性关系，唯一解。d₁编入D-H表第一连杆偏距，保证正运动学z = d₁ + d₄ = z_target。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, scipy, matplotlib, sympy。不引入pybullet、roboticstoolbox等任何第三方机器人库。sympy用于符号推导逆解解析表达式（推导主值分支，第二分支由几何对称性手工补充），numpy用于数值代入和正运动学校验。

 

项目结构树

 

robot_inverse_kinematics/

├── run_inverse_kinematics.py

├── requirements.txt

├── ik/

│ ├── __init__.py

│ ├── models.py

│ ├── dh_transform.py

│ ├── forward_kinematics.py

│ ├── inverse_kinematics.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_inverse_kinematics.py

└── results/

 

核心源码

 

"ik/models.py" — dataclass数据模型

 

"""

4-DOF SCARA装配机器人数据模型

连杆参数、D-H表、末端目标位姿结构体

"""

 

from dataclasses import dataclass, field

from typing import List

import numpy as np

 

 

@dataclass

class DHParam:

    """

    标准D-H参数（Denavit-Hartenberg经典形式）

    SCARA前3轴为旋转关节，第4轴为平移关节

    """

    theta: float # 关节角 (rad)，旋转关节变量

    d: float # 连杆偏距 (m)，平移关节变量

    a: float # 连杆长度 (m)

    alpha: float # 连杆扭角 (rad)

 

    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}必须为有限数值, 实际{val}")

 

 

@dataclass

class SCARAParams:

    """

    4-DOF SCARA装配机器人完整参数

    3旋转+1垂直平移，α序列 [0, 0, 0, -π/2]

    前3轴z轴平行（水平面内旋转），第4轴z翻转为垂直平移

    零位：大臂+小臂水平伸直，末端z = d₁ = 0.350 m

    """

    links: List[DHParam]

    joint_names: List[str] = field(default_factory=lambda: [

        'joint1_base_yaw',

        'joint2_elbow_yaw',

        'joint3_wrist_roll',

        'joint4_z_lift'

    ])

    tool_mass: float = 0.8 # 真空吸盘质量 (kg)

    tool_diameter: float = 0.120 # 吸盘直径 (m)

 

    def __post_init__(self):

        if len(self.links) != 4:

            raise ValueError(f"4-DOF SCARA需要4组D-H参数, 实际{len(self.links)}组")

        if len(self.joint_names) != 4:

            raise ValueError(f"需要4个关节名称, 实际{len(self.joint_names)}个")

 

    @classmethod

    def default_assembly_scar(cls):

        """

        默认4-DOF SCARA装配机器人参数

        大臂350mm, 小臂280mm, 基座高度d₁=350mm编入D-H表

        α序列 [0, 0, 0, -π/2]：前3轴z轴平行（水平旋转），第4轴垂直平移

        """

        pi = 3.141592653589793

        links = [

            # theta, d, a, alpha

            DHParam(0.0, 0.350, 0.350, 0.0),

            DHParam(0.0, 0.0, 0.280, 0.0),

            DHParam(0.0, 0.0, 0.0, 0.0),

            DHParam(0.0, 0.0, 0.0, -pi/2),

        ]

        return cls(links=links, tool_mass=0.8, tool_diameter=0.120)

 

    def get_reach_max(self):

        """最大水平可达半径 = L1 + L2"""

        return self.links[0].a + self.links[1].a

 

    def get_reach_min(self):

        """最小水平可达半径 = |L1 - L2|"""

        return abs(self.links[0].a - self.links[1].a)

 

    def get_base_height(self):

        """基座高度 = d₁"""

        return self.links[0].d

 

 

@dataclass

class TargetPose:

    """

    末端目标位姿

    x, y, z: 基座标系下位置 (m)

    phi: 绕基座z轴的目标姿态角 (rad)，用φ避免与第4关节d₄混淆

    """

    x: float

    y: float

    z: float

    phi: float

 

    def __post_init__(self):

        for name, val in [('x', self.x), ('y', self.y), ('z', self.z)]:

            if not np.isfinite(val):

                raise ValueError(f"TargetPose.{name}必须为有限数值")

        if not np.isfinite(self.phi):

            raise ValueError("TargetPose.phi必须为有限数值")

 

    def get_horizontal_distance(self):

        """到基座中心的水平距离"""

        return np.sqrt(self.x**2 + self.y**2)

 

"ik/dh_transform.py" — 标准D-H变换矩阵

 

"""

标准D-H变换矩阵计算

SCARA构型：前3轴旋转，第4轴平移

"""

 

import numpy as np

from ik.models import DHParam

 

 

class DHTransform:

    """单级标准D-H变换矩阵生成器"""

 

    @staticmethod

    def compute_T(theta, d, a, alpha):

        """

        标准D-H齐次变换矩阵 T_{i-1}^{i}

        变换顺序：绕z转θ → 沿z移d → 沿x移a → 绕x转α

        """

        ct = np.cos(theta)

        st = np.sin(theta)

        ca = np.cos(alpha)

        sa = np.sin(alpha)

 

        return np.array([

            [ct, -st * ca, st * sa, a * ct],

            [st, ct * ca, -ct * sa, a * st],

            [0.0, sa, ca, d],

            [0.0, 0.0, 0.0, 1.0]

        ], dtype=float)

 

    @staticmethod

    def compute_T_from_param(param: DHParam):

        """从DHParam对象计算变换矩阵"""

        return DHTransform.compute_T(

            param.theta, param.d, param.a, param.alpha)

 

"ik/forward_kinematics.py" — 正运动学（用于逆解自洽校验）

 

"""

正运动学求解器：逐级左乘D-H变换矩阵

用于逆解校验——将求出的关节角代回，验证末端是否回到目标位姿

"""

 

import numpy as np

from ik.models import SCARAParams

from ik.dh_transform import DHTransform

 

 

class ForwardKinematicsSCARA:

    """4-DOF SCARA正运动学求解器"""

 

    def __init__(self, params: SCARAParams):

        self.params = params

 

    def compute_pose(self, joint_values):

        """

        joint_values: [θ1, θ2, θ3, d4]

        θ1,θ2,θ3: 旋转关节角 (rad)

        d4: 第4轴平移量 (m)

        返回：末端齐次矩阵 T_0^4

        """

        if len(joint_values) != 4:

            raise ValueError(f"需要4个关节值, 实际{len(joint_values)}个")

 

        T = np.eye(4)

        # 前3轴为旋转关节：将关节值写入params.links供矩阵计算读取

        for i in range(3):

            self.params.links[i].theta = joint_values[i]

            T = T @ DHTransform.compute_T_from_param(self.params.links[i])

 

        # 第4轴为平移关节

        self.params.links[3].d = joint_values[3]

        T = T @ DHTransform.compute_T_from_param(self.params.links[3])

 

        return T

 

    def get_position_and_yaw(self, joint_values):

        """

        提取末端位置(x,y,z)和绕z轴姿态角φ

        返回：(x, y, z, phi)

        姿态角 φ = θ1+θ2+θ3，归一化到 [-π, π]

        """

        T = self.compute_pose(joint_values)

        x = T[0, 3]

        y = T[1, 3]

        z = T[2, 3]

        phi = joint_values[0] + joint_values[1] + joint_values[2]

        phi = np.arctan2(np.sin(phi), np.cos(phi))

        return x, y, z, phi

 

"ik/inverse_kinematics.py" — 逆运动学求解器（核心）

 

"""

逆运动学求解器：给定末端目标位姿，求解全部数学解

SCARA构型解析解：平面几何法 + sympy符号推导

输出全部解分支（2组：肘部下翻/上翻），暂不筛选

"""

 

import numpy as np

import sympy as sp

from ik.models import SCARAParams, TargetPose

from ik.forward_kinematics import ForwardKinematicsSCARA

 

 

class InverseKinematicsSCARA:

    """

    4-DOF SCARA逆运动学解析求解器

    基于平面几何（余弦定理）+ sympy符号推导主值分支

    输出全部数学解（2组），不做筛选

    """

 

    def __init__(self, params: SCARAParams):

        self.params = params

        # FK实例与IK共享同一个params对象

        # IK求解时写入theta/d到params.links，FK验证时读取同一状态

        # 这是设计选择：单目标求解场景下保证正逆闭环数据一致性

        self.fk = ForwardKinematicsSCARA(params)

        self.L1 = params.links[0].a # 大臂长 (m)

        self.L2 = params.links[1].a # 小臂长 (m)

        self.d1 = params.links[0].d # 基座高度 (m)，编入D-H表d₁

 

        # sympy符号推导：建立逆解解析表达式（主值分支）

        self._build_symbolic_solver()

 

    def _build_symbolic_solver(self):

        """

        用sympy建立逆运动学解析解的通用表达式

        推导主值分支（肘部下翻），第二分支由几何对称性手工补充

        输入符号：x, y, L1, L2

        输出符号：θ1, θ2（主值）

        """

        x, y, L1, L2 = sp.symbols('x y L1 L2', real=True)

 

        # 水平距离

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

 

        # 余弦定理求肘角θ2（内角）

        # r² = L1² + L2² - 2·L1·L2·cos(π-θ2) = L1²+L2²+2·L1·L2·cos(θ2)

        # → cos(θ2) = (r² - L1² - L2²) / (2·L1·L2)

        cos_theta2 = (r**2 - L1**2 - L2**2) / (2 * L1 * L2)

        theta2_elbow = sp.acos(cos_theta2) # 主值：肘部"下翻"分支

 

        # θ1 = atan2(y, x) - atan2(L2·sin(θ2), L1+L2·cos(θ2))

        phi = sp.atan2(y, x)

        psi = sp.atan2(L2 * sp.sin(theta2_elbow),

                        L1 + L2 * sp.cos(theta2_elbow))

        theta1_sol = phi - psi

 

        # 存储符号表达式供数值代入

        self._sym_theta1 = sp.lambdify((x, y, L1, L2), theta1_sol, 'numpy')

        self._sym_theta2_elbow = sp.lambdify((x, y, L1, L2),

                                              theta2_elbow, 'numpy')

        self._sym_cos_theta2 = sp.lambdify((x, y, L1, L2),

                                            cos_theta2, 'numpy')

 

    def solve_all(self, target: TargetPose):

        """

        求解全部数学解

        SCARA构型共2组解：

        分支A: 肘部下翻(θ2=-acos(...))，θ3=φ-θ1-θ2

        分支B: 肘部上翻(θ2=+acos(...))，θ3=φ-θ1-θ2

        θ3由姿态约束唯一确定，无±π二义性

        返回：列表 of [θ1, θ2, θ3, d4]，每个元素单位rad/rad/rad/m

        """

        x, y, z = target.x, target.y, target.z

        phi = target.phi # 目标姿态角，用φ避免与关节序号混淆

 

        # 可达性检查

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

        r_max = self.L1 + self.L2

        r_min = abs(self.L1 - self.L2)

 

        if r > r_max + 1e-6:

            raise ValueError(

                f"目标点水平距离{r*1000:.1f}mm超出最大可达半径"

                f"{(r_max)*1000:.1f}mm")

        if r < r_min - 1e-6:

            raise ValueError(

                f"目标点水平距离{r*1000:.1f}mm小于最小可达半径"

                f"{(r_min)*1000:.1f}mm")

 

        # 数值代入符号表达式（主值分支）

        cos_t2 = float(self._sym_cos_theta2(x, y, self.L1, self.L2))

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

 

        # 肘部下翻分支（主值，θ2取负）

        theta2_elbow = -np.arccos(cos_t2)

        theta1_elbow = float(self._sym_theta1(x, y, self.L1, self.L2))

        # 姿态约束唯一确定θ3

        theta3_elbow = phi - theta1_elbow - theta2_elbow

 

        # 肘部上翻分支（几何对称，θ2取正）

        theta2_shoulder = np.arccos(cos_t2)

        # 对应θ1：用对称性重新计算

        phi_angle = np.arctan2(y, x)

        psi_shoulder = np.arctan2(self.L2 * np.sin(theta2_shoulder),

                                   self.L1 + self.L2 * np.cos(theta2_shoulder))

        theta1_shoulder = phi_angle - psi_shoulder

        theta3_shoulder = phi - theta1_shoulder - theta2_shoulder

 

        # z轴平移：d4 = z_target - d1（d1编入D-H表，保证FK自洽）

        d4 = z - self.d1

 

        solutions = []

        solutions.append([theta1_elbow, theta2_elbow,

                          theta3_elbow, d4]) # 分支A：肘部下翻

        solutions.append([theta1_shoulder, theta2_shoulder,

                          theta3_shoulder, d4]) # 分支B：肘部上翻

 

        return solutions

 

    def verify_solution(self, joint_values, target: TargetPose,

                         tol=1e-6):

        """

        正运动学自洽校验：将关节角代回正运动学，验证末端位姿

        z方向：FK z = d1 + d4 = d1 + (z_target - d1) = z_target ✓

        姿态：φ = θ1+θ2+θ3 ✓

        """

        x_calc, y_calc, z_calc, phi_calc = \

            self.fk.get_position_and_yaw(joint_values)

 

        pos_error = np.sqrt((x_calc - target.x)**2 +

                            (y_calc - target.y)**2 +

                            (z_calc - target.z)**2)

 

        # 姿态角误差（考虑2π周期）

        yaw_error = abs(phi_calc - target.phi)

        yaw_error = min(yaw_error, 2*np.pi - yaw_error)

 

        return {

            'pass': pos_error < tol and yaw_error < tol,

            'position_error_m': float(pos_error),

            'yaw_error_rad': float(yaw_error),

            'computed_pose': (x_calc, y_calc, z_calc, phi_calc)

        }

 

"ik/visualize.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 Visualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_all_solutions_positions(self, solutions, target, fk_solver):

        """

        ① 全部逆解末端位置散点验证图

        工程结论：两组解通过正运动学算出的末端位置应汇聚于同一目标点

        （散点重合=逆解正确），若偏差大则说明数值计算有误

        """

        fig, ax = plt.subplots(figsize=(8, 8))

 

        # 目标位置（红色星标）

        ax.scatter(target.x * 1000, target.y * 1000,

                    color='red', s=200, marker='*',

                    label='Target Position', zorder=5)

 

        # 各解末端位置（通过正运动学算出实际位置）

        colors = ['blue', 'green']

        labels = ['Sol 1 (Elbow ↓)', 'Sol 2 (Elbow ↑)']

 

        for i, sol in enumerate(solutions):

            x_calc, y_calc, z_calc, phi_calc = \

                fk_solver.get_position_and_yaw(sol)

            ax.scatter(x_calc * 1000, y_calc * 1000,

                        color=colors[i], s=100, marker='o',

                        label=labels[i],

                        zorder=4, alpha=0.8)

            # 坐标标注放在散点旁边，不塞进legend

            ax.annotate(f'({x_calc*1000:.1f}, {y_calc*1000:.1f})',

                         (x_calc * 1000, y_calc * 1000),

                         textcoords="offset points", xytext=(0, 10),

                         ha='center', fontsize=7, color=colors[i])

 

        ax.set_xlabel('X (mm)')

        ax.set_ylabel('Y (mm)')

        ax.set_title('All IK Solutions — End Effector Position Verification\n'

                      '(Both solutions converge to target = IK correct)')

        ax.legend(fontsize=8)

        ax.grid(True, alpha=0.3)

        ax.set_aspect('equal')

 

        plt.tight_layout()

        path = f"{self.results_dir}/01_all_solutions_positions.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_joint_angles_comparison(self, solutions):

        """

        ② 各解关节角度对比柱状图

        工程结论：直观对比2组解的关节角差异，

        帮助工程师判断哪组解关节运动范围更小、更接近当前构型

        """

        labels = ['Sol 1 (Elbow ↓)', 'Sol 2 (Elbow ↑)']

        joint_names = ['θ₁ (rad)', 'θ₂ (rad)', 'θ₃ (rad)', 'd₄ (mm)']

 

        fig, axes = plt.subplots(2, 2, figsize=(12, 8))

        axes = axes.flatten()

 

        colors = ['steelblue', 'orange', 'green', 'red']

        for j in range(4):

            vals = [sol[j] if j < 3 else sol[j]*1000

                     for sol in solutions]

            ax = axes[j]

            bars = ax.bar(labels, vals, color=colors[j], alpha=0.8,

                           edgecolor='black')

            ax.set_title(f'Joint {joint_names[j]} Across Solutions')

            ax.set_ylabel(joint_names[j])

            ax.grid(True, alpha=0.3, axis='y')

 

            for bar, val in zip(bars, vals):

                offset = 0.01 * max(abs(min(vals)), abs(max(vals)), 1e-6)

                ax.text(bar.get_x() + bar.get_width()/2,

                         bar.get_height() + offset,

                         f'{val:.3f}', ha='center', va='bottom',

                         fontsize=8, fontweight='bold')

 

        plt.tight_layout()

        path = f"{self.results_dir}/02_joint_angles_comparison.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_workspace_boundary(self, params, target):

        """

        ③ 工作空间边界图

        工程结论：绘制SCARA可达圆环（内半径|L1-L2|, 外半径L1+L2），

        标注目标点是否在可达范围内

        """

        fig, ax = plt.subplots(figsize=(8, 8))

 

        L1 = params.links[0].a

        L2 = params.links[1].a

        r_min = abs(L1 - L2)

        r_max = L1 + L2

 

        theta_circle = np.linspace(0, 2*np.pi, 200)

        ax.plot(r_max * np.cos(theta_circle) * 1000,

                 r_max * np.sin(theta_circle) * 1000,

                 'b-', linewidth=2, label=f'Max Reach {(r_max*1000):.0f} mm')

        ax.plot(r_min * np.cos(theta_circle) * 1000,

                 r_min * np.sin(theta_circle) * 1000,

                 'r--', linewidth=2, label=f'Min Reach {(r_min*1000):.0f} mm')

 

        # 标注目标点

        r_target = np.sqrt(target.x**2 + target.y**2) * 1000

        ax.plot(target.x * 1000, target.y * 1000,

                 'r*', markersize=15,

                 label=f'Target (r={r_target:.1f} mm)')

 

        ax.set_xlabel('X (mm)')

        ax.set_ylabel('Y (mm)')

        ax.set_title('SCARA Workspace Boundary (Horizontal Plane)')

        ax.legend(fontsize=8)

        ax.grid(True, alpha=0.3)

        ax.set_aspect('equal')

 

        plt.tight_layout()

        path = f"{self.results_dir}/03_workspace_boundary.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_solution_distribution(self, solutions):

        """

        ④ 解分支分布图（θ1, θ2对比）

        工程结论：用分组柱状图展示2组解在(θ1, θ2)上的差异，

        直观显示构型差异——分离越远说明肘部姿态区别越大

        """

        theta1_vals = [sol[0] for sol in solutions]

        theta2_vals = [sol[1] for sol in solutions]

 

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

 

        labels = ['Elbow ↓', 'Elbow ↑']

        x_pos = np.arange(len(labels))

 

        bars1 = ax1.bar(x_pos, theta1_vals, color='steelblue',

                         edgecolor='black', alpha=0.8, width=0.5)

        ax1.set_xticks(x_pos)

        ax1.set_xticklabels(labels)

        ax1.set_ylabel('θ₁ (rad)')

        ax1.set_title('θ₁ Across Solution Branches')

        ax1.grid(True, alpha=0.3, axis='y')

        for bar, val in zip(bars1, theta1_vals):

            ax1.text(bar.get_x() + bar.get_width()/2,

                      bar.get_height() + 0.05,

                      f'{val:.3f}', ha='center', va='bottom', fontsize=9)

 

        bars2 = ax2.bar(x_pos, theta2_vals, color='orange',

                         edgecolor='black', alpha=0.8, width=0.5)

        ax2.set_xticks(x_pos)

        ax2.set_xticklabels(labels)

        ax2.set_ylabel('θ₂ (rad)')

        ax2.set_title('θ₂ Across Solution Branches')

        ax2.grid(True, alpha=0.3, axis='y')

        for bar, val in zip(bars2, theta2_vals):

            ax2.text(bar.get_x() + bar.get_width()/2,

                      bar.get_height() + 0.05,

                      f'{val:.3f}', ha='center', va='bottom', fontsize=9)

 

        plt.tight_layout()

        path = f"{self.results_dir}/04_solution_distribution.png"

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