考虑曲率扰动的车辆自适应预瞄路径跟踪算法

Adaptive Preview Path Tracking Algorithm for Vehicles Considering Curvature Disturbance

摘要

路径跟踪控制在无人驾驶车辆运动控制中至关重要，其控制性能易受到道路曲率快速变化的干扰。提出了一种面向曲率扰动抑制的自适应预瞄路径跟踪控制方法以提高系统在复杂道路环境下的鲁棒性与精度。通过将扰动项嵌入状态向量构造增广线性二次调节问题，并采用高阶Riccati方程降维求解策略进行求解。随后，经频域响应分析验证预瞄控制器的可行性。依据车速和远端预瞄点曲率设计动态调整预瞄步数的自适应调节策略，实现对预瞄距离的在线优化。硬件在环仿真与实车测试验证了路径跟踪算法提高路径跟踪精度与改善行驶稳定性的有效性。

Abstract

Path tracking control played a vital role in unmanned vehicle motion control， and the control performance was highly sensitive to road curvature rapid variations. Therefore， an adaptive preview path tracking control method was proposed with curvature disturbance suppression to enhance the robustness and accuracy of the systems under complex road conditions. By embedding curvature disturbances into the state vector， an augmented LQR problem was formulated and efficiently solved using a reduced-order Riccati equation. Subsequently， the feasibility of the preview controller was verified through frequency-domain response analyses. Furthermore， an adaptive strategy was designed for adjusting preview steps to dynamically optimize the preview distances based on vehicle speeds and the curvatures at the distant preview points. Hardware-in-the-loop simulations and real-vehicle tests confirm the method's effectiveness in improving tracking accuracy and driving stability.

Graphical abstract

关键词

无人驾驶车辆 / 路径跟踪 / 曲率扰动 / 增广线性二次调节 / 自适应预瞄

Key words

unmanned vehicle / path tracking / curvature disturbance / augmented linear quadratic regulator（LQR） / adaptive preview

0 引言

随着智能化与自动化进程的不断推进，无人驾驶车辆的应用范围正在迅速拓展^［1］。当前无人驾驶技术正在商业和军事领域逐步落地^［2］。无人驾驶车辆主要由环境感知、决策规划、运动控制三大部分组成^［3］，路径跟踪精度与行驶稳定性作为控制模块的核心研究目标，共同构成了评估路径跟踪算法性能的重要标准。文献［4］经过分析跟踪误差收敛率，利用离线样本拟合动态前视距离，并结合侧滑补偿优化了纯追踪算法的跟踪性能。文献［5］在提高软目标车跟踪精度问题的横向控制器设计中，通过遗传算法优化模糊控制规则库，并将该模糊控制器用于动态调整Stanley算法增益参数。文献［6］提出一种基于鲁棒H_{\mathrm{\infty }}容错观测器的PID路径跟踪控制器。文献［7］采用自适应滑模控制（sliding mode control， SMC）确定四轮转向车辆的偏航力矩。文献［8］将PID与滑模控制结合，提出PID补偿的SMC横向控制器。文献［9］考虑轮胎刚度参数偏差导致的误差，建立了带有前馈控制的实时自调整线性二次调节（linear quadratic regulator， LQR）路径跟踪控制器。文献［10］部署Apollo平台验证了多目标优化模型预测控制（model predictive control， MPC）的路径跟踪性能。

为了在保持车辆路径跟踪控制器前向预测优势的同时减少算力消耗，本文以预瞄控制理论^［11］为核心思想，提出了一种面向曲率扰动抑制的自适应预瞄路径跟踪控制方法。首先通过构造增广状态变量的方式构建增广车辆模型，利用增广线性二次调节求解预瞄道路曲率的最优控制律，并通过频域响应分析该方法的可行性；然后，设计了基于车速和远端预瞄点曲率的动态调整预瞄步数的自适应调节策略，实现了对曲率扰动的实时适应性控制；最后，通过硬件在环仿真实验验证预瞄控制算法的具体表现和自适应预瞄策略的性能，并且经过实车实验验证考虑曲率扰动的自适应预瞄控制器在路径跟踪精度与行驶稳定性上的改善。

1 车辆控制系统模型

1.1 动力学模型

本文以图1所示只有侧向和横摆两个自由度的单轨模型为基础设计路径跟踪控制器^［12］，其动力学方程表示为

\left.\begin{array}{l}m{a}_y+m{v}_x\dot{\phi }=F_{y\mathrm{f}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{\mathrm{f}}+F_{y\mathrm{r}}\\ I_{\mathrm{z}}\ddot{\phi }=a{F}_{y\mathrm{f}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{\mathrm{f}}-b{F}_{y\mathrm{r}}\end{array}\right\}（1）

式中：m为车辆质量； a_y为侧向加速度；v_x为绝对坐标系下的纵向车速；\dot{\phi }为横摆角速度；F_{y\mathrm{f}}、F_{y\mathrm{r}}分别为前后车辆所受侧向力；I_{\mathrm{z}}为车辆转动惯量；{\delta }_{\mathrm{f}}为前轮转角；a、b分别为重心到前后轴距离。

前后车轮所受侧向力分别表示为

\left.\begin{array}{l}{F}_{y\mathrm{f}}=C_{\mathrm{f}}(\frac{v_y+\dot{\phi }a}{v_x}-{\delta }_{\mathrm{f}})\\ F_{y\mathrm{r}}=C_{\mathrm{r}}(\frac{v_y-\dot{\phi }b}{v_x})\end{array}\right\}（2）

式中：C_{\mathrm{f}}、C_{\mathrm{r}}分别为前后车轮侧偏刚度，\mathrm{N}/\mathrm{r}\mathrm{a}\mathrm{d}；v_y为绝对坐标系下的横向车速，\mathrm{m}/\mathrm{s}。

结合式（1）所述车辆动力学关系、式（2）所述车轮侧向力表达式，即可将车辆单轨模型微分方程表述为

\left[\begin{array}{c}{\dot{v}}_y\\ \ddot{\phi }\end{array}\right]=\left[\begin{array}{cc}\frac{{\xi }_{\mathrm{1}}}{m{v}_x}& \frac{{\xi }_{\mathrm{2}}}{m{v}_x}-v_x\\ \frac{{\xi }_{\mathrm{2}}}{I_{\mathrm{z}}{v}_x}& \frac{{\xi }_{\mathrm{3}}}{I_{\mathrm{z}}{v}_x}\end{array}\right]\left[\begin{array}{c}{v}_y\\ \dot{\phi }\end{array}\right]-\left[\begin{array}{c}\frac{C_{\mathrm{f}}}{m}\\ \frac{a{C}_{\mathrm{r}}}{I_{\mathrm{z}}}\end{array}\right]\delta（3）

{\xi }_{\mathrm{1}}=C_{\mathrm{f}}+C_{\mathrm{r}}　 {\xi }_{\mathrm{2}}=a{C}_{\mathrm{f}}-b{C}_{\mathrm{r}}　 {\xi }_{\mathrm{3}}=a^{\mathrm{2}}{C}_{\mathrm{f}}+b^{\mathrm{2}}{C}_{\mathrm{r}}

1.2 跟踪误差模型

图2显示了绝对坐标系OXY下车辆重心位置于某时刻向参考路径投影的示意图。以车辆重心为原点，沿车速方向建立纵向单位向量\mathit{\tau }、垂直车速方向建立横向单位向量\mathit{n}；以投影点P为原点，建立切向单位向量为{\mathit{\tau }}_P、法向单位向量为{\mathit{n}}_P的自然坐标系。分别记\mathit{r}为车辆重心的位置矢量，{\mathit{r}}_P为投影点P的位置矢量。图中，v为车辆重心的速度；d为车辆重心到投影点P的距离；\theta为车辆航向角；{\theta }_P为投影点P切线方向与X轴夹角；\dot{s}为重心投影到自然坐标系下的速度。

根据图2所示的矢量关系，投影距离d可表示为

d=(\mathit{r}-{\mathit{r}}_P)\cdot {\mathit{n}}_P（4）

对于式（4）中的车辆重心位置矢量\mathit{r}，当时间增量\mathrm{d}t足够小时，\mathit{r}的时间一阶导数\mathrm{d}\mathit{r}/\mathrm{d}t可近似为车速大小与车速方向向量的乘积，即

\frac{\mathrm{d}\mathit{r}}{\mathrm{d}t}=\dot{\mathit{r}}=v\mathit{\tau }（5）

同理可得投影点位矢{\mathit{r}}_P的时间一阶导数为

\frac{\mathrm{d}{\mathit{r}}_P}{\mathrm{d}t}={\dot{\mathit{r}}}_P=\dot{s}{\mathit{\tau }}_P（6）

将式（4）等号两侧对时间求导，并结合式（5）、式（6）的推论以及Frenet公式^［13］，即可消除等量关系中的矢量：

\dot{d}=v\mathrm{s}\mathrm{i}\mathrm{n}(\theta -{\theta }_P)（7）

假设\phi -{\theta }_P为较小值，即可将式（7）近似转化为\dot{d}与v_y、\phi之间的关系：

\dot{d}\cong v_x(\phi -{\theta }_P)+v_y（8）

