基于RANMPC的移动机器人轨迹跟踪控制

Trajectory Tracking Control of Mobile Robots Based on RANMPC

摘要: 针对两轮移动机器人轨迹跟踪中的非完整约束、外部干扰与执行器饱和问题，本文提出一种结合参数自适应NMPC (ANMPC)与扩张状态观测器(ESO)的鲁棒控制策略。该策略通过ESO对集总扰动进行前馈补偿；利用位姿误差的高阶微分特征识别异常等级，动态调整控制参数；进而融合ANMPC与ESO输出，形成复合控制律。仿真表明，在圆轨迹上本策略跟踪精度高；在存在初始误差的复合轨迹下收敛快，轨迹突变后恢复时间仅为5.5 s。与固定参数NMPC相比，横向跟踪误差降低95.83%，航向角误差减少82.62%，且无失稳。

Abstract: This paper proposes a robust control strategy that combines parameter adaptive NMPC (ANMPC) and extended state observer (ESO) to address the issues of incomplete constraints, external disturbances, and actuator saturation in trajectory tracking of two wheeled mobile robots. This strategy uses ESO to perform feedforward compensation on the lumped disturbance; Using high-order differential features of pose errors to identify anomaly levels and dynamically adjust control parameters; Further integrate the outputs of ANMPC and ESO to form a composite control law. Simulation shows that this strategy has high tracking accuracy on circular trajectories; Under the composite trajectory with initial errors, convergence is fast, and the recovery time after trajectory mutation is only 5.5 seconds. Compared with fixed parameter NMPC, the lateral tracking error is reduced by 95.83%, the heading angle error is reduced by 82.62%, without any instability.

1. 引言

目前移动机器人正因其良好的灵活性与机动性，以及具有重量轻、负载大、机构简单、驱动方便、能耗低等优点逐步于物流中心、无人工厂、智能制造产业得到广泛应用[1] [2]。

两轮移动机器人受非完整约束影响，控制器设计复杂。传统方法如PID、反演与滑模控制在处理约束、抗扰和实时优化方面均不及模型预测控制(MPC)。具体而言，PID依赖经验调参且无法显式处理约束[3]；反演法缺乏预测能力，转弯时易因曲率突变发散[4]；滑模控制存在显著抖振，自适应滑模虽可缓解却实时性差[5]-[7]。此外，传统方法常忽略执行器约束，易引发饱和或损坏[8] [9]。

MPC带来的迭代优化，可以很好地处理模型约束，动态系统等问题，有利于实现平滑运动；尤其因显式处理时域约束的能力而备受关注[10]。但其线性与非线性变体性能差异显著：线性MPC求解快速但对扰动敏感，弯道误差较大[11] [12]；非线性MPC (NMPC)虽精度更高[13]，但存在计算复杂、参数整定困难的问题[14] [15]。传统NMPC的控制参数固定，当路径曲率大或轨迹突变时，难以保证跟踪精度与控制输入平稳性[16] [17]。虽有研究基于车速[18]或模糊规则[19]调整单一参数，但MPC需多参数协同，而现有自适应方法多集中于权重优化[20]-[22]，缺乏对时域与权重参数的综合调节。

针对移动机器人系统中的外界干扰问题，研究者发展了结合扰动估计的鲁棒MPC (RMPC)。郭洋[23]采用非线性扰动观测器(DOB)进行扰动补偿，但其对模型误差敏感；Tang等[24]将广义二型模糊逻辑与MPC结合，增强了不确定性适应能力，但计算复杂、实时性差。相比之下，扩张状态观测器(ESO)通过扩张状态统一估计总扰动，无需精确模型即可实现前馈补偿，结构简单且计算高效[25]；理论分析表明其估计误差有界且可收敛，为鲁棒设计提供支撑[11]。

基于以上讨论，本文针对同时受输入约束的受未知干扰WMR跟踪问题，设计了一种基于扰动补偿的RANMPC算法以获得期望的闭环性能，本文的主要工作如下：

1) 针对传统MPC参数固定的问题，提出一种基于误差高阶动态(加速度/加加速度)的识别方法。通过构建三维异常状态机，实时感知突变并生成全局异常等级，进而自适应调整ANMPC参数，增强了控制器对扰动与工况变化的适应能力。

2) 针对补偿嵌入预测模型带来的相位滞后的问题，构建前馈–反馈复合控制律，将ANMPC的优化输出与ESO的扰动估计结合，共同作用于被控对象，实现精确的轨迹跟踪。

2. 运动学建模

本文选取的移动机器人为两轮差速驱动移动机器人，该机器人左右轮各有一个电机进行独立驱动，移动机器人前端有一个辅助轮，其作用主要为支撑与转向时保持平衡，即从动轮，移动机器人的简化模型如图1所示。

Figure 1. Simplified model of two wheeled mobile robot

图1. 两轮移动机器人简化模型

移动机器人的简化模型如图1所示，在该模型中，假设移动机器人的质心坐标位于驱动轮之间连接的中心处，即O_xy，移动机器人在参考坐标系O_xy下的位姿被描述为$q={\left[x,y,\theta \right]}^{\text{T}}$ ，其中$\theta$ 为前进方向与x坐标轴之间的夹角，v和$\omega$ 代表移动机器人的角速度与线速度，为建立轮式移动机器人模型，假设在不考虑各种干扰的理想情况下，两轮移动机器人在运动过程中车轮与地面不存在或不予考虑相对滑动，车轮只作滚动运动，则有以下约束条件：

$\dot{y}\cos \theta -\dot{x}\sin \theta =0$ (1)

将约束式(1)变形为矩阵形式，得：

$A_q^{\text{T}}\dot{q}=\left[\begin{array}{ccc}-\sin \theta & \cos \theta & 0\end{array}\right]\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]$ (2)

