CN116227221A - 一种基于最优控制的二维轨迹重构方法 - Google Patents

Abstract

本发明提供一种基于最优控制的二维轨迹重构方法，属于交通数据处理技术领域，针对采集到的车辆轨迹数据，运用最优控制理论对采集得到的实测轨迹进行重构，在保障重构结果符合车辆运动约束的基础上，消除轨迹数据采集过程中产生的误差和噪音，提高轨迹数据的质量；该重构方法以重构轨迹和实测轨迹的差异最小为优化目标，将车辆的方位角变化和加速度作为控制变量，考虑车辆的二维运动变量合理范围约束条件，建立最优控制模型进行轨迹重构。与现有的基于平滑或滤波的重构方法不同，该方法提供了一种明确且可解释的方式来保证重构轨迹的真实性，从而能够在消除轨迹数据误差的同时，重构出车辆动态更加真实的车辆轨迹。

Description

一种基于最优控制的二维轨迹重构方法

技术领域

本发明属于交通数据处理技术领域，尤其涉及一种基于最优控制的二维轨迹重构方法。

背景技术

高分辨率车辆轨迹数据在近二十年的交通研究中得到了广泛应用，包括交通行为分析、交通流建模与控制、交通状态评价等。然而车辆轨迹数据采集过程中存在的固有误差和噪声会导致车辆轨迹产生不真实(不符合车辆运动约束)的动态，影响了相关研究的有效性。为了提高高分辨率车辆轨迹数据的质量，一系列对车辆轨迹进行重构的方法被提出。然而针对车辆二维行驶轨迹，目前的重构方法无法保障重构轨迹的各项运动参数完全符合车辆运动约束，并且也未检索到解决该问题的发明专利。

经对现有技术的文献检索发现，有关车辆轨迹的重构方法，主要有以下两种：

1、一维轨迹重构方法。是指重构车辆的纵向动态变量，包括纵向位置、速度、加速度和加加速度。主要采用移动平均法、b样条平滑法、Savitzky–Golay滤波等方法对纵向位置、纵向速度、纵向加速度进行平滑。代表性研究成果包括《Utilizing UAV video datafor in-depth analysis of drivers crash risk at interchange merging areas》《Tracking vehicle trajectories and fuel rates in phantom traffic jams:Methodology and data》《A method to account for non-steady state conditions inmeasuring traffic hysteresis》。

2、二维轨迹重构方法。是指同时重构车辆的纵向和横向动态变量。主要采用移动平均法、小波变换、卡尔曼滤波等方法对车辆纵向和横向的位置、车辆的方位角及横向偏移进行平滑。代表性研究成果包括《Examining traffic conflicts of up stream tollplaza area using vehicles trajectory data》《Vehicle path reconstruction usingRecursively Ensembled Low-pass filter(RELP)and adaptive tri-cubic kernelsmoother》《一种交叉口车辆二维轨迹重构方法》(专利申请号202210897067.4)。

对于方法1一维轨迹重构方法，主要采用平滑技术直接从原始数据重构车辆轨迹，由于纵向变量之间的相互关联性，同时对多个纵向变量进行平滑可能会导致平滑结果的变量间不一致，因此大多数研究只平滑了其中一个参数。为了保持纵向整体轮廓的真实性，一些研究将车辆的纵向位置作为平滑变量。然而对纵向位置进行平滑，很难确保其他纵向动态变量的真实性。针对这个问题，《Tracking vehicle trajectories and fuel rates inphantom traffic jams:Methodology and data》中通过为四阶加权b样条平滑器的平方损失函数选择权重，将加速度保持在物理合理范围内。除了对纵向位置直接平滑，另一种方法是将位置的微分(即速度或加速度等)作为平滑参数，但由于相邻时间窗口的误差累积，可能导致实测轨迹与重构轨迹之间的巨大差异。

对于方法2二维轨迹重构方法，由于二维条件下由相邻时间窗累积误差引起的位置误差比一维条件下更严重，轨迹的整体轮廓可能会被改变，因此几乎所有的研究都使用二维坐标作为平滑变量。与一维轨迹重构不同，对二维轨迹坐标重构时需要同时考虑纵向和横向动态变量的合理性。《Vehicle path reconstruction using RecursivelyEnsembled Low-pass filter(RELP)and adaptive tri-cubic kernel smoother》提出了一个三阶段的车辆路径重构框架。在第二阶段中，他们利用RELP(基于S-G滤波器的集成滤波器)得到的速度和运动学方程来调整车辆的位置以保证内部一致性，第三阶段运用网格搜索算法寻找最优平滑参数，使重构轨迹的动态参数更加合理。

