CN109993798A - 多摄像头检测运动轨迹的方法、设备及存储介质 - Google Patents

Abstract

本发明提供了多摄像头检测运动轨迹的方法、设备及存储介质，其中，方法包括：在地面设置网格图案，建立地面坐标系；在机器人顶部贴一个非对称图案，建立图案坐标系，获得角点在图案坐标系中各自的坐标；基于地面上方设置多个相机拍摄网格图案和机器人的照片，各自建立相机坐标系，获得非对称图案的角点在相机坐标系中各自的像素坐标；获得每个相机坐标系相对于地面坐标系的第一类单应矩阵、与地面坐标系之间的旋转量和平移矢量，以及相机坐标系下地面的方程；计算图案坐标系和地面坐标系之间的旋转量和平移矢量；通过组合每个相机的图像获得机器人的第二类运行轨迹图案。本发明减小了测量偏差，提高了测量精度，定位的范围可无限扩展。

Description

多摄像头检测运动轨迹的方法、设备及存储介质

技术领域

本发明涉及运动轨迹的检测领域，具体地说，涉及多摄像头检测运动轨迹的方法、设备及存储介质。

背景技术

机器人移动时，对轨迹精度的要求越来越高，机器人轨迹精度已成为衡量机器人性能的一项重要指标，为精确测量机器人的轨迹精度，目前通过红外等主动光源的方式进行轨迹测量，测量范围有限，测量偏差大，测量精度容易受到噪声干扰。

因此，本发明提供了一种多摄像头检测运动轨迹的方法、设备及存储介质。

发明内容

针对现有技术中的问题，本发明的目的在于提供多摄像头检测运动轨迹的方法、设备及存储介质，克服机器人运动轨迹测量中测量范围有限，测量效果不稳定问题，能够精确地反映出一时间段内机器人运动的实际位移，减小了测量偏差，提高了测量精度，并且没有累积误差，定位的范围可无限扩展。

本发明的实施例提供一种多镜头运动轨迹监测方法，包括以下步骤：

S110、在地面设置网格图案，建立基于地面的地面坐标系；

S120、在机器人顶部贴一个非对称图案，所述非对称图案具有至少两个角点，建立基于所述非对称图案的图案坐标系，获得所述角点在所述图案坐标系中各自的坐标；

S130、基于地面上方设置多个相机，每个所述相机独立拍摄所述网格图案和机器人的照片，在每个所述相机获得的照片中各自建立相机坐标系，获得所述非对称图案的角点在所述相机坐标系中各自的像素坐标；

S140、获得每个相机的所述相机坐标系相对于地面坐标系的第一类单应矩阵，并获得相机坐标系与地面坐标系之间的旋转量和平移矢量，以及相机坐标系下地面的方程；

S150、根据所述角点在所述图案坐标系中各自的坐标和在地面坐标系中各自的坐标间的坐标关系，计算图案坐标系和地面坐标系之间的旋转量和平移矢量；以及

S160、根据所述相机的第一类单应矩阵，获得机器人在每个所述相机的图像中的第一类运行轨迹图案，将所述相机获得的图案根据所述相机的分布在天花板的位置关系组合，获得所述机器人的第二类运行轨迹图案。

优选地，所述非对称图案是矩形的二维码图案，选择所述二维码图案的四个顶点作为角点，所述步骤S140包括以下步骤：

S141、获得地面坐标系中的地面相对于每个所述相机的成像平面在对应的所述相机坐标系中的第二类单应矩阵；

S142、通过所述第二类单应矩阵获得每个相机坐标系与地面坐标系之间的旋转量和平移矢量，以及每个所述相机坐标系下地面的方程；

S143、设所述相机坐标系中，每个角点所在的平行于地面的第一平面的方程与所述地面的方程之间分别具有四个不同高度；所述相机坐标系下的四条反投影射线与四个所述第一平面的方程的四个交点的坐标，所述交点的坐标满足以下条件：位于所述二维码图案一边上的两个角点之间的第一距离等于所述二维码图案的已知边长；并且位于所述二维码图案一对角线上的两个角点之间的第二距离等于所述二维码图案的已知对角线长；

S144、设对应所述二维码图案的四个第一距离与两个所述第二距离的和为第一总和，所述二维码图案四条已知边长与两条已知对角线的和为第二总和，获得满足第一总和减去第二总和的差最小时的四个所述高度误差；

S145、获得在所述相机坐标系中所述四个角点的坐标。

优选地，所述步骤S150包括以下步骤：

S151、将所述四个角点在所述相机坐标系中各自的坐标通过旋转和平移获得所述四个角点在所述地面坐标系中各自的坐标；

S152、根据所述四个角点在所述图案坐标系中各自的坐标和在地面坐标系中各自的坐标，计算两个坐标系之间的旋转量和平移矢量；

S153、驱动所述机器人进行自转，拍摄自转形成的圆形自转轨迹，获得所述圆形自转轨迹的半径趋近于零时，所述机器人自转中心在X轴上相对于中心坐标的偏移量和在Y轴上相对于中心坐标的偏移量。

