一种基于空间两点投影的协同机器人基坐标系标定方法

摘要

本发明公开了一种基于空间两点投影的协同机器人基坐标系标定方法。将所有机器人以最小标定单位进行分组，建立两台机器人基坐标系，在两台机器人各自基坐标系原点连线的两侧各任意确定一标定点，将机器人末端点在标定点处以握手方式接触获得关节转角信息；建立D‑H坐标系计算末端姿态矩阵、末端RPY转角以及两个标定点在基坐标系下的坐标值；将机器人与标定点进行投影，计算得到三个矩阵，将三个矩阵相组合得到相对位姿变换矩阵，每个最小标定单位均重复步骤而完成协同机器人基坐标系的标定。本发明依靠空间投影充分利用机器人系统自身设施，需要标定空间点数量少，有效降低了成本，显著缩短了时间，有效提高了效率，适应范围广。

权利要求

1.一种基于空间两点投影的协同机器人基坐标系标定方法，其特征在于包括以下步骤：

第一步：将所有n台机器人以相互之间具有协同关系的任意两台机器人为一个小组作为最小标定单位进行划分；

第二步：在最小标定单位的两台机器人Ri，Rj中，建立两台机器人各自的基坐标系Fi，Fj，在两台机器人各自基坐标系原点Oi，Oj连线的两侧各任意确定一个标定点，作为公共工作空间中的两个标定点P1和P2，第三步：将该两台机器人的末端点分别在标定点P1和标定点P2处以握手方式接触，获得各自机器人的关节转角信息；

第四步：分别建立该两台机器人的D-H坐标系，利用关节转角信息计算出各自的末端姿态矩阵 和 k＝1，2，k＝1分别表示机器人在标定点P1处，k＝2分别表示机器人在标定点P2处，n表示机器人的旋转关节数量，再计算出各末端对应的RPY转角中绕基坐标系z轴旋转角γ；

第五步：将该两台机器人以及两个标定点P1和P2进行投影，利用其平面几何约束关系，计算得到相对水平平移矩阵 相对水平旋转矩阵 和相对垂直平移矩阵第六步：将相对水平平移矩阵 相对垂直平移矩阵 与相对水平旋转矩阵 相组合得到两机器人基坐标系之间最终的相对位姿变换矩阵第七步：对于由具有协同关系的两台机器人组成的每个最小标定单位均重复以上步骤二～六，完成协同机器人基坐标系的标定。

2.根据权利要求1所述的一种基于空间两点投影的协同机器人基坐标系标定方法，其特征在于：所述的第五步具体为：将该两台机器人以及两个标定点P1和P2向水平面投影，利用其平面几何约束关系，计算出水平面上该两台机器人基座标Fi，Fj之间的相对水平平移矩阵 和相对水平旋转矩阵 将两台协同作业的机器人以及两个标定点向垂直面投影，利用其平面几何约束关系，计算出垂直面上两台协作机器人基座标Fi，Fj之间的相对垂直平移矩阵

3.根据权利要求1所述的一种基于空间两点投影的协同机器人基坐标系标定方法，其特征在于：所述第一步中所有n台机器人划分后至少共划分有n-1个小组。

4.根据权利要求2所述的一种基于空间两点投影的协同机器人基坐标系标定方法，其特征在于：所述相对水平平移矩阵 和相对水平旋转矩阵 具体采用以下方式得到：建立XYZ三维坐标系，z轴方向竖直向上，将该两台机器人以及标定点P1和标定点P2向水平的X-Y平面投影，利用X-Y平面投影的几何约束关系以及机器人末端点RPY角中的绕z轴旋转角γ，计算出其中一机器人Rj相对于另一机器人Ri的基坐标系在X-Y平面上的x方向偏移量px和y方向偏移量py以及相对于绕z轴的旋转量θrotz，由x方向偏移量px和y方向偏移量py组合得到相对水平平移矩阵 由旋转量θrotz得到相对水平旋转矩阵

