自平衡跟随自行车控制系统设计与实现＊

2021-01-19张宏志李健祝洪宇

科技与创新 2021年1期

张宏志，李健，祝洪宇

自平衡跟随自行车控制系统设计与实现＊

张宏志，李健，祝洪宇

（辽宁科技大学电子与信息工程学院，辽宁鞍山 114051）

为解决平衡自行车跟随行走的问题，提出了以OpenMV视觉模块提供方向引导、惯性飞轮PID算法控制车体平衡、直流电机驱动自行车行进的系统设计方案。该方案以自行车行走过程中的姿态测量数据为反馈量，构成车身平衡双闭环控制系统；通过OpenMV视觉模块识别、锁定目标物，引导自行车行走跟随。在自行设计制作的自行车实物模型上，以STM32F103作为主控制板搭建了控制系统。软、硬件调试结果表明，所设计的方案能够实现自行车自平衡基础上的行进跟随，且控制精准度较高。

惯性飞轮；平衡车；视觉算法；PID

1 引言

自平衡自行车兼具传统自行车灵活便捷和智能控制系统智能高效的特点，是一种全新的交通运输和代步行走工具。与市面上流行的左轮、右轮纵向平衡车不同，自平衡自行车采用的是前轮、后轮行进的结构形式。对于这种结构形式的自行车，控制系统要解决的关键问题是使自行车在保持车身平衡的基础上实现智能跟随。为改善此类系统的控制性能，文献[1-3]建立了无人自行车非线性动力学模型，提出了基于陀螺仪的直立姿态平衡控制方法；文献[4]采用步进电机驱动控制技术实现自行车的相对动态平衡；考虑到自行车是多变量、强耦合的非线性系统，文献[5-6]设计了基于LQR（Linear Quadratic Regulator）的控制算法，借以完成机器人自行车静态平衡。上述方法各有优点和适用条件。

在借鉴前述工作的基础上，本文提出一种基于视觉引导的自平衡跟随自行车控制系统设计方法。该方法运用机器视觉技术对引导物图像进行数据采集和处理，借助伺服控制技术，采用双闭环控制结构辅助实现自行车行进过程中的动静态平衡，在自行设计制作的自行车实物模型上进行了方法验证。实验结果表明，融合了机器视觉技术的控制方法可以使自平衡跟随自行车对引导物进行精准锁定，实现行走过程中的动态、静态自动平衡，具备一定的抗干扰能力和智能行走功能。

2 系统构成及控制系统结构

2.1 系统构成

自行搭建的自平衡自行车物理模型如图1所示。该模型由车体、视觉引导、车身平衡、行走控制和能源动力5个部分组成。车体部分采用Solidworks2016建立三维模型，借助3D快速成型技术进行零部件制作，然后组装而成，主要包括车架、底盘、前后车轮和舵机，前轮为从动轮，后轮为驱动轮。视觉引导部分的核心是视觉传感器，主要包括具有数据处理和传输功能的OpenMV摄像头和配套组件。车体平衡部分主要包括惯性飞轮和附属机械零部件，采用机械加工方式制作而成。控制部分是集成了主控制器、检测模块和各种驱动模块的PCB电路板。能源动力部分包括电池和两个直流电机，电池除了给直流电机提供动力，也给控制模块提供工作电源；两个直流电机中，一个带有编码器，用于驱动惯性飞轮，另一个不带编码器，通过减速装置与后轮相联用于驱动自行车行进。为使自行车具有比较好的动静态控制效果，在制作过程中尽可能做到车体质量分布均匀，重心居中。

图1 自行车车体图

2.2 控制系统结构

根据平衡自行车跟随行走控制要求，将控制系统设计成3个相对独立的子系统，分别是车身平衡控制子系统、目标追踪控制子系统和行走控制子系统，各子系统的控制结构如图2所示。

车身平衡是平衡自行车跟随行走的基础，为同时兼顾稳定性和响应速度的要求，车身平衡子系统采用双闭环串级控制结构，控制结构如图2（a）所示。图2（a）中，外环是速度环，内环是位置环。速度环给定信号*来自主控指令，反馈信号来自惯性飞轮直流电机编码器，速度给定信号*与反馈信号的偏差Δ送给速度调节器ASR，经ASR处理后的输出信号作为位置环的给定*；位置环的反馈值来自姿态检测传感器，它反映了自行车实际倾斜角，位置环的给定*与反馈值的偏差Δ送给位置调节器APR，调节器APR对Δ处理后，输出具有一定占空比的PWM信号，送给电机驱动模块，继而调节电机转速，完成姿态调整。

