基于车载毫米波雷达的前方物体运动状态估计及分类方法

技术领域

本发明申请属于雷达技术领域，涉及前方物体运动状态估计及分类，可用 于先进驾驶员辅助系统。

背景技术

近年来，先进驾驶员辅助系统ADAS已成为汽车安全技术的研究热点。目 前其主要包含自适应巡航控制系统、车道偏离预警系统、前方防碰撞预警系统 等。先进驾驶员辅助系统及雷达和计算机视觉等先进信息传感技术作为基础， 提高了驾驶员的驾驶舒适性和车辆行驶安全性。

毫米波雷达被广泛应用于驾驶员辅助系统中，用于测量前方中远距离内的 车辆、障碍物等目标。对于连续波毫米波雷达，其测速原理基本是：发射机产 生连续高频等幅波，其频率在时间上进行周期性变化。在雷达波传播至目标再 经目标反射返回天线的这段时间内，此时发射机的频率相较回波频率已经有了 变化，因此在混频器输出端变出现了差频电压。该差频电压直接与雷达和前方 目标之间的相对距离相关。而当两者之间相对速度不为0，由于多普勒效应， 回波频率与发射机频率在前述由相对距离引起的差频的基础上还会有频率差值 变化，而该差频直接与两者相对速度有关。但是，相对距离与相对速度的直接 测量基于车载毫米波雷达坐标系，直接测量的前方物体运动状态无法直接用于 驾驶辅助系统，因此需要对前方物体运动状态进行实时准确地估计。

汽车状态参数估计早已广泛用于汽车各类控制系统当中。早期在汽车稳定 性控制系统(Electronic Stability Program，ESP)研究中，利用低成本的轮 速、横摆角速度等车辆信息，再利用卡尔曼滤波估计算法，对汽车质心侧偏 角、路面附着系数等难以直接测量或测量成本较高的车辆信息进行估计，并用 于稳定性控制系统。除了使用经典卡尔曼滤波估计算法，还引入了粒子滤波、 自适应卡尔曼滤波等状态估计算法。因此，本申请在雷达直接测量的前方物体 运动状态信息的基础上，利用自适应卡尔曼滤波估计算法对前方物体基于大地 坐标的运动状态进行实时准确估计。

现有雷达前方物体分类大多是根据其属性进行分类，如分成轮式车辆、履 带车辆、行人、树木等。

发明内容

本发明的目标在于克服上述已有技术的不足，提出一种基于车载毫米波雷 达的前方物体运动状态估计及分类方法，以提高前方物体运动状态的估计及分 类准确性。

本发明的上述第一个目的得以实现的技术方案是：基于车载毫米波雷达的 前方物体运动状态估计方法，其特征在于：基于车载毫米波雷达所直接测量的 有限前方物体运动的侧向速度信息，建立前方物体在大地坐标系下的运动方 程，利用自适应卡尔曼滤波估计算法，实时准确地估计前方物体运动状态。

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

I、通过车载毫米波雷达映射前方物体在大地坐标系下的运动方程为：

$\left(\begin{array}{c}{x}_{o\stackrel{··}{b}\mathrm{jR}}\left(t\right)=[a-a_v]\\ x_{o\stackrel{·}{b}\mathrm{jR}}\left(t\right)=[\stackrel{·}{x}\left(0\right)-{\stackrel{·}{x}}_v\left(0\right)]+[a-a_v]t\\ x_{\mathrm{objR}}\left(t\right)=[x\left(0\right)-x_R\left(0\right)]+[\stackrel{·}{x}\left(0\right)t-{\stackrel{·}{x}}_v\left(0\right)t]+[\frac{1}{2}{\mathrm{at}}^2-\frac{1}{2}{a}_v{t}^2],\end{array}\right)$

上式中，为前方物体的加速度，为前方物体的速度， x_{obj_R}(t)为前方物体的距离；

II、表征前方物体的运动方程：

$\stackrel{·}{x}\left(t\right)=\mathrm{diag}[\Lambda ,\Lambda ]x\left(t\right)+\mathrm{diag}[B,B]w\left(t\right),$

