CN111784771B - 基于双目相机的3d三角测量方法及装置 - Google Patents

Abstract

本申请公开了一种基于双目相机的3D三角测量方法。该基于双目相机的3D三角测量方法包括建立双目相机的坐标系，并获取坐标系参数；通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；通过双目相机的运动减小空间点的实际像素和预期像素的偏差；获取左相机和右相机的光轴，得到固定点p。本申请解决了由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的技术问题。

Description

基于双目相机的3D三角测量方法及装置

技术领域

本申请涉及双目主动视觉领域，具体而言，涉及一种基于双目相机的3D三角测量方法。

背景技术

基于双目主动视觉的3D三角测量技术在计算机视觉和机器人技术中的应用越来越多，双眼主动视觉有益于多种应用，例如操纵，3D重建，导航，3D映射等。基于双目主动视觉的3D坐标估计由于其非接触，低成本和高精度的优点吸引了广泛的研究兴趣。

双目主动视觉***可分为两类：第一类具有固定相机，第二类具有非固定相机。第二类可以提高灵活性并扩展视野，它与人类视觉***更相似。通常，可以使用视差和立体外部参数执行3D三角测量。立体外部参数可以离线或在线进行校准。第一类主动视觉***仅需要离线校准，其立体外部参数是固定的，因为一个相机关于另一个相机是静态的。第二类立体外部参数需要在线校准，因为外部参数可能一直在变化。

相关技术中采用集成双重校正(ITPC)的方法，通过正向运动学和校正后的头眼参数在线计算立体外部参数，使用立体外部参数执行基于视差的3D三角测量，以估计3D空间点的坐标。但是，在基于视差的三角测量方法中，立体外部参数和视差的准确性对3D三角测量有重大影响。

针对基于视差的三角测量方法中由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的问题，目前尚未提出有效的解决方案。

发明内容

本申请的主要目的在于提供一种基于双目相机的3D三角测量方法，以解决由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的技术问题。

为了实现上述目的，根据本申请的一个方面，提供了一种基于双目相机的3D三角测量方法及装置。

第一方面，本申请提供了一种基于双目相机的3D三角测量方法。

根据本申请的基于双目相机的3D三角测量方法包括：

建立双目相机的坐标系，并获取坐标系参数；

通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；

通过双目相机的运动减小空间点的实际像素和预期像素的偏差；

获取左相机和右相机的光轴，得到固定点p。

进一步的，建立双目相机的坐标系，并获取坐标系参数包括：

基于标准D-H方法建立坐标系；

参数d_i，θ_i，a_i，α_i分别是连杆偏距，关节角，连杆长度和连杆转角；关节偏移量是初始状态下θ_i的值；

坐标系i和i-1之间的转换矩阵为：

进一步的，通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵包括：

左相机和右相机分别是左眼和右眼的末端执行器；

末端执行器的速度V_n＝[v_xv_yv_zω_xω_yω_z]^T；其中，v_x，v_y，v_z是线速度，ω_x，ω_y，ω_z为角速度；

关节速度

关节雅可比矩阵

且

通过公式/>

进行计算；其中，Z_i和O_i是Z轴的方向向量和坐标系O_i-x_iy_iz_i的原点；

通过公式

将J_θ从基本坐标系转换到相机坐标系；其中，J_joint是变换后的关节雅可比矩阵，R是基础坐标系关于相机坐标系的旋转矩阵；

则，V_n和

之间的关系为/>

3D空间点的坐标p＝(u，v)；

则，像素速度

和末端执行器速度V_n之间的关系为/>

其中，J_image是图像雅可比矩阵；

且，图像雅可比矩阵

其中，f_u和f_v分别是相机的列和行方向的焦距，Z_c是相机坐标系中空间点p的深度。

进一步的，通过双目相机的运动减小空间点的实际像素和预期像素的偏差包括：

空间点的实际像素坐标和预期像素坐标分别为p和p^*；

则，p和p^*的偏差e＝p^*-p；

则，

其中，/>

是像素速度；

其中，K是影响视觉注视性能的常数矩阵；

