基于Android的双轮机器人控制系统研究与设计*

王素青，黄文龙

(南京航空航天大学 金城学院，南京 211156)

基于Android的双轮机器人控制系统研究与设计*

王素青，黄文龙

(南京航空航天大学 金城学院，南京 211156)

双轮机器人系统具有与传统倒立摆相似的多变量、非线性、强耦合和参数不稳定等特点。针对系统的复杂性，文章提出了基于Android平台和蓝牙通信模块实现对移动机器人的无线控制方式，采用基于DMP和Kalman滤波相结合的姿态数据处理算法及双闭环PID控制算法相结合的控制方法，实现双轮机器人的自平衡控制。文中创新地在系统上增加两个机械臂，借助机械臂能够使系统快速恢复平衡以及当系统休眠时，借助机械臂保持平衡实现节能的目的。测试结果表明，该系统能够通过蓝牙准确地发送、接收数据，控制精度高、系统稳定性好、时间响应快。

双轮机器人；Android；蓝牙；DMP和Kalman滤波；PID控制

0 引言

随着传感器技术、通信技术、自动控制技术和人工智能技术的快速发展，机器人在工业、农业、医学、军事等领域的应用越来越广泛。双轮机器人因其运动灵活、结构简单、能够适应不同的工作环境等特点，在军事领域及其他领域有着广泛的应用前景。双轮机器人具有与传统倒立摆相似的多变量、非线性、强耦合和参数不稳定等特点，是研究多种控制方法的理想平台[1]。针对双轮自平衡机器人系统的复杂性，国内外很多学者对其控制方法进行了一定的研究，主要有PID控制法、状态反馈控制器(LQR)控制法[2]、模糊控制法、神经网络控制法[3]等等。目前，较成熟的机器人控制方式主要有[4]：语音控制方式、红外线遥控方式、基于WiFi无线网络控制方式、基于GSM/GPRS网络的远程控制方式等，这些控制方式基本满足了现有机器人的应用需求。但是这几种控制方式存在不便于携带、移动性较差、且控制环境需要网络等问题。

针对以上问题，并且基于成熟的蓝牙技术和Android在移动互联网市场份额的不断增长，本文提出了利用手机的蓝牙通信功能实现对移动机器人无线控制的解决方案，系统的控制方法则采用的是基于DMP和Kalman滤波相结合的姿态数据处理算法及双闭环PID姿态控制算法，实现双轮机器人的自平衡控制。另外，在系统硬件结构上，创新地增加两个机械臂，其作用有两个方面：①在系统失去平衡时，能够借助机械臂使系统快速恢复平衡；②在不需要工作时，可遥控系统使其处于休眠状态，仅借助机械臂保持系统平衡，达到节能的目的。

1 系统的硬件设计

双轮机器人控制系统要实现的功能为：在没有外界作用力的情况下，仅依靠自身控制系统，完成上位机发出的指令动作(自启、休眠、前进、后退、左转、右转和原地等待等)，在运行过程中能保持姿态的平衡。根据系统功能要求，所设计的系统硬件电路结构框图如图1所示，系统由上位机和下位机两部分组成，上位机采用带蓝牙通讯功能的Android手机，下位机主要由微处理器模块、姿态检测模块、机械臂模块、直流电机驱动模块、编码器、蓝牙通信模块及电源模块等几部分构成。电源模块负责系统各个模块控制电路的电源。微处理器模块是下位机的核心部件，用来接收姿态检测模块实时采集的姿态数据，根据上位机指令控制系统进行相应的动作。姿态检测模块实时对系统的姿态信息进行采集并将数据发送给微处理器。机械臂模块根据微处理器输出的PWM信号控制机械臂的相应动作。直流电机驱动模块根据微处理器输出的PWM信号控制直流电机的速度和方向。编码器负责实时对直流电机的速度和方向进行测量，并将测量数据发送给微处理器，形成闭环控制。蓝牙通信模块负责上位机和下位机之间的数据进行无线传输。

图1 系统的硬件电路结构框图

(1) 微处理器模块

