CN113134837A - 一种基于六自由度串联机械臂提高绝对定位精度的方法 - Google Patents

Abstract

本发明涉及自适应控制领域，具体涉及一种基于六自由度串联机械臂提高绝对定位精度的方法，首先通过激光跟踪仪获取末端靶点信息和预处理进行机械臂与激光跟踪仪之间的坐标转换；然后运用李群李代数建立机械臂的指数积模型与序列二次规划算法求全局最小值的方法相融合，对机械臂关节参数偏差所产生的末端几何误差进行补偿；最后通过激光跟踪仪获取到的实际点位和指数积模型求解运动学逆解，用高斯过程回归算法进行模型训练，对非几何运动误差进行补偿预测，将预测出的补偿后的角度值输入到示教器。本发明能够更加精确的计算出机械臂的实际运动学模型参数，并减小末端点位误差以实现提高机械臂绝对定位精度。

Description

一种基于六自由度串联机械臂提高绝对定位精度的方法

技术领域

本发明涉及自适应控制领域，具体涉及一种基于六自由度串联机械臂提高绝对定位精度的方法。

技术背景

机械臂末端位置误差补偿问题由于其在多个学科领域，如数值计算、自适应控制、凸优化、智能夹取与装配等，所具有的潜在应用前景和价值而吸引了越来越多的研究兴趣。机械臂因为运动学模型参数不精确，装配误差，关节轴零位位置偏差等各种因素，造成机械臂末端名义点位与实际点位偏差过大，这给进一步的机械臂运动控制带来了更多挑战。因此，多种机械臂末端位置补偿方法已经用于机械臂末端绝对定位精度提高中，例如激光跟踪仪在线实时反馈系统、计算机视觉辅助系统、触及信息反馈系统等。然而，诸多未知的、不确定的影响因素，包括电机齿轮的磨损、关节轴间隙误差、结构不匹配、外部干扰和噪声等，通常都存在于非线性的误差源中。这些不确定性会导致仅仅通过几何误差补偿无法进一步提高精度，影响并限制机械臂进行高精度作业任务。因此，机械臂末端绝对定位精度提高即末端位置误差补偿问题被强烈激发出来。如何通过激光跟踪仪进行机械臂运动学参数标定和非几何误差补偿，进一步提高机械臂绝对定位精度，对于实际的复杂工程应用和高端技术研发具有很大意义。

发明内容

为了解决现有技术中存在的上述提到的技术问题，本发明对机械臂末端绝对定位精度较低情况，设计了一种基于六自由度串联机械臂提高绝对定位精度的方法，该方法在参数辨识中将关节轴零位偏差放入非几何误差补偿部分，可以降低参数辨识的维度，同时把剩余的末端点位误差映射到关节角度进行补偿，进一步提高机械臂末端绝对定位精度，其具体技术方案如下：

一种基于六自由度串联机械臂提高绝对定位精度的方法，包括如下步骤：

S1.首先构建机械臂与激光跟踪仪坐标转换矩阵：通过激光跟踪仪获取末端靶点信息和预处理进行机械臂与激光跟踪仪之间的坐标转换；

S2.设计机械臂运动学参数辨识方法：运用李群李代数建立机械臂的指数积模型与序列二次规划算法求全局最小值的方法相融合，对机械臂关节参数偏差所产生的末端几何误差进行补偿；

S3.设计高斯过程回归模型并训练得到补偿角度：通过激光跟踪仪获取到的实际点位和指数积模型求解运动学逆解，对非几何运动误差进行补偿，获取到示教器的角度值和运动学逆解求出的角度值，用高斯过程回归算法进行模型训练，使得模型通过输入目标角度，得到补偿后的角度值，最后将预测出的补偿后的角度值输入到示教器。

进一步的，所述步骤S1具体包括：

机械臂末端标定及将激光跟踪仪坐标系下测得坐标，转换为机械臂坐标系下坐标，通过转动机械臂的1轴和5轴提前计算出1轴和5轴的实际杆长，设计坐标系转换表达式为：

p₀+p₁·x+p₂·y+z＝0 (1)

公式(1)中，(x_i,y_i,z_i)为通过转动1轴获取到的机械臂末端靶点信息，(x_i,y_i,z_i)是以激光跟踪仪坐标系作为基坐标系，(x_ball,y_ball,z_ball)代表拟合出采样点所在球面的球心，(p₁,p₂,1)为通过多项式拟合采样点所在平面所得到的垂直于平面的法向量，归一化得到方向向量n₃；

