A New Vehicle Motion Model for Improved Predictions and Situation Assessment
一种新的车辆运动模型，用于改进预测和情境评估

Joakim Sörstedt, Lennart Svensson, Fredrik Sandblom, and Lars Hammarstrand
乔基姆·索尔斯特德、伦纳特·斯文森、弗雷德里克·桑德布隆和拉尔斯·哈马斯特兰德

Abstract 摘要

Reliable and accurate vehicle motion models are of vital importance for automotive active safety systems for a number of reasons. First of all, these models are necessary in tracking algorithms that provide the safety system with information. Second, the motion model is often used by the safety application to make long-term predictions about the future traffic situation. These predictions are then part of the basic data used by the system to determine if, when, and how to intervene. In this paper, we suggest a framework for designing accurate vehicle motion models. The resulting models differ from conventional models in that the expected control input from the driver is included. By also providing a methodology for a formal treatment of the uncertainties, a model structure well suited, e.g., in a tracking algorithm, is obtained. To utilize the framework in an application will require careful design and validation of submodels to calculate the expected driver control input. We illustrate the potential of the framework by examining the performance for a specific model example using real measurements. The properties are compared with those of a constant acceleration model. Evaluations indicate that the proposed model yields better predictions and that it has an ability to estimate the prediction uncertainties.
可靠和准确的车辆运动模型对于汽车主动安全系统至关重要，原因有很多。首先，这些模型在跟踪算法中是必要的，为安全系统提供信息。其次，运动模型通常被安全应用用来对未来交通情况进行长期预测。这些预测随后成为系统判断是否、何时以及如何干预的基本数据的一部分。在本文中，我们建议了一种设计准确车辆运动模型的框架。所得到的模型与传统模型的不同之处在于包含了来自驾驶员的预期控制输入。通过提供一种对不确定性进行正式处理的方法，获得了一种非常适合于跟踪算法的模型结构。要在应用中利用该框架，需要仔细设计和验证子模型，以计算预期的驾驶员控制输入。我们通过使用真实测量数据检查特定模型示例的性能来说明该框架的潜力。其属性与恒定加速度模型的属性进行了比较。评估表明，所提出的模型产生了更好的预测，并且具有估计预测不确定性的能力。

Index Terms-Active safety systems, automotive, driver models, motion model, optimal control, predictions, tracking.
索引词-主动安全系统，汽车，驾驶员模型，运动模型，最优控制，预测，跟踪。

I. Introduction I. 引言

ACURRENT trend in today’s automotive industry is to equip vehicles with increasingly more active safety systems. These have the objective of aiding the driver in different accident-prone situations, such as unintentional lane departures or dangers caused by distracted drivers. Based on sensor data, active safety systems try to assess the situation and detect dangerous scenarios. Some examples of active safety systems that are currently available on the market are given in [1]. To improve these systems in the future, detailed vehicle motion models will be most valuable. The reasons for this are at least threefold.
当前汽车行业的一个趋势是为车辆配备越来越多的主动安全系统。这些系统的目标是在不同的事故多发情况下帮助驾驶员，例如无意间偏离车道或因分心驾驶者造成的危险。基于传感器数据，主动安全系统试图评估情况并检测危险场景。目前市场上可用的一些主动安全系统的例子见于[1]。为了在未来改进这些系统，详细的车辆运动模型将是最有价值的。原因至少有三个。

The motion model is an integral part of the tracking system that provides the safety system with information
运动模型是跟踪系统的一个重要组成部分，为安全系统提供信息

regarding the surrounding environment. More precise motion models would improve the tracking system’s ability to handle complex traffic scenarios.
关于周围环境。更精确的运动模型将提高跟踪系统处理复杂交通场景的能力。
2) An active safety system needs to decide when and how to intervene. In theory, an accurate motion model enables the safety applications to make better long-term predictions and thereby allow decisions to be made earlier and with greater confidence [2].
2) 一种主动安全系统需要决定何时以及如何进行干预。理论上，准确的运动模型使安全应用能够做出更好的长期预测，从而允许更早地做出决策，并且更有信心地做出决策 [2]。
3) Careful modeling of specific situations of interest, e.g., related to different driver maneuvers, can help the safety application to classify and understand the traffic situation.
3) 对特定感兴趣情况的仔细建模，例如与不同驾驶员操作相关的情况，可以帮助安全应用程序分类和理解交通状况。
The motion models predominantly used in tracking systems are derived from the fundamental laws of dynamics and assume that the input is approximately white Gaussian noise. Such models have extensively been employed in numerous applications and often with satisfactory results [3]-[5]. However, there are reasons to believe that traditional models can be improved to better meet the demands of future safety systems. For instance, more accurate long term predictions would be most useful for a decision making algorithm.
在跟踪系统中主要使用的运动模型源自动力学的基本法则，并假设输入大致为白噪声高斯噪声。这些模型已广泛应用于许多应用中，并且通常取得令人满意的结果[3]-[5]。然而，有理由相信传统模型可以改进，以更好地满足未来安全系统的需求。例如，更准确的长期预测对于决策算法将是非常有用的。

A main weakness in traditional models is that they do not capture the influence the driver has on the vehicle motion. Under normal conditions, it is reasonable to believe that the vehicle is completely controlled by the driver. Attempts to model driver behavior have previously appeared in the literature. In [6], a driver behavior model is used to design an adaptive cruise controller, and in [7], similar ideas have been used for threat assessment. Other examples of driver behavior models have been developed for micro traffic simulators [8] and in the ongoing European Union projects integrated human modeling and simulation to support human error risk analysis of partially autonomous driver assistance systems (ISI-PADAS) [9] and Information Technology for error remediation and trapping emergencies (ITERATE) [10]. A closely related problem exists in the field of robotics, where algorithms are designed to find a collision-free path through a partly unknown environment, see, e.g., [11] and the references therein. We believe that neither of these models fulfill the requirements from a future active safety system. For instance, the models in [6] and [11] are not suitable from a tracking perspective as they do not provide a stochastic description of the vehicle motion. Similarly, the idea in [7] is not well suited for decision making as it does not capture the variations in driving styles and driving preferences among different drivers. Finally, since the models for micro process simulators are designed for simulation purposes, they lack a description of uncertainties in predictions as well as the ability to adapt model parameters to observed data. These properties are essential for filtering and decision making purposes and
传统模型的一个主要弱点是它们未能捕捉到驾驶者对车辆运动的影响。在正常情况下，认为车辆完全由驾驶者控制是合理的。文献中曾出现过对驾驶者行为进行建模的尝试。在[6]中，使用了一个驾驶者行为模型来设计自适应巡航控制器，而在[7]中，类似的思想被用于威胁评估。其他驾驶者行为模型的例子已被开发用于微观交通模拟器[8]，以及正在进行的欧盟项目中，集成人类建模和仿真以支持部分自动驾驶辅助系统的人为错误风险分析（ISI-PADAS）[9]和用于错误修正和紧急情况处理的信息技术（ITERATE）[10]。在机器人领域存在一个密切相关的问题，算法被设计用来在部分未知环境中找到无碰撞路径，参见，例如，[11]及其中的参考文献。我们认为这些模型都未能满足未来主动安全系统的要求。例如，文献[6]和[11]中的模型从跟踪的角度来看并不合适，因为它们没有提供车辆运动的随机描述。同样，文献[7]中的想法不太适合决策，因为它没有捕捉到不同驾驶者之间驾驶风格和驾驶偏好的变化。最后，由于微观过程模拟器的模型是为模拟目的而设计的，它们缺乏对预测不确定性的描述以及根据观察数据调整模型参数的能力。这些特性对于过滤和决策目的至关重要。
make these models inappropriate alternatives in their current form. The same arguments also hold for the models so far presented in the ISI-PADAS and ITERATE projects.
使这些模型在其当前形式下成为不合适的替代品。相同的论点也适用于迄今为止在 ISI-PADAS 和 ITERATE 项目中提出的模型。

In this paper, we continue to develop the model framework introduced in [12] and [13]. The idea is that the driver controls the vehicle by making a tradeoff between different objectives, such as the desire to travel fast and to travel comfortably. To describe the expected driver control signal, we use a cost function that reflects the driver objectives. By minimizing this cost function, the most likely control signal can be calculated. Compared with [12] and [13], the model framework is extended here to cover a more generic model structure. More importantly, it is also adapted to handle uncertainties in the cost function parameters. This allows us to design models that not only contain knowledge regarding typical driver behavior but also able to capture variations among different drivers and to adapt as driver behavior changes.
在本文中，我们继续发展在[12]和[13]中介绍的模型框架。其思想是，驾驶员通过在不同目标之间进行权衡来控制车辆，例如快速行驶的愿望和舒适行驶的愿望。为了描述预期的驾驶员控制信号，我们使用一个反映驾驶员目标的成本函数。通过最小化这个成本函数，可以计算出最可能的控制信号。与[12]和[13]相比，这里扩展了模型框架，以涵盖更通用的模型结构。更重要的是，它还适应于处理成本函数参数中的不确定性。这使我们能够设计不仅包含典型驾驶员行为知识的模型，还能够捕捉不同驾驶员之间的变化，并随着驾驶员行为的变化而调整。

The main contribution of this paper is the model structure described in Section III. This structure suggests a method for including the expected driver input in a model structure and provides guidelines on how to construct cost functions. In contrast to other commonly used methods, our approach has the advantage that model parameters have a clear interpretation. Having said that, a significant amount of work still remains before this approach can be used by an application. First of all, the cost function needs to be adjusted and validated for the intended application. Second, an efficient implementation for minimizing the cost function has to be developed. To illustrate the performance of the suggested model structure, we present one cost function and evaluate it using real measurements. In the studied examples, the proposed model explains the data better and provides better predictions compared with a constant acceleration (CA) model.
本文的主要贡献是第三节中描述的模型结构。该结构提出了一种将预期驱动输入纳入模型结构的方法，并提供了构建成本函数的指导方针。与其他常用方法相比，我们的方法具有模型参数具有明确解释的优势。尽管如此，在该方法可以被应用程序使用之前，仍然需要进行大量工作。首先，需要针对预期应用调整和验证成本函数。其次，必须开发出一种有效的实现方法来最小化成本函数。为了说明所建议的模型结构的性能，我们提出了一个成本函数，并使用实际测量进行评估。在研究的示例中，所提出的模型比恒定加速度（CA）模型更好地解释了数据，并提供了更好的预测。

This paper is organized as follows: Section II presents the notation and some of the most commonly used motion models in tracking applications. In Section III, a general description of the proposed modeling framework is provided. A specific design of the cost function is described in Section IV and later evaluated on measured data in Section V. Finally, Section VI contains the conclusions.
本文的组织结构如下：第二节介绍了符号和一些在跟踪应用中最常用的运动模型。第三节提供了所提建模框架的一般描述。第四节描述了成本函数的具体设计，并在第五节对测量数据进行了评估。最后，第六节包含结论。

II. BACKGROUND II. 背景

The vehicle motion model describes the evolution of a timediscrete vector

x_{k} \in R^{n_{x}}

that is referred to as the state vector of the model. For vectors and matrices (indicated by boldface), we use the subindex

k

as notation for a discrete-time instant with a continuous counterpart in

t_{k}

, whereas the notation

x (k)

is used for scalars. The continuous time interval between two samples is constant and denoted as

T_{s} = t_{k} - t_{k - 1}

. We consider a state vector partitioned as
车辆运动模型描述了一个称为模型状态向量的时间离散向量

x_{k} \in R^{n_{x}}

的演变。对于向量和矩阵（用粗体表示），我们使用下标

k

作为离散时间瞬间的符号，其在

t_{k}

中有一个连续对应，而符号

x (k)

用于标量。两个样本之间的连续时间间隔是恒定的，表示为

T_{s} = t_{k} - t_{k - 1}

。我们考虑一个被划分的状态向量为

x_{k} = {[{(x_{k}^{h})}^{T} {(z_{k}^{t})}^{T} r_{k}^{T}]}^{T}

where

x_{k}^{h}

is the state vector for the host vehicle,

z_{k}^{t}

is the state vector for the nearest target vehicle in front of the host vehicle, and the vector

r_{k}

contains parameters describing the road. Our main interest in this paper is to derive a model for the vector

x_{k}^{h}

.
其中

x_{k}^{h}

是主车的状态向量，

z_{k}^{t}

是主车前方最近目标车的状态向量，向量

r_{k}

包含描述道路的参数。我们在本文中的主要兴趣是推导向量

x_{k}^{h}

的模型。

For

z_{k}^{t}

and

r_{k}

, we use standard models, which are described in this section. The model we propose for the host vehicle can also be used for one or several target vehicles, but for simplicity, we apply it only on the host vehicle.
对于

z_{k}^{t}

和

r_{k}

，我们使用标准模型，这在本节中进行了描述。我们为主车提出的模型也可以用于一个或多个目标车辆，但为了简便起见，我们仅将其应用于主车。

We restrict the scope to Markovian models of the form
我们将范围限制为以下形式的马尔可夫模型

