CN1668938A - 道路曲率估计和车辆目标状态估计系统 - Google Patents

Abstract

第一卡尔曼滤波器(52)根据主车辆(12)上的各个传感器(16，18)产生的相关有噪测量来估计横摆率(ω)和车辆速度(U)的实际测量，且第二卡尔曼滤波器(54)估计道路曲率的回旋曲线的参数(C₀，C₁)。来自目标状态估计子系统(44)如雷达系统(14)的范围(r)、范围速率()和方位角(η)的测量经扩展卡尔曼滤波器(56)处理，以提供目标车辆(36)的状态的无约束估值。产生一个或多个道路车道(38，40)的相关道路有约束目标估值，并和无约束估值进行比较——单独比较或结合比较。如果有约束目标状态估值与无约束估值一致，则通过合并无约束和有约束估值产生目标车辆的状态；否则仅通过无约束估值给定。

Description

道路曲率估计和车辆目标状态估计系统

附图说明

在附图中：

图1说明了关于预测碰撞传感系统的硬件的方框图；

图2说明了预测碰撞传感系统采用的雷达波束的覆盖图；

图3描述了用以说明预测碰撞传感系统的工作的驱动情况；

图4说明了预测碰撞传感系统的硬件及相关信号处理算法的方框图；

图5说明了预测碰撞传感系统的相关信号处理算法的流程图；

图6说明了用于确定道路的曲率参数的几何图形；

图7说明了圆弧的几何图形；

图8a-d说明了用于直线道路的目标位置、横向速率及道路曲率参数的估计的例子；

图9a-b说明了在对应图8a-d的直线道路上的来自无约束(unconstrained)及有约束(constrained)滤波的目标状态RMS误差的例子；

图10a-d说明了用于曲线道路的目标位置、横向速率和道路曲率参数的估计的例子；

图11a-b说明了对应图10a-d的曲线道路的来自无约束和有约束滤波的目标状态RMS误差的例子；

图12a-d说明了用于包含车道转换的直线道路的目标位置、横向速率及相关RMS误差的估计的例子，及

图13a-d说明了用于包含车道转换的曲线道路的目标位置、横向速率及其RMS误差的估计的例子。

具体实施方式

参考图1，合并在主车辆12中的预测碰撞传感系统10包括：用于传感主车辆12外部的物体的雷达系统14；和一套传感器，包括横摆率传感器16，如陀螺传感器，及速度传感器18，用于传感主车辆12的运动。横摆率传感器16和速度传感器18分别提供对主车辆12的横摆率和速度的测量。雷达系统14，如多普勒雷达系统，包括：天线20和雷达处理器22，其中雷达处理器22产生射频信号，它由天线22发送并被在其视角范围内的物体反射。雷达处理器22将天线20接收的相关反射的射频信号进行解调，并探测一个信号，该信号对应于天线20发送的射频信号所辐射到的一个或多个物体。例如，雷达系统14提供主车辆12固定坐标内的目标范围、范围速率和方位角测量。参考图2，天线20适合用于产生射频能量的雷达波束23，其例如对应于波束控制元件24被电或机械扫描一个方向角范围，如+/-γ，如+/-50度，并且它离主车辆12约有例如100米的距离范围，这个距离范围足够远，可使目标在与主车辆12可能发生碰撞之前足够远的地方就被探测到，从而潜在减轻了主车辆12的动作，从而避免了可能发生的碰撞或减轻了损伤或受伤。雷达处理器22、横摆率传感器16和速度传感器18可操作地连接到信号处理器26上，信号处理器26根据相关的预测碰撞传感算法工作，确定是否可能与物体碰撞，如目标车辆36(图3中所示)，如果是，还要确定采取相应的行动，例如，如下一个或多个行动：激活相关警告系统28或安全系统30(如前部安全气囊系统)，或采用规避行动的车辆控制系统32(如相关制动或转向系统)，采取以上一个或多个方式以便避免可能发生的碰撞或减轻后果。

参考图3，主车辆12沿着多车道道路34移动，道路可为直线也可为曲线，另外显示了目标车辆36朝着主车辆12以相反的方向移动。通常，在道路34上可能有任何数目的目标车辆36，每个目标车辆都与主车辆12以相同或相反的方向移动。这些目标车辆36可在主车道38上也可在与主车道38相邻或分离但通常与主车道38平行的邻近车道40上。为了分析起见，假设主车辆12沿着车道38的中心线41稳步移动，没有在车道内偏移，而且所有平行车道38、40的道路曲率是相同的。假设道路曲率很小，以致主车辆12和所有可探测到的目标车辆36的方向角间的差值小于15度。

参考图4，预测碰撞传感系统10采用分别来自速度传感器18和横摆率传感器16的主车辆12的速度U^h和横摆率ω^h的测量值；来自安装在主车辆12上的雷达系统14的对于所有目标车辆36的目标范围r，范围速率

和方位角η的测量值；以及所有这些测量值的对应误差协方差矩阵，以对于每一取样距离估计固定主坐标系统上每一个目标的二维位置、速度和加速度 优选具有尽可能小的误差。预测碰撞传感系统10包括：1)道路曲率估计子系统42，采用主车辆运动传感器，即横摆率传感器16和速度传感器18的测量值来估计道路34的曲率；2)无约束(unconstrained)目标状态估计子系统44，用于估计雷达波束23探照到并被雷达处理器22探测到的目标的状态；3)有约束(constrained)目标状态估计子系统46，用于估计目标上的约束条件(constraint)的状态，假设目标被约束在道路34上，可在主车道38上也可在邻近车道40内，车道38和40每一个都有可能；4)目标状态判定子系统48，用于确定目标状态的最佳估值是无约束目标状态还是受到一个约束条件约束的目标状态；及5)目标状态合并子系统50，用于将无约束的目标状态估计与目标状态判定子系统48所识别的适当约束条件合并，以产生合并的目标状态。目标状态的最佳估值——可为无约束目标状态也可为合并目标状态——被判决或控制子系统用来确定主车辆12是否存在与目标发生碰撞的危险，如果有，则决定并实行最佳行动路线以减轻后果，如采用警告系统28或安全系统30，或车辆控制系统32，或这些措施的结合。如果可能，道路34的几何结构作为目标运动的一个约束条件，提供了更为精确的目标状态估计，因而改善了对应于此所采取任何行动的可靠性。

另外参考图5，说明了探测状态的方法500，即主车辆12可视范围内的目标的运动状态变化，该方法的步骤例如通过信号处理器26执行，在步骤(502)和(504)中，主车辆12相对于道路34的速度U^h和横摆率ω^h分别从速度传感器18和横摆率传感器16中读取。然后，在步骤(506)中，道路34的曲率参数及其相关协方差用第一52和第二54卡尔曼滤波器估计，它们分别估计主车辆12的状态(即主车辆12的运动状态变化)及其相关协方差，然后估计道路34的曲率参数及其相关协方差，如下文所述，其中道路34的曲率参数及其相关协方差随后被有约束目标状态估计子系统46用来在可能发生的目标车辆36的可能位置上产生相关约束条件。

严格定义和构造的道路34可用一套参数来描述，包括曲率，其中道路34的弓形部分的曲率由下式定义：

$C=\frac{1}{R}---\left(1\right)$

这里R为弓形部分的半径。通常，对于一段光滑道路34来说，曲率变化可借用所谓的回旋曲线模型描述成沿着道路34的距离l的函数，即：

$C=C_0+\frac{\mathrm{dC}}{\mathrm{dl}}l=C_0+C_{1}l---\left(2\right)$

这里C₁＝1/A²而A称为回旋曲线参数。

参考图6，定义行进方向的方向角θ由下式给定：

$\mathrm{\θ}={\mathrm{\θ}}_0+{\∫}_0^{1}C\left(\mathrm{\τ}\right)\mathrm{d\τ}---\left(3\right)$

将方程(2)带入方程(3)得到：

Δθ＝θ-θ₀＝C₀l+C₁l²/2                        (4)

参考图6，道路34的方程，即x-y坐标内的道路方程由下式给定：

$x=x_0+{\∫}_0^1\cos \mathrm{\θ}\left(\mathrm{\τ}\right)\mathrm{d\τ}---\left(5\right)$

