基于高斯噪声发散的协作机器人路径优化及避障

胡玉蝶 ，周 勇，王宇琦 ，李卫东

(1.武汉理工大学 交通与物流工程学院，湖北 武汉 430063；2.上海理工大学 机械工程学院，上海 200093)

0 引言

在人机协作领域，示教学习是广泛使用的一种方法，它能使机器人在不同环境下从人类演示中学习、概括并为新环境再现解决方案[1-3]。高斯混合模型、高斯混合回归、隐马尔可夫模型、隐半马尔可夫模型以及动态运动基元等都是实现示教学习的主要智能技术[4-6]。

高斯混合模型对于描述和拟合生成数据具有先天优势，在一定程度上被称为概率密度的万能近似器[7]。它具有很强的编码能力和噪声处理能力，鲁棒性好，能够处理高维问题，对于连续复杂轨迹具有很强的编码再现能力[8]，且相对同类模型运算速度更快，这些特点使它成为众多研究人员开展示教学习研究的优选方案。近年来，高斯混合模型和高斯混合回归已经被一些学者研究，并成功用于人机协作的机器人学习[9-11]。然而，对于高斯混合模型和高斯混合回归应用于人机协作的路径规划目前主要存在两个问题：

(1)回归路径的平滑性问题 回归路径的质量会影响到机器人的运动，而其很大程度上取决于示教数据本身的质量。当数据中包含因人为操作而产生的不利特征时，模型会拟合这些不利特征，导致回归路径曲折，从而影响机器人的任务质量。如刘成菊等[12]利用高斯混合模型以及高斯混合回归对书写轨迹进行学习，该回归路径相较于原始的示教轨迹的平滑度有所提升，但不可避免地保留了一些细微的曲折。类似地，DAVID等[13]基于高斯混合模型以及高斯混合回归的示教学习来学习装配任务，其再现的装配轨迹存在多个多余的拐角，无法平稳地驱动机器人。研究表明，高斯混合回归具有一定的抗干扰性，但仍然难以避免学习示教数据的一些不利特征，如人为产生的抖动或者数据过于分散，从而导致回归路径出现冗余或尖锐曲折，这种不平滑的路径会对机器人运动产生冲击，同时也会增加机器人的运行时间。

(2)回归路径的避障问题 协作机器人避障路径规划一直是一大关键研究点[14-15]，其中很重要的一方面是研究高斯混合模型/回归的避障问题。如KYRARINI等[16]通过改变特定高斯簇的均值矩阵来调整高斯簇的位置以实现高斯混合模型的重构，从而引导高斯混合回归进行避障处理。LIN等[17]还对被障碍物影响的高斯簇尺寸和形状进行变换，基于高斯簇的新形态获取更灵活的避障回归路径。上述局部调整高斯簇的方法需要人为多次尝试，且新的回归路径的避障区域存在曲折。此外，对高斯簇操作不当会导致回归路径失败或发生较大变化，可能带来新的威胁。另一些研究通过整体调整高斯混合模型来实现避障，如MOK等[18]利用高斯混合模型对障碍物进行建模，并根据环境变化实时修改模型，利用高斯分布函数推导潜在向量来设计避障轨迹。然而，这种方法对障碍物的建模是二维的，路径同样会出现曲折，平滑度需要进一步提高。也有研究者通过重构高斯混合模型的数据分布来达到避障的目的，如LI等[19]通过对数据的重采样来扩充示教数据量，基于高斯混合模型/回归方法对障碍物所在的高斯簇进行特征值分解、划分平面计算以及数据点分组删除等操作来得到避障路径。但是该方法增加了数据量，从而增大了计算机的计算负荷，此外，该方法的划分平面不唯一，平面数量会随着被影响的高斯簇个数的增加而呈指数增长，且其中存在部分不合理的平面会导致避障失败。因此，应当在确保避障质量的前提下，尽可能减少人为干涉，降低计算复杂度。

针对回归路径的平滑性问题，本文提出一种基于高斯噪声发散的路径优化策略，利用高斯分布产生的随机值对示教数据进行发散，模糊示教数据中的抖动特征，从而提高路径质量。针对回归路径的避障问题，本文基于优化路径的数据点云，提出一种半贯穿式船型消隐方法，通过对被障碍物影响的发散数据进行船型消隐，形成类拱桥形结构的数据点云，引导高斯簇在障碍物的上方形成，从而使回归路径自适应避开障碍物区域。相比于现有的避障方法，该方法计算简单，结果唯一，且避障路径平滑。

1 算法原理

1.1 高斯混合模型

高斯混合模型是多个高斯分布乘以对应权重线性组合而成的模型，高斯混合模型概率密度函数以及单个高斯分布密度函数分别如式(1)和式(2)所示：

(1)

(2)

高斯混合模型的求解过程如下：

(1)根据数据初始化高斯混合模型参数(ωi,μi,Σi)；

