CN100338433C - 激光扫描器与机器人的相对位置的标定方法 - Google Patents

Abstract

本发明涉及激光扫描器与机器人的相对位置的标定方法，包括：1、机器人的机械臂运动使球体在扫描器的扫描范围；2、计算球体的球心相对于扫描器的坐标，记录此时机械臂末端相对于机器人基座的坐标；3、控制机械臂末端平动；4、重复步骤2；5、改变机器人机械臂的姿态，重复步骤2-4；6、重复步骤5；7、计算扫描器坐标与机器人基座坐标的转动关系；8、控制机械臂使球体平动，扫描器对球体进行扫描，记录下球体表面被扫描点相对于扫描器的坐标和对应时刻机械臂末端相对于机器人基座的坐标；9、改变机器人机械臂的姿态，重复步骤8；10、计算扫描器坐标与机器人基座坐标的平移关系。

Description

激光扫描器与机器人的相对位置的标定方法

技术领域

本发明涉及激光扫描器与机器人的相对位置的标定方法。

背景技术

激光扫描器可以用于三维空间点的重建，但如果扫描器和被扫描物体的相互位置是固定的，其所能重构的范围就受到了很大限制。以线激光为例，其能重构的只是扫描线所照射到的点。为了扩展扫描器的扫描范围，通常使用的方法有两种。一是将扫描器放在一个机械装置上，通过机械装置的运动，使得扫描器的扫描线可以达到更多的空间点。另一种方法是，将被扫描物体放在机械装置上，通过机械装置的运动，使得被扫描物体上的各空间点都能被固定的扫描器扫描到。

因此，无论采用何种方式，扫描器的TCP的标定即确定扫描器坐标系与机械装置坐标系间的关系是首先要解决的问题。但是目前的标定方法均是将扫描器与机械装置的旋转关系和平移关系耦合起来，其标定的鲁棒性和精确性低。

本发明内容

本发明的目的是利用一个半径已知的球体作为工具，实现了激光扫描器与机器人坐标系之间相对位置关系的标定，实现了旋转关系和平移关系的解耦，并且过程简单，精度高而且稳定性好。

为实现上述目的，本发明提供了一种激光扫描器与机器人的相对位置的标定方法，包括以下步骤：

步骤1、扫描器与机器人基座相对静止，机器人的机械臂末端夹持一个球体，机械臂运动使球体在扫描器的扫描范围内；

步骤2、扫描器采集所述球体上的扫描线，计算所述球体的球心相对于扫描器的坐标，记录此时机械臂末端相对于机器人基座的坐标；

步骤3、控制机械臂末端平动，使球体仍在扫描器的扫描范围内；

步骤4、重复步骤2；

步骤5、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤2-4；

步骤6、重复步骤5；

步骤7、计算扫描器坐标与机器人基座坐标的转动关系，即计算扫描器坐标系与机器人基座坐标系的旋转矩阵；

步骤8、控制机械臂使球体平动，在球体平动的过程中扫描器对球体进行扫描，记录下球体表面被扫描点相对于扫描器的坐标和对应时刻机械臂末端相对于机器人基座的坐标；

步骤9、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤8；

步骤10计算扫描器坐标与机器人基座坐标的平移关系，即计算扫描器坐标系与机器人基座坐标系的平移矩阵。

所述步骤9可以执行多次。所述步骤6还可以为重复步骤5一次以上所述方法中的扫描器为线激光扫描器。

本发明还提供了一种激光扫描器与机器人的相对位置的标定方法，包括以下步骤：

步骤1、球体与机器人基座相对静止，机器人的机械臂末端夹持一个扫描器，机械臂运动使球体在扫描器的扫描范围内；

步骤2、扫描器采集所述球体上的扫描线，计算所述球体的球心相对于扫描器的坐标，记录此时机械臂末端相对于机器人基座的坐标；

步骤3、控制机械臂末端平动，使球体仍在扫描器的扫描范围内；

步骤4、重复步骤2；

步骤5、改变机器人的姿态，使机械臂末端平动和转动，重复步骤2-4；

