[0047] 下面结合附图和实施例对本发明的发明内容作进一步地说明。
[0048] 参见图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进行测距。
[0049] 本实施例中，所述激光测距传感器62是相位式激光测距传感器。
[0050] 本实施例中，所述对射管为激光对射管或者红外对射管。
[0051] 本实施例中，所述A测量小板5上还设置有位置测量基准点53。
[0052] 一种双协作机器人初始定位测量装置的初始定位方法，包括以下步骤：
[0053] 步骤一：将所述A测量小板安装于从机器人的末端，建立从机器人末端工具坐标系：以步骤一所述测位标志点为工具坐标系的原点，以所述A测量小板的法线方向为工具坐标系的Z轴的方向，按照右手定则确定工具坐标系X轴方向和Y轴方向；
[0054] 步骤二：使从机器人的末端运动，到达可以让A测量小板上的对射管的接收部无遮挡地接收来自对射管发射部的激光或红外射线，以及来自激光测距传感器的激光的位姿，将这个位姿定义为从机器人末端工具坐标系的基准位姿A(见附图5)，保持这个位姿，在从机器人的控制器中读出在从机器人基坐标系中的位姿矩阵为 在位姿矩阵中，表示坐标系的方向，p0表示从机器人末端工具坐标系原点在从机器人基坐标系中的矢量坐标；其中， rx00，ry00，rz00分别是末端工具坐标系的三个坐标轴
在基坐标系中的坐标矢量；p0＝(px0 py0 pz0)，px0，py0，py0是从机器人末端工具坐标系的原点在从机器人基坐标系中的坐标矢量；
[0055] 步骤三：将B测量小板安装于主机器人的末端，建立主机器人末端工具坐标系：以激光测距传感器的测距起始点为主机器人末端工具坐标系的原点，以激光测距传感器射出的激光的方向为工具坐标系的Z轴的方向，并且应当使Z轴方向与B测量小板的法线方向一致，按照右手定则确定工具坐标系X轴方向和Y轴方向；
[0056] 步骤四：保持从机器人的位姿不变，调整主机器人末端的位姿，使得安装于主机器人末端的B测量小板的对射管发射部发出的激光或红外射线，穿过位于从机器人末端A测量小板上的管状遮光套，射向对射管的接收部，当位于从机器人末端的A测量小板上的对射管接收部接收到射线，会由从机器人发出信号通知主机器人在这个位姿下从机器人末端工具坐标系的Z轴和主机器人末端工具坐标系的Z轴是重合的，将主机器人末端的这个位姿定义为第一次测距位姿，保持主机器人的位姿不变，在主机器人控制器可以读出主机器人末端工具坐标系的原点(即B测量小板上激光测距传感器的起始点)在主机器人基坐标系中的矢量坐标是vz00＝(vxz00 vyz00 vzz00)，启动位于主机器人末端的B测量小板上的激光测距传感器，测出在此位姿下从机器人末端工具坐标系原点和主机器人末端工具坐标系原点间的距离是L00，定义从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标是v0＝(vx0 vy0 vz0)，参见附图6，有下式： v0是待求的，本步骤得到了求解v0的第一个方程；
[0057] 步骤五，使从机器人的末端运动，相对于步骤二中确定的基准位姿A，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是[0058] 步骤六，针对从机器人末端的新的位姿，按照步骤四所述的方式，再一次调整主机器人的末端位姿做相应的改变，使得分别位于主从两个机器人末端的A测量小板、B测量小板上的激光或红外对射管再次完成对射，主从两个机器人的工具坐标系的Z轴方向再次重合，将主机器人末端的这个位姿定义为第二次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标vz01＝(vxz01 vyz01 vzz01)，基于此原点，用B测量小板上的激光测距传感器测出主从两个机器人末端工具坐标系原点间距离是L01，相对于步骤四，从机器人末端工具坐标系原点v0没变，但主机器人末端工具坐标系的原点位置变了，本次测量有： v0是待求的，本步骤得到了求解v0的第二个方程；
[0059] 步骤七，再次使从机器人的末端运动，相对于步骤五确定的位姿，从机器人末端工具坐标系的3个坐标轴方向发生改变，但从机器人末端工具坐标系的原点位置不变，此时在从机器人控制器中读出从机器人末端工具坐标系在从机器人基坐标系中的位姿矩阵是[0060] 步骤八，针对从机器人末端的新的位姿，按照步骤六中所述的方式，再一次调整主机器人的末端位姿做相应的改变，使分别位于主从两个机器人末端的A测量小板、B测量小板上的激光对射管完成对射，从而使主从两个机器人末端工具坐标系的Z轴再次重合，将主机器人末端的这个位姿定义为第三次测距位姿，保持这个位姿，在主机器人控制器中读出此时主机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标vz02＝(vxz02 vyz02 vzz02)，通过B测量小板上激光测距传感器，测出主从两个机器人末端工具坐标系原点的距离是L02，相对于步骤四和步骤六，本次测距时从机器人末端工具坐标系原点v0没变，但主机器人末端工具坐标系的原点位置变了，本次测距有v0是待求的，本步骤得到了求解v0的第三个方程；
[0061] 步骤九：联立步骤四、步骤六、步骤八得到的三个等式，得到方程组：
[0062]
[0063]
[0064]
[0065] 解方程组得到v0＝(vx0 vy0 vz0)，参见步骤二确定的从机器人末端工具坐标系基准位姿A，在基准位姿A，从机器人末端工具坐标系原点是p0＝(px0 py0 pz0)，v0和p0是同一点分别在主机器人基坐标系和从机器人基坐标系中的矢量坐标；
[0066] 步骤十：使从机器人末端运动，恢复到步骤二确定的基准位姿A，从机器人末端工具坐标系的位姿矩阵恢复为 其中 是从机器人的工具坐标系的三个坐标轴在从机器人基坐标系中的矢量坐标，p0＝(px0 py0 pz0)是工具坐标系的原点在从机器人基坐标系中的矢量坐标；
[0067] 步骤十一，再使从机器人末端沿着 的方向运动h，末端工具坐标系的原点到达p1＝(px0+h py0 pz0)的位置(见附图5)，但工具坐标系三个坐标轴的方向并不改变，将此位姿定义为从机器人末端工具坐标系的基准位姿B，此时在从机器人控制器中可以读出，末端工具坐标系在基坐标系中的位姿矩阵为 设此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标为v1＝(vx1 vy1 vz1)，p1和v1是同一点分别基于从机器人和主机器人基坐标系的矢量坐标，p1可以在从机器人控制器中得到，而v1是待求的；
[0068] 步骤十二，针对从机器人末端工具坐标系的基准位姿B的 重复前面步骤四到步骤九的过程，通过这一过程将v1＝(vx1 vy1 vz1)求出；
[0069] 步骤十三：使从机器人末端恢复到步骤二确定的基准位姿A，从机器人末端工具坐标系的位姿矩阵恢复为 其中 是从机器人的工具坐标系的三个坐标轴在从机器人基坐标系中的矢量坐标，p0＝(px0 py0 pz0)是工具坐标系的原点在从机器人基坐标系中的矢量坐标；
[0070] 步骤十四，使从机器人末端沿着 轴运动h，将从机器人末端工具坐标系新的原点调整到p2＝(px0 py0+h pz0)点(见附图5)，而三个坐标轴的方向依然与步骤二确定的基准位姿A的坐标轴方向相同，定义这个位姿为从机器人末端工具坐标系的基准位姿C，在从机器人的控制器中可以读出这时的从机器人末端工具坐标系位姿矩阵是 设此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标为v2＝(vx2 vy2 vz2)，p2和v2表示同一点，但分别是基于从机器人基坐标系和主机器人基坐标系，p2可以在从机器人控制器中得到，而v2是待求的；
[0071] 步骤十五，针对从机器人末端工具坐标系的基准位姿C的 重复前面步骤四到步骤九的过程，通过这一过程将v2＝(vx2 vy2 vz2)求出；
[0072] 步骤十六：使从机器人末端恢复到步骤二确定的基准位姿A，从机器人末端工具坐标系的位姿矩阵恢复为 其中， 是从机器人的工具坐标系的三个坐标轴在从机器人基坐标系中的矢量坐标，p0＝(px0 py0 pz0)是工具坐标系的原点在从机器人基坐标系中的矢量坐标；
[0073] 步骤十七，使从机器人末端沿着 轴运动，将从机器人的工具坐标系新的原点调整到p3＝(px0 py0 pz0+h)点(见附图5)，而三个坐标轴的方向依然与步骤二确定的基准位姿A的坐标轴方向相同，定义这个位姿为从机器人末端工具坐标系的基准位姿D，这时在从机器人的控制器中可以读出末端工具坐标系在基坐标系中的位姿矩阵是 设此时从机器人末端工具坐标系原点在主机器人基坐标系中的矢量坐标为v3＝(vx3 vy3 vz3)，p3和v3表示同一点，但分别是基于从机器人基坐标系和主机器人基坐标系，p3可以在从机器人控制器中得到，而v3是待求的；
[0074] 步骤十八，针对从机器人新的工具坐标系的基准位姿C的 重复前面步骤四到步骤九的过程，通过这一过程将v3＝(vx3 vy3 vz3)求出；
[0075] 步骤十九，根据求出的v0、v1、v2、v3，针对基准位姿计算出从机器人末端工具坐标系在主机器人基坐标系中的位姿矩阵是 其中，Rx00＝V1‑V0，Ry00＝V2‑V0，Rz00＝V3‑V0，前面步骤二时，已经针对同一位姿在从机器人控制器中读出其在从机器人基坐标系中位姿矩阵 分别将上述两个位姿矩阵扩展成
为齐次矩阵 得到主、从两个机器人的坐标系间的变换
‑1
矩阵T＝PS ，根据此变换矩阵，可以将各自独立的主从机器人的基坐标系整合为一个统一的坐标系，由中央控制模块在统一的坐标系中规划主、从机器人的运动轨迹，协调主、从机器人的姿态和动作。
[0076] 如图7所示，是本发明提出的双协作机器人初始位置测量的方法应用于一种基于双机器人的X射线影像系统，该系统包括两个6自由度关节机器人，其中一个机器人夹持X射线球管及束光器，另一机器人夹持平板X射线检测器，夹持X射线管的机器人称为主机器人，夹持平板X射线检测器的机器人称为从机器人，主从机器人由各自的控制器直接控制，在主从机器人之上有中央控制模块，主机器人载于主台车，从机器人载于从台车。
[0077] 在X射线检测开始前，将主从机器人移动到合适的位置，将A小板安装于从机器人的末端的平板X线检测器旁的合适的位置，将B小板安装于主机器人的末端的X射线球管旁的合适的位置，在中央控制模块的控制下，启动本发明所述一种双协作机器人初始相对位置测量方法的步骤三直至步骤二十一，测出主机器人基坐标系和从机器人基坐标系之间的变换矩阵T。
[0078] 引导主机器人的运动以改变其位姿，以使得X射线球管及束光器处于合适的位置以利于X射线照射检查，在主机器人的末端夹持X射线球管及束光器，这样来确定主机器人末端的工具坐标系：以X射线球管及束光器中的焦点的中心为工具坐标系的原点，以X射线的方向为工具坐标系的z轴的方向，按右手定则确定工具坐标系的x轴和y轴。工具坐标系在主机器人基坐标系中的位姿位置矩阵是 其中 rx，ry，rz分别是末端工具坐标系的三个坐标轴在基坐标系中的坐标矢量，p0＝(px0 py0 pz0)是末端工具坐标系的原点(X射线球管及束光器的焦点中心)在主机器人基坐标系中的坐标矢量。
[0079] 主机器人控制器将末端工具坐标位姿(即X射线球管及束光器的位姿)传送给中央控制模块，中央控制模块据此计算出从机器人末端的位姿(即平板X射线检测器的位姿)，在主机器人基坐标系中平板X射线检测器的位姿矩阵应当是 其中p1＝p0+h0，对于一般情况有：h0＝(00h)，p1＝(px0 py0 pz0+h)，h是X射线管到成像介质的距离，也就是平板X射线检测器到X射线球管中焦点中心的空间距离。
[0080] 中央控制模块根据此位姿矩阵B以及前面得到的主从机器人基坐标系的变换矩阵T，可以计算出在从机器人基坐标系中其末端工具坐标系的位姿(即平板X射线检测器的位姿)，并将其传送到从机器人控制器中。
[0081] 从机器人控制器根据步骤四中由中央控制模块发送来的末端工具坐标系的位姿信息，控制自身运动，将平板X射线检测器调整到与主机器人夹持的X射线球管及束光器相对应的合适位置，在此位置上X射线球管及束光器的焦点与平板X射线检测器的空间距离是h。
[0082] 从本实施例可以看出，通过对主从双机器人初始相对位置的测量，中央控制模块就可以将主从机器人的两个独立的坐标系整合为统一的坐标系，在这个统一的坐标系中，中央控制模块根据X线摄影角度和位置的需求，规划主从机器人各自的位姿，协调主从机器人的运动。中央控制模块对高压发生器进行控制，高压发生器驱动X线球管的阴极灯丝。平板X射线检测器将检测影像转化为电信号并传送到影像工作站。
[0083] 本发明主要是为了满足这样一类双机器人系统的控制需求，具有这种控制需求的双机器人系统的特点是：它们是两个完全独立的机器人，它们是可移动的，其基座初始位置不确定，但在协同工作的过程中各自的基座都不改变位置，在协同工作中双机器人的位姿需要有精确的对应关系。
[0084] 本发明可以测量双机器人初始相对位置的方法和装置，针对协同工作的两个独立的机器人，初始时其各自基座的相对位置未知，但协同工作的过程中各自的基座位置不改变，本发明设定双机器人中一个为主机器人，另一个为从机器人，给出了测量从机器人相对于主机器人的初始位置的方法，据此方法可以得出从机器人的基坐标系与主机器人的基坐标系间的变换矩阵，从而可以将主机器人的基坐标系作为共同的坐标系，将从机器人也纳入到这个共同的坐标系中实行集中控制，以保证在协同工作的过程中主从两个机器人位姿的精确对应，运动配合协调，这一方法成功应用于双机器人X射线影像系统中的双机器人初始定位和控制。
[0085] 以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。
