CN105180962A - 一种基于空间两点投影的协同机器人基座标系标定方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

第二步：在最小标定单位的两台机器人R_i，R_j中，建立两台机器人各自的基座标系F_i，F_j，在两台机器人各自基座标系原点O_i，O_j连线的两侧各任意确定一个标定点，作为公共工作空间中的两个标定点P1和P2；

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

ⁱR₁(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)^jR₁(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)

ⁱR₂(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)^jR₂(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)

其中，ⁱR₁(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)表示机器人R_i在第一标定点P1处各关节转角值，ⁱR₂(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)表示机器人R_i在第二标定点P2处各关节转角值，^jR₁(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)表示机器人R_j在第一标定点P1处各关节转角值；^jR₂(ⁱθ₁，ⁱθ₂，ⁱθ₃…ⁱθ_n)表示机器人R_j在第二标定点P2处各关节转角值。

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

$\begin{array}{cc}T_{i-k}^0{}_n=\left[\begin{array}{cccc}{n}_{x-k}^i& o_{x-k}^i& a_{x-k}^i& p_{x-k}^i\\ n_{y-k}^i& o_{y-k}^i& a_{y-k}^i& p_{y-k}^i\\ n_{z-k}^i& o_{z-k}^i& a_{z-k}^i& p_{z-k}^i\\ 0& 0& 0& 1\end{array}\right]& T_{j-k}^0{}_n=\end{array}\left[\begin{array}{cccc}{n}_{x-k}^j& o_{x-k}^j& a_{x-k}^j& p_{x-k}^j\\ n_{y-k}^j& o_{y-k}^j& a_{y-k}^j& p_{y-k}^j\\ n_{z-k}^j& o_{z-k}^j& a_{z-k}^j& p_{z-k}^j\\ 0& 0& 0& 1\end{array}\right]$

其中，k＝1或2，k＝1分别表示机器人在标定点P1处，k＝2分别表示机器人在标定点P2处， $\begin{array}{cc}\left[\begin{array}{ccc}{n}_{x-k}^i& o_{x-k}^i& a_{x-k}^i\\ n_{y-k}^i& o_{y-k}^i& a_{y-k}^i\\ n_{z-k}^i& o_{z-k}^i& a_{z-k}^i\end{array}\right]& \left[\begin{array}{ccc}{n}_{x-k}^j& o_{x-k}^j& a_{x-k}^j\\ n_{y-k}^j& o_{y-k}^j& a_{y-k}^j\\ n_{z-k}^j& o_{z-k}^j& a_{z-k}^j\end{array}\right]\end{array}$ 分别表示该两机器人在标定点P1，P2处末端点姿态相对于各基座标系的旋转矩阵，分别表示机器人R_i的末端点在基座标系F_i中的x，y，z坐标值；分别表示机器人R_j的末端点在基座标系F_j中的x，y，z坐标值；再计算出各末端对应的RPY转角中绕基座标系z轴旋转角γ。

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

所述的第五步具体为：

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

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

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

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

如附图4所示，P₁′，P₂′为标定点P1和标定点P2在x-y平面上的两个投影点，和分别表示两台机器人R_i，R_j末端点处于标定点P1和标定点P2时在x-y平面上的投影，O_i′，O_j′分别为两台协同机器人的基座标系原点在x-y平面的投影点，r_i-1，r_i-2分别表示机器人R_i末端点在P1和P2处位姿的RPY转角γ_i-1，γ_i-2在x-y平面的投影，r_j-1，r_j-2分别表示机器人R_j末端点在P1和P2处位姿的RPY转角γ_j-1，γ_j-2在x-y平面的投影。

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

$\left\{\begin{array}{c}\left|o_i^{\′}{p}_{i1}\right|=\left|o_i^{\′}{p}_1^{\′}\right|=\sqrt{{\left(p_{x-1}^i\right)}^2+{\left(p_{y-1}^i\right)}^2}\\ \left|o_i^{\′}{p}_{i2}\right|=\left|o_i^{\′}{p}_2^{\′}\right|=\sqrt{{\left(p_{x-2}^i\right)}^2+{\left(p_{y-2}^i\right)}^2}\end{array}\right.$

$\left\{\begin{array}{c}\left|o_j^{\′}{p}_{j1}\right|=\left|o_j^{\′}{p}_1^{\′}\right|=\sqrt{{\left(p_{x-1}^j\right)}^2+{\left(p_{y-1}^j\right)}^2}\\ \left|o_j^{\′}{p}_{j2}\right|=\left|o_j^{\′}{p}_2^{\′}\right|=\sqrt{{\left(p_{x-2}^j\right)}^2+{\left(p_{y-2}^j\right)}^2}\end{array}\right.$

其中，|o′_ip_i1|，|o′_ip′₁|表示机器人R_i的末端点在P1处与基坐标系F_i原点O_i之间的连线长度的水平投影，|o′_jp_j1|，|o′_jp′₁|表示机器人R_j的末端点在标定点P1处与基坐标系F_j原点O_j之间的连线长度的水平投影，|o′_ip_i2|，|o′_ip′₂|表示机器人R_i的末端点在标定点P2处与基坐标系F_i原点O_i之间的连线长度的水平投影，|o′_jp_j2|，|o′_jp′₂|表示机器人R_j的末端点在标定点P2处与基坐标系F_j原点O_j之间的连线长度的水平投影。

附图4所示的三角形Vo′_ip′₁p′₂中，已知第一机器人R_i在两个标定点之间的旋转角r_i＝|r_i-1-r_i-2|和第二机器人R_j在两个标定点之间的旋转角r_j＝|r_j-1-r_j-2|利用余弦定理，可计算出∠1以及|p₁′p₂′|，其中|p′₁p′₂|表示两标定点P1和P2连线在x-y水平面的投影长度，∠1表示|o′_ip_i2|与|p′₁p′₂|之间的夹角。

同理在Vo′_jp′₁p′₂中，可计算出∠2，在Vo′_iAp′₂，Vo′_jBp′₂中采用以下公式可依次计算出：

∠3＝π-(∠1+|r_i-2|)

∠4＝π-(∠2+|r_j-2|)

∠5＝∠1+∠2

其中，∠3表示机器人R_i的基坐标系F_i中x轴与|p′₁p′₂|之间的夹角，∠4表示机器人R_j的基坐标系F_j中x轴与|p′₁p′₂|之间的夹角，∠5表示|o′_ip′₂|与|o′_jp′₂|之间的夹角；

在三角形Vo′_ip′₂o′_j中，利用余弦定理可以计算出：|o′_io′_j|以及∠6，∠7＝|r_i-2|-∠6，其中|o′_io′_j|表示机器人R_i和机器人R_j各基坐标系原点连线的水平投影长度，∠6表示|o′_ip′₂|与|o′_io′_j|之间的夹角，∠7表示机器人R_i的基坐标系F_i中x轴与|o′_io′_j|之间的夹角。

因此得到旋转角θ_rotz和平移偏量(p_x，p_y)：

$\left\{\begin{array}{c}{p}_x=\left|o_i^{\′}{o}_j^{\′}\right|\·\cos \∠7\\ p_y=\left|o_i^{\′}{o}_j^{\′}\right|\·\sin \∠7\\ {\mathrm{\θ}}_{\mathrm{rotz}}=\∠3+\∠4\end{array}\right.$

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

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

p_z1＝pⁱ _z-1-p^j _z-1

p_z2＝pⁱ _z-2-p^j _z-2

$p_z=\frac{({p^i}_{z-1}-{p^j}_{z-1})+({p^i}_{z-2}-{p^j}_{z-2})}{2}$

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

(1)针对该两台机器人R_i和R_j均安装在水平地面的情况，即该两台机器人的基座标系F_i和F_j的z轴垂直于水平地面向上，两机器人基座标系F_i和F_j之间的相对位姿变换矩阵为：

$\begin{array}{c}T_{\mathrm{oi}}^{\mathrm{oj}}=\mathrm{Rot}(Z,{\mathrm{\θ}}_{\mathrm{rotz}})\mathrm{Trans}(p_x,p_y,p_z)\\ =\left[\begin{array}{cc}R_i^j& T_i^j+T^{\′}_i^j\\ 0& 1\end{array}\right]\end{array}$

其中Rot(Z，θ_rotz)表示机器人R_j的基坐标系F_j相对于机器人R_i的基坐标系F_i绕z轴旋转的矩阵，表示表示机器人R_j的基坐标系F_j相对于机器人R_i的基坐标系F_i的平移矩阵，表示机器人R_j的基坐标系F_j相对于机器人R_i的基坐标系F_i的变换矩阵；

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

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

$T^{\′}_{\mathrm{oi}}^{\mathrm{oj}}=\left[\begin{array}{cc}{R}^{\′}_i^j& T_i^j+T^{\′}_i^j\\ 0& 1\end{array}\right]$

其中表示机器人R_j的基坐标系F_j相对于机器人R_i的基坐标系F_i的旋转的矩阵，表示表示机器人R_j的基坐标系F_j相对于机器人R_i的基坐标系F_i的平移矩阵，表示机器人R_j的基坐标系F_j相对于机器人R_i的基坐标系F_i的变换矩阵；

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

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

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

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

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

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

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

附图说明

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

图2是本发明中任意两机器人均是基于水平地面安装的协同方式示意图，即该两台机器人R_i和R_j的基座标系F_i和F_j的z轴垂直于水平地面向上。

图3是本发明中任意两机器人的协同方式示意图，其中一台是基于水平地面安装，另外一台是基于顶面安装，即该机器人R_i的基座标系F_i中的z轴垂直于水平地面向下，机器人R_j的基座标系F_j中的z轴垂直于水平地面向上。

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

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

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

图7为本发明的具有3台机器人的协同系统的安装示意图，描述了各机器人基座标系之间的相互变换关系，图中1，2，3分别表示机器人R_i，R_jR_k，表示机器人R_k的基座标系相对于机器人R_i的基座标系之间的相对变换矩阵，表示机器人R_j的基座标系相对于机器人R_k的基座标系之间的变换矩阵，表示机器人R_j的基座标系相对于机器人R_i的基座标系之间的变换矩阵。

具体实施方式

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

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

本发明的实施例：

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

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

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

在第一个标定点P1处，获得机器人R_i，R_j的各关节转角信息：

ⁱR₁(ⁱθ₁，ⁱθ₂，ⁱθ₃，ⁱθ₄，ⁱθ₅，ⁱθ₆)＝[-0.0429，-1.4477，0，0，1.1200，0]

^jR₁(^jθ₁，^jθ₂，^jθ₃，^jθ₄，^jθ₅，^jθ₆)＝[0.0429，-1.4488，0，0，1.1209，0]

在第二个标定点P2处，获得机器人R_i，R_j的各关节转角信息：

ⁱR₂(ⁱθ₁，ⁱθ₂，ⁱθ₃，ⁱθ₄，ⁱθ₅，ⁱθ₆)＝[-1.2868，-1.5746，0，0，1.5746，0]

^jR₂(^jθ₁，^jθ₂，^jθ₃，^jθ₄，^jθ₅，^jθ₆)＝[1.2823，-1.5691，0，0，1.5691，0]

第三步：分别建立两台机器人的D-H坐标系，利用握手动作测定的关节转角信息计算出各机器人的末端姿态矩阵：

a、机器人R_i的末端点在第一个标定点P1处：

机器人R_i的末端点姿态矩阵：

$T_{i-1}^0{}_6=\left[\begin{array}{cccc}0.9459& -0.0429& 0.3216& 0.9565\\ -0.0406& -0.9991& -0.0138& -0.0411\\ 0.3219& 0& -0.9468& 0.8526\\ 0& 0& 0& 1\end{array}\right]$

机器人R_i的末端点RPY转角γ角度值为：r_i-1＝0.0453；机器人R_i的末端点在基坐标系F_i中的坐标值P_i-1(pⁱ _x-1，pⁱ _y-1，pⁱ _z-1)为：P_i-1(0.9565，-0.0411，0.8526)；

b、机器人R_j的末端点在第一个标定点P1处：

机器人R_j的末端点姿态矩阵：

$T_{i-1}^0{}_6=\left[\begin{array}{cccc}0.9458& 0.0430& 0.3217& 0.9558\\ 0.0406& -0.9990& 0.0138& 0.0410\\ 0.3221& 0& -0.9467& 0.8535\\ 0& 0& 0& 1\end{array}\right]$

机器人R_j的末端点RPY转角γ角度值为：r_j-1＝-0.0454；机器人R_j的末端点在基坐标系F_j中的坐标值P_j-1(p^j _x-1，p^j _y-1，p^j _z-1)为：P_j-1(0.9558，0.0410，0.8535)；

c、机器人R_i的末端点在第二个标定点P2处：

机器人R_i的末端点姿态矩阵：

$T_{i-2}^0{}_6=\left[\begin{array}{cccc}0.2802& -0.9599& 0& 0.2248\\ -0.9599& -0.2802& 0& -0.7702\\ 0& 0& -1& 0.9280\\ 0& 0& 0& 1\end{array}\right]$

机器人R_i的末端点RPY转角γ角度值为：r_i-2＝1.2868；机器人R_i的末端点在基坐标系F_i中的坐标值P_i-2(pⁱ _x-2，pⁱ _y-2，pⁱ _z-2)为：P_i-2(0.2248，-0.7702，0.9280)；

d、机器人R_j的末端点在第二个标定点P2处：

机器人R_j的末端点姿态矩阵：

$T_{j-2}^0{}_6=\left[\begin{array}{cccc}0.2845& 0.9587& 0& 0.2294\\ 0.9587& -0.2845& 0& 0.7729\\ 0& 0& -1& 0.9244\\ 0& 0& 0& 1\end{array}\right]$

机器人R_j的末端点RPY转角γ角度值为：r_j-2＝-1.2823；机器人R_j的末端点在基坐标系F_j中的坐标值P_j-2(p^j _x-2，p^j _y-2，p^j _z-2)为：P_j-2(0.2294，0.7729，0.9244)；

第四步：将协同作业机器人系统以及两标定点进行x-y平面投影，如附图4所示，P₁′，P₂′为空间中不重合的两标定点P1，P2在x-y平面上的投影，p_i1，p_i2，p_j1，p_j2分别表示机器人R_i，R_j的末端点在P1，P2处在x-y平面上的投影，O_i′，O_j′为两协同机器人的基座标系原点在x-y平面的投影点，r_i-1，r_i-2表示机器人R_i末端点在P₁′，P₂′处的RPY角γ_i-1，γ_i-2在x-y平面的投影，r_j-1，r_j-2表示机器人R_j末端点在P₁′，P₂′处位姿的RPY角γ_j-1，γ_j-2在x-y平面的投影。

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

$\left\{\begin{array}{c}\left|o_i^{\′}{p}_{i1}\right|=\left|o_i^{\′}{p}_1^{\′}\right|=\sqrt{{\left(p_{x-1}^i\right)}^2+{\left(p_{y-1}^i\right)}^2}\\ \left|o_i^{\′}{p}_{i2}\right|=\left|o_i^{\′}{p}_2^{\′}\right|=\sqrt{{\left(p_{x-2}^i\right)}^2+{\left(p_{y-2}^i\right)}^2}\end{array}\right.$

代入数据求解得：

|o′_ip_i1|＝|o′_ip′₁|＝0.9573

|o′_ip_i2|＝|o′_ip′₂|＝0.8023

$\left\{\begin{array}{c}\left|o_j^{\′}{p}_{j1}\right|=\left|o_j^{\′}{p}_1^{\′}\right|=\sqrt{{\left(p_{x-1}^j\right)}^2+{\left(p_{y-1}^j\right)}^2}\\ \left|o_j^{\′}{p}_{j2}\right|=\left|o_j^{\′}{p}_2^{\′}\right|=\sqrt{{\left(p_{x-2}^j\right)}^2+{\left(p_{y-2}^j\right)}^2}\end{array}\right.$

代入数据求解得：

|o′_jp_j1|＝|o′_jp′₁|＝0.9567

|o′_jp_j2|＝|o′_jp′₂|＝0.8062

在三角形Vo′_ip′₁p′₂中，角度r_i＝|r_i-1-r_i-2|＝1.2415，角度r_j＝|r_j-1-r_j-2|＝1.2370，利用余弦定理，可计算出：角度∠1＝1.0726，p′₁p′₂|＝1.0303；

同理在Vo′_jp′₁p′₂中，可计算出：角度∠2＝1.0717，在Vo′_iAp′₂，Vo′_jBp′₂中可依次计算出：

角度∠3＝π-(∠1+|r_i-2|)＝0.7821

角度∠4＝π-(∠2+|r_j-2|)＝0.7876

角度∠5＝∠1+∠2＝2.1444

在三角形Vo′_ip′₂o′_j中，利用余弦定理可计算出：|o′_io′_j|＝1.4126，角度∠6＝0.4999，角度∠7＝|r_i-2|-∠6＝0.7868，因此：

$\left\{\begin{array}{c}{p}_x={\left|o_i^{\′}{o}_j^{\′}\right|}_{d_{\mathrm{px}}}=\left|o_i^{\′}{o}_j^{\′}\right|\·\cos \∠7\\ p_y={\left|o_i^{\′}{o}_j^{\′}\right|}_{d_{\mathrm{py}}}=\left|o_i^{\′}{o}_j^{\′}\right|\·\sin \∠7\\ {\mathrm{\θ}}_{\mathrm{rotz}}=\∠3+\∠4\end{array}\right.$

$\left\{\begin{array}{c}{p}_x=0.9974\\ p_y=1.0004\\ {\mathrm{\θ}}_{\mathrm{rotz}}=1.5697\end{array}\right.$

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

$R_i^j=\left[\begin{array}{ccc}0.0011& -0.9999& 0\\ 0.9999& 0.0011& 0\\ 0& 0& 1\end{array}\right]$

相对水平平移矩阵：

$T_i^j={\left[\begin{array}{ccc}0.9974& 1.0004& 0\end{array}\right]}^T$

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

$p_z=\frac{({p^i}_{z-1}-{p^j}_{z-1})+({p^i}_{z-2}-{p^j}_{z-2})}{2}$

代入数据计算可得：

p_z＝0.0014

根据p_z可求得协同机器人两基坐标系F_i，F_j之间相对垂直平移矩阵：

$T^{\′}_i^j={\left[\begin{array}{ccc}0& 0& 0.0014\end{array}\right]}^T$

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

$T_{\mathrm{oi}}^{\mathrm{oj}}=\left[\begin{array}{cc}R_i^j& T_i^j+T^{\′}_i^j\\ 0& 1\end{array}\right]$

$T_{\mathrm{oi}}^{\mathrm{oj}}=\left[\begin{array}{cccc}0.0011& -0.9999& 0& 0.9974\\ 0.9999& 0.0011& 0& 1.0004\\ 0& 0& 1& 0.0014\\ 0& 0& 0& 1\end{array}\right]$

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

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

$R^{\′}_i^j=\left[\begin{array}{ccc}0.0011& 0.9999& 0\\ 0.9999& -0.0011& 0\\ 0& 0& -1\end{array}\right]$

相对水平平移矩阵：

$T_i^j={\left[\begin{array}{ccc}0.9974& 1.0004& 0\end{array}\right]}^T$

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

$p_z=\frac{({p^i}_{z-1}+{p^j}_{z-1})+({p^i}_{z-2}+{p^j}_{z-2})}{2}$

代入数据计算可得：

p_z＝1.7793

根据p_z可以求得协同机器人两基坐标系F_i，F_j之间相对垂直平移矩阵：

$T^{\′}_i^j={\left[\begin{array}{ccc}0& 0& 1.7793\end{array}\right]}^T$

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

$T^{\′}_{\mathrm{oi}}^{\mathrm{oj}}=\left[\begin{array}{cc}{R}^{\′}_i^j& T_i^j+T^{\′}_i^j\\ 0& 1\end{array}\right]$

$T^{\′}_{\mathrm{oi}}^{\mathrm{oj}}=\left[\begin{array}{cccc}0.0011& 0.9999& 0& 0.9974\\ 0.9999& -0.0011& 0& 1.0004\\ 0& 0& -1& 1.7793\\ 0& 0& 0& 1\end{array}\right]$

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

在第一个标定点P1′处，获得机器人R_i，R_k的各关节转角信息：

ⁱR₁(ⁱθ₁，ⁱθ₂，ⁱθ₃，ⁱθ₄，ⁱθ₅，ⁱθ₆)＝[-1.5708，-1.5708，0，0，0.5027，0]

^kR₁(^kθ₁，^kθ₂，^kθ₃，^kθ₄，^kθ₅，^kθ₆)＝[0.79，-1.568，0，0，0.5027，0]

在第二个标定点P2′处，获得机器人R_i，R_k的各关节转角信息：

ⁱR₂(ⁱθ₁，ⁱθ₂，ⁱθ₃，ⁱθ₄，ⁱθ₅，ⁱθ₆)＝[-2.7080，-1.946，0.1257，0，0.5030，0]

^kR₂(^jθ₁，^jθ₂，^jθ₃，^jθ₄，^jθ₅，^jθ₆)＝[1.924，-1.945，0.1257，0，0.5060，0]

第三步：分别建立两台机器人R_i，R_k的D-H坐标系，利用握手动作测定的关节转角信息计算出各机器人R_i，R_k的所有标定信息参数：

$\left\{\begin{array}{c}{p}_x=-0.3886\\ p_y=1.4088\\ p_z=0.0018\\ {\mathrm{\θ}}_{\mathrm{rotz}}=1.0525\end{array}\right.$

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

$R_i^k=\left[\begin{array}{ccc}0.4954& -0.8686& 0\\ 0.8686& 0.4954& 0\\ 0& 0& 1\end{array}\right]$

相对水平平移矩阵：

$T_i^k={\left[\begin{array}{ccc}-0.3886& 1.4088& 0\end{array}\right]}^T$

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

p_z＝0.0018

根据p_z可求得协同机器人两基坐标系F_i，F_j之间相对垂直平移矩阵：

$T^{\′}_i^k={\left[\begin{array}{ccc}0& 0& 0.0018\end{array}\right]}^T$

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

$T_{\mathrm{oi}}^{\mathrm{ok}}=\left[\begin{array}{cc}R_i^j& T_i^j+T^{\′}_i^j\\ 0& 1\end{array}\right]$

$T_{\mathrm{oi}}^{\mathrm{ok}}=\left[\begin{array}{cccc}0.4954& -0.8686& 0& -0.3886\\ 0.8686& 0.4954& 0& 1.4088\\ 0& 0& 1& 0.0018\\ 0& 0& 0& 1\end{array}\right]$

完成了针对该两台机器人R_i和R_k均水平安装在地面的情况下基座标系之间的标定工作。

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

$R^{\′}_i^k=\left[\begin{array}{ccc}0.5027& 0.8644& 0\\ 0.8644& -0.5027& 0\\ 0& 0& -1\end{array}\right]$

相对水平平移矩阵：

$T_i^k={\left[\begin{array}{ccc}-0.3886& 1.4088& 0\end{array}\right]}^T$

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

p_z＝2.2445

根据p_z可以求得协同机器人两基坐标系F_i，F_j之间相对垂直平移矩阵：

$T^{\′}_i^j={\left[\begin{array}{ccc}0& 0& 2.2445\end{array}\right]}^T$

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

$T^{\′}_{\mathrm{oi}}^{\mathrm{ok}}=\left[\begin{array}{cc}{R}^{\′}_i^k& T_i^k+T^{\′}_i^k\\ 0& 1\end{array}\right]$

$T^{\′}_{\mathrm{oi}}^{\mathrm{ok}}=\left[\begin{array}{cccc}0.5027& 0.8644& 0& -0.3886\\ 0.8644& -0.5027& 0& 1.4088\\ 0& 0& -1& 2.2445\\ 0& 0& 0& 1\end{array}\right]$

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

$F_j=T_{\mathrm{oi}}^{\mathrm{oj}}{F}_i$

$F_k=T_{\mathrm{oi}}^{\mathrm{ok}}{F}_i$

其中表示机器人R_j基座标系F_j相对于机器人R_i基座标系F_i的变换矩阵，表示机器人R_k基座标系F_k相对于机器人R_i基座标系F_i的变换矩阵；

$T^{-1}_{\mathrm{oi}}^{\mathrm{oj}}{F}_j=F_i$

$T^{-1}_{\mathrm{oi}}^{\mathrm{ok}}{F}_k=F_i$

$T^{-1}_{\mathrm{oi}}^{\mathrm{oj}}{F}_j=T^{-1}_{\mathrm{oi}}^{\mathrm{ok}}{F}_k$

$F_j=T_{\mathrm{oi}}^{\mathrm{oj}}{T}^{-1}_{\mathrm{oi}}^{\mathrm{ok}}{F}_k$

其中表示的逆矩阵，表示的逆矩阵，根据上述计算可以知道机器人R_k的基座标系F_k相对于机器人R_j基座标系F_j的变换矩阵为：

$T_{\mathrm{oj}}^{\mathrm{ok}}=T_{\mathrm{oi}}^{\mathrm{oj}}{T}^{-1}_{\mathrm{oi}}^{\mathrm{oj}}$

代入数据得到：

$T_{\mathrm{oj}}^{\mathrm{ok}}=\left[\begin{array}{cccc}0.8692& -0.4945& 0& 2.0317\\ 0.4945& 0.8692& 0& -0.0319\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

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

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

Claims (5)

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

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

第二步：在最小标定单位的两台机器人R_i，R_j中，建立两台机器人各自的基座标系F_i，F_j，在两台机器人各自基座标系原点O_i，O_j连线的两侧各任意确定一个标定点，作为公共工作空间中的两个标定点P1和P2，

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

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

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

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

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

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

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

4.根据权利要求2所述的一种基于空间两点投影的协同机器人基座标系标定方法，其特征在于：所述相对水平平移矩阵和相对水平旋转矩阵具体采用以下方式得到：

建立XYZ三维坐标系，z轴方向竖直向上，将该两台机器人以及标定点P1和标定点P2向水平的X-Y平面投影，利用X-Y平面投影的几何约束关系以及机器人末端点RPY角中的绕z轴旋转角γ，计算出其中一机器人R_j相对于另一机器人R_i的基座标系在X-Y平面上的x方向偏移量p_x和y方向偏移量p_y以及相对于绕z轴的旋转量θ_rotz，由x方向偏移量p_x和y方向偏移量p_y组合得到相对水平平移矩阵由旋转量θ_rotz得到相对水平旋转矩阵。

5.根据权利要求2所述的一种基于空间两点投影的协同机器人基座标系标定方法，其特征在于：所述相对垂直平移矩阵具体采用以下方式得到：

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