CN110532905A - 一种自装卸机构作业影像辅助操控方法 - Google Patents

Abstract

本发明公开了一种自装卸机构作业影像辅助操控方法，属于工业测量及机械自动化领域。首先在待载箱体/托盘挂点周围布设球形标识点，在钩臂架挂钩中部布置定位AprilTag标识。然后将相机固定在工程车车辆尾部中轴线偏右的位置，保证能够同时拍摄挂点和挂钩上的标识点。实时处理相机获得的图像数据，利用已知的两类标识点分别与待载物挂点和钩臂架挂钩之间的空间实际位置进行相对坐标位置换算，分别求解出挂点和挂钩之间的相对空间位置，并显示在综合显示控制模块，使用数据实时驱动三维模型的运动。最后报警模块对挂点和钩臂架之间的相对位置关系进行对比，系统进行正常或报警提示。本发明精度更高，提高了拉臂式工程车的智能化和自动化水平。

Description

一种自装卸机构作业影像辅助操控方法

技术领域

本发明属于工业测量及机械自动化领域，具体是一种自装卸机构作业影像辅助操控方法。

背景技术

拉臂式工程车辆作业环境复杂，车身庞大，司机在行驶过程中只能依靠车身两侧和车内后视镜来观察车辆四周及后方的环境，存在很大的行驶盲区；为了保证挂载作业的顺利进行，司机需要通过后视镜观察，实时调整车辆与待装载箱体/托盘之间的相对位置和角度，使双方尽量在一条直线上，保证挂装成功率，这对司机的驾驶技术和空间位置感要求较高；同时在作业过程中，操控员使用控制箱/遥控操作板对挂臂进行实时控制，以实现挂钩和挂点的准确可靠连接，仅靠通过视觉观察很难确保作业安全可靠。

提高拉臂式工程车的作业质量和作业效率，一方面可以通过加强教育培训，提高操作人员的技术水平和经验知识；另一方面则是车辆本身的安全设施和设备性能。近年来，随着机器视觉技术的快速发展和摄像机硬件性能不断的提升，视觉辅助类安全保障系统越来越受到重视，很多成果已被应用到家用汽车上，但在工程车辆上的应用还很少。

因此，迫切需要一种系统能够辅助操控员完成作业，让操控员直观的感受到车辆底盘、车载挂臂与待载箱体/托盘之间的相对位置和角度，同时有较强的环境适应性，能够适应复杂作业环境。

发明内容

针对上述问题，本发明提出一种自装卸机构作业影像辅助操控方法，以计算机视觉为基础，通过分别在待载箱体/托盘和钩臂架两个区域布设标识点，标识点与待载箱体/托盘挂点、钩臂架挂钩之间位置相对固定且已知；通过对标识点图像进行实时捕获，利用标识点特征实现图像识别，结合图像识别系统的内外参数，可解算出两处标识点(刚体)在相机坐标系下的空间六自由度信息；

一种自装卸机构作业影像辅助操控方法，具体步骤如下：

步骤一、在待载箱体/托盘挂点周围布设球形标识点，在钩臂架中部布置定位AprilTag标识。

挂点周围的标识由五个球形反光点构成，不对称地分布在挂点左右两边，用来实现对挂点的远距离识别和定位。

钩臂架上的标识点由高折射率玻璃微珠制成反光的二维码标识AprilTag标签构成，以实现对挂钩的近距离识别和定位。

步骤二、将相机固定在工程车车辆尾部中轴线偏右的位置，保证能够同时拍摄到挂点和钩臂架上的标识点，且两者之间不会存在遮挡。

使用近红外补光灯照射工作区域，在相机镜头前增加近红外滤波片。

步骤三、实时处理相机获得的图像数据，利用已知的两类标识点分别与待载物挂点和钩臂架挂钩之间的空间实际位置进行相对坐标位置换算，分别求解出挂点和挂钩之间的相对空间位置。

