一种基于图像的加油机器人视觉引导的方法

摘要

本发明公开了一种基于图像的加油机器人视觉引导的方法，包括：基于提取的汽车圆角矩形油箱盖虚拟角点特征，对小孔成像模型的分析，建立末端相机位姿变化与图像空间特征点变化之间的定量映射关系模型；提出图像雅克比矩阵的基本概念和基于模型的图像雅克比具体表示形式，构建线性控制器进行基于有相机标定参数情况下的加油任务视觉引导仿真分析；通过卡尔曼滤波算法对无标定视觉引导任务中图像雅克比矩阵的在线估计；构建基于卡尔曼滤波估计的无标定视觉引导加油定位系统。本发明在MATLAB环境下对无标定视觉引导加油定位进行了仿真分析，验证了该方法对实现加油任务的有效性。仿真结果定位速度约10秒，机器人速度未超出安全速度，空间路径接近最优。

权利要求

1.一种基于图像的加油机器人视觉引导的方法，其特征在于，包括以下步骤：

1)基于提取的汽车圆角矩形油箱盖虚拟角点特征，通过对小孔成像模型的分析，建立末端相机位姿变化与图像空间特征点变化之间的定量映射关系模型；将视觉引导系统中的机械臂关节作为理想的速度源，使用关节速度反馈来确保关节以要求的速度移动，外部的位置环使用视觉引导反馈来确定需要的关节速度以跟踪目标；

2)提出图像雅克比矩阵的基本概念和基于模型的图像雅克比具体表示形式，并构建简单的线性控制器进行基于有相机标定参数情况下的加油任务视觉引导仿真分析；基于图像中的三个不共线特征点来研究特征点组的期望运动如何决定相机位姿的运动，具体方法如下：对于图像空间中的三个点{(ui,vi),i＝1,2,3}的情况，特征点在图像空间中变化对应的速度为 因此，其雅克比矩阵写为：式中：vx、vy、vz分别表示空间中的点在x、y、z方向的速度；ωx、ωy、ωz分别表示在图像空间中x、y、z方向的速度；

基于式(1)可以从图像特征点的给定特征速度计算得出相机的位姿运动速度，而目标点的特征速度通过构造线性控制器确定如下：其中，λ满足：

式中：ef(k)＝(p*-p)——图像特征误差；Δvmax——机器人末端执行器运动速度上限；

——图像雅克比矩阵Jp的伪逆矩阵；

通过λ保证给出的机器人末端速度控制量不会超过机器人的极限运动能力；该线性控制器将在图像空间中驱动特征点p向期望的目标位置p*运动，将式(2)和(3)带入式(1)可得：该控制器将驱动相机运动，使目标特征点在图像空间中向期望的位置运动；

3)提出通过卡尔曼滤波算法实现对无标定视觉引导任务中图像雅克比矩阵的在线估计；

4)构建基于卡尔曼滤波估计的无标定视觉引导加油定位系统。

2.根据权利要求1所述的一种基于图像的加油机器人视觉引导的方法，其特征在于，步骤3)中将系统噪声和观测噪声近似为高斯白噪声所带来的统计不确定性误差，采用Sage-Husa自适应滤波构造时变噪声估计估值器，实时修正系统噪声和测量噪声的统计特性，具体如下：噪声qk,Qk,rk,Rk可以由时变的Sage-Husa自适应噪声统计估计器递推得到：qk＝(1-dk-1)qk-1+dk-1[xk-xk-1]

式中：xk表示k时刻的残差；Zk表示k时刻的协方差矩阵；Hk表示k时刻的量测矩阵； 表示xk的增广矩阵； 表示Pk的增广矩阵；

dk-1——遗忘因子；其满足：

k

式中：b表示b的k次方；

针对上述算法中的状态初始化，Pk作为状态估计误差的协方差矩阵，其初值取为P0＝

