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的示例：

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

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

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

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

前置知识

Differential Flatness（差分平坦性）

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

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

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

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

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

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

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

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

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

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

最小挠曲优化（Minimum Snap Optimization）

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

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

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

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

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

正文

问题建模

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

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

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

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

多旋翼飞行器平坦输出

z = { r , ψ } ∈ R 3 × S O ( 2 ) z = \{r,\psi\} \in \mathbb{{R^3}}\times SO(2) z={r,ψ}∈R3×SO(2)
位置、偏航角
其中：

平坦化转化
z = { r , ψ } ∈ R 3 × S O ( 2 ) ∣ ∣ x = Ψ x ( z , z ˙ . . . . z s − 1 ) ∣ ∣ μ = Ψ μ ( z , z ˙ . . . . z s − 1 ) ⇓ x = { r , v , R , w } ∈ R 3 × R 3 × S O ( 3 ) × R 3 u = { f , τ } ∈ R ≥ 0 × R 3 z = \{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} z={r,ψ}∈R3×SO(2)∣∣x=Ψx​(z,z˙....zs−1)∣∣μ=Ψμ​(z,z˙....zs−1)⇓x={r,v,R,w}∈R3×R3×SO(3)×R3u={f,τ}∈R≥0​×R3
上面的平坦转化是如何完成的。
显然，我们知道
$$\begin{gathered} r=r \\ v=\dot{r} \end{gathered}$$
飞行器的机体坐标系的X轴和Y轴进行了牛顿方程的点积：
x b = R e 1 , y b = R e 2 ( R e i ) T m v ˙ = ( R e i ) T ( − m g e 3 − R D R T σ ( ∣ ∣ v ∣ ∣ ) v + R f e 3 ) , ∀ i ∈ { 1 , 2 } 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\} xb​=Re1​,yb​=Re2​(Rei​)Tmv˙=(Rei​)T(−mge3​−RDRTσ(∣∣v∣∣)v+Rfe3​),∀i∈{1,2}
得到的结果是
( R e i ) T ( v ˙ + d h m σ ( ∣ ∣ v ∣ ∣ + g e 3 ) = 0 , ∀ i ∈ { 1 , 2 } (Re_i)^T(\dot{v} + \frac{d_h}{m} \sigma(||v||+ge_3) = 0,\forall i \in \{1,2\} (Rei​)T(v˙+mdh​​σ(∣∣v∣∣+ge3​)=0,∀i∈{1,2}
从几何角度来看，我们有
$$\begin{gathered} x_b\perp (\dot{v}+\frac{d_h}{m}\sigma (||v||v)+g{e}_3) \\ {y}_b\perp (\dot{v}+\frac{d_h}{m}\sigma (||v||v)+g{e}_3) \end{gathered}$$
因为推力和机体Z轴共享相同的方向
$z_b=N((\dot{v}+\frac{d_h}{m}\sigma (||v||v)+g{e}_3)),N(x):=x/||x|{|}_2$
对牛顿方程在机体坐标系的Z轴上进行点积：
$(R{e}_3{)}^{T}m\dot{v}=(R{e}_3{)}^T(-mg{e}_3-RD{R}^T\sigma (||v||)v+Rf{e}_3)$
得到
$f=z_b^T(m\dot{v}+d_v\sigma (||v||)v+mg{e}_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=q_z\otimes q_{\psi }$
展开它得到
$q_z=\frac{1}{\sqrt{2(1+z_b(3))}}\left(\begin{array}{c}(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{array}\right)$
因此，姿态旋转矩阵是唯一确定的$R=R_{q}uat(q)$
现在旋转是已知的
$\dot{R}=R\hat{w}$
这意味着
$w=(R^T\dot{R}{)}^{\vee }$
等效地
$w=2(q_z\otimes q_{\psi }{)}^{-1}({\dot{q}}_z\otimes q_{\psi }+q_z\otimes {\dot{q}}_{\psi })$
代入倾斜旋转四元数和偏航旋转四元数给出
$w=\left(\begin{array}{c}{\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{array}\right)$
对机身Z轴进行微分得到
$$\begin{gathered} {\dot{z}}_b=\frac{d_h}{m}DN(\dot{v}+\frac{d_h}{m}\sigma (||V||)v+g{e}_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{x{x}^T}{x^{T}x}). \end{gathered}$$
现在我们已经用平面输出的有限导数表示了体轴速率。
现在所有的状态和输入都已知，除了扭矩。我们通过对体轴速率进行微分来得到它的表达式，使用$r^{(4)},{\psi }^{(2)}$
因此，
$\tau =M\dot{w}+w\times Mw+A(w)+B(R^{T}v)$

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

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

约束建模

定义域空间

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

求解：
$\left[\begin{array}{c}a\\ b\\ 0\\ 0\\ 0\\ 0\end{array}\right]=\left[\begin{array}{cccccc}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& T{2}^1& 1& 0\\ 0& 0& 0& 2& 0& 0\\ 20T^3& 12T^2& 6T^1& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{c}_5\\ c_4\\ c_3\\ c_2\\ c_1\\ c_0\end{array}\right]$
平滑的多段一维轨迹
• 每个段都是一个多项式。
• 不需要固定阶数，但保持相同的阶数可以使问题更简单。
• 每个段的时间持续必须是已知的！

求解：
$$\begin{gathered} \left[\begin{array}{c}a\\ v_0\\ 0\\ b\\ v_T\\ 0\end{array}\right]=\left[\begin{array}{cccccc}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& T{2}^1& 3T^2& 4T^3& 5T^4\\ 0& 0& 2& 6T^1& 12T^2& 20T^3\end{array}\right]\left[\begin{array}{c}{c}_5\\ c_4\\ c_3\\ c_2\\ c_1\\ c_0\end{array}\right],d=A_F(T)c \\ f(t)=\{\begin{array}{ll}{f}_1(t)={\sum }_{i=0}^N{p}_{1,i}{t}^i& T_0\le t\le T_1\\ f_2(t)={\sum }_{i=0}^N{p}_{2,i}{t}^i& T_1\le t\le T_2\\ ⋮& \\ f_M(t)={\sum }_{i=0}^N{p}_{M,i}{t}^i& T_{M-1}\le t\le T_M\end{array} \end{gathered}$$

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

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

贝赛尔曲线表示

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

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

至少需要三阶贝赛尔曲线（四个控制点）才能表示生成曲率连续的曲线.通常定义n+1个控制点组成n阶贝塞尔曲线，其表达式如下 :
$p(t)={\sum }_i^n{p}_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{)}^{2}t+3p_2(1-t{)}^2+p_3{t}^3$
式中，$p_i(x_i，y_i)$；$x_i$和 $y_i$分别代表控制点的横纵坐标。
曲线定义：起始点、终止点（也称锚点）、控制点
数据点： 指一条路径的起始点和终止点。
控制点：控制点决定了一条路径的弯曲轨迹

约束条件

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

如何确定轨迹的阶数？
• 在某个阶数上保证平滑性。• 在某个阶数上保证连续性。• 在某个阶数上最小化控制输入。
平滑性意味着其导数是连续的。为确保单段轨迹的平滑性的最小次数多项式：
最小 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
(声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Guff_9hys/article/detail/879163