构造常数矩阵

其中，k₁和k₂是每个通道中的增益；

则，空间点的像素速度

则，

在左右相机的连接处建立坐标系O_N-x_Ny_Nz_N，则左眼关节速度矢量为：

/>

则，

其中，p_l和/>

是左相机中空间点的实际像素坐标和预期像素坐标；

则，左眼的关节速度为

其中，/>

是矩阵J_l中第i行和第j列上的元素；

同理，右眼得关节速度为

优选的，当左相机和右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为固定点p。

具体的，当左相机和右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为固定点p包括：

L_l和L_r是同时通过固定点p的理想光轴，L′_l和L′_r是实际的光轴，点A和B在L′_l上，点C和D在L′_r上；

在双目相机的初始状态下，L′_l和L′_r分别与

和/>

轴的方向相同；

点A，B，C，D的齐次坐标分别为(x_a，y_a，z_a，1)，(x_b，y_b，z_b，1)，(x_c，y_c，z_c，1)和(x_d，y_d，z_d，1)；点A，B，C，D的初始齐次坐标分别为(0，0，z_a′，1)，(0，0，z_b′，1)，(0，0，z_c′，1)和(0，0，z_d′，1)；

则，A的齐次坐标为：

同理可得点B，C和D的齐次坐标；

X₁＝x_b-x_a，Y₁＝y_b-y_a，Z₁＝z_b-z_a，X₂＝x_d-x_c，Y₂＝y_d-y_c和Z₂＝z_d-z_c，

则，L′_l和L′_r分别为

则，L′_l和L′_r的公垂线的方向向量为：

则，由L′_l和公共垂直线确定的平面为：

X_cp(X-x_a)+Y_cp(Y-y_a)+Z_cp(Z-z_a)＝0

则，解得坐标p₁＝(u₁，v₁)，同理得到坐标p₂＝(u₂，v₂)；

通过确定坐标系中点p₁和p₂的中点坐标来确定固定点坐标

第二方面，本申请提供一种基于双目相机的3D三角测量装置，该装置包括：

坐标系建立模块：用于建立双目相机的坐标系，并获取坐标系参数；

信息获取模块：用于通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；

偏差减小模块：用于通过双目相机的运动减小空间点的实际像素和预期像素的偏差；

固定点获取模块：用于获取左相机和右相机的光轴，得到固定点p。

第三方面，本申请提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现第一方面提供的基于双目相机的3D三角测量方法的步骤。

第四方面，本申请提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现第一方面提供的基于双目相机的3D三角测量方法的步骤。

在本申请实施例中，通过计算相机两个光轴的相交点来执行3D三角测量，从而避免了使用图像视差或立体外部参数的缺点，达到了具有较好的性能和较小的不确定度的目的，解决了由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的技术问题。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，使得本申请的其它特征、目的和优点变得更明显。本申请的示意性实施例附图及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是根据本申请实施例的一种基于双目相机的3D三角测量方法的流程示意图；

图2是根据本申请实施例的双目相机的坐标系；

图3是根据本申请实施例的基于视觉注视的3D三角测量方法；以及

图4时根据本申请实施例的实际情况模型。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

在本申请中，术语“上”、“下”、“左”、“右”、“前”、“后”、“顶”、“底”、“内”、“外”、“中”、“竖直”、“水平”、“横向”、“纵向”等指示的方位或位置关系为基于附图所示的方位或位置关系。这些术语主要是为了更好地描述本实用新型及其实施例，并非用于限定所指示的装置、元件或组成部分必须具有特定方位，或以特定方位进行构造和操作。

并且，上述部分术语除了可以用于表示方位或位置关系以外，还可能用于表示其他含义，例如术语“上”在某些情况下也可能用于表示某种依附关系或连接关系。对于本领域普通技术人员而言，可以根据具体情况理解这些术语在本实用新型中的具体含义。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

如图1所示，该方法包括如下的步骤S1至步骤S4：

S1：建立双目相机的坐标系，并获取坐标系参数。

进一步的，基于标准D-H方法建立坐标系。

