基于激光扫描的卡车跟踪控制

基于激光扫描仪的自动驾驶卡车单轮驱动模块物体状态估计与跟踪控制算法

1 引言

自动驾驶技术不仅在研究方面，而且在商业化方面都已成为备受关注的领域。因此，已开展并整合了大量使用传感器、硬件和控制算法技术的自动驾驶研究。自动驾驶技术不仅能为工人和驾驶员带来便利，还能提高安全性。特别是对于物流工厂等存在危险工业设备的场所，依赖自动驾驶技术来保障工人的安全。本研究中的卡车旨在运输重载，且采用单层激光扫描仪的自动驾驶卡车通过单层激光扫描仪识别工人（物体），并利用单轮驱动模块跟随该物体。本研究的目标是开发一种由感知、决策与控制组成的跟踪控制算法，以实现自动驾驶卡车对物体的跟踪。为了发展自动驾驶技术，已针对各种应用开展了大量研究。

Kim等人（2018）提出了一种能够在建筑工地自主导航并利用点云持续更新三维扫描进度的自主移动机器人。他们使用了多个水平和垂直安装的激光雷达与摄像装置，以实现无信息丢失的扫描。Jung等人（2014）开发了一种用于人体跟踪的马拉松服务机器人，采用远程探测器（LRF）。他们利用LRF的数据确定物体特征，该特征被划分为树、树枝和人等11类。Park等人（2015）采用改进的纯追踪方法将自动驾驶车辆应用于路径跟踪，并使用伺服电机开发了转向控制系统。Song等人（2012）使用了模糊推理系统，用于激光导引车辆的路径跟踪控制。Roy等人（2015）开发了一种鲁棒控制器，以应对非完整轮式移动机器人在路径跟踪中的不确定性，并通过多种轨迹对其性能进行了评估。Huang等人（2014）提出了一种基于输出反馈控制器的自适应跟踪控制方案。Liang等人（2015）提出了一种基于自适应图像的轮式移动机器人跟踪控制方法，使用安装在天花板上的未标定固定相机。Abbasi和Moshayedi（2017）解释了一个从K‐Team公司生产的流行移动机器人 KHEPERA IV的运动学和动力学关系中获得的计算模型。Kim等人（2013）提出了一种基于激光雷达的自动驾驶车辆障碍物识别方法。Ostafew等人（2016）在越野环境中实现了高性能路径跟踪，采用了学习扰动方法。Normey‐Rico等人（2001）提出了一种基于 PID控制器的简单有效的移动机器人路径跟踪解决方案。Wit等人（2004）基于螺旋理论研究了非完整地面的跟踪控制。Maalouf等人（2006）介绍了一种基于模糊逻辑控制器的机器人，能够自动跟随一系列离散航路点。Hajjaji和Bentalba（2003）描述了车辆自动转向的路径跟踪控制。他们设计了模糊控制器，并基于 Lyapunov方法分析了其稳定性。Lenain等人（2006）使用扩展运动学模型以及对未测量参数的观测来实现模型预测控制。Nino‐Suarez等人（2006）提出了一种用于轮式移动机器人路径跟踪的离散时间控制策略。

以往的大多数研究使用了多种传感器、执行器和控制器。一些研究专注于区分人与非生物障碍物，或在存在意外干扰的情况下实现精确识别。此外，为提高跟踪控制性能，已采用各种控制器和执行器。本研究提出了一种仅使用单层激光扫描仪和单轮的自动驾驶卡车目标跟踪自动驾驶算法。本研究考虑的自动驾驶卡车跟随目标的方法采用了三个代表性算法：感知、决策与控制。在感知方面，考虑了坐标变换、降采样、聚类和状态估计。坐标变换算法用于转换坐标，因为单层激光扫描仪可测量距离和角度。降采样算法用于保留有意义的数据以提高效率。聚类算法用于确定物体的位置作为代表点，状态估计算法基于扩展卡尔曼滤波器估计状态的速度矢量。路径规划可根据估计状态进行计算。

基于简化卡车模型，从卡车状态和期望路径推导出误差动态，并利用线性二次调节器计算了收敛误差动态的最优转向输入。本研究采用西克公司型号为 LMS111‐10100的单激光扫描仪。通过单激光扫描仪获取的测量数据，在LABVIEW/MATLAB环境中对感知与决策算法进行了评估。此外，使用 MATLAB/SIMULINK环境中的实际数据进行了跟踪控制的卡车仿真。

