深度剖析Cubli_Mini自平衡立方体的核心控制算法与硬件架构实现

深度剖析Cubli_Mini自平衡立方体的核心控制算法与硬件架构实现

【免费下载链接】Cubli_Mini项目地址: https://gitcode.com/gh_mirrors/cu/Cubli_Mini

Cubli_Mini是一个基于ESP32微控制器的开源自平衡立方体机器人项目，它通过三个正交安装的飞轮实现姿态控制，能够在桌面等平面上实现稳定站立和平衡。该项目源自苏黎世联邦理工学院的Cubli项目，经过小型化、低成本化改造，成为适合电子爱好者和机器人学习者的实践平台。

核心控制算法：LQR与姿态解算的完美融合

Cubli_Mini的核心技术挑战在于如何通过三个飞轮的协同控制实现立方体在边和点上的稳定平衡。项目采用线性二次调节器（LQR）控制算法，结合四元数姿态解算，实现了高精度的姿态控制。

姿态解算与传感器融合

在2.Firmware/Mcu1/include/imu/ahrs.h中，AHRS（姿态航向参考系统）类实现了基于MPU6050传感器数据的姿态解算。关键参数定义如下：

#define Kp 5.0f // 比例增益控制加速度计/磁力计收敛速率 #define Ki 0.01f // 积分增益控制陀螺仪偏差收敛速率

系统采用四元数表示姿态，通过互补滤波或卡尔曼滤波算法融合加速度计和陀螺仪数据，获得稳定的姿态信息。这种融合算法有效解决了加速度计低频噪声和陀螺仪漂移问题。

LQR控制算法实现

在2.Firmware/Mcu1/src/control/cubli_mini.cpp中，LQR控制算法的核心实现如下：

float CubliMiniControl::Lqr(const AxisSensor_t &_sensor, const AxisParam_t &_param) { return (_sensor.angle - _param.angle_offset) * _param.kp + _sensor.gyro * _param.kv + _sensor.speed * _param.ks; }

该算法通过三个控制项实现精确控制：

1. 角度偏差控制：(_sensor.angle - _param.angle_offset) * _param.kp- 基于姿态角度与目标角度的偏差
2. 角速度阻尼：_sensor.gyro * _param.kv- 抑制立方体旋转的角速度
3. 飞轮速度反馈：_sensor.speed * _param.ks- 基于飞轮转速的稳定控制

坐标变换与电机协同控制

Cubli_Mini采用三个正交安装的飞轮，需要将三维姿态控制转换为三个电机的协同控制。在GetMotorMoveSpeed函数中实现了坐标变换：

_set_speed.ch1 = - _y_speed * cos((30) / 57.3f) - _x_speed * sin((30) / 57.3f) + _z_speed / 3.0f; _set_speed.ch2 = _x_speed + _z_speed / 3.0f; _set_speed.ch3 = -_x_speed * sin((30) / 57.3f) + _y_speed * cos((30) / 57.3f) + _z_speed / 3.0f;

这种变换确保了三个飞轮能够协同工作，产生所需的控制力矩。

硬件架构设计：模块化与集成化的平衡

Cubli_Mini的硬件设计体现了模块化与集成化的完美平衡。整个系统分为控制板、传感器模块和机械结构三大模块。

控制板设计：ESP32双核心架构

主控板采用双ESP32架构，每颗ESP32提供6路电机PWM输出，共控制三个飞轮电机。这种设计解决了单颗ESP32 PWM通道不足的问题，同时通过CAN总线实现双MCU间的通信。

控制板集成了以下关键电路：

• 电机驱动电路：基于SimpleFOC开源电路设计，支持三相无刷电机驱动
• 电源管理模块：锂电池充放电电路和降压稳压电路
• CAN通信接口：实现双MCU间的高速数据交换
• 传感器接口：I2C接口连接MPU6050和AS5600传感器

传感器系统：多传感器数据融合

Cubli_Mini采用多传感器融合方案，确保姿态控制的精度和稳定性：

MPU6050惯性测量单元MPU6050提供三轴加速度计和三轴陀螺仪数据，用于实时检测立方体的姿态和角速度。模块采用QFN24 4x4封装，通过I2C接口与主控通信，采样频率可达1000Hz。

AS5600磁编码器AS5600磁编码器提供12位（4096步）角度分辨率，用于精确测量飞轮转速和位置。采用I2C接口，支持绝对位置检测，无需校准零位。

机械结构设计：轻量化与高强度

Cubli_Mini的机械结构采用模块化设计，主要组件包括：

• 立方体框架：10x10x10cm尺寸，采用玻纤板材料，保证刚性的同时控制成本
• 飞轮组件：304不锈钢材质，提供足够的转动惯量
• 电机支架：3D打印结构件，确保电机安装精度
• 角件连接：阳极氧化铝柱连接，兼顾美观和强度