示例的，本发明较佳实施例中的双目相机的坐标系如图2所示。

实例的，L₁为64.27mm，L₂为11.00mm，L_3l为44.80mm，L_3r为47.20mm，L₄为13.80mm，L₅为30.33mm。

进一步的，参数d_i，θ_i，a_i，α_i分别是连杆偏距，关节角，连杆长度和连杆转角；关节偏移量是初始状态下θ_i的值。

具体的，关节偏移量是初始状态下θ_i的值。

示例的，本发明较佳实施例中的d_i，θ_i，a_i，α_i如下表所示：

具体的，坐标系i和i-1之间的转换矩阵为：

相机的光轴垂直于像平面并通过像主点。如果可以通过视觉注视或视觉伺服将3D空间点p的2D图像保持在左右相机的像主点上，则3D空间点p将同时位于左右相机的光轴上。因此，p是两个相机光轴的交点，称为固定点。通过计算两个光轴的交点，可以实时估计固定点p的3D坐标。在实际应用中，由于视觉注视误差和模型误差，两个光轴可能不在同一平面上，因此将两个偏斜光轴的公垂线的中点作为固定点的表示。

S2：通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵。

进一步的，左相机和右相机分别是左眼和右眼的末端执行器。

进一步的，末端执行器的速度定义为V_n＝[v_xv_yv_zω_xω_yω_z]^T；

关节速度定义为

关节雅可比矩阵定义为

进一步的，v_x，v_y，v_z是线速度，ω_x，ω_y，ω_z为角速度；

具体的，

通过公式/>

进行计算。

进一步的，Z_i和O_i是Z轴的方向向量和坐标系O_i-x_iy_iz_i的原点。

具体的，将J_θ从基本坐标系转换到相机坐标系，通过公式

进行计算。

进一步的，J_joint是变换后的关节雅可比矩阵，R是基础坐标系关于相机坐标系的旋转矩阵。

具体的，V_n和

之间的关系为/>

进一步的，3D空间点的坐标定义为p＝(u，v)。

具体的，像素速度

和末端执行器速度V_n之间的关系为/>

进一步的，J_image是图像雅可比矩阵。

具体的，通过公式

计算图像雅可比矩阵。

进一步的，f_u和f_v分别是相机的列和行方向的焦距，Z_c是相机坐标系中空间点p的深度。

S3：通过双目相机的运动减小空间点的实际像素和预期像素的偏差。

进一步的，空间点的实际像素坐标和预期像素坐标分别定义为p和p^*。

进一步的，p和p^*的偏差定义为e＝p^*-p。

具体的，

进一步的，

是像素速度。

具体的，

进一步的，K是影响视觉注视性能的常数矩阵。

具体的，构造常数矩阵

进一步的，k₁和k₂是每个通道中的增益。

优选的，增加k₁和k₂可以缩短视觉注视所需的时间。

具体的，空间点的像素速度为

/>

具体的，

优选的，如图2所示，在左右相机的连接处建立坐标系O_N-x_Ny_Nz_N。

进一步的，左眼关节速度矢量为：

具体的，

进一步的，p_l和

是左相机中空间点的实际像素坐标和预期像素坐标。

具体的，左眼的关节速度为

进一步的，

是矩阵J_l中第i行和第j列上的元素。

同理可得，右眼的关节速度

S4：获取左相机和右相机的光轴，得到固定点p。

示例的，本发明较佳实施例中的基于视觉注视的3D三角测量方法如图3所示，在左右相机的连接处建立左眼坐标系为O₆-x₆y₆z₆，右眼坐标系为O₉-x₉y₉z₉，左相机坐标系为

右相机坐标系为/>

进一步的，且L_l和L_r分别是左相机和右相机的光轴。

具体的，左相机和右相机坐标系关于基本坐标系的变换矩阵分别计算为：

进一步的，

和/>

分别是左相机和右相机的头眼参数，^NT₆和^NT₉分别是左眼坐标系和右眼坐标系关于基础坐标系的变换矩阵。

