，，，，，，

（南昌大学信息工程学院，江西南昌 330031）

1 引言

两轮机器人是一种两轮左右并行结构的新型移动机器人，由于其占地空间小，运动灵活，近年来发展迅速，Segway公司生产的两轮警用车［1］已经被广泛运用于安保巡逻。德国Transporter公司研发的两轮摄影车，可以供摄像工作者在体育运动场或新闻现场使用，还可用于电视或电影的拍摄。同时两轮机器人系统具有多变量、高阶次、强耦合、非线性等特性［2］，它也成为了研究人员检验新型控制策略和方法的重要模型［3］。

国内外已有不少自平衡算法的研究，有学者采用状态反馈［4］、极点配置［5］、LQR［6］等基于现代控制理论的控制方案，它们虽然能得到良好的控制效果，但这些算法依赖精确的数学模型、实现和参数整定复杂、鲁棒性不强。中国台湾Tsai等采用人工神经网络的方法［7］为两轮机器人设计控制器，虽然有适应能力强、不需要建立数学模型等优点，但同时存在计算量大、实时性较差、学习效果不确定性等缺点。

本文提出了一种双回路PID控制方法，能实现机器人倾角与速度两个相互耦合变量的控制。在倾角的测量过程中，应用卡尔曼滤波算法及多传感器数据融合技术，将加速度计和陀螺仪的数据进行融合，实时估算出实际机器人的倾角。实验结果表明，本文设计的控制系统能够实现双轮机器人的自平衡直立行走及速度控制。该控制系统同时继承了传统PID技术不依赖精确模型、实现简单、参数整定方便、鲁棒性强等优点。

2 双回路PID控制原理

两轮机器人由轮子和身体2部分组成，这2部分通过轮子的轴相连。图1为机器人身体的受力原理图。当身体受到外界微弱的干扰后，产生倾斜（即φ≠0）时，如果不对其进行控制，由于重力Mbg的作用，机器人将朝倾斜方向倒下。

图1 机器人身体的受力原理图Fig.1 Force analysis of the two-wheeled robot

如果需保持机器人直立，并使其以期望的速度平稳运行，系统既要实现机器人的平衡控制（即倾角控制），又要实现车轮的速度控制，这2个被控量之间互相耦合，不能看成2个完全不相干的变量，对其采用独立的PID控制。因此，本文研究了一种双回路PID的控制方法。它能同时控制好机器人的倾角和速度，使PID控制能够应用多变量、强耦合的被控制对象。同时该控制方法继承了传统PID控制不依赖精确模型、实现简单、参数整定方便、鲁棒性强等优点。

系统的双回路PID控制框图如图2所示，包括机器人平衡PD控制器与机器人速度PI控制器2个回路。平衡PD控制器用来实现机器人倾角控制，微分环节能很好地改善平衡控制系统的动态性能，提高其抗干扰性；速度PI控制器用来实现机器人速度调节，积分环节实现实际速度无静差跟随期望速度。

图2 机器人平衡和速度控制器Fig.2 The balance and speed controller for the robot

图2中，GB（s）是机器人倾角φ的传递函数，输入是施加在机器人车轮上的电压U（s），输出是机器人的倾角φ(s)；Gs(s)是机器人速度传递函数，输入是施加在机器人车轮上的电压U（s），输出是机器人的速度v（s）。DB(s)和Ds(s)分别是倾角 PD控制器和速度PI控制器：

2个控制器的输出叠加在一起得到控制量，作用于电机上，给轮子动力，从而控制机器人倾角和速度这2个被控变量。

本控制方法的最大特点是采用了正负反馈结合的控制方法。平衡控制回路为常规的负反馈（即Kpb＞0，Kdb＞0），而速度控制回路为正反馈（即Kps＜0，Kis＜0），正负反馈结合实现机器人以期望的速度平衡行走。

3 系统硬件设计

