本发明属于双足机器人平衡控制技术领域，具体涉及一种双足机器人足部柔顺平衡控制系统及方法。
背景技术：
零力矩点(zmp)是双足机器人研究中非常重要的概念，其对双足机器人的行走规划、稳定等有举足轻重的作用。无论是机器人处于站立状态还是行走状态，机器人维持稳定的必要条件是zmp处于机器人的支撑域内。支撑域即机器人双足轮廓线所画出的多边形区域。所以有许多方法都从控制zmp的角度出发，控制机器人的实际zmp，保证其处于支撑域内，属于步行稳定控制。
从另一个角度出发，保持机器人姿态的平衡，是机器人状态稳定的标志。机器人的实际姿态若偏离了规划姿态，机器人的各个状态必然与规划有所出入，无法保证机器人的稳定。尽管轨迹规划时的机器人动力学模型与实际机器人的动力学之间存在误差，但这些误差一般通过别的调节来弥补，改变机器人姿态的调节方法代价太大。所以，可通过控制机器人的姿态，使其跟随规划的姿态，以此来保证机器人的平衡控制，属于机器人的姿态控制。
现有技术的控制对象上主要是躯干控制，通过控制机器人上身或上身联合腿部动作，实现机器人的平衡控制。这种控制方法的优势在于紧密结合了机器人的动力学特性，让机器人在应对外部力的扰动时能有较好的动态平衡控制效果。但缺点也很明显，就是忽略了机器人足部动作对机器人平衡的作用，导致机器人难以应对崎岖的地面环境，难以在角度不断变化的地面上维持平衡。
从控制目标角度，现有技术可分为以zmp为控制目标和以机器人上身姿态为控制目标。前者侧重于考虑地面信息对机器人平衡控制的影响，对机器人整体的姿态偏离反应不灵敏。后者侧重于机器人自身姿态的偏离，但对地面信息掌握不充分，容易由于地面的局部不平整而导致控制器失效。
技术实现要素：
针对现有技术中存在不足，本发明提供了一种双足机器人足部柔顺平衡控制系统及方法，实现机器人在角度变化的地面站立和在不平整地面行走的柔顺平衡控制。
本发明是通过以下技术手段实现上述技术目的的。
一种双足机器人足部柔顺平衡控制系统，包括六维力传感器、惯性测量单元、控制规律分类器、姿态控制器以及柔顺控制模型；六维力传感器、惯性测量单元分别采集机器人实际状态时的足部受力和姿态，并传输给控制规律分类器；姿态控制器根据控制规律和机器人实时姿态，获取虚拟力和虚拟力矩；柔顺控制模型由虚拟力、虚拟力矩以及机器人实时足部受力，获取踝关节角度调节量。
一种双足机器人足部柔顺平衡控制方法，根据机器人实际状态时的姿态和足部受力信息，结合机器人步行状态，设计控制规律分类器，推断相应的控制规律，再通过姿态控制器获取使机器人维持平衡的虚拟力和虚拟力矩，将虚拟力、虚拟力矩以及足部实际受力输入到足部柔顺模型中，从而输出踝关节角度调节量，调节足部平衡。
进一步，所述足部柔顺控制模型的动力学公式为：
其中：fext为足部受力或力矩，fvir为虚拟力或力矩，kp为弹簧阻尼质量块模型的劲度系数，kd为弹簧阻尼质量块模型的阻尼系数，m为弹簧阻尼质量块模型的虚拟质量，e为弹簧阻尼质量块模型位置或角度偏移量输出。
进一步，所述控制规律包括站立情况下机器人在矢状面上的控制规律和站立情况下机器人在冠状面上的控制规律。
更进一步，所述站立情况下机器人在矢状面上的控制规律和冠状面上的控制规律包括机器人单脚支撑地面和脚支撑地面两种情况。
更进一步，在矢状面上单脚支撑地面时，足部所受力矩方向与姿态偏离方向相同，且踝关节的虚拟力矩为：
更进一步，在矢状面上双脚支撑地面时，足部所受力矩方向与姿态偏离方向包括：情况①左脚所受力矩方向与姿态偏离方向相同，右脚所受力矩方向与姿态偏离方向相反；情况②左脚所受力矩方向与姿态偏离方向相反，右脚所受力矩方向与姿态偏离方向相同；情况③包括除情况①、情况②之外的其他情况。
更进一步，所述情况①中踝关节的虚拟力矩为：
进一步，在冠状面上单脚支撑地面时，足部所受力矩方向与姿态偏离方向相同，且踝关节的虚拟力矩为：
更进一步，在冠状面上双脚支撑地面时，踝关节的虚拟力矩为：
本发明的有益效果为：
(1)本发明中控制规律分类器根据机器人实际状态时的姿态和足部受力信息，结合机器人步行状态进行设计的，推断相应的控制规律，控制规律包括站立情况下机器人在矢状面上的控制规律和站立情况下机器人在冠状面上的控制规律，机器人步行状态包括机器人单脚支撑地面和脚支撑地面两种情况；在双脚支撑期中，对地面情况与机器人倾斜状态做了综合分析，能够更好地应对不同情况，实现更佳的平衡控制效果。
(2)本发明中姿态控制器从机器人姿态控制的角度出发，结合以获取的控制规律，将机器人失衡状态结合地面情况进行分离处理，不仅能保证机器人姿态的准确跟踪，还确保了对各种复杂地面环境的高适应性。
(3)本发明中柔顺控制模型将柔顺控制和姿态控制结合起来，柔顺控制模型的输入量为虚拟力、虚拟力矩以及机器人实时足部受力，将柔顺和平衡统一，一定程度上解决了柔顺效果和平衡效果的矛盾，实现适应性更强的平衡控制。
附图说明
图1为本发明所述足部柔顺平衡控制器结构示意图；
图2为本发明所述足部柔顺控制模型示意图；
图3为本发明所述矢状面单脚支撑期虚拟力矩计算模型示意图；
图4为本发明所述矢状面双脚支撑期虚拟力矩类型①计算模型示意图；
图5为本发明所述冠状面单脚支撑期虚拟力矩计算模型示意图；
图6为本发明所述冠状面双脚支撑期虚拟力矩和虚拟力计算模型示意图。
具体实施方式
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
如图1所示，一种双足机器人足部柔顺平衡控制系统，包括六维力传感器、惯性测量单元imu、控制规律分类器、姿态控制器以及柔顺控制模型；六维力传感器采集机器人实际状态时的足部受力，并传输给控制规律分类器；惯性测量单元imu采集机器人实际状态时的姿态，并传输给控制规律分类器；姿态控制器根据控制规律和机器人实时姿态，获取虚拟力和虚拟力矩，输入到柔顺控制模型，柔顺控制模型结合机器人实时足部受力，获取踝关节角度调节量，实现机器人足部柔顺平衡控制。
一种双足机器人足部柔顺平衡控制方法，包括如下步骤：
步骤一，建立足部柔顺控制模型
建立足部柔顺模型的目的一方面在于实现一定的柔顺效果，能够吸收一部分机器人足部着地时的冲击，对机器人的稳定有很大影响；另一方面，机器人的足部柔顺效果和足部调节平衡效果在位置量上往往是相矛盾的，为了将柔顺效果和平衡效果统一，将控制量上升到踝关节角度的二阶导数，从宏观上看，为了使控制量具有实际的物理意义，并让控制足部柔顺控制模型的姿态控制器的设计有所根据，踝关节角度的二阶导数与虚拟力矩相对应。这样一来，就可以将柔顺控制的输入量足部实际受力与平衡控制的输入量虚拟力结合在一起，输入到同一个导纳系统中，以实现柔顺与平衡的统一，这个导纳系统就是足部柔顺控制模型。从效果上看，通过这样的输入量转换与结合，可实现带有柔顺效果的平衡控制。
足部柔顺控制模型采用传统的弹簧阻尼质量块模型即可实现预期的控制效果，模型示意图如图2所示。
该模型的动力学可描述为：
根据本发明的输入量，将上式改写成：
其中：f为系统输入力，fext为六维力传感器采集的足部受力或者力矩，fvir为姿态控制器计算得到的虚拟力或者力矩，kp为弹簧阻尼质量块模型的劲度系数，kd为弹簧阻尼质量块模型的阻尼系数，m为弹簧阻尼质量块模型的虚拟质量，e为弹簧阻尼质量块模型的位置偏移量或角度偏移量输出。
步骤二，根据imu的姿态信息和足部六维力传感器的足部受力信息，结合机器人步行状态，设计控制规律分类器，推断相应的控制规律，再通过姿态控制器得到使机器人维持平衡的虚拟力和虚拟力矩。
本发明希望机器人能够在受到外力冲击下，或者在不平整的地面以及坡度改变的地面上，能够稳定地站立或者行走。在设计姿态控制器时，需要尽可能考虑机器人的状态和外部环境的情况，根据不同的情况组合，得到最为合适的控制规律。
(1)站立情况下机器人在矢状面上的控制
先考虑站立情况下的机器人在矢状面上的控制。若机器人单脚支撑地面(即机器人处于单脚支撑期)，所有受力与控制效果都由支撑脚决定，此时机器人的姿态产生偏离后，需要靠支撑脚调节使姿态恢复平衡状态，故支撑脚需要与姿态偏离方向相同的虚拟力矩。通过倒立摆模型计算踝关节的虚拟力矩：
其中：τvir_sag为作用在矢状面内的虚拟力矩，m为机器人质量，zc为机器人质心高度，θ为矢状面内姿态偏离角度，kv为抖动镇定阻尼系数；计算模型如图3所示。
直接计算出来的虚拟力矩响应快，容易与柔顺作用相冲突。为了避免冲突，可将计算得到的虚拟力矩通过一阶惯性系统，将虚拟力矩滞后作用在柔顺控制模型上。经过调试后的滞后延时常量t可使系统的柔顺与平衡的统一效果达到预期目标。虚拟力矩的滞后更新公式如下：
其中：s为拉氏变换的变量。
若机器人双脚同时支撑地面(即机器人处于双脚支撑期)，这时机器人双脚均受到力的作用，控制效果也由双脚共同决定，需要将机器人的倾斜方向与地面情况结合起来考虑，将虚拟力据加在合适的支撑脚上，以实现适应地面的姿态平衡控制。地面情况可根据双脚的六维力传感器得知，姿态偏离方向与足部受力方向的具体情况包括以下三类：
①左脚所受力矩方向与姿态偏离方向相同，右脚所受力矩方向与姿态偏离方向相反，如图4所示，即：
θ·τl＞0且θ·τr＜0(5)
其中：τl为左脚所受力矩，τr为右脚所受力矩；
这种情况对应的是双脚分别站在坡度不同的局部地面上，左脚所受力矩方向与姿态偏离方向相同，说明左脚的力矩是导致姿态偏离的主要因素，而右脚所受力矩方向与姿态偏离方向相反，说明右脚的力矩对姿态偏离有抵抗作用，这时应当将用于姿态控制的虚拟力矩加在左脚上，如公式(6)、(7)，而虚拟力矩的计算公式同公式(3)。
②左脚所受力矩方向与姿态偏离方向相反，右脚所受力矩方向与姿态偏离方向相同，即：
θ·τl＜0且θ·τr＞0(8)
这种情况与①对立，应当将虚拟力矩加在右脚，如公式(9)、(10)，虚拟力矩的计算公式同公式(3)。
③其他情况(除公式(5)、(8)之外，即θ·τl≤0且θ·τr≤0、θ·τl≥0且θ·τr≥0，左右脚所受力矩方向需要结合具体的坐标系进行描述)
在这里包含了机器人由于受到外力冲击而倾斜、机器人在斜坡上站立等情况，这时机器人双脚的作用相近，将由公式(3)计算得到的虚拟力矩分配到两只脚，分配方法则以两脚受到的竖直方向受力为权重分配，分配公式如下：
其中：
(2)站立情况下机器人在冠状面上的控制
再考虑站立情况下的机器人在冠状面上的控制。若机器人单脚支撑地面(即机器人处于单脚支撑期)，同样地，支撑脚需要与姿态偏离方向相同的虚拟力矩。通过倒立摆模型计算虚拟力矩：
其中：τvir_lat为作用在冠状面内的虚拟力矩，
若机器人双脚同时支撑地面(即机器人处于双脚支撑期)，同样地，控制效果也由双脚共同决定。与矢状面的姿态控制有两点不同，一是冠状面支撑跨度大，稳定性更高；二是冠状面的姿态控制可采取两腿长度占主要作用，踝关节角度起辅助作用的控制思路。综合这两点不同，结合柔顺控制后，就不必考虑地面的情况了。此时由姿态控制器输入到柔顺控制模型的控制量有两个，一是脚部竖直方向的虚拟力，用于控制腿长；二是虚拟力矩，用于控制角度。如图6所示，计算虚拟力矩的公式为：
计算虚拟力的公式为：
其中：d为双脚踝轴心关节距离，fz为脚部竖直方向的虚拟力，规定虚拟力方向向上为正。
步骤三，将虚拟力以及虚拟力矩和六维力传感器测得的足部实际受力输入到足部柔顺模型中，从而输出踝关节角度调节量，最终实现调节足部柔顺平衡的效果。
本发明一种双足机器人足部柔顺平衡控制方法中，调整参数kp、kd、m、kv、t，可实现由满足站立情况的柔顺平衡控制，到满足行走中的柔顺平衡控制。
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。
1.一种双足机器人足部柔顺平衡控制系统，其特征在于，包括六维力传感器、惯性测量单元、控制规律分类器、姿态控制器以及柔顺控制模型；六维力传感器、惯性测量单元分别采集机器人实际状态时的足部受力和姿态，并传输给控制规律分类器；姿态控制器根据控制规律和机器人实时姿态，获取虚拟力和虚拟力矩；柔顺控制模型由虚拟力、虚拟力矩以及机器人实时足部受力，获取踝关节角度调节量。
2.一种根据权利要求1所述双足机器人足部柔顺平衡控制系统的控制方法，其特征在于，根据机器人实际状态时的姿态和足部受力信息，结合机器人步行状态，设计控制规律分类器，推断相应的控制规律，再通过姿态控制器获取使机器人维持平衡的虚拟力和虚拟力矩，将虚拟力、虚拟力矩以及足部实际受力输入到足部柔顺模型中，从而输出踝关节角度调节量，调节足部平衡。
3.根据权利要求2所述的双足机器人足部柔顺平衡控制方法，其特征在于，所述足部柔顺控制模型的动力学公式为：
其中：fext为足部受力或力矩，fvir为虚拟力或力矩，kp为弹簧阻尼质量块模型的劲度系数，kd为弹簧阻尼质量块模型的阻尼系数，m为弹簧阻尼质量块模型的虚拟质量，e为弹簧阻尼质量块模型位置或角度偏移量输出。
4.根据权利要求2所述的双足机器人足部柔顺平衡控制方法，其特征在于，所述控制规律包括站立情况下机器人在矢状面上的控制规律和站立情况下机器人在冠状面上的控制规律。
5.根据权利要求4所述的双足机器人足部柔顺平衡控制方法，其特征在于，所述站立情况下机器人在矢状面上的控制规律和冠状面上的控制规律包括机器人单脚支撑地面和脚支撑地面两种情况。
6.根据权利要求5所述的双足机器人足部柔顺平衡控制方法，其特征在于，在矢状面上单脚支撑地面时，足部所受力矩方向与姿态偏离方向相同，且踝关节的虚拟力矩为：
7.根据权利要求6所述的双足机器人足部柔顺平衡控制方法，其特征在于，在矢状面上双脚支撑地面时，足部所受力矩方向与姿态偏离方向包括：情况①左脚所受力矩方向与姿态偏离方向相同，右脚所受力矩方向与姿态偏离方向相反；情况②左脚所受力矩方向与姿态偏离方向相反，右脚所受力矩方向与姿态偏离方向相同；情况③包括除情况①、情况②之外的其他情况。
8.根据权利要求7所述的双足机器人足部柔顺平衡控制方法，其特征在于，所述情况①中踝关节的虚拟力矩为：
9.根据权利要求5所述的双足机器人足部柔顺平衡控制方法，其特征在于，在冠状面上单脚支撑地面时，足部所受力矩方向与姿态偏离方向相同，且踝关节的虚拟力矩为：
10.根据权利要求9所述的双足机器人足部柔顺平衡控制方法，其特征在于，在冠状面上双脚支撑地面时，踝关节的虚拟力矩为：
