CN114714362B - 一种基于Levenberg-Marquard算法的六轴协作机器人校准方法 - Google Patents

Abstract

本发明涉及一种基于Levenberg‑Marquard算法的六轴协作机器人校准方法和六轴协作机器人。本发明提出的六轴协作机器人校准方法通过分析了六轴协作机器人D‑H参数及校准参数，建立了六轴协作机器人运动学正解模型，采用Levenberg‑Marquard算法迭代求解了符合误差范围的校准参数，该方法通过校准六轴协作机器人的连杆参数、减速比参数、关节零点参数等校准参数，可有效提高机器人算法应用精度，保证六轴协作机器人的安装便利以及运行精度。

Description

一种基于Levenberg-Marquard算法的六轴协作机器人校准 方法

技术领域

本发明涉及工业协作机器人的技术领域，特别是涉及一种基于Levenberg-Marquard算法的六轴协作机器人校准方法和六轴协作机器人。

背景技术

工业机器人的发展日臻成熟，只靠传统单一的机器人独立作业已远远不能满足市场各行各业的需求，尤其是在需要有人机配合作业的3C、医药、食品、物流等行业的自动化流水线上，所以这就有必要推出一种可以人机协作的更智能更安全的协作机器人。

协作机器人由于其运作高效、灵活性强、人机交互性好、安全可靠、重量轻等特点，受到越来越多的关注，被广泛应用于工业领域。为了保证六轴协作机器人的安装便利以及其精度，有必要研发一种使用激光跟踪仪校准六轴协作机器人的校准流程及方法，减少误差的影响，提高机器人控制的精确度以及算法应用精度。

发明内容

基于此，本发明的目的在于提供一种基于Levenberg-Marquard算法的六轴协作机器人校准方法，用于校准六轴协作机器人的连杆参数、减速比参数、关节零点参数等校准参数。

第一方面，本发明提供了一种基于Levenberg-Marquard算法的六轴协作机器人校准方法，所述六轴协作机器人包括

基座；

第一肩关节，固定于所述基座上，并可相对所述基座旋转；

第二肩关节，连接在所述第一肩关节的一侧，并可相对所述第一肩关节进行旋转，且能随着所述第一肩关节来回转动；

肩肘连接直筒，固定安装在所述第二肩关节上，能够随着所述第二肩关节运动；

肘关节，固定安装在所述肩肘连接直筒上，也能随着所述第二肩关节运动；

肘腕连接弯筒，连接在所述肘关节靠近所述基座的一侧，并可相对所述肘关节进行旋转；

肘腕连接直筒，固定安装在所述肘腕连接弯筒上，能够随着所述肘腕连接弯筒运动；

第一腕关节，固定安装在所述肘腕连接直筒上，也能随着所述肘腕连接弯筒运动；

第二腕关节，连接在所述第一腕关节靠近所述肩肘连接直筒的一侧，并能相对所述第一腕关节进行旋转；

第三腕关节，连接在所述第二腕关节上方，并可相对所述第二腕关节进行旋转；

末端安装座，连接在所述第三腕关节一侧，并可相对所述第三腕关节进行旋转；

所述校准方法包括：

S10：将靶球固定于机器人的末端安装座，架设激光跟踪仪；

S20：设置最大迭代次数m，设置极小值ε，随机取n(n≥50)组关节空间角度，任意两组至少有一对关节角相差不小于15°；

S30：建立运动学正解模型，根据该六轴协作机器人基于D-H参数模型和和机械臂关节的构型参数建立坐标系，求解协作六轴机器人末端位姿T，以及该位姿对校准参数的雅可比矩阵

S40：根据SVD法求解激光跟踪仪测量位置坐标P^m与机器人求解位置坐标P^s之间的转换矩阵Trans；

S50：根据Levenberg-Marquard算法迭代求解校准参数；

S60：判断校准参数是否符合误差范围。

第二方面，本发明提供了一种六轴协作机器人，包括：

至少一个存储器以及至少一个处理器；

所述存储器，用于存储一个或多个程序；

当所述一个或多个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如前所述的基于Levenberg-Marquard算法的六轴协作机器人校准方法的步骤。

附图说明

图1为本发明实施例所述的六轴协作机器人的结构示意图。

图2为本发明实施例所述的六轴协作机器人构建坐标系的示意图。

图3为图2坐标系简图。

图4为本发明实施例的校准流程图。

图中：1、基座；2、第一肩关节；3、第二肩关节；4、肩肘连接直筒；5、肘关节；6、肘腕连接弯筒；7、肘腕连接直筒；8、第一腕关节；9、第二腕关节；10、第三腕关节；11、末端安装座。

具体实施方式

以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

需要说明的是，当元件被称为“固定于”另一个元件，它可以是直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。

针对背景技术中的技术问题，本发明提供一种六轴协作机器人以及其基于Levenberg-Marquard算法的校准方法和装置，该六轴协作机器人如图1所示，基座1位于机器人最底部，第一肩关节2固定于基座1上，并可相对基座1旋转。第二肩关节3连接在第一肩关节2的一侧，并可相对第一肩关节2进行旋转，并能随着第一肩关节2来回转动。肩肘连接直筒4固定安装在第二肩关节3上，能够随着第二肩关节3运动。肘关节5固定安装在肩肘连接直筒4上，也能随着第二肩关节3运动。肘腕连接弯筒6连接在肘关节5靠近基座1的一侧，并可相对肘关节5进行旋转，肘腕连接直筒7固定安装在肘腕连接弯筒6上，能够随着肘腕连接弯筒6运动。第一腕关节8固定安装在肘腕连接直筒7上，也能随着肘腕连接弯筒6运动。第二腕关节9连接在第一腕关节8靠近肩肘连接直筒4的一侧，并能相对第一腕关节8进行旋转，第三腕关节10连接在第二腕关节9上方，并可相对第二腕关节9进行旋转，末端安装座11连接在第三腕关节10的一侧，并可相对第三腕关节10进行旋转。

可选的，基座1、第一肩关节2、第二肩关节3、肩肘连接直筒4、肘关节5、肘腕连接弯筒6、肘腕连接直筒7、第一腕关节8、第二腕关节9、第三腕关节10、末端安装座11的机械本体采用高强度的铝合金材料，各关节的密封盖采用高强度的合成塑料材料，最大限度的降低机器本体重量，同时又要保证机器的强度和刚度。

针对上述实施例中的六轴协作机器人，本发明还提供了一种基于Levenberg-Marquard算法的六轴协作机器人校准方法，如图4所示，该方法包括以下步骤：

S10：将靶球固定于机器人的末端安装座11，架设激光跟踪仪。

S20：设置最大迭代次数m，设置极小值ε，随机取n(n≥50)组关节空间角度，任意两组至少有一对关节角相差不小于15°。

S30：建立运动学正解模型，根据该六轴协作机器人基于D-H参数模型和和机械臂关节的构型参数建立坐标系，如图2-3所示，求解协作六轴机器人末端位姿T，以及该位姿对校准参数的雅可比矩阵可按以下步骤进行计算。

基于六轴协作机器人建立其构型参数坐标系，包括0系、1系、2系、3系、4系、5系、6系和工具坐标系(t_x t_y t_z)，其中：

1系原点由0系原点沿z0轴平移L1得到，z1轴由z0轴绕x0轴旋转得到，1轴模组减速比修正因子为k1，关节零点偏差可不考虑；

2系原点由1系原点沿x2轴平移L2得到，x2轴由x1轴绕z1轴旋转得到，2轴模组减速比修正因子为k2，关节零点偏差为θ_o2；

3系原点由2系原点沿x3轴平移L3得到，3轴模组减速比修正因子为k3，关节零点偏差为θ_o3；

4系原点由3系原点沿z3轴平移L7得到，x4轴由x3轴绕z3轴旋转得到，z4轴由z3轴绕x4轴旋转/>得到，4轴模组减速比修正因子为k4，关节零点偏差为θ_o4；

5系原点由4系原点沿z4轴平移L5得到，z5轴由z4轴绕x5轴旋转-得到，5轴模组减速比修正因子为k5，关节零点偏差为θ_o5；

6系原点由5系原点沿z5轴平移L6得到，6轴模组减速比修正因子为k6，关节零点偏差可不考虑。

因此，其D-H参数及校准参数如下所示：

综上，六轴协作机器人共有14个校准参数(不包括工具系)。

