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. 最小化状态 ( x ) (\mathbf{x}) (x)和输入 ( u ) (\mathbf{u}) (u) 的能量泛函。

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

前置知识

Differential Flatness(差分平坦性)

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

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

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

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

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

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

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

  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优化的流程。通过选择合适的多项式表示和设计适当的目标函数和约束条件,可以在路径规划和轨迹生成中实现更加平滑和高效的运动。

正文

问题建模

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

image.png
多旋翼飞行器的动力学建模经历了几次改进,改进点如下:

  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)。

不考虑阻力的动力学模型精度较低。用欧拉角或投影定义的偏航存在奇异性。微分平坦的多旋翼飞行器动力学模型应当考虑阻力但减少奇异性。
image.png**多旋翼飞行器状态
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,τ}R0×R3
总推力、扭矩
多旋翼飞行器非线性动力学
{ r ˙ = v , m v ˙ = − m g e 3 − R D R T σ ( ∣ ∣ v ∣ ∣ ) v + R f e 3 , R ˙ = R ω ^ , M ω ˙ = τ − ω × M ω − A ( ω ) − B ( R T v ) . \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˙=v,mv˙=mge3RDRTσ(∣∣v∣∣)v+Rfe3,R˙=Rω^,Mω˙=τω×MωA(ω)B(RTv).

多旋翼飞行器平坦输出

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˙....zs1)∣∣μ=Ψμ(z,z˙....zs1)x={r,v,R,w}R3×R3×SO(3)×R3u={f,τ}R0×R3
上面的平坦转化是如何完成的。
显然,我们知道
r = r v = r ˙ r = r \\ v = \dot{r} r=rv=r˙
飞行器的机体坐标系的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(mge3RDRTσ(∣∣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}
从几何角度来看,我们有
x b ⊥ ( v ˙ + d h m σ ( ∣ ∣ v ∣ ∣ v ) + g e 3 ) y b ⊥ ( v ˙ + d h m σ ( ∣ ∣ v ∣ ∣ v ) + g e 3 ) 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) xb(v˙+mdhσ(∣∣v∣∣v)+ge3)yb(v˙+mdhσ(∣∣v∣∣v)+ge3)
因为推力和机体Z轴共享相同的方向
z b = N ( ( v ˙ + d h m σ ( ∣ ∣ v ∣ ∣ v ) + g e 3 ) ) , N ( x ) : = x / ∣ ∣ x ∣ ∣ 2 z_b = N((\dot{v} + \frac{d_h}{m} \sigma(||v||v)+ge_3) ),N(x):=x/||x||_2 zb=N((v˙+mdhσ(∣∣v∣∣v)+ge3)),N(x):=x/∣∣x2
对牛顿方程在机体坐标系的Z轴上进行点积:
( R e 3 ) T m v ˙ = ( R e 3 ) T ( − m g e 3 − R D R T σ ( ∣ ∣ v ∣ ∣ ) v + R f e 3 ) (Re_3)^Tm\dot{v} =(Re_3)^T(-mge_3-RDR^T\sigma(||v||)v+Rfe_3) (Re3)Tmv˙=(Re3)T(mge3RDRTσ(∣∣v∣∣)v+Rfe3)
得到
f = z b T ( m v ˙ + d v σ ( ∣ ∣ v ∣ ∣ ) v + m g e 3 ) f = z_b^T(m\dot{v} +d_v\sigma(||v||)v+mge_3) f=zbT(mv˙+dvσ(∣∣v∣∣)v+mge3)
现在我们有偏航航向和机体Z轴都是已知的,偏航旋转四元数是 q ψ = ( c o s ( ψ / 2 ) , 0 , 0 s i n ( ψ / 2 ) ) T q_\psi = (cos(\psi/2),0,0sin(\psi/2))^T qψ=(cos(ψ/2),0,0sin(ψ/2))T
倾斜旋转四元数是 q z = 1 2 ( 1 + z b ( 3 ) ) ( 1 + z b ( 3 ) , − z b ( 2 ) , z b ( 1 ) , 0 ) T q_z = \frac{1}{\sqrt{2(1+z_b(3))}}(1+z_b(3),-z_b(2),z_b(1),0)^T qz=2(1+zb(3)) 1(1+zb(3),zb(2),zb(1),0)T
倾斜旋转没有惯性坐标系Z轴的分量,因为它是使用霍普夫纤维分解的。飞行器的姿态四元数因此是 q = q z ⊗ q ψ q =q_z \otimes q_\psi q=qzqψ
展开它得到
q z = 1 2 ( 1 + z b ( 3 ) ) ( ( 1 + z b ( 3 ) c o s ( ψ / 2 ) − z b ( 2 ) c o s ( ψ / 2 ) + z b ( 1 ) s i n ( ψ / 2 ) z b ( 1 ) c o s ( ψ / 2 ) + z b ( 2 ) s i n ( ψ / 2 ) ( 1 + z b ( 3 ) ) s i n ( ψ / 2 ) ) 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} qz=2(1+zb(3)) 1 (1+zb(3)cos(ψ/2)zb(2)cos(ψ/2)+zb(1)sin(ψ/2)zb(1)cos(ψ/2)+zb(2)sin(ψ/2)(1+zb(3))sin(ψ/2)
因此,姿态旋转矩阵是唯一确定的 R = R q u a t ( q ) R = R_quat(q) R=Rquat(q)
现在旋转是已知的
R ˙ = R w ^ \dot{R} = R\hat{w} R˙=Rw^
这意味着
w = ( R T R ˙ ) ∨ w = (R^T\dot{R})^\vee w=(RTR˙)
等效地
w = 2 ( q z ⊗ q ψ ) − 1 ( q ˙ z ⊗ q ψ + q z ⊗ q ˙ ψ ) w =2(q_z \otimes q_{\psi})^{-1}(\dot{q}_z \otimes q_{\psi}+q_z \otimes \dot{q}_{\psi}) w=2(qzqψ)1(q˙zqψ+qzq˙ψ)
代入倾斜旋转四元数和偏航旋转四元数给出
w = ( z ˙ b ( 1 ) s i n ( ψ ) − z ˙ ( 2 ) c o s ( ψ ) − z ˙ ( 3 ) ( z b ( 1 ) s i n ( ψ ) − z b ( 2 ) c o s ( ψ ) ) / ( 1 + z b ( 3 ) ) z ˙ b ( 1 ) c o s ( ψ ) − z ˙ ( 2 ) s i n ( ψ ) − z ˙ ( 3 ) ( z b ( 1 ) c o s ( ψ ) − z b ( 2 ) s i n ( ψ ) ) / ( 1 + z b ( 3 ) ) ( z b ( 2 ) z ˙ b ( 1 ) − z b ( 1 ) z ˙ b ( 2 ) ) / ( 1 + z b ( 3 ) ) + ψ , ) w = \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} w= z˙b(1)sin(ψ)z˙(2)cos(ψ)z˙(3)(zb(1)sin(ψ)zb(2)cos(ψ))/(1+zb(3))z˙b(1)cos(ψ)z˙(2)sin(ψ)z˙(3)(zb(1)cos(ψ)zb(2)sin(ψ))/(1+zb(3))(zb(2)z˙b(1)zb(1)z˙b(2))/(1+zb(3))+ψ, 
对机身Z轴进行微分得到
z ˙ b = d h m D N ( v ˙ + d h m σ ( ∣ ∣ V ∣ ∣ ) v + g e 3 ) T ( m d h v ¨ + σ ( ∣ ∣ v ∣ ∣ ) v ˙ + σ ˙ ( ∣ ∣ v ∣ ∣ ) v T v ˙ ∣ ∣ v ∣ ∣ v ) , D N ( x ) : = 1 x ( I − x x T x T x ) . \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}). z˙b=mdhDN(v˙+mdhσ(∣∣V∣∣)v+ge3)T(dhmv¨+σ(∣∣v∣∣)v˙+σ˙(∣∣v∣∣)∣∣v∣∣vTv˙v),DN(x):=x1(IxTxxxT).
现在我们已经用平面输出的有限导数表示了体轴速率。
现在所有的状态和输入都已知,除了扭矩。我们通过对体轴速率进行微分来得到它的表达式,使用 r ( 4 ) , ψ ( 2 ) r^{(4)},\psi^{(2)} r(4),ψ(2)
因此,
τ = M w ˙ + w × M w + A ( w ) + B ( R T v ) \tau = M\dot{w} +w\times Mw+A(w) +B(R^Tv) τ=Mw˙+w×Mw+A(w)+B(RTv)
image.png
最后,我们完成了平面输出变换的推导。规划高阶连续性的平面输出轨迹就足以满足动力学要求。
image.png
非线性动力学与反馈前馈控制中的微分平坦变换。
{ r ˙ = v , m v ˙ = − m g e 3 − R D R T σ ( ∣ ∣ v ∣ ∣ ) v + R f 3 3 , R ˙ = R w ^ , M w ˙ = τ − w × M w − A ( w ) − B ( R T v ) . \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˙=v,mv˙=mge3RDRTσ(∣∣v∣∣)v+Rf33,R˙=Rw^,Mw˙=τw×MwA(w)B(RTv).
微分平坦输出
z = r , ψ ∈ R 3 × S O ( 2 ) z = {r,\psi} \in \mathbb{R^3} \times SO(2) z=r,ψR3×SO(2)
在微分平坦输出空间中的轨迹
z ( t ) : [ 0 , T ] → R 3 × S O ( 2 ) z(t):[0,T] \rightarrow \mathbb{R^3} \times SO(2) z(t):[0,T]R3×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 ⟹ 轨迹参数化 x_t =p_5t^5+p_4x^4+p_3x^3+p_2x^2+p_1t+p_0 \Longrightarrow 轨迹参数化 xt=p5t5+p4x4+p3x3+p2x2+p1t+p0轨迹参数化
• 边界条件 没有中间条件

位置速度加速度
t=0a00
t=Tb00

求解:
[ a b 0 0 0 0 ] = [ 0 0 0 0 0 1 T 5 T 4 T 3 T 2 T 1 0 0 0 0 1 0 5 T 4 4 T 3 3 T 2 T 2 1 1 0 0 0 0 2 0 0 20 T 3 12 T 2 6 T 1 2 0 0 ] [ c 5 c 4 c 3 c 2 c 1 c 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} ab0000 = 0T505T4020T30T404T3012T20T303T206T10T20T21220T1100110000 c5c4c3c2c1c0
平滑的多段一维轨迹
• 每个段都是一个多项式。
• 不需要固定阶数,但保持相同的阶数可以使问题更简单。
• 每个段的时间持续必须是已知的!

位置速度加速度
t=0av00
t=TbvT0

求解:
[ a v 0 0 b v T 0 ] = [ 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 3 T 2 4 T 3 5 T 4 0 0 2 6 T 1 12 T 2 20 T 3 ] [ c 5 c 4 c 3 c 2 c 1 c 0 ] , d = A F ( T ) c f ( t ) = { f 1 ( t ) = ∑ i = 0 N p 1 , i t i T 0 ≤ t ≤ T 1 f 2 ( t ) = ∑ i = 0 N p 2 , i t i T 1 ≤ t ≤ T 2 ⋮ f M ( t ) = ∑ i = 0 N p M , i t i T M − 1 ≤ t ≤ T M \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} av00bvT0 = 100100010T10002T2T212000T33T26T1000T44T312T2000T55T420T3 c5c4c3c2c1c0 ,d=AF(T)cf(t)= f1(t)=i=0Np1,itif2(t)=i=0Np2,itifM(t)=i=0NpM,itiT0tT1T1tT2TM1tTM

• 边界条件:起始点,目标位置(方向)
• 中间条件:途中位置(方向)
• 途中位置可以通过路径规划(A*、RRT*等)找到,在前面的三文章中介绍过
• 平滑度标准,最小化“输入”变化率
image.png
约束条件:
• 导数约束: { f j ( k ) ( t j ) = x 0 ( j ) , f j ( k ) ( t j ) = x T ( j ) \begin{cases} \mathbf{f}_j^{(k)}(t_j) = \mathbf{x}_0^{(j)},\\ \mathbf{f}_j^{(k)}(t_j) = \mathbf{x}_T^{(j)} \end{cases} {fj(k)(tj)=x0(j),fj(k)(tj)=xT(j)
• 连续性约束: f j ( k ) ( t j ) = f j + 1 ( k ) ( t j ) \mathbf{f}_j^{(k)}(t_j) = \mathbf{f}_{j+1}^{(k)}(t_j) fj(k)(tj)=fj+1(k)(tj)

贝赛尔曲线表示

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

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

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

约束条件

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

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

如何确定轨迹的阶数?
• 在某个阶数上保证平滑性。• 在某个阶数上保证连续性。• 在某个阶数上最小化控制输入。
平滑性意味着其导数是连续的。为确保单段轨迹的平滑性的最小次数多项式:
最小 jerk:N = 2 * 3(jerk) - 1 = 5
最小 snap:N = 2 * 4(snap) - 1 = 7
为确保 k 段轨迹的平滑性的最小次数多项式:
image.png
最小 jerk:
约束数量:3 + 3 + (k-1) = k + 5
未知数数量:(N+1) * k
(𝑁 + 1) ∗ 𝑘 = 𝑘 + 5 𝑁 =5/k

Minimum Snap Trajectory Generation

Minimum Snap Trajectory Generation算法旨在最小化轨迹的“snap”,即轨迹的第四阶导数,以确保在给定的约束条件下生成最平滑的轨迹。
成本函数
image.png
机器人等系统的轨迹,以最小化由加速度、角加速度和其相应的高阶导数(“snap”)所定义的多项式轨迹的总体运动代价。在这种情况下,一个多项式段的成本函数通常定义为最小化 snap 的总和。Snap 是轨迹的第四阶导数,表示对加速度的变化率。成本函数的形式可能是对 snap 的积分或 snap 的平方和。在最小 snap 的情况下,典型的成本函数形式可能如下所示:
J = ∫ t 0 t f ( d 4 d t 4 x ( t ) ) 2 d t J = \int_{t_0}^{t_f} \left(\frac{d^4}{dt^4}x(t)\right)^2 dt J=t0tf(dt4d4x(t))2dt
这里, x ( t ) x(t) 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阶导数)
轨迹曲线求导如下,转成向量表示 x t = p 5 t 5 + p 4 x 4 + p 3 x 3 + p 2 x 2 + p 1 t + p 0 x ( 0 ) = … , x ( T ) = … x ˙ ( 0 ) = … , x ˙ ( T ) = … x ¨ ( 0 ) = … , x ¨ ( T ) = … ⋮ p 0 = … , p 5 t 5 + p 4 t 4 + p 3 t 3 + p 2 t 2 + p 1 t + p 0 = … [ T 5 , T 4 , T 3 , T 2 , T , 1 ] [ p 5 p 4 p 3 p 2 p 1 p 0 ] = … 把五次元曲线表示的求导向量矩阵带入多段曲线表示方程求解约束方程 f j ( k ) ( T j ) = x j ( k ) ⇒ ∑ i ≥ k i ! ( i − k ) ! T j l − k p j , i = x T , j ( k ) ⇒ [ … i ! ( i − k ) ! T j i − k … ] [ ⋮ p j , i ⋮ ] = x T , j ( k ) ⇒ [ … i ! ( i − k ) ! T j i − k … … i ! ( i − k ) ! T j i − k … ] [ ⋮ p j , i ⋮ ] = [ x 0 , j ( k ) x T , j ( k ) ] ⇒ A j p j = d j \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 轨迹曲线求导如下,转成向量表示xt=p5t5+p4x4+p3x3+p2x2+p1t+p0x(0)=,x(T)=x˙(0)=,x˙(T)=x¨(0)=,x¨(T)=p0=,p5t5+p4t4+p3t3+p2t2+p1t+p0=[T5,T4,T3,T2,T,1] p5p4p3p2p1p0 =把五次元曲线表示的求导向量矩阵带入多段曲线表示方程求解约束方程fj(k)(Tj)=xj(k)ik(ik)!i!Tjlkpj,i=xT,j(k)[(ik)!i!Tjik] pj,i =xT,j(k)[(ik)!i!Tjik(ik)!i!Tjik] pj,i =[x0,j(k)xT,j(k)]Ajpj=dj
两个段之间的连续性约束:
• 在未给定特定导数的情况下,确保轨迹段之间的连续性
f j ( k ) ( T j ) = f j = 1 ( k ) ( T j ) ⇒ ∑ i ≥ k i ! ( i − k ) ! T j i − k p j , i − ∑ i ≥ k l ! ( i − k ) ! T j l − k p j + 1 , l = 0 ⇒ [ … i ! ( i − k ) ! T j l − k … l ! ( l − k ) ! T j l − k … ] [ ⋮ p j , i ⋮ p j + 1 , l ⋮ ] = 0 ⇒ [ A j − A j + 1 ] [ P j p j + 1 ] = 0 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 fj(k)(Tj)=fj=1(k)(Tj)ik(ik)!i!Tjikpj,iik(ik)!l!Tjlkpj+1,l=0[(ik)!i!Tjlk(lk)!l!Tjlk] pj,ipj+1,l =0[AjAj+1][Pjpj+1]=0
有约束的二次规划(QP)表达式:
min ⁡ [ p 1 ⋮ p M ] ⊤ [ Q 1 0 0 0 ⋱ 0 0 0 Q M ] [ p 1 ⋮ p M ] s . t . A e q [ p 1 ⋮ p M ] = d e q \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} min p1pM Q1000000QM p1pM s.t.Aeq p1pM =deq

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

优化求解

决策变量映射

J = [ p 1 ⋮ p M ] T [ Q 1 0 ⋯ 0 0 ⋱ ⋱ ⋮ ⋮ ⋱ ⋱ 0 0 ⋯ 0 Q M ] [ p 1 ⋮ p M ] , M j P j = d j ⇓ ⇓ J = [ d 1 ⋮ d M ] T [ M 1 0 ⋯ 0 0 ⋱ ⋱ ⋮ ⋮ ⋱ ⋱ 0 0 ⋯ 0 M M ] − T [ Q 1 ⋯ 0 ⋮ ⋱ ⋮ 0 ⋯ Q M ] [ M 1 0 ⋯ 0 0 ⋱ ⋱ ⋮ ⋮ ⋱ ⋱ 0 0 ⋯ 0 M M ] − 1 [ d 1 ⋮ d M ] J = \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} J= p1pM T Q1000000QM p1pM MjPj=djJ= d1dM T M1000000MM T Q100QM M1000000MM 1 d1dM
其中多项式路径求导向量表示如下:
x ( t ) = p 5 t 5 + p 4 t 4 + p 3 t 3 + p 2 t 2 + p 1 t + p 0 x ′ ( t ) = 5 p 5 t 4 + 4 p 4 t 3 + 3 p 3 t 2 + 2 p 2 t + p 1 x ′ ′ ( t ) = 20 p 5 t 3 + 12 p 4 t 2 + 6 p 3 t + 2 p 2 M = [ 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 5 T 4 4 T 3 3 T 2 T 2 1 1 0 20 T 3 12 T 2 6 T 1 2 0 0 ] x(t) = 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} x(t)=p5t5+p4t4+p3t3+p2t2+p1t+p0x(t)=5p5t4+4p4t3+3p3t2+2p2t+p1x′′(t)=20p5t3+12p4t2+6p3t+2p2M= 000T55T420T3000T44T312T2000T33T26T1002T2T212010T10100100
• 直接优化多项式轨迹在数值上是不稳定的。
• 更倾向于采用一种变量变换,优化分段端点导数。
• 我们有 M j p j = d j M_j p_j = d_j Mjpj=dj,其中 M j M_j Mj 是将多项式系数映射到导数的映射矩阵。
其中, J J J是矩阵,包含了 d 1 d_1 d1 d M d_M dM的信息。 M j M_j Mj是将多项式系数映射到导数的映射矩阵, p j p_j pj是多项式系数向量, d j d_j dj是导数向量。