105Imn，状态估计的初值 即 可采用最小二乘法获取：在加油机器人识别到油箱盖的初始视觉引导位置，通过任意给定的n步线性无关的试探运动ΔP1,…,ΔPn观察图像空间油箱盖特征变化Δf1,…,Δfn，基于试探运动所获得样本数据即可获得初始图像雅克比矩阵的估计值：初始图像雅克比矩阵的估计值 根据相机透视成像的线性标定模型，进行估计计算得到，其中目标深度信息Z取为机器人末端待机平面与停车位置之间的距离1.2m；

通过上面两种图像雅克比初始值 估计方法，即可构造状态初始估计值 从而实现利用卡尔曼滤波算法进行图像雅克比矩阵在未标定状况下的实时估计。

3.根据权利要求2所述的一种基于图像的加油机器人视觉引导的方法，其特征在于，步骤4)中基于KF估计环节得出的图像雅克比矩阵，进行控制律计算，得出当前末端执行器的位姿变化速度，基于DX200机器人控制柜的精确末端速度控制，驱动加油机器人末端向目标加油工位运动。

说明书

一种基于图像的加油机器人视觉引导的方法

技术领域

[0001] 本发明属于工业机器人和视觉伺服领域，具体涉及一种基于图像的加油机器人视觉引导的方法。

背景技术

[0002] 随着我国经济社会的快速发展、交通基础设施的不断改善和居民生活水平的不断提高，我国的机动车保有量快速增加。加油站作为汽车油料补给场所，已逐渐成为民众生活中不可或缺的一部分。截止2018年底，全国加油站数量已超12万座。可见我国加油站数量庞大且还在不断增加，加油站市场巨大。随着社会的进步和科学技术的发展，机器人技术得到迅猛发展，机器人自动化设备逐渐替代传统的人力走向社会生产和服务的更多领域，而汽车加油服务领域的自动化、智能化升级改造需求也促使机器人进入加油服务领域，为顾客提供便捷的自动化加油服务体验。

[0003] 加油机器人则工作于加油站开放的室外环境中，加油机器人工作的室外环境状况具有多变性。加油机器人工作的加油站一般是全天不停歇营业，加油站室外存在着白天与夜晚、晴天与阴天等不同光照、阴影等变化环境影响。多变且复杂的加油站室外光照环境使得加油机器人对加油车辆目标的识别变得困难，要求加油机器人目标感知识别具有较强的鲁棒性和抗噪声能力。针对加油车辆停靠位姿随机性给自动加油运动带来的困难，需要对视觉伺服引导运动控制技术进行研究，

[0004] 当前，机器人视觉引导有多种实现方式，根据视觉引导系统的反馈信息源不同，视觉引导方式可以分为基于位置的视觉引导和基于图像的视觉引导以及基于两种基本视觉引导方式提出的混合视觉引导方式。张文增等基于位置视觉引导方法构造了Eye-in-Hand激光焊接机器人视觉引导系统，曲道奎等人根据自动化装配与分拣应用需求，构建了基于位置的Eye-in-Hand型双目结构光视觉引导系统，王孙安团队构建了结合Eye-to-Hand和Eye-in-Hand的基于位置视觉引导系统用于果蔬采摘任务。基于位置的视觉引导系统在机器人三维工作空间中进行伺服控制，容易得到空间最优运动轨迹。但目标位姿估计对相机和手眼标定误差及目标模型正确性敏感，且末端运动过程目标可能脱离相机视野导致控制失败，三维位姿误差估计耗时，实时性较差。

发明内容

[0005] 为了解决背景技术中存在的上述技术问题，本发明提供了一种基于图像的加油机器人视觉引导的方法，该方法根据相机透视模型建立了基于标定模型的图像视觉引导定位方法；其中，提出了基于自适应卡尔曼滤波的无标定视觉引导方法来提升加油机器人视觉引导加油定位的灵活性和鲁棒性，并在MATLAB环境下对无标定视觉引导加油定位进行了仿真分析，验证了所用方法对实现加油任务的有效性。