协作六轴机器人末端位姿T的公式为：

其中，A_i为i系在i-1系中的描述，即转换矩阵，A_i的求解公式为：

上述中c代表cos,s代表sin；Tool为工具系在6系中的描述，由于其只涉及平移，所以：

而该位姿对校准参数的雅可比矩阵的求解公式为：

S40：根据SVD法求解激光跟踪仪测量位置坐标P^m与机器人求解位置坐标P^s之间的转换矩阵Trans，可按以下几个子步骤进行计算。

S401：根据n组关节空间角度，使用运动学正解模型求解其对应的末端位姿：

其位置点集为：P^s＝(P^s ₁,P^s ₂,…,P^s _n)。

S402：使用激光跟踪仪测量n组关节空间角度对应的机器人末端靶球位置点集，用

P^m＝(P^m ₁,P^m ₂,…,P^m _n)表示。

S403：计算使用运动学正解模型求解的末端位置的重心μ^s，求解公式为：

计算使用激光跟踪仪测量的机器人末端靶球位置的重心μ^m，求解公式为：

S404：认定求解的末端位置点与测量的末端靶球位置点在绝对坐标系下重合，则其重心也在绝对坐标系下重合。可得上述两个位置点集相对于重心的相对坐标为：

上述为求解n个末端位置点的坐标值相对于其重心的相对坐标，/>为激光跟踪仪测试n个末端位置点的坐标值相对于其重心的相对坐标。

S405：构造协矩阵：

对协矩阵进行SVD分解：

其中UΛV代表SVD法将协矩阵分解成三个矩阵相乘，从上式可得，旋转矩阵即为R_3×3＝VU^T，平移矩阵为T_3×1＝μ^s-Rμ^m，则激光跟踪仪的测量位置坐标P^m到机器人求解位置坐标P^s的转换矩阵Trans为：

S50：根据Levenberg-Marquard算法迭代求解校准参数，可按以下几个子步骤进行计算。

S501：使用转换矩阵Trans求得新的求解位置P^s'，求解公式为：

即：

S502：求解位置坐标误差向量L，以及n个点位置坐标误差的有效值DL。位置坐标误差向量L(维数3n行，1列)的求解公式为：

而n个点位置坐标误差的有效值DL的求解公式为：

其中，rms表示一组数据的平方的平均值的平方根。

S503：求取校准参数的雅可比矩阵J。

第p个位姿雅可比矩阵为：

上式中，J_p是J的其中一个，J为n个J_p垂直排列得到，则完整雅可比矩阵为：

S504：根据雅克比矩阵J和对应点的位置坐标误差向量L求解海森矩阵H和误差向量g，

海森矩阵H的求解公式为：

H＝(J^T·J+μ·I)

其中，μ为一极小数量矩阵，I为单位矩阵。

误差向量g的求解公式为：

g＝J^T·L

并更新校准参数，以η_i+1表示，其公式为：

η_i+1＝η_i+H^-1·g

使迭代计数值i加1，以i++表示。

S60：判断校准参数是否符合误差范围，可按以下几个子步骤进行。

S601：判断误差向量g的模长是否小于极小值ε，如果小于极小值ε，则直接导出校准参数η_i，否则进行下一步；

S602：判断迭代计数值i是否大于最大迭代次数m，如果大于最大迭代次数m，则直接导出校准参数η_i，否则进行下一步；

S603：判断此次误差有效值DL与上一次误差有效值DL之差的绝对值是否小于极小值ε，如果小于极小值ε，则直接导出校准参数η_i，否则进行下一步；

S604：判断迭代计数值i是否等于1，如果等于1，则并带入更新校准参数η_i+1返回继续求解末端位姿，否则进行下一步；

S605：判断此次误差有效值DL是否小于上次误差有效值DL，如果小于上次误差有效值DL，设置μ＝μ/10，如果大于上次误差，设置μ＝μ×10，并带入更新校准参数η_i+1返回继续求解末端位姿。

相对于现有技术，本发明实施例所述的基于Levenberg-Marquard算法的六轴协作机器人校准方法用于校准六轴协作机器人的连杆参数、减速比参数、关节零点参数等校准参数，可有效提高机器人算法应用精度。

