CN108638069B - 一种机械臂末端精确运动控制方法 - Google Patents

Abstract

本发明公开了一种机械臂末端精确运动控制方法，借助信息采集***、上位机控制***、下位机控制***通过机械运动***实现机械臂末端运动的精确控制，1)采集人体骨骼节点信号，获取操作者手臂各关节的准确的运动数据，并将信号转换为操作者手臂各关节连续平滑的运动轨迹曲线数据；2)建立机械臂末端精确控制的数学模型，将人体骨骼数据转换为机器人手臂相应伺服电机的转动角度信息，通过坐标变换与逆运动学计算得出机器人手臂各关节的控制信息；3)下位机控制***接收到上位机控制***发送过来的控制信息后，将平滑处理后的控制信息发送到机械运动***，驱动机器人手臂各关节的电机运动，最终实现末端的精确控制。

Description

一种机械臂末端精确运动控制方法

技术领域

本发明涉及一种提高机械臂运动精确控制机制，尤其是涉及一种高精度实时远程控制功能的机械臂末端精确控制方法。

背景技术

20世纪60年代末70年代初，随着电子数字技术的日益完善，研究人员的研究兴趣转移到集成自主操作与远程控制技术，以提高远程控制体系的工作效率。恰好在这时期工业机器人的概念应运而生。将传统的远程控制与特殊子任务的选择性自主结合能够潜在的减少劳动力的需求，并有效提高重复性工作的工作效率。这种自主操作与远程控制的结合为现在的远程控制机器人(telerobotics)及时奠定了理论基础。20世纪70年代至今，远程控制机器人技术已成为了研究和发展的重点，应用于许多不同的领域，包括核试验、宇宙空间和军事应用等许多环境不确定性的场合。与制造业自动化不同的是，危险和不确定性工作任务环境下的远程控制为确保操作的安全，人的参与是必要的。在许多非确定环境下完成有用的工作需要成熟的远程控制技术。

2005年《世界灾难报告》指出，在1995年～2004年，有超过900000人死于灾难。灾难造成的损失超过7380亿美元。国际红十字会公布的2014年度《世界灾难报告》显示，2013年全球共发生337起自然灾难，全球将近一亿人口受自然灾难牵累，超过2.2万人在灾难中死亡。2014年8月2日，江苏昆山一工厂发生特大***事件。因***后现场十分混乱，救灾人员不能进入***现场关闭电源总闸，导致安全隐患未能及时消除，引发了二次***，加重了经济损失。大部分自然灾害和重特大事故后的现场具有很大的不确定性以及潜在的危险性，给救援工作带来很大的不便，严重影响救援进度。这就要求有机器人能够代替人及时进入现场，返回复杂现场环境信息的同时能够探查灾难现场生存者的情况，并且能够通过远程控制利用高精度的机械臂来完成特定的任务。尤其对于涉及高精度实时远程控制功能的机械臂更是如此。

设计一款具备高精度实时远程控制功能的机器人代替人在非确定性环境中完成特定任务，涉及到获取准确的运动数据，建立高精度的数学模型，设计平滑稳定的控制算法等方面的研究。随着科技的进步，机器人技术也得到了极大的发展，机器人的用途逐渐从工业向家庭生活和服务业领域渗透，结构也从一个用户，一次重新设计向标准化和模块化方向发展。有利于推进机器人更快更好的步入日常生活中。

另外现有机器人技术普遍采用串行处理结构，这种分布式控制方式计算负担重、实时性差，所以大多采用离线规划和前馈补偿解耦等方法来减轻实时控制中的计算负担，当机器人在运行中受到干扰时其性能将受到影响，更难以保证高速运动中所要求的精度指标。

发明内容

本发明针对现有技术不足，提出一种机械臂末端精确运动控制方法，建立高精度实时远程控制功能的机械臂末端精确控制数学模型，通过坐标变换，获得机器人手臂末端信息，并将最终获得的末端信息通过逆运动学的方式求解出机器人每个关节的旋转角度，最终实现末端精准控制。

本发明所采用的技术方案：

一种机械臂末端精确运动控制方法，借助信息采集***、上位机控制***、下位机控制***通过机械运动***实现机械臂末端运动的精确控制，其实现步骤如下：

1)通过信息采集***采集人体骨骼节点信号，在深度摄像头上建立空间坐标系，并在此空间中获取操作者手臂各关节的准确的运动数据，并将信号转换为操作者手臂各关节连续平滑的运动轨迹曲线数据；

2)上位机控制***后利用人体骨骼数据建立机械臂末端精确控制的数学模型，利用模型将人体骨骼数据转换为机器人手臂相应伺服电机的转动角度信息，对待识别的手势动作的二轴陀螺数据序列进行预处理，以去除所述二轴陀螺数据序列中影响手势动作识别的冗余数据，通过坐标变换与逆运动学计算得出机器人手臂各关节的控制信息，并将控制信息发送给下位机控制***；3)下位机控制***接收到上位机控制***发送过来的控制信息后，利用贝塞尔曲线对控制信息进行平滑处理，并将平滑处理后的控制信息发送到机械运动***，驱动机器人手臂各关节的电机运动，最终实现末端的精确控制。

所述的机械臂末端精确运动控制方法，信号采集***使用微软的Kinect体感摄像头，采集人体骨骼节点信号，用于后续的数学建模；空间坐标系包括绝对坐标和相对坐标；其中绝对坐标是指人手臂骨骼点在Kinect CameraSpacePoint坐标系中的坐标；相对坐标是指以O点为原点建立的坐标系。

所述的机械臂末端精确运动控制方法，步骤2)中，上位机控制***利用人体骨骼节点信号建立机械臂末端精确控制的数学模型的过程包括：

步骤(1)，建立关于机器人肩部关节和机器人肘部关节的实时模仿模型，用于考虑肩部与肘部关节的运动；

步骤(2)，通过坐标变化和逆运动求解关节建立末端精确控制模型，利用机器人位置运动学知识进行坐标变换；

步骤(3)，建立机器人坐标系进行整体坐标预览。

所述的机械臂末端精确运动控制方法，上位机控制***将信息采集***采集到的数据进行滤波处理，获取准确的运动数据，同时利用骨骼节点信号建立机械臂末端精确控制的数学模型，经过坐标变换将坐标系变换到操作者身上，获得操作者手臂相对于操作者自身的运动参数；最后将坐标系从人身上变换到机器人身上，获得机器人手臂末端信息，并将最终获得的末端信息通过逆运动学的方式求解出机器人每个关节的旋转角度。

本发明的有益效果：

1、本发明机械臂末端精确运动控制方法，通过建立基于人体骨骼数据建立仿人机器人手臂重要关节的数学模型，利用模型将人体骨骼数据转换为机器人手臂相应伺服电机的转动角度信息。上述建模方法，只需要利用人体三个关节的三维坐标便可控制四个伺服电机的转动，数据处理的工作量更小。然后利用人体骨骼数据来建立相应模型，能够让动作模仿的更加准确，而且也拥有更好的适应性和未来进步完善的空间。

2、本发明机械臂末端精确运动控制方法，以机器人学知识为理论基础，进行坐标变换和利用逆运动学求解关节角，体现了该建模方法的科学性与可行性。同时通过这种方法建立的数学模型能够获得精确的末端坐标，从而使末端控制精准。坐标变换很好的解决了每次不同操作者不同站姿方向等因素引起的误差，使得机器人控制***具备更强的鲁棒性。将机器人手臂前三个关节等效为一个球关节，使得计算量大大降低。该建模方法可以保证末端控制精准，因此配备不同的末端执行装置，可以应用在广泛的领域与场合。

附图说明

图1是本发明机械臂末端精确运动控制方法建立数学模型的Kinect绝对坐标系的示意图；

图2是本发明建立精确控制数学模型相对坐标系的示意图；

图3为本发明机械臂末端精确运动控制方法肩部关节角度变换的示意图；

图4手臂运动时肘部关节坐标系的示意图；

图5为人体坐标系建立的示意图；

图6为手臂关节示意图；

图7为机器人坐标系简图；

图8为由机械臂结构简化的自由度分布简图；

图9为SEW平面变换图；

图10为人手臂平面图；

图11为机器人三维跟踪轨迹图；

图12为机器人二维跟踪轨迹图；

图13为协作机器人控制***框架。

具体实施方式

下面通过具体实施方式，对本发明的技术方案做进一步的详细描述。

实施例1

参见图13，为协作机器人控制***框架。本发明机械臂末端精确运动控制方法，借助信息采集***、上位机控制***、下位机控制***通过机械运动***实现机械臂末端运动的精确控制，其实现步骤如下：