系统中微处理器模块选用的是意法半导体公司的STM32F103C8T6，它是一种基于Cortex-M3内核的32位CPU，最高工作频率为72MHz，片上集成32-512KB的Flash存储器，6-64KB的SRAM存储器。片上集成了SPI，I2C和USART等通信接口，可以方便地和姿态检测传感器和蓝牙通讯模块进行数据交换。含有7个定时器，可输出多路PWM信号能确保电机驱动模块获得稳定的PWM波形，避免软件的频繁中断为系统带来不稳定因素。

(2)姿态检测模块

系统中姿态检测模块选用的是美国InvenSense公司的MPU-6050，整合了3轴陀螺仪和3轴加速度计，能够准确的对机器人的姿态进行实时检测[5-6]。系统中MPU-6050作为从机使用，通过SDA和SCL端口和主机STM32进行通信，其检测的数据经过内置的16位AD转换器进行转换，然后将16位数字量通过I2C总线接口发送给主机。

(3) 电机驱动模块

系统中直流电机驱动模块选用的是TOSHIBA公司的一款具有大电流MOSFET-H桥结构，双通道电路输出的TB6612FNG驱动模块，它和传统的L298N相比，不需外加散热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机，体积减小很多。

(4) 机械臂模块

系统中机械臂模块选用的是Tower Pro公司生产的MG996R大扭矩舵机驱动器，采用金属齿轮及轴承，驱动信号为PWM，根据PWM信号的占空比不同转到对应不同位置，其具有扭矩大、反应快、精度高、性价比高等特点。

(5) 蓝牙通信模块

系统中无线数据通信模块选用的是HC-06蓝牙模块，该模块内置蓝牙V2.0协议标准的CSR蓝牙通信芯片，可以与手机上的蓝牙进行匹配。HC-06模块和Android手机通过串口形式进行数据无线传输。

2 系统的下位机软件设计

2.1 系统的下位机总体流程设计

系统的下位机主程序流程图如图2所示，主要由初始化程序、下位机与上位机的通讯程序、传感器数据采集程序、Kalman滤波程序、直流电机控制程序和舵机控制程序等几部分组成。

系统启动后，首先进行初始化，然后执行下位机与上位机的通讯程序，判断是否已连接上位机，若连接上，则根据上位机的控制指令执行相应的动作。每隔5ms采集一次MPU-6050传感器的角度和角速度数据，然后调用Kalman滤波程序计算出最优估计值，根据直流电机控制程序中双闭环PID控制算法计算出PWM信号，根据PWM的输出来控制直流电机的速度和方向。舵机控制程序根据上位机的控制指令来判断机器人系统的两个机械臂是收起还是放下。

图2 系统的下位机主程序流程图

2.2 数据滤波处理

由于自平衡机器人系统需要在动静态情况下均能准确的测量出机器人的姿态数据，因此需要对陀螺仪和加速度计的采集数据进行滤波处理而得到最优值，本系统采用DMP滤波和Kalman滤波相结合的方案实现对机器人不稳定的姿态数据进行滤波处理[7]。

(1)DMP滤波

DMP滤波是采用MPU-6050自带的数字运动处理器(DMP：Digital Motion Processing)，它可接收并处理来自陀螺仪和加速度计的采集数据，处理结果可直接从DMP寄存器读出，且比直接从陀螺仪和加速度计读出的数据更加稳定。

(2)Kalman滤波

Kalman滤波算法是以最小均方误差为最佳估计准则的递归数据处理算法，利用前一时刻的估计值和当前时刻的测量值来更新对状态变量的估计，最终求出当前时刻的最优估计值。

根据Kalman滤波原理，要估计k时刻的倾角，需要根据k-1时刻的倾角值来预测k时刻倾角，并得到k时刻的高斯噪声偏差。系统中，陀螺仪测量的角速度作为预测值的控制量，加速度计测量的角度作为测量值，根据两者的协方差不断进行递归算法直到估计出最优倾角。Kalman滤波方程如式(1)～式(5)所示[8]。

X(k|k-1)=AX(k-1|k-1)+BU(k)

(1)

P(k|k-1)=AP(k-1|k-1)AT+Q

(2)

Kg(k)=P(k|k-1) HT/(HP(k|k-1)HT+R)

(3)

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))

(4)

