CN1299125C

CN1299125C - Detection method of the target in front of the vehicle

Info

Publication number: CN1299125C
Application number: CN 200410042538
Authority: CN
Inventors: 李克强; 侯德藻; 罗禹贡; 杨殿阁; 连小珉
Original assignee: Tsinghua University
Current assignee: Qingzhi Automobile Technology Suzhou Co ltd
Priority date: 2004-05-21
Filing date: 2004-05-21
Publication date: 2007-02-07
Anticipated expiration: 2024-05-21
Also published as: CN1580816A

Abstract

The detection method of the target in front of the vehicle belongs to the field of vehicle driving information perception and processing technology. Its characteristic is that it uses the fourth-order Kalman filter method to filter the target measurement data collected by the vehicle, which can obtain smoother and more accurate target distance and relative speed information, and further proposes the measurement continuity judgment of the target. Method, the data lost by the radar and the data from another target are processed accordingly.

Description

Detection method of the target in front of the vehicle

技术领域technical field

车辆前方目标物的探测方法属于车辆行车信息感知与处理技术领域。A detection method for a target object in front of a vehicle belongs to the technical field of vehicle driving information perception and processing.

背景技术Background technique

车辆行车信息感知及处理技术是现代汽车控制与安全的关键技术之一，现有测量技术按其应用介质分为微波测量系统和激光测量系统两类，激光雷达系统因测量速度快、精度高、测程远而受到广泛的重视。文献1(屠大维：“用于车辆防撞控制的行车环境传感研究”，中国机械工程，第10卷第6期，1999年6月)中介绍了一种光学扫描式激光测距雷达的设计，可以实现较宽范围的扫描测距，该系统能够测得前方目标物的距离和方位角信息，测量误差未得到有效处理，得到的相对速度波动极大，实际中不能使用。Vehicle driving information perception and processing technology is one of the key technologies for modern automobile control and safety. Existing measurement technologies are divided into microwave measurement systems and laser measurement systems according to their application media. The laser radar system is characterized by fast measurement speed, high precision, It has received wide attention due to its long measuring range. Document 1 (Tu Dawei: "Research on Driving Environment Sensing for Vehicle Collision Avoidance Control", China Mechanical Engineering, Volume 10, Issue 6, June 1999) introduces the design of an optical scanning laser ranging radar , can achieve a wide range of scanning ranging, the system can measure the distance and azimuth information of the target in front, the measurement error has not been effectively dealt with, and the obtained relative speed fluctuates greatly, which cannot be used in practice.

发明内容Contents of the invention

本发明的目的在于，针对现有技术的不足提出一种车辆前方目标物的探测方法，该方法将获得的目标物距离及方位角信息进行滤波处理，能够得到更平滑精确的目标物距离和相对速度信息，并进一步提出了目标的测量连续性判断方法，能够判断所获得的连续测量数据是否来自同一目标物。The object of the present invention is to propose a detection method for the target object in front of the vehicle in view of the deficiencies of the prior art. The method filters the obtained target object distance and azimuth angle information, and can obtain a smoother and more accurate target object distance and relative distance. Velocity information, and further proposed a measurement continuity judgment method of the target, which can judge whether the obtained continuous measurement data come from the same target.

本发明所提出车辆前方目标物的探测方法，其特征在于，它依次含有由车辆处理器控制执行的以下步骤：The detection method of the target object in front of the vehicle proposed by the present invention is characterized in that it contains the following steps controlled and executed by the vehicle processor in sequence:

1)初始化：1) Initialization:

给定雷达测量系统距离测量噪声方差σ_r ²、角度测量噪声方差σ_θ ²、系统噪声方差Q_u、系统状态向量初值X_0e、系统状态估计误差协方差阵初值P_0e；Given radar measurement system distance measurement noise variance σ _r ² , angle measurement noise variance σ _θ ² , system noise variance Q _u , system state vector initial value X _0e , system state estimation error covariance matrix initial value P _0e ;

2)测量目标物距离D_n和方位角A_n，其中，下标n表示测量和计算的次数，其初值为1；现时测量值以相应量加下标n表示；2) Measure the distance D _n and azimuth A _n of the target object, where the subscript n represents the number of times of measurement and calculation, and its initial value is 1; the current measurement value is expressed by adding the subscript n to the corresponding quantity;

3)计算目标物在本车行驶方向，即x方向上距离的现时观测值Z_n：3) Calculate the current observation value Z _n of the distance of the target object in the driving direction of the vehicle, that is, in the x direction:

Z_n＝D_ncosA_n Z _n ＝D _n cosA _n

4)计算本次x向距离测量的噪声方差：4) Calculate the noise variance of this x-direction distance measurement:

${σ σ}_{x x}^{22} = = {σ σ}_{r r}^{22} \cdot \cdot {cos cos}^{22} {A A}_{n no} + + {D D.}_{n no}^{22} \cdot &Center Dot; {σ σ}_{θ θ}^{22} {sin sin}^{22} {A A}_{n no}$

5)按下述四阶卡尔曼滤波递推公式进行滤波计算：5) Perform filter calculation according to the following fourth-order Kalman filter recursive formula:

X_ny＝F·X_(n-1)e X _ny =F·X _(n-1)e

P_ny＝F·P_(n-1)e·F′+G·Q_u·G′P _ny =F·P _(n-1)e ·F′+G·Q _u ·G′

${K K}_{n no} = = {P P}_{ny no} \cdot &Center Dot; {H h}^{' '} \cdot &Center Dot; {((H h \cdot \cdot {P P}_{ny no} \cdot \cdot {H h}^{' '} + + {σ σ}_{x x}^{22}))}^{- - 11}$