在车辆的横向控制中，车辆实际位姿与期望位姿的差异通常用横向误差e_{\mathrm{d}}与航向误差e_{\phi }表示。令e_{\mathrm{d}}=d、e_{\phi }=\phi -{\theta }_P，即可由式（8）得到误差的一阶和二阶导数：

\left.\begin{array}{l}{\dot{e}}_{\mathrm{d}}=v_x{e}_{\phi }+v_y\\ {\dot{e}}_{\phi }=\dot{\phi }-{\dot{\theta }}_P\\ {\ddot{e}}_{\mathrm{d}}=v_x{\dot{e}}_{\phi }+{\dot{v}}_y\\ {\ddot{e}}_{\phi }=\ddot{\phi }-{\ddot{\theta }}_P\end{array}\right\}（9）

将式（9）中各等量关系代入式（3），并设跟踪误差及其变化率为状态量\mathit{x}=(e_{\mathrm{d}},{\dot{e}}_{\mathrm{d}},e_{\phi },{\dot{e}}_{\phi }{)}^{\mathrm{T}}，前轮转角为控制输入量u={\delta }_{\mathrm{f}}，道路曲率为外部扰动量w=\kappa ={\dot{\theta }}_P/v_x，即可表示车辆跟踪误差模型：

\dot{\mathit{x}}=\left[\begin{array}{cccc}\mathrm{0}& \mathrm{1}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \frac{{\xi }_{\mathrm{1}}}{m{v}_x}& -\frac{{\xi }_{\mathrm{1}}}{m}& \frac{{\xi }_{\mathrm{2}}}{m{v}_x}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\\ \mathrm{0}& \frac{{\xi }_{\mathrm{2}}}{I_{\mathrm{z}}{v}_x}& -\frac{{\xi }_{\mathrm{2}}}{I_{\mathrm{z}}}& \frac{{\xi }_{\mathrm{3}}}{I_{\mathrm{z}}{v}_x}\end{array}\right]\mathit{x}+\left[\begin{array}{c}\mathrm{0}\\ -\frac{C_{\mathrm{f}}}{m}\\ \mathrm{0}\\ -\frac{a{C}_{\mathrm{f}}}{I_{\mathrm{z}}}\end{array}\right]u+\left[\begin{array}{c}\mathrm{0}\\ \frac{{\xi }_{\mathrm{2}}}{m}-v_x^{\mathrm{2}}\\ \mathrm{0}\\ \frac{{\xi }_{\mathrm{3}}}{I_{\mathrm{z}}}\end{array}\right]w（10）

为了便于控制器以固定频率周期性地读取传感器信号并输出控制信号，将式（10）表示的连续时间系统转化为含有零阶保持器和固定采样周期的离散时间系统：

\mathit{x}(k+\mathrm{1})=\mathit{A}\mathit{x}(k)+\mathit{B}u(k)+\mathit{C}w(k)（11）

式中：k为采样时刻；\mathit{A}、\mathit{B}、\mathit{C}分别为系统的状态转移矩阵、输入矩阵和扰动矩阵。

2 预瞄路径跟踪控制器设计

2.1 增广线性二次型问题的表述

针对式（11）所述系统，扰动项道路曲率对系统产生不可忽视的非线性影响，因此无法直接用LQR进行解析求解。本文构造增广状态变量，将式（11）所述的跟踪误差模型重新表述为具有预瞄功能的增广车辆模型。该系统符合LQR的理论框架，可以利用增广线性二次调节求解最优控制律。

假设车辆前视摄像头能够识别的道路曲率为w(i),i\in [k,k+M]，在构造增广状态变量\mathit{X}(k)时将车辆即将经过的N个道路曲率添加到状态变量中（），将N称为预瞄步数。该方法构造的增广模型可表示为

\mathit{X}(k+\mathrm{1})=\overline{\mathit{A}}\mathit{X}(k)+\overline{\mathit{B}}u(k)（12）

\overline{\mathit{A}}=\left[\begin{array}{cc}{\mathit{A}}_{\mathrm{4}\times \mathrm{4}}& {\overline{\mathit{C}}}_{\mathrm{4}\times (N+\mathrm{1})}\\ {\mathit{O}}_{(N+\mathrm{1})\times \mathrm{4}}& {\mathit{D}}_{(N+\mathrm{1})\times (N+\mathrm{1})}\end{array}\right]

\overline{\mathit{B}}={\left[{\mathit{B}}_{\mathrm{4}\times \mathrm{1}}{\mathit{O}}_{(N+\mathrm{1})\times \mathrm{1}}\right]}^{\mathrm{T}}　　 \overline{\mathit{C}}=\left[{\mathit{C}}_{\mathrm{4}\times \mathrm{1}}{\mathit{O}}_{\mathrm{4}\times N}\right]

