CN111872934B - 一种基于隐半马尔可夫模型的机械臂控制方法及系统 - Google Patents

Abstract

本发明公开了一种基于隐半马尔可夫模型的机械臂控制方法及系统，其中方法包括：通过对获取的多组机械臂抓取过程观测数据建立对应的HMSS训练模型，再基于BIC准则从训练模型中选择最优模型；使用维特比算法对最优模型进行解码，得到隐藏的基元运动行为，建立基元运动库；计算HSMM新型前向概率，根据HSMM新型前向概率的滤波概率求得当前时刻被激活的运动基元，重新规划基元运动序列；最后基于DMP算法对每个运动基元建立基元轨迹模型，控制机械臂执行基元轨迹。本发明提高了机器的自主学习能力，同时具备稳定性和泛化性。

Description

一种基于隐半马尔可夫模型的机械臂控制方法及系统

技术领域

本发明涉及机械臂控制领域，尤其涉及一种基于隐半马尔可夫模型的机械臂控制方法及系统。

背景技术

机械臂操作技能(Robot manipulation skill)是指机器人基于自身的传感、感知、决策、规划与控制能力，通过机械臂在有限时间内操作环境中特定的物体，使物体由初始状态达到目标状态。目前机械臂操作技能大多通过人工预定义的规则实现，而人类主要是通过模仿及与环境交互，从而实现技能的获取。在多模态感知、自主决策以及运动的灵活性、自适应性上，机械臂操作技能与人类操作相比还有较大差距。而协作任务的动态性和多样性更强，这对机械臂协作技能的灵活部署能力和适应能力提出更高的要求。服务机器人在人类环境中工作往往要面对物体的空间排布不规则，例如物体可能会被放置在盒子、货架或柜子中，对机械臂技能的执行造成挑战。综上所述,机械臂操作技能的实现具有迫切的需求与重大的意义。

当前研究领域已经出现基于机器视觉的和基于强化学习两种主流的机械臂操作方法，操作效果日趋完善，但也分别存在一些不足。基于机器视觉的方法通常需要一个甚至多个摄像头，增加了计算机视觉相关方面的工作，使操作过程复杂化；基于强化学习的方法，由于机械臂自身物理属性(如对机械臂正、逆解的求解)和对计算机性能要求较高的限制，也使得该方法存在一定局限性。

发明内容

发明目的：本发明提出一种操作简单、准确性高的机械臂控制方法。本发明的另一目的在于提供基于该机械臂控制方法的系统。

技术方案：本发明所述的基于隐半马尔可夫模型的机械臂控制方法包括：

(1)获取机械臂抓取的观测数据，利用隐半马尔可夫模型HSMM对观测数据建立训练模型；

(2)利用贝叶斯信息准则BIC对所述训练模型进行优化，得到最优模型；

(3)使用维特比算法对最优模型进行解码，得到隐藏的基元运动行为，建立基元运动库；

(4)计算HSMM新型前向概率，根据HSMM新型前向概率的滤波概率求得当前时刻被激活的运动基元，重新规划基元运动序列；

(5)基于DMP算法对每个运动基元建立基元轨迹模型，控制机械臂执行基元轨迹。

进一步地，所述步骤(1)包括：

(11)获取机械臂完成指定抓取动作的N组观测数据，每组观测数据对应一个隐状态数目取值；不同组的观测数据对应的隐状态数目不同。

指定抓取动作应充分调动机械臂各个连杆和关节，被抓物体始终在在机械臂可操作空间范围内；同时，被抓物体应当具备一定刚度，保证机械爪在操作时(如旋转一定角度)仍然保持牢固。

进一步地，机械臂完成指定抓取动作的观测数据包括：位姿、速度和加速度。其中，位姿包括位移和角度。

进一步地，所述位姿包括机械臂各关节的位姿信息、机械手的位姿信息、被抓物体的位姿信息；所述速度包括机械臂各关节的速度信息、机械手的速度信息；所述速度包括机械臂各关节的加速度信息、机械手的加速度信息。

(12)建立HSMM模型，通过N组观测数据训练得到N个训练模型；所述HSMM 模型可以用六元数组[π,A,B,D,M,N]表示，分别对应初始概率向量，状态转移概率矩阵，观测概率矩阵，驻留时间矩阵，观测序列长度和隐状态个数。

较之于隐马尔可夫模型(HMM)，参数D表征每个隐状态的驻留时间，通常用高斯分布拟合。这使得模型进一步细化，充分挖掘数据的潜在特性。

所述观测数据由从机械臂的各个关节和机械手均可读取的运行轨迹记录中获取。

