Frenet坐标系与Cartesian坐标系相互转换（盲点整合）

已于 2024-01-12 16:55:14 修改
阅读量2.5k 收藏 59
点赞数 27
分类专栏： Frenet坐标系与Cartesian坐标系的相互转换 文章标签： 笔记
于 2024-01-12 13:47:32 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zxy_zxy123/article/details/135545941
版权

文章目录

前言

Frenet坐标系在无人驾驶领域被普遍使用，特别是在城市、高速等道路交通环境下无人驾驶的路径规划系统中。Frenet坐标系使用道路的中心线作为Base frame（参考线Reference line），使用参考线的切线向量和法线向量建立坐标系。相比笛卡尔坐标系，Frenet坐标系简化了路径规划问题。网上关于Frenet坐标系的博客或资料很多，此处不再赘述。本文先给出简单的转换公式推导过程，然后给出三对Frenet坐标系与Cartesian坐标系互转的python代码与使用示例方便自己查阅使用。详细推导参见博客Apollo项目坐标系研究。note：下图中Frenet的q坐标轴也可以用d或l表示。

关于Frenet坐标系形象的理解：

参考：基于Frenet坐标系的无人车轨迹规划详解与实现_frenet坐标系csdn-CSDN博客

既然方法的名字叫“基于Frenet坐标系的轨迹规划”，那首先必须理解什么是“Frenet坐标系”。我们想象一下自己开车时，最简单省劲的策略就是沿着道路上的导引线开。这也是车道线存在的意义。我们把车道线想象成一条连续的曲线。对于一条曲线，如果它是光滑可微的，那么曲线上的每一点处都可以求切线。求切线的目的是它定义了一个方向向量。切线有了，再逆时针旋转90°就能得到一条法线，如下图所示。很显然，切线和法线还有它们的交点就构成了一个直角坐标系，这个坐标系就是传说中的Frenet坐标系。所以Frenet坐标系也是一个直角坐标系，没什么神秘的，大家不用害怕。为了便于理解，我特意用Mathematica做了一个演示动画。黑色曲线代表道路参考线，用鼠标能选择曲线上不同的点，随着鼠标移动道路上的点也在不断变化，对应的Frenet坐标系就跟着变化，很形象了吧。

      

轨迹规划基于Frenet坐标系，完成轨迹规划后，应转化为Cartesian坐标系供控制使用：

在Frenet坐标系下，经规划得到横向、纵向运动轨迹后，需要将其重新映射到全局笛卡尔坐标系，以供控制模块调用。

        如图 所示, 受车辆运动学、动力学特性及道路环境 (避障) 限制, 车辆的实际行驶轨迹与参考线难以重合。

使用Frenet坐标系的优势：

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

1 Frenet公式

  • 曲率：曲线不能形成一条直线的度量值，曲率越趋于0，则曲线越趋近于直线；
  • 挠率：是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于0，则曲线越趋近于在同一平面内运动。

在大地表面，局部路面可看作一个平面，对于无人驾驶路径规划应用，挠率可设定为0。从而Frenet公式进一步简化为：

 

2 简单推导过程

引博客Apollo项目坐标系研究中的图如下：

注意：向量x为当前车的轨迹点，向量r为参考线的点

注意：曲率 kx = dtheart/ds,仅在Cartesian坐标系下成立，在Frenet坐标系下不成立。

此处求s时，需要注意这里的s指的是距离自车位置最近的点（参考曲线上寻找）处的s就是Frenet坐标系下的s 。这里不同于控制使用的目标点（指的是投影点），最近点在控制中称为匹配点。

  

 

二、Frenet坐标系与Cartesian坐标系的相互转换最终公式

1 Cartesian转 Frenet

参考：【自动驾驶】Frenet坐标系与Cartesian坐标系（一）-CSDN博客

2 Frenet转Cartesian

参考：【自动驾驶】Frenet坐标系与Cartesian坐标系（一）-CSDN博客

Apollo项目中Frenet坐标系与笛卡尔坐标系转换代码：

函数声明文件planning/math/frame_conversion/cartesian_frenet_conversion.h：

#ifndef MODULES_PLANNING_MATH_FRAME_CONVERSION_CARTESIAN_FRENET_CONVERSION_H_
#define MODULES_PLANNING_MATH_FRAME_CONVERSION_CARTESIAN_FRENET_CONVERSION_H_

