CN111113425B - 一种有寄生运动的五自由度串并联机器人运动学逆解求解方法 - Google Patents

Abstract

本发明涉及一种有寄生运动的五自由度串并联机器人运动学逆解求解方法，所述方法建立一种三自由度并联机构等效机构，将原串并联机器人转化为由等效机构和两串联关节组成的五自由度串联机构，设末端刀具沿基坐标系Z轴旋转方向自由度为寄生运动，令寄生运动不参与计算，消减方程数量，通过解非线性方程组获得等效关节变量及串联关节驱动变量与末端刀具位置坐标及2个欧拉角的方程，最后利用向量解析法求取等效关节变量与三自由度并联机构驱动变量的关系，本方法能够在避免求取寄生运动与其他自由度间的复杂耦合关系的前提下，求解整体串并联机器人的逆解。

Description

一种有寄生运动的五自由度串并联机器人运动学逆解求解 方法

技术领域

本发明涉及一种机器人的运动学逆解求解方法，具体来说是一种有寄生运动的五自由度串并联机器人运动学逆解求解方法。

背景技术

串并联机器人，如专利CN108500953A所公开的一种含有动平台附加约束的五自由度精密串并联机器人，由并联机构和两单自由度旋转关节组成。相较于普通串联机器人，串并联机器人有刚度大、精度高、部署灵活等优点，在加工领域有很广泛的应用前景。

串并联机器人逆运动学求解是其投入实际使用前必须解决的问题，面临的困难包括以下两点：首先，由于串并联机器人结合了并联串联两种运动机构，目前常用的针对串联机器人DH方法和针对并联机器人的向量解析法均不直接适用。其次，传统机器人运动学逆解需要已知机器人末端沿六个自由度的运动，串并联机器人末端有五个独立自由度和一个寄生运动自由度，寄生运动自由度与其他自由度之间存在复杂的耦合关系，求取此耦合关系以期获取串并联机器人末端所有自由度上运动存在困难。

发明内容

针对现有技术存在的问题，本发明针对专利CN108500953A所公开的一种含有动平台附加约束的五自由度精密串并联机器人，提出了一种有寄生运动的五自由度串并联机器人运动学逆解求解方法，该方法能够在避免求取寄生运动与其他自由度间的复杂耦合关系的前提下，求解整体串并联机器人的逆解。

为实现上述目的，本发明所采用的技术方案是：

一种有寄生运动的五自由度串并联机器人运动学逆解求解方法，包括以下步骤：

步骤一、根据五自由度串并联机器人由三自由度并联机构和两个单自由度串联关节组成的结构特点，建立一种由一个三自由度关节和两个单自由度串联关节组成的等效机构，在等效机构的基座(0)、三自由关节(1)、第一串联关节(2)、第二串联关节(3)、末端刀具(4)依次建立坐标系{0}～坐标系{4}共五个坐标系；

步骤二、建立基座坐标系{0}与三自由度关节坐标系{1}之的关系，记为齐次变换矩阵

其中有等效关节变量θ、σ、z₀，依次建立坐标系{1}～坐标系{4}的关系，记为齐次变换矩阵形式记为

其中有串联关节驱动变量β₁、β₂；

步骤三、以坐标系{4}原点在坐标系{0}下的坐标值X、Y、Z和坐标系{4}在坐标系{0}下的ZYX欧拉角A，B，C定义坐标系{4}与坐标系{0}的关系，记为齐次变换矩阵

f₁₁(B，C)＝c(B)c(C)，f₁₂(B，C)＝-c(B)s(C)，f₁₃(B)＝s(B)，

f₂₁(A，B，C)＝c(A)s(C)+c(C)s(A)s(B)，f₂₂(A，B，C)＝c(A)c(C)-s(A)s(B)s(C)，

f₂₃(A，B)＝-c(B)s(A)，f₃₁(A，B，C)＝s(A)s(C)-c(A)c(C)s(B)，

f₃₂(A，B，C)＝c(C)s(A)+c(A)s(B)s(C)，f₃₃(A，B)＝c(A)c(B)

其中s()代表三角函数sin()，c()代表三角函数cos()，t()代表三角函数tan()；

步骤四、建立

与

的关系：

