Frenet坐标系及其与笛卡尔坐标的转换(2)——原理

夜影惊鸿

已于 2022-05-16 09:37:25 修改

阅读量2k

点赞数 3

分类专栏： AutomaticDriving 文章标签：几何学概率论机器学习

于 2022-05-16 02:18:09 首次发布

本文链接：https://blog.csdn.net/qq_24561095/article/details/124791994

版权

AutomaticDriving 专栏收录该内容

2 篇文章 5 订阅

订阅专栏

从直觉上来讲， $d s$ 代表指引线上一段里程微元， $\theta_{e}$ 代表轨迹上 $P$ 点的切线姿态角，两个变量相互无关，因此似应直接取 $d\theta_{e}/ds=0$ 。这种想法是不正确的， $d s$ 代表投影点 $M$ 在指引线上的微小变化，这一变化直接导致 $P$ 点位置变化， $\theta_{e}$ 也会相应变化，这意味着 $d s$ 与 $\theta_{e}$ 是间接有关联的。那么如何计算 $d\theta_{e}/{ds}$ 呢?我们定义临时变量 $ds_{e}$ 来表示轨迹上的一段微元，根据轨迹在 $P$ 点曲率的定义式 $\kappa_{e} = d \theta_{e} / ds_{e}$ ，以及公式 $\text{(30)}$ 则有：

$\begin{aligned}\frac{d\theta_{e}}{ds} &= \frac{d\theta_{e}}{ds_{e}}\frac{ds_{e}}{dt}\frac{dt}{ds}\\&= \frac{d\theta_{e}}{ds_{e}} \cdot v_{e} \cdot \frac{1}{\dot{s}}\\&= \frac{d\theta_{e}}{ds_{e}} \cdot \frac{(1-\kappa_{r}l)}{cos(\theta_{e} - \theta_{r})}\\&=\kappa_{e} \cdot \frac{(1-\kappa_{r}l)}{cos(\theta_{e} - \theta_{r})}\end{aligned}\tag{35}$

因此有：

$(\Delta\theta)'= \kappa_{e} \cdot \frac{(1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r} \tag{36}$

将公式 $\text{(36)}$ 代入公式 $\text{(31)}$ 则得到：

$\begin{aligned}l'' &= \frac{d \ l'}{ds}\\&= (1-\kappa_{r} l)' \cdot tan(\Delta\theta) + (1-\kappa_{r} l) \cdot [tan(\Delta\theta)]'\\&= -(\kappa_{r} l)' \cdot tan(\Delta\theta) + (1-\kappa_{r} l) \cdot \frac{(\Delta\theta)'}{cos^{2}(\Delta\theta)}\\&= -(\kappa_{r} l)' \cdot tan(\Delta\theta) + \frac{(1-\kappa_{r} l)}{cos^{2}(\Delta\theta)} \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}]\\&= -(\kappa_{r}' l +\kappa_{r} l' ) \cdot tan(\Delta\theta) + \frac{(1-\kappa_{r} l)}{cos^{2}(\Delta\theta)} \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}]\\\end{aligned}\tag{37}$

4.1.5 推导 $\ddot{s}$

根据公式 $\text{(30)}$ 可得：
$\begin{aligned}\ddot{s} &= \frac{d \dot{s}}{dt}\\&=\frac{d {[v_{e}cos(\Delta\theta)}/{(1-\kappa_{r}l)}]}{dt}\\&=\frac{d \dot{v_{e}}}{dt} \cdot \frac{cos(\Delta\theta)}{(1-\kappa_{r}l)} \quad+\quad v_{e} \cdot \frac{d[cos(\Delta\theta)/(1-\kappa_{r}l)]}{dt}\end{aligned}\tag{38}$

我们聚焦于公式 $\text{(38)}$ 第二项中的微分部分，同时使用公式 $\text{(36)}$ ：

