机器人双臂协调运动控制方法、装置和电子设备

摘要

本发明提供了一种机器人双臂协调运动控制方法、装置和电子设备，通过获取主臂和从臂的运动学模型；获取主臂和从臂的坐标转换矩阵；获取主臂的运动轨迹；由主臂的运动轨迹计算从臂的运动轨迹；获取六维力传感器测得的接触力数据；根据从臂的末端位置和接触力数据计算被搬运物体的刚度；根据刚度和BP神经网络预测模型，获取自适应力控制参数；在控制从臂沿从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹；从而可避免机器人的位置跟踪误差而导致两个机械臂对被搬运物体的作用力过大或不足。

权利要求

1.一种机器人双臂协调运动控制方法，应用于双臂机器人，所述双臂机器人包括主臂和从臂，所述从臂的末端工具设置有六维力传感器，所述六维力传感器用于测量所述从臂末端工具的三个轴向力和三个轴向的力矩；其特征在于，包括步骤：A1.获取所述主臂和从臂的运动学模型；

A2.获取所述主臂和从臂的坐标转换矩阵；

A3.获取所述主臂的运动轨迹；

A4.根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；

A5.获取所述六维力传感器测得的接触力数据；

A6.根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；

A7.根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；

A8.在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹；

步骤A8中，根据以下公式修正所述从臂的实际运动轨迹：其中， 为从臂运动轨迹修正量， 为自适应力控制器的期望惯性矩阵， 为自适应力控制器的期望阻尼矩阵， 为自适应力控制器的期望刚度矩阵， 为期望位置， 为实际位置， 为预设的期望接触力， 为六维力传感器测得的接触力数据， 为自适应力控制参数；

步骤A8包括：

把所述从臂运动轨迹修正量分解为位置修正量和姿态修正量；

根据以下公式修正从臂的位置：

其中，表示序列号，用来描述机器人在不同时刻的姿态， 是k时刻的位置，是k+1时刻的位置， 是k时刻的位置修正量；

根据以下公式修正从臂的姿态：

其中， 和 是增益参数， 为k时刻的姿态修正量， 是k时刻的姿态，为 +1时刻的姿态， 为k时刻的姿态修正量， 为转轴的轴向向量，为从臂绕所述向量 旋转的角度， 为向量 的反对称矩阵，是 单位矩阵，表示机器人姿态。

2.根据权利要求1所述的机器人双臂协调运动控制方法，其特征在于，步骤A3包括：获取待搬运物体的位置信息、尺寸信息和姿态信息；

获取目的地位置信息；

根据所述待搬运物体的位置信息、尺寸信息和姿态信息和目的地位置信息，规划所述主臂的运动轨迹。

3.根据权利要求1所述的机器人双臂协调运动控制方法，其特征在于，步骤A4中，根据以下公式计算由所述主臂的运动轨迹计算所述从臂的运动轨迹：其中， 表示主臂末端工具坐标系到工件坐标系的变换矩阵， 是 的逆矩阵， 表示从臂末端工具坐标系到工件坐标系的变换矩阵,  表示主臂坐标系到主臂末端工具坐标系的变换矩阵,  表示从臂坐标系到从臂末端工具坐标系的变换矩阵,  从臂坐标系到主臂坐标系的变换矩阵。

4.根据权利要求3所述的机器人双臂协调运动控制方法，其特征在于，所述从臂与主臂的运动约束关系为 为常变换矩阵。

5.一种机器人双臂协调运动控制装置，应用于双臂机器人，所述双臂机器人包括主臂和从臂，所述从臂的末端工具设置有六维力传感器，所述六维力传感器用于测量所述从臂末端工具的三个轴向力和三个轴向的力矩；其特征在于，包括：第一获取模块，用于获取所述主臂和从臂的运动学模型；

第二获取模块，用于获取所述主臂和从臂的坐标转换矩阵；

第三获取模块，用于获取所述主臂的运动轨迹；

第一执行模块，用于根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；

第四获取模块，用于获取所述六维力传感器测得的接触力数据；

第一计算模块，用于根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；

第五获取模块，用于根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；

第二执行模块，用于在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹；

