Faster Planner——Kinodynamic Astar详解

实际成本和启发式成本

实际成本 Actual Cost

  每条轨迹的真实代价函数定义如下：
$$\mathcal{J}(T)={\int }_0^T\Vert \mathbf{u}(t){\Vert }^{2}dt+\rho T$$
离散形式下：

启发成本 Heuristic Cost

  论文里没有给出启发函数详细的推导过程，可以结合高飞老师在深蓝学院讲的《Motion Planning》的课程来你就饿。下面就详细介绍下我自己的理解，错误的话请大家指出。

  论文中使用的无人机模型为二阶模型，控制输入为加速度，即：
$$\begin{array}{r}\dot{p}(t)=v(t)\\ \dot{v}(t)=u(t)\end{array}$$
因此无人机的运动方程可以简写为：
$$\dot{s}=f_s(s,u)=(v,a)$$
其中状态为$s_k=(p_k,v_k)$，控制输入为$u_k$。

根据上方PPT中的内容，此时系统中只有两个变量，因此系统的协态（costate）为$\lambda =({\lambda }_1,{\lambda }_2)$（系统状态中只有两项，所以只有${\lambda }_1$和${\lambda }_2$，分别对应状态中的$p$和$v$）。系统的哈密顿函数（Hamiltonian function）可以定义为：
$$\begin{gathered} \begin{array}{rl}H(s,u,\lambda )& =\frac{1}{T}{a}^2+{\lambda }^T{f}_s(s,u)\\ & =\frac{1}{T}{a}^2+{\lambda }_{1}v+{\lambda }_{2}a\end{array} \\ \dot{\lambda }=-{▽}_{s}H(s^{\ast },u^{\ast },\lambda )=(0,-{\lambda }_1) \end{gathered}$$
上式中$\dot{\lambda }$是在状态取得最优$s^{\ast }$和控制输入取得最优$u^{\ast }$时，$H$关于的$s$的偏导，$s$中包含$p$和$v$，所以将$H$分别对$p$和$v$求偏导，即
$$\begin{array}{rl}& {\lambda }_1=-\frac{\partial H}{\partial p}=0\\ & {\lambda }_2=-\frac{\partial H}{\partial v}=-{\lambda }_1\end{array}$$
所以得到了上面的$\dot{\lambda }=-{▽}_{s}H(s^{\ast },u^{\ast },\lambda )=(0,-{\lambda }_1)$

根据这个可以得到一组$\lambda$的可行解
$$\lambda =\frac{1}{T}\left[\begin{array}{rl}& -2{\alpha }_{\mu }\\ & 2{\alpha }_{\mu }t+2{\beta }_{\mu }\end{array}\right]$$

  由此可得，将上面的公式$\lambda$带入$H(s,u,\lambda )$可得到此时系统的控制输入$u^{\ast }$为：
$$\begin{array}{r}H(s,u,\lambda )=\frac{1}{T}\left[a^2-2{\alpha }_{\mu }v+(2{\alpha }_{\mu }t+2{\beta }_{\mu })a\right]\end{array}$$
上述公式中的变量为$a$且$u=a$，其他都是已知的，所以控制输入最优时，$a={\alpha }_{\mu }t+\beta$

  因此系统中最优状态为（最优控制量$u\ast$的积分）：
$$s^{\ast }(t)=\left[\begin{array}{r}{p}^{\ast }\\ v^{\ast }\end{array}\right]=\left[\begin{array}{rl}& \frac{1}{6}{\alpha }_{\mu }{t}^3+\frac{1}{2}{\beta }_{\mu }{t}^2+v_{\mu c}t+p_{\mu c}\\ & \frac{1}{2}{\alpha }_{\mu }{t}^2+{\beta }_{\mu }t+v_{\mu c}\end{array}\right]$$
原文中$p^{\ast }=\frac{1}{6}{\alpha }_{\mu }{t}^3+\frac{1}{2}{\beta }_{\mu }{t}^2+v_{\mu c}+p_{\mu c}$的倒数第二项应该是少了个$t$，应该是$p^{\ast }=\frac{1}{6}{\alpha }_{\mu }{t}^3+\frac{1}{2}{\beta }_{\mu }{t}^2+v_{\mu c}t+p_{\mu c}$。系统的cost function为：
$$\begin{array}{rl}J(t)=& {\int }_0^t||u|{|}^{2}dt\\ =& {\int }_0^t||{\alpha }_{\mu }t+\beta |{|}^{2}dt\\ =& {\int }_0^t{\alpha }_{\mu }^{2}t+2{\alpha }_{\mu }{\beta }_{\mu }t+{\beta }_{\mu }^{2}dt\\ =& \frac{1}{3}{\alpha }_{\mu }^2{t}^3+{\alpha }_{\mu }{\beta }_{\mu }{t}^2+{\beta }_{\mu }^{2}t\end{array}$$
cost function中只有时间$t$是变量，因此可以通过对J(t)求导来获取最优的时间及对应的cost 。

