【实战篇】autoware运动规划motion planning

文章目录

• 前言

• 一、功能

• 二、输入

• • 【temporal waypoint】
  • 【current velocity】
  • 【current pose】

• 三、输出

• • 【final waypoint】

速度规划器（速度设置）

方案二、路径规划算法

方案二、路径规划算法

（1）详细阐述了规划的基本原理及其理论基础

（2）对核心代码进行了详细解析，并提供了清晰的技术注释

（1）A*算法中规避障碍物的关键节点设计

（2）A*算法的具体实现过程及其搜索机制

（3）速度控制模块的设计与实现

• 方案三、open_planner
• • （1）规划流程概述
• • （2）代码实现过程中的详细说明
• • （1）主要参数设置
• （2）关键轨迹评估机制
• （3）主要轨迹生成逻辑
• （4）行为选择模块（非必选项）
• （5）运动预测模块（非必选项）

方案四：lattice_planner

• 总结
• 参考资料

前言

相关的无人驾驶技术学习资源，请您关注专栏： <>

无人车&无人机导航合集
<>

本文主要讲述运动规划 motion planning的概述。具体细节将在以后逐步呈现。其余部分则可参考我的其他文章

提示：以下是本篇文章正文内容

一、功能

该规划系统依据预设的时间节点 waypoint数据生成局部可行路径以及全局规划路径。综合考虑车辆的状态特征及动态行为特性, 结合三维地图显示的具体位置信息以及周边区域的障碍物分布状况, 同时考量周边环境中的各类物体、交通法规以及目标导向信息等要素, 在局部环境下, 基于当前环境条件计算出一条最优避障且符合目标导向的最佳路径。

.
.

二、输入

【temporal waypoint】

一段带有速度的轨迹
.
.

【current velocity】

定位与车底盘CAN里程计反馈融合得到的车辆当前速度
.
.

【current pose】

定位模块提供的车辆当前位姿
.

三、输出

【final waypoint】

无人车执行的最终轨迹
.
.

四、算法流程实现

【velovity_planner(velocity setting)】

基于base waypoint的基础上提取一小段路径，并分别进行速度规划和设置步骤。生成了一条具有时间相关的轨迹temporal waypoint，并更新了车辆的速度信息数据。发现所跟踪的所有waypoint节点都包含有速度信息字段，在此过程中该模块主要负责根据实际车速调整这些waypoint节点的速度值。从而使得车辆能够更加灵活地在遇到红灯前停下或进行减速操作等常规动作。

速度轨迹优化

速度轨迹优化

Trajectory optimization

速度轨迹优化

速度轨迹优化

【astar_planner】

防盗标记–盒子君hzj

防盗标记–盒子君hzj

防盗标记–盒子君hzj

路径生成–图搜索的方法

经典

【adas_lattice_planner】

采用了State Lattice规划算法来进行实现。基于预先设定的参数列表以及构建好的语义地图信息。通过mini_jerk样条曲线进行局部路径生成及优化处理，在当前位置前方生成了多条可行的路径选项。计算得到了无人车所遵循的最终轨迹点final waypoint，并对该系统能够用于完成障碍物规避任务或车道变换操作进行了验证。

路径生成+轨迹优化–无人驾驶规划器

路径生成+轨迹优化–无人驾驶规划器

五次样条路径生成并Frenet坐标系解耦轨迹优化

.
.

【open_planner】

实现路径生成与优化

路径生成 + 轨迹优化–无人驾驶场景规划器

方案一、waypoints_maker

功能：生成航线、导入路径点、制定基础速度参数、应用插值算法以及直观展示路径节点位置的操作步骤

waypoint_maker功能包包含以下几个节点

     - waypoint_loader
    - waypoint_replanner
    - 
    -  waypoint_saver
    - waypoint_extractor
    
    -  waypoint_creator
    
    - waypoint_marker_publisher
    - waypoint_velocity_visualizer
    
    
      
      
      
      
      
      
      
      
      
      
    
    代码解释