x_{k} = f_{k - 1} (x_{k - 1}, e_{k - 1})

where

f_{k - 1} (\cdot)

is a possibly time-varying and nonlinear function of

x_{k - 1}

and a stochastic noise vector

e_{k - 1} \in R^{n_{e}}

. The noise process accounts for model uncertainties and is traditionally modeled as an independent identically distributed process with zero mean.
其中

f_{k - 1} (\cdot)

是一个可能随时间变化的非线性函数，依赖于

x_{k - 1}

和一个随机噪声向量

e_{k - 1} \in R^{n_{e}}

。噪声过程考虑了模型的不确定性，通常被建模为一个独立同分布的过程，均值为零。

An important subgroup of the general model structure (2) is the family of linear motion models defined by the matrices

A_{k - 1} \in R^{n_{x} \times n_{x}}

and

B_{k - 1} \in R^{n_{x} \times n_{e}}

, such as
一般模型结构(2)的一个重要子组是由矩阵

A_{k - 1} \in R^{n_{x} \times n_{x}}

和

B_{k - 1} \in R^{n_{x} \times n_{e}}

定义的线性运动模型族，例如

x_{k} = A_{k - 1} x_{k - 1} + B_{k - 1} e_{k - 1} .

These motion models are popular in tracking applications because they enable simple and efficient algorithms such as the Kalman filter [14] to estimate and predict the states.
这些运动模型在跟踪应用中很受欢迎，因为它们使得像卡尔曼滤波器 [14] 这样的简单高效算法能够估计和预测状态。

A frequently used linear motion model is the CA model. In this model, the state vector

z_{k}^{t}

is parameterized as
一个常用的线性运动模型是 CA 模型。在该模型中，状态向量

z_{k}^{t}

被参数化为

z_{k}^{t} = {[η_{x}^{t} (k) η_{y}^{t} (k) {\dot{η}}_{x}^{t} (k) {\dot{η}}_{y}^{t} (k) {\ddot{η}}_{x}^{t} (k) {\ddot{η}}_{y}^{t} (k)]}^{T}

where

(η_{x}^{t} (k), η_{y}^{t} (k))

is the center position of the vehicle, and

({\dot{η}}_{x}^{t} (k), {\dot{η}}_{y}^{t} (k))

and

({\ddot{η}}_{x}^{t} (k), {\ddot{η}}_{y}^{t} (k))

are the first and second order time derivatives of

η_{x}^{t} (k)

and

η_{y}^{t} (k)

. In the version of the CA model employed here, the noise process

e_{k}

determines the jerk between sample instances. The acceleration, velocity, and position increments are thus given by

e_{k} T_{s}, e_{k} T_{s}^{2} / 2

and

e_{k} T_{s}^{3} / 6

, respectively. The system matrices then take the form
其中

(η_{x}^{t} (k), η_{y}^{t} (k))

是车辆的中心位置，

({\dot{η}}_{x}^{t} (k), {\dot{η}}_{y}^{t} (k))

和

({\ddot{η}}_{x}^{t} (k), {\ddot{η}}_{y}^{t} (k))

是

η_{x}^{t} (k)

和

η_{y}^{t} (k)

的一阶和二阶时间导数。在这里采用的 CA 模型版本中，噪声过程

e_{k}

决定了样本实例之间的冲击。因此，加速度、速度和位置增量分别由

e_{k} T_{s}, e_{k} T_{s}^{2} / 2

和

e_{k} T_{s}^{3} / 6

给出。系统矩阵则呈现出以下形式

A = [\begin{array}{cccccc} 1 & 0 & T_{s} & 0 & \frac{T_{s}^{2}}{2} & 0 \\ 0 & 1 & 0 & T_{s} & 0 & \frac{T_{s}^{2}}{2} \\ 0 & 0 & 1 & 0 & T_{s} & 0 \\ 0 & 0 & 0 & 1 & 0 & T_{s} \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{array}], B = [\begin{array}{cc} \frac{T_{s}^{3}}{6} & 0 \\ 0 & \frac{T_{s}^{3}}{6} \\ \frac{T_{s}^{2}}{2} & 0 \\ 0 & \frac{T_{s}^{2}}{2} \\ T_{s} & 0 \\ 0 & T_{s} \end{array}]

where the noise process is modeled as

e_{k} \sim N (0, C_{e})

, where

C_{e} = diag [\begin{array}{cc} σ_{{\ddot{η}}_{x}}^{2} & σ_{{\ddot{η}}_{y}}^{2} \end{array}]

.
噪声过程被建模为

e_{k} \sim N (0, C_{e})

，其中

C_{e} = diag [\begin{matrix} σ_{{\ddot{η}}_{x}}^{2} & σ_{{\ddot{η}}_{y}}^{2} \end{matrix}]

。

In essence, the CA model is a kinematic particle model [15], where the object motion is decoupled in different dimensions. Other examples, where the object’s motion in the different dimensions is instead coupled, are the coordinated turn models [3], [4]. More detailed vehicle motion models, which, for instance, take wheel slip and wheel angle into account, have been derived for applications such as vehicle stability and traction control. In [16], more information is provided on dynamic vehicle models and describes the single track model or bicycle model that has been used for vehicle tracking, at least in a simplified form; see, e.g., [17].
本质上，CA 模型是一个运动学粒子模型[15]，其中物体的运动在不同维度上是解耦的。其他例子，如物体在不同维度上的运动是耦合的，是协调转弯模型[3]，[4]。更详细的车辆运动模型，例如考虑轮胎滑移和轮胎角度的模型，已被推导用于车辆稳定性和牵引控制等应用。在[16]中，提供了有关动态车辆模型的更多信息，并描述了用于车辆跟踪的单轨模型或自行车模型，至少在简化形式上；见，例如，[17]。

In this paper, we use a curved road coordinate system such that the positions of the host vehicle

(η_{x}^{h}, η_{y}^{h})

and the target vehicle

(η_{x}^{t}, η_{y}^{t})

are given relative to the road. This coordinate system, which was previously used, e.g., in [18], is defined
在本文中，我们使用一个曲线路径坐标系统，使得主车

(η_{x}^{h}, η_{y}^{h})

和目标车

(η_{x}^{t}, η_{y}^{t})

的位置相对于道路给出。这个之前使用过的坐标系统，例如在[18]中，定义为

Fig. 1. Global Cartesian coordinate system

(ξ_{x}, ξ_{y})

and curved road coordinate system

(η_{x}, η_{y})

.
图 1. 全球笛卡尔坐标系统

(ξ_{x}, ξ_{y})

和曲线路径坐标系统

(η_{x}, η_{y})

.
in Fig. 1, which also contains a Cartesian coordinate system

(ξ_{x}, ξ_{y})

that will be needed later in Section IV. To find a relation between these coordinate systems, a model of the road curvature is required. Here, we follow suggestions from, e.g., [19] and use a clothoid model. The road curvature is then described by a local linear approximation about the host vehicle, i.e.,
在图 1 中，还包含一个笛卡尔坐标系

(ξ_{x}, ξ_{y})

，在第 IV 节中将需要它。为了找到这些坐标系之间的关系，需要一个道路曲率模型。在这里，我们遵循例如[19]的建议，使用一个克洛索伊德模型。然后，道路曲率通过关于主车的局部线性近似来描述，即，

c (η_{x}^{h} + Δ η_{x}) = c_{0} + Δ η_{x} c_{1}

The parameter

c_{0}

is the local curvature at the center of the road, and

c_{1}

is the curvature change rate as a function of distance

Δ η_{x}

from the host vehicle. We include the clothoid parameters

c_{0} (k)

and

c_{1} (k)

and the lane width

L_{w} (k)

in the road state vector
参数

c_{0}

是道路中心的局部曲率，

c_{1}

是作为距离

Δ η_{x}

从主车辆的曲率变化率。我们在道路状态向量中包含了螺旋线参数

c_{0} (k)

和

c_{1} (k)

以及车道宽度

L_{w} (k)

。

r_{k} = {[L_{w} (k) c_{0} (k) c_{1} (k)]}^{T}

The evolution of

r_{k}

is modeled as

r_{k}

的演变被建模为

r_{k} = [\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & T_{s} {\dot{η}}_{x} (k - 1) \\ 0 & 0 & 1 \end{array}] r_{k - 1} + v_{k - 1}^{r}

where

v_{k - 1}^{r} \sim N (0, Q_{r})

. 在

v_{k - 1}^{r} \sim N (0, Q_{r})

处。

III. Motion Model Framework
III. 运动模型框架

In this section, we describe how the motion model is extended to incorporate the effect of the driver. We base the modeling idea on a set of postulates that are highlighted in the text. The equations for calculating the driver input are also presented, and important model properties are discussed.
在本节中，我们描述了如何扩展运动模型以纳入驾驶员的影响。我们基于文本中强调的一组公设来进行建模。还介绍了计算驾驶员输入的方程，并讨论了重要的模型属性。

A. Model Structure A. 模型结构

We will here motivate and present the structure and some key components in the proposed model.
我们将在这里阐述并展示所提议模型的结构和一些关键组件。

Postulate 1. The driver controls the motion of the vehicle by steering and adjusting the acceleration.
公设 1. 驾驶员通过转向和调整加速度来控制车辆的运动。
As the classical motion models fail to acknowledge the influence of the driver, there is, at least theoretically, room for substantial improvements if we can model the driver actions. Naturally, the vehicle obeys the laws of physics, and the models in Section II are still appropriate although the input is no longer white noise. In a general parameterization, we suggest a model
由于经典运动模型未能考虑驾驶员的影响，因此如果我们能够对驾驶员的行为进行建模，理论上有很大的改进空间。自然，车辆遵循物理法则，尽管输入不再是白噪声，第二节中的模型仍然适用。在一般参数化中，我们建议一个模型。

x_{k}^{h} = f_{k - 1} (x_{k - 1}^{h}, u_{k - 1} (x_{k - 1}), v_{k - 1})

where

u_{k - 1} (x_{k - 1})

is the driver input signal, and

v_{k - 1}

is a white noise process. Modeling the driver input

u_{k - 1} (x_{k - 1})

is a key component here and the topic of Sections III-B and IV.
其中

u_{k - 1} (x_{k - 1})

是驱动输入信号，

v_{k - 1}

是白噪声过程。建模驱动输入

u_{k - 1} (x_{k - 1})

是这里的一个关键组成部分，也是第 III-B 和 IV 节的主题。

The complete state vector for the host vehicle is given by

x_{k}^{h}

, but the kinematic state of the host vehicle is described by the vector

z_{k}^{h}

, which is a subvector of

x_{k}^{h}

. The propagation of this kinematic state vector is described by the model
主车的完整状态向量为

x_{k}^{h}

，但主车的运动状态由向量

z_{k}^{h}

描述，该向量是

x_{k}^{h}

的一个子向量。该运动状态向量的传播由模型描述。

z_{k}^{h} = f_{k - 1}^{h} (z_{k - 1}^{h}, u_{k - 1} (x_{k - 1}) + v_{k - 1}^{z}) .

Here, we use a modified CA model
在这里，我们使用了一个修改过的 CA 模型

z_{k}^{h} = A z_{k - 1}^{h} + B (u_{k - 1} (x_{k - 1}) + v_{k - 1}^{z})

where 哪里

u_{k - 1} (x_{k - 1}) = [\begin{array}{l} {\overset{⃛}{η}}_{x}^{h} (k) \\ {\overset{⃛}{η}}_{y}^{h} (k) \end{array}]

and

v_{k - 1}^{z} \sim N (0, Q_{z})

. There are other model structures, like the bicycle model, which often describe vehicle motion more accurately than the chosen CA model. However, as we will see, the CA model leads to a relatively simple implementation.
和

v_{k - 1}^{z} \sim N (0, Q_{z})

。还有其他模型结构，比如自行车模型，它们通常比所选的 CA 模型更准确地描述车辆运动。然而，正如我们将看到的，CA 模型导致了相对简单的实现。

There is reason to believe that the variations in behavior between different drivers can be large. We therefore allow the function

u_{k - 1} (x_{k - 1})

to be parameterized by a vector

θ_{k - 1}

, which is intended to capture variations in driver preferences and driving styles. For an example of what parameters

θ_{k - 1}

may contain, see Section IV.
有理由相信，不同驾驶员之间的行为差异可能很大。因此，我们允许函数

u_{k - 1} (x_{k - 1})

通过向量

θ_{k - 1}

进行参数化，旨在捕捉驾驶员偏好和驾驶风格的变化。有关参数

θ_{k - 1}

可能包含的内容示例，请参见第四节。

The evolution of

θ_{k}

is described by the model

θ_{k}

的演变由模型描述

θ_{k} = g_{k - 1} (θ_{k - 1}, v_{k - 1}^{θ})

i.e., it is assumed to be independent of all the other elements in

x_{k - 1}^{h}

. We use
即假设它与

x_{k - 1}^{h}

中的所有其他元素独立。我们使用

