嵌入式硬件篇---陀螺仪｜PID

---

---

前言

在STM32F103RCT6上使用陀螺仪（如MPU6050）结合PID算法实现麦克纳姆轮小车的车身稳定控制（如自平衡或抗倾斜），需要融合传感器数据、姿态解算和电机协同控制。以下是详细实现步骤：

---

1. 硬件准备

主控芯片

主控芯片：STM32F103RCT6（72MHz Cortex-M3，带硬件I²C和PWM）。

陀螺仪模块

陀螺仪模块：MPU6050（集成3轴加速度计+3轴陀螺仪，I²C接口）。

电机驱动

电机驱动：4个麦克纳姆轮+4个直流电机（需带编码器反馈，如TB6612驱动）。

电源

电源：12V锂电池（需稳压至5V/3.3V供模块使用）。

其他

其他：OLED（显示姿态角，可选）、稳压模块。

2. 硬件连接

MPU6050引脚 STM32引脚 说明
VCC 3.3V 模块供电
GND GND 共地
SDA PB7 I²C数据线
SCL PB6 I²C时钟线
INT PA0 中断引脚（可选）
电机驱动 STM32引脚 说明
PWM1~4 PA8, PA9, PA10, PA11 4路PWM输出
电机方向控制 任意GPIO 如PB0~PB3

3. 软件实现步骤

(1) MPU6050初始化与数据读取

#include "mpu6050.h"
MPU6050_HandleTypeDef hmpu;void MPU6050_Init() {hmpu.i2c = &hi2c1; // I²C初始化略MPU6050_Reset(&hmpu);MPU6050_SetGyroRange(&hmpu, MPU6050_GYRO_RANGE_500DPS);MPU6050_SetAccelRange(&hmpu, MPU6050_ACCEL_RANGE_4G);
}// 读取原始数据（需定期调用，如1kHz）
void Read_MPU6050() {MPU6050_ReadAccel(&hmpu);MPU6050_ReadGyro(&hmpu);
}

(2) 姿态解算（互补滤波或DMP）

互补滤波示例（融合加速度计和陀螺仪数据）：
float pitch_angle = 0; // 俯仰角（前后倾斜）
float gyro_bias = 0;   // 陀螺仪零偏void Update_Attitude() {float dt = 0.001f;   // 假设1kHz调用float accel_pitch = atan2(hmpu.accel_y, hmpu.accel_z) * 180 / M_PI;float gyro_rate = hmpu.gyro_x - gyro_bias; // 陀螺仪X轴角速度// 互补滤波：α为权重（0.98依赖陀螺仪，0.02依赖加速度计）float alpha = 0.98;pitch_angle = alpha * (pitch_angle + gyro_rate * dt) + (1 - alpha) * accel_pitch;
}

(3) PID控制器设计

控制目标：通过调节麦克纳姆轮转速，使车身倾角pitch_angle稳定在0°。
typedef struct {float Kp, Ki, Kd;float target, error, last_error, integral;
} PID_Controller;PID_Controller pid = {2.0, 0.1, 0.5, 0, 0, 0, 0}; // 示例参数float PID_Update(PID_Controller *pid, float current) {pid->error = pid->target - current;pid->integral += pid->error;float derivative = pid->error - pid->last_error;pid->last_error = pid->error;// 输出限幅（防止电机过载）float output = pid->Kp * pid->error + pid->Ki * pid->integral + pid->Kd * derivative;if (output > 100) output = 100;else if (output < -100) output = -100;return output;
}

(4) 麦克纳姆轮协同控制

根据PID输出调整四个轮子的转速和方向，实现车身稳定：

void Stabilize_Car(float pid_output) {
// 假设小车前后倾斜时，前后轮需反向补偿
float front_speed = -pid_output; // 前轮速度
float rear_speed = pid_output; // 后轮速度

// 麦克纳姆轮运动分解（X型布局）
// 轮子顺序：LF, RF, LB, RB
float wheel_speeds[4] = {
front_speed, // LF
front_speed, // RF
rear_speed, // LB
rear_speed // RB
};

// 更新PWM占空比（需根据电机极性调整方向）
for (int i = 0; i < 4; i++) {
uint16_t pwm = (uint16_t)fabs(wheel_speeds[i]);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1 + i, pwm);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0 + i, wheel_speeds[i] > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
}
}

4. 主程序逻辑

int main(void) {HAL_Init();SystemClock_Config();MX_GPIO_Init();MX_I2C1_Init();     // MPU6050MX_TIM1_Init();     // 4路PWM初始化MPU6050_Init();while (1) {Read_MPU6050();             // 读取传感器数据Update_Attitude();           // 更新姿态角float output = PID_Update(&pid, pitch_angle);Stabilize_Car(output);       // 控制电机HAL_Delay(1); // 控制周期1ms（1kHz）}
}

5. 关键优化与调试技巧

(1) 传感器校准

陀螺仪零偏校准：静止状态下采样100次取平均值。void Calibrate_Gyro() {float sum = 0;for (int i = 0; i < 100; i++) {Read_MPU6050();sum += hmpu.gyro_x;HAL_Delay(10);}gyro_bias = sum / 100;
}

(2) PID参数整定

先调P

先调P：增大Kp直到小车能快速响应倾斜但开始振荡。

再调D

再调D：加入Kd抑制振荡（典型值为Kp的0.2~0.5倍）。

最后调I

最后调I：小幅增加Ki消除稳态误差（如车身轻微倾斜）。

(3) 动态稳定性增强

死去处理

死区处理：当倾角小于2°时关闭PID输出，避免电机抖动。

if (fabs(pitch_angle) < 2.0) output = 0;
抗积分饱和：限制积分项累积范围。

6. 扩展功能

(1) 遥控控制叠加

在稳定基础上叠加遥控指令（如前进时保持车身平衡）：

float remote_speed = 0; // 遥控器输入
void Control_Car() {float balance_output = PID_Update(&pid, pitch_angle);wheel_speeds[0] = remote_speed - balance_output; // LFwheel_speeds[1] = remote_speed - balance_output; // RFwheel_speeds[2] = remote_speed + balance_output; // LBwheel_speeds[3] = remote_speed + balance_output; // RB
}

(2) 多传感器融合

加入编码器反馈，实现速度+姿态双闭环控制：float encoder_speed = Read_Encoder(); // 读取编码器速度
float speed_pid = PID_Update(&speed_pid_ctrl, encoder_speed);
output = balance_output + speed_pid;

7. 常见问题解决

问题1：小车剧烈振荡

解决：降低Kp或增加Kd。

问题2：车身缓慢倾斜

解决：增加Ki或检查加速度计校准。

问题3：电机响应延迟

解决：提高PWM频率（如10kHz）或减少控制周期。
通过上述方法，STM32F103RCT6可有效实现麦克纳姆轮小车的车身稳定控制。实际应用中需根据机械结构（如重心高度）和电
机性能调整参数。

---