双超声波数据融合的两轮机器人平衡行进策略研究

舒英利， 寇 磊， 苑全德， 皮玉珍

(1.长春工程学院电气与信息工程学院，吉林 长春 130012；2.东北电力大学信息工程学院，吉林 吉林 132012)

双超声波数据融合的两轮机器人平衡行进策略研究

舒英利1， 寇 磊2， 苑全德1， 皮玉珍1

(1.长春工程学院电气与信息工程学院，吉林 长春 130012；2.东北电力大学信息工程学院，吉林 吉林 132012)

针对移动轮式自平衡机器人在生产应用中存在的问题，对其结构特点与姿态平衡的控制策略进行了研究。为了保障机器人的平衡与协调运动，提出了基于双超声波测距数据融合的平衡和行进策略。采用双超声波数据融合平衡算法，在Arduino主控的机器人上实现了自平衡姿态的控制，对双超声波传感器数据信息进行处理，从而保证了机器人的自平衡运动；同时，在机器人的运动过程中加入了PID控制算法，以调整机器人的运动状态及加快机器人对信号误差处理的速度。试验与仿真结果表明，该策略设计有效可行，双超声波自平衡机器人可以保持较好的平衡姿态，行进控制与基本方向控制也有效可靠，体现了较好的平衡稳定性和平衡协调性。在平衡算法控制下，双超声波机器人不仅实现了平衡行进，还实现了行进策略下的平衡后退以及方向控制下的平衡转弯，具有广阔的应用前景。

机器人; 传感器； 超声波； 数据； 控制； 平衡； 策略

0 引言

在复杂、狭窄的大转角工作环境下，两轮自平衡移动式机器人可以灵活、快捷地执行任务。这种移动式机器人两轮共轴，机身重心置于轮轴重心上方，可凭借前后运动保持机身的相对平衡，实现动态平衡的直立行走。由于具有体积小、运动灵活、适应地形变化能力强等优点，两轮自平衡移动式机器人可完成一些狭小环境内的任务[1-2]。在复杂环境下能对环境高效感知和及时调整自身姿态，是自平衡机器人的必备功能。由于单一的传感器(比如超声波传感器)感知能力有限，因此需要由多个传感器交互实现有效协调，提高工作效率，同时在不同的应用空间与任务空间中实现多传感器数据融合[3-5]。

为改进两轮自平衡移动式机器人的性能，提出了两轮自平衡机器人姿势平衡和行进策略，利用双超声波测距检测平衡与行进特征，通过不断检测收集超声波传感器与地面的距离，进行数据滤波融合，以提高两轮自平衡机器人的控制效果[6-7]。双超声波检测提高了控制精度和集成度，在保证良好的平衡姿态和移动控制效果的前提下，有效降低了整体生产成本，并运用配套的数据融合处理算法，实现了对平衡姿态与行进姿态的调控。

1 双超声波姿态平衡动力学模型

两轮自平衡移动式机器人的基本平衡控制原理为:当检测到车体产生倾斜时，对主控系统通过检测得到的数据进行融合、得出矫正的角度值与输出的占空比。自平衡机器人主要通过驱动2个轮子的电机所产生的转动力矩来控制机身倾斜方向的速度v，通过控制电机控制机身平衡姿态的调整，让两个轮子朝机身倾斜倒下的方向运动，进而保持机身的动态平衡。平衡控制过程中，直接控制量为轮子的转动力矩[8]。

(1)

由式(1)得速度模型：

(2)

自平衡机器人受力分析图如图1所示。图1中，O为轮轴中心，P为机身重心，机体质量为m，重力加速度为g，机轴倾角为α，超声波A直射到地面的距离为La，超声波B直射到地面的距离为Lb，重心距机身下的平台的距离为L。设机体以加速度a向一边倾斜，则受到水平方向的最优惯性合力为：

∑F*=Ma

(3)

若要使机身保持平衡状态，则∑M=0(M为力矩)。以O点为转轴中心，支持力N作用在转轴上，力矩M=0，即:

mgLsinθ+F*=0

(4)

图1 自平衡机器人受力分析图

