基于OBCA的开放空间路径规划

JoshWindsor · 发表于 2022-5-8 16:10

在上文中，我们提到了利用Hbyrid A*和ReedShepp算法规划开放空间无碰撞的路径。
该算法通过对空间离散化以及迭代的思想求解。但是这一算法存在的问题是由于Hybrid A*对转向角度离散化，所以两个节点的曲率是突变的；ReedShepp曲线也是，在两个曲线交点位置曲率也是突变的，这样的轨迹是不适合控制车辆跟踪轨迹行驶的。所以还需要规划出一条平滑无碰撞的路径。apollo基于OBCA改进了一种TDR-OBCA算法实现路径和速度的规划，并发表了《TDR-OBCA: A Reliable Planner for Autonomous Driving in Free-Space Environment》论文[1]。OBCA算法是基于模型预测控制（MPC）建立模型，并用优化算法进行求解。
对于MPC问题，首先定义状态变量xk，包含了在第k时间的x,y坐标、车速以及航向角：

$x(k) =\begin{pmatrix} x_x(k) \\ x_y(k)\\ x_v(k)\\ x_\phi(k) \end{pmatrix}$
输入变量为前轮转角和加速度：

$u(k)=[\delta (k),a(k)]^T$
则根据车辆运动学模型有预测方程：

$x_x(k+1)=x_v(k)cos(x_\phi(k))dt_t+x_x(k)\\ x_y(k+1)=x_v(k)sin(x_\phi(k))dt_t+x_y(k)\\ x_\phi(k+1)=x_v(k)tan(\frac{tan(\delta(k))}{L})d_t+x_{\phi}(k)\\ x_v(k+1)=a(k)dt_t+x_v(k)$
简化为：

$x(k)=f(x(k-1),u(k-1))$
超平面构建障碍物约束

在OBCA算法中是用超平面构建障碍物集，对于二维空间，超平面就是条直线。
比如对于障碍物的Bonding Box有4条边，则用4条直线描述障碍物占据的空间。
比如对于一条边线的超平面：

过A(x1,y1),B(x2,y2)可以求解出直线y=ax+b为过A,B两点的超平面则可以坐标系空间分成两个部分，
直线以上空间可以表示为y>ax+b，直线以下空间可以表示为y<ax+b。
定义

$tmp1=\begin{bmatrix} x1 &1 \\ x2&1 \end{bmatrix}\\ tmp2=\begin{pmatrix} y1 \\ y2 \end{pmatrix}\\ ab = \begin{pmatrix} a\\ b \end{pmatrix}\\$
则有

$tmp1\times ab =tmp2\\ ab =tmp1^{(-1)}tmp2$
由此可以计算出直线a,b的取值
对于任意选择的A,B两点，我们期望从A到B的右侧为y<ax+b,从A到B的左侧为y>ax+b。
假设x1<x2:
集合可以表示为y<ax+b,转化为矩阵约束的形式：

$A_m=\begin{pmatrix} -a \\ 1 \end{pmatrix}\\ b_m=b\\ X=\begin{pmatrix} x \\ y \end{pmatrix}\\ A_mX\leq b_m$
假设x1>x2：
集合可以表示为y>ax+b，转化为矩阵约束形式：

$A_m=\begin{pmatrix} a \\ -1 \end{pmatrix}\\ b_m=-b\\ X=\begin{pmatrix} x \\ y \end{pmatrix}\\ A_mX\leq b_m$
如果一个障碍物有四个边，则有4个超平面相交构成的集合：

则可以将障碍物m的集合写成