两轮机器人由机器人身体、2个轮子和相应的电机及驱动电路等组成。具体硬件设计框图如图3所示。由于没有直接的倾角传感器，系统无法直接测量机器人的倾角。本设计中倾角信号的采集是由运动传感器MPU6050来实现，它内部整合了3轴陀螺仪和3轴加速器。单片机接收到陀螺仪和加速度器的实时信号之后，通过数据融合得到机器人的倾角。速度信号的采集是通过编码器来实现的。控制处理器采用Arm内核单片机STM32F103芯片。利用传感器对被控变量（倾角和速度）进行测量，将测量数据送入Arm处理器中进行处理，输出相应的PWM信号，再通过功率放大加载到电机上，控制电机转动。为方便调试和观测数据，系统增加了液晶显示和蓝牙通信接口。

图3 系统硬件设计框图Fig.3 The hardware design of the system

4 系统软件设计

4.1 倾角计算方案

机器人倾角测量的准确性直接关系到机器人的平衡控制。如果用传感器将垂直于Z轴的一面贴在机器人上（见图4），那么Z轴方向的输出信号与机器人受到的重力分量成正比。由于MPU6050为16位数字传感器，可以通过下式得到机器人倾角θ：

图4 加速度计测量倾角原理图Fig.4 The principle of inclination measurement with accelerometer

加速度计在实际应用过程中存在静态输出噪声较大，加速度计输出易受其他方向的加速度影响等问题，因此，仅采用加速度计得到的倾角值不准确。陀螺仪是一种角速度计，对其积分可以得到机器人倾角。但陀螺仪的输出零点受温度影响，再经过积分后，误差会随着时间增长而累积［8］。

为了提高倾角测量的精度，本文采用卡尔曼滤波来实现加速度计和陀螺仪信号的数据融合，得到机器人的倾角。卡尔曼滤波器能将过去的测量估计误差合并到新的测量误差中，来估计被测量值将来的误差。

卡尔曼滤波器是建立在带白噪声的状态观测器模型上的，状态观测器模型方程如下。

离散状态方程为

式中：xk和xk-1分别为第k个周期和k-1周期的状态向量；A为作用在xk-1上的状态变换模型矩阵；uk为输入矩阵；B为输入变换模型矩阵；wk为过程激励噪声向量。

观测方程为

卡尔曼滤波算法（见图5）可分为两大块：一块为状态更新，作用是假设系统没有过程噪声，然后根据状态空间表达式对系统的状态变量进行估计并推算出误差协方差（可信度）；第2块为测量更新，其作用是根据推算出的误差协方差计算卡尔曼增益，利用卡尔曼增益把观测值和估计值融合，得出最优估计。其中x为状态变量，P为误差协方差，Q为系统过程误差协方差，R为观测误差协方差。

图5 卡尔曼滤波算法框图Fig.5 Kalman filter algorithm

该算法在计算机中实现的思想是：先选取合适的状态变量建立系统状态方程和观测方程，得到上述公式中x，A，B，u，H矩阵，然后将上述公式编成程序，并以固定的时间周期重复地执行上述程序。

具体实现过程如下：

1）选取合适的状态变量（待估计的变量必须放入其中），建立系统方程和观测方程。得到x，A，B，u，H阵；

2）根据系统激励噪声和观测噪声的特性，选取适当的Q和R；

3）初始化过程：初始化x（0），P（0），K（0）；

4）根据系统方程先验估计状态变量，即计算x（k|k-1）=Ax（k-1）+Bu（k）；

5）推导出当前的误差协方差，即计算P（k|k-1）=AP（k-1）AT+Q；

6）计算卡尔曼增益，即计算K（k）=P（k|k-1）HT[HP（k|k-1）HT+R]-1；

7）根据观测值修正误差，即计算x（k|k）=x（k|k-1）+K（k）[z（k）-Hx（k|k-1）]；

8）更新当前的误差协方差，即计算P（k|k）=[1-K（k）H]P（k|k-1）；

9）在下一个测量周期，从第4）部重新执行程序。经过卡尔曼滤波算法后，就可以得到机器人的实际倾角。

4.2 控制系统的软件设计

系统的控制流程图如图6所示，控制周期为10 ms。

图6 控制程序流程图Fig.6 The diagram for control system

