【自动驾驶】决策规划算法 | 数学基础（三）直角坐标与自然坐标转换Ⅰ

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！

📚 系列专栏：【决策规划】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

引言

  各位小伙伴们大家好，本篇博客是自动驾驶决策规划算法数学基础的第三节第Ⅰ部分，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  本篇博客讲数学基础部分中 Frenet 坐标系和笛卡坐标系之间的坐标转换，即直角坐标和自然坐标的转换。

  本节内容如果只应用，难度其实还好，但如果想真彻底理解它是怎么来，难度非常高。需要非常熟悉微积分以及向量微积分。

一、龙格现象与多项式拟合

1.1 龙格现象概述

  讲解坐标转换之前，首先讲一下龙格现象，是数值分析里的知识，用高次多项式拟合点，可能出现震荡现象，所以要慎用高次多项式。

  对于较多点的拟合，尽可能用分段低次多项式拟合，而不用高次多项式拟合。

1.2 高次多项式拟合的弊端

  一般认为如果多项式次数越高，拟合精度越好，这是想当然的想法，在有些问题确实是这样，比如拟合 $\sin x,\cos x$ 这样的三角函数，次数越高拟合越精确。

  但在有些情况并不是这样，比如在拟合 $y=25x^2+\frac{1}{e}$ 时，用高次多项式拟合会出现震荡。

  在拟合时尽可能不用高次多项式，高次多项式很可能出现震荡现象，所以一般对大量点进行拟合时，用分段一次多样式拟合，而不用高次多项式。

二、Frenet 坐标系的作用

2.1 自然坐标系的优势

为什么要把笛卡尔坐标转化为自然坐标？

  因为实际上道路它都是千变万化的，有不同的曲率和长短。如果只在直角坐标下分析问题，就意味着不同的道路形状、不同道路的长短都要分开考虑，非常麻烦。

  如果以道路中心线为曲线坐标系建立自然坐标，只需要关注怎么进行坐标转换可以，只要把直角坐标转换为自然坐标，无论是什么道路、什么路况，都可以用同一套方法解决问题，也就是只要做把直角坐标转化为自然坐标，解决问题后，再把自然坐标转化为直角坐标。极大简化分析问题的难度。不同道路都可以转化成同样坐标系分析，再加坐标转换就可以了。

  关于转换公式，可以参考以下博客：
  Frenet坐标系与Cartesian坐标系互转（一）：公式推导

  上面的博客给出了笛卡尔坐标转Frenet坐标和Frenet坐标转笛卡尔坐标，直接会用公式就可以了。

  下面使用向量法推导，可降低推导难度，不用上面博客的推导方法也能得出一样的结果。

2.2 坐标转换涉及的变量及其关系

  首先建立直角坐标系：

  第一条曲线是车辆轨迹，第二条曲线是道路几何，在车辆轨迹曲线取一点，有速度$v$，加速度 $a$，位置矢量 $\vec{r}$，以及在直角坐标系下的曲率 $\kappa$。

  无人驾驶车一般用 Host Vehicle 表示，已知车在笛卡尔标系下的 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,{\kappa }_h$，求车在以道路为坐标轴的Frenet坐标系下的坐标$s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime },\dot{l},\ddot{l}$，其中 $\dot{s}=\frac{ds}{dt},\dot{l}=\frac{dl}{dt},\dot{l}=\frac{dl}{ds}$。

共有 $8$ 个坐标要求，但一般实际上用不到这 $8$ 个变量。一般只用 $6$ 个：

• $s,\dot{s},\ddot{s},l,\dot{l},\ddot{l}$
• $s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime }$

到底用哪 $6$ 个变量，取决于规划方法是什么：

• 对于 EM Planner ，求$s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime }$
• 对于 Lattice Planner，求$s,\dot{s},\ddot{s},l,\dot{l},\ddot{l}$

  虽然变量有 $8$ 个，但独立变量只有 $6$ 个，因为$l^{\prime },l^{\prime \prime }$ 和 $\dot{l},\ddot{l}$ 可以互相转化：