选择一个满秩矩阵s(q)为$A_q^{\text{T}}$ 零空间的一组基，即：

$s_q{A}_q^{\text{T}}=0$ (3)

将式(3)代入到式(2)中，并化简所列出的方程，得到运动学模型为：

$\dot{q}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$ (4)

3. 鲁棒自适应NMPC算法

为解决存在未知外部干扰以及固定参数NMPC控制器无法适应工况变化的问题，提出一种基于突变检测的鲁棒自适应NMPC在线滚动优化算法，提升系统的鲁棒性与跟踪精度，具体控制框图如图2所示。该算法的核心在于：基于标称运动学模型进行预测优化，同时利用ESO对集总扰动的实时观测值进行前馈补偿，并结合突变检测机制自适应调整优化问题的控制参数，构成一个强大的“前馈–反馈”复合控制架构，实现移动机器人在未知扰动下的高精度轨迹跟踪。

Figure 2. Robust adaptive NMPC control block diagram

图2. 鲁棒自适应NMPC控制框图

鲁棒自适应NMPC的在线滚动优化算法流程如下：首先获取系统当前状态与参考轨迹，同时ESO并行运行，实时估计集总扰动。随后，突变检测模块根据跟踪误差计算异常等级，动态调整优化问题中的控制参数，以适应系统实时状态。接着，算法构建并求解基于理想运动学模型的优化问题，其目标函数用于最小化状态跟踪误差与控制增量。求解后获得最优控制序列，将首项最优反馈控制量与ESO的前馈补偿量结合，形成复合控制指令，实现精准跟踪与主动抗扰。

3.1. 扩张状态观测器设计

本节重点介绍扩张状态观测器的设计，其目的为估计未知干扰以及系统状态的值。

定义扩张状态为${\xi }_1={\left[x,y,\theta \right]}^{\text{T}}$ ，${\xi }_2={\left[d_1\left(t\right),d_2\left(t\right),d_3\left(t\right)\right]}^{\text{T}}$ 。理想运动学模型式(4)可重构为：

$\{\begin{array}{l}{\dot{\xi }}_1=f\left({\xi }_1,u\right)+{\xi }_2\\ {\dot{\xi }}_2=h\left(t\right)\end{array}$ (5)

假设1：未知扰动${\xi }_2$ 与其时间导数$h\left(t\right)$ 有界，即：$\left|{\xi }_2\right|\le k$ 、$\left|h\left(t\right)\right|\le t$ ,其中$k$ 、$t\in ℝ\left(>0\right)$ 为一个正常数。

由于未知变量$h\left(t\right)$ 的存在，扩张状态在式(5)定义的系统下是不可观测的。因此，本文采用ESO观测移动机器人位姿与未知扰动，令$e_1={\overline{\overline{\xi }}}_1-{\xi }_1$ 、$e_2={\overline{\overline{\xi }}}_2-{\xi }_2$ 则扩张系统的ESO观测器设计为：

$\{\begin{array}{l}{\dot{\overline{\overline{\xi }}}}_1=f\left({\xi }_1,u\right)+{\overline{\overline{\xi }}}_2+{\beta }_{s1}nlf\left(e_1,\delta ,\sigma \right)\\ {\dot{\overline{\overline{\xi }}}}_2={\beta }_{s2}nlf\left(e_1,\delta ,\sigma \right)\end{array}$ (6)

式中${\overline{\overline{\xi }}}_1$ 、${\overline{\overline{\xi }}}_2$ 分别为${\xi }_1$ 、${\xi }_2$ 的估计值，${\beta }_{s1}=\text{diag}\left\{{\beta }_1,{\beta }_3,{\beta }_5\right\}$ 、${\beta }_{s2}=\text{diag}\left\{{\beta }_2,{\beta }_4,{\beta }_6\right\}$ 为扩张状态观测器的增益矩阵，其所有分量均严格大于零。$nlf\left(e_1,\delta ,\sigma \right)$ 为非线性函数，具体形式为：

$nlf\left(e_1,\delta ,\sigma \right)=\{\begin{array}{ll}{e}_1/{\delta }^{\left(1-\sigma \right)}\hfill & \left|e_1\right|\le \delta \hfill \\ {\left|e_1\right|}^{\sigma }sign\left(e_1\right)\hfill & \left|e_1\right|\ge \delta \hfill \end{array}$ (7)

将式(7)代入式(6)，可得观测系统误差动力学方程为：

$\{\begin{array}{l}{\dot{e}}_1=e_2-{\beta }_{1}nlf\left(e_1,\delta ,\sigma \right)\\ {\dot{e}}_2={\beta }_{2}nlf\left(e_1,\delta ,\sigma \right)-h\left(t\right)\end{array}$ (8)

3.2. 基于突变检测的自适应非线性模型预测控制器设计

3.2.1. 基于高阶微分特征的突变检测机制

该机制的核心思想为从系统状态误差的时间序列中提取高阶微分特征，通过同步监测横向、纵向和航向角误差，提取其加速度和加加速度特征——加速度反映“力”的突变，加加速度捕捉“力”的变化速率。该机制作为前馈环节，为后续自适应模型预测控制器提供先验信息。

(一) 构建滑动时序窗口

为了解决轨迹跟踪过程中可能存在的由时变扰动引发的轨迹畸变问题，如式(9)所示，提出了一种基于误差加速度与加加速度的实时突变检测方法，创建一个长度为4的窗口，连续储存移动机器人位置及航向误差的历史序列。

$E_{hist}\left(t\right)={\left[e_x\left(t\right),e_y\left(t\right),\cdots ,e_{\theta }\left(t\right)\right]}^{\text{T}}$ (9)

基于窗口数据，采用式(10)、(11)对每一个自由度($x,y,\theta$ )独立计算中心差分微分算子，分别提取加速度项a_i与加加速度项j_i。

$a_i=\frac{e_i\left(t_k\right)-2e_i\left(t_{k-1}\right)+e_i\left(t_{k-2}\right)}{{\left(\Delta t\right)}^2}$ (10)

$j_i=\frac{e_i\left(t_k\right)-3e_i\left(t_{k-1}\right)+3e_i\left(t_{k-2}\right)-e_i\left(t_{k-3}\right)}{{\left(\Delta t\right)}^3}$ (11)

式中$\Delta t$ 为采样时间，通过分析误差的高阶微分特征实现扰动超前检测，采用上述递推公式，可以实现每个控制周期来固定次数的算术操作，保障系统的实时性。

(二) 三维异常状态机

基于实时计算的高阶微分特征，设计一种三维异常状态机。当某个自由度的误差高阶动态超过其阈值时，系统会通过式(12)计算当前的异常等级，为参数自适应提供理论依据，降低因测量误差造成的误报风险。

$ℒ=\{\begin{array}{l}0,\text{else},正常模式\\ 1,{\lambda }_k\ge 1,一��警\\ 2,{\lambda }_k\ge 2,�急模式\end{array}$ (12)

式中，${\lambda }_k$ 为异常指标，表征系统受到外界干扰的程度，其计算公式如式(13)所示：

${\lambda }_{k+1}\{\begin{array}{l}\min \left(3,{\lambda }_k+1\right),\text{any}\left(\text{flag}=\text{TRUE}\right)\\ 0,\text{any}\left(\text{flag}=\text{FALSE}\right)\end{array}$ (13)

3.2.2. 基于状态机的NMPC控制参数调整

该机制的核心思想为从系统状态误差的时间序列中提取高阶微分特征，通过同步监测横向、纵向和航向角误差，提取其加速度和加加速度特征——加速度反映“力”的突变，加加速度捕捉“力”的变化

基于突变检测预警，驱动控制参数进行自适应调整。该机制的核心在于通过建立异常等级到控制参数的映射，实现控制器参数的实时优化。

控制器参数被视为关于异常等级$ℒ$ 的函数，其自适应调整策略由下列映射关系定义：

(1) 预测时域的自适应调整：

预测时域N_p随着异常等级的提升而动态缩，以应对模型不确定性等扰动，并保障实时性：

$N_p\left(ℒ\right)=\{\begin{array}{l}25,ℒ=0\left(正常模式\right)\\ 22,ℒ=1\left(一��警\right)\\ 18,ℒ=2\left(�急模式\right)\end{array}$ (14)

(2) 权重参数的自适应调整：

状态误差权重矩阵Q与终端惩罚矩阵P随异常等级增大而强化，以提升控制器抗干扰能力：

$Q\left(L\right)=\text{diag}\left(\{\begin{array}{l}\left[\begin{array}{ccc}3.0& 3.5& 4.0\end{array}\right],ℒ=0\\ \left[\begin{array}{ccc}3.0& 4.0& 4.0\end{array}\right],ℒ=1\\ \left[\begin{array}{ccc}3.0& 5.0& 4.0\end{array}\right],ℒ=2\end{array}\right)$ 、$P\left(ℒ\right)=\text{diag}\left(\{\begin{array}{l}\left[\begin{array}{ccc}0.15& 0.15& 0.3\end{array}\right],ℒ=0\\ \left[\begin{array}{ccc}0.25& 0.25& 0.5\end{array}\right],ℒ=1\\ \left[\begin{array}{ccc}0.7& 0.7& 1.0\end{array}\right],ℒ=2\end{array}\right)$ (15)

当$ℒ=0$ 时，终端惩罚矩阵P渐近恢复到一个预设的控制参数，其更新过程如式(16)所示：

$P^{\left(k+1\right)}=\alpha P^{\left(k\right)}+\left(1-\alpha \right)P^0$ (16)

式中，$P^{\left(k\right)}$ 为第k个采样周期的终端惩罚矩阵；$\alpha \in \left(0,1\right)$ 为恢复速率因子，用于控制恢复过程的平滑度和速度，避免参数跳变对系统造成的冲击，保障控制过程的平滑性。

(3) 控制增量约束的自适应控制：

输入约束需兼顾动力学耦合与计算效率。矩形约束无法处理速度耦合，椭圆约束因二阶锥规划求解而实时性差。本文采用菱形约束实现平衡，其1-范数可近似表征耦合且计算更高效。进一步地，通过将异常等级注入缩放矩阵(式(17)、(18))，构建了异常等级驱动的自适应菱形约束机制。

$a_{eff}=a_0{\text{e}}^{-k_vℒ}$ (17)

$b_{eff}=b_0{\text{e}}^{-k_{\omega }ℒ}$ (18)

式中$a_0$ 、$b_0$ 分别为标称约束参数(对应执行器物理限幅)，$ℒ$ 为异常等级，$k_v$ 、$k_{\omega }$ 为敏感参数。自适应机制通过指数函数建立菱形约束与异常等级之间的联系：当$ℒ=0$ 时，约束保持标称值；当$ℒ>0$ 时，随着$ℒ$ 的增长，约束边界动态紧缩，抑制轨迹发散风险。

3.2.3. 自主恢复机制(异常等级恢复机制)

本章设计的自适应NMPC控制策略包含异常等级恢复机制。恢复机制的目标为：当外部扰动减弱后，通过跟踪误差动态调节恢复速度：误差小则加速退出异常状态，误差大则保守操作以稳为主，确保系统在扰动减弱后平稳恢复。其计算公式如式(19)~式(22)所示：

1) 恢复因子的计算：

恢复因子r_f如式(19)所示，根据当前跟踪误差$\epsilon$ 动态调整：

$r_f\left(\epsilon \right)=\{\begin{array}{l}2.0,\epsilon <0.5\\ 1.0,\epsilon \ge 0.5\end{array}$ (19)

式中跟踪误差$\epsilon =\Vert x_e,y_e,1.2\left|{\theta }_e\right|\Vert$ 为当前的加权范数误差。

2) 恢复因子的计算：

$n^{\left(k+1\right)}=n^{\left(k\right)}+1$ (20)

式中n为恢复计数器，上标(k)表示第k个控制周期。

3) 衰减步长计算(自适应调整)

衰减步长$\delta$ 如式(21)所示，根据当前误差动态计算：

$\delta \left(\epsilon \right)=\min \left(0.2,0.05+0.15\left(1-\frac{\epsilon }{2.0}\right)\right)$ (21)

4) 异常等级衰减

当满足恢复条件$n^{\left(k\right)}>N_{\max }$ $\left(N_{\max }=15\right)$ 时，触发如式(23)所示的恢复机制：

$\{\begin{array}{l}\lambda \left(k+1\right)=\max \left(0,\lambda \left(k\right)-\delta \cdot r_f\right)\\ ℒ\left(k+1\right)=\max \left(0,ℒ\left(k\right)-\delta \cdot r_f\right)\\ n^{\left(k+1\right)}=0\end{array}$ (22)

注1：系统的全局异常等级$ℒ$ 为时间上的连续函数，其变化率受限，即满足：

$\Vert ℒ\left(k+1\right)-ℒ\left(k\right)\Vert \le {\Delta }_{\max }$ (23)

式中，${\Delta }_{\max }=\max \left(\delta \left(\epsilon \right)\cdot r_f\left(\epsilon \right)\right)$ 为最大单步衰减步长。

证明：

由恢复机制可知，全局异常等级的更新仅通过衰减机制式(23)进行，由于衰减步长$\delta \left(\epsilon \right)\in \left[{\delta }_{\min },{\delta }_{\max }\right]$ 和恢复因子$r_f\left(\epsilon \right)\in \left\{1.0,2.0\right\}$ 均为有界量。故异常等级$ℒ$ 在任意相邻采样时间的变化量 $\Delta ℒ=ℒ\left(k+1\right)-ℒ\left(k\right)$ 满足：$\left|\Delta ℒ\right|\le {\Delta }_{\max }$ ，式中${\Delta }_{\max }=\max \left(\delta \left(\epsilon \right)\cdot r_f\left(\epsilon \right)\right)$ 。

3.3. 控制器优化问题的表述

在每一采样时刻k，控制器基于当前状态${\xi }_1\left(k\right)$ ，求解如式(24)所示的基于运动学理想模型式(2-1)的有限时域开环优化问题，以获得最优控制序列：

$\begin{array}{l}\underset{\Delta U\left(k\right)}{\min }J\left(k\right)={\displaystyle {\sum }_{i=1}^{N_p}{\Vert e\left(k+i|k\right)\Vert }_{Q_i}^2}+{\displaystyle {\sum }_{j=0}^{N_c-1}{\Vert \Delta u\left(k+i|k\right)\Vert }_R^2}+{\Vert e\left(k+N_p|k\right)\Vert }_{P\left(ℒ\right)}^2\\ \text{s}\text{.t}.{\xi }_1\left(k+1\right)=f\left({\xi }_1\left(k\right),u\left(k\right)\right)\\ u\left(k\right)=u\left(k-1\right)+\Delta u\left(k\right)\\ u_{\min }\le u\left(k\right)\le u_{\max }\\ \Delta u\left(k\right)\in \left\{\Delta u\left(k\right)|\frac{\Delta v}{a\left(ℒ\right)}+\frac{\Delta \omega }{b\left(ℒ\right)}\le 1\right\}\end{array}$ (24)

式中：

$\Delta U\left(k\right)$ 为待优化的控制增量序列。

$e\left(k+i|k\right)$ 为预测状态与参考轨迹之间的误差。

$N_p$ 、$N_c$ 分别为预测时域与控制时域。

$Q_i$ 、R、$P\left(ℒ\right)$ 为权重与终端惩罚矩阵，其参数值由异常等级$ℒ$ 动态调节。

$a\left(ℒ\right)$ 、$b\left(ℒ\right)$ 为由异常等级$ℒ$ 动态调节的菱形约束参数。

求解上述优化问题后，得到的最优控制序列首项为$\Delta u^{*}\left(k|k\right)$ 。结合ESO实时提供的扰动估计值，最终施加于被控对象的复合控制律如式(25)所示：

$u\left(k\right)=u\left(k-1\right)+\Delta u^{*}\left(k|k\right)-\overline{\overline{d}}\left(k\right)$ (25)

为清晰表述整个算法的在线滚动优化过程，现将算法的核心步骤总结如表1所示：

Table 1. Robust NMPC rolling optimization algorithm process

表1. 鲁棒NMPC滚动优化算法流程

步骤	操作内容	备注
1	初始化 设置采样周期T，初始化控制参数。	离线执行。为算法运行提供初始参数配置
2	在线循环启动 在每个采样时刻，启动新一轮优化。	固定采样频率循环执行
3	状态获取与扰动估计 a) 获取系统当前状态。 b) 从ESO读取当前扰动估计值。	由上一采样时刻 状态计算所得
4	参数自适应调整 根据当前时刻跟踪误差计算异常等级，自适应调整参数	通过突变检测模块 实现参数自适应
5	求解优化问题 以当前状态为初始条件，通过求解问题1，得到最优控制序列。	优化问题通常转化为数值优化问题进行求解。只需执行序列中的第一个元素
6	计算并施加复合控制律 结合ESO提供的实时扰动估计值与标称ANMPC计算出的最优控制律。	前馈–反馈复合控制：ESO提供的估计扰动用于前馈补偿，是提升鲁棒性的关键。
7	滚动更新 等待下一个采样时刻	算法循环执行，实现基于最新信息的闭环反馈控制。

4. 系统理论特性

在本章中，我们将给出扩张状态观测器的稳定性证明、模型预测控制优化问题的递归可行性和闭环稳定性等主要理论性质，并给出保证模型预测控制优化问题递归可行的充分条件。

4.1. 扩张状态观测器稳定性证明

定理4.1：考虑假设1的扩张状态系统(26)与扩张状态观测器(27)，当增益矩阵${\beta }_{s1}$ 、${\beta }_{s2}$ 满足条件：${\lambda }_{\min }\left({\beta }_{s1}\right)>\frac{\Vert {\beta }_{s2}\Vert }{2\gamma },\gamma >0$ 时，观测误差$e={\left[e_1^{\text{T}},e_2^{\text{T}}\right]}^{\text{T}}$ 一致最终有界，且收敛半径为：$\underset{t\to \infty }{\lim }\sup \Vert e\left(t\right)\Vert \le \sqrt{\frac{c_2{\iota }^2}{c_1}}$ 。

引理4.1：Young不等式：对于$\forall \delta >0$ ，有

向量a、b满足：$a^{\text{T}}b\le \frac{{\Vert a\Vert }^2}{2\delta }+\frac{\delta {\Vert b\Vert }^2}{2}$ 。

证明：

构造李雅普诺夫函数为：$V\left(e\right)=\frac{1}{2}{e}_1^{\text{T}}{e}_1+\frac{1}{2\gamma }{\left(e_2+{\beta }_{s1}nlf\right)}^{\text{T}}\left(e_2+{\beta }_{s1}nlf\right)$ ，对其求时间导数得：

$\dot{V}=e_1^{\text{T}}{\dot{e}}_1+\frac{1}{\gamma }{\left(e_2+{\beta }_{s1}nlf\right)}^{\text{T}}\left({\dot{e}}_2+{\beta }_{s1}\frac{\partial nlf}{\partial e_1}{\dot{e}}_1\right)$ (26)

代入观测系统误差方程得：

$\dot{V}=e_1^{\text{T}}\left(e_2-{\beta }_{s1}nlf\right)+\frac{1}{\gamma }{\left(e_2+{\beta }_{s1}nlf\right)}^{\text{T}}\left({\beta }_{s2}nlf-h\left(t\right)+{\beta }_{s1}\frac{\partial nlf}{\partial e_1}\left(e_2-{\beta }_{s1}nlf\right)\right)$ (27)

根据假设1：$\left|h\left(t\right)\right|\le \iota$ ，由引理4.1：

${\left(e_2+{\beta }_{s1}nlf\right)}^{\text{T}}h\left(t\right)\le \frac{1}{2ϵ}{\Vert e_2+{\beta }_{s1}nlf\Vert }^2+\frac{ϵ}{2}{\iota }^2$ (28)

将式(28)代入式(27)，整理得：

$\dot{V}\le -\left[{\lambda }_{\min }\left(Q\right)+\frac{1}{2ϵ\gamma }\right]{\Vert e_2+{\beta }_{s1}nlf\Vert }^2-\frac{ϵ}{2\gamma }{\iota }^2+耦合�$ (29)

式中$Q=-\frac{{\beta }_{s1}}{\gamma }\frac{\partial nlf}{\partial e_1}-\frac{1}{2\eta \gamma }$ ，并利用非线性函 nlf的Lipschitz性质(确保耦合项可被吸收)，选择${\beta }_{s1}$ 、${\beta }_{s2}$ 满足${\lambda }_{\min }\left(Q\right)>-\frac{1}{2ϵ\gamma },k_1>0$ ，则：

$\dot{V}\le -c_{1}V+c_2{\iota }^2$ (30)

且观测误差满足：

$\underset{t\to \infty }{\lim }\sup \Vert e\left(t\right)\Vert \le \sqrt{\frac{c_2{\iota }^2}{c_1}}$ (31)

4.2 模型预测优化问题的递归可行性证明

定理4.2：对于存在外界扰动及输入约束的两轮移动机器人轨迹跟踪系统，采用基于异常等级的自适应非线性模型预测控制器时，若满足：

1) k时刻优化问题P_k存在可行解；

2) 异常等级$ℒ$ 满足连续性条件$\left|{ℒ}_{\left(k+1\right)}-{ℒ}_k\right|\le \Delta {ℒ}_{\max }$ ；

3) 终端控制器$u_f$ 满足菱形约束$\Vert D\left({ℒ}_{\left(k+1\right)}\right)u_f\Vert \le 1$ ；

则k+1时刻优化问题$P_{\left(k+1\right)}$ 必定存在可行解，且候选解满足k+1时刻所有约束条件，其中：

$\left(ℒ\right)=\text{diag}\left(1/a_{eff}\left(ℒ\right),1/b_{eff}\left(ℒ\right)\right)$

$a_{eff}\left(L\right)=a_0{\text{e}}^{-k_vℒ}$

$b_{eff}\left(L\right)=b_0{\text{e}}^{-k_{\omega }ℒ}$

引理4.2a：(异常等级连续性条件)$\left|{ℒ}_{\left(k+1\right)}-{ℒ}_k\right|\le k\Delta e_{\max }+u$ ，其连续性由注1保证：

$\Delta e_{\max }=\underset{i\in \left\{x,y,\theta \right\}}{\max }\left|e_i\left(k+1\right)-e_i\left(k\right)\right|$

引理4.2b：(约束参数Lipschitz连续)$\Vert a_{eff}{ℒ}_{\left(k+1\right)}-a_{eff}{ℒ}_{\left(k\right)}\Vert \le {ℒ}_a\left|{ℒ}_{\left(k+1\right)}-{ℒ}_k\right|$

$\Vert b_{eff}\left({ℒ}_{k+1}\right)-b_{eff}\left(L_k\right)\Vert \le L_b\left|{ℒ}_{k+1}-L_k\right|$

引理4.2c：(终端控制器可行性) 对$\forall x\in {\chi }_f$ ，存在终端控制器$u_f=Kx$ 满足：

$\frac{\left|v_f\right|}{a_{eff}\left(L\right)}+\frac{\left|{\omega }_f\right|}{b_{eff}\left(L\right)}\le \text{1}-ϵ$

证明：

(1) 构造候选解：

取k时刻最优解的后$N_p-1$ 项，补终端控制项，即：

$U\left(k+1\right)=\left[\Delta u^{*}\left(k+1|k\right),\cdots ,\Delta u^{*}\left(k+N_p-1|k\right),\Delta u_f\right]$

式中$\Delta u_f$ 为终端控制器增量满足($\Vert D\left({ℒ}_{\left(k+1\right)}\right)\Delta u_f\Vert \le 1$ )。

(2) 扰动补偿下状态误差的偏差分析：

实际闭环系统如式(32)所示受扰动影响：

$q\left(k+1\right)=f\left(q\left(k\right),u\left(k\right)\right)+d\left(k\right)$ (32)

式中$u\left(k\right)=u_{mpc}^{*}\left(k\right)-\overline{\overline{d}}\left(k\right)$

由定理4.1可知，ESO估计误差有界，即：

$\Vert d\left(k\right)-\overline{\overline{d}}\left(k\right)\Vert \le {ϵ}_r$ (33)

而由引理4.2a，异常等级变化不会造成控制参数的突变，因此，实际状态与理想预测状态的偏差为：

$\Vert q\left(k+1\right)-q^{*}\left(k+1|k\right)\Vert \le {ϵ}_l$ (34)

(3) 约束可行性验证：

状态约束：由引理4.2c因状态偏差有界，因此状态约束的可行性依赖于终端域的紧缩设计，由于终端域充分紧缩，终端状态$q\left(k+N_p|k\right)$ 仍位于紧缩后的终端域${\chi }_f\left({ℒ}_k\right)$ ；

输入约束：候选解需满足动态调整后的菱形约束：

${\Vert D\left({ℒ}_{k+1}\right)u_j\Vert }_1\le 1,j=k+1,\cdots ,k+N_c$ (35)

代入引理4.2b与引理4.2c得：

${\Vert D\left({ℒ}_{k+1}\right)u^{*}\left(j|k\right)-D\left({ℒ}_k\right)u^{*}\left(j|k\right)\Vert }_1\le L_D\Delta {ℒ}_{\max }$ (36)

式中$L_D$ 为异常等级的连续利普希茨常数。

通过预设约束紧缩满足：

${\Vert D\left({ℒ}_k\right)u^{*}\left(j|k\right)\Vert }_1\le 1-\alpha \Delta {ℒ}_{\max }$

代入式(34)，得：

${\Vert D\left({ℒ}_{k+1}\right)u^{*}\left(j|k\right)\Vert }_1\le 1-\alpha \Delta {ℒ}_{\max }+L_D\Delta {ℒ}_{\max }\le 1$ (37)

当选取紧缩参数$\alpha$ 满足$\alpha \ge L_D$ 时式(3-10)成立。

4.3. 闭环稳定性证明

定理4.3：在采用基于扩张状态观测器(ESO)的扰动补偿控制律：$\left[\left[\begin{array}{c}{v}_c\\ {\omega }_c\end{array}\right]=\left[\begin{array}{c}{v}^{*}\\ {\omega }^{*}\end{array}\right]-\left[\begin{array}{c}\overline{\overline{d_1}}\\ \overline{\overline{d_3}}\end{array}\right]\right]$ ，其中$\overline{\overline{d_1}}={\left[\begin{array}{cc}\overline{\overline{d_1}}& \overline{\overline{d_2}}\end{array}\right]}^{\text{T}}$、$\overline{\overline{d_3}}$ 为扩张状态观测器估计出的扰动值，及终端控制器$u_f=Kx$ 的条件下，若满足：

引理4.3a：扩张状态观测器满足定理4.1，即扰动估计误差有界$\Vert e\left(t\right)\Vert \le {ϵ}_r$ ，式中${ϵ}_r=\sqrt{\frac{c_2{\iota }^2}{c_1}}$ ；

引理4.3b：终端约束矩阵P和终端控制器增益K满足矩阵不等式：

${\left(A+BK\right)}^{\text{T}}P\left(A+BK\right)-P\le -Q-K^{\text{T}}RK$

则控制系统满足：

1) 全局指数收敛性：位姿误差e指数收敛至有界闭球：

$\Vert e\left(k\right)\Vert \le a{\text{e}}^{-\beta k}+\gamma {ϵ}_r$

2) 扰动不破坏稳定性：ESO补偿作为前馈通道，不改变标称NMPC预测系统稳定性。

