一种基于人机融合的机械臂控制方法

IPC分类号 : B25J9/16,B25J3/00

申请号

CN201911419535.1

可选规格: 数量

库存1件

确认取消

￥30000; 库存1件

首页

立即咨询

看了又看

专利摘要

本发明涉及一种基于人机融合的机械臂控制方法，属于机器人控制技术领域。该控制方法包括以下步骤：(1)获取目标物的周边实时场景图像；(2)在操作端显示实时场景图像，并获取人手臂关节角度数据，及将人手臂关节角度数据映射成用于控制机械臂的映射关节角度数据θH；(3)依据当前位置数据与预测目标物的位置数据，按照多项式路径规划方法规划出预设操作路径，并获取用于控制机械臂的规划关节角度数据θR；(4)依据公式θS＝(1‑α)θH+αθR计算机械臂的当前执行关节角度数据；权重α随机械臂预测目标物体置信度的增加而增加。该方法能很好地集合了人手臂与机械臂的动作优缺点，其机器人远程控制等领域。

权利要求

1.一种基于人机融合的机械臂控制方法，其特征在于，所述机械臂控制方法包括以下步骤：

获取步骤，获取目标物的周边实时场景图像；

映射步骤，在控制端显示所述实时场景图像，并获取人手臂在示范动作过程中的人手臂关节角度系列数据，及将所述人手臂关节角度数据映射成用于控制所述机械臂的映射关节角度数据θH；

规划步骤，依据其执行末端的当前位置数据与预测目标物的位置数据，按照多项式路径规划方法规划出预设操作路径，并依据该所述预设操作路径获取用于控制所述机械臂的规划关节角度数据θR；

融合步骤，依据公式θS＝(1-α)θH+αθR计算所述机械臂的当前执行关节角度数据；α为人机权重系数，0≤α≤1，且随预测目标的置信度的增加而增加；在所述机械臂的执行末端与所述预测目标物之间的间距小于预设间距后，所述置信度随该间距的减小而增大；

控制步骤，以所述当前执行关节角度数据为控制指令，控制所述机械臂执行操作动作；

在所述规划步骤中，基于下式，使用最大熵原理建立人手臂轨迹概率分布模型，用于预测所述预测目标物的位置数据:

其中，G*为最可能到达目标物位置，S为初始位置，U为用户输入位置，ξ表示运动轨迹，CG为成本函数，概率函数P(G)满足下式，

其中，L是机械臂末端的初始位置距离目标位置的距离，l是当前机械臂末端距离目标位置的距离；

人机权重系数α的表达式为：

其中，conf为置信度，其表达式为，

conf＝P(G*|ξS→U)logP(G*|ξS→U)

P(G*|ξS→U)＝P(ξS→U|G*)P(G*)。

2.根据权利要求1所述的机械臂控制方法，其特征在于：

所述预测目标物为与所述执行末端的位置最近的物体。

3.根据权利要求1或2所述的机械臂控制方法，其特征在于，所述多项式路径规划方法为关节角度函数，满足下式：

θ(t)＝a5×t5+a4×t4+a3×t3+a2×t2+a1×t+a0

其中，t为时间，T为运动到指定位置的时间，θ(t)表示关节空间的位置，θT表示关节空间在T时刻的位置，θ0表示关节空间的初始位置，a0、a1、a2、a3、a4、a5表示五次多项式的各项系数。

4.根据权利要求3所述的机械臂控制方法，其特征在于：

在检测到所述人手臂的关节角度的加速度超过预设阈值时，对所述人机权重系数α赋予小于0.5的数值。

5.根据权利要求3所述的机械臂控制方法，其特征在于，所述机械臂为异构仿人机械臂，所述将所述人手臂关节角度数据映射成用于控制所述机械臂的映射关节角度数据θH的步骤包括以下步骤：

匹配步骤，对所述机械臂与所述人手臂的末端工作空间进行匹配，获取两个空间之间的转换矩阵；并基于所述人手臂的当前腕关节坐标数据，利用所述转换矩阵，计算出所述机械臂的目标腕关节坐标数据；

计算步骤，基于所述目标腕关节坐标数据及所述机械臂的结构尺寸数据，并以人体小臂的方向向量作为所述机械臂的小臂方向向量，计算出所述仿人机械臂的肘关节坐标数据；

求解步骤，以机械臂的肘关节及肩关节角度数据为自变量，以机械臂末端工作空间为自变量取值范围，至少以机械臂腕关节位置相对所述目标腕关节坐标数据的计算偏差构建适应度函数，利用遗传算法搜索出最优肘关节与肩关节的角度数据，作为所述机械臂的映射关节角度数据及映射肘关节角度数据；及以所述人手臂的映射腕关节角度数据计算所述机械臂的映射腕关节角度数据。

6.根据权利要求5所述的机械臂控制方法，其特征在于：

以每代个体所表征的角度数据作为肘关节角度与肩关节角度的近似解，计算出近似腕关节位置，所述适应度函数为fit＝Dh+0.03De+0.03Sum，其中，Dh为所述近似腕关节位置与所述目标腕关节坐标数据所表征位置的欧式距离，De为基于肘关节角度近似解计算出的近似肘关节位置与所述肘关节坐标数据所表征位置的欧式距离，Sum为当前个体所表征的关节角度值之和。

7.根据权利要求5所述的机械臂控制方法，其特征在于，所述利用遗传算法搜索出最优肘关节与肩关节的角度数据的步骤包括以下步骤：

以机械臂的末端工作空间为取值区间，随机生成多个肘关节与肩关节角度解构建个体，组成初始种群；

基于适应度函数，计算个体适应度值，并进行高低排序，使用指数分布的形式生成随机整数对个体进行选择，并对随机选择出的两个个体基于直接融合方式繁殖下一代；

依据预设比例，利用繁殖产生的子代个体替代适应度值最大的部分个体，并基于变异概率，增加部分变异生成的新个体；

重复上述步骤至满足预设条件，获取适应度值最小的个体所表征的关节角度数据作为肘关节与肩关节角度的近似解。

8.根据权利要求1或2所述的机械臂控制方法，其特征在于，所述机械臂为异构仿人机械臂，所述将所述人手臂关节角度数据映射成用于控制所述机械臂的映射关节角度数据θH的步骤包括以下步骤：

9.根据权利要求8所述的机械臂控制方法，其特征在于：

10.根据权利要求8所述的机械臂控制方法，其特征在于，所述利用遗传算法搜索出最优肘关节与肩关节的角度数据的步骤包括以下步骤：

以机械臂的末端工作空间为取值区间，随机生成多个肘关节与肩关节角度解构建个体，组成初始种群；

依据预设比例，利用繁殖产生的子代个体替代适应度值最大的部分个体，并基于变异概率，增加部分变异生成的新个体；

重复上述步骤至满足预设条件，获取适应度值最小的个体所表征的关节角度数据作为肘关节与肩关节角度的近似解。

说明书

技术领域

本发明涉及一种机器人控制技术领域，具体地说，涉及一种基于人机融合的异构机械臂的控制方法。

背景技术

随着机器人技术的发展，为了确保操作人员的人身安全，通常会在机器人上搭载摄像头，例如如果公开号为CN108082415A的专利文献所公开的全景成像系统，或者在其工作现场布设摄像头而获取现场场景图像，而避免操作人员需进入深海作业环境、拆弹及扫雷等会对操作人员的安全造成影响的作业环境中。

并在获取环境场景图像后，由人手臂模拟现场操作动作，并通过获取该现场操作动作过程中人手臂的关节角度数据，例如，可采用公开号为CN107967687A的专利文献所公开的获取目标物行走姿态的方法；并基于该关节角度数据控制机器人进行现场操作。

基于前述远程操控方法，可以很好地避免对操作人员造成安全影响，但是由于人通过影响资料来判别现场环境与距离，难以完全获取所有现场三维信息的同时，存在判别误差问题，导致其难以完全基于所获取的人手臂在示范动作过程中的关节数据直接控制机器人的机械臂动作。

此外，在操作过程中，存在以下问题，虽然机械臂为仿人结构，但是仍难以与人手臂为同构结构，例如自由度或臂长不同，即通常都为异构，导致其难以利用人手臂的关节数据直接控制仿人机械臂进行操作，虽然能够利用工作空间矩阵直接获取机械臂控制的理论数据，但是由于二者异构，其所直接计算出的理论数据在部分情况下无法直接执行，而导致跟随控制在部分情况下无法实现。

发明内容

本发明的主要目的是提供一种机械臂的控制方法，以能更好利用现场场景图像对机器人进行远程控制；

本发明的另一目的是提供一种机械臂的控制方法，以在异构仿人机械臂与人手臂的跟随控制过程中能近似地执行一些无法完成的动作，而实现更好地跟随控制。

