CN109129466A - 一种用于立体定向机器人的主动视觉装置及其控制方法 - Google Patents

Abstract

本发明公开了一种用于立体定向机器人的主动视觉装置及其控制方法。本发明采用双目相机设置在机械臂连接架上采集图像，由步进电机带动旋转，并由角度编码器读取转动角度，机械臂连接架刚性固定在机械臂第一关节连杆上，并解算得到双目相机到机械臂之间位置关系，传输至立体定向机器人进行分析应用；本发明避免分离系统中的视觉传感部分和机器人部分使用前需多次调整摆位、及相对坐标关系标定等问题；本发明为相机单独设计具有反馈功能的俯仰运动机构，使得相机在机械臂运动平面内能够进行主动的俯仰运动，既保证了视野的灵活性，又能够准确的获得相机位置信息；既能够获得关键区域的局部信息，又能够获得机器人周围工作环境的整体信息。

Description

一种用于立体定向机器人的主动视觉装置及其控制方法

技术领域

本发明涉及计算机辅助医疗技术，具体涉及一种用于立体定向机器人的主动视觉装置及其控制方法。

背景技术

视觉系统可以为立体定向机器人提供重要的视觉信息输入。具有视觉系统的立体定向机器人作为医疗设备，在手术注册，机械臂定位定向位姿控制，术中患者状态监测，术中人机交互等场合均有巨大的应用潜力。目前具有视觉系统的立体定向机器人中主要有两种布置方案。

其一是视觉系统固定在固定的操作空间，与机械臂基坐标系相对静止(eye tohand系统)，如专利CN105852970A所示方案。其优点在于视觉系统图像与机械臂运动彻底分离，视场固定，坐标变换简单，方案实现相对容易。其缺点在于，该方案中视觉系统完全固定，其视野空间也完全确定，机器人只能在固定的视野空间中执行操作。另外，为了方便调整视野，视觉系统与机械臂作为两个部件分立放置，其相对位置关系不固定，往往需要引入其他的手段来标定视觉系统和机械臂之间的位置关系，不仅增加了系统的复杂度，而且还可能在使用中引入额外的误差。

其二是视觉系统固定于机械臂末端法兰盘(eye in hand系统)，如专利CN104688351A，及文献[International Journal of Computer Assisted Radiology andSurgery,2017,15(8):1355-1368]所示方案。其优点在于视觉系统可由机械臂携带运动到任意视角，术中视野可灵活调整。其缺点在于，机械臂在执行其他操作时也会携带视觉系统运动，相机视野与机械臂末端法兰盘运动耦合，导致视觉系统视场发生被动改变，增加了后续计算的复杂度，且无法持续监测。另外，在手术执行过程中，机械臂末端法兰盘需要执行定位定向运动，此时视觉系统受定位定向运动限制，仅具有局部视野，虽然可以很好的观察手术区域的局部情况，但是对于机器人所处整体环境则不具有感知能力。

发明内容

针对以上现有技术中存在的问题，本发明提出了一种用于立体定向机器人的主动视觉装置及其控制方法。

本发明的一个目的在于提出一种用于立体定向机器人的主动视觉装置。

本发明的用于立体定向机器人的主动视觉装置包括：机械臂连接架、相机转轴、左侧相机固定框架、左侧相机、右侧相机固定框架、右侧相机、步进电机、角度编码器和计算机；其中，步进电机固定在机械臂连接架上；相机转轴位于步进电机的中心轴上，步进电机带动相机转轴共轴转动；相机转轴的两端分别设置左侧相机固定框架和右侧相机固定框架；在左侧相机固定框架和右侧相机固定框架上分别固定安装左侧相机和右侧相机，构成双目相机；左侧相机和右侧相机分别连接至计算机；计算机连接至步进电机；角度编码器的外壳固定安装在机械臂连接架上，角度编码器的转轴与相机转轴相连；角度编码器连接至计算机；机械臂连接架刚性固定在机械臂第一关节连杆上，并且左侧相机和右侧相机分别位于机械臂的两侧，机械臂连接至计算机，从而计算机控制机械臂通过机械臂连接架带动左侧相机和右侧相机做水平旋转；计算机控制步进电机驱动相机转轴做垂直方向的俯仰旋转，从而带动左侧相机和右侧相机一同做垂直方向的俯仰旋转；角度编码器测量相机转轴的转动角度，并传输至计算机。

机械臂连接架的两端设置有垂直的支撑孔，相机转轴的两端分别从支撑孔穿出，支撑孔为相机转轴提供支撑。

