CN116026276A - 港用门座起重机外置式转台转角测量方法及装置 - Google Patents

Abstract

本发明公开了一种港用门座起重机外置式转台转角测量方法及装置，其方法包括：步骤1，在港用门座起重机的转台设置相机、IMU和磁力计、以及在转台周向隔开设置若干视觉标签；步骤2，根据转台处于静止状态的磁力计测量数据、IMU测量数据和视觉标签的图像数据，获取视觉标签在地理坐标系的姿态；步骤3，根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；步骤4，根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角。本发明能够对港用门座起重机的转台转角进行实时测量。

Description

港用门座起重机外置式转台转角测量方法及装置

技术领域

本发明涉及港用门座起重机技术领域，特别是关于一种港用门座起重机外置式转台转角测量方法及装置。

背景技术

港用门座起重机是港口散货起重装卸作业的重要机械，随着港口智能化的发展，对港用作业机械的状态监测十分重要。对于门座式起重机，由于多机同时作业，会因视野盲区、人为操作等众多因素造成多机臂架碰撞，从而引起严重的人身安全问题和经济损失。因此测量门座式起重机的位置、转台转角、臂杆倾角等数据，对于安全作业、多机调度是十分重要的。

对于绕固定轴旋转角度的测量，常在设备内部安装旋转角测量装置，其通过转轴和传动机构带动的编码器进行角度计算。但对于门座式起重机这样的装卸作业大型机械，完成多机调度作业需要多台港用门座起重机有一致的坐标系，且由于机械内部的振动会导致传感器安装位置偏移和传动装置受损，会影响旋转编码器的测量精度和稳定性。因此需要用一种便捷布置且稳定的外置式测量装置，对港用门座起重机这类大型机械的回转角度进行实时测量。

发明内容

本发明的目的在于提供一种港用门座起重机外置式转台转角测量方法及装置来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种港用门座起重机外置式转台转角测量方法，其包括：

步骤1，在港用门座起重机的转台设置相机、IMU（Inertial Measurement Unit，惯性测量单元）和磁力计、以及在转台周向隔开设置若干视觉标签；

步骤2，在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系的姿态，根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿，利用下式获得视觉标签在地理坐标系的姿态；

式中，为IMU到地理坐标系的姿态变换矩阵，为IMU坐标系到相机坐标系的姿态变换矩阵的转置矩阵，为中的视觉标签到相机坐标系的姿态变换矩阵；

步骤3，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；

步骤4，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角。

进一步地，视觉标签的外轮廓具有四个角点，步骤2中的“根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿”的方法具体包括：

步骤21，根据视觉标签的角点在视觉标签坐标系中的三维空间坐标与相机坐标系中的二维像素坐标，利用，通过视觉标签的四个角点构建8个方程，以求解单应矩阵；

步骤22，根据二维像素坐标与三维空间坐标的转换关系，通过获取视觉标签相对于相机的位姿，从而获得相机的绝对位姿；其中，为相机的尺度，为相机的内参矩阵；

步骤23，通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据步骤21获取的IMU在静止状态下在东北天坐标系的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系的绝对姿态；

步骤24，根据步骤23得到的视觉标签在地理坐标系的绝对姿态，利用下式获取视觉标签在相机坐标系的位姿：

式中，为视觉标签到相机坐标系的平移向量。

进一步地，步骤3具体包括：

第一，相机未检测到视觉标签的情形，具体包括：

步骤31a，利用下式获取转台到地理坐标系的姿态变换矩阵：

式中，为IMU坐标系到转台旋转中心的坐标系的姿态变换矩阵的转置矩阵；

步骤32a，根据姿态变换矩阵，获取转台转角；

第二，相机检测到视觉标签的情形，具体包括：

步骤31b，利用下式获取转台的姿态：

式中，是相机坐标系到视觉标签的姿态变换矩阵；

步骤32b，根据转台的姿态，获取转台转角。

进一步地，步骤4具体包括：

第一，相机未检测到视觉标签的情形，具体包括：

步骤41a，利用下式获取时刻转台的姿态为：

式中，为时刻IMU的位姿矩阵对应的四元数，为时刻到时刻的时间差，为IMU的陀螺仪所测量的角速度的插值为零偏；