（1）waypoint_loader节点（加载vector_map的csv路点文件）

功能：把.csv文件的waypoints解析转换成ROS message type

.csv文件的格式有三种

    ver1： consist of x, y, z, velocity（no velocity in the first line）
    ver2： consist of x, y, z, yaw, velocity（no velocity in the first line）
    ver3：consist of x,y,z,yaw,velocity,change_flag（ category names are on the first line）
    
    
      
      
      
    
    代码解释

.
.

（2）waypoint_replanner节点（一般在waypoint_loader后进行速度规划）

功能：离线调整航路点（重新采样并重新规划速度）

    在直线时：轨迹加速到最大的速度
    在进入弯道时：提前完成减速，曲线曲率越大减速越多
    在弯道时：保持恒定速度
    在退出弯道时：轨迹开始加速
    
    
      
      
      
      
    
    代码解释

相关参数

• Overview of app tab

• When using the multi_lane feature, select multiple input files. Additionally, to enable lane changes using lane_select, ensure the input is of type ver3.

• Refer to the replanning_mode setting if you wish to adjust the velocity.

• Consult the realtime_tuning_mode option if you aim to tweak waypoints.

• In resample mode, configure the resample_interval parameter accordingly.

• 规划速度参数设置

  • 若需在曲线区域减速，请检查并启用replan curve mode
  • 若需修改所有路径点的速度，请启用并检查overwrite vmax mode
  • 若需在端点处减速，请启用并检查replan endpoint mode
  • 其中，Vmax表示最大速度
  • Rth则定义了提取曲线时的半径阈值
    通过增加该值（Rth），可以在路径提取中提高敏感度

Rmin and Vmin constitute pairs of values employed in velocity replanning.
A designed velocity plan aims to minimize velocity within an assumed sharpest curve.
For each i-th curve, both the minimum radius r_i and corresponding velocity v_i are defined through these equations.
v_i = Vmax − (_Vmax − V_min)/(R_th − R_min) × (R_th − r_i)

The acceleration limit represents the setting used to control the maximum speed.
The deceleration limit indicates the threshold for reducing speed.
The velocity offset refers to an adjustment made prior to establishing a specific velocity profile.
The braking distance signifies the minimum required velocity needed before reaching the endpoint offset.
The endpoint offset corresponds to zero-velocity points at waypoints' endpoints.

.
.

（3）waypoint_saver节点（录制航线生成csv文件）

功能：当注册订阅至" /current_pose "、" /current_velocity（选项） "的主题后，在固定的时间间隔内将数据保存至路点CSV文件中。

csv文件以第三种格式保存

    ver3：consist of x,y,z,yaw,velocity,change_flag（ category names are on the first line）
    
    
      
    
    代码解释

change_flag为0代表直行，change_flag为1代表左转，change_flag为2代表右转

相关参数

    - ~save_filename
    - ~interval
    - ~velocity_topic
    - ~pose_topic
    - ~save_velocity
    
    
      
      
      
      
      
    
    代码解释

.
.

（4）waypoint_extractor节点(加载录制的航线)

功能模块实现了航线数据的提取并将其发布为消息类型autoware_msgs/LaneArray

相关参数

    - ~lane_csv
    - ~lane_topic
    
    
      
      
    
    代码解释

.
.

（5）waypoint_creator节点(根据原来加载的路径点进行路径插值、路径平滑)

功能采用线性插值法, 或可采用样条函数作为插值手段 (option: "linear" or "spline")

注意，这里提供的是一种方法，插值的方法在interpolate.h实现

.
.

（6）waypoint_marker_publisher节点

该系统的核心功能是实现将交通灯、全局路径规划数据、本地路径规划数据以及车道停止点等话题可视化到RVIZ平台。

.
.

（7）waypoint_velocity_visualizer节点

功能：把路径的速度可视化到RVIZ上

.
.

