CN111307155A - 双协作机器人初始定位测量装置及初始定位方法 - Google Patents

Abstract

本发明公开了一种双协作机器人初始定位测量装置及初始定位方法，测量装置包括A测量小板、B测量小板、中央控制模块，A测量小板包括对射管的接收部，接收部外沿设有管状遮光套，B测量小板包括对射管的发射部和激光测距传感器，中央控制模块接收到对射管发出的完成对射的信号后，中央控制模块启动激光测距传感器进行测距。本发明将从机器人也纳入到这个共同的坐标系中实行集中控制，以保证在协同工作的过程中主从两个机器人位姿的精确对应，运动配合协调，这一方法成功应用于双机器人X射线影像系统中的双机器人初始定位和控制中。

Description

双协作机器人初始定位测量装置及初始定位方法

技术领域

本发明属于机器人控制技术领域，涉及一种双协作机器人初始定位测量装置、双协作机器人及定位方法，尤其涉及一种利用多点激光测距来实现双机器人初始相互定位的装置和方法。

背景技术

近年来，双机器人乃至多机器人协同控制技术成为人们研究的热点，并且在很多场合已经获得了成功的应用，例如在有的机械制造自动装配线的一些工位上就有双机器人或多机器人在协同工作，在医疗设备上也有双机器人协作进行检测的应用例子。

目前研究和应用的双协作机器一般都属于下面两种情况：一、双机器人基于同一个基座，双机器人本来就是在同一个坐标系下，典型的例子就是所谓的双臂机器人，两个机械臂的位姿可以在同一个坐标系下描述。二、在另外一些应用场合，各个机器人在协同工作的过程中，其位姿不要求有精确的对应关系，因而也就不需要利用同一个坐标系来集中控制，在这样一些应用场合，往往采用分散控制的方式，由各个机器人自行决定对其它机器人的响应和规避。

但是上述的两种现有情况并不能包括全部的应用要求。在很多场合，要协同工作的两个机器人不在同一个基座上，它们是两个完全独立的机器人，它们是可移动的，在协同工作开始前其相对位置不确定，但在协同工作的过程中它们各自的基座不改变位置，在协同工作中双机器人的位姿需要有精确的对应关系，双机器人的运动需要精准配合。在这种情况下，需要选定双机器人中一个做主机器人，另一个为从机器人。将主从机器人各自独立的坐标系整合为一个统一的坐标系，在这个统一的坐标系中实行集中控制，以保证在协同工作的过程中主从两个机器人位姿的精确对应关系，所以当每次协同工作开始前，必须先检测双机器人的基座在初始状态下的相对位置，才能建立上述的统一坐标系，因此需要设计一种实现双机器人在协同工作初始时相互定位的装置和方法。

发明内容

本发明针对机器人应用技术中对双机器人协同工作的需求呈现，提供一种双协作机器人初始定位测量装置及初始定位方法。

本发明是通过以下技术方案实现的：

双协作机器人初始定位测量装置，包括主机器人和从机器人，主机器人内设有主机器人控制器，所述的和从机器人内设有从机器人控制器，还包括A测量小板、B测量小板、对射管以及中央控制模块，所述的A测量小板设置于从机器人的末端位置或者末端临近的位置，所述的B测量小板设置于主机器人的末端位置或者末端临近的位置，所述的中央控制模块分别与A测量小板和B测量小板信号连接，所述主机器人通过主机器人控制器与中央控制模块信号连接，所述的从机器人通过从机器人控制器与中央控制模块信号连接，所述的对射管由对射管接收部和对射管发射部组成；其中，所述A测量小板上设有对射管接收部，接收部外沿设有管状遮光套；所述B测量小板上设有对射管发射部和激光测距传感器；所述中央控制模块接收到对射管发出的完成对射的信号后，所述中央控制模块启动所述激光测距传感器进行测距。

为优化上述技术方案，采取的具体措施还包括：

上述的激光测距传感器是相位式激光测距传感器。

上述的对射管为激光对射管或者红外对射管。

上述的A测量小板上还设置有位置测量基准点。

本发明还保护一种双协作机器人初始定位测量装置的初始定位方法，包括以下步骤：

S1：将A测量小板安装于从机器人的末端，建立从机器人末端工具坐标系：以位置测量基准点为从机器人末端工具坐标系的原点，以A测量小板的法线方向为工具坐标系的Z轴的方向，按照右手定则确定工具坐标系X轴方向和Y轴方向；

S2：使从机器人的末端运动，到达可以让A测量小板上的对射管的接收部无遮挡地接收来自对射管发射部的信号，以及来自激光测距传感器的激光的位姿，将这个位姿定义为从机器人末端工具坐标系的基准位姿A，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵为

