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

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

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

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

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

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

引言

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

一、坐标转换与投影计算

1.1 自然坐标系与直角坐标系转换

  在第一章上已经讲了从直角坐标系到自然坐标系之间的坐标转换，也就是上面所写的这七个公式以及下图：

  七个辅助转换公式如下：
$$\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}$$本篇博客接着第三讲上篇遗留的几个问题：

• 如何找到 $(x_r,y_r,{\theta }_r,{\kappa }_r)$
• 如何计算 $s$
• 如何把自然坐标系下的坐标 $s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime },\dot{l},\ddot{l}$ 转回到直角坐标系 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,{\kappa }_h$

这就是本篇博客所解决的三个问题。

1.2 参考轨迹的离散化

  如何找到 $(x_r,y_r,{\theta }_r,{\kappa }_r)$，实际上以前已经讲过，在自动驾驶控制算法第七节：

  【自动驾驶】控制算法（七）离散规划轨迹的误差计算

  虽然以前讲过，但为了考虑系列整个知识的连贯性，所以在此再讲一遍。

  在自动驾驶中，自然坐标系下的参考轨迹一般都是以离散点的形式给出的，所以只讲离散点寻找投影点的方法。

  比如有一条参考轨迹曲线：

  曲线上有一系列离散点，假设已知 $(x_i,y_i,{\theta }_i,{\kappa }_i)$，其中 ${\kappa }_i$ 是曲率，${\theta }_i$ 是点的切线方向与 $x$ 轴的夹角，这里的 ${\kappa }_i$ 和 ${\theta }_i$ 都是指在直角坐标系下的 ${\kappa }_i$ 和 ${\theta }_i$。

  如果有参考轨迹的详细表达式的话，自然可以得到理想投影，因为有蓝色线的表达式，不难找投影点。但在一般情况下没有蓝色线，只有这一堆离散点，以及它们的$(x_i,y_i,{\theta }_i,{\kappa }_i)$ ，所以求车在离散曲线下的投影，必然是近似解，不可能得到理想投影。因为离散点相比连续曲线，天生就缺少信息，连续曲线的信息比较多，在缺失信息下不可能完美解出投影点来，只能得到近似解，但是只要近似程度满足自动驾驶的要求就没问题。

如何计算投影的近似解呢？

  首先要遍历离散曲线，计算出离散点与车辆距离最短的点，称为匹配点。找到匹配点后，就可以计算两个向量，起点为匹配点，终点为车辆当前位置的向量 $\vec{d}$，匹配点的单位切向量 $\vec{\tau }$。

  因为已知匹配点的 $(x_m,y_m,{\theta }_m,{\kappa }_m)$，设车的坐标是 $(x_h,y_h)$，
$$\begin{array}{rl}\vec{d}& =\left(x_h-x_m,y_h-y_m\right)\\ \vec{\tau }& =\left(\cos {\theta }_m,\sin {\theta }_m\right)\end{array}$$  则投影点的近似位矢为
$$\begin{array}{rl}{\vec{r}}_r& =\left(\begin{array}{c}{x}_r\\ y_r\end{array}\right)={\vec{r}}_m+\left(\vec{d}\cdot \vec{\tau }\right)\vec{\tau }\\ & =\left(\begin{array}{c}{x}_m\\ y_m\end{array}\right)+\left(\vec{d}\cdot \vec{\tau }\right)\left(\begin{array}{c}\cos {\theta }_m\\ \sin {\theta }_m\end{array}\right)\end{array}$$

1.3 计算投影点近似解的几何原理

  公式的原理要从几何角度看，画一下相关示意图：

  在直角坐标系中，匹配点位矢为 ${\vec{r}}_m$，则 $\vec{d}$ 向量在 $\vec{\tau }$ 向量上的分量为 $(\vec{d}\cdot \vec{\tau })\cdot \vec{\tau }$，因为 $\vec{d}\cdot \vec{\tau }$ 是向量的大小。$\vec{\tau }$ 代表分量的方向， $(\vec{d}\cdot \vec{\tau })\cdot \vec{\tau }$ 就是 $\vec{d}$ 在方向 $\vec{\tau }$ 方向上分量的向量，也就是上图所画出的绿色向量。

  ${\vec{r}}_m$ 加绿色小向量等于绿色大向量，把绿色大向量近似认为是 ${\vec{r}}_r$，也就是用 ${\vec{r}}_m+\vec{d}$ 在 $\vec{\tau }$ 上的分量向量的和近似认为是 ${\vec{r}}_r$，即
