双轮自平衡小车制作指南

发表于 更新于 Disqus：

双轮自平衡小车制作指南

整体介绍

这将会是我制作自平衡小车的一个开始，本文的目标在于设计一个简单的自平衡小车，实现自平衡站立和行进。作品将依托于开源硬件平台Arduino，使用C语言进行程序开发。

双轮小车与四轮小车不同，双轮小车本身是一个欠稳定系统。将一个双轮小车直立着放在地面上时，小车难以站立，而是会向前或者向后倾倒。若要使小车维持稳定站立，我们需要引入一个控制系统。当小车将要向前倾倒时，控制电机带动轮胎向前移动，便阻止了小车前倾；同理可知小车后倾时可以向后移动阻止后倾，从而实现小车的平衡。

为了实现上述的控制，首先我们需要实时获取小车的“姿态”，即小车向前倾倒还是向后倾倒，以及倾倒的程度（一般使用角度来表示这个倾倒的程度）。“姿态”的获取需要使用可以测量加速度和角加速度的传感器，这种传感器被称作惯性测量单元 (IMU)，惯性测量单元获取的数据可以帮助我们获取小车“姿态”。

第二步是依据小车的“姿态”，控制电机活动，具体的控制内容包括电机的转速和转向。

材料准备

实现一个最基础的平衡小车需要以下组件：

  • 微控制器：Arduino Nano 开发板
  • 惯性测量单元 (IMU) ：MPU6050模块
  • 电机：
    • N20减速直流电机（带霍尔编码器，详细参数见后文）
    • 双母头电机端子线（型号：ZH1.5MM）
    • 端子线贴片插座（型号：ZH1.5MM）
  • 电源：
    • 7.4V锂电池
    • AMS1117降压电源模块（5V）
    • USB-Typec母头（带PCB板）
  • 外壳：3D打印外壳
  • 电路板：立创EDA设计原理图和PCB板
  • 其他：
    • N20减速直流电机配套固定架和D字轴轮胎
    • 螺丝螺母
    • 面包板和杜邦线（学习和测试时使用，实际小车中不使用）

Arduino Nano 介绍

初始Arduino

Arduino是一款开源的嵌入式硬件开发平台，不仅包括了众多型号的开发板，而且包括了一个集成开发环境（IDE）辅助开发者编程，烧录和通信。Arduino本身提供了许多标准库，这些库的封装程度高，因此用户不必去关心寄存器配置而是可以通过库函数的调用来实现各种功能。Arduino作为一个受欢迎的开源平台，在互联网上可以找到大量的资料和例程进行学习。除了上文提到的标准库之外，Arduino还有各种用户针对不同外设写的库，可以在Arduino中轻松的获取和使用。Arduino本身还提供有图形化编程选项，即便是不会编程的非专业人士也可以快速上手。Arduino帮助我们从嵌入式硬件开发底层的细节中挣脱出来，把注意力放在控制本身这件事上。而且，鉴于本文的希望依托于一个可靠的开源平台且尽可能构造一个简单的自平衡小车，因此Arduino平台成为了我的首选。

进行电子设计时，我们首先要确定控制核心的选择。在这里我们选择了Arduino里面的Nano开发板进行开发。下图是Nano的模型图/引脚图。

image-20220226162323982

第一次接触硬件设计的人可能看到这张图会感到很复杂且不知所措。因此接下来我们将会通过一些实验来认识Nano。在结束这一章时再次回来看这张图片，或许你就会感到亲切了。

实验1——控制

Ardino是一块开源的嵌入式硬件平台，这句话对于初学者来说可能会是一个巨大的困扰，因为嵌入式是何意义，向初学者“正确”的解释清楚这件事并不容易。但是，不理解这段话并不影响我们去使用Arduino Nano来进行一些小开发。我们只需要牢牢记住一件事，那就是Nano是一个微控制器，微就是微小的意思，Nano就是个小型的控制器。控制器就是用来做控制的。接下来我们先从一个简单的实验入手，使用这个控制器去做一些控制。