目标追踪控制子系统采用单闭环控制结构，控制结构如图2（b）所示。在该子系统中，目标给定信号*来自主控模块指令，反馈信号来自视觉传感器模块，目标给定信号*与反馈信号的偏差Δ送给舵机，用于调整自行车转向。与目标追踪控制子系统相似，行进控制子系统也采用单闭环控制结构，控制结构如图2（c）所示。

图2 控制系统结构图

3 控制系统硬件设计

控制系统硬件电路结构如图3所示，主要由主控制器模块、视觉图像处理模块、姿态检测模块、直流电机驱动模块及电源模块5部分构成。主控制器模块是控制系统的核心，该模块的主要功能是接收姿态传感器、直流电机编码器和视觉传感器的反馈数据，对数据进行处理及运算，最后将控制信号输出给舵机或直流电机驱动模块。姿态检测模块的功能是通过角速度和角加速度传感器实时检测自行车实际倾斜角，并将其转化为主控制器模块可以识别的信号送给主控；视觉检测模块的功能是利用视觉传感技术对引导物进行位置采集，采集到的数据提供给主控制器作为控制舵机转向的基础。直流电机驱动模块负责将主控制器输出的PWM信号转换为控制信号，驱动2个直流电机的转速和方向。电源模块的功能是为控制系统各组成部分提供能量来源。

图3 控制系统硬件电路结构框图

3.1 主控制器模块

主控制器采用STM32F103C8T6单片机。该控制器是基于Cortex-M3内核的32 bCPU，最高工作频率为72 MHz，在6～64 kB的SRAM存储器片上集成了SPI、I2C和USART等通信接口，方便对外界数据进行读取；含有7个定时器；可输出多路PWM信号，使得电机驱动模块获得稳定的PWM波形。在本系统设计中，STM32F103C8T6采用串口通信方式接收视觉模块和蓝牙模块的数据，采用IIC协议读取姿态传感器MPU-6050的数据，输出PWM波形控制电机和舵机等。

3.2 视觉处理模块

OpenMV是集成了STM32F427CPU微处理器和OV7725图像传感器的开源型机器视觉模块，可方便、快捷地实现图像检测和跟踪、特征提取、颜色跟踪等功能。在OpenMV上，可以用C语言高效实现机器视觉算法；由于该模块搭载了Micro Python解释器，提供了Python编程接口，还可以使用Python脚本语言编程实现I/O端口控制、读取文件系统等基础功能[7-9]。在本设计采用视觉引导模块工作过程中，OpenMV摄像头实时检测引导物的中心坐标、判断引导物的位置和方向，最终实现以引导物为基准的跟随运动。为精确跟踪效果，对摄像头采集的图像需要每10帧处理一次，过程复杂，计算量大。为减轻STM32F103C8T6工作负担，本设计在OpenMV视觉模块的基础上，使用OpenMV IDE软件和OpenCV库进行图像处理。处理后的位置信息通过串口通信发送给主控模块STM32F103C8T6，进而输出相应的电信号去控制舵机移动，使自行车跟随引导物。OV7725摄像头原理电路如图4所示。

图4 OV7725摄像头原理电路

3.3 姿态处理模块

MPU-6050是美国InvenSense公司生产的整合性6轴运动处理器。该处理器整合了3轴陀螺仪和3轴加速度计，其角速度感测范围和加速度感测范围分别有4组参数可供选择，且感测范围可通过编程来控制。本设计选用MPU-6050处理器作为姿态检测模块，对行进中的自行车惯性飞轮角速度和角度加速度进行实时测量。MPU-6050的测量数据经过内置16位A/D转换器转换后，通过IIC总线接口发送给STM32F103C8T6。MPU-6050和STM32F103C8T6之间的通信使用SDA和SCL端口。

MPU-6050模块电路如图5所示。

图5 MPU-6050模块电路

3.4 电机驱动模块

本设计采用TB6612FNG作为电机驱动模块。TB6612FNG是一款新型驱动器件，其电路如图6所示。该模块具有驱动能力强、散热好、抗干扰能力强等特点，能独立双向控制2台直流电机。当图6中的使能端ENA和ENB使能后，从AIN1、AIN2输入的信号驱动电机1的转速和方向，从BIN1、BIN2输入的信号驱动电机2 的转速和方向。

图6 电机驱动电路

3.5 电源转换模块

电源模块包括12 V航模锂电池和稳压电路两部分。航模电池供电电压稳定，可对电机驱动模块直接供电；STM32F103C8T6和其他外设模块的供电要求在3～6 V之间，所以采用稳压电路产生3.3 V和5 V两个电压，其中 3.3 V给STM32F103C8T6和MPU6050姿态传感器供电，5 V给OLED显示屏等外围设备供电。电源稳压电路如图7所示。

图7 电源稳压电路

4 控制系统软件设计

4.1 软件总体流程

主程序主要包括初始化子程序、中断响应子程序、姿态处理模块数据采集子程序、视觉处理模块数据采集子程序、PID控制子程序、电机PWM控制子程序六个部分。主体流程如图8所示。

系统启动后，首先对所有设备进行初始化，分配资源，然后执行中断程序，判断5 ms时间是否到。若时间到，则读取姿态检测模块MPU-6050采集到的偏移角数据，利用内置滤波算法进行数据融合得到姿态估计值；若该估计值不满足平衡要求，则执行增量式PID控制算法对惯性飞轮的角度和角速度进行调节，改变PWM占空比，进而驱动惯性飞轮调整自行车偏移角，直到自行车能够在静止状态下收敛于平衡状态。之后，视觉检测模块的OV7725摄像头开始不断扫描周围环境，一旦引导物出现在检测范围内，主控模块STM32F103C8T6就会根据视觉模块传回的引导物中心坐标判断其位置。若判断出引导物出现在自行车的左侧，则继续判断自行车与引导物的距离是否大于设定值阈值。如果大于该阈值，则舵机向左调整，自行车左转向后继续前进；否则舵机向右调整，自行车右转向、后退，一直退到自行车与引导物的距离大于设定值阈值时自行车再继续跟随引导物前进。若判断出引导物出现在自行车的右侧，则重复与上述类似的调整过程。这样，在自行车行进过程中，OV7725摄像头实时测量自行车与目标引导物的距离、MPU-6050实时采集偏移角数据，从而引导主控模块适时调整舵机转向、车身姿态和行进速度，使自行车始终与引导物保持一定距离稳定跟随。

4.2 系统初始化

系统初始化主要包括系统时钟初始化、NVIC（Nested Vectored Interrupt Controller）中断初始化、定时器外部中断初始化、通信（IIC）初始化、OpenMV视觉模块初始化、MPU-6050初始化、驱动模块TB6612FNG初始化等。STM32F103C8T6有72 MHz和36 MHz两种工作频率可供选择，本系统对时钟频率的要求高，为减小系统误差，在时钟初始化程序中将时钟频率设置为72 MHz。

通信（IIC）初始化主要工作是设置串口波特率、中断允许标志位和模块工作模式。本系统将主控制器模块STM32F103C8T6的串口1波特率设置为115 200 b/s，用于与OpenMV进行通信；将串口3波特率设置为9 600 b/s，用于与蓝牙进行通信。

图8 系统主程序流程图

4.3 控制算法

控制算法是自行车平衡和跟随控制的核心[10-11]。为了提高自行车系统整体反应速度，便于物理实现和实际操作，本设计对不同的控制子系统采用了不同的控制算法。为降低自行车突然加减速对车体的冲击，行进控制系统采用带有中间区的双位控制算法；目标追踪控制系统采用增量式PI控制算法，以兼顾舵机反应速度和摄像头跟踪准确度的要求；对前后轮结构形式的自行车来说，车体平衡控制是自行车稳定行走的前提，控制精度要求高，所以在串级结构基础上主控制器ASR采用增量式PI算法，副控制器APR采用增量式PD算法。

带中间区的双位控制器输出与输入偏差之间的关系如下：