(2)E—步。计算第j个样本pj由第i个高斯混合模型生成的后验概率γji：

(3)

3)M—步。根据E—步计算(ωi,μi,Σi)：

(4)

(5)

。

(6)

(4)重复步骤(2)、(3),直至算法收敛，得到最终参数。

1.2 高斯混合回归

高斯混合回归相比一般回归方法在高维数据上具有更好的稳健性，它是通过构建高斯混合模型，从该模型中推出每个高斯条件概率密度以及回归函数，从而构建回归模型。对于协作机器人示教数据，本文构建了一组时间序列作为输入变量，即t=[t1,t2,…,tm]T，则示教数据点可以改写为[(t1,p1),(t2,p2),…,(tm,pm)]，其中pj=[xj,yj,zj]T为输出变量。则二者的联合概率密度函数为：

(7)

其中:

(8)

mi(t)=μip+Σipt·Σitt-1·(t-μit);

(9)

covi=Σipp-Σipt·Σitt-1·Σitp。

(10)

二者的边缘概率密度以及条件概率密度分别如式(11)和式(12)所示：

(11)

(12)

其中

(13)

则高斯混合回归的表达式为：

(14)

1.3 高斯噪声发散

人工示教由于人行为的不确定性很难保证获取高质量的示教数据。一方面，很难控制示教数据之间的距离，即可能出现示教数据过于分散的情况；另一方面，在示教过程中不可避免地会出现抖动，导致示教数据存在不利的曲折特征。以上因素都有可能造成回归路径过拟合即过于曲折。这里提出一种高斯发散的方法，即在原有示教数据的基础上，使用高斯噪声使其变为彼此相容的点云。高斯噪声发散的具体公式如下：

(15)

其中:xj、yj和zj是原始示教数据中第j个点的三维坐标值；x′j、y′j和z′j是经过高斯发散处理后的点云三维坐标值；而rx，ry，rz分别是X，Y，Z维度服从高斯分布的随机噪声值,

(16)

其中：rx～N(μx-noise,σx-noise)表示X维度噪声值rx服从于均值为μx-noise、方差为σx-noise的高斯分布，ry以及rz定义与其类似，高斯分布的具体表达式如下所示：

(17)

由于高斯分布数据生成的随机性，原示教数据会随机发散，使不同示教路径数据产生交集，从而形成数据点云。基于高斯噪声发散的方法具有以下两个特点：

(1)通过调整X，Y，Z维度对应的噪声值rx,ry,rz可以控制数据的发散区域，如针对三维空间中某一维度进行发散或者针对示教数据的某个区域进行发散，而将无需发散区域所包含数据点的3个维度的噪声值设置为0，从而保证这些区域特征完全保留。

(2)针对示教数据的分散/曲折情况，可以通过调整高斯噪声参数的值来调整发散的程度。其中[μx-noise,μy-noise,μz-noise]为偏移参数，用于控制数据发散时在3个维度的偏移，当回归路径偏移到理想区域时可以控制该参数来调整位置；[σx-noise,σy-noise,σz-noise]为发散参数，用于控制数据在3个维度的发散程度，该取值越大，数据发散程度越大，当数据较为发散或者抖动较为剧烈时，可以选择较大的发散参数。

1.4 自适应避障

避障在路径规划问题中的地位举足轻重，在人机协作中，可以根据障碍物所处的位置执行避障示教，从而获取避障回归路径。但该方法并不具有自适应性，即对于固定障碍物是可行的，但是当障碍物的位置随机或不确定时，就会大大增加示教工作量，而且障碍物一旦移动，示教就需要重新进行，从而影响该方法的实时性和实用性。本文在高斯噪声发散的基础上，提出一种基于半贯穿船型点云消隐的避障策略。避障原理示意图如图1所示。

图1所示的消隐区域主要包括两个辅助圆消隐区及一个外接矩形消隐区，具体消隐算法的步骤如下：

(1)使用3D相机获取障碍物的外接矩形上边界顶点OL、OR点的位置坐标。

(2)在外接矩形左边界和右边界向下延长线上分别遍历搜索，确定位于左右边界线延长线方向上距离OL、OR的最远点，即图1中O1和O2点的位置坐标。

(3)以O1和O2为圆心，O1OL、O2OR为半径分别作左辅助圆和右辅助圆，将位于左右辅助圆以及外接矩形内所有的数据点进行消隐处理。

(4)对消隐后的数据点进行高斯混合模型重构，引导高斯混合回归生成避障回归路径。

本文提出的半贯穿船型的点云消隐方法，一方面可以避免障碍物上下方都有点云留存，导致避障失败，如图2所示。另一方面，船型的消隐方式可以在障碍物上方形成一个类拱桥形的数据分布，易于引导高斯混合模型在障碍物上方形成高斯簇，从而使回归路径避开障碍物。对于尺寸较大的障碍物，可以在执行消隐操作前，对原发散数据进行二次发散，进一步扩大点云覆盖面积，避免消隐剩余点云不足而形成高斯簇，进而导致避障失败。