在位姿矩阵中，

表示坐标系的方向，p₀表示从机器人末端工具坐标系原点在从机器人基坐标系中的矢量坐标；其中，

r_x00，r_y00，r_z00分别是末端工具坐标系的三个坐标轴在基坐标系中的坐标矢量；p₀＝(px₀ py₀ pz₀)，px₀，py₀，py₀是从机器人末端工具坐标系的原点在从机器人基坐标系中的坐标矢量；

S3：将B测量小板安装于主机器人的末端，建立主机器人末端工具坐标系：以激光测距传感器的测距起始点为主机器人末端工具坐标系的原点，以激光测距传感器射出的激光的方向为工具坐标系的Z轴的方向，并且使Z轴方向与B测量小板的法线方向一致，按照右手定则确定工具坐标系X轴方向和Y轴方向；

S4：调整主机器人末端的位姿，使分别位于主从两个机器人末端的A测量小板、B测量小板上的激光对射管完成对射，由主机器人控制器得到主机器人末端工具坐标系的原点在主机器人基坐标系中的矢量坐标，然后使从机器人的末端运动，保证从机器人末端工具坐标系的原点p₀位置不变，改变从机器人末端工具坐标系的3个坐标轴方向，重新调整主机器人末端的位姿使得A测量小板、B测量小板上的激光对射管完成对射，并得到多组主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标，结合激光测距传感器测得的对应位姿下多组主机器人末端工具坐标系原点和从机器人末端工具坐标系原点间的距离，构建方程组，求解从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₀；其中，v₀和p₀是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

S5：恢复从机器人末端的运动至步骤S2确定的基准位姿A，然后使从机器人末端沿着r_x00的方向运动h的距离，将从机器人的工具坐标系的原点调整到p₁，p₁＝(px₀+h py₀pz₀)，保证从机器人工具坐标系三个坐标轴的方向与基准位姿A的坐标轴方向相同，将此位姿定义为从机器人末端工具坐标系的基准位姿B，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵为

S6：保证步骤S5的从机器人末端工具坐标系的原点p₁位置不变，重复步骤S4，求解此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₁；其中，v₁和p₁是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

S7：恢复从机器人末端的运动至步骤S2确定的基准位姿A，然后使从机器人末端沿着r_y00的方向运动h的距离，将从机器人的工具坐标系的原点调整到p₂，p₂＝(px₀ py₀+hpz₀)，保证从机器人工具坐标系三个坐标轴的方向与基准位姿A的坐标轴方向相同，将此位姿定义为从机器人末端工具坐标系的基准位姿C，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵

S8：保证步骤S7的从机器人末端工具坐标系的原点p₂位置不变，重复步骤S4，求解此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₂；其中，v₂和p₂是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

S9：恢复从机器人末端的运动至步骤S2确定的基准位姿A，然后使从机器人末端沿着r_z00的方向运动h的距离，将从机器人的工具坐标系的原点调整到p₃，p₃＝(px₀ py₀ pz₀+h)，保证从机器人工具坐标系三个坐标轴的方向与基准位姿A的坐标轴方向相同，将此位姿定义为从机器人末端工具坐标系的基准位姿D，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵

S10：保证步骤S9的从机器人末端工具坐标系的原点p₃位置不变，重复步骤S4，求解此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₃；其中，v₃和p₃是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

步骤S11：根据求出的v₀、v₁、v₂、v₃，针对基准位姿计算出从机器人末端工具坐标系在主机器人基坐标系中的位姿矩阵是

其中，

R_x00＝V₁-V₀，R_y00＝V₂-V₀，R_z00＝V₃-V₀，根据步骤S2得到的从机器人基坐标系中的位姿矩阵为

分别将上述两个位姿矩阵扩展成为齐次矩阵

得到主、从两个机器人的坐标系间的变换矩阵T＝PS^-1，根据此变换矩阵，可以将各自独立的主从机器人的基坐标系整合为一个统一的坐标系，由中央控制模块在统一的坐标系中规划主、从机器人的运动轨迹，协调主、从机器人的姿态和动作。

进一步地，所述步骤S4中，v₀的具体求解过程包括以下步骤：

S41：根据从机器人的基准位姿A，调整主机器人末端的位姿，使得安装于主机器人末端的B测量小板的对射管发射部发出的激光或红外射线，穿过位于从机器人末端A测量小板上的管状遮光套，射向对射管的接收部，在这个位姿下，从机器人末端工具坐标系的Z轴和主机器人末端工具坐标系的Z轴重合，将主机器人末端的这个位姿定义为第一次测距位姿，保持主机器人的位姿不变，在主机器人控制器读出主机器人末端工具坐标系的原点在主机器人基坐标系中的矢量坐标是v_z00＝(vx_z00 vy_z00 vz_z00)，启动位于主机器人末端的B测量小板上的激光测距传感器，测出在此位姿下从机器人末端工具坐标系原点和主机器人末端工具坐标系原点间的距离是L₀₀，得到下式：

