Towards Time-Optimal Tunnel-Following for Quadrotors---acados quadrotors_nav/common.py——2

6.8

0 GPT提问

def projFrenSerretBasis(s: Union[ca.MX, float]):
    ''' project to the Frenet Serret space
    <-- kap_impl : Curvature
    <-- tau_impl : torsion
    <-- dGamma_ds, d2Gamma_ds2, d3Gamma_ds3 : First 3 derivates of the curve w.r.t arc length
    <-- et_MX, en_MX, eb_MX : tangent , normal , binormal unit vectors'''

    InterOpts = {'degree': [5]}
    y_ref_MX = ca.interpolant("y_ref", "bspline", [s_ref], y_ref, InterOpts)
    z_ref_MX = ca.interpolant("z_ref", "bspline", [s_ref], z_ref, InterOpts)
    x_ref_MX = ca.interpolant("x_ref", "bspline", [s_ref], x_ref, InterOpts)


    [d2GammaX_ds2, dGammaX_ds] = ca.hessian(x_ref_MX(s), s)
    [d2GammaY_ds2, dGammaY_ds] = ca.hessian(y_ref_MX(s), s)
    [d2GammaZ_ds2, dGammaZ_ds] = ca.hessian(z_ref_MX(s), s)

    [d4GammaX_ds4, d3GammaX_ds3] = ca.hessian(d2GammaX_ds2, s)
    [d4GammaY_ds4, d3GammaY_ds3] = ca.hessian(d2GammaY_ds2, s)
    [d4GammaZ_ds4, d3GammaZ_ds3] = ca.hessian(d2GammaZ_ds2, s)

    dGamma_ds = ca.vertcat(dGammaX_ds, dGammaY_ds, dGammaZ_ds)
    d2Gamma_ds2 = ca.vertcat(d2GammaX_ds2, d2GammaY_ds2, d2GammaZ_ds2)
    d3Gamma_ds3 = ca.vertcat(d3GammaX_ds3, d3GammaY_ds3, d3GammaZ_ds3)
    d4Gamma_ds4 = ca.vertcat(d4GammaX_ds4, d4GammaY_ds4, d4GammaZ_ds4)

    kap = ca.norm_2(d2Gamma_ds2)
    kapBar_MX = ca.jacobian(kap, s)

    et_MX = dGamma_ds
    en_MX = d2Gamma_ds2/ kap
    eb_MX = ca.cross(et_MX, en_MX)

    etBar_MX = d2Gamma_ds2
    enBar_MX = (1/ kap**3) * (kap**2 * d3Gamma_ds3 - d2Gamma_ds2 @ d3Gamma_ds3.T @ d2Gamma_ds2)
    ebBar_MX = (1/ kap) * ca.cross(dGamma_ds, d3Gamma_ds3)
    tau_MX =  ca.dot(en_MX, ebBar_MX) 
    tauBar_MX =  ca.jacobian(tau_MX, s)

    return kap, tau_MX, et_MX, en_MX, eb_MX, kapBar_MX, tauBar_MX, etBar_MX, enBar_MX, ebBar_MX 我有一个问题，你能不能教教我这些公式是怎么推导的，并且告诉我这个 ca.jacobian是在干嘛

1 Gpt回答

一、Frenet–Serret 坐标系基本公式

对于一个以弧长$s$ 为参数的平滑曲线 $\gamma (s)$ ，其 Frenet–Serret 坐标系定义如下：

1. 切向量

   $\mathbf{t}(s)=\frac{d\gamma }{ds}$

   如果曲线以弧长参数化，则 $\Vert \mathbf{t}(s)\Vert =1$。

2. 曲率
   曲率描述了曲线弯曲的程度，定义为第二阶导数的范数：

   $\kappa (s)=\Vert \frac{d^2\gamma }{d{s}^2}\Vert$

   在程序中用变量 kap 表示。

3. 法向量
   法向量给出了曲线弯曲的方向，由二阶导数归一化得到：

   $\mathbf{n}(s)=\frac{\frac{d^2\gamma }{d{s}^2}}{\kappa (s)}$

   代码中用 en_MX = d2Gamma_ds2/kap 实现。

4. 副法向量
   副法向量通过切向量和法向量的叉积定义：

   $\mathbf{b}(s)=\mathbf{t}(s)\times \mathbf{n}(s)$

   代码中用 eb_MX = ca.cross(et_MX, en_MX) 实现。

5. Frenet–Serret 公式
   这些单位向量满足以下微分关系：

   $\begin{array}{rl}{\mathbf{t}}^{\prime }(s)& =\kappa (s)\mathbf{n}(s),\\ {\mathbf{n}}^{\prime }(s)& =-\kappa (s)\mathbf{t}(s)+\tau (s)\mathbf{b}(s),\\ {\mathbf{b}}^{\prime }(s)& =-\tau (s)\mathbf{n}(s),\end{array}$

   其中 $\tau (s)$ 是扭率（torsion），描述曲线如何扭转。

---

二、代码中公式的对应与推导

1. 计算导数

程序通过 ca.hessian 求解 B 样条插值得到的函数的导数。比如：

[d2GammaX_ds2, dGammaX_ds] = ca.hessian(x_ref_MX(s), s)

• 一阶导数$\frac{d{\gamma }_x}{ds}$ 用变量 dGammaX_ds 表示；
• 二阶导数$\frac{d^2{\gamma }_x}{d{s}^2}$ 用变量 d2GammaX_ds2 表示。

对 $x$、$y$ 和 $z$ 分量分别计算，然后用 ca.vertcat 将它们组合成向量：

$\frac{d\gamma }{ds}=\left[\begin{array}{c}\frac{d{\gamma }_x}{ds}\\ \frac{d{\gamma }_y}{ds}\\ \frac{d{\gamma }_z}{ds}\end{array}\right]\text{(代码中为 }dGamma\_ds\text{)}$

$\frac{d^2\gamma }{d{s}^2}=\left[\begin{array}{c}\frac{d^2{\gamma }_x}{d{s}^2}\\ \frac{d^2{\gamma }_y}{d{s}^2}\\ \frac{d^2{\gamma }_z}{d{s}^2}\end{array}\right]\text{(代码中为 }d2Gamma\_ds2\text{)}$

同样地，三阶和四阶导数也被计算出来，记为 d3Gamma_ds3 和 d4Gamma_ds4。

2. 曲率和曲率导数

• 曲率
  代码中：

  kap = ca.norm_2(d2Gamma_ds2)
  

  对应数学公式：

  $\kappa =\Vert \frac{d^2\gamma }{d{s}^2}\Vert$

• 曲率的导数
  代码中：

  kapBar_MX = ca.jacobian(kap, s)
  

  这表示求$\kappa$ 关于$s$ 的导数：

  $\dot{\kappa }=\frac{d\kappa }{ds}$

  这里 ca.jacobian 用于计算函数$\kappa (s)$ 对$s$ 的雅可比（即导数）。

3. 单位向量

• 切向量
  代码中：

  et_MX = dGamma_ds
  

  对应公式：

  $\mathbf{t}(s)=\frac{d\gamma }{ds}$

  注意：如果路径已经以弧长参数化，则 $\Vert \mathbf{t}(s)\Vert =1$。

• 法向量
  代码中：

  en_MX = d2Gamma_ds2 / kap
  

  对应公式：

  $\mathbf{n}(s)=\frac{{\gamma }^{\prime \prime }(s)}{\kappa (s)}$

• 副法向量
  代码中：

  eb_MX = ca.cross(et_MX, en_MX)
  

  对应公式：

  $\mathbf{b}(s)=\mathbf{t}(s)\times \mathbf{n}(s)$

4. 单位向量的导数

代码中带有 Bar 的变量代表的是对$s$ 求导后的结果：

• 切向量的导数

  etBar_MX = d2Gamma_ds2
  

  对应公式（由于$\mathbf{t}={\gamma }^{\prime }(s)$）：

  $\dot{\mathbf{t}}(s)={\gamma }^{\prime \prime }(s)$

  在 Frenet–Serret 公式中应满足 $\dot{\mathbf{t}}=\kappa \mathbf{n}$，这与${\gamma }^{\prime \prime }(s)=\kappa \mathbf{n}$ 一致。

• 法向量的导数

enBar_MX = (1/ kap**3) * (kap**2 * d3Gamma_ds3 - d2Gamma_ds2 @ d3Gamma_ds3.T @ d2Gamma_ds2)

该公式是从

$\mathbf{n}=\frac{{\gamma }^{\prime \prime }(s)}{\kappa (s)}$

对$s$ 求导后，通过商法则推导得到的结果。推导过程涉及到${\gamma }^{\prime \prime \prime }(s)$ 和$\dot{\kappa }$ 的结合。

• 副法向量的导数

   ebBar_MX = (1/ kap) * ca.cross(dGamma_ds, d3Gamma_ds3)
  

  对应公式为对$\mathbf{b}=\mathbf{t}\times \mathbf{n}$ 求导，结果中包含${\gamma }^{\prime }(s)$ 与${\gamma }^{\prime \prime \prime }(s)$ 的叉积，再除以曲率$\kappa$。

5. 扭率及其导数

• 扭率

  tau_MX = ca.dot(en_MX, ebBar_MX)
  

  对应公式：

  $-\tau (s)\mathbf{n}(s)=\dot{\mathbf{b}}(s)$

  其中 $\dot{\mathbf{b}}(s)$ 就是副法向量的导数，负号表示$\mathbf{n}$的方向和$\frac{d\mathbf{b}}{ds}$方向相反，$\tau$表示标量，只是大小

• 扭率的导数

  tauBar_MX = ca.jacobian(tau_MX, s)
  

  表示求扭率$\tau (s)$ 关于$s$ 的导数：

  $\dot{\tau }=\frac{d\tau }{ds}$

---

三、ca.jacobian 的作用

• ca.jacobian 是 CasADi 的一个函数，用于计算一个函数对给定变量的雅可比矩阵。

• 对于一个标量函数$f(s)$，其雅可比矩阵就是该函数对$s$ 的导数，即：

  $\frac{df}{ds}$

• 在本代码中：

  • kapBar_MX = ca.jacobian(kap, s) 计算的是曲率$\kappa (s)$ 关于$s$ 的导数$\frac{d\kappa }{ds}$。
  • tauBar_MX = ca.jacobian(tau_MX, s) 计算的是扭率$\tau (s)$ 关于$s$ 的导数$\frac{d\tau }{ds}$。

这样，我们就能够获得曲率和扭率变化的速率，这在动态路径跟踪和控制中是非常有用的信息。

---

四、总结

1. 路径的导数

   • 一阶导数：${\gamma }^{\prime }(s)$（切向量 $\mathbf{t}$）
   • 二阶导数：${\gamma }^{\prime \prime }(s)$ 用于计算曲率 $\kappa =\Vert {\gamma }^{\prime \prime }(s)\Vert$
   • 三阶导数：${\gamma }^{\prime \prime \prime }(s)$ 用于计算法向量和导数

2. Frenet-Serret 基

   • 切向量：$\mathbf{t}={\gamma }^{\prime }(s)$
   • 法向量：$\mathbf{n}=\frac{{\gamma }^{\prime \prime }(s)}{\kappa }$
   • 副法向量：$\mathbf{b}=\mathbf{t}\times \mathbf{n}$

3. 导数及其雅可比

   • 曲率的导数：$\dot{\kappa }=\frac{d\kappa }{ds}$ 由 ca.jacobian(kap, s) 实现
   • 扭率：$-\mathbf{n}\tau =\dot{\mathbf{b}}$ 。负号表示$\mathbf{n}$的方向和$\frac{d\mathbf{b}}{ds}$方向相反，$\tau$表示标量，只是大小
   • 扭率的导数：$\dot{\tau }=\frac{d\tau }{ds}$ 由 ca.jacobian(tau_MX, s) 实现

4. ca.jacobian

   • 用于计算函数相对于某变量的偏导数或导数矩阵。在本例中，它用于求曲率和扭率关于进度$s$ 的导数。

这些公式与代码直接对应，带有 “Bar” 的变量表示对原始量关于$s$ 求导得到的结果。希望这些 LaTeX 公式和解释能帮助你更好地理解代码的数学推导过程。

6.9

0 GPT提问

扭率 为什么是tau_MX = ca.dot(en_MX, ebBar_MX)，
是不是少了个负号，但是程序里面没有负号。
还有就是副法向量的导数是怎么求的
ebBar_MX = (1/ kap) * ca.cross(dGamma_ds, d3Gamma_ds3)

1 GPT回答

下面我来详细说明这段代码中扭率（$\tau$）和副法向量导数的推导思路，以及 ca.jacobian 的作用。

---

一、扭率的定义与符号约定

在经典的 Frenet–Serret 公式中，扭率通常定义：

1. 定义为

   $-\mathbf{n}\tau =\frac{d\mathbf{b}}{ds}$，负号表示$\mathbf{n}$的方向和$\frac{d\mathbf{b}}{ds}$方向相反，$\tau$表示标量，只是大小

---

二、副法向量的导数推导

给定 Frenet–Serret 基的定义：

• 切向量：

  $\mathbf{t}=\frac{d\gamma }{ds}$

• 法向量：

  $\mathbf{n}=\frac{{\gamma }^{\prime \prime }(s)}{\kappa },\kappa =\Vert {\gamma }^{\prime \prime }(s)\Vert$

• 副法向量：

  $\mathbf{b}=\mathbf{t}\times \mathbf{n}$

对副法向量求导，根据叉积的导数法则：

$\frac{d\mathbf{b}}{ds}=\frac{d\mathbf{t}}{ds}\times \mathbf{n}+\mathbf{t}\times \frac{d\mathbf{n}}{ds}$

根据 Frenet–Serret 公式：

$\frac{d\mathbf{t}}{ds}=\kappa \mathbf{n},\frac{d\mathbf{n}}{ds}=-\kappa \mathbf{t}+\tau \mathbf{b}$

代入上式：
$\begin{array}{rl}\frac{d\mathbf{b}}{ds}& =(\kappa \mathbf{n})\times \mathbf{n}+\mathbf{t}\times (-\kappa \mathbf{t}+\tau \mathbf{b})\\ & =0+\tau (\mathbf{t}\times \mathbf{b})\\ & =\tau (\mathbf{t}\times \mathbf{b})\end{array}$

而由于 $\mathbf{t}\times \mathbf{b}=-\mathbf{n}$（遵循右手定则），故有：

$\frac{d\mathbf{b}}{ds}=-\tau \mathbf{n}$
这个公式里面的负号是个方向，负号表示$\mathbf{n}$的方向和$\frac{d\mathbf{b}}{ds}$方向相反，$\tau$表示标量，只是大小

代码中是这样写的，为了后续用到，只用大小，不用方向。

tau_MX = ca.dot(en_MX, ebBar_MX)

tau的推导

来源tau_MX

来源tau_MX

---

三、副法向量导数的计算

代码中计算副法向量导数的部分为：

ebBar_MX = (1/ kap) * ca.cross(dGamma_ds, d3Gamma_ds3)

解释如下：

• $d\Gamma /ds$ 对应 $\mathbf{t}$（切向量），即 dGamma_ds。
• $d^3\gamma /d{s}^3$ 对应 ${\gamma }^{\prime \prime \prime }(s)$，即 d3Gamma_ds3。

在弧长参数化的情况下，副法向量的导数可以推导为：

$\frac{d\mathbf{b}}{ds}=\frac{1}{\kappa }\left(\mathbf{t}\times {\gamma }^{\prime \prime \prime }(s)\right)$

这就是代码中用的公式：

$\text{ebBar\_MX}=\frac{1}{\kappa }\text{ca.cross}(dGamma\_ds,d3Gamma\_ds3)$

这个公式的推导基于对 $\mathbf{b}=\mathbf{t}\times \mathbf{n}$ 求导，并利用 $\mathbf{n}=\frac{{\gamma }^{\prime \prime }(s)}{\kappa }$ 以及 $\mathbf{t}={\gamma }^{\prime }(s)$ 得到。

---

四、总结

1. 关于扭率：

   • 经典 Frenet–Serret 公式中常见的定义为 $\tau =-\mathbf{n}\cdot {\mathbf{b}}^{\prime }$，但也有定义为 $\tau =\mathbf{n}\cdot {\mathbf{b}}^{\prime }$ 的约定。代码中使用的是后一种定义，因此没有负号。

2. 关于副法向量导数：

   • 副法向量 $\mathbf{b}$ 定义为 $\mathbf{b}=\mathbf{t}\times \mathbf{n}$。
   • 其导数 $\frac{d\mathbf{b}}{ds}$ 推导可得：
     $\frac{d\mathbf{b}}{ds}=\frac{1}{\kappa }\left(\mathbf{t}\times {\gamma }^{\prime \prime \prime }(s)\right)$
   • 代码中写为：

     ebBar_MX = (1/ kap) * ca.cross(dGamma_ds, d3Gamma_ds3)
     

3. ca.jacobian：

   • ca.jacobian(f, s) 用于计算函数 $f(s)$ 关于变量 $s$ 的雅可比矩阵，即对 $s$ 求导。代码中用于计算曲率和扭率的导数（如 kapBar_MX 和 tauBar_MX）。

只要整个模型中保持一致的符号约定，即使与某些文献中有负号差异，也是可以接受的。关键在于系统内各个部分的定义和使用是一致的。

希望这能帮助你理解这些公式的推导思路以及代码中 ca.jacobian 的作用！