固定变量与自由变量分离

引入一个permutation matrix (置换矩阵)C来将连续性约束进行引入到代价函数中。C 矩阵本身是一个只包含0和1的矩阵,它的作用就是给所有 [ d 0 , d 1 ⋯ d M ] [ d_0,d_1 ⋯ d_M ] [d0,d1dM]进行一个组合,将固定的变量放在头部,将需要就行优化的变量方位尾部,并且对于连续性约束的变量,由于他们的值是相等的,因此选择其中一个来表达连续性约束。说起来有一些抽象,用数学表达就会清晰很多。
image.png
根据之前的介绍,紫色框中变量的值都是我们事先已经设置好的,浅蓝色框中的变量就是代价函数在优化时分配的最优值,也就是需要优化的变量。
将C带入代价函数,则最终得到
C T [ d F d P ] = [ d 1 ⋮ d M ] ⟹ J = [ d F d P ] T C M − T Q M − 1 C T [ R F F R F P R P F R P P ] [ d F d P ] \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} CT[dFdP]= d1dM J=[dFdP]TCMTQM1CT[RFFRPFRFPRPP][dFdP]
• 使用选择矩阵 C \mathbf{C} C来分离自由变量 ( d P ) ( \mathbf{d}_P ) dP和受约束变量 ( d F ) ( \mathbf{d}_F ) dF
• 自由变量:导数未指定,仅由连续性约束强制执行。

