《Python机器人基础原理》第十篇：车间搬运机器人逆解筛选——结合关节硬件限位剔除超出转角范围的无效解

 

1. 实际工业应用场景

 

周五夜班，某汽车零部件机加工车间的物料转运工位。AGV把装满齿轮毛坯的托盘送到取料点，一台4-DOF关节型搬运机器人需要从托盘上逐个抓取工件，放到数控车床的自动送料轨道上。

 

机器人构型：4-DOF关节型（3旋转+1垂直平移），大臂长400 mm，小臂长320 mm，基座安装高度d₁=300 mm，z轴行程±100 mm。关节硬件限位：θ₁ ±150°、θ₂ ±110°、θ₃ ±180°、d₄ ±100 mm。末端夹持器质量1.2 kg，额定抓取负载5 kg。单节拍周期要求≤3.5 s。

 

凌晨两点，调试工程师老赵蹲在机器人控制柜旁边，手里的咖啡已经凉了。

 

"又撞了。"他对着对讲机那头说，"另一组解，θ₂跑到+125°，直接顶到硬限位，伺服报警E-37。"

 

"你不是用上篇的逆解求解器了吗？怎么还有超限位的解？"对讲机里传来新来的实习生小王的声音。

 

"上篇输出的是全部数学解——两组解都在数学上能到达目标点，但其中一组θ₂=+125°，超出了硬件限位的±110°。数学上成立，物理上撞墙。我得手动一组一组筛，今晚筛了六十多个点位，眼睛都花了。"

 

"那能不能让程序自动筛掉超限的？"

 

"这就是下一篇要写的东西。"

 

本篇解决的问题：在第九篇逆运动学求解器输出的全部数学解（2组）基础上，引入关节硬件限位参数，自动剔除超出每个关节转角范围的无效解，只输出物理可执行的关节构型。不引入任何第三方机器人商业库，纯numpy/scipy实现限位筛选逻辑。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于逆解筛选算法验证。不可直接用于真实机器人控制器、PLC或搬运产线。真实工业现场需额外考虑奇异规避、碰撞检测、动力学约束与实时性要求。

 

2. 痛点分析

 

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

第3章 连杆参数及运动学求解 → 3.4 逆运动学 数学解≠物理可执行解，必须结合关节硬件限位筛选

第3章 连杆参数及运动学求解 → 3.2 D-H参数法 关节变量范围约束（θ_min, θ_max）嵌入D-H参数体系

第3章 连杆参数及运动学求解 → 3.3 正运动学 筛选后剩余解的二次正运动学校验，确认限位内解仍满足位姿精度

 

本篇代码解决 L2 离线解筛选层。输入第九篇输出的全部数学解 + 关节硬件限位参数，输出物理可执行的关节角度组合。代码不是设备实时控制器，不做轨迹插补，不做碰撞检测。它做的事情是：把"数学上能到但物理上撞墙"的解自动扔掉，让工艺工程师拿到手的就是可以直接下发给控制器的候选构型。

 

不覆盖：碰撞检测（需三维几何模型）、动力学约束（力矩/速度/加速度限制）、轨迹规划、实时控制、5轴以上冗余构型、节拍与负载约束（属后续筛选维度，本篇不覆盖）。

 

3. 核心逻辑讲解

 

物理直觉

 

逆运动学求出的数学解就像导航软件给你规划的路线——理论上从A到B有无数条路，但其中有些路是逆行、有些路限高过不去、有些路正在施工。关节硬件限位就是机器人的"交通规则"：每个关节只能在它的物理允许范围内转动，超出去就会触发伺服报警甚至机械损坏。

 

第九篇输出2组解，本篇的任务是对每组解做"体检"：逐个关节检查角度是否落在 [θ_min, θ_max] 区间内。全部关节都合格的解留下，任何一个关节超标的直接淘汰。如果2组全被淘汰，说明这个目标点虽然数学上可达，但在当前限位参数下物理不可达——工程师需要换工位布局或者调整限位参数。

 

业务逻辑 → 代码映射

 

输入：第九篇输出的全部数学解 + 关节硬件限位参数

  ↓

步骤1：读取每组解的关节角度 [θ1, θ2, θ3, d4]

  ↓