针对挂点标识点，将坐标原点设置在挂点中心处，构建待载箱体/托盘物体坐标系，同时获得各个标识点在该坐标系下的位置；

针对挂钩标识点，将挂钩中心点设为钩臂架物体坐标系的原点，同样获得各标识点在钩臂架体坐标系下的位置；

通过计算机视觉算法分别获得挂点标识点和AprilTag标签在相机坐标系下的空间位置；

具体过程为：

首先，通过控制近红外补光灯的闪灭，分别拍摄有补光和无补光两幅特征标识点图像，将两幅图像进行差分处理，得到有效特征标识点图像。

然后，采用计算机视觉PnP算法，计算有效特征标识点图像中各标识点与相机之间的距离，以及各标识点在相机坐标系下的空间三维坐标；

具体为：各标识点在世界坐标系中的集合为{P₁,P₂,P₃,...P_i,...P_n}；各标识点在三维空间的相对位置关系人为设定；从集合中选取3个标识点{P₁,P₂,P₃}，根据各标识点的相对位置依次标记：标识点P₁,P₂之间的距离为c；标识点P₁,P₃之间的距离为b；标识点P₂,P₃之间的距离为a。e_i是从相机中心O指向标识点P_i的向量；向量e₁和向量e₂之间的夹角为γ，向量e₁与向量e₃之间的夹角为β；向量e₂与向量e₃之间的夹角为α；

分别计算三个标识点{P₁,P₂,P₃}分别到相机中心O的距离；

计算公式如下：

假设d₂＝md₁、d₃＝nd₁，则得到：

得到三个标识点{P₁,P₂,P₃}分别到相机中心O的距离d₁,d₂,d₃；

最后，将各挂点标识点在待载箱体/托盘物体坐标系下，各挂钩标识点在钩臂架物体坐标系下的坐标，分别经过平移和旋转转换到相机坐标系下，得到各标识点在相机坐标系下的三维坐标。

其中为物体坐标系下标识点P_i的坐标，为相机坐标系下该标识点P_i的坐标；kR(α)R(β)R(γ)表示物体坐标系与相机坐标系之间的旋转矩阵；[Δx Δy Δz]^T表示物体坐标系与相机坐标系之间的平移矩阵。

钩臂架处的标识点相对相机的位姿关系采用开源AprilTag定位方法获得。

各挂点标识点与挂钩标识点在相机坐标系下的相对位置，即待载箱体/托盘物体坐标系与钩臂架物体坐标系之间的相对位置关系，进一步解算得到挂点和挂钩之间的相对位置信息。

步骤四、将挂点和挂钩之间的相对位置显示在综合显示控制模块，并使用数据实时驱动三维模型的运动，方便操作人员及时获取所需信息。

综合显示控制模块实时显示倒车影像、数值信息和三维模型。

其中倒车影像能够粗略显示车与待载物之间的安全距离信息，提供车辆车位线偏差预警。

数值信息提供精确的计算结果，包括工程车与待载物之间的位置、角度信息和挂点与挂钩之间的相对位置信息。

三维模型直观的展示待载物和挂钩之间的位置关系，根据设计图纸建立托盘、钩臂架的三维模型，采用数据驱动的形式实时显示两者的位置关系。

步骤五、报警模块对挂点和钩臂架之间的相对位置关系进行对比，当挂钩钩柄接近或进入托盘挂点时，系统进行正常提示，当挂钩偏离托盘超过警戒距离，车辆车位线偏差过大，系统进行报警提示。

本发明的优点在于：

1、一种自装卸机构作业影像辅助操控方法，采用高反光材质制成的标识点和ApriTag标识、近红外光源和增加了近红外滤波片的相机，提高了图像的识别效率，增强了本发明的环境适应性；标识点分布在两个平面，AprilTag为固定尺寸的平面，可以实现通过一次图像识别获得两者之间的位置和姿态信息。

