平衡车控制系统，其平衡实现依赖于高实时性传感器采样、高效姿态解算算法、精准电机控制三者的闭环协作。
一、硬件架构
模块 | 关键组件 | 功能说明 | | 主控MCU | PIC32MZ EF系列（带FPU和DSP指令） | 100MHz+主频，硬件浮点加速，实时处理传感器数据 | | 姿态传感器 | MPU6050（六轴IMU） | 集成陀螺仪+加速度计，I2C通信 | | 电机驱动 | MCPWM模块 + DRV8301驱动芯片 | 生成6路PWM，驱动无刷电机 | | 编码器接口 | QEI模块（正交编码器接口） | 实时读取电机转速/位置 | | 通信接口 | UART/USB（调试） + CAN（扩展） | 数据传输与参数校准 |
二、软件实现流程
1. 传感器数据采集与融合
// 使用Microchip Harmony框架配置I2C读取MPU6050
void IMU_ReadData(void) {
uint8_t raw_data[14];
I2C_Read(MPU6050_ADDR, ACCEL_XOUT_H, raw_data, 14); // 读取14字节原始数据
// 解析加速度计数据 (范围: ±2g)
float accel_x = (int16_t)((raw_data[0]<<8) | raw_data[1]) / 16384.0f;
float accel_z = (int16_t)((raw_data[4]<<8) | raw_data[5]) / 16384.0f; // Z轴垂直重力
// 解析陀螺仪数据 (范围: ±250°/s)
float gyro_y = (int16_t)((raw_data[8]<<8) | raw_data[9]) / 131.0f; // Y轴角速度（俯仰）
// 互补滤波融合
float dt = 0.01f; // 10ms采样周期
static float pitch_angle = 0.0f;
float accel_pitch = atan2(accel_x, accel_z) * 180/M_PI; // 加速度计计算倾角
pitch_angle = 0.98f * (pitch_angle + gyro_y * dt) + 0.02f * accel_pitch;
}
2. 双环控制算法实现
// 直立环（PD控制）
float Balance_Control(float pitch, float gyro_y) {
float Kp = -12.0f; // 比例系数（负号表示反向调节）
float Kd = -0.25f; // 微分系数
return Kp * pitch + Kd * gyro_y; // 输出电机基础控制量
}
// 速度环（PI控制 + 积分抗饱和）
float Velocity_Control(float target_speed, float current_speed) {
static float integral = 0;
float Kp = 0.8f, Ki = 0.05f;
float error = target_speed - current_speed;
// 积分限幅 (±100)
if (fabs(error) < 50) integral += error;
else integral = 0;
return Kp * error + Ki * integral; // 输出速度补偿量
}
// 总控制量合成
void Motor_Output(void) {
float pitch = IMU_GetPitch(); // 获取当前俯仰角
float gyro_y = IMU_GetGyroY(); // 获取Y轴角速度
float speed = QEI_GetSpeed(); // 编码器获取当前车速
float balance_out = Balance_Control(pitch, gyro_y);
float speed_out = Velocity_Control(0, speed); // 目标速度=0（平衡点）
float total_out = balance_out + speed_out;
// 转换为PWM占空比（范围：-1000~+1000）
int16_t pwm = (int16_t)(total_out * 10);
MCPWM_SetDutyCycle(MOTOR_L, pwm); // 左电机输出
MCPWM_SetDutyCycle(MOTOR_R, pwm); // 右电机输出
}
先调直立环：增大 Kp 直至车身能勉强站立，再增大 Kd 抑制振荡
后调速度环：从较小 Ki 开始，避免积分累积导致失控
3. 电机精准驱动（利用MCPWM模块）
// 配置MCPWM（中心对齐模式，死区时间保护）
void PWM_Init(void) {
MCPWM_ConfigTimeBase(MCPWM_MODULE1, MCPWM_TIMER2,
10000, // PWM频率10kHz
MCPWM_TIME_BASE_CLOCK_PBCLK,
MCPWM_TIME_BASE_MODE_CONTINUOUS);
// 设置死区时间（防MOSFET直通）
MCPWM_ChannelDeadTimeSetup(MCPWM_CHANNEL1, 100, 100); // 100ns死区
}
// 更新PWM输出
void MCPWM_SetDutyCycle(MCPWM_CHANNEL ch, int16_t duty) {
duty = constrain(duty, -1000, 1000); // 限幅
MCPWM_ChannelPrimaryDutySet(ch, abs(duty)); // 设置占空比
MCPWM_ChannelOutputPolaritySet(ch, (duty >= 0) ? MCPWM_OUT_POL_ACTIVE_HIGH : MCPWM_OUT_POL_ACTIVE_LOW); // 设置方向
}
三、Microchip方案优势
硬件加速
PIC32MZ的硬件FPU/DSP指令加速矩阵运算（如卡尔曼滤波）。
专用外设
MCPWM模块支持互补PWM、死区插入、故障保护，简化电机驱动设计。
QEI模块自动解码编码器信号，减轻CPU负担。
实时性保障
通过中断优先级配置，确保IMU数据读取（1kHz）> 控制计算（500Hz）> PWM更新（10kHz）的时序链。
四、安全与优化设计
故障保护
// 硬件看门狗+软件超限保护
if (fabs(pitch) > 30.0f) { // 倾角过大
MCPWM_ChannelFaultStateSet(MCPWM_CHANNEL1, MCPWM_FAULT_DISABLE); // 立即关闭电机
Watchdog_Reset(); // 触发看门狗复位
}
待机时切换至Sleep模式，IMU中断唤醒（电流 < 1mA）。
| 
楼主
标题置顶
标题高亮