g_{k - 1} (θ_{k - 1}, v_{k - 1}^{θ}) = γ θ_{k - 1} + (1 - γ) μ + v_{k - 1}^{θ}

where

v_{k - 1}^{θ} \sim N (0, Q_{θ})

, whereas

γ \in [0, 1), μ

, and

Q_{θ}

are design parameters. The proposed process model is such that when an element in

θ_{k - 1}

is not observable, its posterior distribution approaches a prior distribution representing the set of all drivers. To illustrate the convergence of the first two moments, we study
其中

v_{k - 1}^{θ} \sim N (0, Q_{θ})

、

γ \in [0, 1), μ

和

Q_{θ}

是设计参数。所提出的过程模型是这样的，当

θ_{k - 1}

中的一个元素不可观察时，其后验分布接近表示所有驱动因素集合的先验分布。为了说明前两个矩的收敛性，我们研究

\begin{aligned} E [θ_{k + l}] & = γ^{l} E [θ_{k}] + μ (1 - γ^{l}) \\ Cov [θ_{k + l}] & = γ^{2 l} Cov [θ_{k}] + \frac{1 - γ^{2 l}}{1 - γ^{2}} Q_{θ} . \end{aligned}

Thus,

γ

determines the convergence speed, whereas

μ

and

Q_{θ} / (1 - γ^{2})

are the limit values of the first two moments as the prediction length

l \to \infty

.
因此，

γ

决定了收敛速度，而

μ

和

Q_{θ} / (1 - γ^{2})

是预测长度

l \to \infty

时前两个矩的极限值。

The task of modeling driver behavior in terms of

u_{k - 1} (x_{k - 1})

and

v_{k - 1}

is an extensive and difficult problem. To simplify the problem, we assume that it is divided into subproblems for which the uncertainties are smaller and

v_{k - 1}

has a unimodal distribution.
将驾驶员行为建模为

u_{k - 1} (x_{k - 1})

和

v_{k - 1}

的任务是一个广泛而困难的问题。为了简化问题，我们假设它被划分为子问题，这些子问题的不确定性较小，并且

v_{k - 1}

具有单峰分布。

Postulate 2. In normal traffic, the actions of a driver can be divided into a set of different categories.
假设 2。在正常交通中，驾驶员的行为可以分为一组不同的类别。

Each category typically corresponds to a driver intention, such as changing lanes, following the lane, or turning left, but it could also capture other aspects of the situation, such as if the driver is distracted or not. Given the intention of the driver, it is easier to capture the driver behavior. We use a scalar parameter

m (k)

to indicate the driver action that is currently active.
每个类别通常对应于驾驶员的意图，例如变换车道、保持车道或左转，但它也可以捕捉到情况的其他方面，例如驾驶员是否分心。考虑到驾驶员的意图，更容易捕捉驾驶员的行为。我们使用一个标量参数

m (k)

来指示当前活跃的驾驶员动作。

The complete state vector for the host vehicle is now given by
主机车辆的完整状态向量现在由以下内容给出

x_{k}^{h} = {[{(z_{k}^{h})}^{T} θ_{k}^{T} m (k)]}^{T}

Note that the function

u_{k - 1} (x_{k - 1})

depends on both

θ_{k}

and

m (k)

, and the implications of this are further discussed in Section III-C.
注意，函数

u_{k - 1} (x_{k - 1})

依赖于

θ_{k}

和

m (k)

，这一点的影响在第 III-C 节中进一步讨论。

B. Calculating the Driver Input $u_{k} (x_{k})$
B. 计算驱动程序输入 $u_{k} (x_{k})$

In the suggested framework, the driver operates the vehicle such that three main objectives, or driving rules, are fulfilled.
在建议的框架中，驾驶员操作车辆，以满足三个主要目标或驾驶规则。

Postulate 3. The driver strives to control the car in a safe and comfortable manner and at a preferred velocity.
假设 3. 驾驶员努力以安全舒适的方式和首选速度控制汽车。
The safety preference corresponds to the driver’s desire to control the vehicle such that it stays in the preferred lane and at a safe distance to other vehicles and objects. The distance is here both spatial and temporal measures. Simultaneously, the driver asks for a comfortable journey and is reluctant to be put under large accelerating forces and jerks. The third objective captures the driver’s desire to maintain a preferred velocity.
安全偏好对应于驾驶员希望控制车辆，使其保持在首选车道并与其他车辆和物体保持安全距离。这里的距离既是空间度量也是时间度量。同时，驾驶员希望旅程舒适，并不愿意承受大的加速力和颠簸。第三个目标体现了驾驶员希望保持首选速度的愿望。

Based on this assumption, we can mathematically represent the driver preferences using a cost function
基于这个假设，我们可以用成本函数在数学上表示驾驶员偏好

cost (u_{k - 1}, x_{k - 1})

Suggestions for cost functions that penalize trajectories that are in conflict with the foregoing preferences are given in Section IV. As we will see, the cost function is implicitly dependent on the vector

θ_{k - 1}

.
对与上述偏好相冲突的轨迹进行惩罚的成本函数建议见第四节。正如我们将看到的，成本函数隐含地依赖于向量

θ_{k - 1}

。

An important driver ability is to make decisions based on both the present and upcoming situations.
一个重要的驾驶能力是根据当前和即将发生的情况做出决策。

Postulate 4. The driver plans ahead and tries to find the optimal route such that the driver preferences are considered for the near-and not only the immediate-future.
假设 4. 驾驶员提前规划，试图找到最佳路线，以便考虑驾驶员的偏好，既包括近期的，也包括不仅仅是眼前的未来。
We wish to incorporate this property in our model such that the expected driver input at time

k - 1

enables a desirable journey also for the near future. The cost function is therefore evaluated for a trajectory of driver input signals and corresponding state vectors. By selecting the trajectory with the lowest cost, we can produce a prediction of the vehicle motion that incorporates knowledge regarding the road, other objects, speed limits, etc.
我们希望将这个属性纳入我们的模型，使得在时间

k - 1

的预期驾驶员输入能够为不久的将来带来理想的旅程。因此，成本函数是针对一系列驾驶员输入信号和相应状态向量进行评估的。通过选择成本最低的轨迹，我们可以生成一个车辆运动的预测，该预测结合了关于道路、其他物体、速度限制等的知识。

In a sense, we are hereby approximating the driver by an optimal controller [20], where the optimality criterion is defined by the cost function. The optimal driver input is obtained as

u_{k - 1} (x_{k - 1}) ≜

从某种意义上说，我们在此通过一个最优控制器来近似驱动器[20]，其中最优性标准由成本函数定义。最优驱动输入被获得为

u_{k - 1} (x_{k - 1}) ≜

。

\underset{u_{k - 1}}{\arg min} {u_{k}, \dots, u_{k + N - 2}} \sum_{n = k - 1}^{k + N - 2} cost (u_{n}, x_{n})

where the future state vectors

x_{n}, n > k - 1

are calculated using the model (9). The driver input

u_{k - 1}

is usually a nonlinear function of the state

x_{k - 1}

, which turns the modified CA model in (11) into a nonlinear motion model. Here, we assume that the driver completely controls the car such that there are no uncertainties in the motion model; we set

v_{n} = 0

for

n = k - 1, \dots, k + N - 3

.
未来状态向量

x_{n}, n > k - 1

是使用模型 (9) 计算的。驱动输入

u_{k - 1}

通常是状态

x_{k - 1}

的非线性函数，这使得 (11) 中的修改 CA 模型变成一个非线性运动模型。在这里，我们假设驾驶员完全控制汽车，因此运动模型中没有不确定性；我们为

n = k - 1, \dots, k + N - 3

设置

v_{n} = 0

。

To solve (19), we take an approach that is very common in optimal control and that is advocated, e.g., in [21]. Instead of parameterizing the problem in the free variables

u_{k - 1}, \dots, u_{k + N - 2}

, we solve for the optimal sequence of state vectors

z_{k}, \dots, z_{k + N - 2}

. The motion model (11) (with

v_{n - 1}^{z} =

0

for

n = k, \dots, k + N - 2

) now enters as linear constraints on the state sequence. As a consequence, the optimization problem becomes more high dimensional but with much nicer properties. In this alternative form, the optimization can conveniently be solved using a built-in routine in TOMLAB (http://tomopt.com/tomlab/) called SNOPT. SNOPT solves the optimization problem using an efficient sequential quadratic programming method [22]. Note, however, that depending on the choice of cost function, the optimization problem (19) may be of different complexity. The properties of the optimization problem hence need to be considered when designing the cost functions. For the cost functions presented in Section IV, which are designed so that their first and second derivatives both exist and are continuous, and for the test scenario described in Section V, SNOPT computes the optimal trajectory in less than 50 ms on a conventional computer.
为了解决 (19)，我们采取一种在最优控制中非常常见的方法，例如在 [21] 中提到的。我们不是在自由变量

u_{k - 1}, \dots, u_{k + N - 2}

中对问题进行参数化，而是求解状态向量

z_{k}, \dots, z_{k + N - 2}

的最优序列。运动模型 (11)（对于

n = k, \dots, k + N - 2

的

v_{n - 1}^{z} =

0

）现在作为状态序列的线性约束进入。因此，优化问题变得更高维，但具有更好的性质。在这种替代形式中，优化可以方便地使用 TOMLAB 中的内置例程 SNOPT（http://tomopt.com/tomlab/）来解决。SNOPT 使用高效的序列二次规划方法 [22] 来解决优化问题。然而，请注意，根据成本函数的选择，优化问题 (19) 可能具有不同的复杂性。因此，在设计成本函数时需要考虑优化问题的性质。对于第四节中提出的成本函数，这些函数的第一和第二导数都存在且连续，并且对于第五节中描述的测试场景，SNOPT 在常规计算机上计算最优轨迹的时间少于 50 毫秒。

C. Model Features C. 模型特征

The idea of approximating the driver as an optimal controller according to an objective function has previously been discussed, e.g., in [6]. An important difference in the new motion model is the inclusion of the parameters

θ_{k}

and

m (k)

. In this section, we discuss the vital role these parameters have for situation assessment and to produce reliable predictions.
将驾驶员近似为根据目标函数的最优控制器的想法之前已经讨论过，例如在[6]中。新运动模型的一个重要区别是包含了参数

θ_{k}

和

m (k)

。在本节中，我们讨论了这些参数在情况评估和产生可靠预测中的重要作用。

Driver Preferences $θ_{k}$ : The purpose of the parameter $θ_{k}$ is to capture the individual driving style and allow the driver preferences to vary over time. Naturally, additional parameters add complexity to the problem, but the benefits obtained by introducing the vector $θ_{k}$ are arguably more significant.
驾驶员偏好 $θ_{k}$ : 参数 $θ_{k}$ 的目的是捕捉个体驾驶风格，并允许驾驶员偏好随时间变化。自然，额外的参数增加了问题的复杂性，但引入向量 $θ_{k}$ 所获得的好处可以说更为显著。

First of all, as time goes by, the system gains information regarding

θ_{k}

by tracking the vehicle trajectory

z_{1}^{h}, z_{2}^{h}, \dots, z_{k}^{h}

. Due to improved knowledge about

θ_{k}

, predictions become more accurate, and biases are reduced. The objective is, of course, to attain this performance for (virtually) all driving styles.
首先，随着时间的推移，系统通过跟踪车辆轨迹

z_{1}^{h}, z_{2}^{h}, \dots, z_{k}^{h}

获得关于

θ_{k}

的信息。由于对

θ_{k}

的知识得到改善，预测变得更加准确，偏差也减少。目标当然是实现（几乎）所有驾驶风格的这种性能。

Second, by describing the uncertainties in

θ_{k}

, we can obtain a parameterized and improved description of the uncertainties in

z_{k}^{h}

. While the cost function framework provides an estimate of the expected control signal, the random vector

θ_{k}

also affects the variance of

u_{k - 1} (x_{k - 1})

. This is a key advantage of the model framework, which owes to the fact that the uncertainties in

θ_{k}

can be propagated to

z_{k}^{h}

according to (10). In situations where the optimal trajectory is essentially independent of

θ_{k}

, i.e., when most drivers would act the same, the predictions are more reliable. On the other hand, in other circumstances, the
其次，通过描述

θ_{k}

中的不确定性，我们可以获得对

z_{k}^{h}

中不确定性的参数化和改进描述。虽然成本函数框架提供了预期控制信号的估计，但随机向量

θ_{k}

也会影响

u_{k - 1} (x_{k - 1})

的方差。这是模型框架的一个关键优势，这得益于

θ_{k}

中的不确定性可以根据(10)传播到

z_{k}^{h}

。在最优轨迹基本上独立于

θ_{k}

的情况下，即大多数驾驶者的行为相同时，预测更可靠。另一方面，在其他情况下，
optimal trajectory may be highly dependent on the driving style, which would lead to a lower confidence in the predictions as

θ_{k}

is not fully known. As we will see in Section V, the variations in prediction uncertainties coincide fairly well with the observed prediction errors.
最佳轨迹可能高度依赖于驾驶风格，这将导致对预测的信心降低，因为

θ_{k}

并不完全为人所知。正如我们将在第五节中看到的，预测不确定性的变化与观察到的预测误差相当吻合。
2) Driver Intention

