CN110954067B - 一种基于靶标的单目视觉挖掘机位姿测量系统及测量方法 - Google Patents

Abstract

本发明公开了一种基于靶标的单目视觉挖掘机位姿测量系统及测量方法，包括靶标、单目相机和计算模块，所述靶标包括安装在挖掘机同一侧的铲斗靶标、斗杆靶标和动臂靶标，所述铲斗靶标固定在铲斗连接杆上靠近铲斗一端，所述斗杆靶标固定在斗杆上，所述动臂靶标固定在动臂上，所述单目相机的视野覆盖铲斗靶标、斗杆靶标和动臂靶标，计算模块用于根据单目相机采集到的靶标图像计算各个靶标的空间六自由度参数。能够有效的避免在动作大、作业环境恶劣、容易与作业对象发生碰撞、震动幅度大的工作装置上安装测量传感器等价格高昂的精密测量件容易损坏的缺陷，从而提高监测的可靠性和稳定性，保证了挖掘机作业的安全性，同时也降低了维护成本。

Description

一种基于靶标的单目视觉挖掘机位姿测量系统及测量方法

技术领域

本发明属于机器测量技术领域，具体涉及一种基于靶标的单目视觉挖掘机位姿测量系统及测量方法。

背景技术

建筑行业一般面临着生产效率低，危险性高发的问题。为了降低建筑行业的高劳动量，高风险，一种重要的方式就是提高建筑施工的自动化程度。液压挖掘机是功能最典型、结构最复杂、用途最广泛的建筑工程机械之一。作为建筑工程机械的主流产品，它在工业与民用建筑、交通运输、水利电力工程、矿山采掘以及军事工程等施工中起着极为重要的作用。

液压挖掘机主要由行走装置、工作装置和回转平台三大部分组成。挖掘任务主要由工作装置完成，工作装置由动臂、斗杆、铲斗、联杆、油缸等组成。其中动臂、斗杆、铲斗三个部位有液压缸，行走装置和回转平台有三个液压马达用于左右行走和回转。挖掘机作业的基本方法是动臂斗杆液压缸置于一定的位置不动，只操作铲斗油缸挖掘手柄，使铲斗转动切削土壤。在铲斗装满后工作装置从挖掘面旋转到卸土地点。这一过程要求铲斗底部一经离开挖掘面，便提升动臂与调整铲斗转角，以适应所要求的卸土高度。当铲斗回转接近装土车辆时，松开回转手柄，用回转制动器慢慢地制动转台，并同时卸土。当工作装置基本停稳后，翻转铲斗卸土。卸土完毕后，工作装置应立刻返回挖掘面。返回过程中，铲斗翻转，调整好切削角，使铲斗切入土中，开始重复挖掘动作。挖掘机的操作过程十分复杂。因此挖掘机对操纵人员的技术要求高，且劳动强度大，工作环境恶劣，危险系数高，对工程的施工效率、成本及质量问题都造成严重的影响。挖掘机的智能化程度对解决上述问题有重大影响，所以为提高挖掘机的工作效率，降低操作人员的劳动强度，要提高挖掘机的智能化程度和发展建筑工程自动化及机器人技术。为了更好的提高建筑机械的自动化程度，检测建筑场地与建筑机械(如挖掘机)的执行机构的位姿是基本的要求，位姿包含位置与姿态。

近几十年来，国内外部分工程机械主要企业和高校对挖掘机的作业自动化做了大量的研究工作。例如国外有：日本小松公司、英国兰卡斯特大学等；国内有同济大学、浙江大学、山河智能公司等。已研制成的自动化挖掘机：操作人员只需发出指令，给出挖掘点及其挖掘任务，控制系统根据实时反馈的机体定位信息、工作装置的姿态信息，控制回转平台的液压马达和三个液压缸的动作自动实现相应的挖掘操作。其机体定位信息通常由GPS导航系统完成。而对工作装置的姿态的测量，多采用在工作装置的动臂、斗杆和铲斗的铰点处安装倾角传感器或光电编码器来实现。其姿态信息获取方式存在的问题是：挖掘机挖掘作业过程中，工作装置不可避免地会与土壤、岩石，或其他物体发生碰撞，产生剧烈振动，极易造成安装在其上的传感器损坏，最终导致整个自动化控制系统无法正常运行。系统的可靠性低，维护成本高。

