目录
3.2.3 Frenet坐标系与笛卡尔坐标系之间车辆运动状态量的转换
1 结构化道路场景
与复杂的 非结构化泊车场景相比，结构化道路场景的最本质区别是其包含着一条 行车参考线。在泊车场景中，车辆可能需要 多次局部前进和后退才能让精准衔接终止位姿，而环境中的 障碍物位置稍有变化就会对泊车轨迹产生较大影响，由于泊车场景复杂多变，一般难以提前构造出一条能够引导车辆泊车的参考线。相比之下，结构化的道路场景更加规则，并且车辆在结构化道路场景中行驶不允许出现倒车，因此车辆的航向角也不会显著偏离道路延伸方向，行车轨迹也不会显著偏离车道中心线方向。因此行车参考线以 车道中心线为基础。
2 参考线的生成
2.1 车道中心线曲率不一定连续
但是将参考线设置为车道中心线只是 直观上较合理，实际上，车道中心线的曲率未必始终连续，因此我们需要在 基于原始车道中心线去 生成一个曲率连续的参考线。
2.2 生成平滑参考线
Apollo的参考线平滑有三种：QpSplineReferenceLineSmoother,SpiralReferenceLineSmoother和DiscretePointsReferenceLineSmoother。本节结合Apollo源码解析 散点参考线平滑。
（1）首先，截取车辆未 来一段特定长度的车道中心线，并在其上 采样 (N+1) 个等距或随机初始化 路途点，并记录在点集中
，我们需要生成的 参考点记为
//初始化点
bool CosThetaIpoptInterface::get_starting_point(int n, bool init_x, double* x,
bool init_z, double* z_L,
double* z_U, int m,
bool init_lambda,
double* lambda) {
CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
std::random_device rd;
std::default_random_engine gen = std::default_random_engine(rd());
std::normal_distribution<> dis{0, 0.05};
//在原始参考点基础上添加随机数初始化点
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i << 1;
x[index] = ref_points_[i].first + dis(gen);
x[index + 1] = ref_points_[i].second + dis(gen);
}
return true;
}
</size_t>
（2）建立 参考线规划命题，它包含代价函数与约束两部分，代价函数由三部分构成，参考点距离代价、平滑性代价、总长度代价。约束条件主要是边值约束。
//代价函数
bool CosThetaIpoptInterface::eval_f(int n, const double* x, bool new_x,
double& obj_value) {
CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
if (use_automatic_differentiation_) {
eval_obj(n, x, &obj_value);
return true;
}</size_t>
参考点距离代价：代表生成的参考点与路途点之间的波动范围，目的让其波动范围小，尽量靠近车道中心线，公式如下
//第1部分——与参考线的距离
obj_value = 0.0;
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i << 1;
obj_value +=
(x[index] - ref_points_[i].first) * (x[index] - ref_points_[i].first) +
(x[index + 1] - ref_points_[i].second) *
(x[index + 1] - ref_points_[i].second);
}
平滑性代价：让参考线足够平滑，公式如下
这相当于第 k 个点与第 k+1 个点连成的线段与第 k+1 个点与第 k+2 个点连成的线段之间的夹角足够小，也就是接近一条直线
//第2部分——cos_theta
for (size_t i = 0; i < num_of_points_ - 2; i++) {
size_t findex = i << 1;
size_t mindex = findex + 2;
size_t lindex = mindex + 2;
obj_value -=
weight_cos_included_angle_ *
(((x[mindex] - x[findex]) * (x[lindex] - x[mindex])) +
((x[mindex + 1] - x[findex + 1]) * (x[lindex + 1] - x[mindex + 1]))) /
std::sqrt((x[mindex] - x[findex]) * (x[mindex] - x[findex]) +
(x[mindex + 1] - x[findex + 1]) *
(x[mindex + 1] - x[findex + 1])) /
std::sqrt((x[lindex] - x[mindex]) * (x[lindex] - x[mindex]) +
(x[lindex + 1] - x[mindex + 1]) *
(x[lindex + 1] - x[mindex + 1]));
}
}
总长度代价：防止参考线为了保证平滑，而路径过长，公式如下
//第3部分——总长度
for (size_t i = 0; i < num_of_points_ - 1; ++i) {
size_t findex = i << 1;
size_t nindex = findex + 2;
*obj_value +=
weight_length_ *
((x[findex] - x[nindex]) * (x[findex] - x[nindex]) +
(x[findex + 1] - x[nindex + 1]) * (x[findex + 1] - x[nindex + 1]));
}
return true;
}
总代价函数（自行调整权重）：
边值约束是比较关键的，边值约束相当于是 衔接两段指引线，保证能够曲率连续。
构建完代价函数与约束条件后，需要用到二次规划（QP）去求得最优解，本篇文章不再赘述。
2.3 离线计算
需要说明的是，上述参考线的生成 依赖于高精地图提供的车道中心线信息，也就是和实时路况没有关系，可以 离线完成计算，所以计算实时性没有要求。
3 Frenet坐标系
3.1 Frenet坐标系的意义
（1）Frenet可以将车辆某一时刻的轨迹投影到参考线上，并 分解成横向和纵向两个维度的运动，可以简化后面的轨迹规划工作。
（2）通常道路都是曲折的，这样的话用 笛卡尔坐标系描述道路会非常复杂。用笛卡尔坐标系去表达道路信息，低阶多项式误差大，高阶多项式计算量大。但frenet可以忽略道路曲率。
（3）Frenet可以将 车辆相对道路的运动描述的非常清晰直观。对于 遵守交通规则个非常有帮助。
s代表基于参考线的纵向位移，d代表偏离参考线的横向位移
3.2 Frenet坐标与笛卡尔坐标的转换
3.2.1 笛卡尔坐标系下的车辆运动状态
任意时刻 t 的车辆运动状态可以描述为
,其中 为车辆当前位置 Q， 表示车辆在全局坐标系下的位置信息 (x, y)， 也可用相对于参考线的纵向位移 s 和横向位移 d 来 描述， 即 ； 为方位角； 为曲率， 其计算公式为 ， 其中, 为 在 Q 处的切线转角， 为 在 Q 处的弧长； 为速 度； 为加速度。 为位置 Q 投影到参考线上的点 P 在全局坐标系下的位置向量。
3.2.2 Frenet坐标系下的车辆运动状态
在 Frenet 坐标系下，车辆的运动状态可以描述为
。其中， 为纵向位移； 为纵向速 度；为纵向加速度；为横向位移；为横向速度； 为横向加速度；为横向位移对弧长的一阶导数；为横向位移对弧长的二阶导数。
3.2.3 Frenet坐标系与笛卡尔坐标系之间车辆运动状态量的转换
具体的公式推导请参考轨迹规划1：Frenet坐标转化公式推导_CK.Ren的博客-CSDN博客_frenet公式
4 Frenet坐标系的局限性
寻找一个点在Frenet坐标系下的位置，其实就是以这个点为圆心，半径从零开始增大，画出无数个圆，直到 有一个圆与参考线有唯一一个交点时，这个交点就是这个点向参考线的投影点，半径就是横向位移 d。Frenet坐标系未必能够把二维平面中的所有点进行有效的描述，当一段参考线是以这个点为圆心的圆弧时，那么这个点向参考线作投影点就有无数个，这时就不满足笛卡尔坐标系向Frenet坐标系转换时一一映射的关系了，所以Frenet坐标系存在一个有效表征范围，这个 有效表征范围与参考线的曲率有关系，也就意味着离参考线较远的位置无法用Frenet坐标系表达，该现象在曲率较大的行车场景中尤为显著，此时需要采取额外处理，或者直接采样笛卡尔坐标系。
Original: https://blog.csdn.net/weixin_65089713/article/details/124245049
Author: 无意2121
Title: 【自动驾驶轨迹规划之Frenet坐标系】
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/597429/
转载文章受原作者版权保护。转载请注明原作者出处！