m (k)

: It may seem like a drawback to include the hypotheses parameter

m (k)

into the state vector as it leads to several difficulties. First, to provide a complete description of the model in (9), one must design a process model for

m (k)

given by the probability function

P (m (k) ∣ x_{k - 1})

. A second complication is that a predetermined list of possible hypotheses is also needed; of course, depending on the position of the vehicles, not all of these hypotheses may be likely. Nonetheless, for many applications, it is also potentially the greatest advantage with the framework since it enables straightforward and formal derivations of the posterior distribution of

m (k)

, as demonstrated in [13]. In particular, once the intention specific motion models have been developed, information regarding

m (k)

is extracted simply by examining the past trajectories of the vehicles. Thereby, one may obtain a powerful tool to draw conclusions regarding

m (k)

and, thus, the traffic situation.
2) 驾驶员意图

m (k)

: 将假设参数

m (k)

纳入状态向量似乎是一个缺点，因为这会导致几个困难。首先，为了提供（9）中模型的完整描述，必须为

m (k)

设计一个由概率函数

P (m (k) ∣ x_{k - 1})

给出的过程模型。第二个复杂性是还需要一个预先确定的可能假设列表；当然，根据车辆的位置，并非所有这些假设都是可能的。然而，对于许多应用来说，这也可能是该框架最大的优势，因为它使得

m (k)

的后验分布的直接和正式推导成为可能，如 [13] 中所示。特别是，一旦开发了特定意图的运动模型，关于

m (k)

的信息可以通过简单地检查车辆的过去轨迹来提取。因此，可以获得一个强大的工具，以便对

m (k)

以及交通状况得出结论。

However, it is beyond the scope of this paper to fully develop and illustrate cost functions for multiple intentions. Thus, although we would like to point out the potential gains with including

m (k)

, it is not properly investigated here. The state vector used in the remainder of this paper is therefore reduced to

x_{k}^{h} = {[{(z_{k}^{h})}^{T} θ_{k}^{T}]}^{T}

.
然而，全面开发和说明多重意图的成本函数超出了本文的范围。因此，尽管我们想指出包括

m (k)

的潜在收益，但在这里并没有进行适当的研究。因此，本文其余部分使用的状态向量被简化为

x_{k}^{h} = {[{(z_{k}^{h})}^{T} θ_{k}^{T}]}^{T}

。

IV. Designing Cost Functions
IV. 设计成本函数

The cost functions should be designed to capture the typical behavior of the driver. We have partly based our choices on behavior studies used to design roads [23] and common practice for driving taught at driving schools. As was argued in Postulate 2, the actions of the driver can be separated into different categories. Each of these will typically require different cost functions. In this paper, we focus on the hypothesis that the driver intends to follow the right lane of the road. Designing cost functions to describe the normal driver behavior is different compared with most previous contributions, which have been focused on predicting dangerous situations [6], [24], [25].
成本函数应设计为捕捉驾驶员的典型行为。我们的选择部分基于用于设计道路的行为研究[23]和驾驶学校教授的驾驶常规。正如公设 2 中所论述的，驾驶员的行为可以分为不同的类别。每个类别通常需要不同的成本函数。在本文中，我们专注于假设驾驶员打算遵循道路的右侧车道。设计成本函数以描述正常驾驶员行为与大多数以前的贡献不同，后者主要集中在预测危险情况[6]、[24]、[25]。

We divide the cost function into four different components related to the longitudinal velocity

c_{l o} (u_{n}, x_{n})

, the lateral positioning of the vehicle

c_{l a} (u_{n}, x_{n})

, the comfort of the trajectory

c_{c} (u_{n}, x_{n})

, and the interaction with other vehicles

c_{i n}^{1} (u_{n}, x_{n})

and

c_{i n}^{2} (u_{n}, x_{n})

. Among these,

c_{l a} (u_{n}, x_{n}), c_{i n}^{1} (u_{n}, x_{n})

, and

c_{i n}^{2} (u_{n}, x_{n})

cover the safety aspect discussed in Postulate 3. Similarly,

c_{c} (u_{n}, x_{n})

ensures that the trajectory is comfortable and

c_{l o} (u_{n}, x_{n})

that the preferred speed of the driver is maintained. A weighted sum of the different components constitutes the total cost
我们将成本函数分为四个不同的组成部分，分别与纵向速度

c_{l o} (u_{n}, x_{n})

、车辆的横向定位

c_{l a} (u_{n}, x_{n})

、轨迹的舒适性

c_{c} (u_{n}, x_{n})

以及与其他车辆的互动

c_{i n}^{1} (u_{n}, x_{n})

和

c_{i n}^{2} (u_{n}, x_{n})

A. Longitudinal Cost Function $c_{l o} (\cdot, \cdot)$
A. 纵向成本函数 $c_{l o} (\cdot, \cdot)$

The longitudinal cost function reflects the driver’s desire to maintain a specific speed given by the parameter

{\dot{η}}_{x - ref}^{h}

. To punish trajectories for which the speed of the host vehicle deviates from the reference speed, we use
纵向成本函数反映了驾驶员希望保持的特定速度，该速度由参数

{\dot{η}}_{x - ref}^{h}

给出。为了惩罚主车速度偏离参考速度的轨迹，我们使用

c_{l o} (u_{n}, x_{n}) = {({\dot{η}}_{x}^{h} (n) - {\dot{η}}_{x - ref}^{h} (n))}^{2}

If this cost function is omitted, the optimal driver tends to slow down as that leads to a safe and comfortable journey. Like the other parameters in

θ_{k}, {\dot{η}}_{x - ref}^{h} (k)

is partially unknown initially. However,

{\dot{η}}_{x - ref}^{h} (k)

is special since it sometimes varies significantly over time, e.g., when the vehicle exits or enters a highway.
如果省略这个成本函数，最佳驾驶员往往会减速，因为这会导致安全和舒适的旅程。像

θ_{k}, {\dot{η}}_{x - ref}^{h} (k)

中的其他参数一样，最初部分未知。然而，

{\dot{η}}_{x - ref}^{h} (k)

是特殊的，因为它有时会随着时间显著变化，例如，当车辆驶出或驶入高速公路时。

B. Lateral Cost Function $c_{l a} (\cdot, \cdot)$
B. 侧向成本函数 $c_{l a} (\cdot, \cdot)$

The objective with the lateral cost function is to associate large costs to trajectories outside the driver’s preferred lane; a cost related to the desire to drive safely. Fig. 2 defines how

c_{l a} (u_{n}, x_{n})

depends on the lateral position

η_{y}

. The background in Fig. 2 consists of a road portrayed with thick grey lines divided into different cost segments. In the expressions,

C W

is the width of the car, and

L M

is a margin to the lane edge, whereas

k_{1}

and

k_{2}

are design parameters used to tune the cost function. We use

k_{1} = 1000

and

k_{2} = 3

.
侧向成本函数的目标是将较大的成本与驾驶者首选车道外的轨迹关联起来；这与安全驾驶的愿望有关。图 2 定义了

c_{l a} (u_{n}, x_{n})

如何依赖于侧向位置

η_{y}

。图 2 的背景由用粗灰线描绘的道路组成，分为不同的成本段。在表达式中，

C W

是汽车的宽度，

L M

是车道边缘的边距，而

k_{1}

和

k_{2}

是用于调整成本函数的设计参数。我们使用

k_{1} = 1000

和

k_{2} = 3

。

C. Comfort Cost Function $c_{c} (\cdot, \cdot)$

The comfort cost corresponds to the driver’s desire to steer the vehicle in a smooth and comfortable manner. Smooth trajectories are accomplished by increasing the cost for large accelerations and jerks. However, the acceleration perceived by the driver is not equal to the acceleration of the vehicle expressed in curved road coordinates. Instead, the global Cartesian coordinate system

(ξ_{x}, ξ_{y})

is used. It is easy to construct a nonlinear mapping

T : R^{2} \to R^{2}

from road coordinates to Cartesian coordinates, i.e.,

T

is such that

T (η_{x}, η_{y}) = {[ξ_{x} ξ_{y}]}^{T}

. Using the mapping

T

, it is also possible to compute

({\ddot{ξ}}_{x}^{h} (n), {\ddot{ξ}}_{y}^{h} (n))

and

({\overset{⃛}{ξ}}_{x}^{h} (n), {\overset{⃛}{ξ}}_{y}^{h} (n))

from

z_{n}^{h}, r_{n}

, and

({\overset{⃛}{η}}_{x}^{h} (n), {\overset{⃛}{η}}_{y}^{h} (n))

.
舒适成本对应于驾驶员希望以平稳舒适的方式操控车辆的愿望。通过增加大加速度和急剧变化的成本来实现平滑的轨迹。然而，驾驶员感知的加速度并不等于在曲线路径坐标中表示的车辆加速度。相反，使用全局笛卡尔坐标系

(ξ_{x}, ξ_{y})

。从道路坐标到笛卡尔坐标构建非线性映射

T : R^{2} \to R^{2}

是很容易的，即

T

使得

T (η_{x}, η_{y}) = {[ξ_{x} ξ_{y}]}^{T}

。使用映射

T

，也可以从

z_{n}^{h}, r_{n}

和

({\overset{⃛}{η}}_{x}^{h} (n), {\overset{⃛}{η}}_{y}^{h} (n))

计算出

({\ddot{ξ}}_{x}^{h} (n), {\ddot{ξ}}_{y}^{h} (n))

和

({\overset{⃛}{ξ}}_{x}^{h} (n), {\overset{⃛}{ξ}}_{y}^{h} (n))

。

To simplify the expressions, we introduce the notation

{\ddot{ξ}}^{h} (n) = \sqrt{{\ddot{ξ}}_{x}^{h} (n)^{2} + {\ddot{ξ}}_{y}^{h} (n)^{2}}

and

{\overset{⃛}{ξ}}^{h} (n) = \sqrt{{\ddot{ξ}}_{x}^{h} (n)^{2} + {\overset{⃛}{ξ}}_{y}^{h} (n)^{2}}

. The cost function is divided into two parts
为了简化表达式，我们引入符号

{\ddot{ξ}}^{h} (n) = \sqrt{{\ddot{ξ}}_{x}^{h} (n)^{2} + {\ddot{ξ}}_{y}^{h} (n)^{2}}

和

{\overset{⃛}{ξ}}^{h} (n) = \sqrt{{\ddot{ξ}}_{x}^{h} (n)^{2} + {\overset{⃛}{ξ}}_{y}^{h} (n)^{2}}

。成本函数被分为两部分。

c_{c} ({\overset{⃛}{η}}_{x} (n), {\overset{⃛}{η}}_{y}^{h} (n), z_{n}^{h}, r_{n}) = c_{a} (\ddot{ξ} (n)) + c_{j} (\overset{⃛}{ξ} (n))

where

c_{a} (\cdot)

is related to the acceleration, and

c_{j} (\cdot)

is related to the jerk.
其中

c_{a} (\cdot)

与加速度相关，

c_{j} (\cdot)

与冲击相关。

Empirical studies indicate that people are fairly insensitive to jerks and accelerations up to a certain level, above which the motions feel more unpleasant (possibly with the exception of accelerating forces). We use functions

c_{a} (\cdot)

and

c_{j} (\cdot)

of the form
实证研究表明，人们对一定程度以下的颠簸和加速度相对不敏感，超过这个水平后，运动会感觉更加不愉快（可能加速力是个例外）。我们使用形式为

c_{a} (\cdot)

和

c_{j} (\cdot)

的函数

