Motion Plan之轨迹生成笔记 (2)

Motion Plan之搜索算法笔记
Motion Plan之基于采样的路径规划算法笔记
Motion Plan之带动力学约束路径搜索

什么是基于优化的轨迹生成

Optimization-Based Trajectory Planning（基于优化的轨迹规划）是一种常用的方法，用于生成自动化系统（如自动驾驶车辆和机器人）的最优运动路径。该方法将轨迹规划问题描述为一个最优控制问题（OCP），通过优化方法来找到最优的运动轨迹。具体流程包括问题建模、约束建模、优化求解和轨迹生成。
下面将详细介绍Optimization-Based Trajectory Planning的流程，并提供一些实例以帮助理解。
1. 问题建模
首先，需要将轨迹规划问题建模为一个最优控制问题。这包括定义系统状态变量、控制变量和目标函数，以及约束条件。

系统状态变量：描述系统在不同时间点的状态，如位置、速度和加速度等。
控制变量：控制系统在不同时间点的行为，如加速度、转向角等。
目标函数：衡量系统运动轨迹的性能指标，如最小时间、最小能量消耗等。
约束条件：描述系统在运动过程中的限制，如避免碰撞、满足动力学约束等。

2. 约束建模
在问题建模的基础上，需要将约束条件具体化，以便进行数值优化求解。约束条件可以分为硬约束和软约束。

硬约束：必须满足的条件，如避免碰撞、括避障、速度限制、加速度限制、遵守交通规则等。这些约束条件是不能被违反的。
软约束：可以被违反但希望尽量满足的条件，如最小化能量消耗、最小化刹车距离等。

3. 优化求解
在约束建模完成后，可以将轨迹规划问题转化为一个优化问题。常见的优化方法包括非线性规划（NLP）和数值优化算法。

非线性规划：将问题转化为一个非线性约束优化问题，并使用数值方法求解。常用的非线性规划算法包括序列二次规划（SQP）等。
数值优化算法：通过迭代过程逐步优化目标函数，使其达到最优值。常见的数值优化算法包括梯度下降法、遗传算法等。

优化求解的目标是找到一个最优解，使得目标函数达到最小或最大值，同时满足约束条件。
4. 轨迹生成
通过优化求解得到的最优解，可以生成最优的运动轨迹。根据系统模型和控制变量的设定，可以计算出每个时间点上的状态和控制量，从而得到连续的运动轨迹。

初始轨迹生成：提供一个初始路径或轨迹，这可以是一个简单的启发式算法生成的初始解。
迭代优化：利用选择的优化算法迭代地优化路径。在每一次迭代中，算法会根据目标函数和约束条件对当前路径进行调整，直到满足停止准则（如达到最大迭代次数或达到足够小的目标函数值）。
路径验证：对生成的最优路径进行验证，确保其满足系统的所有约束条件，如避障、动力学限制等。
输出最优路径：最终，输出优化后的路径作为机器人或车辆的轨迹。

示例
下面是一些Optimization-Based Trajectory Planning的示例：

自动驾驶车辆的轨迹规划：根据车辆的当前状态、目标位置和避免碰撞的约束条件，对车辆的运动轨迹进行优化求解，实现自动驾驶的目标。
机器人路径规划：在给定环境中，根据机器人的起始位置和目标位置，通过优化求解得到机器人的最优移动轨迹，以实现高效的路径规划。
特定场景下的轨迹规划：在特定场景下，如停车场等，根据场景的要求和约束条件，对车辆的运动轨迹进行优化，实现快速、准确、最优的停车轨迹。

总之，Optimization-Based Trajectory Planning是一种常用的方法，可以通过建模、约束建模、优化求解和轨迹生成来实现自动化系统的最优运动轨迹规划。该方法可以适用于各种不同的应用场景，如自动驾驶车辆、机器人路径规划等。
为什么需要基于优化生成平滑轨迹：
• 在轨迹规划中的平滑性不是一个几何概念，许多高质量的轨迹具有非平滑的形状。一个平滑的轨迹至少应该：

满足动力学的微分约束，
最小化状态 $(\mathbf{x})$ 和输入 $(\mathbf{u})$ 的能量泛函。

之所以要生成平滑轨迹是因为：
移动机器人的能源是有限的，如何通过平滑轨迹来提高能源效率是很重要的；比如机器人在转弯时不应停止，这样可以极大节省能源消耗提高能力效率。
移动机器人执行器是受到限制的，速度/高阶动力学是不能立即改变的，所以在路径生成时候需要考虑到，生成平滑轨迹适配移动机器人执行器的需求。
在一些移动任务中，移动机器人需要在经过指定的轨迹点之行特定任务需求，所以轨迹生成时候需要经过这些轨迹点生成平滑轨迹，例如：无人机指定点电缆破损巡检。
• 边界条件：起始点，目标点的位置（朝向）
• 中间条件：路径点的位置（朝向）
• 路径点可以通过路径规划（A*、RRT*等）找到
• 在之前的三堂课中介绍过
• 平滑性标准
• 通常转化为最小化“输入”的变化率

前置知识

Differential Flatness（差分平坦性）

是一种控制系统设计中的概念，它旨在简化非线性系统的控制问题。微分平坦性是一个概念,指在确定的条件下,可以用有限个变量及其导数来描述系统的所有状态变量以及控制变量,从而简化复杂的微分系统。利用微分平坦性,可以消除系统的微分约束条件,简化轨迹生成等过程。这种平坦性使得系统的非线性动态可以通过代数方程来描述，从而简化了控制器的设计和分析。可以通过操纵平坦输出来实现对系统的控制。
一个简单的例子是飞行器动力学系统。如果该系统是微分平坦的，那么通过适当的控制输入，可以使得飞行器的状态（例如位置、速度、姿态等）直接映射到期望的轨迹，而无需解决复杂的微分方程。
差分平坦度（Differential Flatness） 是一种在优化式轨迹规划中常用的技术，它利用系统的差分平坦输出来简化轨迹生成问题。一个系统在差分平坦度意义上是平坦的，意味着系统的状态和控制输入可以被一组平坦输出表达。这些平坦输出是可以通过积分得到的，从而使得轨迹规划问题的复杂性降低到一维问题。

