一种机械臂与灵巧手的融合系统及运动控制方法与流程

1.本发明属于机器人运动控制领域，涉及一种四自由度机械臂与灵巧手的融合系统及运动控制方法。


背景技术：

2.作为机器人与环境相互作用的执行部件，大抓握力多自由度全驱动仿人灵巧手对机器人智能化水平的提高具有非常重要的作用。单纯有了仿人灵巧手，仍然不能独立的完成精细操作任务，而需要机械臂作为灵巧手的载体，因此机械臂与灵巧手的融合系统及运动控制方法是实现机器人智能化和灵巧操作的关键技术。
3.国内大抓握力多自由度全驱动仿人灵巧手的研制水平尚处于起步阶段，现有的产品多为单自由度的手爪或六自由度、十二自由度的欠驱动的灵巧手，并不能完全模仿人手进行精细操作。
4.目前国内外流行的臂手结合的方式是六、七自由度机械臂加末端手爪或者六自由度、十二自由度灵巧手，在这种结合方式下，机械臂与灵巧手都是单独控制，并没有实现联合控制，也没有臂手融合控制。
5.目前现有的机械臂与手爪的结合中往往没有手腕这个环节或者采用机械臂的关节来代替手腕的功能，既不美观也不实用；传统技术中没有正交双自由度手腕的控制方法；对于四自由度机械臂与灵巧手手腕这种四自由度加双自由度串并联融合系统并没有很好的控制方法；四自由度机械臂与灵巧手手腕可以近似看作六自由度机械臂，然而这种臂型末端三轴不正交，不存在解析解。
6.如何提供一种四自由度机械臂与灵巧手手腕的融合系统及运动控制方法是本领域亟待解决的技术问题。


技术实现要素：