本文其余部分安排如下。第2节介绍了算法的主要概念。第3节描述了跟踪控制算法，该算法分为感知、决策与控制例程。算法的性能评估和结论分别在第4和5节中描述。

2 跟踪控制算法概念

本研究中跟踪控制算法的目的是利用单层激光扫描仪和单轮驱动模块来跟踪移动物体。用于跟踪控制的期望卡车模型和模型示意图如图1所示，分为三个方面。感知例程包含四个算法，用于识别和估计物体。物体的速度矢量由感知例程中的状态估计算法基于扩展卡尔曼滤波（EKF）进行估计。聚类点和物体的估计速度矢量这两个状态通过线性方程被用于决策算法中的路径规划。误差动态通过平面自行车模型构建，该模型为简化而替代卡车模型，利用了卡车状态与决策例程中线性方程之间的关系。通过线性二次调节器（LQR）提供的最优输入，使误差动态的误差收敛，从而使卡车跟踪物体。

下一章描述了用于目标跟踪的状态估计和跟踪控制算法。

3 状态估计与跟踪控制算法

本章包含两个主要算法，即状态估计和目标跟踪控制算法。在感知流程中，为了估计状态，执行了坐标变换、降采样和聚类操作。期望路径已通过估计状态（如第3.2节中讨论的目标速度）计算得出，而用于目标跟踪的控制算法则在第3.3节中讨论。

3.1 感知

3.1.1 坐标变换

单层激光扫描仪通过测量激光反射所需的时间来获取物体的距离和角度。因此，本研究中使用的单层激光扫描仪可以获得距离作为角度的函数。坐标变换是用于在x‐y坐标中识别物体的算法。本研究提出的所有算法都需要使用x‐y坐标来计算和估计物体的状态。由于激光扫描仪获取的是极坐标系下的距离和角度数据，因此需要进行坐标变换算法处理。图2描述了使用激光扫描仪的坐标变换方法。

其中 $x_i$ 和 $y_i$ 表示在 x‐y坐标中测得的物体坐标。测得物体的角度根据激光扫描仪的分辨率角度规格确定。x‐y坐标已按方程(1)计算。

$$
x_i = r_i \cos h_i \
y_i = r_i \sin h_i
\tag{1}
$$

其中，$h_i$表示物体位置的第i个角度，为分辨率角度之和，$r_i$表示到该物体的第i个距离。根据变换算法，所有测量数据均以x‐y坐标表示。

3.1.2 降采样

激光扫描仪的分辨率角度越小，每个物体测量到的数据就越多，这表明性能越好。然而，大量的测量数据在算法中处理起来较为复杂，会降低计算速度。因此，本研究提出的降采样算法的目标是仅保留对高效算法计算有意义的数据。降采样的第一步是设置用于取整的网格大小。考虑到本研究中使用的激光扫描仪的分辨率角度和人体尺寸，性能评估所用的网格大小设置为0.25 m × 0.25 m。图3描述了降采样算法的概念。

物体由单层激光扫描仪测量得到原始数据，数据被四舍五入到生成的网格大小。如果数据相互重复，则仅保留其中一个。通过使用降采样算法，可以从单层激光扫描仪的测量数据中获得合理的数据。

3.1.3 聚类

为了在算法中确定物体的精确位置，需要将降采样后的数据表示为一个目标点。因此，在算法中使用降采样后的数据将物体替换为聚类点。第3.1.2节中所述的网格大小也被用于设定聚类尺寸，该尺寸根据人体的宽度被确定为网格大小的三倍。如图4所示，以便于清晰理解。

本研究使用欧几里得最小生成树（EMST）作为聚类算法。EMST 是平面上一组已进行坐标变换的降维点，其中两点之间的欧几里得距离被用作每对点之间边的权重。利用 EMST 计算得到的聚类点，基于扩展卡尔曼滤波器来估计目标速度。图4展示了使用降维数据的聚类算法概念。

3.1.4 基于扩展卡尔曼滤波（EKF）的状态估计

物体的位置已知为聚类点。已进行状态估计，以根据聚类位置计算的物体速度方向进行路径规划。扩展卡尔曼滤波（EKF）是一种鲁棒的滤波器和估计器，已被用于多种仪器和控制例程中。本研究也采用扩展卡尔曼滤波（EKF）进行状态估计。图5描述了卡车与物体之间的运动学关系及各自的速度矢量。

