1 引言（Introduction）

动物具有卓越的运动能力和环境适应能力，是开发先进机器人控制方法的主要灵感来源^[1-2]。人们通过模仿陆生生物，开发出了各种腿式机器人，它们表现出了出色的运动性能^[3-5]，但其控制器大多基于传统的控制方法，例如，整体控制枢纽法^[6]、逆动力学模型控制^[7]、基于优化的控制^[8]等。这些控制方法的性能非常依赖机器人模型的精度和环境状况，而且对控制参数的优化也是一项非常复杂的工作。因此，近些年受生物CPG（中枢模式发生器）启发的无需依赖机器人和环境模型的控制方法受到了广泛的关注。

动物运动的节奏模式是通过一个被称为CPG的神经网络产生的^[9-10]。CPG广泛存在于脊椎动物和无脊椎动物中，主要负责产生协调的、有节奏的运动，例如，爬行、飞行、游泳、跳跃、行走和奔跑等^[11]。学者们受此启发开发出了各类CPG振荡器模型^[12-14]并将其应用到工程实践中。Liu和Sun等^[15-16]将前庭反射与各类CPG结合起来，用于产生适应性的身体姿势，从而使机器人能够走在不同的斜坡地形上。Tutsoy^[17]通过基于CPG的加强学习算法，使机器腿保持直立平衡。Yang^[18]提出了一种基于图论和路径矩阵的统一方法，用于为水下自重构机器人的不同模式生成CPG网络。Li等^[19]基于CPG－零时刻控制算法完成了四足机器人的平滑步态规划和稳定性控制。Chung等^[20]提出了一种双CPG架构，用于六足机器人在复杂地形中的自适应步态控制。

虽然在机器人的运动控制中引入CPG提高了机器人的稳态运动性能和自适应能力，但当机器人行走于狭窄环境、检测土地矿山以及避开障碍物时，需要按照预定轨迹运动，而且还需要具有良好的轨迹跟踪精度和鲁棒性^[21]。轨迹跟踪是自主机器人的关键优势，但目前很少有对该主题的研究。在轨迹跟踪过程中，由于机器人的各部件之间、机器人与环境之间都存在相互作用，因此针对轨迹跟踪问题开发出精度高、鲁棒性好的控制器比较困难。目前常用于轨迹跟踪的控制方法包括滑模控制（SMC）^[22-23]和模型预测控制（MPC）^[24-25]等。Chen等^[21]在轨迹跟踪模型的基础上，建立了一种基于NTSM（nonsingular terminal sliding mode）逼近律的非奇异终端滑模控制算法，在仿真中使六足机器人精确地跟踪上参考轨迹，并通过实验验证了该控制方法的有效性。Mera等^[26]提出了一种基于滑模鲁棒控制的方法，针对更接近现实情况的独轮机器人运动学模型，引入了一个新颖的滑模面以减小滑模面对航向角的抖振影响，进而实现了在具有扰动的情况下对独轮机器人的轨迹跟踪控制。Huang等^[27]提出了一种基于在线估计器的线性时变模型预测控制器（LTV-MPC），解决了在存在时变参数、高频外部负载干扰、测量噪声和一些不可衡量的状态的情况下电动液压执行器的轨迹跟踪问题。实验表明LTV-MPC控制器具有强大的自适应能力和稳健的轨迹跟踪性能。Liu等^[28]将A算法与四轮独立转向机器人的动力学模型结合起来进行轨迹规划，然后建立了具有动态约束的机器人运动学模型和模型预测控制器，实现了机器人的高速轨迹跟踪控制，通过仿真和实验验证了轨迹跟踪方法的有效性和实用性。

模型预测控制因为具有建模方便、系统鲁棒性、稳定性好，能够将多约束问题纳入滚动优化中等优点，已被广泛应用到各类机器人的运动控制中并取得了良好的效果^[29-33]。足式机器人的运动是一类具有多约束的非线性问题，并且在轨迹跟踪过程中需要频繁地切换爬行步态以保证对参考轨迹的跟踪精度，因此本文一方面需要采用CPG网络作为底层运动控制器确保运动控制过程中的平滑性，另一方面引入模型预测控制等算法作为上层控制器，对CPG网络的参数进行实时的调整，进而完成轨迹跟踪任务。

2 基于CPG的运动学模型（Kinematics model based on CPG）

学者们通过模仿生物CPG开发出了各类CPG振荡器模型，目前常用的包括递归神经振荡器、相位振荡器和Hopf振荡器等。本文采用Hopf振荡器作为CPG的基本单元构建底层运动控制网络。CPG网络的数学模型为

$ \begin{equation} \begin{aligned} \dot {\alpha} _i = &-\varpi _i y_i +\mu \alpha _i ({H_i^2 -\alpha _i^2 -\beta _i ^2})+ \\ & h_i ({\alpha_{i-1} \cos \varphi _i +\beta_{i-1} \sin \varphi_{i, i-1}}) \\ \dot {\beta} _i = \; &\varpi _i \alpha _i +\mu \beta _i ({H_i^2 -\alpha _i^2 -\beta _i ^2})+ \\ & h_{i+1} ({\alpha_{i+1} \sin \varphi_{i+1} +\beta_{i+1} \cos \varphi_{i, i+1}}) \end{aligned} \end{equation} $

(1)

其中，$ \alpha _i $和$ \beta _i $分别代表第$ i $个振荡器的状态变量。$ \varpi _i $和$ H_i $分别代表第$ i $个振荡器的振荡频率和振幅。$ h_i $和$ \varphi _i $分别代表第$ i $个振荡器的耦合因子和相位差，$ i =1, 2, \cdots, 6 $。取$ \varpi _i =\pi $，$ H_i =30 $，$ h_i =0.5 $，$ \varphi _i =\pi $。$ \mu >0 $为收敛因子，控制极限环的收敛速度，$ \mu $越大，收敛速度越快，此处取$ \mu =15 $。CPG网络状态变量$ \alpha _i $的输出如图 1所示，由图可知，CPG网络中的各个单元之间能够在短时间内产生稳定的相位差，输出平滑稳定的控制信号。

通过耦合项构建的CPG网络如图 2所示，CPG网络的输出信号经过映射函数放大或缩小后分别传递到机器人6条腿的髋关节、膝关节和踝关节上，进而实现每条腿3个关节的协调运动。最后通过上层控制器实时调节CPG网络的输出，进而通过改变机器人的速度和角速度实现轨迹跟踪控制。

六足机器人的转向是通过左右两侧腿摆动的速度差实现的，为了便于调节机器人爬行时两侧腿的速度差，引入了转向参数$ \delta $，令参数$ \delta $和步长$ A_i $满足如下关系：

$ \begin{align} A_i =\begin{cases} A_{\max} \sin \delta, & i=1, 2, 3 \\ A_{\max} \cos \delta, & i=4, 5, 6 \end{cases} \end{align} $

(2)

其中，$ A_{\max} $表示机械腿的最大步长，$ A_{\max} =L\times $ $ \sin H_{\max} $，$ H_{\max} $是CPG网络的最大输出振幅，$ L $是在机器人的俯视图中机械腿在地面上的投影长度，因为$ H_{\max} $比较小，因此为了便于计算和控制，可令$ A_{\max} =LH_{\max} $，$ i $表示机械腿的编号，与振荡器的编号一致，$ i=1, 2, 3 $为左侧腿，$ i=4, 5, 6 $为右侧腿。

左右两侧腿的速度为

$ \begin{align} v_{\rm L} =A_{\max} f\sin \delta , \quad v_{\rm R} =A_{\max} f\cos \delta \end{align} $

(3)

其中，$ v_{\rm L} $为左侧腿的速度，$ v_{\rm R} $为右侧腿的速度，$ f=\varpi/ 2\pi $为机械腿摆动的频率。

左右两侧腿的速度差$ \Delta v $为

$ \begin{align} v_{\rm R} -v_{\rm L} =\Delta v=A_{\max} f({\cos \delta -\sin \delta}) \end{align} $

(4)

式(4) 可化简为

$ \begin{align} \Delta v=\sqrt 2\:A_{\max} f\sin \left(\frac{\pi} {4}-\delta\right) \end{align} $

(5)

由此可知，当$ \delta <\pi / 4 $时，$ \Delta v>0 $，机器人左转；当$ \delta >\pi / 4 $时，$ \Delta v<0 $，机器人右转；当$ \delta = $ $ \pi / 4 $时，$ \Delta v=0 $，机器人直行。

机器人中心的移动速度为

$ \begin{align} v=\frac{v_{\rm R} +v_{\rm L}}{2}=\frac{\sqrt 2} {2}A_{\max} f\sin \left(\frac{\pi} {4}+\delta\right) \end{align} $

(6)

如图 3所示，当左右两侧腿产生速度差时，机器人开始转向。

图中，$ 2 k $为机器人的宽度，$ \theta $为机器人的偏转角。由图 3可知，机器人转向时的回转半径为

$ \begin{align} R=\overline {OA} +k \end{align} $

(7)

通过几何关系可求得

$ \begin{align} \overline {OA} =\frac{2kv_{\rm L}} {v_{\rm R} -v_{\rm L}} \end{align} $

(8)

将$ \overline {OA} $代入式(7) 中，可得

$ \begin{align} R& =\frac{k({v_{\rm R} +v_{\rm L}})}{v_{\rm R} -v_{\rm L}} \end{align} $

(9)

$ \begin{align} \dot {\theta} & =\omega =\frac{v}{R}=\frac{v_{\rm L} -v_{\rm R}} {2 k} =\frac{\sqrt 2} {2 k}A_{\max} f\sin \left(\frac{\pi} {4}-\delta\right) \end{align} $

(10)

得到了基于CPG的六足机器人运动学模型

$ \left\{ \begin{matrix} v=\frac{\sqrt{2}}{2}{{A}_{\max }}f\sin \left( \frac{{ \pi}}{4}+\delta \right) & (11a) \\ \omega =\frac{\sqrt{2}}{2k}{{A}_{\max }}f\sin \left( \frac{{ \pi}}{4}-\delta \right) & (11b) \\ \end{matrix} \right.$

由六足机器人的运动学可知，机器人的速度$ v $和角速度$ \omega $均与CPG网络的参数$ f $和$ \delta $相关。在轨迹跟踪过程中，由于上层控制器求得的控制量为机器人的速度$ v $和角速度$ \omega $，而下层运动控制网络CPG的输入为参数$ f $和$ \delta $，因此上层控制器无法直接对CPG网络进行控制，需要设计一组转换函数，将上层控制器的输出转换为一组关于$ f $、$ \delta $的解作为调节CPG的控制量，而参数$ f $和$ \delta $耦合在一起，为了便于控制，将(11a) 和(11b) 相除得

$ \begin{align} \frac{v}{\omega} =R=\frac{k(\cos \delta +\sin \delta)}{\cos \delta -\sin \delta} \end{align} $

(12)

化简得

$ \begin{align} \delta =\arctan \frac{R-k}{R+k} \end{align} $

(13)

将式(13) 代入(11a)，经化简得

$ \begin{align} f=\frac{v\sqrt {2R^2+2k^2}} {A_{\max} R} \end{align} $

(14)

因此，转换函数为

$ \begin{align} \begin{cases} f=\dfrac{v\sqrt {2R^2+2k^2}} {A_{\max} R} \\[5pt] \delta =\arctan \dfrac{R-k}{R+k} \end{cases} \end{align} $

(15)

3 MPC-CPG控制器设计（Design of the MPC-CPG controller）

本文一方面采用CPG作为底层运动控制器以确保运动控制过程中的平滑性，另一方面以模型预测控制作为上层控制器对CPG进行实时调整，进而完成轨迹跟踪任务。

六足机器人的轨迹跟踪示意图如图 4所示，运动学方程如式(16) 所示。

$ \begin{align} \begin{bmatrix} x'' \\ y'' \\ \varphi'' \end{bmatrix} = \begin{bmatrix} \cos \varphi & 0 \\ \sin \varphi & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} v \\ w \end{bmatrix} \end{align} $

(16)

其中，$ (x, y) $为机器人中心的坐标，$ \varphi $为机器人的航向角，$ v $、$ \omega $分别为机器人的速度和角速度。

3.1 基于运动学模型的误差模型

由式(16) 可知，系统可以被看作一个输入为$ \boldsymbol u=[v, \omega]^{\rm T} $和状态量为$ \boldsymbol \chi = [x, y, \varphi]^{\rm T} $的控制系统，其一般形式为

$ \begin{align} \dot{\boldsymbol\chi} ={\boldsymbol f}(\boldsymbol\chi, \boldsymbol u) \end{align} $

(17)

给定的参考轨迹上的每一个点都满足式(17) 方程，用下标r代表参考量，式(17) 变为

$ \begin{align} \dot{\boldsymbol\chi}_{\rm r} ={\boldsymbol f}(\boldsymbol\chi_{\rm r}, \boldsymbol u_{\rm r}) \end{align} $

(18)

其中，$ {\boldsymbol \chi} _{\rm r} =[{x_{\rm r}} \; \; {y_{\rm r}} \; \; {\varphi _{\rm r}}]^{\rm T} $，$ {\boldsymbol u}_{\rm r} =[{v_{\rm r}} \; \; {\omega _{\rm r}} ]^{\rm T} $。

通过泰勒展开式将式(17) 在参考轨迹点处展开并忽略高阶项，得到：

$ \begin{align} \dot{\boldsymbol{\chi}}=\;& {\boldsymbol f}(\boldsymbol{\chi}_{\rm r}, \boldsymbol{u}_{\rm r})+\left.\frac{\partial {\boldsymbol f}(\boldsymbol{\chi}, \boldsymbol{u})}{\partial \boldsymbol{\chi}}\right|_{ \begin{subarray}{l} \boldsymbol \chi=\boldsymbol \chi_{\rm r} \\ {\boldsymbol u}={\boldsymbol u}_{\rm r} \end{subarray} } (\boldsymbol{\chi}-\boldsymbol{\chi}_{\rm r} )+ \notag \\ & \left.\frac{\partial {\boldsymbol f}(\boldsymbol{\chi}, \boldsymbol{u})}{\partial \boldsymbol{u}}\right|_{ \begin{subarray}{l} \boldsymbol\chi=\boldsymbol\chi_{\rm r}\\ \boldsymbol u=\boldsymbol u_{\rm r} \end{subarray} } (\boldsymbol u=\boldsymbol{u}_{\rm r}) \end{align} $

(19)

通过式(18)(19)，可以得到线性误差模型为

$ \begin{align} \dot{\tilde{\boldsymbol\chi}} = \begin{bmatrix} \dot{x}-\dot{x}_{\rm r} \\ \dot{y}-\dot{y}_{\rm r} \\ \dot{\varphi}-\dot{\varphi}_{\rm r} \end{bmatrix}=\;& \begin{bmatrix} 0 & 0 & -v_{\rm r} \sin \varphi_{\rm r} \\ 0 & 0 & v_{\rm r} \cos \varphi_{\rm r} \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} x-x_{\rm r} \\ y-y_{\rm r} \\ \varphi-\varphi_{\rm r} \end{bmatrix}+ \\ & \begin{bmatrix} \cos \varphi_{\rm r} & 0 \\ \sin \varphi_{\rm r} & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} v-v_{\rm r} \\ \omega-\omega_{\rm r} \end{bmatrix} \end{align} $

(20)

式(20) 即为线性化的六足机器人的误差模型。为了将该模型应用于模型预测控制器的设计，对式(20) 进行离散化处理：

$ \begin{align} &\tilde{\boldsymbol\chi}(k+1) =\boldsymbol{A}_{k, t} \tilde{\boldsymbol{\chi}}(k)+\boldsymbol{B}_{k, t} \tilde{\boldsymbol{u}}(k) \\ &\boldsymbol A_{k, t} = \begin{bmatrix} 1 & 0 & -v_{\rm r} \sin \varphi _{\rm r} T \\ 0 & 1 & v_{\rm r} \cos \varphi _{\rm r} T \\ 0 & 0 & 1 \end{bmatrix} , \quad \boldsymbol B_{k, t} = \begin{bmatrix} \cos \varphi _{\rm r} T & 0 \\ \sin \varphi _{\rm r} T & 0 \\ 0 & T \end{bmatrix} \end{align} $

(21)

其中，$ T $为采样时间。

3.2 目标函数设计

参照文[34]所使用的软约束方法，采用如下形式的目标函数：

$ \begin{align} J(k)=\;&\sum\limits_{i=1}^{N_{\rm p}}\|\boldsymbol{\eta}(k+i | t)-\boldsymbol{\eta}_{\rm r}(k+i | t)\|_{Q}^{2}+ \\ & \sum\limits_{i=1}^{N_{\rm c}-1}\|\Delta \boldsymbol{U}(k+i | t)\|_{R}^{2}+\rho {\varepsilon}^{2} \end{align} $

(22)

其中，$ \boldsymbol\eta (k+i| t) $为控制输出，$ \Delta\boldsymbol U(k+i| t) $为控制增量，$ N_{\rm p} $为预测时域，$ N_{\rm c} $为控制时域，$ \boldsymbol Q $、$ \boldsymbol R $为权重矩阵，$ \rho $为权重系数，$ {\varepsilon} $为松弛因子，$ \|{\boldsymbol E}\|_{R}^{2} $表示$ {\boldsymbol E}^{\rm T}{\boldsymbol R}{\boldsymbol E} $运算，$ \boldsymbol E $代表式(22) 双竖线中间的部分。

将式(21) 作如下转换：

$ \begin{align} \boldsymbol\xi (k| t)= \begin{bmatrix} \tilde {\boldsymbol\chi} (k| t) \\ \tilde {\boldsymbol u} (k-1| t) \end{bmatrix} \end{align} $

(23)

得到一个新的状态空间表达式

$ \begin{equation} \begin{cases} \boldsymbol{\xi}(k+1 | t) =\tilde{\boldsymbol{A}}_{k, t} \boldsymbol{\xi}(k | t)+\tilde{\boldsymbol{B}}_{k, t} \Delta \boldsymbol{U}(k | t) \\ \boldsymbol{\eta}(k | t) =\tilde{\boldsymbol{C}}_{k, t} \boldsymbol{\xi}(k | t) \end{cases} \end{equation} $

(24)

其中，$ \tilde{\boldsymbol{A}}_{k, t}= \begin{bmatrix} \boldsymbol{A}_{k, t} & \boldsymbol{B}_{k, t} \\ \bf{0}_{m \times n} & \boldsymbol{I}_{m} \end{bmatrix} $，$ \tilde{\boldsymbol{B}}_{k, t}= \begin{bmatrix} \boldsymbol{B}_{k, t} \\ \boldsymbol{I}_{m} \end{bmatrix} $，$ n $为状态量的维度，$ m $为控制量的维度，$ \boldsymbol I_m $为$ m $阶单位矩阵。

为了简化计算，假设

$ \begin{align} {\boldsymbol A}_{k, t} ={\boldsymbol A}_{t, t}, \; \; \quad {\boldsymbol B}_{k, t} ={\boldsymbol B}_{t, t} \end{align} $

(25)

其中，$ k=1, \cdots, t+N-1 $。

推导得到系统的预测输出表达式

$ \begin{align} \boldsymbol Y(t)=\boldsymbol \varPsi _t \xi (k | t)+\boldsymbol \varTheta _t \Delta \boldsymbol U(t) \end{align} $

(26)

其中，

$ \begin{align*} &\boldsymbol Y(t)= \begin{bmatrix} \boldsymbol\eta (t+1 | t) \\ \boldsymbol\eta (t+2 | t) \\ \vdots \\ \boldsymbol\eta ({t+N_{\rm c} | t}) \\ \vdots \\ \boldsymbol\eta ({t+N_{\rm p} | t}) \end{bmatrix}, \quad \; \boldsymbol\varPsi _t =\begin{bmatrix} \tilde {\boldsymbol C}_{t, t} \tilde {\boldsymbol A}_{t, t} \\ \tilde {\boldsymbol C}_{t, t} \tilde {\boldsymbol A}_{t, t}^2 \\ \vdots \\ \tilde {\boldsymbol C}_{t, t} \tilde {\boldsymbol A}_{t, t}^{N_{\rm c}} \\ \vdots \\ \tilde {\boldsymbol C}_{t, t} \tilde {\boldsymbol A}_{t, t}^{N_{\rm p}} \end{bmatrix} \\ & \boldsymbol\varTheta _t = \\ & \begin{bmatrix} \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol B}_{t, t} & 0 & 0 & 0 \\ \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t} \tilde{\boldsymbol B}_{t, t} & \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol B}_{t, t} & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t}^{N_{\rm c} -1} \tilde{\boldsymbol B}_{t, t} & \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t}^{N_{\rm c} -2} \tilde{\boldsymbol B}_{t, t} & \cdots & \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol B}_{t, t} \\ \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t}^{N_{\rm c}} \tilde{\boldsymbol B}_{t, t} & \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t}^{N_{\rm c} -1} \tilde{\boldsymbol B}_{t, t} & \cdots & \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t} \tilde{\boldsymbol B}_{t, t} \\ \vdots & \vdots & \ddots & \vdots \\ \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t}^{N_{\rm p} -1} \tilde{\boldsymbol B}_{t, t} & \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t}^{N_{\rm p} -2} \tilde{\boldsymbol B}_{t, t} & \cdots & \tilde{\boldsymbol C}_{t, t} \tilde{\boldsymbol A}_{t, t}^{N_{\rm p} -N_{\rm c} -1} \tilde{\boldsymbol B}_{t, t} \end{bmatrix}\\ & \Delta \boldsymbol U(t) = \begin{bmatrix} \Delta \boldsymbol u(t | t) \\ \Delta \boldsymbol u(t+1 | t) \\ \vdots \\ \Delta \boldsymbol u({t+N_{\rm c} | t}) \end{bmatrix} \end{align*} $

将式(26) 代入式(22)，即可得到完整形式的目标函数。

3.3 约束条件设计

机器人运动过程中的控制量约束和控制增量约束的表达形式为

$ \begin{align} \begin{cases} \boldsymbol u_{\min} (t+k) \leq \boldsymbol u(t+k) \leq \boldsymbol u_{\max} (t+k) \\ \Delta \boldsymbol u_{\min} (t+k) \leq \Delta \boldsymbol u(t+k) \leq \Delta \boldsymbol u_{\max} (t+k) \end{cases} \end{align} $

(27)

式中，$ k=0, 1, \cdots, N_{\rm c} -1 $。

将约束合并成矩阵形式为

$ \begin{align} \boldsymbol U_{\min} \leq \boldsymbol A\Delta \boldsymbol U_t +\boldsymbol U_t \leq \boldsymbol U_{\max} \end{align} $

(28)

其中，$ \boldsymbol U_{\min} $、$ \boldsymbol U_{\max} $分别为控制时域内的控制量最小值、最大值集合，$ \boldsymbol U_t =\boldsymbol{1}_{N_{\rm c}} \otimes \boldsymbol u(k-1) $，$ \boldsymbol{1}_{N_{\rm c}} $是行数为$ N_{\rm c} $的列向量，$ \otimes $为克罗内克积。

$ \begin{align} \boldsymbol A= \begin{bmatrix} 1 & 0 & \cdots & \cdots & 0 \\ 1 & 1 & 0 & \cdots & 0 \\ 1 & 1 & 1 & \ddots & 0 \\ \vdots & \vdots & \vdots & \ddots & 0 \\ 1 & 1 & \cdots & 1 & 1 \end{bmatrix}_{N_{\rm c} \times N_{\rm c}} \otimes \boldsymbol I_m \end{align} $

(29)

将目标函数转化为标准二次型形式并结合约束条件，将控制增量的求解转变为解决以下优化问题：

$ \begin{align} & J(\boldsymbol{\xi}(t), \boldsymbol{u}(t-1), \Delta \boldsymbol{U}(t)) \\ = & [\Delta \boldsymbol{U}^{\rm T}(t), {\boldsymbol \varepsilon} ]^{\rm T} \boldsymbol{H}_{t} [\Delta \boldsymbol{U}^{\rm T}(t), {\boldsymbol \varepsilon} ]+\boldsymbol{G}_{t} [\Delta \boldsymbol{U}^{\rm T}(t), {\boldsymbol \varepsilon} ] \\ &\quad \text{s.t. } \Delta \boldsymbol{U}_{\min } \leqslant \Delta \boldsymbol{U}_{t} \leqslant \Delta \boldsymbol{U}_{\max }, \\ & \; \; \qquad \boldsymbol{U}_{\min } \leqslant \boldsymbol{A} \Delta \boldsymbol{U}_{t}+\boldsymbol{U}_{t} \leqslant \boldsymbol{U}_{\max }, \\ & \; \; \qquad \boldsymbol{H}_{t}= \begin{bmatrix} \boldsymbol{\varTheta}_{t}^{\rm T} \boldsymbol{Q} \boldsymbol{\varTheta}_{t}+\boldsymbol{R} & 0 \\ \bf{0} & \rho \end{bmatrix} , \\ & \; \; \qquad \boldsymbol{G}_{t}= [2 \boldsymbol{e}_{t}^{\rm T} \boldsymbol{Q} \boldsymbol{\varTheta}_{t} \; \; \; \; \bf{0} ] \end{align} $

(30)

其中，$ \boldsymbol e_t $为预测时域内的跟踪误差。

每经过一个采样时刻都会完成一次对式(30) 的求解，得到的控制时域内的一系列控制输入增量为

$ \begin{align} \Delta \boldsymbol U_i^* =[ \Delta \boldsymbol u_t^*, \Delta \boldsymbol u_{t+1}^*, \cdots, \Delta \boldsymbol u_{i+N_{\rm c} -1}^* ]^{\rm T} \end{align} $

(31)

将该控制序列中第1个元素作为实际的控制输入增量作用于系统，即

$ \begin{align} \boldsymbol u(t)=\boldsymbol u(t-1)+\Delta \boldsymbol u_t^* \end{align} $

(32)

进入下一个控制周期后，重复上述过程，依次循环，实现了对机器人的轨迹跟踪控制。

3.4 稳定性证明

本文采用李雅普诺夫第二方法进行稳定性证明，选取$ k $时刻的最优目标函数作为李雅普诺夫函数，即$ \min J(k)=J^* (k)=V^* (k) $。如果李雅普诺夫函数在$ k +1 $时刻满足$ V^* (k+1)\leqslant V^* (k) $，则系统稳定。

$ \begin{align} V^* (k)=\min \Bigg\{& \sum _{i=1}^{N_{\rm p}} \| {\boldsymbol \eta} (k)-{\boldsymbol \eta} _{\rm r} (k) \| _Q^2 + \\ & \sum _{i=0}^{N_{\rm c} -1} ( \| \Delta {\boldsymbol u}(k) \| _{R}^2 + \| {\boldsymbol \varepsilon} (k) \| _{\rho} ^2) \Bigg\} \end{align} $

(33)

其中，$ \boldsymbol Q $、$ \boldsymbol R $和$ \rho $分别为控制输出、控制增量和松弛因子的权重矩阵。显然，选取的李雅普诺夫函数(33) 在$ k =0 $时$ V^* (k)=0 $，在$ k\ne 0 $时$ V^* (k)>0 $。系统的最优控制量$ \tilde{\boldsymbol u}(k+1+i | k+1) $、最优控制增量$ \Delta \tilde{\boldsymbol u}(k+1+i | k+1) $和松弛因子$ {\boldsymbol \varepsilon} (k+1) $分别为

$ \begin{equation} \begin{cases} \tilde{{\boldsymbol u}}(k+1+i | k+1)=\big[\, \tilde{{\boldsymbol u}}(k+1 | k+1), \; \; \tilde{{\boldsymbol u}} (k+2 | k+1), \; \cdots, \; \tilde{{\boldsymbol u}} (k+N_{\rm c} | k+1 ) \, \big] \\ \kern 3cm = \big[\, \tilde{{\boldsymbol u}}^{*}(k+1 | k), \tilde{{\boldsymbol u}}^{*}(k+2 | k), \cdots, \tilde{{\boldsymbol u}}^{*} (k+N_{\rm c} | k ) \, \big] \\ \Delta \tilde{{\boldsymbol u}}(k+1+i | k+1)=\big[\, \Delta \tilde{{\boldsymbol u}}(k+1 | k+1), \; \; \Delta \tilde{{\boldsymbol u}}(k+2 | k+1), \; \cdots, \; \Delta \tilde{{\boldsymbol u}} (k+N_{\rm c} | k+1 )\, \big] \\ \kern 3cm = \big[\, \Delta \tilde{{\boldsymbol u}}^{*}(k+1 | k), \; \; \Delta \tilde{{\boldsymbol u}}^{*}(k+2 | k), \; \cdots, \; \Delta \tilde{{\boldsymbol u}}^{*} (k+N_{\rm c} | k ) \, \big] \\ {\boldsymbol \varepsilon}(k+1)={\boldsymbol \varepsilon}^{*}(k) \end{cases} \end{equation} $

(34)

根据式(34) 可以推导出$ J(k+1) $和$ V^* (k) $有如下关系：

$ \begin{align} J(k+1)=\;& \sum _{i=1}^{N_{\rm p}} \| {\boldsymbol \eta} (k+1+i | k+1) -{\boldsymbol \eta} _{\rm r}(k+1+i | k+1) \| _Q^2 + \sum _{i=0}^{N_{\rm c} -1} ( \| \Delta {\boldsymbol u}(k+1+i | k+1) \| _{R}^2 + \| {\boldsymbol \varepsilon} (k+1+i | k+1) \| _{\rho} ^2 ) \\ =\;& \sum _{i=2}^{N_{\rm p}} \| {\boldsymbol \eta} ^* (k+i | k)-{\boldsymbol \eta} _{\rm r} (k+i | k) \| _Q^2 + \sum _{i=1}^{N_{\rm c} -1} ({ \| \Delta {\boldsymbol u}^* (k+i | k) \| _{R}^2 + \| {\boldsymbol \varepsilon} ^* (k+i | k) \| _{\rho} ^2}) \\ = \;&\sum _{i=1}^{N_{\rm p}} \| {\boldsymbol \eta} ^* (k+i | k)-{\boldsymbol \eta} _{\rm r} (k+i | k) \| _Q^2 + \sum _{i=0}^{N_{\rm c} -1} ({ \| \Delta {\boldsymbol u}^* (k+i | k) \| _{R}^2 + \| {\boldsymbol \varepsilon} ^* (k+i | k) \| _{\rho} ^2})- \| {\boldsymbol \eta} (k | k)-{\boldsymbol \eta} _{\rm r} (k | k) \| _Q^2 - \\ & \| \Delta {\boldsymbol u}(k | k) \| _{R}^2 - \| {\boldsymbol \varepsilon} (k | k) \| _{\rho} ^2 \\ =\;&V^* (k)- \| {\boldsymbol \eta} (k | k)-{\boldsymbol \eta} _{\rm r} (k | k) \| _Q^2 - \| \Delta {\boldsymbol u}(k | k) \| _{R}^2 - \| {\boldsymbol \varepsilon} (k | k) \| _{\rho} ^2 \end{align} $

(35)

此外，由于$ V^* (k+1) $是成本函数的最优解，因此$ V^* (k+1)\leqslant J(k+1) $。根据式(35) 可得

$ \begin{align} V^* (k+1) \leqslant J(k+1) \leqslant V^* (k)- \| {\boldsymbol \eta} ^* (k)-{\boldsymbol \eta} _{\rm r} (k) \| _Q^2 - \| \Delta {\boldsymbol u}^* (k) \| _{R}^2 - \| {\boldsymbol \varepsilon} ^* (k) \| _{\rho} ^2 \end{align} $

(36)

因为$ \| {\boldsymbol \eta} ^* (k)-{\boldsymbol \eta} _{\rm r} (k)\| _Q^2 $、$ \| \Delta {\boldsymbol u}^* (k) \| _{R}^2 $和$ \| {\boldsymbol \varepsilon} ^* (k) \| _{\rho} ^2 $均不为负，进一步可得

$ \begin{align} V^* (k+1)\leqslant V^* (k) \end{align} $

(37)

由此，系统的稳定性得证。

通过第2节设计的转换函数(15) 即可将模型预测控制器与CPG结合起来，构建的MPC-CPG控制器的控制框图如图 5所示。模型预测控制器根据当前时刻参考轨迹与实际轨迹之间的误差计算下一时刻的控制量。转换函数将MPC输出的控制量转化成CPG参数输入到CPG网络对其进行调节，CPG输出机器人各腿部电机的控制信号。不断循环这个过程，最终完成轨迹跟踪任务。

4 仿真和实验（Simulation and experiment）

为了验证所设计的控制器的有效性，本文对机器人跟踪圆周轨迹进行了仿真和实验，仿真过程中模型预测控制器的控制量约束和控制增量约束分别为

$ \begin{equation} \begin{gathered} 0 \leqslant v \leqslant 1.5\\ \leqslant \omega \leqslant 1\\ -0.05 \leqslant \Delta v \leqslant 0.05\\ -0.05\leqslant \Delta \omega \leqslant 0.05 \end{gathered} \end{equation} $

(38)

对圆周轨迹跟踪的仿真结果如图 6所示，由图 6(a)和6(b)可知，在模型预测控制器的作用下，机器人迅速地消除了位置误差和航向角误差，在5 s后顺利跟踪上了参考轨迹，跟踪效果良好。由图 6(c)可知，位置误差始终保持在$ - $0.06~0.06 m之间，航向角误差在5 s后收敛到0。

在轨迹跟踪的仿真过程中，MPC控制器的输出量如图 7所示。由图 7可知，当机器人开始运动时，为了尽快跟踪到参考轨迹，速度和角速度比较大，但因为控制量和控制增量约束的存在，机器人的速度和角速度分别在达到1.5 m/s和1 rad/s后即停止增大。随着跟踪误差的减小，速度和角速度也减小，最终达到稳态。

转换函数将MPC控制器的输出量转换成可用于调节CPG的参数（频率$ f $和转向系数$ \delta $），$ f $和$ \delta $的变化如图 8所示。由图 8(a)可知，在5 s之前，频率较大，此时机器人快速接近参考轨迹，跟踪上参考轨迹后，频率稳定在2.2 Hz。由图 8(b)可知，转向参数始终小于45$ ^{\circ} $，这是因为机器人在轨迹跟踪过程中始终是左转运动。开始时，转向参数较小，机器人以较大的角速度左转以快速消除跟踪误差，跟踪到参考轨迹之后转向参数保持在33$ ^{\circ} $，以稳定的角速度完成轨迹跟踪。

转换函数将MPC的输出量转换成关于CPG参数的控制量对CPG的输出进行调节，CPG网络的输出如图 9所示。由图 9可知，1.5 s之前CPG网络中的各个振荡器的输出还没有形成稳定的相位差，因此信号比较紊乱。1.5 s后，各振荡器的相位差稳定下来，机器人开始跟踪上参考轨迹。开始时，振荡的频率较大，左右两侧腿的振荡幅值的差值也比较大，使得机器人能够以较大的速度和角速度快速地接近参考轨迹，进而快速地消除跟踪误差。接着，CPG的频率和左右两侧腿的振幅差值开始逐渐减小，并且最终分别稳定在2.2 Hz和9$ ^{\circ} $，使得机器人以0.5 m/s的速度和0.1 rad/s的角速度进行轨迹跟踪。

对直线轨迹跟踪的仿真结果如图 10所示。由图可知，在模型预测控制器的作用下，机器人迅速地消除了位置误差和航向角误差，在8 s后顺利跟踪上了参考轨迹，位置误差和航向角误差均收敛为0，跟踪效果良好。

MPC控制器的输出量如图 11所示。由图可知，当机器人开始运动时，速度和角速度比较大，随着跟踪误差的减小，速度和角速度也减小，最终达到稳态。

转换函数的输出量如图 12所示。由图 12(a)可知，在4 s之前，频率较大，此时机器人快速接近参考轨迹，跟踪上参考轨迹后，频率稳定在2.1 Hz。由图 12(b)可知，开始时，转向参数较小，机器人以较大的角速度左转以快速消除跟踪误差，跟踪到参考轨迹之后转向参数保持在45$ ^{\circ} $，机器人保持直线爬行。

CPG网络的输出如图 13所示。1.5 s时，各振荡器产生了稳定的相位差。与圆周轨迹跟踪类似，机器人开始运动时，振荡的频率较大，左右两侧腿的振荡幅值的差值也较大，使得机器人能够以较大的速度和角速度快速地接近参考轨迹。接着CPG的频率和左右两侧腿的振幅差逐渐减小，最终频率稳定在2.1 Hz，左右两侧腿的幅值差消除，使得机器人以0.5 m/s的速度进行直线爬行，完成轨迹跟踪。

仿真结果表明，在MPC-CPG控制器的调节下，机器人能够迅速地消除位置误差和航向角误差并快速达到稳定状态，并且在达到稳态的过程中具有良好的动态特性。为了进一步验证MPC-CPG控制器的有效性，用图 14所示的机器人样机在仿真的基础上进行实验验证。机器人具有6条机械腿，每条机械腿有3个自由度，即髋关节、膝关节和踝关节各由1个电机驱动。详细的技术参数如表 1所示。

控制模块如图 15所示，通过室内导航定位测距模块和惯性组合导航单元获取机器人的位置、航向和速度信息作为状态量实时地输入到控制器中进行运算。通过E180无线模块可以与用户计算机完成数据交互。上位机采用NVIDIA TW-TX-T300，用于运行运动控制算法。为了准确高效地控制电机，将嵌入STM32H743芯片的电机供电板与上位机连接，用来控制电机。

为了保证机器人的运动协调性，通过映射函数对CPG的输出信号进行放大或缩小，转变为电机转动的角度值，然后传递到每条腿的髋关节、膝关节和踝关节，从而实现每条腿各关节之间的协调运动，映射函数为

$ \begin{equation} \begin{aligned} & \theta_{i1} =k_0 \beta _i , \qquad \theta_{i2} = \begin{cases} k_1 \alpha _i +b_1 , & \dot {\beta} _i \geqslant 0 \\ k_2 \alpha _i +b_2 , & \dot {\beta} _i <0 \end{cases} \\ & \theta_{i3} = \begin{cases} k_3 \alpha _i +b_3 , & \dot {\beta} _i \geqslant 0 \\ k_4 \alpha _i +b_4 , & \dot {\beta} _i <0 \end{cases} \end{aligned} \end{equation} $

(39)

其中，$ \alpha _i $和$ \beta _i $为CPG网络中第$ i $个振荡器的输出信号。$ \theta_{i1} $、$ \theta_{i2} $、$ \theta_{i3} $分别表示第$ i $条机械腿髋关节、膝关节和踝关节的转角，$ i =1, 2, 3, \cdots, 6 $。调节放大因子$ k_0 $~$ k_4 $、$ b_1 $~$ b_4 $可以改变输出角度。令$ k_0 =1 $，$ k_1 =\pm 1 $，$ k_2 =0 $，$ k_3 =\pm 1 $，$ k_4 =0 $，$ b_1 = $ $ \mp 50 $，$ b_2 =\mp 50 $，$ b_3 =\pm 35 $，$ b_4 =\pm 35 $，得到各关节的转角，如图 16所示。

圆周轨迹跟踪实验过程如图 17所示。机器人在MPC-CPG控制器的控制下开始爬行，然后不断减小位置误差，5 s时跟踪上参考轨迹并且沿着参考轨迹继续爬行。最终完成对整个圆周轨迹的跟踪，在整个跟踪过程中，机器人能够顺利完成抵近并跟踪参考轨迹。机器人的圆周轨迹跟踪实验结果如图 18所示。由图可知，机器人在$ X $方向和$ Y $方向的初始位置误差分别为0.25 m和0.65 m，当机器人开始爬行后，$ X $方向和$ Y $方向的位置误差逐渐减小。5 s时，机器人跟踪上参考轨迹，此后$ X $方向和$ Y $方向的位置误差始终保持在$ - $0.1~0.1 m。通过航向角跟踪结果可以看出，在整个轨迹跟踪过程中，由于机器人的转向是依靠左右两侧腿的速度差完成的，在爬行转向时机械腿的足端与地面会产生相对滑动，因此会产生较大的航向角误差。但机器人爬行过程中的实际航向角在参考航向角附近上下波动，波动范围始终保持在$ - $25$ ^{\circ} $~20$ ^{\circ} $，说明机器人在圆周轨迹跟踪过程中具有良好的航向角跟踪性能。

直线轨迹跟踪实验过程如图 19所示。机器人5 s时跟踪上参考轨迹并且沿着参考轨迹继续爬行，最终完成对直线轨迹的跟踪，实验结果如图 20所示。由图可知，机器人在$ X $方向和$ Y $方向的初始位置误差分别为0 m和1 m，在5 s时，机器人跟踪上参考轨迹，此后$ X $方向和$ Y $方向的位置误差始终保持在$ - $0.1~0.1 m，航向角的波动范围始终保持在$ - $27$ ^{\circ} $~20$ ^{\circ} $，说明机器人在直线轨迹跟踪过程中也具有良好的航向角跟踪性能。

与仿真结果相比，当机器人跟踪上参考轨迹之后，机器人的实际运动轨迹和参考轨迹之间始终存在着较小的误差无法消除。原因是机器人在转向时两侧机械腿的速度差使足端与地面产生了相对滑动，进而产生了跟踪误差。但这个误差会在模型预测控制器的每次迭代计算过程中不断地被修正，使得误差始终保持在允许范围内。通过实验表明，机器人对圆周轨迹和直线轨迹都具有良好的跟踪效果，进一步验证了本文提出的MPC-CPG控制器的有效性。

5 结论（Conclusion）

本文基于机器人的运动学模型，建立了模型预测控制器并进行了稳定性证明，然后通过设计的换转函数将模型预测控制器与CPG结合起来，提出了适用于机器人轨迹跟踪控制的MPC-CPG控制器。MPC-CPG控制器由上层的MPC、转换函数和下层的CPG网络组成。模型预测控制器根据实际轨迹和参考轨迹的误差计算并输出控制量，转换函数将MPC的控制量转化成关于CPG参数的控制量，进而实现对CPG网络的调节，最后通过CPG网络调整机器人运动，实现机器人的轨迹跟踪。整个控制结构可根据环境变化提供稳定、适应性强和平稳的步态过渡。最后通过仿真和实验验证了MPC-CPG控制器对圆周轨迹和直线轨迹的跟踪性能，实验表明，在有初始误差的条件下，机器人能够快速地消除位置误差和航向角误差，跟踪上参考轨迹，轨迹跟踪的位置误差始终保持在$ - $0.1~0.1 m，航向角误差保持在$ - $27$ ^{\circ} $~20$ ^{\circ} $。实验验证了所提出的MPC-CPG控制器对机器人轨迹跟踪控制的有效性。