一种人形机器人上肢双机械臂的柔顺控制方法及装置

本发明涉及机械臂控制方法，尤其涉及一种人形机器人上肢双机械臂的柔顺控制方法及装置。

背景技术：

1、当前绝大多数类型的载具在操纵器的安装设计上依然保留着结实耐用的机械传动机构，并与轮胎保持物理连接。这种做法将轮胎与路面的接触情况实时通过方向盘传递给驾驶员，有效提升了驾驶体验，同时辅助系统提供的缓冲裕量与回转力矩也提高了汽车操作性能。然而，这些设计将方向盘控制变得非线性，同时增加了操作任务的复杂度，给机器人实现拟人操作能力带来了很大的挑战。

2、在方向盘操作任务环境中存在回转力矩以及颠簸带来的不确定扰动，这种情况下需要机器人与环境进行柔性交互，避免出现“脱手”状况使得控制不稳定，甚至直接损伤机械臂的情形。然而经典导纳控制在固定操作空间下，往往外力偏差值对应的位置控制变化量极小，在方向盘操作场景中极大地限制了其应用范围。与此同时，拟人机械臂在操作方向盘上是一个动态过程，在保证速度、位置跟踪精度的情况下，还需要维持稳定接触的施力跟踪，这要求对机械臂末端力控与位控完全解耦。

3、综合来看，双臂操作方向盘要比单臂在稳定性、可靠性上强上许多，一是由于方向盘回正力矩特性的存在，单机械臂操作打满方向盘需要腕部能够灵活翻转的同时持续施力，目前大部分机械臂加灵巧手的协同配合难以做到这一点，二是单机械臂转动方向盘时会出现较为“脆弱”的位形，此时方向盘的扰动及回正力矩可能会对机械臂造成损伤。因此，面向驾驶操作去研究双臂操作方向盘中的协同控制问题是十分必要的。

技术实现思路

1、本发明的目的在于提供一种人形机器人上肢双机械臂的柔顺控制方法及装置。

2、为实现上述发明目的，本发明提供一种人形机器人上肢双机械臂的柔顺控制方法，包括：

3、s1.基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对所述上肢双机械臂构建联合动力学模型；

4、s2.对所述上肢双机械臂的双臂协同控制器和协同流程进行设计，其中，所述双臂协同控制器为基于内力一致性的协同控制器，其表示为：

5、；

6、；

7、其中，为控制器输出，为左臂和右臂角速度的修正量，分别为比例、积分系数，为惯性系数，为2×2选择矩阵，代表控制器输出的值控制左臂或者右臂，表示在对操作对象操作的过程中左右臂之间的内力，当时，以减少左臂速率为主，当时，以减少右臂速率为主，表示左臂传感器坐标系下轴向的力，表示右臂传感器坐标系下轴向的力；

8、所述协同流程包括稳定型协同流程和快速型协同流程；

9、s3.以所述上肢双机械臂运行过程中的内力最小为控制目标，基于所述闭链运动学模型、所述联合动力学模型、所述双臂协同控制器和所述协同流程对所述上肢双机械臂进行运动控制。

10、根据本发明的一个方面，步骤s1中，基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型的步骤中，包括：

11、s11.基于操作对象的外形结构获取所述上肢双机械臂与操作对象的几何关系；

12、s12.基于所述几何关系构建所述上肢双机械臂的位置约束和速度约束，以完成所述闭链运动学模型的构建。

13、根据本发明的一个方面，步骤s11中，所述操作对象为方向盘；

14、步骤s12中，所构建的位置约束表示为：

15、；

16、式中，表示所述上肢双机械臂的左臂在基座标系下的位姿矩阵，表示所述上肢双机械臂的右臂在基座标系下的位姿矩阵，表示相对位姿矩阵，且表示为，表示在左臂末端的第一灵巧手坐标系下右臂末端的第二灵巧手的姿态矩阵，表示在第一灵巧手坐标系下第二灵巧手的位置向量，表示误差精度；

17、所构建的速度约束表示为：

18、；

19、其中，表示左臂末端的第一灵巧手的平动速度，表示右臂末端的第二灵巧手的平动速度，表示左臂末端的第一灵巧手的转动速度，表示右臂末端的第二灵巧手的转动速度，表示左臂末端的第一灵巧手与操作对象轴心的转动半径 ，表示右臂末端的第二灵巧手与操作对象轴心的转动半径，，分别为操作对象转动半径与转动速度。

20、根据本发明的一个方面，步骤s1中，对所述上肢双机械臂构建联合动力学模型的步骤中，所述联合动力学模型表示为：

21、；

22、；

23、；

24、；

25、；

26、；

27、其中，表示左臂左关节力矩 ，表示右臂右关节力矩，表示关节电机的加速度矩阵，表示右臂关节的惯性矩阵，表示左臂关节的惯性矩阵，表示右臂关节的科氏力矩阵，表示左臂关节的科氏力矩阵，表示关节电机的速度矩阵，表示右臂关节的重力矩阵，表示左臂关节的重力矩阵，表示右臂雅可比矩阵的逆，表示左臂雅可比矩阵的逆，表示右臂操作矩阵的逆，表示左臂操作矩阵的逆，表示操作对象的回转力矩，表示左臂末端和操作对象接触点的广义力，表示右臂末端和操作对象接触点的广义力，为操作对象的质量，为单位矩阵，为零矩阵，分别为操作对象的质心的速度向量与角速度向量，为目标坐标系下操作对象的转动惯量矩阵，为操作对象的姿态矩阵，为操作对象姿态矩阵的逆，表示基座标系下操作对象的转动惯量矩阵。

28、根据本发明的一个方面，对所述上肢双机械臂的协同流程进行设计的步骤中，所述稳定型协同流程的设计的步骤，包括：