1)通过信息采集***采集人体骨骼节点信号，将空间坐标系建立在深度摄像头上，并在此空间中获取操作者手臂各关节的准确的运动数据，并将信号转换为操作者手臂各关节连续平滑的运动轨迹曲线数据；

2)上位机控制***后利用人体骨骼数据建立机械臂末端精确控制的数学模型，利用模型将人体骨骼数据转换为机器人手臂相应伺服电机的转动角度信息，对待识别的手势动作的二轴陀螺数据序列进行预处理，以去除所述二轴陀螺数据序列中影响手势动作识别的冗余数据，通过坐标变换与逆运动学计算得出机器人手臂各关节的控制信息，并将控制信息发送给下位机控制***；

3)下位机控制***接收到上位机控制***发送过来的控制信息后，利用贝塞尔曲线对控制信息进行平滑处理，并将平滑处理后的控制信息发送到机械运动***，驱动机器人手臂各关节的电机运动，最终实现末端的精确控制。

实施例2

参见图1、图2，本实施例的机械臂末端精确运动控制方法，与实施例1的不同之处在于：信号采集***使用微软的Kinect体感摄像头，使用时Kinect体感摄像头安置在机器人的前方，采集人体骨骼节点信号，用于后续的数学建模；空间坐标系包括绝对坐标和相对坐标；其中绝对坐标是指人手臂骨骼点在Kinect CameraSpacePoint坐标系(以深度摄像头为原点)中的坐标；相对坐标是指以O点为原点建立的坐标系。

Kinect安置高度没有固定标准，根据使用经验，使其略带俯视人体的初始高度与角度效果较好，这样的高度可以使获得的骨骼节点信号更加稳定。

信号采集***通过USB与上位机控制***相连接，将采集到的人体骨骼节点信号传输到上位机控制***。

实施例3

本实施例的机械臂末端精确运动控制方法，与实施例1或实施例2的不同之处在于：步骤2)中，上位机控制***利用人体骨骼节点信号建立机械臂末端精确控制的数学模型的过程包括：

步骤(1)，建立关于机器人肩部关节和肘部关节的实时模仿模型，用于考虑肩部与肘部关节的运动；步骤(2)，通过坐标变化和逆运动求解关节建立末端精确控制模型，利用机器人位置运动学知识进行坐标变换；步骤(3)，建立机器人坐标系进行整体坐标预览。

实施例4

本实施例的机械臂末端精确运动控制方法，与前述各实施例不同的是：上位机控制***将信息采集***采集到的数据进行滤波处理，获取准确的运动数据，同时利用骨骼节点信号建立机械臂末端精确控制的数学模型，经过坐标变换将坐标系变换到操作者身上，获得操作者手臂相对于操作者自身的运动参数；最后将坐标系从人身上变换到机器人身上，获得机器人手臂末端信息，并将最终获得的末端信息通过逆运动学的方式求解出机器人每个关节的旋转角度。

上位机控制***将经USB连接的信息采集***采集到的数据进行处理，得出机器人手臂各关节的控制信息；通过无线通信，将控制信息发送给下位机控制***；下位机控制***将处理后的控制信息发送到机械运动***，驱动机器人手臂各关节的电机运动。

机器人要做出不同的动作来完成任务，需要建立相应的数学模型来实现这一功能。其中有两套数学模型，一套实时模仿模型，另一套为末端精确控制模型。两套模型有不同的应用场合，且都具有不可替代的优点。通过分析关节式机器人机器臂的运动规律,采用反逆转方法建立数学模型,满足其运动要求,实现空间内定位，应用于实际情况。

上位机控制***是一台普通PC，它将Kinect采集到的数据进行滤波处理，同时利用骨骼节点信号建立相应的数学模型，通过坐标变换与逆运动学计算得出机器人手臂各关节的控制信息。上位机控制***通过Bluetooth与下位机控制***进行无线通信，将控制信息发送给下位机控制***。

下位机控制***使用STM32主控板。下位机控制***接收到上位机控制***发送过来的控制信息后，利用贝塞尔曲线对控制信息进行平滑处理。将平滑处理后的控制信息通过CAN总线发送到机械运动***，驱动机器人手臂各关节的电机运动。