2 双超声波测距控制方案

本研究中的自平衡机器人应用2个超声波传感器模块检测距离数据。选用HC-SR04超声波测距模块，Arduino主控板通过一阶滤波的数据融合换算后得出机身当前状态的角度值α与角速度ω，再经过PID参数整定生成对应的PWM输出值，形成一个“双超声波测距、定位高度比较计算、角度值偏差计算、矫正数据融合、偏差调整-输出控制”的自平衡实现过程。

2.1 超声波测距原理与误差调整

采用时间差测距的方式来完成距离测量，超声波传感器发射端向探头正方向发射特定频率的超声波，内部电路的定时器在发射超声波时开始计时。超声波发射与回响检测时序图如图2所示。

图2 超声波发射频率时序图

发射电路会根据所给的脉冲发出特定频率的超声波，超声波碰到被测物界面则会被反射回接收端。文献[9]采用了超声波传感器接收探头接收到反射波后定时器停止计时的方法，进而得出超声波发射和接收回波的时间差t。超声波在传播介质中的传播速度为c(在空气中为340 m/s)。通过计算，得出从TXD端到被测界面之间的距离s为：

s=(ct)/2

(5)

超声波应用中，可由式(5)得出超声波发出速度vi与接收端接收信号所需来回越渡时间的关系为：

(6)

由于超声波频率较高，可满足多种用途的测量需要。但在低维度空间中，超声波测量会受到周边温度、自身响应冗余、回波迟缓、传播介质等因素的影响。超声波在固体中传播速度最快，液体次之，在气体中传播速度最慢。声速c的表征速度与传播介质的温度有关。为了实现超声波传感器的精确测量，文献[9]提出在硬件中设置温度补偿电路，以补偿因温度变化而带来的声速误差。如果环境温度变化不明显，则无需设置温度补偿。

(7)

(8)

针对这一误差，在满足超声波传感器的主要电气参数的条件下，本系统选用了具有回声保护的HC-SR04超声波模块，并且尽可能地把超声波模块A、B隔离开。两轮结构也在一定程度上阻挡了超声波的互相干扰。

2.2 超声波数据融合平衡算法

通过超声波测距获取机器人在XOY极坐标平面内角度θ的变化，以实现机身的平衡。当两轮机器人平衡时，需满足以下角度值换算公式：

(9)

式中：L为机身前后的2个超声波模块A、B间的直线距离；λ为平衡点高度。

在自平衡机器人基于双超声波的检测中，假设采样周期为100ms，在k时刻获取超声波实测值a、b,并结合(k-1)时刻的距离预测值。根据测量的预测协方差S对k时刻的测量值进行滤波估计修正，进而对(k+1)时刻的距离进行预测。

测距获取的平衡角度模型如图3所示。

图3 测距获取的平衡角度模型

在估计平衡姿态机器人的运动状态时，机器人的超声波传感器坐标采用极坐标系。设系统的状态向量E(k)为：

(10)

系统平衡E(k)的状态表达式为：

研究结果显示，‘索邦’花青素苷合成转录因子LhsorMYB12编码区序列长720 bp，核苷酸序列长863 bp(登录号：MG593164)，共编码239个氨基酸(图2)。

E(k)=AE(k-1)+BU(k)+W(k-1)

(11)

式中：E(k)为第k次超声波测量时的状态向量;A为角度值的表征参数；W为角速度的表征参数；U(k)为平衡系统的速度控制量；W(k-1)为上一次检测(k-1)状态的角速度对当前k状态的噪声干扰。

由于超声波测距的采样周期t为300 ms，而机器人保持平衡的运动速度较为平缓，所以在局部时间段内设定调整过程为近似匀速运动，x方向的运动方程为：

(12)

式中:tk为第k次超声波测量状态时的采样周期。

(13)

同理，可得y方向的运动方程为：

(14)

由式(14)可得平衡系统的状态方程为：

E(k)=A×E(k-1)+W(k-1)

(15)

(16)

经过PID数据整定后的PWM输出方程为:

PWM|out=kapA+kadW+ksp×S+ksi×P