v₀是待求的，本步骤得到了求解v₀的第一个方程；

S42：使从机器人的末端运动，相对于步骤S2中确定的基准位姿A，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是

S43：针对步骤S42的从机器人末端的新的位姿，再一次调整主机器人的末端位姿做相应的改变，使得分别位于主从两个机器人末端的A测量小板、B测量小板上的激光或红外对射管再次完成对射，主从两个机器人的工具坐标系的Z轴方向再次重合，将主机器人末端的这个位姿定义为第二次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v_z01＝(vx_z01 vy_z01 vz_z01)，基于此原点，用B测量小板上的激光测距传感器测出主从两个机器人末端工具坐标原点间距离是L₀₁，得到下式：

v₀是待求的，本步骤得到了求解v₀的第二个方程；

S44：使从机器人的末端运动，相对于步骤S42中确定的基准位姿，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是

S45：针对步骤S44的从机器人末端的新的位姿，再一次调整主机器人的末端位姿做相应的改变，使得分别位于主从两个机器人末端的A测量小板、B测量小板上的激光或红外对射管再次完成对射，主从两个机器人的工具坐标系的Z轴方向再次重合，将主机器人末端的这个位姿定义为第二次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v_z02＝(vx_z02 vy_z02 vz_z02)，基于此原点，用B测量小板上的激光测距传感器测出主从两个机器人末端工具坐标原点间距离是L₀₂，得到下式：

v₀是待求的，本步骤得到了求解v₀的第三个方程；

S46：联立步骤S41、步骤S43、步骤S45得到的三个方程，得到方程组：

解方程组得到v₀＝(vx₀ vy₀ vz₀)。

本发明还保护一种基于双机器人的X射线影像系统，包括主台车和从台车，还包括权利要求1所述的双协作机器人初始定位测量装置。

进一步地，主机器人和从机器人均为6自由度关节机器人，A测量小板为平板X射线检测器，B测量小板包括X射线球管及束光器，主机器人夹持X射线球管及束光器，从机器人夹持平板X射线检测器，主机器人和从机器人上设有中央控制模块，主机器人载于主台车，从机器人载于从台车。

本发明的有益效果为：

本发明给出了双协作机器人初始定位测量装置以及主从双协作机器人初始相对位置自动定位的方法，可以自动测量两个完全独立的机器人在初始时刻的彼此间的相对位置，从而自动得出两个独立的机器人的基坐标系之间的变换矩阵，可以将两个机器人的坐标系整合为一个，同时将主从机器人都纳入到这个共同的坐标系中实行集中控制，保证了在协同工作的过程中主从两个机器人位姿的精确对应关系。

附图说明

图1是A种测量小板的结构视图。

图2是B种测量小板的结构视图。

图3是激光测距时主从机器人的位姿。

图4双机器人初始定位装置的结构框图。

图5是从机器人末端工具坐标系的四个基准位姿的示意。

图6是所述步骤S4中第一次测距时各坐标系之间的关系。

图7是基于双机器人的X射线影像系统的示意图。

图中序号，1-主机器人、2-从机器人、3-主机器人控制器、4-从机器人控制器、5-A测量小板、6-B测量小板、7-中央控制模块、51-对射管接收部、52-管状遮光套、53-位置测量基准点、61-对射管发射部、62-激光测距传感器、100-主台车、200-从台车。

具体实施方式

下面结合附图和实施例对本发明的发明内容作进一步地说明。

参见图1-4，双协作机器人初始定位测量装置，包括主机器人1和从机器人2，主机器人1内设有主机器人控制器3，所述的和从机器人2内设有从机器人控制器4，还包括A测量小板5、B测量小板6、对射管以及中央控制模块7，所述的A测量小板5设置于从机器人2的末端位置或者末端临近的位置，所述的B测量小板6设置于主机器人1的末端位置或者末端临近的位置，所述的中央控制模块7分别与A测量小板5和B测量小板6信号连接，所述主机器人1通过主机器人控制器3与中央控制模块7信号连接，所述的从机器人2通过从机器人控制器4与中央控制模块7信号连接，所述的对射管由对射管接收部51和对射管发射部61组成；其中，所述A测量小板5上设有对射管接收部51，接收部外沿设有管状遮光套52；所述B测量小板6上设有对射管发射部61和激光测距传感器62；所述中央控制模块7接收到对射管发出的完成对射的信号后，所述中央控制模块7启动所述激光测距传感器62进行测距。

