本文介绍了Apollo自动驾驶决策规划中使用的CartesianFrenetConverter类，详细解释了如何在笛卡尔坐标系和Frenet坐标系之间进行转换，包括一阶和二阶导数的计算，以及相关的数学公式和函数实现。
Frenet与笛卡尔坐标系的转换详细推导见：b站老王 自动驾驶决策规划学习记录（四）
Apollo相关代码：
modules/common/math/cartesian_frenet_conversion.h
#pragma once
#include <array>
#include "modules/common/math/vec2d.h"
namespace apollo {
namespace common {
namespace math {
// Notations:
// s_condition = [s, s_dot, s_ddot]
// s: 纵向坐标 w.r.t reference line.
// s_dot: ds / dt
// s_ddot: d(s_dot) / dt
// d_condition = [d, d_prime, d_pprime]
// d: 横向坐标 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;
// 将笛卡尔坐标系转换为Frenet坐标系。将一个二维运动解耦为两个独立相对于参考线的一维运动。横向运动是纵向累积距离s的函数，以更好地满足非完整约束。
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);
static void cartesian_to_frenet(const double rs, const double rx,
const double ry, const double rtheta,
const double x, const double y, double* ptr_s,
double* ptr_d);
// 将Frenet框架中的车辆状态转换为笛卡尔框架。将两个相对于参考线的独立1d移动合并为2d移动
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 Vec2d CalculateCartesianPoint(const double rtheta, const 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 math
} // namespace common
} // namespace apollo
modules/common/math/cartesian_frenet_conversion.cc
cartesian_to_frenet函数：计算出SL的一阶导和二阶导数
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;
}
cartesian_to_frenet作用是计算出SL的一阶导数和二阶导数
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)
前六个参数是中心线上投影的点的信息 ( s r , x r , y r , θ r , k r , k r ′ ) (s_r,x_r,y_r,\theta_r,k_r,k^{'}_r) (sr,xr,yr,θr,kr,kr′)，中间的六个参数是轨迹上相应点的信息 ( x , y , v , a , θ , k ) (x,y,v,a,\theta,k) (x,y,v,a,θ,k)，最后两个参数是输出，分别对应 ( s , s ˙ , s ¨ ) (s,\dot{s} ,\ddot{s} ) (s,s˙,s¨)和 ( l , l ′ , l ′ ′ ) (l,l',l'') (l,l′,l′′)
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);
std::copysign是C++标准库中的一个实用函数，用于复制一个数的符号并返回一个新的数
接受两个参数：x和y。它返回一个新的数，其值与x相同，但符号与y相同
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;
l ′ = ( 1 − k r l ) t a n ( θ x − θ r ) l'=(1-k_rl)tan(\theta_x-\theta_r) l′=(1−krl)tan(θx−θr)
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; // s
ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d; // s'
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; // s''
}
frenet_to_cartesian函数:计算笛卡尔坐标系下的轨迹点
(
x
,
y
,
θ
,
k
,
v
,
a
)
(x,y,\theta,k,v,a)
(x,y,θ,k,v,a)
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) {
ACHECK(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 = 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);
}
ACHECK函数，它是一个简单的断言函数，用于检查一个条件是否满足。如果条件不满足，它将抛出一个异常，并附带一个错误信息。在这段代码中，rs和s_condition[0]是两个浮点数，它们被用于计算参考点的位置。std::abs(rs - s_condition[0]) < 1.0e-6是一个比较表达式，用于检查rs和s_condition[0]之间的差异是否小于1.0e-6。如果这个条件不满足，ACHECK函数将抛出一个异常，并附带一个错误信息，指出参考点的位置不匹配。
ACHECK(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 = 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);
测试
CMakeList.txt
cmake_minimum_required(VERSION 3.0.2)
project(frenet_to_cartesian)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES frenet_to_cartesian
# CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(cartesian_frenet_conversian src/cartesian_frenet_conversian.cpp
src/main.cpp)
target_link_libraries(cartesian_frenet_conversian
${catkin_LIBRARIES}
)
main.cpp
#include "frenet_to_cartesian/cartesian_frenet_conversion.h"
#include <iostream>
int main(int argc, char** argv) {
double rs = 10.0;
double rx = 0.0;
double ry = 0.0;
double rtheta = M_PI / 4.0;
double rkappa = 0.1;
double rdkappa = 0.01;
double x = -1.0;
double y = 1.0;
double v = 2.0;
double a = 0.0;
double theta = M_PI / 3.0;
double kappa = 0.11;
std::array<double, 3> s_conditions;
std::array<double, 3> d_conditions;
bigdavid::CartesianFrenetConverter::cartesian_to_frenet(rs, rx, ry, rtheta, rkappa, rdkappa, x, y, v, a, theta, kappa, &s_conditions, &d_conditions);
double x_out;
double y_out;
double theta_out;
double kappa_out;
double v_out;
double a_out;
bigdavid::CartesianFrenetConverter::frenet_to_cartesian( rs, rx, ry, rtheta, rkappa, rdkappa, s_conditions, d_conditions, &x_out, &y_out, &theta_out, &kappa_out, &v_out, &a_out);
if (abs(x - x_out) < 1.0e-6) std::cout << "x match" << std::endl;
if (abs(y - y_out) > 1.0) std::cout << "y match" << std::endl; // 不匹配
if (abs(theta - theta_out) < 1.0e-6) std::cout << "theta match" << std::endl;
if (abs(kappa - kappa_out) < 1.0e-6) std::cout << "kappa match" << std::endl;
if (abs(v - v_out) < 1.0e-6) std::cout << "v match" << std::endl;
if (abs(a - a_out) < 1.0e-6) std::cout << "a match" << std::endl;
return 0;
}
1136
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