证明：

(1) 定义李雅普诺夫函数：

取标称NMPC的代价函数为李雅普诺夫函数，即：

$V^{*}\left(k\right)=\underset{\Delta U}{\min }J\left(k\right)={\displaystyle \sum }_{i=0}^{N_p-1}l\left(x_{k+i},u_{k+i}\right)+V_f\left(x_{k+N_p}\right)$

(2) 证明$\Delta V^{*}<0$ ：

$\Delta V^{*}=-ı\left(x_k,u_k\right)+ı\left(x_{k+N_P},u_{k+N_P}\right)+V_f\left(x_{k+N_P+1}\right)-V_f\left(x_{k+N_P}\right)$ (38)

由引理4.3b，得：

$V_f\left(x_{k+N_p+1}\right)-V_f\left(x_{k+N_p}\right)\le -l\left(x_{k+N_p},u_f\right)$ (39)

将式(39)代入式(38)，化简得：

$\Delta V^{*}\le -l\left(x_{k+N_p},u_{mpc}^{*}\left(k\right)\right)<0$ (40)

由式(34)及引理4.3a得，实际状态与理想预测状态的偏差有界，因$V^{*}$ 连续，则实际代价函数满足：

$\left|J\left(k\right)-V^{*}\left(k\right)\right|\le \gamma {ϵ}_r$ (41)

