CN104318071A - 一种基于线性落脚点补偿器的机器人行走控制方法 - Google Patents

Abstract

本发明涉及一种基于线性落脚点补偿器的机器人行走控制方法，包括以下步骤：1)建立机器人的动力学模型；2)根据机器人的动力学模型，建立线性落脚点补偿器；3)通过尝试-评估-改进的学习方法，学习特定环境下的落脚点补偿器最优增益系数；4)验证所述方法在仿人机器人行走控制中的作用。与现有技术相比，本发明具有学习能力强，抗干扰能力强等优点。

Description

一种基于线性落脚点补偿器的机器人行走控制方法

技术领域

本发明涉及机器人控制领域，尤其是涉及一种基于线性落脚点补偿器的机器人行走控制方法。

背景技术

许多仿人机器人行走控制的研究人员将重点放在规划机器人的质心轨迹，使得机器人行走时的实际ZMP轨迹能精确地跟踪事先规划的ZMP轨迹。而该事先规划的ZMP轨迹一般是由预先规划的机器人的落脚位置(即落脚点)来决定的。这一类控制方法总体来说属于开环控制方法，对机器人预设轨迹的跟踪建立在对机器人可以完全控制的假设之上，即机器人的质心状态是完全可控的。事实上，由于仿人机器人与地面的接触是非稳固的，地面只能对仿人机器人提供推力而不能提供反向的吸引力。因此，仿人机器人的质心控制是半可控的。一旦机器人的落脚点决定了，机器人能够获得用来加速质心的力也就同时被限定在一定的区域以内。

另一方面，由于仿人机器人的落脚点决定了行走控制中质心的可控性，因此动态的调整仿人机器人的落脚点便可以调整机器人的动态特性及可控性，进而可以改善机器人对未知扰动的抑制能力。事实上，落脚点对双足行走的重要性在生物机械学领域已有较深入的研究。最近，在机器人领域也有一些利用落脚点补偿技巧提高仿人机器人行走控制鲁棒性的初步探索。目前的方法大多数是建立在简化的线性机器人动力学模型上，落脚点修改技巧无法动态调整且基于线性模型预测控制方法的计算量较大。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种学习能力强、可靠性高的基于线性落脚点补偿器的机器人行走控制方法。

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

一种基于线性落脚点补偿器的机器人行走控制方法，包括以下步骤：

1)建立机器人的动力学模型；

2)根据机器人的动力学模型，建立线性落脚点补偿器；

3)通过尝试-评估-改进的学习方法，学习特定环境下的落脚点补偿器最优增益系数；

4)验证所述方法在仿人机器人行走控制中的作用。

所述的步骤1)包括以下步骤：

11)建立机器人三维线性倒立摆动力学模型，该动力学模型的表达式为：

${\stackrel{\·}{x}}_c={\mathrm{Ax}}_c+{\mathrm{Bu}}_x$

x_z＝Cx_c

其中， $A=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],C=\left[\begin{array}{ccc}1& 0& -\frac{z_c-z_z}{g}\end{array}\right],x_c={[x_c,{\stackrel{\·}{x}}_c,{\stackrel{\·\·}{x}}_c]}^T$ 为机器人质心在世界坐标系下沿x轴的位置、速度和加速度，u_x为质心加速度的变化量并用来控制质心加速度，p_c＝[x_c，y_c，y_c]^T为质心在世界坐标系下的三维位置，而p_z＝[x_z，y_z，y_z]^T为ZMP在世界坐标系下的位置，g为重力加速度；

所述的步骤2)包括以下步骤：

21)定义落脚点补偿器的输出Δp^f：

${\mathrm{\Δp}}^f\≡({\mathrm{\Δp}}_x^f,{\mathrm{\Δp}}_y^f);$

22)定义线性落脚点补偿器为：

${\mathrm{\Δp}}_x^f=G_{\mathrm{Kx}}\hat{x}+G_{\mathrm{lx}}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}({\hat{p}}_{x,i}-p_{x,i}^{\mathrm{ref}})$

${\mathrm{\Δp}}_y^f=G_{\mathrm{Ky}}\hat{y}+G_{\mathrm{ly}}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}({\hat{p}}_{y,i}-p_{y,i}^{\mathrm{ref}})$

其中，为估计的支撑脚坐标系下质心位置、速度和加速度，和为前i帧中估计的和参考的支撑脚坐标系下ZMP轨迹，G_Kx、G_Ky、G_lx和G_ly为增益系数；

23)选择最优的线性落脚点补偿器增益系数，使增益系数满足最小化的条件：

$J_f\≡\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}({\mathrm{\α}}_x{\mathrm{\Δp}}_{x,j}+{\mathrm{\α}}_y{\mathrm{\Δp}}_{y,j})+\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left({\mathrm{\β}}_x\right|{\mathrm{\Δp}}_{x,i}^f|+{\mathrm{\β}}_y|{\mathrm{\Δp}}_{y,i}^f\left|\right)+\mathrm{\ξ}$

$\mathrm{\ξ}\≡\mathrm{\γ}(\frac{\mathrm{\π}}{2}-\frac{1}{2}a\tan (\frac{2n}{\mathrm{PG}}-3))$

其中， $\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}({\mathrm{\α}}_x{\mathrm{\Δp}}_{x,j}+{\mathrm{\α}}_y{\mathrm{\Δp}}_{y,j})$ 为ZMP的跟踪误差， $\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left({\mathrm{\β}}_x\right|{\mathrm{\Δp}}_{x,i}^f|+{\mathrm{\β}}_y|{\mathrm{\Δp}}_{y,i}^f\left|\right)$ 是为了保证输出不过大，ξ为一非线性函数，n为ZMP累计误差的帧数，α_x和α_y表示x轴和y轴分别所占的比例，m是统计过程落脚点修改的次数，β_x和β_y控制x和y轴的比例，PG为每步需要的时间帧数，γ为常系数。

所述的步骤3)包括以下步骤：

31)赋予机器人线性落脚补偿器初始增益系数φ₀，并将待测参数R≡{r₁，…r_M}根据初始增益系数和原子策略集合ψ_i产生行走参数r_j：

r_j＝{θ₁+Δ₁，…，θ_N+Δ_N}

φ≡[G_Kx，G_lx，G_Ky，G_ly]≡{θ₁，…，θ_N}

ψ_i≡{-ε_i，0，+ε_i}

其中，Δ₁…Δ_N为ψ_i中随机抽取的值，-ε_i，ε_i为趋近于零的实数；

32)机器人按产生的多组行走参数行走，并搜集计算线性落脚点补偿器性能指标所需的数据；

33)在完成多组行走参数行走测试后，计算线性落脚点补偿器的当前梯度并且更新线性落脚点补偿器的增益系数；

34)机器人重复步骤31)-步骤33)，通过迭代过程获取最优的线性落脚点补偿器参数。

所述的步骤33)包括以下步骤：

331)计算平均分值矩阵该矩阵表示在参数第n维选取策略-ε、0或+ε的所有r_j的平均分值；

332)当且时，计算平均分值F_n为0；否则平均分值为 $F_n={\stackrel{\‾}{S}}_{n,+\mathrm{\ϵ}}-{\stackrel{\‾}{S}}_{n,-\mathrm{\ϵ}};$

333)计算落脚点补偿器当前梯度为

334)更新落脚点补偿器的增益系数φ：

$\mathrm{\φ}=\mathrm{\φ}+\▿\mathrm{\φ}\×\mathrm{\η}$

其中，η为每次迭代的步长。

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

一、学习能力强，通过引入基于策略梯度下降法的在线补偿器学习方法，实现了仿人机器人落脚点平衡技巧的自我学习。

二、抗干扰能力强，采用了学习后的线性落脚点补偿器，能够帮助仿人机器人抵御未知外部扰动。

附图说明

图1为本发明的方法流程图。

图2为仿人机器人无落脚点补偿器行走控制框图。

图3为带有线性落脚点补偿器的机器人行走控制框图。

图4为无落脚补偿器作用下机器人在冲击扰动下的行走数据。

图5为线性落脚补偿器作用下机器人在冲击扰动下的行走数据。

图6为X-Y平面下落脚点补偿器在弧线行走时的效果图。

图7为Y轴下落脚点补偿器在弧线行走时的效果图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例：

一种基于线性落脚点补偿器的机器人行走控制方法，包括以下步骤：

1)建立机器人的动力学模型；

2)根据机器人的动力学模型，建立线性落脚点补偿器；

3)通过尝试-评估-改进的学习方法，学习特定环境下的落脚点补偿器最优增益系数；

4)验证所述方法在仿人机器人行走控制中的作用。

如图3所示，该基于线性落脚点补偿器的机器人行走控制方法首先采用LMPC从规划的ZMP轨迹生成规划机器人质心轨迹控制机器人行走。根据测量的机器人质心状态和测量ZMP，采用FPC模块生成落脚点补偿量，修正ZMP生成器，最终影响机器人的动态响应G，实现机器人对行走扰动的抑制。

步骤1)包括以下步骤：

11)建立机器人三维线性倒立摆动力学模型，该动力学模型的表达式为：

${\stackrel{\·}{x}}_c={\mathrm{Ax}}_c+{\mathrm{Bu}}_x$

x_z＝Cx_c

其中， $A=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],C=\left[\begin{array}{ccc}1& 0& -\frac{z_c-z_z}{g}\end{array}\right],x_c={[x_c,{\stackrel{\·}{x}}_c,{\stackrel{\·\·}{x}}_c]}^T$ 为机器人质心在世界坐标系下沿x轴的位置、速度和加速度，u_x为质心加速度的变化量并用来控制质心加速度，p_c＝[x_c，y_c，y_c]^T为质心在世界坐标系下的三维位置，而p_z＝[x_z，y_z，y_z]^T为ZMP在世界坐标系下的位置，g为重力加速度；

步骤2)包括以下步骤：

21)定义落脚点补偿器的输出Δp^f：

${\mathrm{\Δp}}^f\≡({\mathrm{\Δp}}_x^f,{\mathrm{\Δp}}_y^f);$

22)定义线性落脚点补偿器为：

${\mathrm{\Δp}}_x^f=G_{\mathrm{Kx}}\hat{x}+G_{\mathrm{lx}}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}({\hat{p}}_{x,i}-p_{x,i}^{\mathrm{ref}})$

${\mathrm{\Δp}}_y^f=G_{\mathrm{Ky}}\hat{y}+G_{\mathrm{ly}}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}({\hat{p}}_{y,i}-p_{y,i}^{\mathrm{ref}})$

其中，为估计的支撑脚坐标系下质心位置、速度和加速度，和为前i帧中估计的和参考的支撑脚坐标系下ZMP轨迹，G_Kx、G_Ky、G_lx和G_ly为增益系数；

23)选择最优的线性落脚点补偿器增益系数，使增益系数满足最小化的条件：

$J_f\≡\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}({\mathrm{\α}}_x{\mathrm{\Δp}}_{x,j}+{\mathrm{\α}}_y{\mathrm{\Δp}}_{y,j})+\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left({\mathrm{\β}}_x\right|{\mathrm{\Δp}}_{x,i}^f|+{\mathrm{\β}}_y|{\mathrm{\Δp}}_{y,i}^f\left|\right)+\mathrm{\ξ}$

$\mathrm{\ξ}\≡\mathrm{\γ}(\frac{\mathrm{\π}}{2}-\frac{1}{2}a\tan (\frac{2n}{\mathrm{PG}}-3))$

其中， $\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}({\mathrm{\α}}_x{\mathrm{\Δp}}_{x,j}+{\mathrm{\α}}_y{\mathrm{\Δp}}_{y,j})$ 为ZMP的跟踪误差， $\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left({\mathrm{\β}}_x\right|{\mathrm{\Δp}}_{x,i}^f|+{\mathrm{\β}}_y|{\mathrm{\Δp}}_{y,i}^f\left|\right)$ 是为了保证输出不过大，ξ为一非线性函数，n为ZMP累计误差的帧数，α_x和α_y表示x轴和y轴分别所占的比例，m是统计过程落脚点修改的次数，β_x和β_y控制x和y轴的比例，PG为每步需要的时间帧数，γ为常系数。

步骤3)包括以下步骤：

31)赋予机器人线性落脚补偿器初始增益系数φ₀，并将待测参数R≡{r1，…r_M}根据初始增益系数和原子策略集合ψ_i产生行走参数r_j：

r_j＝{θ₁+Δ₁，…，θ_N+Δ_N}

φ≡[G_Kx，G_lx，G_Ky，G_ly]≡{θ₁，…，θ_N}

ψ_i≡{-ε_i，0，+ε_i}

其中，Δ₁…Δ_N为ψ_i中随机抽取的值，-ε_i，ε_i为趋近于零的实数；

32)机器人按产生的多组行走参数行走，并搜集计算线性落脚点补偿器性能指标所需的数据；

33)在完成多组行走参数行走测试后，计算线性落脚点补偿器的当前梯度并且更新线性落脚点补偿器的增益系数；

34)机器人重复步骤31)-步骤33)，通过迭代过程获取最优的线性落脚点补偿器参数。

步骤33)包括以下步骤：

331)计算平均分值矩阵该矩阵表示在参数第n维选取策略-ε、0或+ε的所有r_j的平均分值；

332)当且时，计算平均分值F_n为0；否则平均分值为 $F_n={\stackrel{\‾}{S}}_{n,+\mathrm{\ϵ}}-{\stackrel{\‾}{S}}_{n,-\mathrm{\ϵ}};$

333)计算落脚点补偿器当前梯度为

334)更新落脚点补偿器的增益系数φ：

$\mathrm{\φ}=\mathrm{\φ}+\▿\mathrm{\φ}\×\mathrm{\η}$

其中，η为每次迭代的步长。

如图4所示，在没有落脚点补偿的时候，机器人在扰动下行走ZMP逐渐发散并最终摔倒。如图5、6、7所示，在有落脚点补偿器的时候，机器人可以抑制扰动，实现稳定的行走。

Claims (5)

1.一种基于线性落脚点补偿器的机器人行走控制方法，其特征在于，包括以下步骤：

1)建立机器人的动力学模型；

2)根据机器人的动力学模型，建立线性落脚点补偿器；

3)通过尝试、评估和改进的学习方法，学习特定环境下的线性落脚点补偿器最优增益系数；

4)根据基于线性落脚点补偿器对仿人机器人行走进行预测控制。

2.根据权利要求1所述的一种基于线性落脚点补偿器的机器人行走控制方法，其特征在于，所述的步骤1)包括以下步骤：

11)建立机器人三维线性倒立摆动力学模型，该动力学模型的表达式为：

${\stackrel{\·}{x}}_c={\mathrm{Ax}}_c+{\mathrm{Bu}}_x$

x_z＝Cx_c

其中， $A=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right],$ $B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],$ $C=\left[\begin{array}{ccc}1& 0& -\frac{z_c-z_z}{g}\end{array}\right],$ $x_c={[x_c,{\stackrel{\·}{x}}_c,{\stackrel{\·\·}{x}}_c]}^T$ 为机器人质心在世界坐标系下沿x轴的位置、速度和加速度，u_x为质心加速度的变化量并用来控制质心加速度，p_c＝[x_c，y_c，y_c]^T为质心在世界坐标系下的三维位置，而p_z＝[x_z，y_z，y_z]^T为ZMP在世界坐标系下的位置，g为重力加速度；

3.根据权利要求1所述的一种基于线性落脚点补偿器的机器人行走控制方法，其特征在于，所述的步骤2)包括以下步骤：

