机器人技术的应用已经从工业生产扩展到航空航天、国防安全、社会服务等领域. 足式机器人作为机器人的重要组成部分，具有非连续地面支撑的运动特点，能够跨越复杂的地形，在军事行动、巡检探测、消防营救、物流配送等场景具有巨大的应用潜力.

为了确保四足机器人能够稳定行走，其质心（center of mass，CoM）的轨迹必须根据稳定性标准生成. 基于稳定性标准生成CoM轨迹，往往需要模型^[1]. 使用全尺寸模型进行在线步态规划需要大量的计算，无法保证系统的实时性. 模型近似化是处理此问题的有效方法. 许多研究^[2-3]已经证明在双足机器人运行速度不是很高的情况下可以将其抽象成线性倒立摆模型. Kajita等^[4]提出基于零矩点（zero moment point，ZMP）的预观控制方法，根据预定义的ZMP轨迹生成CoM轨迹. 这种方法使用线性二次调节器（linear quadratic regulator，LQR）进行步态规划，通过构造代价函数来权衡ZMP跟踪与CoM加速度之间的关系，但是此方法不能考虑系统输入的约束条件. Wieber^[5]对预观控制的方法进行改进，提出使用模型预测控制（model predictive control，MPC）的步态生成方法. MPC方法通过考虑ZMP的约束和机器人当前的状态来重新计算CoM轨迹，从而实现对双足机器人HRP-2的平衡控制. Feng等^[6]使用非线性模型，运用微分动态规划（differential dynamic programming，DDP）来在线计算CoM轨迹. 在以上方法中，CoM的位置和速度被视为系统的状态量，通过LIPM的状态方程以及系统当前的状态和输入约束，获得CoM的轨迹. 尽管此方法能够进行步态规划，但通过限制位置和速度，LIPM动力学的不稳定分量和稳定分量都受到约束. Kamioka等^[7]将状态量位置和速度进行坐标变换，将其解耦为不稳定分量与稳定分量，并定义不稳定分量为运动发散分量（divergent component of motion，DCM），Hof^[8]称其为XCoM，Pratt等^[9]也提出了这一概念，并命名为Capture point（CP）. Lanari 等^[10]指出只要确保DCM有界，就可以保证系统的输出有界.

目前有2种方法保证其有界，一种是改变后续的系统输入，另外一种是改变当前DCM的期望值. Wang等^[11]通过优化当前一步的剩余步行周期时长使DCM达到目标值，进而使机器人具有更加鲁棒性的步态. Khadiv等^[12]提出将步行周期时长和落脚点一起优化可以使双足机器人在较大的扰动中恢复平衡. Englsberger等^[13]提出逆向递推方法来求解CP的期望轨迹，通过改变当前一步的ZMP位置来促使系统的CP跟踪其期望轨迹运动. 此方法由于有固定脚印约束，而且必须保证ZMP在支撑多边形内，其只能承受较小的扰动.

本研究将四足机器人简化成线性倒立摆模型，应用模型预测控制，综合考虑系统的控制约束，通过宽松初始状态约束以及调整落脚点的位置，使四足机器人在存在较大的DCM误差中保持稳定，并且能够尽快跟踪其期望轨迹运动.

为了将四足机器人简化为线性倒立摆模型，作如下假设：1）四足机器人的2条摆动腿同时运动，且摆动腿足端相对于躯干有相同的步幅；2）四足机器人的重量集中在躯干上，腿部质量相对于躯干质量较小；3）四足机器人在z方向上高度不变.

如图1所示，将四足机器人抽象成一个线性倒立摆模型后，机器人的运动就可以等效成质心的运动与支点P（脚印）的运动. 图中，F₁~F₄为足端所受到的地面反作用力矢量；p₁~p₄为足底坐标系在世界坐标系的位置向量；τ为足底力在质心处的合成力矩； z₀为机器人质心处的高度. 该系统的动力学可以表述为

(1) ${{\ddot{ r}}}={\omega ^2}{{(r - p)}}.$

式中： $\omega $为LIPM的固有频率， $\omega = \sqrt {{g / {{z_0}}}} $， ${z_0}$为CoM高度； ${{r}}$为CoM水平位置向量， ${{r = }}{\left[ \!\!\!{\begin{array}{*{20}{c}} {{r_{{x}}}},&{{r_{{y}}}} \end{array}} \!\!\!\right]^{\rm{T}}}$； ${{p}}$为ZMP位置向量， ${{p = }}{\left[\!\!\! {\begin{array}{*{20}{c}} {{p_{{x}}}},&{{p_{{y}}}} \end{array}}\!\!\! \right]^{\rm{T}}}$.

将此二阶系统动力学方程写成状态空间的形式：

(2) $\left[ {\begin{array}{*{20}{c}} {{\dot{ r}}} \\ {{\ddot{ r}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{0}}&{{I}} \\ {{\omega ^2{{I}}}}&{{0}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{r}} \\ {{\dot{ r}}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{0}} \\ { - {\omega ^2{{I}}}} \end{array}} \right]{{p}}.$

式中：0、I为2 × 2矩阵。

其状态矩阵的特征值一正一负，由李雅普诺夫稳定性判据可知，此系统不稳定. 将状态量位置和速度进行坐标变换：

(3) $\left[ {\begin{array}{*{20}{c}} {{r}} \\ {{\xi }} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{I}}&{{0}} \\ {{I}}&{{\dfrac{1}{\omega }{{I}} }} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{r}} \\ {{\dot{ r}}} \end{array}} \right].$

定义不稳定分量为运动发散分量 ${{\xi}} $（DCM），经过坐标变换后系统的动力学方程为

(4) ${\dot{ \xi } }= \omega {{(\xi }} - {{p)}},$

(5) ${\dot{ r}} = - \omega {{(r}} - {{\xi )}}.$

式（5）是内在稳定的一阶开环动力学系统，式（4）是一阶不稳定的开环动力学系统. 因此只要确保DCM有界，就可以保证LIPM系统的输出有界. 系统的状态方程可以写成

(6) $\left[ {\begin{array}{*{20}{c}} {{\dot{ r}}} \\ {{\dot{ \xi }}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} { - \omega{{I}} }&\omega {{I}} \\ {{0}}&\omega {{I}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{r}} \\ {{\xi }} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{0}} \\ { - \omega {{I}}} \end{array}} \right]{{p}}.$

通过宽松初始状态约束以及调整落脚点的位置来保证LIPM系统的输出有界. 根据期望的状态轨迹和系统的状态方程，综合考虑控制系统的控制约束、宽松初始状态约束，利用二次规划可以在较短的预测时域内在线求解出一组最优的落脚点序列. 选择控制序列的第2个元素作为下一步落脚点.

(7) $\begin{array}{*{20}{c}} {{\rm{min}}}\;&{\displaystyle\sum\limits_{{{k}} = 0}^N {\left( {{{Q}}{{\left\| {{{{\delta }}_{{k}}} - {{\delta }}_{{k}}^{{*}}} \right\|}^2} + {{R}}{{\left\| {{{{p}}_{{k}}} - {{p}}_{{k}}^{{*}}} \right\|}^2} + {{V}}{{\left\| {{\eta }} \right\|}^2}} \right)} } \end{array}.$

约束方程如下：

(8) $\begin{array}{*{20}{c}} {{{{\delta }}_{{{k + 1}}}} = {{A}}{{{\delta }}_{{k}}} + {{B}}{{{p}}_{{k}}}},&{{{k}} = 0,1, \cdots ,N}, \end{array}$

(9) ${{{p}}_{\rm{0}}} = {{{p}}_{{\rm{cur}}}},$

(10) ${{{\xi }}_0} = {{{\xi }}_{{\rm{cur}}}} - {{\eta }},$

(11) ${{\eta }} \in \left[\!\!\! {\begin{array}{*{20}{c}} {{{{\eta }}_{{\rm{min}}}}},&{{{{\eta }}_{{\rm{max}}}}} \end{array}} \!\!\!\right],$

(12) $\begin{array}{*{20}{c}} {\left| {\left| {{{{r}}_{{{k + 1}}}} - {{{p}}_{{k}}}} \right|} \right| \leqslant \lambda },&{{{k}} = 0,1, \cdots ,N - 1}, \end{array}$

(13) $\begin{array}{*{20}{c}} {\left| {\left| {{{{r}}_{{k}}} - {{{p}}_{{k}}}} \right|} \right| \leqslant \lambda },&{{{k}} = 1,2, \cdots ,N}. \end{array}$

式中： ${{{\delta }}_{{k}}}$、 ${{{p}}_{{k}}}$、 ${{\delta }}_{{k}}^*$、 ${{p}}_{{k}}^*$分别为时间步k的系统状态量和控制输入，期望的系统状态和控制输入， ${{{\delta }}_{{k}}} = {\left[\!\!\! {\begin{array}{*{20}{c}} {{{{r}}_{{k}}^{\rm{T}}}},&{{{{\xi }}_{{k}}^{\rm{T}}}} \end{array}} \!\!\!\right]^{\rm{T}}}$； ${{Q}}$、 ${{R}}$为半正定对角线权重矩阵；V为对角线权重矩阵； ${{A}}$为离散时间系统的状态矩阵； ${{B}}$为离散时间系统的输入矩阵；N为预测步长； ${{{p}}_{\rm{0}}} $为当前系统的输入；p_cur为当前的脚印位置信息； ${{{\xi }}_0} $为系统的初始运动发散分量； ${{{\xi }}_{\rm{cur}}} $为系统的当前运动发散分量；λ为步幅约束；式（9）、（10）为初始状态约束； ${{\eta }}$为宽松初始状态约束（经典模型预测控制 ${{\eta}}={{0}} $）；式（12）、（13）为步幅限制不等式约束. 宽松初始状态模型预测控制在预测时域内遵循约束的同时试图找到一系列控制输入引导系统沿着参照轨迹运动. 这组控制输入可以使系统未来的轨迹偏离参照轨迹的程度最小.

式（6）中DCM的解析解为

(14) ${{\xi}} (t,p) = {{\rm{e}}^{\omega \left( {t - {t_0}} \right)}}{{\xi}} \left( {{t_0}} \right) - \omega \int_{{t_0}}^t {{{\rm{e}}^{\omega (t - \tau )}}} {{p}}(\tau ){\rm{d}}\tau .$

式中： ${{\xi}} \left( {{t_0}} \right)$为DCM在 ${t_0}$时刻的初值. 假设ZMP点与参考脚印重合，每一步的p为常数，式（6）的解析解可以表示为

(15) $\begin{split}{{\delta }}(t) =& \left[ {\begin{array}{*{20}{c}} \left( {{1 - {1 / {{{\rm{e}}^{\omega t}}}}}} \right){{I}}\;\;\left[ {\left( {{\rm{e}}^{2\omega t}} - {{1}} \right) / \left( {{2{{\rm{e}}^{\omega t}}}} \right) } \right]{{I}}\\ {{0}}\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{{{\rm{e}}^{\omega t}}{{I}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{{r}}_{\rm{0}}}} \\ {{{{\xi }}_{\rm{0}}}} \end{array}} \right] {\rm{ + }}\\ &\left[ {\begin{array}{*{20}{c}} \left[ {{{{ - {{\left( {{{\rm{e}}^{\omega t}} - 1} \right)}^2}} /\left( { {2{{\rm{e}}^{\omega t}}}} \right)}} } \right]{{I}}\\ \left( {{1 - {{\rm{e}}^{\omega t}}} } \right){{I}} \end{array}} \right]{{p}}.\end{split}\!\!\!\!\!\!\!\!\!\!\!\!$

定义采样时间为T, 其离散系统的状态方程可以表示为

(16) ${{\delta }}(k + 1) = {{A\delta }}(k) + {{B}}{{p}}(k),$

(17) ${{A}} = \left[\!\!\! {\begin{array}{*{20}{c}} \left( {{1 - {1 / b}}} \right){{I}}&\left( {{{b^2} - {1 / {2b}}}} \right){{I}} \\ {{0}}&b{{I}} \end{array}} \!\!\!\right],$

(18) ${{B}} = \left[ \!\!\!{\begin{array}{*{20}{c}} \left[ { {{{ - {{(b - 1)}^2}} / {2b}}} } \right]{{I}}\\ \left( { {1 - b} } \right){{I}} \end{array}} \!\!\!\right].$

式中： $b = {{\rm{e}}^{\omega T}}$. 由离散状态方程以及初始状态可以得到系统第N步的状态值 ${{\delta }}(k + N)$.

模型预测控制须参考轨迹. 参考轨迹依照规划的脚印生成. 如图2所示为 ${{\delta }}_{{k}}^*$和 ${{p}}_{{k}}^*$参照轨迹. 可以看出ZMP点与参考脚印重合. 为了保证DCM有界，命令最后一步的ZMP就是DCM的终止值. 根据DCM的终止值和参照脚印信息后项递推DCM的初值^[13], 其递推公式为

(19) ${{{\xi}} _{i - 1}} = \left( {{{{\xi}} _i} - {{{p}}_{i - 1}}} \right){{\rm{e}}^{ - \omega T}} + {{{p}}_{i - 1}};\quad i = 1,2, \cdots ,N - 1.$

根据规划的初始质心初始状态，由式（15）前向递推出质心的DCM以及CoM轨迹. 参照轨迹在每一次脚落地后随着目标参照脚印的改变进行重新递推.

为了保证四足机器人能够稳定运动，ZMP须在支撑多边形的内部^[14]. 可以通过公式约束ZMP：

(20) ${{{n}}^{\rm{T}}}{{{p}}^{\rm{d}}} + {{m}} \geqslant {{0}}.$

式中： ${{{p}}^{\rm{d}}} $为期望的ZMP位置向量，由式（21）计算得到； ${{n}}$为支撑多边形边界的法向量，由外部指向内部^[15]， ${{n}}$、 ${{m}}$来确定支撑多边形的边界.

期望ZMP位置可以表示为

(21) ${{{p}}^{\rm{d}}} = {{{p}}^{\rm{*}}} + \left( {1 + {{{k_{{\xi}} }}}/{\omega }} \right){{\eta }}.$

式中： ${k_{{\xi}} }$为DCM跟踪比例系数.

如图3所示为四足机器人ZMP边界范围. 图中，红色实线与点划线分别为2条腿和4条腿着地时支撑多边形的范围. 机器人当前的DCM处于 ${{{\xi }}_{{\rm{cur}}}}$，当前一步的ZMP位置处在点A，这一步结束后期望的DCM位置处于 ${{\xi }}_1^{\rm{*}}$，在线步态规划优化出的期望DCM位置处于 ${{{\xi }}_0}$. 改变当前一步的ZMP位置（不可以超过支撑多边形的边界）到点A₁来促使机器人的DCM跟踪期望的轨迹.

在线步态规划提供一条可执行的期望状态轨迹. 全身运动控制通过控制各个关节产生的力矩来跟踪期望轨迹运动，并且保持系统稳定.

浮动基座四足机器人的动力学模型可以描述为

(22) $\left[ {\begin{array}{*{20}{c}} {{{{H}}_{\rm{f}}}} \\ {{{{H}}_{\rm{a}}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{{\ddot{ \theta }}}_{\rm{b}}}} \\ {{\ddot{ q}}} \end{array}} \right]+\left[ {\begin{array}{*{20}{c}} {{{{C}}_{\rm{f}}}} \\ {{{{C}}_{\rm{a}}}} \end{array}} \right]+\left[ {\begin{array}{*{20}{c}} {{{{G}}_{\rm{f}}}} \\ {{{{G}}_{\rm{a}}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{{J}}{{_{{\rm{st}}}^{{\rm{body}}}}^{\rm{T}}}{{f}}} \\ {{{\tau + J}}{{_{{\rm{st}}}^{{\rm{leg}}}}^{\rm{T}}}{{f}}} \end{array}} \right].$

式中： ${{{H}}_{\rm{f}}}$、 ${{{C}}_{\rm{f}}}$、 ${{{G}}_{\rm{f}}}$分别为浮动基座的惯性矩阵、科氏力矩阵与离心力矩阵、重力项， ${{{H}}_{\rm{f}}} \in {\mathbb{R}^{6 \times 18}}$， ${{{C}}_{\rm{f}}} \in {\mathbb{R}^6}$， ${{{G}}_{\rm{f}}} \in {\mathbb{R}^6}$； ${{{H}}_{\rm{a}}}$、 ${{{C}}_{\rm{a}}}$、 ${{{G}}_{\rm{a}}}$、 ${{\tau }}$分别为驱动关节的惯性矩阵、科里力与离心力矩阵、重力项和驱动关节力矩， ${{{H}}_{\rm{a}}} \in {\mathbb{R}^{12 \times 18}}$， ${{{C}}_{\rm{a}}} \in {\mathbb{R}^{12}}$， ${{{G}}_{\rm{a}}} \in {\mathbb{R}^{12}}$， ${{\tau }} \in {\mathbb{R}^{12}}$； $ {{{\ddot{ \theta }}}_{\rm{b}}} $为优化变量中机器人质心处的加速度； ${{J}}_{{\rm{st}}}^{{\rm{body}}}$、 ${{J}}_{{\rm{st}}}^{{\rm{leg}}}$分别为躯干和支撑腿的雅克比矩阵， ${{J}}_{{\rm{st}}}^{{\rm{body}}} \in {\mathbb{R}^{3k \times 6}}$， ${{J}}_{{\rm{st}}}^{{\rm{leg}}} \in {\mathbb{R}^{3k \times 12}}$， $k$为四足机器人支撑腿的数量； ${{f}}$为支撑腿在地面的反作用力.

由浮动基座四足机器人的动力学方程可知，四足机器人的躯干的加速度由地面的反作用力来控制，而足端在地面的反作用力大小由各个关节的关节力矩来决定. 1）当四足机器人有1条支撑腿时( $k \leqslant 1$)，rank （ ${{J}}_{{\rm{st}}}^{{\rm{body}}}$） $ \leqslant 3$，方程无解，此时四足机器人躯干的6个自由度不能全部控制；2）当四足机器人有2条支撑腿时( $k{\rm{ = 2}}$)，rank （ ${{J}}_{{\rm{st}}}^{{\rm{body}}}$） $ = 5$，此时四足机器人躯干有5个自由度能够控制；3）当四足机器人有3条以上足端不共线的支撑腿时( $k \geqslant 3$)，rank （ ${{J}}_{{\rm{st}}}^{{\rm{body}}}$） $ = 6$，此时四足机器人躯干的6个自由度都能够控制，且方程有无数个解.

根据四足机器人动力学模型，须对逆向动力学进行求解. 正交分解法的本质是剔除系统中的等式约束后，运用伪逆矩阵求得达到期望加速度最优的力矩^[16]. 但是这种方法不能考虑不等式约束. 本研究采用二次规划法来求解四足机器人的逆向动力学方程.

采用二次规划来计算在运动约束、动力学约束、摩擦力约束等一系列约束条件下，达到期望加速度所需要的地面反作用力. 二次规划的优化变量 ${{\chi }} = {\left[ \!\!\!{\begin{array}{*{20}{c}} {{{{\ddot{ \theta }}}_{\rm{b}}}},&{{\ddot{ q}}},&{{f}} \end{array}} \!\!\!\right]^{\rm{T}}}$包括机器人质心处的加速度、各个驱动关节的加速度以及足端处地面的反作用力.

(23) ${\rm{min}}\;\;{{S}}{\left\| {{{{\ddot{ \theta }}}_{\rm{b}}}{{ - \ddot \theta }}_{\rm{b}}^{{*}}} \right\|^2}{{ + W}}{\left\| {{{{J}}_{{\rm{sw}}}}{\ddot{ \theta }} + {{{\dot{ J}}}_{{\rm{sw}}}}{\dot{ \theta }} - {{{\ddot{ p}}}_{{\rm{sw}}}}} \right\|^2}+{\left\| {{f}} \right\|^2}.$

约束方程如下：

(24) $\begin{array}{*{20}{c}} {\left[ \!\!\!{\begin{array}{*{20}{c}} {{{{H}}_{\rm{f}}}},&{{{J}}{{_{{\rm{st}}}^{{\rm{body}}}}^{\rm{T}}}} \end{array}} \!\!\!\right]{{\chi }} = - {{{C}}_{\rm{f}}} - {{{G}}_{\rm{f}}}} , \end{array}$

(25) ${{{J}}_{{\rm{st}}}}{\ddot{ \theta }} + {{\dot{ J}}_{{\rm{st}}}}{\dot{ \theta }} = - {{\alpha }}{{\dot{ J}}_{{\rm{st}}}}{\dot{ \theta }},$

(26) $f_i^z \geqslant 0,\;\left| {\mu f_i^z} \right| \geqslant f_i^x,\;\left| {\mu f_i^z} \right| \geqslant f_i^y,$

(27) ${{{\tau }}_{{\rm{min}}}} - {{{C}}_{\rm{a}}} - {{{G}}_{\rm{a}}} \leqslant \left[\!\!\! {\begin{array}{*{20}{c}} {{{{H}}_{\rm{a}}}},&{{{J}}{{_{{\rm{st}}}^{{\rm{leg}}}}^{\rm{T}}}} \end{array}} \!\!\!\right]{{\chi }} \leqslant {{{\tau }}_{{\rm{max}}}} - {{{C}}_{\rm{a}}} - {{{G}}_{\rm{a}}}.$

式中： $ {{S}}$、 ${{W}} $为权重矩阵， ${{ \ddot \theta }}_{\rm{b}}^{{*}} $为期望的机器人质心加速度， $ {{{J}}_{{\rm{sw}}}} $为摆动腿的雅克比矩阵，J_st为支撑腿的雅克比矩阵， $ {{{\ddot{ p}}}_{{\rm{sw}}}} $为前馈-反馈控制计算得到的摆动腿足端加速度， $f_i^x $， $f_i^y $， $f_i^z $分别为第i条腿足底力在 x、y、z轴的分量；μ为摩擦系数.

优化代价函数是让机器人质心处的加速度与期望的摆动腿足端加速度在满足约束的条件下无限接近，在满足无限接近的情况下地面的反作用力最小.

(28) $\begin{split} {{\ddot {{p}}}_{{\rm{sw}}}} =& {{\ddot {{p}}}_{{\rm{sw,d}}}} + {{{k}}_{\rm{p}}}\left( {{{{p}}_{{\rm{sw,d}}}} - {{{p}}_{{\rm{sw}}}}} \right) +\\ & {{{k}}_{\rm{i}}}\int {\left( {{{{p}}_{{\rm{sw,d}}}} - {{{p}}_{{\rm{sw}}}}} \right){\rm{d}}t} + {{{k}}_{\rm{d}}}\left( {{{\dot {{p}}}_{{\rm{sw,d}}}} - {{\dot {{p}}}_{{\rm{sw}}}}} \right). \!\!\!\!\! \end{split} $

式中： ${{\ddot {{p}}}_{{\rm{sw,d}}}}$为期望的摆动腿足端加速度前馈向量， ${{k}}_{\rm{p}}{\text{、}}{{k}}_{\rm{i}}{\text{、}} {{k}}_{\rm{d}}$为pid系数矩阵.

为了使摆动腿跟踪步态规划的期望运动，控制器在笛卡尔空间内通过前馈-反馈控制来约束摆动腿各个驱动关节的加速度. 式（24）表示机器人质心加速度与地面反作用力的关系. 为了保证支撑腿与地面不发生相对滑动，通过式（25）来约束支撑腿足端的加速度始终与足端的速度成反比. 地面反作用力必须作用在接触点的法向量的摩擦锥内^[17]. 式（26）将摩擦力约束写成线性的形式，此时摩擦锥近似为摩擦金字塔形. 基于此线性化可以保证优化始终为二次规划问题. 式（27）约束质心加速度与地面反作用力，防止逆向动力学使用优化变量计算出来的关节力矩超出机器人关节的输出极限力矩.

如图4所示为本研究的运动控制框架，由上层步态发生器与下层全身运动控制两部分组成. 步态发生器根据参考脚印和机器人当前的状态，在每次足端着地后在线优化出一条可执行的期望状态轨迹、下一步落脚点的位置以及摆动腿的运动轨迹. 全身运动控制计算关节力矩τ_d. 全身运动控制中的DCM跟踪器通过式（21）计算出促使DCM跟踪其参照轨迹 ${{{\xi }}_{\rm{ref}}} $运动的期望ZMP的位置p^d. ZMP控制器^[12]计算出达到期望ZMP位置所需要的质心在水平面的加速度 ${{\ddot {{r}}}_{{{xy,{\rm{ref}}}}}}$. 躯干其余4个自由度的加速度均由PD反馈控制器得到. 二次规划优化出满足约束条件下，可以达到的质心加速度与关节加速度 ${{\ddot{ \theta }}^{\rm{*}}} = {\left[ \!\!\!{\begin{array}{*{20}{c}} {{{{\ddot{ \theta }}}_{\rm b}}},&{{\ddot{ q}}} \end{array}}\!\!\! \right]^{\rm{T}}}$以及期望的地面的反作用力 ${{{f}}_{\rm{d}}}$. 将优化出的变量代入动力学方程式（22）中，计算出达到期望运动所需要的关节力矩τ_st. 为了让摆动腿更好地跟踪期望轨迹，摆动腿的各个关节力矩τ_sw采用前馈反馈综合控制. 将逆向动力学计算出来的力矩τ_sw,ff作为前馈项，反馈项τ_sw,fb由关节位置PD反馈控制器计算得到.

在利用动力学仿真验证本研究所提出的运动控制框架之前，通过MATLAB仿真来验证宽松初始状态模型预测控制与经典模型预测控制的区别. 2组实验的初始ZMP位置相同， ${{{p}}_{{\rm{cur}}}} = {\left[ \!\!\!{\begin{array}{*{20}{c}} 0,&0 \end{array}} \!\!\!\right]^{\rm{T}}}$，步长限制相同. 机器人期望从位置（0，0）以trotting步态行走至位置（0.3，0），步幅为0.1 m，步态周期为0.25 s.

在第1组对照实验中 ${{{\delta }}_{{\rm{cur}}}} \!=\! {\left[\!\!\!\!\!\!\!\! {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {0.05},\!\!\!&\!\!\!0, \end{array}}\!\!\!{\begin{array}{*{20}{c}} {0.05},\!\!\!&\!\!\!0 \end{array}} \end{array}}\!\!\!\!\!\!\!\! \right]^{\rm{T}}}$，结果如图5（a）所示. 图中，CoM_ref、CoM_opt分别为参考CoM轨迹和优化出的期望CoM轨迹，ZMP_ref、ZMP_opt分别为参考ZMP位置和优化出的期望ZMP位置. 经典模型预测控制与宽松初始状态模型预测控制优化出来的质心轨迹均收敛离线规划的质心轨迹上. 但宽松初始状态模型预测控制收敛的速度更快一些. 在第2组对照实验中， ${{{\delta }}_{{\rm{cur}}}}\! =\! \! {\left[ \!\!\!\!\!\!\!\!\!{\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {0.10},\!\!\!\!&\!\!\!\!0 ,\end{array}}\!\!\!\!{\begin{array}{*{20}{c}} {0.10},\!\!\!\!&\!\!\!\!0 \end{array}} \end{array}}\!\!\!\!\!\!\!\!\! \right]^{\rm{T}}}$，结果如图5（b）所示. 通过对比试验的结果可以看出，在相同的预测时域内，宽松初始状态预观控制器在2步以内就可以使轨迹到达参照轨迹，而经典模型预测控制优化出来的轨迹并没有收敛到参照轨迹上. 对比实验表明，与经典模型预测控制相比，宽松初始状态模型预测控制可以承受较大的质心轨迹跟踪误差.

为了验证所提出的运动规划与控制算法的有效性，进行半物理仿真实验. 如图6所示，建立与物理样机动力学参数相同的四足机器人仿真模型. 机器人质量为24 kg，每条腿有3个自由度，其中髋关节具有一个外摆自由度，大腿和小腿具有前摆自由度.

在仿真过程中，机器人期望从位置（0.55，0）以trotting步态行走至位置（2.50，0），步幅为0.15 m，步态周期为0.25 s，其中四足支撑期占步态周期的1/3. 步态时序图如图7所示.

MATLAB作为控制器负责在线步态规划与全身运动控制. 机器人的仿真参数设置如表1所示. 机器人的初始状态为在原点处静止，初始落脚点为（0, 0）. 在线步态规划随着期望脚印的更新根据机器人当前的状态每一步（0.25 s）进行一次重新规划，全身运动控制在每个控制周期内（5 ms）求解出跟随期望轨迹运动的关节力矩. 当机器人4条腿全部着地时，其躯干6个自由度均可控，其权重矩阵S = diag [70，10，100，80，80，80]；当机器人只有2条腿着地时，机器人的躯干处于欠驱动状态，其躯干只有5个自由度可控. 张国腾等^[18]指出，四足动物希望到达其侧方的某个位置时，一般会旋转躯干使自己朝向目标前进，很少采用直接侧向移动的方式. 因此放弃对其y方向的控制，此时权重矩阵为 ${{S}} = {\rm{diag}}\;[70,\;0,\;100,\;80,\;80,\;80]$.

如图8所示为四足机器人直线行走的仿真结果. 图8（a）中，ZMP_off为离线规划的脚印，ZMP_opt为优化出的ZMP位置，ZMP_des、ZMP_act分别为每个控制周期期望的ZMP位置与实际的ZMP位置. 由图8（a）可知，在满足步幅约束的条件下，在线步态规划优化出的ZMP位置可以尽快收敛到离线规划的ZMP位置. 图8（b）中，DCM_off为根据图8（a）中的ZMP_off递推出的参考DCM轨迹，DCM_opt为满足步幅约束与ZMP范围约束条件下，优化出来的期望DCM轨迹，DCM_act为机器人实际的DCM轨迹. 由图8（b）可知，机器人实际的DCM没有发散，最终收敛到最后一步的ZMP位置. 图8（c）中，CoM_act为机器人实际的CoM轨迹. 期望CoM轨迹与实际CoM轨迹几乎重合.

仿真结果表明，在线步态规划在满足约束的条件下，可以使四足机器人在存在较大的跟踪误差时收敛到离线规划的轨迹上, 全身运动控制使其具有较好的轨迹跟踪特性，四足机器人可以稳定地进行troting步态运动.

将四足机器人抽象成线性倒立摆模型，基于DCM的方法论提出在线步态规划方法，可以处理较大的轨迹跟踪误差. 在线步态规划运用宽松初始状态模型预测控制，在满足约束的条件下，在线优化出可快速收敛到离线轨迹的落脚点以及CoM轨迹. 经典的模型预测控制在相同的预测时域内无法收敛到离线规划的轨迹上. 采用四足机器人全身运动控制器来跟踪宽松初始状态模型预测控制优化出来的轨迹，优化出来的轨迹与实际轨迹几乎重合。仿真实验结果表明：与经典模型预测控制相比，宽松初始状态模型预测控制可以承受较大的轨迹跟踪误差；本研究所提出的规划器与控制器可以使四足机器人在存在较大的轨迹跟踪误差时稳定运动。目前所提方法在仿真中获得了较好的效果，但在物理样机实验时受到了诸多因素的影响。所提方法须对四足机器人有较好的状态估计.目前采用逆向运动学来进行状态估计，导致物理样机CoM的速度估计有较大的噪声、位置估计有较大的漂移，使得仿真结果难以在物理样机上进行复现.下一步将研究基于扩展卡尔曼滤波的状态估计来获得较为准确的CoM的速度和位置，并在物理样机上进行相关验证实验.