首先进行信号采样处理，包括倾角及速度的采样。在每个控制周期，分别对陀螺仪和加速度计进行连续20次的采样，然后去掉最大值和最小值后取它们的平均值，以减小传感器测量过程中的随机误差。经过平滑滤波后，还需要进行标度变换，将传感器信号的A/D数值换算为以国际单位度量的物理量。

根据传感器实际检测到的速度和倾角值，分别根据倾角和速度的PID控制算法，计算出相应的PWM输出值，然后将2个PID控制器的输出叠加，控制施加在机器人轮子上的电压，使机器人的倾角和速度同时得到控制，使双轮机器人能够以一定的速度平衡直立行走。双回路PID程序流程如图7所示，分别对倾角误差采用PD负反馈控制算法和速度误差采用PI正反馈控制算法进行计算，然后将2个控制器的输出叠加，控制电机。

图7 PID控制子程序流程图Fig.7 Program flow diagram for PID controller

5 实验

本文采用自制的两轮自平衡小车进行实验，通过理论计算和实验整定，确定双回路PID控制器参数如下：Kpb=45，Kdb=3，Kps=-12，Kis=-10。

5.1 系统定点平衡控制实验

设定机器人的期望速度为0 m/s，期望角度为0 rad，系统实际控制结果如图8所示。系统的倾角和速度两个状态都是稳定的。其中倾角在-0.005～0.005 rad附近波动。速度在-0.05～0.05 m/s范围内波动。

图8 系统定点平衡实验中倾角和速度变化曲线Fig.8 The inclination and speed in the balance control experiment

5.2 速度阶跃响应实验

在这个实验过程中，先让系统处于定点平衡状态，在2.5 s时刻，改变期望的速度为0.5 m/s，测试其双轮机器人的速度跟随的动态性能。实际实验结果如图9所示，在2.5 s之前，系统处于定点平衡状态，机器人轮子首先反向加速，然后正向加速，机器人也相应地向前倾斜，然后又恢复过来，最终在4.8 s时刻速度基本稳定在期望值。相应的倾角在6 s时刻也恢复到初始状态。

图9 速度阶跃响应实验中倾角和速度变化曲线Fig.9 The inclination and speed in the experiment of speed step response

6 结论

本文采用卡尔曼滤波算法将实际检测的陀螺仪和加速度计信号进行数据融合，得到机器人实际倾角信号，并提出了一种双回路PID控制方法，实现两轮机器人的稳定行走控制。该控制方法具有以下特点：不需要精确的数学模型，程序实现简单，控制参数整定方便，抗干扰性较强。但是采用双回路PID控制时，系统超调量和稳定时间略长。如果采用专家PID或分段PID控制算法可实现更精细的控制，明显改善系统的动态时间。

［1］Nguyen H G，Morrell J，Mullens K，et al.Segway Robotic Mobility Platform［OL］.［2008-07-11］.http：//www.spawar.navy.mil/robots/pubs/spie5609-27.pdf.

［2］Oryschuk P，Salerno A，Al-Husseini A M，et al.Experimental Validation of an Underactuated Two-wheeled Mobile Robot［J］.IEEE/ASME Transactions on Mechatronics，2009，14（3）：252-257.

［3］Salerno A，Angeles J.A New Family of Two-wheeled Mobile Robots：Modeling and Controllability［J］.IEEE Transaction on Robotics，2007，23（1）：169-173.

［4］Deng M，Inoue A，Sekiguchi Ket al.Two-wheeled Mobile Robot Motion Control in Dynamic Environments［J］.Robotics and Computer-integrated Manufacturing，2010，26（3）：268-272.

［5］李潮全，高学山，李科杰.基于完整约束的两轮机器人建模及平衡控制［J］.控制工程，2012，19（3）：478-481.

［6］Xu Changkai，Li Ming.The System Design and LQR Control of a Two-wheels Self-balancing Mobile Robot［C］//International Conference on Electrical and Control Engineering，2011：2786-2789.

［7］Tsai C C，Huang H C，Lin S C.Adaptive Neural Network Control of a Self-balancing Two-wheeled Scooter［J］.IEEE Transactions on Industrial Electronics，2010，57（4）：1420-1428.

［8］王陆军，许勇.两轮自平衡电动车及其电机控制器设计［J］.电气传动，2011，41（11）：41-45.