本实施例中，所述激光测距传感器62是相位式激光测距传感器。

本实施例中，所述对射管为激光对射管或者红外对射管。

本实施例中，所述A测量小板5上还设置有位置测量基准点53。

一种双协作机器人初始定位测量装置的初始定位方法，包括以下步骤：

步骤一：将所述A测量小板安装于从机器人的末端，建立从机器人末端工具坐标系：以步骤一所述测位标志点为工具坐标系的原点，以所述A测量小板的法线方向为工具坐标系的Z轴的方向，按照右手定则确定工具坐标系X轴方向和Y轴方向；

步骤二：使从机器人的末端运动，到达可以让A测量小板上的对射管的接收部无遮挡地接收来自对射管发射部的激光或红外射线，以及来自激光测距传感器的激光的位姿，将这个位姿定义为从机器人末端工具坐标系的基准位姿A(见附图5)，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵为

在位姿矩阵中，

表示坐标系的方向，p0表示从机器人末端工具坐标系原点在从机器人基坐标系中的矢量坐标；其中，

r_x00，r_y00，r_z00分别是末端工具坐标系的三个坐标轴在基坐标系中的坐标矢量；p₀＝(px₀ py₀ pz₀)，px₀，py₀，py₀是从机器人末端工具坐标系的原点在从机器人基坐标系中的坐标矢量；

步骤三：将B测量小板安装于主机器人的末端，建立主机器人末端工具坐标系：以激光测距传感器的测距起始点为主机器人末端工具坐标系的原点，以激光测距传感器射出的激光的方向为工具坐标系的Z轴的方向，并且应当使Z轴方向与B测量小板的法线方向一致，按照右手定则确定工具坐标系X轴方向和Y轴方向；

步骤四：保持从机器人的位姿不变，调整主机器人末端的位姿，使得安装于主机器人末端的B测量小板的对射管发射部发出的激光或红外射线，穿过位于从机器人末端A测量小板上的管状遮光套，射向对射管的接收部，当位于从机器人末端的A测量小板上的对射管接收部接收到射线，会由从机器人发出信号通知主机器人在这个位姿下从机器人末端工具坐标系的Z轴和主机器人末端工具坐标系的Z轴是重合的，将主机器人末端的这个位姿定义为第一次测距位姿，保持主机器人的位姿不变，在主机器人控制器可以读出主机器人末端工具坐标系的原点(即B测量小板上激光测距传感器的起始点)在主机器人基坐标系中的矢量坐标是v_z00＝(vx_z00 vy_z00 vz_z00)，启动位于主机器人末端的B测量小板上的激光测距传感器，测出在此位姿下从机器人末端工具坐标系原点和主机器人末端工具坐标系原点间的距离是L₀₀，定义从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标是v₀＝(vx₀vy₀ vz₀)，参见附图6，有下式：

v₀是待求的，本步骤得到了求解v₀的第一个方程；

步骤五，使从机器人的末端运动，相对于步骤二中确定的基准位姿A，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是

步骤六，针对从机器人末端的新的位姿，按照步骤四所述的方式，再一次调整主机器人的末端位姿做相应的改变，使得分别位于主从两个机器人末端的A测量小板、B测量小板上的激光或红外对射管再次完成对射，主从两个机器人的工具坐标系的Z轴方向再次重合，将主机器人末端的这个位姿定义为第二次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v_z01＝(vx_z01vy_z01 vz_z01)，基于此原点，用B测量小板上的激光测距传感器测出主从两个机器人末端工具坐标系原点间距离是L₀₁，相对于步骤四，从机器人末端工具坐标系原点v₀没变，但主机器人末端工具坐标系的原点位置变了，本次测量有：

v₀是待求的，本步骤得到了求解v₀的第二个方程；

步骤七，再次使从机器人的末端运动，相对于步骤五确定的位姿，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，此时在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是

步骤八，针对从机器人末端的新的位姿，按照步骤六中所述的方式，再一次调整主机器人的末端位姿做相应的改变，使分别位于主从两个机器人末端的A测量小板、B测量小板上的激光对射管完成对射，从而使主从两个机器人末端工具坐标系的Z轴再次重合，将主机器人末端的这个位姿定义为第三次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v_z02＝(vx_z02 vy_z02vz_z02)，通过B测量小板上激光测距传感器，测出主从两个机器人末端工具坐标系原点的距离是L₀₂，相对于步骤四和步骤六，本次测距时从机器人末端工具坐标系原点v₀没变，但主机器人末端工具坐标系的原点位置变了，本次测距有

v₀是待求的，本步骤得到了求解v₀的第三个方程；

步骤九：联立步骤四、步骤六、步骤八得到的三个等式，得到方程组：

