一种基于改进场论的无人车轨迹规划方法

技术领域

本发明公开了一种基于改进场论的无人车轨迹规划方法，属于无人车驾驶技术领域；还涉及一种考虑障碍物边界的无人车轨迹规划方法。

背景技术

运动规划是无人车的核心技术部分，一般分为全局路径规划与局部路径规划，局部路径规划是自动驾驶汽车行驶主动安全的重要手段，常用的局部路径规划算法有强化学习、蚁群算法、动态窗口法、快速探索的随机树(RRT*)算法及人工势场法等。人工势场算法原理简单，计算速度快，容易实现，在局部路径规划方面应用广泛，各界学者以此进行了深入的改进与推广。

基于场论的规划算法可以较好的展现出人-车-路之间的影响机理，其算法简单，计算量小，在实时避障时运行效率较高，是应用范围较广的算法之一。然而场论在实际运用中除了局部最优解与目标不可达这两个自身的算法缺陷外，其在复杂环境下规划出的轨迹不平滑，在实际应用场景中，无人车受到的势场力复杂且方向变化频率频繁，这也意味着规划出来的无人车轨迹是很不稳定的，而无人车的运行中要求车身尽可能地平稳，频繁的转换方向不仅会影响到行驶的平顺性，还会影响运送质量，所以基于传统的场论算法无法满足无人车的轨迹规划需求。

发明内容

本发明的目的是提供一种基于改进场论的无人车轨迹规划方法，用以解决上述背景技术中提出的势场法难以应用实际的问题。

为实现上述目的，本发明的方案包括：

本发明的一种基于改进场论的无人车轨迹规划方法的技术方案，包括如下步骤：

S1确定无人车位置与目标点位置，计算目标点对无人车的引力；

S2根据感知到的环境信息，建立对应的车道势场、障碍物静态势场；

S3建立反应运动障碍物对无人车行驶影响程度的动能场；

S4设置安全距离，如果无人车与当前障碍物之间的欧氏距离小于所述安全距离，则进行步骤S5；如果大于所述安全距离则跳至步骤S6；

S5根据所述动能场计算动态障碍物对自车的总场力；

S6根据所述动能场计算动态障碍物对自车总场力中无人车前进方向上的分力；

S7计算无人车受到的合力，并根据合力进行下一步规划。

步骤S2中，利用自车激光雷达、相机等感知障碍物、道路边界线、交通道路线及其他交通参与者的状态信息。

步骤S4中的安全距离，车辆与障碍物之间在小于安全距离时才会趋向于排斥行车风险较大的障碍物的接近。

步骤S5中，如果欧氏距离小于设定的安全距离，则无人车开始排斥障碍物的接近，此时受到全部的场力。

步骤S6中，若欧氏距离大于设定的安全距离，则说明无人车尚未排斥障碍物的接近，只需受到场力的纵向力。

进一步地，步骤S7中，根据合力进行规划时，还判断当前是否陷入局部最优，如果陷入局部最优，则运用触须算法快速逃逸。

对于所述的局部最小值点：若无人车行驶的过程中，其所受的引力与斥力达到平衡时，无人车受力变为0，然而此时无人车并未到达目标点，规划任务还在继续，但无人车受力为0已经不在前进，导致规划任务失败。本发明进一步对局部最小值进行判定识别并运用触须算法进行逃逸。

另外通过检测无人车与目标点距离如果小于某个预设较小值，则认为无人车到达目的地，规划程序即可退出。

进一步地，用于计算目标点对无人车引力的引力模型如下：

式中，U

进一步地，目标点对无人车的引力通过对引力函数求负梯度得到。

进一步地，步骤S2中，车道势场包括道路边界势场和中心线势场，道路边界势场模型如下：

式中，y为无人车在坐标系中的纵向坐标值、l

中心线势场模型模型如下：

式中，k

进一步地，步骤S2中，障碍物静态势场模型如下：

E

式中，G为调节系数、R

