CN103019096B - 一种基于加速度优化的仿人机器人逆动力学控制器 - Google Patents

Abstract

本发明公开了一种基于加速度优化的仿人机器人逆动力学控制器。属于机器人技术领域。所述控制器包括如下步骤：根据仿人机器人的运动约束，得到仿人机器人上身加速度与脚底所需外力的关系；根据外力的约束来计算上身加速度的范围；通过代价函数计算出最优的上身加速度，并计算出机器人所应受外力和关节力矩。该方法给定机器人脚部受力约束,通过优化机器人上身加速度,使机器人实际受到的外力满足该约束条件，避免机器人发生由外界干扰产生不可预测的运动，从而达到稳定控制仿人机器人的目的。

Description

一种基于加速度优化的仿人机器人逆动力学控制器

技术领域

本发明涉及一种基于加速度优化的逆动力学控制器来控制仿人机器人。属于机器人技术领域。

背景技术

自从机器人诞生以来，人类的生活、工作方式发生了巨大的变化，机器人在各种场合辅助人类完成复杂的、重复的工作。仿人机器人具有人的外形特征，可以更好地适应人类的生活环境，更好地为人们服务。近年来，国内外掀起了研究仿人机器人的热潮，其研究内容主要包括稳定行走、灵巧作业、人机交互等方面。

仿人机器人在运动过程中，支撑脚与地面之间是单向的、未驱动的、不可控的自由度。如果不能有效控制，很容易发生机器人绕其支撑脚边缘倾倒的情况。1972年南斯拉夫学者Vukobratovic博士提出的ZMP(Zero-MomentPoint)控制方法被作为仿人机器人的动态稳定控制的基本准则。所谓ZMP，是地面作用力的力矩为零的那个点；所谓支撑多边形，即机器人足底与地面之间的所有接触点的最小多边形区域。要使仿人机器人稳定行走，其ZMP必须始终位于支撑多边形中。

长期以来，仿人机器人的研究者经常使用位置控制模式来实现它的稳定控制。为了实现仿人机器人更好的与人交互，并能使仿人机器人克服各种未知干扰，在最近几年里，基于力控制的仿人机器人已经出现。力控制方法有些是通过叠加逆动力学前馈力矩、PD控制反馈力矩、姿态控制、平衡控制等得到的合力矩来跟踪参考轨迹，但是通过这个力矩计算出的身体加速度和对应的外力不等效于预期的加速度和外力。另一些是通过逆动力学计算出各个关节力矩，并通过优化关节力矩计算出对应的身体加速度和对应的外力来实现对仿人机器人的控制，通过此方法计算出的外力不一定能满足地面受力约束，从而不能保证机器人的稳定。

现有中国专利申请200910190906提出了一种基于逆动力学模型的自适应PID控制器的设计方法，该方法通过选择恰当的控制对象逆动力学模型输入向量，实现了PID控制和自适应逆控制的有机结合，通过控制对象逆动力学模型的在线辨识，获得与控制对象相匹配PID控制特征参数，形成与控制对象特性相适应的自适应PID控制器。其控制方法能实现传统不受外界约束的简单模型的控制，但是对于受地面约束的仿人机器人，该方法不能实现其稳定控制。

现有论文“Inverse Dynamics Control with Floating Base and Constraints”中，Nakanishi et al.提出了一种使用逆动力学控制器来控制仿人机器人稳定行走的方法。该方法是通过优化关节力矩来控制机器人的稳定行走。其计算出来的关节力矩对应的外力不一定能满足ZMP等地面约束，从而不能保证机器人的稳定行走。

发明内容

针对现有技术中的上述技术问题，为了使仿人机器人能更好的适应环境，克服未知干扰的影响，本发明提出了一种基于加速度优化的逆动力学控制器来控制仿人机器人。

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

一种仿人机器人逆动力学控制方法，所述仿人机器人具有上身、左腿和右腿，所述方法包括以下步骤：

对所述仿人机器人建立动力学模型；所述动力学模型中，上身简化为一个集成的质量块，每条腿有六个关节，q_rl∈R^6×1与q_ll∈R^6×1分别代表右腿与左腿的关节角度，上身浮动坐标系∑_R位于盆骨的中心，世界坐标系Σ_W位于地面；

计算所述仿人机器人上身加速度范围；所述加速度范围是使用“空间矢量”方法推导出所述仿人机器人脚底所受外力与上身加速度关系,并根据所述仿人机器人当前状态由脚底所受外力范围计算的；

优化所述仿人机器人的上身加速度；根据所述仿人机器人当前状态的加速度范围和PD控制产生的目标加速度，通过代价函数计算出该状态的仿人机器人最优上身加速度，并根据最优的上身加速度，计算出所述仿人机器人所应受外力和关节力矩，从而驱动所述仿人机器人行走。

以上技术方案给定机器人脚部受力约束,通过优化机器人上身加速度,使机器人实际受到的外力满足该约束条件，避免机器人发生由外界干扰产生不可预测的运动，从而达到稳定控制仿人机器人的目的。

附图说明

图1是本发明所涉及的仿人机器人模型图。

图2是本发明所涉及的“支撑多边形”概念的示意图。

图3是本发明所涉及的控制仿人机器人的方法流程示意图。

具体实施方式

仿人机器人的动力学建模就是如何用数学语言描述机器人。采用浮动基座的方法建立仿人机器人模型。图1示出的是本发明所涉及的仿人机器人的模型图，所述机器人具有上身和两条能够行走的腿，即左腿和右腿。

世界坐标系∑_W与地面固连，x轴指向机器人的正前方，y轴指向机器人的左方，z轴垂直向上。由于本文主要考虑机器人下肢的运动，因此建立动力学模型时，上身简化为一个集成的质量块。上身为浮动坐标系，浮动坐标系∑_R与浮动基座固连，原点在双腿连线的中点，三轴的初始方位与世界坐标系相同。为了描述机器人在世界坐标系中的位置，在世界坐标系与上身浮动坐标系之间引入6个虚拟自由度，但是这些虚拟自由度不引入任何几何约束，不给机器人任何作用力，因此不影响机器人的运动。q_r∈R^6×1表示这6个虚拟自由度，前三个量为浮动基座的位置，后三个为它的姿态。每条腿有6个自由度，q_rl∈R^6×1与q_ll∈R^6×1分别代表右腿与左腿的关节角度(图1)。

①由于机器人脚底与地面必须接触，脚面相对于地面的速度、加速度都为零，该模型的动力学公式为：

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})=\mathrm{\τ}+{\mathrm{Jrf}_T^W}^W{\hat{f}}_{\mathrm{rf}}+{J_{\mathrm{lf}}^T{}_W}^W{\hat{f}}_{\mathrm{lf}}---\left(1\right)$

在式子(1)中，q＝[q_r,q_rl,q_ll]，M(q)∈R^18×18是关节空间的惯性矩阵，是科氏力、离心力与重力的合力的矢量，τ＝[0_6×1,τ_rl,τ_ll],τ_rl∈R^6×1与τ_ll∈R^6×1右腿与左腿的关节力矩输入。是从右脚坐标系转换到世界坐标系Σ_W的雅克比矩阵，是从左脚坐标系转换到Σ_W的雅可比矩阵，是施加在右腿的外力在∑_W中的表示，是施加在左腿的外力在Σ_W中的表示。

②根据以上模型，该部分推导出机器人上身加速度与脚底所需外力的关系。机器人在空间矢量的加速度与传统加速度关系如下：

$\hat{a}_r^W=X_W^{-1}{}_R(a^c-\left[\begin{array}{c}{0}_{3\×1}\\ \mathrm{\ω}_r^R\×V_r^R\end{array}\right])---\left(2\right)$

其中是机器人上身所受的传统加速度，是机器人的空间矢量加速度， $\hat{V}_r^R=[\mathrm{\ω}_r^R;V_r^R]={[{\mathrm{\ω}}_x,{\mathrm{\ω}}_y,{\mathrm{\ω}}_z,V_x,V_y,V_z]}^T$ 是上身的空间矢量速度，^RX_W是把速度、加速度和力从Σ_W转换到∑_R的转换矩阵。

在双脚支撑期，左右腿的关节加速度与都是由决定；在单脚支撑期，支撑腿的关节加速度是由决定，而摆动腿的关节加速度是由摆动腿的空间矢量加速度或决定。因此

$\stackrel{\·\·}{q}=Q(q,\stackrel{\·}{q},\hat{a}_r^W,\hat{a}_{\mathrm{rf}}^R\mathrm{or}\hat{a}_{\mathrm{lf}}^R)---\left(3\right)$

到此为止，可以根据机器人所受加速度用公式(2)和(3)计算关节角加速度。为了计算机器人所受外力，使机器人能达到所需要的加速度，首先使模型作为一个没有外力的固定的模型。在这种情况下，机器人关节力矩通过公式(4)计算。

$\left[\begin{array}{c}{\mathrm{\τ}}_r\\ {\mathrm{\τ}}_{\mathrm{rl}}\\ {\mathrm{\τ}}_{\mathrm{ll}}\end{array}\right]=M\stackrel{\·\·}{q}+C---\left(4\right)$

此时的τ_r是该虚拟自由度的关节力矩，这些虚拟的力矩是来自施加在仿人机器人的虚拟外力产生的，这个虚拟外力与实际期望的外力相等。因此，我们所需要的机器人的外力可由公式(5)计算，公式(5)中为雅可比矩阵转制的逆。在双脚支撑期，所需的外力是由双脚共同提供；在单脚支撑期，所需的外力是由支撑脚提供。

$\hat{f}_{\mathrm{ext}}^W=J_R^{-T}{}_w{\mathrm{\τ}}_r---\left(5\right)$

然后，根据公式(1)-(5)，机器人上身传统加速度a^c与所受外力的关系可以表示为式子(6)。

${\mathrm{Ua}}^c+V=\hat{f}_m^M---\left(6\right)$

其中，U、V为q和的函数，是含有六个元素的空间矢量， nMx,nMy,nMz,fx,fy,fz为空间六维力/力矩。

仿人机器人在实际行走过程中要受到三种地面约束。首先，为了保证机器人的稳定，机器人必须满足ZMP约束，即机器人行走过程中ZMP必须始终位于支撑多边形(见图2)内。其次，机器人在行走过程中，支撑脚脚底受到地面摩擦力，摩擦力跟摩擦力系数和支撑力有关。为了保证机器人行走过程中支撑脚受到的摩擦力为静摩擦力，其摩擦系数必须小于最大静摩擦系数，否则机器人会与地面打滑甚至摔倒。再次，由于支撑脚与地面接触力为单向力，所以机器人所受的竖直方向力方向必须向上。

根据以上三个地面约束，得出的范围，再根据机器人当前状态得出传统加速度a^c的范围。

③当给定机器人一条参考轨迹后，为了使机器人能正确跟踪目标轨迹，目标加速度a^des根据PD控制产生。

$a^{\mathrm{des}}=K_p{R}_W(p_r^{\mathrm{ref}}{}_W-p_r^W)+K_d(R_W^{R}v_r^{\mathrm{ref}}{}_W-\hat{v}_r^R)---\left(7\right)$

在公式(7)中，K_p与K_d是PD增益矩阵，^RR_W＝[I_3×30_3×3；0_3×3R^-1]，R是上身浮动坐标系∑_R在世界坐标系∑_W中的姿态矩阵，与是∑_R的目标位置和速度。

然后根据代价函数式子(8)，在传统加速度ac范围内求出最优的上身传统加速度a^c。

