三種常用坐标系的拉梅系數-tft每日頭條

本文鍊接地址: 「鍊接」

車在道路上行駛，以車的視角來看，車就如同在一條光滑的曲線上移動，且不時帶有左右偏移。為了算法簡單，我們選擇了Frenet坐标系，它可以把直角坐标系下的複雜軌迹轉換為隻有S，L兩個維度的簡單曲線。

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）1

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）2

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）3

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）4

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）5

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）6

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與Frenet坐标系轉換公式推導）7

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坐标系轉換公式推導）8

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）9

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）10

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）11

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）12

三種常用坐标系的拉梅系數（Cartesian與Frenet坐标系轉換公式推導）13