一种无人驾驶汽车动态路径规划避障方法及系统与流程

1.本发明涉及无人驾驶车辆路径规划技术领域，尤其涉及一种无人驾驶汽车动态路径规划避障方法及系统。


背景技术：

2.伴随着时代的进步与发展，无人驾驶技术从根本上带来了社会的变革，无人驾驶可以从本质上改变人们的生活方式与出行方式，十分智能化。无人驾驶车辆是智能交通系统中的重要组成部分，由于无人驾驶车辆融合了环境感知、定位、决策、跟踪控制等众多领域，具有反应灵敏、行驶安全可靠等优点，可以有效降低交通事故发生率，提高道路的车辆行驶效率。目前，很多国内外高端科技公司致力于无人车技术的研发，如百度、谷歌、通用、特斯拉等都投入了巨大的精力。
3.轨迹规划是车辆实现无人驾驶的关键技术之一。轨迹规划可以在车路协同的基础上规划出有效的行驶路径，以此来实现无人车的超车与避障功能。一般而言，无人车的动态路径规划划分为两大类：一类是全局路径规划，是根据先验环境模型规划了全局范围的，探求的是整个地图信息中，从起点到终点中符合条件的最优或次优路径，主要采用一些计算进学科中最短路径算法；一类是局部路径规划，是指在未知或部分未知的环境下通过传感器获取周围环境信息，并使自动驾驶汽车自主获得一条无碰撞最优规划的路径，是在全局规划之后对路径进行一个符合车辆动力学、平稳学的轨迹优化，侧重于考虑车辆当前局部环境信息。
4.目前，局部路径规划方法主要有：人工势场法和启发式搜索算法。人工势场法是将车辆在周围环境中的运动设计成一种抽象的人造引力场中的运动，目标点对车辆产生“引力”，障碍物对车辆产生“斥力”，最后通过求合力来控制车辆的运动。该方法在数学描述上简洁、结构简单、计算量小，但是容易产生局部最优解。启发式搜索算法主要是a*算法和d*算法。a*算法是一种静态路网中求解最短路径最有效的直接搜索方法，需要建立环境模型地图，地图本身充当了人与车辆互相交流的媒介，这使得操作方便可靠，但是计算量大、耗时长；d*算法是对a*算法的扩充，适合动态环境下的路径规划，在动态环境中寻路非常有效，但是它比较复杂，应用范围有限。


技术实现要素：