P_ne＝(I-K_n·H)·P_ny P _ne = (IK _n H) P _ny

X_ne＝X_ny+K_n·(Z_n-H·X_ny)X _ne =X _ny +K _n (Z _n -H X _ny )

其中， $X_{ne}^{'} = [x_{n}, {\overset{\cdot}{x}}_{n}, {\overset{\cdot \cdot}{x}}_{n}, {\overset{\cdot \cdot \cdot}{x}}_{n}]$ 表示当前目标物信息的动态最优估计，′是矩阵转置符号，x_n表示目标物x向距离值的最优估计，表示车辆与目标物间相对速度的最优估计；X_ny为目标物信息的一步预报估计，F为系统矩阵：in, $x_{ne}^{'} = [x_{no}, {\overset{\cdot}{x}}_{no}, {\overset{&Center Dot; &Center Dot;}{x}}_{no}, {\overset{&Center Dot; &Center Dot; \cdot}{x}}_{no}]$ Indicates the dynamic optimal estimate of the current target information, 'is the matrix transposition symbol, x _n indicates the optimal estimate of the target's x-direction distance value, Indicates the optimal estimate of the relative speed between the vehicle and the target; X _ny is the one-step forecast estimate of the target information, and F is the system matrix:

$F f = = [\begin{matrix} 11 & T T & {T T}^{22} / / 22 & {T T}^{33} / / 66 \\ 00 & 11 & T T & {T T}^{22} / / 22 \\ 00 & 00 & 11 & T T \\ 00 & 00 & 00 & 11 \end{matrix}]$

T为系统采样时间；P_ne是当前系统状态估计误差协方差阵，G是系统误差影响矩阵，T is the system sampling time; _Pne is the current system state estimation error covariance matrix, G is the system error influence matrix,

$G G = = [\begin{matrix} 00 \\ 00 \\ 00 \\ T T \end{matrix}]$

Q_u是系统噪声方差；P_ny为系统状态一步预报估计误差协方差阵；K_n为卡尔曼滤波增益矩阵；H为观测矩阵，且H＝[1 0 0 0]；I为单位阵；Q _u is the system noise variance; P _ny is the system state one-step forecast estimation error covariance matrix; K _n is the Kalman filter gain matrix; H is the observation matrix, and H=[1 0 0 0]; I is the identity matrix;

6)储存P_ne、X_ne的值，用于继续的递推计算和输出；储存P_ny、X_ny的值，用于测量连续性判断计算；6) Store the values of P _ne and X _ne for continued recursive calculation and output; store the values of P _ny and X _ny for measurement continuity judgment calculation;

7)测量连续性的判断：7) Judgment of measurement continuity:

7.1)计算观测值的一步预报估计Z_ny：7.1) Calculate the one-step forecast estimate Z _ny of the observed value:

Z_ny＝H·X_ny Z _ny =H·X _ny

7.2)与X_ny相应的方差矩阵S：7.2) Variance matrix S corresponding to X _ny :

$S S = = H h \cdot \cdot {P P}_{ny no} \cdot &Center Dot; {H h}^{' '} + + {σ σ}_{x x}^{22}$

7.3)定义观测因子：d²＝v′·S^-1·v7.3) Define the observation factor: d ² =v′·S ^-1 ·v

其中：v＝Z_n-Z_ny Where: v= _Zn - _Zny

7.4)依据d²值判断最新测量值与上次测量值来自同一目标的概率是否等于或超过阈值，如等于或超过，则认为通过滤波估计获得的目标物信息是准确的，则输出本次距离、相对速度和方位角，并返回第2)步进行下一次测量；7.4) According to the ^d2 value, judge whether the probability that the latest measurement value and the last measurement value come from the same target is equal to or exceeds the threshold value. If it is equal to or exceeds the threshold, it is considered that the target information obtained through filtering estimation is accurate, and the current distance is output , relative velocity and azimuth, and return to step 2) for the next measurement;

如小于该阈值，则表明雷达对目标物测量信息丢失，或测量值来自另一个目标，需要进一步判断小于上述阈值的次数是否连续超过预定值e：If it is less than the threshold, it indicates that the radar’s measurement information of the target is lost, or the measurement value comes from another target. It is necessary to further judge whether the number of times less than the above threshold exceeds the predetermined value e continuously:

若未超过预定值e，则假设在目标物测量信息丢失的这一段时间内，目标物运动状态维持不变，按照卡尔曼滤波假设，得到目标物状态的计算方法如下式：If it does not exceed the predetermined value e, it is assumed that during the period of time when the measurement information of the target is lost, the motion state of the target remains unchanged. According to the assumption of Kalman filtering, the calculation method for the state of the target is as follows:

