上海理工大学机械工程学院，上海

收稿日期：2022年4月3日；录用日期：2022年5月16日；发布日期：2022年7月12日

摘要

针对工业机器人传统示教方法示教效率低、操作难度大的问题，提出了一种演示示教算法，通过人对目标轨迹的多次演示生成形状复杂且位置精确的示教轨迹。将轨迹中运动状态发生较大变化的点定义为关键点，使用改进的多尺度曲率积算法和连续隐半马尔可夫模型获取不同示教轨迹特征一致的公共关键点。以各个公共关键点簇的中心点作为示教轨迹分割点，将形状复杂、不易拟合的轨迹分割成多条结构简单的子轨迹，利用最小二乘B样条分段拟合曲线以形成最终的示教轨迹。通过实验证明该算法具有良好的示教精度及易用性。

关键词

机械臂轨迹，演示示教，关键点提取，连续隐半马尔可夫模型

Research on Teaching Algorithm of Complex Trajectory Demonstration Based on Continuous Hidden Semi-Markov Model

Aoyang Hua

School of Mechanical Engineering, University of Shanghai for Science and Technology, Shanghai

Received: Apr. 3^rd, 2022; accepted: May 16^th, 2022; published: Jul. 12^th, 2022

ABSTRACT

Aiming at the problems of low teaching efficiency and difficult operation in the traditional teaching method of industrial robots, a demonstration teaching algorithm is proposed, which generates a teaching trajectory with complex shape and accurate position through multiple demonstrations of the target trajectory by humans. The points in the trajectory with large changes in motion state are defined as key points, and the improved multi-scale curvature product algorithm and continuous hidden semi-Markov model are used to obtain common key points with consistent characteristics in different taught trajectories. Taking the center point of each common key point cluster as the teaching trajectory segmentation point, the complex-shaped and difficult-to-fit trajectory is divided into simple-shaped sub-trajectories, and the least squares B-spline is used to segmentally fit the curve to form the final teaching trajectory. Experiments show that the algorithm has good teaching accuracy and ease of use.

Keywords:Robotic Arm Trajectory, Teaching by Demonstration, Key Point Extraction, Continuous Hidden Semi-Markov Models

Copyright © 2022 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

随着人工智能的迅猛发展，工业机器人在生产制造业中的运用越来越广泛，传统的机械臂示教方法有示教盒示、拖动示教、离线示教。传统方法示教效率低、上手难度大、专业要求高。为解决这一问题许多位学者提出演示示教的编程方式。演示示教是视觉系统对人的动作进行理解、学习从而自动生成示教轨迹的过程，具有操作简单、编程效率高等优点，有效地降低机械臂的学习使用成本。

Caiwei Song等人 [1] 基于拓扑简化技术以及非极大值抑制提出一种无监督轨迹分割办法，基于HSMM模型及滑动窗口获取公共关键点。此方法无需设置各种阈值即可提取关键点，不过基于差分法的曲率估计算法容易受到噪声干扰，使用前向后向算法算取观测值的隐状态容易引入额外的公共关键点 [2]。Qiyue Wang等人 [3] 基于VR技术研发出一款人机交互焊接系统。在VR系统中通过手柄控制器获取手的运动轨迹，工控机将示教轨迹转化为机械臂的运动轨迹并控制机械臂完成焊接任务。张宏达 [4] 开发了一款用示教笔进行轨迹示教的示教系统，操作者只需手持示教笔画出轨迹即可完成示教，极大地降低轨迹示教的难度 ，但存在示教精度不高、实用性不足的缺点。

由于示教产生的轨迹中不仅包含运动数据如位置、速度等，还承载着重要的任务信息，操作者持示教笔进行多组轨迹示教，这些连续的轨迹点既包含目标轨迹，又包含示教时产生的随机运动，如何从示教者的多次示教中有效地提取目标轨迹是本文研究的重点。

本文的主要贡献有：对多尺度曲率积算法进行改进，能有效地抑制噪声对提取关键点的影响。通过增强曲率较小的弱关键点，改善算法中弱关键点曲率下降、关键点遗漏的情况2提出转移状态的概念，使用连续隐半马尔科夫模型获取所有示教轨迹关键点的转移状态，通过计算转移状态的频率筛选出运动特征一致的公共关键点 [5]。