优选地，所述步骤S140中，相邻的所述相机具有公共视野；

所述步骤S160中，相邻的所述相机获得的两个第一类运行轨迹中位于公共视野区域的轨迹相同，根据所述相机的分布在所述天花板的位置关系排列所有所述第一类运行轨迹图案，相邻所述相机的所述第一类运行轨迹图案之间轨迹相同的部分重叠。

优选地，所述步骤S130包括以下步骤：

S131、在每个所述相机获得的所述网格图案的照片中各自建立相机坐标系，基于所述照片中的网格图案在所述相机坐标系中设置多个图像角点，获得每个所述相机坐标系所述图像角点所在的坐标位置(X_c，Y_c)；

S132、对每个所述相机获得的照片分别做图像畸变校正，并获得所述图像角点映射到所述地面坐标系的地面角点的坐标位置(X_g，Y_g)，并建立每个所述相机坐标系中的每个图像角点与其对应的地面坐标的映射关系；

S133、设表达图像角点在相机坐标系的第一矩阵表达该图像角点映射到地面坐标系的第二矩阵每个所述相机的镜头具有3行3列的单应矩阵为H_i，根据P_g＝H_i×P_c，分别获得每个镜头与地面间的单应矩阵H_i，i大于1；

S134、根据拍摄到同一个所述地面角点的所述相机，建立关于每个地面角点的空间映射序列，设的空间映射序列为其中，为地面坐标系中的第k个点的矩阵，H_i为拍摄到同一个所述地面角点的第i个相机与地面间的单应矩阵，为所述同一个所述地面角点映射于第i个相机的相机坐标系中的第w个图像角点所在的坐标位置的矩阵，m，n，w为该所述地面角点映射的图像角点在该相机拍摄到的所有图像角点中的序号；

S135、根据所述每个地面角点的空间映射序列优化每个所述镜头的单应矩阵H_i。

优选地，根据空间映射序列优化单应矩阵H_i的步骤为：

设与对应的空间映射序列中每个元素的差值的总和作为第一参数T_k，

设所述地面坐标系中每个点的第一参数T_k的总和作为第二参数S，S＝T₁+T₂+......+T_k；

求当第二参数S的值最小时，每个相机的单应矩阵H_i。

优选地，所述单应矩阵H_i中第三行第三列的元素值为1。

优选地，所述相机吊装于所述地面之上的天花板底面。

优选地，所述相机的镜头是以下镜头中的一种：

焦距为24mm至38mm的广角镜头；

焦距为13mm至24mm的超广角镜头；

焦距为6mm至17mm的鱼眼镜头。

优选地，所述步骤S150之后还包括步骤S260，所述步骤S260包括以下步骤：

S261、将一个相机作为基准相机，其余每个所述相机相对于所述基准相机具有偏移时长和时长系数；

S262、建立第一参考坐标系，所述第一参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿所述第二类运行轨迹图案中的X轴方向的第三类轨迹，根据所述相机拍摄到机器人的时间顺序在第一参考坐标系排列所有所述第三类运行轨迹图案，建立第二参考坐标系，所述第二参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿所述第二类运行轨迹图案中的Y轴方向的第四类轨迹，根据所述相机拍摄到机器人的时间顺序在第二参考坐标系排列所有所述第四类轨迹图案；

S263、分别在所述第一参考坐标系和第二参考坐标系进行时间对齐，获得同时满足将相邻的所述第三类轨迹图案之间轨迹相同的部分重叠，并且相邻的所述第三类轨迹图案之间轨迹相同的部分重叠时的偏移时长和时长系数。

本发明的实施例还提供一种多镜头运动轨迹监测设备，包括：

处理器；

存储器，其中存储有所述处理器的可执行指令；

其中，所述处理器配置为经由执行所述可执行指令来执行上述多镜头运动轨迹监测方法的步骤。

本发明的实施例还提供一种计算机可读存储介质，用于存储程序，所述程序被执行时实现上述多镜头运动轨迹监测方法的步骤。

本发明的多摄像头检测运动轨迹的方法、设备及存储介质，能够精确地反映出一时间段内机器人运动的实际位移，减小了测量偏差，提高了测量精度，并且没有累积误差，定位的范围可无限扩展。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显。

图1是使用本发明的多镜头运动轨迹监测方法的流程图；

图2是图1中步骤S140的分解流程图；

图3至7是实施本发明的多镜头运动轨迹监测方法的扫地机器人的步骤示意图；

图8是各个相机拍摄得到的图案组合后的示意图；

图9是根据图8表示机器人在x方向随时间变化的曲线的示意图；

图10是根据图8表示机器人在y方向随时间变化的曲线的示意图；

图11是将图9中的轨迹进行对齐后的示意图；

图12是将图10中的轨迹进行对齐后的示意图；

图13是本发明的多镜头运动轨迹监测设备的结构示意图；以及

图14是本发明一实施例的计算机可读存储介质的结构示意图。

具体实施方式

现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的实施方式。相反，提供这些实施方式使得本发明将全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。在图中相同的附图标记表示相同或类似的结构，因而将省略对它们的重复描述。

