AutowareAuto泊车案例复现及原理简介

作者 | Lxxxb  编辑 | 汽车人

原文链接：https://mp.weixin.qq.com/s/FbN16w6ftVr4IlkCmy0WSQ

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【全栈算法】技术交流群

后台回复【领域综述】获取自动驾驶全栈近80篇综述论文！

前提准备

AutowareAuto ade环境搭建完成

Lgsvl环境和仿真器配置完成

运行环境建议

如果运行 LGSVL 仿真器，需要搭载 NVIDIA 显卡，具体要求如链接所示，推荐配置如下

• Intel(R) Core(TM) i9-9900KF CPU @ 3.60GHz (16 virtual cores) with 64GB RAM
  

• NVIDIA GeForce RTX 2080 with 8 GB memory

如果运行失败或者效果差，可能是计算资源不足造成。

启动

终端一：启动仿真器

ade --rc .aderc-amd64-foxy-lgsvl start --update --enter
/opt/lgsvl/simulator &

终端二：启动可视化界面

ade enter
source /opt/AutowareAuto/setup.bash
ros2 launch autoware_auto_launch autoware_auto_visualization.launch.py

终端三：

ade enter
source /opt/AutowareAuto/setup.bash
ros2 launch autoware_demos avp_sim.launch.py

这个时候，即可看到如图界面

具体流程介绍

1、设定初始位姿与定位简介

使用2D Pose Estimate指令给车辆设定初始位置。如下图所示

此时，车辆便和LGSVL中位置达成一直一致，如果位置不一致，导航将会失败。

AutowareAuto的定位是如何实现的呢？

首先，需要给定车辆一个初始位姿，这个姿态对于车辆而言就是一个绝对位置，相当于GPS位置，至于为什么不使用GPS定位，在定位一栏中会专门介绍。

在初始位姿给定后，NDT算法会对现有为位姿做一个微调，这里其实就是采用点云信息的匹配做到的。

在之后导航的过程中，NDT算法会一直作用与车辆身上，帮助车辆时刻保持定位准确。

2、导航与内部算法简介

定位结束后，AutowareAuto对于车辆的移动采用定点移动，即利用 2D Nav Goal 插件对车辆发布目标点。如下图所示

此时，车辆将会朝着目标点行使。

AutowareAuto的决策规划是如何实现的呢？

这里首先得讲到AutowareAuto背后的一个逻辑，那就是如何判断某个位置是否是可行区域。其实这就是正式驾驶的前提条件，地图构建。

AutowareAuto的地图构建分为两个部分，一部分为点云地图，一部分为语义地图。点云地图将地图中障碍物信息采集，语义地图则根据点云地图画出。

所以，AutowareAuto规划的一个基础为地图，从地图中可行区域中规划出一条从当前位置到目标位置的路径。

3、泊车案例及其内部算法

从导航原理可知，泊车无非也是从语义地图中读出，哪一些地方属于可停车区域，如下图所示

可是，我们虽然直到了其中的一个本质。可是它具体是如何实现的呢？

从泊车案例思考，其内部的一个算法为混合 A* （Hybrid A*）。混合A*算法是斯坦福大学于2010年提出用于解决在侧方位停车和倒车入库过程中的路径规划问题。

其背后的原理其实和A*算法一致，不同的是，其中加入了车辆运动学模型。

伪代码为：

ASTAR()
{
    INIT START
    INIT END
    INIT_VEC OPENSET
    INIT_VEC FATHERSET
    INIT_VEC OBSTACLE


    OPENSET.ADD(START)
    ADD_OPENSET(START)


    WHILE(!IS_OPENSET_EMPTY()){
        MIN_FN_POINT = FIND_MIN_FN(OPENSET)
        IF(MIN_FN_POINT == END){BREAK}
        SEARCH_ENV_BLOCK(MIN_FN_POINT)
        OPENSET.DELETE(MIN_FN_POINT)
        FATHERSET.ADD(MIN_FN_POINT)
    }


    GET_TRAJECTORY()
}


SEARCH_ENV_BLOCK(MIN_FN_POINT)
{
    FOR(AUTO_P : MIN_FN_POINT){
        COST = MIN_FN_POINT.G + CALCU_G(MIN_FN_POINT, AUTO_P)
        IF((AUTO_P IN FATHERSET) || (AUTO_P IN OBSTACLE)){CONTINUE}
        IF(AUTO_P NOT IN OPENSET){
            OPENSET.ADD(AUTO)
        }ELSE IF(AUTO_P.G > COST){
            UPDATE AUTO_P
        }
    }
}

泊车最后实现如图：

注：一旦自动驾驶，手动驾驶将不再起作用，因为车辆会激活制动器以保持在先前定义的目标位置

重要事项

在之前的操作中，我们都是在 docker ade 环境下执行的 /opt/AutowareAuto 目录下的代码，当读者选择通过自行编译的代码，也就是 /home/${USER}/AutowareAuto 下代码执行上述操作时，会出现车辆不出现，点云地图不出现的情况，在此解释：

地图和车辆都是基于 /opt/AutowareAuto 下配置进行的，如果想要用 AutowareAuto 内配置，需要将点云地图和车辆都换成下载代码中的配置

vehicle_characteristics_param_file = os.path.join(
        get_package_share_directory('autoware_demos'), 'param/vehicle_characteristics.param.yaml')


    vehicle_constants_manager_param_file = os.path.join(
        get_package_share_directory('autoware_auto_launch'), 'param/lexus_rx_hybrid_2016.param.yaml')

同理，之后如果想要更换地图或者更换车辆配置，都需要在launch中作详细目录说明，不然都会指到 /opt 中。

在修改后都需要用 colcon build --packages-select 功能包 来重新编译。

问题与解答

1.车辆在模拟器中没有改变，但是在rviz2中显示十分奇怪，比如卡在某一个位置或者定位不准确

原因：计算资源不够

2.出现如下所示报错：

[object_collision_estimator_node_exe-18] [WARN] [1613130016.283356960] [planning.object_collision_estimator_node]: on_bounding_box cannot transform base_link to map.
[behavior_planner_node_exe-19] [INFO] [1613130016.285522103] [planning.behavior_planner_node]: Waiting for localization result to become available
[lanelet2_global_planner_node_exe-15] [ERROR] [1613130016.285673175] [planning.lanelet2_global_planner_node]: Failed to transform Pose to map frame
[behavior_planner_node_exe-19] [INFO] [1613130016.320455277] [planning.behavior_planner_node]: Waiting for localization result to become available
[lanelet2_global_planner_node_exe-15] [ERROR] [1613130016.321167460] [planning.lanelet2_global_planner_node]: Failed to transform Pose to map frame
[p2d_ndt_localizer_exe-4] [ERROR] [1613130016.337780380] [localization.p2d_ndt_localizer_node]: Could not find a connection between 'map' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.

3.解决方法：重新给定一个绝对坐标

[lanelet2_global_planner_node_exe-15] [ERROR] [planning.lanelet2_global_planner_node]: Global route has not been found!

原因：全局路径规划器规划失败，一方面可能是性能原因，一方面可能是车辆本身机制不允许，比如位置在车身之后。

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

加入我们：自动驾驶之心技术交流群汇总！

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！