2、一种自装卸机构作业影像辅助操控方法，用户显示界面保证了拉臂上装过程的可视化，同时从视频、数值、模型三个方面来表现挂点和挂钩之间的位姿关系，满足上装过程不同阶段对结果精度的不同需求，针对异常情况设置报警提示音进行提醒。

3、一种自装卸机构作业影像辅助操控方法，通过程序控制近红外补光灯，将相机采集到的两帧相邻图像进行差分处理，使用差分来消除强环境光产生的偏光对特征图像的影响，得到排除干扰的图像数据；未对图像数据做其他任何操作，差分后反映的是真实情况，不损失图像信息。

4、一种自装卸机构作业影像辅助操控方法，利用图像识别技术在近距离定位上精度很高合乎要求，极大的提高了拉臂式工程车的智能化和自动化水平。

附图说明

图1是本发明一种自装卸机构作业影像辅助操控方法的结构示意图；

图2是本发明一种自装卸机构作业影像辅助操控方法的相机识别模块示意图；

图3是本发明一种自装卸机构作业影像辅助操控方法的特定标识点示意图；

图4是本发明一种自装卸机构作业影像辅助操控方法的工作流程图；

图5是本发明一种自装卸机构作业影像辅助操控方法的综合显示控制示意图；

图6是本发明特征标识点在相机坐标系、世界坐标系和图像坐标系的坐标示意图；

图7是本发明PnP计算方法理论示意图；

图中1-图像识别模块；2-综合显示控制模块；3-报警模块；101-近红外补光灯；102-相机；103-特征点标识；104-位置检测模块；201-三维模型；202-高清摄像机；203-用户显示界面；204-人机接口。

具体实施方式

下面将结合附图和实例对本发明作进一步的详细说明。

本发明一种自装卸机构作业影像辅助操控方法，基于自装卸机构作业影像辅助操控系统；如图1所示，所述的影像辅助操控系统由图像识别模块1、综合显示控制模块2和报警模块3等组成。

所述图像识别模块包括近红外补光灯101、相机102、特征标识点103和位置检测模块104。

如图2所示，特征标识点103布设在工程车挂臂的挂钩和待载箱体/托盘挂点附近的固定位置。挂点附近的标识如图3a所示，由五个球形反光点构成，不对称地分布在两个平面上。挂臂上的标识点如图3b所示，由高折射率玻璃微珠制成反光的AprilTag标签构成。

相机102固定安装在工程车的尾部，保证能拍摄到挂臂且挂臂不会遮挡待载物上的标识点。同时使用近红外补光灯101照射工作区域，在相机102镜头前增加近红外滤波片，保证IR反光标识能够在无可见光或微光环境中被识别检测出，用来获得待载物和挂钩的位置和姿态信息，同时也保证在强光环境下系统能够正常工作。

图像识别模块1用来测量待载物挂点和挂钩之间的空间相对位置信息，并将计算数值结果传给综合显示控制模块2和报警模块3；

所述综合显示控制模块2实时显示控制信息和状态信息，允许用户手动在视频显示和模型显示之间进行切换，选定不同功能；可以进行数值显示和驱动三维模型；具体为显示倒车影像、数值信息和三维模型。包括：三维模型201、高清摄像机202、用户显示界面203与人机接口204。利用高清摄像机202实时传回上装过程，方便操作人员控制，用户显示界面203所显示的信息包括待载物与工程车、待载物和挂钩的实时相对位姿信息以及实时挂载画面。

报警模块3由报警装置构成，用于检测计算出来的相对位姿信息，利用图像识别模块1获取的数值，在挂钩进入挂点时提示正常，在挂钩偏离箱体/托盘时进行报警提示；当车辆车位线偏差较大时系统进行报警提示。

一种自装卸机构作业影像辅助操控方法，如图4所示，具体步骤如下：

步骤一、在待载箱体/托盘挂点周围布设球形标识点，在钩臂架中部布置定位AprilTag标识。

