基于时空联合规划的自动驾驶走廊优化构建方法及系统与流程

1.本发明涉及自动驾驶轨迹规划技术领域，具体地，涉及一种基于时空联合规划的自动驾驶走廊优化构建方法及系统。


背景技术：

2.自动驾驶作为未来交通的重要组成部分，相关的技术发展也被逐渐的重视。在自动驾驶中，轨迹规划承担着承上启下的作用，一条安全、合理、舒适且足够平滑的轨迹，是影响下游是否可以实现安全跟踪、用户体验是否满意的关键。
3.专利文献cn115416693a公开一种基于时空走廊的自动驾驶轨迹规划方法及系统，包括：实时获取自车的位姿信息和自车周围的障碍物；根据所述自车的位姿信息确定所述自车行驶的参考线并构建三维配置空间；在三维配置空间中标识障碍物并在每一目标时间点处截取s-l平面，得到二维凸子空间；对每相邻两个目标时间点处的所有二维凸子空间构建三维凸子空间；根据自车在所述规划时域内的粗略轨迹和所有三维凸子空间确定轨迹规划的时空走廊；根据时空走廊和所述粗略轨迹进行纵向轨迹的优化和横向轨迹的优化，并根据优化后的纵向轨迹和优化后的横向轨迹确定自动驾驶轨迹。该方法中每相邻两点均构建虽然缩小了可行驶区域，且空间数量直线上升，但导致后期优化产生不必要耗时。
4.因此，在基于时空联合规划的自动驾驶走廊优化构建中，保证空间完全包涵所有种子的前提下，降低生成空间个数，获取更大的可优化空间，减少后期优化耗时，是亟待解决的问题。


技术实现要素：