图1是使用本发明的多镜头运动轨迹监测方法的流程图。如图1所示，本发明的多摄像头检测运动轨迹的方法，包括以下步骤：

S110、在地面设置网格图案，建立基于地面的地面坐标系。

S120、在机器人顶部贴一个非对称图案，非对称图案具有至少两个角点，建立基于非对称图案的图案坐标系，获得角点在图案坐标系中各自的坐标。

S130、基于地面上方设置多个相机，每个相机独立拍摄网格图案和机器人的照片，在每个相机获得的照片中各自建立相机坐标系，获得非对称图案的角点在相机坐标系中各自的像素坐标。

S140、获得每个相机的相机坐标系相对于地面坐标系的第一类单应矩阵，并获得相机坐标系与地面坐标系之间的旋转量和平移矢量，以及相机坐标系下地面的方程。

S150、根据角点在图案坐标系中各自的坐标和在地面坐标系中各自的坐标间的坐标关系，计算图案坐标系和地面坐标系之间的旋转量和平移矢量。

S160、根据相机的第一类单应矩阵，获得机器人在每个相机的图像中的第一类运行轨迹图案，将相机获得的图案根据相机的分布在天花板的位置关系组合，获得机器人的第二类运行轨迹图案。

在一个优选实施例中，非对称图案是矩形的二维码图案，选择二维码图案的四个顶点作为角点，图2是图1中步骤S140的分解流程图，如图2所示，步骤S140包括以下步骤：

S141、获得地面坐标系中的地面相对于每个相机的成像平面在对应的相机坐标系中的第二类单应矩阵。

S142、通过第二类单应矩阵获得每个相机坐标系与地面坐标系之间的旋转量和平移矢量，以及每个相机坐标系下地面的方程。(论文《deeper understanding of thehomography decomposition for vision-based control》已经公开了如何从单应矩阵纵横接触相机坐标系与地面坐标系的旋转量和平移矢量。)

S143、设相机坐标系中，每个角点所在的平行于地面的第一平面的方程与地面的方程之间分别具有四个不同高度。相机坐标系下的四条反投影射线与四个第一平面的方程的四个交点的坐标，交点的坐标满足以下条件：位于二维码图案一边上的两个角点之间的第一距离等于二维码图案的已知边长。并且位于二维码图案一对角线上的两个角点之间的第二距离等于二维码图案的已知对角线长。

S144、设对应二维码图案的四个第一距离与两个第二距离的和为第一总和，二维码图案四条已知边长与两条已知对角线的和为第二总和，获得满足第一总和减去第二总和的差最小时的四个高度误差。

S145、获得在相机坐标系中四个角点的坐标。(论文《least-squares fitting two3-d point sets》，中已经介绍了如何通过地面坐标系下4个空间角点的坐标和附着在图案坐标系上四个角点的坐标的匹配关系得到图案坐标系相对于地面坐标系旋转的角度的原理)

在一个优选实施例中，步骤S150包括以下步骤：

S151、将四个角点在相机坐标系中各自的坐标通过旋转和平移获得四个角点在地面坐标系中各自的坐标。

S152、根据四个角点在图案坐标系中各自的坐标和在地面坐标系中各自的坐标，计算两个坐标系之间的旋转量和平移矢量。

S153、驱动机器人进行自转，拍摄自转形成的圆形自转轨迹，获得圆形自转轨迹的半径趋近于零时，机器人自转中心在X轴上相对于中心坐标的偏移量和在Y轴上相对于中心坐标的偏移量。

在一个优选实施例中，步骤S140中，相邻的相机具有公共视野。

步骤S160中，相邻的相机获得的两个第一类运行轨迹中位于公共视野区域的轨迹相同，根据相机的分布在天花板的位置关系排列所有第一类运行轨迹图案，相邻相机的第一类运行轨迹图案之间轨迹相同的部分重叠。

在一个优选实施例中，步骤S130包括以下步骤：

S131、在每个相机获得的网格图案的照片中各自建立相机坐标系，基于照片中的网格图案在相机坐标系中设置多个图像角点，获得每个相机坐标系图像角点所在的坐标位置(X_c，Y_c)。

S132、对每个相机获得的照片分别做图像畸变校正，并获得图像角点映射到地面坐标系的地面角点的坐标位置(X_g，Y_g)，并建立每个相机坐标系中的每个图像角点与其对应的地面坐标的映射关系。

S133、设表达图像角点在相机坐标系的第一矩阵表达该图像角点映射到地面坐标系的第二矩阵每个相机的镜头具有3行3列的单应矩阵为H_i，根据P_g＝H_i×P_c，分别获得每个镜头与地面间的单应矩阵H_i，i大于1。

S134、根据拍摄到同一个地面角点的相机，建立关于每个地面角点的空间映射序列，设的空间映射序列为其中，为地面坐标系中的第k个点的矩阵，H_i为拍摄到同一个地面角点的第i个相机与地面间的单应矩阵，为同一个地面角点映射于第i个相机的相机坐标系中的第w个图像角点所在的坐标位置的矩阵，m，n，w为该地面角点映射的图像角点在该相机拍摄到的所有图像角点中的序号。

