一种基于内力检测的机器人双臂协同作业控制方法

本发明涉及机器人，具体涉及一种基于内力检测的机器人双臂协同作业控制方法。

背景技术：

1、救援机器人在地震、滑坡等重大自然灾难后，能够快速到达灾难现场，协助人类完成救援工作，减少灾后二次伤亡。随着任务难度的不断提高，单臂救援机器人已经难以应对越来越复杂的任务，双臂救援机器人在简化作业难度、降低成本开销等方面具有更大的优势。双臂救援机器人在双臂协同工作过程中，由于运动不同步、外部干扰等原因，存在协同失败的风险，可能会导致夹取物体或者机械臂损坏。

2、在双机械臂协同控制的问题上，已有研究提出了主从控制法、混合力/位控制法和阻抗/导纳控制法等方法。现有技术方法能够实现双机械臂的协同运动，但是在实际使用中往往也存在各自的问题，如主从控制法需要从臂的力跟踪控制具有快速的响应速度，容易导致系统不稳定；混合力/位控制法需要根据实际操作任务选择位置和力控制方向，并且在位置控制和力控制之间实时切换，会增加控制器计算量；传统阻抗控制法未对机械臂与物体之间的内力进行控制，同时在机械臂末端受到外部力干扰时，也不能实现柔顺控制。

技术实现思路

1、有鉴于此，本发明提供了一种基于内力检测的机器人双臂协同作业控制方法，能够对机械臂与夹取物体之间的内力进行分析和控制，提升机械臂与环境接触的柔顺性。

2、为了解决上述技术问题，本发明是这样实现的：

3、一种基于内力检测的机器人双臂协同作业控制方法，构建作用于夹取物质心处的力与力矩ho和机械臂末端施加于夹取物上的力与力矩h间的映射关系；

4、根据机械臂实际作用于夹取物上的力与力矩，和夹取物的期望运动状态，通过所述映射关系得到质心处内力δho；

5、根据所述质心处内力δho，通过所述映射关系得到机械臂末端内力偏差δhii；

6、根据所述机械臂末端内力偏差δhii得到机械臂末端位置修正量，通过位置控制实现机器人双臂协同作业柔顺控制。

7、较佳地，所述映射关系，包括：

8、将所述机械臂末端施加于夹取物上的力与力矩h映射为所述作用于夹取物质心处的力与力矩ho的第一映射关系；

9、将所述作用于夹取物质心处的力与力矩ho映射为所述机械臂末端施加于夹取物上的力与力矩h的第二映射关系。

10、较佳地，所述第一映射关系通过建立雅可比矩阵得到，所述雅可比矩阵为：

11、ho＝[jm1tjm2t…jmnt]h＝jmth；

12、其中，表示由机械臂末端到夹取物质心处的雅可比矩阵；k为从1至n的整数，表示第k个机械臂；为jmkt矩阵中的矩阵表示符号；rk＝[rkx rky rkz]t表示夹取物质心到第k个机械臂末端的向量，rkx、rky和rkz分别表示rk在世界坐标系下x、y和z方向上的投影值；i3为单位矩阵；o3为零矩阵；

13、定义θ＝jmt，θ为行满秩矩阵抓取矩阵；得到所述第一映射关系为：

14、ho＝θh。

15、较佳地，所述第二映射关系通过求取伪逆通解得到，所述求取伪逆通解为：

16、

17、其中，i为单位矩阵；为θ的右伪逆，采用彭罗斯伪逆；ε是一个任意向量。

18、较佳地，所述通过所述映射关系得到机械臂末端内力偏差δhii，为：

19、对式(iii)取并选取中间矩阵使φ位于矩阵的零空间并满足θφ＝0；得到机械臂末端实际内力hiri＝φδho；所述机械臂末端内力偏差δhii＝-hiri。

20、较佳地，所述根据所述机械臂末端内力偏差δhii得到机械臂末端位置修正量，通过位置控制实现机器人双臂协同作业柔顺控制，为：

21、将所述机械臂末端内力偏差δhii输入阻抗方程，所述阻抗方程为：

22、

