CN113580135A - 一种有偏置七轴机器人实时逆解算法 - Google Patents

Abstract

本发明提供一种有偏置七轴机器人实时逆解算法，包括：基于有偏置七轴机器人，对应建立一个满足piper准则的虚拟六轴机器人；重建虚拟六轴机器人的D‑H参数模型；对于给定的

和有偏置七轴机器人的每一个探测点θ_7‑predict，计算出

将

代入虚拟六轴机器人中计算运动学逆解，求解出虚拟六轴机器人的关节角θ_6‑result；计算abs(θ_7‑predict‑θ_6‑result)，将所有abs(θ_7‑predict‑θ_6‑result)中的最小值对应的θ_7‑predict为目标θ_7‑predict，基于目标θ_7‑predict，采用数值迭代方法，确定有偏置七轴机器人的一组最优关节角。

Description

一种有偏置七轴机器人实时逆解算法

技术领域

本发明涉及机器人领域，更具体地，涉及一种有偏置七轴机器人实时逆解算法。

背景技术

工业机器人在喷涂、码垛、冲压、焊接、上下料等工作中的应用愈发广泛，七轴机器人是在传统的六轴工业机器人上增加一个旋转轴，相较于六轴机器人，其具有更好的灵活性和避障能力。有偏置机器人的定义是机器人末端三关节轴线不交于一点，机器人末端连杆的偏置可以增加机器人的工作空间、减少奇异点，但代价是使逆运动学算法变得复杂。

七轴机器人的逆运动学算法取决于其构型，其按照构型主要可以分为存在末端偏置和不存在末端偏置的两类机器人，两者分别对应一类机器人逆解算法，如无偏的七轴机器人可以采用“几何法+解析法”、“固定关节角法”等，而有偏的机器人可以采用“纯数值解法”、“同构型拟合法等”，这些求解算法都比较复杂。

发明内容

本发明针对现有技术中存在的有偏置的机器人的逆解算法复杂的问题，提供一种有偏置七轴机器人实时逆解算法，通过为有偏置七轴机器人对应建立一个满足piper准则的虚拟六轴机器人，基于虚拟六轴机器人的运动学逆解，以及结合探测拟合法，解决了有偏七轴机器人的实时逆运动学运算问题，该方法包括：

基于有偏置七轴机器人，对应建立一个满足piper准则的虚拟六轴机器人；

将虚拟六轴机器人的第三轴设为臂型角ω，重建虚拟六轴机器人的D-H参数模型，以及建立七轴有偏置机器人与虚拟六轴机器人的关节对应关系；

对于给定的

和有偏置七轴机器人的一个探测点θ_7-predict，计算出

将

代入虚拟六轴机器人中计算运动学逆解，求解出虚拟六轴机器人的一组关节角[θ₁，θ₂，θ₃，θ₄，θ₅，θ_6-result]；

计算abs(θ_7-predict-θ_6-result)，其中，abs表示绝对值；

对于每一个探测点θ_7-predict，均计算对应的abs(θ_7-predict-θ_6-result)，确定所有abs(θ_7-predict-θ_6-result)中的最小值对应的θ_7-predict为目标θ_7-predict；

基于目标θ_7-predict，采用数值迭代方法，确定有偏置七轴机器人的一组最优关节角。

本发明提供的一种有偏置七轴机器人实时逆解算法，由于有偏置七轴机器人的逆运动学算法非常复杂，为有偏置七轴机器人对应建立一个满足piper准则的虚拟六轴机器人，基于虚拟六轴机器人的运动学逆解，以及结合探测拟合法，确定最优的θ₇预测值，基于最优的θ₇预测值，确定有偏置七轴机器人的一组最优关节角，此种算法具有实时性高、收敛性好、能处理多解问题等优点，适用于工业场景。

附图说明

图1为七轴机器人模型示意图；

图2为七轴机器人D-H参数模型示意图；

图3为本发明提供的一种有偏置七轴机器人实时逆解算法流程图；

