用于估计系统状态的方法和装置与流程

1.本技术涉及对象跟踪领域，具体地说，本技术涉及具有多个相机的对象跟踪系统中的系统状态估计。


背景技术：

2.在对象跟踪系统中，通常使用眼在手上(eye-in-hand)相机或眼在手外(eye-to-hand)相机进行对象跟踪。在眼在手上和眼在手外被组合进行协作的情况下，只能跟踪对象，因此只能测量对象位姿。可能需要通过使用眼在手上相机和眼在手外相机二者来跟踪机器人基座位置，以获得更准确的测量结果。
3.此外，对于具有六自由度(6-dof)的操纵器，静态的或是移动的，都可以从眼在手上相机位姿推断工具中心点(tcp)位姿。但是，对于具有多于6dof的冗余移动操纵器，可能存在相同的tcp对应于操纵器的多种配置和多个基座位置。因此，不能仅从眼在手上相机数据(例如tcp位置)直接确定操纵器的配置以及冗余移动操纵器的基座位置和朝向。


技术实现要素：

4.提供本发明内容以便介绍一组构思，这组构思将在以下的具体实施方式中做进一步描述。其并非旨在标识所要求保护的主题的关键特征或必要特征，也不旨在用于限制所要求保护的主题的范围。
5.本公开内容的实施例提出了用于估计对象跟踪系统中的系统状态的方法和装置。在一些实施方式中，可以接收来自所述对象跟踪系统中的多个相机的多个图像，其中，所述多个相机包括眼在手外相机和眼在手上相机中的至少一项。可以从所述多个图像中提取位姿信息。可以基于所述位姿信息估计系统状态，其中，所述系统状态包括机器人配置和目标对象位姿。
6.应当注意，以上一个或多个方面包括以下详细描述以及权利要求中具体指出的特征。下面的说明书及附图详细阐述了所述一个或多个方面的某些说明性特征。这些特征仅仅指示可以实施各个方面的原理的多种方式，并且本公开内容旨在包括所有这些方面和其等同变换。
附图说明
7.以下将结合附图描述所公开的多个方面，这些附图被提供用以说明而非限制所公开的方面。
8.图1示出了根据实施例的、用于估计系统状态的对象跟踪系统的示例性框图。
9.图2示出了根据实施例的对象跟踪系统的示例性测量操作。
10.图3示出了根据实施例的对象跟踪系统的移动操纵器的另一示例性测量操作。
11.图4示出了根据实施例的、基于数据融合的估计模型的示例性估计过程。
12.图5示出了根据实施例的、用于估计对象跟踪系统中的系统状态的示例性方法的
流程图。
13.图6示出了根据实施例的、用于估计对象跟踪系统中的系统状态的示例性装置。
14.图7示出了根据实施例的、用于估计对象跟踪系统中的系统状态的示例性控制设备。
具体实施方式
15.现在将参考几种示例性实施方式来讨论本公开内容。应当理解，讨论这些实施方式仅仅用于使得本领域技术人员能够更好地理解并从而实施本公开内容的实施例，而并非暗示对本公开内容的范围的任何限制。
16.本技术提供了通过多相机融合技术，例如融合来自眼在手上相机和眼在手外相机的图像的位姿信息，估计对象位姿和用于移动操纵器的机器人配置(包括世界坐标中的基座位置和朝向以及移动操纵器关节配置)中的至少一项。在本文中，位姿信息可以通过从图像中提取的位姿测量结果来表示，并且由6d位姿向量组成。具体而言，机器人基座可以通过眼在手外相机进行跟踪，以便从相机数据直接估计操纵器的配置和基座位置/朝向。在本文中，操纵器的配置被表示为关节角度的向量，其被称为关节空间坐标。
17.此处描述的方法利用基于来自相机数据和图像处理的位姿测量结果的状态估计来估计对象(例如，工件)在世界坐标中的位姿，以及关节坐标中的冗余移动操纵器的配置。该系统由眼在手外相机和眼在手上相机构建而成，该眼在手外相机观察包含移动对象和移动机器人的场景，该移动机器人具有可移动的基座、操纵器和操纵器末端执行器，以及该眼在手上相机安装在操纵器末端执行器上。眼在手外相机可以跟踪对象以及机器人的基座。基于此，获得了对象和基座相对于眼在手外相机的6d位姿测量结果。眼在手上相机跟踪对象并提供对象相对于其相机坐标的6d位姿测量结果。通过使用无迹卡尔曼滤波器对这三个位姿测量结果或信息进行融合，以获得针对世界坐标中的对象位姿和关节角坐标中的机器人配置的估计。在一个实施例中，无迹卡尔曼滤波器对于对象和移动机器人运动使用运动学模型，并且测量模型基于相机和移动机器人在世界框架中的运动学关系。
18.在一些示例中，通过将对象相对于眼在手外相机坐标的位姿测量结果和对象相对于眼在手上相机坐标的位姿测量结果进行融合，从状态估计中获得对象位姿。本文中假设事先已知世界坐标中的眼在手外相机的位姿测量结果。在一些实施例中，机器人配置是从所有上述三个位姿测量结果重建的，即，对象位姿相对于眼在手外相机坐标的位姿测量结果，基座位置/朝向相对于眼在手外手相机坐标的位姿测量结果，以及对象位姿相对于眼在手上相机坐标的位姿测量结果。在一些实施例中，可以从对象位姿相对于眼在手外相机坐标的位姿测量结果和对象位姿相对于眼在手上相机坐标的位姿测量结果获得相对相机位姿。进一步地，通过组合对象位姿相对于眼在手外相机坐标的位姿测量结果和机器人基座相对于眼在手外相机坐标的位姿测量结果，可以相对于具有相同操纵器末端执行器位姿的不同移动基座上的冗余操纵器重建操纵器的配置。
19.本发明的实施例提出了用于估计对象跟踪系统中的系统状态的方法。在本文中，对象跟踪系统可用于通过多个相机(例如至少一个眼在手上相机和至少一个眼在手外相机)来跟踪对象(例如工件)。在一些实施例中，对象跟踪系统中可以存在至少一个对象、机器人、基于数据融合的估计模型和多个相机。在一些示例中，机器人可以至少具有基座、操
纵器和操纵器末端执行器，眼在手上相机被安装在该末端执行器上。操纵器末端执行器上的工具可以具有工具中心点(tcp)。在一个示例中，基于数据融合的估计模型可以由无迹卡尔曼滤波器实现，用于估计对象跟踪系统的系统状态。应当理解，无迹卡尔曼滤波器仅作为用于实现估计模型的示例而非限制，本文中描述的估计模型可以通过任何其他合适种类的估计模型来实现。系统状态可以包括机器人配置和对象位姿。在一些示例中，机器人配置可以包括基座的基座位置和朝向以及操纵器的配置。通过融合来自多个相机的图像，具体而言，来自至少一个眼在手上相机和至少一个眼在手外相机的图像，可以从这些相机直接估计对象位姿和机器人配置二者。也就是说，可以经由眼在手外相机来跟踪机器人基座和操纵器，以根据由相机捕捉的图像直接估计操纵器的配置和基座位置和朝向。这将与基于关节角度测量结果和/或从其他技术(例如激光定位等)获得的定位数据的机器人配置(包括操纵器的配置以及基座位置和朝向)的计算成本很高的反向推断相反。
20.图1示出了根据实施例的、用于估计系统状态的对象跟踪系统100的示例性框图。
21.在图1中，在对象跟踪系统100中存在眼在手外相机102、对象104、估计模型106和机器人108。
22.眼在手外相机102可以捕捉对象104和机器人108的图像，具体而言，机器人108中的基座。本文中的对象104可以是移动对象，例如由输送机输送的工件。在一些示例中，估计模型106可以是基于数据融合的估计模型，其可以通过无迹卡尔曼滤波器来实现。机器人108可以至少包括基座、操纵器和操纵器末端执行器，图1中未示出。如图1所示，在机器人108中安装有眼在手上相机110，例如安装在操纵器的操纵器末端执行器上。眼在手上相机110可以捕捉对象104的图像。估计模型106可以从眼在手外相机102和眼在手上相机110接收图像，基于数据融合方法融合来自图像的信息以进行状态估计，并生成估计的系统状态。应当理解，虽然估计模型106在图1中被示为与机器人108分离，但如果合适的话，它也可以并入机器人108中。
23.还应当理解，图1所示的所有组件可以是示例性的，并且可以根据特定的应用需求来增加、省略或替换图1中的任何组件。
24.图2示出了根据实施例的对象跟踪系统200的示例性测量操作。
25.在图2中，存在对象202、眼在手外相机204和机器人206，机器人206包括机器人基座208、操纵器210以及安装在操纵器末端执行器中的眼在手上相机212。
26.安置于机器人206外部或与机器人206分离的眼在手外相机204捕捉对象202的图像，并且如图2中的实线所示，可以从捕捉的图像中提取对象202相对于眼在手外相机204的位姿测量结果a。眼在手外相机204还可以捕捉机器人基座208的图像，并且如图2中的点划线所示，可以从捕捉的图像中提取机器人基座208相对于眼在手外相机204的位姿测量结果b。眼在手上相机212可以捕捉对象202的图像，并且如图2中的虚线所示，可以从捕捉的图像中提取对象202相对于眼在手上相机212或者操纵器末端执行器的位姿测量结果c。捕捉的图像可以被馈送到估计模型(例如在图1中但未在图2中示出的估计模型106)，以通过对从图像中提取的信息(例如位姿测量结果)进行融合来估计对象跟踪系统的系统状态。在本文中，估计模型可以通过卡尔曼滤波器来实现，用于状态估计。估计的系统状态可以包括下列中的至少一项：对象202的对象位姿、操纵器210的配置以及机器人基座208的基座位置和朝向。操纵器的配置可由关节臂参数或关节臂空间或关节空间中的向量来表示。在一种实施
方式中，对象位姿可以由世界坐标中的表示，其中指数w表示位姿相对于其表示的坐标系，而est表示位姿p的估计。估计的基座位置和朝向可以与估计的操纵器配置相结合，以从卡尔曼滤波器生成机器人配置估计其中q是关节空间坐标的向量。
27.应当理解，尽管图2中所示的机器人基座似乎没有移动，但机器人基座和/或操纵器可以是移动的或可移动的，如下面的图3中所述。
28.图3示出了根据实施例的对象跟踪系统300的移动操纵器的示例性测量操作。图3中的移动操纵器可以具有多于6个的dof。当机器人基座和/或机器人的操纵器可以移动到另一个位置或位姿时，带有工具的末端执行器和/或安装在末端执行器中的眼在手上相机可能没有移动，因此对于不同的基座位置/朝向和/或操纵器的不同配置，可能存在操纵器工具中心点(tcp)的相同位姿。因此，不能从相同tcp推断出不同的基座位置/朝向和/或操纵器的不同配置。根据本发明的一些实施例，上述问题可以通过经由眼在手外相机跟踪相同机器人基座的不同机器人基座位置/朝向以及相同操纵器的不同配置来解决，如下文参考图3所描述的。
29.如图3中所示，在对象跟踪系统300中，存在对象302、安置于机器人306外部的眼在手外相机304、具有基座308的机器人306、具有操纵器末端执行器的移动操纵器310、以及安装在操纵器末端执行器中的眼在手上相机312。眼在手外相机304捕捉对象302的图像，并且如图3中的实线所示，可以从捕捉的图像中提取对象302相对于眼在手外相机304的位姿测量结果a。眼在手外相机304可以捕捉机器人基座308的图像，并且如图3中的点划线所示，可以从捕捉的图像中提取机器人基座308相对于眼在手外相机304的位姿测量结果b。眼在手上相机312可以捕捉对象302的图像，并且可以从捕捉的图像中提取对象相对于眼在手上相机312或者操纵器末端执行器的位姿测量结果c。
30.机器人基座308可以移动到另一个位置，如308’所示。在一些示例中，移动操纵器310可以与机器人基座一起移动，如310’所示。替代地或可选地，当机器人基座移动时，移动操纵器310可以在机器人基座上移动。也就是说，操纵器上工具的tcp可以相对于基座保持在相同位置(这是因为没有驱动操纵器关节)，而(通过驱动操纵器关节)操纵器上的工具的tcp可以相对于基座移动。当机器人基座308移动时，眼在手外相机304还可以捕捉移动了的机器人基座308’的图像，并且如图3中的点划线所示，可以从捕捉的图像中提取移动了的机器人基座308’相对于眼在手外相机304的位姿测量结果b’。在本文中，测量结果b和b’可以提供关于机器人基座308的移动的不同位置和朝向信息，即机器人基座308的原始位置和朝向信息以及移动了的机器人基座308’的改变了的位置和朝向信息，用于状态估计操作。因此，在机器人基座和/或操纵器移动而操纵器末端执行器或工具保持不动，或者操纵器末端执行器或工具的位姿不变的情况下，通过使用基座相对于眼在手外相机的位姿测量结果和移动了的基座相对于眼在手外相机的位姿测量结果，而不是tcp，可以更准确地确定基座位置和朝向和/或操纵器的配置。
31.捕捉的图像可以被馈送到估计模型(例如在图1中但未在图3中示出的估计模型106)，并且可以从这些图像中提取位姿信息或位姿测量结果，以通过对从图像中提取的位姿信息(例如位姿测量结果a、b或b’和c)进行融合来估计对象跟踪系统的系统状态。在本文中，系统状态可以包括下列中的至少一项：对象302的对象位姿、操纵器310或310’的配置、
机器人基座308或308’的基座位姿。估计的基座位置和朝向可以与估计的操纵器配置相结合，以通过估计模型(例如，无迹卡尔曼滤波器)生成机器人配置估计。
32.图4示出了根据实施例的用于估计模型406的示例性估计过程400。在一些实施例中，估计模型406可以例如由无迹卡尔曼滤波器来实现。该估计过程是通过预测步骤和校正步骤来实现的，这被称为预测器-校正器方法。
33.如图4中所示，估计模型406在框402中从眼在手上相机接收图像，在框404中从眼在手外相机接收图像。估计模型406可以包括预测模型408和校正模型410。
34.在预测模型408中执行的预测过程中，通过将过程模型412传播一个时间步长来估计系统状态，其中先前的系统估计作为初始条件。在过程模型412中，时间实例k 1处的过程状态向量可以如下表示：
35.x
k 1
＝f(xk，uk) vkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式(1)
36.e[vk,vk]＝r
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式(2)
[0037]
其中，xk表示在时刻k的过程状态向量，uk表示在时刻k的输入向量，例如关节速度，f()表示关于随时间传播的参数的过程模型，vk表示模型不确定度的向量，e[vk,vk]＝r表示模型不确定度的方差，将被进一步表示为r。在本文中，x
k 1
可以表示一次模型迭代后的系统状态。
[0038]
可以基于对系统状态的先前估计和系统输入来计算预测过程中的预测系统状态和协方差矩阵，如下所示：
[0039][0040]
p
k 1，k
＝f
°
p
k，k
r
ꢀꢀꢀꢀꢀꢀꢀꢀ
公式(4)
[0041]
其中，表示在时刻k的过程状态向量的预测，是通过将过程模型传播一个时间步长而计算出的系统状态的估计，p
k，k
表示在时刻k的过程状态相对于在时刻k的预测的协方差矩阵，p
k 1，k
表示在时刻k 1的过程状态相对于预测k的协方差矩阵。
[0042]
在校正模型410中执行的校正过程中，将实际传感器输出与估计的传感器输出进行比较。针对系统状态的传感器输出的传感器模型414用于使用来自预测过程的状态估计来计算估计的传感器输出。在传感器模型414中，预测的传感器输出的向量如下进行计算：
[0043]
zk＝h(xk) wkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式(5)
[0044]
e[wk，wk]＝q
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式(6)
[0045]
其中，xk表示在时刻k的过程状态向量，h()表示使用系统状态来计算估计的传感器输出的传感器模型或测量模型，wk表示测量结果不确定度的向量，e[wk，wk]＝q表示测量结果不确定度的方差，其将被进一步表示为q。
[0046]
可以基于预测的测量结果和实际的测量结果来计算在校正模型410中执行的校正过程中的经校正的系统状态和协方差矩阵，如下所示：
[0047][0048]
p
k 1，k 1
＝p
k 1，k-k
·
p
xz，k 1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式(8)
[0049]
[0050]
其中，表示在时刻k 1的过程状态向量的预测，k表示卡尔曼增益，h()表示使用系统状态来计算估计的传感器输出的传感器模型或测量模型，z
meas
是实际测量数据，p
k 1，k 1
表示在时刻k 1的过程状态相对于预测k 1的协方差矩阵，p
xz，k 1
表示在时刻k 1的过程状态和测量结果向量之间的互协方差矩阵，p
zz，k 1，k
表示在时刻k 1相对于预测k的测量结果协方差。在此，协方差矩阵与过程模型412和传感器模型414中的不确定度相关，并且表示不同状态中的不确定度如何影响另一个系统状态或者状态不确定度如何影响传感器输出的关系。协方差矩阵用于计算卡尔曼增益。
[0051]
通过估计模型406的估计过程，可以处理来自眼在手外相机和眼在手上相机的输入图像以生成估计的系统状态作为输出416。
[0052]
图5示出了根据实施例的、用于估计对象跟踪系统中的系统状态的示例性方法500的流程图。
[0053]
在510处，可以从至少具有对象和机器人的对象跟踪系统中的多个相机接收多个图像，其中，多个相机包括眼在手外相机和眼在手上相机中的至少一项。
[0054]
在520处，可以从多个图像中提取位姿信息。
[0055]
在530处，可以基于位姿信息估计系统状态，其中，系统状态包括机器人配置和目标对象位姿。
[0056]
在一种实施方式中，估计系统状态的操作还可以包括：对位姿信息中的多个位姿测量结果进行融合；以及基于融合的位姿测量结果生成估计的系统状态。
[0057]
在一种实施方式中，该方法是由对象跟踪系统中的基于数据融合的估计模型来执行的，机器人至少包括基座、操纵器和操纵器末端执行器，并且其中，眼在手上相机被安装在操纵器末端执行器上。
[0058]
在一种实施方式中，机器人配置包括基座的基座位置和朝向以及操纵器的配置。
[0059]
在一种实施方式中，位姿信息包括下列中的至少一项：对象相对于眼在手外相机的位姿测量结果、基座相对于眼在手外相机的位姿测量结果，以及对象相对于眼在手上相机的位姿测量结果。
[0060]
在一种实施方式中，基于数据融合的估计模型被配置用于：从多个相机接收输入图像；通过基于先前估计的系统状态对输入图像进行预测，获得预测的系统状态；以及通过基于系统状态测量结果校正预测的系统状态来生成当前估计的系统状态。
[0061]
在一种实施方式中，基座是移动基座，并且当操纵器被安装在移动基座上时，该操纵器是可移动的。
[0062]
应该明白的是：方法500还可以包括如上所提到的根据本公开内容的实施例的、用于估计对象跟踪系统中的系统状态的任何步骤/操作。
[0063]
图6示出了根据实施例的、用于估计对象跟踪系统中的系统状态的示例性装置600。
[0064]
装置600可以包括：接收模块610，用于从至少具有对象和机器人的对象跟踪系统中的多个相机接收多个图像，其中，多个相机包括眼在手外相机和眼在手上相机中的至少一项；提取模块620，用于从多个图像中提取位姿信息；以及估计模块630，用于基于位姿信息估计系统状态，其中，系统状态包括机器人配置和对象位姿。
[0065]
在一种实施方式中，估计模块还包括：融合模块，用于对位姿信息中的多个位姿测
量结果进行融合；以及生成模块，用于基于融合的位姿测量结果生成估计的系统状态。
[0066]
在一种实施方式中，该装置是由对象跟踪系统中的基于数据融合的估计模型来实现的，机器人至少包括基座、操纵器和操纵器末端执行器，并且其中，眼在手上相机被安装在操纵器末端执行器上。
[0067]
在一种实施方式中，基于数据融合的估计模型被配置用于：从多个相机接收输入图像；通过基于先前估计的系统状态对输入图像执行预测，获得预测的系统状态；以及通过基于系统状态测量结果校正预测的系统状态来生成当前估计的系统状态。
[0068]
另外，装置600还可以包括根据如上所提及的本公开内容的实施例的、被配置用于估计对象跟踪系统中的系统状态的任何其他模块。
[0069]
图7示出了根据实施例的、用于估计对象跟踪系统中的系统状态的示例性控制设备700。
[0070]
控制设备700可以包括处理器710。控制设备700还可以包括与处理器710连接的存储器720。存储器720可以存储计算机可执行指令，当被执行时，指令使得处理器710根据如上所提及的本公开内容的实施例执行用于估计对象跟踪系统中的系统状态的方法的任何操作。
[0071]
本公开内容的实施例可以体现在非暂时性计算机可读介质中。非暂时性计算机可读介质可以包括指令，当被执行时，指令使得一个计算机执行根据如上所提及的本公开内容的实施例的、用于估计对象跟踪系统中的系统状态的方法的任何操作。
[0072]
应当理解，以上描述的方法中的所有操作都仅仅是示例性的，本公开内容并不受限于方法中的任何操作或这些操作的序列顺序，而是应当涵盖在相同或相似构思下的所有其他等同项。
[0073]
还应当理解，以上描述的装置中的所有模块都可以通过各种方式来实施。这些模块可以被实施为硬件、软件、或其组合。此外，这些模块中的任何模块可以在功能上被进一步划分成子模块或组合在一起。
[0074]
已经结合各种装置和方法描述了处理器。这些处理器可以使用电子硬件、计算机软件或其任意组合来实施。至于这些处理器是实施为硬件还是软件将取决于具体的应用以及施加在系统上的总体设计约束。作为示例，本公开内容中给出的处理器、处理器的任意部分、或者处理器的任意组合可以与微处理器、微控制器、数字信号处理器(dsp)、现场可编程门阵列(fpga)、可编程逻辑器件(pld)、状态机、门逻辑、分立硬件电路、以及配置用于执行贯穿本公开内容描述的各种功能的其他适合的处理部件一起实现。本公开内容给出的处理器、处理器的任意部分、或者处理器的任意组合的功能可以与由微处理器、微控制器、dsp或其他适合的平台所执行的软件一起实现。
[0075]
软件应当被广泛地解释为表示指令、指令集、代码、代码段、程序代码、程序、子程序、软件模块、应用、软件应用、软件包、例程、子例程、对象、运行线程、过程、函数等。软件可以驻留在计算机可读介质中。计算机可读介质可以包括举例而言存储器，例如磁性存储设备(如，硬盘、软盘、磁条)、光盘、智能卡、闪存设备、随机存取存储器(ram)、只读存储器(rom)、可编程rom(prom)、可擦除prom(eprom)、电可擦除prom(eeprom)、寄存器或者可移动盘。尽管在贯穿本公开内容给出的多个方面中将存储器示出为是与处理器分离的，但是存储器也可以位于处理器内部(例如，高速缓存器或寄存器)。
[0076]
提供了先前描述以使本领域的任何技术人员能够实施本文所描述的各种方面。对这些方面的各种修改对于本领域技术人员是显而易见的，以及本文限定的一般性原理可以应用于其他方面。因此，权利要求并非旨在被局限于本文示出的方面。对本领域普通技术人员来说已知或者将要获知的与贯穿本公开内容所描述的各种方面的元素等效的所有结构和功能在此都通过引用的方式明确并入本文，并且旨在被权利要求书所包括。