${x x}_{n no} = = {x x}_{((n no - - 11))} + + {\overset{\cdot \cdot}{x x}}_{((n no - - 11))} T T + + \frac{11}{22} {\overset{\cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} {T T}^{22} + + \frac{11}{66} {\overset{\cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} {T T}^{33}$

${\overset{\cdot &Center Dot;}{x x}}_{n no} = = {\overset{\cdot &Center Dot;}{x x}}_{((n no - - 11))} + + {\overset{\cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} T T + + \frac{11}{22} {\overset{\cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} {T T}^{22}$

${\overset{\cdot &Center Dot; \cdot &Center Dot;}{x x}}_{n no} = = {\overset{\cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} + + {\overset{\cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} T T$

${\overset{\cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;}{x x}}_{n no} = = {\overset{\cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))}$

输出当前目标物在x方向的距离的最优估计x_n，相对速度

的最优估计，以及方位角A_n；返回第2)步进行下一次测量；Output the optimal estimate x _n of the distance of the current target in the x direction, relative speed

The best estimate of , and the azimuth A _n ; return to step 2) for the next measurement;

若超过预定值e，则认为有新的目标物出现，将最新的测量目标物设为跟踪目标物，其测量信息作为目标物的状态初值，即最新目标物x向距离的测量值作为X_ne中x_n的值，X_ne中其他状态值设为0，将P_ne设为单位阵，返回第2)步开始对新的目标物进行跟踪测量。If it exceeds the predetermined value e, it is considered that there is a new target, and the latest measurement target is set as the tracking target, and its measurement information is used as the initial value of the state of the target, that is, the measured value of the latest target’s x-direction distance is taken as X The value of x _n in _ne , the other state values in X _ne are set to 0, set P _ne to the unit matrix, and return to step 2) to start tracking and measuring the new target.

上述第7.4)步中，所述依据d²值判断最新测量值与上次测量值来自同一目标的概率值的阈值为95％，即依据式d²≤3.841判断最新测量值与上次测量值是否来自同一目标。In the above-mentioned step 7.4), the ^threshold value of judging the probability that the latest measured value and the last measured value come from the same target according to the value of ^d2 is 95%, that is, the latest measured value and the last measured value are judged according to the formula d2≤3.841 whether from the same target.

实验证明，本方法能够获得更加平滑精确的目标物距离和相对速度信息；能够针对目标测量连续性的判断结果进行相应处理，保证了目标物信息提取的连续性，达到了预期的效果。Experiments have proved that this method can obtain smoother and more accurate target distance and relative velocity information; it can process the judgment results of target measurement continuity accordingly, ensuring the continuity of target information extraction and achieving the desired effect.

附图说明：Description of drawings:

图1是实现本方法的系统原理框图；Fig. 1 is the system block diagram that realizes this method;

图2是目标物方位角测量原理图；Figure 2 is a schematic diagram of the azimuth angle measurement of the target;

图3是车辆坐标系；其中，301-装有激光雷达探测系统的车辆，302-目标物，x-车辆前进方向，y-车辆前进垂直方向，D-目标物距离，A-目标物方位角；Figure 3 is the vehicle coordinate system; where, 301-the vehicle equipped with the laser radar detection system, 302-the target, x-the direction of the vehicle, y-the vertical direction of the vehicle, D-the distance of the target, A-the azimuth of the target ;

图4是车辆前方目标物探测方法的流程图；Fig. 4 is a flow chart of a method for detecting an object in front of the vehicle;

图5是滤波算法递推过程图；Fig. 5 is a recursive process diagram of the filtering algorithm;

图6是实验数据图一；其中，6(a)是距离数据，601-实验数据局部放大图；6(b)是相对速度数据；6(c)是方位角数据。Fig. 6 is the first experimental data; wherein, 6(a) is the distance data, 601-experimental data partial enlarged view; 6(b) is the relative speed data; 6(c) is the azimuth data.

图7是实验数据图二；其中，7(a)是距离数据，7(b)是相对速度数据，7(c)是测量连续性观测因子数据。Figure 7 is the second figure of the experimental data; among them, 7(a) is the distance data, 7(b) is the relative speed data, 7(c) is the measurement continuity observation factor data.

具体实施方式Detailed ways

结合附图说明本发明的具体实施方式。The specific embodiment of the present invention will be described with reference to the accompanying drawings.

如图1所示，可以采用上海大学屠大维在1999年设计的扫描式激光雷达系统来实现本方法，(见屠大维：“用于车辆防撞控制的行车环境传感研究”，中国机械工程，第10卷第6期，1999年6月)。该系统采用相位法测距，激光二极管经注入式电流调制发出正弦幅度调制的激光束，由发射光组变成满足要求的光束形状，再用扫描转镜实现水平扫描，向空间发射，照射到前方车辆和障碍物上。由目标反射回来的光从扫描转镜经接收光组会聚到雪崩管上，变成电信号，该信号经过具有低噪声放大功能的自动增益控制电路进入鉴相器，获得接收激光相对于发射激光的相位差，鉴相器将此相位差通过插件板传给处理器，在处理器中计算目标物距离。目标物距离D的计算公式如下：As shown in Figure 1, the scanning laser radar system designed by Tu Dawei of Shanghai University in 1999 can be used to realize this method, (see Tu Dawei: "Research on Driving Environment Sensing for Vehicle Collision Avoidance Control", China Mechanical Engineering, No. 10, No. 6, June 1999). The system uses the phase method for distance measurement. The laser diode emits a sinusoidal amplitude modulated laser beam through injection current modulation. The emitted light group becomes a beam shape that meets the requirements, and then the scanning mirror is used to realize horizontal scanning. vehicles and obstacles ahead. The light reflected back from the target converges on the avalanche tube through the receiving light group from the scanning rotating mirror, and becomes an electrical signal. The phase difference, the phase detector transmits this phase difference to the processor through the plug-in board, and calculates the target object distance in the processor. The calculation formula of target distance D is as follows:

$D D. = = \frac{c c \cdot &Center Dot; Δφ Δφ}{22 πf πf} - - - - - - ((11))$

其中，Δφ为反射回的激光束相对于发射激光束的相位延迟，c为光在空气中的传播速度，f为光强调制频率。Among them, Δφ is the phase delay of the reflected laser beam relative to the emitted laser beam, c is the propagation speed of light in the air, and f is the light intensity modulation frequency.

