基于设备体感控制的双轮自平衡搏击机器人*

姜文涛, 张博强, 朱正安, 刘万军

(辽宁工程技术大学 软件学院,辽宁 葫芦岛 125105)

0 引 言

随着科学技术的发展,体感技术在医疗、影视、游戏等领域发挥着重大作用。目前微软Kinect[1～3]在体感识别领域使用较为广泛,且技术较为成熟,但成本较高,使用环境有局限性。目前体感游戏大多数为面对屏幕进行操作,游戏本身的体验感会大打折扣,所以本文设计的重点是实体对实体的体感搏击机器人。在机器人的设计中,现在大部分机器人是双腿站立[4],适逢平衡车[5～8]技术飞速发展,平衡车原理使机器人站立起来,可有效提高搏击机器人移动速度,并且具有高度的灵活性。

本文设计了一种基于设备体感控制的双轮自平衡搏击机器人。采用MPU6050传感器检测控制者的手臂姿态和上身倾斜动作,极大地减少了设备成本。利用传感器识别控制者的姿态,由机器人完成动作,从而实现了实体对实体的搏击体验感。

1 整体架构

基于设备体感控制的双轮自平衡搏击机器人拥有非常复杂的智能控制系统。在设备体感部分,由各个部位的陀螺仪检测控制者的姿态,并将指令发送给机器人,机器人由MPU6050传感器检测姿态信息,经过PD平衡算法处理再对电机施加控制使其保持平衡,整体架构如图1(a)所示。

1.1 体感架构

体感部分由1个STM32的核心板和13只MPU6050传感器构成,传感器在左右胳膊各分布6只,1只在腰部。腰部传感器用于检测控制者的前、后、左、右姿态。胳膊上的6只传感器检测控制者的出拳姿态。

1.2 搏击机器人架构

图1(b)是机器人整体架构,由2个直流电机、2个足轮、2个L5D11光栅测速模块、1个TBDFNG直流电机驱动模块、1个STM32开发板、1个HC-06、1个MPU6050、1个舵机驱动板、2个电源、2个机械手臂等组成。MPU6050检测机器人倾角,机械手臂由5个舵机组成,可以最大程度模拟人类手臂动作。

图1 系统整体架构与搏击机器人整体结构

2 实现原理

2.1 机器人原理

机器人在其所受外力不大于临界值时可以调回平衡状态。在机器人中有一只MPU6050传感器,用于实时检测机器人姿态信息,通过控制双轮的电机使得机器人达到一直平衡的稳态。根据上述原理得出,通过测量机器人的倾角和角速度,控制机器人向对应方向加速,消除倾角,实现机器人直立行走。因此,测量倾角和计算加速度是控制机器人持续直立的关键。

2.1.1 平衡算法

控制机器人作加速运动,分析机器人倒立摆受力情况,受到额外的惯性作用,该方向与车轮的加速度方向相反,大小成正比。受力分析如图2(a)所示,所受到的回复力为式(1)。由于θ很小,因此进行了线性化处理。假设稳态的负反馈控制是给轮子加速度a与θ偏角成正比,比例值k1。如果比例值k1>g,回复力方向与位移方向相反。

F=mgsinθ-macosθ≈mgθ-mk1θ

(1)

为了让机器人的倒立摆模型快速地回归平稳,还需要加入阻尼力。阻尼力与偏角的角速度成正比,方向相反,根据式(1)可得

F=mgθ-mk1θ-mk2θ′

(2)

可得出控制机器人加速度的算法

a=k1θ+k2θ′

(3)

式中θ为小车角度,θ′为角速度。k1,k2为比例系数。

以上内容分析,通过建立速度的比例微分负反馈控制,形成闭环系统使得机器人保持平衡。假设这个机器人受到外力的作用,会产生一个角加速度x(t),沿着垂直于机器人底盘方向进行受力分析,可得到如图2(b)所示,机器人的倾角与足轮运动加速度a(t)、外力干扰加速度x(t)之间的运动方程。

图2 机器人倒立摆模型及外力作用下受力分析

机器人模型运动方程

在角度θ很小时,运动方程简化为

机器人静止时

式中 系数k1,k2分别为比例和微分控制参数。k2等同于阻尼力,可以有效遏制机器人振动。

2.1.2 测 速