在建筑施工过程中，实时准确的监测施工场地，将检测到的信息返回操作人员，有助于操作人员根据施工环境以及作业要求更好地进行施工作业，更高效的完成建筑施工。最终提高建筑施工提高工作效率。

发明内容

本发明主要是基于现有的自动化挖掘机的工作装置的位姿测量系统的缺点与不足，提出的通过单目相机和靶标来测量挖掘机工作装置位姿。这种测量系统和方法能够准确的实时的动态测量到挖掘机处于工作状态时工作装置的位姿,它在挖掘机的工作的装置上不用安装容易损坏。

为达到上述目的，本发明所述一种基于靶标的单目视觉挖掘机位姿测量系统，包括靶标、单目相机和计算模块，所述靶标包括安装在挖掘机同一侧的铲斗靶标、斗杆靶标和动臂靶标，所述铲斗靶标固定在铲斗连接杆上靠近铲斗的一端，所述斗杆靶标固定在斗杆上，所述动臂靶标固定在动臂上，所述单目相机的视野覆盖铲斗靶标、斗杆靶标和动臂靶标，所述计算模块用于根据单目相机采集到的靶标图像计算各个靶标的空间六自由度参数。

进一步的，铲斗靶标、斗杆靶标和动臂靶标的形状相同，均为3×3的黑白相间正方形。

进一步的，黑白相间正方形的每个边缘具有8个凸出的黑色矩形区域。

进一步的，铲斗靶标、斗杆靶标和动臂靶标的大小相同，均为228×228mm²。

一种基于上述的单目视觉挖掘机位姿测量系统的挖掘机位姿测量方法，包括以下步骤：

步骤1、测量系统初始化：通过建立标定物上坐标已知的点与其图像点之间的对应，利用标定算法获得相机的内外参数；校正镜头畸变，使用最大似然估计进行优化，得到相机的内参数矩阵K、外参数旋转矩阵R，平移向量t和径向畸变参数k1，k2，k3，切向畸变参数p1， p2；

步骤2、图像采集：当挖掘机处于工作状态时，利用单目相机拍摄挖掘机整个工作过程中铲斗靶标、斗杆靶标和动臂靶标的数字图像，并将拍摄到的数字图像传递给计算机；

步骤3、靶标检测：根据单目相机拍摄到的图案，得到铲斗靶标、斗杆靶标和动臂靶标上多个鞍点的图像坐标(u_i，v_i)，其中i为整数；

步骤4、工作装置世界坐标系的位姿计算：获取实际挖掘机工作装置的工作平面与图像平面之间的映射关系，通过步骤1得到的相机的内参数矩阵K、外参数旋转矩阵R，平移向量t 和径向畸变参数k1，k2，k3，切向畸变参数p1，p2，以及步骤3测量到的靶标内部鞍点的图像坐标经坐标之间的变换关系转换成该点的实际坐标；

步骤5、实时性动态监测：重复步骤2至步骤4，实时的动态监测出挖掘机处于工作状态时工作装置的位姿信息。

进一步的，步骤4中，坐标系之间的转换关系如下：

其中

x′＝X_C/Z_C，y′＝Y_C/ZC，

x″＝(1+k₁r²+k₂r⁴+k₃r⁶)x′+2p₁x′y′+p₂(r²+2x′²)，

y″＝(1+k₁r²+k₂r⁴+k₃r⁶)y′+p₁(r²+2y′²)+2p₂x′y′，

r²＝x′²+y′²，

u＝f_x·x″+cx，v＝f_y·y″+cy；

其中，(u，v)为图像坐标，(X_w，Y_w，Z_w)为世界坐标，(X_C，Y_C，Z_C)为相机坐标，f_x为图像水平轴的尺度因子，f_y为图像垂直轴的尺度因子，(u₀，v₀)为主点坐标(即光轴与图像平面的交点)，R是标定板平面相对于相机坐标系的旋转矩阵，t是靶标相对于相机坐标系的平移向量，s是比例因子，x′、y′、x″、y″和r均为中间变量；