$$\dot{l}=\frac{dl}{dt}=\frac{dl}{ds}\cdot \frac{ds}{dt}=l^{\prime }\dot{s}$$$$\begin{array}{rl}\ddot{l}& =\frac{\text{d}\dot{l}}{\text{d}t}=\frac{\text{d}\left(l^{\prime }\dot{s}\right)}{\text{d}t}=\frac{\text{d}{l}^{\prime }}{\text{d}t}\dot{s}+l^{\prime }\cdot \frac{\text{d}\dot{s}}{\text{d}t}\\ & =\frac{\text{d}{l}^{\prime }}{\text{d}s}\frac{\text{d}s}{\text{d}t}\dot{s}+l^{\prime }\frac{\text{d}\dot{s}}{\text{d}t}=l^{\prime \prime }{\dot{s}}^2+l^{\prime }\ddot{s}\end{array}$$   EM planner 采用的是$s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime }$。

三、曲线坐标系的特点

3.1 曲线坐标系与直角坐标系的主要区别

曲线坐标系与直角坐标系有两点不同：

• 曲线坐标系的基向量一般不是常向量
• 点的曲线坐标变化与点的实际位移一般不一致

在直角坐标下：

  基向量是 $i$ 和 $j$，显然是常向量$\frac{di}{dx}=\vec{0}$

  但在曲线坐标系：

  基向量为 $\vec{\tau }$， $\vec{\tau }$的大小它不变，但 $\vec{\tau }$ 的方向会随 $s$ 变化而变化，所以$\frac{d\tau }{ds}\ne 0$

3.2 基向量非常数对向量求导的影响

基向量对坐标的导数不为 $0$ 有什么影响？

  最大的影响就在于对向量求导，比如在直角坐标下，如果向量 $v=v_{x}i+v_{y}j$ 对向量 $v$ 求导，只要把坐标去掉就可以，变成
$$\dot{v}={\dot{v}}_{x}i+{\dot{v}}_{y}j$$  但如果 在曲线坐标系下， $\dot{v}={\dot{v}}_x\vec{\tau }+{\dot{v}}_y\vec{n}$，对向导求导不仅要对 $v_x,v_y$ 求导，还要对$\vec{\tau },\vec{n}$ 求导，因为$\vec{\tau },\vec{n}$ 不是常向量，即向量导数不为 $0$，这是和直角坐标系相比最大的区别。

  在曲线坐标系下，对向量求导一般比较复杂，不像在直角坐标下，只要对 $v_x,v_y$ 求导就可以了。

3.3 直角坐标系与自然坐标系中位移描述的差异

  另外，曲线坐标系描述起来比较复杂，举个例子，比如直角坐标系：

  直角坐标系下有点 $(x,y)$，让 $y$ 不动，$x$ 移动 $\Delta x$。在 $y$ 不动的情况下，点的实际位移和坐标的位移一样，点移动了 $\Delta x$ 的距离，坐标也从 $x$ 移到了 $x+\Delta x$ 也是 $\Delta x$ 的距离，在直角坐标系下，如果 $y$ 不动，$x$ 坐标的移动距离和点实际移动的距离一样，都是 $\Delta x$。

  但在自然坐标系：

  同样点 $(s,l)$，让 $l$ 不动，$s$ 坐标移动 $\Delta s$，点也会移动弧长 $\Delta s^{\prime }$。 $\Delta s$ 与 $\Delta s^{\prime }$ 一般不相等。

  这一结论最反直觉，也是对初学者来说是最别扭的结论。自然坐标系和直角坐标系项最大的不同就是在直角坐标系下只有 $\frac{dy}{dx}$，而在曲线坐标系下有 $\frac{d}{ds}$，还有$\frac{d}{d{s}_x}$。

  在直角坐标系下，如果对函数求导，只有 $dx$， $dx$ 不区分 $dx$ 到底是实际点的位移，还是在 $x$ 坐标方向上分量的位移。