21)定义线性落脚点补偿器的输出Δp^f：

$\mathrm{\Δ}{p}^f\≡(\mathrm{\Δ}{p}_x^f,\mathrm{\Δ}{p}_y^f);$

22)定义线性落脚点补偿器为：

$\mathrm{\Δ}{p}_x^f=G_{\mathrm{Kx}}\hat{x}+G_{\mathrm{lx}}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}({\hat{p}}_{x,i}-p_{x,i}^{\mathrm{ref}})$

$\mathrm{\Δ}{p}_y^f=G_{\mathrm{Ky}}\hat{y}+G_{\mathrm{ly}}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}({\hat{p}}_{y,i}-p_{y,i}^{\mathrm{ref}})$

其中，为估计的支撑脚坐标系下质心位置、速度和加速度，和为前i帧中估计的和参考的支撑脚坐标系下ZMP轨迹，G_Kx、G_Ky、G_lx和G_ly为增益系数；

23)选择最优的线性落脚点补偿器增益系数，使增益系数满足最小化的条件：

$J_f\≡\frac{1}{n}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}({\mathrm{\α}}_x\mathrm{\Δ}{p}_{x,j}+{\mathrm{\α}}_y\mathrm{\Δ}{p}_{y,j})+\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left({\mathrm{\β}}_x\right|\mathrm{\Δ}{p}_{x,i}^f|+{\mathrm{\β}}_y|\mathrm{\Δ}{p}_{y,i}^f\left|\right)+\mathrm{\ξ}$

$\mathrm{\ξ}\≡\mathrm{\γ}(\frac{\mathrm{\π}}{2}-\frac{1}{2}a\tan (\frac{2n}{\mathrm{PG}}-3))$

其中，为ZMP的跟踪误差，是为了保证输出不过大，ξ为一非线性函数，n为ZMP累计误差的帧数，α_x和α_y表示x轴和y轴分别所占的比例，m是统计过程落脚点修改的次数，β_x和β_y控制x和y轴的比例，PG为每步需要的时间帧数，γ为常系数。

4.根据权利要求1所述的一种基于线性落脚点补偿器的机器人行走控制方法，其特征在于，所述的步骤3)包括以下步骤：

31)赋予机器人线性落脚补偿器初始增益系数φ₀，并将待测参数R≡{r₁，…r_M}根据初始增益系数和原子策略集合ψ_i产生行走参数r_j：

r_j＝{θ₁+Δ₁，…，θ_N+Δ_N}

φ≡[G_Kx，G_lx，G_Ky，G_ly]≡{θ₁，…，θ_N}

ψ_i≡{-ε_i，0，+ε_i}

其中，Δ₁…Δ_N为ψ_i中随机抽取的值，-ε_i，ε_i为趋近于零的实数；

32)机器人按产生的多组行走参数行走，并搜集计算线性落脚点补偿器性能指标所需的数据；

33)在完成多组行走参数行走测试后，计算线性落脚点补偿器的当前梯度并且更新线性落脚点补偿器的增益系数；

34)机器人重复步骤31)-步骤33)，通过迭代过程获取最优的线性落脚点补偿器参数。

5.根据权利要求4所述的一种基于线性落脚点补偿器的机器人行走控制方法，其特征在于，所述的步骤33)包括以下步骤：

331)计算平均分值矩阵该矩阵表示在参数第n维选取策略-ε、0或+ε的所有r_j的平均分值；

332)当且时，计算平均分值F_n为0；否则平均分值为 $F_n={\stackrel{\‾}{S}}_{n,+\mathrm{\ϵ}}-{\stackrel{\‾}{S}}_{n,-\mathrm{\ϵ}};$

333)计算落脚点补偿器当前梯度为

334)更新落脚点补偿器的增益系数φ：

$\mathrm{\φ}=\mathrm{\φ}+\▿\mathrm{\φ}\×\mathrm{\η}$

其中，η为每次迭代的步长。