CN112428278B - 机械臂的控制方法、装置及人机协同模型的训练方法 - Google Patents

Abstract

本申请公开了一种机械臂的控制方法、装置及人机协同模型的训练方法。该机械臂的控制方法包括获取人机协同模型，其中所述人机协同模型为根据人机交互力确定机械臂期望位姿的模型；获取当前时刻位姿，根据所述人机协同模型获取当前时刻人机交互力对应的期望位姿；根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹；根据所述最优轨迹对机械臂进行控制。本申请解决了机器人无法沿着人类意图的轨迹运动的问题。

Description

机械臂的控制方法、装置及人机协同模型的训练方法

技术领域

本申请涉及机械臂领域，具体而言，涉及一种机械臂的控制方法、装置及人机协同模型的训练方法。

背景技术

在骨科和穿刺机器人领域中，存在这样一类机器人，该机器人可以应用于手术领域，其与医生和和环境均有交互。这类机器人可以根据医生的交互力进行移动并对环境做功，然而，相关技术在面向特定轨迹进行拖动时(如圆弧，直线)，机器人无法根据人类的表现行为来判断人类企图，使机器人无法沿着人类意图的轨迹运动，如何控制机器人以达到准确理解医生意图并优化机器人-医生的交互体验的目的，成为亟待解决的问题。

针对机器人无法沿着人类意图的轨迹运动的问题，目前尚未提出有效的解决方案。

发明内容

本申请的主要目的在于提供一种机械臂的控制方法，以解决机器人无法沿着人类意图的轨迹运动的问题。

为了实现上述目的，本申请提供了一种机械臂的控制方法、装置及人机协同模型的训练方法。

第一方面，本申请提供了一种机械臂的控制方法。

根据本申请的机械臂的控制方法包括：

获取人机协同模型，其中所述人机协同模型为根据人机交互力确定机械臂期望位姿的模型；

获取当前时刻位姿，根据所述人机协同模型获取当前时刻人机交互力对应的期望位姿；

根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹；

根据所述最优轨迹对机械臂进行控制。

进一步的，所述根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹包括：

通过模型预测控制MPC算法，根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿，生成多组随机轨迹；

从所述多组随机轨迹中选择最优轨迹。

进一步的，所述从所述多组随机轨迹中选择最优轨迹包括：

从所述多组随机轨迹，通过最优轨迹控制算法，选择最优轨迹。

进一步的，所述根据所述最优轨迹对机械臂进行控制，包括：

获取机械臂的位置及姿态角运动信息；

对机械臂的位置及姿态角运动信息的法向分量，进行第一模式控制；

对机械臂的位置及姿态角运动信息的切向分量，进行第二模式控制；其中，所述第一模式为机械臂导纳大于所述第二模式的机械臂导纳的机器人引导模式；所述第二模式为人类导纳大于所述第一模式的人类导纳的人类引导模式。

第二方面，本申请提供了一种人机协同模型的训练方法，用于得到第一方面中的机械臂的控制方法中的人机协同模型。

根据本申请的人机协同模型的训练方法包括：

获取机械臂的多组人机交互力和所述多组人机交互力对应的多组机械臂位姿，所述多组人机交互力为多组原始人机交互力；

根据所述多组人机交互力和所述多组机械臂位姿，建立人机协同模型。

进一步的，在所述根据所述多组人机交互力和所述多组机械臂位姿，建立人机协同模型之后，所述方法还包括：

根据监督学习方法对人机协同模型进行优化。

第三方面，本申请提供了一种机械臂的控制装置。

根据本申请的机械臂的控制装置包括：

模型获取模块，用于获取人机协同模型，其中所述人机协同模型为根据人机交互力确定机械臂期望位姿的模型；

位姿获取模块，用于获取当前时刻位姿，根据所述人机协同模型获取当前时刻人机交互力对应的期望位姿；

轨迹生成模块，用于根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹；

控制模块，用于根据所述最优轨迹对机械臂进行控制。

进一步的，所述模型获取模块包括：

优化单元，用于根据对监督学习方法人机协同模型进行优化。

进一步的，所述轨迹生成模块包括：

随机轨迹生成单元，用于通过模型预测控制MPC算法，根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿，生成多组随机轨迹；

最优轨迹生成单元，用于从所述多组随机轨迹中选择最优轨迹。

进一步的，所述最优轨迹生成单元还包括：

用于从所述多组随机轨迹，通过最优轨迹控制算法，选择最优轨迹。

进一步的，所述控制模块包括：

控制器控制单元，用于根据所述最优轨迹，通过机械臂的控制器对机械臂进行控制，其中，所述控制器包括对机械臂进行控制的内层控制器和对人机协同模型进行控制的外层控制器。

第四方面，本申请提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现第一方面提供的机械臂的控制方法和/或第二方面提供的人机协同模型的训练方法的步骤。

第五方面，本申请提供一种机器人，包括机械臂、传感器、控制器、存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现第一方面提供的机械臂的控制方法和/或第二方面提供的人机协同模型的训练方法的步骤。

在本申请实施例中，通过人机协同模型确定当前时刻人机交互力对应的期望位姿，并根据机械臂当前时刻位姿和当前时刻人机交互力对应的期望位姿生成机械臂期望运动的最优轨迹，从而通过机械臂期望运动的最优轨迹对机械臂进行控制，达到了使机器人沿着人类意图的轨迹运动，从而实现了控制轨迹以达到准确理解医生意图并优化人机交互体验的技术效果进而解决了机器人无法沿着人类意图的轨迹运动的问题。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，使得本申请的其它特征、目的和优点变得更明显。本申请的示意性实施例附图及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是根据本申请实施例的机械臂的控制方法的流程示意图；

图2是根据本申请实施例的人机协同模型训练方法的流程示意图；

图3是根据本申请实施例的机械臂的控制装置的结构框图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

如图1所示，该方法包括如下的步骤S11至步骤S14：

S11：获取人机协同模型，其中所述人机协同模型为根据人机交互力确定机械臂期望位姿的模型。

人机协同模型可以是机械臂的控制系统中预存的模型，也可以通过机器学习方法进行训练得到人机协同模型，也可以是通过机器学习方法训练后进行优化的人机协同模型。在该实施例中，示例的，人机协同模型为通过机器学习方法进行训练得到的，具体的训练方法以参见后面的实施例部分图2的说明。具体的，人机协同模型是经由高斯混合模型(Gaussian Mixture Model，以下简称为GMM)作为预训练的各种神经网络模型或高斯过程模型。

S12：获取当前时刻位姿，根据所述人机协同模型获取当前时刻人机交互力对应的期望位姿。

其中，人机交互力可以通过安装在机械臂上的力传感器直接获取。具体的，力传感器为多维力传感器。在该实施例中，示例的，力传感器通过三维力传感器或六维力传感器获取。将获取到的当前时刻的人机交互力输入至人机协同模型，可以得到预测的机械臂下一时刻的期望位姿。该期望位姿被应用于限定区域范围内路径切线方向的控制，且当期望位姿有较大偏离时退出该控制方法。人机交互力还可以为包含人机阻抗力的人机交互力。包含人机阻抗力的人机交互力先可以通过安装在机械臂上的力传感器获取，再对力传感器获取的力和对应的当前时刻位姿求解得到机械臂的虚拟约束(即人机阻抗力)，从而可以通过对力传感器获取的人机交互力和求解得到的虚拟约束求和，确定包含人机阻抗力的人机交互力。

S13：根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂期运动的最优轨迹。

“根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹”具体为：通过模型预测控制(model predictive control，以下简称为MPC)算法，根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿，生成多组随机轨迹；从所述多组随机轨迹中选择最优轨迹。

