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

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

Info

Publication number
CN115981308A
CN115981308A CN202211545950.3A CN202211545950A CN115981308A CN 115981308 A CN115981308 A CN 115981308A CN 202211545950 A CN202211545950 A CN 202211545950A CN 115981308 A CN115981308 A CN 115981308A
Authority
CN
China
Prior art keywords
track
vehicle
force field
time
longitudinal
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211545950.3A
Other languages
English (en)
Inventor
孙羽
徐贤
王红余
惠一
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chery New Energy Automobile Co Ltd
Original Assignee
Chery New Energy Automobile Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chery New Energy Automobile Co Ltd filed Critical Chery New Energy Automobile Co Ltd
Priority to CN202211545950.3A priority Critical patent/CN115981308A/zh
Publication of CN115981308A publication Critical patent/CN115981308A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

本发明涉及汽车自动驾驶路径规划技术领域，特别涉及一种基于势力场的路径规划方法及装置。本发明提出了一种融合自适应势力场模型和最优轨迹生成方法的智能路径规划方法。为了表示运动物体的风险函数，采用了固定车头时距策略设计环境车辆势力场。提出的智能路径规划方法各种道路自动驾驶情况，如车道保持、换道、避撞等。本发明提出的自适应势力场模型通过改变风险的大小克服了此类缺陷。综合考虑各部分性能最优设计代价函数，生成最优轨迹。

Description

一种基于势力场的路径规划方法及装置
技术领域
本发明涉及汽车自动驾驶路径规划技术领域，特别涉及一种基于势力场的路径规划方法及装置。
背景技术
随着道路自动驾驶技术发展，对智能车路径规划提出了更高要求。在现有相关技术中，进行路径规划时，对于障碍物环境，通常处理方式是抽象成数学模型，通过人工智能等算法模型来描述驾驶状态、驾驶行为与交通场景特征间的关系。但是这些常用的路径规划算法解决的多是静态障碍物或静态场景下的路径规划问题。
现有的技术中，CN110471421A公开了一种车辆安全行驶的路径规划方法及路径规划系统；CN109000651A公开了一种路径规划方法及路径规划装置。现有的势力场模型对障碍的动态方面考虑不够。
发明内容
针对上述问题，本发明提供一种基于势力场的路径规划方法及装置。
一种基于势力场的路径规划方法，所述方法包括：
依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；
根据最优轨迹控制车辆行进。
进一步的，所述在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹，包括：
建立道路自动驾驶势力场模型描述行车风险；
根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集；
设计代价函数选取最优轨迹，坐标转换后用于轨迹跟踪。
进一步的，所述建立道路自动驾驶势力场模型，包括环境车辆势力场；
在Frenet坐标系下，采用固定车头时距策略建立环境车辆势力场模型；
s方向(纵坐标)上危险程度变化主要由势力场形状系数决定σvehs，2σvehs表示Frenet坐标系下自车与环境车之间安全车间距，采用当前时刻固定车头时距
Figure BDA0003976255420000021
设定自车与环境车之间的参考距离，其中D0，τ为大于0的常数，因此，
Figure BDA0003976255420000022
第i辆环境车k时刻的势力场表示为：
Figure BDA0003976255420000023
k＝t+1,…,t+Np，其中t为当前时刻，k为向前的预测时刻，t与k之间间隔预测时域Np；
其中Pveh_i表示第i辆环境车的势力场；Aveh表示车辆势力场最大值；其中s，d表示Frenet坐标系下自车的横坐标、纵坐标；其中sveh_i，dveh_i表示Frenet坐标系下第i辆环境车的横坐标、纵坐标；其中
Figure BDA0003976255420000031
Figure BDA0003976255420000032
c表示决定第i辆环境车势力场形状的系数；Np表示预测时域。
进一步的，所述建立道路自动驾驶势力场模型，还包括道路边界势力场，表示如下：
Figure BDA0003976255420000033
m＝1,2
其中Prb_j表示第m个道路边界的势力场；Arb表示道路边界势力场最大值；yr,rb_m表示大地坐标系下到第m个道路边界的横向距离；其中σrb表示道路边界势力场系数。
进一步的，所述建立道路自动驾驶势力场模型，还包括车道中心线势力场，表示如下：
Figure BDA0003976255420000034
Pctr_n表示第n条车道中心线的势力场；Actr表示车道线势力场最大值；yr,ctr_n表示大地坐标系下到第n车道中心线的横向距离；σctr表示车道中心线势力场系数。
进一步的，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，包括计算轨迹纵向速度约束、轨迹曲率约束和轨迹纵向、横向加速度约束；
计算轨迹纵向速度约束具体包括：
考虑驾驶员的舒适性和安全性，s方向上的速度是有极限的，表示为：
Figure BDA0003976255420000041
其中
Figure BDA0003976255420000042
表示考虑乘坐舒适性后的横向加速度最大值；k为道路曲率；
Figure BDA0003976255420000043
为s向速度极限
当道路为直线时，速度极限很大，给定一个速度固定值
Figure BDA0003976255420000044
参考的s向速度定为
Figure BDA0003976255420000045
计算轨迹曲率约束具体包括：
当横向轨迹和纵向轨迹合并以后，需要对轨迹曲率约束kcand进行检查，主要是考虑车辆转向极限，kcand∈[kmin，kmax]，介于道路曲率最大值和最小值之间；
计算轨迹纵向、横向加速度约束具体包括：
当考虑到车辆动力学的物理限制时，需要对纵向加速度和横向加速度进行约束；
Figure BDA0003976255420000051
其中s(t)、d(t)为Frenet坐标系下t时刻车辆的纵坐标和横坐标；amax为车辆加速度最大值。
进一步的，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括生成候选路径，即轨迹集；生成的轨迹集做了横向和纵向分解，横向轨迹采用四阶多项式生成，纵向轨迹采用五阶多项式生成，最后得到的轨迹集要将横向轨迹和纵向轨迹进行合成得到；具体包括：
采用四阶多项式生成横向轨迹
s(t)＝α0+α1t+α2t2+α3t3+α4t4
为了求解5个系数，需要状态量
Figure BDA0003976255420000052
即初始时刻的纵向位移、纵向速度、纵向加速度，终了时刻的纵向速度、纵向加速度。
为了生成不同的轨迹，定义m时刻到n时刻的约束为
Figure BDA0003976255420000053
Tn表示时间间隔
当自车与前车在同一车道，如果max(Pveh_i(k|t))＞Pveh，thres
那么
Figure BDA0003976255420000061
否则，
Figure BDA0003976255420000062
Figure BDA0003976255420000063
为第i辆环境车的纵向速度，Pveh，thres为车辆势力场的阈值；
采用五阶多项式生成纵向轨迹
d(t)＝β0+β1t+β2t2+β3t3+β4t4+β5t5
为了求解六个系数，需要状态量
Figure BDA0003976255420000064
即初始时刻横向位移、横向速度、横向加速度，终了时刻横向位移、横向速度、横向加速度。
为了生成不同轨迹，定义m时刻到n时刻的约束为
Figure BDA0003976255420000065
随着不同的dm和时间间隔Tn变化，设
Figure BDA0003976255420000066
确保轨迹的最后部分是道路方向。
进一步的，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括最优轨迹的选取；
纵向和横向轨迹集相结合，并在候选轨迹中选择满足约束条件的最优轨迹；
定义代价函数：
Jtot＝wsJs+wdJd+wcJc+wpJp
其中，
Figure BDA0003976255420000071
其使得纵向运动最优，cj，s，cv，s，cT，s为每个指数的权重；
Figure BDA0003976255420000072
其使得横向运动最优，cj，d，cT，d为每个指数的权重；
Jc＝(df-df，opt)2
考虑连续重规划的一致性，df，opt是前面选取的最优轨迹；
Figure BDA0003976255420000073
利用上述方法完成了自车驶向中心线的轨迹结果，最外侧灰色的部分是受限制的无效轨迹，中间深色的部分是有效轨迹，最中间浅色的部分是通过代价函数选择的最优轨迹。
进一步的，所述根据最优轨迹控制车辆行进，具体包括：
将选取横向最优轨迹和纵向最优轨迹结合后，转换成笛卡尔坐标系下的轨迹用于跟踪控制；
轨迹跟踪控制选用合适的算法实现。
一种基于势力场的路径规划装置，包括：轨迹计算单元和车辆控制单元；
轨迹计算单元，用于依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；
车辆控制单元，用于根据最优轨迹控制车辆行进。
本发明提出了一种融合自适应势力场模型和最优轨迹生成方法的智能路径规划方法。为了表示运动物体的风险函数，采用了固定车头时距策略设计环境车辆势力场。提出的智能路径规划方法各种道路自动驾驶情况，如车道保持、换道、避撞等。
本发明提出的自适应势力场模型通过改变风险的大小克服了现有的势力场模型对障碍的动态方面考虑不够的缺陷。综合考虑各部分性能最优设计代价函数，生成最优轨迹。
本发明建立道路自动驾驶势力场模型描述行车风险；根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集；设计代价函数选取最优轨迹，坐标转换后用于轨迹跟踪。
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书以及附图中所指出的结构来实现和获得。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例方法流程图；
图2为本发明实施例单元示意图；
图3为本发明实施例Frenet坐标系下环境车辆及道路线形示意图；
图4为本发明实施例自车与环境车之间安全车间距示意图；
图5为本发明实施例轨迹结果示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
现有的势力场模型对障碍的动态方面考虑不够，本发明提出的自适应势力场模型通过改变风险的大小克服了此类缺陷。综合考虑各部分性能最优设计代价函数，生成最优轨迹。
第一方面，如图1所示，本发明提出了一种基于势力场的路径规划方法，所述方法包括：
依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；
根据最优轨迹控制车辆行进。
具体实施时，为了表示运动物体的风险函数，采用了固定车头时距策略设计环境车辆势力场。提出的智能路径规划方法各种道路自动驾驶情况，如车道保持、换道、避撞等。
本实施例中，所述在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹，包括：
建立道路自动驾驶势力场模型描述行车风险；
根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集；
设计代价函数选取最优轨迹，坐标转换后用于轨迹跟踪。
本实施例中，所述建立道路自动驾驶势力场模型，包括环境车辆势力场；
在Frenet坐标系下，采用固定车头时距策略建立环境车辆势力场模型；
s方向(纵坐标)上危险程度变化主要由势力场形状系数决定σvehs，2σvehs表示Frenet坐标系下自车与环境车之间安全车间距，采用当前时刻固定车头时距
Figure BDA0003976255420000101
设定自车与环境车之间的参考距离，其中D0，τ为大于0的常数，因此，
Figure BDA0003976255420000102
第i辆环境车k时刻的势力场表示为：
Figure BDA0003976255420000103
k＝t+1,…,t+Np；其中t为当前时刻，k为向前的预测时刻，t与k之间间隔预测时域Np；
其中Pveh_i表示第i辆环境车的势力场；Aveh表示车辆势力场最大值；其中s，d表示Frenet坐标系下自车的横坐标、纵坐标；其中sveh_i，dveh_i表示Frenet坐标系下第i辆环境车的横坐标、纵坐标；其中
Figure BDA0003976255420000111
Figure BDA0003976255420000112
c表示决定第i辆环境车势力场形状的系数；Np表示预测时域。
本实施例中，所述建立道路自动驾驶势力场模型，还包括道路边界势力场，表示如下：
Figure BDA0003976255420000113
m＝1,2
其中Prb_j表示第m个道路边界的势力场；Arb表示道路边界势力场最大值；yr,rb_m表示大地坐标系下到第m个道路边界的横向距离；其中σrb表示道路边界势力场系数。
本实施例中，所述建立道路自动驾驶势力场模型，还包括车道中心线势力场，表示如下：
Figure BDA0003976255420000114
Pctr_n表示第n条车道中心线的势力场；Actr表示车道线势力场最大值；yr,ctr_n表示大地坐标系下到第n车道中心线的横向距离；σctr表示车道中心线势力场系数。
本实施例中，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，包括计算轨迹纵向速度约束、轨迹曲率约束和轨迹纵向、横向加速度约束；
计算轨迹纵向速度约束具体包括：
考虑驾驶员的舒适性和安全性，s方向上的速度是有极限的，表示为：
Figure BDA0003976255420000121
其中
Figure BDA0003976255420000122
表示考虑乘坐舒适性后的横向加速度最大值；k为道路曲率；
Figure BDA0003976255420000123
为s向速度极限
当道路为直线时，速度极限很大，给定一个速度固定值
Figure BDA0003976255420000124
参考的s向速度定为
Figure BDA0003976255420000125
计算轨迹曲率约束具体包括：
当横向轨迹和纵向轨迹合并以后，需要对轨迹曲率约束kcand进行检查，主要是考虑车辆转向极限，kcand∈[kmin，kmax]，介于道路曲率最大值和最小值之间；
计算轨迹纵向、横向加速度约束具体包括：
当考虑到车辆动力学的物理限制时，需要对纵向加速度和横向加速度进行约束；
Figure BDA0003976255420000131
其中s(t)、d(t)为Frenet坐标系下t时刻车辆的纵坐标和横坐标；amax为车辆加速度最大值。
本实施例中，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括生成候选路径，即轨迹集；生成的轨迹集做了横向和纵向分解，横向轨迹采用四阶多项式生成，纵向轨迹采用五阶多项式生成，最后得到的轨迹集要将横向轨迹和纵向轨迹进行合成得到；具体包括：
采用四阶多项式生成横向轨迹
s(t)＝α0+α1t+α2t2+α3t3+α4t4
为了求解5个系数，需要状态量
Figure BDA0003976255420000132
即初始时刻的纵向位移、纵向速度、纵向加速度，终了时刻的纵向速度、纵向加速度；
为了生成不同的轨迹，定义m时刻到n时刻的约束为：
Figure BDA0003976255420000133
Tn表示时间间隔
当自车与前车在同一车道，如果max(Pveh_i(k|t))＞Pveh，thres
那么
Figure BDA0003976255420000141
否则，
Figure BDA0003976255420000142
Figure BDA0003976255420000146
为第i辆环境车的纵向速度，Pveh，thres为车辆势力场的阈值；
采用五阶多项式生成纵向轨迹
d(t)＝β0+β1t+β2t2+β3t3+β4t4+β5t5
为了求解六个系数，需要状态量
Figure BDA0003976255420000143
即初始时刻横向位移、横向速度、横向加速度，终了时刻横向位移、横向速度、横向加速度；
为了生成不同轨迹，定义m时刻到n时刻的约束为
Figure BDA0003976255420000144
随着不同的dm和时间间隔Tn变化，设
Figure BDA0003976255420000145
确保轨迹的最后部分是道路方向。
本实施例中，所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括最优轨迹的选取；
纵向和横向轨迹集相结合，并在候选轨迹中选择满足约束条件的最优轨迹；
定义代价函数：
Jtot＝wsJs+ωdJd+wcJc+wpJp
其中，
Figure BDA0003976255420000151
其使得纵向运动最优，cj，s，cv，s，cT，s为每个指数的权重；
Figure BDA0003976255420000152
其使得横向运动最优，cj，d，cT,d为每个指数的权重；
Jc＝(df-df，opt)2
考虑连续重规划的一致性，df，opt是前面选取的最优轨迹；
Figure BDA0003976255420000153
本实施例中，所述根据最优轨迹控制车辆行进，具体包括：
将选取横向最优轨迹和纵向最优轨迹结合后，转换成笛卡尔坐标系下的轨迹用于跟踪控制；
轨迹跟踪控制选用合适的算法实现。
第二方面，如图2所示，一种基于势力场的路径规划装置，包括：轨迹计算单元和车辆控制单元；
轨迹计算单元，用于依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；
车辆控制单元，用于根据最优轨迹控制车辆行进。
具体实施时，本发明一种基于势力场的路径规划装置和一种基于势力场的路径规划方法的实现过程一一对应，在此就不赘述。
为使本领域的技术人员能更好的理解本发明，结合附图对本发明的原理阐述如下：
如图1所示，本发明提出了一种融合自适应势力场模型和最优轨迹生成方法的智能路径规划方法。现有的势力场模型对障碍的动态方面考虑不够，提出的自适应势力场模型通过改变风险的大小克服了此类缺陷。为了表示运动物体的风险函数，采用了固定车头时距策略。提出的智能路径规划方法各种道路自动驾驶情况，如车道保持、换道、避撞等。
1.建立势力场模型，frenet坐标系下的道路几何线型如图3所示，
道路自动驾驶环境中势力场包括环境车辆势力场、道路边界势力场、车道中心线势力场
第i辆环境车k时刻的势力场：
在Frenet坐标系下，采用固定车头时距策略建立环境车辆势力场模型。
s方向(纵坐标)上危险程度变化主要由势力场形状系数决定σvehs，2σvehs表示Frenet坐标系下自车与环境车之间安全车间距，采用固定车头时距
Figure BDA0003976255420000161
设定自车与环境车之间的参考距离，其中D0，τ为大于0的常数，因此，
Figure BDA0003976255420000162
如图4所示，frenet坐标系下自车与环境车车间距示意，第i量环境车势力场表示为：
Figure BDA0003976255420000171
k＝t+1,…,t+Np
Pveh_i表示第i辆环境车的势力场；Aveh表示车辆势力场最大值；其中s，d表示Frenet坐标系下自车的横坐标、纵坐标；其中sveh_i，dveh_i表示Frenet坐标系下第i辆环境车的横坐标、纵坐标；其中
Figure BDA0003976255420000172
Figure BDA0003976255420000173
c表示决定第i辆环境车势力场形状的系数；Np表示预测时域；
(2)道路边界势力场：
Figure BDA0003976255420000174
m＝1,2
Prb_j表示第m个道路边界的势力场；Arb表示道路边界势力场最大值；yr,rb_m表示大地坐标系下到第m个道路边界的横向距离；σrb表示道路边界势力场系数；
车道中心线势力场：
Figure BDA0003976255420000175
Pctr_n表示第n条车道中心线的势力场；Actr表示车道线势力场最大值；yr,ctr_n表示大地坐标系下到第n车道中心线的横向距离；σctr表示车道中心线势力场系数；
2.生成最优轨迹
基于采样的多项式轨迹生成时一般是不考虑车辆的运动的，因此需要车辆运动学和动力学约束，同时还要考虑道路规则。由于Frenet坐标系下横向轨迹和纵向轨迹是分别产生的，因此需要在Frenet坐标系下检验很多约束。
2.1约束
(1)轨迹纵向速度约束
考虑驾驶员的舒适性和安全性，s方向上的速度是有极限的，表示为：
Figure BDA0003976255420000181
Figure BDA0003976255420000182
表示考虑乘坐舒适性后的横向加速度最大值；k为道路曲率；
Figure BDA0003976255420000183
为s向速度极限
当道路为直线时，速度极限很大，给定一个速度固定值
Figure BDA0003976255420000184
因此，参考的s向速度定为
Figure BDA0003976255420000185
(2)轨迹曲率约束
当横向轨迹和纵向轨迹合并以后，需要对轨迹曲率约束kcand进行检查，主要是考虑车辆转向极限，kcand∈[kmin，kmax]，介于道路曲率最大值和最小值之间。
(3)轨迹纵向、横向加速度约束
当考虑到车辆动力学的物理限制时，需要对纵向加速度和横向加速度进行约束。
Figure BDA0003976255420000191
amax为车辆加速度最大值。
2.2生成候选路径
采用四阶多项式生成横向轨迹如下：
s(t)＝α0+α1t+α2t2+α3t3+α4t4；
为了求解5个系数，需要状态量
Figure BDA0003976255420000192
即初始时刻的纵向位移、纵向速度、纵向加速度，终了时刻的纵向速度、纵向加速度。为了生成不同的轨迹，定义m时刻到n时刻的约束如下：
Figure BDA0003976255420000193
Tn表示时间间隔。
当自车与前车在同一车道，如果max(Pveh_i(k|t))＞Pveh，thres
那么
Figure BDA0003976255420000194
否则，
Figure BDA0003976255420000195
Figure BDA0003976255420000201
为第辆环境车的纵向速度，Pveh，thres为车辆势力场的阈值。
采用五阶多项式生成纵向轨迹如下：
d(t)＝β0+β1t+β2t2+β3t3+β4t4+β5t5
为了求解六个系数，需要状态量
Figure BDA0003976255420000202
即初始时刻横向位移、横向速度、横向加速度，终了时刻横向位移、横向速度、横向加速度。
为了生成不同轨迹，定义m时刻到n时刻的约束为
Figure BDA0003976255420000203
随着不同的dm和时间间隔Tn变化，设
Figure BDA0003976255420000204
确保轨迹的最后部分是道路方向。
2.3最优轨迹的选取
纵向和横向轨迹集相结合，并在候选轨迹中选择满足约束条件的最优轨迹。
定义代价函数：
Jtot＝wsJs+wdJd+wcJc+wpJp
Jtot这个公式由4项构成，考虑4个控制目标，其中第一项也就是第一个控制目标是关于Js的，为了确保纵向运动最优，第二项也就是第二个控制目标是关于Jd的，为了确保横向运动最优，第三项也就是第三个控制目标是关于Jc的，为了确保连续重规划的一致性，第四项也就是第四个控制目标是关于Jp的，是关于前面势力场合场强的，为了确保安全性的。
其中，
Figure BDA0003976255420000211
其使得纵向运动最优，cj，s，cv，s，cT，s为每个指数的权重；
Figure BDA0003976255420000212
其使得横向运动最优，cj，d，cT，d为每个指数的权重；
Jc＝(df-df，opt)2
考虑连续重规划的一致性，df，opt是前面选取的最优轨迹；
Figure BDA0003976255420000213
利用上述方法完成了自车驶向中心线的轨迹结果，朝向中心线的规划轨迹如图5所示，最外侧灰色的部分是受限制的无效轨迹，中间深色的部分是有效轨迹，最中间浅色的部分是通过代价函数选择的最优轨迹。
3.轨迹跟踪控制
将前一部分选取横向最优轨迹和纵向最优轨迹结合后，转换成笛卡尔坐标系下的轨迹用于跟踪控制。轨迹跟踪控制可以选用合适的算法如纯跟踪法、Stanley法等实现。
尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于势力场的路径规划方法，其特征在于，所述方法包括：
依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；
根据最优轨迹控制车辆行进。
2.根据权利要求1所述的一种基于势力场的路径规划方法，其特征在于，
所述在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹，包括：
建立道路自动驾驶势力场模型描述行车风险；
根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集；
设计代价函数选取最优轨迹，坐标转换后用于轨迹跟踪。
3.根据权利要求2所述的一种基于势力场的路径规划方法，其特征在于，
所述建立道路自动驾驶势力场模型，包括环境车辆势力场；
在Frenet坐标系下，采用固定车头时距策略建立环境车辆势力场模型；
s方向(纵坐标)上危险程度变化主要由势力场形状系数决定σvehs，2σvehs表示Frenet坐标系下自车与环境车之间安全车间距，采用当前时刻固定车头时距
Figure FDA0003976255410000021
设定自车与环境车之间的参考距离，其中D0，τ为大于0的常数，因此，
Figure FDA0003976255410000022
第i辆环境车k时刻的势力场表示为：
Figure FDA0003976255410000023
k＝t+1,…,t+Np；其中t为当前时刻，k为向前的预测时刻，t与k之间间隔预测时域Np；
其中Pveh_i表示第i辆环境车的势力场；Aveh表示车辆势力场最大值；其中s，d表示Frenet坐标系下自车的横坐标、纵坐标；其中sveh_i，dveh_i表示Frenet坐标系下第i辆环境车的横坐标、纵坐标；其中
Figure FDA0003976255410000025
Figure FDA0003976255410000026
c表示决定第i辆环境车势力场形状的系数；Np表示预测时域。
4.根据权利要求2所述的一种基于势力场的路径规划方法，其特征在于，
所述建立道路自动驾驶势力场模型，还包括道路边界势力场，表示如下：
Figure FDA0003976255410000024
其中Prb_j表示第m个道路边界的势力场；Arb表示道路边界势力场最大值；yr,rb_m表示大地坐标系下到第m个道路边界的横向距离；其中σrb表示道路边界势力场系数。
5.根据权利要求2所述的一种基于势力场的路径规划方法，其特征在于，
所述建立道路自动驾驶势力场模型，还包括车道中心线势力场，表示如下：
Figure FDA0003976255410000031
Pctr_n表示第n条车道中心线的势力场；Actr表示车道线势力场最大值；yr,ctr_n表示大地坐标系下到第n车道中心线的横向距离；σctr表示车道中心线势力场系数。
6.根据权利要求2所述的一种基于势力场的路径规划方法，其特征在于，
所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，包括计算轨迹纵向速度约束、轨迹曲率约束和轨迹纵向、横向加速度约束；
计算轨迹纵向速度约束具体包括：
考虑驾驶员的舒适性和安全性，s方向上的速度是有极限的，表示为：
Figure FDA0003976255410000041
其中
Figure FDA0003976255410000042
表示考虑乘坐舒适性后的横向加速度最大值；k为道路曲率；
Figure FDA0003976255410000043
为s向速度极限
当道路为直线时，速度极限很大，给定一个速度固定值
Figure FDA0003976255410000044
参考的s向速度定为
Figure FDA0003976255410000045
计算轨迹曲率约束具体包括：
当横向轨迹和纵向轨迹合并以后，需要对轨迹曲率约束kcand进行检查，主要是考虑车辆转向极限，kcand∈[kmin,kmax]，介于道路曲率最大值和最小值之间；
计算轨迹纵向、横向加速度约束具体包括：
当考虑到车辆动力学的物理限制时，需要对纵向加速度和横向加速度进行约束；
Figure FDA0003976255410000046
其中s(t)、d(t)为Frenet坐标系下t时刻车辆的纵坐标和横坐标；amax为车辆加速度最大值。
7.根据权利要求2所述的一种基于势力场的路径规划方法，其特征在于，
所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括生成候选路径，即轨迹集；生成的轨迹集做了横向和纵向分解，横向轨迹采用四阶多项式生成，纵向轨迹采用五阶多项式生成，最后得到的轨迹集要将横向轨迹和纵向轨迹进行合成得到；具体包括：
采用四阶多项式生成横向轨迹
s(t)＝α0+α1t+α2t2+α3t3+α4t4
为了求解5个系数，需要状态量
Figure FDA0003976255410000051
即初始时刻的纵向位移、纵向速度、纵向加速度，终了时刻的纵向速度、纵向加速度；
为了生成不同的轨迹，定义m时刻到n时刻的约束为：
Figure FDA0003976255410000052
Tn表示时间间隔
当自车与前车在同一车道，如果max(Pveh_i(k|t))>Pveh,thres
那么
Figure FDA0003976255410000053
否则，
Figure FDA0003976255410000054
Figure FDA0003976255410000055
为第i辆环境车的纵向速度，Pveh,thres为车辆势力场的阈值；
采用五阶多项式生成纵向轨迹
d(t)＝β0+β1t+β2t2+β3t3+β4t4+β5t5
为了求解六个系数，需要状态量
Figure FDA0003976255410000056
即初始时刻横向位移、横向速度、横向加速度，终了时刻横向位移、横向速度、横向加速度；
为了生成不同轨迹，定义m时刻到n时刻的约束为
Figure FDA0003976255410000061
随着不同的dm和时间间隔Tn变化，设
Figure FDA0003976255410000062
确保轨迹的最后部分是道路方向。
8.根据权利要求2所述的一种基于势力场的路径规划方法，其特征在于，
所述根据环境车辆信息、实时道路信息、实时自车信息在Frenet坐标系下生成轨迹集，还包括最优轨迹的选取；
纵向和横向轨迹集相结合，并在候选轨迹中选择满足约束条件的最优轨迹；
定义代价函数：
Jtot＝wsJs+wdJd+wcJc+wpJp
其中，
Figure FDA0003976255410000063
其使得纵向运动最优，cj，s，cv，s，cT，s为每个指数的权重；
Figure FDA0003976255410000064
其使得横向运动最优，cj，b，cT，d为每个指数的权重；
Jc＝(df-df，opt)2
考虑连续重规划的一致性，df，opt是前面选取的最优轨迹；
Figure FDA0003976255410000071
9.根据权利要求1所述的一种基于势力场的路径规划方法，其特征在于，
所述根据最优轨迹控制车辆行进，具体包括：
将选取横向最优轨迹和纵向最优轨迹结合后，转换成笛卡尔坐标系下的轨迹用于跟踪控制；
轨迹跟踪控制选用合适的算法实现。
10.一种基于势力场的路径规划装置，其特征在于，包括：轨迹计算单元和车辆控制单元；
轨迹计算单元，用于依据实时自车信息、实时环境车辆信息和实施道路信息，在Frenet坐标系下建立风险模型、生成轨迹集，并从轨迹集中筛选出最优轨迹；
车辆控制单元，用于根据最优轨迹控制车辆行进。
CN202211545950.3A 2022-12-01 2022-12-01 一种基于势力场的路径规划方法及装置 Pending CN115981308A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211545950.3A CN115981308A (zh) 2022-12-01 2022-12-01 一种基于势力场的路径规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211545950.3A CN115981308A (zh) 2022-12-01 2022-12-01 一种基于势力场的路径规划方法及装置

