智能小製作(含源碼、庫)-自平衡雲臺-輸出三維傾斜角度，基於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。
應用
-
現實增強
-
電子穩像 (EIS: Electronic Image Stabilization)
-
光學穩像(OIS: Optical Image Stabilization)
-
行人導航器
-
姿勢快捷方式
-
平衡車
-
飛行器呀
-
雙足機器人
其他硬件介紹
由於在我的其他博客已經寫過，就不在這裏囉嗦了
舵機介紹：
創客小製作(含源代碼)《RFID控制器》，用於智能門禁、物流追蹤、控制開關等，基於Arduino
arduino介紹：
Arduino小白入門全解，學習筆記
LCD介紹：
Arduino實踐（二）lcd1602使用說明，源碼
製作
所需材料
- Arduino
- mpu6050六軸姿態傳感器
- 舵機 —或更高級的伺服電機
- 支架（自行選擇）
- 杜邦線
可選
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.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讓舵機到達指定位置
}