通过安装在电机输出轴上的光码盘测量得到机器人的双轮速度。利用计数器,检测一段时间内脉冲信号数量,可计算出机器人速度。

2.1.3 速度差控制倾角

假使机器人初始静立,然后提升到给定速率,为此机器人需前倾一定角度获得加速度。机器人直立情况下,为获得向前的倾斜角度,轮子需要向后运动,会引起轮子速度下降。由于负反馈,使得机器人往前倾角增大。如此循环,机器人就会倾倒。

为了确保系统的稳态,机器人倾角控制时间常数Tz很大。使得系统产生的振动现象,解决方法是：在控制反馈中加入对角度的微分和积分,这样就出现了一个比例控制环节,便可在稳态中传递函数不变,也避免了微分。因为使用了比例控制,若MPU6050安装有误差机器人会一直往一个方向运行,所以，在角度控制输出中加入了直接积分补偿,便可消除误差。机器人在启动时必须直立放置,才可正常运行。

2.1.4 整体控制

将上面所有对机器人直立行走的算法集中如图3所示。

图3 整体控制

采集MPU6050传感器的X,Y,Z轴信号,角速度信号和电机脉冲信号,便可实现机器人的直立行走。

2.2 体感原理

2.2.1 方向检测

当控制者身体前倾时,传感器的X轴角度会减少约19°；控制者后倾时传感器的X轴会增加约15°；控制者左倾时,传感器的Y轴减少约18°；控制者右倾时,传感器的Y轴增加约17°。控制者做出前后左右动作时陀螺仪采集到的数据如图4所示。

图4 前后左右时陀螺仪数据 (从第一个开始依次为前、后、左、右)

2.2.2 出拳检测

首先需要检测出手臂的姿态,使用MPU6050传感器,因为可以测出X,Y,Z三个方向的角度。只要将角度转化为空间中的坐标,对这些采样点进行曲线拟合,得到出拳轨迹。

1)角度坐标化

图5 手臂出拳的立体示意

在图5中大臂为直线OA传感器Z轴的角度为∠BAO,也是图6(a)中的∠Z1,并且大臂向传感器Y轴偏转的角度为∠OBC也是图6(a)中的∠Y1。

设大臂的MPU6050传感器放置的位置距离肩部为L则L′的表达式为

L′=Lsin∠Z1

(7)

小臂则是在大臂旋转的基础上进行更进一步的旋转,会先旋转到直线OE再到OF,然后传感器Y轴倾斜,直到合适的出拳姿态OG,其中∠GOH为传感器的Z轴为图6(a)中的∠Z2,传感器Y轴角度为∠HOD也是图6(a)中的∠Y2。

设小臂的传感器放置的位置距离肘部为l,则l′的表达式为

l′=lsin(Z2-90°)

(8)

图6 手臂出拳立体图的俯视图与对角线上的投影

在图6(b)中建立平面直角坐标系,以大臂肩部为原点O,其中Y1的角度为传感器Y轴角度。所以

Xd=L′sin∠Y1,Yd=L′cos∠Y1

(9)

将式(7)代入式(9)可得

Xd=Lsin∠Z1sin∠Y1,Yd=Lsin∠Z1cos∠Y1

(10)

由上式可算出大臂和肘部传感器在图6(a)的坐标系中的X,Y坐标值。

小臂上的2个传感器在图6(a)的坐标系中的计算方法要在大臂的基础上进行计算。其中Xd到Xx的距离为ΔX,Yd到Yx的距离为ΔY。

Xx和Yx的计算公式为其中Xd和Yd取肘部的传感器

Xx=ΔX+Xd,Yx=ΔY+Yd

(11)

ΔX和ΔY的计算为

ΔX=l′sin∠Y2,ΔY=l′cos∠Y2

(12)

将式(8)代入式(12)可得

ΔX=lsin(Z2-90°)sin∠Y2,

ΔY=lsin(Z2-90°)cos∠Y2

(13)

所以将式(13)代入式(11)得出

Xx=lsin(Z2-90°)sin∠Y2+Lsin∠Z1sin∠Y1

Yx=lsin(Z2-90°)cos∠Y2+Lsin∠Z1cos∠Y1

(14)

此时小臂、大臂、肘部传感器放置点的坐标可以求出。

2)采样点线性拟合