现有的一维和二维轨迹重构方法主要基于平滑或滤波技术，平滑方法的选择和平滑参数的设置对重构的结果至关重要，过度平滑会使平滑后的轨迹与实测轨迹差异过大，导致轨迹数据失去原有的交通流特性，而欠平滑会导致车辆动态不真实，如何在防止过度平滑的条件下重构出真实的轨迹是目前二维轨迹重构的主要问题。针对这一问题，目前的研究主要通过根据实测数据集搜索最优的平滑或过滤参数，或者用给定值替换不真实值来优化结果，然而这些方法并没有解决保障重构轨迹的各项运动参数完全符合车辆运动约束的问题，其原因在于：(1)平滑参数与车辆运动变量的合理性之间的关系是隐式的，难以确定合适的参数使结果符合物理约束，即便依据数据集找出轨迹平滑的最优参数，由于不同的实测轨迹数据的误差性质不同，该最优参数的可移植性较差；(2)即使假设符合车辆运动约束的部分是正确的，相邻平滑窗口之间的误差可能会累积，导致实测轨迹与重构轨迹不一致。因此，现有的基于平滑/滤波的重构方法存在缺陷，需要新的方法来解决。

发明内容

本发明的目的在于提供一种基于最优控制的二维轨迹重构方法，其特征在于，根据车辆运动建立状态演变、以重构轨迹和实测轨迹间的差异最小作为目标函数，并考虑车辆二维运动参数取值范围约束条件构建优化模型，优化模型为最优控制模型，对优化模型求解，得到重构后的二维轨迹。

进一步地，目标函数为最小化重构轨迹从初始状态至实际终点状态的成本，成本定义为重构轨迹与实际轨迹的均方根误差，通过公式一表示为：

公式一中，X(t)为车辆在t时刻的状态变量；U(t)为车辆在t时刻的控制变量；t₀为实测轨迹的初始时间；t_f为实测轨迹的重点时间；(t_f-t₀)为车辆通过路口所花费的时间；L(X(t))为重构轨迹在t时刻的成本，通过公式二计算，公式二表示为：

L(X(t))＝d(t)²＝(x(t)-x⁰(t))²+(y(t)-y⁰(t))²

公式二中，d(t)为t时刻重构轨迹点与实测轨迹点之间的欧氏距离；x⁰(t)为优化模型输入参数中，t时刻实测轨迹的横坐标；y⁰(t)为优化模型输入参数中，t时刻实测轨迹的纵坐标；x(t)为优化模型输出参数中，t时刻实测轨迹的横坐标；y(t)为优化模型输出参数中，t时刻实测轨迹的纵坐标。

进一步地，根据车辆运动建立状态演变为车辆在t时刻的状态变量随时间的改变，其中车辆在t时刻的状态定义公式三，公式三表示为：

公式三中，θ(t)为车辆在t时刻的方位角；v(t)为车辆在t时刻的纵向速度；车辆的状态演变通过公式四表示为：

X(t+Δt)＝X(t)+ΔtX′(t)

公式四中，X't为车辆在t时刻状态的一阶导数；Δt为轨迹采样的时间步长；X't的计算公式通过公式五表示为：

公式五中，k(t)为车辆在t时刻的曲率，a(t)为在t时刻车辆的纵向加速度。

进一步地，将k和a组成控制变量U(t)，通过公式六表示为：

进一步地，车辆二维运动参数取值范围约束包括车辆速度约束、纵向加速度约束、加加速度约束、曲率约束、横向加速度约束和曲率变化约束。

进一步地，车辆速度约束，由于交叉口内车辆几乎都是向前行驶，不会出现倒行现象，因此车辆的行驶速度应该大于等于0，同时根据交通规则，最大速度也应该小于等于一个合理的值，通过公式七表示为：

0≤v(t)≤v_max

公式七中，v_max为最大加速度；

纵向加速度约束即最小纵向加速度和最大纵向加速度受车辆特性的限制，通过公式八表示为：

a_min≤a(t)≤a_max

公式八中，a_min为最小纵向加速度；a_max为最大纵向加速度；

