基于扩展卡尔曼滤波的横向速度估计

doi:10.13232/j.cnki.jnju.2021.05.017

基于扩展卡尔曼滤波的横向速度估计

崔凡, 张兴敢^,, 柏业超

南京大学电子科学与工程学院，南京，210023

Lateral velocity estimation based on Extended Kalman Filter

Cui Fan, Zhang Xinggan^,, Bai Yechao

School of Electronic Science and Engineering, Nanjing University, Nanjing, 210023, China

通讯作者: E⁃mail：zhxg@nju.edu.cn

收稿日期: 2021-01-27 网络出版日期: 2021-09-29

基金资助:

国家自然科学基金. 61976113

Received: 2021-01-27 Online: 2021-09-29

摘要

雷达在探测运动目标时，能够通过目标运动产生的多普勒频移估计目标的径向速度，但目标的横向速度难以估计.提出一种基于扩展卡尔曼滤波的横向速度估计方法，该方法基于毫米波波段的线性调频连续波雷达和MIMO（Multiple Input Multiple Output）天线，将回波信号与本振信号混频得到差频信号.对差频信号进行傅里叶变换得到快时间维的距离数据.重复发射、接收和处理得到慢时间维数据.对快时间维和慢时间维数据进行扩展卡尔曼滤波，估计目标在笛卡儿坐标系下的位置、速度和速度矢量与雷达观测方向的夹角，计算得到目标横向速度.仿真结果表明，该方法能够有效降低噪声影响，快速收敛，准确估计目标的横向速度.

关键词： 横向速度 ; 扩展卡尔曼滤波 ; 毫米波雷达 ; 线性调频连续波

Abstract

The radar estimates the radial speed of the target through the Doppler shift generated by the target,but the lateral speed of the target is difficult to be estimated. Here,a lateral velocity estimation method based on EKF (Extended Kalman Filter) is proposed. The LFMCW (Linear Frequency Modulated Continuous Wave) radar based on MIMO (Multiple Input Multiple Output) antennas is used. The echo signal and the local oscillator signal is mixed to obtain the beat signal. Fourier transform is then performed on the beat signal to obtain the distance data of the fast time dimension. The data of slow time dimension is obtained by repeatedly transmitting,receiving and processing according to the previous step. EKF is applied to the data of fast and slow dimension to estimate the position,the velocity and the angle between the velocity vector and the radar observation direction in the Cartesian coordinate system. And the lateral velocity can be calculated. The simulation shows that this method effectively reduces the observation noise,converges quickly and accurately estimates the lateral velocity of the target.

Keywords： lateral velocity ; Extended Kalman Filter ; millimeter wave radar ; Linear Frequency Modulated Continuous Wave

PDF (626KB) 元数据多维度评价相关文章导出 EndNote| Ris| Bibtex 收藏本文

本文引用格式

崔凡, 张兴敢, 柏业超. 基于扩展卡尔曼滤波的横向速度估计. 南京大学学报（自然科学）[J], 2021, 57(5): 864-869 doi:10.13232/j.cnki.jnju.2021.05.017

Cui Fan, Zhang Xinggan, Bai Yechao. Lateral velocity estimation based on Extended Kalman Filter. Journal of nanjing University[J], 2021, 57(5): 864-869 doi:10.13232/j.cnki.jnju.2021.05.017

车载毫米波雷达与超声波雷达、激光雷达等传统雷达比较，具有体积小、分辨率高、抗干扰能力强等优势，被广泛应用于高级驾驶辅助系统、自适应巡航等领域^［1］.日常驾驶环境中，附近车辆的速度是雷达探测的重要参数之一，为了还原附近目标车辆的真实运动状态，仅估计其径向速度是不够的，还需要估计目标车辆的横向速度^［2］.Zhang and Bai^［3］利用目标回波信号二次项相位系数估计目标横向速度，其中轨道拟合法对解调要求较高，可能存在一次项干扰，而相位差分法在信噪比低时，效果也不理想.Folster and Rohling^［4］分析了同一目标车辆不同散射点的回波信号中多普勒频率的关系，从而估计横向速度，但这对雷达的角度分辨率提出了过高的要求.

本文分析了雷达测量原理，给出了基于扩展卡尔曼滤波的横向速度估计方法，针对线性调频连续波（Linear Frequency Modulated Continuous Wave，LFMCW）毫米波雷达进行仿真.仿真雷达工作频率为77 GHz^［5］，MIMO（Multiple Input Multiple Output）工作方式，包括两个发射天线（TX）和四个接收天线（RX）^［6］.仿真结果验证了上述方法的有效性.

