CN104407611A - 一种仿人机器人稳定行走的控制方法 - Google Patents

Abstract

本发明涉及一种仿人机器人稳定行走的控制方法，包括步骤S1：获得机器人系统为补偿ZMP跟踪误差所需要产生的动量和角动量；步骤S2：基于微分运动学的方式，利用动量雅可比矩阵得到从机器人配置空间到动量和角动量的映射；步骤S3：通过对动量雅可比矩阵进行阻尼伪逆计算，得到步骤S2中映射的反映射，进而获取机器人配置空间命令，其对应行走中的动量补偿控制任务；步骤S4：采用动态优先级切换方式，将动量补偿控制任务分配，并产生各关节运动命令。与现有技术相比，本发明通过ZMP计算公式将解析动量控制应用到仿人机器人的平衡控制中，并将该控制问题整合到任务空间控制体系中，使得仿人机器人可以利用局部运动产生的补偿动量抑制未知扰动。

Description

一种仿人机器人稳定行走的控制方法

技术领域

本发明涉及机器人控制领域，尤其是涉及一种仿人机器人稳定行走的控制方法。 

背景技术

从对人类自身行走过程的观察可以发现，人类可以通过加速身体局部的平衡技巧来获得抑制扰动的能力，如摆动手臂、晃动游脚、旋转身体等。部分研究人员仿造人类加速局部进行扰动抑制的方法对仿人机器人进行运动补偿。在早期，有Taknishi和Michael等人通过附加可旋转质量块对ZMP跟踪误差进行补偿，在一定程度上抑制了外部扰动。不过，这类方法需要额外的质量块，不适合通用的仿人机器人系统。后来，更多研究人员采用机器人手臂的摆动进行平衡补偿，大多数仅仅控制手臂与行走保持一定相位差做周期运动，没有深入探讨摆臂与平衡的内在关系。从人类行为出发，Hugh等人重点对人类行走的角动量进行研究，发现人类在行走时更倾向于保持各方向角动量守恒，且旋转行走主要由游脚运动产生的角动量实现。本发明则从ZMP框架出发，通过动量加速度与平衡的关系，借助解析动量控制，介绍了一种利用动量进行平衡补偿的统一控制框架。 

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种仿人机器人稳定行走的控制方法，通过ZMP计算公式将解析动量控制应用到仿人机器人的平衡控制中，并将该控制问题整合到任务空间控制体系中，使得仿人机器人可以利用局部运动产生的补偿动量抑制未知扰动。 

本发明的目的可以通过以下技术方案来实现： 

一种仿人机器人稳定行走的控制方法，方法包括以下步骤： 

步骤S1：获得机器人系统为补偿ZMP跟踪误差所需要产生的动量和角动量； 

步骤S2：基于微分运动学的方式，利用动量雅可比矩阵得到从机器人配置空间到机器人动量和角动量的映射； 

步骤S3：通过对动量雅可比矩阵进行阻尼伪逆计算，得到步骤S2中映射的反映射，进而获取机器人配置空间命令，其对应机器人行走中的动量补偿控制任务； 

步骤S4：采用动态优先级切换方式，将动量补偿控制任务分配，并产生机器人的各关节运动命令，进而控制机器人行走。 

所述的步骤S1具体包括： 

101：基于坐标系构建机器人的ZMP计算式，给定测量ZMP和期望ZMP的水平位置分别为和令期望ZMP与测量ZMP的水平高度相同； 

102：为补偿ZMP跟踪误差，计算期望机器人系统的总动量和角动量为： 

$\left\{\begin{array}{c}{\widetilde{\stackrel{\·}{P}}}_x=\frac{(\mathrm{Mg}+{\stackrel{\·}{P}}_z)\left({\tilde{p}}_x{-\hat{p}}_x\right)}{p_z}+{\widehat{\stackrel{\·}{P}}}_x\\ {\widetilde{\stackrel{\·}{L}}}_x=\frac{(\mathrm{Mg}+{\stackrel{\·}{P}}_z)({\tilde{p}}_y-{\hat{p}}_y)}{p_z}+{\widehat{\stackrel{\·}{L}}}_x\end{array}\right.---\left(1\right)$

其中，为当前测量的总动量，为当前测量的角动量，g为重力加速度向量，M为机器人总质量，p_z为ZMP的高度位置，为Z轴的动量； 

103：令采样周期为Δt，给定机器人系统在k时刻的动量为角动量为 则系统下一时刻所需的动量和角动量分别为： 

$\left\{\begin{array}{c}{\tilde{P}}_x(k+1)={\widetilde{\stackrel{\·}{P}}}_x\left(k\right)\mathrm{\Δt}+{\hat{P}}_x\left(k\right)\\ {\tilde{L}}_x(k+1)={\widetilde{\stackrel{\·}{L}}}_x\left(k\right)\mathrm{\Δt}+{\hat{L}}_x\left(k\right)\end{array}\right.---\left(2\right).$

所述的步骤S2具体包括： 

201：采用递归方法计算机器人各连杆的局部惯性张量，令与第j个关节相邻的第j-1个关节所驱动部分的总质量为质心为惯性张量为则由第j个关节所驱动部分的总质量质心和惯性张量计算公式如下： 

${\tilde{m}}_j={\tilde{m}}_{j-1}+m_j---\left(3\right)$

${\tilde{c}}_j=\frac{({\tilde{m}}_{j-1}{\tilde{c}}_{j-1}+m_j{c}_j)}{{\tilde{m}}_{j-1}+m_j}---\left(4\right)$

${\tilde{I}}_j={\tilde{I}}_{j-1}+{\tilde{m}}_{j-1}D({\tilde{c}}_{j-1}-{\tilde{c}}_j)+R_j{I}_j{R}_j^T+m_{j}D(c_j-{\tilde{c}}_j)---\left(5\right)$

其中，R_j为第j个关节自身连杆旋转矩阵，c_j为其质心，m_j为其质量，I_j为其绕关节转轴惯性张量，且

202：计算第j个关节以角速度ω_j旋转时所产生的动量P_j、角动量L_j，公式如 下： 

$\left\{\begin{array}{c}{P}_j={\mathrm{\ω}}_j\×({\tilde{c}}_j-p_i){\tilde{m}}_j\\ L_j={\tilde{c}}_j\×P_j+{\tilde{I}}_j{\mathrm{\ω}}_j\end{array}\right.---\left(6\right)$

其中，p_j为第j个关节的位置； 

203：由步骤S1得到系统总动量以及总角动量由配置空间到动量向量的线性映射可表示为： 

$\left[\begin{array}{c}P\\ L\end{array}\right]=\left[\begin{array}{ccc}{M}_R& \tilde{m}E& -\tilde{m}{\hat{r}}_{b\→\stackrel{\‾}{c}}\\ H_R& 0& \tilde{I}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{q}}_R\\ {\mathrm{\υ}}_b\\ {\mathrm{\ω}}_b\end{array}\right]-J_M\left[\begin{array}{c}{\stackrel{\·}{q}}_R\\ {\mathrm{\υ}}_b\\ {\mathrm{\ω}}_b\end{array}\right]---\left(7\right)$

其中，为系统总质量，为总质心位置，E为3×3单位矩阵，为3×3系统绕质心的总惯性张量矩阵，为身体原点到质心的向量，为与另一向量叉乘的3×3反对称矩阵，为机器人关节角速度向量，υ_b为身体原点的线速度矢量，ω_b为身体原点的角速度矢量，J_M为动量雅可比矩阵，M_R和H_R为各个关节转动对系统总动量和角动量的贡献，且为3×n惯性矩阵，表示为： 

$\left\{\begin{array}{c}{M}_R=[M_1,M_2,...M_n]\\ H_R=[H_1,H_2{,...H}_n]\end{array}\right.---\left(8\right)$

其中，M_j和H_j为分别与第j个关节动量和角动量对应的3×1向量，动量雅可比矩阵中与第j个关节对应的列向量满足下式： 

$\left[\begin{array}{c}{M}_j\\ H_j\end{array}\right]=\left[\begin{array}{cc}\∂P_j& /\∂{\stackrel{\·}{q}}_j\\ \∂L_j& /\∂{\stackrel{\·}{q}}_j\end{array}\right]---\left(9\right)$

为第j个关节角速度向量，结合式(6)，则： 

$\left\{\begin{array}{c}{M}_j={\mathrm{\α}}_j\×({\tilde{c}}_j-p_i){\tilde{m}}_j\\ H_j={\tilde{c}}_j\×M_j+{\tilde{I}}_j{\mathrm{\α}}_j\end{array}\right.---\left(10\right)$

其中，α_j为第j个关节转轴矢量。 

所述的步骤S3具体包括： 

301：基于步骤S1中所需要产生的动量和角动量计算机器人配置空间控制命令公式如下： 

$\stackrel{\·}{q}=J_M^{\#\mathrm{\λ}}\left[\begin{array}{c}{\tilde{P}}_x\\ {\tilde{L}}_x\end{array}\right]---\left(11\right)$

其对应机器人行走中的动量补偿控制的总任务其中，为阻尼最小二乘伪逆； 

302：将总任务Γ_M分为两个子任务Γ_MSwg和Γ_MHand，其中，Γ_MSwg为机器人游腿各关节产生的动量补偿，相应的动量雅可比矩阵记为J_MSwg，J_MSwg为由J_M中与游腿各关节相应的列向量与全零向量组成，即与其他关节对应的列向量为零，Γ_MHand为 机器人双臂各关节产生的动量补偿，相应的动量雅可比矩阵记为J_MHand，由J_M中与双臂关节相应的列向量与全零向量组成。 

所述的步骤S4具体包括： 

401：根据当前机器人运动控制要求动态确定步骤S3中动量补偿控制子任务的优先级，并将子任务按优先级排序分层； 

402：底层任务首先根据微分运动控制和相应的动量雅可比矩阵，将任务控制输出映射到上一层任务控制的零空间中，再将映射后的控制输出合并到上一层任务控制输出中，即进行逐层迭代； 

403：产生机器人的各关节运动命令，进而控制机器人行走。 

所述的步骤402的逐层迭代中进行附加约束，附加约束包括机器人关节限幅、关节运动速度和碰撞限制约束。 

与现有技术相比，本发明具有以下优点： 

1)本发明从ZMP框架出发，通过动量加速度与平衡的关系，借助解析动量控制，使得仿人机器人可以利用局部运动产生的补偿动量抑制未知扰动，帮助仿人机器人抵御未知外部扰动； 

2)本发明将解析动量控制与浮体运动学和优先级任务控制相结合，无需对支撑脚模式进行特殊的动量矩阵处理，方法合理，结构清晰； 

3)本发明为解决各任务的优先级冲突，引入了动态优先级任务空间控制方法并在该框架下将基于任务空间控制的解析动量补偿控制融入到仿人机器人行走控制中，实现了仿人机器人鲁棒的行走。 

附图说明

图1为本发明方法流程图； 

图2为机器人多连杆结构示意图； 

图3为机器人各连杆近似动力学表示图； 

图4为本发明中动态优先级机器人任务空间控制框架图； 

图5为本发明实施例中冲击实验时机器人ZMP、质心轨迹跟踪图； 

图6为本发明实施例中冲击实验时机器人RHipRoll关节运动速度图。 

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。 

如图1所示，一种仿人机器人稳定行走的控制方法，方法包括以下步骤： 

步骤S1：获得机器人系统为补偿ZMP跟踪误差所需要产生的动量和角动量。 

步骤S1具体包括： 

101：已知对于仿人机器人，当考虑各连杆转动时的惯性张量时，基于坐标系构建机器人的ZMP计算式，机器人的ZMP可用下式计算： 

$p_x=\frac{{\mathrm{Mgc}}_x+p_z{\stackrel{\·}{P}}_x-{\stackrel{\·}{L}}_y}{\mathrm{Mg}+{\stackrel{\·}{P}}_z}$

$p_y=\frac{{\mathrm{Mgc}}_y+p_z{\stackrel{\·}{P}}_y-{\stackrel{\·}{L}}_y}{\mathrm{Mg}+{\stackrel{\·}{P}}_z}$

其中，c为质心，g为重力加速度向量，为角动量加速度，M为机器人总质量，ZMP位于p点，为动量加速度，x，y，z下标为相应参数在x，y，z轴上的分量； 

给定测量ZMP和期望ZMP的水平位置分别为和令期望ZMP与测量ZMP的水平高度相同； 

102：为补偿ZMP跟踪误差，计算期望机器人系统的总动量和角动量为： 

$\left\{\begin{array}{c}{\widetilde{\stackrel{\·}{P}}}_x=\frac{(\mathrm{Mg}+{\stackrel{\·}{P}}_z)\left({\tilde{p}}_x{-\hat{p}}_x\right)}{p_z}+{\widehat{\stackrel{\·}{P}}}_x\\ {\widetilde{\stackrel{\·}{L}}}_x=\frac{(\mathrm{Mg}+{\stackrel{\·}{P}}_z)({\tilde{p}}_y-{\hat{p}}_y)}{p_z}+{\widehat{\stackrel{\·}{L}}}_x\end{array}\right.---\left(1\right)$

其中，为当前测量的总动量，为当前测量的角动量，p_z为ZMP的高度位置， 为Z轴的动量； 

103：令采样周期为Δt，给定机器人系统在k时刻的动量为角动量为 则系统下一时刻所需的动量和角动量分别为： 

$\left\{\begin{array}{c}{\tilde{P}}_x(k+1)={\widetilde{\stackrel{\·}{P}}}_x\left(k\right)\mathrm{\Δt}+{\hat{P}}_x\left(k\right)\\ {\tilde{L}}_x(k+1)={\widetilde{\stackrel{\·}{L}}}_x\left(k\right)\mathrm{\Δt}+{\hat{L}}_x\left(k\right)\end{array}\right.---\left(2\right).$

步骤S2：在获得机器人系统为补偿ZMP跟踪误差所需要产生的动量和角动量后，基于微分运动学的方式，利用动量雅可比矩阵得到从机器人配置空间到机器人动量和角动量的映射。本发明利用解析动量控制和实时计算的动量补偿量对仿人机器人的平衡性进行补偿。 

步骤S2具体包括： 

201：为提高计算效率，采用递归方法计算机器人各连杆的局部惯性张量，令与第j个关节相邻的第j-1个关节所驱动部分的总质量为质心为惯性张量为则由第j个关节所驱动部分的总质量质心以和惯性张量计算公式如下： 

${\tilde{m}}_j={\tilde{m}}_{j-1}+m_j---\left(3\right)$

${\tilde{c}}_j=\frac{({\tilde{m}}_{j-1}{\tilde{c}}_{j-1}+m_j{c}_j)}{{\tilde{m}}_{j-1}+m_j}---\left(4\right)$

${\tilde{I}}_j={\tilde{I}}_{j-1}+{\tilde{m}}_{j-1}D({\tilde{c}}_{j-1}-{\tilde{c}}_j)+R_j{I}_j{R}_j^T+m_{j}D(c_j-{\tilde{c}}_j)---\left(5\right)$

其中，R_j为第j个关节自身连杆旋转矩阵，c_j为其质心，m_j为其质量，I_j为其绕关节转轴惯性张量，且

202：如图2所示，对于一个连杆系统，计算第j个关节以角速度ω_j旋转时所产生的动量、角动量分量，公式如下： 

$\left\{\begin{array}{c}{P}_j={\mathrm{\ω}}_j\×({\tilde{c}}_j-p_i){\tilde{m}}_j\\ L_j={\tilde{c}}_j\×P_j+{\tilde{I}}_j{\mathrm{\ω}}_j\end{array}\right.---\left(6\right)$

其中，p_j为第j个关节的位置； 

203：浮体机器人系统的配置空间可以由关节空间和机器人身体位姿联合表示。由步骤S1得到系统总动量以及总角动量由配置空间到动量向量的线性映射可表示为： 

$\left[\begin{array}{c}P\\ L\end{array}\right]=\left[\begin{array}{ccc}{M}_R& \tilde{m}E& -\tilde{m}{\hat{r}}_{b\→\stackrel{\‾}{c}}\\ H_R& 0& \tilde{I}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{q}}_R\\ {\mathrm{\υ}}_b\\ {\mathrm{\ω}}_b\end{array}\right]-J_M\left[\begin{array}{c}{\stackrel{\·}{q}}_R\\ {\mathrm{\υ}}_b\\ {\mathrm{\ω}}_b\end{array}\right]---\left(7\right)$

其中，为系统总质量，为总质心位置，E为3×3单位矩阵，为3×3系统绕质心的总惯性张量矩阵，为身体原点到质心的向量，为与另一向量叉乘的3×3反对称矩阵，机器人关节角速度向量，υ_b为身体原点的线速度矢量，ω_b为身体原点的角速度矢量，J_M为动量雅可比矩阵，M_R和H_R为各个关节转动对系统总动量和角动量的贡献，且为3×n惯性矩阵，表示为： 

$\left\{\begin{array}{c}{M}_R=[M_1,M_2,...M_n]\\ H_R=[H_1,H_2{,...H}_n]\end{array}\right.---\left(8\right)$

其中，M_j和H_j为分别与第j个关节动量和角动量对应的3×1向量，动量雅可比矩阵中与第j个关节对应的列向量满足下式： 

$\left[\begin{array}{c}{M}_j\\ H_j\end{array}\right]=\left[\begin{array}{cc}\∂P_j& /\∂{\stackrel{\·}{q}}_j\\ \∂L_j& /\∂{\stackrel{\·}{q}}_j\end{array}\right]---\left(9\right)$

为第j个关节角速度向量，结合式(6)，则： 

$\left\{\begin{array}{c}{M}_j={\mathrm{\α}}_j\×({\tilde{c}}_j-p_i){\tilde{m}}_j\\ H_j={\tilde{c}}_j\×M_j+{\tilde{I}}_j{\mathrm{\α}}_j\end{array}\right.---\left(10\right)$

其中，α_j为第j个关节转轴矢量。 

为计算身体各个连杆的局部惯性张量，本发明将机器人各个部分用惯性张量计算公式已知的几何体近似表示。如图3所示，这里将机器人头部用质量均匀的球体表示，而将其他各部分用质量均匀的长方体表示。 

步骤S3：基于浮体机器人系统的解析动量控制方法可以得到从机器人配置空间到机器人动量和角动量的映射。通过对动量雅可比矩阵进行阻尼伪逆计算，得到步骤S2中映射的反映射，进而获取机器人配置空间命令，配置空间命令实现机器人的运动以完成动量补偿控制任务，与机器人行走中的动量补偿控制任务对应，从而实现机器人系统对给定动量和角动量的跟踪控制。步骤S3具体包括： 

301：基于步骤S1中机器人自身传感器测量的ZMP位置和式(2)获得的机器人期望动量和角动量计算机器人配置空间控制命令公式如下： 

$\stackrel{\·}{q}=J_M^{\#\mathrm{\λ}}\left[\begin{array}{c}{\tilde{P}}_x\\ {\tilde{L}}_x\end{array}\right]---\left(11\right)$

其对应机器人行走中的动量补偿控制的总任务其中，为阻尼最小二乘伪逆； 

当仿人机器人在正常的行走过程中遇到未知扰动时，可以通过自身的传感器检测到当前机器人ZMP位置与规划ZMP位置间的差异，通过考虑机器人多连杆结构的ZMP理论计算公式得到总体动量和角动量的变化，以补偿ZMP误差。同时，根据机器人全身各个关节的位置传感器、身体惯性单元传感器以及解析动量计算可以得到机器人的J_M。再通过式(11)便可以得到为补偿ZMP误差所需要的机器人各个关节的运动命令。由此，便可以实现通过动量和角动量来实现仿人机器人行走的扰动抑制方法。 

当然，仿人机器人在行走过程中不可能使用所有自由度完成平衡技巧，机器人仍需要一定的自由度完成诸如保持与地面接触、质心的移动等更为重要的任务。因此，若要实现上述动量补偿控制方法在仿人机器人行走控制中的应用，还需要考虑多任务并行执行的问题。 

302：将总任务Γ_M分为两个子任务Γ_MSwg和Γ_MHand，其中，Γ_MSwg为机器人游腿各关节产生的动量补偿，相应的动量雅可比矩阵记为J_MSwg，J_MSwg为由J_M中与游腿各关节相应的列向量与全零向量组成，即与其他关节对应的列向量为零；Γ_MHand为 机器人双臂各关节产生的动量补偿，相应的动量雅可比矩阵记为J_MHand，由J_M中与双臂关节相应的列向量与全零向量组成。 

多任务并行执行的冲突解决问题可以通过前述的任务空间控制方法有效的解决。通过对上述动量补偿控制方法的观察不难看出，该方法同样采用基于浮体机器人系统的微分运动控制方法的框架，因此可以直接将补偿控制作为任务空间控制中的一个子任务，整合到仿人机器人的行走控制中。 

步骤S4：采用动态优先级切换方式，将动量补偿控制任务分配，并产生机器人的各关节运动命令，进而控制机器人行走。将解析动量控制与浮体运动学和优先级任务控制相结合，无需对支撑脚模式进行特殊的动量矩阵处理。 

在基于任务空间法的机器人运动控制中，各子任务间的优先级往往是固定不变的。然而，当机器人运动控制中的子任务数量较多、各子任务相互耦合较为复杂时，这种简单的静态优先级策略将不再适用于各控制任务间的相互协作。例如，在仿人机器人行走控制中，机器人的游脚一方面需要完成预定行走轨迹的跟踪任务，另一方面又可以用来产生动量补偿运动提高仿人机器人行走的平衡。然而，较高的游脚轨迹跟踪任务优先级要求并非一直存在的。当机器人处于双足支撑阶段时，游脚固然要优先满足相对支撑脚无运动并紧密接触地面的约束要求。在机器人单足支撑阶段，对机器人游脚的控制却仅关心其在抬起和落下时的位姿跟踪而对游脚在空中阶段的运动并不关心，此时游脚便可以被用来完成动量补偿这一更为重要的任务。 

因此，在机器人运动控制中更为合理的策略是：各控制子任务能够在机器人运动过程中根据当前的控制需要动态的切换优先级，实现更为灵活的多任务协作控制。采用动态优先级切换的机器人运动任务空间控制方法框架可由图4所示。 

图4中步骤S4具体包括： 

401：根据当前机器人运动控制要求动态确定步骤S3中动量补偿控制子任务的优先级，并将子任务Γ_D1...Γ_DN按优先级排序分层Level 1...Level N； 

402：底层任务首先根据微分运动控制和相应的动量雅可比矩阵，将任务控制输出Γ_task映射到上一层任务控制的零空间中，再将映射后的控制输出合并到上一层任务控制输出Γ_task中，即进行逐层迭代，逐层迭代中需进行附加约束，附加约束包括机器人关节限幅、关节运动速度和碰撞限制等约束： 

403：产生机器人的各关节运动命令，进而控制机器人行走。 

将本发明方法应用于Robocup标准平台组比赛专用机器人NAO。针对设计的 补偿器设计进行了典型的仿真和实体机器人实验，从实验验证方面说明本发明方法在仿人机器人行走控制中的作用。 

在实验过程中，机器人沿前进方向以4cm/s的速度行进并随机遭受重物的扰动冲击。机器人需要在实验中实时计算当前ZMP跟踪误差，再根据动量补偿控制任务计算游脚各关节所需运动量。根据当前游脚的任务等级计算零空间矩阵，再将补偿所需的游脚关节控制角度映射到上一层。当游脚位于地面时，其动量补偿任务等级将低于行走轨迹跟踪任务。此时，动量补偿任务在零空间映射后将基本为零，机器人游脚主要跟踪行走轨迹，无法进行平衡补偿。当游脚位于空中时，游脚动量补偿任务优先级高于行走轨迹跟踪任务。此时，游脚将偏离预定的行走轨迹，而偏向通过加速运动补偿ZMP的跟踪误差。然而，游脚各任务的优先级并不是在游脚离开地面的瞬间相互切换，而是在游脚到达一定高度后才发生切换。因为当机器人脚底与地面距离较近时，游脚的加速摆动可能造成脚底与地面发生摩擦，给机器人带来不期望的作用力。从一次冲击后机器人的运动过程可以观察到，在机器人遭受撞击后，身体发生向左倾斜并将要摔倒。此时，在本发明所述的运动控制方法作用下，机器人游脚(例如右脚)向外突然加速产生反向角动量以抵消撞击对机器人角动量的突然增加。机器人的平衡恢复并非在一步内完成，由于撞击产生的晃动使机器人右脚落地时间较期望时间延后，右脚的延后落脚又造成机器人的不稳定。为此，机器人在第二步左脚抬起时又加速向外侧运动。在实验中，仿人机器人在受到未知外界扰动时表现出了类似人类的摆脚行为，即通过摆动游脚产生补偿动量来平衡机器人，这在一定程度上解释了人类通过摆脚补偿扰动的平衡技巧的动力学机理。 

实验中沿机器人y轴方向的ZMP、质心轨迹数据如图5所示，图5中：纵坐标Position Y为ZMP在y轴方向上的分量，横坐标Time为时间，Impulse表示干扰发生时刻，Ref ZMP为参考ZMP，mVZMP为测量的VZMP，mCoM为测量的质心，RefCoM为参考质心。同时，为补偿机器人平衡机器人右腿髋关节绕x轴方向关节RHipRoll的速度数据如图6所示，图6中：纵坐标Joint Velocity为关节角速度，横坐标Time为时间，RefZMP为参考ZMP，vRHipRoll为RHipRoll关节角速度。冲击发生在46.1s左右，此时仿人机器人的实测ZMP轨迹由于扰动作用偏离了期望轨迹。在动态优先级切换和解析动量补偿控制的作用下，机器人右脚降低了游脚行走轨迹跟踪任务的优先级，并通过加速摆动提供动量补偿，从而修正了ZMP轨迹的跟踪误差，实现了仿人机器人的鲁棒行走。 

Claims (6)

1.一种仿人机器人稳定行走的控制方法，其特征在于，方法包括以下步骤： 

步骡S1：获得机器人系统为补偿ZMP跟踪误差所需要产生的动量和角动量； 

步骤S2：基于微分运动学的方式，利用动量雅可比矩阵得到从机器人配置空间到机器人动量和角动量的映射； 

步骤S3：通过对动量雅可比矩阵进行阻尼伪逆计算，得到步骤S2中映射的反映射，进而获取机器人配置空间命令，其对应机器人行走中的动量补偿控制任务； 

步骤S4：采用动态优先级切换方式，将动量补偿控制任务分配，并产生机器人的各关节运动命令，进而控制机器人行走。 

2.根据权利要求1所述的一种仿人机器人稳定行走的控制方法，其特征在于，所述的步骤S1具体包括： 

101：基于坐标系构建机器人的ZMP计算式，给定测量ZMP和期望ZMP的水平位置分别为和令期望ZMP与测量ZMP的水平高度相同； 

102：为补偿ZMP跟踪误差，计算期望机器人系统的总动量和角动量为： 

其中，为当前测量的总动量，为当前测量的角动量，g为重力加速度向量，M为机器人总质量，p_z为ZMP的高度位置，为Z轴的动量； 

103：令采样周期为Δt，给定机器人系统在k时刻的动量为角动量为 则系统下一时刻所需的动量和角动量分别为： 

3.根据权利要求1所述的一种仿人机器人稳定行走的控制方法，其特征在于，所述的步骤S2具体包括： 

201：采用递归方法计算机器人各连杆的局部惯性张量，令与第j个关节相邻的第j-1个关节所驱动部分的总质量为质心为惯性张量为则由第j个关节所驱动部分的总质量质心和惯性张量计算公式如下： 

其中，R_j为第j个关节自身连杆旋转矩阵，c_j为其质心，m_j为其质量，I_j为其绕关节转轴惯性张量，且

202：计算第j个关节以角速度ω_j旋转时所产生的动量P_j、角动量L_j，公式如下： 

其中，p_j为第j个关节的位置； 

203：由步骤S1得到系统总动量以及总角动量，由配置空间到动量向量的线性映射可表示为： 

其中，为系统总质量，为总质心位置，E为3×3单位矩阵，为3×3系统绕质心的总惯性张量矩阵，为身体原点到质心的向量，为与另一向量叉乘的3×3反对称矩阵，为机器人关节角速度向量，υ_b为身体原点的线速度矢量，ω_b为身体原点的角速度矢量，J_M为动量雅可比矩阵，M_R和H_R为各个关节转动对系统总动量和角动量的贡献，且为3×n惯性矩阵，表示为： 

其中，M_j和H_j为分别与第j个关节动量和角动量对应的3×1向量，动量雅可比矩阵中与第j个关节对应的列向量满足下式： 

为第j个关节角速度向量，结合式(6)，则： 

其中，a_j为第j个关节转轴矢量。 

4.根据权利要求1所述的一种仿人机器人稳定行走的控制方法，其特征在于，所述的步骤S3具体包括： 

301：基于步骤S1中所需要产生的动量和角动量计算机器人配置空间 控制命令，公式如下： 

其对应机器人行走中的动量补偿控制的总任务其中，为阻尼最小二乘伪逆； 

302：将总任务Γ_M分为两个子任务Γ_MSwg和Γ_MHand，其中，Γ_MSwg为机器人游腿各关节产生的动量补偿，相应的动量雅可比矩阵记为J_MSwg，J_MSwg为由J_M中与游腿各关节相应的列向量与全零向量组成，即与其他关节对应的列向量为零，Γ_MHand为机器人双臂各关节产生的动量补偿，相应的动量雅可比矩阵记为J_MHand，由J_M中与双臂关节相应的列向量与全零向量组成。 

5.根据权利要求1所述的一种仿人机器人稳定行走的控制方法，其特征在于，所述的步骤S4具体包括： 

401：根据当前机器人运动控制要求动态确定步骤S3中动量补偿控制子任务的优先级，并将子任务按优先级排序分层； 

402：底层任务首先根据微分运动控制和相应的动量雅可比矩阵，将任务控制输出映射到上一层任务控制的零空间中，再将映射后的控制输出合并到上一层任务控制输出中，即进行逐层迭代； 

403：产生机器人的各关节运动命令，进而控制机器人行走。 

6.根据权利要求5所述的一种仿人机器人稳定行走的控制方法，其特征在于，所述的步骤402的逐层迭代中进行附加约束，附加约束包括机器人关节限幅、关节运动速度和碰撞限制约束。