CN108958246A - 无人车在U-Turn场景的轨迹跟踪控制方法 - Google Patents

Abstract

本发明提供了一种无人车在U‑Turn场景中的轨迹跟踪控制方法，包括以下步骤：步骤1，构建无人车的车辆运动学方程；步骤2，通过车辆运动学方程，构建离散预测控制器模型；步骤3，将离散预测控制器模型转化为标准二次形式的目标函数J'(k)；步骤4，结合控制量约束条件，求解标准二次形式的目标函数J'(k)，得到输入的控制量增量。

Description

无人车在U-Turn场景的轨迹跟踪控制方法

技术领域

本发明属于车辆行为决策控制领域，具体涉及一种无人车在U-Turn场 景的轨迹跟踪控制方法。

背景技术

U-turn是一个常见的交通情况，在明显降低车速的同时也影响着交通流 量。如果车辆在U-turn操作下加速，则可能会导致侧滑或翻车事故。由于 U-turn对交通有较大的影响，因此一些国家或地区禁止了车辆的U-turn操 作，让车辆通过其他绕行方式实现U-turn，从而造成旅行时间延长和燃料的 额外消耗。

由于无人车具有自主路径规划和轨迹跟踪的功能，在进行U-turn操作 时，可以根据高精度地图先验信息及传感器检测到的周围车辆位置信息和道 路设施信息并进行快速轨迹规划和轨迹控制，实现U-turn的快速通行，从 而节省旅行时间和燃料消耗，并将最大限度地减少车辆对道路交通的影响。 轨迹跟踪控制的目的是让无人驾驶车辆跟踪由规划算法得到的轨迹，主要任 务是根据车辆的运动学约束输出相应的控制参数。

目前应用较多的控制算法有PID算法，滑模控制算法，神经网络控制 算法等，这些方法对参数和环境的依赖程度较高，当环境发生较大的变化时， 不能很好的适应新状态下的跟踪。同时，地面无人车在行驶过程中不仅要考 虑运动学约束，还要考虑在高速行驶状态下的动力学约束。

发明内容

针对现有技术存在的不足，本发明的目的在于，提供一种无人车的轨 迹跟踪控制方法，解决现有无人车控制方法在U-turn场景下无法很好的适应 新状态进行跟踪，并且均没有考虑无人车在高速行驶状态下的动力学约束问 题。

为了解决上述技术问题，本发明采用如下技术方案予以实现：

一种无人车在U-Turn场景的轨迹跟踪控制方法，包括以下步骤：

步骤1，构建式(1)所示的无人车的车辆运动学方程；

式(1)中，χ为状态量，u为控制 量，u＝[v_r,δ_f]^T；x_r为无人车后轴的轴心横坐标，y_r为无人车后轴的轴心纵 坐标，为无人车的航向角，v_r为无人车后轴的速度，δ_f为无人车的前轮 转角，l为无人车前轴和后轴的轴距；

步骤2，通过无人车的车辆运动学方程，构建式(2)所示的离散预测 控制器模型：

式(2)中，为第k+1个采样时刻的估计状态量，为第k个 采样时刻的估计状态量，为第k个采样时刻的估计控制量；

v₀为预设参考无人车后轴的速度，为预设参 考无人车的航向角，T为采样时间；

δ₀为预设参考无人车的前轮转角，l为 无人车前轴和后轴的轴距；

步骤3，将离散预测控制器模型转化为式(3)所示的标准二次形式的 目标函数J'(k)：

式(3)中，为包含k时刻控制量的矩阵，

为k时刻的控制量；

f^T(k)为f(k)的转置，

Q和R是权重矩阵；

步骤4，结合式(4)所示的控制量约束条件，求解标准二次形式的目 标函数J'(k)，得到输入的控制量增量其中

式(4)中，u_min为控制时域内的控制量最小值，u_max为控制时域内的控 制量最大值，u₀(k+j)为预设参考无人车在(k+j)时刻的控制量。

本发明与现有技术相比，具有如下技术效果：