解方程组得到v₀＝(vx₀ vy₀ vz₀)，参见步骤二确定的从机器人末端工具坐标系基准位姿A，在基准位姿A，从机器人末端工具坐标系原点是p₀＝(px₀ py₀ pz₀)，v₀和p₀是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

步骤十：使从机器人末端运动，恢复到步骤二确定的基准位姿A，从机器人末端工具坐标系的位姿矩阵恢复为

其中

是从机器人的工具坐标系的三个坐标轴在从机器人基坐标系中的矢量坐标，p₀＝(px₀ py₀ pz₀)是工具坐标系的原点在从机器人基坐标系中的矢量坐标；

步骤十一，再使从机器人末端沿着

的方向运动h，末端工具坐标系的原点到达p₁＝(px₀+h py₀ pz₀)的位置(见附图5)，但工具坐标系三个坐标轴的方向并不改变，将此位姿定义为从机器人末端工具坐标系的基准位姿B，此时在从机器人控制器中可以读出，末端工具坐标系在基坐标系中的位姿矩阵为

设此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标为v₁＝(vx₁ vy₁ vz₁)，p₁和v₁是同一点分别基于从机器人和主机器人基坐标系的矢量坐标，p₁可以在从机器人控制器中得到，而v₁是待求的；

步骤十二，针对从机器人末端工具坐标系的基准位姿B的

重复前面步骤四到步骤九的过程，通过这一过程将v₁＝(vx₁ vy₁ vz₁)求出；

步骤十三：使从机器人末端恢复到步骤二确定的基准位姿A，从机器人末端工具坐标系的位姿矩阵恢复为

其中

是从机器人的工具坐标系的三个坐标轴在从机器人基坐标系中的矢量坐标，p₀＝(px₀ py₀ pz₀)是工具坐标系的原点在从机器人基坐标系中的矢量坐标；

步骤十四，使从机器人末端沿着

轴运动h，将从机器人末端工具坐标系新的原点调整到p₂＝(px₀ py₀+h pz₀)点(见附图5)，而三个坐标轴的方向依然与步骤二确定的基准位姿A的坐标轴方向相同，定义这个位姿为从机器人末端工具坐标系的基准位姿C，在从机器人的控制器中可以读出这时的从机器人末端工具坐标系位姿矩阵是

设此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标为v₂＝(vx₂ vy₂ vz₂)，p₂和v₂表示同一点，但分别是基于从机器人基坐标系和主机器人基坐标系，p₂可以在从机器人控制器中得到，而v₂是待求的；

步骤十五，针对从机器人末端工具坐标系的基准位姿C的

重复前面步骤四到步骤九的过程，通过这一过程将v₂＝(vx₂ vy₂ vz₂)求出；

步骤十六：使从机器人末端恢复到步骤二确定的基准位姿A，从机器人末端工具坐标系的位姿矩阵恢复为

其中，

是从机器人的工具坐标系的三个坐标轴在从机器人基坐标系中的矢量坐标，p₀＝(px₀ py₀ pz₀)是工具坐标系的原点在从机器人基坐标系中的矢量坐标；

步骤十七，使从机器人末端沿着

轴运动，将从机器人的工具坐标系新的原点调整到p₃＝(px₀ py₀ pz₀+h)点(见附图5)，而三个坐标轴的方向依然与步骤二确定的基准位姿A的坐标轴方向相同，定义这个位姿为从机器人末端工具坐标系的基准位姿D，这时在从机器人的控制器中可以读出末端工具坐标系在基坐标系中的位姿矩阵是

设此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标为v₃＝(vx₃ vy₃ vz₃)，p₃和v₃表示同一点，但分别是基于从机器人基坐标系和主机器人基坐标系，p₃可以在从机器人控制器中得到，而v₃是待求的；

步骤十八，针对从机器人新的工具坐标系的基准位姿C的

重复前面步骤四到步骤九的过程，通过这一过程将v₃＝(vx₃ vy₃ vz₃)求出；

步骤十九，根据求出的v₀、v₁、v₂、v₃，针对基准位姿计算出从机器人末端工具坐标系在主机器人基坐标系中的位姿矩阵是

其中，

R_x00＝V₁-V₀，R_y00＝V₂-V₀，R_z00＝V₃-V₀，前面步骤二时，已经针对同一位姿在从机器人控制器中读出其在从机器人基坐标系中位姿矩阵

分别将上述两个位姿矩阵扩展成为齐次矩阵

得到主、从两个机器人的坐标系间的变换矩阵T＝PS^-1，根据此变换矩阵，可以将各自独立的主从机器人的基坐标系整合为一个统一的坐标系，由中央控制模块在统一的坐标系中规划主、从机器人的运动轨迹，协调主、从机器人的姿态和动作。

