robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(balance_chassis): enable wheel-leg balance mode and adjust parameters

Browse Source 
activate wheel-leg balance mode in RC switch case
update crouch and retract leg lengths in config
adjust non-contact theta threshold in chassis module
add README and USAGE documentation files
This commit is contained in:
Robofish 2026-02-09 14:59:50 +08:00
parent b5af2b9967
commit ec22b87dfa
7 changed files with 8048 additions and 6997 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

13978
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

320
README.md Normal file
View File

@ -0,0 +1,320 @@
# 平衡轮足机器人电控系统
基于 STM32H723 的平衡轮腿机器人电控系统，采用 FreeRTOS 实时操作系统，支持 LQR、VMC 等先进控制算法。
## 项目简介
本项目是一个高性能的平衡轮腿机器人电控系统，实现了机器人的平衡控制、运动控制、云台控制、发射机构控制等功能。系统采用模块化设计，具有良好的可扩展性和可维护性。
### 主要特性
- ✅ **高性能硬件平台**：STM32H723 微控制器，480MHz 主频
- ✅ **实时操作系统**：FreeRTOS，多任务并发处理
- ✅ **先进控制算法**：
- LQR（线性二次调节器）平衡控制
- VMC（虚拟模型控制）五连杆运动学解算
- PID 控制器
- 卡尔曼滤波器
- ✅ **多种功能模块**：
- 平衡底盘控制（轮腿结构）
- 云台控制（Yaw/Pitch）
- 发射机构控制
- 遥控接收（DR16）
- AI 视觉系统
- ✅ **丰富的外设接口**：
- CAN/FDCAN 总线
- SPI 接口
- UART 串口
- PWM 输出
- ADC 采集
- ✅ **调试工具**：
- CLI 命令行接口
- VOFA+ 数据可视化
- 监控任务
## 硬件架构
### 主控板
- **型号**：STM32H723XG
- **主频**：480MHz
- **Flash**：1MB
- **RAM**：1.4MB
### 传感器
- **IMU**：BMI088（加速度计 + 陀螺仪）
- **遥控器**：大疆 DR16
- **电机**：
- LZ 直流无刷电机（关节电机）
- LK 直流无刷电机（轮子电机）
- DM 直流无刷电机（云台电机）
### 通信接口
- **CAN/FDCAN**：电机通信、视觉通信
- **SPI**：IMU 数据读取
- **UART**：遥控接收、CLI、VOFA+
## 软件架构
### 目录结构
```
├── Core/ # STM32 HAL 库生成的核心代码
│ ├── Inc/ # 头文件
│ └── Src/ # 源文件
├── Drivers/ # STM32 驱动库
├── User/ # 用户代码
│ ├── bsp/ # 板级支持包
│ │ ├── can.h/.c # CAN/FDCAN 驱动
│ │ ├── uart.h/.c # UART 驱动
│ │ ├── spi.h/.c # SPI 驱动
│ │ ├── gpio.h/.c # GPIO 驱动
│ │ ├── flash.h/.c # Flash 操作
│ │ └── ...
│ ├── component/ # 通用组件
│ │ ├── lqr.h/.c # LQR 控制器
│ │ ├── vmc.h/.c # VMC 虚拟模型控制
│ │ ├── pid.h/.c # PID 控制器
│ │ ├── kalman_filter.h/.c # 卡尔曼滤波
│ │ ├── ahrs.h/.c # 姿态解算
│ │ ├── filter.h/.c # 滤波器
│ │ ├── limiter.c # 限幅器
│ │ └── ...
│ ├── device/ # 设备驱动
│ │ ├── motor_lz.h/.c # LZ 电机驱动
│ │ ├── motor_lk.h/.c # LK 电机驱动
│ │ ├── motor_dm.h/.c # DM 电机驱动
│ │ ├── dr16.h/.c # 遥控器驱动
│ │ ├── bmi088.h/.c # IMU 驱动
│ │ ├── vision_bridge.c # 视觉通信
│ │ ├── vofa.h/.c # VOFA+ 调试
│ │ └── ...
│ ├── module/ # 功能模块
│ │ ├── balance_chassis.h/.c # 平衡底盘
│ │ ├── gimbal.h/.c # 云台
│ │ ├── shoot.h/.c # 发射机构
│ │ └── config.h/.c # 配置管理
│ └── task/ # 任务管理
│ ├── user_task.h/.c # 任务框架
│ ├── rc.c # 遥控任务
│ ├── ctrl_chassis.c # 底盘控制任务
│ ├── ctrl_gimbal.c # 云台控制任务
│ ├── ctrl_shoot.c # 发射控制任务
│ ├── ai.c # AI 任务
│ ├── monitor.c # 监控任务
│ ├── cli.c # CLI 任务
│ └── ...
├── utils/ # 工具和仿真
├── MDK-ARM/ # Keil MDK 工程文件
├── build/ # CMake 构建目录
└── CMakeLists.txt # CMake 构建配置
```
### 任务调度
系统采用 FreeRTOS 实时多任务调度，主要任务包括：
| 任务名称 | 频率 | 功能描述 |
|---------|------|---------|
| Task_rc | 500Hz | 遥控器数据处理 |
| Task_atti_esit | - | 姿态估计 |
| Task_ctrl_chassis | 500Hz | 底盘控制 |
| Task_ctrl_gimbal | 500Hz | 云台控制 |
| Task_ctrl_shoot | 500Hz | 发射控制 |
| Task_ai | 500Hz | AI 视觉处理 |
| Task_monitor | 500Hz | 系统监控 |
| Task_vofa | 500Hz | VOFA+ 数据发送 |
| Task_cli | - | CLI 命令行处理 |
| Task_blink | 500Hz | LED 指示灯 |
### 核心算法
#### 1. LQR 控制
线性二次调节器（Linear Quadratic Regulator）用于实现轮腿机器人的平衡控制。
**状态向量**：
- theta：摆杆角度
- d_theta：摆杆角速度
- x：驱动轮位移
- d_x：驱动轮速度
- phi：机体俯仰角
- d_phi：机体俯仰角速度
**控制输出**：
- T：轮毂力矩
- Tp：髋关节力矩
#### 2. VMC 控制
虚拟模型控制（Virtual Model Control）实现五连杆运动学解算和力矩分配。
**功能**：
- 五连杆正运动学解算（关节角度 → 足端位置）
- 五连杆逆运动学解算（虚拟力 → 关节力矩）
- 等效摆动杆参数计算
- 地面接触检测
#### 3. 平衡控制流程
```
遥控输入 → 运动指令 → LQR 控制 → VMC 解算 → 电机输出 → 平衡控制
↓
姿态反馈
```
## 编译环境
### 推荐开发环境
1. **IDE**：
- Keil MDK-ARM 5.x
- VS Code + Cortex-Debug 插件
2. **编译工具链**：
- ARM GCC Toolchain (arm-none-eabi-gcc)
- CMake 3.22+
3. **调试工具**：
- ST-Link V2/V3
- J-Link
### 依赖库
- STM32 HAL 库
- FreeRTOS
- CMSIS DSP 库
## 快速开始
### 1. 克隆项目
```bash
git clone ssh://git@gitea.qutrobot.top:222/robofish/rm_balance.git
cd balance_infantry
```
### 2. 使用 Keil MDK 编译
1. 打开 `MDK-ARM/CtrBoard-H7_ALL.uvprojx`
2. 选择正确的编译配置（Debug/Release）
3. 点击编译（F7）
4. 下载到开发板（F8）
### 3. 使用 CMake 编译
```bash
# 创建构建目录
mkdir -p build && cd build
# 配置 CMake
cmake ..
# 编译
make -j4
# 生成 HEX 文件位于 build 目录
```
### 4. 使用 VS Code 开发
项目已配置 `.clangd`，支持：
- 智能代码补全
- 语法检查
- 跳转到定义
- 重构功能
## 配置说明
### 参数配置
主要参数配置文件位于 `User/module/config.c`，包括：
- **底盘参数**（`Chassis_Params_t`）：
- 电机参数
- VMC 参数
- LQR 增益矩阵
- PID 参数
- 跳跃参数
- **云台参数**（`Gimbal_Params_t`）：
- 电机参数
- PID 参数
- **发射参数**（`Shoot_Params_t`）：
- 摩擦轮参数
- 拨弹轮参数
### 配置文件格式
项目使用 YAML 格式的配置文件（部分模块）：
- `User/task/config.yaml`
- `User/bsp/bsp_config.yaml`
- `User/component/component_config.yaml`
- `User/device/device_config.yaml`
## 调试工具
### 1. CLI 命令行
通过串口连接，可使用以下命令：
```bash
help # 显示帮助信息
status # 显示系统状态
motor # 电机控制
chassis # 底盘控制
gimbal # 云台控制
```
### 2. VOFA+ 数据可视化
支持实时数据监控和绘图，配置串口参数后即可使用。
### 3. 监控任务
`Task_monitor` 实时监控系统状态，包括：
- 电池电压
- CPU 温度
- 任务运行频率
- 栈使用情况
## 注意事项
### 安全事项
⚠️ **重要**：
1. 首次运行前务必检查电机方向和限位
2. 调试时使用低电压，避免失控
3. 确保 IMU 安装正确，校准零点
4. 测试跳跃功能时做好防护
### 常见问题
1. **编译错误**：
- 检查 ARM GCC 工具链是否正确安装
- 确认 CMake 版本 >= 3.22
2. **下载失败**：
- 检查 ST-Link/J-Link 连接
- 确认 BOOT 模式设置正确
3. **平衡控制失效**：
- 检查 IMU 数据是否正常
- 确认 LQR 参数是否合理
- 验证电机反馈数据
## 贡献指南
欢迎提交 Issue 和 Pull Request！
## 许可证
本项目采用 MIT 许可证。
## 联系方式
- 项目地址：ssh://git@gitea.qutrobot.top:222/robofish/rm_balance.git
---
**祝您使用愉快！**

