Files

NuoDaJia 5d2a300c85 update doc

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

13 KiB

Raw Blame History

MC-S-003_直线运动（LIN）设计方案

双臂 LIN 运动控制设计文档（当前实现）

版本	日期	作者	描述
v1.0.0	2026/03/03	Ray	初版，基于当前代码实现

1. 概述

机械臂直线运动（LIN）用于要求末端执行器沿笛卡尔空间直线路径到达目标位姿的场景。
当前系统在 DualArm.action 中通过 ArmMotionParams.motion_type = MOVEL 表示 LIN 请求。

当前实现目标：

单臂 LIN：基于 MoveIt computeCartesianPath 生成直线路径；
双臂 LIN：左右臂分别规划后做统一时间轴同步，保证同起同止；
兼容现有执行链路：左右臂 FollowJointTrajectory 下发。

2. 用户故事与 SMART 需求

2.1 用户故事（场景描述）

上层应用需要机械臂末端沿直线路径运动到目标位置。
LIN 运动在笛卡尔空间规划，确保末端轨迹为直线。
姿态可保持固定或做平滑插值。
典型应用包括焊接、涂胶、装配等对路径精度敏感的任务。
系统需支持速度比例可调，并提供参数配置与逆解等服务能力。

2.2 SMART 需求条目

编号	SMART 需求
LIN-SR-01	系统在接收 `MOVEL` 目标后，50ms 内必须完成“成功规划”或“失败返回”；失败时返回结构化错误信息（`code + message + stage`）。
LIN-SR-02	单臂 LIN 规划必须使用笛卡尔直线路径生成机制（`computeCartesianPath`），且路径完整度 `fraction >= 0.999` 才允许进入执行阶段。
LIN-SR-03	系统必须支持两种姿态策略：`FIXED`（姿态保持）与 `SLERP`（四元数球面插值）；每条 LIN 指令必须可选其一。
LIN-SR-04	在 `SLERP` 模式下，姿态轨迹应连续无突变；相邻采样点不得出现翻转跳变（按四元数同向性约束处理）。
LIN-SR-05	执行期末端实际速度不得超过设定笛卡尔速度上限；当超限持续超过设定窗口（建议 100ms）时，系统必须停止并上报。
LIN-SR-06	系统必须支持 `velocity_scaling` 在 `1%~100%` 可调，并保证速度上限按比例缩放，缩放行为在当前任务内生效。
LIN-SR-07	到达目标后不得出现超调振荡；在到位判定窗口内（建议 500ms）末端位置应稳定在允许误差带内。
LIN-SR-08	若直线路径上任一点 IK 无解，规划阶段必须立即失败并返回错误（例如 `IK_NO_SOLUTION_ON_PATH`），不得下发执行轨迹。
LIN-SR-09	若规划起始点处于奇异区，系统必须拒绝规划并返回错误（例如 `START_SINGULARITY`）。
LIN-SR-10	若执行过程中检测到路径进入奇异区，系统必须停止当前任务并返回错误（例如 `EXEC_SINGULARITY_ABORT`）。
LIN-SR-11	双臂 LIN 必须使用统一时间轴；左右臂开始与结束偏差应满足同步阈值（建议 ≤20ms）。
LIN-SR-12	系统必须提供 LIN 参数配置服务与逆解服务，支持在线读取/更新关键参数（具体参数集可后续冻结）。

2.3 验收标准映射

验收项	对应需求
姿态平滑（SLERP，无突变）	LIN-SR-03, LIN-SR-04
速度控制（不超过笛卡尔上限）	LIN-SR-05, LIN-SR-06
位置精度与无超调振荡	LIN-SR-07
规划时间 ≤ 50ms	LIN-SR-01
路径点 IK 不可达返回错误	LIN-SR-08
起始点奇异返回错误	LIN-SR-09
执行中经过奇异点返回错误并停止	LIN-SR-10
速度比例可调	LIN-SR-06
参数配置服务/逆解服务	LIN-SR-12

2.4 状态反馈用户故事（场景描述）

上层应用需要实时了解机械臂运动状态，包括当前位置、速度、运动进度等。
状态反馈通过 ROS 2 话题实时发布，并支持多个订阅者同时订阅。
运动完成后通过 Action Result 通知执行结果和最终状态。
适用于在线监控、任务编排、异常追踪等场景。

2.5 状态反馈 SMART 需求条目