$\Lambda =\left(\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right),B=\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right),$

其中Λ为单一坐标轴上物体运动状态系统矩阵；B为过程噪声矩阵； w(t)＝[w_x(t)，w_y(t)]^T，w_x(t)～N(0，σ_wx²)，w_y(t)～N(0，σ_wy²)是相互独立的随机白噪声过程；

前方物体侧纵向运动方程的离散时间模型为：

x_k+1＝diag[Φ，Φ]x_k+diag[G，G]w_k

前方物体运动状态的观测方程为：

z(t)＝Cx(t)+v(t)

$C=\left(\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right),$

其中，z(t)为观测矩阵；C为输出状态矩阵； v(t)＝[v_x(t)，v_x(t)，v_y(t)]^T，v(t)～N(0，R)为高斯白噪声过程；

前方物体运动状态观测方程离散时间模型为：

z_k＝Cx_k+v_k，其中，z_k为观测向量；v_k为高斯白噪声序列；

III、以前方物体运动状态方程的离散时间方程为滤波器的状态方程，以前 方物体运动状态观测方程的离散时间方程为滤波器的观测方程，采用自适应卡 尔曼滤波估计算法，对前方物体的运动状态进行准确实时估计，自适应卡尔曼 滤波估计算法包括预测、校正和噪声估计三个过程，具体过程如下：

一、预测过程：

状态预测方程：其中，x(k)为k时刻的状态向量 和测量向量，A为系统状态转移矩阵，q(k)为系统噪声的均值；

误差协方差预测方程：p(k+1|k)＝Ap(k|k)A^T+Q(k)，其中，P为预测协方差 矩阵；

中间变量：$\left(\begin{array}{c}ϵ(k+1)=y(k+1)-H\hat{x}(k+1|k)-\hat{r}\left(k\right)\\ d\left(k\right)-(1-b)/\left({1-b}^{k+1}\right)\end{array}\right),$其中，y(k)为测量向量，H为 输出状态矩阵，r(k)为观测噪声的均值，b为遗忘因子；

二、校正过程：

增益方程：K(k+1|k)＝P(k+1|k)H^T[HP(k+1|k)H^T+R(k)]^-1，

K_k为卡尔曼增益矩阵，

滤波方程：x(k+1|k+1)＝x(k+1|k)+K(k+1)ε(k+1)，

误差协方差更新方程：P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)，

三、噪声估计过程：

噪声的均值和自协方差矩阵估计方程为：

$\left(\begin{array}{c}\hat{q}(k+1)=[1-d\left(k\right)]\hat{q}\left(k\right)+d\left(k\right)\times [\hat{x}(k+1|k+1)-A\hat{x}\left(k\right|k)]\\ \hat{Q}(k+1)=[1-d\left(k\right)]\hat{Q}\left(k\right)+d\left(k\right)[K(k+1)ϵ(k+1){ϵ}^T(k+1)\times K^T(k+1)+P(k+1|k+1)-\mathrm{AP}\left(k\right|k)A^T]\\ \hat{r}(k+1)=[1-d\left(k\right)\hat{r}\left(k\right)+d\left(k\right)]\times [y(k+1)-H\stackrel{}{\hat{x}(k+1|k)]}\\ \hat{R}(k+1)=[1-d\left(k\right)]\hat{R}\left(k\right)+d\left(k\right)\times [ϵ(k+1){ϵ}^T(k+1)-\mathrm{HP}(k+1|k)H^T]\end{array}\right)$

其中，Q_k为自协方差矩阵。

本发明的上述第二个目的得以实现的技术方案是：在前方物体运动状态估 计的基础上，根据前方物体运动信息，依据所指定的运动状态划分速度阈值和 运动状态转换规则进行分类，所述前方物体运动状态根据现有及历史运动状态 特定分为未分类、静止、同向运动、相向运动、停止但之前同向运动和停止但 之前相向运动几种：

IV、通过定义如下速度阈值，将前方物体运动状态进行分类：

V_t-当前方物体车速等于或高于该阈值时，前方物体被认为是同向运动， 该阈值通过下式在每个控制循环中被动态更新：

V_t＝V_minmoving+V_ego*k₁+a_ego*k₂，其中，V_minmoving为本车速度较低时本车被认为处 于行驶状态的速度阈值，V_ego为当前时刻本车速度，a_ego为当前时刻本车加速 度，k₁和k₂为需要通过实车调试所确定的权值；

V_x-当前方物体车速等于或低于该阈值时，前方物体被认为是相向运动， 该阈被设置为静态参数，当前值为-3m/s。

当前方物体车速在V_t与V_x之间时，其运动状态是静止或停止的进一步判断 需要根据该前方物体的历史运动状态进行；

V、对前方物体运动状态分类与运动状态之间的转换制定相应规则，各状 态之间只能按该规则进行状态转换：

i)、未分类→静止、同向运动、相向运动，如果三个循环内前方物体速度 在范围内且前方物体数据能被稳定测量，则能实现该运动状态转换；

ii)、同向运动→停止但之前同向运动；相向运动→停止但之前同向运 动，前方物体速度连续两个循环内都在0附近，则能实现该运动状态转换；

iii)、静止、停止但之前同向运动、停止但之前相向运动→同向运动、相 向运动，连续三个循环前方物体速度大于0，则能实现该运动状态转换。

本发明前方物体运动状态估计及分类方法的应用实施，其较之于现有技术 突出的效果为：大大提高了运动状态估计的准确性，并指定严格的前方物体运 动状态转换条件，保证了分类准确性。

附图说明

图1为本发明基于车载毫米波雷达前方物体运动状态估计及分类流程框 图。

图2为本发明所采用的自适应卡尔曼滤波估计算法示意图。

图3为本发明前方物体运动状态转换规则示意图。

具体实施方式

本申请对前方物体的分类并非基于其固有属性，直接基于其运动状态将其 分成若干类。可获得的前方物体运动状态包括侧纵向距离、侧纵向速度、侧纵 向加速度、方位角等。考虑到控制系统运算速度及可能出现测量误差，本申请 选用前方物体的纵向速度并设置相应速度阈值来进行分类。同时，制定严格的 前方物体运动状态转换条件，保证分类的准确性。

以下便结合实施例附图，对本发明的具体实施方式作进一步的详述，以使 本发明技术方案更易于理解、掌握。

首先建立前方物体的运动状态模型。车载毫米波雷达的前方物体主要包括 车辆、行人、树木等。这些目标一般情况下无垂直方向运动或其垂直方向运动 速度极小。因此，本发明中所建立的运动状态模型忽略其垂直方向运动，只关 注前方物体在水平面的运动。因此运动状态估计便简化为在大地坐标系下水平 面内前方物体的运动状态估计。当不考虑车载毫米波雷达支架的形变时，可认 为车载毫米波雷达与本车是固连在一起的。因此，首先确定车载毫米波雷达相 对大地坐标系的运动方程为：

$\left(\begin{array}{c}\stackrel{··}{x_R}\left(t\right)=a_v\\ \stackrel{·}{x_R}\left(t\right)=\stackrel{·}{x_v}\left(0\right)+a_{v}t\\ x_R\left(t\right)=x_R\left(0\right)+\stackrel{·}{x_v}\left(0\right)t+\frac{1}{2}{a}_v{t}^2\end{array}\right)---\left(1\right)$

上式中，为车载毫米波雷达的加速度，a_v为本车的加速度，为车载毫 米波雷达的速度，为本车的初始速度，x_R(t)为车载毫米波雷达的安装距 离。

前方目标在大地坐标系下的运动方程为：

$\left(\begin{array}{c}\stackrel{··}{x}\left(t\right)=a\\ \stackrel{·}{x}\left(t\right)=\stackrel{·}{x}\left(0\right)+\mathrm{at}\\ x\left(t\right)=x\left(0\right)+\stackrel{·}{x}\left(0\right)t+\frac{1}{2}{\mathrm{at}}^2\end{array}\right)---\left(2\right)$

上式中，为前方物体的加速度，a为前方物体的加速值，为前方物体的 速度，x(t)为前方物体的距离。

