一种自跟随平衡小车的控制系统及方法与流程

本发明涉及自平衡小车控制技术领域，具体涉及一种自跟随平衡小车的控制系统及方法。

背景技术：

随着自动控制技术的发展，越来越多的研究人员投入到两轮移动机器人方面的研究。两轮自平衡小车具有多变量、强耦合、非线性等复杂性以及高耦合的运动控制特点，是两轮移动机器人和各种控制算法研究的理想平台，目前已经有很多关于自动跟随的技术，以fiveelementsrobotics推出的budgee机器人为例，这是一款专门针对老年人和残疾人的轻量级助力机器人。budgee的最高速度为每小时3.9公里左右，如果主人的行进速度太快，它就会用消息推送的方式提醒主人慢一些。但是这些技术都还不成熟，它们大多是使用简单的传感器，大都存在跟随的稳定性较差，反馈时间长，能耗较高的问题；且有一定安全隐患，产品也相当昂贵。

技术实现要素：

本发明的目的是提供一种自跟随平衡小车的控制系统，解决了现有技术中存在的自动跟随技术中稳定性较差的问题。

本发明的目的是提供一种自跟随平衡小车的控制方法，解决了现有技术中存在的自动跟随技术中稳定性较差的问题。

本发明采用的技术方案是，一种自跟随平衡小车的控制系统，包括主控装置和自动跟随装置，主控装置和自动跟随装置之间通过红外超声测距接收和发送装置实现信息交互；

主控装置采用stm32f103rbt6主控板，stm32f103rbt6主控板连接有小车姿态获取装置，小车姿态获取装置包括传感器模块，stm32f103rbt6主控板连接有红外超声分体测距发送装置、小车姿态获取装置、测速装置、电机驱动装置连接，stm32f103rbt6主控板通过设置与调试模块连接上位机；

自动跟随装置包括红外超声分体测距接收装置。

红外超声分体测距接收装置设置有2个，分别安装在平衡小车的两端

电源装置通过稳压模块为主控装置、电机驱动装置供电，小车姿态获取装置采用5v供电，stm32f103rbt6主控板采用3.3v供电。

传感器模块采用带串口mpu6050型6轴传感器模块；所述带串口mpu6050型6轴传感器模块rx接收端接pa9；tx发送端接pa10；vcc接5v电源；gng接电源的gnd。

电机驱动装置采用tb6612fng型电机驱动模块，vm接12v以内电源；vcc接5v电源；gnd接电源负极；驱动1路pwma接stm32f103rbt6的pa8口；a01、ao2接电机1的两个引脚；驱动2路pwmb接stm32f103rbt6的pa11口；b01、bo2接电机2的两个引脚。

红外超声分体测距发送模块vcc接5v电源；tx发送端接pa3；rx接收端接pa2；gng接电源的gnd。红外超声分体测距发送模块的vcc接普通5号供电电池的正极，gng接电池负极。

一种自跟随平衡小车的控制方法，本方法采用上述一种自跟随平衡小车的控制系统，具体实施步骤为：

步骤1：系统上电，通过mpu6050型6轴传感器模块检测自跟随小车车体的倾角，以及小车的加速度值，并传送给主控装置；

步骤2：利用卡尔曼滤波优化算法对陀螺仪和加速度计数据进行融合，并以最优姿态角和小车速度为反馈量，对mpu6050型6轴传感器回传数据进行优化处理；

步骤3：通过pid控制算法处理步骤2中优化后的数据，使自跟随小车保持平衡，构成平衡和速度的双闭环控制，由tb6612fng型电机驱动模块控制小车运动，若pid输出调节量角度大于45度，平衡小车自动会执行制动命令；

步骤4：在步骤1中的系统上电步骤后，所述红外超声分体测距发送模块超声波与红外就开始发送，红外超声分体测距接收模块采用上电后就接收超声波与红外信号并串口输出距离数。

步骤5：所述的两个红外超声分体测距接收装置与被追踪物体的距离构成了一个三角形，当两个红外超声分体测距接收模块测出到被追踪物体的距离x＝y时，平衡小车追踪行走。当距离x不等于y时，通过调整平衡小车的方向和速度来改变自跟随小车的运动角度实现自动跟随功能。

步骤3中，所述tb6612fng型电机驱动模块的stby口接主控stm32f103rbt6的i/o口清零电机全部停止；置1通过ain1、ain2、bin1、bin2来控制正反转。

本发明提供了一种基于嵌入式控制器的自动跟随平衡小车系统，该系统具有自动测距装置，通过卡尔曼滤波优化算法对陀螺仪和加速度计数据进行融合，并以最优姿态角和小车速度为反馈量构成平衡和速度的双闭环控制，再利用模糊pid控制算法实现小车系统的自平衡控制，能够根据自动测距装置获取前方物体和自平衡小车之间的距离，实现自主追踪。

