rm_balance/User

轮腿机器人LQR平衡控制系统

这是一个完整的轮腿机器人LQR+VMC平衡控制系统，支持6个电机（2个轮电机+4个关节电机）的协调控制。

系统架构

┌─────────────────┐    ┌──────────────────┐    ┌─────────────────┐
│   传感器输入     │    │   平衡控制器      │    │   电机控制输出   │
│                │    │                  │    │                │
│ • IMU姿态数据   │────▶│  LQR控制器       │────▶│ • 轮电机力矩    │
│ • 电机反馈数据   │    │  VMC控制器       │    │ • 关节电机力矩   │
│ • 遥控器命令     │    │  状态估计器       │    │ • MIT控制参数   │
└─────────────────┘    └──────────────────┘    └─────────────────┘

文件结构

核心控制文件

• balance_control.h/c - 主控制器，整合LQR和VMC
• lqr.h/c - LQR控制器实现
• vmc.h/c - 虚拟模型控制器
• kinematics.h/c - 运动学计算

使用示例

• balance_control_example.c - 完整的使用示例

MATLAB工具

• lqr_design_optimized.m - LQR参数设计和系数生成
• vmc.m - VMC控制器设计参考

快速开始

1. 硬件连接

确保以下硬件正确连接：

• IMU传感器（提供机体姿态）
• 2个轮电机（支持转矩电流控制）
• 4个关节电机（支持MIT控制模式）
• CAN总线通信

2. 参数配置

2.1 运行MATLAB脚本生成LQR参数

% 在MATLAB中运行
run('utils/lqr_design_optimized.m');

这将生成K矩阵的拟合系数，复制到lqr.c中的K_fit_coefficients数组。

2.2 修改机器人物理参数

在lqr.h中修改您的机器人参数：

#define R_W 0.09f           // 驱动轮半径 (m)
#define R_L 0.25f           // 两个驱动轮之间距离/2 (m)
#define M_W 0.8f            // 驱动轮质量 (kg)
#define M_L 1.6183599f      // 腿部质量 (kg)
#define M_B 11.542f         // 机体质量 (kg)
// ... 其他参数

2.3 配置电机ID和CAN接口

在balance_control_example.c中修改电机ID：

#define WHEEL_LEFT_ID       0x01
#define WHEEL_RIGHT_ID      0x02
#define HIP_LEFT_ID         0x03
#define HIP_RIGHT_ID        0x04
#define KNEE_LEFT_ID        0x05
#define KNEE_RIGHT_ID       0x06

3. 代码集成

3.1 在main函数中初始化

#include "balance_control.h"

int main(void)
{
    // HAL初始化
    HAL_Init();
    SystemClock_Config();
    
    // 初始化外设
    MX_CAN1_Init();
    MX_TIM1_Init();
    
    // 初始化平衡控制系统
    balance_system_init();
    
    // 启动控制循环
    HAL_TIM_Base_Start_IT(&htim1);  // 1kHz控制频率
    
    while(1)
    {
        // 主循环处理其他任务
        HAL_Delay(100);
    }
}

3.2 定时器中断处理

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM1) {
        balance_control_task();  // 1kHz控制任务
    }
}

4. 传感器数据接口

4.1 实现IMU数据读取

在balance_control_example.c中实现read_imu_data()函数：

void read_imu_data(imu_data_t* imu)
{
    // 从您的IMU传感器读取数据
    imu->pitch = get_imu_pitch();
    imu->roll = get_imu_roll();
    imu->yaw = get_imu_yaw();
    imu->pitch_rate = get_imu_pitch_rate();
    imu->roll_rate = get_imu_roll_rate();
    imu->yaw_rate = get_imu_yaw_rate();
    // ... 其他数据
}

4.2 实现电机反馈读取

void read_motor_feedback(motor_feedback_t* motor_fb)
{
    // 从CAN总线读取电机反馈
    motor_fb->wheel_left_angle = get_motor_angle(WHEEL_LEFT_ID);
    motor_fb->wheel_right_angle = get_motor_angle(WHEEL_RIGHT_ID);
    motor_fb->hip_left_angle = get_motor_angle(HIP_LEFT_ID);
    // ... 其他电机数据
}

4.3 实现电机控制输出

