无人驾驶技术——激光雷达测量

逃离我推掉我的手 2024-04-17 17:52 71阅读 0赞

### 文章目录 ###

*  激光雷达测量模型
 *   *  参数定义
     *  测量向量z
     *  测量矩阵H
     *  协方差矩阵R
 *  编程实现激光跟踪

# 激光雷达测量模型 #

## 参数定义 ##

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70]

## 测量向量z ##

z是测量矢量。对于激光雷达传感器，矢量z是包含位置x和位置y测量。

## 测量矩阵H ##

H是将你对物体当前状态的信息投射到传感器测量空间的矩阵。对于激光雷达来说，这是一种奇特的说法，即我们丢弃状态变量中的速度信息，因为激光雷达传感器只测量位置：状态向量x是一个四维向量， 包含关于位置信息  
![\]][20190829155031611.png]  
矢量z将只包含\[px，py\]，乘以hx，我们可以比较x，我们的信仰，和z，传感器测量值。

x向量中的素数符号代表什么意思呢？  
Px’意味着您已经完成了预测步骤，但尚未完成测量步骤。换言之，这个物体是在位置Px, 经过一段时间 t\_t 之后 。你计算出你认为物体将基于运动模型的位置，得到Px’

举个例子，如何找到正确的H矩阵，从4d状态投影到2d观察空间，如下所示：  
![在这里插入图片描述][2019082916050859.png]  
为了获得我们的测量值（x和y），我们需要确定H矩阵。（x和y）是用H乘以状态向量得到的。

我们先来找出H矩阵的维数。这里我们有一个2乘1的矩阵，它来自m乘n乘h的矩阵乘以四行一列的矩阵。现在，从矩阵乘法中我们知道，第一个矩阵的列数应该等于第二个矩阵的行数，即4。并且第一个矩阵的行数与结果矩阵的行数相同，即2。所以我们的h是一个2行4列的矩阵。最后，我们把1和0放进去，使px和py坐标传播到结果z。  
![在这里插入图片描述][20190829160540644.png]

## 协方差矩阵R ##

测量噪声协方差矩阵R

现在，让我们来看一下协方差矩阵，R，它表示传感器测量中的不确定性。R矩阵的尺寸为正方形，其每边的长度与测量参数的数量相同。

对于激光传感器，我们有一个二维测量矢量。每个位置分量px，py都受到随机噪声的影响。所以我们的噪声向量ω与z的维数相同。它是一个平均值为零的分布，协方差矩阵是2×2，来自于垂直向量ω和它的转置的乘积。

![在这里插入图片描述][2019082916085973.png]

其中R是测量噪声协方差矩阵；换句话说，矩阵R代表我们从激光传感器接收到的位置测量的不确定性。

通常，随机噪声测量矩阵的参数将由传感器制造商提供。对于扩展卡尔曼滤波项目，我们为雷达传感器和激光雷达传感器提供了R矩阵值。

记住，R中的非对角0表示噪声过程不相关。

# 编程实现激光跟踪 #

下面我们就来实现一个在二维坐标系中，利用Kalman滤波器进行追踪行人。

激光雷达测量能预测到行人的位置x和y，但是没有速度信息。![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 1]  
其中F矩阵和Q矩阵如下：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 2]

tracking.cpp