实例的，本发明实施例中实际情况中由于电动机伺服误差和特征点提取误差，有时固定点p不能同时位于L_l和L_r上，如图4所示。在这种情况下，仍将点p的预期像素坐标固定在主点上。

进一步的，L_l和L_r是同时通过固定点p的理想光轴。L′_l和L′_r是实际的光轴，是两条偏斜线。点A和B在L′_l上。点C和D在L′_r上。

进一步的，在双目相机的初始状态下，L′_l和L′_r分别与

和/>

轴的方向相同。

具体的，点A，B，C，D的齐次坐标分别设置为(x_a，y_a，z_a，1)，(x_b，y_b，z_b，1)，(x_c，y_c，z_c，1)和(x_d，y_d，z_d，1)。

具体的，点A，B，C，D的初始齐次坐标分别设置为(0，0，z_a′，1)，(0，0，z_b′，1)，(0，0，z_c′，1)和(0，0，z_d′，1)。

实例的，点A，B，C，D的初始齐次坐标分别为(0，0，10，1)，(0，0，20，1)，(0，0，10，1)和(0，0，20，1)。

实例的，A的齐次坐标为：

进一步的，同理可得点B，C和D的齐次坐标。

具体的，将x_b-x_a，y_b-y_a，z_b-z_a，x_d-x_c，y_d-y_c和z_d-z_c别定义为X₁，Y₁，Z₁，X₂，Y₂和Z₂。

即，X₁＝x_b-x_a，Y₁＝y_b-y_a，Z₁＝z_b-z_a，X₂＝x_d-x_c，Y₂＝y_d-y_c和Z₂＝z_d-z_c，

具体的，L′_l和L′_r分别定义为

具体的，L′_l和L′_r的公垂线的方向向量定义为：

由L′_l和公共垂直线确定的平面为：

X_cp(X-x_a)+Y_cp(Y-y_a)+Z_cp(Z-z_a)＝0

实例的，解得坐标p₁＝(u₁，v₁)，同理得到坐标p₂＝(u₂，v₂)。

具体的，通过确定坐标系中点p₁和p₂的中点坐标来确定固定点坐标

从以上的描述中，可以看出，在本专利提出的方法中，将不再使用图像视差或立体外部参数。相反，通过视觉注视驱动两个相机注视光学中心的3D空间点p。因此，p同时位于两个相机的光轴上，或者说p是两个相机的固定点。可以通过两个相机的两个光轴的交点获得p的3D坐标。如果提供关节位置反馈和头眼参数，则可以通过正向运动学推导两个光轴。

在实际应用中，由于视觉注视误差和模型误差，两个光轴可能不在同一平面上，因此将两个偏斜光轴的共同垂直线段的中点作为固定点的表示。

基于视觉注视的3D三角测量法的优点是不再需要直接的立体外部参数。另一方面，由于固定点是通过两条偏斜线的交点计算的，因此图像检测的误差容忍度高于基于视差的三角测量法。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机***中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

根据本发明实施例，提供了一种基于双目相机的3D三角测量装置，该装置包括：

坐标系建立模块：用于建立双目相机的坐标系，并获取坐标系参数；

信息获取模块：用于通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；

偏差减小模块：用于通过双目相机的运动减小空间点的实际像素和预期像素的偏差；

固定点获取模块：用于获取左相机和右相机的光轴，得到固定点p。

下面以具体示例，对基于双目相机的3D三角测量方法的模拟实验进行详细介绍：

对于模拟实验，执行静态实验和动态实验。

将机器人仿生眼的机械模型导入GAZEBO。诸如关节，连杆等参数被设置为与真实的机器人仿生眼相同。构建一个关节控制器，将关节速度命令发送到模拟的机器人仿生眼，并获得关节角反馈。

左右模拟相机的图像大小为1040×860像素。如下表所示，显示了使用Bouguet工具箱校准的两个模拟相机的固有参数，包括焦距和主点。以亚像素位置精度检测模拟的棋盘格角点。选择模拟棋盘格的第一个右上角点作为要注视的空间点p，该点称为固定点。通过视觉注视，两个相机中p点的预期像素坐标固定在主点(520.50，430.50)上。对于这两个模拟相机的头眼参数，坐标系O₆-x₆y₆z₆和O₉-x₉y₉z₉的Z轴各自仅偏差24.30mm。坐标系

