自动驾驶---OpenSpace之Hybrid A*规划算法

news2024/11/15 19:32:42

1 背景

        笔者在上周发布的博客《自动驾驶---低速场景之记忆泊车》中，大体介绍了记忆泊车中的整体方案，其中详细阐述了planning模块的内容，全局规划及局部规划（会车）等内容，包括使用的算法，但是没有深入详细地展开讲述OpenSpace规划算法，因此在本篇博客中，详细讲解该部分内容。

        OpenSpace规划主要涉及到自动驾驶车辆在开放、非结构化环境中的路径规划问题。这种规划对于没有固定参考线的场景特别重要，比如自主泊车、路边停车，城区路口掉头等。

        在OpenSpace规划中，算法的核心是根据感知信息和感兴趣区域（ROI）来生成和选择最佳的行驶路径。感知信息主要包括周围动静态障碍物的位置、速度等信息，而ROI则包含了地图信息，比如道路边界、车位边界等。

        OpenSpace规划算法的基本流程如下：

  1. 生成可行驶区域：根据感知数据和地图信息，算法首先会确定车辆可以安全行驶的区域。这通常涉及到对道路边界、障碍物位置等信息的解析和处理。
  2. 路径搜索与选择：在确定了可行驶区域后，算法会在这个区域内搜索可能的行驶路径。这个过程可能会使用到各种路径搜索算法，比如A*算法或其变种。搜索到的路径会基于一定的评价标准（比如安全性、效率等）进行排序和选择。
  3. 轨迹优化：选定了路径后，算法还需要对车辆的行驶轨迹进行优化，以确保车辆在行驶过程中的平稳性和舒适性。这通常涉及到对车辆运动学模型和动力学模型的考虑，以及使用优化算法来求解最优的轨迹。

        此外，OpenSpace规划还需要考虑一些额外的因素，比如交通规则、行人和其他车辆的动态行为等。这些因素都可能对车辆的行驶路径产生影响，因此需要在规划过程中进行充分的考虑和处理。

        OpenSpace规划是自动驾驶技术中的一个重要环节，它能够帮助车辆在没有固定参考线的开放环境中实现安全、高效的行驶。随着自动驾驶技术的不断发展，OpenSpace规划也将不断优化和完善，以适应更加复杂和多变的环境。

2 OpenSpace规划算法介绍

        OpenSpace规划在自动驾驶中涵盖了多种算法，这些算法共同协作以在开放、非结构化环境中实现安全、高效的路径规划。以下是一些主要的OpenSpace规划算法：

  1. Hybrid A*算法：

    • A*算法：传统的A*算法是一种广泛使用的路径搜索算法，它能够在给定的图或网格中找到从起点到终点的最短路径。
    • Hybrid A*算法：针对自动驾驶场景，混合A*算法结合了A*算法与车辆运动学模型，考虑了车辆的实际运动约束，从而生成更符合车辆行驶特性的路径。

  2. OBCA算法：

    • MPC预测模型：模型预测控制（MPC）是一种优化控制方法，用于处理具有约束的线性或非线性系统。在OBCA算法中，MPC预测模型用于预测车辆在未来一段时间内的状态，为路径规划提供基础。
    • MPC约束设计：约束设计是确保车辆在行驶过程中遵守交通规则、避免碰撞等的重要步骤。OBCA算法中的MPC约束设计考虑了车辆的运动学约束、避障约束等。
    • MPC目标函数设计：目标函数是优化问题的核心，它定义了优化的目标和评价标准。在OBCA算法中，MPC目标函数设计旨在实现路径的平滑性、安全性以及效率性。

  3. SCP路径平滑算法：SCP（Sequential Convex Programming）是一种基于凸优化的启发式算法，用于解决非凸优化问题。在OpenSpace规划中，SCP路径平滑算法用于对搜索到的路径进行平滑处理，提高路径的连续性和舒适性。

        上述这些算法在OpenSpace规划中各自发挥着重要作用，并相互协作以实现自动驾驶车辆在开放空间中的安全、高效行驶。随着技术的不断发展，新的算法和方法也将不断被引入到OpenSpace规划中，以应对更加复杂和多变的环境。

        下面笔者将介绍目前量产中使用比较多的算法：Hybrid A*。从市面上量产的产品及笔者从行业内了解到的信息来看，目前市面上看到华为、大疆、小米和百度使用到该算法，其它家暂时不清楚。