2. 关键点提取

为了提升轨迹拟合精度，需要利用关键点将形状复杂的示教轨迹分割成多段结构简单的轨迹，因此关键点的选取对拟合轨迹的形状及轨迹精度有至关重要的作用。传统方法中使用轨迹的位置、姿态和速度信息提取关键点，不能有效地反映轨迹形状的变化。由于曲线上一点的曲率值与弯曲程度正相关，因此本文将曲率作为特征点的筛选指标，将一定范围内曲率极大值的点定义为关键点。

为抑制噪声对提取关键点的影响，常使用高斯核函数对轨迹进行滤波处理，然而采用尺度小的高斯核函数不能有效地消除噪声。大尺度的高斯核函数虽然能抑制噪声，但会导致轨迹局部特征的消失，从而丢失真实的关键点。

张小洪 [6] 等人提出多尺度曲率积算法MSCP (Multi Scale Curvature Product)，使用多种尺度的高斯核函数对轨迹进行滤波，通过求取不同尺度下的曲率积，能有效地增强关键点处曲率，降低噪声的干扰。具体过程如下所示：

1) 将轨迹点 $P$ 写成参数 $u$ 的形式： $P\left(u\right)=\left(x\left(u\right),y\left(u\right),z\left(u\right)\right)$。

2) 使用标准差大小为 $\sigma$ 的高斯滤波对轨迹进行滤波得到滤波后的轨迹点 $P\left(u,\sigma \right)$。

3) 根据差分公式求不同尺度下的曲率 $k\left(u,{\sigma }_j\right)$。

4) 求曲率积： $F\left(u\right)={\displaystyle {\prod }_{j=1}^{\text{n}}k\left(u,{\sigma }_j\right)}$，将曲率积大于阈值的定义为关键点。

但MSCP未能解决差分法求曲率对噪声的鲁棒性问题以及对轨迹中曲率较小的弱关键点求曲率积会导致曲率下降、关键点遗漏。

为解决上述问题，对传统的多尺度曲率积算法做出两点改进。1) 采用第一类切比雪夫多项式拟合当前及邻域内的点集，使用参数方程的曲率公式代替传统的差分法求曲率。2) 对曲率较小的弱关键点求不同尺度的曲率和以增强曲率较小处的关键点。实验结果证明本算法的曲率对波动较大的噪声具有一定的鲁棒性，对曲率较小的弱关键点也有较好的提取效果。

2.1. 基于切比雪夫多项式的曲率估计

图1. 切比雪夫多项式的拟合过程

为解决差分法求曲率容易受噪声影响的问题，采用切比雪夫多项式拟合当前及邻域内的点集，通过参数方程的曲率公式求得当前点的曲率。第一类切比雪夫多项式对扰动不敏感、能有效地降低龙格现象等优点 [7]，其拟合过程如图1所示。

设当前轨迹点为 $p_i=\left(x_{i,},y_i,z_i\right)$，以此点为中心，支持域为n的2n + 1个点集为： $\left\{p_{i-n},\cdots ,p_i,\cdots ,p_{i+n}\right\}$。首先需要对离散曲线进行弦长参数化。得到对应的参数为 $\left\{L_{i,-n},\cdots ,L_{i,0},\cdots ,L_{i,n}\right\}$。由于切比雪夫多项式的定义域为[−1, 1]，因此需要对参数 $L_{i,j}$ 进行处理，公式见 [6]。弦长参数化后的得到三个点集分别为：

$\{\begin{array}{c}{\left\{\left(L_{i,j},x_{i+j}\right)\right\}}_{j=-n}^n\\ {\left\{\left(L_{i,j},y_{i+j}\right)\right\}}_{j=-n}^n\\ {\left\{\left(L_{i,j},z_{i+j}\right)\right\}}_{j=-n}^n\end{array}$ (1)

切比雪夫多项式公式为：

$T_K\left(t\right)=\cos \left(k\cdot \text{arccost}\right),k\ge 0$

随着基函数个数的增加可以减少切比雪夫多项式的拟合误差，但过多的基函数会增加矩阵求逆的运算量，降低对噪声的鲁棒性，因此选取 $T_0$，$T_1$，$T_2$，$T_3$ 作为曲线拟合的基函数，拟合曲线为：

$\{\begin{array}{c}x\left(L\right)={\displaystyle {\sum }_{i=0}^3{\alpha }_i\cdot T_i\left(L\right)}\\ y\left(L\right)={\displaystyle {\sum }_{i=0}^3{\beta }_i\cdot T_i\left(L\right)}\\ z\left(L\right)={\displaystyle {\sum }_{i=0}^3{\gamma }_i\cdot T_i\left(L\right)}\end{array}$ (2)

使用最小二乘法对系数 $\alpha ,\beta ,\gamma$ 进行求解， $\alpha$ 相应的最优化问题可以由以下公式表达， $\beta ,\gamma$ 与 $\alpha$ 类似。

$\alpha =\arg {\min }_{\alpha }{\displaystyle {\sum }_{j=-n,j\ne 0}^n{\left(x\left(L_{i,j}\right)-x_{i+j}\right)}^2}$ (3)

$x\left(L_{i,0}\right)=x_i$

求得参数 $\alpha ,\beta ,\gamma$ 后，可得到相应的参数曲线 $P\left(L\right)=(x\left(L\right),y\left(L\right),z\left(L\right)$。则目标点 $P\left(L_{i,0}\right)=\left(x\left(L_{i,0}\right),x\left(L_{i,0}\right),x\left(L_{i,0}\right)\right)$ 处的曲率 $C_i$ 的绝对值计算公式为：

$C_i=\left|\frac{\Vert P\left({\stackrel{\dot{}}{L}}_{i,0}\right)\times P\left({\ddot{L}}_{i,0}\right)\Vert }{{\Vert P\left({\stackrel{\dot{}}{L}}_{i,0}\right)\Vert }^3}\right|$ (4)

分别使用差分法和切比雪夫多项式求星型轨迹的曲率，由图2可知，采用切比雪夫多项式曲线拟合求曲率具有对噪声具有更好的鲁棒性。

2.2. 弱关键点增强

MSCP算法对不同尺度下的曲率求乘积可以有效地增强关键点处的曲率，降低噪声点处的曲率，但是MSCP算法对曲率较小的弱关键点(曲率值小于1)却会产生抑制效果，从而导致关键点的缺失，因此引入多尺度曲率和的概念，在不同的尺度下，对局部曲率极大值处求和，对局部曲率非极大值处求积，流程如图3所示。

星型轨迹弱关键点增强前后曲率图及提取效果如图4所示。

图2. 差分法曲率估计与切比雪夫多项式曲率估计效果对比

图3. 弱关键点增强流程

3. 公共关键点识别

由于操作者在示教过程的抖动、不熟练等因素，单次示教往往不能精确的拟合出示教轨迹，本文采用对同一工作重复示教，通过多条轨迹产生精确的示教轨迹，但不同的示教轨迹的关键点空间位置、数量以及轨迹点的时间戳都不相同。隐马尔可夫模型(HMM)可以对时间序列建模，对复杂的轨迹也具有较高的识别精度 [8]。但HMM模型在同一状态的驻留时间呈指数级下降，这与实际情况不符。而HSMM是隐马尔可夫模型(HMM)的一种扩展形式，与HMM最大的不同是HSMM加入驻留时间的概念。因此本文采用连续隐半马尔可夫模型(CHSMM)模型对连续的轨迹点建模，提取所有轨迹中特征一致的公共关键点 [8]。

CHSMM的拓扑图如图5所示，对于一组观测变量 $O_1,O_2,\cdots ,O_m$，$S_1,\cdots ,S_N$ 是其对应概率最大的隐状态。1、2、3对应的是宏观状态，即隐状态没有发生转移。

图4. 关键点增强前后MSCP算法关键点提取效果对比

图5. CHSMM的拓扑图

3.1. CHSMM的模型初始化

CHSMM模型参数可以表示为 $\lambda =\left(\pi ,A,B,p\right)$，$\pi$ 是初始状态概率， $A=\left\{a_{ij}\right\}$ 是隐状态间的状态转移矩阵， $a_{ij}$ 指t时刻到t + 1时刻隐状态从 $S_i$ 变成 $S_j$ 的概率。 $B$ 是输出概率密度函数，用一个多元高斯分布函数表示每个隐状态的输出概率分布。 $p$ 为状态驻留概率，使用一个单元高斯分布函数描述模型在每个宏观状态驻留时间的概率分布。CHSMM模型的隐状态、观测状态的数量与关键点的个数对应。因此将隐状态及观测状态的数量设为所有演示中关键点数量的最大值 [9]。对所有的示教轨迹使中Baum-Welch算法进行训练，得到CHSMM的参数 $\lambda$。

3.2. 公共关键点识别

获得模型参数后，对于每条轨迹的观察序列 $O_1,O_2,\cdots ,O_t$ 使用Viterbi算法获取对应隐状态序列 [10]。由于关键点是运动特征发生变化的点，因此对应的隐状态也会发生改变。为验证关键点是否为公共关键点，本文提出转移状态 $T$ 的概念，以当前关键点 $O_t$ 为中心设置邻域为5的窗口 $w_t=\left[O_{t-2},O_{t-1},O_t,O_{t+1},O_{t+2}\right]$，若 $w_t$ 内的隐状态从 $S_i$ 变为 $S_j$ (如图6所示)，则定义关键点 $O_t$ 的转移状态为 $T_{i,j}$。若隐状态没有发生改变则定义转移状态为 $T_{i,i}$。

图6. 转移状态 $T_i{}_{,j}$ 的定义

根据上述定义，计算每条示教轨迹关键点的转移状态，若该状态也对应其他演示的关键点(同一转移状态出现的频率阈值为80%)，则认为此状态对应的关键点为公共关键点。

在下图中，三次演示产生的关键点的转移状态如图7所示，根据定义 $T_{2,3}$，$T_{3,4}$，对应的关键点为公共关键点。

图7. 三次演示产生的关键点转移状态

4. 轨迹生成

以各公共关键点簇的中心点作为示教轨迹的分割点，形状复杂的轨迹被分割为多条结构简单的子轨迹。由于三次B样条曲线具有灵活的造型性能，曲线连接光滑，计算简便等优点，因此使用三次准均匀B样条曲线对各子轨迹进行分段拟合，生成最终的示教轨迹 [11]。

B样条曲线的定义为：

$\{\begin{array}{l}{N}_{i,0}\left(u\right)=\{\begin{array}{l}1u_i<u<u_{i+1}\hfill \\ 0\hfill \end{array}\hfill \\ N_{i,k}\left(u\right)=\frac{u-u_i}{u_{i+k}-u_i}{N}_{i,k-1}\left(u\right)+\frac{u_{i+k+1}-u_i}{u_{i+k+1}-u_{i+1}}{N}_{i,k-1}\left(u\right)\hfill \\ \frac{0}{0}=0\hfill \\ p\left(u\right)={\displaystyle {\sum }_{i=0}^n{C}_i{N}_{i,k}\left(u\right)}\hfill \end{array}$ (5)

其中 $N_{i,k}\left(u\right)$ 表示第i + 1条k次B样条的基函数，此处k = 3， $C_i$ 为控制点。

对于每段轨迹采用三次准均匀B样条进行拟合，控制点的个数为4，分别为 $C_0$，$C_1$，$C_2$，$C_3$。为了使不同子轨迹在连接处保持连续，故取四重节点作为端点，节点矢量为[0, 0, 0, 0, 1, 1, 1, 1]。示意图如图8所示。准均匀B样条的首尾控制点 $C_0$，$C_3$ 与轨迹的首尾点重合 [12]，因此需要基于最小二乘法求得中间两个控制点的位置。

图8. 三次准均匀B样条

首先使用弦长参数化来估算每个轨迹点的参数 $\tilde{u}$，轨迹点与拟合曲线上对应点的距离误差公式为：

$S={\displaystyle \underset{i=1}{\overset{n}{\sum }}{\left[d_i-Q\left({\tilde{u}}_i\right)\right]}^2}$ (6)

其中： $d_i$ 是轨迹点的坐标 $\left(x,y,z\right)$，$u_i$ 是 $d_i$ 的参数值。

通过上文公式1~7求得首尾两控制点处的单位切向量 $t_1$ 和 $t_2$。由三次准均匀B样条的属性可知，两个内部控制点在首尾的切向量方向上，与首末控制点的距离分别为 ${\alpha }_1$，${\alpha }_2$。

控制点间的关系为：

$C_1={\alpha }_1{t}_1+C_0$ (7)

$C_2={\alpha }_2{t}_2+C_3$

因此，需要求得 ${\alpha }_1$，${\alpha }_2$ 使得离误差S最小，因此误差S对参数 ${\alpha }_1$，${\alpha }_2$ 求导，公式如下

$\begin{array}{l}\frac{\partial s}{\partial {\alpha }_1}=0\\ \frac{\partial s}{\partial {\alpha }_2}=0\end{array}$ (8)

通过上述过程确定三次准均匀B样条的其余两个控制点 $C_1$ 和 $C_2$，然后根据控制点生成示教轨迹 [12]。

5. 对比验证

为了对示教算法进行评估，搭建如图9(a)所示的实验装置。操作员操作示教笔完成半圆柱体的涂漆工艺的轨迹示教，目标轨迹如图9(b)所示。kinect深度相机及视觉系统跟踪示教笔的位置及姿态，示教完成后通过ROS系统驱动六轴机械臂画出示教轨迹以完成轨迹复现 [13]。

图10实验零件及目标轨迹本次实验产生五组有效轨迹，分别使用传统的多尺度曲率积算法以及改进后的多尺度曲率积算法求取当前轨迹的曲率积，通过设置阈值获取关键点，结果如图10所示，改进的MSCP算法提取关键点的数量和位置都更为精确。

为了获取所有演示的公共关键点，我们使用CHSMM对轨迹进行训练，隐状态数量设置为12，通过Viterbi算法计算每个关键点及其领域的隐状态和转移状态，根据转移状态的出现的频率获取公共关键点。

图9. 示教轨迹获取与目标轨迹

图10. 多尺度曲率算法改进前后的关键点提取效果对比

本文采用的CHSMM提取公共关键点与传统方法使用CHMM的提取效果对比如图11所示，不同颜色的轨迹点代表不同示教轨迹。由图11可知CHSMM提取的公共关键点更集中，无图11(b)中冗余的、离散的公共关键点，公共关键点提取效果更佳。

将公共关键点簇的中心点作为轨迹的分割点以及拟合曲线的起点与终点。利用中心点将复杂轨迹分割为形状简单的子轨迹，对各段子轨迹采用基于最小二乘法的B样条曲线拟合，得到的示教轨迹如图12(a)所示。图12(b)为使用最小二乘B样条拟合得到的轨迹。相较于传统方法中仅使用最小二乘B样条拟合整个轨迹，本文拟合轨迹产生的形变较小，与目标轨迹更具一致性。

图11. CHSMM与CHMM公共关键点提取效果对比

图12. 生成的轨迹对比

6. 结论

本文提出一种通过演示示教生成复杂轨迹的示教算法，它由关键点提取、公共关键点识别、轨迹拟合几个部分组成。相较于传统的轨迹生成办法，如最小二乘曲线拟合示教轨迹，本文利用多条示教轨迹信息，以各个公共关键点簇的中心点作为轨迹分割点，将形状复杂、不易拟合的多条轨迹分割成多条结构简单的子轨迹，使用三次B样条对子轨迹分段拟合。生成的轨迹与目标轨迹具有较高的一致性。

为了提高轨迹分割的效果，本文对MSCP算法进行改进，以切比雪夫多项式的曲率估计代替传统的差分法求曲率，增强了算法的噪声鲁棒性。使用弱角点增强算法有效解决了MSCP小曲率关键点遗漏的问题。通过CHSMM对轨迹建模，提取所有示教轨迹的公共关键点，相较于传统的HMM模型，CHSMM有效解决了HMM同一状态驻留时间呈指数级下降的问题，具有更好的建模精度，提取的公共关键点位置精度更高，冗余公共关键点数量更少。

文章引用

花傲阳. 基于连续隐半马尔可夫模型的复杂轨迹演示示教算法研究
Research on Teaching Algorithm of Complex Trajectory Demonstration Based on Continuous Hidden Semi-Markov Model[J]. 建模与仿真, 2022, 11(04): 1031-1041. https://doi.org/10.12677/MOS.2022.114095

参考文献