$f\left(a^c\right)=W_t\·\left|\right|a^c-a_{\mathrm{des}}^c\left|\right|---\left(8\right)$

在式子(8)中，是根据PD控制产生目标加速度，W_t是一个权重矩阵。当f(a^c)最小时的a^c即为最优的a^c。

因此，根据该动力学模型，实现通过优化得到的机器人上身传统加速度a^c(公式(8))，计算出机器人所受外力(公式(6))。并根据逆动力学(公式(9))计算出机器人关节力矩，此力矩驱使机器人跟踪参考轨迹。

$\left[\begin{array}{c}{0}_{6\×1}\\ {\mathrm{\τ}}_{\mathrm{rl}}\\ {\mathrm{\τ}}_{\mathrm{ll}}\end{array}\right]=M\stackrel{\·\·}{q}+C-J_{\mathrm{rf}}^T{}_W{K}_f\hat{f}_{\mathrm{ext}}^W-J_{\mathrm{lf}}^T{}_W{(1-K_f)}^W{\hat{f}}_{\mathrm{ext}}---\left(9\right)$

在公式(9)中，K_f是外力分布矩阵，选取适当的值保证双脚的期望外力在各自的支撑区域内。以上控制流程如图3所示。

该方法给定机器人脚部受力约束,通过优化机器人上身加速度,使机器人实际受到的外力满足该约束条件，避免机器人发生由外界干扰产生不可预测的运动，从而达到稳定控制仿人机器人的目的。

Claims (2)

1.一种仿人机器人逆动力学控制方法，所述仿人机器人具有上身、左腿和右腿，所述方法包括以下步骤：

对所述仿人机器人建立动力学模型；所述动力学模型中，上身简化为一个集成的质量块，每条腿有六个关节，q_rl∈R^6×1与q_ll∈R^6×1分别代表右腿与左腿的关节角度，上身浮动坐标系∑_R位于盆骨的中心，世界坐标系Σ_W位于地面；

计算所述仿人机器人上身传统加速度范围；所述加速度范围是使用“空间矢量”方法推导出所述仿人机器人脚底所受外力与上身传统加速度关系,并根据所述仿人机器人当前状态由脚底所受外力范围计算的；

优化所述仿人机器人的上身传统加速度；根据所述仿人机器人当前状态的加速度范围和PD控制产生的目标加速度，通过代价函数计算出该状态的仿人机器人最优上身传统加速度，并根据最优的上身传统加速度，计算出所述仿人机器人所应受外力和关节力矩，从而驱动所述仿人机器人行走；

所述动力学模型的动力学公式为：

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})=\mathrm{\τ}J_{\mathrm{rf}}^{\mathrm{TW}}{}_W{\hat{f}}_{\mathrm{rf}}+J_{\mathrm{lf}}^{\mathrm{TW}}{}_W{\hat{f}}_{\mathrm{lf}}---\left(1\right)$

在式子(1)中，q＝[q_r,q_rl,q_ll]，q_r∈R^6×1为虚拟自由度，M(q)∈R^18×18是关节空间的惯性矩阵，是科氏力、离心力与重力的合力的矢量， 与τ_ll∈R^6×1右腿与左腿的关节力矩输入，是从右脚坐标系转换到世界坐标系Σ_W的雅克比矩阵，是从左脚坐标系转换到Σ_W的雅可比矩阵，是施加在右腿的外力在Σ_W中的表示，是施加在左腿的外力在Σ_W中的表示；

所述仿人机器人在空间矢量的加速度与上身传统加速度关系为：

$\hat{a}_r^W=X_W^{-1}{}_R(a^c-\left[\begin{array}{c}{0}_{3\×1}\\ \mathrm{\ω}_R^r\×v_r^R\end{array}\right])---\left(2\right)$

