用于双目头眼协调运动的控制系统及其定位方法与流程

本发明属于仿生机器人控制领域，具体提供一种用于双目头眼协调运动的控制系统及其定位方法。

背景技术：

眼睛是人类获取外界信息最重要的感官，担负着人类80％左右的信息获取任务。经过千百万年的进化，人类的视觉系统已高度发达和完善，形成了极其精确和完备的机制，具备了适应内外环境变化的能力。因此，给机器人和机器视觉设备配备智能双眼，使其像人类一样感知和获取环境信息、快速准确地识别和跟踪目标，是很多国内外科研工作者目前正在致力于的研究方向之一。

然而，由于人类视觉系统和神经回路的复杂性，尚没有较好的模型可以重现视觉系统的运动机理，只能在一定程度上模拟眼或者头部的运动；此外，从神经通路推导出来的模型大多数是生理学实验或者仿真获得的，工程应用还有一定的困难。

相应地，本领域需要一种新的头眼协调运动的控制系统及定位方法来解决上述问题。

技术实现要素：

为了解决现有技术中的上述问题，即为了解决现有头眼协调运动控制系统在工程实际应用中比较困难的问题，本发明提供了一种用于双目头眼协调运动的控制系统及其定位方法，该控制系统包括转动构件，所述转动构件上设置有：第一相机以及驱动第一相机相对于所述转动构件做俯仰和偏转动作的第一驱动装置；第二相机以及驱动第二相机相对于所述转动板做俯仰和偏转动作的第二驱动装置。

在上述控制系统的优选技术方案中，所述第一驱动装置包括用于驱动所述第一相机实现俯仰动作的第一电机和用于驱动所述第一相机实现偏转动作的第二电机，并且所述第一电机上设置有第一旋转编码器，所述第二电机上设置有第二旋转编码器。

在上述控制系统的优选技术方案中，所述第二驱动装置包括用于驱动所述第二相机实现俯仰动作的第三电机和用于驱动所述第二相机实现偏转动作的第四电机，并且所述第三电机上设置有第三旋转编码器，所述第四电机上设置有第四旋转编码器。

在上述控制系统的优选技术方案中，所述第一、第二、第三和第四电机均是步进电机；并且/或者所述第一、第二、第三和第四电机上均设置有限位开关；并且/或者所述第一、第二、第三和第四旋转编码器均是光电式旋转编码器。

在上述控制系统的优选技术方案中，所述控制系统还包括旋转架，所述转动构件可旋转地安装到所述旋转架上，并且所述旋转架的侧面设置有驱动所述转动构件转动的第三驱动装置。

在上述控制系统的优选技术方案中，所述第三驱动装置包括第五电机，所述第五电机用于驱动所述转动构件转动；并且所述第五电机上设置有第五旋转编码器。

在上述控制系统的优选技术方案中，所述第三驱动装置还包括蜗轮蜗杆副，所述第五电机通过蜗轮蜗杆副驱动所述转动构件转动。

在上述控制系统的优选技术方案中，所述旋转架的底部设置有驱动所述旋转架旋转的第四驱动装置；并且所述第四驱动装置包括第六电机；并且所述第六电机上设置有第六旋转编码器。

在上述控制系统的优选技术方案中，所述第五和第六电机均是步进电机；并且/或者所述第五和第六电机上均设置有限位开关；并且/或者所述第五和第六旋转编码器均是光电式旋转编码器。

在上述控制系统的优选技术方案中，所述转动构件上在所述第一相机和所述第二相机之间设置有视觉定位板，所述第一相机和所述第二相机通过定位板进行初始定位。

在另一方面，本发明提供了一种用于双目头眼协调运动的定位方法，所述定位方法包括以下步骤：将两个相机分别进行初始定位；通过离线标定的相机内参数和手眼参数实时获得两个相机之间的外参数矩阵。

在上述定位方法的优选技术方案中，所述将两个相机分别进行初始定位的步骤进一步包括：将控制两个相机的电机通电，使电机顺时针或逆时针旋转，当电机触动限位开关时，使电机停止运动，从而使两个相机被初步粗定位；采用基于椭圆中心拟合的视觉推算定位方法对两个相机进行精确初定位。

