CN112269384A - 一种结合障碍物行为意图的车辆动态轨迹规划方法 - Google Patents

一种结合障碍物行为意图的车辆动态轨迹规划方法 Download PDF

Info

Publication number
CN112269384A
CN112269384A CN202011144751.2A CN202011144751A CN112269384A CN 112269384 A CN112269384 A CN 112269384A CN 202011144751 A CN202011144751 A CN 202011144751A CN 112269384 A CN112269384 A CN 112269384A
Authority
CN
China
Prior art keywords
longitudinal
vehicle
lateral
obstacle
acceleration
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.)
Granted
Application number
CN202011144751.2A
Other languages
English (en)
Other versions
CN112269384B (zh
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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202011144751.2A priority Critical patent/CN112269384B/zh
Publication of CN112269384A publication Critical patent/CN112269384A/zh
Application granted granted Critical
Publication of CN112269384B publication Critical patent/CN112269384B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种结合障碍物行为意图的车辆动态轨迹规划方法，利用传感器获得车辆当前的位置状态信息，利用感知模块获取当前环境的障碍物周围环境信息，利用决策模块获得车辆下一时刻期望的位置状态，利用预测模块获取障碍物在下一时刻的可能位置状态信息。将获取到的车辆和障碍物在当前时刻和下一时刻的信息作为约束，采用在横纵向分别建立五次、四项多项式的方式，对车辆和障碍物分别进行横纵向的解耦；随后分别对车辆和障碍物的横纵向轨迹进行碰撞检测，将没有发生碰撞的轨迹逐个进行损失评价，选出损失最小的车辆轨迹并输出。

Description

一种结合障碍物行为意图的车辆动态轨迹规划方法
技术领域
本发明属于智能汽车技术领域，更为具体地讲，涉及一种结合障碍物行为意图的车辆动态轨迹规划方法。
背景技术
关于车辆轨迹规划，主要是负责产生一条从当前状态到下一个目标状态的运动轨迹，并且在规划轨迹的时候应该考虑到行驶空间的约束条件，以及满足车辆运动学约束与乘坐舒适性。无人驾驶车辆的路径规划算法主要有，一类是基于随机采样的算法，一类是基于优化理论的算法。
随机采样方法主要是通过在空间中通过随机生成目标可能性的状态，来通过概率的方法对生成的状态进行更新迭代。随机抽样方法是随机的去采样。也就是说，只要对当前状态下的运动计划问题有一个理论上的解决方案，算法就需要在无数次迭代之后找到一个理论上的解决方案。随机采样方法通常包括PRM(概率路线图方法)算法和RRT(快速搜索随机树)算法。
但PRM算法无法用于情况较为复杂的环境，如障碍物较多时，同时该算法生成的路径一般可行性比较差，众多学者对其方法进行改进变种算法。原始的RRT算法采用的是均匀采样的方法，从而该方法效率较差，且规划的结果是不连续的，众多专家学者对其进行了改进优化，提出了很多变种算法，如RRT-Connect算法通过在起点与目标同时构建随机树，当两棵树相遇时便找到了解。启发式RRT算法，使用启发函数引导RRT节点的生长方向，从而提高算法的收敛速度。PCL-RRT算法结合交通场景下其他车辆的不确定性，通过将碰撞概率做为启发函数，从而找出最优路径。RRT算法及其变种算法在原有的基础上提高了规划效率，但存在求解结果不稳定等缺陷。
基于优化理论的算法的一种思路是将运动计划问题分为两个步骤。首先，对地图进行随机采样以获取粗略的路径，然后使用优化的路径曲率生成满足道路几何约束的路径。顾天宇等提出了一种基于最优控制理论的轨迹规划，并改进了轨迹评估功能，使当前驾驶环境下的轨迹成为最优轨迹。基于优化理论的运动计划算法可以生成适合于驾驶场景的最优轨迹，但是存在效率低下的问题。
以上车俩轨迹规划算法虽考虑轨迹规划的安全性、可行性，但均仅仅将驾驶环境中行人或者其他车辆障碍物静态信息直接输入，没有考虑到的障碍物动态信息(如障碍物的意图，和可能的未来轨迹)，存在一定的缺陷(如行人在将要横穿马路时，和行人将要继续向前行走或者停止，车辆规划出的最优轨迹是不一样的)。目前有人虽提出障碍物的可能碰撞半径或者采用引力势场等方法来减少动态障碍物对汽车碰撞的可能，但仍未考虑行人、其他车辆的动作意图和未来可能的轨迹情况。针对以上问题，本发明针对动态障碍物避让领域，基于车载移动计算平台，结合行人、车辆的动作意图信息，以车辆和障碍物在横纵向轨迹的分解再融合的方法来规划轨迹，来减少碰撞的可能，从而实现一种结合障碍物动作意图的轨迹规划方法，提高无人驾驶的安全性。
发明内容
本发明的目的在于克服现有技术的不足，提供一种结合障碍物行为意图的车辆动态轨迹规划方法，以轨迹的横纵向解耦为基础，通过障碍物的动作意图，规划出车辆的行驶轨迹，以提高车辆行驶的安全性。
为实现上述发明目的，本发明一种结合障碍物行为意图的车辆动态轨迹规划方法，其特征在于，包括以下步骤：
(1)、利用全局路径模块获取车辆行驶过程中的全局路径；
(2)、车辆解耦
(2.1)、车辆横向解耦
(2.1.1)、利用车辆的车载组合惯导传感器获取t0时刻的经纬度，再将经纬度转换为笛卡尔坐标系下的位置点，在结合全局路径计算到横向行驶距离，记为d(t0)；直接通过车载组合惯导传感器获取t0时刻的横向行驶速度和横向行驶加速度，分别记为
Figure BDA0002739358110000021
和
Figure BDA0002739358110000022
将t0时刻车辆的横向行驶距离、横向行驶速度和横向行驶加速度通过五次多项式表示为：
Figure BDA0002739358110000031
其中，
Figure BDA0002739358110000032
表示待确定的横向系数，上标d表示横向；
(2.1.2)、通过车辆决策节点提供t1时刻车辆到全局路径的期望横向行驶距离，记为d(t1)；以及t1时刻期望横向行驶速度和期望横向行驶加速度，分别记为
Figure BDA0002739358110000033
和
Figure BDA0002739358110000034
其中下标e表示期望；
将t1时刻车辆的期望横向行驶距离、期望横向行驶速度和期望横向行驶加速度通过五次多项式表示为：
Figure BDA0002739358110000035
(2.1.3)、令t0＝0，记T＝t1-t0，对公式(1)、(2)进行联合求解，获得横向系数
Figure BDA0002739358110000036
Figure BDA0002739358110000037
Figure BDA0002739358110000038
(2.1.4)、构建车辆的横向行驶轨迹方程：
Figure BDA0002739358110000039
(2.2)、车辆纵向解耦
(2.2.1)、利用车辆的车载组合惯导传感器获取t0时刻的经纬度，再将经纬度转换为笛卡尔坐标系下的位置点，在结合全局路径计算纵向行驶距离，记为s(t0)；直接通过车载组合惯导传感器获取t0时刻的纵向行驶速度和纵向行驶加速度，分别记为
Figure BDA00027393581100000310
和
Figure BDA00027393581100000311
将t0时刻车辆的纵向行驶距离、纵向行驶速度和纵向行驶加速度通过四次多项式表示为：
Figure BDA0002739358110000041
其中，
Figure BDA0002739358110000042
表示待确定的纵向系数，上标s表示纵向；
(2.2.2)、通过车辆决策节点提供t1时刻车辆期望的纵向行驶速度和纵向行驶加速度，分别记为
Figure BDA0002739358110000043
和
Figure BDA0002739358110000044
将t1时刻车辆的纵向行驶距离、纵向行驶速度和纵向行驶加速度通过四次多项式表示为：
Figure BDA0002739358110000045
(2.2.3)、令t0＝0，记T＝t1-t0，对公式(5)、(6)进行联合求解，获得横向系数
Figure BDA0002739358110000046
Figure BDA0002739358110000047
Figure BDA0002739358110000048
(2.2.4)、构建车辆的纵向行驶轨迹方程：
Figure BDA0002739358110000049
(3)、障碍物解耦
(3.1)、障碍物的横向解耦
(3.1.1)、利用车辆的障碍物感知模块获取t0时刻障碍物的横向位置、横向速度和横向加速度，分别记为d0(t0)、
Figure BDA00027393581100000410
和
Figure BDA00027393581100000411
将t0时刻障碍物的横向位置、横向速度和横向加速度通过五次多项式表示为：
Figure BDA00027393581100000412
其中，
Figure BDA0002739358110000051
表示待确定的横向系数，上标d表示横向；
(3.1.2)、利用车辆的障碍物预测模块获取t1时刻障碍物的横向位置、横向速度和横向加速度，分别记为d0(t1)、
Figure BDA0002739358110000052
和
Figure BDA0002739358110000053
将t1时刻障碍物的横向位置、横向速度和横向加速度通过五次多项式表示为：
Figure BDA0002739358110000054
(3.1.3)、令t0＝0，记T＝t1-t0，对公式(9)、(10)进行联合求解，获得横向系数
Figure BDA0002739358110000055
Figure BDA0002739358110000056
Figure BDA0002739358110000057
(3.1.4)、构建障碍物的横向轨迹方程：
Figure BDA0002739358110000058
(3.2)、障碍物纵向解耦
(3.2.1)、利用车辆的障碍物感知模块获取t0时刻障碍物的纵向位置、纵向速度和纵向加速度，分别记为s0(t0)、
Figure BDA0002739358110000059
和
Figure BDA00027393581100000510
将t0时刻障碍物的纵向位置、纵向速度和纵向加速度通过四次多项式表示为：
Figure BDA00027393581100000511
其中，
Figure BDA00027393581100000512
表示待确定的纵向系数，上标s表示纵向；
(3.2.2)、利用车辆的障碍物预测模块获取t1时刻障碍物的纵向速度和纵向加速度，分别记为
Figure BDA0002739358110000061
和
Figure BDA0002739358110000062
将t1时刻障碍物的纵向速度和纵向加速度通过四次多项式表示为：
Figure BDA0002739358110000063
(3.2.3)、令t0＝0，记T＝t1-t0，对公式(13)、(14)进行联合求解，获得纵向系数
Figure BDA0002739358110000064
Figure BDA0002739358110000065
Figure BDA0002739358110000066
(3.2.4)、构建障碍物的纵向轨迹方程：
Figure BDA0002739358110000067
(4)、分别对横、纵轨迹方程进行碰撞检测，生成车辆的横、纵向的备选轨迹集；
(4.1)、将车辆横向行驶轨迹方程d(t)与障碍物的横向轨迹方程d0(t)进行联合求解，若某一时刻t有解，则判定t时刻车辆与障碍物在横向上发生位置重叠；若某一时刻t无解，则将时刻t对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)加入到备选轨迹集，再进行下一个时刻的碰撞检测；
(4.2)、判断步骤(4.1)中t时刻的解是否也为车辆纵向行驶轨迹方程s(t)与障碍物的纵向轨迹方程s0(t)的解，若是，则判定t时刻车辆与障碍物发生碰撞，并剔除掉车辆在t时刻对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)；若不是，则将时刻t对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)加入到备选轨迹集，再进行下一个时刻的碰撞检测；
(5)、确定车辆最终的行驶轨迹
(5.1)、利用损失函数计算备选轨迹集中每个时刻d(t)与s(t)的总损失值Ctotal(t)；
Ctotal(t)＝Kd(t)Cd(t)+Ks(t)Cs(t) (17)
其中，Kd(t)为t时刻横向距离损失系数，Cd(t)为t时刻的横向损失值，Ks(t)为t时刻纵向距离损失系数，Cs(t)为t时刻的纵向损失值；
(5.2)、选出备选轨迹集中总损失值最小时对应的轨迹d(t)与s(t)，并作为最终规划的行驶轨迹。
本发明的发明目的是这样实现的：
本发明一种结合障碍物行为意图的车辆动态轨迹规划方法，利用传感器获得车辆当前的位置状态信息，利用感知模块获取当前环境的障碍物周围环境信息，利用决策模块获得车辆下一时刻期望的位置状态，利用预测模块获取障碍物在下一时刻的可能位置状态信息。将获取到的车辆和障碍物在当前时刻和下一时刻的信息作为约束，采用在横纵向分别建立五次、四项多项式的方式，对车辆和障碍物分别进行横纵向的解耦；随后分别对车辆和障碍物的横纵向轨迹进行碰撞检测，将没有发生碰撞的轨迹逐个进行损失评价，选出损失最小的车辆轨迹并输出。
同时，本发明一种结合障碍物行为意图的车辆动态轨迹规划方法还具有以下有益效果：
(1)、在进行车辆轨迹规划的时候不仅仅使用车辆的信息和障碍物的位置信息，还加入障碍物的行为意图，来从车辆的横纵向轨迹两个方面分别进行分析，提升轨迹规划的适应性安全性，为自动驾驶的车辆控制的提供可靠轨迹，提高自动驾驶车辆的安全性；
(2)、本方法利用车辆与障碍物信息来构建多项式的方式来求解轨迹，多项式的待定系数的阶数较低，便于求解，相对于其他轨迹规划方式，如利用深度学习等方式过程中的计算量，可以提高整个规划***的实时性；
(3)、本方法还设计了针对横纵向轨迹不同的损失函数，来对横纵向轨迹进行评价，这种评价机制可以进一步的量化轨迹规划结果的好坏，方便其他工作人员在本轨迹规划方法的基础上进行调优，通过对损失函数中不同系数的调整，来使该算法适应更多的不同的行驶环境。
附图说明
图1是本发明一种结合障碍物行为意图的车辆动态轨迹规划方法流程图；
图2是车辆、障碍物解耦的流程图；
具体实施方式
下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。
实施例
图1是本发明一种结合障碍物行为意图的车辆动态轨迹规划方法流程图。
在本实施例中，如图1所示，本发明一种结合障碍物行为意图的车辆动态轨迹规划方法，包括以下步骤：
S1、利用全局路径模块获取车辆行驶过程中的全局路径；
S2、车辆解耦，具体过程如图2所示；
S2.1、车辆横向解耦；
S2.1.1、利用车辆的车载组合惯导传感器获取t0时刻的经纬度，再将经纬度转换为笛卡尔坐标系下的位置点，在结合全局路径计算到横向行驶距离，记为d(t0)；直接通过车载组合惯导传感器获取t0时刻的横向行驶速度和横向行驶加速度，分别记为
Figure BDA0002739358110000081
和
Figure BDA0002739358110000082
将t0时刻车辆的横向行驶距离、横向行驶速度和横向行驶加速度通过五次多项式表示为：
Figure BDA0002739358110000083
其中，
Figure BDA0002739358110000084
表示待确定的横向系数，上标d表示横向；
S2.1.2、通过车辆决策节点提供t1时刻车辆到全局路径的期望横向行驶距离，记为d(t1)；以及t1时刻期望横向行驶速度和期望横向行驶加速度，分别记为
Figure BDA0002739358110000085
和
Figure BDA0002739358110000086
其中下标e表示期望；
将t1时刻车辆的期望横向行驶距离、期望横向行驶速度和期望横向行驶加速度通过五次多项式表示为：
Figure BDA0002739358110000091
S2.1.3、令t0＝0，记T＝t1-t0，T为单位时间；对公式(1)、(2)进行联合求解，获得横向系数
Figure BDA0002739358110000092
Figure BDA0002739358110000093
Figure BDA0002739358110000094
S2.1.4、构建车辆的横向行驶轨迹方程：
Figure BDA0002739358110000095
S2.2、车辆纵向解耦；
S2.2.1、利用车辆的车载组合惯导传感器获取t0时刻的经纬度，再将经纬度转换为笛卡尔坐标系下的位置点，在结合全局路径计算纵向行驶距离，记为s(t0)；直接通过车载组合惯导传感器获取t0时刻的纵向行驶速度和纵向行驶加速度，分别记为
Figure BDA0002739358110000096
和
Figure BDA0002739358110000097
将t0时刻车辆的纵向行驶距离、纵向行驶速度和纵向行驶加速度通过四次多项式表示为：
Figure BDA0002739358110000098
其中，
Figure BDA0002739358110000099
表示待确定的纵向系数，上标s表示纵向；
S2.2.2、通过车辆决策节点提供t1时刻车辆期望的纵向行驶速度和纵向行驶加速度，分别记为
Figure BDA00027393581100000910
和
Figure BDA00027393581100000911
将t1时刻车辆的纵向行驶距离、纵向行驶速度和纵向行驶加速度通过四次多项式表示为：
Figure BDA0002739358110000101
S2.2.3、令t0＝0，记T＝t1-t0，对公式(5)、(6)进行联合求解，获得横向系数
Figure BDA0002739358110000102
Figure BDA0002739358110000103
Figure BDA0002739358110000104
S2.2.4、构建车辆的纵向行驶轨迹方程：
Figure BDA0002739358110000105
S3、障碍物解耦，具体过程如图2所示；
S3.1、障碍物的横向解耦；
S3.1.1、利用车辆的障碍物感知模块获取t0时刻障碍物的横向位置、横向速度和横向加速度，分别记为d0(t0)、
Figure BDA0002739358110000106
和
Figure BDA0002739358110000107
将t0时刻障碍物的横向位置、横向速度和横向加速度通过五次多项式表示为：
Figure BDA0002739358110000108
S3.1.2、利用车辆的障碍物预测模块获取t1时刻障碍物的横向位置、横向速度和横向加速度，分别记为d0(t1)、
Figure BDA0002739358110000109
和
Figure BDA00027393581100001010
将t1时刻障碍物的横向位置、横向速度和横向加速度通过五次多项式表示为：
Figure BDA00027393581100001011
S3.1.3、令t0＝0，记T＝t1-t0，对公式(9)、(10)进行联合求解，获得横向系数
Figure BDA00027393581100001012
Figure BDA0002739358110000111
Figure BDA0002739358110000112
S3.1.4、构建障碍物的横向轨迹方程：
Figure BDA0002739358110000113
S3.2、障碍物纵向解耦；
S3.2.1、利用车辆的障碍物感知模块获取t0时刻障碍物的纵向位置、纵向速度和纵向加速度，分别记为s0(t0)、
Figure BDA0002739358110000114
和
Figure BDA0002739358110000115
将t0时刻障碍物的纵向位置、纵向速度和纵向加速度通过四次多项式表示为：
Figure BDA0002739358110000116
S3.2.2、利用车辆的障碍物预测模块获取t1时刻障碍物的纵向速度和纵向加速度，分别记为
Figure BDA0002739358110000117
和
Figure BDA0002739358110000118
将t1时刻障碍物的纵向速度和纵向加速度通过四次多项式表示为：
Figure BDA0002739358110000119
S3.2.3、令t0＝0，记T＝t1-t0，对公式(13)、(14)进行联合求解，获得纵向系数
Figure BDA00027393581100001110
Figure BDA00027393581100001111
Figure BDA00027393581100001112
S3.2.4、构建障碍物的纵向轨迹方程：
Figure BDA0002739358110000121
S4、分别对横、纵轨迹方程进行碰撞检测，生成车辆的横、纵向的备选轨迹集；
S4.1、将车辆横向行驶轨迹方程d(t)与障碍物的横向轨迹方程d0(t)进行联合求解，若某一时刻t有解，则判定t时刻车辆与障碍物在横向上发生位置重叠；若某一时刻t无解，则将时刻t对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)加入到备选轨迹集，再进行下一个时刻的碰撞检测；
S4.2、判断步骤S4.1中t时刻的解是否也为车辆纵向行驶轨迹方程s(t)与障碍物的纵向轨迹方程s0(t)的解，若是，则判定t时刻车辆与障碍物发生碰撞，并剔除掉车辆在t时刻对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)；若不是，则将时刻t对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)加入到备选轨迹集，再进行下一个时刻的碰撞检测；
S5、确定车辆最终的行驶轨迹；
S5.1、利用损失函数计算备选轨迹集中每个时刻d(t)与s(t)的总损失值Ctotal(t)；
Ctotal(t)＝Kd(t)Cd(t)+Ks(t)Cs(t) (17)
其中，Kd(t)为t时刻横向距离损失系数，Cd(t)为t时刻的横向损失值，Ks(t)为t时刻纵向距离损失系数，Cs(t)为t时刻的纵向损失值；
其中，横向损失值Cd(t)的计算公式为：
Cd(t)＝Kjd(t)Ad(d(t))+Kd(t)(dl(t))2
其中，Kjd(t)为t时刻横向加速度的损失系数，Ad(d(t))为t时刻的横向加速度变化值，Ad(·)为求横向加速度变化绝对值的函数，Kd(t)为t时刻横向距离损失系数，dl(t)为t时刻轨迹此刻偏离全局路径的横向距离。
纵向损失值Cs(t)的计算公式为：
Figure BDA0002739358110000122
其中，Kjs(t)为t时刻纵向加速度的损失系数，As(s(t))为t时刻的纵向加速度变化值，As(·)为求纵向加速度变化绝对值的函数，Ks(t)为t时刻纵向距离损失系数，
Figure BDA0002739358110000131
分别为规划的期望终点的速度和规划点的速度。
S5.2、选出备选轨迹集中总损失值最小时对应的轨迹d(t)与s(t)，并作为最终规划的行驶轨迹。
尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (3)