[0006] 本发明采用如下技术方案来实现的：

[0007] 一种基于图像的加油机器人视觉引导的方法，包括以下步骤：

[0008] 1)基于提取的汽车圆角矩形油箱盖虚拟角点特征，通过对小孔成像模型的分析，建立末端相机位姿变化与图像空间特征点变化之间的定量映射关系模型；

[0009] 2)提出图像雅克比矩阵的基本概念和基于模型的图像雅克比具体表示形式，并构建简单的线性控制器进行基于有相机标定参数情况下的加油任务视觉引导仿真分析；

[0010] 3)提出通过卡尔曼滤波算法实现对无标定视觉引导任务中图像雅克比矩阵的在线估计；

[0011] 4)构建基于卡尔曼滤波估计的无标定视觉引导加油定位系统。

[0012] 本发明进一步的改进在于，步骤1)中将视觉引导系统中的机械臂关节作为理想的速度源，使用关节速度反馈来确保关节以要求的速度移动，外部的位置环使用视觉引导反馈来确定需要的关节速度以跟踪目标。

[0013] 本发明进一步的改进在于，步骤2)中基于图像中的三个不共线特征点来研究特征点组的期望运动如何决定相机位姿的运动，具体方法如下：

[0014] 对于图像空间中的三个点{(ui,vi),i＝1,2,3}的情况，特征点在图像空间中变化对应的速度为 因此，其雅克比矩阵写为：

[0015]

[0016] 式中：vx、vy、vz分别表示空间中的点在x、y、z方向的速度；ωx、ωy、ωz分别表示在图像空间中x、y、z方向的速度；

[0017] 基于式(1)可以从图像特征点的给定特征速度计算得出相机的位姿运动速度，而目标点的特征速度通过构造线性控制器确定如下：

[0018]

[0019] 其中，λ满足：

[0020]

[0021] 式中：ef(k)＝(p*-p)——图像特征误差；Δvmax——机器人末端执行器运动速度上限； ——图像雅克比矩阵Jp的伪逆矩阵；

[0022] 通过λ保证给出的机器人末端速度控制量不会超过机器人的极限运动能力；该线性控制器将在图像空间中驱动特征点p向期望的目标位置p*运动，将式(2)和(3)带入式(1)可得：

[0023]

[0024] 该控制器将驱动相机运动，使目标特征点在图像空间中向期望的位置运动。

[0025] 本发明进一步的改进在于，步骤3)中将系统噪声和观测噪声近似为高斯白噪声所带来的统计不确定性误差，采用Sage-Husa自适应滤波构造时变噪声估计估值器，实时修正系统噪声和测量噪声的统计特性，具体如下：

[0026] 噪声qk,Qk,rk,Rk可以由时变的Sage-Husa自适应噪声统计估计器递推得到：

[0027] qk＝(1-dk-1)qk-1+dk-1[xk-xk-1]

[0028]

[0029]

[0030]

[0031] 式中：xk表示k时刻的残差；Zk表示k时刻的协方差矩阵；Hk表示k时刻的量测矩阵；表示xk的增广矩阵； 表示Pk的增广矩阵；

[0032] dk-1——遗忘因子；其满足：

[0033]

[0034] 式中：bk表示b的k次方；

[0035] 针对上述算法中的状态初始化，Pk作为状态估计误差的协方差矩阵，其初值取为P0＝105Imn，状态估计的初值 即 可采用最小二乘法获取：在加油机器人识别到油箱盖的初始视觉引导位置，通过任意给定的n步线性无关的试探运动ΔP1,…,ΔPn观察图像空间油箱盖特征变化Δf1,…,Δfn，基于试探运动所获得样本数据即可获得初始图像雅克比矩阵的估计值：

[0036]

