Files

NuoDaJia 5d2a300c85 update doc

2026-03-04 14:08:07 +08:00

18 KiB

Raw Blame History

MC-S-002_点到点运动（PTP）设计方案

双臂运动控制设计文档（MoveIt 规划 + Action 下发）

版本	日期	作者	描述
v1.0.0	2026/02/25	Ray	文档新建
v1.0.1	2026/02/28	Ray	完善对外接口部分

1. 概述

机械臂需要快速、平滑地从一个位置运动到另一个位置。点到点运动（PTP）在关节空间进行规划：基于 MoveIt 2 生成轨迹，并通过左右臂各自的 **FollowJointTrajectory** action 下发到 ros2_control 的 JointTrajectoryController 执行。目前支持单臂运动和双臂协同PTP运动类型。

2. 功能需求

在PTP运动规划过程中，系统应生成平滑的运动轨迹，速度曲线，加速度曲线在所有时间点连续，无阶跃。
在PTP运动执行过程中，系统应在轨迹规划失败时返回详细的错误原因，帮助上层应用诊断问题。
在PTP执行过程中，系统应在运动过程中检测到急停或错误时，立即停止运动，返回中断状态。
在PTP执行过程中，系统应在执行运动前检测碰撞风险，发现碰撞风险时拒绝执行，返回碰撞信息。
在PTP运动执行过程中，系统应在执行运动前验证目标位置是否在工作空间内，超出工作空间时返回错误，不执行运动。
在PTP运动规划过程中，系统应在规定时间内完成轨迹规划，确保实时响应，规划时间 ≤ 50ms。
在PTP运动指令规划执行过程中，系统应严格控制实际运动速度，确保不超过设定的速度限制，支持速度比例参数（1%-100%），可动态调整
在PTP运动执行过程中，系统应在运动完成后通过Action Result通知上层应用，包含最终位置和状态信息，运动执行结果：SUCCESS（成功）、FAILED（失败）、CANCELLED（取消）。
在PTP运动规划中，系统应确保机器人到达目标位置时无振荡，到达目标后，位置波动范围 ≤ ±0.05mm。

2.1 状态反馈 SMART 需求（PTP）

编号	SMART 需求
PTP-FB-01	系统必须通过 ROS 2 话题持续发布运动状态，发布频率不低于 100Hz（目标值）；当前实现基线为 `robot_arm_status_period_ms=10~20ms`（50~100Hz），需通过参数与性能优化收敛到 100Hz 稳态。
PTP-FB-02	每次 PTP 任务结束（成功/失败/取消）时，必须通过 Action Result 返回 `success/final_progress/message`，并补充统一错误码语义（当前为 message 文本，后续结构化）。
PTP-FB-03	状态信息必须具备完整性：关节空间（名称/位置/使能/错误码）+ 双臂末端笛卡尔位姿；base 与腿末端笛卡尔状态作为扩展字段进入后续版本。
PTP-FB-04	状态消息必须携带准确时间戳并与系统时钟同步；跨 topic 对齐误差超过阈值时需上报诊断信息。
PTP-FB-05	状态话题需支持多订阅者并发消费（建议基线 10 个）；订阅者异常断开或慢消费不得阻塞发布与运动执行链路。

2.2 状态反馈验收映射（PTP）

验收项	对应需求
实时状态：发布频率 ≥100Hz	PTP-FB-01
完成通知：Result 含成功/失败与错误信息	PTP-FB-02
数据完整性：关节空间 + 笛卡尔空间	PTP-FB-03
时间戳准确：与系统时钟同步	PTP-FB-04
多订阅者支持与断开不阻塞	PTP-FB-05

3. 依赖

ROS 2 / rclcpp
MoveIt 2：move_group、MoveGroupInterface
ros2_control：controller_manager、JointTrajectoryController
控制接口：FollowJointTrajectory action

4. 内部结构设计（按当前代码）