\mathit{D}=\left[\begin{array}{cc}{\mathit{O}}_{N\times \mathrm{1}}& {\mathit{I}}_{N\times N}\\ \mathrm{0}& {\mathit{O}}_{\mathrm{1}\times N}\end{array}\right]

X （k）=（ x （k）， W （k））^T=（ x （k），w（k），…，w（k+N））^Tw(k)

式中： A 为系统增广后的状态转移矩阵； C 为描述增广状态如何映射到输出量； D 为辅助分块对角矩阵； B 为增广输入矩阵； X 为当前采样时刻的道路曲率值；\mathit{W}(k)为未来N个时刻的道路曲率序列。

为兼顾路径跟踪精度和行驶稳定程度，构造如下目标函数：

J(\mathit{X},u)=\frac{\mathrm{1}}{\mathrm{2}}{\displaystyle \sum _{k=\mathrm{0}}^{\mathrm{\infty }}}{\mathit{X}}^{\mathrm{T}}(k)\overline{\mathit{Q}}\mathit{X}(k)+\overline{\mathit{R}}{u}^{\mathrm{2}}(k)（13）

\overline{\mathit{Q}}=\left[\begin{array}{cc}{\mathit{Q}}_{\mathrm{4}\times \mathrm{4}}& {\mathit{O}}_{\mathrm{4}\times (N+\mathrm{1})}\\ {\mathit{O}}_{(N+\mathrm{1})\times \mathrm{4}}& {\mathit{O}}_{(N+\mathrm{1})\times (N+\mathrm{1})}\end{array}\right]

式中：Q和\overline{\mathit{R}}分别为状态量和输入量的权重矩阵。

受机械结构的影响，乘用车辆前轮转角阈值一般为\pm \mathrm{30}°^［14］，将前轮转角及其变化量阈值约束设为

\left.\begin{array}{l}{u}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le u\le u_{\mathrm{m}\mathrm{a}\mathrm{x}}\\ \mathrm{\Delta }{u}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \mathrm{\Delta }u\le \mathrm{\Delta }{u}_{\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right\}（14）

式中：u_{\mathrm{m}\mathrm{i}\mathrm{n}}、u_{\mathrm{m}\mathrm{a}\mathrm{x}}分别为前轮转角下限和上限；\mathrm{\Delta }{u}_{\mathrm{m}\mathrm{i}\mathrm{n}}、\mathrm{\Delta }{u}_{\mathrm{m}\mathrm{a}\mathrm{x}}分别为前轮转角增量的下限和上限。

2.2 考虑曲率扰动的预瞄路径跟踪控制器

为便于在无穷时间上求解增广二次型问题，利用拉格朗日乘子法将式（13）所述的目标函数分解为

J(\mathit{X},u)=\frac{\mathrm{1}}{\mathrm{2}}\{{\mathit{X}}^{\mathrm{T}}(k)\overline{\mathit{Q}}\mathit{X}(k)+\overline{\mathit{R}}{u}^{\mathrm{2}}(k)+{\mathit{\lambda }}^{\mathrm{T}}(k+\mathrm{1})·[\overline{\mathit{A}}\mathit{X}(k)+\overline{\mathit{B}}u(k)-\mathit{X}(k+\mathrm{1})]\}+\mathit{X}(n)\overline{\mathit{Q}}\mathit{X}(n)（15）

式中：n为系统的终端时刻；\mathit{X}(n)\overline{\mathit{Q}}\mathit{X}(n)代表终端时刻的状态惩罚项。

经对增广状态量与输入量分别求偏导，并迭代化简可得Riccati方程和最优控制量u_{\mathrm{o}\mathrm{p}\mathrm{t}}(k)的表达式：

\overline{\mathit{P}}=\overline{\mathit{Q}}+{\overline{\mathit{A}}}^{\mathrm{T}}\overline{\mathit{P}}\overline{\mathit{A}}-{\overline{\mathit{A}}}^{\mathrm{T}}\overline{\mathit{P}}\overline{\mathit{B}}(\overline{\mathit{R}}+{\overline{\mathit{B}}}^{\mathrm{T}}{\overline{\mathit{P}}\overline{\mathit{B}})}^{-\mathrm{1}}{\overline{\mathit{B}}}^{\mathrm{T}}\overline{\mathit{P}}\overline{\mathit{A}}（16）

u_{\mathrm{o}\mathrm{p}\mathrm{t}}(k)=-\mathit{K}\mathit{X}(k)（17）

\mathit{K}=(\overline{\mathit{R}}+{\overline{\mathit{B}}}^{\mathrm{T}}{\overline{\mathit{P}}\overline{\mathit{B}})}^{-\mathrm{1}}{\overline{\mathit{B}}}^{\mathrm{T}}\overline{\mathit{P}}\overline{\mathit{A}}

