CN111595235B - 一种六自由度视觉位姿相对误差测量装置及测量方法 - Google Patents

Abstract

本发明公开了一种六自由度视觉位姿相对误差测量装置及测量方法，属于隧道建设管片拼装机技术领域。本发明通过在管片上设置ArUco码作为相机的靶标，并使其与角位台共面，调节角位台与旋转台使靶标坐标系与相机坐标系对齐。对比X向滑动模组、Y向滑动模组、Z向滑动模组、角位台及旋转台与相机测量的六自由度数值，即得到视觉相对测量误差。按照本发明公开的方法可以测量出管片的x轴平移误差、y轴平移误差、z轴平移误差、绕x轴的转角误差、绕y轴的转角误差和绕z轴的转角误差。由于本发明具有较高的测量精度，故可为视觉六自由姿态测量相对误差，提高视觉测量的精度和可靠性。

Description

一种六自由度视觉位姿相对误差测量装置及测量方法

技术领域

本发明属于隧道建设管片拼装机技术领域，涉及一种六自由度视觉位姿相对误差测量装置及测量方法。

背景技术

盾构机是一种用于隧道施工的专用工程机械，盾构施工法成本比较低、非常安全、速度快、土方量少、机械化水平高，在一些地质复杂的情况下只能依托盾构。盾构集中了光电技术、液压技术、传感器、电控技术为一体的大型机械设备，对周围的环境影响小，在城市的地下综合管廊以及隧道建设过程中应用广泛。当盾构机向前推进一环，推进油缸缩回，盾构机进入管片拼装模式，这时候暴露出来的土体需要及时的进行支护，管片拼装机将预制好的混凝土管片以一定的顺序安装到刚开挖好的隧道表面形成衬砌，为盾构机在下一环掘进的时候提供支撑点。作为盾构机重要的一部分，管片拼装的精度高低将直接影响着整个地下掘进施工效率的提升。

在盾构法建设隧道中，管片的拼装一直是建设中十分重要的一个环节，它不仅起到对已挖隧道的结构形状进行保护，防止隧道沉降，同时还可以防止地下水渗漏以及提供施工更为安全的空间等作用。然而，目前国内大部分管片的拼装，其运输拼装全过程均需要人工操纵管片拼装机，管片拼装的速度与精度都依赖于人的熟练程度。因此，管片拼装机的自动化需求就逐渐呈现出来。管片的自动化拼装对于提高隧道的建设速度和减轻人的工作强度具有重要意义。视觉测量技术是以计算机视觉为基础的新型测量技术，具备了非接触式、测量范围广、信息量大、是其他类型的传感器所无法比拟的，在管片的自动化拼装领域有广泛的应用。但视觉测量的精度难以保证，故提出一种六自由度视觉位姿相对误差测量装置及测量方法。

发明内容

本发明提供了一种六自由度视觉位姿相对误差测量装置及测量方法，通过在管片上设置ArUco码作为相机的靶标，并使其与角位台共面，调节角位台与旋转台使靶标坐标系与相机坐标系对齐。对比X向滑动模组、Y向滑动模组、Z向滑动模组、角位台及旋转台与相机测量的六自由度数值，即得到视觉相对测量误差。

为实现上述目的，本发明采用的技术方案是：

一种六自由度视觉位姿相对误差测量装置，所述的六自由度视觉位姿相对误差测量装置包括X向滑动模组、Y向滑动模组、Z向滑动模组、工业相机1、支架6、旋转台、角位台和放置板14。

所述的支架6为框架结构，支架6的每个顶角安装有支架连接角件9，用于增强结构稳定性。

所述的X向滑动模组，共有两组，每组X向滑动模组包括X向滑块、丝杠5和模组底板7；X向滑动模组通过连接件11固定安装在支架6上端；其中一组X向滑动模组安装有步进电机3，步进电机3通过联轴器4与丝杠5连接；步进电机3用于驱动丝杠5转动，丝杠5的转动带动安装在丝杠5上的X向滑块沿X方向移动；每一组X向滑动模组的端部均安装有同步齿轮12，同步齿轮12与丝杠5固定连接，两个同步齿轮12之间安装有同步带13；通过步进电机3驱动、同步齿轮12和同步带13传动，实现两个X向滑块同步移动；

