仿真环境下不规则物体的高斯过程隐式曲面核函数优化

Optimization of Gaussian Process Implicit Surface Kernel Function for Irregular Objects in Simulation Environment

摘要: 对于一个物体进行探索时，首要目标就是获取物体的形状，而当针对形状复杂的物体如不规则方块时，隐式曲面往往是最优选择。本文针对MuJoCo仿真平台下的建立的不规则物体，采用RBF (Radial Basis Function)高斯核函数和TPS (Thin Plate Spline covariance)薄板协方差核函数分别建立高斯过程，完成对物体的隐式曲面建模，通过对比，相比于最初的TPS核函数，改用调参后的RBF误差减小了93.07%。通过仿真实验结果表明，对于仿真平台中的不规则物体，高斯过程隐式曲面能够有效通过少量的采集数据完成曲面建模。

Abstract: When exploring an object, the primary objective is to acquire its shape. Implicit surfaces often emerge as the optimal choice, particularly when dealing with complex shapes such as irregular blocks. This paper focuses on the implicit surface modeling of irregular objects established within the MuJoCo simulation platform. We employ Radial Basis Function (RBF) Gaussian kernel and Thin Plate Spline Covariance (TPS) kernel to construct Gaussian processes for object modeling. Through comparison, utilizing the optimized RBF kernel results in a 93.07% reduction in error compared to the initial TPS kernel. Simulation experiment results demonstrate the effectiveness of Gaussian process implicit surfaces in modeling irregular objects within the simulation platform using minimal data collection.

1. 引言

机械臂对物体的探索与抓取是一个常见的任务，它需要对笛卡尔空间中的物体进行建模重构。在这类机器人操作中，对于一个未知的新物体，我们的目标是获得需要使用的属性，主要就是获取到物体的形状。而对于形状复杂的物体，隐式曲面模型用标量函数的零等值面来表示物体曲面，有着诸多优良特性，如形式简单，可表达任意拓扑结构的物体曲面，模型存储方便，容易判断点和曲面的位置关系，容易进行布尔操作，容易求得曲面的法向量、曲率和曲面交点等。在处理不规则物体时，由于其形状可能会因为材料、大小、重量、表面特性等因素而各异，难以通过单一的预定义抓取策略来准确地抓取，在形状探索后，机械臂可以更准确的感知和理解物体，识别物体的边缘、凸起和凹陷等特征，并根据不规则物体的实时形态做出调整，从而使其能够根据物体的当前形态来调整抓取策略，提高抓取成功率和效率。在对未知的不规则物体进行探索与抓取之前，可以先通过隐式曲面进行建模工作。

世界范围内最早提出使用触觉信息来探索识别物体的是Allen [1] ，他在文章中提出一个刚性触觉传感器在预先定义的运动中沿着物体表面跟踪，以获取和解释表面形状的3D信息，这项工作随后不停推进，Moll和Erdmann [2] 提出了一种利用三个感知手指重建未知凸起物体形状和运动的方法。采用这种方法对于一个物体表面进行数据采集并建模的前提是，物体的摩擦特性必须是先验的，它的表面必须是光滑的，没有尖锐的棱角。

2018年周少山 [3] 等人总结过传统的隐式曲面建模方法，局部方法利用各数据点以及邻域点的信息构造出局部曲面，然后将其组合成全局模型。其优点是删除、增加或修改数据点时，只需修改此数据点附近局部区域即可，同时局部方法计算较快捷、能处理大规模数据；能有效的保留曲面细节特征。而缺点则是牺牲了整体信息，最终构建的曲面还需进一步处理，如去除噪声，以保证曲面质量。移动最小二乘法就是最常用的局部隐式曲面建模方法；全局方法用函数来整体拟合点云，描述所要构造的曲面。能保持模型的整体特征，较好地处理点云噪声等缺陷，实现曲面的全局建模。但是全局方法不能对曲面进行局部修改调整，同时整体曲面运算量太大，容易造成求解困难或不稳定，因而能够处理的数据规模很有限。常见的全局方法有径向基函数隐式曲面和泊松方程法。而在与人体腔道的探索中，隐式曲面建模要求最少的接触点，没有生成点云的条件，无法使用全局方法，同时，传统局部方法无法整体构建曲面的缺点也很难满足需求，综合考虑，高斯过程隐式曲面能够通过较少的接触点构建整体曲面，在人体腔道探索建模中很有优势。