5.为解决上述技术问题，本发明提供一种无人驾驶汽车动态路径规划避障方法及系统，以解决现有技术中局部路径规划容易陷入局部最优，计算量大，未考虑多车道的问题。
6.本发明提供一种无人驾驶汽车动态路径规划避障方法，该方法包括：
7.步骤s1：分别获取无人车和障碍物的初始信息，所述初始信息包括：地图信息，无人车位置、速度、车头方向和障碍物位置，以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线；
8.步骤s2：基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在
所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径；
9.步骤s3：构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
10.优选地，所述步骤s1中以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线，包括：
11.以无人车位置、车头方向和已知道路的曲率构建弧长s-横向偏移量q的坐标系，利用地图信息，在s-q坐标系内，基于三次样条曲线拟合形成道路基准线，公式为：
[0012][0013]
其中，(x
bf
,y
bf
)为样条曲线上的点在x-y直角坐标系中的坐标，a
x,i
、b
x,i
、c
x,i
、d
x,i
、a
y,i
、b
y,i
、c
y,i
、d
y,i
是样条曲线的参数，s为无人车当前位置映射在基准线上的弧长，si为弧长s第i个道路片段的起点。
[0014]
优选地，所述基准线上每个点的切向角和曲率公式为：
[0015][0016]
其中，θ
bf
为基准线上每个点的切向角，k
bf
为曲率，x
bf
′
、y
bf
′
、x
bf
″
、y
bf
″
分别为x
bf
(s)和y
bf
(s)对s的一阶导数和二阶导数。
[0017]
优选地，所述步骤s2包括：
[0018]
步骤s2.1：基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，获得样条曲线上最接近无人车位置的点坐标；
[0019]
步骤s2.2：基于最接近无人车位置的点坐标、所述横向偏移量q和所述弧长s,在s-q坐标系内，计算产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径，公式为：
[0020][0021]
其中，q
start
、q
end
、s
start
、s
end
分别代表候选路径起点的横向偏移量、终点的横向偏移量、起点对应的弧长和终点对应的弧长，不同的候选路径，对应不同的q
end
值，a，b，c为相应的系数；
[0022]
步骤s2.3：将所述候选路径上的点从s-q坐标系转换到x-y直角坐标系。
[0023]
优选地，所述步骤s3包括：
[0024]
步骤s3.1：构建代价函数，公式为：f(i)＝ω
saffsaf
(i)+ω
smofsmo
(i)+ω
cohfcoh(i)[0025]
其中，i为候选路径标号，f
saf
(i)、f
smo
(i)和f
coh
(i)分别是安全性代价函数、平滑性代价函数和连贯性代价函数，ω
saf
、ω
smo
和ω
coh
分别代表安全性代价函数、平滑性代价函数和连贯性代价函数所占的权重；
[0026]
步骤s3.2：基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
[0027]
优选地，其中，gi[k]为离散高斯函数，σ为碰撞风险标注差，r为候选路径与障碍物或道路边缘关系系数；
[0028]fsmo
(i)＝∫k
i2
(s)ds，其中，ki(s)为第i条候选路径上弧长为s位置的点的曲率，积分的上下限分别为第i条候选路径的弧长s的起点和终点；
[0029]
其中，s1和s2分别是当前候选路径与上次选择路径在基准线重叠部分的起点和终点，di是第i条候选路径上的点到上次选择路径上相同弧长的点的欧式距离。
[0030]
与现有技术相比，本发明提供的一种无人驾驶汽车动态路径规划避障方法具有如下有益效果：分别获取无人车和障碍物的初始信息，所述初始信息包括：地图信息，无人车位置、速度、车头方向和障碍物位置，以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线；基于无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径；构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。本发明能够在不进行迭代的前提下产生多条候选路径，并且构建代价函数，基于障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径，不管是多车道还是单车道，都能产生一条最优的路径，使规划车辆能够安全、舒适的绕过障碍物，从起点到达终点，完成路径的动态规划。而且计算量小，实时性好。
[0031]
本发明还提供一种无人驾驶汽车动态路径规划避障系统，该系统包括：
[0032]
道路基准线模块，用于分别获取无人车和障碍物的初始信息，所述初始信息包括：地图信息，无人车位置、速度、车头方向和障碍物位置，以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线；
[0033]
候选路径模块，用于基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为车辆绕过障碍物的候选路径；
[0034]
最佳路径模块，用于构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
[0035]
所述以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线，包括：
[0036]
以无人车位置、车头方向和已知道路的曲率构建弧长s-横向偏移量q的坐标系，利用地图信息，在s-q坐标系内，基于三次样条曲线拟合形成道路基准线，公式为：
[0037][0038]
其中，(x
bf
,y
bf
)为样条曲线上的点在x-y直角坐标系中的坐标，a
x,i
、b
x,i
、c
x,i
、d
x,i
、a
y,i
、b
y,i
、c
y,i
、d
y,i
是样条曲线的参数，s为无人车当前位置映射在基准线上的弧长，si为弧长s第i个道路片段的起点；
[0039]
所述基准线上每个点的切向角和曲率公式为：
[0040][0041]
其中，θ
bf
为基准线上每个点的切向角，k
bf
为曲率，x
bf
′
、y
bf
′
、x
bf
″
、y
bf
″
分别为x
bf
(s)和y
bf
(s)对s的一阶导数和二阶导数；
[0042]
所述基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为车辆绕过障碍物的候选路径，包括：
[0043]
基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，获得样条曲线上最接近无人车位置的点坐标；
[0044]
基于最接近无人车位置的点坐标、所述横向偏移量q和所述弧长s,在s-q坐标系内，计算产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径，公式为：
[0045][0046]
其中，q
start
、q
end
、s
start
、s
end
分别代表候选路径起点的横向偏移量、终点的横向偏移量、起点对应的弧长和终点对应的弧长，不同的候选路径，对应不同的q
end
值，a，b，c为相应的系数；
[0047]
将所述候选路径上的点从s-q坐标系转换到x-y直角坐标系；
[0048]
所述构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径，包括：
[0049]
构建代价函数，公式为：f(i)＝ω
saffsaf
(i)+ω
smofsmo
(i)+ω
cohfcoh
(i)，其中，gi[k]为离散高斯函数，σ为碰撞风险标注差，r为候选路径与障碍物或道路边缘关系系数；
[0050]fsmo
(i)＝∫k
i2
(s)ds，其中，ki(s)为第i条候选路径上弧长为s位置的点的曲率，积分的上下限分别为第i条候选路径的弧长s的起点和终点；
[0051]
其中，s1和s2分别是当前候选路径与上次选择路径在基准
线重叠部分的起点和终点，di是第i条候选路径上的点到上次选择路径上相同弧长的点的欧式距离；
[0052]
其中，i为候选路径标号，f
saf
(i)、f
smo
(i)和f
coh
(i)分别是安全性代价函数、平滑性代价函数和连贯性代价函数，ω
saf
、ω
smo
和ω
coh
分别代表安全性代价函数、平滑性代价函数和连贯性代价函数所占的权重；
[0053]
基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
[0054]
与现有技术相比，本发明提供的一种无人驾驶汽车动态路径规划避障系统的有益效果与上述技术方案所述一种无人驾驶汽车动态路径规划避障方法的有益效果相同，在此不做赘述。
[0055]
本发明还提供一种电子设备，包括总线、收发器、存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述收发器、所述存储器和所述处理器通过所述总线相连，所述计算机程序被所述处理器执行时实现上述任一项所述的一种无人驾驶汽车动态路径规划避障方法中的步骤。
[0056]
与现有技术相比，本发明提供的电子设备的有益效果与上述技术方案所述一种无人驾驶汽车动态路径规划避障方法的有益效果相同，在此不做赘述。
[0057]
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项所述的一种无人驾驶汽车动态路径规划避障方法中的步骤。
[0058]
与现有技术相比，本发明提供的计算机可读存储介质的有益效果与上述技术方案所述一种无人驾驶汽车动态路径规划避障方法的有益效果相同，在此不做赘述。
[0059]
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
[0060]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0061]
图1示出了本发明实施例所提供的一种无人驾驶汽车动态路径规划避障方法的流程图；
[0062]
图2示出了本发明实施例所提供的s-q坐标系下的道路基准线示意图。
[0063]
图3示出了本发明实施例所提供的多条候选路径示意图。
具体实施方式
[0064]
在本发明的描述中，需要理解的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
[0065]
本实施例中提到的“多个”是指两个或两个以上。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，a和/或b，可以表示：单独存在a，同时存在a和b，单独存在b的情况。“示例性的”或者“例如”等词用于表示作例子、例证或说明，旨在以具体方式呈现相关概念，不应被解释为比其他实施例或设计方案更优选或更具优势。
[0066]
图1示出了本发明实施例所提供的一种无人驾驶汽车动态路径规划避障方法的流程图，如图1所示，该方法包括：
[0067]
步骤s1：分别获取无人车和障碍物的初始信息，所述初始信息包括：地图信息，无人车位置、速度、车头方向和障碍物位置，以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线。
[0068]
需要说明的是，本发明实施例是在已知全局路线的情况进行的局部规划，全局路线通过车道级的高精度导航系统获取。应理解，本发明实施例中的全局路线是由一组道路边缘的有序点组成，并且考虑到计算曲率时曲线二阶导数的连续性，本发明实施例利用三次样条曲线的二阶连续性，结合已知的车道信息产生道路基准线，图2示出了本发明实施例所提供的道路基准线。
[0069]
弧长是最常用的曲线参数，因此，本发明实施例以无人车位置、车头方向和已知道路的曲率构建弧长s-横向偏移量q的坐标系，利用地图信息，在s-q坐标系内，基于三次样条曲线拟合形成道路基准线，采用正交数值法对由道路点拟合成的参数样条曲线弧长做参数化计算，公式为：
[0070][0071]
其中，(x
bf
,y
bf
)为样条曲线上的点在x-y直角坐标系中的坐标，a
x,i
、b
x,i
、c
x,i
、d
x,i
、a
y,i
、b
y,i
、c
y,i
、d
y,i
是样条曲线的参数，s为无人车当前位置映射在基准线上的弧长，si为弧长s第i个道路片段的起点。假设无人车当前位置到基准线上最近的点的距离，即横向偏移量为q，则车辆当前坐标可以用车辆行驶的弧长s和横向偏移量q来表示。本发明实施例中将s和q表示车辆位置的坐标称为s-q坐标。在s-q坐标坐标系中，基准线上每个点的切向角θ
bf
和曲率k
bf
可以用x
bf
(s)和y
bf
(s)对s的一阶导数和二阶导数求得，公式为：
[0072][0073]
其中，θ
bf
为基准线上每个点的切向角，k
bf
为曲率，x
bf
′
、y
bf
′
、x
bf
″
、y
bf
″
分别为x
bf
(s)和y
bf
(s)对s的一阶导数和二阶导数。
[0074]
步骤s2：基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径。
[0075]
需要说明的是，为了使用基准线上点的方向角和曲率，有必要找到车辆在基准线上的位置。
[0076]
所述步骤s2包括：
[0077]
步骤s2.1：基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，获得样条曲线上最接近无人车位置的点坐标。具体地，采用拉夫森二次极小化方法找到曲线上最接近无人车位置的点坐标。如图2所示的p0点,为无人车在基准线上距离最近的点。
[0078]
步骤s2.2：无人车在绕过障碍物时，是否与障碍物碰撞可以用偏移量q来表示，由于车辆行驶的距离是用弧长s表示，因此车辆绕过障碍物的候选路径可以用横向偏移量q和弧长s表示。基于最接近无人车位置的点坐标、所述横向偏移量q和所述弧长s,在s-q坐标系内，计算产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径，假设候选路径也满足三次样条曲线方程，则候选路径公式为：
[0079][0080]
其中，q
start
、q
end
、s
start
、s
end
分别代表候选路径起点的横向偏移量、终点的横向偏移量、起点对应的弧长和终点对应的弧长，不同的候选路径，对应不同的q
end
值，a，b，c为相应的系数；
[0081]
步骤s2.3：将所述候选路径上的点从s-q坐标系转换到x-y直角坐标系。
[0082]
需要说明的是，如图3所示，不同的候选路径，对应不同的q
end
值，因此，为了获取多条候选路径，本发明实施例设置n个不同q
end
值，分别计算得到n组a,b,c的值，以此构造出n个候选路径公式，由于一个候选路径公式表示一条候选路径，而候选路径公式是在s-q坐标系中计算得到的，而道路和障碍物信息一般都是基于x-y直角坐标系。因此本发明实施例需要将候选路径上的点从s-q坐标系转换到x-y直角坐标系。
[0083]
步骤s3：构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
[0084]
需要说明的是，在获取多条候选路径之后，需要从多条候选路径中选出一条最优路径，使无人车能够安全、平滑的绕过障碍物。本发明实施例提出一种代价函数法，通过代价函数的最小化来实现最优路径的选择。
[0085]
所述步骤s3包括：
[0086]
步骤s3.1：构建代价函数，考虑到驾驶时的安全性和舒服性，本发明实施例从安全、平滑和连续三个方面设计代价函数，公式为：
[0087]
f(i)＝ω
saffsaf
(i)+ω
smofsmo
(i)+ω
cohfcoh
(i)，
[0088][0089]fsmo
(i)＝∫k
i2
(s)ds，
[0090][0091]
其中，gi[k]为离散高斯函数，σ为碰撞风险标注差，决定碰撞检测的有效范围，本发明实施例中取σ＝2，为了避开障碍物和道路边缘，需要找到并舍弃与障碍物或车道边缘交叉的候选路径。本发明实施例将圆心到候选路径的最小距离与圆半径进行比较来确定该候选路径是否与障碍物碰撞。r为候选路径与障碍物或道路边缘关系系数，在单车道碰撞检测中，如果候选路径跨越障碍物或车道边缘，则r＝1,否则r＝0。在多车道的碰撞检测中，如
果某候选路径穿过障碍物或道路边缘，则r＝1，越过对向车道线，则r＝0.5，穿过同向车道线，则r＝0.2,在本车道上部穿越任何障碍物，则r＝0，ki(s)为第i条候选路径上弧长为s位置的点的曲率，积分的上下限分别为第i条候选路径的弧长s的起点和终点；s1和s2分别是当前候选路径与上次选择路径在基准线重叠部分的起点和终点，di是第i条候选路径上的点到上次选择路径上相同弧长的点的欧式距离，i为候选路径标号，f
saf
(i)、f
smo
(i)和f
coh
(i)分别是安全性代价函数、平滑性代价函数和连贯性代价函数，ω
saf
、ω
smo
和ω
coh
分别代表安全性代价函数、平滑性代价函数和连贯性代价函数所占的权重，这三个权重决定车辆的驾驶风格，依据经验分别取0.6、0.2和0.2。
[0092]
本发明实施例用离散的高斯卷积结合碰撞检测的方法来定义每条候选路径的碰撞风险。行驶过程中，不平滑的路径可能会引起车轮打滑，影响驾驶的安全性和舒适性，因此，平滑性也是路径选择中必须要考虑的一个因素。由于路径的平滑性与路径的曲率直接相关，所以本发明实施例利用曲率的平方在路径上的积分作为平滑性代价函数。安全性和平滑性只考虑了本次规划所涉及到的信息，但是没有考虑多次规划的连续性问题，无法保证本次规划的路径与上次规划的路径没有出现突变，如果本次规划路径与上次规划路径的距离太远，则会出现路径的突然转向，不仅影响驾驶的舒适度，严重情况下甚至会使车身出现较大的侧倾，存在安全隐患。为了解决这个问题，需要考虑上次选择的最优路径对本次路径选择的影响，因此，本发明实施例利用当前候选路径与上次选择路径之间的距离的积分来计算路径的连贯性代价函数。
[0093]
步骤s3.2：基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
[0094]
与现有技术相比，本发明实施例提供的一种无人驾驶汽车动态路径规划避障方法具有如下有益效果：本发明实施例首先利用地图信息产生道路基准线，然后依据障碍物的位置和车辆的横向偏移等信息，在s-q坐标系下产生一簇候选路径，在路径选择阶段，综合考虑安全性、平滑性和连续性，设计一种新的代价函数，利用代价函数的最小化来选择最优路径。不管是多车道还是单车道，都能产生一条最优的路径，使规划车辆能够安全、舒适的绕过障碍物，从起点到达终点，完成路径的动态规划。而且计算量小，实时性好。
[0095]
本发明实施例还提供一种无人驾驶汽车动态路径规划避障系统，该系统包括：
[0096]
道路基准线模块，用于分别获取无人车和障碍物的初始信息，所述初始信息包括：地图信息，无人车位置、速度、车头方向和障碍物位置，以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线；
[0097]
候选路径模块，用于基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为车辆绕过障碍物的候选路径；
[0098]
最佳路径模块，用于构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
[0099]
所述以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线，包括：
[0100]
以无人车位置、车头方向和已知道路的曲率构建弧长s-横向偏移量q的坐标系，利
用地图信息，在s-q坐标系内，基于三次样条曲线拟合形成道路基准线，公式为：
[0101][0102]
其中，(x
bf
,y
bf
)为样条曲线上的点在x-y直角坐标系中的坐标，a
x,i
、b
x,i
、c
x,i
、d
x,i
、a
y,i
、b
y,i
、c
y,i
、d
y,i
是样条曲线的参数，s为无人车当前位置映射在基准线上的弧长，si为弧长s第i个道路片段的起点；
[0103]
所述基准线上每个点的切向角和曲率公式为：
[0104][0105]
其中，θ
bf
为基准线上每个点的切向角，k
bf
为曲率，x
bf
′
、y
bf
′
、x
bf
″
、y
bf
″
分别为x
bf
(s)和y
bf
(s)对s的一阶导数和二阶导数；
[0106]
所述基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为车辆绕过障碍物的候选路径，包括：
[0107]
基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，获得样条曲线上最接近无人车位置的点坐标；
[0108]
基于最接近无人车位置的点坐标、所述横向偏移量q和所述弧长s,在s-q坐标系内，计算产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径，公式为：
[0109][0110]
其中，q
start
、q
end
、s
start
、s
end
分别代表候选路径起点的横向偏移量、终点的横向偏移量、起点对应的弧长和终点对应的弧长，不同的候选路径，对应不同的q
end
值，a，b，c为相应的系数；
[0111]
将所述候选路径上的点从s-q坐标系转换到x-y直角坐标系；
[0112]
所述构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径，包括：
[0113]
构建代价函数，公式为：f(i)＝ω
saffsaf
(i)+ω
smofsmo
(i)+ω
cohfcoh
(i)，
[0114]
其中，i为候选路径标号，f
saf
(i)、f
smo
(i)和f
coh
(i)分别是安全性代价函数、平滑性代价函数和连贯性代价函数，ω
saf
、ω
smo
和ω
coh
分别代表安全性代价函数、平滑性代价函数和连贯性代价函数所占的权重；
[0115]
其中，gi[k]为离散高斯函数，σ为碰撞风险标注差，r为候选路径与障碍物或道路边缘关系系数；
[0116]fsmo
(i)＝∫k
i2
(s)ds，其中，ki(s)为第i条候选路径上弧长为s位置的点的曲率，积
分的上下限分别为第i条候选路径的弧长s的起点和终点；
[0117]
其中，s1和s2分别是当前候选路径与上次选择路径在基准线重叠部分的起点和终点，di是第i条候选路径上的点到上次选择路径上相同弧长的点的欧式距离；
[0118]
基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。
[0119]
与现有技术相比，本发明实施例提供的一种无人驾驶汽车动态路径规划避障系统的有益效果与上述技术方案所述一种无人驾驶汽车动态路径规划避障方法的有益效果相同，在此不做赘述。
[0120]
此外，本发明实施例还提供了一种电子设备，包括总线、收发器、存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，该收发器、该存储器和处理器分别通过总线相连，计算机程序被处理器执行时实现上述一种无人驾驶汽车动态路径规划避障方法实施例的各个过程，且能达到相同的技术效果，为避免重复，这里不再赘述。
[0121]
此外，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种无人驾驶汽车动态路径规划避障方法实施例的各个过程，且能达到相同的技术效果，为避免重复，这里不再赘述。
[0122]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换的技术方案，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

技术特征：
1.一种无人驾驶汽车动态路径规划避障方法，其特征在于，包括：步骤s1：分别获取无人车和障碍物的初始信息，所述初始信息包括：地图信息，无人车位置、速度、车头方向和障碍物位置，以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线；步骤s2：基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径；步骤s3：构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。2.根据权利要求1所述的一种无人驾驶汽车动态路径规划避障方法，其特征在于，所述步骤s1中以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线，包括：以无人车位置、车头方向和已知道路的曲率构建弧长s-横向偏移量q的坐标系，利用地图信息，在s-q坐标系内，基于三次样条曲线拟合形成道路基准线，公式为：其中，(x
bf
,y
bf
)为样条曲线上的点在x-y直角坐标系中的坐标，a
x,i
、b
x,i
、c
x,i
、d
x,i
、a
y,i
、b
y,i
、c
y,i
、d
y,i
是样条曲线的参数，s为无人车当前位置映射在基准线上的弧长，s
i
为弧长s第i个道路片段的起点。3.根据权利要求2所述的一种无人驾驶汽车动态路径规划避障方法，其特征在于，所述基准线上每个点的切向角和曲率公式为：其中，θ
bf
为基准线上每个点的切向角，k
bf
为曲率，x
bf
′
、y
bf
′
、x
bf
″
、y
bf
″
分别为x
bf
(s)和y
bf
(s)对s的一阶导数和二阶导数。4.根据权利要求2所述的一种无人驾驶汽车动态路径规划避障方法，其特征在于，所述步骤s2包括：步骤s2.1：基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，获得样条曲线上最接近无人车位置的点坐标；步骤s2.2：基于最接近无人车位置的点坐标、所述横向偏移量q和所述弧长s,在s-q坐标系内，计算产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径，公式为：其中，q
star
、q
end
、s
start
、s
end
分别代表候选路径起点的横向偏移量、终点的横向偏移量、
起点对应的弧长和终点对应的弧长，不同的候选路径，对应不同的q
end
值，a，b，c为相应的系数；步骤s2.3：将所述候选路径上的点从s-q坐标系转换到x-y直角坐标系。5.根据权利要求1所述的一种无人驾驶汽车动态路径规划避障方法，其特征在于，所述步骤s3包括：步骤s3.1：构建代价函数，公式为：f(i)＝ω
saf
f
saf
(i)+ω
smo
f
smo
(i)+ω
coh
f
coh
(i)其中，i为候选路径标号，f
saf
(i)、f
smo
(i)和f
coh
(i)分别是安全性代价函数、平滑性代价函数和连贯性代价函数，ω
saf
、ω
smo
和ω
coh
分别代表安全性代价函数、平滑性代价函数和连贯性代价函数所占的权重；步骤s3.2：基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。6.根据权利要求5所述的一种无人驾驶汽车动态路径规划避障方法，其特征在于，其中，g
i
[k]为离散高斯函数，σ为碰撞风险标注差，r为候选路径与障碍物或道路边缘关系系数；f
smo
(i)＝∫k
i2
(s)ds，其中，k
i
(s)为第i条候选路径上弧长为s位置的点的曲率，积分的上下限分别为第i条候选路径的弧长s的起点和终点；其中，s1和s2分别是当前候选路径与上次选择路径在基准线重叠部分的起点和终点，d
i
是第i条候选路径上的点到上次选择路径上相同弧长的点的欧式距离。7.一种无人驾驶汽车动态路径规划避障系统，其特征在于，包括：道路基准线模块，用于分别获取无人车和障碍物的初始信息，所述初始信息包括：地图信息，无人车位置、速度、车头方向和障碍物位置，以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线；候选路径模块，用于基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为车辆绕过障碍物的候选路径；最佳路径模块，用于构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。8.根据权利要求7所述的一种无人驾驶汽车动态路径规划避障系统，其特征在于，所述以无人车位置、车头方向和已知道路的曲率构建坐标系，利用地图信息，在所述坐标系内，基于三次样条曲线拟合形成道路基准线，包括：以无人车位置、车头方向和已知道路的曲率构建弧长s-横向偏移量q的坐标系，利用地图信息，在s-q坐标系内，基于三次样条曲线拟合形成道路基准线，公式为：
其中，(x
bf
,y
bf
)为样条曲线上的点在x-y直角坐标系中的坐标，a
x,i
、b
x,i
、c
x,i
、d
x,i
、a
y,i
、b
y,i
、c
y,i
、d
y,i
是样条曲线的参数，s为无人车当前位置映射在基准线上的弧长，s
i
为弧长s第i个道路片段的起点；所述基准线上每个点的切向角和曲率公式为：其中，θ
bf
为基准线上每个点的切向角，k
bf
为曲率，x
bf
′
、y
bf
′
、x
bf
″
、y
bf
″
分别为x
bf
(s)和y
bf
(s)对s的一阶导数和二阶导数；所述基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，在所述坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为车辆绕过障碍物的候选路径，包括：基于所述无人车位置、速度、车头方向、障碍物位置和所述道路基准线，获得样条曲线上最接近无人车位置的点坐标；基于最接近无人车位置的点坐标、所述横向偏移量q和所述弧长s,在s-q坐标系内，计算产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径，公式为：其中，q
star
、q
end
、s
start
、s
end
分别代表候选路径起点的横向偏移量、终点的横向偏移量、起点对应的弧长和终点对应的弧长，不同的候选路径，对应不同的q
end
值，a，b，c为相应的系数；将所述候选路径上的点从s-q坐标系转换到x-y直角坐标系；所述构建代价函数，基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径，包括：构建代价函数，公式为：f(i)＝ω
saf
f
saf
(i)+ω
smo
f
smo
(i)+ω
coh
f
coh
(i)，其中，g
i
[k]为离散高斯函数，σ为碰撞风险标注差，r为候选路径与障碍物或道路边缘关系系数；f
smo
(i)＝∫k
i2
(s)ds，其中，k
i
(s)为第i条候选路径上弧长为s位置的点的曲率，积分的上下限分别为第i条候选路径的弧长s的起点和终点；其中，s1和s2分别是当前候选路径与上次选择路径在基准线重叠部分的起点和终点，d
i
是第i条候选路径上的点到上次选择路径上相同弧长的点的欧式距离；其中，i为候选路径标号，f
saf
(i)、f
smo
(i)和f
coh
(i)分别是安全性代价函数、平滑性代价
函数和连贯性代价函数，ω
saf
、ω
smo
和ω
coh
分别代表安全性代价函数、平滑性代价函数和连贯性代价函数所占的权重；基于所述障碍物位置，通过使代价函数最小化的方法从所述候选路径中获取最佳路径。9.一种电子设备，包括总线、收发器、存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述收发器、所述存储器和所述处理器通过所述总线相连，其特征在于，所述计算机程序被所述处理器执行时实现如权利要求1-6中任一项所述的一种无人驾驶汽车动态路径规划避障方法中的步骤。10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-6中任一项所述的一种无人驾驶汽车动态路径规划避障方法中的步骤。

技术总结
本发明提供一种无人驾驶汽车动态路径规划避障方法及系统，涉及无人驾驶车辆路径规划技术领域。对于单车道和多车道道路，都能够产生一条最优的路径，使规划车辆能够安全、舒适地绕过障碍物。该方法包括：分别获取无人车和障碍物的初始信息，利用地图信息，在坐标系内，基于三次样条曲线拟合形成道路基准线。基于无人车位置、速度、车头方向、障碍物位置和道路基准线，在坐标系内产生从无人车当前位置到目的位置的一簇平滑曲线，作为无人车绕过障碍物的候选路径，构建代价函数，基于障碍物位置，通过使代价函数最小化的方法从候选路径中获取最佳路径。所述无人驾驶汽车动态路径规划避障系统应用于无人驾驶汽车动态路径规划避障方法。统应用于无人驾驶汽车动态路径规划避障方法。统应用于无人驾驶汽车动态路径规划避障方法。


技术研发人员：牛引军
受保护的技术使用者：牛引军
技术研发日：2023.06.20
技术公布日：2023/8/5