基于轨迹预测与改进人工势场法的机械臂动态避障规划方法①

吴芳 赵云波

(浙江工业大学信息工程学院 杭州 310023)

0 引言

近些年,机器人产业发展迅速,工业机器人广泛应用于多个产业。随着机器人技术和人工智能的蓬勃发展,人机交互和协作中的用户安全是机器人研究的重点问题[1-2],即机器人需要具备动态避障的功能。经典的路径规划算法大多是在静态场景下进行路径规划得到最优或渐进最优的路径,如A*算法[3]、快速扩展随机树(rapidly-exploring random trees,RRT)算法[4]以及蚁群算法[5]等;而对于动态场景,大多使用局部路径规划算法,如动态窗口法(dynamic window approach,DWA)算法[6]以及人工势场法[7]。

人工势场法是由Khatib[7]提出的一种虚拟力法,目标点对机器人会产生一个引力场,障碍物对机器人会产生一个斥力场,最终引力和斥力的合力方向就是机器人的运动方向。人工势场法具有实现简单、计算实时性高等优点,因此常被用于机器人的路径规划。对于人工势场法存在的局部极小值以及障碍附近不可达问题,有较多文章针对不同需求对传统的人工势场法进行了改进。文献[8]建立了新的势场函数,通过对机器人势场区域的限制和实时调整,可以消除局部极小值问题,但是限制势场区域的方案不够智能化。文献[9]考虑的是无人机系统,根据周围障碍物的相对运动状态和无人机自身性能自适应调整安全距离,同时还对斥力和引力函数进行了修改,以此调整无人机的飞行方向,让无人机更快地躲避障碍物。

机械臂的运动空间中障碍物较少,而且主要考虑的障碍物是和机器人交互的用户,因此人工势场法在该场景存在的主要问题是局部极小值问题。针对局部极小值问题,本文提出了一种改进的人工势场法,每次在计算得到机器人的运动方向后,以该方向为中心,在垂直于地面、包含该方向向量的平面上以特定的角度间隔分别逆时针和顺时针方向采样90 °范围内的运动方向,然后选取引力值和斥力值的加权和最小为最佳运动方向。

其次,由于动态场景中障碍物(本文考虑用户手臂作为动态障碍物)的运动可能会导致规划的路径发生突变,路径存在较多的尖峰点。为了处理该情况,本文在路径规划中考虑了用户手臂运动的轨迹预测,以此改善规划路径尖峰点问题。常见的轨迹预测是使用深度学习[10-12],事先使用大量的动作数据集训练网络,然后应用于离线的视频或者真实场景中,完成动作识别从而实现手臂接下来的运动轨迹预测。但是在实际场景中,用户和机器人的交互动作可能是随意的,并不是训练集中特定的动作,因此使用深度学习进行手臂的轨迹预测还需进一步的研究。

然而,对于人类来说,手臂运动是依赖于感官的[13],比如想要拿取物品或者放置物品,那么头部需要先转向物品所在位置,也就是视角朝向物品所在位置,然后手臂才会去拿取或者放置物品。因此本文提出了基于头部姿态估计的手臂运动轨迹预测,最终再结合改进的人工势场法,从而实现机械臂的动态避障规划。目前,头部姿态的检测主要通过深度学习实现[14-16],即神经网络经过数据集的训练可以得到网络模型。其中文献[16]提出的Open-Face 2.0 工具是一个较为完整、并且方便使用的集脸部轮廓检测、脸部动作单元识别、头部姿态以及视角朝向检测为一体的开源工具。因此,本文选择OpenFace 2.0 来完成头部姿态的检测。最终实验结果表明,相比于传统的人工势场法,改进的算法可以有效地在机械臂应用场景中进行动态避障,在动态环境下规划的路径具有更少的尖峰点、路径更加平滑,并且规划的路径更短。

1 问题描述

1.1 人机交互的动态场景

随着机器人技术和产业的快速发展,在不远的将来,机器人可能会在人们生活中随处可见,包括未来能够为人们提供各种家庭服务的机器人。对于家庭服务机器人来说,所处环境存在的主要动态障碍物是人类用户。

本文的实验场景是机器人和用户分别位于桌子的两侧,二者需要合作完成桌面物品的分类,其中仿真环境使用的是Gazebo 仿真器,机器人是TIAGo 模型,如图1 所示。由于机器人抓取物品技术的欠缺,展示通过物品粘贴二维码实现物品的检测识别和区分,如图2(a)所示,该二维码识别结果为26;而图2(b)的二维码识别结果为582。因此粘有不同二维码的物品可以被区分,TIAGo 机器人只处理粘有识别结果为26 的二维码的物品,然后粘有识别结果为582 的二维码的物品则由用户处理(对TIAGo机器人来说用户手臂是障碍物),当用户拿取该物品时,TIAGo 需要检测用户手臂的运动状态,并且实现动态避障。

