Apollo8.0关于在规划轨迹上如何求匹配点

已于 2024-01-10 17:59:52 修改
阅读量1.2k 收藏 26
点赞数 16
分类专栏： 基于运动学模型控制算法 文章标签： 算法
于 2023-12-13 18:33:52 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zxy_zxy123/article/details/134976455
版权

概要

0.基于匹配点求解投影点的位姿信息步骤

1.首先对规划轨迹点结构内容的了解

主要是PathPoint（路经点的位姿），TrajectoryPoint结构体内容。

syntax = "proto2";

// Defined Point types that are commonly used in PnC (Planning and Control)
// modules.

package apollo.common;

message SLPoint {
  optional double s = 1;
  optional double l = 2;
}

message FrenetFramePoint {
  optional double s = 1;
  optional double l = 2;
  optional double dl = 3;
  optional double ddl = 4;
}

message SpeedPoint {
  optional double s = 1;
  optional double t = 2;
  // speed (m/s)
  optional double v = 3;
  // acceleration (m/s^2)
  optional double a = 4;
  // jerk (m/s^3)
  optional double da = 5;
}

message PathPoint {
  // coordinates
  optional double x = 1;
  optional double y = 2;
  optional double z = 3;

  // direction on the x-y plane
  optional double theta = 4;
  // curvature on the x-y planning
  optional double kappa = 5;
  // accumulated distance from beginning of the path
  optional double s = 6;

  // derivative of kappa w.r.t s.
  optional double dkappa = 7;
  // derivative of derivative of kappa w.r.t s.
  optional double ddkappa = 8;
  // The lane ID where the path point is on
  optional string lane_id = 9;

  // derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
  optional double x_derivative = 10;
  optional double y_derivative = 11;
}

message Path {
  optional string name = 1;
  repeated PathPoint path_point = 2;
}

message TrajectoryPoint {
  // path point
  optional PathPoint path_point = 1;
  // linear velocity
  optional double v = 2;  // in [m/s]
  // linear acceleration
  optional double a = 3;
  // relative time from beginning of the trajectory
  optional double relative_time = 4;
  // longitudinal jerk
  optional double da = 5;
  // The angle between vehicle front wheel and vehicle longitudinal axis
  optional double steer = 6;

  // Gaussian probability information
  optional GaussianInfo gaussian_info = 7;
}

相对路经：modules/common_msgs/basic_msgs/pnc_point.proto

2. 计算匹配点主要涉及的函数：
        1) 函数名: MatchToPath(const std::vector< PathPoint >& reference_line, const double x, const double y)
            函数位置: modules/common/math/path_matcher.cc
            函数作用: 在参考线上计算匹配点
        2) 函数名: FindProjectionPoint(const PathPoint& p0,const PathPoint& p1, const double x,const double y)
            函数位置: modules/common/math/path_matcher.cc
            函数作用: 从自车当前点做投影到参考线上,找到在参考线上投影的精确位置
        3) 函数名: InterpolateUsingLinearApproximation(const PathPoint &p0, const PathPoint &p1,const double s)
            函数位置: modules/common/math/linear_interpolation.cc
            函数作用: 使用线性插值的办法来计算投影得到的点在参考线上的x,y,heading,k等一系列值
        4) 函数名: slerp(const double a0, const double t0, const double a1, const double t1, const double t)
            函数位置: modules/common/math/linear_interpolation.cc
            函数作用: 核心还是用的线性插值,不过对角度做了处理,保证在[-pi,pi)之间
        5) 函数名: lerp(const T &x0, const double t0, const T &x1, const double t1, const double t)
            函数位置: modules/common/math/linear_interpolation.h
            函数作用: 线性差值
        6) 函数名: NormalizeAngle(const double angle)
            函数位置: modules/common/math/math_utils.cc
            函数作用: 对角度处理使得在[-pi,pi)之间

函数调用架构流程

函数模块化解析

1.lattice_planner.cc中代码部分

  // 在参考线上计算匹配点, 输入参数:参考线,起点x,起点y, 
相当于说在参考线上进行遍历找到一点,该点离我当前轨迹上车辆所在位置点的距离是最短的,即frenet公式推导中的L最短
  // 2. compute the matched point of the init planning point on the reference
  // line.
  PathPoint matched_point = PathMatcher::MatchToPath(
      *ptr_reference_line, planning_init_point.path_point().x(),
      planning_init_point.path_point().y());

2. 函数-MatchToPath