机械运动***包含机械双臂与伺服电机及其控制器。图6为手臂关节示意图，每只机械臂具有四个自由度，包括肩部的两个自由度，肘部的两个自由度(腕部的一个自由度不使用)。如图8为由机械臂结构简化的自由度分布简图(省略末端执行器)。手臂使用的电机为瑞士MAXSON的RE系列伺服电机，控制器为MLDS3065C。双臂机器人具有灵活度更高，工作效率更高，协作工作可以完成更好。

仿人机器人每只手臂均有4个伺服电机，1个舵机。其中两个电机控制肩部关节的运动，两个电机控制肘部关节的运动，舵机控制手掌的旋转运动(如图6所示)。在仿人机器人手臂动作模仿***中，主要考虑肩部与肘部关节的运动。

实施例5

本实施例的机械臂末端精确运动控制方法，与前述各实施例不同的是：上位机控制***利用人体骨骼节点信号建立机械臂末端精确控制的数学模型的过程包括，建立关于机器人肩部关节和肘部关节的实时模仿模型，用于考虑肩部与肘部关节的运动的步骤。

实时模仿模型主要分为机器人肩部关节建模和机器人肘部关节建模。

机器人肩部关节建模

为便于电机的控制，本发明采用发送角度数据的方式来控制仿人机器人手臂的运动。本模型重点在于将人手臂动作时肩部关节的一个角度，分解成为控制仿人机器人肩关节的两个伺服电机分别需要旋转的角度，并得到相应的数学关系。

如图1、图2，建立绝对坐标和相对坐标。O点代表左肩关节(ShoulderLeft)，P点代表左臂肘部关节(ElbowLeft)，Q点为沿Y轴负方向，与O点距离为k的确定点，但实际上最终结果与k毫无关系，只是为了便于表示。其中，绝对坐标：O(x0，y0，z0)；P(x1，y1，z1)；Q(x0，y0-k，z0)；相对坐标：O(0，0，0)；P(x1-x0，y1-y0，z1-z0)；Q(0，-k，0)。

图3为肩部关节角度变换的示意图；图4为手臂运动时肘部关节坐标系的示意图。

机器人肘部关节建模

肘部关节同样有两个自由度，假定θ1、θ2、θ3、θ4均不为0(θ1、θ2、θ3、θ4分别对应机械臂上的四个电机转动角度，从上到下依次编号)。

O'、P、M分别表示人的左肩、肘部、手腕三个骨骼点，如图3所示，在肩部关节建立坐标系，则：O'P与Y'轴负方向夹角为θ；O'P在Y'O'Z'面上投影与Y'轴负方向夹角为θ1；O'P与其在Y'O'Z'面的投影间的夹角为θ2。

按图3箭头所示方向从左向右看，可得如图4所示的肘部关节坐标系。

空闲数据剔除。用户在手势动作输入开始或者结束阶段，可能会有短时间的静止状态，同时用户在连续动作过程中也有可能出现瞬间的停顿，这些在静止状态下的数据被称为空闲数据。空闲数据不包含任何运动信息，在手势动作识别过程中是无效的，甚至会影响手势动作的正常识别。因此需要剔除这些空闲数据。

其中：M'为手腕实际位置(此时θ3≠0)；M”为人为假设的手腕位置(此时θ3＝0)；当手腕在M”位置时，肘部关节点的坐标系为PX1Y1Z1。设初始状态时，肘部关节点的坐标系为P'X1'Y1'Z1'。

实施例6

本实施例的机械臂末端精确运动控制方法，与实施例5不同的是：步骤2)中，上位机控制***利用人体骨骼节点信号建立机械臂末端精确控制的数学模型的过程包括：通过坐标变化和逆运动求解关节建立末端精确控制模型，利用机器人位置运动学知识进行坐标变换。

末端精确控制模型分为坐标变换和逆运动学求解关节角

图5为人体坐标系建立的示意图。在人的空间里建立一个坐标系ΣM。坐标原点O_M取FOOT_RIGHT和FOOT_LEFT的中点，Z_M轴指向正前方，Y_M轴指向正上方，由右手坐标系法则确定X_M轴。将Kinect坐标系变换到ΣM坐标系。得到KINECT变换到人坐标系的旋转变换矩阵：

平移矩阵

得到齐次变换矩阵

人的空间坐标系ΣM建立起来了，接下来则是建立机器人坐标系，如图7，为机器人坐标系简图。机器人坐标系ΣR的坐标原点O_R取底盘中心点，Z_R轴指向正前方，Y_R轴指向正上方，由右手坐标系法则确定X_R轴。