$${\vec{r}}_m+\left(\vec{d}\cdot \vec{\tau }\right)\vec{\tau }={\vec{r}}_r$$

1.4 自然坐标系坐标轴为直线时投影点的精确解

  原理显而易见，如果自然坐标系得坐标轴是直线

   $\vec{d}$ 向量，在 $\vec{\tau }$ 方向上的投影 $(\vec{d}\cdot \vec{\tau })\cdot \vec{\tau }$ 是上图绿色向量， ${\vec{r}}_m$ 加绿色向量等于 ${\vec{r}}_r$。可以发现在直线的情况下， ${\vec{r}}_r$ 正好就是投影位矢的精确解。

为什么是精确解呢？

  因为上图自然坐标系的坐标是直线，直线上的任何切向量都相等，且任何一点的法向量也都相等，所以匹配点的 $\vec{n}$ 和 ${\vec{r}}_r$ 所指的 $\vec{n}$ 的法向量一样，是同方向，而且和切向量垂直，正好这就是投影的定义，所以是精确解。

1.5 自然坐标系坐标轴为弧线时的投影点计算

  如果自然坐标系的坐标轴是弧线，但如果弧线的曲率不太大，而且离散点比较密时，精度也比较高，比如这样的曲线：

  如果曲率越小，而且划分越密，每段就越像直线，精度也就越高，这样的话 ${\vec{r}}_r$ 就算出来了，但是算出 ${\vec{r}}_r$ 还不够，还得算 ${\theta }_r$ 和 ${\kappa }_r$，下面讲解如何计算${\theta }_r$ 和 ${\kappa }_r$，把图画一下：

  可近似认为 ${\kappa }_r={\kappa }_m$。

  由曲率的定义式，$\Delta s$ 为图中粉色线， $\vec{d}\cdot \vec{\tau }$ 为图中深蓝色线，可近似认为 $\Delta s=\vec{d}\cdot \vec{\tau }$，则：
$$k_m=\frac{d\theta }{ds}\approx \frac{\Delta \theta }{\Delta s}\approx \frac{\Delta \theta }{\left(\vec{d}\cdot \vec{\tau }\right)}\Rightarrow \Delta \theta =k_m\cdot \left(\vec{d}\cdot \vec{\tau }\right)$$  所以
$${\theta }_r={\theta }_m+\Delta \theta ={\theta }_m+{\kappa }_m\cdot \left(\vec{d}\cdot \vec{\tau }\right)$$  即$${\theta }_r={\theta }_m+{\kappa }_m\cdot \left(\vec{d}\cdot \vec{\tau }\right)$$

二、自然坐标系下的弧长计算

2.1 路径长度累加算法

  从起点开始把各个点的长度累加起来，一直加到投影点位置，如图所示：

$$s={\left[{\left(x_1-x_0\right)}^2+{\left(y_1-y_0\right)}^2\right]}^{\frac{1}{2}}+{\left[{\left(x_2-x_1\right)}^2+{\left(y_2-y_1\right)}^2\right]}^{\frac{1}{2}}+...$$

  本质上就是以直线代替弧长，将离散点的弧线以直线的长度近似代替。

  当路径点足够密时，用直线的长度近似代替弧长，误差可以忽略。

2.2 直线近似弧长的合理性及其误差分析

  有人可能会觉得这真的可以吗？看起来好像比刚才讲的算投影点的算法还要粗糙，还要近似得多，这样真的可以吗？

  算法的精度要看规划的范围，如果是几公里到几百公里不等的规划，这么算的话，误差累积肯定很大。因为每一段都用弧线近似直线，误差会累积。但如果规划的长度只有几十米到几百米不等，这样的近似可以接受，这种算法看起来比较糙，但它也有优点：

• 简单

• 稳定

  这是因为任何探测上的数据都会有误差，这是不可避免的。地图有地图的误差，传感器有传感器的误差，探测装置也有硬件误差，就算什么都没问题了，计算机表达的位数也有舍入误差。因为计算机能表达的小数据有限，什么都有误差，那算法的要求就是在参数变化很小范围内，结果不要发生大的变化。

2.3 算法的稳定性与鲁棒性

  用专业术语表达就是 对参数的变化不敏感，如果算法对参数的变化不敏感，就认为是稳定的算法。

其实计算 $s$ 有其他更精确的算法，但是都不是特别稳定，但是以分段直线近似曲线这样的算法虽然比较粗糙，但是对参数的变化不敏感，比较稳定。

  描述对于参数敏感的性质叫鲁棒性。实际上就是英语 Robust 单词的音译，根据发音而来，意思就是算法对参数的变化不敏感。像这种比较糙的算法，它的稳定性的，或者说鲁棒性还是比较好的，路径点稍微有点误差的话， $s$ 的变化并不大，但有些精确算法的稳定性就很差。

2.4 精确算法举例及其局限性

  下面举个使用精确算法计算的例子。

  比如有一段曲线：

  若采用圆弧近似的方法，首先计算平均曲率
$$\bar{\kappa }=\frac{k_i+k_{i+1}}{2}$$  曲率半径等于平均曲率的倒数 $R=\frac{1}{\bar{\kappa }}$。根据三角关系
$$l=2R\sin \frac{\theta }{2}\theta =2\arcsin \frac{l}{2R}$$  则 $s=R\theta$，这样可以算出用圆弧近似曲线的 $s$，这是比用直线近似曲线更精确的算法。

为什么不用这种算法呢？

  因为算法不稳定。曲率 $\bar{\kappa }$ 一般比较小，而 $R=\frac{1}{\bar{\kappa }}$。 $\bar{\kappa }$ 的微小的变化会引起 $R$ 的剧烈波动，导致算法不稳定。

  大家可以想想初中学过的反比例函数 $y=\frac{1}{x}$ 函数的图像在 $x$ 取接近于 $0$ 时，$y$ 的变化非常剧烈，这就是问题的根源。 $\bar{\kappa }$ 如果是比较小的数，稍微有一点点测量误差，就会导致 $R$ 剧烈波动，所以算法很不稳定，对参数变化非常敏感。

2.5 自动驾驶算法的选择与场景适应性

  当然了除了这种算法还有其他精确算法，实际上其他算法都有各种各样的问题，有的非常依赖参数的精确性。对参数很敏感，也有的对参数不敏感，但是计算非常耗时，所以也不行。

  综合起来还是上面说的看起来比较糙的算法最合适。当然如果有些场景需要精确的 $s$ 的话，再单独处理，在这里只是说在大多数情况下都通用的算法。自动驾驶算法没有好与不好，只有合适与不合适。不同算法都有各自的问题，要扬长避短。在大多数情况下，这种比较糙的算法能工作，可以满足当前场景的需求，那算法就没问题。如果某些场景这些算法并不能满足场景的要求，再针对场景做另外更精确的处理。

三、自然坐标转换为直角坐标详细步骤

  已知 Frenet 坐标 $s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime },\dot{l},\ddot{l}$ 和自然坐标系坐标轴的离散点信息 $(x_i,y_i,{\theta }_i,{\kappa }_i)$，求直角坐标 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,{\kappa }_h$

  坐标转换的关键是找投影点$(x_r,y_r,{\theta }_r,{\kappa }_r)$。

3.1 计算映射表

  首先要计算 $x_i\to s_i$ 之间映射的对应表。即每个Frenet 坐标轴上离散点的 $x_i$ 与 $s_i$ 之间的对应关系，比如下表：

3.2 查找投影点

  查找 $x_i\to s_i$ 对应表，找到 $s_n$，使得 $s_n<s<s_{n+1}$，$s_n$ 与 $s_{n+1}$ 对应的 $x_i$ 记为 $x_n$与 $x_{n+1}$，也就是找到 $x_n,y_n,{\theta }_n,{\kappa }_n$ 和 $x_{n+1},y_{n+1},{\theta }_{n+1},{\kappa }_{n+1}$。

3.3 计算投影点的坐标和参数

  已知 ${\vec{r}}_n=\left(x_n,y_n\right)$，图中橙色向量记为 ${\vec{r}}_d$，其大小为 $s-s_n$，方向近似为$(\cos {\theta }_n,\sin {\theta }_n)$，所以
$${\vec{r}}_d=\left(s-s_n\right){\vec{\tau }}_n$$  若想要更精确些，可以使用到 $s_{n+1}$ 的信息，则
$$\bar{\theta }=\frac{{\theta }_n+{\theta }_{n+1}}{2}{\bar{\tau }}_n=\left(\cos \bar{\theta },\sin \bar{\theta }\right)$$  可解出带有 $x_r,y_r$ 信息的 ${\vec{r}}_r$：
$${\vec{r}}_r={\vec{r}}_d+{\vec{r}}_r$$  以及$$\begin{array}{rl}{\theta }_r& ={\theta }_n+{\kappa }_n\cdot \left(s-s_n\right)\\ {\kappa }_r& =\frac{{\kappa }_n+{\kappa }_{n+1}}{2}\\ {\kappa }_r^{{}^{\prime }}& =\frac{d{\kappa }_r}{ds}\approx \frac{{\kappa }_{n+1}-{\kappa }_n}{s_{n+1}-s_n}\\ {\vec{\tau }}_r& =\left(\cos {\theta }_r,\sin {\theta }_r\right){\vec{n}}_r=\left(-\sin {\theta }_r,\cos {\theta }_r\right)\end{array}$$  最后就可以通过第三讲上半篇所讲的这七个辅助公式：
$$\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}$$  已知$s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime },\dot{l},\ddot{l},{\vec{r}}_r,{\vec{\tau }}_r,{\vec{n}}_r,{\kappa }_r,{\kappa }_r^{\prime }$，求 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,{\kappa }_h$