具体的，MPC是一种基于当前时刻的模型预测未来一段时间内的过程输出的，选取目标优化函数，预测未来输出序列并输出当前时刻控制量，下一时刻最新实测数据对前一时刻的过程输出序列进行反馈校正的算法。即，MPC可以使当前时刻的人机交互模型预测未来一段时间内输出的期望位姿。根据当前时刻的位姿和人机协同模型，可以通过MPC预测未来时间的期望位姿，并生成多组随机轨迹，并选择多组随机轨迹的最优轨迹。可选地，在该步骤中生成的机械臂运动的最优轨迹为在机械臂在限定区域范围内运动的最优轨迹，最优轨迹的特征是操作者在切线方向上可以控制前进、后退；但法线方向上由机器人自主控制。由于人类在切线上具有强控制能力，但在法线上机器人控制能力较强，操作者通过人机协同模型将期望位置传递给机械臂，机械臂通过跟踪期望位置在路径上的投影点实现拖动效果。

“从所述多组随机轨迹中选择最优轨迹”具体为：从所述多组随机轨迹，通过最优轨迹控制算法，选择最优轨迹。

具体的，选取最优轨迹可以通过线性二次型调节器算法、非线性二次型调节器(Iterative Linear Quadratic Regulator，以下简称iLQR)算法或微分动态规划进行确定，在此不进行限制。在该实施例中，示例的，选取最优轨迹通过最优轨迹控制算法中的iLQR算法进行确定。其中，iLQR算法可以得到状态非线性反馈的最优控制规律，易于构成闭环最优控制。即，通过iLQR算法可以确定多组随机轨迹中的最优轨迹。可选的，根据当前时刻位姿和当前时刻人机交互力的期望位姿在10ms至500ms间对运动轨迹(运动位置、速度)进行优化。其中，机器人在法线上具有关于位置的价值权重从而精准控制位置、在切线方向上人类具有较大的导纳值从而实现人类引导拖动。另外，人类在切线分量上比机器人的控制能力更强，但在法线分量上机器人的控制能力比人类更强。用户通过上述步骤中的人机协同模型将期望位置传递给机械臂，机械臂通过跟踪期望位置在路径上的投影点实现拖动效果。

S14：根据所述最优轨迹对机械臂进行控制。

“根据所述最优轨迹对机械臂进行控制”具体为：获取机械臂的位置及姿态角运动信息；对机械臂的位置及姿态角运动信息的法向分量，进行第一模式控制；对机械臂的位置及姿态角运动信息的切向分量，进行第二模式控制；其中，第一模式为机械臂导纳大于第二模式的机械臂导纳的机器人引导模式；第二模式为人类导纳大于第一模式的人类导纳的人类引导模式。

具体的，根据机器人动力学，通过机械臂实际运动的阻抗坐标系和机械臂期望运动的期望坐标系构建机械臂的误差反馈量，如式(1)所示：

其中，M(q)为机械臂在笛卡尔空间下的惯性矩阵，矩阵前三列单位为kg，后面所有的元素单位为Ns²/rad；q为关节角；x前三行单位为m，后面所有行对应的单位为rad；

为粘滞矩阵；g(q)为重力向量；f_env为环境交互力旋量，可以通过环境-机械臂交互的力传感器获取；f为人机交互力，可以通过上述步骤S11中的力传感器进行获取。

基于动力学表达，基于反馈线性化方法构建力控制器，完成对人类高刚度、对环境低刚度的机械臂内环表现。内环输入为阻抗坐标系位置和姿态，通过上述步骤中的iLQR方法可以机械臂各关节对应的运动最优轨迹，并根据式(1)的误差反馈量，进行分方向的控制。机械臂在路径跟踪过程中，当机械臂位置及姿态在法向子方向运动时，根据机器大导纳、用户小导纳对机械臂进行控制(即第一模式)；当机械臂位置及姿态在切线子方向运动时，根据用户大导纳、机器小导纳对机械臂进行控制(即第二模式)。示例的，当根据人机协同模型得到期望位姿的预测X_t+1(0m，0.1m，0.1m，0.3°,0.1°,0.2°)，可以根据用户大导纳、机器小导纳的原则对机械臂进行控制。其中，第一模式为机器人引导模式，第二模式为人类引导模式，第一模式和第二模式可以共存，但两种模式的运动方向不同。即，在法线方向上对机械臂进行第一模式控制；在切线方向上对机械臂进行第二模式控制。

从以上的描述中，可以看出，本发明实现了如下技术效果：

通过人机协同模型确定当前时刻人机交互力对应的期望位姿，从而可以确定机械臂当前时刻和预测时刻的预测位移，通过MPC生成预测位移的多组随机轨迹，再根据最优轨迹控制算法确定多组随机轨迹的最优轨迹，并获取机械臂的位置和姿态角运动信息，对机械臂进行控制，达到了使机器人沿着人类意图的轨迹运动的效果。

根据本申请实施例，还提供了一种用于得到上述机械臂的控制方法中的人机协同模型的方法，如图2所示，该人机协同模型训练方法包括如下的步骤S21、步骤S22：

S21：获取机械臂的多组人机交互力和所述多组人机交互力对应的多组机械臂位姿，所述多组人机交互力为多组原始人机交互力；

S22：根据所述多组人机交互力和所述多组机械臂位姿，建立人机协同模型。

人机交互力可以通过可以通过安装在机械臂上的力传感器直接获取。具体的，力传感器为多维力传感器。在该实施例中，示例的，力传感器通过六维力传感器获取。通过力传感器获取的训练力组包括X、Y、Z轴对应的三个训练力分量和三个训练力矩分量。机械臂位姿可以通过建立机械臂的包括X、Y、Z轴坐标系进行记录，具体的，机械臂位姿包括X、Y、Z轴对应的三个距离移动分量和三个角度移动分量。实例的，获取的人机交互力W_t(1N，0N，0N，0.1Nm,0.2Nm,0.3Nm)，机械臂位姿X_t(0.01m,0.02m,0.01m,0.3°,0.4°,0.1°)。具体的，获取的多组人机交互力为3-5组。即，当获取的多组人机交互力为3组时，获取的机械臂位姿也为3组。人机交互力还可以为包含人机阻抗力的人机交互力。通过对力传感器获取的力和对应的当前时刻位姿求解得到的机械臂的虚拟约束(即人机阻抗力)，从而可以通过对力传感器获取的人机交互力和求解得到的虚拟约束求和，确定包含人机阻抗力的人机交互力。

训练人机协同模型的模型输入可以为当前时刻的人机交互力和机械臂当前时刻位姿的采样值，也可以为当前时刻的人机交互力和机械臂期望位姿的采样值，并根据模型输入的采样值对人机协同模型进行训练。具体的，人机协同模型训练的网络模型可以是高斯混合模型(GaussianMixtureModel，以下简称为GMM)、贝叶斯网络模型、神经网络模型等，在此不进行限制。

具体的，步骤S21“获取机械臂的多组人机交互力和所述多组人机交互力对应的多组机械臂位姿”可以为在信赖区域下获取机械臂的多组人机交互力和所述多组人机交互力对应的多组机械臂位姿。其中，信赖区域是指为获取的人机交互力的力传感器的采样分布ps在预设KL散度阈值之间的区域，其中，KL散度指ps和人机协同模型之间的KL散度，如式(2)所示，KL散度可以表达为：

D_KL(p_s,p_m)≤th_KL (2)

其中，ps为力传感器的采样分布，ps由最大似然估计得到；pm为人机协同模型的模型分布，th_KL为第一预设KL散度阈值，可以由用户进行设定，也可以由人机协同模型由机器学习方法通过用户在不同人机阻抗力的学习得到，(例如，第一预设KL散度阈值可以是-20)。

进一步的，在训练人机协同模型的模型之后，该方法还包括：判断人机协同模型是否为有效模型。

具体的，判断人机协同模型是否为有效模型，可以通过判断ps和pm之间的KL散度是否大于第二预设KL散度，若大于，则人机协同模型为有效模型。(例如，ps和pm之间的KL散度为-35，第二预设KL散度为-50，ps和pm之间的KL散度大于第二预设KL散度，则人机协同模型为有效模型)。