S135、根据每个地面角点的空间映射序列优化每个镜头的单应矩阵H_i。

在一个优选实施例中，根据空间映射序列优化单应矩阵H_i的步骤为：

设与对应的空间映射序列中每个元素的差值的总和作为第一参数T_k，

设地面坐标系中每个点的第一参数T_k的总和作为第二参数S，S＝T₁+T₂+......+T_k。

求当第二参数S的值最小时，每个相机的单应矩阵H_i。

在一个优选实施例中，单应矩阵H_i中第三行第三列的元素值为1。

在一个优选实施例中，相机吊装于地面之上的天花板底面。

在一个优选实施例中，相机的镜头是以下镜头中的一种：

焦距为24mm至38mm的广角镜头。

焦距为13mm至24mm的超广角镜头。

焦距为6mm至17mm的鱼眼镜头。

在一个优选实施例中，步骤S150之后还包括步骤S260，步骤S260包括以下步骤：

S261、将一个相机作为基准相机，其余每个相机相对于基准相机具有偏移时长和时长系数。

S262、建立第一参考坐标系，第一参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿第二类运行轨迹图案中的X轴方向的第三类轨迹，根据相机拍摄到机器人的时间顺序在第一参考坐标系排列所有第三类运行轨迹图案，建立第二参考坐标系，第二参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿第二类运行轨迹图案中的Y轴方向的第四类轨迹，根据相机拍摄到机器人的时间顺序在第二参考坐标系排列所有第四类轨迹图案。

S263、分别在第一参考坐标系和第二参考坐标系进行时间对齐，获得同时满足将相邻的第三类轨迹图案之间轨迹相同的部分重叠，并且相邻的第三类轨迹图案之间轨迹相同的部分重叠时的偏移时长和时长系数。

图3至7是实施本发明的多镜头运动轨迹监测方法的扫地机器人的步骤示意图。

如图3所示，在地面设置网格图案1，建立基于地面的地面坐标系。本发明中的网格图案1可以是用笔在地面上画的网格，也可以是地砖或是底板的接缝形成网格图案，但不以此为限。在地面之上的天花板底面吊装多个相机31、32、33、34、35、36、37、38、39，且相机的镜头均位于同一成像平面，相机也可以形成阵列，每个相机独立拍摄网格图案的照片。很显然，设置的相机的范围越广，本发明中轨迹检测的范围也就越大。设置的相机的范围越多，本发明中轨迹检测的精确度也就越高。相机的镜头是以下镜头中的一种：焦距为24mm至38mm的广角镜头。焦距为13mm至24mm的超广角镜头。焦距为6mm至17mm的鱼眼镜头。本实施例中，统一采用焦距为21mm的超广角镜头。

在机器人顶部贴一个二维码图案(图3-6中没有示出二维码图案，关于二维码图案4请参见图7)，选择二维码图案4的四个顶点4a、4b、4c、4d作为角点，建立基于二维码图案4的图案坐标系，获得角点在图案坐标系中各自的坐标。本实施例中，借助AprilTag工具检测贴在机器人表面的图案，这个工具的好处是检测成功率高并且检测到的角点顺序是固定的。AprilTag是一个现有的视觉基准库，在AR，机器人，相机校准领域广泛使用。通过特定的标志(与二维码相似，但是降低了复杂度以满足实时性要求)，可以快速地检测标志，并计算相对位置。

在每个相机获得的网格图案的照片中各自建立相机坐标系，如图4所示是相机35拍摄到的照片，基于照片中的网格图案在相机坐标系中检测图像的多个角点，获得每个相机坐标系图像角点所在的坐标位置A(X_c’，Y_c’)。如图5所示，对每个相机获得的照片分别做图像畸变校正，获得每个相机坐标系中图像角点在畸变校正后的图像中的坐标位置B(X_c，Y_c)。如图5所示，建立每个相机坐标系中的每个图像角点与其对应的地面坐标的映射关系。获得畸变校正后的图像角点的坐标位置B(X_c，Y_c)映射到地面坐标系的地面角点的坐标位置A’(X_g，Y_g)。本实施例中，采用现有技术对广镜头拍摄到的进行照片进行校正，将照片中的弧线轨迹的网格还原到与真实场景一致的横平竖直的网格图案。专利公开号CN104537616A、CN109035170A、CN108198222A、CN104182933A、CN104240236B等中分别介绍了如何将广角镜头或者鱼眼镜头拍摄到的照片进行校正，此处不再赘述。

设表达相机坐标系中图像角点在畸变校正后的图像中的坐标位置B(X_c，Y_c)的第一矩阵，以及表达该畸变校正后的图像角点映射到地面坐标系下的坐标位置A’(X_g，Y_g)的第二矩阵。

其中，第一矩阵为第二矩阵为

每个相机的镜头具有3行3列的单应矩阵为H_i，根据P_g＝H_i×P_c，分别获得每个镜头与地面间的单应矩阵H_i，i大于1。本实施例中，单应矩阵H_i中第三行第三列的元素值为1。

参考图6，本实施例中的地面角点为101、102……125，其中地面角点114的坐标位置就是坐标位置A’(X_g，Y_g)，畸变校正后的图像角点的坐标位置B(X_c，Y_c)与地面角点114的坐标位置(X_g，Y_g)建立映射关系。本实施例中的每个地面角点根据拍摄到同一个地面角点的相机，建立关于每个地面角点的空间映射序列，设的空间映射序列为其中，为地面坐标系中的第k个点的矩阵，H_i为拍摄到同一个地面角点的第i个相机与地面间的单应矩阵，为同一个地面角点映射于第i个相机的相机坐标系中的第w个图像角点所在的坐标位置的矩阵，m，n，w为该地面角点映射的图像角点在该相机拍摄到的所有图像角点中的序号。

根据每个地面角点的空间映射序列优化每个镜头的单应矩阵H_i。设与对应的空间映射序列中每个元素的差值的总和作为第一参数T_k，设地面坐标系中每个点的第一参数T_k的总和作为第二参数S，S＝T₁+T₂+......+T_k。求当第二参数S的值最小时，每个相机的单应矩阵H_i，每个相机的单应矩阵H_i都获得了最优的校正。

如图7所示，单应矩阵H_i并不能直接用于机器人的定位，这是因为计算的是地面与相机的单应关系，而图案贴在机器人上表面，距离地面有一定高度，所以必须算出四个角点距离地面的高度。并将地面与相机的单应关系转化成图案的四个角点与地面的单应关系。可以认为图案上的各个角点在属于各自的4个平行于地面的平面上运动，由于地面与相机的单应矩阵已经计算过，那么当4个角点4a、4b、4c、4d距离地面的高度确定后就可以确定这4个平面在相机坐标系下的方程，利用相机拍摄的图像中通过这4个角点4a、4b、4c、4d的反投影射线与这4个平面相交即可求得这4个角点4a、4b、4c、4d在相机坐标系下的坐标。从上一步计算出的单应矩阵中分解出相机坐标系与地面坐标系的旋转和评议，将这4个空间点的坐标转换到地面坐标系。

随后，获得地面坐标系中的地面相对于每个相机的成像平面在对应的相机坐标系中的第二类单应矩阵。通过第二类单应矩阵获得每个相机坐标系与地面坐标系之间的旋转量和平移矢量，以及每个相机坐标系下地面的方程。设相机坐标系中，每个角点所在的平行于地面的第一平面的方程与地面的方程之间分别具有四个不同高度。相机坐标系下的四条反投影射线与四个第一平面的方程的四个交点的坐标，交点的坐标满足以下条件：位于二维码图案一边上的两个角点之间的第一距离等于二维码图案的已知边长。并且位于二维码图案一对角线上的两个角点之间的第二距离等于二维码图案的已知对角线长。设对应二维码图案的四个第一距离与两个第二距离的和为第一总和，二维码图案四条已知边长与两条已知对角线的和为第二总和，获得满足第一总和减去第二总和的差最小时的四个高度误差。获得在相机坐标系中四个角点的坐标。这一系列步骤的关键前提是获得准确的角点距离地面的高度，这里用到的约束是反投影射线在与当前4个高度的角点构成的平面相交后得到的4个空间坐标共面，并且他们构成一个边长为贴于机器人表面的图案的边长的空间正方形，这个空间正方形的中心就是图案坐标系的原点。用这种方法得到优化后的角点高度。

此时，由于不能保证图案的中心正对着机器人自转中心，所以需要机算出两个中心的偏移量。控制机器人做纯转动，检测出机器人做自转时图案中心点移动形成的轨迹，并找到一个二维的偏移向量，使得在加上偏移量后图案中心点在做自转时的位移趋近于0。例如：将四个角点在相机坐标系中各自的坐标通过旋转和平移获得四个角点在地面坐标系中各自的坐标。根据四个角点在图案坐标系中各自的坐标和在地面坐标系中各自的坐标，计算两个坐标系之间的旋转量和平移矢量。驱动机器人进行自转，拍摄自转形成的圆形自转轨迹，获得圆形自转轨迹的半径趋近于零时，机器人自转中心在X轴上相对于中心坐标的偏移量和在Y轴上相对于中心坐标的偏移量。

对于机器人定位，除了要知道位置信息还要知道旋转角度信息，由于利用AprilTag检测出的角点一直是按固定的顺序排列的，这意味着图案表面的坐标系是固定的，所以算到的地面坐标系下4个空间角点的坐标和附着在图案坐标系上4个角点坐标的匹配关系可以算出图案相对于地面坐标系旋转的角度。

最后，根据相机的第一类单应矩阵，获得机器人在每个相机的图像中的第一类运行轨迹图案，将相机获得的图案根据相机的分布在天花板的位置关系组合，获得机器人的第二类运行轨迹图案。例如：按照相机31、32、33、34、35、36、37、38、39的位置排列，将相机31、32、33、34、35、36、37、38、39每个相机拍摄到的照片拼合在一起，相邻相机的照片有公共视野区域，将相邻照片的公共视野区域部分重叠，使得相邻的相机获得的两个第一类运行轨迹中位于公共视野区域的轨迹相同，根据相机的分布在天花板的位置关系排列所有第一类运行轨迹图案，相邻相机的第一类运行轨迹图案之间轨迹相同的部分重叠。

图8是各个相机拍摄得到的图案组合后的示意图。如图8所示，其中，其中，实线C32代表相机32拍摄到的第一类运行轨迹图案，第一类虚线C33代表相机33拍摄到的第一类运行轨迹图案，第二类虚线C35代表相机35拍摄到的第一类运行轨迹图案，第三类虚线C36代表相机36拍摄到的第一类运行轨迹图案，点划线C39代表相机39拍摄到的第一类运行轨迹图案。五个相机32、33、35、36、39分别拍摄到的第一类运行轨迹图案组合以后得到的第二类运行轨迹图案。机器人在单个坐标系下的位置和旋转都获得了。对每个相机作上述操作，可得到机器人在运动到各个相机视野内时形成的轨迹。

图9是根据图8表示机器人在x方向随时间变化的曲线的示意图。图10是根据图8表示机器人在y方向随时间变化的曲线的示意图。如图9、10所示，其中，图中的横轴X表示时间，单位：秒，两张图的纵轴Y分别表示机器人在x方向和y方向随时间变化的曲线，单位：毫米。五种线条分别代表五个相机(参考图8中的实线C32、第一类虚线C33、第二类虚线C35、第三类虚线C36以及点划线C39)观测到机器人的时间段，可以看出，有些时间段有多个相机同时观测到机器人，但是多个相机的轨迹并不重合。

由于各个相机的时钟频率存在差异，以及相机启动时间存在差异，会导致时间的不对齐，具体表现为，当把位移坐标值随时间变化的曲线画在同一个时间轴下时会发现轨迹存在明显的偏移。为了修正偏移，本发明的步骤S150之后还包括步骤S260，步骤S260包括以下步骤：

S261、将一个相机作为基准相机，其余每个相机相对于基准相机具有偏移时长和时长系数。

S262、建立第一参考坐标系，第一参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿第二类运行轨迹图案中的X轴方向的第三类轨迹，根据相机拍摄到机器人的时间顺序在第一参考坐标系排列所有第三类运行轨迹图案，建立第二参考坐标系，第二参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿第二类运行轨迹图案中的Y轴方向的第四类轨迹，根据相机拍摄到机器人的时间顺序在第二参考坐标系排列所有第四类轨迹图案。

S263、分别在第一参考坐标系和第二参考坐标系进行时间对齐，获得同时满足将相邻的第三类轨迹图案之间轨迹相同的部分重叠，并且相邻的第三类轨迹图案之间轨迹相同的部分重叠时的偏移时长和时长系数。图11是将图9中的轨迹进行对齐后的示意图。图12是将图10中的轨迹进行对齐后的示意图。如图11、12所示，其中，图中的横轴X表示时间，单位：秒，两张图的纵轴Y分别表示机器人在x方向和y方向随时间变化的曲线，单位：毫米。五种线条分别代表五个相机(参考图8中的实线C32、第一类虚线C33、第二类虚线C35、第三类虚线C36以及点划线C39)观测到机器人的时间段，显然，经过时间对齐之后，所有时间段有多个相机同时观测到机器人，多个相机的轨迹重合成为一个轨迹。

本发明的多镜头运动轨迹监测方法能够精确地反映出一时间段内机器人运动的实际位移，减小了测量偏差，提高了测量精度，并且没有累积误差，定位的范围可无限扩展。

本发明实施例还提供一种多镜头运动轨迹监测设备，包括处理器。存储器，其中存储有处理器的可执行指令。其中，处理器配置为经由执行可执行指令来执行的多镜头运动轨迹监测方法的步骤。

如上所示，该实施例能够精确地反映出一时间段内机器人运动的实际位移，减小了测量偏差，提高了测量精度，并且没有累积误差，定位的范围可无限扩展。

所属技术领域的技术人员能够理解，本发明的各个方面可以实现为系统、方法或程序产品。因此，本发明的各个方面可以具体实现为以下形式，即：完全的硬件实施方式、完全的软件实施方式(包括固件、微代码等)，或硬件和软件方面结合的实施方式，这里可以统称为“电路”、“模块”或“平台”。

图13是本发明的多镜头运动轨迹监测设备的结构示意图。下面参照图13来描述根据本发明的这种实施方式的电子设备600。图13显示的电子设备600仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图13所示，电子设备600以通用计算设备的形式表现。电子设备600的组件可以包括但不限于：至少一个处理单元610、至少一个存储单元620、连接不同平台组件(包括存储单元620和处理单元610)的总线630等。

其中，存储单元存储有程序代码，程序代码可以被处理单元610执行，使得处理单元610执行本说明书上述电子处方流转处理方法部分中描述的根据本发明各种示例性实施方式的步骤。例如，处理单元610可以执行如图1中所示的步骤。

