CN103921266A - 一种基于Kinect的体感控制冰雪机器人方法 - Google Patents

Abstract

本发明涉及一种基于Kinect的体感控制冰雪机器人方法，负载机器人由电机驱动，可前进、后退、左右转向，其特征在于：冰雪雕塑设置于负载机器人上方，通过Kinect体感传感器获取人的手臂运动状态变化并转化为控制信号，前述控制信号传送至负载机器人的控制单元，控制单元控制负载机器人进行相应运动。

Description

一种基于Kinect的体感控制冰雪机器人方法

技术领域

本发明涉及一种基于Kinect的体感控制冰雪机器人方法。

背景技术

冰雕、雪雕是北方冰雪文化艺术最重要形式。多年以来，冰雕、雪雕都是利用冰块或雪块雕塑而成的静态观赏作品，辅以灯光等装饰，增强美感。这种传统文化艺术形式的缺点是不具备任何与观赏者互动的功能。目前的冰雕、雪雕作品都没有与现代机器人技术以及多种先进的人机交互技术相结合，尤其是没有与地面移动机器人技术相结合，形成互动的新型的、动态的、能够与人互动的艺术类型。

目前，地面移动机器人是机器人的主要类型之一。地面移动机器人在军事、运输及家庭服务领域都有广泛的应用。工业运输机器人AGV（Automated GuidedVehicle）是轮式地面移动机器人的一种，这种机器人能够负载货物，利用磁导航、光学导航（巡线）、卫星导航以及其他导航方式按照设计好的路径往复运动或者多个同类机器人配合在工厂等工业现场环境中完成一定的运载任务。美国、日本、德国、瑞士、中国等在此类移动机器人研究和应用方面有许多成功的案例。但目前的运输机器人的应用只限于工业领域，没有与艺术、文化、娱乐等领域融合，拓展应用范围。

Kinect是一种3D体感摄影机，同时它导入了即时动态捕捉、影像辨识、麦克风输入、语音辨识、社群互动等功能。它是一种新型的人机交互系统，也是一种新的体态感知传感器，其应用领域很广，如虚拟试衣镜、3D建模、虚拟乐器、虚拟娱乐以及机械控制等。目前，日本借助其先进的机器人技术，科学家应用Kinect传感器对机器人实时控制进行了实验，获得一定成效，证明了用Kinect传感器实现机器人的控制方法是可行的。美国已经将Kinect应用于军事战地巡逻机器人上，Kinect实时侦测机器人前方三维环境信息以决策机器人的运行，同时可以重构机器人运行过程中的三维地图。

发明内容

本发明目的在于提供一种基于Kinect的体感控制冰雪机器人方法，能够将冰雪雕塑与机器人结合，并通过体感进行控制。

实现本发明目的技术方案：

一种基于Kinect的体感控制冰雪机器人方法，负载机器人由电机驱动，可前进、后退、左右转向，其特征在于：冰雪雕塑设置于负载机器人上方，通过Kinect体感传感器获取人的手臂运动状态变化并转化为控制信号，前述控制信号传送至负载机器人的控制单元，控制单元控制负载机器人进行相应运动。

通过Kinect传感器获取人体右肩关节坐标和右手坐标，Kinect传感器将信号传送至PC机，PC机运行手臂运动识别算法，对获得的人体右肩关节坐标和右手坐标进行滤波平滑处理，计算右手臂左右摆动的角度θ、左摆或右摆的标志f₁及前摆或后摆的距离变化值f₂。

负载机器人的控制单元根据f₂控制机器人前进或后退，根据f₁和θ控制机器人左转或右转的角度。

负载机器人的控制单元还接有转动角度测量单元，根据转动角度测量单元测得的机器人实际左转或右转角度，控制单元对转动角度进行PID控制。

负载机器人的控制单元可对转动角度进行PD控制，公式如下，

$u_i=K_p[e_i+\frac{T_D}{T}(e_i-e_{i-1})]$

u_i表示第i次全量输出，e_i表示第i次采样时刻给定值和实际输出值的偏差，e_i-1表示第i-1次采样时刻给定值和实际输出值的偏差，T表示采样周期，K_p是比例系数，T_D是微分时间常数。

取K_p=20，T=0.01s，T_D=0.003s。

PC机运行手臂运动识别算法计算手臂运动时，采用双指数滤波平滑算法。