但在自然坐标系下，因为 $\Delta s$ 与 $\Delta s^{\prime }$ 一般不相等，所以它们的微分 $ds$ 和 $d{s}^{\prime }$ 一般也不一样，所以在自然坐标系下，如果想求变量在 $ds$ 方向上的导数时，必须要指出弧微分 $ds$ 到底是哪个曲线的弧长。

  总结：在直角坐标系下只有 $dx$，在自然坐标系下可能有多个 $ds$。

3.4 实例分析：车辆轨迹与道路几何中的坐标位移差异

  举个例子说明：

  比如短弧线是车辆轨迹，长弧线是道路几何，假设车沿道路平行移动，即车的 $l$ 不动。

  车原来在红色点的位置，经过 $dy$ 的时间，车跑到蓝色点的位置，同样它在坐标轴上的投影也跑到蓝色点处，就有两个 $ds$：

• 车速 $\frac{d{s}_x}{dt}=|\vec{v}|$。
• 投影速度 $\frac{ds}{dt}=\dot{s}$。

  上面算出来的是 $v$ 的大小，不带方向，一般情况下 $|\vec{v}|$ 不等于 $\dot{s}$，因为 $ds$ 和$d{s}_x$ 不一样，所以它们的速度不一样。

四、预备知识1：质点速度的向量表达式与道路几何投影导数推导

4.1 质点速度的向量表达式

  比如在直角坐标系下质点的轨迹是这样：

  证明 $\vec{\dot{r}}=|\vec{v}|\vec{\tau }$， $\vec{\tau }$ 是质点在切线方向上的单位向量，记$|\vec{v}|\vec{\tau }$为$\vec{v}$。

  假设经过 $dt$ 时间， $\vec{r}$ 位置变成了 $r+dr$ ，它划过的弧长为 $ds$。根据向量加减法，紫红色向量为 $dr$，位矢对时间的导数$$\vec{\dot{r}}=\frac{d\vec{r}}{dt}=\frac{d\vec{r}}{ds}\cdot \frac{ds}{dt}$$  当 $dt\to 0$ 时，$\frac{|d\vec{r}|}{ds}\to 1$，方向趋于轨迹在 $r$ 的切线方向 $\vec{\tau }$，所以：
$$\vec{\dot{r}}=1\cdot \vec{\tau }\cdot \frac{ds}{dt}=|\vec{v}|\vec{\tau }=\vec{v}$$

4.2 拓展：质点在道路几何上的投影位矢导数推导

  比如在直角坐标系下有两条曲线：质点轨迹和道路几何

  比如在质点轨迹曲线上有质点，位置为 ${\vec{r}}_h$，质点在道路几何上的投影位矢记为 ${\vec{r}}_r$，根据上面的推导 ${\vec{\dot{r}}}_h=|\vec{v}|\vec{\tau }=\vec{v}$，${\vec{\tau }}_h$ 是质点在轨迹上的切线方向， ${\vec{\tau }}_r$ 是投影在道路几何上的切线方向，投影位矢的导数为：

$${\vec{\dot{r}}}_r=\frac{\text{d}{\vec{r}}_r}{\text{d}t}=\frac{\text{d}{\vec{r}}_r}{\text{d}{s}_r}\cdot \frac{\text{d}{s}_r}{\text{d}t}=\dot{s}{\vec{\tau }}_r$$  如果道路是 Frenet 坐标系的坐标轴，则$\frac{\text{d}{s}_r}{\text{d}t}=\dot{s}$。在自然坐标系下有不同的 $ds$，但在直角坐标系下只有 $dx$。

4.3 质点投影点的定义

  如果点的切线方向和点与质点的连线方向垂直：

  称为质点的投影点

五、预备知识2：Frenet公式

5.1 曲线坐标系基向量特性

  在曲线坐标系下，$\frac{d\vec{\tau }}{ds}$ 一般不为零向量。

问题来了，$\frac{d\vec{\tau }}{ds}$ 到底等于什么呢？包括它的法向量$\frac{d\vec{n}}{ds}$等于什么呢？