$$\begin{array}{rl}{p}_{\mu }^{\ast }(t)& =\frac{1}{6}{\alpha }_{\mu }{t}^3+\frac{1}{2}{\beta }_{\mu }{t}^2+v_{\mu c}+p_{\mu c}\\ & =p_{\mu c}+v_{\mu c}+T^2\ast ((2\ast v_{\mu c}-2\ast v_{\mu g})/(2\ast T)-(6\ast p_{\mu c}-6\ast p_{\mu g}+6\ast T\ast v_{\mu c})/(2\ast T^2))-T^3\ast ((6\ast v_{\mu c}-6\ast v_{\mu g})/(6\ast T^2)-(12\ast p_{\mu c}-12\ast p_{\mu g}+12\ast T\ast v_{\mu c})/(6\ast T^3))\end{array}$$

  ${\alpha }_{\mu }$和${\beta }_{\mu }$的确定依赖于终止条件，也就是经过时间$T$以后无人机要到达$p_{\mu g}$和$v_{\mu g}$，可以得到下述关系：
$$\begin{gathered} \left[\begin{array}{cc}\frac{1}{6}{T}^3& \frac{1}{2}{T}^2\\ \frac{1}{2}{T}^2& T\end{array}\right]\left[\begin{array}{l}\alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c}\Delta p\\ \Delta v\\ \Delta a\end{array}\right] \\ \left[\begin{array}{cc}\frac{1}{6}{T}^3& \frac{1}{2}{T}^2\\ \frac{1}{2}{T}^2& T\end{array}\right]\left[\begin{array}{l}\alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c}{p}_{\mu g}-p_{\mu c}-v_{\mu c}T\\ v_{\mu g}-v_{\mu c}\end{array}\right] \end{gathered}$$
由此可以得到：

$$\begin{array}{rl}\left[\begin{array}{c}{\alpha }_{\mu }\\ {\beta }_{\mu }\end{array}\right]& =\frac{1}{T^3}\left[\begin{array}{cc}-12& 6T\\ 6T& -2T^2\end{array}\right]\left[\begin{array}{c}{p}_{\mu g}-p_{\mu c}-v_{\mu c}T\\ v_{\mu g}-v_{\mu c}\end{array}\right]\end{array}$$

  将上式中的${\alpha }_{\mu }$和${\beta }_{\mu }$带入下式${\mathcal{J}}^{\ast }(T)$中，并将各项按照$T$的阶次分解后可得：