图1 TIAGo 机器人模型

图2 二维码检测识别

1.2 传统的人工势场法

传统的人工势场法核心思想是构建虚拟势场,目标点对运动物体产生引力场,引力场函数定义如式(1);障碍物对运动物体产生斥力场,斥力场函数定义如式(2);最终引力和斥力的合力方向就是物体的运动方向。

其中,ka是引力增益;X表示运动物体当前位置,Xg表示目标点的位置,X和Xg都是三维向量;kr是斥力增益;px表示运动物体和障碍物之间的距离;p0表示障碍物的影响范围,即当物体处于障碍物的影响范围外,所受斥力为0。

在计算引力和斥力后,2 个力的合力方向就是机器人的运动方向。但是,人工势场法也在某些情况下存在不足。

(1) 当障碍物处于目标附近,机器人运动至目标附近时,机器人可能处于障碍物的影响范围内。此时,目标对机器人产生的引力可能不足以“抵抗”障碍物对机器人产生的斥力,那么机器人永远也到达不了目标,也就是目标附近存在障碍物时不可达问题,如图3 所示。

图3 目标附近存在障碍物时不可达问题示意图

(2) 当机器人、障碍物以及目标点3 者处于同一直线。若障碍物对机器人产生的斥力和目标点对机器人产生的引力等大反向时,最终的合力为0,那么机器人就无法运动;若引力稍大于斥力,那么机器人可以往前运动,但往前运动后,斥力可能稍大于引力,那么机器人就往后运动,因此会陷入局部震荡,也就是局部极小值问题,如图4 所示。

图4 合力为0 的局部极小值问题示意图

2 改进的人工势场法

2.1 基于平面采样的局部极小值问题解决方案

本节针对人工势场法存在的局部极小值问题,提出了基于平面采样的解决方案。对于本文中桌面物品分类的实验场景,再结合机械臂的运动主要是左右范围(在前后方向上的运动范围比较有限),因此机械臂的运动方向采样不是整个三维(3D)空间,而是在垂直于地面(XY平面)、包含合力向量的平面上进行采样。为了便于展示,通过YZ二维平面的位置采样进行采样机制的展示说明,如图5 所示。

图5 YZ 二维平面的位置采样示意图

首先,根据目标点对机械臂的引力和障碍物对机械臂的斥力(弧线内侧是斥力范围)得到合力,然后以该合力方向为中心,在YZ平面上分别以15°为间隔,得到逆时针和顺时针90 °范围内其他的位置采样点。对于距离障碍物较近的采样点来说,它们受到的斥力较大;而对于处于障碍物斥力范围边缘的采样点来说,它们受到的斥力较小。一般情况下,希望机器人远离障碍物,又尽快抵达目标点。从斥力、引力层面看,就是在选择斥力较小的采样方向的同时,又要让引力更小,其实就是障碍物斥力范围边缘的那些采样方向。通过代价函数表示的话,则是

其中,x表示机器人的位置;Fat(x)、Fre(x) 分别代表引力函数、斥力函数;α和β是权重,分别代表对引力和斥力的惩罚力度,在本文中以执行任务过程中避免对用户的碰撞为目标,因此对斥力的惩罚力度较大,选择α=0.3,β=0.7。根据该代价函数选择使得代价最低的采样点。

其次,在通过图像展示了平面采样机制后,通过具体的式(4)来表示采样点以及最佳采样点的选择。

为了更加清晰地说明不同方向采样的单位向量和合力向量F(fx,fy,fz) 之间的运算关系,接下来以为例。

式中,θxy表示合力方向在XY平面的夹角,θyz表示合力方向在垂直于XY平面、且包含合力向量的平面的夹角,θ5表示合力向量逆时针旋转的第一个采样点在采样平面的夹角,(fx,fy,fz) 分别表示合力向量在X、Y和Z轴上的各个分量,而分别表示合力向量逆时针旋转15 °的采样点在X、Y和Z轴上的各个分量。transCos函数用于余弦角度值的转换,转换规则为

而transTan函数用于正切角度值的转换,转换规则为

而trans函数用于合力向量逆时针和顺时针旋转的采样点角度转换,转换规则为

在计算得到各个采样点之后,根据式(9)和(10)可以选出最佳采样点αt+1,从而解决人工势场法存在的局部极小值问题。

