CN105252548A - 不规则rpr、rp和pr型机械臂连杆坐标***的运动学性能分析方法 - Google Patents

Abstract

本发明提出了一种不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法，具体为：分辨不规则机械臂连杆中的旋转型杆件和伸缩型杆件；确定旋转型杆件旋转点和旋转轴、伸缩型杆件的伸缩点和伸缩轴，建立各个旋转点、伸缩点的坐标系；获得辅助关节点并建立辅助关节点坐标系；构建辅助关节点的D-H矩阵参数；推导出平移变换矩阵和旋转变换矩阵；将辅助关节点的D-H矩阵参数对应代入所述平移变换矩阵和旋转矩阵中，得到该不规则机械臂连杆的变换矩阵；将得到的变换矩阵***与该不规则机械臂连杆对应的前后规则连杆的坐标系中。该方法简单有效，能快速准确的完成对不规则机械臂连杆坐标***的建模，使其运动学分析成为可能。

Description

不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法

技术领域

本发明涉及连杆坐标***建模技术领域，具体涉及一种不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法。

背景技术

目前，在建立坐标变换方程时，把一系列的坐标系建立在连接连杆的关节上，用其次坐标变换来描述这些坐标之间的相对位置和方向，就可以建立起机器人的运动学方程，其中最常用的就是D-H参数法。由此可知，D-H参数法适用于标准的机械臂连杆结构建立坐标***，使其运动学分析成为可能。

然而，工程机械由于用途的特殊性，往往连杆设计独特并且不规则，这里的不规则是指当连杆包括R-P或者P-R型关节转换时，R型关节点不在P型的轴向上的情况，这种不规则类型通常分为RPR,RP和PR型，其中R为旋转自由度，P为伸缩自由度，不能直接利用D-H法建模，从而不能分析出其运动学性能。

发明内容

为了克服上述现有技术中存在的缺陷，本发明的目的是提供一种不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法，该方法简单有效，能快速准确的完成对不规则RPR、RP和PR型机械臂连杆坐标***的建模，从而使其运动学分析成为可能。

为了实现本发明的上述目的，本发明提供了一种不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法，包括以下步骤：

S1，分辨出不规则机械臂连杆中的旋转型杆件和伸缩型杆件；

S2，确定旋转型杆件旋转点和旋转轴、伸缩型杆件的伸缩点和伸缩轴，并建立各个旋转点、伸缩点的坐标系；

S3，由旋转型杆件的旋转点向伸缩型杆件伸缩轴做垂线，将得到的交点作为辅助关节点；

S4，设定辅助关节点自由度类型为旋转型自由度，建立辅助关节点坐标系；

S5，确定各旋转点到相应的辅助关节点之间的距离，以及辅助关节点到伸缩点之间的距离，确定各旋转型杆件的旋转角度，构建辅助关节点的D-H矩阵参数；

S6，推导出三维坐标系中平移变换矩阵和旋转变换矩阵；

S7，将辅助关节点的D-H矩阵参数对应代入所述平移变换矩阵和旋转矩阵中，得到该不规则机械臂连杆的变换矩阵；

S8，将得到的变换矩阵***与该不规则机械臂连杆对应的前后规则连杆的坐标系中，使得不规则连杆***变为规则连杆***；

S9，按照规则连杆***运动学性能分析方法完成该不规则机械臂连杆坐标***的运动学性能的分析。

分辨出不规则机械臂连杆中的旋转型杆件和伸缩型杆件时可以确定该不规则机械臂连杆中包含的不规则情况，是RPR型、RP型还是PR型。通过对不规则连杆辅助关节点的确定，从而建立机械臂的连杆坐标系模型。通过添加辅助关节点，不规则连杆***成为规则的连杆***，使用D-H法确定连杆参数，建立节点的转换矩阵，进而通过矩阵变换关系求解给定关节位置情况下连杆***末端工具坐标系相对于参考坐标系的位姿矩阵。其中，辅助关节点的D-H矩阵参数的构建采用现有公知的方法构建即可。同时，由旋转型杆件的旋转点向伸缩型杆件的伸缩轴做垂线，与伸缩轴的交点作为辅助关节点，这种添加辅助关节点的方式使得该旋转点、辅助关节点和伸缩点之间形成的角度为90度，这种直角关系可以使运动学矩阵解算过程更加简便。若取伸缩型杆件的伸缩轴上的其他点辅助点，由于角度不是90度特殊角度，因此解算过程较为繁琐。

