# 轮腿机器人控制系统整合方案

## 📋 现有系统分析

### 🏗️ 您的系统架构（优势）
1. **成熟的硬件接口**：已实现HT+BM电机CAN通信
2. **完整的运动学**：四连杆正/逆运动学 + 雅可比矩阵
3. **丰富的控制模式**：平衡、运动、跳跃、悬浮检测
4. **详细的参数数据**：基于实际测量的腿部参数表
5. **MATLAB设计工具**：完整的LQR参数生成流程

### ⚠️ 可以改进的地方
1. **控制逻辑分散**：LQR、VMC、MPC混合使用，逻辑复杂
2. **参数管理困难**：大量硬编码参数分散在各处
3. **缺乏统一接口**：不同控制算法接口不一致

## 🚀 整合方案

### 方案一：渐进式整合（推荐）

#### 第一阶段：保持现有系统，添加新接口
```c
// 在Chassis_Task.c中添加
#include "integrated_balance_control.h"

void Chassis_Task(void const * argument)
{
    vTaskDelay(CHASSIS_TASK_INIT_TIME);
    Chassis_Init(&chassis_control);
    vTaskDelay(100);
    
    while(1)
    {
        Chassis_Data_Update(&chassis_control);
        Chassis_Status_Detect(&chassis_control);
        Chassis_Mode_Set(&chassis_control);
        Chassis_Mode_Change_Control_Transit(&chassis_control);
        Target_Value_Set(&chassis_control);
        
        // 新增：整合控制器调用
        integrated_chassis_control_update();
        
        // 如果使用新控制器，跳过原有的力矩计算
        if (!get_current_controller_type()) {
            Chassis_Torque_Calculation(&chassis_control);
            Chassis_Torque_Combine(&chassis_control);
        }
        
        Motor_CMD_Send(&chassis_control);
        vTaskDelay(1);
    }
}
```

#### 第二阶段：参数统一管理
创建统一的参数配置文件：

```c
// robot_config.h
#ifndef ROBOT_CONFIG_H
#define ROBOT_CONFIG_H

// 机器人物理参数（与MATLAB一致）
#define ROBOT_R_W       0.09f           // 驱动轮半径 (m)
#define ROBOT_R_L       0.25f           // 轮距/2 (m)
#define ROBOT_L_C       0.037f          // 机体质心到关节距离 (m)
#define ROBOT_M_W       0.8f            // 驱动轮质量 (kg)
#define ROBOT_M_L       1.6183599f      // 腿部质量 (kg)
#define ROBOT_M_B       11.542f         // 机体质量 (kg)

// 腿部几何参数（与您的定义一致）
#define LEG_L1          0.15f           // 大腿长度 (m)
#define LEG_L2          0.25f           // 小腿长度 (m)
#define LEG_L3          0.25f           // 
#define LEG_L4          0.15f           //
#define LEG_L5          0.1f            // 髋关节间距/2 (m)

// 控制参数
#define CONTROL_FREQUENCY   500.0f      // 控制频率 (Hz)
#define MAX_WHEEL_TORQUE    50.0f       // 最大轮子力矩 (N*m)
#define MAX_JOINT_TORQUE    18.0f       // 最大关节力矩 (N*m)

// 安全参数
#define MAX_TILT_ANGLE      0.25f       // 最大倾斜角 (rad)
#define LEG_LENGTH_MIN      0.11f       // 最小腿长 (m)
#define LEG_LENGTH_MAX      0.30f       // 最大腿长 (m)

#endif
```

#### 第三阶段：控制器选择接口
添加动态切换功能：

```c
// 在遥控器中添加切换功能
void handle_controller_switch(const Gimbal_ctrl_t* rc_ctrl)
{
    static uint8_t last_switch_state = 0;
    uint8_t current_switch_state = 0; // 从遥控器读取开关状态
    
    if (current_switch_state != last_switch_state) {
        if (current_switch_state == 1) {
            // 切换到新控制器
            set_controller_type(1);
        } else {
            // 切换到原有控制器
            set_controller_type(0);
        }
        last_switch_state = current_switch_state;
    }
}
```

