日日操夜夜添-日日操影院-日日草夜夜操-日日干干-精品一区二区三区波多野结衣-精品一区二区三区高清免费不卡

公告:魔扣目錄網為廣大站長提供免費收錄網站服務,提交前請做好本站友鏈:【 網站目錄:http://www.ylptlb.cn 】, 免友鏈快審服務(50元/站),

點擊這里在線咨詢客服
新站提交
  • 網站:51998
  • 待審:31
  • 小程序:12
  • 文章:1030137
  • 會員:747

Apollo里面最重要的模塊就是感知和規劃模塊,規劃模塊最重要的是路徑規劃和速度規劃任務,對應ROS機器人里面的局部規劃。Apollo的規劃模塊首先根據當前的情況進行多場景(scenario)調度,每個場景又包含一個或者多個階段(stage),每個階段又由多個具有獨立功能的小模塊任務(task)完成。這篇文章就主要解讀一下路徑規劃piecewise jerk path optimizer這個任務。任務最終會生成軌跡(trajectory):每個點的位姿和速度信息,進而輸出給控制模塊去控制車輛。

Content:

  1. 任務代碼調用入口
  2. 二次規劃定義
  3. Apollo 中二次規劃問題的構造
  4. Apollo代碼的實現
  5. Python代碼的實現
  6. 參考文獻

任務代碼調用入口

比如通過測試用例可得到如下的堆棧:

apollo::planning::PiecewiseJerkPathOptimizer::Process(apollo::planning::PiecewiseJerkPathOptimizer * const this, const apollo::planning::SpeedData & speed_data, const apollo::planning::ReferenceLine & reference_line, const apollo::common::TrajectoryPoint & init_point, const bool path_reusable, apollo::planning::PathData * const final_path_data) (piecewise_jerk_path_optimizer.cc:61)
apollo::planning::PathOptimizer::Execute(apollo::planning::PathOptimizer * const this, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * const reference_line_info) (path_optimizer.cc:45)
apollo::planning::scenario::lane_follow::LaneFollowStage::PlanOnReferenceLine(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * reference_line_info) (lane_follow_stage.cc:167)
apollo::planning::scenario::lane_follow::LaneFollowStage::Process(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame) (lane_follow_stage.cc:116)
apollo::planning::scenario::Scenario::Process(apollo::planning::scenario::Scenario * const this, const apollo::common::TrajectoryPoint & planning_init_point, apollo::planning::Frame * frame) (scenario.cc:76)
apollo::planning::PublicRoadPlanner::Plan(apollo::planning::PublicRoadPlanner * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ADCTrajectory * ptr_computed_trajectory) (public_road_planner.cc:38)
apollo::planning::OnLanePlanning::Plan(apollo::planning::OnLanePlanning * const this, const double current_time_stamp, const std::vector<apollo::common::TrajectoryPoint, std::allocator<apollo::common::TrajectoryPoint> > & stitching_trajectory, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:572)
apollo::planning::OnLanePlanning::RunOnce(apollo::planning::OnLanePlanning * const this, const apollo::planning::LocalView & local_view, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:417)
apollo::planning::PlanningTestBase::RunPlanning(apollo::planning::PlanningTestBase * const this, const std::__cxx11::string & test_case_name, int case_num, bool no_trajectory_point) (planning_test_base.cc:229)
apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test::TestBody(apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test * const this) (sunnyvale_big_loop_test.cc:191)

二次規劃定義

二次規劃問題的定義如下:

Apollo二次規劃算法(piecewise jerk path optimizer)解析

 

Apollo使用osqp進行二次規劃問題的求解。第一行為代價函數,第二行為約束條件。二次優化的目標就是在滿足約束條件的基礎上,找到優化變量使得代價函數的值最小。二次規劃只在代價函數為凸函數的時候能夠收斂到最優解,因此這需要P矩陣為半正定矩陣,這是非常重要的一個條件。這反映在Apollo中的規劃算法則為需要進行求解的空間為凸空間,這樣二次規劃才能收斂到一條最優路徑。

Apollo 中二次規劃問題的構造

Apollo二次規劃算法(piecewise jerk path optimizer)解析

 


Apollo二次規劃算法(piecewise jerk path optimizer)解析

 


Apollo二次規劃算法(piecewise jerk path optimizer)解析

 