本发明一种自跟随平衡小车的控制系统及方法的有益效果是，在传统的小车控制基础之上，增加了对驱动装置的优化，以及对交互机制的改良，运用全新的控制方法，提高了基于嵌入式控制器的自动跟随平衡小车系统的整体性能，使平衡小车具有耗时短、稳定性及抗干扰能力强、功耗低等特点。

附图说明

图1为按照本发明一种自跟随平衡小车的控制系统结构示意图；

图2为按照本发明一种自跟随平衡小车的控制方法原理图；

图3为按照本发明一种自跟随平衡小车的控制方法自平衡控制的原理图；

图4为按照本发明一种自跟随平衡小车的控制方法测距追踪的示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

一种自跟随平衡小车的控制系统，包括主控装置和自动跟随装置，主控装置和自动跟随装置之间通过红外超声测距接收和发送装置实现信息交互；

主控装置采用stm32f103rbt6主控板，stm32f103rbt6主控板连接有小车姿态获取装置，小车姿态获取装置包括传感器模块，stm32f103rbt6主控板连接有红外超声分体测距接收装置、小车姿态获取装置、测速装置、电机驱动装置连接，stm32f103rbt6主控板通过设置与调试模块连接上位机；

自动跟随装置包括红外超声分体测距发送装置。

红外超声分体测距接收装置设置有2个，分别安装在平衡小车的两端

电源装置通过稳压模块为主控装置、电机驱动装置供电，小车姿态获取装置采用5v供电，stm32f103rbt6主控板采用3.3v供电。

传感器模块采用带串口mpu6050型6轴传感器模块；所述带串口mpu6050型6轴传感器模块rx接收端接pa9；tx发送端接pa10；vcc接5v电源；gng接电源的gnd。

电机驱动装置采用tb6612fng型电机驱动模块，vm接12v以内电源；vcc接5v电源；gnd接电源负极；驱动1路pwma接stm32f103rbt6的pa8口；a01、ao2接电机1的两个引脚；驱动2路pwmb接stm32f103rbt6的pa11口；b01、bo2接电机2的两个引脚。

红外超声分体测距发送模块vcc接5v电源；tx发送端接pa3；rx接收端接pa2；gng接电源的gnd。红外超声分体测距发送模块的vcc接普通5号供电电池的正极，gng接电池负极。

一种自跟随平衡小车的控制方法，本方法采用上述一种自跟随平衡小车的控制系统，具体实施步骤为：

步骤1：系统上电，通过mpu6050型6轴传感器模块检测自跟随小车车体的倾角，以及小车的加速度值，并传送给主控装置；

步骤2：利用卡尔曼滤波优化算法对陀螺仪和加速度计数据进行融合，并以最优姿态角和小车速度为反馈量，对mpu6050型6轴传感器回传数据进行优化处理；

步骤3：通过pid控制算法处理步骤2中优化后的数据，使自跟随小车保持平衡，构成平衡和速度的双闭环控制，由tb6612fng型电机驱动模块控制小车运动，若pid输出调节量角度大于45度，平衡小车自动会执行制动命令；

步骤4：在步骤1中的系统上电步骤后，所述红外超声分体测距发送模块超声波与红外就开始发送，红外超声分体测距接收模块采用上电后就接收超声波与红外信号并串口输出距离数。

步骤5：所述的两个红外超声分体测距接收装置与被追踪物体的距离构成了一个三角形，当两个红外超声分体测距接收模块测出到被追踪物体的距离x＝y时，平衡小车追踪行走。当距离x不等于y时，通过调整平衡小车的方向和速度来改变自跟随小车的运动角度实现自动跟随功能。

步骤3中，所述tb6612fng型电机驱动模块的stby口接主控stm32f103rbt6的i/o口清零电机全部停止；置1通过ain1、ain2、bin1、bin2来控制正反转。

带串口mpu6050型6轴传感器模块内部集成了卡尔曼滤波和姿态结算技术芯片，对两轮自平衡小车的三轴加速度、三轴角速度进行测量并通过iic时序或者串口直接读取三轴角度。

电机驱动装置包括两个带霍尔编码器的直流电机、直流电机驱动电路和电机驱动模块，所述两个带霍尔编码器的直流电机与直流电机驱动电路、电机驱动模块及两轮自平衡小车车轮连接。

电机驱动装置采用电机驱动模块同时驱动两个带霍尔编码器的直流电机，通过两个带霍尔编码器的直流电机测速、测车轮转向和驱动车轮运动。

电机驱动采用tb6612fng型直流电机驱动模块，tb6612fng型模块为两路h桥双驱动芯片，可以同时驱动两个带霍尔编码器的直流电机，输出电流达3.2a。

tb6612fng型电机驱动模块的stby口接stm32f103rbt6的i/o口清零电机全部停止；置1通过ain1、ain2、bin1、bin2来控制正反转；vm接12v以内电源；vcc接5v电源；gnd接电源负极；驱动1路pwma接stm32f103rbt6的pb6口；a01、ao2接电机1的两个引脚；驱动2路pwmb接stm32f103rbt6的pb7口；b01、bo2接电机2的两个引脚。