图6 是基于平面采样解决人工势场法局部极小值问题的流程图。

图6 基于平面采样的改进人工势场法流程图

2.2 基于头部姿态估计的轨迹预测方法

本文所考虑的人机交互场景中,机器人主要面对的动态障碍物是用户,而对于本文的实验场景来说,机器人运动过程中主要考虑的是用户手臂的运动。在桌面物品分类这样一个人机协作场景中,用户的头部姿态很大程度上预示了手臂接下来的运动方向以及目标位置。因此,本文提出了人机交互场景下基于用户头部姿态进行用户手臂运动的轨迹预测方法。

其中,头部姿态的检测通过一个比较完善、方便使用并且开源的脸部行为分析工具——OpenFace2.0来实现。若要实现头部姿态估计,首先需要实现脸部特征点检测和跟踪。为了实现脸部特征点检测和跟踪,OpenFace 2.0 使用了卷积专家约束局部模型(convolutional experts constrained local model,CECLM[17]),该模型使用点分布模型(point distribution model,PDM)捕捉特征点形状的变化,使用补丁专家(patch experts)对每个特征点的局部外观的变化建模。同时,OpenFace 2.0 为了保证检测的实时性,进行了深度模型和稀疏响应图计算的优化。

由于CE-CLM 内部使用了脸部特征点的3D 表示,并且根据正交投影将脸部特征点的3D 表示投影到图像上,因此当脸部特征点检测后,OpenFace 2.0 就可以实现头部姿态估计。图7 和图8 分别是头部朝向左边和右边的头部姿态估计展示图。头部姿态估计结果不仅仅在图像上有所展示,而且会得到一个六维的头部姿态向量P(Tx,Ty,Tz,Rx,Ry,Rz)。其中Tx、Ty、Tz分别表示头部在X、Y、Z3 个轴上的偏移量,而Rx、Ry、Rz分别表示头部在X、Y、Z3 个轴上旋转的旋转量。

图7 头部朝向左边的头部姿态估计展示图

图8 头部朝向右边的头部姿态估计展示图

本文的实验场景主要涉及物品在左、右方向上的拿取和摆放,而且机械臂的运动在前后范围比较有限,因此只需要考虑头部姿态在Z轴上的转动,即头部的左右转动,以此来预测用户手臂在左右方向的轨迹。图9 是基于头部姿态估计的轨迹预测流程图。

图9 基于头部姿态估计的轨迹预测流程图

图10 所示为基于平面位置采样的改进人工势场法采样示意图。实线圆圈表示真实障碍物的实际斥力范围,虚线圆圈表示预测的障碍物斥力范围。黑实线箭头表示初始合力方向,黑虚线箭头表示以初始合力方向为中心,左右各90 °范围、以15 °为间隔的采样方向,浅色虚线箭头表示最佳采样方向的候选采样方向(浅色虚线箭头只是为了展示采样机制而已,实际上不存在候选采样方向,最佳采样方向直接根据式(3)计算得到)。图10(a)是不进行轨迹预测的采样示意图。从图10(a)可以看出,若不进行轨迹预测,机械臂和障碍物距离较近时才会触发采样机制,而且此时障碍物的斥力范围(虚线空心圆圈)比较大,所以机械臂计算得到的最佳采样点不够平滑。图10(b)是进行轨迹预测但不缩放预测的障碍物斥力范围的采样示意图,图10(c)是进行轨迹预测且缩小预测的障碍物斥力范围的采样示意图。从图10(b)和(c)可以看出,此时即使机械臂和障碍物真实距离比较远,但是机械臂和预测的障碍物位置比较近,也会触发采样机制。

图10 基于平面位置采样的改进人工势场法采样示意图

图10(b)和(c)的区别在于是否对预测的障碍物斥力范围进行缩放。如图10(b)所示,若对预测的斥力范围不进行缩放,即使对障碍物位置进行了预测,仅仅让机械臂提早偏离障碍物,机械臂仍会以较大的转折幅度进行运动,导致最终的路径出现拐点。如图10(c)所示,缩小预测的障碍物斥力范围则能够让机器人提早以较为平缓的幅度偏离避开障碍物。经过调试,采用3/4 作为对预测的障碍物斥力范围进行缩放较为合适,即若当前时刻是t时刻,设定预测的t+1 时刻障碍物的斥力范围是初始斥力范围的3/4,而预测的t+2 时刻障碍物的斥力范围是初始斥力范围的(3/4)2。最终,基于轨迹预测和改进人工势场法的机械臂动态避障方案的总流程如图11 所示。