步骤42a，根据时刻转台的姿态，递推计算下一相邻时刻的转台转角；

第二，相机检测到视觉标签的情形，具体包括：

步骤41b，根据相机在时刻、检测到的视觉标签，计算时刻、相机的姿态、；

步骤42b，利用下式计算、时刻IMU坐标系到相机坐标系的姿态变换矩阵、，进而获得IMU姿态的增量：

式中，、分别为、时刻相机坐标系到地理坐标系的姿态变换矩阵；

步骤43b，将增量转化为四元数为；

步骤44b，将-时刻内IMU的、时刻陀螺仪数据、进行积分，得到姿态四元数的增量；

步骤45b，对四元数、通过卡尔曼滤波得到姿态增量,利用下式计算时刻下融合后的IMU姿态：

；

步骤46b，利用下式计算融合后的转台的姿态，由可以得到转台转角；

。

本发明还提供一种港用门座起重机外置式转台转角测量装置，其包括：

若干视觉标签，其周向隔开设置在港用门座起重机的转台；

磁力计、IMU和相机，其用于分别采集转台的磁力计测量数据、IMU测量数据和视觉标签的图像数据；

视觉标签获取单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系的姿态，根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿，利用下式（14）获得视觉标签在地理坐标系的姿态；

（14）

式中，为IMU到地理坐标系的姿态变换矩阵，为IMU坐标系到相机坐标系的姿态矩阵的转置矩阵，为中的视觉标签到相机坐标系姿态变换矩阵；

第一转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；

第二转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角。

进一步地，视觉标签获取单元具体包括：

IMU姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系的姿态；

视觉标签相对姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据视觉标签的图像数据，获取视觉标签在相机坐标系的位姿；

视觉标签绝对姿态获取子单元，其用于根据姿态和姿态，获得视觉标签在地理坐标系的姿态。

进一步地，视觉标签的外轮廓具有四个角点，视觉标签相对姿态获取子单元具体包括：

相机绝对姿态获取模块，其用于根据视觉标签的角点在视觉标签坐标系中的三维空间坐标与相机坐标系中的二维像素坐标，利用、以及二维像素坐标与三维空间坐标的转换关系，获得相机的绝对位姿；其中，为相机的尺度，为相机的内参矩阵；

视觉标签绝对姿态获取模块，其用于通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据IMU在静止状态下在东北天坐标系的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系的绝对姿态；

视觉标签相对姿态获取模块，其用于根据视觉标签在地理坐标系的绝对姿态，利用下式获取视觉标签在相机坐标系的位姿：

式中，为视觉标签到相机坐标系的平移向量。

进一步地，第一转台转角获取单元具体包括：

第一转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：

步骤31a，利用下式获取转台到地理坐标系的姿态变换矩阵：

式中，为IMU坐标系到转台旋转中心的坐标系的姿态变换矩阵的转置矩阵；

步骤32a，根据姿态变换矩阵，获取转台转角；

第二转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：

步骤31b，利用下式获取转台的姿态：

式中，是相机坐标系到视觉标签的姿态变换矩阵；

步骤32b，根据转台的姿态，获取转台转角。

进一步地，第二转台转角获取单元具体包括：

第三转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：

步骤41a，利用下式获取时刻转台的姿态为：

式中，为时刻IMU的位姿矩阵对应的四元数，为时刻到时刻的时间差，为IMU的陀螺仪所测量的角速度的插值为零偏；

步骤42a，根据时刻转台的姿态，递推计算下一相邻时刻的转台转角；

第四转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：

步骤41b，根据相机在时刻、检测到的视觉标签，计算时刻、相机的姿态、；

步骤42b，利用下式计算、时刻IMU坐标系到相机坐标系的姿态变换矩阵、，进而获得IMU姿态的增量：

式中，、分别为、时刻相机坐标系到地理坐标系的姿态变换矩阵；

步骤43b，将增量转化为四元数为；

步骤44b，将-时刻内IMU的、时刻陀螺仪数据、进行积分，得到姿态四元数的增量；

步骤45b，对四元数、通过卡尔曼滤波得到姿态增量,利用下式计算时刻下融合后的IMU姿态：

；

步骤46b，利用下式计算融合后的转台的姿态，由可以得到转台转角；

。