负载机器人的控制单元接无线通讯模块，PC机通过无线通讯方式将控制信号传送至负载机器人的控制单元。

本发明具有的有益效果：

本发明将冰雪雕塑设置于负载机器人上方，通过Kinect体感传感器实现负载机器人的体感控制，从而实现冰雪雕塑的移动，实现冰雪雕塑与观赏者的互动，实现艺术与娱乐完美的结合。本发明易于实施，操控简单，且控制精度高。本发明PC机运行手臂运动识别算法计算手臂运动时，采用双指数滤波平滑算法，能够有效减少手臂运动过程中抖动对手臂运动计算的影响。本发明负载机器人的控制单元可对转动角度进行PD控制，进一步提高控制精度，同时有效减少计算量。

附图说明

图1是冰雪机器人外观示意图；

图2是本发明控制原理框图；

图3是Kinect体感传感器坐标系示意图；

图4是Kinect体感控制冰雪机器人整体算法流程图；

图5是手臂运动识别算法流程图；

图6是双指数滤波算法实现关节坐标平滑流程图；

图7是右手臂在Kinect前面左右摆动示意图；

图8是冰雪机器人PID控制算法流程图。

具体实施方式

如图1所示，本发明冰雪机器人分为上、下两半部分，上半部分为冰雕或雪雕1，下半部分为用轮子实现移动的负载机器人2，冰雕或雪雕1坐落在负载机器人2的底盘上。负载机器人底盘四周安装了4个轮子，其中前边两个轮子通过电机带动，后边两个轮子跟随前边两个轮子随动运行，这些轮子足以承担机器人车体和冰雕或雪雕的重量。负载机器人电机驱动前轮同时正传或反转可实现机器人的前进或后退，而通过前轮的差速可实现机器人的左转或右转，此为现有技术。

如图2所示，当人站在Kinect体感传感器面前时，通过Kinect体感传感器可获得人体的20个骨骼点的三维坐标，这些坐标值通过PC机读取Kinect传感器的信息即可获取。例如肘关节坐标、肩关节坐标、腕关节坐标、脚踝关节坐标和两只手的坐标等等。本发明中只需要右肩关节坐标和右手坐标，以S(x_s,y_s,z_s)表示右肩关节坐标，以T(x_t,y_t,z_t)表示右手坐标，其中Kinect体感传感器的三维坐标系如图3所示。Kinect传感器将信号传送至PC机，PC机运行手臂运动识别算法，对获得的人体右肩关节坐标和右手坐标进行滤波平滑处理，计算右手臂左右摆动的角度θ、左摆或右摆的标志f₁及前摆或后摆的距离变化值f₂，负载机器人的控制单元根据f₂控制机器人前进或后退，根据f₁和θ控制机器人左转或右转的角度。负载机器人的控制单元接有转动角度测量单元，转动角度测量单元用于其实际转动角度的测量，根据转动角度测量单元测得的机器人实际左转或右转角度，控制单元对转动角度进行PID控制。负载机器人的控制单元还接有显示单元，显示单元用于机器人运动状态信息及速度信息的显示。负载机器人的控制单元接无线通讯模块，PC机通过无线通讯方式将控制信号传送至负载机器人的控制单元。

如图4所示，本发明基于Kinect的体感控制冰雪机器人方法具体步骤如下：

以f₁表示右手臂左摆或右摆的标志，f₂表示右手臂前摆或后摆的距离变化值，θ表示右手臂左右摆动的角度。

步骤1：

通过Kinect传感器获取人体右肩关节坐标和右手坐标。

步骤2：

通过运行于PC机上的手臂运动识别算法对获得的人体右肩关节坐标和右手坐标进行滤波平滑处理，计算右手臂左右摆动的角度θ、左摆或右摆的标志f₁及前摆或后摆的距离变化值f₂。

步骤3：

将步骤2中计算的θ、f₁和f₂结果值组成一个数据包，加上数据包头和校验和，通过无线数字传输模块发送给负载机器人的控制单元。

步骤4：

负载机器人的控制单元接收到步骤3发送的数据包后，驱动器根据f₂的值驱动机器人前进或后退，根据f₁及θ的值利用PID（Proportion Integral Derivative）控制算法控制机器人左转或右转θ角。

步骤5：