具体的，判断人机协同模型是否为有效模型，可以通过计算上述步骤中采集的人机交互力的似然度，并判断似然度是否大于第一预设似然度阈值，若大于，则人机协同模型为有效模型。例如，上述步骤中采集的人机交互力为W₁(1N，0.5N，0N，0.1Nm,0.2Nm,0.3Nm)；W₂(2N，0.5N，0N，0.1Nm,0.2Nm,0.3Nm)；W₃(3N，0.5N，0N，0.1Nm,0.2Nm,0.3Nm)，通过W₁、W₂、W₃可以求解得到模型似然度＝0.3，并判断第一预设似然度阈值是否小于模型似然度，若模型似然度大于模型似然度，则人机协同模型为有效模型(例如，模型似然度＝5，第一预设似然度＝2.5，模型似然度大于第一预设似然度，则人机协同模型为有效模型)。

进一步的，在所述根据所述多组人机交互力和所述多组机械臂位姿，建立人机协同模型之后，所述方法还包括：

根据监督学习方法对人机协同模型进行优化，生成优化后的人机协同模型。

利用监督学习方法对人机协同模型的参数进行优化包括采用先验信息，具体的，利用监督学习方法中的最大似然原理，对人机协同模型的参数进行优化，如式(3)所示，优化后的训练模型的对应参数为：

其中，pm为人机协同模型的模型分布，f_h为通过力传感器获取的人机交互力，x_d为机械臂位姿，t为当前时刻，t+1为下一时刻，θ_C为人机协同模型的参数。具体的，当人机协同模型为GMM时，θ_C为分模型序号；当人机协同模型为神经网络模型时，θ_C为连接节点无量纲权重。

示例的，在对人机协同模型的参数进行优化后，根据优化后的参数对人机协同模型进行优化。具体的，对人机协同模型不同的建模方式使用不同的优化方法，例如，当人机协同模型为GMM时，采用最大化(Expectation-Maximum，以下简称为EM)算法对人机协同模型进行优化；当人机协同模型为神经网络时，采用随机梯度下降(stochastic gradientdescent，以下简称为SGD)方法对人机人机协同模型进行优化。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

根据本发明实施例，还提供了一种用于实施上述机械臂的控制方法的装置10，如图3所示，该机械臂的控制装置10包括：

模型获取模块11，用于获取人机协同模型，其中所述人机协同模型为根据人机交互力确定机械臂期望位姿的模型；

位姿获取模块12，用于获取当前时刻位姿，根据所述人机协同模型获取当前时刻人机交互力对应的期望位姿；

轨迹生成模块13，用于根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹；

控制模块14，用于根据所述最优轨迹对机械臂进行控制。

进一步的，所述模型获取模块11包括：

优化单元，用于根据监督学习方法对人机协同模型进行优化。

进一步的，所述轨迹生成模块13包括：

随机轨迹生成单元，用于通过模型预测控制MPC算法，根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿，生成多组随机轨迹；

最优轨迹生成单元，用于从所述多组随机轨迹中选择最优轨迹。

进一步的，所述最优轨迹生成单元还包括：

用于从所述多组随机轨迹，通过最优轨迹控制算法，选择最优轨迹。

进一步的，所述控制模块14包括：

控制器控制单元，用于根据所述最优轨迹，通过机械臂的控制器对机械臂进行控制，其中，所述控制器包括对机械臂进行控制的内层控制器和对人机协同模型进行控制的外层控制器。

具体的，本实施例中各个模块的实现可以参考方法实施例中的相关实现，不再赘述。

从以上的描述中，可以看出，本申请实现了如下技术效果：

通过人机协同模型确定当前时刻人机交互力对应的期望位姿，从而可以确定机械臂当前时刻和预测时刻的预测位移，通过MPC生成预测位移的多组随机轨迹，再根据最优轨迹控制算法确定多组随机轨迹的最优轨迹，并获取机械臂的位置和姿态角运动信息，对机械臂进行控制，达到了使机器人沿着人类意图的轨迹运动的效果。

显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (1)

1.一种机器人，其特征在于，包括：机械臂、传感器、至少一个处理器；以及与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器执行机械臂的控制方法和/ 或人机协同模型的训练方法；

所述机械臂的控制方法，包括：

