一种轮式倒立摆机器人设计*

汪智祥， 刘益剑， 曹　楠

(南京师范大学 电气与自动化工程学院，江苏 南京 210042)

一种轮式倒立摆机器人设计*

汪智祥， 刘益剑， 曹楠

(南京师范大学 电气与自动化工程学院，江苏 南京 210042)

摘要:倒立摆是检验各种控制理论的理想模型，通过它可以判断各类控制算法的有效性和控制性能。为此，结合K60微控制器强大的运算能力和丰富的接口功能，设计了该轮式倒立摆机器人，可以通过它验证各种新型控制算法。硬件上介绍了与姿态控制相关的姿态检测系统和电机驱动单元。软件上介绍了根据系统数学模型设计的互补滤波器、姿态控制器、方向控制器和速度控制器。实验结果表明：该系统具有良好的鲁棒性和稳定性，可以准确地对控制算法的有效性和控制性能进行判断。

关键词:轮式倒立摆； 互补滤波； 姿态估计； 控制器

0引言

2002年,瑞士联邦工业大学的GrasserF等人研制出基于DSP的轮式倒立摆机器人Joe，通过解耦运动分解并简化控制实现了较好的机动性，一时达到了前所未有的水平[1]，并引发了热潮，带动了诸多学者对轮式倒立摆机器人研究的关注，例如可以自主规划避障行走的Nbot[2];新型载人两轮交通工具Segway[3];采用模块化结构并应用差分电机驱动方式的Legway，可以通过调节自身平衡点实现爬坡等。

我国关于轮式倒立摆机器人的研究起步较晚，其主要局限在数学模型的建立、系统稳定性的论证等。近年来,哈尔滨工业大学[4]、北京工业大学等大学也在这方面进行了相关理论研究和机器人平台设计，并取得了一定的成果。因此，基于轮式倒立摆机器人展开的研究，无论是在理论研究还是工程应用都是具有很大价值的。

本文基于高性能的K60微控制器，设计了一种轮式倒立摆机器人。

轮式倒立摆机器人是本征不稳定的控制系统，具有多变量、高阶次、非线性和参数不确定的特点，它是验证各种控制算法效果的理想实验平台。系统稳定特点在一定程度与空间飞行器有很大的相似性，在控制问题上具有一定的普遍性和典型性。通过对该轮式倒立摆机器人的控制，能够有效检测控制方法是否具有处理非线性和不稳定等问题的能力。与此同时，该系统可以对姿态、速度、方向进行控制，并可以作为一种姿态估计和机器人控制算法的实验平台，降低算法验证的成本与复杂度。

1硬件设计

本文设计的轮式倒立摆机器人如图1所示。

硬件主要由微控制器、电机驱动、图像采集、速度检测、姿态检测等模块组成，图2为系统框图。

图1　轮式倒立摆机器人Fig 1　Wheeled inverted pendulum robot

图2　硬件系统框图Fig 2　Block diagram of hardware system

本文选用了Freescale公司Kinetis系列的32位微控制器K60。它基于ARMCortex—M4内核，具有高达32位的DMA，并能够提供多种CPU频率；它的运算能力强、处理速度快，并拥有丰富的片内资源，包括多个高精度16位ADC、12位DAC,6位模拟比较器、I/O口等；同时还拥有丰富的外部接口，包括SPI，IIS，IIC，CAN，SDHC，UART等，可以很好地实现机器人的控制任务。

1.1姿态检测模块

姿态检测模块由陀螺仪、加速度计及其放大电路组成，采用惯性测量方法进行姿态估计。设计选用IDG—650式陀螺仪，可以检测运动时x轴和y轴角速度；选用MMA7361式三轴MEMS加速度计，可以同时输出x，y，z三个方向上加速度信号； 姿态检测模块固定在机器人车身的垂直方向上，并假设它们之间是刚性连接。

1.2电机驱动电路