通过最小化距离平方获取拟合球的球心(x_ball,y_ball,z_ball)，与拟合出的平面法向量(p₁,p₂,1)相结合，设计求拟合平面圆心公式：

p₁·(x-x_ball)+p₂·(y-y_ball)-(z-z_ball)＝0

p₀+p₁·x+p₂·y-z＝0 (2)

通过求解等式(2)获得1轴转动拟合圆的圆心(x_c,y_c,z_c)，此时(x_c,y_c)也是机械臂基座标原点与激光跟踪仪基座标之间的距离；

利用同样方法转动2轴采集点位，获取到2轴拟合平面的法向量并归一化得方向向量o₂，此时获取到的拟合圆心z_c等于1轴杆长l₁，然后通过z轴方向向量a₃和y轴方向向量o₂叉乘获取x轴方向向量n₁；

根据获取到的方向向量n₁,o₂,a₃,以及转动1轴得到的(x_1c,y_1c,z_1c)得到激光跟踪仪基座标到机械臂基座标的转换矩阵：

对

求逆，获得

通过

获得机械臂坐标系下的采样点坐标，通过转动5轴，拟合出圆，计算出圆半径，减去靶点夹具长度，获得第5轴杆长l₅信息。

进一步的，所述步骤S2具体包括：

基于指数积模型，构造机械臂的运动学模型：

公式(4)中，F(θ)为机械臂关节转角为θ时，机械臂末端的位姿，其中M为θ_1～6＝0时末端位姿，依据李群李代数公式获得e^[s]θ如下：

其中

通过公式(4)和公式(5)结合，可以求得名义机械臂末端位姿F(θ)，取矩阵中的点位部分，得到机械臂末端点位为

T_p＝f_fk(θ，x) (6)

T_p∈SE(3)x为第2、3、4轴杆长信息，θ为从示教器中获得的机械臂名义关节转角。

根据公式(6)中计算出的名义末端点位和激光跟踪仪采样得到的实际点位结合，辨识出真实的杆长信息：

min∑||T_real-T_p||

＝min∑||T_real-f_fk(θ，l)|| (7)

采用序列二次规划算法进行求解变量l，使得∑||T_real-T_p||最小：

minf(x)

b(x)≥0

c(x)＝0 (8)

其中，通过b(x)≥0来约束杆长的变化范围。

进一步的，所述步骤S3具体包括：

经过公式(7)辨识出的第2、3、4轴杆长信息，和步骤S1中获得的第1、5轴杆长，通过指数积模型求逆解，获得采样点位的真实关节角度θ_real，根据高斯过程回归公式，设置输入为通过逆解求出的1～6轴的真实角度θ_real,

输出为1～6轴的名义值θ_nominal,

θ_nominal～N(0,K)

其中θ_reali代表第i次解算得到的6轴角度，K为某一种合适的协方差函数，同时设定k(θ_reali,θ_realj)＝K_ij，据高斯过程公式，新的输入项θ'_real，过去观察得到的数据为(θ_real,θ_nominal)，他们的分布满足：

同时新的输入项θ'_real和相关的输出θ'_nominal也满足公式(10)的高斯分布，因此给定新的输入项θ'_real，相应的输出项θ'_nominal可以通过公式(11)预测出：

根据公式(11)，得出输入到示教器中的名义值，使得机械臂末端逼近目标点位，提高机械臂的绝对定位精度。

本发明的优点：

本发明能够更加精确的计算出机械臂的实际运动学模型参数，并减小末端点位误差以实现提高机械臂绝对定位精度。

附图说明

附图1是本发明的机械臂误差补偿模型示意图；

附图2是本发明的机械臂结构示意图；

附图3是本发明的误差补偿效果示意图；

附图4a是本发明的控制方法流程示意图；

附图4b是本发明的运动学几何误差补偿流程示意图；

附图4c是本发明的运动学非几何误差补偿流程示意图；

附图5是本发明实施例的机械臂各个关节的参数值表示意图，

其中，1机械臂，2激光跟踪仪靶点，3激光跟踪仪。

具体实施方式

为了使本发明的目的、技术方案和技术效果更加清楚明白，下面结合说明书附图和具体实施步骤来对本发明的技术方案作进一步详细的说明。

一种基于六自由度串联机械臂提高绝对定位精度的方法，所述方法包含以下步骤：

首先构建机械臂与激光跟踪仪坐标转换矩阵：机械臂末端标定及将激光跟踪仪坐标系下测得坐标，转换为机械臂坐标系下坐标，通过转动1轴和5轴提前计算出1轴和5轴的实际杆长，设计坐标系转换表达式为：

p₀+p₁·x+p₂·y+z＝0 (1)

公式(1)中，(x_i,y_i,z_i)为通过转动1轴获取到的机械臂末端靶点信息，(x_i,y_i,z_i)是以激光跟踪仪坐标系作为基坐标系，(x_ball,y_ball,z_ball)代表拟合出采样点所在球面的球心，(p₁,p₂,1)为通过多项式拟合采样点所在平面所得到的垂直于平面的法向量，归一化得到方向向量n₃；

通过最小化距离平方获取拟合球的球心(x_ball,y_ball,z_ball)，与拟合出的平面法向量(p₁,p₂,1)相结合，设计求拟合平面圆心公式：

p₁·(x-x_ball)+p₂·(y-y_ball)-(z-z_ball)＝0

p₀+p₁·x+p₂·y-z＝0 (2)

通过求解等式(2)获得1轴转动拟合圆的圆心(x_c,y_c,z_c)，此时(x_c,y_c)也是机械臂基座标原点与激光跟踪仪基座标之间的距离，利用同样方法转动2轴采集点位，获取到2轴拟合平面的法向量并归一化得方向向量o₂，此时获取到的拟合圆心z_c等于1轴杆长l₁，然后通过z轴方向向量a₃和y轴方向向量o₂叉乘获取x轴方向向量n₁；

根据获取到的方向向量n₁,o₂,a₃,以及转动1轴得到的(x_1c,y_1c,z_1c)得到激光跟踪仪基座标到机械臂基座标的转换矩阵：

对

求逆，获得

通过

获得机械臂坐标系下的采样点坐标，通过转动5轴，利用上述方法拟合出圆，计算出圆半径，减去靶点夹具长度，获得第5轴杆长l₅信息。

然后设计机械臂运动学参数辨识方法，该几何参数辨识方法无需每次刻意选择固定点位，它能够随机选取若干采样点，根据机械臂作用域内随机点位数据不断更新和迭代机械臂运动学模型参数，所以能够根据系统状态在很短时间内辨识出机械臂指数积模型的实际参数，将被控机械臂绝对定位精度收敛到期望值(域)附近，具体的：

基于指数积模型，构造机械臂的运动学模型：

公式(4)中，F(θ)为机械臂关节转角为θ时，机械臂末端的位姿，其中M为θ_1～6＝0时末端位姿，依据李群李代数公式获得e^[s]θ如下：

其中

通过公式(4)和公式(5)结合，可以求得名义机械臂末端位姿F(θ)，取矩阵中的点位部分，得到机械臂末端点位为

T_p＝f_fk(θ，x) (6)

T_p∈SE(3)x为第2、3、4轴杆长信息，θ为从示教器中获得的机械臂名义关节转角。

根据公式(6)中计算出的名义末端点位和激光跟踪仪采样得到的实际点位结合，辨识出真实的杆长信息：

min∑‖T_real-T_p‖

＝min∑‖T_real-f_fk(θ，l)‖ (7)

公式(7)中对二次多项式求最小值问题，针对这一具有平方项求全局最值设计，采用序列二次规划算法进行求解变量l，使得∑‖T_real-T_p||最小：

minf(x)

b(x)≥0

c(x)＝0 (8)

其中，通过b(x)≥0来约束杆长的变化范围，防止求得的极值点不满足实际意义。

最后通过激光跟踪仪获取到的实际点位和指数积模型求解运动学逆解，相当于把剩余非几何误差映射到关节角度中，设计高斯过程回归进行误差预测：针对非几何误差补偿部分，经过公式(7)辨识出的第2、3、4轴杆长信息，和步骤S1中获得的第1、5轴杆长，通过指数积模型求逆解，获得采样点位的真实关节角度θ_real，根据高斯过程回归公式，设置输入为通过逆解求出的1～6轴的真实角度θ_real,

输出为1～6轴的名义值θ_nominal,

θ_nominal～N(0,K)

其中θ_reali代表第i次解算得到的6轴角度，K为某一种合适的协方差函数，同时设定k(θ_reali,θ_realj)＝K_ij，据高斯过程公式，新的输入项θ'_real，过去观察得到的数据为(θ_real,θ_nominal)，分布满足：

