本发明涉及视觉的目标姿态误差测量技术领域，具体为一种用于水下双目视觉的目标姿态误差测量方法，是一种结合三维定位系统进行视觉目标姿态误差测试的方法，适用于水下目标物体三维视觉姿态误差测量。
背景技术：
视觉目标姿态测量是通过视觉传感器获取目标信息图像，并通过视觉处理获得目标的姿态信息。目标姿态信息对目标运动状态分析、故障分析等方面都具有重要的理论意义与工程价值，在航天、交通、工业自动化等领域扮演着举足轻重的作用。对于目标运动状态分析等功能而言，目标姿态误差是一个重要问题。能够确定自身携带的双目相机视觉的目标姿态误差范围，是水下机器人成功获取目标姿态信息并执行抓取等任务的必要条件。实际上，复杂的水下环境给机器人视觉的目标姿态误差测试带来很多困难，导致目前水下机器人不能对双目视觉的目标姿态误差进行监测。传统的视觉目标姿态误差校正只能在执行任务之前进行，而且误差测量过程非常复杂。如何在任务过程中实时获取目标姿态误差信息就成为一个需要研究的重要问题。
很多研究者尝试通过改进双目视觉的目标姿态测量方法来修正误差，但是能够在任务执行过程中实时监测双目相机的目标姿态误差，并应用于水下环境的测试方法却不多见。有文献提出一种基于视觉快速实现复杂构件装夹位姿误差在线测量的方法，首先进行参考框架生成，得到参考坐标系；再进行点云坐标系变换和分割，得到实测位姿模型；最后将实测位姿模型和参考位姿模型对比进行装夹位姿误差。有文献还提出了一种移动工业机器人单点多视角挂表位置和姿态误差的检测方法，通过多次拍照的图片进行模板匹配比较，确定特征点的相对误差，从而最终确定末端执行器的位置和姿态误差。它们的方法步骤非常繁琐复杂，不能够实时监测目标姿态误差，而且不适用于水下环境。
技术实现要素：
为了解决现有技术存在的问题，本发明提出一种用于水下双目视觉的目标姿态误差测量方法。利用三维定位系统完成世界坐标系的标定，并通过支架坐标系的构建简化相机坐标系以及目标物体坐标系的测量。世界坐标系与支架坐标系以及目标坐标系的构建以及协同工作极大地简化了姿态误差的计算过程，而且测试平台便于搭建，测试过程非常简便。
世界坐标系的构建主要通过三维定位系统对特定标定物进行标定来实现。在进行标定工作之前首先要确定三维定位系统的工作范围，并选取合适的位置作为世界坐标系原点。如图1所示的四个标定物位置，由四个标定物组成的两条直线的交点作为坐标系原点，短轴为世界坐标系ye轴，长轴为世界坐标系xe轴。为了便于识别，同时减小标定数据误差，标定物选择标准的荧光小球，并且标定物的直径远小于三维定位系统的工作范围。在申请人所做实验中，将使用的标定物放在直角杆上，其中长边70cm，短边40cm。标定杆的颜色为黑色，与标定物有区分度，对三维定位系统识别标定物过程不造成干扰。
支架坐标系的构建也是利用三维定位系统通过对标定物进行标定来实现。标定物的位置如图2所示a，b，c三点处，本发明通过运用三个标定物构建支架坐标系，以bc方向为y轴，以ba方向为x轴，z轴则通过右手法则确定。而相机坐标系原点位于双目相机的左相机位置，即支架坐标系原点正下方，双目相机的两个摄像头连线方向为xc轴，与支架坐标系x轴平行，yc轴与支架坐标系y轴平行，支架坐标系的x轴与y轴垂直。
在进行双目相机的图像采集之前，需要利用计算机以及棋盘对双目相机完成标定，过程为：使用matlab对相机的内外参数进行计算，然后将参数结果输入opencv的视觉测距程序，最后完成视觉标定。
此外，还要构建目标坐标系，也是利用三维定位系统通过对标定物进行标定来实现。标定物的位置如图2所示d，e，f三点处，本发明通过运用三个标定物构建目标坐标系，以ef方向为yt轴，以ed方向为xt轴，zt轴则通过右手法则确定。
本发明基本的计算思路是，首先由三维定位系统测出构建支架坐标系的标定物的世界坐标系坐标值，然后由三维定位系统测出空气中构建目标坐标系的标定物的世界坐标系坐标值。利用支架坐标系与目标坐标系的标定物坐标值分别构建三维坐标系坐标向量，计算得出支架坐标系与目标坐标系的旋转矩阵，此时的姿态欧拉角值作为目标物体在相机坐标系下的理论姿态真值。在水下测试环境中，由双目相机测出目标物体标定物在水下的相机坐标系坐标，利用相机坐标系坐标值构建目标三维坐标向量，计算得出水下相机坐标系与目标坐标系的旋转矩阵。进一步计算实验前后姿态欧拉角变化值，得出要求的测试误差。在实验的验证过程中，为了验证实验结果的可靠性，可以采用双目相机对不同的待测目标进行测试。
经过实验证明，支架坐标系与相机坐标系的转化过程存在有明显的向量平移。向量数据主要有三方面，首先是支架坐标系x轴，y轴产生的平移距离，其次是z轴方向的垂直距离。在数据处理过程中，将以上三种常值转化为三维坐标值进行坐标平移，本方法只涉及到了物理结构转换，简化了坐标计算过程，有效提高了相机测试误差精度。
基于上述原理，本发明的技术方案为：
所述一种用于水下双目视觉的目标姿态误差测量方法，包括以下步骤：
步骤1：在水箱上方的空气中布置四个标定物，其中三个标定物固定在一根连杆上，两个标定物布置在另一根连杆上，两根连杆垂直相交于其中一个标定物；所述的标定物能够被三维定位系统拍摄，并能够被清晰识别；以两根连杆垂直相交的标定物中心点为世界坐标系的原点oe，以一根连杆作为世界坐标系ye轴，以另一根连杆作为世界坐标系xe轴；根据世界坐标系的定义，世界坐标系中三个方向向量分别为
步骤2：通过以下过程计算支架坐标系：
步骤2.1：在水箱内部放置标定好的双目相机，此时水箱内未装水，双目相机光轴垂直指向水箱底部，在双目相机的相机坐标系原点正上方空间中布置一个标定物作为支架坐标系的原点，两个相互垂直的连杆在该点处相交，在这两个相互垂直的连杆上分别再放置一个标定物；通过三维定位系统测得三个标定物在世界坐标系下的坐标值a(x1,y1,z1)，b(x2,y2,z2)，c(x3,y3,z3)，b点为支架坐标系原点；以ba方向为支架坐标系x方向，bc为支架坐标系y方向，根据右手法则确定z方向；
步骤2.2：根据公式
计算支架坐标系x轴、y轴、z轴的方向向量，其中
其特征在于：还包括以下步骤：
步骤3：通过以下过程计算目标坐标系：
步骤3.1：在水箱内部放置一个目标物体，此时水箱内未装水，在目标上放置一个标定物e作为目标坐标系的原点ot'，并且放置额外两个标定物d,f，且de垂直于ef；通过三维定位系统测得目标坐标系标定物在世界坐标系下的坐标值d(x4,y4,z4)，e(x5,y5,z5)，f(x6,y6,z6)，以ed方向为xt方向，ef为yt方向，根据右手法则确定zt方向；
步骤3.2：根据公式
计算目标坐标系xt轴、yt轴、zt轴的方向向量，其中
步骤4：根据公式：
计算全局坐标系下，支架坐标系相对于目标坐标系的旋转矩阵
步骤5：通过以下过程计算相机坐标系下的目标坐标系：
步骤5.1：将标定好的双目相机放入密封舱内，在水箱中装水，在水下环境中通过双目相机对目标物体的标定物d,e,f坐标进行测量，得到标定物d,e,f在相机坐标系下的坐标值d'(x4',y4',z4')，e'(x5',y5',z5')，f'(x6',y6',z6')；
步骤5.2：根据公式
计算相机坐标系下的目标坐标系三轴的方向向量
步骤6：根据公式
计算相机坐标系下，相机坐标系相对于目标坐标系的旋转矩阵
步骤7：根据公式
计算当前实验下目标物体姿态前后变化的旋转矩阵r，其中
步骤8：根据步骤7得到的旋转矩阵r解算出当前实验下的姿态误差值：俯仰角r1，横滚角r2，偏航角r3。
进一步的，调用matlab函数dcm2angle()对旋转矩阵r进行解算，得到当前实验下的姿态误差值。
有益效果
本发明的有益效果是：利用三维定位系统完成世界坐标系的标定，并通过支架坐标系的构建简化相机坐标系以及目标物体坐标系的测量。世界坐标系与支架坐标系以及目标坐标系的构建以及协同工作极大地简化了姿态误差的计算过程，而且测试平台便于搭建，测试过程非常简便。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1为全局坐标系标定示意图；
图2为系统误差测量模型。
具体实施方式
下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
本发明提出一种用于水下双目视觉的目标姿态误差测量方法。利用三维定位系统完成世界坐标系的标定，并通过支架坐标系的构建简化相机坐标系以及目标物体坐标系的测量。世界坐标系与支架坐标系以及目标坐标系的构建以及协同工作极大地简化了姿态误差的计算过程，而且测试平台便于搭建，测试过程非常简便。
一、系统组成
世界坐标系的构建主要通过三维定位系统对特定标定物进行标定。在进行标定工作之前首先要确定三维定位系统的工作范围，并选取合适的位置作为世界坐标系原点。如图1所示的四个标定物位置，由标定物组成的两条直线的交点作为坐标系原点，短轴为世界坐标系ye轴，长轴为世界坐标系xe轴。为了便于识别，同时减小标定数据误差，标定物选择标准的荧光小球，并且标定物的直径远小于三维定位系统的工作范围。在发明人所做实验中，将使用的标定物放在直角杆上，其中长边70cm,短边40cm。标定杆的颜色为黑色，与标定物有区分度，对三维定位系统识别标定物过程不造成干扰。
支架坐标系的构建，需要三维定位系统通过对特定标定物进行标定。标定物的位置如图2所示a，b，c三点处，本发明通过运用三个标定物构建支架坐标系，以bc方向为y轴，以ba方向为x轴，z轴则通过右手法则确定。相机坐标系原点位于双目相机的左相机位置，即支架坐标系原点正下方，双目相机的两个摄像头连线方向为xc轴，与支架坐标系x轴平行，yc轴与支架坐标系y轴平行，支架坐标系的x轴与y轴垂直。
目标坐标系的构建，需要三维定位系统通过对特定标定物进行标定。标定物的位置如图2所示d，e，f三点处，本发明通过运用三个标定物构建目标坐标系，以ef方向为yt轴，以ed方向为xt轴，zt轴则通过右手法则确定。
二、水下双目视觉目标姿态的误差测试方法
本发明基本的计算思路是，首先由三维定位系统测出构建支架坐标系标定物的世界坐标系坐标值，然后由三维定位系统测出空气中目标坐标系标定物的世界坐标系坐标值。利用支架坐标系与目标坐标系的标定物坐标值分别构建三维坐标系坐标向量，计算得出全局坐标系下，支架坐标系相对于目标坐标系的旋转矩阵；由于全局坐标系下，支架坐标系与相机坐标系的姿态相同，所以这也是相机坐标系相对于目标坐标系的旋转矩阵，此时旋转矩阵反映的姿态欧拉角值作为目标物体在相机坐标系下的理论姿态真值。
在水下测试环境中，由双目相机测出目标物体标定物在水下的相机坐标系坐标，利用相机坐标系下目标物体标定物三维坐标，计算相机坐标系下，相机坐标系相对于目标坐标系的旋转矩阵。进一步计算实验前后姿态欧拉角变化值，得出要求的测试误差。在实验的验证过程中，为了验证实验结果的可靠性，可以采用双目相机对不同的待测目标进行测试。
三、向量平移分析以及处理方法
经过实验证明，支架坐标系与相机坐标系的转化过程存在有明显的向量平移。向量数据主要有三方面，首先是支架坐标系x轴，y轴产生的平移距离，其次是z轴方向的垂直距离。在数据处理过程中，将以上三种常值转化为三维坐标值进行坐标平移，本方法只涉及到了物理结构转换，简化了坐标计算过程，有效提高了相机测试误差精度。
基于上述技术方案，下面给出具体实施例：
步骤1：在水箱上方的空气中布置四个标定物，其中三个标定物固定在一根连杆上，两个标定物布置在另一根连杆上，两根连杆垂直相交于其中一个标定物；所述的标定物能够被三维定位系统拍摄，并能够被清晰识别；以两根连杆垂直相交的标定物中心点为世界坐标系的原点oe，以一根连杆作为世界坐标系ye轴，以另一根连杆作为世界坐标系xe轴；根据世界坐标系的定义，世界坐标系中三个方向向量分别为
步骤2：通过以下过程计算支架坐标系：
步骤2.1：在水箱内部放置标定好的双目相机，此时水箱内未装水，双目相机光轴垂直指向水箱底部，在双目相机的相机坐标系原点正上方空间中布置一个标定物作为支架坐标系的原点，两个相互垂直的连杆在该点处相交，在这两个相互垂直的连杆上分别再放置一个标定物；通过三维定位系统测得三个标定物在世界坐标系下的坐标值a(x1,y1,z1)，b(x2,y2,z2)，c(x3,y3,z3)，b点为支架坐标系原点；以ba方向为支架坐标系x方向，bc为支架坐标系y方向，根据右手法则确定z方向；
本实施例中
a(x1,y1,z1)＝(513.62；367.5；1121.92)
b(x2,y2,z2)＝(510.09；321.31；1121.97)
c(x3,y3,z3)＝(407.41；325.47；1131.31)
步骤2.2：根据公式
计算支架坐标系x轴、y轴、z轴的方向向量，其中
本实施例中
步骤3：通过以下过程计算目标坐标系：
步骤3.1：在水箱内部放置一个目标物体，此时水箱内未装水，在目标上放置一个标定物e作为目标坐标系的原点ot'，并且放置额外两个标定物d,f，且de垂直于ef；通过三维定位系统测得目标坐标系标定物在世界坐标系下的坐标值d(x4,y4,z4)，e(x5,y5,z5)，f(x6,y6,z6)，以ed方向为xt方向，ef为yt方向，根据右手法则确定zt方向；
本实施例中
d(x4,y4,z4)＝(508.59；280.61；100.59)
e(x5,y5,z5)＝(427.36；285.55；102.25)
f(x6,y6,z6)＝(433.54；325.43；101.91)
步骤3.2：根据公式
计算目标坐标系xt轴、yt轴、zt轴的方向向量，其中
本实施例中
步骤4：根据公式：
计算全局坐标系下，支架坐标系相对于目标坐标系的旋转矩阵
本实施例中
步骤5：通过以下过程计算相机坐标系下的目标坐标系：
步骤5.1：将标定好的双目相机放入密封舱内，在水箱中装水，在水下环境中通过双目相机对目标物体的标定物d,e,f坐标进行测量，得到标定物d,e,f在相机坐标系下的坐标值d'(x'4,y'4,z'4)，e'(x'5,y'5,z'5)，f'(x'6,y'6,z'6)；
本实施例中
d'(x'4,y'4,z'4)＝(25.6885；93.5684；790.3663)
e'(x'5,y'5,z'5)＝(5.4326；94.8149；790.8728)
f'(x'6,y'6,z'6)＝(26.8997；100.3245；791.2354)
步骤5.2：根据公式
计算相机坐标系下的目标坐标系三轴的方向向量
本实施例中
步骤6：根据公式
计算相机坐标系下，相机坐标系相对于目标坐标系的旋转矩阵
本实施例中
步骤7：根据公式
计算当前实验下目标物体姿态前后变化的旋转矩阵r；
本实施例中
步骤8：根据步骤7得到的旋转矩阵r解算出当前实验下的姿态误差值：俯仰角r1，横滚角r2，偏航角r3。本实施例中俯仰角r1＝-2.2665，横滚角r2＝-0.2067，偏航角r3＝1.6419。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
1.一种用于水下双目视觉的目标姿态误差测量方法，包括以下步骤：
步骤1：在水箱上方的空气中布置四个标定物，其中三个标定物固定在一根连杆上，两个标定物布置在另一根连杆上，两根连杆垂直相交于其中一个标定物；所述的标定物能够被三维定位系统拍摄，并能够被清晰识别；以两根连杆垂直相交的标定物中心点为世界坐标系的原点oe，以一根连杆作为世界坐标系ye轴，以另一根连杆作为世界坐标系xe轴；
步骤2：通过以下过程计算支架坐标系：
步骤2.1：在水箱内部放置标定好的双目相机，此时水箱内未装水，双目相机光轴垂直指向水箱底部，在双目相机的相机坐标系原点正上方空间中布置一个标定物作为支架坐标系的原点，两个相互垂直的连杆在该点处相交，在这两个相互垂直的连杆上分别再放置一个标定物；通过三维定位系统测得三个标定物在世界坐标系下的坐标值a(x1,y1,z1)，b(x2,y2,z2)，c(x3,y3,z3)，b点为支架坐标系原点；以ba方向为支架坐标系x方向，bc为支架坐标系y方向，根据右手法则确定z方向；
步骤2.2：根据公式
计算支架坐标系x轴、y轴、z轴的方向向量，其中
其特征在于：还包括以下步骤：
步骤3：通过以下过程计算目标坐标系：
步骤3.1：在水箱内部放置一个目标物体，此时水箱内未装水，在目标上放置一个标定物e作为目标坐标系的原点ot'，并且放置额外两个标定物d,f，且de垂直于ef；通过三维定位系统测得目标坐标系标定物在世界坐标系下的坐标值d(x4,y4,z4)，e(x5,y5,z5)，f(x6,y6,z6)，以ed方向为xt方向，ef为yt方向，根据右手法则确定zt方向；
步骤3.2：根据公式
计算目标坐标系xt轴、yt轴、zt轴的方向向量，其中
步骤4：根据公式：
计算全局坐标系下，支架坐标系相对于目标坐标系的旋转矩阵
步骤5：通过以下过程计算相机坐标系下的目标坐标系：
步骤5.1：将标定好的双目相机放入密封舱内，在水箱中装水，在水下环境中通过双目相机对目标物体的标定物d,e,f坐标进行测量，得到标定物d,e,f在相机坐标系下的坐标值d'(x′4,y′4,z′4)，e'(x′5,y′5,z′5)，f'(x′6,y′6,z′6)；
步骤5.2：根据公式
计算相机坐标系下的目标坐标系三轴的方向向量
步骤6：根据公式
计算相机坐标系下，相机坐标系相对于目标坐标系的旋转矩阵
步骤7：根据公式
计算当前实验下目标物体姿态前后变化的旋转矩阵r，其中
步骤8：根据步骤7得到的旋转矩阵r解算出当前实验下的姿态误差值：俯仰角r1，横滚角r2，偏航角r3。
2.根据权利要求1所述一种用于水下双目视觉的目标姿态误差测量方法，其特征在于：步骤8中调用matlab函数dcm2angle()对旋转矩阵r进行解算，得到当前实验下的姿态误差值。
技术总结