【Landzo C1试用体验】+ 第四篇 ☞头部运动追踪第一步：MPU6050控制舵机

论坛

      做飞思卡尔智能车比赛的时候看到平衡组的用过这个6050，当时就很感兴趣，一直想玩一下。
      后来我就想这个东西是不是可以用来做头部运动的追踪，没想到经过一天的查资料，在别的论坛这些早都被玩过了。
      经过一天的学习，我实现了我想要的功能，完整的头追要等开学以后才能发帖，我回老家只带了这么点东西回来。
   下面进入正题，
     1.什么是MPU6050?

      MPU6050是一种非常流行的空间运动传感器芯片，可以获取器件当前的三个加速度分量和三个旋转角速度。由于其体积小巧，功能强大，精度较高，不仅被广泛应用于工业，同时也是航模爱好者的神器，被安装在各类飞行器上驰骋蓝天。
  2.所使用套件中的蓝色舵机简介

      蓝宙的PDF里已经给的很全了，这里不做赘述。
  3.接线示意图

      另外的舵机按照上一节图片中相应去接就可以了。
  4.俯仰角和横滚角

      通过阅读上述资料，以及具体的实验观察现象，其对应关系为pitch（俯仰角）和roll（横滚角）具体方法是使用卡尔曼滤波算法中的例程《MPU6050》，这个例程我会上传到附件中。把程序下载到arduino，再打开串口监视器，（这里注意要把波特率修改为115200，和程序中的设定同步，不然你看到的将会是乱码），我们就可以看到第二张图中定义的几个量了。如果需要显示acc和gyro则把程序中的0改为1。
  5.具体实现代码
      通过对第四节和第二节的学习，我们知道舵机的角度是0~180度，而我们通过串口监视器观察到的roll大概是在-90度~90度之间。为了实现同步，在舵机赋值的时候我们把roll加个90再附进去就差不多了。手上硬件全的话，你们就根据实际情况矫正的精准一些，我这里暂时没有这个必要了。我在附件中上传了实验效果的视频，有兴趣的同学可以查看。再添加舵机语句的时候要注意放的位置，不要放到某一个if判断的末尾。添加的语句已经标注为红色。

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
#include
#include
#include // Source: https://github.com/TKJElectronics/KalmanFilter
Servo servo_pin_10;
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
// TODO: Make calibration routine
void setup() {
servo_pin_10.attach(10);
Serial.begin(115200);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
void loop() {
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
servo_pin_10.write(roll+ 90);
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("t");
Serial.print(accY); Serial.print("t");
Serial.print(accZ); Serial.print("t");
Serial.print(gyroX); Serial.print("t");
Serial.print(gyroY); Serial.print("t");
Serial.print(gyroZ); Serial.print("t");
Serial.print("t");
#endif
Serial.print(roll); Serial.print("t");
Serial.print(gyroXangle); Serial.print("t");
Serial.print(compAngleX); Serial.print("t");
Serial.print(kalAngleX); Serial.print("t");
Serial.print("t");
Serial.print(pitch); Serial.print("t");
Serial.print(gyroYangle); Serial.print("t");
Serial.print(compAngleY); Serial.print("t");
Serial.print(kalAngleY); Serial.print("t");
#if 0 // Set to 1 to print the temperature
Serial.print("t");
double temperature = (double)tempRaw / 340.0 + 36.53;
Serial.print(temperature); Serial.print("t");
#endif
Serial.print("rn");
delay(2);
}

复制代码

  6.应用展望
      如果我手上有两个舵机，那么水平面和竖直面就都可以转动，配合上程序，我们就可以远程的更加人性化的控制摄像头的转动，再把图像显示在头盔中，从而改善现有的监控设备；
      如果我把它装在一辆联网的载具上，就可以远程看到正常人坐在载具中驾驶室的图像，配合上远程操控设备从而时间远程驾驶；
      如果我们在人的鞋子里装上加速度计，通过不同的值来换算成小车的移动速度以及轨迹，再配合上其他传感器，那么，是不是就有可能做出一台跟着人同步走的车？进而我们就可以把书包什么的都丢到车身上，然后我们就可以跑步回家锻炼身体而不是挤公交了。

antimatter2333 · 2016-8-9 20:02:08

担心给读者的感觉不够直接，上图，

yuanrk810 · 2016-9-23 08:32:52

学习了，感谢分享！！

雷翌仁 · 2016-10-31 14:33:36

謝謝樓主分享！

好的够了没有 · 2016-11-2 22:32:07

我只想把这个装在hmz t1上把它改造成vr，要怎么没实现呢，需要什么硬件和软件呢

antimatter2333 · 2016-11-3 13:20:43

好的够了没有发表于 2016-11-2 22:32
我只想把这个装在hmz t1上把它改造成vr，要怎么没实现呢，需要什么硬件和软件呢

我不知道hmz t1是什么啊

h1654155956.8608 · 2016-11-8 14:15:33

卡尔曼滤波不错，是要找的。

hellolgml · 2016-11-14 16:24:26

好玩意

hellolgml · 2016-11-14 16:35:30

antimatter2333 发表于 2016-8-9 20:02
担心给读者的感觉不够直接，上图，

程序编译错误

老张 · 2016-12-1 09:56:35

在这找到了卡尔曼滤波，十分谢谢！

antimatter2333 · 2016-12-19 13:36:05

zjy586 发表于 2016-12-1 09:56
在这找到了卡尔曼滤波，十分谢谢！

有用就好，没想到还真能帮到别人呢

however1241 · 2016-12-28 09:35:19

謝謝樓主分享！

andrew · 2017-2-18 20:55:15

很好，谢谢

我是胖子aaaaaaaa · 2017-3-5 19:03:27

则个真么下载工

我是胖子aaaaaaaa · 2017-3-5 19:12:14

编译错误。这什么原因啊

Arduino: 1.5.4 (Windows 7), Board: "Arduino Uno"

sketch_mar05a:27: error: 'Kalman' does not name a type
sketch_mar05a:28: error: 'Kalman' does not name a type
sketch_mar05a.ino: In function 'void setup()':
sketch_mar05a:58: error: 'i2cWrite' was not declared in this scope
sketch_mar05a:59: error: 'i2cWrite' was not declared in this scope
sketch_mar05a:61: error: 'i2cRead' was not declared in this scope
sketch_mar05a:70: error: 'i2cRead' was not declared in this scope
sketch_mar05a:86: error: 'kalmanX' was not declared in this scope
sketch_mar05a:87: error: 'kalmanY' was not declared in this scope
sketch_mar05a.ino: In function 'void loop()':
sketch_mar05a:98: error: 'i2cRead' was not declared in this scope
sketch_mar05a:122: error: expected primary-expression before '<' token
sketch_mar05a:122: error: 'font' was not declared in this scope
sketch_mar05a:122: error: expected `;' before 'color'
sketch_mar05a:122: error: expected primary-expression before '<' token
sketch_mar05a:122: error: expected primary-expression before '/' token
sketch_mar05a:124: error: expected primary-expression before 'double'
sketch_mar05a:124: error: expected `;' before 'double'
sketch_mar05a:130: error: 'kalmanX' was not declared in this scope
sketch_mar05a:135: error: 'kalmanX' was not declared in this scope
sketch_mar05a:135: error: 'gyroXrate' was not declared in this scope
sketch_mar05a:139: error: 'kalmanY' was not declared in this scope
sketch_mar05a:155: error: 'gyroXrate' was not declared in this scope
sketch_mar05a.ino: In function 'void setup()':
sketch_mar05a:204: error: redefinition of 'void setup()'
sketch_mar05a:44: error: 'void setup()' previously defined here
sketch_mar05a.ino: In function 'void loop()':
sketch_mar05a:209: error: redefinition of 'void loop()'
sketch_mar05a:96: error: 'void loop()' previously defined here

  This report would have more information with
  "Show verbose output during compilation"
  enabled in File > Preferences.

何高鹏 · 2017-4-18 08:50:14

LZ编译错误啊

何高鹏 · 2017-4-18 08:52:35

楼主可否加个 QQ好友呢 64029637

h1654155963.7302 · 2017-5-3 19:57:55

编译有错误怎么回事？

h1654155963.7302 · 2017-5-3 19:58:31

Arduino:1.8.2 (Windows 10), 开发板："Arduino/Genuino Uno"

C:UsersAdministratorPicturesCamera Rollsketch_may03asketch_may03a.ino: In function 'void setup()':

sketch_may03a:56: error: 'i2cWrite' was not declared in this scope

while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once

                                       ^

sketch_may03a:57: error: 'i2cWrite' was not declared in this scope

while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

                                 ^

sketch_may03a:59: error: 'i2cRead' was not declared in this scope

while (i2cRead(0x75, i2cData, 1));

                              ^

sketch_may03a:68: error: 'i2cRead' was not declared in this scope

while (i2cRead(0x3B, i2cData, 6));

                              ^

C:UsersAdministratorPicturesCamera Rollsketch_may03asketch_may03a.ino: In function 'void loop()':

sketch_may03a:96: error: 'i2cRead' was not declared in this scope

while (i2cRead(0x3B, i2cData, 14));

                                 ^

sketch_may03a:120: error: expected primary-expression before '<' token

servo_pin_10.write(roll+ 90);

^

sketch_may03a:120: error: 'font' was not declared in this scope

servo_pin_10.write(roll+ 90);

  ^

sketch_may03a:120: error: expected primary-expression before '<' token

servo_pin_10.write(roll+ 90);

                                                      ^

sketch_may03a:120: error: expected primary-expression before '/' token

servo_pin_10.write(roll+ 90);

                                                      ^

sketch_may03a:122: error: expected primary-expression before 'double'

double gyroXrate = gyroX / 131.0; // Convert to deg/s

^

sketch_may03a:133: error: 'gyroXrate' was not declared in this scope

   kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

                                    ^

sketch_may03a:153: error: 'gyroXrate' was not declared in this scope

gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter

               ^

exit status 1
'i2cWrite' was not declared in this scope

在文件 -> 首选项开启
“编译过程中显示详细输出”选项
这份报告会包含更多信息。