机器人采用了RS380型直流电机驱动同轴的两个车轮。为了使电机充分发挥性能，提高灵敏性，设计了双H桥电机驱动电路(如图3所示)，为电机提供稳定的工作电压。使用NTMFS4833N型N沟道MOS管设计的驱动电路具有导通电阻小，导通电流可达191A，电机噪声小，发热少等特点[5]，并且极大地简化了硬件电路。

图3　单个H桥驱动电路Fig 3　Single H-bridge driver circuit

2控制结构

控制部分由三个闭环控制回路：姿态环、速度环和方向环构成(如图4所示)，其中，姿态环的优先级最高。

图4　系统控制框图Fig 4　Block diagram of system control

2.1姿态检测单元

运动物体的姿态通常采用惯性坐标系中的横滚角(φ)、俯仰角(θ)和航向角(ψ)来描述[7]。当机器人车身竖直时，以车轴中点为原点建立惯性坐标系，使车身的姿态通过欧拉角来描述。其中，车身方向为俯仰轴(z)，车轴方向为航向轴(x)，垂直于这两个方向的为横滚轴(y)。

姿态环由姿态检测系统和姿态控制器构成，影响俯仰轴方向的稳定性。精确获取当前的姿态，才能进行有效的姿态控制，实现机器人的直立平衡。机器人俯仰轴的运动是通过俯仰角θ描述，对应的角速度为ω，因此，只需要检测俯仰角θ。

忽略其余加速度的影响，假设重力是唯一的加速度来源，可直接利用加速度计的输出来估计俯仰角θ，计算公式为[7]

(1)

式中θacc为加速度计输出得到的俯仰角，ax,ay,az为加速度计在体坐标系中测得加速度的分量。

由于运动过程中摆杆摆动造成很强的干扰，使加速度计输出信号产生高频误差，无法获得准确的姿态。而陀螺仪存在累计误差和温漂现象，静态特性差，也不能单独使用陀螺仪的输出[8]。

因此，将陀螺仪和加速度计的输出进行数据融合，得到估计的俯仰角θ。

通常数据融合采用卡尔曼滤波或互补滤波。本文结合陀螺仪和加速度计的频响特性采用互补滤波进行数据融合。由于陀螺仪的高频特性较好，对陀螺仪的输出进行高通滤波；而加速度计的低频特性较好，对加速度计的输出进行低通滤波，将滤波后的数据进行融合，获得精确的俯仰角θ。互补滤波的原理如图5。

图5　互补滤波原理Fig 5　Principle of complementary filtering

预估得到的姿态表达式为[7]

(2)

将式(2)离散化后得到预估的俯仰角为

(3)

式中θn为第n次滤波后角度，θn-1为第n-1次滤波后角度，a为高通滤波系数，T为采样时间，b为低通滤波系数，az为加速度计z轴上的分量。

2.2姿态控制器

以估计的俯仰角θ作为姿态控制器的反馈输入，进行姿态控制，可以使机器人达到直立平衡的状态。本文采用比例微分(PD)控制器进行姿态控制。姿态控制器的输出为

(4)

2.3速度控制器

在保证了稳定平衡的情况下，可以对机器人进行速度控制和转向控制等。采用数字比例积分微分(PID)控制器实现速度的闭环控制。以旋转编码器反馈的速度作为输入，则速度控制器的输出为

(5)

式中ev为输出占空比与旋转编码器测得的占空比之差，亦即速度偏差，Tv为速度控制周期，Kvp为比例系数，Tvd为微分系数，Tvi为积分系数。

2.4转向控制器

为了使机器人实现可以沿着测试道路的中线自动循迹，设计了由摄像头和转向控制器构成的方向环。通过摄像头采集图像，微控制器对采集的图像处理、提取道路信息并计算机器人中心偏离道路中线偏差。根据偏差控制车轮的速度差，实现机器人的转向。PD控制器能很好地实现转向，它的输出为

(6)

式中ep为方向偏差，Tp为速度控制周期，Kpp为比例系数，Tpd为微分系数。