所述的Y向滑动模组包括Y向滑块、丝杠5和模组底板7，Y向滑动模组安装在两个X向滑块上；Y向滑动模组上安装有步进电机3，步进电机3通过联轴器4与丝杠5连接，步进电机3用于驱动丝杠5转动，丝杠5的转动带动安装在丝杠5上的Y向滑块沿Y方向移动。

所述的Z向滑动模组包括Z向滑块、丝杠5和模组底板7，Z向滑动模组通过连接板10安装在Y向滑块上，Z向滑动模组上安装有步进电机3，步进电机3通过联轴器4与丝杠5连接，步进电机3用于驱动丝杠5转动，丝杠5的转动带动安装在丝杠5上的Z向滑块沿Z方向移动。

所述的X向滑动模组、Y向滑动模组和Z向滑动模组的滑动方向两两垂直。

所述的工业相机1共有两台，安装在相机连接板2两端，相机连接板2中部与Z向滑块连接。

所述的旋转台包括旋转台底座15、锁紧螺丝16、粗精调切换螺丝17、微分头18、刻度盘19和旋转手柄20；旋转台底座15安装在支架6底端；旋转台底座15的侧边上设置有粗精调切换螺丝17、刻度盘19和旋转手柄20。粗精调切换螺丝17的两侧安装有锁紧螺丝16和微分头18。所述的锁紧螺丝16用于锁紧旋转台；所述的粗精调切换螺丝17用于切换调节模式，当粗精调切换螺丝17松开时旋转手柄20处于工作状态，当粗精调切换螺丝17锁紧时微分头18处于工作状态。所述的刻度盘19安装在旋转台底座15上。

所述的角位台安装在旋转台上，角位台设置有用于驱动角位台转动的角位台Y轴调节螺丝21和角位台X轴调节螺丝22。

所述的放置板14安装在角位台之上。

进一步地，所述的X向滑动模组通过12个直角连接件与支架连接。

进一步地，所述支架6由40x40mm规格的铝型材组成，用于安装T型螺栓。

进一步地，所述的步进电机3步距角为1.8度，为减弱消除步进电机的低频振动，驱动器细分为2000。

进一步地，所述的工业相机1为1920x1200分辨率的RGB相机，镜头为8mm定焦镜头。

进一步地，所述的角位台调整范围为-10～10度，精度为0.1度。

进一步地，所述的旋转台调整范围为0～360度，精度为0.1度。

本发明实例采用两个工业相机对靶标同时对靶标进行测量，即双目测量。关于靶标坐标原点在相机坐标系位置的求取采用基于射线交汇法的双目模型。以下对该方法作进一步说明。设世界坐标系为OXYZ,设世界坐标系任一点P点的坐标为(X_w,Y_w,Z_w)，点P在左右相机的成像平面的坐标分别为P₁和P₂，两摄像机的投影矩阵分别为：

由相机坐标系与世界坐标系之间的转换关系：

其中M₁＝m₁R₁T₁,M₂＝m₂R₂T₂；m₁、m₂分别为左右摄像机的内参矩阵，R₁、T₁分别为左摄像机坐标系相对于世界坐标系的旋转矩阵和相对于原点之间的平移向量，R₂、T₂分别为右摄像机坐标系相对于世界坐标系的旋转矩阵和相对于原点之间的平移向量，z₁和z₂分别为空间点P在两台相机的光心坐标系中沿光轴方向的坐标值。

设左摄像机坐标系与世界坐标系重合，则此时：

将(5)(6)式代入(3)(4)式即可求得靶标坐标原点在左相机坐标系下的坐标。