为了实现上述目的，本发明提供的机械臂控制方法基于人机融合策略，具体包括以下步骤：(1)获取目标物的周边实时场景图像；(2)在操作端显示实时场景图像，并获取人手臂在示范动作过程中的人手臂关节角度系列数据，及将人手臂关节角度数据映射成用于控制机械臂的映射关节角度数据θ_H；(3)依据当前位置数据与预测目标物的位置数据与人手臂轨迹概率分布模型预测目标，按照多项式路径规划方法规划出预设操作路径，并依据该预设操作路径获取用于控制机械臂的规划关节角度数据θ_R；(4)依据公式θ_S＝(1-α)θ_H+αθ_R计算机械臂的当前执行关节角度数据；0≤α≤1，为人机权重系数，且随着预测置信度的增加而增加，在机械臂的执行末端与预测目标物之间的间距小于预设间距后，该置信度随二者间间距的减小而增大；(5)以当前执行关节角度数据为控制指令，控制机械臂执行操作动作。

在控制过程中，首先根据用户当前输入的动作，基于预测目标物的置信度分配权重关节角度数据，进行人机融合，从而在远距离时，以人手臂的动作数据控制为主，而在近距离时，以现场规划方案为主，从而可有效地避免因人在查看现场作业环境图像时的判断误差等问题。且该方案为基于多项式路径规划方法而能有效避免由于人手控制过程中，出现碰坏目标物等。

预测目标物为与执行末端的位置最近的物体。

具体的方案为预测目标物的位置数据为依据人手臂轨迹概率分布模型进行预测。

更具体的方案为基于下式，利用获取人手臂轨迹概率分布模型预测目标物的位置数据:

其中，G^*为最可能到达目标物位置，S为初始位置，U为用户输入位置，ξ表示运动轨迹，θ为任何可能对预测有影响的参数，C_G为成本函数，概率函数P(G)满足下式，

其中，L是机械臂末端的初始位置距离目标位置的距离，l是当前机械臂末端距离目标位置的距离。

进一步的方案为人机权重系数α的表达式为：

其中，conf为置信度，其表达式为，

conf＝P(G^*|ξ_S→U)logP(G^*|ξ_S→U)

P(G^*|ξ_S→U)＝P(ξ_S→U|G^*)P(G^*)。

优选的方案为置信度在第一置信度区间内时，α为0；第一置信度区间位于前半程中；置信度在第二置信度区间内时，α随置信度增加而线性增加；置信度在第三置信度区间内时，α随置信度增加而指数增加；第三区间位于后半程中，且第一置信度区间、第二置信度区间及第三置信度区间相邻接且拼接成整个运动置信度区间。

优选的方案为多项式路径规划方法为关节角度函数，满足下式：

θ(t)＝a₅×t⁵+a₄×t⁴+a₃×t³+a₂×t²+a₁×t+a₀

其中，t为时间，T为运动到指定位置的时间。

优选的方案为在检测到人手臂的关节角度的加速度超过预设阈值时，对人机权重系数α赋予小于0.5的数值。

为了实现上述另一目的，本发明提供的优选的方案为机械臂为异构仿人机械臂，将人手臂关节角度数据映射成用于控制机械臂的映射关节角度数据θ_H的步骤包括以下步骤：(1)对机械臂与人手臂的末端工作空间进行匹配，获取两个空间之间的转换矩阵；并基于人手臂的当前腕关节坐标数据与机械臂的肩关节坐标数据，利用转换矩阵，计算出机械臂的目标腕关节坐标数据；(2)基于目标腕关节坐标数据及肩关节坐标数据，并以人体小臂的方向向量作为的小臂方向向量，计算出仿人机械臂的肘关节坐标数据；(3)以机械臂的肘关节及肩关节角度数据为自变量，以机械臂末端工作空间为自变量取值范围，至少以机械臂腕关节位置相对目标腕关节坐标数据的计算偏差构建适应度函数，利用遗传算法搜索出最优肘关节与肩关节的角度数据，作为机械臂的关节角度数据及肘关节角度数据；及以人手臂的腕关节角度数据计算机械臂的腕关节角度数据。

由以上方案可见，先获取人手臂与机械臂的末端工作空间的转换矩阵，依此获取机械臂末端的理论坐标数据，并获取肘关节的理论坐标数据；再基于遗传算法搜索出最优肘关节与肩关节的角度数据，且适应度函数主要考虑末端位置的计算偏差，即主要关注其执行末端的动作精度，以利用遗传算法搜索满足适应度函数要求的近似解进行近似执行，从而让仿人机械臂在执行动作过程中的关节角度数据无需完全参照人手臂的关节角度数据，且能近似执行，从而能更好进行异构控制。

优选的方案为求解步骤包括以下步骤：(1)以每代个体所表征的角度数据作为肘关节角度与肩关节角度的近似解，计算出近似腕关节位置；(2)基于目标腕关节数据与近似腕关节位置，获取腕关节位置的计算偏差，至少以计算偏差最小为基准，获取最优解。