由于预瞄步数的不确定性与N+5阶高阶\overline{\mathit{P}}矩阵，直接求解Riccati方程十分复杂，因此将\overline{\mathit{P}}划分为4个子矩阵：

\overline{\mathit{P}}=\left[\begin{array}{cc}{\mathit{P}}_{\mathrm{1}}& {\mathit{P}}_{\mathrm{2}}\\ {\mathit{P}}_{\mathrm{3}}& {\mathit{P}}_{\mathrm{4}}\end{array}\right]（18）

将式（18）代入式（16）、式（17），即可得出最优控制量u_{\mathrm{o}\mathrm{p}\mathrm{t}}(k)与\overline{\mathit{P}}子矩阵的关系：

\begin{array}{c}{u}_{\mathrm{o}\mathrm{p}\mathrm{t}}(k)=-(\mathit{R}+{\mathit{B}}^{\mathrm{T}}{\mathit{P}}_{\mathrm{1}}{\mathit{B})}^{-\mathrm{1}}{\mathit{B}}^{\mathrm{T}}{\mathit{P}}_{\mathrm{1}}\mathit{A}\mathit{x}(k)-\\ (\mathit{R}+{\mathit{B}}^{\mathrm{T}}{\mathit{P}}_{\mathrm{1}}{\mathit{B})}^{-\mathrm{1}}{\mathit{B}}^{\mathrm{T}}({\mathit{P}}_{\mathrm{1}}\overline{\mathit{C}}+{\mathit{P}}_{\mathrm{2}}\mathit{D})\mathit{W}(k)=\end{array}

-{\mathit{K}}_{\mathrm{1}}\mathit{x}(k)-{\mathit{K}}_{\mathrm{2}}\mathit{W}(k)（19）

式中：{\mathit{K}}_{\mathrm{1}}为不考虑曲率扰动的状态反馈增益矩阵；{\mathit{K}}_{\mathrm{2}}为考虑曲率扰动的前馈增益矩阵。

显然u_{\mathrm{o}\mathrm{p}\mathrm{t}}(k)仅与{\mathit{P}}_{\mathrm{1}}、{\mathit{P}}_{\mathrm{2}}相关，因此可实现\overline{\mathit{P}}的降维处理。结合矩阵求逆引理表示可将{\mathit{P}}_{\mathrm{1}}、{\mathit{P}}_{\mathrm{2}}表示为

\left.\begin{array}{l}{\mathit{P}}_{\mathrm{1}}=\mathit{Q}+{\mathit{A}}^{\mathrm{T}}(\mathit{I}+{\mathit{P}}_{\mathrm{1}}\mathit{B}{\mathit{R}}^{-\mathrm{1}}{\mathit{B}}^{\mathrm{T}}{)}^{-\mathrm{1}}{\mathit{P}}_{\mathrm{1}}\mathit{A}\\ {\mathit{P}}_{\mathrm{2}}={\mathit{A}}^{\mathrm{T}}(\mathit{I}+{\mathit{P}}_{\mathrm{1}}\mathit{B}{\mathit{R}}^{-\mathrm{1}}{\mathit{B}}^{\mathrm{T}}{)}^{-\mathrm{1}}({\mathit{P}}_{\mathrm{1}}\overline{\mathit{C}}+{\mathit{P}}_{\mathrm{2}}\mathit{D})\end{array}\right\}（20）

将式（19）代回到式（11）中，可得到闭环系统动力学方程：

\mathit{x}(k+\mathrm{1})=(\mathit{A}-\mathit{B}{\mathit{K}}_{\mathrm{1}})\mathit{x}(k)+(\overline{\mathit{C}}-\mathit{B}{\mathit{K}}_{\mathrm{2}})\mathit{W}(k)（21）

该方程描述了原系统在反馈作用与前馈作用共同控制下的行为。

2.3 系统频域响应分析

频域响应函数可以描述系统的幅度和相位变化，用于评价系统稳定性和控制性能。式（21）经过Z变换得到系统传递函数：

G(z)=[z\mathit{I}-(\mathit{A}-\mathit{B}{\mathit{K}}_{\mathrm{1}}{)]}^{-\mathrm{1}}(\overline{\mathit{C}}-\mathit{B}{\mathit{K}}_{\mathrm{2}})（22）

若将控制器退化为无预瞄的反馈控制，即不考虑道路曲率的传统LQR控制，则其传递函数为

G(z)=[z\mathit{I}-(\mathit{A}-\mathit{B}{\mathit{K}}_{\mathrm{1}}{)]}^{-\mathrm{1}}\mathit{C}（23）

