差动机器人已经成为采矿勘探、环境监测、军事侦察以及现代农业等应用场景中的重要工具. 路径规划和跟踪控制作为差动机器人的2项核心技术^[1]，受到了广泛重视. 路径规划旨在利用环境信息，生成从起点至终点的最优连接路径^[2]，而跟踪控制需要在多约束条件下，尽可能地减小实际轨迹与参考路径之间的偏差^[3].

路径规划领域中常用的算法包括：基于采样的快速扩展随机树（rapidly-exploring random tree, RRT）、基于图搜索的A^*算法、基于仿生优化的蚁群算法等^[4-6]. 其中，A^*算法因其结构简单、鲁棒性强且具有较高的搜索效率，被广泛应用于差动机器人的路径规划. 然而，其生成的路径存在较多拐点，难以满足机器人运动学约束，须对路径进行平滑处理. 常用的平滑算法包括贝塞尔曲线、B样条曲线^[7]、多项式曲线；此外，已有研究还采用圆弧、杜宾曲线和回旋曲线等特殊平滑方法^[8]. B样条曲线凭借其局部可调性和较高的灵活性，已成为路径平滑研究中的常用方法^[9].

针对路径跟踪控制问题，现有研究主要集中于传统控制方法、模型预测控制（model predictive control, MPC）方法以及基于学习的控制方法. 在传统控制方法中，学者们较为常用的方法包括PID控制、滑模控制、纯跟踪控制^[10]等，这些方法均能够实现路径跟踪控制. 然而，传统控制方法在处理多约束条件和强非线性系统方面存在一定局限. 新兴的强化学习^[11]、神经网络^[12]等方法虽然展现出较强的潜力，但是面临着较高的学习与训练成本，系统的可靠性也有待提升. 与其它控制策略相比，MPC策略因其在系统约束处理、多变量优化问题方面的卓越性能，以及独特的滚动优化机制，在路径跟踪控制领域中得到了广泛应用^[13]. MPC包括单参考点的线性MPC和多参考点的非线性MPC这2种^[14]. 单参考点的线性MPC通过线性化系统模型大幅降低了计算量，其实时性较好，但是控制精度不佳^[15]. 而多参考点的非线性MPC能够充分利用参考路径的先验信息，与单参考点MPC相比，其控制精度更高^[16-17].

A^*算法和多参考点MPC（multi-reference point MPC, M-MPC）是性能较好的路径规划和跟踪控制方法，但是两者间的结合仍然面临着一些挑战. 具体来说，A^*算法生成的路径存在拐点多、不平滑以及点间距不均匀等问题，难以被M-MPC控制器直接利用. 这一问题的根源在于：控制器以固定采样时间运行，并依赖于等时间间隔的预测状态来进行滚动优化，而机器人行驶速度的变化幅度不大，单个预测周期内的预测状态所形成的轨迹点间隔可以被视为恒定值，因此，不规则的参考路径点间距与等间隔的预测轨迹点之间产生了根本性的不匹配问题. 这种失配会导致控制器内部的成本函数评估不准确，从而影响路径跟踪精确性，最终会引发控制振荡，甚至控制失败. 针对该失配问题，现有研究将A^*算法与M-MPC结合，以改善轨迹平滑性与泊车性能^[18-19]，但是这些研究主要侧重于A^*路径的几何平滑，尚未深入分析A^*规划路径的点间距不均匀性对M-MPC控制精度的影响. 此外，路径规划和跟踪控制需要解决规划路径方向和起始航向不一致的问题. 对于乘用车或类车机器人等不具备原地转向能力的系统，通常在路径规划阶段引入初始航向约束以生成满足初始姿态连续性的可行轨迹^[20-21]，而对于差动机器人，在控制起步阶段直接进行航向对齐会更为高效.

针对上述问题，提出融合B样条曲线平滑和等弧长间距离散化处理的A^*路径规划方法，以生成平滑、连续、点间距一致且包含航向信息的参考路径，并将其作为M-MPC控制器的输入，以提升控制精确性和平顺性. 此外，针对路径跟踪初始阶段存在的初始航向偏差，设计初始航向对齐机制，以确保规划层与控制层的平稳衔接.

差动机器人的规划和控制采用分层架构，总体框架如图1所示. 首先，定位层通过GPS/GNSS与实时动态定位/惯性测量单元（RTK/IMU）获取差动机器人的高精度位姿信息. 其次，在坐标转换层中，里程计（Odometry）融合传感器的原始数据并结合差动机器人运动学模型，构建机器人的连续状态估计模型，用于提供高频的位姿信息. 随后，完成通用横轴墨卡托（universal transverse Mercator, UTM）坐标系与车身坐标系之间的转换. 在此基础上，规划层生成全局路径，并以此为参考，完成局部平滑后从起始点到目标点的最优路径选择. 在生成连续、平滑且满足机器人运动学约束的路径后，再进行等弧长间距离散化处理，使每个参考路径点之间的距离保持一致. 为了确保规划层与跟踪控制层之间的平稳衔接，设计初始航向对齐机制. 跟踪控制层接收均匀离散后的参考路径以及机器人的当前状态信息，通过TF变换将全局坐标系下的参考路径映射至车身坐标系，并基于M-MPC实时求解出满足系统约束的角速度、线速度控制指令. 底盘执行层将控制指令转换为左、右履带的目标转速，最后通过控制器局域网(controller area network, CAN)总线，将十进制的转速信息转换为数据帧信息，并将其下发至电机驱动系统，实现对差动机器人行驶状态的闭环控制和路径平稳跟踪.

