CN109754428B - 一种用于水下双目视觉定位误差的测量方法 - Google Patents

Abstract

本发明提出一种用于水下双目视觉定位误差的测量方法，首先求得构建的世界坐标系以及支架坐标系之间的转换矩阵，然后由三维定位系统测出空气中目标物体的世界坐标。利用转换矩阵和目标物体的世界坐标，计算得出支架坐标系下目标物体的坐标，进而通过坐标平移得出双目相机坐标系下目标物体的坐标，此时的值作为目标物体在相机坐标系下的理论真值。在水下测试环境中，由双目相机测出目标物体在水下的相机坐标系坐标，通过与计算得出的理论真值进行比较，得出要求的测试误差。在实验的验证过程中，为了验证实验结果的可靠性，可以用双目相机对不同位置的待测目标进行测试。本发明在确保误差分析精度的基础上，极大地降低了双目相机定位误差测量的复杂度。

Description

一种用于水下双目视觉定位误差的测量方法

技术领域

本发明涉及视觉定位误差测量技术领域，具体为一种用于水下双目视觉定位误差的测量方法，是一种结合三维定位系统进行视觉误差测试的方法，适用于水下目标物体三维视觉定位误差测量。

背景技术

视觉定位是通过视觉传感器获取环境信息图像，并通过视觉处理获得目标物体的定位信息。视觉中的视觉引导功能已经广泛应用于汽车、电子、农业等领域。对于视觉引导而言，定位的误差是一个重要的问题。能够确定自身携带的双目相机视觉定位的误差范围，是水下机器人成功执行抓取等任务的必要条件。实际上，复杂的水下环境给机器人视觉误差测试带来很多困难，水下机器人不能对双目视觉测试距离的误差进行监测。传统的视觉测距误差校正只会在执行任务之前，而且误差测试过程非常复杂，因此在任务过程中获取定位误差信息显得尤为重要。

很多研究者尝试通过改进双目视觉的定位方法来修正误差，但是在任务执行过程中双目相机的定位误差分析却不多见。有文献利用四轴视觉运动平台，研究了机器人视觉系统的跟踪误差问题，通过采用典型的几何图案，测试了定位误差并分析了规律，机器人系统对于不同的图案，有不同的定位误差。由于该方法需要提前判断物体形状，工作过程较为复杂，工作量较大，不适用于水下视觉误差测试。

发明内容

为了解决现有技术存在的问题，本发明提出一种用于水下双目视觉定位误差的测量方法，利用三维定位系统建立世界坐标系，并通过支架坐标系的构建简化相机坐标系的测量。世界坐标系与支架坐标系的构建以及协同工作极大地简化了计算过程，而且测试平台便于搭建，不仅能够用于水下环境，还能够推广到航天等领域。

三维定位系统通过对特定标定物的标定，完成世界坐标系的构建。在实验之前首先要确定三维定位系统的工作范围，选取合适的位置作为世界坐标系原点。如图1所示的标定物位置，以四个标定物组成的两条直线的交点为坐标系原点，短轴为世界坐标系Y_e轴，长轴为世界坐标系X_e轴。为了便于识别，同时减小标定误差，标定物选择标准的荧光小球，并且标定物的直径远小于三维定位系统的工作范围。在申请人所做实验中，将使用的标定物放在直角杆上，其中长边70cm，短边40cm。标定杆的颜色为黑色，与标定物有区分度，对三维定位系统识别不造成干扰。

三维定位系统通过对特定标志物的标定，完成支架坐标系的构建。标志物的位置如图2所示A，B，C三点处，本发明通过运用三个标志物构建支架坐标系，以BC方向为Y轴，以BA方向为X轴，Z轴通过右手法则确定。相机坐标系原点位于双目相机的左相机位置，即支架坐标系原点，双目相机的两个摄像头连线方向为X_c轴，与支架坐标系X轴平行，Y_c轴与支架坐标系Y轴平行，支架坐标系X轴与Y轴垂直。

在进行双目相机的图像采集之前，需要利用计算机以及棋盘对双目相机完成标定，过程为：使用MATLAB对相机的内外参数进行计算，然后将参数结果输入OpenCV的视觉测距程序，最后完成视觉标定。

本发明基本的计算思路是，首先求得构建的世界坐标系以及支架坐标系之间的转换矩阵，然后由三维定位系统测出空气中目标物体的世界坐标。利用转换矩阵和目标物体的世界坐标，计算得出支架坐标系下目标物体的坐标，进而通过坐标平移得出双目相机坐标系下目标物体的坐标，此时的值作为目标物体在相机坐标系下的理论真值。在水下测试环境中，由双目相机测出目标物体在水下的相机坐标系坐标，通过与计算得出的理论真值进行比较，得出要求的测试误差。在实验的验证过程中，为了验证实验结果的可靠性，可以用双目相机对不同位置的待测目标进行测试。