建立了机器人坐标系后，目前是将人的坐标空间和机器人的坐标空间1：1映射，将WRIST_RIGHT在ΣM坐标系中的值复制到机器人坐标系ΣR。

在机器人的肩关节处还建立了一个坐标系Σ0，这个坐标系是用于逆运动学求解关节角度的坐标系，所以我们需要将ΣR坐标系变换到关节坐标系Σ0。

变换关系为绕Y_R轴旋转-90度，O_R平移至O₀得到齐次变换矩阵

则从Kinect坐标系变换到关节坐标系Σ0的齐次变换矩阵

设WRIST_RIGHT在Kinect坐标为

则WRIST_RIGHT在相对于坐标系Σ0的坐标

机器人正运动学问题主要是为了得到机械臂的各个关节与工具或末端执行器的位置和姿态之间的关系。在关节为转动关节时，对应的关节变量为转动角度；而对于平动或滑动关节，关节变量为连杆的伸缩距离。

实施例7

本实施例的机械臂末端精确运动控制方法，与实施例6的不同之处在于：为了简化正运动学的分析，在机器人应用中比较广泛使用的方法是DH建模，而DH建模是建立在一个Denavit-Hartenberg约定(DH约定)上的，这是一种参考坐标系配置方式的约定。Denavit和Hartenberg在其文献中介绍了几种不同的基本约定，其中之一就是本发明采用的由Khalil和Dombre提出的约定，该约定相比于其他方式，只需用四个参数就能确定一个坐标系相对于另一个坐标系的位置和姿态关系，并且下标配置更加合理。

物体与关节的编号约定如下：

机器人的N个连杆从1到N编号，基座编号为0；机器人的N个关节从1到N编号，关节i位于连杆i-1和i之间。经上述编号后，参考坐标系约定如下(N、i为自然数)：

Z_i-1轴配置在关节i的轴线上，并作为下一关节的驱动轴；

X_i轴垂直相交轴Z_i-1

采用上述坐标系后，四个参数的定义如下：

a_i是轴Z_i-1和轴Z_i之间公垂线距离；

α_i是轴Z_i-1和轴Z_i之间的空间夹角，当轴Z_i-1和轴Z_i与轴X_i满足右手法则时为正。

d_i是轴X_i-1和轴X_i之间公垂线距离；

θ_i是轴X_i-1和轴X_i之间的空间夹角，当轴X_i-1和轴X_i与轴Z_i-1满足右手法则时为正。

满足这些约定后，则每一个齐次变换矩阵A_i都可以表示为四个基本矩阵的乘积。

针对本发明的机械臂，对其进行DH建模，其坐标系布置如图4.其DH参数表见表1。

表1DH参数表：

1	α<sub>i</sub>	a<sub>i</sub>	d<sub>i</sub>	θ<sub>i</sub>
					2	-90	0	0	θ<sub>1</sub>
3	90	0	0	θ<sub>2</sub>
					4	90	0	L3	θ<sub>3</sub>
5	0	L4	0	θ<sub>4</sub>

A1＝[cos(θ1),0,-sin(θ1),0；sin(θ1),0,cos(θ1),0；0,-1,0,0；0,0,0,1]；

A2＝[cos(θ2),0,sin(θ2),0；sin(θ2),0,-cos(θ2),0；0,1,0,0；0,0,0,1]；

A3＝[cos(θ3),0,sin(θ3),0；sin(θ3),0,-cos(θ3),0；0,1,0,L3；0,0,0,1]；

A4＝[cos(θ4),-sin(θ4),0,L4*cos(θ4)；sin(θ4),cos(θ4),0,L4*sin(θ4)；

0,0,1,0；0,0,0,1]；

则坐标系Σ0到坐标系Σ4的齐次变换矩阵

实施例8

本实施例的机械臂末端精确运动控制方法，与实施例7的不同之处在于：逆运动学求解关节角的过程如下：

经过坐标变换后，我们获得了右手掌相对于肩部坐标系Σ0的坐标

现在开始建立机械臂的逆运动学模型。首先可以通过几何的方法解算肘部关节角度q4。如图6所示，可以从Korein在文献里证明过，相邻关节的转动轴分别垂直相交的三个关节可以等效为一个球关节。而我们的肩部则可等效为一个球形关节，设此球形关节的中心点为S，E点为肘关节点，W点为末端点。则由余弦定理得

其中α_E为手臂平面SEW与SW所在竖直平面(也是初始位置平面)的夹角，旋转α_E不会影响末端位置。

