基于Piecewise Jerk Speed Optimizer的速度规划算法(附ROS C++/Python仿真)

1 时空解耦运动规划

在自主移动系统的运动规划体系中，时空解耦的递进式架构因其高效性与工程可实现性被广泛采用。这一架构将复杂的高维运动规划问题分解为路径规划与速度规划两个层次化阶段：路径规划阶段基于静态环境约束生成无碰撞的几何轨迹；速度规划阶段则在此基础上引入时间维度，通过动态障碍物预测、运动学约束建模等为机器人或车辆赋予时间轴上的运动规律。这种解耦策略虽在理论上牺牲了时空联合优化的最优性，却通过分层降维大幅提升了复杂场景下的计算效率，使其成为自动驾驶、服务机器人等实时系统的经典方案。

在这里插入图片描述

2 PJSO速度规划原理

2.1 优化变量

分段加加速度优化(Piecewise Jerk Speed Optimizer, PJSO)算法是常用的纵向速度优化方式，核心原理是用三次多项式表示s-t图中每个时间区间 $\left[ t_k,t_{k+1} \right)$ 的速度曲线，并约束每个区间加加速度相等，其中 $k=0,1,\cdots ,n-2$ ，可取为路径规划的路点数量。PJSO算法的优化变量为

$\boldsymbol{x}=\left[ \begin{matrix}{l} s_0& \dot{s}_0& \ddot{s}_0& \cdots& s_k& \dot{s}_k& \ddot{s}_k& \cdots& s_{n-1}& \dot{s}_{n-1}& \ddot{s}_{n-1}\\\end{matrix} \right] _{3n\times 1}^{T}$

2.2 代价函数

代价函数可设计为

$J=\sum_{i=0}^{n-2}{\left( w_s\left( s_i-s_{i,\mathrm{ref}} \right) ^2+w_v\left( \dot{s}_i-\dot{s}_{i,\mathrm{ref}} \right) ^2+w_a\ddot{s}_{i}^{2}+w_j{s}_{i}^{(3)2} \right)}\\+w_{s,\mathrm{end}}\left( s_{n-1}-s_{\mathrm{end}} \right) ^2+w_{v,\mathrm{end}}\left( \dot{s}_{n-1}-\dot{s}_{\mathrm{end}} \right) ^2+w_{a,\mathrm{end}}\left( \ddot{s}_n-\ddot{s}_{\mathrm{end}} \right) ^2$

通过

${s}_i^{(3)}=\frac{\ddot{s}_{i+1}-\ddot{s}_i}{\Delta t}$

隐式地消除加加速度变量，代入代价函数并消除常数项可化简为

$\begin{aligned}J=&w_s\sum_{i=0}^{n-2}{s_{i}^{2}}+w_{s,\mathrm{end}}s_{n-1}^{2}-2w_s\sum_{i=0}^{n-2}{s_{i,\mathrm{ref}}s_i}-2w_{s,\mathrm{end}}s_{\mathrm{end}}s_{n-1}\\&+w_v\sum_{i=0}^{n-2}{\dot{s}_{i}^{2}}+w_{v,\mathrm{end}}\dot{s}_{n-1}^{2}-2w_v\sum_{i=0}^{n-2}{\dot{s}_{i,\mathrm{ref}}\dot{s}_i}-2w_{v,\mathrm{end}}\dot{s}_{\mathrm{end}}\dot{s}_{n-1}\\&+\left( w_a+\frac{w_j}{\Delta t^2} \right) \ddot{s}_{0}^{2}+\left( w_a+w_{a,\mathrm{end}}+\frac{w_j}{\Delta t^2} \right) \ddot{s}_{n-1}^{2}-2w_{a,\mathrm{end}}\ddot{s}_{\mathrm{end}}\ddot{s}_{n-1}\\&+\left( w_a+\frac{2w_j}{\Delta t^2} \right) \sum_{i=1}^{n-2}{\ddot{s}_{i}^{2}}-\frac{2w_j}{\Delta t^2}\sum_{i=0}^{n-2}{\ddot{s}_i\ddot{s}_{i+1}}\end{aligned}$

从而得到核矩阵

