Cartesian 坐标系与Frenet坐标系的转换

作者: 北瓜头的头 | 来源:发表于2019-12-13 12:02 被阅读0次

前言

在机器人或者无人车规划的时候，通常需要引入Frenet坐标系．这篇文章主要介绍了笛卡尔坐标系和Frenet坐标系之间的转换．

什么是Frenet坐标系

Frenet坐标系是一种以比传统x，y笛卡尔坐标更直观的方式表示道路位置的方式。用Frenet坐标，我们使用变量 s和d描述车辆在道路上的位置。该s坐标表示沿道路的距离（也称为纵向位移）和d坐标表示道路上的左右位置（也称为横向位移）。
由于通常道路都是曲折的，这样的话用笛卡尔坐标系描述道路会非常复杂，但是如果采用Frenet坐标系，　则会非常直观和简单．关于Frenet坐标系就不赘述了．
下图是机器人在Frenet 坐标系下生成的轨迹．

image.png

从Frenet坐标系转换到笛卡尔坐标系，也就是已知如下公式中左边的参数，将其转换成下面式子右边的参数：
$[s, \dot{s}, \ddot{s}; d, \dot{d}, \ddot{d} / d', d''] \rightarrow[\mathbf{x}, \theta_x, \kappa_x, v_x, a_x]$
下面对以上转换中用到的符号的含义进行说明：
在Frenet坐标系下的参数：
$s$ , 在Frenet坐标系下的纵向坐标；
$\dot{s}=\frac{\text{d}s}{\text{d}t}$ , 纵向坐标对时间的导数，也就是纵向速度；
$\ddot{s}=\frac{d^2 s}{dt^2}$ , 纵向坐标对时间的二阶导数，也就是纵向加速度；
$d$ , 横向坐标;
$s$ : 在Frenet坐标系下的纵向坐标；
$\dot{d}=\frac{\text{d}}{\text{d}t}d$ , 横向坐标对时间的导数，也就是横向速度；
$\ddot{d}=\frac{\text{d}^2}{\text{d}t^2}d$ , 横向坐标对时间的二阶导数，也就是横向加速度；
$d'=\frac{\text{d}}{\text{d} s}d(s)$ , 横向坐标对纵向坐标的导数；
$d''=\frac{\text{d}^2}{\text{d}s^2}d(s)$ , 横向坐标对纵向坐标的二阶导数；
而在笛卡尔坐标系下的参数为:
$\mathbf{x}$ , 在笛卡尔坐标系下的坐标，是一个向量；
$\theta_x$ , 是在笛卡尔坐标下的朝向；
$\kappa_x$ , 是在笛卡尔坐标系下的曲率，有 $\kappa_x=\frac{\text{d}\theta_x}{\text{d}s_x}$ , 其中， $s_x$ 为在当前的轨迹的弧长；
$v_x$ , 为在笛卡尔坐标系下的速度， $v_x=||\dot{\mathbf{x}}||_2$ .
$a_x$ , 在笛卡尔坐标系下的加速度， $a_x=\dot{v}_x$ .

在明确了上述符号的含义后，我们可以推导从Frenet到笛卡尔坐标系的转换了．
$\mathbf{x}(s(t), d(t)) = \mathbf{r}(s(t)) + d(t)\mathbf{n}_r(s(t))$
上面的式子是从笛卡尔坐标系转换到Frenet坐标系下的公式，可以推导得到：
$d=(\mathbf{x}-\mathbf{r})^T\mathbf{n}_r$
这里的 $d$ 是一个标量．
那么我们对 $d$ 关于时间 $t$ 求导，有：
$\dot{d}=[\dot{\mathbf{x}}-\dot{\mathbf{r}}]^T\mathbf{n}_r+[\mathbf{x}-\mathbf{r}]^T\dot{\mathbf{n}}_r = v_x{\mathbf{t}_x}^T\mathbf{n}_r - \dot{s}{\mathbf{t}_r}^T\mathbf{n}_r-\dot{s}\kappa_r[\mathbf{x}-\mathbf{r}]^T\mathbf{t}_r$
其中， ${\mathbf{t}_r}^T\mathbf{n}_r = 0$ (垂直)， $\kappa_r[\mathbf{x}-\mathbf{r}]^T\mathbf{t}_r=0$ (垂直)．
因此， $\dot{d}=v_x{\mathbf{t}_x}^T\mathbf{n}_r=v_x\sin\Delta\theta$ .
这里的转换是由下面的式子得到的： $t_x=[\cos \theta_x, \sin \theta_x]^T$ , $n_x=[-\sin \theta_x, \cos \theta_x]^T$ , $t_r=[\cos \theta_r, \sin \theta_r]^T$ , $n_r=[-\sin \theta_r, \cos \theta_r]^T$ , $\Delta \theta = \theta_x -\theta_r$ . $||\Delta \theta||<\frac{\pi}{2}$ , ${\theta}^{\prime}_r = \frac{\text{d}{\theta}_r}{\text{d}s_r}=\kappa_r$ .

其中， $\dot{\text{n}}_r = [-\cos(\theta_r), -\sin(\theta_r)]^T\dot{\theta}_r = -t_r \dot{\theta}_r = -\dot{s}t_r\theta^{\prime}_r = -\dot{s}t_r\kappa_r$

所以可以得到
$\dot{d} = v_x \mathbf{t}_x^T\mathbf{n}_r = v_x[\cos(\theta_x), \sin(\theta_x)][-\sin(\theta_r), \cos(\theta_r)]^T = v_x \sin(\Delta \theta)$

$v_x = \Arrowvert\dot{\mathbf{x}}\Arrowvert_2= \Arrowvert \frac{\text{d}(\mathbf{r}(s(t)) + d(t)\mathbf{n}_r(s(t)))}{\text{d}t} \Arrowvert_2 = \Arrowvert [\mathbf{t}_r, \mathbf{n}_r] \begin{bmatrix} 1-\kappa_rd & 0 \\ 1 & 0 \end{bmatrix} \begin{bmatrix} \dot{s} \\ \dot{d} \end{bmatrix}\lVert_2 = \sqrt{(1-\kappa_rd)^2\dot{s}^2 + \dot{d}^2}$

进一步，
$d'=\frac{\text{d}}{\text{d}s}d =\frac{\text{d}t}{\text{d}s}\frac{\text{d}}{\text{d}t}d = \frac{1}{\dot{s}}\dot{d} = \frac{1}{\dot{s}}v_x\sin \Delta \theta = \frac{1}{\dot{s}}\sqrt{(1-\kappa_rd)^2\dot{s}^2 + \dot{d}^2} \sin \Delta \theta$
$d'^2=[[1-\kappa_rd]^2+d'^2]\sin^2 \Delta \theta$
$d'^2[1-\sin^2\Delta \theta]=[1-\kappa_rd]^2\sin^2 \Delta \theta$

