基于栅格搜索轨迹规划方法、系统、汽车、设备及介质与流程

1.本发明涉及汽车自动驾驶技术，具体是用于复杂环境中的低速行车动态轨迹规划，包括避障，车道内偏移等实时路径规划。


背景技术：

2.轨迹规划算法作为自动驾驶系统的基础和技术核心，其作用是在遵守车辆动力学及运动学、驾驶道路环境以及交通法规等时空约束条件的基础上，生成衔接车辆起点和终点的集合路径。其中，图搜索是轨迹规划中常用的一种方法。图搜索通常通过高精度地图信息和传感器识别的环境信息生成栅格，然后用搜索算法在生成的栅格上搜索路径。公开号cn111857160a发明名称“一种无人车路径规划方法和装置”的发明专利申请，引入了人工势场法获取每个栅格的斥力系数，采用混合a星进行路径搜索，采用梯度下降的算法对轨迹进行平滑，其专利的核心还是基于混合a星。人工势场的引入，确实可以提高搜索的效率，但是复杂环境下，需要计算每一个栅格的斥力系数，算力要求很高，同时混合a星算法本身对算力需要也很高。整个方案在工程化中很难实现动态时时规划。公开号cn113267199a名称“行驶轨迹规划方法及装置”的中国发明专利申请，通过对基准线进行偏移生成多条路径，然后计算每一条候选路径的代价函数，最终选择最优的路径，能够实现动态场景的规划，公开号cn108241370b，名称“通过栅格地图获取避障路径的方法及装置”的中国发明专利，公开了一种通过栅格地图获取避障路径的方法，通过获取需避障区域的静态全局地图；将所述静态栅格地图中的每个栅格作为一个栅格点；在静态栅格地图中确定障碍物区域；确定可达区域中各栅格点的障碍代价值；根据起点及终点在所述静态栅格地图中的位置以及所述可达区域中各栅格点的障碍代价值确定避障路径。但是上述方法的核心思想更接近于采样优化，只是基于栅格地图进行规划而已，并未用到任何搜索算法，算力较低。公开号cn113561968a“一种车辆水平泊车轨迹规划算法及装置、车辆、存储介质”的发明专利申请，是一种基于几何的泊车规划方法，不适合行车场景的规划。公开号 cn 113467456 a“一种未知环境下用于特定目标搜索的路径规划算法”的发明专利申请也是提出了一种新的路径规划算法，所不同的是路径规划的核心算法是利用人工势场引导的rrt规划算法。
3.因此，有必要开发一种能够充分利用栅格便捷表示周围环境，同时对算力要求低，便于工程化，抗干扰能力强，能够满足行车过程中时时动态规划的轨迹规划方法。


技术实现要素：