在上述定位方法的优选技术方案中，所述采用基于椭圆中心拟合的视觉推算定位方法对两个相机进行精确初定位的步骤进一步包括：在初始位置在视觉定位板上打印标准圆；采用一维Prewitt算子分别沿横向和纵向对图像进行扫描获取边缘点；对边缘点用最小二乘法进行椭圆拟合，获得椭圆中心；控制相机运动，使椭圆中心与相机像平面中心重合，从而完成视觉推算定位。

在上述定位方法的优选技术方案中，所述通过离线标定的相机内参数和手眼参数实时获得两台相机之间的外参数矩阵的步骤进一步包括：采用平面标定法分别获得两个相机的内参数和畸变参数；通过立体标定获得初始位置时第一相机相对于第二相机的外参数矩阵；通过手眼标定获得两个相机相对于各自运动模块末端的外参数。

在上述定位方法的优选技术方案中，所述通过离线标定的相机内参数和手眼参数实时获得两台相机之间的外参数矩阵的步骤进一步包括：根据初始位置时第一相机相对于第二相机的外参数矩阵和两个相机相对于各自运动模块末端的外参数，实时计算两个相机之间的外参数矩阵。

本领域技术人员能够理解的是，在本发明的优选技术方案中，通过步进电机分别驱动第一相机和第二相机相对于转动板做俯仰和偏转动作，模拟人的双眼运动；通过步进电机驱动相互啮合的蜗轮蜗杆带动转动板转动，模拟人头部的俯仰(抬头、低头)动作；通过步进电机驱动旋转架旋转，模拟人头部的转动(转头)动作。进一步，在每个步进电机上都设置有光电式旋转编码器和限位开关；当控制系统通电后，第一相机和第二相机由步进电机驱动旋转，触动限位开关时完成初步粗定位，然后根据椭圆中心拟合的视觉推算定位方法对两个相机进行精确初定位，最后通过离线标定的相机内参数和手眼参数实时获得两台相机之间的外参数矩阵。因此，本发明的控制系统只需在初始位置立体标定一次，就可以在相机运动过程中实时获得两台相机之间的外参数，能够被容易地应用于工程实践中。

附图说明

图1是本发明的用于双目头眼协调运动的控制系统的机械结构示意图；

图2是本发明的用于双目头眼协调运动的定位方法的步骤流程图；

图3是本发明的用于双目头眼协调运动的定位方法的初始定位的详细流程图；

图4是本发明的用于双目头眼协调运动的定位方法的两相机之间实时定位的详细流程图；

图5是采用本发明的双目头眼协调运动的定位方法和采用用立体视觉标定方法获得的两个相机之间的实时外参数实验结果的对比表格。

具体实施方式

下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。例如，虽然附图中的各部件之间是按一定比例关系绘制的，但是这种比例关系并非一成不变，本领域技术人员可以根据需要对其作出调整，以便适应具体的应用场合。

需要说明的是，在本发明的描述中，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方向或位置关系的术语是基于附图所示的方向或位置关系，这仅仅是为了便于描述，而不是指示或暗示所述装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

此外，还需要说明的是，在本发明的描述中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域技术人员而言，可根据具体情况理解上述术语在本发明中的具体含义。

如图1所示，本发明的用于双目头眼协调运动的控制系统在机械结构上主要包括：转动板1、旋转架2和底座3。转动板1的两端通过旋转装置11可旋转地固定到旋转架2上，优选地，旋转装置11是轴承转轴组，或者本领域技术人员也可以采用其他形式的转动件将转动板1和旋转架2可旋转地固定到一起，例如销钉。旋转架2的底端(图1中旋转架2的底端)可旋转地固定到底座3上。

继续参阅图1，转动板1上设置有第一相机41以及驱动第一相机41相对于转动板1做俯仰和偏转动作的第一驱动装置(图中未标示)、第二相机51以及驱动第二相机51相对于转动板1做俯仰和偏转动作的第二驱动装置(图中未标示)和视觉定位板6。在本发明的优选实施方案中，第一相机41和第二相机51用于模仿人的双眼。