从上述方程可以得到R和t：

t^T＝[t₁ t₂ t₃]；

将检测到的鞍点i的图像坐标系坐标(u_i，v_i)转换为以动臂、斗杆和铲斗被拍摄平面的世界坐标系坐标(X_w，Y_w，Z_w)和三个转角(α，β，γ)：

其中α是靶标绕z轴的转角，β是靶标绕y轴的转角，γ是靶标绕x轴的转角；

故得到靶标的六自由度(α，β，γ，X_w，Y_w，Z_w)参数，此六自由度参数是以相机为原点的世界坐标系的坐标参数。

进一步的，步骤3的具体过程为：首先进行连通区域检测和识别标记的靶标；然后根据连通区域的面积大小和欧拉数作为基本判断准则，识别靶标内部的标记区域；接着进行四边形拟合，计算测量图像的梯度，获取区域边缘和靶标的四个边缘方向；接着获取的边缘进行最小二乘拟合计算，得到四条直线作为初始值，计算四条直线的极坐标方程，并对其极角按大小进行逆时针排序，进行铲斗靶标、斗杆靶标和动臂靶标内的鞍点检测和亚像素细化；最后得到铲斗靶标、斗杆靶标和动臂靶标上多个鞍点的图像坐标(u_i，v_i)，其中i为整数。

进一步的，骤3中，若有靶标图案中的某些鞍点未检测到，根据鞍点之间的几何关系恢复检测过程中的丢失鞍点。

与现有技术相比，本发明至少具有以下有益的技术效果：

一、本发明提出了一种通过非接触式被动视觉单目相机来识别挖掘机的工作装置信息的测量系统，将安装在三脚架上的单目相机放置在能够保证相机视野完全覆盖挖掘机工作装置的作业点附近，布置简单，操纵方便，没有复杂的安装步骤，并且安装不受位置条件和周边环境的制约，工作装置上贴有粘贴简单，价格低廉，由不易损坏的二维码编码图案的靶标样式作为检测点。它能够有效的避免在动作大、作业环境恶劣、容易与作业对象发生碰撞、震动幅度大的工作装置上安装测量传感器等价格高昂的精密测量件容易损坏的缺陷，从而提高监测的可靠性和稳定性，保证了挖掘机作业的安全性，同时也降低了维护成本。

此外，单目相机相对于编码器等能大幅降低测量装置的成本；且相机是一种便于布置的设备，因此容易对土方施工现场进行监控；

进一步的，靶标为3×3的黑白相间正方形，其中每个正方形具有唯一的二进制编码，边缘突出的8个黑色矩形区域增加对比度，以提高鞍点检测率。

进一步的，由于较大的CALTag在实际应用中可能会影响到挖掘机的机械臂进行一些动作，较小的CALTag可能在测量过程中容易导致测量系统失效。检测5t的挖掘机，选用靶标的大小为228×228mm²。

二、本测量方法的优点：(1)视觉测量能够提供更加丰富的信息，不仅可以得到挖掘机工作装置的参数，还可以得到挖掘机与作业介质的交互环境以及施工设备的周围环境，能够给操作人员提供更好的挖掘决策；(2)本测量方法与其他测量方法不同在于可以测量一定角度的旋转——相机可以检测到靶标，则可以进行位姿测量，不仅仅局限于挖掘机机械臂平面与相机平面平行；(3)本测量方法有些的结合了CALTag的优点即可以抵抗一定情况的靶标遮挡，即一定情况下的遮挡还可以进行位姿测量，提高检测的鲁棒性，而有些靶标如棋盘格不具备如此特点，当棋盘格某些角点受到遮挡会导致本帧图像测量失效；(4)通过实例测量分析，本测量方法能够达到厘米级别的测量精度，精度通过实验测试和对比分析，通过附图6显示测量精度，误差在厘米级的范围内，满足施工作业要求，为挖掘机机械臂位姿测量提供了一种新的选择。

附图说明

图1为靶标样式示意图；

图2为基于靶标的单目视觉挖掘机位姿测量系统整体布置图；