由

得到Rs的数值表示方式。如图6和图9所示，其中坐标系ΣE是q1q2q3均为0时建立的。ΣO为在连杆3固定的一个坐标系；前三个关节运动后ΣO变为ΣD.

Z_o＝X_o cross Y_o

Z_d＝X_d cross Y_d

其中E(α_E)＝SO_E+R(X_E*cosα_E+Y_E*sinα_E)

所以Rs＝[X_d；Y_d；Z_d]*[X_o，Y_o，Z_o]

如图4，ΣO变为ΣD的过程是绕Z3轴旋转q1，再绕新坐标系的X3轴旋转q2,最后绕新坐标系的Y3轴旋转q3。所以

其中Si代表sin(qi)；Ci代表cos(qi)。

联立方程组Rs(q1，q2，q3)＝Rs解出q1,q2,q3。分两种情况讨论。

当Rs(1，2)和Rs(2，2)不全为0时，则cos(q2)≠0.

当q2<π/2时，

q2＝atan2(Rs(3,2),sqrt(1-Rs(3,2)^2))

q1＝atan2(-Rs(1,2),Rs(2,2))

q3＝atan2(-Rs(3,1),Rs(3,3))

其中atan2为双参数反正切函数。

当q2>π/2时,

q2＝atan2(Rs(3,2),-sqrt(1-Rs(3,2)^2))

q1＝atan2(Rs(1,2),-Rs(2,2))

q3＝atan2(Rs(3,1),-Rs(3,3))

当Rs(1，2)和Rs(2，2)全为0时，则cos(q2)＝0。因为0≤q2≤π,则

q2＝π/2

q1+q3＝atan2(Rs(2,1),Rs(1,1))

令q1＝0，则q3＝atan2(Rs(2,1),Rs(1,1))

目前采用最简单的α_E规划法，采用人的手臂平面去规划机械臂的α_E，也就是将人手臂平面与竖直平面的夹角赋给α_E。接下来对求解方法进行详细描述。

如图10所示,当Elbow点在由Y0向量和SH向量确定的竖直平面时，α_E＝0，定义此竖直平面的法向量为

而实际人的手臂平面不一定是在竖直平面，设实际肘部在E’点，定义此时平面的法向量为

则

与

向量的夹角即为α_E。

则

通过坐标变换将Kinect采集到的人右手腕在空间中的运动轨迹变换到机器人坐标系上，利用本发明研究的数学模型计算的关节角度经过正运动学计算出的机器人运动空间轨迹，人的三维运动轨迹与机器人的三维运动轨迹如图11，可以看出，机器人的手臂末端可以很好的跟踪人的运动轨迹，验证了本运动控制模型的正确性。

图12是机器人手臂末端跟踪人的末端在XYZ三个方向上的跟踪轨迹，可以看出机器人的跟踪精度为-5mm～5mm，说明了本运动控制模型的高精度。

以上实施例仅用以说明本发明的具体实施方式。本领域的普通技术人员应当理解，对本发明的技术方案进行无实质改进的修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (5)

1.一种机械臂末端精确运动控制方法，借助信息采集***、上位机控制***、下位机控制***通过机械运动***实现机械臂末端运动的精确控制，其特征在于：实现步骤如下：

1)通过信息采集***采集人体骨骼节点信号，在深度摄像头上建立空间坐标系，并在此空间中获取操作者手臂各关节的准确的运动数据，并将信号转换为操作者手臂各关节连续平滑的运动轨迹曲线数据；

2)上位机控制***后利用人体骨骼数据建立机械臂末端精确控制的数学模型，利用模型将人体骨骼数据转换为机器人手臂相应伺服电机的转动角度信息，对待识别的手势动作的二轴陀螺数据序列进行预处理，以去除所述二轴陀螺数据序列中影响手势动作识别的冗余数据，通过坐标变换与逆运动学计算得出机器人手臂各关节的控制信息，并将控制信息发送给下位机控制***；

3)下位机控制***接收到上位机控制***发送过来的控制信息后，利用贝塞尔曲线对控制信息进行平滑处理，并将平滑处理后的控制信息发送到机械运动***，驱动机器人手臂各关节的电机运动，最终实现末端的精确控制；