c (x) = {\begin{cases} \frac{x^{2}}{g^{2}}, & when x < g \\ {(\frac{5}{6} + \frac{x^{2}}{6 g^{2}})}^{6}, & otherwise \end{cases}

As a consequence, we get cost functions that increase more rapidly for

x > g

and have continuous derivatives. Based on information from [23], we set

g = 2

for

c_{a} (\cdot)

and

g = 1.5

for

c_{j} (\cdot)

.
因此，我们得到的成本函数对于

x > g

增加得更快，并且具有连续的导数。根据 [23] 的信息，我们为

c_{a} (\cdot)

设置

g = 2

，为

c_{j} (\cdot)

设置

g = 1.5

。

D. Cost Functions for Vehicle Interaction $c_{in} (\cdot, \cdot)$
D. 车辆交互的成本函数 $c_{in} (\cdot, \cdot)$

An important aspect in the proposed motion model is that it not only incorporates the interaction between the vehicle and the road, e.g., in

c_{l a} (u_{n}, x_{n})

but the interaction with other vehicles as well. Here, we define a cost function that seeks to penalize trajectories where vehicles are too close. We limit our design in this paper to situations when there is one more vehicle close to the host vehicle. The vehicle interaction cost contains two parts
在所提议的运动模型中，一个重要的方面是它不仅考虑了车辆与道路之间的互动，例如在

c_{l a} (u_{n}, x_{n})

中，还考虑了与其他车辆的互动。在这里，我们定义了一个成本函数，旨在惩罚车辆过于接近的轨迹。我们在本文中将设计限制在当有一辆或多辆车辆靠近主车辆的情况。车辆互动成本包含两个部分。

c_{i n} (u_{n}, x_{n}) = c_{i n}^{1} (u_{n}, x_{n}) + c_{i n}^{2} (u_{n}, x_{n})

which concern the temporal and spatial distances to the other vehicle, respectively.
分别涉及与其他车辆的时间和空间距离。

To measure the temporal distance, we introduce the time headway
为了测量时间距离，我们引入了时间间隔

t_{h} (k) = \frac{η_{x}^{t} (k) - η_{x}^{h} (k)}{{\dot{η}}_{x}^{h} (k)}

Fig. 3. Test scenario. Host vehicle described by a state vector

z_{k}^{h}

traveling on a straight road behind a second vehicle described by

z_{k}^{t}

.
图 3. 测试场景。主车由状态向量

z_{k}^{h}

描述，行驶在一条直路上，后面跟着一辆由

z_{k}^{t}

描述的第二辆车。
which reflects the time it takes for the host vehicle to reach the current position of the target vehicle. A typical driver is reluctant to allow the time headway to become too small, whereas it matters less if

t_{h}

is, say, 4 or 10 s . The cost function
这反映了主车达到目标车当前位置所需的时间。典型的驾驶员不愿意让时间间隔变得太小，而如果

t_{h}

是 4 秒或 10 秒则关系不大。成本函数

c_{int}^{1} (u_{n}, x_{n}) = {\begin{cases} \frac{{({\bar{t}}_{h} - t_{h} (k))}^{2}}{t_{h} (k)}, & if 0 < t_{h} (k) \leq {\bar{t}}_{h} \\ 0, & otherwise \end{cases}

is selected to enable our model to capture this behavior, and

{\bar{t}}_{h}

is included in the state vector

θ_{k}

.
被选中以使我们的模型能够捕捉这种行为，

{\bar{t}}_{h}

被包含在状态向量

θ_{k}

中。

The spatial distance is here defined as the distance between the longitudinal positions of the two vehicles
空间距离在此定义为两辆车的纵向位置之间的距离

d (k) = η_{x}^{t} (k) - η_{x}^{h} (k)

In this case, we design the cost function for a scenario where the target vehicle is initially positioned ahead of the host vehicle on a single lane road. A reasonable cost function for the spatial distance should be large when

d (k)

is small (and even larger when

d (k)

is negative). We use the cost function
在这种情况下，我们为目标车辆最初位于主车辆前方的单车道道路场景设计成本函数。当

d (k)

较小时，空间距离的合理成本函数应该较大（当

d (k)

为负时甚至更大）。我们使用成本函数

c_{int}^{2} (u_{n}, x_{n}) = {\begin{cases} 0, & if \bar{d} \leq d \\ \frac{(\bar{d} - d)^{2}}{(\bar{d} - \underset{―}{d})^{2}}, & if \underset{―}{d} \leq d \leq \bar{d} \\ {(\frac{5}{6} + \frac{(\bar{d} - d)^{2}}{6 (\bar{d} - \underset{―}{d})^{2}})}^{6}, & if d \leq \underset{―}{d} \end{cases}

Note the resemblance to (23) if we set

x = \bar{d} - d

and

g = \bar{d} - \underset{―}{d}

. The parameter values used in our evaluations are

\bar{d} = 5

and

\underset{―}{d} = 2

.
注意与(23)的相似之处，如果我们设置

x = \bar{d} - d

和

g = \bar{d} - \underset{―}{d}

。我们评估中使用的参数值是

\bar{d} = 5

和

\underset{―}{d} = 2

。

V. Evaluation Results V. 评估结果

To evaluate our model, we study how well it explains (predicts) two different driving sequences. The sequences are real traffic situations collected using the same driver on straight busy two-lane roads at two different places in a city. The host vehicle is positioned in the middle of the road and is traveling behind a target vehicle (see Fig. 3). The two vehicles are closely spaced such that the host vehicle has to slow down when the target vehicle slows down (see Figs. 4 and 5).
为了评估我们的模型，我们研究它如何解释（预测）两种不同的驾驶序列。这些序列是在城市的两个不同地点，由同一位驾驶员在繁忙的双车道直路上收集的真实交通情况。主车位于道路中间，正跟随一辆目标车辆行驶（见图 3）。这两辆车的间距很近，以至于当目标车辆减速时，主车也必须减速（见图 4 和图 5）。

The host vehicle is equipped with a

77 - GHz

radar sensor that measures the range and range rate to the target vehicle. Based on these and internal measurements on the host vehicle speed and acceleration, we estimate the trajectories

Z_{L}^{h} =

[z_{1}^{h}, \dots, z_{L}^{h}]

and

Z_{L}^{t} = [z_{1}^{t}, \dots, z_{L}^{t}]

using a Kalman smoother. As benchmark, we use a CA model also for the host vehicle. For notation, we introduce
主机车辆配备了一个

77 - GHz

雷达传感器，用于测量与目标车辆的距离和距离变化率。基于这些以及主机车辆速度和加速度的内部测量，我们使用卡尔曼平滑器估计轨迹

Z_{L}^{h} =

[z_{1}^{h}, \dots, z_{L}^{h}]

和

Z_{L}^{t} = [z_{1}^{t}, \dots, z_{L}^{t}]

。作为基准，我们也为主机车辆使用 CA 模型。为了便于表述，我们引入

$M_{1}$ : constant acceleration model
$M_{1}$ : 恒定加速度模型

M_{2}

: proposed motion model

M_{2}

: 提议的运动模型
to indicate the choice of model.
以指示模型的选择。

Fig. 4. Positions, velocities, and accelerations of the two vehicles, in the first sequence, as functions of time.
图 4. 两辆车辆在第一序列中的位置、速度和加速度随时间的变化。

Fig. 5. Positions, velocities, and accelerations of the two vehicles in the second sequence.
图 5. 第二个序列中两辆车的位置、速度和加速度。

A. Evaluation Criteria A. 评估标准

We will mainly study two different properties of the model, where the first concerns the prediction capability and the variations in prediction uncertainties. More specifically, we will compare the mean and the variance of the density

^{1} p (z_{k}^{h} ∣ Z_{k - 1}^{h}

Z_{k - 1}^{t}, M_{i})

for

i = 1

and 2 to the observed trajectories.
我们将主要研究模型的两个不同特性，第一个涉及预测能力和预测不确定性的变化。更具体地说，我们将比较密度

^{1} p (z_{k}^{h} ∣ Z_{k - 1}^{h}

、

Z_{k - 1}^{t}, M_{i})

对于

i = 1

和 2 的均值和方差与观察到的轨迹。

The second property is the model likelihood
第二个属性是模型似然性

p (Z_{L}^{h}, Z_{L}^{t} ∣ M_{i}) = p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{i}) p (Z_{L}^{t} ∣ M_{i})

which is a standard criterion in model testing (see [2]). As we are mainly interested in the ratio between the two likelihoods, our subsequent evaluations study

p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{i})

since

p (Z_{L}^{t} ∣ M_{i})

does not depend on

M_{i}

.
这是模型测试中的一个标准标准（见[2]）。由于我们主要关注两个似然之间的比率，我们后续的评估研究

p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{i})

，因为

p (Z_{L}^{t} ∣ M_{i})

不依赖于

M_{i}

。

B. Filtering $θ_{k}$ B. 过滤 $θ_{k}$

In this section, we describe how

θ_{k}

is treated in the evaluation.
在本节中，我们描述了

θ_{k}

在评估中的处理方式。

Motivation: We notice that both criteria involve the computation of densities of the host state vector either $p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{i})$ or $p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{i})$ . For the CA model, these densities are completely determined by the process noise covariance matrix $C_{e} = diag [σ_{{\ddot{η}}_{x}}^{2} σ_{{\ddot{η}}_{y}}^{2}]$ . In the evaluations, we select $σ_{{\ddot{η}}_{x}}^{2}$ and $σ_{{\ddot{η}}_{y}}^{2}$ to maximize $p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{1})$ .
动机：我们注意到这两个标准都涉及到主状态向量 $p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{i})$ 或 $p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{i})$ 的密度计算。对于 CA 模型，这些密度完全由过程噪声协方差矩阵 $C_{e} = diag [σ_{{\ddot{η}}_{x}}^{2} σ_{{\ddot{η}}_{y}}^{2}]$ 决定。在评估中，我们选择 $σ_{{\ddot{η}}_{x}}^{2}$ 和 $σ_{{\ddot{η}}_{y}}^{2}$ 以最大化 $p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{1})$ 。

For the proposed motion model, these densities are, in principal, evaluated in the same way. We will describe the evaluation steps for the density

p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{2})

first. Later, at the end of Section V-B3, we explain how the algorithm can be adapted to instead compute

p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2})

. To simplify the calculations, we decompose the likelihood as
对于所提出的运动模型，这些密度原则上是以相同的方式进行评估的。我们将首先描述密度

p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{2})

的评估步骤。稍后，在第 V-B3 节的末尾，我们将解释如何调整算法以计算

p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2})

。为了简化计算，我们将似然分解为

p (Z_{L}^{h} ∣ Z_{L}^{t}, M_{2}) = \prod_{k = 1}^{L} p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

and our primary goal is, therefore, to find

p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

. Note that this expression is not conditioned on

θ_{k}

. By marginalizing the dependence on

θ_{k}

, the variance of

θ_{k}

will influence the uncertainties in the predictions of

z_{k}^{h}

.
因此，我们的主要目标是找到

p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

。请注意，这个表达式并不依赖于

θ_{k}

。通过边际化对

θ_{k}

的依赖，

θ_{k}

的方差将影响对

z_{k}^{h}

预测的不确定性。

To evaluate 评估

\begin{aligned} p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2}) & = \int p (z_{k}^{h} ∣ θ_{k - 1}, Z_{k - 1}^{h}, Z_{L}^{t}, M_{2}) \\ \times p (θ_{k - 1} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2}) d θ_{k - 1} \end{aligned}

we approximate

p (θ_{k - 1} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

as a Gaussian density and then apply the unscented transform [26] to obtain a Gaussian approximation of (31). In Section V-B3, we describe how to obtain

p (θ_{k - 1} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

using the unscented Kalman filter (UKF), but first we describe

θ_{k}

and its model in more detail.
我们将

p (θ_{k - 1} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

近似为高斯密度，然后应用无迹变换[26]以获得(31)的高斯近似。在第 V-B3 节中，我们描述如何使用无迹卡尔曼滤波器(UKF)获得

p (θ_{k - 1} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

，但首先我们更详细地描述

θ_{k}

及其模型。
2) Parameterization of

θ_{k}

: The time evolution of

θ_{k}

was already described in (13) and (14). We have previously mentioned that

θ_{k}

defines

α_{l o}, α_{i n}, {\dot{η}}_{x - ref}^{h} (k)

, and

{\bar{t}}_{h} (k)

, and we now wish to specify how. For some of the parameters, it appears equally likely that they should increase by, say

30 %

, or decrease by the same amount. For the process model in (13) and (14) to agree with this, we use the parameterization
2)

θ_{k}

的参数化：

θ_{k}

的时间演化已在 (13) 和 (14) 中描述。我们之前提到过

θ_{k}

定义了

α_{l o}, α_{i n}, {\dot{η}}_{x - ref}^{h} (k)

和

{\bar{t}}_{h} (k)

，现在我们希望具体说明如何。对于某些参数，它们增加，比如

30 %

，或减少相同的数量似乎同样可能。为了使 (13) 和 (14) 中的过程模型与此一致，我们使用参数化。

θ_{k} = [\begin{matrix} \log \frac{α_{l o} (k)}{α_{l} (k)} \\ \log \frac{α_{i n} (k)}{α_{i n}} \\ \log \frac{{\bar{t}}_{h} (k)}{t_{h}} \\ {\dot{η}}_{x - ref}^{h} (k) \end{matrix}]

where

{\tilde{α}}_{l o} = 1, {\tilde{α}}_{i n} = 70

, and

{\tilde{t}}_{h} = 2

. The other model parameters are

μ = {[\begin{array}{llll} 0 & 0 & 0 & 6 \end{array}]}^{T}, γ = {0.95}^{T_{s}}

, and

Q_{θ} =

diag [11110.08] / 80

.
其中

{\tilde{α}}_{l o} = 1, {\tilde{α}}_{i n} = 70

和

{\tilde{t}}_{h} = 2

。其他模型参数为

μ = {[\begin{matrix} 0 & 0 & 0 & 6 \end{matrix}]}^{T}, γ = {0.95}^{T_{s}}

和

Q_{θ} =

diag [11110.08] / 80

。
3) Filter Recursion: Here, we present a recursive algorithm to compute

p (θ_{k} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})

from

p (θ_{k - 1} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

. We use the UKF, which is a Kalman-like technique to
3) 过滤递归：在这里，我们提出了一种递归算法来从

