原创文章,转载请注明: 转载自慢慢的回味
本文链接地址: Cartesian与Frenet坐标系转换公式推导
车在道路上行驶,以车的视角来看,车就如同在一条光滑的曲线上移动,且不时带有左右偏移。为了算法简单,我们选择了Frenet坐标系,它可以把直角坐标系下的复杂轨迹转换为只有S,L两个维度的简单曲线。
Frenet坐标系简介
如下图所示,3维空间中一条连续可微的曲线K,P为曲线K上的一个点,方格背景平面为曲线K在点P处的运动平面。为曲线K在点P处的切向量,即质点再P处的运动方向;为K在P处的法向量,垂直于质点运动方向,和在同一运动平面;为曲线K在P处的副法向量,且同时垂直于和,即垂直于运动平面。
Frenet的定义公式如下:
其中,表示某一方向向量对弧长s的导数,为曲率为曲线相对于直线的弯曲程度,当为0时为直线,表述为曲线运动方向的变化关于弧长的导数(),为挠率是曲线不能形成在同一平面内运动曲线的度量值,挠率越趋于0,则曲线越趋近于在同一平面内运动。Apollo的运动在大地上,局部路面可看作一个平面,挠率可设定为0,而Frenet公式可简化为:
(1)
Cartesian与Frenet坐标系转换公式推导
整个公式的推导以下图的标注为蓝图:
有如下变量:
Frenet坐标系:;
Cartesian坐标系:。
其中:
:Frenet纵坐标,即曲线Reference Line上质点运动的位置;
:Frenet纵坐标对时间的导数,也即沿Reference Line的速度;
:沿Reference Line的加速度;
:Frenet横坐标,质点横向的偏移量;
:Frenet横向速度;
:Frenet横向加速度;
:Frenet横向坐标对纵向坐标的导数;
:Frenet横向坐标对纵向坐标的二阶导数;
:为对应Cartesian坐标系下的坐标,是一个向量;
:Cartesian坐标系下的运动方向角;
:曲率;
:Cartesian坐标系下的线速度;
:Cartesian坐标系下的加速度。
如下为Apollo代码中从cartesian到frenet的转换方法:
ptr_s_condition分别存储,和;
ptr_d_condition分别存储,和。
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; } |
已知推导
求
通过找到曲线上离最近的参考点,该参考点处的即为在Frenet坐标系下的。这可以通过高清地图上路径的投影找到:
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; } |
求
分别为参考点与待转换点在Cartesian坐标系下的向量,它们的关系为:,即质点朝着的方向移动距离为:
上式中,为向量的方向角度,为单位向量的方向角度(对比上一张图可知),则上式可利用向量点乘公式得到。为向量与向量之间的夹角。
设,则。
在Frenet坐标系下,每个点到Reference Line上参考点的向量都与该参考点的法向量同向或反向,因为我们找的是到Reference Line的最近点,因此,=1 或 -1。为向量的角度,因此,。即可根据的正负来确定的正负号,公式如下:
(2)
已知推导
求
由可得:
由Frenet公式(见上式式(1))有,于是有,代入上式可得:
由于与正交,,代入上式得:
(3)
由于:
(16)
因此:
(4)
求
由与可得:
(5)
已知推导
求
由,可得:
(10)
假定为为车辆当前轨迹的弧长,有:
(11)
又有曲率:
(12)
(13)
故式10可得:
(14)
由式11和式13:
(15)
综上14和15可得:
(6)
求
由式5得:
有三角公式的导数:
由式12和式15:
由式4得 :
(17)
(7)
由式5,7,2,4,6得到Cartesian转Frenet坐标系公式:
(8)
由式2,4,16,17,6得到Frenet转Cartesian坐标系公式:
(9)
参考链接:
https://blog.csdn.net/u013468614/article/details/108748016
https://blog.csdn.net/davidhopper/article/details/79162385本作品采用知识共享署名 4.0 国际许可协议进行许可。