一种带机械臂的四足机器人协同运动控制方法与流程

本技术属于机器人，尤其涉及一种带机械臂的四足机器人协同运动控制方法。

背景技术：

1、四足机器人相较于传统的轮式和履带式移动机器人在复杂环境下具有更好的运动能力和更高的机动性，然而它并不具有抓取物体的能力。机械臂与四足机器人本体的组合，可有效提高四足机器人的灵活性，可模拟人类手臂的活动，实现抓取、搬运以及其他复杂动作。这种灵活性使得四足机器人在仓储、物流和医疗等领域的应用变得更为广泛。目前大多数带机械臂的四足机器人，仅能够将四足机器人和机械臂作为独立的硬件单元分别进行控制，没有充分考虑四足机器人和机械臂之间的相互影响，尤其是相互作用力的影响。对每个单元独立控制的方法并不能发挥足式机器人本体的六自由度运动优势，且极有可能产生机械臂活动导致得机器人整体重心失稳的问题，这将极大地削弱机械臂末端的灵活工作空间、可达性和精准性，严重时甚至导致机器人倾倒。因此，对四足机器人本体和机械臂进行整体优化控制以实现对机械臂末端执行器高精度、高鲁棒性、高稳定性的协同运动控制是非常有必要的。

2、目前最具影响力的带机械臂的四足机器人协同运动控制方法是由苏黎世联邦理工学院(eth)提出的基于多刚体全身动力学模型的全身控制(whole-body control，wbc)优化方法。该方法首先构建机械臂和四足机器人本体的多刚体全身动力学模型，然后采用wbc优化控制方法，实现对机械臂本体6个关节电机力矩和四足机器人本体12个关节电机力矩的控制。该方法构造了完整的机械臂和四足机器人多刚体动力学模型，通过wbc控制器计算出18个电机的输出力矩，再通过各电机的力矩控制模式，以实现带机械臂的四足机器人协同运动控制。

3、带机械臂的四足机器人wbc控制方法需构造完整的多刚体全身动力学模型。带机械臂的四足机器人为18个自由度的超冗余度复合机器人，使用多刚体动力学建模需要已知四足机器人本体和机械臂各活动杆件的质量、转动惯量、各杆长度、各杆件之间关系等，故其整体的多刚体全身动力学模型构造复杂、建模困难、需已知信息多且运算量大。此外，该方法需采用提供力矩控制模式的机械臂系统。然而，市场大多机械臂仅能够提供位置控制模式，且提供力矩控制模式的机械臂价格高昂，整机开发难度大。

技术实现思路

1、本发明的主要目的在于提供一种带机械臂的四足机器人协同运动控制方法，以解决现有技术中多刚体全身动力学模型构造复杂、建模困难的、需已知信息多且运算量大的问题。

2、为了解决上述问题，根据本发明的一个方面，提供了一种带机械臂的四足机器人协同运动控制方法，带机械臂的四足机器人包括机械臂和四足机器人本体，其中，控制方法包括：步骤s1，构造四足机器人本体的单刚体动力学模型和带机械臂的四足机器人整体运动学模型；步骤s2，对单刚体动力学模型和整体运动学模型，设计未来n个时刻的模型预测矩阵；步骤s3，构造变权重代价函数、四足机器人本体足端受力条件和对机械臂关节转速的约束条件；步骤s4，将变权重代价函数、约束条件转化为二次规划问题，计算出地面对四足机器人本体足端作用力和机械臂期望关节角速度；步骤s5，计算出四足机器人本体单腿关节电机期望力矩和机械臂各关节的期望位置指令。

3、进一步地，上述步骤s1包括：使用六维力传感器检测机械臂对四足机器人本体的作用力和力矩，构造四足机器人本体的单刚体动力学模型和带机械臂的四足机器人整体运动学模型。

4、进一步地，上述四足机器人本体的单刚体动力学模型和带机械臂的四足机器人整体运动学模型包括：

5、

6、m0和g0为：

7、