$\begin{aligned}&\frac{d[cos(\Delta\theta)/(1-\kappa_{r}l)]}{dt}\\&= \frac{ \frac{dcos(\Delta\theta)}{dt} \cdot (1-\kappa_{r}l) \quad-\quad \frac{d (1-\kappa_{r}l)}{dt} \cdot cos(\Delta\theta) }{(1-\kappa_{r}l)^{2}}\\&= \frac{-sin(\Delta\theta) \cdot (1-\kappa_{r}l) \cdot \frac{d\Delta\theta}{ds} \frac{ds}{dt} \quad+\quad \frac{d (\kappa_{r}l)}{ds} \frac{ds}{dt} \cdot cos(\Delta\theta) }{(1-\kappa_{r}l)^{2}}\\&= \frac{-sin(\Delta\theta) \cdot (1-\kappa_{r}l) \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] \dot{s} \quad+\quad (\kappa_{r}'l+\kappa_{r}l') \dot{s} \cdot cos(\Delta\theta) }{(1-\kappa_{r}l)^{2}}\\&=\dot{s} \cdot \frac{-sin(\Delta\theta) \cdot (1-\kappa_{r}l) \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] \quad+\quad (\kappa_{r}'l+\kappa_{r}l') \cdot cos(\Delta\theta) }{(1-\kappa_{r}l)^{2}}\\\end{aligned}\tag{39}$

将公式 $\text{(28)}$ 代入公式 $\text{(39)}$ ，则有：