(1)本发明利用较为简单的方式处理由于状态或输入限制约束的最优 控制问题，从而实现实时车辆控制；

(2)本发明对参数和环境的依赖程度较低，当环境发生较大的变化时， 可以很好的适应新状态下的跟踪；

(3)本发明容易被建模和控制，也具有很好的鲁棒性。并且，对未来 轨迹的预测和处理多目标约束条件的能力较强，可以用于解决多变量和约束 的问题，并实现在线优化；

(4)本发明采用标准二次规划来求解出最优控制序列，通过在线优化 —反馈校正—优化这一模式实现对轨迹的跟踪控制；

(5)本发明对线性化的误差模型做离散化处理，从而得到离散的时间 模型来应用于模型预测控制器的设计。

(6)本发明考虑到在U-turn场景下无人车易发生侧滑，用横向梯度来 计算道路的安全速度；当无人车在具有较大的附着系数的道路上高速行驶 时，无人车将会有由离心力和横向附着力引起的翻车现象，对此，本发明根 据较大路面附着系数下的车辆临界翻车速度来控制车辆实际速度。

附图说明

图1为无人车运动学模型；

图2为无人车在U-Turn场景中的轨迹跟踪控制的策略图；

图3是本发明的无人车在U-Turn场景中的轨迹跟踪控制的算法流程图；

图4是无人车模拟U-Turn轨迹跟踪结果；

图5是在CU_CVIS测试场进行U-Turn轨迹跟踪测试；

图6是无人车在真实场景下进行20次U-TURN的轨迹。

以下结合附图对本发明的具体内容作进一步详细解释说明。

具体实施方式

本发明结合构建的无人车运动学模型，创建目标函数和约束条件，并对 目标函数进行优化求解，得到当前时刻的最优控制序列，并根据每一阶段的 状态量输出来重新预测未来时域的轨迹跟踪状况，求解新的控制序列，直至 完成对轨迹的跟踪。

以下给出本发明的具体实施例，需要说明的是本发明并不局限于以下 具体实施例，凡在本申请技术方案基础上做的等同变换均落入本发明的保 护范围。

实施例：

本实施例提供了一种无人车在U-Turn场景的轨迹跟踪控制方法，如图 3，包括以下步骤：

步骤1，在路面状况良好的低速工况下，无人车运动学模型可以简化为 两轮自行车模型。

在平面直角坐标系下，无人车的运动学模型如图1所示。其中，(x_r,y_r) 和(x_f,y_f)分别为无人车后轴和前轴的轴心的坐标，v_r和v_f分别为无人车后轴 和前轴中心的速度，为车体的航向角，δ_f为前轮转角，l为无人车前后轴 的轴距。

从图1中可得到式(5)所示的运动关系：

其中，和分别是无人车后轴的横向速度和纵向速度。

通过整理式(5)的运动关系，得到式(1)所示的无人车的车辆运动学 方程；

式(1)中，χ为状态量，u为控制 量，u＝[v_r,δ_f]^T；x_r为无人车后轴的轴心横坐标，y_r为无人车后轴的轴心纵 坐标，为无人车的航向角，v_r为无人车后轴的速度，δ_f为无人车的前轮 转角，l为无人车前轴和后轴的轴距；

步骤2，通过车辆运动学方程，构建式(2)所示的离散预测控制器模 型：

式(2)中，为第k+1个采样时刻的估计状态量，为第k个 采样时刻的估计状态量，为第k个采样时刻的估计控制量；

v₀为预设参考无人车后轴的速度，为预设参 考无人车的航向角，T为采样时间；

δ₀为预设参考无人车的前轮转角，l为 无人车前轴和后轴的轴距；

本实施例中预设的参考无人车为参考车辆按照预设的参考轨迹进行行 驶的无人车，使用0表示参考无人车，则参考无人车的运动学方程为：

采用泰勒级数展开并忽略高阶项，可以将式(6)近似表示为：

将式(6)与式(7)相减，可以得到线性化的无人车误差模型：

