CN109693235A - 一种仿人眼视觉跟踪装置及其控制方法 - Google Patents

Abstract

本发明涉及一种仿人眼视觉跟踪装置及其控制方法，包括头颈机器人本体，包括3个自由度旋转关节和CCD相机，用于通过旋转关节调整CCD相机的方位正对于跟踪目标；视觉计算模块，用于根据CCD相机采集的图像信息计算跟踪目标的位置信息，并返回给机器人控制器；机器人控制器，用于实时计算跟踪目标需要旋转的关节角度，发送给关节控制器进行跟踪。本发明采用一种几何分解的关节角求解方法，不同于常用的虚拟连杆方法，避免了冗余自由度机器人反解的多解问题；计算简单，求解效率高；并且引入了舒适角优化模型，在保证可行解的基础上，最大效率的利用机器人的动态特性。

Description

一种仿人眼视觉跟踪装置及其控制方法

技术领域

本发明涉及机器视觉、机器人控制和自适应控制领域，具体地说是一种仿人眼视觉跟踪装置及其控制方法。

背景技术

制造能够替代人劳动的机器一直是人类的梦想。但目前即使是世界上智能最高的机器人,对外部环境变化的适应能力也非常有限,距人们预想的目标还有很大距离,这极大地影响了机器人的推广使用。其中的一个重要原因就是机器人缺乏像人一样的感知能力。

随着机器视觉的快速发展，视觉作为一种有利的获取信息的方式被应用到诸多领域，这也给机器人感知带来了机遇。仿人头的机器人装置上视觉传感器后能获得广泛的环境信息，但是如何能够像人一样进行灵活的目标观测和跟踪涉及到了视觉伺服控制问题。

目前机器人视觉控制主要有两种方法，基于位置的视觉伺服和基于图像的视觉伺服。基于位置的方法，直接利用视觉传感器获得空间物体的笛卡尔坐标，然后进行控制。这种方法的优点是把视觉问题和机器人运动控制分开考虑，这样方便对二者单独研究。但是这种方法在进行视觉伺服反馈控制时涉及到机器人求逆、雅克比计算等任务。比如基于虚拟连杆的跟踪方法，在目标和相机坐标系之间添加一根虚拟的连杆，相机获得目标位置后，伺服控制问题就变成了一个冗余自由度的机器人控制问题，求解复杂。基于图像的伺服系统，伺服误差直接定义在图像特征空间。这样不需要对目标进行3D重构，跟踪误差受机器人运动学及标定误差的影响小。但是这种方法需要动态的估计图像雅克比，需要不断进行更新和求逆，计算复杂高。

发明内容

针对现有技术的不足，本发明提供一种仿人眼视觉跟踪装置及其控制方法，是一种针对3个自由度的机器人结构提出的几何分解的控制方法，不需要计算雅克比，直接利用几何投影计算控制量，计算简单，同时也保证了跟踪的实时性。

本发明为实现上述目的所采用的技术方案是：

一种仿人眼视觉跟踪装置，包括

头颈机器人本体，包括3个自由度旋转关节和CCD相机，用于通过旋转关节调整CCD相机的方位正对于跟踪目标；

视觉计算模块，用于根据CCD相机采集的图像信息计算跟踪目标的位置信息，并返回给机器人控制器；

机器人控制器，用于实时计算跟踪目标需要旋转的关节角度，发送给关节控制器进行跟踪。

所述3个自由度旋转关节包括：微俯仰关节、水平旋转关节和宏俯仰关节；其中

微俯仰关节，用于模拟人眼在垂直方向的小范围的旋转运动；

水平旋转关节，用于模拟人颈部在水平方向的旋转运动；

宏俯仰关节，用于模拟人颈部在垂直方向的大范围的旋转运动。

所述小范围为-15°～30°；所述旋转关节的旋转角度范围为：-90°～90°；所述大范围为0°～70°。

所述旋转关节包括：

制动器，用于使关节轴减速、停止或保持停止状态；

码盘，用于测量当前旋转关节的旋转角度；

关节驱动器，用于根据机器人控制器发送的控制命令驱动头颈机器人本体的旋转关节运动。

一种仿人眼视觉跟踪控制方法，包括以下步骤：

步骤1：初始化旋转关节位置；

步骤2：判断机器人视场的中心点是否与跟踪目标的位置一致，如果是，则停留等待，否则执行步骤3；

步骤3：通过视觉计算模块获取跟踪目标的位置信息，输出给机器人控制器，通过机器人控制器计算出旋转关节的旋转角度；

步骤4：关节驱动器根据得到的旋转关节的旋转角度驱动电机运动，进行目标跟踪；

步骤5：判断是否接收到终止跟踪命令，如果是，则跟踪结束，否则返回步骤2。