该方法简单有效，能快速准确的完成对不规则RPR、RP和PR型机械臂连杆坐标***的建模。

进一步的，所述步骤S4包括以下步骤：

S4-1，设定辅助关节点自由度类型为旋转型自由度，设定其旋转轴，使其经过该辅助关节点，且平行于该辅助关节点相邻的旋转型杆件的旋转轴，将此旋转轴定为该辅助关节点坐标系的z轴；

S4-2，选取经过该辅助关节点，且平行或重合于该辅助关节点相邻的旋转型杆件的x轴的轴线为该辅助关节点坐标系的x轴；

S4-3，根据外积运算求解得到该辅助关节点坐标系的y轴，从而辅助关节点坐标系建立完成。

用这种方法建立的辅助关节点坐标系使得本方法中对辅助关节点的D-H矩阵参数的确定更加简单方便。

进一步的，所述步骤S6中推导出三维坐标系中平移变换矩阵和旋转变换矩阵的具体步骤为：

S6-1，假设从xyz坐标系变换到x’y’z’坐标系，需要沿着x轴平移d个单位，坐标系xyz的点变换到的x’y’z’坐标系以后变为： $\begin{array}{c}{x}^{\′}=x+0+0+d\\ y^{\′}=0+y+0+0\\ z^{\′}=0+0+z+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& d\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right],$

得到沿着x轴的平移变换矩阵 $T(x,d)=\left[\begin{array}{cccc}1& 0& 0& d\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

S6-2，根据S6-1中的方法可得到:

沿着y轴的平移变换矩阵 $T(y,d)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& d\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$

沿着y轴的平移变换矩阵 $T(z,d)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d\\ 0& 0& 0& 1\end{array}\right];$

S6-3，假设坐标系xyz绕z轴旋转θ，则xyz坐标系中的点坐标在旋转后的x’y’z’坐标系中的坐标根据几何投影关系确定为： $\begin{array}{c}{x}^{\′}=x\cos \mathrm{\θ}-y\sin \mathrm{\θ}+0+0\\ y^{\′}=x\sin \mathrm{\θ}+y\cos \mathrm{\θ}+0+0\\ z^{\′}=0+0+z+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\\ 1\end{array}\right]=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

得到沿着z轴的旋转变换矩阵 $R(z,\mathrm{\θ})=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

S6-4，根据S6-3中的方法可得到:

沿着x轴的旋转变换矩阵 $R(x,\mathrm{\θ})=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\left(\mathrm{\θ}\right)& -\sin \left(\mathrm{\θ}\right)& 0\\ 0& sin\left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)& 0\\ 0& 0& 0& 1\end{array}\right],$

沿着y轴的旋转变换矩阵 $R(y,\mathrm{\θ})=\left[\begin{array}{cccc}cos\left(\mathrm{\θ}\right)& 0& sin\left(\mathrm{\θ}\right)& 0\\ 0& 1& 0& 0\\ -\sin \left(\mathrm{\θ}\right)& 1& cos\left(\mathrm{\θ}\right)& 0\\ 0& 0& 0& 1\end{array}\right].$

该平移变换矩阵和旋转变换矩阵推算方法简单，准确。

本发明解决了工程机械中具有RPR/PR/RP型不规则连杆***的建模问题，只需根据辅助点选取规则添加合适的辅助点，便可以使用D-H法则建立坐标***模型，同时有直观的表达形式及明确的物理意义，方法简单高效。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1不规则机械臂连杆图；

图2坐标轴平移示意图；

图3坐标轴旋转示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明提供了一种不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法,包括以下步骤：

S1，分辨出不规则机械臂连杆中的旋转型杆件和伸缩型杆件。

S2，确定旋转型杆件旋转点和旋转轴、伸缩型杆件的伸缩点和伸缩轴，并建立各个旋转点、伸缩点的坐标系。