其中是所述仿人机器人上身所受的上身传统加速度，是所述仿人机器人的空间矢量加速度，[^Rω_r；^Rv_r]＝[ω_x,ω_y,ω_z,v_x,v_y,v_z]^T是上身的空间矢量速度，^RX_W是把速度、加速度和力从Σ_W转换到∑_R的转换矩阵；

在双脚支撑期，左右腿的关节加速度与都是由决定；在单脚支撑期，支撑腿的关节加速度是由决定，而摆动腿的关节加速度是由摆动腿的空间矢量加速度或决定，因此

$\stackrel{\·\·}{q}=Q(q,\stackrel{\·}{q}\hat{a}_r^W,\hat{a}_{\mathrm{rf}}^R\mathrm{or}\hat{a}_{\mathrm{lf}}^R)---\left(3\right)$

根据所述仿人机器人所受加速度用公式(2)和(3)计算关节角加速度；为了计算所述仿人机器人所受外力，使其能达到所需要的加速度，首先使模型作为一个没有外力的固定的模型，在这种情况下，所述仿人机器人关节力矩通过公式(4)计算：

$\left[\begin{array}{c}{\mathrm{\τ}}_r\\ {\mathrm{\τ}}_{r1}\\ {\mathrm{\τ}}_{11}\end{array}\right]=M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})---\left(4\right)$

此时的τ_r是该虚拟自由度的关节力矩，这些虚拟的力矩是来自施加在仿人机器人的虚拟外力产生的，这个虚拟外力与实际期望的外力相等；因此，我们所需要的机器人的外力可由公式(5)计算；在双脚支撑期，所需的外力是由双脚共同提供；在单脚支撑期，所需的外力是由支撑脚提供；

$\hat{f}_{\mathrm{ext}}^W=J_R^{-T}{}_w{\mathrm{\τ}}_r---\left(5\right)$

公式(5)中为雅可比矩阵转制的逆；

然后，根据公式(1)-(5)，机器人上身传统加速度a^c与所受外力的关系可以表示为式子(6)；

${\mathrm{Ua}}^c+V=\hat{f}_m^M---\left(6\right)$

其中，U、V为q和的函数，是含有六个元素的空间矢量， nMx,nMy,nMz,fx,fy,fz为空间六维力/力矩；

根据仿人机器人三个地面约束，得出的范围，再根据所述仿人机器人当前状态得出上身传统加速度的范围；

在得出上身传统加速度a^c的范围后，根据代价函数式子(7)，在范围内求出最优的上身传统加速度a^c，

$f\left(a^c\right)=W_t\·\left|\right|a^c-a_{\mathrm{des}}^c\left|\right|---\left(7\right)$

在式子(7)中，是根据PD控制产生目标加速度，W_t是一个权重矩阵，当f(a^c)最小时的a^c即为最优的a^c。

2.一种仿人机器人逆动力学控制器，所述仿人机器人具有上身、左腿和右腿，所述控制器包括：

用于对所述仿人机器人建立动力学模型的装置；所述动力学模型中，上身简化为一个集成的质量块，每条腿有六个关节，q_rl∈R^6×1与q_ll∈R^6×1分别代表右腿与左腿的关节角度，上身浮动坐标系∑_R位于盆骨的中心，世界坐标系Σ_W位于地面；

用于计算所述仿人机器人上身传统加速度范围的装置；所述加速度范围是使用“空间矢量”方法推导出所述仿人机器人脚底所受外力与上身传统加速度关系,并根据所述仿人机器人当前状态由脚底所受外力范围计算的；

用于优化所述仿人机器人的上身传统加速度的装置；其根据所述仿人机器人当前状态的加速度范围和PD控制产生的目标加速度，通过代价函数计算出该状态的仿人机器人最优上身加速，并根据最优的上身传统加速度，计算出所述仿人机器人所应受外力和关节力矩，从而驱动所述仿人机器人行走；

所述动力学模型的动力学公式为：

