草坪割草机闭环控制系统

系统架构

┌─────────────────────────────────────────────────────────────┐
│                        STM32H7 MCU                           │
│                                                              │
│  GPS/IMU (UART2)  →  解析  →  PythonLink  →  UART5         │
│       10Hz/100Hz         ↓                     ↓ 921600bps  │
│                      数据打包              发送GPS/IMU       │
│                                                              │
│  UART5接收  ←  PythonLink  ←  控制命令解析                   │
│      ↓                                                       │
│  SBUS模块 (CH8检测)                                          │
│      ↓                                                       │
│  CH8 > 1500? ─YES→ 自动模式 → 应用Python命令 → PWM输出      │
│      ↓ NO                                                    │
│  手动模式 → 应用SBUS遥控器 → PWM输出                         │
└─────────────────────────────────────────────────────────────┘
                              ↕ UART5 (921600bps)
┌─────────────────────────────────────────────────────────────┐
│                      Python 控制程序                          │
│                                                              │
│  接收GPS/IMU  →  坐标转换  →  MowerController               │
│                                  ↓                           │
│                          计算控制信号                         │
│                          (前进, 转向)                         │
│                                  ↓                           │
│                      转换为PWM值 (1000-2000us)               │
│                                  ↓                           │
│                          发送控制命令                         │
└─────────────────────────────────────────────────────────────┘

通信协议

1. STM32 → Python (GPS/IMU数据)

帧格式:
AA 55 TYPE LEN(2) PAYLOAD CHECKSUM(2) 0D 0A

GPS数据包 (Type=0x01, 56字节):
- 纬度/经度 (double, 8字节 x2)
- 航向角 (float, 4字节)
- 东/北/天速度 (float, 4字节 x3)
- 高程 (float, 4字节)
- UTC时间 (uint32, 4字节)
- 定位质量/卫星数 (uint8, 1字节 x2)
- 对齐保留 (2字节)

IMU数据包 (Type=0x02, 32字节):
- XYZ加速度 (float, 4字节 x3)
- XYZ角速度 (float, 4字节 x3)
- 温度 (float, 4字节)
- UTC时间 (uint32, 4字节)

校验和: 16位累加和 (sum(所有字节) & 0xFFFF)

频率:
- GPS: 10Hz
- IMU: 100Hz

2. Python → STM32 (控制命令)

帧格式:
AA 55 10 04 00 STEERING(2) THROTTLE(2) CHECKSUM(2) 0D 0A

控制数据包 (Type=0x10, 4字节):
- 转向PWM值 (uint16, 1000-2000us)
- 油门PWM值 (uint16, 1000-2000us)

校验和: 16位累加和

频率: 74Hz (控制周期)

超时保护: 1秒无命令则恢复中位值(1500us)

模式切换 (SBUS CH8)

手动模式 (CH8 < 1500):
- 遥控器直接控制PWM输出
- Python命令被忽略
- 适用于手动操作和测试

自动模式 (CH8 > 1500):
- Python命令控制PWM输出
- 遥控器信号被忽略
- 适用于自动路径跟踪

安全保护:
- SBUS信号丢失1秒 → 中立位置(1500us)
- Python命令超时1秒 → 中立位置(1500us)
- 任何时候可通过CH8切换回手动模式

文件说明

STM32端

核心模块:
- STM32H743/FML/PythonLink.h/c - Python通信链路
- GPS/IMU数据发送 (DMA)
- 控制命令接收和解析
- 超时保护

STM32H743/FML/SBUS.h/c - SBUS遥控器
RC信号解析
CH8模式检测
自动/手动模式切换
Failsafe保护
STM32H743/FML/GPS.h/c - GPS/IMU解析
NMEA解析 (GPRMI)
IMU数据解析 (GPIMU)
即时发送到Python
STM32H743/FML/pwm_ctrol.h/c - PWM控制
转向/油门PWM输出
脉宽设置函数

配置:
- UART2: GPS/IMU接收 (115200 bps)
- UART4: SBUS接收 (100000 bps, 偶校验, 2停止位)
- UART5: Python通信 (921600 bps, DMA TX/RX)

Python端

核心模块:
- python/gps_imu_receiver.py - GPS/IMU数据接收器
- 协议解析
- 校验和验证
- 数据结构定义

python/realtime_control.py - 实时闭环控制器
GPS/IMU接收
坐标转换
运动控制算法集成
控制命令发送
python/mower_controller.py - 运动控制算法
纯追踪路径跟踪
PID控制
速度规划
python/test_control_protocol.py - 协议测试工具
测试控制命令发送
PWM值验证
硬件测试辅助