$\Psi_x(z,\dot{z}....z^{s-1}) \\ \mu = \Psi_\mu(z,\dot{z}....z^{s-1})$
微分平坦性可以消除微分系统中的微分约束条件。
差分平坦度的主要步骤：

系统建模与平坦输出的选择：
- 系统建模：首先，对系统进行建模，得到其动力学方程。这可以是非线性系统，例如机器人、飞行器或其他动态系统。
- 选择平坦输出：根据系统的特性，选择一个或多个平坦输出，这些输出可以通过代数方程表示。这些平坦输出的选择可以通过系统的解耦性和简化控制的角度来进行。
状态变量的表示：
- 利用系统的平坦输出，将系统的状态表示为与这些平坦输出相关的变量。这可以通过坐标变换来实现。
轨迹参数化：
- 将轨迹参数化为一组未知参数。这些参数将在优化问题中被调整，以生成满足动力学和约束条件的轨迹。
目标函数的定义：
- 定义一个目标函数，该函数包括最小化轨迹与期望轨迹的差异，同时满足系统动力学方程和约束条件。这可能涉及到轨迹的平滑性、最小化能量消耗等方面的考虑。
约束的添加：
- 添加任何必要的约束，如初始和终端状态的约束、动力学约束等。这确保了生成的轨迹在实际系统中是可行的。
优化问题的求解：
- 将上述目标函数和约束构建为一个优化问题，然后使用数值优化方法求解该问题。常见的优化方法包括线性规划、非线性规划或拓扑优化方法。
轨迹执行：
- 将优化求解得到的参数值代入轨迹参数化的公式中，得到最优轨迹。这个轨迹可以被用作实际系统的控制输入。

示例：二次系统的差分平坦度轨迹规划
考虑一个简单的连续时间二次系统：
$\ddot{x} = u ]$
其中 (x) 是系统的位置， (u) 是控制输入。

差分平坦输出： 对系统方程进行两次积分，得到平坦输出：

$\frac{1}{2}u\tau^2 ]$
其中， $(\tau)$ 是时间。

轨迹参数化： 将需要规划的轨迹表示为平坦输出的参数化形式：

$\frac{1}{2}u(t)t^2 ]$

目标函数和约束条件： 设定目标，例如最小化时间或最小化轨迹长度。同时，考虑系统的动力学约束，如控制输入的限制。
优化求解： 使用数学优化方法（如非线性规划）求解上述目标函数和约束条件的优化问题。
反参数化： 根据得到的优化参数，反解平坦输出，得到实际的轨迹。

这个简单的例子展示了如何使用差分平坦度方法对一个系统进行轨迹规划。在实际中，系统的动力学可能更为复杂，但通过利用差分平坦度，可以将轨迹规划问题变得更易处理。这种方法常见于机器人、飞行器等控制系统的轨迹规划中。

最小挠曲优化（Minimum Snap Optimization）

是一种用于轨迹规划的优化方法。它旨在生成具有最小挠曲（snap）的轨迹，以实现飞行器或机器人的平滑运动。在此答案中，我将详细介绍Minimum Snap优化的流程，并提供一个例子来帮助您更好地理解。
Minimum Snap优化通常涉及光滑多段轨迹的生成。以下是Minimum Snap优化的详细流程：

定义问题：首先，需要明确问题的定义。这包括设置路径的起始状态（位置和姿态）和终止状态，并确定中间路径点（如果有）。例如，对于四旋翼无人机，起始状态可能是无人机在某一位置上的初始姿态和速度，终止状态可能是无人机到达目标位置时的姿态和速度。
选择多项式表示: 轨迹选择多项式表示，这是因为多项式具有良好的光滑性和求导性质。例如，使用五次多项式可以表示一段平滑的轨迹段。
确定轨迹多项式的阶次: 下一步是确定每个轨迹段多项式的阶次。通常，为了保持光滑性，每个轨迹段的多项式阶次保持一致。这可以简化问题并提高计算效率。阶次的选择取决于问题的特定要求和系统的动力学特性。
设计目标函数: 对于每个轨迹段，需要设计一个目标函数，以明确优化的目标。例如，在最小挠曲优化中，目标可能是最小化差分推力，从而实现能量的节省。
确定约束条件: 在进行优化之前，需要确定约束条件。这些约束条件可能涉及起始和终止状态的位置、速度和加速度约束，以及中间路径点的位置约束等。同时，可以根据具体应用的需求添加其他约束条件。
解决优化问题: 最后，使用优化算法来解决 Minimum Snap 优化问题。常用的优化算法包括线性规划、二次规划或非线性规划等。通过求解优化问题，可以得到满足约束条件的最优轨迹。

这是Minimum Snap优化的一个简单例子，以飞行器的路径规划为例：
假设我们希望将无人机从起始位置飞到目标位置，并要求无人机在飞行过程中保持平滑的运动。

首先，我们明确路径的起始状态和目标状态，包括位置和姿态。
然后，选择一个合适的多项式来表示轨迹。假设我们选择五次多项式。
确定每段轨迹多项式的阶次。我们可以选择所有轨迹段的多项式阶次都为五次，以保持光滑性。
设计目标函数。在这个例子中，我们可以选择最小化差分推力作为目标，以减少能量消耗。
确定约束条件。约束条件可能包括起始和目标状态的位置、速度和加速度等。
解决优化问题。使用适当的优化算法，例如二次规划，求解满足约束条件的最优轨迹。

通过以上流程，我们可以生成满足约束条件的平滑轨迹，使无人机从起始位置飞行到目标位置。
希望这个例子能够帮助您理解Minimum Snap优化的流程。通过选择合适的多项式表示和设计适当的目标函数和约束条件，可以在路径规划和轨迹生成中实现更加平滑和高效的运动。

正文

问题建模

多旋翼飞行器的动力学建模

多旋翼飞行器的动力学建模经历了几次改进，改进点如下：

不考虑阻力,用Z-X-Y欧拉角定义偏航。(Mellinger ICRA 2011)。
不考虑阻力,用Z-Y-X欧拉角定义偏航(Faessler RAL 2017)。
不考虑阻力,用机体X轴投影定义偏航(Faessler RAL 2017)。
在3的基础上考虑线性阻力(Faessler RAL 2017)。
用Hopf纤维丛定义偏航(Watterson ISRR 2017)。
SE(3)控制器存在奇异性(Lee CDC 2010)。

不考虑阻力的动力学模型精度较低。用欧拉角或投影定义的偏航存在奇异性。微分平坦的多旋翼飞行器动力学模型应当考虑阻力但减少奇异性。
**多旋翼飞行器状态
$\{r,v,R,w\} \in \mathbb{R^3 \times R^3}\times SO(3) \times \mathbb{R^3}$
位置、速度、姿态、体轴角速率
多旋翼飞行器控制输入(映射后)
$\{f,\tau\} \in \mathbb{R_{\geq 0} \times R^3}$
总推力、扭矩
多旋翼飞行器非线性动力学
$\begin{cases} \dot{r} = v, \\ m \dot{v} = -mge_3 -RDR^T \sigma(||v||)v +Rfe_3, \\ \dot{R} = R \hat{\omega}, \\ M \dot{\omega} = \tau - \omega \times M \omega - A(\omega) -B(R^Tv). \end{cases}$

多旋翼飞行器平坦输出

$\{r,\psi\} \in \mathbb{{R^3}}\times SO(2)$
位置、偏航角
其中：
在这里插入图片描述

