开源平衡车/zh

来自Microduino Wikipedia
502748957@qq.com讨论 | 贡献2016年1月25日 (一) 08:08的版本 程序下载
跳转至: 导航搜索

概述

  • 项目名称:Microduino开源平衡车
  • 目的:制作一台可以用遥控板控制的自平衡机器人小车
  • 难度:中
  • 耗时:2小时
  • 制作者:
  • 简介:

两轮自平衡小车是一个集多种功能于一体的综合系统,是自动控制理论与动力学理论及技术相结合的研究课题,其关键问题是在完成自身平衡的同时,还能够适应各种环境下的控制任务。本次教程我们使用Microduino产品模块快速搭建一个可以用遥控板控制的自平衡机器人小车,玩家可以迅速上手,并且看到小车运动和平衡的效果,玩家们可以在制作结束后,继续更深一步的智能控制部分的研究。

材料清单

  • Microduino设备
模块 数量 功能
Microduino-Core+/zh 1 核心板
Microduino-USBTTL/zh 1 下载程序
Microduino-nRF24/zh 1 无线通信
Microduino-Shield Stepper/zh 1 驱动连接地板
  • 其他设备
模块 数量 功能
2.4G天线 1 2.4G通讯
固定支架 1 固定支撑
尼龙螺丝 4 固定
尼龙螺母 12 固定
电池盒 1 装载电池
电池 2 供电
Micro-USB数据线 1 串口通信,下载程序
车轴 2 连接车轮
车轮子 2 结构
步进电机 2 驱动车轮
  • Joypad材料
模块 数量 功能
Microduino-Core 1 核心
Microduino-nRF24 1 无线通讯
Microduino-Joypad 1 遥控器
Microduino-TFT 1 显示
Microduino-USBTTL 1 下载程序
锂电池 1 供电
亚克力面板 1 面板
尼龙柱 4 固定
长尼龙螺丝 4 固定
短尼龙螺丝 4 固定
尼龙螺母 4 固定
海绵板 1 固定

实验原理

  • PID原理

PID在自动控制领域有着极其重要的作用,作为最早实用化的控制技术已经有70多年的历史,近几年一些创客们自制的一些很酷的东西,如:四轴飞行器,自平衡车等等都离不开它的身影。 首先了解什么是PID。PID实指“比例proportional”、“积分integral”、“微分derivative”,如果我们要求被控制的对象最终趋于一个稳定的结果,一般就可以使用PID算法。 假设说,有一辆速度为1m/s的小车,我们要求他的速度改变为5m/s,要完成这样的一件事,我们必须要有:

  • 小车驱动装置(用程序控制它输出多大的电压,电压决定驱动的马力);
  • 被驱动器控制的部分(即小车);
  • 检测当前速度的装置(当前速度与目标速度的差称为误差);

本来,我们可以给小车一个驱动力让小车加速,直到检测到小车速度达到5m/s,撤去驱动力。但是,这样做会带来几个问题: 1)当小车速度达到5m/s时,从装置检测到这个速度,通知控制器,让控制器改变输出的电压,这一个过程需要耗费一定时间,在这个时间里面,小车速度可能增加了不少。 2)撤去驱动力后,外界条件如摩擦会让小车速度进一步改变。 然而,PID算法可以在一定误差内解决这些问题。 使用PID算法时,大致是这样的。每一个采样周期,通过速度检测装置获得当前速度,传入程序,通过程序计算得到电压控制小车得到新速度。下一个采样周期又把新速度传入,获得新电压,再传入速度,再获得电压,如此反复。 PID算法的关键,是如何根据当前得到的速度值,输出一个“恰当”的电压,以致小车最终能够趋于稳定。 PID算法采用比例,积分,微分(Proportion Integral Differential)三种方法进行控制。三种方法都有自己对应的一个常量(pconst,iconst,dconst)。这三个变量都需要在实验中多次尝试得出。用数学公式表达PID算法如下图所示: 此处:e = 期望值 – 输入值

平衡车之所以可以自己掌握平衡,首先通过Microduino-10DOF模块的加速度和陀螺仪测出相应的姿态数据,然后利用kalman滤波算法得出当前平衡车的角度。平衡的角度是180度,如果测出的角度偏离180,PID算法会调整输出相应的PWM值给电机,从而保持小车平衡。 PID原理有点像锅炉房里烧锅炉,首先定下来锅炉的恒定温度,比如26摄氏度,锅炉房里的墙上有一个温度计,能够实时测得锅炉的实时温度。锅炉房里通常有个老大爷时不时(每十分钟看一次)的看着墙上的温度计,如果温度高了就给锅炉降温,低了就给锅炉升温。如果让一个没有经验的年轻小伙子来管理锅炉的温度,可以想象温度表的值会上下浮动的,有经验的老大爷会把这个浮动降到最低。其实PID就是这个烧锅炉的例子,在代码中就有这个故事的影子: 1)规定的26度就是setpoint 2)当前的温度就是CurrentAngle 3)实际值与26度的偏差就是error 4)没有经验的小伙子有点像PID中的P 5)有经验的老大爷相当于PID了 6)每十分钟看一次相当于PID计算的周期时间 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)最大的一个 
    //而第二个使用该设定值 
    output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; 
    //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); 
}