优选的方案为适应度函数基于腕关位置及肘关节位置的计算偏差进行构建，并优先考虑腕关节位置的计算偏差。在主要考虑末端动作执行精度的前提下，部分考虑肘关节的精度，即优先考虑执行动作精度的前提下，使整个仿人机械臂的动作更接近人手臂的动作，从而便于人通过视频决定下一动作，从而更好地指导机械臂的现场操作。

优选的方案为适应度函数为fit＝D_h+0.03D_e+0.03Sum，其中，D_h为近似腕关节位置与目标腕关节坐标数据所表征位置的欧式距离，D_e为基于肘关节角度近似解计算出的近似肘关节位置与肘关节坐标数据所表征位置的欧式距离，Sum为当前个体所表征的关节角度值之和。在保证末端精度的前提下，对于肘部位置与目标偏离过大的解，以及解畸变从而产生的其他构型的解，适应度函数就会上升，排除大值，从而更好地进行场外控制。

优选的方案为利用遗传算法搜索出最优肘关节与肩关节的角度数据的步骤包括以下步骤：(1)以机械臂的末端工作空间为取值区间，随机生成多个肘关节与肩关节角度解构建个体，组成初始种群；(2)基于适应度函数，计算个体适应度值，并进行高低排序，使用指数分布的形式生成随机整数对个体进行选择，并对随机选择出的两个个体基于直接融合方式繁殖下一代；(3)依据预设比例，利用繁殖产生的子代个体替代适应度值最大的部分个体，并基于变异概率，增加部分变异生成的新个体；(4)重复上述步骤至满足预设条件，获取适应度值最小的个体所表征的关节角度数据作为肘关节与肩关节角度的近似解。

优选的方案为基于所获取机械臂的工作场所三维信息，自变量取值范围为机械臂末端工作空间排除干涉空间后的可行工作空间。从而在控制过程中，能有效避免现场环境对机械臂动作的干涉，从而人在远程操控的过程中，无需精确地判断现场干涉环境，从而更容易进行远程控制机械臂的动作。

附图说明

图1为本发明实施例中机械臂控制方法的工作流程图；

图2为本发明实施例中动作映射方法的工作流程图；

图3为本发明实施例中人手臂DH建模的示意图；

图4为本发明实施例中右臂的关节点及向量的几何关系；

图5为本发明实施例中左臂的关节点及向量的几何关系；

图6为本发明实施例中人手臂与机械臂之间动作映射的自由度匹配关系；

图7为本发明实施例中人手臂与机械手的末端工作空间在X-Z坐标系中的匹配过程示意图；

图8为本发明实施例中人手臂与机械手的末端工作空间在Y-Z坐标系中的匹配过程示意图；

图9为本发明实施例中人手臂与机械手的末端工作空间在X-Y坐标系中的匹配过程示意图；

图10为本发明实施例中人手臂与机械手的末端工作空间在X-Y-Z三维坐标系中的匹配过程示意图；

图11为本发明实施例中机械臂肘部坐标数据的归一化过程意图；

图12为本发明实施例中机械臂DH建模的示意图；

图13为人机权重系数与置信度关系；

图14为本发明实际实验中人机权重系数与置信度关系图；

图15为X轴向的人、机及二者融合的动作匹配效果图；

图16为Y轴向的人、机及二者融合的动作匹配效果图；

图17为Z轴向的人、机及二者融合的动作匹配效果图；

图18为单目标避障实验的照片，第一横排为机器单独控制，第二横排为人单独控制，第三排与第四排为人机共享；

图19为多目标预测抓取实验的照片，第一横排与第二横排为远距离目标物抓取，第三排与第四排为近距离目标抓取。

以下结合实施例及其附图对本发明作进一步说明。

具体实施方式

实施例

如图1所示，本发明基于人机融合的机械臂控制方法包括获取步骤S1、映射步骤S2、规划步骤S2、融合步骤S4及控制步骤S5，具体过程如下：

获取步骤S1，获取目标物的周边实时场景图像。

具体采用搭载在机器人上的摄像头或布设在工作现场的摄像头获取该机械人在工作过程中的场景图像，摄像头的数量根据实际进行布设，还可以基于布设深度摄像头而获取工作场景的深度信息。

映射步骤S2，在操作端显示实时场景图像，并获取人手臂在示范动作过程中的人手臂关节角度系列数据，及将该人手臂关节角度数据映射成用于控制机械臂的映射关节角度数据θ_H。

如图2所示，该映射步骤S2具体包括获取步骤S21、匹配步骤S22、计算步骤S23及求解步骤S24，具体过程如下：