获取人机协同模型，其中所述人机协同模型为根据人机交互力确定机械臂期望位姿的模型，通过机器学习方法进行训练得到人机协同模型；训练人机协同模型的模型输入可以为当前时刻的人机交互力和机械臂当前时刻位姿的采样值，也可以为当前时刻的人机交互力和机械臂期望位姿的采样值，并根据模型输入的采样值对人机协同模型进行训练；

获取当前时刻位姿，根据所述人机协同模型获取当前时刻人机交互力对应的期望位姿；

根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹；

根据所述最优轨迹对机械臂进行控制；

所述最优轨迹的特征是操作者在切线方向上可以控制前进、后退；但法线方向上由机器人自主控制；

所述根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹包括：

通过模型预测控制MPC算法，根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿，生成多组随机轨迹；

从所述多组随机轨迹中选择最优轨迹；

所述从所述多组随机轨迹中选择最优轨迹包括：

从所述多组随机轨迹，通过最优轨迹控制算法，选择最优轨迹；

所述根据所述最优轨迹对机械臂进行控制，包括：

获取机械臂的位置及姿态角运动信息；

对机械臂的位置及姿态角运动信息的法向分量，进行第一模式控制；

对机械臂的位置及姿态角运动信息的切向分量，进行第二模式控制；其中，所述第一模式为机械臂导纳大于所述第二模式的机械臂导纳的机器人引导模式；所述第二模式为人类导纳大于所述第一模式的人类导纳的人类引导模式；

根据机器人动力学，通过机械臂实际运动的阻抗坐标系和机械臂期望运动的期望坐标系构建机械臂的误差反馈量，如式所示：

其中，M(q)为机械臂在笛卡尔空间下的惯性矩阵，矩阵前三列单位为kg，后面所有的元素单位为Ns²/rad；q为关节角；x前三行单位为m，后面所有行对应的单位为rad；

为粘滞矩阵；g(q)为重力向量；f_env为环境交互力旋量，可以通过环境-机械臂交互的力传感器获取；f为人机交互力；

所述人机协同模型的训练方法包括：

在信赖区域下获取机械臂的多组人机交互力和所述多组人机交互力对应的多组机械臂位姿，其中，信赖区域是指为获取的人机交互力的力传感器的采样分布ps在预设KL散度阈值之间的区域，其中，KL散度指ps和人机协同模型之间的KL散度，如式所示，KL散度可以表达为：

D_KL(p_s，p_m)≤th_KL

其中，ps为力传感器的采样分布，ps由最大似然估计得到；pm为人机协同模型的模型分布，th_KL为第一预设KL散度阈值，可以由用户进行设定，也可以由人机协同模型由机器学习方法通过用户在不同人机阻抗力的学习得到；

所述多组人机交互力和所述多组机械臂位姿，建立人机协同模型之后，所述方法还包括：

利用监督学习方法对人机协同模型的参数进行优化包括采用先验信息，具体的，利用监督学习方法中的最大似然原理，对人机协同模型的参数进行优化，如式所示，优化后的训练模型的对应参数为：

其中，pm为人机协同模型的模型分布，f_h为通过力传感器获取的人机交互力，x_d为机械臂位姿，t为当前时刻，t+1为下一时刻，θ_C为人机协同模型的参数，具体的，当人机协同模型为GMM时，θ_C为分模型序号；当人机协同模型为神经网络模型时，θ_C为连接节点无量纲权重；

所述机械臂的控制装置，包括：

模型获取模块，用于获取人机协同模型，其中所述人机协同模型为根据人机交互力确定机械臂期望位姿的模型；

位姿获取模块，用于获取当前时刻位姿，根据所述人机协同模型获取当前时刻人机交互力对应的期望位姿；

轨迹生成模块，用于根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿生成机械臂运动的最优轨迹；

控制模块，用于根据所述最优轨迹对机械臂进行控制；

所述轨迹生成模块包括：

随机轨迹生成单元，用于通过MPC算法，根据所述当前时刻位姿和所述当前时刻人机交互力对应的期望位姿，生成多组随机轨迹；

最优轨迹生成单元，从所述多组随机轨迹中选择最优轨迹；

所述机器人还包括，计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行所述的机械臂的控制方法和/或人机协同模型的训练方法。

Images