首先我们先理解控制的概念，我们会希望控制什么呢？可能有人说，控制机器人！或者是控制一个遥控飞机飞行！是的，这些是控制，但是这种难度的控制并不适合一个初学者去学习。其实，控制无处不在，比如，我们按下开关，就可以打开灯，开关就是一个控制器，控制着灯的亮灭。现在，让我们把开关换成Nano，用它来控制一个LED小灯的亮灭。

实验需要准备：

  • Arduino Nano开发板（焊接好排针），Arduino IDE，Micro USB接口的数据线
  • 面包板和杜邦线（双公头）
  • LED灯和电阻

首先，按照下图的方法连接电路。

然后打开Arduino IDE：用数据线将Nano与电脑相连接，选择相应的型号的开发板和端口，在编辑器中写入代码，然后点击编译并烧录，具体操作如下图：

图中代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
LED = 7;               // 给D7引脚重命名为LED

void setup() {
  pinMode(LED, OUTPUT);        // 将名为LED的引脚设置为（数字）输出
}

void loop() {
  digitalWrite(LED, HIGH);       // 向LED引脚进行写入“高”，表示输出高电平
  delay(1000);                       // 延时1000ms即1秒
  digitalWrite(LED, LOW);        // 向LED引脚进行写入“低”，表示输出低电平
  delay(1000);                       // 延时1000ms即1秒
}

完成上述操作后，就能看到LED亮一秒，灭一秒了。

使用模拟量（analogWrite(Pin,Value)函数的本质是PWM驱动）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
const byte GND = 5;
const byte LED = 6;

void setup() {
  pinMode(GND, OUTPUT);
  pinMode(LED, OUTPUT);
}

void loop() {
  analogWrite(GND, 0);
  for (int i = 0; i < 250; i = i + 50)
  {
    analogWrite(LED, i);
    delay(500);
  }
  analogWrite(LED, 0);
  delay(1500);
}

实验2——通信

实验3——

电机基础

了解直流电机

学习和使用电机之前，我们首先要对电机的基本原理有一定的认识。最基础的电机的是我们在中学阶段就学习过的直流电机。直流电机是一个放在磁场中的导线转子，当有直流电流过导线转子时，转子便受力旋转。直流电机通入直流后转子转速很快，但是其扭矩——或者说转动的力度，很小。但是为了能带动轮胎转动实现小车行进，我们需要转子拥有较大的扭矩。于是我们有了减速直流电机。减速直流电机相比于直流电机多了一个减速箱。减速箱内部有许多齿轮，转子转动带动减速箱内部的齿轮转动，这一过程将导致转轴转速降低而扭矩提高，因此我们称其为减速电机。减速电机中最重要的参数是减速比。减速比的大小等于减速箱外的转轴转一圈时直流电机转子转过的圈数。我们使用的N20电机就是一个减速直流电机。

驱动直流电机的方法非常简单，只需要在电机的两个端子上接上正负电压，电机便开始转动。若是交换两端子上的电压，则电机会沿于之前相反的方向转动。对于装有编码器的N20电机来说，标注为M1和M2的端子为直流电机的端子。

为了更全面的理解电机，我们应该了解另一种常见的电机类型——步进电机，以便在其他时候遇到的话不至于与现在所学的电机相混淆。步进电机是一种可以顾名思义的东西，只不过大多数人在最开始想到的含义可以不太正确。步进电机不是专指用来前进或者行进的电机，毕竟很显然的是，只要外界轮子，直流电机也可以前进。步进电机这个名字来源于电机的内部构造，转子在脉冲信号输入下发生一步一步的转动，这种一步一步的转动的效果有点像钟表的秒针，每一秒在表盘上走一小步，一步一步的旋转，最终一分钟后旋转了一圈。如果我们加快这一过程，这样电机就不再是做一顿一顿的运动了，而是一个连续不断的快速的旋转。