所述通过机器人控制器计算出旋转关节的旋转角度包括以下过程：

步骤1：通过D-H方法建立机器人的连杆坐标系，并根据D-H参数得到各个关节坐标系之间的变换关系；

步骤2：CCD相机捕捉到跟踪目标从始点S运动到末点D，通过视觉计算模块获取从始点S到末点D的始末坐标；

步骤3：计算水平旋转关节的旋转角度；

步骤4：建立舒适角优化模型，计算微俯仰关节和宏俯仰关节的旋转角度。

所述计算水平旋转关节的旋转角度包括以下过程：

步骤1：利用坐标变换关系得到始末点S、D相对于水平旋转关节坐标系的坐标；

步骤2：得出始末点S、D在水平旋转关节坐标系的XY平面的投影向量；

步骤3：根据投影向量计算水平旋转关节的旋转角度。

所述建立舒适角优化模型，计算微俯仰关节和宏俯仰关节的旋转角度包括以下步骤：

步骤1：利用坐标变换关系得到始末点S、D相对于宏俯仰关节坐标系的坐标S′,D′；

步骤2：将S′,D′投影到宏俯仰关节坐标系的XY平面，分别得到S′,D′对应的投影向量；

步骤3：设宏俯仰关节旋转一定角度，其始末状态分别对应微俯仰关节的始末状态，将微俯仰关节始末状态下对应坐标系的原点转换到宏俯仰关节坐标系下，并投影到其XY平面；

步骤4：在宏俯仰关节坐标系的XY平面，通过几何关系和舒适角优化模型得出宏俯仰关节和微俯仰关节的旋转角度。

所述通过几何关系和舒适角优化模型得出宏俯仰关节和微俯仰关节的旋转角度包括以下过程：

所述宏俯仰关节的旋转角度表达式为：

所述宏俯仰关节的旋转角度表达式为：

θ₃＝θ_3s+θ_3d

所述舒适角优化模型为：