5.根据权利要求2所述的一种基于空间两点投影的协同机器人基坐标系标定方法，其特征在于：所述相对垂直平移矩阵 具体采用以下方式得到：建立XYZ坐标系，z轴方向竖直向上，将该两台机器人以及标定点P1和标定点P2向垂直的Y-Z平面投影，利用Y-Z平面投影的几何约束关系以及标定点P1和标定点P2在各自基坐标系下的z轴坐标值，计算出其中一机器人Rj相对于另一机器人Ri的基坐标系在z轴竖直方向上的z方向偏移量pz，由z方向偏移量pz得到相对垂直平移矩阵

说明书

一种基于空间两点投影的协同机器人基坐标系标定方法

技术领域

[0001] 本发明涉及了一种机器人标定方法，尤其是涉及了一种基于空间两点投影的协同机器人基坐标系标定方法，涉及工业生产中的焊接，搬运，雕刻以及喷涂等诸多工业机器人复杂柔性化系统制造领域。

背景技术

[0002] 随着工业4.0时代的到来，许多领域诸如大型复杂零部件的加工，装配和搬运，无夹具系统的焊接，喷涂以及雕刻等，传统的单个机器人已经不能很好的满足其加工要求，因此研究具有协同作业关系的多机器人系统将成为工业发展的必然趋势。多机器人系统在协同作业时需要预先对其每个机器人的相对位置进行标定确认，即对于具有协同作业关系的机器人之间需要精确的对其基坐标系进行标定，从而准确的知道每个机器人基坐标系之间的相对位姿关系，然后才能对其运行的轨迹进行准确规划。

[0003] 当前工业生产中，针对多机器人系统的基坐标系标定依然缺乏较为有效的方法，主要分为两大类：接触式标定方法和非接触式标定方法，非接触式标定方法中一般都需要增加外部传感器和测量仪等装置，标定步骤繁琐以及精度也较为有限。现有的接触式标定方法中，已经提出有通过插孔、法兰盘接触等方法，标定步骤较非接触式标定方法也有了一定优化，但插孔、法兰盘重合等方式中示教的难度较大，需要选定的标定点数量较多并且选择规则复杂。

发明内容

[0004] 为了解决多机器人协同作业系统的基坐标系标定问题，本发明的目的是提出了一种基于空间两点投影的协同机器人基坐标系标定方法，针对基坐标系z轴垂直于同一平面的协同机器人系统，不需要增加外部的精密传感仪器以及其他测量工具，更简单，高效，可行性更强，通过让具有协同作业关系的两机器人以任意姿态握手到达空间的两个标定点，基于空间投影的几何约束关系，从而快速，精确的对其机器人基坐标系进行相对位姿的标定。

[0005] 本发明采用的技术方案是采用以下步骤：

[0006] 第一步：将所有n台机器人以相互之间具有协同关系的任意两台机器人为一个小组作为最小标定单位进行划分；

[0007] 所述第一步中所有n台机器人划分后至少共划分有n-1个小组。

[0008] 第二步：在最小标定单位的两台机器人Ri，Rj中，建立两台机器人各自的基坐标系Fi，Fj，在两台机器人各自基坐标系原点Oi，Oj连线的两侧各任意确定一个标定点，作为公共工作空间中的两个标定点P1和P2；

[0009] 第三步：将该两台机器人的末端点分别在标定点P1和标定点P2处以握手方式接触，获得以下各自机器人的关节转角信息；

[0010] iR1(iθ1,iθ2,iθ3…iθn)jR1(iθ1,iθ2,iθ3…iθn)

[0011] iR2(iθ1,iθ2,iθ3…iθn)jR2(iθ1,iθ2,iθ3…iθn)

[0012] 其中，iR1(iθ1,iθ2,iθ3…iθn)表示机器人Ri在第一标定点P1处各关节转角值，iR2(iθ1,iθ2,iθ3…iθn)表示机器人Ri在第二标定点P2处各关节转角值，jR1(iθ1,iθ2,iθ3…iθn)表示机器人Rj在第一标定点P1处各关节转角值；jR2(iθ1,iθ2,iθ3…iθn)表示机器人Rj在第二标定点P2处各关节转角值。

[0013] 第四步：分别建立该两台机器人的D-H坐标系，利用关节转角信息计算出以下各自的末端姿态矩阵 和末端姿态矩阵

[0014]

