rm_balance/User/整合方案说明.md

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

📋 现有系统分析

🏗️ 您的系统架构（优势）

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

⚠️ 可以改进的地方

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

🚀 整合方案

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

第一阶段：保持现有系统，添加新接口

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

第二阶段：参数统一管理

创建统一的参数配置文件：

// 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

第三阶段：控制器选择接口

添加动态切换功能：

// 在遥控器中添加切换功能
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主循环

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. 数据接口适配

// 现有的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中的参数直接用于新系统：

// 使用您的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. 回滚机制：确保可以快速回到原有系统

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