【Roban教程】基于线性倒立摆的双足步态生成

本节继续探讨上一节提到的单质点模型，通过添加额外的限制来简化上一式中的式14，用其来进行双足步态质心运动轨迹规划。使用线性倒立摆规划质心轨迹时，ZMP集中于支撑杆末端，对应于支撑脚的脚掌中心，这样可以得到理论上稳定的运动轨迹。

一、质心轨迹生成

倒立摆模型 (inverted pendulum model, IPM) 由一个无质量的支撑杆和一个位于支撑杆顶端的质点 (center of mass, COM) 构成。机器人的行走轨迹分别由冠状面和矢状面的倒立摆轨迹组合而成。对于一个固定支撑杆长度的倒立摆来说，其质点在两个平面上的运动方程是耦合的，很难求解。通过引入运动过程中质心高度H不变的约束，可以使得两个运动方程变得独立。这就是线性倒立摆 (innear  inverted pendulum model, LIPM) ，如图1所示。在实际的机器人控制中，保持质心固定的高度不变并不是一个很严格的限制，甚至还有让安装于头部的相机拍摄更平稳的优势。通过在运动过程中改变腿伸直的幅度可以实现保持固定的质心高度。

图1  二维平面下的线性倒立摆运动

线性倒立摆的运动方程导出如下，支撑杆顶端质点受到竖直方向重力mg作用，支撑点受到地面支撑力的作用。整个系统在绕支撑点位置会受到合扭矩 τ=mgx, 其中x为质心与支撑点的水平面距离。当支撑杆的长度会时刻智能变化，保持质心始终位于同一高度时，此扭矩会在水平方向上对质心加速，加速力为 F=r/H 此时有：

式1

该公式表明，线性倒立摆上质点的运动趋势和线性倒立摆的参数及本身状态有关。从数学角度上理解，该公式为一个二阶常微分方程。求解其通解，可得：

式2

上式中各符号分别为：g为重力加速度，H为质心高度，√g/H为倒立摆的时间常数，本书中为简化描述用ω代替。

从数学上来说，确定了某变量的初值及其随时间变化的导数，则可以确定该变量随时间变化的轨迹。从通解公式中可得，在已知初值x₀和u₀时，可求得x随时间t变化的任意时刻的值。线性倒立摆模型中，x₀和u₀分别为质点的初始位置和初始速度。通解公式表明，知道质点初始状态之后，就可以根据线性倒立摆模型求解质点任意时刻的状态了。线性倒立摆的微分方程表征了质点的运动趋势，其通解公式则表达了质点具体的运动轨迹。把式2微分，即可得到质点的速度运动轨迹：

式3

倒立摆在三维空间中的运动由冠状面的侧向运动和矢状面的前向运动构成。两个平面的运动可以单独地由倒立摆轨迹来描述。两个方向的运动合成后如图2所示。分别查看倒立摆冠状面和矢状面的轨迹可以发现二者有一个明显区别，即冠状面轨迹没有越过零位置，矢状面轨迹则越过了零位置。一般来说规划前进运动的时候倒立摆的轨迹会这样分布。规划侧移运动时则冠状面和矢状面的质心轨迹都不会越过零位置，且冠状面的轨迹会是不对称的，从而实现一步一步的侧移。

图2  三维空间中的线性倒立摆运动

在多步连续行走时认为每一个单足支撑期存在一个线性倒立摆，脚掌踩在线性倒立摆的末端位置。考虑简单情况，运行完一个单足支撑期后会进行支撑脚的瞬间切换，接着质心运行下一个倒立摆的运动轨迹，各个倒立摆的轨迹根据该步的步行参数来规划。

首先确定当前步的步行周期T 、双脚的冠状面支撑间距D和双脚的矢状面支撑间距F 。然后分别根据式4和式5分别计算冠状面、矢状面的质心初速度u_0，再根据每一步的质心初始位置x₀和式2计算当前倒立摆的质心轨迹。

式4

式5

最后得到由多个线性倒立摆轨迹拼接而成的质心运动轨迹，如图3所示。

图3  连续行走时的线性倒立摆运动

二、足端轨迹运动

足端轨迹分为支撑脚轨迹和摆动脚轨迹，生成支撑脚轨迹时只需让支撑脚踩在倒立摆的支撑点保持不动即可。

而对双足机器人的摆动腿来说，摆动相的任务是使足端尽快从当前位置摆动至下一步的着地位置，摆动的过程中要保证机器人双腿不发生相互干涉，摆动腿的运动不会对机器人整体产生过大的冲击。在摆动相的任意时刻内，机器人足位置包含四个变量。以 机器人正常站立时重心对地面的投影为坐标原点，建立坐标系，需要的四个变量为竖直高度H、冠状面位置x，矢状面位置 y，围绕Z轴的旋转角θ。在直线行走的情况下，对于冠状面位置x，只需设置为机器人正常站立时的x值，并在行走过程中，保持该值不变即可。因为在步态规划中，机器人矢状面倒立摆摆动跨度F是可变的，而冠状面的倒立摆摆动跨度D是不变的，其值一直为机器人正常站立时双腿中心的间距。

对矢状面位置 y, 使用一个简单的插值函数来解决：

式6

此公式表示摆动腿矢状面位置y在时间T/2内，由F/2变化为F/2 。在双腿交换时， 摆动腿会有小突变，由此会对机器人整体产生冲击。但在机器人实体实验中，发现这样简单的规划下，双腿交换时机器人并没有产生很大的冲击，可以连续行走。推测原因是机器人各个关节的执行器本身具有一定的柔性。当速度突变时，各关节因为本身的柔性能起到缓冲作用，不会机器人产生不良影响。矢状面位置y与时间的变化关系如图4。

图4  摆动腿矢状面轨迹

对竖直高度H, 同样采用插值函数确定。但是此时需要注意的是，摆动腿离地需要干脆利落，避免离地过程中脚面与地面不平滑的部分摩擦，使得机器人受到整体的旋转力矩而改变方向。摆动腿着地时需要稍微缓慢地接触地面，使得脚接触地面时不会受到过大的地面反力作用而不稳。因此，在摆动足上升阶段和下降阶段，用不同的插值函数来规划。

上升阶段为：

式7

下降阶段为：

式8

其相对时间的变化如图5：

围绕Z轴的旋转θ，定义第i1步机器人上身绕Z轴的转角为θ_i-1 ，第i步机器人上身绕Z轴的转角为θ_i，第i+1步机器人上身绕Z轴的转角为θ_i+1。由于第i步的摆动

图5  摆动腿高度变化轨迹

腿就是第i-1步的支撑腿。则在第i步内，摆动腿需要摆动的角度为Δθ=θ_i+1-θ_i-1，故规划转角为：

式9

由此规划，可以在一步开始和结束时，摆动腿的转动速度为零，这样的性质有助于交换支撑腿时保持稳定。假设θ_i-1的值为零，摆动腿转动了 0.2 弧度，则其相对时间的变化为图6。

图6  摆动腿朝向脚变化轨迹

三、台阶及斜坡地形的步态规划

上文介绍了平面地形下的双足步态轨迹规划，其核心是利用线性倒立摆模型得到支撑腿和躯干的运动轨迹。同时手动规划摆动脚的轨迹，使摆动脚从当前步的落脚点摆动至下一步的落脚点。台阶和斜坡地形与上述情况的区别有两个方面。其一是线性倒立摆模型有一个最重要的假设，即在单足支撑期，机器人躯干质心高度不变，速度为零。其二是摆动运动的起点和终点高度不一致了，且终点的高度与落脚姿态可能是已知的，可能是未知的。对于第一个区别，需要在规划台阶和斜坡步态时，规划躯干上升或下降的轨迹，且上升轨迹需要规划在单步周期内机器人不容易失稳的时间段。对于第二个区别， 则需把摆动腿轨迹的末端姿态设为变量，通过视觉及压感检测等方式，得到落脚点姿态， 来实时规划摆动腿轨迹。

1.台阶地形轨迹规划 

为了增强运动的稳定性，避免在躯干执行上升的运动轨迹时摔倒，台阶步态的单步周期，需要由双足支撑相和单足支撑相组成。在一步周期中，支撑腿支撑于当前台阶，摆动脚从上一个台阶开始运动，摆动至下一个台阶。此时躯干运动可以分为以下四个部分的组合：

运动 1: 躯干前进方向的运动。

运动 2: 躯干由两腿中间向支撑脚方向的运动。

运动 3: 躯干由支撑脚向两腿中间方向的运动。

运动 4: 躯干竖直方向的运动。

这四种运动可以由不同的方式规划，再相互叠加。同时叠加的时间分段也可以先后不同，因而造成规划台阶步态时具有非常多样化的选择。而不同的运动叠加方式，得到的台阶步态运动结果，在稳定性、流畅性等方面有巨大的不同。

其中运动4对行走稳定性的影响巨大。因为在线性倒立摆模型中，有一个重要假设， 即认为在机器人行走过程中，躯干质心高度保持不变。而实际过程中，为了完成台阶步态，必须要有运动4

经实验验证，以下几种轨迹规划组合都是可行的，如表1

表1  运动参数

其中因为运动4的特殊性，导致运动1、运动2、运动3与运动4有两种不同的叠加方式，使用以时间为横轴的时序图来表示，其中箭头方向为时间轴：

图7  运动方式叠加1

图8  运动叠加方式2

除了躯干轨迹规划，还需要进行摆动腿的轨迹规划。台阶步态摆动腿运动示意如图6所示，图中ZO₁X为支撑腿所在平面，S为台阶长度，D为台阶高度，摆动腿从上一个台阶运动至下一个台阶。

图9  台阶步态摆动腿运动规则

摆动腿在与平行的平面上运动，其轨迹可以由一段上一台阶落脚点至下一台阶落脚点的斜线与一拱形曲线的叠加。使用不同的拱形曲线具有不同的优势：

sin函数曲线：形状不可调节，加减速过程不柔顺。

摆线：形状不可调节，加减速过程柔顺。

样条曲线：形状可调节，高次样条曲线采样点较多。

贝塞尔曲线：形状可调节，加减速过程柔顺，取样点的轨迹坐标不直观。 

本文采用最终采用贝塞尔曲线的方式来规划足端位置曲线，同时在跨越较高的台阶时，分段规划足端Pitch角的运动轨迹，避免脚掌与台阶产生干涉。 

贝塞尔曲线 (Bezier curve) ，又称贝兹曲线或贝济埃曲线，是应用于二维图形应用程序的数学曲线。一般的矢量图形软件通过它来精确画出曲线，贝赛尔曲线由线段与节点组成，节点是可拖动的支点，线段像可伸缩的皮筋。贝塞尔曲线主要由起始点、终止点 （也称描点）、控制点构成，通过调整控制点，贝塞尔曲线的形状会发生变化，如图10所示。

图10  贝塞尔曲线示意图

其中点P₀为起点，点P₁为控制点，点P₂为终止点。当控制点前后上下移动时，整个曲线的形状会随着发生变化。因此当脚掌与台阶表面可能产生干涉时，可以手动调整P₁点的位置，使得摆动腿的运动轨迹后移或者上抬，避开台阶表面。本文使用二阶贝塞尔曲线来生成摆动腿轨迹，公式如下：

式10

其中t为贝塞尔曲线的从0-1的描述参数， B(t) 为贝塞尔曲线上个点的位置，其为二维列向量， P0至为起点、控制点和终止点的位置坐标，也是二维列向量。

经实验验证发现，规划方式约接近静平衡，则机器人行走越稳定。但运动学限制则会越大，导致能跨过的台阶高度降低，机器人屈腿的幅度变大。同时单步周期需要调大， 且加大双足支撑期的时间占比，否则会容易失稳摔倒。而规划方式越动态，则机器人受到的运动学限制越小，行走越流畅，但稳定程度会下降。更容易受到电压波动、地面不平整等偶然因素影响而摔倒。

2.斜坡地形轨迹规划 

对于斜地形的轨迹规划，基于线性倒立摆模型有两种可行的规划方式。一种是类似与台阶地形规划，考虑当行走步幅固定时，每两个落脚点之间的高度差是固定的，此时可以把斜坡当成台阶地形处理，但是需要把脚掌运动轨迹的Pitch角设为倾斜的，以便和斜坡保持良好接触，如图11 ，其中空白圆为鹘关节、膝关节和踝关节，实心圆点为脚掌结构示意。

图11  斜坡地形行走示意图

另一种是类似与平面地形。直接按照平面地形规划躯干及双腿的运动轨迹，然后对Z轴方向，再叠加坡度和斜坡一致的倾斜直线运动轨迹。这样处理躯干轨迹和足端轨迹之后，再把脚掌运动轨迹的Pitch角设为倾斜，倾斜幅度和斜坡斜度一致。设斜坡坡度为，则对躯干和双腿，可以使用同一个公式实时计算需要Z轴方向需要叠加的高度。式11中H_l为某个时刻的局部坐标系下的额外叠加高度，X_l为某个时刻的按照平面步态规划的躯干或足端在局部坐标系下的X方向坐标值。

式11

经实验该种方法可以稳定地完成下坡行走，但在上坡行走时会有随机的摔倒现象出 现。另外其行走效果较为流畅，步行周期可以接近平面步态的步行周期。

当要求稳定的上坡步态规划时，可以使用第 种方法，类比规划台阶步态，来规划 斜坡步态。由此可以实现稳定的上坡行走。