Apollo星火计划学习笔记——参考线平滑算法解析及实现（以U型弯道场景仿真调试为例）

前言

Apollo星火计划课程链接如下
星火计划2.0基础课：https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课：https://apollo.baidu.com/community/online-course/12

1. Apollo参考线介绍

1.1 参考线的作用

1.2 导航规划的路线

    导航规划的路线一般由三个部分组成：Routing、参考线以及轨迹。
    Routing： 全局路径规划的结果，一般规划出来的长度为几十公里甚至上百公里，若没有障碍物，车辆将会沿着这条路径一直行驶。若有障碍物，则会由Planning模块发布回Routing模块进行重新规划。
    参考线： 决策规划模块根据车辆的实时位置进行计算。参考线的计算依赖于Routing的结果，但同时需要车辆周围的动态信息（障碍物、交通规则）。参考线相当于Routing结果里局部数据（车辆当前位置的一段Routing），距离通常只有几百米。参考线可能会有多条。
    轨迹： 是规划决策模块的最终输出结果。轨迹的输入是参考线。轨迹不仅包含车辆的路径信息，也包括车辆的动态信息（速度、加速度等）。

1.3 为什么需要重新生成参考线

1.4 ReferenceLine数据结构

优先级：当有多条参考线时，具有优先级。
SpeedLimit：车辆在一段参考线里的速度限制。起始点的速度限制/终点的限制
speed_limit_：是一个数组，分段限速。
reference_points_：参考点，同样是数组。
map_path_:地图中的参考线，与planning中的参考线具有一一映射关系，参考点的数目是相同的。

1.5 ReferencePoint数据结构

     ReferencePoint继承自MapPathPoint,MapPathPoint继承自Vec2d。
Vec2d:包含了点的二维数据。
MapPathPoint:包含了点的朝向以及地图上的点对应道路上的点。
ReferencePoint:包含了曲线的曲率与曲率的导数。

1.6 参考线处理流程

     参考线的处理主要涉及两个线程：一个是参考线提供线程，另一个是规划线程。参考线提供线程提供参考线给规划线程去规划出一条轨迹。参考线的处理有两个步骤：首先是参考线的生成，直行时生成一条参考线，当有变道发生时，生成多条参考线。接下来是参考线的平滑，常见的参考线平滑方式有：离散点的平滑（Apollo采用）、螺旋线的平滑以及样条曲线的平滑。

1.7 参考线生成

     参考线生成是由CreateReferenceLine函数实现的。首先由UpdateVehicleState函数获取车辆的状态，再由UpdateRoutingResponse函数获取Routing的结果，生成RouteSegment。若routing是新生成的，则对其进行平滑与分割；若不是，则沿用上一个周期的参考线，并对其进行扩展与延伸。

2. 参考线平滑算法

2.1 参考线平滑算法总览

     常见的参考线平滑方式有：离散点的平滑（Apollo采用）、螺旋线的平滑以及样条曲线的平滑。

// /home/yuan/apollo-edu/modules/planning/reference_line/reference_line_provider.cc
  if (smoother_config_.has_qp_spline()) {
    smoother_.reset(new QpSplineReferenceLineSmoother(smoother_config_));
  } else if (smoother_config_.has_spiral()) {
    smoother_.reset(new SpiralReferenceLineSmoother(smoother_config_));
  } else if (smoother_config_.has_discrete_points()) {
    smoother_.reset(new DiscretePointsReferenceLineSmoother(smoother_config_));
  } else {
    ACHECK(false) << "unknown smoother config "
                  << smoother_config_.DebugString();
  }

2.2 参考线平滑算法流程

// /home/yuan/apollo-edu/modules/planning/reference_line/reference_line_provider.cc
bool ReferenceLineProvider::SmoothReferenceLine(
    const ReferenceLine &raw_reference_line, ReferenceLine *reference_line) {
  if (!FLAGS_enable_smooth_reference_line) {
    *reference_line = raw_reference_line;
    return true;
  }
  // generate anchor points:
  std::vector<AnchorPoint> anchor_points;
  GetAnchorPoints(raw_reference_line, &anchor_points);
  smoother_->SetAnchorPoints(anchor_points);
  if (!smoother_->Smooth(raw_reference_line, reference_line)) {
    AERROR << "Failed to smooth reference line with anchor points";
    return false;
  }
  if (!IsReferenceLineSmoothValid(raw_reference_line, *reference_line)) {
    AERROR << "The smoothed reference line error is too large";
    return false;
  }
  return true;
}