左侧相机通过左侧相机连接片，固定连接在左侧相机固定框架。右侧相机通过右侧相机连接片，固定连接在右侧相机固定框架上。左侧相机连接片与左侧相机固定框架以及右侧相机连接片与右侧相机固定框架通过孔轴配合方式连接，调整完毕后孔轴配合可通过顶丝锁死。

机械臂末端法兰盘上安装标定用的棋盘格，用以获取相机与机械臂的位置关系。或者，机械臂末端法兰盘上安装立体定向工具，用以执行立体定向操作。

左侧相机和右侧相机采用电荷耦合器件CCD相机或者互补金属氧化物半导体CMOS相机。

本发明的另一个目的在于提供一种用于立体定向机器人的主动视觉装置的控制方法。

本发明的用于立体定向机器人的主动视觉装置的控制方法，包括以下步骤：

1)设备安装：

将机械臂连接架刚性固定在机械臂第一关节连杆上，并且左侧相机和右侧相机分别位于机械臂的两侧，构成双目相机，在机械臂末端法兰盘上安装棋盘格，通过调整双目相机的转动角度，使得机械臂末端法兰盘同时呈现在左侧相机和右侧相机的视野中央；

2)双目相机参数标定：

固定主动视觉装置中步进电机的运动角度，及机械臂第一关节的转动角度，机械臂末端法兰盘携带棋盘格运动到n个不同位姿下，通过双目相机同时获取棋盘格图像，求取左侧相机内参矩阵左H_Left-camera、右侧相机内参矩阵H_Rigth-camera、左侧相机畸变系数Dis_Left-camera，右侧相机Dis_Right-camera、相机间相对位置矩阵^Left-cameraH_Right-camera，及不同位姿下左侧相机相对于棋盘格的位置矩阵和右侧相机相对于棋盘格的位置矩阵其中n为≥3的自然数；

3)双目相机到机械臂之间位置关系参数标定：

分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

4)相机视角控制：

根据实际应用需求，控制步进电机转动带动双目相机到达感兴趣的视角；

5)相机视角数据读取：

计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

6)图像数据获取：

计算机发出控制指令同时读取左侧相机和右侧相机中的图像数据，并保存；

7)双目相机视角解算：

采用步骤5)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

8)双目相机视角及图像数据应用：

将步骤6)获得的图像，以及步骤7)得到的左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，传输给计算机，进行分析应用；

9)判断是否需要再次获取图像：

判断是否需要再次获取图像，如果是则返回步骤4)，否则结束。

其中，在步骤3)中，双目相机到机械臂之间位置关系参数标定采用先标定左侧相机或者先标定右侧相机两种方法实现。

采用先标定左侧相机到机械臂之间位置关系，再标定双目相机到机械臂之间位置关系的方法，双目相机到机械臂之间位置关系参数标定包括以下步骤：

a)左侧相机与机械臂相对位置固定时相机到机械臂基坐标系间位置标定：

固定机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'，控制机械臂重复步骤2)，每个标定位姿均满足以下恒等关系：

其中，^flanH_board表示机械臂末端法兰盘坐标系到棋盘格坐标系之间的位置矩阵为恒定值，表示第i个位姿下机械臂末端法兰盘坐标系到机械臂基坐标系之间的位置矩阵，由机械臂控制器中直接读出，^Left_^cameraH_board表示左侧相机相对于棋盘格坐标系之间的位置矩阵，由步骤2)双目相机参数标定求得，^baseH_{Left_camera}表示左侧相机相对于机械臂基坐标系之间的位置矩阵；从公式(1)构建如下方程：

求解以上方程组，获得在固定的机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'时的左侧相机到机械臂基坐标系的关系矩阵^baseH_{Left_camera}；

b)左侧相机到机械臂基坐标系间存在两个旋转自由度时相机机械臂位置关系标定：

当从左侧相机到机械臂基坐标系间具有两个旋转自由度，给定左侧相机与机械臂基坐标系的位置矩阵^baseH_{Left_camera}获得以下方程：

^baseH_{Left_camera}＝^baseH_{Left_link1}(θ₁)·^Left_link1H_{Left_link2}(θ_2')·^Left_link2H_{Left_camera}(3)

其中，θ₁为第i个位姿下机械臂第一关节的转动角度，θ_2'为第i个位姿下本视觉装置系统编码器获得的转动角度，^baseH_{Left_link1}(θ₁)、^Left_link1H_{Left_link2}(θ_2')、^Left_link2H_{Left_camera}分别表示在θ₁、θ_2'位置下从左侧相机到基坐标系之间的变换矩阵，在不同的θ₁和θ_2'下重复步骤a)，获取多组^baseH_{Left_camera}，记为代入方程(3)中求解方程得到各变换矩阵参数，i＝1,2,……,n；