[0015] 其中，k＝1或2，k＝1分别表示机器人在标定点P1处，k＝2分别表示机器人在标定点P2处， 分别表示该两机器人在标定点P1，P2处末端点姿态相对于各基坐标系的旋转矩阵， 分别表示机器人Ri的末端点在基坐标
系Fi中的x,y,z坐标值； 分别表示机器人Rj的末端点在基坐标系Fj中的x,
y,z坐标值；再计算出各末端对应的RPY转角中绕基坐标系z轴旋转角γ。

[0016] 第五步：将该两台机器人以及两个标定点P1和P2进行投影，利用其平面几何约束关系，计算得到相对水平平移矩阵 相对水平旋转矩阵 和相对垂直平移矩阵

[0017] 所述的第五步具体为：

[0018] 将该两台机器人以及两个标定点P1和P2向水平面投影，利用其平面几何约束关系，计算出水平面上该两台机器人基座标Fi，Fj之间的相对水平平移矩阵 和相对水平旋转矩阵

[0019] 将两台协同作业的机器人以及两个标定点向垂直面投影，利用其平面几何约束关系，计算出垂直面上两台协作机器人基座标Fi，Fj之间的相对垂直平移矩阵

[0020] 所述相对水平平移矩阵 和相对水平旋转矩阵 具体采用以下方式得到：

[0021] 建立XYZ坐标系，z轴方向竖直向上，将该两台机器人以及标定点P1和标定点P2向水平的X-Y平面投影，利用X-Y平面投影的几何约束关系以及RPY角中的绕z轴旋转角γ，计算出其中一机器人Rj相对于另一机器人Ri的基坐标系在X-Y平面上的x方向偏移量px和y方向偏移量py以及相对于绕z轴的旋转量θrotz，由x方向偏移量px和y方向偏移量py组合得到相对水平平移矩阵 由旋转量θrotz得到相对水平旋转矩阵

[0022] 如附图4所示，P1′,P2′为标定点P1和标定点P2在x-y平面上的两个投影点，和 分别表示两台机器人Ri，Rj末端点处于标定点P1和标定点P2时在x-y平面上的投影，Oi′，Oj′分别为两台协同机器人的基坐标系原点在x-y平面的投影点，ri-1，ri-2分别表示机器人Ri末端点在P1和P2处位姿的RPY转角γi-1，γi-2在x-y平面的投影，rj-1，rj-2分别表示机器人Rj末端点在P1和P2处位姿的RPY转角γj-1，γj-2在x-y平面的投影。

[0023] 在x-y平面确定两协同关系的机器人 基坐标系的相对位姿，采用以下公式先求解得到标定点与基坐标系原点之间的距离，再进一步求解出两基坐标系之间在x-y平面上投影的旋转角θrotz和平移偏量(px,py)：

[0024]

[0025]

[0026] 其中，|oi′pi1|，|oi′p1′|表示机器人Ri的末端点在P1处与基坐标系Fi原点Oi之间的连线长度的水平投影，|oj′pj1|，|oj′p1′|表示机器人Rj的末端点在标定点P1处与基坐标系Fj原点Oj之间的连线长度的水平投影，|oi′pi2|，|oi′p2′|表示机器人Ri的末端点在标定点P2处与基坐标系Fi原点Oi之间的连线长度的水平投影，|oj′pj2|，|oj′p2′|表示机器人Rj的末端点在标定点P2处与基坐标系Fj原点Oj之间的连线长度的水平投影。

[0027] 附图4所示的三角形Δoi′p1′p2′中，已知第一机器人Ri在两个标定点之间的旋转角ri＝|ri-1-ri-2|和第二机器人Rj在两个标定点之间的旋转角rj＝|rj-1-rj-2|利用余弦定理，可计算出∠1以及|p1′p2′|，其中|p1′p2′|表示两标定点P1和P2连线在x-y水平面的投影长度，∠1表示|oi′pi2|与|p1′p2′|之间的夹角。

[0028] 同理在Δoj′p1′p2′中，可计算出∠2，在Δoi′Ap2′，Δoj′Bp2′中采用以下公式可依次计算出：

[0029] ∠3＝π-(∠1+|ri-2|)

[0030] ∠4＝π-(∠2+|rj-2|)

[0031] ∠5＝∠1+∠2