图4为建立的虚拟六轴机器人的D-H参数模型示意图；

图5为有偏置七轴机器人的D-H参数模型示意图；

图6为虚拟六轴机器人的八组解的示意图；

图7为虚拟六轴机器人正运动学算法流程图；

图8为虚拟六轴机器人逆运动学算法流程图；

图9为数值迭代算法的具体流程示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

在对本发明实施例提供的有偏置七轴机器人实时逆解算法进行详细介绍之前，对有偏置七轴机器人进行介绍，比如，如图1，机器人采用串联形式，由于其任意三关节的轴线不交于一点，所以是关节耦合度最高的一类机器人，具有代表性。

采用改进D-H参数模型表示机器人，如图2所示，图2中坐标系{0}为基坐标系，与杆i固连的坐标系为坐标系{i}。同时对照参数模型建立参数表，如表1所示。

表1七轴机器人D-H参数表

根据表1中的D-H参数表，可以得到相连杆间的齐次变换矩阵：

设

可得

各项如下：

n_x＝((((c1c2c3-s1s3)c4-c1s2s4)c5-(c1c2s3+s1c3)s5)c6-c1c2c3-s1s3s4+c1s2c4s6c7+-c1c2c3-s1s3c4-c1s2s4s5-c1c2s3+s1c3c5s7；

n_y＝((((s1c2c3+c1s3)c4-s1s2s4)c5-(s1c2s3-c1c3)s5)c6-s1c2c3+c1s3s4+s1s2c4s6c7+-s1c2c3+c1s3c4-s1s2s4s5-s1c2s3-c1c3c5s7；

n_z＝((-(s2c3c4+c2s4)c5+s2s3s5)c6-(-s2c3s4+c2c4)s6)c7+((s2c3c4+c2s4)s5+s2s3c5)s7；

o_x＝(-(((c1c2c3-s1s3)c4-c1s2s4)c5-(c1c2s3+s1c3)s5)c6+((c1c2c3-s1s3)s4+c1s2c4)s6)s7+(-((c1c2c3-s1s3)c4-c1s2s4)s5-(c1c2s3+s1c3)c5)c7；

o_y＝(-(((s1c2c3+c1s3)c4-s1s2s4)c5-(s1c2s3-c1c3)s5)c6+s1c2c3+c1s3s4+s1s2c4s6s7+-s1c2c3+c1s3c4-s1s2s4s5-s1c2s3-c1c3c5c7；

o_z＝(((s2c3c4+c2s4)c5-s2s3s5)c6+(-s2c3s4+c2c4)s6)s7+((s2c3c4+c2s4)s5-s2s3c5)c7；

a_x＝(((c1c2c3-s1s3)c4-c1s2s4)c5-(c1c2s3-s1c3)s5)s6+((c1c2c3-s1s3)s4+c1s2c4)c6；

a_y＝(((s1c2c3+c1s3)c4-s1s2s4)c5+(-s1c2s3+c1c3)s5)s6+((s1c2c3+c1s3)s4+s1s2c4)c6；

a_z＝(-(s2c3c4+c2s4)c5+s2s3s5)s6+(-s2c3s4+c2c4)c6；

p_x＝((((c1c2c3-s1s3)c4-c1s2s4)c5-(c1c2s3+s1c3)s5)s6+c1c2c3-s1s3s4+c1s2c4c6d7-c1c2c3-s1s3c4-c1s2s4s5+c1c2s3+s1c3c5d6+c1c2c3-s1s3s4+c1s2c4d5-c1c2s3+s1c3d4+c1s2d3-s1d2；

p_y＝((((s1c2c3+c1s3)c4-s1s2s4)c5-(s1c2s3-c1c3)s5)s6+s1c2c3+c1s3s4+s1s2c4c6d7-s1c2c3+c1s3c4+s1s2s4s5+s1c2s3-c1c3c5d6+s1c2c3-c1s3s4+s1s2c4d5-s1c2s3-c1c3d4+s1s2d3+c1d2；

p_z＝((-(s2c3c4+c2s4)c5+s2s3s5)s6-(s2c3s4-c2c4)c6)d₇+((s2c3c4+c2s4)s5+s2s3c5)d₆-(s2c3s4-c2c4)d₅+s2s3d₄+c2d₃+d₁；

其中sn＝sinθ_n，cn＝cosθ_n。

对于有偏置七轴机器人，在求运动学逆解时，存在两个难点：1、有偏置机器人由于不满足piper准则，所以不具备解析解，需要结合其他优化算法与启发式算法进行求解；2、关节空间与笛卡尔空间维度不同:机器人的关节空间为七维空间，笛卡尔空间为六维空间，根据正运动学的齐次变换矩阵，可以列出含有7个未知数的6个独立方程，这种不定方程组理论上具有无数个解。

基于上述难点1，本发明实施例提出了采用“探测拟合算法”进行解决；对于难点2，本发明实施例采用“实时重建D-H参数模型法”进行解决

参见图3，图3为本发明提供的一种有偏置七轴机器人实时逆解算法流程图，如图3所示，逆解算法主要包括：101、基于有偏置七轴机器人，对应建立一个满足piper准则的虚拟六轴机器人。

可以理解的是，为了解决有偏置七轴机器人不存在封闭解的问题，为其建立一个满足piper准则的虚拟机器人以辅助计算：

1、令d₁＝d₆＝d₇＝0，虚拟机器人的第七轴与第五轴共线，此时机器人的末端三关节交于一点，虚拟机器人的坐标系{0}与坐标系{1}原点重合，其余关节和连杆与原机器人相同。

2、将机器人的第三轴设为“臂型角”，记为ω，此时机器人关节空间降为六维，构型等效为一个满足piper准则的六轴机器人；当臂型角改变时，算法需要重建D-H参数模型，并且同时触发机器人在零空间中的运动。

虚拟六轴机器人的D-H参数模型如图4所示，D-H参数表如表2所示。

表2虚拟六轴机器人D-H参数表

其中，a表示连杆偏距，α_i-1表示从(i-1)连杆到第i连杆的扭角，d_i表示第i连杆的长度，θ_i表示从第(i-1)连杆到第i连杆的转动角度。

102、将虚拟六轴机器人的第三轴设为臂型角ω，重建虚拟六轴机器人的D-H参数模型，以及建立七轴有偏置机器人与虚拟六轴机器人的关节对应关系。

基于有偏置七轴机器人，建立对应的虚拟六轴机器人，虚拟机器人与真实机器人的对应关系如表3所示，虚拟六轴机器人与真实的有偏七轴机器人的对应关节角度相等，虚拟六轴机器人的坐标系{0}与真实有偏七轴机器人的坐标系{1}原点重合、与真实有偏置七轴机器人坐标系{0}的方向重合。令真实有偏置七轴机器人为机器人A，虚拟六轴机器人为机器人B，分别参见图5和图4。

表3真实有偏置七轴机器人与虚拟六轴机器人关节对应关系

103、对于给定的

和有偏置七轴机器人的一个探测点θ_7-predict，计算出

将

代入虚拟六轴机器人中计算运动学逆解，求解出虚拟六轴机器人的一组关节角[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]。

可以理解的是，对于有偏置七轴机器人的逆解运算，设机器人的工件坐标系为坐标系{G}，工具坐标系为坐标系{T}，已知工件坐标系与机器人基坐标系之间的变换关系

机器人末端坐标系与工具坐标系之间的变换关系

对于给定的

由矩阵运算可以求得：

于是对有偏置七轴机器人的逆解问题转换为寻找一组关节角，使其正运动学的解为

对有偏置七轴机器人的逆解算法的原理为，假设对于给定

存在一组机器人关节角，使得

此时机器人构型如图4所示。随机在[-π,π]区间内选择一个探测点θ_7-predict，真实有偏置七轴机器人坐标系{5}的位置T₅为：

将坐标系{6}的姿态和坐标系{5}的位置融合，得到新的坐标系{P}，坐标系{P}的原点位于真实机器人的坐标系{5}原点处；

将

减掉d₁连杆长度，然后带入到虚拟六轴机器人中计算运动学逆解

求解出对应的一组[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]。

其中，根据

ω求解对应的[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]的算法原理具体为，对满足piper准则的机器人采用反变换法计算反解，当

中的数值已知时，希望通过下列方程求解[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]。

对式(1-1)求解，得到[θ₁，θ₂，θ₃，θ₄，θ₅，θ₆]，其具体的求解过程为：

(1)首先求解θ₃(对应真实有偏置七轴机器人的θ₄)，将式(1-1)中含有θ₁的项

移动到等式左边：

根据左右矩阵的(1，4)，(2，4)，(3，4)三项平方和相等建立方程：

p_x ²+p_y ²+p_z ²＝a₂ ²+d₂ ²+d₃ ²+d₄ ²+2d₂d₃c_ω+2a₂d₄c₃+2d₂d₄s_ωs₃；

式中：c_ω＝cos(ω)，s_ω＝sin(ω)。

令m＝p_x ²+p_y ²+p_z ²-(a₂ ²+d₂ ²+d₃ ²+d₄ ²+2d₂d₃c_ω)；

k1＝2a₂d₄；k2＝2d₂d₄s_ω，构造三角函数

则式(1-1)变为

则有：

通过反正切运算，可以求得θ₃的两个解为

(2)然后求解θ₁，由式(1-1)方程左右矩阵的(2，2)元素相等建立方程

p_yc₁+p_xs₁＝d₂+d₃c_ω+d₄s_ωs₃；

令m＝d₂+d₃c_ω+d₄s_ωs₃；

构造三角函数

则式(1-1)变为

则有：

通过反正切运算，可以求得θ₁的两个解为

(3)下一步求解θ₂，由式(1-1)方程左右矩阵的(1，4)元素与(3，4)元素对应相等建立方程

化简得

通过解此二元方程组得

θ₂＝Atan2(s₂，c₂)。

(4)求解θ₅，目前[θ₁，θ₂，θ₃]已经求解完成，可以根据位姿变化得到

由式(1-1)中(2，3)元素对应相等建立方程

cos(θ₅)＝a_x(c₁c₃s₂-s_ωs₁s₃+c_ωc₁c₂s₃)+a_y(s_ωc₁s₃+c₃s₁s₂+cωc2s1s3+azc2c3-cωs2s3；

由方程解出

θ₅＝±acos(a_x(c₁c₃s₂-s_ωs₁s₃+c_ωc₁c₂s₃)+a_y(s_ωc₁s₃+c₃s₁s₂+c_ωc₂s₁s₃)+a_z(c₂c₃-c_ωs₂s₃))

(5)求解θ₄，由式(1-1)中(3，1)元素和(3，3)元素对应相等建立方程

c₄s₅＝a_x(-c₃s_ωs₁-c₁s₂s₃+c_ωc₁c₂c₃)+a_y(c₁c₃s_ω-s₁s₂s₃+cωc2c3s1+az-c2s3-cωc3s2；

s₄s₅＝a_x(-c_ωs₁-c₁c₂s_ω)+a_y(c_ωc₁-c₂s_ωs₁)+az(s_ωs₂)。

令k1＝a_x(-c₃s_ωs₁-c₁s₂s₃+c_ωc₁c₂c₃)+a_y(c₁c₃s_ω-s₁s₂s₃+cωc2c3s1+az-c2s3-cωc3s2，

令k2＝a_x(-c_ωs₁-c₁c₂s_ω)+a_y(c_ωc₁-c₂s_ωs₁)+az(s_ωs₂)，

则θ₄＝atan2(k2，k1)或θ₄＝atan2(-k2，-k1)。

(6)求解θ₆，由式(1-1)中(2，1)元素和(2，2)元素对应相等建立方程

c₆s₅＝n_x(-c₁c₃s₂+s_ωs₁s₃-c_ωc₁c₂s₃)+n_y(-c₁s_ωs₃-c₃s₁s₂-cωc2s1s3+nzCws2s3-c2c3；

s₆s₅＝o_x(c₁c₃s₂-s_ωs₁s₃+c_ωc₁c₂s₃)+o_y(c₁s_ωs₃+c₃s₁s₂+cωc2s1s3+ozc2c3-cωs2s3。

令k1＝n_x(-c₁c₃s₂+s_ωs₁s₃-c_ωc₁c₂s₃)+n_y(-c₁s_ωs₃-c₃s₁s₂-c_ωc₂s₁s₃)+n_z(c_ws₂s₃-c₂c₃)，

令k2＝o_x(c₁c₃s₂-s_ωs₁s₃+c_ωc₁c₂s₃)+o_y(c₁s_ωs₃+c₃s₁s₂+cωc2s1s3+ozc2c3-cωs2s3，

则θ₆＝atan2(k2，k1)或θ₆＝atan2(-k2，-k1)。

可以理解的是，对于6个轴都为旋转关节的6轴机器人来说，给定末端工具坐标系，至多可能存在16组解，由于本发明实施例的虚拟六轴机器人姿态矩阵解耦，故存在8组解，其计算逻辑如图6所示。尽管有多组解存在，但机器人在实际运行的过程中只会选择其中一组解；常用的多解选择原则有能耗最小原则、受力合理原则、行程最短原则等，其中行程最短原则更为合理。

基于此，本发明实施例提出一种划分多组解的方法，将多组解编码为关节属性，然后通过分析多解产生的原因，将关节属性与多组解一一对应起来。

其中，值所以会产生八组的原因主要是肘关节角度θ₃、肩关节角度θ₁以及腕关节角度θ₅存在双解，因此，本发明实施例定义θ₃的角度为肘关节角度，θ₁的角度为肩关节角度，θ₅的角度为腕关节角度，定义关节属性为一个包含三个数的数组[Bit3，Bit2，Bit1]，数组中的每一位可以取值为0或1。通过分析肘关节角度θ₃双解产生的原因，定义Bit1的取值，通过分析肩关节角度θ₁双解产生的原因，定义Bit2的取值，以及分析腕关节角度θ₅双解产生的原因，定义Bit3的取值；根据给定的关节属性[Bit1、Bit2、Bit3]，确定八组解中对应的一组解[θ₁、θ₂、θ₃、θ₄、θ₅和θ₆]。

具体的过程为：

(1)定义θ₃的角度为肘关节角度，定义θ₁的角度为肩关节角度，定义θ₅的角度为腕关节角度，定义关节属性为一个包含三个数的数组[Bit3,Bit2,Bit1]，数组中的每一位可以取值为0或1，分别表示肘关节角度θ₃、肩关节角度θ₁和腕关节角度θ₅的取值。

机器人的正运动学函数除了需要计算齐次变换矩阵之外，还需要同时计算当前这组关节角所属的关节属性，其程序流程图如图7所示。

在逆运动学算法中，算法输入齐次变换矩阵和关节属性，输出唯一一组关节角，其流程图如图8所示；值得注意的是，当机器人执行器在一条直线上运行时，若输入的关节属性相同，那么连续的多组逆解结果满足行程最短原则。

(2)通过分析肘关节角度θ₂、肩关节角度θ₁和腕关节角度θ₄双解产生的原因，确定Bit1、Bit2和Bit3的取值。

(21)首先分析肘关节角度θ₃双解产生的原因，根据式(1-1)可知,

的双解是与

的两个解一一对应的；由三角函数公式

在正解计算中，k1、k2、θ₃都是已知量，所以令

同理在计算逆运动学时，可以根据Bit1的值选定其中一组解

(22)然后分析肩关节角度θ₁双解产生的原因，根据式(1-1)可知，

的双解是与

的两个解一一对应的；由三角函数公式

在正解计算中θ₁已知，p_x、p_y可由正运动学公式求得，所以令

同理在计算逆运动学时，可以根据Bit2的值选定其中一组解

(23)最后分析腕关节角度θ₅双解产生的原因，根据式(1-1)可知，θ₅的双解是与反余弦函数对应的；

令k1＝a_x(c₁c₃s₂-s_ωs₁s₃+c_ωc₁c₂s₃)+a_y(s_ωc₁s₃+c₃s₁s₂+cωc2s1s3+azc2c3-cωs2s3，则有：

同理在计算逆运动学时，可以根据Bit3的值选定其中一组解

由上述的整个分析过程可知，如果给定了机器人的关节属性，则机器人肘关节角度θ₃、肩关节角度θ₁和腕关节角度θ₅就确定了，那么就确定了一组解[θ₁，θ₂，θ₃，θ₄，θ₅，θ₆]。对于θ_7-predict，也就是求解出一组[θ₁，θ₂，θ₃，θ₄，θ₅，θ_6-result]。

104、计算abs(θ_7-predict-θ_6-result)，其中，abs表示绝对值；105、对于每一个探测点θ_7-predict，均计算对应的abs(θ_7-predict-θ_6-result)，确定所有abs(θ_7-predict-θ_6-result)中的最小值对应的θ_7-predict为目标θ_7-predict；106、基于目标θ_7-predict，采用数值迭代方法，确定有偏置七轴机器人的一组最优关节角。

可以理解的是，对于真实有偏置七轴机器人，假设随机选择的θ_7-predict和真实的θ_7-true相等，那么此时计算出虚拟六轴机器人的θ_6-result就等于对真实有偏置七轴机器人的猜测值θ_7-predict，相应的虚拟六轴机器人的其他关节角也等于待求解的真实有偏置七轴机器人的相应关节角。在实际应用中，当abs(θ_7-predict-θ_6-result)小于一个阈值，即可认为当前θ_7-predict是真实值的近似值。

对于每一个探测点θ_7-predict，均可求解出一个对应的θ_6-result，均计算对应的abs(θ_7-predict-θ_6-result)，并判断abs(θ_7-predict-θ_6-result)是否小于一个阈值。

其中，对于多个探测点θ_7-predict，满足abs(θ_7-predict-θ_6-result)小于一个阈值的解[θ_7-predict，θ_6-result]可能有多个，具体的，选取abs(θ_7-predict-θ_6-result)最小的一组解[θ_7-predict，θ_6-result]作为目标解。其中，在选取探测点θ_7-predict可能角度时，在机器人控制中，运动学逆解算法会在两种情况下使用：

情况1：目标关节角度与当前关节角度相差较大(也就是说，机器人的两个连续的关节点的关节角度相差较大)，当前关节角度不能作为参考，需要在θ_7-predict∈[-π，π]等分探测，等分探测的策略是每隔10°取一个探测点；这种情况包括计算与当前末端位姿相距较远的一个坐标系的逆解、插补器验证一个坐标系是否可达、待求解的坐标系对应的关节属性与当前关节属性不相等、机械臂处于初始奇异姿态。

情况2：末端执行器在笛卡尔空间内连续运动，此时相邻两次逆解对应的关节角度差值相对较小，所以对θ_7-predict∈[θ₇-10°，θ₇+10°]等分探测，策略是每隔2°取一个探测点。

其中，创建一个记录表，对于每个探测点θ_7-predict，计算相应的

带入虚拟机器人逆运动学算法中计算[θ₁，θ₂，θ₃，θ₄，θ₅，θ_6-result]，在记录表中添加记录[θ₁，θ₂，θ₃，θ₄，θ₅，θ_6-result，θ_7-predict，(abs(θ_6-result-θ7-predict。找到记录表中absθ6-result-θ7-predict一项最小的一组记录，将其中的[θ₁，θ₂，θ₃，θ₄，θ₅，θ_6-result]作为求得的目标解，对应到有偏置七轴机器人中，即为[θ₁，θ₂，ω，θ₄，θ₅，θ₆，θ₇]，该组解最接近真实解。考虑到算法时间复杂度的问题，探测点数不宜过多；此时真实θ₇的近似解精度还较差，所以下一步通过数值迭代的方法，提高结果精度直至符合精度要求。

在一种具体的实施例方式中，基于目标θ_7-predict，采用数值迭代方法，确定有偏置七轴机器人的一组最优关节角，包括：将目标θ_7-predict、对应的[θ₁，θ₂，θ₃，θ₄，θ₅，θ_6-result]和

作为初始关节角，经过多次数值迭代，输出有偏置七轴机器人的一组最优关节角。

可以理解的是，数值迭代的算法采用Levenberg-Marquart算法，它是高斯牛顿法与梯度下降法的结合体，解决了牛顿法在初始值距离最小值较远时不能保证收敛和黑森矩阵近似奇异时逆解无法计算的问题。LM算法通过在黑森矩阵中添加阻尼系数λ来控制迭代的步长和方向：

数值迭代的基本程序流程图如图9所示，本发明实施例的具体迭代过程为：以目标解为初始值，进行数值迭代，每迭代一次，生成一组[θ₁，θ₂，θ₃，θ₄，θ₅，θ₆，θ₇]，根据[θ₁，θ₂，θ₃，θ₄，θ₅，θ₆，θ₇]，通过正运动学解算，求得相连杆间的齐次变换矩阵，基于齐次变换矩阵，计算迭代过程中产生的误差；重复进行迭代，直到迭代产生的误差小于设定的误差阈值，最后一次迭代产生的[θ₁，θ₂，θ₃，θ₄，θ₅，θ₆，θ₇]即为最终求得的一组最优解，该组解非常接近于真实解，精度非常高。

本发明提供的一种有偏置七轴机器人实时逆解算法，由于有偏置七轴机器人的逆运动学算法非常复杂，为有偏置七轴机器人对应建立一个满足piper准则的虚拟六轴机器人，基于虚拟六轴机器人的运动学逆解，以及结合探测拟合法，确定最优的θ₇预测值，基于最优的θ₇预测值，确定有偏置七轴机器人的一组最优关节角，此种算法具有实时性高、收敛性好、能处理多解问题等优点，适用于工业场景。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包括这些改动和变型在内。

Claims (9)

1.一种有偏置七轴机器人实时逆解算法，其特征在于，包括：

基于有偏置七轴机器人，对应建立一个满足piper准则的虚拟六轴机器人；

将虚拟六轴机器人的第三轴设为臂型角ω，重建虚拟六轴机器人的D-H参数模型，以及建立七轴有偏置机器人与虚拟六轴机器人的关节对应关系；

对于给定的

和有偏置七轴机器人的一个探测点θ_7-predict，计算出

将

代入虚拟六轴机器人中计算运动学逆解，求解出虚拟六轴机器人的一组关节角[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]；

计算abs(θ_7-predict-θ_6-result)，其中，abs表示绝对值；

对于每一个探测点θ_7-predict，均计算对应的abs(θ_7-predict-θ_6-result)，确定所有abs(θ_7-predict-θ_6-result)中的最小值对应的θ_7-predict为目标θ_7-predict；

基于目标θ_7-predict，采用数值迭代方法，确定有偏置七轴机器人的一组最优关节角。

2.根据权利要求1所述的有偏置七轴机器人实时逆解算法，其特征在于，所述基于有偏置七轴机器人，对应建立一个满足piper准则的虚拟六轴机器人，包括：

建立的虚拟六轴机器人与有偏置七轴机器人的对应关节角度相等，虚拟六轴机器人的基坐标系{0}与有偏置七轴机器人的坐标系{1}原点重合、与有偏置七轴机器人坐标系{0}的方向重合。

3.根据权利要求1所述的有偏置七轴机器人实时逆解算法，其特征在于，所述对于给定的

和有偏置七轴机器人的一个探测点θ_7-predict，计算出

包括：

在θ₇的角度区间范围内选择一个探测点θ_7-predict，有偏置七轴机器人坐标系{5}的位置T₅为：

将坐标系{6}的姿态和坐标系{5}的位置融合，得到新的坐标系{P}，坐标系{P}的原点位于有偏置七轴机器人的坐标系{5}原点处；

所述将

代入虚拟六轴机器人中计算运动学逆解，求解出虚拟六

轴机器人的一组关节角[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]，包括：

其中，ω为给定的臂型角，Bit[3]为虚拟六轴机器人的关节属性。

4.根据权利要求3所述的有偏置七轴机器人实时逆解算法，其特征在于，所述所述将

代入虚拟六轴机器人中计算运动学逆解，求解出虚拟六轴机器人的一组关节角[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]，包括：

对于满足piper准则的虚拟六轴机器人，当

中的数值已知时，可通过下列方程求解[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]：

其中，

和

均为齐次变换矩阵；

基于上述方程，依次求解θ₃、θ₁、θ₂、θ₅、θ₄和θ₆，分别对应有偏置七轴机器人的θ₄、θ₁、θ₂、θ₆、θ₅和θ₇；

其中，求解出的[θ₁、θ₂、θ₃、θ₄、θ₅和θ₆]有八组解，根据给定的关节属性，确定对应的一组解。

5.根据权利要求4所述的有偏置七轴机器人实时逆解算法，其特征在于，还包括：

定义θ₃的角度为肘关节角度，θ₁的角度为肩关节角度，θ₅的角度为腕关节角度，定义关节属性为一个包含三个数的数组[Bit3,Bit2,Bit1]，数组中的每一位可以取值为0或1；

其中，通过分析肘关节角度θ₃双解产生的原因，定义Bit1的取值，通过分析肩关节角度θ₁双解产生的原因，定义Bit2的取值，以及分析腕关节角度θ₅双解产生的原因，定义Bit3的取值；

根据给定的关节属性[Bit1、Bit2、Bit3]，确定八组解中对应的一组解[θ₁、θ₂、θ₃、θ₄、θ₅和θ₆]。

6.根据权利要求5所述的有偏置七轴机器人实时逆解算法，其特征在于，通过如下方式确定有偏置七轴机器人每一个探测点θ_7-predict：

如果有偏置七轴机器人的连续两个关节角度相差大，则在θ_7-predict的探测范围[-π,π]内，每隔10°取一个探测点θ_7-predict；

如果有偏置七轴机器人的连续两个关节角度相差小，θ_7-predict在探测范围[θ₇-10°,θ₇+10°]内，每隔2°取一个探测点θ_7-predict。

7.根据权利要求6所述的有偏置七轴机器人实时逆解算法，其特征在于，还包括：

创建一个记录表，对于每个探测点θ_7-predict，计算相应的

带入虚拟机器人逆运动学算法中计算[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]，在记录表中添加记录[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result,θ_7-predict,(abs(θ_6-result-θ7-predict；

找到记录表中(abs(θ_6-result-θ_7-predict))最小的一组记录，以确定目标θ_7-predict。

8.根据权利要求1所述的有偏置七轴机器人实时逆解算法，其特征在于，所述基于目标θ_7-predict，采用数值迭代方法，确定有偏置七轴机器人的一组最优关节角，包括：

将目标θ_7-predict、对应的[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]和

作为初始关节角，经过多次数值迭代，输出有偏置七轴机器人的一组最优关节角。

9.根据权利要求8所述的有偏置七轴机器人实时逆解算法，其特征在于，所述基于目标θ_7-predict、对应的[θ₁,θ₂,θ₃,θ₄,θ₅,θ_6-result]和

作为初始关节角，经过多次数值迭代，输出有偏置七轴机器人的一组最优关节角，包括：

每迭代一次，生成一组[θ₁,θ₂,θ₃，θ₄,θ₅,θ₆，θ₇]，根据[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆，θ₇]，通过正运动学解算，求得相连杆间的齐次变换矩阵，基于齐次变换矩阵，计算迭代过程中产生的误差；

重复进行迭代，直到迭代产生的误差小于设定的误差阈值。

Images