智駕車技術棧 | Apollo 感知模塊

簡介

雷達感知(Radar Perception)的主要內容,是整個感知模塊中較為簡單的部分,也是最好入手的部分,代碼邏輯清晰簡潔。其中包含了整個感知體系里最經典的處理邏輯,也就是主要的pipline,preprocess -> detect -> track。

文章的后續部分我們將從每個模塊遞進,看看雷達感知究竟做了點什么。

文章主要分為以下幾部分:

1. 代碼目錄結構
2. 入口及查看
3. 預處理
4. 感知主流程

01

代碼目錄結構

圖1 目錄結構

app中主要存放了Radar檢測的主邏輯,common里存放的是一些工具,lib里存了主邏輯下各子模塊的實現。

02

入口及查看

該模塊的啟動是通過融合模塊的dag文件而啟動的,在Apollo/modules/perception/production/launch并沒有單獨啟動radar的launch文件或者單獨啟動的dag文件。其具體路徑為:

Apollo/modules/perception/production/dag/dag_streaming_perception.dag

可以看到啟動的兩個分別是前雷達的detect和后雷達的detect,使用的是同一個雷達檢測入口RadarDetectionComponent.cc, 唯獨的區別是讀取的參數文件不同

圖2 啟動配置文件

參數文件對比:

圖3 雷達參數文件

可以看到主要的區別就是前雷達推理距離是200米,后雷達是120米,處理的pipeline也分為了前、后兩種,但是具體查看其配置文件發現是一模一樣的。

圖4 pipeline配置文件

RadarDetectionComponent

在融合感知模塊,可以找到RadarDetectionComponent,我們現在便進入這一模塊進行查看:

• RadarDetectionComponent::Init()

初始化參數,參數列表見圖3

• RadarDetectionComponent::InternalProc()

輸入:從ContiRadar(大陸雷達)獲取的原始radar信息

輸出:SensorFrameMessage ,即為最終的radar感知結果

其核心流程主要分為兩步, 雷達數據前處理流程Preprocess,及后續的感知流程Perceive