Apollo二次規劃算法(piecewise jerk path optimizer)解析

 


Apollo二次規劃算法(piecewise jerk path optimizer)解析

 


Apollo二次規劃算法(piecewise jerk path optimizer)解析

 

Apollo代碼的實現


PiecewiseJerkProblem::Optimize方法中,首先調用FormulateProblem()構造QP問題的Q,P和A,b,然后進行OSQP設置osqp_setup,最后調用osqp_solve求解。

bool PiecewiseJerkProblem::Optimize(const int max_iter) {
  OSQPData* data = FormulateProblem();
 
  OSQPSettings* settings = SolverDefaultSettings();
  settings->max_iter = max_iter;
 
  OSQPWorkspace* osqp_work = nullptr;
  osqp_work = osqp_setup(data, settings);
  // osqp_setup(&osqp_work, data, settings);
 
  osqp_solve(osqp_work);
 
  auto status = osqp_work->info->status_val;
 
  if (status < 0 || (status != 1 && status != 2)) {
    AERROR << "failed optimization status:t" << osqp_work->info->status;
    osqp_cleanup(osqp_work);
    FreeData(data);
    c_free(settings);
    return false;
  } else if (osqp_work->solution == nullptr) {
    AERROR << "The solution from OSQP is nullptr";
    osqp_cleanup(osqp_work);
    FreeData(data);
    c_free(settings);
    return false;
  }
 
  // extract primal results
  x_.resize(num_of_knots_);
  dx_.resize(num_of_knots_);
  ddx_.resize(num_of_knots_);
  for (size_t i = 0; i < num_of_knots_; ++i) {
    x_.at(i) = osqp_work->solution->x[i] / scale_factor_[0];
    dx_.at(i) = osqp_work->solution->x[i + num_of_knots_] / scale_factor_[1];
    ddx_.at(i) =
        osqp_work->solution->x[i + 2 * num_of_knots_] / scale_factor_[2];
  }
 
  // Cleanup
  osqp_cleanup(osqp_work);
  FreeData(data);
  c_free(settings);
  return true;
}

其中,調用CalculateKernel進行P的構造,調用CalculateAffineConstraint進行A的構造,調用CalculateOffset進行Q的構造。

OSQPData* PiecewiseJerkProblem::FormulateProblem() {
  // calculate kernel
  std::vector<c_float> P_data;
  std::vector<c_int> P_indices;
  std::vector<c_int> P_indptr;
  CalculateKernel(&P_data, &P_indices, &P_indptr);
 
  // calculate affine constraints
  std::vector<c_float> A_data;
  std::vector<c_int> A_indices;
  std::vector<c_int> A_indptr;
  std::vector<c_float> lower_bounds;
  std::vector<c_float> upper_bounds;
  CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
                            &upper_bounds);
 
  // calculate offset
  std::vector<c_float> q;
  CalculateOffset(&q);
 
  OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
  CHECK_EQ(lower_bounds.size(), upper_bounds.size());
 
  size_t kernel_dim = 3 * num_of_knots_;
  size_t num_affine_constraint = lower_bounds.size();
 
  data->n = kernel_dim;
  data->m = num_affine_constraint;
  data->P = csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data),
                       CopyData(P_indices), CopyData(P_indptr));
  data->q = CopyData(q);
  data->A =
      csc_matrix(num_affine_constraint, kernel_dim, A_data.size(),
                 CopyData(A_data), CopyData(A_indices), CopyData(A_indptr));
  data->l = CopyData(lower_bounds);
  data->u = CopyData(upper_bounds);
  return data;
}

構建P:
參見公式10。

