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

权利要求书

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₀和一组两串联关节驱动变量β₁β₂。