步骤2：角度归一化——旋转关节统一到 [-π, π]

       隐含前提：关节是±摆动关节，不是多圈连续转轴

       （SCARA/搬运构型成立，连续转轴机器人不适用此约定）

  ↓

步骤3：逐关节比对限位区间

   - θ1 ∈ [-150°, +150°] ?

   - θ2 ∈ [-110°, +110°] ?

   - θ3 ∈ [-180°, +180°] ?（归一化后恒通过，列出为完整性）

   - d4 ∈ [-100mm, +100mm] ?

  ↓

步骤4：全部通过 → 标记为"有效解"，任一超限 → 标记为"无效解"并丢弃

  ↓

步骤5：筛选后剩余有效解二次正运动学校验（精度兜底，tol=1e-9）

  ↓

步骤6：输出有效解列表 + 淘汰原因统计（按关节索引计数，非字符串匹配）

 

关键公式文字翻译

 

限位区间判定：对每个关节变量 qᵢ，检查 qᵢ,min ≤ qᵢ ≤ qᵢ,max。旋转关节用弧度比较，平移关节用米比较。浮点比较使用1e-10容差，避免边界舍入误判。判定逻辑是简单的区间包含测试——核心难点不在算法，在工程设计：限位参数从机器人本体的伺服驱动器参数表或机械硬挡块设计文档里读取。

 

角度归一化：旋转关节角度可能在不同约定下表示（例如+200°和-160°是同一个物理位置）。筛选前统一用 

"arctan2(sin(θ), cos(θ))" 归一化到 [-π, π]，这是工业控制器的标准做法，防止同一角度因表示方式不同被误判。

 

θ₃限位恒通过说明：θ₃是腕部roll关节，归一化后∈[-π, π]即[-180°, +180°]，与限位区间完全重合，因此这个检查永远通过。物理上合理（roll关节常为±180°或连续转），列出是为了参数完整性，真实约束瓶颈来自θ₁、θ₂、d₄三项。

 

二次正运动学校验：筛选后剩余的解重新代回正运动学，验证末端位置误差仍满足精度要求（<1e-9 m）。这是兜底逻辑——防止浮点舍入或角度归一化引入的微小偏差导致解在实际执行时偏离目标。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。sympy仅用于第九篇符号推导，本篇不涉及。

 

项目结构树

 

robot_ik_solver_filter/

├── run_ik_filter.py

├── requirements.txt

├── ik_filter/

│ ├── __init__.py

│ ├── models.py

│ ├── joint_limits.py

│ ├── inverse_kinematics.py

│ ├── solution_filter.py

│ ├── forward_kinematics.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_ik_filter.py

└── results/

 

核心源码

 

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

 

"""

4-DOF搬运机器人数据模型

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

"""

 

from dataclasses import dataclass, field

from typing import List, Tuple

import numpy as np

 

 

@dataclass

class JointLimit:

    """

    单关节硬件限位

    min_val: 最小允许值 (rad 或 m)

    max_val: 最大允许值 (rad 或 m)

    """

    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:

        """

        判断关节值是否在限位区间内

        浮点比较使用1e-10容差，避免边界舍入误判

        """

        return (self.min_val - 1e-10) <= value <= (self.max_val + 1e-10)

 

 

@dataclass