$M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})=\mathrm{\τ}J_{\mathrm{rf}}^{\mathrm{TW}}{}_W{\hat{f}}_{\mathrm{rf}}+J_{\mathrm{lf}}^{\mathrm{TW}}{}_W{\hat{f}}_{\mathrm{lf}}---\left(1\right)$

在式子(1)中，q＝[q_r,q_rl,q_ll]，q_r∈R^6×1为虚拟自由度，M(q)∈R^18×18是关节空间的惯性矩阵，是科氏力、离心力与重力的合力的矢量， 与τ_ll∈R^6×1右腿与左腿的关节力矩输入，是从右脚坐标系转换到世界坐标系Σ_W的雅克比矩阵，是从左脚坐标系转换到Σ_W的雅可比矩阵，是施加在右腿的外力在Σ_W中的表示，是施加在左腿的外力在Σ_W中的表示；

所述仿人机器人在空间矢量的加速度与上身传统加速度关系为：

$\hat{a}_r^W=X_W^{-1}{}_R(a^c-\left[\begin{array}{c}{0}_{3\×1}\\ \mathrm{\ω}_R^r\×v_r^R\end{array}\right])---\left(2\right)$

其中是所述仿人机器人上身所受的上身传统加速度，是所述仿人机器人的空间矢量加速度，[^Rω_r；^R v_r]＝[ω_x,ω_y,ω_z,v_x,v_y,v_z]^T是上身的空间矢量速度，^RX_W是把速度、加速度和力从Σ_W转换到∑_R的转换矩阵；

在双脚支撑期，左右腿的关节加速度与都是由决定；在单脚支撑期，支撑腿的关节加速度是由决定，而摆动腿的关节加速度是由摆动腿的空间矢量加速度或决定，因此

$\stackrel{\·\·}{q}=Q(q,\stackrel{\·}{q}\hat{a}_r^W,\hat{a}_{\mathrm{rf}}^R\mathrm{or}\hat{a}_{\mathrm{lf}}^R)---\left(3\right)$

根据所述仿人机器人所受加速度用公式(2)和(3)计算关节角加速度；为了计算所述仿人机器人所受外力，使其能达到所需要的加速度，首先使模型作为一个没有外力的固定的模型，在这种情况下，所述仿人机器人的关节力矩通过公式(4)计算：

$\left[\begin{array}{c}{\mathrm{\τ}}_r\\ {\mathrm{\τ}}_{r1}\\ {\mathrm{\τ}}_{11}\end{array}\right]=M\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})---\left(4\right)$

此时的τ_r是该虚拟自由度的关节力矩，这些虚拟的力矩是来自施加在仿人机器人的虚拟外力产生的，这个虚拟外力与实际期望的外力相等；因此，我们所需要的机器人的外力可由公式(5)计算；在双脚支撑期，所需的外力是由双脚共同提供；在单脚支撑期，所需的外力是由支撑脚提供；

$\hat{f}_{\mathrm{ext}}^W=J_R^{-T}{}_w{\mathrm{\τ}}_r---\left(5\right)$

公式(5)中为雅可比矩阵转制的逆；

然后，根据公式(1)-(5)，机器人上身传统加速度a^c与所受外力的关系可以表示为式子(6)；

${\mathrm{Ua}}^c+V=\hat{f}_m^M---\left(6\right)$

其中，U、V为q和的函数，是含有六个元素的空间矢量， nMx,nMy,nMz,fx,fy,fz为空间六维力/力矩；

根据仿人机器人三个地面约束，得出的范围，再根据所述仿人机器人当前状态得出加速度a^c的范围；

在得出加速度a^c的范围后，根据代价函数式子(7)，在上身传统加速度a^c范围内求出最优的上身传统加速度a^c，

$f\left(a^c\right)=W_t\·\left|\right|a^c-a_{\mathrm{des}}^c\left|\right|---\left(7\right)$

在式子(7)中，是根据PD控制产生目标加速度，W_t是一个重量矩阵，当f(a^c)最小时的a^c即为最优的a^c。