利用图1中的一维位置探测器测定扫描光束角度，进而确定目标物的方位角。目标物方位角计算原理如图2所示。从目标物201反射回的光线204，经扫描转镜203的会聚，成为一个光点，打在一维位置探测器205上，一维位置探测器205中含有光敏元件，能够探测此光点到中轴线202间的距离，图中以x₁表示。一维位置探测器205安装位置与扫描转镜203转动轴间的距离以L表示，由图2所示几何关系，可以获得目标物方位角计算公式如下：Use the one-dimensional position detector in Figure 1 to measure the scanning beam angle, and then determine the azimuth angle of the target. The calculation principle of target azimuth angle is shown in Fig. 2. The light 204 reflected back from the target object 201 becomes a light spot after being converged by the scanning rotating mirror 203, and hits on the one-dimensional position detector 205. The one-dimensional position detector 205 contains a photosensitive element, which can detect the light point to The distance between the central axes 202 is represented by _x1 in the figure. The distance between the installation position of the one-dimensional position detector 205 and the rotation axis of the scanning rotating mirror 203 is represented by L. From the geometric relationship shown in FIG. 2, the calculation formula for the target azimuth angle can be obtained as follows:

$A A = = arctg arctg ((\frac{L L}{{x x}_{11}})) - - - - - - ((22))$

本发明所提出的方法在图1所示的处理器中实现。图3为车辆行驶坐标系，其中x方向为车辆301行驶的方向，y方向为与车辆301行驶方向垂直的方向，目标物为302，目标物距离为D，目标物方位角为A。本方法的具体流程见图4。The method proposed by the present invention is implemented in the processor shown in FIG. 1 . 3 is a vehicle coordinate system, where the x direction is the direction in which the vehicle 301 is traveling, the y direction is the direction perpendicular to the direction in which the vehicle 301 is traveling, the target is 302, the distance of the target is D, and the azimuth of the target is A. The specific flow of this method is shown in Figure 4.

如图4所示，算法开始运行后，首先进行系统初始化，给定雷达测量系统距离测量噪声方差σ_r ²(由雷达测量数据统计获得，一般雷达测量系统可取为1米²)、角度测量噪声方差σ_θ ²(由雷达测量数据统计获得，一般雷达测量系统可取为1度²)、系统噪声方差Q_u(对于车辆应用环境，取为0.5m/s⁴)、系统状态向量初值X_0e(由算法的收敛性，系统状态向量初始值可以任意给定，一般给定零向量)、系统状态估计误差协方差阵初值P_0e(由算法的收敛性，系统状态估计误差协方差阵初始值可以任意给定，一般给定单位阵)。As shown in Figure 4, after the algorithm starts to run, the system is initialized first, given the radar measurement system distance measurement noise variance σ _r ² (obtained from the statistics of radar measurement data, generally the radar measurement system can be taken as 1 m ² ), the angle measurement noise Variance σ _θ ² (obtained statistically from radar measurement data, general radar measurement system can be taken as 1 degree ² ), system noise variance Q _u (for vehicle application environment, taken as 0.5m/s ⁴ ), initial value of system state vector X _0e (According to the convergence of the algorithm, the initial value of the system state vector can be given arbitrarily, generally a zero vector is given), the initial value of the system state estimation error covariance matrix P _0e (According to the convergence of the algorithm, the initial value of the system state estimation error covariance matrix The value can be given arbitrarily, generally the unit matrix is given).

系统初始化完成后，利用式(1)、(2)获得目标物距离和方位角信息，并将这些信息送入卡尔曼滤波程序进行滤波处理，通过滤波处理获得平滑的目标物距离和其相对速度信息，有以下步骤：After the system initialization is completed, the distance and azimuth information of the target object are obtained by using formulas (1) and (2), and these information are sent to the Kalman filter program for filtering processing, and the smooth target object distance and its relative speed are obtained through filtering processing information, the steps are as follows:

1)计算目标物在本车行驶方向(x向)上距离的现时观测值Z_n：1) Calculate the current observation value Z _n of the distance of the target object in the driving direction (x direction) of the vehicle:

Z_n＝D_ncosA_n (3)Z _n ＝ D _n cos A _n (3)

其中，下标n表示测量和计算的次数，其初值为1；现时测量值以相应量加下标n表示，如D_n是目标物距离的现时测量值，A_n是目标物方位角的现时测量值；上一次测量值以相应量加下标(n-1)表示，如Z_(n-1)表示目标物x向距离的上一次测量值。以下有关变量的下标n的含义与上述相同。Among them, the subscript n represents the number of measurements and calculations, and its initial value is 1; the current measurement value is expressed by adding the subscript n to the corresponding quantity, such as D _n is the current measurement value of the distance of the target object, A _n is the azimuth of the target object The current measurement value; the last measurement value is represented by the corresponding quantity plus a subscript (n-1), such as Z _(n-1) represents the last measurement value of the x-direction distance of the target. The subscript n of the following variables has the same meaning as above.

2)计算本次x向距离测量的噪声方差：2) Calculate the noise variance of this x-direction distance measurement:

${σ σ}_{x x}^{22} = = {σ σ}_{r r}^{22} \cdot \cdot {cos cos}^{22} {A A}_{n no} + + {D D.}_{n no}^{22} \cdot \cdot {σ σ}_{θ θ}^{22} {sin sin}^{22} {A A}_{n no} - - - - - - ((44))$

