CN110244791B - 一种双足机器人足部力和力矩跟随控制方法 - Google Patents

Abstract

本发明公开了一种双足机器人足部力和力矩跟随控制方法，该方法设计了双弹簧阻尼模型，并用LQR优化方法设计力跟随控制器，实现了双足机器人足部受力和力矩的跟随；而足部期望受力、足部期望力矩由规划ZMP分配方法计算得到，最终使得双足机器人的ZMP跟随更好，且能够适应一定的不平整地面。本发明舍弃了传统的ZMP跟随以实现双足机器人稳定行走和不平整地面适应的控制方式，直接计算使机器人稳定行走的足部期望受力和受力矩，并直接控制实现足部受力和受力矩的跟随，从更加本质、更容易实现的方式进行稳定控制，控制响应更快，适应不平整地面的能力更强，而ZMP跟随效果很理想。

Description

一种双足机器人足部力和力矩跟随控制方法

技术领域

本发明属于机器人技术领域，具体涉及一种双足机器人足部力和力矩跟随控制方法。

背景技术

零力矩点ZMP是双足机器人研究中非常重要的概念，其对双足机器人的行走规划、稳定等有举足轻重的作用。评估双足机器人行走是否的稳定地一个重要手段是实际ZMP是否能够良好地跟随规划的ZMP。而足部受到的力和力矩可以体现机器人的ZMP情况，一般也是通过足部受到的力和力矩来计算机器人在平地上行走时实际的ZMP，故可以通过控制机器人实际的足部力和力矩，使其能够跟随规划的力和力矩，以此来实现ZMP的跟随，并使机器人行走地更加稳定。

这个方法从物理的角度来看，能够使得足部一直提供机器人稳定行走所需要的力和力矩，所以理论上还能实现让机器人在任意不平整地面上行走的效果，但由于机器人***带来的滞后以及冲击带来的各种误差，力和力矩的控制效果无法达到理论的理想状态，机器人只能适应较小的地面起伏，但对于位置控制机器人来说，这也是很难得可贵的。

现有技术公开了双足机器人不平整地面行走的稳定控制方法，该方法结合柔顺控制，能够减小机器人足部着地冲击，以保证足部落地时的ZMP在脚底板支撑域内，以满足机器人稳定行走条件，但没有考虑ZMP能够尽量跟随规划值，更没有考虑足部力和力矩的跟随情况，仅仅控制ZMP在支撑域内，是无法解决机器人在不平整地面上行走的稳定性问题的。现有技术公开了一种基于全身动量补偿的ZMP跟随方法，能够利用全身关节的转动产生的动量来补偿ZMP的差值，但未考虑机器人在不平整地面上行走的状况，也未考虑机器人足部的受力情况，也无法应对不平整地面的ZMP跟随问题。

现有技术还提出了利用规划ZMP计算机器人规划力和力矩的方法，其重心并不在于使足部力和力矩跟随该规划值已达到不平地面行走的目的，故只采用了非常简单的导纳控制器来控制以达到调整效果，但控制器可调节的范围太小，无法满足略带突起与凹陷的不平整地面的控制要求。

综上，现有的技术大多直接通过检测机器人的实际ZMP，通过控制手段让其跟随规划的ZMP，来实现机器人的稳定行走。但是使用ZMP跟随实现这一切，有两个弊端，一是计算不平整地面上的ZMP从理论上讲，既不方便，也不准确，所以无法良好地反应机器人的行走状态，也就是说在不平整的地面上单使用ZMP跟随的控制方法效果必然不会太好，不能一味地追求通过计算得到的机器人ZMP去跟随规划值。另一方面，由于力传感器的冲击和误差，从足部受力和力矩计算而来的ZMP也非常不准，加上理论上的误差，这样计算出来的机器人真实ZMP一点也不真实，这就必然对控制效果造成影响。

发明内容

为了解决现有技术中存在的问题，本发明公开了一种双足机器人足部力和力矩跟随控制方法，不仅能够使双足机器人在平地上实现实际ZMP对规划ZMP的良好跟随，还能很好地实现较小不平整地面的稳定行走，另外，力跟随控制器控制模型的设计，还能减小足部在着地时受到的冲击。

本发明是通过以下技术方案实现上述技术目的的。

一种双足机器人足部力和力矩跟随控制方法，通过规划的机器人零力矩点分配机器人足部期望受力和足部期望力矩，将足部期望受力、足部期望力矩分别与足部实际受力、足部实际力矩作差，差值作为力跟随控制器的输入量，力跟随控制器输出踝关节三个方向的位置调节量，并将位置调节量加在原有的踝关节轨迹规划，得到调节后的踝关节轨迹，通过逆运动学得到各关节角度，实现机器人足部力和力矩的跟随，继而实现ZMP跟随。

进一步，所述踝关节三个方向包括踝关节的竖直方向、俯仰方向和翻滚方向。

进一步于，所述足部实际受力、足部实际力矩是通过机器人踝部的六维力传感器来计算。

进一步，所述足部期望受力、足部期望力矩由规划ZMP分配方法计算得到。

更进一步，所述规划ZMP分配方法计算的具体过程为：

(1)计算单脚支撑期的足部期望受力和力矩

根据ZMP与两脚踝关节之间的距离之比计算并分配左脚和右脚的竖直方向足部期望受力，再将ZMP与两脚踝关节距离分别与两期望受叉乘，分别得到两脚的足部期望力矩；

(2)计算双脚支撑期的足部期望受力和力矩

足部期望受力的计算方法与单脚支撑期相同，计算足部期望力矩时，先用计算单脚支撑期足部期望力矩的方法计算两足的期望力矩之和，再根据ZMP与两脚踝关节之间的距离之比计算并分配左脚和右脚的足部期望力矩。

进一步，所述力跟随控制器的表达式为：

其中e为两个弹簧阻尼***的形变量之和，F为输入外力，F_old为上一个控制周期的输入力，e_old为上一控制周期的输出位置调节量，

为上一控制周期的输出位置调节量导数，T_CONTROL为控制周期。

进一步，所述两个弹簧阻尼***串联组成双弹簧阻尼模型，每个弹簧阻尼***由弹簧和阻尼并联而成。

本发明的有益效果为：

(1)本发明通过ZMP规划值计算机器人足部受力和力矩的规划值。

(2)本发明利用导纳控制来实现由力的误差输出位置调节量，需要使用其他控制手段来减弱导纳控制作用于机器人***产生的滞后带来的影响。

(3)本发明使用双弹簧阻尼模型设计导纳控制器，具有吸收冲击、柔顺的效果，并能达到良好地力和力矩跟随效果。

(4)本发明改进了由ZMP分配计算机器人足部规划力和力矩的方法，能够更加真实地反应理论上机器人足部应提供的力和力矩。

(5)本发明将ZMP的跟随反映到足部力和力矩的跟随，在考虑机器人稳定行走的问题上，尝试解决更加根本的问题，能够适应不平整度更大的地面，实际的ZMP跟随效果也比较理想。

附图说明

图1为本发明双足机器人足部力和力矩跟随控制流程图；

图2为由六维力传感器计算足部实际受力过程示意图；

图3为本发明双足机器人足部z方向受力映射示意图；

图4为世界坐标系中机器人双脚踝关节位置以及ZMP位置规划示意图；

图5为本发明双弹簧阻尼模型示意图；

图6为足部受力跟随效果实验数据曲线图，图6(a)为左脚受力跟随情况曲线图，图6(b)为右脚受力跟随情况曲线图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，一种双足机器人足部力和力矩跟随控制方法，通过规划的机器人零力矩点ZMP来分配机器人足部期望受力和足部期望力矩，将足部期望受力、足部期望力矩分别与足部实际受力、足部实际力矩作差，作为力跟随控制器的输入量，力跟随控制器输出踝关节三个方向(分别是踝关节的竖直方向、俯仰方向Pitch和翻滚方向Roll)的位置调节量，并将该踝关节位置调节量加在原有的踝关节轨迹规划，得到调节后的踝关节轨迹，通过逆运动学(IK)得到各关节角度，实现机器人足部力和力矩的跟随，而实现ZMP跟随。足部实际受力、足部实际力矩是通过机器人踝部的六维力传感器测量并计算，具体的计算过程为：

如图2所示，F_z.sensor为力传感器测得的足部坐标系下z方向受力，F_y.sensor为力传感器测得的足部坐标系下y方向受力，τ_sensor力传感器测得的足部坐标系下x方向力矩。需要根据力传感器测得的力和力矩计算得到机器人腰部坐标系下实际的力和力矩。

当机器人足部转动时，足部坐标系的姿态不再与腰部坐标系相同，导致力传感器测得的F_z.sensor与用于本发明的控制方法的F_z有出入，需要先将F_z.sensor向腰部坐标系映射，得到F_z。如图3所示，坐标系O为机器人腰部坐标系，是相对机器人自身的有效坐标系。由于机器人腿部自由度的安排顺序，先进行Pitch方向转动，再进行Roll方向转动，其转角分别为q5、q6。再将F_z.sensor投影到坐标系O的z轴上，计算实际受力F_z：

F_z＝F_z.sensor·cosq₆·cosq₅ (1)

再计算力矩，以计算x方向力矩为例，利用下式进行计算实际力矩τ：

τ＝F_y.sensor·H_Ankle+τ_sensor (2)

其中H_Ankle为传感器几何中心与踝关节轴心的高度差。

机器人足部期望受力和足部期望所受力矩的计算采用由规划ZMP分配方法实现。先计算出足部期望受力和足部期望力矩，即机器人足部给地面的期望力和力矩，而期望受力和期望受力矩为机器人足部给地面的期望力和力矩的相反数。如图4所示，单脚支撑期期望力和力矩公式为：

其中：

为左脚的期望力和力矩，

为右脚的期望力和力矩，Mg为机器人全身重力，p_{Ankle_L}、p_{Ankle_R}分别为机器人期望的左、右脚踝关节在世界坐标系中的位置，

为机器人期望的ZMP在世界坐标系中的位置，通常将机器人复位状态下，两踝连线的线段中点作为该坐标系的零点(图4中点O)；此外，比例系数α的计算方法如下：

其中：p_{Edge_L}、p_{Edge_R}为机器人左、右脚的内侧边缘。

由式(2)可以看出，当

大于p_{Ankle_R}时，α出现负值，左脚期望受力方向向上，这不合乎常理；当

小于p_{Ankle_L}时，(1-α)出现负值，右脚期望受力方向向上，这同样不合乎常理，故需要对α的范围进行限制，在实际中的意义就是当机器人规划的ZMP移到任意一只脚的踝关节正下方或踝关节外侧时，就认为该脚支撑起了整个机器人的重量，判断方法如下：

故单脚支撑期以及ZMP在任意脚的支撑域内时，期望力和力矩就能用上述的方法计算；但双脚支撑期的其他时刻，就需要考虑双脚力矩的分配问题，由于机器人左右脚受到的力、力矩之和在ZMP点处的和力矩为零，左右脚和力矩τ的计算式如下：

双脚支撑期的足部力矩分配如下：

将计算出的期望力矩分配到x(翻滚方向)、y(俯仰方向)两个方向，即可算出左右脚各自的期望力矩。

力跟随控制器的设计方法如下：

如图5所示，构建的双弹簧阻尼模型，由两个弹簧阻尼***串联而成，每个弹簧阻尼***由弹簧和阻尼并联而成，弹簧1的形变量为e1、弹性系数为K1、阻尼系数为D1，弹簧2的形变量为e2、弹性系数为K2、阻尼系数为D2，设弹簧原长状态下的e1、e2、两个弹簧阻尼***的形变量之和e都为零，且e＝e1+e2，F为输入外力，则两个弹簧分别满足下式：