由上述方程推导出前方物体在车载毫米波雷达运动坐标系下的运动方程 为：

$\left(\begin{array}{c}{x}_{o\stackrel{··}{b}\mathrm{jR}}\left(t\right)=[a-a_v]\\ x_{o\stackrel{·}{b}\mathrm{jR}}\left(t\right)=[\stackrel{·}{x}\left(0\right)-{\stackrel{·}{x}}_v\left(0\right)]+[a-a_v]t\\ x_{\mathrm{objR}}\left(t\right)=[x\left(0\right)-x_R\left(0\right)]+[\stackrel{·}{x}\left(0\right)t-{\stackrel{·}{x}}_v\left(0\right)t]+[\frac{1}{2}{\mathrm{at}}^2-\frac{1}{2}{a}_v{t}^2]\end{array}\right)---\left(3\right)$

上式中，为前方物体的加速度，为前方物体的速度，x_{obj_R}(t)为 前方物体的距离。

采用描述前方物体的运动状态，包括其侧纵向距离、速度 与加速度等。前方目标的侧纵向运动方程可以表示为：

$\stackrel{·}{x}\left(t\right)=\mathrm{diag}[\Lambda ,\Lambda ]x\left(t\right)+\mathrm{diag}[B,B]w\left(t\right)$

$\Lambda =\left(\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right),B=\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)---\left(4\right)$

其中Λ为单一坐标轴上物体运动状态系统矩阵；B为过程噪声矩阵；

w(t)＝[w_x(t)，w_y(t)]^T，w_x(t)N(0，σ_wx²)，w_y(t)N(0，σ_wy²)是相互独立的随机白噪声过程。

前方物体侧纵向运动方程的离散时间模型为：

x_k+1＝diag[Φ，Φ]x_k+diag[G，G]w_k      (5)

前方物体运动状态的观测方程为：

z(t)＝Cx(t)+v(t)

$C=\left(\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right)---\left(6\right)$

其中，z(t)为观测矩阵；C为输出状态矩阵；v(t)＝[v_x(t)，v_x(t)，v_y(t)]^T，v(t)～N(0，R)为 高斯白噪声过程。

前方物体运动状态观测方程离散时间模型为：

z_k＝Cx_k+v_k      (7)

其中，z_k为观测向量；v_k为高斯白噪声序列。

针对所建立的前方物体运动状态方程，同时考虑到车载毫米波雷达在内的 各种车载传感器的统计特性难以预先确定，故采用自适应卡尔曼滤波算法对前 方物体运动状态进行实时准确估计。

自适应卡尔曼滤波算法包括预测、矫正和噪声估计三个过程。

参照图2，在预测过程中，基于当前时刻的运动状态，会对下一时刻的运 动状态进行先验估计：

状态预测方程：

$\hat{x}(k+1|k)=A\hat{x}\left(\hat{k}|k\right)+\hat{q}\left(k\right|k)---\left(8\right)$

其中，x(k)为k时刻的状态向量和测量向量，A为系统状态转移矩阵，q(k)为系 统噪声的均值。

误差协方差预测方程：

p(k+1|k)＝Ap(k|k)A^T+Q(k)        (9)

其中，P为预测协方差矩阵。

中间变量：

$\left(\begin{array}{c}ϵ(k+1)=y(k+1)-H\hat{x}(k+1|k)-\hat{r}\left(k\right)\\ d\left(k\right)-(1-b)/\left({1-b}^{k+1}\right)\end{array}\right)---\left(10\right)$

其中，y(k)为测量向量，H为输出状态矩阵，r(k)为观测噪声的均值，b为遗忘 因子。

在矫正过程中，将所观测的运动状态与预先估计的运动状态进行结合，获 得后验估计：

增益方程：

K(k+1|k)＝P(k+1|k)H^T[HP(k+1|k)H^T+R(k)]^-1     (11)

K_k为卡尔曼增益矩阵。

滤波方程：

x(k+1|k+1)＝x(k+1|k)+K(k+1)ε(k+1)     (12)

误差协方差更新方程：

P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)     (13)

