与传统移动机器人相比，仿人机器人以其高度的灵活性和与人的亲和力，在抢险救灾、野外探测、家庭服务等领域有着广阔的应用前景. 近年来，国内外在仿人机器人样机研制^[1]、步态规划^[2-3]和步行控制^[4]等方面取得了一定的研究成果. 为了满足实际应用的需求，提高真实环境中机器人的步行稳定性需要深入研究.

在仿人机器人-地面-环境系统中存在各种非理想因素，如建模误差和外力扰动. 从步行策略上看，人类可以通过实时调整落脚点以保持稳定^[5]. Diedam等^[6]考虑机器人质心（center of mass，CoM）状态变化，利用线性模型预测控制（model predictive control，MPC）策略实现落脚点调整. Fu^[7]通过外力冲击估计与可捕捉区域检验，实时计算机器人落脚点. 张继文等^[8]通过感知躯干倾斜角度，利用比例-微分控制器调整机器人落脚点. Zhang等^[9]通过分析摆动腿动力学特性，提出能耗最优控制策略. Yu等^[10]通过检测质心速度和加速度，将测量零力矩点（zero moment point，ZMP）和名义ZMP的误差映射为落脚点调整量. 此外，通过步行状态机切换，Ding等^[11]实现了落脚点序列调整，但是仅调整落脚点对于机器人抗扰稳定性的提升有限.

仿生学研究表明，当面临外力干扰时，人类可以利用躯干转动以保持平衡^[12]. 鉴于此，仿人机器人行走的髋关节运动策略引起了越来越多的关注. Kudoh等^[13]通过采集人类动作，提出躯干转动最优控制策略. Chen等^[14]通过学习人类步行控制策略，实现足部落脚点调整和躯干旋转运动优化. Li等^[15]针对不同的外力形式设计不同的控制策略，利用开环控制方式实现了外力推动下的平衡保持.

实际上，步态优化面临结构尺寸、驱动能力、ZMP支撑范围等方面的约束. 从本质上讲，仿人机器人步态规划是约束优化问题. MPC采用滚动优化机制且能够求解多约束优化问题，在该问题求解上得到了越来越多的应用^[16]. Aftab等^[17]采用飞轮倒立摆模型（inverted pendulum plus flywheel model，IPFM），考虑躯干转动角度范围、足部支撑范围、腿长限制等可行性约束，提出线性MPC策略，实现落脚点和躯干旋转的同步优化. Lack等^[18-19]从旋转角动量优化出发，实现了外力扰动下的稳定行走. 以上方法的目标函数中均只优化躯干旋转角速度或角加速度而没有考虑旋转角度，在外力消失后不能保证躯干恢复竖直状态.

本文提出基于MPC的机器人步态实时生成与优化策略. 考虑足部稳定支撑区域、躯干旋转角度范围和躯干旋转力矩范围等可行性约束，将该问题建模成为二次规划（quadratic programming，QP）问题. 基于CoM状态和躯干倾斜状态，通过在目标函数中引入足部落脚点和躯干摆动角度跟踪惩罚项，实时生成最优的CoM参考轨迹、足部落脚点和躯干旋转角度. 该方法既能够在无外力推动情况下生成变步行参数步态，也能够在有外力推动时调整足部落脚点和躯干旋转角度.

传统的线性倒立摆模型假设：1）机器人质量集中在质心；2）腿部无质量且可伸缩^[20]. 虽然极大地简化了计算，但是不能描述机器人躯干转动对ZMP位置变动的影响. IPFM额外引入以下假设：3）飞轮具有旋转惯量；4）CoM位于飞轮中心^[21]. 如图1所示为双足步行飞轮倒立摆模型，左侧为矢状面前向运动，右侧为冠状面侧向运动， ${\varSigma _{\rm w}}$为全局坐标原点， ${\left[ {d_x,d_y} \right]^{\rm T}}$表示足部落脚点的前向和侧向位置，该模型易于对躯干旋转进行建模. 使用IPFM作为行走动力学模型.

基于该模型，ZMP可以计算如下：

(1) $\left.\begin{aligned} {p_{{x}}} = {c_{{x}}} - \frac{{{c_{{z}}}}}{g}{{\ddot c}_{{x}}} - \frac{{{{\dot L}_{{y}}}}}{{mg}}, \\ {p_{{y}}} = {c_{{y}}} - \frac{{{c_{{z}}}}}{g}{{\ddot c}_{{y}}} + \frac{{{{\dot L}_{{x}}}}}{{mg}}. \end{aligned} \right\}$

式中： ${[{p_{{x}}},{p_{{y}}}]^{\rm T} }$表示前向（沿x轴，向前为正）和侧向（沿y轴，向左为正）的ZMP位置； ${[{c_{{x}}},{c_{{y}}}]^{\rm{T}}}$表示前向和侧向的CoM位置； ${c_{{z}}}$为倒立摆固定高度（即CoM高度）； ${[{\ddot c_{{x}}},{\ddot c_{{y}}}]^{\rm{T}}}$为前向和侧向CoM加速度；m为机器人总质量；g为重力加速度； ${[{\dot L_{{x}}},{\dot L_{{y}}}]^{\rm T} }$为飞轮绕x轴和y轴的旋转角动量的变化量，

(2) ${\dot L_{{x}}} = {I_{{x}}}{\ddot \theta _{\rm{r}}},\begin{array}{*{20}{c}} {}&{} \end{array}{\dot L_{{y}}} = {I_{{y}}}{\ddot \theta _{\rm{p}}}.$

其中， ${[{I_{{x}}},{I_{{y}}}]^{\rm T} }$为绕x轴和y轴的转动惯量，本文设为常量， ${[{\theta _{\rm{r}}},{\theta _{\rm{p}}}]^{\rm T} }$为躯干横滚角和俯仰角.

如图2所示， $p_{{x}}^{\min }$、 $p_{{x}}^{{\rm{max}}} $、 $ p_{{y}}^{\min }$、 $p_{{y}}^{\max } $为单足支撑期间的ZMP运动范围约束边界. 要保证行走稳定性，ZMP应该位于支撑脚形成的几何多边形以内^[22]. 特别地，在单足支撑期间，支撑多边形由支撑脚掌的几何形状决定.

将步态规划问题转化为：给定落脚点参考位置和躯干转动参考角度，实时优化CoM轨迹、足部落脚点和躯干旋转角度，以保证ZMP位于足部支撑多边形以内.

基于IPFM模型和MPC理论^[23]，以CoM前向、侧向位置和躯干横滚角和俯仰角的3阶导数为控制输入，假设在每个采样时间间隔dt内的控制输入保持恒定，则第k个采样时刻，系统状态方程为

(3) ${{\hat{ x}}_{(k + 1)}} = {{A}}{{\hat{ x}}_{(k)}} + B{\dddot { x}_{(k)}}.$

式中： ${{\hat{ x}}_{(k)}} = {[{x_{(k)}},{\dot x_{(k)}},{\ddot x_{(k)}}]^{\rm T} }$为当前时刻的系统状态矢量，由各参量的位置、速度和加速度构成； ${\dddot x_{(k)}}$为当前时刻的系统控制输入，是各参量位置的三阶导数（ $x \in \left\{ {{c_{{x}}},{c_{{y}}},{\theta _{\rm{r}}},{\theta _{\rm{p}}}} \right\}$）；常系数矩阵A和B为

(4) ${{A}} = \left[ {\begin{array}{*{20}{c}} 1&{{\rm d}t}&{{\rm d}{t^2}/2} \\ 0&1&{{\rm d}t} \\ 0&0&1 \end{array}} \right],\quad{{B}} = \left[ {\begin{array}{*{20}{c}} {{\rm d}{t^3}/6} \\ {{\rm d}{t^2}/2} \\ {{\rm d}t} \end{array}} \right].$

利用上述状态模型，给定当前状态，在预测时域（包含未来N_h个采样时长）内各参量的位置、速度、加速度可以由控制输入序列计算：

(5) $\left.\begin{aligned} {{{X}}_{(k)}} = {{{P}}_{{\rm{ps}}}}{{{\hat{ x}}}_{(k)}} + {{{P}}_{{\rm{pu}}}}{{{\dddot{ X}}}_{(k)}}, \\ {{{{\dot X}}}_{(k)}} = {{{P}}_{{\rm{vs}}}}{{{\hat{ x}}}_{(k)}} + {{{P}}_{{\rm{vu}}}}{{{\dddot{ X}}}_{(k)}}, \\ {{{{\ddot X}}}_{(k)}} = {{{P}}_{{\rm{as}}}}{{{\hat{ x}}}_{(k)}} + {{{P}}_{{\rm{au}}}}{{{\dddot{ X}}}_{(k)}}. \end{aligned} \right\}$

式中： ${{{X}}_{(k)}} = {\left[ {{x_{(k + 1)}}, \cdots ,{x_{(k + {N_{\rm{h}}})}}} \right]^{\rm{T}}}$为预测时域内参量位置状态， ${{{\dot X}}_{(k)}} = {\left[ {{{\dot x}_{(k + 1)}}, \cdots ,{{\dot x}_{(k + {N_{\rm{h}}})}}} \right]^{\rm{T}}}$为预测时域内参量速度状态， ${{{\ddot X}}_{(k)}} = {\left[ {{{\ddot x}_{(k + 1)}}, \cdots ,{{\ddot x}_{(k + {N_{\rm{h}}})}}} \right]^{\rm{T}}}$为预测时域内各参量加速度状态， ${{\dddot{ X}}_{(k)}} = {\left[ {{{\dddot x}_{(k + 1)}}, \cdots ,{{\dddot x}_{(k + {N_{\rm{h}}})}}} \right]^{\rm{T}}}$为预测时域内各参量控制输入序列. 参量 ${{X}} \in \left\{ {{{{C}}_{{x}}},{{{C}}_{{y}}},{{{\varTheta }}_{\rm{r}}},{{{\varTheta }}_{\rm{p}}}} \right\}$分别表示预测时域内CoM前向、侧向轨迹、躯干横滚角和俯仰角状态向量，比如第k个采样时刻，预测时域内CoM前向位置状态 ${{{C}}_{{{x(}}k{\rm{)}}}}{\rm{ = }}{\left[ {{c_{{{x}}(k + 1)}}, \cdots ,{c_{{{x}}(k + {N_{\rm{h}}})}}} \right]^{\rm{T}}}$.

式（5）中，各系数矩阵 ${{{P}}_{{\rm{ps}}}}\text{、}{{{P}}_{{\rm{vs}}}}\text{、}{{{P}}_{{\rm{as}}}} \in {{\bf R} ^{{N_{\rm{h}}} \times 3}}$， ${{{P}}_{{\rm{pu}}}}\text{、}{{{P}}_{{\rm{vu}}}}\text{、}{{{P}}_{{\rm{au}}}} \in {{\bf R} ^{{N_{\rm{h}}} \times {N_{\rm{h}}}}}$可以由式（3）、（4）迭代计算. 以P_ps和P_pu为例，

(6) $\left.\begin{aligned}&{{{P}}_{{\rm{ps}}}} = \left[ {\begin{array}{*{20}{c}} {{{{c}}_1}{{A}}} \\ {{{{c}}_1}{{{A}}^2}} \\ \vdots \\ {{{{c}}_1}{{{A}}^{{N_{\rm{h}}}}}} \end{array}} \right],\\&{{{P}}_{{\rm{pu}}}} = \left[ {\begin{array}{*{20}{c}} {{{{c}}_1}{{B}}}&0& \ldots &0 \\ {{{{c}}_1}{{{A}}^1}{{B}}}&{{{{c}}_1}{{B}}}& \ldots &{} \\ \vdots &{\vdots}& {}&{\vdots} \\ {{{{c}}_1}{{{A}}^{{N_{\rm{h}}} - 1}}{{B}}}&{{{{c}}_1}{{{A}}^{{N_{\rm{h}}} - 2}}{{B}}}& \ldots &{{{{c}}_1}{{B}}} \end{array}} \right].\end{aligned}\right\}$

式中： ${{{c}}_1} = \left[ {1,0,0} \right]$为位置状态选择矩阵（由 ${{ x}_k} = {{{c}}_1}{{\hat{ x}}_k}$推导出）.

考虑可行性约束，建立能够调整足部落脚点和躯干倾斜姿态的步态规划约束优化模型. 该问题可以表示成下列QP模型：

(7) $\left.\begin{array}{l} \begin{array}{*{20}{c}} {\mathop {\min }\limits_{{\chi }} }&{}&{f({{\chi }})= {{{\chi }}^{\rm T} }{{G\chi }} + {{{g}}^{\rm T} }{{\chi }};} \end{array} \\ \begin{array}{*{20}{c}} {{\rm s} .{\rm t}.}&{}&{f({{\chi }})\leqslant 0,} \end{array} \\ \begin{array}{*{20}{c}} {}&{}&{}&{f({{\chi }})= {{v}}_j^{\rm T} {{\chi }} + {\sigma _j},} \end{array}j \in \left\{ {1, \cdots ,{N_{\rm{c}}}} \right\}. \\ \end{array} \right\}$

式中： ${{\chi }} \in {{\bf R} ^{{N_{\rm{t}}}}}$为由优化变量组成的未知矢量； ${{G}} \in {{\bf R} ^{{N_{\rm{t}}} \times {N_{\rm{t}}}}}$为目标函数Hessian矩阵； ${{g}}\text{、}{{{v}}_j} \in {{\bf R} ^{{N_{\rm{t}}}}}$、 ${\sigma _j} \in {\bf R} $分别为目标函数的一次项系数矩阵、约束条件的一次项系数矩阵和约束条件常数项；N_t和N_c分别为优化变量数目和约束条件数目.

预先给定参考的足部落脚点位置和步行周期（本文中各周期时长为定值T），为了实现CoM轨迹生成并且能够在线调整足部落脚点和躯干摆动角度，在优化目标中引入CoM跟踪误差惩罚、躯干摆动角度跟踪误差惩罚以及足部落脚点跟踪误差惩罚. 考虑机器人驱动能力和系统控制能耗，引入CoM速度惩罚、躯干摆动角速度惩罚和控制输入惩罚，最终提出如下多目标惩罚函数：

(8) $\begin{split} f =& \sum\limits_{{X}}^{{{{C}}_{{x}}},{{{C}}_{{y}}},{{{\varTheta }}_{\rm{r}}},{{{\varTheta }}_{\rm{p}}}} {\left\{ {\frac{{{\alpha _{{X}}}}}{2}{{\left\| {{{{X}}_{(k)}} - {{X}}_{(k)}^{{\rm{ref}}}} \right\|}^2} + \frac{{{\beta _{{X}}}}}{2}{{\left\| {{{{{\dot X}}}_{(k)}}} \right\|}^2}} \right.}+ \\ &{\left. { \frac{{{\gamma _{{X}}}}}{2}{{\left\| {{{{\dddot{ X}}}_{(k)}}} \right\|}^2}} \right\} + } \sum\limits_{{U}}^{{{{D}}_{{x}}},{{{D}}_{{y}}}} {\left\{ {\frac{{{\delta _{{U}}}}}{2}{{\left\| {{{{U}}_{(k)}} - {{U}}_{(k)}^{{\rm{ref}}}} \right\|}^2}} \right\}} . \\ \end{split} $

