专利名称：一种双轮机器人的自由舞步生成方法
技术领域：
本发明涉及一种双轮机器人的舞步规划方法，属于智能机器人领域。它利用状态流程控制和事件驱动实现双轮机器人的自由舞步生成。
背景技术：
双轮自平衡机器人属于轮式移动机器人，源于倒立摆。其两轮共轴，独立驱动，通过相关控制策略实现其动态平衡。双轮自平衡机器人运动灵活、结构简单，两轮差动输入，具有零回转半径和U型回转等优点，并且适合在狭小和危险的空间内工作，应用前景广泛。机器人运动规划问题是机器人基本的研究内容。舞步规划属于轨迹规划的范畴，是在无碰撞或无障碍的前提下，规划机器人按照一定的轨迹运动，以模拟人的舞歩行为。目前，在机器人舞步规划技术中，普遍采用的是动作示教法(动作一数据)，即先直观的将机器人摆置成预想的舞步动作，再将此动作对应的电机各參数记录下来，按照时间排列形成动作数据文件。这种方法局限性在干，必须在舞蹈机器人机械结构和控制程序都完成后才能进行，需要配套的硬件平台和相关的软件，实施成本较高，开发周期长。本发明提出了一种双轮机器人舞步行为模型，并基于该模型设计了ー种自由舞步生成方法，基于该方法，机器人能够跟随节拍自由舞蹈。模块化的设计简洁清晰，设计与调试时间大幅度減少，成本降低，动作的可控性增强。
发明内容
本发明提出一种双轮机器人舞步行为模型，并基于该模型设计了ー种的自由舞步生成方法，双轮机器人由两个车轮、直流电动机、DSP主控制器、姿态传感器、编码器、电机驱动和电源组成。为了减轻机器人重量，且使机械结构更加紧凑，机器人的底盘采用塑料水晶板进行切割加工而成，电池固定于底盘上部，两个车轮同轴并用铝制角铁与直流电动机分别固定于底盘两侧。机器人等角轴侧结构图和仰视图分别如图I和图2所示。机器人机身安装有DSP主控器、姿态传感器、电机驱动板、电源管理板、编码器等，各配件均用铜柱固定于机器人躯干部分。机器人的电气连接图如附图3所示。本发明涉及的机器人电气结构分为4个模块主控模块，传感器模块，执行模块和电源模块。主控模块选择TMS320F28335芯片，该芯片提供了 88个多功能引脚，它们被分成3组，即3部分32位的I/O ロ，用户可以将这些引脚作为片内外设的输入/输出引脚，也可将它们用作数字I/O ロ。传感器模块选用5自由度惯性导航传感器MU5，5个自由度分别与主控器5个ADCIN ロ连接。执行模块选用金属齿轮大扭矩减速电机，额定电压6v，功率3w，扭カ6kg*cm，定转速150转/分，量85克，电机后端输出轴长为8毫米，方便测速。采用2个数字I/O ロ控制电机的转向，将其设置成通用输 出口，通过改变端ロ的输出状态I或0，实现电机的正反转。同时需要两路PWM信号分别控制左右两个电机，因此选用2路ePWM2功能模块执行这一功能。电源模块采用9V直流电池与电源管理器(9V转3. 3V，5V)相连，给整个系统供电。
本发明涉及的机器人舞歩行为状态机为ー个五元组M= {Q, Τ, δ，q0, D}。其中，(I)Q为机器人行为状态集合，包括静止、前进、后退、转向这几种基本的状态；Q = {S，F，B，L，R}其中，S表示机器人静止；F表示机器人前进；B表不机器人后退；L表示机器人左转；R表示机器人右转。以上所述旋转方向均表示俯视视角中机器人的旋转方向。(2) T表示机器人舞步动作指令集合同时作为舞歩行为状态集合Q的子状态集合，包括前进，后退，转弯3种基本的行为，配合不同的速度和角度，规定机器人左右轮正反转方向描述如下当机器人左右轮转速一致，使机体前进的左右轮转向为正方向，反之为反方向。T= {f1； f2, b1； b2, I卜3, 14_6, iv3, r4_6}其中，表示机器人左右轮均慢速正转，机体以慢速前进；f2表示机器人左右轮均快速正转，机体以快速前进；Id1表示机器人左右轮均慢速反转，机体以慢速后退；b2表示机器人左右轮均快速反转，机体以快速后退；I1表示机器人左轮静止，右轮以慢速正转，机体以左轮为圆心慢速左转45° ;I2表示机器人左轮以慢速反转，右轮静止，机体以右轮为圆心慢速左转45° ;I3表示机器人左轮以慢速反转,右轮以慢速正转,机体呈慢速逆时针自旋转90° ;I4表示机器人左轮静止，右轮以快速正转，机体以左轮位圆心快速左转90° ;I5表示机器人左轮以快速反转，右轮静止，机体以右轮为圆心快速左转90° ;I6表示机器人左轮以快速反转，右轮以快速正转，机体呈快速逆时针自旋转180。；F1表示机器人左轮以慢速正转，右轮静止，机体以右轮为圆心慢速右转45° ;r2表示机器人左轮静止，右轮以慢速反转，机体以左轮为圆心慢速右转45° ;r3表示机器人左轮以慢速正转,右轮以慢速反转,机体呈慢速顺时针自旋转90° ;r4表示机器人左轮以快速正转，右轮静止，机体以右轮为圆心快速右转90° ;r5表示机器人左轮静止，右轮以快速反转，机体以左轮为圆心快速右转90° ;r6表示机器人左轮以快速正转，右轮以快速反转，机体呈快速顺时针自旋转180。；
、
假设机器人当前行为状态持续时间为ls，旋转角度由姿态传感器配合PID算法控制。机器人舞步行为状态示意图如图4所示。(3) δ为映射关系集合，即机器人行为状态转移函数的集合；
机器人的所有行为状态遵循以下映射关系 δ (Q, fj) = F ； (i = I, 2)δ (Q, bj) =B;δ (Q, Ij) = L ； (j = 1,2,3,4,5,6)δ (Q, rj) = R ；上述映射关系中等式左边括号里的两个參数分别表示转移源状态(Q中的元素)，转移要素或输入信号(T中的元素)等式右边表示转移终状态(Q中的元素)。(4) q0为机器人的初始状态。初始状态为静止状态S，即qQ = S e Q ;(5) D为终端状态集合，终端状态是Q中的元素，即D e Q。根据上述五元组状态机，本发明设计了ー种双轮机器人的自由舞步生成方法。步骤如下(I)运用Stateflow状态流程控制和事件驱动方法对舞步行为进行建摸。将机器人舞步的姆个行为状态，即Q中的姆个元素作为ー个行为状态模块，T中的元素作为舞步行为状态Q的子行为状态模块；(2)人类华尔兹是三步舞，即三个基本动作组合形成一个舞步，其中每个基本动作对应ー个节柏。将双轮机器人的一个行为状态对应人类华尔兹的ー个节拍，根据上述搭建的Stateflow舞步行为模型，将双轮机器人三个舞步行为状态组合构成一个行为状态组，即确定了行为状态之间的互锁关系，并把这些组合编号，如下所示I.左前进舞步①δ (S, I1) = L,② δ (L,も)=F,③ δ (F, =R;2.右前进舞步①δ (S, r) = R,② δ (R,も)=F,③ δ (F, I1) =L;3.左后退舞步①δ (S, I2) = L,② δ (L, Id1) = B,③ δ (B, r2) =R;4.右后退舞步①δ (S, r2) = R,② δ (R, Id1) = B,③ δ (B, I2) =L; 5.鋳躇左前进舞步①δ (S, f) = F,② δ (F, I1) = L,③ δ (R, I2) =L;6.鋳躇右前进舞步①δ (S, f) = F,② δ (F, = R,③ δ (R, r2) =R;7.鋳躇左后退舞步①δ (S, b) = B,② δ (B, I2) = L,③ δ (R, I1) =L;8.鋳躇右后退舞步①δ (S, b) = B,② δ (B, r2) = R,③ δ (R, =R;9.前进左旋转180舞步①δ (S, f) = F,② δ (B, I3) = L,③ δ (R, I3) =L;10.前进右旋转180舞步①δ (S, = F,② δ (B, r3) = R,③ δ (R, r3) =R;11.后退左旋转180舞步
①δ (S, b) = B,② δ (B, I3) = L,③ δ (R, I3) =L;12.后退右旋转180舞步①δ (S, Id1) = B,② δ (B, r3) = R,③ δ (R, r3) =R;13.前进交叉180舞步①δ (S, f) = F,② δ (B, I6) = L,③ δ (L, r6) =R;14.后退交叉180舞步①δ (S, b) = B,② δ (B, r6) = R,③ δ (R, I6) =L;15.原地左自旋转舞步 ①δ (S, I3) = L,② δ (L, I3) = L,③ δ (L, I3) =L;16.原地右自旋转舞步①δ (S, r3) = R,② δ (R, r3) = R,③ δ (R, r3) =R;(3)随机产生ー组I 16的数字，即生成了一串行为状态组的序列号，作为上述状态流程模型的驱动事件，当前序列号对应的三个行为状态被依次激活，将当前激活的行为状态翻译成双轮机器人动作指令，用控制算法计算出电机控制量，进而生成了一套双轮机器人的华尔兹舞步。本发明的重要特征在于运用Stateflow状态流程控制和事件驱动方法对舞步行为进行建摸。该模型描述了机器人模拟人类华尔兹舞步的所有行为状态，机器人通过自主生成一组控制指令和机器人当前激活的行为状态模块之间的映射关系来完成自由舞步生成，使机器人能够伴随音乐节柏，自由舞蹈，具有很好的观赏性。本发明的具体方法步骤如下(I)初始化配置DSP主控制器寄存器的状态，包括IO端ロ、Α/D通道设置，初始化电机控制量记录姿态传感器零点模拟电压值、初始化电机控制量；设置初始时间b = 0，机器人初始状态为静止状态，即qd = S ;给定停止时间tf ；(2)开中断等待读取舞步指令，并依据姿态传感器检测机体状态；(3)根据跳舞停止时间tf，随机产生ー组(tf/3个)I 16的数字，即生成了一串行为状态组的序列号；(4)读入ー个序列号，对应该序列号的3个行为状态被依次激活；(5)将当前激活的行为状态翻译成双轮机器人的运动指令，计算电机控制量；其中，计算电机控制量的具体方法如下机器人电机的控制量由两部分叠加平衡控制量和行进控制量。其中，平衡控制量由姿态传感器检测到机器人的倾角值以及倾角速度配合非线性PID算法计算得出；而行进控制量由机器人左右轮上编码器测得的位移、速度和加速度配合PID算法得出。左右轮电机的控制量分别为U1 = n* (Ub+Ua)Ur = n* (Ub+Ua)其中，U1表示左轮电机控制量，Ur表示右轮电机控制量，Ub表示平衡控制量，Ua表示行进控制量，η取值I或2,表示速度系数,当η = I时,表示电机慢速转动，当η = 2时表示电机快速转动。机器人两轮的电机控制量分别由上述平衡控制量和行进控制量配合速度系数叠加而成。电机上连接有2路信号，一路为电机控制量的大小，由上述方法计算得出；另一路为电机转动方向，即控制电机正转或者反转，当该路信号为高电平，电机正转，反之电机反转。(6)机器人根据计算的电机控制量开始跳舞。(7)判断是否有下一个序列号读入，如果有，重复步骤(4) (6)，如果没有，机器
人停止跳舞。本发明方法的流程图见附图5。本发明可以获得如下有益效果
本发明对双轮机器人舞步行为进行建模，将机器人的行为状态模块化，结构清晰，调试方便；本发明机器人不是完成预定在机器内部的指令，而是能够伴随音乐节拍，自由舞蹈，使机器人舞步动作更具有真实性和观赏性。以下结合附图和具体实施方式
对本发明做进一步说明。
附图1，本发明双轮机器人轴侧结构图；附图2，本发明双轮机器人仰视图；附图3，本发明双轮机器人电气连接图；附图4，本发明机器人舞步行为状态示意图；附图5，本发明方法流程图；附图6，本发明双轮机器人华尔兹前进后退舞步集合图；附图7，本发明双轮机器人华尔兹鋳躇舞步集合图；附图8，本发明双轮机器人华尔兹旋转舞步集合图；附图9，本发明双轮机器人华尔兹交叉舞步集合图；图中1_机器人主体水晶板；2-车轮；3_电机；4_电池；5-DSP28335主控板；6_电源管理板；7_姿态传感器；8_电机驱动板；9_编码器；10_磁铁
具体实施例方式首先分析人类华尔兹舞步规则，确定双轮机器人的舞步行为状态以及行为状态转移；然后搭建华尔兹舞步行为状态1 型，确定华尔兹舞步行为状态之间的互锁关系；最后自主生成一套双轮机器人的华尔兹舞步。该实验具体步骤如下(I)初始化配置DSP主控制器寄存器的状态，包括IO端口、A/D通道设置，初始化电机控制量，记录姿态传感器零点模拟电压值，设置开始时间h = 0，机器人初始状态q(l =S,给定停止时间tf = 30s ；(2)分析华尔兹舞步人类华尔兹是三步舞，即三个基本动作组合形成一个舞步，其中每个基本动作对应一个节拍。华尔兹舞姿基本由前进、后退、旋转三种舞姿配合相应的速度和角度变换构成。(3)分析双轮机器人模拟华尔兹舞步可行性双轮机器人可以完成基本的前进、后退、旋转动作，并且可以通过电机控制变换不同的运行速度和旋转角度。(4)搭建双轮机器人Stateflow舞步行为模型
首先确定机器人舞步行为状态集合Q = {S，F，B，L，R}然后确定机器人舞步动作指令集合T = {f1； f2, b1； b2, I卜3，14_6，iv3，r4_6}将其作为舞步行为状态的子状态；(5)根据上述搭建的Stateflow舞步行为模型，将双轮机器人三个舞步行为状态组合构成一个行为状态组，即确定该状态组内行为状态之间的互锁关系，并把这些组合编号，如下所示I)左前进舞步
①δ (S, I1) = L,② δ (L, = F,③ δ (F, T1) =L;2)右前进舞步①δ (S, T1) = R,② δ (R, fj) = F,③ δ (F, I1) =L;3)左后退舞步①δ (S, I2) = L,② δ (L, Id1) = B,③ δ (B, r2) =R;4)右后退舞步①δ (S, r2) = R,② δ (R, Id1) = B,③ δ (B, I2) =L;5)鋳躇左前进舞步①δ (S, f) = F,② δ (F, I1) = L,③ δ (R, I2) =L;6)鋳躇右前进舞步①δ (S, = F,② δ (F, = R,③ δ (R, r2) =R;7)鋳躇左后退舞步①δ (S, b) = B,② δ (B, I2) = L,③ δ (R, I1) =L;8)鋳躇右后退舞步①δ (S, Id1) = B,② δ (B, r2) = R,③ δ (R, =R;9)前进左旋转180舞步①δ (S, f) = F,② δ (B, I3) = L,③ δ (R, I3) =L;10)前进右旋转180舞步①δ (S, = F,② δ (B, r3) = R,③ δ (R, r3) =R;11)后退左旋转180舞步①δ (S, b) = B,② δ (B, I3) = L,③ δ (R, I3) =L;12)后退右旋转180舞步①δ (S, b) = B,② δ (B, r3) = R,③ δ (R, r3) =R;13)前进交叉180舞步①δ (S, = F,② δ (B, I6) = L,③ δ (L, r6) =R;14)后退交叉180舞步①δ (S, b) = B,② δ (B, r6) = R,③ δ (R, I6) =L;15)原地左自旋转舞步①δ (S, I3) = L,② δ (L, I3) = L,③ δ (L, I3) =L;16)原地右自旋转舞步
①δ (S, r3) = R,② δ (R, r3) = R,③ δ (R, r3) =R;本发明双轮机器人华尔兹舞步示意图如附图6-9所示。(6)双轮机器人所有华尔兹舞步如上所述，随机产生一组I 16的数字，即生成了一串行为状态组的序列号，作为上述状态流程模型的驱动事件；(7)当前序列号对应的三个行为状态被依次激活；(8)将当前激活的行为状态翻译成双轮机器人动作指令，用平衡控制量和行进控制量叠加的控制算法计算出电机控制量；(9)机器人根据电机控制量计算结果控制电机转动，机器人开始自由舞蹈。 (10)判断是否读入下一个状态组序列号，如果是重复步骤(7) (9)，如果否，机
器人停止跳舞。
权利要求
1.一种双轮机器人的自由舞步生成方法，其特征在于 (I)机器人初始化； (2 )生成自由舞步指令，检测状态； (3)机器人根据生成的自由舞步指令，开始自由舞蹈。
2.根据权利要求I所述的一种双轮机器人自由舞步生成方法，其特征在于自由舞步指令由以下步骤生成 (I)确定模型参数，本发明涉及的机器人舞步行为状态机为一个五元组 M= {Q, T, 8 , q0, D}； 其中， Q为机器人舞步行为状态集合，包括静止、前进、后退、转向这几种基本的状态； Q= {S，F，B，L, R} 其中， S表示机器人静止； F表示机器人前进； B表示机器人后退； L表不机器人左转； R表不机器人右转； 以上所述前进和后退方向以当前机器人的朝向为参考方向，旋转方向均表示俯视视角中机器人的旋转方向； T表示机器人舞步行为指令集合，同时作为舞步行为状态集合Q的子状态集合，包括前进，后退，转弯3种基本的行为，配合不同的速度和角度，规定机器人左右轮正反转方向描述如下当机器人左右轮转速一致，使机体前进的左右轮转向为正方向，反之为反方向。
T- {f D f2> b1； b2, 11-3, 14-6) ^V3, r4_6} 其中， f表示机器人左右轮均慢速正转，机体以慢速前进； f2表示机器人左右轮均快速正转，机体以快速前进； b:表示机器人左右轮均慢速反转，机体以慢速后退； b2表示机器人左右轮均快速反转，机体以快速后退； I1表示机器人左轮静止，右轮以慢速正转，机体以左轮为圆心慢速左转45° ; I2表示机器人左轮以慢速反转，右轮静止，机体以右轮为圆心慢速左转45° ; I3表示机器人左轮以慢速反转，右轮以慢速正转，机体呈慢速逆时针自旋转90° ; I4表示机器人左轮静止，右轮以快速正转，机体以左轮位圆心快速左转90 ° ; I5表示机器人左轮以快速反转，右轮静止，机体以右轮为圆心快速左转90° ; I6表示机器人左轮以快速反转，右轮以快速正转，机体呈快速逆时针自旋转180° ； r:表示机器人左轮以慢速正转，右轮静止，机体以右轮为圆心慢速右转45° ; r2表示机器人左轮静止,右轮以慢速反转,机体以左轮为圆心慢速右转45° ; r3表示机器人左轮以慢速正转,右轮以慢速反转,机体呈慢速顺时针自旋转90° ; r4表示机器人左轮以快速正转，右轮静止，机体以右轮为圆心快速右转90° ; r5表示机器人左轮静止，右轮以快速反转，机体以左轮为圆心快速右转90° ;r6表示机器人左轮以快速正转，右轮以快速反转，机体呈快速顺时针自旋转180° ； 假设机器人当前行为状态持续时间为ls，旋转角度由姿态传感器配合PID算法控制； .6为映射关系集合，即机器人行为状态转移函数的集合； 机器人的所有行为状态遵循以下映射关系.6 (Q, fi)=F；(i = 1,2) . 6 (QA)=B ；.6 (Q, IjO=L ；(j=l,2,3,4,5,6) .5 (Q, r」)=R ； 上述映射关系中等式左边括号里的两个参数分别表示转移源状态，其为Q中的元素，转移要素或输入信号，其为T中的元素等式右边表示转移终状态，其为Q中的元素； Q0为机器人的初始状态。令机器人的初始状态为静止状态S，即q(l = S G Q ; D为终端状态集合，终端状态是Q中的元素，即D G Q ; (2)运用状态流程控制和事件驱动方法对舞步行为进行建模；将机器人舞步的每个行为状态，即Q中的每个元素作为一个行为状态模块，T中的元素作为舞步行为状态Q的子行为状态模块； (3)人类华尔兹是三步舞，即三个基本动作组合形成一个舞步，其中每个基本动作对应一个节拍；将双轮机器人的一个行为状态对应人类华尔兹的一个节拍，根据上述搭建的Stateflow舞步行为模型，将双轮机器人三个舞步行为状态组合构成一个行为状态组，即确定了行为状态之间的互锁关系，并把这些组合编号，如下所示 .1.左如进舞步① 8 (S，I1) =L,② 6 (LJ1) =F,③ 6 (Fj1)=R; .2.右如进舞步① 6 (Sj1) =R,② 6 (R，fl)=F，③ 6 (F1I1)=L5 .3.左后退舞步① 8 (S，I2) =L,② 6 (LA)=B,③ 6 (B, r2)=R； .4.右后退舞步① 6 (S，r2)=R，② 6 (RJ1) =B,③ 6 (B，I2) =L; .5.鋳躇左前进舞步① 6 (SJ1) =F,② 6 (F，I1) =L,③ 6 (IU2) =L; .6.鋳躇右前进舞步① 6 (SJ1) =F,② 6 (F, T1) =R, (3) 6 (R, r2)=R； .7.鋳躇左后退舞步① 8 (SA)=B,② 6 (B，I2) =L,③ 6 (R1I1)=L5 .8.鋳躇右后退舞步 ① 8 (SA)=B,② 6 (B，r2)=R，③ 6 (Rj1)=R; .9.前进左旋转180舞步① 6 (SJ1) =F,② 6 (B，I3) =L,③ 6 (IU3) =L; .10.前进右旋转180舞步① 6 (SJ1)=F,② 6 (B，r3)=R，③ 6 (R, r3)=R；.11.后退左旋转180舞步① 8 (SA)=B,② 6 (B，I3) =L,③ 6 (IU3) =L;.12.后退右旋转180舞步① 8 (SA)=B,② 6 (B，r3)=R，③ 6 (R, r3)=R；.13.前进交叉180舞步① 6 (SJ1) =F,② 6 (B，I6) =L,③ 6 (L, r6)=R；.14.后退父叉180舞步 ① 8 (SA)=B,② 6 (B，r6)=R，③ 6 (R，16)=L;.15.原地左自旋转舞步① S (S，13)=L，② 5 (L, 13)=L，③ 5 (L, I3) =L ；. 16.原地右自旋转舞步① 8 (S，r3) =R,② 6 (R, r3) =R,③ 6 (R, r3) =R ； (4)随机产生一组I 16的数字，即生成了一串行为状态组的序列号，所述的序列号对应(3)中同一数字所表示的舞步，作为状态流程模型的驱动事件，当前序列号对应的三个行为状态被依次激活，将当前激活的行为状态翻译成双轮机器人动作指令，用控制算法计算出电机控制量，进而生成了一套双轮机器人的华尔兹舞步。
其中，计算电机控制量的具体方法如下 设机器人电机的控制量由两部分叠加平衡控制量和行进控制量。其中，平衡控制量由姿态传感器检测到机器人的倾角值以及倾角速度配合非线性PID算法计算得出；而行进控制量由机器人左右轮上编码器测得的位移、速度和加速度配合PID算法得出。左右轮电机的控制量分别为U1=]!*(Ub+Ua) (I)Ur=n*(Ub+Ua) (2) 其中，U1表示左轮电机控制量，Ur表示右轮电机控制量，Ub表示平衡控制量，Ua表示行进控制量，n取值I或2,表示速度系数,当n=l时,表示电机慢速转动，当n=2时表示电机快速转动； 机器人两轮的电机控制量分别由上述平衡控制量和行进控制量配合速度系数叠加而成。电机上连接有2路信号，一路为电机控制量的大小，由上述式(I)和式(2)计算得出；另一路为电机转动方向，即控制电机正转或者反转，当该路信号为高电平，电机正转，反之电机反转。
全文摘要
一种双轮机器人的舞步规划方法，属于智能机器人领域。为了使机器人能够真实的模拟人类的舞步行为，本发明采用状态流程控制和事件驱动的方法，将机器人前进、后退、转向3种基本行为，配合相应的速度和角度变化，对机器人舞步行为进行建模，基于该模型，规定各行为状态之间的互锁关系，产生操作(控制量)，智能生成一套双轮机器人舞步。
文档编号G05D1/02GK102707717SQ20121013564
公开日2012年10月3日 申请日期2012年5月3日 优先权日2012年5月3日
发明者于建均, 周路, 孙亮, 杜洪伟, 杨琼, 王冠伟, 阮晓钢 申请人:北京工业大学