$\boldsymbol{P}=\left[ \begin{matrix} \boldsymbol{P}_s& & \\ & \boldsymbol{P}_{\dot{s}}& \\ & & \boldsymbol{P}_{\ddot{s}}\\\end{matrix} \right] _{3n\times 3n}$

和偏置向量

$\boldsymbol{q}=\left[ \begin{array}{c} -2w_ss_{0,\mathrm{ref}}\\ \vdots\\ -2w_ss_{n-2,\mathrm{ref}}\\ -2w_{s,\mathrm{end}}s_{\mathrm{end}}\\ \hline -2w_v\dot{s}_{0,\mathrm{ref}}\\ \vdots\\ -2w_v\dot{s}_{n-2,\mathrm{ref}}\\ -2w_{v,\mathrm{end}}\dot{s}_{\mathrm{end}}\\ \hline 0\\ \vdots\\ 0\\ -2w_{a,\mathrm{end}}\ddot{s}_{\mathrm{end}}\\\end{array} \right] _{3n\times 1}$

2.3 约束条件

根据运动学公式可得运动学等式约束

$\begin{cases} \dot{s}_{i+1}=\dot{s}_i+\ddot{s}_i\Delta t+\frac{1}{2}j_i\Delta t^2=\dot{s}_i+\frac{1}{2}\ddot{s}_i\Delta t+\frac{1}{2}\ddot{s}_{i+1}\Delta t\\ s_{i+1}=s_i+\dot{s}_i\Delta t+\frac{1}{2}\ddot{s}_i\Delta t^2+\frac{1}{6}j_i\Delta t^3=s_i+\dot{s}_i\Delta t+\frac{1}{3}\ddot{s}_i\Delta t^2+\frac{1}{6}\ddot{s}_{i+1}\Delta t^3\\\end{cases}$

约束初始状态

$\dot{s}_0=\dot{s}_{\mathrm{init}}, \dot{s}_0=\dot{s}_{\mathrm{init}}, \ddot{s}_0=\ddot{s}_{\mathrm{init}}$

此外还需保证各阶状态量满足不等式约束

$s_{\min}\leqslant s_i\leqslant s_{\max}, \dot{s}_{\min}\leqslant \dot{s}_i\leqslant \dot{s}_{\max}, \ddot{s}_{\min}\leqslant \ddot{s}_i\leqslant \ddot{s}_{\max}, \ddot{s}_{\min}^{(3)}\Delta t\leqslant \ddot{s}_{i+1}-\ddot{s}_i\leqslant \ddot{s}_{\max}^{(3)}\Delta t$

从而得到约束矩阵

$\boldsymbol{A}=\left[ \begin{array}{c} \boldsymbol{A}_{\mathrm{init}, 3\times 3n}\\ \boldsymbol{A}_{\mathrm{bound}, 3n+\left( n-1 \right) \times 3n}\\ \boldsymbol{A}_{\mathrm{sys}, 2\left( n-1 \right) \times 3n}\\\end{array} \right] _{6n\times 3n}, \boldsymbol{l}=\left[ \begin{array}{c} \boldsymbol{l}_{\mathrm{init}}\\ \boldsymbol{l}_{\mathrm{bound}}\\ \boldsymbol{l}_{\mathrm{sys}}\\\end{array} \right] _{6n\times 1}, \boldsymbol{u}=\left[ \begin{array}{c} \boldsymbol{u}_{\mathrm{init}}\\ \boldsymbol{u}_{\mathrm{bound}}\\ \boldsymbol{u}_{\mathrm{sys}}\\\end{array} \right] _{6n\times 1}$

2.4 二次规划形式

综上所述，可得到PJSO算法的二次规划形式

$\min _{\boldsymbol{x}}J=\boldsymbol{x}^T\boldsymbol{Px}+\boldsymbol{q}^T\boldsymbol{x}\\\mathrm{s}.\mathrm{t}. \boldsymbol{l}\leqslant \boldsymbol{Ax}\leqslant \boldsymbol{u}$

3 算法仿真

3.1 ROS C++仿真

核心代码如下所示：