本套件利用陀螺仪和加速度传感器(Microduino-10DOF/zh)来检测车体态的变化,并利用步进电机控制核心(Microduino-Stepper/zh),精确地驱动电机进行相应的调整,以保持系统的平衡。

  • 主要传感器

Microduino-10DOF

文档

百度盘地址: 【平衡车代码

github地址: BalanceCar_Microduino

程序下载

将Microduino-Core+与Microduino-USBTTL叠加(无上下顺序),通过USB数据线与电脑连接起来

打开Arduino IDE编程软件,点击 【文件】->【打开】

Dl1.jpg

浏览到项目程序地址,点击“Joypad_Balance_Reception.ino”程序打开


点击“工具”,在板选项里面选择板卡(Microduino-Core+),在处理器选项里面选择处理器(Atmega644pa@16M,5V),再在端口选项里面选择正确的端口号,然后直接烧录程序

平衡车搭建

  • Step1:首先将A1和A2对接,然后将B1和B2插在A1两端
  • Step2:将电机与C1对接,再用螺丝帽固定好
  • Step3:将第一步与第二部的结果拼接在一起,用B1进行固定
  • Step4:将轮胎用螺丝帽和螺丝固定在电机上
  • Step5:将电池模块放在图中指定位置,分别用C2和C3接在对应位置,用C4,A3,C5固定
  • Step6:将Core+,USBTTL,10DOF和底板进行拼接再用螺丝和螺丝帽固定好,最后使用传感器线和电池接入如图位置.

注意:在连接电机线的时候要注意方向,并稍微用力才能插入电机的接口,请按照图中方向插接。

Joypad搭建

  • Step 1:将Microduino-TFT从Microduino-Joypad面板后面卡进Microduino-Joypad面板上,用尼龙螺丝固定,注意Microduino-TFT安装方向
Joypadstep1.jpg

注意:接不同的电池需要拨动中间的开关,在图中已有标志。需要先拨开关再接入电池,否则会影响使用。

Joypadstep1 1.jpg
  • Step 2:将传感器接线插在Microduino-TFT的接口上
Joypadstep2.jpg
  • Step 3:首先分别将两个摇杆按键、电池、四个白色按键放入对应位置,之后将连接好天线的nRF模块和Core装到Joypad底板上
Joypadstep3.jpg
  • Step 4:将Microduino-TFT传感器接线的另一头接到底板上的相应位置,之后将长版螺丝帽放到四个角的相应位置
Joypadstep4.jpg
  • Step 5:将Joypad的表壳和底板使用螺丝和螺丝帽固定好
Joypadstep5.jpg
  • Step 6:组装完成后将天线上的贴纸撕下,将天线贴在底板背面的任意位置,至此Joypad组装完毕
Joypadstep6.jpg
  • 自平衡小车和Joypad测试

Joypad调试

  • 按键对应

在打开Joypad之后的4秒左右时间之内按下Key1(下方最左侧的按键),会进入设置(Config)模式

Step1进入设置.jpg
  • 进入设置模式

按照图中的颜色,从左至右对应为Key1~Key4

Step1按键对应.jpg

注意:必须在进入操作界面前进入(4S左右时间)。若未进入则重启进入

  • 摇杆校准

按动Key3和Key4使光标上下移动,Key1为返回,Key2为确认 选择第一项Joystick Config进入摇杆设置模式 继续选择Joystick Correct进入摇杆校准模式。 进入之后会显示如图中第三张图所示的界面,初始状态为两个十字 此时摇动左右摇杆至最上,最下,最左,最右四个极限状态 (推荐操作方式:将摇杆摇动一圈) 摇动之后会看到十字的四个方向出现圆圈,圆圈扩大到最大状态证明已经是摇杆的极限位置 校准之后按Key2确认并返回上一页面

Step2摇杆校准.jpg
  • 选择控制模式

按Key1回到主界面,选择第二项Protocol Config进入模式选择 选择第一项Mode,之后选择nRF24即robot控制模式,按下Key2确认并返回

  • 设置通信信道

返回二级菜单,选择nRF24 Channel按下Key2确认 选择70,它是与Robot_Microduino.ino中nRF24的配置函数设置相对应的

注意问题

  • 下载程序时候最好只叠加core(core+)和USBTTL,虽然本次搭建涉及的nRF24不会引起冲突,但是别的通信模块有时会造成串口冲突,养成好习惯。
  • Core+要叠在nRF24,USB的底下,紧贴ROBOT板。
  • 锂电池正负极别接错了,否则会烧坏电路。
  • 调试好后,实际运行时不要使用USB供电,供电电压不足,请使用电池

平衡车程序说明

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位要发送的数据

视频