3)按下述四阶卡尔曼滤波递推公式进行滤波计算：3) Carry out filter calculation according to the following fourth-order Kalman filter recursive formula:

X_ny＝F·X_(n-1)e (5)X _ny =F·X _(n-1)e (5)

P_ny＝F·P_(n-1)e·F′+G·Q_u·G′ (6)P _ny =F·P _(n-1)e ·F′+G·Q _u ·G′ (6)

${K K}_{n no} = = {P P}_{ny no} \cdot &Center Dot; {H h}^{' '} \cdot \cdot {((H h \cdot \cdot {P P}_{ny no} \cdot &Center Dot; {H h}^{' '} + + {σ σ}_{x x}^{22}))}^{- - 11} - - - - - - ((77))$

P_ne＝(I-K_n·H)·P_ny (8)P _ne = (IK _n H) P _ny (8)

X_ne＝X_ny+K_n·(Z_n-H·X_ny) (9)X _ne ＝X _ny +K _n ·(Z _n −H·X _ny ) (9)

其中：in:

${X x}_{ne ne}^{' '} = = [[{x x}_{n no},, {\overset{\cdot &Center Dot;}{x x}}_{n no},, {\overset{\cdot &Center Dot; \cdot &Center Dot;}{x x}}_{n no},, {\overset{\cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;}{x x}}_{n no}]] - - - - - - ((1010))$

为当前目标物信息的动态最优估计(′是矩阵转置符号，x_n表示目标物x向距离值的最优估计，is the dynamic optimal estimate of the current target information (' is the symbol of matrix transposition, x _n represents the optimal estimate of the target's x-direction distance value,

表示车辆与目标物间相对速度的最优估计)，X_ny为目标物信息的一步预报估计，F为系统矩阵： represents the optimal estimate of the relative speed between the vehicle and the target), X _ny is the one-step forecast estimate of the target information, and F is the system matrix:

$F f = = [\begin{matrix} 11 & T T & {T T}^{22} / / 22 & {T T}^{33} / / 66 \\ 00 & 11 & T T & {T T}^{22} / / 22 \\ 00 & 00 & 11 & T T \\ 00 & 00 & 00 & 11 \end{matrix}] - - - - - - ((1111))$

$G G = = [\begin{matrix} 00 \\ 00 \\ 00 \\ T T \end{matrix}] - - - - - - ((1111))$

4)根据上述递推公式得到目标物在x方向的距离x_n和相对速度的最优估计。4) Obtain the distance x _n and relative velocity of the target in the x direction according to the above recursive formula best estimate of .

5)储存P_ne、X_ne的值，用于继续的递推计算和输出；储存P_ny、X_ny的值，用于测量连续性判断的计算。5) Store the values of P _ne and X _ne for continuous recursive calculation and output; store the values of P _ny and X _ny for calculation of measurement continuity judgment.

对于上述滤波过程解释如下：The above filtering process is explained as follows:

考虑实际车辆运动状况，假设目标车辆在x向以恒定加速度行驶，受到白噪声加速度的干扰，取系统状态向量如式(10)所示，则离散的x向系统模型表示为：Considering the actual vehicle movement conditions, assuming that the target vehicle is traveling at a constant acceleration in the x direction and is disturbed by the acceleration of white noise, the system state vector is taken as shown in equation (10), then the discrete x direction system model is expressed as:

X_n+1＝F·X_n+G·u_n (13)X _n+1 = F X _n +G u _n (13)

Z_n是目标物的x向距离的现时测量值，T是测量系统采样时间，G的选择是考虑多重积分的效果，即假设系统噪声只直接影响到车辆加速度的变化，对其他状态的影响通过层层积分来体现，并无直接影响。系统噪声u_n的方差为Q_u。Z _n is the current measured value of the x-direction distance of the target, T is the sampling time of the measurement system, and the choice of G is to consider the effect of multiple integration, that is, it is assumed that the system noise only directly affects the change of vehicle acceleration, and the influence on other states is passed through It is reflected layer by layer, and has no direct impact. The variance of the system noise u _n is Q _u .

系统x向测量模型为：The x-direction measurement model of the system is:

Z_n＝H·X_n+v_n (14)Z _n =H X _n +v _n (14)

其中Z_n由(3)式计算得到。Among them, Z _n is calculated by formula (3).

H＝[1 0 0 0 ] (15)H＝[1 0 0 0 ]

v_n为第n次测量的x向距离测量噪声，其方差按下式算得：v _n is the x-direction distance measurement noise of the nth measurement, and its variance is calculated by the following formula:

${σ σ}_{x x}^{22} = = {σ σ}_{r r}^{22} \cdot &Center Dot; {cos cos}^{22} θ θ + + {r r}^{22} \cdot &Center Dot; {σ σ}_{θ θ}^{22} {sin sin}^{22} θ θ - - - - - - ((1616))$

其中σ_r ²是距离测量误差的方差，σ_θ ²是方位角测量误差的方差。where σ _r ² is the variance of the range measurement error and σ _θ ² is the variance of the azimuth measurement error.

卡尔曼(Kalman)滤波公式如下所示：The Kalman filter formula is as follows:

X_ne＝X_ny+K_n·(Z_n-H·X_ny) (17)X _ne =X _ny +K _n (Z _n -H X _ny ) (17)

其中，X_ne为当前目标物信息的动态最优估计，X_ny为目标物信息的一步预报估计，Z_n为目标物x向距离的现时观测值，K_n为卡尔曼(Kalman)滤波增益矩阵。Among them, X _ne is the dynamic optimal estimation of the current target information, X _ny is the one-step forecast estimation of the target information, Z _n is the current observation value of the target x-direction distance, K _n is the Kalman filter gain matrix .