利用矩阵对应元素相等建立方程，将存在寄生运动自由度的方程剔除，形成一个由5个方程组成的非线性方程组并求解，方程中求解出的未知变量为三自由度关节变量θ、σ、Z₀和和两串联关节驱动变量β₁、β₂，求解时的已知变量为坐标系{4}原点在坐标系{0}下的坐标值X、Y、Z和坐标系{4}在坐标系{0}下的ZYX欧拉角A，B，C；

步骤五、采用向量解析法建立等效机构中三自由度关节变量θ、σ、z₀与原三自由度并联机构驱动变量q₁、q₂、q₃的关系，求出并联机构驱动变量q₁、q₂、q₃。

所述步骤一等效机构基座坐标系{0}与原机器人基座坐标系重合{0’}，三自由度关节坐标系{1}原点与原并联机构末端坐标系{1’}原点重合，三自由度关节与原并联机构末端具有一致的自由度，能够绕X1轴旋转、绕Y1轴旋转、沿Z0轴平移，沿X0轴有一寄生运动，能够平移，两单自由度旋转关节坐标系{2}{3}原点与原机器人两旋转关节坐标系{2’}{3’}原点重合，各轴方向一致。

所述步骤二包括以下步骤：

2.1、根据三自由度关节具有的自由度特点，建立坐标系{0}和坐标系{1}的关系：

矩阵中包括等效关节变量：绕Y1旋转角θ，绕X1旋转角σ，沿Z0平移量z₀；

2.2、根据标准DH机器人运动学建模法，依次建立坐标系{1}到坐标系{4}两两之间的关系，分别用其次变化矩阵

表示：

β_i-1为绕坐标系{i-1}Z轴，将坐标系{i-1}X轴旋转到与坐标系{i}X轴重合的角度；

d_i-1为沿坐标系{i-1}Z轴，将坐标系{i-1}X轴平移到与坐标系{i}X轴重合的距离；

α_i-1为沿坐标系{i}X轴，将坐标系{i-1}Z轴旋转到与坐标系{i}Z轴重合的角度；

a_i-1为沿坐标系{i}X轴，将坐标系{i-1}Z轴平移到与坐标系{i}Z轴重合的距离；

其中

中β₁为第一串联关节驱动变量，

中β₂为第二串联关节驱动变量。

所述步骤四包括以下步骤：

4.1、建立

与

的关系：

其中

g_ij()表示

相乘后矩阵第i行，第j列的元素，“()”表示此元素是关于“()”内变量的函数；

利用矩阵对应元素相等建立12个方程：

4.2、设末端刀具沿坐标系Z0轴旋转方向自由度为寄生运动，既变量C随X、Y、Z、A、B变化，且在求解过程中未知，令其不参与计算，从以上12个方程中中剔除含有C的方程和等效的方程，形成一个由5个方程组成的非线性方程组：

4.3、利用信赖域法数值求解以上非线性方程组，得到一组等效机构关节变量θσZ₀和一组两串联关节驱动变量β₁ β₂。

所述步骤五包括以下步骤：

5.1、建立向量

其中

由虎克铰U_i(i＝1～2)或旋转副R₃原点B_i(i＝1～3)指向坐标系{0’}的原点O₀，

由坐标系{0’}的原点O₀指向坐标系{1’}的原点O₁、

由坐标系{1’}的原点O₁指向旋转副R_i(i＝1～2)或球副S₁原点A_i(i＝1～3)，

由旋转副R_i(i＝1～2)或球副S₁原点A_i(i＝1～3)指向虎克铰U_i(i＝1～2)或旋转副R₃原点B_i(i＝1～3)，并建立向量之间关系：

5.2、将向量关系改写为由旋转矩阵R、平移向量

驱动变量q_i(i＝1～3)、单位向量

向量

向量

表示：R为坐标系{1’}相对于坐标系{0’}的旋转矩阵、

为坐标系{1’}相对于坐标系{0’}的平移向量、

为

的单位向量，

表示坐标系{0’}下向量

表示坐标系{1’}下向量

坐标系{1’}与坐标系{1}重合，坐标系{0’}与坐标系{0}重合，因此：

r＝[z₀t(θ)0 z₀]^T

5.3、将向量式两边平方标量化，求得并联机构驱动变量q₁、q₂、q₃：

本发明的与现有技术相比，具有的优点和积极效果主要有：

(1)本方法建立了一种由一个三自由度关节和两个单自由度旋转关节组成的等效机构，先求解等效机构关节变量，再求解并联机构实际驱动变量，解决了目前针对串联机器人DH方法和针对并联机器人的向量解析方法对串并联机器人求解不直接适用的问题。

(2)本方法设末端刀具绕Z₀轴旋转方向自由度为寄生运动，在数学运算过程中排除了含有寄生运动变量C，逆解时仅需输入末端刀具的3个位置变量和2个姿态变量，避免了在求解逆解前，先要计算寄生运动与其他自由度间的复杂耦合关系的问题，便于实际控制中实现。

附图说明

图1是本发明涉及的五自由度串并联机器人的实际结构示意图。

图2是本发明涉及的五自由度串并联机器人坐标系建立及等效机构坐标系建立示意图。

图3是本发明涉及的五自由度串并联机器人并联机构向量解析法求解并联机构驱动变量示意图。

图4是本发明算法流程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

参照图1所示，本发明参考的一种五自由度串并联机器人，包括转动平台1、第一杆2、第二杆3、第三杆4、动平台5、第一串联关节6、第二串联关节7；所述第一杆2和第二杆3结构相同，包括胡克铰8、旋转铰链9、杆体和平移驱动装置，所述第三杆4包括旋转铰链12、球铰11、杆体和平移驱动装置；旋转平台1，第一杆2、第二杆3、第三杆4、动平台5组成了并联机构，2个虎克铰8和旋转铰链12与一固定底座连接，所述第一杆2和第二杆3分别通过2个胡克铰8穿过并连接于转动平台1的两侧，所述第一杆2和第二杆3通过2个旋转铰链9对称地连接于动平台5的左右两侧，所述第三杆4通过所述球铰11连接于动平台5的后侧；所述的第二串联关节7通过转动铰链连接于所述第一串联关节6的内部中间位置，所述的第一串联关节6通过转动铰链连接于动平台5的下侧。

参照图2所示，一种有寄生运动的五自由度串并联机器人运动学逆解求解方法，包括以下步骤：

根据五自由度串并联机器人由三自由度并联机构和两单自由度串联关节组成的结构特点，建立一种由一个三自由度关节和两个单自由度串联关节组成的等效机构，在等效机构的基座(0)、三自由关节(1)、第一串联关节(2)、第二串联关节(3)、末端刀具(4)依次建立坐标系{0}～坐标系{4}共五个坐标系，如图3所示，等效机构基座坐标系{0}与原机器人基座坐标系重合{0’}，三自由度关节坐标系{1}原点与原并联机构末端坐标系{1’}原点重合，三自由度关节与原并联机构末端具有一致的自由度，可绕X1轴旋转、绕Y1轴旋转、沿Z0轴平移，沿X0轴有一寄生运动，可平移，两单自由度旋转关节坐标系{2}{3}原点与原机器人两旋转关节坐标系{2’}{3’}原点重合，各轴方向一致；

以上述建立的坐标系为基础，建立从{0}到{4}各坐标系间两两关系：

根据三自由度关节具有的自由度特点，建立坐标系{0}和坐标系{1}的关系，用齐次变换矩阵

表示：

其中s()代表三角函数sin()，c()代表三角函数cos()，t()代表三角函数tan()，后文均以此方法定义三角函数，矩阵中包括关节变量：绕坐标系{1}Y1轴旋转角θ，绕坐标系{1}X1轴旋转角σ，沿坐标系{0}Z0轴平移量z₀；

根据标准DH机器人运动学建模法，依次建立坐标系{1}到坐标系{4}两两之间的关系，分别用齐次变化矩阵

表示：

β_i-1为绕坐标系{i-1}Z轴，将坐标系{i-1}X轴旋转到与坐标系{i}X轴重合的角度；

d_i-1为沿坐标系{i-1}Z轴，将坐标系{i-1}X轴平移到与坐标系{i}X轴重合的距离；

α_i-1为沿坐标系{i}X轴，将坐标系{i-1}Z轴旋转到与坐标系{i}Z轴重合的角度；

a_i-1为沿坐标系{i}X轴，将坐标系{i-1}Z轴平移到与坐标系{i}Z轴重合的距离；

其中描述坐标系{1}与坐标系{2}关系的

为：

β₁为第一串联关节驱动变量，a₁为沿坐标系{2}X2轴，将坐标系{1}Z1轴平移到与坐标系{2}Z2轴重合的距离，d₁是沿坐标系{1}Z1轴，将坐标系{1}X1轴平移到与坐标系{2}X2轴重合的距离，两者均为结构参数，认为是常数；

其中描述坐标系{2}与坐标系{3}关系的

为：

β₂为第二串联关节驱动变量，a₂为沿坐标系{3}X3轴，将坐标系{2}Z2轴平移到与坐标系{3}Z3轴重合的距离，其为结构参数，认为是常数；

其中描述坐标系{3}与坐标系{4}关系的

为：

d₃为是沿坐标系{3}Z3轴，将坐标系{3}X3轴平移到与坐标系{4}X4轴重合的距离，其为结参数，认为是常数；

以建立的坐标系为基础，定义基座标系{0}与末端坐标系{4}的关系：

以坐标系{4}原点在坐标系{0}下的坐标值X、Y、Z和坐标系{4}在坐标系{0}下的ZYX欧拉角A，B，C定义坐标系{4}与坐标系{0}的关系，X、Y、Z分别是坐标系{4}原点在X0、Y0、Z0轴方向上的坐标值，A，B，C分别是坐标系{4}绕X0、Y0、Z0旋转的ZYX欧拉角，写成齐次变换矩阵记为

f₁₁(B，C)＝c(B)c(C)，f₁₂(B，C)＝-c(B)*s(C)，f₁₃(B)＝s(B)，

f₂₁(A，B，C)＝c(A)s(C)+c(C)s(A)s(B)，f₂₂(A，B，C)＝c(A)c(C)-s(A)s(B)s(C)，

f₂₃(A，B)＝-c(B)s(A)，f₃₁(A，B，C)＝s(A)s(C)-c(A)c(C)s(B)，

f₃₂(A，B，C)＝c(C)s(A)+c(A)s(B)s(C)，f₃₃(A，B)＝c(A)c(B)

根据先前建立的从{0}到{4}各坐标系间两两关系以及定义基座标系{0}与末端坐标系{4}的关系，建立

与

的关系：

其中

g₁₁(θσ β₁ β₂)＝c(β₂)(c(β₁)c(θ)+s(β₁)s(σ)s(θ))-s(β₂)c(σ)s(θ)

g₁₂(θσ β₁ β₂)＝c(β₁)s(σ)s(θ)-s(β₁)c(θ)

g₁₃(θσ β₁ β₂)＝s(β₂)(c(β₁)c(θ)+s(β₁)s(σ)s(θ))+c(β₂)c(σ)s(θ)

g₁₄(θσ z₀ β₁ β₂)

＝a₀c(θ)+d₄(s(β₂)(c(β₁)c(θ)+s(β₁)s(σ)s(θ))+c(β₂)c(σ)s(θ))+z₀t(θ)

+a₁c(β₁)c(θ)+d₁c(σ)s(θ)+a₂c(β₂)(c(β₁)c(θ)+s(β₁)s(σ)s(θ))

-a₂s(β₂)c(σ)s(θ)+a₁s(β₁)s(σ)s(θ)

g₂₁(σ β₁ β₂)＝s(β₂)s(σ)+c(β₂)s(β₁)c(σ)

g₂₂(σ β₁)＝c(β₁)c(σ)

g₂₃(σ β₁ β₂)＝s(β₁)s(β₂)c(σ)-c(β₂)s(σ)

g₂₄(θσ β₁ β₂)＝a₁s(β₁)c(σ)-d₁s(σ)-d₄(c(β₂)s(σ)-s(β₁)s(β₂)c(σ))+a₂s(β₂)s(σ)

+a₂c(β₂)s(β₁)c(σ)

g₃₁(θσ β₁ β₂)＝-c(β₂)(c(β₁)s(θ)-s(β₁)c(θ)s(σ))-s(β₂)c(σ)c(θ)

g₃₂(θσ β₁ β₂)＝s(β₁)s(θ)+c(β₁)c(θ)s(σ)

g₃₃(θσ β₁ β₂)＝c(β₂)c(σ)c(θ)-s(β₂)(c(β₁)s(θ)-s(β₁)c(θ)s(σ))

g₃₄(θσ z₀ β₁ β₂)

＝z₀-d₄(s(β₂)(c(β₁)s(θ)-s(β₁)c(θ)s(σ))-c(β₂)c(σ)c(θ))-a₀s(θ)

-a₁c(β₁)s(θ)+d₁c(σ)c(θ)-a₂c(β₂)(c(β₁)s(θ)-s(β₁)c(θ)s(σ))

-a₂s(β₂)c(σ)c(θ)+a₁s(β₁)c(θ)s(σ)

利用矩阵对应元素相等建立12个方程：

设末端刀具沿坐标系Z0轴旋转方向自由度为寄生运动，既变量C随X、Y、Z、A、B变化，且在求解过程中未知，令其不参与计算，从以上12个方程中中剔除含有C的方程和等效的方程，形成一个由5个方程组成的非线性方程组：

利用信赖域法数值求解以上非线性方程组，得到等效机构关节变量θσ Z₀和一组两串联关节驱动变量β₁ β₂；

根据上述步骤得到的等效机构关节变量θσ Z₀，求取并联机构驱动变量q₁、q₂、q₃：

参考图4，建立向量

其中

由虎克铰U_i(i＝1～2)或旋转副R₃原点B_i(i＝1～3)指向坐标系{0’}的原点O₀，

由坐标系{0’}的原点O₀指向坐标系{1’}的原点O₁、

由坐标系{1’}的原点O₁指向旋转副R_i(i＝1～2)或球副S₁原点A_i(i＝1～3)，

由旋转副R_i(i＝1～2)或球副S₁原点A_i(i＝1～3)指向虎克铰U_i(i＝1～2)或旋转副R₃原点B_i(i＝1～3)，并建立向量之间关系：

将向量关系改写为由旋转矩阵R、平移向量

长度既驱动变量q_i(i＝1～3)、单位向量

向量

向量

表示：R为坐标系{1’}相对于坐标系{0’}的旋转矩阵、

为坐标系{1’}相对于坐标系{0’}的平移向量、

为

的单位向量，

表示坐标系{0’}下向量

表示坐标系{1’}下向量

坐标系{1’}与坐标系{1}重合，坐标系{0’}与坐标系{0}重合，因此：

r＝[z₀t(θ)0 z₀]^T

将向量式两边平方标量化，求得并联机构驱动变量q₁、q₂、q₃：

Claims (3)

1.一种有寄生运动的五自由度串并联机器人运动学逆解求解方法，其特征在于：包括以下步骤：

步骤一、根据五自由度串并联机器人由三自由度并联机构和两个单自由度串联关节组成的结构特点，建立一种由一个三自由度关节和两个单自由度串联关节组成的等效机构，在等效机构的基座(0)、三自由关节(1)、第一串联关节(2)、第二串联关节(3)、末端刀具(4)依次建立坐标系{0}～坐标系{4}共五个坐标系；

步骤二、建立基座坐标系{0}与三自由度关节坐标系{1}之的关系，记为齐次变换矩阵

其中有等效关节变量θ、σ、z₀，依次建立坐标系{1}～坐标系{4}的关系，记为齐次变换矩阵形式记为

其中有串联关节驱动变量β₁、β₂；

步骤三、以坐标系{4}原点在坐标系{0}下的坐标值X、Y、Z和坐标系{4}在坐标系{0}下的ZYX欧拉角A，B，C定义坐标系{4}与坐标系{0}的关系，记为齐次变换矩阵

f₁₁(B,C)＝c(B)c(C)，f₁₂(B,C)＝-c(B)s(C)，f₁₃(B)＝s(B)，

f₂₁(A,B,C)＝c(A)s(C)+c(C)s(A)s(B)，f₂₂(A,B,C)＝c(A)c(C)-s(A)s(B)s(C)，

f₂₃(A,B)＝-c(B)s(A)，f₃₁(A,B,C)＝s(A)s(C)-c(A)c(C)s(B)，

f₃₂(A,B,C)＝c(C)s(A)+c(A)s(B)s(C)，f₃₃(A,B)＝c(A)c(B)

其中s()代表三角函数sin()，c()代表三角函数cos()，t()代表三角函数tan()；

步骤四、建立

与

的关系：

利用矩阵对应元素相等建立方程，将存在寄生运动自由度的方程剔除，形成一个由5个方程组成的非线性方程组并求解，方程中求解出的未知变量为三自由度关节变量θ、σ、Z₀和两串联关节驱动变量β₁、β₂，求解时的已知变量为坐标系{4}原点在坐标系{0}下的坐标值X、Y、Z和坐标系{4}在坐标系{0}下的ZYX欧拉角A，B，C；

步骤五、采用向量解析法建立等效机构中三自由度关节变量θ、σ、z₀与原三自由度并联机构驱动变量q₁、q₂、q₃的关系，求出并联机构驱动变量q₁、q₂、q₃；

所述步骤二包括以下步骤：

2.1、根据三自由度关节具有的自由度特点，建立坐标系{0}和坐标系{1}的关系：

矩阵中包括等效关节变量：绕Y1旋转角θ，绕X1旋转角σ，沿Z0平移量z₀；

2.2、根据标准DH机器人运动学建模法，依次建立坐标系{1}到坐标系{4}两两之间的关系，分别用其次变化矩阵

表示：

β_i-1为绕坐标系{i-1}Z轴，将坐标系{i-1}X轴旋转到与坐标系{i}X轴重合的角度；

d_i-1为沿坐标系{i-1}Z轴，将坐标系{i-1}X轴平移到与坐标系{i}X轴重合的距离；

α_i-1为沿坐标系{i}X轴，将坐标系{i-1}Z轴旋转到与坐标系{i}Z轴重合的角度；

a_i-1为沿坐标系{i}X轴，将坐标系{i-1}Z轴平移到与坐标系{i}Z轴重合的距离；

其中

中β₁为第一串联关节驱动变量，

中β₂为第二串联关节驱动变量；

所述步骤四包括以下步骤：

4.1、建立

与

的关系：

其中

g_ij()表示

相乘后矩阵第i行，第j列的元素，“()”表示此元素是关于“()”内变量的函数；

利用矩阵对应元素相等建立12个方程：

4.2、设末端刀具沿坐标系Z0轴旋转方向自由度为寄生运动，既变量C随X、Y、Z、A、B变化，且在求解过程中未知，令其不参与计算，从以上12个方程中剔除含有C的方程和等效的方程，形成一个由5个方程组成的非线性方程组：

4.3、利用信赖域法数值求解以上非线性方程组，得到一组等效机构关节变量θσZ₀和一组两串联关节驱动变量β₁β₂。

2.根据权利要求1所述有寄生运动的五自由度串并联机器人运动学逆解求解方法，其特征在于：所述步骤一等效机构基座坐标系{0}与原机器人基座坐标系重合{0’}，三自由度关节坐标系{1}原点与原并联机构末端坐标系{1’}原点重合，三自由度关节与原并联机构末端具有一致的自由度，能够绕X1轴旋转、绕Y1轴旋转、沿Z0轴平移，沿X0轴有一寄生运动，能够平移，两单自由度旋转关节坐标系{2}{3}原点与原机器人两旋转关节坐标系{2’}{3’}原点重合，各轴方向一致。

3.根据权利要求1所述有寄生运动的五自由度串并联机器人运动学逆解求解方法，其特征在于：所述步骤五包括以下步骤：

5.1、建立向量

其中

由虎克铰U_i(i＝1～2)或旋转副R₃原点B_i(i＝1～3)指向坐标系{0’}的原点O₀，

由坐标系{0’}的原点O₀指向坐标系{1’}的原点O₁、

由坐标系{1’}的原点O₁指向旋转副R_i(i＝1～2)或球副S₁原点A_i(i＝1～3)，

由旋转副R_i(i＝1～2)或球副S₁原点A_i(i＝1～3)指向虎克铰U_i(i＝1～2)或旋转副R₃原点B_i(i＝1～3)，并建立向量之间关系：

5.2、将向量关系改写为由旋转矩阵R、平移向量

驱动变量q_i(i＝1～3)、单位向量

向量

向量

表示：R为坐标系{1’}相对于坐标系{0’}的旋转矩阵、

为坐标系{1’}相对于坐标系{0’}的平移向量、

为

的单位向量，

表示坐标系{0’}下向量

表示坐标系{1’}下向量

坐标系{1’}与坐标系{1}重合，坐标系{0’}与坐标系{0}重合，因此：

r＝[z₀t(θ) 0 z₀]^T

5.3、将向量式两边平方标量化，求得并联机构驱动变量q₁、q₂、q₃：

Images