(17)

基于Arduino的双超声波机器人平衡驱动系统经过PID调整后，输出实现姿态的平衡。具体的超声波双向测距定位平衡模型如图4所示。由模型图可得测量值Z(k)为：

图4 双向测距定位平衡模型

由于各状态变量的误差符合均值为0的高斯分布，故平衡策略下的测量方程为：

Z(K)=CE(k)+V(k)

(18)

式中:C为测量系统的表征参数，当系统为多测量系统时，C为矩阵;V(k)为测量系统在k状态下的噪声误差。由平衡系统在极坐标下的双超声波测距径向距离和融合后偏差方向角的测量数据可得:

(19)

测速码盘对电机编码器产生的PWM脉冲进行记数，监测机身的位移与轮子的速度。根据超声波传感器和姿态换算监测车体运动姿态信号，经过双超声波数据融合流程，文献[11]通过相关的控制算法计算出输出控制电压的数值大小，对双超声波的2个驱动电机进行控制，进而调整车体平台的行进姿态与平衡位置，从而使机身保持良好的运动协调性和平衡状态[11-12]。

数据优化融合前的PWM输出近似波形与融合优化后的实测波形如图5所示。从波形对比中可以看出，经过数据优化融合后的波形更为稳定、平缓，姿态控制效果更好。

图5 数据优化融合前后PWM波形对比图

2.3 控制方案与行进策略

Arduino如今已被运用到各个领域，是一款使用Atmel Atmega328微处理控制器的开源控制板。其开发操作界面和搭建外设环境简易，有0～13个数字模拟I/O、0～5个Analog I/O，控制效果显著。自平衡机器人采用超声波电路Arduino为核心控制模块，电机驱动模块为L298N，电机为TT马达，并以2个HC-SR04作为超声波测距模块，采用7.4 V/2 200 mAh锂电池供电。电位器用于平衡点设置与PID中kp、ki和kd系数的调试，方便数据的更新与调试，有效减少了开发时间与生产成本。

双超声波自平衡机器人的行进控制主要通过PWM占空比输出控制电机转速大小，进而调整角度值的偏差，使得角度值趋近系统平衡零点。

控制部分由无线蓝牙遥控的代码实现，功能实现为“前进状态”、“后退状态”、“左转状态”、“右转状态”。从程序初始化参数设置可以看出，双超声波自平衡机器人的行进控制方案是先调整平衡点和PID设定系数，然后调整左右电机的PWM输出，从而实现行进角度的控制与电机控制[13]。

3 试验与仿真

双超声波机器人运用了主控Arduino，以简单、快捷的编程实现与I/O接口连接，通过双超声波传感器进行环境测距定位，并利用控制算法进行机器人姿态平衡控制。在试验验证方面，通过仿真显示超声波采集到的回声检测数据。由超声波接收端接收到的回声检测波形图可知，平衡系统的姿态摇摆幅度不大，基本处于平衡零点上下，具备基本稳定性与数据的可计算性。

假设2个超声波传感器经过互补滤波后，换算出的角度数据为M1，超声波采集数据为s，超声波传感器测距换算出的偏差角度值相对加速度的因子为k，前一次超声波传感器经过滤波后的角度数据为M2，增益选择固定值为g。滤波后的角度数据为realM，最后得出融合出的角度数据。

试验结果证明，双超声波自平衡机器人能有效保持平衡，在平衡算法控制下能平衡行进；可实现行进策略下的平衡后退和方向控制下的平衡转弯，稳定效果好。

4 结束语

为实现基于Arduino的双超声波机器人的姿态平衡与行进控制，提出了双超声波传感器数据融合策略。试验与仿真结果表明，该策略有效可行，双超声波自平衡机器人可以保持较好的平衡姿态，行进控制与基本方向控制有效可靠，体现出了较好的平衡稳定性和平衡协调性。该策略整体硬件开发周期相对较短，选用材料性价比高，能满足在较好控制效果的前提下降低生产成本的要求。在接下来的工作中，将对多超声波定位自平衡机器人在上下坡和不平整复杂行动界面上实现姿态平衡进行研究。

