# 割草机运动控制器状态机 ## 概览 该状态机包含 3 个主阶段（GOTO_START, FOLLOW_PATH, FINISHED），以及多个子状态和模式标志。 --- ## 完整状态机图（分层结构） ```mermaid stateDiagram-v2 [*] --> GOTO_START: 初始化 state GOTO_START { [*] --> Rotate Rotate --> Move: abs(heading_err) <= start_turn_first_heading Move --> Approach: dist_to_start < start_approach_dist Move --> Align: dist_to_start < start_tolerance Approach --> Align: dist_to_start < start_tolerance Align --> [*]: dist_to_start < start_tolerance AND\nabs(heading_err_final) < start_heading_tolerance note right of Rotate 原地转向面向起点 forward=0, turn=yawrate 条件：heading_err > start_turn_first_heading end note note right of Move 面向起点前进 forward=scaled, turn=correction 条件：dist_to_start >= start_tolerance end note note right of Approach 减速接近起点 speed = slow_speed * dist_ratio 条件：dist < start_approach_dist end note note right of Align 原地对齐路径起始航向 forward=0, turn=yawrate 条件：到达起点但航向不对 end note } GOTO_START --> FOLLOW_PATH: 到达起点且航向对齐 state FOLLOW_PATH { [*] --> NormalFollow NormalFollow --> WaypointApproach: 检测到未转向拐点\n且 dist < 2.0m NormalFollow --> Recovery: abs(xte) > recovery_dist (2.0m) state WaypointApproach { [*] --> ApproachingCorner ApproachingCorner --> [*]: dist_to_waypoint < waypoint_turn_tolerance (0.12m) note right of ApproachingCorner 直接朝向拐点前进（忽略路径） 减速：dist<0.5m时逐渐降速 forward=scaled, turn=to_waypoint end note } WaypointApproach --> WaypointTurn: 到达拐点位置 state WaypointTurn { [*] --> TurningInPlace TurningInPlace --> [*]: abs(heading_err) < waypoint_turn_heading_tol (0.1rad) note right of TurningInPlace 原地转向到下一段路径方向 forward=0, turn=yawrate 完成后标记拐点已转向 end note } WaypointTurn --> NormalFollow: 转向完成 state Recovery { [*] --> RecoveringToPath RecoveringToPath --> [*]: abs(xte) < recovery_exit_dist (0.8m) note right of RecoveringToPath 大偏离恢复模式 朝向路径投影点前进 speed = recovery_speed (0.55 m/s) end note } Recovery --> NormalFollow: 回到路径附近 state NormalFollow { [*] --> PurePursuit PurePursuit --> SlowZone: final_dist < slow_zone (1.5m) SlowZone --> FinalApproach: final_dist < final_approach_dist (0.8m) FinalApproach --> PurePursuit: final_dist >= final_approach_dist note right of PurePursuit 标准纯追踪控制 lookahead = f(speed, xte) 速度 = base_speed * scale(曲率,航向误差,xte) 转向 = PID(heading_err) + xte_correction end note note right of SlowZone 接近终点减速区 speed *= dist_scale end note note right of FinalApproach 终点精确接近 speed *= 0.6 增加航向/xte精度要求 end note } NormalFollow --> [*]: final_dist < goal_tolerance (0.3m) } FOLLOW_PATH --> FINISHED: 到达终点 state FINISHED { [*] --> Stopped note right of Stopped forward=0, turn=0 任务完成 end note } FINISHED --> [*] ``` --- ## 主要状态与参数 ### 顶层阶段 | 状态 | 说明 | 进入条件 | 退出条件 | |------|------|----------|----------| | **GOTO_START** | 从任意位置移动到路径起点 | 初始化 | 到达起点且航向对齐 | | **FOLLOW_PATH** | 跟随路径行驶 | 完成 GOTO_START | 到达终点 | | **FINISHED** | 停止 | 到达终点 | 无 | ### GOTO_START 子状态 | 子状态 | 说明 | 控制输出 | 进入条件 | 退出条件 | |--------|------|----------|----------|----------| | **Rotate** | 原地转向面向起点 | forward=0, turn=k*heading_err | 初始/航向偏差大 | `abs(heading_err) <= start_turn_first_heading` (~15°) | | **Move** | 朝向起点前进 | forward>0, turn=correction | 完成 Rotate | `dist_to_start < start_tolerance` (0.12m) | | **Approach** | 减速接近起点 | forward=slow_speed*scale | `dist < start_approach_dist` (1.5m) | 到达起点容差内 | | **Align** | 原地对齐路径起始航向 | forward=0, turn=k*heading_err_final | 到达位置但航向不对 | `abs(heading_err_final) < start_heading_tolerance` (0.01rad) | **关键参数：** - `start_tolerance = 0.12m` - 位置到达容差 - `start_heading_tolerance = 0.01rad` (~0.57°) - 航向对齐容差 - `start_approach_dist = 1.5m` - 开始减速距离 - `start_turn_first_heading = pi/50` (~3.6°) - 先转向再前进的航向阈值 ### FOLLOW_PATH 模式 #### 标准跟踪（Normal Follow） - **Pure Pursuit 前视点选择** - 动态 lookahead：`lookahead_min + lookahead_time * speed - 0.3 * xte` - 范围：[0.35m, 1.5m] - **速度调度** - 基础速度：`base_speed = 0.5 m/s` - 缩放因子： - 曲率：`1 / (1 + abs(curvature) * 2.0)` - 航向误差：`1 / (1 + abs(heading_err) * 1.5)` - 横向误差：`1 / (1 + abs(xte) * 1.0)` - 最终速度：`target_speed = base_speed * 混合缩放` - **转向控制（PID + 横向修正）** - P: `k_heading * heading_err` (k=1.8) - D: `k_heading_d * heading_err_d` (k=0.25) - I: `k_heading_i * heading_err_sum` (k=0.01) - XTE修正: `-k_xte * xte_normalized` (k=1.2) #### 拐点模式（Waypoint Mode） | 模式 | 说明 | 触发条件 | 控制策略 | 退出条件 | |------|------|----------|----------|----------| | **WaypointApproach** | 接近拐点 | 检测到未转向拐点 & dist<2m | 直接朝向拐点前进（忽略路径），dist<0.5m时减速 | `dist < waypoint_turn_tolerance` (0.12m) | | **WaypointTurn** | 原地转向 | 到达拐点位置 | forward=0, turn=k*heading_err | `abs(heading_err) < waypoint_turn_heading_tol` (0.1rad ~6°) | **拐点检测条件：** - 原始路径点前后方向变化 > 45° (pi/4) - 未在 `waypoint_turned` 集合中（避免重复） - 按路径顺序依次处理（不跳过） #### 恢复模式（Recovery Mode） | 属性 | 值/说明 | |------|---------| | **触发条件** | `abs(xte) > recovery_dist` (2.0m) | | **退出条件** | `abs(xte) < recovery_exit_dist` (0.8m) | | **控制策略** | 朝向路径投影点前进，固定速度 `recovery_speed = 0.55 m/s` | | **优先级** | 覆盖所有其他速度调度逻辑 | #### 终点接近 | 区域 | 距离阈值 | 速度调整 | 精度要求 | |------|----------|----------|----------| | **Slow Zone** | `< slow_zone` (1.5m) | `speed *= dist_scale` (0.3~1.0) | 标准 | | **Final Approach** | `< final_approach_dist` (0.8m) | `speed *= 0.6`, 再乘 dist_scale | 航向= n - 2` - `abs(heading_err) < 0.3rad` - `abs(xte) < 0.2m` --- ## 并发标志与特殊逻辑 ### 状态标志 - `stage`: GOTO_START / FOLLOW_PATH / FINISHED - `goto_substage`: rotate / move / align（仅在 GOTO_START 有效） - `waypoint_approach_mode`: bool（拐点接近模式） - `waypoint_turn_mode`: bool（拐点转向模式） - `in_recovery`: bool（恢复模式） - `finished`: bool（任务完成） ### 优先级（从高到低） 1. **finished** → 立即返回 forward=0, turn=0 2. **GOTO_START** → 按子状态执行（rotate/move/align） 3. **waypoint_turn_mode** → 原地转向（覆盖其他控制） 4. **waypoint_approach_mode** → 直接朝向拐点 5. **in_recovery** → 恢复模式（覆盖速度调度） 6. **Normal Follow** → 标准纯追踪 ### 安全与约束 - **最小前进速度**：`min_follow_speed = 0.08 m/s`（防止静止） - **加速度限制**：`accel = 0.2 m/s²`（软启动） - **倒车条件**：`abs(heading_err) > 2.9rad` (~166°) - **错误恢复**：连续大误差（xte>0.75m 或 heading_err>pi/2.5）超过 10 次 → 重置积分项 --- ## 控制输出映射 ``` forward_signal = int(round((current_speed / max_forward_mps) * 100)) turn_signal = int(round((yawrate_cmd / max_yawrate) * 100)) 范围：[-100, 100] - forward: -100 → -0.2 m/s (倒车), 0 → 静止, +100 → +0.6 m/s - turn: -100 → -pi/4 rad/s (右转), 0 → 直行, +100 → +pi/4 rad/s (左转) ``` --- ## 典型执行流程示例 ### 场景1：从远处到达起点并开始作业 ``` [*] → GOTO_START(Rotate) → 原地转向面向起点 (heading_err 从 120° 降到 3°) → GOTO_START(Move) → 前进接近起点 (dist 从 5m 降到 1.5m) → GOTO_START(Approach) → 减速接近 (dist 从 1.5m 降到 0.1m) → GOTO_START(Align) → 原地调整航向对齐路径方向 (heading_err_final 从 8° 降到 0.5°) → FOLLOW_PATH(NormalFollow/PurePursuit) → 标准纯追踪跟随路径 ``` ### 场景2：路径跟随中遇到拐点 ``` FOLLOW_PATH(NormalFollow) → 检测到前方 2m 处有拐点（方向变化 60°） → WaypointApproach → 直接朝向拐点前进，逐渐减速 (dist 从 2m 降到 0.1m) → WaypointTurn → 原地转向到下一段方向 (heading_err 从 60° 降到 5°) → 标记拐点已转向，回到 NormalFollow ``` ### 场景3：大幅偏离路径 ``` FOLLOW_PATH(NormalFollow) → 横向误差 xte 增大到 2.5m（触发恢复） → Recovery → 朝向路径投影点，固定速度 0.55 m/s (xte 从 2.5m 降到 0.7m) → 回到 NormalFollow ``` ### 场景4：接近并到达终点 ``` FOLLOW_PATH(NormalFollow/PurePursuit) → final_dist < 1.5m → SlowZone（速度降到 0.3~0.5 m/s） → final_dist < 0.8m → FinalApproach（速度进一步降到 0.2 m/s） → final_dist < 0.3m → FINISHED(Stopped) ``` --- ## 关键数值速查表 | 参数 | 默认值 | 说明 | |------|--------|------| | `max_forward_mps` | 0.6 | 最大前进速度 (m/s) | | `max_reverse_mps` | 0.2 | 最大倒车速度 (m/s) | | `max_yawrate` | pi/4 | 最大偏航角速度 (rad/s) | | `base_speed` | 0.5 | 基础速度 (m/s) | | `k_heading` | 1.8 | 航向误差比例增益 | | `k_heading_d` | 0.25 | 航向误差微分增益 | | `k_xte` | 1.2 | 横向误差修正增益 | | `lookahead_min` | 0.35 | 最小前视距离 (m) | | `lookahead_max` | 1.5 | 最大前视距离 (m) | | `lookahead_time` | 2.0 | 前视时间系数 (s) | | `goal_tolerance` | 0.3 | 终点到达容差 (m) | | `start_tolerance` | 0.12 | 起点到达容差 (m) | | `waypoint_turn_tolerance` | 0.12 | 拐点到达容差 (m) | | `recovery_dist` | 2.0 | 触发恢复模式距离 (m) | | `recovery_exit_dist` | 0.8 | 退出恢复模式距离 (m) | --- ## 状态机实现建议（C移植） ```c // 主状态枚举 typedef enum { STAGE_GOTO_START, STAGE_FOLLOW_PATH, STAGE_FINISHED } MowerStage; // GOTO_START 子状态 typedef enum { GOTO_SUBSTAGE_ROTATE, GOTO_SUBSTAGE_MOVE, GOTO_SUBSTAGE_ALIGN } GotoSubstage; // FOLLOW_PATH 模式标志 typedef struct { bool waypoint_approach_mode; bool waypoint_turn_mode; bool in_recovery; } FollowModeFlags; // 控制器状态 typedef struct { MowerStage stage; GotoSubstage goto_substage; FollowModeFlags follow_flags; uint16_t current_target_idx; uint16_t waypoint_turn_target_idx; float current_speed; bool finished; } MowerControlState; // 状态转移函数原型 void update_goto_start_state(MowerControlState *state, float dist, float heading_err); void update_follow_path_state(MowerControlState *state, float xte, float waypoint_dist); void compute_goto_start_control(const MowerControlState *state, int16_t *forward, int16_t *turn); void compute_follow_path_control(const MowerControlState *state, int16_t *forward, int16_t *turn); ``` --- ## 注意事项与已知问题 1. **重复定义**：`_find_nearest_original_waypoint` 函数出现两次，后者覆盖前者 2. **日志频繁**：74Hz 写文件 + flush，嵌入式需改为缓冲或减少频率 3. **参数注释不符**：`start_heading_tolerance = 0.01` 注释说 ~14°，实际 ~0.57° 4. **多级速度缩放**：难以调参，建议简化为统一公式 5. **拐点检测无距离过滤**：锯齿路径可能频繁触发，建议加入"前后边长 > min_seg_len" 6. **恢复模式触发可能过晚**：lookahead 前视可能使车继续偏离，建议提前触发 7. **终点完成条件重复**：两个分支可能同时满足，建议统一 8. **倒车逻辑**：基于 heading_err > 166° 可能导致不必要倒车，建议结合距离判断 --- 生成时间：2025-11-13 基于：`mower_controller.py` (约 1200 行)