平坦化转化
$\{r,\psi\} \in \mathbb{{R^3}}\times SO(2) \\ || x = \Psi_x(z,\dot{z}....z^{s-1}) \\ ||\mu = \Psi_\mu(z,\dot{z}....z^{s-1}) \\ \Downarrow \\ x = \{r,v,R,w\} \in \mathbb{R^3 \times R^3}\times SO(3) \times \mathbb{R^3} \\ u = \{f,\tau\} \in \mathbb{R_{\geq 0} \times R^3}$
上面的平坦转化是如何完成的。
显然，我们知道
$\\ v = \dot{r}$
飞行器的机体坐标系的X轴和Y轴进行了牛顿方程的点积：
$x_b = Re_1,y_b = Re_2\\ (Re_i)^Tm\dot{v} = (Re_i)^T(-mge_3 -RDR^T\sigma(||v||)v +Rfe_3),\forall i \in \{1,2\}$
得到的结果是
$(Re_i)^T(\dot{v} + \frac{d_h}{m} \sigma(||v||+ge_3) = 0,\forall i \in \{1,2\}$
从几何角度来看，我们有
$x_b \perp (\dot{v} + \frac{d_h}{m} \sigma(||v||v)+ge_3) \\ y_b \perp (\dot{v} + \frac{d_h}{m} \sigma(||v||v)+ge_3)$
因为推力和机体Z轴共享相同的方向
$z_b = N((\dot{v} + \frac{d_h}{m} \sigma(||v||v)+ge_3) ),N(x):=x/||x||_2$
对牛顿方程在机体坐标系的Z轴上进行点积：
$(Re_3)^Tm\dot{v} =(Re_3)^T(-mge_3-RDR^T\sigma(||v||)v+Rfe_3)$
得到
$z_b^T(m\dot{v} +d_v\sigma(||v||)v+mge_3)$
现在我们有偏航航向和机体Z轴都是已知的，偏航旋转四元数是 $q_\psi = (cos(\psi/2),0,0sin(\psi/2))^T$
倾斜旋转四元数是 $q_z = \frac{1}{\sqrt{2(1+z_b(3))}}(1+z_b(3),-z_b(2),z_b(1),0)^T$
倾斜旋转没有惯性坐标系Z轴的分量，因为它是使用霍普夫纤维分解的。飞行器的姿态四元数因此是 $=q_z \otimes q_\psi$
展开它得到
$q_z = \frac{1}{\sqrt{2(1+z_b(3))}}\begin{pmatrix} (1+z_b(3)cos(\psi /2)\\ -z_b(2)cos(\psi /2)+z_b(1)sin(\psi /2)\\ z_b(1)cos(\psi /2)+z_b(2)sin(\psi /2)\\ (1+z_b(3))sin(\psi/2)\\ \end{pmatrix}$
因此，姿态旋转矩阵是唯一确定的 $R = R_quat(q)$
现在旋转是已知的
$\dot{R} = R\hat{w}$
这意味着
$(R^T\dot{R})^\vee$
等效地
$=2(q_z \otimes q_{\psi})^{-1}(\dot{q}_z \otimes q_{\psi}+q_z \otimes \dot{q}_{\psi})$
代入倾斜旋转四元数和偏航旋转四元数给出
$\begin{pmatrix} \dot{z}_b(1)sin(\psi)-\dot{z}(2)cos(\psi)-\dot{z}(3)(z_b(1)sin(\psi)-z_b(2)cos(\psi))/(1+z_b(3))\\ \dot{z}_b(1)cos(\psi)-\dot{z}(2)sin(\psi)-\dot{z}(3)(z_b(1)cos(\psi)-z_b(2)sin(\psi))/(1+z_b(3))\\ (z_b(2)\dot{z}_b(1) - z_b(1)\dot{z}_b(2))/(1+z_b(3))+\psi,\ \end{pmatrix}$
对机身Z轴进行微分得到
$\dot{z}_b = \frac{d_h}{m}DN(\dot{v}+\frac{d_h}{m}\sigma(||V||)v+ge_3)^T(\frac{m}{d_h}\ddot{v}+\sigma(||v||)\dot{v}+\dot{\sigma}(||v||)\frac{v^T\dot{v}}{||v||}v),\\ DN(x) := \frac{1}{x}(I-\frac{xx^T}{x^Tx}).$
现在我们已经用平面输出的有限导数表示了体轴速率。
现在所有的状态和输入都已知，除了扭矩。我们通过对体轴速率进行微分来得到它的表达式，使用 $r^{(4)},\psi^{(2)}$
因此，
$\tau = M\dot{w} +w\times Mw+A(w) +B(R^Tv)$

最后，我们完成了平面输出变换的推导。规划高阶连续性的平面输出轨迹就足以满足动力学要求。

非线性动力学与反馈前馈控制中的微分平坦变换。
$\begin{cases} \dot{r} = v ,\\ m\dot{v} = -mge_3 -RDR^T\sigma(||v||)v +Rf3_3, \\ \dot{R} =R\hat{w}, \\ M\dot{w} = \tau -w\times Mw -A(w) - B(R^Tv). \end{cases}$
微分平坦输出
${r,\psi} \in \mathbb{R^3} \times SO(2)$
在微分平坦输出空间中的轨迹
$\rightarrow \mathbb{R^3} \times SO(2)$
对多旋翼飞行器控制动力学方程做了微分平坦化转变后，接下来就是通过样条曲线参数化生成行动轨迹，为什么要样条曲线参数化平面输出轨迹呢？优势如下：
• 易于确定平滑性标准
• 导数的简单闭式计算
• 在三个维度上解耦的轨迹生成
• 高数值稳定性

约束建模

定义域空间

轨迹生成约束包括：
1.局部约束：必须要经过哪些点、到哪些点的状态如何（位置、位姿、速度、角速度）
2.全局约束：整体平滑（衡量平滑有轨迹的jerk、snap…值总和平均代价）、耗时、平均能耗…
然而不管是哪种约束都是和轨迹形态有关系，因为求解约束代价用的是离散化采样方式求解，对应的也就是轨迹上的采样点。相当于是轨迹函数可行的定义域空间。
那么就会涉及到我们如何用前面带动力学约束的采样点来如何构建轨迹，也就是说我们用什么函数来拟合轨迹。这个拟合函数会决定轨迹的采样点会影响到代价函数的求值，所以这部分我们需要先介绍下我们选取什么函数来拟合轨迹。
平滑的一维轨迹
5阶多项式轨迹： $x_t =p_5t^5+p_4x^4+p_3x^3+p_2x^2+p_1t+p_0 \Longrightarrow 轨迹参数化$
• 边界条件没有中间条件

	位置	速度	加速度
t=0	a	0	0
t=T	b	0	0

求解：
$\begin{bmatrix}a\\b \\ 0\\ 0\\0\\0 \end{bmatrix} = \begin{bmatrix}0&0&0&0&0&1\\T^5&T^4&T^3&T^2&T&1\\ 0&0&0&0&1&0\\ 5T^4&4T^3&3T^2&T2^1&1&0\\0&0&0&2&0&0\\20T^3&12T^2&6T^1&2&0&0 \end{bmatrix} \begin{bmatrix}c_5\\c_4 \\ c_3\\ c_2\\c_1\\c_0 \end{bmatrix}$
平滑的多段一维轨迹
• 每个段都是一个多项式。
• 不需要固定阶数，但保持相同的阶数可以使问题更简单。
• 每个段的时间持续必须是已知的！

	位置	速度	加速度
t=0	a	v0	0
t=T	b	vT	0

求解：
$\begin{bmatrix}a\\v_0 \\ 0\\b\\ v_T\\0 \end{bmatrix} = \begin{bmatrix}1&0&0&0&0&0\\0&1&0&0&0&0\\0&0&2&0&0&0\\1&T&T^2&T^3&T^4&T^5\\ 0&1&T2^1&3T^2&4T^3&5T^4\\0&0&2&6T^1&12T^2&20T^3\end{bmatrix} \begin{bmatrix}c_5\\c_4 \\ c_3\\ c_2\\c_1\\c_0 \end{bmatrix} , d=A_F(T)c\\ f(t) =\begin{cases} f_1(t) = \sum_{i=0}^{N} p_{1,i}t^i & T_0 \leq t \leq T_1\\ f_2(t) = \sum_{i=0}^{N} p_{2,i}t^i & T_1 \leq t \leq T_2\\ \vdots \\ f_M(t) = \sum_{i=0}^{N} p_{M,i}t^i & T_{M-1} \leq t \leq T_M\\ \end{cases}$

• 边界条件：起始点，目标位置（方向）
• 中间条件：途中位置（方向）
• 途中位置可以通过路径规划（A*、RRT*等）找到，在前面的三文章中介绍过
• 平滑度标准，最小化“输入”变化率

约束条件：
• 导数约束： $\begin{cases} \mathbf{f}_j^{(k)}(t_j) = \mathbf{x}_0^{(j)},\\ \mathbf{f}_j^{(k)}(t_j) = \mathbf{x}_T^{(j)} \end{cases}$
• 连续性约束： $\mathbf{f}_j^{(k)}(t_j) = \mathbf{f}_{j+1}^{(k)}(t_j)$

贝赛尔曲线表示

贝塞尔曲线通过控制点的选取来改变曲线的形状，具有轨迹曲率连续可导、易于跟踪、满足移动机器人动力学约束且仅需少量控制点就可生成轨迹等优点。根据控制点的个数，贝塞尔曲线被分为一阶贝塞尔曲线（0个控制点）、二阶贝塞尔曲线（1个控制点）、三阶贝塞尔曲线（2个控制点）等等。
贝塞尔曲线特点：

在贝塞尔曲线中，只有起点和终点在曲线上，其他点均为调整曲线形状和结束的控制点；
贝塞尔曲线通过起始点和终止点，并与起始点和终止点的折线相切，在对自车路径规划过程中可根据曲线的起始点和终止点的切线方向确定自车起始点姿态和目标点姿态；
至少需要三阶贝塞尔曲线（四个控制点）才能生成曲率连续的路径；
若要求两端弧线拼接在一起依然是曲率连续的，必须要求两段弧线在连接处的曲率是相等的；
贝塞尔曲线中一个控制点改变，曲线的形状都会随之改变；

至少需要三阶贝赛尔曲线（四个控制点）才能表示生成曲率连续的曲线.通常定义n+1个控制点组成n阶贝塞尔曲线，其表达式如下 :
$=\sum_i^np_i B_{i,n}(t),t \in [0,1]$
式中， $p_i$ 与 t 分别是控制点的坐标值与参数， $B_{i,n}(t)$ 为Bernstein多项式，其表达式如式：
$B_{i,n}(t) =C_n^i t^i(1-t)^{n-i},i =0,1 \dots n$
式中， $C_n^i$ 为二次项系数。 3次贝塞尔曲线的参数方程表达式可以表述为式：
$p(t) = p_0(1-t)^3+3p_1(1-t)^2t +3p_2(1-t)^2+p_3t^3$
式中， $p_i(x_i ，y_i)$ ； $x_i$ 和 $y_i$ 分别代表控制点的横纵坐标。
曲线定义：起始点、终止点（也称锚点）、控制点
数据点：指一条路径的起始点和终止点。
控制点：控制点决定了一条路径的弯曲轨迹

约束条件

在平面输出空间中显式地最小化某些导数
• 最小Jerk：最小化角速度，适用于视觉跟踪
• 最小Snap：最小化差分推力，节省能量

导数	平移	旋转	推力
0	位置
1	速度
2	加速度	旋转
3	加加速度	角速度	推力
4	捏角	角加速度	差分推力

如何确定轨迹的阶数？
• 在某个阶数上保证平滑性。• 在某个阶数上保证连续性。• 在某个阶数上最小化控制输入。
平滑性意味着其导数是连续的。为确保单段轨迹的平滑性的最小次数多项式：
最小 jerk：N = 2 * 3(jerk) - 1 = 5
最小 snap：N = 2 * 4(snap) - 1 = 7
为确保 k 段轨迹的平滑性的最小次数多项式：

最小 jerk：
约束数量：3 + 3 + (k-1) = k + 5
未知数数量：(N+1) * k
(𝑁 + 1) ∗ 𝑘 = 𝑘 + 5 𝑁 =5/k

Minimum Snap Trajectory Generation

Minimum Snap Trajectory Generation算法旨在最小化轨迹的“snap”，即轨迹的第四阶导数，以确保在给定的约束条件下生成最平滑的轨迹。
成本函数

机器人等系统的轨迹，以最小化由加速度、角加速度和其相应的高阶导数（“snap”）所定义的多项式轨迹的总体运动代价。在这种情况下，一个多项式段的成本函数通常定义为最小化 snap 的总和。Snap 是轨迹的第四阶导数，表示对加速度的变化率。成本函数的形式可能是对 snap 的积分或 snap 的平方和。在最小 snap 的情况下，典型的成本函数形式可能如下所示：
$\int_{t_0}^{t_f} \left(\frac{d^4}{dt^4}x(t)\right)^2 dt$
这里， $x (t)$ 是轨迹的位置。这个成本函数的目标是通过调整轨迹的系数，以使得轨迹的 snap 尽可能小。
对于多段轨迹的snap代价函数推导如下：
$f(t) = \sum_ip_it^i\
\Rightarrow f^{(4)}(t) = \sum_{i \geq4}i(i-1)(i-2)(i-3)t^{i-4}p_i \
\Rightarrow (f^{(4)}(t))2 = \sum_{i \geq 4,l \geq 4}i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)t^{i+l-8}p_ip_l \
\Rightarrow J(T) = \int_{T_{j-1}}^{T_j}(f{(4)}(t))^2 = \sum_{i \geq 4,l \geq 4} \frac{i(i-1)(i-2)(i-3)j(l-1)(l-2)(l-3)}{i+l-7}(T_j^{{i+l-7}-T_{j-1}}{i+l-7})p_ip_l \
\Rightarrow J(T) = \int_{T_{j-1}}^{T_j}(f{(4)}(t))^2 \
=\begin{bmatrix}\vdots \p_i \ \vdots\ \end{bmatrix}^T \begin{bmatrix}\vdots \ \dots \frac{i(i-1)(i-2)(i-3)j(l-1)(l-2)(l-3)}{i+l-7}T^{i+l-7} \dots \ \vdots\ \end{bmatrix} \begin{bmatrix}\vdots \p_l \ \vdots\ \end{bmatrix} \
\Rightarrow J_j(T) = P_j^T Q_j P_j \text(最小化这项)\$
一个多项式段的导数约束推到，起止点，中间必经点约束（0阶导数）
$\text{轨迹曲线求导如下，转成向量表示} \\ x_t =p_5t^5+p_4x^4+p_3x^3+p_2x^2+p_1t+p_0 \\ x(0) = \dots , x(T) = \dots \\ \dot{x}(0) = \dots,\dot{x}(T) = \dots \\ \ddot{x}(0) = \dots,\ddot{x}(T) = \dots \\ \vdots \\ p_0=\dots,\\ p_5t^5 + p_4t^4 + p_3t^3 + p_2t^2 + p_1t + p_0 = \dots\\ [T^5,T^4,T^3,T^2,T,1]\begin{bmatrix}p_5 \\p_4\\p_3 \\ p_2 \\p_1 \\ p_0\\ \end{bmatrix} =\dots\\ \text{把五次元曲线表示的求导向量矩阵带入多段曲线表示方程求解约束方程} \\ f_j^{(k)}(T_j) = x_j^{(k)} \\ \Rightarrow \sum_{i \geq k} \frac{i!}{(i-k)!}T_j^{l-k}p_{j,i}=x_{T,j}^{(k)} \\ \Rightarrow \begin{bmatrix}\dots \frac{i!}{(i-k)!}T_j^{i-k} \dots\\ \end{bmatrix} \begin{bmatrix}\vdots \\p_{j,i}\\ \vdots\\ \end{bmatrix} = x_{T,j}^{(k)} \\ \Rightarrow \begin{bmatrix}\dots \frac{i!}{(i-k)!}T_j^{i-k} \dots\\ \dots \frac{i!}{(i-k)!}T_j^{i-k} \dots\\\end{bmatrix} \begin{bmatrix}\vdots \\p_{j,i}\\ \vdots\\ \end{bmatrix} = \begin{bmatrix} x_{0,j}^{(k)}\\ x_{T,j}^{(k)}\end{bmatrix}\\ \Rightarrow A_j p_j = d_j$
两个段之间的连续性约束：
• 在未给定特定导数的情况下，确保轨迹段之间的连续性
$f_j^{(k)}(T_j) = f_{j=1}^{(k)}(T_j) \\ \Rightarrow \sum_{i \geq k} \frac{i!}{(i-k)!}T_j^{i-k}p_{j,i} - \sum_{i \geq k} \frac{l!}{(i-k)!}T_j^{l-k}p_{j+1,l} =0 \\ \Rightarrow \begin{bmatrix} \dots \frac{i!}{(i-k)!}T_j^{l-k} \dots \frac{l!}{(l-k)!}T_j^{l-k} \dots\end{bmatrix} \begin{bmatrix} \vdots \\ p_{j,i} \\ \vdots \\ p_{j+1,l}\\ \vdots\end{bmatrix} =0 \\ \Rightarrow \begin{bmatrix} A_j - A_{j+1}\end{bmatrix} \begin{bmatrix} P_{j} \\ p_{j+1}\end{bmatrix} =0$
有约束的二次规划（QP）表达式：
$\min \begin{bmatrix} \mathbf{p}_1 \\ \vdots \\ \mathbf{p}_M \end{bmatrix}^\top \begin{bmatrix} \mathbf{Q}_1 & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \ddots & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{Q}_M \end{bmatrix} \begin{bmatrix} \mathbf{p}_1 \\ \vdots \\ \mathbf{p}_M \end{bmatrix} \\ s. t. \mathbf{A}_{eq} \begin{bmatrix} \mathbf{p}_1 \\ \vdots \\ \mathbf{p}_M \end{bmatrix} = \mathbf{d}_{eq}$

这是一个典型的凸优化约束求解表达形式，用于最小化“Minimum Snap Trajectory Generation”。

优化求解

决策变量映射

$\begin{bmatrix} p_1 \\ \vdots \\ p_M \end{bmatrix}^T \begin{bmatrix} Q_1 & 0 & \cdots & 0 \\ 0 & \ddots & \ddots & \vdots \\ \vdots & \ddots & \ddots & 0 \\ 0 & \cdots & 0 & Q_M \end{bmatrix} \begin{bmatrix} p_1 \\ \vdots \\ p_M \end{bmatrix} ，M_jP_j=d_j \\ \Downarrow \\\Downarrow\\ J = \begin{bmatrix} d_1 \\ \vdots \\ d_M \end{bmatrix}^T \begin{bmatrix} M_1 & 0 & \cdots & 0 \\ 0 & \ddots & \ddots & \vdots \\ \vdots & \ddots & \ddots & 0 \\ 0 & \cdots & 0 & M_M \end{bmatrix}^{-T} \begin{bmatrix} Q_1 & \cdots & 0 \\ \vdots & \ddots & \vdots \\ 0 & \cdots & Q_M \end{bmatrix} \begin{bmatrix} M_1 & 0 & \cdots & 0 \\ 0 & \ddots & \ddots & \vdots \\ \vdots & \ddots & \ddots & 0 \\ 0 & \cdots & 0 & M_M \end{bmatrix}^{-1} \begin{bmatrix} d_1 \\ \vdots \\ d_M \end{bmatrix}$
其中多项式路径求导向量表示如下：
$p_5t^5 + p_4t^4 + p_3t^3 + p_2t^2 + p_1t + p_0 \\ x'(t) = 5p_5t^4 + 4p_4t^3 + 3p_3t^2 + 2p_2t + p_1 \\ x''(t) = 20p_5t^3 + 12p_4t^2 + 6p_3t + 2p_2 \\ M = \begin{bmatrix}0&0&0&0&0&1\\ 0&0&0&0&1&0\\ 0&0&0&2&0&0\\ T^5&T^4&T^3&T^2&T&1\\ 5T^4&4T^3&3T^2&T2^1&1&0\\ 20T^3&12T^2&6T^1&2&0&0 \end{bmatrix}$
• 直接优化多项式轨迹在数值上是不稳定的。
• 更倾向于采用一种变量变换，优化分段端点导数。
• 我们有 $M_j p_j = d_j$ ，其中 $M_j$ 是将多项式系数映射到导数的映射矩阵。
其中， $J$ 是矩阵，包含了 $d_1$ 到 $d_M$ 的信息。 $M_j$ 是将多项式系数映射到导数的映射矩阵， $p_j$ 是多项式系数向量， $d_j$ 是导数向量。

固定变量与自由变量分离

引入一个permutation matrix (置换矩阵)C来将连续性约束进行引入到代价函数中。C 矩阵本身是一个只包含0和1的矩阵，它的作用就是给所有 $d_0,d_1 ⋯ d_M ]$ 进行一个组合，将固定的变量放在头部，将需要就行优化的变量方位尾部，并且对于连续性约束的变量，由于他们的值是相等的，因此选择其中一个来表达连续性约束。说起来有一些抽象，用数学表达就会清晰很多。

根据之前的介绍，紫色框中变量的值都是我们事先已经设置好的，浅蓝色框中的变量就是代价函数在优化时分配的最优值，也就是需要优化的变量。
将C带入代价函数，则最终得到
$\mathbf{C}^T \begin{bmatrix} \mathbf{d}_F \\ \mathbf{d}_P \end{bmatrix} = \begin{bmatrix} \mathbf{d}_1 \\ \vdots \\ \mathbf{d}_M \end{bmatrix} \Longrightarrow J = \begin{bmatrix} \mathbf{d}_F \\ \mathbf{d}_P \end{bmatrix}^T \mathbf{C} \mathbf{M}^{-T} \mathbf{Q} \mathbf{M}^{-1} \mathbf{C}^T \begin{bmatrix} \mathbf{R}_{FF} & \mathbf{R}_{FP} \\ \mathbf{R}_{PF} & \mathbf{R}_{PP} \end{bmatrix} \begin{bmatrix} \mathbf{d}_F \\ \mathbf{d}_P \end{bmatrix}$
• 使用选择矩阵 $\mathbf{C}$ 来分离自由变量 $\mathbf{d}_P ）$ 和受约束变量 $\mathbf{d}_F ）$ 。
• 自由变量：导数未指定，仅由连续性约束强制执行。