733
USAGE.md Normal file
View File

@ -0,0 +1,733 @@
# 平衡轮足机器人使用指南
本指南提供详细的操作步骤，帮助您快速上手使用平衡轮足机器人系统。
## 目录
- [环境准备](#环境准备)
- [编译与烧录](#编译与烧录)
- [系统启动](#系统启动)
- [操作说明](#操作说明)
- [调试与监控](#调试与监控)
- [参数调优](#参数调优)
- [故障排查](#故障排查)
---
## 环境准备
### 1. 硬件准备
**必需设备**：
- STM32H723 控制板
- ST-Link V2/V3 或 J-Link 调试器
- 电池（推荐 24V 锂电池）
- 大疆 DR16 遥控器
- USB 转 TTL 串口模块（用于 CLI 调试）
- 各类电机（LZ、LK、DM）
**可选设备**：
- VOFA+ 数据可视化软件
- 示波器
- 万用表
### 2. 软件准备
**开发工具**：
- [Keil MDK-ARM 5.x](https://www.keil.com/download/product/)（推荐）
- 或 [VS Code](https://code.visualstudio.com/) + [Cortex-Debug](https://marketplace.visualstudio.com/items?itemName=marus25.cortex-debug)
- [STM32CubeMX](https://www.st.com/zh/development-tools/stm32cubemx.html)
**调试工具**：
- [VOFA+](https://www.vofa.plus/) - 数据可视化
- 串口调试助手（如 SecureCRT、XShell、Putty）
**编译工具链**（如使用 CMake 编译）：
- ARM GCC Toolchain: [下载地址](https://developer.arm.com/downloads/-/gnu-rm)
- CMake: [下载地址](https://cmake.org/download/)
### 3. 驱动安装
1. **ST-Link 驱动**：
- 下载 [ST-Link 驱动包](https://www.st.com/zh/development-tools/stsw-link009.html)
- 安装完成后，设备管理器中应能看到 ST-Link 设备
2. **串口驱动**（如使用 CH340）：
- 下载并安装对应芯片的 USB 串口驱动
---
## 编译与烧录
### 方法一：使用 Keil MDK
#### 步骤 1：打开项目
1. 启动 Keil MDK
2. 打开项目文件：`MDK-ARM/CtrBoard-H7_ALL.uvprojx`
#### 步骤 2：配置编译选项
1. 点击菜单栏 `Project` → `Options for Target`
2. 在 `Target` 选项卡中确认：
- MCU: STM32H723XG
- ARM Compiler: 版本 6.x 或更高
3. 在 `Debug` 选项卡中选择调试器：
- 使用 `ST-Link Debugger` 或 `J-Link Debugger`
4. 在 `Flash Download` 选项卡中确认：
- Programming Algorithm: STM32H7x3 1MB Flash
- 选中 `Reset and Run` 选项
#### 步骤 3：编译项目
1. 点击 `Project` → `Build Target`（或按 F7）
2. 等待编译完成，检查是否有错误
3. 成功后会在 `MDK-ARM/CtrBoard-H7_ALL/` 目录生成 `.hex` 文件
#### 步骤 4：烧录程序
1. 连接 ST-Link 到控制板
2. 点击 `Flash` → `Download`（或按 F8）
3. 等待下载完成，提示 "Programming Done"
4. 控制板会自动复位运行
### 方法二：使用 CMake
#### 步骤 1：配置构建环境
```bash
# 进入项目根目录
cd balance_infantry
# 创建并进入构建目录
mkdir build && cd build
# 配置 CMake
cmake ..
```
#### 步骤 2：编译项目
```bash
# 使用 4 个线程并行编译
make -j4
# 编译完成后，生成的文件位于 build 目录
```
#### 步骤 3：烧录程序
使用 OpenOCD 或 ST-Link 工具烧录：
```bash
# 使用 OpenOCD 烧录（需要先安装 openocd）
openocd -f interface/stlink.cfg -f target/stm32h7x.cfg \
-c "program CtrBoard-H7_ALL.hex verify reset exit"
```
或使用 ST-LINK Utility 图形界面工具。
---
## 系统启动
### 1. 首次启动检查清单
⚠️ **重要**：首次启动前请务必完成以下检查：
- [ ] 电池电压正常（推荐 22V-28V）
- [ ] 所有电机连接正确，没有短路
- [ ] IMU 安装稳固，方向正确
- [ ] 遥控器已配对，电量充足
- [ ] 电机 ID 配置正确（参考 `User/device/` 目录下电机驱动）
- [ ] 机械结构紧固，无松动
- [ ] 运动空间充足，无障碍物
### 2. 上电启动流程
1. **连接电源**：
- 将电池连接到电源输入端
- 确认电压稳定在正常范围
2. **系统初始化**：
- 系统会自动运行初始化任务（`Task_Init`）
- LED 指示灯开始闪烁
- 电机进行零点校准
3. **等待就绪**：
- 等待约 2-3 秒，系统进入待机状态
- LED 常亮或按固定频率闪烁（根据代码配置）
4. **连接遥控器**：
- 打开 DR16 遥控器
- 确认遥控器与接收器已连接
- 遥控器 LED 常绿表示连接成功
### 3. 模式选择
系统支持多种控制模式，通过遥控器切换：
| 模式 | 说明 | 切换方式 |
|------|------|---------|
| 放松模式 | 电机不输出，用于调试 | 遥控器左摇杆按下 |
| 复位模式 | 机器人回到初始位置 | 遥控器特定按键 |
| 平衡模式 | 机器人自我平衡 | 遥控器特定按键 |
| 小陀螺模式 | 陀螺仪辅助平衡 | 遥控器特定按键 |
**具体按键配置请参考 `User/task/rc.c` 文件。**
---
## 操作说明
### 1. 基本运动控制
#### 遥控器操作（DR16）
```
左摇杆：
- 前后推拉：前进/后退
- 左右推拉：平移
- 按下：模式切换
右摇杆：
- 前后推拉：云台 Pitch 轴控制
- 左右推拉：云台 Yaw 轴控制
- 按下：发射机构控制
左拨轮：调节目标高度
右拨轮：调节运动速度
```
#### 注意事项
1. **首次操作**：
- 从低速开始，逐步熟悉控制手感
- 保持足够的缓冲空间
- 随时准备切换到放松模式
2. **平衡模式**：
- 机器人会自动保持平衡
- 输入较小的运动指令测试响应
- 观察机器人姿态是否稳定
3. **高度控制**：
- 使用左拨轮调整目标高度
- 高度变化应平缓，避免突变
### 2. 云台控制
#### 云台 Yaw 轴控制
- 使用遥控器右摇杆左右推拉
- 控制底盘旋转角度
- 平稳转动，避免快速摆动
#### 云台 Pitch 轴控制
- 使用遥控器右摇杆前后推拉
- 控制发射机构俯仰角
- 注意角度限制，避免碰撞
### 3. 发射机构控制
#### 开启发射机构
1. 切换到发射模式
2. 按下右摇杆（或特定按键）启动摩擦轮
3. 等待摩擦轮达到稳定转速
4. 再次按键发射弹丸
#### 注意事项
- ⚠️ 发射前确保前方安全，无人员
- ⚠️ 不使用时及时关闭发射机构
- 定期检查摩擦轮磨损情况
### 4. 跳跃功能
#### 触发跳跃
跳跃功能仅在 `CHASSIS_MODE_WHELL_LEG_BALANCE` 模式下可用。
**跳跃流程**：
1. **蓄力阶段（JUMP_CROUCH）**：
- 机器人降低重心，缩短腿长
- 蓄力时间：`jump_params.crouch_time_ms`（默认配置）
2. **起跳阶段（JUMP_LAUNCH）**：
- 机器人发力向下，产生向上推力
- 发力时间：`jump_params.launch_time_ms`
3. **收腿阶段（JUMP_RETRACT）**：
- 腿部收缩，准备落地
- 收腿时间：`jump_params.retract_time_ms`
4. **落地阶段（JUMP_LAND）**：
- 缓冲着陆，恢复平衡
- 缓冲时间：`jump_params.land_time_ms`
#### 跳跃参数调整
在 `User/module/config.c` 中修改跳跃参数：
```c
.jump_params = {
.crouch_time_ms = 500, // 蓄力时间 (ms)
.launch_time_ms = 100, // 起跳发力时间 (ms)
.retract_time_ms = 200, // 收腿时间 (ms)
.land_time_ms = 300, // 落地缓冲时间 (ms)
.crouch_leg_length = 0.3, // 蓄力时腿长 (m)
.launch_force = 500.0, // 起跳力 (N)
.retract_leg_length = 0.2, // 收腿时腿长 (m)
.retract_force = -100.0, // 收腿前馈力 (N)
},
```
**调试建议**：
- 先调整蓄力时间和腿长，找到合适的蓄力状态
- 逐步增加起跳力，避免跳得太高
- 根据实际表现调整各阶段时间
---
## 调试与监控
### 1. CLI 命令行调试
#### 连接方式
1. 使用 USB 转 TTL 串口模块连接到开发板的串口
2. 串口参数配置：
- 波特率：115200
- 数据位：8
- 停止位：1
- 校验位：无
3. 使用串口调试工具打开对应 COM 口
#### 常用命令
```bash
# 显示帮助信息
help
# 查看系统状态
status
# 输出示例：
# Battery: 24.5V (80%)
# CPU Temp: 45°C
# Tasks: Running normally
# 电机控制
motor
# 子命令：
# list 列出所有电机状态
# calibrate [id] 校准指定电机
# enable [id] 启用指定电机
# disable [id] 禁用指定电机
# 底盘控制
chassis
# 子命令：
# mode [mode] 设置底盘模式
# balance 进入平衡模式
# relax 进入放松模式
# 云台控制
gimbal
# 子命令：
# reset 复位云台
# calibrate 校准云台
# 监控任务信息
monitor
# 输出各任务运行频率、栈使用情况
```
### 2. VOFA+ 数据可视化
#### 配置步骤
1. 打开 VOFA+ 软件
2. 新建工程，选择 "JustFloat" 协议
3. 配置串口：
- 波特率：115200
- 数据位：8
- 停止位：1
4. 点击连接按钮
#### 可视化数据
系统会自动发送以下数据：
- IMU 数据：加速度、陀螺仪、欧拉角
- 电机数据：位置、速度、电流
- 控制输出：LQR 输出、VMC 输出
- 系统状态：电池电压、CPU 温度
#### 数据导出
- 点击 "录制" 按钮开始记录数据
- 停止录制后，数据可导出为 CSV 格式
- 使用 Excel、MATLAB 等工具分析数据
### 3. 监控任务
`Task_monitor` 实时监控系统状态，包括：
**监控内容**：
- 电池电压和电量百分比
- CPU 温度
- 各任务运行频率
- 任务栈使用情况
- 设备在线状态
**查看方式**：
- 通过 CLI 命令 `monitor`
- 或通过 VOFA+ 实时查看
**正常指标**：
- 电池电压：22V - 28V
- CPU 温度：< 70°C
- 任务频率：稳定在设定值（如 500Hz）
- 栈使用率：< 80%
---
## 参数调优
### 1. LQR 参数调优
#### LQR 增益矩阵
LQR 控制器的增益矩阵存储在 `User/module/config.c` 中：
```c
LQR_GainMatrix_t lqr_gains = {
// K 矩阵第一行（轮毂力矩 T）
.k11_coeff = { ... }, // theta 的增益
.k12_coeff = { ... }, // d_theta 的增益
.k13_coeff = { ... }, // x 的增益
.k14_coeff = { ... }, // d_x 的增益
.k15_coeff = { ... }, // phi 的增益
.k16_coeff = { ... }, // d_phi 的增益
// K 矩阵第二行（髋关节力矩 Tp）
.k21_coeff = { ... },
.k22_coeff = { ... },
.k23_coeff = { ... },
.k24_coeff = { ... },
.k25_coeff = { ... },
.k26_coeff = { ... },
};
```
#### 调优方法
**步骤 1：确定目标**
- 平衡性：机器人能稳定平衡
- 响应性：对指令响应迅速
- 稳定性：无明显振荡
**步骤 2：逐步调整**
1. 从小增益开始，逐步增大
2. 先调整 theta 和 phi 的增益（姿态控制）
3. 再调整 x 的增益（位置控制）
4. 最后调整角速度的增益（阻尼）
**步骤 3：测试验证**
- 每次调整后重新编译烧录
- 测试平衡性能
- 使用 VOFA+ 观察响应曲线
**调优技巧**：
- 增益过小：响应慢，容易失衡
- 增益过大：振荡，不稳定
- 角速度增益影响阻尼，防止振荡
- 位置增益影响响应速度
### 2. PID 参数调优
#### PID 控制器位置
系统中有多个 PID 控制器：
- Yaw 轴 PID（`pid.yaw`）
- Roll 轴 PID（`pid.roll`）
- 摆力矩 PID（`pid.tp`）
- 腿长 PID（`pid.leg_length`）
- 摆角 PID（`pid.leg_theta`）
#### 调优步骤（经典方法）
**步骤 1：设置初始值**
```
Kp = 0
Ki = 0
Kd = 0
```
**步骤 2：调整 Kp**
- 逐步增大 Kp
- 直到系统开始振荡
- 将 Kp 减半（约为临界值的 50%-70%）
**步骤 3：调整 Kd**
- 增大 Kd
- 减小振荡
- 直到振荡消失
- 保持适当的阻尼
**步骤 4：调整 Ki**
- 增大 Ki
- 消除稳态误差
- 避免积分饱和
**步骤 5：微调**
- 小幅调整三个参数
- 找到最佳平衡点
#### 参数配置示例
```c
KPID_Params_t yaw = {
.kp = 5.0,
.ki = 0.1,
.kd = 0.5,
.integral_limit = 10.0,
.output_limit = 10.0,
};
```
### 3. VMC 参数调优
#### VMC 运动学参数
```c
VMC_Param_t vmc_param = {
.hip_length = 0.15, // 髋关节间距 (m)
.leg_1 = 0.2, // 大腿前端长度 (m)
.leg_2 = 0.2, // 大腿后端长度 (m)
.leg_3 = 0.25, // 小腿长度 (m)
.leg_4 = 0.1, // 小腿前端长度 (m)
.wheel_radius = 0.06, // 轮子半径 (m)
.wheel_mass = 0.5, // 轮子质量 (kg)
};
```
#### 调优要点
1. **运动学参数**：
- 根据实际机械尺寸精确测量
- 使用 MATLAB 工具（`utils/Simulation-master/`）验证
- 确保单位一致（全部使用米）
2. **滤波参数**：
- 调整 `low_pass_cutoff_freq` 优化信号质量
- 太高频：噪声大
- 太低频：响应慢
### 4. 使用工具辅助调优
项目提供 MATLAB 仿真工具：
1. **LQR 增益计算**：
- 运行 `utils/6. 综合运动控制验证/LQR_K.m`
- 输入系统参数
- 自动计算最优 LQR 增益
2. **运动学仿真**：
- 使用 `utils/Simulation-master/` 中的 MATLAB 模型
- 验证五连杆运动学
- 测试不同参数组合
3. **参数优化脚本**：
- 使用 `utils/k_calc/` 中的脚本
- 批量测试参数
- 找到最优解
---
## 故障排查
### 1. 系统无法启动
**症状**：上电后无反应，LED 不亮
**可能原因**：
- 电池电压不足或未连接
- 电源线路故障
- MCU 未烧录程序
**解决方法**：
1. 检查电池电压是否正常（22V-28V）
2. 检查电源线路是否接触良好
3. 重新烧录程序
### 2. 电机不转动
**症状**：系统运行正常，但电机不转动
**可能原因**：
- 电机 ID 配置错误
- CAN 通信故障
- 电机未使能
- 电机驱动板故障
**解决方法**：
1. 使用 CLI 命令 `motor list` 检查电机状态
2. 检查 CAN 线连接
3. 确认电机 ID 配置正确
4. 使用 `motor enable [id]` 使能电机
5. 检查电机驱动板供电
### 3. IMU 数据异常
**症状**：姿态角不稳定或数据异常
**可能原因**：
- IMU 供电不稳定
- IMU 安装不牢固
- SPI 通信错误
- IMU 未校准
**解决方法**：
1. 检查 IMU 供电电压（3.3V）
2. 重新安装 IMU，确保稳固
3. 检查 SPI 线连接
4. 使用 CLI 命令校准 IMU
5. 更新 AHRS 算法参数
### 4. 机器人无法平衡
**症状**：机器人启动平衡模式后立即倒下
**可能原因**：
- LQR 参数不合理
- IMU 数据异常
- 电机响应不足
- 机械结构问题
**解决方法**：
1. 检查 IMU 数据是否正常
2. 使用 VOFA+ 观察姿态角变化
3. 重新调整 LQR 参数（参考参数调优章节）
4. 检查电机输出能力
5. 检查机械结构，确保无松动
### 5. 通信故障
**症状**：遥控器失联或 VOFA+ 无法连接
**可能原因**：
- 遥控器未配对
- 串口参数错误
- 通信线路故障
- 任务未正常运行
**解决方法**：
1. 重新配对 DR16 遥控器
2. 检查串口参数（115200, 8N1）
3. 检查通信线路
4. 使用 CLI 命令 `monitor` 查看任务状态
### 6. 编译错误
**症状**：编译时出现错误
**可能原因**：
- 工具链版本不兼容
- CMake 版本过低
- 路径问题
- 代码语法错误
**解决方法**：
1. 更新 ARM GCC 到最新版本
2. 确认 CMake 版本 >= 3.22
3. 检查项目路径是否有中文或特殊字符
4. 查看错误信息，定位代码问题
5. 清理构建目录后重新编译：`make clean && make`
### 7. 跳跃功能异常
**症状**：跳跃后无法落地或姿态失控
**可能原因**：
- 跳跃参数不合理
- 腿长限位设置错误
- 落地检测失效
- 控制器响应不足
**解决方法**：
1. 降低起跳力，从较小值开始测试
2. 调整各阶段时间参数
3. 检查腿长限位是否合理
4. 优化落地缓冲参数
5. 使用 VOFA+ 观察跳跃过程数据
### 获取更多帮助
如果以上方法无法解决问题：
1. **查看日志**：
- 使用 VOFA+ 记录故障时的数据
- 使用 CLI 命令查看详细状态
2. **查阅文档**：
- 检查代码注释
- 查看 `utils/` 目录中的技术文档
3. **提交 Issue**：
- 在项目仓库提交详细的问题描述
- 包含：硬件型号、软件版本、错误日志、重现步骤
---
## 附录
### A. 术语表
| 术语 | 全称 | 说明 |
|------|------|------|
| LQR | Linear Quadratic Regulator | 线性二次调节器 |
| VMC | Virtual Model Control | 虚拟模型控制 |
| IMU | Inertial Measurement Unit | 惯性测量单元 |
| PID | Proportional-Integral-Derivative | 比例-积分-微分控制器 |
| CAN | Controller Area Network | 控制器局域网 |
| FDCAN | Flexible Data-rate CAN | 灵活数据速率 CAN |
| CLI | Command Line Interface | 命令行接口 |
| AHRS | Attitude and Heading Reference System | 姿态和航向参考系统 |
### B. 串口引脚定义
| 串口 | TX | RX | 功能 |
|------|----|----|------|
| USART1 | PA9 | PA10 | CLI 调试 |
| USART2 | PA2 | PA3 | VOFA+ 数据 |
| USART3 | PB10 | PB11 | 视觉通信 |
| UART4 | PA0 | PA1 | 遥控接收 |
### C. 电机 ID 分配
| 电机类型 | ID | 位置 | 用途 |
|---------|----|----|------|
| LZ 电机 | 1-4 | 关节 | 髋关节控制 |
| LK 电机 | 1-2 | 轮子 | 驱动轮控制 |
| DM 电机 | 1 | 云台 | Yaw 轴控制 |
| DM 电机 | 2 | 云台 | Pitch 轴控制 |
### D. 常用按键映射
| 按键 | 功能 | 说明 |
|------|------|------|
| 左摇杆按下 | 模式切换 | 在不同控制模式间切换 |
| 右摇杆按下 | 发射控制 | 控制发射机构开关 |
| 左拨轮 | 高度调节 | 调节机器人目标高度 |
| 右拨轮 | 速度调节 | 调节运动速度上限 |
---
**祝您调试顺利！**
如有问题，请参考 README.md 中的联系方式获取支持。

2
User/module/balance_chassis.c
View File

@ -24,7 +24,7 @@
#define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */ #define LEG_LENGTH_RANGE 0.16f /* 腿长调节范围 (m) */
#define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */ #define MIN_LEG_LENGTH 0.10f /* 最小腿长 (m) */
#define MAX_LEG_LENGTH 0.34f /* 最大腿长 (m) */ #define MAX_LEG_LENGTH 0.34f /* 最大腿长 (m) */
#define NON_CONTACT_THETA 0.16f /* 离地时的摆角目标 (rad) */ #define NON_CONTACT_THETA 0.0f /* 离地时的摆角目标 (rad) */
#define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */ #define LEFT_BASE_FORCE 60.0f /* 左腿基础支撑力 (N) */
#define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */ #define RIGHT_BASE_FORCE 60.0f /* 右腿基础支撑力 (N) */

4
User/module/config.c
View File

@ -336,9 +336,9 @@ Config_RobotParam_t robot_config = {
.launch_time_ms = 120, .launch_time_ms = 120,
.retract_time_ms = 80, .retract_time_ms = 80,
.land_time_ms = 300, .land_time_ms = 300,
.crouch_leg_length = 0.14f, .crouch_leg_length = 0.1f,
.launch_force = 200.0f, .launch_force = 200.0f,
.retract_leg_length = 0.16f, /* 收腿目标更短 */ .retract_leg_length = 0.1f, /* 收腿目标更短 */
.retract_force = -120.0f, /* 收腿前馈力加大 */ .retract_force = -120.0f, /* 收腿前馈力加大 */
}, },
.theta = 0.0f, .theta = 0.0f,

8
User/task/rc.c
View File

@ -160,13 +160,13 @@ switch (dr16.data.sw_l) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX; cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break; break;
case DR16_SW_MID: case DR16_SW_MID:
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break; break;
case DR16_SW_DOWN: case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR; cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE; // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
break; break;
default: default: