CN102323818B - 一种仿人机器人斜坡步行模式的在线生成的方法 - Google Patents

Abstract

本发明公开了一种仿人机器人斜坡步行模式的在线生成的方法，该方法首先建立仿人机器人在斜面上的桌子-小车模型；然后将建立的桌子-小车模型转化为系统状态方程并在线生成斜坡步行模式，最后根据质心轨迹得到仿人机器人在斜面上步行的关节角度，左右腿分别依次为腰关节、踝关节、膝关节和髋关节，从而控制仿人机器人的伺服驱动器，保证仿人机器人在斜面上的稳定行走；本发明采用反馈校正的技术，保证仿人机器人在斜面行走的稳定性，减小了由于摔倒等带来的器材损耗，从而具有工业适应性强等效果。

Description

一种仿人机器人斜坡步行模式的在线生成的方法

技术领域

本发明属于机器人领域，尤其涉及一种仿人机器人斜坡步行模式的在线生成的方法。

背景技术

机器人是随着现代科技的发展而出现的综合学科，而仿人机器人是机器人研究中的重要分支，其涉及到机器人制造中的各个领域，如自主行为控制、人工智能、动态管理、机械设计等，因此仿人机器人在一定程度上代表着机器人研究的最高水平。 

仿人机器人要应用在各个领域，关键且首要问题是稳定行走。针对这一问题，已有不少学者做了研究，其中，1972年由Vukobratovic和Stepanenko在一篇关于仿人机器人控制的论文开头提出的零力矩点（ZMP zero-moment point）在如今仿人机器人步态控制和规划领域都得到了广泛的应用。而仿人机器人在行走中，很容易碰到上下坡的情况，要保证其稳定性，就要求在控制规划中考虑到这一情况，只有这样，才能使机器人具有更高的环境适应性。

中国专利号CN101323325发明了一种以被动行走为基础，通过在支撑腿前摆过程中伸直支撑腿膝关节逐步提高机器人的重心，补入重力势能的方法。该方法提高了机器人的行走速度，但由于运用的是开环控制，因此在稳定性方面有所欠缺。

中国专利号CN101847009A发明了一种系统化的步态能效优化方法，解决了高能耗产生的机器人实用化障碍，并有效的降低了机器人能耗并保证了其稳定性。此方法较为复杂，且没有考虑机器人在行走过程中地面未必平整等问题。

发明内容

本发明要解决的技术问题是，克服现有技术上的问题，提供一种仿人机器人斜坡步行模式的在线生成的方法，本发明环境适应性强，工程实用性强。

本发明解决该技术问题采用的技术方案是，一种仿人机器人斜坡步行模式的在线生成的方法，包括如下步骤：

（1）建立仿人机器人在斜面上的桌子-小车模型；

（2）将步骤1建立的桌子-小车模型转化为系统状态方程；

（3）在线生成斜坡步行模式；

（4）根据步骤3得到的质心轨迹得到仿人机器人在斜面上步行的关节角度，左右腿分别依次为腰关节、踝关节、膝关节和髋关节，从而控制仿人机器人的伺服驱动器，保证仿人机器人在斜面上的稳定行走。

其中，所述步骤（1）中，首先定义双足机器人近似为不计质量的桌子及在其水平面上行走的质量为m的小车；小车的运动状态决定地面对桌子的压力中心，即小车的运动改变ZMP；然后定义一个固定坐标系，其原点为机器人处于初始状态时正下方斜面上的那个点，其x轴沿斜面指向前方，y轴沿斜面指向右边，z轴垂直斜面指向上方。在此坐标系下，设定小车坐标为（x,y,Zc），对应该时刻的ZMP坐标为（Px,Py,0）；同时，小车在x方向的位置，速度，加速度分别为x，                                                

，

，小车在y方向的位置，速度，加速度分别为y，

，

，且设桌子对小车的力在x，y和z方向分别为

,,

，小车对桌子的力为

,

,

；最后，通过以下子步骤建立在斜面上（与平面成