其中 $\vec{v} {obj,x}$ 和 $\vec{v} {obj,y}$ 分别表示相对于x轴和y轴的速度矢量。图5中的黑色圆圈表示来自激光雷达的物体测量数据，位于黑色圆圈中间的红色叉号是聚类点（$p_{x,obj}$；$p_{y,obj}$），如图5所示。状态估计的状态向量和输入向量定义如下：

$$
x = [p_{x,obj}, p_{y,obj}, v_{x,obj}, v_{y,obj}]^T \tag{2}
$$

$$
u = [v_{truck}, w_{truck}]^T \tag{3}
$$

其中，$p_{x,obj}$ 和 $p_{y,obj}$ 为物体的x和y坐标。$v_{x,obj}$ 和 $v_{y,obj}$ 分别为物体的x和y方向速度分量。参数 $v_{truck}$ 和 $w_{truck}$ 分别为卡车的纵向速度和偏航率，它们被用作输入 $u$。

在公式(4)中，扩展卡尔曼滤波（EKF）采用包含 $w_k$ 的状态向量进行设计，其中 $w_k$ 是具有协方差矩阵 $Q_k$ 的高斯噪声，用于EKF的状态非线性函数 $f$ 如公式(4)所示：

$$
x_{k+1} = f(x_k, u_k) + w_k \tag{4}
$$

$$
f(x) = [f_1, f_2, f_3, f_4]^T
$$

$$
f_1 = P_{x,obj} \cos(w_{truck} \Delta t) + P_{y,obj} \sin(w_{truck} \Delta t) \
- v_{truck} \cos(w_{truck} \Delta t)\Delta t + v_{x,obj} \cos(\dot{w} {truck} \Delta t)\Delta t + v {y,obj} \sin(\dot{w}_{truck} \Delta t)\Delta t
$$

$$
f_2 = P_{y,obj} \cos(\dot{w} {truck} \Delta t) - P {x,obj} \sin(\dot{w} {truck} \Delta t) \
+ v {truck} \sin(\dot{w} {truck} \Delta t)\Delta t - v {x,obj} \sin(\dot{w} {truck} \Delta t)\Delta t + v {y,obj} \cos(\dot{w}_{truck} \Delta t)\Delta t
$$

$$
f_3 = v_{x,obj} \cos(\dot{w} {truck} \Delta t) + v {y,obj} \sin(\dot{w}_{truck} \Delta t)
$$

$$
f_4 = v_{y,obj} \cos(\dot{w} {truck} \Delta t) - v {x,obj} \sin(\dot{w}_{truck} \Delta t)
\tag{5}
$$

方程(5) 可由图5 的几何关系得出。方程(5) 中的 $\Delta t$ 为系统的采样时间，在本研究中取为0.1秒。为了对所述非线性方程进行线性化，雅可比矩阵推导如下：

$$
F[i,j] = \frac{\partial f[i]}{\partial x[j]}, \quad G[i,j] = \frac{\partial f[i]}{\partial u[j]} \tag{6}
$$

其中 $F$ 和 $G$ 是雅可比矩阵，分别表示对状态函数和输入函数的偏导数。方程（5）可根据雅可比矩阵进行线性化，如方程（7）所示。

$$
x(k + 1) = Fx(k) + Gu(k) \tag{7}
$$

本研究中，$v_{truck}$ 和 $w_{truck}$ 被假定为零，因为在进行感知算法的性能评估时激光扫描仪是固定的。因此，方程(7)的输入项 $Gu(k)$ 可以省略。测量方程定义了噪声 $v_k$，其为具有协方差矩阵 $R_k$ 的测量噪声，如下所示：

$$
z_{k+1} = H(x_k) + v_k, \quad H = \begin{bmatrix} 1 & 0 & 0 & 0 \ 0 & 1 & 0 & 0 \end{bmatrix} \tag{8}
$$

$$
z = [p_{x,obj}, p_{y,obj}]^T \tag{9}
$$

为了最优地估计状态，以下方程在每个时间实例中被重复计算。

$$
\hat{x}^- k = F {k-1}\hat{x}^+_{k-1} \tag{10}
$$

$$
P^- k = F {k-1}P^+ {k-1} F^T {k-1} + Q_{k-1} \tag{11}
$$

$$
K_k = P^- k H^T {k-1}(H_{k-1}P^- k H^T {k-1} + R_k)^{-1} \tag{12}
$$