void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,
                                               std::vector<c_int>* P_indices,
                                               std::vector<c_int>* P_indptr) {
  const int n = static_cast<int>(num_of_knots_);
  const int num_of_variables = 3 * n;
  const int num_of_nonzeros = num_of_variables + (n - 1);
  std::vector<std::vector<std::pair<c_int, c_float>>> columns(num_of_variables);
  int value_index = 0;
 
  // x(i)^2 * (w_x + w_x_ref[i]), w_x_ref might be a uniform value for all x(i)
  // or piecewise values for different x(i) 參見公式7
  for (int i = 0; i < n - 1; ++i) {
    columns[i].emplace_back(i, (weight_x_ + weight_x_ref_vec_[i]) /
                                   (scale_factor_[0] * scale_factor_[0]));
    ++value_index;
  }
  // x(n-1)^2 * (w_x + w_x_ref[n-1] + w_end_x)  
  columns[n - 1].emplace_back(
      n - 1, (weight_x_ + weight_x_ref_vec_[n - 1] + weight_end_state_[0]) /
                 (scale_factor_[0] * scale_factor_[0]));
  ++value_index;
 
  // x(i)'^2 * w_dx  參見公式8
  for (int i = 0; i < n - 1; ++i) {
    columns[n + i].emplace_back(
        n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));
    ++value_index;
  }
  // x(n-1)'^2 * (w_dx + w_end_dx)
  columns[2 * n - 1].emplace_back(2 * n - 1,
                                  (weight_dx_ + weight_end_state_[1]) /
                                      (scale_factor_[1] * scale_factor_[1]));
  ++value_index;
 
  auto delta_s_square = delta_s_ * delta_s_;
  // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)  參見公式9的對角線
  columns[2 * n].emplace_back(2 * n,
                              (weight_ddx_ + weight_dddx_ / delta_s_square) /
                                  (scale_factor_[2] * scale_factor_[2]));
  ++value_index;
  for (int i = 1; i < n - 1; ++i) {
    columns[2 * n + i].emplace_back(
        2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;
 
  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''  參見公式9的對角線下面的次對角線
  for (int i = 0; i < n - 1; ++i) {
    columns[2 * n + i].emplace_back(2 * n + i + 1,
                                    (-2.0 * weight_dddx_ / delta_s_square) /
                                        (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
 
  CHECK_EQ(value_index, num_of_nonzeros);
 
  int ind_p = 0;
  for (int i = 0; i < num_of_variables; ++i) {
    P_indptr->push_back(ind_p);
    for (const auto& row_data_pair : columns[i]) {
      P_data->push_back(row_data_pair.second * 2.0);
      P_indices->push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  P_indptr->push_back(ind_p);
}

構建Q:
參見公式11。

void PiecewiseJerkPathProblem::CalculateOffset(std::vector<c_float>* q) {
  CHECK_NOTNULL(q);
  const int n = static_cast<int>(num_of_knots_);
  const int kNumParam = 3 * n;
  q->resize(kNumParam, 0.0);
 
  if (has_x_ref_) {
    for (int i = 0; i < n; ++i) {
      q->at(i) += -2.0 * weight_x_ref_vec_.at(i) * x_ref_[i] / scale_factor_[0];
    }
  }
 
  if (has_end_state_ref_) {
    q->at(n - 1) +=
        -2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0];
    q->at(2 * n - 1) +=
        -2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1];
    q->at(3 * n - 1) +=
        -2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2];
  }
}

構建A:

void PiecewiseJerkProblem::CalculateAffineConstraint(
    std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
    std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
    std::vector<c_float>* upper_bounds) {
  // 3N params bounds on x, x', x''
  // 3(N-1) constraints on x, x', x''
  // 3 constraints on x_init_
  const int n = static_cast<int>(num_of_knots_);
  const int num_of_variables = 3 * n;
  const int num_of_constraints = num_of_variables + 3 * (n - 1) + 3;
  lower_bounds->resize(num_of_constraints);
  upper_bounds->resize(num_of_constraints);
 
  std::vector<std::vector<std::pair<c_int, c_float>>> variables(
      num_of_variables);
 
  int constraint_index = 0;
  // set x, x', x'' bounds  對應公式23的前3n行
  for (int i = 0; i < num_of_variables; ++i) {
    if (i < n) {
      variables[i].emplace_back(constraint_index, 1.0);
      lower_bounds->at(constraint_index) =
          x_bounds_[i].first * scale_factor_[0];
      upper_bounds->at(constraint_index) =
          x_bounds_[i].second * scale_factor_[0];
    } else if (i < 2 * n) {
      variables[i].emplace_back(constraint_index, 1.0);
 
      lower_bounds->at(constraint_index) =
          dx_bounds_[i - n].first * scale_factor_[1];
      upper_bounds->at(constraint_index) =
          dx_bounds_[i - n].second * scale_factor_[1];
    } else {
      variables[i].emplace_back(constraint_index, 1.0);
      lower_bounds->at(constraint_index) =
          ddx_bounds_[i - 2 * n].first * scale_factor_[2];
      upper_bounds->at(constraint_index) =
          ddx_bounds_[i - 2 * n].second * scale_factor_[2];
    }
    ++constraint_index;
  }
  CHECK_EQ(constraint_index, num_of_variables);
 
  // x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s   對應公式12
  for (int i = 0; i + 1 < n; ++i) {
    variables[2 * n + i].emplace_back(constraint_index, -1.0);
    variables[2 * n + i + 1].emplace_back(constraint_index, 1.0);
    lower_bounds->at(constraint_index) =
        dddx_bound_.first * delta_s_ * scale_factor_[2];
    upper_bounds->at(constraint_index) =
        dddx_bound_.second * delta_s_ * scale_factor_[2];
    ++constraint_index;
  }
 
  // x(i+1)' - x(i)' - 0.5 * delta_s * x(i)'' - 0.5 * delta_s * x(i+1)'' = 0  對應公式17
  for (int i = 0; i + 1 < n; ++i) {
    variables[n + i].emplace_back(constraint_index, -1.0 * scale_factor_[2]);
    variables[n + i + 1].emplace_back(constraint_index, 1.0 * scale_factor_[2]);
    variables[2 * n + i].emplace_back(constraint_index,
                                      -0.5 * delta_s_ * scale_factor_[1]);
    variables[2 * n + i + 1].emplace_back(constraint_index,
                                          -0.5 * delta_s_ * scale_factor_[1]);
    lower_bounds->at(constraint_index) = 0.0;
    upper_bounds->at(constraint_index) = 0.0;
    ++constraint_index;
  }
 
  // x(i+1) - x(i) - delta_s * x(i)'
  // - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)''  對應公式16
  auto delta_s_sq_ = delta_s_ * delta_s_;
  for (int i = 0; i + 1 < n; ++i) {
    variables[i].emplace_back(constraint_index,
                              -1.0 * scale_factor_[1] * scale_factor_[2]);
    variables[i + 1].emplace_back(constraint_index,
                                  1.0 * scale_factor_[1] * scale_factor_[2]);
    variables[n + i].emplace_back(
        constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]);
    variables[2 * n + i].emplace_back(
        constraint_index,
        -delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]);
    variables[2 * n + i + 1].emplace_back(
        constraint_index,
        -delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]);
 
    lower_bounds->at(constraint_index) = 0.0;
    upper_bounds->at(constraint_index) = 0.0;
    ++constraint_index;
  }
 
  // constrain on x_init
  variables[0].emplace_back(constraint_index, 1.0);
  lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
  upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
  ++constraint_index;
 
  variables[n].emplace_back(constraint_index, 1.0);
  lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
  upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
  ++constraint_index;
 
  variables[2 * n].emplace_back(constraint_index, 1.0);
  lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
  upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
  ++constraint_index;
 
  CHECK_EQ(constraint_index, num_of_constraints);
 
  int ind_p = 0;
  for (int i = 0; i < num_of_variables; ++i) {
    A_indptr->push_back(ind_p);
    for (const auto& variable_nz : variables[i]) {
      // coefficient
      A_data->push_back(variable_nz.second);
 
      // constraint index
      A_indices->push_back(variable_nz.first);
      ++ind_p;
    }
  }
  // We indeed need this line because of
  // https://github.com/oxfordcontrol/osqp/blob/master/src/cs.c#L255
  A_indptr->push_back(ind_p);
}