bool RadarDetectionComponent::InternalProc(    const std::shared_ptr<ContiRadar>& in_message,    std::shared_ptr<SensorFrameMessage> out_message) {  PERF_FUNCTION_WITH_INDICATOR(radar_info_.name);  ContiRadar raw_obstacles = *in_message;  {    std::unique_lock<std::mutex> lock(_mutex);    ++seq_num_;  }  double timestamp = in_message->header().timestamp_sec();  const double cur_time = Clock::NowInSeconds();  const double start_latency = (cur_time - timestamp) * 1e3;  AINFO << "FRAME_STATISTICS:Radar:Start:msg_time[" << timestamp        << "]:cur_time[" << cur_time << "]:cur_latency[" << start_latency        << "]";  PERF_BLOCK_START();  radar::PreprocessorOptions preprocessor_options;  ContiRadar corrected_obstacles;  //預處理的具體流程  radar_preprocessor_->Preprocess(raw_obstacles, preprocessor_options,                                  &corrected_obstacles);  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "radar_preprocessor");  timestamp = corrected_obstacles.header().timestamp_sec();  out_message->timestamp_ = timestamp;  out_message->seq_num_ = seq_num_;  out_message->process_stage_ = ProcessStage::LONG_RANGE_RADAR_DETECTION;  out_message->sensor_id_ = radar_info_.name;    //初始化一系列的參數,如radar2world轉換矩陣,rader2egovechile轉換矩陣,自車線速度,角速度  radar::RadarPerceptionOptions options;  options.sensor_name = radar_info_.name;    Eigen::Affine3d radar_trans;  //radar2world轉換矩陣  if (!radar2world_trans_.GetSensor2worldTrans(timestamp, &radar_trans)) {    out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;    AERROR << "Failed to get pose at time: " << timestamp;    return true;  }  Eigen::Affine3d radar2novatel_trans;  //radar2egovechile (自車)轉換矩陣  if (!radar2novatel_trans_.GetTrans(timestamp, &radar2novatel_trans, "novatel",                                     tf_child_frame_id_)) {    out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;    AERROR << "Failed to get radar2novatel trans at time: " << timestamp;    return true;  }  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "GetSensor2worldTrans");  Eigen::Matrix4d radar2world_pose = radar_trans.matrix();  options.detector_options.radar2world_pose = &radar2world_pose;  Eigen::Matrix4d radar2novatel_trans_m = radar2novatel_trans.matrix();  options.detector_options.radar2novatel_trans = &radar2novatel_trans_m;  //獲取自車車速  if (!GetCarLocalizationSpeed(timestamp,                               &(options.detector_options.car_linear_speed),                               &(options.detector_options.car_angular_speed))) {    AERROR << "Failed to call get_car_speed. [timestamp: " << timestamp;    // return false;  }  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "GetCarSpeed");    //Radar2world的矩陣偏移  base::PointD position;  position.x = radar_trans(0, 3);  position.y = radar_trans(1, 3);  position.z = radar_trans(2, 3);  options.roi_filter_options.roi.reset(new base::HdmapStruct());  if (FLAGS_obs_enable_hdmap_input) {    hdmap_input_->GetRoiHDMapStruct(position, radar_forward_distance_,                                    options.roi_filter_options.roi);  }  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "GetRoiHDMapStruct");  // 感知主要流程  std::vector<base::ObjectPtr> radar_objects;  if (!radar_perception_->Perceive(corrected_obstacles, options,                                   &radar_objects)) {    out_message->error_code_ =        apollo::common::ErrorCode::PERCEPTION_ERROR_PROCESS;    AERROR << "RadarDetector Proc failed.";    return true;  }  out_message->frame_.reset(new base::Frame());  out_message->frame_->sensor_info = radar_info_;  out_message->frame_->timestamp = timestamp;  out_message->frame_->sensor2world_pose = radar_trans;  out_message->frame_->objects = radar_objects;  const double end_timestamp = Clock::NowInSeconds();  const double end_latency =      (end_timestamp - in_message->header().timestamp_sec()) * 1e3;  AINFO << "FRAME_STATISTICS:Radar:End:msg_time["        << in_message->header().timestamp_sec() << "]:cur_time["        << end_timestamp << "]:cur_latency[" << end_latency << "]";  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "radar_perception");  return true;}

03

預處理

Apollo/modules/perception/radar/lib/preprocessor/conti_ars_preprocessor/http://conti_ars_preprocessor.cc

• ContiArsPreprocessor::Preprocess()

雷達預處理主要包括了以下三個主要步驟:

1. 跳過不在時間范圍內的目標
2. 重新分配ID
3. 修正時間戳

bool ContiArsPreprocessor::Preprocess(    const drivers::ContiRadar& raw_obstacles,    const PreprocessorOptions& options,    drivers::ContiRadar* corrected_obstacles) {  PERF_FUNCTION();  SkipObjects(raw_obstacles, corrected_obstacles);  ExpandIds(corrected_obstacles);  CorrectTime(corrected_obstacles);  return true;}

04

感知主流程

Apollo/modules/perception/radar/app/http://radar_obstacle_perception.cc

• RadarObstaclePerception::Perceive()

接下來我們便進入了Radar Perception的核心處理邏輯,其包含了 Detect -> ROI Filter -> Track 這三個核心部分

bool RadarObstaclePerception::Perceive(    const drivers::ContiRadar& corrected_obstacles,    const RadarPerceptionOptions& options,    std::vector<base::ObjectPtr>* objects) {  PERF_FUNCTION();  const std::string& sensor_name = options.sensor_name;  PERF_BLOCK_START();  base::FramePtr detect_frame_ptr(new base::Frame());  if (!detector_->Detect(corrected_obstacles, options.detector_options,                         detect_frame_ptr)) {    AERROR << "radar detect error";    return false;  }  ADEBUG << "Detected frame objects number: "         << detect_frame_ptr->objects.size();  PERF_BLOCK_END_WITH_INDICATOR(sensor_name, "detector");  if (!roi_filter_->RoiFilter(options.roi_filter_options, detect_frame_ptr)) {    ADEBUG << "All radar objects were filtered out";  }  ADEBUG << "RoiFiltered frame objects number: "         << detect_frame_ptr->objects.size();  PERF_BLOCK_END_WITH_INDICATOR(sensor_name, "roi_filter");  base::FramePtr tracker_frame_ptr(new base::Frame);  if (!tracker_->Track(*detect_frame_ptr, options.track_options,                       tracker_frame_ptr)) {    AERROR << "radar track error";    return false;  }  ADEBUG << "tracked frame objects number: "         << tracker_frame_ptr->objects.size();  PERF_BLOCK_END_WITH_INDICATOR(sensor_name, "tracker");  *objects = tracker_frame_ptr->objects;  return true;}

4.1 Detect

Apollo/modules/perception/radar/lib/detector/conti_ars_detector/http://conti_ars_detector.cc

在檢測這個部分,Apollo直接使用了conti雷達的檢測結果,輸出了obstacle粒度的目標,這一部分的核心則在于conti雷達數據的一些后處理,其中包括了

radar2world和radar2egovechile的坐標系轉換,之前已在外層函數RadarDetectionComponent::InternalProc()中實現,這里直接拿來用,具體可查看2.1代碼注釋

• 相對速度和絕對速度的轉換
• 運動狀態、類別等屬性的賦值,從conti的輸出格式轉化為apollo的通用障礙物格式

4.1 RoiFilter

Apollo/modules/perception/radar/lib/roi_filter/hdmap_radar_roi_filter/http://hdmap_radar_roi_filter.cc

這里就像autoware的weighted-map一樣,通過引入高精地圖模塊,幫助radar區分哪里是道路,哪里是非道路,因此可以過濾掉那些不感興趣的目標,如人行道上的物體。

bool HdmapRadarRoiFilter::RoiFilter(const RoiFilterOptions& options,                                    base::FramePtr radar_frame) {  std::vector<base::ObjectPtr> origin_objects = radar_frame->objects;  return common::ObjectInRoiCheck(options.roi, origin_objects,                                  &radar_frame->objects);}

4.3 Track

Apollo/modules/perception/radar/lib/tracker/conti_ars_tracker/http://conti_ars_tracker.cc

跟蹤采用了apollo經典的關聯,匹配,更新的三個過程。我們接下來主要講一下這個部分,可以幫助理解更加復雜的Lidar Track 和 Fusion Track。

ContiArsTracker::TrackObjects()

bool ContiArsTracker::Track(const base::Frame &detected_frame,                            const TrackerOptions &options,                            base::FramePtr tracked_frame) {  //進入Track核心處理邏輯  TrackObjects(detected_frame);  //再對track輸出的結果做一層重組和封裝  CollectTrackedFrame(tracked_frame);  return true;}void ContiArsTracker::TrackObjects(const base::Frame &radar_frame) {  std::vector<TrackObjectPair> assignments;  std::vector<size_t> unassigned_tracks;  std::vector<size_t> unassigned_objects;  TrackObjectMatcherOptions matcher_options;  const auto &radar_tracks = track_manager_->GetTracks();  //匹配  matcher_->Match(radar_tracks, radar_frame, matcher_options, &assignments,                  &unassigned_tracks, &unassigned_objects);  //更新已經匹配的track  UpdateAssignedTracks(radar_frame, assignments);  UpdateUnassignedTracks(radar_frame, unassigned_tracks);  //刪除尚未匹配的track  DeleteLostTracks();  //創建新的track  CreateNewTracks(radar_frame, unassigned_objects);}

4.3.1 CreateNewTracks

我們需要首先理解的是,從percieve模塊開始的時候,就是一幀一幀的Radar數據送入的,所以此時我們拿到的也是一幀雷達數據(經過很多重新封裝的過程之后的)。

第一幀雷達數據進入后,由于此時還沒有track,所以我們首先跳過了前三步,進入了新建track的這個函數。其代碼邏輯就是將尚未被分配到track的物體,依次新建一個track。track_manager_的addtrack方法涉及到了如何把物體信息轉換到track信息中。

void ContiArsTracker::CreateNewTracks(    const base::Frame &radar_frame,    const std::vector<size_t> &unassigned_objects) {  for (size_t i = 0; i < unassigned_objects.size(); ++i) {    RadarTrackPtr radar_track;    radar_track.reset(new RadarTrack(radar_frame.objects[unassigned_objects[i]],                                     radar_frame.timestamp));    track_manager_->AddTrack(radar_track);  }}

4.3.2 matcher_->Match

假設第一幀數據提供了5個物體,也在上一步里新建了5個track,那么此時第一幀數據處理完畢,等待第二幀數據進來,也就進入了匹配的過程。匹配的過程分為兩步:

bool HMMatcher::Match(const std::vector<RadarTrackPtr> &radar_tracks,                      const base::Frame &radar_frame,                      const TrackObjectMatcherOptions &options,                      std::vector<TrackObjectPair> *assignments,                      std::vector<size_t> *unassigned_tracks,                      std::vector<size_t> *unassigned_objects) {  IDMatch(radar_tracks, radar_frame, assignments, unassigned_tracks,          unassigned_objects);  TrackObjectPropertyMatch(radar_tracks, radar_frame, assignments,                           unassigned_tracks, unassigned_objects);  return true;}

第一步,IDMatch。先檢查track中物體的id和此時這幀傳感器數據的物體id是否一樣,如果一樣,那么該物體就與該track匹配上了,并進棧到assiment_track中(匹配過的track)。那這一步,其實是默認利用了conti雷達對物體的追蹤,從而完成了ID匹配。

第二步,TrackObjectPropertyMatch。這一步本質上的目的其實是,在雷達自身匹配的同時,也利用一些雷達數據的性質,擴充一點額外的匹配。具體的匹配方法是,使用一個二維矩陣來計算radar物體和track物體的關聯值。關聯值是通過兩者的距離來計算的,也很好理解,此時觀測到離軌跡中保存物體最近的那個就屬于這個軌跡。

radar通過對關聯值的距離計算了兩次,然后取了平均值。分別使用了上一幀和當前幀的速度做預測。

double distance_forward = DistanceBetweenObs(          track_object, track_timestamp, frame_object, frame_timestamp);      double distance_backward = DistanceBetweenObs(          frame_object, frame_timestamp, track_object, track_timestamp);      association_mat->at(i).at(j) =          0.5 * distance_forward + 0.5 * distance_backward;

在剛開始的時候我一直困惑匹配這個過程是在做什么,但現在理解下來,就是將實時的傳感器數據放入對應的Track中,并用assignment作為保存,之后就可以對軌跡本身進行重新演算,得到軌跡的速度等性質; 至于unassigned_tracks 和 unassigned_obj,這些會有后續的處理邏輯來決定是否丟棄。

4.3.3 UpdateAssignedTracks & UpdateUNAssignedTracks

這里的更新track,我們可以為上一步已經把最新一幀的傳感器數據放入了它屬于的某個物體軌跡中,這一步就要對軌跡一些性質進行有效更新。

進一步閱讀代碼我們可以發現,這里并沒有使用卡爾曼濾波器,而是直接用使用Radar的觀測量做了更新。

對每一個匹配到的結果,把里面的觀測量拿出來用于更新軌跡,里面涉及到的主要就是物體中心,物體速度,以及相應的不確定性。

void RadarTrack::UpdataObsRadar(const base::ObjectPtr& obs_radar,                                const double timestamp) {  *obs_radar_ = *obs_radar;  *obs_ = *obs_radar;  double time_diff = timestamp - timestamp_;  if (s_use_filter_) {    Eigen::VectorXd state;    state = filter_->UpdateWithObject(*obs_radar_, time_diff);    obs_->center(0) = static_cast<float>(state(0));    obs_->center(1) = static_cast<float>(state(1));    obs_->velocity(0) = static_cast<float>(state(2));    obs_->velocity(1) = static_cast<float>(state(3));    Eigen::Matrix4d covariance_matrix = filter_->GetCovarianceMatrix();    obs_->center_uncertainty(0) = static_cast<float>(covariance_matrix(0, 0));    obs_->center_uncertainty(1) = static_cast<float>(covariance_matrix(1, 1));    obs_->velocity_uncertainty(0) = static_cast<float>(covariance_matrix(2, 2));    obs_->velocity_uncertainty(1) = static_cast<float>(covariance_matrix(3, 3));  }  tracking_time_ += time_diff;  timestamp_ = timestamp;  ++tracked_times_;}

對于unassignedTrack,如果數組中已經沒有object,則直接設為dead;如果還有object,但是已經有一段時間沒更新過了,也設為dead

oid ContiArsTracker::UpdateUnassignedTracks(    const base::Frame &radar_frame,    const std::vector<size_t> &unassigned_tracks) {  double timestamp = radar_frame.timestamp;  auto &radar_tracks = track_manager_->mutable_tracks();  for (size_t i = 0; i < unassigned_tracks.size(); ++i) {    if (radar_tracks[unassigned_tracks[i]]->GetObs() != nullptr) {      double radar_time = radar_tracks[unassigned_tracks[i]]->GetTimestamp();      double time_diff = fabs(timestamp - radar_time);      if (time_diff > s_tracking_time_win_) {        radar_tracks[unassigned_tracks[i]]->SetDead();      }    } else {      radar_tracks[unassigned_tracks[i]]->SetDead();    }  }}

4.3.4 DeleteLostTracks();

刪除不用的track,邏輯很簡單,check所有的track,如果有dead的,則刪掉。

int RadarTrackManager::RemoveLostTracks() {  size_t track_count = 0;  for (size_t i = 0; i < tracks_.size(); ++i) {    if (!tracks_[i]->IsDead()) {      if (i != track_count) {        tracks_[track_count] = tracks_[i];      }      ++track_count;    }  }  int removed_count = static_cast<int>(tracks_.size() - track_count);  ADEBUG << "Remove " << removed_count << " tracks";  tracks_.resize(track_count);  return static_cast<int>(track_count);}

總結

至此,Apollo感知模塊的雷達感知(Radar Perception)部分已介紹完畢了。后續我們會在本公眾號陸續推出一系列Apollo定位、感知、預測、規劃、控制及監控等幾大模塊的系列文章,先從感知模塊開啟,敬請期待~

作者簡介

浦東范特西,也叫機智的米飯,本科就讀于北京郵電大學,碩士畢業于哥倫比亞大學,研究內容主要包含60GHz毫米波網絡的特征應用及基于數據驅動的機器學習,作為碩士生,擁有頂會2篇+專利2項?，F從事自動駕駛算法的開發工作,主攻2D/3D感知,感知后融合,車輛及人體軌跡預測。當下正與團隊小伙伴們共同投入于自動駕駛領域,為使大家的生活更便捷而不斷努力。

i車Gear聯

一群致力于推動汽車智能網聯化發展進程的工程師,一群投身于汽車產業數字化變革的小年輕,一個以數據處理和AI技術為汽車研發行業服務的團隊,將日常所見、所想、所感分享于此,一起笑看風云起。