所述第二执行模块，根据以下公式修正所述从臂的实际运动轨迹：其中， 为从臂运动轨迹修正量， 为自适应力控制器的期望惯性矩阵， 为自适应力控制器的期望阻尼矩阵， 为自适应力控制器的期望刚度矩阵， 为期望位置， 为实际位置， 为预设的期望接触力， 为六维力传感器测得的接触力数据， 为自适应力控制参数；

所述第二执行模块在修正从臂的实际运动轨迹时，把所述从臂运动轨迹修正量分解为位置修正量和姿态修正量；

根据以下公式修正从臂的位置：

其中，表示序列号，用来描述机器人在不同时刻的姿态， 是k时刻的位置，是k+1时刻的位置， 是k时刻的位置修正量；

根据以下公式修正从臂的姿态：

其中， 和 是增益参数， 为k时刻的姿态修正量， 是k时刻的姿态，为 +1时刻的姿态， 为k时刻的姿态修正量， 为转轴的轴向向量，为从臂绕所述向量 旋转的角度， 为向量 的反对称矩阵，是 单位矩阵，表示机器人姿态。

6.根据权利要求5所述的机器人双臂协调运动控制装置，其特征在于，所述第三获取模块在获取所述主臂的运动轨迹时，获取待搬运物体的位置信息、尺寸信息和姿态信息；

获取目的地位置信息；

根据所述待搬运物体的位置信息、尺寸信息和姿态信息和目的地位置信息，规划所述主臂的运动轨迹。

7.一种电子设备，其特征在于，包括处理器和存储器，所述存储器中存储有计算机程序，所述处理器通过调用所述存储器中存储的所述计算机程序，用于执行权利要求1-4任一项所述的机器人双臂协调运动控制方法。

说明书

机器人双臂协调运动控制方法、装置和电子设备

技术领域

[0001] 本发明涉及机器人控制技术领域，尤其涉及一种机器人双臂协调运动控制方法、装置和电子设备。

背景技术

[0002] 在一些危险的场合，如排爆、进入有险情的灾区进行救灾，为了保护救灾人员的生命安全，往往会使用双臂机器人进入现场进行作业，并由救灾人员远程对机器人进行遥控操作。

[0003] 在救灾现场，常常需要双臂机器人的两个机械臂协同搬运一些物品，目前，在控制两个机械臂协同搬运物品时，一般是把其中一个机械臂设置为主臂，把另一个机械臂设置为从臂，然后对主臂的运动轨迹进行规划，再根据运动约束关系和主臂的运动轨迹计算出从臂的运动轨迹，从而实现两个机械臂的协同工作。

[0004] 然而，机器人的位置跟踪精度是有限的，会存在一定的跟踪误差，容易导致两个机械臂对被搬运物体的作用力过大或不足，若作用力过大则容易损坏物体，若作用力不足则可能搬不起物体或使物体在搬运途中掉落。

发明内容

[0005] 鉴于上述现有技术的不足之处，本申请实施例的目的在于提供一种机器人双臂协调运动控制方法、装置和电子设备，可避免机器人的位置跟踪误差而导致两个机械臂对被搬运物体的作用力过大或不足。

[0006] 第一方面，本申请实施例提供一种机器人双臂协调运动控制方法，应用于双臂机器人，所述双臂机器人包括主臂和从臂，所述从臂的末端工具设置有六维力传感器，所述六维力传感器用于测量所述从臂末端工具的三个轴向力和三个轴向的力矩；包括步骤：

[0007] A1.获取所述主臂和从臂的运动学模型；

[0008] A2.获取所述主臂和从臂的坐标转换矩阵；

[0009] A3.获取所述主臂的运动轨迹；

[0010] A4.根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；

[0011] A5.获取所述六维力传感器测得的接触力数据；

[0012] A6.根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；

[0013] A7.根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；

[0014] A8.在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹。

[0015] 所述的机器人双臂协调运动控制方法中，步骤A3包括：

[0016] 获取待搬运物体的位置信息、尺寸信息和姿态信息；

[0017] 获取目的地位置信息；

[0018] 根据所述待搬运物体的位置信息、尺寸信息和姿态信息和目的地位置信息，规划所述主臂的运动轨迹。

[0019] 所述的机器人双臂协调运动控制方法中，步骤A4中，根据以下公式计算由所述主臂的运动轨迹计算所述从臂的运动轨迹：

[0020]

[0021] 其中， 表示主臂末端工具坐标系到工件坐标系的变换矩阵， 是 的逆矩阵， 表示从臂末端工具坐标系到工件坐标系的变换矩阵,  表示主臂坐标系到主臂末端工具坐标系的变换矩阵,  表示从臂坐标系到从臂末端工具坐标系的变换矩阵,  从臂坐标系到主臂坐标系的坐标系的变换矩阵。

[0022] 所述的机器人双臂协调运动控制方法中，所述从臂与主臂的运动约束关系为为常变换矩阵。

[0023] 所述的机器人双臂协调运动控制方法中，步骤A8中，根据以下公式修正所述从臂的实际运动轨迹：

[0024]

[0025] 其中， 为从臂运动轨迹修正量， 为自适应力控制器的期望惯性矩阵， 为自适应力控制器的期望阻尼矩阵， 为自适应力控制器的期望刚度矩阵， 为期望位置，为实际位置， 为预设的期望接触力， 为六维力传感器测得的接触力数据， 为自适应力控制参数。

[0026] 所述的机器人双臂协调运动控制方法中，步骤A8包括：

[0027] 把所述从臂运动轨迹修正量分解为位置修正量和姿态修正量；

[0028] 根据以下公式修正从臂的位置：

[0029]

[0030] 其中，表示序列号，用来描述机器人在不同时刻的姿态， 是k时刻的位置，是k+1时刻的位置， 是k时刻的位置修正量；

[0031] 根据以下公式修正从臂的姿态：

[0032]

[0033]

[0034]

[0035]

[0036] 其中， 和 是增益参数， 为k时刻的姿态修正量， 是k时刻的姿态， 为 +1时刻的姿态， 为k时刻的姿态修正量， 为转轴的轴向向量，为从臂绕所述向量 旋转的角度， 为向量 的反对称矩阵，  是 单位矩阵，表示机器人姿态。

[0037] 第二方面，本申请实施例提供一种机器人双臂协调运动控制装置，包括：

[0038] 第一获取模块，用于获取所述主臂和从臂的运动学模型；

[0039] 第二获取模块，用于获取所述主臂和从臂的坐标转换矩阵；

[0040] 第三获取模块，用于获取所述主臂的运动轨迹；

[0041] 第一执行模块，用于根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；

[0042] 第四获取模块，用于获取所述六维力传感器测得的接触力数据；

[0043] 第一计算模块，用于根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；

[0044] 第五获取模块，用于根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；

[0045] 第二执行模块，用于在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹。

[0046] 所述的机器人双臂协调运动控制装置中，所述第三获取模块在获取所述主臂的运动轨迹时，

[0047] 获取待搬运物体的位置信息、尺寸信息和姿态信息；

[0048] 获取目的地位置信息；

[0049] 根据所述待搬运物体的位置信息、尺寸信息和姿态信息和目的地位置信息，规划所述主臂的运动轨迹。

[0050] 所述的机器人双臂协调运动控制装置中，所述第二执行模块，根据以下公式修正所述从臂的实际运动轨迹：

[0051]

[0052] 其中， 为从臂运动轨迹修正量， 为自适应力控制器的期望惯性矩阵， 为自适应力控制器的期望阻尼矩阵， 为自适应力控制器的期望刚度矩阵， 为期望位置，为实际位置， 为预设的期望接触力， 为六维力传感器测得的接触力数据， 为自适应力控制参数。

[0053] 第三方面，本申请实施例提供一种电子设备，包括处理器和存储器，所述存储器中存储有计算机程序，所述处理器通过调用所述存储器中存储的所述计算机程序，用于执行所述的机器人双臂协调运动控制方法。

[0054] 有益效果：

[0055] 本申请实施例提供的一种机器人双臂协调运动控制方法、装置和电子设备，通过获取所述主臂和从臂的运动学模型；获取所述主臂和从臂的坐标转换矩阵；获取所述主臂的运动轨迹；根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；获取所述六维力传感器测得的接触力数据；根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹；从而可避免机器人的位置跟踪误差而导致两个机械臂对被搬运物体的作用力过大或不足。

附图说明

[0056] 图1为本申请实施例提供的机器人双臂协调运动控制方法的流程图。

[0057] 图2为本申请实施例提供的机器人双臂协调运动控制装置的模块图。

[0058] 图3为本申请实施例提供的电子设备的结构示意图。

[0059] 图4为主臂、从臂和物体之间的位置关系示意图。