图3为图像坐标系、相机坐标系和世界坐标系之间的空间关系示意图；

图4为本发明挖掘机位姿测量系统流程图；

图5为视觉深度测量与激光深度测量方法对比图；

图6为本文视觉倾角测量精度图；

附图中：1-铲斗，2-铲斗靶标，3-斗杆，4-斗杆靶标，5-动臂，6-动臂靶标，7-挖掘机驾驶室，8-单目相机。

具体实施方式

为了使本发明的目的和技术方案更加清晰和便于理解。以下结合附图和实施例，对本发明进行进一步的详细说明，此处所描述的具体实施例仅用于解释本发明，并非用于限定本发明。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

参照图1，一种基于靶标的单目视觉挖掘机位姿测量系统，主要由三部分组成：靶标、和单目视觉系统，单目视觉系统包含单目相机与计算机。

一、靶标生成方式与布置

靶标的形状和生成方式：靶标(即CALTag)的形状如图1所示，为3×3的黑白相间正方形，其中每个正方形具有唯一的二进制编码，正方形的边缘具有突出的8个黑色矩形区域，这8 个黑色矩形区域用于增加对比度，以提高鞍点检测率。靶标通过计算机编码生成：首先产生二进制编码的矩阵，每个矩阵对应一个正方形区域；然后按照每个正方形区域对应的矩阵编码产生黑白相间的图像区域；最后生成相应的靶标图案。

靶标的布置：首先将靶标生成的靶标打印出来，靶标(CALTag)的尺寸需要满足测量系统的测量范围，较大的CALTag在实际应用中可能会影响到挖掘机的机械臂进行一些动作，较小的CALTag可能在测量过程中容易导致测量系统失效；然后将三个CALTag分别用双面胶平整的粘贴在挖掘机动臂5、斗杆3和铲斗1的机械臂平面上，在固定用于测量铲斗1的靶标时，靶标需要布置在铲斗与铲斗油缸的连接杆上记为铲斗靶标2，由于此处是一个四连杆机构，因此可以用来测量铲斗的位姿；其中，动臂5上粘贴的靶标记为动臂靶标6，斗杆3上粘贴的靶标记为斗杆靶标4，动臂靶标6固定在动臂5上，斗杆靶标4固定在斗杆3上，铲斗靶标2固定在铲斗连接杆9上。动臂靶标6斗杆靶标4以及铲斗靶标2固定在挖掘机的同一侧。最后保证所有靶标在单目相机8的视野范围内，保证能够实时检测到靶标。结合测量需求，对于本实验中5t的挖掘机，选用靶标的大小为228×228mm²。

单目视觉系统布置：本系统采用工业相机实时采集图像，单目相机8安装在三脚架上，三脚架放置在挖掘机贴有靶标点的一侧，根据挖掘机的施工场地三脚架的高度和角度可调，以满足相机能够实时捕捉到靶标的移动。并且单目相机8的视野能够完全覆盖挖掘机工作装置上的铲斗靶标2、斗杆靶标4和动臂靶标6。单目相机8通过USB接口与计算机相连接。计算机用于实时采集图像和计算位姿，系统整体布置图如图2所示。

二、基于靶标的单目视觉挖掘机位姿测量方法，参照图4，包括以下步骤：

步骤1、测量系统初始化

使用张正友标定法进行单目摄相机8拍摄前的相机标定，通过建立标定物上坐标已知的点与其图像点之间的对应，利用张正友标定算法获得相机的内外参数。校正镜头畸变，使用最大似然估计进行优化，得到相机的内参数矩阵K、外参数旋转矩阵R，平移向量t和径向畸变参数k1，k2，k3，切向畸变参数p1，p2。

步骤2、图像采集

当挖掘机处于工作状态时，已经布置好的单目相机8拍摄挖掘机整个工作过程中铲斗靶标 2、斗杆靶标4和动臂靶标6的数字图像，并通过USB接口将拍摄完整的数字图像传递给计算机。

步骤3、靶标检测

