CN110253574B - 一种多任务机械臂位姿检测和误差补偿方法 - Google Patents

Abstract

本发明公开了一种多任务机械臂位姿检测和误差补偿方法，属于工业机器人技术领域，该方法所使用的装置包括双摄像机、六关节机械臂、上位机和机械臂控制柜。两台摄像机分别安装在机械臂两侧，用于采集机械臂的图像信息并将其传送至上位机，从而获取机械臂的位姿信息；六关节机械臂用于完成焊接、零件装配和货物搬运工作；上位机用于进行图像的标定和处理、任务模式的选择和数据的计算；机械臂控制柜用于接收上位机的指令，实现对机械臂的控制。依据优化函数最小化的原则，计算每一步的新增误差补偿量，从而获得新的误差补偿量，进而控制机械臂的下一步运动。

Description

一种多任务机械臂位姿检测和误差补偿方法

技术领域

本发明涉及工业机器人技术领域，具体地说，涉及一种多任务机械臂位姿检测和误差补偿方法。

背景技术

近年来，机械臂由于具有操作灵活、控制方便的特点，被广泛应用于焊接、零件装配和货物搬运等任务中。由于机械臂的各连杆在制造、装配等过程中不可避免地要引入参数误差，这就使得机械臂各杆件参数的实际值与名义值之间存在差异，从而导致机械臂末端实际到达的位姿与期望到达的位姿之间存在着一定的偏差。

为实现机械臂精确的运动控制，需要对其进行位姿误差补偿。现在的运动控制方案主要存在以下问题：

(1)公开号为CN107457785A的中国专利文献公开了一种基于关节反馈的机器人位置补偿方法，该方法在分析机械臂运动学的同时，还需要对电机进行精确的数学建模，分析电机转动的传递函数，建模和控制器设计过程复杂，数据处理量大；

(2)公开号为CN108297101A的中国专利文献公开了一种多关节臂串联机器人末端位姿误差检测和动态补偿方法，该方法利用倾角传感器获取末端位姿误差。由于各杆参数的实际值与名义值存在差异，因此位姿获取不够准确；

(3)公开号为CN106247932A的中国专利文献公开了一种基于摄影系统的机器人在线误差补偿装置及方法，该方法运用到了多组相机和二维倾角测量仪，硬件系统较为复杂，且未提出较为明确的位姿误差补偿算法；

(4)以上各专利未考虑不同任务位姿补偿需求的差异性，其设计只针对某一项任务(例如焊接、零件装配和货物搬运)。

发明内容

本发明的目的为提供一种多任务机械臂位姿检测和误差补偿方法，该方法可以针对不同的任务、不同的工作环境进行特定的位姿补偿，适用性强。

为了实现上述目的，本发明提供的多任务机械臂位姿检测和误差补偿方法包括以下步骤：

1)标定双目立体视觉传感器，建立机械臂的D-H运动模型，得到D-H运动模型的各坐标转换矩阵^jT₆(j＝0～5)的理论表达式；

2)设置机械臂的任务类型，根据该任务类型及工作环境，自动设置位姿补偿的权重系数w_i(i＝1～6)。该权重系数表征不同方向位置调整和姿态调整的重要程度，从而可以使机械臂能够使用不同的工况和任务要求。例如，在承担零件装配任务时同时注重位置和姿态角精度，在承担焊接和货物搬运任务时更加注重位置精度；在距离工作点较远时更加注重位置精度的控制，在距离较近时同时注重位置和姿态角的控制。

3)给定目标位姿，求取各关节角度q_t＝[q₁,q₂,q₃.q₄,q₅,q₆]^T，并设置初始误差补偿量Δq＝[0,0,0,0,0,0]^T；

4)根据双目立体视觉的原理，利用两台摄像机同时获取机械臂的图像，确定机械臂末端点在两个图像中的具体位置，根据两台摄像机的位置与机械臂末端点的图像信息反推出机械臂末端的位姿信息，计算末端位姿的综合误差D＝[d_x,d_y,d_z,δ_x,δ_y,δ_z]^T，其中，d_x,d_y,d_z为位置误差，δ_x,δ_y,δ_z为姿态误差；

5)根据各坐标转换矩阵^jT₆求取雅克比矩阵J(q)；

6)求取各关节的新增误差补偿量Δq_add，则新的误差补偿量为Δq＝Δq+Δq_add；

7)给定下一点的目标位姿，求取各关节的理论角度q_t，并设置各关节的实际运动角度为q＝q_t-Δq，按照q进行运动；

8)重复步骤4)～7)，直至完成任务。