具体地，第一驱动装置包括用于驱动第一相机41实现俯仰动作的第一电机42和用于驱动第一相机41实现偏转动作的第二电机44；第一电机42的伸出轴(图中未示出)垂直于第一相机41的镜头轴线方向并与第一相机41固定连接，使得第一电机42能够驱动第一相机41绕其伸出轴旋转，进而实现第一相机41的俯仰动作；第二电机44例如通过法兰与转动板1固定连接，其伸出轴(图中未示出)垂直于第一电机42的伸出轴并与第一电机42固定连接，使得第二电机44能够驱动第一电机42和第一相机41绕其伸出轴旋转，进而实现第一相机41的偏转动作。

类似地，第二驱动装置包括用于驱动第二相机51实现俯仰动作的第三电机52和用于驱动第二相机51实现偏转动作的第四电机54。第三电机52的伸出轴(图中未示出)垂直于第二相机51的镜头轴线方向并与第二相机51固定连接，使得第三电机52能够驱动第二相机51绕其伸出轴旋转，进而实现第二相机51的俯仰动作；第四电机54例如通过法兰与转动板1固定连接，其伸出轴(图中未示出)垂直于第三电机52的伸出轴并与第三电机52固定连接，使得第四电机54能够驱动第三电机52和第二相机51绕其伸出轴旋转，进而实现第二相机51的偏转动作。

进一步，电机42、44、52和54的转轴上与输出轴相对的另一端上分别设置有第一光电式旋转编码器43、第二光电式旋转编码器45、第三光电式旋转编码器53和第四光电式旋转编码器55，以便通过光电式旋转编码器43、45、53和55来实时监测电机42、44、52和54旋转的角度，进而精确控制第一相机41和第二相机51的俯仰、偏转角度。

如图1所示，视觉定位板6固定在转动板1上，并且位于第一相机41和第二相机51之间，优选地，视觉定位板6位于第二电机44和第四电机54伸出轴在转动板1上连线的中间位置。进一步，视觉定位板6上设置有圆孔61，优选地，当第一相机41的镜头和第二相机51的镜头同时面对视觉定位板6时，其轴线能够同时与该圆孔61的轴线平行。

继续参阅图1，在旋转架2的一侧(图1中旋转架2的左侧)设置有用于驱动转动板1转动的第三驱动装置(图中未标示)，第三驱动装置用于驱动转动板1来模拟人头部的俯仰(抬头、低头)动作。具体地，第三驱动装置包括第五电机71和与第五电机71的一端固定连接的第五光电式旋转编码器72。进一步，第三驱动装置和转动板1之间还设置有蜗轮蜗杆副8。由于蜗轮与蜗杆之间具有自锁功能，使得当转动板1旋转至一定角度时，能够通过蜗轮蜗杆副8进行锁定。作为示例，蜗杆与第五电机71的伸出轴(图中未示出)固定连接，蜗轮与旋转装置11固定连接，使得当第五电机71驱动蜗杆旋转时，蜗轮会通过旋转装置11驱动转动板1旋转。本领域技术人员也可以根据需要将蜗轮与第五电机71的伸出轴固定连接，将蜗杆与旋转装置11固定连接。

进一步参阅图1，旋转架2的底端设置有驱动其旋转的第四驱动装置(图中未标示)，该第四驱动装置包括第六电机91和与第六电机91的一端固定连接的第六光电式旋转编码器92。第六电机91通过螺栓与底座3固定连接，第六电机91的伸出轴(图中未示出)与旋转架2固定连接。第六电机91驱动旋转架2旋转，用以模拟人头部的旋转(左右扭头)动作。

优选地，电机42、44、52、54、71和91均是步进电机，或者本领域技术人员也可以根据需要选用伺服电机等其他电机。

另外，电机42、44、52、54、71和91上还分别设置有限位开关(图中未示出)，当电机旋转并触动限位开关时，控制系统会控制电机停止旋转。在本发明的优选实施方案中，限位开关用于当第一相机41和第二相机51旋转至视觉定位板6上的圆孔处时，使所有电机停止转动，进而对第一相机41和第二相机51实现初步粗定位。

