CN114995415A - 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法 - Google Patents

一种基于时空可达集理论的自动驾驶汽车轨迹规划方法 Download PDF

Info

Publication number
CN114995415A
CN114995415A CN202210603003.9A CN202210603003A CN114995415A CN 114995415 A CN114995415 A CN 114995415A CN 202210603003 A CN202210603003 A CN 202210603003A CN 114995415 A CN114995415 A CN 114995415A
Authority
CN
China
Prior art keywords
time
reachable
longitudinal
transverse
node
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
CN202210603003.9A
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.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
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 Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202210603003.9A priority Critical patent/CN114995415A/zh
Publication of CN114995415A publication Critical patent/CN114995415A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001—Planning or execution of driving tasks
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • 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

Abstract

本发明公开了一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，通过自车的位置、速度等状态信息和障碍物的预测轨迹计算前向可达集并均匀离散化；基于二维高斯分布和车辆运动学模型表征障碍物在时空域下的位置分布，计算出各时刻下的风险水平，并将风险过高区域从前向可达集中移除；根据前向可达集计算时刻间结点的父子关系，生成时域内结点树；根据行驶效率、安全性、交通规则评价结点成本，采用动态规划得出最优结点链；根据各时刻最优结点进行扩展，得到各时刻最优行驶区域；最后通过二次规划考量横纵向的乘坐舒适性和最优性得出最终平滑轨迹。本发明提高了自动驾驶汽车轨迹规划算法的可靠性，灵活性和鲁棒性。

Description