c)双目相机到机械臂基坐标系求解：

利用步骤b)的方法求取左侧相机与机械臂基坐标系之间的位置关系如下式(4)：

^baseH_Left-camera＝^baseH_Left-link1(θ₁)·^Left-link1H_Left-link2(θ_2')·^Left-link2H_Left-camera(4)

右侧相机与机械臂基坐标系之间的位置关系如下式(5)：

^baseH_Right-camera＝^baseH_Left-camera·^Left-cameraH_Right-camera (5)。

采用先标定右侧相机到机械臂之间位置关系，再标定双目相机到机械臂之间位置关系的方法，双目相机到机械臂之间位置关系参数标定包括以下步骤：

a)右侧相机与机械臂相对位置固定时相机到机械臂基坐标系间位置标定：

固定机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'，控制机械臂重复步骤2)，每个标定位姿均满足以下恒等关系：

^flanH_board＝(^bas _i ^eH_flan)^-1baseH_{Right_camera}·^Right_camer _i ^aH_board (6)

其中，^flanH_board表示机械臂末端法兰盘坐标系到棋盘格坐标系之间的位置矩阵为恒定值，表示第i个位姿下机械臂末端法兰盘坐标系到机械臂基坐标系之间的位置矩阵，由机械臂控制器中直接读出，^Right_cameraH_board表示右侧相机相对于棋盘格坐标系之间的位置矩阵，由步骤2)双目相机参数标定求得，^baseH_{Right_camera}表示右侧相机相对于机械臂基坐标系之间的位置矩阵；从公式(1)构建如下方程：

求解以上方程组，获得在固定的机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'时的右侧相机到机械臂基坐标系的关系矩阵^baseH_{Right_camera}；

b)右侧相机到机械臂基坐标系间存在两个旋转自由度时相机机械臂位置关系标定：

当从右侧相机到机械臂基坐标系间具有两个旋转自由度，给定右侧相机与机械臂基坐标系的位置矩阵^baseH_{Right_camera}获得以下方程：

^baseH_{Right_camera}＝^baseH_{Right_link1}(θ₁)·^Right_link1H_{Right_link2}(θ_2')·^Right_link2H_{Right_camera}(8)

其中，θ₁为第i个位姿下机械臂第一关节的转动角度，θ_2'为第i个位姿下本视觉装置系统编码器获得的转动角度，^baseH_{Right_link1}(θ₁)、^Right_link1H_{Right_link2}(θ_2')、^{Right_ link2}H_{Right_camera}分别表示在θ₁、θ_2'位置下从右侧相机到基坐标系之间的变换矩阵，在不同的θ₁和θ_2'下重复步骤a)，获取多组^baseH_{Right_camera}，记为代入方程(8)中求解方程得到各变换矩阵参数，i＝1,2,……,n；

c)双目相机到机械臂基坐标系求解：

利用步骤b)的方法求取右侧相机与机械臂基坐标系之间的位置关系如下式(9)：

^baseH_Right-camera＝^baseH_Right-link1(θ₁)·^Right-link1H_Right-link2(θ_2')·^Right-link2H_Right-camera(9)

右侧相机与机械臂基坐标系之间的位置关系如下式(10)：

^baseH_Left-camera＝^baseH_Right-camera·(^Left-cameraH_Right-camera)^-1 (10)。

进一步，用于立体定向机器人的主动视觉装置在获取了双目相机参数标定后，应用于获取机器人与立体定向目标靶点空间位姿坐标关系以及机械臂立体定向控制。

本发明的用于立体定向机器人的主动视觉装置的控制方法应用于获取机器人与立体定向目标靶点空间位姿坐标关系的方法，包括以下步骤：

1)选定在立体定向目标内部的立体定向目标靶点，并选定在立体定向目标的表面的多个标志点，通过调整双目相机的转动角度，使得立体定向目标同时呈现在左侧相机和右侧相机的视野内，并固定步进电机的转动角度和机械臂第一关节的转动角度；

2)计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

3)手动控制探针工具针的尖端与一个标志点重合；

4)分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

5)采用步骤2)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

6)根据左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，利用双目视觉定位算法获取探针工具的尖端点在机械臂基坐标系下的空间坐标，进而获取与其接触的标志点的坐标；

7)重复步骤3)～6)，直至获得多个标志点的坐标，根据标志点的坐标和标志点与立体定向目标靶点间相对位置关系，获得机械臂基坐标系与立体定向目标靶点空间位姿坐标关系，即为机器人与立体定向目标靶点空间位姿坐标关系。