[0060] 图5为基于位置的自适应力控制模型的示意图。

[0061] 图6为从臂末端工具接触力的仿真结果示意图。

具体实施方式

[0062] 下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

[0063] 应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

[0064] 请参阅图1，本申请实施例提供的一种机器人双臂协调运动控制方法，应用于双臂机器人，双臂机器人包括主臂和从臂，从臂的末端工具设置有六维力传感器，六维力传感器用于测量从臂末端工具的三个轴向力和三个轴向的力矩；其特征在于，包括步骤：

[0065] A1.获取主臂和从臂的运动学模型；

[0066] A2.获取主臂和从臂的坐标转换矩阵；

[0067] A3.获取主臂的运动轨迹；

[0068] A4.根据主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由主臂的运动轨迹计算从臂的运动轨迹；

[0069] A5.获取六维力传感器测得的接触力数据；

[0070] A6.根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；

[0071] A7.根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；

[0072] A8.在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹。

[0073] 该方法在获取主臂的运动轨迹后，根据从臂与主臂的运动约束关系计算从臂的运动轨迹，在控制从臂沿该从臂的运动轨迹运动时，根据六维力传感器事实测量的接触力数据，并根据接触力数据和从臂的末端位置计算被搬运物体的实际刚度，以输入BP神经网络预测模型来获取相匹配的自适应力控制参数，基于位置的自适应力控制，适当修正了从臂的轨迹，从而达到了作用力控制的目的，避免机器人的位置跟踪误差而导致两个机械臂对被搬运物体的作用力过大或不足。

[0074] 实际上，在其它条件相同的情况下，被搬运物体的刚度不同的时候，机械臂对物体的作用力也应该不同，需要与被搬运物体的实际刚度相匹配，从而避免压坏被搬运物体和损坏机械臂本身，该方法能够根据不同刚度的物体自动调节自适应力控制参数，最终实现机械臂与不同刚度的物体接触的自适应力控制。

[0075] 步骤A1中，可通过D-H坐标变换方法分别建立两条机械臂的运动学模型，并以此求解机械臂正运动学方程和逆运动学方程作为机器人控制的基础。

[0076] 步骤A2中，主臂和从臂的坐标转换矩阵可通过预先标定得到，标定方法如下：通过空间中不在同一直线上的三个空间点P1、P2、P3，获取该三个点在主臂坐标系下的坐标和在从臂坐标系下的坐标，以P2为原点，(P1-P2)得到的向量为X轴，(P3-P2)得到的向量为Y轴，X轴叉乘Y轴得到Z轴，由计算得到的X轴、Y轴和Z轴构建一个空间坐标系作为两条机械臂的公共坐标系S，通过主臂的正运动学计算得到主臂座坐标系B1到公共坐标系S的变换矩阵为，通过从臂的正运动学计算得到从臂座坐标系B2到公共坐标系S的变换矩阵为 ，由此来推导出主臂座坐标系到从臂座坐标系的变换矩阵关系为 。

[0077] 在一些实施方式中，步骤A3包括：

[0078] A301.获取待搬运物体的位置信息、尺寸信息和姿态信息；

[0079] A302.获取目的地位置信息；

[0080] A303.根据待搬运物体的位置信息、尺寸信息和姿态信息和目的地位置信息，规划主臂的运动轨迹。

[0081] 例如，对于设置有双目视觉系统的机器人，可通过双目视觉系统获取待搬运物体的位置信息、尺寸信息和姿态信息；在A302中，可采集现场图像并发送至远端的机器人控制器，由操作人员根据现场图像选定物体搬运的目的地位并生成目的地位置信息发回机器人，或者在采集现场图像后通过图像分析方法获取适合放置待搬运物体的区域，并选取其中离物体初始位置最近的区域作为目的地，获取该区域的中心位置信息作为目的地位置信息；在A303中，可通过基于采样的运动规划方法，得到主臂的路点，该主臂的路点表示主臂运动轨迹上的点。 以上只是举例说明，步骤A301-A303的具体实施方式不限于此。

[0082] 在本实施例中，步骤A4中，根据以下公式计算由主臂的运动轨迹计算从臂的运动轨迹：

[0083]