构建解选择矩阵

$d_{0,j}^{(k)}$ 就是对应的受约束元素，下面的图描述的就是上面
j是指代第几段曲线中的第几个点，k是几阶导数，0是指代第几段生成曲线
固定导数：固定起始点、目标状态和中间位置的导数
自由导数：中间连接点的所有导数

选择矩阵直接包括了起点和终点的各阶导数、中间点位置的固定约束信息，也隐式的包括了中间点各阶导数连续的约束条件。该选择矩阵，直接将原问题（带约束的QP问题）转化为无约束的QP问题，可直接求导求出。
其中，上式表示选择矩阵 $\mathbf{C}$ 与包含自由变量 $\mathbf{d}_F$ 和受约束变量 $\mathbf{d}_P$ 的列向量组合的矩阵相乘。
其中，上述方程中包含了选择矩阵 $\mathbf{C}$ 和映射矩阵 $\mathbf{M}$ ，用于分离自由变量和受约束变量。
最终将带约束的QP优化问题，转换成了一个无约束的QP问题，对于上式因为它是一个有全局最小值的凸函数，可以通过闭式求解所以对其求导取极值点就是它的全局最优值：
令 $\mathbf{C} \mathbf{M}^{-T} \mathbf{Q} \mathbf{M}^{-1} \mathbf{C}^T =R$
并对R矩阵根据dF 和dP 的尺寸进行分块，则得到如下变换：
$J = d_F^TR_{FF}d_F+d_F^TR_{FP}d_p+d_P^TR_{PF}d_F+d_P^TR_{PP}d_P$
对于上式J是一个标量，并且Q矩阵是对称矩阵，而 $\mathbf{C} \mathbf{M}^{-T} \mathbf{Q} \mathbf{M}^{-1} \mathbf{C}^T =(\mathbf{C} \mathbf{M}^{-T} \mathbf{Q} \mathbf{M}^{-1} \mathbf{C}^T)^T$ ，所以R也是对称矩阵，则 $R_{PF} = R_{FP}^T$
对于上式我们要求的变量是 $d_P$ ，因此将其对求导， $d_P$ 并零导数为0时取极值点的值对应的就是最优的 $d_P^*$
$\frac{\partial{J}}{\partial{d_p}} =2d_F^TR_{FP}+2R_{PP}d_p = 0 \\ \Rightarrow d_p^* = -R_{PP}^{-1}R_{FP}^Td_F$
求解出来了 $d_P^*$ 之后，只需要将它带入C 和M就能求出最终的各个多项式的系数：
$M^{-1}C^T\begin{bmatrix} d_F \\ d_P^* \end{bmatrix}$