按照最优估计理论，确定系统递推过程如下所示：According to the optimal estimation theory, the recursive process of the determination system is as follows:

X_ny＝F·X_(n-1)e (18)X _ny =F·X _(n-1)e (18)

P_ny＝F·P_(n-1)e·F′+G·Q_u·G′ (19)P _ny =F·P _(n-1)e ·F′+G·Q _u ·G′ (19)

${K K}_{n no} = = {P P}_{ny no} \cdot &Center Dot; {H h}^{' '} \cdot &Center Dot; {((H h \cdot &Center Dot; {P P}_{ny no} \cdot &Center Dot; {H h}^{' '} + + {σ σ}_{x x}^{22}))}^{- - 11} - - - - - - ((2020))$

P_ne＝(I-K_n·H)·P_ny (21)P _ne = (IK _n H) P _ny (21)

其中，P_ne是当前系统状态估计误差协方差阵，P_ny是系统状态一步预报估计误差协方差阵。这样，给定系统采样时间T、随机加速度方差Q_u、雷达测量噪声方差σ_r ²和σ_θ ²、系统初值P_0e和X_0e，按照上述公式(17)～(21)进行雷达目标物信息的卡尔曼(Kalman)滤波估计，可以获得准确、平滑的目标物距离及相对速度信息。滤波算法递推过程如图5所示。Among them, P _ne is the current system state estimation error covariance matrix, P _ny is the system state one-step forecast estimation error covariance matrix. In this way, given the system sampling time T, random acceleration variance Q _u , radar measurement noise variance σ _r ² and σ _θ ² , and system initial values P _0e and X _0e , the radar target The Kalman filter estimation of information can obtain accurate and smooth target distance and relative velocity information. The recursive process of the filtering algorithm is shown in Figure 5.

接下来判断目标物测量的连续性，实际车辆的运行环境比较复杂，不能够保证雷达测量的数据是连续并来自同一目标物，本发明设计了目标的测量连续性判断方法，能对同一目标物进行连续跟踪测量，去掉由于雷达个别数据丢失带来的对整个滤波过程的干扰，并能防止道路环境等因素对系统的干扰，具体步骤如下：Next, judge the continuity of the measurement of the target object. The operating environment of the actual vehicle is relatively complicated, and it cannot be guaranteed that the data measured by the radar is continuous and comes from the same target object. Carry out continuous tracking measurement, remove the interference to the entire filtering process caused by the loss of individual radar data, and prevent the interference of the system from factors such as the road environment. The specific steps are as follows:

6)计算观测值的一步预报估计Z_ny：Z_ny＝H·X_ny (22)6) Calculate the one-step forecast estimate Z _ny of the observed value: Z _ny =H·X _ny (22)

7)与X_ny相应的方差矩阵7) Variance matrix corresponding to X _ny

$S S : : S S = = H h \cdot \cdot {P P}_{ny no} \cdot \cdot {H h}^{' '} + + {σ σ}_{x x}^{22} - - - - - - ((23 twenty three))$

8)定义观测因子：d²＝v′·S^-1·v (24)8) Define the observation factor: d ² =v′·S ^-1 ·v (24)

其中：v＝Z_n-Z_ny (25)Where: v= _Zn - _Zny (25)

9)由于测量误差服从正态分布，因此，d²服从自由度为n_m的x²分布，其中n_m为能测量的目标物信息的个数，例如本发明中，在状态变量X_n中，仅有目标物x向距离信息可以测量，因此n_m＝1。如取显著水平为5％，对于单一观测量的系统，由x²分布查表可得到假设检验的判断准则为：9) Since the measurement error obeys the normal distribution, ^d2 obeys the ^x2 distribution with the degree of freedom being n _m , where n _m is the number of target object information that can be measured, for example, in the present invention, in the state variable X _n , only the x-direction distance information of the target can be measured, so n _m =1. If the significance level is 5%, for a single observation system, the judgment criterion for hypothesis testing can be obtained from the ^x2 distribution lookup table:

d²≤3.841 (26)d ² ≤3.841 (26)

如果新的测量结果使式(26)成立，则可判断最新测量值与上次测量值来自同一目标的概率大于等于95％，可以认为通过滤波估计获得的目标物信息是准确的，结果正常输出；如新的测量值不能满足式(26)，则判断新的测量值不是来自跟踪目标或出现了雷达对目标物测量信息丢失的概率为95％，按照小概率事件为不可能事件的假设，可以认为此时通过滤波估计获得的目标车辆信息是不准确的，不能以此作为目标车辆信息的最优估计。If the new measurement result makes formula (26) valid, it can be judged that the probability that the latest measurement value and the last measurement value come from the same target is greater than or equal to 95%. It can be considered that the target information obtained through filtering estimation is accurate, and the result is normally output If the new measured value cannot satisfy formula (26), then it is judged that the new measured value does not come from the tracking target or the probability that the radar loses the target object measurement information occurs is 95%. According to the assumption that the small probability event is an impossible event, It can be considered that the target vehicle information obtained through filtering estimation at this time is inaccurate and cannot be used as the optimal estimate of the target vehicle information.

