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

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

2026-02-09 14:59:50 +08:00

17 KiB

Raw Blame History

平衡轮足机器人使用指南

本指南提供详细的操作步骤，帮助您快速上手使用平衡轮足机器人系统。

环境准备

1. 硬件准备

必需设备：

STM32H723 控制板
ST-Link V2/V3 或 J-Link 调试器
电池（推荐 24V 锂电池）
大疆 DR16 遥控器
USB 转 TTL 串口模块（用于 CLI 调试）
各类电机（LZ、LK、DM）

可选设备：

VOFA+ 数据可视化软件
示波器
万用表

2. 软件准备

开发工具：

调试工具：

VOFA+ - 数据可视化
串口调试助手（如 SecureCRT、XShell、Putty）

编译工具链（如使用 CMake 编译）：

ARM GCC Toolchain: 下载地址
CMake: 下载地址

3. 驱动安装

ST-Link 驱动：
- 下载 ST-Link 驱动包
- 安装完成后，设备管理器中应能看到 ST-Link 设备
串口驱动（如使用 CH340）：
- 下载并安装对应芯片的 USB 串口驱动

编译与烧录

方法一：使用 Keil MDK

步骤 1：打开项目

启动 Keil MDK
打开项目文件：MDK-ARM/CtrBoard-H7_ALL.uvprojx

步骤 2：配置编译选项

点击菜单栏 Project → Options for Target
在 Target 选项卡中确认：
- MCU: STM32H723XG
- ARM Compiler: 版本 6.x 或更高
在 Debug 选项卡中选择调试器：
- 使用 ST-Link Debugger 或 J-Link Debugger
在 Flash Download 选项卡中确认：
- Programming Algorithm: STM32H7x3 1MB Flash
- 选中 Reset and Run 选项

步骤 3：编译项目

点击 Project → Build Target（或按 F7）
等待编译完成，检查是否有错误
成功后会在 MDK-ARM/CtrBoard-H7_ALL/ 目录生成 .hex 文件

步骤 4：烧录程序

连接 ST-Link 到控制板
点击 Flash → Download（或按 F8）
等待下载完成，提示 "Programming Done"
控制板会自动复位运行

方法二：使用 CMake

步骤 1：配置构建环境

# 进入项目根目录
cd balance_infantry

# 创建并进入构建目录
mkdir build && cd build

# 配置 CMake
cmake ..

步骤 2：编译项目

# 使用 4 个线程并行编译
make -j4

# 编译完成后，生成的文件位于 build 目录

步骤 3：烧录程序

使用 OpenOCD 或 ST-Link 工具烧录：

# 使用 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. 上电启动流程

连接电源：
- 将电池连接到电源输入端
- 确认电压稳定在正常范围
系统初始化：
- 系统会自动运行初始化任务（Task_Init）
- LED 指示灯开始闪烁
- 电机进行零点校准
等待就绪：
- 等待约 2-3 秒，系统进入待机状态
- LED 常亮或按固定频率闪烁（根据代码配置）
连接遥控器：
- 打开 DR16 遥控器
- 确认遥控器与接收器已连接
- 遥控器 LED 常绿表示连接成功

3. 模式选择

系统支持多种控制模式，通过遥控器切换：

模式	说明	切换方式
放松模式	电机不输出，用于调试	遥控器左摇杆按下
复位模式	机器人回到初始位置	遥控器特定按键
平衡模式	机器人自我平衡	遥控器特定按键
小陀螺模式	陀螺仪辅助平衡	遥控器特定按键

具体按键配置请参考 User/task/rc.c 文件。

操作说明

1. 基本运动控制

遥控器操作（DR16）

左摇杆：
  - 前后推拉：前进/后退
  - 左右推拉：平移
  - 按下：模式切换

右摇杆：
  - 前后推拉：云台 Pitch 轴控制
  - 左右推拉：云台 Yaw 轴控制
  - 按下：发射机构控制

左拨轮：调节目标高度

右拨轮：调节运动速度

注意事项