7.本发明解决的技术问题是：克服现有技术的不足，提出一种机械臂与灵巧手的融合系统及运动控制方法。
8.本发明解决技术的方案是：
9.一种机械臂与灵巧手的融合系统，包括一条四自由度机械臂、一个灵巧手手腕和一个灵巧手，灵巧手手腕安装在四自由度机械臂的末端，灵巧手手掌安装在灵巧手手腕上；灵巧手手腕和四自由度机械臂通过电子线路连接。
10.优选的，灵巧手手腕是一个两自由度并联机构，包括两台电机、两根推杆和两台伺服位置控制器；
11.两台伺服位置控制器分别为主动伺服位置控制器和从动伺服位置控制器，两台电机包括第一电机和第二电机，两根推杆包括第一推杆和第二推杆，主动伺服位置控制器用于驱动第一电机运动，第一电机运动带动第一推杆运动，从动伺服位置控制器用于驱动第二电机运动，第二电机运动带动第二推杆运动；第一推杆的末端和第二推杆的末端同时与
灵巧手手掌连接。
12.优选的，两台电机采用同步差动控制方式，两台电机驱动两根推杆运动，实现灵巧手的俯仰运动和偏航转动。
13.当两根推杆同向运动时灵巧手进行俯仰运动，俯仰角度范围为-70度—70度；当两根推杆反向运动运动时灵巧手进行偏航转动，偏航角度范围为-45度—45度。
14.一种机械臂与灵巧手的融合系统的运动控制方法，包括：
15.将四自由度机械臂与灵巧手融合系统看作一个六轴串联机械臂，以四自由度机械臂基座中心为原点，使用d-h方法建立机械臂坐标系，并给出机械臂各关节及连杆的d-h参数；
16.根据四自由度机械臂的关节角度θ1、θ2、θ3、θ4和灵巧手手腕的关节角度θ5、θ6，通过四自由度机械臂与灵巧手手腕融合系统的正运动学方法，求得灵巧手手腕末端的位置姿态矩阵实现对融合系统正向运动控制；r表示灵巧手手腕末端的旋转矩阵，p表示灵巧手手腕末端的位置矩阵；
17.若已知灵巧手手腕末端位置姿态通过四自由度机械臂与灵巧手手腕融合系统的逆运动学数值求解方法，求得融合系统的关节角度θ1、θ2、θ3、θ4、θ5、θ6，根据关节角度获得控制指令，发送给伺服位置控制器，以驱动灵巧手手腕末端达到预定的位置姿态。
18.优选的，对融合系统正向运动控制时，灵巧手手腕末端的位置姿态
19.为融合系统中，机械臂基坐标系到机械臂第一关节的坐标转换矩阵；
20.为融合系统中，机械臂第一关节到机械臂第二关节的坐标转换矩阵；
21.为融合系统中，机械臂第二关节到机械臂第三关节的坐标转换矩阵；
22.为融合系统中，机械臂第三关节到机械臂第四关节的坐标转换矩阵；
23.为融合系统中，机械臂第四关节到灵巧手手腕第一关节的坐标转换矩阵；
24.为融合系统中，灵巧手手腕第一关节到灵巧手手腕第二关节的坐标转换矩阵；
25.为融合系统中，机械臂基坐标系到灵巧手手腕末端的坐标转换矩阵。
[0026][0027]
[0028]
其中，dj为第j个关节与第j-1个关节的连杆距离，j＝2，3，4，5；a
p
为第p个关节的连杆长度，p＝2；
[0029]
si为第i个关节角度正弦，ci为第i个关节角度余弦，i＝1，2，3，4，5，6。
[0030]
si＝sinθi，ci＝cosθi。
[0031]
优选的，关节角度θ6采用迭代方式求数值解。
[0032]
θ6的第k 1次迭代公式如下：
[0033][0034]
θ
6(k 1)
为θ6的第k 1次迭代结果，θ
6(k)
为θ6的第k次迭代结果，θ
6(k-1)
为θ6的第k-1次迭代结果，f(θ
6(k 1)
)为θ
6(k 1)
处的逆运动学公式，f(θ
6(k)
)为θ
6(k)
处的逆运动学公式，f(θ
6(k-1)
)为θ
6(k-1)
处的逆运动学公式。
[0035]
本发明与现有技术相比的有益效果是：
[0036]
(1)本发明设计了一种正交双自由度灵巧手的手腕，仅占用单关节的空间，通过冗余双电机同步差动控制，有效保证了可靠性。
[0037]
(2)本发明采用了四自由度机械臂与灵巧手双自由度手腕相结合的六自由度串并联机构，来进行机械臂与灵巧手的融合控制，四自由度机械臂与灵巧手腕部两关节组成的六自由度臂型不满足末端三轴交于一点，没有逆运动学解析解，只有数值解，因此设计了一种基于这种臂型的逆运动学数值求解方法，适用于所有串联机械臂数值解求解。
[0038]
(3)本发明将灵巧手腕部两个耦合关节进行解耦，将其视为两个独立的机械臂关节，关节5，关节6，这样通过本发明的控制方法就将灵巧手手腕复杂并联机构能够与机械臂融合起来按照六自由度串联机械臂的控制方法进行控制。
[0039]
(4)本发明中四自由度机械臂与灵巧手双自由度手腕相结合的六自由度串并联机构，具有多自由度冗余，这样就可以让机械臂不用大范围运动，即可以实现五指灵巧手的转动。
附图说明
[0040]
图1为灵巧手手腕部分控制示意图；
[0041]
图2为单关节空间正交双自由度集成的手腕；
[0042]
图3为四自由度机械臂与灵巧手双自由度手腕相结合的构型。
具体实施方式
[0043]
下面结合附图对本发明作进一步阐述。
[0044]
本发明在不修改灵巧手的前提下，设计一种既美观又实用的机械臂与灵巧手融合系统，节约研发成本，缩短研发周期；同时提出一种逆运动学数值解求解方法，实现一种机械臂与灵巧手融合系统的运动控制。
[0045]
一种机械臂与灵巧手的融合系统如图3所示，包括一条四自由度机械臂、一个灵巧手手腕(单关节空间正交双自由度集成的手腕)和一个灵巧手。灵巧手双自由度的手腕安装在四自由度机械臂的末端，两者通过电子线路连接，进行通信和供电。灵巧手手掌安装在灵
巧手手腕上。一种四自由度机械臂与灵巧手手腕融合系统的运动控制方法，包括单关节空间正交双自由度集成手腕的控制方法、机械臂与灵巧手的融合系统正运动学方法、机械臂与灵巧手的融合系统逆运动学方法。
[0046]
本发明单关节空间正交双自由度集成的手腕，如图2所示，采用双电机m1、m2同步差动控制。使用双电机推动双推杆l1、l2实现灵巧手的俯仰运动和偏航转动，当双推杆l1、l2同向运动时灵巧手进行俯仰运动，此时可以将手腕看作关节j1，关节j1的俯仰角度范围为-70度—70度；当双推杆l1、l2反向运动运动时灵巧手进行偏航转动，此时可以将手腕看作关节j2，关节j2的偏航角度范围为-45度—45度。双电机中其中一个电机m1由伺服位置控制器a(主动)驱动，另一个电机m2由伺服位置控制器b(从动)驱动。
[0047]
灵巧手双自由度手腕控制方法如图1所示，输入灵巧手的目标位置，融合系统通过控制律计算控制指令，将控制指令发送给伺服位置控制器a(主动)和伺服位置控制器b(从动)，伺服控制器再将伺服控制指令结合负载传动比ka、kb，得到对应电机的驱动指令。关节实际位置通过位置传感器之后通过积分器运算将结果反馈给融合系统，从而实现大闭环控制。
[0048]
一种四自由度机械臂与灵巧手双自由度手腕融合系统的运动控制方法，步骤如下：
[0049]
(1)将四自由度机械臂与灵巧手手腕融合系统看作一个六轴串联机械臂。以机械臂基座中心为原点，使用d-h方法建立坐标系，并列出各连杆和关节的d-h参数，其中关节的d-h参数包括四自由度机械臂的关节角度θ1、θ2、θ3、θ4，灵巧手手腕的关节角度θ5、θ6。
[0050]
(2)根据已知的四自由度机械臂的关节角度θ1、θ2、θ3、θ4和手腕的关节角度θ5、θ6，通过四自由度机械臂与灵巧手手腕融合系统的正运动学方法，求得灵巧手手腕末端的位置姿态从而实现对系统正向运动控制。r表示灵巧手手腕末端的旋转矩阵，p表示灵巧手手腕末端的位置矩阵。
[0051]
四自由度机械臂与灵巧手手腕融合系统各坐标系之间的变换矩阵分别为：
[0052][0053][0054][0055]
为融合系统中，机械臂基坐标系到机械臂第一关节的坐标转换矩阵；
[0056]
为融合系统中，机械臂第一关节到机械臂第二关节的坐标转换矩阵；
[0057]
为融合系统中，机械臂第二关节到机械臂第三关节的坐标转换矩阵；
[0058]
为融合系统中，机械臂第三关节到机械臂第四关节的坐标转换矩阵；
[0059]
为融合系统中，机械臂第四关节到灵巧手手腕第一关节的坐标转换矩阵；
[0060]
为融合系统中，灵巧手手腕第一关节到灵巧手手腕第二关节的坐标转换矩阵；
[0061]
为融合系统中，机械臂基坐标系到灵巧手手腕末端的坐标转换矩阵。
[0062]
dj为第j个关节与第j-1个关节的连杆距离，j＝2，3，4，5；a
p
为第p个关节的连杆长度，p＝2；
[0063]
si为第i个关节角度正弦，ci为第i个关节角度余弦，i＝1，2，3，4，5，6。si＝sinθi，ci＝cosθi，θi为第i个关节角度。
[0064]
第1个关节角度范围为-90
°
《θ1《90
°
。第2个关节角度范围为0
°
《θ2《90
°
。第3个关节角度范围为-120
°
《θ3《0
°
。第4个关节角度范围为0
°
《θ4《180
°
。第5个关节角度范围为-70
°
《θ5《70
°
。第6个关节角度范围为-45
°
《θ6《45
°
。
[0065]
(3)根据已知的系统末端位置姿态由于系统末端三轴不正交，不存在解析解，通过四自由度机械臂与灵巧手手腕融合系统的逆运动学数值解方法，求得系统的关节角度θ1、θ2、θ3、θ4、θ5、θ6，从而实现对系统反向运动控制。
[0066]
通过机器人逆运动学求解可得θ1、θ2、θ3、θ4、θ5，而θ6无法求得解析解，因此只能求数值解。
[0067]
从本质上讲，数值解是猜测和迭代，直到错误足够小，或者直到认为放弃。本方法中采用常见的牛顿
‑‑
拉夫逊算法，因为它在概念上简单并且如果初始猜测与解“足够接近”时具有二次收敛速率。newton迭代法需要计算f(x)在xk处的一阶导数，对复杂的函数，特别是多元隐函数，求导数或偏导数是相对繁琐和复杂的，采用近似计算的割线法，公式如下：
[0068][0069]
但是，不能保证算法会收敛或足够快地满足应用要求，并且只返回一个解决方案。为了针对各种可能的姿势产生解决方案，必须使用不同的初始条件。
[0070]
因此，本发明中，第k 1次关节角度θ6的求解公式如下：
[0071][0072]
θ
6(k 1)
为θ6的第k 1次迭代结果，θ
6(k)
为θ6的第k次迭代结果，θ
6(k-1)
为θ6的第k-1次迭代结果，f(θ
6(k 1)
)为θ
6(k 1)
处的逆运动学公式，f(θ
6(k)
)为θ
6(k)
处的逆运动学公式，f(θ
6(k-1)
)为θ
6(k-1)
处的逆运动学公式。
[0073]
本发明采用了四自由度机械臂与灵巧手双自由度手腕相结合的六自由度串并联机构，来进行机械臂与灵巧手的融合控制，四自由度机械臂与灵巧手腕部两关节组成的六自由度臂型不满足末端三轴交于一点，没有逆运动学解析解，只有数值解，因此设计了一种基于这种臂型的逆运动学数值解方法，适用于所有串行机械手数值解求解。
[0074]
本发明中四自由度机械臂与灵巧手双自由度手腕相结合的六自由度串并联机构，
具有多自由度冗余，这样就可以让机械臂不用大范围运动，即可以实现五指灵巧手的转动。
[0075]
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。