4.1 总体架构

flowchart LR
  subgraph App[上层应用]
    A1[DualArm Action Client]
    A2["Service Client<br/>IK/Enable/ClearError"]
    A3[Status Subscriber]
  end

  subgraph RC[robot_control]
    N[robot_control_node]
    AM[ActionManager]
    DA[DualArmAction]
    RCM[RobotControlManager]
    AC[ArmControl]
    KS[KinematicsService]
    AHS[ArmHardwareService]
    ASS[ArmStatusService]
  end

  subgraph MoveIt[MoveIt 2]
    MGI["MoveGroupInterface<br/>arm_left/arm_right/dual_arm"]
    RS[RobotState IK/FK]
  end

  subgraph Ctrl[ros2_control]
    L[left_arm_controller]
    R[right_arm_controller]
    G1[left_arm_gpio_controller]
    G2[right_arm_gpio_controller]
  end

  A1 --> DA
  A2 --> KS
  A2 --> AHS
  A3 --> ASS

  N --> AM --> DA
  DA --> RCM --> AC --> MGI
  KS --> RCM --> AC --> RS
  AHS --> G1
  AHS --> G2
  ASS --> RCM
  DA --> L
  DA --> R

4.2 分层职责

`robot_control_node`（编排层）

负责节点生命周期、订阅/发布器/service/action server 的创建与绑定。
保持“薄节点”原则，不承载具体业务算法。
启动时激活左右臂控制器并初始化 MoveIt。

`ActionManager` + `DualArmAction`（PTP主链路）

对外提供 DualArm action。
负责单臂/双臂 PTP 目标校验、规划调用、轨迹导出、拆分与并发下发。
负责执行期反馈、取消处理和结果回传。

`RobotControlManager`（领域能力聚合）

聚合运动学、状态映射、控制器状态等核心能力。
提供 PlanArmMotion / PlanDualArmJointMotion、轨迹导出、关节状态查询等统一接口。
提供 IK 与 FK 路径：
- IK：调用 ArmControl::SolveInverseKinematics（纯 IK，setFromIK）
- FK：使用当前 jointPositions_ 计算末端位姿。

`ArmControl`（MoveIt 适配层）

管理 arm_left / arm_right / dual_arm 的 MoveGroupInterface。
轨迹规划、时间参数化、轨迹缓存导出。
纯 IK 与基于给定关节值的 FK 计算。

新增服务层（`services/`）

KinematicsService：对外逆解服务入口，负责请求/响应封装与错误语义。
ArmHardwareService：对外使能与清错服务，负责 GPIO 命令下发和缓存更新。
ArmStatusService：解析 DynamicJointState，定时发布 RobotArmStatus。

结论：第4版设计从“节点内聚”调整为“节点编排 + 业务服务分层”，便于维护与扩展。

5. 配置项设计

5.1 控制器与MoveIt配置

left_arm_controller、right_arm_controller：6轴 JointTrajectoryController。
controller_manager/switch_controller：启动时由节点激活。
MoveIt 使用三个 group：arm_left、arm_right、dual_arm。

5.2 关键运行参数

robot_arm_status_period_ms：状态发布周期，范围 [10, 20] ms，默认 20 ms。
velocity_scaling：Action 目标速度比例（兼容 1~100 输入语义）。
状态反馈目标频率：>=100Hz（建议将周期参数收敛到 10ms 并验证 CPU 余量）。

5.3 主题与服务约定

GPIO命令：
- /left_arm_gpio_controller/commands
- /right_arm_gpio_controller/commands
状态输入：
- /joint_states
- /dynamic_joint_states
状态输出：
- /robot_control/arm_status
对外服务：
- /robot_control/inverse_kinematics
- /robot_control/set_arm_enable
- /robot_control/clear_arm_error
- /robot_control/reset_estop

6. 接口定义（当前实现）

6.1 PTP执行接口（Action）