上述技术方案中，根据不同的任务类型和工况情况自动生成合适的位姿补偿的权重系数，利用双目立体视觉的原理获取机械臂的位姿信息，根据机械臂的雅克比矩阵实时迭代产生该权重系数下的各运动关节误差补偿量，从而实现机械臂的实时误差补偿。

作为优选，步骤1)中，假设从第k-1个关节(第0个关节为基座)到第k个关节的坐标变换为：

上式中，关节转角θ_i表示X_i-1轴绕Z_i-1轴转动到X_i轴所需旋转的角度；关节偏置d_i表示X_i-1轴沿着Z_i-1轴移动到X_i轴的距离；连杆扭角α_i表示Z_i-1轴绕X_i轴转动到Z_i轴所需旋转的角度；连杆长度a_i表示Z_i-1轴沿着X_i轴转动到Z_i轴所需旋转的角度。

则机械臂末端的坐标系与连杆j-1坐标系的转换矩阵^jT₆(j＝0～5)的理论表达式为：

其中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、p_x、p_y、p_z分别表示^jT_n前三行中12个元素对应的数值。

作为优选，步骤3)中，初始误差补偿量Δq＝[0,0,0,0,0,0]^T。

作为优选，步骤5)中，雅克比矩阵J(q)的第m列由^mT₆决定，为：

其中，n,o,a和p是^mT₆的四个列向量。

作为优选，步骤6)中，根据优化函数最小化原则，求取各关节的新增误差补偿量Δq_add，设置优化函数为：

其中，W＝diag{w₁,w₂,w₃,w₄,w₅,w₆}，diag用于构造对角矩阵。令

得到：

-J(q)^TW[D-J(q)Δq_add]＝0

由上式，可得到各关节新增误差补偿量：

Δq_add＝[Δq_1add,Δq_2add,Δq_3add,Δq_4add,Δq_5add,Δq_6add]^T

作为优选，步骤3)和步骤7)中，通过逆运动学法求取各关节的理论角度q_t。机器人的逆向运动学即已知机器人末端的位置和姿态，根据已知的物理参数(如各机械臂连杆的长度等)逆向求解各关节相应的转动角度的过程。

与现有技术相比，本发明的有益效果为：

(1)本发明的方法利用机械臂的物理参数进行建模，而不需要研究电机的运动特性，具有设计方便、数据量小的优点；

(2)本发明的方法利用机器视觉的原理，采集机械臂末端的位姿信息，具有布置方便、测量精确的优点；

(3)本发明的方法可以针对不同的任务、不同的工作环境进行特定的位姿补偿，适用性强。

附图说明

图1是本发明实施例中多任务机械臂位姿检测和误差补偿方法所使用装置的结构示意图；

图2是本发明实施例中多任务机械臂位姿检测和误差补偿方法的流程图；

图3是多任务机械臂的D-H模型图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，以下结合实施例及其附图对本发明作进一步说明。

实施例

参见图1，本实施例的多任务机械臂位姿检测和误差补偿方法所使用的装置包括双摄像机(摄像机4和摄像机5)、六关节机械臂1、上位机2和机械臂控制柜3。

两台摄像机分别安装在六关节机械臂1的两侧，用于采集机械臂的图像信息并将其传送至上位机2，从而获取机械臂的位姿信息。六关节机械臂1用于完成焊接、零件装配和货物搬运工作。上位机2用于进行图像的标定和处理、任务模式的选择和数据的计算。机械臂控制柜3用于接收上位机的指令，实现对机械臂的控制。

参见图2，本实施例的多任务机械臂位姿检测和误差补偿方法包括以下步骤：

步骤S1，标定双摄像头传感器，导入机械臂的名义物理参数，在考虑参数误差的基础上，建立机械臂的D-H运动模型。

已知从第k-1个关节(第0个关节为基座)到第k个关节的坐标变换为：

式中各符号示意见图3。则机械臂末端(连杆6)的坐标系与连杆j-1坐标系的转换矩阵^jT₆(j＝0～5)为：

其中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、p_x、p_y、p_z分别表示^jT₆前三行中12个元素对应的数值。

步骤S2，设置机械臂的任务类型，例如焊接、零件装配或者货物搬运。

步骤S3，控制器根据机械臂的任务类型和工作环境，利用模糊控制的原理，输出位姿补偿的权重系数w_i(i＝1～6)。其权重设置的基本思路为：在承担零件装配任务时同时注重位置和姿态角精度，在承担焊接和货物搬运任务时更加注重位置精度；在距离工作点较远时更加注重位置精度的控制，在距离较近时同时注重位置和姿态角的控制。