P(k|k)=(I- Kg(k)H)P(k|k-1)

(5)

式(4)为状态估计方程，Z(k)=Accel为加速度计的角度测量值。

2.3 姿态控制算法

经过硬件滤波和软件滤波后的机器人姿态数据，其数据质量有明显改善，有利于对机器人进行姿态控制。本系统采用双闭环PID控制算法对机器人系统进行自平衡控制，其结构框图如图3所示，速度闭环控制作为系统的内环，姿态闭环控制作为系统的外环。

姿态闭环控制[11]采用了PD控制器，利用数据滤波处理后的倾角和角速度的最优值和给定姿态倾角进行比较，得到误差信号，然后经过PID控制器处理后得到输出值，其输出值作为内环的输入量。PID的输出控制电机PWM信号的占空比，表达式为：

PWM=Kp×θ+Kd×θ·

(6)

速度闭环控制采用了P控制，利用增量编码器检测到的实时速度和给定速度进行比较得到误差信号，然后经过PID控制器处理后得到输出值，其输出作为电机PWM信号占空比的增量值，表达式为：

ΔPWM=Ksp×speed

(7)

姿态闭环控制和速度闭环控制是相互影响的[12]，将其融合后的PWM信号为：

PWMzong=Kp×θ+Kd×θ·+Kps×speed

(8)

图3 系统双闭环PID控制算法框图

经过PID控制算法后得到PWM信号，由微处理器直接发送给直流电机驱动模块，从而控制直流电机的速度和方向。电机驱动模块输出两路PWM信号，使用时通过给其中一路占空比为零，另外一路不为零的方法来控制电机的正反转[13]。

3 系统的上位机软件设计

上位机软件运行平台采用Android平台，由运行在Android手机上的APP应用程序来实现机器人的无线控制，且Android手机能够实时接收、存储和显示系统的姿态信息。Android应用程序采用Java语言编程软件实现，其程序流程图如图4所示，主要由蓝牙通信程序设计[14]、UI控制程序设计、UI显示程序设计。

图4 系统的上位机程序流程图

3.1 蓝牙通信程序设计

Android设备和蓝牙模块进行通信，需要完成4个步骤[15]：①蓝牙打开：使用getDefaultAdapter( )方法来判断设备是否支持蓝牙传输功能，确认有蓝牙后再调用isEnabled( )方法来检查蓝牙是否可用，打开蓝牙。②蓝牙搜索：调用startDiscovery( )方法搜索附近已配对或可用的设备，将搜索到的设备名称和MAC地址存储在列表中。③蓝牙连接：蓝牙配对后还必须通过蓝牙连接，连接成功后才可以进行数据传输，手机蓝牙和蓝牙模块要彼此共享RFCOMM通道，调用Bluetoothdevice类中的createRfcommSocketToServiceRecord(MY-UUID)方法，获取BluetoothSocket后才能建立连接。④数据通信：连接成功后，手机蓝牙和蓝牙模块拥有同一个BluetoothSocket，通过getInputStream( )和getOutputStream( )方法获取输入输出数据流，然后调用write( )、read( )方法写入或读出数据，实现蓝牙数据的通信。蓝牙通信程序设计即按照蓝牙通信的4个步骤进行相应的程序设计。

3.2 UI控制程序设计

UI控制程序设计是指在上位机上编写界面及程序用以实现对机器人的远程控制。UI控制界面上共布局9个Button按钮，分别控制系统的运行状态，按钮布局如图5所示，界面中间的5个按钮分别控制的动作为：左转、前进、右转、后退、停止(中间按钮)，界面下方的4个按钮分别控制的动作为：休眠、附加功能(待扩展用)、机械臂收、机械臂放。每个按钮组件的属性都需进行设置，此外，每个按钮组件都绑定一个监听事件，当单击某一按钮时，就会启动对应的监听事件onClick(View arg0)，从而执行监听事件里的程序，即执行代码sendMessage(“ ” +1)(每个按钮对应一个字符串：+1～+9，此次“+1”代表“前进”按钮)，调用sendMessage( )函数，执行函数中代码：byte[] send=message.getBytes( ); mChatService.write(send)，调用write( )函数，执行函数中代码：mmOutStream.write(buffer);mHandler.obtainMessage(BluetoothChat.MESSAGE_WRITE,-1,-1,buffer).sendToTarget()，将数据写入到缓冲区并给mHandler发送消息。

