[发明专利]一种自跟随平衡小车的控制系统及方法

权利要求书

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

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

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

2.根据权利要求1所述的一种自跟随平衡小车的控制系统，其特征在于，所述红外超声分体测距接收装置设置有2个，分别安装在平衡小车的两端。

3.根据权利要求1所述的一种自跟随平衡小车的控制系统，其特征在于，所述电源装置通过稳压模块为主控装置、电机驱动装置供电，小车姿态获取装置采用5V供电，STM32F103RBT6主控板采用3.3V供电。

4.根据权利要求1所述的一种自跟随平衡小车的控制系统，其特征在于，所述传感器模块采用带串口MPU6050型6轴传感器模块；所述带串口MPU6050型6轴传感器模块Rx接收端接PA9；Tx发送端接PA10；VCC接5V电源；GNG接电源的GND。

5.根据权利要求1所述的一种自跟随平衡小车的控制系统，其特征在于，所述电机驱动装置采用TB6612FNG型电机驱动模块，VM接12V以内电源；VCC接5V电源；GND接电源负极；驱动1路PWMA接STM32F103RBT6的PA8口；A01、AO2接电机1的两个引脚；驱动2路PWMB接STM32F103RBT6的PA11口；B01、BO2接电机2的两个引脚。

6.根据权利要求1所述的一种自跟随平衡小车的控制系统，其特征在于，所述红外超声分体测距发送模块VCC接5V电源；Tx发送端接PA3；Rx接收端接PA2；GNG接电源的GND。红外超声分体测距发送模块的VCC接普通5号供电电池的正极，GNG接电池负极。

7.一种自跟随平衡小车的控制方法，其特征在于，本方法采用如权利要求1-6所述的一种自跟随平衡小车的控制系统，具体实施步骤为：

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

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

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

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

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

8.根据权利要求7所述的一种自跟随平衡小车的控制方法，其特征在于，步骤3中，所述TB6612FNG型电机驱动模块的STBY口接主控STM32F103RBT6的I/O口清零电机全部停止；置1通过AIN1、AIN2、BIN1、BIN2来控制正反转。