[发明专利]一种六关节工业机器人通过姿态奇点的控制方法及系统

摘要

本发明属于工业机器人运动控制领域，并具体公开了一种六关节工业机器人通过姿态奇点的控制方法及系统，包括如下步骤：1)截取姿态奇点所处工业机器人预设的规划路径中的一段轨迹，获取轨迹起点的位置值和后三个关节变量值以及终点的位置值和后三个关节变量值；2)以插补方式获得姿态奇点位置的后三个关节变量值；3)求取姿态奇点位置对应的前三个关节变量值；4)以所述姿态奇点位置对应的后三个关节变量值以及前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。本发明在不改变原有规划路径的前提下使得机器人顺利通过姿态奇点，简单易行且效率高。

主权项

1.一种六关节工业机器人通过姿态奇点的控制方法，该六关节工业机器人包括依次相连的基座、第一关节、第一连杆、第二关节、第二连杆、第三关节、第三连杆、第四关节、第四连杆、第五关节、第五连杆、第六关节和第六连杆，第六连杆与工具固接，第一、二、三关节为前三个关节，第四、五、六关节为后三个关节，其特征在于，包括如下步骤：(1)截取姿态奇点所处工业机器人预设的规划路径中的一段轨迹，获取该轨迹起点对应的位置值和后三个关节变量值以及该轨迹终点对应的位置值和后三个关节变量值；(2)根据所述轨迹起点的后三个关节变量值以及轨迹终点的后三个关节变量值以插补的方式获得姿态奇点位置对应的后三个关节变量值；(3)建立运动学反解方程求取姿态奇点位置对应的前三个关节变量值：(3.1)首先建立如下矩阵方程：其中，为工具坐标系相对于基坐标系的位置矢量，为工业机器人第三连杆坐标系相对于基坐标系的齐次变换矩阵，表示工具坐标系相对于第三连杆坐标系的位置矢量，其由插补获得的后三个关节变量值及工具坐标系相对于第六连杆坐标系的位置矢量计算获得，θ₁、θ₂和θ₃分别为待求解的第一关节的关节变量值、第二关节的关节变量值和第三关节的关节变量值；所述采用如下公式计算：其中，s_i＝sin(θ_i)，c_i＝cos(θ_i)，i＝4,5,6，θ_i为插补获得的第i关节的关节变量值，a₃为从坐标系{3}的z轴z₃到坐标系{4}的z轴z₄沿坐标系{3}的x轴x₃的距离，d₄为从坐标系{3}的x轴x₃到坐标系{4}的x轴x₄沿坐标系{4}的z轴z₄的距离，{i}表示与第i连杆固接的坐标系；(3.2)对步骤(3.1)中建立的矩阵方程两端同时左乘第一连杆坐标系相对于基坐标系的齐次变换矩阵的逆矩阵建立运动学反解方程求得第一关节和第三关节的关节变量值θ₁和θ₃如下：其中，Atan为反正切函数，x_t,y_t,z_t为工具坐标系原点相对于基坐标系的三个坐标分量，s₁＝sin(θ₁)，s₂＝sin(θ₂)，s₂₃＝sin(θ₂+θ₃)，c₁＝cos(θ₁)，c₂＝cos(θ₂)，c₂₃＝cos(θ₂+θ₃)，θ₁,θ₂,θ₃为工业机器人第一关节至第三关节的关节变量值，a₁和a₂分别表示第一连杆和第二连杆的长度，d₂表示第一连杆和第二连杆之间的偏置，x₃,y₃,z₃为工具坐标系原点相对于第三连杆坐标系的三个坐标分量；(3.3)对步骤(3.1)中建立的矩阵方程两端同时左乘第三连杆坐标系相对于基坐标系的齐次变换矩阵的逆矩阵建立运动学反解方程求得第二关节的关节变量值θ₂如下：其中，Atan为反正切函数，c₃＝cos(θ₃)，s₃＝sin(θ₃)；(4)以步骤(2)插补获得的所述姿态奇点位置对应的后三个关节变量值以及步骤(3)求解获得的前三个关节变量值作为工业机器人在姿态奇点位置对应的运动控制参数，实现工业机器人的运动控制，以此使得六关节工业机器人顺利通过姿态奇点。