轨迹生成

如何获得waypoints？

因为闭式解涉及到C、M矩阵求逆，当矩阵较大时，为了加快求解速度。可以用Hierarchical approach 层级式方法：path planning + trajectory generation路径规划+轨迹生成
path planning：低维的状态空间。 trajectory generation：天然包含导数，更高维。
在path planning时候求解出可行解，作为求解的选择空间，减少求解的搜索范围，加快解收敛速度。

如何解决生成的轨迹与障碍物碰撞

轨迹生成属于back-end部分，前面经过前面几篇文章介绍的方法找到了路径点。以搜索到的路径为必经点做解搜索，并且以搜索到的路径点作为生成每段轨迹的起止点。然后轨迹的生成我们使用了多项式方式拟合出一条轨迹，有可能拟合的轨迹点会碰到障碍物。这就是为什么搜索完路径点生成轨迹还能碰到障碍物的原因所在。为了解决拟合出的轨迹碰到障碍物有两大思路：
1.对拟合出的轨迹做后判断是否碰到障碍物，如果碰到在碰到点附近增加采样点，让碰撞点拟合轨迹的范围更小，提高拟合的精度
2.以一定时间段范围为采样区间，在这个时间段内机器人可行驶的距离范围内+障碍物约束，得到这个时间段可通行的范围多边形；然后假设这个范围生成的轨迹端点生成下一个时间段的距离范围+障碍物约束得到下一个时间段的可通行范围多边形，如此一直下去得到一条可行的飞行走廊；因为前面有个假设就是一段一段的生成轨迹，以这样轨迹端点作为下一个可通行范围多边形的生成边界。但是这样生成轨迹很可能是不光滑不符合动力学特性最优的，所以我们需要对多段可通行方位组成的多边形重新规划可行的轨迹。
如此可见每一段的轨迹时间间隔是如何的是非常有讲究的，对生成轨迹质量、生成轨迹的速度都是有很大影响的。

• 分段轨迹取决于分段时间分配。
• 时间分配显著影响最终轨迹。
• 如何获得适当的时间分配？
要获得适当的时间分配，通常有以下几种方法：