经过实验证明，支架坐标系与相机坐标系的转化过程存在有明显的向量平移。向量数据主要有三方面，首先是支架坐标系X轴产生的偏航值，其次是支架坐标系Y轴产生的俯仰值，最后是Z轴方向的垂直距离。在数据处理过程中，将以上三种常值转化为三维坐标值进行坐标平移，本方法只涉及到了物理结构转换，简化了坐标计算过程，有效提高了相机测试误差精度。

基于上述原理，本发明的技术方案为：

所述一种用于水下双目视觉定位误差的测量方法，其特征在于，包括以下步骤：

步骤1：在水箱上方的空气中布置四个标定物，其中三个标定物固定在一根连杆上，两个标定物布置在另一根连杆上，两根连杆垂直相交于其中一个标定物；所述的标定物能够被三维定位系统拍摄，并能够被清晰识别；以两根连杆垂直相交的标定物中心点为世界坐标系的原点O_e，以一根连杆作为世界坐标系Y_e轴，以另一根连杆作为世界坐标系X_e轴；

步骤2：在水箱内部放置一个目标物体，此时水箱内未装水，通过三维定位系统获取目标物体在世界坐标系的定位数据(X_n,Y_n,Z_n)，并将(X_n,Y_n,Z_n)视作目标物体在世界坐标系的坐标真值；

步骤3：通过以下过程计算支架坐标系：

步骤3.1：在水箱内部放置标定好的双目相机，双目相机光轴垂直指向水箱底部，在双目相机的相机坐标系原点正上方空间中布置一个标定物作为支架坐标系的原点，两个相互垂直的连杆在该点处相交，在这两个相互垂直的连杆上分别再放置一个标定物；通过三维定位系统测得三个标定物在世界坐标系下的坐标值A(X₁,Y₁,Z₁)，B(X₂,Y₂,Z₂)，C(X₃,Y₃,Z₃)，B点为支架坐标系原点；以BA方向为支架坐标系X方向，BC为支架坐标系Y方向，根据右手法则确定Z方向；

步骤3.2：根据公式

计算支架坐标系X轴、Y轴、Z轴的方向向量，其中

表示X轴方向向量，

表示Y轴方向向量，

表示Z轴方向向量；其中

步骤3.3：根据世界坐标系的定义，确定世界坐标系中三个方向向量分别为

步骤3.4：根据公式：

计算支架坐标系相对于世界坐标系的旋转矩阵

步骤4：根据公式：

计算目标在支架坐标系中的坐标值²P，其中

¹P为步骤2得到的目标在世界坐标系中的坐标真值；¹P_2ORG为支架坐标系原点在世界坐标系中的表示¹P_2ORG＝(X₂,Y₂,Z₂)；

步骤5：根据公式：

^cP＝²P-O_cO'

其中^cP表示目标在相机坐标系中的坐标，O_cO'为相机坐标系与支架坐标系之间的平移向量；

步骤6：将标定好的双目相机放入密封舱内，在水箱中装水，在水下环境中通过双目相机对目标物体的坐标进行测量，测量值表示为^aP；

步骤7：根据公式

计算测试误差

有益效果

本发明的有益效果是，设计了一套简单易行，双系统融合且精度较高的双目视觉定位误差测量方法。通过合理的布置四个标定物，对三维定位系统进行世界坐标系标定，再利用三个标定物，通过三维定位系统的识别，构建支架坐标系。在确保误差分析精度的基础上，极大地降低了双目相机定位误差测量的复杂度。也可以将这种测量方案进行推广，应用其他领域的误差测量。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为全局坐标系标定示意图；

图2为系统误差测量模型。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本发明提出一种用于水下双目视觉定位误差的测量方法。利用三维定位系统，能够构建出世界坐标系。通过支架坐标系的构建，简化相机坐标系的测量。世界坐标系与支架坐标系的构建以及协同工作极大地简化了计算过程，而且测试平台便于搭建，不仅能够用于水下环境，还能够推广到航天等领域。

一、系统组成

三维定位系统通过对标定物的标定，完成世界坐标系的构建。在实验之前首先要确定三维定位系统的工作范围，选取合适的位置作为世界坐标系原点。如图1所示的标定物位置，以四个标定物组成的两条直线的交点为坐标系原点，短轴为世界坐标系Y_e轴，长轴为世界坐标系X_e轴。为了便于识别，同时减小标定误差，标定物选择标准的荧光小球，并且标定物的直径远小于三维定位系统的工作范围。在发明人所做实验中，将使用的标定物放在直角杆上，其中长边70cm,短边40cm。标定杆的颜色为黑色，与标定物有区分度，对三维定位系统识别不造成干扰。

