Frenet坐标系与Cartesian坐标系相互转换(盲点整合)

文章目录


前言

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博客

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博客

  • 27
    点赞
  • 40
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
### 回答1: 坐标系 Frenet坐标系Cartesian坐标系都是用于描述物体在空间中的位置和运动状态的坐标系。 Frenet坐标系是一种基于曲线的坐标系,它描述了物体在曲线上的位置和运动状态。它由三个坐标轴组成:切线轴、法线轴和副法线轴。切线轴指向曲线的切线方向,法线轴指向曲线的凸侧,副法线轴与切线轴和法线轴垂直,指向曲线的弯曲方向。 Cartesian坐标系是一种基于直角坐标系坐标系,它描述了物体在三维空间中的位置和运动状态。它由三个坐标轴组成:x轴、y轴和z轴。x轴和y轴在水平面内,z轴垂直于水平面。 两种坐标系都有各自的优缺点,选择使用哪种坐标系取决于具体的应用场景和需求。 ### 回答2: Frenet坐标系Cartesian坐标系都是数学中常见的坐标系。它们都可以用来描述物体在不同的状态下的位置和运动状态。不同之处在于它们的定义和使用方法略有不同。 Frenet坐标系,也称为切向量坐标系,是一种基于曲线的坐标系,用于描述物体在曲线上的位置和运动状态。Frenet坐标系的三个坐标轴分别是曲线的切向量、法向量和副法向量。其中,切向量是曲线在任意一点的切线方向,法向量是垂直于切向量的向量,而副法向量是垂直于切向量和法向量的向量。这种坐标系的优点在于能够有效地描述曲线的弯曲和转向情况,使得计算曲线上任意一点的速度和加速度等物理量更加方便。 相比之下,Cartesian坐标系是一种基于直线的坐标系,用于描述物体在直角坐标系中的位置和运动状态。在这种坐标系中,物体的三个坐标轴分别是x、y、z轴,它们垂直于彼此形成一个直角三角形。这种坐标系的优点在于计算简单,容易理解。但它不能有效地描述曲线的特性,因为在直线上一些物理量的计算需要涉及到微积分和向量的运算,这使得计算比较麻烦并且容易出现误差。 总的来说,Frenet坐标系Cartesian坐标系各有优缺点,具体使用哪种坐标系取决于需要描述物体的特性和应用场景。在实际应用中,这两种坐标系常常相互转换,以便更好地描述物体在运动过程中的位置和状态。 ### 回答3: Frenet坐标系是一种曲线坐标系,用于描述空间中的曲线。与笛卡尔坐标系不同,Frenet坐标系以曲线的切线、法向和副法向为基准,通过参数化曲线的方式来描述曲线的位置。因此,Frenet坐标系可以更好地描述曲线的几何性质,例如曲率和扭率等。 在Frenet坐标系中,曲线的位置可以由三个参数来描述,即曲线的弧长s、曲率k和扭率τ。这三个参数分别对应着曲线的沿切线变化、沿法向变化和沿副法向变化。通过这些参数,可以准确地描述曲线的位置和方向,而且可以轻松地计算曲线上任意点的切线、法向和副法向。 相比之下,笛卡尔坐标系则是以直角坐标系为基准,使用x、y和z三个坐标轴来描述空间的位置。尽管笛卡尔坐标系在描述平面和立体图形时比较简便,但当描述曲线的时候就显得比较复杂。因为曲线的位置和方向不能通过单一的坐标轴来描述,必须使用更复杂的公式和计算方法。 总的来说,Frenet坐标系相对于笛卡尔坐标系来说,在描述曲线几何性质上更加直观和准确。在实际应用中,Frenet坐标系有广泛的应用,例如在计算机图形学、机器视觉等领域中,以及在车辆导航和机器人路径规划中也有广泛的应用。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值