首先进行连通区域检测和识别标记的靶标；然后根据连通区域的面积大小和欧拉数作为基本判断准则，识别靶标内部的标记区域；接着进行四边形拟合，计算测量图像的梯度，获取区域边缘，同时对这些图像梯度进行k-means聚类(k＝4)，获取靶标的四个边缘方向；接着获取的边缘进行最小二乘拟合计算，得到四条直线作为初始值，计算四条直线的极坐标方程，并对其极角按大小进行逆时针排序，进行铲斗靶标2、斗杆靶标4和动臂靶标6内的鞍点检测和亚像素细化，其中每个靶标内部检测到的靶标鞍点至少为三个，并且这三点不能在同一条直线上；将检测到的靶标进行标记验证，即提取靶标内部区域(此时靶标内部经过二值化后已经是二值图，即01组成的编码)，将靶标内部区域图案的编码与已知的编码进行对比。需要说明的是：此处对比是将提取到的图案内部区域编码与生成此靶标内部区域的编码进行对比，换句话说将检测到图案编码与生成此团的编码进行对比验证，以确认检测到的区域是我们所贴的靶标区域，再进行CRC验证，进一步确定检测到的图案；如果有靶标图案中的某些鞍点未检测到，可以根据鞍点之间的几何关系恢复检测过程中的丢失鞍点，最后得到铲斗靶标2、斗杆靶标4和动臂靶标6上多个鞍点的图像坐标(u_i，v_i)，(u_i，v_i)为图像坐标(u,v)的每个点的坐标，其中i为整数。

步骤4、工作装置世界坐标系的位姿计算

由于挖掘机的工作装置的位姿测量是通过单目相机8拍摄真实挖掘机工作过程的工作装置的位姿变化来获取图像的，这是一个透视投影、采取鞍点、计算、优化的过程，因此只要获取实际挖掘机工作装置的工作平面与图像平面之间的映射关系，就可以通过测量到的靶标内部鞍点的图像坐标经坐标之间的变换关系转换成该点的实际坐标(世界坐标)。

这一过程主要涉及图像坐标系、相机坐标系和世界坐标系，各坐标系之间的空间关系如图 3所示。

通过计算坐标关系，各坐标系转换关系如下：

其中

x′＝X_C/Z_C，y′＝Y_C/ZC，

x″＝(1+k₁r²+k₂r⁴+k₃r⁶)x′+2p₁x′y′+p₂(r²+2x′²)，

y″＝(1+k₁r²+k₂r⁴+k₃r⁶)y′+p₁(r²+2y′²)+2p₂x′y′，

r²＝x′²+y′²，

u＝f_x·x″+cx，v＝f_y·y″+cy；

其中(u，v)为图像坐标，(X_w，Y_w，Z_w)为世界坐标，(X_C，Y_C，Z_C)为相机坐标，f_x为图像水平轴的尺度因子，f_y为图像垂直轴的尺度因子，(u₀，v₀)为主点坐标(即光轴与图像平面的交点)，R是标定板平面相对于相机坐标系的旋转矩阵，t是靶标相对于相机坐标系的平移向量， s是比例因子。

从上述方程可以得到R和t：

t^T＝[t₁ t₂ t₃]；

将用来进行标定的CALTag用来求解挖掘机机械臂铲斗(1)，斗杆(3)和动臂(5)的空间六自由度参数。因此将检测到的鞍点i的图像坐标系坐标(u_i，v_i)转换为以动臂(5)、斗杆(3)和铲斗(1)被拍摄平面的世界坐标系坐标(X_w，Y_w，Z_w)和三个转角(α，β，γ)。

由坐标变换公式计算可得：

其中α是靶标绕z轴的转角，β是靶标绕y轴的转角，γ是靶标绕x轴的转角。

故得到靶标的六自由度(α，β，γ，X_w，Y_w，Z_w)参数，此六自由度参数是以相机为原点的世界坐标系的坐标参数——该点的实际坐标。