获取步骤S21，获取人手臂在示范动作过程中的关节角度数据。

人体上肢使用最广泛的模型是7自由度模型，如图3所示，基坐标(X₀Y₀Z₀)位于人体躯干和肩关节之间，在肩关节处建立三个运动坐标系，分别对应肩关节的三个运动自由度，具体为内收/外展(X₁Y₁Z₁)、旋内/旋外(X₂Y₂Z₂)和屈/伸(X₃Y₃Z₃)；在肘关节处有屈/伸(X₄Y₄Z₄)自由度；而在手腕关节处根据旋内/旋外(X₅Y₅Z₅)、内收/外展(X₆Y₆Z₆)和屈/伸(X₇Y₇Z₇)，分别建立三个运动坐标系；末端执行器的运动坐标(X₈Y8Z8)位于手指处。

根据人体运动肢体尺寸关于人体身高的比例关系可以确定D-H模型中杆件的几何参数a_i-1和d_i，如下表1所示，绕第i个关节自由度Z_i轴旋转角度θ_i，取决于所给出的对应关节运动自由度的运动范围β_i。

表1：人体手臂DH参数表

本文使用一个单目摄像头，采用基于ResNet-50的深度卷积神经网络，用于进行三维空间中的人体姿态估计任务，并成功将网络训练至梯度收敛，并输出期望的相关骨骼关节点坐标值。如图4及图5所示。其中，代表左臂从肩关节到肘关节的向量(shoulder to elbow,left)，代表左臂从肘关节到腕关节的向量(elbow to wrist,left)，右臂同理。另外，还有几个肢体向量是在计算过程中用到的辅助向量：的方向从左肩到右肩，的方向从右肩到左肩，它们分别用来表示人体所在平面的水平方向；的方向从颈部到盆骨，用来表示人体所在平面的竖直向下方向，在左臂和右臂的角度计算中都要用到这个辅助向量；和分别是与和叉乘的结果；类似的，和分别是与以及与叉乘的结果。

以左臂为例计算公式如下。

手腕三个关节自由度由姿态传感器IMU测得，在本实施例中，采用型号为HWT901B的IMU，布放在手腕处。由DH模型可得其中已知，为待求矩阵。根据IMU坐标可求出：

其中，A、B、C、D是前述八个坐标系，即0号坐标系到7号坐标系之间的转化矩阵，T_AX、T_AY、T_AZ是IMU三个欧拉角与初始坐标系之间产生的转化矩阵。

6₆＝arccos(-a_x)

如图6所示，人机动作映射算法可以将人体手臂肩膀的三个自由度和肘部的一个自由度，映射到YUMI的前五个自由度。YUMI末端的两个自由度由，由IMU测得的人体手腕的旋内/旋外和伸/曲直接控制；主要包括工作空间匹配和遗传算法两个部分。

匹配步骤S22，对机械臂与人手臂的末端工作空间进行匹配，获取两个空间之间的转换矩阵；并基于人手臂的当前腕关节坐标数据，利用转换矩阵，计算出机械臂的目标腕关节坐标数据。

YUMI机器人单手臂的7个关节全部为转动关节，在手臂建立坐标系12所示，YUMI DH参数如下表2所示，其中a_i-1和α_i-1分别表示连杆i-1的长度和扭角，d_i和θ_i表示连杆i的偏置和关节角。

表2、YUMI DH参数表

根据表中的DH参数，可以得到人体手臂和YUMI手臂运动学模型。图12中相邻两个坐标之间的齐次变换可以用DH参数表示为:

其中，c是三角函数cos的缩写，s是sin的缩写。此外，端部执行器位置与基座位置的关系可以计算如下:

其中，n表示自由度，P＝[x,y,z,1]，用于表示相关关节的位置，表示式(1)中的齐次矩阵。

运动空间匹配是通过一定的几何变换，使得主从异构机械手或人手臂与机械臂等两个大小、形状原本完全不同的运动空间或者速度空间能够尽量吻合。目前普遍采用的主从机械手运动匹配控制方法主要有三种：关节—关节匹配、末端位姿匹配及点对点匹配。在这三种匹配方法中，关节—关节匹配方法主要用于同构主从机械手，点对点匹配方法是所有方法中使用最为普遍的，可适用于所有同构和异构主从机械手控制。在本实施例中，主从异构机械手运动匹配中位置匹配的通用方法如式所示：

