CN114248855B - 双足机器人空间域步态规划与控制的方法 - Google Patents

Abstract

本发明公开了双足机器人空间域步态规划与控制的方法，将双足机器人髋关节相对支撑腿踝关节的水平距离设为相变量δ，通过当前状态、参考步长及参考速度设计优化出基于相变量的摆动腿关节参考角度轨迹；完成空间域的摆动腿步态规划；建立二阶倒立摆模型，通过非线性模型预测控制优化出用于速度控制的支撑腿髋关节参考力矩；基于获得上述的摆动腿关节参考角度轨迹以及支撑腿髋关节参考力矩之后，利用全身动力学Quadratic Programming控制在多目标任务中优化出关节力矩，进而控制机器人步态行走。本方法将摆动腿踝关节的运动与时间脱离开来，使其仅依赖自身状态，增强鲁棒性。

Description

双足机器人空间域步态规划与控制的方法

技术领域

本发明属于机器人控制技术领域，尤其是双足机器人空间域步态规划与控制的方法。

背景技术

双足机器人是拥有与人类外形、双足相似特征的智能机器人，也成为人们想象中能代表机器人的最终具体形式，它可以实现拟人的灵巧行走动作，对未知环境的适应能力相比轮式、履带机器人更强，大大减少专用机器人需要对环境改造的成本。双足机器人是集多门基础学科技术于一体、配合仿生学、控制理论、计算机技术及人工智能等学科发展的综合性应用平台系统。近年来，双足机器人在结构、驱动和控制的研究更加依赖人类仿生学基础理论，而反过来双足机器人的研究也同时展示出人类运动的一般规律，相互之间的促进对特殊人群的服务和提高人类生活水平具有极其重要的作用，随着人民日益增长的对美好生活的需要，仿人服务机器人将具有很大市场。所以，双足机器人具有重要的研究意义与应用价值。

双足机器人的动态行走能够适应复杂环境而被广泛研究和使用，但通常需要通过动态调节落脚点达到自身平衡稳定和速度控制的目的。但双足机器人会遇到如下两种情况：1、双足机器人需要通过梅花桩类型的固定落脚点地形，那机器人将出现落脚点被限制并且还需要稳定平衡的冲突需求，从而无法适应而失去稳定；2、如果固定落脚点，那么在行走运动中，由于外部冲击或环境地形变化的干扰，如被冲撞或踩空等情况发生时，机器人非常容易由于调节不迅速而失去稳定。

所以在现有技术基础上，使双足机器人既能够准确踩在期望的落脚位置上，还能够拥有强大的环境适应性与抗扰动能力，这将提升双足机器人的应用价值。

现有公开的文献，类似的论文如“Dynamic Walking on Stepping Stones withGait Library and Control Barrier Functions”“Dynamic bipedal locomotion overstochastic discrete terrain”“3D Dynamic Walking on Stepping Stones withControl Barrier Functions”，但是其轨迹是通过离线的优化建立的动作库，然后通过在线插值实现的离散地形运动，这种方法在机器人自由度增多时将消耗大量时间产生轨迹库，并且对未构建的环境适应不强。

发明内容

为了解决现有技术中存在的不足，本发明提出了双足机器人空间域步态规划与控制的方法，将摆动腿轨迹与自身状态建立关系，那么机器人在空间中的姿态将能够被完整预测，就可以规划适合环境的步态；通过支撑腿的髋关节对速度控制的方法，以及全身动力学Quadratic Programming(以下简称QP)优化的多目标协同，在线生成步态轨迹，从而能够适应更多的环境变化。

本发明所采用的技术方案如下：

双足机器人空间域步态规划与控制的方法，将双足机器人髋关节相对支撑腿踝关节的水平距离设为相变量δ，通过当前状态、参考步长及参考速度设计优化出基于相变量的摆动腿关节参考角度轨迹；完成空间域的摆动腿步态规划；

建立二阶倒立摆模型，通过非线性模型预测控制优化出用于速度控制的支撑腿髋关节参考力矩；

基于获得上述的摆动腿关节参考角度轨迹以及支撑腿髋关节参考力矩之后，利用全身动力学QP控制在多目标任务中优化出关节力矩，进而控制机器人步态行走。

进一步，空间域的摆动腿步态规划如下：

S1、设定髋关节相对支撑腿脚踝的水平距离x_h-a为相变量δ＝x_h-a，记开始时的x_h-a记为

期望x_h-a能够到达参考步长

的一半，因此归一化后的相变量的取值s表示为：

S2、在x方向上，使用简单的五次插值方法规划，函数变量是s，让摆动腿脚踝x轴坐标x_f从上一步长

连续变化到达

当s＝0和s＝1时曲线的速度和加速度为零，从而得到参考的摆动腿脚踝x轴坐标轨迹

及其速度

S3、在z方向上，通过优化贝塞尔曲线参数来规划参考的摆动腿脚踝z轴坐标

轨迹；设优化变量为ξ＝[α₀ α₁…α_M]^T，通过建立二次优化问题，来获得符合要求的贝塞尔曲线，其中，α_M为第M+1个系数。

进一步，摆动腿脚踝z轴坐标

轨迹表示为：

摆动腿脚踝z轴坐标速度

为：

其中，M是贝塞尔曲线的维度，α_k为系数，k为求和运算迭代变量，(·)！表示阶乘运算。

进一步，所构建建立二次优化问题表示为：

ξ>0

其中，

为当s＝ε时摆动腿脚踝的期望高度，优化目标是使其约等于期望的最大抬脚高度

优化问题的约束包括当s＝0时位置和速度分别为零，当s＝1时位置和速度也分别为零。

进一步，以支撑腿脚踝为坐标原点构建二阶倒立摆模型，表示为：

其中，M_ip表示动力学的惯性矩阵，N_ip表示动力学的科氏力矩阵，G_ip表示动力学的重力向量，B_ip表示力矩筛选矩阵，u是二阶倒立摆两个连杆连接关节的驱动力矩；

表示模型速度向量，

表示模型加速度向量。

进一步，使用非线性模型预测控制得到控制速度的关节驱动力矩u，设控制周期时间为Δt，优化变量设为

(·)^T运算表示求向量或矩阵的转置，其中

分别是t₀、t₀+Δt、t₀+nΔt时刻对应的关节驱动力矩，计数量n＝T/Δt。设定非线性优化目标如下：

其中||·||²表示2范数运算，W_v和W_u分别是两个范数运算的权重矩阵。同时优化问题的约束条件分别如下：

其中(·)^k的上角标表示该量在第k次计数时的数值，

和

分别是该计数下二级摆与竖直方向的最大和最小角度，

和

分别是该计数下关节驱动力矩最大和最小值，最终优化得到的结果中

其他量将被舍弃。

进一步，利用全身动力学QP控制在多目标任务中优化出关节力矩的方法包括如下步骤：

S1、双足机器人全身动力学的浮动基模型表示为：

其中，q^s表示机器人广义坐标系的位置，

是q^s的微分向量，分别表示速度和加速度；M^s,C^s,G^s分别表示动力学的惯性矩阵、科氏力矩阵和重力向量，τ表示驱动力矩向量，B^s表示力矩筛选矩阵；

分别表示右腿和左腿末端受到的外界力，而J_r,J_l表示右腿和左腿末端在世界坐标系中的雅可比矩阵；

S2、构造QP问题，设优化变量包括模型加速度

驱动力矩τ和右左脚末端的外界力

则优化的目标函数表示为：

其中，

是广义坐标的期望加速度；τ^last表示驱动力矩向量的上一次值；

W_h、W_τ分别是目标函数中各范数的权重矩阵；χ∈{r,l}表示摆动腿；

表示摆动腿雅可比矩阵的导数，

表示期望的摆动腿踝关节加速度；

S3、优化问题的约束条件还包括驱动力矩的边界限制和脚末端外力的摩擦锥限制，分别表示为：

驱动力矩的边界限制：

τ_min≤τ≤τ_max

其中，τ_min和τ_max分别是关节驱动力矩的最小值和最大值向量；

脚末端外力的摩擦锥限制：

其中，上式第一个表示摆动腿末端受力

应该为零，定义Θ(χ)表示支撑腿，即假设χ＝r时Θ(χ)＝l，第二个式子表示支撑腿的竖直接触力

要大于零，μ是摩擦系数，

表示支撑腿x方向接触力，第三个式子表示摩擦锥约束。

进一步，通过PD控制器计算得到期望的摆动腿踝关节加速度

表示为：

其中，A和B分别表示PD参数矩阵，x_f是摆动腿脚踝x轴坐标，

是是摆动腿脚踝x轴速度；z_f是摆动腿脚踝z轴坐标，

是摆动腿脚踝z轴速度。

本发明的有益效果：

1、本方法利用空间域中相变量与姿态的关系，进行摆动腿轨迹的规划方法。将摆动腿踝关节的运动与时间脱离开来，使其仅依赖自身状态，增强鲁棒性。

2、在第一点之上，利用支撑腿的髋关节进行速度的控制。本方法首先将机双足机器人以支撑点为原点映射为二阶倒立摆，在非线性模型预测控制中得到更好控制速度跟随的髋关节力矩，能够实现精准优化的速度控制。

3、此外，本方法还通过全身动力学QP控制实现上述的参考目标，使其更复合机器人模型与关节驱动能力。

附图说明

图1是本发明空间域步态规划与控制流程示意图；

图2是本发明空间域步态规划示意图；

图3是本发明双足机器人模型到二阶倒立摆模型映射。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

双足机器人空间域步态规划与控制的方法，具体实现分为空间域的摆动腿步态规划、二阶倒立摆对髋关节速度的控制方法以及全身动力学QP优化控制方法。双足机器人在矢状面和冠状面上的结构相同，因此以下以矢状面为例进行方法的描述，而冠状面的思路完全相同。本方法的控制框图如图1所示。

首先，通过上层规划(可以是用户输入)获得步行参考值(即参考步长

和参考速度

)，将双足机器人髋关节相对支撑腿踝关节的水平距离设为相变量δ，通过当前状态、参考步长及参考参考速度设计优化出基于相变量的摆动腿关节参考角度轨迹；完成空间域的摆动腿步态规划；上述当前状态是指上身姿态信息、移动速度、还有关节角度和角速度信息，由这些信息可以确定机器人在空间中的位置和姿态。同时通过二阶倒立摆映射获得控制速度的支撑腿髋关节参考力矩，基于获得上述的摆动腿关节参考角度轨迹以及支撑腿髋关节参考力矩之后，利用全身动力学QP控制在多目标任务中优化出关节力矩，进而控制机器人步态行走。

如图2，空间域的摆动腿步态规划的具体过程如下：

以平面7连杆双足机器人模型为例，如图2所示，在右手系中的x-z平面内讨论，将支撑腿的踝关节设定称坐标原点。设定髋关节hip相对支撑腿脚踝ankle的水平距离x_h-a为相变量δ＝x_h-a，当机器人从左向右摆动身体时，这个量单调递增。则摆动腿在这一过程中，空间位置应该如图虚线所示轨迹一样，从支撑腿后面摆动到前面。摆动腿的轨迹起始于脚离地瞬间，终止于触地瞬间，将开始时的x_h-a记为

结束时我们希望x_h-a能够到达参考步长

的一半，因此归一化后的相变量的取值s为：

随着相变量δ的变化，s在0-1之间连续变化。我们将摆动腿末端的轨迹规划分为x方向和z方向分别进行。在x方向上，使用简单的五次插值方法规划，函数变量是s，让摆动腿脚踝x轴坐标x_f从上一步长

连续变化到达

当s＝0和s＝1时曲线的速度和加速度为零，从而得到参考的摆动腿脚踝x轴坐标轨迹

及其速度

在z方向上，通过优化贝塞尔曲线参数来规划参考的摆动腿脚踝z轴坐标

轨迹：

其中，M是贝塞尔曲线的维度，该曲线被M+1个系数α_k完全定义；k为求和运算迭代变量，(·)！表示阶乘运算。则参考的摆动腿脚踝z轴坐标速度

为：

根据环境信息和参考步态等其他信息，我们可以优化出一条满足约束的曲线。设优化变量为ξ＝[α₀ α₁…α_M]^T，建立二次优化问题：

ξ>0

其中，

为当s＝ε时摆动腿脚踝的期望高度，优化目标是使其约等于期望的最大抬脚高度

优化问题的约束包括当s＝0时位置和速度分别为零，当s＝1时位置和速度也分别为零。通常我们选择M＝8,ε＝0.5来获得符合要求的贝塞尔曲线。

因此，当双足机器人运动时，相变量δ的变化引起s在0-1之间变化，通过上述规划方法即可获得对应相变量δ的摆动腿踝关节期望坐标和速度信息。对于摆动腿的脚掌，只需要让其始终与地面平行即可。而对于支撑腿的轨迹，要求在支撑期间长度不变，也就是膝关节的角度不变保持恒定值

控制运动速度的支撑腿髋关节参考力矩

的确定：

由于上述步态规划中不涉及时间，因此步态轨迹无法形成对速度的约束，也就无法达到双足机器人的参考速度

考虑到支撑腿髋关节可以摆动上身影响自身动力学特性，从而控制机器人的行走速度，因此本方法提出基于二阶倒立摆的非线性模型预测控制(Nonlinear Model Predictive Control:NMPC)方法来控制双足机器人达到参考速度。如图3所示，可以将七连杆双足机器人以支撑腿脚踝为坐标原点简化为右边的二阶倒立摆模型，图中l₀为二阶倒立摆中第一级摆的长度，其等于双足机器人髋关节与支撑腿脚踝连线的长度；m₁和I₁分别表示第一级摆的质量和惯量，它们等于支撑腿的质量和惯量，同样c₁表示质心相对转动轴的长度；m₂和I₂分别表示第二级摆的质量和惯量，它们等于上身躯干的总质量和总惯量，同样c₂表示质心相对转动轴的长度；β₁和β₂分别表示一级摆相对竖直方向的夹角和二级摆相对一级摆的夹角，顺时针方向为正。通过上述描述可知当已知机器人的状态时即可计算出上述各参数的值。而且机器人的实际速度v_x刚好与二阶倒立摆中一级摆与二阶摆连接关节的速度相等。

利用拉格朗日方程法建立二阶倒立摆动力学模型：

上式中，

表示动力学的惯性矩阵，

表示动力学的科氏力矩阵，

表示动力学的重力向量，

表示力矩筛选矩阵即将驱动力矩作用到相应自由度上，u是二阶倒立摆两个连杆连接关节的驱动力矩。

表示实数集其上角标的数字n表示向量或矩阵的维度。

表示模型速度向量，

表示模型加速度向量。

为了更好的控制双足机器人实现参考速度，我们可以从当前t₀时刻开始，预测t₀+T时段内系统状态，由于二阶倒立摆的动力学模型是非线性的，所以使用非线性模型预测控制(Nonlinear Model Predictive Control:NMPC)得到控制速度的关节驱动力矩u。设控制周期时间为Δt，优化变量设为

(·)^T运算表示求向量或矩阵的转置，其中

分别是t₀、t₀+Δt、t₀+nΔt时刻对应的关节驱动力矩，计数量n＝T/Δt。设定非线性优化目标如下：

其中||·||²表示2范数运算，W_v和W_u分别是两个范数运算的权重矩阵。同时优化问题的约束条件分别如下：

其中(·)^k的上角标表示该量在第k次计数时的数值，

和

分别是该计数下二级摆与竖直方向的最大和最小角度，

和

分别是该计数下关节驱动力矩最大和最小值，最终优化得到的结果中

其他量将被舍弃。

全身动力学QP优化控制过程：

图2所示平面双足模型包括1个上身驱干、2个大腿、2个小腿和2个脚掌，使用θ表示相邻连杆间的相对角度，上角标r,l分别表示右腿(right)和左腿(left)，下角标h,k,a分别表示髋关节(hip)、膝关节(knee)和踝关节(ankle)，而x,z表示上身质心一点在世界坐标系中的位置坐标，下角标tor表示上身躯干(torso)，θ_tor表示上身与竖直方向的夹角即俯仰角度。利用拉格朗日方程法计算得到双足机器人全身动力学的浮动基模型：

其中，

表示机器人广义坐标系的位置，

是q^s的微分向量分别表示速度和加速度。

分别表示动力学的惯性矩阵、科氏力矩阵和重力向量，

表示驱动力矩向量，

表示力矩筛选矩阵，即将驱动力矩作用到相应自由度上。

分别表示右腿和左腿末端受到的外界力，而

表示右腿和左腿末端在世界坐标系中的雅可比矩阵。

为使机器人实现上述的多个期望参考状态，构造QP问题，设优化变量包括模型加速度

驱动力矩τ和右左脚末端的外界力

优化的目标函数为：

是广义坐标的期望加速度，包括支撑腿膝关节与踝关节分别通过PD控制器计算出的期望角加速度，以及上身俯仰角度通过PD控制器计算出的期望角加速度。τ^last表示驱动力矩向量的上一次值，这样做是为了避免力矩出现过大跳变。

W_h、W_τ分别是目标函数中各范数的权重矩阵。χ∈{r,l}表示摆动腿，它可以是右腿也可以是左腿，

表示摆动腿雅可比矩阵的导数，

表示期望的摆动腿踝关节加速度，它可以通过PD控制器计算得到：

该式中

和

分别表示PD参数矩阵。优化问题的约束条件除了上述双足机器人全身动力学的方程外，还有驱动力矩的边界限制：

τ_min≤τ≤τ_max

τ_min和τ_max分别是关节驱动力矩的最小值和最大值向量。还有脚末端外力的摩擦锥限制：

上式第一个表示摆动腿末端受力应该为零，定义Θ(χ)表示支撑腿，既假设χ＝r时Θ(χ)＝l，第二个式子表示支撑腿的竖直接触力要大于零，μ是摩擦系数。

通过上述的规划与控制，最终向双足机器人关节下发优化出的力矩τ完成行走运动。综上，本申请所提出的双足机器人空间域步态规划与控制方法能够解决如下技术问题：

1、解决双足机器人运动中多种时变因素带来的不确定扰动从而降低环境适应性的问题。传统规划中，机器人的步态序列是通过时间来索引的，既t时刻对应状态序列x(通常包括期望的位置和速度信息)。但双足机器人在实际环境中运行时，由于机器人执行误差、非结构环境的建模不完整和未知突发扰动的存在，理想步态轨迹将变的不再稳定。比如，机器人想往前迈脚下踩时，由于某种原因发生后仰，如果按照预期轨迹执行，随着时间的变化机器人必将踩空失去稳定。但如果步态序列是基于空间域中某个变量的，那么机器人后仰时摆动腿将回退回上一步的起点。

2、解决双足机器人调节速度时无法保证期望步长的任务式需求。在机器人的动态行走中，摆动腿往往为了控制期望速度需要不断变换落脚点，这就很难实现期望的特定步长。因此使用上身的摆动控制速度就能保证摆动腿的期望落脚位置，并且控制落脚时间从而实现速度的精准跟踪，而且如果能预测未来一段时间模型变化将更好决策当前控制量信息。

3、解决多目标任务的耦合。要想实现既控制摆动腿轨迹又控制支撑腿髋关节力矩，单独实现各目标任务的耦合现象将使控制混乱不堪，因此通过全身动力学QP(Quadratic Optimization：QP)优化，在多约束条件下寻找最优解能够大大减弱耦合影响。

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (6)

1.双足机器人空间域步态规划与控制的方法，其特征在于，将双足机器人髋关节相对支撑腿踝关节的水平距离设为相变量δ，通过当前状态、参考步长及参考速度设计优化出基于相变量的摆动腿关节参考角度轨迹；完成空间域的摆动腿步态规划；

以支撑腿脚踝为坐标原点构建二阶倒立摆模型，表示为：

其中，M_ip表示动力学的惯性矩阵，N_ip表示动力学的科氏力矩阵，G_ip表示动力学的重力向量，B_ip表示力矩筛选矩阵，u是二阶倒立摆两个连杆连接关节的驱动力矩；

表示模型速度向量，

表示模型加速度向量，通过非线性模型预测控制优化出用于速度控制的支撑腿髋关节参考力矩；

基于获得上述的摆动腿关节参考角度轨迹以及支撑腿髋关节参考力矩之后，利用全身动力学QP控制在多目标任务中优化出关节力矩，进而控制机器人步态行走；

空间域的摆动腿步态规划如下：

S1、设定髋关节相对支撑腿脚踝的水平距离x_h-a为相变量δ＝x_h-a，记开始时的x_h-a为

期望x_h-a能够到达参考步长

的一半，因此归一化后的相变量的取值s表示为：

S2、在x方向上，使用五次插值方法规划，设s是变量，让摆动腿脚踝x轴坐标x_f从上一步长

连续变化到达

当s＝0和s＝1时曲线的速度和加速度为零，从而得到参考的摆动腿脚踝x轴坐标轨迹

及其速度

S3、在z方向上，通过优化贝塞尔曲线参数来规划参考的摆动腿脚踝z轴坐标

轨迹；设优化变量为ξ＝[α₀ α₁…α_M]^T，通过建立二次优化问题，来获得符合要求的贝塞尔曲线，其中，α_M为第M+1个系数。

2.根据权利要求1所述的双足机器人空间域步态规划与控制的方法，其特征在于，摆动腿脚踝z轴坐标

轨迹表示为：

摆动腿脚踝z轴坐标速度

为：

其中，M是贝塞尔曲线的维度，α_k为系数，k为求和运算迭代变量，(·)！表示阶乘运算。

3.根据权利要求1所述的双足机器人空间域步态规划与控制的方法，其特征在于，所构建建立二次优化问题表示为：

ξ>0

其中，

为当s＝ε时摆动腿脚踝的期望高度，优化目标是使其约等于期望的最大抬脚高度

优化问题的约束包括当s＝0时位置和速度分别为零，当s＝1时位置和速度也分别为零。

4.根据权利要求1所述的双足机器人空间域步态规划与控制的方法，其特征在于，使用非线性模型预测控制得到控制速度的关节驱动力矩u，设控制周期时间为Δt，优化变量设为

(·)^T运算表示求向量或矩阵的转置，其中

分别是t₀、t₀+Δt、t₀+nΔt时刻对应的关节驱动力矩，计数量n＝T/Δt；设定非线性优化目标如下：

其中||·||²表示2范数运算，W_v和W_u分别是两个范数运算的权重矩阵；同时优化问题的约束条件分别如下：

其中(·)^k的上角标表示该量在第k次计数时的数值，

和

分别是该计数下二阶倒立摆模型的第二级摆与竖直方向的最大和最小角度，

和

分别是该计数下关节驱动力矩最大和最小值，最终优化得到的结果中

其他量将被舍弃。

5.根据权利要求1所述的双足机器人空间域步态规划与控制的方法，其特征在于，利用全身动力学QP控制在多目标任务中优化出关节力矩的方法包括如下步骤：

S1、双足机器人全身动力学的浮动基模型表示为：

其中，q^s表示机器人广义坐标系的位置，

是q^s的微分向量，分别表示速度和加速度；M^s,C^s,G^s分别表示动力学的惯性矩阵、科氏力矩阵和重力向量，τ表示驱动力矩向量，B^s表示力矩筛选矩阵；

分别表示右腿和左腿末端受到的外界力，而J_r,J_l表示右腿和左腿末端在世界坐标系中的雅可比矩阵；

S2、构造QP问题，设优化变量包括模型加速度

驱动力矩τ和右左脚末端的外界力

则优化的目标函数表示为：

其中，

是广义坐标的期望加速度；τ^last表示驱动力矩向量的上一次值；

W_h、W_τ分别是目标函数中各范数的权重矩阵；χ∈{r,l}表示摆动腿；

表示摆动腿雅可比矩阵的导数，

表示期望的摆动腿踝关节加速度；

S3、优化问题的约束条件还包括驱动力矩的边界限制和脚末端外力的摩擦锥限制，分别表示为：

驱动力矩的边界限制：

τ_min≤τ≤τ_max

其中，τ_min和τ_max分别是关节驱动力矩的最小值和最大值向量；

脚末端外力的摩擦锥限制：

其中，上式第一个表示摆动腿末端受力

应该为零，定义Θ(χ)表示支撑腿，即假设χ＝r时Θ(χ)＝l，第二个式子表示支撑腿的竖直接触力

要大于零，μ是摩擦系数，

表示支撑腿x方向接触力，第三个式子表示摩擦锥约束。

6.根据权利要求4所述的双足机器人空间域步态规划与控制的方法，其特征在于，通过PD控制器计算得到期望的摆动腿踝关节加速度

表示为：

其中，A和B分别表示PD参数矩阵，x_f是摆动腿脚踝x轴坐标，

是摆动腿脚踝x轴速度；z_f是摆动腿脚踝z轴坐标，

是摆动腿脚踝z轴速度。

Images