步进电机只是转子旋转的原理和直流电机不同，但是依然可以在步进电机外接入减速箱。这两个电机的定语是两种平行的分类方式，这点不要混淆。

我们回到上文的直流电机，如果想要了解更多关于步进电机的知识，可以在搜索引擎上搜索“步进电机”，在这里我只是做简单的介绍。

直流电机有两个端子，施加正负电压即可驱动马达转动，交换正负电压就可以改变马达转向。前者非常简单，只需要提供一个外接直流电源，或者使用Nano的5V引脚和GND引脚就可以驱动电机转动，手动改变电流源极性或者使用微控制器调整输出电压信号都可以改变马达转向。但是如开篇我们所说的，我们需要同时控制电机转速和转向。为此，我们需要学习使用一种要脉冲宽度调制（Pulse-width modulation）的技术来控制电机的转速。下面我们就来详细地了解一下PWM技术，并且使用Nano来控制电机的转速和转向。

脉冲宽度调制（PWM）

PWM简介*

TODO

在Arduino中使用PWM

TODO

编码器

TODO

MPU6050

原始数据

双轮自平衡小车使用的惯性测量单元 (IMU) 是MPU6050，包含一个三轴的加速度传感器和一个三轴的陀螺仪。加速度传感器可以获取运动的物体在三个坐标轴方向上的加速度，陀螺仪可以获取绕坐标轴旋转的物体的角加速度。

对于双轮自平衡小车，我们需要其在一个坐标轴方向上的角度信息。从小车侧面看，仅有可能向左右两个方向倾倒，而不会向前后倾倒，并且小车的静止和行进都是在地面这个水平面进行，因此不需要关心Z轴上的变化。因此，设计一个自平衡小车，我们只需要关注其绕X轴或是绕Y轴的角度（取决于模块的放置方式）问题即可。

在本次设计中，我将按照下图的方式放置MPU6050，因此我们关注的是绕X轴的角度变化。

照片预览_2022-02-27

为了实时获取小车的角度信息，我们需要同时使用加速度传感器和陀螺仪。单独测试MPU6050模块时，可以将将MPU6050模块的VCC引脚接Nano的5V引脚，MPU6050模块的GND引脚接Nano的GND引脚。接入电源后模块便能实时产生六个数据（实际数据比六个多，还包括温度数据，这里不进行介绍），我们使用\(I^2C\)通信方式进行MPU6050模块和Nano之间的通信。进行\(I^2C\)通信时，首先要将MPU6050模块的SCL引脚与Nano的SCL引脚相连，MPU6050模块的SDA引脚与Nano的SDA引脚相连。但是，这里存在一个问题是，在Nano的PCB板上我们找不到名为SCL与SDA的引脚。这时候我们就要回到文章开头的那张Nano引脚图了。

image-20220226162741791

在图中我们可以看到Nano的A4引脚外面写着SDA，A5引脚外写着SCL，其实这是表示引脚的公用，即A4引脚既能做一个单纯的模拟引脚使用，也可以作为SDA引脚使用。因此，为了将MPU6050中产生的数据导入到Nano中，我们MPU6050模块的SDA引脚与Nano的A4引脚相连，SCL与A5相连。连接好之后，我们就可以通过代码来进行程序读取了。

使用Arduino进行\(I^2C\)通信一般需要用到“Wire.h”这个标准库，使用“Wire.h”我们首先要将它包含进来。使用时，首先要进行初始化，与串口通信相似。

1
2
3
4
5
6
7
8
9
#include <Wire.h>

void setup() {
  Wire.begin();
}

void loop() {
  
}

如果我们想要读取MPU6050中的数据，我们需要知道其地址，然后进行数据传输，具体的代码操作如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#include <Wire.h>