所述隐状态个数对应不同组观测数据设置不同，其他HSMM的参量均一样。

进一步地，所述步骤(2)包括：将满足贝叶斯信息准则 BIC＝kln(n_i)-2ln(L_i),i＝1,...,N且取值最大的训练模型作为最优模型；其中，k代表训练模型的参数个数，n_i为第i个训练模型的样本数量，L_i为第i个训练模型的似然函数。

进一步地，所述似然函数L_i如下公式：

其中，o_1:t表示1到t时刻内的观测值，S_t＝j表示t时刻系统隐状态为j，λ表示隐半马尔可夫模型。

进一步地，步骤(3)中，所述维特比算法采用如下迭代表达式：

上述公式建立了前后两个最可能的隐状态(含持续时间)的数学迭代关系。其中，δ_t(j,d)代表隐状态j且持续时间d在t时刻发生的概率最大，即参数j、d分别表示模型在t时刻最可能的隐状态和持续时间；δ_t-d(i,h)代表隐状态i且持续时间h在t-d时刻发生的概率最大，即参数i、h分别表示模型在t-d时刻最可能的隐状态和持续时间。a_(i,h)(j,d)代表在i状态驻留时间为h转移到j状态且驻留时间为d的概率，b_j,d(O_t-d+1:t)代表在j 状态下观测到t-d+1至t时刻这段观测值的概率。将模型参数代入后可得：

其中，O代表观测数据序列，Q代表该模型下所有可能的隐状态路径集合，λ代表模型参数集。PATH表示从Q中找到一条路径，且这条路径出现的概率最大，是模型解码后最优的一种隐状态路径，可以写成向量的形式。

解码后的直观效果是观测向量被分为多段，每一段是一个隐状态，对应一个基元运动行为。因此，可以将最优模型下最可能的隐状态分布路径作为运动基元序列，建立基元运动库。

进一步地，所述HSMM新型前向概率定义为：

上述公式建立了前后两个隐状态(含持续时间)的数学迭代关系；其中，α_t(j,d)代表t时刻隐状态为j且隐状态持续时间已经为d的概率，α_t-d(i,h)代表在t-d时刻，模型隐状态为i且隐状态持续时间已经为h的概率；a_(i,h)(j,d)代表在i状态驻留时间为h转移到j状态且驻留时间为d的概率，b_j,d(O_t-d+1:t)代表在j状态下观测到t-d+1至t时刻这段观测值的概率；

所述滤波概率定义为：

其中，o_1:t表示1到t时刻内的观测值，S_[t-d+1:t]＝j代表t-d+1至t这段时刻隐状态为j的概率，λ代表模型参数集，α_t-d(i,h)代表在t-d时刻，模型隐状态为i且隐状态持续时间已经为h的概率；α_(i,h)(j.d+k)代表模型在隐状态i驻留时间为h且转移到隐状态j 驻留时间为d+k的概率，b_j,d+k(O_t-d+1:t+k)代表在j状态下观测到t-d+1至t+k时刻这段观测值的概率。

进一步地，所述DMP算法为：

其中y、

分别代表系统运动位移、速度和加速度；g为系统运动目标值；f_target是系统强迫项；τ为时间缩放因子；α_y,β_y是典型系统参数，决定了学习多种不同复杂程度轨迹的能力，进而影响轨迹泛化跟踪效果。所述系统指机械臂各关节和机械手，而DMP算法即用于描述机械臂各关节和机械手的轨迹执行，该算法允许轨迹终点与目标点之间存在一定偏差，因此机械臂操作具有一定泛化能力。

本发明所述的基于隐半马尔可夫模型的机械臂控制系统包括：存储器、处理器及在所述存储器上存储并可运行的机械臂控制的程序，所述机械臂控制的程序被处理器执行时实现所述基于隐半马尔科夫模型的机械臂控制方法的部分或全部步骤。

有益效果：与现有技术相比，本发明通过隐半马尔可夫模型更准确地处理时间序列问题，提高了机器自主学习能力；同时利用上层任务规划结合下层DMP执行，在再现轨迹的同时保证了系统的稳定性和泛化性。

附图说明

图1是本发明一种基于隐半马尔可夫模型的服务机械臂抓取操作方法的总体结构图；

图2是传统隐半马尔可夫结构示意图；

图3是本发明涉及到的底层动作执行采用DMP算法的流程图；

图4是本发明一种基于隐半马尔可夫模型的服务机械臂抓取操作方法的实验环境图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