由于本发明融合磁力计测量数据、IMU测量数据、视觉标签的图像数据，对港用门座起重机转角进行实时测量，从而实现了在转台静止和转动的两种工况下高频输出当前角度，进而能够针对差异场景针对性设计方案，实现在不同工况的转角姿态准确而稳定地测量。

附图说明

图1为本发明中适用的港用门座起重机的结构示意图。

图2为本发明实施例中视觉标签的布置方式示意图。

图3为本发明实施例中各坐标系的分布示意图。

1、港用门座起重机的承载部分；2、港用门座起重机的转动部分；3、立柱；4、相机；5、视觉标签；11、承载平台；21、转台；A、相机采集到的图像存在视觉标签的情形；B、相机采集到的图像存在视觉标签的情形。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1和图2所示，港用门座起重机包括承载部分1和与转动部分2，承载部分1包括承载平台11，转动部分2包括转台21。由于本发明将测量系统外置于转台和门架上，因此不用对机械内部进行改装即可得到实时的转台转角数据。

本发明实施例提供的港用门座起重机外置式转台转角测量方法包括：

步骤1，在港用门座起重机的转台21的底部设置相机4、IMU和磁力计，在转台21的承载平台11的顶面临近外圆周的位置，以立柱3为中心周向间隔布置若干视觉标签5。视觉标签5的布置方式例如但不限于图2所示，图2中示意的视觉标签5的数量是8个，并沿周向均匀隔开45°布置。单个相机的视角能够覆盖单个视觉标签5，因此，在转台转动的过程中，带动相机转动，那么，相机采集到的图像会分两种，一种是存在视觉标签5，如图2中的A所示，另一种是不存在视觉标签5，如图2中的B所示。

鉴于此，实施例中的各个坐标系如图3所示，转台旋转中心的坐标系表示为，相机坐标系表示为，视觉标签坐标系表示为，IMU和磁力计常耦合在一起，因此其具有共同的坐标系，表示为，地理坐标系采用东北天坐标系，表示为。

步骤2，在一个视觉标签5呈现在相机4的视野中心位置时，根据转台21处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系的姿态根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿，利用下式（14）获得视觉标签5在地理坐标系的姿态。

例如在下面实施例中，通过计算磁力计和IMU在转台静止状态的姿态，以及利用安装参数标记视觉标签5在地理坐标系的姿态，并对每一个视觉标签5重复上述操作，获取视觉标签5在地理坐标系的姿态。

在一个实施例中，步骤2中的“根据转台21处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系的姿态”具体包括：

在一个视觉标签5呈现在相机4的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系的姿态。由于本发明通过磁力计与IMU测量数据组合推算，得到转台在地理坐标系的姿态数据，因此便于多机共同作业时坐标系的统一。

下面是IMU测量数据的获取方法。

IMU实质上包括加速度计与陀螺仪，因此，IMU测量数据包括加速度计的测量数据和陀螺仪的测量数据，而本步骤仅采用加速度计的测量数据，当然也可以根据实际情况变化单独或叠加使用陀螺仪的测量数据。

IMU在静止理想状态的和两个轴所组成的平面与水平面平行，因此布置在转台的IMU中的加速度计的输出为加速度计的测量数据，为重力加速度，为转置。

但在实际中，港用门座起重机的布置位置与水平面有一定的倾角，因此导致加速度计的测量数据，为IMU坐标系下轴方向的加速度值，为IMU坐标系下轴方向的加速度值，为IMU坐标系下轴方向的加速度值。

通过地理坐标系到当前的IMU坐标系的姿态变换矩阵，地理坐标系的重力加速度可以转换到当前的IMU坐标系下，IMU的横滚角为，俯仰角为，偏航角为，由此可以得到下式（1）-式（3）：

（1）

（2）

 （3）

在一个实施例中，由于IMU偏航角不可观，由上式（1）-式（3）可得：当前IMU的俯仰角，当前IMU的横滚角。

下面是磁力计测量数据的获取方法。

磁力计的测量数据包括磁力计在坐标系的、、三个轴的磁分量、、，。

本实施例将磁力计的测量数据通过IMU的横滚角为和俯仰角转到水平方向，得到转换到坐标系下后的磁力计数据，其具体表示为下式（4）。

（4）

