智能平衡移动机器人（MATLABSimulink版·微课视频版）课件 7.1-7.4 平衡控制 直立环- 红外循迹应用

智能平衡移动机器人第7章

综合实验合肥工业大学

7.1平衡控制——直立环

7.1.1平衡控制原理 7.1.2平衡控制模型 7.1.3平衡控制中基于模型设计与自动代码生成技术 7.1.4平衡控制应用现象

7.2平衡控制——速度环

7.2.1应用原理 7.2.2速度控制模型 7.2.3速度控制应用现象7.3APP控制 7.3.1应用原理 7.3.2APP控制模型 7.3.3APP控制应用现象 7.4红外循迹应用 7.4.1模块介绍 7.4.2使用原理 7.4.3模型搭建

7.4.4具体实验现象

7.1平衡控制——直立环7.1.1平衡控制原理下图是一个单摆模型，对其进行受力分析如图。在重力作用下,单摆受到和角度成正比,运动方向相反的回复力。而且在空气中运动的单摆,由于受到空气的阻尼力,单摆最终会停止在垂直平衡位置。空气的阻尼力与单摆运动速度成正比,方向相反。F=−mgsinθ≈−mgθ总结单摆能够稳定在垂直位置的条件有两个:受到与位移(角度)相反的回复力，在此回复力作用下，单摆便进行周期运动。受到与运动速度(角速度)相反的阻尼力。阻尼力越大，单摆越会尽快在垂直位置稳定下来。直立着的车模可以看成放置在可以左右移动平台上的倒立着的单摆。倒立摆之所以不能象单摆一样可以稳定在垂直位置，就是因为在它偏离平衡位置的时候，所受到的回复力与位移方向相同，而不是相反！因此，倒立摆便会加速偏离垂直位置，直到倒下。如何通过控制使得倒立摆能够像单摆一样，稳定在垂直位置呢？两个办法：X改变重力的方向；增加额外的受力，使得恢复力与位移方向相反才行。控制倒立摆底部车轮，使得它作加速运动。这样站在小车上（非惯性系，以车轮作为坐标原点）分析倒立摆受力，它就会受到额外的惯性力，该力与车轮的加速度方向相反，大小成正比。这样倒立摆所受到的回复力为：式中，由于θ很小，所以进行了线性化。假设负反馈控制是车轮加速度a与偏角θ成正比，比例为k1。如果比例k1>g（是重力加速度）那么回复力的方向便于位移方向相反了。此外，为了使得倒立摆能够尽快地在垂直位置稳定下来，还需要增加阻尼力。虽然存在着空气和摩擦力等阻尼力，相对阻尼力比较小。因此需要另外增加控制阻尼力。增加的阻尼力与偏角的速度成正比，方向相反。因此可以得到下式：按照上面的控制方法，把倒立摆模型变为单摆模型，能够稳定在垂直位置。因此，可得控制车轮加速度的控制算法。式中，θ为车模倾角；θ‘为角速度；k1、k2均为比例系数；两项相加后作为车轮加速度的控制量。只要保证在条件下，可以使得车模像单摆一样维持在直立状态。其中有两个控制参数k1>g、k2>0，决定了车模是否能够稳定到垂直平衡位置，它必须大于重力加速度；决定了车模回到垂直位置的阻尼系数，选取合适的阻尼系数可以保证车模尽快稳定在垂直位置。在角度反馈控制中，与角度成比例的控制量是称为比例控制；与角速度成比例的控制量称为微分控制。因此上面系数分别称为比例和微分控制参数。其中微分参数相当于阻尼力，可以有效抑制车模震荡。总结控制车模直立稳定的条件如下：（1）能够精确测量车模倾角θ的大小和角速度θ'的大小；（2）可以控制车轮的加速度。直立控制算法的关键在于获取车模的角度和角速度，通过测量车模的倾角和倾角速度控制车模车轮的加速度来消除车模的倾角。测量车模倾角和倾角速度可以通过安装在车模上的加速度传感器和陀螺仪实现。而车模的倾角以及角速度的测量，我们使用的MPU6050集成了三轴陀螺仪和三轴加速度计，可以通过IIC接口同时输出三个方向上的加速度信号和陀螺仪信号。车模运行速度和加速度是通过控制车轮速度实现的，车轮通过车模两个后轮电机经由减速齿轮箱驱动，因此通过控制电机转速可以实现对车轮的运动控制。加速度控制量由施加在电机上面的电压代替，直接调节控制电机的PWM的占空比来调节电机转动的加速度。7.1.2平衡控制模型关键模块关键模块关键模块关键模块7.1.3平衡控制中基于模型设计与自动代码生成技术具体过程前面章节已经有详细介绍，在此简单回顾一下。在模型中打开Configuration，Parameters参数设置对话框，在solver解算器中设置相关参数，在HardwareImplemention中设置芯片类型等相关参数，在CodeGeneration中设置SimulinkCoder等相关参数。具体参数配置见下图参数配置完后，启动模型编译，进行相关处理后将编译好的代码烧入TMS320F28069芯片中，从而实现对小车的直立环平衡控制。7.1.4平衡控制应用现象仅仅有直立控制，平衡车在平衡很短的时间之后，就会加速往一个方向倒下。当我们向前倾斜平衡车时，车轮会加速往前转动，当向后倾斜平衡车时，车轮会加速向后转动。当我们尽量水平放置平衡车时，车轮转速很慢。说明我们基本可以实现平衡车的平衡控制，但是我们会发现平衡车很难长时间稳定在一个地方7.2平衡控制——速度环7.2.1应用原理车模的速度是通过调节车模倾角来完成的。车模不同的倾角会引起车模的加减速,从而达到对于速度的控制。实际上最后还是演变成通过控制电机的转速来实现车轮速度的控制。当需要提高平衡车向前的行驶速度，就需要增加平衡车的倾角，倾角增大后，车轮在直立控制作用下需要向前运动保持平衡车平衡，速度增大；如果要降低平衡车向前行驶速度，就需要减小平衡车的倾角，倾角减小以后车轮在直立控制作用下运行速度减小保持平衡车平衡。由上述可知，可搭建如下数学模型：