A^*算法是基于邻域搜索和代价函数的最优路径搜索算法^[22]，其节点扩展策略可以表述为

(1)$ f(n)=g(n)+h(n). $

式中：$ n $为当前节点，$ f(n) $为当前节点的估价函数，$ g(n) $为从起始点到当前节点$ \,n $的精确累积成本，$ h(n) $为当前节点$ n $到目标点的启发式估计成本. $ g(n) $和$ h(n) $共同决定了A^*算法的搜索效率.

在A^*算法中，启发函数一般由曼哈顿距离、欧几里得距离或切比雪夫距离等方式进行估算^[23]. 选择曼哈顿距离作为启发函数，其计算公式为

(2)$ h(n)=\left| {x}_{\mathrm{t}}-{x}_{n}\right|+\left| {y}_{\mathrm{t}}-{y}_{n}\right| . $

式中：$ {x}_{n}、{y}_{n} $分别为待计算节点$ n $的横、纵坐标，$ {x}_{\mathrm{t}}、 {y}_{\mathrm{t}} $分别为目标所在节点的横、纵坐标.

根据上述节点扩展策略和距离估算方法，A^*算法在栅格地图的基础上定义起始点与终止点. 随后，在搜索过程中遵循节点之间$ f(n) $值最小的原则，向终点进行定向搜索.

由于A^*算法生成的路径存在拐点多和曲率突变等问题，整体连续性较差，难以满足差动机器人平稳跟踪的要求，须进行平滑处理. 图2展示了不同次数B样条曲线的平滑情况. 由图2可见，三次B样条既能够保证良好的平滑效果，又可以避免路径过于靠近障碍物. 同时，三次B样条具有连续的二阶导数特性，使得机器人能够平滑地跟踪参考路径，保证了路径的易行性^[24]. 此外，准均匀B样条的节点分布均匀，基函数在各节点区间内形式统一，计算简洁且效率较高^[25]. 因此，选用三次准均匀B样条进行平滑处理.

根据B样条曲线的定义，通过控制点集合的线性组合来构建平滑路径曲线. B样条曲线的控制顶点$ {\boldsymbol{P}}_{i} $由最小二乘法拟合得到，以满足路径端点和切向约束. 所构建的平滑路径可以表示为^[26]

(3)$ \boldsymbol{C}(u)=\sum\limits_{i=0}^{n}{\boldsymbol{P}}_{i}{N}_{i,k}(u). $

式中：$ \boldsymbol{C}(u) $为B样条曲线；$ {\boldsymbol{P}}_{i} $为控制顶点，其中$ i=0,1,\cdots ,n $；$ {N}_{i,k}(u) $为$ k $次规范B样条基函数，且$ k\geqslant 0 $；$ u $为B样条曲线的自变量，且$ u\in [0,\;1.0] $.

依据de Boor-Cox递推公式计算B样条基函数$ {N}_{i,k}(u) $. 当$ k=0 $时，B样条基函数的计算形式为