GetAnchorPoints(raw_reference_line, &anchor_points);用于设置设置中间点
if (!smoother_->Smooth(raw_reference_line, reference_line)) {为平滑算法

2.2.1 设置中间点anchor points

// /home/yuan/apollo-edu/modules/planning/reference_line/reference_line_smoother.h
struct AnchorPoint {
  common::PathPoint path_point;
  double lateral_bound = 0.0;
  double longitudinal_bound = 0.0;
  // enforce smoother to strictly follow this reference point
  bool enforced = false;
};

     根据ReferenceLine选取中间点。根据ReferenceLine s（纵向距离上的投影）的值，进行均匀采样，采样的间隔约为0.25m。采样之后得到一系列的AnchorPoint，每个AnchorPoint包含PathPoint、横纵向的裕度以及是否为强约束的判断（若是，则必须为满足横纵向裕度，一条参考线只有首尾的点是强约束）。

// /home/yuan/apollo-edu/modules/planning/conf/discrete_points_smoother_config.pb.txt
max_constraint_interval : 0.25
longitudinal_boundary_bound : 2.0
max_lateral_boundary_bound : 0.5
min_lateral_boundary_bound : 0.1
curb_shift : 0.2
lateral_buffer : 0.2

discrete_points {
  smoothing_method: FEM_POS_DEVIATION_SMOOTHING
  fem_pos_deviation_smoothing {
    weight_fem_pos_deviation: 1e10
    weight_ref_deviation: 1.0
    weight_path_length: 1.0
    apply_curvature_constraint: false
    max_iter: 500
    time_limit: 0.0
    verbose: false
    scaled_termination: true
    warm_start: true
  }
}

主要参数
max_constraint_interval采样间隔
longitudinal_boundary_bound纵向边界
max_lateral_boundary_bound最大横向边界
min_lateral_boundary_bound最小横向边界
curb_shift与道路边缘的缓冲，若为实线，再增加一个buffer缓冲
lateral_buffer与道路边缘的缓冲

smoothing_method: FEM_POS_DEVIATION_SMOOTHING采用FemPosSmooth平滑算法
weight_fem_pos_deviation平滑度的代价权值$cos{t}_{smooth}$
weight_ref_deviation相对原始点偏移量的代价权值$cos{t}_{err}$
weight_path_length参考线长度的代价权值$cos{t}_{length}$
实验时主要考虑平滑，所以将平滑度的权值设得非常大，而其他权值忽略不计。

2.2.2 Smooth函数

/home/yuan/apollo-edu/modules/planning/reference_line/discrete_points_reference_line_smoother.cc
     两种平滑算法通过配置文件去选择。

2.2.3FemPosSmooth

2.2.4 Solve函数

Apollo默认不考虑曲率的约束

2.3 优化问题构造和求解

     Apollo主要是对离散点进行求解，通过构造二次规划问题，利用OSQP求解器进行求解。

2.4 平滑代价

     $cos{t}_{smooth}$有两种计算方式：FemPosSmooth与CosThetaSmooth。
FemPosSmooth：

CosThetaSmooth：
    构造方式：利用相邻两个向量$P_0{P}_1$、$P_1{P}_2$的夹角$\theta$的余弦值进行构造。

2.5 约束条件

2.5.1 位置约束

约束方程：
    保证点在boundingbox之内。

2.5.2 曲率约束

曲率约束首先有以下三个假设：

通过三个假设，构造出约束方程：

$∆s$是采样平均长度，$cu{r}_{cstr}$是最大曲率约束.

3. U型道路仿真演示

云平台地址——Apollo规划之U型路口仿真调试

启动DreamView

bash scripts/apollo_neo.sh bootstrap 

如图所示选好

曲率较大的U型弯

打开 jupyter notebook，绘制脚本在/modules/tools 下

 jupyter notebook

脚本代码

import json
import sys
import argparse
import sys
import numpy as np
import matplotlib.pyplot as plt
import math
 
from cyber.python.cyber_py3.record import RecordReader
from modules.common_msgs.planning_msgs import planning_pb2
  
class RefLineInfo():
    def __init__(self, x, y, s, theta, kappa, dkappa):
        self.x = x
        self.y = y
        self.s = s
        self.theta = theta
        self.kappa = kappa
        self.dkappa = dkappa
 
def trim_path_by_distance(adc_trajectory, s):
    path_coords = []
    path_points = adc_trajectory.trajectory_point
    for point in path_points:
        if point.path_point.s <= s:
            path_coords.append([point.path_point.x, point.path_point.y])
    return path_coords
 
def project(point, ref_line):
    start_point = ref_line[0]
    end_point = start_point
    for line_point in ref_line:
        if line_point.s - ref_line[0].s < 0.2:
            continue
        point_dir = [start_point.x - point.x, start_point.y - point.y]
        line_dir = [line_point.x - start_point.x, line_point.y - start_point.y]
        dot = point_dir[0] * line_dir[0] + point_dir[1] * line_dir[1]
        if dot > 0.0:
            start_point = line_point
            continue
        s = dot / math.sqrt(line_dir[0] * line_dir[0] + line_dir[1] * line_dir[1])
        return start_point.s - s
        
 
def align_reference_lines(ref_line1, ref_line2):
    if len(ref_line1) < 2 or len(ref_line2) < 2:
        return [0.0, 0.0]
    if ref_line1[-1].s < 0.5 or ref_line2[-1].s < 0.5:
        return [0.0, 0.0]
    
    s_ref_line1 = [ref_line1[0].x, ref_line1[0].y]
    cur_index = 1
    e_ref_line1 = [ref_line1[cur_index].x, ref_line1[cur_index].y]
    while ref_line1[cur_index].s < 0.5:
        cur_index = cur_index + 1
        e_ref_line1 = [ref_line1[cur_index].x, ref_line1[cur_index].y]
        
    start_point2 = ref_line2[0]
    line_dir = [e_ref_line1[0] - s_ref_line1[0], e_ref_line1[1] - s_ref_line1[1]]
    start_dir = [s_ref_line1[0] - start_point2.x, s_ref_line1[1] - start_point2.y]
    dot = line_dir[0] * start_dir[0] + line_dir[1] * start_dir[1]
    if dot > 0.0:
        return [0.0, project(ref_line1[0], ref_line2)]
    return [project(start_point2, ref_line1), 0.0]
    
    
def get_ref_line(record_path, ref_line_index=0):
    reader = RecordReader(record_path)
    current_ref_line_index = 0
    for msg in reader.read_messages():
        if msg.topic == "/apollo/planning":
            if current_ref_line_index != ref_line_index:
                current_ref_line_index = current_ref_line_index + 1
                continue
            adc_trajectory = planning_pb2.ADCTrajectory()
            adc_trajectory.ParseFromString(msg.message)
 
            for path in adc_trajectory.debug.planning_data.path:
                if path.name != 'planning_reference_line':
                    continue
                path_coords = trim_path_by_distance(adc_trajectory, 5.0)
 
                ref_line = []
                last_theta = path.path_point[0].theta
                for point in path.path_point:
                    if point.theta - last_theta > math.pi:
                        point.theta = point.theta - 2.0 * math.pi
                    elif last_theta - point.theta > math.pi:
                        point.theta = point.theta + 2.0 * math.pi
                    ref_line.append(RefLineInfo(point.x, point.y, point.s, point.theta, point.kappa, point.dkappa))
                return ref_line
            
def plot_ref_line(start_s, ref_line, use_dot):
    x = []
    y = []
    s = []
    theta = []
    kappa = []
    dkappa = []
    scale_factor = 10.0
    for point in ref_line:
        if point.s < start_s:
            continue
        x.append(point.x)
        y.append(point.y)
        s.append(point.s)
        theta.append(point.theta)
        kappa.append(point.kappa * scale_factor)
        dkappa.append(point.dkappa * scale_factor)
    if use_dot:
        plt.plot(s, theta, 'b--', alpha=0.5, label='theta')
        plt.plot(s, kappa, 'r--', alpha=0.5, label='kappa')
        plt.plot(s, dkappa, 'g--', alpha=0.5, label='dkappa')
    else:
        plt.plot(s, theta, 'b', alpha=0.5, label='theta')
        plt.plot(s, kappa, 'r', alpha=0.5, label='kappa')
        plt.plot(s, dkappa, 'g', alpha=0.5, label='dkappa')
                    
def plot_ref_path(record_file1, record_file2):
    ref_line1 = get_ref_line(record_file1)
    ref_line2 = get_ref_line(record_file2)
    [s1, s2] = align_reference_lines(ref_line1, ref_line2)
    plot_ref_line(s1, ref_line1, True)
    plot_ref_line(s2, ref_line2, False)
    
        
record_file1 = './20221221113737.record.00000'
record_file2 = './20221221120201.record.00000'
 
plot_ref_path(record_file1, record_file2)
      
plt.legend()
plt.show()

cyber_recorder record -a记录数据

cyber_recorder record -a

我们所需要记录并绘制的数据将记录好的数据移动到./modules/tools/下

到/apollo/modules/planning/conf/planning.conf 下查看所用的是哪个平滑算法

vim /apollo/modules/planning/conf/planning.conf 

进入平滑算法的配置文件

vim /apollo/modules/planning/conf/discrete_points_smoother_config.pb.txt 

修改参数，并重启DreamView，防止数据残留。

 bash scripts/bootstrap_neo.sh restart

按照之前的步骤得到第二份数据。

按照之前的步骤得到第三份数据。
文章来源地址https://www.toymoban.com/news/detail-661374.html

到了这里，关于Apollo星火计划学习笔记——参考线平滑算法解析及实现（以U型弯道场景仿真调试为例）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！