[0084] 其中， 表示主臂末端工具坐标系到工件坐标系的变换矩阵， 是 的逆矩阵， 表示从臂末端工具坐标系到工件坐标系的变换矩阵,  表示主臂坐标系到主臂末端工具坐标系的变换矩阵,  表示从臂坐标系到从臂末端工具坐标系的变换矩阵,  从臂坐标系到主臂坐标系的坐标系的变换矩阵。

[0085] 见图4，为搬运过程中主臂90、从臂91和物体92之间的位置关系示意图，其中，主臂90的坐标系为{R1}，从臂91的坐标系为{R2}，主臂90的末端工具坐标系为{E1}，从臂91的末端工具坐标系为{E2}，物体92的工件坐标系为{P}，主臂和从臂在协同运动时有如下关系：

[0086]

[0087] 其中， 表示主臂坐标系到工件坐标系的变换矩阵；通过上式可推导得到。

[0088] 进一步的，由于在搬运过程中，主臂90的末端工具、从臂91的末端工具、物体92之间是相对静止的（即相对位置固定），且物体92的尺寸是不变的，因此，主臂90末端工具坐标系到从臂91末端工具坐标系之间的转换矩阵是不变的且为常变换矩阵，该常变换矩阵可预先计算得到；所以，从臂与主臂的运动约束关系为 为常变换矩阵。由于 已经通过预先标定得到，因此 与 之间的转换关系是确定的，从而可根据该转换关系由主臂的运动轨迹计算从臂的运动轨迹。

[0089] 在步骤A6中，可根据以下公式计算被搬运物体的刚度：

[0090]

[0091] 其中， 为接触物体的刚度， 为在i时刻从臂末端的位置， 为在i时刻从臂末端所受到的接触力（等于在i时刻六维力传感器测得的接触力数据）。

[0092] 在一些实施方式中，见图5，可通过图5中的基于位置的自适应力控制模型（该模型为自适应自适应力控制模型）进行从臂的实际运动轨迹的修正，该基于位置的自适应力控制模型的数学表达式为：

[0093]

[0094] 其中， 为自适应力控制器的期望惯性矩阵， 为自适应力控制器的期望阻尼矩阵， 为自适应力控制器的期望刚度矩阵， 为期望位置， 为实际位置， 为预设的期望接触力， 为六维力传感器测得的接触力数据， 为自适应力控制参数。其中，， 、 、 分别为三个轴向的作用力， 、 、分别为三个轴向的力矩。

[0095] 该基于位置的自适应力控制模型中，引入了实时的力偏差反馈闭环 和自适应力控制参数 ，一般地，被搬运物体刚度越大，自适应力控制参数 应该越小，这样才能保证不损坏机械臂。根据该原则，可设计被搬运物体的不同刚度和自适应力控制参数的映射关系数据集，并把该数据集合作为BP神经网络训练的样本数据，经过BP神经网络训练后，生成BP神经网络预测模型。在实际工作过程中，把步骤A6中计算得到的刚度值输入到该BP神经网络预测模型中，得到对应的自适应力控制参数 。因此，无需根据被搬运物体的实际刚度预先设置（或修改）固定的自适应力控制参数 ，不但使用方便，且可适应不同刚度的物体的搬运，适用性强。

[0096] 通过对基于位置的自适应力控制模型的数学表达式进行积分可得到以下计算公式，可根据该公式修正从臂的实际运动轨迹：

[0097]

[0098] 其中， 为从臂运动轨迹修正量。

[0099] 在该基于位置的自适应力控制模型中，在自适应力控制中引入自适应调整刚度参数，在自适应力控制器中增加实时的力偏差反馈闭环，根据这个力偏差反馈来在线调整自适应力控制器的刚度参数，从而在无需修改任何参数的条件下能够适应不同刚度的物体的表面接触力控制。

[0100] 进一步的，步骤A8包括：

[0101] A801.把从臂运动轨迹修正量 分解为位置修正量 和姿态修正量 ；

[0102] 其中， 为6*1大小的向量，包括6个参数，分别为三个方向的坐标修正量和三个方向的姿态角度修正量，可从其中把三个方向的坐标修正量提取出来形成位置修正量（即 为3*1大小的向量），从其中把三个方向的姿态角度修正量提取出来形成姿态修正量（即 为3*1大小的向量）；

[0103] A802.根据以下公式修正从臂的位置：

[0104]