S3，由旋转型杆件的旋转点向伸缩型杆件伸缩轴做垂线，将得到的交点作为辅助关节点。

S4，设定辅助关节点自由度类型为旋转型自由度，建立辅助关节点坐标系。

具体包含三个步骤：第一步，设定辅助关节点自由度类型为旋转型自由度，设定其旋转轴，使其经过该辅助关节点，且平行于该辅助关节点相邻的旋转型杆件的旋转轴，将此旋转轴定为该辅助关节点坐标系的z轴。第二步，选取经过该辅助关节点，且平行或重合于该辅助关节点相邻的旋转型杆件的x轴的轴线为该辅助关节点坐标系的x轴。第三步，根据外积运算求解得到该辅助关节点坐标系的y轴，从而辅助关节点坐标系建立完成。

S5，确定各旋转点到相应的辅助关节点之间的距离，以及辅助关节点到伸缩点之间的距离，确定各旋转型杆件的旋转角度，构建辅助关节点的D-H矩阵参数。

S6，推导出三维坐标系中平移变换矩阵和旋转变换矩阵。

如图2-3所示，具体推导方法为：具体步骤为：

首先，假设从xyz坐标系变换到x’y’z’坐标系，需要沿着x轴平移d个单位，坐标系xyz的点变换到的x’y’z’坐标系以后变为： $\begin{array}{c}{x}^{\′}=x+0+0+d\\ y^{\′}=0+y+0+0\\ z^{\′}=0+0+z+0\\ 1=0+0+0+1\end{array},$ 将该式写成矩阵形式为： $\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& d\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right],$ 得到沿着x轴的平移变换矩阵 $T(x,d)=\left[\begin{array}{cccc}1& 0& 0& d\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

然后，根据上述方法可得到:

沿着y轴的平移变换矩阵 $T(y,d)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& d\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ 和沿着y轴的平移变换矩阵 $T(z,d)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d\\ 0& 0& 0& 1\end{array}\right].$

再假设坐标系xyz绕z轴旋转θ，则xyz坐标系中的点坐标在旋转后的x’y’z’坐标系中的坐标根据几何投影关系确定为： $\begin{array}{c}{x}^{\′}=x\cos \mathrm{\θ}-y\sin \mathrm{\θ}+0+0\\ y^{\′}=x\sin \mathrm{\θ}+y\cos \mathrm{\θ}+0+0\\ z^{\′}=0+0+z+0\\ 1=0+0+0+1\end{array},$ 将该式写成矩阵形式为： $\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\\ 1\end{array}\right]=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& cos\mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right],$ 得到沿着z轴的旋转变换矩阵 $R(z,\mathrm{\θ})=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

最后，根据上述方法可得到:

沿着x轴的旋转变换矩阵 $R(x,\mathrm{\θ})=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\left(\mathrm{\θ}\right)& -\sin \left(\mathrm{\θ}\right)& 0\\ 0& sin\left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)& 0\\ 0& 0& 0& 1\end{array}\right],$

沿着y轴的旋转变换矩阵 $R(y,\mathrm{\θ})=\left[\begin{array}{cccc}cos\left(\mathrm{\θ}\right)& 0& sin\left(\mathrm{\θ}\right)& 0\\ 0& 1& 0& 0\\ -\sin \left(\mathrm{\θ}\right)& 1& cos\left(\mathrm{\θ}\right)& 0\\ 0& 0& 0& 1\end{array}\right].$

S7，将辅助关节点的D-H矩阵参数对应代入所述平移变换矩阵和旋转矩阵中，得到该不规则机械臂连杆的变换矩阵。

S8，将得到的变换矩阵***与该不规则机械臂连杆对应的前后规则连杆的坐标系中，使得不规则连杆***变为规则连杆***。

S9，按照规则连杆***运动学性能分析方法完成该不规则机械臂连杆坐标***的运动学性能的分析。

通过矩阵变换，包括旋转和平移变换，便可以将原点的坐标及姿态即三个轴向，根据各个连杆的自由度的大小，变换到连杆***的末端，完成运动学正解。由于不规则连杆***经过S1-S8已经变规则，此时连杆***的反解过程和规则连杆***的解法一致，而规则连杆***的运动学性能分析方法为现有方法，此处不再赘述。