步骤6、重复步骤5；

步骤7、计算扫描器坐标与机器人机械臂末端坐标的转动关系，即计算扫描器坐标系与机器人机械臂末端坐标系的旋转矩阵；

步骤8、控制机械臂使扫描器平动，在扫描器平动的过程中扫描器对球体进行扫描，记录下球体表面被扫描点相对于扫描器的坐标和对应时刻机械臂末端相对于机器人基座的坐标；

步骤9、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤8；

步骤10、计算扫描器坐标与机器人机械臂末端坐标的平移关系，即计算扫描器坐标系与机器人机械臂末端坐标系的平移矩阵。

所述步骤9可以执行多次。所述步骤6还可以为重复步骤5一次以上。所述方法中的扫描器为线激光扫描器。

因此，本发明具有以下优点：

1、利用球体做为工具，实现标定，其过程简单易行；

2、利用针对局部扫描做局部标定的方法，可以提高扫描恢复精度；

3、能实现平移关系与旋转关系的解耦，提高标定的稳定性和精度。

下面结合附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明相对位置的标定方法实施例1的位置示意图。

图2为本发明相对位置的标定方法实施例1的方法流程图。

图3为本发明相对位置的标定方法实施例2的位置示意图。

图4为本发明相对位置的标定方法实施例2的方法流程图。

具体实施方式

本发明的发明思想是利用线激光扫描器和一个球体而实现当机器人夹持扫描器时，标定扫描器坐标系和机器人末端坐标系之间的关系；当机器人夹持球体时，标定扫描器坐标系和机器人基座坐标系之间的关系。因此本方法标定机器人与线激光扫描器的相互位置关系，线激光扫描器不仅是标定工具，还是被标定对象。

实施例1

如图1所示，为本发明实施例1的示意图。机器人1具有一个可以夹持物体的机械臂11，机械臂11的顶端12夹持一个球体3，机器人1和线激光扫描器2均固定在世界坐标系中，且球体3在扫描器2的扫描范围之内。本实施例是标定扫描器2与机器人底座10之间的位置关系，即确定扫描器2的坐标系与底座10的坐标系之间的旋转矩阵R_s和平移矩阵T_s。机械臂顶端12的坐标系设为Tool₀坐标系。

如图2所示，为本发明实施例1的方法流程图。

步骤101、扫描器与机器人基座相对静止，机器人的机械臂末端夹持一个球体，机械臂运动使球体在扫描器的扫描范围内；

步骤102、扫描器采集所述球体上的扫描线，计算所述球体的球心相对于扫描器的坐标，记录此时机械臂末端相对于机器人基座的坐标；

步骤103、控制机械臂末端平动，使球体仍在扫描器的扫描范围内；

步骤104、重复步骤102；

步骤105、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤102-104；

步骤106、重复步骤105；可以重复一次或多次；

步骤107、计算扫描器坐标与机器人基座坐标的转动关系，即计算扫描器坐标系与机器人基座坐标系的旋转矩阵；

步骤108、控制机械臂使球体平动，在球体平动的过程中扫描器对球体进行扫描，记录下球体表面被扫描点相对于扫描器的坐标和对应时刻机械臂末端相对于机器人基座的坐标；

步骤109、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤8；为了提高测量精度，本步骤可以执行多次。

步骤110、计算扫描器坐标与机器人基座坐标的平移关系，即计算扫描器坐标系与机器人基座坐标系的平移矩阵。

1、计算线激光扫描器坐标与机器人基座坐标的转动关系，即计算扫描器坐标与机器人基座坐标的旋转矩阵R_s；

对于一个与Tool₀坐标系位置固定的点(如球体的球心)，它在Tool₀下的坐标(x_t，y_t，z_t)与其相对于扫描器坐标系的坐标(x_l，y_l，z_l)间的关系满足：

$\left(\begin{array}{c}{X}_t\\ 1\end{array}\right)={\left(\begin{array}{cc}{R}_0& T_0\\ 0& 1\end{array}\right)}^{-1}\·\left(\begin{array}{cc}{R}_s& T_s\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_l\\ 1\end{array}\right)---\left(1\right)$