构建解选择矩阵

d 0 , j ( k ) d_{0,j}^{(k)} d0,j(k)就是对应的受约束元素,下面的图描述的就是上面
j是指代第几段曲线中的第几个点,k是几阶导数,0是指代第几段生成曲线
固定导数:固定起始点、目标状态和中间位置的导数
自由导数:中间连接点的所有导数
image.png
选择矩阵直接包括了起点和终点的各阶导数、中间点位置的固定约束信息,也隐式的包括了中间点各阶导数连续的约束条件。该选择矩阵,直接将原问题(带约束的QP问题)转化为无约束的QP问题,可直接求导求出。
其中,上式表示选择矩阵 C \mathbf{C} C与包含自由变量 d F \mathbf{d}_F dF和受约束变量 d P \mathbf{d}_P dP的列向量组合的矩阵相乘。
其中,上述方程中包含了选择矩阵 C \mathbf{C} C和映射矩阵 M \mathbf{M} M,用于分离自由变量和受约束变量。
最终将带约束的QP优化问题,转换成了一个无约束的QP问题,对于上式因为它是一个有全局最小值的凸函数,可以通过闭式求解所以对其求导取极值点就是它的全局最优值:
C M − T Q M − 1 C T = R \mathbf{C} \mathbf{M}^{-T} \mathbf{Q} \mathbf{M}^{-1} \mathbf{C}^T =R CMTQM1CT=R
并对R矩阵根据dF 和dP 的尺寸进行分块,则得到如下变换:
J = d F T R F F d F + d F T R F P d p + d P T R P F d F + d P T R P P d P 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=dFTRFFdF+dFTRFPdp+dPTRPFdF+dPTRPPdP
对于上式J是一个标量,并且Q矩阵是对称矩阵,而 C M − T Q M − 1 C T = ( C M − T Q M − 1 C T ) T \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 CMTQM1CT=(CMTQM1CT)T ,所以R也是对称矩阵,则 R P F = R F P T R_{PF} = R_{FP}^T RPF=RFPT
对于上式我们要求的变量是 d P d_P dP,因此将其对 求导, d P d_P dP并零导数为0时取极值点的值对应的就是最优的 d P ∗ d_P^* dP
∂ J ∂ d p = 2 d F T R F P + 2 R P P d p = 0 ⇒ d p ∗ = − R P P − 1 R F P T d F \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 dpJ=2dFTRFP+2RPPdp=0dp=RPP1RFPTdF
求解出来了 d P ∗ d_P^* dP之后,只需要将它带入C 和M就能求出最终的各个多项式的系数:
P = M − 1 C T [ d F d P ∗ ] P = M^{-1}C^T\begin{bmatrix} d_F \\ d_P^* \end{bmatrix} P=M1CT[dFdP]

轨迹生成

如何获得waypoints?

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

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

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

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

总体来说,选择适当的时间分配方法取决于具体的问题和应用场景。一些情况下简单的方法就足够了,而在复杂的环境中可能需要更高级的技术。
image.png
• 瞬时线性约束:
⎯ 起始点、目标状态约束 A p = b \mathbf{A}\mathbf{p} = \mathbf{b} Ap=b
⎯ 过渡点约束 A p = b , A p ≤ b \mathbf{A}\mathbf{p} = \mathbf{b}, \, \mathbf{A}\mathbf{p} \leq \mathbf{b} Ap=b,Apb
⎯ 连续性约束 A p i = A p i + 1 \mathbf{A}\mathbf{p}_i = \mathbf{A}\mathbf{p}_{i+1} Api=Api+1
• 区间线性约束:
⎯ 边界约束 A ( t ) p ≤ b , ∀ t ∈ [ t l , t r ] \mathbf{A}(t)\mathbf{p} \leq \mathbf{b}, \, \forall t \in [t_l, t_r] A(t)pb,t[tl,tr]
⎯ 动态约束 A ( t ) p ≤ b , ∀ t ∈ [ t l , t r ] \mathbf{A}(t)\mathbf{p} \leq \mathbf{b}, \, \forall t \in [t_l, t_r] A(t)pb,t[tl,tr]
o 速度约束
o 加速度约束