首次操作：
- 从低速开始，逐步熟悉控制手感
- 保持足够的缓冲空间
- 随时准备切换到放松模式
平衡模式：
- 机器人会自动保持平衡
- 输入较小的运动指令测试响应
- 观察机器人姿态是否稳定
高度控制：
- 使用左拨轮调整目标高度
- 高度变化应平缓，避免突变

2. 云台控制

云台 Yaw 轴控制

使用遥控器右摇杆左右推拉
控制底盘旋转角度
平稳转动，避免快速摆动

云台 Pitch 轴控制

使用遥控器右摇杆前后推拉
控制发射机构俯仰角
注意角度限制，避免碰撞

3. 发射机构控制

开启发射机构

切换到发射模式
按下右摇杆（或特定按键）启动摩擦轮
等待摩擦轮达到稳定转速
再次按键发射弹丸

注意事项

⚠️ 发射前确保前方安全，无人员
⚠️ 不使用时及时关闭发射机构
定期检查摩擦轮磨损情况

4. 跳跃功能

触发跳跃

跳跃功能仅在 CHASSIS_MODE_WHELL_LEG_BALANCE 模式下可用。

跳跃流程：

蓄力阶段（JUMP_CROUCH）：
- 机器人降低重心，缩短腿长
- 蓄力时间：jump_params.crouch_time_ms（默认配置）
起跳阶段（JUMP_LAUNCH）：
- 机器人发力向下，产生向上推力
- 发力时间：jump_params.launch_time_ms
收腿阶段（JUMP_RETRACT）：
- 腿部收缩，准备落地
- 收腿时间：jump_params.retract_time_ms
落地阶段（JUMP_LAND）：
- 缓冲着陆，恢复平衡
- 缓冲时间：jump_params.land_time_ms

跳跃参数调整

在 User/module/config.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 命令行调试

连接方式

使用 USB 转 TTL 串口模块连接到开发板的串口
串口参数配置：
- 波特率：115200
- 数据位：8
- 停止位：1
- 校验位：无
使用串口调试工具打开对应 COM 口

常用命令

# 显示帮助信息
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+ 数据可视化

配置步骤

打开 VOFA+ 软件
新建工程，选择 "JustFloat" 协议
配置串口：
- 波特率：115200
- 数据位：8
- 停止位：1
点击连接按钮

可视化数据

系统会自动发送以下数据：

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 中：

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：逐步调整

从小增益开始，逐步增大
先调整 theta 和 phi 的增益（姿态控制）
再调整 x 的增益（位置控制）
最后调整角速度的增益（阻尼）

步骤 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：微调

小幅调整三个参数
找到最佳平衡点

参数配置示例

KPID_Params_t yaw = {
    .kp = 5.0,
    .ki = 0.1,
    .kd = 0.5,
    .integral_limit = 10.0,
    .output_limit = 10.0,
};

3. VMC 参数调优

VMC 运动学参数

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)
};

调优要点

运动学参数：
- 根据实际机械尺寸精确测量
- 使用 MATLAB 工具（utils/Simulation-master/）验证
- 确保单位一致（全部使用米）
滤波参数：
- 调整 low_pass_cutoff_freq 优化信号质量
- 太高频：噪声大
- 太低频：响应慢

4. 使用工具辅助调优

项目提供 MATLAB 仿真工具：

LQR 增益计算：
- 运行 utils/6. 综合运动控制验证/LQR_K.m
- 输入系统参数
- 自动计算最优 LQR 增益
运动学仿真：
- 使用 utils/Simulation-master/ 中的 MATLAB 模型
- 验证五连杆运动学
- 测试不同参数组合
参数优化脚本：
- 使用 utils/k_calc/ 中的脚本
- 批量测试参数
- 找到最优解

故障排查

1. 系统无法启动

症状：上电后无反应，LED 不亮

可能原因：

电池电压不足或未连接
电源线路故障
MCU 未烧录程序

解决方法：

检查电池电压是否正常（22V-28V）
检查电源线路是否接触良好
重新烧录程序

2. 电机不转动

症状：系统运行正常，但电机不转动

可能原因：