存储单元620可以包括易失性存储单元形式的可读介质，例如随机存取存储单元(RAM)6201和/或高速缓存存储单元6202，还可以进一步包括只读存储单元(ROM)6203。

存储单元620还可以包括具有一组(至少一个)程序模块6205的程序/实用工具6204，这样的程序模块6205包括但不限于：操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。

总线630可以为表示几类总线结构中的一种或多种，包括存储单元总线或者存储单元控制器、外围总线、图形加速端口、处理单元或者使用多种总线结构中的任意总线结构的局域总线。

电子设备600也可以与一个或多个外部设备700(例如键盘、指向设备、蓝牙设备等)通信，还可与一个或者多个使得用户能与该电子设备600交互的设备通信，和/或与使得该电子设备600能与一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口650进行。并且，电子设备600还可以通过网络适配器660与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。网络适配器660可以通过总线630与电子设备600的其它模块通信。应当明白，尽管图中未示出，可以结合电子设备600使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储平台等。

本发明实施例还提供一种计算机可读存储介质，用于存储程序，程序被执行时实现的多镜头运动轨迹监测方法的步骤。在一些可能的实施方式中，本发明的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当程序产品在终端设备上运行时，程序代码用于使终端设备执行本说明书上述电子处方流转处理方法部分中描述的根据本发明各种示例性实施方式的步骤。

如上所示，该实施例能够精确地反映出一时间段内机器人运动的实际位移，减小了测量偏差，提高了测量精度，并且没有累积误差，定位的范围可无限扩展。

图14是本发明的计算机可读存储介质的结构示意图。参考图14所示，描述了根据本发明的实施方式的用于实现上述方法的程序产品800，其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码，并可以在终端设备，例如个人电脑上运行。然而，本发明的程序产品不限于此，在本文件中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

计算机可读存储介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。可读存储介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。可读存储介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、有线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码，程序设计语言包括面向对象的程序设计语言—诸如Java、C++等，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中，远程计算设备可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算设备，或者，可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。

综上，本发明的目的在于提供多摄像头检测运动轨迹的方法、设备及存储介质，能够精确地反映出一时间段内机器人运动的实际位移，减小了测量偏差，提高了测量精度，并且没有累积误差，定位的范围可无限扩展。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (12)

1.一种多摄像头检测运动轨迹的方法，其特征在于，包括以下步骤：

S110、在地面设置网格图案，建立基于地面的地面坐标系；

S120、在机器人顶部贴一个非对称图案，所述非对称图案具有至少两个角点，建立基于所述非对称图案的图案坐标系，获得所述角点在所述图案坐标系中各自的坐标；

S130、基于地面上方设置多个相机，每个所述相机独立拍摄所述网格图案和机器人的照片，在每个所述相机获得的照片中各自建立相机坐标系，获得所述非对称图案的角点在所述相机坐标系中各自的像素坐标；

S140、获得每个相机的所述相机坐标系相对于地面坐标系的第一类单应矩阵，并获得相机坐标系与地面坐标系之间的旋转量和平移矢量，以及相机坐标系下地面的方程；

S150、根据所述角点在所述图案坐标系中各自的坐标和在地面坐标系中各自的坐标间的坐标关系，计算图案坐标系和地面坐标系之间的旋转量和平移矢量；以及

S160、根据所述相机的第一类单应矩阵，获得机器人在每个所述相机的图像中的第一类运行轨迹图案，将所述相机获得的图案根据所述相机的分布在天花板的位置关系组合，获得所述机器人的第二类运行轨迹图案。

2.如权利要求1所述的多摄像头检测运动轨迹的方法，其特征在于：所述非对称图案是矩形的二维码图案，选择所述二维码图案的四个顶点作为角点，所述步骤S140包括以下步骤：

S141、获得地面坐标系中的地面相对于每个所述相机的成像平面在对应的所述相机坐标系中的第二类单应矩阵；

S142、通过所述第二类单应矩阵获得每个相机坐标系与地面坐标系之间的旋转量和平移矢量，以及每个所述相机坐标系下地面的方程；

S143、设所述相机坐标系中，每个角点所在的平行于地面的第一平面的方程与所述地面的方程之间分别具有四个不同高度；所述相机坐标系下的四条反投影射线与四个所述第一平面的方程的四个交点的坐标，所述交点的坐标满足以下条件：位于所述二维码图案一边上的两个角点之间的第一距离等于所述二维码图案的已知边长；并且位于所述二维码图案一对角线上的两个角点之间的第二距离等于所述二维码图案的已知对角线长；

S144、设对应所述二维码图案的四个第一距离与两个所述第二距离的和为第一总和，所述二维码图案四条已知边长与两条已知对角线的和为第二总和，获得满足第一总和减去第二总和的差最小时的四个所述高度误差；

S145、获得在所述相机坐标系中所述四个角点的坐标。

3.如权利要求2所述的多摄像头检测运动轨迹的方法，其特征在于：所述步骤S150包括以下步骤：

S151、将所述四个角点在所述相机坐标系中各自的坐标通过旋转和平移获得所述四个角点在所述地面坐标系中各自的坐标；