和/>

的X和Y轴与坐标系O₆-x₆y₆z₆和O₉-x₉y₉z₉相同。在常数矩阵K中设置k₁＝1.8，k₂＝1.8。

在模拟实验中，放置了模拟棋盘格，将固定点设置在关于基础坐标系O_N-x_Ny_Nz_N的(-0.00，-145.00，1500)处。将模拟的机器人仿生眼设置为初始状态。通过视觉注视将固定点p的像素坐标在左相机中从(551.15，374.35)移动到(520.50，430.50)和在右相机中从(489.74，374.16)移动到(520.50，430.50)，总共需要600ms。在模拟实验中，使用本专利提出的方法需55μs来计算固定点的3D坐标。

在静态实验中，希望通过在固定点稳定的情况下来验证本专利提出的方法的有效性。

通过将模拟棋盘放置在21个不同的位置上，获得了21个不同的静态固定点。在每个位置上，使用本专利提出的方法在相同的时间间隔内记录基本坐标系O_N-x_Ny_Nz_N的静态固定点的估计3D坐标。静态固定点的3D坐标关于基础坐标系O_N-x_Ny_Nz_N的基准真值，如下表所示，可以从GAZEBO中获得。

使用本专利提出的方法，计算了3D坐标的基准真值与估计3D坐标的平均值之间的绝对误差。在模拟实验中的绝对误差主要来自正向运动学中的累积误差，使用Bouguet进行校正的误差以及前眼校正的误差。本专利提出的方法在X轴上的绝对误差在0.73mm和3.96mm之间；在Y轴上的绝对误差在1.14mm和6.18mm之间。当Z基准真值为1100mm时，在Z轴上的最小绝对误差为0.02mm。本专利提出的方法在Z轴上的最大绝对误差仅为深度基准真值的0.5％。

使用本专利提出的方法估计的3D坐标的不确定度可以通过应用不确定度的传递定理计算得出。估计的3D坐标的不确定度主要来自图像检测的不确定度。图像检测的不确定度可以通过检测点的像素坐标的不确定度来反映，定义为u_u和u_v。u_u和u_v可以计算如下：

其中u_i，v_i分别是像素坐标u，v的第i个独立观测值(i＝1，2，...，n)。

分别是像素坐标u，v的n个独立重复观测值的平均值。

假设本专利的方法在相同的时间t内，检测点的像素坐标与主点一致。在时间t之后，在本专利提出的方法中，通过视觉注视，仿生眼关节角θ_i(i＝4，5，7，8)随两个相机中检测点的像素坐标的变化而变化。对于左眼，关节角度θ₄，θ₅可以表示如下：

其中u_l，v_l是左相机中检测点的像素坐标。

是左相机中的主点。/>

分别是左相机在列和行方向上的焦距。右眼关节角θ₇，θ₈的计算方法与θ₄，θ₅相似。仿生眼关节角θ_i(i＝4，5，7，8)的不确定度可以通过在两个相机中检测点像素坐标传递的不确定度来计算。θ₄，θ₅的不确定度可计算如下：

在

的结果中，/>

在/>

的结果中，/>

右眼关节角θ₇，θ₈不确定度的计算方法与θ₄，θ₅相似。

使用正向运动学和头眼参数来计算两个相机的光轴；使用基于视觉注视的3D三角测量算法来计算被定义为(x_p，y_p，z_p)的检测点p的估计3D坐标。通过使用本专利提出的方法，估计的检测点p的3D坐标的不确定度可以通过仿生眼关节角θ_i(i＝4，5，7，8)传递的不确定度来计算。x_p，y_p，z_p的不确定度使用本专利提出的方法的可以表示为：

/>

其中，在

和/>

的结果中，/>

其中/>

是第i(i＝4，5，7，8)个关节角反馈结果的n个独立重复观测值的平均值。