5.2 Frenet公式及其证明

  Frenet公式给出了结果：
$$\frac{d\vec{t}}{ds}=\kappa \vec{n}\frac{d\vec{n}}{ds}=-\kappa \vec{\tau }$$  其中，$ds$ 为所画曲线的弧微分。

  证明：假设 $\tau$ 经过 $ds$ 变成了 $\tau +d\tau$，因为 $\tau$ 和$\tau +d\tau$ 是单位向量，所以模都是 $1$。

  几何关系如下：

  夹角为 $d\theta$，三角形是等腰三角形，根据几何关系，$d\vec{\tau }$ 的长度为：
$$|d\vec{\tau }|=2\cdot 1\cdot \sin \left(\frac{d\theta }{2}\right)$$  当 $ds$ 趋于 $0$ 时，$d\vec{\tau }$ 的方向趋于 $\vec{\tau }$ 的垂直方向 $\vec{n}$，大小为
$$\frac{|d\stackrel{\rightharpoonup }{\tau }|}{ds}=\frac{2\sin \frac{d\theta }{2}}{ds}=\frac{d\theta }{ds}=\kappa$$  其中，$\frac{d\theta }{ds}$ 就是曲率的定义，所以
$$\frac{d\vec{\tau }}{ds}=\kappa \vec{n}$$  同理
$$\frac{d\vec{n}}{ds}=-\kappa \vec{\tau }$$  证明方式和上面一样。

5.3 拓展1：质点轨迹与道路几何的方向导数

  假设上面弧曲线是质点轨迹，下面是道路几何，有质点的位矢以及质点的投影位矢。记质点的切线方向是 ${\vec{\tau }}_h$，它的投影切线方向是 ${\vec{\tau }}_r$，${\vec{\tau }}_h$ 对时间的导数：
$${\vec{\dot{\tau }}}_h=\frac{d{\vec{\tau }}_h}{dt}=\frac{d{\vec{\tau }}_h}{d{s}_h}\cdot \frac{d{s}_h}{dt}={\kappa }_h{\vec{n}}_h|\vec{v}|$$   其中，${\kappa }_h$ 为质点所在轨迹的曲率。

  同理，质点法向量的导数为：
$${\vec{\dot{n}}}_h=-\kappa {\vec{\tau }}_h\cdot |\vec{v}|=-{\kappa }_h|\vec{v}|{\vec{\tau }}_h$$  同理，投影点的切向量和法向量对时间的导数分别为：
$$\begin{array}{rl}{\vec{\dot{\tau }}}_r& =\kappa \dot{s}{\vec{n}}_r\\ {\vec{\dot{n}}}_r& =-\kappa \dot{s}{\vec{\tau }}_r\end{array}$$  这四个公式是坐标转换的核心公式。

  坐标转换有$l,l^{\prime },l^{\prime \prime }$，其中，$l^{\prime }=\frac{dl}{ds}$ 。$ds$ 指的是 Frenet 坐标系下坐标轴曲线的弧微分。

  注意：上述四个核心公式中的曲率，无论是 ${\kappa }_h$ 还是${\kappa }_r$，都是指在直角坐标系下的曲率。因为自然坐标系下也有曲线及其曲率，自然坐标系下的曲率和直角坐标系下的曲率不一样。

5.4 拓展2：切向加速度与法向加速度的分解

  同样在直角坐标系下有轨迹和质点：

  已知 $\vec{r},\vec{\tau },\vec{n},\kappa$，求 $\vec{v}$：
$$\vec{v}=\vec{\dot{r}}=|\vec{v}|\vec{\tau }$$  求 $\vec{a}$，利用预备知识的拓展1：$\vec{\dot{\tau }}=\kappa |\vec{v}|\vec{n}$

