Fast Planner—— Kinodynamic A*公式推导

叉车司机

已于 2024-08-30 14:48:30 修改

阅读量928

点赞数 9

分类专栏：轨迹规划文章标签：算法

于 2024-07-08 11:53:09 首次发布

本文链接：https://blog.csdn.net/m0_61616957/article/details/140202528

版权

轨迹规划专栏收录该内容

3 篇文章 0 订阅

订阅专栏

论文链接：Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight

参考文章：Faster Planner——Kinodynamic Astar详解-CSDN博客

实际代价与启发代价

1.实际代价

代价函数定义为：

$J\left ( T \right )=\int_{0}^{T}\left \| u(t) \right \|^{2}dt+\rho T$

其中 $\left \| u(t) \right \|^{2}$ 是控制输入的平方， $\rho$ 是时间加权因子。

对于给定的离散化输入 $u(t)=u_{d}$ 和持续时间 $\tau$ ，EdgeCost() 为：

$e_{c}=(\left \| u_{d} \right \|^{2}+\rho )\tau$

实际代价 $g_{c}$ 表示从初始状态 $x_{s}$ 到当前状态 $x_{c}$ 的路径代价，假设路劲由 $J$ 个原始段 Edge 组成，则

$g_{c}=\sum_{j=1}^{J}(\left \| u_{dj} \right \|^{2}+\rho )\tau$

2.启发函数设计

启发函数 $h_{c}$ 表示从当前状态 $x_{c}$ 到目标状态 $x_{g}$ 的最佳估计代价，采用庞特里亚金极小值原理，求解最优控制轨迹。根据庞特里亚金极小值原理，可以得到轨迹的解析形式(原文 $v_{\mu c}t$ 项少写了一个 $t$ )：

$p_{\mu }^{*}(t)=\frac{1}{6}\alpha _{\mu }t^{3}+\frac{1}{2}\beta _{\mu }t^{2}+v_{\mu c}t+p_{\mu c}$

其中 $\alpha _{\mu }$ 和 $\beta _{\mu }$ 由边界条件确定：

$\begin{bmatrix} \alpha _{\mu }\\ \beta _{\mu } \end{bmatrix}=\frac{1}{T^{3}}\begin{bmatrix} -12 &6T \\ 6T&-2T^{2} \end{bmatrix}\begin{bmatrix} p_{\mu g}-p_{\mu c}-v_{\mu c}T\\ v_{\mu g}-v_{\mu c} \end{bmatrix}$

以下为上面两个公式的具体推导。

2.1 系统运动方程

考虑一个三维系统，其中 $\mu \in \left \{ x,y,z \right \}$ ,状态 $p_{\mu }(t)$ 和速度 $v_{\mu }(t)$ 的运动方程为：