本发明还提供了一种六轴协作机器人，包括：

至少一个存储器以及至少一个处理器；

所述存储器，用于存储一个或多个程序；

当所述一个或多个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如本发明上述的基于Levenberg-Marquard算法的六轴协作机器人校准方法的步骤。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (5)

1.一种基于Levenberg-Marquard算法的六轴协作机器人校准方法，其特征在于，所述六轴协作机器人包括：

基座；

第一肩关节，固定于所述基座上，并可相对所述基座旋转；

第二肩关节，连接在所述第一肩关节的一侧，并可相对所述第一肩关节进行旋转，且能随着所述第一肩关节来回转动；

肩肘连接直筒，固定安装在所述第二肩关节上，能够随着所述第二肩关节运动；

肘关节，固定安装在所述肩肘连接直筒上，也能随着所述第二肩关节运动；

肘腕连接弯筒，连接在所述肘关节靠近所述基座的一侧，并可相对所述肘关节进行旋转；

肘腕连接直筒，固定安装在所述肘腕连接弯筒上，能够随着所述肘腕连接弯筒运动；

第一腕关节，固定安装在所述肘腕连接直筒上，也能随着所述肘腕连接弯筒运动；

第二腕关节，连接在所述第一腕关节靠近所述肩肘连接直筒的一侧，并能相对所述第一腕关节进行旋转；

第三腕关节，连接在所述第二腕关节上方，并可相对所述第二腕关节进行旋转；

末端安装座，连接在所述第三腕关节一侧，并可相对所述第三腕关节进行旋转；

所述校准方法包括：

S10：将靶球固定于机器人的所述末端安装座，架设激光跟踪仪；

S20：设置最大迭代次数m，设置极小值ε，随机取n(n≥50)组关节空间角度，任意两组至少有一对关节角相差不小于15°；

S30：建立运动学正解模型，根据该六轴协作机器人基于D-H参数模型和机械臂关节的构型参数建立坐标系，求解协作六轴机器人末端位姿T，以及对校准参数的雅可比矩阵

S40：根据SVD法求解激光跟踪仪测量位置坐标P^m与机器人求解位置坐标P^s之间的转换矩阵Trans；

S50：根据Levenberg-Marquard算法迭代求解校准参数；

S60：判断校准参数是否符合误差范围；

步骤S50中根据Levenberg-Marquard算法迭代求解符合误差范围的校准参数包括以下步骤：

S501：使用转换矩阵Trans求得新的求解位置P^s′；

S502：求解位置坐标误差向量L，以及n个点位置坐标误差的有效值DL；

S503：求取校准参数的雅可比矩阵记所述雅可比矩阵/>为J；

S504：根据雅克比矩阵J和对应点的位置坐标误差向量L求解海森矩阵H和误差向量g，并更新校准参数，以η_i+1表示，使迭代计数值i加1，以i++表示；

步骤S501中使用转换矩阵Trans求得新的求解位置P^s′的公式为：

即：/>

步骤S502中求解位置坐标误差向量L，以及n个点位置坐标误差的有效值DL的公式为：

其中，rms表示一组数据的平方的平均值的平方根；

步骤S503中求取校准参数的雅可比矩阵J的公式为：

第p个位姿雅可比矩阵为：

J_p是J的其中一个，J为n个J_p垂直排列得到，则完整雅可比矩阵为：

步骤S504中海森矩阵H的求解公式为：

H＝(J^T·J+μ·I)

其中，μ为一极小数量矩阵，I为单位矩阵；

误差向量g的求解公式为：

g＝J^T·L

更新校准参数η_i+1的公式为：

η_i+1＝η_i+H^-1·g。

2.根据权利要求1所述的一种基于Levenberg-Marquard算法的六轴协作机器人校准方法，其特征在于，步骤S30中的构型参数坐标系为：

z0、z1、z2、z3、z4、z5为1、2、3、4、5、6关节旋转轴，以t_x t_y t_z表示工具坐标；

1系原点由0系原点沿z0轴平移L1得到，z1轴由z0轴绕x0轴旋转得到，1轴模组减速比修正因子为k1，关节零点偏差可不考虑；

2系原点由1系原点沿x2轴平移L2得到，x2轴由x1轴绕z1轴旋转得到，2轴模组减速比修正因子为k2，关节零点偏差为θ_o2；