[0037] 初始图像雅克比矩阵的估计值 根据相机透视成像的线性标定模型，进行估计计算得到，其中目标深度信息Z取为机器人末端待机平面与停车位置之间的距离1.2m；

[0038] 通过上面两种图像雅克比初始值 估计方法，即可构造状态初始估计值 从而实现利用卡尔曼滤波算法进行图像雅克比矩阵在未标定状况下的实时估计。

[0039] 本发明进一步的改进在于，步骤4)中基于KF估计环节得出的图像雅克比矩阵，进行控制律计算，得出当前末端执行器的位姿变化速度，基于DX200机器人控制柜的精确末端速度控制，驱动加油机器人末端向目标加油工位运动。

[0040] 本发明具有如下有益的技术效果：

[0041] 本发明提供的一种基于图像的加油机器人视觉引导的方法，加油机器人的加油运动任务可分解为以下四个步骤：目标搜寻运动阶段、视觉引导定位运动阶段、加油工位固定加油运动阶段和返回待机工位运动阶段。在第二阶段中，由于其中车型期望加油工位指针对于某一型号的车型，由于其结构完全一致，因此，只要加油机器人加油末端执行器运动到相对汽车油箱口某一相对固定位姿，就可以按照与该车型对应预先示教好的固定加油运动轨迹，通过机器人的工作参考点位姿变换调整实现一系列的加油动作。车型固定加油运动轨迹起点的末端执行器位姿即为该车型的期望加油工位。在执行这一系列相对固定加油动作过程中，加油机器人末端执行器与对应车型的加油运动相对轨迹与车辆停靠位姿无关，只与车型油箱口结构相关。因此，自动加油运动任务的难点在于针对于停靠位姿具有一定随机性的待加油车辆，加油机器人如何从待机运动平面通过视觉系统的反馈引导准确运动到待加油车辆车型的期望加油工位的位姿，从而为该车辆的标准加油动作实现提供前提条件。而本方法主要对视觉引导运动控制技术进行研究，探究适合加油机器人系统的视觉引导机器人运动控制技术，实现了加油机器人对停靠位姿具有一定随机性的待加油车辆的目标加油工位的跟踪定位。

[0042] 在使用卡尔曼滤波算法对图像雅克比矩阵元素状态向量进行估计时，由于系统的过程噪声方差Q和观测噪声方差R的统计特性难以确定，一般将其根据实际设置为符合标准高斯分布的高斯白噪声。根据经验设定的噪声统计特性如果与实际系统不符，会引起卡尔曼滤波发散。为解决这一问题，提升滤波的精度和收敛性，采用Sage-Husa自适应滤波方法构造噪声估计估值器，对系统噪声和观测噪声进行实时估计和修正。

[0043] 本发明在MATLAB环境下对无标定视觉引导加油定位进行了仿真分析，验证了所用方法对实现加油任务的有效性。仿真结果定位速度约10秒，机器人速度未超出安全速度，空间路径接近最优。

附图说明

[0044] 图1为加油机器人加油运动过程示意图。

[0045] 图2为加油机器人基于图像的视觉引导位姿变换示意图。

[0046] 图3基于自适应KF的图像视觉引导控制系统结构框图。

[0047] 图4为图像空间油箱盖虚拟角点特征运动轨迹示意图，图中圆圈代表待加油汽车圆角矩形油箱盖的四个虚拟角点在机器人末端处于视觉伺服初始位姿时的图像特征点，星形代表末端到达期望加油工位时的油箱盖虚拟角点图像特征

[0048] 图5为世界坐标系下机器人末端实际运动轨迹示意图。

[0049] 图6为视觉引导油箱盖虚拟角点图像特征误差变化示意图。

[0050] 图7为视觉引导油箱盖虚拟角点图像特征误差变化示意图。

具体实施方式

[0051] 为使本发明的目的、技术方案和特点更加清楚明白，以下结合参考附图，对本发明进一步详细说明。