三维定位系统通过对标志物的标定，完成支架坐标系的构建。标志物的位置如图2所示的红色点处，通过运用三个标志物构建支架坐标系，以BC方向为Y轴，以BA方向为X轴，Z轴通过右手法则确定。相机坐标系原点位于双目相机的左相机位置，即支架坐标系原点，双目相机的两个摄像头连线方向为X_c轴，与支架坐标系X轴平行，Y_c轴与支架坐标系Y轴平行。

二、水下双目视觉定位的误差测试方法

本发明基本的计算思路是，首先求得构建的世界坐标系以及支架坐标系之间的转换矩阵，然后由三维定位系统测出空气中目标物体的世界坐标。利用转换矩阵和目标物体的世界坐标，计算得出支架坐标系下目标物体的坐标，进而通过坐标平移得出双目相机坐标系下目标物体的坐标，此时的值作为目标物体在相机坐标系下的理论真值。在水下测试环境中，由双目相机测出目标物体在水下的相机坐标系坐标，通过与计算得出的理论真值进行比较，得出要求的测试误差。在实验的验证过程中，为了验证实验结果的可靠性，可以用双目相机对不同位置的待测目标进行测试。

三、向量平移分析以及处理方法

经过实验证明，支架坐标系与相机坐标系的转化过程存在有明显的向量平移。向量数据主要有三方面，首先是支架坐标系X轴产生的偏航值，其次是支架坐标系Y轴产生的俯仰值，最后是Z轴方向的垂直距离。在数据处理过程中，将以上三种常值转化为三维坐标值进行坐标平移，本方法只涉及到了物理结构转换，简化了坐标计算过程，有效提高了相机测试误差精度。

本发明的具体步骤为：

步骤1：在水箱上方的空气中布置四个标定物，其中三个标定物固定在一根连杆上，两个标定物布置在另一根连杆上，两根连杆垂直相交于其中一个标定物；所述的标定物能够被三维定位系统拍摄，并能够被清晰识别；以两根连杆垂直相交的标定物中心点为世界坐标系的原点O_e，以一根连杆作为世界坐标系Y_e轴，以另一根连杆作为世界坐标系X_e轴。

步骤2：在水箱内部放置一个目标物体，此时水箱内未装水，通过三维定位系统获取目标物体在世界坐标系的定位数据(X_n,Y_n,Z_n)，并将(X_n,Y_n,Z_n)视作目标物体在世界坐标系的坐标真值。

步骤3：通过以下过程计算支架坐标系：

步骤3.1：在水箱内部放置标定好的双目相机，双目相机光轴垂直指向水箱底部，在双目相机的相机坐标系原点正上方空间中布置一个标定物作为支架坐标系的原点，两个相互垂直的连杆在该点处相交，在这两个相互垂直的连杆上分别再放置一个标定物，如图2所示；通过三维定位系统测得三个标定物在世界坐标系下的坐标值A(X₁,Y₁,Z₁)，B(X₂,Y₂,Z₂)，C(X₃,Y₃,Z₃)，B点为支架坐标系原点；以BA方向为支架坐标系X方向，BC为支架坐标系Y方向，根据右手法则确定Z方向。

步骤3.2：根据公式

计算支架坐标系X轴、Y轴、Z轴的方向向量，其中

表示X轴方向向量，

表示Y轴方向向量，

表示Z轴方向向量；其中

步骤3.3：根据世界坐标系的定义，确定世界坐标系中三个方向向量分别为

步骤3.4：根据公式：

计算支架坐标系相对于世界坐标系的旋转矩阵

步骤4：根据公式：

计算目标在支架坐标系中的坐标值²P，其中

¹P为步骤2得到的目标在世界坐标系中的坐标真值；¹P_2ORG为支架坐标系原点在世界坐标系中的表示¹P_2ORG＝(X₂,Y₂,Z₂)。

步骤5：根据公式：

^cP＝²P-O_cO'

其中^cP表示目标在相机坐标系中的坐标，O_cO'为相机坐标系与支架坐标系之间的平移向量，数据形式表示为三维列向量；可以通过多次测量相机坐标系与支架坐标系之间的平移向量，去除不合理数据，取数据平均值作为O_cO'取值。

步骤6：将标定好的双目相机放入密封舱内，在水箱中装水，在水下环境中通过双目相机对目标物体的坐标进行测量，测量值表示为^aP。

步骤7：根据公式

计算测试误差

其数据形式表示为三维列向量。

为了验证结果的可靠性，改变待测目标物体的位置进行多次实验，对实验误差进行测量。