综上所述，由式(40)驱动理想运动状态收敛至平衡点，而实际状态与理想预测状态的偏差有界，$\Delta V^{*}<0$ 迫使实际状态进入终端域$x_f$ ，终端域内由引理4.3b保证渐近稳定性。

5. 仿真与实验结果

为评估本文提出的RANMPC控制策略的性能，本章将进行两组仿真工况，分别为理想工况与扰动工况，其中理想工况主要跟踪两条参考轨迹，分别为复合轨迹与圆轨迹；扰动工况在复合轨迹的基础上依次加入了位置型扰动与速度型扰动。采用固定参数的非线性模型预测控制策略与本文提出的控制策略进行对比，如表2所示，非线性模型预测控制策略的控制参数与本文提出的自适应非线性模型预测控制策略中正常模式下的控制参数一致。

Table 2. Fixed parameter NMPC control parameters

表2. 固定参数NMPC控制参数

参数	数值
预测时域$N_P$	25
控制时域$N_c$	3
控制周期T(s)	0.05
纵向加速度约束(m/s^2)	−1~1
航向角加速度约束(m/s^2)	−1.4~1.4
状态量权重矩阵Q	$\left[\begin{array}{ccc}3.0& 0& 0\\ 0& 3.5& 0\\ 0& 0& 4.0\end{array}\right]$
控制量权重矩阵R	$\left[\begin{array}{cc}0.01& 0\\ 0& 0.01\end{array}\right]$