4.本发明针对现有技术中存在的复杂环境下行驶轨迹规划对算力要求很高，抗干扰能力不强，工程化应用不方便等问题，提出一种基于栅格搜索的避障轨迹规划方法。在城区自动驾驶、地下车库自动驾驶行车场景，由于存在墙壁、柱子、路沿等各种不规则的障碍物，这些障碍物很难用常规提取出障碍物目标的属性表示，此时的障碍物通常采用障碍物点云表示。本发明中提出的方法能够在较小算力的情况下实现对障碍物点云的栅格化。
5.本发明解决上述技术问题的技术方案是：通过传感器探测障碍物目标类别、速度、
轮廓等信息，通过定位系统、全局路径规划定位当前局部路径规划需要的参考基准线，综合参考基准线和障碍物目标信息，采用基于栅格图搜索的方式寻找最佳路径，并通过样条拟合等方式对路径进行平滑和优化，最终输出可执行的规划路径，进而通过控制模块，实现对轨迹的跟随控制。主要是基于参考基准线，在frenet坐标系下，生成栅格，其中，沿着参考基准线方向定义为“行”，垂直参考基准线方向定义为“列”，同时将地图边界信息、传感器感知信息投射到栅格中，形成规划空间。在进行图搜索时，不再采用a*等搜索方式，对逐个节点进行搜索，为了提高效率，直接对规划空间的“列”进行搜索，寻找最优的“列”，形成安全基准线，然后根据当前车辆位置、栅格可通行区域情况，计算每一“行”栅格的最优值，最后对搜索生成的路径进行平滑优化。由于搜索方式改进，能够在较短周期内完成搜索，在对车辆行驶路径动态规划中可以适用较简单的算法，节约算力，易于工程实现，实现复杂路径下车辆动态规划路径目的。具体为：一种基于栅格搜索的避障轨迹规划方法，根据车辆当前位置，匹配全局路径上的点，选取从当前车辆位置前行方向固定距离的路径点，作为局部路径规划的参考基准线，根据参考基准线和车辆感知信息进行轨迹规划，输出规划轨迹给横纵向控制模块，再通过整车执行机构实现对整车的控制，所述进行轨迹规划具体为：根据参考基准线和感知信息生成栅格，在栅格中添加高精度地图边界，在栅格中投影障碍物并进行膨胀，根据当前车辆位置及运动学约束处理栅格，判断是否存在完全可通行“列”，如果存在完全可通行列，对安全线进行防抖动处理，并生成可通行路径，对可通行路径进行平滑处理，如果不存在完全可通行列，寻找是否存在左侧或右侧绕障安全线，如果存在左侧或右侧绕障安全线就对安全线进行防抖动处理，生成可通行路径并进行平滑处理，如果不存在左侧或右侧绕障安全线，输出上一时刻路径作为当前路径。
6.进一步优选，传感模块检测到障碍物信息进行感知融合、环境认知处理，输出到局部路径规划模块，局部路径规划模块根据参考轨迹在frenet坐标系下生成栅格，其中，沿着参考基准线方向定义为“行”，“行”间距为参考基准线两个点之间的距离，垂直参考基准线方向定义为“列”。根据车速和控制器运算能力确定“行”间距，根据道路的宽度确定“列”的间距。“行”间距与“列”间距比值满足一定约束要求，此约束来源于车辆运动学要求。
7.进一步优选，栅格的“行”从车辆当前位置到参考线方向逐渐增大，栅格的“列”为由左向右逐渐增大，可通行栅格的值设置为1，不可通行的栅格值设置为0。
8.进一步优选，所述在栅格中添加高精度地图边界具体为，将高精度地图的边界在栅格中进行投影，边界以外的栅格为不可通行区域，边界内的栅格为可通行区域，将感知障碍物和障碍物点云膨胀后投入栅格，有障碍物遮挡的栅格为不可通行区域。所述安全线，是基于搜索确定平移距离后对参考基准线按照“列”方向进行平移后的线。
9.进一步优选，所述生成可通行路径具体包括：根据当前车辆所在栅格中的位置、车辆运动学约束，生成基于当前车辆位姿的可通行栅格；计算基于当前车辆位姿可通行栅格中每一“行”到安全线代价最小的栅格坐标，形成搜索后的路径为通行路径。
10.进一步优选，在栅格中投影障碍物包括：从传感器模块获取目标障碍物的类型、动静属性、顶点位置；将障碍物顶点转换到参考基准线的坐标系；寻找障碍物中每个顶点坐标所对应的栅格；对坐标顶点对应的栅格进行膨胀，膨胀范围内的栅格均为不可通行区域。
11.进一步优选，在栅格中投影障碍物包括：将障碍物点云转换到参考基准线的坐标系；根据自动驾驶场景，选取栅格的行、列生成闭合区域；闭合区域内的障碍物点云为有效
点云，计算每一个有效点云到参考基准线的垂线距离和对应的纵向距离；根据垂线距离值对有效点云进行分类；根据点云对应在参考基准线上的纵向距离进行排序；根据有效点云到参考基准线距离、分类、排序情况，对所有点云进行合并投射到栅格中。
12.进一步优选，判断是否存在完全可通行的“列”具体包括，如果栅格中当前列对应的行上均为1为通行列，形成完全可通行列集；形成以参考基准线所在“列”，向左右两侧按照垂直距离大小排序的搜索列表；根据搜索列表，按照限定的搜索宽度在可完全通行列集中进行搜索。
13.进一步优选，寻找是否存在左侧或右侧绕障的安全线进一步包括，选取“列”值小于参考基准线的区域，判断寻找该区域内每一“行”中最小可通行的“列”值，此“列”即为左侧绕障安全线。选取“列”值大于参考基准线的区域，判断寻找该区域内每一“行”中最大可通行的“列”值，此“列”即为右侧绕障安全线。
14.本发明还提出一种基于栅格搜索的避障轨迹规划系统，包括：传感模块、定位模块、全局路径规划模块、局部路径规划模块、横纵向控制模块、参考基准线模块、整车执行机构，参考基准线模块根据定位模块提供的车辆当前位置，匹配全局路径规划模块确定的全局路径上的点，选取从当前车辆位置前行方向固定距离的路径点，作为局部路径规划的参考基准线，局部路径规划模块根据参考基准线和传感模块提供的感知信息进行轨迹规划，输出轨迹给横纵向控制模块，再通过整车执行机构实现对整车的控制，所述轨迹规划为：根据参考基准线和感知信息生成栅格，在栅格中添加高精度地图边界，在栅格中投影障碍物并进行膨胀，根据当前车辆位置及运动学约束处理栅格，判断是否存在完全可通行“列”，如果存在完全可通行列，对安全线进行防抖动处理，并生成可通行路径，对可通行路径进行平滑处理，如果不存在完全可通行列，寻找是否存在左侧或右侧绕障安全线，如果存在左侧或右侧绕障安全线就对安全线进行防抖动处理，生成可通行路径并进行平滑处理，如果不存在左侧或右侧绕障安全线，输出上一时刻路径作为当前路径。
15.进一步优选，传感模块检测到障碍物信息进行感知融合、环境认知处理，输出到局部路径规划模块，局部路径规划模块根据参考轨迹在frenet坐标系下生成栅格，其中，沿着参考基准线方向定义为“行”，“行”间距为参考基准线两个点之间的距离，垂直参考基准线方向定义为“列”，根据车速和控制器运算能力确定“行”间距，根据道路的宽度确定“列”的间距。“行”间距与“列”间距比值满足一定约束要求，此约束来源于车辆运动学要求。
16.进一步优选，在栅格中投影障碍物包括：从传感器模块获取目标障碍物的类型、动静属性、顶点位置；将障碍物顶点转换到参考基准线的坐标系；寻找障碍物中每个顶点坐标所对应的栅格；对坐标顶点对应的栅格进行膨胀，膨胀范围内的栅格均为不可通行区域。
17.进一步优选，在栅格中投影障碍物包括：将障碍物点云转换到参考基准线的坐标系；根据自动驾驶场景，选取栅格的行、列生成闭合区域；闭合区域内的障碍物点云为有效点云，计算每一个有效点云到参考基准线的垂线距离和对应的纵向距离；根据垂线距离值对有效点云进行分类；根据点云对应在参考基准线上的纵向距离进行排序；根据有效点云到参考基准线距离、分类、排序情况，对所有点云进行合并投射到栅格中。
18.第三方面，本发明还提出一种汽车，包括如上所述的基于栅格搜索的避障轨迹规划系统。
19.第四方面，本发明还提出一种控制设备，包括：处理器和存储器；其中，所述处理器
通过读取所述存储器中存储的可执行程序代码来运行与所述可执行程序代码对应的程序，以用于实现如上所述的基于栅格搜索的避障轨迹规划方法。
20.第五方面，本发明提出一种计算机可读存储介质，所述可读存储介质上存储程序或指令，所述程序或指令能够被处理器加载和运行以执行如权上所述的基于栅格搜索的避障轨迹规划方法。
21.本发明在实现避障规划规划的过程中，不再对每一个节点进行遍历搜索，而是通过对“列”的搜索，减少了搜索的次数，找到安全线，然后计算每一“行”可通行栅格到安全线代价最小的栅格，从而形成规划路径。可以最大限度的节省控制器运算资源，并实现动态场景情况下路径的时时规划。
附图说明
22.图1本发明基于栅格搜索的避障轨迹规划系统架构图；图2本发明基于栅格搜索的避障轨迹规划流程图；图3可通行栅格示意图。
具体实施方式
23.为方便理解本发明，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
24.下面结合附图对本发明作进一步说明。
25.如图1所示为本发明基于栅格搜索的避障轨迹规划系统架构图，包括，传感模块（1）、定位模块（2）、全局路径规划模块（3）、参考基准线模块（4）、局部路径规划模块（5）、横纵向控制模块（6）、整车执行机构（7）。参考基准线模块根据定位模块提供的车辆当前位置，匹配全局路径规划模块确定的全局路径上的点，从而选取从当前车辆位置前行方向固定距离的路径点，作为局部路径规划的参考基准线，局部路径规划模块根据参考基准线和传感模块提供的感知信息进行轨迹规划，输出轨迹给横纵向控制模块，再通过整车执行机构实现对整车的控制，从而实现自动驾驶功能。
26.所述传感系统，主要是基于视觉传感器、激光雷达、毫米波雷达、超声波雷达等传感器，用于对障碍物目标、车道线等信息进行检测；所述定位系统，主要提供当前车辆的位置、航向信息；所述全局路径系统，主要是根据起点、终点和地图信息，规划一条连接起点到终点的路径；所述参考基准线模块，根据车辆当前的位置匹配在全局路径上的点，从而选取从当前车辆位置前行方向固定距离的路径点，作为局部路径规划的参考基准线；所述局部路径规划模块，根据传感信息、参考基准线信息，进行避障、纵向速度等轨迹规划；所述横纵向控制，根据局部路径规划的轨迹，进行横纵向控制，确保车辆在一定误差范围内跟随轨迹；所述整车执行机构，整车执行机构包括ems、esp、eps、挡位、bcm等，其中eps接收横向控制角度请求信息，ems接收纵向控制扭矩请求信息，esp接收纵向控制减速度请求信息。
27.图2所示为本发明基于栅格搜索的避障轨迹规划流程图。根据参考基准线和感知信息生成栅格，在栅格中添加高精度地图边界，在栅格中投影障碍物并进行膨胀，根据当前
车辆位置及运动学约束处理栅格，是否存在完全可通行“列”，如果存在完全可通行列，对安全线进行防抖动处理，并生成路径，对路径进行平滑处理。如果不存在完全可通行列，寻找是否存在左侧或右侧安全线，如果存在左侧或右侧绕障安全线就对安全线进行防抖动处理，生成路径并进行平滑处理，如果不存在左侧或右侧安全线，输出上一时刻路径。
28.当本车处于自动驾驶模式；传感模块检测到障碍物信息进行感知融合、环境认知处理，然后输出到局部路径规划模块，同时，参考基准线模块输出参考轨迹到局部路径规划模块；局部路径规划模块在frenet坐标系下生成栅格，其中，沿着参考基准线方向定义为“行”，“行”间距为参考基准线两个点之间的距离，此距离与车速、控制器运算能力有关，垂直参考基准线方向定义为“列”，“列”间距与“行”间距的比例关系需考虑车辆的运动学信息，同时，“列”的总数考虑车辆行驶区域的宽度。在本实施例中，栅格的“行”从车辆当前位置到参考线前方逐渐增大，栅格的“列”为由左向右逐渐增大，可通行的栅格的值设置为1，不可通行的栅格值设置为0；将高精度地图中的边界在栅格中进行投影，边界以外的栅格为不可通行区域，边界内的栅格为可通行区域；将感知障碍物和障碍物点云膨胀后投入栅格，有障碍物遮挡的栅格为不可通行区域。
29.根据车辆运动学约束，对形成的栅格进行处理。主要处理的原则是基于当前可通行的区域，车辆在满足车辆运动学约束的情况下，该栅格是可到达的，否则，即使该栅格是在高精度地图边界内，也没有障碍物填充，依然会处理为不可通行栅格；搜索确定安全线。所述安全线，即是对参考基准线按照“列”方向进行平移后的线，其平移的具体值基于搜索实现。
30.为了防止障碍物的不稳定导致安全线的波动，对安全线进行防抖处理，处理方法可采用本领域技术人员采用的常规的防抖动处理方法。
31.根据当前车辆所在栅格中的位置、车辆运动学约束的处理，生成基于当前车辆位姿的可通行栅格。
32.计算基于当前车辆位姿可通行栅格中每一“行”到安全线代价最小的栅格坐标，形成搜索后的路径。
33.对生成的路径进行平滑处理，可以但不限于用样条插值、最小二乘法等方法进行平滑处理。
34.由于复杂场景很难用障碍物完整表征，需要用点云进行补充，障碍物的栅格投射分为两个部分，具体可采用如下所示方式将障碍物投射到栅格，实施方式一，顶点投射方式：（1）从传感器模块获取目标障碍物的类型、动静属性、顶点位置；（2）将目标顶点转换到参考基准线的坐标系；（3）寻找每个目标中每个顶点坐标所对应的栅格；（4）对坐标顶点对应的栅格进行膨胀，膨胀范围内的栅格均处理成不可通行区域。
35.实施方式二，点云投射方式，障碍物点云投射到栅格。
36.（1）将障碍物点云转换到参考路径对应的坐标系下；（2）根据不同的自动驾驶场景，选取栅格的行、列生成闭合区域，例如选取栅格的
第2-30列，1-50行形成闭合区域；（3）闭合区域内的障碍物点云为有效点云，计算每一个点云到参考基准线的垂线距离和对应在参考基准线上的纵向距离，其中，可定义参考基准线垂线距离左侧为负，右侧为正；（4）根据垂线距离的正负值对感知点云进行左右分类；（5）根据点云对应在参考基准线上的纵向距离进行由近及远排序，其中，距离本车越近的点云优先级越高；（6）根据计算的距离、分类、排序情况，对所有点云进行合并处理；（7）对点云进行碰撞处理后，投射到栅格中。
37.对于搜索确定安全线方式，本实施例具体可采用如下搜索方法：（1）判断是否存在完全可通行的“列”，完全可通行的“列”是指这一“列”栅格上每一“行”的栅格均是可通行的。具体判断方法是基于参考基准线，形成以参考基准线所在“列”，向左右两侧按照垂直距离大小排序的搜索列表。根据搜索列表进行搜索，如果搜索成功，则对安全线进行防抖处理，否则执行下一步搜索。安全可通行“列”详细的搜索步骤如下：第一步：在形成的栅格中查找完全可通行的列，具体方法可检查栅格每一列对应的行上是否均为1，如果是，则该列为可通行列，最终形成完全可通行列集。
38.第二步：根据参考基准线排序生成搜索列表，生成列表的原则是左侧搜索优先，从当前参考基准线所在列开始左右排序。例如当前基准线所在列号为21，其中一种生成的搜索列表为{21、20、22、19、23、17
……
}。
39.第三步：根据搜索列表，按照限定的搜索宽度，在生成的完全可通行列集中进行搜索，如果当前搜索的列范围内，完全可通行列集存在，表示搜索成功，对安全线进行防抖处理，如搜索不成功则进行下一步搜索。如下举一实例予以说明：所述搜索宽度是指当前列向左右延伸的范围，例如，搜索宽度2，当前列为21，则应该搜索的列范围为{19、20、21、22、23}，如果完全可通行列中有19、20、21、22、23，则表示搜索成功，否则为不成功，（2）左侧绕行优先，即优先搜索参考基准线左侧是否存在安全线。具体方法是选取左侧感兴趣区域，即“列”值小于参考基准线的区域。判断寻找该区域内每一“行”中最小可通行的“列”值，此“列”即为左侧绕障的安全线，如果搜索成功，对安全线进行防抖处理，如搜索不成功则进行下一步搜索。
40.（3）右侧绕行，即搜索参考基准线右侧是否存在安全线。具体方法是选取右侧感兴趣区域，即“列”值大于参考基准线的区域。判断寻找该区域内每一“行”中最大可通行的“列”值，此“列”即为即为右侧绕障的安全线，如果搜索成功，对安全线进行防抖处理，否则当前区域为不可通行区域，保持前一时刻路径，执行停车操作。
41.对安全线进行防抖处理，处理的目的是为了防止障碍物的不稳定导致安全线的波动。根据当前车辆所在栅格中的位置、车辆运动学约束的处理，生成基于当前车辆位姿的可通行栅格。
42.基于当前车辆位置及车辆运动学约束计算可通行栅格的一种方法描述如下：（1）根据车辆定位信息，匹配所在栅格中的位置，（2）车辆从当前栅格出发，最多能够达到下一“行”左中右三个栅格，
图3所示为可通行栅格示意图，其中，数字2表示当前车辆所在的位置，数字3表示由于障碍物填充导致的不可通行栅格，数字1为可通行栅格，数字0为根据车辆运动学约束生成的不同通行栅格。在生成最终在行车可通行栅格时，数字2只能向下一列中的左中又3个位置的栅格移动，所以只有这3个栅格值为1，这一列其他栅格尽管没有被障碍物占据，依然填充0，作为不可通行栅格对待。
43.尽管只是结合了有限数量的实施例来详细解释，但本发明并不仅仅限于说明书和实施例中所列运用，它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的工程技术人员而言，可容易地实现的修改、补充和替代，因此，在不背离权利要求及等同范围所限定的一般概念下，本发明不应视为受先前描述所限。