本发明的用于立体定向机器人的主动视觉装置的控制方法应用于机械臂立体定向控制的控制方法，包括以下步骤：

1)在机械臂末端法兰盘上安装立体定向工具，通过调整双目相机的转动角度，使得立体定向工具同时呈现在左侧相机和右侧相机的视野内；

2)计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

3)分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

4)采用步骤2)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

5)利用目标识别和双目视觉定位算法获取立体定向工具的位置和姿态，并根据左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，将立体定向工具的位置和姿态解算到机械臂基坐标系下；

6)将所获得的机械臂基坐标系下的立体定向工具的位置和姿态与目标位置和姿态对比获得差值，控制机械臂进一步运动减小差值；

7)重复步骤2)～6)，直到测量的机械臂基坐标系下的立体定向工具的位置和姿态与目标位置和姿态差值达到预先设定的阈值内，此时立体定向工具达到目标立体定向位置。

本发明的优点：

本发明采用双目相机设置在机械臂连接架上采集图像，由步进电机带动旋转，并由角度编码器读取转动角度，机械臂连接架刚性固定在机械臂第一关节连杆上，并得到获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，传输至立体定向机器人进行分析应用；本发明避免分离系统中的视觉传感部分和机器人部分使用前需多次调整摆位、及相对坐标关系标定等问题；六自由度串联机械臂具有一轴以上各部分均在同一平面运动的特点，本发明将视觉系统安装于机械臂第一关节连杆上，能够使机械臂运动平面始终处于视野中心位置附近，既保证了视觉系统的视野广度，又保证了机械臂末端法兰盘始终在视野范围内；本发明为相机单独设计具有反馈功能的俯仰运动机构，使得相机在机械臂运动平面内能够进行主动的俯仰运动，既保证了视野的灵活性，又能够准确的获得相机位置信息；既能够获得关键区域的局部信息，又能够获得机器人周围工作环境的整体信息。

附图说明

图1为本发明的用于立体定向机器人的主动视觉装置的一个实施例的示意图；

图2为本发明的用于立体定向机器人的主动视觉装置与机械臂之间的安装关系的示意图；

图3为本发明的用于立体定向机器人的主动视觉装置的标定方法的示意图；

图4为本发明的用于立体定向机器人的主动视觉装置的控制方法的流程图；

图5为本发明的用于立体定向机器人的主动视觉装置的标定方法的坐标关系图；

图6为本发明的用于立体定向机器人的主动视觉装置应用于获取机器人与立体定向目标靶点空间位姿坐标关系的示意图；

图7为本发明的用于立体定向机器人的主动视觉装置应用于机械臂立体定向控制的示意图。

具体实施方式

下面结合附图，通过具体实施例，进一步阐述本发明。

如图1所示，本实施例的用于立体定向机器人的主动视觉装置包括：机械臂连接架11、相机转轴17、左侧相机固定框架12、左侧相机14、右侧相机固定框架10、右侧相机19、步进电机15、角度编码器16和计算机；其中，步进电机固定在机械臂连接架11上；相机转轴17位于步进电机15的中心轴上，步进电机15带动相机转轴17共轴转动；相机转轴17的两端分别设置左侧相机固定框架12和右侧相机固定框架10；在左侧相机固定框架12上通过左侧相机连接片13固定安装左侧相机，在右侧相机固定框架10上通过右侧相机连接片18固定安装右侧相机19，构成双目相机；左侧相机14和右侧相机19分别连接至计算机；计算机连接至步进电机15；角度编码器16的外壳固定安装在机械臂连接架11上，角度编码器16的转轴与相机转轴17相连；角度编码器16连接至计算机；机械臂连接架11刚性固定在机械臂第一关节连杆上，并且左侧相机14和右侧相机19分别位于机械臂的两侧。

如图2所示，机械臂2通过机械臂第一关节连杆固定在主动视觉装置1的机械臂连接架上，机械臂末端法兰盘上安装立体定向工具3，用以执行立体定向操作。如图3所示，机械臂末端法兰盘上安装标定用的棋盘格4，用以获取相机与机械臂的位置关系。

如图4所示，本实施例的用于立体定向机器人的主动视觉装置方法，包括以下步骤：

1)设备安装：

将机械臂连接架刚性固定在机械臂第一关节连杆上，并且左侧相机和右侧相机分别位于机械臂的两侧，构成双目相机，在机械臂末端法兰盘上安装棋盘格，通过调整双目相机的转动角度，使得机械臂末端法兰盘同时呈现在两相机视野中央。