[0052] 如图1至图3所示，本发明提供加油机器人运动过程中基于KF估计图像雅克比矩阵的无标定视觉引导控制的相关方法，包括以下步骤：

[0053] (1)系统初始化：加油机器人发现目标特征后，根据线性相机标定模型得到的结果进行图像雅克比矩阵初始化估计 得到初始化状态向量X0。

[0054] (2)计算观测矩阵：基于上一步控制变量V(k-1)计算观测矩阵Hk-1，

[0055] (3)更新状态向量：从第k-1步到第k步，状态向量Xk和输出向量Zk可以通过状态方程和观测方程进行更新计算。

[0056] (4)状态估计并求雅克比矩阵：基于KF算法，用k-1步的状态向量估计值 进行第k步状态向量的最小偏差估计 从而获得该步的最佳图像雅克比矩阵

[0057] (5)计算机器人末端控制量：计算当前步机器人末端执行器运动控制量V(k).[0058] (6)驱动机器人运动：机器人控制器内部速度控制环基于(5)中计算的末端执行器运动量，控制机器人末端按照预定速度向目标位置运动，当图像特征误差ef(k)＜ε时，机器人迭代运动完成，否则继续(2)-(6)步的迭代过程，直至达到允许误差结束。

[0059] 视觉引导定位仿真分析

[0060] 本发明基于上一小节中构建的加油机器人无标定视觉引导控制系统，在MATLAB仿真环境下，进行加油机器人视觉引导的仿真验证。

[0061] 首先，通过示教，使机器人末端垂直于油箱盖中心法向远离油箱盖大约30cm位置，在此位置拍摄油箱盖图像，作为期望位置的期望油箱盖图像特征，并记录当前机器人末端期望标准位姿。

[0062] 然后，设置机器人仿真的初始位姿，通过示教器移动机器人末端回到待机平面即Y＝400mm平面附近，并调整其位姿随机，保证目标油箱盖图像保持在相机视野范围内，记录初始位姿机器人末端位姿。

[0063] 通过以上仿真条件中设置的期望加油位位姿及期望图像特征及初始加油机器人位姿和初始图像特征，可以计算获得初始状态，初始圆角矩形油箱盖四边虚拟角点图像特征与期望图像特征之间的初始像素坐标误差如表1所示。

[0064] 表1视觉伺服初始状态汽车圆角矩形油箱盖虚拟角点图像特征参数表

[0065]

[0066]

[0067] 在以上初始条件下进行的加油机器人视觉引导定位仿真实验的油箱盖虚拟特征角点在图像空间中的运动轨迹如图4所示，图4与图5可以看出，在起始引导时刻，图像特征点运动路径呈现非光滑波动，随着迭代次数增加，图像特征点相对平滑收敛到目标特征位姿，这是由于起始状态向量采用标定模型和不准确深度信息进行的粗略估计，随着迭代优化，标定误差和深度不确定性引起的误差逐渐被减小，体现了卡尔曼滤波对图像雅可比矩阵在线最优估计性能。

[0068] 视觉引导过程中，最为重要的就是保证当前末端相机中的图像特征能够收敛与目标图像特征，即使当前图形特征与期望图形特征不断减小到允许误差。基于卡尔曼滤波估计的图像视觉引导过程中，汽车油箱盖的四个虚拟角点的图像特征误差变化如图6所示。从图中可以看出，随着机器人末端的运动，油箱盖图像特征误差逐渐减小，在末端运动接近10s后，圆角矩形油箱盖的四个虚拟角点与期望位姿的期望特征点误差小于2个像素，完成视觉引导任务

[0069] 从图7中可以看出，在当前设定的初始条件下，机器人末端速度在初始位姿附近速度较大，各线速度分量均小于1.2m/s，角速度分量小于0.16rad/s，通过查询安川MPX2600型机器人使用说明书可知，机器人末端的最大工作速度为2m/s，因此，视觉伺服规划的末端运动速度满足机器人的运动要求。