CN106020125B - 一种基于速度连续的工业机器人运动学算法 - Google Patents

Abstract

本发明涉及一种基于速度连续的工业机器人运动学算法，包括以下两个部分：(1)对工业机器人轴进行速度和位置的规划；(2)误差控制。本发明以6自由度工业机器人为例，依据D‑H参数，计算机器人的雅克比矩阵，在笛卡尔空间进行TCP速度Ve的规划，反雅克比运算，再反雅克比矩阵中增加反奇异位的变量α，求出插补时间的关节速度然后对各轴速度进行积分出各轴的位置量q，运算过程严密，保证了运算结果的准确度。

Description

一种基于速度连续的工业机器人运动学算法

技术领域

本发明涉及工业机器人控制方法技术领域，具体的说是一种基于速度连续的工业机器人运动学算法。

背景技术

随着工业自动化的发展，工业机器人的使用领域越来越大，现场工艺对机器人运动要求越来越严格，工业机器人不仅仅是关节运动，针对工业机器人的结构不同，笛卡尔坐标系下运动存在奇异位置，有些工艺要求必须速度连续地通过奇异位，因此人们需要一种速度连续的工业机器人运动学算法，帮助工业机器人迅速通过笛卡尔坐标系下的奇异位置。

发明内容

针对上述技术的缺陷，本发明提出一种基于速度连续的工业机器人运动学算法。

一种基于速度连续的工业机器人运动学算法，包括以下两个部分：

(1)对工业机器人轴进行速度和位置的规划，具体算法为：

①依据工业机器人D-H矩阵参数求出工业机器人的雅克比矩阵J(q)，引入反奇异位变量α，得出反雅克比矩阵J^*与反奇异位变量α的关系，见公式(1)：

J^*＝J^T(JJ^T+α²I)^-1 (1)

式中：I为单位阵；J^T为J的旋转矩阵。

②按照公式(2)和公式(3)计算出工业机器人的关节速度

式中：Ve为机器人笛卡尔坐标系下的速度量。

③按照公式(4)计算出关节位置q：

式中：T_k+1和T_k为时间周期。

(2)误差控制：反奇异位变量α的引入，会同时引入位姿的误差，即控制位姿与实际位姿的差值(e_p,e_o)，其中：误差(e_p,e_o)由公式(5)(6)(7)定义，为了减小误差，需要对误差进行抑制；

e_p＝p_d-p_e(q) (5)

式中：e_p为位置误差；p_d为命令位置，p_e为实际位置。

e_o＝γsinθ (6)

式中：e_o为姿态误差，γ为旋转轴，θ为旋转角度。

R(θ,γ)＝R_dR_e ^T(q) (7)

式中：R为姿态矩阵，R_e为实际姿态矩阵，R_d为命令姿态矩阵。

具体抑制方法为：建立一个控制位姿与实际位姿的差值(e_p,e_o)、增益K、反雅克比矩阵J^*之间的闭环控制模型，通过调整参数变量α与增益K的值，满足机器人奇异位的速度连续，同时保证轨迹精度。

本发明的有益效果是：

本发明以6自由度工业机器人为例，依据D-H参数，计算机器人的雅克比矩阵，在笛卡尔空间进行TCP速度Ve的规划，反雅克比运算，再反雅克比矩阵中增加反奇异位的变量α，求出插补时间的关节速度然后对各轴速度进行积分出各轴的位置量q，运算过程严密，通过调整增益，可以控制路径的轨迹误差精度在0.005mm以内。

附图说明

下面结合附图和实施例对本发明进一步说明。

图1是本发明的差值(e_p,e_o)、增益以及反雅克比矩阵J^*之间的闭环控制模型图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面对本发明进一步阐述。

如图1所示，一种基于速度连续的工业机器人运动学算法，一种基于速度连续的工业机器人运动学算法，包括以下两个部分：

(3)对工业机器人轴进行速度和位置的规划，具体算法为：

①依据工业机器人D-H矩阵参数求出工业机器人的雅克比矩阵J(q)，引入反奇异位变量α，得出反雅克比矩阵J^*与反奇异位变量α的关系，见公式(1)：

J^*＝J^T(JJ^T+α²I)^-1 (1)

②结合公式(1)，按照公式(2)和公式(3)计算出工业机器人的关节速度

③结合公式(3)，按照公式(4)计算出关节位置q：

(4)误差控制：令控制位姿与实际位姿的差值为(e_p,e_o)，其中：误差(e_p,e_o)由公式(5)(6)(7)定义；

e_p＝p_d-p_e(q)(5)

e_o＝γsinθ (6)

R(θ,γ)＝R_dR_e ^T(q) (7)

对误差进行抑制的具体方法为：建立一个控制位姿与实际位姿的差值(e_p,e_o)、增益K、增益的比例系数K(·)、反雅克比矩阵J^*之间的闭环控制模型，如图1，通过调整参数变量α与增益K的值，满足机器人奇异位的速度连续，同时保证轨迹精度。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (1)

1.一种基于速度连续的工业机器人运动学算法，其特征在于：包括以下两个部分：

(1)对工业机器人轴进行速度和位置的规划，具体算法为：

①依据工业机器人D-H矩阵参数求出工业机器人的雅克比矩阵J(q)，引入反奇异位变量α，得出反雅克比矩阵J^*与反奇异位变量α的关系，见公式(1)：

J^*＝J^T(JJ^T+α²I)^-1 (1)

式中：I为单位阵；J^T为J的旋转矩阵；

②按照公式(2)和公式(3)计算出工业机器人的关节速度

式中：Ve为机器人笛卡尔坐标系下的速度量；

③按照公式(4)计算出关节位置q：

式中：T_k+1和T_k为时间周期；

(2)误差控制：反奇异位变量α的引入，会同时引入位姿的误差，即控制位姿与实际位姿的差值(e_p,e_o)，其中：误差(e_p,e_o)由公式(5)(6)(7)定义，为了减小误差，需要对误差进行抑制；

e_p＝p_d-p_e(q) (5)

式中：e_p为位置误差；p_d为命令位置，p_e为实际位置；

e_o＝γsinθ (6)

式中：e_o为姿态误差，γ为旋转轴，θ为旋转角度；

R(θ,γ)＝R_dR_e ^T(q) (7)

式中：R为姿态矩阵，R_e为实际姿态矩阵，R_d为命令姿态矩阵；

具体抑制方法为：建立一个控制位姿与实际位姿的差值(e_p,e_o)、增益K、反雅克比矩阵J^*之间的闭环控制模型，通过调整参数变量α与增益K的值，满足机器人奇异位的速度连续，同时保证轨迹精度。