#include <array>

#include "modules/common/math/vec2d.h"

namespace apollo {
namespace planning {

// Notations:
// s_condition = [s, s_dot, s_ddot]
// s: longitudinal coordinate w.r.t reference line.
// s_dot: ds / dt
// s_ddot: d(s_dot) / dt
// d_condition = [d, d_prime, d_pprime]
// d: lateral coordinate w.r.t. reference line
// d_prime: dd / ds
// d_pprime: d(d_prime) / ds
// l: the same as d.
class CartesianFrenetConverter {
 public:
  CartesianFrenetConverter() = delete;
  /**
   * Convert a vehicle state in Cartesian frame to Frenet frame.
   * Decouple a 2d movement to two independent 1d movement w.r.t. reference
   * line.
   * The lateral movement is a function of longitudinal accumulated distance s
   * to achieve better satisfaction of nonholonomic constraints.
   */
  static void 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);
  /**
   * Convert a vehicle state in Frenet frame to Cartesian frame.
   * Combine two independent 1d movement w.r.t. reference line to a 2d movement.
   */
  static void frenet_to_cartesian(const double rs, const double rx,
                                  const double ry, const double rtheta,
                                  const double rkappa, const double rdkappa,
                                  const std::array<double, 3>& s_condition,
                                  const std::array<double, 3>& d_condition,
                                  double* const ptr_x, double* const ptr_y,
                                  double* const ptr_theta,
                                  double* const ptr_kappa, double* const ptr_v,
                                  double* const ptr_a);

  // given sl point extract x, y, theta, kappa
  static double CalculateTheta(const double rtheta, const double rkappa,
                               const double l, const double dl);

  static double CalculateKappa(const double rkappa, const double rdkappa,
                               const double l, const double dl,
                               const double ddl);

  static common::math::Vec2d CalculateCartesianPoint(
      const double rtheta, const common::math::Vec2d& rpoint, const double l);
  /**
   * @brief: given sl, theta, and road's theta, kappa, extract derivative l,
   *second order derivative l:
   */
  static double CalculateLateralDerivative(const double theta_ref,
                                           const double theta, const double l,
                                           const double kappa_ref);

  // given sl, theta, and road's theta, kappa, extract second order derivative
  static double CalculateSecondOrderLateralDerivative(
      const double theta_ref, const double theta, const double kappa_ref,
      const double kappa, const double dkappa_ref, const double l);
};

}  // namespace planning
}  // namespace apollo

 函数实现文件planning/math/frame_conversion/cartesian_frenet_conversion.cc：

#include "modules/planning/math/frame_conversion/cartesian_frenet_conversion.h"

#include <cmath>

#include "modules/common/log.h"
#include "modules/common/math/math_utils.h"

namespace apollo {
namespace planning {

using apollo::common::math::Vec2d;

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;
  // 求解d
  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);
  // 求解d' = dd / ds
  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);

  // 求解d'' = dd' / ds
  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);

  // 求解s
  ptr_s_condition->at(0) = rs;
  // 求解ds / dt
  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;
  // 求解d(ds) / dt
  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;
  return;
}

void CartesianFrenetConverter::frenet_to_cartesian(
    const double rs, const double rx, const double ry, const double rtheta,
    const double rkappa, const double rdkappa,
    const std::array<double, 3>& s_condition,
    const std::array<double, 3>& d_condition, double* const ptr_x,
    double* const ptr_y, double* const ptr_theta, double* const ptr_kappa,
    double* const ptr_v, double* const ptr_a) {
  CHECK(std::abs(rs - s_condition[0]) < 1.0e-6)
      << "The reference point s and s_condition[0] don't match";

  const double cos_theta_r = std::cos(rtheta);
  const double sin_theta_r = std::sin(rtheta);

  *ptr_x = rx - sin_theta_r * d_condition[0];
  *ptr_y = ry + cos_theta_r * d_condition[0];

  const double one_minus_kappa_r_d = 1 - rkappa * d_condition[0];

  const double tan_delta_theta = d_condition[1] / one_minus_kappa_r_d;
  const double delta_theta = std::atan2(d_condition[1], one_minus_kappa_r_d);
  const double cos_delta_theta = std::cos(delta_theta);

  *ptr_theta = common::math::NormalizeAngle(delta_theta + rtheta);

  const double kappa_r_d_prime =
      rdkappa * d_condition[0] + rkappa * d_condition[1];
  *ptr_kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *
                 cos_delta_theta * cos_delta_theta) /
                    (one_minus_kappa_r_d) +
                rkappa) *
               cos_delta_theta / (one_minus_kappa_r_d);

  const double d_dot = d_condition[1] * s_condition[1];
  *ptr_v = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *
                         s_condition[1] * s_condition[1] +
                     d_dot * d_dot);

  const double delta_theta_prime =
      one_minus_kappa_r_d / cos_delta_theta * (*ptr_kappa) - rkappa;

  *ptr_a = s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +
           s_condition[1] * s_condition[1] / cos_delta_theta *
               (d_condition[1] * delta_theta_prime - kappa_r_d_prime);
}

double CartesianFrenetConverter::CalculateTheta(const double rtheta,
                                                const double rkappa,
                                                const double l,
                                                const double dl) {
  return common::math::NormalizeAngle(rtheta + std::atan2(dl, 1 - l * rkappa));
}

double CartesianFrenetConverter::CalculateKappa(const double rkappa,
                                                const double rdkappa,
                                                const double l, const double dl,
                                                const double ddl) {
  double denominator = (dl * dl + (1 - l * rkappa) * (1 - l * rkappa));
  if (std::fabs(denominator) < 1e-8) {
    return 0.0;
  }
  denominator = std::pow(denominator, 1.5);
  const double numerator = rkappa + ddl - 2 * l * rkappa * rkappa -
                           l * ddl * rkappa + l * l * rkappa * rkappa * rkappa +
                           l * dl * rdkappa + 2 * dl * dl * rkappa;
  return numerator / denominator;
}

Vec2d CartesianFrenetConverter::CalculateCartesianPoint(const double rtheta,
                                                        const Vec2d& rpoint,
                                                        const double l) {
  const double x = rpoint.x() - l * std::sin(rtheta);
  const double y = rpoint.y() + l * std::cos(rtheta);
  return Vec2d(x, y);
}

double CartesianFrenetConverter::CalculateLateralDerivative(
    const double rtheta, const double theta, const double l,
    const double rkappa) {
  return (1 - rkappa * l) * std::tan(theta - rtheta);
}

double CartesianFrenetConverter::CalculateSecondOrderLateralDerivative(
    const double rtheta, const double theta, const double rkappa,
    const double kappa, const double rdkappa, const double l) {
  const double dl = CalculateLateralDerivative(rtheta, theta, l, rkappa);
  const double theta_diff = theta - rtheta;
  const double cos_theta_diff = std::cos(theta_diff);
  const double res = -(rdkappa * l + rkappa * dl) * std::tan(theta - rtheta) +
                     (1 - rkappa * l) / (cos_theta_diff * cos_theta_diff) *
                         (kappa * (1 - rkappa * l) / cos_theta_diff - rkappa);
  if (std::isinf(res)) {
    AWARN << "result is inf when calculate second order lateral "
             "derivative. input values are rtheta:"
          << rtheta << " theta: " << theta << ", rkappa: " << rkappa
          << ", kappa: " << kappa << ", rdkappa: " << rdkappa << ", l: " << l
          << std::endl;
  }
  return res;
}

总结

本文为学习总结笔记，主要参考下述大神博主的讲解，进行归纳整合，如涉及侵权，请联系删除。

参考博客：

1.Frenet坐标系与Cartesian坐标系互转（一）：公式推导_frenet坐标系转换-CSDN博客

2.轨迹规划1：Frenet坐标转化公式推导_frenet坐标系参数转化-CSDN博客

3.Apollo项目坐标系研究_flu坐标系-CSDN博客

4.https://adamshan.blog.csdn.net/article/details/80779615

5.【自动驾驶】Frenet坐标系与Cartesian坐标系（二）_frenet和cartesian坐标转换实例-CSDN博客

6.基于Frenet坐标系的无人车轨迹规划详解与实现_frenet坐标系csdn-CSDN博客