式中θ代表角度,θ′代表角速度，e(k)是实际速度与设定速度的偏差,∑e(k)偏差求和代表积分。因此，可以从理论上说明小车的速度控制是可以通过搭建上述的数学模型来实现。7.1.2速度控制模型关键模块7.2.3速度控制应用现象将平衡车放在水平桌面上，上电，会发现平衡车基本上稳定在一个位置（不同系统可能会有微小的来回摆动）。7.3APP控制7.3.1应用原理APP实验是基于平衡控制进阶实验为基础，利用美国TI公司CC2541芯片的蓝牙模块实现手机与平衡车的通信，进而实现手机控制平衡车的功能。如下图所示。该CC2541蓝牙模块主要用于短距离的数据无线传输领域，可以方便的和PC机的蓝牙设备相连，也可以两个模块之间的数据互通，避免繁琐的线缆连接，能直接替代串口线。控制小车速度：通过调节小车的倾角来实现小车速度控制控制小车方向：通过控制两个电机之间的转动差速实现小车转向控制。7.3.2APP控制模型关键模块关键模块关键模块7.3.3APP控制应用现象点击APP前进区域，小车向前运动点击APP后退区域，小车向后运动点击APP左转区域，小车向左运动点击APP右转区域，小车项右运动不点击任何区域，小车停止运动7.4红外循迹应用该传感器模块对环境光线适应能力强，其具有一对红外线发射与接收管，发射管发射出一定频率的红外线。可通过电位器旋钮调节检测距离，有效距离范围2~60cm，工作电压为3.3V-5V。该传感器的探测距离可以通过电位器调节、具有干扰小、便于装配、使用方便等特点。可以广泛应用于机器人避障、避障小车、流水线计数及黑白线循迹等众多场合。当检测方向遇到障碍物(反射面)时，红外线反射回来被接收管接收，经过比较器LM339电路处理之后，同时信号输出接口输出数字信号(一个低电平信号)。7.4.1模块介绍7.4.2使用原理我们以第一路来简单说明一下：IN1-为电位器调节的电压输入端，IN1+为探头输出的电压(就是探头OUT与GND的电压)，当IN1-电压>IN1+电压时，对应的OU1输出的电平电压接近0V，此时，对应的LED灯亮；当IN1-电压<IN1+电压时，对应的OU1输出的电平电压接近5V，此时，对应的LED灯灭。按照要求将探头与红外中控板连接好，移开探头前面所有物体，且探头不要指向有阳光的地方(光线对探头有较大干扰)，调节每一路的电位器，直到灯好灭掉，然后用探头移向白纸空白处，会发现蓝色LED会点亮，移向黑线处蓝色LED灭，这样就算测试成功了，然后我们就可以将中控板的输出信号传入主控板进行高低电平检测了。7.4.3模型搭建程序设计思路首先设置系统时钟为80MHz，Timer0为5ms执行一次中断，接着初始化本次实验需要的各个模块，包含按键的GPIO模块、EQep1和EQep2模块、IIC模块以及MPU6050模块，红外探头传感器GPIO的初始化，然后完成传感器数据采集，最后根据采集到数据完成控制。初始化各个模块:定义全局变量初始化好九个全局变量，Gyro_Balance表示陀螺仪x的角速度的值，Angle_Balance表示车身相对于Z轴的角度，Gyro_Turn表示陀螺仪z的角速度的值，Key_Flag表示按键电平检测标志值，Flag_qian、Flag_hou、Flag_left、Flag_right分别为前、后、左、右标志位，Dir表示方向偏转标志。完成传感器数据采集按键标志根据红外循迹给出方向偏差信号将读取到的数据进行卡尔曼滤波后，输出并存储读取左右编码器脉冲数此模块用作得到MPU6050的原始数据。if(RIN1==0&&RIN3==1&&RIN4==1)Dir=1;//右边检测到黑线，左偏if(RIN4==0&&RIN1==1&&RIN2==1)Dir=2;//左边检测到黑线，右偏直立环PD控制倾角保护，当角度的绝对值大于30时，关闭电机转动

由于红外寻迹模块的安装导致小车的平衡位置发生改变，经调试当小车的中值角度在3度左右，所以将直立环的ZHONGZHI设为3。Balance_Pwm=Balance(Angle_Balance,Gyro_Balance);速度环PI控制

速度环给定一个前进的速度，即将Movement设为<0的数，这里设为-7。Velocity_Pwm=Velocity(Encoder_Left,Encoder_Right);转向环P控制根据Dir值给定转向值。Dir=1表示小车相对黑线右偏，给一个向左转的量；Dir=2表示小车相对黑线