进一步地，步骤S3中，所述动能场的模型为：

E

式中，E

进一步地，考虑障碍物尺寸的等效距离L

其中，k

进一步地，物体i的等效质量M

M

其中，m

进一步地，步骤S4中，所述安全距离L通过下式计算：

式中，t为无人车制动系统延迟时间、v为无人车速度、v

进一步地，步骤S5中动态障碍物对自车的总场力F通过下式计算：

F＝F

式中，F

进一步地，步骤S6中动态障碍物对自车总场力中无人车前进方向上的分力F通过下式计算：

F＝cos(θ

式中，F

进一步地，比较无人车最近若干次规划的位置坐标，如果位置坐标变化都在一个预设的极小范围内，则判定当前陷入局部最优。

自车陷入局部最小值时，自车位置变化极小，如果检测自车在近5次迭代中没有发生位置坐标变化或者变化极小，则判定自车已处于局部最优位置。

进一步地，所述触须算法生成若干条触须路径，根据加权后的障碍物与触须路径之间的距离及无人车在触须路径上行驶时侧面距离选择最优触须路径。

触须算法生成81条触须路径，无人车在不同的运动状态下对应生成的触须路径的形状也不同，通常将无人车从0至最高车速分为15个速度区间，不同速度区间内根据不同的方向盘转角设定81条触须路径，即自车在一个速度区间里下有81个可行轨迹，具体触须路径的计算为：

l＝8+33.5q

q＝j/15

式中，i＝0,1,…,80是车辆在某一速度值时所生成的触须路径(规划轨迹)编号、r

生成触须路径之后需要对生成的路径轨迹进行选择，在本发明中，分别计算距离参数、侧面距离参数并进行加权计算，根据最终值选择触须路径。

本发明改进了场论的建模方法，首先考虑障碍物尺寸，使得模型更加精确，场强分布更加合理；同时针对场论的轨迹过于受环境因素影响而抖动的问题，通过设定一个安全范围用于继续维持自身运动方向的方法，进而解决上述问题，使得轨迹更加平滑；在场论的局部最优解的问题上，引用运算速度快的触须算法来引导自车逃离局部最优点，最终完成规划。

附图说明

图1是本发明基于改进场论的无人车轨迹规划方法流程框图；

图2是本发明所提供方法中的目标点引力势场示意图；

图3是本发明所提供方法中的道路边界与道路中心线组成的势场示意图；

图4是本发明所提供方法中的改进后的动能场势场示意图；

图5是本发明所提供方法中步骤S5中自车的受力示意图；

图6是本发明所提供方法中步骤S6中自车的受力示意图；

图7是本发明所提供方法中触须算法生成的触须路径示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明了，下面结合附图及实施例，对本发明作进一步的详细说明。

无人车轨迹规划方法实施例：

本发明的基于改进场论的无人车轨迹规划方法如图1所示，包括如下步骤。

步骤S1：自车开机后将建立坐标系确定自身位置，使用者下发目标点位置信息，无人车确定目标点位置后即可计算目标点引力场场强U

式中，U

场论中，无人车在交通环境车辆距离较远时不受斥力影响，但会时刻受到引力的影响，通过引力函数对应的负梯度方向进行运动，可以完成无人车初步的运动规划。

根据引力函数计算其对应的负梯度，得到目标点引力F

引力势场由目标点发出，实时指向目标点方向用来引导无人车靠近目标点，引力势场效果如图2所示，图中x、y轴代表道路平面位置，z轴表示所示场的强度，z轴数值越大说明势场的强度越大，图中颜色越深表示z轴数据越小场强越小，颜色越浅表示z轴数值越大场强越大，无人车趋向于向场强小的方向规划路径，图3、图4同理。

同时进行下一步步骤S2，无人车启动激光雷达扫描获知环境信息，进行坐标转换，确定道路边界及车道线位置坐标。

根据传感器获得的信息建立模型，车道中心线必要时可以跨越，在设计势场模型时，对应的场力不会太高；道路边界线对行车安全有很大的影响，距离越近，风险越大，本质上不可跨越。

根据以上性质，所建立的边界势场函数模型、中心线势场模型以及障碍物静态势场如下所示：

式中，y表示自车在坐标系中的纵向坐标值，l

式中，k

道路势场效果如图3所示，图3中两侧场强趋向无穷大，表示道路边界，中间的场强峰值表示道路中心线对无人车寻路时的影响。

障碍物静态势场模型为：

E

M

式中，G、k

进行步骤S3：动能场属于一种“物理场”，主要表示道路上运动物体对于自车行驶风险影响的大小程度，表达式为：

E

式中，E

计算出场强之后继续下一步步骤S4：从微观角度分析，模拟车辆与障碍物之间在相互运动时会有“场力”相互作用，如车辆在与行车风险较大的障碍物距离较远时受到一个潜在风险影响，自车会降低自身车速以减少潜在的行车风险；距离小于一个距离阈值时会趋向于排斥行车风险较大的障碍物的接近，此时会采取一些安全措施(如转向)来保持自身的行车安全，如果假定车辆在小于一个安全距离L时开始改变运动方向，那么可以对安全距离L的进行一个简单的计算：

式中，t表示制动系统延迟时间，v为无人车速度，v

计算自车与障碍物之间的欧式距离L

如果自车行驶时与障碍物之间的距离小于L，则自车进行步骤S5，如果大于L则进行步骤S6。

步骤S5，计算此时动态障碍物对自车的总场力，自车受力示意图如图5所示：

F＝F

式中，F表示自车受到的总场力，F

步骤S6：自车仅减速不改变方向，此时自车不受转向力，计算此时动态障碍物对自车的总场力，自车受力示意图如图6所示：

F＝cos(θ

式中，F表示自车受到的总场力，F

步骤S7：根据所受的场力大小及方向将计算的所有场力相加得到最终的合力，根据最终的合力方向及大小进行下一步轨迹规划。

进行下一步规划完成后，开始步骤S8：检测自车是否陷入局部最小值点。

控制器设置变量储存自车近5次的位置坐标，如果检测自车在近5次迭代中没有发生位置坐标变化或者变化极小，则判定自车已处于局部最优位置。

确定自车陷入局部最优解后，运用触须算法逃离局部最优，触须算法生成81条触须，如图7所示，即自车在一个车速区间里生成有81个可行轨迹，具体方法为：

l＝8+33.5q

q＝j/15

式中，i＝0,1,…,80是车辆在某一速度值时所生成的触须(规划轨迹)编号，r

在本算法中，选择两个评价参数，分别计算距离参数、侧面距离参数并进行加权计算，并根据计算结果选择触须路径。

障碍物与触须之间的距离对触须选取的影响较大，设障碍物与触须的距离为l

其中，v

自车行驶时，侧面距离越大越容易通过，利用一个参数来表示侧面通过性：

式中，v

最终将所计算的两个评价参数进行加权计算，得到最终结果v

v

式中，a

根据得到的结果选择一个最优路径并开始进行下一步规划。

对下一步进行规划之后则进行步骤S9：判定所处位置是否为目标点，计算当前位置与目标点位置距离，如果小于一个极小范围，则判断已经到达目标标点位置，如果不是则跳转至步骤S2继续执行规划任务，如果已经到达目标点，则退出规划程序，完成本次无人车轨迹规划。

本发明的一种基于场论与触须算法的无人车轨迹规划方法，以无人车所处的环境建立引力场、动能场、势能场；引力场中目标点对无人车产生引力，固定障碍物及道路边界、车道线构建的势能场等对无人车产生斥力，交通参与者等形成动能场对无人车产生动态斥力，无人车根据其所受的合力的方向与大小决定其移动方向与速度，设置安全范围L，规定周围车辆在大于此安全范围时，不受斥力的横向力，同时引入触须算法，当无人车陷入局部最优时，利用触须算法逃逸。