【机械臂】工件坐标系标定算法

三点法，含原点

1. 已知用户坐标系的三个位置点：坐标系原点P0，X轴上一点Px，XY平面上一点Pxy
   

2. 原点指向X轴正方向的向量OX = Px - P0，原点指向XY平面上点的向量OP = Pxy - P0
   

3. 检查两向量的模长，模长太短则不能实现标定

4. 单位向量nOX，nOP
   

5. nOX与nOP叉乘求得Z轴方向向量OZ
   

6. 单位向量nOZ
   

7. nOZ与nOX叉乘求得Y轴方向向量OY
   

8. 单位向量nOY
   

9. 以P0作为新坐标系原点，向量n,o,a相对nOX,nOY,nOZ作为新坐标系的三个轴的单位向量
   

10. 根据原点和三个坐标轴单位向量的值，得到新坐标系的齐次矩阵
    

三点法，不含原点

1. 该方法需求出新坐标系原点，然后套用含原点的三点法计算新坐标系

2. 已知用户坐标系的三个位置点：X轴方向的两点Px1和Px2，以及Y轴方向的一点Py
   

3. 点Px1指向点Px2的向量VX1X2 = Px2 - Px1，点Px1指向点Py的向量VX1Y = Py - Px1
   

4. 向量VX1X2 的单位向量n，为用户坐标系的x轴方向单位向量

5. 根据向量点乘公式
   

6. 向量VX1X2点乘向量n，由于n为单位向量，其点积scale为Px1到Py长度的余弦
   

7. 几何意义为Px1到在原点P0向量n方向的距离，P0 = Px1 + scale * n
   

8. 将P0、Px2和Py代入到含原点三点法中求得用户坐标系

一点法，保持姿态

1. 已知空间一点P0作为用户坐标系原点，用户坐标系的姿态保持与原来坐标系相同
   

2. 得到用户坐标系的转换矩阵