[发明专利]基于几何法的工业机器人逆解算法

权利要求书

1.基于几何法的工业机器人逆解算法，其特征在于，包括以下步骤：

步骤S11，建立基坐标系，根据标准D-H表示法对机器人进行建模，获得机器人的标准D-H参数；

步骤S12，根据预先规划的末端位姿获取机器人在满足该末端位姿下的当前构型的机器人各关节的关节角；

根据预先规划的末端位姿获取机器人各关节的关节角的方法包括：

步骤S21，分析机器人的几何机构和投影关系，求解逆运动学解的第一关节的关节角θ₁；

步骤S22，在获得θ₁的情况下，机器人的大臂和小臂简化为三角形，求解逆运动学解的第二关节的关节角θ₂和第三关节的关节角θ₃；

步骤S23，分析机器人的几何机构和投影关系，基于获得的θ₁、θ₂和θ₃求解逆运动学解的第五关节的关节角θ₅，然后求出第四关节的关节角θ₄和第六关节的关节角θ₆。

2.根据权利要求1所述的基于几何法的工业机器人逆解算法，其特征在于，机器人的第一种构型和第二种构型的θ₁的计算公式如下：

θ₁＝atan2(p_y，p_x)。

3.根据权利要求1所述的基于几何法的工业机器人逆解算法，其特征在于，机器人的第三种构型θ₁有两种可能解如下：

将θ₁的两种可能解中与前一个时刻的第一关节的关节角的差的绝对值较小的一个作为θ₁的取值。

4.根据权利要求1所述的基于几何法的工业机器人逆解算法，其特征在于，机器人的第一种构型的角度q₁的计算公式如下：

机器人的第一种构型的角度q₂的计算公式如下：

机器人的第一种构型的角度q₃的计算公式如下：

机器人的第二种构型的角度q₃的计算公式如下：

机器人的第三种构型的角度q₃的计算公式如下：

第二关节的关节角θ₂通过公式θ₂＝-(q₁+q₃)即可求得；

第三关节的关节角θ₃分为两种情况：

若θ₂＝-(q₁+q₃)，则

θ₃＝π-q₂-atan2(d₄,a₃)；

若θ₂＝q₁-q₃则

θ₃＝q₂+atan2(d₄,a₃)-π。

5.根据权利要求1所述的基于几何法的工业机器人逆解算法，其特征在于，关节角θ₅的计算公式如下：

cos(θ₅)＝O_4z·O_6z；

θ₅＝acos(O_4z·O_6z)或θ₅＝-acos(O_4z·O_6z)。

6.根据权利要求1所述的基于几何法的工业机器人逆解算法，其特征在于，关节角θ₄的计算公式如下：

或

其中，R₁为R_6Z在4轴的法平面的分量，R₂为R_6Z′在4轴的法平面的分量。