加加速度约束定义为单位时间内纵向加速度的变化量，是评价车辆纵向运动变化带来的舒适性的一个重要变量，其可接受范围，通过公式九表示为：

公式九中，j_max为最大加加速度；

曲率约束为车辆转弯时，最小转弯半径受到限制，通过公式十表示为：

所述公式十中，r_min为最小转弯半径；

横向加速度约束，除了纵向加速度外，横向加速度也受到车辆特性的限制，通过公式十一表示为：

-g(e+f_s)≤v(t)²k(t)≤g(e+f_s)

公式十一中，g为重力加速度；e为道路超仰角；f_s为道路横向摩擦系数；

曲率变化约束，人类驾驶员为了驾驶舒适性，不会频繁地转动方向盘，故曲率变化位于一个合理范围内，通过公式十二表示为：

所述公式十二中，k_max为单位时间内曲率的最大变化量。

进一步地，具体包括如下步骤：

S1：测量车辆轨迹，得到轨迹数据，根据轨迹数据构建初始时刻的车辆状态变量X(0)；

S2：将初始时刻的车辆状态变量X(0)和控制变量U(t)代入公式四，得到状态变量基于控制变量的递推函数；

S3：利用递推函数将模型改写为以曲率和加速度为决策变量的非线性规划模型，并将输入参数x⁰(t)、y⁰(t)、v_max、a_min、a_max、j_max、k_max、r_min、g、e和f_s代入非线性规划模型；

S4：求解非线性规划模型，得到最优控制变量，并抵用递推关系算出最优的重构轨迹。

进一步地，S4中，通过非线性求解器或求解软件对所属非线性规划模型求解。

进一步地，最优控制变量记为U^*(t)，递推关系通过公式十三计算，公式十三表示为：

X^*(t+Δt)＝f(X(0),U^*(t))

公式十三中，X^*为最优的重构轨迹状态变量；f为S2中得到的状态变量基于控制变量的递推函数。

与现有技术相比，本发明的有益效果主要体现在：

1、本发明以重构轨迹与实测轨迹的差异最小化为目标，以详细的车辆运动参数合理范围为约束条件，具有可解释性。

2、本发明提供了一种显式的方法来保障重构结果符合车辆运动约束和物理运动约束，克服了以往方法平滑参数与车辆运动变量的合理性之间的关系是隐式的问题。

3、本发明的方法有一定的灵活性，模型的目标函数可以替换为其他成本函数或者进行扩展以同时考虑不同的目标，约束可以替换为针对当前问题的不同车辆运动模型和阈值，以运用在不同的场景。

附图说明

图1为本发明方法的流程图；

图2为本发明实施例的重构结果对比图。

具体实施方式

下面将结合示意图对本发明一种基于最优控制的二维轨迹重构方法进行更详细的描述，其中表示了本发明的优选实施例，应该理解本领域技术人员可以修改在此描述的本发明，而仍然实现本发明的有利效果，因此，下列描述应当被理解为对于本领域技术人员的广泛知道，而并不作为对本发明的限制。

如图1和2所示，图1为本发明一种基于最优控制的二维轨迹重构方法的流程图，图2为本实施例的实测轨迹，实测轨迹数据如表1所示包括如下步骤：

现对该实测轨迹进行重构，采用本发明中的方法，在符合约束条件的情况下得出最优的重构轨迹，并将重构轨迹与实测轨迹、常用平滑/滤波重构得到的轨迹进行对比。模型输入参数为，t时刻实测轨迹的横、纵坐标，x⁰(t)和y⁰(t)，对应表1中的取值；最大车速，v_max，取23m/s；最小、最大纵向加速度，a_min、a_max，分别取-5m/s²和5m/s²；最大加加速度，j_max，取10m/s³；最小转弯半径，r_min，取5m；重力加速度，g，取9.8m/s²；超仰角，e，取0；侧摩擦系数，f_s，取0.18；单位时间内曲率的最大变化量，k_max，取0.1，rad/m/s；轨迹采样的时间步长，Δt，取0.05s；实测轨迹的初始时间，t₀，取0s；实测轨迹的终点时间，t_f，取8.5s；实测轨迹的点数，n，为167；

表1

具体过程简述如下：