及

假设方向角θ在15度内，即|θ|＜15°，方程(5)和方程(6)可近似为：

Δx＝x-x₀≈l                 (7)

$\mathrm{\Δy}=y-y_0\≈C_0{l}^2/2+C_1{l}^3/6\≈C_0\frac{{\mathrm{\Δx}}^2}{2}+C_1\frac{\mathrm{\Δ}{x}^3}{6}---\left(8\right)$

因此，道路34可用曲率系数C₀和C₁表示的增量道路方程来模拟。这个增量道路方程描述了道路形状的主要范围：1)直线道路34：C₀＝0和C₁＝1；2)，环形道路34：C₁＝0；及3)具有任意形状的一般道路34，其方向角θ的变化小于15度：C₀＞0。

道路曲率参数C₀和C₁采用来自主车辆12的运动传感器(横摆率传感器16和速度传感器18)的数据来估计，它基于主车辆12沿着道路34或相关主车道38的中心线41运动的假设。

道路曲率参数C₀和C₁根据与来自可得到的主车辆12运动传感器的主车辆12的横摆率ω^h和速度U^h测量值对应的ω，

，U， 数据来计算。但是，通常分别来自横摆率传感器16和速度传感器18的横摆率ω^h和速度U^h的测量值是有噪声的。由第一卡尔曼滤波器52实现的主状态滤波器有利于根据相关的横摆率ω^h和速度U^h的有噪测量值来估计ω，

，U， ；之后，由第二卡尔曼滤波器54实现的曲率滤波器用来产生曲率参数C₀和C₁的平滑估计。用于主状态滤波器的主车辆12的动力学遵循一套预定义的动力学方程(在这种情况下为常速度)由下式给定：

${\underset{\‾}{x}}_{k+1}^h=F_k^h\·{\underset{\‾}{x}}_k^h+{\underset{\‾}{w}}_k^h,{\underset{\‾}{w}}_k^h~N(0,Q_k^h)---\left(9\right)$

${\underset{\‾}{z}}_k^h=H_k^h\·{\underset{\‾}{x}}_k^h+{\underset{\‾}{v}}_k^h,{\underset{\‾}{v}}_k^h~N(0,R_k^h)---\left(10\right)$

其中

