基于改进D-H方法的串联机械臂正向运动学建模方法与流程

基于改进d-h方法的串联机械臂正向运动学建模方法
技术领域
1.本发明涉及一种建模方法，尤其是一种基于改进d-h方法的串联机械臂正向运动学建模方法。


背景技术：

2.串联机械臂可以看作为一个开式运动链，它由一系列连杆通过转动或移动关节串联而成。为了研究操作臂各连杆之间的位移关系，可以在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。denavit和hartenberg提出一种通用的方法(即d-h方法)，用一个4
×
4的齐次变换矩阵描述相邻两连杆的空间关系，从而推导出“手爪坐标系”相对于“参考坐标系”的等价齐次变换矩阵，建立操作臂的正向运动学方程。
3.若相邻两个连杆i和连杆i-1由关节i相连，则每个连杆可以由四个d-h参数表示：
4.(1)a
i-1
表示连杆i-1的长度；
5.(2)α
i-1
表示连杆的i-1扭角；
6.(3)di表示两个连杆之间的距离，即a
i-1
与轴线i的交点到ai与轴线i的交点间的距离，沿轴线i的测量；
7.(4)θi表示连杆a
i-1
与ai之间的夹角。
8.为了确定机器人各连杆之间相对运动关系，在各连杆上分别固接一个坐标系。与基座标固接的坐标系标记为{0}，与连杆i固接的坐标系记为{i}，对于中间连杆：
9.坐标系{i-1}的z轴z
i-1
与关节轴i-1共线，指向任意；
10.坐标系{i-1}的x轴x
i-1
与关节轴i-1的公垂线重合，指向由关节i-1到关节i，当连杆i-1的长度a
i-1
为0时，坐标系{i-1}的x轴x
i-1
＝
±
zi×zi-1
；
11.坐标系{i-1}的y轴y
i-1
按右手法则规定，即y
i-1
＝z
i-1
×
x
i-1
。
12.对于首末连杆：
13.基坐标系{0}与基座固接，固定不动，原则上可以任意规定，末端连杆坐标系{n}的规定与基坐标系{0}相似。
14.根据所设定的连杆坐标系，相应的连杆参数可以定义如下：
15.(1)a
i-1
为从z
i-1
到zi沿x
i-1
测量的距离；
16.(2)α
i-1
为从z
i-1
到zi绕x
i-1
旋转的角度；
17.(3)di为从x
i-1
到xi沿zi测量的距离；
18.(4)θi为从x
i-1
到xi绕zi旋转的角度。
19.对于一些比较复杂串联机械臂，直接采用d-h方法描述不能完全描述坐标系{i-1}原点和坐标系{i}原点的相对位置关系。如：
20.(1)坐标系{i-1}的z轴z
i-1
为偏转关节，即z
i-1
竖直向上(下)，坐标系{i}的z轴zi为俯仰关节，即zi垂直纸面向里(外)，坐标系{i}的原点在坐标系{i-1}原点的上方，但不在坐标系{i-1}的z轴z
i-1
上。按照d-h方法，坐标系{i-1}的x轴x
i-1
为水平方向。此时，从z
i-1
到zi沿x
i-1
测量的距离a
i-1
，从x
i-1
到xi沿zi测量的距离di为0，a
i-1
和di不能完全描述坐标系{i-1}
原点和坐标系{i}原点的相对位置关系。
21.(2)坐标系{i-1}的z轴z
i-1
为偏转关节，即z
i-1
竖直向上(下)，坐标系{i}的z轴zi为水平移动关节，即zi为水平向右(左)，坐标系{i}的原点在坐标系{i-1}的z轴z
i-1
上，但是与坐标系{i-1}原点不重合。按照d-h方法，坐标系{i-1}的x轴x
i-1
方向垂直于纸面。此时，从z
i-1
到zi沿x
i-1
测量的距离a
i-1
为0，从x
i-1
到xi沿zi测量的距离di，a
i-1
和di不能完全描述坐标系{i-1}原点和坐标系{i}原点的相对位置关系。
22.针对串联机械臂运动学建模d-h方法的不足，本发明提出一种基于虚拟d-h坐标系的改进d-h方法，实现任意构型的串联机械臂运动学建模。