同时新的输入项θ'_real和相关的输出θ'_nominal也满足公式(10)的高斯分布，因此给定新的输入项θ'_real，相应的输出项θ'_nominal可以通过公式(11)预测出：

根据公式(11)，可以得出输入到示教器中的名义值，使得机械臂末端逼近目标点位，提高机械臂的绝对定位精度。

具体实施例：

如图1所示，选取工业机械臂标定的场景来进行技术方案的实施，设备由机械臂、激光跟踪仪、定值的夹持靶点的夹具组成，靶点安装在机械臂末端用于测定末端点位，其中，机械臂的名义杆长参数已知，机械臂连杆的参数如图2所示，其中第六轴杆长信息设置为0，其余轴设置为

l₁＝455mm,l₂＝592mm,l₃＝153mm,l₄＝638mm,l₅＝193mm,l₆＝150mm。机械臂在出厂之前进行了标定并回到零位。同时已经知道夹具长度，并且安装的夹具与末端法兰盘保证同轴心。

首先转动5轴，拟合出所在平面圆弧圆心，求出圆弧半径，得到参数l₅＝193.16mm，转动1轴，同时用激光跟踪仪进行采样，获得一条圆弧采样轨迹，通过最小二乘法拟合出采样平面内圆弧的圆心(x_1c,y_1c,z_1c)以及平面法向量(p₁,p₂,1)归一化为n₁。然后机械臂返回零点，再转动2轴，用同样的方法获取采样平面的法向量并归一化为o₂，计算n₁×o₂得y方向向量。综合得到机械臂到激光跟踪仪的转换矩阵

对

求逆，获得

通过

获得机械臂坐标系下的采样点坐标。

在机械臂坐标系{B}中，机械臂末端的正向运动学为：

对应的

q具体值如图5所示，θ值从示教器中读取，记为θ_nominal，通过计算

取F(θ)中的点位信息，获得末端坐标的名义值，然后利用序列二次规划算法求解变量l_2,3,4,6，求出min∑||T_real-f_fk(θ，l)||，得到真实的杆长信息l。本实施例例中通过计算得出l₂＝592.467mm,l₃＝152.398mm,l₄＝641.202mm,l₆＝150.186mm，将获得的杆长信息，带入机械臂指数积模型求逆解，输出θ_real。

设置高斯过程回归核函数为平方指数函数，输入为6个关节角度值θ_real，输出为第i关节的示教器值即θ_nominal，通过采集到的100组输入输出数据训练模型，得到各个关节轴对应模型，通过运动学逆解求出目标点位对应关节角度，输入目标角度θ_targe到模型，获得补偿后的角度θ，将θ角输入到示教器中，运行到对应的点位。

如图4a-4c所示，一种六自由度串联机械臂绝对定位精度提高的方法流程，主要包含坐标系转换、运动学参数标定和高斯过程回归训练得到补偿角度三个部分，该方法流程思路清晰、明朗，方便实施。

其中，高斯过程回归是基于贝叶斯理论和统计学习理论发展起来的一种全新机器学习方法,适于处理高维数、小样本和非线性等复杂回归问题。它有着严格的统计学习理论基础,对处理高维数、小样本、非线性等复杂的问题具有很好的适应性,且泛化能力强.与神经网络、支持向量机相比，高斯过程回归具有容易实现、超参数自适应获取、非参数推断灵活以及输出具有概率意义等优点。本发明利用高斯过程回归对数据量要求不敏感，处理维度高的特点，用来解决非几何误差补偿部分。

Claims (4)

1.一种基于六自由度串联机械臂提高绝对定位精度的方法，其特征在于，包括如下步骤：

S1.首先构建机械臂与激光跟踪仪坐标转换矩阵：通过激光跟踪仪获取末端靶点信息和预处理进行机械臂与激光跟踪仪之间的坐标转换；

S2.设计机械臂运动学参数辨识方法：运用李群李代数建立机械臂的指数积模型与序列二次规划算法求全局最小值的方法相融合，对机械臂关节参数偏差所产生的末端几何误差进行补偿；

S3.设计高斯过程回归模型并训练得到补偿角度：通过激光跟踪仪获取到的实际点位和指数积模型求解运动学逆解，对非几何运动误差进行补偿，获取到示教器的角度值和运动学逆解求出的真实角度值，用高斯过程回归算法进行模型训练，使得模型通过输入目标角度，得到补偿后的角度值，最后将预测出的补偿后的角度值输入到示教器。

