物体间位置姿态关系实时统一系统及方法与流程

本发明涉及摄像测量技术领域，尤其涉及一种物体间位置姿态关系实时统一系统及方法。

背景技术：

随着图像处理、光学测量和计算机视觉等学科算法的发展，以及摄像机、计算机、数字信号处理器(digital signal processor，DSP)等硬件设备的性能增加、成本降低，摄像测量在越来越多的领域得到了广泛的应用。

在某些特定的测量情况下，需要对各个待测物体进行单独的位置姿态参数测量，并联立其相互的位置姿态参数解得所需信息。但由于摄像机镜头的视场有限，或者两个物体间存在物体隔挡的情况，使得两物体不能由同一台摄像机进行采图处理，这时就需要多台摄像机对各个物体进行单独处理。例如在非接触四轮定位领域，就需要联立四个车轮之间的位置姿态关系来解得汽车四轮定位所需的所有定位参数；但由于汽车车身的遮挡以及有限的四轮定位空间，单个测量装置不能测得四个车轮的位置姿态参数，这时就需要联立多个测量装置的测量结果来求得汽车四轮定位所需的所有定位参数。

当两待测物体的位置相对固定时，已有的方法通过全站仪或者相关标定设备去标定两物体参数测量模块的位置姿态关系，从而求出两待测物体之间的位置姿态关系。但当两待测物体间的相对位置发生多次变化时，再利用上述方法去进行位置姿态关系测量，测量操作复杂，耗费较多的人力物力，并且可能由于人为因素的干扰而产生较大的测量误差。

上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。

技术实现要素：

本发明的主要目的在于提供一种物体间位置姿态关系实时统一系统及方法，旨在解决现有技术中测量操作复杂、耗费人力物力且测量误差大的技术问题。

为实现上述目的，本发明提供一种物体间位置姿态关系实时统一系统，所述系统包括：两个位姿测量装置、相对位姿采集装置和上位机，所述两个位姿测量装置分为第一位姿测量装置和第二位姿测量装置；

所述相对位姿采集装置，用于采集所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系；

所述第一位姿测量装置，用于获取第一物体的第一图像，并将所述第一图像发送至所述上位机；

所述第二位姿测量装置，用于获取第二物体的第二图像，并将所述第二图像发送至所述上位机；

所述上位机，用于根据所述第一图像确定所述第一位姿测量装置和第一物体之间的第一位置姿态关系，根据所述第二图像确定所述第二位姿测量装置和第二物体之间的第二位置姿态关系，根据所述第一位置姿态关系、第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中。

优选地，所述相对位姿采集装置包括：摄像机和合作标志靶，所述摄像机和合作标志靶分别设于所述第一位姿测量装置和第二位姿测量装置上，所述摄像机与所述合作标志靶相对设置；

所述摄像机，用于获取所述合作标志靶的第三图像，并将所述第三图像发送至所述上位机；

所述上位机，还用于根据所述第三图像确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系。

优选地，所述上位机，还用于获取所述第一位姿测量装置和摄像机之间的第三位置姿态关系、以及所述第二位姿测量装置和合作标志靶之间的第四位置姿态关系，并根据所述第一位置姿态关系、第二位置姿态关系、第三位置姿态关系、第四位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中。

优选地，所述上位机，还用于根据所述第三图像采用正交迭代算法确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系。

此外，为实现上述目的，本发明还提供一种物体间位置姿态关系实时统一方法，所述方法包括：

相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系；

所述第一位姿测量装置获取第一物体的第一图像，并将所述第一图像发送至上位机，所述第二位姿测量装置获取第二物体的第二图像，并将所述第二图像发送至所述上位机；

所述上位机根据所述第一图像确定所述第一位姿测量装置和第一物体之间的第一位置姿态关系，根据所述第二图像确定所述第二位姿测量装置和第二物体之间的第二位置姿态关系；

所述上位机根据所述第一位置姿态关系、第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中。

优选地，所述相对位姿采集装置包括：摄像机和合作标志靶，所述摄像机和合作标志靶分别设于所述第一位姿测量装置和第二位姿测量装置上，所述摄像机与所述合作标志靶相对设置；

相应地，所述相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系，具体包括：

所述摄像机获取所述合作标志靶的第三图像，并将所述第三图像发送至所述上位机；

所述上位机根据所述第三图像确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系。

优选地，所述相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系之前，所述方法还包括：

所述上位机获取所述第一位姿测量装置和摄像机之间的第三位置姿态关系、以及所述第二位姿测量装置和合作标志靶之间的第四位置姿态关系；

相应地，所述上位机根据所述第一位置姿态关系、第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中，具体包括：

所述上位机根据所述第一位置姿态关系、第二位置姿态关系、第三位置姿态关系、第四位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中。

优选地，所述上位机根据所述第三图像确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系，具体包括：

所述上位机根据所述第三图像采用正交迭代算法确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系。

