[发明专利]一种用于水下双目视觉的目标姿态误差测量方法

权利要求书

1.一种用于水下双目视觉的目标姿态误差测量方法，包括以下步骤：

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

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

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

步骤2.2：根据公式

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

其特征在于：还包括以下步骤：

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

步骤3.1：在水箱内部放置一个目标物体，此时水箱内未装水，在目标上放置一个标定物E作为目标坐标系的原点O_t'，并且放置额外两个标定物D,F，且DE垂直于EF；通过三维定位系统测得目标坐标系标定物在世界坐标系下的坐标值D(X₄,Y₄,Z₄)，E(X₅,Y₅,Z₅)，F(X₆,Y₆,Z₆)，以ED方向为X_t方向，EF为Y_t方向，根据右手法则确定Z_t方向；

步骤3.2：根据公式

计算目标坐标系X_t轴、Y_t轴、Z_t轴的方向向量，其中表示X_t轴方向向量，表示Y_t轴方向向量，表示Z_t轴方向向量；其中

步骤4：根据公式：

计算全局坐标系下，支架坐标系相对于目标坐标系的旋转矩阵全局坐标系下，支架坐标系与相机坐标系的姿态相同，也是在全局坐标系下，相机坐标系相对于目标坐标系的旋转矩阵；

步骤5：通过以下过程计算相机坐标系下的目标坐标系：

步骤5.1：将标定好的双目相机放入密封舱内，在水箱中装水，在水下环境中通过双目相机对目标物体的标定物D,E,F坐标进行测量，得到标定物D,E,F在相机坐标系下的坐标值D'(X′₄,Y′₄,Z′₄)，E'(X′₅,Y′₅,Z′₅)，F'(X′₆,Y′₆,Z′₆)；

步骤5.2：根据公式

计算相机坐标系下的目标坐标系三轴的方向向量

步骤6：根据公式

计算相机坐标系下，相机坐标系相对于目标坐标系的旋转矩阵

步骤7：根据公式

计算当前实验下目标物体姿态前后变化的旋转矩阵R，其中

步骤8：根据步骤7得到的旋转矩阵R解算出当前实验下的姿态误差值：俯仰角r₁，横滚角r₂，偏航角r₃。

2.根据权利要求1所述一种用于水下双目视觉的目标姿态误差测量方法，其特征在于：步骤8中调用matlab函数dcm2angle()对旋转矩阵R进行解算，得到当前实验下的姿态误差值。