3 Hybrid A*算法介绍

3.1 A*算法介绍

        在介绍Hybrid A*之前，笔者会先介绍A*算法，Hybrid A*算法其实是由A*演变而来（A*算法又由D*算法演化而来，这里就不往前追溯了）。

        A*算法起源于1968年，由Peter E. Hart、Nilsson和Raphael在他们的论文《A Formal Basis for the Heuristic Determination of Minimum Cost Paths in Graphs》中首次提出，该算法是求解静态路网中最短路径问题的一种有效方法，同时也是解决许多搜索问题的有力工具。

        下面先介绍A*算法：

        假设start为起始节点，那么在网格中下一步可扩展的节点数量为8个。

        关于障碍物，在Grid Map中，如果存在障碍物会将生成的网格状态变成不可用，即在节点选择中为不可选的状态。如下图所示（左边可选节点数量为8，右边可选节点数量为7）。

a40fe02448a341b0b1ffa5a5c469e28d.png

        那么如何选择下一个节点呢？因此引入了一个概念：代价函数。

        代价函数的意思就是这次选择你，付出的代价是多少，定义为f(n)。在A*中代价函数的定义有两个，第一个是已经走过的路径的代价g(n)，另一个是还没有走过的路径的代价h(n)。走过的路径很容易理解，没走过的路径是指从当前节点走到目标节点的距离，在算法中可使用曼哈顿距离或者欧式距离，这里是不考虑障碍物的。

        那么最终周围的每一个可用节点都有一个代价值：f(n) = g(n) + h(n)，如下图所示。

1a8f5be27dac441e83f82ffef4953caa.png

         A*算法的逻辑结构如下：

  1. 初始化。从上游获取 grid map信息，设置起点 start、终点 end、栅格数量 m*n 等。
  2. 数据预处理。定义 “待计算子节点” 的数组 openlist 以及 “已选中的节点” 的数组 closelist，保存路径的数组 path_closelist。并且还需建立一个当前子节点集合 children，用来保存当前父节点周围8个子节点的坐标，以及父节点本身 parent；还有保存代价值 g , h , f 的数组openlist_cost 和 closelist_cost。
  3. 对子节点们 children 中的每个节点 child：若该子节点不在 “待计算子节点” 节点 openlist 中，则追加进去；若在，则计算出该 child 的 g 值，该 g 值是从起点到父节点 parent 的距离加上父节点到该子节点的距离。若该 g 值小于之前 openlist_cost 中的 g 最小值，那么就将openlist_cost 中的最小 g 值更新；
  4. 由于该代价最小点已经加入了轨迹，因此将该点加入 clost_list 和 path_closelist，并从openlist 中剔除；
  5. 更新 openlist 中的最小代价值，并以其为父节点开始新一轮搜索。

        A*算法最为核心的部分，就在于它的估值函数的设计上：f(n)=g(n)+h(n)。

3.2 Hybrid A*算法

        最早的Hybrid A*算法来自于斯坦福大学无人车团队发表的《Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments》。

        两者的主要区别如下：

算法种类是否考虑障碍物是否考虑运动约束增加的曲线扩展方式
 A*是否无
Hybrid A*是是RS曲线

        A*以及Hybrid A*节点的扩展方式见下图所示，在Hybrid A*中考虑了车辆的运动学约束，因此扩展出来的轨迹更符合车辆的实际轨迹。

d66070df6b89490bb7825a1e06e73d93.png

（1）Reeds Shepp曲线    

        在Hybrid A*中，使用了Reeds Shepp曲线（直线-圆弧的48种组合曲线）去拓展节点，由于Reeds Shepp曲线生成比较简单且快速，所以初始的构造过程是没有考虑碰撞检测的，可以在构造完成后再进行碰撞检测，如果整条轨迹没有碰撞，则放入备选轨迹之中。这里有一个问题可以思考：如果遍历的节点上万个甚至更多，每一个节点都去进行RS扩展的碰撞检测，耗时如何解决？

（2）代价函数的定义

        相比A*算法，其中有一个大的变化在于代价函数的使用，在g(n)的计算中，增加了换挡、转向的代价；在h(n)的计算中，采用了两个子函数，h1(n)表示符合车辆运动学约束但是忽略障碍物的最短路径，h2(n)表示满足障碍物约束但是忽略车辆运动学约束的最短路径，取两者之中的最大值。

（3）Voronoi势场函数

        最后还有一个大的区别：Voronoi势场函数。最终生成的路径保证安全，因此需要距离障碍物有一定的距离。该函数的意义在于：在比较开阔的区域轨迹远离障碍物，在狭窄的区域也可以充分利用空间。

698527791209478ba5f8a435d44f6140.png

        其中，eq?d_o 表示路径节点到最近障碍物的距离，eq?d_v 表示最近的GVD（Generalized Voronoi Diagram）的长度，eq?%5Calpha 来控制势场的衰减率，eq?d%5E%7Bmax%7D_o 控制势场的最大影响范围。如下图所示，颜色越黑的地方说明势场较大。

8c140fc602a0466b812c5e89edff7b0b.png

 主要代码如下（展示的是Apollo的源代码，相对原始算法做了一些改动）：

