移动式捡球机器人避障路径视觉反馈控制方法

闫慧君，韩玉彬

（1.河南财政金融学院，河南郑州 450046；2.河南理工大学，河南焦作 454000）

1 引言

自人类进入21世纪以来，智慧地球理念频繁提出，机器人技术作为行业领先技术受到国家的重视[1]。在网球等球类的体育教学中，捡球教学环节中较为枯燥的过程，捡球机器人的出现成功将人工从该项环节中解脱出来[2]。至此，自主智能移动机器人研究成为人们一时间热议的话题之一。路径规划作为移动式捡球机器人工作中重要的环节，对其进行必要的视觉反馈控制成为机器人研究领域急需解决的问题。

文献[3]提出基于伪谱法的自由漂浮柔性空间机器人姿态运动规划与反馈控制。该方法采用假设模态法对机器人柔性进行表述，利用Lagrange方法建立动力模型，依据规划算法完成模型求解，并将结果输入控制系统中；最后根据Pontryagin原理，对参考序列的移动轨迹跟踪问题变换为两点边值问题，使用Legendre伪谱法对其进行求解，实现对机器人避障路径的控制。该方法由于未能在进行反馈控制前，利用蚁群算法对移动式捡球机器人的避障路径进行了规划，所以该方法的抗干扰能力差。

文献[4]提出基于自适应RBFNN噪声估计的自抗扰控制在姿态控制中的应用。

该方法首根据自适应径向基函数实时补偿机器人外部干扰；利用状态误差反馈控制方法跟踪机械臂姿态；最后依据控制平台实现机器人的反馈控制。该方法由于在补偿函数时存在一定问题，所以该方法的控制精度低。

文献[5]提出基于深度学习的单目视觉水下机器人目标跟踪方法研究。该方法利用深度卷积神经网络分析机器人移动轨迹，获取图像深度信息估计数据；再基于图像估计数据值对机器人运动目标进行跟踪，实现控制。该方法由于在分析图像时存在一定问题，所以该方法的抗干扰能力差。

该方法为解决上述机器人路径视觉反馈控制方法中存在的问题，提出移动式捡球机器人避障路径视觉反馈控制方法。

2 路径规划

针对移动式捡球机器人工作环境等相关因素，基于蚁群算法，对移动式捡球机器人的避障路径进行规划[6]。

2.1 算法描述

为规划捡球机器人的移动避障路径，将捡球机器人的出发位置设定为gbegin，各个球的移动节点作为食物随机在训练场中分布。设定相关约束条件对捡球机器人的移动过程进行适当约束。设定在训练场中，需捡球的位置坐标点有n个，蚂蚁会依据信息素对球的位置进行判定，获取最佳路径，然后进行移动完成任务。

2.2 寻找最佳路径

移动式捡球机器人的捡球路径问题实质上是组合的极小化问题，路径规划过程具体流程如下：

（1）蚁群初始化

首先设定蚁群数量为m，概率的随机数为Q0，训练场中的网络数量为N，挥发因子用ρ表示，计算次数为T，距离启发因子用β表示，转角因子用γ标记，信息素因子用α进行表示，该算法评价函数的转角权值为W1，距离权值为W2，训练场中各个球节点之间的距离为dij，可依据下式对其进行计算：

式中：X、Y—球位置坐标的两个方向；i、j—训练场中第i个球的位置以及第j个球的位置。

设定训练场中各个球的位置为训练节点，赋予信息素τij(0)=C至场中所有节点路径当中，且τij(0)=(n，Lnn)−1，其中相邻区域的路径长度为Lnn，训练场子区域中的网球个数为n。捡球机器人自起始点移动到最近节点，再由该节点出发移动到剩余未到的其他节点中，直至回到出发位置。

（2）构造路径

将m只蚂蚁均匀的分布在训练场节点n位置中，第k只蚂蚁的移动方向，需要依据场内各个路径的信息素值来确定。在第t时刻，蚂蚁从i点移动到j的转移概率如下式所示：

