CN113386128B - 一种面向多自由度机器人的体势交互方法 - Google Patents

一种面向多自由度机器人的体势交互方法 Download PDF

Info

Publication number
CN113386128B
CN113386128B CN202110512320.5A CN202110512320A CN113386128B CN 113386128 B CN113386128 B CN 113386128B CN 202110512320 A CN202110512320 A CN 202110512320A CN 113386128 B CN113386128 B CN 113386128B
Authority
CN
China
Prior art keywords
coordinate system
robot
coordinates
coordinate
space
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110512320.5A
Other languages
English (en)
Other versions
CN113386128A (zh
Inventor
张平
陈佳新
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202110512320.5A priority Critical patent/CN113386128B/zh
Publication of CN113386128A publication Critical patent/CN113386128A/zh
Application granted granted Critical
Publication of CN113386128B publication Critical patent/CN113386128B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • B25J11/0005—Manipulators having means for high-level communication with users, e.g. speech generator, face recognition means

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Health & Medical Sciences (AREA)
  • Audiology, Speech & Language Pathology (AREA)
  • General Health & Medical Sciences (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)

Abstract

本发明提供一种面向多自由度机器人的体势交互方法，包括：采用人体骨骼关键点识别算法得到人体骨骼关键点像素坐标，根据到人体骨骼关键点的像素坐标得到各人体骨骼关键点的三维空间坐标；检测交互过程中过程是否存在肩部被手臂遮挡的异常错误；将人体空间姿态进行矫正，并对关键点空间坐标进行坐标重建；将手臂上腕部相对于肩部的坐标进行归一化，得到归一化后的坐标，同时在手掌上建立局部空间坐标系，得到手掌上局部坐标系相对于肩部为原点的坐标系的姿态角Eler(ψ,θ,γ)；结合归一化后的坐标、机器人连杆长度和手掌姿态，得到机器人各关节的关节角，以驱动机器人运动。可以使得交互过程中在人不超出传感器有效视野的条件下覆盖整个机械臂的工作空间。

Description

一种面向多自由度机器人的体势交互方法
技术领域
本发明属于人机交互领域，具体涉及一种面向多自由度机器人的体势交互方法。
背景技术
随着世界许多国家将工业4.0发展计划持续深入推进，工业生产对机器人智能化要求越来越高，自然高效的先进的人与机器人交互接口得到社会的广泛重视。
人机交互是利用设备采集人的信息，将人的意图传递给机器的过程；人机交互接口是将人的意图转变为机器能够执行指令的算法或者程序。根据交互方式的不同，人机交互分别有通过语音交互、通过佩戴传感器交互、通过游戏手柄交互、通过指挥棒交互、通过脑电波交互、以及通过视觉交互方式。从交互过程的自然性和交互***设计的复杂程度比较，基于体势的交互过程既能够有效避免环境噪音的干扰，也能减少人被佩戴的传感器所带来的束缚。
传统的基于人机交互过程，基于特征定义的交互语义的数量始终是有限的，难以满足复杂交互过程中的多样性需求；利用动态体势交互时，虽然通过追踪人体骨骼关键点的三维运动轨迹可以实现复杂的交互需求，但是传感器有效视野的限制和不同结构多自由度机器人的尺寸差异限制了这种交互方式的使用，不得不在人超过传感器视野有效视野打断交互过程并进行恢复，这个过程一方面束缚了人的活动范围，另外一方面增加了交互过程失败的概率。虽然由研究人员利用多个传感器数据来扩大单一传感器视野，但是这在增加成本的同时也增加了***复杂性。
发明内容
为了现有技术中存在的问题，本发明提供一种基于深度传感器和人体骨骼关键点识别算法的体势交互方法。
为了实现本发明目的，本发明提供的一种面向多自由度机器人的体势交互方法，包括以下步骤：
采用人体骨骼关键点识别算法得到人体骨骼关键点像素坐标，根据到人体骨骼关键点的像素坐标得到各人体骨骼关键点的三维空间坐标；
检测交互过程中过程是否存在肩部被手臂遮挡的异常错误，若存在，则进行恢复或将肩部的关键点标记为无效点；
将人体空间姿态进行矫正，并对关键点空间坐标进行坐标重建，所述坐标重建为，建立以左肩关键点为坐标原点的空间直角坐标系O·x'y'z'，其他骨骼关键点pi均以该坐标系为参考系进行坐标重建得到pi'；
将手臂上腕部相对于肩部的坐标进行归一化，得到归一化后的坐标Np7，同时在手掌上建立局部空间坐标系，得到手掌上局部坐标系相对于肩部为原点的坐标系的姿态角Eler(ψ,θ,γ)，Eler(ψ,θ,γ)表示手掌的空间姿态；
结合归一化后的坐标、机器人连杆长度和手掌空间姿态，得到机器人各关节的关节角，以驱动机器人运动。
进一步地，所述人体骨骼关键点识别算法为Open Pose。
进一步地，所述根据到人体骨骼关键点的像素坐标得到各人体骨骼关键点的三维空间坐标，包括：
利用预设窗口大小进行滤波得到骨骼关键点像素坐标的有效值；
将采集到的同一时刻的深度图和RGB视频帧进行像素级别对齐，得到每个像素点以相机为空间坐标系原点对应的三维坐标。
进一步地，所述检测交互过程中过程是否存在左肩部被左手臂遮挡的异常错误，若存在，则进行恢复或将肩部的关键点标记为无效点，包括：
计算左手小臂p6 p7的方向向量
Figure GDA0003198953650000021
P7指向p5的向量
Figure GDA0003198953650000022
P6指向p5的向量
Figure GDA0003198953650000023
计算p2p4在p2p3上的投影平方值
Figure GDA0003198953650000024
其中，p3是右手小臂和大臂交接处的骨骼关键点，p4为右手小臂与右手手腕交接处的骨骼关键点；
通过计算p2与直线p3 p4的空间距离探测p2是否被遮挡
Figure GDA0003198953650000031
其中，xa、ya、za、xb、yb、zb分别表示向量
Figure GDA0003198953650000032
的x，y，z分量。
仅仅通过p5与直线p6 p7的空间距离不足以判断是否真的发生遮挡，必须增加p5在垂直于
Figure GDA0003198953650000033
并且分别经过p6、p7的平面之间的约束条件：
将
Figure GDA0003198953650000034
代入经过以
Figure GDA0003198953650000035
为法向量且经过p6的空间平面方程：
Figure GDA0003198953650000036
xn、yn、zn表示向量
Figure GDA0003198953650000037
的x，y，z分量；
将
Figure GDA0003198953650000038
代入经过以
Figure GDA0003198953650000039
为法向量且经过p7的空间平面方程：
Figure GDA00031989536500000310
取s1，s2的符号，如果符号为负号，说明左肩关键点p5位于两平面之间，则
s＝s1·s2
当
Figure GDA00031989536500000311
成立，左肩关键点p5发生遮挡。
进一步地，所述将人体空间姿态进行矫正中，以两肩之间的空间向量平行于相机坐标系O·xyz的x轴的方式来对人体姿态进行矫正。
进一步地，所述建立以左肩关键点为坐标原点的空间直角坐标系O·x'y'z'，其他骨骼关键点pi均以该坐标系为参考系进行坐标重建得到pi'，包括：
以左肩为原点，
Figure GDA00031989536500000312
为x'轴，在平行于传感坐标系的o·xz平面上垂直于
Figure GDA00031989536500000313
且指向传感器方向为y'轴，与传感器y轴相反方向作为z'轴的重建后的坐标系O·x'y'z'；
p2指向p5的空间向量v
v＝p2-p5＝[x y z]T (7)
v与yoz平面的夹角
Figure GDA0003198953650000041
θx对应的旋转矩阵
Figure GDA0003198953650000042
v通过R(θx)旋转变换后与yoz平面平行的空间向量
v'＝R(θx)×v (10)
空间向量v'与xoy平面的夹角
Figure GDA0003198953650000043
θz对应的旋转矩阵
Figure GDA0003198953650000044
v'通过R(θx)旋转变换后与yoz平面平行的空间向量
v”＝R(θz)×v' (13)
经过旋转变换后，p2新的空间坐标
p2'＝p5+v” (14)
总的旋转变换
R＝R(θz)×R(θx) (15)
对于骨骼关键点pi，其重建后的坐标pi'
pi'＝p5+vi'
其中，vi'＝R×vi
vi＝pi-p5
式中，R是从相机坐标系到肩部为原点的坐标系的旋转矩阵。
进一步地，所述将手臂上腕部相对于肩部的坐标进行归一化，得到坐标Np7，包括：
分别求取大臂p5'p6'长度dist1，小臂p6'p7'长度dist2以及手掌到肩部p5'p7'的距离dist3，计算式如下：
Figure GDA0003198953650000051
Figure GDA0003198953650000052
Np7是归一化后的手部在左肩为原点的坐标系O·x'y'z'空间单位球(球上坐标内积为1)中的坐标，sacle为自适应缩放因子。
进一步地，所述在手掌上建立局部空间坐标系，得到手掌上局部坐标系相对于肩部为原点的坐标系的姿态角Eler(ψ,θ,γ)表示的姿态，包括：
以手掌上的关键点p30'指向p32'的向量
Figure GDA0003198953650000053
作为局部坐标系的O·x轴，
Figure GDA0003198953650000054
与p31'指向p33'的向量
Figure GDA0003198953650000055
作为局部坐标系的O·xy平面，过p31'的向量
Figure GDA0003198953650000056
且
Figure GDA0003198953650000057
且
Figure GDA00031989536500000517
以
Figure GDA0003198953650000058
作为O·z轴，则有：
Figure GDA0003198953650000059
x，y，z为每个向量的坐标分量；
求解O.xz平面的法向量
Figure GDA00031989536500000510
并且
Figure GDA00031989536500000511
将向量
Figure GDA00031989536500000512
归一化：
Figure GDA00031989536500000513
r11、r21、r31为向量
Figure GDA00031989536500000514
归一化后的三个坐标分量，r12、r22、r32为向量
Figure GDA00031989536500000515
归一化后的三个坐标分量，r13、r23、r33为向量
Figure GDA00031989536500000516
归一化后的三个坐标分量；
Rh是Oh·x'y'z'在O·x'y'z'的旋转矩阵，他的姿态角Eler(ψ,θ,γ)通过下式计算：
Figure GDA0003198953650000061
ψ表示将坐标系绕x轴旋转的角度，θ表示坐标轴绕y轴旋转的角度，γ表示将坐标轴绕z轴旋转的角度。atan2为反三角函数，计算得到正切角。
进一步地，在结合归一化后的坐标、机器人连杆长度和手掌姿态，得到关节角前还包括对归一化后的坐标和手掌姿态角进行滤波操作。
进一步地，所述结合归一化后的坐标、机器人连杆长度和手掌姿态，得到机器人各关节的关节角，以驱动机器人运动，包括：
ROS逆运动学求解器根据手掌的姿态姿态角Eler(ψ,θ,γ)、机器人末端位置得到机器人各关节的关节角；其中，机器人末端位置Pe的计算公式如下：
Pe＝Np7·L
式中，Np7为归一化后手腕在肩部为原点的坐标系中的坐标，L为机器人连杆总长。Eler(ψ,θ,γ)作为机器人末端的姿态，Pe作为末端位置，输人机交互***中，人机交互系先通过ROS自带逆运动学求解器为目标位置姿态计算机器人各关节角，再通过网络套接字链接控制机器人运动。
与现有技术相比，本发明能够实现的有益效果至少如下：
(1)本发明的人机交互***利用人手臂与手掌同时控制多自由度机器人的位置和姿态。
(2)利用人手臂组成的空间三角形，形成手掌相对于肩部在最大工作空间中的唯一空间位置坐标，将此坐标归一化处理后映射到不同大小机械臂上，可以使得交互过程中在人不超出传感器有效视野的条件下覆盖整个机械臂的工作空间。
(3)相比于传统的跟踪动态手势难以确定缩放因子产生的问题，本发明的方法稳定性强，可自适应调节缩放因子，实用性广。
(4)将手掌在空间中的姿态映射到机械臂TCP的姿态，能够实现人的意图快速高效的传递给机器人。
(5)本发明事先对人体姿态进行矫正，以两肩之间的的连线作为参考，对人体姿态进行坐标系重建，矫正后的人体姿态无论如何朝向传感器，当人未脱离传感器的有效视野，人体自身构建的局部坐标系中各关键点相对位置不会变化，极大提高了人的舒适性。
(6)由于不用提前标定传感、，机器人、人之间的相对位置关系，提高了人机交互的效率。
(7)对于手臂的自遮挡问题，采用遮挡检测算法进行探测和恢复，保证了在复杂环境下也能够正常工作，***抗干扰强。
附图说明
图1为本发明的***结构示意图。
图2为本发明实施例提供的一种面向多自由度机器人的体势交互方法流程示意图。
图3为机器人运动空间与人手臂运动空间映射关系示意图。
图4为人体姿态预矫正示意图。
图5为骨骼关键点像素坐标滑动窗口均值滤波计算有效值示意图。
图6为遮挡检测与自动恢复示意图。
图7为利用体势控制机器人末端示意图。
图8为人体骨骼关键点和指关节关键点示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都是本发明保护的范围。
如图1，人机交互***界面包括体势识别、虚拟仿真、视频监控和控制面板。***基于开源的机器人操作***ROS开发，利用ROS的仿真功能和运动规划器Moveit实现对机器人的虚拟仿真、碰撞检测、运动规划和逆运动求解功能。
本发明提供的一种面向多自由度机器人的体势交互方法利用人手臂与手掌同时控制多自由度机器人的位置和姿态，如附图2，***工作时，利用深度相机采集人体视频，深度相机可以同时得到视频每一帧图像的彩色图像和它的深度图；将彩色图像输入人体骨骼关键点识别算法提取骨骼关键点，然后从深度图中获取每个关键点的深度信息，进行遮挡检测恢复后得到骨骼关键点的空间坐标；将得到的人体骨骼关键点以两肩连线作为参考进行坐标系重建；最后将手腕相对于肩部的空间坐标归一化后映射为机器人末端相对于基座的空间坐标，将手掌的姿态Eler(ψ,θ,γ)映射为机器人末端姿态，实现人机交互。此方法中，利用人手臂组成的空间三角形，形成手掌相对于肩部在最大工作空间中的唯一空间位置坐标，将此坐标归一化处理后映射到不同大小机械臂上，可以使得交互过程中在人不超出传感器有效视野的条件下覆盖整个机械臂的工作空间，如附图3，随着人手腕伸展，机器人也伸展，同时，在人手伸展到极限，机器人末端也接近了它的工作空间边缘。同时，将手掌在空间中的姿态映射到机械臂末端的姿态，能够实现人的意图快速高效的传递给机器人。为了解决人与传感器位置变化导致的手掌相对肩部坐标不固定问题，本发明采用一种姿态预矫正方法，以两肩之间的空间向量平行于相机坐标系O·xyz的x轴，如附图4，矫正后的人体姿态无论如何朝向传感器，当人未脱离传感器的有效视野，人体自身构建的局部坐标系中相对位置不会变化，极大提高了人的舒适性。考虑到手臂的自遮挡问题，采用遮挡检测算法进行探测，利用该关键点历史坐标和其他未遮挡位置坐标自动恢复，***抗干扰强。
具体的，本发明提供的一种面向多自由度机器人的体势交互方法，包括以下步骤：
步骤S1、采用人体骨骼关键点识别算法得到人体骨骼关键点像素坐标，根据人体骨骼关键点的像素坐标得到各人体骨骼关键点的三维空间坐标，所述人体骨骼关键点包括位于人体上的和位于手掌上的骨骼关键点。
步骤S1包括以下步骤：
步骤S11：采用图像采集传感器采集人体视频，得到视频每一帧图像的彩色图像和深度图像；
在本发明其中一个实施例中，图像采集传感器为深度相机。
步骤12：将彩色图像输入人体骨骼关键点识别算法提取得到骨骼关键点像素坐标；
在本发明其中一个实施例中，采用的人体骨骼关键点识别算法为Open Pose，当然在其他实施例中，也可采用其他用于识别骨骼关键点的算法。
在本发明其中一个实施例中，人体骨骼关键点识别算法采集到的骨骼关键点信息包括三个元素k＝[px py score]，px、py是每个视频帧中骨骼关键点识别对应的像素坐标，score是该关键点的可信度。
由于不同的环境光照，不同的图像采集传感器，以及不同的人机交互人员等差异性，识别出的关键点置信度将会不相同，因此传统的基于固定阈值的分割是否为有效识别信息的方法不再适用；另外，基于频率域的自动阈值分割算法虽然可以较好的对关节点有效性进行识别，但是这个方法处理过程比较复杂，运算量很大。本发明中，当人体骨骼关键点被识别出来后，他们的置信度比较接近，而识别错误的关键点一般与正确识别的差别很大，因此，采用一种对必须识别关键点的自适应阈值分割方法，该方法是在传统基于固定值的阈值分割方法的基础上，以必须识别出关键点为基准，取上下各预设的阈值空间作为有效性的阈值区间(经数据分析本实施例取上下各20％的阈值空间作为有效性的阈值区间)。在本发明其中一个实施例中，采用左肩和右肩的骨骼关键点p5,p2作为必须识别出的关键点(后面步骤将以这两点的连线进行坐标系重建和姿态矫正)。计算过程如下：
标记每个关键点是否被有效识别：ValidMatrix＝[[false][false][false]...[false]]
选择必须识别出的骨骼关键点的置信度平均值作为参考置信度，在本发明其中一个实施例中，选择左肩和右肩的关键点p5,p2点的置信度平均值作为参考置信度s：
Figure GDA0003198953650000091
其中，scorei表示关键点ki的置信度，i为关键点的索引号；
Figure GDA0003198953650000092
判断每个关键点是否被有效识别，若ValidMatrix[i]为false,去除该无效点。
在本发明其中一个实施例中，通过深度相机采集人体彩色图像，通过人体骨骼关键点识别算法识别出人体25个骨骼关键点和右手20个指关节关键点，如图8所示，各关键点的序号和具***置表1和表2所示。在人体骨骼关键点中选择25个关节中的左手臂上的骨骼关键点p6，p7，左肩上的骨骼关键点p5，脖子上的骨骼关键点p1，右肩上的骨骼关键点p2，右手臂上的骨骼关键点p3，p4，作为最重要的关键点，这些关键点是后续步骤的基础；其中，在本发明其中一个实施例中，人体骨骼识别算法得到关键点均位于骨骼的关节处，如果考虑到肢体的宽度，它位于中心位置，这些都由人体骨骼关键点识别算法本身决定。
表1人体骨骼关键点序号和位置
Figure GDA0003198953650000093
Figure GDA0003198953650000101
表2手掌骨骼关键点序号和位置
Figure GDA0003198953650000102
Figure GDA0003198953650000111
步骤S13：利用固定大小窗口进行移动均值滤波得到骨骼关键点像素坐标x，y的有效值(有效值指像素坐标滤除骨骼关键点识别算法噪音之后的值)。
在本发明其中一个实施例中，人体静止时人体骨骼关键点识别算法的噪音是近似以30个视频帧作为周期性的上下波动的，因此采用大小为30的滑动窗口进行滑动均值滤波，如图5所示。滤波的具体过程如下：
步骤S131：为每个骨骼关键点配置预设大小的窗口window，本实施例中预设窗口的预设大小为30。
Figure GDA0003198953650000112
Figure GDA0003198953650000113
表示关键点ki的第j个输入值，取
Figure GDA0003198953650000114
中的px，py计算像素点坐标；
步骤S132：为所有骨骼关键点配置滑动滤波窗口WINDOW；
WINDOW＝[window0 ... windowi ...]
windowi为关键点ki的滑动滤波窗口；
步骤S133：如果采集的图像帧数不足30，则
Figure GDA0003198953650000115
sumi为第i个滤波窗口内元素的和，i表示这是为第i个骨骼关键点配置的滤波窗口。如果采集的图像帧数大于30，将原来的和
Figure GDA0003198953650000116
加上新输入数据
Figure GDA0003198953650000117
再减去最先输入的数
Figure GDA0003198953650000118
即更新过程为
Figure GDA0003198953650000121
在滑动均值滤波过程中，采用将当前窗口中总和加上待***数据减去窗口中最早***元素的方法，可以有效降低滤波过程反复求和的弊端，利用固定大小的循环队列实现。
步骤S134：计算得到关键点pi滑动窗口中所有数的平均值：
Figure GDA0003198953650000122
根据分析实际实验的数据和周期函数的特点，周期性上下波动噪音在一个完整周期内函数值和为零，因为sum是累计了30个帧的和，通过求平均得到单帧的结果。本步骤是为了降低人体骨骼关键点识别算法本身的噪音。
步骤S14：将人体骨骼关键点坐标由二维像素坐标转变为三维空间坐标。
在本发明其中一个实施例中，将图像采集传感器采集到的同一时刻的深度图和RGB视频帧进行像素级别对齐，得到每个像素点以相机为空间坐标系原点对应的三维坐标p(x,y,z)。将二维的像素坐标通过深度相机的深度图转换为三维坐标：pi＝remap[pxi pyi]，remap是深度相机开发套件(SDK)提供的开发接口(API)，功能是将像素坐标转换为以相机为原点的空间坐标。
每个骨骼关键点的空间坐标为：p＝[x y z]
步骤S2、检测交互过程肩部被手臂遮挡的异常错误，并自动恢复或标记为无效点，如图6所示。
本步骤中，首先利用遮挡探测算法检测左肩关键点p5是否被遮挡，检测到被遮挡后，则利用右肩关键点p2的深度信息进行恢复；如果右肩关键点p2也被遮挡，则选择左肩关键点的历史坐标
Figure GDA0003198953650000123
作为当前时刻的坐标值进行恢复，如果穷尽上述两种方法还不能恢复右肩关键点p5深度信息，则将左肩关键点p5标记为无效点，放弃本帧图像采集的数据。所述遮挡探测算法的过程如下：
计算左手小臂p6 p7的方向向量
Figure GDA0003198953650000124
P7指向p5的向量
Figure GDA0003198953650000125
P6指向p5的向量
Figure GDA0003198953650000126
计算p2p4(两个关键点之间的连线)在p2p3上的投影平方值
Figure GDA0003198953650000131
通过计算p2与直线p3 p4的空间距离探测p2是否被遮挡
Figure GDA0003198953650000132
其中，xa、ya、za、xb、yb、zb分别表示向量
Figure GDA0003198953650000133
的x，y，z分量。
仅仅通过p5与直线p6 p7的空间距离不足以判断是否真的发生遮挡，必须增加p5在垂直于
Figure GDA0003198953650000134
并且分别经过p6、p7的平面之间的约束条件。
将
Figure GDA0003198953650000135
代入经过以
Figure GDA0003198953650000136
为法向量且经过p6的空间平面方程：
Figure GDA0003198953650000137
xn、yn、zn表示向量
Figure GDA0003198953650000138
的x，y，z分量；
将
Figure GDA0003198953650000139
代入经过以
Figure GDA00031989536500001310
为法向量且经过p7的空间平面方程：
Figure GDA00031989536500001311
取s1，s2的符号，如果符号为负号，说明左肩关键点p5位于两平面之间，则
s＝s1·s2
当
Figure GDA00031989536500001312
成立，左肩关键点p5发生遮挡。threshhold可以调节，取决于传感器噪音水平，它表示左手腕与左小臂所在空间直线的空间距离，另外，由于图像与真实世界的镜像关系，图像与真实世界是左右颠倒的。在本发明其中一个实施例中，threshhold取值50mm，当然，在其他实施例中，也可根据实际采用其他数值。
S3、将人体空间姿态进行矫正，并对关键点空间坐标进行坐标重建。
初步得到的人体骨骼关键点三维坐标系是以传感器作为原点，当人与传感器成不同朝向时，人体姿态在相机坐标系将不相同，如图4矫正前的示例。在本发明其中一个实施例中，利用预先矫正的方法，即将人体姿态以左肩p5(x,y,z)指向右肩p2(x,y,z)的连线通过旋转变换R，最终平行于相机坐标系的x坐标轴o-x，即以两肩之间的空间向量平行于相机坐标系O·xyz的x轴的方式来对人体姿态进行矫正。
人的手臂在改变姿态时，左右肩膀p2，p5相对位置不会改变，坐标系需要建立在稳定的参考物上。将从左肩指向其他各个骨骼关键点的三维坐标都进行相同的旋转变换，最终建立以左肩为原点，
Figure GDA0003198953650000141
为x'轴，在平行于传感器坐标系的o·xz平面上垂直于
Figure GDA0003198953650000142
且指向传感器方向为y'轴，与传感器y轴相反方向作为z'轴的重建后的坐标系O·x'y'z'，如附图7。过程如下：
p2指向p5的空间向量v
v＝p2-p5＝[x y z]T (7)
v与yoz平面的夹角
Figure GDA0003198953650000143
θx对应的旋转矩阵
Figure GDA0003198953650000144
v通过R(θx)旋转变换后与yoz平面平行的空间向量
v'＝R(θx)×v (10)
空间向量v'与xoy平面的夹角
Figure GDA0003198953650000145
θz对应的旋转矩阵
Figure GDA0003198953650000146
v'通过R(θx)旋转变换后与yoz平面平行的空间向量
v”＝R(θz)×v' (13)
经过旋转变换后，p2新的空间位置
p2'＝p5+v” (14)
总的旋转变换
R＝R(θz)×R(θx) (15)
(15)式的R是从相机坐标系到肩部为原点的坐标系的旋转矩阵。
对于骨骼关键点pi，其重建后的坐标pi'
pi'＝p5+vi'
其中，vi'＝R×vi
vi＝pi-p5
vi是骨骼关键点pi与左肩p5的矢量，vi'是通过旋转变换后的空间向量。
步骤S4、将手臂上手掌相对于肩部的坐标进行归一化，得到归一化后的坐标Np7，同时在手掌上建立局部空间坐标系，求解该局部坐标系在S3中建立的坐标系用欧拉角表示的姿态Euler(ψ,θ,γ)。
在本发明其中一个实施例中，步骤S4包括如下步骤：
步骤S41：分别求取大臂p5'p6'长度dist1，小臂p6'p7'长度dist2以及手掌到肩部p5'p7'的距离dist3，计算式如下：
Figure GDA0003198953650000151
x6'、y6'、z6'是p6'在重建后的空间坐标系O·x'y'z'的坐标分量，x5'、y5'、z5'是p5'在重建后的空间坐标系O·x'y'z'的坐标分量；
Figure GDA0003198953650000152
Np7是归一化后的手部在左肩为原点的坐标系O·x'y'z'空间单位球(球上坐标内积为1)中的坐标，sacle为自适应缩放因子，通过该因子可以方便的转换到其他坐标系下。
步骤S42：为求解手掌在O·x'y'z'坐标系中的姿态，在手掌上建立局部空间坐标系Oh·x'y'z'。以手掌上的关键点p30'指向p32'的向量
Figure GDA0003198953650000161
作为局部坐标系的O·x轴，
Figure GDA0003198953650000162
与p31'指向p33'的向量
Figure GDA0003198953650000163
作为局部坐标系的O·xy平面，过p31'的向量
Figure GDA0003198953650000164
且
Figure GDA0003198953650000165
且
Figure GDA0003198953650000166
以
Figure GDA0003198953650000167
作为O·z轴，则有：
Figure GDA0003198953650000168
xc、yc、zc是向量
Figure GDA0003198953650000169
的三个坐标分量，xd、yd、zd是向量
Figure GDA00031989536500001610
的三个坐标分量；
求解O.xz平面的法向量
Figure GDA00031989536500001611
并且
Figure GDA00031989536500001612
将向量
Figure GDA00031989536500001613
归一化：
Figure GDA00031989536500001614
r11、r21、r31为向量
Figure GDA00031989536500001615
归一化后的三个坐标分量，r12、r22、r32为向量
Figure GDA00031989536500001616
归一化后的三个坐标分量，r13、r23、r33为向量
Figure GDA00031989536500001617
归一化后的三个坐标分量；
Rh是Oh·x'y'z'在O·x'y'z'的旋转矩阵，姿态角Eler(ψ,θ,γ)即手掌的空间姿态通过下式计算：
Figure GDA00031989536500001618
式中，ψ表示将坐标系绕x轴旋转的角度、θ表示坐标轴绕y轴旋转的角度、γ表示将坐标轴绕z轴旋转的角度。atan2为反三角函数，计算得到正切角。
步骤S5、对步骤S4得到的
Figure GDA00031989536500001619
轨迹点进行滤波处理，降低噪音对手腕空间坐标的不利影响，减少传感器累计误差的影响；对S4得到的Eler(ψ,θ,γ)进行滤波处理，减少手部局部坐标系姿态的抖动。
步骤S6、将步骤S5经滤波处理后的
Figure GDA00031989536500001620
乘以与机器人连杆长度之和L，与手掌姿态组成一个空间位姿ps(x,y,z,ψ,θ,γ)。
用上述ps(x,y,z,ψ,θ,γ)，输入人机交互***中，人机交互系先通过ROS自带逆运动学求解器为目标位置姿态计算机器人各关节角，再通过网络套接字链接控制机器人运动。
通过机器人的动力学数据计算机器人连杆总长
Figure GDA0003198953650000171
lidx为机器人第idx根连杆的长度，dof为机器人自由度，idx表示机器人连杆的序号。
机器人末端位置
Pe＝Np7·L (22)
Np7为归一化后手腕在肩部为原点的坐标系中的坐标，代表了空间单位球中的一个坐标。
本发明的体势交互方式具有巨大的优点，人的每个肢体都可以表达丰富的语义，而且人的肢体之间蕴含了丰富的空间关系，人的手臂的运动过程与机器人手臂的运动过程具有非常类似的特点。本发明的人机交互***利用人手臂与手掌同时控制多自由度机器人的位置和姿态。利用人手臂组成的空间三角形，形成手掌相对于肩部在最大工作空间中的唯一空间位置坐标，将此坐标归一化处理后映射到不同大小机械臂上，可以使得交互过程中在人不超出传感器有效视野的条件下覆盖整个机械臂的工作空间。相比于传统的跟踪动态手势难以确定缩放因子产生的问题，本发明的方法稳定性强，可自适应调节缩放因子，实用性广。将手掌在空间中的姿态映射到机械臂TCP的姿态，能够实现人的意图快速高效的传递给机器人。本发明采用一种人体姿态预矫正方法，以两肩之间的的连线作为参考，对人体姿态进行坐标系重建，矫正后的人体姿态无论如何朝向传感器，当人未脱离传感器的有效视野，人体自身构建的局部坐标系中各关键点相对位置不会变化，极大提高了人的舒适性。由于不用提前标定传感、机器人、人之间的相对位置关系，提高了人机交互的效率。对于手臂的自遮挡问题，采用遮挡检测算法进行探测和恢复，保证了在复杂环境下也能够正常工作，***抗干扰强。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种面向多自由度机器人的体势交互方法，其特征在于，包括以下步骤：
采用人体骨骼关键点识别算法得到人体骨骼关键点像素坐标，根据到人体骨骼关键点的像素坐标得到各人体骨骼关键点的三维空间坐标；
检测交互过程中过程是否存在肩部被手臂遮挡的异常错误，若存在，则进行恢复或将肩部的关键点标记为无效点；
将人体空间姿态进行矫正，并对关键点空间坐标进行坐标重建，所述坐标重建为，建立以左肩关键点为坐标原点的空间直角坐标系O·x'y'z'，其他骨骼关键点pi均以该坐标系为参考系进行坐标重建得到pi'；
将手臂上腕部相对于肩部的坐标进行归一化，得到归一化后的坐标Np7，并在手掌上建立局部空间坐标系，得到手掌上局部坐标系相对于肩部为原点的坐标系的姿态角Eler(ψ，θ，γ)，姿态角Eler(ψ,θ,γ)表示手掌的空间姿态；
结合归一化后的坐标、机器人连杆长度和手掌空间姿态，得到机器人各关节的关节角，以驱动机器人运动。
2.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述人体骨骼关键点识别算法为Open Pose。
3.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述根据到人体骨骼关键点的像素坐标得到各人体骨骼关键点的三维空间坐标，包括：
利用预设窗口大小进行滤波得到骨骼关键点像素坐标的有效值；
将采集到的同一时刻的深度图和RGB视频帧进行像素级别对齐，得到每个像素点以相机为空间坐标系原点对应的三维坐标。
4.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述检测交互过程中过程是否存在左肩部被左手臂遮挡的异常错误，若存在，则进行恢复或将肩部的关键点标记为无效点，包括：
计算左手小臂p6 p7的方向向量
Figure RE-FDA0003154290950000011
P7指向p5的向量
Figure RE-FDA0003154290950000012
P6指向p5的向量
Figure RE-FDA0003154290950000021
计算p2p4在p2p3上的投影平方值
Figure RE-FDA0003154290950000022
通过计算p2与直线p3 p4的空间距离探测p2是否被遮挡
Figure RE-FDA0003154290950000023
其中，xa、ya、za、xb、yb、zb分别表示向量
Figure RE-FDA0003154290950000024
的x，y，z分量；
p5在垂直于
Figure RE-FDA0003154290950000025
并且分别经过p6、p7的平面之间的约束条件：
将
Figure RE-FDA0003154290950000026
代入经过以
Figure RE-FDA0003154290950000027
为法向量且经过p6的空间平面方程：
Figure RE-FDA0003154290950000028
xn、yn、zn表示向量
Figure RE-FDA0003154290950000029
的x，y，z分量；
将
Figure RE-FDA00031542909500000210
代入经过以
Figure RE-FDA00031542909500000211
为法向量且经过p7的空间平面方程：
Figure RE-FDA00031542909500000212
取s1，s2的符号，如果符号为负号，说明左肩关键点p5位于两平面之间，则
s＝s1·s2
当
Figure RE-FDA00031542909500000213
成立，左肩关键点p5发生遮挡，threshhold表示左手腕与左小臂所在空间直线的空间距离。
5.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述将人体空间姿态进行矫正中，以两肩之间的空间向量平行于相机坐标系O·xyz的x轴的方式来对人体姿态进行矫正。
6.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述建立以左肩关键点为坐标原点的空间直角坐标系O·x'y'z'，其他骨骼关键点pi均以该坐标系为参考系进行坐标重建得到pi'，包括：
以左肩为原点，
Figure FDA0003060761830000031
为x'轴，在平行于传感坐标系的o·xz平面上垂直于
Figure FDA0003060761830000032
且指向传感器方向为y'轴，与传感器y轴相反方向作为z'轴的重建后的坐标系O·x'y'z'；
p2指向p5的空间向量v
v＝p2-p5＝[x y z]T (7)
v与yoz平面的夹角
Figure FDA0003060761830000033
θx对应的旋转矩阵
Figure FDA0003060761830000034
v通过R(θx)旋转变换后与yoz平面平行的空间向量
v'＝R(θx)×v (10)
空间向量v'与xoy平面的夹角
Figure FDA0003060761830000035
θz对应的旋转矩阵
Figure FDA0003060761830000036
v'通过R(θx)旋转变换后与yoz平面平行的空间向量
v”＝R(θz)×v' (13)
经过旋转变换后，p2新的空间坐标
p2'＝p5+v” (14)
总的旋转变换
R＝R(θz)×R(θx) (15)
对于骨骼关键点pi，其重建后的坐标pi'
pi'＝p5+vi'
其中，vi'＝R×vi
vi＝pi-p5
式中，R是从相机坐标系到肩部为原点的坐标系的旋转矩阵。
7.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述将手臂上腕部相对于肩部的坐标进行归一化，得到坐标Np7，包括：
分别求取大臂p5'p6'长度dist1，小臂p6'p7'长度dist2以及手掌到肩部p5'p7'的距离dist3，计算式如下：
Figure FDA0003060761830000041
Figure FDA0003060761830000042
Np7是归一化后的手部在左肩为原点的坐标系O·x'y'z'空间单位球中的坐标，x6'、y6'、z6'是p6'在重建后的空间坐标系O·x'y'z'的坐标分量，x5'、y5'、z5'是p5'在重建后的空间坐标系O·x'y'z'的坐标分量，sacle为自适应缩放因子。
8.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述在手掌上建立局部空间坐标系，得到手掌上局部坐标系相对于肩部为原点的坐标系的姿态角Eler(ψ,θ,γ)表示的姿态，包括：
以手掌上的关键点p30'指向p32'的向量
Figure FDA0003060761830000043
作为局部坐标系的O·x轴，
Figure FDA0003060761830000044
与p31'指向p33'的向量
Figure FDA0003060761830000045
作为局部坐标系的O·xy平面，过p31'的向量
Figure FDA0003060761830000046
且
Figure FDA0003060761830000047
且
Figure FDA0003060761830000048
以
Figure FDA0003060761830000049
作为O·z轴，则有：
Figure FDA00030607618300000410
x，y，z为每个向量的坐标分量；
求解O.xz平面的法向量
Figure FDA00030607618300000411
并且
Figure FDA00030607618300000412
将向量
Figure FDA00030607618300000413
归一化：
Figure FDA0003060761830000051
Figure FDA0003060761830000052
r11、r21、r31为向量
Figure FDA0003060761830000053
归一化后的三个坐标分量，r12、r22、r32为向量
Figure FDA0003060761830000054
归一化后的三个坐标分量，r13、r23、r33为向量
Figure FDA0003060761830000055
归一化后的三个坐标分量；
Rh是Oh·x'y'z'在O·x'y'z'的旋转矩阵，其姿态角Eler(ψ,θ,γ)通过下式计算：
Figure FDA0003060761830000056
ψ表示将坐标系绕x轴旋转的角度，θ表示坐标轴绕y轴旋转的角度，γ表示将坐标轴绕z轴旋转的角度，atan2为反三角函数，计算得到正切角。
9.根据权利要求1所述的一种面向多自由度机器人的体势交互方法，其特征在于，在结合归一化后的坐标、机器人连杆长度和手掌姿态，得到机器人各关节的关节角前还包括对归一化后的坐标和手掌姿态角进行滤波操作。
10.根据权利要求1-9任一所述的一种面向多自由度机器人的体势交互方法，其特征在于，所述结合归一化后的坐标、机器人连杆长度和手掌姿态，得到机器人各关节的关节角，以驱动机器人运动，包括：
ROS逆运动学求解器根据手掌的姿态姿态角Eler(ψ,θ,γ)、机器人末端位置得到机器人各关节的关节角；其中，机器人末端位置Pe的计算公式如下：
Pe＝Np7·L
式中，Np7为归一化后手腕在肩部为原点的坐标系中的坐标，L为机器人连杆总长。
CN202110512320.5A 2021-05-11 2021-05-11 一种面向多自由度机器人的体势交互方法 Active CN113386128B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110512320.5A CN113386128B (zh) 2021-05-11 2021-05-11 一种面向多自由度机器人的体势交互方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110512320.5A CN113386128B (zh) 2021-05-11 2021-05-11 一种面向多自由度机器人的体势交互方法

Publications (2)

Publication Number Publication Date
CN113386128A CN113386128A (zh) 2021-09-14
CN113386128B true CN113386128B (zh) 2022-06-10

Family

ID=77616921

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110512320.5A Active CN113386128B (zh) 2021-05-11 2021-05-11 一种面向多自由度机器人的体势交互方法

Country Status (1)

Country Link
CN (1) CN113386128B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114327047B (zh) * 2021-12-01 2024-04-30 北京小米移动软件有限公司 设备控制方法、设备控制装置及存储介质
CN115331153B (zh) * 2022-10-12 2022-12-23 山东省第二人民医院(山东省耳鼻喉医院、山东省耳鼻喉研究所) 一种用于辅助前庭康复训练的姿态监测方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106313049A (zh) * 2016-10-08 2017-01-11 华中科技大学 一种仿人机械臂体感控制***及控制方法
CN107160364A (zh) * 2017-06-07 2017-09-15 华南理工大学 一种基于机器视觉的工业机器人示教***及方法
CN107363813A (zh) * 2017-08-17 2017-11-21 北京航空航天大学 一种基于可穿戴设备的桌面工业机器人示教***和方法
CN107953331A (zh) * 2017-10-17 2018-04-24 华南理工大学 一种应用于人形机器人动作模仿的人体姿态映射方法
CN110480634A (zh) * 2019-08-08 2019-11-22 北京科技大学 一种针对机械臂运动控制的手臂引导运动控制方法
CN111738092A (zh) * 2020-05-28 2020-10-02 华南理工大学 一种基于深度学习的恢复被遮挡人体姿态序列方法
CN112149455A (zh) * 2019-06-26 2020-12-29 北京京东尚科信息技术有限公司 一种检测人体姿势的方法和装置
JP2021068438A (ja) * 2019-10-21 2021-04-30 ダッソー システムズＤａｓｓａｕｌｔ Ｓｙｓｔｅｍｅｓ モデル化された身体の骨格に姿勢を取らせるコンピュータにより実行される方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106313049A (zh) * 2016-10-08 2017-01-11 华中科技大学 一种仿人机械臂体感控制***及控制方法
CN107160364A (zh) * 2017-06-07 2017-09-15 华南理工大学 一种基于机器视觉的工业机器人示教***及方法
CN107363813A (zh) * 2017-08-17 2017-11-21 北京航空航天大学 一种基于可穿戴设备的桌面工业机器人示教***和方法
CN107953331A (zh) * 2017-10-17 2018-04-24 华南理工大学 一种应用于人形机器人动作模仿的人体姿态映射方法
CN112149455A (zh) * 2019-06-26 2020-12-29 北京京东尚科信息技术有限公司 一种检测人体姿势的方法和装置
CN110480634A (zh) * 2019-08-08 2019-11-22 北京科技大学 一种针对机械臂运动控制的手臂引导运动控制方法
JP2021068438A (ja) * 2019-10-21 2021-04-30 ダッソー システムズＤａｓｓａｕｌｔ Ｓｙｓｔｅｍｅｓ モデル化された身体の骨格に姿勢を取らせるコンピュータにより実行される方法
CN111738092A (zh) * 2020-05-28 2020-10-02 华南理工大学 一种基于深度学习的恢复被遮挡人体姿态序列方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
李瑞.图像和深度图中的动作识别与手势姿态估计.《中国博士学位论文全文数据库 信息科技辑》.2019, *
王志红.基于视觉手势识别的机械手操控***的研究.《中国优秀硕士学位论文全文数据库 信息科技辑》.2017, *

Also Published As

Publication number Publication date
CN113386128A (zh) 2021-09-14

Similar Documents

Publication Publication Date Title
CN106909216B (zh) 一种基于Kinect传感器的仿人机械手控制方法
Lee et al. Model-based analysis of hand posture
CN113386128B (zh) 一种面向多自由度机器人的体势交互方法
Heap et al. Towards 3D hand tracking using a deformable model
CN108972494A (zh) 一种仿人机械手抓取控制***及其数据处理方法
Triesch et al. Robotic gesture recognition
Schröder et al. Real-time hand tracking using synergistic inverse kinematics
JP4765075B2 (ja) ステレオ画像を利用した物体の位置および姿勢認識システムならびに物体の位置および姿勢認識方法を実行するプログラム
Guo Research of hand positioning and gesture recognition based on binocular vision
CN109048918A (zh) 一种轮椅机械臂机器人的视觉引导方法
CN113505694A (zh) 一种基于视线追踪的人机交互方法、装置及计算机设备
CN115576426A (zh) 一种针对混合现实飞行模拟器的手部交互方法
CN108115671B (zh) 基于3d视觉传感器的双臂机器人控制方法及***
CN113070877B (zh) 一种面向七轴机械臂视觉示教的可变姿态映射方法
JP7171294B2 (ja) 情報処理装置、情報処理方法及びプログラム
CN114495273A (zh) 一种机器人手势遥操作方法及相关装置
CN108268125A (zh) 一种基于计算机视觉的运动手势检测及跟踪方法
Chaudhary et al. A vision-based method to find fingertips in a closed hand
Sun et al. Visual hand tracking on depth image using 2-D matched filter
CN109214295B (zh) 基于Kinect v2和Leap Motion的数据融合的手势识别方法
Ehlers et al. Self-scaling Kinematic Hand Skeleton for Real-time 3D Hand-finger Pose Estimation.
Triesch et al. Robotic gesture recognition by cue combination
Liang et al. Hand pose estimation by combining fingertip tracking and articulated ICP
Fujiki et al. Real-time 3D hand shape estimation based on inverse kinematics and physical constraints
Jaume-i-Capó et al. Adding image constraints to inverse kinematics for human motion capture

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant