一种双轮足机器人的实时负载估计与自适应控制方法及系统

本发明属于机器人相关，更具体地，涉及一种双轮足机器人的实时负载估计与自适应控制方法及系统。

背景技术：

1、双轮足机器人兼具轮式移动的高效性与腿式机构的地形适应能力，在物料搬运、快递配送、室内服务及人机协作等场景中具有应用潜力。在上述场景中，机器人可能承载质量和位置不断变化的外部负载，负载变化会引起机器人系统质量分布和惯性参数变化，进而影响机体姿态、平衡点位置和闭环稳定性。因此，如何实时获得负载质量和负载位置，并将其用于控制模型更新，是双轮足机器人稳定运行需要解决的重要问题。

2、在轮式倒立摆机器人、轮式人形机器人和双轮足机器人方向，jeong和takahashi针对i-pentar助手机器人建立了轮式倒立摆型机器人的设计与运动方程，nasir和mahmoud进一步针对同类平台开展了鲁棒移动控制研究；zafar等人将质量模型误差表述为质心误差，并结合鲁棒控制和在线学习更新质量模型；baek等人面向未知负载下的轮式人形机器人，通过位置和俯仰角时间序列估计新的平衡点；yu等人则针对双轮足机器人，将质心状态和等效扰动纳入模型预测控制框架进行补偿。上述方案能够缓解模型失配对平衡控制的影响，但其主要目标是平衡点修正、质心误差补偿或等效扰动抑制，通常不直接输出外部负载质量以及负载在机体坐标系中的纵向位置、横向位置等显式物理参数。

3、对于面向物料搬运等应用的双轮足机器人，仅进行平衡点修正或等效扰动补偿，难以满足负载状态识别、承载能力判断以及动力学参数在线更新的需求。同时，若通过额外力/力矩传感器或足底力传感器获取支撑力信息，则会增加机器人硬件成本、结构复杂度和维护难度。因此，现有技术仍缺乏一种能够在无需额外足底力传感器、无需主动姿态激励的条件下，利用机器人已有的关节角度信息、关节输出扭矩信息和机体姿态信息，以闭式解算方式实时获得负载质量及其在机体坐标系中位置的负载估计方法。

技术实现思路

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、其中，为带负载后机器人与负载组成的系统相对于更新后系统总质心的转动惯量矩阵；为与机器人当前腿部构型相对应的、无负载机器人本体相对于其无负载质心的转动惯量矩阵；为机器人本体的无负载质量；为负载质量；为与机器人当前腿部构型相对应的机器人本体无负载质心在机体坐标系下的位置向量；为负载等效质心在机体坐标系下的位置向量；为带负载后机器人与负载组成的系统总质心在机体坐标系下的位置向量；其中，表示机体坐标系；为3×3单位矩阵。

28、按照本发明的另一个方面，提供了一种双轮足机器人的实时负载估计与自适应控制系统，该系统包括执行器，该执行器用于实现上述所述的一种双轮足机器人的实时负载估计与自适应控制方法。

29、按照本发明的又一个方面，提供了一种计算机存储介质，其上存储有计算机程序，所述程序用于实现上述所述的一种双轮足机器人的实时负载估计与自适应控制方法。

30、按照本发明的再一个方面，提供了一种计算机程序产品，其上包括计算机程序，所述计算机程序被处理器执行时实现上述所述的一种双轮足机器人的实时负载估计与自适应控制方法。

31、总体而言，通过本发明所构思的以上技术方案与现有技术相比，具备下列有益效果：

32、1. 本发明提供的方法在双轮足机器人处于双侧有效支撑状态时，根据腿部几何关系和雅可比映射关系将关节输出扭矩解算为左右腿部垂向支撑力，并计算总支撑力和力矩的增量，最后更新平衡控制器的模型参数或控制增益，以此使平衡控制器所采用的模型参数与机器人当前带载状态相匹配，或使控制增益随负载变化相应调整，从而减小模型失配对闭环控制的影响，有效解决由于负载变化导致的机器人动力学模型失配、控制性能下降乃至失稳的问题。

33、2.本发明首先根据当前腿部构型和机体姿态确定对应的无负载支撑力及俯仰力矩基准，并利用关节角度、关节输出扭矩及腿部雅可比关系解算当前左右垂向支撑力，通过作差获得由外部负载引起的支撑力增量和力矩增量；在满足准静态估计条件时，根据所述增量实时估计负载质量及其位置，进而更新系统总质量、总质心位置、等效倒立摆长度和转动惯量等动力学参数；平衡控制器再根据更新后的动力学参数调整控制模型或反馈增益，例如根据更新后的系统总质量和等效倒立摆长度，插值得到与当前负载状态相匹配的lqr反馈增益矩阵。由此，可减小控制器所采用的动力学模型与机器人当前带载状态之间的参数偏差，使控制作用随负载变化进行调整，从而降低因模型失配造成的姿态调节性能下降及闭环失稳风险。当不满足准静态估计条件时，保持上一时刻可信估计值或降低当前估计权重，以避免不可靠估计造成控制参数误更新。

34、3. 本发明计算该方法中的支撑力由关节输出扭矩和腿部几何关系解算得到，而非由额外足底力传感器直接测量得到，同时无需主动姿态激励，可降低负载变化引起的模型失配对双轮足机器人平衡控制的影响。

35、4. 本发明根据总垂向支撑力增量、俯仰力矩增量以及左右垂向支撑力增量，分别估计负载质量、纵向位置和横向位置，所得负载参数具有明确的物理含义，可为负载状态识别以及系统总质量、总质心位置、等效倒立摆长度和转动惯量的在线更新提供参数依据。

36、5.本发明根据当前腿部构型和机体姿态确定相应的无负载支撑力及俯仰力矩基准，并结合准静态条件判定、负载阈值判定、信号滤波及可信估计值保持机制，有助于降低姿态变化、快速运动、轮地打滑和测量扰动引起的负载误判或参数误更新，提高负载估计结果的可靠性和连续性。