一种基于时空可达集理论的自动驾驶汽车轨迹规划方法
技术领域
本发明涉及一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，属于智能辅助驾驶技术领域。
背景技术
智能汽车通常采用包含感知、规划和控制的分层架构。其中轨迹规划层起着承上启下的作用，负责输出汽车的可行驶路径和期望加速度。自动驾驶汽车的轨迹规划理论最早起源于移动机器人领域，考虑到汽车运动学特性，通常采用路径-速度解耦的规划方法。但是，车辆在实际行驶中，面对的交通环境往往是动态的，当道路场景较为复杂，尤其是考虑动态障碍物由于其运动产生的不确定性时，解耦的轨迹规划方法的实际效果通常难以达到理想的效果。
可达集是指动态系统在有界约束的限制下，从初始状态出发所能达到的所有可能状态的集合，通过可达集轨迹，我们可以得到系统正常的运行范围，之后应用相应的控制措施，避免系统进入异常运行的范围。传统可达集方法在确定凸走廊时由于同一时刻内各可达集面积不同，可达集内部风险不均布，难以定量评价各可达集风险水平。且由于时刻间可达集父子关系不明确，难以形成时空连接结构，而通过后向可达集计算凸走廊只考虑了最后时刻的收益，对于过程中的风险和收益没有考量，在实际规划过程中可靠性不足。
因此，本领域技术人员急需要解决现有技术中存在的自动驾驶汽车局部轨迹规划方法灵活性不足，传统可达集方法未考虑不确定性风险和过程中风险收益的问题。
发明内容
目的：为了克服现有技术中存在的自动驾驶汽车局部轨迹规划方法灵活性不足，传统可达集方法未考虑不确定性风险和过程中风险收益的问题，本发明提供一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，通过对前向可达集的离散化并引入二维高斯分布来定量表征风险并构建结点树，通过动态规划搜索时空域下整体最优路径，在保证安全性的前提下提高了轨迹规划算法的灵活性和可靠性。技术方案：为解决上述技术问题，本发明采用的技术方案为：
一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，包括如下步骤：
根据车辆运动学模型获取自车的初始状态信息，再根据自车的初始状态信息和交通规则相关信息计算横向、纵向前向可达集。
将横向、纵向前向可达集进行耦合并后投影到X-Y坐标系，得到在X-Y坐标系下车辆各时刻可达区域。
将车辆各时刻可达区域离散化，得到车辆各时刻可达点集。
利用表达障碍物预测轨迹及障碍物运动学模型的二维高斯分布，得到得出障碍物在时空域下的位置分布函数，根据位置分布函数计算出各时刻下的风险分布，并对风险分布进行风险表征，将风险表征超过阈值的区域从可达区域中移除，得到移除后的可达区域，再根据移除后的可达区域得到移除后的可达点集。
根据移除后的可达点集和车辆运动学模型，计算相邻时间间隔间可达点集中离散结点的父子关系，根据离散结点的父子关系生成所有时刻内的整体结点树。根据行驶效率、安全性、交通规则评价所有时刻内的整体结点树的离散结点的成本，求解出从0时刻至预瞄最后时刻的最优结点链。
根据从0时刻至预瞄最后时刻的最优结点链内的离散结点进行扩展，得到各时刻最优行驶区域。
对各时刻最优行驶区域进行考量横纵向的乘坐舒适性的二次规划，得出车辆最终平滑轨迹曲线。
作为优选方案，根据车辆运动学模型获取自车的初始状态信息，再根据自车的初始状态信息和交通规则相关信息计算横向、纵向前向可达集，包括：
步骤1.1：根据自车的初始状态信息，获取车辆在X-Y坐标下的纵向坐标x，纵向速度vx，横向坐标y，横向速度vy，纵向最大速度，横向最大速度。
步骤1.2：提取t0时刻的纵向的前向可达集边界点，下一时刻t1边界点为(x1,v1)，x1为车辆t1时刻的纵向坐标，v1为车辆t1时刻的纵向速度。
步骤1.3：分别计算每个t1时刻纵向的前向可达集边界点产生的凸集边界点。
步骤1.4：计算所有凸集边界点的Minkowski集合，根据Minkowski集合得到t1时刻的理想纵向的前向可达集边界点。
步骤1.5：根据交通规则相关信息获取道路边界限制，道路车道线限制，根据纵向最大速度、道路边界限制和道路车道线限制对理想纵向的前向可达集边界点进行饱和处理，得到t1时刻纵向的前向可达集。
步骤1.6：将t1时刻纵向的前向可达集边界点代入步骤1.2到步骤1.5进行迭代，直到计算至规划预瞄时刻tT，得到从t0到tT时刻的纵向的前向可达集。
步骤1.7：提取t0时刻的横向的前向可达集边界点，下一时刻t1边界点为(y1，vy1)，y1为车辆t1时刻的横向坐标，vy1为车辆t1时刻的横向速度。步骤1.8：分别计算每个t1时刻横向的前向可达集边界点产生的凸集边界点。步骤1.9：计算所有凸集边界点的Minkowski集合，根据Minkowski集合得到t1时刻的理想横向的前向可达集边界点。
步骤1.10：根据交通规则相关信息获取道路边界限制，道路车道线限制，根据横向最大速度、道路边界限制和道路车道线限制对理想横向的前向可达集边界点进行饱和处理，得到t1时刻横向的前向可达集。
步骤1.11：将t1时刻横向的前向可达集边界点代入步骤1.7到步骤1.10进行迭代，直到计算至规划预瞄时刻tT，得到从t0到tT时刻的横向的前向可达集。
作为优选方案，将车辆各时刻可达区域离散化，得到车辆各时刻可达点集，包括：将车辆各时刻可达区域按横向离散结点间距、纵向离散结点间距进行离散化，得到车辆各时刻可达点集。
作为优选方案，障碍物的加速度、速度、初始位置的误差分别服从均值为0、方差为
Figure BDA0003661585260000031
的二维高斯分布。障碍物每时刻间位置的误差不确定度增量服从纵横向方差为Var(x′)、Var(Y′)的二维高斯分布。障碍物的航向角服从协方差为Rt的二维高斯分布。
作为优选方案，根据位置分布函数计算出各时刻下的风险分布，并对风险分布进行风险表征，包括：
以每时刻可达点集中各个离散结点为质心、车辆外轮廓为边界在二维高斯分布函数中求解二重积分得出风险表征。
作为优选方案，阈值设置为3％。
作为优选方案，根据移除后的可达点集和车辆运动学模型，计算相邻时间间隔间可达点集中离散结点的父子关系，根据离散结点的父子关系生成所有时刻内的整体结点树，包括：
步骤2.1：由t0时刻的移除后的可达点集中得出t0时刻下离散结点n的纵、横向速度的可达范围。
步骤2.2：再根据运动学模型计算得到离散结点n在t1时刻可到达的X-Y区域边界，则在离散结点n的X-Y区域边界内的所有t1时刻下的离散结点均为结点n的子结点。
步骤2.3：重复步骤2.1-2.2直到计算至规划预瞄时刻tT，获得所有时刻每个离散结点的子结点，根据各子结点形成整体结点树。
作为优选方案，根据行驶效率、安全性、交通规则评价所有时刻内的整体结点树的离散结点的成本，求解出从0时刻至预瞄最后时刻的最优结点链。
根据行驶效率、安全性、交通规则评价所有时刻内的整体结点树的离散结点的成本，所述行驶效率表示离散结点的纵向位置和当前时刻可达集纵向位置下边界值，安全性表示离散结点的风险表征值，交通规则表示离散结点横向距车道中心距离。
根据整体结点树的离散结点的成本和离散结点所在层数，提取符合本层离散结点成本最小和本层离散结点对应的子结点成本最小的本层离散结点作为本层最优结点，直到遍历完所有层的离散结点，得到所有层最优结点作为最优结点链。
作为优选方案，对各时刻最优行驶区域进行考量横纵向的乘坐舒适性的二次规划，得出车辆最终平滑轨迹曲线，包括：
获取各时刻最优行驶区域的纵向范围[xmin；xmax]，最优结点链的纵向坐标向量Xx。
根据纵向成本函数求解出平滑轨迹点纵向向量x＝[xt0；xt1…；xtT]。所述纵向成本函数f(x)表示为下式：
f(x)＝C1Ax 2+C2Jx 2+C3(Xx-x)2
x≥xmin，x≤xmax
Figure BDA0003661585260000041
Figure BDA0003661585260000042
式中，C1、C2、C3为权重系数；xmin、xmax为x优化的上、下边界；Axk为单一点纵向加速度成本；Ax为总体加速度成本，由t0-tT时刻Axk累加得到；Jk为单一点纵向冲击度成本；Jx为总体冲击度成本，由t0-tT时刻Jk累加得到；xk、xk+1、xk-1、xk-2、xk-3分别为第k、k+1、k-1、k-2、k-3时刻纵向坐标最优值，t为时间间隔。
获取各时刻最优行驶区域的横向范围[ymin；ymax]，最优结点链的横向坐标向量Yy。
根据横向成本函数求解出平滑轨迹点横向向量y＝[yt0；yt1…；ytT]。所述横向成本函数f(y)表示为下式：
f(y)＝C4Ay 2+C5Cky 2+C6(Yy-y)2
y≥ymin，y≤ymax
Figure BDA0003661585260000051
Figure BDA0003661585260000052
式中，C4、C5、C6为权重系数，ymin、ymax为y优化的上、下边界；Ayk为单一点横向加速度成本；Ay为总体加速度成本，由t0-tT时刻Ayk累加得到；Ck为单一点曲率成本；Cky为总体曲率成本，由t0-tT时刻Ck累加得到；yk、yk+1、yk-1、yk-2分别为第k、k+1、k-1、k-2时刻横向坐标最优值，t为时间间隔。
根据平滑轨迹点纵向向量x、平滑轨迹点横向向量y获得t0到tT时刻的车辆最终平滑轨迹曲线。
有益效果：本发明提供的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，通过对前向可达集的离散化并引入二维高斯分布来定量表征风险并构建结点树，通过动态规划搜索时空域下整体最优路径，在保证安全性的前提下提高了轨迹规划算法的灵活性和可靠性。
本发明基于时空可达集理论的轨迹规划相较于传统方法在轨迹规划时灵活度不足的缺点进行了较大改善，并且鲁棒性更强。提高了自动驾驶汽车轨迹规划算法的可靠性和灵活性。
附图说明
图1是一种基于时空可达集理论的自动驾驶汽车轨迹规划方法总体架构框图。
图2是随机测试场景及在随机测试场景下与对照算法仿真1000s的对比结果，对照算法采用MOBIL换道模型以及IDM跟车模型进行决策规划。
图3是结点扩展为可行驶区域的具体方法示意图。
具体实施方式
下面结合具体实施例对本发明作更进一步的说明。
本发明的可达集的含义是车辆在满足运动学约束的情况下，各个时刻可以到达的运动状态的集合，其中运动状态包含车辆的横向、纵向位置信息和速度信息。通过考虑运动学约束和障碍物的分布，减少求解可行轨迹的搜索空间。
本发明通过前向可达集离散化可以量化每个结点的风险水平，并且可以明确时刻间结点父子关系，通过动态规划考量整体过程的风险及收益，得出最优凸走廊。相较于传统可达集算法本方法在时空域中考虑了不确定性风险，进行了时空耦合轨迹规划，提高了轨迹规划的灵活性和可靠性。
参照图1～图3所示，本发明提供的一个实例中的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法包括以下步骤：
1)接收上层处理算法得到的自车的初始状态信息，自车的初始状态信息至少包括以下所述之下：在Frenet坐标系下的路程、侧向偏差值、横向速度、纵向速度、自车最大纵横向速度、自车最大纵横向加速度、障碍物的预测轨迹以及交通规则相关信息。
2)基于自车的初始状态信息、交通规则相关信息和运动学模型计算横纵向前向可达集，并对横纵向前向可达集进行结合得到可达区域，对可达区域均匀离散化得到各时刻可达点集。
3)根据障碍物的预测轨迹，基于二维高斯分布和车辆运动学模型得出障碍物在时空域下的位置分布函数，根据位置分布函数计算出各时刻下的风险分布，并对风险分布进行风险表征，将风险表征超过阈值的区域从各时刻可达区域中移除，得到移除后的可达区域，再根据移除后的可达区域得到移除后的可达点集。
4)根据可达点集和车辆运动学模型计算相邻时间间隔间结点的父子关系，生成时域内结点树。
5)根据行驶效率、安全性、交通规则评价时域内结点树的结点的成本，采用动态规划得出从0时刻至预瞄最后时刻的最优结点链。
6)根据前向可达集对最优结点链内的结点进行扩展，得到各时刻最优行驶区域。
7)对各时刻最优行驶区域进行考量横纵向的乘坐舒适性和最优性的二次规划，得出最终平滑轨迹曲线。
进一步地，在所述的步骤1)中，障碍物的预测轨迹包括从0时刻至预瞄时间T以t为时间间隔的障碍物的位置、横纵向速度、航向角及纵向加速度信息。交通规则相关信息包括道路边界限制，道路车道线限制。
进一步地，在所述的步骤2)中，分别进行横向和纵向前向可达集的生成，横向和纵向前向可达集生成方法相同，以纵向前向可达集为例：
在纵向坐标x-纵向速度vx坐标系下表示的，计算从0时刻开始依次递推的0-T时刻的前向可达集，下面将正在计算的纵向前向可达集的所在时刻记为t1时刻。其计算步骤如下：
1)提取的t0时刻的前向可达集边界点，当t1为t0时刻的下一时刻时，边界点为(x1，v1)，x1为车辆的当前纵向坐标，v1为车辆的当前纵向速度。
2)分别计算每个t1时刻边界点在t1时刻产生的凸集边界点，计算方法为：通过计算车辆在一个时间间隔t内先以最大纵向加速度加速运动μ*t时间，再以最大纵向减速度减速运动(1-μ)*t时间，μ为比例系数，(此处μ取0、0.2、0.4、0.6、0.8、1)。可得出该边界点在t0时刻产生的凸集上边界点，再计算一个时间间隔t内先以最大减速度减速运动μ*t时间，再以最大加速度加速运动(1-μ)*t时间(此处μ取0、0.2、0.4、0.6、0.8、1)可得出该边界点在t0时刻产生的凸集下边界点。
3)计算t1时刻的所有边界点在t1时刻产生的所有理想可达集边界点，计算所有凸集边界点的Minkowski集合，根据Minkowski集合得到t1时刻的理想前向可达集边界点。
4)由于车辆模型会受到速度极限、道路车道线限制、道路边界的限制，还需要对理想前向可达集边界进行裁剪：理想前向可达集的纵向值代表车辆可以达到的纵向速度，故纵向值应大于0且小于车辆模型所允许的最大速度，凸集的横向值代表车辆位置，应在道路车道线、边界限制内，对边界点在两个维度上进行饱和处理得到t1时刻的实际可达集，用于t2时刻的计算。
5)重复1)至4)步骤直至计算时刻至规划预瞄时刻T，可得到从0-T的纵向前向可达集合。
依据上述方法可分别得到每个时刻车辆在纵向和横向前向可达集，两者耦合并投影到X-Y坐标系，可得到在X-Y坐标系下车辆每时刻可达区域。
进一步地，在所述的步骤2)中，将X-Y坐标系下车辆每时刻可达区域按横向离散结点间距、纵向离散结点间距进行离散化，得到各时刻可达点集，横向离散结点间距为xm，，优选0.5m，纵向离散结点间距为(b+ic)m，b、c为参数，优选0.2+i*0.1，其中i为第i个时间间隔，m为米。
进一步地，在所述的步骤3)中，基于二维高斯分布函数用于描述障碍物的不确定性位置分布。该二维高斯分布由加速度、速度、初始位置三个子高斯分布经过运动学模型的推导得出，可以有效增加分布的合理性。假设加速度、速度、初始位置的误差分别服从均值为0、方差为
Figure BDA0003661585260000081
则障碍物每时刻间位置的误差不确定度增量也服从二维高斯分布，其纵横向方差Var(X′)、Var(Y′)表示为：
Figure BDA0003661585260000082
Figure BDA0003661585260000083
式中，t表示时间间隔；ayt、axt表示预测障碍物的横纵向加速度；vyt、vxt表示预测障碍物的横纵向速度；
Figure BDA0003661585260000084
为加速度、速度、初始位置的误差分别服从的分布的方差。则t时刻的位置方差有t0到tT时刻的误差增量累计得出，在车身坐标系下横纵向的位置分布为解耦计算的，但在X-Y坐标系下s-ρ为非解耦的，其仅与车辆当前航向角有关，则二维高斯分布协方差Rt表示为式(4)，式中θt为预测障碍物航向角。
Figure BDA0003661585260000091
进一步地，在所述的步骤3)中，计算出各时刻下的二维高斯分布函数后，通过以每时刻可达点集中各个离散结点为质心、车辆外轮廓为边界在二维高斯分布函数中求解二重积分得出风险表征。将风险表征过高对应的区域从可达区域中移除，得到移除后的可达区域，再根据移除后的可达区域得到移除后的可达点集。将风险表征大于3％的可达区域视为风险过高区域。
进一步地，在所述的步骤4)中，根据移除后的可达点集中各时刻离散结点，计算时刻间离散结点的父子关系。以t0时刻的离散结点n为例：由t0时刻的移除后的可达点集中得出该时刻下离散结点n的纵、横向速度的可达范围，再根据运动学模型计算得到离散结点n在t0+t时刻可到达的X-Y区域边界，则在离散结点n的X-Y区域边界内的所有t0+t时刻下的离散结点均为结点n的子结点。由此方法计算t0时刻到tT时刻的每个离散结点的子结点形成整体结点树。
进一步地，在所述的步骤5)中，根据整体结点树，计算每个离散结点的结点成本，结点成本包括三个部分，分别表示为：行驶效率表示为该结点的纵向位置和当前时刻可达集纵向位置下边界值，安全性表示为所述步骤3)所计算的风险表征值。交通规则表示为结点横向距车道中心距离。成本计算后通过动态规划求解出0-T时刻的最优结点链。动态规划具体实施步骤如下：
1)基于行驶效率、安全性、交通规则对每个结点的成本赋值。并生成一个与结点树层数、每层结点数相同的查询表dptable。用于存储最小子问题的最优解。
2)将第T层每个结点的成本值输入到dptable中的对应位置。
3)从T-1层开始，将每个结点的最优解赋值为本结点成本值+该结点子节点的成本值的最小值并记录最小值所在结点。若该结点无子结点，其最优解输入为无穷大。
4)重复3)直至第0层。
从第0层依次向下提取最小值结点信息。所得到结点链为最优结点链。最优结点链表示为[Xt0，Yt0；Xt1，Yt1...；XtT，YtT]，XtT，YtT分别代表第tT时刻结点的纵向坐标值，横向坐标值。
进一步地，在所述的步骤6)中，结点扩展方法为：以此时刻最优结点链内结点为中心，首先长宽同时以结点间隔扩展，如图3-1所示；当边界存在危险结点时停止，之后先横向两侧扩展，如图3-2所示；最后向纵向扩展，如图3-3所示。
可得到每时刻最优行驶区域。
进一步地，在所述的步骤7)中，根据每时刻最优行驶区域，进行二次规划得出最终平滑轨迹曲线。二次规划时先进行纵向优化再进行横向优化，纵向二次规划需要根据每时刻最优行驶区域的纵向范围[xmin；xmax]，基于设定的成本函数求解出平滑轨迹点向量x＝[xt0；xt1...；xtT]，在纵向优化时舒适性指标包括车辆纵向速度、加速度、冲击度，距最优结点纵向距离作为最优性指标。成本函数f(x)表示为下式：
f(x)＝C1Ax 2+C2Jx 2+C3(Xx-x)2 (4)x≥xmin，x≤xmax
Figure BDA0003661585260000101
Figure BDA0003661585260000102
式中，Xx为最优结点链纵向坐标向量[Xt0；Xt1…；XtT]；C1、C2、C3为权重系数；
xmin、xmax为x优化的上下边界；Axk为单一点纵向加速度成本；Ax为总体加速度成本，由0-T时刻Axk累加得到；Jk为单一点纵向冲击度成本；Jk为总体冲击度成本，由0-T时刻Jk累加得到；xk、xk-1、xk-2、xk-3分别为第k、k-1、k-2、k-3时刻纵向最优值；t为时间间隔。
纵向优化完成后进行横向优化，横向二次规划需要根据每时刻最优行驶区域的横向范围[ymin；ymax]，基于设定的成本函数求解出平滑轨迹点向量y＝[yt0；yt1…；ytT]，横向优化时舒适性指标包括车辆横向速度、路径曲率，距最优结点横向距离作为最优性指标。如下式：
f(y)＝C4Ay 2+C5Cxy 2+C6(Yy-y)2 (5)
y≥ymin，y≤ymax
Figure BDA0003661585260000111
Figure BDA0003661585260000112
式中，Yy为最优结点链横向坐标向量[Yt0；Yt1...；YtT]；C4、C5、C6为权重系数；ymin、ymax为优化的上下边界；Ayk为单一点横向加速度成本；Ay为总体加速度成本，由0-T时刻Ayk累加得到；Ck为单一点曲率成本；Cky为总体曲率成本，由0-T时刻Ck累加得到；yk、yk+1、yk-1分别为第k、k+1、k-1时刻横向最优值；t为时间间隔。
本方法数值优化部分需要求解的是含不等式约束的二次规划问题，针对该问题的求解方式是通过log函数，将含不等式约束的二次规划问题转化为无约束的优化问题，之后利用牛顿法，通过对优化目标函数泰勒展开进行求解。先求解纵向二次规划问题得到纵向最优轨迹点，将纵向最优轨迹点作为已知值带入横向二次规划问题，即可最终得到趋于最优解的最终优化轨迹点[xt0，yt0；xt1，yt1…；xtT，ytT]。由此得到平滑规划轨迹曲线。
本发明的工作原理：
下面将结合附图对本发明技术方案的实施例进行详细的描述，包括仿真测试工况及仿真结果如图2所示，仿真测试的道路为一条长约900m的道路，道路类型为通向三车道。在仿真过程中，当自车行驶至设定的激活点时，在道路中随机生成了8个障碍物，其中6个为运动的车辆，其余为两辆静止车辆。当自车行驶至预设的障碍物消失点位置时，所有障碍物消失，等待自车到达下一个激活点后再次激活。环境中的动态车辆基于MOBIL换道模型和IDM跟车模型进行决策规划。本发明提供了一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其总体结构框图如图1所示，具体包括：
1.前向可达集的生成与离散化，每个时刻，基于运动学约束生成可行驶区域是在横向和纵向分别生成的，以纵向前向可达集生成为例，车辆运动学模型为
Figure BDA0003661585260000121
式中，x为车辆纵向x坐标；x0车辆上一时刻纵向x坐标；Ax为车辆纵向加速度；dt为时间间隔；x′为车辆纵向速度；x′0为车辆上一时刻纵向速度。由上述运动学模型可知，一个状态点或一个凸集经过一个时间步长在s-v维度下能达到的一定是一个凸集，故前向可达集推导步骤为：
1)首先提取每个时间间隔下的凸集的上下边界，实际操作时通过边界上采样点近似表示边界。
2)分别计算上下边界点每个点产生的凸集，通过计算车辆在一个时间间隔dt内先以最大加速度加速运动μdt时间，再以最大减速度减速(1-μ)dt时间(此处μ取0、0.2、0.4、0.6、0.8、1)。可得出凸集上边界，再计算一个时间间隔dt内先以最大减速度减速运动μdt时间，再以最大加速度加速(1-μ)dt时间(此处μ取0、0.2、0.4、0.6、0.8、1)得出凸集下边界。
3)计算所有点集合的Minkowski和得到下一时刻的可达集。
4)由于车辆模型会受到速度极限、交通规则、道路边界的限制，还需要对凸集进行裁剪。
5)重复1)至4)步骤直至时刻至规划预瞄时刻T。
在分别得到每个时刻车辆在纵向和横向的可达集之后，两者耦合并投影到Frenet坐标系就可得到在Frenet坐标系下车辆每时刻可到达区域。
2.本方法采用二维高斯分布表征障碍物在空间中的风险水平，本方法将障碍物各时刻在空间中所处的位置描述为一个二维高斯分布，且该高斯分布是根据轨迹预测数据和初始位置、速度、加速度三个子高斯分布根据运动学模型求解得出，假设加速度、速度、初始位置的误差分别服从均值为0、方差为
Figure BDA0003661585260000131
则每时刻间位置的误差不确定度增量也服从二维高斯分布，其纵横向方差Var(X′)、Var(Y′)表示为：
Figure BDA0003661585260000132
Figure BDA0003661585260000133
式中，t表示时间间隔；ayt、axt表示预测障碍物的横纵向加速度；vyt、vxt表示预测障碍物的横纵向速度；
Figure BDA0003661585260000134
为加速度、速度、初始位置的误差分别服从的分布的方差。则t时刻的位置方差有0-t时刻的误差增量累计得出，在车身坐标系下横纵向的位置分布为解耦计算的，但在Frenet坐标系下s-ρ为非解耦的，其仅与车辆当前航向角有关，则二维高斯分布协方差Rt表示为式(4)，式中θt为预测障碍物航向角。
Figure BDA0003661585260000135
3.本方法采用动态规划在结点树中搜索最优子链，动态规划可以有效降低树状结构下最优路径搜索问题的计算复杂度并保证搜索得到的路径为全局最优路径，具体实施步骤如下：
5)基于行驶效率、安全性、交通规则对每个结点的成本赋值。并生成一个与结点树层数、每层结点数相同的查询表dptable。用于存储最小子问题的最优解。
6)从底层第T层开始计算，将第T层每个结点的成本值输入到dptable中的对应位置。
7)从T-1层开始，将每个结点的最优解赋值为本结点成本值+该结点子节点的成本值的最小值并记录最小值所在结点。若该结点无子结点，其最优解输入为无穷大。
8)重复3)直至第0层。
9)从第0层依次向下提取最小值结点信息。所得到结点链为最优结点链。
在获得凸走廊后采用二次规划的方式求解最优轨迹，二次规划时先进行纵向优化再进行横向优化，纵向二次规划需要根据每时刻最优行驶区域的纵向范围[xmin；xmax]，基于设定的成本函数求解出平滑轨迹点向量x＝[xt0；xt1…；xtT]，在纵向优化时舒适性指标包括车辆纵向速度、加速度、冲击度，距最优结点纵向距离作为最优性指标。成本函数f(x)表示为下式：
f(x)＝C1Ax 2+C2Jx 2+C3(Xx-x)2 (10)
x≥xmin，x≤xmax
Figure BDA0003661585260000141
Figure BDA0003661585260000142
式中，Xx为最优子链纵向坐标向量[Xt0；Xt1…；XtT]；C1、C2、C3为权重系数；xmin、xmax为x优化的上下边界；Axk为单一点纵向加速度成本；Ax为总体加速度成本，由0-T时刻Axk累加得到；Jk为单一点纵向冲击度成本；Jx为总体冲击度成本，由0-T时刻Jk累加得到；xk、xk-1、xk-2、xk-3分别为第k、k-1、k-2、k-3时刻纵向最优值；t为时间间隔。
纵向优化完成后进行横向优化，横向二次规划需要根据每时刻最优行驶区域的纵向范围[ymin；ymax]，基于设定的成本函数求解出平滑轨迹点向量y＝[yt0；yt1...；ytT]，横向优化时舒适性指标包括车辆横向速度、路径曲率，距最优结点横向距离作为最优性指标。如下式：
f(y)＝C4Ay 2+C5Cky 2+C6(Yy-y)2 (11)
y≥ymin，y≤ymax
Figure BDA0003661585260000151
Figure BDA0003661585260000152
式中，Yy为最优结点链横向坐标向量[Yt0；Yt1...；YtT]；C4、C5、C6为权重系数；ymin、ymax为优化的上下边界；Ayk为单一点横向加速度成本；Ay为总体加速度成本，由0-T时刻Ayk累加得到；Ck为单一点曲率成本；Cky为总体曲率成本，由0-T时刻Ck累加得到；yk、yk+1、yk-1分别为第k、k+1、k-1时刻横向最优值；t为时间间隔。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：包括如下步骤：
根据车辆运动学模型获取自车的初始状态信息，再根据自车的初始状态信息和交通规则相关信息计算横向、纵向前向可达集；
将横向、纵向前向可达集进行耦合并后投影到X-Y坐标系，得到在X-Y坐标系下车辆各时刻可达区域；
将车辆各时刻可达区域离散化，得到车辆各时刻可达点集；
利用表达障碍物预测轨迹及障碍物运动学模型的二维高斯分布，得到得出障碍物在时空域下的位置分布函数，根据位置分布函数计算出各时刻下的风险分布，并对风险分布进行风险表征，将风险表征超过阈值的区域从可达区域中移除，得到移除后的可达区域，再根据移除后的可达区域得到移除后的可达点集；
根据移除后的可达点集和车辆运动学模型，计算相邻时间间隔间可达点集中离散结点的父子关系，根据离散结点的父子关系生成所有时刻内的整体结点树；
根据行驶效率、安全性、交通规则评价所有时刻内的整体结点树的离散结点的成本，求解出从0时刻至预瞄最后时刻的最优结点链；
根据从0时刻至预瞄最后时刻的最优结点链内的离散结点进行扩展，得到各时刻最优行驶区域；
对各时刻最优行驶区域进行考量横纵向的乘坐舒适性的二次规划，得出车辆最终平滑轨迹曲线。
2.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：根据车辆运动学模型获取自车的初始状态信息，再根据自车的初始状态信息和交通规则相关信息计算横向、纵向前向可达集，包括：
步骤1.1：根据自车的初始状态信息，获取车辆在X-Y坐标下的纵向坐标x，纵向速度vx，横向坐标y，横向速度vy，纵向最大速度，横向最大速度；
步骤1.2：提取t0时刻的纵向的前向可达集边界点，下一时刻t1边界点为(x1,v1)，x1为车辆t1时刻的纵向坐标，v1为车辆t1时刻的纵向速度；
步骤1.3：分别计算每个t1时刻纵向的前向可达集边界点产生的凸集边界点；
步骤1.4：计算所有凸集边界点的Minkowski集合，根据Minkowski集合得到t1时刻的理想纵向的前向可达集边界点；
步骤1.5：根据交通规则相关信息获取道路边界限制，道路车道线限制，根据纵向最大速度、道路边界限制和道路车道线限制对理想纵向的前向可达集边界点进行饱和处理，得到t1时刻纵向的前向可达集；
步骤1.6：将t1时刻纵向的前向可达集边界点代入步骤1.2到步骤1.5进行迭代，直到计算至规划预瞄时刻tT，得到从t0到tT时刻的纵向的前向可达集；
步骤1.7：提取t0时刻的横向的前向可达集边界点，下一时刻t1边界点为(y1,vy1)，y1为车辆t1时刻的横向坐标，vy1为车辆t1时刻的横向速度；
步骤1.8：分别计算每个t1时刻横向的前向可达集边界点产生的凸集边界点；
步骤1.9：计算所有凸集边界点的Minkowski集合，根据Minkowski集合得到t1时刻的理想横向的前向可达集边界点；
步骤1.10：根据交通规则相关信息获取道路边界限制，道路车道线限制，根据横向最大速度、道路边界限制和道路车道线限制对理想横向的前向可达集边界点进行饱和处理，得到t1时刻横向的前向可达集；
步骤1.11：将t1时刻横向的前向可达集边界点代入步骤1.7到步骤1.10进行迭代，直到计算至规划预瞄时刻tT，得到从t0到tT时刻的横向的前向可达集。
3.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：将车辆各时刻可达区域离散化，得到车辆各时刻可达点集，包括：
将车辆各时刻可达区域按横向离散结点间距、纵向离散结点间距进行离散化，得到车辆各时刻可达点集。
4.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：障碍物的加速度、速度、初始位置的误差分别服从均值为0、方差为
Figure FDA0003661585250000021
的二维高斯分布；障碍物每时刻间位置的误差不确定度增量服从纵横向方差为Var(X′)、Var(Y′)的二维高斯分布；障碍物的航向角服从协方差为Rt的二维高斯分布。
5.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：根据位置分布函数计算出各时刻下的风险分布，并对风险分布进行风险表征，包括：
以每时刻可达点集中各个离散结点为质心、车辆外轮廓为边界在二维高斯分布函数中求解二重积分得出风险表征。
6.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：根据移除后的可达点集和车辆运动学模型，计算相邻时间间隔间可达点集中离散结点的父子关系，根据离散结点的父子关系生成所有时刻内的整体结点树，包括：
步骤2.1：由t0时刻的移除后的可达点集中得出t0时刻下离散结点n的纵、横向速度的可达范围；
步骤2.2：再根据运动学模型计算得到离散结点n在t1时刻可到达的X-Y区域边界，则在离散结点n的X-Y区域边界内的所有t1时刻下的离散结点均为结点n的子结点；
步骤2.3：重复步骤2.1-2.2直到计算至规划预瞄时刻tT，获得所有时刻每个离散结点的子结点，根据各子结点形成整体结点树。
7.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：根据行驶效率、安全性、交通规则评价所有时刻内的整体结点树的离散结点的成本，求解出从0时刻至预瞄最后时刻的最优结点链；
根据行驶效率、安全性、交通规则评价所有时刻内的整体结点树的离散结点的成本，所述行驶效率表示离散结点的纵向位置和当前时刻可达集纵向位置下边界值，安全性表示离散结点的风险表征值，交通规则表示离散结点横向距车道中心距离；
根据整体结点树的离散结点的成本和离散结点所在层数，提取符合本层离散结点成本最小和本层离散结点对应的子结点成本最小的本层离散结点作为本层最优结点，直到遍历完所有层的离散结点，得到所有层最优结点作为最优结点链。
8.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：对各时刻最优行驶区域进行考量横纵向的乘坐舒适性的二次规划，得出车辆最终平滑轨迹曲线，包括：
获取各时刻最优行驶区域的纵向范围[xmin；xmax]，最优结点链的纵向坐标向量Xx；
根据纵向成本函数求解出平滑轨迹点纵向向量x＝[xt0；xt1...；xtT]；所述纵向成本函数f(x)表示为下式：
f(x)＝C1Ax 2+C2Jx 2+C3(Xx-x)2
x≥xmin，x≤xmax
Figure FDA0003661585250000041
Figure FDA0003661585250000042
式中，C1、C2、C3为权重系数；xmin、xmax为x优化的上、下边界；Axk为单一点纵向加速度成本；Ax为总体加速度成本，由t0-tT时刻Axk累加得到；Jk为单一点纵向冲击度成本；Jx为总体冲击度成本，由t0-tT时刻Jx累加得到；xk、xk+1、xk-1、xk-2、xk-3分别为第k、k+1、k-1、k-2、k-3时刻纵向坐标最优值，t为时间间隔；
获取各时刻最优行驶区域的横向范围[ymin；ymax]，最优结点链的横向坐标向量Yy；
根据横向成本函数求解出平滑轨迹点横向向量y＝[yt0；yt1...；ytT]；所述横向成本函数f(y)表示为下式：
f(y)＝C4Ay 2+C5Cky 2+C6(Yy-y)2
y≥ymin，y≤ymax
Figure FDA0003661585250000043
Figure FDA0003661585250000044
式中，C4、C5、C6为权重系数，ymin、ymax为y优化的上、下边界；Ayk为单一点横向加速度成本；Ay为总体加速度成本，由t0-tT时刻Ayk累加得到；Ck为单一点曲率成本；Cky为总体曲率成本，由t0-tT时刻Ck累加得到；yk、yk+1、yk-1、yk-2分别为第k、k+1、k-1、k-2时刻横向坐标最优值，t为时间间隔；
根据平滑轨迹点纵向向量x、平滑轨迹点横向向量y获得t0到tT时刻的车辆最终平滑轨迹曲线。
9.根据权利要求1所述的一种基于时空可达集理论的自动驾驶汽车轨迹规划方法，其特征在于：阈值设置为3％。
CN202210603003.9A 2022-05-25 2022-05-25 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法 Pending CN114995415A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210603003.9A CN114995415A (zh) 2022-05-25 2022-05-25 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210603003.9A CN114995415A (zh) 2022-05-25 2022-05-25 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法

