基于图像信息采集的自动寻迹平衡小车设计

崔渊 姬丰欣 陈祝洋 高倩 钱铮

摘要：针对循迹自平衡车的两个关键技术难点——直立控制和视觉导航，采用STM32F407VET6单片机为控制核心。一方面，采集MPU6050六轴传感器的加速度与角速度数据，经卡尔曼滤波后得到精准的直立倾角信息，结合速度-姿态串级PID控制器，加快电机对误差的响应速度，实现对平衡车直立的精确控制;另一方面，利用DMA机制采集OV7670摄像头的图像信息，并对其进行灰度化、高斯图像滤波、图像二值化、路径提取和拟合等处理，得到精确的路径信息，实现平衡车的视觉导航。为了增强人机交互体验，平衡车的关鍵数据还将实时传输到Android 端App，并可通过App对平衡车进行控制。

关键词：STM32;自平衡车;卡尔曼滤波;循迹;串级PID;Android

中图分类号：TM935.3文献标识码：A文章编号：2095-7394（2021）06-0031-11

自平衡车一直是机器人领域的研究热点，它的出现改变了人们的生产生活方式，极大地提高了生产效率，降低了生产成本和危险性，进一步促进了人们对智能化生活的需求。自平衡车是一个典型的倒立摆系统，由于倒立摆系统其本质是不稳定的，所以需对车身进行实时调整才能保持其稳定状态[1]。目前，基于自平衡车这种面临环境的不确定性，人们对其自主处理事务能力的要求越来越高，国内外的相关研究也取得了大量重要成果，包括典型的PID控制、模糊控制以及人工神经网络控制等[2]。

相比于传统的超声波、光电对管等传感器只能获取单一的环境信息而言，摄像头获得的信息更加全面丰富，但同时也加剧了处理器的运算负担，所以视觉算法难以获得广泛应用。近年来，由于半导体制造技术的快速发展以及机器视觉算法的成熟，这些瓶颈逐步得到突破，为基于视觉导航的自平衡车的研究奠定了重要基础。目前，虽然市面上平衡车琳琅满目，但大多价格昂贵、功能单一，并且需要人为控制，在智能化程度方面尚不能满足人们的需求。本文旨在结合自平衡车和视觉导航的优点，设计和制作一款基于STM32视觉循迹的自平衡车。

1系统方案设计

本设计以STM32F407单片机为主控核心，通过六轴姿态传感器和摄像头分别采集平衡车姿态信息和路面图像信息，实现两轮小车的直立控制和循迹行驶。系统主控由六大模块构成。

1.1姿态检测模块

姿态检测模块基于六轴姿态传感器的方式[3]。如图1所示，通过姿态传感器测量平衡车的加速度和角速度数据，之后利用卡尔曼滤波进行姿态融合，得到平衡车倾角。

1.2姿态控制模块

姿态控制模块采用PID闭环控制[4]。如图2所示，根据平衡车的测量倾角和期望倾角计算得到控制偏差，之后分别对偏差进行比例、积分、微分运算，将这3项相加得到输出的控制量，然后输出PWM控制电机维持车体平衡姿态。PID的输出式为：

其中：参数P可以提高姿态控制的控制力度，但是P参数过大会造成平衡车前后剧烈摇晃，使控制效果变差;参数D可以显著抑制平衡车的前后摇晃，但是D参数过大会使平衡车出现高频颤动，可能会损坏电机驱动;在调试过程中发现，引入I参数后，平衡车姿态控制变得不稳定，并且仅仅使用P、D参数控制效果已经足够好，故在姿态PID中，将I参数值设为0，变为PD控制。

1.3路径检测模块

如图3所示，为最小二乘法路径检测框图。

首先，利用摄像头采集路面的灰度图像，即单片机通过DCMI模块对摄像头的图像进行采集，为了减少CPU资源的浪费，提高实时性，采用DMA方式进行传输;与此同时，CPU可以处理其他工作。

然后，对其进行滤波、二值化等处理，去除掉图像的噪点与其他冗余信息。由于路径与地面其他部分对比度很大，故在二值化图像中路径为黑色，其他部分为白色。统计灰度图像的直方图中存在的2个波峰，选取2个波峰之间的中点作为阈值。默认图像服从高斯分布，可以通过对滤波器窗口内的像素点按高斯核进行加权平均得到输出。

在对图像经过处理得到二值化图像后，根据路径的左边缘为白色到黑色的跳变、右边缘为黑色到白色的跳变这两个特点，即可定位路径的边缘。由于实际路径可能会出现弯折的情况，只从行方向扫描可能无法获得全面的路径信息，故选择从行、列两个方向对图像进行扫描。同时，在水平和垂直两个方向上检测黑色路径的两个边缘，通过计算便可以得出中点。

最后，根据最小二乘法[5]原理拟合出一条直线表示当前的路径。通过最小化误差的平方和寻找数据的最佳函数匹配，利用最小二乘法可以简便地求得未知数据，并使这些求得的数据与实际数据之间误差的平方和为最小。最小二乘法拟合路径示意图如图4所示。设对图像进行边缘检测后

使用最小二乘法噪声和干扰的影响较小，并且对直线和弯折的路径都能准确表示，还能够对图像中路径丢失的区域进行预测。其拟合得到的路径直线的斜率包含了方向信息，直线的函数值包含了车体偏离路径信息，从而能够满足实时处理的要求。

1.4路径控制模块

如图5所示，为偏差加权PID控制框图。输入期望路径值，与测量路径做偏差，再将偏差加权，输出到PID控制器计算得到电机输出，从而控制左右差速。

由于摄像头采集的图像中越靠近上边的图像离平衡车的距离越远，而远处的图像对平衡车接下来位置起预测作用，故越远处的图像信息对路径控制越重要。将其相加可以得到路径偏差：

此时，需要对图像中的路径进行加权处理，远处的路径给大一些的权重，近处的路径给小一点的权重，以增加平衡车的方向灵敏度。

式（3）中，road为加权后的路径，i表示图像的行数，i越小，离车身越近，center_i表示第i行的路径偏差，k_i表示第i行的权重值。权重带分区越多，比例系数越小，方向控制灵敏度越好。但是，考虑到实际计算量以及噪点的影响，对图像进行权重带分离，同一个权重带下路径加权值相同，如图6所示。在实验条件下，光照强度不变，将一张图片分为6部分，取前5部分作运算，中间的权重更大些，底部最小，最上面的图像由于摄像头角度的安装，经常会拍到墙或者线外，故可以不作处理和运算。再将根据式（3）计算出的拟合后的路径偏差值作为输出，送入PID控制器得到控制量，据此调整两电机的转速差，从而控制平衡车紧跟路径行驶。

1.5速度检测模块

系统用霍尔编码器测速[6]。如图7所示，将霍尔编码器安装至两个电机上，定时读取两个编码器A相和B相输出的脉冲数P₁和P₂，两个电机的转速R₁和R₂由下式计算：

将两者求平均后乘以轮胎的周长L即可得到平衡车的速度v：

1.6速度控制模块

速度控制模块与姿态控制模块结合构成串级PID控制。由于平衡车的姿态控制和速度控制密切相关，故可以将两者组合在一起进行控制。如图8所示，速度环和姿态环串联在一起，速度环为外环，姿态环为内环。在控制过程中，速度环根据平衡车的速度计算出当前平衡车的最优倾角，姿态环则负责控制平衡车达到这一倾角值，如此间接实现速度控制。

2软件方案设计

2.1STM32F407软件系统设计

在整个平衡车系统中，主控制器STM32F407VET6无疑起着关键的作用，平衡车的姿态控制、路径控制、速度控制、菜单界面显示、与Android端进行通信以及控制电机旋转等重要功能均由它来完成。

STM32F407软件系统主程序流程如图9所示。系统上电后首先进行HAL库、系统时钟以及诸如摄像头、MPU6050等其他各种外设的初始化;然后，判断各个定时时间是否溢出来执行相关软件模块。在5 ms的定时溢出后，系统进行一次平衡车的姿态、路径、速度等控制，并输出PWM给电机;在20 ms的定时溢出后，系统对菜单显示进行更新;在50 ms的定时溢出后，系统对OV7670触发一次图像采集，并处理与Android之间的通信任务。当系统检测到图像采集结束时，将对图像进行一系列处理，以便后续使用。

2.2姿态控制软件设计

如图10所示，平衡车软件姿态控制部分的主要任务是解算平衡车倾角，并控制平衡车保持平衡姿态。平衡车的姿态控制软件主要分为三个部分：传感器信息采集、倾角解算、PID控制。其中：传感器信息采集部分负责采集平衡车的加速度和角速度信息;倾角解算部分利用前面获得的信息，通过卡尔曼滤波解算出平衡车的倾角[9];PID控制部分则负责对平衡车倾角误差进行控制，以维持平衡车的平衡姿态[10]。

2.3路径控制与拟合软件设计