// 在参考线上计算匹配点
PathPoint PathMatcher::MatchToPath(const std::vector<PathPoint>& reference_line,
                                   const double x, const double y) {
  CHECK_GT(reference_line.size(), 0);

  // 定义了一个内部的函数,用于计算距离
  auto func_distance_square = [](const PathPoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };

  // 首先计算了一下参考线的起点到traj上当前车辆位置的距离
  double distance_min = func_distance_square(reference_line.front(), x, y);
  std::size_t index_min = 0; // 不仅要知道大小,还要知道索引

  // 从index为1的点开始(因为index=0的点前面刚计算过),依次计算和车辆位置的距离
  for (std::size_t i = 1; i < reference_line.size(); ++i) {
    double distance_temp = func_distance_square(reference_line[i], x, y);
    // 找到参考线上距离自车当前位置最近的点的index
    if (distance_temp < distance_min) {
      distance_min = distance_temp;
      index_min = i;
    }
  } 
 
  // index_min == 0,意味着最近的点是referenceLine的起点,不等于0就 index_min - 1,
  std::size_t index_start = (index_min == 0) ? index_min : index_min - 1;
  // 判断index_min+1的值是否等于参考线的size,否的话 index_end=index_min+1
  std::size_t index_end =
      (index_min + 1 == reference_line.size()) ? index_min : index_min + 1;
  // 到这里找下来相当于在referenceLine上找了三个点,即参考线上距离车辆位置最近的点,以及这个点
  // 的前后各一个点,为什么要在前后各找一个点呢?因为他要做车辆当前的点 在referenceLine上的投影,
  // 这里会把index_start和index_end连起来,当作一条线(方向start指向end),供我们做投影,公式为:
  // 向量A(index_start -> traj上的车辆当前位置) 点乘 向量B(index_start -> index_end) 

  if (index_start == index_end) {
    return reference_line[index_start];
  }

  // 做投影,向量A(index_start -> traj上的车辆当前位置) 点乘 向量B(index_start -> index_end)
  return FindProjectionPoint(reference_line[index_start],
                             reference_line[index_end], x, y);
}

3.函数-FindProjectionPoint

为什么需要在邻近点附近向后找一个点index_end和向邻近点前面找一个点index_start?

主要便于计算出向量start->vehicle_position 在 向量start->end 上的投影，从而便于计算出邻近点到投影点的位姿信息。

PathPoint PathMatcher::FindProjectionPoint(const PathPoint& p0,
                                           const PathPoint& p1, const double x,
                                           const double y) {
  // 为什么投影用的 向量start->vehicle_position 在 向量start->end 上的投影而不用
  // 向量index_min->vehicle_position 在 向量start->end 上的投影呢?
//主要原因：保证求得的投影距离始终是一个正值，向量start->vehicle_position 在 向量start->end 作点积，只有夹脚是锐角，点积后的结果才是正值；如果向量index_min->vehicle_position 在 向量start->end 上点积，则需要考虑，vehicle_position在index_min之前，或者之后二者向量的夹角是锐角还是钝角，增加计算复杂度，使用前者可始终保证夹角是锐角。
  // 详见参考:https://www.bilibili.com/video/BV1rM4y1g71p?spm_id_from=333.999.0.0

//计算车辆当前点与邻近点前一个点的向量的分量
  double v0x = x - p0.x();
  double v0y = y - p0.y();

//计算车辆邻近点后一个点与邻近点前一个点的向量的分量
  double v1x = p1.x() - p0.x();
  double v1y = p1.y() - p0.y();

//计算车辆邻近点后一个点与邻近点前一个点的向量的模
  double v1_norm = std::sqrt(v1x * v1x + v1y * v1y);

//计算车辆邻近点后一个点与邻近点前一个点的向量的点积
  double dot = v0x * v1x + v0y * v1y;

  // v0 · v1 = ||v0||*||v1||*cos(theta)    theta为v0和v1的夹角，此夹角未知，但向量vo和v1已知。
  // (v0 · v1) / ||v1|| = ||v0||*cos(theta)
  // ||v0||*cos(theta) 即为v0在v1上的投影的大小

  double delta_s = dot / v1_norm;
  // 这个函数主要使用线性插值的办法来计算投影得到的点在参考线上的坐标,heading等一系列值；
//p0.s(),为从参考线的起点开始到p0这个点的距离,p0.s() + delta_s就是v0在参考线上的投影点到参考线起点的距离
  return InterpolateUsingLinearApproximation(p0, p1, p0.s() + delta_s);
}

4.函数-InterpolateUsingLinearApproximation

PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0,
                                              const PathPoint &p1,
                                              const double s) {
  double s0 = p0.s();
  double s1 = p1.s();

  PathPoint path_point;
//关于s0和s1以及s可以参考概述中 0的图 s0=p0_s,s1=p1_s,s=d_s+p0_s
  double weight = (s - s0) / (s1 - s0);  // 计算投影点的权重，也即投影比

  // 计算投影点位姿信息公式: x = p0.x + weight * (p1.x - p0.x),  下面他用的是合并同类项之后的,都一样
//其中p0.x为index_start的坐标信息

  double x = (1 - weight) * p0.x() + weight * p1.x();
  double y = (1 - weight) * p0.y() + weight * p1.y();
  // 核心还是用的线性插值,不过对角度做了处理,保证在[-pi,pi)之间
  double theta = slerp(p0.theta(), p0.s(), p1.theta(), p1.s(), s);
  double kappa = (1 - weight) * p0.kappa() + weight * p1.kappa();
  double dkappa = (1 - weight) * p0.dkappa() + weight * p1.dkappa();
  double ddkappa = (1 - weight) * p0.ddkappa() + weight * p1.ddkappa();

//把计算得到的投影点位姿信息写入到路经点的结构体中
  path_point.set_x(x);
  path_point.set_y(y);
  path_point.set_theta(theta);
  path_point.set_kappa(kappa);
  path_point.set_dkappa(dkappa);
  path_point.set_ddkappa(ddkappa);
  path_point.set_s(s);
  return path_point;
}

5. 函数-slerp，对航向角进行归一化处理，保证在[-pi,pi)

double slerp(const double a0, const double t0, const double a1, const double t1,
             const double t) {
  // start和end两个点距离太近
  if (std::abs(t1 - t0) <= kMathEpsilon) {
    ADEBUG << "input time difference is too small";
    return NormalizeAngle(a0);
  }
  // 对两个heading角处理使得在[-pi,pi)之间
  const double a0_n = NormalizeAngle(a0);
  const double a1_n = NormalizeAngle(a1);
  // 得到delta(heading)
  double d = a1_n - a0_n;
  if (d > M_PI) {
    d = d - 2 * M_PI;
  } else if (d < -M_PI) {
    d = d + 2 * M_PI;
  }

  const double r = (t - t0) / (t1 - t0);
  const double a = a0_n + d * r;
  return NormalizeAngle(a);
}

6.函数-NormalizeAngle

double NormalizeAngle(const double angle) {
  // 这一步取余的操作可以保证结果在(-2pi,2pi)范围内
  double a = std::fmod(angle + M_PI, 2.0 * M_PI);
  // 这个保证结果在(0,2pi)范围内
  if (a < 0.0) {
    a += (2.0 * M_PI);
  }
  // 这个保证结果在(-pi,pi)范围内
  return a - M_PI;
}

 技术细节

1.关于邻近点的前一个点和后一个点的选取，一般前一个点也即index_start = 0;后一个点end_start=1;根据这两个点，做投影，找到投影点，并根据投影比计算出投影点的位姿信息，作为规划的参考点信息。