电机 ID 配置错误
CAN 通信故障
电机未使能
电机驱动板故障

解决方法：

使用 CLI 命令 motor list 检查电机状态
检查 CAN 线连接
确认电机 ID 配置正确
使用 motor enable [id] 使能电机
检查电机驱动板供电

3. IMU 数据异常

症状：姿态角不稳定或数据异常

可能原因：

IMU 供电不稳定
IMU 安装不牢固
SPI 通信错误
IMU 未校准

解决方法：

检查 IMU 供电电压（3.3V）
重新安装 IMU，确保稳固
检查 SPI 线连接
使用 CLI 命令校准 IMU
更新 AHRS 算法参数

4. 机器人无法平衡

症状：机器人启动平衡模式后立即倒下

可能原因：

LQR 参数不合理
IMU 数据异常
电机响应不足
机械结构问题

解决方法：

检查 IMU 数据是否正常
使用 VOFA+ 观察姿态角变化
重新调整 LQR 参数（参考参数调优章节）
检查电机输出能力
检查机械结构，确保无松动

5. 通信故障

症状：遥控器失联或 VOFA+ 无法连接

可能原因：

遥控器未配对
串口参数错误
通信线路故障
任务未正常运行

解决方法：

重新配对 DR16 遥控器
检查串口参数（115200, 8N1）
检查通信线路
使用 CLI 命令 monitor 查看任务状态

6. 编译错误

症状：编译时出现错误

可能原因：

工具链版本不兼容
CMake 版本过低
路径问题
代码语法错误

解决方法：

更新 ARM GCC 到最新版本
确认 CMake 版本 >= 3.22
检查项目路径是否有中文或特殊字符
查看错误信息，定位代码问题
清理构建目录后重新编译：make clean && make

7. 跳跃功能异常

症状：跳跃后无法落地或姿态失控

可能原因：

跳跃参数不合理
腿长限位设置错误
落地检测失效
控制器响应不足

解决方法：

降低起跳力，从较小值开始测试
调整各阶段时间参数
检查腿长限位是否合理
优化落地缓冲参数
使用 VOFA+ 观察跳跃过程数据

获取更多帮助

如果以上方法无法解决问题：

查看日志：
- 使用 VOFA+ 记录故障时的数据
- 使用 CLI 命令查看详细状态
查阅文档：
- 检查代码注释
- 查看 utils/ 目录中的技术文档
提交 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 中的联系方式获取支持。

17 KiB Raw Blame History Unescape Escape

平衡轮足机器人使用指南

目录

环境准备

1. 硬件准备

2. 软件准备

3. 驱动安装

编译与烧录

方法一：使用 Keil MDK

步骤 1：打开项目

步骤 2：配置编译选项

步骤 3：编译项目

步骤 4：烧录程序

方法二：使用 CMake

步骤 1：配置构建环境

步骤 2：编译项目

步骤 3：烧录程序

系统启动

1. 首次启动检查清单

2. 上电启动流程

3. 模式选择

操作说明

1. 基本运动控制

遥控器操作（DR16）

注意事项

2. 云台控制

云台 Yaw 轴控制

云台 Pitch 轴控制

3. 发射机构控制

开启发射机构

注意事项

4. 跳跃功能

触发跳跃

跳跃参数调整

调试与监控

1. CLI 命令行调试

连接方式

常用命令

2. VOFA+ 数据可视化

配置步骤

可视化数据

数据导出

3. 监控任务

参数调优

1. LQR 参数调优

LQR 增益矩阵

调优方法

2. PID 参数调优

PID 控制器位置

调优步骤（经典方法）

参数配置示例

3. VMC 参数调优

VMC 运动学参数

调优要点

4. 使用工具辅助调优

故障排查

1. 系统无法启动

2. 电机不转动

3. IMU 数据异常

4. 机器人无法平衡

5. 通信故障

6. 编译错误

7. 跳跃功能异常

获取更多帮助

附录

A. 术语表

B. 串口引脚定义

C. 电机 ID 分配

D. 常用按键映射

17 KiB

Raw Blame History