$$
\hat{x}^+ k = \hat{x}^-_k + K_k(z_k - H {k-1}\hat{x}^-_k) \tag{13}
$$

$$
P^+ k = (I - K_kH {k-1})P^-_k \tag{14}
$$

其中 $\hat{x}^-_k$ 和 $\hat{x}^+_k$ 分别为先验估计和后验估计。$Q_k$ 和 $R_k$ 被称为噪声的协方差矩阵。$P_k$ 是状态估计误差的协方差，$K_k$ 是卡尔曼增益。因此，可以通过公式(10)–(14)的过程来估计状态。

3.2 路径规划

目标识别与状态估计在感知例程中被引入。目标位置可计算为聚类点，并且可根据前一节中的扩展卡尔曼滤波（EKF）基于聚类点来估计物体的速度矢量。本节讨论用于确定卡车路径的路径规划。路径规划是生成自动驾驶卡车应行驶路径的重要过程。本研究中，该路径具有已使用聚类点和估计速度矢量计算得出。物体的速度方向可通过物体的位置和速度矢量在如公式(15)和(16)所示的线性方程中表达，如图5所示。

$$
y_p = a_l x_p + b_l \tag{15}
$$

$$
a_l = \frac{\vec{v} {obj,y}}{\vec{v} {obj,x}}, \quad b_l = y_c - \frac{\vec{v} {obj,y}}{\vec{v} {obj,x}} x_c \tag{16}
$$

其中参数 $a_l$ 和 $b_l$ 可分别通过速度矢量和物体位置由方程 (16)计算得到，该方程由线性方程生成。此线性方程描述了卡车需遵循的期望路径。因此，通过第3.3节中提出的控制算法，在每一时刻对卡车进行控制以使其沿该直线行驶。

3.3 控制算法

3.3.1 卡车误差动力学

本研究中使用的自动驾驶卡车被假设为配备单轮驱动模块的四轮卡车，前两个轮子可根据卡车速度方向自由旋转。为了推导出使用单轮驱动模块的卡车在目标跟踪控制中的最优转向输入，需要建立简化线性数学模型。该模型基于车辆动力学采用自行车模型构建了简化卡车模型。图6描述了基于自行车模型构建的简化卡车模型。

其中 $l_f$ 为前轴与质心之间的距离，$l_m$ 为单轮驱动模块与质心之间的距离，$l_r$ 为前轴与质心之间的距离。此外，$h_f$ 为前轮的旋转角度。$d$ 表示单轮驱动模块的转向角。$w$ 在图6中表示卡车的偏航率。$F_{yf}$、$F_{ym}$ 和 $F_{yr}$ 分别表示前轮、驱动模块和后轮的侧向力。假设纵向速度恒定且侧滑较小，则侧向力 $F_y$ 可通过侧偏刚度 $C_f$、$C_m$ 和 $C_r$ 计算得到。

$$
F_{yf} = 2C_f \left( h_f - \frac{\dot{y} + l_f \dot{w}}{v_x} \right) \tag{17}
$$

$$
F_{ym} = C_m \left( d - \frac{\dot{y} + l_m \dot{w}}{v_x} \right) \tag{18}
$$

$$
F_{yr} = 2Cr \left( -\frac{\dot{y} + l_r \dot{w}}{v_x} \right) \tag{19}
$$

用于推导误差动态的简化卡车模型，即自行车模型，构建如下。

$$
m(\ddot{y} + v_x \dot{w}) = F_{yf} + F_{ym} + F_{yr} \tag{20}
$$

$$
I_z \ddot{w} = l_f F_{yf} + l_m F_{ym} - l_r F_{yr} \tag{21}
$$

其中 $m$、$v_x$ 和 $\dot{y}$ 分别为卡车的总质量、纵向速度和横向速度。$I_z$ 为车辆绕z轴的转动惯量。基于公式(20)和(21)，构造的状态空间方程如下所示。

$$
\begin{bmatrix}
\dot{y} \
\ddot{y} \
\dot{w} \
\ddot{w} \
\dot{h} f \
\ddot{h}_f
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 & 0 & 0 & 0 & 0 \
0 & A {22} & 0 & A_{24} & A_{25} & 0 \
0 & 0 & 0 & 1 & 0 & 0 \
0 & A_{42} & 0 & A_{44} & A_{45} & 0 \
0 & 0 & 0 & 0 & 0 & 1 \
0 & A_{62} & 0 & A_{64} & A_{65} & A_{66}
\end{bmatrix}
\begin{bmatrix}
y \
\dot{y} \
w \
\dot{w} \
h_f \
\dot{h}_f
\end{bmatrix}
+
\begin{bmatrix}
0 \
\frac{C_f}{m} \
0 \
\frac{l_m C_m}{I_z} \
0 \
0
\end{bmatrix}
d
\tag{22}
$$