本领域技术人员能够理解的是，在保证第一相机41与第二相机51俯仰、偏转精度的前提下和转动板1转动精度以及旋转架2旋转精度的前提下，上述旋转编码器43、45、53、55、72和92还可以采用其他任何适当的形式，例如绝对值编码器等。

综上所述，当本发明的用于双目头眼协调运动的控制系统动作时，第一相机41和第二相机51，先通过视觉定位板6进行初步粗定位，然后控制系统控制电机42、44、52、54驱动第一相机41和第二相机51俯仰或偏转，模拟人的双眼动作，控制电机71驱动转动板1转动，模拟人头部的俯仰动作，控制电机91驱动旋转架2旋转，模拟人头部的扭动旋转动作。需要说明的是，上述动作既可以是单独进行也可以同时进行。

如图2所示，本发明的用于双目头眼协调运动的控制系统中的双目头眼协调运动的定位方法主要包括以下步骤：步骤S100，将两个相机分别进行初始定位；步骤S200，通过离线标定的相机内参数和手眼参数实时获得两台相机之间的外参数矩阵。

如图3所示，步骤S100又具体包括：步骤S110，将控制两个相机的电机通电，使电机顺时针或逆时针旋转，当电机触动限位开关时，电机停止运动，两个相机被初步粗定位；步骤S120，采用基于椭圆中心拟合的视觉推算定位方法对两个相机进行精确初定位。

在步骤S110中，由于电机42、44、52和54在断电后不具有保持功能，使得第一相机41和第二相机51在自身重力和其他外力的作用下，会迫使电机42、44、52和54产生相应的角位移，所以当控制系统通电之后需要对电机42、44、52和54的角度进行重新调整和对第一相机41和第二相机51进行重新定位。具体地，先将控制系统通电，控制系统会控制电机42、44、52和54沿某一方向(顺时针或逆时针)转动，当电机42、44、52和54分别触动限位开关时停止转动。此时，第一相机41和第二相机51的镜头分别面向视觉定位板6，并与其平行，第一相机41和第二相机51完成初步粗定位。

本领域技术人员能够理解的是，电机42、44、52和54的初始转动方向可事先在控制程序中设定好，也可以根据控制系统检测到的光电式旋转编码器43、45、53和55的角度信息，粗略确定电机42、44、52和54的角度位置，从而通过控制系统驱动电机42、44、52和54向与各自的限位开关角度距离较小的方向旋转。

在步骤S120中，由于安装误差、连接结构形变和畸变，当第一相机41和第二相机51分别面向视觉定位板6时，视觉定位板6的平面与相机成像平面很难平行，导致两个相机41、51各自镜头采集到的圆孔61并非是一个圆形，而是一个椭圆形。基于此，采用椭圆中心拟合的视觉推算定位方法对第一相机41和第二相机51分别进行精确初定位。电机42、44、52和54转过固定角度后(如步骤S110所述)，第一相机41和第二相机51能够分别捕捉到视觉定位板6上的圆孔61，进而，第一相机41和第二相机51分别采集圆孔61的图像并对其进行处理，提取出图形的中心。更具体地，采用一维Prewitt算子分别沿横向和纵向对图像进行扫描获取边缘，经过阈值处理获取进行椭圆拟合的边缘点。当采集到的图形中心不在相机41、51的图像中心时，控制系统控制电机42、44、52和54转动，使采集到的图形中心与相机41、51的图像中心重合并保持稳定。

需要说明的是，Prewitt算子是一种一阶微分算子的边缘检测方法，其利用像素点上下、左右邻点的灰度差，在边缘处达到极值检测边缘，去掉部分伪边缘，对噪声具有平滑作用。其原理是在图像空间利用两个方向模板与图像进行邻域卷积来完成的，这两个方向模板一个检测水平边缘，一个检测垂直边缘。