$(d^{\prime})^2 = [1-\kappa_rd]^2 \tan^2(\Delta \theta)$

$d'=[1-\kappa_rd]\tan \Delta \theta$

因此，可以计算
$\theta_x=\theta_r + \arctan\frac{d'} {1-\kappa_r d }$
可以推导出来 $\dot{d}=d'\dot{s}$
由 $(\mathbf{x} - \mathbf{r})^T\mathbf{t}_r = 0$ , 两边对时间 $t$ 求导，得到，
$0 = (\dot{\mathbf{x}}-\dot{\mathbf{r}})^T\mathbf{t}_r + (\mathbf{x} - \mathbf{r})^T\dot{\mathbf{t}}_r = (v_x\mathbf{t}_x - \dot{s}\mathbf{t}_r)^T\mathbf{t}_r + \dot{s}(\mathbf{x} - \mathbf{r})^T\dot{\mathbf{t}}_r$

$0 = v_x \mathbf{t}_x^T\mathbf{t}_r - \dot{s} + \dot{s}d\kappa_r = v_x\cos(\Delta \theta) - \dot{s}(1 - \kappa_rd)$

$\frac{v_x}{\dot{s}} \cos \Delta \theta -1+\kappa_rd =0$

$v_x=\dot{s}\frac{1-\kappa_rd}{\cos \Delta \theta}$

有链式求导法则
$\frac{\text{d}}{\text{d}s} = \frac{\text{d}s_x}{\text{d}s} \frac{\text{d}}{\text{d}s_x}=\frac{v_x}{\dot{s}} \frac{\text{d}}{\text{d}s_x}=\frac{1-\kappa_rd}{\cos \Delta \theta} \frac{\text{d}}{\text{d}s_x}$
因此，对 $d'$ 关于 $s$ 求导数，得到
$d''=-[\kappa_r' d+\kappa_r d'] \tan \Delta \theta + \frac{1-\kappa_rd}{\cos^2 \Delta \theta}[\kappa_x\frac{1-\kappa_r d}{\cos \Delta \theta}-\kappa_r]$
由上面的式子，可以求得 $\kappa_x$ .
最后，来求 $a_x$ , 有
$a_x=\dot{v}_x$
因此
$a_x = \dot{v}_x = \ddot{s} \frac{1-k_rd}{\cos(\Delta \theta)} + \dot{s}^2 \frac{\text{d}}{\text{d}s} \frac{(1-\kappa_rd)}{\cos(\Delta \theta)} = \ddot{s} \frac{1-k_rd}{\cos(\Delta \theta)} + \frac{\dot{s}^2}{\cos(\Delta \theta)}(\tan(\Delta \theta)\Delta \theta^{\prime}(1-\kappa_rd)-(\kappa_r^{\prime}d + \kappa_rd^{\prime}))$
其中， $\Delta \theta^{\prime} = \frac{\text{d}}{\text{d}s_r}(\theta_x -\theta_r) = \frac{\text{d}\theta_x}{\text{d}s_x} \frac{\text{d}s_x}{\text{d}s_r} - \kappa_r = \kappa_x \frac{ \text{d}s_x}{\text{d}t} \frac{\text{d}t}{\text{d}s_r} - \kappa_r =\kappa_x \frac{v_x}{\dot{s}} - \kappa_r = \kappa_x \frac{1-\kappa_rd}{\cos(\Delta \theta)} - \kappa_r$

总结如下:

S-L坐标系转换成笛卡尔坐标系，即为 $(s,\dot{s}, \ddot{s}, d, d^{\prime}, d^{\prime\prime})\rightarrow(\mathbf{x},v_x, a_x, k_x,\theta_x)$

$\mathbf{x} = \mathbf{r}(s) + \mathbf{n}_rd$ , 其中， $\mathbf{r(s)}$ 为参考点坐标， $\mathbf{n}_r$ 为参考线法向量， $\mathbf{n}_r = [-\sin(\theta_r), \cos(\theta_r)]^T$
由（30）得到
$v_x =\dot{s} \frac{1-\kappa_rd}{\cos \Delta \theta }$
由(26)得到

$\theta_x=\theta_r + \arctan{\frac{d'}{1-\kappa_r d }}$

由(34)得到

$a_x = \ddot{s} \frac{1-k_rd}{\cos(\Delta \theta)} + \frac{\dot{s}^2}{\cos(\Delta \theta)}(\tan(\Delta \theta)\Delta \theta^{\prime}(1-\kappa_rd )-(\kappa_r^{\prime}d + \kappa_rd^{\prime}))$
其中， $\Delta \theta^{\prime} = \kappa_x \frac{1-\kappa_rd}{\cos(\Delta \theta)} - \kappa_r$

由（32）得到

$k_x = \frac{\cos^3(\Delta \theta)(d^{\prime\prime} + (k_r^{\prime}d + k_rd^{\prime})) + \tan(\Delta \theta)}{(1-k_rd)^2} + \frac{k_r\cos \Delta \theta}{1-k_rd}$

笛卡尔坐标系转换成SL坐标系 即为 $(\mathbf{x},v_x, a_x, k_x,\theta_x)\rightarrow(s,\dot{s}, \ddot{s}, d, d^{\prime}, d^{\prime\prime})$

$s$ 为参考点 $\mathbf{r}(s)$ 的自变量， $d= (\mathbf{x} - \mathbf{r}(s)]^T\mathbf{n}_r$
由（30）可知道 $\dot{s}$ :
$\dot{s} = \frac{v_x\cos\Delta \theta}{1- \kappa_rd}$
由(34)可知道
$\ddot{s} = \frac{\cos\Delta \theta}{1-\kappa_rd}[a_x - \frac{\dot{s}}{\cos \Delta \theta}(\tan \Delta \theta)\Delta\theta(1-\kappa_rd) - (\kappa_r^{\prime}d+\kappa_rd^{\prime})$
$d^{\prime}$ : 由(25)可以知道
$d'=[1-\kappa_rd]\tan \Delta \theta$
由（32）可以知道
$d''=-[\kappa_r' d+\kappa_r d'] \tan \Delta \theta + \frac{1-\kappa_rd}{\cos^2 \Delta \theta}[\kappa_x\frac{1-\kappa_r d}{\cos \Delta \theta}-\kappa_r]$