[0105] 其中，表示序列号，用来描述机器人在不同时刻的位姿， 是k时刻的位置，是k+1时刻的位置， 是k时刻的位置修正量；

[0106] A803.根据以下公式修正从臂的姿态：

[0107]

[0108]

[0109]

[0110]

[0111] 其中， 和 是增益参数， 为k时刻的姿态修正量(用欧拉角表示)，是k时刻的姿态（用旋转矩阵表示）， 为 +1时刻的姿态（用旋转矩阵表示）， 为k时刻的姿态修正量（用旋转矩阵表示）， 为转轴的轴向向量，为从臂绕所述向量 旋转的角度， 为向量 的反对称矩阵，  是 单位矩阵， 表示机器人姿态（用旋转矩阵表示）。

[0112] 根据上述方法进行如下仿真：

[0113] 双臂机器人搬运工件的末端期望接触力设置为20N，自适应力控制器的期望惯性矩阵 ，自适应力控制器的期望阻尼矩阵 ，自适应力控制器的期望刚度矩阵 ，此处的 是 单位矩阵，经仿真，得到的搬运过程中的法向接触力如图6所示，可看到，实际接触力的峰值与期望接触力的偏差为 ，最大超调量为，接触力的稳态偏差为 。从结果可以看出该自适应自适应力控制器
具有优秀的力控制效果。

[0114] 由上可知，该机器人双臂协调运动控制方法，通过获取所述主臂和从臂的运动学模型；获取所述主臂和从臂的坐标转换矩阵；获取所述主臂的运动轨迹；根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；获取所述六维力传感器测得的接触力数据；根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹；从而可避免机器人的位置跟踪误差而导致两个机械臂对被搬运物体的作用力过大或不足。

[0115] 请参阅图2，本申请实施例提供一种机器人双臂协调运动控制装置，包括第一获取模块1、第二获取模块2、第三获取模块3、第一执行模块4、第四获取模块5、第一计算模块6、第五获取模块7、第二执行模块8；

[0116] 其中，第一获取模块1，用于获取主臂和从臂的运动学模型；

[0117] 其中，第二获取模块2，用于获取主臂和从臂的坐标转换矩阵；

[0118] 其中，第三获取模块3，用于获取主臂的运动轨迹；

[0119] 其中，第一执行模块4，用于根据主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由主臂的运动轨迹计算从臂的运动轨迹；

[0120] 其中，第四获取模块5，用于获取六维力传感器测得的接触力数据；

[0121] 其中，第一计算模块6，用于根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；

[0122] 其中，第五获取模块7，用于根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；

[0123] 其中，第二执行模块8，用于在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹。

[0124] 在一些实施方式中，第三获取模块3在获取主臂的运动轨迹时，

[0125] 获取待搬运物体的位置信息、尺寸信息和姿态信息；

[0126] 获取目的地位置信息；

[0127] 根据待搬运物体的位置信息、尺寸信息和姿态信息和目的地位置信息，规划主臂的运动轨迹。

[0128] 在一些实施方式中，第一执行模块4根据以下公式计算由主臂的运动轨迹计算从臂的运动轨迹：

[0129]

[0130] 其中， 表示主臂末端工具坐标系到工件坐标系的变换矩阵， 是 的逆矩阵， 表示从臂末端工具坐标系到工件坐标系的变换矩阵,  表示主臂坐标系到主臂末端工具坐标系的变换矩阵,  表示从臂坐标系到从臂末端工具坐标系的变换矩阵,  从臂坐标系到主臂坐标系的坐标系的变换矩阵。

[0131] 进一步的，从臂与主臂的运动约束关系为： 为常变换矩阵。

[0132] 在一些实施方式中，第一计算模块6根据以下公式计算被搬运物体的刚度：

[0133]

[0134] 其中， 为接触物体的刚度， 为在i时刻从臂末端的位置， 为在i时刻从臂末端所受到的接触力（等于在i时刻六维力传感器测得的接触力数据）。

[0135] 在一些实施方式中，步骤A7包括：把第一计算模块6计算得到的刚度值输入到BP神经网络预测模型中，得到对应的自适应力控制参数。

[0136] 可设计被搬运物体的不同刚度和自适应力控制参数的映射关系数据集，并把该数据集合作为BP神经网络训练的样本数据，经过BP神经网络训练后，生成BP神经网络预测模型。