如前所述，在21个不同的位置放置了一个棋盘。在每个位置，n＝1150。在X轴上的绝对不确定度在0.009mm至0.130mm之间；在Y轴上的绝对不确定度都在0.028mm至0.188mm之间；在Z轴上的绝对不确定度都在0.088mm至4.545mm之间。

在动态实验中，希望通过在固定点移动的情况下来验证本专利提出的方法的有效性。

将棋盘格放置在基础坐标系O_N-x_Ny_Nz_N的Z轴上1000mm处。

在关于基础坐标系O_N-x_Ny_Nz_N的X和Y轴上的固定点的轨迹分别为x＝r*sin(t/1000)和y＝-100+r*cos(t/1000)，其中r＝100mm。使用本专利提出的方法以相同的时间间隔记录了估计的移动固定点关于基础坐标系O_N-x_Ny_Nz_N的3D坐标。

本专利提出的方法计算了基准真值和估计结果之间的绝对误差，在X轴上的平均绝对误差为1.76mm；在Y轴上的平均绝对误差为2.03mm；在Z轴上的平均绝对误差为1.69mm，仅达到深度基准真值的0.17％。使用标准偏差计算本专利提出的方法在Z轴上的不确定度。在动态模拟实验中，n＝3780。本专利提出的方法在Z轴上的绝对不确定度为0.30mm。

下面以具体示例，对基于双目相机的3D三角测量方法的实物实验进行详细介绍：

对于实物实验，在设计号的机器人仿生眼上执行静态实验和动态实验。左右相机的图像大小为640×480像素。

如下表所示，显示了使用Bouguet工具箱校准的两个相机的固有参数，包括焦距和主点。

左右相机的失真系数分别为[-0.0437，0.1425，0.0005，-0.0012，0.0000]和[-0.0425，0.1080，0.0001，-0.0015，0.0000]。AprilTag信息是通过ViSP库获得的。选择AprilTag的中心(点p)作为固定点。左右相机中点p的预期像素坐标分别设置为主点(362.94，222.53)和(388.09，220.82)。在常数矩阵K中设置k₁＝3.6，k₂＝3.6。左右相机校准后的头眼参数如下所示：

在常数矩阵K中设置k₁＝4.0，k₂＝4.0。在实物实验中，放置AprilTag用于将固定点设置在关于基础坐标系O_N-x_Ny_Nz_N的(-0.00，-145.00，1500)处。将机器人仿生眼设置为初始状态。通过视觉注视将固定点p的像素坐标在左相机中从(385.98，196.60)移动到(362.94，222.53)和在右相机中从(361.09，191.82)移动到(388.09，220.82)，总共需要650ms。在实物实验中，使用本专利提出的方法计算固定点的3D坐标需要15μs。

在静态实验中，将AprilTag放置在21个不同的位置，以获得21个不同的静态固定点。在每个位置，以相同的时间间隔记录使用本专利提出的方法估计的静态固定点的3D坐标。选择AprilTag中心的3D坐标作为静态固定点的基准真值，如下表所示。

/>

使用本专利提出的方法计算了基准真值与估计3D坐标平均值之间的绝对误差。在物理实验中的绝对误差主要来自正向运动学的累积误差，固有参数校准，头眼校准和关节偏移的误差。

特别是当Z基准真值大于2000mm时，本专利提出的方法在Y轴上的绝对误差在0.55mm和12.28mm之间，本专利提出的方法在Z轴上的最小绝对误差为1.42mm，达到基准真值的0.23％。在Z轴上的最大绝对误差为124.49mm，达到深度基准真值的4.97％。

将AprilTag放置在21个不同的位置。在每个位置，n＝1200。使用本专利提出的方法估算的3D坐标在X轴上的绝对不确定度在0.119mm至3.905mm之间；在Y轴上的绝对不确定度都在0.091mm和0.640mm之间；在Z轴上的绝对不确定度都在0.268mm至7.975mm之间。