(4)$ {N}_{i,0}(u)= \left\{\begin{array}{ll} 1,& {u}_{i}\leqslant u \lt {u}_{i+1};\\0,& {\text{其他}}.\end{array}\right. $

式中：$ {u}_{i} $为节点矢量V中的第$ i $个节点. 当$ k \gt 0 $时，通过递推计算得到B样条基函数：

(5)$ {N}_{i,k}(u)=\frac{u-{u}_{i}}{{u}_{i+k}-{u}_{i}}{N}_{i,k-1}(u)+\frac{{u}_{i+k+1}-u}{{u}_{i+k+1}-{u}_{i+1}}{N}_{i+1,k-1}(u). $

在上式中，定义$ 0/0=0 $.

三次准均匀B样条的节点矢量构造策略的目标在于，确保内部节点均匀分布且具有与曲线阶数相同的重数，而曲线始、末端点的行为由首、尾节点的重数来精确控制. 因此，对于具有$ n $个控制点的$ k $次准均匀B样条曲线，其节点矢量两端各有$ k+1 $个重复的0和1，且长度为$ n+k+2 $. 该节点矢量表示为

(6)$ \boldsymbol{V}=\left[\underset{k+1}{\underbrace{0,\cdots ,0} },{u}_{k+1},{u}_{k+2},\cdots ,{u}_{n},\underset{k+1}{\underbrace{1,\cdots ,1} }\right]. $

节点计算公式为

(7)$ {u}_{i}=\begin{cases} 0, & i\leqslant k;\\{u}_{i-1}+1/\left(n-k\right), &k<i\leqslant n ;\\1, &n<i\leqslant n+k+1 .\end{cases} $

为了满足M-MPC控制器的输入需求，基于三次准均匀B样条并结合线性插值，以实现对路径的等弧长间距离散化. 根据式(3)定义的路径$ \boldsymbol{C}(u) $，将其弧长$ L(u) $定义为从起始点到当前点的路径长度：

(8)$ L(u)=\int_{0}^{u}||\boldsymbol{C}'(\tau )||\mathrm{d}\tau . $

式中：$ \boldsymbol{C}'(\tau ) $为B样条曲线对$ \tau $的一阶导数，表示曲线在$ x $和$ y $方向上的瞬时变化率，即曲线的切向量；$ \tau $为积分变量.

由于式(8)中的积分部分难以获得解析解，且为了满足实时性要求，采用数值方法进行弧长近似操作. 在B样条曲线参数域$ [0,\;1.0] $内，以固定步长$ \Delta u $对路径进行密集采样，得到一系列中间点$ {\boldsymbol{P}}_{k} $：

(9)$ {\boldsymbol{P}}_{k}=\boldsymbol{C}({u}_{k}). $

式中：$ {u}_{k}=k\Delta u $，其中$ k\in \{0,1,\cdots ,{N}_{\text{s}}-1\} $为采样点索引，$ {N}_{\text{s}} $为采样点总数，$ \Delta u $为固定参数采样步长，即每次采样参数u的增量.

相邻中间点之间的弧长$ \Delta {L}_{k} $可以近似为欧几里得距离：

(10)$ \Delta {L}_{k}\approx \;||{\boldsymbol{P}}_{k+1}-{\boldsymbol{P}}_{k}||. $

对式(10)计算得到的微小弧长进行累加，即可得到每个中间点对应的累积弧长：

(11)$ {L}_{k}=\sum\limits_{j=0}^{k-1}\Delta {L}_{j}. $

式中：$ {L}_{0}=0 $.

为了获得平滑插值曲线的离散点序列，使其累积弧长为$ {L}_{\mathrm{d}} $，设定等弧长离散间隔为$ \Delta {L}_{\mathrm{dis}} $. 对于每个目标弧长$ {L}_{\mathrm{d}} $，在由式(11)累加得到的弧长序列$ \{{L}_{k}\} $中，找到满足$ {L}_{k}\leqslant {L}_{\mathrm{d}} \lt {L}_{k+1} $的区间$ [{L}_{k},{L}_{k+1}] $，目标点位于中间点$ {\boldsymbol{P}}_{k} $和$ {\boldsymbol{P}}_{k+1} $之间. 插值系数$ \alpha $为

(12)$ \alpha =\frac{{L}_{\mathrm{d}}-{L}_{k}}{{L}_{k+1}-{L}_{k}}. $

式中：$ {L}_{k} $和$ {L}_{k+1} $为累积弧长序列中恰好包含目标弧长$ {L}_{\mathrm{d}} $的2个相邻的已知累积弧长.

根据由式(12)计算得到的插值系数，通过线性插值，得到离散路径点$ {\boldsymbol{P}}_{\mathrm{d}} $：

(13)$ {\boldsymbol{P}}_{\mathrm{d}}={\boldsymbol{P}}_{k}+\alpha ({\boldsymbol{P}}_{k+1}-{\boldsymbol{P}}_{k}). $

将其展开为分量形式，得到

(14)$ \left. \begin{aligned} & {x}_{\mathrm{d}}={x}_{k}+\alpha ({x}_{k+1}-{x}_{k}),\\& {y}_{\mathrm{d}}={y}_{k}+\alpha ({y}_{k+1}-{y}_{k}).\end{aligned}\right\} $

式中：$ {x}_{\mathrm{d}}、{y}_{\mathrm{d}} $为$ {\boldsymbol{P}}_{\mathrm{d}} $在x、y方向上的分量，$ ({x}_{k},{y}_{k}) $和$ ({x}_{k+1},{y}_{k+1}) $为中间采样点${\boldsymbol{P}}_k 、{\boldsymbol{P}}_{k+1} $的坐标.

在完成对平滑路径的等弧长间距离散化、获得离散参考点序列后，还需要计算每个离散点对应的航向角. 对于路径中间点，基于其前、后相邻点来计算航向角，对其分量$ x $和$ y $求导：

(15)$ \begin{split} \dfrac{\mathrm{d}x}{\mathrm{d}L}=\dfrac{\mathrm{d}x}{\mathrm{d}u}\dfrac{\mathrm{d}u}{\mathrm{d}L},\;\dfrac{\mathrm{d}y}{\mathrm{d}L}=\dfrac{\mathrm{d}y}{\mathrm{d}u}\dfrac{\mathrm{d}u}{\mathrm{d}L}.\end{split} $

可以得到第$ k $个离散点$ {\boldsymbol{P}}_{k} $的航向角$ {\theta }_{k} $：

(16)$ {\theta }_{k}=\text{atan}2\left(\frac{\mathrm{d}y}{\mathrm{d}L}{|}_{{{\boldsymbol{P}}_{k}}},\frac{\mathrm{d}x}{\mathrm{d}L}{|}_{{{\boldsymbol{P}}_{k}}}\right). $