式中，、、分别为当磁力计数据转到水平方向后在坐标系的、、三个轴的磁分量，为姿态变换矩阵的逆矩阵。

在IMU坐标系的轴的磁分量数据，轴的磁分量数据为，则推算得到转台与地磁磁北的夹角。

上述磁力计解算的航向角是轴相对于磁北而言的，而地理北方与磁北之间存在一个磁偏角，若某地的经纬度和海拔已知，则此磁偏角固定，可通过国际地磁参考场计算得到，因此IMU坐标轴相对于地理北方的偏航角。

在一个实施例中，步骤2中的“根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿”的方法可以借助视觉标签5的外轮廓所具有的四个角点实现。例如视觉标签5采用AprilTag码时，通过采用相机检测到的AprilTag码，来恢复相机的位姿。当检测到视觉标签5提供的AprilTag码的四个角点在相机成像平面所在的像素坐标系中的像素坐标（下文简称为：“像素坐标”）后，角点像素坐标到视觉标签坐标系的转换视为二维坐标变换，通过匹配求解单应矩阵，可以恢复标签与相机之间的相对位姿。根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿的方法具体包括：

步骤21，根据视觉标签5的角点在视觉标签坐标系中的三维空间坐标与相机坐标系中的二维像素坐标，利用，通过视觉标签5的四个角点构建8个方程，以求解获得单应矩阵，为3×3的单应矩阵。

根据和，视觉标签坐标系所在二维平面的四个角点在相机坐标系中的坐标分别为、、、，其中，为视觉标签5的实际边长。

步骤22，根据视觉标签5的二维像素坐标与三维空间坐标的转换关系，有下式（5），获取视觉标签5相对于相机的位姿：

 （5）

式中，为相机的尺度，为下式（6）提供的单应矩阵，为单应矩阵的9个参数，为下式（7）提供的相机的内参矩阵，在相机图像坐标系中，为方向的缩放比例，为方向的缩放比例，为光心在方向的像素坐标，为光心在方向的像素坐标；为下式（8）提供的视觉标签5相对于相机坐标系的位姿转换矩阵，、分别为中的视觉标签到相机坐标系的平移向量和姿态变换矩阵。

（6）

（7）

（8）

在一个实施例中，由于式（5）隐含了一次矩阵的维度变换，三维空间坐标的，因此本实施例可以略去位姿变换矩阵的第三列，并显式表示矩阵维度变换，则有下式（9）：

（9）

式中，略去第三列的变换矩阵中，有的六个参数、、、、和，还有的三个参数、和，从而形成下式（10）：

（10）

在一个实施例中，在包含相机尺度的情况下，可求解出平移向量和姿态变换矩阵，姿态变换矩阵为严格的正定矩阵，尺度可以近似为下式（11）描述的姿态变换矩阵前两列模长的均值：

（11）

式中，为的矩阵转置，为的矩阵转置，为向量的模长。

姿态变换矩阵由、、、、和组成，第三列可由下式（12）描述的前两列叉乘得到：

 （12）

根据上式（10）求出和后，可得到位姿转换矩阵，其描述为下式（13）：

 （13）

若已知视觉标签5的绝对位姿，则相机的绝对位姿表示为。

步骤23，通过转台21带动相机4，使视觉标签5处于相机4可观测视野的中心位置，根据步骤21获取的IMU在静止状态下在东北天坐标系的位姿、磁力计数据和相机的图像数据，利用下式（14）获取视觉标签5在地理坐标系的绝对姿态。

（14）

式中，为IMU到地理坐标系的姿态变换矩阵，、、为IMU的偏航角、俯仰角、横滚角，为IMU坐标系到相机坐标系的姿态变换矩阵的转置矩阵，为视觉标签到相机的姿态变换矩阵，其可以根据上式（13）获得。

（15）

步骤23是获取单个视觉标签5在地理坐标系的姿态，因此，重复步骤23，从而可以获得8个视觉标签5在地理坐标系的姿态。

需要说明的是，上述实施例的视觉标签5采用AprilTag码，此视觉标签5可以被Aruco码等多种类型的视觉标签5所替代。

由于本发明采用磁力计、IMU标记视觉标签5的姿态，因此去除了人工繁杂的测量，提高了视觉标签5姿态的准确性。

步骤3，根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机是否能够检测到视觉标签5的情形，获取转台静止状态的转台转角。

第一，相机未检测到视觉标签5的情形，具体包括：

步骤31a，利用下式（16）获取转台到地理坐标系的姿态变换矩阵：

（16）

式中，为IMU到地理坐标系的姿态变换矩阵，为IMU坐标系到转台旋转中心的坐标系的变换矩阵中的姿态变换的转置矩阵，可以通过安装参数获得。

步骤32a，根据转台到地理坐标系的姿态变换矩阵，转台绕立柱的中心轴旋转的欧拉角，即转台转角。

第二，相机检测到视觉标签5的情形，具体包括：

步骤31b，利用下式（17）获取转台的姿态：

（17）

式中，为步骤23获得的视觉标签5的绝对姿态，是相机坐标系到视觉标签5的姿态变换矩阵，通过上式（13）的的逆矩阵获得，为IMU坐标系到相机坐标系的姿态变换矩阵，相当于已知量，为IMU坐标系到转台旋转中心的坐标系的变换矩阵中的姿态变换的转置矩阵。

步骤32b，根据转台的姿态，获取转台绕立柱的中心轴旋转的欧拉角，即转台转角。

步骤4，根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机是否能够检测到视觉标签5的情形，获取转台转动状态的转台转角。

第一，相机未检测到视觉标签5的情形，具体包括：

步骤41a，利用下式（18）获取时刻转台的姿态为：

（18）

式中，为时刻IMU的姿态矩阵对应的四元数，为下式（19）提供的时刻IMU的姿态矩阵对应的四元数，为时刻到时刻的时间差，在时刻转台的姿态为，对-时刻内IMU的陀螺仪数据进行中值积分，为下式（20）提供的IMU的陀螺仪所测量的角速度的插值为零偏，为时刻IMU陀螺仪的角速度测量值，为时刻IMU陀螺仪的角速度测量值。

（19）

（20）

步骤42a，根据时刻转台的姿态为，提供递推计算下一相邻时刻的转台转角。

第二，相机检测到视觉标签5的情形，在转动状态下时，若相机连续检测到视觉标签5，则基于卡尔曼滤波融合IMU测量数据和视觉标签5的图像数据，对转台转角进行测量，具体包括：

步骤41b，根据相机4在前一时刻检测到视觉标签，后一时刻检测到视觉标签，则由公式（13）、公式（14）可得时刻相机的姿态，时刻相机的姿态。

步骤42b，经过下式（21）和（22）提供的相机与IMU间的外部参数推算公式，得到时刻的IMU姿态为和时刻的IMU姿态，进而获得IMU姿态的增量为：

（21）

（22）

式中，为时刻相机坐标系到地理坐标系的姿态变换矩阵，为时刻IMU坐标系到相机坐标系的姿态变换矩阵，为时刻相机坐标系到地理坐标系的姿态变换矩阵，为时刻IMU坐标系到相机坐标系的姿态变换矩阵。

步骤43b，将姿态的增量为，转化为四元数为。

步骤44b，将-时刻内IMU的陀螺仪数据、进行积分，得到姿态四元数的增量表示为下式（23）：

（23）

步骤45b，对四元数、通过卡尔曼滤波得到姿态增量,利用下式（24）将姿态增量累加到上，得到时刻下融合后的IMU姿态：

      （24）

步骤46b，通过IMU与转台间的外部参数变换矩阵，利用下式（25）得到融合后的转台的姿态：

（25）

步骤47b，由可以得到转台绕Z轴旋转的欧拉角，即转台转角。

本发明实施例还提供一种港用门座起重机外置式转台转角测量装置，其包括若干视觉标签、磁力计、IMU、相机、视觉标签获取单元、第一转台转角获取单元和第二转台转角获取单元，其中：

若干视觉标签周向隔开设置在港用门座起重机的转台。

磁力计、IMU和相机用于分别采集转台的磁力计测量数据、IMU测量数据和视觉标签的图像数据。

视觉标签姿态获取单元用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据、IMU测量数据和视觉标签的图像数据，获取视觉标签在地理坐标系的姿态。

第一转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角。

第二转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角。

在一个实施例中，视觉标签获取单元具体包括IMU姿态获取子单元、视觉标签相对姿态获取子单元和视觉标签绝对姿态获取子单元，其中：

IMU姿态获取子单元用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系的姿态。

视觉标签相对姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据视觉标签的图像数据，获取视觉标签在相机坐标系的姿态。

视觉标签绝对姿态获取子单元，其用于根据姿态和姿态，利用式（14）获得视觉标签在地理坐标系的姿态。

在一个实施例中，视觉标签的外轮廓具有四个角点，视觉标签相对姿态获取子单元具体包括相机绝对姿态获取模块、视觉标签绝对姿态获取模块和视觉标签相对姿态获取模块，其中：

相机绝对姿态获取模块用于根据视觉标签的角点在视觉标签坐标系中的三维空间坐标与相机坐标系中的二维像素坐标，利用、以及与的转换关系，获得相机的绝对位姿表示为；其中，为相机的尺度。

视觉标签绝对位姿获取模块用于通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据IMU在静止状态下在东北天坐标系的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系的绝对姿态。

视觉标签相对姿态获取模块用于根据视觉标签在地理坐标系的绝对姿态，利用式（13）获取视觉标签在相机坐标系的姿态。

第一转台转角获取单元具体包括第一转台转角计算模块和第二转台转角计算模块，其中：

第一转台转角计算模块用于在相机未检测到视觉标签的情形下计算转台转角，具体包括步骤31a和步骤32a。

第二转台转角计算模块用于在相机检测到视觉标签的情形下计算转台转角，具体步骤31b和步骤32b。

第二转台转角获取单元具体包括第三转台转角计算模块和第四转台转角计算模块，其中：

第三转台转角计算模块用于在相机未检测到视觉标签的情形下计算转台转角，具体包括步骤41a和步骤42a。

第四转台转角计算模块用于在相机检测到视觉标签的情形下计算转台转角，具体包括步骤41b-步骤46b。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (5)

1.一种港用门座起重机外置式转台转角测量方法，其特征在于，包括：

步骤1，在港用门座起重机的转台设置相机、IMU和磁力计、以及在转台周向隔开设置若干视觉标签；

步骤2，在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系的姿态，根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿，利用下式获得视觉标签在地理坐标系的姿态；

式中，为IMU到地理坐标系的姿态变换矩阵，为IMU坐标系到相机坐标系的姿态变换矩阵的转置矩阵，为中的视觉标签到相机坐标系的姿态变换矩阵；

步骤3，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；

步骤3具体包括：

第一，相机未检测到视觉标签的情形，具体包括：

步骤31a，利用下式获取转台到地理坐标系的姿态变换矩阵：

式中，为IMU坐标系到转台旋转中心的坐标系的姿态变换矩阵的转置矩阵；

步骤32a，根据姿态变换矩阵，获取转台转角；

第二，相机检测到视觉标签的情形，具体包括：

步骤31b，利用下式获取转台的姿态：

式中，是相机坐标系到视觉标签的姿态变换矩阵；

步骤32b，根据转台的姿态，获取转台转角；

步骤4，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角；

步骤4具体包括：

第一，相机未检测到视觉标签的情形，具体包括：

步骤41a，利用下式获取时刻转台的姿态为：

式中，为时刻IMU的位姿矩阵对应的四元数，为时刻到时刻的时间差，为IMU的陀螺仪所测量的角速度的插值为零偏；

步骤42a，根据时刻转台的姿态，递推计算下一相邻时刻的转台转角；

第二，相机检测到视觉标签的情形，具体包括：

步骤41b，根据相机在时刻、检测到的视觉标签，计算时刻、的相机的姿态、；

步骤42b，利用下式计算、时刻IMU坐标系到相机坐标系的姿态变换矩阵、，进而获得IMU姿态的增量：

式中，、分别为、时刻相机坐标系到地理坐标系的姿态变换矩阵；

步骤43b，将增量转化为四元数为；

步骤44b，将-时刻内IMU的、时刻陀螺仪数据、进行积分，得到姿态四元数的增量；

步骤45b，对四元数、通过卡尔曼滤波得到姿态增量,利用下式计算时刻下融合后的IMU姿态：

；

步骤46b，利用下式计算融合后的转台的姿态，由可以得到转台转角；

。

