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

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

Info

Publication number
CN116026276B
CN116026276B CN202310313749.0A CN202310313749A CN116026276B CN 116026276 B CN116026276 B CN 116026276B CN 202310313749 A CN202310313749 A CN 202310313749A CN 116026276 B CN116026276 B CN 116026276B
Authority
CN
China
Prior art keywords
turntable
camera
coordinate system
imu
visual
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310313749.0A
Other languages
English (en)
Other versions
CN116026276A (zh
Inventor
刘硕
周云水
秦晓辉
秦兆博
谢国涛
王晓伟
徐彪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Original Assignee
Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd filed Critical Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Priority to CN202310313749.0A priority Critical patent/CN116026276B/zh
Publication of CN116026276A publication Critical patent/CN116026276A/zh
Application granted granted Critical
Publication of CN116026276B publication Critical patent/CN116026276B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Image Analysis (AREA)

Abstract

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

Description

港用门座起重机外置式转台转角测量方法及装置
技术领域
本发明涉及港用门座起重机技术领域，特别是关于一种港用门座起重机外置式转台转角测量方法及装置。
背景技术
港用门座起重机是港口散货起重装卸作业的重要机械，随着港口智能化的发展，对港用作业机械的状态监测十分重要。对于门座式起重机，由于多机同时作业，会因视野盲区、人为操作等众多因素造成多机臂架碰撞，从而引起严重的人身安全问题和经济损失。因此测量门座式起重机的位置、转台转角、臂杆倾角等数据，对于安全作业、多机调度是十分重要的。
对于绕固定轴旋转角度的测量，常在设备内部安装旋转角测量装置，其通过转轴和传动机构带动的编码器进行角度计算。但对于门座式起重机这样的装卸作业大型机械，完成多机调度作业需要多台港用门座起重机有一致的坐标系，且由于机械内部的振动会导致传感器安装位置偏移和传动装置受损，会影响旋转编码器的测量精度和稳定性。因此需要用一种便捷布置且稳定的外置式测量装置，对港用门座起重机这类大型机械的回转角度进行实时测量。
发明内容
本发明的目的在于提供一种港用门座起重机外置式转台转角测量方法及装置来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种港用门座起重机外置式转台转角测量方法，其包括：
步骤1，在港用门座起重机的转台设置相机、IMU(Inertial Measurement Unit，惯性测量单元)和磁力计、以及在转台周向隔开设置若干视觉标签；
步骤2，在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系CENU的姿态
Figure GDA0004221421440000011
根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000012
利用下式获得视觉标签在地理坐标系CENU的姿态
Figure GDA0004221421440000013
Figure GDA0004221421440000021
式中，
Figure GDA0004221421440000022
为IMU到地理坐标系CENU的姿态变换矩阵，
Figure GDA0004221421440000023
为IMU坐标系CIMU到相机坐标系Ccam的姿态变换矩阵
Figure GDA0004221421440000024
的转置矩阵，
Figure GDA0004221421440000025
为
Figure GDA0004221421440000026
中的视觉标签到相机坐标系Ccam的姿态变换矩阵；
步骤3，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；
步骤4，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角。
进一步地，视觉标签的外轮廓具有四个角点，步骤2中的“根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000027
”的方法具体包括：
步骤21，根据视觉标签的角点在视觉标签坐标系CTag中的三维空间坐标P＝[X,Y,0]T与相机坐标系Ccam中的二维像素坐标p＝[x,y]T，利用[x,y,1]T＝H[X,Y,1]T，通过视觉标签的四个角点构建8个方程，以求解单应矩阵H；
步骤22，根据二维像素坐标p＝[x,y]T与三维空间坐标P＝[X,Y,0]T的转换关系
Figure GDA0004221421440000028
通过
Figure GDA0004221421440000029
获取视觉标签相对于相机的位姿
Figure GDA00042214214400000210
从而获得相机的绝对位姿
Figure GDA00042214214400000211
其中，s为相机的尺度，K为相机的内参矩阵；
步骤23，通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据步骤21获取的IMU在静止状态下在地理坐标系CENU的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系CENU的绝对姿态
Figure GDA00042214214400000212
步骤24，根据步骤23得到的视觉标签在地理坐标系CENU的绝对姿态
Figure GDA00042214214400000213
利用下式获取视觉标签在相机坐标系Ccam的位姿
Figure GDA00042214214400000214
Figure GDA00042214214400000215
式中，
Figure GDA00042214214400000216
为视觉标签到相机坐标系的平移向量。
进一步地，步骤3具体包括：
第一，相机未检测到视觉标签的情形，具体包括：
步骤31a，利用下式获取转台到地理坐标系CENU的姿态变换矩阵
Figure GDA00042214214400000217
Figure GDA00042214214400000218
式中，
Figure GDA0004221421440000031
为IMU坐标系CIMU到转台旋转中心的坐标系CB的姿态变换矩阵
Figure GDA0004221421440000032
的转置矩阵；
步骤32a，根据姿态变换矩阵
Figure GDA0004221421440000033
获取转台转角；
第二，相机检测到视觉标签的情形，具体包括：
步骤31b，利用下式获取转台的姿态
Figure GDA0004221421440000034
Figure GDA0004221421440000035
式中，
Figure GDA0004221421440000036
是相机坐标系到视觉标签的姿态变换矩阵；
步骤32b，根据转台的姿态
Figure GDA0004221421440000037
获取转台转角。
进一步地，步骤4具体包括：
第一，相机未检测到视觉标签的情形，具体包括：
步骤41a，利用下式获取tk+1时刻转台的姿态为
Figure GDA0004221421440000038
Figure GDA0004221421440000039
式中，
Figure GDA00042214214400000310
为tk时刻IMU的位姿矩阵对应的四元数，δt为tk时刻到tk+1时刻的时间差，ω为IMU的陀螺仪所测量的角速度的插值
Figure GDA00042214214400000311
为零偏；
步骤42a，根据tk+1时刻转台的姿态
Figure GDA00042214214400000312
递推计算下一相邻时刻的转台转角；
第二，相机检测到视觉标签的情形，具体包括：
步骤41b，根据相机在时刻tk、tk+1检测到的视觉标签，计算时刻tk、tk+1相机的姿态
Figure GDA00042214214400000313
步骤42b，利用下式计算tk、tk+1时刻IMU坐标系到相机坐标系的姿态变换矩阵
Figure GDA00042214214400000314
进而获得IMU姿态的增量
Figure GDA00042214214400000315
Figure GDA00042214214400000316
Figure GDA00042214214400000317
式中，
Figure GDA00042214214400000318
分别为tk、tk+1时刻相机坐标系到地理坐标系的姿态变换矩阵；
步骤43b，将增量ΔR转化为四元数为
Figure GDA00042214214400000319
步骤44b，将tk-tk+1时刻内IMU的tk、tk+1时刻陀螺仪数据
Figure GDA00042214214400000320
进行积分，得到姿态四元数的增量
Figure GDA00042214214400000321
步骤45b，对四元数
Figure GDA0004221421440000041
通过卡尔曼滤波得到姿态增量Δq,利用下式计算tk+1时刻下融合后的IMU姿态
Figure GDA0004221421440000042
Figure GDA0004221421440000043
步骤46b，利用下式计算融合后的转台的姿态
Figure GDA0004221421440000044
由
Figure GDA0004221421440000045
可以得到转台转角；
Figure GDA0004221421440000046
本发明还提供一种港用门座起重机外置式转台转角测量装置，其包括：
若干视觉标签，其周向隔开设置在港用门座起重机的转台；
磁力计、IMU和相机，其用于分别采集转台的磁力计测量数据、IMU测量数据和视觉标签的图像数据；
视觉标签获取单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系CENU的姿态
Figure GDA0004221421440000047
根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000048
利用下式(14)获得视觉标签在地理坐标系CENU的姿态
Figure GDA0004221421440000049
Figure GDA00042214214400000410
式中，
Figure GDA00042214214400000411
为IMU到地理坐标系CENU的姿态变换矩阵，
Figure GDA00042214214400000412
为IMU坐标系CIMU到相机坐标系Ccam的姿态矩阵
Figure GDA00042214214400000413
的转置矩阵，
Figure GDA00042214214400000414
为
Figure GDA00042214214400000415
中的视觉标签到相机坐标系Ccam的姿态变换矩阵；
第一转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；
第二转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角。
进一步地，视觉标签获取单元具体包括：
IMU姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系CENU的姿态
Figure GDA00042214214400000416
视觉标签相对姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据视觉标签的图像数据，获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000051
视觉标签绝对姿态获取子单元，其用于根据姿态
Figure GDA0004221421440000052
和姿态
Figure GDA0004221421440000053
获得视觉标签在地理坐标系CENU的姿态
Figure GDA0004221421440000054
进一步地，视觉标签的外轮廓具有四个角点，视觉标签相对姿态获取子单元具体包括：
相机绝对姿态获取模块，其用于根据视觉标签的角点在视觉标签坐标系CT中的三维空间坐标P＝[X,Y,0]T与相机坐标系Ccam中的二维像素坐标p＝[x,y]T，利用[x,y,1]T＝H[X,Y,1]T、以及二维像素坐标p＝[x,y]T与三维空间坐标P＝[X,Y,0]T的转换关系
Figure GDA0004221421440000055
获得相机的绝对位姿
Figure GDA0004221421440000056
其中，s为相机的尺度，K为相机的内参矩阵；
视觉标签绝对姿态获取模块，其用于通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据IMU在静止状态下在地理坐标系CENU的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系CENU的绝对姿态
Figure GDA0004221421440000057
视觉标签相对姿态获取模块，其用于根据视觉标签在地理坐标系CENU的绝对姿态
Figure GDA0004221421440000058
利用下式获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000059
Figure GDA00042214214400000510
式中，
Figure GDA00042214214400000511
为视觉标签到相机坐标系的平移向量。
进一步地，第一转台转角获取单元具体包括：
第一转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：
步骤31a，利用下式获取转台到地理坐标系CENU的姿态变换矩阵
Figure GDA00042214214400000512
Figure GDA00042214214400000513
式中，
Figure GDA00042214214400000514
为IMU坐标系CIMU到转台旋转中心的坐标系CB的姿态变换矩阵
Figure GDA00042214214400000515
的转置矩阵；
步骤32a，根据姿态变换矩阵
Figure GDA00042214214400000516
获取转台转角；
第二转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：
步骤31b，利用下式获取转台的姿态
Figure GDA0004221421440000061
Figure GDA0004221421440000062
式中，
Figure GDA0004221421440000063
是相机坐标系到视觉标签的姿态变换矩阵；
步骤32b，根据转台的姿态
Figure GDA0004221421440000064
获取转台转角。
进一步地，第二转台转角获取单元具体包括：
第三转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：
步骤41a，利用下式获取tk+1时刻转台的姿态为
Figure GDA0004221421440000065
Figure GDA0004221421440000066
式中，
Figure GDA0004221421440000067
为tk时刻IMU的位姿矩阵对应的四元数，δt为tk时刻到tk+1时刻的时间差，ω为IMU的陀螺仪所测量的角速度的插值
Figure GDA0004221421440000068
为零偏；
步骤42a，根据tk+1时刻转台的姿态
Figure GDA0004221421440000069
递推计算下一相邻时刻的转台转角；
第四转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：
步骤41b，根据相机在时刻tk、tk+1检测到的视觉标签，计算时刻tk、tk+1相机的姿态
Figure GDA00042214214400000610
步骤42b，利用下式计算tk、tk+1时刻IMU坐标系到相机坐标系的姿态变换矩阵
Figure GDA00042214214400000611
进而获得IMU姿态的增量
Figure GDA00042214214400000612
Figure GDA00042214214400000613
Figure GDA00042214214400000614
式中，
Figure GDA00042214214400000615
分别为tk、tk+1时刻相机坐标系到地理坐标系的姿态变换矩阵；
步骤43b，将增量ΔR转化为四元数为
Figure GDA00042214214400000616
步骤44b，将tk-tk+1时刻内IMU的tk、tk+1时刻陀螺仪数据
Figure GDA00042214214400000617
进行积分，得到姿态四元数的增量
Figure GDA00042214214400000618
步骤45b，对四元数
Figure GDA0004221421440000071
通过卡尔曼滤波得到姿态增量Δq,利用下式计算tk+1时刻下融合后的IMU姿态
Figure GDA0004221421440000072
Figure GDA0004221421440000073
步骤46b，利用下式计算融合后的转台的姿态
Figure GDA0004221421440000074
由
Figure GDA0004221421440000075
可以得到转台转角；
Figure GDA0004221421440000076
由于本发明融合磁力计测量数据、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所示，转台旋转中心的坐标系表示为CB，相机坐标系表示为Ccam，视觉标签坐标系表示为CTag，IMU和磁力计常耦合在一起，因此其具有共同的坐标系，表示为CIMU，地理坐标系采用东北天坐标系，表示为CENU。
步骤2，在一个视觉标签5呈现在相机4的视野中心位置时，根据转台21处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系CENU的姿态
Figure GDA0004221421440000081
根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000082
利用下式(14)获得视觉标签5在地理坐标系CENU的姿态
Figure GDA0004221421440000083
例如在下面实施例中，通过计算磁力计和IMU在转台静止状态的姿态，以及利用安装参数标记视觉标签5在地理坐标系CENU的姿态，并对每一个视觉标签5重复上述操作，获取视觉标签5在地理坐标系CENU的姿态
Figure GDA0004221421440000084
在一个实施例中，步骤2中的“根据转台21处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系CENU的姿态
Figure GDA0004221421440000085
”具体包括：
在一个视觉标签5呈现在相机4的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系CENU的姿态
Figure GDA0004221421440000086
由于本发明通过磁力计与IMU测量数据组合推算，得到转台在地理坐标系的姿态数据，因此便于多机共同作业时坐标系的统一。
下面是IMU测量数据的获取方法。
IMU实质上包括加速度计与陀螺仪，因此，IMU测量数据包括加速度计的测量数据和陀螺仪的测量数据，而本步骤仅采用加速度计的测量数据，当然也可以根据实际情况变化单独或叠加使用陀螺仪的测量数据。
IMU在静止理想状态的x和y两个轴所组成的平面与水平面平行，因此布置在转台的IMU中的加速度计的输出为加速度计的测量数据
Figure GDA0004221421440000087
g为重力加速度，T为转置。
但在实际中，港用门座起重机的布置位置与水平面有一定的倾角，因此导致加速度计的测量数据a＝[ax ay az]T，ax为IMU坐标系CIMU下x轴方向的加速度值，ay为IMU坐标系CIMU下y轴方向的加速度值，az为IMU坐标系CIMU下z轴方向的加速度值。
通过地理坐标系CENU到当前的IMU坐标系CIMU的姿态变换矩阵
Figure GDA0004221421440000091
地理坐标系CENU的重力加速度可以转换到当前的IMU坐标系CIMU下，IMU的横滚角为γ，俯仰角为θ，偏航角为ψ，由此可以得到下式(1)-式(3)：
Figure GDA0004221421440000092
Figure GDA0004221421440000093
[ax ay az]T＝[-sinθ×g cosθsinγ×g cosθcosγ×g]T (3)
在一个实施例中，由于IMU偏航角ψ不可观，由上式(1)-式(3)可得：当前IMU的俯仰角
Figure GDA0004221421440000094
当前IMU的横滚角
Figure GDA0004221421440000095
下面是磁力计测量数据的获取方法。
磁力计的测量数据mIMU包括磁力计在坐标系CIMU的x、y、z三个轴的磁分量
Figure GDA0004221421440000096
Figure GDA0004221421440000097
本实施例将磁力计的测量数据mIMU通过IMU的横滚角为γ和俯仰角θ转到水平方向，得到转换到坐标系CIMU下后的磁力计数据
Figure GDA0004221421440000098
其具体表示为下式(4)。
Figure GDA0004221421440000099
式中，
Figure GDA00042214214400000910
分别为当磁力计数据转到水平方向后在坐标系CIMU的x、y、z三个轴的磁分量，
Figure GDA00042214214400000911
为姿态变换矩阵
Figure GDA00042214214400000912
的逆矩阵。
在IMU坐标系CIMU的x轴的磁分量数据
Figure GDA00042214214400000913
Figure GDA00042214214400000914
y轴的磁分量数据为
Figure GDA00042214214400000915
则推算得到转台与地磁磁北的夹角
Figure GDA00042214214400000916
上述磁力计解算的航向角ψm是x轴相对于磁北而言的，而地理北方与磁北之间存在一个磁偏角Δψ，若某地的经纬度和海拔已知，则此磁偏角固定，可通过国际地磁参考场计算得到，因此IMU坐标x轴相对于地理北方的偏航角ψ＝ψm+Δψ。
在一个实施例中，步骤2中的“根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000101
”的方法可以借助视觉标签5的外轮廓所具有的四个角点实现。例如视觉标签5采用AprilTag码时，通过采用相机检测到的AprilTag码，来恢复相机的位姿。当检测到视觉标签5提供的AprilTag码的四个角点在相机成像平面所在的像素坐标系中的像素坐标(下文简称为：“像素坐标”)后，角点像素坐标到视觉标签坐标系CTag的转换视为二维坐标变换，通过匹配求解单应矩阵H，可以恢复标签与相机之间的相对位姿。根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure GDA0004221421440000102
的方法具体包括：
步骤21，根据视觉标签5的角点在视觉标签坐标系CTag中的三维空间坐标P＝[X,Y,0]T与相机坐标系Ccam中的二维像素坐标p＝[x,y]T，利用[x,y,1]T＝H[X,Y,1]T，通过视觉标签5的四个角点构建8个方程，以求解获得单应矩阵H，H为3×3的单应矩阵。
根据P＝[X,Y,0]T和p＝[x,y]T，视觉标签坐标系CTag所在二维平面的四个角点在相机坐标系Ccam中的坐标分别为
Figure GDA0004221421440000103
Figure GDA0004221421440000104
其中，L为视觉标签5的实际边长。
步骤22，根据视觉标签5的二维像素坐标p＝[x,y]T与三维空间坐标P＝[X,Y,0]T的转换关系
Figure GDA0004221421440000105
有下式(5)，获取视觉标签5相对于相机的位姿
Figure GDA0004221421440000106
Figure GDA0004221421440000107
式中，s为相机的尺度，H为下式(6)提供的单应矩阵，h1-h9为单应矩阵H的9个参数，K为下式(7)提供的相机的内参矩阵，在相机图像坐标系Ccam中，fx为x方向的缩放比例，fy为y方向的缩放比例，cx为光心在x方向的像素坐标，cy为光心在y方向的像素坐标；
Figure GDA0004221421440000108
为下式(8)提供的视觉标签5相对于相机坐标系的位姿转换矩阵，
Figure GDA0004221421440000109
分别为
Figure GDA00042214214400001010
中的视觉标签到相机坐标系Ccam的平移向量和姿态变换矩阵。
Figure GDA00042214214400001011
Figure GDA0004221421440000111
Figure GDA0004221421440000112
在一个实施例中，由于式(5)隐含了一次矩阵的维度变换，三维空间坐标P＝[X,Y,0]T的Z＝0，因此本实施例可以略去位姿变换矩阵
Figure GDA0004221421440000113
的第三列，并显式表示矩阵维度变换，则有下式(9)：
Figure GDA0004221421440000114
式中，略去第三列的变换矩阵中，有
Figure GDA0004221421440000115
的六个参数R1、R2、R4、R5、R7和R8，还有
Figure GDA0004221421440000116
的三个参数t1、t2和t3，从而形成下式(10)：
Figure GDA0004221421440000117
在一个实施例中，在包含相机尺度s的情况下，可求解出平移向量
Figure GDA0004221421440000118
和姿态变换矩阵
Figure GDA0004221421440000119
姿态变换矩阵
Figure GDA00042214214400001110
为严格的正定矩阵，尺度s可以近似为下式(11)描述的姿态变换矩阵前两列模长的均值：
Figure GDA00042214214400001111
式中，[R1,R4,R7]T为[R1,R4,R7]的矩阵转置，[R2,R5,R8]T为[R2,R5,R8]的矩阵转置，‖‖2为向量的模长。
姿态变换矩阵
Figure GDA00042214214400001112
由R1、R2、R4、R5、R7和R8组成，第三列可由下式(12)描述的前两列叉乘得到：
Figure GDA0004221421440000121
根据上式(10)求出
Figure GDA0004221421440000122
和
Figure GDA0004221421440000123
后，可得到位姿转换矩阵
Figure GDA0004221421440000124
其描述为下式(13)：
Figure GDA0004221421440000125
若已知视觉标签5的绝对位姿
Figure GDA0004221421440000126
则相机的绝对位姿表示为
Figure GDA0004221421440000127
Figure GDA0004221421440000128
步骤23，通过转台21带动相机4，使视觉标签5处于相机4可观测视野的中心位置，根据步骤21获取的IMU在静止状态下在地理坐标系CENU的位姿、磁力计数据
Figure GDA0004221421440000129
和相机的图像数据，利用下式(14)获取视觉标签5在地理坐标系CENU的绝对姿态
Figure GDA00042214214400001210
Figure GDA00042214214400001211
式中，
Figure GDA00042214214400001212
为IMU到地理坐标系CENU的姿态变换矩阵，ψ、θ、γ为IMU的偏航角、俯仰角、横滚角，
Figure GDA00042214214400001213
为IMU坐标系CIMU到相机坐标系Ccam的姿态变换矩阵
Figure GDA00042214214400001214
的转置矩阵，
Figure GDA00042214214400001215
为视觉标签到相机的姿态变换矩阵，其可以根据上式(13)获得。
Figure GDA00042214214400001216
步骤23是获取单个视觉标签5在地理坐标系CENU的姿态，因此，重复步骤23，从而可以获得8个视觉标签5在地理坐标系的姿态。
需要说明的是，上述实施例的视觉标签5采用AprilTag码，此视觉标签5可以被Aruco码等多种类型的视觉标签5所替代。
由于本发明采用磁力计、IMU标记视觉标签5的姿态，因此去除了人工繁杂的测量，提高了视觉标签5姿态的准确性。
步骤3，根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机是否能够检测到视觉标签5的情形，获取转台静止状态的转台转角。
第一，相机未检测到视觉标签5的情形，具体包括：
步骤31a，利用下式(16)获取转台到地理坐标系CENU的姿态变换矩阵
Figure GDA0004221421440000131
Figure GDA0004221421440000132
式中，
Figure GDA0004221421440000133
为IMU到地理坐标系ENU的姿态变换矩阵，
Figure GDA0004221421440000134
为IMU坐标系CIMU到转台旋转中心的坐标系CB的变换矩阵
Figure GDA0004221421440000135
中的姿态变换
Figure GDA0004221421440000136
的转置矩阵，
Figure GDA0004221421440000137
可以通过安装参数获得。
步骤32a，根据转台到地理坐标系CENU的姿态变换矩阵
Figure GDA0004221421440000138
转台绕立柱的中心轴旋转的欧拉角ψ，即转台转角。
第二，相机检测到视觉标签5的情形，具体包括：
步骤31b，利用下式(17)获取转台的姿态
Figure GDA0004221421440000139
Figure GDA00042214214400001310
式中，
Figure GDA00042214214400001311
为步骤23获得的视觉标签5的绝对姿态，
Figure GDA00042214214400001312
是相机坐标系到视觉标签5的姿态变换矩阵，通过上式(13)的
Figure GDA00042214214400001313
的逆矩阵获得，
Figure GDA00042214214400001314
为IMU坐标系到相机坐标系的姿态变换矩阵，相当于已知量，
Figure GDA00042214214400001315
为IMU坐标系CIMU到转台旋转中心的坐标系CB的变换矩阵
Figure GDA00042214214400001316
中的姿态变换
Figure GDA00042214214400001317
的转置矩阵。
步骤32b，根据转台的姿态
Figure GDA00042214214400001318
获取转台绕立柱的中心轴旋转的欧拉角ψ，即转台转角。
步骤4，根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机是否能够检测到视觉标签5的情形，获取转台转动状态的转台转角。
第一，相机未检测到视觉标签5的情形，具体包括：
步骤41a，利用下式(18)获取tk+1时刻转台的姿态为
Figure GDA00042214214400001319
Figure GDA00042214214400001320
式中，
Figure GDA00042214214400001321
为tk时刻IMU的姿态矩阵对应的四元数，
Figure GDA00042214214400001322
为下式(19)提供的tk+1时刻IMU的姿态矩阵对应的四元数，δt为tk时刻到tk+1时刻的时间差，在tk时刻转台的姿态为
Figure GDA00042214214400001323
对tk+1-tk时刻内IMU的陀螺仪数据进行中值积分，ω为下式(20)提供的IMU的陀螺仪所测量的角速度的插值
Figure GDA00042214214400001324
为零偏，
Figure GDA00042214214400001325
为tk时刻IMU陀螺仪的角速度测量值，
Figure GDA00042214214400001326
为tk+1时刻IMU陀螺仪的角速度测量值。
Figure GDA00042214214400001327
Figure GDA0004221421440000141
步骤42a，根据tk+1时刻转台的姿态为
Figure GDA0004221421440000142
提供递推计算下一相邻时刻的转台转角。
第二，相机检测到视觉标签5的情形，在转动状态下时，若相机连续检测到视觉标签5，则基于卡尔曼滤波融合IMU测量数据和视觉标签5的图像数据，对转台转角进行测量，具体包括：
步骤41b，根据相机4在前一时刻tk检测到视觉标签，后一时刻tk+1检测到视觉标签，则由公式(13)、公式(14)可得tk时刻相机的姿态
Figure GDA0004221421440000143
tk+1时刻相机的姿态
Figure GDA0004221421440000144
步骤42b，经过下式(21)和(22)提供的相机与IMU间的外部参数推算公式，得到Tk时刻的IMU姿态为
Figure GDA0004221421440000145
和tk+1时刻的IMU姿态
Figure GDA0004221421440000146
进而获得IMU姿态的增量为
Figure GDA0004221421440000147
Figure GDA0004221421440000148
Figure GDA0004221421440000149
式中，
Figure GDA00042214214400001410
为tk时刻相机坐标系到地理坐标系的姿态变换矩阵，
Figure GDA00042214214400001411
为tk时刻IMU坐标系到相机坐标系的姿态变换矩阵，
Figure GDA00042214214400001412
为tk+1时刻相机坐标系到地理坐标系的姿态变换矩阵，
Figure GDA00042214214400001413
为tk+1时刻IMU坐标系到相机坐标系的姿态变换矩阵。
步骤43b，将姿态的增量为
Figure GDA00042214214400001414
转化为四元数为
Figure GDA00042214214400001415
步骤44b，将tk-tk+1时刻内IMU的陀螺仪数据
Figure GDA00042214214400001416
进行积分，得到姿态四元数的增量
Figure GDA00042214214400001417
表示为下式(23)：
Figure GDA00042214214400001418
步骤45b，对四元数
Figure GDA00042214214400001419
通过卡尔曼滤波得到姿态增量Δq,利用下式(24)将姿态增量累加到
Figure GDA00042214214400001420
上，得到tk+1时刻下融合后的IMU姿态
Figure GDA00042214214400001421
Figure GDA00042214214400001422
步骤46b，通过IMU与转台间的外部参数变换矩阵
Figure GDA00042214214400001423
利用下式(25)得到融合后的转台的姿态
Figure GDA00042214214400001424
Figure GDA00042214214400001425
步骤47b，由
Figure GDA0004221421440000151
可以得到转台绕Z轴旋转的欧拉角ψ，即转台转角。
本发明实施例还提供一种港用门座起重机外置式转台转角测量装置，其包括若干视觉标签、磁力计、IMU、相机、视觉标签获取单元、第一转台转角获取单元和第二转台转角获取单元，其中：
若干视觉标签周向隔开设置在港用门座起重机的转台。
磁力计、IMU和相机用于分别采集转台的磁力计测量数据、IMU测量数据和视觉标签的图像数据。
视觉标签姿态获取单元用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据、IMU测量数据和视觉标签的图像数据，获取视觉标签在地理坐标系CENU的姿态
Figure GDA0004221421440000152
第一转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角。
第二转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角。
在一个实施例中，视觉标签获取单元具体包括IMU姿态获取子单元、视觉标签相对姿态获取子单元和视觉标签绝对姿态获取子单元，其中：
IMU姿态获取子单元用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系CENU的姿态
Figure GDA0004221421440000153
视觉标签相对姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据视觉标签的图像数据，获取视觉标签在相机坐标系Ccam的姿态
Figure GDA0004221421440000154
视觉标签绝对姿态获取子单元，其用于根据姿态
Figure GDA0004221421440000155
和姿态
Figure GDA0004221421440000156
利用式(14)获得视觉标签在地理坐标系CENU的姿态
Figure GDA0004221421440000157
在一个实施例中，视觉标签的外轮廓具有四个角点，视觉标签相对姿态获取子单元具体包括相机绝对姿态获取模块、视觉标签绝对姿态获取模块和视觉标签相对姿态获取模块，其中：
相机绝对姿态获取模块用于根据视觉标签的角点在视觉标签坐标系CTag中的三维空间坐标P＝[X,Y,0]T与相机坐标系Ccam中的二维像素坐标p＝[x,y]T，利用[x,y,1]T＝H[X,Y,1]T、以及p＝[x,y]T与P＝[X,Y,0]T的转换关系
Figure GDA0004221421440000161
获得相机的绝对位姿表示为
Figure GDA0004221421440000162
其中，s为相机的尺度。
视觉标签绝对位姿获取模块用于通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据IMU在静止状态下在地理坐标系CENU的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系CENU的绝对姿态
Figure GDA0004221421440000163
视觉标签相对姿态获取模块用于根据视觉标签在地理坐标系CENU的绝对姿态
Figure GDA0004221421440000164
利用式(13)获取视觉标签在相机坐标系Ccam的姿态
Figure GDA0004221421440000165
第一转台转角获取单元具体包括第一转台转角计算模块和第二转台转角计算模块，其中：
第一转台转角计算模块用于在相机未检测到视觉标签的情形下计算转台转角，具体包括步骤31a和步骤32a。
第二转台转角计算模块用于在相机检测到视觉标签的情形下计算转台转角，具体步骤31b和步骤32b。
第二转台转角获取单元具体包括第三转台转角计算模块和第四转台转角计算模块，其中：
第三转台转角计算模块用于在相机未检测到视觉标签的情形下计算转台转角，具体包括步骤41a和步骤42a。
第四转台转角计算模块用于在相机检测到视觉标签的情形下计算转台转角，具体包括步骤41b-步骤46b。
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (5)

1.一种港用门座起重机外置式转台转角测量方法，其特征在于，包括：
步骤1，在港用门座起重机的转台设置相机、IMU和磁力计、以及在转台周向隔开设置若干视觉标签；
步骤2，在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系CENU的姿态
Figure FDA0004221421430000011
根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure FDA0004221421430000012
利用下式获得视觉标签在地理坐标系CENU的姿态
Figure FDA0004221421430000013
Figure FDA0004221421430000014
式中，
Figure FDA0004221421430000015
为IMU到地理坐标系CENU的姿态变换矩阵，
Figure FDA0004221421430000016
为IMU坐标系CIMU到相机坐标系Ccam的姿态变换矩阵
Figure FDA0004221421430000017
的转置矩阵，
Figure FDA0004221421430000018
为
Figure FDA0004221421430000019
中的视觉标签到相机坐标系Ccam的姿态变换矩阵；
步骤3，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；
步骤3具体包括：
第一，相机未检测到视觉标签的情形，具体包括：
步骤31a，利用下式获取转台到地理坐标系CENU的姿态变换矩阵
Figure FDA00042214214300000110
Figure FDA00042214214300000111
式中，
Figure FDA00042214214300000112
为IMU坐标系CIMU到转台旋转中心的坐标系CB的姿态变换矩阵
Figure FDA00042214214300000113
的转置矩阵；
步骤32a，根据姿态变换矩阵
Figure FDA00042214214300000114
获取转台转角；
第二，相机检测到视觉标签的情形，具体包括：
步骤31b，利用下式获取转台的姿态
Figure FDA00042214214300000115
Figure FDA00042214214300000116
式中，
Figure FDA00042214214300000117
是相机坐标系到视觉标签的姿态变换矩阵；
步骤32b，根据转台的姿态
Figure FDA00042214214300000118
获取转台转角；
步骤4，根据IMU测量数据和磁力计测量数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角；
步骤4具体包括：
第一，相机未检测到视觉标签的情形，具体包括：
步骤41a，利用下式获取tk+1时刻转台的姿态为
Figure FDA0004221421430000021
Figure FDA0004221421430000022
式中，qIMUk为tk时刻IMU的位姿矩阵对应的四元数，δt为tk时刻到tk+1时刻的时间差，ω为IMU的陀螺仪所测量的角速度的插值
Figure FDA0004221421430000023
为零偏；
步骤42a，根据tk+1时刻转台的姿态
Figure FDA0004221421430000024
递推计算下一相邻时刻的转台转角；
第二，相机检测到视觉标签的情形，具体包括：
步骤41b，根据相机在时刻tk、tk+1检测到的视觉标签，计算时刻tk、tk+1的相机的姿态
Figure FDA0004221421430000025
步骤42b，利用下式计算tk、tk+1时刻IMU坐标系到相机坐标系的姿态变换矩阵
Figure FDA0004221421430000026
进而获得IMU姿态的增量
Figure FDA0004221421430000027
Figure FDA0004221421430000028
Figure FDA0004221421430000029
式中，
Figure FDA00042214214300000210
分别为tk、tk+1时刻相机坐标系到地理坐标系的姿态变换矩阵；
步骤43b，将增量ΔR转化为四元数为
Figure FDA00042214214300000211
步骤44b，将tk-tk+1时刻内IMU的tk、tk+1时刻陀螺仪数据
Figure FDA00042214214300000212
进行积分，得到姿态四元数的增量
Figure FDA00042214214300000213
步骤45b，对四元数
Figure FDA00042214214300000214
通过卡尔曼滤波得到姿态增量Δq,利用下式计算tk+1时刻下融合后的IMU姿态
Figure FDA00042214214300000215
Figure FDA00042214214300000216
步骤46b，利用下式计算融合后的转台的姿态
Figure FDA00042214214300000217
由
Figure FDA00042214214300000218
可以得到转台转角；
Figure FDA00042214214300000219
2.如权利要求1所述的港用门座起重机外置式转台转角测量方法，其特征在于，视觉标签的外轮廓具有四个角点，步骤2中的“根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure FDA0004221421430000031
”的方法具体包括：
步骤21，根据视觉标签的角点在视觉标签坐标系CTag中的三维空间坐标P＝[X,Y,0]T与相机坐标系Ccam中的二维像素坐标p＝[x,y]T，利用[x,y,1]T＝H[X,Y,1]T，通过视觉标签的四个角点构建8个方程，以求解单应矩阵H；
步骤22，根据二维像素坐标p＝[x,y]T与三维空间坐标P＝[X,Y,0]T的转换关系
Figure FDA0004221421430000032
通过
Figure FDA0004221421430000033
获取视觉标签相对于相机的位姿
Figure FDA0004221421430000034
从而获得相机的绝对位姿
Figure FDA0004221421430000035
其中，s为相机的尺度，K为相机的内参矩阵；
步骤23，通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据步骤21获取的IMU在静止状态下在地理坐标系CENU的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系CENU的绝对姿态
Figure FDA0004221421430000036
步骤24，根据绝对姿态
Figure FDA0004221421430000037
利用下式获取位姿
Figure FDA0004221421430000038
Figure FDA0004221421430000039
式中，
Figure FDA00042214214300000310
为视觉标签到相机坐标系的平移向量。
3.一种港用门座起重机外置式转台转角测量装置，其特征在于，包括：
若干视觉标签，其周向隔开设置在港用门座起重机的转台；
磁力计、IMU和相机，其用于分别采集转台的磁力计测量数据、IMU测量数据和视觉标签的图像数据；
视觉标签获取单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据获取IMU在静止状态下在地理坐标系CENU的姿态
Figure FDA00042214214300000311
根据视觉标签的图像数据获取视觉标签在相机坐标系Ccam的位姿
Figure FDA00042214214300000312
利用下式获得视觉标签在地理坐标系CENU的姿态
Figure FDA00042214214300000313
Figure FDA00042214214300000314
式中，
Figure FDA0004221421430000041
为IMU到地理坐标系CENU的姿态变换矩阵，
Figure FDA0004221421430000042
为IMU坐标系CIMU到相机坐标系Ccam的姿态矩阵
Figure FDA0004221421430000043
的转置矩阵，
Figure FDA0004221421430000044
为
Figure FDA0004221421430000045
中的视觉标签到相机坐标系Ccam的姿态变换矩阵；
第一转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台静止状态的转台转角；
第二转台转角获取单元，其用于根据IMU测量数据、磁力计测量数据和相机图像数据，按照相机图像数据是否检测到视觉标签的情形，获取转台转动状态的转台转角；
第一转台转角获取单元具体包括：
第一转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：
步骤31a，利用下式获取转台到地理坐标系CENU的姿态变换矩阵
Figure FDA0004221421430000046
Figure FDA0004221421430000047
式中，
Figure FDA0004221421430000048
为IMU坐标系CIMU到转台旋转中心的坐标系CB的姿态变换矩阵
Figure FDA0004221421430000049
的转置矩阵；
步骤32a，根据姿态变换矩阵
Figure FDA00042214214300000410
获取转台转角；
第二转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：
步骤31b，利用下式获取转台的姿态
Figure FDA00042214214300000411
Figure FDA00042214214300000412
式中，
Figure FDA00042214214300000413
是相机坐标系到视觉标签的姿态变换矩阵；
步骤32b，根据转台的姿态
Figure FDA00042214214300000414
获取转台转角；
第二转台转角获取单元具体包括：
第三转台转角计算模块，其用于在相机未检测到视觉标签的情形下计算转台转角，具体包括：
步骤41a，利用下式获取tk+1时刻转台的姿态为
Figure FDA00042214214300000415
Figure FDA00042214214300000416
式中，
Figure FDA0004221421430000051
为tk时刻IMU的位姿矩阵对应的四元数，δt为tk时刻到tk+1时刻的时间差，ω为IMU的陀螺仪所测量的角速度的插值
Figure FDA0004221421430000052
为零偏；
步骤42a，根据tk+1时刻转台的姿态
Figure FDA0004221421430000053
递推计算下一相邻时刻的转台转角；
第四转台转角计算模块，其用于在相机检测到视觉标签的情形下计算转台转角，具体包括：
步骤41b，根据相机在时刻tk、tk+1检测到的视觉标签，计算时刻tk、tk+1的相机的姿态
Figure FDA0004221421430000054
步骤42b，利用下式计算tk、tk+1时刻IMU坐标系到相机坐标系的姿态变换矩阵
Figure FDA0004221421430000055
进而获得IMU姿态的增量
Figure FDA0004221421430000056
Figure FDA0004221421430000057
Figure FDA0004221421430000058
式中，
Figure FDA0004221421430000059
分别为tk、tk+1时刻相机坐标系到地理坐标系的姿态变换矩阵；
步骤43b，将增量ΔR转化为四元数为
Figure FDA00042214214300000510
步骤44b，将tk-tk+1时刻内IMU的tk、tk+1时刻陀螺仪数据
Figure FDA00042214214300000511
进行积分，得到姿态四元数的增量
Figure FDA00042214214300000512
步骤45b，对四元数
Figure FDA00042214214300000513
通过卡尔曼滤波得到姿态增量Δq,利用下式计算tk+1时刻下融合后的IMU姿态
Figure FDA00042214214300000514
Figure FDA00042214214300000515
步骤46b，利用下式计算融合后的转台的姿态
Figure FDA00042214214300000516
由
Figure FDA00042214214300000517
可以得到转台转角；
Figure FDA00042214214300000518
4.如权利要求3所述的港用门座起重机外置式转台转角测量装置，其特征在于，视觉标签获取单元具体包括：
IMU姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据转台处于静止状态的磁力计测量数据和IMU测量数据，获取IMU在静止状态下在地理坐标系CENU的姿态
Figure FDA00042214214300000519
视觉标签相对姿态获取子单元，其用于在一个视觉标签呈现在相机的视野中心位置时，根据视觉标签的图像数据，获取视觉标签在相机坐标系Ccam的位姿
Figure FDA0004221421430000061
视觉标签绝对姿态获取子单元，其用于根据姿态
Figure FDA0004221421430000062
和姿态
Figure FDA0004221421430000063
获得视觉标签在地理坐标系CENU的姿态
Figure FDA0004221421430000064
5.如权利要求4所述的港用门座起重机外置式转台转角测量装置，其特征在于，视觉标签的外轮廓具有四个角点，视觉标签相对姿态获取子单元具体包括：
相机绝对姿态获取模块，其用于根据视觉标签的角点在视觉标签坐标系CTag中的三维空间坐标P＝[X,Y,0]T与相机坐标系Ccam中的二维像素坐标p＝[x,y]T，利用[x,y,1]T＝H[X,Y,1]T、以及二维像素坐标p＝[x,y]T与三维空间坐标P＝[X,Y,0]T的转换关系
Figure FDA0004221421430000065
获得相机的绝对位姿
Figure FDA0004221421430000066
其中，s为相机的尺度，K为相机的内参矩阵；
视觉标签绝对姿态获取模块，其用于通过转台带动相机，使视觉标签处于相机可观测视野的中心位置，根据IMU在静止状态下在地理坐标系CENU的姿态、磁力计数据和相机的图像数据，获取视觉标签在地理坐标系CENU的绝对姿态
Figure FDA0004221421430000067
视觉标签相对姿态获取模块，其用于根据绝对姿态
Figure FDA0004221421430000068
利用下式获取位姿
Figure FDA0004221421430000069
Figure FDA00042214214300000610
式中，
Figure FDA00042214214300000611
为视觉标签到相机坐标系的平移向量。
CN202310313749.0A 2023-03-28 2023-03-28 港用门座起重机外置式转台转角测量方法及装置 Active CN116026276B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310313749.0A CN116026276B (zh) 2023-03-28 2023-03-28 港用门座起重机外置式转台转角测量方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310313749.0A CN116026276B (zh) 2023-03-28 2023-03-28 港用门座起重机外置式转台转角测量方法及装置

Publications (2)

Publication Number Publication Date
CN116026276A CN116026276A (zh) 2023-04-28
CN116026276B true CN116026276B (zh) 2023-06-13

Family

ID=86070929

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310313749.0A Active CN116026276B (zh) 2023-03-28 2023-03-28 港用门座起重机外置式转台转角测量方法及装置

Country Status (1)

Country Link
CN (1) CN116026276B (zh)

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104978476B (zh) * 2014-04-14 2019-06-25 李巍岳 利用智能手机进行室内地图现场补测的方法
CA2934102A1 (en) * 2015-06-25 2016-12-25 Appropolis Inc. A system and a method for tracking mobile objects using cameras and tag devices
CN105836627B (zh) * 2016-04-20 2018-02-06 中船第九设计研究院工程有限公司 一种基于三维坐标定位的门座起重机位姿确定方法
CN108375382B (zh) * 2018-02-22 2021-04-02 北京航空航天大学 基于单目视觉的位置姿态测量系统精度检校方法和装置
CN108439216B (zh) * 2018-03-22 2019-11-15 东南大学 一种门座式起重机形变倾角在线监测系统及方法
CN109164829B (zh) * 2018-10-23 2021-08-27 哈尔滨工业大学（深圳） 一种基于力反馈装置和vr感知的飞行机械臂系统及控制方法
CN109751070A (zh) * 2018-12-29 2019-05-14 中国矿业大学 基于imu实时监测支护位姿的液压支架及其检测方法
CN111366125B (zh) * 2020-02-29 2021-10-08 上海市特种设备监督检验技术研究院 一种履带式起重机回转角度测量方法
CN113566774A (zh) * 2021-07-29 2021-10-29 上海市特种设备监督检验技术研究院 一种回转角度的测量装置、系统和方法
CN217132155U (zh) * 2021-12-29 2022-08-05 南通恒信精工机械有限公司 一种起重机回转角测量装置

Also Published As

Publication number Publication date
CN116026276A (zh) 2023-04-28

Similar Documents

Publication Publication Date Title
CN108061855B (zh) 一种基于mems传感器的球形电机转子位置检测方法
CN103808331B (zh) 一种mems三轴陀螺仪误差标定方法
EP2557394B1 (en) System for processing pulse signals within an inertial navigation system
KR100533106B1 (ko) 지자계 센서의 자세 오차 보상장치 및 방법
CN100516775C (zh) 一种捷联惯性导航系统初始姿态确定方法
CN109540135B (zh) 水田拖拉机位姿检测和偏航角提取的方法及装置
CN101900559B (zh) 一种捷联惯性导航系统双轴旋转调制方法
CN102879832B (zh) 用于地磁要素测量系统的非对准误差校正方法
CN101617199B (zh) 一种用于补偿位置测量设备中的漂移的方法
CN106257911A (zh) 用于视频图像的图像稳定方法和装置
CN112923924B (zh) 一种锚泊船舶姿态与位置监测方法及系统
CN109916394A (zh) 一种融合光流位置和速度信息的组合导航算法
CN113267794B (zh) 一种基线长度约束的天线相位中心校正方法及装置
CN112116651B (zh) 一种基于无人机单目视觉的地面目标定位方法和系统
CN112762964B (zh) 自动驾驶车辆的惯性测量单元的标定方法及装置、系统
CN110006454B (zh) 一种imu标定三轴转台垂直度和初始姿态的方法
CN103727939A (zh) 一种双轴旋转的姿态测量系统及其测量方法
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN109470274B (zh) 一种车载光电经纬仪载车平台变形测量系统及方法
CN116026276B (zh) 港用门座起重机外置式转台转角测量方法及装置
CN116972875A (zh) 一种基于陀螺仪的物体运动轨迹监测方法
CN116753953A (zh) 一种基于视觉的无人机组合导航方法
CN116518959A (zh) 基于uwb与3d视觉结合的空间摄像机的定位方法和装置
CN114674405B (zh) 重力测量方法、装置、计算机设备和计算机可读存储介质
CN112833910A (zh) 惯性测量单元的水平安装偏差角标定方法、设备及介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant