一种基于势力场的路径规划方法及装置

技术领域

本发明涉及汽车自动驾驶路径规划技术领域，特别涉及一种基于势力场的路径规划方法及装置。

背景技术

随着道路自动驾驶技术发展，对智能车路径规划提出了更高要求。在现有相关技术中，进行路径规划时，对于障碍物环境，通常处理方式是抽象成数学模型，通过人工智能等算法模型来描述驾驶状态、驾驶行为与交通场景特征间的关系。但是这些常用的路径规划算法解决的多是静态障碍物或静态场景下的路径规划问题。

现有的技术中，CN110471421A公开了一种车辆安全行驶的路径规划方法及路径规划系统；CN109000651A公开了一种路径规划方法及路径规划装置。现有的势力场模型对障碍的动态方面考虑不够。

发明内容

针对上述问题，本发明提供一种基于势力场的路径规划方法及装置。

一种基于势力场的路径规划方法，所述方法包括：

依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；

根据最优轨迹控制车辆行进。

进一步的，所述在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹，包括：

建立道路自动驾驶势力场模型描述行车风险；

根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集；

设计代价函数选取最优轨迹，坐标转换后用于轨迹跟踪。

进一步的，所述建立道路自动驾驶势力场模型，包括环境车辆势力场；

在Frenet坐标系下，采用固定车头时距策略建立环境车辆势力场模型；

s方向(纵坐标)上危险程度变化主要由势力场形状系数决定σ

第i辆环境车k时刻的势力场表示为：

k＝t+1,…,t+N

其中Pveh_i表示第i辆环境车的势力场；A

进一步的，所述建立道路自动驾驶势力场模型，还包括道路边界势力场，表示如下：

m＝1,2

其中P

进一步的，所述建立道路自动驾驶势力场模型，还包括车道中心线势力场，表示如下：

P

进一步的，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，包括计算轨迹纵向速度约束、轨迹曲率约束和轨迹纵向、横向加速度约束；

计算轨迹纵向速度约束具体包括：

考虑驾驶员的舒适性和安全性，s方向上的速度是有极限的，表示为：

其中

当道路为直线时，速度极限很大，给定一个速度固定值

计算轨迹曲率约束具体包括：

当横向轨迹和纵向轨迹合并以后，需要对轨迹曲率约束k

计算轨迹纵向、横向加速度约束具体包括：

当考虑到车辆动力学的物理限制时，需要对纵向加速度和横向加速度进行约束；

其中s(t)、d(t)为Frenet坐标系下t时刻车辆的纵坐标和横坐标；a

进一步的，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括生成候选路径，即轨迹集；生成的轨迹集做了横向和纵向分解，横向轨迹采用四阶多项式生成，纵向轨迹采用五阶多项式生成，最后得到的轨迹集要将横向轨迹和纵向轨迹进行合成得到；具体包括：

采用四阶多项式生成横向轨迹

s(t)＝α

为了求解5个系数，需要状态量

为了生成不同的轨迹，定义m时刻到n时刻的约束为

T

当自车与前车在同一车道，如果max(P

那么

为第i辆环境车的纵向速度，P

采用五阶多项式生成纵向轨迹

d(t)＝β

为了求解六个系数，需要状态量

为了生成不同轨迹，定义m时刻到n时刻的约束为

随着不同的d

进一步的，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括最优轨迹的选取；

纵向和横向轨迹集相结合，并在候选轨迹中选择满足约束条件的最优轨迹；

定义代价函数：

J

其中，

其使得纵向运动最优，c

其使得横向运动最优，c

J

考虑连续重规划的一致性，d

利用上述方法完成了自车驶向中心线的轨迹结果，最外侧灰色的部分是受限制的无效轨迹，中间深色的部分是有效轨迹，最中间浅色的部分是通过代价函数选择的最优轨迹。

进一步的，所述根据最优轨迹控制车辆行进，具体包括：

将选取横向最优轨迹和纵向最优轨迹结合后，转换成笛卡尔坐标系下的轨迹用于跟踪控制；

轨迹跟踪控制选用合适的算法实现。

一种基于势力场的路径规划装置，包括：轨迹计算单元和车辆控制单元；

轨迹计算单元，用于依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；

车辆控制单元，用于根据最优轨迹控制车辆行进。

本发明提出了一种融合自适应势力场模型和最优轨迹生成方法的智能路径规划方法。为了表示运动物体的风险函数，采用了固定车头时距策略设计环境车辆势力场。提出的智能路径规划方法各种道路自动驾驶情况，如车道保持、换道、避撞等。

本发明提出的自适应势力场模型通过改变风险的大小克服了现有的势力场模型对障碍的动态方面考虑不够的缺陷。综合考虑各部分性能最优设计代价函数，生成最优轨迹。

本发明建立道路自动驾驶势力场模型描述行车风险；根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集；设计代价函数选取最优轨迹，坐标转换后用于轨迹跟踪。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书以及附图中所指出的结构来实现和获得。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例方法流程图；

图2为本发明实施例单元示意图；

图3为本发明实施例Frenet坐标系下环境车辆及道路线形示意图；

图4为本发明实施例自车与环境车之间安全车间距示意图；

图5为本发明实施例轨迹结果示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

现有的势力场模型对障碍的动态方面考虑不够，本发明提出的自适应势力场模型通过改变风险的大小克服了此类缺陷。综合考虑各部分性能最优设计代价函数，生成最优轨迹。

第一方面，如图1所示，本发明提出了一种基于势力场的路径规划方法，所述方法包括：

依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；

根据最优轨迹控制车辆行进。

具体实施时，为了表示运动物体的风险函数，采用了固定车头时距策略设计环境车辆势力场。提出的智能路径规划方法各种道路自动驾驶情况，如车道保持、换道、避撞等。

本实施例中，所述在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹，包括：

建立道路自动驾驶势力场模型描述行车风险；

根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集；

设计代价函数选取最优轨迹，坐标转换后用于轨迹跟踪。

本实施例中，所述建立道路自动驾驶势力场模型，包括环境车辆势力场；

在Frenet坐标系下，采用固定车头时距策略建立环境车辆势力场模型；

s方向(纵坐标)上危险程度变化主要由势力场形状系数决定σ

第i辆环境车k时刻的势力场表示为：

k＝t+1,…,t+N

其中P

本实施例中，所述建立道路自动驾驶势力场模型，还包括道路边界势力场，表示如下：

m＝1,2

其中P

本实施例中，所述建立道路自动驾驶势力场模型，还包括车道中心线势力场，表示如下：

P

本实施例中，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，包括计算轨迹纵向速度约束、轨迹曲率约束和轨迹纵向、横向加速度约束；

计算轨迹纵向速度约束具体包括：

考虑驾驶员的舒适性和安全性，s方向上的速度是有极限的，表示为：

其中

当道路为直线时，速度极限很大，给定一个速度固定值

计算轨迹曲率约束具体包括：

当横向轨迹和纵向轨迹合并以后，需要对轨迹曲率约束k

计算轨迹纵向、横向加速度约束具体包括：

当考虑到车辆动力学的物理限制时，需要对纵向加速度和横向加速度进行约束；

其中s(t)、d(t)为Frenet坐标系下t时刻车辆的纵坐标和横坐标；a

本实施例中，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括生成候选路径，即轨迹集；生成的轨迹集做了横向和纵向分解，横向轨迹采用四阶多项式生成，纵向轨迹采用五阶多项式生成，最后得到的轨迹集要将横向轨迹和纵向轨迹进行合成得到；具体包括：

采用四阶多项式生成横向轨迹

s(t)＝α

为了求解5个系数，需要状态量

为了生成不同的轨迹，定义m时刻到n时刻的约束为：

T

当自车与前车在同一车道，如果max(P

那么

为第i辆环境车的纵向速度，P

采用五阶多项式生成纵向轨迹

d(t)＝β

为了求解六个系数，需要状态量

为了生成不同轨迹，定义m时刻到n时刻的约束为

随着不同的d

本实施例中，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括最优轨迹的选取；

纵向和横向轨迹集相结合，并在候选轨迹中选择满足约束条件的最优轨迹；

定义代价函数：

J

其中，

其使得纵向运动最优，c

其使得横向运动最优，c

J

考虑连续重规划的一致性，d

本实施例中，所述根据最优轨迹控制车辆行进，具体包括：

将选取横向最优轨迹和纵向最优轨迹结合后，转换成笛卡尔坐标系下的轨迹用于跟踪控制；

轨迹跟踪控制选用合适的算法实现。

第二方面，如图2所示，一种基于势力场的路径规划装置，包括：轨迹计算单元和车辆控制单元；

轨迹计算单元，用于依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；

车辆控制单元，用于根据最优轨迹控制车辆行进。

具体实施时，本发明一种基于势力场的路径规划装置和一种基于势力场的路径规划方法的实现过程一一对应，在此就不赘述。

为使本领域的技术人员能更好的理解本发明，结合附图对本发明的原理阐述如下：

如图1所示，本发明提出了一种融合自适应势力场模型和最优轨迹生成方法的智能路径规划方法。现有的势力场模型对障碍的动态方面考虑不够，提出的自适应势力场模型通过改变风险的大小克服了此类缺陷。为了表示运动物体的风险函数，采用了固定车头时距策略。提出的智能路径规划方法各种道路自动驾驶情况，如车道保持、换道、避撞等。

1.建立势力场模型，frenet坐标系下的道路几何线型如图3所示，

道路自动驾驶环境中势力场包括环境车辆势力场、道路边界势力场、车道中心线势力场

第i辆环境车k时刻的势力场：

在Frenet坐标系下，采用固定车头时距策略建立环境车辆势力场模型。

s方向(纵坐标)上危险程度变化主要由势力场形状系数决定σvehs，2σ

如图4所示，frenet坐标系下自车与环境车车间距示意，第i量环境车势力场表示为：

k＝t+1,…,t+Np

Pveh_i表示第i辆环境车的势力场；A

(2)道路边界势力场：

m＝1,2

P

车道中心线势力场：

Pctr_n表示第n条车道中心线的势力场；Actr表示车道线势力场最大值；yr,ctr_n表示大地坐标系下到第n车道中心线的横向距离；σctr表示车道中心线势力场系数；

2.生成最优轨迹

基于采样的多项式轨迹生成时一般是不考虑车辆的运动的，因此需要车辆运动学和动力学约束，同时还要考虑道路规则。由于Frenet坐标系下横向轨迹和纵向轨迹是分别产生的，因此需要在Frenet坐标系下检验很多约束。

2.1约束

(1)轨迹纵向速度约束

考虑驾驶员的舒适性和安全性，s方向上的速度是有极限的，表示为：

表示考虑乘坐舒适性后的横向加速度最大值；k为道路曲率；/>

当道路为直线时，速度极限很大，给定一个速度固定值

因此，参考的s向速度定为

(2)轨迹曲率约束

当横向轨迹和纵向轨迹合并以后，需要对轨迹曲率约束k

(3)轨迹纵向、横向加速度约束

当考虑到车辆动力学的物理限制时，需要对纵向加速度和横向加速度进行约束。

a

2.2生成候选路径

采用四阶多项式生成横向轨迹如下：

s(t)＝α

为了求解5个系数，需要状态量

T

当自车与前车在同一车道，如果max(P

那么

为第辆环境车的纵向速度，P

采用五阶多项式生成纵向轨迹如下：

d(t)＝β

为了求解六个系数，需要状态量

为了生成不同轨迹，定义m时刻到n时刻的约束为

随着不同的d

2.3最优轨迹的选取

纵向和横向轨迹集相结合，并在候选轨迹中选择满足约束条件的最优轨迹。

定义代价函数：

J

J

其中，

其使得纵向运动最优，c

其使得横向运动最优，c

J

考虑连续重规划的一致性，d

利用上述方法完成了自车驶向中心线的轨迹结果，朝向中心线的规划轨迹如图5所示，最外侧灰色的部分是受限制的无效轨迹，中间深色的部分是有效轨迹，最中间浅色的部分是通过代价函数选择的最优轨迹。

3.轨迹跟踪控制

将前一部分选取横向最优轨迹和纵向最优轨迹结合后，转换成笛卡尔坐标系下的轨迹用于跟踪控制。轨迹跟踪控制可以选用合适的算法如纯跟踪法、Stanley法等实现。

尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。