[发明专利]一种双轮机器人的自由舞步生成方法

权利要求书

1.一种双轮机器人的自由舞步生成方法，其特征在于：

（1）机器人初始化；

（2）生成自由舞步指令，检测状态；

（3）机器人根据生成的自由舞步指令，开始自由舞蹈。

2.根据权利要求1所述的一种双轮机器人自由舞步生成方法，其特征在于：自由舞步指令由以下步骤生成：

（1）确定模型参数，本发明涉及的机器人舞步行为状态机为一个五元组

M={Q,T,δ,q₀,D}；

其中，

Q为机器人舞步行为状态集合，包括静止、前进、后退、转向这几种基本的状态；

Q={S,F,B,L,R}

其中，

S表示机器人静止；

F表示机器人前进；

B表示机器人后退；

L表示机器人左转；

R表示机器人右转；

以上所述前进和后退方向以当前机器人的朝向为参考方向，旋转方向均表示俯视视角中机器人的旋转方向；

T表示机器人舞步行为指令集合，同时作为舞步行为状态集合Q的子状态集合，包括前进，后退，转弯3种基本的行为，配合不同的速度和角度，规定机器人左右轮正反转方向描述如下：当机器人左右轮转速一致，使机体前进的左右轮转向为正方向，反之为反方向。

T={f₁,f₂,b₁,b₂,l_1-3,l_4-6,r_1-3,r_4-6}

其中，

f₁表示机器人左右轮均慢速正转，机体以慢速前进；

f₂表示机器人左右轮均快速正转，机体以快速前进；

b₁表示机器人左右轮均慢速反转，机体以慢速后退；

b₂表示机器人左右轮均快速反转，机体以快速后退；

l₁表示机器人左轮静止，右轮以慢速正转，机体以左轮为圆心慢速左转45°；

l₂表示机器人左轮以慢速反转，右轮静止，机体以右轮为圆心慢速左转45°；

l₃表示机器人左轮以慢速反转，右轮以慢速正转，机体呈慢速逆时针自旋转90°；

l₄表示机器人左轮静止，右轮以快速正转，机体以左轮位圆心快速左转90°；

l₅表示机器人左轮以快速反转，右轮静止，机体以右轮为圆心快速左转90°；

l₆表示机器人左轮以快速反转，右轮以快速正转，机体呈快速逆时针自旋转180°；

r₁表示机器人左轮以慢速正转，右轮静止，机体以右轮为圆心慢速右转45°；

r₂表示机器人左轮静止，右轮以慢速反转，机体以左轮为圆心慢速右转45°；

r₃表示机器人左轮以慢速正转，右轮以慢速反转，机体呈慢速顺时针自旋转90°；

r₄表示机器人左轮以快速正转，右轮静止，机体以右轮为圆心快速右转90°；

r₅表示机器人左轮静止，右轮以快速反转，机体以左轮为圆心快速右转90°；

r₆表示机器人左轮以快速正转，右轮以快速反转，机体呈快速顺时针自旋转180°；

假设机器人当前行为状态持续时间为1s，旋转角度由姿态传感器配合PID算法控制；

δ为映射关系集合，即机器人行为状态转移函数的集合；

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

δ(Q,f_i)=F；(i＝1，2)

δ(Q,b_i)=B；

δ(Q,l_j)=L；(j=1，2，3，4，5，6)

δ(Q,r_j)=R；

上述映射关系中：等式左边括号里的两个参数分别表示转移源状态，其为Q中的元素，转移要素或输入信号，其为T中的元素：等式右边表示转移终状态，其为Q中的元素；

q₀为机器人的初始状态。令机器人的初始状态为静止状态S，即q₀＝S∈Q；

D为终端状态集合，终端状态是Q中的元素，即D∈Q；

（2）运用状态流程控制和事件驱动方法对舞步行为进行建模；将机器人舞步的每个行为状态，即Q中的每个元素作为一个行为状态模块，T中的元素作为舞步行为状态Q的子行为状态模块；