2 实验分析

2.1 高斯发散

本节基于UR5机器人、迈德威视双目结构光相机、大寰AG-95电动夹爪来进行实验研究，以验证本文方法的有效性。通过人工多次拖拽UR5机器人的执行端完成从不同起点搬运物体到同一终点的示教，实验平台以及示教操作如图3所示，获取的末端示教数据如图4a所示，利用上文中高斯发散的方法，采取[0，6，0，6，0，4]的参数对示教数据进行发散处理，处理后得到的数据点云如图4b所示。

高斯簇的个数会影响到最终回归路径的质量，高斯簇过少，会导致路径丢失基本特征即欠拟合；而高斯簇个数过多，则回归路径会保留路径中多余的不利特征，导致路径曲折。为了获得合理的高斯簇个数，本文采取广泛使用的贝叶斯信息准则(Bayesian Information Criterion, BIC)[20]作为高斯簇个数的选取原则，对于原始示教数据以及发散数据的不同高斯混合模型的BIC计算结果如图5所示。

其中未发散数据的BIC最低分数对应的最佳高斯簇个数为18，经过高斯发散处理后的数据的最佳高斯簇个数为5，根据计算结果生成的回归曲线分别如图6a和图6b所示。从图中可以看出，未经发散处理计算出来的回归路径相比发散处理之后的回归路径保留了很多不利的曲折，这些不利特征在驱动机器人时，会使机器人在速度或者加速度上产生突变，从而对机器人结构造成冲击。

为了进一步量化说明，采用曲率来对两条路径进行比较，曲率越大表明弯曲越剧烈，即路径比较曲折。以X-Y平面为例，两条回归路径的曲率计算结果如图7b所示。

从图7a可以看出，经过高斯发散处理的数据基本没有存在冗余偏折的地方，整体都非常平滑，而原始示教数据的回归路径在路径起始段与结束段都存在着尖锐曲折，故本文提出的方法使示教数据在BIC计算中更容易收敛，且回归路径也更为光滑，进一步验证了该方法在改善回归质量方面的有效性。

2.2 自适应避障

对于避障而言，这里以X-Y平面避障为例进行说明。图8展示了数据点消隐策略对应的数据处理、高斯混合建模以及回归曲线生成的仿真图。由图可以看出，重构的高斯簇避开了被障碍物影响的区域，生成的避障路径整体非常平滑，不存在尖锐的曲折等缺陷。

根据1.4节仿真流程，分别以位于同一路径上半径(R)为5 mm、10 mm和15 mm的圆形障碍物进行避障仿真，避障路径与原始路径对比结果如图9所示，为了清晰显示仿真结果，这里对纵坐标进行了比例放大处理，其中虚线为避障路径，实线为原始路径，圆形为障碍物。针对不同尺寸障碍物的避障最小距离(避障轨迹距离障碍物最近距离)如表1所示。

表1 不同障碍物的避障距离 mm

当障碍物的尺寸半径达到20 mm时，障碍物边界与点云上下边界持平，如图10a所示，实施消隐后，避障区域没有足够数据支持高斯簇形成，会导致避障失败。此时，可以对障碍物所在区域的点云进行二次发散，扩大该区域点云覆盖范围，同时不会影响其他区域的数据分布，如图10b所示，再进行消隐，最后的避障轨迹如图10c所示。

为了实际测试避障效果，使用3D相机及UR5机器人进行避障实验，根据本文方法计算的三维避障路径如图11所示，利用回归路径驱动机器人的实验过程如图12所示，其中固体胶棒为障碍物，图12a为正面拍摄避障照片，图12b为侧面拍摄避障照片。实验表明，本文提出的方法可以使机器人平顺地避开障碍物。

3 结束语

本文提出一种基于高斯噪声数据预处理以及点云消隐的自适应避障方法，在不增加数据样本数量的情况下，能有效改善由高斯混合模型/回归生成的回归路径的平滑度。在此基础上，基于障碍物位置尺寸等信息对发散的数据点云进行消隐操作，并重构高斯混合模型，进而引导回归路径的自适应生成。本文方法对于示教区域大，但示教次数少，即数据较为分散的情况效果明显，在扩大示教区域的同时，能有效减少人为示教的不确定性导致的路径冗余偏折，整体提高了示教学习的质量，降低了对示教质量的要求。此外，在无需二次示教的情况下能自适应生成平滑避障路径，整体操作简单且计算量小。所提出的高斯发散方法其参数是基于手动设置，效果易受人员主观影响。后续工作将研究不同发散参数对结果的影响规律，利用深度学习或强化学习等手段自动选择最优参数组合，在减少人工干预的同时达到优化效果。