[0137] 在一些实施方式中，第二执行模块8，根据以下公式修正从臂的实际运动轨迹：

[0138]

[0139] 其中， 为从臂运动轨迹修正量， 为自适应力控制器的期望惯性矩阵， 为自适应力控制器的期望阻尼矩阵， 为自适应力控制器的期望刚度矩阵， 为期望位置，为实际位置， 为预设的期望接触力， 为六维力传感器测得的接触力数据， 为自适应力控制参数。

[0140] 进一步的，第二执行模块8在修正从臂的实际运动轨迹时，

[0141] 把从臂运动轨迹修正量分解为 位置修正量 和姿态修正量 ；

[0142] 根据以下公式修正从臂的位置：

[0143]

[0144] 其中，表示序列号，用来描述机器人在不同时刻的姿态， 是k时刻的位置，是k+1时刻的位置， 是k时刻的位置修正量；

[0145] 根据以下公式修正从臂的姿态：

[0146]

[0147]

[0148]

[0149]

[0150] 其中， 和 是增益参数， 为k时刻的姿态修正量(用欧拉角表示)，是k时刻的姿态（用旋转矩阵表示）， 为 +1时刻的姿态（用旋转矩阵表示）， 为k时刻的姿态修正量（用旋转矩阵表示）， 为转轴的轴向向量，为从臂绕所述向量 旋转的角度， 为向量 的反对称矩阵，  是 单位矩阵， 表示机器人姿态（用旋转矩阵表示）。

[0151] 由上可知，该机器人双臂协调运动控制装置，通过获取所述主臂和从臂的运动学模型；获取所述主臂和从臂的坐标转换矩阵；获取所述主臂的运动轨迹；根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；获取所述六维力传感器测得的接触力数据；根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹；从而可避免机器人的位置跟踪误差而导致两个机械臂对被搬运物体的作用力过大或不足。

[0152] 请参阅图3，本申请实施例还提供一种电子设备100，包括处理器101和存储器102，存储器102中存储有计算机程序，处理器101通过调用存储器102中存储的计算机程序，用于执行上述的机器人双臂协调运动控制方法。

[0153] 其中，处理器101与存储器102电性连接。处理器101是电子设备100的控制中心，利用各种接口和线路连接整个电子设备的各个部分，通过运行或调用存储在存储器102内的计算机程序，以及调用存储在存储器102内的数据，执行电子设备的各种功能和处理数据，从而对电子设备进行整体监控。

[0154] 存储器102可用于存储计算机程序和数据。存储器102存储的计算机程序中包含有可在处理器中执行的指令。计算机程序可以组成各种功能模块。处理器101通过调用存储在存储器102的计算机程序，从而执行各种功能应用以及数据处理。

[0155] 在本实施例中，电子设备100中的处理器101会按照如下的步骤，将一个或一个以上的计算机程序的进程对应的指令加载到存储器102中，并由处理器101来运行存储在存储器102中的计算机程序，从而实现各种功能：获取所述主臂和从臂的运动学模型；获取所述主臂和从臂的坐标转换矩阵；获取所述主臂的运动轨迹；根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；获取所述六维力传感器测得的接触力数据；根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹。

[0156] 由上可知，该电子设备，通过获取所述主臂和从臂的运动学模型；获取所述主臂和从臂的坐标转换矩阵；获取所述主臂的运动轨迹；根据所述主臂和从臂的运动学模型、主臂和从臂的坐标转换矩阵以及从臂与主臂的运动约束关系，由所述主臂的运动轨迹计算所述从臂的运动轨迹；获取所述六维力传感器测得的接触力数据；根据所述从臂的末端位置和所述接触力数据计算被搬运物体的刚度；根据所述刚度和BP神经网络预测模型，获取自适应力控制参数；在控制所述从臂沿所述从臂的运动轨迹运动时，根据预设的期望接触力、测得的接触力数据和所述自适应力控制参数，采用基于位置的自适应力控制方法来修正从臂的实际运动轨迹；从而可避免机器人的位置跟踪误差而导致两个机械臂对被搬运物体的作用力过大或不足。

[0157] 综上所述，虽然本发明已以优选实施例揭露如上，但上述优选实施例并非用以限制本发明，本领域的普通技术人员，在不脱离本发明的精神和范围内，均可作各种更动与润饰，其方案与本发明实质上相同。