双臂机器人控制方法、装置、双臂机器人和可读存储介质与流程

1.本发明涉及机器人技术领域，尤其涉及一种双臂机器人控制方法、装置、双臂机器人和可读存储介质。


背景技术：

2.单臂机器人的操作任务简单、灵巧性不足，与传统单个机械臂相比，双臂机器人更为灵活，可以执行较为复杂的任务，例如，在双臂机器人利用第一末端和第二末端夹持负载时，控制负载随着交互人员施加的交互力作跟随运动，或者，在双臂机器人和其他设备进行协同工作，控制负载随着其他设备施加的交互力作跟随运动等。
3.现有的双臂机器人在控制负载随着外部施加的交互力作跟随运动时，通常在机器人的双臂分别建立坐标系，对双臂分别进行控制，控制过程复杂，导致双臂机器人双臂的协同性较差，可能使双臂机器人夹持的负载跌落。


技术实现要素：

4.鉴于上述问题，本技术提出一种双臂机器人控制方法、装置、双臂机器人和可读存储介质。
5.本技术提出一种双臂机器人控制方法，所述方法包括：
6.在双臂机器人夹持负载时，将双臂末端的六维力转换至负载坐标系下，所述负载坐标系为以所述负载的中心为原点建立的坐标系；
7.根据所述负载坐标系下的各个末端的六维力确定各个末端对应的内力、外力和外力矩，所述各个末端对应的内力用于夹持负载，所述各个末端对应的外力和外力矩用于控制所述双臂机器人夹持负载跟随所述外力和外力矩运动；
8.根据所述负载坐标系下的各个末端对应的内力、外力和外力矩控制所述双臂机器人的双臂做协同运动。
9.本技术所述的双臂机器人控制方法，所述将双臂末端的六维力转换至负载坐标系下，包括：
10.根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵将第一末端的六维力转换至所述负载坐标系下；
11.根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵将第二末端的六维力转换至所述负载坐标系下。
12.本技术所述的双臂机器人控制方法，所述根据所述负载坐标系下的各个末端对应的内力、外力和外力矩控制所述双臂机器人的双臂做协同运动，包括：
13.根据所述第一末端对应的内力确定第一末端坐标系下的所述第一末端的第一内力控制调整量和第二末端坐标系下的第二末端的第二内力控制调整量；
14.根据所述第一末端对应的外力确定所述第一末端坐标系下的第一外力控制调整量和所述第二末端坐标系下的第二外力控制调整量；
15.根据所述第一末端对应的外力矩确定所述第一末端坐标系下的第一位姿调整量和所述第二末端坐标系下的第二位姿调整量；
16.根据所述第一内力控制调整量、所述第一外力控制调整量和所述第一位姿调整量控制所述双臂机器人的第一机械臂，同时根据所述第二内力控制调整量、所述第二外力控制调整量和所述第二位姿调整量控制所述双臂机器人的第二机械臂。
17.本技术所述的双臂机器人控制方法，所述根据所述第一末端对应的内力确定第一末端坐标系下的所述第一末端的第一内力控制调整量和第二末端坐标系下的第二末端的第二内力控制调整量，包括：
18.根据所述第一末端对应的内力确定所述负载坐标系下的第一内力调整量；
19.根据所述第一内力调整量确定所述负载坐标系下的所述第二末端的第二内力调整量；
20.根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述第一内力调整量在所述第一末端坐标系下的第一内力控制调整量；
21.根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述第二内力调整量在所述第二末端坐标系下的第二内力控制调整量。
22.本技术所述的双臂机器人控制方法，所述根据所述第一末端对应的外力确定所述第一末端坐标系下的第一外力控制调整量和所述第二末端坐标系下的第二外力控制调整量，包括：
23.根据所述第一末端对应的外力确定所述负载坐标系下的第一外力调整量；
24.根据所述第一外力调整量确定所述负载坐标系下的所述第二末端的第二外力调整量；
25.根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述第一外力调整量在所述第一末端坐标系下的第一外力控制调整量；
26.根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述第二外力调整量在所述第二末端坐标系下的第二外力控制调整量。
27.本技术所述的双臂机器人控制方法，所述第一位姿调整量包括所述第一末端坐标系下的第一角度调整量和第一位置调整量，第二位姿调整量包括所述第二末端坐标系下的第二角度调整量和第二位置调整量，所述根据所述第一末端对应的外力矩确定所述第一末端坐标系下的第一位姿调整量和所述第二末端坐标系下的第二位姿调整量，包括：
28.根据所述第一末端对应的外力矩确定负载角度变化量；
29.根据所述负载角度变化量确定姿态旋转矩阵；
30.确定第一末端坐标系原点和第二末端坐标系原点之间的距离；
31.根据所述距离确定所述第一末端坐标系原点在所述负载坐标系下的第一位置坐标；
32.根据所述距离确定所述第二末端坐标系原点在所述负载坐标系下的第二位置坐标；
33.根据所述负载角度变化量、所述第一位置坐标和所述姿态旋转矩阵确定所述第一末端坐标系下的所述第一角度调整量和所述第一位置调整量；
34.根据所述负载角度变化量、所述第二位置坐标和所述姿态旋转矩阵确定所述第二
末端坐标系下的所述第二角度调整量和所述第二位置调整量。
35.本技术所述的双臂机器人控制方法，所述根据所述负载角度变化量、所述第一位置坐标和所述姿态旋转矩阵确定所述第一末端坐标系下的所述第一角度调整量和所述第一位置调整量，包括：
36.根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述负载角度变化量在所述第一末端坐标系下的所述第一角度调整量；
37.根据所述姿态旋转矩阵预测所述第一位置坐标对应的姿态调整后的第一预测位置坐标；
38.根据所述第一预测位置坐标和所述第一位置坐标确定第一末端位置变化量；
39.根据所述第一坐标系旋转矩阵确定所述第一末端位置变化量在所述第一末端坐标系下的所述第一位置调整量。
40.本技术所述的双臂机器人控制方法，所述根据所述负载角度变化量、所述第二位置坐标和所述姿态旋转矩阵确定所述第二末端坐标系下的所述第二角度调整量和所述第二位置调整量，包括：
41.根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述负载角度变化量在所述第二末端坐标系下的所述第二角度调整量；
42.根据所述姿态旋转矩阵预测所述第二位置坐标对应的姿态调整后的第二预测位置坐标；
43.根据所述第二预测位置坐标和所述第二位置坐标确定第二末端位置变化量；
44.根据所述第二坐标系旋转矩阵确定所述第二末端位置变化量在所述第二末端坐标系下的所述第二位置调整量。
45.本技术还提出一种双臂机器人控制装置，所述装置包括：
46.坐标系转换模块，用于在双臂机器人夹持负载时，将双臂末端的六维力转换至负载坐标系下，所述负载坐标系为以所述负载的中心为原点建立的坐标系；
47.内外力确定模块，用于根据所述负载坐标系下的各个末端的六维力确定各个末端对应的内力、外力和外力矩，所述各个末端对应的内力用于夹持负载，所述各个末端对应的外力和外力矩用于控制所述双臂机器人夹持负载跟随所述外力和外力矩运动；
48.协同控制模块，用于根据所述负载坐标系下的各个末端对应的内力、外力和外力矩控制所述双臂机器人的双臂做协同运动。
49.本技术还提出一种双臂机器人，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行本技术所述的双臂机器人控制方法。
50.本技术还提出一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行本技术所述的双臂机器人控制方法。
51.本技术在双臂机器人夹持负载时，将双臂末端的六维力转换至负载坐标系下，所述负载坐标系为以所述负载的中心为原点建立的坐标系；根据所述负载坐标系下的各个末端的六维力确定各个末端对应的内力、外力和外力矩，所述各个末端对应的内力用于夹持负载，所述各个末端对应的外力和外力矩用于控制所述双臂机器人夹持负载跟随所述外力和外力矩运动；根据所述负载坐标系下的各个末端对应的内力、外力和外力矩控制所述双臂机器人的双臂做协同运动。由于将双臂机器人的各个末端的六维力转换至负载坐标系
下，所有计算在同一坐标系下进行，可以降低计算过程的复杂性，提高计算速度，进而提高双臂协同工作的协同性，有效降低负载跌落的风险。
附图说明
52.为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
53.图1示出了本技术实施例提出的一种双臂机器人控制方法的流程示意图；
54.图2示出了本技术实施例提出的三个坐标系的示意图；
55.图3示出了本技术实施例提出一种双臂机器人的双臂调整量确定方法的流程示意图；
56.图4示出了本技术实施例提出一种双臂机器人的双臂内力调整量确定方法的流程示意图；
57.图5示出了本技术实施例提出一种双臂机器人的双臂外力调整量确定方法的流程示意图；
58.图6示出了本技术实施例提出一种双臂机器人的双臂外力矩调整量确定方法的流程示意图；
59.图7示出了本技术实施例提出一种双臂机器人的第一末端对应的机械臂位置调整量确定方法的流程示意图；
60.图8示出了本技术实施例提出一种双臂机器人的第二末端对应的机械臂位置调整量确定方法的流程示意图；
61.图9示出了本技术实施例提出的一种双臂机器人控制装置的结构示意图；
62.图10示出了本技术实施例提出的一种双臂机器人的结构示意图。
63.主要元件符号说明：
64.10
‑
双臂机器人控制装置；11
‑
坐标系转换模块；12
‑
内外力确定模块；13
‑
协同控制模块；100
‑
双臂机器人；110
‑
存储器；120
‑
处理器。
具体实施方式
65.下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
66.通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
67.在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
68.此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
69.除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
70.本技术公开的双臂机器人控制方法，将所述双臂机器人的各个末端的六维力转换至负载坐标系下，在负载坐标系下，确定各个末端的六维力对应的内力(用于夹持负载)、外力(用于控制机器人夹持负载跟随外力运动)和外力矩(用于控制机器人夹持负载跟随外力矩转动)，根据所述双臂机器人的第一末端对应的内力、外力和外力矩确定负载坐标系下的第一内力调整量、第二内力调整量、第一外力调整量、第二外力调整量和负载角度变化量，再将第一内力调整量和第一外力调整量分别转换至第一末端坐标系下以获得第一末端的第一内力控制调整量和第一末端的第一外力控制调整量，再将第二内力调整量和第二外力调整量分别转换至第二末端坐标系下以获得第二末端的第二内力控制调整量和第二末端的第二外力控制调整量，再根据负载角度变化量确定第一末端坐标系下第一末端的第一位姿调整量和第二末端的第二位姿调整量，最后，根据所述第一内力控制调整量、第一外力控制调整量和所述第一位姿调整量控制所述双臂机器人的第一机械臂，同时根据所述第二内力控制调整量、所述第二外力控制调整量和所述第二位姿调整量控制所述双臂机器人的第二机械臂。
71.本技术一方面，将双臂机器人的各个末端的六维力转换至负载坐标系下，在负载坐标系下进行计算确定负载坐标系下用于控制双臂的各个控制量，由于计算在同一坐标系下进行，降低计算过程的复杂性，提高计算速度，进而提高双臂协同工作的协同性，有效降低负载跌落的风险。
72.另一方面，将负载坐标系下的第一内力调整量和第一外力调整量分别转换至第一末端坐标系下以获得第一末端的第一内力控制调整量和第一末端的第一外力控制调整量，将负载坐标系下的第二内力调整量和第二外力调整量分别转换至第二末端坐标系下以获得第二末端的第二内力控制调整量和第二末端的第二外力控制调整量，实现双臂机器人夹持负载跟随外力作平移运动，提高双臂机器人的柔顺性。
73.再一方面，根据负载坐标系下的负载角度变化量确定第一末端坐标系下第一末端的第一位姿调整量和第二末端的第二位姿调整量，实现双臂机器人夹持负载跟随外力矩作旋转运动，提高双臂机器人的灵活性。
74.实施例1
75.本技术的一个实施例，请参见图1，提出一种双臂机器人控制方法包括以下步骤：
76.s100：在双臂机器人夹持负载时，将双臂末端的六维力转换至负载坐标系下。
77.可以理解，双臂机器人包括两个末端，分别记为第一末端和第二末端，双臂机器人可以利用第一末端和第二末端夹持负载。
78.双臂机器人夹持的负载运动过程中包括多个坐标系，本技术控制过程主要涉及三个坐标系，分别是以负载(双臂机器人夹持的负载)的中心为原点建立的负载坐标系，以双
臂机器人的第一末端中心为原点建立的第一末端坐标系，以双臂机器人的第二末端中心为原点建立的第二末端坐标系，例如，可参见图2，o1表示负载坐标系，o2表示第一末端坐标系，o3表示第二末端坐标系。
79.根据图2中所示的负载坐标系、第一末端坐标系和第二末端坐标系，可以确定负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵，可以将第一坐标系旋转矩阵记为(可以将第一末端坐标系下的六维力转换至负载坐标系)，由于负载坐标系的x轴、y轴和z轴与第一末端坐标系的x轴、y轴和z轴的方向相同，并且，不同坐标系下的力的大小是不变的，只有力的方向发生改变，所以可以理解，根据图2中所示的负载坐标系、第一末端坐标系和第二末端坐标系，还可以确定负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵，可以将第二坐标系旋转矩阵记为(可以将第二末端坐标系下的六维力转换至负载坐标系)，由于负载坐标系的x轴与第二末端坐标系的x轴的方向相同，负载坐标系的y轴和z轴与第二末端坐标系的y轴和z轴的方向相反，并且，不同坐标系下的力的大小是不变的，只有力的方向发生改变，所以
80.示范性的，第一末端的六维力传感器获取的六维力是第一末端坐标系下的，可记为利用可以将转换至负载坐标系下，负载坐标系下的第一末端的六维力可以表示为第二末端的六维力传感器获取的六维力是第二末端坐标系下的，可记为利用可以将转换至负载坐标系下，负载坐标系下的第一末端的六维力可以表示为
81.s200：根据所述负载坐标系下的各个末端的六维力确定各个末端对应的内力、外力和外力矩。
82.各个末端的六维力在负载坐标系下可以表示为对进行内外力分解，以确定各个末端对应的内力、外力和外力矩。
83.可以利用现有的方法对进行内外力分解，例如，可以参见“walker id,freeman ra,marcus si.analysis of motion and internal loading of objects grasped by multiple cooperating manipulators.the international journal of robotics research.1991；10(4):396
‑
409.doi:10.1177/027836499101000408”中公开的内外力分解方法对行内外力分解。
84.对负载坐标系下的进行内外力分解后，可以获得负载坐标系下的内力负载坐标系下的外力其中，表示第一末端对应的内力，表示第一末端对应的内力，示第二末端对应的内力，表示第一末端对应的外力，表示第二末端对应的外力。
85.进一步的，根据第一末端中心和第二末端中心之间的距离(近似等于负载在夹持方向上的长度)可以确定负载坐标系下的外力矩。
86.可以理解，各个末端对应的内力用于夹持负载，各个末端对应的外力用于双臂机器人夹持负载跟随外力作平移运动，各个末端对应的外力矩用于控制所述双臂机器人夹持负载跟随外力矩作旋转运动。
87.s300：根据所述负载坐标系下的各个末端对应的内力、外力和外力矩控制所述双臂机器人的双臂做协同运动。
88.可以理解，将双臂机器人的各个机械臂末端的六维力转换至负载坐标系下，在负载坐标系下进行计算确定负载坐标系下用于控制双臂的各个控制量，由于计算在同一坐标系下进行，可以降低计算过程的复杂性，提高计算速度，进而提高双臂协同工作的协同性，有效降低负载跌落的风险。
89.示范性的，请参见图3，步骤s300包括步骤s310、s320、s330和s340。
90.s310：根据第一末端对应的内力确定第一末端坐标系下的第一内力控制调整量和第二末端坐标系下的第二内力控制调整量。
91.第一内力控制调整量用于调整第一末端对应的机械臂，第二内力控制调整量用于调整第二末端对应的机械臂，以使第一末端对应的机械臂和第二末端对应的机械臂可以对负载施加一定的夹持力。
92.s320：根据所述第一末端对应的外力确定第一末端坐标系下的第一外力控制调整量和第二末端坐标系下第二外力控制调整量。
93.第一外力控制调整量用于调整第一末端对应的机械臂，第二外力控制调整量用于调整第二末端对应的机械臂，以在外界施加的交互力控制负载平行移动时，使第一末端对应的机械臂和第二末端对应的机械臂跟随外界施加的交互力作平行移动。
94.s330：根据所述第一末端对应的外力矩确定第一末端坐标系下的第一位姿调整量和第二末端坐标系下第二位姿调整量。
95.第一位姿调整量用于调整第一末端对应的机械臂，第二位姿调整量用于调整第二末端对应的机械臂，以在外界施加的交互力控制负载旋转时，使第一末端对应的机械臂和第二末端对应的机械臂跟随外界施加的交互力作旋转运动。可以理解，步骤s310、s320和s330之间无先后顺序，可以按照任意顺序执行步骤s310、s320和s330。
96.s340：根据所述第一内力控制调整量、所述第一外力控制调整量和所述第一位姿调整量控制所述双臂机器人的第一机械臂，同时根据所述第二内力控制调整量、所述第二外力控制调整量和所述第二位姿调整量控制所述双臂机器人的第二机械臂。
97.可以理解，将负载坐标系下的第一内力调整量和第一外力调整量分别转换至第一末端坐标系下以获得第一末端的第一内力控制调整量和第一末端的第一外力控制调整量，将负载坐标系下的第二内力调整量和第二外力调整量分别转换至第二末端坐标系下以获得第二末端的第二内力控制调整量和第二末端的第二外力控制调整量，实现双臂机器人夹持负载跟随外力作平移运动，提高双臂机器人的柔顺性。
98.可以理解，根据负载坐标系下的负载角度变化量确定第一末端坐标系下第一末端的第一位姿调整量和第二末端的第二位姿调整量，实现双臂机器人夹持负载跟随外力矩作旋转运动，提高双臂机器人的灵活性。
99.示范性的，请参见图4，步骤s310包括以下步骤：
100.s311：根据所述第一末端对应的内力确定所述负载坐标系下的第一内力调整量。
101.示范性的，可以根据负载坐标系下第一末端对应的内力确定第一末端对应的机械臂在负载坐标系下的第一内力调整量
102.s312：根据所述第一内力调整量确定所述负载坐标系下的所述第二末端的第二内力调整量。
103.根据内外力分解规律可知，第一末端对应的内力与第二末端对应的内力用于夹持负载，保证负载不跌落，因此第一末端对应的内力与第二末端对应的内力大小相同，方向相反，则进而表示第二内力调整量。
104.s313：根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述第一内力调整量在所述第一末端坐标系下的第一内力控制调整量。
105.将负载坐标系下的第一内力调整量转换至第一末端坐标系下，以得到第一内力控制调整量
106.s314：根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述第二内力调整量在所述第二末端坐标系下的第二内力控制调整量。
107.将负载坐标系下的第二内力调整量转换至第二末端坐标系下，以得到第二内力控制调整量
108.可以理解，第一内力控制调整量用于调整第一末端对应的机械臂，第二内力控制调整量用于调整第二末端对应的机械臂，以使第一末端对应的机械臂和第二末端对应的机械臂可以对负载施加一定的夹持力，保证物体不跌落。
109.示范性的，请参见图5，步骤s320包括以下步骤：
110.s321：根据所述第一末端对应的外力确定所述负载坐标系下的第一外力调整量。
111.根据第一末端对应的外力可以确定负载坐标系下的第一外力调整量
112.s322：根据所述第一外力调整量确定所述负载坐标系下的所述第二末端的第二外力调整量。
113.由于第一末端对应的外力与第二末端对应的外力用于使机器人夹持的负载跟随外力运动，因此第一末端对应的外力与第二末端对应的外力大小相同，方向相向，则进而表示第二外力调整量。
114.s323：根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述第一外力调整量在所述第一末端坐标系下的第一外力控制调整量。
115.将负载坐标系下的第一外力调整量转换至第一末端坐标系下，以得到第一外力控制调整量
116.s324：根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述第二外力调整量在所述第二末端坐标系下的第二外力控制调整量。
117.将负载坐标系下的第二外力调整量转换至第二末端坐标系下，以得到第二外力控制调整量
118.可以理解，第一外力控制调整量用于调整第一末端对应的机械臂，第二外力控制调整量用于调整第二末端对应的机械臂，以在外界施加的交互力控制负载平行移动时，使第一末端对应的机械臂和第二末端对应的机械臂跟随外界施加的交互力作平行移动。
119.示范性的，请参见图6，步骤s330包括以下步骤s331～s337：
120.s331：根据所述第一末端对应的外力矩确定负载角度变化量。
121.根据负载坐标系下的外力矩可以确定负载坐标系下的负载角度变化量，可以将负载坐标系下的负载角度变化量记为
122.s332：根据所述负载角度变化量确定姿态旋转矩阵。
123.将负载角度变化量分解至负载坐标系下的x轴、y轴和z轴，以获得和以根据以下公式确定姿态旋转矩阵：
[0124][0125]
表示姿态旋转矩阵，表示姿态旋转矩阵，
[0126]
s333：确定第一末端坐标系原点和第二末端坐标系原点之间的距离。
[0127]
可以理解，根据第一末端中心和第二末端中心之间的距离近似等于负载在夹持方向上的长度，即可以将负载在夹持方向上的长度l记为第一末端坐标系原点和第二末端坐标系原点之间的距离。
[0128]
s334：根据所述距离确定所述第一末端坐标系原点在所述负载坐标系下的第一位置坐标。
[0129]
示范性的，可参见图2中第一末端坐标系o2和负载坐标系o1之间的关系位置关系，确定第一位置坐标
ori
x2＝(0,
‑
l/2,0)。
[0130]
s335：根据所述距离确定所述第二末端坐标系原点在所述负载坐标系下的第二位置坐标。
[0131]
示范性的，可参见图2中第二末端坐标系o3和负载坐标系o1之间的关系位置关系，确定第二位置坐标
ori
x3＝(0,l/2,0)。
[0132]
s336：根据所述负载角度变化量、所述第一位置坐标和所述姿态旋转矩阵确定所述第一末端坐标系下的所述第一角度调整量和所述第一位置调整量。
[0133]
示范性的，请参见图7，步骤s336包括以下步骤s3361～s3364：
[0134]
s3361：根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述负载角度变化量在所述第一末端坐标系下的所述第一角度调整量。
[0135]
负载角度变化量在所述第一末端坐标系下的所述第一角度调整量可以利用以下公式确定：
[0136][0137]
其中，表示第一角度调整量，表示第一末端角度变化量，
[0138]
s3362：根据所述姿态旋转矩阵预测所述第一位置坐标对应的姿态调整后的第一预测位置坐标。
[0139]
第一预测位置坐标
[0140]
s3363：根据所述第一预测位置坐标和所述第一位置坐标确定第一末端位置变化量。
[0141]
第一末端位置变化量
[0142]
s3364：根据所述第一坐标系旋转矩阵确定所述第一末端位置变化量在所述第一末端坐标系下的所述第一位置调整量。
[0143]
第一末端坐标系下的第一位置调整量
[0144]
s337：根据所述负载角度变化量、所述第二位置坐标和所述姿态旋转矩阵确定所述第二末端坐标系下的所述第二角度调整量和所述第二位置调整量。
[0145]
示范性的，请参见图8，步骤s337包括以下步骤s3371～s3371：
[0146]
s3371：根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述负载角度变化量在所述第二末端坐标系下的所述第二角度调整量。
[0147]
负载角度变化量在第二末端坐标系下的第二角度调整量可以利用以下公式确定：
[0148][0149]
其中，表示第二角度调整量，表示第二末端角度变化量，
[0150]
s3372：根据所述姿态旋转矩阵预测所述第二位置坐标对应的姿态调整后的第二预测位置坐标。
[0151]
第二预测位置坐标
[0152]
s3373：根据所述第二预测位置坐标和所述第二位置坐标确定第二末端位置变化量。
[0153]
第二末端位置变化量
[0154]
s3374：根据所述第二坐标系旋转矩阵确定所述第二末端位置变化量在所述第二末端坐标系下的所述第二位置调整量。
[0155]
第二末端坐标系下的第二位置调整量
[0156]
进一步的，将第一末端坐标系下的第一内力控制调整量第一外力控制调
整量和第一位姿调整量中的第一位置调整量相加，利用逆解计算方法根据第一相加结果和第一角度调整量得到第一末端对应的机械臂的各个关节的关节角，利以用各个关节的关节角控制所述双臂机器人的第一末端对应的机械臂，同时将第二末端坐标系下的所述第二内力控制调整量第二外力控制调整量和第二位姿调整量中的第二位置调整量相加以确定第二相加结果，利用逆解计算方法根据第二相加结果和第二角度调整量得到第二末端对应的机械臂的各个关节的关节角，利以用各个关节的关节角控制所述双臂机器人的第二末端对应的机械臂。
[0157]
实施例2
[0158]
本技术的一个实施例，请参见图9，提出一种双臂机器人控制装置10包括：坐标系转换模块11、内外力确定模块12和协同控制模块13。
[0159]
坐标系转换模块11，用于在双臂机器人夹持负载时，将双臂末端的六维力转换至负载坐标系下，所述负载坐标系为以所述负载的中心为原点建立的坐标系；内外力确定模块12，用于根据所述负载坐标系下的各个末端的六维力确定各个末端对应的内力、外力和外力矩，所述各个末端对应的内力用于夹持负载，所述各个末端对应的外力和外力矩用于控制所述双臂机器人夹持负载跟随所述外力和外力矩运动；协同控制模块13，用于根据所述负载坐标系下的各个末端对应的内力、外力和外力矩控制所述双臂机器人的双臂做协同运动。
[0160]
进一步的，所述将双臂末端的六维力转换至负载坐标系下，包括：根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵将所述第一末端的六维力转换至所述负载坐标系下；根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵将所述第二末端的六维力转换至所述负载坐标系下。
[0161]
进一步的，所述根据所述负载坐标系下的各个末端对应的内力、外力和外力矩控制所述双臂机器人的双臂做协同运动，包括：根据所述第一末端对应的内力确定第一末端坐标系下的所述第一末端的第一内力控制调整量和第二末端坐标系下的第二末端的第二内力控制调整量；根据所述第一末端对应的外力确定所述第一末端坐标系下的第一外力控制调整量和所述第二末端坐标系下的第二外力控制调整量；根据所述第一末端对应的外力矩确定所述第一末端坐标系下的第一位姿调整量和所述第二末端坐标系下的第二位姿调整量；根据所述第一内力控制调整量、第一外力控制调整量和所述第一位姿调整量控制所述双臂机器人的第一机械臂，同时根据所述第二内力控制调整量、所述第二外力控制调整量和所述第二位姿调整量控制所述双臂机器人的第二机械臂。
[0162]
进一步的，所述根据所述第一末端对应的内力确定第一末端坐标系下的所述第一末端的第一内力控制调整量和第二末端坐标系下的第二末端的第二内力控制调整量，包括：根据所述第一末端对应的内力确定所述负载坐标系下的第一内力调整量；根据所述第一内力调整量确定所述负载坐标系下的所述第二末端的第二内力调整量；根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述第一内力调整量在所述第一末端坐标系下的第一内力控制调整量；根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述第二内力调整量在所述第二末端坐标系下的第二内力控制调整
量。
[0163]
进一步的，所述根据所述第一末端对应的外力确定所述第一末端坐标系下的第一外力控制调整量和所述第二末端坐标系下的第二外力控制调整量，包括：根据所述第一末端对应的外力确定所述负载坐标系下的第一外力调整量；根据所述第一外力调整量确定所述负载坐标系下的所述第二末端的第二外力调整量；根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述第一外力调整量在所述第一末端坐标系下的第一外力控制调整量；根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述第二外力调整量在所述第二末端坐标系下的第二外力控制调整量。
[0164]
进一步的，所述第一位姿调整量包括所述第一末端坐标系下的第一角度调整量和第一位置调整量，第二位姿调整量包括所述第二末端坐标系下的第二角度调整量和第二位置调整量，所述根据所述第一末端对应的外力矩确定所述第一末端坐标系下的第一位姿调整量和所述第二末端坐标系下的第二位姿调整量，包括：根据所述第一末端对应的外力矩确定所述负载角度变化量；根据所述负载角度变化量确定姿态旋转矩阵；确定第一末端坐标系原点和第二末端坐标系原点之间的距离；根据所述距离确定所述第一末端坐标系原点在所述负载坐标系下的第一位置坐标；根据所述距离确定所述第二末端坐标系原点在所述负载坐标系下的第二位置坐标；根据所述负载角度变化量、所述第一位置坐标和所述姿态旋转矩阵确定所述第一角度调整量和所述第一位置调整量；根据所述负载角度变化量、所述第二位置坐标和所述姿态旋转矩阵确定所述第二角度调整量和所述第二位置调整量。
[0165]
进一步的，所述根据所述负载角度变化量、所述第一位置坐标和所述姿态旋转矩阵确定所述第一角度调整量和所述第一位置调整量，包括：根据所述负载坐标系和第一末端坐标系之间的第一坐标系旋转矩阵确定所述负载角度变化量在所述第一末端坐标系下的所述第一角度调整量；根据所述姿态旋转矩阵预测所述第一位置坐标对应的姿态调整后的第一预测位置坐标；根据所述第一预测位置坐标和所述第一位置坐标确定第一末端位置变化量；根据所述第一坐标系旋转矩阵确定所述第一末端位置变化量在所述第一末端坐标系下的所述第一位置调整量。
[0166]
进一步的，所述根据所述负载角度变化量、所述第二位置坐标和所述姿态旋转矩阵确定所述第二角度调整量和所述第二位置调整量，包括：根据所述负载坐标系和第二末端坐标系之间的第二坐标系旋转矩阵确定所述负载角度变化量在所述第二末端坐标系下的所述第二角度调整量；根据所述姿态旋转矩阵预测所述第二位置坐标对应的姿态调整后的第二预测位置坐标；根据所述第二预测位置坐标和所述第二位置坐标确定第二末端位置变化量；根据所述第二坐标系旋转矩阵确定所述第二末端位置变化量在所述第二末端坐标系下的所述第二位置调整量。
[0167]
本实施例公开一种双臂机器人控制装置10包括：坐标系转换模块11、内外力确定模块12和协同控制模块13的配合使用，用于执行上述实施例所述的双臂机器人控制方法，上述实施例所涉及的实施方案以及有益效果在本实施例中同样适用，在此不再赘述。
[0168]
本技术的一个实施例，请参见图10，提出一种双臂机器人100包括存储器110和处理器120，所述存储器110存储有计算机程序，所述计算机程序在所述处理器120上运行时执行本技术所述的双臂机器人控制方法。
[0169]
本技术的一个实施例，还提出一种可读存储介质，其存储有计算机程序，所述计算
机程序在处理器上运行时执行本技术所述的双臂机器人控制方法。
[0170]
在本技术所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0171]
另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
[0172]
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个可读存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的可读存储介质包括：u盘、移动硬盘、只读存储器(rom，read
‑
only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0173]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。