返回步骤1，重复上述步骤，最终使冰雪机器人跟踪人体右手臂的运动。

以下分别说明步骤2中的手臂运动识别算法和步骤4中的PID控制算法的详细步骤。

如图5所示，步骤2中的手臂运动识别算法具体如下：

（1）对从Kinect体感传感器获得的右肩关节坐标和右手坐标数据利用双指数滤波算法进行平滑，以减少右手臂运动过程中的抖动。

关节坐标双指数滤波算法原理及流程：

本发明采用双指数平滑滤波算法。t表示时间，{x_t}表示原始数据序列，{s_t}表示t时刻双指数平滑结果，{b_t}表示t时刻数据序列趋势的最优估计，F_t+m表示x在t+m时刻的最优估计，m为预测因子，且m>0，双指数平滑滤波算法具体公式如下：

s₁=x₀    (1)

b₁=x₁-x₀    (2)

s_t=αx_t+(1-α)(s_t-1+b_t-1),t>1    (3)

b_t=β(s_t-s_t-1)+(1-β)b_t-1,t>1    (4)

F_t+m=s_t+mb_t    (5)

其中α表示数据平滑因子，且0<α<1,β表示趋势平滑因子，且0<β<1。定义F₁=s₀+b₀，这样x在所有时刻的值都可按照公式估计出来。

如图6所示，以右肩关节坐标平滑为例，右手坐标平滑类似，具体平滑实现步骤如下：

步骤2.1：初始化多个参数。

数据平滑因子α初始化为0.5，趋势平滑因子β初始化为0.25，预测因子m初始化为0.5，s_n表示右肩关节坐标平滑输出，始化为(0,0,0)，b_n表示右肩关节坐标趋势最优估计，初始化为(0,0,0)，F_n+1表示右肩关节坐标最终结果最优估计，初始化为(0,0,0)，v_n表示当前从Kinect获取的右肩关节坐标，初始化为(0,0,0)，n表示计数变量，每获取一次右肩关节的坐标，n加1，定义为整型变量，初始化n=0。

步骤2.2：

从Kinect获取右肩关节坐标v₀，进入第一次迭代n=0，趋势预测b₀赋值为0，平滑输出s₀赋值为当前右肩关节坐标v₀，右肩关节坐标最终输出F₁=s₀+b₀，n加1。

步骤2.3：

从Kinect获取右肩关节坐标v₁，进入第二次迭代n=1，平滑输出s₁赋值为当前右肩关节坐标v₁和上次右肩关节v₀的平均值，即趋势预测b₁=(s₁-s₀)β，右肩关节坐标最终输出F₂=s₁+mb₁，n加1。

步骤2.4：

从Kinect获取右肩关节坐标v₂，进入第三次迭代n=2，平滑输出s₂按照公式

s_n=αv_n+(1-α)(s_n-1+b_n-1)计算，趋势预测b₂按照公式b_n=β(s_n-s_n-1)+(1-β)b_n-1计算，右肩关节坐标最终输出按照公式F_n+1=s_n+mb_n计算，n加1。

步骤2.5：

从Kinect获取下一右肩关节坐标v_n重复步骤4，迭代计数变量n不断的加1，由于n为整型变量，所以不断的加1最终使n溢出清零，又回到步骤2重新迭代，因此，随着Kinect前方的人右手臂运动，就能够实现对右肩关节坐标的不断平滑。右手坐标平滑类似。

（2）右肩关节坐标和右手坐标经平滑处理后，将其坐标值每隔100ms进行一次如下计算：

以和分别表示上一次右手坐标平滑结果及当前右手坐标平滑结果，^S(x_s,y_s,z_s)同时也表示右肩关节坐标平滑结果。参考图7，假设右手臂初始位于中心位置，100ms后向左摆到图7中的位置，形成夹角θ。在Kinect坐标系下构建向量 $\stackrel{\&RightArrow;}{{\mathrm{ST}}^p}=(x_t^p-x_s,y_t^p-y_s,z_t^p-z_s),$

$\stackrel{\&RightArrow;}{{\mathrm{ST}}^c}=(x_t^c-x_s,y_t^c-y_s,z_t^c-z_s),$ 根据三维向量夹角计算公式，设

$\stackrel{\&RightArrow;}{a}=(x_1,y_1,z_1),\stackrel{\&RightArrow;}{b}=(x_2,y_2,z_2),$ 则向量和的夹角余弦为