5.针对现有技术中的缺陷，本发明的目的是提供一种基于时空联合规划的自动驾驶走廊优化构建方法及系统。
6.根据本发明提供的一种基于时空联合规划的自动驾驶走廊优化构建方法，包括：
7.步骤s1：获取当前规划初始位置、指引点种子和环境表征信息；
8.步骤s2：将所述指引点种子和环境表征信息投影到参考线坐标系，得到对应的frenet坐标，同时根据当前规划初始位置初始化语义地图；
9.步骤s3：将当前环境表征信息和指引点种子分别由frenet坐标转换为对应的语义地图坐标；
10.步骤s4：选取初始两个种子构成初始走廊空间并检查所述走廊覆盖的语义地图中是否包含障碍物，若是，则返回错误；若否，则执行步骤s5；
11.步骤s5：对所述初始走廊空间进行膨胀，进而得到可通行的语义走廊。
12.优选地，所述初始化语义地图包括以当前规划的初始位置为原点，分别根据s轴、l轴、t轴三个维度的分辨率及范围构建三维网格地图，并将三维网格地图中每个点的值初始化为0；
13.其中，s表示相对于参考线的纵向距离，l表示相对于参考线的横向距离，t表示以
当前时刻为起始时刻的相对时间。
14.优选地，所述环境表征信息包括障碍物位姿随时间变化的动态语义信息、障碍物位姿在时间段内不变的静态语义信息；
15.将当前环境表征信息由frenet坐标转换为对应的语义地图坐标包括：
16.步骤s3.1：针对动态语义信息依次获取不同障碍物、不同时刻的角点位置，针对静态语义信息将静态障碍物所占位置扩展到每个时间点上，得到对应时间点的静态障碍物位置坐标；
17.步骤s3.2：根据实际的角点frenet坐标，将位置信息依据初始化语义地图所设定的分辨率转换为对应的语义地图坐标；
18.步骤s3.3：判断所述语义地图坐标是否在设定范围内，若是，则根据转换后的不同时刻的障碍物角点坐标，将对应的障碍物所占位置进行填充；若否，则排除该位置点。
19.优选地，步骤s5包括：
20.步骤s5.1：选取初始两个种子构建第一个初始空间；
21.步骤s5.2：判断当前初始空间是否与障碍物存在碰撞，若否，则依次对s轴、l轴的正负方向进行膨胀；若是，则返回错误并退出；
22.步骤s5.3：判断当前位置点是否属于上一个空间，若是，则压入对应空间包含的种子中，并取上一个空间中最后一个点以及不属于所述空间的第一个点进行膨胀；若否，则选取上一个点与当前点，生成新的一个初始空间，执行步骤s5.2；
23.步骤s5.4：将依次获得的空间压入对应的空间列表中，最终获得的可以通行的语义走廊。
24.优选地，所述膨胀是根据预设的膨胀步数，按照预设方向进行扩展；所述膨胀根据当前方向是否有障碍物决定，若有障碍物则膨胀结束，若无障碍物则膨胀继续；
25.当所有方向的膨胀结束时，对应空间膨胀结束。
26.根据本发明提供的一种基于时空联合规划的自动驾驶走廊优化构建系统，包括：
27.模块m1：获取当前规划初始位置、指引点种子和环境表征信息；
28.模块m2：将所述指引点种子和环境表征信息投影到参考线坐标系，得到对应的frenet坐标，同时根据当前规划初始位置初始化语义地图；
29.模块m3：将当前环境表征信息和指引点种子分别由frenet坐标转换为对应的语义地图坐标；
30.模块m4：选取初始两个种子构成初始走廊空间并检查所述走廊覆盖的语义地图中是否包含障碍物，若是，则返回错误；若否，则触发模块m5；
31.模块m5：对所述初始走廊空间进行膨胀，进而得到可通行的语义走廊。
32.优选地，所述初始化语义地图包括以当前规划的初始位置为原点，分别根据s轴、l轴、t轴三个维度的分辨率及范围构建三维网格地图，并将三维网格地图中每个点的值初始化为0；
33.其中，s表示相对于参考线的纵向距离，l表示相对于参考线的横向距离，t表示以当前时刻为起始时刻的相对时间。
34.优选地，所述环境表征信息包括障碍物位姿随时间变化的动态语义信息、障碍物位姿在时间段内不变的静态语义信息；
35.将当前环境表征信息由frenet坐标转换为对应的语义地图坐标包括：
36.模块m3.1：针对动态语义信息依次获取不同障碍物、不同时刻的角点位置，针对静态语义信息将静态障碍物所占位置扩展到每个时间点上，得到对应时间点的静态障碍物位置坐标；
37.模块m3.2：根据实际的角点frenet坐标，将位置信息依据初始化语义地图所设定的分辨率转换为对应的语义地图坐标；
38.模块m3.3：判断所述语义地图坐标是否在设定范围内，若是，则根据转换后的不同时刻的障碍物角点坐标，将对应的障碍物所占位置进行填充；若否，则排除该位置点。
39.优选地，模块m5包括：
40.模块m5.1：选取初始两个种子构建第一个初始空间；
41.模块m5.2：判断当前初始空间是否与障碍物存在碰撞，若否，则依次对s轴、l轴的正负方向进行膨胀；若是，则返回错误并退出；
42.模块m5.3：判断当前位置点是否属于上一个空间，若是，则压入对应空间包含的种子中，并取上一个空间中最后一个点以及不属于所述空间的第一个点进行膨胀；若否，则选取上一个点与当前点，生成新的一个初始空间，触发模块m5.2；
43.模块m5.4：将依次获得的空间压入对应的空间列表中，最终获得的可以通行的语义走廊。
44.优选地，所述膨胀是根据预设的膨胀步数，按照预设方向进行扩展；所述膨胀根据当前方向是否有障碍物决定，若有障碍物则膨胀结束，若无障碍物则膨胀继续；
45.当所有方向的膨胀结束时，对应空间膨胀结束。
46.与现有技术相比，本发明具有如下的有益效果：
47.1、本发明通过对时间和空间两个方面构建未来一定时间内可以通行的可行驶区域，在可行驶区域中得到一条横纵向均高阶可导的轨迹，提高了原始路径得到轨迹的合理性；
48.2、本发明依据参考线方向建立走廊可以更好的增强轨迹可通行空间与参考线的相关性，在道路存在较大曲率的情况下，相比于笛卡尔坐标系能更精确的获取可通行区域，减少为完全覆盖弯道区域所造成的冗余；
49.3、本发明将走廊以时间为基准拆分为横向和纵向两个层面主要是由于在城市路况所允许的车速下，在生成轨迹时以时间为基准分别考虑横向和纵向可以获得更平滑合理的横向速度以及加速度；
50.4、本发明采取膨胀的方式扩展初始空间，可以在合理的最大限度内获得较为精确的空间，提高后期轨迹优化的成功率。
附图说明
51.通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
52.图1为本发明的工作流程示意图；
53.图2为本发明中依次对s轴、l轴的正负方向进行膨胀的流程示意图。
具体实施方式
54.下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
55.实施例一
56.根据本发明提供的一种基于时空联合规划的自动驾驶走廊优化构建方法，如图1所示，包括：
57.步骤s1：获取当前规划初始位置、指引点种子和环境表征信息。环境表征信息包括静态障碍物位姿、动态障碍物位姿、车道边界线、停止线等，获取时根据需要对环境表征进行筛选。
58.步骤s2：将所述指引点种子和环境表征信息投影到参考线坐标系，得到对应的frenet坐标(s,l,t)，同时根据当前规划初始位置初始化语义地图。所述初始化语义地图包括以当前规划的初始位置为原点，分别根据s轴、l轴、t轴三个维度的分辨率及范围构建三维网格地图，并将三维网格地图中每个点的值初始化为0。其中，s表示相对于参考线的纵向距离，l表示相对于参考线的横向距离，t表示以当前时刻为起始时刻的相对时间。
59.步骤s3：将当前环境表征信息和指引点种子分别由frenet坐标转换为对应的语义地图坐标。步骤s1中获取的环境表征信息包括两类，分别为：位姿随时间变化的动态语义信息、位姿在时间段内不变的静态语义信息。其中，动态语义信息包括动态障碍物的位姿，即对应时刻的障碍物质心位置，障碍物的角点位置等。静态语音信息包括静态障碍物，规则限定的可行驶边界，停止线等。具体地，将当前环境表征信息由frenet坐标转换为对应的语义地图坐标包括：
60.步骤s3.1：针对动态语义信息依次获取不同障碍物、不同时刻的角点位置，即对应的(s,l,t)，针对静态语义信息将静态障碍物所占位置扩展到每个时间点上，得到对应时间点的静态障碍物位置坐标。
61.步骤s3.2：根据实际的角点frenet坐标，将位置信息依据初始化语义地图所设定的分辨率转换为对应的语义地图坐标。
62.步骤s3.3：判断所述语义地图坐标是否在设定范围内，若是，则根据转换后的不同时刻的障碍物角点坐标，将对应的障碍物所占位置进行填充；若否，则排除该位置点。
63.步骤s4：选取初始两个种子构成初始走廊空间并检查所述走廊覆盖的语义地图中是否包含障碍物，若是，则返回错误；若否，则执行步骤s5。
64.步骤s5：对所述初始走廊空间进行膨胀，进而得到可通行的语义走廊。步骤s5包括：步骤s5.1：选取初始两个种子构建第一个初始空间。步骤s5.2：判断当前初始空间是否与障碍物存在碰撞，若否，则依次对s轴、l轴的正负方向进行膨胀；若是，则返回错误并退出。步骤s5.3：判断当前位置点是否属于上一个空间，若是，则压入对应空间包含的种子中，并取上一个空间中最后一个点以及不属于所述空间的第一个点进行膨胀。若否，则选取上一个点与当前点，生成新的一个初始空间，执行步骤s5.2。步骤s5.4：将依次获得的空间压入对应的空间列表中，最终获得的可以通行的语义走廊。
65.具体地，所述判断当前位置点是否属于上一个空间为判断被筛除的原始种子是否
属于该初始空间，若不属于则加入该种子，并重新构建该初始空间，进而在保证空间完全包含所有种子的前提下，降低生成空间个数，获取更大的可优化空间。
66.具体地，所述膨胀是根据预设的膨胀步数，按照预设方向进行扩展。所述膨胀根据当前方向是否有障碍物决定，若有障碍物则膨胀结束，若无障碍物则膨胀继续。当所有方向的膨胀结束时，对应空间膨胀结束。也就是说，如图2所示依次对s轴、l轴的正负方向进行膨胀包括如下：
67.首先膨胀s正方向，预设膨胀步数n_step，每一步向膨胀方向扩展单位长度(即分辨率长度)，判断膨胀后是否在语义地图范围内，若在范围内，则依次判断当前s对应的所有l，t是否有障碍物，若有障碍物，则当前s正向膨胀停止；若没有障碍物，则想膨胀方向加单位长度后再进行障碍物判断；
68.然后，在s正方向判断停止或到达指定步数后，依次用同样的方法判断s负方向以及l正方向和l负方向。
69.接着，进行完一个预设周期后，判断对应方向是否因障碍物膨胀停止，如果不是，则继续膨胀。
70.最后，判断是否所有方向均膨胀结束，若所有方向均结束，则对应空间膨胀结束；若没有结束，则继续进行膨胀循环。
71.实施例二
72.本发明还提供了一种基于时空联合规划的自动驾驶走廊优化构建系统，本领域技术人员可以通过执行所述基于时空联合规划的自动驾驶走廊优化构建方法的步骤流程实现所述基于时空联合规划的自动驾驶走廊优化构建系统，即可以将所述基于时空联合规划的自动驾驶走廊优化构建方法理解为所述基于时空联合规划的自动驾驶走廊优化构建系统的优选实施方式。
73.根据本发明提供的一种基于时空联合规划的自动驾驶走廊优化构建系统，包括：
74.模块m1：获取当前规划初始位置、指引点种子和环境表征信息。
75.模块m2：将所述指引点种子和环境表征信息投影到参考线坐标系，得到对应的frenet坐标，同时根据当前规划初始位置初始化语义地图。所述初始化语义地图包括以当前规划的初始位置为原点，分别根据s轴、l轴、t轴三个维度的分辨率及范围构建三维网格地图，并将三维网格地图中每个点的值初始化为0。其中，s表示相对于参考线的纵向距离，l表示相对于参考线的横向距离，t表示以当前时刻为起始时刻的相对时间。
76.模块m3：将当前环境表征信息和指引点种子分别由frenet坐标转换为对应的语义地图坐标。所述环境表征信息包括障碍物位姿随时间变化的动态语义信息、障碍物位姿在时间段内不变的静态语义信息。将当前环境表征信息由frenet坐标转换为对应的语义地图坐标包括：模块m3.1：针对动态语义信息依次获取不同障碍物、不同时刻的角点位置，针对静态语义信息将静态障碍物所占位置扩展到每个时间点上，得到对应时间点的静态障碍物位置坐标。模块m3.2：根据实际的角点frenet坐标，将位置信息依据初始化语义地图所设定的分辨率转换为对应的语义地图坐标。模块m3.3：判断所述语义地图坐标是否在设定范围内，若是，则根据转换后的不同时刻的障碍物角点坐标，将对应的障碍物所占位置进行填充；若否，则排除该位置点。
77.模块m4：选取初始两个种子构成初始走廊空间并检查所述走廊覆盖的语义地图中
是否包含障碍物，若是，则返回错误；若否，则触发模块m5。
78.模块m5：对所述初始走廊空间进行膨胀，进而得到可通行的语义走廊。具体地，模块m5包括：模块m5.1：选取初始两个种子构建第一个初始空间。模块m5.2：判断当前初始空间是否与障碍物存在碰撞，若否，则依次对s轴、l轴的正负方向进行膨胀；若是，则返回错误并退出。模块m5.3：判断当前位置点是否属于上一个空间，若是，则压入对应空间包含的种子中，并取上一个空间中最后一个点以及不属于所述空间的第一个点进行膨胀；若否，则选取上一个点与当前点，生成新的一个初始空间，触发模块m5.2。模块m5.4：将依次获得的空间压入对应的空间列表中，最终获得的可以通行的语义走廊。
79.其中，所述膨胀是根据预设的膨胀步数，按照预设方向进行扩展；所述膨胀根据当前方向是否有障碍物决定，若有障碍物则膨胀结束，若无障碍物则膨胀继续。当所有方向的膨胀结束时，对应空间膨胀结束。
80.本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
81.以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。