2.如权利要求1所述的港用门座起重机外置式转台转角测量方法，其特征在于，视觉标签的外轮廓具有四个角点，步骤2中的“根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿”的方法具体包括：

步骤21，根据视觉标签的角点在视觉标签坐标系中的三维空间坐标与相机坐标系中的二维像素坐标，利用，通过视觉标签的四个角点构建8个方程，以求解单应矩阵；

步骤22，根据二维像素坐标与三维空间坐标的转换关系，通过获取视觉标签相对于相机的位姿，从而获得相机的绝对位姿；其中，为相机的尺度，为相机的内参矩阵；

步骤23，通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据步骤21获取的IMU在静止状态下在东北天坐标系的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系的绝对姿态；

步骤24，根据绝对姿态，利用下式获取位姿：

式中，为视觉标签到相机坐标系的平移向量。

3.一种港用门座起重机外置式转台转角测量装置，其特征在于，包括：

若干视觉标签，其周向隔开设置在港用门座起重机的转台；

磁力计、IMU和相机，其用于分别采集转台的磁力计测量数据、IMU测量数据和视觉标签的图像数据；

视觉标签获取单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系的姿态，根据视觉标签的图像数据获取视觉标签在相机坐标系的位姿，利用下式获得视觉标签在地理坐标系的姿态；

式中，为IMU到地理坐标系的姿态变换矩阵，为IMU坐标系到相机坐标系的姿态矩阵的转置矩阵，为中的视觉标签到相机坐标系的姿态变换矩阵；

第一转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；

第二转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角；

第一转台转角获取单元具体包括：

第一转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：

步骤31a，利用下式获取转台到地理坐标系的姿态变换矩阵：

式中，为IMU坐标系到转台旋转中心的坐标系的姿态变换矩阵的转置矩阵；

步骤32a，根据姿态变换矩阵，获取转台转角；

第二转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：

步骤31b，利用下式获取转台的姿态：

式中，是相机坐标系到视觉标签的姿态变换矩阵；

步骤32b，根据转台的姿态，获取转台转角；

第二转台转角获取单元具体包括：

第三转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：

步骤41a，利用下式获取时刻转台的姿态为：

式中，为时刻IMU的位姿矩阵对应的四元数，为时刻到时刻的时间差，为IMU的陀螺仪所测量的角速度的插值为零偏；

步骤42a，根据时刻转台的姿态，递推计算下一相邻时刻的转台转角；

第四转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：

步骤41b，根据相机在时刻、检测到的视觉标签，计算时刻、的相机的姿态、；

步骤42b，利用下式计算、时刻IMU坐标系到相机坐标系的姿态变换矩阵、，进而获得IMU姿态的增量：

式中，、分别为、时刻相机坐标系到地理坐标系的姿态变换矩阵；

步骤43b，将增量转化为四元数为；

步骤44b，将-时刻内IMU的、时刻陀螺仪数据、进行积分，得到姿态四元数的增量；

步骤45b，对四元数、通过卡尔曼滤波得到姿态增量,利用下式计算时刻下融合后的IMU姿态：

；

步骤46b，利用下式计算融合后的转台的姿态，由可以得到转台转角；

。

4.如权利要求3所述的港用门座起重机外置式转台转角测量装置，其特征在于，视觉标签获取单元具体包括：

IMU姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系的姿态；

视觉标签相对姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据视觉标签的图像数据，获取视觉标签在相机坐标系的位姿；

视觉标签绝对姿态获取子单元，其用于根据姿态和姿态，获得视觉标签在地理坐标系的姿态。

5.如权利要求4所述的港用门座起重机外置式转台转角测量装置，其特征在于，视觉标签的外轮廓具有四个角点，视觉标签相对姿态获取子单元具体包括：

相机绝对姿态获取模块，其用于根据视觉标签的角点在视觉标签坐标系中的三维空间坐标与相机坐标系中的二维像素坐标，利用、以及二维像素坐标与三维空间坐标的转换关系，获得相机的绝对位姿；其中，为相机的尺度，为相机的内参矩阵；

视觉标签绝对姿态获取模块，其用于通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据IMU在静止状态下在东北天坐标系的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系的绝对姿态；

视觉标签相对姿态获取模块，其用于根据绝对姿态，利用下式获取位姿：

式中，为视觉标签到相机坐标系的平移向量。

Images