$\cos <\stackrel{\&RightArrow;}{a},\stackrel{\&RightArrow;}{b}>=\frac{(x_1{x}_2+y_1{y}_2+z_1{z}_2)}{\sqrt{x_1^2+y_1^2+z_1^2}\&CenterDot;\sqrt{x_2^2+y_2^2+z_2^2}},$ 其夹角

$\mathrm{\&alpha;}=\arccos \frac{(x_1{x}_2+y_1{y}_2+z_1{z}_2)}{\sqrt{x_1^2+y_1^2+z_1^2}\&CenterDot;\sqrt{x_2^2+y_2^2+z_2^2}}.$ 从而可得

$\mathrm{\&theta;}=\arccos \frac{((x_t^p-x_s)\&CenterDot;(x_t^c-x_s)+(y_t^p-y_s)\&CenterDot;(y_t^c-y_s)+(z_t^p-z_s)\&CenterDot;(z_t^c-z_s))}{\sqrt{{(x_t^p-x_s)}^2+{(y_t^p-y_s)}^2+{(z_t^p-z_s)}^2}\&CenterDot;\sqrt{{(x_t^c-x_s)}^2+{(y_t^c-y_s)}^2+{(z_t^c-z_s)}^2}}$

最后用当前的右手坐标覆盖上一次的右手坐标以备下次使用。

参考图3，当人站在Kinect体感传感器面前右手水平左右摆动时，其坐标主要在x轴方向变化，如果当前右手坐标平滑结果比上一次右手坐标平滑结果的x坐标值大，说明手向右摆动，反之向左摆动。当人右手水平前后摆动时，其坐标主要在z轴方向变化，如果当前右手坐标平滑结果比上一次右手坐标平滑结果的z坐标值大，说明手朝远离Kinect体感传感器方向摆动，反之朝靠近Kinect体感传感器方向摆动。以f₁表示右手左摆或右摆的标志，f₂表示右手前摆或后摆的距离变化值，f₁及f₂定义如下：

f₁默认为0，其定义中的值和阈值0.05作比较，只有才认为右手臂摆动，如果则认为右手臂只是很小幅度的摆动，f₂类似。保存计算结果θ、f₁和f₂。

如图8所示，步骤4中的冰雪机器人PID控制算法具体如下：

PID控制器是一种线性调节器，这种调节器是将系统的给定值r与实际输出值y构成的控制偏差c=r-y的比例（P）、积分（I）、微分（D），通过线性组合构成控制量，所以简称PID控制器。连续控制系统中的模拟PID控制规律为：

$u\left(t\right)=K_p[e\left(t\right)+\frac{1}{T_I}\&Integral;e\left(t\right)\mathrm{dt}+T_D\frac{\mathrm{de}\left(t\right)}{\mathrm{dt}}]---\left(6\right)$

式中u(t)是控制器的输出，e(t)是系统给定量与输出量的偏差，K_p是比例系数，T_I是积分时间常数，T_D是微分时间常数。

机器人控制系统是采样控制系统，不能直接利用上述连续控制系统的公式，只能根据采样时刻的偏差值计算控制量。因此将（6）式离散化后如下：

$u_i=K_p[e_i+\frac{T}{T_I}\underset{j=0}{\overset{i}{\mathrm{\&Sigma;}}}{e}_j+\frac{T_D}{T}(e_i-e_{i-1})]---\left(7\right)$

当采样周期足够小时，这种离散逼近相当准确。上式中u_i表示第i次全量输出，它对应于被控对象的执行机构第i次采样时刻应达到的位置，T表示采样周期，e_i表示第i次采样时刻给定值和实际输出值的偏差，e_i-1表示第i-1次采样时刻给定值和实际输出值的偏差，表示从零时刻开始到第i次采样时刻给定值和实际输出值的偏差求和。由于PID控制器中的积分项主要用于消除稳态误差，而冰雪机器人对稳态误差要求比较小，所以只需要PD控制即可满足要求，同时也可减少计算量及偏差存储量，则公式变为

$u_i=K_p[e_i+\frac{T_D}{T}(e_i-e_{i-1})]$

$\left(8\right)$

冰雪机器人PID控制算法步骤如下：