上位机控制***将信息采集***采集到的数据进行滤波处理，获取准确的运动数据，同时利用骨骼节点信号建立机械臂末端精确控制的数学模型，经过坐标变换将坐标系变换到操作者身上，获得操作者手臂相对于操作者自身的运动参数；最后将坐标系从人身上变换到机器人身上，获得机器人手臂末端信息，并将最终获得的末端信息通过逆运动学的方式求解出机器人每个关节的旋转角度；

建立关于机器人肩部关节和肘部关节的实时模仿模型，采用发送角度数据的方式来控制仿人机器人手臂的运动，为便于电机的控制，将人手臂动作时肩部关节的一个角度，分解成为控制仿人机器人肩关节的两个伺服电机分别需要旋转的角度，并得到相应的数学关系：

采用O点代表左肩关节，P点代表左臂肘部关节，Q点为沿Y轴负方向，与O点距离为k的确定点，建立绝对坐标和相对坐标：

机器人肘部关节建模：

肘部关节同样有两个自由度，假定机械臂上的四个电机转动角度θ1、θ2、θ3、θ4均不为0，O'、P、M分别表示人的左肩、肘部、手腕三个骨骼点，在肩部关节建立坐标系，则：O'P与Y'轴负方向夹角为θ；O'P在Y'O'Z'面上投影与Y'轴负方向夹角为θ1；O'P与其在Y'O'Z'面的投影间的夹角为θ2，从左向右看，可得肘部关节坐标系；

空闲数据剔除：用户在手势动作输入开始或者结束阶段，可能会有短时间的静止状态，同时用户在连续动作过程中也有可能出现瞬间的停顿，这些在静止状态下的数据被称为空闲数据；空闲数据不包含任何运动信息，在手势动作识别过程中是无效的，甚至会影响手势动作的正常识别，因此需要剔除这些空闲数据：

肘部关节坐标系中：M'为手腕实际位置，此时θ3≠0；M”为人为假设的手腕位置，此时θ3＝0；当手腕在M”位置时，肘部关节点的坐标系为PX1Y1Z1；设初始状态时，肘部关节点的坐标系为P'X1'Y1'Z1'；

末端精确控制模型分为坐标变换和逆运动学求解关节角：

在人的空间里建立一个坐标系ΣM：坐标原点O_M取FOOT_RIGHT和FOOT_LEFT的中点，Z_M轴指向正前方，Y_M轴指向正上方，由右手坐标系法则确定X_M轴；

将Kinect坐标系变换到ΣM坐标系，得到KINECT变换到人坐标系的旋转变换矩阵：

平移矩阵

得到齐次变换矩阵

人的空间坐标系ΣM建立起来后，接下来建立机器人坐标系：

机器人坐标系ΣR的坐标原点O_R取底盘中心点，Z_R轴指向正前方，Y_R轴指向正上方，由右手坐标系法则确定X_R轴；

建立了机器人坐标系后，将人的坐标空间和机器人的坐标空间1：1映射，将WRIST_RIGHT在ΣM坐标系中的值复制到机器人坐标系ΣR；

在机器人的肩关节处建立一个坐标系Σ0，这个坐标系是用于逆运动学求解关节角度的坐标系，所以我们需要将ΣR坐标系变换到关节坐标系Σ0；

变换关系为绕Y_R轴旋转-90度，O_R平移至O₀得到齐次变换矩阵

则从Kinect坐标系变换到关节坐标系Σ0的齐次变换矩阵

设WRIST_RIGHT在Kinect坐标为

则WRIST_RIGHT在相对于坐标系Σ0的坐标

得到机械臂的各个关节与工具或末端执行器的位置和姿态之间的关系；

在关节为转动关节时，对应的关节变量为转动角度；而对于平动或滑动关节，关节变量为连杆的伸缩距离。

2.根据权利要求1所述的机械臂末端精确运动控制方法，其特征在于：信号采集***使用微软的Kinect体感摄像头，采集人体骨骼节点信号，用于后续的数学建模；空间坐标系包括绝对坐标和相对坐标；其中绝对坐标是指人手臂骨骼点在Kinect CameraSpacePoint坐标系中的坐标；相对坐标是指以O点为原点建立的坐标系。

3.根据权利要求2所述的机械臂末端精确运动控制方法，其特征在于：步骤2)中，上位机控制***利用人体骨骼节点信号建立机械臂末端精确控制的数学模型的过程包括：

步骤(1)，建立关于机器人肩部关节和机器人肘部关节的实时模仿模型，用于考虑肩部与肘部关节的运动；

