欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

多传感器融合定位1(激光雷达+毫米波雷达)

程序员文章站 2022-03-07 13:06:12
...

前言

LZ最近在看Udacity的无人驾驶课程,该课程主要分为三部分,第一部分的课程主要使用Python实现的车道线识别、车牌识别等计算机视觉项目。由于我对定位、建图等方面有些知识储备,所以先从第二部分课程开始。
本节将用最简洁的话讲解卡尔曼滤波KF、非线性卡尔曼滤波EKF等知识点,并就此实现一个多传感器融合定位的小demo,后面会就粒子滤波PF专门开一个章节讲解。

对于匀速运动模型,KF和EKF的状态预测(Predict)过程是一样的;KF和EKF唯一区别的地方在于测量值更新(Update)这一步。在测量值更新中,KF使用的测量矩阵H是不变的,而EKF的测量矩阵H是Jacobian矩阵。

一、卡尔曼滤波 KF

1、引子

1)两个传感器测量同一个信号,为了减小误差我们可以采用取平均的方式,
2)进一步的我们采用加权平均(由方差大小分配),但加权平均是一种静态分配方式。3)方差是随外界环境而变的,加权值也应该随之改变,这就是卡尔曼滤波出现的原因,它是一种动态更新加权值,不断迭代的算法。

卡尔曼滤波器就是根据上一时刻x(k-1)的状态,预测当前时刻x(k)的状态,将预测的状态x^(k)与当前时刻的测量值z进行加权,加权后的结果才认为是当前的实际状态。

2、对象:线性高斯模型,这个模型最好的地方在于两个高斯分布的乘积仍然是一高斯分布,由此实现模型的动态迭代。

3、本质:参数化的贝叶斯模型。先验估计:对下一时刻系统初步状态估计;结合先验估计以及测量反馈,得到后验估计。

4、应用:最优化自回归数据处理算法、机器人导航、雷达系统、图像处理等

2、黄金公式及推导

直接上结论,下面这个公式分为两部分,上面的是预测,下面是更新。
多传感器融合定位1(激光雷达+毫米波雷达)
具体公式推导,我竟然发现了我之前的笔记,下面直接上图解释。
为了避免推导晦涩难懂,采用小汽车的模型进行一步一步的推导。

1、建立模型

多传感器融合定位1(激光雷达+毫米波雷达)
多传感器融合定位1(激光雷达+毫米波雷达)

2、简单一维小车

假设有个小车匀速运动,我们在左侧安装了一个测量小车距离和速度传感器,每秒测一次小车位置s和速度v。我们用向量xtx_{t}状态向量。

由于测量误差的存在,无法获得真实值,我们可以以一个高斯分布来表示结果在各个地方出现的概率,如下图所示,在真值附近的概率最高。
多传感器融合定位1(激光雷达+毫米波雷达)
xt1=[st1vt1]x_{t-1}=\begin{bmatrix} s_{t-1}\\ v_{t-1} \end{bmatrix}
接下来是使用历史信息对未来的位置进行推测,以速度v匀速行走Δt\Delta t后,小车位置的红色区域范围变大了,这是因为预测时加入了速度估计的噪声,放大了不确定性。
多传感器融合定位1(激光雷达+毫米波雷达)
[stvt]=[1Δt01][st1vt1]\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}
此时传感器对小车做了一次观测,结果为 z

多传感器融合定位1(激光雷达+毫米波雷达)
上图蓝色区域为此时观测结果,红色为预测结果,那么最终的结果是怎样的呢?下图绿色部分给出了答案。
多传感器融合定位1(激光雷达+毫米波雷达)
多传感器融合定位1(激光雷达+毫米波雷达)
我们对预测和观测结果用不同的权重得到最终结果,两个权值的计算是根据预测结果和观测结果的不确定性来的,这个不确定性就是高斯分布中的方差的大小,方差越大,波形分布越广,不确定性越高,这样一来给的权值就会越低。
多传感器融合定位1(激光雷达+毫米波雷达)

3、预测(Predict)

多传感器融合定位1(激光雷达+毫米波雷达)
上面是一维小车,状态向量里只有它的位置和速度,当二维小车在平面时它的状态向量有分别是位置和速度的x、y方向(x,y,vx,vy)
x=[xyvxvy]x_{}=\begin{bmatrix}x\\ y\\vx\\vy\end{bmatrix}