$O^m= \left\{ o\in \mathbb{R}^2:A^mo \leq b^{m} \right \} \qquad \forall m =1,...,M\\ A^m \in \mathbb{R}^{4\times 2} \quad\ b^m \in \mathbb R^4 \quad\ o \in \mathbb{R}^2$
对应代码：
bool OpenSpaceRoiDecider::GetHyperPlanes(
const size_t &obstacles_num, const Eigen::MatrixXi &obstacles_edges_num,
const std::vector<std::vector<Vec2d>> &obstacles_vertices_vec,
Eigen::MatrixXd *A_all, Eigen::MatrixXd *b_all) {
  ...
  // start building H representation
  for (size_t i = 0; i < obstacles_num; ++i) {
// take two subsequent vertices, and computer hyperplane
for (size_t j = 0; j < current_vertice_num; ++j) {
   Vec2d v1 = obstacles_vertices_vec[j];
   Vec2d v2 = obstacles_vertices_vec[j + 1];

   Eigen::MatrixXd A_tmp(2, 1), b_tmp(1, 1), ab(2, 1);
   // find hyperplane passing through v1 and v2
   if (std::abs(v1.x() - v2.x()) < kEpsilon) {
      if (v2.y() < v1.y()) {
      A_tmp << 1, 0;
      b_tmp << v1.x();
      } else {
      A_tmp << -1, 0;
      b_tmp << -v1.x();
      }
   } else if (std::abs(v1.y() - v2.y()) < kEpsilon) {
      if (v1.x() < v2.x()) {
      A_tmp << 0, 1;
      b_tmp << v1.y();
      } else {
      A_tmp << 0, -1;
      b_tmp << -v1.y();
      }
   } else {
      Eigen::MatrixXd tmp1(2, 2);
      tmp1 << v1.x(), 1, v2.x(), 1;
      Eigen::MatrixXd tmp2(2, 1);
      tmp2 << v1.y(), v2.y();
      ab = tmp1.inverse() * tmp2;
      double a = ab(0, 0);
      double b = ab(1, 0);

      if (v1.x() < v2.x()) {
      A_tmp << -a, 1;
      b_tmp << b;
      } else {
      A_tmp << a, -1;
      b_tmp << -b;
      }
   }


自车占据空间的也可以为4个超平面相交的集合表示，假设自车中心位于原点的空间表示为

$\mathbb{B}:=\left\{ e:Ge \leq g \right\}$

图中，w代表车宽，l代表车长，则有

$G =\begin{bmatrix} 1&0\\ 0&1\\ -1&0\\ 0&-1 \end{bmatrix}, g =\begin{bmatrix} \frac{l}{2}\\ \frac{w}{2}\\ \frac{l}{2}\\ \frac{w}{2} \end{bmatrix}$

用R(x(k))表示在k时间主车相对原点位置的旋转，t(x(k))表示k时间主车相对原点位置的平移，则有表示第k时间主车占据的位置：

$\mathbb E(x(k))=R(x(k))\mathbb B+t(x(k))$
其中旋转矩阵R(x(k))为：

$R(x(k))=\begin{bmatrix} cos(x_\phi(k))&&sin(x_\phi(k))\\ sin(x_\phi(k))&&cos(x_\phi(k)) \end{bmatrix}$
平移向量t(x(k))为：

$t(x(k))=\begin{bmatrix} x_x(k)+cos(x_\phi(k))\cdot l_{offset}\\ x_y(k)+sin(x_\phi(k))\cdot l_{offset}\\ \end{bmatrix}$
其中l_offset是后轴中心到车辆几何中心的偏移，因为x_x,x_y指的都是车辆后轴中心的位置，要换算到几何中心上来

$l_{offset}=ego_{lengh} -ego_{back\_to\_center}$
构建免碰撞约束

如果要求主车和障碍物没有碰撞，显然有：

$\mathbb{E}(x(k))\cap \mathbb{O}^{(m)}=\emptyset \qquad \forall m =1,...,M$
就是自车在时间k的空间和所有障碍物的空间交集为空。
如果我们需要自车和障碍物至少保证d_min的间距，我们定义一个自车和障碍物最短距离的函数dist：

$dist(\mathbb E(x),\mathbb O_m):=\min_{t} \{\left \| t \right \|:(\mathbb{E}(x)+t )\cap \mathbb{O_m} =\emptyset \}$
对于第m个障碍物根据论文[2]的推论有：

$dist(\mathbb E(x),\mathbb O_m)>d_{min}\\ \Leftrightarrow \exists \lambda_m \geq 0 \mu_m \geq 0:-g^\top \mu_m+(A^{(m)}t(x(k))-b_m)^\top\lambda _m>d_{min} \\ G^\top\mu_m+R(x)^\top A^{(m)\top} \lambda_m =0 \\ \left \| A^{(m)\top} \lambda \right \|_2 \le 1\\ \lambda_m \geq 0, \mu_m \geq 0\\$
式中引入了两个对偶变量 $\mu \in \mathbb R^4,\lambda \in \mathbb R^{n}$ 其中n是第m个障碍物的边数，注意这里边数可以不为4，也可以不是闭合的。
MPC主体框架

接下来可以介绍OBCA的MPC问题目标函数和约束了,假设每两个控制状态之间的采样时间是固定的ts：

$\min_{x,y,\mu ,\lambda} {\sum_{k = 1}^{K}l(x(k),u(k-1)) }\\ subject\space to:\\ x(0)=x_0\\ x(K)=x_F\\ x(k)=f(x(k-1),u(k-1))\\ h(x(k),u(k)) \leq 0\\ -g^\top \mu_m(k)+(A^{(m)}t(x(k))-b_m)^\top\lambda _m(k)>d_{min} \\ G^\top\mu_m(k)+R(x(k))^\top A^{(m)\top} \lambda_m(k) =0 \\ \left \| A^{(m)\top} \lambda \right \|_2 \le 1\\ \lambda_m(k) \geq 0, \mu_m(k) \geq 0\\ for \space k=1...K,m=1...M$
其中l为目标函数：

$l(x(k),u(k-1)) =a _x\left \| x(k)-x_{ref}(k) \right \|_2^2+ a _u\left \| u(k-1) \right \|_2^2+\\ a_{\tilde{u} }\left \| \frac{u(1)- \tilde{u}}{t_s}\right \|_2^2 + a_{u'}\left \| \frac{(u(k-1)-u(k-2))}{t_s} \right \|_2^2$
l的第一项描述了状态量和参考值尽可能的小，参考值也是就是算法优化的初始值，因为对于非线性优化算法来讲，初始值的选择对优化结果有着非常重要的影响，所以这里选用了Hybrid A*产生的轨迹作为优化算法的初始值（warm start）。ax为状态变量的权重向量，
l的第二项描述了期望控制量（方向盘转角，加减速度）尽可能小
l的第三项描述了控制量第一个分量和轨迹初始点的加速度、方向盘转角越接近越好，官文给出的原因是因为连续控制决策之间的较大波动可能会使执行器无法跟踪所需的控制命令
l的第四项描述了控制量的变化率越小越好
从约束来看：
第一项是初始点值和给定的起点一致
第二项是终点状态和给定的终点一致
第三项是控制状态满足车辆运动学约束
第四项是对控制状态和输入的约束：

$-\delta'_{max}\leq\frac{x_\phi(k)-x_\phi(k-1)}{ts} \leq \delta'_{max}$
第五、六、七、八项是障碍物相关约束，前文已经介绍过了。这个就是apollo DISTANCE_APPROACH_IPOPT_FIXED_TS算法的建模过程，然后调用ipopt算法进行求解。ipopt的优化变量结构定义为：

$\begin{array}{c} (x(0),y(0),\phi(0),v(0),x(1),y(1),\phi(1),v(1),...,x(K),y(K),\phi (K),v(K),\\ \delta(0),a(0),...,\delta(K-1),a(K-1),\\ \lambda _{0,0},...,\lambda _{E,0}, \lambda _{0,1},...,\lambda _{E,K},\\ \mu _{0,0},...,\mu _{4M,0}, \lambda _{0,1},...,\lambda _{4M,K},)\\ \end{array}$
其中E表示障碍物总边数，M为障碍物的数量
ipopt的起始点定义get_start_point()函数，来自于Hybrid A*+ReedShepp规划的轨迹采样点。还有对偶变量λ和μ的初始值会在后文讲到。
代码可以参考modules/planning/open_space/trajectory_smoother/http://distance_approach_ipopt_fixed_ts_interface.cc
此外根据场景需求不同，apollo还设计了几种变形的算法：
采样时间可变的OBCA算法

在该算法中优化变量增加了一个采样时间变化系数t(k),则每两个控制状态之间的时间间隔为t(k)*ts,则问题定义变成：

$\min_{x,y,\mu ,\lambda,t} {\sum_{k = 1}^{K}l(x(k),u(k-1)) }\\ subject\space to:\\ x(0) = x_0\\ x(K) = x_F\\ x(k) = f(x(k-1),u(k-1),t(k))\\ h(x(k),u(k),t(k)) \leq 0\\ -g^\top \mu_m(k)+(A^{(m)}t(x(k))-b_m)^\top\lambda _m(k)>d_{min} \\ G^\top\mu_m(k)+R(x(k))^\top A^{(m)\top} \lambda_m(k) = 0 \\ \left \| A^{(m)\top} \lambda \right \|_2 \le 1\\ \lambda_m(k) \geq 0, \mu_m(k) \geq 0\\ for \space k = 1...K,m = 1...M$

车辆运动学约束函数变成:

$x_x(k+1)=x_x(k)+t_st(k)(x_v(k)+\frac{1}{2}t_st(k)a(k) )cos(x_\phi(k)+\frac{1}{2}t_st(k)x_v(k) tan(\frac{\delta(k)}{L}))\\ x_y(k+1)=x_y(k)+t_st(k)(x_v(k)+\frac{1}{2}t_st(k)a(k) )sin(x_\phi(k)+\frac{1}{2}t_st(k)x_v(k) tan(\frac{\delta(k)}{L}))\\ x_\phi(k+1)=x_{\phi}(k)+t_st(k)(x_v(k))+\frac{1}{2}t_st(k)a(k))tan(\frac{\delta(k)}{L}) \\ x_v(k+1)=x_v(k)+t_st(k)a(k)$
状态变量和输入量约束函数h变成：

$-\delta'_{max}\leq\frac{x_\phi(k)-x_\phi(k-1)}{t_st(k)} \leq \delta'_{max}$

$t(k)=t(k-1)$

$t_{min\_sample\_scaling}\leq t(k)\leq t_{max\_sample\_scaling}$
目标函数l变为：

$l(x(k),u(k-1)) =a _x\left \| x(k)-x_{ref}(k) \right \|_2^2+ a _u\left \| u(k-1) \right \|_2^2+\\ a_{\tilde{u} }\left \| \frac{u(1)- \tilde{u}}{t_st(k)}\right \|_2^2 + a_{u'}\left \| \frac{(u(k-1)-u(k-2))}{t_st(k)} \right \|_2^2 +a_{t2}t(k)^2++a_{t1}t(k)$
多了关于时间系数的优化（为什么要约束1阶和2阶两次）
对应apollo 中DISTANCE_APPROACH_IPOPT的算法
modules/planning/open_space/trajectory_smoother/http://distance_approach_problem.cc
放松终点状态约束

在之前的算法终点位置是硬约束的，硬约束会导致ipopt求解速度变慢，因此apollo将硬约束转化为软约束，因此新的目标函数变为：

$J(x,u) =\sum_{k = 1}^{K}l(x(k),u(k-1))+\alpha_e \|x(K)-x_F\|_2^2$
对应apollo中 DISTANCE_APPROACH_IPOPT_RELAX_END 算法
modules/planning/open_space/trajectory_smoother/http://distance_approach_ipopt_relax_end_interface.cc
安全距离可调

在之前的算法中安全距离d_min是一个常值，对于任何障碍物都是一样的。但是自动驾驶驾驶中可能有这样的需求，对于行人、对于车辆、对于道路边沿期望的安全距离要求不一样，apollo将安全距离也加入cost函数中，新的目标函数变为：

$J(x,u) =\sum_{k = 1}^{K}l(x(k),u(k-1))+\alpha_e \|x(K)-x_F\|_2^2+\beta \sum_{m = 1}^{M}\sum_{k = 1}^{K}d_m(k)\\ \beta>0\quad d_m(k)<0$
其中d_m(k)是安全距离的相反数，则约束变为:

$subject\space to:\\ x(0) = x_0\\ x(k+1) = f(x(k),u(k),t(k))\\ h(x(k),u(k),t(k)) \leq 0\\ -g^\top \mu_m(k)+(A^{(m)}t(x(k))-b_m)^\top\lambda _m(k)+d_{min}=0 \\ G^\top\mu_m(k)+R(x(k))^\top A^{(m)\top} \lambda_m(k) = 0 \\ \left \| A^{(m)\top} \lambda \right \|_2 \le 1\\ \lambda_m(k) \geq 0, \mu_m(k) \geq 0，d_m(k)<0\\ for \space k = 1...K,m = 1...M$
对偶变量和轨迹初始值

对于OBCA优化问题，是非凸优化问题，因此算法优化变量初始的猜想值对于最终的优化结果至关重要，不同的初始值会导致求解器获得不同的局部最优解。对于路径的初始值只要来自于上一篇文章介绍的基于Hybrid A*和ReedSheep曲线的Open Space规划器，对于速度、加速度的初始值采用基于二次规划的速度规划算法，文章可以参见：
代码位于modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc:
bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
  ...
  std::vector<double> x_ref(num_of_knots, path_length);
  piecewise_jerk_problem.set_x_ref(ref_s_weight_, std::move(x_ref));
  piecewise_jerk_problem.set_dx_ref(ref_v_weight_, max_v * 0.8);
  piecewise_jerk_problem.set_weight_ddx(acc_weight_);
  piecewise_jerk_problem.set_weight_dddx(jerk_weight_);
  piecewise_jerk_problem.set_x_bounds(std::move(x_bounds));
  piecewise_jerk_problem.set_dx_bounds(std::move(dx_bounds));
  piecewise_jerk_problem.set_ddx_bounds(std::move(ddx_bounds));
  piecewise_jerk_problem.set_dddx_bound(max_acc_jerk_);

  // solve the problem
  if (!piecewise_jerk_problem.Optimize()) {
AERROR << &#34;Piecewise jerk speed optimizer failed!&#34;;
return false;
  }

  // extract output
  const std::vector<double>& s = piecewise_jerk_problem.opt_x();
  const std::vector<double>& ds = piecewise_jerk_problem.opt_dx();
  const std::vector<double>& dds = piecewise_jerk_problem.opt_ddx();
  ...
}
再通过采样时间对轨迹离散，就获得了离散后状态变量x(k)，和输入量u(k)的初始值。
有了x和u的初始值，计算对偶变量的初始值就可以引入前文介绍的d_min转化为以下优化问题：

$\min_{\mu,\lambda ,d}{\sum_{m=1}^{M}\sum_{k=1}^{K}}d_m(k)\\ subject \space to:\\ -g^\top \mu_m(k)+(A^{(m)}t(x(k))-b_m)^\top\lambda _m(k)+d_{min}=0 \\ G^\top\mu_m(k)+R(x(k))^\top A^{(m)\top} \lambda_m(k) = 0 \\ \left \| A^{(m)\top} \lambda \right \|_2 \le 1\\ \lambda_m(k) \geq 0, \mu_m(k) \geq 0，d_m(k)<0\\ for \space k = 1...K,m = 1...M$
根据论文所述，这是一个二次约束的二次规划问题（QCQP，但是目标函数是一阶的啊？），不好求解，apollo进一步简化成了一个基本QP问题，主要方法是把二次不等式约束拿到目标函数中：


apollo中也提供了3套求解器，第一个算法用的ipopt求解：
modules/planning/open_space/trajectory_smoother/http://dual_variable_warm_start_ipopt_interface.cc
第二个算法既用了ipopt求解也用了osqp求解（为啥还用ipopt求解？）：
modules/planning/open_space/trajectory_smoother/http://dual_variable_warm_start_ipopt_qp_interface.cc
modules/planning/open_space/trajectory_smoother/http://dual_variable_warm_start_osqp_interface.cc
仿真与结果分析

选取sunnyvale with office地图，调用open space规划器，使主车泊入纵向车位：

仿真结果如下所示：
几种算法路径仿真结果：

几种OBCA算法和warm start规划路径比较

终点松弛后，终点位置发生了变化

phi-t曲线

相比于与Hybrid A*产生的路径，OBCA算法产生的结果更为平滑，另外对于relax_end和relax_end_slack由于对终点位置进行了松弛，终点位置发生了变化。从航向角随时间变化曲线中，也可以看出来整体phi的变化相较于warm start曲线也更为平滑，relax_end算法的终点姿态也因为松弛不再是期望终点姿态，relax_end曲率变化更小。
几种算法速度仿真结果

速度曲线v-t

加速度曲线a-t

速度变化曲线可以看出，fix_ts和warm_start曲线采样时间相同(0.5s)，所以整体规划时间要比其他obca 采样时间可变的算法(0.4s)要长，obca算法速度和加速度变化相比与warm start也更为平滑。在速度曲线中由于relax_end算法终点位置没做约束，导致规划的终点速度不是0，车辆可能无法及时停下来。个人认为位置终点虽然松弛了，但是终点的速度还是需要加一个硬约束。另外由于没有对输入量施加约束，几种算法终点的加速度也都不是0.
参考

^TDR-OBCA: A Reliable Planner for Autonomous Driving in Free-Space Environmenthttps://arxiv.org/abs/2009.11345

^Optimization-Based Collision Avoidancehttps://arxiv.org/abs/1711.03449

		自动登录	找回密码
密码			立即注册

基于OBCA的开放空间路径规划

本帖子中包含更多资源