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$的阶次分解后可得：
J ∗ ( T ) = ∑ μ ∈ { x , y , z } ( 1 3 α μ 2 T 3 + α μ β μ T 2 + β μ 2 T ) = ∑ μ ∈ { x , y , z } ( ( 4 ∗ ( T 2 ∗ v μ c 2 + T 2 ∗ v μ c ∗ v μ g + T 2 ∗ v μ g 2 + 3 ∗ T ∗ p μ c ∗ v μ c + 3 ∗ T ∗ p μ c ∗ v μ g − 3 ∗ T ∗ p μ g ∗ v μ c − 3 ∗ T ∗ p μ g ∗ v μ g + 3 ∗ p μ c 2 − 6 ∗ p μ c ∗ p μ g + 3 ∗ p μ g 2 ) ) / T 3 ) = ∑ μ ∈ { x , y , z } ( ( 12 ∗ p μ c 2 ) / T 3 + ( 12 ∗ p μ g 2 ) / T 3 − ( 24 ∗ p μ c ∗ p μ g ) / T 3 ) + ∑ μ ∈ { x , y , z } ( ( 12 ∗ p μ c ∗ v μ c ) / T 2 + ( 12 ∗ p μ c ∗ v μ g ) / T 2 − ( 12 ∗ p μ g ∗ v μ c ) / T 2 − ( 12 ∗ p μ g ∗ v μ g ) / T 2 ) + ∑ μ ∈ { x , y , z } ( ( 4 ∗ v μ c 2 ) / T + ( 4 ∗ v μ g 2 ) / T + ( 4 ∗ v μ c ∗ v μ g ) / T ) = ∑ μ ∈ { x , y , z } ( 12 ( p μ c − p μ g ) 2 / T 3 − 12 ( v μ c + v μ g ) ( p μ g − p μ c ) / T 2 + 4 ( v μ c 2 + v μ c ∗ v μ g + v μ g 2 ) / T ) \begin{aligned} \mathcal{J}^{*}(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*(T^2*v_{\mu c}^2 + T^2*v_{\mu c}*v_{\mu g} + T^2*v_{\mu g}^2 + 3*T*p_{\mu c}*v_{\mu c} + 3*T*p_{\mu c}*v_{\mu g} - 3*T*p_{\mu g}*v_{\mu c} - 3*T*p_{\mu g}*v_{\mu g} + 3*p_{\mu c}^2 - 6*p_{\mu c}*p_{\mu g} + 3*p_{\mu g}^2))/T^3\right) \\ &=\sum_{\mu \in\{x, y, z\}}\left((12*p_{\mu c}^2)/T^3 + (12*p_{\mu g}^2)/T^3 - (24*p_{\mu c}*p_{\mu g})/T^3 \right) + \sum_{\mu \in\{x, y, z\}}\left((12*p_{\mu c}*v_{\mu c})/T^2 + (12*p_{\mu c}*v_{\mu g})/T^2 - (12*p_{\mu g}*v_{\mu c})/T^2 - (12*p_{\mu g}*v_{\mu g})/T^2 \right) + \sum_{\mu \in\{x, y, z\}}\left((4*v_{\mu c}^2)/T + (4*v_{\mu g}^2)/T + (4*v_{\mu c}*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}*v_{\mu g} + v_{\mu g}^2)/T \right) \end{aligned} J∗(T)​=μ∈{x,y,z}∑​(31​αμ2​T3+αμ​βμ​T2+βμ2​T)=μ∈{x,y,z}∑​((4∗(T2∗vμc2​+T2∗vμc​∗vμg​+T2∗vμg2​+3∗T∗pμc​∗vμc​+3∗T∗pμc​∗vμg​−3∗T∗pμg​∗vμc​−3∗T∗pμg​∗vμg​+3∗pμc2​−6∗pμc​∗pμg​+3∗pμg2​))/T3)=μ∈{x,y,z}∑​((12∗pμc2​)/T3+(12∗pμg2​)/T3−(24∗pμc​∗pμg​)/T3)+μ∈{x,y,z}∑​((12∗pμc​∗vμc​)/T2+(12∗pμc​∗vμg​)/T2−(12∗pμg​∗vμc​)/T2−(12∗pμg​∗vμg​)/T2)+μ∈{x,y,z}∑​((4∗vμc2​)/T+(4∗vμg2​)/T+(4∗vμc​∗vμg​)/T)=μ∈{x,y,z}∑​(12(pμc​−pμg​)2/T3−12(vμc​+vμg​)(pμg​−pμc​)/T2+4(vμc2​+vμc​∗vμg​+vμg2​)/T)​
上式是关于$T$的多项式，为了求得最优的${\mathcal{J}}^{\ast }(T)$的闭式解，因此需要对其进行求关于时间$T$的偏导，即$\frac{\partial {\mathcal{J}}^{\ast }(T)}{\partial T}$，由此可以得到：
∂ J ∗ ( T ) ∂ T = ∑ μ ∈ { x , y , z } ( 36 ( p μ c − p μ g ) 2 / T − 4 − 24 ( v μ c + v μ g ) ( p μ g − p μ c ) / T − 3 + 4 ( v μ c 2 + v μ c ∗ v μ g + v μ g 2 ) / T − 2 ) \frac{\partial{\mathcal{J}^{*}(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}*v_{\mu g} + v_{\mu g}^2)/T^{-2} \right) ∂T∂J∗(T)​=μ∈{x,y,z}∑​(36(pμc​−pμg​)2/T−4−24(vμc​+vμg​)(pμg​−pμc​)/T−3+4(vμc2​+vμc​∗vμg​+vμg2​)/T−2)

  这里的$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;
}