自动驾驶小卡
关注
专栏目录
从Cartesian坐标系向Frenet坐标系转换的匹配点计算——向量投影法
昔风不起，唯有努力生存！
09-13 515
曲线上离该点最近的点，被称为该点在Frenet坐标系曲线上的匹配点。在自动驾驶系统中，我们一般采用一系列顺序拼接的局部参数曲线【直线段、圆曲线、螺旋曲线、多项式曲线等】来表示一条道路或车道的走势。但是我们不会直接用这些参数曲线来构建Frenet坐标系，而是采用这些曲线生成的点序列。本质原因有二：1）求匹配点需要求点到曲线的最短距离，点到参数化曲线的距离公式不统一，甚至对于某些曲线并没有一个所谓的最短距离公式；
Frenet坐标系与Cartesian坐标系互转（三）：应用示例
昔风不起，唯有努力生存！
05-19 4287
已知一个U-turn道路中心线的参数化曲线如下所示： {x(s)=−1.205e−05s5+0.0004733s4−0.0008037s3−0.09783s2−0.002081s+5y(s)=−5.383e−19s5+0.000294s4−0.009237s3+0.009687s2+0.9875s+0.0007563\left\{\begin{matrix}x(s)=&-1.205e-05 s^5 + 0.0004733 s^4 - 0.0008037 s^3 - 0.09783 s^2 - 0.0
01Frenet与Cartesian坐标系(基础知识)
穿越临界点的博客
09-17 1086
Frenet坐标系与Cardesian坐标系
动手学运动规划:1.5 Frenet坐标系
最新发布
自动驾驶小白说
09-23 1983
All tragedy erased, I see only wonders. 我忘却了所有的悲剧，所见皆是奇迹。—Hollow Knight 空洞骑士🏰：请参考!Frenet坐标系是一种在几何学和物理学中常用的坐标系，特别是在轨迹规划和机器人控制中，它能够更直观地表示车辆在弯曲道路上的位置。
frenet坐标系与笛卡尔坐标系转换公式
yanfang7722的博客
03-25 1169
即沿参考线上的里程s，其相对时间的一二阶导数，横向偏移l，其相对s的一二阶导数。选择指引线上距离车辆位置（x，y）最近点的的点做参考点，参考点插值投影得。和reference line，根据指引线插值得到s_r对应参考点状态。，即参考点的累计长度s，位置xy，航向角，曲率，曲率对s导数。，即车辆位置x,y，航向角，曲率，速度，加速度。，和reference line，求参考点状态。frenet坐标系转笛卡尔坐标系。笛卡尔坐标系转frenet坐标系。
Frenet坐标系与Cartesian坐标系互转（一）：公式推导
热门推荐
昔风不起，唯有努力生存！
09-23 4万+
Frenet坐标系在无人驾驶领域被普遍使用，特别是在城市、高速等道路交通环境下无人驾驶的路径规划系统中。Frenet坐标系使用道路的中心线作为Base frame，使用参考线的切线向量和法线向量建立坐标系。相比笛卡尔坐标系，Frenet坐标系简化了路径规划问题。网上关于Frenet坐标系的博客或资料很多，此处不再赘述。本文先给出简单的转换公式推导过程，然后给出三对Frenet坐标系与Cartesian坐标系互转的python代码与使用示例方便自己查阅使用。详细推导参见博客Apollo项目坐标系研究。not.
Frenet坐标系及其与笛卡尔坐标的转换(1)——原理
qq_24561095的博客
05-16 4653
目录 文章目录目录1 Frenet坐标系2 Frenet坐标系基本概念3 位置：笛卡尔与Frenet坐标点位置的转换3.1 位置：笛卡尔坐标 —> Frenet坐标3.1.1 求解 s∗s^{\ast}s∗ , PMPMPM 长度最短的第一种方法称为二阶最小化方法3.1.2 求解 s∗s^{\ast }s∗ , 求PMPMPM 长度最短的第二种方法称为牛顿法3.1.3 两种方法比较3.1.4 求解 l∗l^{\ast }l∗3.2 位置：Frenet坐标 —> 笛卡尔坐标4 运动状态：笛卡尔
【自动驾驶】Frenet坐标系与Cartesian坐标系（一）
weixin_42301220的博客
06-09 8173
一般情况下，我们使用Cartesian坐标系（笛卡尔坐标系）来描述物体的坐标，但对于车辆来说，笛卡尔坐标系并不是最佳选择。因为即使知道了笛卡尔坐标系下车辆的位置信息，也难以表达车辆与道路之间的相对位置，导致二者之间的相对关系不明确。因此，传统规划算法在笛卡尔坐标系下规划出的路径对于开放道路有良好的效果，但是对于公路环境，忽略车道信息导致路径的自由度太高而容易违反道路交通规则。Frenet坐标系在无人驾驶领域被普遍使用，特别是在城市、高速等道路交通环境下无人驾驶的路径规划系统中。Frenet坐标系使用道路的中
frenet坐标与cartesian坐标相互转换与代码实现（第一部分）
jianmo的博客
02-26 1187
frenet坐标与cartesian坐标相互转换与代码实现
一种基于Frenet坐标系的优化轨迹动作规划方法.pdf
07-12
本文探讨的是一种基于Frenet坐标系的优化轨迹动作规划方法，这种方法在高速驾驶和无人驾驶场景中表现出高度的实用性和效率。Frenet坐标系是由BMW的Moritz Werling提出的，因此在后续讨论中，我们也将其称为Werling...
frenet坐标系与cartesian
06-28
Frenet坐标系和Cartesian坐标系都是用于描述物体在空间中的位置和运动状态的坐标系。 Frenet坐标系是一种基于曲线的坐标系，它描述了物体在曲线上的位置和运动状态。它由三个坐标轴组成：切线轴、法线轴和副法线轴...
【自动驾驶】Frenet坐标系与Cartesian坐标系（二）
weixin_42301220的博客
06-09 3782
11. 求vxv_xvx​12. 求θx\theta_xθx​13. 求axa_xax​14. 求κx\kappa_xκx​15. 总结在Frenet坐标系下，经规划得到横向、纵向运动轨迹后，需要将其重新映射到全局笛卡尔坐标系，以供控制模块调用。如图 所示, 受车辆运动学、动力学特性及道路环境 (避障) 限制, 车辆的实际行驶轨迹与参考线难以重合。图中主要参数说明首先, 由于 t⃗,n⃗\mathrm{\vec{t}}, \mathrm{\vec{n}}t,n 都是单位向量且有: {t⃗r=[cos⁡θr,
TF坐标系变换
robots的博客
09-27 1823
TF坐标系变换1. 概述2. 代码3. 最终结果4. 参考链接 1. 概述 使用代码实例来理解tf发布信息中的位姿变换具体含义：在父坐标系下描述父坐标系到子坐标系的变换关系，希望能帮助大家，同时给自己一个参考。 2. 代码 参考代码如下 #include <ros/ros.h> #include <tf/transform_broadcaster.h> int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_b
规划决策算法（四）---Frenet坐标系
许挂挂的博客
07-24 2199
参考线模块
Frenet坐标转换
qq_39429669的博客
08-31 195
【自动驾驶】运动规划丨轨迹规划丨Frenet坐标转换 - summer的文章 - 知乎 https://zhuanlan.zhihu.com/p/109193953
自动驾驶规划算法基础第三节-frenet与笛卡尔cartesian坐标系的转换
qq_41655797的博客
03-25 2440
该笔记来自b站up主（偶像）：憨厚老实的老王
【自动驾驶轨迹规划之Frenet坐标系】
weixin_65089713的博客
04-18 5078
目录 1 结构化道路场景 2 参考线的生成 2.1 车道中心线曲率不一定连续 2.2 生成平滑参考线 2.3 离线计算 3 Frenet坐标系 3.1Frenet坐标系的意义 3.2Frenet坐标与笛卡尔坐标的转换 3.2.1 笛卡尔坐标系下的车辆运动状态 3.2.2 Frenet坐标系下的车辆运动状态 3.2.3 Frenet坐标系与笛卡尔坐标系之间车辆运动状态量的转换 4 Frenet坐标系的局限性 1 结构化道路场景 与复杂的非结构化泊车场景相比，结构化道路场景的..
Frenet坐标系与Cartesian坐标系互转（二）：Python代码函数实现
昔风不起，唯有努力生存！
05-19 4896
1D [x,y]→[s,d][x, y]\rightarrow[s, d][x,y]→[s,d] import numpy as np from math import * def cartesian_to_frenet1D(rs, rx, ry, rtheta, x, y): s_condition = np.zeros(1) d_condition = np.zeros(1) dx = x - rx dy = y - ry cos_theta_
Frenet坐标系
qq_31329259的博客
08-23 1574
Frenet坐标系
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值