车辆前方目标物的探测方法

1、车辆前方目标物的探测方法，其特征在于，它依次含有由车辆处理器控制执行的以下步骤：
1)初始化：
给定雷达测量系统距离测量噪声方差σr 2、角度测量噪声方差σθ 2、系统噪声方差Qu、系统 状态向量初值X0e、系统状态估计误差协方差阵初值P0e；
2)测量目标物距离Dn和方位角An，其中，下标n表示测量和计算的次数，其初值为1； 现时测量值以相应量加下标n表示；
3)计算目标物在本车行驶方向，即x方向上距离的现时观测值Zn：
Zn＝DncosAn
4)计算本次x向距离测量的噪声方差：
${\sigma }_x^2={\sigma }_r^2{·\cos }^2{A}_n+{D_n}^2·{\sigma }_{\theta }^2{\sin }^2{A}_n$
5)按下述四阶卡尔曼滤波递推公式进行滤波计算：
$X_{\mathrm{ny}}=F·X_{(n-1)e}$
$P_{\mathrm{ny}}=F·P_{(n-1)e}·F^{\prime }+G·Q_u·G^{\prime }$
$K_n=P_{\mathrm{ny}}·H^{\prime }·{(H·P_{\mathrm{ny}}·H^{\prime }+{\sigma }_x^2)}^{-1}$
Pne＝(I-Kn·H)·Pny
Xne＝Xny+Kn·(Zn-H·Xny)
其中， $X_{\mathrm{ne}}^{\prime }=[x_n,{\stackrel{·}{x}}_n,{\stackrel{··}{x}}_n,{\stackrel{···}{x}}_n]$ 表示当前目标物信息的动态最优估计，是矩阵转置符号，xn表 示目标物x向距离值的最优估计， 表示车辆与目标物间相对速度的最优估计；Xny为目标 物信息的一步预报估计，F为系统矩阵：
$F=\left[\begin{array}{cccc}1& T& T^2/2& T^3/6\\ 0& 1& T& T^2/2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$
T为系统采样时间；Pne是当前系统状态估计误差协方差阵，G是系统误差影响矩阵，
$G=\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right]$
Qu是系统噪声方差；Pny为系统状态一步预报估计误差协方差阵；Kn为卡尔曼滤波增益矩阵； H为观测矩阵，且H＝[1 0 0 0]；I为单位阵；
6)储存Pne、Xne的值，用于继续的递推计算和输出；储存Pny、Xny的值，用于测量连续 性判断计算；
7)测量连续性的判断：
7.1)计算观测值的一步预报估计Zny：
Zny＝H·Xny
7.2)与Xny相应的方差矩阵S：
$S=H·P_{\mathrm{ny}}·H^{\prime }+{\sigma }_x^2$
7.3)定义观测因子：d2＝v′·S-1·v
其中：v＝Zn-Zny
7.4)依据d2值判断最新测量值与上次测量值来自同一目标的概率是否等于或超过阈值， 如等于或超过，则认为通过滤波估计获得的目标物信息是准确的，则输出本次距离、相对速 度和方位角，并返回第2)步进行下一次测量；
如小于该阈值，则表明雷达对目标物测量信息丢失，或测量值来自另一个目标，需要进 一步判断小于上述阈值的次数是否连续超过预定值e：
若未超过预定值e，则假设在目标物测量信息丢失的这一段时间内，目标物运动状态维 持不变，按照卡尔曼滤波假设，得到目标物状态的计算方法如下式：
$x_n=x_{(n-1)}+{\stackrel{·}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{··}{x}}_{(n-1)}{T}^2+\frac{1}{6}{\stackrel{···}{x}}_{(n-1)}{T}^3$
${\stackrel{·}{x}}_n={\stackrel{·}{x}}_{(n-1)}+{\stackrel{··}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{···}{x}}_{(n-1)}{T}^2$
${\stackrel{··}{x}}_n={\stackrel{··}{x}}_{(n-1)}+{\stackrel{···}{x}}_{(n-1)}T$
${\stackrel{···}{x}}_n={\stackrel{···}{x}}_{(n-1)}$
输出当前目标物在x方向的距离的最优估计xn，相对速度 的最优估计，以及方位角An； 返回第2)步进行下一次测量；
若超过预定值e，则认为有新的目标物出现，将最新的测量目标物设为跟踪目标物，其 测量信息作为目标物的状态初值，即最新目标物x向距离的测量值作为Xne中xn的值，Xne中 其他状态值设为0，将Pne设为单位阵，返回第2)步开始对新的目标物进行跟踪测量。
2、如权利要求1所述的车辆前方目标物的探测方法，其特征在于，上述第7.4)步中，所述 依据d2值判断最新测量值与上次测量值来自同一目标的概率值的阈值为95％，即依据式 d2≤3.841判断最新测量值与上次测量值是否来自同一目标。