2)双目相机参数标定：

固定主动视觉装置中步进电机的运动角度，及机械臂第一关节的转动角度，机械臂末端法兰盘携带棋盘格运动到n个不同位姿下，通过双目相机同时获取棋盘格图像，使用张正友方法[1]求取左侧相机内参矩阵左H_Left-camera、右侧相机内参矩阵H_Rigth-camera、左侧相机畸变系数Dis_Left-camera，右侧相机Dis_Right-camera、相机间相对位置矩阵^Left-cameraH_Right-camera，及不同位姿下左侧相机相对于棋盘格的位置矩阵和右侧相机相对于棋盘格的位置矩阵其中n≥3。

3)如图5所示，采用先标定左侧相机，双目相机到机械臂之间位置关系参数标定：

a)左侧相机与机械臂相对位置固定时相机到机械臂基坐标系间位置标定：

固定机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'，控制机械臂重复步骤2)，每个标定位姿均满足以下恒等关系：

其中，^flanH_board表示机械臂末端法兰盘坐标系到棋盘格坐标系之间的位置矩阵为恒定值，表示第i个位姿下机械臂末端法兰盘坐标系到机械臂基坐标系之间的位置矩阵，由机械臂控制器中直接读出，^Left_cameraH_board表示左侧相机相对于棋盘格坐标系之间的位置矩阵，由步骤2)双目相机参数标定求得，^baseH_{Left_camera}表示左侧相机相对于机械臂基坐标系之间的位置矩阵；从公式(1)构建如下方程：

求解以上方程组，获得在固定的机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'时的左侧相机到机械臂基坐标系的关系矩阵^baseH_{Left_camera}；

b)左侧相机到机械臂基坐标系间存在两个旋转自由度时相机机械臂位置关系标定：

当从左侧相机到机械臂基坐标系间具有两个旋转自由度，给定左侧相机与机械臂基坐标系的位置矩阵^baseH_{Left_camera}获得以下方程：

^baseH_{Left_camera}＝^baseH_{Left_link1}(θ₁)·^Left_link1H_{Left_link2}(θ_2')·^Left_link2H_{Left_camera}(3)

其中，θ₁为第i个位姿下机械臂第一关节的转动角度，θ_2'为第i个位姿下本视觉装置系统编码器获得的转动角度，^baseH_{Left_link1}(θ₁)、^Left_link1H_{Left_link2}(θ_2')、^Left_link2H_{Left_camera}分别表示在θ₁、θ_2'位置下从左侧相机到基坐标系之间的变换矩阵，在不同的θ₁和θ_2'下重复步骤a)，获取多组^baseH_{Left_camera}，记为代入方程(3)中求解方程得到各变换矩阵参数；

c)双目相机到机械臂基坐标系求解：

利用步骤b)的方法求取左侧相机与机械臂基坐标系之间的位置关系如下式(4)：

^baseH_Left-camera＝^baseH_Left-link1(θ₁)·^Left-link1H_Left-link2(θ_2')·^Left-link2H_Left-camera(4)

右侧相机与机械臂基坐标系之间的位置关系如下式(5)：

^baseH_Right-camera＝^baseH_Left-camera·^Left-cameraH_Right-camera (5)。

4)相机视角控制：

根据实际应用需求，控制步进电机转动带动双目相机到达感兴趣的视角；

5)相机视角数据读取：

计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

6)图像数据获取：

计算机发出控制指令同时读取左侧相机和右侧相机中的图像数据，并保存；

7)双目相机视角解算：

采用步骤5)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

8)双目相机视角及图像数据应用：

将步骤6)获得的图像，以及步骤7)得到的左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，传输给立体定向机器人，进行分析应用；

9)判断是否需要再次获取图像：

判断是否需要再次获取图像，如果是则返回步骤4)，否则结束。

实施例一

本实例中，用于立体定向机器人的主动视觉装置应用于获取机器人与立体定向目标靶点空间位姿坐标关系的方法，如图6所示，包括以下步骤：

1)选定在立体定向目标7内部的立体定向目标靶点8，并选定在立体定向目标的表面的多个标志点6，通过调整双目相机的转动角度，使得立体定向目标同时呈现在左侧相机和右侧相机的视野内，并固定步进电机的转动角度和机械臂第一关节的转动角度；

2)计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

3)手动控制探针工具针5的尖端与一个标志点重合；

4)分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

5)采用步骤4)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

