一种偏置机械臂逆运动学求解方法与流程

本发明属于机器人领域，具体的涉及一种偏置机械臂逆运动学求解方法。

背景技术：

为了满足机械臂灵活和避障的要求，机械臂往往具有冗余的自由度(如具有7自由度)；此外，为了满足机械加工以及负载的要求，具有冗余自由度的机械臂往往具有不同的构型。

图1为具有两7自由度机械臂的机器人，其中肩关节组10包括第一关节11、第二关节12和第三关节13；肘部只有一个关节，即第四关节20，也称肘关节；腕关节组30包括第五关节31、第六关节32和第七关节33；共7个关节，每个关节具有一个自由度，共7个自由度。

参考图2，第一关节11、第二关节12和第三关节13的关节旋转轴相交于一点，该点称为肩点Ps；第四关节20的坐标系原点称为肘点Pe；第五关节31、第六关节32和第七关节33的关节旋转轴不能相交于一点，将第五关节31和第六关节32的旋转轴交点称为第一腕点Pw1，第七关节33的坐标系原点称为第二腕点Pw2。第五关节31、第六关节32和第七关节33的关节旋转轴不能交于一点的机械臂称为偏置腕关节机械臂，在本发明中简称为偏置机械臂。

机械臂末端的位置不仅仅由肩点Ps和肘点Pe确定，腕点也会对机械臂末端的位置造成影响。对于具有球形腕关节(第五关节31、第六关节32和第七关节33旋转轴交于一点)的机械臂的基于位置的逆运动学的解析解已经得到了广泛的研究，最具代表性的是固定臂角法和固定关节法。

但是，固定臂角法要求机械臂具有球形腕关节，不适用于偏置腕关节机械臂。此外，固定关节法要求将其中一个关节角度固定，将其转换为6自由度关节进行计算；但是发现，无论固定哪一个关节，由于第五关节31、第六关节32和第七关节33的旋转轴不相交于一点，利用反变化法求解其逆运动学是非常复杂或者是不可行的,此外这种逆解往往难以优化，不能满足现有的控制要求。

技术实现要素：

本发明的目的在于针对现有技术中偏置机械臂逆运动学求解困难。

为实现上述目的，本发明提供一种偏置机械臂逆运动学求解方法，求解方法包括：

S100：根据偏置机械臂构造虚拟机械臂：

虚拟机械臂的第一关节至第六关节与偏置机械臂的第一关节至第六关节相同；虚拟机械臂的第七关节与偏置机械臂的第七关节角度相同，虚拟机械臂的第七关节位于第五关节和第六关节旋转轴交点处；

S200：利用固定臂角法求解虚拟机械臂的逆运动学解；

S300：利用第七关节的角度筛选出对应虚拟机械臂的逆运动学解，从而得到偏置机械臂的逆运动学解。

进一步，步骤S100还包括：

定义虚拟机械臂的第一关节、第二关节和第三关节称为肩关节组，肩关节组的旋转轴交于一点，称为肩点Ps；

定义虚拟机械臂第四关节称为肘关节，其坐标系原点称为肘点Pe；

定义虚拟机械臂第五关节、第六关节和第七关节称为腕关节组，腕关节组的旋转轴交点称为腕点Pw。

进一步，步骤S200包括：

(1)计算肘关节角度θ₄：

其中，L_sw是肩点Ps指向腕点Pw的向量，⁰L_sw是肩点Ps指向腕点Pw在基坐标下的向量；

L_se是肩点Ps指向肘点Pe的向量，L_ew是肘点Pe指向腕点Pw的向量；

(2)计算肩关节角度θ₁，θ₂，θ₃：

定义由肩点Ps、肘点Pe和腕点Pw组成的平面为手臂平面，定义θ₃＝π时的平面称为参考平面，臂角为手臂平面和参考平面间的夹角；

其中，

As_ij,Bs_ij和Cs_ij是As，Bs和Cs的第(i,j)元素；⁰R₃⁰是在参考平面上第三关节相对于基坐标的旋转矩阵；U_sw∈R³是向量L_sw的单位向量，[⁰U_sw×]是U_sw斜对称阵；

(3)计算腕关节角度θ₅，θ₆，θ₇：

其中，

Aw＝³R₄^T As^T0R₇；

Bw＝³R₄^T Bs^T0R₇；

Cw＝³R₄^T Cs^T0R₇；

Aw_ij，Bw_ij和Cw_ij是Aw，Bw和Cw的第(i，j)元素；³R₄是第四关节相对于第三关节的旋转矩阵；⁰R₇是第七关节相对于基坐标的旋转矩阵。

进一步，步骤S300为利用以下公式计算臂角然后求解虚拟机械臂的逆运动学解；

其中，

A＝Aw(3，2)-Aw(3，1)tanθ₇；