其中

$$
A_{22} = \frac{-2C_f - C_m - 2C_r}{m v_x}; \quad A_{23} = \frac{2C_f + C_m + 2C_r}{m};
$$

$$
A_{24} = \frac{-2l_f C_f - l_m C_m + 2l_r C_r}{m v_x}; \quad A_{25} = \frac{2C_f}{m};
$$

$$
A_{42} = \frac{-2l_f C_f - l_m C_m + 2l_r C_r}{I_z v_x}; \quad A_{43} = \frac{2l_f C_f + l_m C_m - 2l_r C_r}{I_z};
$$

$$
A_{44} = \frac{-2l_f^2 C_f - l_m^2 C_m - 2l_r^2 C_r}{I_z v_x}; \quad A_{45} = \frac{2l_f C_f}{I_z}
$$

$$
A_{62} = \frac{2C_f}{I_{wz} v_x}; \quad A_{64} = \frac{2l_f C_f}{I_{wz} v_x}; \quad A_{65} = -\frac{2C_f}{I_{wz}}; \quad A_{66} = -\frac{b}{I_{wz}}
$$

$C_f$、$C_m$ 和 $C_r$ 分别为前轮、中轮和后轮的侧偏刚度系数。$y$ 为横向距离。$I_{wz}$ 为执行器绕z轴的转动惯量。$b$ 为前轮绕z轴的阻尼系数。

本研究设计了误差动态，以使用所构建的卡车模型使卡车轨迹收敛于期望路径。图7描述了本研究中定义的横向误差 $e_1$ 和偏航角误差 $e_2$。基于平面卡车模型，构建了用于推导单轮驱动模块最优转向输入的误差动态。误差 $e_1$ 被定义为期望路径与卡车之间的距离。误差 $e_2$ 被定义为卡车航向方向与期望路径方向之间的夹角。根据误差状态的定义，可利用以下一些方程（Rajamani 2005）推导出最优转向控制输入的误差动态方程。

$$
\ddot{e} 1 = a_y - a {ydes} = (\ddot{y} + v_x \dot{w}) - \frac{v_x^2}{R_{des}} = \ddot{y} + v_x \dot{w} - v_x \omega_{des} \tag{23}
$$

$$
I_z \ddot{e} 2 = I_z \ddot{w} - I_z \ddot{w} {des} \tag{24}
$$

其中 $R_{des}$ 是期望曲率半径。推导出的误差动态可以重写为状态空间中的一个方程。

$$
\begin{bmatrix}
\dot{e} 1 \
\ddot{e}_1 \
\dot{e}_2 \
\ddot{e}_2
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 & 0 & 0 \
0 & A {22} & A_{23} & A_{24} \
0 & 0 & 0 & 1 \
0 & A_{42} & A_{43} & A_{44}
\end{bmatrix}
\begin{bmatrix}
e_1 \
\dot{e} 1 \
e_2 \
\dot{e}_2
\end{bmatrix}
+
\begin{bmatrix}
0 \
B_2 \
0 \
B_4
\end{bmatrix}
d
+
\begin{bmatrix}
0 \
F_2 \
0 \
F_4
\end{bmatrix}
\begin{bmatrix}
\omega {des} \
\ddot{\omega}_{des}
\end{bmatrix}
\tag{25}
$$

其中

$$
B_2 = \frac{2C_f}{m} \left( \frac{l_f + l_r}{l_m + l_r} \right) + \frac{C_m}{m};
$$

$$
B_4 = \frac{2l_f C_f}{I_z} \left( \frac{l_f + l_r}{l_m + l_r} \right) + \frac{l_m C_m}{I_z}
$$

$$
F_2 = \frac{-2l_f C_f - l_m C_m + 2l_r C_r}{m v_x}; \quad F_4 = -1
$$

因此，方程(23)和(24)可表示为方程。(25).

$$
\dot{e} = A_e e + B_e d + F w \tag{26}
$$