Python代碼的實現


import matplotlib.pyplot as plt
import osqp
import numpy as np
from scipy import sparse
import random
# 障礙物設置
obs = [[5, 10, 2, 3], [15, 20, -2, -0.5], [25, 30, 0, 1]]  # start_s,end_s,l_low,l_up
 
s_len = 50
delta_s = 0.1
n = int(s_len / delta_s)
x = np.linspace(0, s_len, n)
up_bound = [0] * (5 * n + 3)
low_bound = [0] * (5 * n + 3)
s_ref = [0] * 3 * n
 
dddl_bound = 0.01
 
####################邊界提取################
l_bound = 5
for i in range(n):
    for j in range(len(obs)):
        if x[i] >= obs[j][0] and x[i] <= obs[j][1]:            
            low_ = obs[j][2]
            up_ = obs[j][3]         
            break
        else:
            up_ = l_bound
            low_ = -l_bound
    up_bound[i] = up_
    low_bound[i] = low_   
    s_ref[i] = 0.5 * (up_ + low_)
 
for i in range(3 * n, 4 * n):
    up_bound[i] = dddl_bound * delta_s * delta_s * delta_s / 6
    low_bound[i] = -dddl_bound * delta_s * delta_s * delta_s / 6  
for i in range(4 * n, 5 * n):
    up_bound[i] = dddl_bound * delta_s * delta_s / 2
    low_bound[i] = -dddl_bound * delta_s * delta_s / 2
 