Publications (1)

Publication Number Publication Date
CN114995415A true CN114995415A (zh) 2022-09-02

Family

ID=83031520

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210603003.9A Pending CN114995415A (zh) 2022-05-25 2022-05-25 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法

Country Status (1)

Country Link
CN (1) CN114995415A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115617078A (zh) * 2022-12-12 2023-01-17 北京理工大学 基于膨化障碍的无人机三维航迹快速规划方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111123701A (zh) * 2019-11-27 2020-05-08 武汉理工大学 基于管道预测模型的自动驾驶路径跟踪抗干扰控制方法
CN111845774A (zh) * 2020-07-20 2020-10-30 上海大学 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法
WO2021073079A1 (zh) * 2019-10-17 2021-04-22 南京航空航天大学 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法
CN113619603A (zh) * 2021-08-25 2021-11-09 华中科技大学 一种双阶段自动驾驶车辆调头轨迹规划方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021073079A1 (zh) * 2019-10-17 2021-04-22 南京航空航天大学 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法
CN111123701A (zh) * 2019-11-27 2020-05-08 武汉理工大学 基于管道预测模型的自动驾驶路径跟踪抗干扰控制方法
CN111845774A (zh) * 2020-07-20 2020-10-30 上海大学 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法
CN113619603A (zh) * 2021-08-25 2021-11-09 华中科技大学 一种双阶段自动驾驶车辆调头轨迹规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
周兴珍，裴晓飞，张鑫康: "基于可达集优化的智能汽车轨迹规划研究", 武汉理工大学学报, vol. 44, no. 6, 30 June 2022 (2022-06-30) *
周维;过学迅;裴晓飞;张震;余嘉星;: "基于RRT与MPC的智能车辆路径规划与跟踪控制研究", 汽车工程, no. 09, 25 September 2020 (2020-09-25) *
杨旭;沈鹏;奉柳;刘秉政;曹凯;: "可达集约束下的自主车辆路径规划势场蚁群算法", 科技通报, no. 04, 30 April 2020 (2020-04-30) *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115617078A (zh) * 2022-12-12 2023-01-17 北京理工大学 基于膨化障碍的无人机三维航迹快速规划方法
CN115617078B (zh) * 2022-12-12 2023-07-07 北京理工大学 基于膨化障碍的无人机三维航迹快速规划方法

