割草机运动控制器状态机

概览

该状态机包含 3 个主阶段（GOTO_START, FOLLOW_PATH, FINISHED），以及多个子状态和模式标志。

完整状态机图（分层结构）

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 子状态

关键参数：
- 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）

拐点检测条件：
- 原始路径点前后方向变化 > 45° (pi/4)
- 未在 waypoint_turned 集合中（避免重复）
- 按路径顺序依次处理（不跳过）

恢复模式（Recovery Mode）

终点接近

| Goal Tolerance | < goal_tolerance (0.3m) | 停止 | - |

扩展完成条件：
- final_dist < goal_tolerance * 1.5 (0.45m)
- current_target_idx >= 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)

关键数值速查表

状态机实现建议（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 行)