度）行走的仿人机器人的桌子-小车模型：

（1.1）对小车进行受力分析：

    x方向：

，

    y方向：

 ，                            

    z方向：

；

    从以上三式可以得到：，

     ，

     ；

（1.2）对桌子进行受力分析：

     

；

    代入

的值可以得到：

     ；  

即：；

从上面三式可推得：

；

；

   因为机器人在斜面上步行时，应保证身体与地面保持垂直，所以此时质心高度不再是，而是，

， 用

替代

可得仿人机器人在斜面上质心与ZMP之间的关系：

          

  ，             

  。

所述步骤（2）中，定义小车加速度对时间的微分为系统输入变量

，以此为输入，ZMP用p来表示，定义为系统输出变量，则可根据以下子步骤分别得到x方向和y方向的系统状态方程：

（2.1）x方向上的系统状态方程：

，

 ；                            

将规划ZMP的x方向平移 

 得到：

；

利用采样时间

对连续系统方程（x方向）进行离散化：

；   

（2.2）y方向上的系统状态方程：

           

，

    ；                                   

利用采样时间

对连续系统方程（y方向）进行离散化：

。

所述步骤（3）包括以下子步骤：

（3.1）由仿人机器人规划的ZMP为

与步骤二中所述的系统状态方程输出的

进行比较，得到误差，

，然后进行反馈校正，最终使

趋向于零；为了到达这一目标，考虑性能指标极小化的问题：

        

 ；

其中：    

，

，

；

，

，是正的加权系数，

是期望输出。

（3.2）根据预观控制理论，可以通过使用未来N步目标参考值的输入进行极小化：

         

；

其中：

，

，

，

，

，

，

，

，

，

；

，

，

是正的加权系数， P是用Riccati方程求得的，

是期望输出。

（3.3）把输入控制量u分别代入步骤2中x和y方向上系统状态方程，得到质心的轨迹。

本发明与现有技术相比，有益效果是：本发明通过建立仿人机器人在斜面上的桌子-小车模型，采用反馈校正的技术，保证仿人机器人在斜面行走的稳定性，减小了由于摔倒等带来的器材损耗，从而具有工业适应性强等效果。

附图说明

图1是平面上的桌子-小车模型示意图；

图2是斜面上的桌子-小车模型示意图；

图3是跟踪目标ZMP的伺服控制器示意图；

图4 是实施例1的x方向上ZMP与质心对比图；

图5 是实施例1的y方向上ZMP与质心对比图；

图6是实施例2腿部运动仿真图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步说明。

一、建立仿人机器人在斜面上的桌子-小车模型

桌子-小车模型对于在平面上行走的仿人机器人已经有了广泛的应用，其定义是双足机器人近似为不计质量的桌子及在其水平面上行走的质量为m的小车。小车的运动状态决定地面对桌子的压力中心，即小车的运动改变ZMP（见图1）。

在此基础上，建立了所示的在斜面上（与平面成

度）行走的仿人机器人的桌子-小车模型（见图2）。我们定义一个固定坐标系，其原点为机器人处于初始状态时正下方斜面上的那个点，其x轴沿斜面指向前方，y轴沿斜面指向右边，z轴垂直斜面指向上方。在此坐标系下，我们设定小车坐标为（x,y,Zc），对应该时刻的ZMP坐标为（Px,Py,0）。同时，小车在x和y方向的位置，速度，加速度分别为x，

，

，且设桌子对小车的力在x，y和z方向分别为

,

,

，小车对桌子为

,,。

1.1、对小车进行受力分析：

    x方向：

，

    y方向：

 ，                            

    z方向：

；

    从以上三式可以得到：

，

     ，

     

；

1.2、对桌子进行受力分析：

     

；

    代入

的值可以得到：

     ；  

即：

；

从上面三式可推得：

；

；

   因为机器人在斜面上步行时，应保证身体与地面保持垂直，所以此时质心高度不再是

，而是

，

， 用替代

可得仿人机器人在斜面上质心与ZMP之间的关系：

          

  ，             

  。                                

二、将数学模型转化为系统状态方程

定义小车加速度对时间的微分为系统输入变量

，以此为输入，ZMP用p来表示，为系统输出变量，则可得到系统状态方程。

2.1、x方向上的系统状态方程：

，

 ；                            

将规划ZMP的x方向平移 

 得到：

；

利用采样时间对连续系统方程（x方向）进行离散化：

；   

2.2 、y方向上的系统状态方程：

           

，

    ；                                   

利用采样时间

对连续系统方程（y方向）进行离散化：

。

三、斜坡步行模式的在线生成

3.1、由仿人机器人规划的ZMP为

与步骤二中所述的系统状态方程输出的进行比较，得到误差，

，然后进行反馈校正，最终使

趋向于零（见图3）。为了到达这一目标，考虑性能指标极小化的问题：

        

 ；

其中：    

，

，

；

，

，是正的加权系数，

是期望输出。

3.2、根据预观控制理论，这一性能指标可以通过使用未来N步目标参考值的输入进行极小化：

         

；

其中：

，

，

，

，

，

，

，

，

，

；

，

，

是正的加权系数， P是用Riccati方程求得的，

是期望输出。

3.3、把输入控制量u分别代入步骤二中x和y方向上系统状态方程，得到质心的轨迹。

四、根据步骤三得到的质心轨迹得到仿人机器人在斜面上步行的关节角度，左右腿分别依次为腰关节、踝关节、膝关节和髋关节，从而控制仿人机器人的伺服驱动器，保证仿人机器人在斜面上的稳定行走。

可以根据梶田秀思的《仿人机器人》书中运动学这一章节中的多连杆模型和逆运动学的内容，由质心轨迹得到仿人机器人在斜面上步行的关节角度。至此已经实现了本发明所诉的仿人机器人斜坡步行模式的在线生成。以下我们用实例来进行验证。

采用具体应用实例并通过matlab仿真来观察实验结果。

我们做了两个例子的实例。第一个例子的目的在于验证用前述的方法，由规划好的ZMP轨迹计算得的质心轨迹能否很好的跟随ZMP轨迹，以保证机器人在斜面上运动是的稳定性。我们假设仿人机器人的步行周期T（T=1s），采样周期Ts（Ts=0.01s），步行速度0.5km/h，质心高度Zc（Zc=0.15m）。其结果如图4和图5：分别为仿人机器人在x和y方向上的ZMP规划轨迹与质心轨迹的对比图，从中可以看出，质心轨迹对ZMP轨迹的跟随效果较好，从而保证了仿人机器人斜面行走时的稳定性。

第二个例子是在第一个实例的基础上，由质心轨迹，结合多连杆模型和逆运动学，得到仿人机器人在斜面上步行的各个关节角度，并对腿部运动做了仿真。我们假设机器人的大腿和小腿长度都为0.3m。其结果如图6：实线表示左脚，虚线表示右脚，这里的腿部运动即为右脚不动左脚迈一步的仿真图，可以看出，仿人机器人能在斜面上行走，从而证明了此方法的有效性。

Claims (3)

1.一种仿人机器人斜坡步行模式的在线生成的方法，其特征在于，包括以下步骤：

（1）建立仿人机器人在斜面上的桌子-小车模型；

（2）将步骤1建立的桌子-小车模型转化为系统状态方程；

（3）在线生成斜坡步行模式；

（4）根据步骤3得到的质心轨迹得到仿人机器人在斜面上步行的关节角度，左右腿分别依次为腰关节、踝关节、膝关节和髋关节，从而控制仿人机器人的伺服驱动器，保证仿人机器人在斜面上的稳定行走；

其中，所述步骤（1）中，首先定义双足机器人近似为不计质量的桌子及在其水平面上行走的质量为m的小车；小车的运动状态决定地面对桌子的压力中心，即小车的运动改变ZMP；然后定义一个固定坐标系，其原点为机器人处于初始状态时正下方斜面上的那个点，其x轴沿斜面指向前方，y轴沿斜面指向右边，z轴垂直斜面指向上方。在此坐标系下，设定小车坐标为（x,y,Zc），对应该时刻的ZMP坐标为（Px,Py,0）；同时，小车在x方向的位置，速度，加速度分别为x，小车在y方向的位置，速度，加速度分别为y，

且设桌子对小车的力在x，y和z方向分别为f_x,f_y,N，小车对桌子的力为f_x,f_y,N；最后，通过以下子步骤建立在斜面上（与平面成α度）行走的仿人机器人的桌子-小车模型：

（1.1）对小车进行受力分析：

x方向： $f_x-\mathrm{mg}\sin \mathrm{\α}=m\stackrel{\·\·}{x},$

y方向： $f_y=m\stackrel{\·\·}{y},$

z方向：N-mg cosα=0；

从以上三式可以得到： $f_x=\mathrm{mg}\sin \mathrm{\α}+m\stackrel{\·\·}{x},$

$f_y=m\stackrel{\·\·}{y},$

N=mg cosα；

（1.2）对桌子进行受力分析：

(x,y,z_c)+k(-f_x,-f_y,-N)=(P_x,P_y,0)；

代入f_x f_y N的值可以得到：

$(x,y,z_c)+k(-(g\sin \mathrm{\α}+\stackrel{\·\·}{x}),-\stackrel{\·\·}{y},-g\cos \mathrm{\α})=(P_x,P_y,0);$

$x-k(g\sin \mathrm{\α}+\stackrel{\·\·}{x})=P_x$

即： $y=k\stackrel{\·\·}{y}=P_y$

z_c-kg cosα=0；

从上面三式可推得：

$k=\frac{z_c}{g\cos \mathrm{\α}};$

$P_x=x-k(g\sin \mathrm{\α}+\stackrel{\·\·}{x})=x-\frac{z_c}{g\cos \mathrm{\α}}(g\sin \mathrm{\α}+\stackrel{\·\·}{x})=x-z_c\tan \mathrm{\α}-\frac{z_c}{g\cos \mathrm{\α}}\stackrel{\·\·}{x}$

$P_y=y-k\stackrel{\·\·}{y}=y-\frac{z_c}{g\cos \mathrm{\α}}\stackrel{\·\·}{y};$

因为机器人在斜面上步行时，应保证身体与地面保持垂直，所以此时质心高度不再是Z_c，而是Z_c′，Z_c′＝Z_ccosα，用Z_c′替代Z_c可得仿人机器人在斜面上质心与ZMP之间的关系：

$p_x=x-{Z_c}^{\′}\tan \mathrm{\α}-\frac{{Z_c}^{\′}}{g\cos \mathrm{\α}}\stackrel{\·\·}{x}=x-Z_c\sin \mathrm{\α}-\frac{Z_c}{g}\stackrel{\·\·}{x},$

$p_y=y-\frac{{Z_c}^{\′}}{g\cos \mathrm{\α}}\stackrel{\·\·}{y}=y-\frac{Z_c}{g}\stackrel{\·\·}{y}.$

2.根据权利要求1所述仿人机器人斜坡步行模式的在线生成的方法，其特征在于，所述步骤（2）中，定义小车加速度对时间的微分为系统输入变量

以此为输入，ZMP用p来表示，定义为系统输出变量，则可根据以下子步骤分别得到x方向和y方向的系统状态方程：

（2.1）x方向上的系统状态方程：

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ \stackrel{\·\·}{x}\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ \stackrel{\·\·}{x}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u,$

$p_x=\left[\begin{array}{ccc}1& 0& -\frac{Z_c}{g}\end{array}\right]\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ \stackrel{\·\·}{x}\end{array}\right]-Z_c\sin \mathrm{\α};$

将规划ZMP的x方向平移Z_csinα得到：

${p_x}^{\′}=p_x+Z_c\sin \mathrm{\α}=\left[\begin{array}{ccc}1& 0& -\frac{Z_c}{g}\end{array}\right]\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ \stackrel{\·\·}{x}\end{array}\right];$

利用采样时间T_s对连续系统方程（x方向）进行离散化：

$A\≡\left[\begin{array}{ccc}1& T_s& {T_s}^2/2\\ 0& 1& T_s\\ 0& 0& 1\end{array}\right],$ $B\≡\left[\begin{array}{c}{T_s}^3/6\\ {T_s}^2/2\\ T_s\end{array}\right],$ $C\≡\left[\begin{array}{ccc}1& 0& -\frac{Z_c}{g}\end{array}\right];$

（2.2）y方向上的系统状态方程：

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ \stackrel{\·\·}{x}\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ \stackrel{\·\·}{x}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u,$

$p_x=\left[\begin{array}{ccc}1& 0& -\frac{Z_c}{g}\end{array}\right]\left[\begin{array}{c}x\\ \stackrel{\·}{x}\\ \stackrel{\·\·}{x}\end{array}\right];$

利用采样时间T_s对连续系统方程（y方向）进行离散化：

$A\≡\left[\begin{array}{ccc}1& T_s& {T_s}^2/2\\ 0& 1& T_s\\ 0& 0& 1\end{array}\right],$ $B\≡\left[\begin{array}{c}{T_s}^3/6\\ {T_s}^2/2\\ T_s\end{array}\right],$ $C\≡\left[\begin{array}{ccc}1& 0& -\frac{Z_c}{g}\end{array}\right].$

3.根据权利要求1所述仿人机器人斜坡步行模式的在线生成的方法，其特征在于，所述步骤（3）包括以下子步骤：

（3.1）由仿人机器人规划的ZMP为y_d(k)与步骤二中所述的系统状态方程输出的y(k)进行比较，得到误差，e(k)=y(k)-y_d(k)，然后进行反馈校正，最终使e(k)趋向于零；为了到达这一目标，考虑性能指标极小化的问题：

$J=\underset{i=k}{\overset{\∞}{\mathrm{\Σ}}}[e^T\left(i\right)Q_{e}e\left(i\right)+{\mathrm{\Δx}}^T\left(i\right)Q_x\mathrm{\Δx}\left(i\right)+{\mathrm{\Δu}}^T\left(i\right)\mathrm{R\Δu}\left(i\right)];$

其中：e(k)=y(k)-y_d(k)

Δx(k)=x(k)-x(k-1)，

Δu(k)=u(k)-u(k-1)；

Q_e，Q_x，R₁是正的加权系数，y_d(k)是期望输出。

（3.2）根据预观控制理论，e(k)可以通过使用未来N步目标参考值的输入进行极小化：

$u\left(k\right)=-G_l\underset{i=0}{\overset{k}{\mathrm{\Σ}}}e\left(i\right)-G_{x}x\left(k\right)-\underset{l=1}{\overset{N}{\mathrm{\Σ}}}{G}_d\left(l\right)y_d(k+l);$

其中：G₁=(R₁+B₁ ^TPB₁)^-1B₁ ^TPI₁，

G_x=(R₁+B₁ ^TpB₁)^-1B₁ ^TpF₁，

G_d(1)=-G₁，

G_d(i)=-(R₁+B₁ ^TPB₁)^-1B₁ ^T(A_c ^T)^i-1PI₁，

A_c=A₁-B₁(R₁+B₁ ^TPB₁)^-1B₁ ^TPA₁，

$B_1=\left[\begin{array}{c}\mathrm{CB}\\ B\end{array}\right],$ $I_1=\left[\begin{array}{c}{I}_p\\ 0\end{array}\right],$ $F_1=\left[\begin{array}{c}\mathrm{CA}\\ A\end{array}\right],$ $Q_1=\left[\begin{array}{cc}{Q}_e& 0\\ 0& Q_x\end{array}\right],$ A₁=[I₁  F₁]；

Q_e，Q_x，R₁是正的加权系数，P是用Riccati方程求得的，y_d(k)是期望输出。

（3.3）把输入控制量u分别代入步骤二中x和y方向上系统状态方程，得到质心的轨迹。

Images