### 方案二：完全替换（激进）

直接用新的balance_control替换现有的控制逻辑：

#### 修改Chassis_Task主循环
```c
void Chassis_Task(void const * argument)
{
    vTaskDelay(CHASSIS_TASK_INIT_TIME);
    
    // 初始化新的平衡控制器
    balance_controller_t balance_ctrl;
    balance_control_init(&balance_ctrl, TASK_RUN_TIME);
    
    vTaskDelay(100);
    
    while(1)
    {
        // 数据更新（保留）
        Chassis_Data_Update(&chassis_control);
        
        // 转换数据格式
        imu_data_t imu_data;
        motor_feedback_t motor_fb;
        control_command_t cmd;
        
        convert_chassis_to_balance_imu(&chassis_control, &imu_data);
        convert_chassis_to_balance_motor_fb(&chassis_control, &motor_fb);
        convert_chassis_to_balance_cmd(&chassis_control, &cmd);
        
        // 使用新控制器
        balance_control_update(&balance_ctrl, &imu_data, &motor_fb, &cmd);
        
        // 获取控制输出
        motor_control_t motor_ctrl;
        get_motor_control_output(&balance_ctrl, &motor_ctrl);
        
        // 转换回原有格式并发送
        convert_balance_to_chassis_output(&motor_ctrl, &chassis_control);
        Motor_CMD_Send(&chassis_control);
        
        vTaskDelay(1);
    }
}
```

## 🔧 具体修改步骤

### 1. 保留您的优势代码
- **保留**：`Forward_kinematic_solution()` - 运动学计算
- **保留**：`Supportive_Force_Cal()` - 支撑力计算
- **保留**：`CAN_HT_CMD()` / `CAN_BM_CONTROL_CMD()` - 电机通信
- **保留**：MATLAB LQR设计流程

### 2. 整合控制架构
- **替换**：分散的LQR/VMC控制逻辑 → 统一的balance_control
- **简化**：复杂的状态机 → 清晰的模式管理
- **统一**：参数管理 → robot_config.h

### 3. 数据接口适配
```c
// 现有的HT电机数据
typedef struct {
    const HT_motor_measure_t *motor_measure;
    fp32 position;
    fp32 velocity; 
    fp32 torque_out, torque_get;
} joint_motor_t;

// 转换为新系统格式
void convert_ht_motor_to_balance_fb(const joint_motor_t* ht_motor, 
                                   float* angle, float* velocity, float* torque)
{
    *angle = ht_motor->position;
    *velocity = ht_motor->velocity;
    *torque = ht_motor->torque_get;
}
```

### 4. 参数迁移
将您MATLAB中的参数直接用于新系统：

```c
// 使用您的LQR拟合系数
const float K_fit_coefficients[40][6] = {
    // 复制您MATLAB生成的系数
    // ...
};

// 使用您的机器人参数
#define R_W_ACTUAL     0.09f    // 与您的R_w_ac一致
#define R_L_ACTUAL     0.25f    // 与您的R_l_ac一致
// ...
```

## 🎯 推荐的实施步骤

### 第1周：准备工作
1. 备份现有代码
2. 创建新的git分支
3. 编译并测试现有系统确保正常

### 第2周：集成准备
1. 添加integrated_balance_control文件
2. 修改Chassis_Task添加切换接口
3. 保持原有控制器为默认

### 第3周：参数对齐
1. 创建robot_config.h统一参数
2. 运行MATLAB脚本生成LQR系数
3. 对比调试确保参数一致

### 第4周：功能测试
1. 在安全环境下测试新控制器
2. 对比两套控制器的性能
3. 调试和优化

### 第5周：全面切换
1. 将新控制器设为默认
2. 移除冗余的旧代码
3. 文档整理和代码规范化

## 🛡️ 风险控制

1. **渐进式切换**：保留原有系统作为备份
2. **参数验证**：MATLAB仿真验证参数正确性
3. **安全测试**：在安全环境下测试新控制器
4. **性能对比**：记录并对比控制效果
5. **回滚机制**：确保可以快速回到原有系统

这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？