CN107856014A - 基于手势识别的机械臂位姿控制方法 - Google Patents
基于手势识别的机械臂位姿控制方法 Download PDFInfo
- Publication number
- CN107856014A CN107856014A CN201711088709.1A CN201711088709A CN107856014A CN 107856014 A CN107856014 A CN 107856014A CN 201711088709 A CN201711088709 A CN 201711088709A CN 107856014 A CN107856014 A CN 107856014A
- Authority
- CN
- China
- Prior art keywords
- msub
- mrow
- mtd
- msubsup
- mtr
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000012545 processing Methods 0.000 claims abstract description 24
- 210000000707 wrist Anatomy 0.000 claims abstract description 20
- 238000004088 simulation Methods 0.000 claims abstract description 12
- 230000009471 action Effects 0.000 claims abstract description 10
- 238000001914 filtration Methods 0.000 claims abstract description 6
- 241000446313 Lamella Species 0.000 claims description 45
- 239000011159 matrix material Substances 0.000 claims description 17
- 210000000245 forearm Anatomy 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 5
- 239000012636 effector Substances 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000004804 winding Methods 0.000 claims description 2
- 210000003857 wrist joint Anatomy 0.000 claims description 2
- 238000012546 transfer Methods 0.000 abstract description 3
- 238000004891 communication Methods 0.000 description 14
- 238000004519 manufacturing process Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 241001269238 Data Species 0.000 description 1
- 241001062009 Indigofera Species 0.000 description 1
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000010420 art technique Methods 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 239000010437 gem Substances 0.000 description 1
- 229910001751 gemstone Inorganic materials 0.000 description 1
- 231100001261 hazardous Toxicity 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012806 monitoring device Methods 0.000 description 1
- 210000003205 muscle Anatomy 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J3/00—Manipulators of master-slave type, i.e. both controlling unit and controlled unit perform corresponding spatial movements
-
- 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
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- User Interface Of Digital Computer (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
Abstract
基于手势识别的机械臂位姿控制方法,使得可以通过智能腕带获取人臂的位姿,再将位姿通过正逆运动学,求解出工作机械臂每个关节角的角度;具体实施是智能腕带通过蓝牙无线连接PC端,将智能腕带模块采集的肌电信号传输给远程客户端;远程客户端接收到信号后,将信号转到数据处理模块,在数据处理模块中,对信号进行滤波降噪处理,处理后将手势分类;在数据处理模块中去除噪声之后再利用正逆运动学来求解关节角度;通过两个智能腕带获取操作人员的手臂的关节角度;之后就可以通过远程客户端将关节角度的信号以及操作指令信号传递给智能腕带模块;然后将手势动作的信号发送给仿真模块中的仿真机械臂;仿真臂发送信号给工作机械臂,机械臂执行命令。
Description
技术领域
本发明涉及一种基于手势识别的机械臂位姿控制方法,使得可以通过智能腕 带获取人臂的位姿,再将位姿通过正逆运动学,求解出工作机械臂每个关节角的 角度。
背景技术
随着《中国制造2025》的出台,我国制造业正式踏上了以智能制造为重要 发展方向的转型升级之路。机器人替代人工生产已经成为未来制造业重要的发展 趋势,工业机器人作为“制造业皇冠顶端的明珠”,将大力推动工业自动化、数 字化、智能化的早日实现,为智能制造奠定基础。
在工业机器人方面,中国对机械臂的需求一直以飞快的速度在增长,目前来 说,中国早已成为全球机械臂市场的一个不可或缺的组成部分。对工业机械臂的 任务要求也越来越复杂,为了提高工业机械臂的工作效率和工作范围,尤其是在 一些特殊的环境中,例如:工件加工、高温或者无菌环境等,远程控制工业机械 臂能够让操作人员处于安全的环境中,所以远程控制机械臂的作用也越来越大。 而且安排给机械臂的任务不仅越来越重,而且对机械臂的灵活性的要求也越来越 高,而且随着智能化时代的到来,要求人机交互更加自然,对操作者的要求反而 越来越低,希望能够直接通过手势来直接实现机械臂的控制,即将传感器技术对 外部信息进行实时采集和决策应用到机械臂的控制上。
目前关于手势识别的研究很多,但国内的研究和国外的相比,进展较慢。朱 茂娟、王林冰、曾奇等提出一种机器人及其示教通信系统(朱茂娟,王林冰,曾 奇等.机器人及其示教通信系统:中国,205620710[P].2016-01-04),实现了示教 采集器与控制器之间的数据通信,但是通信方式是有线的,具有空间局限性且缺 少保密性;张健、金喆提出一种非接触式手势控制机器人(张健、金喆.一种非 接触式手势控制机器人:中国,CN203973550U,2014-12-03),通过在机械手上 安装摄像头作为手势监测装置,并结合通讯装置,实现机械手的抓取,但是对摄 像头的要求高,并且安装不方便并且控制的精度和稳定性不高,不利于对实际机 械臂的控制使其存在很大的安全隐患。。
发明内容
本发明要克服现有技术的上述缺点,提出一种基于手势识别的机械臂位姿控 制方法。
首先该方法需要通过蓝牙无线通信技术,进行手势信号与机械臂之间的无线 通信,而且无线控制可以扩展工作空间;其次该系统采用两个智能腕带,通过它 们所采集到的数据得出人臂的姿态和手势从而将其转换成机械臂控制的指令,使 用智能腕带使采集到的数据更加稳定,计算结果更加准确,控制机械臂也更加准 确;最后该系统利用肌电信号和陀螺仪信号来识别手势,识别的精度更高,效果 更好。
本发明为解决现有技术问题所采用的方法是:
一种基于手势识别的机械臂位姿控制方法,使用两个智能腕带进行信号采集, 将采集到的信息进行处理,并且智能腕带通过蓝牙适配器与远程客户端进行无线 连接,远程客户端与工作机械臂通过TCP/IP协议连接;
所述的基于手势识别的机械臂位姿控制方法,其主要的特点是使用两个智能 腕带进行信号采集,采集的信号主要是陀螺仪信号;然后通过对信号的处理将信 号转换为人臂的关节角度;智能腕带的信号通过无线蓝牙传递给远程客户端;远 程客户端中的数据处理模块则将得到的数据信号先通过提出的机械臂位姿控制 方法,求出各个关节角的角度信息,再通过正运动学求解得到仿人臂的末端位置, 进一步优化之后,再通过逆运动学求解出工作机械臂的各个关节角度。
所述的基于手势识别的机械臂位姿控制系统的电脑客户端是基于Linux系 统进行开发的,当系统中的数据处理模块接收到来自远程客户端发送过来的陀螺 仪信号和肌电信号后,数据处理模块将接收到的信号与数据先通过滤波算法去除 信号中的噪声;然后结合正逆运动学求解,将接收到的陀螺仪信号,采用四元数 和坐标变换理论计算出腕带使用者手臂的关节角度,同时对于可疑的数据进行剔 除,将范围内的关节角度数据优化后传递给仿真模块下的机械臂,使其做出与人 体相同的动作;
具体步骤如下:
1)首先是通过两个腕带获取关节角,建立世界坐标系(xG,yG,zG)以及建 立人手臂的上臂坐标系(xH,yH,zH),前臂坐标系(xF,yF,zF);R代表旋转矩 阵;上标i代表初始位置;下标G、H、F分别代表世界坐标系框架,上臂框架 以及前臂框架;相对于第一个智能腕带的坐标系和相对于第二个智能腕带对应的 下标的坐标系可以表示为:
其中下标“U”表示佩戴在上臂上的第一个智能腕带臂章框架,下标“L” 表示带在下臂上的第二个智能腕带臂章框架;当操作者的手臂做出一个新动作之 后,前臂和上臂在世界坐标系的框架下可以用下面的旋转矩阵来描述:
2)上标f代表操作者手臂的一个新动作;通过第一个腕带的陀螺仪,可以 得到一个四元数q=[x,y,z,w]T,其中(x,y,z)是标量,w是矢量;
q=xi+yj+zk+w (1)
其中(x,y,z)是标量,表示笛卡尔坐标方向,w是矢量,i,j,k分别为各坐 标的参数值。
其中代表上臂相对于全局的坐标系,其中,r11表示旋转矩阵的第一行第一列的参数
通过(5)计算出欧拉角α,β,γ,分别代表肩部的三个角度,α代表偏航角,β 代表俯仰角,γ代表滚动角:
3)根据两个智能腕带测到的数据,可以计算出两个肘部关节的角度,Afe代 表肘部弯曲的角度,Aps代表肘部滚动的角度,计算公式如下:
Afe=arccos(a12r13+a22r23+a32r33) (4)
Aps=arccos(r11a11+r21a21+r31a33) (5)
4)肌电信号EMG的大小由IEMG信号的平均值AIEMG表示:
其中IEMGl(n)(l=1,2,...,8)是第l个肌电信号传感器的肌电数据;所以其 平均值为:
腕关节的角度则是假设与肌电信号EMG呈近似线性关系,腕关节角度计 算如下:
5)经过滤波处理以及角度判断之后将角度值α,β,γ,Afe,Aps,发送给仿 真模块中的仿人臂,通过正运动学的计算得出手臂的末端位置p(x,y,z);
2.根据权利要求1所述的基于手势识别的机械臂位姿控制方法,其特征在 于:通过两个智能腕带的传感器数据通过正运动学得到的手臂的末端位置,其中 所述的正运动学求解的算法如下:
2.1)首先对示教机械臂构建其D-H参数,(即关节1≤i≤6的连杆长度ai、 连杆转角αi、连杆偏距di和关节角θi)作为输入量,选择工作机械臂各个关节 的关节角θi作为输出量;
2.2)由于各杆件相对参考坐标系有转动和平移两个动作,因此对每个杆件 沿关节轴建立一个关节坐标系,用以下四个参数来描述杆件:
(S1)连杆长度ai:关节轴i和关节轴i+1之间公垂线的长度;
(S2)连杆转角αi:作一个与两关节轴之间的公垂线垂直的平面,将关节 轴i和关节轴i+1投影到该平面,在平面内轴i按右手法则绕ai转向轴i+1,其转 角即为连杆转角;
(S3)连杆偏距di:公垂线ai-1与关节轴i的交点到公垂线ai与关节轴i+1 的交点的有向距离长度;
(S4)关节角θi:ai-1的延长线与ai之间绕关节轴i旋转所形成的夹角,即 采集得到的示教机械臂关节角度;
2.3)以基座坐标系作为T0坐标系,设表示i关节坐标系相对i-1关节坐 标系的位置和姿态,则:
其中,sθi=sinθi,cθi=cosθi,sαi=sinαi,cαi=cosαi;
2.4)对于六自由度机械臂,获得机械臂末端执行器位姿相对固定参考坐标 系的变换矩阵(即机械臂末端位置点坐标的变换矩阵):
其中,表示六自由度机械臂末端执行器位姿相对于基轴参考坐标系的变换 矩阵,r11表示变换矩阵中第一行第一列的参数,px表示x轴的参数。
3.根据权利要求1所述的基于手势识别的机械臂位姿控制方法,其特征在 于:利用智能腕带获取到操作人员手臂的各个关节的角度,再通过正运动学获得 手臂的末端位置,然后再通过逆运动学求解出六自由度机械臂的六个关节角,则 可按顺序求解得到,具体的逆运动学的求解方法如下所示:
θ1表示第一个关节角度参数,求解方法为
由第一个关节角,通过中间运算公式可得第二个和第三个关节角为
其中,a2为连杆1的长度,d4为连杆4的偏距,c3是cosθ3的缩写,s3是sinθ3的缩写,K为逆运动学求解时的固定常量;
以此类推可以求出第四关节角θ4,并通过c5和s5求出θ5
θ4=Atan2(-r13s1+r23c1,-r13c1c23-r23s1c23+r33s23) (13)
其中,c23=c2c3-s2s3,s23=c2s3+s2c3。r13表示变换矩阵0 中第一行第三 列的参数值。
同理可以通过求解c6和s6,求出θ6角度:
其中,Atan2为反正切函数,si=sinθi,ci=cosθi;
在每个关节角度求出后,接着判断每个关节角度是不是在限定范围内,将各 关节角度发送到仿真模块中的仿真机械臂进行仿真;仿真效果调试好之后,将关 节角度和手势指令发送给实际机械臂,实现通过人体手势的动作来控制实际机械 臂动作。
在每个关节角度求出后,接着判断每个关节角度是不是在限定范围内,将各 关节角度发送到仿真模块中的仿真机械臂进行仿真;仿真效果调试好之后,将关 节角度和手势指令发送给实际机械臂,实现通过人体手势的动作来控制实际机械 臂动作。
本发明的优点和积极效果是:
本发明设计了PC端软件,使用蓝牙无线通信技术,相较于传统的示教器, 蓝牙无线通信可以避免有线的拘束,还可以应用于危险环境中;该系统采用肌电 信号和陀螺仪信号实现手势的识别,识别精度更高,识别的手势更多,控制机械 臂更准确;并对信号进行滤波处理,能够对信号进一步优化;数据处理模块对数 据进行正逆运动学求解使再次提高了手势识别的精度和效率;其扩展的智能腕带 握拳复位指令,可以实现控制机械臂无线复位,提高了机械臂复位的灵活性和实 用性;通过腕带姿态控制的手势识别方式实现了勉力矩传感器的机器人位置控制。
附图说明
图1a-图1c为本发明的平台组成示意图,其中图1a是机械臂位姿控制系统 的上位机操作界面,图1b是MYO智能腕带,图1c是UR5机器人。
图2为使用本发明方法的系统示意图。
图3为本发明的控制原理框图。
具体实施方式
以下结合附图对本发明做进一步详述。
一种基于手势识别的机械臂位姿控制方法,主要基于的平台如图1所示,平 台组成主要包括linux电脑客户端软件1、两个MYO智能腕带2和六自由度机 械臂3。linux电脑客户端软件安装在用户的电脑上,便于读取MYO智能腕带的 信号以及处理数据,智能腕带与PC端软件,通过蓝牙连接,将无线蓝牙适配器 插在电脑上;PC端软件与六自由度机械臂通过局域网无线连接。
结合图2和图3,本发明专利的具体实施方式如下:
使用本发明方法的系统依次包括智能腕带模块、蓝牙通信模块、远程客户端 模块、数据处理模块、仿真模块、机械臂执行模块,各模块构成如下:
智能腕带模块:该模块通过蓝牙模块连接远程客户端,并且该模块是可穿戴 的,智能腕带模块为无线智能腕带,其包含九轴惯性测量单元,三轴陀螺仪,三 轴加速度计和三轴磁力计,进行陀螺仪信号的采集;智能腕带还有8块大小厚薄 不一的生物电传感器单元,每个传感器又分为3个电极,通过这24个电极就可 以捕捉用户手臂肌肉运动时产生的生物电变化即肌电信号。智能腕带捕捉手势的 速度非常快,它捕捉的是人体手臂的肌电信号,并且智能腕带不受场地的限制, 交互时会更加自然,同时这是可穿戴的,在移动领域智能腕带具有比较大的优势。
蓝牙通信模块:蓝牙模块一端连接智能腕带模块另一端通过蓝牙的无线传输 功能实现与远程客户端的无线连接;蓝牙通信模块通过无线连接实现蓝牙模块与 智能腕带的双向通信;蓝牙模块通过接收远程客户端传输过来的数据和指令信号, 并将其发送给智能腕带模块;智能腕带和远程客户端软件通过蓝牙模块无线连接, 远程客户端软件通过蓝牙适配器接收信号;通过蓝牙通信模块,智能腕带可以向 远程客户端软件发送传感器信号和肌电信号。
远程客户端模块:远程客户端模块一端通过蓝牙通信模块连接智能腕带模块, 另一端连接数据处理模块;远程客户端模块通过蓝牙通信模块接收智能腕带模块 采集到的肌电信号和陀螺仪信号数据,再将肌电信号和陀螺仪信号传输给数据处 理模块;
数据处理模块:该模块一端连接远程客户端,另一端连接仿真模块;该模块 接收来自远程客户端模块中的肌电信号和陀螺仪信号,将接收到的信号与数据通 过滤波和正逆运动学求解计算出腕带使用者手臂的关节角度,同时对于可疑的数 据进行剔除,将范围内的关节角度数据传递给仿真模块下的机械臂使其做出与人 体相同的动作;
在该模块中MYO智能腕带内部的体感传感器(陀螺仪)模块通过MYO智 能腕带和usb蓝牙适配器组成,体感传感器模块连接linux电脑客户端软件之后, 执行采集功能,将陀螺仪信号和肌电信号实时传递给PC端软件,PC端软件将 收到的数据发送到数据处理模块,经过数据处理之后得到仿真模块中仿人臂的关 节角度,通过将关节角度转换为机械臂的末端位置,将末端位置发送给机械臂并 保存;
在采集过程中,采集到的角度数据会通过蓝牙无线通信同步发送给linux电 脑客户端软件中的仿真机械臂,仿真机械臂会跟随操作人员的手势同步运动;仿 真机械臂再通过局域网将信号传递给真实的机械臂,真实的机械臂接收到信号之 后也随着操作人员的动作一起运动,同时操作人员手掌可以做出特定的手势向机 械臂发送指令,这样就能让真实的机械臂跟随操作人员的手势运动,执行操作指 令,可以完成很多人无法进入实际空间的操作。
数据处理模块收到末端位置数据包后,结合当前所使用的机械臂型号的结构 参数,用逆运动学公式结合当前所使用的机械臂型号的结构参数得到每个采集时 刻优化后的运动角度数据包,具体步骤如下:
1)首先对示教机械臂构建其D-H参数模型,以UR5机器人为例,由于各 杆件相对参考坐标系有转动和平移两个动作,因此对每个杆件沿关节轴建立一个 关节坐标系,取第n个采集时刻示教机械臂六个关节的D-H连杆参数,如表2 所示:
关节i | 连杆长度ai | 连杆转角αi | 连杆偏距di | 关节角θi |
1 | 0 | 0 | 0 | 30° |
2 | -90° | 0 | 0 | -30° |
3 | 0 | 3 | 6 | -60° |
4 | -90° | 4 | 8 | -90° |
5 | 90° | 0 | 0 | 60° |
6 | -90° | 0 | 0 | 90° |
由i关节坐标系相对i-1关节坐标系的位置和姿态的表达式如公式(9)所示。其中且sθi=sinθi,cθi=cosθi,sαi=sinαi,cαi=cosαi, 可以求得示教机械臂末端执行器在第n个采集时刻的位姿相对固定参考坐标系 的变换矩阵为:
2)通过两个腕带获取关节角的方法:
建立世界坐标系(xG,yG,zG)以及建立人手臂的上臂坐标系(xH,yH,zH), 前臂坐标系(xF,yF,zF);R代表旋转矩阵;上标i代表初始位置;下标G、H、 F分别代表世界坐标系框架,上臂框架以及前臂框架;相对于第一个智能腕带的 坐标系和相对于第二个智能腕带对应的下标的坐标系可以表示为:
其中下标“U”表示佩戴在上臂上的第一个MYO臂章框架,下标“L”表 示穿在下臂上的第二个MYO臂章框架;当操作者的手臂做出一个新动作之后, 前臂和上臂在世界坐标系的框架下可以用下面的旋转矩阵来描述:
上标f代表操作者手臂的一个新动作;通过第一个腕带的陀螺仪,可以得到 一个四元数q=[x,y,z,w]T,其中(x,y,z)是标量,w是矢量,可通过公式(1) 求得q的值。
通过上面等式中的四元数,上臂在全局框架中可以用公式(2)与公式(3) 的方式表示。
其中通过这些计算出欧拉角α,β,γ,分别代表肩部的三个角度,α代表偏航 角,β代表俯仰角,γ代表滚动角:
根据两个智能腕带测到的数据,可以计算出两个肘部关节的角度,Afe代表 肘部弯曲的角度,Aps代表肘部滚动的角度,计算结果由公式(4)和公式(5) 求出。
经过滤波处理以及角度判断之后将角度值α,β,γ,Afe,Aps发送给仿真模 块中的仿人臂,通过计算得出手腕的末端位置p(x,y,z);得到末端位置之后通过 逆运动学求解出六自由度机械臂的六个关节角,可以依次通过公式(11),(12), (13),(14),(15)求得θ1等六个关节角度值。
仿真模块:该模块一端与数据处理模块相连接,另一端通过TCP/IP协议与 执行机械臂相连接;模拟实际机械臂的工作执行情况;该模块通过网络通信接收 数据处理模块的数据,再将接收到的数据传输到仿真模块中的仿真机械臂,使仿 真模块中的仿真机械臂模拟出使用者手臂的动作;再通过TCP/IP协议,向机械 臂执行模块发送仿真的数据包;
机械臂执行模块:该模块通过TCP/IP协议与仿真模块相连接,通过接收仿 真模块的数据包使实际机械臂完成与仿真机械臂相同的动作;
用户将MYO智能腕带通过蓝牙与PC端软件相连,linux电脑客户端软件与 机械臂通过局域网连接之后,用户即可运行机器人系统。
linux电脑客户端软件接收到来自用户的开启指令之后,进入工作状态;用 户佩戴好腕带之后做握拳手势,即复位命令;信号稳定之后,用户便可以做出相 应的手势来控制机械臂。
要强调的是,本发明所述的实施例是说明性的,而不是限定性的,因此本发 明包括并不限于具体实施方式中所述的实施例,凡是由本领域技术人员根据本发 明的技术方案得出的类似的其它实施方式,同样属于本发明的保护范围。
Claims (3)
1.一种基于手势识别的机械臂位姿控制方法,其特征在于:使用两个智能腕带进行信号采集,采集的信号主要是两个智能腕带中的陀螺仪信号;然后通过对信号的处理将信号转换为人臂的关节角度;智能腕带的信号通过无线蓝牙传递给远程客户端;远程客户端中的数据处理模块将得到的数据信号先通过提出的机械臂位姿控制方法,求出各个关节角的角度信息,再通过正运动学求解得到仿人臂的末端位置,进一步优化之后,再通过逆运动学求解出工作机械臂的各个关节角度;
具体步骤如下:
1)首先是通过两个腕带获取关节角,建立世界坐标系(xG,yG,zG)以及建立人手臂的上臂坐标系(xH,yH,zH),前臂坐标系(xF,yF,zF);R代表旋转矩阵;上标i代表初始位置;下标G、H、F分别代表世界坐标系框架,上臂框架以及前臂框架;相对于第一个智能腕带的坐标系和相对于第二个智能腕带对应的下标的坐标系可以表示为:
<mrow>
<msubsup>
<mi>R</mi>
<mrow>
<mi>U</mi>
<mi>H</mi>
</mrow>
<mi>i</mi>
</msubsup>
<mo>=</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>R</mi>
<mrow>
<mi>G</mi>
<mi>U</mi>
</mrow>
<mi>i</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<msubsup>
<mi>R</mi>
<mrow>
<mi>G</mi>
<mi>H</mi>
</mrow>
<mi>i</mi>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>R</mi>
<mrow>
<mi>L</mi>
<mi>F</mi>
</mrow>
<mi>i</mi>
</msubsup>
<mo>=</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>R</mi>
<mrow>
<mi>G</mi>
<mi>L</mi>
</mrow>
<mi>i</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<msubsup>
<mi>R</mi>
<mrow>
<mi>G</mi>
<mi>F</mi>
</mrow>
<mi>i</mi>
</msubsup>
</mrow>
其中下标“U”表示佩戴在上臂上的第一个智能腕带臂章框架,下标“L”表示带在下臂上的第二个智能腕带臂章框架;当操作者的手臂做出一个新动作之后,前臂和上臂在世界坐标系的框架下可以用下面的旋转矩阵来描述:
<mrow>
<msubsup>
<mi>R</mi>
<mrow>
<mi>U</mi>
<mi>H</mi>
</mrow>
<mi>f</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>R</mi>
<mrow>
<mi>G</mi>
<mi>U</mi>
</mrow>
<mi>f</mi>
</msubsup>
<msubsup>
<mi>R</mi>
<mrow>
<mi>U</mi>
<mi>H</mi>
</mrow>
<mi>i</mi>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>R</mi>
<mrow>
<mi>L</mi>
<mi>F</mi>
</mrow>
<mi>f</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>R</mi>
<mrow>
<mi>G</mi>
<mi>L</mi>
</mrow>
<mi>f</mi>
</msubsup>
<msubsup>
<mi>R</mi>
<mrow>
<mi>L</mi>
<mi>F</mi>
</mrow>
<mi>i</mi>
</msubsup>
</mrow>
2)上标f代表操作者手臂的一个新动作;通过第一个腕带的陀螺仪,可以得到一个四元数q=[x,y,z,w]T,其中(x,y,z)是标量,w是矢量;
q=xi+yj+zk+w (1)
其中(x,y,z)是标量,表示笛卡尔坐标方向,w是矢量,i,j,k分别为各坐标的参数值。
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&beta;</mi>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msqrt>
<mrow>
<msup>
<msub>
<mi>r</mi>
<mn>31</mn>
</msub>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<msub>
<mi>r</mi>
<mn>32</mn>
</msub>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
<mo>,</mo>
<msub>
<mi>r</mi>
<mn>33</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&alpha;</mi>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>tan</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>r</mi>
<mn>23</mn>
</msub>
<mo>/</mo>
<mi>s</mi>
<mi>&beta;</mi>
<mo>,</mo>
<msub>
<mi>r</mi>
<mn>13</mn>
</msub>
<mo>/</mo>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&gamma;</mi>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>tan</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>r</mi>
<mn>32</mn>
</msub>
<mo>/</mo>
<mi>s</mi>
<mi>&beta;</mi>
<mo>,</mo>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>31</mn>
</msub>
<mo>/</mo>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
其中代表上臂相对于全局的坐标系,其中,r11表示旋转矩阵的第一行第一列的参数
通过(5)计算出欧拉角α,β,γ,分别代表肩部的三个角度,α代表偏航角,β代表俯仰角,γ代表滚动角:
3)根据两个智能腕带测到的数据,可以计算出两个肘部关节的角度,Afe代表肘部弯曲的角度,Aps代表肘部滚动的角度,计算公式如下:
Afe=arccos(a12r13+a22r23+a32r33) (4)
Aps=arccos(r11a11+r21a21+r31a33) (5)
4)肌电信号EMG的大小由IEMG信号的平均值AIEMG表示:
<mrow>
<msub>
<mi>AIEMG</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>m</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mi>t</mi>
</mfrac>
<msubsup>
<mo>&Integral;</mo>
<mn>0</mn>
<mi>t</mi>
</msubsup>
<msub>
<mi>IEMG</mi>
<mi>l</mi>
</msub>
<mi>d</mi>
<mi>t</mi>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
其中IEMGl(n)(l=1,2,...,8)是第l个肌电信号传感器的肌电数据;所以其平均值为:
<mrow>
<mi>E</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mi>L</mi>
</mfrac>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>l</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mn>8</mn>
</munderover>
<msub>
<mi>AIEMG</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
腕关节的角度则是假设与肌电信号EMG呈近似线性关系,腕关节角度计算如下:
<mrow>
<msub>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mi>i</mi>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mi>E</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msubsup>
<mi>E</mi>
<mi>i</mi>
<mi>min</mi>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mi>E</mi>
<mi>i</mi>
<mi>max</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>E</mi>
<mi>i</mi>
<mi>min</mi>
</msubsup>
</mrow>
</mfrac>
<msubsup>
<mi>&theta;</mi>
<mi>i</mi>
<mi>max</mi>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>8</mn>
<mo>)</mo>
</mrow>
</mrow>
5)经过滤波处理以及角度判断之后将角度值α,β,γ,Afe,Aps,发送给仿真模块中的仿人臂,通过正运动学的计算得出手臂的末端位置p(x,y,z);
2.根据权利要求1所述的基于手势识别的机械臂位姿控制方法,其特征在于:通过两个智能腕带的传感器数据通过正运动学得到的手臂的末端位置,其中所述的正运动学求解的算法如下:
2.1)首先对示教机械臂构建其D-H参数,(即关节1≤i≤6的连杆长度ai、连杆转角αi、连杆偏距di和关节角θi)作为输入量,选择工作机械臂各个关节的关节角θi作为输出量;
2.2)由于各杆件相对参考坐标系有转动和平移两个动作,因此对每个杆件沿关节轴建立一个关节坐标系,用以下四个参数来描述杆件:
(S1)连杆长度ai:关节轴i和关节轴i+1之间公垂线的长度;
(S2)连杆转角αi:作一个与两关节轴之间的公垂线垂直的平面,将关节轴i和关节轴i+1投影到该平面,在平面内轴i按右手法则绕ai转向轴i+1,其转角即为连杆转角;
(S3)连杆偏距di:公垂线ai-1与关节轴i的交点到公垂线ai与关节轴i+1的交点的有向距离长度;
(S4)关节角θi:ai-1的延长线与ai之间绕关节轴i旋转所形成的夹角,即采集得到的示教机械臂关节角度;
2.3)以基座坐标系作为T0坐标系,设表示i关节坐标系相对i-1关节坐标系的位置和姿态,则:
<mrow>
<mmultiscripts>
<mi>T</mi>
<mi>i</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mmultiscripts>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>a</mi>
<mi>i</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&theta;c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s&theta;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&theta;s&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>c&alpha;</mi>
<mi>i</mi>
</msub>
<msub>
<mi>d</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>9</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,sθi=sinθi,cθi=cosθi,sαi=sinαi,cαi=cosαi;
2.4)对于六自由度机械臂,获得机械臂末端执行器位姿相对固定参考坐标系的变换矩阵(即机械臂末端位置点坐标的变换矩阵):
<mrow>
<mmultiscripts>
<mi>T</mi>
<mn>6</mn>
<mn>0</mn>
</mmultiscripts>
<mo>=</mo>
<mmultiscripts>
<mi>T</mi>
<mn>1</mn>
<mn>0</mn>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mmultiscripts>
<mi>T</mi>
<mn>2</mn>
<mn>1</mn>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mmultiscripts>
<mi>T</mi>
<mn>3</mn>
<mn>2</mn>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mmultiscripts>
<mi>T</mi>
<mn>4</mn>
<mn>3</mn>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mmultiscripts>
<mi>T</mi>
<mn>5</mn>
<mn>4</mn>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mmultiscripts>
<mi>T</mi>
<mn>6</mn>
<mn>5</mn>
</mmultiscripts>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>r</mi>
<mn>11</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>12</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>13</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>p</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>r</mi>
<mn>21</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>22</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>23</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>p</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>r</mi>
<mn>31</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>32</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>33</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>p</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>10</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,表示六自由度机械臂末端执行器位姿相对于基轴参考坐标系的变换矩阵,r11表示变换矩阵中第一行第一列的参数,px表示x轴的参数。
3.根据权利要求1所述的基于手势识别的机械臂位姿控制方法,其特征在于:利用智能腕带获取到操作人员手臂的各个关节的角度,再通过正运动学获得手臂的末端位置,然后再通过逆运动学求解出六自由度机械臂的六个关节角,则可按顺序求解得到,具体的逆运动学的求解方法如下所示:
θ1表示第一个关节角度参数,求解方法为
<mrow>
<msub>
<mi>&theta;</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>p</mi>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mi>p</mi>
<mi>y</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mi>A</mi>
<mi> </mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>d</mi>
<mn>3</mn>
</msub>
<mo>&PlusMinus;</mo>
<msqrt>
<mrow>
<msubsup>
<mi>p</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>p</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>d</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
由第一个关节角,通过中间运算公式可得第二个和第三个关节角为
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>K</mi>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>p</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>p</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>p</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>a</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>a</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>d</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>d</mi>
<mn>4</mn>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&theta;</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>tan</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>a</mi>
<mn>3</mn>
</msub>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mi>A</mi>
<mi> </mi>
<mi>tan</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<mi>K</mi>
<mo>,</mo>
<mo>&PlusMinus;</mo>
<msqrt>
<mrow>
<msubsup>
<mi>a</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>d</mi>
<mn>4</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msup>
<mi>K</mi>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&theta;</mi>
<mn>23</mn>
</msub>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>tan</mi>
<mn>2</mn>
<mo>&lsqb;</mo>
<mrow>
<mo>(</mo>
<mrow>
<mo>-</mo>
<msub>
<mi>a</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>3</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<msub>
<mi>p</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>p</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>p</mi>
<mi>y</mi>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>d</mi>
<mn>4</mn>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>3</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
<msub>
<mi>s</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>d</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<msub>
<mi>p</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>a</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
<msub>
<mi>c</mi>
<mn>3</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>p</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>p</mi>
<mi>y</mi>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&theta;</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<msub>
<mi>&theta;</mi>
<mn>23</mn>
</msub>
<mo>-</mo>
<msub>
<mi>&theta;</mi>
<mn>3</mn>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>12</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,a2为连杆1的长度,d4为连杆4的偏距,c3是cosθ3的缩写,s3是sinθ3的缩写,K为逆运动学求解时的固定常量;
以此类推可以求出第四关节角θ4,并通过c5和s5求出θ5
<mrow>
<msub>
<mi>&theta;</mi>
<mn>4</mn>
</msub>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>13</mn>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mn>23</mn>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>,</mo>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>13</mn>
</msub>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>23</mn>
</msub>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mn>33</mn>
</msub>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>13</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s</mi>
<mn>5</mn>
</msub>
<mo>=</mo>
<mo>-</mo>
<mo>&lsqb;</mo>
<msub>
<mi>r</mi>
<mn>13</mn>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>4</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>r</mi>
<mn>23</mn>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>33</mn>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c</mi>
<mn>5</mn>
</msub>
<mo>=</mo>
<msub>
<mi>r</mi>
<mn>13</mn>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>r</mi>
<mn>23</mn>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<mo>-</mo>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>r</mi>
<mn>33</mn>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&theta;</mi>
<mn>5</mn>
</msub>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>tan</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>5</mn>
</msub>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>5</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>14</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,c23=c2c3-s2s3,s23=c2s3+s2c3。r13表示变换矩阵中第一行第三列的参数值。
同理可以通过求解c6和s6,求出θ6角度:
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>11</mn>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<msub>
<mi>s</mi>
<mn>4</mn>
</msub>
<mo>-</mo>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>21</mn>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<msub>
<mi>s</mi>
<mn>4</mn>
</msub>
<mo>+</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>r</mi>
<mn>31</mn>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
<msub>
<mi>s</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
<mo>=</mo>
<msub>
<mi>r</mi>
<mn>11</mn>
</msub>
<mo>&lsqb;</mo>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<msub>
<mi>c</mi>
<mn>5</mn>
</msub>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
<msub>
<mi>s</mi>
<mn>5</mn>
</msub>
<mo>&rsqb;</mo>
<mo>+</mo>
<msub>
<mi>r</mi>
<mn>21</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>4</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<msub>
<mi>c</mi>
<mn>5</mn>
</msub>
<mo>-</mo>
<msub>
<mi>s</mi>
<mn>1</mn>
</msub>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
<msub>
<mi>s</mi>
<mn>5</mn>
</msub>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>31</mn>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>23</mn>
</msub>
<msub>
<mi>c</mi>
<mn>4</mn>
</msub>
<msub>
<mi>c</mi>
<mn>5</mn>
</msub>
<mo>+</mo>
<msub>
<mi>c</mi>
<mn>23</mn>
</msub>
<msub>
<mi>s</mi>
<mn>5</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&theta;</mi>
<mn>6</mn>
</msub>
<mo>=</mo>
<mi>A</mi>
<mi> </mi>
<mi>tan</mi>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>s</mi>
<mn>6</mn>
</msub>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>6</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>15</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,Atan2为反正切函数,si=sinθi,ci=cosθi;
在每个关节角度求出后,接着判断每个关节角度是不是在限定范围内,将各关节角度发送到仿真模块中的仿真机械臂进行仿真;仿真效果调试好之后,将关节角度和手势指令发送给实际机械臂,实现通过人体手势的动作来控制实际机械臂动作。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711088709.1A CN107856014B (zh) | 2017-11-08 | 2017-11-08 | 基于手势识别的机械臂位姿控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711088709.1A CN107856014B (zh) | 2017-11-08 | 2017-11-08 | 基于手势识别的机械臂位姿控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107856014A true CN107856014A (zh) | 2018-03-30 |
CN107856014B CN107856014B (zh) | 2020-10-09 |
Family
ID=61701389
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711088709.1A Active CN107856014B (zh) | 2017-11-08 | 2017-11-08 | 基于手势识别的机械臂位姿控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107856014B (zh) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109276245A (zh) * | 2018-11-01 | 2019-01-29 | 重庆中科云丛科技有限公司 | 一种表面肌电信号特征处理与关节角度预测方法及系统 |
CN110370264A (zh) * | 2018-04-13 | 2019-10-25 | 江门市印星机器人有限公司 | 机械手臂远程模仿控制系统 |
CN110625609A (zh) * | 2018-06-25 | 2019-12-31 | 精工爱普生株式会社 | 控制装置、机器人及机器人系统 |
CN111430022A (zh) * | 2020-03-20 | 2020-07-17 | 苏州新医智越机器人科技有限公司 | 一种穿刺手术机器人穿刺算法 |
CN112365580A (zh) * | 2020-11-16 | 2021-02-12 | 同济大学 | 一种面向人机技能传授的虚拟作业演示系统 |
CN112775931A (zh) * | 2019-11-05 | 2021-05-11 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、装置、计算机可读存储介质及机器人 |
CN112828916A (zh) * | 2021-01-20 | 2021-05-25 | 清华大学深圳国际研究生院 | 冗余机械臂遥操作组合交互装置及冗余机械臂遥操作系统 |
CN113126568A (zh) * | 2021-03-10 | 2021-07-16 | 上海乾庾智能科技有限公司 | 一种基于增强现实技术的工业机器人操作和演示系统 |
CN114714358A (zh) * | 2022-04-18 | 2022-07-08 | 山东大学 | 基于手势协议遥操作机械臂方法及系统 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104002307A (zh) * | 2014-05-23 | 2014-08-27 | 智慧城市系统服务(中国)有限公司 | 穿戴式救援机器人控制方法及系统 |
US20150025355A1 (en) * | 2013-07-22 | 2015-01-22 | Thalmic Labs Inc. | Systems, articles and methods for strain mitigation in wearable electronic devices |
CN104317196A (zh) * | 2014-09-29 | 2015-01-28 | 华南理工大学 | 一种基于虚拟现实的上肢康复训练机器人控制方法 |
US20150057770A1 (en) * | 2013-08-23 | 2015-02-26 | Thaimic Labs Inc. | Systems, articles, and methods for human-electronics interfaces |
CN105014676A (zh) * | 2015-07-03 | 2015-11-04 | 浙江大学 | 一种机器人运动控制方法 |
CN106406518A (zh) * | 2016-08-26 | 2017-02-15 | 清华大学 | 手势控制装置及手势识别方法 |
CN106695794A (zh) * | 2017-02-20 | 2017-05-24 | 苏州晨本智能科技有限公司 | 一种基于表面肌电信号的移动机器臂系统及其控制方法 |
-
2017
- 2017-11-08 CN CN201711088709.1A patent/CN107856014B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20150025355A1 (en) * | 2013-07-22 | 2015-01-22 | Thalmic Labs Inc. | Systems, articles and methods for strain mitigation in wearable electronic devices |
US20150057770A1 (en) * | 2013-08-23 | 2015-02-26 | Thaimic Labs Inc. | Systems, articles, and methods for human-electronics interfaces |
CN104002307A (zh) * | 2014-05-23 | 2014-08-27 | 智慧城市系统服务(中国)有限公司 | 穿戴式救援机器人控制方法及系统 |
CN104317196A (zh) * | 2014-09-29 | 2015-01-28 | 华南理工大学 | 一种基于虚拟现实的上肢康复训练机器人控制方法 |
CN105014676A (zh) * | 2015-07-03 | 2015-11-04 | 浙江大学 | 一种机器人运动控制方法 |
CN106406518A (zh) * | 2016-08-26 | 2017-02-15 | 清华大学 | 手势控制装置及手势识别方法 |
CN106695794A (zh) * | 2017-02-20 | 2017-05-24 | 苏州晨本智能科技有限公司 | 一种基于表面肌电信号的移动机器臂系统及其控制方法 |
Non-Patent Citations (3)
Title |
---|
孙斌: ""六轴工业机器人的离线编程与仿真系统研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
李飞: ""某型六自由度机械臂的运动控制与仿真研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
顾辰: ""六自由度机械臂的动态路径规划和控制研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110370264A (zh) * | 2018-04-13 | 2019-10-25 | 江门市印星机器人有限公司 | 机械手臂远程模仿控制系统 |
CN110625609A (zh) * | 2018-06-25 | 2019-12-31 | 精工爱普生株式会社 | 控制装置、机器人及机器人系统 |
CN109276245A (zh) * | 2018-11-01 | 2019-01-29 | 重庆中科云丛科技有限公司 | 一种表面肌电信号特征处理与关节角度预测方法及系统 |
CN109276245B (zh) * | 2018-11-01 | 2020-02-07 | 重庆中科云从科技有限公司 | 一种表面肌电信号特征处理与关节角度预测方法及系统 |
CN112775931A (zh) * | 2019-11-05 | 2021-05-11 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、装置、计算机可读存储介质及机器人 |
CN111430022A (zh) * | 2020-03-20 | 2020-07-17 | 苏州新医智越机器人科技有限公司 | 一种穿刺手术机器人穿刺算法 |
CN112365580A (zh) * | 2020-11-16 | 2021-02-12 | 同济大学 | 一种面向人机技能传授的虚拟作业演示系统 |
CN112365580B (zh) * | 2020-11-16 | 2022-10-28 | 同济大学 | 一种面向人机技能传授的虚拟作业演示系统 |
CN112828916A (zh) * | 2021-01-20 | 2021-05-25 | 清华大学深圳国际研究生院 | 冗余机械臂遥操作组合交互装置及冗余机械臂遥操作系统 |
CN112828916B (zh) * | 2021-01-20 | 2022-07-08 | 清华大学深圳国际研究生院 | 冗余机械臂遥操作组合交互装置及冗余机械臂遥操作系统 |
CN113126568A (zh) * | 2021-03-10 | 2021-07-16 | 上海乾庾智能科技有限公司 | 一种基于增强现实技术的工业机器人操作和演示系统 |
CN114714358A (zh) * | 2022-04-18 | 2022-07-08 | 山东大学 | 基于手势协议遥操作机械臂方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN107856014B (zh) | 2020-10-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107856014A (zh) | 基于手势识别的机械臂位姿控制方法 | |
CN107943283A (zh) | 基于手势识别的机械臂位姿控制系统 | |
CN103112007B (zh) | 基于混合传感器的人机交互方法 | |
CN108638069B (zh) | 一种机械臂末端精确运动控制方法 | |
CN107932504A (zh) | 基于PyQt的机械臂运行控制系统 | |
CN103895022A (zh) | 穿戴式体感控制机械手 | |
CN109079794B (zh) | 一种基于人体姿态跟随的机器人控制与示教方法 | |
Xu et al. | Development of a hybrid motion capture method using MYO armband with application to teleoperation | |
CN115469576B (zh) | 一种基于人-机械臂异构运动空间混合映射的遥操作系统 | |
CN107450376B (zh) | 一种基于智能移动平台的服务机械臂抓取姿态角计算方法 | |
CN103192387A (zh) | 机器人及其控制方法 | |
CN111645093A (zh) | 一种用于遥操作的力觉反馈数据手套 | |
CN112828916B (zh) | 冗余机械臂遥操作组合交互装置及冗余机械臂遥操作系统 | |
CN109529274B (zh) | 基于冗余机械臂上肢关节主动康复系统及其训练方法 | |
CN104002307A (zh) | 穿戴式救援机器人控制方法及系统 | |
CN105904457B (zh) | 一种基于位置跟踪器及数据手套的异构型冗余机械臂控制方法 | |
CN107544299A (zh) | 用于六自由度机械臂示教控制的pc端app系统 | |
CN108340352A (zh) | 基于示教关节臂的工业机器人远程实时操控方法 | |
CN111300408A (zh) | 一种结合形似与神似的仿人双臂机器人运动规划控制方法 | |
CN107914273A (zh) | 基于手势控制的机械臂示教系统 | |
CN113305830A (zh) | 一种基于人体姿态控制的仿人机器人动作系统及控制方法 | |
CN113618717B (zh) | 人体上肢技能动作感测学习装置及方法 | |
CN112057083B (zh) | 可穿戴人体上肢位姿采集设备及采集方法 | |
CN104267820A (zh) | 一种双并联结构多维触觉反馈装置 | |
CN114248283A (zh) | 一种蓝牙感知的外骨骼维修机器手 |
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 |