S152、根据所述四个角点在所述图案坐标系中各自的坐标和在地面坐标系中各自的坐标，计算两个坐标系之间的旋转量和平移矢量；

S153、驱动所述机器人进行自转，拍摄自转形成的圆形自转轨迹，获得所述圆形自转轨迹的半径趋近于零时，所述机器人自转中心在X轴上相对于中心坐标的偏移量和在Y轴上相对于中心坐标的偏移量。

4.如权利要求2所述的多摄像头检测运动轨迹的方法，其特征在于：所述步骤S140中，相邻的所述相机具有公共视野；

所述步骤S160中，相邻的所述相机获得的两个第一类运行轨迹中位于公共视野区域的轨迹相同，根据所述相机的分布在所述天花板的位置关系排列所有所述第一类运行轨迹图案，相邻所述相机的所述第一类运行轨迹图案之间轨迹相同的部分重叠。

5.如权利要求2所述的多摄像头检测运动轨迹的方法，其特征在于：所述步骤S130包括以下步骤：

S131、在每个所述相机获得的所述网格图案的照片中各自建立相机坐标系，基于所述照片中的网格图案在所述相机坐标系中设置多个图像角点，获得每个所述相机坐标系所述图像角点所在的坐标位置(X_c，Y_c)；

S132、对每个所述相机获得的照片分别做图像畸变校正，并获得所述图像角点映射到所述地面坐标系的地面角点的坐标位置(X_g，Y_g)，并建立每个所述相机坐标系中的每个图像角点与其对应的地面坐标的映射关系；

S133、设表达图像角点在相机坐标系的第一矩阵表达该图像角点映射到地面坐标系的第二矩阵每个所述相机的镜头具有3行3列的单应矩阵为H_i，根据P_g＝H_i×P_c，分别获得每个镜头与地面间的单应矩阵H_i，i大于1；

S134、根据拍摄到同一个所述地面角点的所述相机，建立关于每个地面角点的空间映射序列，设的空间映射序列为其中，为地面坐标系中的第k个点的矩阵，H_i为拍摄到同一个所述地面角点的第i个相机与地面间的单应矩阵，为所述同一个所述地面角点映射于第i个相机的相机坐标系中的第w个图像角点所在的坐标位置的矩阵，m，n，w为该所述地面角点映射的图像角点在该相机拍摄到的所有图像角点中的序号；

S135、根据所述每个地面角点的空间映射序列优化每个所述镜头的单应矩阵H_i。

6.如权利要求5所述的多摄像头检测运动轨迹的方法，其特征在于：根据空间映射序列优化单应矩阵H_i的步骤为：

设与对应的空间映射序列中每个元素的差值的总和作为第一参数T_k，

设所述地面坐标系中每个点的第一参数T_k的总和作为第二参数S，S＝T₁+T₂+......+T_k；

求当第二参数S的值最小时，每个相机的单应矩阵H_i。

7.如权利要求6所述的多摄像头检测运动轨迹的方法，其特征在于：所述单应矩阵H_i中第三行第三列的元素值为1。

8.如权利要求1所述的多摄像头检测运动轨迹的方法，其特征在于：所述相机吊装于所述地面之上的天花板底面。

9.如权利要求8所述的多摄像头检测运动轨迹的方法，其特征在于：所述相机的镜头是以下镜头中的一种：

焦距为24mm至38mm的广角镜头；

焦距为13mm至24mm的超广角镜头；

焦距为6mm至17mm的鱼眼镜头。

10.如权利要求1所述的多摄像头检测运动轨迹的方法，其特征在于：所述步骤S150之后还包括步骤S260，所述步骤S260包括以下步骤：

S261、将一个相机作为基准相机，其余每个所述相机相对于所述基准相机具有偏移时长和时长系数；

S262、建立第一参考坐标系，所述第一参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿所述第二类运行轨迹图案中的X轴方向的第三类轨迹，根据所述相机拍摄到机器人的时间顺序在第一参考坐标系排列所有所述第三类运行轨迹图案，建立第二参考坐标系，所述第二参考坐标系的X轴表示时间，Y轴表示机器人在每个时间段内多个相机同时观察到的沿所述第二类运行轨迹图案中的Y轴方向的第四类轨迹，根据所述相机拍摄到机器人的时间顺序在第二参考坐标系排列所有所述第四类轨迹图案；

S263、分别在所述第一参考坐标系和第二参考坐标系进行时间对齐，获得同时满足将相邻的所述第三类轨迹图案之间轨迹相同的部分重叠，并且相邻的所述第三类轨迹图案之间轨迹相同的部分重叠时的偏移时长和时长系数。

11.一种多摄像头检测运动轨迹的设备，其特征在于，包括：

处理器；

存储器，其中存储有所述处理器的可执行指令；

其中，所述处理器配置为经由执行所述可执行指令来执行权利要求1至10中任意一项所述多摄像头检测运动轨迹的方法的步骤。

12.一种计算机可读存储介质，用于存储程序，其特征在于，所述程序被执行时实现权利要求1至10中任意一项所述多摄像头检测运动轨迹的方法的步骤。