θ^c表示机器人实际转过的角度，e^c表示机器人当前给定角度θ和实际转过角度θ^c的偏差，即e^c=θ-θ^c，e^p表示机器人上次给定角度和实际转过角度的偏差，u表示给电机的控制信号，即脉宽调制信号。通过把手臂运动控制信息转化为脉宽调制信号即可实现机器人电机的运动控制。

步骤4.1：

取K_p=20，T=0.01s，T_D=0.003s，机器人核心控制单元通过无线数字传模块获取手臂运动识别算法的输出结果包括θ、f₁和f₂。

步骤4.2：

判断f₂的值，如果|f₂|≥0.15，对于f₂>0，则给机器人一个PWM信号使其前进，返回步骤4.1；对于f₂<0，则给机器人一个PWM信号使其后退，返回步骤4.1。否则进入步骤4.3。

步骤4.3：

判断f₁的值，如果|f₁|不等于0，对于f₁=1，给机器人一个PWM信号使其右转θ角；对于f₁=2，给机器人一个PWM信号使其左转θ角。

步骤4.4：

等待机器人采样时间到，读取当前机器人实际转过的角度θ^c，计算e^c=θ-θ^c。

步骤4.5：

如果e^c等于0，则说明机器人已经转过θ角，返回步骤4.1，否则由公式计算给电机的控制信号u，使机器人朝e^c=0的方向转动。将当前偏差保存到e^p，即令e^p=e^c，以备下次计算使用，返回步骤4.4。

按照上述步骤不断重复即可使冰雪机器人在人的右手臂前摆时前进，后摆时后退，左摆θ角时能够立刻跟踪并转动θ角，右摆θ角时也能够立刻跟踪并转动θ角。最终实现了观赏者通过体感控制冰雪机器人，增加了冰雕或雪雕作品的娱乐性、互动性、灵活性、趣味性及操控性，使观赏者和冰雕或雪雕作品之间的交互更加友好。

Claims (8)

1.一种基于Kinect的体感控制冰雪机器人方法，负载机器人由电机驱动，可前进、后退、左右转向，其特征在于：冰雪雕塑设置于负载机器人上方，通过Kinect体感传感器获取人的手臂运动状态变化并转化为控制信号，前述控制信号传送至负载机器人的控制单元，控制单元控制负载机器人进行相应运动。

2.根据权利要求1所述的基于Kinect的体感控制冰雪机器人方法，其特征在于：通过Kinect传感器获取人体右肩关节坐标和右手坐标，Kinect传感器将信号传送至PC机，PC机运行手臂运动识别算法，对获得的人体右肩关节坐标和右手坐标进行滤波平滑处理，计算右手臂左右摆动的角度θ、左摆或右摆的标志f₁及前摆或后摆的距离变化值f₂。

3.根据权利要求2所述的基于Kinect的体感控制冰雪机器人方法，其特征在于：负载机器人的控制单元根据f₂控制机器人前进或后退，根据f₁和θ控制机器人左转或右转的角度。

4.根据权利要求3所述的基于Kinect的体感控制冰雪机器人方法，其特征在于：负载机器人的控制单元还接有转动角度测量单元，根据转动角度测量单元测得的机器人实际左转或右转角度，控制单元对转动角度进行PID控制。

5.根据权利要求4所述的基于Kinect的体感控制冰雪机器人方法，其特征在于：负载机器人的控制单元可对转动角度进行PD控制，公式如下，

$u_i=K_p[e_i+\frac{T_D}{T}(e_i-e_{i-1})]$

u_i表示第i次全量输出，e_i表示第i次采样时刻给定值和实际输出值的偏差，e_i-1表示第i-1次采样时刻给定值和实际输出值的偏差，T表示采样周期，K_p是比例系数，T_D是微分时间常数。

6.根据权利要求5所述的基于Kinect的体感控制冰雪机器人方法，其特征在于：取K_p=20，T=0.01s，T_D=0.003s。

7.根据权利要求6所述的基于Kinect的体感控制冰雪机器人方法，其特征在于：PC机运行手臂运动识别算法计算手臂运动时，采用双指数滤波平滑算法。

8.根据权利要求7所述的基于Kinect的体感控制冰雪机器人方法，其特征在于：负载机器人的控制单元接无线通讯模块，PC机通过无线通讯方式将控制信号传送至负载机器人的控制单元。