通过上述步骤，在平滑曲线的基础上，获取1条路径点均匀、离散化分布且包含航向角信息的路径点序列. 对于第$ k $个离散点，其状态向量为

(17)$ {\boldsymbol{\zeta }}_{\text{ref}}={\left[{x}_{k},\;{y}_{k},\;{\theta }_{k}\right]}^{\text{T}}. $

式中：$ {x}_{k} $和$ {y}_{k} $为等弧长间距离散化处理后的路径点坐标，$ {\theta }_{k} $为计算得到的航向角. 因此，完整的参考路径点序列可以表示为

(18)$ {\boldsymbol{P}}_{\mathrm{ref}}=\{{\boldsymbol{\zeta }}_{0},{\boldsymbol{\zeta }}_{\text{1}},\cdots ,{\boldsymbol{\zeta }}_{N-1}\}. $

式中：$ N $为离散点的总数量. 随后，将此序列引入M-MPC控制器，作为差动机器人跟踪控制层所需的精确参考输入.

在差动机器人的路径跟踪初始阶段，规划层和控制层之间的衔接面临关键问题：规划路径的初始期望航向与机器人当前的实际航向不一致，如图3所示. 其中，$ {\boldsymbol{P}}_{1} $、$ {\boldsymbol{P}}_{n} $、$ {\boldsymbol{P}}_{n+1} $分别为规划路径中的第1、$ n $、$ n+1 $个路径点；$ C $为差动机器人质心，$ {{\boldsymbol{CN}}} $为机器人当前实际方向；$ {\theta }_{\mathrm{r}} $为实际航向角，由机器人实际朝向决定；将规划路径的起点至下一个期望点之间的方向定义为规划路径方向$ {{\boldsymbol{CE}}} $，对应于期望航向$ {\theta }_{\mathrm{e}} $. 2个方向之间的差值为初始航向误差$ {\delta }_{\theta } $，出现这种误差的原因主要在于规划层. A^*算法在全局坐标系中独立生成离散的几何路径，目标是寻找代价最小的路径，但是其在生成路径时不会强制路径起始点的期望航向与差动机器人当前的实际航向对齐. 换言之，规划层输出的是一系列空间位置点，而这些点对应的期望航向与机器人实际的初始位姿是相互独立的. 因此，需要对初始阶段的航向偏差进行校正.

为了解决路径跟踪初始阶段中存在的航向偏差问题，提出基于原地转向的航向对齐机制. 该机制充分利用差动机器人原地旋转的运动特性，确保规划层和跟踪控制层之间平稳衔接，从而避免因初始航向误差而导致的位移误差激增问题. 定义初始航向偏差为

(19)$ {\delta }_{\theta }={\theta }_{\mathrm{r}}-{\theta }_{\mathrm{e}}. $

设定初始航向偏差的阈值为$ {\varepsilon }_{\theta } $；当$ \left| {\delta }_{\theta }\right| \gt {\varepsilon }_{\theta } $时，机器人以原地旋转的方式修正航向，此时线速度$ {v}_{x} $为零，角速度$ \omega $的控制律为

