本发明涉及一种基于速度连续的工业机器人运动学算法，包括以下两个部分：(1)对工业机器人轴进行速度和位置的规划；(2)误差控制。本发明以6自由度工业机器人为例，依据D‑H参数，计算机器人的雅克比矩阵，在笛卡尔空间进行TCP速度Ve的规划，反雅克比运算，再反雅克比矩阵中增加反奇异位的变量α，求出插补时间的关节速度然后对各轴速度进行积分出各轴的位置量q，运算过程严密，保证了运算结果的准确度。
1.一种基于速度连续的工业机器人运动学算法，其特征在于：包括以下两个部分：(1)对工业机器人轴进行速度和位置的规划，具体算法为：
①依据工业机器人D-H矩阵参数求出工业机器人的雅克比矩阵J(q)，引入反奇异位变量α，得出反雅克比矩阵J*与反奇异位变量α的关系，见公式(1)：J*＝JT(JJT+α2I)-1 (1)式中：I为单位阵；JT为J的旋转矩阵；
②按照公式(2)和公式(3)计算出工业机器人的关节速度
(2)误差控制：反奇异位变量α的引入，会同时引入位姿的误差，即控制位姿与实际位姿的差值(ep,eo)，其中：误差(ep,eo)由公式(5)(6)(7)定义，为了减小误差，需要对误差进行抑制；
ep＝pd-pe(q) (5)式中：ep为位置误差；pd为命令位置，pe为实际位置；
eo＝γsinθ (6)式中：eo为姿态误差，γ为旋转轴，θ为旋转角度；
R(θ,γ)＝RdReT(q) (7)式中：R为姿态矩阵，Re为实际姿态矩阵，Rd为命令姿态矩阵；
具体抑制方法为：建立一个控制位姿与实际位姿的差值(ep,eo)、增益K、反雅克比矩阵J*之间的闭环控制模型，通过调整参数变量α与增益K的值，满足机器人奇异位的速度连续，同时保证轨迹精度。
一种基于速度连续的工业机器人运动学算法
技术领域
[0001] 本发明涉及工业机器人控制方法技术领域，具体的说是一种基于速度连续的工业机器人运动学算法。
背景技术
[0002] 随着工业自动化的发展，工业机器人的使用领域越来越大，现场工艺对机器人运动要求越来越严格，工业机器人不仅仅是关节运动，针对工业机器人的结构不同，笛卡尔坐标系下运动存在奇异位置，有些工艺要求必须速度连续地通过奇异位，因此人们需要一种速度连续的工业机器人运动学算法，帮助工业机器人迅速通过笛卡尔坐标系下的奇异位置。
发明内容
[0003] 针对上述技术的缺陷，本发明提出一种基于速度连续的工业机器人运动学算法。
[0004] 一种基于速度连续的工业机器人运动学算法，包括以下两个部分：
[0005] (1)对工业机器人轴进行速度和位置的规划，具体算法为：
[0006] ①依据工业机器人D-H矩阵参数求出工业机器人的雅克比矩阵J(q)，引入反奇异位变量α，得出反雅克比矩阵J*与反奇异位变量α的关系，见公式(1)：
[0007] J*＝JT(JJT+α2I)-1 (1)
[0008] 式中：I为单位阵；JT为J的旋转矩阵。
[0009] ②按照公式(2)和公式(3)计算出工业机器人的关节速度
[0010]
[0011]
[0012] 式中：Ve为机器人笛卡尔坐标系下的速度量。
[0013] ③按照公式(4)计算出关节位置q：
[0014]
[0015] 式中：Tk+1和Tk为时间周期。
[0016] (2)误差控制：反奇异位变量α的引入，会同时引入位姿的误差，即控制位姿与实际位姿的差值(ep,eo)，其中：误差(ep,eo)由公式(5)(6)(7)定义，为了减小误差，需要对误差进行抑制；
[0017] ep＝pd-pe(q) (5)
[0018] 式中：ep为位置误差；pd为命令位置，pe为实际位置。
[0019] eo＝γsinθ (6)
[0020] 式中：eo为姿态误差，γ为旋转轴，θ为旋转角度。
[0021] R(θ,γ)＝RdReT(q) (7)
[0022] 式中：R为姿态矩阵，Re为实际姿态矩阵，Rd为命令姿态矩阵。
[0023] 具体抑制方法为：建立一个控制位姿与实际位姿的差值(ep,eo)、增益K、反雅克比矩阵J*之间的闭环控制模型，通过调整参数变量α与增益K的值，满足机器人奇异位的速度连续，同时保证轨迹精度。
[0024] 本发明的有益效果是：
[0025] 本发明以6自由度工业机器人为例，依据D-H参数，计算机器人的雅克比矩阵，在笛卡尔空间进行TCP速度Ve的规划，反雅克比运算，再反雅克比矩阵中增加反奇异位的变量α，求出插补时间的关节速度 然后对各轴速度进行积分出各轴的位置量q，运算过程严密，通过调整增益，可以控制路径的轨迹误差精度在0.005mm以内。
附图说明
[0026] 下面结合附图和实施例对本发明进一步说明。
[0027] 图1是本发明的差值(ep,eo)、增益以及反雅克比矩阵J*之间的闭环控制模型图。
具体实施方式
[0028] 为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面对本发明进一步阐述。
[0029] 如图1所示，一种基于速度连续的工业机器人运动学算法，一种基于速度连续的工业机器人运动学算法，包括以下两个部分：
[0030] (3)对工业机器人轴进行速度和位置的规划，具体算法为：
[0031] ①依据工业机器人D-H矩阵参数求出工业机器人的雅克比矩阵J(q)，引入反奇异位变量α，得出反雅克比矩阵J*与反奇异位变量α的关系，见公式(1)：
[0032] J*＝JT(JJT+α2I)-1 (1)
[0033] ②结合公式(1)，按照公式(2)和公式(3)计算出工业机器人的关节速度[0034]
[0035]
[0036] ③结合公式(3)，按照公式(4)计算出关节位置q：
[0037]
[0038] (4)误差控制：令控制位姿与实际位姿的差值为(ep,eo)，其中：误差(ep,eo)由公式(5)(6)(7)定义；
[0039] ep＝pd-pe(q)(5)
[0040] eo＝γsinθ (6)
[0041] R(θ,γ)＝RdReT(q) (7)
[0042] 对误差进行抑制的具体方法为：建立一个控制位姿与实际位姿的差值(ep,eo)、增益K、增益的比例系数K(·)、反雅克比矩阵J*之间的闭环控制模型，如图1，通过调整参数变量α与增益K的值，满足机器人奇异位的速度连续，同时保证轨迹精度。
[0043] 以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明内。本发明要求保护范围由所附的权利要求书及其等效物界定。