本发明提出一种基于隐半马尔可夫模型(HSMM)的服务机械臂抓取操作方法，其总体结构图如图1所示。主要包括操作演示、模型学习和抓取操作再现三部分。其中HSMM 结构图如图2所示，隐状态的驻留分布拟合为高斯分布。为了能够获取较优的模型表达，对于同一动作采用多次示教演示，每一次演示都进行隐半马尔可夫模型学习，除了模型的隐状态数不同外，其他设定均相同。在求出模型似然的情况下，运用BIC准则选出最优模型。对最优模型使用维特比算法求解序列中包含的隐状态。定义新型前向变量，该变量结合隐半马尔可夫模型滤波概率，可以求出当前时刻数据所属的隐状态，即属于何种运动行为。将该行为作为DMP算法输入，调整算法参数，采用f_target-f＜ε作为基元轨迹执行优劣的衡量指标，直至输出跟踪轨迹。其中f代表原始输入轨迹的非线性扰动，即原始系统强迫项，f_target是执行轨迹的非线性扰动，即输出系统强迫项，当两者差值小于某一阈值ε时，输出轨迹与原始轨迹近似，达到轨迹跟踪的目的。该算法流程图如图 3所示。

本发明实验环境装置设定如图4所示：桌上放置A、B两个水杯，A中含有半杯水， B中无水，两者直线距离约40cm，且不处于同一水平面。机械臂位于桌上一角，初始时位于原始位姿，且A、B两水杯均在机械臂操作空间内。

具体的，其实施方式如下：

S1、人为带动机械臂进行示教动作演示，完成的指定动作如下：

S11、转动机械臂各关节使得各连杆及机械手末端位于A杯附近，调整末端机械手位姿至恰好夹紧含有半杯水的A杯状态。

S12、继续带动机械臂至B杯附近，行进过程中保持A杯始终处于水平状态。

S13、调整末端机械手位姿，使A杯倾斜90°，将水倒入B杯，倾斜90°后的机械手须保持静止5s，以彻底完成倒水动作。

S14、末端机械手回位90°至倒水前姿态(垂直于桌面)，带动机械臂各连杆和机械手处于初始状态。

S2、对以上动作整体完成5次，在计算机端记录每次各关节处及机械手数据，机械臂各连杆和关节示意图如图4所示。

S21机械臂底端固定在桌上一角，共有5个连杆和一个末端抓手。

S22、机械臂共有6个关节，对于末端抓手也建立一个末端坐标系，用于表示末端位姿。

S23、在记录机械臂操作演示数据前，机械臂硬件参数规定如下：

S231、实验使用的机械臂底座旋转关节坐标系原点定义为O₀。各关节数据均是相对于基坐标系而言。

S232、每个关节和机械手均定义一个位姿坐标系。定义第i个关节数据O_i：

O_i＝[x_i,y_i,z_i,α_i,β_i,γ_i],1≤i≤7

其中，x_i,y_i,z_i分别表示第i个关节在基坐标系三个方向上的位移偏量，α_i,β_i,γ_i分别表示第i个关节在基坐标系三个方向上的角度偏量。

S233、机械手共有两个指尖，每个指尖合拢至恰好夹紧水杯时的角度变化量为[f₁,f₂]。f₁、f₂为人为设置的指尖抓取时的变化角度。

S24、执行机械臂操作演示时，一组完整的数据采集包括以下三个部分：

S241、在每个关节处记录下其运动位姿，则一次完整动作的位姿轨迹数据是由多个关节处的数据拼接组成的高维矩阵。

S242、在每个关节处记录下其运动速度，则一次完整动作的速度数据是由多个关节处的数据拼接组成的高维矩阵。

S243、在每个关节处记录下其运动加速度，则一次完整动作的加速度数据是由多个关节处的数据拼接组成的高维矩阵。

S3、对完成的五组动作所采集到的数据，分别使用隐半马尔可夫模型建模，模型结构如图2所示，每个观测数据属于一个隐状态，一个隐状态包含多个观测数据，其持续时间拟合为高斯分布。由于一次机械臂抓取一般分为靠近、抓取、抓取时运动、释放、远离这5个过程。而每个模型的隐状态数目决定了在维特比算法进行最优模型解码时，数据将会被分割成5个过程中的1个或几个。考虑到机械臂动作的多样性，本实施例设置了5组观测数据，对于某个机械臂动作的完整过程建立5个仅隐状态数不同的HSMM 模型，各模型的隐状态数目分别为3、4、5、6、7。通过BIC筛选出对数据分割效果最好的HSMM模型。