const int MPU = 0x68; // MPU6050 I2C 地址
void setup() {
  Wire.begin();
  
  //配置MPU6050的加速度计模块
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void loop() {
  
}

我们本阶段的目标在于读取MPU6050中的原始数据，包括三个坐标方向上的加速度信息和三个方向上的角加速度信息。为了存储这些读取来的数据，我们设置了6个浮点数类型的变量

1
2
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;

来存储数据，之所以是变量数据类型而不是数组，因为这6个信息是实时测量值，在loop()这个函数中每一列都会获取当下的这六个数据，并立即进行处理，通过数学运算获取实时的角度信息，而下一刻的角度信息则有下一刻的六个数据计算得来。loop()函数中实时获取六个数据的方法如下。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
void loop() {
  //读取加速度的值：
  //开启加速度计模块
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  //从MPU6050中读取加速度数据并转化为g为单位的通用数据
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;
    
  //读取角加速度的值：
  //开启角加速度模块
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  //从MPU6050中读取角加速度数据并转化为g为单位的通用数据
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
}

代码中，以

1
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;

为例，在MPU6050中直接读取的数据为\(AccX*16384.0\)，之所以这里要将\(16384.0\)这个数字除掉，其实是在进行单位转换，将加速度的单位转化为\(g\)，这里的\(g=9.8\)其实就是重力加速度。这里之所以将加速度的单位统一为重力加速度，是因为从原理上看，加速度计测量加速度原理就是以重力加速度为参考求出来的，所以一般将加速度的单位设为重力加速度。

使用上述代码

数字滤波器

TODO

一阶互补滤波器（仅roll值）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
#include <Wire.h>

const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX;
float accAngleX, gyroAngleX;
float roll;
float elapsedTime, currentTime, previousTime;
float AccErrorX = -1.5,GyroErrorX = -0.25,rollError = -3.2;

void setup() 
{
  Serial.begin(19200);
  Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        //end the transmission
}
void loop() 
{
  // === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
    
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - AccErrorX;
   
  // === Read gyroscope data === //
  previousTime = currentTime;
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000;
    
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
    
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroX = GyroX - GyroErrorX;
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
  
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX -rollError;
  Serial.println(roll);
}

卡尔曼滤波器

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
#include <Wire.h>

float kalman(float y)
{
    static float x = 1;
    static float p = 5;
    static float q = 0.2; //important
    static float r = 1;   //important
    static float kg;

    p = p + q;
    kg = p / (p + r);
    x = x + kg * (y - x);
    p = (1 - kg) * p;
    return x;
}

//******卡尔曼参数************
static float Q_angle = 0.001;//角度数据置信度
static float Q_gyro  = 0.005;//角速度数据置信度
static float R_angle = 0.5;
static float dt  = 0.004;    //dt为kalman滤波器采样时间;
static char  C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4]  = {0, 0, 0, 0};
static float PP[2][2] = { { 1, 0 }, { 0, 1 } };

float Angle;
float Gyro_y;

const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
static float atan_coeff  = 57.3f;   //arctan转换角度系数
float angle_a,angle_g;
static float gro_zero  = 24.05f;    //角速度为0时的校准值
static float gro_coeff = -0.0645f;  //角速度对应角度变化系数
extern float Angle, Gyro_y;         //kalman融合后得出的最优角度和角速度

int ave_num = 0;

void Kalman_Filter(float Accel, float Gyro);
void calculate_IMU_error();

void setup() 
{
  Serial.begin(19200);
  Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        //end the transmission
  calculate_IMU_error();
  delay(20);
}
void loop() 
{
  // === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
    
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - AccErrorX;
   
  // === Read gyroscope data === //
  previousTime = currentTime;
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000;
    
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); 
    
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroX = GyroX - GyroErrorX;
  gyroAngleX = gyroAngleX + GyroX * elapsedTime;
  
  //angle_a = (float)atan((float)AccX/AccZ)*atan_coeff;
  angle_a = accAngleX; 
  angle_g = gyroAngleX;
  Kalman_Filter(angle_a, angle_g);
  Serial.println(Angle);
  //Serial.println(Gyro_y);
}