如图7所示，是本发明提出的双协作机器人初始位置测量的方法应用于一种基于双机器人的X射线影像系统，该系统包括两个6自由度关节机器人，其中一个机器人夹持X射线球管及束光器，另一机器人夹持平板X射线检测器，夹持X射线管的机器人称为主机器人，夹持平板X射线检测器的机器人称为从机器人，主从机器人由各自的控制器直接控制，在主从机器人之上有中央控制模块，主机器人载于主台车，从机器人载于从台车。

在X射线检测开始前，将主从机器人移动到合适的位置，将A小板安装于从机器人的末端的平板X线检测器旁的合适的位置，将B小板安装于主机器人的末端的X射线球管旁的合适的位置，在中央控制模块的控制下，启动本发明所述一种双协作机器人初始相对位置测量方法的步骤三直至步骤二十一，测出主机器人基坐标系和从机器人基坐标系之间的变换矩阵T。

引导主机器人的运动以改变其位姿，以使得X射线球管及束光器处于合适的位置以利于X射线照射检查，在主机器人的末端夹持X射线球管及束光器，这样来确定主机器人末端的工具坐标系：以X射线球管及束光器中的焦点的中心为工具坐标系的原点，以X射线的方向为工具坐标系的z轴的方向，按右手定则确定工具坐标系的x轴和y轴。工具坐标系在主机器人基坐标系中的位姿位置矩阵是

其中

r_x，r_y，r_z分别是末端工具坐标系的三个坐标轴在基坐标系中的坐标矢量，p₀＝(px₀ py₀ pz₀)是末端工具坐标系的原点(X射线球管及束光器的焦点中心)在主机器人基坐标系中的坐标矢量。

主机器人控制器将末端工具坐标位姿(即X射线球管及束光器的位姿)传送给中央控制模块，中央控制模块据此计算出从机器人末端的位姿(即平板X射线检测器的位姿)，在主机器人基坐标系中平板X射线检测器的位姿矩阵应当是

其中p₁＝p₀+h₀，对于一般情况有：h₀＝(00h)，p₁＝(px₀ py₀ pz₀+h)，h是X射线管到成像介质的距离，也就是平板X射线检测器到X射线球管中焦点中心的空间距离。

中央控制模块根据此位姿矩阵B以及前面得到的主从机器人基坐标系的变换矩阵T，可以计算出在从机器人基坐标系中其末端工具坐标系的位姿(即平板X射线检测器的位姿)，并将其传送到从机器人控制器中。

从机器人控制器根据步骤四中由中央控制模块发送来的末端工具坐标系的位姿信息，控制自身运动，将平板X射线检测器调整到与主机器人夹持的X射线球管及束光器相对应的合适位置，在此位置上X射线球管及束光器的焦点与平板X射线检测器的空间距离是h。

从本实施例可以看出，通过对主从双机器人初始相对位置的测量，中央控制模块就可以将主从机器人的两个独立的坐标系整合为统一的坐标系，在这个统一的坐标系中，中央控制模块根据X线摄影角度和位置的需求，规划主从机器人各自的位姿，协调主从机器人的运动。中央控制模块对高压发生器进行控制，高压发生器驱动X线球管的阴极灯丝。平板X射线检测器将检测影像转化为电信号并传送到影像工作站。

本发明主要是为了满足这样一类双机器人系统的控制需求，具有这种控制需求的双机器人系统的特点是：它们是两个完全独立的机器人，它们是可移动的，其基座初始位置不确定，但在协同工作的过程中各自的基座都不改变位置，在协同工作中双机器人的位姿需要有精确的对应关系。

本发明可以测量双机器人初始相对位置的方法和装置，针对协同工作的两个独立的机器人，初始时其各自基座的相对位置未知，但协同工作的过程中各自的基座位置不改变，本发明设定双机器人中一个为主机器人，另一个为从机器人，给出了测量从机器人相对于主机器人的初始位置的方法，据此方法可以得出从机器人的基坐标系与主机器人的基坐标系间的变换矩阵，从而可以将主机器人的基坐标系作为共同的坐标系，将从机器人也纳入到这个共同的坐标系中实行集中控制，以保证在协同工作的过程中主从两个机器人位姿的精确对应，运动配合协调，这一方法成功应用于双机器人X射线影像系统中的双机器人初始定位和控制。

以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (8)

