[发明专利]一种基于TCP坐标系下平移运动的旋转矩阵标定方法

权利要求书

1.一种基于TCP坐标系下平移运动的旋转矩阵标定方法，其特征在于，包括如下步骤：

步骤1、固定标准球于预设位置，扫描仪固定于机器人末端，调整机器人运动坐标系为TCP坐标系，调整机器人到达初始位置，保证扫描仪可以扫描到标准球；

步骤2、控制机器人携带扫描仪沿TCP坐标系的XYZ轴方向各做一次平移运动，在运动过程中对标准球进行扫描；

步骤3、建立扫运动过程中描仪坐标系下各位置处标准球表面点云采集方法，并对采集的点云用最小二乘法完成球心拟合，获取运动过程中不同位置处标准球的球心在描仪坐标系下的坐标；

步骤4、根据相对运动原理获得球心在TCP坐标系下的单位平移向量；并对扫描仪下平移运动得到的球心数据进行处理，计算球心在扫描仪坐标系下的单位平移向量；

步骤5、根据球心在TCP坐标系下和扫描仪坐标系下的平移向量标定旋转矩阵，旋转矩阵的具体标定为：

对于TCP坐标系下获得的单位向量，可构建矩阵U_t＝(α₁,α₂,α₃)，对于扫描仪坐标系下获得的单位向量，可构建矩阵V_s＝(β₁,β₂,β₃)；

设扫描仪坐标系转换到TCP坐标系的旋转矩阵为R_s，对于旋转矩阵R_s的求解：

R_s·V_s＝U_t 公式(3)

解公式(3)得旋转矩阵为

2.根据权利要求1所述的基于TCP坐标系下平移运动的旋转矩阵标定方法，其特征在于，步骤2中，机器人携带扫描仪在TCP坐标系下沿XYZ轴方向的平移运动的具体运动形式为：

以X轴方向平移为例，机器人携带扫描仪沿TCP坐标系X轴负方向由初始位置a点平移一段距离d_x到达b点，同理之后沿Y轴平移后到达c点，然后沿Z轴平移后到达d点；机器人带动扫描仪运动，使扫描仪在不同视野范围内扫描标准球。

3.根据权利要求1或2所述的基于TCP坐标系下平移运动的旋转矩阵标定方法，其特征在于，步骤3中，建立标准球表面点云采集方法具体步骤如下：

对标准球的扫描点云进行采样，对标准球中部靠近赤道的区域进行采样，具体采用x＝x_min+d和x＝x_min+3d两个截面对点云进行采样，令d＝0.25R_b，其中R_b为标准球半径，x_min为标准球表面点云数据中横坐标的最小值。

4.根据权利要求3所述的基于TCP坐标系下平移运动的旋转矩阵标定方法，其特征在于，步骤3中，利用最小二乘法完成球心拟合具体步骤如下：

对扫描点云提取标准球球面点云，根据获取球面点云位置信息，采用最小二乘法进行球拟合计算标准球心坐标(x₀,y₀,z₀)，其表达式如下：

其中，(x_i,y_i,z_i)为标准球面点云坐标，R为拟合标准球半径，公式(1)满足

求解公式(2)，即求得标准球心坐标(x₀,y₀,z₀)。

5.根据权利要求2所述的基于TCP坐标系下平移运动的旋转矩阵标定方法，其特征在于，步骤4中，根据相对运动原理获得球心在TCP坐标系下的单位平移向量具体方法如下：

由于标准球固定不动，仅机器人在TCP坐标系下沿X轴负方向平移，可视为TCP坐标系不动，标准球球心沿TCP坐标系下X轴正方向平移了一段距离d_x，根据相对运动原理可得球心在TCP坐标系下沿X轴的单位平移向量α₁＝(1,0,0)，同理可得Y轴的单位平移向量α₂＝(0,1,0)，Z轴的单位平移向量α₃＝(0,0,1)。