interfaces/action/DualArm（当前代码）

# DualArm.action 

# 状态常量定义
uint8 STATUS_PLANNING = 0   # 状态: 规划中
uint8 STATUS_EXECUTING = 1  # 状态: 执行中
uint8 STATUS_DONE = 2       # 状态: 完成

# Action Goal: 双臂控制目标
interfaces/ArmMotionParams[] arm_motion_params   # 每条手臂的运动参数 (包含目标位姿、轨迹等)
int32 velocity_scaling               # 速度百分比系数  [1, 100] (100表示全速)

---

# Action Result: 执行结果
bool success                         # 执行是否成功 (true=成功, false=失败)
string message                       # 结果描述信息
float64 final_progress               # 最终进度  [0.0, 1.0]

---

# Action Feedback: 实时反馈
float64 progress                     # 实时总体进度  [0.0, 1.0]
uint8 status                         # 实时状态 (0:规划中, 1:执行中, 2:完成)
float64[] joints_left                # 左臂当前关节角 [单位: 弧度]
float64[] joints_right               # 右臂当前关节角 [单位: 弧度]

6.2 运动学与硬件服务

interfaces/srv/InverseKinematics（当前代码）

int32 arm_id                    #0-左臂，1-右臂
geometry_msgs/Pose pose         #目标位姿
---
int32 result                    #结果：0-成功，非0-失败
float32[] joint_angles          #解算出来的关节角

interfaces/srv/SetArmEnable（当前代码）

# SetArmEnable.srv
# 机械臂关节上下使能服务
#
# 用途：
# - 对单关节进行使能/失能
# - 对单臂全部关节进行使能/失能
# - 对双臂全部关节（或同编号关节）进行使能/失能
#
# 参数组合说明：
# - arm_id=0, joint_num=0: 左臂全部关节
# - arm_id=1, joint_num=0: 右臂全部关节
# - arm_id=2, joint_num=0: 双臂全部关节
# - arm_id=0/1, joint_num=1~6: 指定单臂的指定关节
# - arm_id=2, joint_num=1~6: 双臂的同编号关节（如左右第3关节）
#
# 注意：
# - joint_num=0 表示“该作用范围内的全部关节”
# - 关节编号按 1 开始计数

int8 arm_id     # 机械臂ID: 0=左臂, 1=右臂, 2=双臂
int8 joint_num  # 关节编号: 1~6=指定关节, 0=全部关节
bool enable     # 目标状态: true=使能, false=失能
---
bool success    # true=操作成功, false=操作失败
string message  # 执行结果描述（失败原因/调试信息）

interfaces/srv/ClearArmError（当前代码）

# ClearArmError.srv
# 清除机械臂错误服务

int8 arm_id     # 机械臂ID (例如: 0=左臂, 1=右臂, 2=双臂)
int8 joint_num  # 关节编号 (1~7表示第1个到第7个关节, 0表示所有关节)
---
bool success    # 操作是否成功
string message  # 结果信息

interfaces/srv/ResetEStop（当前代码）

# ResetEStop.srv
# 急停复位服务（用于清除上层软件锁存态）
#
# 注意：
# - 该服务仅表达“复位请求”，是否允许复位由安全链路策略决定
# - 硬件安全回路未恢复时，应返回 success=false

bool reset        # true=请求复位，false=无效请求
---
bool success      # true=复位成功，false=复位失败
string message    # 结果描述（失败原因/诊断信息）

6.3 状态发布接口

interfaces/msg/RobotArmStatus（topic: /robot_control/arm_status，当前代码）

builtin_interfaces/Time stamp
string[] joint_names
float64[] joint_positions
bool[] joint_enabled
int32[] joint_error_codes
bool estop_latched
uint32 estop_generation
geometry_msgs/Pose left_arm_pose
geometry_msgs/Pose right_arm_pose