[0032] 其中，∠3表示机器人Ri的基坐标系Fi中x轴与|p1′p2′|之间的夹角，∠4表示机器人Rj的基坐标系Fj中x轴与|p1′p2′|之间的夹角，∠5表示|oi′p2′|与|oj′p2′|之间的夹角；

[0033] 在三角形Δoi′p2′oj′中，利用余弦定理可以计算出：|oi′oj′|以及∠6，∠7＝|ri-2|-∠6，其中|oi′oj′|表示机器人Ri和机器人Rj各基坐标系原点连线的水平投影长度，∠6表示|oi′p2′|与|oi′oj′|之间的夹角，∠7表示机器人Ri的基坐标系Fi中x轴与|oi′oj′|之间的夹角。

[0034] 因此得到旋转角θrotz和平移偏量(px,py)：

[0035]

[0036] 所述相对垂直平移矩阵 具体采用以下方式得到：

[0037] 建立XYZ坐标系，z轴方向竖直向上，将该两台机器人以及标定点P1和标定点P2向垂直的Y-Z平面投影，利用Y-Z平面投影的几何约束关系以及P1，P2点在各自基坐标系下的z轴坐标值，计算出其中一机器人Rj相对于另一机器人Ri的基坐标系在z轴竖直方向上的z方向偏移量pz，由z方向偏移量pz得到相对垂直平移矩阵 具体如下述方式可得：

[0038] pz1＝piz-1-pjz-1

[0039] pz2＝piz-2-pjz-2

[0040]

[0041] 其中，piz-1，pjz-1表示机器人Ri和机器人Rj的末端点在标定点P1处在各基坐标系中z轴上的坐标值，piz-2，pjz-2表示机器人Ri和机器人Rj的末端点在标定点P2处在各基坐标系中z轴上的坐标值；根据上述步骤已经求解得到了全部参数和信息(即相对水平平移矩阵相对水平旋转矩阵 和相对垂直平移矩阵 )，从而根据坐标变换，对协同作业的机器人基坐标系进行相对位姿的标定，根据步骤四的标定方程如下：

[0042] (1)针对该两台机器人Ri和Rj均安装在水平地面的情况，即该两台机器人的基坐标系Fi和Fj的z轴垂直于水平地面向上，两机器人基坐标系Fi和Fj之间的相对位姿变换矩阵为：

[0043]

[0044] 其中Rot(Z,θrotz)表示机器人Rj的基坐标系Fj相对于机器人Ri的基坐标系Fi绕z轴旋转的矩阵， 表示表示机器人Rj的基坐标系Fj相对于机器人Ri的基坐标系Fi的平移矩阵， 表示机器人Rj的基坐标系Fj相对于机器人Ri的基坐标系Fi的变换矩阵；

[0045] 将上述相对水平平移矩阵 相对垂直平移矩阵 与相对水平旋转矩阵 相组合得到两机器人基坐标系之间最终的相对位姿变换矩阵

[0046] (2)针对该两台机器人Ri和Rj，其中机器人Ri安装在水平地面，另外一台机器人Rj安装在顶面的情况，即该机器人Ri的基坐标系Fi中的z轴垂直于水平地面向上，机器人Rj的基坐标系Fj中的z轴垂直于水平地面向下，两机器人基坐标系Fi和Fj之间的相对位姿变换矩阵为：

[0047]

[0048]

[0049]

[0050] 其中 表示机器人Rj的基坐标系Fj相对于机器人Ri的基坐标系Fi的旋转的矩阵，表示表示机器人Rj的基坐标系Fj相对于机器人Ri的基坐标系Fi的平移矩阵， 表示机器人Rj的基坐标系Fj相对于机器人Ri的基坐标系Fi的变换矩阵；

[0051] 第六步：将上述相对水平平移矩阵 相对垂直平移矩阵 与相对水平旋转矩阵相组合得到两机器人基坐标系之间最终的相对位姿变换矩阵

[0052] 第七步：对于由具有协同关系的两台机器人组成的每个最小标定单位均重复以上步骤二～六，完成协同机器人基坐标系的标定。

[0053] 基于空间两点投影的协同作业机器人基坐标系标定方法的基本原理在于：在具有协同关系的两台机器人基坐标系原点的连线异侧，预先在其公共工作空间范围内确定两个标定点，让两协同机器人通过握手的方式，以任意空间姿态分别到达两标定点，从而获取相应的各机器人关节转角信息以及RPY角数值，利用协同作业机器人系统以及确定的两标定点在空间平面投影的几何约束关系，求解出两协同作业机器人基座标之间的旋转及平移偏量，最终完成两基坐标系之间的位姿标定。

[0054] 本发明的优势和有益效果在于如下几点：

[0055] 1.针对协同作业的多机器人系统，本发明作为协同作业的多机器人 基坐标系的标定是保证系统正常工作的前提，是使得机器人系统能高效，快速，精确的完成工作任务的技术基础，本发明在标定过程中没有使用任何的附加外部传感器以及测量仪器，完全依靠机器人系统自身的内部测量传感器，通过让协同作业的机器人末端点以任意姿态到达标定点，从而获得标定需要的全部信息。本发明在保证标定的高精度的同时充分利用了机器人系统自身的硬件条件和设施，有效的降低了标定成本，依靠空间投影建立协同机器人系统的几何约束关系图，从而完成基坐标系的标定。

[0056] 2.本发明方法，在现有的所有针对协同作业的多机器人系统基坐标系标定的接触式方法中，需要标定的空间点的数量是最少的，只需要在具有协同关系的两机器人基坐标系原点连线的异侧以及重叠公共工作空间内，确定两个标定点即可完成标定，显著缩短了标定的时间，有效提高了标定的效率。

[0057] 3.本发明方法，相比于现有的标定方法，在标定点的选取规则上更加的简单，只需要在具有协同关系的两机器人基坐标系原点连线的异侧以及重叠公共工作空间内，任意的选定两个不重复的点即可，具有操作步骤简单，标定精度较高，适应范围广，能运用于工业生产中的各类工作环境，为今后协同作业的多机器人系统的大规模推广提供了技术基础和支持。

附图说明

[0058] 图1是本发明的方法流程框图。

[0059] 图2是本发明中任意两机器人均是基于水平地面安装的协同方式示意图，即该两台机器人Ri和Rj的基坐标系Fi和Fj的z轴垂直于水平地面向上。

[0060] 图3是本发明中任意两机器人的协同方式示意图，其中一台是基于水平地面安装，另外一台是基于顶面安装，即该机器人Ri的基坐标系Fi中的z轴垂直于水平地面向下，机器人Rj的基坐标系Fj中的z轴垂直于水平地面向上。

[0061] 图4为本发明的机器人和空间两标定点在X-Y平面进行投影形成的几何约束关系图。

[0062] 图5为本发明的两基坐标系Z轴同向的情况下两机器人和空间两标定点在Y-Z平面投影的几何约束关系图。

[0063] 图6为为本发明的两基坐标系Z轴反向情况下两机器人和空间两标定点在Y-Z平面投影的几何约束关系图。

[0064] 图7为本发明的具有3台机器人的协同系统的安装示意图，描述了各机器人基坐标系之间的相互变换关系，图中1,2,3分别表示机器人Ri，Rj Rk， 表示机器人Rk的基坐标系相对于机器人Ri的基坐标系之间的相对变换矩阵， 表示机器人Rj的基坐标系相对于机器人Rk的基坐标系之间的变换矩阵, 表示机器人Rj的基坐标系相对于机器人Ri的基坐标系之间的变换矩阵。

具体实施方式

[0065] 下面结合附图及具体实施例对本发明作进一步详细说明。

[0066] 本发明的基本原理主要在于：在两协同作业机器人基坐标系原点连线的异侧以及相互重叠的公共工作空间内任意确定两个不重复的标定点，通过构造机器人握手的动作，使得两协同机器人的末端点以任意空间位姿分别同时到达标定点，对协同作业机器人系统以及确定的标定点进行X-Y，Y-Z平面的投影，建立平面的投影几何约束关系图，从而获取协同作业机器人基坐标系相对位姿的旋转及平移偏量，最终求得两基坐标系的位姿变换矩阵，完成基坐标系的标定工作。

[0067] 本发明的实施例：

[0068] 附图1中的框图为本发明提出的新型标定方法的具体流程示意图，如附图所示。本发明以3台新松负载6kg的六轴机器人为试验平台，如附图7所示，针对协同作业的多机器人系统基坐标系标定，结合实施案例进行说明，其中涉及的角度均以弧度制表示，本发明的具体标定流程主要包括以下步骤：

[0069] 第一步：将3台机器人按照具有协同关系以每两台一个小组进行划分，至少共划分为2个小组；

[0070] 第二步：以具有协同关系的两台机器人(Ri，Rj)(Ri，Rk)为最小标定单位系统，任意选定协同机器人工作空间中处于基坐标系原点连线两侧的两个标定点P1，P2，通过让两台机器人以握手的方式到达预先确定的标定点，分别获得各机器人的关节转角信息，先对第一小组两台机器人(Ri，Rj)进行标定：

[0071] 在第一个标定点P1处，获得机器人Ri，Rj的各关节转角信息：

[0072] iR1(iθ1,iθ2,iθ3,iθ4,iθ5,iθ6)＝[-0.0429,-1.4477,0,0,1.1200,0][0073] jR1(jθ1,jθ2,jθ3,jθ4,jθ5,jθ6)＝[0.0429,-1.4488,0,0,1.1209,0][0074] 在第二个标定点P2处，获得机器人Ri，Rj的各关节转角信息：

[0075] iR2(iθ1,iθ2,iθ3,iθ4,iθ5,iθ6)＝[-1.2868,-1.5746,0,0,1.5746,0][0076] jR2(jθ1,jθ2,jθ3,jθ4,jθ5,jθ6)＝[1.2823,-1.5691,0,0,1.5691,0][0077] 第三步：分别建立两台机器人的D-H坐标系，利用握手动作测定的关节转角信息计算出各机器人的末端姿态矩阵：

[0078] a、机器人Ri的末端点在第一个标定点P1处：

[0079] 机器人Ri的末端点姿态矩阵：

[0080]

[0081] 机器人Ri的末端点RPY转角γ角度值为：ri-1＝0.0453；机器人Ri的末端点在基坐标i i i系Fi中的坐标值Pi-1(px-1,py-1,pz-1)为：Pi-1(0.9565,-0.0411,0.8526)；

[0082] b、机器人Rj的末端点在第一个标定点P1处：

[0083] 机器人Rj的末端点姿态矩阵：

[0084]

[0085] 机器人Rj的末端点RPY转角γ角度值为：rj-1＝-0.0454；机器人Rj的末端点在基坐标系Fj中的坐标值Pj-1(pjx-1,pjy-1,pjz-1)为：Pj-1(0.9558,0.0410,0.8535)；

[0086] c、机器人Ri的末端点在第二个标定点P2处：

[0087] 机器人Ri的末端点姿态矩阵：

[0088]

[0089] 机器人Ri的末端点RPY转角γ角度值为：ri-2＝1.2868；机器人Ri的末端点在基坐标系Fi中的坐标值Pi-2(pix-2,piy-2,piz-2)为：Pi-2(0.2248,-0.7702,0.9280)；

[0090] d、机器人Rj的末端点在第二个标定点P2处：

[0091] 机器人Rj的末端点姿态矩阵：

[0092]

[0093] 机器人Rj的末端点RPY转角γ角度值为：rj-2＝-1.2823；机器人Rj的末端点在基坐标系Fj中的坐标值Pj-2(pjx-2,pjy-2,pjz-2)为：Pj-2(0.2294,0.7729,0.9244)；

[0094] 第四步：将协同作业机器人系统以及两标定点进行x-y平面投影，如附图4所示，P1′,P2′为空间中不重合的两标定点P1，P2在x-y平面上的投影，pi1，pi2，pj1，pj2分别表示机器人Ri，Rj的末端点在P1，P2处在x-y平面上的投影，Oi′，Oj′为两协同机器人的基坐标系原点在x-y平面的投影点，ri-1，ri-2表示机器人Ri末端点在P1′,P2′处的RPY角γi-1，γi-2在x-y平面的投影，rj-1，rj-2表示机器人Rj末端点在P1′,P2′处位姿的RPY角γj-1，γj-2在x-y平面的投影。

[0095] 如附图4所示，在x-y平面确定两协同关系的机器人基坐标系的相对位姿，需要求解出两基坐标系之间在x-y平面上投影的旋转角θrotz和平移偏量(px,py)，具体描述方程如下：

[0096]