void send_motor_control(const motor_control_t* motor_ctrl)
{
    // 发送轮电机转矩控制指令
    send_wheel_torque_cmd(WHEEL_LEFT_ID, motor_ctrl->wheel_left_torque_cmd);
    send_wheel_torque_cmd(WHEEL_RIGHT_ID, motor_ctrl->wheel_right_torque_cmd);
    
    // 发送关节电机MIT控制指令
    send_mit_control_cmd(HIP_LEFT_ID, 
                        0,  // 位置指令
                        0,  // 速度指令  
                        motor_ctrl->hip_left_kp,
                        motor_ctrl->hip_left_kd,
                        motor_ctrl->hip_left_torque_cmd);
    // ... 其他关节电机
}

控制模式

1. 平衡模式 (ROBOT_STATE_BALANCE)

• 自动保持机体平衡
• 响应遥控器前进/转向命令
• 自动调节腿长以适应地形

2. 运动模式 (ROBOT_STATE_MOVE)

• 在平衡基础上执行运动控制
• 支持前进、后退、转向
• 支持高度调节

3. 紧急停止 (ROBOT_STATE_EMERGENCY)

• 立即停止所有电机输出
• 安全保护机制

控制参数调节

LQR参数调节

在MATLAB脚本lqr_design_optimized.m中修改Q和R矩阵：

% 状态权重矩阵Q - 增大数值提高控制精度
Q = diag([
    10,      % s - 水平位移
    5,       % ds - 水平速度  
    20,      % phi - 偏航角
    10,      % dphi - 偏航角速度
    100,     % theta_ll - 左腿角
    50,      % dtheta_ll - 左腿角速度
    100,     % theta_lr - 右腿角  
    50,      % dtheta_lr - 右腿角速度
    200,     % theta_b - 机体倾斜角 (最重要)
    100      % dtheta_b - 机体角速度
]);

% 控制权重矩阵R - 增大数值降低控制量
R = diag([
    0.1,     % T_wl - 左轮力矩
    0.1,     % T_wr - 右轮力矩  
    1.0,     % T_bl - 左腿力矩
    1.0      % T_br - 右腿力矩
]);

VMC参数调节

在balance_control.c中修改VMC参数：

vmc_params_t vmc_params = {
    .K_spring = 800.0f,      // 径向弹簧刚度 - 控制腿长刚度
    .K_damper = 80.0f,       // 径向阻尼系数 - 控制腿长阻尼
    .K_theta = 150.0f,       // 角度刚度 - 控制腿角刚度
    .K_dtheta = 15.0f,       // 角速度阻尼 - 控制腿角阻尼
    .max_radial_force = 1000.0f,     // 最大径向力
    .max_tangential_force = 500.0f,  // 最大切向力
};

安全机制

1. 倾斜角保护

#define MAX_TILT_ANGLE 0.5f  // 最大倾斜角 (rad)

2. 腿长保护

#define LEG_MIN_REACH 0.05f  // 最小腿长 (m)
#define LEG_MAX_REACH 0.28f  // 最大腿长 (m)

3. 力矩限制

#define MAX_WHEEL_TORQUE 50.0f   // 最大轮子力矩 (N*m)
#define MAX_JOINT_TORQUE 100.0f  // 最大关节力矩 (N*m)

调试和监控

1. 串口调试输出

系统会定期输出关键状态信息：

=== Balance Control Status ===
Task Count: 1000
Robot State: 1
Safety Flag: 1
IMU Pitch: 0.050 rad (2.9 deg)
Left Leg Length: 0.200 m
Right Leg Length: 0.200 m
==============================

2. 状态监控

可以通过以下变量监控系统状态：

• g_balance_ctrl.state - 机器人状态
• g_balance_ctrl.safety_flag - 安全标志
• g_balance_ctrl.robot_state - 完整机器人状态

常见问题

Q: 机器人启动后不稳定？

A:

1. 检查IMU标定是否正确
2. 调整LQR的Q矩阵，增大机体角度权重
3. 检查电机正负方向是否正确

Q: 腿部抖动？

A:

1. 降低VMC的刚度参数
2. 增加VMC的阻尼参数
3. 检查关节电机的刚度设置

Q: 轮子打滑？

A:

1. 降低LQR的轮子力矩输出
2. 增加R矩阵中轮子力矩的权重
3. 检查地面摩擦条件

Q: 系统响应慢？

A:

1. 增加控制频率（目前为1kHz）
2. 调整LQR的Q矩阵权重
3. 检查CAN通信延迟

技术支持

如需技术支持或有疑问，请：

1. 检查上述常见问题
2. 确认硬件连接正确
3. 验证参数配置无误
4. 查看串口调试输出

更新日志

• v1.0 (2025-08-30): 初始版本，支持LQR+VMC协调控制
• 完整的6电机控制接口
• MATLAB参数设计工具
• 完善的安全保护机制