通过在工作装置上粘贴具有明显图案的靶标作为特征点，然后对单目相机拍摄的挖掘机工作装置的图像应用鞍点检测，然后根据靶标正方形区域的面积大小和欧拉数作为基本判断准则，识别靶标内部的正方形区域，并进行四边形拟合，计算正方形区域的梯度，获取正方形区域边缘，进行检测靶标区域内的所有鞍点和亚像素细化，提取靶标内的图案编码和已有图案编码进行对比，最后得到靶标内所有鞍点坐标(即图像坐标)，进而通过本节步骤得到工作装置的位姿 (即工作装置的世界坐标系)。这一个测量的过程动态响应特性速度快，保证了实时性，且测量方法简单高效精确。也能够有效保障工作的安全性。

步骤5、实时性动态监测

重复进行图像采集—靶标检测—工作装置世界坐标系的位姿计算步骤，就可以实施的动态监测出挖掘机处于工作状态时工作装置的位姿信息。

这种测量方法能够准确的实时的动态测量到挖掘机处于工作状态时工作装置的位姿参数, 还可以动态监测挖掘机在进行一定幅度的回转作业时的工作装置的位姿参数，它在挖掘机的工作的装置上不用安装容易损坏，造价高的精密件，使挖掘机的维护和使用成本降低，挖掘机工作装置的参数测量的可靠性提高。且本发明的位姿测量系统具有精度高、实时性强、安装方便、系统结构简单、计算步骤少和动态响应强等优点。另一点在于直接以相机的固定点为参考坐标系因为相机可以固定在建筑施工场地中，这一点在于现代施工中更加广泛的采用视频监控技术，因此给实时进行监控设备运行提供了可能，提高施工的信息化，智能化程度。

下面结合具体实例说明本发明

为了验证测量系统的有效性，进行了距离相机不同距离上的21个关键点测量，测量点分布大致范围为从2m到11m。为了证明本测量方法的有效性，实验主要分两部分进行：一是测量本发明提出的单目位姿测量系统最远测距，另一个是测量本测量系统的最大可测角度。

视觉测试系统布置：首先固定好单目相机8，通过激光测距仪分别选定距离单目相机8的镜头2m-11m的位置，其中每隔0.5m设置一个测量靶标；然后将单目相机8拍摄图像传输给计算机，经过步骤3靶标检测提取鞍点坐标，然后通过步骤4节位姿计算原理计算出靶标点的位置坐标，则得到每个测量点的深度(即相机距离靶标的距离)；最后将测量点的深度与激光测距仪的测量值相比(本发明将激光测距仪设置为基准参考值，激光测距仪精度是mm级别)分析，最后计算出误差分布曲线。

便于分析系统测量精度，实验分析了测量系统深度和俯仰角精度。

深度测量：其中系统布置如上所述，但是设置靶标平面设置正对相机平面，试验中尽量保持靶标的原点与镜头的光心重合，靶标的原点即图1靶标左下角的黑白交叉鞍点。其中靶标距离相机的深度即为

俯仰角测量：其中系统布置如上所述，但是在每个测量点，分别设置靶标俯仰角范围为 0-75°，每隔15°设置测量一次。试验中尽量保持靶标的原点与镜头的光心重合；并且旋转轴设置为靶标下面的外围线。其中靶标相对于相机的俯仰角为β。如下图5和图6所示，实验证明测量系统深度平均误差为1.65cm，最大深度误差为2.33cm；系统俯仰角平均角度误差为4.19°，最大角度误差为7.91°。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (4)

1.一种基于靶标的单目视觉挖掘机位姿测量系统的挖掘机位姿测量方法，其特征在于，所述系统包括靶标、单目相机(8)和计算模块，所述靶标包括安装在挖掘机同一侧的铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)，所述铲斗靶标(2)固定在铲斗连接杆(9)上靠近铲斗(1)的一端，所述斗杆靶标(4)固定在斗杆(3)上，所述动臂靶标(6)固定在动臂(5)上，所述单目相机(8)的视野覆盖铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)，所述计算模块用于根据单目相机(8)采集到的靶标图像计算各个靶标的空间六自由度参数；所述单目相机(8)安装在三脚架上，三脚架放置在挖掘机贴有靶标点的一侧，所述三脚架的高度和角度可调；

铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)的形状相同，均为3×3的黑白相间正方形，其中每个正方形具有唯一的二进制编码；