本研究假设 $\omega_{des}$ 和 $\ddot{\omega} {des}$ 可以忽略，因为 $\omega {des}$ 的幅值和变化率较小。$A_e$ 和 $B_e$ 分别表示方程(26)中定义的系统矩阵和输入矩阵。基于上述假设，方程(26)可重写如下：

$$
\dot{e} = A_e e + B_e d \tag{27}
$$

下一节描述了基于LQR的跟踪控制算法。

3.3.2 基于LQR的最优转向角计算

为了使 $e_1$ 和 $e_2$ 收敛到零，本研究采用LQR控制器作为最优转向控制控制器。通过最小化方程(28)中定义的性能指标 $J$，可计算出反馈控制增益矩阵 $K$。为了获得最小化方程(27)中定义误差的最优反馈控制增益，需最小化的性能指标如下所示。

$$
J = \frac{1}{2} \int_0^\infty (e^T Q e + d^T R d) dt \tag{28}
$$

$Q$ 和 $R$ 用作权重矩阵。输入 $d$ 被计算为使 $J$ 最小，并与反馈增益 $K$ 一起表示如下：

$$
d_{opt} = -K e \tag{29}
$$

其中 $K$ 由下式给出

$$
K = R^{-1} B_e^T P \tag{30}
$$

其中 $P$ 是正定矩阵，通过代数Riccati方程计算得到，如方程(31)所示。

$$
A_e^T P + P A_e - P B_e R^{-1} B_e^T P + Q = 0 \tag{31}
$$

通过使用方程(29)，可利用线性二次调节器计算目标跟踪控制的最优转向输入 $d_{opt}$。方程 $K$ 在方程(30)中用于计算最优转向输入，可通过MATLAB函数‘lqr’轻松获得。

4 基于实际数据的性能评估

在 MATLAB/SIMULINK 和 LABVIEW 环境中，通过三个实验场景进行了性能评估。根据这三个场景，开展感知程序实验，利用安装在平面上的激光扫描仪对运动物体进行测量。场景1和场景2分别基于直线路径和半圆形路径。场景3基于呈无穷大数学表达式的路径。实验所用激光扫描仪的图片和规格如图8和表1所示。

实际数据来自激光扫描仪，用于执行感知程序。此外，采用感知程序生成的期望路径，对基于线性二次调节器的卡车跟踪控制算法进行了评估。

为了评估感知算法，将激光扫描仪安装在物体的水平面上，用于测量运动物体。

4.1 感知算法的性能评估

图9展示了使用定义的场景构建的用于性能评估的实验环境。感知例程的测试在MATLAB和LABVIEW环境中通过SICK传感器进行。图10显示了根据各个场景得到的目标轨迹。场景1、2和3的轨迹分别如图10的a–c所示。

图11的a、b部分展示了场景1在7.9秒和11.9秒时感知算法的结果，包括坐标变换、降采样、聚类和状态估计的结果。场景2的结果如图12所示，图12的a、b部分展示了6.9 秒和 12.9 秒时的感知算法结果。图13显示了场景3中 a、b 在 23.5 秒和 41.5 秒时的算法结果。图11、12和13的 c 部分显示了 x 和 y 的估计速度结果。物体开始移动后，x 和 y 的速度由扩展卡尔曼滤波（EKF）进行估计，并用于构建期望路径，如方程 (14) 所示。

在图11、12和13的 a、b 部分中，黑色圆圈是来自坐标变换算法的降采样点，蓝色圆圈是表示物体的聚类点，期望路径以红色虚线表示。单层激光扫描仪在 (0,0) 坐标处测量物体，单层激光扫描仪的位置在图11、12和13的 a、b 部分中用红色‘X’标记表示。在图11、12和13的 c 部分中，估计速度初始为零，因为物体从静止开始运动。

4.2 基于仿真的跟踪控制算法评估结果

在MATLAB/Simulink环境中，使用实际数据对跟踪控制算法进行了评估。仿真中将感知算法计算出的实际状态作为自动驾驶卡车的跟踪目标。位置和航向角的建模的卡车的实际期望路径由状态估计算法计算得出，并据此计算了误差动态。卡车仿真的LQR参数如下（表2）。

表2 跟踪控制仿真用LQR参数

为了对本研究中构建的卡车跟踪控制算法进行基于仿真的性能评估，定义了卡车规格的参数。表3显示了用于性能评估的参数。这些参数是使用计算机辅助设计软件得出的，其中侧偏刚度等参数是基于实际车辆参数在此阶段估算的。