1.一种结合障碍物行为意图的车辆动态轨迹规划方法，其特征在于，包括以下步骤：
(1)、利用全局路径模块获取车辆行驶过程中的全局路径；
(2)、车辆解耦
(2.1)、车辆横向解耦
(2.1.1)、利用车辆的车载组合惯导传感器获取t0时刻的经纬度，再将经纬度转换为笛卡尔坐标系下的位置点，在结合全局路径计算到横向行驶距离，记为d(t0)；直接通过车载组合惯导传感器获取t0时刻的横向行驶速度和横向行驶加速度，分别记为
Figure FDA0002739358100000011
和
Figure FDA0002739358100000012
将t0时刻车辆的横向行驶距离、横向行驶速度和横向行驶加速度通过五次多项式表示为：
Figure FDA0002739358100000013
其中，
Figure FDA0002739358100000014
表示待确定的横向系数，上标d表示横向；
(2.1.2)、通过车辆决策节点提供t1时刻车辆到全局路径的期望横向行驶距离，记为d(t1)；以及t1时刻期望横向行驶速度和期望横向行驶加速度，分别记为
Figure FDA0002739358100000015
和
Figure FDA0002739358100000016
其中下标e表示期望；
将t1时刻车辆的期望横向行驶距离、期望横向行驶速度和期望横向行驶加速度通过五次多项式表示为：
Figure FDA0002739358100000017
(2.1.3)、令t0＝0，记T＝t1-t0，对公式(1)、(2)进行联合求解，获得横向系数
Figure FDA0002739358100000018
Figure FDA0002739358100000019
Figure FDA0002739358100000021
(2.1.4)、构建车辆的横向行驶轨迹方程：
Figure FDA0002739358100000022
(2.2)、车辆纵向解耦
(2.2.1)、利用车辆的车载组合惯导传感器获取t0时刻的经纬度度，再将经纬度转换为笛卡尔坐标系下的位置点，在结合全局路径计算纵向行驶距离，记为s(t0)；直接通过车载组合惯导传感器获取t0时刻的纵向行驶速度和纵向行驶加速度，分别记为
Figure FDA0002739358100000023
和
Figure FDA0002739358100000024
将t0时刻车辆的纵向行驶距离、纵向行驶速度和纵向行驶加速度通过四次多项式表示为：
Figure FDA0002739358100000025
其中，
Figure FDA0002739358100000026
表示待确定的纵向系数，上标s表示纵向；
(2.2.2)、通过车辆决策节点提供t1时刻车辆期望的纵向行驶速度和纵向行驶加速度，分别记为
Figure FDA0002739358100000027
和
Figure FDA0002739358100000028
将t1时刻车辆的纵向行驶距离、纵向行驶速度和纵向行驶加速度通过四次多项式表示为：
Figure FDA0002739358100000029
(2.2.3)、令t0＝0，记T＝t1-t0，对公式(5)、(6)进行联合求解，获得横向系数
Figure FDA00027393581000000210
Figure FDA00027393581000000211
Figure FDA0002739358100000031
(2.2.4)、构建车辆的纵向行驶轨迹方程：
Figure FDA0002739358100000032
(3)、障碍物解耦
(3.1)、障碍物的横向解耦
(3.1.1)、利用车辆的障碍物感知模块获取t0时刻障碍物的横向位置、横向速度和横向加速度，分别记为d0(t0)、
Figure FDA0002739358100000033
和
Figure FDA0002739358100000034
将t0时刻障碍物的横向位置、横向速度和横向加速度通过五次多项式表示为：
Figure FDA0002739358100000035
其中，
Figure FDA0002739358100000036
表示待确定的横向系数，上标d表示横向；
(3.1.2)、利用车辆的障碍物预测模块获取t1时刻障碍物的横向位置、横向速度和横向加速度，分别记为d0(t1)、
Figure FDA0002739358100000037
和
Figure FDA0002739358100000038
将t1时刻障碍物的横向位置、横向速度和横向加速度通过五次多项式表示为：
Figure FDA0002739358100000039
(3.1.3)、令t0＝0，记T＝t1-t0，对公式(9)、(10)进行联合求解，获得横向系数
Figure FDA00027393581000000310
Figure FDA00027393581000000311
Figure FDA0002739358100000041
(3.1.4)、构建障碍物的横向轨迹方程：
Figure FDA0002739358100000042
(3.2)、障碍物纵向解耦
(3.2.1)、利用车辆的障碍物感知模块获取t0时刻障碍物的纵向位置、纵向速度和纵向加速度，分别记为s0(t0)、
Figure FDA0002739358100000043
和
Figure FDA0002739358100000044
将t0时刻障碍物的纵向位置、纵向速度和纵向加速度通过四次多项式表示为：
Figure FDA0002739358100000045
其中，
Figure FDA0002739358100000046
表示待确定的纵向系数，上标s表示纵向；
(3.2.2)、利用车辆的障碍物预测模块获取t1时刻障碍物的纵向速度和纵向加速度，分别记为
Figure FDA0002739358100000047
和
Figure FDA0002739358100000048
将t1时刻障碍物的纵向速度和纵向加速度通过四次多项式表示为：
Figure FDA0002739358100000049
(3.2.3)、令t0＝0，记T＝t1-t0，对公式(13)、(14)进行联合求解，获得纵向系数
Figure FDA00027393581000000410
Figure FDA00027393581000000411
Figure FDA00027393581000000412
(3.2.4)、构建障碍物的纵向轨迹方程：
Figure FDA00027393581000000413
(4)、分别对横、纵轨迹方程进行碰撞碰撞检测，生成车辆的横、纵向的备选轨迹集；
(4.1)、将车辆横向行驶轨迹方程d(t)与障碍物的横向轨迹方程d0(t)进行联合求解，若某一时刻t有解，则判定t时刻车辆与障碍物在横向上发生位置重叠；若某一时刻t无解，则将时刻t对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)加入到备选轨迹集，再进行下一个时刻的碰撞检测；
(4.2)、判断步骤(4.1)中t时刻的解是否也为车辆纵向行驶轨迹方程s(t)与障碍物的纵向轨迹方程s0(t)的解，若是，则判定t时刻车辆与障碍物发生碰撞，并剔除掉车辆在t时刻对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)；若不是，则将时刻t对应的横向行驶轨迹方程d(t)和纵向行驶轨迹方程s(t)加入到备选轨迹集，再进行下一个时刻的碰撞检测；
(5)、确定车辆最终的行驶轨迹
(5.1)、利用损失函数计算备选轨迹集中每个时刻d(t)与s(t)的总损失值Ctotal(t)；
Ctotal(t)＝Kd(t)Cd(t)+Ks(t)Cs(t)(17)
其中，Kd(t)为t时刻横向距离损失系数，Cd(t)为t时刻的横向损失值，Ks(t)为t时刻纵向距离损失系数，Cs(t)为t时刻的纵向损失值；
(5.2)、选出备选轨迹集中总损失值最小时对应的轨迹d(t)与s(t)，并作为最终规划的行驶轨迹。
2.根据权利要求1所述的一种结合障碍物行为意图的车辆动态轨迹规划方法，其特征在于，所述横向损失值Cd(t)的计算公式为：
Cd(t)＝Kjd(t)Ad(d(t))+Kd(t)(dl(t))2
其中，Kjd(t)为t时刻横向加速度的损失系数，Ad(d(t))为t时刻的横向加速度变化值，Ad(·)为求横向加速度变化绝对值的函数，Kd(t)为t时刻横向距离损失系数，dl(t)为t时刻轨迹此刻偏离全局路径的横向距离。
3.根据权利要求1所述的一种结合障碍物行为意图的车辆动态轨迹规划方法，其特征在于，所述纵向损失值Cs(t)的计算公式为：
Figure FDA0002739358100000051
其中，Kjs(t)为t时刻纵向加速度的损失系数，As(s(t))为t时刻的纵向加速度变化值，As(·)为求纵向加速度变化绝对值的函数，Ks(t)为t时刻纵向距离损失系数，
Figure FDA0002739358100000061
分别为规划的期望终点的速度和规划点的速度。
CN202011144751.2A 2020-10-23 2020-10-23 一种结合障碍物行为意图的车辆动态轨迹规划方法 Active CN112269384B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011144751.2A CN112269384B (zh) 2020-10-23 2020-10-23 一种结合障碍物行为意图的车辆动态轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011144751.2A CN112269384B (zh) 2020-10-23 2020-10-23 一种结合障碍物行为意图的车辆动态轨迹规划方法