$$\begin{array}{rl}{\mathcal{J}}^{\ast }(T)& =\sum _{\mu \in \{x,y,z\}}\left(\frac{1}{3}{\alpha }_{\mu }^2{T}^3+{\alpha }_{\mu }{\beta }_{\mu }{T}^2+{\beta }_{\mu }^{2}T\right)\\ & =\sum _{\mu \in \{x,y,z\}}\left((4\ast (T^2\ast v_{\mu c}^2+T^2\ast v_{\mu c}\ast v_{\mu g}+T^2\ast v_{\mu g}^2+3\ast T\ast p_{\mu c}\ast v_{\mu c}+3\ast T\ast p_{\mu c}\ast v_{\mu g}-3\ast T\ast p_{\mu g}\ast v_{\mu c}-3\ast T\ast p_{\mu g}\ast v_{\mu g}+3\ast p_{\mu c}^2-6\ast p_{\mu c}\ast p_{\mu g}+3\ast p_{\mu g}^2))/T^3\right)\\ & =\sum _{\mu \in \{x,y,z\}}\left((12\ast p_{\mu c}^2)/T^3+(12\ast p_{\mu g}^2)/T^3-(24\ast p_{\mu c}\ast p_{\mu g})/T^3\right)+\sum _{\mu \in \{x,y,z\}}\left((12\ast p_{\mu c}\ast v_{\mu c})/T^2+(12\ast p_{\mu c}\ast v_{\mu g})/T^2-(12\ast p_{\mu g}\ast v_{\mu c})/T^2-(12\ast p_{\mu g}\ast v_{\mu g})/T^2\right)+\sum _{\mu \in \{x,y,z\}}\left((4\ast v_{\mu c}^2)/T+(4\ast v_{\mu g}^2)/T+(4\ast v_{\mu c}\ast v_{\mu g})/T\right)\\ & =\sum _{\mu \in \{x,y,z\}}\left(12(p_{\mu c}-p_{\mu g}{)}^2/T^3-12(v_{\mu c}+v_{\mu g})(p_{\mu g}-p_{\mu c})/T^2+4(v_{\mu c}^2+v_{\mu c}\ast v_{\mu g}+v_{\mu g}^2)/T\right)\end{array}$$
上式是关于$T$的多项式，为了求得最优的${\mathcal{J}}^{\ast }(T)$的闭式解，因此需要对其进行求关于时间$T$的偏导，即$\frac{\partial {\mathcal{J}}^{\ast }(T)}{\partial T}$，由此可以得到：
$$\frac{\partial {\mathcal{J}}^{\ast }(T)}{\partial T}=\sum _{\mu \in \{x,y,z\}}\left(36(p_{\mu c}-p_{\mu g}{)}^2/T^{-4}-24(v_{\mu c}+v_{\mu g})(p_{\mu g}-p_{\mu c})/T^{-3}+4(v_{\mu c}^2+v_{\mu c}\ast v_{\mu g}+v_{\mu g}^2)/T^{-2}\right)$$

  这里的$36(p_{\mu c}-p_{\mu g})$、$-24(v_{\mu c}+v_{\mu g})(p_{\mu g}-p_{\mu c})$、$4(v_{\mu c}^2+v_{\mu c}\ast v_{\mu g}+v_{\mu g}^2)$分别对应四次项、三次项和二次项，也就是代码中的$c_1$$c_2$$c_3$；因为没有一次项，所以$c_4=0$；常数项$c_5$是自定义的，影响不大（可以联想下一个曲线，常数项只是让该曲线沿着Y轴上下移动）。

  因为这里${\mathcal{J}}^{\ast }(T)$中的多项式是关于$T$的，且是负的次幂，所以在代码中假设$t=\frac{1}{T}$，然后求解四次多项式的根。这里的根为最优的时间$T\ast$。需要注意的是，在最优的时间$T\ast$时对应的cost ${\mathcal{J}}^{\ast }(T)$也是有大有小的，所以为了获得最优的时间$T\ast$，需要对求得的多个根可进行判断，判断的过程后面再讲。

  在求四次多项式的根时，调用了**quartic()**函数，该函数中使用费拉里方法来求解四次多项式的根。然后在求解三次多项式时，代码中使用了两种方法，当判别式大于0及等于0的情况利用了求根公式，判别式小于0的情况则是使用了三角函数解法。

double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2, double& optimal_time)
{
    
  const Eigen::Vector3d dp = x2.head(3) - x1.head(3);
  const Eigen::Vector3d v0 = x1.segment(3, 3);
  const Eigen::Vector3d v1 = x2.segment(3, 3);

  double c1 = -36 * dp.dot(dp);
  double c2 = 24 * (v0 + v1).dot(dp);
  double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));
  double c4 = 0;
  double c5 = w_time_;

  std::vector<double> ts = quartic(c5, c4, c3, c2, c1);

  double v_max = max_vel_ * 0.5;
  double t_bar = (x1.head(3) - x2.head(3)).lpNorm<Eigen::Infinity>() / v_max;
  ts.push_back(t_bar);

  double cost = 100000000;
  double t_d = t_bar;

  for (auto t : ts)
  {
    
    if (t < t_bar)
      continue;
    double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t;
    if (c < cost)
    {
    
      cost = c;
      t_d = t;
    }
  }

  optimal_time = t_d;

  return 1.0 * (1 + tie_breaker_) * cost;
}