# 轮腿机器人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
% 在MATLAB中运行
run('utils/lqr_design_optimized.m');
```
这将生成K矩阵的拟合系数，复制到`lqr.c`中的`K_fit_coefficients`数组。

#### 2.2 修改机器人物理参数
在`lqr.h`中修改您的机器人参数：
```c
#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：
```c
#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函数中初始化
```c
#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 定时器中断处理
```c
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()`函数：
```c
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 实现电机反馈读取
```c
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 实现电机控制输出
```c
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矩阵：
```matlab
% 状态权重矩阵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参数：
```c
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. 倾斜角保护
```c
#define MAX_TILT_ANGLE 0.5f  // 最大倾斜角 (rad)
```

### 2. 腿长保护
```c
#define LEG_MIN_REACH 0.05f  // 最小腿长 (m)
#define LEG_MAX_REACH 0.28f  // 最大腿长 (m)
```

### 3. 力矩限制
```c
#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参数设计工具
- 完善的安全保护机制