3.4 计算车辆位矢

  计算 ${\vec{r}}_h$， 有如下三角关系：

  可直接得到
$${\vec{r}}_h={\vec{r}}_r+l{\vec{n}}_r$$

3.5 计算车辆速度向量

  计算 ${\vec{v}}_h$，由公式 $(2)(3)$： $\dot{l}=\vec{v}\cdot {\vec{n}}_r$、$\dot{s}=\frac{\vec{v}\cdot {\vec{\tau }}_r}{1-{\kappa }_{r}l}$ 可推导出：
$$\begin{array}{rl}{\vec{v}}_h\cdot {\vec{n}}_r=\dot{l}& \Rightarrow |{\vec{v}}_h||{\vec{n}}_r|\cos <{\vec{v}}_h,{\vec{n}}_r>=\dot{l}\\ {\vec{v}}_h\cdot {\vec{\tau }}_r=\left(1-{\kappa }_{r}l\right)\dot{s}& \Rightarrow |{\vec{v}}_n||{\vec{\tau }}_r|\cos \left({\vec{v}}_h\cdot {\vec{\tau }}_r\right)=\left(1-{\kappa }_{r}l\right)\dot{s}\end{array}$$  设 $v_h$ 与 $x$ 轴夹角为 ${\theta }_h$，因为 ${\vec{\tau }}_r$ 与 $x$ 轴的夹角为 ${\theta }_r$，${\vec{n}}_r$ 与 ${\vec{\tau }}_r$ 垂直，所以 ${\vec{n}}_r$ 与 $x$ 轴夹角为 ${\theta }_r+\frac{\pi }{2}$，所以：
$$\begin{array}{rl}\cos <{\vec{v}}_h,{\vec{n}}_r>& =\cos \left({\theta }_h-\left({\theta }_r+\frac{\pi }{2}\right)\right)=\sin \left({\theta }_h-{\theta }_r\right)\\ \cos <{\vec{v}}_h,{\vec{\tau }}_r>& =\cos \left({\theta }_h-{\theta }_r\right)\end{array}$$  代入到上式，得到 $\vec{v}$ 的大小：
$$\begin{array}{r}|\vec{v}|\sin \left({\theta }_h-{\theta }_r\right)=\dot{l}\\ |\vec{v}|\cos \left({\theta }_h-{\theta }_r\right)=\left(1-{\kappa }_{r}l\right)\dot{s}\end{array}\}\stackrel{\text{平方相加}}{\Rightarrow }|\vec{v}|={\left[{\dot{l}}^2+{\left(1-{\kappa }_{r}l\right)}^2{\dot{s}}^2\right]}^{\frac{1}{2}}$$  下面计算 $\vec{v}$ 的方向，根据公式 $(5)$:$l^{\prime }=\frac{\dot{l}}{\dot{s}}$：
$$\begin{array}{r}|\vec{v}|\sin \left({\theta }_h-{\theta }_r\right)=\dot{l}\\ |\vec{v}|\cos \left({\theta }_h-{\theta }_r\right)=\left(1-{\kappa }_{r}l\right)\dot{s}\end{array}\}\stackrel{\text{相除}}{\Rightarrow }\tan \left({\theta }_h-{\theta }_r\right)=\frac{\dot{l}}{\left(1-{\kappa }_{r}l\right)\dot{s}}=\frac{l^{\prime }}{1-{\kappa }_{r}l}$$  可得：
$${\theta }_h={\theta }_r+\arctan \left(\frac{l^{\prime }}{1-{\kappa }_{r}l}\right)$$  可得到 $\vec{v}$ 的切向方向向量 ${\vec{\tau }}_h$ 以及 $v$ 的法向方向向量 ${\vec{n}}_h$：
$${\vec{\tau }}_h=\left(\cos {\theta }_h,\sin {\theta }_h\right){\vec{n}}_h=\left(-\sin {\theta }_h,\cos {\theta }_h\right)$$  其中，${\vec{\tau }}_h$ 代表 $\vec{v}$ 的方向。