void calculate_IMU_error() {
  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
  // Read accelerometer values 200 times
  while (ave_num < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    ave_num++;
  }
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  ave_num = 0;
  // Read gyro values 200 times
  while (ave_num < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    ave_num++;
  }
  //Divide the sum by 200 to get the error value
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  // Print the error values on the Serial Monitor
//  Serial.print("AccErrorX: ");
//  Serial.println(AccErrorX);
//  Serial.print("AccErrorY: ");
//  Serial.println(AccErrorY);
//  Serial.print("GyroErrorX: ");
//  Serial.println(GyroErrorX);
//  Serial.print("GyroErrorY: ");
//  Serial.println(GyroErrorY);
//  Serial.print("GyroErrorZ: ");
//  Serial.println(GyroErrorZ);
}

void Kalman_Filter(float Accel, float Gyro)
{
    Angle += (Gyro - Q_bias) * dt; //先验估计
    Angle_err = Accel - Angle;     //zk-先验估计

    // Pk-先验估计误差协方差的微分
    Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
    Pdot[1] = -PP[1][1];
    Pdot[2] = -PP[1][1];
    Pdot[3] =  Q_gyro;

    // Pk-先验估计误差协方差微分的积分=先验估计误差协方差
    PP[0][0] += Pdot[0] * dt;
    PP[0][1] += Pdot[1] * dt;
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;

    PCt_0 = C_0 * PP[0][0];
    PCt_1 = C_0 * PP[1][0];

    E = R_angle + C_0 * PCt_0;

    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;

    t_0 = PCt_0;
    t_1 = C_0 * PP[0][1];

    // 后验估计误差协方差
    PP[0][0] -= K_0 * t_0;
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;

    Angle    += K_0 * Angle_err;   //后验估计  (最优角度)
    Q_bias   += K_1 * Angle_err;   //后验估计
    Gyro_y   =  Gyro - Q_bias;     //输出值(后验估计)的微分=角速度  (最优角速度)
}


PID控制

TODO

3D建模——制作小车外壳

TODO

PCB设计——制作小车电路板

与使用面包板进行测试不同，绘制PCB板时使用的连线在打印PCB后就会固定下来，因此我们必须在编程时调用相应的引脚，这样才不会出错。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
//HC06
//TX->Nano_Rx
//RX->Nano_TX

//左电机——6个引脚
const byte L1_M1 = 9;
//L2_GND->Nano_GND
const byte L3_C1 = 4;
const byte L4_C2 = 11;
//L5_VCC->Nano_5V
const byte L6_M2 = 10;

//右电机——6个引脚
const byte R1_M1 = 5;
//R2_GND->Nano_GND
const byte R3_C1 = 3;
const byte R4_C2 = 2;
//R5_VCC->Nano_5V
const byte R6_M2 = 6;

//MPU6050
//SCL->Nano_A5
//SDA->Nano_A4
#include <Wire.h>
const int MPU = 0x68; // MPU6050 I2C 地址

//MPU6050 仅计算Roll时需要的变量如下
float AccX, AccY, AccZ;
float GyroX;
float accAngleX, gyroAngleX;
float roll;
float elapsedTime, currentTime, previousTime;
float AccErrorX,GyroErrorX,rollError;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
const byte L1_M1 = 9;
//L2_GND->Nano_GND
const byte L3_C1 = 4;
const byte L4_C2 = 11;
//L5_VCC->Nano_5V
const byte L6_M2 = 10;

const byte R1_M1 = 5;
//R2_GND->Nano_GND
const byte R3_C1 = 3;
const byte R4_C2 = 2;
//R5_VCC->Nano_5V
const byte R6_M2 = 6;

void setup(){
  pinMode(L1_M1,OUTPUT);
  pinMode(L6_M2,OUTPUT);

  pinMode(R1_M1,OUTPUT);
  pinMode(R6_M2,OUTPUT);
}

void loop(){
  //小车直行代码
  analogWrite(L1_M1,255);
  analogWrite(L6_M2,0);
  analogWrite(R1_M1,0);
  analogWrite(R6_M2,255);
}