在噪声估计过程中，利用估计残差序列估计修正观测噪声和系统噪声协方 差矩阵，实现对前方物体运动状态的准确实时估计：

噪声的均值和自协方差矩阵估计方程：

$\left(\begin{array}{c}\hat{q}(k+1)=[1-d\left(k\right)]\hat{q}\left(k\right)+d\left(k\right)\times [\hat{x}(k+1|k+1)-A\hat{x}\left(k\right|k)]\\ \hat{Q}(k+1)=[1-d\left(k\right)]\hat{Q}\left(k\right)+d\left(k\right)[K(k+1)ϵ(k+1){ϵ}^T(k+1)\times K^T(k+1)+P(k+1|k+1)-\mathrm{AP}\left(k\right|k)A^T]\\ \hat{r}(k+1)=[1-d\left(k\right)\hat{r}\left(k\right)+d\left(k\right)]\times [y(k+1)-H\stackrel{}{\hat{x}(k+1|k)]}\\ \hat{R}(k+1)=[1-d\left(k\right)]\hat{R}\left(k\right)+d\left(k\right)\times [ϵ(k+1){ϵ}^T(k+1)-\mathrm{HP}(k+1|k)H^T]\end{array}\right)---\left(14\right)$

其中，Q_k为自协方差矩阵。

根据前述前方物体在大地坐标系下的运动方程和观测方程，式(5)为卡 尔曼滤波器的状态方程，式(7)为卡尔曼滤波器的量测方程。

滤波器的输入信息包括车载毫米波雷达的直接测量信息，包括前方物体的 相对距离、方位角和相对速度：

z_k＝[x，v_r，y]^T     (15)

有自适应卡尔曼滤波器所估计得到的前方物体运动状态中，选取前方物体 的纵向速度，用于前方物体运动状态分类。

首先计算两个速度阈值。

V_t-当前方物体车速等于或高于该阈值时，前方物体被认为是同向运动。 该阈值通过下式在每个控制循环中被动态更新：

V_t＝V_minmoving+V_ego*k₁+a_ego*k₂    (16)

其中，V_minmoving为本车速度较低时本车被任务在运动的速度阈值，V_ego为当前时刻 本车速度，a_ego为当前时刻本车加速度，k₁和k₂为需要通过实车调试所确定的权 值。

V_x-当前方物体车速等于或低于该阈值时，前方物体被认为是相向运动。 该阈值被设置为静态参数，当前值为-3m/s。

当前方物体车速在这两个速度阈值之间时，其运动状态可能是静止或停 止。进一步判断需要根据该前方物体的历史运动状态。

因此，为了将前方物体进行更准确地分类，需要对前方物体运动状态分类 与运动状态之间的转换制定相应规则。具体来说，如图3所示，当前方物体运 动状态满足下列条件时，能实现相应的运动状态分类：

当前方物体的之前运动状态为未分类即没有足够可用测量数据，同时连续 三个循环其数据能被稳定测量时，若其速度在V_t与V_x之间，其运动状态会转换 为静止；若其速度小于或等于V_x，其运动状态会转换为相向行驶；若其速度大 于或等于V_t，其运动状态会转换为同向行驶。

当前方物体的之前运动状态为同向运动，同时前方物体速度连续两个循环 内都在0附近或在V_t与V_x之间时，其运动状态会转换为停止但之前同向运动。

当前方物体的之前运动状态为相向运动，同时前方物体速度连续两个循环 内都在0附近或在V_t与V_x之间时，其运动状态会转换为停止但之前相向运动。

当前方物体的之前运动状态为静止、停止但之前同向运动或停止之前相向 运动，同时连续三个循环前方物体速度大于V_t，其运动状态会转换为同向运 动。

当前方物体的之前运动状态为静止、停止但之前同向运动或停止之前相向 运动，同时连续三个循环前方物体速度小于V_x，其运动状态会转换为相向运 动。

综上所述，是对本发明具体实施例的详细描述，对本案保护范围不构成任 何限制。凡采用等同变换或者等效替换而形成的技术方法，均落在本发明权利 保护范围之内。