Similar Documents

Publication Publication Date Title
CN110834644B (zh) 一种车辆控制方法、装置、待控制车辆及存储介质
CN109960261B (zh) 一种基于碰撞检测的动态障碍物避让方法
US20200149906A1 (en) Path planning method, system and device for autonomous driving
Fulgenzi et al. Dynamic obstacle avoidance in uncertain environment combining PVOs and occupancy grid
CN112965476B (zh) 一种基于多窗口模型的高速无人车轨迹规划系统及方法
JP2021526478A (ja) 車両の制御システム、車両を制御する方法、及び非一時的コンピュータ可読メモリ
CN112810630B (zh) 一种自动驾驶车辆轨迹规划方法及系统
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN106371439B (zh) 一种统一的自动驾驶横向规划方法与系统
CN111397622B (zh) 基于改进A*算法与Morphin算法的智能汽车局部路径规划方法
CN112068588A (zh) 一种基于飞行走廊和贝塞尔曲线的无人飞行器轨迹生成方法
CN103823466A (zh) 一种动态环境下移动机器人路径规划方法
CN112269384B (zh) 一种结合障碍物行为意图的车辆动态轨迹规划方法
CN112092810B (zh) 一种车辆泊出方法、装置及电子设备
CN113239986A (zh) 车辆轨迹评价网络模型的训练方法、装置和存储介质
CN114995415A (zh) 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法
CN115077553A (zh) 基于栅格搜索轨迹规划方法、系统、汽车、设备及介质
Liu et al. Impact of sharing driving attitude information: A quantitative study on lane changing
CN114647246A (zh) 一种时间空间耦合搜索的局部路径规划方法及系统
CN114237256B (zh) 一种适用于欠驱动机器人的三维路径规划与导航方法
Tang et al. Integrated decision making and planning framework for autonomous vehicle considering uncertain prediction of surrounding vehicles
Wu et al. Trajectory prediction based on planning method considering collision risk
Gu et al. Safe-state enhancement method for autonomous driving via direct hierarchical reinforcement learning
CN114879665A (zh) 用于车辆远程召唤系统的安全节能运动规划方法及系统
CN115140048A (zh) 一种自动驾驶行为决策与轨迹规划模型与方法

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