如何自制自平衡云台基于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. 双足机器人

在这里插入图片描述
在这里插入图片描述

其他硬件介绍

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

在这里插入图片描述

制作

所需材料

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

接线

MPU6050 引脚说明

MPU6050 引脚Arduino引脚
VCC5V
GND地线
SCLMPU6050作为从机时IIC时钟线
SDAMPU6050作为从机时IIC数据线
XCLMPU6050作为主机时IIC时钟线
XDAMPU6050作为主机时IIC数据线
AD0地址管脚,该管脚决定了IIC地址的最低一位
INT中断引脚

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

舵机引脚说明

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

舵机是一种位置伺服的驱动器,其工作原理是由接收机或者单片机发出信号给舵机,其内部有一个基准电路,产生周期为20ms,宽度为1.5ms 的基准信号,将获得的直流偏置电压与电位器的电压比较,获得电压差输出。经由电路板上的IC 判断转动方向,再驱动无核心马达开始转动,透过减速齿轮将动力传至摆臂,同时由位置检测器送回信号,判断是否已经到达定位。适用于那些需要角度不断变化并可以保持的控制系统。当电机转速一定时,通过级联减速齿轮带动电位器旋转,使得电压差为0,电机停止转动。一般舵机旋转的角度范围是0 度到180 度。

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

库文件

在我的主页文件中

说明

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

源代码

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
 /*舵机*/
#include <Servo.h>
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让舵机到达指定位置
    
}
  • 39
    点赞
  • 271
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 33
    评论
评论 33
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

GuanFuXinCSDN

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值