1 雷达测距和测速原理

1.1　距离测量

目标静止时，信号传输时延 $τ$ 与目标距离 $R$ 和电磁波传播速度c的关系为：

τ = \frac{2 R}{c}

（1）

发射信号与回波信号混频得到差频信号^［7］，其频率 $f_{B}$ 为：

f_{B} = S τ

（2）

其中，S为调频斜率.由式（1）和式（2）可得目标距离为：

R = \frac{f_{B} * c}{2 S}

（3）

因此对差频信号进行快速傅里叶变换（Fast Fourier Transform，FFT），即可得到差频信号的频率，计算目标的距离.因为差频频率与目标距离呈线性关系，故此次变换也称为距离FFT.同时因为距离FFT是在一个线性调频脉冲（chirp）对应的采样点上进行，故距离维也称快时间维.

1.2　径向速度测量

目标运动时，为了测量目标径向速度，需要连续发送两个以上的chirp^［8］，目标距离的变化会导致相邻两个chirp的回波时延变化，故相邻chirp对应的差频信号会产生相位差^［9-11］：

∆ φ_{v} = 2 π f_{0} ∆ τ = \frac{4 π f_{0} T_{c} v}{c}

（4）

其中， $f_{0}$ 为雷达工作的中心频率， $∆ τ$ 为相邻两个chirp的回波时延之差， $T_{c}$ 为chirp重复周期， $v$ 为目标的运动速度.因此，目标运动产生的多普勒频移为：

f_{d} = \frac{2 v f_{0}}{c}

（5）

对于同一个运动目标，不同chirp对应的差频信号经过距离FFT之后，会在相同频率处产生峰值，但是峰值对应的相位却在发生变化，故可在距离FFT之后，在不同chirp对应的峰值处进行第二次FFT，得到多普勒频移，进而由式（5）计算目标径向速度.故第二次FFT称为多普勒FFT，又因此次FFT是在不同chirp对应的采样点上进行，故速度维也称为慢时间维.

距离⁃多普勒FFT之后，使用二维恒虚警检测技术^［12］可有效检测目标反射点，通过聚类算法^［13-14］将同一目标的反射点归为一个簇，即可得到目标的距离和径向速度的观测结果.

2 横向速度估计方法

卡尔曼滤波器（Kalman Filter，KF）是在最小均方误差准则下的最佳线性滤波器，将被估计的信号视为在高斯白噪声作用下的线性系统的输出，且其输出是在时域由状态转移方程、观测方程及白噪声作用下给出，故对于非平稳随机信号同样适用^［15］.其计算过程是在不断地“预测⁃修正”，每次迭代只需前一时刻的估计值和当前时刻的观测值便可计算当前时刻的状态估计值，无需存储大量数据.但在大多数情况下，状态方程或观测方程可能并非线性关系.当非线性函数在工作点附近存在各阶导数或偏导数，且迭代时工作点仅在小范围内变化时，一种解决非线性的办法就是在状态估计值处利用泰勒（Taylor）级数将非线性方程展开，忽略其二阶及以上的部分，得到近似线性的模型，通过KF获得状态估计.这种方法就是扩展卡尔曼滤波^［16-17］.

日常驾驶环境中，在观测的一小段时间内，可认为目标车辆以匀速直线的状态行驶，采用匀速直线模型，不考虑模型中加速度等控制量的输入，则矢量形式的非线性状态方程和非线性观测方程可以表示为：

S (k) = f (S (k - 1)) + W (k)

（6）

Z (k) = h (S (k)) + V (k)

（7）

其中，过程噪声 $W (k)$ 和观测噪声 $V (k)$ 为彼此独立的零均值高斯白噪声.

将状态方程在 $k$ -1时刻的状态估计值 $\hat{S} (k - 1| k - 1)$ 处做一阶Taylor展开，并忽略高阶部分的线性化的状态方程：

\begin{array}{l} S (k) \approx f (\hat{S} (k - 1| k - 1)) + \\ {\frac{\partial f}{\partial S (k - 1)}|}_{S (k - 1) = \hat{S} (k - 1| k - 1)} \\ [S (k - 1) - \hat{S} (k - 1| k - 1)] + W (k) \end{array}

（8）

令

F (k - 1) = {\frac{\partial f}{\partial S (k - 1)}|}_{S (k - 1) = \hat{S} (k - 1| k - 1)}

（9）