方案二、waypoint_planner（astar做局部规划）

（A*是负责基于栅格地图的规划探索）

（1）规划原理介绍

功能：航路点上的避让行为决策逻辑、动态速度规划等等

要启动astar进行局部规划, 只需将astar的标志位设置为true即可（在对应的launch文件中）

基于之前建立的lane_planner规划所得出的全局路径base_waypoints之上，在waypoint_planner中采用了A算法来进行局部避障操作，并成功生成了一条安全可靠的safety_waypoints路径。
其中,A搜索作为该算法的核心机制,A避障则针对A策略的一些先决条件进行了优化设置。
这些安全路径可用于车辆控制模块的应用中。

waypoint_planner功能包对应的节点

    astar_avoid
    astar_search
    Velocity_set
    
    
      
      
      
    
    代码解释

.

（2）规划代码解析及注释

rqt_graph可以查看核对一下的

Rqt-Graph可被审查或核查

Rqt-Graph可被审查或核查

该方案A仅规划一次,因而难以处理动态障碍物.只需修改逻辑使其边行进边执行A规划,持续不断地规划就能有效规避动态障碍.

（1）astar_avoid节点

功能：
前置逻辑功能模块用于初始化A*算法。
该模块采用基于if-else条件下的简单逻辑决策。
其包含planning、stop和replan等多种状态。
规划路径终点target由障碍物obsta的位置决定（可设置多个目标点goal供选择），起点则可通过当前定位获得。

astar_avoid对象分为中继模式和避让模式两种类型。通过配置选项来控制这些模式的启用。

-中继模式：不进行避障规划，正常沿着全局路径行驶

避障策略：采用混合A*搜索算法实现路径规划，并利用动态状态转换机制规避障碍物。当您配置了ADAS地图中的wayarea参数时，在costmap_generator节点中启用Use wayarea选项后可以在规划过程中限定可行区域，并进一步提升系统的安全性能。具体实现细节可参考freespace_planner/README.md

（2）astar算法的实现astar_search节点

提醒大家注意一下，在经过基础上进行优化升级后的autoware A*算法版本值得推荐使用；然而，在实际应用中这些算法往往表现不佳。

.

（3）Velocity_set速度规划模块

safety_waypoint旨在规划位置，
final_waypoint也对速度进行了规划
velicity_set用于发布final_waypoint
velicity_set仅对轨迹的位置做出了小幅调整，在紧急状况中（例如行人过马路时的停车行为、遇到移动障碍物时采取减速措施等）对轨迹的速度进行了优化修改

直接用点云的数量来判断是否为障碍物！
.

方案三、open_planner

局部动态轨迹级规划

局部动态路径级优化

局部动态路径级优化

（1）规划原理流程介绍

/lane_waypoints_array是从mission_planner得到的全局路径

（1）op_common_param

（2）轨迹生成/op_trajectory_generator
轨迹生成的过程，在生成过程中主要基于...模型进行运算。
需要注意的是，该系统主要利用vector_map中的多条lane_waypoints_array数据进行轨迹计算。

（3）轨迹代价评分/op_trajectory_Evaluator
轨迹优化过程
在Open Planner中会基于全局路径生成多条候选路线；每条路线都会被赋予一个评价值；算法会选择评价值最低的那种路线；其中不可通行的道路被标注为红色线段；该系统能够实现在线规划。

（2）规划代码解析及注释

open_planner的原理

（1）op_common_params（主要）

（2）op_trajectory_evaluator（主要）

（3）op_trajectory_generator（主要）

（4）op_behavior_selector（可选）

（5）op_motion_predictor（可选）

从new_op_planner.launch看起

.
.

方案四、lattice_planner

（1）规划原理流程介绍

待补充…
.
.

（2）规划代码解析及注释

（1）lattice_trajectory_gen（主要）

（2）lattice_twist_convert（主要）

（3）path_select（可选）

（4）lattice_velocity_set（可选）

.
.

总结

参考资料