(20)$ \omega =\left\{\begin{array}{ll} -1,&{\delta }_{\theta } \geqslant {\varepsilon }_{\theta };\\1,& {\delta }_{\theta } \leqslant -{ \varepsilon }_{\theta }.\end{array}\right. $

式中：角速度的幅值为1 rad/s. 在此阶段中，机器人保持恒定角速度进行旋转，而当初始航向偏差$ \left| {\delta }_{\theta }\right| \leqslant {\varepsilon }_{\theta } $时，机器人航向修正结束，控制系统自动切换至正常路径跟踪控制模式.

M-MPC控制器在处理非线性系统和多约束条件方面具有优越性，且能够前瞻性地考虑未来一段时间内的系统行为，具有更高的控制精度. 因此，将M-MPC作为差动机器人的路径跟踪控制算法^[27].

差动机器人系统是高度耦合的非线性系统，其运动特性较为复杂^[28]. 尽管动力学模型可以更全面地描述机器人的物理运动过程，但是其依赖于精确的物理参数识别，而这些参数往往难以准确获取^[29]. 此外，动力学模型的复杂性会增加额外的计算负担，难以满足控制算法的实时性要求^[30]. 本研究的重点是在确保控制精度和平顺性的前提下，实现路径规划与跟踪控制的有机结合，而非在极限工况下执行复杂机动. 运动学模型能够充分描述机器人位姿随控制输入的变化规律，满足路径跟踪控制的需求. 因此，选择结构简单、便于实时控制的运动学模型作为控制器的预测模型^[31]. 差动机器人的运动学模型如图4所示.

差动机器人的运动学方程表述为

(21)$ \left[\dot{x},\,\dot{y},\,\dot{\theta }\right]^{\mathrm{T}}=\left[v\cos \theta ,\,v\sin \theta ,\,\omega \right]^{\mathrm{T}}. $

式中：$ x、y $分别为机器人的横、纵坐标，$ \theta $为机器人的航向角，$ v $为机器人的线速度.

基于M-MPC的路径跟踪控制器由预测模型和惩罚函数2个关键部分组成. 对运动学模型进行离散化处理，可以得到预测时域内差动机器人的位姿状态：

(22)$ \dot{\boldsymbol{\zeta }}=f(\boldsymbol{\zeta },\boldsymbol{U}), $

(23)$ \begin{split} \boldsymbol{\zeta }={\left[x,\;y,\;\theta \right]}^{\text{T}},\; \boldsymbol{U}={\left[v,\;\omega \right]}^{\text{T}}.\end{split} $

式中：f为系统的非线性状态函数. 使用欧拉法对式（22）进行离散化处理，得到离散时间的状态控制模型：

(24)$ \boldsymbol{\zeta }(t+1|t)=\boldsymbol{\zeta }(t|t)+T\dot{\boldsymbol{\zeta }}(t|t). $

式中：T为控制周期. 将式（22）代入式（24），得到

(25)$ \boldsymbol{\zeta }(t+1|t)=\boldsymbol{\zeta }(t|t)+Tf(\boldsymbol{\zeta }(t|t),\;\boldsymbol{U}(t|t)). $

定义模型预测控制的预测时域为N_p，控制时域为N_c，建立预测模型：

(26)$ \left.\begin{array}{c}\boldsymbol{\zeta }(t+1|t)=\boldsymbol{\zeta }(t|t)+Tf\left(\boldsymbol{\zeta }(t|t),\;\boldsymbol{U}(t+1|t)\right),\\\vdots \\\boldsymbol{\zeta }(t+{N}_{\mathrm{c}}+1|t)=\boldsymbol{\zeta }(t+{N}_{\mathrm{c}}|t)+\\Tf(\boldsymbol{\zeta }(t+{N}_{\mathrm{c}}|t),\;\boldsymbol{U}(t+{N}_{\mathrm{c}}|t)),\\\vdots \\\boldsymbol{\zeta }(t+{N}_{\mathrm{p}}+1|t)=\boldsymbol{\zeta }(t+{N}_{\mathrm{p}}|t)+\\Tf(\boldsymbol{\zeta }(t+{N}_{\mathrm{p}}-1|t),\;\boldsymbol{U}(t+{N}_{\mathrm{c}}|t)).\end{array}\right\} $

式中：$ \boldsymbol{\zeta }(t+i|t) $为 $ t $时刻的第 $ i $个状态预测信号，$ \boldsymbol{U}(t+ i|t) $为$ \,t $时刻的第$ \,i $个未来控制信号.

由于控制器的惩罚函数与参考路径点之间存在耦合关系，参考路径信息须根据控制器输出的纵向速度来动态获取. 具体而言，选取参考路径上与差动机器人当前位置最接近的点作为基准点；随后，在该基准点的前方选取一系列路径点，构成当前控制周期内的参考路径点序列. 该序列的长度应与预测步长保持一致，且相邻路径点之间的弧长应满足

(27)$ s=T{v}_{x}(t+1). $

将参考路径点的状态信息写为矢量形式：

(28)$\begin{split} {\boldsymbol{\zeta }}_{\text{ref}}(t+i|t)= & \left[{x}_{\mathrm{ref}}(t+i|t)，{y}_{\mathrm{ref}}(t+i|t)，{\theta }_{\mathrm{ref}}(t+i|t)\right]^{\mathrm{T}}; \\& i=1,2,\cdots ,{N}_{\mathrm{p}}. \end{split} $

将多目标优化函数构建为

(29)$ \begin{split} {J}=&\sum\nolimits_{i=1}^{N_{\mathrm{p}}}\left\|{\boldsymbol{\zeta}}(t+i \mid t)-{\boldsymbol{\zeta}}_{\mathrm{ref}}(t+i \mid t)\right\|_{\boldsymbol{Q}}^2+ \\& \sum\nolimits_{i=0}^{N_{\mathrm{c}}-1}\|\boldsymbol{U}(t+i+1 \mid t)-\boldsymbol{U}(t+i \mid t)\|_{\boldsymbol{R}}^2 .\end{split}$

式中：Q为误差惩罚项的权重矩阵，R为控制量增量惩罚项的权重矩阵. 控制量增量惩罚项的作用是确保控制器以较为平稳的方式引导差动机器人跟踪参考路径，防止机器人在短时间内过于频繁地加、减速和转向，进而保证跟踪控制精度和平顺性^[32].

路径跟踪控制问题可以转化为带约束的优化问题：

(30)$ \left.\begin{array}{c}\underset{{v}(t), \;{\omega}(t)}{\min J,}\\\mathrm{s}.\mathrm{t}.\;\dot{v}\in ({\dot{v}}_{\min },{\dot{v}}_{\max }),\;\dot{\omega }\in ({\dot{\omega }}_{\min },{\dot{\omega }}_{\max }).\end{array}\right\} $

式中：$ {\dot{v}}_{\min }、{\dot{v}}_{\max } $为加速度上、下限，$ {\dot{\omega }}_{\min }、{\dot{\omega }}_{\max } $为角加速度上、下限. 通过求解该优化问题，即可获得差动机器人路径跟踪控制所需的输入变量序列：