可见两者传递函数中决定系统极点的特征多项式皆为[z\mathit{I}-(\mathit{A}-\mathit{B}{\mathit{K}}_{\mathrm{1}}{)]}^{-\mathrm{1}}，故考虑曲率扰动的预瞄路径跟踪控制器不会影响整个系统的稳定性和极点。

如图3所示，从控制理论的角度对两控制器在频域上的响应强度进行分析，可见：

1）在一定频率区间内，预瞄路径跟踪控制器能够有效抑制系统的横向误差与航向误差，超过一定频率时，对系统的抑制作用将会减弱，但依旧比传统LQR控制器的系统状态幅值低。

2）预瞄步数较小时，系统对跟踪误差的抑制作用较弱，预瞄步数增大时，系统对跟踪误差的抑制作用明显增强，在对横向误差的抑制上尤为明显。

3 自适应预瞄策略

在预瞄控制算法中，预瞄距离的取值起着至关重要的作用。为进一步提高预瞄控制算法的跟踪精度，将车辆前视摄像头识别的最远点即预瞄步数为M(M>N)处的点作为远点预瞄获取远端预瞄点曲率信息，提出基于车速、远端预瞄点曲率的自适应预瞄策略，自适应调节预瞄步数。

3.1 车速调节模块

考虑高速与低速行驶对预瞄步数的需求不同，设定调节策略为

（24）

式中：T_{\mathrm{v}}为基于车速调节策略的预瞄时间；T_{\mathrm{m}\mathrm{i}\mathrm{n}}、T_{\mathrm{m}\mathrm{a}\mathrm{x}}分别为最小预瞄时间和最大预瞄时间；v_{\mathrm{m}\mathrm{i}\mathrm{n}}、v_{\mathrm{m}\mathrm{a}\mathrm{x}}分别表示最小车速阈值和最大车速阈值。

3.2 远端预瞄点曲率调节模块

远端预瞄点曲率调节模块在车速调节策略确定的预瞄时间T_{\mathrm{v}}的基础上作出进一步调整，调整策略可表示为

（25）

式中：{\kappa }_M、{\kappa }_N分别表示预瞄步数为M和N时的道路曲率；{\kappa }_{\mathrm{b}}为边界曲率，用于界定曲率变化大小；\rho为补偿系数；\epsilon为车速影响系数；T_{\mathrm{l}\mathrm{a}\mathrm{s}\mathrm{t}}为上一时刻的预瞄时间；T为最终确定的预瞄时间；\dot{\kappa }为道路曲率的变化率。

若用\tau表示时间步长，则T与预瞄步数的关系为

N=round(T/\tau )（26）

式中：round(·)为取整函数。

另外，车辆过弯时所受侧向力F_y与道路曲率关系为

F_y=mw{v}_x^{\mathrm{2}}=m\kappa v_x^{\mathrm{2}}（27）

极限侧向力F_y^{\mathrm{m}\mathrm{a}\mathrm{x}}由地面附着提供：

F_y^{\mathrm{m}\mathrm{a}\mathrm{x}}=\mu mg（28）

式中：\mu为路面附着系数；g为重力加速度。

可得最大道路曲率与车速的关系：

{\kappa }^{\mathrm{m}\mathrm{a}\mathrm{x}}=\frac{\mathrm{1}}{R}=\frac{\mu g}{v_x^{\mathrm{2}}}（29）

其中，R为道路转弯半径。将{\kappa }_M与{\kappa }^{\mathrm{m}\mathrm{a}\mathrm{x}}对比，便于为介入纵向控制预留接口。

4 验证与分析

4.1 硬件在环仿真实验

首先通过PreScan、CarSim软件以及控制器搭建图4所示的硬件在环仿真平台，从预瞄控制算法具体表现与自适应预瞄策略性能两方面对控制器进行验证。仿真车辆参数见表1。

4.1.1 预瞄控制算法性能

预瞄控制算法性能的验证中不考虑自适应预瞄策略，仅对比分析预瞄控制算法与MPC算法在路径跟踪精度与求解速度上的性能表现。采用双移线工况作为路径的参考输入。

分析图5a~图5c所示仿真结果，在不同预瞄步数下，两算法的转向行为几乎一致。预瞄步数较小时，两算法的跟踪轨迹有不同程度的偏离，预瞄控制算法更贴近参考路径。当预瞄步数调整至较大值，预瞄控制算法的路径跟踪效果近似等于MPC算法，最大差异仅有毫米级的0.0036 m。

从图5d中可以看出，MPC算法在求解带有非线性扰动问题时，随着预测步数的增加，每步计算所消耗的时间从约0.05 s提高至约0.35 s；而预瞄控制算法即使在N=25时，在线求解Riccati方程每步消耗的时间也保持在0.008 s。