设手臂上这5个点为A(x1,y1),B(x2,y2),C(x3,y3),D(x4,y4),E(x5,y5)位置如图7(a)所示。

图7 手臂点示意与理想出拳线

使用拉格朗日插值法对这5个点进行曲线拟合,拟合公式为

pn(x)=f(x0)l0(x)+f(x1)l1(x)+…+f(xn)ln(x)

(15)

式中li(x)为

将A～E这5个点代入式(16)可得到l0(x)～l4(x)，再将其代入式(15)可以得到拟合成功的曲线。

根据这些点得到曲线后,分别求出B点和D点的斜率,在这两点的斜率设定一定范围用来确定控制者动作,进而实现了对控制者手臂姿态检测。其中，斜率的界限如图7(b)所示。

有曲线后再求出一阶导数可以计算出采样点处的斜率,图7(b)中的j1是直拳的下界,j2是直拳上届也是左勾拳、上勾拳大臂的下界,j3是左勾拳的上界,j4是左勾拳的下界和上勾拳的上界。当斜率在所属的范围内就可以判定拳种。

3 测 试

3.1 出拳测试

为验证三种出拳姿态的准确性和实时性。控制者手臂处于竖直状态时,进行陀螺仪角度初始化。控制者做出直拳时的姿态如图8(a)所示。当直拳动作时,陀螺仪采集到的数据如图8(b)所示。

图8 控制者出直拳姿态及采集到的数据 (从第一行左侧开始依次为A、B、C、D、E五个点)

控制者做直拳动作时,Z轴和Y轴曲线逐渐上升,分别在80°和45°附近保持稳定。

上勾拳动作时陀螺仪采集到的数据如图9所示。

图9 上勾拳时陀螺仪数据 (从第一行左侧开始依次为A、B、C、D、E五个点)

控制者做上勾拳动作时,A，B两点Y轴和Z轴曲线逐渐上升,分别为45°和75°附近保持稳定。C,D,E三点Y轴和Z轴曲线逐渐上升,前者在5°附近保持稳定,后者在120°附近保持稳定。

左勾拳动作时陀螺仪采集到的数据如图10所示。

图10 左勾拳时陀螺仪数据 (从第一行左侧开始依次为A、B、C、D、E五个点)

控制者做出左勾拳动作,A、B两点Y轴Z轴曲线逐渐上升,都在75°附近保持稳定。C,D,E三点Y轴Z轴曲线逐渐上升,前者在110°附近保持稳定,后者在20°附近保持稳定。出拳测试数据见表1。

表1 出拳测试数据

将表1中X,Y点代入式(16)就可以拟合出出拳轨迹。

3.2 机器人性能测试

为了测试小车在稳定状态下的最大运动速度、转向角速度、起步加速度,与双足机器人做对比,进行如下测试。

3.2.1 匀速运动

在测试开始和结束位置各放置一对光电门,然后记录经过2个光电门间隔时间。

间隔时间数据集中分布在[1.48,1.89],计算得到机器人的平均速度为0.728 m/s。

3.2.2 角速度测试

在机器人旋转数周进入匀速转动状态后,开始计时,并采集数据，通过测试机器人旋转一周用时集中在[3.64,4.82],计算得到机器人的角速度为1.505。

3.2.3 加速度测试

将纸带穿过打点计时器固定于机器人后方测量数据，测试数据见表2。

表2 机器人加速度测试数据

纸带上每4个点为一个计数点,表中的s1,s2,s3,s4为相邻计数点间距。计算得出平均加速度为2.015 m/s2。

本文设计的双轮自平衡搏击机器人和现有双足机器人相比,双轮机器人的运行速度可达到0.728 m/s,加速度可达到2.015 m/s2,加速到0.728 m/s时仅需要0.36 s,转向的角速度可以达到1.505,1.04 s可完成直角转弯。

4 结 论

本文所设计的基于设备体感控制的双轮自平衡搏击机器人,其中体感部分是由陀螺仪来实现的,相比于微软的Kinect,具有低成本和适用环境普适的优点。搏击机器人采用双轮代替双腿,使机器人移动敏捷性较强,实现了体感游戏中实体对实体的特殊体验,同时增强了用户的沉浸感与代入感。在实际的体验中具有稳定性高、娱乐性强的特点。