其中，表示S′,D′在宏俯仰关节坐标系XY平面内的投影坐标，(x_A,y_A),(x,y)表示微俯仰关节始末状态下对应坐标系的原点在宏俯仰关节坐标系XY平面内的投影坐标，l表示宏俯仰关节和微俯仰关节坐标系原点的距离在宏俯仰关节XY平面上的投影长度，θ_3s,θ_3d分别表示微俯仰关节两次旋转的角度。λ_`1,λ₃表示宏俯仰关节和微俯仰关节的阻抗系数，表示宏俯仰关节和微俯仰关节的最佳舒适角度。

本发明具有以下有益效果及优点：

1.本发明采用一种几何分解的关节角求解方法，不同于常用的虚拟连杆方法，避免了冗余自由度机器人反解的多解问题；

2.本发明的几何分解的方法，计算简单，求解效率高；

3.本发明引入了舒适角优化模型，在保证可行解的基础上，最大效率的利用机器人的动态特性。

附图说明

图1是本发明的头颈机器人示意图；

图2是本发明的头颈机器人结构框图和控制框图；

图3是本发明的头颈跟踪装置控制架构图；

图4是本发明的机器人控制器算法流程图；

图5是本发明的水平旋转关节计算几何示意图；

图6是本发明的微俯仰关节和宏俯仰关节计算几何示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

如图1所示为本发明的头颈机器人示意图。

装置包含三个自由度，能够通过左右旋转和上下俯仰跟踪空间中的目标。3个自由度旋转关节包括：微俯仰关节、水平旋转关节和宏俯仰关节；其中微俯仰关节，用于模拟人眼在垂直方向-15°～30°角度范围内的旋转运动；水平旋转关节，用于模拟人颈部在水平方向-90°～90°角度范围内的旋转运动；宏俯仰关节，用于模拟人颈部在垂直方向0°～70°角度范围内的大范围的旋转运动。

如图2所示是本发明的头颈机器人控制框图。

机器人本体机械结构包括基座、三个旋转关节、相机安装座、CCD相机以及头颈外壳。三个关节采用模块化设计，每个关节包括电机、谐波减速器、制动器、编码器和关节驱动器。电机，用于独立驱动三个关节旋转。制动器，用于使关节轴减速、停止或保持停止状态。尤其在断电时，能够瞬间固定关节轴，防止自重引起的运动损坏。编码器，用于测量当前关节的旋转角度，反馈的角度信息经过正运动学计算，可以获得当前机器人的末端位置。关节驱动器，控制关节的运动。能够规划关节运动的速度、加速度以及运动轨迹。它们的连接关系如下：电机后端和制动器相连，制动器后端放置增量式编码器；电机前段连接谐波减速器，谐波减速器的另一端放置绝对编码器；三个关节驱动器固定在CCD相机安装座上。

控制过程如下：机器人控制器得到视觉计算模块的信息，计算输出三个关节的旋转角度。这三个角度指令依次发给每一个关节。每个关节里面有关节驱动器和电机，关节驱动器先接收到机器人控制器的指令，然后输出力矩或者电流信息驱动电机运动，完成跟踪。

如图3所示是本发明的头颈跟踪装置控制流程图。

组成单元包括头颈机器人本体、机器人控制器、视觉计算单元以及关节驱动器。当跟踪目标发生位移时，视觉计算单元通过图像处理和位姿估计，得到跟踪目标的位置坐标。机器人本体的传感器读取机器人当前的状态，通过正运动学得到机器人当前的位姿。机器人控制器输入机器人位姿和跟踪目标的位姿差，根据控制算法输出控制指令。机器人关节的关节驱动器接收机器人控制器的控制指令，驱动机器人本体跟踪目标。

如图4所示为本发明的机器人控制器算法流程图。

步骤1：初始化旋转关节位置；

步骤2：判断机器人视场的中心点是否与跟踪目标的位置一致，如果是，则停留等待，否则执行步骤3；

步骤3：通过视觉计算模块获取跟踪目标的位置信息，输出给机器人控制器，通过机器人控制器计算出旋转关节的旋转角度；

步骤4：关节驱动器根据得到的旋转关节的旋转角度驱动电机运动，进行目标跟踪；

步骤5：判断是否接收到终止跟踪命令，如果是，则跟踪结束，否则返回步骤2。

实施例

整个头颈机器人视觉伺服控制过程可以按照以下步骤实施，这里结合一个具体的例子，如跟踪物体从空间坐标S(30，-20,40)运动到D(20,10，-20)。跟踪过程如下：

如图5所示为本发明的水平旋转关节计算几何示意图。

首先，运用D-H方法建立机器人的连杆坐标系，参考坐标系为{0}，各个关节的坐标系记为{i}，i表示第i个关节，也即{1}、{2}、{3}分别表示关节1,2,3的坐标系。相对于i坐标系的坐标值表示为(x_i,y_i,z_i)。根据D-H参数可以得到各个关节坐标系之间的变换关系，用表示坐标系i相对于i-1坐标系的变换关系。另外，再定义相机的参考坐标系{Camera}，相应的视觉返回的空间物体的坐标应该是相对于相机的参考坐标的，表示为(x_car,y_car,z_car)。

始化关节位置，参见图5位置对应初始角度是[0°0°0°]。

此时目标位置不在视线中心，进入下一步。

CCD相机扑捉到空间跟踪目标相邻时刻从点S运动到D，通过时视觉控制器获得此次运动的始末坐标分别为：和

利用坐标变换关系得到S、D相对于坐标系{2}的坐标其中变换到坐标系{2}后对应的坐标是：(100,120,80)和(80，-20,60)。

得到S、D在X₂O₂Y₂平面的投影向量

计算第二个关节的转角θ₂，其中由此可得：

||a||,||b||表示投影距离。

利用坐标变换关系得到S、D相对于坐标系{1}的坐标

其中

如图6所示为本发明的微俯仰关节和宏俯仰关节计算几何示意图。

把S’、D’两点投影到X₁O₁Y₁平面，得到向量

通过坐标变换得到关节3相对于坐标系{1}的坐标，并将其投影到坐标系{1}的XOY平面，得到相应的投影点A、B。

A坐标通过坐标变换和投影，以及根据当前码盘角度读数可以计算出，设为(x_A,y_A)＝(-40,-80)，是已知的。B的坐标设为(x,y)，此时可以求出：同理得到：对应由反余弦可以求出相应的角。

根据实际需求，确定关节1和关节3的最佳舒适角度分别为60°和0°，并确定其阻抗系数λ，λ越大表示阻抗越大，关节的灵活性越差。

建立优化模型，先用权利要求8中步骤d的公式替换θ₁,θ_3d，利用优化模型求出x,y，再反代入θ₁,θ_3d，求出θ₁,θ_3d。其中

由以上模型得出一个可取值(x,y)＝(-70.71,-54.77)，输出结果

cosθ₃＝0.427,θ₃＝64.72,θ₃＝θ_3s+θ_3d。

把控制器获得的关节转角(64.23°，25.51°，64.72°)下发给机器人关节控制器；关节控制器控制电机运动，开始跟踪；这样从获得目标位置参数到关节转角计算，再到执行器运动就完成了一步跟踪，当一次指令会执行下一步跟踪，这样一直循环，跟踪装置就能保持随目标运动的效果。

综上所述，以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种仿人眼视觉跟踪装置，其特征在于，包括

头颈机器人本体，包括3个自由度旋转关节和CCD相机，用于通过旋转关节调整CCD相机的方位正对于跟踪目标；

视觉计算模块，用于根据CCD相机采集的图像信息计算跟踪目标的位置信息，并返回给机器人控制器；

机器人控制器，用于实时计算跟踪目标需要旋转的关节角度，发送给关节控制器进行跟踪。

2.根据权利要求1所述的仿人眼视觉跟踪装置，其特征在于：所述3个自由度旋转关节包括：微俯仰关节、水平旋转关节和宏俯仰关节；其中

微俯仰关节，用于模拟人眼在垂直方向的小范围的旋转运动；

水平旋转关节，用于模拟人颈部在水平方向的旋转运动；

宏俯仰关节，用于模拟人颈部在垂直方向的大范围的旋转运动。

3.根据权利要求2所述的仿人眼视觉跟踪装置，其特征在于：所述小范围为-15°～30°；所述旋转关节的旋转角度范围为：-90°～90°；所述大范围为0°～70°。

4.根据权利要求2或3所述的仿人眼视觉跟踪装置，其特征在于：所述旋转关节包括：

制动器，用于使关节轴减速、停止或保持停止状态；

码盘，用于测量当前旋转关节的旋转角度；

关节驱动器，用于根据机器人控制器发送的控制命令驱动头颈机器人本体的旋转关节运动。

5.根据权利要求1～4任一项所述装置的仿人眼视觉跟踪控制方法，其特征在于，包括以下步骤：

步骤1：初始化旋转关节位置；

步骤2：判断机器人视场的中心点是否与跟踪目标的位置一致，如果是，则停留等待，否则执行步骤3；

步骤3：通过视觉计算模块获取跟踪目标的位置信息，输出给机器人控制器，通过机器人控制器计算出旋转关节的旋转角度；

步骤4：关节驱动器根据得到的旋转关节的旋转角度驱动电机运动，进行目标跟踪；

步骤5：判断是否接收到终止跟踪命令，如果是，则跟踪结束，否则返回步骤2。

6.根据权利要求5所述的仿人眼视觉跟踪控制方法，其特征在于，所述通过机器人控制器计算出旋转关节的旋转角度包括以下过程：

步骤1：通过D-H方法建立机器人的连杆坐标系，并根据D-H参数得到各个关节坐标系之间的变换关系；

步骤2：CCD相机捕捉到跟踪目标从始点S运动到末点D，通过视觉计算模块获取从始点S到末点D的始末坐标；

步骤3：计算水平旋转关节的旋转角度；

步骤4：建立舒适角优化模型，计算微俯仰关节和宏俯仰关节的旋转角度。

7.根据权利要求6所述的仿人眼视觉跟踪控制方法，其特征在于，所述计算水平旋转关节的旋转角度包括以下过程：

步骤1：利用坐标变换关系得到始末点S、D相对于水平旋转关节坐标系的坐标；

步骤2：得出始末点S、D在水平旋转关节坐标系的XY平面的投影向量；

步骤3：根据投影向量计算水平旋转关节的旋转角度。

8.根据权利要求6所述的仿人眼视觉跟踪控制方法，其特征在于，所述建立舒适角优化模型，计算微俯仰关节和宏俯仰关节的旋转角度包括以下步骤：

步骤1：利用坐标变换关系得到始末点S、D相对于宏俯仰关节坐标系的坐标S′,D′；

步骤2：将S′,D′投影到宏俯仰关节坐标系的XY平面，分别得到S′,D′对应的投影向量；

步骤3：设宏俯仰关节旋转一定角度，其始末状态分别对应微俯仰关节的始末状态，将微俯仰关节始末状态下对应坐标系的原点转换到宏俯仰关节坐标系下，并投影到其XY平面；

步骤4：在宏俯仰关节坐标系的XY平面，通过几何关系和舒适角优化模型得出宏俯仰关节和微俯仰关节的旋转角度。

9.根据权利要求8所述的仿人眼视觉跟踪控制方法，其特征在于，所述通过几何关系和舒适角优化模型得出宏俯仰关节和微俯仰关节的旋转角度包括以下过程：

所述宏俯仰关节的旋转角度表达式为：

所述宏俯仰关节的旋转角度表达式为：

θ₃＝θ_3s+θ_3d

所述舒适角优化模型为：

其中，表示S′,D′在宏俯仰关节坐标系XY平面内的投影坐标，(x_A,y_A),(x,y)表示微俯仰关节始末状态下对应坐标系的原点在宏俯仰关节坐标系XY平面内的投影坐标，l表示宏俯仰关节和微俯仰关节坐标系原点的距离在宏俯仰关节XY平面上的投影长度，θ_3s,θ_3d分别表示微俯仰关节两次旋转的角度。λ_`1,λ₃表示宏俯仰关节和微俯仰关节的阻抗系数，表示宏俯仰关节和微俯仰关节的最佳舒适角度。