软件架构：实时多任务控制系统

FreeRTOS任务调度

在2.Firmware/Mcu1/src/main.cpp中，系统基于FreeRTOS实现多任务调度：

// core 0 - ESP32官方将WIFI、蓝牙类运行在Core0 xTaskCreatePinnedToCore(WifiCommanderTask, "WifiCommanderTask", 20000, NULL, 6, NULL, 0); // core 1 - 控制任务运行在Core1 xTaskCreatePinnedToCore(ModecontrolTask, "ModecontrolTask", 10000, NULL, 6, NULL, 1); xTaskCreatePinnedToCore(ControlTask, "ControlTask", 20000, NULL, 3, NULL, 1); xTaskCreatePinnedToCore(SensorUpdateTask, "ImulTask", 10000, NULL, 2, NULL, 1);

这种双核心任务分配确保了控制系统的实时性，WiFi通信等非实时任务在Core0运行，而关键的控制算法和传感器数据处理在Core1运行。

控制模式与状态机

系统支持多种控制模式，在cubli_mini.h中定义：

enum ControlMode_e { eNORMAL = 0x00, ePOINT_BALANCE = 0x01, // 点平衡模式 eUNILATERAL_BALANCE = 0x02, // 边平衡模式 ePOINT_CALIBRATION = 0x03, // 点平衡校准 eUNILATERA_CALIBRATION = 0x04, // 边平衡校准 eMOTOR_TEST = 0x05 // 电机测试模式 };

关键技术挑战与解决方案

挑战一：传感器数据同步与融合

问题：MPU6050的加速度计数据存在高频噪声，陀螺仪存在低频漂移，AS5600编码器数据需要与姿态数据同步。

解决方案：

1. 采用互补滤波算法融合加速度计和陀螺仪数据
2. 实现传感器数据的时间戳同步
3. 建立四元数姿态表示，避免欧拉角奇异性问题

挑战二：电机控制精度与响应速度

问题：飞轮需要快速响应姿态变化，同时保持精确的速度控制。

解决方案：

1. 采用SimpleFOC库实现磁场定向控制（FOC）
2. 使用AS5600编码器实现闭环速度控制
3. 优化PWM频率和死区时间，提高控制精度

挑战三：系统稳定性与保护机制

问题：立方体在平衡过程中可能出现过冲或失控。

解决方案：

#define STATIC_ANGLE_PROTECTION_THRESHOLD 5 #define BALABCE_PROTECTION_THRESHOLD 15 #define VOLTAGE_LIMIT 11

系统设置了多重保护机制，包括静态角度保护、平衡保护阈值和电压限制，确保系统安全运行。

参数调优与性能优化

控制参数调整

在2.Firmware/Mcu1/include/control/param.h中定义了关键控制参数：

• 角度比例增益：控制姿态恢复的响应速度
• 角速度阻尼系数：抑制振荡的阻尼项
• 速度反馈增益：基于飞轮转速的稳定项

实时调参接口

系统提供UART和WiFi两种调参方式，通过serial_commander和wifi_commander类实现。用户可以通过串口命令或网络调试助手实时调整控制参数，优化系统性能。

与其他自平衡项目的技术对比

相比传统的倒立摆或平衡车项目，Cubli_Mini具有以下技术特点：

1. 三维姿态控制：传统项目多为二维平衡，Cubli_Mini实现三维空间的全姿态控制
2. 飞轮动量控制：通过飞轮角动量变化产生控制力矩，而非直接驱动轮子
3. 无接触点平衡：能够在立方体的角点上实现平衡，展示更复杂的控制算法
4. 低成本实现：总成本控制在800元以内，降低了技术门槛

技术实现要点总结

Cubli_Mini项目的成功实现依赖于以下几个关键技术要点：

1. 精确的姿态解算：基于MPU6050的AHRS算法提供稳定的姿态信息
2. 高效的LQR控制：多变量最优控制算法实现快速稳定的平衡控制
3. 硬件协同设计：控制板、传感器和机械结构的紧密配合
4. 实时任务调度：FreeRTOS确保控制算法的实时性
5. 模块化软件架构：清晰的代码结构便于调试和扩展

未来技术发展方向

基于当前架构，Cubli_Mini可以进一步优化：

1. 自适应控制算法：根据负载变化自动调整控制参数
2. 机器学习优化：使用强化学习优化控制策略
3. 无线充电技术：实现无接触充电，延长工作时间
4. 多机协同控制：多个Cubli_Mini协同完成复杂任务

Cubli_Mini不仅是一个完整的自平衡机器人实现，更是一个优秀的控制算法学习和硬件设计平台。通过深入分析其技术实现，开发者可以掌握现代机器人控制的核心技术，为更复杂的机器人系统开发奠定基础。

【免费下载链接】Cubli_Mini项目地址: https://gitcode.com/gh_mirrors/cu/Cubli_Mini