一种机器人6D位姿估计系统及方法与流程

一种机器人6d位姿估计系统及方法
技术领域
1.本公开属于机器人姿态估计领域，特别涉及一种机器人6d位姿估计系统及方法。


背景技术：

2.提高机器人的运动精度是机器人执行精密操作的基础。如图1所示，机器人在执行重载操作时，机器人本体和末端执行器易产生机械结构变形，从而引入机器人运动学的非几何误差，因此必须对机器人运动的位置和姿态进行补偿。
3.目前，提高机器人运动精度的方式有两种。
4.一种是利用机器人定位精度的离线测量数据对机器人的运动学进行标定和补偿。具体实现方法是在机器人运动学基础上基于非几何误差的力学、热学等产生机理建立误差传递模型，但该方法的缺点是运动补偿的精度取决于模型的准确性，精度往往难以保证。
5.此外还可通过数据逼近的方式建立运动误差的数据模型，利用空间网格、多项式拟合、神经网络和高斯过程回归等方法直接建立机器人构形空间与机器人运动学误差的映射关系，并取得了较好的实验结果。但该方法的缺点是建模补偿方法用于在线运动补偿时受采集的样本数据的分布和数量的制约。
6.第二种是借助于外部测量设备为机器人运动提供在线全闭环反馈。具体实现方法是利用立体视觉系统获取三维位姿信息，实时性好，但该方法的缺点是受相机视野、分辨率等限制，一般取得的位置和姿态精度相对于较低。
7.此外还可以通过激光跟踪仪进行6d位姿测量，可对由多个反射靶球构建测量工具进行多目标测量，利用机器人末端固定，凭借高精度的位置测量，可以获得较高的姿态反馈精度。但该方法的缺点是受限于一台激光跟踪仪同一时间只能测量一个目标点，该方法只能进行离线测量，此外，通过3台以上的激光跟踪仪构建测量网络或者使用昂贵的6d位姿测量的t-mac(tracker-machine，跟踪仪－机械控制传感器)配件来实现，成本过于高昂。


技术实现要素：

8.本公开的目的是为克服已有技术的不足之处，提供一种机器人6d位姿估计装置及方法。本公开易于实现，可提高大负载、易变形的机器人装配操作过程的精度。
9.本公开第一方面实施例提出一种机器人6d位姿估计系统，包括：机器人、激光跟踪仪、计算机和t字形测量装置；其中，所述t字形测量装置包括一个t字形框架和分别位于所述t字形框架每个末端的一个靶座，每个所述靶座上设置一个靶球；所述t字形测量装置固定在所述机器人的末端，将所述靶球作为采集所述机器人末端位姿的目标点，其中一个靶球作为实测目标点，其余两个靶球作为预测目标点；所述激光跟踪仪放置在所述机器人前方，作为实测目标点的所述靶球位于所述激光跟踪仪的视野范围之内。
10.在本公开的一个具体实施例中，
11.所述机器人用于携带工件进行运动；
12.所述t字形测量装置用于使得所述靶球跟随所述机器人进行运动；
13.所述激光跟踪仪用于在所述机器人运动过程中采集作为实测目标点的所述靶球的位置信息；
14.所述计算机，用于根据预设的运动学误差模型计算作为预测目标点的所述靶球的位置信息，结合作为实测目标点的所述靶球的位置信息，以获得所述机器人6d位姿估计结果。
15.在本公开的一个具体实施例中，所述t字形测量装置正面左上方的靶球为实测目标点，其余两个靶球为预测目标点。
16.在本公开的一个具体实施例中，所述靶座和所述靶球通过磁性吸引连接。
17.本公开第二方面实施例提出一种基于上述系统的机器人6d位姿估计方法，包括：
18.1)标定所述机器人6d位姿估计系统中各坐标系以及坐标系间的变换矩阵，包括：机器人基础坐标系，激光跟踪仪坐标系，t字形测量装置坐标系，机器人末端坐标系，机器人基础坐标系与激光跟踪仪坐标系的变换矩阵
l
tb，机器人末端坐标系与t字形测量装置坐标系的变换矩阵
19.2)将测试工件装载到机器人末端执行器上，此时的机器人位姿记为初始位姿；将t字形测量装置上对应实测目标点的靶球作为1号靶球，将对应预测目标点的靶球分别记为2号和3号靶球，分别获取1、2、3号靶球在机器人末端坐标系下的位置ep1、ep2和ep3；
20.3)令机器人末端执行器携带测试工件运动到任一关节位形qi下，分别测量2号靶球和3号靶球的预测目标点在激光跟踪仪坐标系下对应关节位形qi的实测位置和依据预设的机器人运动学模型，计算在关节位形qi下每个预测目标点分别对应的理论位置和得到关节位形qi下该两个预测目标点分别对应的定位误差，表达式如下：
[0021][0022]
式中，i＝1,2,3,
…
表示关节位形序号；
[0023][0024]
fkine(qi)表示机器人从关节位形qi到末端位姿的机器人正运动学模型；
[0025]
4)重复步骤3)，按照设定的频率采样多个关节位形及每个关节位形下两个预测目标点分别对应的定位误差，以分别获取对应2号靶球预测目标点的训练数据集和对应3号靶球预测目标点的训练数据集；
[0026]
其中，每个预测目标点的训练数据集中的每个训练样本包含任一关节位形和该关节位形下所述预测目标点的定位误差；
[0027]
5)对两个预测目标点分别建立对应的机器人运动学误差模型，利用步骤4)获取的训练数据集训练所述机器人运动学误差模型，得到训练完毕的所述机器人运动学误差模型；
[0028]
6)利用训练完毕的所述机器人运动学误差模型进行预测；包括：
[0029]
令机器人末端执行器携带实际工件进行运动，利用激光跟踪仪测量1号靶球对应的实测目标点在任意t时刻的位置记为将t时刻的机器人关节位形记为q
t
；
[0030]
将q
t
分别输入步骤5)训练完毕的两个机器人运动学误差模型，分别得到在关节位
形q
t
下两个预测目标点的定位误差的预测值和
[0031]
计算在关节位形q
t
下所述两个预测目标点的预测位置为：
[0032][0033]
其中，ω2(q)为2号靶球对应预测目标点的机器人运动学误差模型，ω3(q)为3号靶球对应预测目标点的机器人运动学误差模型，q代表关节位形；
[0034]
7)计算机器人6d位姿；
[0035]
根据和按照空间目标位姿计算方法，计算t字形测量装置坐标系在激光跟踪仪下的6d位姿通过坐标变换得到机器人末端坐标系中心的6d位姿
[0036]
在本公开的一个具体实施例中，所述机器人运动学模型为六自由度机器人运动学模型。
[0037]
在本公开的一个具体实施例中，所述机器人运动学误差模型为基于高斯过程回归的机器人运动学误差模型。
[0038]
在本公开的一个具体实施例中，所述实际工件与所述测试工件的重量差值不超过设定的重量阈值。
[0039]
在本公开的一个具体实施例中，所述利用训练完毕的所述机器人运动学误差模型进行预测，还包括：基于平差模型计算平差值，根据所述平差值修正所述实测目标点的位置和所述两个预测目标点的预测位置。
[0040]
在本公开的一个具体实施例中，所述基于高斯过程回归的机器人运动学误差模型，包括：
[0041]
1)令由结构变形引入的机器人末端位置误差e
p
服从高斯分布，则位置误差的测量值包括一个关于机器人关节位形q的高斯过程和一个独立分布的高斯白噪声叠，表达式如下：
[0042][0043]
式中，高斯过程ω(q)是任意有限个具有联合高斯分布的随机变量组成的集合，由均值函数m(q)和协方差函数k(q,q')确定；则第i个关节的噪声εi服从高斯分布服从高斯分布表示n次测量的综合误差；
[0044]
2)记机器人末端位置误差任一维分量的测量序列为e＝[e1,e2,
…
,em]
t
，其中m＝1,2,
…
为测量序号，将机器人的关节位形序列q＝[q1,q1,
…
,qm]
t
作为输入样本，则输出样本的概率分布为：
[0045][0046]
式中，k(q,q)为q的协方差矩阵，i为单位矩阵；
[0047]
对于任一新的关节位形q
*
，q
*
的位置误差为e
*
，则e和e
*
的联合概率分布为：
[0048]
[0049]
3)依据多维高斯分布的性质，则新输出的位置误差e
*
服从后验分布：
[0050][0051]
式中，的均值和协方差分别为：
[0052][0053][0054]
4)最大似然函数表示为：
[0055][0056]
本公开的特点及有益效果在于：
[0057]
1、本公开的一种机器人6d位姿估计系统中所采用的t字形装置可以固结有三个位置不共线的反射靶球作为测量目标点，由三个目标点可确定机器人末端的6d位置和姿态，结构简单灵巧方便。
[0058]
2、本公开中的6d位姿估计方法中建立了高斯过程回归的机器人运动学误差模型，能够精确地补偿结构变形引入的非几何误差。
[0059]
3、本公开中的6d位姿估计方法，利用运动学误差模型预测非在线测量的目标点的空间位置，能够精确地描述机器人空间的末端位置误差分布，该方法能够有效地预测机器人在不同负载下的运动误差。
[0060]
4、本公开中的6d位姿估计方法用平差模型对测量目标点的实测值和预测值进行修正，可提高6d位姿的估计精度。
附图说明
[0061]
图1为重载下机器人末端的结构变形示意图。
[0062]
图2为本公开实施例中的一种机器人6d位姿估计系统的结构示意图。
[0063]
图3为本公开实施例中t字形测量装置连接机器人末端的结构示意图。
[0064]
图4为本公开实施例中机器人末端携带工件时的t字形测量装置连接机器人末端的位置示意图。
[0065]
图5为本公开实施例中t字形装置俯视示意图。
[0066]
图6为本公开实施例中一种机器人6d位姿估计方法的整体流程图。
具体实施方式
[0067]
本公开提出一种机器人6d位姿估计系统及方法，下面结合附图和具体实施例进一步详细说明如下。
[0068]
本公开第一方面实施例提出一种机器人6d位姿估计系统，整体结构如图2所示，包括：机器人、t字形测量装置、激光跟踪仪和计算机(其中计算机在图2中未显示)。其中，所述t字形测量装置包括：一个t字形框架和分别位于所述框架三个末端的磁性靶座，其中每个末端设置一个磁性靶座，每个磁性靶座上设置有一个靶球，所述靶球与所述靶座通过磁性吸引连接。
[0069]
所述机器人用于携带位于末端的工件进行运动；
[0070]
所述t字形测量装置通过连接件固定在所述机器人的末端上，使得三个靶球跟随机器人进行运动；将三个靶球分别作为采集机器人末端位姿的三个目标点，其中位于t字形测量装置正面左上方的靶球为实测目标点，其余两个靶球为预测目标点。其中，本公开一个具体实施例中t字形测量装置与机器人末端的连接示意图如图3所示。图3中，t字形框架通过一个连接机器人末端的机械结构可固定在机器人的末端。
[0071]
所述激光跟踪仪位于机器人的前方，用于在机器人运动过程中采集作为实测目标点的靶球的位置信息。
[0072]
所述计算机，用于根据预设的运动学误差模型计算作为预测目标点的靶球的位置信息，再结合作为实测目标点的靶球信息，以获得所述机器人6d位姿估计结果。
[0073]
进一步地，图4为本公开实施例中t字形测量装置与工件位置关系示意图。如图4所示，机器人在执行移动工件的任务时，所述工件固定于机器人执行器末端，所述t字形测量装置的上方连接结构固定在机器人末端和工件上表面之间，应确保所述t字形测量装置中对应实测目标点的靶球在激光跟踪仪的视野范围中。
[0074]
进一步地，图5为本公开实施中所述t字形装置的俯视图，如图5所示，靶球序号排列规则是从左到右从上到下依次记为1、2、3号靶球，其中1号靶球对应实测目标点，2号和3号靶球对应预测目标点。
[0075]
本公开一个具体实施例中，所述t字形测量装置由铝材质本体的t字形框架、连接机器人末端的机械结构和靶球靶座组成。框架的尺寸选取应满足：以t字形的交汇点为中心，中心距每个端点的长度相差不能超过2倍。每个靶座上固定有一个直径为1.5"反射靶球，每个反射靶球与对应靶座连接。三个靶座分布在t字型测量装置的三个末端，其相对位置尺寸可取工件尺寸的十分之一，但可选范围下限为100mm，上限为500mm。t字形框架与机器人末端执行器通过螺丝固定的机械结构连接，该机械结构的尺寸可根据工件和机器人末端尺寸进行设计，符合下面条件：使得靶座外表面分布在工件表面；连接孔的尺寸和位置与机器人末端匹配。
[0076]
本公开对机器人及激光跟踪仪型号和参数无要求，本公开一个具体实施例中机器人型号为abb irb 4600机器人，激光跟踪仪型号为leicaat901-b。
[0077]
本公开第二方面实施例提出一种基于上述系统的机器人6d位姿估计方法，该方法整体流程如图6所示，包括以下步骤：
[0078]
1)标定所述机器人6d位姿估计系统中各坐标系以及坐标系间的变换矩阵，包括：机器人基础坐标系{b}(如图2所示，其中，ob为机器人基础坐标系的原点，xb,yb和zb分别代表机器人基础坐标系xyz轴)，激光跟踪仪坐标系{l}(如图2所示，其中，o
l
为激光跟踪仪坐标系的原点，x
l
,y
l
和z
l
分别代表激光跟踪仪坐标系xyz轴)，t字形测量装置坐标系{t}(如图2所示，其中，定义t字形框架的交叉点为该坐标系的原点o
t
，x
t
,y
t
和z
t
分别代表t字形测量装置坐标系xyz轴)，机器人末端坐标系{e}(如图2所示，其中，oe为机器人末端坐标系的原点，xe,ye和ze分别代表机器人末端坐标系xyz轴)，机器人基础坐标系与激光跟踪仪坐标系的变换矩阵
l
tb，机器人末端坐标系与t字形测量装置坐标系的变换矩阵
[0079]
2)将任一工件固定于机器人末端，将装载时的机器人位姿记为初始位姿。将t字形测量装置上的实测目标点的靶球作为1号靶球，将对应预测目标点的靶球记为2号和3号靶球，分别获取1、2、3号靶球在机器人末端坐标系下的位置ep1、ep2和ep3。然后如图4所示将t字
形测量装置通过机器人末端连接机械结构固定在机器人末端上，使得三个靶座分布在该工件外表面，固定方法是使用螺丝进行机械连接。
[0080]
需要说明的是，所述工件是机器人在操作时的常规负载，本公开对所述工件无特殊要求要求；在本公开一个具体实施例中选用50kg的铝材质长方体作为工件。
[0081]
3)按照设定的频率进行离线测量，获取训练数据集；具体方法如下：
[0082]
令机器人携带工件运动到不同关节位形qi下，离线测量机器人末端的2号和3号靶球预测目标点在激光跟踪仪坐标系下对应该关节位形的实测位置和并依据预设的该机器人运动学模型(本具体实施例中即六自由度机器人运动学模型)，计算每个预测目标点对应的理论位置和可计算得到该关节位形下两个预测目标点分别对应的定位误差，表达式如下：
[0083][0084]
式中，i＝1,2,3,
…
表示关节位形序号，表示关节位形序号，表示关节位形序号，fkine(qi)表示机器人从关节位形qi到末端位姿的机器人正运动学模型。
[0085]
4)重复步骤3)，按照一定的样本采样频率fs采样多个关节位形及每个关节位形下两个预测目标点分别对应的定位误差，以分别获取对应2号靶球预测目标点的训练数据集和对应3号靶球预测目标点的训练数据集；假设当前的机器人运动速度是vs，则时间间隔，则时间间隔时间间隔越小，后续处理精度越高。在本公开的一个具体实施例中样本采样频率采用δfs＝30hz，采集时间持续10分钟。
[0086]
采样完成后，分别获取对应2号靶球预测目标点的训练数据集和对应3号靶球预测目标点的训练数据集；
[0087]
其中，每个预测目标点的训练数据集中的每个训练样本包含任一关节位形和该关节位形下对应预测目标点的定位误差。
[0088]
5)对两个预测目标点分别建立对应的机器人运动学误差模型，利用步骤4)获取的训练数据集训练机器人运动学误差模型；
[0089]
本实施例中，分别建立两个预测目标点分别对应的基于高斯过程回归的机器人运动学误差模型ω2(q)和ω3(q)，其中，q代表关节位形；每个机器人运动学误差模型以机器人的各关节位形作为输入，该关节位形下对应预测目标点的定位误差为输出，并依据最大似然函数对每个机器人运动学误差模型的参数进行训练，得到训练完毕的两个机器人运动学误差模型。
[0090]
6)在线测量和模型预测；
[0091]
机器人在实际携带工件的运动过程中(其中机器人在实际运动时，负载工件的重量与训练时的工件重量差值不能超过设定的阈值，本公开实施例中该差值不能超过10kg)，利用激光跟踪仪在线测量1号靶球对应的实测目标点的在任意t时刻的空间位置记为将t时刻的机器人关节位形记为q
t
；
[0092]
将q
t
分别输入步骤5)训练完毕的两个机器人运动学误差模型，可得到两个模型分别输出的两个预测目标点对应该关节位形的定位误差的预测值和
[0093]
则在q
t
下该两个预测目标点的预测位置可表示为：
[0094][0095]
7)计算6d位姿；
[0096]
由当前时间点的关节位形下由激光跟踪仪实测的目标点位置和两组预测的目标点位置和组成测量时间点t完整的三组目标点的空间位置序列。结合三组目标点的位置序列和测量工具的几何尺寸，按照传统的空间目标位姿计算方法，可计算t字形测量装置坐标系在激光跟踪仪下的6d位姿通过坐标变换得到机器人末端坐标系中心的6d位姿6d位姿即包括3个自由度的平移，以及3个自由度的旋转，其中为机器人末端坐标系与t字形测量装置坐标系的变换矩阵。
[0097]
进一步地，本公开一个具体实施例中所述运动学误差模型使用了一种基于高斯过程回归的运动学误差模型，具体包括：
[0098]
1)假设由结构变形引入的机器人末端位置误差e
p
服从高斯分布，则位置误差的测量值可由一个关于机器人关节位形q的高斯过程和一个独立分布的高斯白噪声叠加而成，即：
[0099][0100]
式中，高斯过程ω(q)是任意有限个具有联合高斯分布的随机变量组成的集合，由均值函数m(q)和协方差函数k(q,q')确定；则第i个关节的噪声εi服从高斯分布服从高斯分布表示n次测量的综合误差。
[0101]
2)记机器人末端位置误差某一维分量的测量序列为e＝[e1,e2,
…
,em]
t
，作为已知输出样本，其中m＝1,2,
…
为测量序号，相应的机器人的关节位形序列的q＝[q1,q1,
…
,qm]
t
为输入样本，则已知输出样本的概率分布为：
[0102][0103]
式中，k(q,q)为q的协方差矩阵，i为单位矩阵。给定新的关节位形q
*
，q
*
的位置误差为e
*
，则由高斯过程的定义可知，e和e
*
的联合概率分布可表示为：
[0104][0105]
3)e
*
服从后验分布：
[0106][0107]
式中，的均值和协方差分别为：
[0108][0109]
[0110]
4)最大似然函数可表示为：
[0111][0112]
进一步地，所述在线测量和模型预测步骤中，还包括：基于平差模型的位姿修正方法，具体如下：
[0113]
将实测点和预测点之间的距离约束作为冗余约束，利用平差模型对目标点的实测值和预测值进行修正。具体包含如下步骤：
[0114]
1)建立冗余约束条件，如图5所示，设任一个时刻实测目标点的位置pm坐标为(x1,y1,z1)，两个预测目标点的位置和的坐标分别为(x2,y2,z2)和(x3,y3,z3)，令则平差条件方程可简化为：
[0115]
γ＝[γ
12
,γ
13
,γ
23
]
t
＝0
ꢀꢀꢀꢀꢀꢀꢀ
(10)
[0116]
2)计算实测位置的权阵，实测目标点的位置的方差由激光跟踪仪的测量误差确定，本公开实施例中取距离1m的测量均方差为单位权均方差σ0，则t字形测量装置坐标系中1号靶球对应的实测目标点的位置的权阵可表示为：
[0117][0118]
其中，为实测目标点的位置的权阵；为预测位置的协方差矩阵。并且，
[0119][0120]
式中，和分别为激光跟踪仪x、y、z坐标的方差，分别为激光跟踪仪x、y、z坐标的方差，为激光跟踪仪预测值的均方差，gpr是gaussian process regression，高斯过程回归。
[0121]
3)计算预测位置的权阵，预测目标点的测量误差来源于激光跟踪仪的测量误差和高斯过程回归预测的误差，实测误差和预测误差相互独立，则二者叠加后的预测值的总方差可表示为：
[0122]
(σ
pre
)2＝(σm)2 (σ
gpr
)2ꢀꢀꢀꢀꢀ
(12)
[0123]
式中，σ
pre
、σm、σ
gpr
分别是预测目标点的测量误差、激光跟踪仪的测量误差、高斯过程回归预测的误差。
[0124]
预测目标点的位置的x、y和z坐标分别由不同的高斯过程回归模型预测，则预测目标点位置的协方差矩阵为对角阵，即：
[0125][0126]
预测目标点的位置的权阵可表示为：
[0127][0128]
4)距离测量的权阵，目标点之间的距离可以用激光跟踪仪或者更高精度的三坐标测量机进行测量，其中三坐标测量机的长度测量误差一般以固定误差和比例误差的形式，即：
[0129][0130]
式中，σd、a
cmm
、b
cmm
为三坐标测量机的长度测量误差、固定误差和比例误差，单位为μm。d为测量点的间距。不同的距离值的测量相互独立，故距离测量的协方差矩阵为对角阵，即：
[0131][0132]
距离测量的权阵可表示为：
[0133][0134]
5)确定由三部分独立测量的权阵共同组成的最终全阵，即：
[0135][0136]
6)解算平差模型，条件方程是测量平差的数学模型，权阵是测量平差的随机模型，改正数则是平差解算的未知量，记为δm，则t字形测量工具的目标点的平差值la可表示为：
[0137]
la＝lm δmꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0138]
其中测量工具坐标系的测量值lm由3个1号点实测坐标x1,y1,z1、6个2号点和3号点预测坐标x2,y2,z2,x3,y3,z3以及3个实测距离d
12
,d
13
,d
23
组成，d
ij
为i点和j点的间距。
[0139]
lm＝[x1,y1,z1,x2,y2,z2,x3,y3,z3,d
12
,d
13
,d
23
]
t
。
[0140]
由于距离约束的条件方程为非线性方程，可通过泰勒展开进行线性化处理，得到改正数的线性条件方程：
[0141]cm
δm em＝0
ꢀꢀꢀꢀꢀꢀꢀ
(20)
[0142]
式中，为3
×
12的系数。其中γ＝[γ
12
γ
13
γ
23
]
t
，γ
ij
＝(x
i-xj)2 (y
i-yj)2 (z
i-zj)
2-d
ij2
。em为测量工具坐标系的闭合差，可表示为em＝γ(lm)。矩阵改正数的选取一般遵循最小二乘准则，即要求最小，故测量平差的解算问题实际上是一个多目标最优化问题：
[0143][0144]
拉格朗日乘数法求解上述优化问题，引入乘数λ，则可构造综合目标函数：
[0145][0146]
函数对δm求一阶导数等于零，有：
[0147][0148]
代入式(20)，有
[0149][0150]
式(24)的方程有唯一解：
[0151][0152]
将式(25)代入式(23)得到改正数δm，代入式(19)得到最终的平差值la。至此，测量平差模型解算完毕，利用平差值la修正后的t字形测量装置上3个目标点的位置，即可求得更高精度的6d位姿。
[0153]
本公开未尽事宜为公知技术。
[0154]
上述实施例只为说明本公开的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本公开的内容并据以实施，并不能以此限制本公开的保护范围。凡根据本公开精神实质所作的等效变化或修饰，都应涵盖在本公开的保护范围之内。