表3 卡车参数和初始参数

为了对本研究中构建的卡车跟踪控制算法进行基于仿真的性能评估，定义了卡车规格的参数。表3显示了用于性能评估的参数。这些参数是使用计算机辅助设计软件得出的，其中侧偏刚度等参数是基于实际车辆参数在此阶段估算的。

图14、19和24中的黑点、黑线和蓝点线分别是目标轨迹、物体位置和期望路径。此外，黑框、红点线、红点和蓝点分别表示建模的卡车、卡车轨迹、卡车位置和驱动轮。每种场景下的卡车跟踪控制结果如图14至图28所示。

场景1的卡车轨迹如图14所示，可以看出卡车能够合理地跟踪物体，且卡车位置与期望路径之间的误差相对较小。图15显示了侧滑，即方程($a_l$)中的15。横向与偏航角的误差如图16所示。尽管在由于初始状态与期望路径之间存在差异，如图16所示，误差大约收敛到零。图17显示了卡车的运动情况，包括偏航率和偏航角。场景1中基于LQR推导的最优转向角如图18所示。场景2的结果如图19、20、21、22和23。场景2中卡车的轨迹如图19所示，侧滑情况如图20所示。在大半径情况下，图19和24中的卡车轨迹与物体轨迹不重合，这意味着卡车从当前位置沿期望路径进行高效路径规划。

场景2的横向误差与偏航角如图21所示，场景2的偏航率和偏航角如图22所示。最优转向输入计算结果如图23所示。场景3的轨迹和滑移角分别如图24和25所示。图26显示了误差结果。在起始点处误差值较大，这与场景1和场景2的误差结果类似。偏航率和偏航角如图27所示，计算得到的最优转向输入如图28所示。场景1、2和3的转向输入在初始阶段均表现出较大的值，以从初始条件开始跟踪物体。

图11、12、13、14、15、16、17、18、19、20、21、22、23、24、25、26、27和28描述了跟踪控制算法的结果。基于激光扫描仪的感知算法识别了物体并进行了状态估计。坐标变换、降采样、聚类和状态估计算法的结果在第4.1节中作为感知算法结果展示。期望路径根据估计状态进行计算，并在感知算法结果中显示。所计算的状态包括聚类点、目标速度以及期望路径被用于卡车的跟踪控制。基于感知算法计算出的状态，推导出了误差动态，并得到了最优转向输入。使用基于线性二次调节器的误差动态计算，并且在带有误差的仿真评估结果中，卡车能够很好地跟踪物体。

5 结论

本文提出了一种使用单层激光扫描仪的自动驾驶卡车目标跟踪控制算法，该卡车配备单轮驱动模块。为了构建跟踪控制算法，采用单层激光扫描仪测量目标的距离和角度信息。基于测得的距离和角度等信息，在感知阶段通过坐标变换、降采样和聚类等多种算法对目标状态进行估计。计算得到的聚类点用于估计目标的速度和位置，进而推导出期望路径。根据基于自行车模型的平面卡车模型，利用横向误差和偏航角误差等状态量推导出线性误差动态。基于所推导的卡车误差动态，采用线性二次调节器计算最优转向输入。用于最小化定义的误差。为了对本研究中提出的跟踪控制算法进行合理的性能评估，使用激光扫描仪的实际数据进行了基于仿真的性能评估。结果表明，估计算法能够合理地估计物体的位置和速度等状态。跟踪控制算法表现出良好的评估结果，卡车能够在合理的误差范围内稳定地跟踪目标。然而，在生成合适的期望路径方面存在局限性，因为直线并非目标跟踪的最优路径。此外，该方法假设卡车的速度和卡车的偏航率均为零，因此存在一定的局限性。

该研究侧重于使用实车激光雷达数据进行感知算法的性能评估以及基于仿真的跟踪控制性能。因此，开发优化的路径规划算法和实车以增强和验证控制性能被视为未来工作。此外，由于本文提出的控制算法在当前阶段仅专注于跟踪控制算法的评估，未考虑与物体的安全距离（如结果所示），因此目标分类、目标选择和紧急停止算法的开发也被视为实现安全可靠的跟踪控制的未来工作。预计该卡车的跟踪控制算法可应用于工厂自动化和物流系统自动化等不同应用领域的自动驾驶卡车的跟踪控制算法。