还需要说明的是，最小二乘法是最早的椭圆拟合方法，它是数据拟合中的基本方法，它的基本思想就是考虑数据受随机噪声的影响进而追求整体误差的最小化。对椭圆拟合而言，就是先假设椭圆参数，得到每个待拟合点到该椭圆的距离之和，也就是点到假设椭圆的误差，求出使这个和最小的参数。

如图4所示，步骤S200又具体包括：步骤S210，采用平面标定法分别获得第一相机41和第二相机51的内参数和畸变参数；步骤S220，通过立体标定获得初始位置时第一相机41相对于第二相机51的外参数矩阵；步骤S230，通过手眼标定获得两个相机相对于各自运动模块末端的外参数；步骤S240；根据初始位置时第一相机41相对于第二相机51的外参数矩阵和两个相机相对于各自运动模块末端的外参数，实时计算两个相机之间的外参数矩阵。

具体地，在步骤S210中，采用平面标定法获得第一相机41和第二相机51的内参数M_inL和M_inR以及畸变参数K_dL和K_dR，以便第一相机41和第二相机51能够通过调节其焦距来使其获取到的图像尽可能地相同。需要说明的是，相机内参数的意义在于：对于从相机坐标系到像平面坐标系的变换，像平面坐标系是用像素单位来表示的，而相机坐标系则是以毫米为单位来表示，因此，要完成改变换过程就需要先得到像平面的像素单位与毫米单位之间的线性关系；相机畸变是由相机镜头设计造成的。

具体地，在步骤S220中，通过立体标定获得初始位置时第一相机41相对于第二相机51的变换旋转矩阵R和平移向量p。为了确保R和p的准确性，在本发明的优选实施方案中通过重复实验获得R和p的一组数据，并将与这组数据中的其他数据都比较靠近的R和p作为真实结果的初始近似值。接着运行稳健的L-M迭代算法查找出棋盘角点在两个相机41、51视图上的最小投影误差，进而得到R和p的结果。

需要说明的是，L-M迭代算法是一种非线性规划方法，其主要用于无约束的多维非线性规划问题，其是一阶牛顿法的一种改进，改进的目的是为了更快的收敛。

在步骤S230中，通过手眼标定获得两个相机41和51相对于其各自运动模块(驱动两个相机41和51转动的电机42、44、52和54)末端的外参数T_mL和T_mR。手眼标定是本领域技术人员熟知的操作，可以采用本领域已知的任意合适的方法来实现，此处不再赘述。

在步骤S240中，根据初始位置时第一相机41相对于第二相机51的外参数矩阵和两个相机41、51相对于各自运动模块末端的外参数，实时计算两个相机之间的外参数矩阵，其计算公式如下：

其中，Ti11是第一相机41相对于第二相机51的旋转矩阵：

Ti22是两个相机41、51相对于各自运动模块末端的平移向量：

R_ml是第一相机41相对于第一运动模块的旋转矩阵，R_mr是第二相机51相对于第二运动模块的旋转矩阵。R_il是第一运动模块末端转过一定角度后，其相对于第一运动模块基坐标系的旋转矩阵；R_ir是第二运动模块末端转过一定角度后，其相对于第二运动模块基坐标系的旋转矩阵。R是初始位置时，第一相机41坐标系相对于第二相机51坐标系的旋转矩阵。

如图5所示，该图示出了实时计算两个相机41和51之间外参数的实验结果。具体地，两个运动模块分别控制两个相机41和51运动到160个不同的位置，分别用立体视觉标定的方法和利用本发明提供的方法计算得到两个相机坐标系之间的变换关系，共160组数据。为了显示方便，每隔一定距离截取一组数据，共计20组数据。计算两个方法所获得数据之间的角度偏差和平移偏差，其中，角度偏差是通过对两个相机41、51的旋转变换矩阵求通用旋转变换的转角得到的，平移偏差是立体标定的平移向量和计算得到的平移向量的欧氏距离。从图4中不难看出，本发明的优选实施方案和立体标定得到的值比较接近，然而，本发明的方法避免了相机41、51在工作过程中边工作边标定，只需在相机41、51初始位置立体标定一次，就可以在相机运动过程中实时得到两个相机41、51之间的外参数。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。