末端位姿采用 jointPositions_ + FK 计算，确保与同帧关节值一致。当前版本对外稳定字段为“关节空间 + 双臂末端位姿”；base 与腿末端笛卡尔状态定义为下一版本扩展项，以保持接口兼容演进。

7. 行为与流程设计

7.1 单臂 PTP 主流程

sequenceDiagram
  participant C as Client
  participant DA as DualArmAction
  participant RCM as RobotControlManager
  participant AC as ArmControl
  participant FJT as JointTrajectoryController

  C->>DA: Goal(单臂MOVEJ)
  DA->>DA: 参数/状态校验
  DA->>RCM: PlanArmMotion()
  RCM->>AC: PlanJointMotion()
  AC-->>RCM: 规划轨迹缓存
  DA->>RCM: ExportArmPlannedTrajectory()
  DA->>FJT: async_send_goal()
  FJT-->>DA: result/feedback
  DA-->>C: result

7.2 双臂 PTP 主流程

sequenceDiagram
  participant C as Client
  participant DA as DualArmAction
  participant RCM as RobotControlManager
  participant AC as ArmControl(dual_arm)
  participant L as left FJT
  participant R as right FJT

  C->>DA: Goal(双臂)
  DA->>RCM: PlanDualArmJointMotion()
  RCM->>AC: PlanJointMotion(arm_id=2)
  AC-->>RCM: 12轴轨迹
  DA->>RCM: ExportDualArmPlannedTrajectory()
  DA->>DA: 拆分左右轨迹
  par left
    DA->>L: send goal
  and right
    DA->>R: send goal
  end
  L-->>DA: result
  R-->>DA: result
  DA-->>C: result

7.3 服务链路流程

InverseKinematics：KinematicsService -> RobotControlManager -> ArmControl(setFromIK)
SetArmEnable/ClearArmError：ArmHardwareService -> GPIO controllers
RobotArmStatus：ArmStatusService 定时汇总发布（状态输入来自 joint_states + dynamic_joint_states）

8. 时序与性能设计

8.1 PTP链路性能目标

规划阶段：以“稳定优先”，不再文档强约束固定 50ms。
执行反馈：依赖 action feedback 周期与控制器状态更新频率。
状态发布：10~20ms 周期（参数可调，默认20ms）。

8.2 时序一致性策略

关节位置：来自 joint_states 映射后的 jointPositions_。
末端位姿：基于同一时刻 jointPositions_ 进行 FK。
使能/错误：优先由 dynamic_joint_states 更新，服务操作会同步更新缓存。

8.3 取消与停止

取消沿用 DualArmAction 既有策略（含软停逻辑，按当前实现执行）。
硬急停状态机作为后续增强项，不在本次服务化重构内改变执行协议。

8.4 急停分层链路与状态机

分层链路（与当前代码对齐）

flowchart LR
  IO[外部急停IO] --> HAL[HAL安全输入]
  HAL --> Driver[驱动层安全状态]
  Driver --> Topic["/safety/estop_state"]
  Topic --> SS[SafetyService]
  SS --> DA[DualArmAction]
  SS --> ASS[ArmStatusService]
  DA --> Stop1["Stop1减速停车<br/>MakeStopTrajectory + cancel"]
  ASS --> Status["/robot_control/arm_status<br/>estop_latched/generation"]

SafetyService 订阅 /safety/estop_state，维护 latched/generation 原子状态。
SafetyService 提供 /robot_control/reset_estop：
- 必须已收到至少一次 /safety/estop_state（完成驱动侧状态同步）；
- 且当前 latched=false（硬件链路已释放）；
- 否则返回 success=false，拒绝复位请求。
DualArmAction 在 接收目标前 与 执行循环中 检查急停锁存：
- 锁存态下拒绝新目标；
- 执行中触发时，执行 Stop1 风格减速停车并返回中断结果（E_STOP_STOP1）。
ArmStatusService 将 estop_latched/estop_generation 合并进 RobotArmStatus，便于上层统一观测执行态与安全态。