（3）人类华尔兹是三步舞,即三个基本动作组合形成一个舞步，其中每个基本动作对应一个节拍；将双轮机器人的一个行为状态对应人类华尔兹的一个节拍，根据上述搭建的Stateflow舞步行为模型，将双轮机器人三个舞步行为状态组合构成一个行为状态组，即确定了行为状态之间的互锁关系，并把这些组合编号，如下所示：

1.左前进舞步

①δ(S,l₁)=L，②δ(L,f₁)=F，③δ(F,r₁)=R；

2.右前进舞步

①δ(S,r₁)=R，②δ(R,f₁)=F，③δ(F,l₁)=L；

3.左后退舞步

①δ(S,l₂)=L，②δ(L,b₁)=B，③δ(B,r₂)=R；

4.右后退舞步

①δ(S,r₂)=R，②δ(R,b₁)=B，③δ(B,l₂)=L；

5.踌躇左前进舞步

①δ(S,f₁)=F，②δ(F,l₁)=L，③δ(R,l₂)=L；

6.踌躇右前进舞步

①δ(S,f₁)=F，②δ(F,r₁)=R，③δ(R,r₂)=R；

7.踌躇左后退舞步

①δ(S,b₁)=B，②δ(B,l₂)=L，③δ(R,l₁)=L；

8.踌躇右后退舞步

①δ(S,b₁)=B，②δ(B,r₂)=R，③δ(R,r₁)=R；

9.前进左旋转180舞步

①δ(S,f₁)=F，②δ(B,l₃)=L，③δ(R,l₃)=L；

10.前进右旋转180舞步

①δ(S,f₁)=F，②δ(B,r₃)=R，③δ(R,r₃)=R；

11.后退左旋转180舞步

①δ(S,b₁)=B，②δ(B,l₃)=L，③δ(R,l₃)=L；

12.后退右旋转180舞步

①δ(S,b₁)=B，②δ(B,r₃)=R，③δ(R,r₃)=R；

13.前进交叉180舞步

①δ(S,f₁)=F，②δ(B,l₆)=L，③δ(L,r₆)=R；

14.后退交叉180舞步

①δ(S,b₁)=B，②δ(B,r₆)=R，③δ(R,l₆)=L；

15.原地左自旋转舞步

①δ(S,l₃)=L，②δ(L,l₃)=L，③δ(L,l₃)=L；

16.原地右自旋转舞步

①δ(S,r₃)=R，②δ(R,r₃)=R，③δ(R,r₃)=R；

(4)随机产生一组1～16的数字，即生成了一串行为状态组的序列号，所述的序列号对应(3)中同一数字所表示的舞步，作为状态流程模型的驱动事件，当前序列号对应的三个行为状态被依次激活，将当前激活的行为状态翻译成双轮机器人动作指令，用控制算法计算出电机控制量，进而生成了一套双轮机器人的华尔兹舞步。

其中，计算电机控制量的具体方法如下：

设机器人电机的控制量由两部分叠加：平衡控制量和行进控制量。其中，平衡控制量由姿态传感器检测到机器人的倾角值以及倾角速度配合非线性PID算法计算得出；而行进控制量由机器人左右轮上编码器测得的位移、速度和加速度配合PID算法得出。左右轮电机的控制量分别为：

U_l=n*(U_b+U_a)（1）

U_r=n*(U_b+U_a)（2）

其中，U_l表示左轮电机控制量，U_r表示右轮电机控制量，U_b表示平衡控制量，U_a表示行进控制量，n取值1或2，表示速度系数，当n=1时，表示电机慢速转动，当n=2时表示电机快速转动；

机器人两轮的电机控制量分别由上述平衡控制量和行进控制量配合速度系数叠加而成。电机上连接有2路信号，一路为电机控制量的大小，由上述式（1）和式（2）计算得出；另一路为电机转动方向，即控制电机正转或者反转，当该路信号为高电平，电机正转，反之电机反转。