记录一个可以使用的卡尔曼滤波器，直接使用下面的代码可以使用串口绘图器绘制出Roll的角度变化，打开绘图器时Roll的值记作0，然后表现为相对偏移，这个滤波器效果还不错，至少比那个一阶互补滤波器强，但是感觉相应速度不是很快。

主函数：

Kalman Filter

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
//连线方法
// MPU->UNO
// VCC->VCC
// GND->GND
// SCL->A5
// SDA->A4
// INT->2 (Optional)

#include "Kalman.h"
#include <Wire.h>
#include <Math.h>

float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int MPU = 0x68;           // MPU-6050的I2C地址
const int nValCnt = 7;          //一次读取寄存器的数量

const int nCalibTimes = 1000; //校准时读数的次数
int calibData[nValCnt];       //校准数据

unsigned long nLastTime = 0; //上一次读数的时间
float fLastRoll = 0.0f;      //上一次滤波得到的Roll角
Kalman kalmanRoll;           // Roll角滤波器

void setup()
{
  Serial.begin(9600);   //初始化串口，指定波特率
  Wire.begin();         //初始化Wire库
  WriteMPUReg(0x6B, 0); //启动MPU6050设备

  Calibration();        //执行校准
  nLastTime = micros(); //记录当前时间
}

void loop()
{
  //向串口打印输出Roll角和Pitch角，运行时在Arduino的串口监视器中查看
  Serial.print("Roll:");
  Serial.println(GetValue());
  
}

float GetValue()
{
  int readouts[nValCnt];
  ReadAccGyr(readouts); //读出测量值

  float realVals[7];
  Rectify(readouts, realVals); //根据校准的偏移量进行纠正

  //计算加速度向量的模长，均以g为单位
  float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
  float fRoll = GetRoll(realVals, fNorm); //计算Roll角
  if (realVals[1] > 0)
  	fRoll = -fRoll;

  //计算两次测量的时间间隔dt，以秒为单位
  unsigned long nCurTime = micros();
  float dt = (double)(nCurTime - nLastTime) / 1000000.0;
  //对Roll角和Pitch角进行卡尔曼滤波
  float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
  //跟据滤波值计算角度速
  float fRollRate = (fNewRoll - fLastRoll) / dt;

  //更新Roll角和Pitch角
  fLastRoll = fNewRoll;
  //更新本次测的时间
  nLastTime = nCurTime;
    
  return fNewRoll;
}

//向MPU6050写入一个字节的数据
//指定寄存器地址与一个字节的值
void WriteMPUReg(int nReg, unsigned char nVal)
{
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.write(nVal);
  Wire.endTransmission(true);
}

//从MPU6050读出一个字节的数据
//指定寄存器地址，返回读出的值
unsigned char ReadMPUReg(int nReg)
{
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.requestFrom(MPU, 1, true);
  Wire.endTransmission(true);

  return Wire.read();
}

//从MPU6050读出加速度计三个分量、温度和三个角速度计
//保存在指定的数组中
void ReadAccGyr(int *pVals)
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.requestFrom(MPU, nValCnt * 2, true);
  Wire.endTransmission(true);
  for (long i = 0; i < nValCnt; ++i)
    pVals[i] = Wire.read() << 8 | Wire.read();
}

//对大量读数进行统计，校准平均偏移量
void Calibration()
{
  float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0}; //先求和
  for (int i = 0; i < nCalibTimes; ++i)
  {
    int mpuVals[nValCnt];
    ReadAccGyr(mpuVals);
    for (int j = 0; j < nValCnt; ++j)
      valSums[j] += mpuVals[j];
  }
  //再求平均
  for (int i = 0; i < nValCnt; ++i)
    calibData[i] = int(valSums[i] / nCalibTimes);
  calibData[2] += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}

//算得Roll角。算法见文档。
float GetRoll(float *pRealVals, float fNorm)
{
  float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormXZ / fNorm;

  return acos(fCos) * fRad2Deg;
}