6)根据左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，利用双目视觉定位算法获取探针工具的尖端点在机械臂基坐标系下的空间坐标，进而获取与其接触的标志点的坐标；

7)重复步骤3)～6)，直至获得多个标志点的坐标，根据标志点的坐标和标志点与立体定向目标靶点间相对位置关系，获得机械臂基坐标系与立体定向目标靶点空间位姿坐标关系，即为机器人与立体定向目标靶点空间位姿坐标关系。

实施例二

本实施例中，用于立体定向机器人的主动视觉装置应用于机械臂立体定向控制的控制方法，如图7所示，包括以下步骤：

1)在机械臂末端法兰盘上安装立体定向工具3，通过调整双目相机的转动角度，使得立体定向工具同时呈现在左侧相机和右侧相机的视野内；

2)计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

3)分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

4)采用步骤4)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

5)利用目标识别和双目视觉定位算法获取立体定向工具的位置和姿态，并根据左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，将立体定向工具的位置和姿态解算到机械臂基坐标系下；

6)将所获得的机械臂基坐标系下的立体定向工具的位置和姿态与目标位置和姿态对比获得差值，控制机械臂进一步运动减小差值；

7)重复步骤2)～6)，直到测量的立体定向工具的位置和姿态与目标位置和姿态差值达到阈值内，此时立体定向工具达到目标立体定向位置。

最后需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在不脱离本发明及所附的权利要求的精神和范围内，各种替换和修改都是可能的。因此，本发明不应局限于实施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

参考文献

[1]Z.Zhang,"A Flexible New Technique for Camera Calibration,"TPAMI,2000,vol.22,no.11,pp.1330-1334,2000.

Claims (10)

1.一种用于立体定向机器人的主动视觉装置，其特征在于，所述主动视觉装置包括：机械臂连接架、相机转轴、左侧相机固定框架、左侧相机、右侧相机固定框架、右侧相机、步进电机、角度编码器和计算机；其中，所述步进电机固定在机械臂连接架上；所述相机转轴位于步进电机的中心轴上，步进电机带动相机转轴共轴转动；所述相机转轴的两端分别设置左侧相机固定框架和右侧相机固定框架；在左侧相机固定框架和右侧相机固定框架上分别固定安装左侧相机和右侧相机，构成双目相机；所述左侧相机和右侧相机分别连接至计算机；所述计算机连接至步进电机；所述角度编码器的外壳固定安装在机械臂连接架上，角度编码器的转轴与相机转轴相连；所述角度编码器连接至计算机；所述机械臂连接架刚性固定在机械臂第一关节连杆上，并且左侧相机和右侧相机分别位于机械臂的两侧，机械臂连接至计算机，从而计算机控制机械臂通过机械臂连接架带动左侧相机和右侧相机做水平旋转；计算机控制步进电机驱动相机转轴做垂直方向的俯仰旋转，从而带动左侧相机和右侧相机一同做垂直方向的俯仰旋转；角度编码器测量相机转轴的转动角度，并传输至计算机。

2.如权利要求1所述的主动视觉装置，其特征在于，所述机械臂连接架的两端设置有垂直的支撑孔，相机转轴的两端分别从支撑孔穿出，支撑孔为相机转轴提供支撑。

3.如权利要求1所述的主动视觉装置，其特征在于，还包括左侧相机连接片和右侧相机连接片，所述左侧相机通过左侧相机连接片，固定连接在左侧相机固定框架；所述右侧相机通过右侧相机连接片，固定连接在右侧相机固定框架上。

4.如权利要求1所述的主动视觉装置，其特征在于，所述机械臂末端法兰盘上安装标定用的棋盘格；或者，机械臂末端法兰盘上安装立体定向工具。

5.如权利要求1所述的主动视觉装置，其特征在于，所述左侧相机和右侧相机采用电荷耦合器件CCD相机或者互补金属氧化物半导体CMOS相机。

6.一种用于立体定向机器人的主动视觉装置的控制方法，其特征在于，所述控制方法包括以下步骤：

1)设备安装：

将机械臂连接架刚性固定在机械臂第一关节连杆上，并且左侧相机和右侧相机分别位于机械臂的两侧，构成双目相机，在机械臂末端法兰盘上安装棋盘格，通过调整双目相机的转动角度，使得机械臂末端法兰盘同时呈现在左侧相机和右侧相机的视野中央；

2)双目相机参数标定：