将两式进行拉氏变换：

将弹簧形变量之和拉氏变换得到：

e(s)＝e₁(s)+e₂(s) (10)

将式(9)代入式(10)得到：

拉氏反变换即可得到e与F的关系：

将上式线性离散化，得到力跟随控制器的表达式为：

其中，F_old为上一个控制周期的输入力，e_old为上一控制周期的输出位置调节量，

为上一控制周期的输出位置调节量导数，T_CONTROL为控制周期。

使用LQR对力跟随控制器系数进行优化，在调出较好的控制器系数后，就可以实现较理想的力和力矩跟随效果，并且由于使用了双弹簧阻尼模型，实际应用时足部带有一定的柔顺效果。在设置两个弹簧的弹性系数和阻尼系数时，配置成大刚度-中等阻尼及小刚度-大阻尼的组合形式，具体数据见表1：

表1两个弹簧的弹性系数和阻尼系数

K1	10000
		D1	5700
K2	2000000
		D2	2000

这样足部能够有吸收冲击的效果，并产生不错的力和力矩跟随效果，见图6，图6(a)为左脚受力跟随情况曲线图，图6(b)为右脚受力跟随情况曲线图，由图可见实际受力能够很好地跟随期望受力，虽然存在由于冲击过大导致的抖动以及由于***惯性导致的小量滞后，总体跟随效果满足控制需求。

以上，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修饰改为等同变化的等效实施例。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

Claims (7)

1.一种双足机器人足部力和力矩跟随控制方法，其特征在于，通过规划的机器人零力矩点分配机器人足部期望受力和足部期望力矩，将足部期望受力、足部期望力矩分别与足部实际受力、足部实际力矩作差，差值作为力跟随控制器的输入量，力跟随控制器输出踝关节三个方向的位置调节量，并将位置调节量加在原有的踝关节轨迹规划，得到调节后的踝关节轨迹，通过逆运动学得到各关节角度，实现机器人足部力和力矩的跟随，继而实现ZMP跟随。

2.根据权利要求1所述的双足机器人足部力和力矩跟随控制方法，其特征在于，所述踝关节三个方向包括踝关节的竖直方向、俯仰方向和翻滚方向。

3.根据权利要求1所述的双足机器人足部力和力矩跟随控制方法，其特征在于，所述足部实际受力、足部实际力矩是通过机器人踝部的六维力传感器来计算。

4.根据权利要求1所述的双足机器人足部力和力矩跟随控制方法，其特征在于，所述足部期望受力、足部期望力矩由规划ZMP分配方法计算得到。

5.根据权利要求4所述的双足机器人足部力和力矩跟随控制方法，其特征在于，所述规划ZMP分配方法计算的具体过程为：

(1)计算单脚支撑期的足部期望受力和力矩

根据ZMP与两脚踝关节之间的距离之比计算并分配左脚和右脚的竖直方向足部期望受力，再将ZMP到两脚踝关节位置的向量分别与两期望受力叉乘，分别得到两脚的足部期望力矩；

(2)计算双脚支撑期的足部期望受力和力矩

足部期望受力的计算方法与单脚支撑期相同，计算足部期望力矩时，先用计算单脚支撑期足部期望力矩的方法计算两足的期望力矩之和，再根据ZMP与两脚踝关节之间的距离之比计算并分配左脚和右脚的足部期望力矩。

6.根据权利要求1所述的双足机器人足部力和力矩跟随控制方法，其特征在于，所述力跟随控制器的表达式为：

其中e为两个弹簧阻尼***的形变量之和，F为输入外力，F_old为上一个控制周期的输入力，e_old为上一控制周期的输出位置调节量，

为上一控制周期的输出位置调节量导数，T_CONTROL为控制周期。

7.根据权利要求6所述的双足机器人足部力和力矩跟随控制方法，其特征在于，所述两个弹簧阻尼***串联组成双弹簧阻尼模型，每个弹簧阻尼***由弹簧和阻尼并联而成。

Images