3.6 计算车辆加速度向量

  计算 ${\vec{a}}_h$，使用这两个公式：
$$\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}$$  可计算出
$$\begin{array}{rl}{\vec{a}}_h\cdot {\vec{n}}_r& =\ddot{l}+{\kappa }_r\left(1-{\kappa }_{r}l\right){\dot{s}}^2\\ {\vec{a}}_h\cdot {\vec{\tau }}_r& =\ddot{s}\left(1-{\kappa }_{r}l\right)-{\dot{s}}^2{\kappa }_r{l}^{\prime }+{\dot{s}}^2\left({\kappa }_r^{\prime }l+{\kappa }_r{l}^{\prime }\right)\end{array}$$  同理，上面两式平方相加可得
∣ a ⃗ h ∣ = { [ l ˙ + κ r ( 1 − κ r l s ˙ 2 ] 2 + [ s ¨ ( 1 − κ r l ) − s ˙ 2 κ r l ′ + s ˙ 2 ( κ r ′ l + κ r l ′ ) ] 2 } 1 2 |\vec{a}_h|=\{\left[ \dot{l}+\kappa _r\text{(}1-\kappa _rl\dot{s}^2 \right] ^2+\left[ \ddot{s}\left( 1-\kappa _rl \right) -\dot{s}^2\kappa _rl'+\dot{s}^2\left( \kappa _{r}'l+\kappa _rl' \right) \right] ^2\}^{\frac{1}{2}} ∣a h​∣={[l˙+κr​(1−κr​ls˙2]2+[s¨(1−κr​l)−s˙2κr​l′+s˙2(κr′​l+κr​l′)]2}21​  设 ${\vec{a}}_h$ 与 $x$ 轴夹角为 ${\theta }_{ah}$，则
$${\theta }_{ah}=\arctan \left(\frac{\ddot{l}+{\kappa }_r\left(1-{\kappa }_{r}l\right){\dot{s}}^2}{\ddot{s}\left(1-{\kappa }_{r}l\right)-{\dot{s}}^2{\kappa }_r{l}^{\prime }+{\dot{s}}^2\left({\kappa }_r^{\prime }l+{\kappa }_r{l}^{\prime }\right)}\right)+{\theta }_r$$
  注意：$|\vec{\dot{v}}|\ne |{\vec{a}}_h|$。因为

$${\vec{a}}_h=|\vec{\dot{v}}|{\vec{\tau }}_r+|\vec{v}{|}^2{\kappa }_h{\vec{n}}_r$$

  其中，$|\vec{\dot{v}}|{\vec{\tau }}_r$ 为切向加速度，$|\vec{v}{|}^2{\kappa }_h{\vec{n}}_r$ 为向心加速度。

  由 ${\theta }_{ah}$ 可计算出 ${\vec{\tau }}_{ah}$：
$${\vec{\tau }}_{ah}=\left(\cos \left({\theta }_{ah}\right),\sin \left({\theta }_{ah}\right)\right)$$  由此可得到向量 ${\vec{a}}_h$ 的完整形式：
$${\vec{a}}_h=|{\vec{a}}_h|{\vec{\tau }}_{ah}$$

3.7 计算曲率

  计算曲率 ${\kappa }_h$，由 ${\vec{a}}_h=|\vec{\dot{v}}|{\vec{\tau }}_r+|\vec{v}{|}^2{\kappa }_h{\vec{n}}_r$ 两边点乘 ${\vec{n}}_r$ ：
$$\vec{a}\cdot {\vec{n}}_r=|\vec{v}{|}^2{\kappa }_h$$  则$${\kappa }_h=\frac{\vec{a}\cdot {\vec{n}}_r}{|\vec{v}{|}^2}$$  这样就把所有的 ${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,{\kappa }_h$ 都算出来了。

四、总结

  数学基础部分的所有内容基本上全讲完了，这也是数学基础部分的完结篇。从第二章开始，将正式进入决策规划的主要内容，讲相关算法。因为数学基础部分的铺垫就已经将所需要的数学知识基本上都已经铺垫完了。

  在博主看来，第一章中最重要、最难的内容肯定是第三节，但最重要的内容实际上是第二集，要仔细体会凸优化以及非凸优化是怎么转化为凸问题的，在求解这些东西时要理解透彻，对于第一节和第三节，其实只是工具类的说明，只要会用就可以了。所以大家要把第一章的第二节好好看一看。后续的第二章将详细讲解决策规划的相关内容，欢迎关注！

参考资料

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

后记：

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

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

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

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

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