1.双协作机器人初始定位测量装置，包括主机器人(1)和从机器人(2)，主机器人(1)内设有主机器人控制器(3)，所述的和从机器人(2)内设有从机器人控制器(4)，其特征在于：还包括A测量小板(5)、B测量小板(6)、对射管以及中央控制模块(7)，所述的A测量小板(5)设置于从机器人(2)的末端位置或者末端临近的位置，所述的B测量小板(6)设置于主机器人(1)的末端位置或者末端临近的位置，所述的中央控制模块(7)分别与A测量小板(5)和B测量小板(6)信号连接，所述主机器人(1)通过主机器人控制器(3)与中央控制模块(7)信号连接，所述的从机器人(2)通过从机器人控制器(4)与中央控制模块(7)信号连接，所述的对射管由对射管接收部(51)和对射管发射部(61)组成；其中，所述A测量小板(5)上设有对射管接收部(51)，接收部外沿设有管状遮光套(52)；所述B测量小板(6)上设有对射管发射部(61)和激光测距传感器(62)；所述中央控制模块(7)接收到对射管发出的完成对射的信号后，所述中央控制模块(7)启动所述激光测距传感器(62)进行测距。

2.根据权利要求1所述的双协作机器人初始定位测量装置，其特征在于：所述激光测距传感器(62)是相位式激光测距传感器。

3.根据权利要求1所述的双协作机器人初始定位测量装置，其特征在于：所述对射管为激光对射管或者红外对射管。

4.根据权利要求1所述的双协作机器人初始定位测量装置，其特征在于：所述A测量小板(5)上还设置有位置测量基准点(53)。

5.权利要求4所述的双协作机器人初始定位测量装置的初始定位方法，其特征在于，包括以下步骤：

S1：将A测量小板安装于从机器人的末端，建立从机器人末端工具坐标系：以位置测量基准点为从机器人末端工具坐标系的原点，以A测量小板的法线方向为工具坐标系的Z轴的方向，按照右手定则确定工具坐标系X轴方向和Y轴方向；

S2：使从机器人的末端运动，到达可以让A测量小板上的对射管的接收部无遮挡地接收来自对射管发射部的信号，以及来自激光测距传感器的激光的位姿，将这个位姿定义为从机器人末端工具坐标系的基准位姿A，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵为

在位姿矩阵中，

表示坐标系的方向，p₀表示从机器人末端工具坐标系原点在从机器人基坐标系中的矢量坐标；其中，

r_x00，r_y00，r_z00分别是末端工具坐标系的三个坐标轴在基坐标系中的坐标矢量；p₀＝(px₀ py₀ pz₀)，px₀，py₀，py₀是从机器人末端工具坐标系的原点在从机器人基坐标系中的坐标矢量；

S3：将B测量小板安装于主机器人的末端，建立主机器人末端工具坐标系：以激光测距传感器的测距起始点为主机器人末端工具坐标系的原点，以激光测距传感器射出的激光的方向为工具坐标系的Z轴的方向，并且使Z轴方向与B测量小板的法线方向一致，按照右手定则确定工具坐标系X轴方向和Y轴方向；

S4：调整主机器人末端的位姿，使分别位于主从两个机器人末端的A测量小板、B测量小板上的激光对射管完成对射，由主机器人控制器得到主机器人末端工具坐标系的原点在主机器人基坐标系中的矢量坐标，然后使从机器人的末端运动，保证从机器人末端工具坐标系的原点p₀位置不变，改变从机器人末端工具坐标系的3个坐标轴方向，重新调整主机器人末端的位姿使得A测量小板、B测量小板上的激光对射管完成对射，并得到多组主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标，结合激光测距传感器测得的对应位姿下多组主机器人末端工具坐标系原点和从机器人末端工具坐标系原点间的距离，构建方程组，求解从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₀；其中，v₀和p₀是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

S5：恢复从机器人末端的运动至步骤S2确定的基准位姿A，然后使从机器人末端沿着r_x00的方向运动h的距离，将从机器人的工具坐标系的原点调整到p₁，p₁＝(px₀+h py₀ pz₀)，保证从机器人工具坐标系三个坐标轴的方向与基准位姿A的坐标轴方向相同，将此位姿定义为从机器人末端工具坐标系的基准位姿B，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵为

S6：保证步骤S5的从机器人末端工具坐标系的原点p₁位置不变，重复步骤S4，求解此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₁；其中，v₁和p₁是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

S7：恢复从机器人末端的运动至步骤S2确定的基准位姿A，然后使从机器人末端沿着r_y00的方向运动h的距离，将从机器人的工具坐标系的原点调整到p₂，p₂＝(px₀ py₀+h pz₀)，保证从机器人工具坐标系三个坐标轴的方向与基准位姿A的坐标轴方向相同，将此位姿定义为从机器人末端工具坐标系的基准位姿C，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵

S8：保证步骤S7的从机器人末端工具坐标系的原点p₂位置不变，重复步骤S4，求解此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₂；其中，v₂和p₂是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