2.如权利要求1所述的一种基于六自由度串联机械臂提高绝对定位精度的方法，其特征在于，所述步骤S1具体包括：

机械臂末端标定及将激光跟踪仪坐标系下测得坐标，转换为机械臂坐标系下坐标，通过转动机械臂的1轴和5轴提前计算出1轴和5轴的实际杆长，设计坐标系转换表达式为：

p₀+p₁·x+p₂·y+z＝0 (1)

公式(1)中，(x_i,y_i,z_i)为通过转动1轴获取到的机械臂末端靶点信息，(x_i,y_i,z_i)是以激光跟踪仪坐标系作为基坐标系，(x_ball,y_ball,z_ball)代表拟合出采样点所在球面的球心，(p₁,p₂,1)为通过多项式拟合采样点所在平面所得到的垂直于平面的法向量，归一化得到方向向量n₃；

通过最小化距离平方获取拟合球的球心(x_ball,y_ball,z_ball)，与拟合出的平面法向量(p₁,p₂,1)相结合，设计求拟合平面圆心公式：

p₁·(x-x_ball)+p₂·(y-y_ball)-(z-z_ball)＝0

p₀+p₁·x+p₂·y-z＝0 (2)

通过求解等式(2)获得1轴转动拟合圆的圆心(x_c,y_c,z_c)，此时(x_c,y_c)也是机械臂基座标原点与激光跟踪仪基座标之间的距离；

利用同样方法转动2轴采集点位，获取到2轴拟合平面的法向量并归一化得方向向量o₂，此时获取到的拟合圆心z_c等于1轴杆长l₁，然后通过z轴方向向量a₃和y轴方向向量o₂叉乘获取x轴方向向量n₁；

根据获取到的方向向量n₁,o₂,a₃,以及转动1轴得到的(x_1c,y_1c,z_1c)得到激光跟踪仪基座标到机械臂基座标的转换矩阵：

对

求逆，获得

通过

获得机械臂坐标系下的采样点坐标，通过转动5轴，拟合出圆，计算出圆半径，减去靶点夹具长度，获得第5轴杆长l₅信息。

3.如权利要求2所述的一种基于六自由度串联机械臂提高绝对定位精度的方法，其特征在于，所述步骤S2具体包括：

基于指数积模型，构造机械臂的运动学模型：

公式(4)中，F(θ)为机械臂关节转角为θ时，机械臂末端的位姿，其中M为θ_1～6＝0时末端位姿，依据李群李代数公式获得

如下：

其中

通过公式(4)和公式(5)结合，可以求得名义机械臂末端位姿F(θ)，取矩阵中的点位部分，得到机械臂末端点位为

T_p＝f_fk(θ，x) (6)

T_p∈SE(3)_x为第2、3、4轴杆长信息，θ为从示教器中获得的机械臂名义关节转角。

根据公式(6)中计算出的名义末端点位和激光跟踪仪采样得到的实际点位结合，辨识出真实的杆长信息：

min∑||T_real-T_p||

＝min∑||T_real-f_fk(θ，l)|| (7)

采用序列二次规划算法进行求解变量l，使得∑||T_real-T_p||最小：

min f(x)

b(x)≥0

c(x)＝0 (8)

其中，通过b(x)≥0来约束杆长的变化范围。

4.如权利要求3所述的一种基于六自由度串联机械臂提高绝对定位精度的方法，其特征在于，所述步骤S3具体包括：

经过公式(7)辨识出的第2、3、4轴杆长信息，和步骤S1中获得的第1、5轴杆长，通过指数积模型求逆解，获得采样点位的真实关节角度θ_real，根据高斯过程回归公式，设置输入为通过逆解求出的1～6轴的真实角度θ_real,

输出为1～6轴的名义值θ_nominal,

θ_nominal～N(0,K)

其中θ_reali代表第i次解算得到的6轴角度，K为某一种合适的协方差函数，同时设定k(θ_reali,θ_realj)＝K_ij，据高斯过程公式，新的输入项θ′_real，过去观察得到的数据为(θ_real,θ_nominal)，分布满足：

同时新的输入项θ′_real和相关的输出θ′_nominal也满足公式(10)的高斯分布，因此给定新的输入项θ′_real，相应的输出项θ′_nominal可以通过公式(11)预测出：

根据公式(11)，得出输入到示教器中的名义值，使得机械臂末端逼近目标点位。

Images