bool HybridAStar::Plan(
    double sx, double sy, double sphi, double ex, double ey, double ephi,
    const std::vector<double>& XYbounds,
    const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,
    HybridAStartResult* result) {
  // clear containers
  open_set_.clear();
  close_set_.clear();
  open_pq_ = decltype(open_pq_)();
  final_node_ = nullptr;

  std::vector<std::vector<common::math::LineSegment2d>>
      obstacles_linesegments_vec;
  for (const auto& obstacle_vertices : obstacles_vertices_vec) {
    size_t vertices_num = obstacle_vertices.size();
    std::vector<common::math::LineSegment2d> obstacle_linesegments;
    for (size_t i = 0; i < vertices_num - 1; ++i) {
      common::math::LineSegment2d line_segment = common::math::LineSegment2d(
          obstacle_vertices[i], obstacle_vertices[i + 1]);
      obstacle_linesegments.emplace_back(line_segment);
    }
    obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
  }
  obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);

  // load XYbounds
  XYbounds_ = XYbounds;
  // load nodes and obstacles
  start_node_.reset(
      new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_open_space_config_));
  end_node_.reset(
      new Node3d({ex}, {ey}, {ephi}, XYbounds_, planner_open_space_config_));
  if (!ValidityCheck(start_node_)) {
    ADEBUG << "start_node in collision with obstacles";
    return false;
  }
  if (!ValidityCheck(end_node_)) {
    ADEBUG << "end_node in collision with obstacles";
    return false;
  }
  double map_time = Clock::NowInSeconds();
  grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,
                                                  obstacles_linesegments_vec_);
  ADEBUG << "map time " << Clock::NowInSeconds() - map_time;
  // load open set, pq
  open_set_.emplace(start_node_->GetIndex(), start_node_);
  open_pq_.emplace(start_node_->GetIndex(), start_node_->GetCost());

  // Hybrid A* begins
  size_t explored_node_num = 0;
  double astar_start_time = Clock::NowInSeconds();
  double heuristic_time = 0.0;
  double rs_time = 0.0;
  while (!open_pq_.empty()) {
    // take out the lowest cost neighboring node
    const std::string current_id = open_pq_.top().first;
    open_pq_.pop();
    std::shared_ptr<Node3d> current_node = open_set_[current_id];
    // check if an analystic curve could be connected from current
    // configuration to the end configuration without collision. if so, search
    // ends.
    const double rs_start_time = Clock::NowInSeconds();
    if (AnalyticExpansion(current_node)) {
      break;
    }
    const double rs_end_time = Clock::NowInSeconds();
    rs_time += rs_end_time - rs_start_time;
    close_set_.emplace(current_node->GetIndex(), current_node);
    for (size_t i = 0; i < next_node_num_; ++i) {
      std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
      // boundary check failure handle
      if (next_node == nullptr) {
        continue;
      }
      // check if the node is already in the close set
      if (close_set_.find(next_node->GetIndex()) != close_set_.end()) {
        continue;
      }
      // collision check
      if (!ValidityCheck(next_node)) {
        continue;
      }
      if (open_set_.find(next_node->GetIndex()) == open_set_.end()) {
        explored_node_num++;
        const double start_time = Clock::NowInSeconds();
        CalculateNodeCost(current_node, next_node);
        const double end_time = Clock::NowInSeconds();
        heuristic_time += end_time - start_time;
        open_set_.emplace(next_node->GetIndex(), next_node);
        open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());
      }
    }
  }
  if (final_node_ == nullptr) {
    ADEBUG << "Hybrid A searching return null ptr(open_set ran out)";
    return false;
  }
  if (!GetResult(result)) {
    ADEBUG << "GetResult failed";
    return false;
  }
  ADEBUG << "explored node num is " << explored_node_num;
  ADEBUG << "heuristic time is " << heuristic_time;
  ADEBUG << "reed shepp time is " << rs_time;
  ADEBUG << "hybrid astar total time is "
         << Clock::NowInSeconds() - astar_start_time;
  return true;
}

        由Hybrid A*生成的粗糙轨迹是不能直接给控制使用的，需要进行轨迹平滑，平滑的方法有很多种，这里同样可使用行车的平滑方法《自动驾驶---Motion Planning之参考线Path平滑》（非原文中使用的方法）。最终呈现的效果如下：

31f28173ea0b45f2992579d5bf4c1e1a.png

4 展望

        OpenSpace算法不仅适用于泊车场景，也可适用于行车场景。目前各家都在卷城区自动驾驶，那么在城区的狭窄路段需要前后多次调整（比如城市道路常见的U型弯场景，方向盘打到最大无法一次性通过，需要多次腾挪），再比如单车道会车的场景，也需要退让的场景，所以掌握OpenSpace方法对于各家公司规划的 “行泊一体” 也是很有帮助的。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1616587.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

.gitignore语法及配置问题

.gitignore语法及配置问题

语法及配置 前言.gitignore语法Git 忽略规则优先级gitignore规则不生效Java项目中常用的.gitignore文件c项目中常用的.gitignore注意事项 前言 在工程中&#xff0c;并不是所有文件都需要保存到版本库中&#xff0c;例如“target”目录及目录下的文件就可以忽略。在Git工作区的…
阅读更多...
四信AI睿析—边缘智脑：赋能农业新时代，开启智慧种植新篇章

四信AI睿析—边缘智脑：赋能农业新时代，开启智慧种植新篇章

方案简介 本系统前端安装土壤墒情监测站&#xff0c;包括温湿度传感器、二氧化碳传感器、PH值传感器、土壤电导率传感器、土壤温湿度传感器、光照传感器等组成&#xff1b;高清枪机摄像头等、负责种植区域温湿度、土壤EC、土壤温湿度、光照等环境因子、视频数据、农作物生长图…
阅读更多...
Spring 依赖-ApiHug准备-测试篇-015

Spring 依赖-ApiHug准备-测试篇-015