[0097] 代入数据求解得：

[0098] |oi′pi1|＝|oi′p1′|＝0.9573

[0099] |oi′pi2|＝|oi′p2′|＝0.8023

[0100]

[0101] 代入数据求解得：

[0102] |o′jpj1|＝|o′jp1′|＝0.9567

[0103] |o′jpj2|＝|o′jp′2|＝0.8062

[0104] 在三角形Δoi′p1′p2′中，角度ri＝|ri-1-ri-2|＝1.2415，角度rj＝|rj-1-rj-2|＝1.2370，利用余弦定理，可计算出：角度∠1＝1.0726，p1′p′2|＝1.0303；

[0105] 同理在Δo′jp1′p2′中，可计算出：角度∠2＝1.0717，在Δoi′Ap2′，Δoj′Bp2′中可依次计算出：

[0106] 角度∠3＝π-(∠1+|ri-2|)＝0.7821

[0107] 角度∠4＝π-(∠2+|rj-2|)＝0.7876

[0108] 角度∠5＝∠1+∠2＝2.1444

[0109] 在三角形Δoi′p2′oj′中，利用余弦定理可计算出：|oi′o′j|＝1.4126，角度∠6＝0.4999，角度∠7＝|ri-2|-∠6＝0.7868，因此：

[0110]

[0111]

[0112] 根据上述参数可以求得协同机器人两基坐标系Fi，Fj之间相对水平旋转矩阵：

[0113]

[0114] 相对水平平移矩阵：

[0115]

[0116] 如附图5所示，在Y-Z平面确定两协同关系的机器人基坐标系的相对位姿，需要求解出两基坐标系之间在Y-Z平面上投影的平移偏量pz，具体描述方程如下：

[0117]

[0118] 代入数据计算可得：

[0119] pz＝0.0014

[0120] 根据pz可求得协同机器人两基坐标系Fi，Fj之间相对垂直平移矩阵：

[0121]

[0122] 根据上述步骤已经求解得到了基于空间两点投影的协同机器人基坐标系标定方法所需要的全部参数和信息，从而根据坐标变换，对协同作业的机器人基坐标系进行相对位姿的标定，标定方程如下：

[0123]

[0124]

[0125] 上述是针对该两台机器人Ri和Rj均水平安装在地面的情况，如附图2的双机器人协同安装方式，完成了机器人Ri，Rj基坐标系之间的标定工作。

[0126] 针对前面所述的该两台机器人Ri和Rj相对安装方式的另外一种情况，即其中机器人Ri安装在水平地面，另外一台机器人Rj安装在顶面的情况，即该机器人Ri的基坐标系Fi中的z轴垂直于水平地面向上，机器人Rj的基坐标系Fj中的z轴垂直于水平地面向下，如附图3所示的协同安装方式：

[0127]

[0128]

[0129] 相对水平平移矩阵：

[0130]

[0131] 如附图6所示，在Y-Z平面确定两协同关系的机器人基坐标系的相对位姿，需要求解出两基坐标系之间在Y-Z平面上投影的平移偏量pz，具体描述方程如下：

[0132]

[0133] 代入数据计算可得：

[0134] pz＝1.7793

[0135] 根据pz可以求得协同机器人两基坐标系Fi，Fj之间相对垂直平移矩阵：

[0136]

[0137] 根据上述步骤已经求解得到了基于空间两点投影的协同机器人Ri，Rj的基坐标系标定所需要的全部参数和信息，从而根据坐标变换，对协同作业的机器人基坐标系进行相对位姿的标定，标定方程如下：

[0138]

[0139]

[0140] 第五步：对第二小组两台机器人(Ri，Rk)进行标定，标定过程与第一小组机器人(Ri，Rj)完全相同，Rj和Rk的投影关系与附图4，附图5及附图6相同。

[0141] 在第一个标定点P1′处，获得机器人Ri，Rk的各关节转角信息：

[0142] iR1(iθ1,iθ2,iθ3,iθ4,iθ5,iθ6)＝[-1.5708,-1.5708,0,0,0.5027,0][0143] kR1(kθ1,kθ2,kθ3,kθ4,kθ5,kθ6)＝[0.79,-1.568,0,0,0.5027,0]