p (θ_{k - 1} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})

计算

p (θ_{k} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})

。我们使用 UKF，这是一种类似于卡尔曼的技术来
approximate the updated density

^{2} p (θ_{k - 1} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})

and the predicted density

p (θ_{k} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})

.
近似更新后的密度

^{2} p (θ_{k - 1} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})

和预测密度

p (θ_{k} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})

。

In the update step, we adjust the prior density with the information obtained from the likelihood function

^{3}

在更新步骤中，我们根据从似然函数

^{3}

获得的信息调整先验密度

\begin{aligned} p (z_{k}^{h} ∣ θ_{k - 1}, z_{k - 1}^{h}, Z_{L}^{t}) \\ \propto N (B^{†} [z_{k}^{h} - A z_{k - 1}^{h}]; u_{k - 1} (θ_{k - 1}, z_{k - 1}^{h}, Z_{L}^{t}), Q_{z}) . \end{aligned}

In (33), we have used the singularity of

p (z_{k}^{h} ∣ θ_{k - 1}, z_{k - 1}^{h}, Z_{L}^{t})

(there are only uncertainties in some dimensions) to reduce the dimension by multiplying with the Moore-Penrose pseudoinverse

B^{†}

. To employ the UKF, it is more convenient to express (33) as
在 (33) 中，我们利用了

p (z_{k}^{h} ∣ θ_{k - 1}, z_{k - 1}^{h}, Z_{L}^{t})

的奇异性（某些维度中只有不确定性）通过与 Moore-Penrose 伪逆

B^{†}

相乘来降低维度。为了使用 UKF，将 (33) 表达为更方便。

B^{†} [z_{k}^{h} - A z_{k - 1}^{h}] = u_{k - 1} (θ_{k - 1}, z_{k - 1}^{h}, Z_{L}^{t}) + v_{k}^{z}

where

B^{†} [z_{k}^{h} - A z_{k - 1}^{h}]

is regarded as the measurement equation. The update step of the standard UKF algorithm can now be applied based on (34), see [26] and [27]. We employ the UKF parameterization defined in [27] with

[α β κ] = [1.0520]

.
其中

B^{†} [z_{k}^{h} - A z_{k - 1}^{h}]

被视为测量方程。现在可以基于 (34) 应用标准 UKF 算法的更新步骤，参见 [26] 和 [27]。我们采用 [27] 中定义的 UKF 参数化，使用

[α β κ] = [1.0520]

。

In the prediction step, we assume that we have a Gaussian prior density
在预测步骤中，我们假设我们有一个高斯先验密度

p (θ_{k - 1} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2}) \approx N (θ_{k - 1}; {\hat{θ}}_{k - 1 ∣ k}, P_{k - 1 ∣ k})

Since the process model for

θ_{k}

, which is described in (14), is linear and Gaussian, the predicted density is also Gaussian, i.e.,
由于在(14)中描述的

θ_{k}

的过程模型是线性和高斯的，因此预测密度也是高斯的，即，

p (θ_{k} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2}) \approx N (θ_{k}; {\hat{θ}}_{k ∣ k}, P_{k ∣ k})

where

{\hat{θ}}_{k ∣ k} = γ {\hat{θ}}_{k - 1 ∣ k} + (1 - γ) μ

, and

P_{k ∣ k} = γ^{2} P_{k - 1 ∣ k} + Q_{z}

.
在

{\hat{θ}}_{k ∣ k} = γ {\hat{θ}}_{k - 1 ∣ k} + (1 - γ) μ

和

P_{k ∣ k} = γ^{2} P_{k - 1 ∣ k} + Q_{z}

处。
From an implementation perspective, the UKF is very convenient to use but has the disadvantage that it requires us to evaluate

u_{k - 1} (θ_{k - 1}, z_{k - 1}^{h}, Z_{L}^{t})

several times (once for each sigma point; since

θ_{k - 1}

contains four elements, we have

2 \times

4 + 1 = 9

sigma points).
从实现的角度来看，UKF 使用起来非常方便，但缺点是需要我们评估

u_{k - 1} (θ_{k - 1}, z_{k - 1}^{h}, Z_{L}^{t})

多次（每个 sigma 点一次；由于

θ_{k - 1}

包含四个元素，我们有

2 \times

4 + 1 = 9

个 sigma 点）。

We have here written the densities conditional on

Z_{L}^{t}

. The calculations when we instead condition on

Z_{k}^{t}

are analogous but with a slight difference in the update step. As we have seen, the function

u_{k - 1}

takes

Z_{L}^{t}

as input to know how to compensate for the vehicle interaction

c_{i n} (\cdot, \cdot)

. When

z_{k}^{t}, z_{k + 1}^{t}, \dots, z_{L}^{t}

are unknown, we predict these based on

z_{k - 1}^{t}

. The approach taken here is to use the predicted mean of the CA model

{\hat{z}}_{k - 1 + l}^{t} =

A^{l} z_{k - 1}^{t}

. (Note that

t

stands for target, whereas

A

is raised to the power of

l

.)
我们在这里写出了条件在

Z_{L}^{t}

下的密度。当我们改为在

Z_{k}^{t}

下进行条件计算时，过程是类似的，但在更新步骤上有一些细微的差别。正如我们所看到的，函数

u_{k - 1}

以

Z_{L}^{t}

作为输入，以了解如何补偿车辆交互

c_{i n} (\cdot, \cdot)

。当

z_{k}^{t}, z_{k + 1}^{t}, \dots, z_{L}^{t}

不可知时，我们根据

z_{k - 1}^{t}

预测这些值。这里采取的方法是使用 CA 模型

{\hat{z}}_{k - 1 + l}^{t} =

A^{l} z_{k - 1}^{t}

的预测均值。（注意，

t

代表目标，而

A

是

l

的幂。）

C. Results C. 结果

As previously mentioned, we observe two cars driving on a straight road in a city. We evaluate the criteria in Section V-A on two different sequences (see Figs. 4 and 5). Both sequences
如前所述，我们观察到两辆汽车在城市的一条直路上行驶。我们在两个不同的序列上评估第 V-A 节中的标准（见图 4 和图 5）。两个序列

Fig. 6. Predicted and true jerk for the two trajectories.
图 6. 两条轨迹的预测和真实冲击。
are interesting since there is an interaction between the two vehicles; the host vehicle is forced to slow down when the target vehicle slows down. Measurement data are obtained with the frequency of 20 Hz . However, to speed up the optimization algorithm, we assume

T_{s} = 0.3

and a prediction horizon

N =

10 [see (19)] during our calculation of

u_{k - 1} (\cdot)

.
很有趣，因为两辆车之间存在相互作用；当目标车辆减速时，主车辆被迫减速。测量数据的频率为 20 Hz。然而，为了加快优化算法，我们在计算

u_{k - 1} (\cdot)

时假设

T_{s} = 0.3

和预测视野

N =

10 [见(19)]。

Prediction Accuracy: We now wish to evaluate how well the predicted density $p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2})$ corresponds to the observed trajectories. The motion model in (11) is driven only by the jerk values between the sample instances. We will therefore compare the predicted jerk values (obtained from $u_{k - 1} (\cdot))$ with the values computed from the measurements. For the CA model, we use the values $σ_{{\ddot{η}}_{x}}^{2} = 0.95$ and $σ_{{\ddot{η}}_{x}}^{2} = 1.08$ for the first and second sequences, respectively, since these values maximize the likelihood (see Section V-B1).
预测准确性：我们现在希望评估预测的密度 $p (z_{k}^{h} ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2})$ 与观察到的轨迹之间的对应关系。公式 (11) 中的运动模型仅由样本实例之间的冲击值驱动。因此，我们将比较从 $u_{k - 1} (\cdot))$ 获得的预测冲击值与从测量中计算得出的值。对于 CA 模型，我们使用值 $σ_{{\ddot{η}}_{x}}^{2} = 0.95$ 和 $σ_{{\ddot{η}}_{x}}^{2} = 1.08$ 分别作为第一和第二序列，因为这些值最大化了似然性（见第 V-B1 节）。

Fig. 6 shows the jerk predictions obtained from our model

E {{\ddot{η}}_{x}^{h} (k) ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2}}

, which is denoted here as modified CA, together with the observed jerk values. Note that the jerk predictions provided by the CA model are zero at all times. As we can see, the jerk predictions are fairly accurate most of the time. In the first sequence, the predictions are less accurate at around

t = 6

when the host vehicle temporarily accelerates although the target vehicle slows down. Similarly, for the second sequence, the model incorrectly predicts an increased deceleration at

t \approx 14

. However, overall, the predictions from the modified CA model are substantially better than those from the standard CA model.
图 6 显示了我们模型

E {{\ddot{η}}_{x}^{h} (k) ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2}}

获得的冲击预测，这里称为修改后的 CA，以及观察到的冲击值。请注意，CA 模型提供的冲击预测在所有时间点都是零。正如我们所看到的，冲击预测在大多数情况下相当准确。在第一个序列中，当主车暂时加速而目标车减速时，预测在

t = 6

附近的准确性较低。类似地，在第二个序列中，模型错误地预测了在

t \approx 14

时减速增加。然而，总体而言，修改后的 CA 模型的预测明显优于标准 CA 模型的预测。

For the predicted density to be accurate, it is also important that it has a reasonable variance. Let us denote the error between the predicted and observed jerks by

Δ {\overset{⃛}{η}}_{x}^{h} (k)

. Ideally, we would like the variance of the predicted density

Var {{\overset{⃛}{η}}_{x}^{h} (k) ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2}}

to be identical to

{(Δ {\overset{⃛}{η}}_{x}^{h} (k))}^{2}

on the average. As previously discussed, we hope that the model also has an ability to know when the predictions are reliable and when they are more uncertain (see Section III-C1).
为了使预测密度准确，合理的方差也是重要的。我们用

Δ {\overset{⃛}{η}}_{x}^{h} (k)

表示预测和观察到的冲击之间的误差。理想情况下，我们希望预测密度

Var {{\overset{⃛}{η}}_{x}^{h} (k) ∣ Z_{k - 1}^{h}, Z_{k - 1}^{t}, M_{2}}

的方差在平均上与

{(Δ {\overset{⃛}{η}}_{x}^{h} (k))}^{2}

相同。如前所述，我们希望模型也能够知道何时预测是可靠的，何时则更不确定（见第 III-C1 节）。

The variance of the predictions (the uncertainties in the predictions) and the prediction errors are given in Figs. 7 and 8 for the respective data sequences. On the average, the variance
预测的方差（预测中的不确定性）和预测误差在图 7 和图 8 中分别给出。平均而言，方差

Fig. 7. Prediction uncertainties and prediction errors for the first sequence.
图 7. 第一序列的预测不确定性和预测误差。

Fig. 8. Prediction uncertainties and prediction errors for the second sequence.
图 8. 第二个序列的预测不确定性和预测误差。
is 0.65 and 0.83 in the first and second sequences, respectively, whereas the corresponding errors

{(Δ {\overset{⃛}{η}}_{x}^{h} (k))}^{2}

are 0.45 and 0.36 . The model is thus overestimating the uncertainties, but at least the order of magnitude is correct. At several occasions, like at around

t = 6

and

t = 8

in the first sequence and at

t \approx 13

in the second sequence, larger prediction variances coincide with larger errors. Since an increased prediction variance can only be caused by uncertainties in

θ_{k}

, this is a good sign in favor of the filter described in Section V-B. Although the evaluation is limited, this is still a promising indication.
在第一和第二序列中分别为 0.65 和 0.83，而相应的误差

{(Δ {\overset{⃛}{η}}_{x}^{h} (k))}^{2}

为 0.45 和 0.36。因此，该模型高估了不确定性，但至少数量级是正确的。在多个场合，例如在第一序列的

t = 6

和

t = 8

附近以及第二序列的

t \approx 13

处，较大的预测方差与较大的误差相吻合。由于增加的预测方差只能由

θ_{k}

中的不确定性引起，这对第 V-B 节中描述的滤波器是一个良好的迹象。尽管评估有限，这仍然是一个有希望的指示。
2) Model Likelihood: We previously stated that we wish to compute

p (Z_{L}^{h}, Z_{L}^{t} ∣ M_{i})

for

M_{1}

and

M_{2}

as a measure of how well the models explain the data. However, by studying
2) 模型似然性：我们之前提到我们希望计算

p (Z_{L}^{h}, Z_{L}^{t} ∣ M_{i})

对于

M_{1}

和

M_{2}

的值，以衡量模型对数据的解释程度。然而，通过研究

\log p (Z_{k}^{h} ∣ Z_{L}^{t}, M_{i}) = \sum_{j = 1}^{k} \log p (z_{j}^{h} ∣ Z_{j - 1}^{h}, Z_{L}^{t}, M_{i})

Fig. 9. Log-likelihood functions for the first data sequence.
图 9. 第一个数据序列的对数似然函数。

Fig. 10. Log-likelihood functions for the second data sequence.
图 10. 第二个数据序列的对数似然函数。
for

