精华内容
下载资源
问答
  • 卡尔曼滤波是自动驾驶领域最常用的数据最优估计算法。人们对它的第一印象往往是它那复杂的线性代数表达式。本文将介绍卡尔曼滤波及其变种扩展卡尔曼滤波的数学原理及代码实现,帮助初学者熟悉和掌握卡尔曼滤波。在...

    Kalman Filter and Extended Kalman Filter

    卡尔曼滤波是自动驾驶领域最常用的数据最优估计算法。人们对它的第一印象往往是它那复杂的线性代数表达式。本文将介绍卡尔曼滤波及其变种扩展卡尔曼滤波的数学原理及代码实现,帮助初学者熟悉和掌握卡尔曼滤波。在实际项目中我们会融合激光雷达和毫米波雷达的测量数据,从而精确地追踪目标的位置和速度。

    Github: https://github.com/williamhyin/CarND-Extended-Kalman-Filter

    Email: williamhyin@outlook.com

    知乎专栏: 自动驾驶全栈工程师

    Kalman Filter

    1. 首先我们需要了解什么是卡尔曼滤波?

    根据维基百科的官方解释,卡尔曼滤波(Kalman Filter)使用随时间推移观察到的一系列测量值(包含统计噪声和其他误差),生成未知变量的估计值,该估计值往往更多通过估计每个时间范围内变量的联合概率分布,结果因此比仅基于单个测量的结果更准确。

    听起来稍微有点复杂,也就是说,卡尔曼滤波器是一种最优估计算法,它能够从一系列的不完全及包含噪声的测量中,估计动态系统的状态。

    1. 其次我们要了解为什么需要卡尔曼滤波?

    使用卡尔曼滤波的根本原因其实是来自传感器的测量结果不是100%可靠,如果毫无误差,或者误差远低于期望阈值,则完全不需要卡尔曼滤波,直接使用测量数据更新下一个状态即可。

    在自动驾驶领域,卡尔曼滤波器广泛被用于通过特征级融合激光雷达和毫米波雷达的数据,估计汽车的位置和速度。 不同的传感器提取得到的测量结果如距离,速度,角度,因为受到信号漂移或噪声的影响,往是不准确的。而卡尔曼滤波为我们提供了一种将来自不同传感器的测量结果与预测位置的数学模型相结合的方法。它根据我们对一个特定传感器或模型的信任程度,来更新测量值和预估值之间的可信权重,从而获得对准确位置的最佳估计。

    1. 卡尔曼滤波的步骤?

    卡尔曼滤波器的操作包括两个阶段:预测更新。在预测阶段,滤波器使用上一状态的估计结果,做出对当前状态的估计。在更新阶段,滤波器利用当前状态的观测值优化在预测阶段获得的预估值,以获得一个当前阶段更精确的新估计值。

    每当我们从传感器获取新数据时,我们都会重复这两个步骤,不断更新位置状态估计值和误差协方差矩阵P的准确性。

    值得注意的是测量更新基于的依据是贝叶斯法则。当我们有了上一时刻的状态向量的先验概率P(x), 又有了测量值的似然P(z|x),似然是指在当前位置下,我们能得到的测量值
    ,在P(z)为一个常数的情况下,我们求目标位置的概率分布其实就是在求后验概率P(x|z)
    P ( x ∣ z ) = P ( z ∣ x ) P ( x ) P ( z ) P(\mathbf{x} | \mathbf{z})=\frac{P(\mathbf{z} | \mathbf{x}) P(\mathbf{x})}{P(\mathbf{z})} P(xz)=P(z)P(zx)P(x)
    预测更新是基于全概率公式
    P ( B ) = ∑ i = 1 n P ( A i ) P ( B ∣ A i ) P(B)=\sum_{i=1}^{n} P\left(A_{i}\right) P\left(B | A_{i}\right) P(B)=i=1nP(Ai)P(BAi)

    在下面的例子中,我们可以用在概率密度函数的帮助下解释卡尔曼滤波的工作原理。

    在最初的时间步骤 k-1,目标的位置是平均位置为 x_k-1的高斯分布。 之后基于运动模型进行预测,得到时刻 k 的 目标位置x_k ,由于预测本身具有较大的不确定性,因此预测的位置结果具有较大的方差。与此同时我们获得来自传感器的测量结果 y ,测量值的不确定性较小,因此测量的位置结果具有较小的方差。卡尔曼滤波器通过将预测和测量两个概率函数相乘,来估计最佳位置。值得注意的是通过数学推导可以发现相乘的结果是一个分布紧密具有更小方差的高斯函数,意味着最终位置估计准确性得到了提高。

    1. 卡尔曼滤波与隐马尔科夫模型的关系?

    卡尔曼滤波建立在线性代数和隐马尔可夫模型(hidden Markov model)上。其基本动态系统可以用一个马尔可夫链表示。它是是一种递归的估计,即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值,因此不需要记录观测或者估计的历史信息。

    卡尔曼向量与矩阵

    卡尔曼滤波器的在预测和测量步骤中相互传递的数值有以下两个:

    • x ^ : 状 态 的 估 计 \hat{\mathbf{x}} : 状态的估计 x^:

    • P : 状 态 误 差 协 方 差 矩 阵 , 度 量 估 计 值 的 精 确 程 度 。 \mathbf{P} :状态误差协方差矩阵,度量估计值的精确程度。 P

    以下是维基百科中对于卡尔曼滤波各个向量与矩阵的数学描述,初看非常复杂,但其实熟悉之后就会发现,你只需要套公式。我会在后续章节中,详细讲解各个向量和矩阵的意义。在下文中Xk-1|k-1用x表示,Xk|k用x’ 表示,其他变量相似的表示有同样的含义。

    预测

    假设我们知道一个物体的当前位置和速度,我们把它保持在 x 变量中。 现在一秒钟已经过去了。因为我们一秒钟前就知道了物体的位置和速度,以及匀加速运动的加速度 a,那么我们可以在一秒钟后预测物体的位置。如果我们假设物体以同样的速度运动,则我们可以用等式计算预测值。

    但也许物体并没有保持完全相同的速度。 也许物体改变了方向,加速或减速。 所以当我们一秒钟后预测位置时,我们的不确定性就增加了。

    代表不确定性增加了。

    我们来分别讨论各个变量的含义:

    • x是平均状态向量,包含被t-1时刻被跟踪对象综合预测和测量得到的估计值,该估计值包括四个值:(px, py, vx, vy),均基于笛卡尔坐标。x之所以被称为“mean state vector”,是因为位置和速度由均值为x的高斯分布表示。

    • F是预测下一个状态值的状态变换矩阵,为此我们需要一个现实中的线性运动模型。

    { p x ′ = p x + v x Δ t + a x Δ t 2 2 p y ′ = p y + v y Δ t + a y Δ t 2 2 v x ′ = v x + a x Δ t v y ′ = v y + a y Δ t \left\{\begin{array}{l} p_{x}^{\prime}=p_{x}+v_{x} \Delta t+\frac{a_{x} \Delta t^{2}}{2} \\ p_{y}^{\prime}=p_{y}+v_{y} \Delta t+\frac{a_{y} \Delta t^{2}}{2} \\ v_{x}^{\prime}=v_{x}+a_{x} \Delta t \\ v_{y}^{\prime}=v_{y}+a_{y} \Delta t \end{array}\right. px=px+vxΔt+2axΔt2py=py+vyΔt+2ayΔt2vx=vx+axΔtvy=vy+ayΔt

    根据上述线性速度模型,我们得到下列状态转移方程:
    ( p x ′ p y ′ v x ′ v y ′ ) = ( 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ) ( p x p y v x v y ) + ( Δ t 2 2 Δ t 2 2 Δ t Δ t ) ( a x a y a x a y ) x ′ = F x + B u + w \left(\begin{array}{c} p_{x}^{\prime} \\ p_{y}^{\prime} \\ v_{x}^{\prime} \\ v_{y}^{\prime} \\ \end{array}\right)=\left(\begin{array}{cccc} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right)\left(\begin{array}{c} p_{x} \\ p_{y} \\ v_{x} \\ v_{y} \end{array}\right)+\left(\begin{array}{c} \frac{ \Delta t^{2}}{2} \\ \frac{ \Delta t^{2}}{2} \\ \Delta t \\ \Delta t \end{array}\right)\left(\begin{array}{c} a_{x} \\ a_{y} \\ a_{x} \\ a_{y} \end{array}\right)\\ \\ x^{\prime}=F x+Bu+w\\ pxpyvxvy=10000100Δt0100Δt01pxpyvxvy+2Δt22Δt2ΔtΔtaxayaxayx=Fx+Bu+w
    这里F是状态变换矩阵,x‘ 是基于t-1时刻估计状态x下理论预测的t时刻的状态,B被称为控制矩阵,u被称为控制变量,由于汽车的运动并不严格遵循匀加速运动,因此需要在等式后面加上w噪声项。
    x ′ = F x + ν x^{\prime}=F x+\nu\\ x=Fx+ν
    但是在本项目的表达式中我们省略了Bu,原因是Bu代表加速度对于位置更新的影响,而在本项目采用恒速模型,我们无法测量或知道被跟踪物体的确切加速度,因此我们设置Bu=0,将加速度的影响加入到w噪声项中,用v表示它们。

    可以看到,这个等式其实就是 x'= F * x +noise。这个噪声被称为过程噪声,也被叫做运动噪声。过程噪声是表示预测位置时,物体位置的不确定性。由于我们采用恒速模型,因此我们可以在状态预测代码中彻底忽略这个噪声,将这个噪声的影响放入过程协方差矩阵Q中,用来描述状态预测的不确定性,也就是说对于恒速模型的代码x'= F * x

    • Q是过程协方差矩阵,它是与过程噪声相关的协方差矩阵。

    对于过程协方差矩阵的计算稍微复杂些,下面我会为大家推导:

    在上文中说道我们的状态矢量x只跟踪位置和速度,由于加速度是未知的,我们将加速度建模为一个随机噪声。

    这种随机噪声可以用上面导出的方程中的最后一项解析地表示出来。 v 过程噪声,可以理解为随机加速度噪声矢量:
    ν = ( ν p x ν p y ν v x ν v y ) = ( a x Δ t 2 2 a y Δ t 2 2 a x Δ t a y Δ t ) \nu=\left(\begin{array}{c} \nu_{p x} \\ \nu_{p y} \\ \nu_{v x} \\ \nu_{v y} \end{array}\right)=\left(\begin{array}{c} \frac{a_{x} \Delta t^{2}}{2} \\ \frac{a_{y} \Delta t^{2}}{2} \\ a_{x} \Delta t \\ a_{y} \Delta t \end{array}\right) ν=νpxνpyνvxνvy=2axΔt22ayΔt2axΔtayΔt
    这个矢量可以用均值为0,协方差为Q的二元独立高斯分布表示,即νN (0,Q).

    我们可以将v继续分解
    ν = ( a z Δ t 2 2 a y Δ t 2 2 a x Δ t a y Δ t ) = ( Δ t 2 2 0 0 Δ t 2 2 Δ t 0 0 Δ t ) ( a x a y ) = G a \nu=\left(\begin{array}{c} \frac{a_{z} \Delta t^{2}}{2} \\ \frac{a_{y} \Delta t^{2}}{2} \\ a_{x} \Delta t \\ a_{y} \Delta t \end{array}\right)=\left(\begin{array}{cc} \frac{\Delta t^{2}}{2} & 0 \\ 0 & \frac{\Delta t^{2}}{2} \\ \Delta t & 0 \\ 0 & \Delta t \end{array}\right)\left(\begin{array}{l} a_{x} \\ a_{y} \end{array}\right)=G a ν=2azΔt22ayΔt2axΔtayΔt=2Δt20Δt002Δt20Δt(axay)=Ga
    通过两个卡尔曼滤波步骤之间的时间差计算delta_t ,而加速度ax, ay是一个均值为0,标准偏差为sigma_ax 和 sigma_ay的随机矢量。

    我们将Q定义为过程噪声矢量v的期望值乘以噪声矩阵的转置:
    Q = E [ ν ν T ] = E [ G a a T G T ] Q=E\left[\nu \nu^{T}\right]=E\left[G a a^{T} G^{T}\right] Q=E[ννT]=E[GaaTGT]
    由于G不包括任何随机变量,因此我们将G移出期望值的计算。
    Q = G E [ a a T ] G T = G ( σ a x 2 σ a x y σ a x y σ a y 2 ) G T = G Q ν G T Q=G E\left[a a^{T}\right] G^{T}=G\left(\begin{array}{cc} \sigma_{a x}^{2} & \sigma_{a x y} \\ \sigma_{a x y} & \sigma_{a y}^{2} \end{array}\right) G^{T}=G Q_{\nu} G^{T} Q=GE[aaT]GT=G(σax2σaxyσaxyσay2)GT=GQνGT
    由于ax,ay互相不相关,这就意味着sigma_axy = 0,我们可以对Qv进一步简化:
    Q ν = ( σ a x 2 σ a x y σ a x y σ a y 2 ) = ( σ a x 2 0 0 σ a y 2 ) Q_{\nu}=\left(\begin{array}{cc} \sigma_{a x}^{2} & \sigma_{a x y} \\ \sigma_{a x y} & \sigma_{a y}^{2} \end{array}\right)=\left(\begin{array}{cc} \sigma_{a x}^{2} & 0 \\ 0 & \sigma_{a y}^{2} \end{array}\right) Qν=(σax2σaxyσaxyσay2)=(σax200σay2)
    将其带入Q矩阵的计算公式,我们可以得到:
    Q = G Q ν G T = ( Δ t 4 4 σ a x 2 0 Δ t 3 2 σ a x 2 0 0 Δ t 4 4 σ a y 2 0 Δ t 3 2 σ a y 2 Δ t 3 2 σ a x 2 0 Δ t 2 σ a x 2 0 0 Δ t 3 2 σ a y 2 0 Δ t 2 σ a y 2 ) Q=G Q_{\nu} G^{T}=\left(\begin{array}{cccc} \frac{\Delta t^{4}}{4} \sigma_{a x}^{2} & 0 & \frac{\Delta t^{3}}{2} \sigma_{a x}^{2} & 0 \\ 0 & \frac{\Delta t^{4}}{4} \sigma_{a y}^{2} & 0 & \frac{\Delta t^{3}}{2} \sigma_{a y}^{2} \\ \frac{\Delta t^{3}}{2} \sigma_{a x}^{2} & 0 & \Delta t^{2} \sigma_{a x}^{2} & 0 \\ 0 & \frac{\Delta t^{3}}{2} \sigma_{a y}^{2} & 0 & \Delta t^{2} \sigma_{a y}^{2} \end{array}\right) Q=GQνGT=4Δt4σax202Δt3σax2004Δt4σay202Δt3σay22Δt3σax20Δt2σax2002Δt3σay20Δt2σay2
    可以看到,Q 矩阵包含了大量时间delta_t项,这是因为随着时间的推移,我们对自己的位置和速度变得越来越不确定。 随着增量 t 的增加,状态协方差矩阵 P 增加了更多的不确定性。

    值得注意的是sigma_ax,sigma_ay 需要根据实际运动模型进行估量,在本项目中我们给定sigma_ax=sigma_ay =3

    • P是状态协方差矩阵,其中包含有关对象位置和速度的不确定性的信息,通常可以认为它包含方差。可又为什么叫做协方差矩阵呢?

    我们通过一个实例来理解P的含义:

    由于状态向量的元素有n个,因此p矩阵为n x n = 4 x 4。 假设位置Py的幅度变大,Vx速度的幅度也变高,这就意味着PyVx具有协方差。如果状态元素之间互相不影响,则可以说它们的协方差为零。如在下图中,如果目标保持横向运动或者纵向运动,PxPy之间不会相互影响,协方差为0,而当目标斜向运动时,则PxPy之间会相互影响,协方差不为0。

    P 的对角线包含了元素自身的方差,数值越大,代表状态元素的不确定性就越大。 在对角线的周围是两个状态元素之间的协方差,体现了状态元素的值如何相互“共同变化”。 所有像这样的协方差矩阵(如下文中的 R, S, Q)都是对称矩阵,即 PyVx 的协方差也是 VxPy 的协方差。如下表所示:

    PxPyVxVy
    PxPx-varianceA-covarianceD-covarianceF-covariance
    PyA-covariancePy-varianceB-covarianceE-covariance
    VxD-covarianceB-covarianceVx-varianceC-covariance
    VyF-covarianceE-covarianceC-covarianceVy-variance

    在理解了P矩阵各个成分的含义之后,我们就可以选择合适的参数初始化P矩阵。如果滤波器知道准确的初始位置,则我们可以给出一个各项均为0的初始协方差矩阵。 如
    P 0 ∣ 0 = [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] \mathbf{P}_{0 | 0}=\left[\begin{array}{ll} 0 & 0 & 0 & 0 \\ 0 & 0& 0 & 0 \\ 0 & 0& 0 & 0 \\ 0 & 0& 0 & 0 \\ \end{array}\right] P00=0000000000000000
    但是这种情况比较少见,一般情况第一次预测是基于已有的测量结果,往往带有噪声。

    这种情况下可以尝试用Radar或者Lidar的测量噪声方差来初始化P矩阵。

    如果不确切知道最初的位置与速度,那么协方差矩阵可以初始化为一个对角线元素是B的矩阵,B取一个合适的比较大的数。如在本项目中,我们只知道初始状态的位置PxPy,并不知道速度VxVy, 这种情况下我们可以为Vx-variance和 Vy-variance设置一个较大的初始值。

    以下是初始化代码:

    is_initialized_ = false;
    previous_timestamp_ = 0;
    // initializing matrices
    R_laser_ = MatrixXd(2, 2);
    R_radar_ = MatrixXd(3, 3);
    H_laser_ = MatrixXd(2, 4);
    Hj_ = MatrixXd(3, 4);
    //measurement covariance matrix - laser
    R_laser_ << 0.0225, 0,
    0, 0.0225;
    //measurement covariance matrix - radar
    R_radar_ << 0.09, 0, 0,
    0, 0.0009, 0,
    0, 0, 0.09;
    // Initialize P
    ekf_.P_ = MatrixXd(4, 4);
    ekf_.P_ << 1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1000, 0,
    0, 0, 0, 1000;
    H_laser_ << 1, 0, 0, 0,
    0, 1, 0, 0;
    

    预测更新代码如下:

    x_ = F_ * x_;
    MatrixXd Ft = F_.transpose();
    P_ = F_ * P_ * Ft + Q_;
    

    测量更新

    我们将从两种测量传感器中获取数据:

    • 激光测量

    • 雷达测量

      1.激光测量

    在本项目中,激光测量会获取物体的当前位置,但不提供物体的速度。激光相对于雷达传感器具有更高的空间分辨率,尽管激光传感器也能计算速度,但是准确性不如雷达高。

    让我们来看看测量更新x 和 p 的公式:
    测 量 残 差 : y = z − H x ′ 测 量 残 差 协 方 差 : S = H P ′ H T + R 最 优 卡 尔 曼 增 益 : K = P ′ H T S − 1 更 新 的 状 态 估 计 : x = x ′ + K y 更 新 的 协 方 差 估 计 : P = ( I − K H ) P ′ \begin{aligned} &测量残差: y=z-H x^{\prime}\\ &测量残差协方差:S=H P^{\prime} H^{T}+R\\ &最优卡尔曼增益:K=P^{\prime} H^{T} S^{-1}\\ &更新的状态估计:x=x^{\prime}+K y\\ &更新的协方差估计:P=(I-K H) P^{\prime} \end{aligned} :y=zHx:S=HPHT+R:K=PHTS1:x=x+Ky:P=(IKH)P

    我们来分别讨论各个变量的含义:

    • 测量矢量z

      对于激光雷达,z 矢量包含位置 x 和位置 y 的测量值。

    Z = ( p x p y ) Z=\left(\begin{array}{l} p_{x} \\ p_{y} \end{array}\right) Z=(pxpy)

    • 变换矩阵H

      H矩阵是将目标的理论估计状态矢量x’投射到传感器的测空间中的矩阵。对于激光雷达,因为激光雷达传感器仅测量位置,因此我们从状态变量中丢弃速度信息,即状态矢量x包含有关[ pxpyvxvy ]的信息,而z向量将仅包含[ pxpy ]。乘以变换H可以计算传感器真实测量值z与我们的理论估计状态矢量x‘之间的残差。

      对于激光雷达:
      ( p x p y ) = H ( p x ′ p y ′ v x ′ v y ′ ) H = ( 1 0 0 0 0 1 0 0 ) \left(\begin{array}{l} p_{x} \\ p_{y} \end{array}\right)=H\left(\begin{array}{l} p_{x}^{\prime} \\ p_{y}^{\prime} \\ v_{x}^{\prime} \\ v_{y}^{\prime} \end{array}\right)\\ H=\left(\begin{array}{llll} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{array}\right) (pxpy)=HpxpyvxvyH=(10010000)

    • 测量噪声协方差矩阵R

      R矩阵代表传感器测量中的不确定性。R矩阵是正方形,并且矩阵的每一侧与测量参数的数量相同。

      对于激光雷达,我们有一个二维测量矢量。 每个位置成分 px,py 都会受到随机噪声的影响。 所以我们的噪声矢量 ω 和 z 的维数是一样的。测量噪声ω是一个均值为零,协方差为R的分布,即ω∼N(0,R)。因此R矩阵是均值为零的2 x 2协方差矩阵,是噪声矢量 ω 和它的转置乘积的期望值。
      R = E [ ω ω T ] = ( σ p x 2 0 0 σ p y 2 ) R=E\left[\omega \omega^{T}\right]=\left(\begin{array}{cc} \sigma_{p x}^{2} & 0 \\ 0 & \sigma_{p y}^{2} \end{array}\right) R=E[ωωT]=(σpx200σpy2)
      注意,非对角线为0表示噪声互不相关。

      通常,随机噪声测量矩阵的参数将由传感器制造商提供,在本项目中已经提供了雷达和激光传感器的R矩阵。

    • 测量残差协方差矩阵S

      理论估计值与测量值之间的误差也可以用协方差矩阵S来表示:
      S t = cov ⁡ ( z t − H x t ∣ t − 1 ) = cov ⁡ ( H x t + v t − H x t ∣ t − 1 ) = cov ⁡ ( H ( x t − x t ∣ t − 1 ) H T ) + cov ⁡ ( v t ) = H P t ∣ t − 1 H T + R t = H P ′ H T + R \begin{aligned} \boldsymbol{S}_{t} &=\operatorname{cov}\left(\boldsymbol{z}_{t}-\boldsymbol{H} \boldsymbol{x}_{t | t-1}\right) \\ &=\operatorname{cov}\left(\boldsymbol{H} \boldsymbol{x}_{t}+\boldsymbol{v}_{t}-\boldsymbol{H} \mathbf{x}_{t | t-1}\right) \\ &=\operatorname{cov}\left(\boldsymbol{H}\left(\boldsymbol{x}_{t}-\boldsymbol{x}_{t | t-1}\right) \boldsymbol{H}^{\mathrm{T}}\right)+\operatorname{cov}\left(\boldsymbol{v}_{t}\right) \\ &=\boldsymbol{H} \boldsymbol{P}_{t | t-1} \boldsymbol{H}^{\mathrm{T}}+\boldsymbol{R}_{t} \\ &=\boldsymbol{H} \boldsymbol{P}^{\prime}\boldsymbol{H}^{\mathrm{T}}+\boldsymbol{R} \end{aligned} St=cov(ztHxtt1)=cov(Hxt+vtHxtt1)=cov(H(xtxtt1)HT)+cov(vt)=HPtt1HT+Rt=HPHT+R

    • 测量残差矢量y

      y矢量是实际测量值和预测估计值之间的残差。

    • 卡尔曼增益K

      卡尔曼滤波的思想是分别给预测估计值和实际测量值一个权重,通过预测估计值与实际测量值的加权线性组合来得到估计值,即:
      x t ∣ t = K t z t + ( I − K t H ) x t ∣ t − 1 x t ∣ t = x t ∣ t − 1 + K t ( z t − H x t ∣ t − 1 ) x = x ′ + K y P = ( I − K H ) P ′ \boldsymbol{x}_{t | t}=\boldsymbol{K}_{t} \boldsymbol{z}_{t}+\left(\boldsymbol{I}-\boldsymbol{K}_{t} \boldsymbol{H}\right) \boldsymbol{x}_{t | t-1} \\ \boldsymbol{x}_{t | t}= \boldsymbol{x}_{t | t-1}+\boldsymbol{K}_{t} (\boldsymbol{z}_{t}- \boldsymbol{H}\boldsymbol{x}_{t | t-1}) \\ x=x^{\prime}+K y \\ P=(I-K H) P^{\prime} xtt=Ktzt+(IKtH)xtt1xtt=xtt1+Kt(ztHxtt1)x=x+KyP=(IKH)P
      卡尔曼增益 k是给予给矢量y的权重,包含了预测估计值 x‘和当前观测值 z 的权重信息,用来继续更新状态矢量x和状态协方差矩阵 P。一般来说,较低的权重赋予较高的不确定性。也就是说如果我们的传感器测量是非常不确定的(R高于P′ ) ,那么卡尔曼滤波器将给予更多的权重给我们认为的状态估计x′。 如果我们的状态估计是不确定的(P′高于R) ,卡尔曼滤波器会把更多的权重给传感器测量值 z

      激光雷达更新代码如下:

      void KalmanFilter::Update(const VectorXd &z) {
          /**
           * TODO: update the state by using Kalman Filter equations
           */
          VectorXd z_pred = H_ * x_;
          VectorXd y = z - z_pred;;
          MatrixXd Ht = H_.transpose();
          MatrixXd S = H_ * P_ * Ht + R_;
          MatrixXd Si = S.inverse();
          MatrixXd K = P_ * Ht * Si;
      
          x_ = x_ + K * y;
          int x_size = x_.size();
          MatrixXd I = MatrixXd::Identity(x_size, x_size);
          P_ = (I - K * H_) * P_;
      }
      

      2. 雷达测量

    尽管激光传感器能提供准确的行人位置数据,但实际上我们无法直接观察其速度,这个时候我们就需要雷达了。根据多普勒效应,雷达能够得到直接测量移动对象的径向速度。径向速度是指对象靠近或者远离传感器的移动速度。但是雷达提供的测量结果是包含距离,角度和距离速率的极坐标数值,我们需要转换成X状态矢量所在的笛卡尔坐标,这一转换是非线性的,我们无法通过传统卡尔曼滤波实现,我将在扩展卡尔曼滤波中详细解释这一部分。

    Extended Kalman Filter

    在上文中,我们已经把激光测量值整合到卡尔曼滤波中,但是雷达测量值并不是一样的整合方式。

    原因是雷达观察世界的方式是不同的。

    • 距离ρ是原点到目标的径向距离,可以定义为 ρ=sqrt(px2+py2)

    • 方向角φ射线与x方向的夹角,φ=atan(py/px)。请注意,φ是以x轴逆时针为正参考方向的,因此上述图中φ实际上为负。

    • 距离变化率ρ是速度v在沿着射线L方向上的投影,也被称做径向速度。

    激光测量的H矩阵和雷达测量的h(x)方程实际上是在完成相同的工作。它们都需要在更新步骤中解*y* = z - Hx'等式。

    但是对于雷达,没有H矩阵可以将状态向量x映射到极坐标中,需要手动计算映射以将笛卡尔坐标转换为极坐标。

    下面是h(x)函数,它指定如何将预测的位置和速度映射到距离,角度和距离变化率的极坐标。

    下图是笛卡尔坐标转换极坐标的方法:

    因此,对于雷达,y = z - Hx '等式变成了y = z - h(x')

    现在我已经有了一个非线性测量函数h(x),如果我们想用新的测量值z更新预测状态x*,我们能否继续使用卡尔曼滤波方程式了呢?

    答案是否定的,原因是:我们的预测状态x已经由高斯分布表示,如果我们将该分布映射到上述的非线性函数h(x),那么所得函数将不是高斯函数。

    务必注意卡尔曼滤波器仅适用于高斯函数。

    这个时候就需要引入非线性处理函数:扩展卡尔曼滤波

    h(x)函数线性化是扩展卡尔曼滤波器(EKF)的关键。

    EKF将当前估计值的平均值周围的分布线性化,即在原始高斯分布的均值位置,用一个和h(x)相切的线性函数,近似出测量值函数,然后在卡尔曼滤波器算法的预测和更新状态中使用此线性化。

    如上图所示,非线性函数的分布不是高斯分布。

    而在下图中,对非线性函数进行近似线性化得到的分布是高斯分布。

    EKF 使用一种称为“ **一阶泰勒展开”**的方法来线性化函数。
    h ( x ) ≈ h ( μ ) + ∂ h ( μ ) ∂ x ( x − μ ) h(x) \approx h(\mu)+\frac{\partial h(\mu)}{\partial x}(x-\mu) h(x)h(μ)+xh(μ)(xμ)
    函数h(x)对应于x的导数被称为雅可比矩阵,它包含所有偏导数。对于雅可比矩阵的推导稍显复杂,在此我直接给出结果。感兴趣的朋友可以对h(x)一一求偏导数。
    H j = [ ∂ ρ ∂ p x ∂ ρ ∂ p y ∂ ρ ∂ v x ∂ ρ ∂ v y ∂ φ ∂ p x ∂ φ ∂ p y ∂ φ ∂ v x ∂ φ ∂ v y ∂ ρ ˙ ∂ p x ∂ ρ ˙ ∂ p y ∂ ρ ˙ ∂ v x ∂ ρ ∂ v y ] = [ p x p x 2 + p y 2 p y p x 2 + p y 2 0 0 − p y p x 2 + p y 2 p x p x 2 + p y 2 0 0 p y ( v x p y − v y p x ) ( p x 2 + p y 2 ) 3 / 2 p x ( v y p x − v x p y ) ( p x 2 + p y 2 ) 3 / 2 p x p x 2 + p y 2 p y p x 2 + p y 2 ] H_{j}=\left[\begin{array}{cccc} \frac{\partial \rho}{\partial p_{x}} & \frac{\partial \rho}{\partial p_{y}} & \frac{\partial \rho}{\partial v_{x}} & \frac{\partial \rho}{\partial v_{y}} \\ \frac{\partial \varphi}{\partial p_{x}} & \frac{\partial \varphi}{\partial p_{y}} & \frac{\partial \varphi}{\partial v_{x}} & \frac{\partial \varphi}{\partial v_{y}} \\ \frac{\partial \dot{\rho}}{\partial p_{x}} & \frac{\partial \dot{\rho}}{\partial p_{y}} & \frac{\partial \dot{\rho}}{\partial v_{x}} & \frac{\partial \rho}{\partial v_{y}} \end{array}\right] =\left[\begin{array}{cccc} \frac{p_{x}}{\sqrt{p_{x}^{2}+p_{y}^{2}}} & \frac{p_{y}}{\sqrt{p_{x}^{2}+p_{y}^{2}}} & 0 & 0 \\ -\frac{p_{y}}{p_{x}^{2}+p_{y}^{2}} & \frac{p_{x}}{p_{x}^{2}+p_{y}^{2}} & 0 & 0 \\ \frac{p_{y}\left(v_{x} p_{y}-v_{y} p_{x}\right)}{\left(p_{x}^{2}+p_{y}^{2}\right)^{3 / 2}} & \frac{p_{x}\left(v_{y} p_{x}-v_{x} p_{y}\right)}{\left(p_{x}^{2}+p_{y}^{2}\right)^{3 / 2}} & \frac{p_{x}}{\sqrt{p_{x}^{2}+p_{y}^{2}}} & \frac{p_{y}}{\sqrt{p_{x}^{2}+p_{y}^{2}}} \end{array}\right] Hj=pxρpxφpxρ˙pyρpyφpyρ˙vxρvxφvxρ˙vyρvyφvyρ=px2+py2 pxpx2+py2py(px2+py2)3/2py(vxpyvypx)px2+py2 pypx2+py2px(px2+py2)3/2px(vypxvxpy)00px2+py2 px00px2+py2 py
    虽然数学证明看起来有些复杂,但是卡尔曼滤波方程和扩展卡尔曼滤波器方程是非常相似的。

    主要区别在于如果单独只对非线性模型变量完成预测和更新:

    • 计算P ’ 时,F 矩阵将被 Fj 替换。
    • 计算SKP时,将用雅可比矩阵Hj替换卡尔曼滤波器中的H矩阵。
    • 为了计算x ',使用预测更新函数 f 代替 F 矩阵。
    • 为了计算y,使用 h 函数代替 H 矩阵。

    如果我们在只对非线性模型进行估计最优化,我们需要在预测和测量更新步骤中都使用非线性模型等式,意味着我们在预测中也需要用它的雅可比矩阵Fj 来代替 F 矩阵,如我们只用雷达完成位置速度的最优估计。

    但是在本项目中,由于我们的目的是融合激光和雷达传感器,激光雷达使用线性模型,因此我们在两个传感器各自的预测中都不使用 f 函数或 Fj函数,仍然使用常规的卡尔曼滤波方程和 F 矩阵。对于测量更新的步骤,则根据传感器的不同选择卡尔曼滤波方程或者扩展卡尔曼滤波方程。

    雷达更新代码如下:

    void KalmanFilter::UpdateEKF(const VectorXd &z) {
        /**
         * TODO: update the state by using Extended Kalman Filter equations
         */
        double px = x_(0);
        double py = x_(1);
        double vx = x_(2);
        double vy = x_(3);
        // Coordinate conversion from Cartesian coordinates to Polar coordinates
        double rho = sqrt(px*px + py*py);
        double theta = atan2(py, px);
        double rho_dot = (px*vx + py*vy) / rho;
        VectorXd h = VectorXd(3);
        h << rho, theta, rho_dot;
        VectorXd y = z - h;
        // Nominalization of angle
        while (y(1)>M_PI) y(1)-=2*M_PI;
        while (y(1)<-M_PI) y(1)+=2*M_PI;
    
        MatrixXd Ht = H_.transpose();
        MatrixXd S = H_ * P_ * Ht + R_;
        MatrixXd Si = S.inverse();
        MatrixXd K =  P_ * Ht * Si;
    
        x_ = x_ + (K * y);
        int x_size = x_.size();
        MatrixXd I = MatrixXd::Identity(x_size, x_size);
        P_ = (I - K * H_) * P_;
    
    }
    

    至此我们完成了卡尔曼滤波和扩展卡尔曼滤波的原理讲解,现在来让我们看下激光和雷达传感器融合的流程。

    Sensor fusion framework

    这是激光和雷达传感器融合的整体处理流程:

    • 首次测量 -卡尔曼滤波器将接收目标相对于汽车位置的初始测量值。这些测量值来自雷达或激光雷达传感器。

    • 初始化状态和协方差矩阵 -卡尔曼滤波器将基于第一次测量值来初始化目标的位置。

    • Δ t 时间后,汽车将收到另一个传感器测量值。

    • 预测 -算法将预测Δ t 时间后的目标位置。预测自行车位置的一种方式是假设自行车的速度是恒定的,Δ t 时间后目标将移动v * Δ t 距离。在本扩展卡尔曼滤波项目中,我们将假设速度是恒定的。

    • 更新 -滤波器将预测的位置与传感器测量值进行比较。将预测的位置和测量的位置合并以给出更新的位置。卡尔曼滤波器将根据每个值的不确定性将更多的权重放在预测位置或测量位置上。

      • 如果当前观察数据来自雷达传感器,我们使用雷达的HR矩阵来设置扩展卡尔曼滤波器,而且我们必须计算新的雅可比矩阵Hj,使用非线性函数来变化坐标系,并调用测量更新。
      • 如果当前观察数据来自激光雷达,我们使用激光雷达的HR矩阵来设置卡尔曼滤波器,然后调用测量更新。
    • Δ t 时间后,汽车又收到另一个传感器测量值。算法将开始执行下一个预测更新步骤。

    Evaluation Kalman Filter Performance

    在我们实现了追踪算法之后,我们需要检查算法效果,来评估估算结果和真实结果差别有多大。

    最常见的检查标准叫做均方根误差(Root mean squared error)。
    R M S E = 1 n ∑ t = 1 n ( x t e s t − x t t r u e ) 2 R M S E=\sqrt{\frac{1}{n} \sum_{t=1}^{n}\left(x_{t}^{e s t}-x_{t}^{t r u e}\right)^{2}} RMSE=n1t=1n(xtestxttrue)2
    估算状态和真实状态之间的差值叫做残差。对这些残差先平方然后求均值就可以得到均方根误差。

    在本项目中,真实状态的值已经给出。

    均方根误差评估代码如下:

    MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) {
      /**
       * TODO:
       * Calculate a Jacobian here.
       */
        MatrixXd Hj(3,4);
       // state parameters
        double px = x_state(0);
        double py = x_state(1);
        double vx = x_state(2);
        double vy = x_state(3);
    
        // preparation of Jacobian terms
        double c1 = px*px+py*py;
        double c2 = sqrt(c1);
        double c3 = (c1*c2);
    
        if(fabs(c1) < 0.0001){
            cout << "ERROR - Division by Zero" << endl;
            return Hj;
        }
        // compute jacobian matrix
        Hj << (px/c2), (py/c2), 0, 0,
                -(py/c1), (px/c1), 0, 0,
                py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2;
    
        return Hj;
    
    }
    

    Conclusion

    • 卡尔曼滤波虽然计算复杂度低,但是仅能解决线性问题,对于非线性问题,需要使用扩展卡尔曼滤波等算法。
    • 尽管扩展卡尔曼滤波能解决非线性问题,但是仍有一些缺点:
      • 如果需要用解析方法求雅可比矩阵,则计算很困难的。
      • 如果用数值分析的方法求出雅可比矩阵,则计算开销很大。
      • 扩展卡尔曼滤波器只适用于具有可微分模型的系统。
    • 由于雷达测量是非线性的,因此我们需要在雷达测量更新周期中使用扩展卡尔曼滤波器,而激光测量是线性的,我们在激光测量更新中使用卡尔曼滤波器,在预测周期中两者都可以使用相同的卡尔曼滤波器公式。

    在下一篇博客中我会介绍无迹卡尔曼滤波(UKF),这是一种更强大的处理非线性模型的卡尔曼滤波变种。

    实际上Unscented Kalman Filter应该被翻译成无味卡尔曼滤波,因为作者当年就是觉得扩展卡尔曼滤波太没意思了,才发明了Unscented Kalman Filter 😃 !

    如果有什么疑问,可以随时联系我的个人邮箱。

    如果你觉得我的文章对你有帮助,请帮忙点个赞~\(≧▽≦)/~

    转载请私信作者!

    引用:

    展开全文
  • 自动驾驶感知融合-无迹卡尔曼滤波(Lidar&Radar)

    千次阅读 多人点赞 2020-05-19 10:42:08
    Github:https://github.com/williamhyin/SFND_Unscented_Kalman_Filter Email: williamhyin@outlook.com 知乎专栏: 自动驾驶全栈工程师 Overview 我们简单回顾下KF及EKF滤波: 卡尔曼滤波器: 假设数据为高斯分布...

    Unscented Kalman Filter

    在上一篇的文章“卡尔曼及扩展卡尔曼滤波(Lidar&Radar)”中我已经详细介绍了卡尔曼KF及扩展卡尔曼滤波EKF,今天我们来看以下在处理非线性问题上更为强大的无迹卡尔曼滤波UKF。在本文中,首先我会介绍无迹卡尔曼滤波的原理,并分析EKFUKF的优缺点,然后我会介绍一个更符合现实场景的过程模型“恒转弯速率和速度幅值模型(CTRV)” ,最后我会在CTRV上执行UKF,从而实现基于激光雷达和毫米波雷达的多目标追踪。

    Github:https://github.com/williamhyin/SFND_Unscented_Kalman_Filter

    Email: williamhyin@outlook.com

    知乎专栏: 自动驾驶全栈工程师

    Overview

    我们简单回顾下KFEKF滤波:

    • 卡尔曼滤波器: 假设数据为高斯分布形式,我们将线性方程式应用于该高斯分布的数据,然后使用一系列卡尔曼滤波公式获得最优状态预估。

    • 扩展卡尔曼滤波:然而现实世界并不是线性的,当传感器在一个方向上进行测量时,我们可能会在另一个方向上进行预测,这样会涉及非线性的角度和正弦余弦函数。因此,EKF可以借助泰勒级数(以及雅可比矩阵)在高斯均值附近线性近似非线性函数,然后预测其值。

    1. 既然EKF已经能够很好的解决非线性问题了,为什么还需要UKF?

      原因是现实生活中存在很多完全不规律的分布,我们需要更好的近似方式和更高的性能。

      让我们来看下在EKF中如何解决非线性问题的:

      可以看到原始高斯分布经过非线性变换之后其实是一个不规则的非线性分布,在EKF中我们在高斯均值点附近用泰勒级数近似非线性分布,从而获得近似高斯分布。但是问题在于,这个近似高斯分布有时不是足够精确,单单一个均值点往往无法预测上图中这种很不规则的分布的。这个时候我们就需要无迹卡尔曼滤波UKF了。

      值得注意的是尽管Unscented Kalman Filter通常被叫做无迹卡尔曼滤波,但是在有的文献中存在另外一种叫法:无损卡尔曼滤波,这两种叫法都是可行的。

      1. UKF是怎么解决复杂不规则分布的?

        UKF从原始高斯分布中提取包括均值点在内的一系列代表点,并将代表点带入到非线性等式中,围绕这些点进行近似,从而获得更好的近似结果。

        在上图中我们可以看到,蓝色椭圆代表的UKF近似高斯分布更接近真实的不规则分布。

        ​ 总的来说,与EKF相比,UKF是处理非线性过程模型或非线性测量模型的更好的替代方法。UKF不会对非线性函数进行线性化处理,它用所谓的sigma 点来近似概率分布。其优势在于在很多情况下,sigma 点近似非线性转换的效果比线性化更好,此外它还可以省去计算复杂的雅可比矩阵的过程

    2. UKF 代表点的数量是不是越多越好?

      从原始高斯分布中提取的点的数目越多,我们的近似就越精确,但是相应计算资源的消耗就越大。将原始高斯分布中所有的数据点进行非线性变换和近似处理是最简单的解决方案,但不是最佳方案。

    3. 如何选择合适的UKF代表点?

      UKF代表点被称为sigma 点,它们分布在状态均值的周围,并且和每个状态维度的标准偏差信号(即σ)有关,因此被叫做sigma 点。它们代表了整个分布。在这里值得注意的,我们可能会根据实际需求或多或少优先考虑某些点,从而使我们的近似性更好,这就需要为每一个sigma 点赋予权重。如何合理选择sigma 点的个数,并且为不同的sigma 点赋予权重是UKF应用的重点,我会在接下来的章节详细描述。

    由于无迹卡尔曼滤波是在卡尔曼滤波的基础上开发的,在本文中对于一些基础概念我并没有详细解释,如果有什么疑问,请参看我的上一篇文章“卡尔曼及扩展卡尔曼滤波(Lidar&Radar)”。

    Unscented Transform

    当高斯分布经过非线性函数变换后,它不再是高斯分布,而无迹卡尔曼滤波通过无迹转换将非线性函数变换的结果近似成高斯分布。

    以下是无迹变换执行的步骤:

    1.计算Sigma点集

    2.为每个Sigma点分配权重

    3.把所有单个Sigma点代入非线性函数f

    4.对经过上述加权和转换的点近似新的高斯分布

    5.计算新高斯分布的均值和方差。

    计算Sigma 点集

    选择合适的sigma 点数量n取决于系统的状态维度N:n=2N+1

    公式1

    我们可以根据公式1计算sigma 点 矩阵,值得注意的是到第1个点和其之后的点有不同的计算公式,这是由于第1个点是均值点,其余点都是围绕均值的sigma 点。

    以下是公式1中部分参数的含义:

    • χ表示 sigma 点矩阵。矩阵中每一列都表示一组sigma 点。如果系统是2维空间,那么矩阵的大小将是2 x 5。 每个维度有n=2N+1=5个sigma 点。可以看到,同一列的sigma 点有两个对称的计算公式,这是由于同一列的上下两个sigma 点在原始高斯分布中围绕均值点呈对称分布。
    • λ是比例因子,它告诉我们应该选择距离均值多远的sigma 点。 数学研究表明,λ的最佳值为3-n。
    • Σ被称为协方差矩阵,对Σ求平方需要找到一个满足以下条件的矩阵 S: Σ = S.S or Σ = S.S_transpose,S= √∑。

    计算sigma点的权重

    在我们挑选出合适的sigma 点之后,我们需要为每个sigma 点赋予合适的权重:

    公式2

    值得注意的是计算均值权重的公式与计算其余sigma 点权重的公式也是不同的,而且所有代表点权重之和等于1。

    无迹转换参数

    sigma 点的选择和权重计算公式主要有以下四个可调节参数,这些参数被称作UT Parameters, 我们可以使用这些参数对无迹变换进行缩放:

    其中最重要的是 λ 扩散参数,数学研究表明,λ的最佳值为3-n。如果λ已经给出,其余UT参数可以考虑省略,从而简化公式,如在有的资料中计算sigma 点权重的公式为

    如在这个公式中没有ωc和ωm。

    本项目中使用简化的sigma 点权重计算公式

    但是理解 λ 扩散参数的原理是很有必要的, λ 扩散参数由k,α,n三个参数计算得到,下图是不同k和α的组合对于sigma 点分布的影响。

    计算近似高斯分布的均值和协方差

    现在我们已经得到了带有权重的,经过非线性等式转换后的sigma 点,我们可以通过这些sigma 点近似新的高斯分布,并计算其均值及协方差。

    公式3

    下面是公式3中各个参数的含义:

    μ′ -> Predicted Mean
    Σ′ -> Predicted Covariance
    w -> Weights of sigmas
    g -> Non Linear function
    χ(Caligraphic X) -> Sigmas Matrix
    n -> Dimentionality

    这样我们就完成了无迹变换的全部过程,现在让我在预测更新和测量更新中融入无迹卡尔曼滤波。

    UKF Roadmap

    Prediction

    预测步骤基本上与我们上文中讨论到的无迹变换非常接近

    1. 使用公式1计算Sigma 点
    2. 使用公式2计算Sigma点的权重
    3. 使用公式3转换Sigma 点并计算新的均值和协方差。由于此时我们的不确定性增加了一些,因此我们必须考虑添加过程噪声R

    公式4

    Measurement

    在预测更新后我们得到了预测值的均值和协方差。 当我们得到来自传感器的测量值后,如果我们想获得预测值和测量值之间的差值,我们得首先从预测的状态空间转换到测量状态空间。

    如果状态空间的转换是非线性的,则就需要像预测步骤一样进行无迹转换

    通过公式h(x)我们可以把预测状态空间转换为测量状态空间。

    公式5

    下面是公式5中各个参数的含义:

    Z -> transformed sigmas in measurement space
    χ(Caligraphic X) -> Sigmas Matrix
    ẑ -> Mean in measurement space
    S -> Covariance in measurement space
    Q-> Noise
    h-> is a function that maps our sigmas to measurement space

    这里需要注意,由于我们不再将函数线性化,因此我们不再有雅可比行列式。

    接下来我们需要计算卡尔曼增益k

    首先我们要计算预测误差:状态空间中的sigma点与测量空间中的sigma点之间的交叉关联矩阵T。

    公式6

    下面是公式6中各个参数的含义:

    T -> Cross Co-relation Matrix between state space and predicted space
    S-> Predicted Covariance Matrix
    K-> Kalman Gain

    虽然EKFUKF计算卡尔曼增益k的公式是看起来不同,但其实很相似:

    公式7

    而最终在测量状态中更新预测值的等式则是相同的。

    公式8

    下面是公式7, 8中各个参数的含义:

    μ -> Mean
    Σ -> Covariance
    μ′ -> Predicted Mean
    Σ′ -> Predicted Covariance
    K -> Kalman Gain
    z-> Actual Measurement Mean coming from the sensor
    ẑ -> Mean in measurement space
    T -> It is the same as H in Kalman Filter and Hⱼ in EKF. Here it is cross co-relation matrix

    EKF VS UKF

    我们已经介绍完无迹卡尔曼滤波了,现在来总结一下两者的区别。

    对香蕉形状的近似估计是最能体现两者性能区别的实验。

    我们可以明显看到UKF近似非线性模型的优越性。

    下图真实展示了EKF近似非线性模型的不足,即很有可能大范围偏离真实分布。

    虽然UKF不需要雅可比矩阵,但是其实具有相同的复杂度等级,对于规则分布的近似甚至可能会慢一点,与EKF的效果差异有的时候很小,只是在EKF中有的时候雅可比矩阵很难推导,这也是无迹卡尔曼滤波作者开发这一算法的起因之一。

    Constant turn rate and velocity magnitude model (CTRV)

    在扩展卡尔曼滤波器课程中,我们使用了恒速模型(CV)。 恒速运动模型是目标跟踪最基本的运动模型之一。

    但是恒速模型在现实生活中存在局限性,恒速模型无法正确预测拐弯的车辆。

    除了恒速模型还有许多其他模型,包括:

    • constant turn rate and velocity magnitude model (CTRV)
    • constant turn rate and acceleration (CTRA)
    • constant steering angle and velocity (CSAV)
    • constant curvature and acceleration (CCA)
    • Constant Heading and Constant Velocity (CHCV)

    每个模型都对一个物体的运动做出不同的假设。这些模型适合真实的交通场景。

    我们可以在任意一个模型上使用扩展卡尔曼滤波器或者无迹卡尔曼滤波。

    在本文中,我们只讨论CTRV模型。

    1. 首先我们来看看CTRV模型的状态向量及过程模型的定义:

    其中ψ为角度,ψ˙为角速度,ψ¨为角速度变化率。

    尽管我们知道了CTRV模型的状态向量,我们需要预测时间k+1时车辆的位置,但是我们对于过程模型的推导仍然毫无头绪。这个时候我们就需要借助状态向量的变化率,通过差分方程来推演真实世界中的过程模型。

    通过对状态向量求导我们得到了其差分方程

    时间差

    求微积分:

    微积分推导的步骤相对复杂,可以使用WolframAlpha的积分工具求微积分。

    微分结果

    但是这个等式有一个问题:ψ˙ 不能为0,也是说这个等式不能解决车辆沿(斜)直线行驶的问题(ψ˙=0,ψ可以不等于0)。

    对于ψ˙=0的情况我们可以直接求解斜三角。

    即:

    1. 在确定过程模型的固定部分之后,我们还需要考虑过程噪声项,这就涉及到过程噪声νk。

    我们用一个二维噪声向量νk来描述过程模型,不确定性νk由两个独立的标量过程噪声组成。第一个过程噪声是纵向加速噪声νa,νa是随机分布的白噪声,其均值为0,方差为σa的平方,即

    第二个过程噪声是角加速度噪声νψ˙˙,


    而Q为过程噪声协方差矩阵。当我们知道噪声项之后,我们需要考虑它对整个积分方程的影响。

    对于速度:

    对于角度:

    对于角速度:

    对于px和py的影响则稍微复杂点,这是由于纵向加速度和角加速度都会对位置产生影响,我们也可以用积分求得,但是如果角加速度不大的话,我们可以忽略角加速度的影响用近似求解。

    px:

    py:

    1. 在确定过程模型固定项和噪声项之后我们能得到最终的过程模型。


      注意:此公式只适用于ψ˙不等于0的情况。

      CTRV based Unscented Kalman Filter

      我们已经介绍了无迹变换CTRV模型了,现在我们来在代码中使用上述技术实现基于激光雷达和毫米波雷达的多目标追踪。

    预测阶段

    1. 计算sigmal点

      在预测阶段的第一步,我们从先前时刻最后更新的状态和协方差矩阵中生成 sigma 点。 对于 CTRV 模型,我们状态维度 N =5。 但是,由于二维过程噪声向量也具有非线性效应,因此需要将二维过程噪声向量加入到新的状态向量x_aug中。 其中n_aug=5+2=7. 我们将选择2n_aug+1=15个sigma 点。

    2. 预测sigma点

      在第二步中,我们简单地将每个 sigma 点插入到 CTRV 的过程模型中,得到预测后的 sigma 点。

      此处需要注意最初状态向量是5维,过程模型的输入是个7维增强向量,过程模型返回5维向量,也就是说预测的sigma点的矩阵有5行15列。

    3. 使用预测的sigma点近似高斯分布,并计算均值和协方差

      备注:对于权重公式,我在无迹转换参数中提到了一种详细的定义方式,此处使用简化的权重公式。除此之外有的人可能对于sigma点生成公式中出现扩散系数λ,而权重公式中又出现一次λ感到疑惑。这是因为我们在sigma点生成时,使用λ扩散sigma点,在我们预测了sigma点之后,如果我们需要恢复协方差矩阵,我们必须反转sigma点的扩散,这就是权重公式要做的事情,即协助重新近似新的高斯分布并计算其均值和协方差。

      void UKF::Prediction(double delta_t) {
          // create augmented mean vector
          VectorXd x_aug = VectorXd(n_aug_);
          // create augmented state covariance
          MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
          P_aug.fill(0.0);
          // create sigma point matrix
          MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);
          Xsig_aug.fill(0.0);
          // create augmented mean state
          x_aug.head(n_x_) = x_;
          x_aug(5) = 0;
          x_aug(6) = 0;
          // create augmented covariance matrix
          P_aug.fill(0.0);
          P_aug.topLeftCorner(n_x_, n_x_) = P_;
          P_aug(5, 5) = std_a_ * std_a_;
          P_aug(6, 6) = std_yawdd_ * std_yawdd_;
          // create square root matrix
          MatrixXd L = P_aug.llt().matrixL();
          // create augmented sigma points
          Xsig_aug.col(0) = x_aug;
          for (int i = 0; i < n_aug_; ++i) {
              Xsig_aug.col(i + 1) = x_aug + sqrt(lambda_ + n_aug_) * L.col(i);
              Xsig_aug.col(i + 1 + n_aug_) = x_aug - sqrt(lambda_ + n_aug_) * L.col(i);
          }
          // predict sigma points
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              // extract values for better readability
              double p_x = Xsig_aug(0, i);
              double p_y = Xsig_aug(1, i);
              double v = Xsig_aug(2, i);
              double yaw = Xsig_aug(3, i);
              double yawd = Xsig_aug(4, i);
              double nu_a = Xsig_aug(5, i);
              double nu_yawdd = Xsig_aug(6, i);
              // predicted state values
              double px_p, py_p;
              // avoid division by zero
              if (fabs(yawd) > 0.001) {
                  px_p = p_x + v / yawd * (sin(yaw + yawd * delta_t) - sin(yaw));
                  py_p = p_y + v / yawd * (cos(yaw) - cos(yaw + yawd * delta_t));
              } else {
                  px_p = p_x + v * delta_t * cos(yaw);
                  py_p = p_y + v * delta_t * sin(yaw);
              }
              double v_p = v;
              double yaw_p = yaw + yawd * delta_t;
              double yawd_p = yawd;
              // add noise
              px_p = px_p + 0.5 * nu_a * delta_t * delta_t * cos(yaw);
              py_p = py_p + 0.5 * nu_a * delta_t * delta_t * sin(yaw);
              v_p = v_p + nu_a * delta_t;
              yaw_p = yaw_p + 0.5 * nu_yawdd * delta_t * delta_t;
              yawd_p = yawd_p + nu_yawdd * delta_t;
              // write predicted sigma point into right column
              Xsig_pred_(0, i) = px_p;
              Xsig_pred_(1, i) = py_p;
              Xsig_pred_(2, i) = v_p;
              Xsig_pred_(3, i) = yaw_p;
              Xsig_pred_(4, i) = yawd_p;
          }
          //create vector for predicted state
          // create covariance matrix for prediction
          x_.fill(0.0);
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              x_ = x_ + weights_(i) * Xsig_pred_.col(i);
          }
          P_.fill(0.0);
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              VectorXd x_diff = Xsig_pred_.col(i) - x_;
              while (x_diff(3) > M_PI) x_diff(3) -= 2. * M_PI;
              while (x_diff(3) < -M_PI) x_diff(3) += 2. * M_PI;
              P_ = P_ + weights_(i) * x_diff * x_diff.transpose();
          }
      }
      

    测量阶段

    1. 雷达测量

      由于预测状态空间到雷达测量空间是非线性变换,因此我们需要执行无迹变化,将sigma 点放入测量模型中。原本我们需要用之前得到的预测空间的高斯分布生成一遍sigma点,但是这里我们可以有两个捷径。

      • 首先,我们可以将“预测步骤”已经生成的预测sigma 点直接放入测量模型中。
      • 其次,我们不必使用测量噪声矢量来增强预测的sigma点,即在测量模型中,预测状态空间sigma 点的维度是5(之前在预测模型中增强状态是7),因为它在我们的测量模型中没有非线性影响。

      值得注意的是雷达测量更新,需要首先使用非线性转换函数h(x)将状态向量从预测空间转换到测量空间。

      这里我们需要添加测量协方差噪声R, 值得注意的是ωk+1是单纯的误差累计,不具有非线性,其对测量预测结果的影响由R表示,因此在代码中,zk+1的推导没有ωk+1项,一般情况下测量协方差噪声R中各项的标准差会由硬件厂商给出。

      void UKF::UpdateRadar(MeasurementPackage meas_package) {
      
          // set measurement dimension, radar can measure r, phi, and r_dot
          int n_z_ = 3;
          VectorXd z = VectorXd(n_z_);
          double meas_rho = meas_package.raw_measurements_(0);
          double meas_phi = meas_package.raw_measurements_(1);
          double meas_rhod = meas_package.raw_measurements_(2);
          z << meas_rho,
                  meas_phi,
                  meas_rhod;
      
          // create matrix for sigma points in measurement space
          MatrixXd Zsig = MatrixXd(n_z_, 2 * n_aug_ + 1);
      
          // mean predicted measurement
          VectorXd z_pred = VectorXd(n_z_);
          z_pred.fill(0.0);
          // measurement covariance matrix S
          MatrixXd S = MatrixXd(n_z_, n_z_);
          S.fill(0.0);
      
          // transform sigma points into measurement space
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              // 2n+1 simga points
              // extract values for better readability
              double p_x = Xsig_pred_(0, i);
              double p_y = Xsig_pred_(1, i);
              double v = Xsig_pred_(2, i);
              double yaw = Xsig_pred_(3, i);
              double v1 = cos(yaw) * v;
              double v2 = sin(yaw) * v;
      
              // measurement model
              Zsig(0, i) = sqrt(p_x * p_x + p_y * p_y); //r
              Zsig(1, i) = atan2(p_y, p_x); // phi
              Zsig(2, i) = (p_x * v1 + p_y * v2) / sqrt(p_x * p_x + p_y * p_y); //r_dot
      
              //calculate mean predicted measurement
              z_pred += weights_(i) * Zsig.col(i);
          }
      	///
          // the above code is different for Radar and Lidar Measurement Update
          // the following is just similiar
      	//
          // innovation covariance matrix S
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              // 2n+1 simga points
              // residual
              VectorXd z_diff = Zsig.col(i) - z_pred;
      
              // angle normalization
              while (z_diff(1) > M_PI) z_diff(1) -= 2. * M_PI;
              while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI;
      
              S += weights_(i) * z_diff * z_diff.transpose();
          }
          S += R_radar_;
          // create matrix for cross correlation Tc
          MatrixXd Tc = MatrixXd(n_x_, n_z_);
          Tc.fill(0.0);
      
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              // 2n+1 simga points
              // residual
              VectorXd z_diff = Zsig.col(i) - z_pred;
              // angle normalization
              while (z_diff(1) > M_PI) z_diff(1) -= 2. * M_PI;
              while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI;
      
              // state difference
              VectorXd x_diff = Xsig_pred_.col(i) - x_;
              // angle normalization
              while (x_diff(3) > M_PI) x_diff(3) -= 2. * M_PI;
              while (x_diff(3) < -M_PI) x_diff(3) += 2. * M_PI;
      
              Tc += weights_(i) * x_diff * z_diff.transpose();
          }
          //  Kalman gain K;
          MatrixXd K = Tc * S.inverse();
          // residual
          VectorXd z_diff = z - z_pred;
          // angle normalization
          while (z_diff(1) > M_PI) z_diff(1) -= 2. * M_PI;
          while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI;
          // update state mean and covariance matrix
          x_ += K * z_diff;
          P_ -= K * S * K.transpose();
          // compute normalized innovation squared(NIS)
          nis_radar_ = z_diff.transpose() * S.inverse() * z_diff;
      }
      
    2. 激光测量

      激光测量更新的基本流程与雷达更新一致,唯一的区别在于不需要将转换预测状态空间到测量空间,即不需要非线性转换函数h(x),它们之间的传递是线性的。

      void UKF::UpdateLidar(MeasurementPackage meas_package) {
          // set measurement dimension, px,py
          int n_z_ = 2;
          // create example vector for incoming lidar measurement
          VectorXd z = VectorXd(n_z_);
          z << meas_package.raw_measurements_(0),
                  meas_package.raw_measurements_(1);
          // create matrix for sigma points in measurement space
          MatrixXd Zsig = MatrixXd(n_z_, 2 * n_aug_ + 1);
          // mean predicted measurement
          VectorXd z_pred = VectorXd(n_z_);
          z_pred.fill(0.0);
          // measurement covariance matrix S
          MatrixXd S = MatrixXd(n_z_, n_z_);
          S.fill(0.0);
          // transform sigma points into measurement space
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              // 2n+1 simga points
              // extract values for better readability
              double p_x = Xsig_pred_(0, i);
              double p_y = Xsig_pred_(1, i);
              // measurement model
              Zsig(0, i) = p_x;
              Zsig(1, i) = p_y;
              // mean predicted measurement
              z_pred += weights_(i) * Zsig.col(i);
          }
          
          ///
          // the above code is different for Radar and Lidar Measurement Update
          // the following is just similiar
      	//
      
          // innovation covariance matrix S
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              // 2n+1 simga points
              // residual
              VectorXd z_diff = Zsig.col(i) - z_pred;
      
              S += weights_(i) * z_diff * z_diff.transpose();
          }
          S += R_lidar_;
          // create matrix for cross correlation Tc
          MatrixXd Tc = MatrixXd(n_x_, n_z_);
          Tc.fill(0.0);
          for (int i = 0; i < 2 * n_aug_ + 1; ++i) {
              // 2n+1 simga points
              // residual
              VectorXd z_diff = Zsig.col(i) - z_pred;
              // state difference
              VectorXd x_diff = Xsig_pred_.col(i) - x_;
              // normalize angles
              while (x_diff(3) > M_PI) x_diff(3) -= 2 * M_PI;
              while (x_diff(3) < -M_PI) x_diff(3) += 2 * M_PI;
              Tc += weights_(i) * x_diff * z_diff.transpose();
          }
          //  Kalman gain K;
          MatrixXd K = Tc * S.inverse();
          // residual
          VectorXd z_diff = z - z_pred;
          // update state mean and covariance matrix
          x_ += K * z_diff;
          P_ -= K * S * K.transpose();
          // compute normalized innovation squared(NIS)
          nis_lidar_ = z_diff.transpose() * S.inverse() * z_diff;
          }
      }
      
    3. 更新状态和协方差矩阵

    至此我们已经完成了整个无迹卡尔曼滤波项目,实现了基于激光雷达和毫米波雷达的多目标追踪。

    让我们来看看结果把!

    第一个项目是简单的对单一目标的追踪:

    GITHUB代码库:https://github.com/williamhyin/CarND-Unscented-Kalman-Filter-Project

    第二个项目稍微复杂点是对多目标的追踪:

    GITHUB代码库: https://github.com/williamhyin/SFND_Unscented_Kalman_Filter

    备注: 汽车上方的红色球体代表激光雷达探测的(x,y),紫色线条雷达探测到的沿着探测角度的速度大小。 在跟踪时没有考虑 z 轴,因此只沿 x / y 轴进行跟踪。而绿色球体上面的数字,箭头的长度和方向,代表经过无迹卡尔曼滤波预测后的车辆的位置(x,y),速度和角度。

    如果有什么疑问,可以随时联系我的个人邮箱。

    如果你觉得我的文章对你有帮助,请帮忙点个赞~\(≧▽≦)/~

    转载请私信作者!

    引用:

    • Udacity

    • [Cyrill Stachniss]: (http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam05-ukf.pdf “Unscented kalman filter”)

    展开全文
  • 前一篇文章讲了,Apollo 6.0 中融合的代码逻辑流程,但那是基于软件的角度进行梳理和分析的,这一篇文章基于上篇的成果进一步对算法进行较深入的分析,主要涉及到多目标跟踪中的数据关联和滤波算法。 目标跟踪的基础...

    前一篇文章讲了,Apollo 6.0 中融合的代码逻辑流程,但那是基于软件的角度进行梳理和分析的,这一篇文章基于上篇的成果进一步对算法进行更详细的分析,因为代码量奇大,所以本文重点讨论数据关联的一些细节。

    目标跟踪的基础知识概念

    为保持文章内容由浅入深,先做一些基础的概念介绍。

    卡尔曼滤波如何工作?

    数据融合最常见的算法是卡尔曼滤波及其变种。
    卡尔曼滤波是一个多次迭代的算法,一个周期内的工作如图所示:

    在这里插入图片描述
    一个目标初始状态,经过 T 时间后,调用 Predict() 方法,初始状态会移植到预测状态,一般 Predict() 方法都是基于特定运动公式进行计算的,比如手机上的 GPS,5s 更新一次 GPS 位置,但 5s 颗粒度太粗了,就将 1s 切分成 10 段,每一段 100ms 自己调用公式推断一次位置,这就是 Predict 过程。

    大家会发现问题,这样的结果虽然凑合着可用,但毕竟是理论上推断出来的,不会怎么靠谱。

    那就需要一个靠谱的东西,将结果及时纠偏。

    比如,你每隔 100ms 自行推断的位置,我每隔 5s 收到 GPS 数据时,用这个数据来更新数据。

    这就涉及到融合的过程了,这一过程称为 Update 或者是 Measure,融合可以简单理解为加权,就是一个比例,相信历史数据多一些还是相信检测到的数据多一些。

    然后,融合得到的值传递给下一周期的状态值,这样一轮一轮进行下去。

    单目标跟踪和多目标跟踪区别?

    讲完滤波算法,讲讲单目标跟踪和多目标跟踪的区别。

    多目标比单目标难。

    假设,车辆前方只有一个目标需要跟踪,比如 ACC 功能。

    在这里插入图片描述

    蓝色正方形是融合系统每次推断的结果值。
    橙色三角形是每次传感器检测到的结果。
    绿色圆形则是融合的数据。

    这是单个目标跟踪,基本上是 1 对 1 的关系,也就是一个目标状态值对应一个观测值。

    当然,也许传感器有杂波可能产生多个观测值,但我们同样可以通过算法进行筛选或者做加权融合最终得到一个观测值,总之在单目标跟踪中观测值和状态值一般是 1 对 1 的。

    再来看多目标跟踪问题。
    在这里插入图片描述
    紫色正方形是另外一个目标的状态值,大家很容易发现问题。

    上图标记红色问题的地方会存在一个困惑,那个橙色三角形代表的观测数据究竟属于哪个目标真实的检测信号呢?

    因为传感器本身也存在抖动,所以,你不能简单说左边,因为左边更近吧?

    这涉及到一个多目标匹配的问题,需要靠谱的数据关联(data associate)技术解决。

    Apollo 中的目标融合

    目标跟踪领域,一般用 Track 来代表一个被稳定跟踪的对象,
    但 Track 是有生命周期状态变换的。

    生命周期和状态

    • initial
    • tracking
    • temporary lost
    • destroyed

    在这里插入图片描述
    上面这张图已经跟踪清晰了,对应到代码中其实就是 3 个:

    • 新建 Track
    • 维护 Track
    • 销毁过时 Track

    讲了目标的状态后,我们需要解决一个问题:
    将检测信号和正确的 Track 进行匹配。

    数据关联中的关联矩阵?

    在代码中如何表示新检测的目标和历史 Track 的关系呢?
    在这里插入图片描述
    比如,上图中左侧代表 Tracks,右侧代表新检测到的 Objects。
    两个节点之间有连线,说明这个object 和对应的 Track 可能是同一个对象,线段上可以保存距离值,如果没有连接,距离值就为 -1。
    但大家可以看到,会出现一些多对多的情况,这个时候就需要我们用一个矩阵将objects和tracks之间的距离值保存下来。
    比如,上图的关系完全可以用一个 4x5 的数组来表示,我们称为关联矩阵。

    5,6,-1,-1,-1;
    3,-1,4,-1,-1;
    1,-1,-1,-1,-1;
    -1,-1,2,-1,3
    

    然后,就需要通过数据关联的算法去寻找最佳匹配,这个过程会迭代很多次,最终迭代到最佳状态,就是我们匹配的结果。
    比如这样:
    在这里插入图片描述
    上图所示,找到了一个好的匹配结果。
    节点 I 是 unassign obj,需要为它单独创建新的 Track。
    另外,还会出现一种情况,就是可能某些历史节点 Track,没有新的 obj 和它匹配,这个时候就需要看情况,是不是删掉这个 Track,还是继续跟踪一段时间。

    Apollo 6.0 中会有这样的关联矩阵计算过程。

    在这里插入图片描述
    先不管实现代码,单单从函数参数中我们就知道,这个函数要通过 fusion tracks 和 sensor objs 计算一个距离矩阵,并且还要详细记录未匹配上的 tracks 和 objs。

    因为代表很多,我们要克服好奇心,先不探索这个函数怎么实现的,我们关心它被谁调用。

    其实它是 hm_tracks_objects_match.cc 内部的方法,在 Associate() 方法中被调用。
    在这里插入图片描述
    Associate() 方法的代码实在是长,我简单概括了一下,它内部调用了好几个方法。

    我们很容易看到,ComputeAssociationDistanceMat() 之前有个很重要的方法 IDAssign。
    并且,我们也需要知道 ComputeAssociationDistanceMat 里面的参数变量如何得到的。
    在这里插入图片描述

    IDAssign

    在这里插入图片描述
    目标融合有几种情况需要处理:

    第 1 种情况,系统第 1 次融合时,FusionTracks 数量为 0.

    这个时候会发生什么呢?
    根据上面的代码,它会将 unassigned_measurements 的数量置为 objs 的数量.
    在这里插入图片描述
    就是一个 vector,用来保存未分配的 obj 的信息。
    如果 fusion_tracks 为空,调用 std::itoa 方法给 unassigned_measurements 赋值,从 0 开始。

    第2种情况,某次融合时,sensor objs 数量为 0

    这和第 1 种情况相反,会将 unassigned_tracks 结果更新。

    第 3 种情况,fusion_tracks,objs 数量都正常

    这个时候会根据 sensor id 和时间戳去重置位置相关的信息,然后就进入到 IDAssign 方法。

    当然,如果是前向毫米波雷达,代码是有区别对待的,它将不参与 ID 分配.

    我们需要关注的是正常情况下,ID 是怎么分配的?这个 ID 代表什么?

    在这里插入图片描述
    在 IDAssign() 方法中,有上面这么一段。
    我们需要思考 sensor_id_2_track_ind 这个 map 有什么作用,从字面意思上看应该是要标记一个 track 来自于哪个 sensor。
    可事实上不是这样的,里面保存的是track_id 和 track 在 fusion_tracks 中的索引位置一一对应关系。
    假设 fusion_tracks 里面有 3 个 Track。
    在这里插入图片描述
    那么,可以推出:
    在这里插入图片描述
    当然,前面还有一个细节,那就是前视摄像头的对象要区别对待。
    在这里插入图片描述
    这一段代码干嘛呢?
    从 sensor_objs 中提取一个 obj 的 trackid,然后去 fusion_tracks中找相同的 trackid 的 track,如果找到了就给 sensor_used 和 fusion_used 赋值,并且将这种简单的 fusion_tracks 和 sensor_objs 的匹配关系的数组索引保存到 assignment。

    然后分别检测未在 IDAssign 阶段得到 trackid 匹配的 fusion_tracks 和 sensor_objects 保存到 unassigned_fusion_tracks 和 unassigned_sensor_objects 中。

    其实 IDAssign 的目的很简单,无非就是从 sensor objs 中去查找已经存在于 fusion tracks 中的 track。

    绕回ComputeAssociationDistanceMat

    在这里插入图片描述
    了解了 IDAssign 做了什么之后,再看 ComputeAssociationDistanceMat 就会比较容易了。
    在这里插入图片描述
    association_mat 是针对 trackid 未匹配上的 track 和 sensor objs 的二维矩阵,这样避免了已经匹配上的 track 进行重复计算。
    在这里默认距离是 s_match_distance_thresh_。
    要计算的是两者中心位置的欧式距离。
    如果 center_dist 小于 s_association_center_dist_threshold_,
    那么将调用 track_object_distance_.Compute() 做精细化计算。
    在这里插入图片描述
    代码注释中讲到,因为 2D 到 3D 转换误差太大,所以数据关联的时候,阈值设置的很宽松,在这里是 30m,为了防止 camera 检测到的目标和 Lidar 的目标匹配不上。

    这一行解释其实也透露出了在实际数据关联时常见的无奈。
    重头戏在这里。
    在这里插入图片描述
    这个函数的逻辑很复杂,值得独立一小节来介绍它。

    TrackObjectDistance.compute()

    之前说过 Apollo 6.0 感知融合的传感器有 Lidar、Radar、Camera。
    所以,在某个时刻一个 Track 其实有对应 LidarObject、RadarObject、CameraObject。
    而做数据关联的时候,需要计算 sensor obj 和 track 中的哪个 object 距离最近。

    在这里插入图片描述
    这个是一一比较的过程。
    我们先看 computeLidarLidar 的实现。
    在这里插入图片描述
    在这里插入图片描述
    如果两个 Lidar 对象的中心距离大于 10 米,那么它们的距离就被重置为 max,也就是一个最大数,表示不是一个对象。

    如果在 10 米范围内,那就调用 ComputePolygonDistance3d()方法。
    在这里插入图片描述
    求 track 和 sensor 之间的距离时,并不是直接求距离。
    需要计算 track 在 sensor obj 同样时间戳下的距离,有一个航迹推断的过程,上图红线圈了出来。
    其实也就是我在文章前面部分讲到的

    measure() 前要经过 predict()

    比如 track 是在 0 s 时的位置为 p0,sensor obj 有数据参与融合时是在 1.5s,那么这个时候 track 的位置为p1= p0 + track 的速度乘以 1.5s,才是当前 track 的位置,而与 sensor obj 进行距离计算是 p1 不是 p0。

    ComputeLidarRadar 和 ComputeLidarLidar 逻辑是一样的,ComputeLidarCamera 逻辑有点不同。

    ComputeLidarCamera

    这个方法初看不起眼,没想到跟踪代码时简直让我感受到陷入泥坑当中,代码量太多了。

    这个方法是计算 Lidar 目标和 Camera 目标的距离。

    在这里插入图片描述
    首先是判断 lidar 检测到的 object 和 camera 检测到的 object id 是不是一致。

    如果不一致的话,需要判断两者的距离是不是超过了一个动态的距离阈值。
    调用的是 LidarCameraCenterDistanceExceedDynamicThreshold 方法。
    判断的依据是两个obj的中心点的差值,是否大于 5 + 0.15 * local_distance,默认情况下 local_distance 为 60,如果存在点云数据则取最近的点云距离,总体阈值应该是 9 米的样子。

    如果超过了,直接返回 distance_thresh_ 在这里插入图片描述
    计算 Lidar 和 Camera object 的 distance 有两种情况。
    一种是当前有点云数据,另外一种是无点云数据。

    有点云数据的情况需要计算一个相似度,这个相似度取值范围是 0 ~ 1,1 代表完全一致,0 代表不一致。

    相似度怎么计算呢?

    1. 计算点云数据和camera数据的距离相似度和形状相似度;
    2. 融合距离相似度和形状相似度。

    ComputePtsBoxSimilarity。

    在这里插入图片描述
    求距离的相似度时,基本思路是算一个点云到 camera bbox 的平均距离,然后再以 box 的尺寸进行归一化操作。

    在这里插入图片描述
    在这里假设 mean_pixel_dist 是服从高斯分布,然后计算卡方分布。
    这里运用了统计学中的卡方检验工具。
    卡方检验是要假设 H0 和 H1 的。
    在这里 H0 假设应该是:Lidar 和 camera 是一致的。
    然后通过计算两者在 x,y 维度的卡方值,来验证假设。
    ChiSquaredCdf2TableFun 求的是累积的概率。
    在这里插入图片描述
    这个是需要查询卡方分布表的。
    在这里插入图片描述
    提前将卡方值和对应的 p 值存储起来,dist_int 代表的是卡方分布表中的卡方值,在上面这个 table 中通过 dist_int 能够查找到对应的 p 值。

    原始的卡方检验中,卡方值越大,p 值越小,如果 p 值小于显著性水平比如 0.05,那么原假设就可以算不成立。

    但我观察 Apllo 上方的分布表,dist_int 越小,p 值也越小。那么这个 p 代表什么呢?

    代表 H0 假设不成立的概率?

    而后面相似度的计算是

    double location_similarity =
          1 - ChiSquaredCdf2TableFun(square_norm_mean_pixel_dist);
    

    需要 1 减去这个概率,得到 H0 假设成立的概率,似乎也符合我的推断,但说实话,我这边还没有怎么整明白,只是根据代码去推断它的算法思想,在这一块比较了解的同学可以留言交流。

    上面计算的是距离相似度,还有个形状相似度,最终两个相似度要进行融合,产生一个新的相似度。

    在这里插入图片描述
    注释写的很明白,输入的概率值如果 sum 大于 1,那么最终的结果会大于 0.5,反之小于 0.5.
    有了相似度后,终于可以得到最终的 distance 了。
    在这里插入图片描述
    相似度越高,distance 越小。

    以上就是 Lidar 和 Camera 目标间的距离计算。

    ComputeRadarCamera

    计算 Radar 和 Camera 之间的距离和 LidarCamera 类似,但又有一些不同。

    Radar 返回来的是一个点,需要通过一个点生成 8 个顶点,其实就是一个长方体的 8 个顶点。

    在这里插入图片描述
    代码很容易编写。
    在这里插入图片描述
    需要注意到的细节是,因为 Radar 探测到的目标是有方向信息的,所以生成长方体的时候要考虑到 xy 平面的旋转。

    radar 的中心点需要投影到相机坐标系,形成 local_pt。

    最终通过求 X、Y、Loc、Velocity 的相似度,得到最终融合后的相似度 fused_similarity。

    通过 fused_similarity 求得最终的 distance,这个和之前的 LidarCamera 基本思路一致。
    在这里插入图片描述
    X 相似度主要通过一个 WelshVarLossFunc 确定的。
    在这里插入图片描述
    说实话,我不大懂它的用意,但看代码可以粗略估计到,如果 dist 非常小,小于阈值,那么说明两者相似度非常高,给一个权重值。
    如果 dist 很大,那么就需要缩放,将 p 最终的结果限制在(0,1] 区间。

    我比较关心的是 Radar 的速度和 Camera 的速度如何求相似度。
    在这里插入图片描述

    ComputeCameraCamera

    在这里插入图片描述
    这里直接返回最大的数了,意思肯定就是计算 Ojbect 和 track 的时候优先考虑 Liar、Radar 与 Camera 的距离关系。

    MinimizeAssignment

    计算了 Object 和 track 之间的距离后,用矩阵保存下来。
    然后就需要执行匈牙利匹配算法了。
    在这里插入图片描述
    optimizer 声明如下:
    在这里插入图片描述
    就把它当成一个黑盒子吧,下一篇文章讲解 Apollo 6.0 中的匈牙利算法实现。

    所以,MinimizeAssignment 进行了实际的目标匹配。

    postIdAssign

    数据关联之后,还需要进行一些后处理。
    主要是要提取一些有效的 track.
    在这里插入图片描述
    如果一个 track 没有匹配上,但它只有上一次的 camera 数据,没有 lidar 数据,那么还是认为它有效。

    ComputeDistance

    数据关联的最后,需要计算距离,距离指得是一个距离分数。

    总结

    数据关联还挺难的,代码很多,很多细节都还来不及展开。

    总之,要做好数据融合要注意下面几个点:

    1. 目标的生命周期管理
    2. 目标的 ID 匹配
    3. ID 匹配需要注意计算 Object 和 Measurement 的距离
    4. 距离不等同于欧式距离,它是为了计算相似度,相似度越大,距离越小。
    5. 匈牙利算法比较复杂,但好在有成熟的现成的代码可以实现。
    6. 数据关联之后,还需要进行一些后期处理。
    7. 数据关联做好之后,后面的卡尔曼滤波算法才能正确的运行起来。
    展开全文
  • 自动驾驶感知技术 视觉感知技术发展 本报告主要介绍面向自动驾驶的视觉感知技术。首先是对自动驾 驶视觉感知发展的行业综述, 介绍了自动驾驶感知技术的发展路 线, 以及视觉传感器在其中的作用; 其次介绍了车载...
  • 传感器融合标定算法,另外还包含了自动驾驶学习资料 涵盖感知,规划和控制,ADAS,传感器; 1. apollo相关的技术教程和文档; 2.adas(高级辅助驾驶)算法设计(例如AEB,ACC,LKA等) 3.自动驾驶鼻祖mobileye的论文和...
  • 没法做推理、类比、联想 空中飘浮的塑料袋、树叶 路上的水洼、阴影 路上电子广告牌中嵌入几帧STOP等一些字样 在道路上投影人影、路标、道路标志线 ...周边环境的声音感知 听到警车、救护车警笛要避让 ...

    没法做推理、类比、联想

    • 空中飘浮的塑料袋、树叶
    • 路上的水洼、阴影
    • 路上电子广告牌中嵌入几帧STOP等一些字样
    • 在道路上投影人影、路标、道路标志线
    • 花花绿绿的广告牌、汽车红色尾灯与交通信号灯的识别
    • 雨雪雾天气场景
    •  长隧道场景
    • 道路上贴上贴纸

    周边环境的声音感知

    • 听到警车、救护车警笛要避让
    展开全文
  • 引言 自动驾驶感知和定位中传感器融合成了无人驾驶领域的趋势,融合按照实现原理分为硬件层的融合, 如禾赛和Mobileye等传感器厂商, 利用传感器的底层数据进行融合;数据层, 利用传感器各种得到的后期数据,即每个...
  • 自动驾驶多传感器融合

    万次阅读 2018-01-02 10:01:50
    简单地说,Pandora指的是一套以激光雷达、环视摄像头模组、多传感器融合感知识别算法为一体的自动驾驶开发者套件,它实际上是一种新型的“多传感器融合”技术。 通常,业界所说的“多传感器
  • Pandora是全球第一套专为自动驾驶场景设计的多传感器环境感知融合与识别系统。核心硬件包括一个禾赛科技40线机械式激光雷达、四个广角黑 白摄像头和一个彩色摄像头。通过数据同步采集和空间匹配,Pandora可以实现...
  • 融合后的多维综合数据进行感知。 在原始层把数据都融合在一起,融合好的数据就好比是一个 Super 传感器,而且这个传感器不仅有能力可以看到红外线,还有能力可以看到摄像头或者 RGB ,也有能力看到 LiDAR 的三...
  • 自动驾驶感知概述

    千次阅读 2020-08-29 20:52:05
    为了确保自动驾驶车辆对周围环境的获取和识别,自动驾驶系统的感知模块需要通过各种传感器获取大量的环境信息,包括自车的状态、交通流信息、道路状况、交通标志等,这些传感器主要包括:激光雷达(Lidar)、摄像头...
  • 基于多模态融合自动驾驶感知及计算.pdf
  • 说起自动驾驶感知系统,大家都会谈论到感知融合,这涉及到不同传感器数据在时间、空间的对齐和融合,最终的结果将提升自动驾驶系统的感知能力,因为我们都知道单一的传感器都是有缺陷的。本篇文章梳理 Apollo 6.0 中...
  • 本文将继续为大家解析自动驾驶所涉及到的核心技术,上篇《感知篇》内容可以查看历史原文。 二、决策篇 在一套相对成熟的自动驾驶技术体系中,如果将环境感知模块比作人的眼睛和耳朵,那么决策规划模块就相当于自动...
  • 自动驾驶 感知融合
  • 传感器融合-数据篇(自动驾驶

    千次阅读 2020-03-05 07:30:00
    自动驾驶感知模块中 传感器融合 已经成为了标配,只是这里融合的层次有不同,可以是硬件层(如禾赛,Innovusion的产品),也可以是数据层(这里的讨论范围),还可以是任务层像障碍物检测(obstacle detection),...
  • 在simulink中搭建的基于相机和毫米波雷达的障碍物检测,检测前方车辆并进行ACC的整套模型。已在实车中进行验证。
  • 浅谈自动驾驶系统感知系统

    千次阅读 2020-09-30 15:27:52
    自动驾驶并不是一项单一的技术,而是汽车电子、智能控制以及互联网等技术发展融合的产物,其原理为自动驾驶系统通过感知系统,获取车辆自身信息与周围环境信息,经过处理器对采集到的数据信息进行分析计算和处理,...
  • 如何入门学习自动驾驶感知技术?

    千次阅读 2019-12-04 19:29:16
    点击上方“3D视觉工坊”,选择“星标”干货第一时间送达本文授权转载自公众号:自动驾驶视觉感知工程师一、前言目前车企正在经历着前所未有的改革,包括前两天奥迪的裁员计划:将在2025年裁员9...
  • 自动驾驶概述

    万次阅读 2020-02-08 20:27:27
    随着5G逐渐走进人们的视线,自动驾驶汽车成为一个社会热门话题。那么自动驾驶究竟是什么样的?它真的能实现吗?什么时候我们才能真正使用上呢? 汽车是当今社会的主要交通工具之一,自动驾驶汽车是目前可以看到,...
  • 点上方蓝字人工智能算法与Python大数据获取更多干货在右上方···设为星标★,第一时间获取资源仅做学术分享,如有侵权,联系删除转载于 :Kittcamp自动驾驶学院/导读/随着...
  • 讲座内容主要包括自动驾驶系统的总览,自动驾驶感知的介绍,以及感知的前沿动态和总结。 1.自动驾驶系统总览 关于自动驾驶系统,目前主流的L4级别自动驾驶系统通常包括预先提供的地图以及众多传感器,具体传感器系统...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 9,825
精华内容 3,930
关键字:

自动驾驶感知融合