式中:—转移概率标记；τ—路径的信息量标记；η—启发函数标记，机器人移动时产生的转动角度标记成θ形式，信息素集合标记成s。依据上述获取的转移概率构造路径。

（3）信息素更新

信息素在进行更新时，需要遵守局部信息素更新规则以及全局信息素更新规则[7]。蚂蚁需要遍历训练成内所有球节点，形成搜索路径，搜索的节点编码为∗，在搜索路径上释放信息素。在所有蚂蚁都完成遍历评价最优路径。过程如下式所示：

式中：Lgh—规划后的最优路径。最后基于上述计算结果实现对移动时捡球机器人的路径规划。具体流程，如图1所示。

图1 捡球机器人路径规划流程图Fig.1 The Path Planning Flow Chart of the Ball Picking Robot

3 视觉反馈控制

基于上述规划的捡球机器人路径，对移动式捡球机器人进行避障路径的视觉反馈控制[8]。

3.1 构建动力学模型

基于上述规划的捡球机器人规划的路径，构建机器人的动力学模型。首先将移动式机器人的左、右轮速度设定成vl和vr，旋转角度用θ表示，平均速度设定成v，角速度标记成ω，并通过计算获取机器人的动力学方程，结果如下式所示：

式中：p—捡球机器人广义坐标；M(p)—惯性矩阵；p—移动状态；T—约束力矩阵标记成A(p)；G(p)—重力影响矩阵；λ—移动系数；r—维的输入矩阵标记成τ；V(p,p)—离心率矩阵。过程中，对约束力矩阵进行计算，结果如下式所示：

式中：η—参数矩阵；vs—移动速度。构建的动力学模型如下式所示：

式中：P—构建的动力学模型；S(p)—零空间矩阵；vs(t)—零空间移动速度。

计算时，需保证参数矩阵为已知数据，从而保障机器人系统能被有效控制。

3.2 控制策略

视觉伺服控制技术是利用多个摄像机对训练场地内的数据进行采集，从而实现机器人的运动控制。

该技术基于机器人的控制系统中一个或多个自由度，通过对各个球位置的追踪，实现控制。过程中需要对采集视觉的信号特征，实现机器人的期望自由度，因此需让采集信号特征达到期望值，及时完成控制任务。

基于上述分析结果，制定捡球机器人的控制策略[9]。假设捡球机器人臂端基于左右轮位置处各装载一个高清摄像机，对准前后左右各个方位的观测对象。采集的图像特征向量为s，摄像机期望视角图特征向量为s∗，依据二者，可获取图像的误差向量，计算结果，如下式所示：

式中：t—摄像机在的图像误差向量为e(t)。

由于摄像机的移动速度与捡球机器人的移动速度是一致的Vc=(vc，ωc)，将摄像机的瞬时线速度设定为vc，瞬时角速度为ωc，并由此计算图像特征向量中，各个向量之间的关系，过程如下式所示：

式中：特征向量s的交互矩阵为Ls，且Le=Ls，Vc—摄像机速度。若将Vc当做捡球机器人控制器输入值，且误差随指数规律减少，结合上式获取摄像机移动速度与误差之间的关系，结果如下式所示：

式中：λ—移动系数；Le—伪逆矩阵为Le+e。

由于捡球机器人与摄像机的移动速度一致，由上至下进行推导可将获取摄像机速度Vc看作捡球机器人的控制指令，发送至机器人控制系统中，保证s−s∗=0从而控制捡球机器人。

3.3 控制器设计

在视觉反馈控制系统中，控制器的反馈信号与输入信号获取方式都是来自于传感器采集的图像数据。视觉传感器采集的图像数据是由三维场景投影得来。高清摄像机的成像模型，如图2所示。

图2 摄像机成像模型Fig.2 Camera Imaging Model

依据图2可知，三维训练场坐标为A(Xw，Yw，Zw)，a为二维坐标中的坐标点位置，坐标值为(u，v)。首先将三维坐标系中的坐标位置转化为摄像机坐标，过程如下式所示：

式中：R—旋转矩阵，坐标系位置用(xc，yc，zc)表示，摄像机坐标位置为(xw，yw，zw)，平移矩阵用τ标记。