根据式（1），当摄像头与引导物之间的距离大于给定值某一数值后（即偏差大于某一阈值），主控模块STM32F103C8T6输出具有一定占空比的信号max，电机加速，正向前进，靠近引导物；反之，当摄像头与引导物之间的距离小于给定值某一数值后（即偏差小于某一阈值），STM32F103C8T6输出具有一定占空比的信号min，电机减速，反向后退，离开引导物。当偏差在中间区变化时，主控模块STM32F103C8T6输出值不变。

目标追踪控制系统采用增量式PI控制算法。根据式（2）所示的增量式离散PID公式，得到采用增量式PI控制算法表达式，如式（3）所示。

Δ=p[（）－（－1）]+i（）+

d[（）－2（－1）+（－2）] （2）

Δ舵机=p舵机[（）－（－1）]+i舵机（）（3）

式（2）（3）中：（）为当前时刻偏差；（－1）、（）、（－2）分别代表相邻三个时刻偏差；Δ舵机为主控模块STM32F103C8T6根据PI算法计算出的PWM信号占空比的增量值。

于是，STM32F103C8T6输出的、用于控制舵机旋转的PWM信号为：

舵机=（）+ Δ舵机（4）

车体平衡控制系统采用了串级控制结构。根据经验可知，惯性飞轮的旋转速度和自行车的倾斜角有关，当倾斜角增大后，在副控制器APR的作用下，产生一个加速度使飞轮旋转速度加快；当倾斜角减小后，APR仍然会产生一个加速度使飞轮旋转速度减慢。为了快速克服扰动和编码器噪声对调节精度的影响，APR采用增量式PD控制算法，主控制器ASR采用增量式PI控制算法。

ASR增量式PI控制算法和APR增量式PD控制算法表达式分别如式（5）（6）所示。

ΔASR=pASR[（）－（－1）]+iASR（）（5）

ΔAPR=pAPR[（）－（－1）]+dAPR[（）－

2（－1）+（－2）] （6）

4.4 电机控制策略

本设计中，惯性飞轮和后轮驱动使用的都是12 V直流电机。电机转速由施加给电机驱动使能端ENA和ENB的PWM信号决定，电机正、反转通过改变电机驱动IN1和IN2正负极性来实现，电机的加减速通过增大或减小PWM的方法来实现。通过PID控制算法后的PWM信号由主控制器STM32F103C8T6直接发送给直流电机驱动模块，从而控制直流电机的速度和方向。自行车转弯是通过主控制器控制舵机旋转带动扶手来实现的。

5 系统调试

5.1 OpenMV目标追踪

目标追踪调试包括阈值设定和测距两个内容。在软件代码中，主要通过运行OpenMV视觉模块中的OpenMV IDE软件和调用OpenCV图像处理数据库实现引导物的识别和测距定位。

阈值设定基本步骤如下：①运行基础程序hello word.py，在IDE中运行framebuffer，显示图案；②运行Mechine Vision，选择Threshold Edito；③点击Frame Buffer获取IDE中的图像，选择阈值；④通过find_blobs函数找到引导物图像；⑤通过uart.write函数将max_blob.cx（引导物中心横坐标）和max_blob.cy（引导物中心纵坐标）发送给主控。

OpenMV实现测距定位的步骤如下：①设定引导物与摄像头距离=10 cm；②通过find_blobs计算引导物图像像素点M；③根据步骤①和②中的数据计算常数K=×M；④将引导物放在摄像头检测范围内的任意位置；⑤通过find_blobs函数找到引导物图像，计算像素点；⑥根据公式K=×M计算引导物与摄像头之间的距离，通过uart.write将该数据发送回主控。

5.2 舵机PI控制参数整定

追踪系统是单闭环控制系统，其PI控制器参数采用经验试凑法进行整定。舵机的控制一般需要20 ms左右的时钟脉冲，该脉冲的高电平部分一般为0.5～2.5 ms范围内的角度控制脉冲部分，分别对应0°～180°。因为PWM太大会使舵机位移太大，使系统失去平衡，所以对PWM限幅为1 000（舵机最大偏移量为45°）。经实际调试，控制周期为5 ms，p舵机值为﹣0.3，i舵机值为﹣0.005时，舵机能较平稳的向着目标物体靠近。

5.3 车身平衡系统PID控制参数整定