#include "tracking.h"
    #include <iostream>
    #include "Dense"
    
    using Eigen::MatrixXd;
    using Eigen::VectorXd;
    using std::cout;
    using std::endl;
    using std::vector;
    
    Tracking::Tracking() {
      is_initialized_ = false;
      previous_timestamp_ = 0;
    
      // create a 4D state vector, we don't know yet the values of the x state
      kf_.x_ = VectorXd(4);
    
      // state covariance matrix P
      kf_.P_ = MatrixXd(4, 4);
      kf_.P_ << 1, 0, 0, 0,
                0, 1, 0, 0,
                0, 0, 1000, 0,
                0, 0, 0, 1000;
    
    
      // measurement covariance
      kf_.R_ = MatrixXd(2, 2);
      kf_.R_ << 0.0225, 0,
                0, 0.0225;
    
      // measurement matrix
      kf_.H_ = MatrixXd(2, 4);
      kf_.H_ << 1, 0, 0, 0,
                0, 1, 0, 0;
    
      // the initial transition matrix F_
      kf_.F_ = MatrixXd(4, 4);
      kf_.F_ << 1, 0, 1, 0,
                0, 1, 0, 1,
                0, 0, 1, 0,
                0, 0, 0, 1;
    
      // set the acceleration noise components
      noise_ax = 5;
      noise_ay = 5;
    }
    
    Tracking::~Tracking() {
    
    }
    
    // Process a single measurement
    void Tracking::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
      if (!is_initialized_) {
        //cout << "Kalman Filter Initialization " << endl;
    
        // set the state with the initial location and zero velocity
        kf_.x_ << measurement_pack.raw_measurements_[0], 
                  measurement_pack.raw_measurements_[1], 
                  0, 
                  0;
    
        previous_timestamp_ = measurement_pack.timestamp_;
        is_initialized_ = true;
        return;
      }
    
      // compute the time elapsed between the current and previous measurements
      // dt - expressed in seconds
      float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
      previous_timestamp_ = measurement_pack.timestamp_;
      
      // TODO: YOUR CODE HERE
      
      float dt_2 = dt * dt;
      float dt_3 = dt_2 * dt;
      float dt_4 = dt_3 * dt;
      
      // 1. Modify the F matrix so that the time is integrated
      
      kf_.F_(0,2) = dt;
      kf_.F_(1,3) = dt;
      
      // 2. Set the process covariance matrix Q
      kf_.Q_ = MatrixXd(4, 4);
      kf_.Q_ << dt_4 *noise_ax/4 , 0, dt_3 *noise_ax/2, 0,
                0, dt_4 *noise_ay/4, 0, dt_3 *noise_ay/2,
                dt_3 *noise_ax/2, 0, dt_2*noise_ax, 0,
                0, dt_3 *noise_ay/2, 0, dt_2*noise_ay;
      
      // 3. Call the Kalman Filter predict() function
      kf_.Predict();
      // 4. Call the Kalman Filter update() function
      //      with the most recent raw measurements_
      kf_.Update(measurement_pack.raw_measurements_);
      
      cout << "x_= " << kf_.x_ << endl;
      cout << "P_= " << kf_.P_ << endl;
    }

tracking.h

#ifndef TRACKING_H_
    #define TRACKING_H_
    
    #include <vector>
    #include <string>
    #include <fstream>
    #include "kalman_filter.h"
    #include "measurement_package.h"
    
    class Tracking {
     public:
      Tracking();
      virtual ~Tracking();
      void ProcessMeasurement(const MeasurementPackage &measurement_pack);
      KalmanFilter kf_;
    
     private:
      bool is_initialized_;
      int64_t previous_timestamp_;
    
      //acceleration noise components
      float noise_ax;
      float noise_ay;
    
    };
    
    #endif  // TRACKING_H_

kalman\_filter.h

#ifndef KALMAN_FILTER_H_
    #define KALMAN_FILTER_H_
    
    #include "Dense"
    
    using Eigen::MatrixXd;
    using Eigen::VectorXd;
    
    class KalmanFilter {
     public:
    
      /**
       * Constructor
       */
      KalmanFilter();
    
      /**
       * Destructor
       */
      virtual ~KalmanFilter();
    
      /**
       * Predict Predicts the state and the state covariance
       *   using the process model
       */
      void Predict();
    
      /**
       * Updates the state and
       * @param z The measurement at k+1
       */
      void Update(const VectorXd &z);
      
      // state vector
      VectorXd x_;
    
      // state covariance matrix
      MatrixXd P_;
    
      // state transistion matrix
      MatrixXd F_;
    
      // process covariance matrix
      MatrixXd Q_;
    
      // measurement matrix
      MatrixXd H_;
    
      // measurement covariance matrix
      MatrixXd R_;
    
    };
    
    #endif  // KALMAN_FILTER_H_

kalman\_filter.cpp

#include "kalman_filter.h"
    
    KalmanFilter::KalmanFilter() {
    }
    
    KalmanFilter::~KalmanFilter() {
    }
    
    void KalmanFilter::Predict() {
      x_ = F_ * x_;
      MatrixXd Ft = F_.transpose();
      P_ = F_ * P_ * Ft + Q_;
    }
    
    void KalmanFilter::Update(const VectorXd &z) {
      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 PHt = P_ * Ht;
      MatrixXd K = PHt * Si;
    
      //new estimate
      x_ = x_ + (K * y);
      long x_size = x_.size();
      MatrixXd I = MatrixXd::Identity(x_size, x_size);
      P_ = (I - K * H_) * P_;
    }