k = 1, \dots, L

, we also get information about how well the models explain different parts of the sequences (the logarithm is merely included to enable us to illustrate functions of different magnitudes in the same figure). In addition, we also include the function
对于

k = 1, \dots, L

，我们还获得了关于模型如何解释序列不同部分的信息（对数仅用于使我们能够在同一图中展示不同大小的函数）。此外，我们还包括了该函数。

\sum_{j = 1}^{k} \log p (z_{j}^{h} ∣ Z_{j - 1}^{h}, Z_{j}^{t}, M_{2})

which is made up of the predictive densities discussed in Section V-C1. In the figures, we refer to

\log p (Z_{k}^{h} ∣ Z_{L}^{t}, M_{1})

CA, \log p (Z_{k}^{h} ∣ Z_{L}^{t}, M_{2})

as conditional CA and

\sum_{j = 1}^{k} \log p (z_{j}^{h} ∣ Z_{j - 1}^{h}, Z_{j}^{t}, M_{2})

as modified CA.
由第 V-C1 节讨论的预测密度构成。在图中，我们将

\log p (Z_{k}^{h} ∣ Z_{L}^{t}, M_{1})

称为

CA, \log p (Z_{k}^{h} ∣ Z_{L}^{t}, M_{2})

，作为条件 CA，将

\sum_{j = 1}^{k} \log p (z_{j}^{h} ∣ Z_{j - 1}^{h}, Z_{j}^{t}, M_{2})

称为修改后的 CA。

The three log-likelihood functions are shown in Figs. 9 and 10 for the first and second sequences, respectively. Generally speaking, modified CA and conditional CA are similar and significantly larger (better) than CA. Compared with the proposed model, here represented by modified CA and conditional CA, CA explains the data very poorly when the acceleration Authorized licensed use limited to: New York University. Downloaded on January 22,2025 at 01:32:19 UTC from IEEE Xplore. Restrictions apply.
三个对数似然函数分别在图 9 和图 10 中显示，针对第一和第二个序列。一般来说，修改后的 CA 和条件 CA 相似，并且显著大于（更好）CA。与所提出的模型相比，这里用修改后的 CA 和条件 CA 表示，当加速度时，CA 对数据的解释非常差。
changes notably; its likelihood function quickly decreases at those times, whereas both modified CA and conditional CA are much smoother. The only clear exception is the conditional CA, which substantially decreases around

t = 6

in the first sequence. The reason for this is that the model expects the driver to break harder when there is instead a slight acceleration; the same phenomenon was discussed previously related to Fig. 6. To put the numbers in Figs. 9 and 10 into perspective, Jeffreys [28] states that a log-likelihood difference greater than 2 should be viewed as decisive evidence as to what model is true.
变化显著；其似然函数在这些时刻迅速下降，而修改后的 CA 和条件 CA 则平滑得多。唯一明显的例外是条件 CA，它在第一序列中的

t = 6

附近大幅下降。原因在于模型期望驾驶员在轻微加速时会更用力地刹车；之前与图 6 相关的现象也有讨论。为了将图 9 和图 10 中的数字放在一个更广阔的视角中，Jeffreys [28]指出，似然差异大于 2 应被视为对哪个模型是真实的决定性证据。

VI. Conclusion and Future Work
VI. 结论与未来工作

We have presented a general framework for modeling vehicle motion. By including the influence of the driver into the model, an improved prediction ability is obtained. Moreover, with a formalized treatment of uncertainties in the underlying model parameters, the description of the prediction uncertainties is improved. Specific attention was here given to adapting the model to when the driver follows the right lane of the road. Accurate data from real driving situations were collected to study the model properties. For the evaluated test scenarios, we have shown that a model developed using our framework describes reference data considerably better than the commonly used CA model.
我们提出了一个用于建模车辆运动的通用框架。通过将驾驶员的影响纳入模型，获得了更好的预测能力。此外，通过对基础模型参数中的不确定性进行正式化处理，改善了对预测不确定性的描述。特别关注的是将模型适应于驾驶员遵循道路右侧车道的情况。我们收集了来自真实驾驶情况的准确数据，以研究模型特性。对于评估的测试场景，我们已经证明，使用我们的框架开发的模型比常用的 CA 模型更好地描述了参考数据。
There are several possibilities to further develop the motion model. First of all, more driver intentions should be added to capture interesting scenarios, such as lane changes, overtakings, and distracted drivers. For these scenarios, the main challenge is to design suitable cost functions that accurately represent the driver behavior and still result in a manageable optimization problem. Further, a more extensive validation is needed using more test scenarios and several different drivers. The validation data should be used to investigate the model’s capability to adapt to different driving styles, as well as to be used to fine tune the cost functions presented in this paper. Finally, it would be interesting to explore the usefulness of the prediction capability of the model in a decision making algorithm, for instance, in a collision avoidance application or for run off road detection.
有几种可能性可以进一步发展运动模型。首先，应该增加更多的驾驶员意图，以捕捉有趣的场景，例如变道、超车和分心驾驶。对于这些场景，主要挑战是设计合适的成本函数，以准确表示驾驶员行为，并仍然导致可管理的优化问题。此外，需要使用更多的测试场景和几种不同的驾驶员进行更广泛的验证。验证数据应被用来调查模型适应不同驾驶风格的能力，以及用于微调本文中提出的成本函数。最后，探索模型在决策算法中的预测能力的有用性将是有趣的，例如在碰撞避免应用或越界检测中。

REFERENCES 参考文献

[1] “Requirements for preventive safety applications,” PReVENT IP Deliverable 4, Apr. 16, 2005. [Online]. Available: www.ip-prevent.org
[1] “预防安全应用的要求，”PReVENT IP 交付物 4，2005 年 4 月 16 日。[在线]。可用： www.ip-prevent.org
[2] C. P. Robert, The Bayesian Choice: A Decision-Theoretic Motivation. New York: Springer-Verlag, 1994.
[2] C. P. Robert, 《贝叶斯选择：决策理论动机》。纽约：施普林格-弗拉格，1994。
[3] X. R. Li and V. Jilkov, “Survey of maneuvering target tracking. Part I. Dynamic models,” IEEE Trans. Aerosp. Electron. Syst., vol. 39, no. 4, pp. 1333-1364, Oct. 2003.
[3] X. R. Li 和 V. Jilkov，“机动目标跟踪综述。第一部分。动态模型，”IEEE Trans. Aerosp. Electron. Syst., vol. 39, no. 4, pp. 1333-1364, 2003 年 10 月。
[4] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation With Applications to Tracking and Navigation. New York: Wiley, 2001.
[4] Y. Bar-Shalom, X. R. Li, 和 T. Kirubarajan, 《估计及其在跟踪和导航中的应用》。纽约：Wiley，2001。
[5] S. Blackman and R. Popoli, Design and Analysis of Modern Tracking Systems. Norwood, MA: Artech House, 1999.
[5] S. Blackman 和 R. Popoli, 现代跟踪系统的设计与分析. 诺伍德, MA: Artech House, 1999.
[6] R. Mobus, M. Baotic, and M. Morari, “Multi-object adaptive cruise control,” in Proc. 6th Int. Workshop HSCC, Prague, Czech Republic, Sep. 2003.
[6] R. Mobus, M. Baotic, 和 M. Morari, “多目标自适应巡航控制,” 在第六届国际研讨会 HSCC 论文集中, 捷克共和国布拉格, 2003 年 9 月。
[7] A. Broadhurst, S. Baker, and T. Kanade, “Monte Carlo road safety reasoning,” in Proc. IEEE Intell. Vehicles Symp., Las Vegas, NV, 2005, pp. 319-324.
[7] A. Broadhurst, S. Baker, 和 T. Kanade, “蒙特卡洛道路安全推理,” 在 IEEE 智能车辆研讨会论文集中, 拉斯维加斯, NV, 2005, 第 319-324 页。
[8] P. Gipps, “A behavioural car-following model for computer simulation,” Transp. Res. Part B, vol. 15, no. 2, pp. 105-111, 1981.
[8] P. Gipps, “一种用于计算机模拟的行为跟车模型,” 交通研究 B 部分, 第 15 卷, 第 2 期, 第 105-111 页, 1981.
[9] [Online]. Available: http://www.isi-padas.eu/
[9] [在线]. 可用: http://www.isi-padas.eu/
[10] [Online]. Available: http://www.iterate-project.eu/
[10] [在线]. 可用: http://www.iterate-project.eu/
[11] S. M. LaValle, Planning Algorithms. Cambridge, U.K.: Cambridge Univ. Press, 2006.
[11] S. M. LaValle, 规划算法。英国剑桥：剑桥大学出版社，2006 年。
[12] L. Svensson and J. Gunnarsson, “A new motion model for tracking of vehicles,” in Proc. 14th IFAC Symp. Syst. Identification, Newcastle, Australia, Mar. 2006.
[12] L. Svensson 和 J. Gunnarsson, “一种用于车辆跟踪的新运动模型,” 载于第 14 届 IFAC 系统识别研讨会论文集, 澳大利亚纽卡斯尔, 2006 年 3 月。
[13] J. Gunnarsson, L. Svensson, F. Bengtsson, and L. Danielsson, “Joint driver intention classification and tracking of vehicles,” in Proc. Nonlinear Stat. Signal Process. Workshop, Cambridge, U.K., Sep. 2006, pp. 95-98.
[13] J. Gunnarsson, L. Svensson, F. Bengtsson, 和 L. Danielsson, “联合驾驶员意图分类与车辆跟踪,” 在非线性统计信号处理研讨会, 剑桥, 英国, 2006 年 9 月, 第 95-98 页.
[14] R. Kalman, “A new approach to linear filtering and prediction problems,” Trans. ASME, Ser. D. J. Basic Eng., vol. 82, pp. 24-45, 1960.
[14] R. Kalman, “一种新的线性滤波和预测问题的方法，” ASME 交易，D 系列。基础工程杂志，第 82 卷，第 24-45 页，1960 年。
[15] J. Meriam and L. Kraige, Engineering Mechanics. New York: Wiley, 1993.
[15] J. Meriam 和 L. Kraige，《工程力学》。纽约：Wiley，1993。
[16] U. Kiencke and L. Nielsen, Automotive Control Systems for Engine, Driveline and Vehicle. New York: Springer-Verlag, 2000.
[16] U. Kiencke 和 L. Nielsen, 汽车控制系统：发动机、传动系统和车辆. 纽约: Springer-Verlag, 2000.
[17] N. Kaempchen, K. Weiss, M. Schaefer, and K. Dietmayer, “IMM object tracking for high dynamic driving maneuvers,” in Proc. IEEE Intell. Vehicles Symp., Jun. 2004, pp. 825-830.
[17] N. Kaempchen, K. Weiss, M. Schaefer, 和 K. Dietmayer, “IMM 对象跟踪用于高动态驾驶机动,” 在 IEEE 智能车辆研讨会论文集, 2004 年 6 月, 第 825-830 页。
[18] A. Eidehall, “An automotive lane guidance system,” Licentiate Thesis no. 1122, Dept. Elect. Eng., Linköping Univ., Linköping, Sweden, 2004.
[18] A. Eidehall, “汽车车道引导系统,” 学位论文第 1122 号，电子工程系，林雪平大学，瑞典林雪平，2004 年。
[19] E. Dickmanns and A. Zapp, “A curvature-based scheme for improving road vehicle guidance by computer vision,” in Proc. SPIE Conf. Mobile Robots, 1986, pp. 161-168.
[19] E. Dickmanns 和 A. Zapp, “一种基于曲率的方案，通过计算机视觉改善道路车辆引导,” 在 Proc. SPIE Conf. Mobile Robots, 1986, pp. 161-168.
[20] G. Leitmann, The Calculus of Variations and Optimal Control. New York: Plenum, 1981.
[20] G. Leitmann, 变分法与最优控制. 纽约: Plenum, 1981.
[21] J. Betts, Practical Methods for Optimal Control Using Nonlinear Programming. Philadelphia, PA: SIAM, 2001.
[21] J. Betts, 实用的非线性编程最优控制方法。费城，宾夕法尼亚州：SIAM，2001。
[22] W. Murray, P. E. Gill, and M. A. Saunders, “SNOPT: An SQP algorithm for large-scale constrained optimization,” SIAM J. Optim., vol. 12, no. 4, pp. 979-1006, 2002.
[22] W. Murray, P. E. Gill, 和 M. A. Saunders, “SNOPT: 一种用于大规模约束优化的 SQP 算法,” SIAM J. Optim., vol. 12, no. 4, pp. 979-1006, 2002.
[23] ISSN: 1401-9612 Vgu, vv publikation 2004:80, grundvärden, Jun. 2004.
[23] ISSN: 1401-9612 Vgu, vv publikation 2004:80, grundvärden, 2004 年 6 月。
[24] E. Bertolazzi, F. Biral, and M. Da Lio, “Future advanced driver assistance systems based on optimal control: The influence of “risk functions” on overall system behavior and on prediction of dangerous situations,” in Proc. Intell. Vehicles Symp., Parma, Italy, 2004, pp. 386-391.
[24] E. Bertolazzi, F. Biral, 和 M. Da Lio, “基于最优控制的未来高级驾驶辅助系统：‘风险函数’对整体系统行为和危险情况预测的影响，” 载于 Proc. Intell. Vehicles Symp., 意大利帕尔马, 2004, pp. 386-391.
[25] E. Bertolazzi, F. Biral, M. Da Lio, A. Saroldi, and F. Tango, “Supporting drivers in keeping safe speed and safe distance: The SASPENCE subproject within the European framework programme 6 integrating project PReVENT,” IEEE Trans. Intell. Transp. Syst., vol. 11, no. 3, pp. 525-538, Sep. 2010.
[25] E. Bertolazzi, F. Biral, M. Da Lio, A. Saroldi, 和 F. Tango, “支持驾驶员保持安全速度和安全距离：欧洲第六框架计划下的 SASPENCE 子项目，整合项目 PReVENT,” IEEE Trans. Intell. Transp. Syst., vol. 11, no. 3, pp. 525-538, 2010 年 9 月。
[26] S. Julier and J. Uhlmann, “A new extension of the Kalman filter to nonlinear systems,” in Proc. AeroSense: 11th Int. Symp. Aerosp./Defense Sens., Simul. Controls, Multi Sensor Fusion, Tracking Resour. Manage. II, 1997, pp. 182-193.
[26] S. Julier 和 J. Uhlmann，“卡尔曼滤波器对非线性系统的新扩展，”发表于 Proc. AeroSense: 第 11 届国际航空/防御传感器、仿真控制、多传感器融合、跟踪资源管理研讨会 II，1997 年，页 182-193。
[27] E. Wan and R. Van Der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proc. IEEE Adapt. Syst. Signal Process., Commun., Control Symp., 2000, pp. 153-158.
[27] E. Wan 和 R. Van Der Merwe, “用于非线性估计的无迹卡尔曼滤波器,” 见于 Proc. IEEE Adapt. Syst. Signal Process., Commun., Control Symp., 2000, pp. 153-158.
[28] H. Jeffreys, Theory of Probability, 3rd ed. London, U.K.: Oxford Univ. Press, 1961.
[28] H. Jeffreys, 概率论, 第 3 版. 英国伦敦: 牛津大学出版社, 1961.

Joakim Sörstedt was born in Skara, Sweden, on April 3, 1976. He received the M.S. and Ph.D. degrees in electrical engineering from Chalmers University of Technology, Gothenburg, Sweden, in 2001 and 2007, respectively.
乔基姆·索尔斯特德于 1976 年 4 月 3 日出生于瑞典斯卡拉。他于 2001 年和 2007 年分别获得瑞典哥德堡查尔默斯科技大学的电气工程硕士和博士学位。
Since 2007, he has been with Volvo Car Corporation, Gothenburg, where he has worked on developing active safety systems. His research interests are in the area of signal processing, specifically subspace-based estimation methods and object tracking.
自 2007 年以来，他一直在瑞典哥德堡的沃尔沃汽车公司工作，致力于开发主动安全系统。他的研究兴趣在于信号处理领域，特别是基于子空间的估计方法和目标跟踪。

Lennart Svensson was born in Älvängen, Sweden, in 1976. He received the M.S. and Ph.D. degrees in electrical engineering from Chalmers University of Technology, Gothenburg, Sweden, in 1999 and 2004, respectively.
伦纳特·斯文森于 1976 年出生在瑞典的艾尔文根。他于 1999 年和 2004 年分别获得瑞典哥德堡查尔姆斯科技大学的电气工程硕士和博士学位。
He is currently an Associate Professor with the Signal Processing Group, Chalmers University of Technology. His research interests include Bayesian inference in general and nonlinear filtering and tracking in particular.
他目前是查尔姆斯科技大学信号处理组的副教授。他的研究兴趣包括贝叶斯推断一般以及非线性滤波和跟踪特别。

Fredrik Sandblom was born in Mölndal, Sweden, in 1979. He received the M.S. degree in electrical engineering and the Licentiate of Engineering degree from Chalmers University of Technology, Gothenburg, Sweden, in 2004 and 2008, respectively.
弗雷德里克·桑德布隆于 1979 年出生于瑞典莫尔达尔。他于 2004 年和 2008 年分别获得瑞典哥德堡查尔默斯理工大学的电气工程硕士学位和工程学许可学位。

Since 2005, he has been with Volvo 3P, Gothenburg, where he works on active safety system research as a part of his Ph.D. studies. His research interests concern object tracking and sensor data fusion, particularly methods for estimating statistical moments.
自 2005 年以来，他一直在哥德堡的沃尔沃 3P 工作，作为其博士研究的一部分从事主动安全系统研究。他的研究兴趣涉及目标跟踪和传感器数据融合，特别是估计统计矩的方法。

Lars Hammarstrand received the M.Sc. and Ph.D degrees in electrical engineering from Chalmers University of Technology, Gothenburg, Sweden, in 2004 and 2010, respectively
拉尔斯·哈马斯特兰德于 2004 年和 2010 年分别获得瑞典哥德堡查尔默斯理工大学的电气工程硕士和博士学位。

He is currently with the Active Safety and Chassis Department, Volvo Car Corporation, Gothenburg, where he works on sensor data fusion development. His main research interests are in the fields of target tracking and radar sensor modeling, particularly with application to active safety systems.
他目前在沃尔沃汽车公司哥德堡的主动安全与底盘部门工作，专注于传感器数据融合开发。他的主要研究兴趣在于目标跟踪和雷达传感器建模领域，特别是应用于主动安全系统。

Manuscript received September 9, 2010; revised March 31, 2011; accepted April 22, 2011. Date of publication July 22, 2011; date of current version December 5, 2011. This work was supported by the Swedish Intelligent Vehicle Safety Systems (IVSS) program and was a part of the SEnsor Fusion for Safety Systems (SEFS) project. The Associate Editor for this paper was H. Dia.
手稿于 2010 年 9 月 9 日收到；2011 年 3 月 31 日修订；2011 年 4 月 22 日接受。出版日期为 2011 年 7 月 22 日；当前版本日期为 2011 年 12 月 5 日。本工作得到了瑞典智能车辆安全系统（IVSS）项目的支持，并且是安全系统传感器融合（SEFS）项目的一部分。本文的副编辑是 H. Dia。
J. Sörstedt and L. Hammarstrand are with Volvo Car Corporation, 40531 Gothenburg, Sweden (e-mail: jsorsted@volvocars.com; lhammar5@volvocars. com).
J. Sörstedt 和 L. Hammarstrand 在瑞典哥德堡 40531 的沃尔沃汽车公司工作（电子邮件：jsorsted@volvocars.com；lhammar5@volvocars.com）。
L. Svensson is with the Department of Signals and Systems, Chalmers University of Technology, 41296 Gothenburg, Sweden (e-mail: lennart.svensson@ chalmers.se).
L. Svensson 在瑞典哥德堡 41296 的查尔默斯科技大学信号与系统系（电子邮件：lennart.svensson@chalmers.se）。
F. Sandblom is with Volvo 3P, 40508 Gothenburg, Sweden (e-mail: fredrik. sandblom.2@ volvo.com).
F. Sandblom 在瑞典哥德堡 40508 的沃尔沃 3P 工作（电子邮件：fredrik.sandblom.2@volvo.com）。

Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org.
本论文中一个或多个图的彩色版本可在线获取，网址为 http://ieeexplore.ieee.org。

Digital Object Identifier 10.1109/TITS.2011.2160342
数字对象标识符 10.1109/TITS.2011.2160342
$^{1}$ In this section, all densities are conditioned on the initial states $z_{0}^{h}$ and $z_{0}^{t}$ , even though these variables are omitted for brevity.
在本节中，所有密度都以初始状态 $z_{0}^{h}$ 和 $z_{0}^{t}$ 为条件，尽管为了简洁起见，这些变量被省略。
$^{2} As z_{k}^{h}$ is related more directly to $θ_{k - 1}$ than to $θ_{k}$ , the suggested order (first update, then predict) is more straightforward; normally, one would instead first predict, compute $p (θ_{k} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})$ , and then do the update and calculate $p (θ_{k} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})$ .
$^{2} As z_{k}^{h}$ 与 $θ_{k - 1}$ 的关系比与 $θ_{k}$ 更直接，建议的顺序（先更新，再预测）更简单；通常，人们会先进行预测，计算 $p (θ_{k} ∣ Z_{k - 1}^{h}, Z_{L}^{t}, M_{2})$ ，然后再进行更新并计算 $p (θ_{k} ∣ Z_{k}^{h}, Z_{L}^{t}, M_{2})$ 。
$^{3}$ In previous sections, $u_{k - 1} (\cdot)$ only took $x_{k - 1}$ as input. However, when $Z_{L}^{t}$ is known, we use it as an additional input. To understand how this influences the computations, see the discussion at the end of Section V-B3.
$^{3}$ 在前面的章节中， $u_{k - 1} (\cdot)$ 仅将 $x_{k - 1}$ 作为输入。然而，当 $Z_{L}^{t}$ 已知时，我们将其作为额外的输入。要了解这如何影响计算，请参见第 V-B3 节末尾的讨论。

A New Vehicle Motion Model for Improved Predictions and Situation Assessment 一种新的车辆运动模型，用于改进预测和情境评估

Abstract 摘要

I. Introduction I. 引言

II. BACKGROUND II. 背景

III. Motion Model FrameworkIII. 运动模型框架

A. Model Structure A. 模型结构

B. Calculating the Driver Input u k ( x k ) u k x k u_(k)(x_(k))\mathbf{u}_{k}\left(\mathbf{x}_{k}\right)B. 计算驱动程序输入 u k ( x k ) u k x k u_(k)(x_(k))\mathbf{u}_{k}\left(\mathbf{x}_{k}\right)

C. Model Features C. 模型特征

IV. Designing Cost FunctionsIV. 设计成本函数

A. Longitudinal Cost Function c l o ( ⋅ , ⋅ ) c l o ( ⋅ , ⋅ ) c_(lo)(*,*)c_{l o}(\cdot, \cdot)A. 纵向成本函数 c l o ( ⋅ , ⋅ ) c l o ( ⋅ , ⋅ ) c_(lo)(*,*)c_{l o}(\cdot, \cdot)

B. Lateral Cost Function c l a ( ⋅ , ⋅ ) c l a ( ⋅ , ⋅ ) c_(la)(*,*)c_{l a}(\cdot, \cdot)B. 侧向成本函数 c l a ( ⋅ , ⋅ ) c l a ( ⋅ , ⋅ ) c_(la)(*,*)c_{l a}(\cdot, \cdot)

C. Comfort Cost Function c c ( ⋅ , ⋅ ) c c ( ⋅ , ⋅ ) c_(c)(*,*)c_{c}(\cdot, \cdot)

D. Cost Functions for Vehicle Interaction c in ( ⋅ , ⋅ ) c in ( ⋅ , ⋅ ) c_("in ")(*,*)c_{\text {in }}(\cdot, \cdot)D. 车辆交互的成本函数 c in ( ⋅ , ⋅ ) c in ( ⋅ , ⋅ ) c_("in ")(*,*)c_{\text {in }}(\cdot, \cdot)

V. Evaluation Results V. 评估结果

M 1 M 1 M_(1)\mathcal{M}_{1} : constant acceleration model M 1 M 1 M_(1)\mathcal{M}_{1} : 恒定加速度模型

A. Evaluation Criteria A. 评估标准

B. Filtering θ k θ k theta_(k)\boldsymbol{\theta}_{k} B. 过滤 θ k θ k theta_(k)\boldsymbol{\theta}_{k}

C. Results C. 结果

VI. Conclusion and Future WorkVI. 结论与未来工作

REFERENCES 参考文献

A New Vehicle Motion Model for Improved Predictions and Situation Assessment
一种新的车辆运动模型，用于改进预测和情境评估

III. Motion Model Framework
III. 运动模型框架

B. Calculating the Driver Input $u_{k} (x_{k})$
B. 计算驱动程序输入 $u_{k} (x_{k})$

IV. Designing Cost Functions
IV. 设计成本函数

A. Longitudinal Cost Function $c_{l o} (\cdot, \cdot)$
A. 纵向成本函数 $c_{l o} (\cdot, \cdot)$

B. Lateral Cost Function $c_{l a} (\cdot, \cdot)$
B. 侧向成本函数 $c_{l a} (\cdot, \cdot)$

C. Comfort Cost Function $c_{c} (\cdot, \cdot)$

D. Cost Functions for Vehicle Interaction $c_{in} (\cdot, \cdot)$
D. 车辆交互的成本函数 $c_{in} (\cdot, \cdot)$

$M_{1}$ : constant acceleration model
$M_{1}$ : 恒定加速度模型

B. Filtering $θ_{k}$ B. 过滤 $θ_{k}$

VI. Conclusion and Future Work
VI. 结论与未来工作