以不规则RPR型机械臂连杆为例，如图1所示，确定连杆L1为旋转型杆件，旋转点为A点，旋转轴为z2，连杆L2为伸缩型杆件，伸缩轴为z4，伸缩点为E点，连杆L3为旋转型杆件，旋转点为D点，旋转轴为z6，并建立旋转点A点的坐标***(x2，y2，z2)，伸缩点E点的坐标***(x4，y4，z4)，旋转点D点的坐标***(x6，y6，z6)。

连杆L1到连杆L2的转换就是R型到P型的转换，L2到L3的转换就是P型到R型的转换，并且旋转点A点和D点都不在伸缩轴z4上。

添加辅助点：如图1所示，由A点向z4轴做垂线，交点为B点，B点即为R型自由度到P型自由度转换过程添加的辅助关节点。同理由D点向z4轴做垂线，交点为C点，C点即为P型自由度到R型自由度转换过程中添加的辅助关节点。采用此种方式的添加的辅助关节点使∠ABE和∠ECD成90度，直角关系可以使运动学矩阵解算过程更加简便。若取P型自由度伸缩轴上的其他点辅助点，由于角度不是90度特殊角度，因此解算过程较为繁琐。

建立辅助关节点坐标系：将辅助关节点自由度类型设定为旋转型自由度，旋转轴的选取规则为：经过辅助关节点，并且和相邻的旋转自由度转轴平行。对于辅助关节点B点，设定旋转轴z3经过B点，并且平行于旋转轴z2。对于辅助点C，设定旋转轴z5经过C点，并且平行于旋转轴z6。

辅助关节点的x轴确定规则为：选取和相邻R型自由度的x轴平行或重合的轴作为x轴向。由此得到辅助关节点B点坐标系的x轴向x3轴和辅助关节点C点坐标系的x轴向x5，其中x3轴平行或重合于x2轴，x5轴平行或重合于x6轴。

通过向量运算——外积运算求解得到辅助关节点B点坐标系的y轴向和辅助关节点C点坐标系的y轴，至此完成辅助关节点B点的坐标系(x3，y3，z3)和C点坐标系(x5，y5，z5)的建立。

确定辅助关节点的D-H矩阵参数：由于添加的辅助关节点的坐标系的坐标轴均垂直于伸缩轴的坐标轴，即夹角为90°，设AB长度为d1，CD长度为d2，BE长度为p1，EC长度为p2，A点自由度的旋转角为θ2，D点自由度的旋转角为θ3，则构建的A点坐标系(x2，y2，z2)到E点坐标系(x4，y4，z4)以及E点坐标系(x4，y4，z4)到D坐标系(x6，y6，z6)的D-H矩阵参数，如表1所示：

i	αi	ai	di	θi
					2	0	d1	0	θ2
3	90	0	p1	0
					4	-90	0	p2	0
5	0	d2	0	θ3

表1

表1中，(αi，ai)的αi表示绕xi轴正向旋转的角度，ai表示沿xi轴正向平移的距离，(di,θi)的θi表示绕zi轴正向旋转的角度，di表示沿zi轴正向平移的距离。

