如何自制自平衡云台基于mpu6050，arduino输出三维倾斜角度的方法（含源码,库）

智能小制作(含源码、库)-自平衡云台-输出三维倾斜角度，基于mpu6050、arduino

准备知识

介绍、思路

当你需要保持一个物品的平衡，或者需要得到物品倾斜的角度，不妨看看下面文章，下面内容即是实现这一功能

功能：
当你的立足的空间倾斜或者角度改变时，在自平衡稳定器将保持平衡或保持一定角度，防止倾斜导致不利的结果。

稍微修改也可以用于输出空间俯仰、滚转、偏航三个方向的倾斜角度

实现思路

通过处理arduino使用六轴姿态传感器得到的原始数据，得到空间俯仰、滚转、偏航三个方向的倾斜角度，进而arduino控制舵机的角度，达到自我平衡的效果，当然这里也可以使用LCD、OLED等输出俯仰、滚转、偏航三个方向的倾斜角度

示例这里使用的舵机可能或出现抖动的情况，需要要求精度，可以使用伺服电机或其他设备

mpu6050六轴姿态传感器介绍

MPU-6000为全球首例整合性6轴运动处理组件，整合了3轴陀螺仪、3轴加速器，解决了组合陀螺仪与加速器时存在的一些问题问题，还包含了内建的温度感测器、DMP（Digital Motion Processor）

DMP：可以直接输出四元数，减少复杂的融合演算数据、感测器同步化、姿势感应等的负荷

MPU-6000的角速度感测范围为±250、±500、±1000与±2000°/sec (dps)，可以准确捕捉快速与慢速动作，用户可程式控制的加速器全格感测范围为±2g、±4g±8g与±16g。产品传输可透过最高至400kHz的I2C或最高达20MHz的SPI。

应用

1. 现实增强

2. 电子稳像 (EIS: Electronic Image Stabilization)

3. 光学稳像(OIS: Optical Image Stabilization)

4. 行人导航器

5. 姿势快捷方式

6. 平衡车

7. 飞行器呀

8. 双足机器人

其他硬件介绍

由于在我的其他博客已经写过，就不在这里啰嗦了

舵机介绍：
创客小制作(含源代码)《RFID控制器》，用于智能门禁、物流追踪、控制开关等，基于Arduino

arduino介绍：
Arduino小白入门全解，学习笔记

LCD介绍：
Arduino实践（二）lcd1602使用说明，源码

制作

所需材料

1. Arduino
2. mpu6050六轴姿态传感器
3. 舵机 —或更高级的伺服电机
4. 支架（自行选择）
5. 杜邦线
   可选
   LCD、OLED

接线

MPU6050 引脚说明

MPU6050 引脚	Arduino引脚
VCC	5V
GND	地线
SCL	MPU6050作为从机时IIC时钟线
SDA	MPU6050作为从机时IIC数据线
XCL	MPU6050作为主机时IIC时钟线
XDA	MPU6050作为主机时IIC数据线
AD0	地址管脚，该管脚决定了IIC地址的最低一位
INT	中断引脚

在本项目中只需要接VCC、GND 、SCL、SDA

舵机引脚说明

所有的舵机都有外接三根线，分别用棕、红、橙(黄)三种颜色进行区分，由于舵机品牌不同，颜色也会有所差异，棕色为接地线，红色为电源正极线，橙(黄)色为信号线

关于支架根据自己选择的支架安装，这里不详细说明

库文件

MPU6050.zip
https://download.csdn.net/download/GuanFuXinCSDN/12248710

I2Cdev.zip
https://download.csdn.net/download/GuanFuXinCSDN/12248704

说明

这两个zip文件内都包含了.h 和 .cpp文件，将两个文件解压放到arduino代码的同目录下即可正常使用

感谢阅读

持续更新arduino 树莓派 python Linux c语言等等
接到有帮助的话 求点赞 求关注❤️ 求分享
有问题可以评论
点关注，不迷路
如果有任何错误，如何建议，请批评指教，不胜感激 ！

源代码

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
 /*舵机*/
#include 
Servo myservo;  //创建一个舵机控制对象
Servo myservo2;  //创建一个舵机控制对象
// 使用Servo类最多可以控制8个舵机
MPU6050 accelgyro;
 
unsigned long now, lastTime = 0;
float dt;                                          //微分时间
 
int16_t ax, ay, az, gx, gy, gz;                   //加速度计和陀螺仪的原始数据
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0;    //角度变量
long axo = 0, ayo = 0, azo = 0;                   //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0;                   //陀螺仪偏移量
 
float pi = 3.1415926; 
float AcceRatio = 16384.0;                       //加速度计比例系数
float GyroRatio = 131.0;                          //陀螺仪比例系数
 
uint8_t n_sample = 8;                            //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0};         //x,y轴采样队列
long aax_sum, aay_sum,aaz_sum;                       //x,y轴采样和
 