梯形速度时间曲线： 使用梯形速度时间曲线来确定每个段的时间持续。梯形速度曲线是一种通用的速度规划方法，其中在每个段内先加速到最大速度，然后再减速到零。这种方法简单而直观，但可能并不总是最优。
期望平均速度： 假设在每个段内，机器人以最大速度加速到最大速度，然后再减速到零。通过使用期望的平均速度来计算每个段的时间，可以获得相对较好的时间分配。
迭代优化： 可以将问题建模为优化问题，其中目标是最小化轨迹的代价函数，同时满足一些约束条件，包括时间分配。通过迭代求解这个优化问题，可以得到更精确的时间分配。
环境感知： 考虑环境感知信息，例如障碍物的位置和速度，来调整时间分配。这可以通过实时感知环境并在轨迹生成过程中进行调整来实现。

总体来说，选择适当的时间分配方法取决于具体的问题和应用场景。一些情况下简单的方法就足够了，而在复杂的环境中可能需要更高级的技术。

• 瞬时线性约束：
⎯ 起始点、目标状态约束 $\mathbf{A}\mathbf{p} = \mathbf{b}$
⎯ 过渡点约束 $\mathbf{A}\mathbf{p} = \mathbf{b}, \, \mathbf{A}\mathbf{p} \leq \mathbf{b}$
⎯ 连续性约束 $\mathbf{A}\mathbf{p}_i = \mathbf{A}\mathbf{p}_{i+1}$
• 区间线性约束：
⎯ 边界约束 $\mathbf{A}(t)\mathbf{p} \leq \mathbf{b}, \, \forall t \in [t_l, t_r]$
⎯ 动态约束 $\mathbf{A}(t)\mathbf{p} \leq \mathbf{b}, \, \forall t \in [t_l, t_r]$
o 速度约束
o 加速度约束

迭代加点的方法

如果发生碰撞，就在两个航迹点的中间再选一个航迹点，然后重新优化生成轨迹，重复这样的操作，直到生成安全的轨迹。如下图：

• 初始路径假设无碰撞。当计算时候路径上碰到障碍物，可以通过添加中间路径点重新生成这段的轨迹。完成的我们可以通过迭代将轨迹逼近到路径。

迭代加额外约束bounding box

先生成一条轨迹，检查极值，若极值超出bounding box，则施加一个bounding box往下压。
缺点：迭代求解非常耗时，假如是一个没有可行解的问题，可能要迭代很多次才能确定无解。

在离散时间点添加大量约束。
• 总是生成过于保守的轨迹。
• 太多的约束，计算负担很重。

构建安全飞行走廊

步骤 1. 检测障碍物
步骤 2. 搜索飞行走廊
步骤 3. 扩展飞行走廊
步骤 4. 生成完全位于飞行走廊内的动态可行轨迹

• 在单一片段中看起来可能不够智能。
• 在基于走廊的轨迹生成中效果良好。
• 走廊中的重叠区域提供了大的自动调整空间。
构建出可安全飞行的走廊，求解轨迹的方法和上面的一样。飞行走廊改变的是采样空间其实也就是对定义域层面做了优化来达到轨迹求解优化，在求解方程上没做什么变化。

小结：

文章开始从为什要轨迹优化这一问题视角切入，介绍了轨迹优化是什么、轨迹优化的求解流程和包括哪些模块。轨迹优化这个问题就是如何基于采样路径点来生成一条符合约束的轨迹，约束一般包括移动机器人动力学约束、路径可通行不能碰到障碍物、飞行轨迹光滑…所以可以看到轨迹生成其实是一个约束求解问题，代价函数包括：
1.局部约束：必须要经过哪些点、到哪些点的状态如何（位置、位姿、速度、角速度）
2.全局约束：整体平滑（衡量平滑有轨迹的jerk、snap…值总和平均代价）、耗时、平均能耗…
为了基于采样路径生成可行的轨迹空间，我们需要基于采样路径点做轨迹拟合（可以用多项式拟合、贝赛尔曲线拟合…),在文章分两个章节介绍了如何构建代价函数、常用的轨迹拟合函数。现在就存在两个方程代价函数、拟合曲线，如何把轨迹拟合函数融合到代价函数得到一个统一的求解方程呢，文章花了一个章节来介绍。
上面其实都是在对问题建立求解的数学模型，有了数学模型接下来要讨论的就是如何求解问题呢。文章在“优化求解”部分对如何把这个非凸问题转成凸问题做了详细介绍：包括推导、求解技巧。
求解完理论应该就可以生成移动轨迹了，然而实际上还是会存在很多问题，包括：求解速度、求解一些边界问题。其中很重要一个问题就是安全飞行（生成轨迹不要碰到障碍物），之所以生成轨迹可能会有碰到障碍物原因在于，轨迹生成其实是利用搜索的路径点拟合生成的，拟合过程是会带入不确定的碰到障碍物的概率进来。为了解决这个问题有两种思路，这部分在“轨迹生成”部分做了详细介绍。
了解决拟合出的轨迹碰到障碍物有两大思路：
1.对拟合出的轨迹做后判断是否碰到障碍物，如果碰到在碰到点附近增加采样点，让碰撞点拟合轨迹的范围更小，提高拟合的精度
2.以一定时间段范围为采样区间，在这个时间段内机器人可行驶的距离范围内+障碍物约束，得到这个时间段可通行的范围多边形；然后假设这个范围生成的轨迹端点生成下一个时间段的距离范围+障碍物约束得到下一个时间段的可通行范围多边形，如此一直下去得到一条可行的飞行走廊；因为前面有个假设就是一段一段的生成轨迹，以这样轨迹端点作为下一个可通行范围多边形的生成边界。但是这样生成轨迹很可能是不光滑不符合动力学特性最优的，所以我们需要对多段可通行方位组成的多边形重新规划可行的轨迹。