其中，X_t为该固定点在Tool₀坐标系下的坐标

X_l为该固定点在扫描器中恢复出的坐标(即其相对于扫描器的坐标

)，R₀为Tool₀相对于机器人基座的旋转矩阵，T₀为Tool₀相对于机器人基座的平移矩阵，R_s与T_s是所要标定的扫描器相对于基座坐标系的旋转和平移矩阵。

将(1)式展开后，可以得到：

             R₀·X_t+T₀＝R_s·X_l+T_s(2)

控制机器人，使得扫描器两次恢复同一固定点，可以得到：

             R₀₁·X_t+T₀₁＝R_s·X_l1+T_s(3)

             R₀₂·X_t+T₀₂＝R_s·X_l2+T_s(4)

如果使控制过程中机器人的姿态保持不变，即R₀₁＝R₀₂，将(4)-(3)可以得到：       T₀₂-T₀₁＝R_s·(X_l2-X_l1)(5)

对于线激光扫描器，找到某一空间点在扫描器下的坐标是困难的，可以通过让扫描器恢复虚拟空间点(此处指球体球心)来解决这一问题。在扫描器扫到球体时，其激光线拟合出一个空间圆，测出球体半径，通过几何关系球得球心。但球心将有两个解，通过实验过程中去除伪解：实验者可以由扫描线在球体中的位置判断球心方向，再通过以约定次序选定扫描线中的上、中、下部三点，将此情况通知计算机，而计算机则根据这三点得到两个向量，再由这两个向量叉乘的结果得到球心所在的大致方向，从而去除伪解。

由步骤101机器人夹持一个球，运动至扫描器的可视范围内，由步骤102扫描器采集打在球体上的扫描线，得到球心点在扫描器下的坐标，即由上面的计算方法得出X_l1，记下此时的Tool₀即从机器人控制器中读出T₀₁。之后由步骤103，控制机器人平动，仍然使球在扫描器的可视范围内，再由步骤104扫描器采集打在球体上的扫描线，恢复球心在扫描器下的坐标，得到X_l2，并记下此时的Tool₀即从机器人控制器中读出T₀₂。至此，可以得到一组求解方程(5)的数据。由步骤105和步骤106改变机器人姿态，一次或一次以上，如上所述再采集一组或一组以上的实验数据代入方程(5)，即可求出Rx，Ry，Rz。

2、计算扫描器坐标与机器人基座坐标的平移关系，即通过扫描球面的方法来求解扫描器坐标系与机器人基座坐标系的平移矩阵，标定参数X，Y，Z：

由(2)式可得，对于与Tool₀位置固定的空间点有：

     X_t＝R₀ ^-1·(R_s·X_l+T_s-T₀)＝R₀ ^-1·(R_s·X_l-T₀)+R₀ ^-1T_s(6)

在上述扫描器和机器人结合的三维重构系统中，扫描器的恢复结果(空间点相对于Tool₀坐标系的位置)X_t与其在扫描器坐标系下的坐标X_l的关系如(6)式所示。对于不同空间点X_t1与X_t2，可以得到其相对位置关系：

     X_t2-X_t1＝R₀₂ ^-1·(R_s·X_l2+T_s-T₀₂)-R₀₁ ^-1·(R_s·X_l1+T_s-T₀₁)(7)

当机器人在扫描过程中只进行平移即R₀₁＝R₀₂时，上式可化简为：

     X_t2-X_t1＝R₀ ^-1·(R_s·(X_l2-X_l1)-T₀₂+T₀₁)(8)

从(8)式可以得到，当机器人以平动(姿态不变)来扫描一个球面时，无论T_s取何值，其恢复结果都是一个球面。恢复出的结果间的相对位置关系(即恢复出的物体形状)只与旋转矩阵R_s有关。因此在上述扫描器和机器人结合的三维重构系统中，当机器人在扫描过程中只进行平移时，扫描器恢复出的模型形状只与扫描器标定中的旋转矩阵R_s有关，而与平移关系T_s无关。