5.1. 理想工况

5.1.1. 复合轨迹

为全面评估控制器在复合运动下的跟踪性能与鲁棒性，本文设计了一条如式(42)所示的复合轨迹。该轨迹能模拟移动机器人在实际应用中常见的直行、转弯等连续运动，其轨迹切换点可以检验控制器的动态响应与平滑过渡能力。

$r_r\left(t\right)=\{\begin{array}{l}{r}_{水平}\left(t\right),0\le t<10\text{s}\\ r_{�弧}\left(t\right),10\text{s}\le t<25\text{s}\\ r_{�直}\left(t\right),25\text{s}\le t\le 55\text{s}\end{array}$ (42)

式中$r_r\left(t\right)={\left[x_r\left(t\right),y_r\left(t\right),{\theta }_r\left(t\right)\right]}^{\text{T}}$ 分别表示参考位置的x坐标、y坐标和航向角。

分段轨迹数学模型如式(43)~(45)所示：

$r_{水平}\left(t\right)=\{\begin{array}{l}{x}_r\left(i\right)=i\cdot T\\ y_r\left(i\right)=2.0\\ {\theta }_r\left(i\right)=0\end{array},\text{for}i=1:200$ (43)

该阶段移动机器人保持恒定高度，沿x轴以1m/s的速度匀速运动。

$r_{�弧}\left(t\right)=\{\begin{array}{l}{x}_r\left(i\right)=10+10\cos \left({\theta }_k\right)\\ y_r\left(i\right)=10+10\sin \left({\theta }_k\right)\\ {\theta }_r\left(i\right)={\theta }_k+\pi /2\end{array},\text{for}i=201:500$ (44)

该阶段移动机器人跟踪的轨迹为以(10,12)为圆心，10为半径的1/4圆弧曲线，式中$k=i-200$ ，航向角随角度变化，确保与轨迹切线方向一致。

$r_{�直}\left(t\right)=\{\begin{array}{l}{x}_r\left(i\right)=20.0\\ y_r\left(i\right)=12+\left(i-500\right)\cdot T\\ {\theta }_r\left(i\right)=\pi /2\end{array}$ (45)

该阶段移动机器人沿y以1 m/s的速度匀速运动，横向位置与航向角保持固定。

为验证所提ANMPC控制器的收敛性及动态性能，仿真实验设置了初始误差。图3、图4分别展示了两种初始误差工况下移动机器人的跟踪效果和跟踪误差。

a) $\left[x_e^0,y_e^0,{\theta }_e^0\right]=\left[0,0,0\right]$ b) $\left[x_e^0,y_e^0,{\theta }_e^0\right]=\left[0.1,1,0\right]$

Figure 3. Comparison of complex trajectory tracking effects

图3. 复杂轨迹跟踪效果对比

(a) $\left[x_e^0,y_e^0,{\theta }_e^0\right]=\left[0,0,0\right]$ (b) $\left[x_e^0,y_e^0,{\theta }_e^0\right]=\left[0.1,1,0\right]$

Figure 4. Comparison of complex trajectory tracking errors

图4. 复杂轨迹跟踪误差对比

实验结果揭示了两种控制器截然不同的动态响应特性：

1) 自适应NMPC展现出主动、快速的调节能力：在初始横向误差1 m的极端工况下，通过参数调整机制，使误差在5 s内收敛至0.02 m以内，收敛速度较固定参数方案提速了39%，其X方向0.147 m超调与航向角26.6˚峰值是控制器为优先抑制横向失稳风险(当Y误差 > 0.5 m时)而主动付出的可控代价，确保了轨迹紧密贴合参考路径。

2) 固定参数NMPC表现为保守与迟缓：由于恒定增益与刚性约束，其在相同初始误差下调节时间达8.2 s，且因参数固定，在轨迹末端产生了0.28 m的累积误差，动态性能不足。