S9：恢复从机器人末端的运动至步骤S2确定的基准位姿A，然后使从机器人末端沿着r_z00的方向运动h的距离，将从机器人的工具坐标系的原点调整到p₃，p₃＝(px₀ py₀ pz₀+h)，保证从机器人工具坐标系三个坐标轴的方向与基准位姿A的坐标轴方向相同，将此位姿定义为从机器人末端工具坐标系的基准位姿D，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵

S10：保证步骤S9的从机器人末端工具坐标系的原点p₃位置不变，重复步骤S4，求解此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v₃；其中，v₃和p₃是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；

步骤S11：根据求出的v₀、v₁、v₂、v₃，针对基准位姿计算出从机器人末端工具坐标系在主机器人基坐标系中的位姿矩阵是

其中，

R_x00＝V₁-V₀，R_y00＝V₂-V₀，R_z00＝V₃-V₀，根据步骤S2得到的从机器人基坐标系中的位姿矩阵为

分别将上述两个位姿矩阵扩展成为齐次矩阵

得到主、从两个机器人的坐标系间的变换矩阵T＝PS^-1，根据此变换矩阵，可以将各自独立的主从机器人的基坐标系整合为一个统一的坐标系，由中央控制模块在统一的坐标系中规划主、从机器人的运动轨迹，协调主、从机器人的姿态和动作。

6.权利要求5所述的双协作机器人初始定位测量装置的初始定位方法，其特征在于，所述步骤S4中，v₀的具体求解过程包括以下步骤：

S41：根据从机器人的基准位姿A，调整主机器人末端的位姿，使得安装于主机器人末端的B测量小板的对射管发射部发出的激光或红外射线，穿过位于从机器人末端A测量小板上的管状遮光套，射向对射管的接收部，在这个位姿下，从机器人末端工具坐标系的Z轴和主机器人末端工具坐标系的Z轴重合，将主机器人末端的这个位姿定义为第一次测距位姿，保持主机器人的位姿不变，在主机器人控制器读出主机器人末端工具坐标系的原点在主机器人基坐标系中的矢量坐标是v_z00＝(vx_z00 vy_z00 vz_z00)，启动位于主机器人末端的B测量小板上的激光测距传感器，测出在此位姿下从机器人末端工具坐标系原点和主机器人末端工具坐标系原点间的距离是L₀₀，得到下式：

v₀是待求的，本步骤得到了求解v₀的第一个方程；

S42：使从机器人的末端运动，相对于步骤S2中确定的基准位姿A，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是

S43：针对步骤S42的从机器人末端的新的位姿，再一次调整主机器人的末端位姿做相应的改变，使得分别位于主从两个机器人末端的A测量小板、B测量小板上的激光或红外对射管再次完成对射，主从两个机器人的工具坐标系的Z轴方向再次重合，将主机器人末端的这个位姿定义为第二次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v_z01＝(vx_z01 vy_z01 vz_z01)，基于此原点，用B测量小板上的激光测距传感器测出主从两个机器人末端工具坐标原点间距离是L₀₁，得到下式：

v₀是待求的，本步骤得到了求解v₀的第二个方程；

S44：使从机器人的末端运动，相对于步骤S42中确定的基准位姿，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是

S45：针对步骤S44的从机器人末端的新的位姿，再一次调整主机器人的末端位姿做相应的改变，使得分别位于主从两个机器人末端的A测量小板、B测量小板上的激光或红外对射管再次完成对射，主从两个机器人的工具坐标系的Z轴方向再次重合，将主机器人末端的这个位姿定义为第二次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标v_z02＝(vx_z02 vy_z02 vz_z02)，基于此原点，用B测量小板上的激光测距传感器测出主从两个机器人末端工具坐标原点间距离是L₀₂，得到下式：

v₀是待求的，本步骤得到了求解v₀的第三个方程；

S46：联立步骤S41、步骤S43、步骤S45得到的三个方程，得到方程组：

解方程组得到v₀＝(vx₀ vy₀ vz₀)。

7.一种基于双机器人的X射线影像系统，包括主台车(100)和从台车(200)，其特征在于：还包括权利要求1所述的双协作机器人初始定位测量装置。

8.根据权利要求7所述的一种基于双机器人的X射线影像系统，其特征在于：主机器人(1)和从机器人(2)均为6自由度关节机器人，A测量小板(5)为平板X射线检测器，B测量小板(6)包括X射线球管及束光器，主机器人(1)夹持X射线球管及束光器，从机器人(2)夹持平板X射线检测器，主机器人(1)和从机器人(2)上设有中央控制模块(7)，主机器人(1)载于主台车(100)，从机器人(2)载于从台车(200)。

Images