按照车身平衡采用串级控制系统的情况，其控制器参数整定采用两步整定法，即先整定副控制器APR，后整定主控制器ASR，最终整定出能使自行车实现自平衡的最佳参数。在本设计中，主控模块PWM输出设置值是7 200，对应占空比为100%。根据这一设定，如果pAPR值为720，则自行车在倾斜角度为±10°的时候飞轮就会满转。考虑到自行车倾斜过程中飞轮反应速度对车体平衡的影响，设定车体倾斜角度在±4°时，飞轮满转。据此估算出pAPR的取值范围为[0，1 800]。当pAPR增大到1 500时，可以看到自行车在发生倾斜时，飞轮产生的反作用惯性力矩使车体能够暂近收敛于直立的静止平衡状态，但存在低频抖动。此时加入微分控制，估算dAPR值应该在0～8之间，设定dAPR值为7.2。

在速度环控制器参数整定中，首先通过STM32定时器的编码器接口模式对编码器模式进行四倍频，并使用M法测速得到飞轮的速度信息。此时，编码器最大值在200左右。考虑到系统的反应时间，假定当速度偏差达到最大速度50%的时候，电机速度最快，根据PWM输出设置值与占空比之间的对应关系，得到pASR最大值为72。所以，iASR值应设定为pASR/50。经过实际测试，当pASR=50，iASR=1时，自行车可以保持趋近平衡状态。

5.4 系统的动态调试

在小车自平衡过程中，若给小车一个外力来破坏小车的平衡，则小车能迅速响应这个变化，立即运动来减小倾角的变化，建立稳定平衡状态。开始调试时，能响应，但波动较大，建立平衡的时间较长，通过修正PID参数，最终能使小车快速达到平衡状态。

5.5 调试结果

大量实验和调试结果表明，系统通电后，自行车完全实现静止自平衡的时间为3 s左右；在普通水磨石地面上，平衡时倾角范围约在±10°。给予一定外力干扰时，小车能够立刻自动调节达到自平衡状态；跟随目标在1 m范围内跟随效果最优。

6 结论

实验结果表明，所提出的设计方案和实现方法可使自平衡跟随自行车在不需要人为控制的情况下保持平衡跟随。该控制系统把机器视觉技术、嵌入式技术、运动控制技术融合在一起，有效解决了自平衡自行车行进过程中的姿态控制和目标跟随问题，符合当前各个领域智能和自动化产品的趋势。经过实验室进一步完善改进后，在货物输送、智能交通等领域会有很好的发展与应用前景。

［1］郭磊，黄用华，廖启征，等.自平衡自行车机器人的运动学分析［J］.北京邮电大学学报，2011，34（6）：99-102.

［2］郭磊，廖启征，魏世民.用速率陀螺仪实现基于单片机的角度随动系统研究——在自行车机器人的平衡控制中的应用［J］.机电产品开发与创新，2005，18（2）：1-3.

［3］吕宽州，陈素霞，黄金振.基于陀螺仪的无人自行车平衡控制方法［J］.计算机测量与控制，2015，23（7）：2344-2346.

［4］黄用华，廖启征，魏世民，等.前轮驱动自行车机器人直线运动的控制实现［J］.北京邮电大学学报，2012，35（3）：65-69.

［5］古文生，王峰，周全，等.基于LQR控制的机器人自行车静态平衡研究［J］.电脑知识与技术，2020，16（4）：192-194.

［6］杨凌霄，李晓阳.基于GA的两轮自平衡小车LQR最优控制器设计［J］.河南理工大学学报（自然科学版），2015，34（1）：85-90.

［7］田渠，罗淦，尹海涛.基于OpenMV的智能跟踪小车设计［J］.计算机测量与控制，2019， 27（8）：168-169.

［8］庄琼云.基于OpenMV的智能寻迹小车设计与实现［D］.泉州：黎明职业大学，2018.

［9］谢彦昆.基于单目视觉的自平衡车编队行进控制设计与实现［D］.重庆：重庆大学，2018.

［10］党新安，唐鹏，杨立军.单片机控制PID调节的自平衡自行车［J］.机械设计与制造，2019（10）：115-118.

［11］王素青，熊维堂.基于STM32的两轮自平衡小车系统设计［J］.实验室研究与探索，2016，35（5）：146-150.

TH17