急停状态机（软件视角）

stateDiagram-v2
  [*] --> RUNNING
  RUNNING --> E_STOP_LATCHED: estop_latched=true
  E_STOP_LATCHED --> STOPPING: 执行中检测到急停
  STOPPING --> ABORTED: Stop1完成/Action中止
  E_STOP_LATCHED --> REJECT_NEW_GOAL: 新目标到达
  REJECT_NEW_GOAL --> E_STOP_LATCHED
  ABORTED --> WAIT_RESET: 等待安全链路复位
  WAIT_RESET --> RUNNING: latched=false 且允许复位

generation 用于区分“同一锁存态”与“新触发事件”，上层可据此做边沿判定与告警去重。
ResetEStop 服务仅作为复位请求入口，复位成功前提是“驱动状态已同步 + 锁存已释放”，不允许绕过硬件安全链路。

9. 错误处理与恢复策略

9.1 错误分类

请求参数错误：arm_id/joint_num 越界、目标维度不匹配。
状态错误：未初始化完成、控制器不可用、资源冲突。
规划错误：MoveIt 无解/规划失败。
执行错误：FJT 执行失败、超时、被取消。

9.2 上报语义

Action：result.success=false + message。
Service：success/result + message 明确失败原因。
日志：在 service/action 层保留关键错误日志，便于定位。

9.3 恢复策略

参数错误：立即失败返回，不进入执行链路。
规划失败：保持控制器状态不变，允许重试。
执行失败：由上层决定重试、回零或清错。
清错与使能：通过 service 明确触发，避免隐式副作用。

10. 可观测性与调试支持

统一状态topic：/robot_control/arm_status 作为运维与调试主观测口。
建议录包：joint_states、dynamic_joint_states、robot_control/arm_status、Action feedback/result。
调试逆解：调用 /robot_control/inverse_kinematics 并与 FK结果做闭环比对。
建议新增监控项：状态发布时间抖动、Action Result 延迟、订阅者数量与发布阻塞告警。

11. 安全性与可靠性

11.1 安全性

关节限位在规划入口前校验。
使能/清错接口显式化，避免业务层绕过硬件控制语义。
双臂/单臂关节范围通过 arm_id + joint_num 严格限定。

11.2 可靠性

节点瘦身 + 服务分层后，修改影响面更小。
状态发布与执行链路解耦，降低互相阻塞风险。
IK 与 FK 语义清晰，避免“规划末点冒充逆解”的歧义。

12. 测试设计

12.1 功能测试

单臂/双臂 PTP 成功路径（MOVEJ）。
SetArmEnable：
- arm_id=0/1/2
- joint_num=0（全部）与 1~6（单关节）
ClearArmError：单关节、单臂全部、双臂全部。
InverseKinematics：左/右臂典型位姿、不可达位姿。

12.2 一致性测试

校验 RobotArmStatus.joint_positions 与实际 joint_states 对齐。
校验 RobotArmStatus 中 pose 是否与同帧关节值 FK 一致。
校验 service 调用后使能/错误缓存刷新行为。

12.3 稳定性测试

连续PTP请求压力（含取消）。
状态topic长时间发布（10ms 周期）稳定性与CPU占用。
控制器重启后的恢复行为（switch_controller + service/action 重新可用）。
多订阅者压力（建议 10+ 并发订阅）与慢订阅者/断开场景不阻塞验证。

13. 变更影响分析

架构层面：从“node内聚实现”升级为“service分层实现”，可维护性提升。
兼容性：对外 action/service/topic 名称保持不变，调用方无感。
性能影响：新增服务对象几乎无额外开销；状态发布逻辑迁移不改变频率与数据结构。
风险点：需关注缓存一致性（dynamic state 与 service写入并存场景）；建议通过录包回归验证。

18 KiB Raw Blame History Unescape Escape