$$\begin{array}{rl}\vec{a}& =\frac{d\vec{v}}{dt}=\frac{d|\vec{v}|\vec{\tau }}{dt}=\frac{d|\vec{v}|}{dt}\vec{\tau }+|\vec{v}|\frac{d\vec{\tau }}{dt}\\ & =|\vec{\dot{v}}|\vec{\tau }+|\vec{v}|\vec{\dot{\tau }}\\ & =|\vec{\dot{v}}|\vec{\tau }+|\vec{v}{|}^2\kappa \vec{n}\\ & =|\vec{\dot{v}}|\vec{\tau }+\frac{|\vec{v}{|}^2}{\rho }\vec{n}\end{array}$$  其中，曲率半径 $\rho =\frac{1}{\kappa }$，在本例中为负值，$|\vec{\dot{v}}|\vec{\tau }$ 为切向加速度，$\frac{|\vec{v}{|}^2}{\rho }\vec{n}$ 为法向加速度，即向心加速度。

六、预备知识总结

  这些公式是以后推导坐标转换时非常有用的辅助公式。

6.1 变量含义

  以下变量都以笛卡尔坐标为基准

6.2 辅助公式

  $7$ 个辅助公式是求自然坐标和直角坐标之间转化的关键：

$$\begin{array}{rl}{\vec{\dot{r}}}_h& =|\vec{v}|{\vec{\tau }}_h\\ {\vec{\dot{r}}}_r& =\dot{s}{\vec{\tau }}_r\\ {\vec{\dot{\tau }}}_h& ={\kappa }_h|\vec{v}|{\vec{n}}_h\\ {\vec{\dot{n}}}_h& =-{\kappa }_h|\vec{v}|{\vec{\tau }}_h\\ {\vec{\dot{\tau }}}_r& ={\kappa }_r\dot{s}{\vec{n}}_r\\ {\vec{\dot{n}}}_r& =-{\kappa }_r\dot{s}{\vec{\tau }}_r\\ \vec{a}& =|\vec{\dot{v}}|{\vec{\tau }}_h+|\vec{v}{|}^2{\kappa }_h{\vec{n}}_h\end{array}$$  有了这 $7$ 个辅助公式，后面一切推导都水到渠成，就是微积分的计算。

七、坐标转换算法

7.1 问题描述

  问题非常明晰了：

  已知笛卡尔坐标下的 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,{\kappa }_h,{\vec{\tau }}_h,{\vec{n}}_h$

  已知 Frenet 坐标系下的起点 $(x_0,y_0)$

  求 Frenet 坐标系下的 $s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime }$。其中，$\dot{s}=\frac{ds}{dt}$，$l^{\prime }=\frac{dl}{ds}$，$ds$ 为 Frenet 坐标轴的弧长。

7.2 坐标转换算法步骤概述

  算法分三步：

  第一步：$7$ 个辅助公式

  第二步：找到车在Frenet坐标系下的投影点在笛卡尔坐标系下的坐标，记为$x_r,y_r,{\theta }_r,k_r$，

  计算
$${\vec{r}}_r=\left(x_r,y_r\right){\vec{\tau }}_r=\left(\cos {\theta }_r,\sin {\theta }_r\right){\vec{n}}_r=\left(-\sin {\theta }_r,\cos {\theta }_r\right)$$  其中，${\theta }_r$ 代表投影点的切线方向 ${\vec{\tau }}_r$ 与 $x$ 轴的夹角，如下图所示：

  第三步：利用向量三角形以及微积分求出 $s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime }$

7.3 核心公式及其应用

  由向量三角形关系得到坐标转换的核心公式：
$$\overrightarrow{r_r}+l{\vec{n}}_r={\vec{r}}_h$$  但公式目前还不能直接用，因为 $\overrightarrow{r_r}$ 和 ${\vec{n}}_r$ 都不知道，知道的只有 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,{\kappa }_h,{\vec{\tau }}_h,{\vec{n}}_h$。

  首先要找到投影，即 $(x_h,y_h)$在 Frenet 坐标系下的投影$(x_r,y_r,{\theta }_r,{\kappa }_r)$，但在这里先不讲，假设已经找到了$(x_r,y_r,{\theta }_r,{\kappa }_r)$。找投影的过程下一节再讲，因为找投影比较绕。

  如果找到投影点的信息，自然可以得到 ${\vec{r}}_r,{\vec{\tau }}_r,{\vec{n}}_r,{\kappa }_r$。

  有了这些自然就可以用核心公式 $\overrightarrow{r_r}+l{\vec{n}}_r={\vec{r}}_h$。

