文献精读：Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight
文献精读：Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight
- 提出了一种鲁棒且高效的轨迹生成方法
Kinodynamic Path Searching
继承了标准的A*框架，但其中Node为primitive，Edge为kinodynamic motion primitive
Primitives Generation
采用无人机的二重积分模型
\mathbf{p}(t)=[p_x(t),p_y(t),p_z(t)]^\top，p_\mu(t)=\sum\limits_{k=0}^{K}a_kt^k,\ \mu=\{x,y,z\}
\mathbf{x}(t):=[\mathbf{p}(t)^\top,\mathbf{\dot{p}}(t)^\top]^\top，\mathbf{u}(t):=\mathbf{\ddot{p}}(t)
\mathbf{\dot{x}}=\mathbf{Ax}+\mathbf{Bu}
\mathbf{A=\begin{bmatrix}0&\mathbf{I_3}\\ 0&0 \end{bmatrix}}，\mathbf{B}=\begin{bmatrix}0\\ \mathbf{I_3} \end{bmatrix}
\mathbf{x}(t)=e^{\mathbf{A}t}\mathbf{x}_0+\int_0^t e^{\mathbf{A}(t-\tau)}\mathbf{Bu}(\tau)d\tau
在控制量离散时，对于每个轴的 [-u_{max},u_{max}]，平均离散为 \{-u_{max},-\frac{r-1}{r}u_{max},...,\frac{r-1}{r}u_{max},u_{max}\}，即共得到 (2r+1)^3 个 primitives
轨迹的目标是时间最优和控制输入最优，因此代价设为
\mathcal{J}(T)=\int_0^T \Vert \mathbf{u}(t)\Vert^2dt + \rho T
当离散输入为 \mathbf{u}(t)=\mathbf{u}_d 且时间段为 \tau 时
e_c=(\Vert \mathbf{u}_d\Vert +\rho)\tau
用 g_c 表示起点到当前状态的实际代价，假设含 J 个 primitives，则
g_c=\sum_{j=1}^J (\Vert \mathbf{u}_{dj}\Vert^2+\rho)\tau
利用庞特里亚金最小原则设计了 A* 所需要的启发式函数 h_c
总代价为 f_c=g_c+h_c
搜索结束条件：
由于搜索终点很难正好落于目标，每次从 open set 弹出节点后，都计算一条从当前点到终点的轨迹，若轨迹通过了安全性和可行性检查，则提前终止搜索。
B-spline Trajectory Optimization
uniform b-spline
b-spline 由次数 p_b，N+1 个控制点 {Q_0,Q_1,\dots,Q_n}，knot vector [t_0,t_1,\dots,t_M] 决定，且满足 M=N+p_b+1，定义域为 t\in[t_{p_b},t_{M-p_b}]
对于 uniform b-spline，knot vector 是均匀的，即 \triangle t_m=t_{m-1}-t_m 为常数 \triangle t
对一段时间进行归一化，即令 s(t)=(t-t_m)/\triangle t，则 t 时刻的位置可由矩阵形式表达为
\begin{aligned} \mathbf{p}(s(t)) &= \mathbf{s}(t)^\top \mathbf{M}_{p_b+1}\mathbf{q}_m \\ \mathbf{s}(t) &= \begin{bmatrix}1&s(t)&s^2(t)&\dots&s^{p_b}(t) \end{bmatrix}^\top\\ \mathbf{q}_m &= \begin{bmatrix}\mathbf{Q}_{m-p_b}&\mathbf{Q}_{m-p_b+1}&\mathbf{Q}_{m-p_b+2}&\dots&\mathbf{Q}_{m} \end{bmatrix}^\top \end{aligned}
convex hull property
利用 b-spline 的凸包性质，完成动力学约束的表示
b-spline 的导数仍是 b-spline，因此可得速度和加速度轨迹的控制点，只需保证控制点满足约束，则轨迹也满足约束
\{\mathbf{V}_0,\mathbf{V}_1,\dots,\mathbf{V}_{N-1}\}, \qquad \mathbf{V}_i\in[-v_{max},v_{max}]^3
\{\mathbf{A}_0,\mathbf{A}_1,\dots,\mathbf{A}_{N-2}\}, \qquad\mathbf{A}_i\in[-a_{max},a_{max}]^3
\mathbf{V}_i=\frac{1}{\triangle t}(\mathbf{Q}_{i+1}-\mathbf{Q}_i)
\mathbf{A}_i=\frac{1}{\triangle t}(\mathbf{V}_{i+1}-\mathbf{V}_i)
b-spline 的障碍物约束
d_h>d_c-r_h,\qquad r_h\le r_{12}+r_{23}+r_{34}
d_h>d_c-r_n\ge d_c-(r_{12}+r_{23}+r_{34})
如果保证 d_c>0,\ r_{j,j+1}
d_h>0
problem formulation
对于有 N+1 个控制点 \{Q_0,Q_1,\dots,Q_n\} 的 p_b 次 b-spline，优化 N+1-2p_b 个控制点 \{Q_{p_b},Q_{p_b+1},\dots,Q_{N-p_b}\}，开始和结束的控制点用来决定边界状态，因此不做更改
总的 cost 函数为
f_{total}=\lambda_1f_s+\lambda_2f_c+\lambda_3(f_v+f_a)
f_s 为 smoothness cost，与时间分配无关
f_s=\sum\limits_{i=p_b-1}^{N-p_b+1}\ \Vert(\mathbf{Q}_{i+1}-\mathbf{Q}_i)+(\mathbf{Q}_{i-1}-\mathbf{Q}_i) \Vert^2
该式通过控制点之间的向量差关系，表达了轨迹的平滑性。虽然控制点不在轨迹上，但控制点会影响轨迹的趋势。可见，若该表达式趋于0，说明控制点逼近一条直线，那么轨迹也逼近一条直线，则轨迹也趋近于最平滑。可看作是连接各节点的两个弹簧的合力。
f_c 为 collision cost
f_c=\sum\limits_{i=p_b}^{N-p_b}F_c(d(\mathbf{Q}_i))
KaTeX parse error: Unknown column alignment: ( at position 42: …{\begin{array}(̲d(\mathbf{Q}_i)…
其中，d(\mathbf{Q}_i) 为控制点 \mathbf{Q}_i 到最近障碍物的距离，d_{thr} 为距离阈值
f_v 和 f_a 为速度和加速度 cost
KaTeX parse error: Unknown column alignment: ( at position 32: …{\begin{array}(̲(v_\mu^2-v_{max…
F_a(a_\mu) 类似
利用凸包性，可得
f_v=\sum\limits_{\mu\in\{x,y,z\}}\sum\limits_{i=p_b-1}^{N-p_b}F_v(V_{i\mu})
f_a=\sum\limits_{\mu\in\{x,y,z\}}\sum\limits_{i=p_b-2}^{N-p_b}F_a(A_{i\mu})
time adjustment
优化后的轨迹可能会不满足动力学约束，因此通过调整 Knot vector 来使之满足约束
对于一个 non-uniform b-spline，其微分的控制点为
\mathbf{V}_i'=\frac{p_b(\mathbf{Q}_{i+1}-\mathbf{Q}_{i})}{t_{i+p_b+1}-t_{i+1}}
\mathbf{A}_i'=\frac{(p_b-1)(\mathbf{V'}_{i+1}-\mathbf{V'}_{i})}{t_{i+p_b+1}-t_{i+2}}
设不可行的速度控制点为 \mathbf{V'}_i=[V'_{i,x},V'_{i,y},V'_{i,z}]^\top，其中最大的不可行值为 \vert V'_{i,\mu}\vert=v_m，若我们将时间段调整为 \hat{t}_{i+p_b+1}-\hat{t}_{i+1}=\mu_v(t_{i+p_b+1}-t_{i+1})，则
\begin{aligned} \hat{V}_{i,\mu}&=\frac{p_b}{\hat{t}_{i+p_b+1}-\hat{t}_{i+1}}(Q_{i+1,\mu}-Q_{i,\mu})\\ &=\frac{1}{\mu_v}\frac{p_b}{t_{i+p_b+1}-t_{i+1}}(Q_{i+1,\mu}-Q_{i,\mu})\\ &=\frac{1}{\mu_v}V'_{i,u} \end{aligned}
令 \mu_v=\frac{v_m}{v_{max}}，则速度就可行了，即 \vert\hat{V}_{i,\mu}\vert=\frac{v_{max}}{v_m}\vert V'_{i,\mu}\vert=v_{max}\in[-v_{max},v_{max}]
对于加速度也类似，由于 A'_{i,\mu} 与 V'_{i,\mu},V'_{i+1,\mu} 的耦合关系，实际受 t_{i+p_b+2}-t_{i+1} 影响，因此修改 \triangle t_m=t_{m+1}-t_m 为 \triangle\hat{t}_m=\mu_a\triangle t_m,m\in\{i+1,i+2,\dots,i+p_b+1\}，得
\begin{aligned} \hat{A}_{i,\mu}&=\frac{p_b-1}{\hat{t}_{i+p_b+1}-\hat{t}_{i+2}}(\hat{V}_{i+1,\mu}-\hat{V}_{i,\mu})\\ &=\frac{1}{\mu_a}\frac{p_b-1}{t_{i+p_b+1}-t_{i+2}}(\frac{1}{\mu_a}V'_{i+1,\mu}-\frac{1}{\mu_a}V'_{i,\mu})\\ &=\frac{1}{\mu_a^2}\frac{p_b-1}{t_{i+p_b+1}-t_{i+2}}(V'_{i+1,\mu}-V'_{i,\mu})\\ &=\frac{1}{\mu_a^2}A'_{i,u} \end{aligned}
令 \mu_a=(\frac{a_m}{a_{max}})^{\frac{1}{2}}，则加速度就可行了，即 \vert\hat{A}_{i,\mu}\vert=\frac{a_{max}}{a_m}\vert A'_{i,\mu}\vert=a_{max}\in[-a_{max},a_{max}]