[0144] 在第二个标定点P2′处，获得机器人Ri，Rk的各关节转角信息：

[0145] iR2(iθ1,iθ2,iθ3,iθ4,iθ5,iθ6)＝[-2.7080,-1.946,0.1257,0,0.5030,0][0146] kR2(jθ1,jθ2,jθ3,jθ4,jθ5,jθ6)＝[1.924,-1.945,0.1257,0,0.5060,0][0147] 第三步：分别建立两台机器人Ri，Rk的D-H坐标系，利用握手动作测定的关节转角信息计算出各机器人Ri，Rk的所有标定信息参数：

[0148]

[0149] 根据上述参数可以求得协同机器人两基坐标系Fi，Fk之间相对水平旋转矩阵：

[0150]

[0151] 相对水平平移矩阵：

[0152]

[0153] 如附图5所示，在Y-Z平面确定两协同关系的机器人基坐标系的相对位姿，需要求解出两基坐标系之间在Y-Z平面上投影的平移偏量pz，代入数据计算可得：

[0154] pz＝0.0018

[0155] 根据pz可求得协同机器人两基坐标系Fi，Fj之间相对垂直平移矩阵：

[0156]

[0157] 根据上述步骤已经求解得到了基于空间两点投影的协同机器人基坐标系标定方法所需要的全部参数和信息，从而根据坐标变换，对协同作业的机器人基坐标系进行相对位姿的标定，标定方程如下：

[0158]

[0159]

[0160] 完成了针对该两台机器人Ri和Rk均水平安装在地面的情况下基坐标系之间的标定工作。

[0161] 针对前面所述的该两台机器人Ri和Rk相对安装方式的另外一种情况，如附图3中的协同安装方式，即其中机器人Ri安装在水平地面，另外一台机器人Rk安装在顶面的情况，即该机器人Ri的基坐标系Fi中的z轴垂直于水平地面向上，机器人Rk的基坐标系Fk中的z轴垂直于水平地面向下：

[0162]

[0163]

[0164] 相对水平平移矩阵：

[0165]

[0166] 如附图6所示，在Y-Z平面确定两协同关系的机器人基坐标系的相对位姿，需要求解出两基坐标系之间在Y-Z平面上投影的平移偏量pz，代入数据计算可得：

[0167] pz＝2.2445

[0168] 根据pz可以求得协同机器人两基坐标系Fi，Fj之间相对垂直平移矩阵：

[0169]

[0170] 根据上述步骤已经求解得到了基于空间两点投影的协同机器人基坐标系标定方法所需要的全部参数和信息，从而根据坐标变换，对协同作业的机器人基坐标系进行相对位姿的标定，标定方程如下：

[0171]

[0172]

[0173] 在本实施案例中，3台新松6kg机器人均是安装在同一水平地面上，如附图7所示，根据坐标系变换关系可知：

[0174]

[0175]

[0176] 其中 表示机器人Rj基坐标系Fj相对于机器人Ri基坐标系Fi的变换矩阵， 表示机器人Rk基坐标系Fk相对于机器人Ri基坐标系Fi的变换矩阵；

[0177]

[0178]

[0179]

[0180]

[0181] 其中 表示 的逆矩阵， 表示 的逆矩阵，根据上述计算可以知道机器人Rk的基坐标系Fk相对于机器人Rj基坐标系Fj的变换矩阵为：

[0182]

[0183] 代入数据得到：

[0184]

[0185] 第六步：根据第一步所述，3台机器人按照协同关系至少划分成了2个小组，重复上述步骤2次，完成2组两两协作的机器人基坐标系标定，利用彼此不同机器人之间的相对位姿关系，可以计算出任意两台机器人之间的相对位姿关系，进而完成了3台机器人Ri，Rj，Rk基坐标系之间的标定工作。

[0186] 由此可见，本发明在标定过程中没有使用任何的附加外部传感器以及测量仪器，完全依靠机器人系统自身的内部测量传感器，通过让协同作业的机器人末端点以任意姿态到达标定点，从而获得标定需要的全部信息。本发明在保证标定的高精度的同时充分利用了机器人系统自身的硬件条件和设施，有效的降低了标定成本，显著缩短了标定的时间，能有效提高标定效率40％左右，适应范围广，具有突出显著的技术效果。