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

---

文章目录

• 前言
• [1. 硬件准备](#1. 硬件准备)
• • 主控芯片
  • 陀螺仪模块
  • 电机驱动
  • 电源
  • 其他
• [2. 硬件连接](#2. 硬件连接)
• [3. 软件实现步骤](#3. 软件实现步骤)
• • [(1) MPU6050初始化与数据读取](#(1) MPU6050初始化与数据读取)
  • [(2) 姿态解算（互补滤波或DMP）](#(2) 姿态解算（互补滤波或DMP）)
  • [(3) PID控制器设计](#(3) PID控制器设计)
  • [(4) 麦克纳姆轮协同控制](#(4) 麦克纳姆轮协同控制)
• [4. 主程序逻辑](#4. 主程序逻辑)
• [5. 关键优化与调试技巧](#5. 关键优化与调试技巧)
• • [(1) 传感器校准](#(1) 传感器校准)
  • [(2) PID参数整定](#(2) PID参数整定)
  • • 先调P
    • 再调D
    • 最后调I
  • [(3) 动态稳定性增强](#(3) 动态稳定性增强)
  • • 死去处理
• [6. 扩展功能](#6. 扩展功能)
• • [(1) 遥控控制叠加](#(1) 遥控控制叠加)
  • [(2) 多传感器融合](#(2) 多传感器融合)
• [7. 常见问题解决](#7. 常见问题解决)
• • 问题1：小车剧烈振荡
  • 问题2：车身缓慢倾斜
  • 问题3：电机响应延迟

---

前言

在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();     // MPU6050
  MX_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; // LF
  wheel_speeds[1] = remote_speed - balance_output; // RF
  wheel_speeds[2] = remote_speed + balance_output; // LB
  wheel_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可有效实现麦克纳姆轮小车的车身稳定控制 。实际应用中需根据机械结构（如重心高度）和电
机性能调整参数。

---