经仿真结果的对比分析可知，预瞄步数较小时，预瞄控制算法具有更高的跟踪精度；在预瞄步数较大时两者跟踪精度几乎相同。无论预瞄步数如何变化，预瞄控制算法的计算耗时更少，在保持预测能力的同时具有更佳的控制精度与更高的计算效率。

4.1.2 自适应预瞄策略性能

为测试车辆自适应预瞄策略在阶跃道路曲率中的响应，参考路径起始段为直线，随后驶入0.025的恒定曲率转向道路，最后进入0.04的反向曲率道路。

图6展示了自适应预瞄路径跟踪算法在仿真中的表现。图6a中，车辆在行驶至两段路径拼接点附近时，依据远端预瞄点曲率调节模块作出预瞄步数调整。移除自适应预瞄策略时，预瞄步数不会随道路曲率发生变化。

如图6b所示，与没有自适应预瞄的传统LQR反馈控制相比，预瞄控制器有明显的提早转向趋势，预瞄步数N=\mathrm{10}时系统约提前0.4s响应，预瞄步数N=\mathrm{15}时约提前0.6 s响应。图6c的横向误差曲线中，相比之下，采用预瞄步数自适应调节后，系统显著抑制了超调现象，优化了稳态性能，两段转向工况的稳态误差分别稳定在0.021 m和-0.027 m的较高精度水平。此外，图6d的航向误差曲线表明，动态预瞄步数调节使车辆能够更早调整航向，从而提高跟踪响应速度。尽管预瞄步数的切换会引入轻微波动，但整体上仍显著改善了车辆的路径跟随性能。

经仿真结果对比，自适应预瞄策略能够依据前方道路曲率调整车辆预瞄步数，改善车辆转角响应时间，同时提高控制器的整体性能。

4.2 实车测试

为进一步验证考虑曲率扰动的自适应预瞄路径跟踪算法的可靠性与实用性，搭建图7所示的实验车辆开展道路实验。

图8所示路段包含多种不同曲率转向场景，曲率变化如图9所示，存在坡度、路面附着系数等外界干扰变化的细微影响。将测试车速分别设为符合校园限速的18 km/h和正常通过社会复杂道路的36 km/h。控制器内预留考虑曲率扰动的自适应预瞄路径跟踪算法与没有自适应预瞄的传统LQR路径跟踪算法。

如图10a所示，在自适应预瞄策略的影响下，初始预瞄步数由车速调节模块决定，远端预瞄点曲率调节模块根据前方道路曲率作出调节。图10b、图10c所示测试结果中，车辆横向误差随车速增大而增大。考虑曲率扰动的自适应预瞄路径跟踪算法求解的前轮转角由跟踪误差反馈作用与前方道路曲率前馈作用两部分构成，因此在图10c中表现出更好的跟踪效果，在18 km/h的测试中横向误差最大降低了约58.7%，在36 km/h的测试中横向误差最大降低了约65.5%。在车辆入弯与出弯时，受预瞄最优控制律中负增益的影响，会输出与道路相反的转角。

如图10c、图10d所示，在曲率频繁阶跃变化的复杂路段下，缺少预瞄算法的跟踪稳定性显著下降。然而，通过引入预瞄控制的提前转向响应机制和自适应预瞄策略的动态调节，系统性能得到显著提高。图10c显示，车速为18 km/h和36 km/h时，最大航向误差分别降低3.1%和2.9%；图10e中，18 km/h和36 km/h车速下，最大横摆角速度峰值分别削减6.0%和4.6%。这种双重控制策略的协同作用有效抑制了曲率突变带来的扰动，使车辆跟踪稳定性获得了实质性改善。

5 结论

本文提出了一种考虑曲率扰动的自适应预瞄路径跟踪控制方法，通过硬件在环仿真与实车测试，得出以下结论：

1）引入了道路曲率信息构建增广状态方程，设计了最优预瞄控制器。频域分析与仿真结果表明，该方法在保证系统稳定性的前提下有效实现了对曲率扰动的抑制。相较于传统方法，该控制器兼具高跟踪精度与高计算效率，具备良好的实时性与工程应用可拓展性。

2）提出了基于车速和远端预瞄点曲率的自适应预瞄策略。该策略能根据车辆行驶状态动态调节预瞄步数，克服了固定预瞄策略的局限性。实验证实，该方法在复杂道路工况下能有效抑制转向超调，显著提高了控制系统的鲁棒性、稳态性能及对突发地形变化的适应能力。

3）实车测试验证了算法的有效性。与传统 LQR算法相比，本文算法在弯道工况下展现出更强的前瞻响应能力，能够提前识别曲率变化并进行补偿，有效抑制了因曲率突变引发的车辆动态振荡，显著增强了整车行驶的平稳性与安全性。