在动态实验中，移动AprilTag以将固定点的3D坐标的基准真值从(-278.60，-23.13、957.84)移动到(-390.02，-30.39、1111.91)，相对于基础坐标系O_N-x_Ny_Nz_N平均速度为0.01m/s。使用本专利提出的方法在相同的时间间隔内，记录了关于基础坐标系O_N-x_Ny_Nz_N估计的移动注视点的3D坐标。

使用本专利提出的方法计算了基准真值和估计结果之间的绝对误差。在X轴上的平均绝对误差为11.81mm；在Y轴上的平均绝对误差为14.89mm；在Z轴上的平均绝对误差为23.74mm。

显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (7)

1.一种基于双目相机的3D三角测量方法，其特征在于，包括：

建立双目相机的坐标系，并获取坐标系参数；

通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；

通过所述双目相机的运动减小空间点的实际像素和预期像素的偏差；

获取左相机和右相机的光轴，得到固定点p；

所述获取所述左相机和所述右相机的光轴，得到固定点p包括：

建立左眼坐标系为O₆-x₆y₆z₆，右眼坐标系为O₉-x₉y₉z₉，所述左相机坐标系为

所述右相机坐标系为/>

且L_l和L_r分别是所述左相机和所述右相机的光轴：

所述左相机和所述右相机坐标系关于基本坐标系的变换矩阵分别为：

其中，

和/>

分别是所述左相机和所述右相机的头眼参数，^NT₆和^NT₉分别是所述左眼坐标系和所述右眼坐标系关于基础坐标系的变换矩阵；

当所述左相机和所述右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为所述固定点p；

当所述左相机和所述右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为所述固定点p包括：

L_l和L_r是同时通过所述固定点p的理想光轴，L′_l和L′_r是实际的光轴，点A和B在L′_l上，点C和D在L′_r上；

在双目相机的初始状态下，L′_l和L′_r分别与

和/>

轴的方向相同；

点A，B，C，D的齐次坐标分别为(x_a,y_a,z_a,1)，(x_b，y_b，z_b,1)，(x_c，y_c，z_c，1)和(x_d，y_d,z_d,1)；点A，B，C，D的初始齐次坐标分别为(0，0，z_a′，1)，(0，0，z_b′，1)，(0,0,z_c′,1)和(0，0，z_d′，1)；

则，A的齐次坐标为：

同理可得点B，C和D的齐次坐标：

X₁＝x_b-x_a，Y₁＝y_b-y_a，Z₁＝z_b-z_a，X₂＝x_d-x_c，Y₂＝y_d-y_c和Z₂＝z_d-z_c，

则，L′_l和L′_r分别为

则，L′_l和L′_r的公垂线的方向向量为：

则，由L′_l和公共垂直线确定的平面为：

X_cp(X-x_a)+Y_cp(Y-y_a)+Z_cp(Z-z_a)＝0

则，解得坐标p₁＝(u₁，v₁)，同理得到坐标p₂＝(u₂,v₂)；

通过确定坐标系中点p₁和p₂的中点坐标来确定所述固定点坐标

2.根据权利要求1所述的一种基于双目相机的3D三角测量方法，其特征在于，所述建立双目相机的坐标系，并获取坐标系参数包括：

基于标准D-H方法建立坐标系；

参数d_i，θ_i，a_i，α_i分别是连杆偏距，关节角，连杆长度和连杆转角；关节偏移量是初始状态下θ_i的值；

坐标系i和i_1之间的转换矩阵为：

3.根据权利要求1所述的一种基于双目相机的3D三角测量方法，其特征在于，所述通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵包括：

所述左相机和所述右相机分别是左眼和右眼的末端执行器；

所述末端执行器的速度V_n＝[v_x v_y v_z ω_x ω_y ω_z]^T；其中，v_x，v_y，v_z是线速度，ω_x，ω_y，ω_z为角速度；

所述关节速度

所述关节雅可比矩阵

且所述

通过公式/>

确定；其中，Z_i和O_i是Z轴的方向向量和坐标系O_i-x_iy_iz_i的原点；

通过公式

将所述J_θ从基本坐标系转换到相机坐标系；其中，J_joint是变换后的关节雅可比矩阵，R是基础坐标系关于相机坐标系的旋转矩阵；