(1) 计算弧长

  第一步：计算 $l$，根据核心公式 $l{\vec{n}}_r={\vec{r}}_h-\overrightarrow{r_r}$，两边点乘${\vec{n}}_r$：
$$l=\left({\vec{r}}_h-{\vec{r}}_r\right)\cdot {\vec{n}}_r$$

(2) 计算弧速度

  第二步：计算 $\dot{s}$，核心公式 $\overrightarrow{r_r}+l{\vec{n}}_r={\vec{r}}_h$ 两边对时间求导：
$${\vec{\dot{r}}}_r+l{\vec{\dot{n}}}_r+\dot{l}{\vec{n}}_r={\vec{\dot{r}}}_h$$  利用辅助公式$(1)(2)(5)(6)$，代入得到：
$$\dot{s}{\vec{\tau }}_r+l\left(-{\kappa }_r\dot{s}{\vec{\tau }}_r\right)+\dot{l}{\vec{n}}_r={\vec{v}}_h$$  两边同时点乘 ${\vec{\tau }}_r$，得到
$$\dot{s}+l\left(-{\kappa }_r\dot{s}\right)={\vec{v}}_h\cdot {\vec{\tau }}_r$$  这样得到
$$\dot{s}=\frac{{\vec{v}}_h\cdot {\vec{\tau }}_r}{1-k_{r}l}$$  又因为
$${\vec{v}}_h\cdot {\vec{\tau }}_r=|{\vec{v}}_h|{\vec{\tau }}_h\cdot {\vec{\tau }}_r=|{\vec{v}}_h||{\vec{\tau }}_h||{\vec{\tau }}_r|\cos <{\vec{\tau }}_h,{\vec{\tau }}_r>=|{\vec{v}}_h|\cos \left({\theta }_h-{\theta }_r\right)$$  得到另一种形式：
$$\dot{s}=\frac{|{\vec{v}}_h|\cos \left({\theta }_h-{\theta }_r\right)}{1-{\kappa }_{r}l}$$  推荐使用上面的向量法表示，更简洁。

(3) 计算弧长的时间导数

  第三步：计算 $\dot{l}$，由第二步推导的 $\dot{s}{\vec{\tau }}_r+l\left(-{\kappa }_r\dot{s}{\vec{\tau }}_r\right)+\dot{l}{\vec{n}}_r={\vec{v}}_h$，两边同时点乘 ${\vec{n}}_r$ 得：
$$\dot{l}={\vec{v}}_h\cdot {\vec{n}}_r$$  若将向量形式展开，得到 $\dot{l}=|{\vec{v}}_h|\sin ({\theta }_h-{\theta }_r)$。

(4) 计算弧长的弧坐标导数

  第四步：计算 $l^{\prime }$：
$$l^{\prime }=\frac{dl}{ds}=\frac{\frac{dl}{dt}}{\frac{ds}{dt}}=\frac{\dot{l}}{\dot{s}}=\frac{\vec{v}\cdot {\vec{n}}_r}{\frac{\vec{v}\cdot {\vec{\tau }}_r}{1-{\kappa }_{r}l}}=\left(1-{\kappa }_{r}l\right)\frac{\vec{v}\cdot {\vec{n}}_r}{\vec{v}\cdot {\vec{\tau }}_r}$$其中， $ds$ 为 Frenet 坐标轴的弧坐标导数，因此 $\frac{ds}{dt}=\dot{s}$。

(5) 计算弧加速度

  第五步：计算 $\ddot{s}$，由第二步计算的$\dot{s}=\frac{{\vec{v}}_h\cdot {\vec{\tau }}_r}{1-k_{r}l}$，利用复合求导，得到：

