[发明专利]工业机器人末端位姿快速测量装置及其测量方法

权利要求书

1.一种基于工业机器人末端位姿快速测量装置的测量方法，所述装置包括依次连接的平面旋转装置、同心三轴调整装置、磁性基座和靶球；

所述平面旋转装置包括依次连接的法兰连接件、伺服电机和横向摆臂；所述法兰连接件用于固定连接工业机器人的法兰盘；所述横向摆臂与伺服电机的输出轴固定连接；

所述同心三轴调整装置包括X轴伺服电机、Y轴伺服电机、Z轴伺服电机和连接模块；所述连接模块用于连接X轴伺服电机、Y轴伺服电机和Z轴伺服电机；所述磁性基座位于连接模块内；所述X轴伺服电机、Y轴伺服电机和Z轴伺服电机的轴线交点与靶球的球心相重合，其特征是，所述方法包括如下步骤：

1)对靶球空间位置数据进行测量，具体步骤如下：

101)将末端位姿快速测量装置安装在工业机器人的法兰盘上，激光跟踪仪放置在待标定的工业机器人附近，使得靶球能够接收激光跟踪仪发射的激光光线；

102)控制平面旋转装置转动，使靶球在三维空间中做圆周运动，激光跟踪仪依次记录靶球的运动位置P₁,P₂,…,P_m，m≥3，其中在针对不同位姿时所测量的数据点相同；

2)根据步骤1)得到的数据计算工业机器人末端位姿，具体步骤如下：

201)根据测量点拟合平面及空间轨迹圆，求出空间轨迹圆的圆心，包括：

2011)拟合靶球的运动轨迹所在平面的方程z＝Ex+Fy+G，其中，x、y、z为旋转平面上的点坐标，E、F、G表示旋转平面的参数；

2012)根据最小二乘法得到运动轨迹所在平面的方程参数如下式所示：其中，(x_k,y_k,z_k)为靶球运动轨迹上第k个测量点的坐标，m为轨迹测量点的个数；

拟合平面对应的平面法向量为n_i＝[E,F,-1]，由此得到与末端位姿快速测量装置的坐标系{E}的Z轴同向的单位向量为Z_E＝n_i/|n_i|；

2013)将上述测量点投影至该拟合平面上，平面的法向量为n_i＝[E,F,-1]，测量点(x_i,y_i,z_i)在该拟合平面上的投影应点为(x_pi,y_pi,z_pi)，记为P_pi，其投影公式为其中，

2014)利用步骤2013)中的投影点建立拟合平面的坐标系{P}，以拟合平面与测量坐标系的Z轴交点作为坐标原点O₁，初始点P_p1与最后一个测量点P_pm的连线作为坐标系的X₁轴，而坐标系的Z₁轴与该平面的法向量方向相同，根据坐标系的正交法则计算坐标系的Y₁轴；

2015)计算坐标系{P}与测量坐标系{S}之间的转换矩阵其中，P_pm为运动轨迹上第m个测量点在拟合平面上的投影坐标；

根据得到测量点位置坐标数据拟合圆的轨迹模型，其中^PP_i表示P_i点在坐标系{P}内的坐标，^SP_i表示P_i点在测量坐标系{S}内的坐标；

拟合圆的标准形式为(x_p-x_c)²+(y_p-y_c)²＝r²，其中，(x_c,y_c)和r分别表示圆弧的圆心和半径；

2016)将拟合圆的标准形式转换为w＝x_p²+y_p²+Ax_p+By_p+C，利用最小二乘法，求得式中参数如下所示：其中，(x_pk,y_pk)为靶球运动

轨迹上第k个测量点在拟合平面上的投影坐标；

根据上述公式计算得到轨迹的圆心坐标；

202)建立位姿快速测量装置的坐标系{E}，并根据工业机器人法兰坐标系{N}和末端位姿快速测量装置的坐标系{E}之间的转换矩阵，计算得到工业机器人法兰坐标系{N}的空间位姿。