其中，

将时间离散化，有其中T是采样时间。

对式(8)做离散化处理，得到式(2)所示的离散的时间模型，即为离 散预测控制器模型。

步骤3，将离散预测控制器模型转化为式(3)所示的标准二次形式的 目标函数J'(k)：

式(3)中，为包含k时刻控制量的矩阵，

为k时刻的控制量；

f^T(k)为f(k)的转置，

Q和R是权重矩阵；

本实施例为了使目标函数转换成标准二次规划形式，使用以下目标函数 来设计轨迹跟踪控制器：

式(9)中，Q和R是权重矩阵。第一项反映了系统对参考轨迹的跟随 能力；第二项反映了对控制量变化的约束。因此，

N_c是控制输入的采样点数，N_p是系统状态输出的预测点数，通常 N_p≥N_c。

式(9)可以表示为：

其中，

为了简化式(10)，假设N_p＝N_c＝N，则得到式(11)：

其中，

根据式(10)和式(11)，目标函数可以转换为标准二次形式：

式(12)中，

L(k)可以忽略，因为它与无关，而且不影响u^*的确定。

因此，最终得到式(3)所示的标准二次形式的目标函数J'(k)。

步骤4，结合式(4)所示的控制量约束条件，求解标准二次形式的目 标函数J'(k)，得到输入的控制量增量其中

式(4)中，u_min为控制时域内的控制量最小值，u_max为控制时域内的控 制量最大值，u₀(k+j)为预设参考无人车在(k+j)时刻的控制量；

本实施例中控制量中的速度偏差v_r-v_d和前轮偏角δ取值范围为：

v_d为车辆期望速度。

速度变化量Δv和前轮偏角变化量Δδ的取值范围为：

由于车辆在U-turn时有可能会出现侧滑或者侧倾现象，因此本发明还 考虑了对无人车速度的约束；

根据道路设计标准，考虑到车辆侧滑，用横向梯度计算道路的安全速度 (有内外侧侧滑的临界速度)的公式是：

其中，μ是路面的附着系数；β是弯道路面横向坡度；R是车辆运行轨 迹的半径；g是重力加速度。

当无人车在具有较大的附着系数的道路上高速行驶时，车辆将会有由离 心力和横向附着力引起的翻车现象。翻车转态可能导致车辆重心转移到外部 轮胎(即横向载荷转移现象)。一旦翻转增加到使轮胎内侧离开路面的程度， 就会发生翻车。在较大的路面附着系数下的车辆临界翻车速度，v_r为：

其中B是车辆宽度，h是车辆重心高度。

车辆翻车的主要因素是重心高度-h，如果重心高度足够低，则不会发生 翻车。在本发明中，研究对象是小型无人车，通常重心高度相对较低。因此， 道路安全速度-v_r大于v_p。

对于步骤3得到的式(3)所示的目标函数，结合式(4)所示的控制量 约束条件，可通过在线二次规划问题求解器(如MTALAB)求解。

将u(k)作为当前控制量输入给被控无人车的车辆运动学方程中，被控无 人车按照当前控制量u(k)进行控制并得到状态输出量观测值x(t)，然后将此 值输入到线性车辆误差模型式(2)中，接着，再次将此误差模型转换为式 (3)所示的标准二次形式的目标函数并进行最优化求解，以得到未来一段 时间的控制序列。如此循环，就实现了对车辆的轨迹跟踪。如图2为无人车 的U-turn轨迹跟踪控制策略：它从第k个采样周期开始，通过优化生成一 组控制增量并将该控制序列的第一个元素作为 受控对象的实际控制量，当来到下一时刻时，重复上述过程，如此滚动地完 成一个个带约束的优化问题，以确保在预计时间范围[k，k+N_p]预测轨迹与 参考车辆的轨迹高度吻合，同时也限制了控制输出的最小量变化。

本实施例使用以下参数μ＝0.65,β＝3°,g＝9.8m/s²，h＝0.7m,R＝10m， v_p＝8.442m/s。在不同车速情况下，模拟U-Turn轨迹跟踪结果如图4所示。 仿真实验表明，速度从8.44m/s变化到3.6m/s，负步长为-0.3。本发明使 用以下参数μ＝0.65,β＝3°,g＝9.8m/s²，h＝0.7m,R＝10m，v_p＝8.442m/s。在 不同车速情况下，速度从8.44m/s变化到3.6m/s，负步长为-0.3。从图4 中可以看出，当车速降低时，车辆运动轨迹越接近参考轨迹。

同时，本实施例计算了实验数据，并用轨迹重叠率(TOR)来呈现仿真 结果，其定义如下：

式中，N是0.04s的采样周期内的总轨迹点数，N_b是位于参考轨迹缓冲 区中的轨迹点数。该参考轨迹缓冲区是一个宽度等于车辆宽度的条带状区 域，并且将参考轨迹作为平均轴。

图5是在CU_CVIS测试场进行U-Turn轨迹跟踪测试。我们进一步将 所提出的算法应用到现实的测试环境中，并进行10次相同的U-Turn轨迹跟 踪实验。

图6是无人车在真实场景下进行20次U-TURN的轨迹。实验结果显示 该算法可以同仿真实验产生非常一致的结果，进一步证明了所提出方法的有 效性。

表1-表3是5辆相同小车的仿真数据结果。

表1

表2

表3

表4

其中表1为5辆相同小车的仿真数据结果(R＝10m)，表2为5辆相同小 车的仿真数据结果(R＝15m)。表3为5辆相同小车的仿真数据结果(R＝20m)。

本实施例计算了实验数据，并用轨迹重叠率(TOR)来呈现仿真结果， 其定义如下：

式中，N是0.04s的采样周期内的总轨迹点数，N_b是位于参考轨迹缓冲 区中的轨迹点数。该参考轨迹缓冲区是一个宽度等于车辆宽度的条带状区 域，并且将参考轨迹作为平均轴。从表1，2，3均可看出，无论转弯半径为 多少时，均有速度越小，轨迹重叠率越高这一结论，则表明提出的U-Turn 算法在重复性，鲁棒性和轨迹跟踪方面性能良好。

表4是无人车在真实场景下进行20次U-TURN的数据结果。由于实车 配备了ESP等电子设备来帮助控制车辆的稳定性，因此相同的MPC算法对 实车的控制效果要好于仿真的结果。

Claims (1)

1.一种无人车在U-Turn场景中的轨迹跟踪控制方法，其特征在于，包括以下步骤：

步骤1，构建式(1)所示的无人车的车辆运动学方程；

式(1)中，χ为状态量，u为控制量，u＝[v_r,δ_f]^T；x_r为无人车后轴的轴心横坐标，y_r为无人车后轴的轴心纵坐标，为无人车的航向角，v_r为无人车后轴的速度，δ_f为无人车的前轮转角，l为无人车前轴和后轴的轴距；

步骤2，通过无人车的车辆运动学方程，构建式(2)所示的离散预测控制器模型：

式(2)中，为第k+1个采样时刻的估计状态量，为第k个采样时刻的估计状态量，为第k个采样时刻的估计控制量；

v₀为预设参考无人车后轴的速度，为预设参考无人车的航向角，T为采样时间；

δ₀为预设参考无人车的前轮转角，l为无人车前轴和后轴的轴距；

步骤3，将离散预测控制器模型转化为式(3)所示的标准二次形式的目标函数J'(k)：

式(3)中，为包含k时刻控制量的矩阵，

为k时刻的控制量；

f^T(k)为f(k)的转置，

Q和R是权重矩阵；

步骤4，结合式(4)所示的控制量约束条件，求解标准二次形式的目标函数J'(k)，得到输入的控制量增量其中

式(4)中，u_min为控制时域内的控制量最小值，u_max为控制时域内的控制量最大值，u₀(k+j)为预设参考无人车在(k+j)时刻的控制量。