&#x1f917; ApiHug {Postman|Swagger|Api...} 快↑ 准√ 省↓ GitHub - apihug/apihug.com: All abou the Apihug apihug.com: 有爱&#xff0c;有温度&#xff0c;有质量&#xff0c;有信任ApiHug - API design Copilot - IntelliJ IDEs Plugin | Marketplace gradle…
阅读更多...
Golang | Leetcode Golang题解之第44题通配符匹配

Golang | Leetcode Golang题解之第44题通配符匹配

题目&#xff1a; 题解&#xff1a; func isMatch(s string, p string) bool {for len(s) > 0 && len(p) > 0 && p[len(p)-1] ! * {if charMatch(s[len(s)-1], p[len(p)-1]) {s s[:len(s)-1]p p[:len(p)-1]} else {return false}}if len(p) 0 {retur…
阅读更多...
【Linux学习】初始认识操作系统

【Linux学习】初始认识操作系统

文章目录 &#x1f680;简单认识操作系统(Operator System)&#x1f302;操作系统的定义&#x1f302;为什么需要操作系统呢&#xff1f;&#x1f302;系统调用和库函数概念 &#x1f680;简单认识操作系统(Operator System) &#x1f302;操作系统的定义 操作系统(Operation…
阅读更多...
20240331-2-文本表征方式

20240331-2-文本表征方式

静态语义表示方法 知识体系 主要包括词袋模型 BoW、TFIDF、LDA、Word2vec、Golve、Doc2Vec 等。 Questions 在小数据集中 Skip-Gram 和 CBoW 哪种表现更好&#xff1f; Skip-Gram 是用一个 Center Word 预测其 Context 里的 Word&#xff1b;而 CBoW 是用 Context 里的所有…
阅读更多...
Python 数据库简化操作：dataset 库介绍

Python 数据库简化操作：dataset 库介绍

文章目录 Python 数据库简化操作&#xff1a;dataset 库介绍第一部分&#xff1a;背景介绍第二部分&#xff1a;库是什么&#xff1f;第三部分&#xff1a;如何安装这个库&#xff1f;第四部分&#xff1a;库函数使用方法第五部分&#xff1a;场景应用第六部分&#xff1a;常见…
阅读更多...
小清新思维题

小清新思维题

CF521D shop Problem Solution 首先如果确定了执行的操作&#xff0c;执行顺序一定为赋值、加、乘。赋值操作只保留最大的&#xff0c;并可以转化为加法。每个数的加法操作按从大到小顺序排序后可以转化为乘法。最后将所有乘法操作从大到小排序选前&#x1d45a;个即可。 先…
阅读更多...
数据库服务类--Redis--未授权访问终端Getshell

数据库服务类--Redis--未授权访问终端Getshell

免责声明:本文仅做技术交流与学习. 目录 前提条件: windows上开启redis服务: Linux上创建&开启redis服务: 操作: 1-连接靶机redis 2-写入webshell 3-访问后门 redis--->webshell Redis未授权访问漏洞复现与利用 - 知乎 (zhihu.com) 前提条件: 端口开放(6379) 目录…
阅读更多...
数据库左连接、右连接、内连接、全连接

数据库左连接、右连接、内连接、全连接

在数据库增删改查中&#xff0c;数据通常不在同一张表中&#xff0c;涉及多表数据查询就需要表之间的连接方式&#xff0c;常用的数据库表连接方式有&#xff1b; 1&#xff09;内连接&#xff1a;inner join 2&#xff09;全连接&#xff1a;union 3&#xff09;右连接&#x…
阅读更多...
centos7安装了没有图形界面的解决办法

centos7安装了没有图形界面的解决办法

如果你的centos7是这样的 只需要重新选择虚拟机 按照这个配置调即可
阅读更多...
面试宝典（1）——数据库篇（MySQL）

面试宝典（1）——数据库篇（MySQL）