关于靶标坐标系相对相机坐标系三个轴转角的测量采用n点透视问题的解决方案，具体的是通过相机识别靶标图像并获取靶标的四个特征点，即可得到四个特征点在图像坐标系的坐标，靶标特征点的世界坐标已知，采用透视问题即可解得靶标坐标系相对相机坐标系三个轴转角。

通过以上双目汇聚模型和n点透视问题即可求得靶标相对于相机坐标系的六自由度姿态，由于靶标固定于角位台的放置板14之上，故角位台旋转台的姿态即为靶标的姿态。以上所述模组与旋转台在安装时不可避免存在安装误差，在具体的测量过程中可先由相机获取靶标的六自由姿态，在测量一个自由的的误差时，可调节旋转台、角位台、X向滑动模组、Y向滑动模组和Z向滑动模组使其剩余的无关自由度数值为零，即可精确的测量出每个自由度的相对误差。

本发明具有以下有益效果：

1、相机可通过特定算法测量出靶标的六自由姿态，由于模组及支架安装的误差不可避免会产生系统误差。通过调节角位台和旋转台可将靶标坐标系与相机坐标系对齐。

2、由于系统具有较高的测量精度，故可为视觉六自由姿态测量相对误差，提高视觉测量的精度和可靠性。

附图说明

图1为本发明实施例整体结构主视图；

图2为本发明实施例整体结构左视图；

图3为本发明实施例整体结构俯视图；

图4为本发明实施例整体结构后视图；

图5为本发明实施例旋转台和角位台视图；

图6为本发明实施例旋转台和角位台补充视图。

图7为本发明实施例相机识别的靶标ArUco码。

图中：1工业相机；2相机连接板；3步进电机；4联轴器；5丝杠；6支架；7模组底板；8角位台和旋转台；9支架连接角件；10连接板；11连接件；

12同步齿轮；13同步带；14放置板；15旋转台底座；16锁紧螺丝；

17粗精调切换螺丝；18微分头；19刻度盘；20旋转手柄；21角位台Y轴调节螺丝；22角位台X轴调节螺丝。

具体实施方式

为使得本发明的发明目的、特征、优点能够更加的明显和易懂，以下参照附图并结合具体实施方式来进一步描述本发明，以令本领域技术人员参照说明书文字能够据以实施，本发明保护范围并不受限于该具体实施方式。显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明的测量方法如下：

第一步，对两台工业相机1进行相机标定，获取两台工业相机1的内参矩阵k₁、k₂和两台相机间的旋转矩阵R、平移矩阵T，其中k₁是3×3大小的矩阵，其参数包含(5)式中的f_lx、f_ly、u_ol、v_ol，k₂是3×3大小的矩阵，其参数包含(6)式中的f_rx、f_ry、u_or、v_or，R是3×3大小的矩阵，其参数包含(6)式中的r₁-r₉，T是3×1大小的矩阵，其参数包含(6)式中t₁、t₂、t₃。

第二步，将带有靶标ArUco码的管片放置在放置板14上，建立靶标坐标系oxyz，靶标(图7)的左上角点为靶标坐标系原点o，x轴方向为o点指向右上角点，y轴方向为o点指向左下角点，z轴方向为经过o点且垂直靶标ArUco码所在的平面的直线。使靶标ArUco码的坐标系的X轴与角位台X轴调节螺丝22的轴线平行，使靶标ArUco码的坐标系的Y轴与角位台Y轴调节螺丝21的轴线平行；调解X向滑动模组、Y向滑动模组和Z向滑动模组，使工业相机1对准管片上的靶标ArUco码；获取靶标ArUco码左上角点在两工业相机1像素平面的坐标值，由双目汇聚模型求取左上角点在左相机坐标系下的坐标值x、y、z。

第三步，两台工业相机1识别靶标并获取靶标ArUco码外轮廓的四个角点在像素平面的坐标，由PnP(n点透视问题)算法可求解出靶标坐标系相对于左相机坐标系的旋转矩阵R₁，进而求出靶标坐标系绕左相机坐标系的三个欧拉角α、β、γ，