S4、运用BIC信息准则BIC＝kln(n_i)-2ln(L_i),i＝1,...,5确定五个模型中的最优者。

S41、计算每个模型的似然函数L_i,i＝1,2,3,4,5，由似然函数的定义，在数值上 L(θ|x)＝P(X＝x|θ)，θ是模型的参数集。结合HSMM，似然函数求解如下，

λ代表隐半马尔可夫模型参数集，S_t＝j表示t时刻系统处于隐状态j。其中，所述参数包括初始概率向量π、状态转移概率矩阵A、观测概率矩阵B、驻留时间矩阵D、观测序列长度M、隐状态个数N。

S42、确定k和n_i。其中k代表模型参数个数，本实施例中k＝6。n_i为第i个模型的样本数量，取决于观测序列。计算BIC取值，取值越大说明模型越优。

S5、针对计算出的最优模型，使用维特比算法求解运动基元行为并保存为运动基元库，库中每个基元用二元元组可以表示为(i,h),(j,d)，其意义表达为状态为i，状态驻留时间为h；状态为j，状态驻留时间为d。基元的个数等于隐状态数目。

S6、准备再现示教动作，首先求出当前时刻运动基元行为j及其驻留时间d。

S61、新型前向变量被定义为

这是一个迭代公式，其初值由BIC选出的最优HSMM的初始概率向量π决定。其中，α_t(j,d)代表 t时刻隐状态为j且隐状态持续时间已经为d的概率，α_t-d(i,h)代表在t-d时刻，模型隐状态为i且隐状态持续时间已经为h的概率；a_(i,h)(j,d)代表在i状态驻留时间为h转移到 j状态且驻留时间为d的概率，b_j,d(O_t-d+1:t)代表在j状态下观测到t-d+1至t时刻这段观测值的概率。

S62、隐半马尔可夫模型中的滤波概率被定义为：

其中，S_[t-d+1:t]＝j代表t-d+1至t这段时刻系统处于运动基元j。当滤波概率最大时，基元执行概率最大。据此公式即可求出当前时刻运动基元行为及其驻留时间。

对于分母项：

其中，α_t-d(i,h)代表在t-d时刻，模型隐状态为i且隐状态持续时间已经为h的概率；α_(i,h)(j.d+k)代表模型在隐状态i驻留时间为h且转移到隐状态j驻留时间为d+k的概率，b_j,d+k(O_t-d+1:t+k)代表在j状态下观测到t-d+1至t+k时刻这段观测值的概率。分别对第三项和第二项的所有k求和，再对第一项的所有j，d，i，h求和，即可求得P[o_1:t|λ]。

S7、已经求得系统当前时刻所处的运动基元行为，到S5步骤中所述的运动基元库寻找该运动基元行为，准备进行轨迹跟踪控制，即再现该运动基元。

S8、将当前时刻运动基元行为作为DMP算法输入进行轨迹学习，输出为机械臂跟踪轨迹。该算法主要分为轨迹学习和轨迹泛化两部分。算法流程图如图3所示。

所述DMP算法为：

其中y,

分别代表系统运动位移、速度和加速度；g为系统运动目标值；f_target是系统强迫项；τ为时间缩放因子；α_y,β_y是典型系统参数。所述系统为机械臂各关节和机械手。

S81、f_target作为DMP算法的强迫项，决定着学习不同复杂程度轨迹信息的能力，由多个基函数及他们的权重决定。本发明内容中基函数设置为高斯函数，权重计算采用局部加权回归。

S82、将已经求得的运动基元行为作为算法输入，设置算法参数包括时间因子、基函数个数及基函数为高斯分布时的高斯分布参数等。

S83、利用局部加权回归的方法求得输入的运动基元行为的强迫函数并提取出此时各个基函数的权重。

S84、进行轨迹学习泛化，首先设置DMP二阶动态系统方程参数，令4β_y＝α_y，这样能确保运动位移y单调收敛至目标点g，并将上一步提取的基函数权重代入方程，求得新强迫函数f_target。

S85、判断条件f_target-f＜ε，即学习泛化出的轨迹与原轨迹的相近程度。若不满足，再次执行S81步骤，直至条件满足，输出执行轨迹的位置，速度和加速度，算法结束。

S9、当前时刻运动基元行为执行完毕后，循环执行S6-S8步骤。从时间维度上看，机械臂完成了示教动作的学习和再现过程。

本发明还提供了一种基于隐半马尔可夫模型的机械臂控制系统，该系统包括：存储器、处理器及在所述存储器上存储并可运行的机械臂控制的程序，所述机械臂控制的程序被处理器执行时实现所述基于隐半马尔科夫模型的机械臂控制方法的部分或全部步骤。