技术特征：
1.一种基于时空联合规划的自动驾驶走廊优化构建方法，其特征在于，包括：步骤s1：获取当前规划初始位置、指引点种子和环境表征信息；步骤s2：将所述指引点种子和环境表征信息投影到参考线坐标系，得到对应的frenet坐标，同时根据当前规划初始位置初始化语义地图；步骤s3：将当前环境表征信息和指引点种子分别由frenet坐标转换为对应的语义地图坐标；步骤s4：选取初始两个种子构成初始走廊空间并检查所述走廊覆盖的语义地图中是否包含障碍物，若是，则返回错误；若否，则执行步骤s5；步骤s5：对所述初始走廊空间进行膨胀，进而得到可通行的语义走廊。2.根据权利要求1所述的基于时空联合规划的自动驾驶走廊优化构建方法，其特征在于，所述初始化语义地图包括以当前规划的初始位置为原点，分别根据s轴、l轴、t轴三个维度的分辨率及范围构建三维网格地图，并将三维网格地图中每个点的值初始化为0；其中，s表示相对于参考线的纵向距离，l表示相对于参考线的横向距离，t表示以当前时刻为起始时刻的相对时间。3.根据权利要求1所述的基于时空联合规划的自动驾驶走廊优化构建方法，其特征在于，所述环境表征信息包括障碍物位姿随时间变化的动态语义信息、障碍物位姿在时间段内不变的静态语义信息；将当前环境表征信息由frenet坐标转换为对应的语义地图坐标包括：步骤s3.1：针对动态语义信息依次获取不同障碍物、不同时刻的角点位置，针对静态语义信息将静态障碍物所占位置扩展到每个时间点上，得到对应时间点的静态障碍物位置坐标；步骤s3.2：根据实际的角点frenet坐标，将位置信息依据初始化语义地图所设定的分辨率转换为对应的语义地图坐标；步骤s3.3：判断所述语义地图坐标是否在设定范围内，若是，则根据转换后的不同时刻的障碍物角点坐标，将对应的障碍物所占位置进行填充；若否，则排除该位置点。4.根据权利要求2所述的基于时空联合规划的自动驾驶走廊优化构建方法，其特征在于，步骤s5包括：步骤s5.1：选取初始两个种子构建第一个初始空间；步骤s5.2：判断当前初始空间是否与障碍物存在碰撞，若否，则依次对s轴、l轴的正负方向进行膨胀；若是，则返回错误并退出；步骤s5.3：判断当前位置点是否属于上一个空间，若是，则压入对应空间包含的种子中，并取上一个空间中最后一个点以及不属于所述空间的第一个点进行膨胀；若否，则选取上一个点与当前点，生成新的一个初始空间，执行步骤s5.2；步骤s5.4：将依次获得的空间压入对应的空间列表中，最终获得的可以通行的语义走廊。5.根据权利要求4所述的基于时空联合规划的自动驾驶走廊优化构建方法，其特征在于，所述膨胀是根据预设的膨胀步数，按照预设方向进行扩展；所述膨胀根据当前方向是否有障碍物决定，若有障碍物则膨胀结束，若无障碍物则膨胀继续；当所有方向的膨胀结束时，对应空间膨胀结束。
6.一种基于时空联合规划的自动驾驶走廊优化构建系统，其特征在于，包括：模块m1：获取当前规划初始位置、指引点种子和环境表征信息；模块m2：将所述指引点种子和环境表征信息投影到参考线坐标系，得到对应的frenet坐标，同时根据当前规划初始位置初始化语义地图；模块m3：将当前环境表征信息和指引点种子分别由frenet坐标转换为对应的语义地图坐标；模块m4：选取初始两个种子构成初始走廊空间并检查所述走廊覆盖的语义地图中是否包含障碍物，若是，则返回错误；若否，则触发模块m5；模块m5：对所述初始走廊空间进行膨胀，进而得到可通行的语义走廊。7.根据权利要求6所述的基于时空联合规划的自动驾驶走廊优化构建系统，其特征在于，所述初始化语义地图包括以当前规划的初始位置为原点，分别根据s轴、l轴、t轴三个维度的分辨率及范围构建三维网格地图，并将三维网格地图中每个点的值初始化为0；其中，s表示相对于参考线的纵向距离，l表示相对于参考线的横向距离，t表示以当前时刻为起始时刻的相对时间。8.根据权利要求6所述的基于时空联合规划的自动驾驶走廊优化构建系统，其特征在于，所述环境表征信息包括障碍物位姿随时间变化的动态语义信息、障碍物位姿在时间段内不变的静态语义信息；将当前环境表征信息由frenet坐标转换为对应的语义地图坐标包括：模块m3.1：针对动态语义信息依次获取不同障碍物、不同时刻的角点位置，针对静态语义信息将静态障碍物所占位置扩展到每个时间点上，得到对应时间点的静态障碍物位置坐标；模块m3.2：根据实际的角点frenet坐标，将位置信息依据初始化语义地图所设定的分辨率转换为对应的语义地图坐标；模块m3.3：判断所述语义地图坐标是否在设定范围内，若是，则根据转换后的不同时刻的障碍物角点坐标，将对应的障碍物所占位置进行填充；若否，则排除该位置点。9.根据权利要求7所述的基于时空联合规划的自动驾驶走廊优化构建系统，其特征在于，模块m5包括：模块m5.1：选取初始两个种子构建第一个初始空间；模块m5.2：判断当前初始空间是否与障碍物存在碰撞，若否，则依次对s轴、l轴的正负方向进行膨胀；若是，则返回错误并退出；模块m5.3：判断当前位置点是否属于上一个空间，若是，则压入对应空间包含的种子中，并取上一个空间中最后一个点以及不属于所述空间的第一个点进行膨胀；若否，则选取上一个点与当前点，生成新的一个初始空间，触发模块m5.2；模块m5.4：将依次获得的空间压入对应的空间列表中，最终获得的可以通行的语义走廊。10.根据权利要求9所述的基于时空联合规划的自动驾驶走廊优化构建系统，其特征在于，所述膨胀是根据预设的膨胀步数，按照预设方向进行扩展；所述膨胀根据当前方向是否有障碍物决定，若有障碍物则膨胀结束，若无障碍物则膨胀继续；当所有方向的膨胀结束时，对应空间膨胀结束。

技术总结
本发明提供了一种基于时空联合规划的自动驾驶走廊优化构建方法及系统，包括：获取当前规划初始位置、指引点种子和环境表征信息；将指引点种子和环境表征信息投影到参考线坐标系，得到对应的Frenet坐标，同时根据当前规划初始位置初始化语义地图；将当前环境表征信息和指引点种子分别由Frenet坐标转换为对应的语义地图坐标；选取初始两个种子构成初始走廊空间并检查走廊覆盖的语义地图中是否包含障碍物，若是，则返回错误；若否，则对初始走廊空间进行膨胀，进而得到可通行的语义走廊。本发明通过对时间和空间两个方面构建未来一定时间内可以通行的可行驶区域，提高了原始路径得到轨迹的合理性。得到轨迹的合理性。得到轨迹的合理性。


技术研发人员：廖文龙 袁子叶 何弢 张润玺
受保护的技术使用者：上海酷移机器人有限公司
技术研发日：2023.03.16
技术公布日：2023/6/28