2.5电机控制

驱动电路提供两路10kHz的脉宽调制(PWM)信号来控制电机，使电机转动。两路PWM信号的占空比是姿态、速度和转向控制器输出的线性叠加。左右电机驱动获得的占空比分别为

(7)

式中Uleft，Uright分别为左、右驱动获得的占空比，Ua为姿态控制的输出，Uv为速度控制的输出，Up为转向控制的输出。

3测试

最终对一些控制算法进行了测试，得出机器人运行轨迹如图6所示，运行时俯仰角如图7所示。从图中可以看出：机器人大致是沿着道路的中线行驶的，行驶轨迹比较好，俯仰角的变化也比较小，说明控制算法是有效的，并且性能很好。

图6　机器人运行轨迹图Fig 6　Trajectory figure of robot running

4结论

实验结果表明:该控制系统具有良好的鲁棒性和稳定性，可以准确地对控制算法的有效性和控制性能进行判断。因此，可以将轮式倒立摆机器人作为实验平台，以其姿态、速度、位置作为被控对象，来验证各类控制算法，并判定优劣。

图7　机器人姿态俯仰角曲线Fig 7　Curve of pitch angle of robot attitude

参考文献:

[1]GrasserF,ArrigoAD,ColombiS,etal.JOE:Amobile,inverted,pendulum[J].IEEETransonIndustrialElectronic,2002,49(1):107-114.

[2]SalernoA,AngelesJ.Anewfamilyoftwo-wheeledmobilerobots:Modelingandcontrollability[J].IEEETransactionsonRobotics,2007,23(1):169-173.

[3]ArlingAW,ChangST,FieldJD,etal.Personaltransporter：US,D528468S[P].2006—09—19.

[4]王晓宇.两轮自平衡机器人的研究[D].哈尔滨：哈尔滨工业大学,2007.

[5]吴苗苗,沈世斌,王亮,等.基于CMOS摄像头的直立循迹智能车系统设计[J].自动化技术与应用,2014,33(7):21-25.

[6]兰建军,李佳璐，刘颖，等.基于自适应互补滤波算法的轮式倒立摆控制[J].组合机床与自动化加工技术，2014(6):66-68.

[7]LaiYC,JanSS,HsiaoFB.Developmentofalow-costattitudeandheadingreferencesystemusingathree-axisrotatingplat-form[J].Sensors,2010(10):2472-2491.

[8]杨天雨,贾文峰,赖际舟,等.惯性/光流/磁组合导航技术在四旋翼飞行器中的应用[J].传感器与微系统,2016,35(1):156-160.

Designofawheeledinvertedpendulumrobot*

WANGZhi-xiang，LIUYi-jian，CAONan

(SchoolofElectricalEngineeringandAutomation,NanjingNormalUniversity,Nanjing210042,China)

Abstract:Inverted pendulum is an ideal model for testing different control strategies,through which effectiveness and control performance of various control algorithms can be judged.A control system for wheeled inverted pendulum is designed by using powerful operation function and interface of K60 micro control unit(MCU) based on ARM Cortex-M4,and it is used to verify various new type control algorithm.For hardware,attitude detection system and motor driver unit which related to attitude control are introduced.For software,complementary filter,attitude controller,direction controller and velocity controller are introduced according to math model for system.Experimental results show that the control system has good robustness and stability,and it can exactly judge validity and control performance of control algorithm.

Key words:wheeled inverted pendulum; complementary filtering; attitude estimation; controller

DOI:10.13873/J.1000—9787(2016)04—0108—03

收稿日期：2016—03—10

*基金项目：江苏省普通高校研究生科研创新计划项目(1812000002A573)

中图分类号:TP 242.6

文献标识码:A

文章编号:1000—9787(2016)04—0108—03

作者简介:

汪智祥(1994-)，男，江苏泰兴人，主要研究方向为机器人开发与设计。

刘益剑，通讯作者，E—mail:63055@njnu.edu.cn。