8、其中，分别为四足机器人本体质心在世界系下的位置、欧拉角、线速度和角速度向量，分别为机械臂末端执行器在世界系的位置、欧拉角、线速度和角速度向量，为控制周期，为3×3的单位矩阵，为3×3的零矩阵，为3×6零矩阵，为四足机器人本体简化的角速度到欧拉角速度的旋转矩阵，为机械臂末端执行器的角速度到欧拉角速度的旋转矩阵，为四足机器人本体的质量，为四足机器人本体在世界系下的转动惯量，为四足机器人本体四个足端到四足机器人本体质心的位置向量，分别为四足机器人本体质心坐标系到世界系和基座坐标系到四足机器人本体质心坐标系的旋转矩阵，分别为机械臂末端执行器到机械臂基座的位置雅可比矩阵和旋转雅可比矩阵，为六维力传感器坐标系到四足机器人本体质心坐标系的平移向量，为六维力传感器坐标系下的机械臂对四足机器人本体的作用力，为六维力传感器坐标系到四足机器人本体质心坐标系的旋转矩阵，为重力加速度矢量，分别为机械臂末端执行器在机械臂基座坐标系的位置和角速度，为机械臂基座坐标系到四足机器人本体质心坐标系的平移向量，为四足机器人本体四个足端对地面的反作用力，机械臂选自六自由度机械臂，为六自由度机械臂关节角速度向量。

9、进一步地，上述步骤s2中，未来n个时刻的模型预测矩阵包括被控量在未来n个步长与控制量的关系。

10、进一步地，上述将步骤s1中构造的模型简化为：

11、

12、其中，xi+1为被控量，ui为控制量；对简化模型进行n次迭代，设计如下模型预测矩阵：

13、

14、其中，n为模型预测控制计算步长，

15、预测模型矩阵可简化为：

16、

17、其中，

18、进一步地，上述构造变权重代价函数的过程包括：使机械臂末端执行器期望位姿与机械臂末端执行器当前位姿差、四足机器人本体期望质心位姿与四足机器人本体初始质心位姿的差的范数与控制量范数之和最小：

19、

20、其中，为期望位姿向量，和为能够调整的控制权重，均为对角正定矩阵，q权重关于四足机器人本体期望质心位姿与四足机器人本体初始质心位姿的差的部分为变权重设置，设置机械臂末端执行器位姿误差的权重远大于四足机器人本体质心位姿误差的权重，当四足机器人本体期望质心位姿与初始质心位姿的差较小时，设置q权重较小；当四足机器人本体期望质心位姿与四足机器人本体初始质心位姿的差较大时，设置q权重逐渐增大。

21、进一步地，上述四足机器人本体四个足端对地面的反作用力f1,f2,f3,f4以摩擦锥为受力条件，机械臂关节角速度以最大转速构造约束条件。

22、进一步地，上述步骤s4中，将变权重代价函数和约束条件转化为二次规划问题包括：

23、

24、h和g分别为：

25、

26、其中，h和g为常值，u为控制量。

27、进一步地，上述使用二次规划求解器计算得u，u为n个步长控制量ui的向量，取第一个步长的ui作为控制量，得到四足机器人本体四个足端对地面的反作用力f1,f2,f3,f4和六自由度机械臂关节角速度向量

28、进一步地，上述利用四足机器人本体各条腿的雅可比矩阵和f1,f2,f3,f4，计算得到四足机器人本体单腿关节电机期望力矩；对机械臂各关节期望角速度积分，计算得到机械臂各关节的期望位置指令。

29、应用本发明的方案，本发明的目的是提供一种基于单刚体动力学模型的带机械臂的四足机器人足臂协同控制方法和系统，使用更加简单的单刚体四足机器人动力学模型和整体运动学模型，实现控制模型的简化、机械臂各关节期望位置指令的生成和四足机器人本体质心的稳定，以解决现有的足臂协同控制方法中需构造控制模型复杂的全身动力学问题、需采用价格高昂的具有力矩控制模式的机械臂所带来的成本问题，以及机械臂灵活运动所带来的四足机器人本体质心失稳的问题。