在2006年，Williams [4] 等人将高斯过程与隐式曲面结合，提出了高斯过程隐式曲面(Gaussian Process Implicit Surfaces, GPIS)的概念，并且推导了一个薄板协方差(Thin Plate Spline Covariance, TPS)核函数作为高斯过程的核函数，在文章中展现了TPS的优秀性能，随后Dragiev等人提出使用GPIS的形式来表示物体形状并以此引导抓取动作 [5] 。该论文仅利用了形状的最大后验(MAP)估计，因此没有利用高斯过程的能力来捕获表面估计中的不确定性。同一作者后来提供了一种方法 [6] ，在形状估计中选择具有特定确定性水平的模型区域，并引入了探索-抓取的概念。后来，Ottenhaus S等人 [7] 系统性的将高斯过程隐式曲面与其他隐式曲面模型进行比较，得到了GPIS在建模时对于棱角和关节处细节容易丢失的特点。

后来，Rosales C [8] 等人利用高斯过程中还未被使用过的方差作为判断GPIS中预测的不确定性的依据，结合Jaillet L [9] 等人于2017年提出的对于三维曲面探索的AtlasRRT算法，提出了一种依赖于视觉点云信息和机械臂触碰信息的GP-AtlasRRT，在三维空间中使用切空间的圆近似代表曲面形状，并完成机械臂探索的轨迹规划。总体来说，GPIS相比于倪天杰等人 [10] 结合动态图卷积网络对点云进行编码来添加先验构建隐式曲面的方法，摆脱了传统隐式曲面方法对点云的依赖。

根据已有的相关研究，本文拟采用GPIS方法对一个自己搭建的不规则物体进行探索与建模。实验的仿真器采用MuJoCo，与传统的Gazebo平台相比，MuJoCo提供了高度精确和稳定的物理仿真引擎，其仿真结果通常更加真实和可靠。同时，MuJoCo的物理引擎经过高度优化，具有较高的计算效率和性能，能够处理大规模的物理模拟。首先，基于MuJoCo仿真平台搭建了一个机械臂在关节空间的位置控制器，通过MuJoCo仿真平台获取位置信息和对应的力信息，作为本次实验的数据集完成隐式曲面的建模工作。本次实验拟采用RBF (Radial Basis Function)和TPS核函数作为高斯过程核函数，并选择TPE (Tree-Structured Parzen Estimator)方法 [11] 对于RBF核函数中的参数进行训练，获取效果最好的一组参数作为最终的RBF核函数参数。通过RBF核函数和TPS核函数效果的对比，建立更好的隐式曲面。

2. 仿真平台搭建

2.1. MuJoCo物理仿真引擎

MuJoCo (Multi-Joint Dynamics with Contact)是一种基于物理模拟的动力学引擎 [12] ，它由Roboti LLC开发，针对模拟模块进行了调优，能够最大程度的提高模拟模块的运行性能，并提供精确的动力学仿真结果提供了丰富的功能和灵活的API，被广泛应用于机器人学、控制论、仿真学、人工智能等领域。

在本次实验中，需要借助C++语言进行对多关节机械臂的控制，同时要使用python语言完成高斯过程的建立，这就需要借助MuJoCo丰富的控制接口，将机器学习与实验的控制器进行集成，同时凭借MuJoCo对多关节机器人和复杂物体更好的动力学模拟，在实验场景中加入自己定义的多关节模型和接触力模型，更好地搭建实验的仿真平台。

基于以上优势，我们选择使用MuJoCo作为本次实验的仿真平台，来进行实验数据的采集工作，图1是本次基于MuJoCo搭建出的实验场景，包含Diana_Med机械臂模型和一个自建的不规则物体模型。

Figure 1. MuJoCo experimental scenario

图1. MuJoco实验场景

2.2. 机械臂位置控制器

在本次实验的仿真部分中，为了完成在仿真中对机械臂的控制，基于MuJuCo物理引擎搭建了一个机器人底层仿真框架——ROBOPAL [13] 。MuJuCo使用了一种快速且稳定的迭代求解器来模拟物体的物理现象，从而提供精确的模拟结果。基于此物理引擎，ROBOPAL将整个框架分为三部分。首先是机器人可变控制频率的底层，其次是依据动力学模型进行的力矩/电流环设计，以及机器人模块化系统集成方法。其次是机器人中间层，实现了基于机器人的运动学模型的精准控制，包括关节空间的位置和扭矩控制，以及笛卡尔空间的位置控制。最后一层实现了笛卡尔空间的力控制，以及关节/笛卡尔空间下的轨迹规划和运动规划。基于此框架，我们可以在仿真环境中灵活组合任意机械臂、末端执行器、移动底盘和任务场景。这样，我们可以方便地继承这些底层环境或方法，并快速搭建自己的模型，整体结构如图2所示。

Figure 2. ROBOPAL control framework

图2. ROBOPAL控制框架

2.3. 力传感器数据采集

基于MuJoCo的仿真引擎和搭建好的机械臂位置控制器，我们可以精确控制机械臂的移动，并通过在末端法兰工具接口位置添加力传感器，来获取初步的力信息，如图3所示。

Figure 3. Force sensor information visualization

图3. 力传感器信息可视化

在机器人运动学中，对于末端执行器，我们通常会定义一个坐标系，一般称为法兰坐标系。这个坐标系的原点位于执行器的参考点，通常是其旋转中心或工具接口。

同时，机械臂空间坐标是基于极坐标系作为全局坐标系定义的，全局坐标系用作所有位置和方向计算的基准。通用的法兰坐标变换涉及计算从参考坐标系到法兰坐标系的转换。这通常包括平移(移动)和旋转(方向改变)两个部分，但是由于本次实验需要将每组力信息与位置信息对应，我们只需要计算平移部分，即可获取在全局坐标系中实验所需要的力传感器信息。

基于机械臂的模型文件我们可以获取机械臂的DH参数，同时基于MuJoCo引擎的强大性能实时获得机械臂所有的实时位置，以此确定每一刻的 $T_0^7$ ，通过公式(1)的计算就能得到数据集中力信息的部分：

$f_{base}=T_0^7*f_{flange}$ (1)

3. 高斯过程隐式曲面

3.1. 高斯过程

在本次研究中，对于三维空间中的曲面，我们使用隐函数方式对曲面进行建模研究。当采用隐函数方法进行曲面建模时，我们需要解决三个问题：

1) 对应曲面的隐函数F如何表示；

2) 如何找到满足方程的点集；

3) 如何保证点的误差最小。

高斯过程作为常用的数据拟合工具，可以作为F的数学表达。

概率论中的中心极限定理指出，对于有限个独立且完全相同的随机变量 $Y_1,Y_2,Y_3,\cdots ,Y_n$ ，它们的期望和方差分别为 $\mu$ 和 ${\sigma }^2$ ，如果从中抽取样本量为n的样本，当n区域无穷大时，它们的抽样分布服从均值为 $\mu$ ，方差为 ${\sigma }^2/n$ 的标准高斯分布。即在独立同分布的随机变量中抽样时，抽取的样本的分布会收敛于标准高斯分布。

基于该结论，我们在三维空间中取点时，可以将空间点视作具有联合高斯分布的随机变量，这些变量的关系满足高斯过程的约束关系，即式(2)：