B＝Bw(3，2)-Bw(3，1)tanθ₇；

C＝Cw(3，2)-Cw(3，1)tanθ₇。

进一步，步骤S300之后还包括：

将逆运动学解带入关节运动范围以及关节速度评价函数，计算优化指标。

进一步，关节运动范围的评价函数为：

其中，

θ_imin,θ_imax是关节的下极限和上极限位置；

θ_i是求得的关节角度；

θ_cen是关节运动范围中心，θ_cen＝(θ_imin+θ_imax)/2；当有一个关节达到极限位置时，直接将这组解舍去；关节速度的评价函数为：

其中，

θ_i0是当前关节角度；

l(v_i)是每个关节的权值；

优化指标为：

L＝k₁L(θ)+k₂L(v)

其中，

k₁--避关节极限的权值；

k₂--避大范围关节运动权值。

进一步，计算优化指标后还包括：

改变第七关节角度，计算不同角度下的逆解及优化指标；

根据优化指标，找出最优解。

与现有技术相比，本技术方案具有以下优点：

本发明提供的技术方案能够快速的找出偏置机械臂的逆运动学解，且计算量小，速度快；该算法具有优化指标核算，优化指标能够根据实际使用进行配置，找出最优解。

附图说明

图1为具有两7自由度机械臂的机器人的示意图；

图2为现有技术中偏置机械臂的简化模型示意图；

图3为本发明实施例中偏置机械臂逆运动学求解方法的流程图；

图4为本发明实施例中虚拟机械臂的简化模型示意图；

图5为本发明实施例中虚拟机械臂的手臂平面和参考平面的示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

参考图3，本实施例提供一种偏置机械臂逆运动学求解方法，包括：

步骤S100，根据偏置机械臂构造虚拟机械臂：

步骤S200，利用固定臂角法求解虚拟机械臂的逆运动学解；

步骤S300，利用第七关节的角度筛选出对应虚拟机械臂的逆运动学解，从而得到偏置机械臂的逆运动学解。

本实施例所述的偏置机械臂包括7个关节，其中第一关节、第二关节和第三关节称为肩关节组，肩关节组的旋转轴交于一点，称为肩点Ps。

第四关节称为肘关节，其坐标系原点称为肘点Pe。

第五关节、第六关节和第七关节称为腕关节组，腕关节组的旋转轴不交于一点，其中第五关节和第六关节的旋转轴交点称为第一腕点Pw1；第七关节的坐标系原点称为第二腕点Pw2。

步骤S100构造的虚拟机械臂的第一关节至第六关节与偏置机械臂的第一关节至第六关节相同，即相关的参数均完全一样。

虚拟机械臂的第七关节与偏置机械臂的第七关节的角度相同，但虚拟机械臂的第七关节位于第五关节和第六关节旋转轴交点处。

参考图4，虚拟机械臂的第五、第六和第七关节旋转轴交于一点，即腕点Pw，虚拟机械臂的模型得到了简化。

假设虚拟机械臂是由偏置机械臂驱动的，即虚拟机械臂的各个关节角度始终与偏置机械臂的关节角度保持一致，则对于两机械臂有：

⁰H_kⁿ＝⁰H_k (k＝1，2...5) (式1)

是虚拟机械臂第k关节相对于基坐标系的齐次变换矩阵，⁰H_k是偏置机械臂第k关节相对于基坐标系的齐次变换矩阵；和⁰R_k是相应的旋转矩阵。

是虚拟机械臂第七关节相对于基坐标的位置，是虚拟机械臂第五关节相对于基坐标的位置，⁰P₅是偏置机械臂第五关节相对于基坐标的位置，⁰H₆是偏置机械臂第六关节相对于基坐标系的齐次变换矩阵，⁶P₅是坐标系5相对于坐标系6原点的向量。

⁰H₇是偏置机械臂第七关节相对于基坐标系的齐次变换矩阵，⁷T₆是第六关节相对于第七关节的转换矩阵。

通过以上表达式，可以得到：

通过以上等式可以发现，齐次变换矩阵由[θ₁，θ₂，θ₃，θ₄，θ₅，θ₆，θ₇]决定，其中θ_k(k＝1，2...7)表示第k关节的关节角度。

同时也可以发现，当⁰H₇与θ₇给定时，即可求出。如果偏置机械臂的位姿为⁰H₇，第七关节的角度为θ₇，那么虚拟机械臂的位姿必定为且第七关节的角度为θ₇。所以，若求得虚拟机械臂的位姿且第七关节的角度为θ₇的逆运动学解，那么该虚拟机械臂的逆运动学解必定是偏置机械臂的逆运动学解。

步骤S200中求解虚拟机械臂的逆运动学解的方法包括：