bool PiecewiseJerkVelocityPlanner::plan(const Points3d& waypoints, VelocityProfile& velocity_profile)
{
  int dim = static_cast<int>(waypoints.size());
  std::vector<double> s_ref(dim, 0.0);
  for (int i = 1; i < dim; ++i)
  {
    s_ref[i] =
        s_ref[i - 1] + std::hypot(waypoints[i].x() - waypoints[i - 1].x(), waypoints[i].y() - waypoints[i - 1].y());
  }
  x_max_ = s_ref.back();

  // construct QP Problem
  Eigen::MatrixXd P, A;
  std::vector<c_float> l, u, q;
  _calKernel(dim, P);
  _calOffset(dim, s_ref, q);
  _calAffineConstraint(dim, A);
  _calBoundary(dim, s_ref, l, u);

  std::vector<c_float> P_data;
  std::vector<c_int> P_indices;
  std::vector<c_int> P_indptr;
  int ind_P = 0;
  for (int col = 0; col < P.cols(); ++col)
  {
    P_indptr.push_back(ind_P);
    for (int row = 0; row <= col; ++row)
    {
      P_data.push_back(P(row, col));
      P_indices.push_back(row);
      ind_P++;
    }
  }
  P_indptr.push_back(ind_P);

  std::vector<c_float> A_data;
  std::vector<c_int> A_indices;
  std::vector<c_int> A_indptr;
  int ind_A = 0;
  for (int col = 0; col < A.cols(); ++col)
  {
    A_indptr.push_back(ind_A);
    for (int row = 0; row < A.rows(); ++row)
    {
      double data = A(row, col);
      if (std::fabs(data) > kMathEpsilon)
      {
        A_data.push_back(data);
        A_indices.push_back(row);
        ++ind_A;
      }
    }
  }
  A_indptr.push_back(ind_A);

  // solve
  OSQPWorkspace* work = nullptr;
  OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
  OSQPSettings* settings = reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
  osqp_set_default_settings(settings);
  settings->verbose = false;
  settings->warm_start = true;
  data->n = 3 * dim;
  data->m = 6 * dim;
  data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(), P_indices.data(), P_indptr.data());
  data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(), A_indices.data(), A_indptr.data());
  data->q = q.data();
  data->l = l.data();
  data->u = u.data();
  osqp_setup(&work, data, settings);
  osqp_solve(work);
  auto status = work->info->status_val;
  if ((status < 0) || (status != 1 && status != 2))
  {
    R_DEBUG << "failed optimization status: " << work->info->status;
    return false;
  }

  // parse
  for (int i = 0; i < dim; ++i)
  {
    velocity_profile.push(dt_ * i, work->solution->x[i], work->solution->x[dim + i], work->solution->x[2 * dim + i]);
  }

  // Cleanup
  osqp_cleanup(work);
  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);

  return true;
}

这里设置初始速度 $v_0=1.0\ m/s$ ，初始加速度 $a_0=0\ m/s^2$ ；终点速度 $v_0=0.0\ m/s$ ，终点加速度 $a_0=0\ m/s^2$

在这里插入图片描述

3.2 Python仿真

核心代码如下所示：

def process(self, path: List[Point3d]) -> List[Dict]:
	velocity_profiles = []
	path_segs, path_refs = PathPlanner.getPathSegments(path)
	for i, path_seg in enumerate(path_segs):
	    s_ref = path_refs[i]
	    n = max(
	        len(path_refs[i]),
	        round(
	            self.r * (self.dx_max ** 2 + s_ref[-1] * self.ddx_max) \
	                / (self.dx_max * self.ddx_max * self.dt)
	        )
	    )
	    s_ref = np.linspace(0, s_ref[-1], n)
	
	    '''
	    min x^T P x + q^T x
	    s.t. l <= Ax <= u
	    '''
	    P = sparse.csc_matrix(self.calKernel(n))
	    q = self.calOffset(n, s_ref)
	    A = sparse.csc_matrix(self.calAffineConstraint(n))
	    l, u = self.calBoundary(n, s_ref)
	    solver = osqp.OSQP()
	    solver.setup(P, q, A, l, u, verbose=False)
	    res = solver.solve()
	    sol = res.x
	
	    velocity_profiles.append({
	        "time": [self.dt * i for i in range(n)],
	        "path": PathPlanner.pathInterpolation(path_seg, n),
	        "distance": sol[:n].tolist(),
	        "velocity": sol[n:2 * n].tolist(),
	        "acceleration": sol[2 * n:3 * n].tolist(),
	    })
	return velocity_profiles