//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals)
{
  for (int i = 0; i < 3; ++i)
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;

  pRealVals[3] = pReadout[3] / 340.0f + 36.53;

  for (int i = 4; i < 7; ++i)
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
}

新建标签："Kalman.cpp"

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
#include "Kalman.h"

Kalman::Kalman() 
{
  Q_angle = 0.001f;
  Q_bias = 0.003f;
  R_measure = 0.03f;

  angle = 0.0f;
  bias = 0.0f;

  P[0][0] = 0.0f;
  P[0][1] = 0.0f;
  P[1][0] = 0.0f;
  P[1][1] = 0.0f;
};
float Kalman::getAngle(float newAngle, float newRate, float dt) 
{
  rate = newRate - bias;
  angle += dt * rate;
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;

  float S = P[0][0] + R_measure;
  float K[2];
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;

  float y = newAngle - angle;
  angle += K[0] * y;
  bias += K[1] * y;

  float P00_temp = P[0][0];
  float P01_temp = P[0][1];

  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;

  return angle;
};
void Kalman::setAngle(float angle) 
{
  this->angle = angle;
};
float Kalman::getRate() 
{
  return this->rate;
};
void Kalman::setQangle(float Q_angle) 
{
  this->Q_angle = Q_angle;
};
void Kalman::setQbias(float Q_bias) 
{
  this->Q_bias = Q_bias;
};
void Kalman::setRmeasure(float R_measure) 
{
  this->R_measure = R_measure;
};
float Kalman::getQangle() 
{
  return this->Q_angle;
};
float Kalman::getQbias() 
{
  return this->Q_bias;
};
float Kalman::getRmeasure() 
{
  return this->R_measure;
};

新建标签"Kalman.h"

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
#ifndef _Kalman_h_
#define _Kalman_h_

class Kalman 
{
  public:
    Kalman();

    float getAngle(float newAngle, float newRate, float dt);

    void setAngle(float angle);
    float getRate();

    void setQangle(float Q_angle);
    void setQbias(float Q_bias);
    void setRmeasure(float R_measure);

    float getQangle();
    float getQbias();
    float getRmeasure();

  private:
    float Q_angle;
    float Q_bias;
    float R_measure;

    float angle;
    float bias;
    float rate;
    
    float P[2][2];
};
#endif
1
2
3
4
5
6
7

Kalman_Filter(Adxl_angle, Gyro_sensor); //卡尔曼融合获取angle
Input = angle;
myPID.Compute();  //PID计算获取 Output
Drive(Output);   //根据Output驱动电机


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
//连线方法
// MPU->UNO
// VCC->VCC
// GND->GND
// SCL->A5
// SDA->A4
// INT->2 (Optional)
#include <PID_v1.h>
#include "Kalman.h"
#include <Wire.h>
#include <Math.h>

double Setpoint, Input, Output;
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int MPU = 0x68;           // MPU-6050的I2C地址
const int nValCnt = 7;          //一次读取寄存器的数量

const int nCalibTimes = 1000; //校准时读数的次数
int calibData[nValCnt];       //校准数据

unsigned long nLastTime = 0; //上一次读数的时间
float fLastRoll = 0.0f;      //上一次滤波得到的Roll角
Kalman kalmanRoll;           // Roll角滤波器

//左电机——6个引脚
const byte L1_M1 = 9;
//L2_GND->Nano_GND
const byte L3_C1 = 4;
const byte L4_C2 = 11;
//L5_VCC->Nano_5V
const byte L6_M2 = 10;

//右电机——6个引脚
const byte R1_M1 = 5;
//R2_GND->Nano_GND
const byte R3_C1 = 3;
const byte R4_C2 = 2;
//R5_VCC->Nano_5V
const byte R6_M2 = 6;
  PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT); //PID对象声明