Publications (2)

Publication Number Publication Date
CN112269384A true CN112269384A (zh) 2021-01-26
CN112269384B CN112269384B (zh) 2021-09-14

Family

ID=74341553

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011144751.2A Active CN112269384B (zh) 2020-10-23 2020-10-23 一种结合障碍物行为意图的车辆动态轨迹规划方法

Country Status (1)

Country Link
CN (1) CN112269384B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113359763A (zh) * 2021-07-05 2021-09-07 广州蓝胖子移动科技有限公司 轮式机器人及动态障碍物轨迹预测方法、装置、存储介质
CN113516862A (zh) * 2021-07-22 2021-10-19 中国第一汽车股份有限公司 一种预警方法、装置、电子设备以及存储介质
CN113753079A (zh) * 2021-08-23 2021-12-07 上海宏景智驾信息科技有限公司 用于车辆状态跟踪过程中的时间对齐方法
CN116300973A (zh) * 2023-05-18 2023-06-23 上海伯镭智能科技有限公司 一种复杂天气下无人驾驶矿车自主避障方法
CN117962932A (zh) * 2024-04-02 2024-05-03 福瑞泰克智能***有限公司 障碍物的行驶轨迹生成方法、装置和存储介质及电子设备

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103605368A (zh) * 2013-12-04 2014-02-26 苏州大学张家港工业技术研究院 一种动态未知环境中路径规划方法及装置
CN106527452A (zh) * 2016-12-30 2017-03-22 广州汽车集团股份有限公司 一种无人驾驶汽车遇障时运动路径规划方法及***
CN108597223A (zh) * 2018-04-28 2018-09-28 北京智行者科技有限公司 一种用于智能车辆行为描述的数据处理方法和***
CN109131326A (zh) * 2018-08-08 2019-01-04 大连理工大学 一种具有换道辅助功能的自适应巡航控制器及其工作方法
CN109855639A (zh) * 2019-01-15 2019-06-07 天津大学 基于障碍物预测与mpc算法的无人驾驶轨迹规划方法
CN109960261A (zh) * 2019-03-22 2019-07-02 北京理工大学 一种基于碰撞检测的动态障碍物避让方法
CN110263607A (zh) * 2018-12-07 2019-09-20 电子科技大学 一种用于无人驾驶的道路级全局环境地图生成方法
CN110362096A (zh) * 2019-08-13 2019-10-22 东北大学 一种基于局部最优性的无人驾驶车辆动态轨迹规划方法
US20190367019A1 (en) * 2018-05-31 2019-12-05 TuSimple System and method for proximate vehicle intention prediction for autonomous vehicles

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103605368A (zh) * 2013-12-04 2014-02-26 苏州大学张家港工业技术研究院 一种动态未知环境中路径规划方法及装置
CN106527452A (zh) * 2016-12-30 2017-03-22 广州汽车集团股份有限公司 一种无人驾驶汽车遇障时运动路径规划方法及***
CN108597223A (zh) * 2018-04-28 2018-09-28 北京智行者科技有限公司 一种用于智能车辆行为描述的数据处理方法和***
US20190367019A1 (en) * 2018-05-31 2019-12-05 TuSimple System and method for proximate vehicle intention prediction for autonomous vehicles
CN109131326A (zh) * 2018-08-08 2019-01-04 大连理工大学 一种具有换道辅助功能的自适应巡航控制器及其工作方法
CN110263607A (zh) * 2018-12-07 2019-09-20 电子科技大学 一种用于无人驾驶的道路级全局环境地图生成方法
CN109855639A (zh) * 2019-01-15 2019-06-07 天津大学 基于障碍物预测与mpc算法的无人驾驶轨迹规划方法
CN109960261A (zh) * 2019-03-22 2019-07-02 北京理工大学 一种基于碰撞检测的动态障碍物避让方法
CN110362096A (zh) * 2019-08-13 2019-10-22 东北大学 一种基于局部最优性的无人驾驶车辆动态轨迹规划方法

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
DONGHAN LEE，等: "Convolution Neural Network-based Lane Change Intention Prediction of Surrounding Vehicles for ACC", 《2017 IEEE 20TH INTERNATIONAL CONFERENCE ON INTELLIGENT TRANSPORTATION SYSTEMS》 *
HUIQIN ZHAN,等: "Pedestrian Detection and Behavior Recognition Based on Vision", 《2019 IEEE INTELLIGENT TRANSPORTATION SYSTEMS CONFERENCE》 *
MARCUS GREIFF，ANDERS ROBERTSSON: "Optimisation-based motion planning with obstacle and priorities", 《IFAC PAPERSONLINE》 *
NASER AZIM MOHSENI，AHMAD FAKHARIAN: "Optimal Trajectory Planning for an Omni-Directional Mobile Robot with Static Obstacles:A Polynomial Based Approach", 《IEEE》 *
ZHAN HUIQIN，等: "Modeling and Analysis of a Testing System Using Hybrid Petri Net", 《THE EIGHTH INTERNATIONAL CONFERENCE ON ELECTRONIC MEASUREMENT AND INSTRUMENTS》 *
刘铭，等: "基于Multi-Agent***的多飞行器协同路径规划方法的研究", 《计算机科学》 *
吴功跃: "基于障碍函数的多目标规划最优问题研究", 《智库时代》 *
钟星，等: "多障碍物环境下挖掘机的连续避障轨迹规划", 《计算机工程与科学》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113359763A (zh) * 2021-07-05 2021-09-07 广州蓝胖子移动科技有限公司 轮式机器人及动态障碍物轨迹预测方法、装置、存储介质
CN113516862A (zh) * 2021-07-22 2021-10-19 中国第一汽车股份有限公司 一种预警方法、装置、电子设备以及存储介质
CN113753079A (zh) * 2021-08-23 2021-12-07 上海宏景智驾信息科技有限公司 用于车辆状态跟踪过程中的时间对齐方法
CN116300973A (zh) * 2023-05-18 2023-06-23 上海伯镭智能科技有限公司 一种复杂天气下无人驾驶矿车自主避障方法
CN117962932A (zh) * 2024-04-02 2024-05-03 福瑞泰克智能***有限公司 障碍物的行驶轨迹生成方法、装置和存储介质及电子设备
CN117962932B (zh) * 2024-04-02 2024-06-11 福瑞泰克智能***有限公司 障碍物的行驶轨迹生成方法、装置和存储介质及电子设备