class JointLimitSet:

    """

    完整关节限位集合（4-DOF搬运机器人）

    对应θ1, θ2, θ3旋转关节和d4平移关节

    """

    limits: List[JointLimit]

 

    def __post_init__(self):

        if len(self.limits) != 4:

            raise ValueError(f"需要4个关节限位, 实际{len(self.limits)}个")

 

    @classmethod

    def default_4dof_transfer(cls):

        """

        默认4-DOF搬运机器人关节限位

        θ1: ±150° (±2.618 rad)

        θ2: ±110° (±1.9199 rad)

        θ3: ±180° (±3.1416 rad) — 归一化后恒通过，列出为完整性

        d4: ±100 mm (±0.100 m)

        """

        pi = 3.141592653589793

        return cls(limits=[

            JointLimit(-150 * pi / 180, 150 * pi / 180, 'joint1_base_yaw'),

            JointLimit(-110 * pi / 180, 110 * pi / 180, 'joint2_elbow_yaw'),

            JointLimit(-180 * pi / 180, 180 * pi / 180, 'joint3_wrist_roll'),

            JointLimit(-0.100, 0.100, '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 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 SCARAParams:

    """

    4-DOF搬运机器人完整参数（继承第九篇SCARA构型）

    限位筛选不改变构型参数，只增加约束层

    """

    links: List[DHParam]

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

        'joint1_base_yaw', 'joint2_elbow_yaw',

        'joint3_wrist_roll', 'joint4_z_lift'

    ])

 

    @classmethod

    def default_transfer(cls):

        """

        默认搬运机器人D-H参数

        大臂400mm, 小臂320mm, d1=300mm

        """

        pi = 3.141592653589793

        links = [

            DHParam(0.0, 0.300, 0.400, 0.0),

            DHParam(0.0, 0.0, 0.320, 0.0),

            DHParam(0.0, 0.0, 0.0, 0.0),

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

        ]

        return cls(links=links)

 

 

@dataclass

class TargetPose:

    """

    末端目标位姿

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

    phi: 绕基座z轴目标姿态角 (rad)

    """

    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必须为有限数值")

 

"ik_filter/joint_limits.py" — 关节限位管理器

 

"""

关节限位管理器：封装限位检查、角度归一化、超限统计

超限统计使用关节索引计数，避免字符串子串匹配脆弱性

"""

 

import numpy as np

from ik_filter.models import JointLimitSet, JointLimit

 

 

class JointLimitChecker:

    """

    关节限位检查器

    提供：单解筛选、批量筛选、超限统计（按索引）、角度归一化

    """

 

    def __init__(self, limit_set: JointLimitSet):

        self.limit_set = limit_set

 

    @staticmethod

    def normalize_angle(angle_rad: float) -> float:

        """

        将角度归一化到 [-π, π] 区间

        使用arctan2(sin, cos)方法，避免分支判断

        隐含前提：关节是±摆动关节，不是多圈连续转轴

        对SCARA/搬运构型成立；连续转轴机器人不适用此约定

        """

        return np.arctan2(np.sin(angle_rad), np.cos(angle_rad))

 

    def normalize_joint_values(self, joint_values: list) -> list:

        """

        对旋转关节（前3个）做角度归一化

        平移关节（第4个）不做归一化

        """

        normalized = list(joint_values)

        for i in range(min(3, len(normalized))):

            normalized[i] = self.normalize_angle(normalized[i])

        return normalized

 

    def check_single(self, joint_values: list) -> tuple:

        """

        检查单组解的限位合规性

        返回：(是否通过, 超限关节索引列表, 归一化后的关节值)

        使用索引列表而非字符串匹配，调用方按需拼装展示文本

        """

        # 先归一化旋转关节角度

        norm_values = self.normalize_joint_values(joint_values)

        # 逐关节检查（返回索引）

        passed, violation_indices = \

            self.limit_set.check_all_with_indices(norm_values)

        return passed, violation_indices, norm_values

 

    def filter_solutions(self, solutions: list,

                         return_details: bool = False) -> list:

        """

        批量筛选多组逆解

        参数：

            solutions: 第九篇输出的解列表 [[θ1,θ2,θ3,d4], ...]

            return_details: 是否返回详细淘汰原因

        返回：

            有效解列表（若return_details=True，返回[(解, 详情), ...]）

        """

        valid_solutions = []

        rejection_stats = {i: 0 for i in range(4)} # 按关节索引统计淘汰数

 

        for sol in solutions:

            passed, violation_indices, norm_sol = self.check_single(sol)

            if passed:

                if return_details:

                    valid_solutions.append((norm_sol, "PASS"))

                else:

                    valid_solutions.append(norm_sol)

            else:

                if return_details:

                    valid_solutions.append((None, violation_indices))

                # 按索引统计淘汰原因（无字符串匹配，直接索引计数）

                for idx in violation_indices:

                    rejection_stats[idx] += 1

 

        if return_details:

            return valid_solutions, rejection_stats

        return valid_solutions

 

"ik_filter/inverse_kinematics.py" — 逆运动学求解器（复用第九篇逻辑）

 

"""

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

本篇复用第九篇的解析求解逻辑，作为筛选器的上游输入

输出2组数学解（肘部下翻/上翻），不做筛选

"""

 

import numpy as np

from ik_filter.models import SCARAParams, TargetPose

 

 

class InverseKinematicsSCARA:

    """

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

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

    """

 

    def __init__(self, params: SCARAParams):

        self.params = params

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

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

        self.d1 = params.links[0].d # 基座高度 (m)

 

    def solve_all(self, target: TargetPose):

        """

        求解全部数学解（2组：肘部下翻/上翻）

        """

        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 = (r**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)

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

 

        # 肘部下翻分支

        theta2_elbow = -np.arccos(cos_t2)

        phi_angle = np.arctan2(y, x)

        psi_elbow = np.arctan2(self.L2 * np.sin(theta2_elbow),

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

        theta1_elbow = phi_angle - psi_elbow

        theta3_elbow = phi - theta1_elbow - theta2_elbow

 

        # 肘部上翻分支

        theta2_shoulder = np.arccos(cos_t2)

        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

        d4 = z - self.d1

 

        solutions = []

        solutions.append([theta1_elbow, theta2_elbow, theta3_elbow, d4])

        solutions.append([theta1_shoulder, theta2_shoulder, theta3_shoulder, d4])

 

        return solutions

 

"ik_filter/forward_kinematics.py" — 正运动学（二次校验用）

 

"""

正运动学求解器：用于筛选后有效解的二次精度校验

IK求解与FK校验共享同一个params对象，确保状态一致性

"""

 

import numpy as np

from ik_filter.models import SCARAParams

 

 

class ForwardKinematicsSCARA:

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

 

    def __init__(self, params: SCARAParams):

        self.params = params

 

    def compute_pose(self, joint_values):

        """逐级D-H变换，返回末端齐次矩阵"""

        if len(joint_values) != 4:

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

 

        T = np.eye(4)

        for i in range(3):

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

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

 

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

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

 

        return T

 

    @staticmethod

    def _dh_matrix(param):

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

        ct, st = np.cos(param.theta), np.sin(param.theta)

        ca, sa = np.cos(param.alpha), np.sin(param.alpha)

        return np.array([

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

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

            [0, sa, ca, param.d],

            [0, 0, 0, 1]

        ], dtype=float)

 

    def get_position_and_yaw(self, joint_values):

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

        T = self.compute_pose(joint_values)

        x, y, z = T[0, 3], T[1, 3], 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_filter/solution_filter.py" — 解筛选器（核心业务类）

 

"""

逆解筛选器：结合关节限位剔除无效解 + 二次正运动学校验

"""

 

import numpy as np

from ik_filter.models import SCARAParams, TargetPose, JointLimitSet

from ik_filter.joint_limits import JointLimitChecker

from ik_filter.forward_kinematics import ForwardKinematicsSCARA

 

 

class SolutionFilter:

    """

    逆解筛选器：限位检查 + 精度兜底

    设计原则：IK求解与FK校验共享同一个params对象，

    确保筛选过程中关节状态的一致性（第九篇隐患在本篇闭环）

    """

 

    def __init__(self, params: SCARAParams,

                 limit_set: JointLimitSet,

                 fk: ForwardKinematicsSCARA):

        self.params = params

        self.limit_checker = JointLimitChecker(limit_set)

        self.fk = fk

        self.tolerance = 1e-9 # 二次校验容差，实测误差~1e-17，留余量

 

    def filter_and_verify(self, solutions: list,

                          target: TargetPose) -> dict:

        """

        完整筛选流程：限位检查 → 二次正运动学校验 → 输出结果

        返回结构化结果字典

        """

        results = {

            'total_input': len(solutions),

            'valid_solutions': [],

            'rejected_solutions': [],

            'verification_passed': 0,

            'verification_failed': 0,

            'rejection_stats': {i: 0 for i in range(4)}

        }

 

        for idx, sol in enumerate(solutions):

            # 步骤1：限位检查（返回索引列表，无字符串匹配）

            passed, violation_indices, norm_sol = \

                self.limit_checker.check_single(sol)

 

            if not passed:

                results['rejected_solutions'].append({

                    'index': idx,

                    'raw_solution': sol,

                    'normalized': norm_sol,

                    'violation_indices': violation_indices

                })

                # 按索引统计淘汰原因

                for vi in violation_indices:

                    results['rejection_stats'][vi] += 1

                continue

 

            # 步骤2：二次正运动学校验（精度兜底）

            x_calc, y_calc, z_calc, phi_calc = \

                self.fk.get_position_and_yaw(norm_sol)

 

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

                                (y_calc - target.y)**2 +

                                (z_calc - target.z)**2)

            yaw_error = abs(phi_calc - target.phi)

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

 

            if pos_error < self.tolerance and yaw_error < self.tolerance:

                results['verification_passed'] += 1

                results['valid_solutions'].append({

                    'index': idx,

                    'joint_values': norm_sol,

                    'position_error_m': float(pos_error),

                    'yaw_error_rad': float(yaw_error)

                })

            else:

                results['verification_failed'] += 1

                results['rejected_solutions'].append({

                    'index': idx,

                    'raw_solution': sol,

                    'normalized': norm_sol,

                    'violation_indices': [-1] # -1标记校验失败

                })

 

        return results

 

"ik_filter/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_joint_limit_ranges(self, limit_set):

        """

        ① 关节限位范围图

        工程结论：直观展示每个关节的允许运动范围

        """

        fig, ax = plt.subplots(figsize=(10, 6))

 

        joint_names = [lim.joint_name for lim in limit_set.limits]

        y_pos = np.arange(len(joint_names))

 

        for i, lim in enumerate(limit_set.limits):

            if i < 3:

                min_d = np.degrees(lim.min_val)

                max_d = np.degrees(lim.max_val)

                ax.barh(y_pos[i], max_d - min_d, left=min_d,

                         color='steelblue', alpha=0.6, edgecolor='black')

                ax.text(min_d - 5, y_pos[i], f'{min_d:.0f}°',

                         ha='right', va='center', fontsize=8)

                ax.text(max_d + 5, y_pos[i], f'{max_d:.0f}°',

                         ha='left', va='center', fontsize=8)

            else:

                min_m = lim.min_val * 1000

                max_m = lim.max_val * 1000

                ax.barh(y_pos[i], max_m - min_m, left=min_m,

                         color='orange', alpha=0.6, edgecolor='black')

                ax.text(min_m - 10, y_pos[i], f'{min_m:.0f}mm',

                         ha='right', va='center', fontsize=8)

                ax.text(max_m + 10, y_pos[i], f'{max_m:.0f}mm',

                         ha='left', va='center', fontsize=8)

 

        ax.set_yticks(y_pos)

        ax.set_yticklabels(joint_names)

        ax.set_xlabel('Joint Range (° or mm)')

        ax.set_title('Joint Hardware Limit Ranges')

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

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_solution_status(self, filter_results, limit_set):

        """

        ② 解筛选状态图

        工程结论：展示输入解总数、有效解数、各关节淘汰数量

        """

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

 

        # 左图：有效/淘汰/校验失败

        categories = ['Valid', 'Rejected', 'Verify Failed']

        counts = [

            len(filter_results['valid_solutions']),

            len(filter_results['rejected_solutions']) -

            filter_results['verification_failed'],

            filter_results['verification_failed']

        ]

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

        bars1 = ax1.bar(categories, counts, color=colors, alpha=0.8,

                         edgecolor='black')

        ax1.set_ylabel('Number of Solutions')

        ax1.set_title(f'Solution Filter Results '

                       f'(Total Input: {filter_results["total_input"]})')

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

        for bar, count in zip(bars1, counts):

            if count > 0:

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

                          bar.get_height() + 0.05,

                          str(count), ha='center', va='bottom',

                          fontweight='bold')

 

        # 右图：按关节统计淘汰原因（使用索引映射名称）

        joint_labels = ['θ1', 'θ2', 'θ3', 'd4']

        rej_counts = [filter_results['rejection_stats'][i]

                       for i in range(4)]

        bars2 = ax2.bar(joint_labels, rej_counts, color='crimson',

                         alpha=0.8, edgecolor='black')

        ax2.set_ylabel('Rejection Count')

        ax2.set_title('Rejection Reasons by Joint (Index-Based Count)')

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

        for bar, count in zip(bars2, rej_counts):

            if count > 0:

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

                          bar.get_height() + 0.05,

                          str(count), ha='center', va='bottom',

                          fontweight='bold')

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

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