$$\begin{array}{rl}\ddot{s}& =\frac{\text{d}\dot{s}}{\text{d}t}=\frac{1}{{\left(1-{\kappa }_{r}l\right)}^2}\left(\frac{\text{d}\left(\vec{v}\cdot {\vec{\tau }}_r\right)}{\text{d}t}\cdot \left(1-{\kappa }_{r}l\right)-\left(\vec{v}\cdot {\vec{\tau }}_r\right)\cdot \left(-{\dot{\kappa }}_{r}l-{\kappa }_r\dot{l}\right)\right)\\ & =\frac{1}{1-{\kappa }_{r}l}\left(\frac{d\vec{v}}{dt}\cdot {\vec{\tau }}_r+\vec{v}\cdot \frac{d{\vec{\tau }}_r}{dt}\right)+\frac{1}{1-{\kappa }_{r}l}\frac{\vec{v}\cdot {\vec{\tau }}_r}{1-{\kappa }_{r}l}\left({\dot{\kappa }}_{r}l+{\kappa }_r\dot{l}\right)\\ & =\frac{1}{1-{\kappa }_{r}l}\text{(}\vec{a}\cdot {\vec{\tau }}_r+\vec{v}\cdot \left({\kappa }_r\cdot \dot{s}\cdot {\vec{n}}_r\right)+\frac{1}{1-{\kappa }_{r}l}\dot{s}\left(\frac{d{\kappa }_r}{ds}\frac{ds}{dt}l+{\kappa }_r\cdot \frac{dl}{ds}\cdot \frac{ds}{dt}\right)\\ & =\frac{\vec{a}\cdot {\vec{\tau }}_r}{1-{\kappa }_{r}l}+\frac{\left({\kappa }_r\dot{s}\right)\left(\vec{v}\cdot {\vec{n}}_r\right)}{1-{\kappa }_{r}l}+\frac{{\dot{s}}^2}{1-{\kappa }_{r}l}\left({\kappa }_r^{\prime }l+{\kappa }_r{l}^{\prime }\right)\end{array}$$  又因为 $\vec{v}\cdot {\vec{n}}_r=\dot{l}，\dot{l}=l^{\prime }\cdot \dot{s}$，代入最终得到
$$\ddot{s}=\frac{\vec{a}\cdot {\vec{\tau }}_r}{1-{\kappa }_{r}l}+\frac{{\kappa }_r{\dot{s}}^2{l}^{\prime }}{1-{\kappa }_{r}l}+\frac{{\dot{s}}^2}{1-{\kappa }_{r}l}\left({\kappa }_r^{\prime }l+{\kappa }_r{l}^{\prime }\right)$$  其中，$\vec{a}\cdot {\vec{\tau }}_r$ 的计算如下，利用辅助公式 $(7)$：$\vec{a}=|\vec{\dot{v}}|{\vec{\tau }}_h+|\vec{v}{|}^2{\kappa }_h{\vec{n}}_h$，可得
$$\begin{array}{rl}\vec{a}\cdot {\vec{\tau }}_r& =|\vec{\dot{v}}|\cos <{\vec{\tau }}_h,{\vec{\tau }}_r>+|\vec{v}{|}^2{\kappa }_h{\vec{n}}_h\cdot {\vec{\tau }}_r\\ & =|\vec{\dot{v}}|\cos \left({\theta }_h-{\theta }_r\right)+{\kappa }_h\cdot \frac{{\left(1-{\kappa }_{r}l\right)}^2}{{\cos }^2\left({\theta }_h-{\theta }_r\right)}{\dot{s}}^2\cdot \left(-\sin \left({\theta }_h-{\theta }_r\right)\right)\\ & =|\vec{\dot{v}}|\cos \left({\theta }_h-{\theta }_r\right)+{\kappa }_h{\dot{s}}^2\frac{1-{\kappa }_{r}l}{\cos \left({\theta }_n-{\theta }_r\right)}\cdot \left(-l^{\prime }\right)\end{array}$$

(6) 计算弧长的二阶时间导数

  第六步：计算 $\ddot{l}$，由辅助公式 $(3)$：$\dot{l}=\vec{v}\cdot {\vec{n}}_r$ 可得：
$$\ddot{l}=\frac{d\vec{v}}{dt}\cdot \overrightarrow{n_r}+\vec{v}\cdot \frac{d{\vec{n}}_r}{dt}$$  由辅助公式 $(6)$：${\vec{n}}_r=-{\kappa }_r\dot{s}{\vec{\tau }}_r$ 可得：
$$\begin{array}{rl}\ddot{l}& =\vec{a}\cdot {\vec{n}}_r+\vec{v}\cdot \left(-{\kappa }_r\dot{s}{\vec{\tau }}_r\right)\\ & =\vec{a}\cdot {\vec{n}}_r-{\kappa }_r\dot{s}\left(\vec{v}\cdot {\vec{\tau }}_r\right)\\ & =\vec{a}\cdot {\vec{n}}_r-{\kappa }_r\left(1-{\kappa }_{r}l\right){\dot{s}}^2\end{array}$$

(7) 计算弧长的二阶弧坐标导数

  第七步：计算 $l^{\prime \prime }$，用 $\ddot{l}$ 计算：
$$\begin{array}{rl}\ddot{l}& =\frac{d\dot{l}}{dt}=\frac{d\left(l^{\prime }\dot{s}\right)}{dt}=\frac{d{l}^{\prime }}{dt}\dot{s}+l^{\prime }\cdot \ddot{s}\\ & =\frac{d{l}^{\prime }}{ds}\cdot \frac{ds}{dt}\cdot \dot{s}+l^{\prime }\ddot{s}\\ & =l^{\prime }‘{\dot{s}}^2+l^{\prime }\ddot{s}\end{array}$$  所以：
$$l^{\prime \prime }=\frac{\ddot{l}-l^{\prime }\ddot{s}}{{\dot{s}}^2}$$  这样基本上所有直角坐标转自然坐标都已讲完。

八、总结

  共有 $8$ 个坐标需要转化，即 $s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime },\dot{l},\ddot{l}$ ，总结公式如下：