固定主动视觉装置中步进电机的运动角度，及机械臂第一关节的转动角度，机械臂末端法兰盘携带棋盘格运动到n个不同位姿下，通过双目相机同时获取棋盘格图像，求取左侧相机内参矩阵左H_Left-camera、右侧相机内参矩阵H_Rigth-camera、左侧相机畸变系数Dis_Left-camera，右侧相机Dis_Right-camera、相机间相对位置矩阵^Left-cameraH_Right-camera，及不同位姿下左侧相机相对于棋盘格的位置矩阵和右侧相机相对于棋盘格的位置矩阵其中n为≥3的自然数；

3)双目相机到机械臂之间位置关系参数标定：

分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

4)相机视角控制：

根据实际应用需求，控制步进电机转动带动双目相机到达感兴趣的视角；

5)相机视角数据读取：

计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

6)图像数据获取：

计算机发出控制指令同时读取左侧相机和右侧相机中的图像数据，并保存；

7)双目相机视角解算：

采用步骤5)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

8)双目相机视角及图像数据应用：

将步骤6)获得的图像，以及步骤7)得到的左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，传输给计算机，进行分析应用；

9)判断是否需要再次获取图像：

判断是否需要再次获取图像，如果是则返回步骤4)，否则结束。

7.如权利要求6所述的控制方法，其特征在于，在步骤3)中，采用先标定左侧相机到机械臂之间位置关系，再标定双目相机到机械臂之间位置关系的方法，双目相机到机械臂之间位置关系参数标定包括以下步骤：

a)左侧相机与机械臂相对位置固定时相机到机械臂基坐标系间位置标定：

固定机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'，控制机械臂重复步骤2)，每个标定位姿均满足以下恒等关系：

其中，^flanH_board表示机械臂末端法兰盘坐标系到棋盘格坐标系之间的位置矩阵为恒定值，表示第i个位姿下机械臂末端法兰盘坐标系到机械臂基坐标系之间的位置矩阵，由机械臂控制器中直接读出，^Left_cameraH_board表示左侧相机相对于棋盘格坐标系之间的位置矩阵，由步骤2)双目相机参数标定求得，^baseH_{Left_camera}表示左侧相机相对于机械臂基坐标系之间的位置矩阵；从公式(1)构建如下方程：

求解以上方程组，获得在固定的机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'时的左侧相机到机械臂基坐标系的关系矩阵^baseH_{Left_camera}；

b)左侧相机到机械臂基坐标系间存在两个旋转自由度时相机机械臂位置关系标定：

当从左侧相机到机械臂基坐标系间具有两个旋转自由度，给定左侧相机与机械臂基坐标系的位置矩阵^baseH_{Left_camera}获得以下方程：

^baseH_{Left_camera}＝^baseH_{Left_link1}(θ₁)·^Left_link1H_{Left_link2}(θ_2')·^Left_link2H_{Left_camera} (3)

其中，θ₁为第i个位姿下机械臂第一关节的转动角度，θ_2'为第i个位姿下本视觉装置系统编码器获得的转动角度，^baseH_{Left_link1}(θ₁)、^Left_link1H_{Left_link2}(θ_2')、^Left_link2H_{Left_camera}分别表示在θ₁、θ_2'位置下从左侧相机到基坐标系之间的变换矩阵，在不同的θ₁和θ_2'下重复步骤a)，获取多组^baseH_{Left_camera}，记为_iθ₁、_iθ_2'、代入方程(3)中求解方程得到各变换矩阵参数，i＝1,2,……,n；

c)双目相机到机械臂基坐标系求解：

利用步骤b)的方法求取左侧相机与机械臂基坐标系之间的位置关系如下式(4)：

^baseH_Left-camera＝^baseH_Left-link1(θ₁)·^Left-link1H_Left-link2(θ_2')·^Left-link2H_Left-camera (4)

右侧相机与机械臂基坐标系之间的位置关系如下式(5)：

^baseH_Right-camera＝^baseH_Left-camera·^Left-cameraH_Right-camera (5)。

8.如权利要求6所述的控制方法，其特征在于，在步骤3)中，采用先标定右侧相机到机械臂之间位置关系，再标定双目相机到机械臂之间位置关系的方法，双目相机到机械臂之间位置关系参数标定包括以下步骤：

a)右侧相机与机械臂相对位置固定时相机到机械臂基坐标系间位置标定：

固定机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'，控制机械臂重复步骤2)，每个标定位姿均满足以下恒等关系：

其中，^flanH_board表示机械臂末端法兰盘坐标系到棋盘格坐标系之间的位置矩阵为恒定值，表示第i个位姿下机械臂末端法兰盘坐标系到机械臂基坐标系之间的位置矩阵，由机械臂控制器中直接读出，^Right_cameraH_board表示右侧相机相对于棋盘格坐标系之间的位置矩阵，由步骤2)双目相机参数标定求得，^baseH_{Right_camera}表示右侧相机相对于机械臂基坐标系之间的位置矩阵；从公式(1)构建如下方程：

