两轮机器人的舞步运动规划和仿真研究

孙 亮，呼德夫

SUN Liang,HU De-fu

（北京工业大学 人工智能与机器人研究所，北京 100124）

0 引言

两轮自平衡机器人属于轮式机器人的范畴，这种机器人两轮共轴，独立驱动，作为差动驱动式，通过运动控制保持平衡，可直立行走。它结构简单、运动灵活，适于在狭小和危险的空间内工作，具有较好的机动性。两轮自平衡机器人属于非线性、时变、欠驱动、非完整约束系统 。目前，两轮机器人的自平衡控制方面的研究较多，常用DSP或单片机作为主控制器，陀螺仪和加速度计采集机器人姿态和倾角数据，同时电机编码器采集车轮转速数据，将数据传到主控制器，通过卡尔曼滤波数据融合和PID算法来进行机器人车轮转速控制，从而实现自平衡控制效果。两轮机器人在实现自平衡的前提下可以完成多种动作。若机器人能够响应舞曲优美的韵律，完成诸如华尔兹这样的特定舞步动作，则会最大程度地体现出两轮机器人动态性能好、机动性强的特点。本文基于两轮机器人的基本运动方式，提出机器人完成简单舞步动作的运动规划，给出了基于有限状态自动机的运动规划描述方法，并用基于广义雅可比矩阵的两轮机器人分解运动速度控制算法 进行了仿真实验。实验结果表明算法具有良好的准确性与可靠性。

1 运动学模型

两轮移动机器人的运动学模型如图1所示。其中主要有：C为机器人车身前端一点；O为机器人平台质心；b为两车轮间距；r为车轮半径；lc为点C与质心O点之间距离；φ为机器人车身倾角；θl和θr分别为左右轮转角。

图1 两轮移动机器人几何模型

J为两轮机器人的广义雅克比矩阵，反映机器人上点C速度与车轮转速之间的关系[2]。

2 基于有限状态自动机的运动规划描述

为了规划两轮机器人的运动轨迹，使其按照华尔兹舞步轨迹运动，可以用有限状态自动机来描述其状态变化情况。有限状态自动机（Finite State Automata,FSA）定义为一个五元变量组[3]，即：

其中，T为对应于基元的符号集合，Q为机器状态集合，δ为映射关系集合，q0为机器的初始状态，q0∈Q， F为终端状态集合，F∈Q。

两轮机器人的符号集合为：

其中，f表示机器人前进（forward），b表示机器人后退（backward），r表示右轮以左轮为轴转动，即转动轮为右轮，同理l表示左轮以右轮为轴转动，即转动轮为左轮，s表示机器人无半径自旋转，c表示在俯视视角中机器人顺时针（clockwise）转动，即向右转，a表示在俯视视角中机器人逆时针（anti-clockwise）转动，即向左转，45等数字表示其相应转动的角度，所以ra45表示机器人右轮以左轮为轴逆时针转动45°，即机器人左转45°。其他的符号以此类推。

机器人状态集合，即机器人车身朝向状态集合为Q={F,B,L,R,FL,FR,BL,BR}

其中，F表示向前方向（Forword），B表示向后方向（Backward），L表示向左方向（Left），R表示向右方向（Right），FL表示左前方向，FR表示右前方向，BL表示左后方向，BR表示右后方向，如图2所示。

图2 两轮机器人舞步运动方向

在映射关系δ=(A,a)=B中：A表示转移源状态，为状态Q的元素；a表示转移要素或输入信号，即符号集合T中的元素：B表示转移终状态，也为状态Q的元素。

机器人的所有状态遵循以下映射关系：

上式(5)中，qi表示第i时刻机器人的朝向状态，当它收到raa符号指令（即机器人右轮以左轮为轴逆时针转动α0）后逆时针转α0，状态变为qi+1，表示为qi+α。其他的映射关系以此类推，如式(7)-(9)所示。

图3 人的连续左转舞步示意图

下面以较为典型的华尔兹舞步——连续左转为例，给出相应的有限状态自动机描述方式。如图3所示，人的连续左转90°舞步由前进左转90度和后退左转90度组成，共可以分解为六个步伐。以男士步伐为例，在图3中以向上方向为向前：

1）迈左脚前进；

2）再迈右脚，同时身体向左侧身，开始左转；