式中，[X_p,Y_p,Z_p]^T为从机械手末端执行器位置；[X,Y,Z]^T为人手臂末端位置；α为绕从机械手基坐标Z轴旋转角度；S_x为沿人手臂基坐标X轴放大倍数；S_y为沿人手臂基坐标Y轴放大倍数；S_z为沿人手臂基坐标Z轴放大倍数；l_x为沿人手臂基坐标X轴平移距离；l_y为沿人手臂基坐标Y轴平移距离；l_z为沿人手臂基坐标Z轴平移距离。取人体肩关节和YUMI肩关节分别为主从基坐标系，S_x＝0.9186，S_y＝0.8476，S_z＝0.8775，α＝180°，获得变化关系如式所示。图10为经过匹配处理后的人和YUMI工作空间三维云，图7至图9为平面投影图。

计算步骤S23，基于目标腕关节坐标数据及肩关节坐标数据，并以人体小臂的方向向量作为的小臂方向向量，计算出仿人机械臂的肘关节坐标数据。

如图11所示，考虑映射模仿人体手臂构型，由于肩部位置较为固定，确定了末端位置和肘部位置，也就确定了手臂的基本形态。首先按照上文方式，将手腕的位置映射到YUMI的工作空间，考虑到人手小臂长度与YUMI小臂长度不一致，需要调整肘部的位置，进行归一化处理。我们保留小臂的方向，使小臂长度为YUMI的小臂长度。

其中l为YUMI小臂长度，O点为肩部原点，E为肘部坐标，W为腕部坐标，调整之后得到对应人手当前动作的YUMI的腕部坐标E’。

求解步骤S24，以机械臂的肘关节及肩关节角度数据为自变量，以机械臂末端工作空间为自变量取值范围，至少以机械臂腕关节位置相对目标腕关节坐标数据的计算偏差构建适应度函数，利用遗传算法搜索出最优肘关节与肩关节的角度数据，作为机械臂的关节角度数据及肘关节角度数据；及以人手臂的腕关节角度数据计算机械臂的腕关节角度数据。

即使用遗传算法得出所对应的角度逆解S，即为[x_h，y_h，z_h，x_e，y_e，z_e]→[θ₁，θ₂，θ₃，θ₄，θ₅]

利用遗传算法搜索出最优肘关节与肩关节的角度数据，以作为关节角度的近似解的步骤包括以下步骤：

初始化步骤S41，以机械臂的末端工作空间为取值区间，随机生成多个肘关节与肩关节角度解构建个体，组成初始种群；并构建适应度函数。

在机械臂工作空间S中，随机生成N个解作为种群，编码方式采用实数编码方式；即定义个体为：

ind[i]＝[θ₁，θ₂，θ₃，θ₄，θ₅]^Ti＝1，2，……，n

对于个体的适应度函数，我们利用建模中的DH矩阵，计算每个个体对应的机械臂末端位置W，肘部位置E，以及角度和再分别计算末端与肘部位置与目标位置的欧式距离：

D_h＝||H-H₀||

D_e＝||E-E₀||

在本实施例中，该适应度函数定义为：

fit＝D_h+0.03D_e+0.03Sum

即D_h为近似腕关节位置与目标腕关节坐标数据所表征位置的欧式距离，D_e为基于肘关节角度近似解计算出的近似肘关节位置与肘关节坐标数据所表征位置的欧式距离，Sum为当前个体所表征的关节角度值之和。从而在保证末端精度的前提下，对于肘部位置与目标偏离过大的解，以及解畸变从而产生的其他构型的解，适应度函数就会上升，从而将大值排除，意味着在本实施例中，适应度函数fit为基于腕关位置及肘关节位置的计算偏差进行构建，并优先考虑腕关节位置的计算偏差，“优先考虑”被配置为其权重系数至少是其他参数的权重系数的10倍以上，以在主要考虑末端动作执行精度的前提下，部分考虑肘关节的精度，即优先考虑执行动作精度的前提下，使整个仿人机械臂的动作更接近人手臂的动作，从而便于人通过视频决定下一动作，从而更好地指导机械臂的现场操作。

选择与繁殖步骤S42，基于适应度函数fit，计算每代个体适应度值，并进行高低排序，使用指数分布的形式生成随机整数对个体进行选择，并对随机选择出的两个个体基于直接融合方式繁殖下一代。

对个体适应度计算之后，按照适应度从高到低，也就是适应度函数从小到大的顺序对个体进行编号。使用指数分布的形式生成随机整数对个体进行选择。

即编号为k的个体被选择到的概率P为：

P(k)＝λe^-λk，k＝1，2，……n

从而使得适应度函数小的解被选中的可能性远大于适应度函数小的解。

对于按照上述方式选择出的两个个体，采用直接融合的方式来产生下一代：