Also Published As

Publication number Publication date
CN112269384B (zh) 2021-09-14

Similar Documents

Publication Publication Date Title
CN112269384B (zh) 一种结合障碍物行为意图的车辆动态轨迹规划方法
CN113359757B (zh) 一种无人驾驶车辆路径规划与轨迹跟踪方法
Wang et al. Trajectory planning and safety assessment of autonomous vehicles based on motion prediction and model predictive control
CN112368662B (zh) 用于自主运载工具操作管理的定向调整动作
JP6911214B1 (ja) 軌道計画
US11698638B2 (en) System and method for predictive path planning in autonomous vehicles
CN109085820B (zh) 临界条件的自主车辆驾驶***和方法
CN108062094B (zh) 基于处理器实现车辆行驶轨迹规划的自主***和方法
US10688991B2 (en) Systems and methods for unprotected maneuver mitigation in autonomous vehicles
Wei et al. A behavioral planning framework for autonomous driving
US10495493B2 (en) Systems and methods for controlling sensing device field of view
US10678245B2 (en) Systems and methods for predicting entity behavior
CN114771563A (zh) 一种自动驾驶车辆轨迹规划控制实现方法
WO2022173880A9 (en) System, method, and computer program product for topological planning in autonomous driving using bounds representations
KR20220110069A (ko) 샘플링 기반 기동 실현기
WO2023147769A1 (en) Systems, methods, and computer-readable media for spatio-temporal motion planning
Yijing et al. Local path planning of autonomous vehicles based on A* algorithm with equal-step sampling
CN117315970A (zh) 涉及交叉路口处交通拥堵的自主车辆的车道改变
CN114639267A (zh) 一种车路协同环境中车辆避撞预警方法
US11904855B2 (en) Cooperative driving system and method
US20230211802A1 (en) Motion planning using spatio-temporal convex corridors
Ren et al. Intelligent path planning and obstacle avoidance algorithms for autonomous vehicles based on enhanced rrt algorithm
CN115218902A (zh) 一种轨迹规划方法、装置、设备及存储介质
Sun et al. A dynamic local trajectory planning and tracking method for ugv based on optimal algorithm
You Real Time Motion Planning Using Constrained Iterative Linear Quadratic Regulator for On-Road Self-Driving

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
GR01 Patent grant
GR01 Patent grant