则 $F (k - 1)$ 就是非线性函数 $f (x)$ 在 $\hat{S} (k - 1| k - 1) 处$ 对应的Jacobi矩阵^［18］，线性化的状态方程为：

\begin{array}{l} S (k) = F (k - 1) S (k - 1) + \\ \{f (\hat{S} (k - 1| k - 1)) F (k - 1) \hat{S} (k - 1| k - 1)\} + \\ W (k) \end{array}

（10）

同理，将观测方程在 $k$ 时刻的状态预测值 $\hat{S} (k| k - 1)$ 处做一阶Taylor展开：

\begin{array}{l} Z (k) \approx h (\hat{S} (k| k - 1)) + \\ {\frac{\partial h}{\partial S (k)}|}_{S (k) = \hat{S} (k| k - 1)} [S (k) - \hat{S} (k| k - 1)] \end{array}

（11）

令

H (k) = {\frac{\partial h}{\partial S (k)}|}_{S (k) = \hat{S} (k| k - 1)}

（12）

则 $H (k)$ 就是非线性函数 $h (x)$ 在 $\hat{S} (k| k - 1)$ 处对应的Jacobi矩阵，线性化的观测方程为：

\begin{array}{l} Z (k) = H (k) S (k) + \\ [h (\hat{S} (k| k - 1)) - H (k) \hat{S} (k| k - 1)] + V (k) \end{array}

（13）

为了估计目标横向速度 $v_{l} (k)$ ，构建笛卡儿坐标系下的目标状态向量为：

S (k) = {[x (k), y (k), v_{x} (k), v_{y} (k), θ (k)]}^{T}

如图1所示，假设雷达处于坐标原点，x轴和y轴分别代表正东和正北方向， $x (k)$ 和 $y (k)$ 表示k时刻目标的位置坐标， $v_{x} (k)$ 和 $v_{y} (k)$ 表示k时刻目标速度矢量 $v (k)$ 的x轴和y轴分量， $θ (k)$ 表示k时刻目标速度矢量与雷达观测方向的夹角.则目标的横向速度为：

v_{l} (k) = v (k) \cdot s i n (θ (k))

（14）

图1

新窗口打开| 下载原图ZIP| 生成PPT

图1 k时刻目标状态示意图

Fig.1 Target state at time k

经过距离⁃多普勒FFT，由式（3）和式（5），可以得到观测向量为：

Z (k) = {[r (k), v_{r} (k)]}^{T}

如图2所示， $r (k)$ 表示k时刻目标与雷达的距离， $v_{r} (k)$ 表示 $k$ 时刻目标与雷达的径向相对速度，目标接近雷达时为正，远离雷达时为负.

图2

新窗口打开| 下载原图ZIP| 生成PPT

图2 k时刻雷达观测示意图

Fig.2 Radar observation at time k

假设相邻时刻时间间隔为 $∆ t$ ，忽略噪声的影响，状态向量 $S (k)$ 可由 $S (k - 1)$ 计算：

S (k) = [\begin{matrix} x (k) \\ y (k) \\ v_{x} (k) \\ v_{y} (k) \\ θ (k) \end{matrix}] = f (S (k - 1)) = [\begin{matrix} x (k - 1) + v_{x} (k - 1) ∆ t \\ y (k - 1) + v_{y} (k - 1) ∆ t \\ v_{x} (k - 1) \\ v_{y} (k - 1) \\ a r c t a n (\frac{v_{y} (k - 1)}{v_{x} (k - 1)}) - a r c t a n (\frac{y (k - 1) + v_{y} (k - 1) ∆ t}{x (k - 1) + v_{x} (k - 1) ∆ t}) \end{matrix}]

（15）

观测向量 $Z (k)$ 与状态向量 $S (k)$ 关系如下：

\begin{array}{l} Z (k) = [\begin{matrix} r (k) \\ v_{r} (k) \end{matrix}] = h (S (k)) = \\ [\begin{matrix} \sqrt[]{x {(k)}^{2} + y {(k)}^{2}} \\ \sqrt[]{v_{x} {(k)}^{2} + v_{y} {(k)}^{2}} ∙ c o s [θ (k)] \end{matrix}] \end{array}

（16）

假设过程噪声 $W (k)$ 和观测噪声 $V (k)$ 的协方差矩阵分别为 $Q$ 和 $R$ ，k时刻的先验估计协方差为 $P (k| k - 1) (k| k - 1)$ ，后验估计协方差为 $P (k| k)$ ，那么扩展卡尔曼滤波的一次迭代过程如下.