后续研究中，将根据预瞄步数切换过快导致的前轮转角振荡以及负前馈增益带来的出入弯道反方向转角问题展开研究，结合考虑通信延迟与转向迟滞，进一步完善控制器性能。

参考文献

[1] 彭育辉， 江铭， 马中原， 等. 汽车自动驾驶关键技术研究进展［J］. 福州大学学报（自然科学版）， 2021， 49（5）： 691-703.

[2] PENG Yuhui， JIANG Ming， MA Zhongyuan， et al. Review on Development of Key Technology for Autonomous Vehicle［J］. Journal of Fuzhou University （Natural Science Edition）， 2021， 49（5）： 691-703.

[3] 陈冰. 无人驾驶，我们准备好了吗？［J］.新民周刊， 2025（14）：22-24.

[4] Chen Bing. Are We Ready for Autonomous Driving ［J］. Xinmin Weekly， 2025（14）： 22-24.

[5] YURTSEVER E， LAMBERT J， CARBALLO A， et al. A Survey of Autonomous Driving： Common Practices and Emerging Technologies［J］. IEEE Access， 2020， 8： 58443-58469.

[6] SEO J H， YOUN S H， KIM J， et al. An Accurate Path Tracking Algorithm for Autonomous Vehicles Based on Pure Pursuit with Systematically Designed Look-ahead Distance and Sideslip Compensation［C］∥2023 23rd International Conference on Control， Automation and Systems （ICCAS）. Yeosu， 2023： 931-936.

[7] 李文礼， 易帆， 封坤， 等. 基于改进Stanley算法的目标假车路径跟踪控制［J］. 重庆理工大学学报（自然科学）， 2024， 38（2）： 20-31.

[8] LI Wenli， YI Fan， FENG Kun， et al. Path Tracking Control of Soft Target Vehicle Based on Improved Stanley Algorithm［J］. Journal of Chongqing University of Technology （Natural Science）， 2024， 38（2）： 20-31.

[9] CHEN B S， LIU Haoting， WU R S. Robust-fault-tolerant Observer-based PID Path Tracking Control of Autonomous Ground Vehicle with Control Saturation［J］. IEEE Open Journal of Vehicular Technology， 2024， 5： 298-311.

[10] JEONG Y. Path Tracking Control for Four-wheel-steering Autonomous Vehicles Based on Adaptive Sliding Mode Control with Control Allocation［C］∥2021 21st International Conference on Control， Automation and Systems （ICCAS）. Jeju， 2021： 1741-1746.

[11] 韩爱国， 李志新. 基于PID-SMC方法的无人车轨迹跟踪横向控制策略［J］. 武汉理工大学学报， 2022， 44（3）： 40-45.

[12] HAN Aiguo， LI Zhixin. Trajectory Tracking Lateral Control Strategy of Unmanned Vehicles Based on PID-SMC Method［J］. Journal of Wuhan University of Technology， 2022， 44（3）： 40-45.

[13] GUO Lie， TAN Zhenyu， ZHAO Yijie. Real-time Self-Ajusting LQR Method for Autonomous Vehicle Lateral Control［C］∥2023 7th CAA International Conference on Vehicular Control and Intelligence （CVCI）. Changsha， 2023： 1-6.

[14] ZHANG Lanyong， CHUAI Tianyu. Model Predictive Control Based on Multi-objective Optimization in Multiple Scenarios［C］∥2023 China Automation Congress （CAC）. Chongqing， 2023： 7994-7999.

[15] 郭孔辉. 驾驶员-汽车闭环系统操纵运动的预瞄最优曲率模型［J］. 汽车工程， 1984， 6（3）： 1-16.

[16] GUO Konghui. Drivers-Vehicle Close-loop Simulation of Handling by “Preselect Optimal Curvature Method”［J］. Automotive Engineering， 1984， 6（3）： 1-16.

[17] LIU Qin， ZHANG Xiaobo， XIE Jiachao， et al. Optimization Design of Vehicle Path Tracking Controller Based on High Accuracy Positioning［C］∥2023 7th International Conference on Transportation Information and Safety （ICTIS）. Xi'an， 2023： 1685-1691.

[18] WERLING M， ZIEGLER J， KAMMEL S， et al. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame［C］∥2010 IEEE International Conference on Robotics and Automation. Anchorage， 2010： 987-993.

[19] LI Haoran， LIU Kai， YANG Bo， et al. Path Tracking of Autonomous Vehicle Based on NMPC with Pre-steering［J］. IEEE Transactions on Consumer Electronics， 2024， 70（1）： 966-979.

基金资助

国家自然科学基金(52405070)

特种车辆设计制造集成技术全国重点实验室开放课题(GZ2023KF004)

教育部“春晖计划”合作科研项目(HZKY20220330)