本发明通过相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置的相对位置姿态关系，从而能够根据第一位姿测量装置和第一物体之间的第一位置姿态关系、第二位姿测量装置和第二物体之间的第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中，不需进行多余的操作，即可在对单个物体进行参数测量的同时，将两个相互独立且相对位置自由物体的位置姿态参数统一到同一个坐标系之下，便于联立两待测物体的测量参数进行结果的处理，操作简单便捷，适应范围广，且转换精度较高。

附图说明

图1为本发明一种实施例的物体间位置姿态关系实时统一系统的功能模块示意图；

图2为图1所示的物体间位置姿态关系实时统一系统的具体结构示意图；

图3为图1所示的物体间位置姿态关系实时统一系统中第二位姿测量装置的工作原理图；

图4为图1所示的物体间位置姿态关系实时统一系统中第一位姿测量装置的工作原理图；

图5为本发明一种实施例的物体间位置姿态关系实时统一方法的流程示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参照图1，本发明第一实施例提供一种物体间位置姿态关系实时统一系统，所述系统包括：两个位姿测量装置、相对位姿采集装置101和上位机102，所述两个位姿测量装置分为第一位姿测量装置103和第二位姿测量装置104；

所述相对位姿采集装置101，用于采集所述第一位姿测量装置103和第二位姿测量装置104之间的相对位置姿态关系；

需要说明的是，所述相对位置采集装置101可采用多种方式实现，例如：全站仪或者相关标定设备等设备实现，参照图2～4，所述相对位置采集装置101可通过图像采集的方式来实现，也就是说，所述相对位姿采集装置101包括：摄像机1011和合作标志靶1012，所述摄像机1011和合作标志靶1012分别设于所述第一位姿测量装置103和第二位姿测量装置104上，所述摄像机1011与所述合作标志靶1012相对设置；

所述摄像机1011，用于获取所述合作标志靶1012的第三图像，并将所述第三图像发送至所述上位机102；

所述上位机102，还用于根据所述第三图像确定所述第一位姿测量装置103和第二位姿测量装置104之间的相对位置姿态关系。

为保证相对位置姿态关系的确定准确率，在具体实现中，所述合作标志靶1012可采用黑白相间的棋盘格标志靶。

为便于调整所述第一位姿测量装置103和第二位姿测量装置104之间的相对位置姿态关系，可设置一个移动平台H1，所述第一位姿测量装置103设于所述移动平台H1上，能够沿着所述移动平台H1进行滑动。

为便于实现与上位机的互联，所述第一位姿测量装置103、第二位姿测量装置104和摄像机1011通过交换机或者网线与上位机102相连。

为便于确定第三位置姿态关系和第四位置姿态关系，可对摄像机1011、第一位姿测量装置103与摄像机1011的固连体、以及第二位姿测量装置104与合作标志靶1012的固连体进行标定，即可确定摄像机1011的内参数以及第一位姿测量装置103与摄像机1011、以及第二位姿测量装置104与合作标志靶1012之间的位置姿态关系。

其中，摄像机1011标定可采用张正友标定方法。在以合作标志靶1012为基础建立的三维坐标系下，合作标志靶1012中每个点的三维坐标都可确定；同时，在固定好摄像机拍摄位置后，合作标志靶1012上的各个点在图像中的位置也能解算得到。依据这些特性，根据合作标志靶1012上各个点的三维坐标以及通过图像处理手段获得对应点在图像中的二维坐标，通过二维坐标和对应点的合作标志靶的三维坐标可建立透视变换方程，即能解得摄像机的主点、焦距和畸变。其中，主点是指摄像机光轴与图像平面的交点；焦距是摄像机光心与主点之间的距离；畸变是指一些图像畸变系数。

其中，第一位姿测量装置103与摄像机1011、以及第二位姿测量装置104与合作标志靶1012之间的位置姿态关系标定可采用机器人手眼标定的两步法。以第一位姿测量装置103和摄像机1011之间的位置姿态关系标定为例。固定某一物体和合作标志靶1012的位置，使得第一位姿测量装置103能测量物体的参数、摄像机1011能采集合作标志靶1012的图像，记录初始位置的物体参数及合作标志靶1012图像；变换第一位姿测量装置103和摄像机1011的位置，再次采集上述参数；重复上述步骤；通过上述步骤可得到多组物体参数和对应的棋盘格图像，通过对之建立位置姿态方程，可解得第一位姿测量装置103和摄像机1011之间的位置姿态关系。

确定相对位置姿态关系可采用正交迭代算法，正交迭代算法迭代次数少，计算精度高，鲁棒性好，具有全局收敛能力，并可实现实时在线位姿估计，也就是说，本实施例中，所述上位机102，还用于根据所述第三图像采用正交迭代算法确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系。

所述第一位姿测量装置103，用于获取第一物体105的第一图像，并将所述第一图像发送至所述上位机；

所述第二位姿测量装置104，用于获取第二物体106的第二图像，并将所述第二图像发送至所述上位机；

为便于获取第一图像和第二图像，可理解的是，所述第一位姿测量装置103和第二位姿测量装置104的实时位置可根据第一物体105和第二物体106具体所在位置来决定。