void setup()
{
  Serial.begin(9600);   //初始化串口，指定波特率
  Wire.begin();         //初始化Wire库
  WriteMPUReg(0x6B, 0); //启动MPU6050设备

  Calibration();        //执行校准
  nLastTime = micros(); //记录当前时间
    

  setupPID();  //PID初始化
    
  pinMode(L1_M1,OUTPUT);
  pinMode(L6_M2,OUTPUT);

  pinMode(R1_M1,OUTPUT);
  pinMode(R6_M2,OUTPUT);
}

void loop()
{
  //向串口打印输出Roll角和Pitch角，运行时在Arduino的串口监视器中查看
  Serial.print("Roll:");
  Serial.println();
  
  Input = GetValue();
  myPID.Compute();  //PID计算获取 Output
  Drive(Output);   //根据Output驱动电机
}

void Drive(float Output)
{    
  analogWrite(L1_M1,Output);
  analogWrite(L6_M2,0);
  analogWrite(R1_M1,0);
  analogWrite(R6_M2,Output);
}
void setupPID(){
  Input = 0;
  Setpoint = 17;  //我的小车自平衡角度为17
  myPID.SetSampleTime(100);  //控制器的采样时间100ms
  //myPID.SetOutputLimits(0, 2000); 
  myPID.SetMode(AUTOMATIC);
}

float GetValue()
{
  int readouts[nValCnt];
  ReadAccGyr(readouts); //读出测量值

  float realVals[7];
  Rectify(readouts, realVals); //根据校准的偏移量进行纠正

  //计算加速度向量的模长，均以g为单位
  float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
  float fRoll = GetRoll(realVals, fNorm); //计算Roll角
  if (realVals[1] > 0)
    fRoll = -fRoll;

  //计算两次测量的时间间隔dt，以秒为单位
  unsigned long nCurTime = micros();
  float dt = (double)(nCurTime - nLastTime) / 1000000.0;
  //对Roll角和Pitch角进行卡尔曼滤波
  float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
  //跟据滤波值计算角度速
  float fRollRate = (fNewRoll - fLastRoll) / dt;

  //更新Roll角和Pitch角
  fLastRoll = fNewRoll;
  //更新本次测的时间
  nLastTime = nCurTime;
    
  return fNewRoll;
}

//向MPU6050写入一个字节的数据
//指定寄存器地址与一个字节的值
void WriteMPUReg(int nReg, unsigned char nVal)
{
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.write(nVal);
  Wire.endTransmission(true);
}

//从MPU6050读出一个字节的数据
//指定寄存器地址，返回读出的值
unsigned char ReadMPUReg(int nReg)
{
  Wire.beginTransmission(MPU);
  Wire.write(nReg);
  Wire.requestFrom(MPU, 1, true);
  Wire.endTransmission(true);

  return Wire.read();
}

//从MPU6050读出加速度计三个分量、温度和三个角速度计
//保存在指定的数组中
void ReadAccGyr(int *pVals)
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.requestFrom(MPU, nValCnt * 2, true);
  Wire.endTransmission(true);
  for (long i = 0; i < nValCnt; ++i)
    pVals[i] = Wire.read() << 8 | Wire.read();
}

//对大量读数进行统计，校准平均偏移量
void Calibration()
{
  float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0}; //先求和
  for (int i = 0; i < nCalibTimes; ++i)
  {
    int mpuVals[nValCnt];
    ReadAccGyr(mpuVals);
    for (int j = 0; j < nValCnt; ++j)
      valSums[j] += mpuVals[j];
  }
  //再求平均
  for (int i = 0; i < nValCnt; ++i)
    calibData[i] = int(valSums[i] / nCalibTimes);
  calibData[2] += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}

//算得Roll角。算法见文档。
float GetRoll(float *pRealVals, float fNorm)
{
  float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormXZ / fNorm;

  return acos(fCos) * fRad2Deg;
}

//对读数进行纠正，消除偏移，并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals)
{
  for (int i = 0; i < 3; ++i)
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;

  pRealVals[3] = pReadout[3] / 340.0f + 36.53;

  for (int i = 4; i < 7; ++i)
    pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
}