[发明专利]一种机器人及其工艺设备相对位姿的测定方法及测定装置

权利要求书

1.一种机器人及其工艺设备相对位姿的测定方法，其特征在于，包括以下步骤：

将测量球(3)固定在机器人工具(2)上，机器人(1)运行N个位置，机器人(1)记录下腕部坐标系的在机器人基坐标系下的位置M_i和姿态R_i，其中同时记录下这N次测量中测量球(3)的球心在工艺设备坐标系下的位置P_i，其中

将所得到的N个腕部坐标系的在机器人基坐标系下的位置M_i和姿态R_i与N次测量中测量球(3)的球心的位置P_i带入相关公式进行求解，得到工艺设备坐标系相对于机器人基坐标系的相对位姿；

所述相关公式为，已知的输入为所采集的数据M_i，R_i，P_i，

未知的参数为工艺设备坐标系相对于机器人基坐标系的位置t和姿态s；

另外设测量球(3)的球心相对于腕部坐标系的位置为X，也为一个待求得未知参数；

存在以下方程：

令

有：

化简得到：

其中

故有：

其中

对于以上方程组，不能保证每一个方程的解都相同，其中两个或两个以上方程的解可能彼此远离，因此，全局的解决方案也不能得到保证；

令:

找到使λ达到最小化的X，即是方程组的解上述函数为非凸函数，只能得到局部的极小值，为了得到全局的极小值，通过选择初始点可以解决这一问题，因此可以按下面步骤：

步骤一，生成m个初始点集，每个集合是{X⁽⁰⁾}^m，m∈{1，2，...，M}，对于每一组初始点，用牛顿方法求得方程最小化问题的解X_m

步骤二，求出对应X_m的值

步骤三，根据的值，生成另外m个初始点集，再继续步骤二

重复上述过程，直到函数值不再发生变化或只有非常小的变化，然后以最小函数值λ_m的解X_m作为最终方案返回

得到解X_m后，继续进行下一步计算，代入X_m原始方程为：

即

B_i＝t+sP_i，其中

即：

D_i＝sG_i，其中展开为

对于S₁₁，S₁₂，S₁₃有：

可见，这是一个关于未知数S₁₁，S₁₂，S₁₃的三元一次方程组

令

则：

化为三元一次方程组，且有三个等式，用平方根法或者SOR方法求解对于s₂₁，s₂₂，s₂₃，对于s₃₁，s₃₂，s₃₃可用相同的方法得出

将s代入等式B_i＝t+sP_i中，即可得：

t＝-sP_i+B_i

到此，工艺设备坐标系相对于机器人基座标系的位置t和姿态s均已得到。

2.根据权利要求1所述的一种机器人及其工艺设备相对位姿的测定方法，其特征在于：所述测量球(3)的球心在工艺设备坐标系下的位置P_i由激光追踪仪(6)测量得出。

3.一种测定装置，其特征在于：利用如权利要求1或2任意一项所述的一种机器人及其工艺设备相对位姿的测定方法。