所述上位机102，用于根据所述第一图像确定所述第一位姿测量装置103和第一物体105之间的第一位置姿态关系，根据所述第二图像确定所述第二位姿测量装置104和第二物体106之间的第二位置姿态关系，根据所述第一位置姿态关系、第二位置姿态关系及相对位置姿态关系将所述第一物体105和第二物体106转换至同一坐标系中。

虽然所述摄像机1011和合作标志靶1012分别设于所述第一位姿测量装置103和第二位姿测量装置104上，但所述摄像机1011和第一位姿测量装置103之间以及合作标志靶1012和第二位姿测量装置104之间均存在一定距离，假设将摄像机1011的位置作为所述第一位姿测量装置103所处位置，并将合作标志靶1012作为第二位姿测量装置104所处位置，非常容易使得物体间位置姿态关系获取不准确，为避免该问题，本实施例中，所述上位机102，还可用于获取所述第一位姿测量装置和摄像机之间的第三位置姿态关系、以及所述第二位姿测量装置和合作标志靶之间的第四位置姿态关系，并根据所述第一位置姿态关系、第二位置姿态关系、第三位置姿态关系、第四位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中。

需要说明的是，将所述第一物体105和第二物体106转换至同一坐标系中，可理解为，将所述第一物体105转换至第二物体106所处的坐标系中，又或是，将所述第二物体106转换至第一物体105所处的坐标系中。

本实施例通过相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置的相对位置姿态关系，从而能够根据第一位姿测量装置和第一物体之间的第一位置姿态关系、第二位姿测量装置和第二物体之间的第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中，不需进行多余的操作，即可在对单个物体进行参数测量的同时，将两个相互独立且相对位置自由物体的位置姿态参数统一到同一个坐标系之下，便于联立两待测物体的测量参数进行结果的处理，操作简单便捷，适应范围广，且转换精度较高。

参照图5，本发明第一实施例提供一种物体间位置姿态关系实时统一方法，所述方法包括：

S10：相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系；

S20：所述第一位姿测量装置获取第一物体的第一图像，并将所述第一图像发送至上位机，所述第二位姿测量装置获取第二物体的第二图像，并将所述第二图像发送至所述上位机；

S30：所述上位机根据所述第一图像确定所述第一位姿测量装置和第一物体之间的第一位置姿态关系，根据所述第二图像确定所述第二位姿测量装置和第二物体之间的第二位置姿态关系，根据所述第一位置姿态关系、第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中。

本实施例通过相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置的相对位置姿态关系，从而能够根据第一位姿测量装置和第一物体之间的第一位置姿态关系、第二位姿测量装置和第二物体之间的第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中，不需进行多余的操作，即可在对单个物体进行参数测量的同时，将两个相互独立且相对位置自由物体的位置姿态参数统一到同一个坐标系之下，便于联立两待测物体的测量参数进行结果的处理，操作简单便捷，适应范围广，且转换精度较高。

需要说明的是，所述相对位置采集装置可采用多种方式实现，例如：红全站仪或者相关标定设备等设备实现，所述相对位置采集装置可通过图像采集的方式来实现，也就是说，所述相对位姿采集装置包括：摄像机和合作标志靶，所述摄像机和合作标志靶分别设于所述第一位姿测量装置和第二位姿测量装置上，所述摄像机与所述合作标志靶相对设置；

所述摄像机，用于获取所述合作标志靶的第三图像，并将所述第三图像发送至所述上位机；

所述上位机，还用于根据所述第三图像确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系。

虽然所述摄像机和合作标志靶分别设于所述第一位姿测量装置和第二位姿测量装置上，但所述摄像机和第一位姿测量装置之间以及合作标志靶和第二位姿测量装置之间均存在一定距离，假设将摄像机的位置作为所述第一位姿测量装置所处位置，并将合作标志靶作为第二位姿测量装置所处位置，非常容易使得物体间位置姿态关系获取不准确，为避免该问题，本实施例中，所述相对位姿采集装置采集第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系之前，所述方法还包括：

所述上位机获取所述第一位姿测量装置和摄像机之间的第三位置姿态关系、以及所述第二位姿测量装置和合作标志靶之间的第四位置姿态关系；

相应地，所述上位机根据所述第一位置姿态关系、第二位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中，具体包括：

所述上位机根据所述第一位置姿态关系、第二位置姿态关系、第三位置姿态关系、第四位置姿态关系及相对位置姿态关系将所述第一物体和第二物体转换至同一坐标系中。

确定相对位置姿态关系可采用正交迭代算法，正交迭代算法迭代次数少，计算精度高，鲁棒性好，具有全局收敛能力，并可实现实时在线位姿估计，也就是说，所述上位机根据所述第三图像确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系，具体包括：所述上位机根据所述第三图像采用正交迭代算法确定所述第一位姿测量装置和第二位姿测量装置之间的相对位置姿态关系。

为保证相对位置姿态关系的确定准确率，在具体实现中，所述合作标志靶为棋盘格标志靶。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。