进而得到A点坐标系(x2，y2，z2)到B点坐标系(x3，y3，z3)的平移变换矩阵 $T(x2,d1)=\left[\begin{array}{cccc}1& 0& 0& d1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(z2,\mathrm{\θ}2)=\left[\begin{array}{cccc}cos\mathrm{\θ}2& sin\mathrm{\θ}2& 0& 0\\ -sin\mathrm{\θ}2& cos\mathrm{\θ}2& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

B点坐标系(x3，y3，z3)到E点坐标系(x4，y4，z4)的平移变换矩阵 $T(z3,p1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& p1\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(x3,90)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\left(90\right)& -sin\left(90\right)& 0\\ 0& sin\left(90\right)& \cos \left(90\right)& 0\\ 0& 0& 0& 1\end{array}\right].$

E点坐标系(x4，y4，z4)到C点坐标系(x5，y5，z5)的平移变换矩阵 $T(z4,p2)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& p2\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(x4,-90)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos(-90)& -sin(-90)& 0\\ 0& sin(-90)& \cos (-90)& 0\\ 0& 0& 0& 1\end{array}\right].$

C点坐标系(x5，y5，z5)到D点坐标系(x6，y6，z6)的平移变换矩阵 $T(x5,-d2)=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(z5,\mathrm{\θ}3)=\left[\begin{array}{cccc}cos\mathrm{\θ}3& sin\mathrm{\θ}3& 0& 0\\ -\sin \mathrm{\θ}3& \cos \mathrm{\θ}3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

从而得到A点坐标系(x2，y2，z2)到B点坐标系(x3，y3，z3)的变换矩阵A2＝R(z2,θ2)*T(x2,d1)。

B点坐标系(x3，y3，z3)到E点坐标系(x4，y4，z4)的变换矩阵A3＝R(x3,90)*T(z3,p1)。

E点坐标系(x4，y4，z4)到C点坐标系(x5，y5，z5)的平移变换矩阵A4＝T(z4,p2)*R(x4,-90)。

C点坐标系(x5，y5，z5)到D点坐标系(x6，y6，z6)的平移变换矩阵A5＝T(x5,-d2)*R(z5,θ3)。

至此得到辅助关节点自由度之间的全部变换矩阵，将这些变换矩阵***前后规则连杆的坐标系中就可以完成连杆坐标系的建模，然后按照规则连杆***运动学性能分析方法完成该不规则机械臂连杆坐标***的运动学性能的分析。其中，规则连杆坐标系可以通过D-H方法进行构建，为现有技术此处不再赘述。

当不规则机械臂连杆为RP型时，其建模方法为:如图1中所示连杆L1到连杆L2，确定连杆L1为旋转型杆件，旋转点为A点，旋转轴为z2，连杆L2为伸缩型杆件，伸缩轴为z4，伸缩点为E点，并建立旋转点A点的坐标***(x2，y2，z2)，伸缩点E点的坐标***(x4，y4，z4)。

连杆L1到连杆L2的转换就是R型到P型的转换，并且旋转点A不在伸缩轴z4上。

添加辅助点：如图1所示，由A点向z4轴做垂线，交点为B点，B点即为R型自由度到P型自由度转换过程添加的辅助关节点。

建立辅助关节点坐标系：对于辅助关节点B点，设定旋转轴z3经过B点，并且平行于旋转轴z2。确定辅助关节点B点坐标系的x轴向x3轴，其中x3轴平行或重合于x2轴。通过向量运算——外积运算求解得到辅助关节点B点坐标系的y轴向，至此完成辅助关节点B点的坐标系(x3，y3，z3)的建立。

确定辅助关节点的D-H矩阵参数：由于添加的辅助关节点的坐标系的坐标轴均垂直于伸缩轴的坐标轴，即夹角为90°，设AB长度为d1，BE长度为p1，A点自由度的旋转角为θ2，则构建的A点坐标系(x2，y2，z2)到E点坐标系(x4，y4，z4)的D-H矩阵参数，如表2所示：

i	αi	ai	di	θi
					2	0	d1	0	θ2
3	90	0	p1	0

表2

表2中，(αi，ai)的αi表示绕xi轴正向旋转的角度，ai表示沿xi轴正向平移的距离，(di,θi)的θi表示绕zi轴正向旋转的角度，di表示沿zi轴正向平移的距离。

进而得到A点坐标系(x2，y2，z2)到B点坐标系(x3，y3，z3)的平移变换矩阵 $T(x2,d1)=\left[\begin{array}{cccc}1& 0& 0& d1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(z2,\mathrm{\θ}2)=\left[\begin{array}{cccc}cos\mathrm{\θ}2& sin\mathrm{\θ}2& 0& 0\\ -sin\mathrm{\θ}2& cos\mathrm{\θ}2& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

B点坐标系(x3，y3，z3)到E点坐标系(x4，y4，z4)的平移变换矩阵 $T(z3,p1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& p1\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(x3,90)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\left(90\right)& -sin\left(90\right)& 0\\ 0& sin\left(90\right)& \cos \left(90\right)& 0\\ 0& 0& 0& 1\end{array}\right].$

从而得到A点坐标系(x2，y2，z2)到B点坐标系(x3，y3，z3)的变换矩阵A2＝R(z2,θ2)*T(x2,d1)。

B点坐标系(x3，y3，z3)到E点坐标系(x4，y4，z4)的变换矩阵A3＝R(x3,90)*T(z3,p1)。

至此得到辅助关节点自由度之间的全部变换矩阵，将这些变换矩阵***前后规则连杆的坐标系中就可以完成连杆坐标系的建模，然后按照规则连杆***运动学性能分析方法完成该不规则机械臂连杆坐标***的运动学性能的分析。其中，规则连杆坐标系可以通过D-H方法进行构建，为现有技术此处不再赘述。

同样的，可以得到，当不规则机械臂连杆为PR型时，如图1中所示连杆L2到连杆L3，确定连杆L2为伸缩型杆件，伸缩轴为z4，伸缩点为E点，连杆L3为旋转型杆件，旋转点为D点，旋转轴为z6，并建立伸缩点E点的坐标***(x4，y4，z4)，旋转点D点的坐标***(x6，y6，z6)。

L2到L3的转换就是P型到R型的转换，并且旋转点D点不在伸缩轴z4上。

添加辅助点：如图1所示，由D点向z4轴做垂线，交点为C点，C点即为P型自由度到R型自由度转换过程中添加的辅助关节点。

建立辅助关节点坐标系：对于辅助点C，设定旋转轴z5经过C点，并且平行于旋转轴z6。确定辅助关节点C点坐标系的x轴向x5轴，x5轴平行或重合于x6轴。通过向量运算——外积运算求解得到辅助关节点C点坐标系的y轴，至此完成C点坐标系(x5，y5，z5)的建立。

确定辅助关节点的D-H矩阵参数：由于添加的辅助关节点的坐标系的坐标轴均垂直于伸缩轴的坐标轴，即夹角为90°，设CD长度为d2，EC长度为p2，D点自由度的旋转角为θ3，则构建的E点坐标系(x4，y4，z4)到D坐标系(x6，y6，z6)的D-H矩阵参数，如表3所示：

i	αi	ai	di	θi
					4	-90	0	p2	0
5	0	d2	0	θ3

表3

表3中，(αi，ai)的αi表示绕xi轴正向旋转的角度，ai表示沿xi轴正向平移的距离，(di,θi)的θi表示绕zi轴正向旋转的角度，di表示沿zi轴正向平移的距离。

E点坐标系(x4，y4，z4)到C点坐标系(x5，y5，z5)的平移变换矩阵 $T(z4,p2)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& p2\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(x4,-90)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos(-90)& -sin(-90)& 0\\ 0& \sin (-90)& \cos (-90)& 0\\ 0& 0& 0& 1\end{array}\right].$

C点坐标系(x5，y5，z5)到D点坐标系(x6，y6，z6)的平移变换矩阵 $T(x5,-d2)=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ 旋转变换矩阵 $R(z5,\mathrm{\θ}3)=\left[\begin{array}{cccc}cos\mathrm{\θ}3& sin\mathrm{\θ}3& 0& 0\\ -sin\mathrm{\θ}3& cos\mathrm{\θ}3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

E点坐标系(x4，y4，z4)到C点坐标系(x5，y5，z5)的平移变换矩阵A4＝T(z4,p2)*R(x4,-90)。

C点坐标系(x5，y5，z5)到D点坐标系(x6，y6，z6)的平移变换矩阵A5＝T(x5,-d2)*R(z5,θ3)。

至此得到辅助关节点自由度之间的全部变换矩阵，将这些变换矩阵***前后规则连杆的坐标系中就可以完成连杆坐标系的建模，然后按照规则连杆***运动学性能分析方法完成该不规则机械臂连杆坐标***的运动学性能的分析。其中，规则连杆坐标系可以通过D-H方法进行构建，为现有技术此处不再赘述。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (3)

1.一种不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法，其特征在于，包括以下步骤：

S1，分辨出不规则机械臂连杆中的旋转型杆件和伸缩型杆件；

S2，确定旋转型杆件旋转点和旋转轴、伸缩型杆件的伸缩点和伸缩轴，并建立各个旋转点、伸缩点的坐标系；

S3，由旋转型杆件的旋转点向伸缩型杆件伸缩轴做垂线，将得到的交点作为辅助关节点；

S4，设定辅助关节点自由度类型为旋转型自由度，建立辅助关节点坐标系；

S5，确定各旋转点到相应的辅助关节点之间的距离，以及辅助关节点到伸缩点之间的距离，确定各旋转型杆件的旋转角度，构建辅助关节点的D-H矩阵参数；

S6，推导出三维坐标系中平移变换矩阵和旋转变换矩阵；

S7，将辅助关节点的D-H矩阵参数对应代入所述平移变换矩阵和旋转矩阵中，得到该不规则机械臂连杆的变换矩阵；

S8，将得到的变换矩阵***与该不规则机械臂连杆对应的前后规则连杆的坐标系中，使得不规则连杆***变为规则连杆***；

S9，按照规则连杆***运动学性能分析方法完成该不规则机械臂连杆坐标***的运动学性能的分析。

2.根据权利要求1所述的不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法，其特征在于，所述步骤S4包括以下步骤：

S4-1，设定辅助关节点自由度类型为旋转型自由度，设定其旋转轴，使其经过该辅助关节点，且平行于该辅助关节点相邻的旋转型杆件的旋转轴，将此旋转轴定为该辅助关节点坐标系的z轴；

S4-2，选取经过该辅助关节点，且平行或重合于该辅助关节点相邻的旋转型杆件的x轴的轴线为该辅助关节点坐标系的x轴；

S4-3，根据外积运算求解得到该辅助关节点坐标系的y轴，从而辅助关节点坐标系建立完成。

3.根据权利要求1所述的不规则RPR、RP和PR型机械臂连杆坐标***的运动学性能分析方法，其特征在于，所述步骤S6中推导出三维坐标系中平移变换矩阵和旋转变换矩阵的具体步骤为：

S6-1，假设从xyz坐标系变换到x’y’z’坐标系，需要沿着x轴平移d个单位，坐标系xyz的点变换到的x’y’z’坐标系以后变为： $\begin{array}{c}{x}^{\′}=x+0+0+d\\ y^{\′}=0+y+0+0\\ z^{\′}=0+0+z+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& d\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right],$

得到沿着x轴的平移变换矩阵 $T(x,d)=\left[\begin{array}{cccc}1& 0& 0& d\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

S6-2，根据S6-1中的方法可得到:

沿着y轴的平移变换矩阵 $T(y,d)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& d\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$

沿着y轴的平移变换矩阵 $T(z,d)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d\\ 0& 0& 0& 1\end{array}\right];$

S6-3，假设坐标系xyz绕z轴旋转θ，则xyz坐标系中的点坐标在旋转后的x’y’z’坐标系中的坐标根据几何投影关系确定为： $\begin{array}{c}{x}^{\′}=x\cos \mathrm{\θ}-y\sin \mathrm{\θ}+0+0\\ y^{\′}=x\sin \mathrm{\θ}+y\cos \mathrm{\θ}+0+0\\ z^{\′}=0+0+z+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\\ 1\end{array}\right]=\left[\begin{array}{cccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

得到沿着z轴的旋转变换矩阵 $R(z,\mathrm{\θ})=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

S6-4，根据S6-3中的方法可得到:

沿着x轴的旋转变换矩阵 $R(x,\mathrm{\θ})=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\left(\mathrm{\θ}\right)& -\sin \left(\mathrm{\θ}\right)& 0\\ 0& sin\left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)& 0\\ 0& 0& 0& 1\end{array}\right],$

沿着y轴的旋转变换矩阵 $R(y,\mathrm{\θ})=\left[\begin{array}{cccc}cos\left(\mathrm{\θ}\right)& 0& sin\left(\mathrm{\θ}\right)& 0\\ 0& 1& 0& 0\\ -\sin \left(\mathrm{\θ}\right)& 1& cos\left(\mathrm{\θ}\right)& 0\\ 0& 0& 0& 1\end{array}\right].$