3.3 UI显示程序设计

UI显示程序设计是指在上位机界面上实时显示远程机器人的当前倾角和实时速度。UI显示界面上共布置2个TextView组件，组件的名称属性设置语句分别为：android:id=“@+id/qingjiao”，android:id=“@+id/sudu”，组件初始化语句分别为：qingjiao.setText(“当前倾角: ”)，sudu.setText(“实时速度: ”)。Android设备和蓝牙模块都有一个ConnectedThread线程，一旦有数据进来，就会调用mHandler. obtainMessage(BluetoothChat.MESSAGE_READ,bytes,-1,buffer).sendToTarget()，即给mHandler发送消息。当前倾角和实时速度的信息更新是利用read( )方法从缓冲区读取数据，其更新代码为：qingjiao.setText(“当前倾角: ”+readMessage.substring(1)); sudu.setText(“实时速度: ”+readMessage.substring(0))。

4 系统调试

4.1 系统的整体调试

调试之前，先检查系统的接线是否正确，供电是否正常，硬件检查没有问题后便可进行下载程序，然后分模块进行调试，具体为：测试电机驱动电路，测试机械臂模块，测试姿态检测模块，测试蓝牙通信模块等，调试都通过后即可进行整体调试。

在手机上安装上位机APP应用程序：BluetoothChat.apk，打开程序，进行蓝牙配对和连接，连接成功后操作手机控制界面上不同功能的按钮，系统能够完成和按钮所对应的动作。系统在运行时，手机显示界面上能够实时显示当前倾角和实时速度具体数值并且数值能够更新。

4.2 系统的PID参数整定

系统的PID控制参数需要通过多次实验进行试凑法进行整定，整定出能使系统平衡的最佳参数。

姿态闭环PID控制的调试过程：首先调整Kp值，让系统能站立且抖动相对较小；接着保持Kp不变，调整Kd值，进一步减少抖动；最后微调Kp和Kd，使系统处于最佳状态，经多次调试得到，当Kp=4.45、Kd=0.75时，系统能够在10s内达到平衡状态。

采用同样方法，对速度闭环PID参数进行多次调试，当Ksp=6.5时，系统能快速稳定的达到平衡状态。

4.3 系统的动态调试

Kalman滤波方程中的协方差矩阵Q中D(Angle)和D(Q_bias)的不同取值代表滤波器对传感器的不同信任程度且对滤波效果造成不同影响。通过多次实验修正，当D(Angle)值为0.001，D(Q_bias)值为0.003，角度测量噪声值R_angle值为0.5时，滤波器能达到较好的滤波效果。

当系统失去平衡时(平衡条件：连续50组传感器输出倾角均小于±100)，通过12次实验测试可得：在有机械臂情况下，系统能够在10s内重新恢复平衡；在无机械臂情况下，系统能够在25s内重新恢复平衡。表1中仅列出了6组实验测试数据。

表1 系统恢复平衡的时间比较表

系统平衡时，施加一个外力使系统倾角±45°≤θ≤±60°时，要求系统能迅速响应这个外力作用，并能快速减小倾角变化，重新建立平衡状态。通过多次调试及修正PID参数，最终系统受到这样的外力作用时，能够在25s内通过自身调节达到自平衡状态。系统运行时，手机控制界面如图5所示，此时系统处于“左转”运行状态，当前倾角为-5.737°，实时速度为1.867转/s。机器人平衡时效果图如图6所示。

图5 手机控制界面

图6 机器人平衡时效果图

5 结束语

本文研究和设计了一个基于Android的双轮机器人控制系统，利用手机的蓝牙功能实现对移动机器人的无线控制，系统采用基于DMP和Kalman滤波相结合的姿态数据处理算法及双闭环PID姿态控制算法，实现系统的自平衡控制。系统能够借助机械臂，使其快速恢复平衡或实现节能的目的。实验结果表明，通过蓝牙可以实时地、灵活地控制机器人的运行状态，并能实时显示当前姿态信息，系统也能快速稳定地达到自平衡状态。文中提出的无线控制方案及控制方法在小型移动机器人系统的控制方面有一定的应用前景。