3系原点由2系原点沿x3轴平移L3得到，3轴模组减速比修正因子为k3，关节零点偏差为θ_o3；

4系原点由3系原点沿z3轴平移L7得到，x4轴由x3轴绕z3轴旋转得到，z4轴由z3轴绕x4轴旋转/>得到，4轴模组减速比修正因子为k4，关节零点偏差为θ_o4；

5系原点由4系原点沿z4轴平移L5得到，z5轴由z4轴绕x5轴旋转得到，5轴模组减速比修正因子为k5，关节零点偏差为θ_o5；

6系原点由5系原点沿z5轴平移L6得到，6轴模组减速比修正因子为k6，关节零点偏差可不考虑；

步骤S30中求解协作六轴机器人末端位姿T的公式为：

其中，A_i为i系在i-1系中的描述，即转换矩阵，A_i的求解公式为：

上述中c代表cos,s代表sin；

以及该位姿对校准参数的雅可比矩阵的求解公式为：

其中：η_g为校准参数，Tool为工具系在6系中的描述，由于其只涉及平移，所以：

3.根据权利要求2所述的一种基于Levenberg-Marquard算法的六轴协作机器人校准方法，其特征在于，步骤S40根据SVD法求解激光跟踪仪测量位姿与机器人求解位姿之间的转换矩阵包括以下步骤：

S401：根据n组关节空间角度，使用运动学正解模型求解其对应的末端位姿：其位置点集为：P^s＝(P^s ₁,P^s ₂,…,P^s _n)；

S402：使用激光跟踪仪测量n组关节空间角度对应的机器人末端靶球位置点集，用

P^m＝(P^m ₁,P^m ₂,…,P^m _n)表示；

S403：计算使用运动学正解模型求解的末端位置的重心μ^s，以及使用激光跟踪仪测量的机器人末端靶球位置的重心μ^m，其求解公式为：

S404：在认定求解的末端位置点与测量的末端靶球位置点在绝对坐标系下重合，则其重心也在绝对坐标系下重合，求解n个末端位置点的坐标值相对于其重心的相对坐标激光跟踪仪测试n个末端位置点的坐标值相对于其重心的相对坐标/>其求解公式为：

S405：构造协矩阵，对协矩阵进行SVD分解，求得激光跟踪仪的测量位置坐标P^m到机器人求解位置坐标P^s的转换矩阵Trans，其中构造协矩阵的公式为：

对协矩阵进行SVD分解公式为：

其中UΛV代表SVD法将协矩阵分解成三个矩阵相乘，从上式可得，旋转矩阵即为R_3×3＝VU^T，平移矩阵为T_3×1＝μ^s-Rμ^m，则激光跟踪仪的测量位置坐标P^m到机器人求解位置坐标P^s的转换矩阵Trans为：

4.根据权利要求1所述的一种基于Levenberg-Marquard算法的六轴协作机器人校准方法，其特征在于，步骤S60中判断校准参数是否符合误差范围包括以下步骤：

S601：判断误差向量g的模长是否小于极小值ε，如果小于极小值ε，则直接导出校准参数η_i，否则进行下一步；

S602：判断迭代计数值i是否大于最大迭代次数m，如果大于最大迭代次数m，则直接导出校准参数η_i，否则进行下一步；

S603：判断此次误差有效值DL与上一次误差有效值DL之差的绝对值是否小于极小值ε，如果小于极小值ε，则直接导出校准参数η_i，否则进行下一步；

S604：判断迭代计数值i是否等于1，如果等于1，则并带入更新校准参数η_i+1返回继续求解末端位姿，否则进行下一步；

S605：判断此次误差有效值DL是否小于上次误差有效值DL，如果小于上次误差有效值DL，设置μ＝μ/10，如果大于上次误差，设置μ＝μ×10，并带入更新校准参数η_i+1返回继续求解末端位姿。

5.一种六轴协作机器人，其特征在于，包括：

至少一个存储器以及至少一个处理器；

所述存储器，用于存储一个或多个程序；

当所述一个或多个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如权利要求1至4任一项所述基于Levenberg-Marquard算法的六轴协作机器人校准方法的步骤。