编号	SMART 需求
LIN-FB-01	系统必须通过 ROS 2 话题持续发布运动状态，发布频率不低于 100Hz；消息需包含关节空间与笛卡尔空间状态字段（手臂末端、base、腿末端），以及运动类型、运动状态与进度。
LIN-FB-02	每次 LIN 任务结束（成功/失败/取消）时，系统必须在 Action Result 返回执行结果、错误码和最终状态摘要；Result 发送延迟应小于 100ms（相对任务终止时刻）。
LIN-FB-03	系统必须支持多订阅者并发订阅状态话题（建议基线 10 个），且任一订阅者断开或消费变慢时，发布端持续发布、不阻塞控制链路。

2.6 状态反馈验收标准映射

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

2.7 PTP/LIN 状态反馈统一需求矩阵

维度	PTP（MC-S-002）	LIN（本方案）	统一结论
发布通道	`/robot_control/arm_status` + Action feedback/result	`/robot_control/arm_status` + Action feedback/result	统一
发布频率目标	>=100Hz（目标），当前参数 10~20ms	>=100Hz（目标）	统一为 >=100Hz，按性能分阶段收敛
数据范围	关节空间 + 双臂末端位姿（当前）	关节空间 + 笛卡尔状态（当前重点手臂）	统一扩展到“关节+手臂/base/腿末端”
完成通知	Action Result（success/message/final_progress）	Action Result（success/message/final_progress）	统一错误码语义（后续结构化）
多订阅者鲁棒性	设计要求支持并发与断开不阻塞	设计要求支持并发与断开不阻塞	统一压测基线 10+ 订阅者
时间戳一致性	需与系统时钟对齐	需与系统时钟对齐	统一校验策略

3. 依赖

ROS 2 / rclcpp
MoveIt 2：MoveGroupInterface、computeCartesianPath
ros2_control：JointTrajectoryController
control_msgs/action/FollowJointTrajectory
interfaces/action/DualArm
interfaces/msg/ArmMotionParams

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

4.1 总体架构

flowchart LR
  Client[上层应用<br/>DualArm Action Client] --> DA[DualArmAction]
  DA --> RCM[RobotControlManager]
  RCM --> AC[ArmControl]
  AC --> MGI_L[MoveGroup arm_left]
  AC --> MGI_R[MoveGroup arm_right]
  DA --> TU[trajectory_utils]
  DA --> LFJT[left_arm_controller FJT]
  DA --> RFJT[right_arm_controller FJT]

4.2 分层职责

`DualArmAction`（业务编排层）

LIN 目标校验与执行状态机；
单臂/双臂分支调度；
双臂 LIN 同步重采样调用；
FJT 下发、反馈发布、取消和异常处理。

`RobotControlManager`（领域能力聚合）

提供 PlanArmMotion / 轨迹导出等统一能力；
将 MOVEL 请求路由至 ArmControl::PlanCartesianMotion。

`ArmControl`（MoveIt 适配层）

PlanCartesianMotion 调用 computeCartesianPath；
轨迹时间参数化并写入缓存；
导出单臂轨迹供执行层使用。

`trajectory_utils`（轨迹工具层）

起点时间修正；
双臂时间轴同步重采样；
S 型进度映射；
速度/加速度回填。

5. 配置项设计（LIN 相关）

5.1 同步重采样参数

参数名	默认值	说明
`dual_arm.movel_sync.use_s_curve_progress`	`true`	是否启用 S 型进度映射
`dual_arm.movel_sync.min_dt_sec`	`0.01`	同步轨迹采样的最小时间分辨率
`dual_arm.movel_sync.max_samples`	`500`	同步轨迹最大采样点数

5.2 轨迹执行参数

velocity_scaling：来自 DualArm.goal（1~100 映射为 [0,1]）；
acceleration_scaling：当前执行链路固定使用内部配置值（与 PTP 一致）。

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

6.1 LIN 目标定义

# ArmMotionParams.msg (节选)
uint8 MOVEJ = 0
uint8 MOVEL = 1
uint8 arm_id
uint8 motion_type
geometry_msgs/Pose target_pose
float64[] target_joints

说明：

MOVEL 仅使用 target_pose；
MOVEJ 仅使用 target_joints。

6.2 执行入口

统一通过 DualArm.action 触发；
单臂：arm_motion_params.size() == 1；
双臂：arm_motion_params.size() == 2（需包含 left/right）。