状态预测：

\hat{S} (k| k - 1) = f (\hat{S} (k - 1| k - 1))

（17）

先验估计协方差矩阵：

\begin{array}{l} P (k| k - 1) = \\ F (k - 1) P (k - 1| k - 1) F^{T} (k - 1) + Q \end{array}

（18）

卡尔曼增益矩阵：

\begin{array}{l} K (k) = \\ P (k| k - 1) H^{T} (k) {[H (k) P (k| k - 1) H^{T} (k) + R]}^{- 1} \end{array}

（19）

修正状态预测值，得到后验状态估计值：

\begin{array}{l} \hat{S} (k| k) = \hat{S} (k| k - 1) + \\ K (k) [Z (k) - h (\hat{S} (k| k - 1))] \end{array}

（20）

计算后验估计协方差矩阵：

P (k| k) = (I - K (k) H (k)) P (k| k - 1)

（21）

其中，状态转移矩阵 $F$ 和观测矩阵 $H$ ，如式（9）和式（12）所示，分别为非线性函数 $f (x)$ 和 $h (x)$ 的Jacobi矩阵.经过四至五次迭代，EKF即可快速收敛，由式（14）准确估计目标的横向速度.

3 仿真分析

仿真中雷达发射波形中心频率 $f_{0}$ 为 $77 G H z$ ，调频带宽 $B$ 为 $1.5 G H z$ ，chirp周期 $T_{c}$ 为 $20 μ s$ ，采用时分复用（Time⁃Division Multiplexing，TDM）发射方式.为了验证横向速度估计方法的有效性，以雷达为坐标系原点模拟一个匀速直线运动的目标，目标相对于雷达的距离、方位和速度及速度的方向见表1，其中，速度为目标相对于雷达的运动速度，速度方向为速度矢量与x轴正向夹角.可见，此时目标速度与雷达径向夹角 $θ$ 不为零，运动速度不仅包含了径向速度分量，还有横向速度分量.

表1 模拟目标参数

Table 1 Target parameters of simulation

参数	径向距离	方位	速度	速度方向
数值	20 m	20°	58.31 km·h^-1	59.04°

新窗口打开| 下载CSV

设目标观测时间为5 s，观测时间间隔 $∆ t$ 为0.1 s，进行50次迭代.在EKF处理过程中，通常依据第一次观测设定系统初始状态 $\hat{S} (0| 0)$ ，将后验估计协方差矩阵初始值 $P (0| 0)$ 设定为对角阵，从而启动整个滤波过程.目标的真实状态和EKF估计结果如图3所示.

图3

新窗口打开| 下载原图ZIP| 生成PPT

图3 目标真实状态与EKF估计结果

Fig.3 The real state of the target and the EKF estimation result

目标位置，速度及速度矢量与雷达观测方向夹角 $θ$ 的均方根误差如图4所示.

图4

新窗口打开| 下载原图ZIP| 生成PPT

图4 EKF估计误差

Fig.4 EKF estimation errors

可见由于系统初始状态 $\hat{S} (0| 0)$ 和后验估计协方差矩阵初始值 $P (0| 0)$ 的设定与真实值存在一定差异，故在0~0.5 s内目标状态估计存在明显误差，但经过四至五次迭代，EKF能够快速收敛，准确估计目标运动状态.据此可以估计目标的横向速度，横向速度的估计值与真实值如图5所示.横向速度的均方根误差如图6所示.

图5

新窗口打开| 下载原图ZIP| 生成PPT

图5 横向速度估计

Fig.5 Lateral velocity estimation

图6

新窗口打开| 下载原图ZIP| 生成PPT

图6 横向速度估计误差

Fig.6 Lateral velocity estimation error

在横向速度估计过程中，EKF系统初值的设定引入了误差，在0~0.5 s内横向速度估计误差较大，但经过四至五次迭代即可收敛，可见基于EKF的横向速度估计，估计精度高，收敛速度快.

4 结论

本文分析了LFMCW毫米波雷达测量目标距离和径向速度的原理，基于扩展卡尔曼滤波，估计目标在笛卡儿坐标系下的位置、速度和速度矢量与雷达观测方向的夹角，进而估计目标横向速度.仿真结果表明：此方法能够准确、实时且快速地估计目标横向速度.

参考文献

原文顺序

文献年度倒序

文中引用次数倒序

被引期刊影响因子

[1]

Menzel

Millimeter⁃wave radar for civil applications

∥The 7^th European Radar Conference. Paris,France：IEEE,2010：89-92.