5.1.2. 圆轨迹

为量化控制器在稳态周期运动下的性能，本文采用标准圆形轨迹这一典型场景进行评估。验将参考轨迹的圆心设定于(10, 10)，半径设置8 m，并为机器人施加1 m/s的恒定线速度指令。图5、图6分别展示了移动机器人在圆轨迹下的跟踪效果和跟踪误差对比。

Figure 5. Comparison of circular trajectory tracking effects

图5. 圆轨迹跟踪效果对比

Figure 6. Comparison of circular trajectory tracking errors

图6. 圆轨迹跟踪误差对比

基于圆轨迹跟踪实验数据，自适应NMPC较固定参数NMPC展现出了显著优势：横向控制精度全面提升，均方根误差降低10.96%，最大误差锐减57.67%，平均误差优化4.67%，弧线区域自适应NMPC更贴合参考轨迹，虽然x、$\theta$ 方向存在小幅波动，但自适应机制通过在线调整有效补偿了系统非线性，体现了更强的鲁棒性和控制稳定性。

在理想轨迹跟踪场景下，自适应NMPC展现出显著优势：复合机动轨迹中，面对初始Y方向1 m误差时，通过突变检测机制动态调整参数，5秒内收敛至0.03 m以内，较固定参数NMPC提速39%；虽允许X方向0.147 m瞬时超调作为稳定性代价，但全程无失稳现象；验证了其“感知–决策–执行”闭环的鲁棒性。