measurement\_package.h

#ifndef MEASUREMENT_PACKAGE_H_
    #define MEASUREMENT_PACKAGE_H_
    
    #include "Dense"
    
    class MeasurementPackage {
     public:
    
      enum SensorType {
        LASER, RADAR
      } sensor_type_;
    
      Eigen::VectorXd raw_measurements_;
      
      int64_t timestamp_;
    
    };
    
    #endif  // MEASUREMENT_PACKAGE_H_

main.cpp

#include <iostream>
    #include <sstream>
    #include <vector>
    #include "Dense"
    #include "measurement_package.h"
    #include "tracking.h"
    
    using Eigen::MatrixXd;
    using Eigen::VectorXd;
    using std::cout;
    using std::endl;
    using std::ifstream;
    using std::istringstream;
    using std::string;
    using std::vector;
    
    
    int main() {
    
      /**
       * Set Measurements
       */
      vector<MeasurementPackage> measurement_pack_list;
    
      // hardcoded input file with laser and radar measurements
      string in_file_name_ = "obj_pose-laser-radar-synthetic-input.txt";
      ifstream in_file(in_file_name_.c_str(), ifstream::in);
    
      if (!in_file.is_open()) {
        cout << "Cannot open input file: " << in_file_name_ << endl;
      }
    
      string line;
      // set i to get only first 3 measurments
      int i = 0;
      while (getline(in_file, line) && (i<=3)) {
    
        MeasurementPackage meas_package;
    
        istringstream iss(line);
        string sensor_type;
        iss >> sensor_type; // reads first element from the current line
        int64_t timestamp;
        if (sensor_type.compare("L") == 0) {  // laser measurement
          // read measurements
          meas_package.sensor_type_ = MeasurementPackage::LASER;
          meas_package.raw_measurements_ = VectorXd(2);
          float x;
          float y;
          iss >> x;
          iss >> y;
          meas_package.raw_measurements_ << x,y;
          iss >> timestamp;
          meas_package.timestamp_ = timestamp;
          measurement_pack_list.push_back(meas_package);
    
        } else if (sensor_type.compare("R") == 0) {
          // Skip Radar measurements
          continue;
        }
        ++i;
      }
    
      // Create a Tracking instance
      Tracking tracking;
    
      // call the ProcessingMeasurement() function for each measurement
      size_t N = measurement_pack_list.size();
      // start filtering from the second frame 
      // (the speed is unknown in the first frame)
      for (size_t k = 0; k < N; ++k) {
        tracking.ProcessMeasurement(measurement_pack_list[k]);
      }
    
      if (in_file.is_open()) {
        in_file.close();
      }
      return 0;
    }

运行结果如下：

x\_= 0.96749  
0.405862  
4.58427  
\-1.83232  
P\_= 0.0224541 0 0.204131 0  
0 0.0224541 0 0.204131  
0.204131 0 92.7797 0  
0 0.204131 0 92.7797  
x\_= 0.958365  
0.627631  
0.110368  
2.04304  
P\_= 0.0220006 0 0.210519 0  
0 0.0220006 0 0.210519  
0.210519 0 4.08801 0  
0 0.210519 0 4.08801  
x\_= 1.34291  
0.364408  
2.32002  
\-0.722813  
P\_= 0.0185328 0 0.109639 0  
0 0.0185328 0 0.109639  
0.109639 0 1.10798 0  
0 0.109639 0 1.10798

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/c3b294a7b95348da80a3c55bb1cfcd82.png
[20190829155031611.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/2116eee7ef79444d8d9d9782e1c12278.png
[2019082916050859.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/4737c2b6ef2f4a23ad543c7afffdc194.png
[20190829160540644.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/d3c2ab4c89804935b31bab9c49e965d0.png
[2019082916085973.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/507ab0dadb6e4a1084b9c389cf4e73a6.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 1]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/d9ce3105a4884dd39513322a4498f5f6.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 2]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/bac1e0f76f1440428559c1dd67e056c2.png