(31)$ {\boldsymbol{U}}^{*}={\left[\boldsymbol{U}(1),\boldsymbol{U}(2),\cdots ,\boldsymbol{U}({N}_{\text{c}})\right]}^{\text{T}}. $

式中：第1个元素$ \boldsymbol{U}(1) $即为控制器在下一时刻输出的控制变量. 由此，可以实现对未来状态量的预测.

为了验证提出的路径规划和跟踪控制系统的有效性，自主设计并构建实验系统. 该系统以国兴智能Safari-900T履带式底盘为基础，整体硬件架构由3个核心子系统组成：1）主控单元，搭载超恩智能RCX-1430FR-PEG型工控机，负责算法运算与决策生成；2）执行控制器，为集成式线控驱动系统，用于实现对行驶速度（0~1.2 m/s）与航向角速度（0~1.2 rad/s）的精确控制；3）定位模块，型号为戴氏科技DAISCH IFS 2000，在实时动态定位（real-time kinematic, RTK）模式下，其测量精度可以达到坐标误差$ \leqslant \;0.01 $ m、航向误差$ \leqslant \;0.03 $ rad，为算法验证提供了可靠支持.

实验系统在Linux Ubuntu 18.04操作系统下运行，并基于ROS Melodic架构实现算法的模块化部署与实时通信. 将差动机器人的定位、坐标转换、路径规划、跟踪控制以及底盘执行等模块分别构建为独立ROS节点，通过ROS Topic机制完成实时数据交互和状态反馈. M-MPC控制器的参数如表1所示，其中预测步数N_p设为20，在保持控制器前瞻性的同时避免由长预测引发的提前转向和误差累积；控制步数N_c设置为2，以降低优化问题的维度并提升求解效率；误差惩罚项权重矩阵Q通过系统化的参数调优，在控制精度与平顺性之间实现了合理平衡；同时，控制量增量惩罚项权重矩阵R对角速度变化幅度施加适当约束，使转向过程更平稳，且不损失必要的响应灵活性.

实验共分为3部分. 第1部分旨在说明不同的路径预处理策略对A^*与M-MPC控制系统性能的影响，具体对比3种路径生成与控制系统的组合：1）M-MPC-A，即直接将由式(1)、(2)定义的启发函数与节点扩展规则生成的A^*原始路径输入M-MPC控制器，不进行平滑或离散化处理；2）M-MPC-BA，即在原始路径的基础上，采用式(3)~(7)所述的三次准均匀B样条进行路径平滑处理后，再将路径输入M-MPC控制器；3）M-MPC-EBA，即在平滑路径的基础上，进一步采用式(8)~(18)中的等弧长间距离散化方法，对参考路径进行离散化处理，将生成的完整离散点序列输入M-MPC控制器. 在第2部分中，通过4组对比实验，分析在有/无初始航向对齐机制条件下的路径跟踪控制性能差异，其中对齐策略按照式(19)、(20)实现. 第3部分以M-MPC-EBA生成的参考路径为基准，对比提出的A^*与M-MPC结合的系统，与A^*分别结合纯跟踪控制（pure pursuit, PP）和单参考点MPC（single reference point MPC, S-MPC）的系统的性能差异，其中M-MPC基于式(21)~(31)中建立的运动学预测模型与代价函数框架来实现.

图5展示了在3种路径预处理方法与M-MPC控制器的组合下，差动机器人实际运行轨迹与参考路径的对比结果. 由于等弧长间距离散路径在几何形状上与B样条（B-spline）路径保持一致，故而未单独绘出. 由图5可见，在M-MPC-A中，由于参考路径存在明显的几何不连续性，差动机器人的实际运行轨迹在路径拐角处出现明显偏离，表明原始A^*路径对M-MPC控制器的跟踪控制性能造成了负面影响. M-MPC-BA通过路径平滑处理提升了几何连续性，实际运行轨迹与参考路径的契合度明显优于前者，尤其在曲率过渡阶段，其表现更为平稳. 进一步地，M-MPC-EBA在平滑路径的基础上结合等弧长间距离散化处理，有效解决了路径点间距不均匀及其与M-MPC控制器等间隔预测轨迹点之间的结构性失配问题. 此时，差动机器人的实际轨迹几乎与参考路径重合，跟踪控制精度和平顺性明显提升，表明该路径与M-MPC控制器更为适配.

位移误差$ {e}_{\mathrm{d}} $作为实验精度评估的主要指标，用于直观衡量机器人实际运行轨迹与参考路径之间的空间偏差. 图6展示了3种路径算法的位移误差结果. M-MPC-A算法的位移误差均值$ {e}_{\mathrm{d},\text{mean}} $为0.0201 m，峰值$ {e}_{\mathrm{d},\text{max}} $达到0.1240 m，并且在路径弯道和曲率突变位置出现了大幅度震荡，控制平顺性较差. 经M-MPC-BA的路径平滑处理后，$ {e}_{\mathrm{d},\text{mean}} $下降至0.0184 m，$ {e}_{\mathrm{d},\text{max}} $下降至0.0811 m，表明几何连续性的提升有助于提升控制精度. 然而，受限于不均匀的路径点间距，预测时域内参考点的选取间隔与路径结构仍然存在不匹配问题，整体控制精度的提升幅度有限. M-MPC-EBA进一步结合了等弧长间距离散化处理，使$ {e}_{\mathrm{d},\text{mean}} $降低至0.0160 m，同时将$ {e}_{\mathrm{d},\text{max}} $降低至0.0597 m，表明路径结构的一致性对于控制器的精度提升具有关键作用.