5.2. 扰动工况

5.2.1. 位置突变工况

为了进一步验证本文所提自适应NMPC控制器在应对突发外部干扰时的鲁棒性能与动态响应能力，我们在上述理想工况第一种参考轨迹跟踪实验的基础上，引入了如式(46)所述高斯扰动测试。用以模拟移动机器人可能面临的突发状况，如突如其来的侧向风力、路面不平造成的颠簸等。

$x_{pert}\left(t\right)=A\exp \left(-\frac{{\left(t-35\right)}^2}{{\sigma }^2}\right)$ (46)

式中$A=2.5\text{m}$ 为扰动幅值，$\sigma =1.25$ 控制扰动宽度。扰动后的横坐标x为：$x_r\left(t\right)=20.0-x_{pert}\left(t\right)$ 。图7、图8分别展示了移动机器人面临位置扰动的情况下的跟踪效果和跟踪误差。

在位置突变扰动工况中，自适应NMPC展现出显著的鲁棒性优势。如图8所示，扰动时段(35~40 s)自适应方案(红色曲线)的Y方向误差峰值仅0.2376 m，而固定参数方案(绿色曲线)飙升至9.1546 m，轨迹完全失控。性能方面，位置突变扰动下X/Y方向RMSE分别降低51.3%和97.0%，虽然航向误差积分(IAE)因瞬时尖峰被放大，但通过后续控制快速收敛，自适应NMPC的扰动后恢复时间为5.5 s，确保全程平均航向偏差仅4.2˚。

Figure 7. Comparison of tracking effects under positional mutation

图7. 位置突变下跟踪效果对比

Figure 8. Comparison of tracking errors under positional mutation

图8. 位置突变下跟踪误差对比

5.2.2. 时变打滑干扰

为了进一步验证本文提出的自适应NMPC控制策略在应对瞬态、时变运动学扰动方面的能力，在轨迹突变的工况下，于4~8 s的时段内引入了一个如式(47)所示的模拟轮胎打滑扰动。该扰动模型通过函数$S\left(v,\omega \right)$ 其值域设计在$\left[S_{\min },S_{\max }\right]$ 的范围内，从而更真实地模拟了打滑扰动从轻微到剧烈的连续动态过程。图9、图10分别展示了移动机器人面临位置扰动和打滑干扰的情况下的跟踪效果和跟踪误差，图10展示了扰动期间实际扰动值与ESO估计的扰动误差示意图。

$d=K\cdot \mathrm{sgn}\left(v\right)\cdot S\left(v,\omega \right)$ (47)

式中，K为打滑强度，$S\left(v,\omega \right)$ 为与速度有关的函数。

Figure 9. Comparison of tracking effects under slip interference

图9. 打滑干扰下跟踪效果对比

Figure 10. Comparison of tracking errors under slip interference

图10. 打滑干扰下跟踪误差对比

由图9可以得出，即使10期间x方向的扰动达到了0.38 m/s的较大扰动值，但在本文提出的基于扩张状态观测器的扰动补偿机制下，如图所示的两种控制器都能较好的贴合参考轨迹，x方向误差均被成功抑制在0.15 m的范围内，且未出现发散或失稳现象。在此基础上，图10的放大图进一步揭示了两种控制器的性能差异。在扰动期间，自适应NMPC、的误差波动幅度和稳态误差均显著小于固定参数NMPC。

6. 结论

本文提出一种基于突变检测的鲁棒自适应NMPC策略，用于解决两轮移动机器人在非完整约束、未知干扰和执行器饱和下的轨迹跟踪问题。通过建立运动学模型，结合ESO进行扰动前馈补偿，并利用突变检测机制自适应调整控制参数，构建前馈–反馈复合控制律。仿真结果表明，该策略在初始大误差下收敛速度提升39%，在轨迹突变后5.5 s内恢复跟踪，且未出现失稳。未来将探索突变检测与强化学习的结合以增强扰动泛化检测能力，并引入预瞄算法提前感知曲率变化，进一步抑制惯性超调。