3）收回左脚，两腿并拢，身体朝左，完成前进左转90°；

4）退右脚后退；

5）再退左脚，同时身体向左侧身，开始左转；

6）收回右脚，两腿并拢，身体朝后，完成后退左转90°。

同理，如图4所示，可以得到机器人的模拟舞步运动示意图，分别模拟得到上述6个步伐：

1）左右轮匀速前进；

2）右轮绕左轮左转（前进）45°；

3）左轮绕右轮左转（后退）45°，完成前进左转90°；

4）左右轮匀速后退；

5）左轮绕右轮左转（后退）45°；

6）右轮绕左轮左转（前进）45°，完成后退左转90°。

图4 两轮机器人模拟的连续左转舞步示意图

根据以上六个步伐分解，应用基于有限状态自动机的舞步规划方法得到连续左转90度舞步的相应集合为：

图5 机器人连续左转舞步的状态转移图

4）绘制状态转移图。根据步骤3）中的映射关系，得到机器人舞步的状态转移图，如图5所示。

以此类推，可以得到机器人模仿华尔兹舞步的所有映射关系及其基于FSA的表达方式。

3 分解运动速度控制及仿真实验

由式(4)可知，由机器人上一点C的速度也可以求出车轮转速。此时车轮控制量如下：

采用计算机控制，该方程变为离散式。速度项可以表示为单位时间里（控制周期Δt）的位移增量， 即：

图6 两轮机器人的期望运动轨迹

图7 两轮机器人运动轨迹仿真结果

式中：Δp为一个控制周期内的位移变化；ΔΦ为同一时间间隔内的角度变化。如果给定C点的轨迹，当Δt选定时，可以确定Δp，将其代人上式求出各个角度增量ΔΦ，可得到各个角度的给定值，最后由角度伺服控制系统去实现位置控制。差动驱动式移动机器人的分解运动速度控制是一种基于逆广义雅可比矩阵的连续路径跟踪控制方法。该算法要考虑合理的控制周期，通过控制每个周期内的角度输出量实现对轨迹的跟踪或速度控制。

为了验证本文提出的两轮移动机器人运动规划算法的正确性和可行性，在MATLAB平台上做华尔兹基本舞步-左转90°的模拟仿真实验。为了能使机器人按照指定的轨迹运动并移到指定的位置，可以让机器人跟踪某种特定的轨迹[4]。机器人的参数取：b=1.5,lc=0.5,r= 1.3。

初始位置为(xc0,yc0,φ0)=(20,0,90º)， 终止位置(xc1,yc1,φ1)=(0,43,180º)，为了实现机器人车身左转，使C点跟踪近似轨迹为y=40。图6给出了机器人平台上点C的期望运动轨迹，图7给出了两轮机器人运动过程仿真结果。从仿真结果可以看出，控制算法使两轮机器人驱动两车轮转动可以实现机器人车身上一点跟踪预定轨迹，完成了基本舞步左转90°的位置与方向角的相应改变。

4 结束语

本文用有限状态自动机描述了两轮移动机器人的舞步运动规划。有限状态自动机是具有有限状态的离散输入输出的系统数学模型。华尔兹基本舞步动作是由若干个基本动作元素组合而成的，且这些元素都是较为固定的运动模式。所以可以用有限状态自动机规范地描述两轮机器人舞步的运动规划。同时本文运用基于广义雅克比矩阵的分解运动速度控制算法来实现机器人对轨迹的跟踪和速度控制，并用MATLAB验证了该算法的准确性。

[1] 王建俊.两轮同轴机器人的自平衡控制研究[J].数字技术与应用,2010,7：105-106.

[2] 柳长安,李国栋,刘春阳.差动驱动式移动机器人的运动规划[J].哈尔滨工业大学学报,2003,35(9)：1095-1097.

[3] 孙亮,禹晶.模式识别原理[M].北京：北京工业大学出版社,2009,2：54-56.

[4] Yoshio Yamamoto,Xiaoping Yun.Coordinating Locomotion and Manipulation of a Mobile Manipulator. IEEE Transactions on Automatic Control,1994,39(6)：1326-1332.

[5] SERJI H.A unified approach to motion control of mobile manipulators[J].The International Journal of Robotics research,1998,17(2)：10-15.