取T_s＝0，将其扫描恢复结果(即X_t)做球形拟合，得到的球心位置X_B与球心的真实位置X_b间将满足(6)式，即：

                X_b＝X_B+R₀ ^-1T_s(9)

通过步骤108，控制机器人进行平动扫描。在扫描过程中，将记录下被扫描点在扫描器下的坐标以及对应时刻的Tool₀。根据(6)式，取T_s＝0，可以进行所扫描的球面的三维恢复，利用恢复出的结果进行球面拟合，可以得到(7)式中的X_B，同时记下此刻的Tool₀。由步骤109改变机器人姿势一次或一次以上，按上述方法，再得到一组或一组以上的X_B和Tool₀值，利用方程(7)即可求解出T_s，得出X，Y，Z。

本方法利用球体做为工具，实现标定，其过程简单易行；在标定过程中，能使其旋转关系(Rx，Ry，Rz)与平移关系(X，Y，Z)解耦，提高结果的稳定性和精度。

实施例2

如图3所示，为本发明实施例2的示意图。机器人1具有一个可以夹持物体的机械臂11，机械臂11的顶端12夹持一个扫描器2，机器人1和球体3均固定在世界坐标系中，且球体3在线激光扫描器2的扫描范围之内。本实施例是标定扫描器2与机械臂顶端12之间的位置关系，即确定扫描器2的坐标系与机械臂顶端12的坐标系之间的旋转矩阵R_s和平移矩阵T_s。机械臂顶端12的坐标系设为Tool₀坐标系。

如图4所示，为本发明实施例2的方法流程图。

步骤201、球体与机器人基座相对静止，机器人的机械臂末端夹持一个扫描器，机械臂运动使球体在扫描器的扫描范围内；

步骤202、扫描器采集球体上的扫描线，计算球体的球心相对于扫描器的坐标，记录此时机械臂末端相对于机器人基座的坐标；

步骤203、控制机械臂末端平动，使球体仍在扫描器的扫描范围内；

步骤204、重复步骤202；

步骤205、改变机器人的姿态，使机械臂末端平动和转动，重复步骤202-204；

步骤206、重复步骤205；可以重复一次也可以重复一次以上。

步骤207、计算扫描器坐标与机器人机械臂末端坐标的转动关系，即计算扫描器坐标系与机器人机械臂末端坐标系的旋转矩阵；

步骤208、控制机械臂使扫描器平动，在扫描器平动的过程中扫描器对球体进行扫描，记录下球体表面被扫描点相对于扫描器的坐标和对应时刻机械臂末端相对于机器人基座的坐标；利用局部扫描的方法，可以提高扫描恢复精度；

步骤209、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤208；为了提高精度，本步骤可以执行多次。

步骤210、计算扫描器坐标与机器人机械臂末端坐标的平移关系，即计算扫描器坐标系与机器人机械臂末端坐标系的平移矩阵。

1、计算扫描器坐标系与机器人机械臂顶端坐标系的转动关系，即计算扫描器坐标系与机器人机械臂顶端坐标系的旋转矩阵R_s；

对于一个与机器人基座坐标系位置固定的点(如球体球心)，它在基座坐标系下的坐标(x_w，y_w，z_w)与其相对于扫描器坐标系的坐标(x_l，y_l，z_l)间的关系满足：

$\left(\begin{array}{c}{X}_w\\ 1\end{array}\right)=\left(\begin{array}{cc}{R}_0& T_0\\ 0& 1\end{array}\right)\·\left(\begin{array}{cc}{R}_t& T_t\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_l\\ 1\end{array}\right)---\left(10\right)$

其中，X_w为该固定点在基座坐标系下的坐标 X_l为该固定点在扫描器中恢复出的坐标(即其相对于扫描器的坐标

)，R₀为Tool₀相对于基座坐标系的旋转矩阵，T₀为Tool₀相对于基座坐标系的平移矩阵，R_t与T_t是所要标定的扫描器相对于Tool₀的旋转和平移矩阵。

