对某个篇章感兴趣的，请直接跳转
00-灵感篇
小型自平衡机器人EVA的制作灵感，来源于2018年的STM峰会的一次work shop活动。
work shop上的这位大神，格子（别称，因为他穿着高深程序员的标志-格子衬衫）被邀请到峰会上介绍STM的有关产品，格子当时大四，能被邀请到STM32峰会的大学生想必不用多做介绍。
在演讲中可知，格子是个综合能力极强的人，只要有想法，就能从0做起，构思-设计-原理图-电路图-外观-组装-测试等全部自己完成，而且十分痴迷“小”的东西，自制了非常多有趣的物品。小白也钟爱“小”的玩物，被其中一个吸引，就是他自制的迷你自平衡机器人nano。
第一次接触Arduino、第一次接触PID、第一次接触姿态解算……，很是忧愁，何以解忧，唯有动手。
01-设计篇
首先是确定功能，核心功能是自平衡，为了比较好玩，外加的功能有：OLED（显示）、蓝牙（手机控制）、超声波（跟随物体）、红外阵列传感器（识别感应火焰），以下是思维导图（红外阵列删了的原因是不小心用烙铁头把镜片焊坏了…）：
02-硬件篇
根据思维导图，可以得出材料清单：
| 名称 | 型号/驱动 | 用途 | 简述 |
|---|---|---|---|
| Arduino MCU | nano | 主控芯片 | 建议买贵一点的，有质量保证 |
| 6轴陀螺仪 | GY-521 MPU6050 | 姿态解算，获得x轴偏移角度（pitch） | 需要用到加速度计和角速度计 |
| 减速电机 | GA12N20 | 带动轮子转动 | 可以不用减速，但是一定要自带码盘进行测速 |
| 电机驱动 | TB6612 | 驱动电机，控制正反转和速度 | TB6612比较小型，推荐 |
| OLED液晶屏 | SSD1306 | 自检信息显示、图片显示 | IIC或者SPI都可以，最好IIC，能节省MCU端口 |
| 蓝牙 | HC05-V13 | 手机端控制 | 主从一体或者只有从模式都可以 |
| 超声波 | HC-SR04 | 跟随物品移动 | 只能直线行驶，超声波不能判断方向 |
| 红外阵列传感器 | GY-AMG8833 | 在一个矩阵范围内识别火焰位置 | 这个传感器非常贵，注意资金 |
还有一些硬件比如蜂鸣器、LED和按键等等，就随意购买了。
03-原理篇
两轮自平衡的数学模型是倒立摆，与单摆相似的运动。
对于倒立摆（轴是可转动的），在不施加外力的情况下，由于重力作用，上方的物体会向左或者向右摆，而我们的期望是让上方的物体保持在垂直位置（平衡位置），所以需要添加外力让倒立摆保持平衡。
添加外力的控制算法一般是PID（比例-积分-微分），一个短小精悍的算法，要使用PID，系统中需要有回环，回环简单理解为一个单元发出的信号需要被反馈回来。
举个例子，拿出自己的手机，一边说话一边测分贝，你发出的声音被手机上的分贝反馈回来让自己知道，这个过程就是回环，然后用反馈回来的分贝调整自己的声音停留在某一个分贝值附近，这个过程就是PID。
两轮平衡的回环由电机的转速和重心的偏移角度构成，PID算法使用3个参数去控制平衡，分别是P（比例）、I（积分）和D（微分），假设传感器对重心偏移角度的历史采样序列为：X1，X2， ······ ，Xk-1，Xk，而平衡位置的目标值为Sv（一般为0，意思是没有偏移）
比例P控制，基本思想是关心当前偏差，用目标值减去最近的一次测量，得到Pk = Sv - Xk
Pk>0; 当前控制未达标(输出信号要升高)
Pk=0; 当前控制达标(不控制)
Pk<0; 当前控制超标(输出信号要降低)
得到 Pout = P * Pk + Pm（Pm是常数，用于抵消机器阻力）
积分I控制，基本思想是关心历史偏差， 把历史采样点数据序列逐个与Sv比较，得到历史偏差序列：P1，P2， ······ Pk-1，Pk，然后对历史偏差序列积分：Ik = P1+P2+······+Pk-1+Pk (每一项都可正可负)
Ik>0; 所有偏差之和为正，控制总体偏低，未达标(输出信号要升高)
Ik=0; 所有偏差之和为零，控制总体正常，达标(不输出信号)
Ik<0; 所有偏差之和为负，控制总体偏高，超标(输出信号要降低)
得到 Iout = I * Ik + Im（Im是常数，用于抵消机器阻力）
微分D控制，基本思想是关心近期偏差， 把最近的两次偏差相减得到Dk = Pk - Pk-1
Dk>0; 这一次的偏差值大于上一次，越来越偏离我们的目标，偏差有增大趋势
Dk=0; 这一次采样和后一次采样之间的变化没有产生变化
Dk<0; 这一次的偏差值小于上一次，越来越偏离我们的目标，偏差有减小趋势
得到 Dout = D * Dk + Dm（Dm是常数，用于抵消机器阻力）
最后的PID控制输出 OUT = Pout + Iout + Dout，可以看出，如果要编写PID，只有几行的代码，就可以控制平衡。
以上是MCU的运算，直观而言，可以简单粗暴理解为，当检测到机器人身体向前倾，就让轮子猛一下往前转，利用惯性让身体往后摆动，同理，当检测机器人身体往后倾，就让轮子猛一下往后转，由此不断重复，进而保持平衡。
04-软件篇
接下来就是每个模块的驱动程序编写，MCU平台为Arduino，开发非常快捷，为了方便移植，在编程中小白尽量自己写库或者引用一些已经非常完善的平台无关库。
MPU6050姿态结算后使用一阶互补滤波算法得到pitch的偏移角，因为Arduino计算处理能力并不是很强，也不需要太精准的滤波，用一阶互补滤波算法足矣，但是小白在源码中还是编写了卡尔曼滤波，可以比较精准得到当前角度，移植到STM的时候可以使用
/**
* @brief 一阶互补滤波函数，得到偏移角度
*/
float MPU6050::one_filter(float angle_m,float gyro_m,float dt)
{
float K1 =0.07; //Weight of accelerometer
float one_filter_angle = K1 * angle_m+ (1-K1) * (one_filter_angle + gyro_m * dt);
return one_filter_angle;
}
/**
* @brief 卡尔曼滤波函数，得到具体角度
*/
float MPU6050::Kalman_filter(float newAngle, float newRate, float dt)
{
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
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;
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
float S = P[0][0] + R_measure; // Estimate error
/* Step 5 */
float K[2]; // Kalman gain - This is a 2x1 vector
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
float y = newAngle - angle; // Angle difference
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
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;
}
开启蓝牙模式后，EVA一直保持平衡，同时解析手机发送的数据开始行走，手机连接蓝牙的APP使用一个两轮自平衡机器人开源网站提供的APP-Balanduino，本来小白想自己写，但是发现了这个宝物后就懒得动手了，可以从Balanduino源程序中或者官方说明中得到数据协议。
其中一个模拟摇杆的数据协议是：（CJ，x，y ），其中摇杆的坐标是单位坐标，上下左右坐标轴长度都为1，只要根据x，y判断哪个现象后，再赋予两个轮子不同的速度，就可以进行转弯、前进和后退，速度的大小需要自己去调试，每个机器人都不一样。
#ifdef HC05_DEBUG
if((micros() - hc05_time) > 200000)
{
data = hc05.recv();
char str[32];
if(!data.equals(AT_ERROR))
{
/* CJ,x,y */
data.toCharArray(str, data.length());
strtok(str, ",");
float x = atof(strtok(NULL, ","));
float y = atof(strtok(NULL, ","));
speed_setpoint = x * 15;
if(x >0 && y >0) //第一象限
{
int x_speed = x * 15;
int y_speed = y * 30;
tb6612_A.set_rotate_dir(TB6612_LEFT);
tb6612_A.set_speed(x_speed);
tb6612_B.set_rotate_dir(TB6612_LEFT);
tb6612_B.set_speed(y_speed);
}
if(x < 0 && y > 0) //第二象限
{
int x_speed = x * 15;
int y_speed = y * 60;
tb6612_A.set_rotate_dir(TB6612_LEFT);
tb6612_A.set_speed(x_speed);
tb6612_B.set_rotate_dir(TB6612_LEFT);
tb6612_B.set_speed(y_speed);
}
if(x < 0 && y < 0) //第三象限
{
int x_speed = x * 30;
int y_speed = y * 15;
tb6612_A.set_rotate_dir(TB6612_LEFT);
tb6612_A.set_speed(x_speed);
tb6612_B.set_rotate_dir(TB6612_LEFT);
tb6612_B.set_speed(y_speed);
}
if(x > 0 && y < 0) //第四象限
{
int x_speed = x * 60;
int y_speed = y * 15;
tb6612_A.set_rotate_dir(TB6612_LEFT);
tb6612_A.set_speed(x_speed);
tb6612_B.set_rotate_dir(TB6612_LEFT);
tb6612_B.set_speed(y_speed);
}
}
PID_speed_compute();
}
#endif
超声波的跟随控制就比较直观了，只要检测到物体在跟随范围内，就赋予一个特定的速度，让EVA前进，同时保持与物体的距离，让EVA后退，当物体不在跟随范围的时候，就停止。
#ifdef HC_SR04_DEBUG
if((micros() - hcsr_time) > 200000)
{
hcsr_distance = hcsr.get_distance();
if(hcsr_distance > 10.0 && hcsr_distance < 15.0) //向前跟
{
speed_setpoint = 25;
}
else if(hcsr_distance > 2.0 && hcsr_distance < 10.0) //往后退
{
speed_setpoint = -25;
}
else
{
speed_setpoint = SPEED_DEFAULT; //停止
}
hcsr_time = micros();
PID_speed_compute();
}
#endif
其它模块的驱动都比较简单，可以在文件末尾链接得到，这里贴出PID的控制算法代码，PID应用在两个环上，分别是速度环和角度环，角度环令EVA平衡，速度环令EVA行走
/* out = (P * P_err) + (I * I_err) */
static void PID_speed_compute(void)
{
if(micros() - PID_speed_timer > 10000)
{
P_speed_err = (count_L + count_R ) * 1.25 - speed_setpoint ;
I_speed_err += P_speed_err;
pitch_setpoint = P_speed * P_speed_err + I_speed * I_speed_err;
PID_speed_timer = micros();
}
}
/* out = (P * P_err) + (I * I_err) + (D * D_err) */
static void PID_angle_compute(void)
{
if(micros() - PID_angle_timer > 10000)
{
float pitch_current = accelgyro.get_filter_pitch(); //当前的pitch
float P_angle_err = pitch_setpoint - pitch_current; //获得偏差
I_angle_err += P_angle_err; //积分，累计微小变化量
D_angle_err = P_angle_err - P_angle_err_last; //微分，偏差的变化趋势
P_angle_err_last = P_angle_err; //记录当前误差
float angle_output = P_angle*P_angle_err + I_angle*I_angle_err + D_angle*D_angle_err;
int speed;
if(angle_output > 255.0)
speed = 255;
else if(angle_output < -255.0)
speed = -255;
else
speed = (int)angle_output;
do_motor(speed);
PID_angle_timer = micros();
}
}
05-3D打印篇
三维建模工具是CAD，三维建模而言，CAD并没有像Solidworks这样方便快捷，不过头一次建模也够用了，到CAD官网认证学生后有3年的免费试用！！不用烦破解安装。
CAD只是建模工具，实际的模型还是需要图纸的，和最初图纸有一定的差别，因为小白不想把外壳做得复杂，然后用纸皮做了模具，头、身体、连接、电池夹和底座：
06-测试篇
怕轮子打滑厉害，因为橡胶圈磨损得比较薄了，放了件衣服增加摩擦测试平衡，EVA平衡的摆动幅度不大，已经可以自平衡了。
其实文章是EVA一边开发一边编写的，本来这个时候要编写蓝牙连接控制的，但是由于参数调的过高，在测试蓝牙控制的时候EVA突然过冲撞向对面的桌子，跌倒在地，本来焊接好的线很多都断了，难以重新连接，还发现电机编码器的电路板被刮破了，一个电机已经不能使用，下面是大型翻车现场…
上图还是小白补救后的图，摔倒在地后其实身体和基座都断开了，电机驱动的线基本全部断，重新焊接后才得到上面的图，非常心疼EVA，还有超声波跟随测试和蓝牙控制测试没有进行，只能重新买模块组装，不过条件有限，EVA只能暂时到此，等小白有条件后再回来……
核心功能已经完成，其实后面的超声波和蓝牙代码已经写好，只欠测试，另外还有很多【买多/买错/凑单】的零件，可能后面拿来做有趣的东西。
- 自平衡
- 超声波跟随
- 蓝牙控制
网盘链接
最后分享所有的硬件资料+Arduino源代码+3D文件：百度网盘 密码：zn9e