一种冗余机械臂的臂型角参数化逆运动学求解方法

1.本发明属于空间冗余机械臂逆运动学技术领域，特别涉及一种冗余机械臂的臂型角参数化逆运动学求解方法。


背景技术：

2.与6自由度机械臂相比，冗余机械臂在操作灵活性、避障、避奇异、容错性、关节力矩优化等方面都具有非常明显的优势。安装于国际空间站上的加拿大2臂(canadian2)、欧洲机械臂(era)、专用灵巧机械臂(spdm)以及我国空间站的核心舱机械臂(cmm)和实验舱机械臂(emm)等均为冗余机械臂。这类机械臂在肩部、肘部、腕部均存在偏置，为了方便起见，通常将关节排列与此相似的机械臂称为ssrms型机械臂。由于偏置量的存在，ssrms型机械臂在获得较大工作空间的同时使得位置和姿态不能解耦，与7自由度拟人机械臂经常采用的球腕(srs)构型方案相比，其逆运动学的求解更加困难。


技术实现要素：

3.本发明的目的是提供一种冗余机械臂的臂型角参数化逆运动学求解方法，以解决ssrms型机械臂的逆运动学结果求解的技术问题。本发明中所说的冗余机械臂的臂型角与以往球腕型机械臂的臂型角概念有所不同，本发明中所说的冗余机械臂的臂型角在下述技术方案部分有详细定义。
4.本发明所采用的技术方案是，一种冗余机械臂的臂型角参数化逆运动学求解方法，其特殊之处在于，包括以下步骤：
5.步骤1：建立冗余机械臂的运动学模型，并在冗余机械臂末端相对于基座的位姿矩阵给定的条件下，计算从1号坐标系的原点到6号坐标系的原点所确定的向量的值；所述从1号坐标系的原点到6号坐标系的原点所确定的向量具体为：将1号坐标系的原点定义为s点，将6号坐标系的原点定义为w点，定义向量向量p
16
即为从1号坐标系的原点到6号坐标系的原点所确定的向量；
6.步骤2：定义当前臂型面，并计算当前臂型面的法向量在sw方向上的投影向量；
7.步骤3：定义参考平面，并计算参考平面的单位法向量；
8.步骤4：根据步骤2中定义的当前臂型面和步骤3中定义的参考平面，定义当前臂型面与参考平面之间的臂型角；并将所述臂型角作为参数，计算当前臂型面的法向量；
9.步骤5：根据步骤4中计算得到的当前臂型面的法向量值，计算关节角2的值；
10.步骤6：根据步骤5中计算得到的关节角2的值，再按照单关节锁定方法求出其余关节角的值，进而最终得到8组逆运动学结果，逆运动学求解完成。
11.进一步地，所述步骤1具体包括以下步骤：
12.步骤1.1：建立冗余机械臂的运动学模型；
13.冗余机械臂的运动学模型如下式(1)和式(2)所示：
[0014]0t7＝0t
11
t2…6t7＝f(θ)(1)；
[0015][0016]
式(1)和式(2)中：0t7表示冗余机械臂末端相对于基座的位姿矩阵；0t1表示1号坐标系相对于0号坐标系的位姿矩阵；1t2表示2号坐标系相对于1号坐标系的位姿矩阵；6t7表示7号坐标系相对于6号坐标系的位姿矩阵；θ＝[θ1θ2θ3θ4θ5θ6θ7]表示冗余机械臂的7个关节角；n＝[n
x
,ny,nz]
t
、o＝[o
x
,oy,oz]
t
、a＝[a
x
,ay,az]
t
分别表示末端坐标系的x、y、z轴在基坐标系{x0、y0、z0}中表示的单位向量；p＝[p
x
,py,pz]
t
为末端坐标系原点相对于基坐标系的位置向量；
[0017]
步骤1.2：计算从1号坐标系的原点到6号坐标系的原点所确定的向量p
16
的值；
[0018]
在冗余机械臂末端相对于基座的位姿矩阵0t7给定的条件下，根据步骤1.1中式(2)，得到下式(3)：
[0019][0020]
式(3)中：d7表示6号坐标系和7号坐标系之间的连杆偏距；d1表示0号坐标系和1号坐标系之间的连杆偏距。
[0021]
进一步地，所述步骤2具体为：
[0022]
作ss
′
，ss
′
满足ss
′
平行于z4且ss
′
＝d3+0.5d4；作ww
′
，ww
′
满足ww
′
平行于z4且ww
′
＝d5+0.5d4；sw与s
′w′
相交于点o；所述z4表示4号坐标系的z轴在基坐标系中表示的单位向量；所述d3表示2号坐标系和3号坐标系之间的连杆偏距；所述d4表示3号坐标系和4号坐标系之间的连杆偏距；所述d5表示4号坐标系和5号坐标系之间的连杆偏距；将4号坐标系的原点定义为e点；第4个连杆的一端端面中心点与e点重合，另一端端面中心点为o4′
点；点e
′
为eo4′
的中点；定义平面s
′e′w′
为所述当前臂型面；
[0023]
过所述点o作向量过所述点o作向量满足与z4平行且相等，将向量记为eo，则向量eo为所述当前臂型面的法向量，且有下式(4)成立：
[0024]
p
16
·eo
＝ss
′
+w
′
w＝d3+d4+d5(4)；
[0025]
过点r作rd垂直sw于点d，向量记为ed，则ed即为所述当前臂型面的法向量在sw方向上的投影向量，则可以得出ed如下式(5)、式(6)所示：
[0026][0027][0028]
进一步地，所述步骤3具体为：
[0029]
定义向量与z1所组成的平面为所述参考平面，所述z1为1号坐标系的z轴在基坐标系中表示的单位向量；
[0030]
过步骤2中所述的点d，作所述参考平面的单位法向量记向量为e
r0
；向量z1＝[001]
t
，则可以得出所述参考平面的单位法向量如下式(7)所示：
[0031][0032]
进一步地，所述步骤4具体为：
[0033]
记向量为er，步骤3中参考平面的单位法向量e
r0
按照右手螺旋定则绕向量p
16
旋转ψ后与向量er重合；将ψ定义为当前臂型面与参考平面之间的臂型角；
[0034]
根据罗德里格斯旋转公式可知下式(8)：
[0035][0036]
则所述当前臂型面的法向量，即向量eo如下式(9)所示：
[0037]eo
＝ed+er(9)。
[0038]
进一步地，所述步骤5具体为：
[0039]
根据冗余机械臂dh坐标系之间的关系和运动学方程可知0r3如下式(10)所示：
[0040][0041]
式(10)中：0r3表示第3个连杆相对于基座的姿态；ci＝cos(θi)，si＝sin(θi)，其中i取1或2；
[0042]
由冗余机械臂可知，向量z3和向量eo相等，因此，由所述式(10)可得下式(11)：
[0043]
z3＝eo＝[c1s2s1s
2-c2]
t
(11)；
[0044]
定义eo(j)表示向量eo第j个元素的值，则所述关节角2的值如下式(12)所示：
[0045]
θ2＝
±
arccos(eo(3))(12)。
[0046]
进一步地，所述冗余机械臂是指ssrms型机械臂。
[0047]
本发明的有益效果是：
[0048]
(1)本发明根据ssrms型机械臂自身的自运动特点，给出了一种新的臂型角参数的定义。不同于球腕型机械臂，ssrms型机械臂在其肩部、肘部、腕部均有偏置，其逆运动学的求解较为困难。本发明所提出的臂型角参数化逆运动学求解方法可以直接得到逆运动学的解析解，有效地解决了该类偏置式的ssrms型冗余机械臂的逆运动学结果求解的技术问题，与其它数值方法相比计算效率高，精度绝对。
[0049]
(2)本发明给出的臂型角参数可以直观的描述工作空间中机械臂的整体构型，其几何意义明显，适用于臂型控制，还可以通过控制臂型角参数，实现避障等其它特殊问题的处理。本发明方法与现有其它方法相比，很好的解决了ssrms型机械臂的实时运动规划与控制问题。
附图说明
[0050]
图1是本发明实施例中ssrms型机械臂dh坐标系图；
[0051]
图2是本发明实施例中当前臂型面及臂型角的示意图。
具体实施方式
[0052]
下面结合附图和具体实施方式对本发明进行详细说明。
[0053]
本发明一种冗余机械臂的臂型角参数化逆运动学求解方法，包括以下步骤：
[0054]
步骤1：建立冗余机械臂的运动学模型，并在冗余机械臂末端相对于基座的位姿矩阵给定的条件下，计算从1号坐标系的原点到6号坐标系的原点所确定的向量的值；上述从1号坐标系的原点到6号坐标系的原点所确定的向量具体为：参见图2，将1号坐标系的原点定义为s点，将6号坐标系的原点定义为w点，定义向量向量p
16
即为从1号坐标系的原点到6号坐标系的原点所确定的向量；
[0055]
步骤2：定义当前臂型面，并计算当前臂型面的法向量在sw方向上的投影向量；
[0056]
步骤3：定义参考平面，并计算参考平面的单位法向量；
[0057]
步骤4：根据步骤2中定义的当前臂型面和步骤3中定义的参考平面，定义当前臂型面与参考平面之间的臂型角；并将上述臂型角作为参数，计算当前臂型面的法向量；
[0058]
步骤5：根据步骤4中计算得到的当前臂型面的法向量值，计算关节角2的值；
[0059]
步骤6：根据步骤5中计算得到的关节角2的值，再按照单关节锁定方法求出其余关节角的值，进而最终得到8组逆运动学结果，逆运动学求解完成。
[0060]
本实施例中，上述步骤1具体包括以下步骤：
[0061]
步骤1.1：建立冗余机械臂的运动学模型；
[0062]
本实施例中，冗余机械臂为ssrms型机械臂，该ssrms型机械臂dh坐标系图如图1所示，dh参数见下表1：
[0063]
表1：ssrms型机械臂的dh参数
[0064][0065]
表1中：αi表示绕x
i-1
轴，从z
i-1
旋转到zi的角度；ai表示沿x
i-1
轴，从z
i-1
移动到zi的距离；θi表示绕z
i-1
轴，从x
i-1
旋转到xi的角度；di表示沿z
i-1
轴，从x
i-1
移动到xi的距离；
[0066]
冗余机械臂的运动学模型如下式(1)和式(2)所示：
[0067]0t7＝0t
11
t2…6t7＝f(θ)(1)；
[0068]
[0069]
式(1)和式(2)中：0t7表示冗余机械臂末端相对于基座的位姿矩阵；0t1表示1号坐标系相对于0号坐标系的位姿矩阵；1t2表示2号坐标系相对于1号坐标系的位姿矩阵；6t7表示7号坐标系相对于6号坐标系的位姿矩阵；θ＝[θ1θ2θ3θ4θ5θ6θ7]表示冗余机械臂的7个关节角；n＝[n
x
,ny,nz]
t
、o＝[o
x
,oy,oz]
t
、a＝[a
x
,ay,az]
t
分别表示末端坐标系的x、y、z轴在基坐标系{x0、y0、z0}中表示的单位向量；p＝[p
x
,py,pz]
t
为末端坐标系原点相对于基坐标系的位置向量；
[0070]
步骤1.2：计算从1号坐标系的原点到6号坐标系的原点所确定的向量p
16
的值；
[0071]
在冗余机械臂末端相对于基座的位姿矩阵0t7给定的条件下，根据步骤1.1中式(2)，得到下式(3)：
[0072][0073]
式(3)中：d7表示6号坐标系和7号坐标系之间的连杆偏距；d1表示0号坐标系和1号坐标系之间的连杆偏距。
[0074]
本实施例中，上述步骤2具体为：
[0075]
参见图2，作ss
′
，ss
′
满足ss
′
平行于z4且ss
′
＝d3+0.5d4；作ww
′
，ww
′
满足ww
′
平行于z4且ww
′
＝d5+0.5d4；sw与s
′w′
相交于点o；上述z4表示4号坐标系的z轴在基坐标系中表示的单位向量；上述d3表示2号坐标系和3号坐标系之间的连杆偏距；上述d4表示3号坐标系和4号坐标系之间的连杆偏距；上述d5表示4号坐标系和5号坐标系之间的连杆偏距；将4号坐标系的原点定义为e点；第4个连杆的一端端面中心点与e点重合，另一端端面中心点为o4′
点；点e
′
为eo4′
的中点；定义平面s
′e′w′
为上述当前臂型面；
[0076]
参见图2，过上述点o作向量过上述点o作向量满足与z4平行且相等，将向量记为eo，则向量eo为上述当前臂型面的法向量，且有下式(4)成立：
[0077]
p
16
·eo
＝ss
′
+w
′
w＝d3+d4+d5(4)；
[0078]
参见图2，过点r作rd垂直sw于点d，向量记为ed，则ed即为上述当前臂型面的法向量在sw方向上的投影向量，则可以得出ed如下式(5)、式(6)所示：
[0079][0080][0081]
本实施例中，上述步骤3具体为：
[0082]
参见图2，定义向量与z1所组成的平面为上述参考平面，上述z1为1号坐标系的z轴在基坐标系中表示的单位向量；
[0083]
过步骤2中上述的点d，作上述参考平面的单位法向量记向量为e
r0
；向量z1＝[001]
t
，则可以得出上述参考平面的单位法向量如下式(7)所示：
[0084]
[0085]
本实施例中，上述步骤4具体为：
[0086]
参见图2，记向量为er，步骤3中参考平面的单位法向量e
r0
按照右手螺旋定则绕向量p
16
旋转ψ后与向量er重合；将ψ定义为当前臂型面与参考平面之间的臂型角；
[0087]
根据罗德里格斯旋转公式可知下式(8)：
[0088][0089]
则上述当前臂型面的法向量，即向量eo如下式(9)所示：
[0090]eo
＝ed+er(9)。
[0091]
本实施例中，上述步骤5具体为：
[0092]
根据冗余机械臂dh坐标系之间的关系和运动学方程可知0r3如下式(10)所示：
[0093][0094]
式(10)中：0r3表示第3个连杆相对于基座的姿态；ci＝cos(θi)，si＝sin(θi)，其中i取1或2；
[0095]
由冗余机械臂可知，向量z3和向量eo相等，因此，由上述式(10)可得下式(11)：
[0096]
z3＝eo＝[c1s2s1s
2-c2]
t
(11)；
[0097]
定义eo(j)表示向量eo第j个元素的值，则上述关节角2的值如下式(12)所示：
[0098]
θ2＝
±
arccos(eo(3))(12)。
[0099]
本发明中根据ssrms型机械臂自身的自运动特点，给出了一种新的臂型角的定义，在给定冗余机械臂的末端位姿的情况下，可根据臂型角参数求出该末端位姿下的逆运动学结果。
[0100]
通过上述求解过程可以看出，本发明所提出的方法是一种解析法，具有简单实用、几何意义明显、适用于臂型控制等优点，可通过控制臂型角参数的范围实现避障等特殊问题的处理，很好地解决了ssrms型机械臂的实时运动规划与控制问题，具有重要的意义。