7. LIN 行为与流程设计

7.1 单臂 LIN 规划流程

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

  C->>DA: Goal(单臂, MOVEL)
  DA->>RCM: PlanArmMotion(MOVEL)
  RCM->>AC: PlanCartesianMotion
  AC->>AC: computeCartesianPath(current->target)
  AC-->>RCM: 轨迹缓存
  RCM-->>DA: ExportArmPlannedTrajectory
  DA->>FJT: async_send_goal(traj)
  FJT-->>DA: result
  DA-->>C: Action Result

7.2 双臂 LIN 规划与同步流程

sequenceDiagram
  participant C as Client
  participant DA as DualArmAction
  participant RCM as RobotControlManager
  participant TU as trajectory_utils
  participant LFJT as Left FJT
  participant RFJT as Right FJT

  C->>DA: Goal(双臂, 含MOVEL)
  DA->>RCM: PlanArmMotion(left)
  DA->>RCM: PlanArmMotion(right)
  DA->>RCM: ExportArmPlannedTrajectory(left/right)
  DA->>TU: synchronize_dual_arm_trajectory_timebase
  TU-->>DA: left_sync/right_sync
  DA->>LFJT: send left_sync
  DA->>RFJT: send right_sync
  LFJT-->>DA: result
  RFJT-->>DA: result
  DA-->>C: Action Result

8. 直线性与同步性设计说明

8.1 单臂直线性保证

单臂 MOVEL 直线性由 computeCartesianPath 保证，关键点：

使用当前位姿与目标位姿构造 waypoints；
采用固定 eef_step 离散；
要求 fraction 接近 1（当前阈值为 >= 0.999）。

8.2 双臂同步策略

当前双臂 LIN 不做“联合笛卡尔约束规划”，而是：

左右臂各自直线规划；
统一时间轴重采样；
并发下发 FJT。

该策略可在保持各臂 LIN 路径特征的同时，提高双臂执行同步性。

8.3 平滑性策略

同步重采样时支持：

S 型进度映射（quintic smoothstep）；
重采样后回填 vel/acc；
配合非零起点时间处理，降低起步冲击。

9. 取消、急停与异常处理

9.1 取消

检测到 cancel 请求后：
- 发送减速/保持轨迹；
- 取消当前 FJT goal；
- Action 返回 canceled。

9.2 急停

执行循环内持续检查急停锁存；
触发后立即停止并 abort。

9.3 规划失败与参数非法

无法生成笛卡尔路径；
目标位姿非法（NaN/Inf/四元数异常）；
导出轨迹失败；
控制器不可用或 goal 被拒绝。
均通过 Action Result 的 success=false 与 message 返回。

10. 可观测性与调试支持

Action Feedback：
- status：规划/执行/完成；
- progress：执行进度估算值。
统一状态 topic：
- /robot_control/arm_status 提供关节空间与笛卡尔状态观测；
- 建议与 joint_states、dynamic_joint_states 同步录包做一致性比对。
轨迹文件导出：
- 可保存左右臂下发轨迹（CSV）用于离线分析。
关键日志：
- LIN 规划成功率、同步重采样参数、FJT 接受/执行结果。
- 状态发布频率、Action Result 延迟、多订阅者阻塞告警。

11. 测试设计

11.1 功能测试

左臂/右臂单臂 LIN 到达测试；
双臂 LIN 同步执行测试；
MOVEJ + MOVEL 混合输入测试。

11.2 直线性测试

记录末端实际轨迹并做线段拟合；
评估最大偏离与平均偏离。

11.3 同步性测试

比较左右臂完成时间差；
比较相同归一化时刻的状态一致性。

11.4 稳定性测试

长时间连续 LIN；
随机注入 cancel/急停；
重复启动/停止场景回归。
多订阅者并发 + 慢订阅者/断开场景下的非阻塞验证。

12. 边界与后续优化

12.1 当前边界

双臂 LIN 采用“单臂规划 + 同步重采样”，并非全局联合最优；
时长差异较大时，快臂会被拉伸到慢臂时长；
动态品质仍受控制器参数影响。

12.2 后续优化建议

引入双臂联合笛卡尔约束规划；
增加在线同步偏差监控指标；
将同步重采样参数纳入任务级可配置输入；
增加 LIN 段间 blending 设计。

13 KiB Raw Blame History Unescape Escape