技术实现要素：

23.本发明的目的是针对传统d-h方法在一些比较复杂串联机械臂正向运动学建模时，不能完全描述坐标系{i-1}原点和坐标系{i}原点的相对位置关系的问题，提供一种基于改进d-h方法的串联机械臂正向运动学建模方法，该方法可以根据需要任意增设虚拟d-h坐标系，简化d-h参数测绘过程；同时该方法计算过程重复度高，便于编程运算；该方法适用于任意构型的串联机械臂正向运动学建模。
24.为实现上述目的，本发明采用下述技术方案：
25.一种基于改进d-h方法的串联机械臂正向运动学建模方法，包括：
26.步骤s1，基于传统d-h方法，建立串联机械臂连杆坐标系。坐标系{0}为基座，坐标系{1}为大臂偏转关节，坐标系{2}为大臂俯仰关节，坐标系{3}为大臂平移关节，坐标系{4}为小臂偏转关节，坐标系{5}为小臂回转关节，坐标系{6}为小臂平移关节，坐标系{7}为小臂俯仰关节，坐标系{8}为机械臂末端坐标系。坐标系{0}的z轴z0与坐标系{1}的z轴z1重合，为大臂偏转关节的旋转轴。坐标系{2}的z轴z2为大臂俯仰关节的旋转轴。坐标系{3}的z轴z3为大臂平移关节的平移方向，其坐标原点与坐标系{4}的坐标原点重合。坐标系{4}的z轴z4为小臂偏转关节的旋转轴。坐标系{5}的z轴z5为小臂回转关节的旋转轴。坐标系{6}的z轴z6为小臂平移关节的平移方向，其坐标原点与坐标系{7}的坐标原点重合。坐标系{7}的z轴z7为小臂俯仰关节的旋转轴。坐标系{8}的坐标原点位于执行器的末端。坐标系{0}的x轴x0指向执行器的末端，坐标系{8}的x轴x8方向与基座1坐标系{0}的x轴x0方向相同。中间坐标系均满足坐标系{i-1}的x轴x
i-1
坐标系{i-1}的x轴x
i-1
与关节轴i-1的公垂线重合，指向由关节i-1到关节i，i为1-8的整数，当连杆i-1的长度a
i-1
为0时，坐标系{i-1}的x轴x
i-1
＝
±
zi×zi-1
。
27.步骤s2，步骤1机械臂连杆坐标系，分析串联机械臂d-h参数，寻找d-h参数不能完全描述连杆坐标系相对位置的情况。
28.(1)坐标系{2}相对于坐标系{1}，从z1到z2沿x1测量的距离为l1，从x1到x2沿z2测量的距离为0。此时，按照d-h方法，坐标系{2}的原点与坐标系{1}的原点之间的距离d2没有体现。
29.(2)坐标系{5}相对于坐标系{4}，从z4到z5沿x4测量的距离为0，从x4到x5沿z5测量的距离为0。此时，按照d-h方法，坐标系{5}的原点与坐标系{4}的原点之间的距离d5没有体现。
30.步骤s3，根据步骤s2分析结果，插入虚拟d-h坐标系，形成改进的d-h坐标系。虚拟
为控制变量；
41.坐标系{6}相对于坐标系{5}，从z5到z6沿x5测量的距离为l5，从z5到z1绕x5旋转的角度为0，从x5到x6沿z6测量的距离为d6，从x5到x6绕z6旋转的角度为0，其中，d6为包含坐标系{6}原点相对于坐标系{5}原点初始距离的控制变量；
42.坐标系{7}相对于坐标系{6}，从z6到z7沿x6测量的距离为0，从z6到z7绕x6旋转的角度为从x6到x7沿z7测量的距离为0，从x6到x7绕z7旋转的角度为其中，θ7为控制变量。
43.坐标系{8}相对于坐标系{7}，从z7到z8沿x7测量的距离为l7，从z7到z8绕x7旋转的角度为从x7到x8沿z8测量的距离为d8，从x7到x8绕z8旋转的角度为0。
44.表1基于改进d-h坐标系的串联机械臂连杆参数
[0045][0046]
步骤s5，利用改进的d-h坐标系，将步骤s4中建立的基于改进d-h坐标系的串联机械臂连杆参数带入连杆坐标系{i}相对于{i-1}的变换通式t
ii-1
中，建立串联机械臂各关节相邻两连杆的坐变换矩阵
[0047]
其中，
[0048]
这里a
i-1
表示连杆长度，α
i-1
表示连杆扭角，di表示相邻两连杆之间的距离，θi表示相邻两连杆之间的夹角。
[0049]
大臂偏转关节相对于基座，即坐标系{1}相对于坐标系{0}的变换矩阵：
[0050]
大臂俯仰关节相对于大臂偏转关节，即坐标系{2}相对于坐标系{1}的变换矩阵：
[0051]
大臂平移关节相对于大臂俯仰关节，即坐标系{3}相对于坐标系{2}的变换矩阵：
[0052]
小臂偏转关节相对于大臂平移关节，即坐标系{4}相对于坐标系{3}的变换矩阵：
[0053]
小臂回转关节相对于小臂偏转关节，即坐标系{5}相对于坐标系{4}的变换矩阵：
[0054]
小臂平移关节相对于小臂回转关节，即坐标系{6}相对于坐标系{5}的变换矩阵：
[0055]
小臂俯仰关节相对于小臂平移关节，即坐标系{7}相对于坐标系{6}的变换矩阵：
[0056]
机械臂末端相对于小臂俯仰关节，即坐标系{8}相对于坐标系{7}的变换矩阵：
[0057]
步骤s6，在步骤s5的基础上，建立串联机械臂末端连杆坐标系相对于基座的坐标系{0}的描述，即串联机械臂正向运动学方程。
[0058][0059]
本发明中改进的d-h坐标系是指在原来的d-h坐标系基础上，插入虚拟坐标系{2a}、坐标系{5a}之后的坐标系。d-h方法是一种建立关节坐标系的方法，利用这种方法建立的坐标系称之为d-h坐标系，改进d-h方法是指，在原来的d-h坐标系的的基础上增加了虚拟坐标系，这样增加了传统d-h方法的适用范围。
[0060]
本发明相对于现有技术具有如下优点：
[0061]
(1)本发明提出的基于虚拟d-h坐标系的改进d-h方法，可以适用于任意构型的串联机械臂正向运动学建模；
[0062]
(2)本发明提出的基于虚拟d-h坐标系的改进d-h方法，可以根据需要任意增设虚拟d-h坐标系，简化d-h参数测绘过程；
[0063]
(3)本发明提出的基于虚拟d-h坐标系的改进d-h方法，计算过程重复度高，便于编程运算。
附图说明
[0064]
图1为本发明串联机械臂机构运动简图及对应关节坐标系示意图；图2为本发明串联机械臂d-h坐标系示意图；
[0065]
图3为本发明基于虚拟d-h坐标系的改进d-h坐标系示意图；
[0066]
图中，1.基座，2.大臂偏转关节，3.大臂俯仰关节，4.大臂平移关节，5.小臂偏转关节，6.小臂回转关节，7.小臂平移关节，8.小臂俯仰关节，9.机械臂末端。
具体实施方式
[0067]
下面结合附图和实施例对本发明进一步说明。
[0068]
本说明书附图所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容涵盖的范围内。同时，本
说明书中所引用的如“上”、“下”、“左”、“右”、“中间”及“一”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
[0069]
步骤s1，如图1所示，基于d-h方法，建立串联机械臂连杆坐标系。坐标系{0}为基座1，坐标系{1}为大臂偏转关节2，坐标系{2}为大臂俯仰关节3，坐标系{3}为大臂平移关节4，坐标系{4}为小臂偏转关节5，坐标系{5}为小臂回转关节6，坐标系{6}为小臂平移关节7，坐标系{7}为小臂俯仰关节8，坐标系{8}为机械臂末端9坐标系。坐标系{0}的z轴z0与坐标系{1}的z轴z1重合，为大臂偏转关节2的旋转轴。坐标系{2}的z轴z2为大臂俯仰关节3的旋转轴。坐标系{3}的z轴z3为大臂平移关节4的平移方向，其坐标原点与坐标系{4}的坐标原点重合。坐标系{4}的z轴z4为小臂偏转关节5的旋转轴。坐标系{5}的z轴z5为小臂回转关节6的旋转轴。坐标系{6}的z轴z6为小臂平移关节7的平移方向，其坐标原点与坐标系{7}的坐标原点重合。坐标系{7}的z轴z7为小臂俯仰关节8的旋转轴。坐标系{8}的坐标原点位于执行器(机械臂)的末端9。坐标系{0}的x轴x0指向执行器(机械臂)的末端9，坐标系{8}的x轴x8方向与基座1坐标系{0}的x轴x0方向相同。中间坐标系均满足坐标系{i-1}的x轴x
i-1
坐标系{i-1}的x轴x
i-1
与关节轴i-1的公垂线重合，指向由关节i-1到关节i，当连杆i-1的长度a
i-1
为0时，坐标系{i-1}的x轴x
i-1
＝
±
zi×zi-1
。
[0070]
步骤s2，根据图1所示机械臂连杆坐标系，分析串联机械臂d-h参数，寻找d-h参数不能完全描述连杆坐标系相对位置的情况，如图2所示。
[0071]
(1)坐标系{2}相对于坐标系{1}，从z1到z2沿x1测量的距离为l1，从x1到x2沿z2测量的距离为0。此时，按照d-h方法，坐标系{2}的原点与坐标系{1}的原点之间的距离d2没有体现。
[0072]
(2)坐标系{5}相对于坐标系{4}，从z4到z5沿x4测量的距离为0，从x4到x5沿z5测量的距离为0。此时，按照d-h方法，坐标系{5}的原点与坐标系{4}的原点之间的距离d5没有体现。
[0073]
步骤s3，如图3所示，根据步骤s2分析结果，插入虚拟d-h坐标系。坐标系{2a}为坐标系{1}和坐标系{2}之间的虚拟d-h坐标系,坐标系{5a}为坐标系{4}和坐标系{5}之间的虚拟d-h坐标系。虚拟d-h坐标系建立的目的是补充描述传统d-h方法不能完全描述的串联机械臂连杆参数，其中虚拟d-h坐标系z轴的方向可以任意选择，通常与串联机械臂相邻关节轴线垂直或者平行。对于由关节i相连的相邻两个连杆i和连杆i-1，当虚拟d-h坐标系的z轴与连杆i的z轴平行时，虚拟d-h坐标系的x轴用来补充描述传统d-h方法不能完全描述的串联机械臂连杆参数；当虚拟d-h坐标系的z轴与连杆i的z轴垂直时，虚拟d-h坐标系的z轴用来补充描述传统d-h方法不能完全描述的串联机械臂连杆参数。
[0074]
由于传统d-h方法中，坐标系{2}相对于坐标系{1}，从z1到z2沿x1测量的距离为l1，虚拟d-h坐标系{2a}相对于坐标系{1}，从z1到沿x1测量的距离应保持l1不变，且能够描述坐标系{2}的原点与坐标系{1}的原点之间的距离d2。因此虚拟d-h坐标系{2a}的坐标原点应位于x1轴上，且位于坐标系{2}原点的正下方。虚拟d-h坐标系{2a}z轴的方向与坐标系{2}z轴的方向平行，坐标系{2a}x轴的方向满足d-h方法定义，且能够包含坐标系{2}的原点与坐标系{1}的原点之间的距离d2，因此，坐标系{2a}x轴方向垂直向上。
[0075]
虚拟d-h坐标系{5a}的坐标原点与坐标系{5}重合，其z轴的方向与坐标系{5}z轴
的方向垂直，根据d-h方法定义，其x轴的方向垂直纸面向里(外)，虚拟d-h坐标系{5a}的z轴z
5a
为基础d-h坐标系{5}的原点与坐标系{4}的原点之间的距离d5的补充描述。
[0076]
步骤s4，如图3所示，在步骤s3的基础上，利用d-h方法分析含有虚拟d-h坐标系的串联机械臂连杆参数，如表1所示：
[0077]
坐标系{1}相对于坐标系{0}，从z0到z1沿x0测量的距离为0，从z0到z1绕x0旋转的角度为0，从x0到x1沿z1测量的距离为0，从x0到x1绕z1旋转的角度为π θ1，其中，θ1为控制变量；
[0078]
坐标系{2a}相对于坐标系{1}，从z1到沿x1测量的距离为l1，从z1到绕x1旋转的角度为从x1到沿测量的距离为0，从x1到绕旋转的角度为
[0079]
坐标系{2}相对于坐标系{2a}，从到z2沿测量的距离为d2，从到z2绕旋转的角度为0，从到x2沿z2测量的距离为0，从到x2绕z2旋转的角度为θ2，其中，θ2为控制变量；
[0080]
坐标系{3}相对于坐标系{2}，从z2到z3沿x2测量的距离为0，从z2到z3绕x2旋转的角度为从x2到x3沿z3测量的距离为d3，从x2到x3绕z3旋转的角度为其中，d3为包含坐标系{3}原点相对于坐标系{2}原点初始距离的控制变量；
[0081]
坐标系{4}相对于坐标系{3}，从z3到z4沿x3测量的距离为0，从z3到z4绕x3旋转的角度为从x3到x4沿z4测量的距离为0，从x3到x4绕z4旋转的角度为θ4，其中，θ4为控制变量；
[0082]
坐标系{5a}相对于坐标系{4}，从z4到沿x4测量的距离为0，从z4到绕x4旋转的角度为0，从x4到沿测量的距离为d5(d5＜0)，从x4到绕旋转的角度为0；
[0083]
坐标系{5}相对于坐标系{5a}，从到z5沿测量的距离为0，从到z5绕旋转的角度为从到x5沿z5测量的距离为0，从到x5绕z5旋转的角度为其中，θ5为控制变量；
[0084]
坐标系{6}相对于坐标系{5}，从z5到z6沿x5测量的距离为l5，从z5到z1绕x5旋转的角度为0，从x5到x6沿z6测量的距离为d6，从x5到x6绕z6旋转的角度为0，其中，d6为包含坐标系{6}原点相对于坐标系{5}原点初始距离的控制变量；
[0085]
坐标系{7}相对于坐标系{6}，从z6到z7沿x6测量的距离为0，从z6到z7绕x6旋转的角度为从x6到x7沿z7测量的距离为0，从x6到x7绕z7旋转的角度为其中，θ7为控制变量。
[0086]
坐标系{8}相对于坐标系{7}，从z7到z8沿x7测量的距离为l7，从z7到z8绕x7旋转的角度为从x7到x8沿z8测量的距离为d8，从x7到x8绕z8旋转的角度为0。
[0087]
表1基于改进d-h坐标系的串联机械臂连杆参数
[0088][0089]
步骤s5，利用改进的d-h坐标系，将步骤s4中建立的基于改进d-h坐标系的串联机械臂连杆参数带入连杆坐标系{i}相对于{i-1}的变换通式t
ii-1
中，建立串联机械臂各关节相邻两连杆的坐变换矩阵
[0090]
其中，这里a
i-1
表示连杆长度，α
i-1
表示连杆扭角，di表示相邻两连杆之间的距离，θi表示相邻两连杆之间的夹角。
[0091]
如图1所示，大臂偏转关节2相对于基座1，即坐标系{1}相对于坐标系{0}的变换矩阵
[0092]
大臂俯仰关节3相对于大臂偏转关节2，即坐标系{2}相对于坐标系{1}的变换矩阵
[0093]
大臂平移关节4相对于大臂俯仰关节3，即坐标系{3}相对于坐标系{2}的变换矩阵
[0094]
小臂偏转关节5相对于大臂平移关节4，即坐标系{4}相对于坐标系{3}的变换矩阵
[0095]
小臂回转关节6相对于小臂偏转关节5，即坐标系{5}相对于坐标系{4}的变换矩阵
[0096]
小臂平移关节7相对于小臂回转关节6，即坐标系{6}相对于坐标系{5}的变换矩阵
[0097]
小臂俯仰关节8相对于小臂平移关节7，即坐标系{7}相对于坐标系{6}的变换矩阵
[0098]
机械臂末端9相对于小臂俯仰关节8，即坐标系{8}相对于坐标系{7}的变换矩阵
[0099]
步骤s6，在步骤s5的基础上，建立串联机械臂末端9连杆坐标系相对于基座1的坐标系{0}的描述，即串联机械臂正向运动学方程。
[0100][0101]
实施例验证
[0102]
利用旋转算子rot(x,γ)，rot(y,β)，rot(z,α)和平移算子trans(p
x
,py,pz)，构造坐标系{i}相对于坐标系{i-1}的齐次坐标变换矩阵从而建立如图1所示串联机械臂正向运动学模型。其中，rot(x,γ)表示坐标系{i}绕坐标系{i-1}的x轴旋转的角度为γ，rot(y,β)表示坐标系{i}绕坐标系{i-1}的y轴旋转的角度为β，rot(z,α)表示坐标系{i}绕坐标系{i-1}的z轴旋转的角度为α，trans(p
x
,py,pz)表示坐标系{i}的原点相对于坐标系{i-1}的x轴、y轴、z轴的平移量分别为p
x
,py,pz。
[0103]
其中，平移算子
[0104]
旋转算子
[0105]
旋转算子
[0106]
旋转算子
[0107]
坐标系{i}相对于坐标系{i-1}的齐次坐标变换矩阵
[0108][0109]
如图1所示，大臂偏转关节2相对于基座1，即坐标系{1}相对于坐标系{0}的齐次变换矩阵：
[0110]
大臂俯仰关节3相对于大臂偏转关节2，即坐标系{2}相对于坐标系{1}的变换矩
阵:
[0111]
大臂平移关节4相对于大臂俯仰关节3，即坐标系{3}相对于坐标系{2}的变换矩阵：
[0112]
小臂偏转关节5相对于大臂平移关节4，即坐标系{4}相对于坐标系{3}的变换矩阵：
[0113]
小臂回转关节6相对于小臂偏转关节5，即坐标系{5}相对于坐标系{4}的变换矩阵：
[0114]
小臂平移关节7相对于小臂回转关节6，即坐标系{6}相对于坐标系{5}的变换矩阵：
[0115]
小臂俯仰关节8相对于小臂平移关节7，即坐标系{7}相对于坐标系{6}的变换矩阵
[0116]
机械臂末端9相对于小臂俯仰关节8，即坐标系{8}相对于坐标系{7}的变换矩阵
[0117]
综上，对于复杂构型的串联机械臂，采用本发明提出的基于虚拟d-h坐标系的d-h改进方法获得的坐标系{i}相对于坐标系{i-1}的位姿描述与采用齐次坐标变换矩阵获
得的结果一致，从而证明本发明提出方法的正确性。
[0118]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。