####################構造P和Q################
w_l = 0.005
w_dl = 1
w_ddl = 1
w_dddl = 0.1
eye_n = np.identity(n)
zero_n = np.zeros((n, n))
 
P_zeros = zero_n
P_l = w_l * eye_n
P_dl = w_dl * eye_n
P_ddl = (w_ddl + 2 * w_dddl / delta_s / delta_s) * eye_n - 2 * w_dddl / delta_s / delta_s * np.eye(n, k=-1)
P_ddl[0][0] = w_ddl + w_dddl / delta_s / delta_s
P_ddl[n - 1][n - 1] = w_ddl + w_dddl / delta_s / delta_s
 
P = sparse.csc_matrix(np.block([
    [P_l, P_zeros, P_zeros],
    [P_zeros, P_dl, P_zeros],
    [P_zeros, P_zeros, P_ddl]
    ]))
q = np.array([-w_l * s_ for s_ in s_ref])
 
####################構造A和LU################
 
# 構造:l(i+1) = l(i) + l'(i) * delta_s + 1/2 * l''(i) * delta_s^2 + 1/6 * l'''(i) * delta_s^3
A_ll = -eye_n + np.eye(n, k=1)
A_ldl = -delta_s * eye_n
A_lddl = -0.5 * delta_s * delta_s * eye_n
A_l = (np.block([
    [A_ll, A_ldl, A_lddl]
    ]))
 
# 構造:l'(i+1) = l'(i) + l''(i) * delta_s + 1/2 * l'''(i) * delta_s^2
A_dll = zero_n
A_dldl = -eye_n + np.eye(n, k=1)
A_dlddl = -delta_s * eye_n
A_dl = np.block([
    [A_dll, A_dldl, A_dlddl]
    ])
 
A_ul = np.block([
    [eye_n, zero_n, zero_n],
    [zero_n, zero_n, zero_n],
    [zero_n, zero_n, zero_n]
    ])  # 3n*3n
# 初始化設置
A_init = np.zeros((3, 3 * n))
A_init[0][0] = 1
 
A = sparse.csc_matrix(np.row_stack((A_ul, A_l, A_dl, A_init)))
 
low_bound[5 * n] = 1
up_bound[5 * n] = 1
l = np.array(low_bound)
u = np.array(up_bound)
 
# Create an OSQP object
prob = osqp.OSQP()
 
# Setup workspace and change alpha parameter
prob.setup(P, q, A, l, u, alpha=1.0)
 
# Solve problem
res = prob.solve()
 
plt.plot(u[:n], '.', color='blue')
plt.plot(l[:n], '.', color='black')
plt.plot(s_ref[:n],'.', color='yellow')
plt.plot(res.x[:n], '.', color='red')
plt.show()

Apollo二次規劃算法(piecewise jerk path optimizer)解析

 

參考文獻

規劃控制:Piecewise Jerk Path Optimizer的python實現
Apollo 6.0 QP(二次規劃)算法解析
Piecewise Jerk Path Optimizer

分享到:
標簽:算法 規劃
用戶無頭像

網友整理

注冊時間:

網站:5 個   小程序:0 個  文章:12 篇

  • 51998

    網站

  • 12

    小程序

  • 1030137

    文章

  • 747

    會員

趕快注冊賬號,推廣您的網站吧!
最新入駐小程序

數獨大挑戰2018-06-03

數獨一種數學游戲,玩家需要根據9

答題星2018-06-03

您可以通過答題星輕松地創建試卷

全階人生考試2018-06-03

各種考試題,題庫,初中,高中,大學四六

運動步數有氧達人2018-06-03

記錄運動步數,積累氧氣值。還可偷

每日養生app2018-06-03

每日養生,天天健康

體育訓練成績評定2018-06-03

通用課目體育訓練成績評定