西南交大-电子技术课程设计：主动稳定云台

题目介绍：

主动稳定云台是一种电子与机械结合的装置，在云台的基座姿态发生变化时，云台能够检 测到姿态变化，并驱动相应的运动部件使云台的的输出保持稳定。运用于坦克、战舰上，主动 稳定云台可以在坦克战舰行进中保持炮塔的姿态稳定，提高射击精度；运用于摄影则可以保证 摄影师在跑动过程摄像机的稳定，使拍摄的画面稳定。 本课题的主要工作：使用高性能单片机 ESP32 为控制器，设计并实现一个主动平衡云台。 电路结构如下图：

 

六轴（三轴重力加速度和三轴陀螺仪）姿态传感器型号为 MPU6500，安装在云台的顶部， 加速度及陀螺仪数据通过数据总线传输给单片机，单片机对数据进行解算，求解出云台输出端 的倾斜角度，产生特定占空比的 PWM 波给舵机，使舵机转动，调平云台输出端。 课题的工作主要集中在编写 ESP32 上的数据读取、解算、控制程序，另外还有基本的电 路设计、电路板设计、焊接、调试。 一、基本要求 1． 设计并制作专用 PCB， ESP32 模块通过插针和插座安装到所做 PCB 上。 2． MPU6500 模块固定在云台顶端，用杜邦线引出并插到所做 PCB 的插座上。 3． 舵机线插到所做 PCB 的插座上。 4． 云台顶端固定安装一个点状激光头，光点照射在 3 米外的墙上，云台底座滚动 90 度范 围内，光点在垂直范围偏移不超过+10 厘米。偏移距离越小越好。 5． 响应速度越快越好。 6． 整机使用 USB 供电。

器件说明：

开发板esp32见：esp32

MPU-6500：六轴运动处理传感器，在大小为3.0×3.0×0.9mm的芯片上，通过QFN 封装（无引线方形封装），集成了 3 轴 MEMS 陀螺仪，3 轴 MEMS加速度计，以及一个数字运动处理器 DMP（ Digital Motion Processor）。还可以通过辅助I2C端口与多个非惯性数字传感器（例如压力传感器、磁力计）进行连接。

舵机SG90:位置（角度）伺服的驱动器，适用于那些需要角度不断变化并可以保持的控制系统。其本质是一个伺服电机。G90 的主要电气参数：使用电压： 4.8V - 6V，尺寸： 221.5mm x 11.8mm x 22.7mm，重量： 9g，角度范围：0-180°，舵机的转动的角度是通过调节 PWM（脉冲宽度调制）信号的占空比来实现的，标准 PWM（脉冲宽度调制）信号的周期固定为 20ms（50Hz）

重要库函数：

1.采用了mpu6500自带的dmp进行姿态演算。可以快速方便地计算出四元数，重力加速度和三个姿态角yaw,roll,pitch。避免了繁琐复杂的姿态角计算，并且精确度较高。

注：esp32解算数据（欧拉角计算与舵机偏转角度的计算）与控制信号输出算法：通过mpu6500自带的dmp,可以快速计算出四元数，重力向量和三个姿态角yaw,roll,pitch。且yaw开始时为随机值，故跳过自校准过程的前300个读数。通过esp32servo库中的read函数可以读取舵机当前的角度。在水平方向上，用舵机当前角度加上yaw角进行补偿，即可保持水平方向上的稳定。在竖直方向上，用舵机当前角度加上roll角进行补偿，即可保持竖直方向上的稳定。

2.本方案实现控制云台采用esp32servo库中的函数write和read：使云台能够转到指定的角度和可以读出云台现在的角度。

引脚锁定：

MPU6500只使用VCC、GND 、SCL（接esp32的引脚22）、SDA（接esp32的引脚21）四个引脚。

舵机的三根线全部使用，分别接Vin,GND,ESP32的D12,D13引脚（输出pwm信号控制两个舵机转动）

本贴不给出pcb设计。参考帖见评论。

主要代码（并不完整）：

void setup()
{
  Wire.begin();
  Wire.setClock(400000);
  Serial.begin(115200); 

  mpu.initialize();
  mpu.dmpInitialize();
  mpu.CalibrateAccel(6); 
  mpu.CalibrateGyro(6);
  mpu.PrintActiveOffsets();
  mpu.setDMPEnabled(true);

  // 分配硬件定时器,有0，1，2，3四个定时器
  ESP32PWM::allocateTimer(0);   
  // 设置频率
  my_servo.setPeriodHertz(50);
  my_servo1.setPeriodHertz(50);

  // 关联 servo 对象与 GPIO 引脚，设置脉宽范围
  my_servo.attach(SERVO_PIN, MIN_WIDTH, MAX_WIDTH);
  my_servo1.attach(SERVO_PIN1, MIN_WIDTH, MAX_WIDTH);

  my_servo1.write(90);
  my_servo.write(0);
  delay(50);
}

void loop()
{
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
  {
    mpu.dmpGetQuaternion(&q, fifoBuffer);// 获取四元数数据
    mpu.dmpGetGravity(&gravity, &q);//获取重力向量
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    
    static const float dt = 8.0 / 1000;
    static const  float ZERO_OFFSET_COUN (1 / dt);
    static int g_GetZeroOffset = 0;
    static float gyroZ_offset = 0.0f;

    ypr[0]=ypr[0] * 180 / M_PI;
    ypr[1]=ypr[1] * 180 / M_PI;
    ypr[2]=ypr[2] * 180 / M_PI;

   if(j<=300)
   {
     correct=ypr[0];
     correct2=ypr[2];
     temp1=correct;
     j++;
   }

   else
   {
     ypr[0]=ypr[0]-correct;
     ypr[2]=ypr[2]-correct2;

// 间隔8ms一次采样，统计125次，共1秒时间
  if (g_GetZeroOffset++ < ZERO_OFFSET_COUN)
	   gyroZ_offset += ypr[0] * dt;  // 每次8%积分，累计加权125次

      // 除去零偏
  if (g_GetZeroOffset > ZERO_OFFSET_COUN) // 统计完零偏后开始累计偏角
    ypr[0] -= gyroZ_offset; 

    int targetAngle =my_servo.read()+ypr[2];
    my_servo.write(targetAngle);

   // 将目标位置限制在舵机允许范围内
    int targetAngle1 = my_servo1.read() + ypr[0]; 
    if(targetAngle1<=180&&targetAngle1>=-180)
    my_servo1.write(targetAngle1);

    delay(50);
   }
  }
}

 

源文件请看个人主页的资源。