son＝k·ind[i]+(1-k)·ind[j]

其中k为随机产生的0-1之间的实数。

交叉及变异步骤S43，依据预设比例，利用繁殖产生的子代个体替代适应度值最大的部分个体，并基于变异概率，增加部分变异生成的新个体。

这样重复产生种群个数20％的子代，替换掉适应度函数最大的后20％的个体。在替换后，每个个体以0.05的概率变异，在原来的基础上向随机方向增加一个小量作为新个体。

重复上述步骤至满足预设条件，获取适应度值最小的个体所表征的关节角度数据作为肘关节与肩关节角度的近似解。

即先获取人手臂与机械臂的末端工作空间的转换矩阵，依此获取机械臂末端的理论坐标数据，并获取肘关节的理论坐标数据；再基于遗传算法搜索出最优肘关节与肩关节的角度数据，且适应度函数主要考虑末端位置的计算偏差，即主要关注其执行末端的动作精度，以利用遗传算法搜索满足适应度函数要求的近似解进行近似执行，从而让仿人机械臂在执行动作过程中的关节角度数据无需完全参照人手臂的关节角度数据，且能近似模仿执行，从而能更好进行异构控制。

即在上述求解步骤中，先以每代个体所表征的角度数据作为肘关节角度与肩关节角度的近似解，计算出近似腕关节位置；再基于目标腕关节数据与近似腕关节位置，获取腕关节位置的计算偏差，至少以计算偏差最小为基准，获取最优解。

规划步骤S3，依据当前位置数据与预设目标物的位置数据，按照多项式路径规划方法规划出预设操作路径，并依据该预设操作路径获取用于控制机械臂的规划关节角度数据θ_R。

在本实施例中，预设目标物的位置数据为依据人手概率轨迹分布模型进行预测，具体过程如下：

首先，我们需要基于用户输入的路径，来计算下一步可能到达的目标位置，即预测最可能的目标物体，如式所示

式中G^*为最可能到达目标位置，S为初始位置，U为用户输入位置，ξ表示运动轨迹，θ为任何可能对预测有影响的参数。

考虑用户输入的轨迹ξ_S→U，G^*计算公式可以写成

我们对用户输入的轨迹ξ_S→U出现的概率做出最大熵假设，即轨迹出现概率与轨迹成本值增加呈现指数衰减趋势。

P(ξ|G)∝exp(-C_G(ξ))

C_G＝T_x²+T_y²+T_z²+R_x²+R_y²+R_z²

其中，C_G为成本函数，T_i(i＝x,y,z)和R_i(i＝x,y,z)分别为X、Y、Z轴上平移速度和旋转速度。

在实际计算中，我们使用拉普拉斯方法来逼近轨迹上的积分，可以简化为:

P(G)计算公式如下：

其中，L是机械臂末端的初始位置距离目标位置的距离，l是当前机械臂末端距离目标位置的距离。

其次，在预测出预测目标物的位置后，再利用视觉获得所有目标物体的三维位置P_G∈g；通过机械臂逆运动学来计算关节空间的位置θ_T，根据给定的速度v，来确定运动到指定位置的时间T；使用五次函数在关节空间进行插值，如下式所示。

θ(t)＝a₅×t⁵+a₄×t⁴+a₃×t³+a₂×t²+a₁×t+a₀

根据约束条件，如下式所示，来确定各项系数，从而获得时间与工作空间角度的对应关系。

根据运动捕捉的采样时间，计算机械臂关节空间位置，根据当前位置的置信度与权重进行人机融合，生成新的关节位置。

融合步骤S4，依据公式θ_S＝(1-α)θ_H+αθ_R计算机械臂的当前执行关节角度数据；0≤α≤1，为人机权重系数，且随机械臂的执行末端与预设目标物之间的置信度增加而增加。

在人机融合的过程中，需要根据抓取目标的可能性，对任务分配权重进行调整。本实施例使用分段函数来进行任务权重的调整，前半段使用线性表达式使机器可以平稳的运动，后半段使用指数型函数，快速切换人机控制权重，提高响应速度；具体，当机械臂执行末端与预测目标物的置信度在第一置信度区间内时，α为0，该第一置信度区间位于前半程中；置信度在第二置信度区间内时，α随置信度增加而线性增加；置信度在第三置信度区间内时，α随前述置信度增加而指数增加，该第三区间位于后半程中，且第一置信度区间、第二置信度区间及第三置信度区间相邻接且拼接成整个运动置信度区间，在本实施例中，置信度与协同权重关系如13所示，具体表达式如下：