求解以上方程组，获得在固定的机械臂第一关节的转动角度θ₁，步进电机的转动角度θ_2'时的右侧相机到机械臂基坐标系的关系矩阵^baseH_{Right_camera}；

b)右侧相机到机械臂基坐标系间存在两个旋转自由度时相机机械臂位置关系标定：

当从右侧相机到机械臂基坐标系间具有两个旋转自由度，给定右侧相机与机械臂基坐标系的位置矩阵^baseH_{Right_camera}获得以下方程：

^baseH_{Right_camera}＝^baseH_{Right_link1}(θ₁)·^Right_link1H_{Right_link2}(θ_2')·^Right_link2H_{Right_camera}(8)

其中，θ₁为第i个位姿下机械臂第一关节的转动角度，θ_2'为第i个位姿下本视觉装置系统编码器获得的转动角度，^baseH_{Right_link1}(θ₁)、^Right_link1H_{Right_link2}(θ_2')、^Right_link2H_{Right_camera}分别表示在θ₁、θ_2'位置下从右侧相机到基坐标系之间的变换矩阵，在不同的θ₁和θ_2'下重复步骤a)，获取多组^baseH_{Right_camera}，记为_iθ₁、_iθ_2'、代入方程(8)中求解方程得到各变换矩阵参数，i＝1,2,……,n；

c)双目相机到机械臂基坐标系求解：

利用步骤b)的方法求取右侧相机与机械臂基坐标系之间的位置关系如下式(9)：

^baseH_Right-camera＝^baseH_Right-link1(θ₁)·^Right-link1H_Right-link2(θ_2')·^Right-link2H_Right-camera (9)

右侧相机与机械臂基坐标系之间的位置关系如下式(10)：

^baseH_Left-camera＝^baseH_Right-camera·(^Left-cameraH_Right-camera)^-1 (10)。

9.如权利要求6所述的控制方法，其特征在于，所述控制方法应用于获取机器人与立体定向目标靶点空间位姿坐标关系的方法，包括以下步骤：

1)选定在立体定向目标内部的立体定向目标靶点，并选定在立体定向目标的表面的多个标志点，通过调整双目相机的转动角度，使得立体定向目标同时呈现在左侧相机和右侧相机的视野内，并固定步进电机的转动角度和机械臂第一关节的转动角度；

2)计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

3)手动控制探针工具针的尖端与一个标志点重合；

4)分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

5)采用步骤2)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

6)根据左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，利用双目视觉定位算法获取探针工具的尖端点在机械臂基坐标系下的空间坐标，进而获取与其接触的标志点的坐标；

7)重复步骤3)～6)，直至获得多个标志点的坐标，根据标志点的坐标和标志点与立体定向目标靶点间相对位置关系，获得机械臂基坐标系与立体定向目标靶点空间位姿坐标关系，即为机器人与立体定向目标靶点空间位姿坐标关系。

10.如权利要求6所述的控制方法，其特征在于，所述控制方法应用于机械臂立体定向控制的控制方法，包括以下步骤：

1)在机械臂末端法兰盘上安装立体定向工具，通过调整双目相机的转动角度，使得立体定向工具同时呈现在左侧相机和右侧相机的视野内；

2)计算机读取角度编码器采集的步进电机的转动角度θ_2'，以及机械臂第一关节的转动角度θ₁，并保存；

3)分别得到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系；

4)采用步骤2)得到的机械臂第一关节的转动角度θ₁和角度编码器采集的步进电机的转动角度θ_2'，代入到左侧相机坐标系与机械臂基坐标系之间的位置关系以及右侧相机坐标系与机械臂基坐标系之间的位置关系中，分别获得当前左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera；

5)利用目标识别和双目视觉定位算法获取立体定向工具的位置和姿态，并根据左侧相机相对于机械臂基坐标系的位置关系^baseH_Left-camera和右侧相机相对于机械臂基坐标系位置关系^baseH_Right-camera，将立体定向工具的位置和姿态解算到机械臂基坐标系下；

6)将所获得的机械臂基坐标系下的立体定向工具的位置和姿态与目标位置和姿态对比获得差值，控制机械臂进一步运动减小差值；

7)重复步骤2)～6)，直到测量的机械臂基坐标系下的立体定向工具的位置和姿态与目标位置和姿态差值达到预先设定的阈值内，此时立体定向工具达到目标立体定向位置。