和上面的一维对应
[stvt]=[1Δt01][st1vt1]\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}
这里的二维应该是
多传感器融合定位1(激光雷达+毫米波雷达)
[xtytvxtvyt]=[10Δt0010Δt00100001][xt1yt1vxt1vyt1]\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}=\begin{bmatrix} 1& 0 & \Delta t& 0\\ 0 & 1 & 0 & \Delta t\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1 \end{bmatrix}\begin{bmatrix}x_{t-1}\\ y_{t-1}\\vx_{t-1}\\vy_{t-1}\end{bmatrix}+Bu
接下来是预测的第二个模块,状态协方差矩阵P和过程噪声Q。P表示系统的不确定程度,卡尔曼滤波器初始化时很大,随着更多数据注入滤波器,不确定度会逐渐变小。Q暂时设为单位矩阵。
多传感器融合定位1(激光雷达+毫米波雷达)
P=[10000100001000000100]P=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 100 & 0\\ 0 & 0& 0 & 100 \end{bmatrix}
Q=[1000010000100001]Q=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1\end{bmatrix}

4、观测更新(measurement update)

多传感器融合定位1(激光雷达+毫米波雷达)
多传感器融合定位1(激光雷达+毫米波雷达)
先说第一个公式括号里面的z-Hx,z表示的是测量值,x是4维的状态向量,对于激光雷达测量的z就是位置(x,y),对于毫米波雷达测量的z是位置和角度。需要对状态向量x左乘一个矩阵H,才能将二者映射到同一个空间,直接进行加减运算。
以激光雷达的测量值z为:
z=[xmym]z=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}
z-Hx扩展开为:
[ΔxΔy]=[xmym][10000100][xtytvxtvyt]\begin{bmatrix}\Delta x\\\Delta y\end{bmatrix}=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}-\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}*\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}
测量矩阵 H 是一个2*4的矩阵
H=[10000100]H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}

    H_lidar_ = Eigen::MatrixXd(2, 4);
    H_lidar_ << 1, 0, 0, 0,
                0, 1, 0, 0;

接下来说K(z-Hx)中的权重K是如何取的,简单来说是和协方差矩阵相关。

    R_lidar_ = Eigen::MatrixXd(2, 2);
    R_lidar_ << 0.0225, 0,
                0, 0.0225;

多传感器融合定位1(激光雷达+毫米波雷达)
这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K(Kalman Gain),用人话讲就是求y值的权值。第一个公式中的R是测量噪声矩阵(measurement covariance matrix),这个表示的是测量值与真值之间的差值,R应该是2*2的矩阵。一般情况下,传感器的厂家会提供该值。
多传感器融合定位1(激光雷达+毫米波雷达)
求得K之后,当前时刻的x和P都可以求解出来了,第一个公式是完成了当前状态向量x的更新,不仅考虑了上一时刻的预测值,也考虑了测量值,和整个系统的噪声,第二个公式根据卡尔曼增益,更新了系统的不确定度P,用于下一个周期的运算。

二、非线性卡尔曼滤波EKF

KF中测量矩阵H是固定的,代码中直接赋值即可。
H=[10000100]H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}
但是真实情况下,测量矩阵 H应该是需要求雅克比矩阵求出的。我们接下来以毫米波雷达为例
多传感器融合定位1(激光雷达+毫米波雷达)
毫米波原理是多普勒效应,能够测量障碍物在极坐标系下雷达的距离ρ,方向角ϕ和距离的变化率(径向速度)ρ’。
多传感器融合定位1(激光雷达+毫米波雷达)
已知量:状态向量x为4*1,
多传感器融合定位1(激光雷达+毫米波雷达)

已知量:观测值z的数据维度为3*1.
多传感器融合定位1(激光雷达+毫米波雷达)
观测值z和预测值x之间的差值y关系为:
多传感器融合定位1(激光雷达+毫米波雷达)
多传感器融合定位1(激光雷达+毫米波雷达)
但是如果转化为 y=Hx时,由于第二部分的转化是非线性的,无法找到一个常数矩阵H。
多传感器融合定位1(激光雷达+毫米波雷达)
所以我们需要将上述非线性函数转化为近似的线性函数,用一阶泰勒展开。对状态向量x求偏导数,即雅克比Jacobian矩阵。
多传感器融合定位1(激光雷达+毫米波雷达)
多传感器融合定位1(激光雷达+毫米波雷达)
多传感器融合定位1(激光雷达+毫米波雷达)

三、代码讲解

KF和EKF都在kalmanfilter.h/cpp文件中,头文件KalmanFilter类为:

class KalmanFilter {
public:
    // 构造函数和析构函数
    KalmanFilter();
    ~KalmanFilter();
    // 初始化
    void Initialization(Eigen::VectorXd x_in);
    bool IsInitialized();