如图11所示，路径控制主要分为三个部分：图像采集与处理、路径检测与拟合、加权控制。其中：图像采集与处理部分主要负责从OV7670摄像头采集图像信息，并对图像进行滤波、二值化等处理，以供后续路径检测使用[11];路径检测与拟合部分主要负责从二值化图像中将路径分离出来，获得路径各个点的坐标，并拟合出路径方程;加权控制部分主要负责对路径按重要程度进行加权，从而控制平衡车沿地面路径行驶[12]。

路径拟合方程的流程如图12所示。先通过所有的路径中点计算出均值和协方差矩阵，然后遍历所有点，将坐标代入下式：

得到该点的概率密度函数值。通过判断该值是否大于设定的阈值来判定该点是否为干扰点，如果不是干扰点，则将其保留下来进行后续处理。

至此，得到的路径中点都是离散的点，甚至可能存在缺失，这样不利于后续处理;因此，需要拟合出一条曲线来表示路径。考虑到高次多项式拟合计算量大、容易出现过拟合等问题，这里采用最小二乘法进行线性拟合，即拟合出一条直线代表路径。

设待拟合直线的方程为y=kx+b。由于直线斜率k在直线垂直于x轴时趋于无穷大，而平衡车在直线路径上行驶时正是这种情况，这样不便于后续处理;因此，这里以图像的垂直方向为x轴，水平方向为y轴，当在直线路径上行驶时，直线的斜率为0。

2.4蓝牙通信软件设计

Android设备的蓝牙由BluetoothAdapter类进行管理[13]。蓝牙通信部分的流程图设计如图13所示。程序为各个按钮设置了监听器，用来监听各个按钮的按下事件。当写入参数按钮按下时，程序将读取UI界面中用户输入的PID参数数据，并将其按照一定的帧格式进行打包，之后将打包好的数据帧通过蓝牙输出流进行发送;由于发送过程会阻塞程序，因此，每次发送都将创建新的线程。其他按钮的处理过程与此类似，故不再赘述。

3实验结果与分析

3.1姿态检测结果展示

如图14所示，蓝色的线是卡尔曼滤波之前的倾角，黄色的线是滤波之后的倾角。可以看到：在Q值过大时，系统动态特性好，但是噪声也同时增大;在R值过大时，滤波结果较为平滑，但是滞后较为严重。通过多次尝试，最终确定卡尔曼滤波器的Q值为0.001，R值为0.005，此时，滤波后的角速度失真较小，滞后不明显，波形较为平滑，符合平衡车控制的要求。整定参数后的卡尔曼滤波器效果如图15所示。

3.2路径检测结果展示

二值化过程用于去除灰度图像中的冗余信息，将之转换为使用黑白两种颜色表示的二值化图像，以突出路径的特征。平衡车的二值化效果如图16所示。其中：左侧为灰度图像;右侧为二值化处理的结果。

如图17所示，在实际运行中，由于平衡车所处的环境复杂，上述步骤提取的路径中可能存在干扰点需要去除，以防止对后续的处理造成影响。

在平衡车行驶的过程中，还有可能由于环境的原因，导致提取到的路径不连续，甚至出现缺失的现象;因此，直接通过提取路径控制平衡车的行进是不合理的，可使用最小二乘法对路径进行擬合。对于不同类型的路径，拟合的效果如图18所示。

3.3蓝牙发送结果展示

平衡车与Android手机之间通过蓝牙进行数据交换。为了保证通信过程不发生错乱，双方通过约定的数据格式进行传输。如图19所示，平衡车正确地向Android手机发送了传感器、参数以及图像等信息。

3.4整车测试效果及PID参数确定

针对平衡车的联合调试，制作模拟路径，并进行整车的实际运行测试。平衡车行驶时的情况如图20所示。平衡车的PID参数确定主要分为速度-姿态串级PID参数的整定以及路径PID参数的整定两个部分。其中：PID控制器中的P参数是比例系数，表示系统调节的力度;I参数是积分系数，用来消除控制系统的静差;D参数是微分系数，用来抑制系统的超调，对控制量的变化起预测作用。

（1）测试中将速度环的所有参数和姿态环的D参数置为0，逐渐加大姿态环的P参数，观察平衡车的姿态控制效果。在不同P参数下，平衡车的姿态控制效果分析如表1所示。

由表1可见，加大参数P可以提高姿态控制的控制力度，但是，P参数过大又会造成平衡车前后剧烈摇晃，使控制效果变差;因此，以下调试将P参数取值为-90。