基于上述技术方案，下面给出两个实施实例(数据单位mm)：

【实施例一】

第一步：测得目标物体的世界坐标为

(x_o1,y_o1,z_o1)＝(369.53；297.02；-739.97)

第二步：通过三维定位系统测得支架坐标系标定物的世界坐标值，记作

A(X₁,Y₁,Z₁)＝(492.40；291.19；-64.47)

B(X₂,Y₂,Z₂)＝(413.80；278.47；-65.05)

C(X₃,Y₃,Z₃)＝(399.53；363.99；-60.31)

第三步：根据上步数据得出支架坐标系X轴、Y轴、Z轴的方向向量

第四步：支架坐标系相对于世界坐标系的旋转矩阵

第五步：目标物体在支架坐标系的坐标

²P＝[45.6532；-11.2976；674.9779]

平移向量O_cO'＝[35；10；90]

^cP＝[10.6532；-21.2976；584.9779]

第六步：相机测得^aP＝[12.7460；-24.8200；577.5830]

第七步：最后所求误差

【实施例二】

第一步：测得目标物体的世界坐标为

(x_o1,y_o1,z_o1)＝(374.68；408.05；-848.45)

第二步：通过三维定位系统测得支架坐标系标定物的世界坐标值，记作

A(X₁,Y₁,Z₁)＝(492.40；291.19；-64.47)

B(X₂,Y₂,Z₂)＝(413.80；278.47；-65.05)

C(X₃,Y₃,Z₃)＝(399.53；363.99；-60.31)

第三步：根据上步数据得出支架坐标系X轴、Y轴、Z轴的方向向量

第四步：支架坐标系相对于世界坐标系的旋转矩阵

第五步：目标物体在支架坐标系的坐标

²P＝[23.6226；91.2869；789.3999]

平移向量O_cO'＝[35；10；90]，

^cP＝[-12.6226；81.2869；699.3999]

第六步：相机测得^aP＝[-16.2400；75.6400；694.6610]

第七步：最后所求误差

误差分析：本例中，水下双目视觉定位误差测量结果小于9mm

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

1.一种用于水下双目视觉定位误差的测量方法，其特征在于，包括以下步骤：

步骤1：在水箱上方的空气中布置四个标定物，其中三个标定物固定在一根连杆上，两个标定物布置在另一根连杆上，两根连杆垂直相交于其中一个标定物；所述的标定物能够被三维定位系统拍摄，并能够被清晰识别；以两根连杆垂直相交的标定物中心点为世界坐标系的原点O_e，以一根连杆作为世界坐标系Y_e轴，以另一根连杆作为世界坐标系X_e轴；

步骤2：在水箱内部放置一个目标物体，此时水箱内未装水，通过三维定位系统获取目标物体在世界坐标系的定位数据(X_n,Y_n,Z_n)，并将(X_n,Y_n,Z_n)视作目标物体在世界坐标系的坐标真值；

步骤3：通过以下过程计算支架坐标系：

步骤3.1：在水箱内部放置标定好的双目相机，双目相机光轴垂直指向水箱底部，在双目相机的相机坐标系原点正上方空间中布置一个标定物作为支架坐标系的原点，两个相互垂直的连杆在该点处相交，在这两个相互垂直的连杆上分别再放置一个标定物；通过三维定位系统测得三个标定物在世界坐标系下的坐标值A(X₁,Y₁,Z₁)，B(X₂,Y₂,Z₂)，C(X₃,Y₃,Z₃)，B点为支架坐标系原点；以BA方向为支架坐标系X方向，BC为支架坐标系Y方向，根据右手法则确定Z方向；

步骤3.2：根据公式

计算支架坐标系X轴、Y轴、Z轴的方向向量，其中

表示X轴方向向量，

表示Y轴方向向量，

表示Z轴方向向量；其中

步骤3.3：根据世界坐标系的定义，确定世界坐标系中三个方向向量分别为

步骤3.4：根据公式：

计算支架坐标系相对于世界坐标系的旋转矩阵

步骤4：根据公式：

计算目标物体在支架坐标系中的坐标值²P，其中

¹P为步骤2得到的目标物体在世界坐标系中的坐标真值；¹P_2ORG为支架坐标系原点在世界坐标系中的表示¹P_2ORG＝(X₂,Y₂,Z₂)；

步骤5：根据公式：

^cP＝²P-O_cO'

其中^cP表示目标物体在相机坐标系中的坐标，O_cO'为相机坐标系与支架坐标系之间的平移向量；

步骤6：将标定好的双目相机放入密封舱内，在水箱中装水，在水下环境中通过双目相机对目标物体的坐标进行测量，测量值表示为^aP；

步骤7：根据公式

计算测试误差

Images