$f\left(x\right)GP\left(m\left(x\right),k\left(x,x^{\prime }\right)\right)$ (2)

由原始变量的均值与方差决定，即满足公式(3)：

$\{\begin{array}{c}m\left(x\right)=E\left(x\right)\\ k\left(x,x^{\prime }\right)=E\left(f\left(x\right)-m\left(x\right)\right)\left(f^{\prime }\left(x\right)-m^{\prime }\left(x\right)\right)\end{array}$ (3)

此时，我们将空间三维位置信息添加标识符和噪声信息，将单纯的位置信息拓展成训练数据 $S_i=\left(x_i,{\sigma }_i,y_i\right)$ ，其中 $x_i$ 是三维位置信息， ${\sigma }_i$ 是数据噪声信息， $y_i$ 是标识信息，我们在这里使用符号距离函数 $f_{sdf}\left(x\right)$ 来对它做进一步约束：

$f_{sdf}\left(x\right)=\{\begin{array}{l}dx,x\in {\Omega }_{out}\hfill \\ 0,x\in \Omega \hfill \\ -dx,x\in {\Omega }_{in}\hfill \end{array}$ (4)

此时的 $d\left(x\right)$ 是一个基于距离的函数，我们可以通过每个数据点对应的力信息来获取当前位置的曲面法向量位置，并基于此法向量向腔内和腔外进行不同单位长度的延伸，以此给定不同的距离值，通过 $d\left(x\right)$ 给出对应的符号表达函数值。此时的高斯分布关系就转变成式(5)：

$y~N\left(0,k\left(x,x^{\prime }\right)+{\sigma }^{T}I\sigma \right)$ (5)

当我们采集同分布的其他数据点作为测试点集时，测试点y_\ast与训练集y也同为具有联合高斯分布的随机变量，满足式(6)：

$\left[\begin{array}{c}y\\ y_{*}\end{array}\right]\sim N\left(0,\left[\begin{array}{cc}k+{\sigma }^{T}I\sigma & k_{*}\\ k_{*}^T& k_{**}\end{array}\right]\right)$ (6)

根据高斯过程的性质，我们可以得知训练集和测试集的均值和方差存在如下关系，如公式(7)和(8)：

$y_{*}=k_{*}^T{\left[k+{\sigma }^{T}I\sigma \right]}^{-1}y$ (7)

$V\left(y_{*}\right)=k_{**}-k_{*}^T{\left[k+{\sigma }^{T}I\sigma \right]}^{-1}{k}_{*}$ (8)

此处的k取决于GP选定的核函数，满足 $k=k\left(x,x\right)$ ， $k_{\text{*}}=k\left(x,x_{\text{*}}\right)$ ， $k_{\text{**}}=k\left(x_{\text{*}},x_{\text{*}}\right)$ 。基于这些性质，我们可以直接通过训练点集内部的信息来直接获取测试点的相关信息，并以此来判断出新的空间点与物体边缘的位置关系。

3.2. 核函数选择与优化

在Williams [6] 等人第一次提出GPIS的文章中，他们提出了一种全新的核函数，即Thin Plate Splines (TPS)核函数：

$k\left(r\right)=2r^3+3R{r}^2+R^3$ (9)

其中R为数据中r的最大值，该核函数在常见的曲面表达中有如下优点：

1) 它可以用来描述光滑的曲面，并且满足无限可导；

2) TPS不需要人工调参；

3) 它具有用于变形和参数估计的封闭式解决方案；

4) 它的能量方程是可以得到物理意义上的解释的。

也就是说，TPS核函数对于绝大部分物体，都可以直接用于拟合实际形状，并给出误差较小的隐式曲面，在我们使用TPS作为核函数去拟合高斯过程时，只需要将点集按照监督学习需要的方式分类输入，就可以获得当前训练集对应的隐式曲面的数学表达，由此获得点与曲面的几何关系、曲面各位置的梯度等信息。

但是在实际使用中，TPS在部分数据集中仍存在一定误差，在针对不规则物体时，仍然需要针对采集到的数据集以均方误差等指标为标准，选取合适的核函数来完成GPIS的建立。

因此，我们选择高斯过程中经典的RBF径向基函数作为高斯过程隐式曲面建模的核函数，该核函数的数学表达式为：

$k\left(r\right)={\text{e}}^{\frac{-r^2}{2l^2}}$ (10)

同时，对于RBF中存在的超参数，我们使用TPE这一机器学习算法进行优化，针对MuJoCo仿真引擎中采集的数据，我们选择对于不规则物体进行更大数量的探索，获取验证集来对初步建模完成的模型进行验证工作。图4为TPE训练过程。

Figure 4. TPE method training process

图4. TPE方法训练过程

4. 实验结果

核函数对比效果

为了验证本文提出的基于优化RBF的高斯核函数使用在高斯过程隐式曲面中对于整个曲面建模能力的优化效果，我们在MuJoCo平台中基于标准的圆柱圆锥模型切割出不同的不规则方块来模仿不规则物体，使用传统的TPS薄板协方差和参数优化后的RBF核函数对于采集到的三维点信息及对应的三维力信息分别通过建立高斯过程进行隐式曲面建模，将结果可视化并进行对比分析。

根据传统的TPS核函数经过高斯过程隐式曲面拟合出的曲面示意图，如图5所示，采用TPS核函数对20个数据进行拟合后，使用高斯过程对整个三维空间遍历点进行预测，将预测的SDF函数值小于0.01的点标注为物体表面点，构建成一个完整的曲面，可以发现，相比于原始采样数据，大部分的曲面点都向曲面外有了一定的偏移。

Figure 5. The effectiveness of the TPS kernel function

图5. TPS方法训练结果

Figure 6. The effectiveness of the RBF kernel function

图6. RBF方法训练结果

图6则是采用传统RBF函数对20个数据进行拟合后，通过高斯过程对整个三维空间点进行遍历预测后构建得的曲面，相比于原始采样数据，大部分的曲面点都向曲面内有一点的偏移。同时，由于GPIS本身趋向于拟合闭合曲面的特性，在少量样本下拟合出的曲面都类似一个球形。

图7是采用不同核函数对于同一训练集进行曲面建模后，对于测试集数据的预测值之差，将测试集内的空间点位置输入训练好的曲面模型后，将模型输出的预测值与测试集每个点的标识值进行误差计算，可以看出在使用TPS核函数时与原曲面误差较大，而改用优化参数后的RBF作为核函数后，误差减小了93.07%，也就是说，经过训练的RBF核函数相比于TPS核函数，预测形状与原始形状的贴合程度要强很多。

5. 总结与展望

本文在机械臂探索与抓取任务中，针对不规则物体这一目标，选择高斯过程隐式曲面作为三维建模的主要方法。实验中，通过对空间三维点信息进行初步处理来获取原始数据集，随后使用力信息与三维位置信息的相关性以及高斯过程隐式曲面的方差信息来对原始数据集进行有效的数据增强。同时，针对高斯过程中核函数这一核心问题，我们选择将RBF核函数进行参数调优，与TPS核函数进行建模效果对比。

Figure 7. Comparison graph of kernel function errors

图7. 核函数误差对比图

实验结果表明，本次实验中优化了RBF核函数参数的高斯过程隐式曲面，相比于原始的TPS核的高斯过程隐式曲面具有更高效的处理能力，预测的形状也更加贴合物体形状。

然而，在实物上对于数据的采集工作仍然存在一些问题，传感器实物在数据采集中会产生相当的噪声，同时，力传感器的物理体积也决定了它无法随心所欲的安装在任何物体的末端，它的实际质量也会产生难以忽略的重力影响，这都会大大影响采集到的原始数据集的可靠程度。下一步拟将这些方法应用到实际数据采集和处理上，并降低各类噪声的影响。