由于车辆的颠簸及摆动带来的雷达对目标物测量信息的丢失只是暂时的，为保持目标物信息输出结果的连续性，按照卡尔曼滤波假设，可以认为在目标物测量信息丢失的这一段时间内，目标物运动状态维持不变，得到目标物状态的计算方法如式(27)所示。The loss of radar target measurement information due to vehicle bumps and swings is only temporary. In order to maintain the continuity of the target information output, according to the Kalman filter assumption, it can be considered that during the period when the target measurement information is lost , the motion state of the target remains unchanged, and the calculation method to obtain the state of the target is shown in formula (27).

${\overset{\cdot &Center Dot;}{x x}}_{n no} = = {\overset{\cdot &Center Dot;}{x x}}_{((n no - - 11))} + + {\overset{\cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} T T + + \frac{11}{22} {\overset{\cdot &Center Dot; \cdot &Center Dot; \cdot &Center Dot;}{x x}}_{((n no - - 11))} {T T}^{22} - - - - - - ((2727))$

n＝n+1，返回第1)步。n=n+1, return to step 1).

如连续不能满足式(26)的测量次数超过某一预定值e时(e的取值依据目标物的运动性质和测量系统的采样频率决定，如本发明中，对于车载环境，可取e为1秒内的测量次数，即e＝10)，说明这种不满足不是由于雷达对目标车辆测量信息的暂时丢失引起的，而是有新的目标车辆出现，此时，将最新的测量目标物设为跟踪目标物，其测量信息作为目标物的状态初值(最新目标物x向距离的测量值作为X_ne中x_n的值，X_ne中其他状态值设为0)，将P_ne设为单位阵，开始对新的目标物进行跟踪测量。When the number of times of measurements that cannot satisfy formula (26) exceeds a certain predetermined value e (the value of e is determined according to the motion properties of the target object and the sampling frequency of the measurement system, as in the present invention, for the vehicle environment, e can be taken as 1 second, that is, e=10), indicating that this dissatisfaction is not caused by the temporary loss of the radar’s measurement information of the target vehicle, but the appearance of a new target vehicle. At this time, set the latest measurement target In order to track the target, its measurement information is used as the initial state value of the target (the measured value of the latest x-direction distance of the target is the value of x _n in X _ne , and other state values in X _ne are set to 0), and P _ne is set to Unit array, start to track and measure the new target.

图6为利用本发明的方法进行实车实验的一段实验数据。其中，图6(a)是实车实验的距离数据，为便于对比，图中分别给出了滤波前的原始测量值和滤波后的雷达输出值；601是实验数据局部放大图；图6(b)是相对速度实车实验数据，其中的相对速度真实值是利用微波测速仪测得的；6(c)是本发明所获得的方位角实车实验数据，单位为度。Fig. 6 is a section of experimental data of a real vehicle experiment using the method of the present invention. Among them, Figure 6(a) is the distance data of the real vehicle experiment. For the convenience of comparison, the original measured value before filtering and the radar output value after filtering are respectively shown in the figure; 601 is a partial enlarged view of the experimental data; Figure 6( b) is the real vehicle experimental data of relative speed, wherein the real value of relative speed is measured by microwave velocimeter; 6(c) is the real vehicle experimental data of azimuth angle obtained by the present invention, and the unit is degree.

从图6可见，利用本发明所提的卡尔曼滤波算法，可以去除测量噪声及系统噪声对测量结果的影响，获得准确、实时的目标物距离和相对速度信息。It can be seen from Fig. 6 that by using the Kalman filter algorithm proposed in the present invention, the influence of measurement noise and system noise on the measurement results can be removed, and accurate and real-time target distance and relative speed information can be obtained.

图7为利用本发明的方法进行实车实验的另一段测量结果，实验时由于路面的颠簸，出现了雷达测量数据的暂时丢失。其中，图7(a)为雷达原始测量数据与经信号处理后的雷达输出数据的对比；图7(b)为相对速度的实验结果；图7(c)为实车实验过程中，用于测量连续性判断的观测因子的值。Fig. 7 is the measurement result of another section of the actual vehicle experiment by using the method of the present invention. During the experiment, due to the bumpy road surface, the radar measurement data was temporarily lost. Among them, Fig. 7(a) is the comparison between the original radar measurement data and the radar output data after signal processing; Fig. 7(b) is the experimental result of relative speed; Measures the value of the observation factor for continuity judgments.

从图7(a)可见，虽然雷达测量原始数据存在短暂的丢失，但雷达输出数据保持了较好的连续性。从图7(b)可见，雷达装置输出的相对速度值并没有受雷达数据暂时丢失的影响。从图7(c)可见，利用本发明的目标物测量连续性判断方法，能够及时发现雷达对目标物测量数据的丢失。图7显示，利用本发明所设计的目标物测量连续性判断方法，能够有效的去除由于雷达数据的暂时丢失带来的影响，验证了本发明的有益效果。It can be seen from Fig. 7(a) that although the original radar measurement data is temporarily lost, the radar output data maintains a good continuity. It can be seen from Fig. 7(b) that the relative velocity value output by the radar device is not affected by the temporary loss of radar data. It can be seen from Fig. 7(c) that by using the method for judging the continuity of target measurement of the present invention, the loss of radar measurement data of the target can be found in time. Fig. 7 shows that the method for judging the continuity of target measurement designed in the present invention can effectively remove the influence caused by the temporary loss of radar data, which verifies the beneficial effect of the present invention.

Claims

1. The method for detecting the object in front of the vehicle is characterized by comprising the following steps executed by a vehicle processor in sequence:

1) initialization:

given radar measurement system range measurement noise variance σ_r ²Angle measurement noise variance σ_θ ²System noise variance Q_uThe initial value X of the system state vector_0eInitial value P of covariance matrix of system state estimation error_0e；