Publications (1)

Publication Number Publication Date
CN115981308A true CN115981308A (zh) 2023-04-18

Family

ID=85971355

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211545950.3A Pending CN115981308A (zh) 2022-12-01 2022-12-01 一种基于势力场的路径规划方法及装置

Country Status (1)

Country Link
CN (1) CN115981308A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117746639A (zh) * 2024-02-18 2024-03-22 江苏大学 基于自动驾驶的背景交通流模型构建方法及系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117746639A (zh) * 2024-02-18 2024-03-22 江苏大学 基于自动驾驶的背景交通流模型构建方法及系统
CN117746639B (zh) * 2024-02-18 2024-05-10 江苏大学 基于自动驾驶的背景交通流模型构建方法及系统

Similar Documents

Publication Publication Date Title
US11783614B2 (en) Pedestrian behavior predictions for autonomous vehicles
CN112389427B (zh) 车辆轨迹优化方法、装置、电子设备和存储介质
US11161502B2 (en) Cost-based path determination
US11970168B2 (en) Vehicle trajectory modification for following
CN110488802B (zh) 一种网联环境下的自动驾驶车辆动态行为决策方法
CN108256233B (zh) 基于驾驶员风格的智能车轨迹规划及跟踪方法和系统
US11256260B2 (en) Generating trajectories for autonomous vehicles
CN112896188B (zh) 一种考虑前车遭遇的自动驾驶决策控制的系统
CN113165652A (zh) 使用基于网格的方法检验预测轨迹
CN109804420B (zh) 车辆控制装置
US20230021615A1 (en) Vehicle control device, and vehicle control system
US11801864B1 (en) Cost-based action determination
US11433922B1 (en) Object uncertainty detection
US11780466B1 (en) Vehicle fleet remote ride comfort tuning management system
CN111679660A (zh) 一种融合类人驾驶行为的无人驾驶深度强化学习方法
US20220204056A1 (en) Permeable Speed Constraints
CN115981308A (zh) 一种基于势力场的路径规划方法及装置
WO2023048943A1 (en) Optimization based planning system
CN115841252A (zh) 一种基于预测轨迹的高速公路行车风险评估方法
CN115140048A (zh) 一种自动驾驶行为决策与轨迹规划模型与方法
US11970164B1 (en) Adverse prediction planning
US11591011B1 (en) Dynamic vehicle steering and/or suspension constraints
CN114407902B (zh) 一种基于道路水层深度估计的驾驶决策的系统
WO2024073532A1 (en) Xyz motion planning for vehicles

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination