[发明专利]一种三自由度混联机械臂的运动学正解求解方法

权利要求书

1.一种三自由度混联机械臂的运动学正解求解方法，其特征在于，由于该三自由度混联机械臂的姿态与位置是耦合的，所述的三自由度混联机械臂的正解是指已知三移动副变量l₁、l₂、l₃的长度，求解末端参考点M的位置坐标^OP_M＝(x_M,y_M,z_M)^T；

根据所述机械臂的结构特点，建立求正解坐标系，其中，所述求正解坐标系，包括：基准坐标系{O}-Oxyz、与基座相连的坐标系{O₁}-O₁x₁y₁z₁、动坐标系{B}-Bx_By_Bz_B、动坐标系{G}-Gx_Gy_Gz_G、动坐标系{G₁}-G₁x_G1y_G1z_G1；

利用所述求正解坐标系，求解所述末端参考点M的位置坐标；

其中，所述基准坐标系与基座相连，O为A、C连线的中点，y轴沿AC指向C，BO⊥AC，x轴在BO延长线上，所述坐标系{O₁}-O₁x₁y₁z₁的原点相对所述基准坐标系平移至O₁点，O₁点为AD、CE延长线的交点O₂的轨迹形成的圆的圆心，所述动坐标系{B}-Bx_By_Bz_B由所述基准坐标系平移至B点，再将z轴旋转至大臂BG方向，并与大臂固联，所述动坐标系{G}-Gx_Gy_Gz_G由所述动坐标系{B}-Bx_By_Bz_B沿大臂BG平移至G点，并与BG杆固联，所述动坐标系{G}-Gx_Gy_Gz_G的y轴方向恰与G点转动副轴线重合，所述动坐标系{G₁}-G₁x_G1y_G1z_G1与小臂GM固联，A、B、C、D、E、F、G、H分别为各铰链中心点，A、C相对万向铰B对称布置，G点的位置随移动副变量l₁、l₂的长度变化而变化。

2.如权利要求1所述的一种三自由度混联机械臂的运动学正解求解方法，其特征在于，由于A、B、C与基座固定，因此A、B、C点在所述基准坐标系中的坐标已知。

3.如权利要求2所述的一种三自由度混联机械臂的运动学正解求解方法，其特征在于，通过以下方式求解动坐标系{B}-Bx_By_Bz_B相对所述基准坐标系的变换矩阵

假想将D、E两万向铰固联，与大臂BG解除约束，将AD、CE延长交于O₂，则AO₂、CO₂的长度为两移动副的运动学尺寸，记作则三脚架AO₂C只能绕AC轴线转动，点O₂的轨迹是以点O₁为圆心，以长度r为半径的圆，其中，r表示点O₁到点O₂之间的距离；

经计算，这些圆心O₁点的坐标：

同时得到：

以O₁为参考点建立与基座相连的坐标系{O₁}-O₁x₁y₁z₁，坐标系{O₁}-O₁x₁y₁z₁原点相对基准坐标系{O}-Oxyz平移至O₁点；

令O₁O₂与x₁轴正向夹角为则O₂在基座相连的坐标系{O₁}-O₁x₁y₁z₁下的坐标为：

由于基座相连的坐标系{O₁}-O₁x₁y₁z₁相对基准坐标系{O}-Oxyz的平移向量则O₂在基准坐标系{O}-Oxyz下的坐标为：

由于O₂B为定长，设O₂B长度为L，且B点坐标根据机构尺寸可知，则利用O₂B＝L可列机械臂的约束方程：

从而求出：

由于受机械臂运动范围所限，因此：

将(6)式和(7)式代入(3)式和(4)式，则O₂在基准坐标系{O}-Oxyz下的坐标可得；

建立动坐标系{B}-Bx_By_Bz_B，动坐标系{B}-Bx_By_Bz_B相对基准坐标系{O}-Oxyz的平移向量为：

^OP_BORG＝(X_B,Y_B,0)^T (8)

动坐标系{B}-Bx_By_Bz_B相对基准坐标系{O}-Oxyz的旋转变换矩阵可通过矢量BO₂＝(a,b,c)计算，其中：

旋转变换的实现步骤为：首先将BO₂绕x轴旋转α角到xz平面为BO₂’，再将BO₂’绕y轴旋转β角使之与z轴重合，其中：

则有

其中：

则动坐标系{B}-Bx_By_Bz_B相对基准坐标系{O}-Oxyz的变换矩阵为：

通过以下方式求解动坐标系{G}-Gx_Gy_Gz_G相对基准坐标系{O}-Oxyz的变换矩阵

建立动坐标系{G}-Gx_Gy_Gz_G，由于BG杆长是确定的，则动坐标系{G}-Gx_Gy_Gz_G相对动坐标系{B}-Bx_By_Bz_B的平移向量可知：

^BP_GORG＝(0,0,^BZ_G)^T (16)

则动坐标系{G}-Gx_Gy_Gz_G相对基准坐标系{O}-Oxyz的变换矩阵为：

通过以下方式求解末端参考点M的位置坐标：

在G点建立第二个动坐标系{G₁}-G₁x_G1y_G1z_G1与小臂GM固联，令机械臂的初始位姿为小臂GM与大臂BG互相垂直，则x_G1在小臂GM的延长线上，则机械臂末端参考点M在动坐标系{G₁}-G₁x_G1y_G1z_G1下的坐标为

通过∠FGH的角度变化求得动坐标系{G₁}-G₁x_G1y_G1z_G1相对动坐标系{G}-Gx_Gy_Gz_G的旋转角度；

将FH定为该移动副的运动学尺寸，令FH＝l₃，则根据l₃的变化可求得∠FGH的角度变化σ；

由于GF和GH的长度由机械臂确定，分别记为L_GF和L_GH，∠FGH＝θ，则cosθ和sinθ为：

将∠FGH在初始位姿时的角度定为θ₁，θ₁的值根据初始位姿为小臂GM与大臂BG互相垂直的特点确定，设l₃变化后∠FGH角度为θ₂，则

σ＝θ₂-θ₁ (20)

cos(σ)＝cosθ₂cosθ₁+sinθ₂sinθ₁ (21)

sin(σ)＝sinθ₂cosθ₁-cosθ₂sinθ₁ (22)

则动坐标系{G₁}-G₁x_G1y_G1z_G1相对动坐标系{G}-Gx_Gy_Gz_G的变换矩阵

由于该机械臂为三自由度机构，其末端姿态与位置是耦合的，末端姿态通过动坐标系{G₁}-G₁x_G1y_G1z_G1相对基准坐标系{O}-Oxyz的旋转矩阵可得，因此只需求解末端参考点M的坐标^OP_M＝(x_M,y_M,z_M)^T与输入量l₁、l₂、l₃的关系即可，从而得出该混联机械臂的正解：