$F_k^h=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],H_k^h=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right],{\underset{\‾}{x}}_k^h={\left[\begin{array}{c}U\\ \stackrel{\·}{U}\\ \mathrm{\ω}\\ \stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_k,$ 及 ${\underset{\‾}{z}}_k^h={\left[\begin{array}{c}{U}^h\\ {\mathrm{\ω}}^h\end{array}\right]}_k---\left(11\right)$

且其中T为采样周期，上标(·)^h用来表示滤波器为主滤波器，而U^h和ω^h为主车辆12速度和横摆率测量值。第一卡尔曼滤波器52用来估计主状态

及它的误差协方差P_k|k ^h，如图4中所示。

来自第一卡尔曼滤波器52即主状态滤波器的主状态估计用来产生合成测量值，它输入到第二卡尔曼滤波器54上，即曲率系数滤波器，其中相关的卡尔曼滤波器52、54根据卡尔曼滤波过程工作，将在下文附录中更充分说明。道路曲率参数C₀和C₁及主状态变量ω，

，U，

间的关系推导如下：

根据方程(4)，道路曲率的半径R通常表示为沿着道路的距离l的函数R(l)，如图7中所示。在方程(4)两边都对时间取导数，得到：

$\stackrel{\·}{\mathrm{\θ}}=C_0\·\stackrel{.}{l}+C_1\·l\·\stackrel{.}{l}=(C_0+C_1\·l)\·\stackrel{.}{l}---\left(12\right)$

注意： $\stackrel{.}{\mathrm{\θ}}=\mathrm{\ω},$ 主车辆12的横摆率，及 $\stackrel{.}{l}=U,$ 主车辆12的速度，将方程(2)的回旋曲线模型带入方程(12)中，得到：

ω＝C·U                          (13)

或

$C=\frac{\mathrm{\ω}}{U}---\left(14\right)$

回旋曲线参数C₀由l＝0处的曲率值给定，或

$C_0=C{|}_{l=0}=\frac{\mathrm{\ω}}{U}---\left(15\right)$

对方程(14)的两边都求导，得到：

$\stackrel{\·}{C}=\frac{\stackrel{\·}{\mathrm{\ω}}}{U}-\frac{\mathrm{\ω}\·\stackrel{\·}{U}}{U^2}---\left(16\right)$

使用根据方程(2)的C₁的定义，C₁可用主状态来表示，如下：

$C_1=\frac{\mathrm{dC}}{\mathrm{dl}}=\frac{\mathrm{dC}}{\mathrm{dt}}\·\frac{\mathrm{dt}}{\mathrm{dl}}=\frac{\stackrel{\·}{C}}{U}=\frac{\stackrel{\·}{\mathrm{\ω}}}{U^2}-\frac{\mathrm{\ω}\·\stackrel{\·}{U}}{U^3}---\left(17\right)$

第二卡尔曼滤波器54即曲率滤波器产生曲率估计 和 ，其系统方程由下式给定：F

${\underset{\‾}{x}}_{k+1}^C=F_k^C\·{\underset{\‾}{x}}_k^C+{\underset{\‾}{w}}_k^C,{\underset{\‾}{w}}_k^C~N(0,Q_k^C)---\left(18\right)$

${\underset{\‾}{z}}_k^C=H^C\·{\underset{\‾}{x}}_k^C+v_k^C,v_k^C~N(0,R_k^C)---\left(19\right)$

其中

$F_k^C=\left[\begin{array}{cc}1& \mathrm{\Δt}\·\hat{U}\\ 0& 1\end{array}\right],H_k^h=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],{\underset{\‾}{x}}_k^C={\left[\begin{array}{c}{C}_0\\ C_1\end{array}\right]}_k---\left(19a\right)$

Δt为第二卡尔曼滤波器54的更新时间周期，而测量向量 z _k ^C的元素值由曲率滤波器的状态变量——即回旋曲线参数C₀和C₁——的对应值给定。测量值 z _k ^C从估计的状态 转换得来，如下：

${\underset{\‾}{z}}_k^C={\left[\begin{array}{c}\frac{\widehat{\mathrm{\ω}}}{\hat{U}}\\ \frac{\widehat{\stackrel{\·}{\mathrm{\ω}}}}{{\hat{U}}^2}-\frac{\widehat{\mathrm{\ω}}\·\widehat{\stackrel{\·}{U}}}{{\hat{U}}^3}\end{array}\right]}_{k|k}---\left(20\right)$

及测量值的相关协方差由下式给定：

$R_k^C=J_k^C{P}_{k|k}^h{\left(J_k^C\right)}^T---\left(21\right)$

其中

$J_k^C=\frac{\∂\left[\begin{array}{c}{C}_0\\ C_1\end{array}\right]}{\∂{\underset{\‾}{x}}^h}{{|}_{{\underset{\‾}{x}}^h={\underset{\‾}{\overset{^}{x}}}_{k|k}^h}=\left[\begin{array}{cccc}-\frac{\widehat{\mathrm{\ω}}}{{\hat{U}}^2}& 0& \frac{1}{\hat{U}}& 0\\ -\frac{2\·\widehat{\stackrel{\·}{\mathrm{\ω}}}}{{\hat{U}}^3}+\frac{3\·\widehat{\mathrm{\ω}}\·\widehat{\stackrel{\·}{U}}}{{\hat{U}}^4}& -\frac{\widehat{\stackrel{\·}{\mathrm{\ω}}}}{{\hat{U}}^3}& -\frac{\widehat{\stackrel{\·}{U}}}{{\hat{U}}^3}& \frac{1}{{\hat{U}}^2}\end{array}\right]}_{k|k}---\left(22\right)$

应该理解，估计道路34的曲率参数的其它系统和方法也可替代上述的道路曲率估计子系统42。例如，道路曲率参数也可通过可视系统，根据道路34的图像来估计，可以取代也可结合基于来自相关运动传感器的速度U^h和横摆率ω^h的测量值的上述系统。而且，应该理解，横摆率可以各种方式或采用各种手段测量或判断，例如可以采用(但不限于)横摆陀螺传感器、转向角传感器、差动轮速度传感器或基于GPS的传感器；及其结合；或其测量值的函数(即特别是其中的转向角速率的函数)。

再次参考图5，在步骤(508)中，目标范围r、范围速度

和方位角η的测量值从雷达处理器22中读取，用作扩展卡尔曼滤波器56即主滤波器的输入，其在步骤(510)中产生无约束目标状态的估值——即目标的动力学状态变量——这个估值是随之运动的主车辆12的局部坐标系统(即主固定坐标系统)中的相对值。在步骤(512)中，无约束目标状态，即目标速率和加速度，被转换成当前时刻固定在主车辆12上的绝对坐标系统的绝对坐标，如图3中所示，以便与绝对坐标系统一致，在绝对坐标系统中推导得到道路约束方程，且为了产生约束目标状态的估值，当用于下文所述的相关约束方程时，相关的曲率参数假设为常数。绝对坐标系统叠加当前瞬时的空间中的移动坐标系统，以便实现步骤(512)中的转换，它通过将主车辆12的x和y方向上的速率和加速度加到对应的目标估值上。

来自扩展卡尔曼滤波器56的输出的根据步骤(512)中的坐标转换的结果被分成如下部分，分别对应相对于主车辆12的目标车辆36的x和y位置，其中，上标1是指目标车辆36的无约束目标状态：

${\underset{\‾}{\overset{^}{X}}}_{t_{k|k}}^1={\left[\begin{array}{c}{\underset{\‾}{\overset{^}{x}}}_t^1\\ {\underset{\‾}{\overset{^}{y}}}_t^1\end{array}\right]}_{k|k}$ 及 $P_{t_{k|k}}^1={\left[\begin{array}{cc}{P}_{x_t}& P_{{\mathrm{xy}}_t}^1\\ P_{y{x}_t}^1& P_{y_t}^1\end{array}\right]}_{k|k}---\left(23\right)$

再次参考图5，紧随步骤(506)和(512)的步骤(514)到(524)在下文将进行更充分说明，在这些步骤中，加载并检验目标车辆36的可能轨迹上的各种约束条件，以判断目标车辆36是否可能按照一个可能的约束条件行进。例如，假设约束条件来自于包括主车道38和可能的邻近车道40的一组车道，可能按照一个可能约束条件而行进的目标车辆36可能在主车道38或者在可能的邻近车道40上行进。在步骤(524)中，假设目标车辆36在主车道38或一个可能的邻近车道40上行进，这个假设要针对每一个可能的车道进行测试。如果这个假设不满足一个可能的车道，那么，在步骤(526)中，假设目标的状态为无约束状态，然后将它用于对应的后续预测碰撞传感分析和控制。否则，从步骤(524)，在步骤(528)中，目标状态通过目标状态合并子系统50计算得到，它合并了无约束目标状态和步骤(524)中识别的最可能的相关的约束状态。

在讨论步骤(514)到(524)的过程之前，先就与主车辆12在相同车道上运动的目标车辆36的情况，对合并无约束目标状态和有约束的状态的过程进行说明，其中步骤(514)到(524)用于判定目标是否可能受到约束条件的约束，如果是，还要判定最可能的约束条件。假设约束条件仅在y方向有效，与主车辆12沿着其车道38的中心线41稳步移动而没有车道内偏移及所有平行车道38、40的道路曲率相同的假设一致，同时假设在当前瞬时，绝对坐标系统固定在主车辆12上。假设目标车辆36与主车辆12在相同的车道38上移动，采用包含估计系数的道路约束方程，在步骤(514)中，约束状态变量用横向运动变量给出：

${\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}={\left[\begin{array}{c}\stackrel{\‾}{y}\\ \stackrel{\‾}{\stackrel{\·}{y}}\\ \stackrel{\‾}{\stackrel{\·\·}{y}}\end{array}\right]}_{t_{k|k}}=\left[\begin{array}{c}{\hat{C}}_0{\left({\hat{x}}^1\right)}^2/2+{\hat{C}}_1{\left({\hat{x}}^1\right)}^3/6\\ {\hat{C}}_0{\hat{x}}^1{\widehat{\stackrel{\·}{x}}}^1+{\hat{C}}_1{\left({\hat{x}}^1\right)}^2{\widehat{\stackrel{\·}{x}}}^1/2\\ {\hat{C}}_0{\left({\widehat{\stackrel{\·}{x}}}^1\right)}^2+{\hat{C}}_0{\hat{x}}^1{\widehat{\stackrel{\·\·}{x}}}^1+{\hat{C}}_1{\hat{x}}^1{({\widehat{\stackrel{\·}{x}}}^1)}^2+{\hat{C}}_1{\left({\hat{x}}^1\right)}^2{\widehat{\stackrel{\·\·}{x}}}^1/2\end{array}\right]---\left(24\right)$

及

${\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}=A_k^1{P}_{{\mathrm{xt}}_{k|k}}{\left(A_k^1\right)}^T+A_k^2{P}_{k|k}^C{\left(A_k^2\right)}^T---\left(25\right)$

其中

$A_k^1=\left[\begin{array}{ccc}{\hat{C}}_0{\hat{x}}^1+{\hat{C}}_1{\left({\hat{x}}^1\right)}^2/2& 0& 0\\ {\hat{C}}_0{\widehat{\stackrel{\·}{x}}}^1+{\hat{C}}_1{\hat{x}}^1{\widehat{\stackrel{\·}{x}}}^1& {\hat{C}}_0{\hat{x}}^1+{\hat{C}}_1{\left({\hat{x}}^1\right)}^2/2& 0\\ {\hat{C}}_0{\widehat{\stackrel{\·\·}{x}}}^1+{\hat{C}}_1{\left({\widehat{\stackrel{\·}{x}}}^1\right)}^2+{\hat{C}}_1{\hat{x}}^1{\widehat{\stackrel{\·\·}{x}}}^1& 2{\hat{C}}_0\widehat{\hat{\stackrel{.}{x}+}2{\hat{C}}_1{\hat{x}}^1\hat{\stackrel{\·}{x}}}& {\hat{C}}_0{\hat{x}}^1+{\hat{C}}_1{\left({\hat{x}}^1\right)}^2/2\end{array}\right]---\left(26\right)$

及

$A_k^2=\left[\begin{array}{cc}{\left({\hat{x}}^1\right)}^2/2& {\left({\hat{x}}^1\right)}^3/6\\ {\hat{x}}^1\·{\widehat{\stackrel{\·}{x}}}^1& {\left({\hat{x}}^1\right)}^2\·{\widehat{\stackrel{\·}{x}}}^1/2\\ {\left({\widehat{\stackrel{\·}{x}}}^1\right)}^2+{\hat{x}}^1\·{\widehat{\stackrel{\·\·}{x}}}^1& {\hat{x}}^1\·{\left({\widehat{\stackrel{\·}{x}}}^1\right)}^2+{\left({\hat{x}}^1\right)}^2\·{\widehat{\stackrel{\·\·}{x}}}^1/2\end{array}\right]---\left(27\right)$

在步骤(528)中，两个y坐标估值，一个来自主滤波器而另—个来自道路约束，这两个值合并为如下：

${\underset{\‾}{\overset{^}{y}}}_{t_{k|k}}^f=P_{{\mathrm{yt}}_{k|k}}^f[{\left({\stackrel{\‾}{P}}_{y{t}_{k|k}}\right)}^{-1}{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}+{\left(P_{{\mathrm{yt}}_{k|k}}^1\right)}^{-1}{\underset{\‾}{\overset{^}{y}}}_{t_{k|k}}^1]---\left(29\right)$

最后，目标状态的组合估值为

${\underset{\‾}{\overset{^}{X}}}_{t_{k|k}}=\left[\begin{array}{c}{\underset{\‾}{\overset{^}{x}}}_{t_{k|k}}^1\\ {\underset{\‾}{\overset{^}{y}}}_{t_{k|k}}^f\end{array}\right]---\left(30\right)$

及

$P_{t_{k|k}}={\left[\begin{array}{cc}{P}_{x_t}& {\stackrel{\‾}{P}}_{x{y}_t}\\ {\stackrel{\‾}{P}}_{x{y}_t}^{\′}& P_{y_t}^f\end{array}\right]}_{k|k}---\left(31\right)$

其中

${\stackrel{\‾}{P}}_{{\mathrm{xy}}_t}=P_{x_t}{\left(A_k^1\right)}^t---\left(32\right)$

在步骤(530)中，如果从步骤(514)到(524)目标车辆36被判定在主车道38上运动，则这个组合估值将作为目标状态的估值的输出。

返回到步骤(514)到(524)的过程，该过程用于判定目标是否可能受到约束条件的约束，如果是，则判定最可能的约束条件；根据目标沿着相同的道路34的假设，如果目标车辆36已知在特定车道上运动，那么用该车道的估计道路参数作为估计目标运动的主滤波器中的约束条件是合理的。但是，目标车辆36当前在哪一车道的信息通常是不可得到的，尤其是当目标在弯曲的道路34上移动时。因为道路方程(8)仅适用于主中心坐标系统内的主车道38，约束滤波需要知道目标所在的车道，因此对于不同的车道需要不同的约束方程。忽略平行车道间的道路曲率参数的差异，即假设每个道路的曲率相同，则任意道路的道路方程可写成：

$y=\mathrm{mB}+{\hat{C}}_0\frac{x^2}{2}+{\hat{C}}_1\frac{x^3}{6},m=0,\±1,\±2,\·\·\·\·\·---\left(33\right)$

其中B为车道宽度，m表示将描述的车道(m＝0对应于主车道38，m＝1对应于右边邻近车道40，m＝-1对应于左边邻近车道40等等)。由于预先不了解目标车道位置，形成多约束系统(类似于所谓的多模型系统)的多个约束条件中的每一个条件都要进行检验，以判定识别哪一个约束条件有效。多约束(MC)系统属于有限数个N^C约束条件之一。在任何给定时刻仅有一个约束条件有效。这种系统称作混合系统——它们具有连续(噪声)状态变量和不连续个约束条件。

下面的定义和模型假设有助于解决这个问题：

约束方程：

${\underset{\‾}{y}}_{t_k}={\underset{\‾}{f}}_{t_k}\left({\underset{\‾}{x}}_{t_k}\right)---\left(34\right)$

其中， f _tk表示在采样周期过程的在终止时刻t_k有效的约束条件。约束条件：在可能的N^C个约束条件之中

${\underset{\‾}{f}}_{t_k}\∈\{{\underset{\‾}{f}}^j{\}}_{j=1}^{N^C}---\left(35\right)$

：用约束条件 f _tk ^j在时刻t_k的状态估值P_ytk|k ^j， P_xytk|k ^j：在约束条件f_tk ^j下的时刻t_k的协方差矩阵μ_tk-1 ^j：在t_k-1时刻目标满足约束条件j的概率约束条件转移过程：是一个具有已知转移概率的马尔可夫链

$P\{{\underset{\‾}{f}}_{t_k}={\underset{\‾}{f}}^j|{\underset{\‾}{f}}_{t_{k-1}}={\underset{\‾}{f}}^i\}=p_{\mathrm{ij}}---\left(36\right)$

为了实现马尔可夫模型——对于包含超过一个可能约束状态的系统——假设在每一扫描时刻目标会从约束状态i转换到约束状态j的概率为p_ij。假设这些概率为已知的先验概率(a priori)，且可用如下所示的概率转换矩阵表示：

                           新状态

f ^j正确( f ^j有效)的先验概率为

$P\left({\underset{\‾}{f}}^j\right|Z^0)={\mathrm{\μ}}_{t_0}^j---j=1,\·\·\·,N^C---\left(38\right)$

其中Z⁰为先验信息及

$\underset{j=1}{\overset{N^C}{\mathrm{\Σ}}}{\mathrm{\μ}}_{t_0}^j=1---\left(39\right)$

因为正确约束条件在假定的N^C个可能约束条件中。

约束目标状态估计子系统46用来判定目标状态是否对应可能的约束状态，如果是，然后用来判定最可能的约束状态。

判定的方法之一是由Bar-Shalom提出的多模型滤波算法，其中，N^C个并联滤波器同时并行工作。

另一种方法中，多约束(MC)估计算法采用无约束状态估值 作为测量值，混合并更新N^C个约束条件的状态估值，同时计算与每个约束条件有关的似然性函数及概率。在多约束(MC)估计算法的一个实施例中，约束状态估值输出是所有约束条件的状态估值的复合。如果约束状态估值有效，即如果约束状态估值对应无约束状态估值，那么目标状态通过合并约束和无约束状态估值给定，否则目标状态由无约束状态估值给定。多约束(MC)估计算法的实施例包括如下步骤：

1.根据多约束条件的状态变量的估计：在步骤(514)中，采用多车道道路方程(33)替换方程(24)中的第一行，多约束状态估值由下式给定：

${\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^{0j}={\left[\begin{array}{c}\stackrel{\‾}{y}\\ \stackrel{\‾}{\stackrel{\·}{y}}\\ \stackrel{\‾}{\stackrel{\·\·}{y}}\end{array}\right]}_{t_{k|k}}=\left[\begin{array}{c}{B}_j+{\hat{C}}_0\·{\left({\hat{x}}^1\right)}^2/2+{\hat{C}}_1\·{\left({\hat{x}}^1\right)}^3/6\\ {\hat{C}}_0\·{\hat{x}}^1\·{\widehat{\stackrel{\·}{x}}}^1+{\hat{C}}_1\·{\left({\hat{x}}^1\right)}^2\·\widehat{\stackrel{\·}{x}/2}\\ {\hat{C}}_0\·{\left({\widehat{\stackrel{\·}{x}}}^1\right)}^2+{\hat{C}}_0\·{\hat{x}}^1\·{\widehat{\stackrel{\·\·}{x}}}^1+{\hat{C}}_1\·{\hat{x}}^1\·{\left({\widehat{\stackrel{\·}{x}}}^1\right)}^2+{\hat{C}}_1\·{\left({\hat{x}}^1\right)}^2\·{\widehat{\stackrel{\·\·}{x}}}^1/2\end{array}\right]---\left(40\right)$

其中， $B_j=0,\±,...,\±\frac{N^C-1}{2}-B,$ 而B为车道宽度。换言之，约束状态估值对应于目标车辆36所处的每一个可能车道的中心线的y位置。

相关协方差由下式给定：

${\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{0j}=A_k^1\·P_{{\mathrm{xt}}_{k|k}}\·{\left(A_k^1\right)}^T+A_k^2\·P_{k|k}^C\·{\left(A_k^2\right)}^T---\left(41\right)$

其中A_k ¹和A_k ²由方程(26)和方程(27)给定，P_xtk|k来自方程(23)而P_k|k ^C来自曲率滤波器。

2.以约束条件为条件的更新：在步骤(516)中，以有效约束条件为条件的状态估值和协方差被更新，以及每一个约束条件j＝1，...N^C的约束条件似然性函数也被更新。对应约束条件j的更新状态估值和协方差采用测量值

得到，如下：

${\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^j={\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^{0j}+{\stackrel{\‾}{P}}_{y{t}_{k|k}}^{0j}{({\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{0j}+P_{{\mathrm{yt}}_{k|k}}^1)}^{-1}\left({\underset{\‾}{\overset{^}{y}}}_{t_{k|k}}\text{-}{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^{0j}\right)---\left(42\right)$

${\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^j={\stackrel{\‾}{P}}_{y{t}_{k|k}}^{0j}-{\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{0j}{({\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{0j}+P_{y{t}_{k|k}}^1)}^{-1}{\stackrel{\‾}{P}}_{y{t}_{k|k}}^{0j}---\left(43\right)$

${\stackrel{\‾}{P}}_{{\mathrm{xyt}}_{k|k}}^j={\stackrel{\‾}{P}}_{x{\mathrm{yt}}_{k|k}}^{0j}-{\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{0j}{({\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{0j}+P_{{\mathrm{yt}}_{k|k}}^1)}^{-1}{\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{0j}---\left(44\right)$

3.似然性计算：在步骤(518)中，对应约束条件j的似然性函数以无约束目标状态估值的 y _tk|k ¹来估算，假设每一个约束条件j＝1，...N^C的约束条件状态估值周围的测量值为高斯分布，如下：

${\mathrm{\Λ}}_{t_k}^j=N({\underset{\‾}{y}}_{t_{k|k}}^1;{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^{0j},{\stackrel{\‾}{P}}_{y{t}_{k|k}}^{0j}+P_{y{t}_{k|k}}^1)---\left(45\right)$

其中，高斯分布N(；，)具有均值 及相关协方差 P_ytk|k ^0j+P_ytk|k ¹。

4.约束条件概率估算：在步骤(520)中，就每一个约束条件j＝1，...N^C计算更新的约束概率，如下：

${\mathrm{\μ}}_{t_k}^j=\frac{1}{a}{\mathrm{\Λ}}_{t_k}^j{\stackrel{\‾}{a}}_j---\left(46\right)$

其中， a_j为约束条件j有效时变换后的概率，由下式给定：

${\stackrel{\‾}{a}}_j=\underset{i=1}{\overset{N^C}{\mathrm{\Σ}}}{p}_{\mathrm{ij}}\·{\mathrm{\μ}}_{t_{k-1}}^i---\left(47\right)$

及标准化常数为：

$a=\underset{j=1}{\overset{N^C}{\mathrm{\Σ}}}{\mathrm{\Λ}}_{t_k}^j{\stackrel{\‾}{a}}_j---\left(48\right)$

5.整体状态估值和协方差：在步骤(522)中，最新的约束条件状态估值和协方差的组合由下式给定：

${\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}=\underset{j=1}{\overset{N^C}{\mathrm{\Σ}}}{\mathrm{\μ}}_{t_{k|k}}^j\·{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^j---\left(49\right)$

${\stackrel{\‾}{P}}_{y{t}_{k|k}}=\underset{j=1}{\overset{N^C}{\mathrm{\Σ}}}{\mathrm{\μ}}_{t_k}^j\·[{\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^j+({\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^j-{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}})\·{({\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^j-{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}})}^{\′}]---\left(50\right)$

${\stackrel{\‾}{P}}_{\mathrm{xy}{t}_{k|k}}=\underset{j=1}{\overset{N^C}{\mathrm{\Σ}}}{\mathrm{\μ}}_{t_k}^j\·{\stackrel{\‾}{P}}_{\mathrm{xy}{t}_{k|k}}^j---\left(51\right)$

在上述算法中步骤(522)的估计器输出用作方程(28)和(29)中所述的合并过程的约束估值，而方程(51)的结果取代方程(32)的结果用于方程(31)中。

当目标车辆36不沿着道路34或改变车道时，强加于目标运动状态变量的道路约束将产生不正确的估计，它可能比采用相关的无约束估值更差。但是，噪声相关的估计误差可能导致正确的道路约束变得无效。因此，将两种情况合并的措施是有利的，即当它们有效，如当目标车辆36沿着特定车道时使约束条件保持有效；而当它们无效，如当目标车辆36偏离车道时，迅速将它们移走。无约束目标状态估值在道路约束有效上起很大作用，因为它提供了独立的目标状态估值。

一种方法就是检验以下的假设：无约束目标状态估值满足道路约束方程，或等价于约束估值和无约束估计各自对应同一目标。最佳检验要求采用到时刻t_k过程中所有有效目标状态估值，这通常是不切实际的。实际方法是顺序假设检验，即只根据最近状态估值来进行检验。根据上文所用的符号，有约束和无约束目标状态估值间的差值(仅y方向)表示为：

${\underset{\‾}{\overset{^}{\mathrm{\δ}}}}_{t_k}={\underset{\‾}{\overset{^}{y}}}_{t_{k|k}}^1-{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}---\left(52\right)$

作为如下的估值

${\underset{\‾}{\mathrm{\δ}}}_{t_k}={\underset{\‾}{y}}_{t_k}^1-{\underset{\‾}{\overset{\‾}{y}}}_{t_k}---\left(53\right)$

其中 y _tk ¹为实际目标状态且 y _tk为沿着道路34(或车道)移动的目标的真实状态。在步骤(524)中，“同一目标”假设被检验，即

$H_0:{\underset{\‾}{\mathrm{\δ}}}_{t_k}=0---\left(54\right)$

对

$H_1:{\underset{\‾}{\mathrm{\δ}}}_{t_k}\≠0---\left(55\right)$

主滤波器误差

${\underset{\‾}{\overset{~}{y}}}_{t_k}^1={\underset{\‾}{y}}_{t_k}^1-{\underset{\‾}{\overset{^}{y}}}_{t_{k|k}}^1---\left(56\right)$

假设独立于误差

${\underset{\‾}{\overset{~}{\stackrel{\‾}{y}}}}_{t_k}={\underset{\‾}{\overset{\‾}{y}}}_{t_k}-{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}---\left(57\right)$

其是来自约束条件的。在假设H₀的前提下，差值

的协方差由下式给定：

$\begin{array}{c}{P}_{t_k}^{\mathrm{\δ}}=E\left({\underset{\‾}{\overset{~}{\mathrm{\δ}}}}_{t_k}{\underset{\‾}{\overset{~}{\mathrm{\δ}}}}_{t_k}^t\right)=E\left[({\underset{\‾}{\overset{~}{y}}}_{t_k}^1-{\underset{\‾}{\overset{~}{\stackrel{\‾}{y}}}}_{t_k}){({\underset{\‾}{\overset{~}{y}}}_{t_k}^1-{\underset{\‾}{\overset{~}{y}}}_{t_k}^1)}^{\′}\right]\\ =P_{{\mathrm{yt}}_{k|k}}^1+{\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}\end{array}---\left(58\right)$

假设估计误差为高斯型，H₀对H₁的检验如下：如果

${\mathrm{\ρ}}_{t_k}={\underset{\‾}{\overset{^}{\mathrm{\δ}}}}_{t_k}^t{\left(P_{t_k}^{\mathrm{\δ}}\right)}^{-1}{\underset{\‾}{\overset{^}{\mathrm{\δ}}}}_{t_k}\≤\mathrm{\γ}---\left(59\right)$

接受H₀选择的门限使得

$P({\mathrm{\ρ}}_{t_k}>\mathrm{\γ}|H_0)=\mathrm{\α}---\left(60\right)$

其中α为预定义误差容许值。注意，基于上述高斯误差假设，ρ_tk是自由度为n_y的卡方(chi-square)分布。门限的选择是很重要的设计因素，应该基于特殊的应用需要。在道路上的车辆碰撞预测中，注意到主车道38上的目标处在碰撞路线，认为它比邻近车道40之一的目标更危险。因此，对于主车道38上的目标希望具有高门限(低误差容许值)，因为约束滤波可提供精确的目标状态估计，同时这个目标的“改变车道”操纵不会对主车辆12构成威胁。另一方面，邻近车道40上的目标通常认为是从旁边经过的车辆。虽然约束滤波可进一步减少错误报警率，但是这个目标“改变车道”操纵(进入主车道38)会对主车辆12构成实际的威胁。因此，如果错误报警速率已经足够低，对于邻近车道的目标，希望具有低门限(高误差容许值)。

根据上述分析，假设检验方案对不同车道上的目标有效采用不同的门限值，多约束滤波算法提供目标当前最可能所处车道的信息。假设在道路34上有N^C个可能的车道，每一个车道都用一个约束方程来描述，对于目标的具有最大概率μ_tk ^j的约束方程对应于t_k时刻(当前时刻)该目标最可能位于的车道。用l_t来表示这个最可能的车道，则

${\mathrm{\μ}}_{t_k}^{l_t}=\underset{j}{\max }\{{\mathrm{\μ}}_{t_k}^j,j=1,\·\·\·,r\}---\left(61\right)$

无约束状态估值和车道l_t约束状态估值间的差值(仅y方向)表示为：

${\widehat{\underset{\‾}{\mathrm{\δ}}}}_{t_k}^{l_t}={\widehat{\underset{\‾}{y}}}_{t_{k|k}}^1-{\widehat{\stackrel{\‾}{\underset{\‾}{y}}}}_{t_{k|k}}^{l_t}---\left(62\right)$

是如下的估值：

${\underset{\‾}{\mathrm{\δ}}}_{t_k}^{l_k}={\underset{\‾}{y}}_{t_k}^1-{\underset{\‾}{\overset{\‾}{y}}}_{t_k}^{l_t}---\left(63\right)$

其中 y _tk ¹为实际目标状态而 y _tk ^l1为沿着车道l_t移动的目标的实际状态。

对于“同一目标”假设的检验由下式给定：

$H_0:{\underset{\‾}{\mathrm{\δ}}}_{t_k}^{l_t}=0---\left(64\right)$

对

$H_1:{\underset{\‾}{\mathrm{\δ}}}_{t_k}^{l_t}\≠0---\left(65\right)$

约束估计误差由下式给定：

${\widetilde{\stackrel{\‾}{\underset{\‾}{y}}}}_{t_k}^{l_t}={\underset{\‾}{\overset{\‾}{y}}}_{t_k}-{\underset{\‾}{\overset{^}{\stackrel{\‾}{y}}}}_{t_{k|k}}^{l_t}---\left(66\right)$

假设估计误差是独立的且为高斯型，H₀：H₁的检验变成：如果

${\mathrm{\ρ}}_{t_k}^{l_t}={\left({\underset{\‾}{\overset{^}{\mathrm{\δ}}}}_{t_k}^{l_t}\right)}^{\′}{\left(P_{t_k}^{{\mathrm{\δ}}_l}\right)}^{-1}{\underset{\‾}{\overset{^}{\mathrm{\δ}}}}_{t_k}^{l_t}\≤{\mathrm{\γ}}_{l_t}---\left(67\right)$

则接受H₀其中

$\begin{array}{c}{P}_{t_k}^{{\mathrm{\δ}}_t}=E\left[({\underset{\‾}{\mathrm{\δ}}}_{t_k}^{l_t}-{\underset{\‾}{\overset{^}{\mathrm{\δ}}}}_{t_k}^{l_t}){({\underset{\‾}{\mathrm{\δ}}}_{t_k}^{l_t}-{\underset{\‾}{\overset{^}{\mathrm{\δ}}}}_{t_k}^{l_t})}^{\′}\right]=E\left[({\underset{\‾}{\overset{~}{y}}}_{t_k}^1-{\underset{\‾}{\overset{~}{\stackrel{\‾}{y}}}}_{t_k}^{l_t}){({\underset{\‾}{\overset{~}{y}}}_{t_k}^1-{\underset{\‾}{\overset{~}{\stackrel{\‾}{y}}}}_{t_k}^{l_t})}^{\′}\right]\\ =P_{{\mathrm{yt}}_{k|k}}^1+{\stackrel{\‾}{P}}_{{\mathrm{yt}}_{k|k}}^{l_t}\end{array}---\left(68\right)$

且门限使得：

$P({\mathrm{\ρ}}_{t_k}^{l_t}>{\mathrm{\γ}}_{l_t}|H_0,l_t)={\mathrm{\α}}_{l_t}---\left(69\right)$

其中

${\mathrm{\γ}}_{l_t}\∈\{{\mathrm{\γ}}_j{\}}_{j=1}^r$ 且 ${\mathrm{\α}}_{l_t}\∈\{{\mathrm{\α}}_j{\}}_{j=1}^r---\left(70\right)$

当目标车辆36离开其当前车道时，这种车道自适应假设检验方案提供将目标状态估计输出迅速转换到无约束估值，同时主车道38上的目标的估计精度通过约束滤波得到充分改善。

在多约束(MC)估计算法的另一实施例中，用于假设检验的约束状态估值是分散的有约束目标状态估值中的最可能的情况(即根据“胜者全得”战略)，而不是所有的有约束目标状态估值的复合组合。如果最可能的有约束状态估计有效，即如果最可能的有约束状态估值对应无约束状态估计，则目标状态通过将最可能的有约束状态估计和无约束状态估计合并给定；否则目标状态由无约束状态估计给定。

然而在多约束(MC)估计算法的又一实施例中，要对每一个有约束状态估值进行假设检验。如果没有一个假设满足，则目标状态由无约束状态估值给定。如果有一个假设满足，则目标状态通过将对应的有约束状态估值和无约束状态估值合并给定。如果不只一个假设满足，则通过对多个方法的表决结果或用不同的相关门限重复假设检验来识别最可能的有约束状态。

通常，约束条件的个数(即道路车道的个数)随时间而改变，此外相关参数如道路的车道宽度也会改变，以适应主车道12环境的变化。例如，一个旅途中的主车辆12可在一车道公路、具有反向通行的两车道公路、具有中心转向车道的三车道公路、两个车道反向通行的四车道公路或多车道分开的高速公路上行进。

对四种情况进行采用有约束和无约束滤波的道路车辆跟踪仿真。在所有情况中，主车道12以15.5m/s速度移动，目标车辆36以15.5m/s在相同的道路34上逼近。在x方向上，目标的初始位置离主车辆125m，假设所有车道的车道宽为3.6m。车辆速度传感器的测量偏差为0.02m/s，陀螺横摆率测量偏差为0.0063rad/s。雷达范围、范围速率和方位角测量偏差分别为0.5m、1m/s及1.5°。然后用100个蒙特卡罗法(Monte-Carlo)分布的相关跟踪滤波器产生仿真结果。

在第一种情况，主车辆12和目标车辆36在直线道路34(C₀＝0及C₁＝1)上移动，且目标车辆36在相同的车道上朝着主车车辆12移动。图8a-d显示了无约束和有约束滤波方案的目标状态估计和道路曲线估计结果，图9a-b显示了有约束和无约束滤波方案的平均目标车辆36的横向位置、速率和加速度RMS误差。有约束滤波的估计误差充分减小。在48次雷达扫描前，当目标车辆36距离主车辆12大于65m时，有约束滤波使得目标横向速率估计的误差减少量大于40％，横向加速度估计的误差减少量超过60％。当目标车辆36距离主车辆12小于65米时，这个条件与碰撞预测更为相关，通过有约束滤波，大于50％的横向位置估计误差和大于90％的横向速率和加速度估计误差减少了。

在第二种情况，主车辆12和目标车辆36在弯曲道路34(C₀＝-10^-5及C₁＝-3×10^-5)上移动，且目标车辆36在同一车道上朝着主车辆12移动。图10a-d显示了无约束和有约束滤波方案的目标状态估计和曲率估计结果，图11a-b显示了有约束和无约束滤波方案的平均目标车辆36横向位置、速率和加速度RMS误差。当目标车辆36距离主车辆12小于65米时，有约束滤波的估计误差在大约48次雷达扫描后充分地减小了。当目标车辆36距离主车辆12大约100m时，在20次雷达扫描前，有约束和无约束滤波的估计误差相同。目标车辆36所在位置与主车辆12的距离在100m和65m之间时，有约束滤波导致横向速率和加速度估计的误差减少了大约30％，且当目标车辆36距离主车辆12小于65m时，通过有约束滤波使得横向位置估计误差减少量大于50％，横向速率和加速度估计误差减少量大于90％。当目标车辆36很远时，有约束滤波的改善有限由道路曲率参数的估计误差致使，它使得约束误差与主车辆12和目标车辆36之间的距离成比例。这在弯曲道路34的情况更为明显，其中曲率估计误差变得更大，且使得远距离目标车辆36的车道位置更不明确。

在第三种情况，主车辆12和目标车辆36在直线道路34(C₀＝0及C₁＝0)上移动，目标车辆36最初在左边邻近车道上逼近。在t＝2.2s(55个雷达扫描)，目标车辆36开始脱离它的车道，转向主车道38，这导致在t＝4s(100次雷达扫描)时发生碰撞。图12a-d显示了有约束和无约束滤波方案的目标状态估计结果及横向位置和速率RMS误差。约束条件有效性假设检验的误差容限水平(方程(69))，对于主车道38选择α1，对于所有邻近车道40选择α＝0.5。尽管在目标车辆36转向前，未验证的有约束滤波充分降低了估计误差，但是在目标车辆36开始远离它的车道(左边邻近车道)后，相关的目标状态估计结果是不正确的，它的RMS误差要比无约束滤波的误差更大，意味着没有即时移走目标车辆36开始脱离它的车道后变成无效的道路约束。另一方面，经验证的有约束滤波的性能非常接近于无约束滤波的性能，在目标36转向前略微减小了估计误差，且具有的目标状态估计结果和RMS误差与在目标车辆36开始从它的车道转向后的无约束滤波相同，意味着在目标车辆36开始脱离它的车道后，道路约束被即时移走了。

第四种情况类似于第三种情况，唯一的差别是车辆在弯曲道路34(C₀＝-10^-5及C₁＝-3×10^-5)而不是直线道路。目标车辆36在t＝2.2s时开始脱离车道，在t＝4s时发生碰撞。图13a-d显示了无约束和有约束滤波方案的目标状态估计结果及横向位置和速率RMS误差。误差容限水平与第三种情况相同，且结果和观察数据与第三种情况相同。在目标36开始脱离车道后，通过所提出的约束条件验证，道路约束被即时移走。通常，假设目标车辆36横向运动的估计精度通常受到很差的雷达角度分辨率的限制，有约束滤波在目标车辆36的横向运动的估计精度上的整体改善很大。

因此，在直线和弯曲道路34上的道路车辆跟踪的仿真结果表明：当目标车辆36在主车道38上时，预测碰撞传感系统10可充分减小目标车辆36横向运动的估计误差。当目标车辆36从邻近车道转到主车道38上时，预测碰撞传感系统10即时探测到这个动作，并移走道路约束，以避免另外的错误约束结果。由于很差的雷达角度分辨率通常会引起很差的横向运动估计，预测碰撞传感系统10提供了对目标车辆36横向运动的估计精度的极大改善，这有利于早期可靠的道路车辆碰撞预测。

虽然特定的实施例在上述详细说明中进行了详细解释并在附图中显示，本领域的技术人员应当明白，各种细节上的修正及替换都可根据本发明的总体教导发展得到。因此，所公开的特定布置仅作为例证说明且不作为对本发明的范围的限制，本发明的范围在所附权利要求及其等同物的整个范围中给出。

                 附录—卡尔曼滤波器的说明

卡尔曼滤波器用来从一组有噪测量值中估计受噪声影响的动态系统的状态和相关测量值。

系统动力学关系由下式定义：

x _k+1＝F_k· x _k+ w _k， w _k～N(0，Q_k)          (A-1)

其中 x _k为系统状态向量，F_k为系统矩阵而 w _k为对应每一个状态变量的噪声变量的相关向量，每一个噪声变量的平均值都为0，方差由相关方差向量Q_k的对应元素给定。相关系统测量值的动力学关系由下式给定：

z _k＝H_k· x _k+ v _k， v _k～N(0，R_k)               (A-2)

其中 z _k为系统测量值向量，H_k为测量值矩阵而 v _k为对应每一个测量变量的噪声变量的相关向量，每一个噪声变量的均值为0，方差由相关方差向量R_k的对应元素给定。相关协方差矩阵R_k的元素值可根据对相关典型的工作条件组的相关系统的典型测量值的分析来决定先验值。相关协方差矩阵Q_k的元素值解释了模型误差。通常，相关矩阵F_k、Q_k、H_k、R_k可随时间改变。

假设在k时刻的测量值 z _k及状态 x _k-1|k-1的初始值和k-1时刻的相关协方差P_k-1|k-1，卡尔曼滤波器用来估计k时刻的相关状态 x _k|k及相关协方差P_k|k。

滤波过程的第一步是根据k-1时刻的估值来计算k时刻的状态x _k|k-1的估值和相关协方差P_k|k-1，如下：

x _k|k-1＝F_k· x _k-1|k-1                          (A-3)

$P_{k|k-1}=F_k\·P_{k-1|k-1}\·F_k^T+Q_k---(A-4)$

下一步是预计k时刻的测量值 和相关协方差矩阵S_k，如下：

${\widehat{\underset{\‾}{z}}}_k=H_k\·{\underset{\‾}{x}}_{k|k-1}---(A-5)$

$S_k=\mathrm{cov}\left({\underset{\‾}{\hat{z}}}_k\right)=H_k\·P_{k|k-1}\·H_k^T+R_k---(A-6)$

下一步是计算用于更新状态向量 x _k|k和相关协方差矩阵P_k|k的增益矩阵G_k，如下：

$G_k=P_{k|k-1}\·H_k^T\·S_k^{-1}---(A-7)$

最后，估计k时刻的状态向量 x _k|k和相关协方差矩阵P_k|k，它对应于相关测量值 z _k，如下：

${\underset{\‾}{x}}_{k|k}={\underset{\‾}{x}}_{k|k-1}+G_k\·({\underset{\‾}{z}}_k-{\underset{\‾}{\hat{z}}}_k)---(A-8)$

$P_{k|k}=P_{k|k-1}-G_k\·S_k\·G_k^T---(A-9)$

Claims (37)

1.一种估计道路曲率的方法，包括：

a.通过测量道路上行进的主车辆的纵向速度来产生速度测量值；

b.通过测量或确定所述主车辆的横摆率来产生横摆率测量值；及

c.估计至少一个曲率参数，其中所述的至少一个曲率参数表示道路曲率，估计至少一个曲率参数的操作对应于所述的速度测量值和所述的横摆率测量值。

2.如权利要求1所述的估计道路曲率的方法，其中，所述的至少一个曲率参数包含曲率的回旋曲线模型的至少一个参数，所述曲率的回旋曲线模型以所述主车辆的坐标系统为参考。

3.如权利要求2所述的估计道路曲率的方法，其中，所述的至少一个曲率参数包含回旋曲线模型的第一和第二参数，其中所述的第一参数是常数，且所述的第二参数是对沿着道路的距离的曲率的灵敏度。

4.如权利要求1所述的估计道路曲率的方法，其中，估计至少一个曲率参数的操作包括用至少一个卡尔曼滤波器来处理所述的速度和角速度的测量值。

5.如权利要求1所述的估计道路曲率的方法，其中，所述的至少一个卡尔曼滤波器包括第一和第二卡尔曼滤波器，所述第一卡尔曼滤波器适于根据所述速度测量值和所述横摆率测量值来估计第一组状态变量，而第二卡尔曼滤波器适于根据所述第一组状态变量的所述估值来估计所述至少一个曲率参数。

6.如权利要求5所述的估计道路曲率的方法，其中，所述的第一组状态变量包括车辆速率、车辆加速度、车辆横摆率和车辆横摆加速度。

7.一种估计道路上的目标车辆的状态的方法，包括：

a.产生道路曲率的估值；

b.估计目标车辆的无约束状态及其相关的协方差；

c.建立目标车辆的至少一个预期约束条件，其中至少一个预期约束条件对应于所述的道路曲率的估值；

d.估计目标车辆的至少一个有约束状态及其相关的协方差，它对应于目标车辆的所述至少一个预期约束条件；

e.确定目标车辆的最可能状态，其中所述的目标车辆的最可能状态从所述的目标车辆的无约束状态和目标车辆的所述至少一个有约束状态中选择；及

f.如果目标车辆的所述至少一个有约束状态是最可能状态，则将目标车辆的无约束状态及其协方差和所述最可能状态的相关有约束状态及协方差合并，并输出目标的至少一个合并状态及其相关的合并协方差，作为目标的估计状态或协方差；否则输出目标的至少一个无约束状态及其相关的无约束协方差，作为目标的估计状态或协方差。

8.如权利要求7所述的估计道路上的目标车辆的状态的方法，其中，估计道路曲率的操作包括：

a.通过测量在道路上行进的主车辆的纵向速度，产生速度测量值；

b.通过测量或确定所述主车辆的横摆率，产生横摆率测量值；及

c.估计至少一个曲率参数，其中，所述的至少一个曲率参数表示道路的曲率，且估计至少一个曲率参数的操作对应于所述速度测量值和对应于所述的横摆率测量值。

9.如权利要求8所述的估计道路上的目标车辆的状态的方法，其中，所述至少一个曲率参数包括曲率的回旋曲线模型的至少一个参数，且所述的曲率的回旋曲线模型是以所述主车辆的坐标系统为参考的。

10.如权利要求9所述的估计道路上的目标车辆的状态的方法，其中，所述的至少一个曲率参数包括回旋曲线模型的第一和第二参数，其中，所述第一参数为常数，且所述第二参数是沿着道路距离曲率的灵敏度。

11.如权利要求8所述的估计道路上的目标车辆的状态的方法，其中，估计至少一个曲率参数的操作包括用至少一个卡尔曼滤波器来处理所述的速度和横摆率的测量值。

12.如权利要求11所述的估计道路上的目标车辆的状态的方法，其中，所述的至少一个卡尔曼滤波器包括第一和第二卡尔曼滤波器，所述第一卡尔曼滤波器适于根据所述速度测量值和所述横摆率测量值来估计第一组状态变量，且所述第二卡尔曼滤波器适于根据所述第一组状态变量的所述估值来估计所述的至少一个曲率参数。

13.如权利要求12所述的估计道路上的目标车辆的状态的方法，其中，所述第一组状态变量包括车辆速率、车辆加速度、车辆横摆率及车辆横摆加速度。

14.如权利要求7所述的估计道路上的目标车辆的状态的方法，其中，估计目标车辆的无约束状态和相关协方差的操作包括：

a.测量目标车辆相对于主车辆的范围、范围速率和方位角；及

b.根据目标车辆相对于主车辆的范围、范围速率和方位角的所述测量值来估计目标车辆的所述无约束状态。

15.如权利要求7所述的估计道路上的目标车辆的状态的方法，还包括：将目标车辆的所述无约束状态转换位对应于道路曲率的所述估值的坐标系统的操作。

16.如权利要求7所述的估计道路上的目标车辆的状态的方法，其中，目标车辆的所述至少一个预期约束条件应用于目标车辆的横向位置，且目标车辆的纵向位置是无约束的。

17.如权利要求7所述的估计道路上的目标车辆的状态的方法，其中，目标车辆的所述至少一个预期约束条件包括多个预期约束条件，且至少两个不同的预期约束条件对应于道路的不同车道。

18.如权利要求7所述的估计道路上的目标车辆的状态的方法，其中，确定目标车辆的最可能状态的操作包括：

a.确定至少一个似然值，其中所述的至少一个似然值对应于目标车辆的所述的至少一个有效预期约束条件，且所述至少一个似然值对应于相应的至少一个分布函数，该分布函数以对应于目标车辆的所述无约束状态的值来估计。

b.确定目标车辆的第一有约束状态的概率，其中，所述概率对应于所述的至少一个似然值；

c.检验至少一个假设，即目标车辆的所述无约束状态对应于所述第一有约束状态，及

d.响应于检验所述至少一个假设的操作，识别目标车辆的最可能状态。

19.如权利要求18所述的估计道路上的目标车辆的状态的方法，其中，确定目标车辆的最可能状态的操作包括：建立从目标车辆的第一状态转换到第二状态的至少一个先验概率，其中，所述的第一和第二状态可以是相同的状态或者不同的状态，且目标车辆的所述第一有约束状态的所述概率对应于所述的至少一个先验概率。

20.如权利要求18所述的估计道路上的目标车辆的状态的方法，其中，所述至少一个分布函数对应于目标车辆的特定有约束状态的所述协方差。

21.如权利要求18所述的估计道路上的目标车辆的状态的方法，其中，所述至少一个有约束状态包括多个有约束状态，所述的第一有约束状态包括所述多个有约束状态的组合。

22.如权利要求18所述的估计道路上的目标车辆的状态的方法，其中，所述第一有约束状态包括目标车辆的所述至少一个有约束状态的最可能状态。

23.如权利要求18所述的估计道路上的目标车辆的状态的方法，其中，所述第一有约束状态包括目标车辆的所述至少一个有约束状态。

24.如权利要求18所述的估计道路上的目标车辆的状态的方法，其中，检验至少一个假设的操作对应于目标车辆的所述状态的最近估值。

25.如权利要求18所述的估计道路上的目标车辆的状态的方法，其中，检验至少一个假设的操作包括：识别与目标车辆的所述至少一个有约束状态的最可能状态对应的目标车辆的特定有约束状态，计算与所述无约束状态和所述特定有约束状态间差值对应的函数的值，并将所述函数的所述值与一门限比较，由此，如果所述函数的所述值小于所述门限，则满足所述假设。

26.如权利要求25所述的估计道路上的目标车辆的状态的方法，其中，所述门限对应于与目标车辆的所述特定有约束状态有关的特定约束条件。

27.如权利要求26所述的估计道路上的目标车辆的状态的方法，其中，对于与目标车辆对主车辆的威胁增大对应的所述特定有约束状态，所述阈值相对较低。

28.如权利要求27所述的估计道路上的目标车辆的状态的方法，其中，与目标车辆对主车辆的威胁增大对应的所述特定有约束状态选自与主车辆在同一车道上移动的目标车辆及改变到与主车辆的所述车道相同的目标车辆。

29.一种用于估计道路曲率的系统，包括：

a.速度传感器，适于测量道路上车辆的纵向速度；

b.横摆率传感器，适于测量所述车辆的横摆率；及

c.与所述速度传感器和所述横摆率传感器连接的处理器，其中，所述处理器适于估计对应于来自所述速度传感器和所述横摆率传感器的测量值的道路曲率。

30.如权利要求29所述的用于估计道路曲率的系统，其中，所述处理器包括至少一个卡尔曼滤波器，且响应于对纵向速度的所述测量和对横摆率的所述测量，所述至少一个卡尔曼滤波器适于产生至少一个曲率参数的估值，其中，所述至少一个曲率参数表示道路曲率。

31.如权利要求30所述的用于估计道路曲率的系统，其中，所述至少一个卡尔曼滤波器包括第一和第二卡尔曼滤波器，响应于对纵向速度的所述所述和对横摆率的所述测量，所述第一个卡尔曼滤波器适于产生一个输出，该输出包括横摆率、横摆加速度、纵向速度和纵向加速度的估值；且响应于来自所述第一个卡尔曼滤波器的所述输出，所述第二个卡尔曼滤波器适于产生所述至少一个曲率参数的所述估值。

32.一种用于估计道路上的目标车辆的状态的系统，包括：

a.道路曲率估计子系统，用于估计在主车辆行进道路上的道路曲率；

b.目标状态估计子系统，可操作地与所述主车辆连接，其中，所述目标状态子系统适于跟踪道路上的目标车辆；及

c.至少一个处理器，可操作地与所述道路曲率估计子系统和所述目标状态估计子系统连接或者其一部分，其中，响应于所述道路曲率估计子系统所估计的所述曲率和响应于来自所述目标状态估计子系统的目标运动的测量，所述处理器适于确定目标车辆是否在道路的特定车道上行进。

33.如权利要求32所述的用于估计道路上的目标车辆的状态的系统，其中，用于估计道路曲率的所述道路曲率估计子系统包括：

a.速度传感器，适于测量道路上的所述主车辆的纵向速度；及

b.横摆率传感器，适于测量道路上的所述主车辆的横摆率，其中，所述至少一个处理器可操作地与所述速度传感器和所述横摆率传感器连接，且响应于来自所述速度传感器和所述横摆率传感器的测量值，所述处理器适于估计道路曲率。

34.如权利要求32所述的用于估计道路上的估计目标车辆的状态的系统，其中，所述目标状态估计子系统包括可操作地与所述主车辆连接的雷达系统。

35.如权利要求32所述的用于估计道路上的目标车辆的状态的系统，其中，所述处理器包括扩展卡尔曼滤波器，响应于所述目标状态估计子系统的范围、范围速率和方位角的测量，扩展卡尔曼滤波器适于估计目标运动的测量。

36.如权利要求35所述的用于估计道路上的目标车辆的状态的系统，其中，目标运动的所述测量包括相对于主车辆的坐标系统的目标位置的测量。

37.如权利要求36所述的用于估计道路上的目标车辆的状态的系统，其中，目标运动的所述测量还包括相对于主车辆的所述坐标系统的目标速度的测量。

Images