面试宝典&#xff08;1&#xff09;——数据库篇&#xff08;MySQL&#xff09; 1.什么是索引&#xff1f; 索引是一种用于加快数据库查询速度的数据结构。 索引可以帮助数据库快速定位到数据库表中特定列的记录&#xff0c;从而加快数据检索和查询的速度。 通过在表的列上…
阅读更多...
protoc初识

protoc初识

protoc初识 参考资料 https://blog.csdn.net/qq_46637011/article/details/135085568 https://blog.csdn.net/qq_39400324/article/details/134172333 一 前期准备 windows查看proto是否安装好及版本号 protoc --version windows查看路径下文件 dir 想在124.50机器上安…
阅读更多...
Linux进程概念（四）：进程优先级 环境变量

Linux进程概念（四）：进程优先级 环境变量

目录 进程优先级 学前补充 命令行参数 环境变量 环境变量$PATH 整体理解环境变量 environ指针 获取环境变量的三种方式 内建指令 证明内建指令的存在 进程优先级 基本概念&#xff1a;指定进程获取某种资源的先后顺序 linux的优先级是进程控制块task_struct中的…
阅读更多...
华为交换机重置密码

华为交换机重置密码

1.进入bootrom 加电后&#xff0c;18S左右&#xff0c;在启动菜单按 CtrlB 进入bootrom&#xff08;3s内&#xff09; 注意&#xff1a;本步骤属于高危操作&#xff0c;一定小心切勿删除系统或修改bootrom密码&#xff01; 输入bootrom密码&#xff0c;按6 看到提示成功后按…
阅读更多...
Django模型的继承

Django模型的继承

Django模型的继承 Django模型的继承&#xff0c;包括模型的抽象基类、Meta继承、related_name和related_query_name属性、多表继承、Meta和多表继承、继承与反向关系、代理模型、代理模型继承和未托管模型&#xff0c;以及多重继承等内容。本文讲解一下抽象基类继承&#xff0…
阅读更多...
详解Qt中实现树状结构图

详解Qt中实现树状结构图

在Qt中&#xff0c;实现树状结构图通常采用QTreeWidget或QTreeView组件。这两个组件都允许我们创建具有层次结构的列表&#xff0c;但它们之间存在一些差异。QTreeWidget提供了更简单的API&#xff0c;适用于轻量级、快速开发的需求&#xff1b;而QTreeView则更为灵活和可定制&…
阅读更多...
.NET WebAPI服务端启用跨源请求 (CORS)

.NET WebAPI服务端启用跨源请求 (CORS)

跨源请求 (CORS)问题 在API服务端启用跨源请求 (CORS) 调用 UseCors 扩展方法并指定 PolicyCorsName CORS 策略。 UseCors 添加 CORS 中间件。 对 UseCors 的调用必须放在 UseRouting 之后&#xff0c;但在 UseAuthorization 之前。 Program.cs添加如下代码&#xff08;注意中间…
阅读更多...
货拉拉0-1数据指标体系构建与应用

货拉拉0-1数据指标体系构建与应用

目录 一、背景 二、指标体系搭建 2.1 指标设计 2.2 指标体系搭建 2.3 指标维度拆解 三、指标标准化建设 四、指标元数据管理 五、指标应用&未来规划 原文大佬介绍的这篇指标体系构建有借鉴意义&#xff0c;现摘抄下来用作沉淀学习。如有侵权请告知~ 一、背景 指标…
阅读更多...
二叉搜索树及相关操作（图文详解）

二叉搜索树及相关操作（图文详解）

1.概念 二叉搜索树又称二叉排序树&#xff0c;它或者是一棵空树&#xff0c;或者是具有以下性质的二叉树: 若它的左子树不为空&#xff0c;则左子树上所有节点的值都小于根节点的值 若它的右子树不为空&#xff0c;则右子树上所有节点的值都大于根节点的值 它的左右子树也分…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023