步骤S4，给定目标位姿，通过逆运动法，按照能量使用最小的原则，求取唯一的一组关节理论角度q_t＝[q₁,q₂,q₃.q₄,q₅,q₆]^T，设置初始误差补偿量Δq＝[0,0,0,0,0,0]^T；

步骤S5，利用机器视觉的原理，测取机械臂末端的位姿信息，计算末端位姿的综合误差D＝[d_x,d_y,d_z,δ_x,δ_y,δ_z]^T，其中d_x,d_y,d_z为位置误差，δ_x,δ_y,δ_z为姿态误差；

步骤S6，实时求取各坐标转换矩阵^jT₆，得到雅克比矩阵J(q)。由于几个关节都为转动关节，因此雅克比矩阵J(q)的第m列由^mT₆决定，为：

式中，n,o,a和p是^mT₆的四个列向量。

步骤S7，构建权重矩阵W＝diag{w₁,w₂,w₃,w₄,w₅,w₆}，设置优化函数

令

得到：

-J(q)^TW[D-J(q)Δq_add]＝0

由此可得各关节新增误差补偿

Δq_add＝[Δq_1add,Δq_2add,Δq_3add,Δq_4add,Δq_5add,Δq_6add]^T

求取新的误差补偿量Δq＝Δq+Δq_add。

步骤S8，给定下一点的目标位姿，通过逆运动法求取各关节理论角度值：

q_t＝[q₁,q₂,q₃.q₄,q₅,q₆]^T

设置各关节按照实际运动角度q＝q_t-Δq进行运动；

步骤S9，重复步骤S5～步骤S8，直至完成所需任务。

Claims (6)

1.一种多任务机械臂位姿检测和误差补偿方法，其特征在于，包括以下步骤：

1)标定双目立体视觉传感器，建立机械臂的D-H运动模型，得到D-H运动模型的各坐标转换矩阵^jT₆(j＝0～5)的理论表达式；

2)设置机械臂的任务类型，根据该任务类型及工作环境，系统自动生成位姿补偿的权重系数w_i(i＝1～6)；

3)给定目标位姿，求取各关节的理论角度q_t＝[q₁,q₂,q₃.q₄,q₅,q₆]^T，并设置初始误差补偿量Δq＝[0,0,0，0,0,0]^T；

4)利用双目立体视觉的方法，测量机械臂末端的位姿信息，计算末端位姿的综合误差D＝[d_x,d_y,d_z,δ_x,δ_y,δ_z]^T，其中，d_x,d_y,d_z为位置误差，δ_x,δ_y,δ_z为姿态误差；

5)实时求取各坐标转换矩阵，得到雅克比矩阵J(q)；

6)求取各关节的新增误差补偿量Δq_add，则新的误差补偿量为Δq＝Δq+Δq_add；

7)给定下一点的目标位姿，求取各关节的理论角度q_t＝[q₁,q₂,q₃.q₄,q₅,q₆]^T，并设置各关节实际运动角度为q＝q_t-Δq进行运动；

8)重复步骤4)～7)，直至完成任务。

2.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤1)中，假设从第k-1个关节(第0个关节为基座)到第k个关节的坐标变换为：

上式中，关节转角θ_i表示X_i-1轴绕Z_i-1轴转动到X_i轴所需旋转的角度；关节偏置d_i表示X_i-1轴沿着Z_i-1轴移动到X_i轴的距离；连杆扭角α_i表示Z_i-1轴绕X_i轴转动到Z_i轴所需旋转的角度；连杆长度a_i表示Z_i-1轴沿着X_i轴转动到Z_i轴所需旋转的角度；

则机械臂末端的坐标系与连杆j-1坐标系的转换矩阵^jT₆(j＝0～5)的理论表达式为：

其中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、p_x、p_y、p_z分别表示^jT₆前三行中12个元素对应的数值。

3.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤3)中，初始误差补偿量Δq＝[0,0,0,0,0,0]^T。

4.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤5)中，雅克比矩阵J(q)的第m列由^mT₆决定，可表示为：

其中，n,o,a和p是^mT₆的四个列向量。

5.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤6)中，根据优化函数最小化原则，求取各关节的新增误差补偿量Δq_add，设置优化函数为：

其中，W＝diag{w₁,w₂,w₃,w₄,w₅,w₆}，令

得到：

-J(q)^TW[D-J(q)Δq_add]＝0

由上式，可得到各关节新增误差补偿量：

Δq_add＝[Δq_1add,Δq_2add,Δq_3add,Δq_4add,Δq_5add,Δq_6add]^T。

6.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤3)和步骤7)中，通过逆运动法求取各关节的理论角度。

Images