步骤1：根据实测轨迹的坐标值，可以计算出初始时刻的状态变量X(0)。其中初始时刻的状态变量的横、纵坐标值与实测轨迹在t₀时刻的横、纵坐标值相同，初始时刻的状态变量的方位角和速度可分别取实测轨迹前六个时刻(t₀到t₅)方位角和速度的平均值。

步骤2：将初始状态变量X(0)和控制变量U(t)代入公式四，可以得出状态变量基于控制变量的递推函数。

.....

X(t+Δt)＝f(X(0),U(t)

v(t+Δt)＝f_v(v(0),a(t)

步骤3：利用递推函数将模型改写为以曲率和加速度为决策变量的非线性规划模型，并将上述输入参数带入模型。

S.t. 0≤f_v(v(0),a(t-Δt))≤23

-5≤a(t)≤5

-9.8×(0+0.18)≤(f_v(v(0),a(t-Δt)))²k(t-Δt)≤9.8×(0+0.18)

步骤4：上述非线性规划模型，可通过非线性求解器或求解软件,如用Python调用数值优化工具箱CasADi进行求解。优化结果为最优曲率和加速度，即最优控制变量U^*(t)，利用递推关系X^*(t+Δt)＝f(X(0),U^*(t))算出最优的重构轨迹。

重构得到的轨迹数据如表2所示。

时刻(s)	实测横坐标(m)	实测纵坐标(m)	重构横坐标(m)	重构纵坐标(m)
					0	61.64254959	39.08286686	61.64254959	39.08286686
0.05	61.27705059	39.08287861	61.43822256	39.04903
					0.1	60.91526225	39.08289024	61.22108336	39.01618414
0.15	61.68508022	38.9409471	60.99113172	38.98460688
					……	……	……	……	……
8.4	32.36044331	9.108288102	32.35101	9.317916
					8.45	32.35638872	9.109146478	32.37461	9.048176
8.5	32.39891935	8.967226715	32.4012	8.791277

表2

步骤5：设计方案评价。将重构轨迹与实测轨迹间的RMSE和重构轨迹的不真实点占轨迹总点数的百分比作为评价指标，对现有的基于平滑/滤波的重构方法和本发明的重构方法进行对比。其中轨迹的不真实点是指该轨迹点对应的速度、纵向加速度、加速度、曲率、重力加速度、曲率变化等车辆动态变量值不位于前文规定的合理范围内。用于对比的方法为移动平均值法(简称MA)和Savitzky-Golay滤波器(简称SG)，其中移动平均值法的平滑窗口设为0.55s，Savitzky-Golay滤波的平滑窗口设为0.55s，拟合曲线方程的阶数设为3。

如表3和表4所示，按本发明方法对实测轨迹进行重构，从轨迹一致性的角度看，该方法略微优于用于对比的方法，与对比的两种方法中的最好值相比，该方法的RMSE从0.835268降低到0.832085，降低了0.38％。从轨迹的真实性角度看，本发明的重构轨迹的不真实数据占比为0，而现有基于平滑的轨迹重构模型中，所有车辆运动变量的不真实数据占比均大于0。故本发明的方法在以最小的RMSE代价重构出了符合物理逻辑的轨迹。

重构方法	RMSE(m)
		本发明	0.832085
移动平均值法	0.835268
		Savitzky-Golay滤波法	0.838899

表3

表4

上述仅为本发明的优选实施例而已，并不对本发明起到任何限制作用。任何所属技术领域的技术人员，在不脱离本发明的技术方案的范围内，对本发明揭露的技术方案和技术内容做任何形式的等同替换或修改等变动，均属未脱离本发明的技术方案的内容，仍属于本发明的保护范围之内。

Claims (9)

1.一种基于最优控制的二维轨迹重构方法，其特征在于，根据车辆运动建立状态演变、以重构轨迹和实测轨迹间的差异最小作为目标函数，并考虑车辆二维运动参数取值范围约束条件构建优化模型，所述优化模型为最优控制模型，对所述优化模型求解，得到重构后的二维轨迹。

2.根据权利要求1所述的基于最优控制的二维轨迹重构方法，其特征在于，所述目标函数为最小化重构轨迹从初始状态至实际终点状态的成本，成本定义为重构轨迹与实际轨迹的均方根误差，通过公式一表示为：

所述公式一中，X(t)为车辆在t时刻的状态变量；U(t)为车辆在t时刻的控制变量；t₀为实测轨迹的初始时间；t_f为实测轨迹的重点时间；(t_f-t₀)为车辆通过路口所花费的时间；L(X(t))为重构轨迹在t时刻的成本，通过公式二计算，所述公式二表示为：

L(X(t))＝d(t)²＝(x(t)-x⁰(t))²+(y(t)-y⁰(t))²

所述公式二中，d(t)为t时刻重构轨迹点与实测轨迹点之间的欧氏距离；x⁰(t)为所述优化模型输入参数中，t时刻实测轨迹的横坐标；y⁰(t)为所述优化模型输入参数中，t时刻实测轨迹的纵坐标；x(t)为所述优化模型输出参数中，t时刻实测轨迹的横坐标；y(t)为所述优化模型输出参数中，t时刻实测轨迹的纵坐标。

3.根据权利要求1所述的基于最优控制的二维轨迹重构方法，其特征在于，根据车辆运动建立状态演变为车辆在t时刻的状态变量随时间的改变，其中车辆在t时刻的状态定义公式三，所述公式三表示为：

所述公式三中，θ(t)为车辆在t时刻的方位角；v(t)为车辆在t时刻的纵向速度；车辆的状态演变通过公式四表示为：

X(t+Δt)＝X(t)+ΔtX′(t)

所述公式四中，X'(t)为车辆在t时刻状态的一阶导数；Δt为轨迹采样的时间步长；所述X'(t)的计算公式通过公式五表示为：

所述公式五中，k(t)为车辆在t时刻的曲率，a(t)为在t时刻车辆的纵向加速度。

4.根据权利要求3所述的基于最优控制的二维轨迹重构方法，其特征在于，将k和a组成控制变量U(t)，通过公式六表示为：

5.根据权利要求1所述的基于最优控制的二维轨迹重构方法，其特征在于，所述车辆二维运动参数取值范围约束包括车辆速度约束、纵向加速度约束、加加速度约束、曲率约束、横向加速度约束和曲率变化约束。

6.根据权利要求5所述的基于最优控制的二维轨迹重构方法，其特征在于，所述车辆速度约束通过公式七表示为：

0≤v(t)≤v_max

所述公式七中，v_max为最大加速度；

所述纵向加速度约束即最小纵向加速度和最大纵向加速度受车辆特性的限制，通过公式八表示为：

a_min≤a(t)≤a_max

所述公式八中，a_min为最小纵向加速度；a_max为最大纵向加速度；

所述加加速度约束定义为单位时间内纵向加速度的变化量，通过公式九表示为：

所述公式九中，j_max为最大加加速度；

所述曲率约束为车辆转弯时，最小转弯半径受到限制，通过公式十表示为：

所述公式十中，r_min为最小转弯半径；

所述横向加速度约束通过公式十一表示为：

-g(e+f_s)≤v(t)²k(t)≤g(e+f_s)

所述公式十一中，g为重力加速度；e为道路超仰角；f_s为道路横向摩擦系数；

所述曲率变化约束通过公式十二表示为：

所述公式十二中，k_max为单位时间内曲率的最大变化量。

7.根据权利要求1所述的基于最优控制的二维轨迹重构方法，其特征在于，具体包括如下步骤：

S1：测量车辆轨迹，得到轨迹数据，根据轨迹数据构建初始时刻的车辆状态变量X(0)；

S2：将所述初始时刻的车辆状态变量X(0)和控制变量U(t)代入所述公式四，得到状态变量基于控制变量的递推函数；

S3：利用递推函数将模型改写为以曲率和加速度为决策变量的非线性规划模型，并将输入参数x⁰(t)、y⁰(t)、v_max、a_min、a_max、j_max、k_max、r_min、g、e和f_s代入所述非线性规划模型；

S4：求解所述非线性规划模型，得到最优控制变量，并抵用递推关系算出最优的重构轨迹。

8.根据权利要求7所述的基于最优控制的二维轨迹重构方法，其特征在于，所述S4中，通过非线性求解器或求解软件对所属非线性规划模型求解。

9.根据权利要求8所述的基于最优控制的二维轨迹重构方法，其特征在于，所述最优控制变量记为U^*(t)，所述递推关系通过公式十三计算，所述公式十三表示为：

X^*(t+Δt)＝f(X(0),U^*(t))

所述公式十三中，所述X^*为最优的重构轨迹状态变量；f为S2中得到的状态变量基于控制变量的递推函数。

Images