$\begin{aligned}&\frac{d[cos(\Delta\theta)/(1-\kappa_{r}l)]}{dt}\\&=\dot{s} \cdot \frac{-sin(\Delta\theta) \cdot (1-\kappa_{r}l) \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta} - \kappa_{r}] \quad+\quad (\kappa_{r}'l+\kappa_{r}l') \cdot cos(\Delta\theta) }{(1-\kappa_{r}l)^{2}}\\&=\dot{s} \cdot \frac{-cos(\Delta\theta) \cdot tan(\Delta\theta) \cdot (1-\kappa_{r}l) \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] \quad+\quad (\kappa_{r}'l+\kappa_{r}l') \cdot cos(\Delta\theta) }{(1-\kappa_{r}l)^{2}}\\&=\dot{s} \cdot cos(\Delta\theta) \cdot \frac{-l' [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] \quad+\quad (\kappa_{r}'l+\kappa_{r}l') }{(1-\kappa_{r}l)^{2}}\\\end{aligned}\tag{40}$

将公式 $\text{(40)}$ 代入公式 $\text{(38)}$ 中，并利用公式 $\text{(30)}$ 对其进行简化，则有:

$\begin{aligned}\ddot{s} &= \frac{d \dot{s}}{dt}\\&= \frac{d {[v_{e}cos(\Delta\theta)}/{(1-\kappa_{r}l)}]}{dt}\\&=\frac{d \dot{v_{e}}}{dt} \cdot \frac{cos(\Delta\theta)}{(1-\kappa_{r}l)} \quad+\quad v_{e} \cdot \frac{d[cos(\Delta\theta)/(1-\kappa_{r}l)]}{dt}\\ &= a_{e} \cdot \frac{cos(\Delta\theta)}{(1-\kappa_{r}l)} \quad+\quad v_{e} \cdot \{\dot{s} \cdot cos(\Delta\theta) \cdot \frac{-l' [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] \quad+\quad (\kappa_{r}'l+\kappa_{r}l') }{(1-\kappa_{r}l)^{2}}\}\\&= a_{e} \cdot \frac{cos(\Delta\theta)}{(1-\kappa_{r}l)} \quad+\quad \dot{s} \cdot \frac{v_{e} \cdot cos(\Delta\theta)}{(1-\kappa_{r}l)} \cdot \frac{-l' [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] \quad+\quad (\kappa_{r}'l+\kappa_{r}l') }{(1-\kappa_{r}l)}\\&= a_{e} \cdot \frac{cos(\Delta\theta)}{(1-\kappa_{r}l)} \quad-\quad \dot{s}^{2} \cdot \frac{l' [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] \quad-\quad (\kappa_{r}'l+\kappa_{r}l') }{(1-\kappa_{r}l)}\\&= \frac{1}{(1-\kappa_{r}l)} \cdot \{ a_{e} \cdot cos(\Delta\theta) \quad-\quad \dot{s}^{2} \cdot [( \frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}) l' \quad-\quad (\kappa_{r}'l+\kappa_{r}l') ]\}\\\end{aligned}\tag{41}$

4.1.6 推导 $\ddot{l}$

使用公式 $\text{(36)}$ 和公式 $\text{(30)}$ ，求出：

$\begin{aligned}\ddot{l} &= \frac{d \dot{l}}{dt}=\frac{d[v_{e}sin(\Delta\theta)]}{dt}\\&=a_{e} \cdot sin(\Delta\theta) \quad+\quad v_{e}cos(\Delta\theta)\frac{d\Delta\theta}{ds}\frac{ds}{dt}\\&=a_{e} \cdot sin(\Delta\theta) \quad+\quad v_{e}cos(\Delta\theta) (\frac{\kappa_{e} \cdot (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}) \dot{s}\\&=a_{e} \cdot sin(\Delta\theta) \quad+\quad v_{e}cos(\Delta\theta) (\frac{\kappa_{e} \cdot (1-\kappa_{r}l)}{cos(\Delta\theta)} \frac{v_{e}cos(\Delta\theta)}{(1-\kappa_{r} l)}- \kappa_{r}\dot{s})\\&=a_{e} \cdot sin(\Delta\theta) \quad+\quad v_{e}cos(\Delta\theta) (v_{e}\kappa_{e} - \kappa_{r}\dot{s})\\\end{aligned}\tag{42}$

4.1.7 综合结果:

$\left\{\begin{aligned}\Delta\theta &= \theta_{e} - \theta_{r}\\\dot{l} &= v_{e}sin(\Delta\theta)\\l' &= (1-\kappa_{r} l) \cdot tan(\Delta\theta)\\\dot{s} &= \frac{v_{e}cos(\Delta\theta)}{(1-\kappa_{r} l)}\\l'' &= -(\kappa_{r}' l +\kappa_{r} l' ) \cdot tan(\Delta\theta) + \frac{(1-\kappa_{r} l)}{cos^{2}(\Delta\theta)} \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}]\\\ddot{s} &= \frac{1}{(1-\kappa_{r}l)} \cdot \{ a_{e} \cdot cos(\Delta\theta) \quad-\quad \dot{s}^{2} \cdot [( \frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}) l' \quad-\quad (\kappa_{r}'l+\kappa_{r}l') ]\}\\\\\ddot{l} &= a_{e} \cdot sin(\Delta\theta) \quad+\quad v_{e}cos(\Delta\theta) (v_{e}\kappa_{e} - \kappa_{r}\dot{s})\\\end{aligned}\right.\tag{43}$

4.2 运动状态：Frenet坐标 —> 笛卡尔坐标

**Q:已知Frenet坐标系下的 $s^{\ast }、l^{\ast }、\theta_{r}、\dot{s}、\ddot{s}、\dot{l}、\ddot{l}、l'、l''$ ；推导 ** 笛卡尔坐标系下 $(x_{e},y_{e})、\theta _{e}、v_{e}、a_{e}、\kappa _{e}$ 。

4.2.1 推导 $\theta_{e}$

根据公式 $\text{(28)}$ 可推导出 $\theta_{e}$ ：

$\theta_{e} = arctan(\frac{l'}{1-\kappa_{r}l}) + \theta_{r} \tag{44}$

4.2.2 推导 $v_{e}$

根据公式 $\text{(30)}$ 可推导出 $v_{e}$ ：

$v_{e}=\frac{\dot{s}(1-\kappa_{r}l)}{cos(\theta_{e}-\theta_{r})} = \frac{\dot{s}(1-\kappa_{r}l)}{cos(\Delta\theta)}\tag{45}$

4.2.3 推导 $\kappa_{e}$

根据公式 $\text{(37)}$ 可反推导出 $\kappa_{e}$ ：

$\kappa_{e} = \{ \frac{[l''+(\kappa_{r}'l+\kappa_{r}l')tan(\Delta\theta)] \cdot cos^{2}(\Delta\theta)}{1-\kappa_{r}l}+\kappa_{r} \} \cdot \frac{cos(\Delta\theta)}{1-\kappa_{r}l} \tag{46}$

4.2.4 推导 $a_{e}$

根据公式 $\text{(38)}$ 可反推导出 $a_{e}$ ：

$a_{e} = \frac{\ddot{s}(1-\kappa_{r}l)}{cos(\Delta\theta)} + \frac{\dot{s}^{2}}{cos(\Delta\theta)} \{ l'[\frac{\kappa_{e}(1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] - (\kappa_{r}'l+\kappa_{r}l')\} \tag{47}$

4.2.5 综合结果：

$\left\{\begin{aligned}\theta_{e} &= arctan(\frac{l'}{1-\kappa_{r}l}) + \theta_{r} \\v_{e} &= \frac{\dot{s}(1-\kappa_{r}l)}{cos(\Delta\theta)}\\\kappa_{e} &= \{ \frac{[l''+(\kappa_{r}'l+\kappa_{r}l')tan(\Delta\theta)] \cdot cos^{2}(\Delta\theta)}{1-\kappa_{r}l}+\kappa_{r} \} \cdot \frac{cos(\Delta\theta)}{1-\kappa_{r}l} \\a_{e} &= \frac{\ddot{s}(1-\kappa_{r}l)}{cos(\Delta\theta)} + \frac{\dot{s}^{2}}{cos(\Delta\theta)} \{ l'[\frac{\kappa_{e}(1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] - (\kappa_{r}'l+\kappa_{r}l')\}\end{aligned}\right.\tag{48}$

5 Frenet坐标系的局限性

第2节曾提到， Frenet 坐标系未必能够将二维平面上所有点位进行有效的描述。在这里，有效描述是指点位与 Frenet 坐标值之间构成一一映射。以图F4所示的由圆弧段构成的指引线为例，图中 $P$ 点是圆弧段的圆心；原则上来讲， $P$ 点在指引线上的投影点 $M$ 应该是使得 $M P$ 线段长度最短的点，然而此时这样的点有无穷多个——圆弧段上所有点全部符合此要求。由于投影点不唯一， $P$ 点在指引线上坐标无从确定。我们发现，只有当 $P$ 点落人图中阴影区域内时， $P$ 点的投影点 M 才确保是唯一的。

图F4：反映Frenet坐标系表示范围受限的例子
Frenet_point_introduce_4

现考虑另一种极端情况——指引线为无限延伸的直线，此时 Frenet 坐标系的点位表示范围完全覆盖整个二维平面，它与笛卡儿坐标系没有任何差异。不难发现，指引线的走势与形态决定了相应 Frenet 坐标系的表示范围大小。

为了定量地确定 Frenet 坐标系在二维平面上的有效作用范围，给出了指引线曲率的一种计算方法，它与曲率定义式实质上是等价的（感兴趣的读者可自行推导）:

$\kappa(s) = \frac{ \left| y(s)''x(s)'-x(s)''y(s))' \right|}{\{[x(s)']^{2} + [y(s)']^{2}\}^{3/2}}\tag{49}$

给定一条参数化指引线 $\Gamma[ x(s), y(s)]$ ，可针对指引线上每一点 $[x (s), y (s)]$ 计算曲率，曲率的倒数为该点的曲率半径 $\kappa^{-1}(s)$ 。如图F5所示，在指引线上点 $M$ 处切线所对应的法线 $N$ 上，在指引线凹陷侧定义临界点 $P$ 使得 $M P$ 长度为 $\kappa^{-1}(s)$ 。

**将法线 $N$ 上从 $P$ 点到指向曲线凸出侧无穷远处的一段开区间（不包括端点 $P$ ）定义为扫描线。随着里程 $s$ 连续变化．扫描线连续扫过的区域合并起来统称 Frenet 坐标系的有效表征区域。**在有效表征区域内的点位均确保能够对应唯一的 Frenet 坐标值，这一论断涉及微分同胚理论，详情可参阅文献或更早期文献。

Frenet 坐标系作用范围受限意味着距离指引线较远的移动障碍预测轨迹等未必能够准确转换至 Frenet 坐标系中，该现象在指引线曲率较大的行车场景由尤为显著，此时还需采取额外措施避免坐标系转换误差，或干脆直接以类似于泊车决策规划的方式在笛卡儿坐标系中生成行车轨迹¹²。

图F5：Frenet坐标系有效作用范围量化模型
Frenet_point_introduce_5

6 总结

曲线指引线参数方程：
$\begin{aligned} &\Gamma [ x(s), y(s) ],s\varepsilon [0,L],\\\end{aligned}\tag{1}$
轨迹上某点 $P$ 在笛卡尔坐标系下的表示 $x_{e} ,y_{e})$ ，在Frenet坐标系下的表示 $(s^{\ast },l^{\ast })$ ；

$P$ 点在指引线上的投影点 $M$ 在笛卡尔坐标系下的表示 $M=(x(s^{\ast }),y(s^{\ast }))$ ，在Frenet坐标系下的表示 $(s^{\ast },0)$ ；

6.1 笛卡尔坐标 —> Frenet坐标

已知：笛卡尔坐标系下 $(x_{e},y_{e})、v_{e}、a_{e}、\theta _{e}、\kappa _{e}$

求解：Frenet坐标系下的 $s^{\ast }、l^{\ast }、\theta r、\dot{s}、\ddot{s}、\dot{l}、\ddot{l}、l'、l''$
$\text{Frenet坐标系下位置状态}\left\{\begin{aligned}&\widehat{s} = s_{init} - \frac{D'(s_{init})}{D''(s_{init})},D^{'}(s^{\ast })=0,\text{二阶最小化+牛顿法迭代}\\&l^{\ast } = sign[(y_{e}-y(s^{\ast }))cos\theta_{r} - (x_{e}-x(s^{\ast }))sin\theta_{r}] \cdot \sqrt{[x_{e}-x(s^{\ast })]^{2} + [y_{e}-y(s^{\ast })]^{2}}\\&\theta_{r} = arctan[\frac{y'(s^{\ast })}{x'(s^{\ast })}]\\\end{aligned}\right.\tag{51}$

$\text{Frenet坐标系下运动状态}\left\{\begin{aligned}\Delta\theta &= \theta_{e} - \theta_{r}\\\dot{l} &= v_{e}sin(\Delta\theta)\\l' &= (1-\kappa_{r} l) \cdot tan(\Delta\theta)\\\dot{s} &= \frac{v_{e}cos(\Delta\theta)}{(1-\kappa_{r} l)}\\l'' &= -(\kappa_{r}' l +\kappa_{r} l' ) \cdot tan(\Delta\theta) + \frac{(1-\kappa_{r} l)}{cos^{2}(\Delta\theta)} \cdot [\frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}]\\\ddot{s} &= \frac{1}{(1-\kappa_{r}l)} \cdot \{ a_{e} \cdot cos(\Delta\theta) \quad-\quad \dot{s}^{2} \cdot [( \frac{\kappa_{e} (1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}) l' \quad-\quad (\kappa_{r}'l+\kappa_{r}l') ]\}\\\\\ddot{l} &= a_{e} \cdot sin(\Delta\theta) \quad+\quad v_{e}cos(\Delta\theta) (v_{e}\kappa_{e} - \kappa_{r}\dot{s})\\\end{aligned}\right.\tag{52}$

6.2 Frenet坐标 —> 笛卡尔坐标

已知：Frenet坐标系下的 $s^{\ast }、l^{\ast }、\theta_{r}、\dot{s}、\ddot{s}、\dot{l}、\ddot{l}、l'、l''$ ；

求解：笛卡尔坐标系下 $(x_{e},y_{e})、\theta _{e}、v_{e}、a_{e}、\kappa _{e}$ ；
$\text{笛卡尔坐标系下位置状态}\left\{\begin{aligned} &\theta_{r} = arctan[\frac{y'(s^{\ast })}{x'(s^{\ast })}]\\&(x_{e},y_{e}) \equiv [ x(s^{\ast }) + l^{\ast } \cdot cos(\theta_{r} + \frac{\pi }{2}), y(s^{\ast }) + l^{\ast } \cdot sin(\theta_{r} + \frac{\pi }{2})]\\&\theta_{e} = arctan(\frac{l'}{1-\kappa_{r}l}) + \theta_{r} \\\end{aligned}\right.\tag{53}$

$\text{笛卡尔坐标系下运动状态}\left\{\begin{aligned}v_{e} &= \frac{\dot{s}(1-\kappa_{r}l)}{cos(\Delta\theta)}\\\kappa_{e} &= \{ \frac{[l''+(\kappa_{r}'l+\kappa_{r}l')tan(\Delta\theta)] \cdot cos^{2}(\Delta\theta)}{1-\kappa_{r}l}+\kappa_{r} \} \cdot \frac{cos(\Delta\theta)}{1-\kappa_{r}l} \\a_{e} &= \frac{\ddot{s}(1-\kappa_{r}l)}{cos(\Delta\theta)} + \frac{\dot{s}^{2}}{cos(\Delta\theta)} \{ l'[\frac{\kappa_{e}(1-\kappa_{r}l)}{cos(\Delta\theta)} - \kappa_{r}] - (\kappa_{r}'l+\kappa_{r}l')\}\end{aligned}\right.\tag{54}$

7 参考文献

【自动驾驶】笛卡尔坐标系和frenet坐标系相互转换

Apollo cartesian_frenet_conversion.c

ving on curvy roads without Frenet frame : Cartesian frame is al you need [ J ], IEEE Transactions on Inteligent Transportation Systems , under review .

LI B , OUYANG Y , ZHANG Y , et al . Trajectory planning for autonomous driving on curvy roads without Frenet frame : Cartesian frame is al you need [ J ], IEEE Transactions on Inteligent Transportation Systems , under review . ↩︎
LI B , ZHANG YM . Fast trajectory planning in Cartesian rather than Frenet frame : A precise solution for autonomous driving in complex urban scenarios [ J ]. IFAC - PapersOnLine ,2020, ↩︎