$$\begin{array}{rl}l& =\left({\vec{r}}_h-{\vec{r}}_r\right)\cdot {\vec{n}}_r\\ \dot{l}& =\vec{v}\cdot {\vec{n}}_r\\ \dot{s}& =\frac{\vec{v}\cdot {\vec{\tau }}_r}{1-{\kappa }_{r}l}\\ l^{\prime }& =\frac{\dot{l}}{\dot{s}}\\ \ddot{l}& =\vec{a}\cdot {\vec{n}}_r-{\kappa }_r\left(1-{\kappa }_{r}l\right){\dot{s}}^2\\ \ddot{s}& =\frac{\vec{a}\cdot {\vec{\tau }}_r}{1-{\kappa }_{r}l}+\frac{{\dot{s}}^2{\kappa }_r{l}^{\prime }}{1-{\kappa }_{r}l}+\frac{{\dot{s}}^2\left({\kappa }_r^{\prime }l+{\kappa }_r{l}^{\prime }\right)}{1-{\kappa }_{r}l}\\ l^{\prime \prime }& =\frac{\ddot{l}-l^{\prime }\ddot{s}}{{\dot{s}}^2}\end{array}$$  现在算出了 $7$ 个，还差 $s$ 没算，$s$ 和投影点相关。

  本篇博客讲解了从直角坐标转换到自然坐标，下一篇博客讲解 $s$ 如何计算，以及投影点怎么找，如何从自然坐标转换到直角坐标。

  本篇博客到此结束，欢迎关注后续内容！

参考资料

  自动驾驶决策规划算法第一章第三节(上) 直角坐标与自然坐标转换

后记：

🌟 感谢您耐心阅读这篇关于 直角坐标与自然坐标转换Ⅰ 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