图7展示了3种路径算法下差动机器人在路径跟踪过程中产生的航向误差$ {e}_{\mathrm{h}} $的变化趋势. 作为本实验中的次要精度指标，航向误差用于辅助评估3种路径算法的性能. 结果显示，在路径跟踪初始阶段，所有方案均存在较大的航向误差峰值$ {e}_{\mathrm{h},\mathrm{max}} $，最大误差接近0.80 rad. 机器人在路径规划完成后、控制起步前修正航向误差，避免了因初始航向误差引发的位移误差激增问题. 通过该机制，系统航向迅速趋于稳定，航向误差在跟踪过程中始终维持在较低水平.

需要说明的是，3种路径算法所跟踪的参考路径在几何结构上存在差异，M-MPC-A直接采用A^*原始路径，而M-MPC-BA和M-MPC-EBA分别在此基础上引入平滑处理与等弧长间距离散化处理. 考虑到路径结构的不同可能不仅影响了跟踪精度，而且会改变控制器的动态输出特性，在位移误差与航向误差等精度指标的基础上，进一步引入控制平顺性指标作为补充评价依据.

为了比较3种路径算法下M-MPC控制器的平顺性表现，图8展示了角速度$ \omega $随时间变化的曲线. 结果表明，M-MPC-EBA的角速度曲线波动幅度最小，响应过程平滑，整体变化趋势更为平稳. 控制增量累加值$ {S} _{\Delta \omega } $分析结果显示，M-MPC-EBA的$ {S} _{\Delta \omega } $最低，为14.5976 rad/s，低于M-MPC-A的20.5195 rad/s和M-MPC-BA的15.2333 rad/s. 这一结果与前述跟踪控制精度分析结果一致，验证了M-MPC-EBA在控制输出连续性与稳定性方面更具优势，有助于提升路径跟踪控制的平顺性.

第1部分的实验数据如表2所示. 通过对比3种路径生成与控制系统组合下的位移误差和航向误差，验证了M-MPC-EBA在各项精度指标上均具有显著优势. 与M-MPC-BA和M-MPC-A相比，其位移误差峰值$ {e}_{\mathrm{d},\text{max}} $分别降低了26.39%和51.85%，均值$ {e}_{\mathrm{d},\text{mean}} $分别降低了13.04%和20.40%；航向误差均值$ {e}_{\mathrm{h},\mathrm{m}\text{ean}} $分别降低了18.18%和45.78%. 同时，由平顺性分析结果可知，M-MPC-EBA在控制输出平顺性方面表现更优. 与M-MPC-BA和M-MPC-A相比，其$ {S} _{\Delta \omega } $分别降低了4.18%和28.86%. 这些结果综合表明，M-MPC-EBA系统在保障路径跟踪精度的同时，显著提升了控制平顺性，展现出更强的整体控制性能.

为了进一步验证初始航向对齐机制在路径跟踪控制中的关键作用，在有对齐机制和无对齐机制2种情况下开展共4组实验，结果如图9所示. 其中，采用航向对齐机制的实验记为M-MPC-HA；其余3组实验在无航向对齐的条件下进行，记为M-MPC-WHA；EBA路径为M-MPC-EBA策略下生成的路径. 结果表明，在缺少航向对齐的前2组实验中，差动机器人在控制起步阶段难以跟踪参考轨迹，跟踪控制失败，实际运行轨迹出现持续摆动现象并逐渐发散. 在第3组实验中，虽然能够跟踪参考轨迹，但是在控制开始阶段出现了幅度较大的超调动作，导致位移误差激增. 其根本原因在于：M-MPC在每个控制周期内均须基于机器人运动学模型和参考轨迹来求解滚动优化问题；当初始航向与参考路径方向存在较大偏差时，预测模型的初始状态与机器人的实际状态不一致，导致代价函数的最优解趋近于约束边界，因此控制器会输出幅值较大的控制指令，以快速纠正偏差. 然而，差动机器人的动态响应能力有限，幅值过大的控制指令会超出其瞬时可执行范围，进而引发控制失败或出现较大的超调动作，降低了跟踪控制精度和控制平顺性. 相比之下，航向对齐机制通过在控制起步前消除差动机器人初始航向与参考轨迹方向之间的差异，使预测模型的初始状态更接近于实际状态，从而减小了优化过程中的修正幅度，使控制器能够在约束条件下快速收敛，从而有效抑制了位移误差激增问题，并提升了整体跟踪性能.

位移误差结果如图10所示. 由于M-MPC-WHA-1和M-MPC-WHA-2出现控制失败，误差呈持续发散趋势，未绘出该结果. M-MPC-WHA-3在控制起始阶段出现了明显的超调动作，其初始位移误差骤增至0.5 m，降低了跟踪控制精度和控制平顺性. 对比结果表明，初始航向对齐机制能够有效抑制因初始航向误差引起的位移误差激增问题，提升了控制平顺性.