挂点周围的标识由五个球形反光点构成，不对称地分布在挂点左右两边，用来实现对挂点的远距离识别和定位。

钩臂架上的标识点由高折射率玻璃微珠制成反光的AprilTag标签构成，以实现对挂钩的近距离识别和定位。

采用反光材质使得通过差分图像能够尽可能排除干扰，易于标识点的提取。

采用两类标识点是综合考虑拉臂式工程车的作业过程和动作特点，在远距离时的作业重点是将工程车与待载箱体/托盘对齐，辅助驾驶员倒车操控，因此在待载箱体/托盘上布设可远距离识别定位的球形反光点；钩臂架表面积较小且在车上移动范围有限，因此采用小尺寸AprilTag标签，在实现对钩臂架可靠识别定位的同时，避免了对球形反光点识别的影响，保证了对待载箱体/托盘和钩臂架在同一场景下可靠的独立识别和实时定位。

步骤二、将相机固定在工程车车辆尾部中轴线偏右的位置，保证能够同时拍摄到挂点和钩臂架上的标识点，且两者之间不会存在遮挡。

使用近红外补光灯照射工作区域，在相机镜头前增加近红外滤波片。

步骤三、实时处理相机获得的图像数据，利用已知的两类标识点分别与待载物挂点和钩臂架挂钩之间的空间实际位置进行相对坐标位置换算，分别求解出挂点和挂钩、待载物和工程车车体之间的相对空间位置关系。

挂点标识点固定在待载箱体/托盘的设定位置，将其坐标原点设置在挂点中心处，构建待载箱体/托盘物体坐标系，同时获得各个标识点在体坐标系下的位置；

二维码标识(AprilTag)固定在钩臂架上，将挂钩中心点设为钩臂架物体坐标系的原点，同样可获得标识在钩臂架体坐标系下的位置；

通过计算机视觉算法可以分别获得标识点和AprilTag在相机坐标系下的空间位置，即两个物体坐标系之间的相对位置关系，可进一步解算得到挂点和挂钩之间的相对位置信息。

挂钩和挂点之间的位置关系是为了两者之间的可靠挂载；待载物和车辆之间的关系是为了避免车辆倒车时与待载物之间出现干涉。

具体坐标位置换算过程为：

首先，近红外补光灯配合相机捕捉图像，通过控制近红外补光灯的闪灭，拍摄得到的第一幅图像为近红外补光灯和自然光照射下的特征标识点，第二幅为仅自然光照射下的特征标识点图像，将两幅图像差分(相减)后，消除环境干扰，得到补光灯照射下的有效特征标识点图像。

然后，将差分处理后的图像数据传输给位置检测模块104，位置检测模块104读取相机102捕捉到的图像，采用计算机视觉PnP(Perspective-N-Point)算法，其中N为特征点个数。计算有效特征标识点图像中各标识点与相机之间的距离，以及各标识点在相机坐标系下的空间三维坐标；利用已知的标识点和挂点、挂钩之间的空间拓扑关系进行相对坐标位置换算，求解出挂点和挂钩的空间相对位置关系并传递给综合显示控制系统2和报警模块3。

以P3P为例，进行相对位姿解算：

各标识点在世界坐标系中的集合为{P₁,P₂,P₃,...P_i,...P_n}；各标识点在三维空间的相对位置关系人为设定且已知；如图7所示，从集合中选取3个标识点{P₁,P₂,P₃}，根据各标识点的相对位置依次标记相邻两个标识点之间的已知距离：标识点P₁,P₂之间的距离为c；标识点P₁,P₃之间的距离为b；标识点P₂,P₃之间的距离为a。e_i是从相机中心O指向标识点P_i的向量；向量e₁和向量e₂之间的夹角为γ，向量e₁与向量e₃之间的夹角为β；向量e₂与向量e₃之间的夹角为α；三个夹角α,β和γ通过标识点P_i在相平面上的成像点与相机的内参获得。

