开源平衡车/zh
概述
两轮自平衡小车是一个集多种功能于一体的综合系统，是自动控制理论与动力学理论及技术相结合的研究课题，其关键问题是在完成自身平衡的同时，还能够适应各种环境下的控制任务。本教程将指导读者基于Microduino产品模块组建一个可以遥控的自平衡小车，能够实现两轮小车平衡直立和运动的功能，用户在制作完成后，可以在该平台上进行二次开发，实现更多有趣功能。 两轮平衡车的原理简介平衡车的结构Microduino平衡车采用如图所示结构，车体由三层组成，从上到下依次是主控层、电池层、电机驱动层。主控层作为平衡车的大脑，集成Microduino模块和传感器；电池层作为心脏，采用2S锂电池（7.4V）给整个小车提供充足的电力；电机驱动层就是平衡车的两条腿，能够带动小车跑起来。但是以上这些构件怎么能让两轮小车能够直立不到，并且能够前后左右灵活运动呢，我们可以从以下问题分析平衡小车背后的工作原理。 问题一：两轮平衡小车是一个怎样的结构？两轮车是一个高度不稳定的系统，在重力的作用下车体姿态本征不稳定，在没有外力的作用下小车必然出现倾倒的现象，其受力情况如下： 在理想状态下，在小车直立的情况下，小车的重力和车轮支持力受力平衡，小车可以稳定不倒。但是在轻微的扰动下，小车偏离直立方向，小车和垂直方向形成θ角，只要产生θ角，即使角度很小，小车重心M的方向与车轮支持力H的方向亦产生了角度，合力不为0，根据牛顿运动定律可知，小车重心会向下移动，θ角度将越来越大，直至车体倾倒在地上。 问题二：对于这样一个高度不稳定的系统，怎么能让小车保持直立不倒？从工作方式上，两轮小车的运动方式更像是人行走的过程。对于人而言，当人像前倾倒并失去平衡时，人通过自身的感觉器官能够察觉到自己身体的倾斜角度，然后人会做出反应——向前迈出一步来防止自己因失去平衡摔倒。而如果迈出一步仍不能站稳，人就会一步又一步地向前走，直到能回到平衡状态。因此对于两轮小车而言，我们可以将两个电机驱动的车轮看成是人的双腿，同时需要一个能检测小车倾斜角度的传感器，在如下的控制策略下便能够实现车体的动态平衡。 图上所示，我们设定一个车体要保持的倾角，对于水平地面，我们希望车体能保持0度倾角，这个角度称为目标倾角。同时倾斜传感器会检测车体的姿态，得到一个车体的当前倾角。我们可以根据目标倾角及当前倾角的差值计算出下一步电机该怎样运动，从而能够纠正小车的姿态。小车就可以一直重复上述步骤，让小车实际倾角越来越趋近目标倾角，我们将这个循环控制称为平衡小车的角度环控制。 问题三：在小车动态平衡的时候怎控制小车的运动速度和方向？在问题二中，我们可以通过倾斜传感器和电机配合能够让两轮小车保持平衡，但是电机在驱动车轮调节车体倾角的时候，小车的运动方向和速度是一直变化的。此时我们需要一个能检测车轮转速的传感器，然后利用车轮转速就能计算出小车的运动速度和方向，然后利用类似控制小车倾角的方式控制小车的速度。 图上所示，我们给小车确定一个目标速度，同时转速传感器检测车轮的转速，然后计算出小车整体的实际速度。我们就可以根据小车的目标速度和实际转速的差值调整电机转速，让小车实际速度接近目标速速。小车在一直循环上述步骤时，小车的实际速度会越来越接近目标转速，我们将小车的这个循环控制称为速度环控制。 Microduino两轮平衡小车原理组成在了解了两轮小车保持平衡的几个基本问题后，两轮小车系统的基本组成原理可用如下拓扑图示意 两轮平衡小车是一个典型的闭环控制系统，为了精确控制小车倾角和速度的目的，此处同时采用两个闭环控制器：角度环控制和速度环控制。
上述的两个闭环控制器一直处于动态工作状态，实时检测两轮小车的实际速度和实际倾角，并及时调整目标倾角和电机速度，这样可以实现两轮小车在不倒的同时还能按照我们想要的速度和方向运动。 Microduino两轮小车硬件方案
为了实现以上功能，可以采用下列Microduino模块来满足需求：
Microduino两轮小车软件方案在确定的硬件方案下，可以通过如下软件流程实现两轮小车平衡原理。 上图中初始化部分主要包括Microduino几个主要功能模块的设置， 材料清单
文档百度盘地址: 【平衡车代码】 github地址： BalanceCar_Microduino 程序下载将Microduino-Core+与Microduino-USBTTL叠加（无上下顺序），通过USB数据线与电脑连接起来 打开Arduino IDE编程软件，点击 【文件】->【打开】  浏览到项目程序地址，点击“Joypad_Balance_Reception.ino”程序打开
点击“工具”，在板选项里面选择板卡（Microduino-Core+），在处理器选项里面选择处理器（Atmega644pa@16M,5V），再在端口选项里面选择正确的端口号，然后直接烧录程序
烧录程序下载源码先从以下地址下载源程序 BalanceCar_Microduino 堆叠模块
配置环境
浏览到项目程序地址，点击“Joypad_Balance_Reception.ino”程序打开
点击“工具”，在板选项里面选择板卡（Microduino-Core+），在处理器选项里面选择处理器（Atmega644pa@16M,5V），再在端口选项里面选择正确的端口号，然后直接烧录程序
下载程序
平衡车搭建
注意：在连接电机线的时候要注意方向，并稍微用力才能插入电机的接口，请按照图中方向插接。 Joypad搭建
 注意：接不同的电池需要拨动中间的开关，在图中已有标志。需要先拨开关再接入电池，否则会影响使用。 