其中α是指靶标坐标系绕左相机坐标系X轴的旋转角度，β是指靶标坐标系绕左相机坐标系Y轴的旋转角度，γ是指靶标坐标系绕左相机坐标系Z轴的旋转角度。

第四步，对x轴平移误差进行测量；松开锁紧螺丝16和粗精调切换螺丝17，转动旋转手柄20使γ的数值逼近0，锁紧粗精调切换螺丝17，调节微分头18使γ＝0，调节角位台Y轴调节螺丝21使β＝0，调节角位台X轴调节螺丝22使α＝0，使用PLC控制器移动Y向滑块使y＝0。记录此时x的数值为x₁，通过PLC控制器给定X向滑块的移动距离x₂，再次记录此时x的数值为x₃，则此次x轴平移误差Δx＝x₃-x₁-x₂。重复测量得到多个Δx，求其平均值即为x轴平移的相对误差。

第五步，对y轴平移误差进行测量；使用PLC控制器移动X向滑块使x＝0。记录此时y的数值为y₁，通过PLC控制器给定Y向滑块的移动距离y₂，再次记录此时y的数值为y₃，则此次y轴平移误差Δy＝y₃-y₁-y₂。重复测量得到多个Δy，求其平均值即为y轴平移的相对误差。

第六步，对z轴平移误差进行测量，使用PLC控制器移动Y向滑块使y＝0。记录此时z的数值为z₁，通过PLC控制器给定Z向滑块的移动距离z₂，再次记录此时z的数值为z₃，则此次z轴平移误差Δz＝z₃-z₁-z₂。重复测量得到多个Δz，求其平均值即为z轴平移的相对误差。

第七步，对绕z轴的转角误差进行测量；调节Z向滑块使靶标能够在相机视野内清楚成像。记录此时γ的数值为γ₁，松开粗精调切换螺丝17，转动旋转手柄20并调节微分头18给定绕z轴的转角度数γ₂，再次记录此时γ的数值为γ₃，则此次绕z轴的转角误差Δγ＝γ₃-γ₁-γ₂。重复测量得到多个Δγ，求其平均值即为绕z轴的转角误差。

第八步，对绕y轴的转角误差进行测量；转动旋转手柄20并调节微分头18使γ＝0，锁紧粗精调切换螺丝17和锁紧螺丝16。记录此时β的数值为β₁，调节角位台Y轴调节螺丝21给定绕y轴的转角度数β₂，再次记录此时β的数值为β₃，则此次绕y轴的转角误差Δβ＝β₃-β₁-β₂。重复测量得到多个Δβ，求其平均值即为绕y轴的转角误差。

第九步，对绕x轴的转角误差进行测量；调节角位台Y轴调节螺丝21使β＝0。记录此时α的数值为α₁，调节角位台X轴调节螺丝22给定绕x轴的转角度数α₂，再次记录此时α的数值为α₃，则此次绕x轴的转角误差Δα＝α₃-α₁-α₂。重复测量得到多个Δα，求其平均值即为绕x轴的转角误差。

按照上述步骤准确获取使用靶标ArUco码间接测量管片位姿的六自由度相对误差。

Claims (5)

1.一种六自由度视觉位姿相对误差测量装置的测量方法，其特征在于，所述测量方法基于六自由度视觉位姿相对误差测量装置实现，所述的六自由度视觉位姿相对误差测量装置包括X向滑动模组、Y向滑动模组、Z向滑动模组、工业相机(1)、支架(6)、旋转台、角位台和放置板(14)；

所述的支架(6)为框架结构，其每个顶角安装有支架连接角件(9)，用于增强结构稳定性；

所述的X向滑动模组，共有两组，每组X向滑动模组包括X向滑块、丝杠(5)和模组底板(7)；X向滑动模组通过连接件(11)固定安装在支架(6)上端；其中一组X向滑动模组安装有步进电机(3)，步进电机(3)通过联轴器(4)与丝杠(5)连接；步进电机(3)用于驱动丝杠(5)转动，丝杠(5)的转动带动安装在丝杠(5)上的X向滑块沿X方向移动；每一组X向滑动模组的端部均安装有同步齿轮(12)，同步齿轮(12)与丝杠(5)固定连接，两个同步齿轮(12)之间安装有同步带(13)；通过步进电机(3)驱动、同步齿轮(12)和同步带(13)传动，实现两个X向滑块同步移动；

所述的Y向滑动模组，包括Y向滑块、丝杠(5)和模组底板(7)，Y向滑动模组安装在两个X向滑块之间；Y向滑动模组上安装有步进电机(3)，步进电机(3)通过联轴器(4)与丝杠(5)连接；步进电机(3)用于驱动丝杠(5)转动，丝杠(5)的转动带动安装在丝杠(5)上的Y向滑块沿Y方向移动；

所述的Z向滑动模组，包括Z向滑块、丝杠(5)和模组底板(7)，Z向滑动模组通过连接板(10)安装在Y向滑块上，Z向滑动模组上安装有步进电机(3)，步进电机(3)通过联轴器(4)与丝杠(5)连接；步进电机(3)用于驱动丝杠(5)转动，丝杠(5)的转动带动安装在丝杠(5)上的Z向滑块沿Z方向移动；

其中，两组X向滑动模组、一组Y向滑动模组和一组Z向滑动模组，相互间滑动方向两两垂直；

所述的工业相机(1)共有两台，安装在相机连接板(2)两端，相机连接板(2)中部与Z向滑块连接；

所述的旋转台，包括旋转台底座(15)、锁紧螺丝(16)、粗精调切换螺丝(17)、微分头(18)、刻度盘(19)和旋转手柄(20)；旋转台底座(15)安装在支架(6)底端；旋转台底座(15)的侧边上设置有粗精调切换螺丝(17)、刻度盘(19)和旋转手柄(20)；粗精调切换螺丝(17)的两侧安装有锁紧螺丝(16)和微分头(18)；所述的锁紧螺丝(16)用于锁紧旋转台；所述的粗精调切换螺丝(17)用于切换调节模式，当粗精调切换螺丝(17)松开时旋转手柄(20)处于工作状态，当粗精调切换螺丝(17)锁紧时微分头(18)处于工作状态；所述的刻度盘(19)安装在旋转台底座(15)上；

所述的角位台安装在旋转台上，角位台设置有用于驱动角位台转动的角位台Y轴调节螺丝(21)和角位台X轴调节螺丝(22)；

所述的放置板(14)安装在角位台之上；

所述测量方包括以下步骤：

第一步，对两台工业相机(1)进行相机标定，获取两台工业相机(1)的内参矩阵k₁、k₂和两台相机间的旋转矩阵R、平移矩阵T；

第二步，将带有靶标ArUco码的管片放置在放置板(14)上，建立靶标ArUco码坐标系oxyz，靶标ArUco码的左上角点为靶标坐标系原点o，x轴方向为o点指向右上角点，y轴方向为o点指向左下角点，z轴方向为经过o点且垂直靶标ArUco码所在的平面的直线；使靶标ArUco码的坐标系的x轴与角位台X轴调节螺丝(22)的轴线平行，使靶标ArUco码的坐标系的Y轴与角位台Y轴调节螺丝(21)的轴线平行；调解X向滑动模组、Y向滑动模组和Z向滑动模组，使工业相机(1)对准管片上的靶标ArUco码；获取靶标ArUco码左上角点在两工业相机(1)像素平面的坐标值，由双目汇聚模型求取左上角点在左相机坐标系下的坐标值x、y、z；

第三步，两台工业相机(1)识别靶标ArUco码并获取靶标ArUco码外轮廓的四个角点在像素平面的坐标，由PnP算法求解出靶标坐标系相对于左相机坐标系的旋转矩阵R₁，进而求出靶标ArUco码坐标系绕左相机坐标系的三个欧拉角α、β、γ，其中α是指靶标坐标系绕左相机坐标系X轴的旋转角度，β是指靶标坐标系绕左相机坐标系Y轴的旋转角度，γ是指靶标坐标系绕左相机坐标系Z轴的旋转角度；

