【自动驾驶轨迹规划之Frenet坐标系】

欢迎大家关注我的B站：

郑同学的个人空间以其独特的风格在B站上广受关注。该个人空间以其独特的风格著称，在B站拥有大量粉丝，并以其幽默风趣的内容风格迅速走红网络。其视频内容不仅涵盖日常生活分享还融入了许多创新元素令人印象深刻；访问量持续攀升互动活跃度也在逐步提升体现出其在B站上的积极影响力

目录

1 结构化道路场景

2 参考线的生成

2.1 车道中心线曲率不一定连续

2.2 生成平滑参考线

2.3 离线计算

3 Frenet坐标系

3.1 Frenet坐标系的意义

3.2 Frenet坐标与笛卡尔坐标的转换

3.2.1 笛卡尔坐标系下的车辆运动状态

3.2.2 Frenet坐标系下的车辆运动状态

3.2.3 Frenet坐标系与笛卡尔坐标系之间车辆运动状态量的转换

4 Frenet坐标系的局限性

1 结构化道路场景

在具有复杂性的非结构化停车场环境当中进行泊车操作相较于较为简单的道路场景而言存在显著的难度。具体来说，在传统的道路行驶模式下驾驶员能够根据明确的道路标线直接控制车辆运行方向而无需频繁调整行驶策略以适应周围环境的变化而在此类复杂环境下车辆必须经历多次局部前进步骤和倒车操作才能完成精准的终止位姿衔接。这种高频率的操作不仅增加了任务难度还可能导致微小的变化就可能导致较大的轨迹偏差因此难以实现可靠的自适应泊车功能。相比之下在典型的结构化道路场景中驾驶员的操作空间会受到严格限制特别是在掉头这一动作上会被明确禁止这种严格的规定使得车辆的航向角始终保持在与道路延伸方向一致的状态下从而保证了行车安全并使得车道偏离的风险降至最低值在这种情况下行车参考线得以建立并以车道中心线为基准逐步构建

2 参考线的生成

2.1 车道中心线曲率不一定连续

但是将参考线设定为车道中心线仅为直观上较为合理 的选择，在实际应用中由于车道中心线可能具有不连续的曲率特征，则需要在基于原始车道中心线的基础上去生成一个具有连续曲率的理想参考线。

2.2 生成平滑参考线

Apollo的参考线平滑技术分为三种类型：QpSplineReferenceLineSmoother、SpiralReferenceLineSmoother和DiscretePointsReferenceLineSmoother。在本节中，我们将深入解析散点式的参考线平滑处理方法。

Apollo的参考线平滑技术分为三种类型：QpSplineReferenceLineSmoother、SpiralReferenceLineSmoother和DiscretePointsReferenceLineSmoother。在本节中，我们将深入解析散点式的参考线平滑处理方法。

第一步操作：从车辆后方开始截取一段连续一定长度的车道中心线，并在其上进行采样操作。具体来说，在车道中心线上采样出(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;
    
 }
    
    
    
    
    AI助手

（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;
    
   }
    
    
    
    
    AI助手

参考点间距成本：衡量生成的参考点与其对应的目标点之间的位置偏差。该指标旨在使其波动幅度较小，并尽可能靠近车道中线。公式如下

 //第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);
    
   }
    
    
    
    
    AI助手

平滑性代价：让参考线足够平滑，公式如下

即为第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]));
    
   }
    
   }
    
    
    
    
    AI助手

总长度代价：防止参考线为了保证平滑，而路径过长，公式如下

 //第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;
    
 }
    
    
    
    
    AI助手

总代价函数（自行调整权重）：

边值约束在整体设计中起着至关重要的作用，在曲线拼接过程中等同于将两个指引线段进行连接，并通过确保整体过渡光滑来实现目标曲线的平滑连接。

在完成目标函数和约束条件的构建后，可运用二次规划（QP）方法求取最优解。具体内容超出了本文的讨论范围。

2.3 离线计算

需要强调的是，在生成上述参考线时需基于高质量的道路地图数据提供车道中心线信息 ，即与实时交通状况无直接关联性，在离线环境下即可完成相关计算工作。由此可见，在实际应用中该算法无需面对实时数据输入的限制，并且其运行效率也无需受到处理延迟的影响。

3 Frenet坐标系

3.1 Frenet坐标系的意义

Frenet方法通过将车辆在某时刻的运动轨迹投影到参考线上，并将其划分为横向与纵向两个维度来描述运动特性。该种方式有助于简化后续的轨迹规划工作。

（2）大多数道路呈现曲折特征。因此，在笛卡尔坐标系下描述这些复杂的道路形态会产生较大的困难。在笛卡尔坐标系下表示道路信息时，若采用低阶多项式模型将会导致较大的建模误差；而使用高阶多项式的计算负担较重。尽管如此，在Frenet框架下对道路曲率进行简化处理。

Frenet系统能够精确且易于理解地刻画车辆相对于道路的运动状态。对于遵循交通法规的部分路段而言，这一方法具有显著的优势与应用价值。

s代表基于参考线的纵向位移，d代表偏离参考线的横向位移

3.2 Frenet坐标与笛卡尔坐标的转换

3.2.1 笛卡尔坐标系下的车辆运动状态

任意时刻 t 的车辆运动状态可以描述为 ,其中

为了提高表达的清晰度，在保持原有含义的基础上对文本进行了改写

为了提高表达的清晰度，在保持原有含义的基础上对文本进行了改写

；

为方位角；

为曲率， 其计算公式为

， 其中,

为

在 Q 处的切线转角，

为

在 Q 处的弧长；

为速 度；

为加速度。

为位置 Q 投影到参考线上的点 P 在全局坐标系下的位置向量。

3.2.2 Frenet坐标系下的车辆运动状态

在 Frenet 坐标系下，车辆的运动状态可以描述为

。其中，

为纵向位移；

为纵向速 度；

为纵向加速度；

为横向位移；

为横向速度；

为横向加速度；

为横向位移对弧长的一阶导数；

为横向位移对弧长的二阶导数。

3.2.3 Frenet坐标系与笛卡尔坐标系之间车辆运动状态量的转换

如需了解详细的公式的推导过程，请参考轨迹规划1：Frenet坐标转化公式推导_CK.Ren的博客-博客_frenet公式

4 Frenet坐标系的局限性

确定该点在Frenet坐标系中的位置等价于以该点为圆心并从小到大逐渐增加半径绘制一系列圆弧，在某一个圆与参考曲线仅相交于一点时（如图1所示），这个唯一的交点即为此处该曲线上的垂足（perpendicularfoot）；此时对应的半径值即定义为横向偏移量d。需要注意的是，在这种情况下（如图2所示），当一段连续可微分的参考曲线是一条以当前分析对象为中心的一段圆弧时，则会存在无穷多个垂足位于同一位置上；这使得基于笛卡尔坐标的逆变换关系不再满足一一映射的要求；因此在这种情况下（如图3所示），我们定义了一个有限的有效表征范围（finiteeffective range），其大小取决于所考察曲线段所在空间区域内的弯曲程度；换句话说，在距离基准曲线较远的位置上将无法得到有效的Frenet坐标描述；这种现象在车辆行驶过程中涉及较大转弯半径的情况中表现得尤为明显；针对这种情况（如图4所示），我们需要采取相应的处理措施或者直接采用笛卡尔坐标系来进行表示以避免信息丢失的问题。