Joypad调试
在打开Joypad之后的4秒左右时间之内按下Key1(下方最左侧的按键)，会进入设置(Config)模式 
按照图中的颜色，从左至右对应为Key1~Key4  注意：必须在进入操作界面前进入（4S左右时间）。若未进入则重启进入
按动Key3和Key4使光标上下移动，Key1为返回，Key2为确认 选择第一项Joystick Config进入摇杆设置模式 继续选择Joystick Correct进入摇杆校准模式。 进入之后会显示如图中第三张图所示的界面，初始状态为两个十字 此时摇动左右摇杆至最上，最下，最左，最右四个极限状态 （推荐操作方式：将摇杆摇动一圈） 摇动之后会看到十字的四个方向出现圆圈，圆圈扩大到最大状态证明已经是摇杆的极限位置 校准之后按Key2确认并返回上一页面 
按Key1回到主界面，选择第二项Protocol Config进入模式选择 选择第一项Mode，之后选择nRF24即robot控制模式，按下Key2确认并返回
返回二级菜单，选择nRF24 Channel按下Key2确认 选择70，它是与Robot_Microduino.ino中nRF24的配置函数设置相对应的 注意问题
平衡车程序说明nrf设置部分 //nRF==============================
SPI.begin(); //初始化SPI总线
radio.begin();
//此处与Joypad上设定的通道数对应
network.begin(/*channel*/ 70, /*node address*/ this_node);
digitalWrite(LED, HIGH);
Serial.println("===========start===========");
端口设定 #define MOTOR_EN 4 //PORTB,0
#define MOTOR1_DIR A0 //PORTA,7
#define MOTOR1_STEP 5 //PORTB,1
#define MOTOR2_DIR A1 //PORTA,6
#define MOTOR2_STEP 6 //PORTB,2
#define MOTOR3_DIR A2 //PORTA,5
#define MOTOR3_STEP 7 //PORTB,3
#define MOTOR4_DIR A3 //PORTA,4
#define MOTOR4_STEP 8 //PORTD,6
与遥控器进行数据交换的数组 struct receive_a //接收
{
uint32_t ms;
uint16_t rf_CH0;
uint16_t rf_CH1;
uint16_t rf_CH2;
uint16_t rf_CH3;
uint16_t rf_CH4;
uint16_t rf_CH5;
uint16_t rf_CH6;
uint16_t rf_CH7;
};
PID控制实现 // PD的实施。 DT是毫秒
float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
{
float error;
float output;
error = setPoint - input;
// Kd的两部分实施
//仅使用输入（传感器）的一部分而不是设定值输入输入（T-2）最大的一个
//而第二个使用该设定值，使之更有点侵略性设定点设定点（T-1）
output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; // + 错误 - PID_error_Old2
//Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
PID_errorOld2 = PID_errorOld;
PID_errorOld = input; // 误差为Kd值是唯一的输入组件
setPointOld = setPoint;
return (output);
}
//P控制实现。
float speedPControl(float input, float setPoint, float Kp)
{
float error;
error = setPoint - input;
return (Kp * error);
}
// PI实现。 DT是毫秒
float speedPIControl(float DT, float input, float setPoint, float Kp, float Ki)
{
float error;
float output;
error = setPoint - input;
PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);
PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);
output = Kp * error + Ki * PID_errorSum * DT * 0.001;
return (output);
}
PID算法-平衡控制 void robot()
{
//===============================================================
timer_value = millis();
// 新的DMP定位解决方案
fifoCount = mpu.getFIFOCount();
if (fifoCount >= 18)
{
if (fifoCount > 18) // 如果我们有一个以上的数据包，我们采取简单的路径：丢弃缓冲区
{
mpu.resetFIFO();
return;
}
loop_counter++;
slow_loop_counter++;
dt = (timer_value - timer_old);
timer_old = timer_value;
angle_adjusted_Old = angle_adjusted;
angle_adjusted = dmpGetPhi() + ANGLE_FIX;
Serial.println(angle_adjusted);
mpu.resetFIFO(); // 我们始终复位FIFO
// 我们计算估计机器人的速度
actual_robot_speed_Old = actual_robot_speed;
actual_robot_speed = (speed_m[1] - speed_m[0]) / 2; // 正面：前锋
// 角速度角度调整角度调整旧
int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 90.0;
// 我们利用机器人速度（T-1）或（T-2），以补偿该延迟
int16_t estimated_speed = actual_robot_speed_Old - angular_velocity;
//估计速度估计过滤滤速
estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05;
//目标角速度PIC ONTROL dt的速度估计过滤油门Kp_thr Ki_thr
target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
//有限的输出 目标角度约束目标角度最大目标角度最大目标角度
target_angle = constrain(target_angle, -max_target_angle, max_target_angle);
if (pushUp_counter > 0) // pushUp mode?
target_angle = 10;
//我们整合输出（加速度）
control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
control_output = constrain(control_output, -800, 800); // 限制最大输出控制
// 控制的转向部分的输出直接注射
motor1 = control_output + steering;
motor2 = -control_output + steering; //马达2反转
// 限制最大速度
motor1 = constrain(motor1, -500, 500);
motor2 = constrain(motor2, -500, 500);
// Is robot ready (upright?)
if ((angle_adjusted < 66) && (angle_adjusted > -66))
{
if (node_STA) // pushUp mode?
{
pushUp_counter++;
if (pushUp_counter > 60) // 0.3 seconds
{
// Set motors to 0 => disable steppers => robot
setMotorSpeed(0, 0);
setMotorSpeed(1, 0);
// We prepare the raiseup mode
Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
Kd = KD_RAISEUP;
Kp_thr = KP_THROTTLE_RAISEUP;
control_output = 0;
estimated_speed_filtered = 0;
}
else
{
setMotorSpeed(0, motor1);
setMotorSpeed(1, motor2);
}
}
else
{
// NORMAL MODE
setMotorSpeed(0, motor1);
setMotorSpeed(1, motor2);
pushUp_counter = 0;
}
if ((angle_adjusted < 40) && (angle_adjusted > -40))
{
Kp = Kp_user; // Default or user control gains
Kd = Kd_user;
Kp_thr = Kp_thr_user;
Ki_thr = Ki_thr_user;
}
else
{
Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
Kd = KD_RAISEUP;
Kp_thr = KP_THROTTLE_RAISEUP;
Ki_thr = KI_THROTTLE_RAISEUP;
}
}
else // Robot not ready, angle > 70º
{
setMotorSpeed(0, 0);
setMotorSpeed(1, 0);
PID_errorSum = 0; // Reset PID I term
Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
Kd = KD_RAISEUP;
Kp_thr = KP_THROTTLE_RAISEUP;
Ki_thr = KI_THROTTLE_RAISEUP;
}
}
}
遥控器数据接收后选择数据控制平衡车的前后与左右 float * _i = _speed;
//CH1,CH0,CH7对应之前的发送/接收部分定义的数组
_i[0] = map(rec.rf_CH1, 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE);
_i = _turn;
_i[0] = map(rec.rf_CH0, 1000, 2000, -MAX_STEERING, MAX_STEERING);
node_STA = (rec.rf_CH7 > 1500 ? true : false); //接收请求时序赋值
Joypad程序及说明Joypad部分 def.h中 定义了 uint8_t nrf_channal = 70; //0~125
nrf_channal为nrf通信的通道，joypad和Cube小车的代码中都会有该定义 当通道一致时则Joypad可与Cube小车成功连接。 在小车代码中会有如下程序段 //nRF==============================
SPI.begin(); //初始化SPI总线
radio.begin();
network.begin(/*channel*/ 70 , /*node address*/ this_node);
在data.h中 outBuf[0] = Joy1_x;
outBuf[1] = Joy1_y;
outBuf[2] = Joy_x;
outBuf[3] = Joy_y;
outBuf[4] = map(AUX[0], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
outBuf[5] = map(AUX[1], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
outBuf[6] = map(AUX[2], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
outBuf[7] = map(AUX[3], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
8位数组outBuf表示Joypad发出的8位数据，0位为右摇杆左右，1为右摇杆上下，2为左摇杆左右，3位左摇杆上下,4~7位对应AUX0~4 在nrf.h中 struct send_a //发送
{
uint32_t ms;
uint16_t rf_CH0;
uint16_t rf_CH1;
uint16_t rf_CH2;
uint16_t rf_CH3;
uint16_t rf_CH4;
uint16_t rf_CH5;
uint16_t rf_CH6;
uint16_t rf_CH7;
};
此处定义的send_a结构体位对应的0位要发送的数据 视频 |