则，所述V_n和所述

之间的关系为/>

3D空间点的坐标p＝(u，v)；

则，像素速度

和所述V_n之间的关系为/>

其中，J_image是图像雅可比矩阵；

且，所述图像雅可比矩阵

其中，f_u和f_v分别是相机的列和行方向的焦距，Z_c是相机坐标系中空间点p的深度。

4.根据权利要求1所述的一种基于双目相机的3D三角测量方法，其特征在于，所述通过所述双目相机的运动减小空间点的实际像素和预期像素的偏差包括：

空间点的实际像素坐标和预期像素坐标分别为p和p^*；

则，所述p和所述p^*的偏差e＝p^*-p；

则，

其中，/>

是像素速度；

所述

其中，K是影响视觉注视性能的常数矩阵；

构造所述常数矩阵

其中，k₁和k₂是每个通道中的增益；

则，空间点像素速度

则，

在所述左相机和所述右相机的连接处建立坐标系O_N-x_Ny_Nz_N，则所述左眼关节速度矢量

则，

其中，p_l和p_l ^*是所述所述左相机中空间点的实际像素坐标和预期像素坐标；

则，所述左眼关节速度为

其中，/>

是矩阵J_l中第i行和第j列上的元素；

同理，所述右眼关节速度为

5.一种基于双目相机的3D三角测量装置，其特征在于，包括：

坐标系建立模块：用于建立双目相机的坐标系，并获取坐标系参数；

信息获取模块：用于通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；

偏差减小模块：用于通过所述双目相机的运动减小空间点的实际像素和预期像素的偏差；

固定点获取模块：用于获取所述左相机和所述右相机的光轴，得到所述固定点p；

所述获取所述左相机和所述右相机的光轴，得到固定点p包括：

建立左眼坐标系为O₆-x₆y₆z₆，右眼坐标系为0₉-x₉y₉z₉，所述左相机坐标系为

所述右相机坐标系为/>

且L_l和L_r分别是所述左相机和所述右相机的光轴；

所述左相机和所述右相机坐标系关于基本坐标系的变换矩阵分别为：

其中，

和/>

分别是所述左相机和所述右相机的头眼参数，^NT₆和^NT₉分别是所述左眼坐标系和所述右眼坐标系关于基础坐标系的变换矩阵；

当所述左相机和所述右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为所述固定点p；

当所述左相机和所述右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为所述固定点p包括：

L_l和L_r是同时通过所述固定点p的理想光轴，L′_l和L′_r是实际的光轴，点A和B在L′_l上，点C和D在L′_r上；

在双目相机的初始状态下，L′_l和L′_r分别与

和/>

轴的方向相同；

点A，B，C，D的齐次坐标分别为(x_a,y_a,z_a,1)，(x_b,y_b,z_b,1)，(x_c，y_c，z_c，1)和(x_d，y_d,z_d,1)；点A，B，C，D的初始齐次坐标分别为(0，0，z_a′，1)，(0,0,z_b′，1)，(0,0,z_c′，1)和(0，0，z_d′，1)；

则，A的齐次坐标为：

同理可得点B，C和D的齐次坐标；

X₁＝x_b-x_a，Y₁＝y_b-y_a，Z₁＝z_b-z_a，X₂＝x_d-x_c，Y₂＝y_d-y_c和Z₂＝z_d-z_c，

则，L′_l和L′_r分别为

则，L′_l和L′_r的公垂线的方向向量为：

则，由L′_l和公共垂直线确定的平面为：

X_cp(X-x_a)+Y_cp(Y-y_a)+Z_cp(Z-z_a)＝0

则，解得坐标p₁＝(u₁,v₁)，同理得到坐标p₂＝(u₂,v₂)；

通过确定坐标系中点p₁和p₂的中点坐标来确定所述固定点坐标

6.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行权利要求1-4任一项所述的一种基于双目相机的3D三角测量方法。

7.一种电子设备，其特征在于，包括：至少一个处理器；以及与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器执行权利要求1-4任一项所述的一种基于双目相机的3D三角测量方法。

Images