为了系统评估不同控制系统的性能差异，在第3部分的实验中对M-MPC与PP、S-MPC进行对比分析. M-MPC参数与第1部分的实验中保持一致；S-MPC参数与M-MPC保持一致，PP控制器的预瞄距离$ {L}_{\mathrm{pre}} $设定为1 m.

图11展示了相同参考路径下M-MPC、PP和S-MPC控制器的路径跟踪结果. 当路径中存在航向和曲率突变的弯道时，M-MPC控制器能够有效应对突变，使差动机器人的实际运行轨迹紧密贴合参考路径. 相比之下，PP控制器在面对此工况时出现了明显的响应滞后现象，在弯道出口处出现明显偏离，导致路径跟踪精度下降；而S-MPC在路径跟踪初始阶段即控制失败，无法完成路径跟踪，符合以往研究中无预瞄信息的控制方法精确性较差的结论^[33].

图12、13分别展示了M-MPC、PP和S-MPC控制器的位移误差和航向误差. PP控制器的$ {e}_{\mathrm{d},\text{max}} $和$ {e}_{\mathrm{h},\mathrm{max}} $分别为0.1551 m和0.7850 rad，$ {e}_{\mathrm{d},\text{mean}} $和$ {e}_{\mathrm{h},\mathrm{m}\text{ean}} $分别为0.0291 m和0.0678 rad；而采用M-MPC控制器时的$ {e}_{\mathrm{d},\text{max}} $和$ {e}_{\mathrm{h},\mathrm{max}} $分别为0.0732 m和0.0786 rad，$ {e}_{\mathrm{d},\text{mean}} $和$ {e}_{\mathrm{h},\mathrm{m}\text{ean}} $分别为0.0170 m和0.0640 rad. S-MPC在路径跟踪初始阶段控制失败，因此误差发散.

图14展示了相同的参考输入条件下，M-MPC、PP和S-MPC控制器的角速度输出对比. 结果表明，M-MPC在跟踪精度方面具有明显优势，但是其角速度输出在某些时间点存在一定波动，整体控制平顺性略逊于PP控制器. 其原因在于，M-MPC在预测时域内不断向前选取参考点，并通过不断调整角速度输出以更紧密地贴合参考轨迹，从而影响了平顺性. M-MPC在控制平顺性方面仍然有提升空间，未来会在此基础上进一步提升控制精度和平顺性，以实现更高性能的路径跟踪控制.

第3部分的实验数据如表3所示. 通过对比3种控制器的误差数据可知，M-MPC控制器在综合性能上表现更优. 与PP控制器相比，M-MPC控制器的位移误差峰值降低了52.80%，均值降低了41.58%；航向误差均值下降了5.60%. 由于S-MPC控制器在实验中控制失败，误差发散，未能产生有效结果. 此外，PP控制器无须进行在线滚动优化求解^[34]，其实时性远优于M-MPC和S-MPC2种控制器. 在实时性方面3种控制方法不具备可比性，故未进行对比.

上述3组实验的对比分析结果表明，提出的路径规划与跟踪控制方法解决了A^*算法生成路径存在的几何不连续、平滑性差以及点间距不均等问题. 基于等弧长间距离散化的三次准均匀B样条路径在提升M-MPC控制器的路径跟踪精度与控制平顺性方面具有显著优势.

（1）利用基于A^*算法与三次准均匀B样条曲线的路径规划方法，解决了A^*路径固有的拐点多、平滑性差以及曲率不连续等问题，结果与文献[22]、[35]的结论一致. 该方法在提升规划路径几何连续性和平滑性的同时，确保了差动机器人在跟踪参考轨迹的过程中的稳定性.

（2）由于A^*路径与M-MPC控制器期望的等间隔预测轨迹点之间存在结构失配问题，在A^*路径规划和B样条曲线平滑的基础上，进一步结合等弧长间距离散化方法，生成结构连续、点间距均匀且包含航向信息的离散路径点序列，以实现参考路径结构与期望输入的一致性. 实验结果表明，该方法相比于A^*路径规划或仅进行平滑处理的方案，降低了跟踪控制过程中的位移误差和航向误差，并提升了控制平顺性. 实验验证了路径结构一致性对M-MPC控制性能的重要作用.

（3）在路径跟踪初始阶段，规划路径的期望航向与机器人实际航向之间存在偏差. 提出的基于原地转向的初始航向对齐机制能够在控制起步前迅速修正初始航向偏差. 实验结果表明，该机制能够避免由初始航向误差引发的位移误差激增问题，确保了规划层与控制层之间的平稳衔接，增强了系统整体的鲁棒性.

（4）提出的路径规划与跟踪控制集成方法通过优化路径结构，解决了期望输入与控制器之间的失配问题，提升了系统的路径跟踪控制精度和控制平顺性. 与已有的平滑A^*算法和PP控制器结合的系统、平滑A^*算法和S-MPC控制器结合的系统相比，所提方法表现出更高的跟踪控制精度.