自动跟随装置采用红外超声分体测距发送和接收模块；所述红外超声分体测距发送和接收模块识别并获取当前小车的位置和被跟随物体的位置等信息，采用等距追踪算法和双闭环pid控制调节算法，实现小车的自主追踪和平衡控制。

主控装置的接口通过设置与调试模块连接上位机。

具有测距跟踪能力的两轮自平衡小车是在自平衡小车基础上加入红外超声分体测距发送和接收模块和等距追踪算法，实现平衡车对前方目标的追踪。自平衡小车本身就是一个具有多变量、强耦合、非线性等复杂性以及高耦合的运动控制系统，要实现自平衡，主要在于平衡控制算法的设计与实现。尤其在追踪过程中的加速和减速相当于在平衡控制中加入了人为干扰，其控制难度也相对增加。采用本发明的技术方案，自平衡小车通过红外超声分体测距接收模块获取前方目标的距离、位置等信息，采用等距追踪算法和双闭环pid控制调节算法，实现小车的自主追踪和平衡控制。

如图1所示，电源为18650可充电锂电池，供电电压12v，18650可充电锂电池通过稳压滤波可直接为tb6612fng型电机驱动模块和gm37编码电机供电。由18650可充电锂电池通过5v稳压模块和3.3v稳压模块分别为带串口mpu6050型6轴传感器、红外超声分体测距接收模块和主控stm32f103rbt6供电。稳压模块起到稳压滤波作用。带串口mpu6050型6轴传感器通过iic时序或者串口实时检测并反馈给主控stm32f103rbt6平衡小车的倾角和速度信息，主控stm32f103rbt6通过输出两路pwm信号给tb6612fng型电机驱动模块驱动两个直流电机的转速和方向。普通5号电池为红外超声分体测距发送模块提供5v供电，红外超声分体测距接收模块通过接收红外超声分体测距发送模块发出的红外线进行实时测距并将数据通过串口传送给主控stm32f103rbt6，进而调整平衡小车的方向和速度。

实现平衡车自动跟随的功能，需要选用能获取当前小车的位置和被追踪物体位置的传感器。本实施方案选用红外超声分体测距发送和接收模块进行测距，在车上的两端各安装一个红外超声分体测距接收模块，被追踪物体安装一个红外超声分体测距发送模块。这样左侧与右侧离被追踪物体的距离就构成了一个三角形。当两红外超声分体测距接收模块测出的距离x＝y时，平衡小车追踪行走。当距离x不等于y时，通过调整平衡小车的方向和速度来实行自主追踪。由此可获得人与被追踪物体的距离以及被追踪物体的行走路线，来实现小车的自动跟随功能。

上述tb6612fng型电机驱动模块的stby口接主控stm32f103rbt6的i/o口清零电机全部停止；置1通过ain1、ain2、bin1、bin2来控制正反转；

上述红外超声分体测距发送模块默认上电后超声波与红外就开始发送，红外超声分体测距接收模块为了方便使用，采用上电后就接收超声波与红外信号并串口输出距离数。

除上述的硬件结构部分外，本实施例所述的两轮自平衡小车自动跟随系统还包括电机驱动模块、姿态获取模块、pid控制模块和红外超声分体测距发送和接收模块等系统控制软件部分。

以下结合图2至图4对一种自跟随平衡小车的控制方法进行具体说明。

平衡控制利用带霍尔编码盘的直流电机实现直立环和速度环，通过卡尔曼滤波优化算法和双闭环pid控制算法实时调整实现两轮自平衡小车控制并提高小车自平衡控制的抗干扰性，能够根据红外超声分体测距发送和接收模块获取的前方物体距离，实现自主追踪。系统软件部分的主体框架软件程序流程如图2所示。

通过卡尔曼滤波优化算法对陀螺仪和加速度计数据进行融合，并以最优姿态角和小车速度为反馈量，再通过pid控制算法使其保持平衡，构成平衡和速度的双闭环控制，并且当pid输出调节量角度大于45度，不具有平衡行走能力的时候，平衡小车会自动进行制动。使平衡小车具有耗时短、稳定性强、功耗低等特点。pid控制过程如图3所示。

实现平衡车自动跟随的功能，如图4所示，需要选用能获取当前小车的位置和被追踪物体位置的传感器。本实施方案选用红外超声分体测距发送和接收模块进行测距，在车上的两端各安装一个红外超声分体测距接收模块，被追踪物体安装一个红外超声分体测距发送模块。这样左侧与右侧离被追踪物体的距离就构成了一个三角形。当两红外超声分体测距接收模块测出的距离x＝y时，平衡小车追踪行走。当距离x不等于y时，通过调整平衡小车的方向和速度来实行自主追踪。由此可获得人与被追踪物体的距离以及被追踪物体的行走路线，来实现小车的自动跟随功能。