Cartesian与Frenet坐标系转换公式推导

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Cartesian与Frenet坐标系转换公式推导

车在道路上行驶，以车的视角来看，车就如同在一条光滑的曲线上移动，且不时带有左右偏移。为了算法简单，我们选择了Frenet坐标系，它可以把直角坐标系下的复杂轨迹转换为只有S，L两个维度的简单曲线。

目录:
  1. Frenet坐标系简介
  2. Cartesian与Frenet坐标系转换公式推导
    1. 已知\(\vec{x}=[x_{x}, y_{x}]\)推导\([s, l]\)
      1. 求\(s\)
      2. 求\(l\)
    2. 已知\(\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x\)推导\([s, l,\dot{s},l']\)
      1. 求\(l'\)
      2. 求\(\dot{s} \)
    3. 已知\(\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x,a_x,\kappa_{x} \)推导\([s, l,\dot{s},l',\ddot{s},l'']\)
      1. 求\(\l''\)
      2. 求\(\ddot{s}\)

Frenet坐标系简介

如下图所示，3维空间中一条连续可微的曲线K，P为曲线K上的一个点，方格背景平面为曲线K在点P处的运动平面。\vec{T}为曲线K在点P处的切向量，即质点再P处的运动方向；\vec{N}为K在P处的法向量，垂直于质点运动方向\vec{T}，\vec{N}和\vec{T}在同一运动平面；\vec{B}为曲线K在P处的副法向量，且同时垂直于\vec{T}和\vec{N}，即垂直于运动平面。


Frenet的定义公式如下：

    \[ \begin{Bmatrix} \frac{d\vec{T}}{ds}= & \kappa\vec{N} \\ \frac{d\vec{N}}{ds}= & -\kappa\vec{T}+\tau\vec{B} \\ \frac{d\vec{B}}{ds}= & -\tau\vec{N} \end{Bmatrix} \]

其中，\frac{d}{ds}表示某一方向向量对弧长s的导数，\kappa为曲率为曲线相对于直线的弯曲程度，当为0时为直线，表述为曲线运动方向的变化关于弧长的导数（\kappa = \left\| \frac {d\mathbf {T} }{ds}}\right\|），\tau为挠率是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于0，则曲线越趋近于在同一平面内运动。Apollo的运动在大地上，局部路面可看作一个平面，挠率可设定为0，而Frenet公式可简化为：

(1)   \[ \begin{Bmatrix} \frac{d\vec{T}}{ds}= & \kappa\vec{N} \\ \frac{d\vec{N}}{ds}= & -\kappa\vec{T} \end{Bmatrix} \right \]

Cartesian与Frenet坐标系转换公式推导

整个公式的推导以下图的标注为蓝图：

有如下变量：
Frenet坐标系：[s, \dot{s}, \ddot{s}, l, \dot{l}, \ddot{l}, l', l'']；
Cartesian坐标系：[\vec{x},v_{x}, a_{a}, \theta_{x},\kappa_{x}]。
其中：
s：Frenet纵坐标，即曲线Reference Line上质点运动的位置；
\dot{s}=\frac{ds}{dt}：Frenet纵坐标对时间的导数，也即沿Reference Line的速度；
\ddot{s}=\frac{d\dot{s}}{dt}：沿Reference Line的加速度；
l：Frenet横坐标，质点横向的偏移量；
\dot{l}=\frac{dl}{dt}：Frenet横向速度；
\ddot{l}=\frac{d\dot{l}}{dt}：Frenet横向加速度；
l'=\frac{dl}{ds}：Frenet横向坐标对纵向坐标的导数；
l''=\frac{dl'}{ds}：Frenet横向坐标对纵向坐标的二阶导数；
\vec{x}：为对应Cartesian坐标系下的坐标，是一个向量；
\theta_{x}：Cartesian坐标系下的运动方向角；
\kappa_{x}=\frac{d\theta_{x}}{ds}：曲率；
v_{x}=||\dot{\vec{x}}||_{2}：Cartesian坐标系下的线速度；
a_{x}=\frac{dv_{x}}{dt}：Cartesian坐标系下的加速度。

如下为Apollo代码中从cartesian到frenet的转换方法：
ptr_s_condition分别存储s，\dot{s}和\ddot{s}；
ptr_d_condition分别存储l，\l'和\l''。

void CartesianFrenetConverter::cartesian_to_frenet(
    const double rs, const double rx, const double ry, const double rtheta,
    const double rkappa, const double rdkappa, const double x, const double y,
    const double v, const double a, const double theta, const double kappa,
    std::array<double, 3>* const ptr_s_condition,
    std::array<double, 3>* const ptr_d_condition) {
  const double dx = x - rx;
  const double dy = y - ry;
 
  const double cos_theta_r = std::cos(rtheta);
  const double sin_theta_r = std::sin(rtheta);
 
  const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
  ptr_d_condition->at(0) =
      std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);
 
  const double delta_theta = theta - rtheta;
  const double tan_delta_theta = std::tan(delta_theta);
  const double cos_delta_theta = std::cos(delta_theta);
 
  const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);
  ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;
 
  const double kappa_r_d_prime =
      rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);
 
  ptr_d_condition->at(2) =
      -kappa_r_d_prime * tan_delta_theta +
      one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *
          (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);
 
  ptr_s_condition->at(0) = rs;
 
  ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;
 
  const double delta_theta_prime =
      one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;
  ptr_s_condition->at(2) =
      (a * cos_delta_theta -
       ptr_s_condition->at(1) * ptr_s_condition->at(1) *
           (ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) /
      one_minus_kappa_r_d;
}
已知\vec{x}=[x_{x}, y_{x}]推导[s, l]
求s

通过找到曲线上离\vec{x}=[x_{x}, y_{x}]最近的参考点\vec{r}=[x_{r}, y_{r}]，该参考点处的s即为[x_{x}, y_{x}]在Frenet坐标系下的s。这可以通过高清地图上路径的投影找到：
Apollo的代码如下：

bool ReferenceLine::XYToSL(const common::math::Vec2d& xy_point,
                           SLPoint* const sl_point) const {
  double s = 0.0;
  double l = 0.0;
  if (!map_path_.GetProjection(xy_point, &s, &l)) {
    AERROR << "Cannot get nearest point from path.";
    return false;
  }
  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}
求l

\vec{x},\vec{r}分别为参考点与待转换点在Cartesian坐标系下的向量，它们的关系为：\vec{x}=\vec{r}+l\vec{N_{r}}，即质点\vec{r}朝着\vec{N_{r}}的方向移动距离l为\vec{x}：

    \begin{align*} l &=(\vec{x}-\vec{r})^{T}\vec{N_{r}} \\ &=||\vec{x}-\vec{r}||_{2}\cos(\theta_{x-r}-(\theta_{r}+\frac{\pi}{2})) \\ &=||\vec{x}-\vec{r}||_{2}(\sin(\theta_{x-r})\cos(\theta_{r})-\cos(\theta_{x-r})\sin(\theta_{r})) \end{align*}

上式中，\theta_{x-r}为向量\vec{x}-\vec{r}的方向角度，\theta_{r}+\frac{\pi}{2}为单位向量\vec{N}_{r}的方向角度（对比上一张图可知），则上式可利用向量点乘公式\vec{a}\times \vec{b}=|a||b|\cos{\theta}得到。\theta为向量\vec{a}与向量\vec{b}之间的夹角。

设\vec{x}=(x_{x}, y_{x}), \vec{r}=(x_{r}, y_{r})，则||\vec{x}-\vec{r}||_{2}=\sqrt{(x_{x}-x_{r})^2+(y_{x}-y_{r})^2}。

在Frenet坐标系下，每个点到Reference Line上参考点的向量都与该参考点的法向量\vec{N}同向或反向，因为我们找的是到Reference Line的最近点，因此，\sin(\theta_{x-r})\cos(\theta_{r})-\cos(\theta_{x-r})\sin(\theta_{r})=1 或 -1。\theta_{x-r}为向量\vec{x}-\vec{r}的角度，因此，\frac{\sin(\theta_{x-r})}{\cos(\theta_{x-r})}=\frac{y_{x}-y_{r}}{x_{x}-x_{r}}。即可根据(y_{x}-y_{r})\cos(\theta_{r})-(x_{x}-x_{r})\sin(\theta_{r})的正负来确定l的正负号，公式如下：

(2)   \[ l = sign\left((y_{x}-y_{r})\cos(\theta_{r})-(x_{x}-x_{r})\sin(\theta_{r})\right)\sqrt{(x_{x}-x_{r})^2+(y_{x}-y_{r})^2} \]

已知\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x推导[s, l,\dot{s},l']
求l'

由l=(\vec{x}-\vec{r})^{T}\vec{N_{r}}可得:

    \begin{align*} \dot{l} &=(\dot{\vec{x}}-\dot{\vec{r}})^{T}\vec{N_{r}}+(\vec{x}-\vec{r})^{T}\dot{\vec{N_{r}}} \\ &= v_{x}\vec{T_{x}}^{T}\vec{N_{r}}-\dot{s}\vec{T_{r}}^{T}\vec{N_{r}}+l\vec{N_{r}}^{T}\dot{\vec{N_{r}}} \end{align*}

由Frenet公式（见上式式(1)）有\vec{N_{r}}'=\frac{d\vec{N_{r}}}{ds}=-\kappa_{r}\vec{T_{r}}，于是有\dot{\vec{N_{r}}}=\frac{ds}{dt}\frac{d\vec{N_{r}}}{ds}=-\kappa_{r}\dot{s}\vec{T_{r}}，代入上式可得：

    \[ \dot{l}=v_{x}\vec{T_{x}}^{T}\vec{N_{r}}-\dot{s}\vec{T_{r}}^{T}\vec{N_{r}}-l\dot{s}\kappa_{r}\vec{N_{r}}^{T}\vec{T_{r}} \]

由于\vec{N_{r}}与\vec{T_{r}}正交\Rightarrow \vec{N_{r}}^{T}\vec{T_{r}}=\vec{T_{r}}^{T}\vec{N_{r}}=0，\vec{T_{x}}^{T}\vec{N_{r}}=\cos(\theta_{x}-\theta_{r}-\frac{\pi}{2})=\sin(\theta_{x}-\theta_{r}),代入上式得：

(3)   \[ \dot{l}=v_{x}\sin(\theta_{x}-\theta_{r}) \]

由于：

    \begin{align*} \dot{\vec{x}} &=\frac{d\vec{x}}{dt} \\ &=\frac{\vec{r}+l\vec{N_{r}}}{dt} \\ &=\dot{\vec{r}}+\dot{l}\vec{N_{r}}+l\dot{\vec{N_{r}}} \\ &=\dot{s}\vec{T_{r}}+\dot{l}\vec{N_{r}}-l\kappa_{r}\dot{s}\vec{T_{r}} \\ &=\dot{s}(1-l\kappa_{r})\vec{T_{r}}+\dot{l}\vec{N_{r}} \end{align*}

(16)   \begin{align*} v_{x} &=||\dot{\vec{x}}||_{2} \\ &=\sqrt{\dot{\vec{x}}^{T}\dot{\vec{x}}} \\ &=\sqrt{[\dot{s}(1-\kappa_{r}l)]^{2}+\dot{l}^{2}} \end{align*}

    \begin{align*} l'^{2} &=(\frac{dl}{ds})^{2}=(\frac{\dot{l}}{\dot{s}})^{2} \\ &=(\frac{v_{x}\sin(\theta_{x}-\theta_{r})}{\dot{s}})^{2} \\ &=\sin^{2}(\theta_{x}-\theta_{r})\frac{[\dot{s}(1-\kappa_{r}l)]^{2}+\dot{l}^{2}}{\dot{s}^{2}} \\ &=\sin^{2}(\theta_{x}-\theta_{r})[(1-\kappa_{r}l)^{2}+(\frac{\dot{l}}{\dot{s}})^{2}] \\ &=\sin^{2}(\theta_{x}-\theta_{r})[(1-\kappa_{r}l)^{2}+l'^{2}] \\ &\Rightarrow \frac{1}{\sin^{2}(\theta_{x}-\theta_{r})}l'^{2}=(1-\kappa_{r}l)^{2}+l'^{2} \\ &\Rightarrow (\frac{1}{\sin^{2}(\theta_{x}-\theta_{r})}-1)l'^{2}=(1-\kappa_{r}l)^{2} \\ &\Rightarrow \frac{\s\cos^{2}(\theta_{x}-\theta_{r})}{\sin^{2}(\theta_{x}-\theta_{r})}l'^{2}=(1-\kappa_{r}l)^{2} \\ &\Rightarrow l'=(1-\kappa_{r}l)\tan(\theta_{x}-\theta_{r}) \end{align*}

因此：

(4)   \[ l'=(1-\kappa_{r}l)\tan(\theta_{x}-\theta_{r}) \]

求\dot{s}

由l'=\frac{\dot{l}}{\dot{s}}=\frac{v_{x}\sin(\theta_{x}-\theta_{r})}{\dot{s}}与l'=(1-\kappa_{r}l)\tan(\theta_{x}-\theta_{r})可得:

(5)   \[ \dot{s}=\frac{v_{x}\cos(\theta_{x}-\theta_{r})}{1-\kappa_{r}l} \]

已知\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x,a_x,\kappa_{x}推导[s, l,\dot{s},l',\ddot{s},l'']
求\l''

由l'=(1-\kappa_{r}l)\tan(\theta_{x}-\theta_{r})，可得:

(10)   \[ \(l''=\frac{dl'}{ds}=(1-\kappa_{r}l)'\tan(\theta_{x}-\theta_{r})+(1-\kappa_{r}l)\frac{(\theta_{x}-\theta_{r})'}{\cos^{2}(\theta_{x}-\theta_{r})}\) \]

​
假定s_{x}为为车辆当前轨迹\vec{x}的弧长，有：

(11)   \[ \frac{d}{ds}=\frac{ds_{x}}{dt}\frac{dt}{ds}\frac{d}{ds_{x}}=\frac{v_{x}}{\dot{s}}\frac{d}{ds_{x}}=\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}\frac{d}{ds_{x}} \]

又有曲率:

(12)   \[ \kappa_{r}=\frac{d\theta_{r}}{ds} \]

(13)   \[ \kappa_{x}=\frac{d\theta_{x}}{ds_{x}} \]

故式10可得：

(14)   \[ l''=\frac{dl'}{ds}=-(\kappa_{r}'l+\kappa_{r}l')\tan(\theta_{x}-\theta_{r})+\frac{(1-\kappa_{r}l)}{\cos^2(\theta_{x}-\theta_{r})}(\frac{d\theta_{x}}{ds}-\frac{d\theta_{r}}{ds}) \]

由式11和式13:

(15)   \[ \frac{d\theta_{x}}{ds}=\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}\frac{d\theta_{x}}{ds_{x}}=\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}\kappa_{x} \]

综上14和15可得：

(6)   \[ l''=-(\kappa_{r}'l+\kappa_{r}l')\tan(\theta_{x}-\theta_{r})+\frac{(1-\kappa_{r}l)}{\cos^2(\theta_{x}-\theta_{r})}(\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}\kappa_{x}-\kappa_{r}) \]

求\ddot{s}

由式5得：

    \[ v_{x}=\frac{\dot{s}(1-\kappa_{r}l)}{\cos(\theta_{x}-\theta_{r})} \]

有三角公式的导数：

    \[ \frac{1}{\cos(\theta)}'=\frac{\tan(\theta)}{\cos(\theta)} \]

由式12和式15:

    \begin{align*} (\theta_{x}-\theta_{r})' &=\theta_{x}'-\theta_{r}' \\ &=\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}\kappa_{x}-\kappa_{r} \end{align*}

由式4得 l'=(1-\kappa_{r}l)\tan(\theta_{x}-\theta_{r}):

(17)   \begin{align*} a_{x} &=\frac{dv_{x}}{dt} \\ &=\ddot{s}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}+\dot{s}\frac{d}{ds}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}\dot{s} \\ &=\ddot{s}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}+\frac{\dot{s}^{2}}{\cos(\theta_{x}-\theta_{r})}\left[(1-\kappa_{r}l)\tan(\theta_{x}-\theta_{r})(\theta_{x}-\theta_{r})'-(\kappa_{r}'l+\kappa_{r}l')\right] \\ &=\ddot{s}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}+\frac{\dot{s}^{2}}{\cos(\theta_{x}-\theta_{r})}\left[l'(\theta_{x}-\theta_{r})'-(\kappa_{r}'l+\kappa_{r}l')\right] \\ &=\ddot{s}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}+\frac{\dot{s}^{2}}{\cos(\theta_{x}-\theta_{r})}\left[l'(\kappa_{x}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}-\kappa_{r})-(\kappa_{r}'l+\kappa_{r}l')\right] \end{align*}

(7)   \[ \ddot{s}=\frac{a_{x}\cos(\theta_{x}-\theta_{r})-\dot{s}^{2}\left[l'(\kappa_{x}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}-\kappa_{r})-(\kappa_{r}'l+\kappa_{r}l')\right]}{1-\kappa_{r}l} \]

由式5，7，2，4，6得到Cartesian转Frenet坐标系公式：

(8)   \[ \begin{Bmatrix} s=& s_{r} \\ \dot{s}=& \frac{v_{x}\cos(\theta_{x}-\theta_{r})}{1-k_{r}l} \\ \ddot{s}=& \frac{a_{x}\cos(\theta_{x}-\theta_{r})-\dot{s}^{2}\left[l'(k_{x}\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}-k_{r})-(k_{r}'l+k_{r}l')\right]}{1-k_{r}l} \\ l=& sign\left((y_{x}-y_{r})\cos(\theta_{r})-(x_{x}-x_{r})\sin(\theta_{r})\right)\sqrt{(x_{x}-x_{r})^2+(y_{x}-y_{r})^2} \\ l'=& (1-k_{r}l)\tan(\theta_{x}-\theta_{r}) \\ l''= &-(k_{r}'l+k_{r}l')\tan(\theta_{x}-\theta_{r})+\frac{(1-k_{r}l)}{\cos^2(\theta_{x}-\theta_{r})}(\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}k_{x}-k_{r}) \end{Bmatrix} \right. \]

由式2，4，16，17,6得到Frenet转Cartesian坐标系公式：

(9)   \[ \begin{Bmatrix} x_{x}= & x_{r}-l\sin(\theta_{r}) \\ y_{x}= & y_{r}+l\cos(\theta_{r}) \\ \theta_{x}= & \arctan(\frac{l'}{1-\kappa_{r}l})+\theta_{r} \in [-\pi, \pi] \\ v_{x}= &\sqrt{[\dot{s}(1-\kappa_{r}l)]^{2}+(\dot{s}l')^{2}} \\ a_{x}= &\ddot{s}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}+\frac{\dot{s}^{2}}{\cos(\theta_{x}-\theta_{r})}\left[l'(\kappa_{x}\frac{1-\kappa_{r}l}{\cos(\theta_{x}-\theta_{r})}-\kappa_{r})-(\kappa_{r}'l+\kappa_{r}l')\right] \\ \kappa_{x}= & ((l''+(\kappa_{r}'l+\kappa_{r}l')\tan(\theta_{x}-\theta_{r}))\frac{\cos^{2}(\theta_{x}-\theta_{r})}{1-\kappa_{r}l}+\kappa_{r})\frac{\cos(\theta_{x}-\theta_{r})}{1-\kappa_{r}l} \end{Bmatrix} \right. \]

参考链接：
https://blog.csdn.net/u013468614/article/details/108748016
https://blog.csdn.net/davidhopper/article/details/79162385本作品采用知识共享署名 4.0 国际许可协议进行许可。

发表回复