依赖:
pip install pyserial numpy matplotlib

使用流程

1. 硬件准备

确保GPS模块连接到UART2
SBUS接收器连接到UART4
USB转串口连接到UART5 (连接到PC)
电机和舵机正确接线

2. STM32固件烧录

# 使用Keil MDK或STM32CubeIDE编译并烧录
cd MDK-ARM
# 打开 STM32H743.uvprojx
# 编译并下载到STM32

3. Python环境配置

cd python
pip install -r requirements.txt

4. 测试步骤

步骤1: 验证GPS/IMU数据接收
bash python gps_imu_receiver.py # 修改COM端口为实际端口 # 应该看到GPS和IMU数据打印

步骤2: 测试控制命令协议
bash python test_control_protocol.py # 选择测试项目 # 在手动模式下(CH8<1500)验证遥控器控制正常 # 切换到自动模式(CH8>1500)验证Python命令生效

步骤3: 准备路径文件
bash # 使用path_planner.py或手动创建路径JSON文件 # 格式: [[x1,y1], [x2,y2], ...]

步骤4: 运行实时控制
bash python realtime_control.py # 修改配置: PORT, BAUDRATE, PATH_FILE # 确保CH8 > 1500 (自动模式) # 系统开始闭环控制

调试技巧

STM32端调试

// 在main.c或app.c中打印调试信息
PythonLink_PrintDebugInfo();  // 打印通信统计
SBUS_PrintDebugInfo();         // 打印SBUS状态

Python端调试

# 在realtime_control.py中启用详细日志
# 每秒打印一次控制信息
# 查看横向误差、航向误差等

常见问题

Q: GPS数据无法接收?
- 检查UART2连接和波特率(115200)
- 确认GPS模块有卫星信号(LED闪烁)
- 检查GPS_ParseGPRMI()是否调用PythonLink_SendGPSData()

Q: Python命令不生效?
- 确认CH8 > 1500 (切换到自动模式)
- 检查UART5连接和波特率(921600)
- 用test_control_protocol.py测试协议
- 查看PythonLink_PrintDebugInfo()统计

Q: 控制不稳定或振荡?
- 调整mower_controller.py中的PID参数
- 降低max_yawrate限制转向速度
- 增加lookahead_min提高稳定性
- 检查GPS更新频率和延迟

Q: 突然停止不动?
- 检查SBUS信号是否丢失(1秒failsafe)
- 检查Python命令是否超时(1秒超时)
- 确认电池电量充足
- 查看PWM输出是否正常

安全注意事项

⚠️ 重要安全提示:

始终保持遥控器开机 - CH8可随时切换到手动模式
测试时保持低速 - 先用小的PWM偏移测试
确保急停可用 - 遥控器的停止开关或断电开关
在空旷区域测试 - 避免障碍物和人员
检查电池电压 - 低电压会导致异常行为
验证方向正确 - 前进/后退、左转/右转方向与预期一致

参数调优

控制器参数 (realtime_control.py)

params = {
    'k_heading': 2.0,        # 航向增益 (增大→更激进转向)
    'k_xte': 0.8,            # 横向误差增益 (增大→更快纠正偏离)
    'k_heading_d': 0.1,      # 航向微分增益 (增大→抑制振荡)
    'max_forward_mps': 1.2,  # 最大速度 m/s
    'max_yawrate': π/2.5,    # 最大转向速率 rad/s
    'lookahead_min': 0.35,   # 最小前视距离 m
    'lookahead_max': 1.2,    # 最大前视距离 m
}

PWM映射 (realtime_control.py)

# 前进信号: -100~100 映射到 1000~2000us
throttle_pwm = 1500 + forward * 5.0

# 转向信号: -100~100 映射到 1000~2000us  
steering_pwm = 1500 + turn * 5.0

根据实际电机和舵机响应调整映射关系。

性能指标

GPS频率: 10Hz
IMU频率: 100Hz
控制频率: 74Hz
通信波特率: 921600 bps
路径跟踪精度: < 0.5m (取决于GPS质量)
响应延迟: < 50ms (GPS采样 + 算法计算 + 串口传输)

扩展功能建议

障碍物检测 - 添加超声波/激光雷达传感器
电池监控 - 添加电压监测和低电报警
数据记录 - 记录轨迹和控制信号用于分析
Web界面 - 实时监控和参数调整
多路径规划 - 支持复杂割草区域

开发团队

自动生成的草坪割草机控制系统
基于STM32H7 + Python运动控制算法

最后更新: 2025-11-13
版本: v1.0