float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0}; 
float Px=1, Rx, Kx, Sx, Vx, Qx;             //x轴卡尔曼变量
float Py=1, Ry, Ky, Sy, Vy, Qy;             //y轴卡尔曼变量
float Pz=1, Rz, Kz, Sz, Vz, Qz;             //z轴卡尔曼变量
 
void setup()
{    myservo.attach(9);  // 该舵机由arduino第九脚控制
      myservo2.attach(8);  // 该舵机由arduino第八脚控制
    Wire.begin();
    Serial.begin(115200);
 
    accelgyro.initialize();                 //初始化
 
    unsigned short times = 200;             //采样次数
    for(int i=0;i<times;i++)
    {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
        axo += ax; ayo += ay; azo += az;      
        gxo += gx; gyo += gy; gzo += gz;
    
    }
    
    axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
    gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
}
 
void loop()
{
    unsigned long now = millis();              //当前时间
    dt = (now - lastTime) / 1000.0;             //微分时间
    lastTime = now;                            //上一次采样时间
 
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  //读取六轴原始数值
 
    float accx = ax / AcceRatio;                //x轴加速度
    float accy = ay / AcceRatio;                //y轴加速度
    float accz = az / AcceRatio;                //z轴加速度
 
    aax = atan(accy / accz) * (-180) / pi;    //y轴对于z轴的夹角
    aay = atan(accx / accz) * 180 / pi;       //x轴对于z轴的夹角
    aaz = atan(accz / accy) * 180 / pi;       //z轴对于y轴的夹角
 
    aax_sum = 0;                               // 对于加速度计原始数据的滑动加权滤波算法
    aay_sum = 0;
    aaz_sum = 0;
  
    for(int i=1;i<n_sample;i++)
    {
        aaxs[i-1] = aaxs[i];
        aax_sum += aaxs[i] * i;
        aays[i-1] = aays[i];
        aay_sum += aays[i] * i;
        aazs[i-1] = aazs[i];
        aaz_sum += aazs[i] * i;
    
    }
    
    aaxs[n_sample-1] = aax;
    aax_sum += aax * n_sample;
    aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0;    //角度调幅至0-90°
    aays[n_sample-1] = aay;                          //此处应用实验法取得合适的系数
    aay_sum += aay * n_sample;                        //本例系数为9/7
    aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
    aazs[n_sample-1] = aaz; 
    aaz_sum += aaz * n_sample;
    aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;
 
    float gyrox = - (gx-gxo) / GyroRatio * dt;     //x轴角速度
    float gyroy = - (gy-gyo) / GyroRatio * dt;    //y轴角速度
    float gyroz = - (gz-gzo) / GyroRatio * dt;    //z轴角速度
    agx += gyrox;                                //x轴角速度积分
    agy += gyroy;                             //x轴角速度积分
    agz += gyroz;
    
    /* kalman start */
    Sx = 0; Rx = 0;
    Sy = 0; Ry = 0;
    Sz = 0; Rz = 0;
    
    for(int i=1;i<10;i++)
    {                                          //测量值平均值运算
        a_x[i-1] = a_x[i];                        //即加速度平均值
        Sx += a_x[i];
        a_y[i-1] = a_y[i];
        Sy += a_y[i];
        a_z[i-1] = a_z[i];
        Sz += a_z[i];
    
    }
    
    a_x[9] = aax;
    Sx += aax;
    Sx /= 10;                                  //x轴加速度平均值
    a_y[9] = aay;
    Sy += aay;
    Sy /= 10;                                  //y轴加速度平均值
    a_z[9] = aaz; 
    Sz += aaz;
    Sz /= 10;
 
    for(int i=0;i<10;i++)
    {
        Rx += sq(a_x[i] - Sx);
        Ry += sq(a_y[i] - Sy);
        Rz += sq(a_z[i] - Sz);
    
    }
    
    Rx = Rx / 9;                               //得到方差
    Ry = Ry / 9;                        
    Rz = Rz / 9;
  
    Px = Px + 0.0025;                           // 0.0025在下面有说明...
    Kx = Px / (Px + Rx);                       //计算卡尔曼增益
    agx = agx + Kx * (aax - agx);              //陀螺仪角度与加速度计速度叠加
    Px = (1 - Kx) * Px;                        //更新p值
 
    Py = Py + 0.0025;
    Ky = Py / (Py + Ry);
    agy = agy + Ky * (aay - agy); 
    Py = (1 - Ky) * Py;
  
    Pz = Pz + 0.0025;
    Kz = Pz / (Pz + Rz);
    agz = agz + Kz * (aaz - agz); 
    Pz = (1 - Kz) * Pz;
 
 
    Serial.print(agx);Serial.print(",");
    Serial.print(agy);Serial.print(",");
    Serial.print(agz);Serial.println();
    myservo.write(agx);        // 指定舵机转向的角度
    delay(15);                       // 等待15ms让舵机到达指定位置
    myservo2.write(agy);        // 指定舵机转向的角度
    delay(15);                       // 等待15ms让舵机到达指定位置
    
}