图11 基于轨迹预测和改进人工势场法的机械臂动态避障方案的总流程图

3 实验及结果

为了验证本文提出算法的可行性和有效性,本节对传统人工势场法、改进人工势场法和结合轨迹预测的改进人工势场法进行了对比实验,并对3 种算法规划得到的路径长度、路径点数量、规划时间和尖峰点数量进行了定量分析。实验所使用的计算机是 搭载Intel i7-9750H CPU、Ubuntu16.04 操作系统的惠普笔记本电脑,仿真环境使用的是Gazebo 仿真器。

图12 所示为Gazebo 仿真器中的实验场景,TIAGo 一号代表机器人,TIAGo 二号代表与机器人交互的用户。因为仿真环境下人的运动不方便模拟,因此通过TIAGo 机器人模拟人类用户。贴有二维码的物品1 表示物品分类任务中机器人需要处理的物品,而贴有二维码的物品2 表示物品分类任务中用户需要处理的物品。

图12 人机交互动态场景的Gazebo 环境示意图

传统人工势场法、改进人工势场法以及结合轨迹预测的改进人工势场法在三维空间下的路径如图13所示。可以明显看出,传统人工势场法由于没有采样机制和轨迹预测,机器人的运动路径存在一定的振荡,规划得到的路径存在较多的尖峰点。单纯改进人工势场法虽然增加了采样机制,可以避免发生振荡,但是路径还是存在着突变的尖峰点。而结合轨迹预测的改进人工势场法规划的路径不仅不存在振荡,而且更加平滑。三种算法规划的路径长度、路径点数量、尖峰点数量以及规划时间如表1 所示,相比传统人工势场法,结合轨迹预测的改进人工势场法所规划的路径长度缩短约18%,路径点数量减少约23%,尖峰点数量减少100%,规划时间缩短约27%。

表1 实验数据对比

图13 三维空间运动路径曲线图

为了更加详细地说明本算法的设计方案,对实验结果进行了2.1 节描述的采样机制分析。图14所示为结合轨迹预测的改进人工势场法的采样机制分析。五角星表示目标点,实线圆圈表示当前障碍物的斥力范围;小实心圆表示障碍物位置以及预测的障碍物位置,小实心圆越来越小的方向是障碍物的运动方向;虚线圆圈表示预测的障碍物的斥力范围;大实心圆表示机器人位置,大实心圆周围的黑色箭头指向初始合力方向;虚线箭头指向可选采样方向,灰色实线箭头指向根据采样机制得到的最佳采样方向,即机器人接下来会沿着该采样方向运动一个步长。实验数据如图14 所示,子图(a)和(c)分别是路径点数量为4 和7 的采样机制示意图,而子图(b)和(d)分别是路径点数量为5 和8 的机器人和障碍物轨迹图,即子图(b)和(d)分别对子图(a)和(c)采样机制的结果进行了验证。根据采样机制需要远离障碍物、又尽快抵达目标点的工作原理,对于远离障碍物的条件,采样机制选择那些处于障碍物斥力范围外的采样方向,即斥力最小的采样方向;对于尽快抵达目标点的条件,采样机制需要选择那些朝向目标点的采样方向,而机器人沿着朝向目标点的采样方向运动一个步长所处位置的引力小于沿着其他采样方向运动一个步长所处位置的引力,即引力最小的采样方向。因此,机器人沿着最佳采样方向运动一个步长所处位置的斥力和引力两者的加权和是最小的,即最小化式(3)所表述的代价函数值,即min(αFat(x)+βFre(x)),其中,x表示机器人的位置,Fat(x) 是引力函数,Fre(x) 是斥力函数。为了对采样机制进行分析,分别选取了机器人路径前4 个路径点和前7 个路径点所形成的轨迹,然后根据min(αFat(x)+βFre(x)) 计算得到最佳采样方向如图14(a)和(c)中的灰色实线箭头所示,并且图14(b)和(d)分别验证了机器人实际上是沿着该最佳采样方向运动的。

图14 结合轨迹预测的改进人工势场法的采样机制分析

4 结论

本文针对传统人工势场法存在的局部极小值问题,采用了基于平面位置采样的解决方案,避免机器人的运动发生振荡。同时,根据头部姿态和手臂运动的关联关系,通过OpenFace 2.0 进行头部姿态估计,进而对用户手臂运动进行轨迹预测,可以使得规划的路径更加平滑。通过实验数据发现,本文提出的基于轨迹预测和改进人工势场法的机械臂动态避障规划方法不仅可以解决传统人工势场法的局部极小值问题,还可以应用于动态环境下的机械臂动态避障场景。