将(10)式展开后，可以得到：

          X_w＝R₀·R_t·X_l+R₀·T_t+T₀(11)

控制机器人，使扫描器两次恢复同一固定点，可以得到：

          X_w1＝R₀₁·R_t·X_l1+R₀₁·T_t+T₀₁(12)

          X_w2＝R₀₂·R_t·X_l2+R₀₂·T_t+T₀₂(13)

如果使控制过程中机器人的姿态保持不变，即X_w1＝X_w2，R₀₁＝R₀₂，将(13)-(12)可以得到：

          R₀·R_r·(X_l1-X_l2)＝T₀₂-T₀₁(14)

对于线激光扫描器，找到某一空间点在扫描器下的坐标是困难的，可以通过让扫描器恢复虚拟空间点(此处指球体球心)来解决这一问题。在扫描器扫到球体时，其激光线拟合出一个空间圆，测出球体半径，通过几何关系球得球心。但球心将有两个解，通过实验过程中去除伪解：实验者可以由扫描线在球体中的位置判断球心方向，再通过以约定次序选定扫描线中的上、中、下部三点，将此情况通知计算机，而计算机则根据这三点得到两个向量，再由这两个向量叉乘的结果得到球心所在的大致方向，从而去除伪解。

通过步骤201机器人夹持扫描器，运动至固定在世界坐标中的一个球体的周围，使得该球体在扫描器的可视范围内，由步骤202，扫描器采集打在球体上的扫描线，得到球心点在扫描器下的坐标，记下此时的Tool₀即从机器人控制器中读出T₀₁和R₀。之后由步骤203，控制机器人平动，仍然使球在扫描器的可视范围内，再由步骤204扫描器采集打在球体上的扫描线，恢复球心在扫描器下的坐标，并记下此时的Tool₀即从机器人控制器中读出T₀₂和R₀。至此，可以得到一组求解方程(14)的实验数据。由步骤205和步骤206改变机器人姿态一次或一次以上，如上所述再采集一组或一组以上的实验数据，代入方程(14)，即可求出Rx，Ry，Rz。

2、计算扫描器坐标与机械臂顶端坐标的平移关系，即通过扫描球面的方法来求解扫描器坐标系与机械臂顶端坐标系的平移矩阵，标定参数X，Y，Z：

由(11)式可得，对于与基坐标系位置固定的空间点有：

                X_w＝R₀·R_t·X_l+R₀·T_t+T₀(15)

在上述扫描器和机器人结合的三维重构系统中，扫描器的恢复结果(空间点相对于基座坐标系的位置)X_w与其在扫描器坐标系下的坐标X_l的关系如(15)式所示。对于不同空间点X_w1与X_w2，可以得到其相对位置关系：

X_w2-X_w1＝R₀₂·R_t·X_l2+R₀₂·T_t+T₀₂-(R₀₁·R_t·X_l1+R₀₁·T_t+T₀₁)(16)

当机器人在扫描过程中只进行平移即R₀₁＝R₀₂时，上式可化简为：

                X_w2-X_w1＝R₀·R_t·(X_l2-X_l1)+T₀₂-T₀₁(17)

从(17)式可以得到，当机器人以平动(姿态不变)来扫描一个球面时，无论T_t取何值，其恢复结果都是一个球面。恢复出的结果间的相对位置关系(即恢复出的物体形状)只与旋转矩阵R_t有关。在上述扫描器和机器人结合的三维重构系统中，当机器人在扫描过程中只进行平移时，扫描器恢复出的模型形状只与扫描器标定中的旋转矩阵R_t有关，而与平移关系T_t无关。取T_t＝0，将其扫描恢复结果(即X_w)做球形拟合，得到的球心位置X_B与球心的真实位置X_b间将满足(15)式，即：

                   X_b＝X_B+R₀·T_t(18)