2) Measuring the distance D of the target_nAnd orientationAngle A_nWherein, the subscript n represents the number of measurements and calculations, and its initial value is 1; the current measured values are indicated by the corresponding amounts with the subscript n;

3) calculating the current observed value Z of the distance of the target object in the driving direction of the vehicle, namely the x direction_n：

Z_n＝D_ncosA_n

4) Calculating the noise variance of the current x-direction distance measurement:

σ_{x}^{2} = σ_{r}^{2} {\cdot \cos}^{2} A_{n} + {D_{n}}^{2} \cdot σ_{θ}^{2} \sin^{2} A_{n}

5) and carrying out filtering calculation according to the following fourth-order Kalman filtering recursion formula:

X_{ny} = F \cdot X_{(n - 1) e}

P_{ny} = F \cdot P_{(n - 1) e} \cdot F^{'} + G \cdot Q_{u} \cdot G^{'}

K_{n} = P_{ny} \cdot H^{'} \cdot {(H \cdot P_{ny} \cdot H^{'} + σ_{x}^{2})}^{- 1}

P_ne＝(I-K_n·H)·P_ny

X_ne＝X_ny+K_n·(Z_n-H·X_ny)

wherein,

X_{ne}^{'} = [x_{n}, {\overset{\cdot}{x}}_{n}, {\overset{\cdot \cdot}{x}}_{n}, {\overset{\cdot \cdot \cdot}{x}}_{n}]

the dynamic optimal estimate representing the current target information is a matrix transposed symbol, x_nRepresents an optimal estimate of the x-direction distance value of the object,

an optimal estimate representing the relative velocity between the vehicle and the target object; x_nyFor one-step forecast estimation of target object information, F is a system matrix:

F = [\begin{matrix} 1 & T & T^{2} / 2 & T^{3} / 6 \\ 0 & 1 & T & T^{2} / 2 \\ 0 & 0 & 1 & T \\ 0 & 0 & 0 & 1 \end{matrix}]

t is system sampling time; p_neIs the current system state estimation error covariance matrix, G is the system error impact matrix,

G = [\begin{matrix} 0 \\ 0 \\ 0 \\ T \end{matrix}]

Q_uis the system noise variance; p_nyPredicting an estimation error covariance matrix for the system state in one step; k_nIs a Kalman filter gain matrix; h is an observation matrix, and H ═ 1000](ii) a I is a unit array;

6) store P_ne、X_neFor continued recursive computation and output; store P_ny、X_nyA value of (d) for measurement continuity judgment calculation;

7) and (3) judging the continuity of measurement:

7.1) calculation of a one-step predictive estimate of the observed value Z_ny：

Z_ny＝H·X_ny

7.2) with X_nyThe corresponding variance matrix S:

S = H \cdot P_{ny} \cdot H^{'} + σ_{x}^{2}

7.3) defining the observation factors: d²＝v′·S^-1·v

Wherein: v ═ Z_n-Z_ny

7.4) according to d²Judging whether the probability that the latest measured value and the last measured value come from the same target is equal to or exceeds a threshold value or not, if so, judging that the target object information obtained through filtering estimation is accurate, outputting the current distance, relative speed and azimuth angle, and returning to step 2) for next measurement;

if the number of times of the target object measurement is less than the threshold value, the radar is indicated to lose the target object measurement information, or the measurement value comes from another target object, and whether the number of times of the target object measurement is less than the threshold value continuously exceeds a preset value e needs to be further judged:

if the target object state does not exceed the preset value e, assuming that the target object motion state is kept unchanged in the period of time when the target object measurement information is lost, and obtaining the calculation method of the target object state according to the Kalman filtering assumption as follows:

x_{n} = x_{(n - 1)} + {\overset{\cdot}{x}}_{(n - 1)} T + \frac{1}{2} {\overset{\cdot \cdot}{x}}_{(n - 1)} T^{2} + \frac{1}{6} {\overset{\cdot \cdot \cdot}{x}}_{(n - 1)} T^{3}

{\overset{\cdot}{x}}_{n} = {\overset{\cdot}{x}}_{(n - 1)} + {\overset{\cdot \cdot}{x}}_{(n - 1)} T + \frac{1}{2} {\overset{\cdot \cdot \cdot}{x}}_{(n - 1)} T^{2}

{\overset{\cdot \cdot}{x}}_{n} = {\overset{\cdot \cdot}{x}}_{(n - 1)} + {\overset{\cdot \cdot \cdot}{x}}_{(n - 1)} T

{\overset{\cdot \cdot \cdot}{x}}_{n} = {\overset{\cdot \cdot \cdot}{x}}_{(n - 1)}

outputting the optimal estimation x of the distance of the current target object in the x direction_nRelative velocity of

Is estimated optimally, and the azimuth angle A_n(ii) a Returning to the step 2) to perform next measurement;

if the value exceeds a preset value e, a new target object is considered to appear, the latest measurement target object is set as a tracking target object, the measurement information of the tracking target object is taken as the initial state value of the target object, namely the measurement value of the X-direction distance of the latest target object is taken as X_neIn x_nValue of (A), X_neSetting other state values to 0, and setting P_neSetting as a unit array, returning to the step 2) and starting to track and measure a new target object.

2. The method for detecting an object ahead of a vehicle according to claim 1, wherein in the step 7.4), the criterion d is set²The threshold value of the probability value of the latest measured value and the last measured value from the same target is judged to be 95 percent according to the formula d²< 3.841 it is determined whether the latest measurement value and the last measurement value are from the same target.