步骤(2)，通过坐标变化和逆运动求解关节建立末端精确控制模型，利用机器人位置运动学知识进行坐标变换；

步骤(3)，建立机器人坐标系进行整体坐标预览。

4.根据权利要求1所述的机械臂末端精确运动控制方法，其特征在于：为了简化正运动学的分析，采用DH建模约定参考坐标系配置方式，采用由Khalil和Dombre提出的约定，只需用四个参数就能确定一个坐标系相对于另一个坐标系的位置和姿态关系，并且下标配置更加合理：

物体与关节的编号约定如下：

机器人的N个连杆从1到N编号，基座编号为0；

机器人的N个关节从1到N编号，关节i位于连杆i-1和i之间；

经上述编号后，参考坐标系约定如下：

Z_i-1轴配置在关节i的轴线上，并作为下一关节的驱动轴；

X_i轴垂直相交轴Z_i-1

采用上述坐标系后，四个参数的定义如下：

a_i是轴Z_i-1和轴Z_i之间公垂线距离；

α_i是轴Z_i-1和轴Z_i之间的空间夹角，当轴Z_i-1和轴Z_i与轴X_i满足右手法则时为正；

d_i是轴X_i-1和轴X_i之间公垂线距离；

θ_i是轴X_i-1和轴X_i之间的空间夹角，当轴X_i-1和轴X_i与轴Z_i-1满足右手法则时为正；

满足这些约定后，则每一个齐次变换矩阵A_i都可以表示为四个基本矩阵的乘积：

5.根据权利要求1或4所述的机械臂末端精确运动控制方法，其特征在于：逆运动学求解关节角的过程如下：

经过坐标变换后获得右手掌相对于肩部坐标系∑O的坐标

建立机械臂的逆运动学模型：首先通过几何的方法解算肘部关节角度q4，将肩部等效为一个球形关节，设此球形关节的中心点为S，E点为肘关节点，W点为末端点；则由余弦定理得

其中α_E为手臂平面SEW与SW所在竖直平面/初始位置平面的夹角，旋转α_E不会影响末端位置；

由

得到Rs的数值表示方式，其中坐标系∑E是q1q2q3均为0时建立的；∑O为在连杆3固定的一个坐标系；前三个关节运动后∑O变为∑D；

Z_o＝X_ocrossY_o

Y_d＝E(α_E)，

Z_d＝X_dcrossY_d

其中E(α_E)＝SO_E+R(X_E*cosα_E+Y_E*sinα_E)

所以Rs＝[X_d；Y_d；Z_d]*[X_o，Y_o，Z_o]

∑O变为∑D的过程是绕Z3轴旋转q1，再绕新坐标系的X3轴旋转q2，最后绕新坐标系的Y3轴旋转q3；所以

其中Si代表sin(qi)；Ci代表cos(qi)；

联立方程组Rs(q1，q2，q3)＝Rs解出q1，q2，q3.分两种情况讨论：

当Rs(1，2)和Rs(2，2)不全为0时，则cos(q2)≠0，

sin(q2)＝Rs(3，2)，

当q2<π/2时，

q2＝atan2(Rs(3，2)，sqrt(1-Rs(3，2)^2))

q1＝atan2(-Rs(1，2)，Rs(2，2))

q3＝atan2(-Rs(3，1)，Rs(3，3))

其中atan2为双参数反正切函数；

当q2＞π/2时，

q2＝atan2(Rs(3，2)，-sqrt(1-Rs(3，2)^2))

q1＝atan2(Rs(1，2)，-Rs(2，2))

q3＝atan2(Rs(3，1)，-Rs(3，3))

当Rs(1，2)和Rs(2，2)全为0时，则cos(q2)＝0.因为0≤q2≤π，则

q2＝π/2

q1+q3＝atan2(Rs(2，1)，Rs(1，1))

令q1＝0，则q3＝atan2(Rs(2，1)，Rs(1，1))

采用最简单的α_E规划法，采用人的手臂平面去规划机械臂的α_E，也就是将人手臂平面与竖直平面的夹角赋给α_E；求解方法如下：

当Elbow点在由Y0向量和SH向量确定的竖直平面时，α_E＝0，定义此竖直平面的法向量为

而实际人的手臂平面不一定是在竖直平面，设实际肘部在E’点，定义此时平面的法向量为

则

与

向量的夹角即为α_E；

则

Images