式中： ${\alpha _{{X}}}\text{、}{\beta _{{X}}}\text{、}{\gamma _{{X}}}\text{、}{\delta _{{U}}}$分别为位置跟踪惩罚系数、速度惩罚系数、控制输入惩罚系数和足部落脚点跟踪惩罚系数； ${{X}}_{(k)}^{{\rm{ref}}}= \left[x_{(k + 1)}^{{\rm{ref}}},\cdots,x_{(k + {N_{\rm{h}}})}^{{\rm{ref}}}\right]^{{\rm T} }$为预测时域内CoM位置和躯干转动角度参考值； ${{{U}}_{(k)}}{\rm{ =}} $ $\left[{u_{(k,1)}},\cdots,{u_{(k,{N_{\rm{f}}})}}\right]{^{\rm T} }$和 ${{U}}_{(k)}^{{\rm{ref}}}{\rm{ = }}\left[u_{(k,1)}^{{\rm{ref}}},\cdots,u_{(k,{N_{\rm{f}}})}^{{\rm{ref}}}\right]{^{\rm T} }$分别表示预测时域所利用的未来N_f个步行周期（当前支撑周期足部落脚点不再参与优化）的实际落脚点和参考落脚点水平位置（ $u \in {{\rm{[}}{d_{{x}}},{d_{{y}}}{\rm{]}}^{\rm T} }$）； ${{\rm{[}}{{{D}}_{{x}}},{{{D}}_{{y}}}{\rm{]}}^{\rm T} }$表示未来N_f个步行周期前向和侧向足部落脚点位置（对第k个采样时刻，预测时域内有 ${{{D}}_{x(k)}} = $ ${\left[ {{d_{{{x}}(k,1)}}, \cdots ,{d_{{{x}}(k,{N_{\rm{f}}})}}} \right]^{\rm{T}}}$）.（值得注意的是，本文用d_x（k，i）表示预测时域覆盖的未来第i个周期的前向落脚点位置，用d_x（k+i）表示预测时域内第i个采样时刻对应的前向落脚点位置）.

一方面，与文献[24]和[25]中跟踪CoM速度不同，本文关注于位置跟踪，从而保证在外力干扰消失后机器人能够继续跟踪预定足部落脚点. 另一方面，与传统髋关节策略研究关注于躯干旋转角速度、角加速度或者旋转角动量优化不同，通过引入躯干角度跟踪误差惩罚，保证机器人在干扰消失时能够恢复姿态竖直状态.

利用该目标函数，式（7）中优化矢量 ${{\chi }}$为

(9) ${{{\chi }}^k}{\rm{ = }}{\left[ {{{{\dddot{ C}}}_{{{x(}}k{\rm{)}}}},{{{\dddot{ C}}}_{{{y(}}k{\rm{)}}}},{{{\dddot{ \varTheta }}}_{{\rm{r(}}k{\rm{)}}}},{{{\dddot{ \varTheta }}}_{{\rm{p(}}k{\rm{)}}}},{{{D}}_{{{x(}}k{\rm{)}}}},{{{D}}_{{{y(}}k{\rm{)}}}}} \right]^{\rm{T}}}.$

因此，优化变量数目 ${N_{\rm{t}}}{\rm{ = 4}}{N_{\rm{h}}} + 2{N_{\rm{f}}}$.

式（7）中的Hessian矩阵G推导如下：

(10) ${{G}}{\rm{ = diag}}\;\left[ {{{{\varPsi }}_{{{{C}}_{{x}}}}},{{{\varPsi }}_{{{{C}}_{{y}}}}},{{{\varPsi }}_{{{{\varTheta }}_{\rm{r}}}}},{{{\varPsi }}_{{{{\varTheta }}_{\rm{p}}}}},{{{\varPhi }}_{{{{D}}_{{x}}}}},{{{\varPhi }}_{{{{D}}_{{y}}}}}} \right].$

式中：diag函数以各矩阵为对角元素生成对角矩阵； ${{X}} \in \left\{ {{{{C}}_{{x}}},{{{C}}_{{y}}},{{{\varTheta }}_{\rm{r}}},{{{\varTheta }}_{\rm{p}}}} \right\}$、 ${{U}} \in \left\{ {{{{D}}_{{x}}},{{{D}}_{{y}}}} \right\}$，式（10）中各对角元素矩阵为

(11) $\left.\begin{aligned} {{{\varPsi }}_{{X}}} =& \frac{{{\alpha _{{X}}}}}{2}{{P}}_{{\rm{pu}}}^{\rm{T}}{{{P}}_{{\rm{pu}}}} + \frac{{{\beta _{{X}}}}}{2}{{P}}_{{\rm{vu}}}^{\rm{T}}{{{P}}_{{\rm{vu}}}} + \frac{{{\gamma _{{X}}}}}{2}{{{I}}_{{N_{\rm{h}}} \times {N_{\rm{h}}}}}, \\ {{{\varPhi }}_{{U}}} =& {\frac{{{\delta _{{U}}}}}{2}{{{I}}_{{N_{\rm{f}}} \times {N_{\rm{f}}}}},} \end{aligned} \right\}$

其中，I为单位矩阵.

式（7）中的目标函数一次项矩阵g计算如下：

(12) ${{g}} = \left[ {\begin{array}{*{20}{c}} {\left( {{\alpha _{{{{C}}_{{x}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{{P}}_{{\rm{ps}}}}{\rm{ + }}{\beta _{{{{C}}_{{x}}}}}{{P}}_{{\rm{vu}}}^{\rm{T}}{{{P}}_{{\rm{vs}}}}} \right){{{\hat{ c}}}_{{{x(}}k{\rm{)}}}} - {\alpha _{{{{C}}_{{x}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{C}}_{{{x(}}k{\rm{)}}}^{{\rm{ref}}}} \\ {\left( {{\alpha _{{{{C}}_{{y}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{{P}}_{{\rm{ps}}}}{\rm{ + }}{\beta _{{{{C}}_{{y}}}}}{{P}}_{{\rm{vu}}}^{\rm{T}}{{{P}}_{{\rm{vs}}}}} \right){{{\hat{ c}}}_{{{y(}}k{\rm{)}}}} - {\alpha _{{{{C}}_{{y}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{C}}_{{{y(}}k{\rm{)}}}^{{\rm{ref}}}} \\ {\left( {{\alpha _{{{{\varTheta }}_{\rm{r}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{{P}}_{{\rm{ps}}}}{\rm{ + }}{\beta _{{{{\varTheta }}_{\rm{r}}}}}{{P}}_{{\rm{vu}}}^{\rm{T}}{{{P}}_{{\rm{vs}}}}} \right){{{\hat{ \theta }}}_{{\rm{r(}}k{\rm{)}}}} - {\alpha _{{{{\varTheta }}_{\rm{r}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{\varTheta }}_{{\rm{r(}}k{\rm{)}}}^{{\rm{ref}}}} \\ {\left( {{\alpha _{{{{\varTheta }}_{\rm{p}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{{P}}_{{\rm{ps}}}}{\rm{ + }}{\beta _{{{{\varTheta }}_{\rm{p}}}}}{{P}}_{{\rm{vu}}}^{\rm{T}}{{{P}}_{{\rm{vs}}}}} \right){{{\hat{ \theta }}}_{{\rm{p(}}k{\rm{)}}}} - {\alpha _{{{{\varTheta }}_{\rm{p}}}}}{{P}}_{{\rm{pu}}}^{\rm{T}}{{\varTheta }}_{{\rm{p(}}k{\rm{)}}}^{{\rm{ref}}}} \\ { - {\delta _{{{{D}}_{{x}}}}}{{D}}_{{{x(}}k{\rm{)}}}^{{\rm{ref}}}} \\ { - {\delta _{{{{D}}_{{y}}}}}{{D}}_{{{y(}}k{\rm{)}}}^{{\rm{ref}}}} \end{array}} \right].$

在目标函数（8）中，需要给定预测时域内参考的足部落脚点位置、CoM位置和躯干转动角度.

不同周期的足部落脚点，可以通过预先指定参考步长和步宽等步行参数求出. 通过坐标切换，求得参考落脚点的水平位置如下：

(13) $ \left. \begin{array}{*{20}{l}} {d_{{{x}}(j)}^{\rm{ref}}} = d_{{{x}}(j - 1)}^{\rm{ref}} + s_{{{x}}(j - 1)}^{\rm{ref}},&{j \geqslant 2;} \\ {d_{{{y}}(j)}^{\rm{ref}} }= d_{{{y}}(j - 1)}^{\rm{ref}} + {{( - 1)}^j}s_{{{y}}(j - 1)}^{\rm{ref}},& j \geqslant 2. \end{array} \right\} $

式中： $s_{{{x}}(j)}^{{\rm{ref}}}$和 $s_{{{y}}(j)}^{{\rm{ref}}}$为各周期的参考步长和步宽，第一个参考足部落脚点 ${{\rm{[}}d_{{{x}}(1)}^{{\rm{ref}}},d_{{{y}}(1)}^{{\rm{ref}}}{\rm{]}}^{\rm T} }$由机器人初始姿态决定.

在目标函数（8）中，不同于Diedam等^{[6, 17]}在目标函数中惩罚ZMP位置误差实现CoM轨迹生成，本文直接使用CoM位置误差惩罚项. 为了简化计算过程，将CoM参考位置关联到每个周期的支撑脚中心位置（即各个周期的落脚点）. 以前向运动为例，在预测时域内参考CoM位置由下式给出：

(14) ${{C}}_{{{x(}}k{\rm{)}}}^{{\rm{ref}}}{\rm{ = }}{\bar{ D}}_{{{x(}}k{\rm{)}}}^{{\rm{ref}}}.$

式中： ${\bar{ D}}_{{{x(}}k{\rm{)}}}^{{\rm{ref}}}{\rm{ = }}{\left[ {d_{{{x}}(k + 1)}^{{\rm{ref}}}, \cdots ,d_{{{x}}(k + {N_{\rm{h}}})}^{{\rm{ref}}}} \right]^{\rm{T}}}$表示采样时域内所有采样时刻对应的足部参考落脚点.

给定步行周期时长和预测时长，预测时域内将会利用N_f个未来周期的落脚点信息. 此外，对于实际机器人而言，当前已落下的支撑脚应该保持不动. 需要将当前周期足部落脚点和预先指定的未来N_f周期落脚点映射成整个预测时域的参考落脚点.

在每个周期内，不考虑双足支撑阶段，参考足部支撑位置应保持不变. 预测时域内参考足部落脚点（以前向落脚点为例）可由下式计算：

(15) ${\bar{ D}}_{{{x(}}k{\rm{)}}}^{{\rm{ref}}}{\rm{ = }}{{{e}}_{{\rm{d(}}k{\rm{)}}}}{\tilde d_{{{x(}}k{\rm{)}}}} + {{{E}}_{{\rm{d(}}k{\rm{)}}}}{{D}}_{{{x(}}k{\rm{)}}}^{{\rm{ref}}}.$

式中： ${\tilde d_{{{x(}}k{\rm{)}}}}$表示第k个采样时刻对应的当前周期的实际足部落脚点； ${{{e}}_{{\rm{d(}}k{\rm{)}}}} \in {{\bf R} ^{{N_{\rm{h}}}}}$和 ${{{E}}_{{\rm{d(}}k{\rm{)}}}}\in{{\bf R} ^{{N_{\rm{h}}} \times {N_{\rm{f}}}}}$为预测时域内当前周期与未来周期足部落脚点映射矩阵，

(16) $\left[ {\begin{array}{*{20}{c}} {{{{e}}_{{\rm{d(}}k{\rm{)}}}}}& \vdots &{{{{E}}_{{\rm{d(}}k{\rm{)}}}}} \end{array}} \right] = {\left[ {\begin{array}{*{20}{c}} {1 \cdots 1\;\;0 \cdots 0\;\;0 \cdots 0\;\;0 \cdots 0} \\ {0 \cdots 0\;\;1 \cdots 1\;\;0 \cdots 0\;\;0 \cdots 0} \\ {0 \cdots 0\;\;0 \cdots 0\;\;1 \cdots 1\;\;0 \cdots 0} \\ \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\; \ddots \\ {0 \cdots 0\;\;0 \cdots 0\;\;0 \cdots 0\;\;0 \cdots 1} \end{array}} \right]^{\rm{T}}},$

其中，各行对应采样时刻，各列对应支撑周期.

在正常模式下，人类趋向于保持躯干竖直，以减少能量消耗. 本文将预测时域内的躯干参考横滚角和俯仰角设置为0，

(17) ${{\varTheta }}_{{\rm{r(}}k{\rm{)}}}^{{\rm{ref}}}{\rm{ = }}{{\varTheta }}_{{\rm{p(}}k{\rm{)}}}^{{\rm{ref}}}{\rm{ = }}{{{0}}_{{N_{\rm{h}}} \times 1}}.$

式中：0为零元素矩阵.

为了保证生成步态的可行性，需要考虑ZMP稳定性约束、足部落脚点变动范围限制、髋关节力矩限制等可行性约束.

根据ZMP稳定性理论，机器人在行走过程中应保证ZMP在足部支撑多边形内. 由于单足支撑阶段支撑多边形稳定区域最小，且当采样时间较大时可以忽略双足支撑过程，本文只考虑单足支撑阶段以简化约束形式. 以前向运动为例，在预测时域内ZMP位置面临以下约束：

(18) $\begin{array}{*{20}{c}} {p_{{x}}^{\min } \leqslant {p_{{{x(}}k + i{\rm{)}}}} - {d_{{{x(}}k + i{\rm{)}}}} \leqslant p_{{x}}^{\max };}&{i = 1, \cdots ,{N_{\rm{h}}}} \end{array}.$

式中： $\left[ {{p_{{{x(}}k + i{\rm{)}}}}, \cdots ,{p_{{{x(}}k + {N_{\rm{h}}}{\rm{)}}}}} \right]$表示采样时域内所有采样时刻对应的ZMP前向位置； $\left[ {{d_{{{x(}}k + i{\rm{)}}}}, \cdots ,{d_{{{x(}}k + {N_{\rm{h}}}{\rm{)}}}}} \right]$表示采样时域内所有采样时刻对应的实际足部落脚点； $p_{{x}}^{\min }$和 $p_{{x}}^{\max }$为前向ZMP运动约束下边界和上边界，由机器人足部尺寸决定，如图2所示.

由目标函数（8）可知，实际足部落脚点会根据当前状态发生变化. 该变化受到腿长、关节角速度、自碰撞等约束，需要予以考虑. 从以下2个方面进行讨论.

1）由于机器人结构尺寸和驱动能力的限制，机器人步行参数如步长和步宽将会受到限制. 以前向足部落脚点为例，使用如下约束：

(19) $\left. \begin{array}{l} s_{{x}}^{\min } \leqslant {d_{{{x}}(k,1)}} - {{\tilde d}_{{{x}}(k)}} \leqslant s_{{x}}^{\max }; \\ \begin{array}{*{20}{l}} {s_{{x}}^{\min } \leqslant {d_{{{x}}(k,i)}} - {d_{{{x}}(k,i - 1)}} \leqslant s_{{x}}^{\max },}&{i{\rm{ = }}2, \cdots ,{N_{\rm{f}}}.} \end{array} \end{array} \right\}$

式中： ${\tilde d_{{{x(}}k{\rm{)}}}}$表示已经生成的当前支撑周期足部前向落脚点， $s_{{x}}^{\min }$和 $s_{{x}}^{\max }$为步长约束下边界和上边界.

2）由于预测时域内足部落脚点在每次循环中都被重新计算，可能导致不同循环生成的未来某一周期的足部落脚点发生变动. 由于驱动能力的限制，摆动腿的摆动位置和速度（跟每个周期的足部落脚点直接相关）不能任意变化. 本文不直接约束摆动腿轨迹，而是限制由不同的控制循环计算生成的同一周期的落脚点位置变化. 由于该约束在每个循环均生效，本文只限制预测时域内未来下一个周期的落脚点变化. 提出如下约束：

(20) $\dot d_{{x}}^{\min }{\rm d}t \leqslant {d_{{{x}}(k,1)}} - {d_{{{x}}(k - 1,1)}} \leqslant \dot d_{{x}}^{\max }{\rm d}t.$

式中： ${d_{{{x}}(k,1)}}$和 ${d_{{{x}}(k - 1,1)}}$分别表示当前循环和上一循环生成的未来下一周期足部前向落脚点， $\dot d_{{x}}^{\min }$和 $\dot d_{{x}}^{\max }$为前向落脚点变化速度约束下边界和上边界.

躯干旋转范围受到结构尺寸、驱动能力等限制，以躯干绕x轴转动（横滚角变化）为例,使用如下约束：

(21) $\begin{array}{*{20}{c}} {\theta _{\rm{r}}^{\min } \leqslant {\theta _{{\rm{r}}(k,i)}} \leqslant \theta _{\rm{r}}^{\max };}&{i = 1, \cdots ,{N_{\rm{h}}}.} \end{array}$

式中： $\theta _{\rm{r}}^{\min }$和 $\theta _{\rm{r}}^{\max }$分别为横滚角极小值和极大值.

髋关节输出力矩存在以下约束：

(22) $\begin{array}{*{20}{c}} {\tau _{\rm{r}}^{\min } \leqslant {I_{{x}}}{{\ddot \theta }_{{\rm{r}}(k,i)}} \leqslant \tau _{\rm{r}}^{\max };}&{i = 1, \cdots ,{N_{\rm{h}}}} \end{array}.$

式中： $\tau _{\rm{r}}^{\min }$和 $\tau _{\rm{r}}^{\max }$分别为髋关节绕x轴力矩约束的下边界和上边界.

使用IPFM模型，以上可行性约束条件均为线性约束，可以转化为式（7）中线性约束函数的表达形式，因此该优化问题是二次规划问题.

使用由意大利技术研究所（Italiatan Institution of Technology，IIT）仿人机器人课题组开发的基于开放式动力学引擎（open dynamics engine，ODE）的动力学仿真平台和IIT研发的COMAN^[26]机器人，如图3所示. 机器人总高度为1.2 m，总质量为31 kg，每条腿各有6个自由度（髋关节3个、膝关节1个、踝关节2个），直立状态下髋关节高0.467 m. 机器人模型和控制器模型参数如表1所示，式（18）~（22）所列的约束条件参数如表2所示. 在仿真过程中，默认的步行周期为0.8 s，参考步长为10 cm，参考步宽为14.5 cm，摆动腿最大摆动高度为8 cm，双足支撑相占步行周期的比例为20%. 在行走过程中，摆动腿轨迹通过5次多项式插值获得. 仿真器底层的通讯频率为200 Hz.

由于存在动力学模型简化、机器人-地面接触模型不精确、多刚体系统姿态估计算法不准确等不确定性，机器人的实际状态与理想状态必然存在偏差. 改变步行参数将导致步态的非周期，进一步放大这种偏差，甚至可能导致失稳. 因此，需要机器人能够实时调整自身状态，以完成预定的行走目标. 首先验证该算法在使用实时反馈数据的情况下实现变参数稳定行走的能力.

除表3所列周期的步行参数发生改变外，其他周期均使用默认参数. 从表3可以看出，机器人将从第6周期（每个周期步行参数变化将影响下一周期落脚点）开始在矢状面内前进方向作变速运动，同时在第8~12周期调整侧向落脚点.

仿真结果表明，只利用足部调整，由于实际反馈状态的不理想，机器人在行走过程中发生倾覆. 从图4可见，机器人上半身从6 s后开始剧烈晃动，最终失稳. 在集成躯干转动优化后，机器人可以顺利完成行走.

图5~7分别给出集成躯干转动后机器人其他重要状态参数的优化生成与实际测量结果. 图5、6表明，机器人在行走过程中能够较好地跟踪预定的足部落脚点. 不需要指定参考ZMP轨迹，由于足部落脚点的微调和躯干的转动，机器人能够保持生成ZMP轨迹和实际ZMP轨迹始终在足部支撑区域以内. 结合图4~7可以看出，在第6~14周期内，机器人利用躯干转动（图4中6~13 s内俯仰角的调整），在矢状面内的前进方向较准确地跟踪参考落脚点行走. 从图7可以看出，机器人侧向质心在第8周期（6 s左右）不能及时向左支撑脚靠近，导致后续周期CoM轨迹发生较明显的振荡. 由于足部落脚点调整（图6中8~10 s内侧向落脚点的调整）以及躯干转动（图4中8~10 s内算法生成横滚角的调整），保证机器人在侧向稳定.

结合图4、7可以看出，基于机器人实际状态，本文所提算法能够实时解算出最优的CoM轨迹和躯干转动角度. 在第14周期后，机器人逐渐跟踪预定参数并恢复周期行走. 不同于文献[17-19]，由于本文在目标函数（8）中加入了旋转角度惩罚项，可以发现最大横滚角和俯仰角都被抑制在[3º，−3º]. 在第14周期后，机器人躯干转动角度快速减少，基本恢复竖直状态.

仿真中，在3.6 s时刻（此刻左脚支撑，且右脚摆动高度达到最高）对髋关节中心施加持续0.1 s的不同方向水平外力，观察机器人能否保持稳定.

以沿y轴负方向施加25 N的推力为例，图8给出不同策略下生成的足部落脚点和实际CoM轨迹. 可以看出，当施加同样的外力，只调整足部落脚点将会导致比较剧烈的落脚点变动，比如推力施加后下一落脚点从参考值（0.3，−0.725）变化到了（0.3，−0.869）. 通过引入躯干转动，下一落脚点仅变化到了（0.3，−0.732）. 此外，只调整落脚点时，需要经过多步（图8中后续多步落脚点与参考值均有较大的变动）才能跟踪到预定的落脚位置. 集成躯干转动后，在同样外力下只需要一步调整即可回归.

通过引入躯干转动，机器人在遇到外力干扰时能够及时调整躯干转动角度以保持平衡. 如图9所示，当沿y轴负方向外力出现时，机器人能够主动增大横滚角以减少落脚点的调整量. 与仅利用落脚点调整时躯干横滚角会发生正、反两个方向较大的变动不同，在目标函数中引入躯干转动角度惩罚项，能够让机器人减轻躯干的左、右摆动，利用更少的周期恢复正常的摆动姿态.

表4给出不同策略能够抵抗的不同方向的最大外力. 可以看出，在各个方向上，集成躯干转动优化，机器人能够抵抗更大的外力干扰而保持稳定.

数值分析表明，在矢状面沿x轴方向，对外力的抵抗能力提高了10%以上；在冠状面沿y轴方向，对外力的抵抗能力提高了20%以上. 特别地，机器人对沿x轴正向的外界推力抵抗能力最强，是由于足部沿该方向的长度最长，单足相可以利用的支撑区域最大. 机器人对沿y轴负方向外力的抵抗能力比其他几个方向均低很多. 原因是推动力出现时机器人处于左腿支撑且质心已经朝y轴负方向开始移动. 机器人足部宽度明显比长度窄，y轴负向的推力将加速摆动腿落地，从而缩短落脚点调整和躯干转动的反应时间.

该算法及仿真程序使用C++语言编程实现. 在Ubuntu16.04系统中，使用开源优化求解库QuadProg++，求解该QP问题. 该求解器利用对偶法求解约束优化问题，通过代码优化，在主频3.0 GHz 的四核Intel CPU上每个控制循环耗时小于2 ms. 考虑到该算法属于规划层，底层可以以更高的频率进行通讯（实际上，以往的试验表明，500 Hz通讯频率能够满足多数任务的需求）. 该算法能够满足未来硬件试验的要求.

（1）提出可在线生成质心轨迹、调整落脚点和优化躯干转动角度的优化目标. 通过简化可行性约束，建立二次规划求解模型. 数值求解表明，每个循环在2 ms以内完成，能够满足实时控制的需求.

（2）通过多刚体动力学仿真，验证了该算法的可行性. 面对模型不精确对机器人状态产生的干扰，实现了变参数行走任务. 结果表明，利用躯干转动，可以实现更大范围的变参数步态在线生成.

（3）利用躯干转动，提高了机器人对外界推力的抵抗能力. 结果表明，通过引入躯干旋转优化，在同样大小和方向的外力推动下，所需要的落脚点调整量明显减少. 与只进行落脚点调整相比，在矢状面沿x轴方向，对外力的抵抗能力提高了10%以上；在冠状面沿y轴方向，对外力的抵抗能力提高了20%以上.

以后将在IIT开发的新一代仿人机器人样机COMAN+上进行验证试验. 未来一方面考虑将目标函数从落脚点位置跟踪变成速度跟踪，提高对外力干扰的抵抗能力. 另一方面，考虑集成实时变高度求解，开展更加鲁棒的多任务步行算法研究.