迭代加点的方法

如果发生碰撞,就在两个航迹点的中间再选一个航迹点,然后重新优化生成轨迹,重复这样的操作,直到生成安全的轨迹。如下图:
image.png
• 初始路径假设无碰撞。当计算时候路径上碰到障碍物,可以通过添加中间路径点重新生成这段的轨迹。完成的我们可以通过迭代将轨迹逼近到路径。

迭代加额外约束bounding box

先生成一条轨迹,检查极值,若极值超出bounding box,则施加一个bounding box往下压。
缺点:迭代求解非常耗时,假如是一个没有可行解的问题,可能要迭代很多次才能确定无解。
image.png
在离散时间点添加大量约束。
• 总是生成过于保守的轨迹。
• 太多的约束,计算负担很重。

构建安全飞行走廊

image.png
步骤 1. 检测障碍物
步骤 2. 搜索飞行走廊
步骤 3. 扩展飞行走廊
步骤 4. 生成完全位于飞行走廊内的动态可行轨迹
image.png
• 在单一片段中看起来可能不够智能。
• 在基于走廊的轨迹生成中效果良好。
• 走廊中的重叠区域提供了大的自动调整空间。
构建出可安全飞行的走廊,求解轨迹的方法和上面的一样。飞行走廊改变的是采样空间其实也就是对定义域层面做了优化来达到轨迹求解优化,在求解方程上没做什么变化。

小结:

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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/195939.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

2.4 API 开发和集成

文章目录 API 开发和集成API 的概念和作用API 开发基础API 集成API 鉴权和安全API 文档和测试微服务和 API 网关云服务和 API 集成未来趋势和发展实验实验一: 通过api post方式传入 json实验二:通过api将所需数据传入 API 开发和集成 API 的概念和作用介…

StarRocks上新,“One Data、All Analytics”还有多远?

K.K在《未来十二大趋势》中认为,我们正处于一个数据流动的时代。商业乃数据之商业。归根结底,你在处理的都是数据。 的确,当数据成为新的核心生产要素之际,数据分析就犹如最重要的生产工具之一,决定着企业在数字化时代…

Python爬虫超详细讲解(零基础入门,包教包会)

先看后赞,养成习惯。 点赞收藏,人生辉煌。 讲解我们的爬虫之前,先概述关于爬虫的简单概念(毕竟是零基础教程) 爬虫 网络爬虫(又被称为网页蜘蛛,网络机器人)就是模拟浏览器发送网络…

【Avue】select的远程搜索 [模糊搜索]

一、需求 【模糊搜索】 二、实现avue的远程搜索 1、search为搜索 2、remote远程搜索 3、dictValue{{key}}为输入的值

数实融合!低代码推动工业数字化转型走“深”向“实”

当下,“数字化、智能化”已经不再是新鲜词。毕竟,在早几年前就已经有企业喊出大举进军数字化的口号,轰轰烈烈的数字化转型运动也持续了很长一段时间,有一些业内人士甚至判断“如今的企业数字化已经走过了成熟期,来到了…

使用群晖Docker搭建HomeAssistant并实现异地公网访问家中智能设备

最近,我发现了一个超级强大的人工智能学习网站。它以通俗易懂的方式呈现复杂的概念,而且内容风趣幽默。我觉得它对大家可能会有所帮助,所以我在此分享。点击这里跳转到网站。 使用群晖Docker搭建HomeAssistant并实现异地公网访问 文章目录 使…

Shopee买家通系统内置防指纹技术可解决多账号管理操作

为了解决多账号管理的难题,我们发现了一款强大的利器——Shopee买家通系统,它为我们提供了便捷而高效的辅助操作。这款系统基于先进的指纹浏览器技术开发,实现了全自动化的操作,让多账号管理变得轻而易举。 Shopee买家通系统内置了…

element-ui upload组件中将file文件数据转成二进制流数据格式

方法一 handleBeforeUpload (file)const reader new FileReader()reader.readAsArrayBuffer(file)reader.onload async function (theFile) {const binary new Blob([theFile.target.result]) // 转成二进制流数据 即binary数据格式}}方法二 const aBlob new Blob([file],…

有关ADW400环保监测模块的详细介绍-安科瑞 蒋静

1 概述 ADW400 环保监测模块主要用于计量低压网络的三相有功电能,同时可选择最大四个回路的电流输入,具有 RS485 通讯和 470MHz 无线通讯功能,方便用户进行用电监测、集抄和管理。可灵活安装于配电箱内,实现对不 同区域和不同负荷…

深入探讨网络抓取:如何使用 Scala 和 Dispatch 获取 LinkedIn 图片

网络抓取是一种从互联网上获取数据的技术,它可以用于各种目的,例如数据分析、信息检索、竞争情报等。网络抓取的过程通常包括以下几个步骤: 发送 HTTP 请求到目标网站解析响应的 HTML 文档提取所需的数据存储或处理数据 在本文中&#xff0…

【1】基于多设计模式下的同步异步日志系统-项目介绍

1. 项目介绍 本项⽬主要实现⼀个日志系统, 其主要支持以下功能: • 支持多级别日志消息 • 支持同步日志和异步日志 • 支持可靠写⼊日志到控制台、文件以及滚动文件中 • 支持多线程程序并发写日志 • 支持扩展不同的日志落地⽬标地 2. 开发环境 • CentOS 7 • vs…

Python 类:探索面向对象编程的奇妙世界

在 Python 中,类是一种强大的工具,可以让你更有组织地编写代码,实现真正的面向对象编程。 本篇文章将详细介绍 Python 类的知识点和使用方法,通过通俗易懂的解释和使用案例,帮助大家轻松理解并掌握类的奥秘。 1、类和…

封装带插槽的表格

子组件 <template><div><table><thead><tr><th v-for"col,colIndex in columns" :key"colIndex">{{ col.title }}</th></tr></thead><tbody v-if"instList.length >0"><tr …

Altair 电子可靠性解决方案

原文链接&#xff1a;Altair 电子可靠性解决方案

fpga rom 初始化文件的一些心得

目录 可能遇到的问题 问题 解决方案 rom的初始化 用途 文件类型 如何生成初始化文件 示例 Altera Xilinx 可能遇到的问题 问题 altera FPGA的rom找不到初始化文件&#xff0c;编译过程会提示类似的问题 Error(127001): Cant find Memory Initialization File or He…

运行游戏找不到x3daudio1_7.dll怎么解决?教你如何快速修复的教程

在计算机使用过程中&#xff0c;我们经常会遇到一些错误提示&#xff0c;其中之一就是“x3daudio1_7.dll丢失”。这个错误提示可能让我们感到困惑和烦恼&#xff0c;但是不用担心&#xff0c;本文将为您介绍x3daudio1_7.dll丢失的原因以及五种修复方法&#xff0c;帮助您解决这…

Nginx(缓存机制)

对于性能优化而言&#xff0c;缓存是一种能够大幅度提升性能的方案&#xff0c;因此几乎可以在各处都能看见缓存&#xff0c;如客户端缓存、代理缓存、服务器缓存等等&#xff0c;Nginx的缓存则属于代理缓存的一种。对于整个系统而言&#xff0c;加入缓存带来的优势额外明显&am…

Redis Desktop Manager for Mac:高效管理Redis数据的必备工具

Redis是一种快速、可扩展的内存数据库&#xff0c;被广泛应用于缓存、消息队列和实时分析等领域。而Redis Desktop Manager for Mac作为一款专为Mac用户设计的Redis桌面管理工具&#xff0c;为用户提供了高效便捷的方式来管理和操作Redis数据。 首先&#xff0c;Redis Desktop…

【已解决】xxljob连接报错HTTP 302(HTTP 401账号或密码错误)

目录 问题现象&#xff1a; 问题分析&#xff1a; 1、密码中的特殊字符。 2、密码长度问题。 解决方法&#xff1a; 拓展&#xff1a; 问题现象&#xff1a; 今天在生产环境使用xxljob任务调度来创建并执行任务时&#xff0c;出现了程序报错&#xff1a; 通过查询xxljob日志…

降本增笑?滴滴史上最严重服务故障,裁员真不能裁测试

2023 年 11 月 27 日晚间&#xff0c;滴滴因系统故障导致 App 服务异常&#xff0c;不显示定位且无法打车。11 月 27 日晚&#xff0c;滴滴出行进行了回复&#xff1a;非常抱歉&#xff0c;由于系统故障。 2023 年 11 月 28 日早间&#xff0c;滴滴出行消息称&#xff0c;网约…