[1] 刘斌.两轮自平衡小车软硬件研发与基于模糊线性化模型的变结构控制研究[D].西安：西安电子科技大学，2009.

[2] 陈伟，延文杰，周超英，等.两轮自平衡机器人控制系统的设计[J].传感器与微系统，2008，27(4):117-120.

[3] 闫飞，庄严，王伟.移动机器人基于多传感器信息融合的室外场景理解[J].控制理论与应用，2011，28(8):1093-1098.

[4] 苏志毅，赵伟，黄松岭.多传感器信息融合技术在现代测量领域的地位和重要作用[J].电测与仪表，2013，50(3):1-5.

[5] 王洪波.物联网信息融合技术及存在的问题研究[J].计算机应用研究，2013，30(8):2252-2255，2261.

[6] 陈辉，邓记才，吴晓辉，等.多传感器信息融合在轮式机器人运动控制中的应用[J].传感技术学报，2011，24(6):915-918.

[7] 张伟民，段晓明，赵艳花.两轮自平衡小车控制研究[J].自动化技术与应用，2011，30(4):10-13.

[8] 孙建勤，何雅静，孟祥忠.两轮不平衡小车的优化模糊控制研究[J].计算机仿真，2011，28(2):184-186，406.

[9] 张剑，王维，何俊峰，等.基于双超声波模块的新型定位方法[J].传感器与微系统，2012，31(9):22-24，31.

[10]蒋林，闫继宏，臧希喆，等.一种新的超声波绝对定位方法[J].吉林大学学报(工学版)，2009，39(1):188-193.

[11]蔡建羡，阮晓钢.OCPA仿生自主学习系统及在机器人姿态平衡控制上的应用[J].模式识别与人工智能，2011，24(1),138-146.

[12]朱效洲，李宇波，卢惠民，等.基于视觉的移动机器人可通行区域识别研究综述[J].计算机应用研究，2012，28(6):1135-1137.

[13]阮晓钢，蔡建羡，陈静.强化学习规则的两轮机器人自平衡控制[J].计算机测量与控制，2009，17(2):321-323.

Research on the Balanced Traveling Strategy Based on Dual Ultrasonic Data Fusion for Two Wheeled Robot

SHU Yingli1,KOU Lei2,YUAN Quande1,PI Yuzhen1

(1.School of Electrical and Information Engineering， Changchun Institute of Technology， Changchun 130012，China; 2.School of Information Engineering，Northeast Dianli University， Jilin 132012，China)

Aiming at the problems in production and application of the mobile self-balanced robot,its structural features and control strategy of posture balancing are researched.In order to ensure balanced and coordinated movement of the robot,the balanced traveling strategy based on dual ultrasonic distance measurement of data fusion is proposed.By utilizing the dual ultrasonic data fusion balancing algorithm,the control of self-balanced pose can be achieved in Arduino-controlled robots.In order to ensure the self-balanced motion of the robot,the dual ultrasonic sensor data information is processed;at the same time,the PID control algorithm is added in robot motion process,to adjust the motion status of robot and the speed of error processing is greatly improved.The results of experiment and simulation show that the proposed method is effective and feasible.The dual ultrasonic self balancing robot can maintain a good posture balance,and the moving control and basic direction control are effective and reliable,which reflects the better balance stability and coordination.Under control of balancing algorithm,the dual ultrasonic robot is not only balanced traveling,but also implementing balanced retreat,and balanced turning around under marching strategy,this possesses wide application prospects.

Robot； Sensor； Ultrasonic； Data； Control; Balance; Strategy

吉林省教育厅“十二五”科学技术研究项目(2014324)

舒英利(1965—)，男，硕士，高级试验师，主要从事电力电子应用等研究。E-mail：shuyl@scitimes.com。 皮玉珍(通信作者)，女，硕士，主要从事智能电网技术、物联网技术等方向的研究和应用。E-mail：piyuzhen@gmail.com。

TH-3；TP27

A

10.16086/j.cnki.issn 1000-0380.201702010

修改稿收到日期:2016-06-22