技术领域\n车辆前方目标物的探测方法属于车辆行车信息感知与处理技术领域。\n背景技术\n车辆行车信息感知及处理技术是现代汽车控制与安全的关键技术之一，现有测量技术按 其应用介质分为微波测量系统和激光测量系统两类，激光雷达系统因测量速度快、精度高、 测程远而受到广泛的重视。文献1(屠大维：“用于车辆防撞控制的行车环境传感研究”，中 国机械工程，第10卷第6期，1999年6月)中介绍了一种光学扫描式激光测距雷达的设计， 可以实现较宽范围的扫描测距，该系统能够测得前方目标物的距离和方位角信息，测量误差 未得到有效处理，得到的相对速度波动极大，实际中不能使用。\n发明内容\n本发明的目的在于，针对现有技术的不足提出一种车辆前方目标物的探测方法，该方法 将获得的目标物距离及方位角信息进行滤波处理，能够得到更平滑精确的目标物距离和相对 速度信息，并进一步提出了目标的测量连续性判断方法，能够判断所获得的连续测量数据是 否来自同一目标物。\n本发明所提出车辆前方目标物的探测方法，其特征在于，它依次含有由车辆处理器控制 执行的以下步骤：\n1)初始化：\n给定雷达测量系统距离测量噪声方差σr 2、角度测量噪声方差σθ 2、系统噪声方差Qu、系统 状态向量初值X0e、系统状态估计误差协方差阵初值P0e；\n2)测量目标物距离Dn和方位角An，其中，下标n表示测量和计算的次数，其初值为1； 现时测量值以相应量加下标n表示；\n3)计算目标物在本车行驶方向，即x方向上距离的现时观测值Zn：\nZn＝DncosAn\n4)计算本次x向距离测量的噪声方差：\n${\sigma }_x^2={\sigma }_r^2·{\cos }^2{A}_n+{D_n}^2·{\sigma }_{\theta }^2{\sin }^2{A}_n$\n5)按下述四阶卡尔曼滤波递推公式进行滤波计算：\nXny＝F·X(n-1)e\nPny＝F·P(n-1)e·F′+G·Qu·G′\n$K_n=P_{\mathrm{ny}}·H^{\prime }·{(H·P_{\mathrm{ny}}·H^{\prime }+{\sigma }_x^2)}^{-1}$\nPne＝(I-Kn·H)·Pny\nXne＝Xny+Kn·(Zn-H·Xny)\n其中， $X_{\mathrm{ne}}^{\prime }=[x_n,{\stackrel{·}{x}}_n,{\stackrel{··}{x}}_n,{\stackrel{···}{x}}_n]$ 表示当前目标物信息的动态最优估计，′是矩阵转置符号，xn表示 目标物x向距离值的最优估计， 表示车辆与目标物间相对速度的最优估计；Xny为目标物信 息的一步预报估计，F为系统矩阵：\n$F=\left[\begin{array}{cccc}1& T& T^2/2& T^3/6\\ 0& 1& T& T^2/2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$\nT为系统采样时间；Pne是当前系统状态估计误差协方差阵，G是系统误差影响矩阵，\n$G=\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right]$\nQu是系统噪声方差；Pny为系统状态一步预报估计误差协方差阵；Kn为卡尔曼滤波增益矩阵；H 为观测矩阵，且H＝[1 0 0 0]；I为单位阵；\n6)储存Pne、Xne的值，用于继续的递推计算和输出；储存Pny、Xny的值，用于测量连续性 判断计算；\n7)测量连续性的判断：\n7.1)计算观测值的一步预报估计Zny：\nZny＝H·Xny\n7.2)与Xny相应的方差矩阵S：\n$S=H·P_{\mathrm{ny}}·H^{\prime }+{\sigma }_x^2$\n7.3)定义观测因子：d2＝v′·S-1·v\n其中：v＝Zn-Zny\n7.4)依据d2值判断最新测量值与上次测量值来自同一目标的概率是否等于或超过阈值， 如等于或超过，则认为通过滤波估计获得的目标物信息是准确的，则输出本次距离、相对速 度和方位角，并返回第2)步进行下一次测量；\n如小于该阈值，则表明雷达对目标物测量信息丢失，或测量值来自另一个目标，需要进 一步判断小于上述阈值的次数是否连续超过预定值e：\n若未超过预定值e，则假设在目标物测量信息丢失的这一段时间内，目标物运动状态维 持不变，按照卡尔曼滤波假设，得到目标物状态的计算方法如下式：\n$x_n=x_{(n-1)}+{\stackrel{·}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{··}{x}}_{(n-1)}{T}^2+\frac{1}{6}{\stackrel{···}{x}}_{(n-1)}{T}^3$\n${\stackrel{·}{x}}_n={\stackrel{·}{x}}_{(n-1)}+{\stackrel{··}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{···}{x}}_{(n-1)}{T}^2$\n${\stackrel{··}{x}}_n={\stackrel{··}{x}}_{(n-1)}+{\stackrel{···}{x}}_{(n-1)}T$\n${\stackrel{···}{x}}_n={\stackrel{···}{x}}_{(n-1)}$\n输出当前目标物在x方向的距离的最优估计xn，相对速度 的最优估计，以及方位角An； 返回第2)步进行下一次测量；\n若超过预定值e，则认为有新的目标物出现，将最新的测量目标物设为跟踪目标物，其 测量信息作为目标物的状态初值，即最新目标物x向距离的测量值作为Xne中xn的值，Xne中其 他状态值设为0，将Pne设为单位阵，返回第2)步开始对新的目标物进行跟踪测量。\n上述第7.4)步中，所述依据d2值判断最新测量值与上次测量值来自同一目标的概率值 的阈值为95％，即依据式d2≤3.841判断最新测量值与上次测量值是否来自同一目标。\n实验证明，本方法能够获得更加平滑精确的目标物距离和相对速度信息；能够针对目标 测量连续性的判断结果进行相应处理，保证了目标物信息提取的连续性，达到了预期的效果。\n附图说明：\n图1是实现本方法的系统原理框图；\n图2是目标物方位角测量原理图；\n图3是车辆坐标系；其中，301-装有激光雷达探测系统的车辆，302-目标物，x-车辆 前进方向，y-车辆前进垂直方向，D-目标物距离，A-目标物方位角；\n图4是车辆前方目标物探测方法的流程图；\n图5是滤波算法递推过程图；\n图6是实验数据图一；其中，6(a)是距离数据，601-实验数据局部放大图；6(b)是相对 速度数据；6(c)是方位角数据。\n图7是实验数据图二；其中，7(a)是距离数据，7(b)是相对速度数据，7(c)是测量连 续性观测因子数据。\n具体实施方式\n结合附图说明本发明的具体实施方式。\n如图1所示，可以采用上海大学屠大维在1999年设计的扫描式激光雷达系统来实现本方 法，(见屠大维：“用于车辆防撞控制的行车环境传感研究”，中国机械工程，第10卷第6期， 1999年6月)。该系统采用相位法测距，激光二极管经注入式电流调制发出正弦幅度调制的 激光束，由发射光组变成满足要求的光束形状，再用扫描转镜实现水平扫描，向空间发射， 照射到前方车辆和障碍物上。由目标反射回来的光从扫描转镜经接收光组会聚到雪崩管上， 变成电信号，该信号经过具有低噪声放大功能的自动增益控制电路进入鉴相器，获得接收激 光相对于发射激光的相位差，鉴相器将此相位差通过插件板传给处理器，在处理器中计算目 标物距离。目标物距离D的计算公式如下：\n$D=\frac{c·\mathrm{\Delta \phi }}{2\mathrm{\pi f}}---\left(1\right)$\n其中，Δφ为反射回的激光束相对于发射激光束的相位延迟，c为光在空气中的传播速度， f为光强调制频率。\n利用图1中的一维位置探测器测定扫描光束角度，进而确定目标物的方位角。目标物方 位角计算原理如图2所示。从目标物201反射回的光线204，经扫描转镜203的会聚，成为 一个光点，打在一维位置探测器205上，一维位置探测器205中含有光敏元件，能够探测此 光点到中轴线202间的距离，图中以x1表示。一维位置探测器205安装位置与扫描转镜203 转动轴间的距离以L表示，由图2所示几何关系，可以获得目标物方位角计算公式如下：\n$A=\mathrm{arctg}\left(\frac{L}{x_1}\right)---\left(2\right)$\n本发明所提出的方法在图1所示的处理器中实现。图3为车辆行驶坐标系，其中x方向 为车辆301行驶的方向，y方向为与车辆301行驶方向垂直的方向，目标物为302，目标物距 离为D，目标物方位角为A。本方法的具体流程见图4。\n如图4所示，算法开始运行后，首先进行系统初始化，给定雷达测量系统距离测量噪声 方差σr 2(由雷达测量数据统计获得，一般雷达测量系统可取为1米2)、角度测量噪声方差σθ 2 (由雷达测量数据统计获得，一般雷达测量系统可取为1度2)、系统噪声方差Qu(对于车辆 应用环境，取为0.5m/s4)、系统状态向量初值X0e(由算法的收敛性，系统状态向量初始值可 以任意给定，一般给定零向量)、系统状态估计误差协方差阵初值P0e(由算法的收敛性，系 统状态估计误差协方差阵初始值可以任意给定，一般给定单位阵)。\n系统初始化完成后，利用式(1)、(2)获得目标物距离和方位角信息，并将这些信息送 入卡尔曼滤波程序进行滤波处理，通过滤波处理获得平滑的目标物距离和其相对速度信息， 有以下步骤：\n1)计算目标物在本车行驶方向(x向)上距离的现时观测值Zn：\nZn＝DncosAn                       (3)\n其中，下标n表示测量和计算的次数，其初值为1；现时测量值以相应量加下标n表示， 如Dn是目标物距离的现时测量值，An是目标物方位角的现时测量值；上一次测量值以相应量 加下标(n-1)表示，如Z(n-1)表示目标物x向距离的上一次测量值。以下有关变量的下标n的含 义与上述相同。\n2)计算本次x向距离测量的噪声方差：\n${\sigma }_x^2={\sigma }_r^2·{\cos }^2{A}_n+{D_n}^2·{\sigma }_{\theta }^2{\sin }^2{A}_n---\left(4\right)$\n3)按下述四阶卡尔曼滤波递推公式进行滤波计算：\nXny＝F·X(n-1)e                                  (5)\nPny＝F·P(n-1)e·F′+G·Qu·G′                 (6)\n$K_n=P_{\mathrm{ny}}·H^{\prime }·{(H·P_{\mathrm{ny}}·H^{\prime }+{\sigma }_x^2)}^{-1}---\left(7\right)$\nPne＝(I-Kn·H)·Pny                           (8)\nXne＝Xny+Kn·(Zn-H·Xny)                    (9)\n其中：\n$X_{\mathrm{ne}}^{\prime }=[x_n,{\stackrel{·}{x}}_n,{\stackrel{··}{x}}_n,{\stackrel{···}{x}}_n]---\left(10\right)$\n为当前目标物信息的动态最优估计(′是矩阵转置符号，xn表示目标物x向距离值的最优估计，\n表示车辆与目标物间相对速度的最优估计)，Xny为目标物信息的一步预报估计，F为系统矩 阵：\n$F=\left[\begin{array}{cccc}1& T& T^2/2& T^3/6\\ 0& 1& T& T^2/2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]---\left(11\right)$\nT为系统采样时间；Pne是当前系统状态估计误差协方差阵，G是系统误差影响矩阵，\n$G=\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right]---\left(11\right)$\nQu是系统噪声方差；Pny为系统状态一步预报估计误差协方差阵；Kn为卡尔曼滤波增益矩阵；H 为观测矩阵，且H＝[1 0 0 0]；I为单位阵；\n4)根据上述递推公式得到目标物在x方向的距离xn和相对速度 的最优估计。\n5)储存Pne、Xne的值，用于继续的递推计算和输出；储存Pny、Xny的值，用于测量连续性判断 的计算。\n对于上述滤波过程解释如下：\n考虑实际车辆运动状况，假设目标车辆在x向以恒定加速度行驶，受到白噪声加 速度的干扰，取系统状态向量如式(10)所示，则离散的x向系统模型表示为：\nXn+1＝F·Xn+G·un                          (13)\nZn是目标物的x向距离的现时测量值，T是测量系统采样时间，G的选择是考虑多重积分 的效果，即假设系统噪声只直接影响到车辆加速度的变化，对其他状态的影响通过层层积 分来体现，并无直接影响。系统噪声un的方差为Qu。\n系统x向测量模型为：\nZn＝H·Xn+vn                              (14)\n其中Zn由(3)式计算得到。\nH＝[1 0 0 0 ]                                 (15)\nvn为第n次测量的x向距离测量噪声，其方差按下式算得：\n${\sigma }_x^2={\sigma }_r^2·{\cos }^2\theta +r^2·{\sigma }_{\theta }^2{\sin }^2\theta ---\left(16\right)$\n其中σr 2是距离测量误差的方差，σθ 2是方位角测量误差的方差。\n卡尔曼(Kalman)滤波公式如下所示：\nXne＝Xny+Kn·(Zn-H·Xny)                 (17)\n其中，Xne为当前目标物信息的动态最优估计，Xny为目标物信息的一步预报估计，Zn为目 标物x向距离的现时观测值，Kn为卡尔曼(Kalman)滤波增益矩阵。\n按照最优估计理论，确定系统递推过程如下所示：\nXny＝F·X(n-1)e                               (18)\nPny＝F·P(n-1)e·F′+G·Qu·G′              (19)\n$K_n=P_{\mathrm{ny}}·H^{\prime }·{(H·P_{\mathrm{ny}}·H^{\prime }+{\sigma }_x^2)}^{-1}---\left(20\right)$\nPne＝(I-Kn·H)·Pny                         (21)\n其中，Pne是当前系统状态估计误差协方差阵，Pny是系统状态一步预报估计误差协方差阵。 这样，给定系统采样时间T、随机加速度方差Qu、雷达测量噪声方差σr 2和σθ 2、系统初值 P0e和X0e，按照上述公式(17)～(21)进行雷达目标物信息的卡尔曼(Kalman)滤波估计， 可以获得准确、平滑的目标物距离及相对速度信息。滤波算法递推过程如图5所示。\n接下来判断目标物测量的连续性，实际车辆的运行环境比较复杂，不能够保证雷达测量 的数据是连续并来自同一目标物，本发明设计了目标的测量连续性判断方法，能对同一目标 物进行连续跟踪测量，去掉由于雷达个别数据丢失带来的对整个滤波过程的干扰，并能防止 道路环境等因素对系统的干扰，具体步骤如下：\n6)计算观测值的一步预报估计Zny：Zny＝H·Xny    (22)\n7)与Xny相应的方差矩阵\n$S:S=H·P_{\mathrm{ny}}·H^{\prime }+{\sigma }_x^2---\left(23\right)$\n8)定义观测因子：d2＝v′·S-1·v               (24)\n其中：v＝Zn-Zny                               (25)\n9)由于测量误差服从正态分布，因此，d2服从自由度为nm的x2分布，其中nm为能测量的目 标物信息的个数，例如本发明中，在状态变量Xn中，仅有目标物x向距离信息可以测量，因 此nm＝1。如取显著水平为5％，对于单一观测量的系统，由x2分布查表可得到假设检验的 判断准则为：\nd2≤3.841                                      (26)\n如果新的测量结果使式(26)成立，则可判断最新测量值与上次测量值来自同一目标的 概率大于等于95％，可以认为通过滤波估计获得的目标物信息是准确的，结果正常输出；如 新的测量值不能满足式(26)，则判断新的测量值不是来自跟踪目标或出现了雷达对目标物测 量信息丢失的概率为95％，按照小概率事件为不可能事件的假设，可以认为此时通过滤波估 计获得的目标车辆信息是不准确的，不能以此作为目标车辆信息的最优估计。\n由于车辆的颠簸及摆动带来的雷达对目标物测量信息的丢失只是暂时的，为保持目标物 信息输出结果的连续性，按照卡尔曼滤波假设，可以认为在目标物测量信息丢失的这一段时 间内，目标物运动状态维持不变，得到目标物状态的计算方法如式(27)所示。\n$x_n=x_{(n-1)}+{\stackrel{·}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{··}{x}}_{(n-1)}{T}^2+\frac{1}{6}{\stackrel{···}{x}}_{(n-1)}{T}^3$\n${\stackrel{·}{x}}_n={\stackrel{·}{x}}_{(n-1)}+{\stackrel{··}{x}}_{(n-1)}T+\frac{1}{2}{\stackrel{···}{x}}_{(n-1)}{T}^2---\left(27\right)$\n${\stackrel{··}{x}}_n={\stackrel{··}{x}}_{(n-1)}+{\stackrel{···}{x}}_{(n-1)}T$\n${\stackrel{···}{x}}_n={\stackrel{···}{x}}_{(n-1)}$\nn＝n+1，返回第1)步。\n如连续不能满足式(26)的测量次数超过某一预定值e时(e的取值依据目标物的运动 性质和测量系统的采样频率决定，如本发明中，对于车载环境，可取e为1秒内的测量次数， 即e＝10)，说明这种不满足不是由于雷达对目标车辆测量信息的暂时丢失引起的，而是有新 的目标车辆出现，此时，将最新的测量目标物设为跟踪目标物，其测量信息作为目标物的状 态初值(最新目标物x向距离的测量值作为Xne中xn的值，Xne中其他状态值设为0)，将Pne设 为单位阵，开始对新的目标物进行跟踪测量。\n图6为利用本发明的方法进行实车实验的一段实验数据。其中，图6(a)是实车实验的 距离数据，为便于对比，图中分别给出了滤波前的原始测量值和滤波后的雷达输出值；601 是实验数据局部放大图；图6(b)是相对速度实车实验数据，其中的相对速度真实值是利用 微波测速仪测得的；6(c)是本发明所获得的方位角实车实验数据，单位为度。\n从图6可见，利用本发明所提的卡尔曼滤波算法，可以去除测量噪声及系统噪声对测量 结果的影响，获得准确、实时的目标物距离和相对速度信息。\n图7为利用本发明的方法进行实车实验的另一段测量结果，实验时由于路面的颠簸，出 现了雷达测量数据的暂时丢失。其中，图7(a)为雷达原始测量数据与经信号处理后的雷达 输出数据的对比；图7(b)为相对速度的实验结果；图7(c)为实车实验过程中，用于测量 连续性判断的观测因子的值。\n从图7(a)可见，虽然雷达测量原始数据存在短暂的丢失，但雷达输出数据保持了较好 的连续性。从图7(b)可见，雷达装置输出的相对速度值并没有受雷达数据暂时丢失的影响。 从图7(c)可见，利用本发明的目标物测量连续性判断方法，能够及时发现雷达对目标物测 量数据的丢失。图7显示，利用本发明所设计的目标物测量连续性判断方法，能够有效的去 除由于雷达数据的暂时丢失带来的影响，验证了本发明的有益效果。