2.计算出的参考点的航向角，记得做角度归一化处理，避免出现大角度,导致计算异常，控制在[-pi,pi）

3.计算投影点的长度（投影比）要用邻近点前一个点index_start->vehicle_position连线的向向量在index_start->index_end连线的投影，而非邻近点->vehicle_position连线的向向量在index_start->index_end连线的投影！

4.控制最终使用的不是匹配点，而是投影点。

补充：关于匹配点和投影点的关系

车辆横向误差微分方程(look：车辆横向误差微分方程的推导)：

如何选择投影点：

上层轨迹规划模块给控制模块的轨迹为离散点轨迹，而不是连续的。

                

匹配点的寻找与定义：

控制使用的是基于匹配点计算后的投影点的位姿信息！！！

详细参考博主：百度阿波罗（APOLLO）自动驾驶项目中的横向误差与纵向误差的计算方法该怎么理解？ - 知乎

小结

参考博客：

https://blog.csdn.net/weixin_44558122/article/details/124585202

读书思过投稿视频-读书思过视频分享-哔哩哔哩视频

百度阿波罗（APOLLO）自动驾驶项目中的横向误差与纵向误差的计算方法该怎么理解？ - 知乎

自动驾驶小卡
关注
专栏目录
Apollo 8.0 安装教程(Vmware)
wws的博客
08-07 434
Vmware ： 17pro (17.0.0 build-20800274) Ubuntu ：20.04 Apollo ：8.0 docker ：24.0.5 添加docker用户 3、安装apollo 8.0 在VMware自带浏览器输入就可以看到仿真界面 1）启动： 上面的第三步是第一次安装的时候进行的，如果安装成功，电脑重启以后需要快速启动，可以运行下面的指令： 2）退出： 在container里面输入退出 然后输入 5、下载Demo并运行 启动Dreamview以后，在container里面运
Ubuntu 22.04.2 LTS安装Apollo8.0
harryxia2014的博客
02-22 2589
Ubuntu 22.04.2 LTS安装Apollo8.0 相对官网，有些要注意的地方如下 一、本人采用了Ubuntu 22.04.2 LTS 二、GPU安装启动时，CUDA Toolkit要对上版本号 三、docker安装，采用我这步聚比较靠普 四、接官网采用git 并将源码 clone 下来的话，我是执行了近十次都失败。（说访问失败），最后我用下载模式来做完的。 五、下完代码后，进入docker，之后要用root权限，不然编译不过。
Apollo6.0代码Lattice算法详解——Part2:寻找匹配点
weixin_44558122的博客
05-06 5173
Apollo6.0代码Lattice算法详解——Part2:寻找匹配点0.前置知识1.涉及主要函数2.函数关系3.函数代码详解    3.1 lattice_planner.cc中代码部分    3.2 函数-MatchToPath    3.3 函数-FindProjectionPoint    3.4 函数
apollo小知识点——匹配点计算
q384515674的博客
05-04 974
问题具体描述 求traj上当前位置在reference_line上的match_point(匹配点) 匹配点定义 位于reference_line上，且在该点处的切线与车辆当前位置和匹配点连成的线相垂直 步骤一、遍历reference_line，取与车辆当前位置点A最近的点A’ 步骤二、取A’前后点A_f,A_b,，并将两点的连线看作A’点的切线(也可直接取A’点处的切线) 步骤三、计算车辆当前位置A点到A’切线的投影点的s值 方法1(apollo).计算AA’到A’切线的投影，记投影长度为Δs，s=s_A
【Apollo】Apollo 8.0框架学习以及感知模块的实践
最新发布
一个普通的算法牛马
08-15 2424
学习Apollo感知的第一步：Apollo 8.0 感知框架的初探与实践
apollo7.0中path_point的没一个参数含义
weixin_35755823的博客
12-23 196
Apollo 7.0是一个自动驾驶操作系统，path_point是其中的一个概念。它代表在地图中的一个位置点，通常用来表示车辆的预期路径。 在Apollo 7.0中，path_point包含以下参数： x: 表示path_point位置的横坐标，单位是米。 y: 表示path_point位置的纵坐标，单位是米。 z: 表示path_point位置的高度，单位是米。 theta: 表示path_p...
解析百度Apollo之参考线与轨迹——保罗的酒吧
David's Tweet
12-27 7068
本文紧接着上一篇文章：《解析百度Apollo之决策规划模块》。会详细讲解决策规划模块中是如何生成参考线和行车轨迹供车辆行驶的。参考线和轨迹直接影响了自动驾驶车辆对于方向和速度的控制，因此，说它是自动驾驶系统中最重要的数据都不为过。 前言 本文是百度自动驾驶平台Apollo项目系列文章中的一篇，会讲解Planning模块中的参考线逻辑。 对于刚接触Apollo项目的读者可以阅读我博客中的另外一篇文章 -《解析百度Apollo自动驾驶平台》，那里对Apollo项目做了整体的介绍。 ReferenceL.
Apollo MatchPoint的获取方式
u012448312的博客
07-18 166
如上图所示:其中(x,y)为主车所在位置，p0~pn为规划的轨迹；假设p_i为规划轨迹中距离主车最近的点，获取p_i后，并获取点p_i-1和p_i+1;在区间[p_i-1,p_i+1]范围内通过黄金比例搜索算法找到该区间内距离主车(x,y)的点，该点即为MatchPoint。4、在最小距离的轨迹点附近找到两个邻近的点通过黄金分割搜索算法找到临近两个点内距离主车(x,y)最小的点，此点即为匹配点。3、遍历生成的轨迹点，找到轨迹中距离主车(x,y)最小的的点；1、设主车的坐标为(x,y)
如何在gitcodes上下载apollo8.0并编译？
08-18
您可以按照以下步骤在GitCodes上下载Apollo 8.0并进行编译： 1. 打开GitCodes网站，点击“+New Project”新建一个项目。 2. 在GitCodes的终端中输入以下命令，克隆Apollo仓库到您的项目中： ``` git clone -b ...
Apollo路径-速度规划轨迹生成方法
此心光明，亦复何言~
04-24 8293
Apollo路径-速度规划轨迹生成方法 Apollo是百度开源的无人驾驶解决方案，其中规划中会按照不同的场景生成一条临时的路径，通过临时路径来障碍物规避，或者临时变道。 情况 假设前面有一个缓慢行驶的汽车离我们越来越近。 ...
第3章：车辆横向控制
weixin_44063383的博客
05-09 2295
自行车模型 + 阿克曼几何 + 纯追踪法 + 斯坦利控制
path-match:请路由中间件
05-17
考阿路径比赛 一个简单的围绕路由包装器。 与类似，不同之处在于它可以选择性地更好地处理方法。 所有这些路由器都在下面使用了，这也是Express所使用的。 const route = require ( 'koa-path-match' ) ( { /* options passed to path-to-regexp */ } ) app . use ( route ( '/:id(\\d+)' , ( ctx , next ) => { const id = ctx . params . id // do stuff switch ( ctx . request . method ) { } } ) ) 或者，您可以为每个方法创建中间件： app . use ( route ( '/:id(\\d+)' ) . get ( async ctx => {
Apollo 轨迹规划算法解析
WenbinYao&YouweiHu
02-02 4727
这篇文章主要整理一下前段时间学习的轨迹规划算法，关于轨迹规划问题可以查到的相关资料很多，但能把轨迹规划问题详细说清楚的并不太多，这导致我在学习过程中出现很多疑惑。这里我把Apollo的各个版本轨迹规划算法进行简单总结和汇总。
进阶课程㉓丨Apollo规划技术详解——Motion Planning with Environment
weixin_43619346的博客
03-24 713
目录运动规划的环境变化Vehicle Model的建立曲线坐标系SLSL坐标系到XY坐标系的投影XY坐标系到SL坐标系的投影 原创 阿波君 Apollo开发者社区 2019-06-26 敲黑板，本文需要学习的知识点有 坐标转换 刚体 投影 切线 交点 自由度 当行为层决定要在当前环境中执行的驾驶行为时，其可以是例如巡航-车道，改变车道或右转，所选择的行为必须被转换成路径或轨迹，可由低级反馈...
百度Apollo规划算法——轨迹拼接
Eleven的博客
03-05 4035
在apollo的规划算法中，在每一帧规划开始时会调用一个轨迹拼接函数，返回一段拼接轨迹点集，并告诉我们是否重规划以及重规划的原因，那大家是否深入了解并思考过什么是轨迹拼接、为什么要进行轨迹拼接以及是如何进行轨迹拼接的呢？本篇文章便是针对这几个问题，在参考了前辈的一些文章和Apollo代码的基础上进行解答。
C++11通过Lambda表达式操作向量元素
视觉与物联智能
02-25 555
C++11提供了Lambda表达式，可以方便地操作向量元素。虽然C++11的Lambda表达式是未命名函数，但是可以像变量一样命名一个Lambda表达式并且调用方式跟常规函数没有什么区别。
Apollo星火计划学习笔记——Apollo决策规划技术详解及实现（以交通灯场景检测为例）
sinat_52032317的博客
12-19 5083
接着判断当前道路状况是否可以换道，若可以换道，则路径决策生成一个换道边界，若不可以换道，则会生成一个道内的边界。Traffic Light场景的进入这部分主要有两个部分：第一个部分通过Overlap重叠判断是否进入交叉路口，第二个部分通过由高精地图提供的车道信息以及交通信号等情况进入不同的场景，选择是有保护的场景还是无保护的场景。速度决策的流程：对当前路径产生一个或多个速度边界，再将其集成一个速度边界的集合，生成一个ST图。在得到路径边界之后，通过路径规划器，得到平滑的路径曲线，之后再对曲线进行路径决策。
计算车辆纵向误差Compute Longitudinal Errors
无人驾驶吕同学的博客
05-11 994
#ComputeLongitudinalErrors主函数 void LonController::ComputeLongitudinalErrors( const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time, const double ts, SimpleLongitudinalDebug *debug) { // the decomposed vehicle motion onto Frenet
Baidu Apollo代码解析之Lattice Planner
silver bullet
05-30 3219
大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/c_1132958996826546176）。以后，我会把日常的思考放在CSDN上，梳理过的精华文章放在知乎上，希望大家可以多多交流，互相学习。 Apollo中提到了几种轨迹规划方法，这里简要介绍Lattice，主要思路可以参考这一篇博文，我为了自己方便...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值