参考图5，根据固定臂角法，定义由肩点Ps、肘点Pe和腕点Pw组成的平面为手臂平面1，定义θ₃＝π时的平面称为参考平面2，臂角为手臂平面1和参考平面2间的夹角，向量L_sw是肩点Ps指向腕点Pw的向量,向量L_se是肩点Ps指向肘点Pe的向量，向量L_ew是肘点Pe指向腕点Pw的向量。此时可以将虚拟机械臂的运动看为：首先在参考平面2运动到指定位姿，然后沿着Ps-Pw轴旋转到手臂平面1。

⁰R_k⁰是在参考平面上第k关节相对于基坐标的旋转矩阵,⁰R_ψ是绕Ps-Pw轴的旋转矩阵，I₃∈R^3×3是单位矩阵，U_sw∈R³是向量L_sw的单位向量，[⁰U_sw×]是U_sw斜对称阵。

(1)肘关节角度的计算

当确定时,肘关节角度被确定，在由Pe，Pw和Ps构成的三角形中

⁰L_sw是肩点和腕点在基坐标下的向量。

(2)肩关节角度

肩关节角度由臂角来求得，首先确定在参考平面2处的肩关节角度。

是在参考平面上第三关节相对于基坐标的旋转矩阵，³L₂₃是第二关节和第三关节在第三坐标系下的向量，³R₄是第四关节到第三坐标系的旋转矩阵，⁴L₄₅是第四关节和第五关节在第四坐标系下的向量。

是在考平面2中第一关节、第二关节和第三关节的关节角度,是在考平面2中第一关节、第二关节和第三关节的关节角度相对于手臂平面1的旋转矩阵。

⁰R₃是第三关节相对于基坐标系的旋转矩阵，是臂角相对于基坐标系的旋转矩阵。

式中

由机器人的齐次变换矩阵可得

对比两个矩阵的元素，我们可以得出：

式中AS_ij，Bs_ij和Cs_ij--As，Bs和Cs的第(i，j)元素。

(3)腕关节角度

⁵R₇＝(³R₄)^T(⁰R₃)^T0R₇ (式17)

式中

Aw＝³R₄^T As^T0R₇

Bw＝³R₄^T Bs^T0R₇

Cw＝³R₄^T Cs^T0R₇

由机器人的齐次变换矩阵可得

(式19)

对比量矩阵的元素，可以得出

Aw_ij，Bw_ij和Cw_ij是Aw，Bw和Cw的第(i，j)元素

步骤S300，利用第七关节的角度筛选出对应虚拟机械臂的逆运动学解，从而得到偏置机械臂的逆运动学解的步骤包括：

步骤S200给出了臂角和关节角度间的关系，当末端的位姿确定时，但是在实际求解的过程中，虚拟机械臂末端的位姿是由偏置机械臂的末端位姿和第七关节的角度来确定的，所以当偏置机械臂的末端位姿和第七关节的角度给定后，臂角可以通过式23求解，继而求解虚拟机械臂的逆运动学解。

式中

A＝Aw(3，2)-Aw(3，1)tanθ₇

B＝Bw(3，2)-Bw(3，1)tanθ₇

C＝Cw(3，2)-Cw(3，1)tanθ₇。

在本实施例中，步骤S300之后还包括将逆运动学解带入关节运动范围以及关节速度优化函数，计算优化指标。

对于相同的末端位置和姿态，当改变第七关节的角度后，总会有一组解满足要求，这样就会存在无数多组解，这样就需要一个评价函数来对求出的逆解进行评价并筛选。

(1)避关节极限优化：

为了保证机械臂的灵活性，[θ_imin，θ_imax]关节的理想位置总是希望远离关节的极限位置。这样才能保证关节在两侧有足够的运动空间。为了满足该特性，提出如下的评价函数。

(式24)

式中

θ_imin,θ_imax--关节的下极限和上极限位置；

θ_i--求得的关节角度；

θ_cen--关节运动范围中心θ_cen＝(θ_imin+θ_imax)/2；

当有一个关节达到极限位置时，直接将这组解舍去。

(2)避大范围关节运动：

机械臂在运动过程中，首先将笛卡尔空间轨迹通过逆运动学转换到关节空间中，而笛卡尔空间轨迹的间隔时间是确定的，则关节空间中的运动时间与之保持一致，这就要求关节能够在短时间内从当前位置移动到目标位置，当关节转动角度过大时，则关节会产生电机超速行为。为了避免该情况，提出了如下的评价函数。

式中

θ_i0--当前关节角度

l(v_i)--每个关节的权值

则最终的优化目标函数为

L＝k₁L(θ)+k₂L(v) (式26)

式中

k₁--避关节极限的权值

k₂--避大范围关节运动权值。

本实施例提供的技术方案能够快速的找出偏置机械臂的逆运动学解，且计算量小，速度快；该算法具有优化指标核算，优化指标能够根据实际使用进行配置，找出最优解。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。