CN112051741B - 一种双足机器人动态运动生成与控制方法 - Google Patents

Abstract

本发明公开了一种双足机器人动态运动生成与控制方法，在单脚支撑期，通过建立全身动力学反馈线性化控制器控制支撑腿的长度和上身俯仰的姿态保持双足机器人动态运动特性；基于全身动力学模型解耦摆动腿与全身的动力学耦合，通过建立摆动腿预测控制模型控制在未来观测规划的位置，落脚点调节给出了未来腿末端一系列目标点，根据环境信息规划摆动腿末端轨迹、实时调整动腿的落脚点；由支撑腿的全身动力学力矩控制量和摆动腿的控制力矩一同作为双足机器人运动控制输入；本发明将双足机器人自身动力学引入实时规划和在线控制，解耦高维非线性系统，独立控制摆动腿达到动态稳定运动。

Description

一种双足机器人动态运动生成与控制方法

技术领域

本发明属于仿人机器人技术领域，尤其是一种双足机器人动态运动生成与控制方法。

背景技术

双足机器人是具有人类外形，双足等特征的智能机器人，也成为人们想象中代表机器人的具体形式，它可以实现类人的灵巧行走动作，对未知环境的适应能力强，大大减少专用机器人需要对环境改造的成本，同时代替人工劳动完成更加复杂工作任务等。双足机器人是集多门基础学科技术于一体，配合仿生学、控制理论及人工智能等学科发展的综合性应用平台系统，高级创新性密集的机电一体化平台可以评价国家科技发展水平。近年来，双足机器人在结构、驱动和控制的研究更加依赖人类仿生学基础理论，而反过来双足机器人的研究也同时展示出人类运动的一般规律，相互之间的促进对特殊人群的服务和提高人类生活水平具有极其重要的作用。所以，双足机器人具有重要的研究意义与应用价值。

双足机器人运动仍在静态与准静态平衡控制中，即规划的轨迹是实时稳定的，需要驱动关节精准跟随参考角度，但这样做的成本是高能耗、动作僵硬以及灵活度低。所以另一个研究方向是双足机器人动态运动的稳定控制，目标是在自身动力学基础上通过动态调整达到自身运动的稳定，不要求实时稳定性的做法会大大降低能耗需求。为了实现在线规划和实时控制，现有算法会简化模型，离线优化，可以想象这样会降低双足机器人与环境的动态运动高机动交互能力。现有技术中所存在的技术问题集中在：

在传统双足机器人运动生成与控制中，先通过简化模型生成离线运动稳定轨迹，然后在线调节使机器人稳定运动。但往往由于动力学耦合特性，这种稳定轨迹会在环境交互中被破坏，从而导致步行运动的失败。

现有的方法局限于自身适用的单一双足机器人类型。如有脚底板的机器人需要考虑零力矩点(Zero Moment Point:ZMP)，而没有脚底板的机器人只能通过点足与地面接触，这种基于ZMP方法将不再适用。而由于点足双足机器人的动态特性，适用于它的步态生成方法又无法在有脚底板的双足机器人上使用。

现有方法不能在线规划、实时调节与优化的技术问题。由于双足机器人系统方程具有高维非线性特性，在线优化往往是耗时的，从而无法实时优化。为了在固定控制周期中运行控制器，大规模在线规划和实时调节不被考虑。

在位置控制双足机器人中，ZMP误差补偿，柔顺控制等多种算法被同时用于稳定调节导致计算量繁复。

发明内容

针对现有技术中所存在的不足，本发明提出了一种双足机器人动态运动生成与控制方法，将双足机器人自身动力学引入实时规划和在线控制，解耦高维非线性系统，独立控制摆动腿达到动态稳定运动。

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

一种双足机器人动态运动生成与控制方法，在单脚支撑期，通过建立全身动力学反馈线性化控制器控制支撑腿的长度和上身俯仰的姿态保持双足机器人动态运动特性；

基于全身动力学模型解耦摆动腿与全身的动力学耦合，通过建立摆动腿预测控制模型控制在未来观测规划的位置，落脚点调节给出了未来腿末端一系列目标点，根据环境信息规划摆动腿末端轨迹、实时调整动腿的落脚点；

由支撑腿的全身动力学力矩控制量和摆动腿的控制力矩一同作为双足机器人运动控制输入。

进一步，建立全身动力学反馈线性化控制器的方法为：

S1.1，建立双足机器人全身动力学的浮动基动力学模型，表示为：

其中，M、C、G分别表示动力学的惯性矩阵、科氏力矩阵和重力向量，B为力矩筛选矩阵，

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

S1.2，由双足机器人在坐标系的位置q^s和双足机器人的速度

组成状态变量

S1.3，基于状态变量x，将双足机器人动力学系统写成非线性控制系统的形式：

其中：f(x)为状态转移向量，g(x)为输入转移矩阵，u为驱动力矩向量；

S1.4，为控制期望支撑腿的腿长

和期望上身姿态

设计非线性控制系统的输出为y＝h(q^s)，h(q^s)是一个只与当前系统状态有关的函数；

S1.5，基于S1.3和S1.4，设计得到控制腿长和上身姿态的全身动力学控制量u_WBDC：

其中，

是输出函数y的一阶微分量，参数ε是满足1＞ε＞0的实数，需要在控制中适量选取，L_g、L_f分别为y沿着f(x)、g(x)的Lie导数。

进一步，S1.3中的f(x)和g(x)只依赖于q^s,

表示为：

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

分别表示右腿和左腿末端收到的外界力，J_r、J_l表示右腿和左腿末端在世界坐标系中的雅可比矩阵，0_n×m表示一个n行m列的零矩阵，B为力矩筛选矩阵。

进一步，输出函数y＝h(q^s)表示为

其中，θ_tor L_sup分别是当前上身俯仰角度实际值、支撑腿长度的实际值；

进一步，建立摆动腿预测控制模型的方法为：

S2.1，通过拉格朗日方程建立三连杆固定基模型描述摆动腿的运动趋势；

S2.2，由双足机器人摆动腿在坐标系的位置q和双足机器人的速度

构造状态变量

S2.3，基于状态变量x^sw得到摆动腿非线性控制系统：

其中，用f(x^sw,u^sw)表示

关于x^sw和u^sw的函数关系式，

是x^sw的一阶微分值，M^sw、C^sw、G^sw分别为摆动腿动力学的惯性矩阵、科氏力矩阵和重力向量；u^sw是由摆动腿髋关节驱动力矩、膝关节驱动力矩和踝关节驱动力矩组成的向量；

S2.4，建立摆动腿非线性控制系统后利用线性化理论在参考点处得到线性状态方程：

其中，^refx^sw是状态变量x^sw的参考值，^refu^sw是u^sw驱动力矩向量的参考值，A、B分别为摆动腿非线性控制系统f(x^sw,u^sw)对x^sw和u^sw的偏导数在参考值^refx^sw与^refu^sw处的向量矩阵；

S2.5，设^refu^sw＝0_3×1，设新的控制系统状态变量为ξ＝(x^sw-refx^sw)且

则线性状态方程表示为：

S2.6，对S2.5中的线性状态方程进行离散化，得到线性离散状态方程：

其中，A_k,B_k对应第k时刻参考值

时的系数矩阵，ξ_k、ξ_k+1分别是第k时刻、第k+1时刻的线性化系统状态变量，

为第k时刻由摆动腿髋关节驱动力矩、膝关节驱动力矩和踝关节驱动力矩组成的向量，

分别为ξ_k与

的系数矩阵，I_6×6表示6维的单位矩阵，采样周期为Δt；S2.7，设定预测步数NT_p，得到在预测步数内的摆动腿MPC预测模型Ξ_k表示为：

其中，

分别为Ψ_k和Φ_k系数矩阵，Ψ_k、Φ_k分别为MPC预测模型中的由线性离散状态组成的状态向量和输入向量，

S2.8，设定评价函数J^*使状态误差最小以及控制量即驱动力矩最小，则有：

对于优化问题这里只引入关节最大力矩τ_max的不等式约束：

-τ_max＜Φ_k＜τ_max

取经优化得到的多个控制量只取第一组作为摆动腿控制力矩给双足机器人，即

进一步，通过落脚点算法得到水平落脚点

而竖直方向的落脚点

其中，k_p和k_d均为正实数的增益系数，然后通过环境信息与当前摆动腿末端相对髋关节位置，可以规划出未来NT_p个控制周期落脚点参考位置即脚踝规划轨迹，通过摆动腿逆运动学可求解出i＝{k,k+1,…k+NT_p-1}时刻摆动腿关节参考角度和角速度

用于MPC预测模型中未来参考值。

本发明的有益效果：

1、全身动力学被解耦成支撑腿和摆动腿模型，解耦摆动腿与全身的动力学耦合，但保留了倒立摆自由摆动特性，无需踝关节的主动推动从而降低能耗。

2、本发明将传统关节角度控制转化为控制支撑腿长度，将腿长控制和上身姿态控制引入全身动力学，利用反馈线性化控制这一目标。

3、通过摆动腿的MPC预测模型控制落脚点，可以根据环境信息感知障碍实时规划落脚点，实时调整自身落脚点，提高了稳定性。

4、实时控制和调节共同完成了双足机器人动态运动生成和控制方法。

5、在模型预测控制的优化环节由于全身动力学模型被降维至单腿动力学模型，所以可在控制程序中实时优化。关节层次使用力矩控制可以增加机器人的柔顺性，减少与环境交互过程中的能量损失，还可以增加人机交互过程中的安全性。

附图说明

图1是双足机器人动态运动生成与控制框图；

图2是x-z平面内双足机器人坐标系示意图；

图3是摆动腿MPC控制实时调节落脚点原理图。

具体实施方式

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

如图1，一种双足机器人动态运动生成与控制方法，在单脚支撑期，通过建立全身动力学反馈线性化控制(Whole Body Dynamic Control:WBDC)器控制支撑腿的长度和上身俯仰的姿态保持双足机器人动态运动特性。由于支撑腿的长度固定不变，则整个机器人的物理性质与倒立摆模型近似。

建立全身动力学反馈线性化控制器的方法为：以平面7连杆双足机器人模型为例，该模型包括1个上身驱干、2个大腿、2个小腿和2个脚掌，如图2所示为x-z平面内双足机器人坐标系示意图，其中，θ表示相邻连杆间的相对角度，上角标r、l分别表示右腿(right)和左腿(left)，下角标hip、kne、ank分别表示髋关节(hip)、膝关节(knee)和踝关节(ankle)，而x,z表示一点在世界坐标系中的位置坐标点，下角标tor表示上身躯干(torso)，θ_tor表示上身与竖直方向的夹角即俯仰角度。而L_sup表示支撑腿髋关节到支撑点的相对距离称为腿长，与当前状态有关。利用拉格朗日方程法计算得到双足机器人全身动力学的浮动基动力学模型：

其中，

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

表示实数集其上角标的数字n表示向量或矩阵的维度，本实例中n＝9，m是驱动关节的数量，本实例中m＝6，

表示力矩筛选矩阵由0或1组成，即将驱动力矩作用到相应自由度上，

表示机器人广义坐标系的位置，x_tor是上身躯干世界坐标系的x轴坐标，z_tor是上身躯干世界坐标系的z轴坐标，θ_tor是上身与竖直方向的夹角即俯仰角度实际值，

是右腿髋关节夹角，

是右腿膝关节夹角，

是右腿踝关节夹角，

是左腿髋关节夹角，

是左腿膝关节夹角，

是左腿踝关节夹角，(·)^T运算表示求向量或矩阵的转置，

分别是q^s的一阶和二阶微分值表示机器人的速度和加速度。

表示驱动力矩向量。

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

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

设状态变量

为状态变量的一阶微分值，则可以将双足机器人动力学系统写成非线性控制系统的形式，即得到全身动力学反馈线性化控制器：

其中：f(x)为状态转移向量，g(x)为输入转移矩阵，f(x)、g(x)也被称为向量场，只依赖于q^s,

u为驱动力矩向量；具体表示为：

0_n×m表示一个n行m列的零矩阵，运算(·)^-1表示求矩阵的逆，B为力矩筛选矩阵。

由于双足机器人的结构对称性，以及运动的周期性，控制单脚支撑期中的动态运动即可通过周期对称将下文提及控制策略扩展至整个运动空间中。假设当前右腿为支撑腿，则设

为控制期望支撑腿的腿长

和期望上身姿态

为上述全身动力学反馈线性化控制器设计只与当前状态有关的输出函数y＝h(q^s)：

该输出函数不局限于上述形式，如需控制其他例如踝关节转动速度等都可以添加在其中。分别求取y沿着向量场f(x)、g(x)的Lie导数L_gL_fh(q^s),

分别用L_g、L_f表示L_gL_fh(q^s)和

则可以设计得到控制腿长和上身姿态的全身动力学控制量u_WBDC：

其中，

是输出函数y的一阶微分量；参数ε是满足1＞ε＞0的实数，需要在控制中适量选取。

基于全身动力学模型解耦摆动腿与全身的动力学耦合，通过建立摆动腿预测控制(Model Predictive Control:MPC)模型实时调节落脚点，即在未来观测规划的位置，落脚点调节给出了未来腿末端一系列目标点，根据环境信息规划摆动腿末端轨迹、实时调整动腿的落脚点。

在WBDC控制下，摆动腿被从全身动力学中解耦出来，假设摆动腿髋关节固连世界坐标系中，在本实例中可以通过拉格朗日方程建立三连杆固定基模型(如图1右图所示)描述摆动腿(即左腿为摆动腿)的运动趋势：

其中，

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

分别为摆动腿动力学的惯性矩阵、科氏力矩阵和重力向量，

是由摆动腿髋关节驱动力矩、膝关节驱动力矩和踝关节驱动力矩组成的向量。设定状态变量

从而得到摆动腿非线性控制系统：

其中，

是x^sw的一阶微分值，建立摆动腿非线性控制系统后利用线性化理论在参考点(使用上角标ref表示角度的参考)处得到线性状态方程：

其中，

分别是左腿髋关节夹角

左腿膝关节夹角

左腿踝关节夹角

的参考值，

分别是

的参考值，^refx^sw是状态变量x^sw的参考值，^refu^sw是驱动力矩向量的参考值，q^ref,

分别是机器人坐标位置和速度的参考值，A、B分别为摆动腿非线性控制系统f(x^sw,u^sw)对x^sw和u^sw的偏导数在参考值^refx^sw与^refu^sw处的向量矩阵，表示为：

通常设^refu^sw＝0_3×1即参考的输入全为零，设新的控制系统状态变量

且

则上述线性状态方程可以重新写为：

然后将上述线性方程进行离散化，采样周期为Δt，记k时刻的状态ξ_k、输入

和下一时刻的状态ξ_k+1，线性离散状态方程可表示为：

A_k,B_k对应第k时刻参考值

时的系数矩阵，

分别为ξ_k与

的系数矩阵，记

I_6×6表示6维的单位矩阵。设定预测步数

表示非零正整数集，因此可以得到在预测步数内的摆动腿MPC预测模型Ξ_k表示为：

其中，

分别为Ψ_k和Φ_k系数矩阵，

Ψ_k、Φ_k分别为MPC预测模型的状态向量和输入向量，表示为

摆动腿未来时刻的角度参考值可以通过落脚点调节算法和环境感知信息规划。以摆动腿髋关节为坐标原点，通过落脚点算法得到水平落脚点

而竖直方向的落脚点

其中，k_p和k_d均为正实数的增益系数，然后通过环境信息与当前摆动腿末端相对髋关节位置(如图3所示)，可以规划出未来NT_p个控制周期落脚点参考位置即脚踝规划轨迹，通过摆动腿逆运动学可求解出i＝{k,k+1,…k+NT_p-1}时刻摆动腿关节参考角度和角速度

用于MPC预测模型中未来参考值。

设定评价函数J^*使状态误差最小以及控制量即驱动力矩最小，则有：

对于优化问题这里只引入关节最大力矩τ_max的不等式约束：

-τ_max＜Φ_k＜τ_max

取经优化得到的多个控制量只取第一组即k阶段作为摆动腿控制力矩给双足机器人，即

最终如图1所示，整个双足机器人动态运动生成与控制方法的控制框图，控制量由WBDC控制器和摆动腿MPC控制器输出组成，即u＝u_MPC+u_WBDC。

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

Claims (5)

1.一种双足机器人动态运动生成与控制方法，其特征在于，在单脚支撑期，通过建立全身动力学反馈线性化控制器控制支撑腿的长度和上身俯仰的姿态保持双足机器人动态运动特性；基于全身动力学模型解耦摆动腿与全身的动力学耦合，通过建立摆动腿预测控制模型控制在未来观测规划的位置，落脚点调节给出了未来腿末端一系列目标点，根据环境信息规划摆动腿末端轨迹、实时调整动腿的落脚点；由支撑腿的全身动力学力矩控制量和摆动腿的控制力矩一同作为双足机器人运动控制输入；

建立全身动力学反馈线性化控制器的方法为：

S1.1，建立双足机器人全身动力学的浮动基动力学模型，表示为：

其中，M、C、G分别表示动力学的惯性矩阵、科氏力矩阵和重力向量，D为力矩筛选矩阵，

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

是q^s的二阶微分值表示机器人的加速度；

S1.2，由双足机器人在坐标系的位置q^s和双足机器人的速度

组成状态变量

S1.3，基于状态变量x，将双足机器人动力学系统写成非线性控制系统的形式：

其中：f(x)为状态转移向量，g(x)为输入转移矩阵，u为驱动力矩向量；

S1.4，为控制期望支撑腿的腿长

和期望上身姿态

设计非线性控制系统的输出为y＝h(q^s)，h(q^s)是一个只与当前系统状态有关的函数；

S1.5，基于S1.3和S1.4，设计得到控制腿长和上身姿态的全身动力学控制量u_WBDC：

其中，

是输出函数y的一阶微分量，参数ε是满足1＞ε＞0的实数，需要在控制中适量选取，L_g、L_f分别为y沿着f(x)、g(x)的Lie导数。

2.根据权利要求1所述的一种双足机器人动态运动生成与控制方法，其特征在于，S1.3中的f(x)和g(x)分别表示为：

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

分别表示右腿和左腿末端收到的外界力，J_r、J_l表示右腿和左腿末端在世界坐标系中的雅可比矩阵，0_n×m表示一个n行m列的零矩阵，D为力矩筛选矩阵。

3.根据权利要求2所述的一种双足机器人动态运动生成与控制方法，其特征在于，输出函数y＝h(q^s)表示为

其中，θ_tor 、 L_sup分别是当前上身俯仰角度实际值、支撑腿长度的实际值。

4.根据权利要求1-3中任意一项权利要求所述的一种双足机器人动态运动生成与控制方法，其特征在于，建立摆动腿预测控制模型的方法为：

S2.1，通过拉格朗日方程建立三连杆固定基模型描述摆动腿的运动趋势；

S2.2，由双足机器人摆动腿在坐标系的位置q和双足机器人摆动腿的速度

构造状态变量

S2.3，基于状态变量x^sw得到摆动腿非线性控制系统：

其中，用f(x^sw,u^sw)表示

关于x^sw和u^sw的函数关系式，

是x^sw的一阶微分值，M^sw、C^sw、G^sw分别为摆动腿动力学的惯性矩阵、科氏力矩阵和重力向量；u^sw是由摆动腿髋关节驱动力矩、膝关节驱动力矩和踝关节驱动力矩组成的向量；

S2.4，建立摆动腿非线性控制系统后利用线性化理论在参考点处得到线性状态方程：

其中，^refx^sw是状态变量x^sw的参考值，^refu^sw是u^sw驱动力矩向量的参考值，A、B分别为摆动腿非线性控制系统f(x^sw,u^sw)对x^sw和u^sw的偏导数在参考值^refx^sw与^refu^sw处的向量矩阵；

S2.5，设^refu^sw＝0_3×1，设新的控制系统状态变量为ξ＝(x^sw-^refx^sw)且

则线性状态方程表示为：

S2.6，对S2.5中的线性状态方程进行离散化，得到线性离散状态方程：

其中，A_k,B_k分别对应第k时刻参考值为

时ξ与u^sw的系数矩阵，ξ_k、ξ_k+1分别是第k时刻、第k+1时刻的线性化系统状态变量，

为第k时刻由摆动腿髋关节驱动力矩、膝关节驱动力矩和踝关节驱动力矩组成的向量，

分别为ξ_k与

的系数矩阵，I_6×6表示6维的单位矩阵，采样周期为Δt；S2.7，设定预测步数NT_p，得到在预测步数内的摆动腿MPC预测模型Ξ_k表示为：

其中，

分别为Ψ_k和Φ_k系数矩阵，Ψ_k、Φ_k分别为MPC预测模型中的由线性离散状态组成的状态向量和输入向量；

S2.8，设定评价函数J^*使状态误差最小以及控制量即驱动力矩最小，则有：

对于优化问题这里只引入关节最大力矩τ_max的不等式约束：-τ_max＜Φ_k＜τ_max；取经优化得到的多个控制量只取第一组作为摆动腿控制力矩给双足机器人，即

5.根据权利要求4所述的一种双足机器人动态运动生成与控制方法，其特征在于，通过落脚点算法得到水平落脚点

其中，x_tor是上身躯干世界坐标系的x轴坐标，

是x_tor的一阶导数，

是

的参考值；而竖直方向的落脚点

其中，k_p和k_d均为正实数的增益系数，然后通过环境信息与当前摆动腿末端相对髋关节位置，可以规划出未来NT_p个控制周期落脚点参考位置即脚踝规划轨迹，通过摆动腿逆运动学可求解出i＝{k,k+1,…k+NT_p-1}时刻摆动腿关节参考角度和角速度

用于MPC预测模型中未来参考值。

Images