|
|
@@ -0,0 +1,71 @@
|
|
|
+──────────────────────────────────────────────────────────────────────────────────────────
|
|
|
+ 智能轮椅自主回充系统逻辑流程
|
|
|
+──────────────────────────────────────────────────────────────────────────────────────────
|
|
|
+
|
|
|
+ [ 开始 ]
|
|
|
+ │
|
|
|
+ ▼
|
|
|
+ ┌────────────┐ 1. 严重低电量 (Critical Battery)
|
|
|
+ │ READY 状态 │ ────▶ 2. iPad 手动触发 (ipad_phone_interface_start)
|
|
|
+ └────────────┘ 3. 场景服务调用 (SceneControls Service)
|
|
|
+ │
|
|
|
+ ▼
|
|
|
+ ┌──────────────┐
|
|
|
+ │ SEARCHING 状态│ ◀──────────────────┐
|
|
|
+ └──────────────┘ │
|
|
|
+ │ │
|
|
|
+ ├─[ 10s 内检测到基站? ]── 否 ──▶ ┌──────────────┐
|
|
|
+ │ │ ROTATING 状态 │ (原地 0.15rad/s 旋转)
|
|
|
+ │ └──────────────┘
|
|
|
+ │ ┌──────────────────────────┘ │
|
|
|
+ │ │ [ 40s 超时未果? ] ── 是 ──▶ [ 报告错误并返回 READY ]
|
|
|
+ │ └───────────┬──────────────┘
|
|
|
+ │ │
|
|
|
+ 是 是 (检测到基站特征)
|
|
|
+ │ │
|
|
|
+ ▼ ▼
|
|
|
+ ┌────────────────────────────────┐
|
|
|
+ │ WALKING 状态 │ (进入 LidarScanController 闭环控制)
|
|
|
+ └────────────────────────────────┘
|
|
|
+ │
|
|
|
+ ▼
|
|
|
+ ┌──────────────────────────┐
|
|
|
+ │ 激光雷达特征提取与处理 │
|
|
|
+ │ 1. 过滤杂波与聚类 │
|
|
|
+ │ 2. 提取 V 型/直线特征 │
|
|
|
+ │ 3. 计算角平分线 (中线) │
|
|
|
+ └──────────────────────────┘
|
|
|
+ │
|
|
|
+ ▼
|
|
|
+ ┌──────────────────────────┐ [ 姿态校正机制 ]
|
|
|
+ │ 计算误差: dx, dy, yaw │ ──▶ 如果 距离 < 0.7m 且 角度 > 8°:
|
|
|
+ └──────────────────────────┘ 停止前进 -> 原地旋转对准 -> 重新行走
|
|
|
+ │
|
|
|
+ ▼
|
|
|
+ ┌──────────────────────────┐
|
|
|
+ │ 动力输出 (cmd_vel) │
|
|
|
+ │ 线速度: 基于 dx 比例衰减 │
|
|
|
+ │ 角速度: PID 纠偏 (dy+yaw) │
|
|
|
+ └──────────────────────────┘
|
|
|
+ │
|
|
|
+ ├─[ 检测到充电电压 OR 距离 < 0.54m? ]── 否 ──┐
|
|
|
+ │ │
|
|
|
+ 是 │
|
|
|
+ │ ▼
|
|
|
+ ▼ [ 继续执行 WALKING 迭代 ]
|
|
|
+ ┌────────────────┐
|
|
|
+ │ CHARGING 状态 │ ◀─── [ 异常处理: 若对接中基站丢失 -> 触发 base_station_lost ]
|
|
|
+ └────────────────┘ (系统会尝试重新进入 SEARCHING 或报错)
|
|
|
+ │
|
|
|
+ ▼
|
|
|
+ ┌──────────────────────────┐
|
|
|
+ │ 监控电池状态 │
|
|
|
+ │ 1. 充满 (Battery Full) │ ──▶ [ 返回 READY 状态，任务完成 ]
|
|
|
+ │ 2. 物理拔出 (Power Off) │
|
|
|
+ └──────────────────────────┘
|
|
|
+
|
|
|
+──────────────────────────────────────────────────────────────────────────────────────────
|
|
|
+ 核心安全中断 (Global Interrupts)
|
|
|
+ 任何阶段检测到以下信号，立即停止运动并切换回 READY：
|
|
|
+ - 摇杆干预 (Joystick) / 手动推行模式 (Push Start) / 蓝牙断开 / iPad 取消
|
|
|
+──────────────────────────────────────────────────────────────────────────────────────────
|