其中，其中，conf为置信度，即刚开始人的权重大，引导机器进行粗略的规划，最后机器权重变大，完成精准控制；人机融合的权重α需要根据对于预测抓取目标的正确性来进行判断。置信度计算公式采用概率分布熵进行计算，具体如下式：

conf＝P(G^*|ξ_S→U)logP(G^*|ξ_S→U)

P(G^*|ξ_S→U)＝P(ξ_S→U|G^*)P(G^*)。

即在本实施例中，当预测目标正确时，置信度增加，机器权重增大，完成精准操作。预测错误时，置信度降低，人的控制权重增加，机器按照用户输入进行运动，直至预测出现新的任务目标，置信度不断增加，机器控制权重增加，完成任务。

控制步骤S5，以当前执行关节角度数据为控制指令，控制机械臂执行操作动作。

在本实施例中，人机融合可以发挥人和机器各自的优越性，远距离粗略的任务由人来做规划，近距离精确任务由机器来执行，从而可以充分利用人手臂动作与机械臂动作的优势互补，而克服对应缺点。

此外，为了避免人在判断出前方出现干涉等问题时，而执行末端与预测目标物之间的间距较短而难以对机械臂进行操控，此时，约定在检测到人手臂的关节角度的加速度超过预设阈值时，即出现紧急调整动作时，对人机权重α赋予小于0.5的数值，甚至接近0，例如取0至0.1。

如图13至图18所示，申请人进行了单目标避障抓取测试与多目标预测抓取测试。如图18所示，人和机器隔离，模拟遥操作，人根据显示屏中的目标进行操作，由摄像头捕捉人体运动信息，通过动作映射算法，将其转换成机器的动作，yumi机器控制使用伯克利大学的开源程序进行控制。采样频率约为1HZ。其中目标位置已知，图中第一排为机器单独使用五次多项式插值得到的路径进行运动，机器能到达目标位置，但是会碰倒障碍物。第二排为人单独使用动作映射方式对机械臂进行控制，机械臂可以躲避开障碍，但是很难准确的到达目标位置来进行抓取。图中第三排及第四排为人机融合控制，刚开始人的控制权重较大，控制机器躲避障碍物，待距离目标物体较近后，机器控制权重增大，准确的完成目标物体的抓取，并回到初始位置。

机器、人及二者融合控制对比如图15至图16所示，即分别为三种控制方式末端位置变化曲线，从图中可以看出，单独的机器控制大约在5s左右可以到达目标位置。人机融合控制约12s到达目标位置。单独的人控制在30s内无法到达目标位置。如图14所示为分配绘制了线性、指数和修正的三种融合控制方法的置信度与权重的变化曲线。从图中可以看出指数型最早变化到权重为1，即机器完全控制，波动幅度最大。线性变化最为平稳，但是到达机器完全控制速度最慢。修正型融合控制方式权重变化较为平稳，最后切换速度较快，允许机械臂末端在距离目标位置较近时，人对其运动进行修正。

图19所示。在平台上分别放有两个待抓取物体。一个距离机械臂末端较近，一个较远，采用不同颜色，前者为白色，而后者绿色，目标位置已知。如果没有人为干预，机械臂会自动抓取距离其末端最近的物体。机器会根据人的当前动作以及距目标物体距离，来对待抓取目标物体进行预测，确定抓取每个物体的置信度，根据置信度高低来判断待抓取物体，计算人机任务分配权重。在抓取绿色目标时，(a)前1.4s，机器权重为0，由人完全控制。1.4-2.3s，离白色物体较近，预测抓取对象为白色物体。由于人的动作不断发生变化，2.3s之后，绿色物体的置信度增加，人机协同进行控制，大约在12s左右完成对绿色目标的抓取。抓取白色物体时，(b),前1.5s，人先控制其向远离目标方向运动，1.5s后向控制其向目标物体靠近，此时为人完全控制。3s后，机器权重逐渐增加，约11s完成目标物体抓取。

在上述实施例中，首先用户根据所识别物体位置做出相应的动作U，运动捕捉系统根据用户的动作来计算手臂各个关节的角度，利用人机映射算法得到控制机器手臂的关节角度θ_H，与此同时，机器根据所识别物体的位置P，以及当前用户所做的动作U，进行意图预测，得到可能性最大的目标物体G，然后进行轨迹规划，根据运动学逆解得到θ_R，根据预测抓取目标的正确性来进行任务的权重分配，将人的控制与机器的控制相融合，获得人机融合后的控制角度θ_S，控制机械臂运动，根据机械臂的运动效果来不断改变控制角度，调整权重，直至任务完成。

一种基于人机融合的机械臂控制方法专利购买费用说明