所述黑白相间正方形的每个边缘具有8个凸出的黑色矩形区域；

所述方法包括以下步骤：

步骤1、测量系统初始化：通过建立标定物上坐标已知的点与其图像点之间的对应，利用标定算法获得相机的内外参数；校正镜头畸变，使用最大似然估计进行优化，得到相机的内参数矩阵K、外参数旋转矩阵R，平移向量t和径向畸变参数k1，k2，k3，切向畸变参数p1，p2；

步骤2、图像采集：当挖掘机处于工作状态时，利用单目相机(8)拍摄挖掘机整个工作过程中铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)的数字图像，并将拍摄到的数字图像传递给计算机；

步骤3、靶标检测：根据单目相机(8)拍摄到的图案，得到铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)上多个鞍点的图像坐标(u_i，v_i)，其中i为整数；

步骤4、工作装置世界坐标系的位姿计算：获取实际挖掘机工作装置的工作平面与图像平面之间的映射关系，通过步骤1得到的相机的内参数矩阵K、外参数旋转矩阵R，平移向量t和径向畸变参数k1，k2，k3，切向畸变参数p1，p2，以及步骤3测量到的靶标内部鞍点的图像坐标经坐标之间的变换关系转换成该点的实际坐标；

步骤5、实时性动态监测：重复步骤2至步骤4，实时的动态监测出挖掘机处于工作状态时工作装置的位姿信息；

步骤3的具体过程为：首先进行连通区域检测和识别标记的靶标；然后根据连通区域的面积大小和欧拉数作为基本判断准则，识别靶标内部的标记区域；接着进行四边形拟合，计算测量图像的梯度，获取区域边缘和靶标的四个边缘方向；接着获取的边缘进行最小二乘拟合计算，得到四条直线作为初始值，计算四条直线的极坐标方程，并对其极角按大小进行逆时针排序，进行铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)内的鞍点检测和亚像素细化；最后得到铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)上多个鞍点的图像坐标(u_i，v_i)，其中i为整数。

2.根据权利要求1所述的挖掘机位姿测量方法，其特征在于，步骤4中，坐标系之间的转换关系如下：

其中

x′＝X_C/Z_C，y′＝Y_C/ZC，

x″＝(1+k₁r²+k₂r⁴+k₃r⁶)x′+2p₁x′y′+p₂(r²+2x′²)，

y″＝(1+k₁r²+k₂r⁴+k₃r⁶)y′+p₁(r²+2y′²)+2p₂x′y′，

r²＝x′²+y′²，

u＝f_x·x″+cx，v＝f_y·y″+cy；

其中，(u，v)为图像坐标，(X_w，Y_w，Z_w)为世界坐标，(X_C，Y_C，Z_C)为相机坐标，f_x为图像水平轴的尺度因子，f_y为图像垂直轴的尺度因子，(u₀，v₀)为主点坐标(即光轴与图像平面的交点)，R是标定板平面相对于相机坐标系的旋转矩阵，t是靶标相对于相机坐标系的平移向量，s是比例因子，x′、y′、x″、y″和r均为中间变量；

从上述方程可以得到R和t：

t^T＝[t₁ t₂ t₃]；

将检测到的鞍点i的图像坐标系坐标(u_i，v_i)转换为以动臂(5)、斗杆(3)和铲斗(1)被拍摄平面的世界坐标系坐标(X_w，Y_w，Z_w)和三个转角(α，β，γ)：

其中α是靶标绕z轴的转角，β是靶标绕y轴的转角，γ是靶标绕x轴的转角；

故得到靶标的六自由度(α，β，γ，X_w，Y_w，Z_w)参数，此六自由度参数是以相机为原点的世界坐标系的坐标参数。

3.根据权利要求1所述的挖掘机位姿测量方法，其特征在于，所述骤3中，若有靶标图案中的某些鞍点未检测到，根据鞍点之间的几何关系恢复检测过程中的丢失鞍点。

4.根据权利要求1所述的挖掘机位姿测量方法，其特征在于，所述铲斗靶标(2)、斗杆靶标(4)和动臂靶标(6)的大小相同，均为228×228mm²。

Images