$\left\{\begin{matrix} \dot{p}_{\mu }(t)=v_{\mu }(t)\\ \dot{v}_{\mu }(t)=u_{\mu }(t) \end{matrix}\right.$

希望找到从当前状态 $(p_{\mu c},v_{\mu c})$ 到目标状态 $(p_{\mu g},v_{\mu g})$ 的最优轨迹。

2.2 构建哈密顿函数

首先，构建哈密顿函数 $H$ :

$H=\left \| u(t) \right \|^{2}+\lambda _{1}v_{\mu }+\lambda _{2}u_{\mu }$

其中 $\lambda _{1}$ 和 $\lambda _{2}$ 为共轭变量。

2.3 应用庞特里亚金极小值原理

根据应用庞特里亚金极小值原理，在最优控制 $u_{\mu }^{*}(t)$ 作用下，哈密顿函数 $H$ 最小。通过对 $u$ 求偏导等于0求得：

$\frac{\partial H}{\partial u}=2u_{\mu }+\lambda _{2}=0 \Rightarrow u_{\mu }^{*}=-\frac{\lambda _{2}}{2}$

2.4 状态方程和共轭方程

状态方程：

$\left\{\begin{matrix} \dot{p}_{\mu }=v_{\mu }\\ \dot{v}_{\mu }=u_{\mu } \end{matrix}\right.$

共轭方程（是通过哈密顿函数 $H$ 对状态变量 $p_{\mu }$ 和 $v_{\mu }$ 求偏导得到的，表示共轭变量 $\lambda _{1}$ 和 $\lambda _{2}$ 随时间的变化）：

$\left\{\begin{matrix} \dot{\lambda }_{1}=-\frac{\partial H}{\partial p_{\mu }}=0\\ \dot{\lambda }_{2}=-\frac{\partial H}{\partial v_{\mu }}=-\lambda _{1} \end{matrix}\right.$

(负号的存在是为了保证目标函数 $J$ 最小化，我们需要考虑每个状态变量对应的哈密顿函数的变化率，并且这种变化率必须对目标函数产生正确的影响方向)

对共轭方程积分可以得到一组解（解不唯一，下面解为对应原论文公式结果）：

$\lambda _{1}(t)=const=2\alpha _{\mu }$

$\dot{\lambda }_{2}=-2\alpha _{\mu }\Rightarrow \lambda _{2}(t)=-2\alpha _{\mu }t-2\beta _{\mu }$

将共轭变量代入最优控制公式 $u_{\mu }^{*}$ ：

$u_{\mu }^{*}=-\frac{\lambda _{2}}{2}=\alpha _{\mu }t+\beta _{\mu }$

2.5 轨迹的解析形式

结合状态方程和最优控制结果，得到轨迹最优运动方程：

$\dot{v}_{\mu }=u_{\mu }^{*}=\alpha _{\mu }t+\beta _{\mu }\Rightarrow v_{\mu }(t)=\frac{1}{2}\alpha _{\mu }t^{2}+\beta _{\mu }t+v_{\mu c}$

$\dot{p}_{\mu }=v_{\mu }\Rightarrow p_{\mu }^{*}(t)=\frac{1}{6}\alpha _{\mu }t^{3}+\frac{1}{2}\beta _{\mu }t^{2}+v_{\mu }t+p_{\mu c}$

2.6 边界条件确定系数

根据边界条件：

$p_{\mu }(0)=p_{\mu c}, p_{\mu }(T)=p_{\mu g}$

$v_{\mu }(0)=v_{\mu c}, v_{\mu }(T)=v_{\mu g}$

可以确定 $\alpha _{\mu }$ 和 $\beta _{\mu }$ ，利用边界条件 $p_{\mu }(0)=p_{\mu c}, p_{\mu }(T)=p_{\mu g}$ ，轨迹满足：

$p_{\mu }(T)=\frac{1}{6}\alpha _{\mu }T^{3}+\frac{1}{2}\beta_{\mu }T^{2}+v_{\mu c}T+p_{\mu c}=p_{\mu g}$

$v_{\mu }(T)=\frac{1}{4}\alpha _{\mu }T^{2}-\frac{1}{2}\beta _{\mu }T+v_{\mu g}=v_{\mu g}$

将边界条件写为矩阵形式：

$\begin{bmatrix} \frac{T^{3}}{6} &\frac{T^{2}}{2} \\ \frac{T^{2}}{4}& -\frac{T}{2} \end{bmatrix}\begin{bmatrix} \alpha _{\mu }\\ \beta _{\mu } \end{bmatrix}=\begin{bmatrix} p_{\mu g}-p_{\mu c}-v_{\mu c}T\\ v_{\mu g}-v_{\mu c} \end{bmatrix}$

解方程得到系数：

$\begin{bmatrix} \alpha _{\mu }\\ \beta _{\mu } \end{bmatrix}=\frac{1}{T^{3}}\begin{bmatrix} -12 & 6T\\ 6T & -2T^{2} \end{bmatrix}\begin{bmatrix} p_{\mu g}-p_{\mu c}-v_{\mu c}T\\ v_{\mu g}-v_{\mu c} \end{bmatrix}$