（2）在整定完姿态环的P参数后，整定D参数，即逐渐加大D参数，观察平衡车的姿态控制效果。在不同D参数下，平衡车的姿态控制效果分析如表2所示。

由表2可见，加大参数D可以显著抑制平衡车的前后摇晃，但是，D参数过大会造成平衡车出现高频颤动，并损坏电机驱动;因此，需要合理确定D参数的取值，以下调试将D参数取值为-1.10。

（3）在整定完姿态环参数后，平衡车已经能够维持短时间的平衡姿态了;但是，由于缺乏速度控制，在经过短暂的平衡后，它会加速向一个方向前进。当速度达到电机输出极限时，平衡车将无法继续控制姿态，进而倒下。因此，为了控制住平衡车在平衡状态下的速度，需要对速度环进行调参。速度环与路径PID参数整定大致步骤同上，这里不再赘述。

按照上述过程，对平衡车姿态环、速度环和路径环的参数进行了反复多次的调节，最终确定的参数如表3所示。

4结语

综上所述，基于图像信息采集的自动寻迹平衡小车设计充分发挥了STM32单片机的性能优势，使平衡车能够跟随地面上的单根黑线路径行驶;同时，针对实际运用中可能遇到的双线或者引导线不连续的情况，设计进行了算法优化。在后续的研究中，将尝试更换分辨率更高、角度更广的摄像头，同时搭配速度更快的嵌入式处理器、更优化的上位机控制软件和更完善的视觉导航算法，以进一步改进和完善该平衡车设计，使其可以真正投入市場，并广泛适用于实地勘察、目标引导、自动驾驶等现实应用场景。

参考文献：

[1]孙传开.基于视觉自主导航的自平衡车设计与实现[D].广州：华南理工大学，2015.

[2]孙俊杰.自主双轮平衡车智能控制研究[D].广州：华南理工大学，2016.

[3]赖义汉，王凯.基于MPU6050的双轮平衡车控制系统设计[J].河南工程学院学报（自然科学版），2014，26（1）：53-57.

[4]白志刚.自动调节系统解析与PID整定[M].北京：化学工业出版社，2012.

[5]田垅，刘宗田.最小二乘法分段直线拟合[J].计算机科学，2012，39（S1）：482-484.

[6]时为.STM8S103F3P单片机正交编码器接口的使用方法[J].扬州职业大学学报，2017，21（1）：47-49.

[7]叶俊明，马海琴.基于MPU6050的陀螺仪测控系统设计[J].中国高新技术企业，2015（27）：26-27.

[8]李斐然.基于霍尔原理的绝对式磁编码器的研究[D].哈尔滨：哈尔滨工业大学，2015.

[9]秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].3版.西安：西北工业大学出版社，2015.

[10]王迪.基于卡尔曼滤波的惯性导航技术[D].北京：北方工业大学，2018.

[11]王海菊，谭常玉，王坤林，等.自适应高斯滤波图像去噪算法[J].福建电脑，2017（11）：5-6.

[12] GANG W，CHOI S，LEE J. Balance control of a 12-DOF mobilemanipulator based on two- wheel inverted pendulum robot[J].Journal of Measurement Science and Instrumentation ，2013，4（1）：52-57.

[13]郑昊.基于Arduino/Android的蓝牙通信系统设计与实现[D].武汉：湖北大学，2012.

Design of Automatic Tracing Balance Car Based on Image Information Acquisition

CUI Yuan，JI Fengxin，CHEN Zhuyang，GAO Qian，QIAN Zheng

（School of Electricity and Information Engineering，Jiangsu University of Technology，Changzhou 213001，China）

Abstract：Aiming at the two key technical difficulties of tracking self-balancing vehicle，namely upright control and visual navigation，STM32F407VET6 single-chip microcomputer is used as the control core in this paper. On the one hand，the accurate vertical angle information is obtained by Kalman filter using the collected acceleration and angular velocity data of MPU6050 six-axis sensor. Combined with the speed attitude cascade PID controller，the response speed of the motor to the error is accelerated，and the accurate control of the upright of the balance vehicle is realized. On the other hand，the image information of ov7670 camera is collected by DMA mechanism，and processed by graying，Gaussian image filtering，image binarization，path extraction and fitting，so as to obtain accurate path information and realize the visual navigation of balance vehicle. In order to enhance the humancomputer interaction experience，the key data of the balancing car will also be transmitted to the Android App in real time，and the balancing car can be controlled through the App.

Key words：STM32;self-balancing vehicle;Kalman filtering;tracking;cascade PID;Android