23、其中，mi、bi和ki分别表示第i个机械臂的惯性、阻尼和刚度矩阵；所述阻抗方程的输出δχi表示第i个机械臂末端的位置偏差；根据所述δχi得到机械臂末端位置修正量σi；根据所述机械臂末端位置修正量σi和所述夹取物的期望运动状态得到机械臂末端参考轨迹χri；将所述机械臂末端参考轨迹χri作为双臂机器人位置控制系统的输入，从而实现机械臂末端的协同柔顺控制。

24、有益效果：

25、1、本发明通过构建作用于夹取物质心处的力组ho与机械臂末端施加于夹取物上的力组h间的相互映射关系，能够对机械臂与夹取物体之间的内力进行分析和控制，将无法通过直接测量准确得到的机械臂与夹取物间内力通过计算分析得到，进而通过将内力转换为机械臂末端位置修正量实现机器人双臂协同作业的柔顺性位置控制，提升了机械臂与环境接触的柔顺性。

26、2、本发明通过建立雅可比矩阵与求取伪逆通解，实现了作用于夹取物质心处的力组ho与机械臂末端施加于夹取物上的力组h间的相互映射关系，该映射关系的计算表达不仅能够应用于本发明的机械臂控制方法，还能够推广应用于同类的夹取机械臂末端内力分析，具有良好的适用性和启发性。

27、3、本发明通过对和中间矩阵φ的具体选用，构建了θφ＝0的内力约束条件，在夹取式双机械臂这一物理实现与数学表达之间建立了联系，进而实现了机械臂末端实际内力和内力偏差的计算分析。

28、4、本发明根据阻抗控制方程，通过机械臂-物体间的内力，实时调整机械臂末端参考轨迹，提高了双臂运动的协调性。在机械臂末端受到外部干扰时，能够同步调整双臂末端轨迹，实现机械臂末端与环境的柔顺控制。

技术特征：

1.一种基于内力检测的机器人双臂协同作业控制方法，其特征在于，构建作用于夹取物质心处的力与力矩ho和机械臂末端施加于夹取物上的力与力矩h间的映射关系；

2.如权利要求1所述的基于内力检测的机器人双臂协同作业控制方法，其特征在于，所述映射关系，包括：

3.如权利要求2所述的基于内力检测的机器人双臂协同作业控制方法，其特征在于，所述第一映射关系通过建立雅可比矩阵得到，所述雅可比矩阵为：

4.如权利要求3所述的基于内力检测的机器人双臂协同作业控制方法，其特征在于，所述第二映射关系通过求取伪逆通解得到，所述求取伪逆通解为：

5.如权利要求4所述的基于内力检测的机器人双臂协同作业控制方法，其特征在于，所述通过所述映射关系得到机械臂末端内力偏差δhii，为：

6.如权利要求1-5中任意一项所述的基于内力检测的机器人双臂协同作业控制方法，其特征在于，所述根据所述机械臂末端内力偏差δhii得到机械臂末端位置修正量，通过位置控制实现机器人双臂协同作业柔顺控制，为：

技术总结
本发明公开了一种基于内力检测的机器人双臂协同作业控制方法，构建作用于夹取物质心处的力与力矩h<subgt;o</subgt;和机械臂末端施加于夹取物上的力与力矩h间的映射关系；根据机械臂实际作用于夹取物上的力与力矩，和夹取物的期望运动状态，通过所述映射关系得到质心处内力Δh<subgt;o</subgt;；根据所述质心处内力Δh<subgt;o</subgt;，通过所述映射关系得到机械臂末端内力偏差Δh<subgt;Ii</subgt;；根据所述机械臂末端内力偏差Δh<subgt;Ii</subgt;得到机械臂末端位置修正量，通过位置控制实现机器人双臂协同作业柔顺控制；使用本发明能够对机械臂与夹取物体之间的内力进行分析和控制，提升机械臂与环境接触的柔顺性。

技术研发人员：赵江波,陈程远,王军政,汪首坤,马立玲,沈伟,李静
受保护的技术使用者：北京理工大学
技术研发日：
技术公布日：2024/1/15