[1] 季鹏飞，朱燕，程传统,等. 基于STM32的两轮自平衡小车控制系统设计[J]. 电子科技，2014，27(11)：96-99.

[2] 武俊峰，孙雷. 两轮自平衡机器人的控制方法研究[J]. 哈尔滨理工大学学报，2014，19(6)：22-26.

[3] 林文建，钟杭，黎福海,等. 两轮自平衡机器人控制系统设计与实现[J]. 电子测量与仪器学报，2013，27(8)：750-759.

[4] 李瑞，李晓明. 基于Mobile-Android小型移动机器人平台控制系统[J]. 机电工程，2013，30(11)：1414-1419.

[5] 卢秀和，李建波. 基于STM32的四旋翼姿态控制系统[J]. 计算机测量与控制，2014，22(3)：761-763.

[6] 杨莘，刘海涛. 基于STM32的两轮自平衡遥控小车[J]. 数字技术与应用，2014(5)：151-152.

[7] 万云霞，许伦豹，胡龙,等. 基于Arduino的四旋翼飞行器控制系统设计[J]. 吉林大学学报(信息科学版)，2015，33(4)：389-396.

[8] Liu Kun,Ming Bai,Yuhua Ni. Two-wheel self-balanced carbased on Kalman filtering and PID algorithm[C].Industrial Engineering and Engineering Management (IE&EM) 2011 IEEE 18thInternational Conference, 2011, (1) : 281-285.

[9] 张团善，何颖. 卡尔曼滤波在两轮自平衡代步车姿态检测中的应用[J]. 单片机与嵌入式系统应用，2014(5)：33-35.

[10] 杨凌霄，李晓阳. 基于卡尔曼滤波的两轮自平衡车姿态检测方法[J]. 计算机仿真，2014，31(6)：406-409.

[11] 马思远，鲁庭勇，张丽君. 两轮自平衡车运动姿态的测量和控制[J]. 测控技术，2015，34(4)：71-73.

[12] 梁光胜，杜梦楠，周子豪,等. 基于互补滤波的两轮自平衡车姿态控制[J]. 测控技术，2015，34(5)：72-74.

[13] 周牡丹，康恺，蔡普郎，等. 两轮自平衡车控制系统的设计与实现[J]. 自动化技术与应用，2014，33(10)：4-8.

[14] 龚成莹. 一种基于蓝牙的无线传感网测控小车设计[J]. 无线电工程，2016，46(5)：72-74.

[15] 汪国磊. 基于Android的数据采集处理实验系统设计[J]. 数字通信，2014，41(5)：70-73.

(编辑 李秀敏)

The Research and Design of the Two-Wheel Robot Control System Based on Android

WANG Su-qing, HUANG Wen-long

(JinCheng College, Nanjing University of Aeronautics and Astronautics， Nanjing 211156, China)

The two-wheel robot system had the traditional pendulum similar multivariable, nonlinear, strong coupling and parameter instability and so on. For the complexity of the system, realization of wireless mobile robot control based on the Android platform and Bluetooth communication module is proposed in this paper. The self-balancing control of the two-wheel robot is achieved using a combination of control methods which are attitude data processing algorithm based on DMP and Kalman filter and double-loop PID control algorithm. The two arms are increased innovatively on the system, with which can quickly restore balance and maintain balance in order to achieve energy-saving purposes when the system is dormant. The test results show that the system can accurately send data and receive data via bluetooth. The system has high accuracy, good stability, and fast response time.

two-wheel robot; Android; bluetooth; DMP and Kalman filter; PID control

1001-2265(2016)12-0060-05

10.13462/j.cnki.mmtamt.2016.12.017

2016-08-08；

2016-09-18

南京航空航天大学金城学院大学生实践创新训练计划项目(2015048YP)

王素青(1976—)，女，江苏盐城人，南京航空航天大学金城学院讲师，硕士，研究方向为网络控制系统，(E-mail) wsq0214@nuaa.edu.cn。

TH166;TG659

A