    // 5个矩阵赋值
    void SetF(Eigen::MatrixXd F_in);
    void SetP(Eigen::MatrixXd P_in);
    void SetQ(Eigen::MatrixXd Q_in);
    void SetH(Eigen::MatrixXd H_in);
    void SetR(Eigen::MatrixXd R_in);
    // 预测
    void Prediction();
    // KF更新
    void KFUpdate(Eigen::VectorXd z);
    // EKF更新
    void EKFUpdate(Eigen::VectorXd z);
    // 获得状态x
    Eigen::VectorXd GetX();

private:
    // Jacobian矩阵
    void CalculateJacobianMatrix();

    // 是否初始化标志
    bool is_initialized_;

    // 状态向量x
    Eigen::VectorXd x_;

    // 状态协方差矩阵P
    Eigen::MatrixXd P_;
    // 状态转移矩阵F  
    Eigen::MatrixXd F_;
    // 过程噪声Q 
    Eigen::MatrixXd Q_;
    // 测量矩阵H 
    Eigen::MatrixXd H_;
    // 测量噪声R 
    Eigen::MatrixXd R_;
};

多传感器融合定位1(激光雷达+毫米波雷达)
主要成员变量为:状态转移矩阵F、状态协方差矩阵P、测量矩阵H、过程噪声Q、测量噪声R、状态向量x
主要成员函数为:初始化Initialization()、5个矩阵赋值set()、预测prediction()、KF更新KFUpdate()、EKF更新EKFUpdate().

1、预测Prediction()

多传感器融合定位1(激光雷达+毫米波雷达)

// x=F*x
// P=F*P*Ft+Q
void KalmanFilter::Prediction()
{
    x_ = F_ * x_;
    Eigen::MatrixXd Ft = F_.transpose();
    P_ = F_ * P_ * Ft + Q_;
}

2、KF更新KFUpdate()

多传感器融合定位1(激光雷达+毫米波雷达)

// x=x+k*(z-H*x)
// P=(I-K*h)*P
// K=P*Ht*(H*P*Ht+R)t
void KalmanFilter::KFUpdate(Eigen::VectorXd z)
{
    Eigen::VectorXd y = z - H_ * x_;
    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd S = H_ * P_ * Ht + R_;
    Eigen::MatrixXd Si = S.inverse();
    Eigen::MatrixXd K =  P_ * Ht * Si;
    x_ = x_ + (K * y);
    int x_size = x_.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
}

3、EKF更新EKFUpdate()

EKF的预测两个公式和之前一样,都是Prediction()函数,区别在于求解测量矩阵H矩阵。
首先是要求解测量值z和预测值x之间的偏差。y=z-Hx。
这里Hx用的就是将x从笛卡尔坐标系转化为极坐标系。
接下来的主要问题是在求解H矩阵。

// KF和EKF区别在于测量矩阵H的计算
void KalmanFilter::EKFUpdate(Eigen::VectorXd z)
{
    double rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1));
    double theta = atan2(x_(1), x_(0));
    double rho_dot = (x_(0)*x_(2) + x_(1)*x_(3)) / rho;
    Eigen::VectorXd h = Eigen::VectorXd(3);
    h << rho, theta, rho_dot;
    Eigen::VectorXd y = z - h;

    CalculateJacobianMatrix();

    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd S = H_ * P_ * Ht + R_;
    Eigen::MatrixXd Si = S.inverse();
    Eigen::MatrixXd K =  P_ * Ht * Si;
    x_ = x_ + (K * y);
    int x_size = x_.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
}

4、CalculateJacobianMatrix()函数

多传感器融合定位1(激光雷达+毫米波雷达)

void KalmanFilter::CalculateJacobianMatrix()
{
    Eigen::MatrixXd Hj(3, 4);

    // get state parameters
    float px = x_(0);
    float py = x_(1);
    float vx = x_(2);
    float vy = x_(3);

    // pre-compute a set of terms to avoid repeated calculation
    float c1 = px * px + py * py;
    float c2 = sqrt(c1);
    float c3 = (c1 * c2);

    // Check division by zero
    if(fabs(c1) < 0.0001){
        H_ = Hj;
        return;
    }

    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;
    H_ = Hj;
}

5、main主函数

主函数基本流程为:

int main()
{
    KalmanFilter kf;
    while(getlint(x,y,时间戳)){
         if(激光雷达尚未初始化){
             kf.Initialization(x_in);
             P<< 4*4;
             Q<< 4*4;
             H<< 2*4;
             R<<2*2;
         }
         获取两帧时间差delta t;
         F<< 4*4;
         kf.Prediction();
         kf.KFUpdate();
         VectorXd x_out=kf.GetX();
    }
}

多传感器融合定位1(激光雷达+毫米波雷达)

参考:
https://zhuanlan.zhihu.com/p/45238681

相关标签: 无人驾驶