分别计算三个标识点{P₁,P₂,P₃}分别到相机中心O的距离；

计算公式如下：

假设d₂＝md₁、d₃＝nd₁，则得到：

进一步简化得到：

a₄n⁴+a₃n³+a₂n²+a₁n+a₀＝0

其中：

最后，计算三个标识点{P₁,P₂,P₃}分别到相机中心O的距离d₁,d₂,d₃的值。

可以看出：P3P可以得到四个解；为保证解的唯一性，则需要增加另一个点进行验证，即采用P4P问题，最终获得唯一解。当标识点个数更多时，算法的鲁棒性更强。因此本文采用五个特征标识点进行计算。

而挂臂处的标识点相对相机的位姿关系采用开源AprilTag定位方法获得。

由于标识点与挂点和挂钩之间的拓扑关系已知，通过坐标系换算可以得到挂点和挂钩之间的相对位置和姿态信息。

如图6所示，将各个标识点在物体坐标系下，经过平移和旋转转换到相机坐标系下，得到各标识点在相机坐标系下的三维坐标。

其中为物体坐标系下标识点P_i的坐标，为相机坐标系下该标识点P_i的坐标；kR(α)R(β)R(γ)表示物体坐标系与相机坐标系之间的旋转矩阵；[Δx Δy Δz]^T表示物体坐标系与相机坐标系之间的平移矩阵。

步骤四、将挂点和挂钩之间的相对位置和姿态信息显示在综合显示控制模块，并使用数据实时驱动三维模型的运动，并显示在用户显示界面上，方便操作人员及时获取所需信息。

如图5所示，综合显示控制模块的显示界面由视频/模型显示窗口、功能切换按钮、待载物与车体相对位姿信息和挂钩与挂点相对位姿信息构成；实时显示倒车影像、数值信息和三维模型。视频由高清摄像机202输入，模型显示通过位置检测模块104获得的数据驱动三维模型201产生；操作人员通过人机接口204完成功能切换。

其中倒车影像能够粗略显示车与待载物之间的安全距离信息，提供车辆车位线偏差预警。

数值信息提供精确的计算结果，包括工程车与待载物之间的位置、角度信息和挂点与挂钩之间的相对位置信息。

三维模型直观的展示待载物和挂钩之间的位置关系，根据设计图纸建立托盘、钩臂架的三维模型，采用数据驱动的形式实时显示两者的位置关系。

步骤五、报警模块对挂点和钩臂架之间的相对位置关系进行对比，当挂钩钩柄接近或进入托盘挂点时，系统进行正常提示，当挂钩偏离托盘超过警戒距离，车辆车位线偏差过大，系统进行报警提示。

Claims (6)

1.一种自装卸机构作业影像辅助操控方法，其特征在于，具体步骤如下：

步骤一、在待载箱体/托盘挂点周围布设球形标识点，在钩臂架中部布置定位AprilTag标识；

步骤二、将相机固定在工程车车辆尾部中轴线偏右的位置，保证能够同时拍摄到挂点和钩臂架上的标识点，且两者之间不会存在遮挡；

步骤三、实时处理相机获得的图像数据，利用已知的两类标识点分别与待载物挂点和钩臂架挂钩之间的空间实际位置进行相对坐标位置换算，分别求解出挂点和挂钩之间的相对空间位置；

具体为：

针对挂点标识点，将坐标原点设置在挂点中心处，构建待载箱体/托盘物体坐标系，同时获得各个标识点在该坐标系下的位置；

针对挂钩标识点，将挂钩中心点设为钩臂架物体坐标系的原点，同样获得各标识点在钩臂架体坐标系下的位置；

通过计算机视觉算法分别获得挂点标识点和AprilTag标签在相机坐标系下的坐标；

具体过程为：

首先，通过控制近红外补光灯的闪灭，分别拍摄有补光和无补光两幅特征标识点图像，将两幅图像进行差分处理，得到有效特征标识点图像；