第四步，对x轴平移误差进行测量；松开锁紧螺丝(16)和粗精调切换螺丝(17)，转动旋转手柄(20)使γ的数值逼近0，锁紧粗精调切换螺丝(17)，调节微分头(18)使γ＝0，调节角位台Y轴调节螺丝(21)使β＝0，调节角位台X轴调节螺丝(22)使α＝0，使用PLC控制器移动Y向滑块使y＝0；记录此时x的数值为x₁，通过PLC控制器给定X向滑块的移动距离x₂，再次记录此时x的数值为x₃，则此次X轴平移误差Δx＝x₃-x₁-x₂；重复测量得到多个Δx，求其平均值即为x轴平移的相对误差；

第五步，对y轴平移误差进行测量；使用PLC控制器移动X向滑块使x＝0；记录此时y的数值为y₁，通过PLC控制器给定Y向滑块的移动距离y₂，再次记录此时y的数值为y₃，则此次Y轴平移误差Δy＝y₃-y₁-y₂；重复测量得到多个Δy，求其平均值即为y轴平移的相对误差；

第六步，对z轴平移误差进行测量，使用PLC控制器移动Y向滑块使y＝0；记录此时z的数值为z₁，通过PLC控制器给定Z向滑块的移动距离z₂，再次记录此时z的数值为z₃，则此次Z轴平移误差Δz＝z₃-z₁-z₂；重复测量得到多个Δz，求其平均值即为z轴平移的相对误差；

第七步，对绕z轴的转角误差进行测量；调节Z向滑块使靶标能够在相机视野内清楚成像；记录此时γ的数值为γ₁，松开粗精调切换螺丝(17)，转动旋转手柄(20)并调节微分头(18)给定绕z轴的转角度数γ₂，再次记录此时γ的数值为γ₃，则此次绕z轴的转角误差Δγ＝γ₃-γ₁-γ₂；重复测量得到多个Δγ，求其平均值即为绕z轴的转角误差；

第八步，对绕y轴的转角误差进行测量；转动旋转手柄(20)并调节微分头(18)使γ＝0，锁紧粗精调切换螺丝(17)和锁紧螺丝(16)；记录此时β的数值为β₁，调节角位台Y轴调节螺丝(21)给定绕y轴的转角度数β₂，再次记录此时β的数值为β₃，则此次绕y轴的转角误差Δβ＝β₃-β₁-β₂；重复测量得到多个Δβ，求其平均值即为绕y轴的转角误差；

第九步，对绕x轴的转角误差进行测量；调节角位台Y轴调节螺丝(21)使β＝0；记录此时α的数值为α₁，调节角位台x轴调节螺丝(22)给定绕X轴的转角度数α₂，再次记录此时α的数值为α₃，则此次绕x轴的转角误差Δα＝α₃-α₁-α₂；重复测量得到多个Δα，求其平均值即为绕x轴的转角误差；

按照上述步骤准确获取使用靶标ArUco码间接测量管片位姿的六自由度相对误差。

2.根据权利要求1所述的一种六自由度视觉位姿相对误差测量装置的测量方法，其特征在于，所述的步进电机(3)步距角为1.8度，驱动器细分为2000。

3.根据权利要求1或2所述的一种六自由度视觉位姿相对误差测量装置的测量方法，其特征在于，所述的角位台调整范围为-10～10度，精度为0.1度；所述的旋转台调整范围为0～360度，精度为0.1度。

4.根据权利要求1或2所述的一种六自由度视觉位姿相对误差测量装置的测量方法，其特征在于，所述的工业相机(1)是分辨率为1920x1200的RGB相机，镜头为8mm定焦镜头。

5.根据权利要求3所述的一种六自由度视觉位姿相对误差测量装置的测量方法，其特征在于，所述的工业相机(1)是分辨率为1920x1200的RGB相机，镜头为8mm定焦镜头。

Images