再将该坐标点位置映射到二维平面上，获取(x，y)，通过计算摄像机坐标位置与二维映射坐标位置之间的关系，获取图像坐标点a(u，v)，结果如下式所示：

式中：f∗—关系系数为；cu,cv—光心坐标为；pupv—摄像机焦比为。

将上述获取的图像特征向量与雅克比矩阵相结合，实现对移动式捡球机器人避障路径的视觉反馈控制器的设计[10]。过程如下式所示：

式中：Vc—视觉反馈控制器。最后基于设计的视觉反馈控制器完成对移动式捡球机器人避障路径的视觉反馈控制。

4 实验

为了验证上述视觉反馈控制方法的整体有效性，需要对此方法进行测试。

4.1 实验结果及分析

分别采用移动式捡球机器人避障路径视觉反馈控制方法（方法1）、基于伪谱法的自由漂浮柔性空间机器人姿态运动规划与反馈控制（方法2）、基于自适应RBFNN噪声估计的自抗扰控制在姿态控制中的应用（方法3）进行测试；

（1）采用方法1、方法2以及方法3对捡球机器人避障路径进行视觉反馈控制，测试三种方法在控制过程中方向、位置误差，测试结果，如图3所示。

图3 不同方法视觉反馈控制误差检测结果Fig.3 Different Methods of Visual Feedback Control Error Detection Results

分析图3可知，随着捡球机器人跟踪距离的不断增长，三种视觉反馈控制方法的控制误差均呈不同程度的上升趋势。在进行位置误差检测过程中，方法3在测试初期所检测出的误差与方法1的检测结果相同，但随着跟踪距离的增加，该方法检测出的位置误差呈急速上升趋势。方法2所检测出的误差虽然高于方法1，但是对比方法3来看，低于方法3。综上所述，方法1在进行视觉反馈控制时的控制误差小。

（2）捡球机器人在进行工作时，会不停的在训练场地内进行活动，采用方法1、方法2以及方法3进行跟踪控制时，对捡球机器人的转向角控制量进行测试，测试结果，如图4所示。

图4 视觉控制时不同方法的转向角控制量测试结果Fig.4 Test Results of Steering Angle Control Amount of Different Methods Under Visual Control

分析图4可知，在转向角的检测中，检测出的转向角控制量与期望转向角控制量越相近，说明该方法控制时的控制精度越高。方法2与方法3检测出的转向角控制量与期望转向角控制量之间存在较大偏差，方法1检测出的转向角控制量接近于期望转向角控制量，说明方法1在进行捡球机器人视觉控制时的控制精度优于其他两种方法。综上所述，方法1在进行捡球机器人反馈控制时的控制精度高。

（3）采用方法1、方法2 以及方法3 对角球机器人进行反馈控制，测试三种方法在控制时的抗干扰能力，检测结果，如图5所示。

分析图5可知，检测出的干扰值越低说明控制方法的抗干扰能力越强。方法2在测试初期检测出的干扰值与方法1相同，但是随着跟踪距离的不断增加，该方法的干扰值呈急速上升趋势。方法1检测出的干扰值低于其他两种方法，这主要是因为方法1在进行反馈控制前利用蚁群算法对移动式捡球机器人的避障路径进行了规划，所以该方法在进行跟踪控制时的抗干扰能力强。

图5 不同方法的抗干扰能力测试结果Fig.5 Anti−Interference Ability Test Results of Different Methods

5 结束语

近年来机器人制造技术飞速发展，应用范围广泛。针对传统机器人视觉反馈控制方法中存在的问题，提出基于移动式捡球机器人避障路径视觉反馈控制方法。

该方法首先利用蚁群算法对移动式捡球机器人的避障路径进行了规划；再通过构建的动力学模型，利用控制策略，设计视觉反馈控制器，最后通过该控制器完成对捡球机器人避障路径视觉反馈控制。该方法由于在构建动力学模型时存在一定问题，今后会针对该缺陷继续对该方法进行优化。