Claims (8)

1.一种基于隐半马尔可夫模型的机械臂控制方法，其特征在于，包括步骤：

(1)获取机械臂抓取的观测数据，利用隐半马尔可夫模型HSMM对观测数据建立训练模型；

(2)利用贝叶斯信息准则BIC对所述训练模型进行优化，得到最优模型；

(3)使用维特比算法对最优模型进行解码，得到隐藏的基元运动行为，建立基元运动库；

(4)计算HSMM新型前向概率，根据HSMM新型前向概率的滤波概率求得当前时刻被激活的运动基元，重新规划基元运动序列；

(5)基于DMP算法对每个运动基元建立基元轨迹模型，控制机械臂执行基元轨迹；

步骤(3)包括：利用维特比算法

将最优模型下最可能的隐状态分布路径作为基元运动序列，并建立基元运动库；其中，O代表观测数据序列，Q代表该模型下所有可能的隐状态路径集合，λ代表模型参数集；

步骤(4)中，所述HSMM新型前向概率定义为：

其中，α_t(j,d)代表t时刻隐状态为j且隐状态持续时间已经为d的概率，α_t-d(i,h)代表在t-d时刻，模型隐状态为i且隐状态持续时间已经为h的概率；a_(i,h)(j,d)代表在i状态驻留时间为h转移到j状态且驻留时间为d的概率，b_j,d(O_t-d+1:t)代表在j状态下观测到t-d+1至t时刻这段观测值的概率；

所述滤波概率定义为：

其中，o_1:t表示1到t时刻内的观测值，S_[t-d+1:t]＝j代表t-d+1至t这段时刻隐状态为j的概率，λ代表模型参数集，α_t-d(i,h)代表在t-d时刻，模型隐状态为i且隐状态持续时间已经为h的概率；D代表每个隐状态的驻留时间，α_(i,h)(j,d+k)代表模型在隐状态i驻留时间为h且转移到隐状态j驻留时间为d+k的概率，b_j,d+k(O_t-d+1:t+k)代表在j状态下观测到t-d+1至t+k时刻这段观测值的概率。

2.根据权利要求1所述的基于隐半马尔可夫模型的机械臂控制方法，其特征在于，所述步骤(1)包括：

(11)获取机械臂完成指定抓取动作的N组观测数据，每组观测数据对应一个隐状态数目取值；

(12)建立HSMM模型，通过N组观测数据训练得到N个训练模型；所述HSMM模型的参量包括：初始概率向量、状态转移概率矩阵、观测概率矩阵、驻留时间矩阵、观测序列长度和隐状态个数。

3.根据权利要求1所述的基于隐半马尔可夫模型的机械臂控制方法，其特征在于，所述步骤(2)包括：将满足贝叶斯信息准则BIC＝kln(n_i)-2ln(L_i),i＝1,...,N且取值最大的训练模型作为最优模型；其中，k代表训练模型的参数个数，n_i为第i个训练模型的样本数量，L_i为第i个训练模型的似然函数。

4.根据权利要求1所述的基于隐半马尔可夫模型的机械臂控制方法，其特征在于，所述DMP算法为：

其中y、

分别代表系统运动位移、速度和加速度；g为系统运动目标值；f_target是系统强迫项；τ为时间缩放因子；α_y,β_y是典型系统参数。

5.根据权利要求2所述的基于隐半马尔可夫模型的机械臂控制方法，其特征在于，机械臂完成指定抓取动作的观测数据包括：位姿、速度和加速度。

6.根据权利要求5所述的基于隐半马尔可夫模型的机械臂控制方法，其特征在于：所述位姿包括机械臂各关节的位姿信息、机械手的位姿信息和被抓物体的位姿信息；所述速度包括机械臂各关节的速度信息和机械手的速度信息；所述加速度包括机械臂各关节的加速度信息和机械手的加速度信息。

7.根据权利要求3所述的基于隐半马尔可夫模型的机械臂控制方法，其特征在于，所述似然函数为如下公式：

其中，i表示模型序号，i＝1,…,N，o_1:t表示1到t时刻内的观测值，S_t＝j表示t时刻系统隐状态为j，λ表示模型参数集。

8.一种基于隐半马尔可夫模型的机械臂控制系统，其特征在于，该系统包括：存储器、处理器及在所述存储器上存储并可运行的机械臂控制的程序，所述机械臂控制的程序被处理器执行时实现如权利要求1至7中任一项所述基于隐半马尔可夫模型的机械臂控制方法的步骤。

Images