然后，采用计算机视觉PnP算法，计算有效特征标识点图像中各标识点与相机之间的距离，以及各标识点在相机坐标系下的空间三维坐标；

最后，将各挂点标识点在待载箱体/托盘物体坐标系下，挂钩标识点在钩臂架物体坐标系下的坐标，分别经过平移和旋转转换到相机坐标系下，得到各标识点在相机坐标系下的三维坐标；

其中为物体坐标系下标识点P_i的坐标，为相机坐标系下该标识点P_i的坐标；kR(α)R(β)R(γ)表示物体坐标系与相机坐标系之间的旋转矩阵；[Δx Δy Δz]^T表示物体坐标系与相机坐标系之间的平移矩阵；

各挂点标识点与挂钩标识点在相机坐标系下的相对位置，即待载箱体/托盘物体坐标系与钩臂架物体坐标系之间的相对位置关系，进一步解算得到挂点和挂钩之间的相对位置信息；

步骤四、将挂点和挂钩之间的相对位置显示在综合显示控制模块，并使用数据实时驱动三维模型的运动，方便操作人员及时获取所需信息；

步骤五、报警模块对挂点和钩臂架之间的相对位置关系进行对比，当挂钩钩柄接近或进入托盘挂点时，系统进行正常提示，当挂钩偏离托盘超过警戒距离，车辆车位线偏差过大，系统进行报警提示。

2.如权利要求1所述的一种自装卸机构作业影像辅助操控方法，其特征在于，所述的步骤一中挂点周围的标识由五个球形反光点构成，不对称地分布在挂点左右两边，用来实现对挂点的远距离识别和定位；

钩臂架上的标识点由高折射率玻璃微珠制成反光的二维码标识AprilTag标签构成，以实现对挂钩的近距离识别和定位。

3.如权利要求1所述的一种自装卸机构作业影像辅助操控方法，其特征在于，所述的步骤二中使用近红外补光灯照射工作区域，在相机镜头前增加近红外滤波片。

4.如权利要求1所述的一种自装卸机构作业影像辅助操控方法，其特征在于，所述的步骤三中钩臂架处的标识点相对相机的位姿关系采用开源AprilTag定位方法获得。

5.如权利要求1所述的一种自装卸机构作业影像辅助操控方法，其特征在于，所述的步骤三中计算机视觉PnP算法具体过程为：

各标识点在世界坐标系中的集合为{P₁,P₂,P₃,...P_i,...P_n}；各标识点在三维空间的相对位置关系人为设定；从集合中选取3个标识点{P₁,P₂,P₃}，根据各标识点的相对位置依次标记：标识点P₁,P₂之间的距离为c；标识点P₁,P₃之间的距离为b；标识点P₂,P₃之间的距离为a；e_i是从相机中心O指向标识点P_i的向量；向量e₁和向量e₂之间的夹角为γ，向量e₁与向量e₃之间的夹角为β；向量e₂与向量e₃之间的夹角为α；

分别计算三个标识点{P₁,P₂,P₃}到相机中心O的距离；

计算公式如下：

假设d₂＝md₁、d₃＝nd₁，则得到：

得到三个标识点{P₁,P₂,P₃}分别到相机中心O的距离d₁,d₂,d₃。

6.如权利要求1所述的一种自装卸机构作业影像辅助操控方法，其特征在于，所述的综合显示控制模块实时显示倒车影像、数值信息和三维模型；

其中倒车影像能够粗略显示车与待载物之间的安全距离信息，提供车辆车位线偏差预警；

数值信息提供精确的计算结果，包括工程车与待载物之间的位置、角度信息和挂点与挂钩之间的相对位置信息；

三维模型直观的展示待载物和挂钩之间的位置关系，根据设计图纸建立托盘、钩臂架的三维模型，采用数据驱动的形式实时显示两者的位置关系。