29、s021.分别设置左臂和右臂的三种状态，其中，所述左臂的三种状态包括：左臂操作方向盘运动状态、左臂换手运动状态、左臂等待同步状态，所述右臂的三种状态包括：右臂操作方向盘运动状态、右臂等待同步状态、右臂换手运动状态；

30、s022.基于所述左臂的三种状态构建封闭的左臂运动循环，其中，所述左臂操作方向盘运动状态、所述左臂换手运动状态、所述左臂等待同步状态依次循环的建立连接，以构成所述左臂运动循环；

31、s023.基于所述右臂的三种状态构建封闭的右臂运动循环，其中，所述右臂操作方向盘运动状态、所述右臂等待同步状态、所述右臂换手运动状态依次循环的建立连接，以构成所述右臂运动循环；

32、s024.将所述左臂运动循环和所述右臂运动循环进行连接，以对所述左臂运动循环和所述右臂运动循环建立同步流程，其中，将所述左臂等待同步状态和所述右臂等待同步状态建立连接，将所述右臂换手运动状态与所述左臂等待同步状态建立连接，以构建所述左臂运动循环和所述右臂运动循环的左右同步等待循环。

33、根据本发明的一个方面，所述快速型协同流程的设计的步骤，包括：

34、s21.分别设置左臂和右臂的四种状态，其中，所述左臂的四种状态包括：第一等待同步状态、操作方向盘运动状态、第二等待同步状态、换手运动状态，所述右臂的四种状态包括：松开方向盘向后运动状态、第三等待同步状态、向前运动抓握方向盘运动状态、第四等待同步状态；

35、s22.基于所述左臂的四种状态构建封闭的第一运动循环，其中，所述第一等待同步状态、所述操作方向盘运动状态、所述第二等待同步状态、所述换手运动状态依次建立连接，以构成所述第一运动循环；

36、s23.基于所述右臂的四种状态构建封闭的第二运动循环，其中，所述松开方向盘向后运动状态、所述第三等待同步状态、所述向前运动抓握方向盘运动状态、所述第四等待同步状态依次循环的建立连接，以构成所述第二运动循环；

37、s24.将所述第一运动循环和所述第二运动循环进行连接，以对所述第一运动循环和所述第二运动循环建立同步流程，其中，将所述第一等待同步状态、所述第四等待同步状态、所述第二等待同步状态和所述第三等待同步状态依次建立连接，以构建所述第一运动循环和所述第二运动循环的同步等待循环。

38、实现上述发明目的，本发明提供一种采用前述的柔顺控制方法的柔顺控制装置，包括：

39、模型构建模块，所述模型构建模块基于上肢双机械臂与操作对象组成的封闭运动链构建闭链运动学模型，以及对所述上肢双机械臂构建联合动力学模型；

40、控制器构建模块，用于生成所述上肢双机械臂的双臂协同控制器和协同流程，其中，所述双臂协同控制器为基于内力一致性的协同控制器，其表示为：

41、；

42、；

43、其中，为控制器输出，为左臂和右臂角速度的修正量，分别为比例、积分系数，为惯性系数，为2×2选择矩阵，代表控制器输出的值控制左臂或者右臂，表示在对操作对象操作的过程中左右臂之间的内力，当时，以减少左臂速率为主，当时，以减少右臂速率为主，表示左臂传感器坐标系下轴向的力，表示右臂传感器坐标系下轴向的力；

44、所述协同流程包括稳定型协同流程和快速型协同流程；

45、输出控制模块，所述输出控制模块以所述上肢双机械臂运行过程中的内力最小为控制目标，基于所述闭链运动学模型、所述联合动力学模型、所述双臂协同控制器和所述协同流程对所述上肢双机械臂进行运动控制。

46、根据本发明的一种方案，本发明针对双臂协同操作方向盘这一任务进行了探索，提出的方案在实验环境中证实了有效性与鲁棒性。由于协同流程的拟人化操作（操作方向盘外圈），可以适应多种机械臂末端执行器，无需专门定制。首次在方向盘操作任务中引入导纳控制这一概念，具有创新性，结合视觉定位、模式识别等智能算法，能够应用于多种不同驾驶场景。

47、根据本发明的一种方案，本发明通过对双臂协同问题进行建模，分析了双臂同时操作方向盘的运动学约束，并从动力学角度分析方向盘回转特性在闭链系统中对双臂造成的影响；此外，为了减少双臂操作过程中的额外能量损耗，结合选择矩阵与单积分动力学控制器的思想，设计了调节左右臂运动角速率的协同控制器，达到操作过程中间接消减内力的目的；在此基础上，通过设计实验，在操作过程中对方向盘施加扰动证明了基于内力调节的协同控制方法具有一定鲁棒性。

48、根据本发明的一种方案，本发明通过对方向盘转动速度分解为左右臂姿态运动角速度，在引入自适应导纳控制器实现末端柔顺控制的基础上，设计可以间接调节双臂操作过程中内力的协同控制器，通过方向盘单周运动区间下双臂协同操作实验对此进行了验证；结合了自适应导纳控制器、内力一致协同控制器的柔顺控制方法在双臂操作方向盘上也可以较好地应用，对比单臂的方向盘操控鲁棒性更强。

49、根据本发明的一种方案，本发明基于内力调节的双臂协同控制方案较好地完成了驾驶操作中方向盘控制目标，且提出的两种协同流程中，双臂同时抓握方向盘运动的稳定控制流程较从臂辅助主臂操作方向盘进行配合控制流程的抗扰性更强，但转动效率更低，二者各有优劣。实际应用中，面向不同的使用场景应采用相应的控制流程，提高机械臂对方向盘操作的稳定性、鲁棒性和高效性。