由步骤208控制机器人进行平动扫描，在扫描过程中，记录下被扫描点在扫描器下的坐标以及对应时刻的Tool₀。根据(15)式，取T_t＝0，可以进行所扫描的球面的三维恢复，利用恢复出的结果进行球面拟合，可以得到(18)式中的X_B，同时记下此刻的Tool₀。再由步骤209改变机器人姿态一次或一次以上，按上述方法，再得到一组或一组以上的X_B和Tool₀值，利用方程(18)即可求解出T_s，即得出X，Y，Z。

本方法利用球体做为工具，实现标定，其过程简单易行；在标定过程中，实现了旋转关系(Rx，Ry，Rz)与平移关系(X，Y，Z)的解耦，提高结果的稳定性和精度。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (8)

1、一种激光扫描器与机器人的相对位置的标定方法，其特征在于包括以下步骤：

步骤1、扫描器与机器人基座相对静止，机器人的机械臂末端夹持一个球体，机械臂运动使球体在扫描器的扫描范围内；

步骤2、扫描器采集所述球体上的扫描线，计算所述球体的球心相对于扫描器的坐标，记录此时机械臂末端相对于机器人基座的坐标；

步骤3、控制机械臂末端平动，使球体仍在扫描器的扫描范围内；

步骤4、重复步骤2；

步骤5、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤2-4；

步骤6、重复步骤5；

步骤7、计算扫描器坐标与机器人基座坐标的转动关系，即计算扫描器坐标系与机器人基座坐标系的旋转矩阵；

步骤8、控制机械臂使球体平动，在球体平动的过程中扫描器对球体进行扫描，记录下球体表面被扫描点相对于扫描器的坐标和对应时刻机械臂末端相对于机器人基座的坐标；

步骤9、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤8；

步骤10、计算扫描器坐标与机器人基座坐标的平移关系，即计算扫描器坐标系与机器人基座坐标系的平移矩阵。

2、根据权利要求1所述的激光扫描器与机器人的相对位置的标定方法，其特征在于：所述步骤9执行多次。

3、根据权利要求1所述的激光扫描器与机器人的相对位置的标定方法，其特征在于：所述步骤6为重复步骤5一次以上。

4、根据权利要求1所述的激光扫描器与机器人的相对位置的标定方法，其特征在于：所述方法中的扫描器为线激光扫描器。

5、一种激光扫描器与机器人的相对位置的标定方法，其特征在于包括以下步骤：

步骤1、球体与机器人基座相对静止，机器人的机械臂末端夹持一个扫描器，机械臂运动使球体在扫描器的扫描范围内；

步骤2、扫描器采集所述球体上的扫描线，计算所述球体的球心相对于扫描器的坐标，记录此时机械臂末端相对于机器人基座的坐标；

步骤3、控制机械臂末端平动，使球体仍在扫描器的扫描范围内；

步骤4、重复步骤2；

步骤5、改变机器人的姿态，使机械臂末端平动和转动，重复步骤2-4；

步骤6、重复步骤5；

步骤7、计算扫描器坐标与机器人机械臂末端坐标的转动关系，即计算扫描器坐标系与机器人机械臂末端坐标系的旋转矩阵；

步骤8、控制机械臂使扫描器平动，在扫描器平动的过程中扫描器对球体进行扫描，记录下球体表面被扫描点相对于扫描器的坐标和对应时刻机械臂末端相对于机器人基座的坐标；

步骤9、改变机器人机械臂的姿态，使机械臂末端平动和转动，重复步骤8；

步骤10、计算扫描器坐标与机器人机械臂末端坐标的平移关系，即计算扫描器坐标系与机器人机械臂末端坐标系的平移矩阵。

6、根据权利要求5所述的激光扫描器与机器人的相对位置的标定方法，其特征在于：所述步骤9执行多次。

7、根据权利要求5所述的激光扫描器与机器人的相对位置的标定方法，其特征在于：所述步骤6为重复步骤5一次以上。

8、根据权利要求5所述的激光扫描器与机器人的相对位置的标定方法，其特征在于：所述方法中的扫描器为线激光扫描器。

Images