CN114715192A - 一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统 - Google Patents

一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统 Download PDF

Info

Publication number
CN114715192A
CN114715192A CN202210396754.8A CN202210396754A CN114715192A CN 114715192 A CN114715192 A CN 114715192A CN 202210396754 A CN202210396754 A CN 202210396754A CN 114715192 A CN114715192 A CN 114715192A
Authority
CN
China
Prior art keywords
track
longitudinal
displacement
transverse
longitudinal displacement
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
CN202210396754.8A
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.)
Chongqing Chang'an Wangjiang Industry Group Co ltd
Chongqing University
University of Shanghai for Science and Technology
Original Assignee
Chongqing Chang'an Wangjiang Industry Group Co ltd
Chongqing University
University of Shanghai for Science and Technology
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 Chongqing Chang'an Wangjiang Industry Group Co ltd, Chongqing University, University of Shanghai for Science and Technology filed Critical Chongqing Chang'an Wangjiang Industry Group Co ltd
Priority to CN202210396754.8A priority Critical patent/CN114715192A/zh
Publication of CN114715192A publication Critical patent/CN114715192A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
    • B60W30/095—Predicting travel path or likelihood of collision
    • B60W30/0953—Predicting travel path or likelihood of collision the prediction being responsive to vehicle dynamic parameters
    • 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
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
    • B60W30/095—Predicting travel path or likelihood of collision
    • B60W30/0956—Predicting travel path or likelihood of collision the prediction being responsive to traffic or environmental parameters
    • 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
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • 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
    • B60W60/0015—Planning or execution of driving tasks specially adapted for safety
    • 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

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统，属于自动驾驶领域，通过将一个三维空间的轨迹规划问题解耦为两个二维的轨迹规划问题，并在每个二维问题下利用单元格分解法与动态规划算法得到粗略路径与梯形走廊，同时利用二次规划算法在梯形走廊内对每个二维问题进行优化求解，最后通过轨迹合并得到可行轨迹。解决了自动驾驶车辆在轨迹规划时三维问题难以求解、使用采样方法得到的轨迹并非最优且不适用于复杂城市道路场景、以及离散规划空间会出现小障碍物无法处理等问题，能够实时规划出无碰撞、可执行、高效的轨迹。

Description

一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统
技术领域
本发明涉及自动驾驶领域，特别是涉及一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统。
背景技术
自动驾驶车辆轨迹规划的任务主要是利用地图数据、感知数据、定位数据以及车辆状态等信息，计算出一个无碰撞可执行的轨迹，保证车辆从起点安全地驾驶到目的地，并尽可能高效。其问题的本质是一个多目标的数学优化问题，其主要的优化目标主要包括：安全性、稳定性、舒适性、驾驶效率，然而在实际的驾驶场景中，还应包括车辆的加减速度约束、非完整性约束以及动力学约束等各种物理约束。
若不考虑自动驾驶车辆在z方向的移动，轨迹规划问题是一个三维的问题，即规划模块输出的轨迹应该是位置关于时间的函数。然而三维的优化问题往往很难直接进行求解，故目前主流的做法是将三维的轨迹规划问题利用frenet坐标系解耦为两个二维规划问题，即横向规划问题与纵向规划问题。
现有的《一种面向多场景的自动驾驶规划方法及系统》，其分别在横向规划中通过采样不同的横向位移与纵向位移的组合，在纵向规划中通过采样不同的时间与纵向轨迹的组合，并使用多项式进行连接，然后使用特定的代价函数挑选出候选轨迹中代价最小的轨迹进行合并，若无最优轨迹则重新生成参考路线。但该方法存在即使顺利生成了轨迹也无法保证轨迹的最优性，因为其采用采样的方法，故原本存在可行的轨迹，但因其并未被采样到，从而产生规划失败的可能。除此之外，该方法并不能保证采样轨迹之间是否存在障碍物的情况，因其只能检测被采样轨迹的安全性，故若采样轨迹之间存在不确定性的微小障碍物，其无法处理。再者，其规划失败后，即未能找到一条安全的轨迹，其采样重新生成参考线的方法进行处理。然而实际场景中，规划失败后，应调用备用轨迹使车辆停车再执行后续相关指令。
现有技术无论是在笛卡尔坐标系还是在Frenet下进行规划，大多均采用采样的方法获得轨迹。然而，通过采样得到的轨迹均是次优的，无法满足最优性，且可能存在原本存在可行的轨迹，但因其并未被采样到，从而产生规划失败的可能。除此之外，采样的方法并不能保证采样轨迹之间是否存在障碍物的情况，因其只能检测被采样轨迹的安全性，故若采样轨迹之间存在不确定性的微小障碍物，其无法处理。再者，采样方法只能适用于较简单的场景，如高速公路或简单的城市道路，在复杂的场景中很难采样得到可行的轨迹。
可见，轨迹规划是一个连续问题，无论是通过采样还是离散纵向位移得到的轨迹，其无论是在横向规划还是纵向规划上得到的轨迹均是次优的，无法达到最优，并且其并不能够保证采样轨迹之间或者离散纵向位移点之间是否安全，故其可能会得到一条不安全的轨迹。
发明内容
本发明的目的是提供一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统，以实时规划出无碰撞、可执行、高效的轨迹。
为实现上述目的，本发明提供了如下方案：
一种解耦的自动驾驶车辆实时轨迹规划方法，所述规划方法包括：
实时获取自车的位姿信息和自车周围的障碍物；
在自车的全局路径上截取包含自车当前位姿的一段路径作为参考线，并将参考线从笛卡尔坐标系转换到Frenet坐标系下，获得纵向位移-横向位移图；
将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中；
利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线；
在每一条分解线上布设多个采样点，并从起点开始依次连接分解线上的采样点，形成多条备选路径；所述备选路径由多条依次首尾连接的直线段构成，所述直线段为起点与相邻分解线上采样点直线连接或相邻分解线上采样点直线连接形成的线段；
采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径；
将粗略路径上的每条直线段所在四边形的边框构成梯形走廊，并采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，获得横向优化轨迹；
根据所述横向优化轨迹，构建纵向位移-时间图；
将运动速度大于或等于速度阈值的障碍物投影到纵向位移-时间图中，并将纵向位移-时间图替换纵向位移-横向位移图，返回步骤“利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线”，获得纵向优化轨迹；
合并横向优化轨迹与纵向优化轨迹，并对合并后的优化轨迹进行安全检测，将通过安全检测的合并后的优化轨迹确定为最优可行轨迹。
可选的，所述将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中，具体包括：
当运动速度小于速度阈值的障碍物为静态障碍物时，将静态障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影静态障碍物；
当运动速度小于速度阈值的障碍物为低速障碍物时，将低速障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影低速障碍物；所述低速障碍物与自车的行驶方向相同，所述低速障碍物的运动速度大于零且小于速度阈值；
当运动速度小于速度阈值的障碍物为来向障碍物时，将来向障碍物的预测轨迹与自车的当前轨迹发生碰撞位置处的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影来向障碍物。
可选的，所述利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线，具体包括：
在每个障碍物的每个顶点建立一条垂直线；
选取垂直线与纵向位移-横向位移图边界框的交点以及垂直线与障碍物边界框的交点作为备选交点；
沿垂直线在每个顶点的两侧分别选出与每个顶点距离最近的备选交点；
分别连接每个顶点与各自选出的两个备选交点，获得每个顶点的两个连接线段；
删除与障碍物相交的连接线段，获得每个障碍物的每个顶点的一条分解线。
可选的，所述采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径，具体包括：
沿纵向位移-横向位移图的s轴正方向将分解线从第1层开始依次编号；
初始化层数u＝1；
利用公式Ctotal(Lv)＝Csmooth(Lv)+Cobs(Lv)+Cguidance(Lv)，计算纵向位移-横向位移图中起点与第u层之间每条直线段的代价值；其中，Ctotal(Lv)为起点与第u层之间第v条直线段的代价值，Csmooth(Lv)为起点与第u层之间第v条直线段的平滑性代价，Cobs(Lv)为起点与第u层之间第v条直线段与障碍物的距离代价，Cguidance(Lv)为起点与第u层之间第v条直线段与参考线的代价；Csmooth(Lv)＝w1*L′v 2+w2*L″v 2+w3*L″′v 2，w1、w2与w3分别为一阶导数、二阶导数与三阶导数的权重，L′v、L″v与L″′v分别为第v条直线段的一阶导数、二阶导数与三阶导数，
Figure BDA0003597566040000041
Figure BDA0003597566040000042
p为第v条直线段离散的个数，lj为第j个离散点的横向位移，ds为离散的纵向间隔；
Figure BDA0003597566040000043
Figure BDA0003597566040000044
与
Figure BDA0003597566040000045
分别为第v条直线段的右端点所在分解线的长边与短边的长度；Cguidance(Lv)＝Lv-Gv(s)，Gv(s)为起点与第u层之间第v条直线段处的参考线在s处的横向位移，Lv为起点与第u层之间第v条直线段的的横向位移，
Figure BDA0003597566040000046
将代价值最小的直线段作为起点与第u层之间的最优直线段；
以最优直线段在第u层上的交点为起点，令u的数值增加1，并返回步骤“利用公式Ctotal(Lv)＝Csmooth(Lv)+Cobs(Lv)+Cguidance(Lv)，计算起点与第u层之间每条直线段的代价值”，直至u的数值大于最大层数，停止循环，获得纵向位移-横向位移图中的粗略路径。
可选的，采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-时间图中的粗略路径，具体包括：
沿纵向位移-时间图的t轴正方向将分解线从第1层开始依次编号；
初始化层数u＝1；
利用公式Ctotal(Sv)＝w1*(S′v-Vref)+w2*S″v 2+w3*S″′v 2+w4*Cobs(s)，计算纵向位移-时间图中起点与第u层之间每条直线段的代价值；其中，Ctotal(Sv)为纵向位移-时间图中起点与第u层之间第v条直线段的代价值，w4为与障碍物距离的代价，Vref为当前道路段的参考速度，Cobs(s)为纵向位移-时间图中起点与第u层之间第v条直线段与障碍物的距离代价，
Figure BDA0003597566040000051
Figure BDA0003597566040000052
与
Figure BDA0003597566040000053
分别为纵向位移-时间图中第v条直线段的右端点所在分解线的长边与短边的长度，S′v、S″v与S″′v分别为第v条直线段的一阶导数、二阶导数与三阶导数，
Figure BDA0003597566040000054
sj为第j个离散点的纵向位移，dt为离散的纵向间隔；
将代价值最小的直线段作为起点与第u层之间的最优直线段；
以最优直线段在第u层上的交点为起点，令u的数值增加1，并返回步骤“利用公式Ctotal(Sv)＝w1*(S′v-Vref)+w2*S″v 2+w3*S″′v 2+w4*Cobs(s)，计算纵向位移-时间图中起点与第u层之间每条直线段的代价值”，直至u的数值大于最大层数，停止循环，获得纵向位移-时间图中的粗略路径。
可选的，所述采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，具体包括：
当采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行横向轨迹优化时：
构建横向轨迹优化的代价函数为
Figure BDA0003597566040000061
其中，Jpath代表轨迹优化的代价，轨迹优化为横向轨迹优化或纵向轨迹优化；k为分段贝塞尔曲线的编号，同时对应梯形走廊中梯形的编号；s为纵向位移，l为横向位移，m为分段贝塞尔曲线总的段数，Sk为第k个梯形的宽度，lk(s)为第k段贝塞尔曲线，wl为横向位移的权重，wl′为横向位移关于纵向位移的一阶导数的权重，wl″为横向位移关于纵向位移的二阶导数的权重，wl″′为横向位移关于纵向位移的三阶导数的权重，wref为横向位移与参考误差的权重，
Figure BDA0003597566040000062
为粗略路径在第k个梯形中的直线段；
Figure BDA0003597566040000063
S1、S2和Sm分别为第1、2和m个梯形的宽度，Tm-1为第m个梯形的左边界，Tm为第m个梯形的右边界，
Figure BDA0003597566040000064
分别表示第1段、第2段、第m段贝塞尔曲线的第i个控制点，
Figure BDA0003597566040000065
表示第i个贝塞尔曲线控制点的系数，其形式为
Figure BDA0003597566040000066
其中
Figure BDA0003597566040000067
t表示自变量，t在路径规划周期中为纵向位移s，t在速度规划周期中为时间，n表示控制点的个数；
构建横向轨迹优化的约束条件为：
Figure BDA0003597566040000068
Figure BDA0003597566040000069
Figure BDA00035975660400000610
Figure BDA0003597566040000071
Figure BDA0003597566040000072
Figure BDA0003597566040000073
其中，
Figure BDA0003597566040000074
为第k段贝塞尔曲线的第r阶导数的第i个控制点，
Figure BDA0003597566040000075
为第k段贝塞尔曲线的第r阶导数的第n个控制点，
Figure BDA0003597566040000076
为第k+1段贝塞尔曲线的第r阶导数的第0个控制点，Sk为第k个梯形的宽度，
Figure BDA0003597566040000077
分别为Sk、Sk+1的(1-r)次幂，
Figure BDA0003597566040000078
表示在s＝sk时，lk(s)对s的r阶导数值，
Figure BDA0003597566040000079
为第k个梯形的下边界函数，
Figure BDA00035975660400000710
为第k个梯形的上边界界函数，l′min与l″max分别为横向位移关于纵向位移一阶导数的最小值与最大值；l″min与l″max分别为横向位移关于纵向位移二阶导数的最小值与最大值；l″min与l″max分别为横向位移关于纵向位移三阶导数的最小值与最大值；
当采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行纵向轨迹优化时：
构建纵向轨迹优化的代价函数为
Figure BDA00035975660400000711
其中，Jspeed为纵向轨迹优化的代价值，s为纵向位移，t为横向位移，同时也为梯形走廊中总的梯形的个数，Tk为第k个梯形的宽度，sk(t)为第k段贝塞尔曲线，
Figure BDA00035975660400000712
为纵向位移-时间图中粗略路径在第k个梯形中的直线段，ws为纵向位移接近参考纵向位移的权重，ws为纵向速度接近参考速度的权重，
Figure BDA00035975660400000717
为纵向加速度的权重，
Figure BDA00035975660400000718
为纵向加加速度的权重；
构建纵向轨迹优化的约束条件为：
Figure BDA00035975660400000713
Figure BDA00035975660400000714
Figure BDA00035975660400000715
Figure BDA0003597566040000081
Figure BDA0003597566040000082
Figure BDA0003597566040000083
Figure BDA0003597566040000084
其中，
Figure BDA0003597566040000085
为第k段贝塞尔曲线的第r阶导数的第i个控制点，Tk为第k个梯形的宽度，
Figure BDA0003597566040000086
为第k个梯形的下边界函数，
Figure BDA0003597566040000087
为第k个梯形的上边界界函数，vmin与vmax分别为纵向速度的最小值与最大值；amin与amax分别为纵向加速度的最小值与最大值；jmin与jmax分别为纵向加加速度的最小值与最大值；acm为最大的离心加速度；κk为第k段贝塞尔曲线的最大曲率；
将横向轨迹优化的代价函数和约束条件、纵向轨迹优化的代价函数和约束条件分别转化为一个二阶规划表达式：
Figure BDA0003597566040000088
s.t.l≤Acx≤u
其中，x为轨迹优化中的l(s)或sk(t)，P为轨迹优化中代价函数的二次项，qT为轨迹优化中代价函数的线性项，l与u分别为轨迹优化中约束条件的下边界与上边界，等式约束的下边界与上边界均为0，Ac为轨迹优化中约束条件形成的约束矩阵；
求解二阶规划表达式，获得横向优化轨迹或纵向优化轨迹。
可选的，所述合并横向优化轨迹与纵向优化轨迹，具体包括：
将纵向位移-时间图上的时间段均匀划分，获得一系列采样时间点；
从纵向位移-时间图中的纵向优化轨迹上得到采样时间点ti处的纵向位移si、纵向速度
Figure BDA0003597566040000089
以及纵向加速度
Figure BDA00035975660400000810
从纵向位移-横向位移图中的横向优化轨迹上得到纵向位移si处的横向位移li、横向速度l′i以及横向加速度l″i；
确定frenet坐标系下的一个轨迹点
Figure BDA00035975660400000811
将确定的轨迹点转换到笛卡尔坐标系下，获得笛卡尔坐标系下的一个轨迹点(ti，xi，yi，si，θi，κi，vi，ai)；其中，xi与yi为笛卡尔坐标系下的位姿，θi为笛卡尔坐标下的朝向，κi为曲率，vi与ai分别为速度与加速度；
重复以上步骤，获得所有采样时间点在笛卡尔坐标系下的轨迹点，依次连接所有轨迹点形成合并后的优化轨迹。
可选的，所述对合并后的优化轨迹进行安全检测，具体包括：
判断合并后的优化轨迹上每个轨迹点是否满足安全条件，获得第一判断结果；所述安全条件为速度小于或等于速度阈值，加速度小于或等于加速度阈值，加加速度小于或等于加加速度阈值且曲率小于或等于曲率阈值；
若所述第一判断结果表示是，则判断每个轨迹点是否与障碍物发生碰撞，获得第二判断结果；
若所述第二判断结果表示否，则判定合并后的优化轨迹通过安全检测；
若所述第一判断结果表示否或所述第二判断结果表示是，则判定合并后的优化轨迹没有通过安全检测，并将合并后的优化轨迹确定为备用轨迹。
一种解耦的自动驾驶车辆实时规划装置，所述规划装置包括：路由模块、定位模块、感知模块、规划模块和控制模块；
路由模块、定位模块、感知模块均与规划模块的输入端连接，控制模块与规划模块的输出端连接；
所述路由模块用于向规划模块发送自车的全局路径；
所述定位模块用于向规划模块发送自车的当前位姿；
所述感知模块用于向规划模块发送自车周围的障碍物信息；
所述规划模块用于根据自车的全局路径、当前位姿和自车周围的障碍物信息，采用前述的解耦的自动驾驶车辆实时轨迹规划方法，获得最优可行轨迹，并将所述最优可行轨迹发送至控制模块；
所述控制模块用于控制自车执行所述最优可行轨迹。
一种解耦的自动驾驶车辆实时规划系统，所述规划系统包括：
实时信息获取模块，用于实时获取自车的位姿信息和自车周围的障碍物；
纵向位移-横向位移图获得模块，用于在自车的全局路径上截取包含自车当前位姿的一段路径作为参考线，并将参考线从笛卡尔坐标系转换到Frenet坐标系下，获得纵向位移-横向位移图；
投影模块，用于将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中；
分解模块，用于利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线；
备选路径形成模块，用于在每一条分解线上布设多个采样点，并从起点开始依次连接分解线上的采样点，形成多条备选路径；所述备选路径由多条依次首尾连接的直线段构成，所述直线段为起点与相邻分解线上采样点直线连接或相邻分解线上采样点直线连接形成的线段；
粗略路径确定模块，用于采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径；
横向优化轨迹获得模块，用于将粗略路径上的每条直线段所在四边形的边框构成梯形走廊，并采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，获得横向优化轨迹；
纵向位移-时间图构建模块，用于根据所述横向优化轨迹，构建纵向位移-时间图；
纵向优化轨迹获得模块，用于将运动速度大于或等于速度阈值的障碍物投影到纵向位移-时间图中，并将纵向位移-时间图替换纵向位移-横向位移图，返回步骤“利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线”，获得纵向优化轨迹；
最优可行轨迹确定模块，用于合并横向优化轨迹与纵向优化轨迹，并对合并后的优化轨迹进行安全检测，将通过安全检测的合并后的优化轨迹确定为最优可行轨迹。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明公开一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统，通过将一个三维空间的轨迹规划问题解耦为两个二维的轨迹规划问题，并在每个二维问题下利用单元格分解法与动态规划算法得到粗略路径与梯形走廊，同时利用二次规划算法在梯形走廊内对每个二维问题进行优化求解，最后通过轨迹合并得到可行轨迹。解决了自动驾驶车辆在轨迹规划时三维问题难以求解、使用采样方法得到的轨迹并非最优且不适用于复杂城市道路场景、以及离散规划空间会出现小障碍物无法处理等问题，能够实时规划出无碰撞、可执行、高效的轨迹。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例一提供的一种解耦的自动驾驶车辆实时轨迹规划方法的原理图；
图2为本发明实施例一提供的笛卡尔坐标系与Frenet坐标系的转换关系示意图；
图3为本发明实施例一提供的纵向位移-横向位移图构建原理示意图；
图4为本发明实施例一提供的参考线平滑示意图；
图5为本发明实施例一提供的单元格分解原理图；图5(a)为交点形成示意图，图5(b)为分解线形成示意图；
图6为本发明实施例一提供的梯形走廊示意图；
图7为本发明实施例一提供的纵向位移-横向位移图中备选路径示意图；
图8为本发明实施例一提供的纵向位移-横向位移图中粗略路径示意图；
图9为本发明实施例一提供的纵向位移-时间图中备选路径示意图；
图10为本发明实施例一提供的纵向位移-时间图中粗略路径示意图；
图11为本发明实施例二提供的解耦的自动驾驶车辆实时规划装置的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统，以实时规划出无碰撞、可执行、高效的轨迹。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例一
本发明提供的一种解耦的自动驾驶车辆实时轨迹规划方法，如图1所示，规划方法包括：
步骤1，实时获取自车的位姿信息和自车周围的障碍物。
除了获取自车的位姿信息外，还可以获取自车的几何形状、速度、加速度、底盘等信息，以及障碍物的位姿、几何形状、类型(如行人、车辆等)、速度、预测轨迹等信息。
步骤2，在自车的全局路径上截取包含自车当前位姿的一段路径作为参考线，并将参考线从笛卡尔坐标系转换到Frenet坐标系下，获得纵向位移-横向位移图。
基于路由模块提供的全局路径与S1中获取的自车信息，在全局路径上截取一段路径作为每次规划的参考线，并对所截取出来的参考线进行路径平滑处理；所述的参考线是通过在全局路径上找到距离自车的当前位姿中最近的路径点，然后截取自车后面一定距离到自车前面一定距离的全局路径，自车后面的距离与自车前面的距离均可根据实际的场景进行调整；所述的参考线平滑方法不限于某一种特定的方法，但该方法应能保证平滑后的参考线本身足够的平滑、长度尽可能短、离平滑前的参考线尽可能近，曲率尽可能连续。平滑示意图如图4所示。
将所得到的平滑后的参考线从笛卡尔坐标系转换到Frenet坐标系下从而构建的s-l图(纵向位移-横向位移图)，如图2-3所示。Frenet坐标系的s轴为沿着(平滑过后的)参考线的方向，l轴(图中的d轴)为(平滑过后的)参考线的法向。
步骤3，将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中。
运动速度小于速度阈值的障碍物主要指静态障碍物、低速的动态障碍物以及对向驶来的障碍物。
示例性的，障碍物投影具体包括：
当运动速度小于速度阈值的障碍物为静态障碍物时，将静态障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影静态障碍物。当运动速度小于速度阈值的障碍物为低速障碍物时，将低速障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影低速障碍物；低速障碍物与自车的行驶方向相同，低速障碍物的运动速度大于零且小于速度阈值。当运动速度小于速度阈值的障碍物为来向障碍物时，将来向障碍物的预测轨迹与自车的当前轨迹发生碰撞位置处的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影来向障碍物。
根据获取的自车的相关信息，将每个路径规划周期的起点投影到s-l图中，每个路径规划周期的起点可根据实际的场景进行确定，但起点不应距离此时自车的位姿在s-l图中的投影太远。
步骤4，利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线。
作为一种示例方式，参照图5和图6，单元格分解法具体过程为：
在每个障碍物的每个顶点建立一条垂直线；选取垂直线与纵向位移-横向位移图边界框的交点以及垂直线与障碍物边界框的交点作为备选交点；沿垂直线在每个顶点的两侧分别选出与每个顶点距离最近的备选交点；分别连接每个顶点与各自选出的两个备选交点，获得每个顶点的两个连接线段；删除与障碍物相交的连接线段，获得每个障碍物的每个顶点的一条分解线。
即对障碍物进行分解的单元格分解法是通过首先在每个障碍物的每个顶点建立一条垂直线，然后选取垂直线与s-l图(s-t图)的交点或者垂直线与障碍物的交点中距离当前障碍物顶点最近且与当前障碍物顶点的连线不与当前障碍物相交的点，然后使用最近的交点与当前障碍物顶点建立一条分解线。使用该方法，从左至右地对每个障碍物的每个顶点分别建立每个顶点的分解线。
步骤5，在每一条分解线上布设多个采样点，并从起点开始依次连接分解线上的采样点，形成多条备选路径；备选路径由多条依次首尾连接的直线段构成，直线段为起点与相邻分解线上采样点直线连接或相邻分解线上采样点直线连接形成的线段。
在每一条障碍物分解线上以一定的间隔进行撒点(若某一条分解线的长度小于横向边界缓冲值，则不对该条分解线进行撒点)，撒点的间隔与横向缓冲值均可根据实际情况进行改变；然后将路径规划周期的起点与离起点最近的分解线上的采样点用直线进行连接，并且将每两条相邻的分解线之间的采样点也是用直线进行连接，如图7所示。
步骤6，采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径。
利用动态规划算法对每一条直线段进行代价评估，最后取代价值最小的一条作为s-l图中的粗略路径，如图8所示。动态规划是先得到规划起点与第一层(距离起点最近的分解线)的点(规划起点为第0层)的最优路径(代价最小的路径)，然后仅使用第一层中的最优路径的终点与第二层的点形成一系列候选路径，然后找到第二次中的最优路径；以此类推，直到得到最后一层的最优路径。
示例性的，采用动态规划算法计算粗略路径的具体步骤为：
S6-1，沿纵向位移-横向位移图的s轴正方向将分解线从第1层开始依次编号；
S6-2，初始化层数u＝1；
S6-3，利用每一条直线段的代价评估函数，计算起点与第u层之间每条直线段的代价值；
S6-4，将代价值最小的直线段作为起点与第u层之间的最优直线段；
S6-5，以最优直线段在第u层上的交点为起点，令u的数值增加1，并返回步骤“利用公式Ctotal(Lv)＝Csmooth(Lv)+Cobs(Lv)+Cguidance(Lv)，计算起点与第u层之间每条直线段的代价值”，直至u的数值大于最大层数，停止循环，获得纵向位移-横向位移图中的粗略路径。
其中，所述的每一条直线段的代价评估函数表示成式：
Ctotal(Lv)＝Csmooth(Lv)+Cobs(Lv)+Cguidance(Lv)
Ctotal(Lv)为起点与第u层之间第v条直线段的代价值。
Csmooth(Lv)为起点与第u层之间第v条直线段的平滑性代价，Csmooth(Lv)＝w1*L′v 2+w2*L″v 2+w3*L″′v 2，w1、w2与w3分别为一阶导数、二阶导数与三阶导数的权重，L′v、L″v与L″′v分别为第v条直线段的一阶导数、二阶导数与三阶导数；
Figure BDA0003597566040000151
Figure BDA0003597566040000152
Figure BDA0003597566040000153
p为第v条直线段离散的个数，lj为第j个离散点的横向位移，ds为离散的纵向间隔；
Cobs(Lv)为起点与第u层之间第v条直线段与障碍物的距离代价，
Figure BDA0003597566040000154
Figure BDA0003597566040000155
与
Figure BDA0003597566040000156
分别为第v条直线段的右端点所在分解线的长边与短边的长度；
Cguidance(Lv)为起点与第u层之间第v条直线段与参考线的代价；Cguidance(Lv)＝Lv-Gv(s)，Gv(s)为起点与第u层之间第v条直线段处的参考线在s处的横向位移，Lv为起点与第u层之间第v条直线段的的横向位移，
Figure BDA0003597566040000157
步骤7，将粗略路径上的每条直线段所在四边形的边框构成梯形走廊，并采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，获得横向优化轨迹，如图7所示。
梯形走廊如图6所示。
示例性的，采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，具体包括：
构建轨迹优化的代价函数为
Figure BDA0003597566040000161
其中，Jpath代表轨迹优化的代价，轨迹优化为横向轨迹优化或纵向轨迹优化；k为分段贝塞尔曲线的编号，同时对应梯形走廊中梯形的编号；s为纵向位移，l为横向位移，m为分段贝塞尔曲线总的段数，Sk为第k个梯形的宽度，lk(s)为第k段贝塞尔曲线，wl为横向位移的权重，wl′为横向位移关于纵向位移的一阶导数的权重，wl″为横向位移关于纵向位移的二阶导数的权重，wl″′为横向位移关于纵向位移的三阶导数的权重，wref为横向位移与参考误差的权重，
Figure BDA0003597566040000162
为粗略路径在第k个梯形中的直线段；
Figure BDA0003597566040000163
S1、S2和Sm分别为第1、2和m个梯形的宽度，Tm-1为第m个梯形的左边界，Tm为第m个梯形的右边界，
Figure BDA0003597566040000164
分别表示第1段、第2段、第m段贝塞尔曲线的第i个控制点，
Figure BDA0003597566040000165
表示第i个贝塞尔曲线控制点的系数，其形式为
Figure BDA0003597566040000166
其中
Figure BDA0003597566040000167
t表示为自变量，在路径规划周期中为纵向位移s，在速度规划周期中为时间t，n表示控制点的个数；
构建轨迹优化的约束条件为：
Figure BDA0003597566040000171
Figure BDA0003597566040000172
Figure BDA0003597566040000173
Figure BDA0003597566040000174
Figure BDA0003597566040000175
Figure BDA0003597566040000176
其中，
Figure BDA0003597566040000177
为第k段贝塞尔曲线的第r阶导数的第i个控制点，
Figure BDA0003597566040000178
为第k段贝塞尔曲线的第r阶导数的第n个控制点，
Figure BDA0003597566040000179
为第k+1段贝塞尔曲线的第r阶导数的第0个控制点，
Figure BDA00035975660400001710
分别为，sr为，
Figure BDA00035975660400001711
为第k个梯形的下边界函数，
Figure BDA00035975660400001712
为第k个梯形的上边界界函数，l′min与l′max分别为横向位移关于纵向位移一阶导数的最小值与最大值；l″min与l″max分别为横向位移关于纵向位移二阶导数的最小值与最大值；l″min与l″max分别为横向位移关于纵向位移三阶导数的最小值与最大值；
将轨迹优化的代价函数和约束条件转化为一个二阶规划表达式：
Figure BDA00035975660400001713
s.t.l≤Acx≤u
其中，x为轨迹优化中的l(s)，P为横向轨迹优化(或纵向轨迹优化)中代价函数的二次项，qT为横向轨迹优化(或纵向轨迹优化)中代价函数的线性项，l与u分别为横向轨迹优化(或纵向轨迹优化)中约束条件的下边界与上边界，其中等式约束的下边界与上边界均为0，Ac为横向轨迹优化(或纵向轨迹优化)中约束条件形成的约束矩阵；
求解二阶规划表达式，获得优化轨迹。
横向优化轨迹如图8所示。
步骤8，根据横向优化轨迹，构建纵向位移-时间图。
纵向位移-时间图构建原理与步骤2中纵向位移-横向位移图的构建原理相同。
步骤9，将运动速度大于或等于速度阈值的障碍物投影到纵向位移-时间图中，并将纵向位移-时间图替换纵向位移-横向位移图，返回步骤“利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线”，获得纵向优化轨迹。
运动速度大于或等于速度阈值的障碍物主要指高速的动态障碍物。
高速的动态障碍物投影过程为：将高速动态障碍物在0时刻处边界框(BoundingBox)中的每个顶点从x-y坐标系转到s-l坐标系，将0时刻处的s-l坐标中的s(由smin与smax)作为s-t图中的下边线((0，smin)与(0，smax))，将在tend(tend为tlast与tmax的最小值，tlast某个高速动态障碍物预测轨迹中最后一个点处的时间，tmax为规划周期所考虑的未来最大时间)处边界框(Bounding Box)中的每个顶点从x-y坐标系转到s-l坐标系，将tend时刻处的s-l坐标中的s(由smin与smax)作为s-t图中的上边线((tend，smin)与(tmax，smax))。
在纵向位移-时间图中确定纵向优化轨迹的方法与在纵向位移-横向位移图中确定横向优化轨迹的方法相同，简要步骤为：
根据S1中获取的自车的相关信息，将每个速度规划周期的起点投影到s-t中，每个速度规划周期的起点可根据实际的场景进行确定，但起点不应距离此时自车的位姿在s-t图中的投影太远；
利用单元格分解法对s-t图进行分解从而得到一系列障碍物分解线，此处与S4中的步骤相同。
利用动态规划算法对每一条直线段进行代价评估，最后取代价值最小的一条作为s-t图中的粗略路径，如图9所示。
所述的每一条直线段的代价评估函数表示成式：
Figure BDA0003597566040000181
其中n为直线段的总段数，w1为一阶导数与参考速度的误差权重，w2与w3为一阶导数、二阶导数与三阶导数的权重，w4与障碍物距离的代价，S′i、S″i与s″′i分别为第i个直线段的一阶导数、二阶导数与三阶导数，其表示成式：
Figure BDA0003597566040000191
Figure BDA0003597566040000192
Figure BDA0003597566040000193
其中p为第i个直线段离散的个数，sj为第j个离散点的纵向位移，dt为离散的纵向间隔；而Cobs(S)与式中相同。
根据s-t图中的粗略路径，得到s-t图的梯形走廊；然后使用纵向轨迹优化与分段贝塞尔速度曲线在s-t图的梯形走廊内部优化速度曲线。
纵向轨迹优化的代价函数表示成式：
Figure BDA0003597566040000194
其中，Jspeed代表纵向轨迹优化的代价，s为纵向位移，t为横向位移，k为分段贝塞尔曲线某一段的编号，同时也为梯形走廊中某一个梯形的编号，m为分段贝塞尔曲线总的段数，同时也为梯形走廊中总的梯形的个数，Tk为第k个梯形的宽度，sk(t)为第k段贝塞尔曲线，也即第k段纵向位移关于时间位移的函数，
Figure BDA0003597566040000198
为S5中所得到的粗略路径的第k段，ws为纵向位移接近参考纵向位移的权重，
Figure BDA0003597566040000199
为纵向速度接近参考速度的权重，
Figure BDA00035975660400001910
为纵向加速度的权重，
Figure BDA00035975660400001911
为纵向加加速度的权重。
纵向轨迹优化的约束条件表示成式：
Figure BDA0003597566040000195
Figure BDA0003597566040000196
Figure BDA0003597566040000197
Figure BDA0003597566040000201
Figure BDA0003597566040000202
Figure BDA0003597566040000203
Figure BDA0003597566040000204
其中第1个式子为边界约束；第2个式子式为连续性约束；第3个式子式为安全性约束；第4个式子式与为物理约束；
Figure BDA0003597566040000205
表示第k段贝塞尔曲线的第r阶导数的第i个控制点，Tk为第k个梯形的宽度，
Figure BDA0003597566040000206
为第k个梯形的下边界函数，
Figure BDA0003597566040000207
为第k个梯形的上边界界函数，vmin与vmax分别为纵向速度的最小值与最大值；amin与amax分别为纵向加速度的最小值与最大值；jmin与jmax分别为纵向加加速度的最小值与最大值；acm为最大的离心加速度；κk为第k段贝塞尔曲线的最大曲率。
纵向优化轨迹如图10所示。
步骤10，合并横向优化轨迹与纵向优化轨迹，并对合并后的优化轨迹进行安全检测，将通过安全检测的合并后的优化轨迹确定为最优可行轨迹。
示例性的，合并横向优化轨迹与纵向优化轨迹，具体包括：
将纵向位移-时间图上的时间段均匀划分，获得一系列采样时间点；
从纵向位移-时间图中的纵向优化轨迹上得到采样时间点ti处的纵向位移si、纵向速度
Figure BDA0003597566040000208
以及纵向加速度
Figure BDA0003597566040000209
从纵向位移-横向位移图中的横向优化轨迹上得到纵向位移si处的横向位移li、横向速度l′i以及横向加速度l″i；
确定frenet坐标系下的一个轨迹点
Figure BDA00035975660400002010
将确定的轨迹点转换到笛卡尔坐标系下，获得笛卡尔坐标系下的一个轨迹点(ti，xi，yi，si，θi，κi，vi，ai)；其中，xi与yi为笛卡尔坐标系下的位姿，θi为笛卡尔坐标下的朝向，κi为曲率，vi与ai分别为速度与加速度；
重复以上步骤，获得所有采样时间点在笛卡尔坐标系下的轨迹点，依次连接所有轨迹点形成合并后的优化轨迹。
作为一种示例方式，对合并后的优化轨迹进行安全检测，具体包括：
判断合并后的优化轨迹上每个轨迹点是否满足安全条件，获得第一判断结果；安全条件为速度小于或等于速度阈值，加速度小于或等于加速度阈值，加加速度小于或等于加加速度阈值且曲率小于或等于曲率阈值；
若第一判断结果表示是，则判断每个轨迹点是否与障碍物发生碰撞，获得第二判断结果；
若第二判断结果表示否，则判定合并后的优化轨迹通过安全检测；
若第一判断结果表示否或第二判断结果表示是，则判定合并后的优化轨迹没有通过安全检测，并将合并后的优化轨迹确定为备用轨迹。
本发明提出了一种解耦的自动驾驶车辆实时轨迹规划方法，通过将一个三维空间的轨迹规划问题解耦为两个二维的轨迹规划问题，并在每个二维问题下利用单元格分解法与动态规划算法得到粗略路径与梯形走廊，同时利用二次规划在梯形走廊内对每个二维问题进行优化求解，最后得到可行轨迹。
本方法首先将比较难求解的三维规划问题转化为两个二次规划问题来进行求解，降低了复杂度；并且利用单元格分解法对所有投影过后的感知障碍物进行分解，因此不存在微小障碍物的影响；此外，首先对单元格分解法分解过后得到的一系列障碍物分解线进行散点，随后利用动态规划算法对得到粗略路径；再者，本方法通过所得到的粗略路径可以得到由一个一个梯形所组成的路径凸走廊；最后，本方法使用分段贝塞尔曲线并利用二次规划的方法在路径凸走廊中进行优化，因为使用的是分段贝塞尔曲线，故每段曲线不会超出每个梯形的范围，并且因为贝塞尔曲线的导数仍然是贝塞尔曲线，故对每段贝塞尔曲线控制点的速度、加速度、加加速度进行限制，可以领每段贝塞尔曲线的倒数也分别在每个控制点的速度、加速度、加加速度组成的凸包内部；因为使用的是二次规划，其求解速度可以达到实时性的要求。故本方法解决了自动驾驶车辆在轨迹规划时三维问题难以求解、使用采样方法得到的轨迹并非最优且不适用于复杂城市道路场景、以及离散规划空间会出现小障碍物无法处理等问题。
实施例二
本实施例提供了一种解耦的自动驾驶车辆实时规划装置，如图11所示，规划装置包括：路由模块、定位模块、感知模块、规划模块和控制模块。
路由模块、定位模块、感知模块均与规划模块的输入端连接，控制模块与规划模块的输出端连接。路由模块用于向规划模块发送自车的全局路径。定位模块用于向规划模块发送自车的当前位姿。感知模块用于向规划模块发送自车周围的障碍物信息。规划模块用于根据自车的全局路径、当前位姿和自车周围的障碍物信息，采用前述的解耦的自动驾驶车辆实时轨迹规划方法，获得最优可行轨迹，并将所述最优可行轨迹发送至控制模块。控制模块用于控制自车执行所述最优可行轨迹。
实施例三
本实施例提供了一种解耦的自动驾驶车辆实时规划系统，规划系统包括：
实时信息获取模块，用于实时获取自车的位姿信息和自车周围的障碍物；
纵向位移-横向位移图获得模块，用于在自车的全局路径上截取包含自车当前位姿的一段路径作为参考线，并将参考线从笛卡尔坐标系转换到Frenet坐标系下，获得纵向位移-横向位移图；
投影模块，用于将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中；
分解模块，用于利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线；
备选路径形成模块，用于在每一条分解线上布设多个采样点，并从起点开始依次连接分解线上的采样点，形成多条备选路径；备选路径由多条依次首尾连接的直线段构成，直线段为起点与相邻分解线上采样点直线连接或相邻分解线上采样点直线连接形成的线段；
粗略路径确定模块，用于采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径；
横向优化轨迹获得模块，用于将粗略路径上的每条直线段所在四边形的边框构成梯形走廊，并采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，获得横向优化轨迹；
纵向位移-时间图构建模块，用于根据横向优化轨迹，构建纵向位移-时间图；
纵向优化轨迹获得模块，用于将运动速度大于或等于速度阈值的障碍物投影到纵向位移-时间图中，并将纵向位移-时间图替换纵向位移-横向位移图，返回步骤“利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线”，获得纵向优化轨迹；
最优可行轨迹确定模块，用于合并横向优化轨迹与纵向优化轨迹，并对合并后的优化轨迹进行安全检测，将通过安全检测的合并后的优化轨迹确定为最优可行轨迹。
投影模块，具体包括：
静态障碍物投影子模块，用于当运动速度小于速度阈值的障碍物为静态障碍物时，将静态障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影静态障碍物；
低速障碍物投影子模块，用于当运动速度小于速度阈值的障碍物为低速障碍物时，将低速障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影低速障碍物；低速障碍物与自车的行驶方向相同，低速障碍物的运动速度大于零且小于速度阈值；
来向障碍物投影子模块，用于当运动速度小于速度阈值的障碍物为来向障碍物时，将来向障碍物的预测轨迹与自车的当前轨迹发生碰撞位置处的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影来向障碍物。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，所述规划方法包括：
实时获取自车的位姿信息和自车周围的障碍物；
在自车的全局路径上截取包含自车当前位姿的一段路径作为参考线，并将参考线从笛卡尔坐标系转换到Frenet坐标系下，获得纵向位移-横向位移图；
将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中；
利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线；
在每一条分解线上布设多个采样点，并从起点开始依次连接分解线上的采样点，形成多条备选路径；所述备选路径由多条依次首尾连接的直线段构成，所述直线段为起点与相邻分解线上采样点直线连接或相邻分解线上采样点直线连接形成的线段；
采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径；
将粗略路径上的每条直线段所在四边形的边框构成梯形走廊，并采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，获得横向优化轨迹；
根据所述横向优化轨迹，构建纵向位移-时间图；
将运动速度大于或等于速度阈值的障碍物投影到纵向位移-时间图中，并将纵向位移-时间图替换纵向位移-横向位移图，返回步骤“利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线”，获得纵向优化轨迹；
合并横向优化轨迹与纵向优化轨迹，并对合并后的优化轨迹进行安全检测，将通过安全检测的合并后的优化轨迹确定为最优可行轨迹。
2.根据权利要求1所述的解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，所述将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中，具体包括：
当运动速度小于速度阈值的障碍物为静态障碍物时，将静态障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影静态障碍物；
当运动速度小于速度阈值的障碍物为低速障碍物时，将低速障碍物的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影低速障碍物；所述低速障碍物与自车的行驶方向相同，所述低速障碍物的运动速度大于零且小于速度阈值；
当运动速度小于速度阈值的障碍物为来向障碍物时，将来向障碍物的预测轨迹与自车的当前轨迹发生碰撞位置处的边界框中的每个顶点从笛卡尔坐标系转换到Frenet坐标系，在纵向位移-横向位移图中投影来向障碍物。
3.根据权利要求1所述的解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，所述利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线，具体包括：
在每个障碍物的每个顶点建立一条垂直线；
选取垂直线与纵向位移-横向位移图边界框的交点以及垂直线与障碍物边界框的交点作为备选交点；
沿垂直线在每个顶点的两侧分别选出与每个顶点距离最近的备选交点；
分别连接每个顶点与各自选出的两个备选交点，获得每个顶点的两个连接线段；
删除与障碍物相交的连接线段，获得每个障碍物的每个顶点的一条分解线。
4.根据权利要求1所述的解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，所述采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径，具体包括：
沿纵向位移-横向位移图的s轴正方向将分解线从第1层开始依次编号；
初始化层数u＝1；
利用公式Ctotal(Lv)＝Csmooth(Lv)+Cobs(Lv)+Cguidance(Lv)，计算纵向位移-横向位移图中起点与第u层之间每条直线段的代价值；其中，Ctotal(Lv)为起点与第u层之间第v条直线段的代价值，Csmooth(Lv)为起点与第u层之间第v条直线段的平滑性代价，Cobs(Lv)为起点与第u层之间第v条直线段与障碍物的距离代价，Cguidance(Lv)为起点与第u层之间第v条直线段与参考线的代价；
Figure FDA0003597566030000031
w1、w2与w3分别为一阶导数、二阶导数与三阶导数的权重，L′v、L″v与L″′v分别为第v条直线段的一阶导数、二阶导数与三阶导数，
Figure FDA0003597566030000032
Figure FDA0003597566030000033
p为第v条直线段离散的个数，lj为第j个离散点的横向位移，ds为离散的纵向间隔；
Figure FDA0003597566030000034
Figure FDA0003597566030000035
与
Figure FDA0003597566030000036
分别为第v条直线段的右端点所在分解线的长边与短边的长度；Cguidance(Lv)＝Lv-(Gv(s)，Gv(s)为起点与第u层之间第v条直线段处的参考线在s处的横向位移，Lv为起点与第u层之间第v条直线段的的横向位移，
Figure FDA0003597566030000037
将代价值最小的直线段作为起点与第u层之间的最优直线段；
以最优直线段在第u层上的交点为起点，令u的数值增加1，并返回步骤“利用公式Ctotal(Lv)＝Csmooth(Lv)+Cobs(Lv)+Cguidance(Lv)，计算起点与第u层之间每条直线段的代价值”，直至u的数值大于最大层数，停止循环，获得纵向位移-横向位移图中的粗略路径。
5.根据权利要求1所述的解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-时间图中的粗略路径，具体包括：
沿纵向位移-时间图的t轴正方向将分解线从第1层开始依次编号；
初始化层数u＝1；
利用公式
Figure FDA0003597566030000038
计算纵向位移-时间图中起点与第u层之间每条直线段的代价值；其中，Ctotal(Sv)为纵向位移-时间图中起点与第u层之间第v条直线段的代价值，w4为与障碍物距离的代价，Vref为当前道路段的参考速度，Cobs(s)为纵向位移-时间图中起点与第u层之间第v条直线段与障碍物的距离代价，
Figure FDA0003597566030000041
Figure FDA0003597566030000042
与
Figure FDA0003597566030000043
分别为第v条直线段的右端点所在分解线的长边与短边的长度，S′v、S″v与S″′v分别为第v条直线段的一阶导数、二阶导数与三阶导数，
Figure FDA0003597566030000044
Figure FDA0003597566030000045
sj为第j个离散点的纵向位移，dt为离散的纵向间隔；
将代价值最小的直线段作为起点与第u层之间的最优直线段；
以最优直线段在第u层上的交点为起点，令u的数值增加1，并返回步骤“利用公式
Figure FDA0003597566030000046
计算纵向位移-时间图中起点与第u层之间每条直线段的代价值”，直至u的数值大于最大层数，停止循环，获得纵向位移-时间图中的粗略路径。
6.根据权利要求1所述的解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，所述采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，具体包括：
当采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行横向轨迹优化时：
构建横向轨迹优化的代价函数为
Figure FDA0003597566030000047
其中，Jpath代表轨迹优化的代价，轨迹优化为横向轨迹优化或纵向轨迹优化；k为分段贝塞尔曲线的编号，同时对应梯形走廊中梯形的编号；s为纵向位移，l为横向位移，m为分段贝塞尔曲线总的段数，Sk为第k个梯形的宽度，wl为横向位移的权重，wl′为横向位移关于纵向位移的一阶导数的权重，wl″为横向位移关于纵向位移的二阶导数的权重，wl″′为横向位移关于纵向位移的三阶导数的权重，wref为横向位移与参考误差的权重，
Figure FDA0003597566030000048
为粗略路径在第k个梯形中的直线段；lk(s)为第k段贝塞尔曲线，
Figure FDA0003597566030000051
S1、S2和Sm分别为第1、2和m个梯形的宽度，Tm-1为第m个梯形的左边界，Tm为第m个梯形的右边界，
Figure FDA0003597566030000052
分别表示第1段、第2段、第m段贝塞尔曲线的第i个控制点，
Figure FDA0003597566030000053
表示第i个贝塞尔曲线控制点的系数，其形式为
Figure FDA0003597566030000054
其中
Figure FDA0003597566030000055
t表示自变量，t在路径规划周期中为纵向位移s，t在速度规划周期中为时间，n表示控制点的个数；
构建横向轨迹优化的约束条件为：
Figure FDA0003597566030000056
Figure FDA0003597566030000057
Figure FDA0003597566030000058
Figure FDA0003597566030000059
Figure FDA00035975660300000510
Figure FDA00035975660300000511
其中，
Figure FDA00035975660300000512
为第k段贝塞尔曲线的第r阶导数的第i个控制点，
Figure FDA00035975660300000513
为第k段贝塞尔曲线的第r阶导数的第n个控制点，
Figure FDA00035975660300000514
为第k+1段贝塞尔曲线的第r阶导数的第0个控制点，Sk为第k个梯形的宽度，
Figure FDA00035975660300000515
分别为Sk、Sk+1的(1-r)次幂，
Figure FDA00035975660300000516
表示在s＝sk时，lk(s)对s的r阶导数值，
Figure FDA00035975660300000517
为第k个梯形的下边界函数，
Figure FDA00035975660300000518
为第k个梯形的上边界界函数，l′min与l′max分别为横向位移关于纵向位移一阶导数的最小值与最大值；l″min与l″max分别为横向位移关于纵向位移二阶导数的最小值与最大值；l″min与l″max分别为横向位移关于纵向位移三阶导数的最小值与最大值；
当采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行纵向轨迹优化时：
构建纵向轨迹优化的代价函数为
Figure FDA0003597566030000061
其中，Jspeed为纵向轨迹优化的代价值，s为纵向位移，t为横向位移，同时也为梯形走廊中总的梯形的个数，Tk为第k个梯形的宽度，sk(t)为第k段贝塞尔曲线，
Figure FDA0003597566030000062
为纵向位移-时间图中粗略路径在第k个梯形中的直线段，ws为纵向位移接近参考纵向位移的权重，
Figure FDA0003597566030000063
为纵向速度接近参考速度的权重，
Figure FDA0003597566030000064
为纵向加速度的权重，
Figure FDA0003597566030000065
为纵向加加速度的权重；
构建纵向轨迹优化的约束条件为：
Figure FDA0003597566030000066
Figure FDA0003597566030000067
Figure FDA0003597566030000068
Figure FDA0003597566030000069
Figure FDA00035975660300000610
Figure FDA00035975660300000611
Figure FDA00035975660300000612
其中，
Figure FDA00035975660300000613
为第k段贝塞尔曲线的第r阶导数的第i个控制点，Tk为第k个梯形的宽度，
Figure FDA00035975660300000614
为第k个梯形的下边界函数，
Figure FDA00035975660300000615
为第k个梯形的上边界界函数，vmin与vmax分别为纵向速度的最小值与最大值；amin与amax分别为纵向加速度的最小值与最大值；jmin与jmax分别为纵向加加速度的最小值与最大值；acm为最大的离心加速度；κk为第k段贝塞尔曲线的最大曲率；
将横向轨迹优化的代价函数和约束条件、纵向轨迹优化的代价函数和约束条件分别转化为一个二阶规划表达式：
Figure FDA0003597566030000071
s.t. l≤Acx≤u
其中，x为轨迹优化中的l(s)或sk(t)，P为轨迹优化中代价函数的二次项，qT为轨迹优化中代价函数的线性项，l与u分别为轨迹优化中约束条件的下边界与上边界，等式约束的下边界与上边界均为0，Ac为轨迹优化中约束条件形成的约束矩阵；
求解二阶规划表达式，获得横向优化轨迹或纵向优化轨迹。
7.根据权利要求1所述的解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，所述合并横向优化轨迹与纵向优化轨迹，具体包括：
将纵向位移-时间图上的时间段均匀划分，获得一系列采样时间点；
从纵向位移-时间图中的纵向优化轨迹上得到采样时间点ti处的纵向位移si、纵向速度
Figure FDA0003597566030000072
以及纵向加速度
Figure FDA0003597566030000073
从纵向位移-横向位移图中的横向优化轨迹上得到纵向位移si处的横向位移li、横向速度l′i以及横向加速度l″i；
确定frenet坐标系下的一个轨迹点
Figure FDA0003597566030000074
将确定的轨迹点转换到笛卡尔坐标系下，获得笛卡尔坐标系下的一个轨迹点{ti，xi，yi，si，θi，κi，vi，ai)；其中，xi与yi为笛卡尔坐标系下的位姿，θi为笛卡尔坐标下的朝向，κi为曲率，vi与ai分别为速度与加速度；
重复以上步骤，获得所有采样时间点在笛卡尔坐标系下的轨迹点，依次连接所有轨迹点形成合并后的优化轨迹。
8.根据权利要求1所述的解耦的自动驾驶车辆实时轨迹规划方法，其特征在于，所述对合并后的优化轨迹进行安全检测，具体包括：
判断合并后的优化轨迹上每个轨迹点是否满足安全条件，获得第一判断结果；所述安全条件为速度小于或等于速度阈值，加速度小于或等于加速度阈值，加加速度小于或等于加加速度阈值且曲率小于或等于曲率阈值；
若所述第一判断结果表示是，则判断每个轨迹点是否与障碍物发生碰撞，获得第二判断结果；
若所述第二判断结果表示否，则判定合并后的优化轨迹通过安全检测；
若所述第一判断结果表示否或所述第二判断结果表示是，则判定合并后的优化轨迹没有通过安全检测，并将合并后的优化轨迹确定为备用轨迹。
9.一种解耦的自动驾驶车辆实时规划装置，其特征在于，所述规划装置包括：路由模块、定位模块、感知模块、规划模块和控制模块；
路由模块、定位模块、感知模块均与规划模块的输入端连接，控制模块与规划模块的输出端连接；
所述路由模块用于向规划模块发送自车的全局路径；
所述定位模块用于向规划模块发送自车的当前位姿；
所述感知模块用于向规划模块发送自车周围的障碍物信息；
所述规划模块用于根据自车的全局路径、当前位姿和自车周围的障碍物信息，采用权利要求1-8任一项所述的解耦的自动驾驶车辆实时轨迹规划方法，获得最优可行轨迹，并将所述最优可行轨迹发送至控制模块；
所述控制模块用于控制自车执行所述最优可行轨迹。
10.一种解耦的自动驾驶车辆实时规划系统，其特征在于，所述规划系统包括：
实时信息获取模块，用于实时获取自车的位姿信息和自车周围的障碍物；
纵向位移-横向位移图获得模块，用于在自车的全局路径上截取包含自车当前位姿的一段路径作为参考线，并将参考线从笛卡尔坐标系转换到Frenet坐标系下，获得纵向位移-横向位移图；
投影模块，用于将运动速度小于速度阈值的障碍物投影到纵向位移-横向位移图中；
分解模块，用于利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线；
备选路径形成模块，用于在每一条分解线上布设多个采样点，并从起点开始依次连接分解线上的采样点，形成多条备选路径；所述备选路径由多条依次首尾连接的直线段构成，所述直线段为起点与相邻分解线上采样点直线连接或相邻分解线上采样点直线连接形成的线段；
粗略路径确定模块，用于采用动态规划算法计算每条备选路径的代价值，并将代价值最小的一条备选路径作为纵向位移-横向位移图中的粗略路径；
横向优化轨迹获得模块，用于将粗略路径上的每条直线段所在四边形的边框构成梯形走廊，并采用分段贝塞尔路径曲线在梯形走廊内对粗略路径进行轨迹优化，获得横向优化轨迹；
纵向位移-时间图构建模块，用于根据所述横向优化轨迹，构建纵向位移-时间图；
纵向优化轨迹获得模块，用于将运动速度大于或等于速度阈值的障碍物投影到纵向位移-时间图中，并将纵向位移-时间图替换纵向位移-横向位移图，返回步骤“利用单元格分解法对纵向位移-横向位移图中每个障碍物的每个顶点建立一条分解线”，获得纵向优化轨迹；
最优可行轨迹确定模块，用于合并横向优化轨迹与纵向优化轨迹，并对合并后的优化轨迹进行安全检测，将通过安全检测的合并后的优化轨迹确定为最优可行轨迹。
CN202210396754.8A 2022-04-15 2022-04-15 一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统 Pending CN114715192A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210396754.8A CN114715192A (zh) 2022-04-15 2022-04-15 一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210396754.8A CN114715192A (zh) 2022-04-15 2022-04-15 一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统

Publications (1)

Publication Number Publication Date
CN114715192A true CN114715192A (zh) 2022-07-08

Family

ID=82244464

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210396754.8A Pending CN114715192A (zh) 2022-04-15 2022-04-15 一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统

Country Status (1)

Country Link
CN (1) CN114715192A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115355916A (zh) * 2022-10-24 2022-11-18 北京智行者科技股份有限公司 移动工具的轨迹规划方法、设备和计算机可读存储介质
CN115933701A (zh) * 2023-01-06 2023-04-07 北京理工大学 一种基于二次规划的安全走廊优化生成方法及系统

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115355916A (zh) * 2022-10-24 2022-11-18 北京智行者科技股份有限公司 移动工具的轨迹规划方法、设备和计算机可读存储介质
CN115355916B (zh) * 2022-10-24 2023-03-03 北京智行者科技股份有限公司 移动工具的轨迹规划方法、设备和计算机可读存储介质
CN115933701A (zh) * 2023-01-06 2023-04-07 北京理工大学 一种基于二次规划的安全走廊优化生成方法及系统
CN115933701B (zh) * 2023-01-06 2023-06-06 北京理工大学 一种基于二次规划的安全走廊优化生成方法及系统

Similar Documents

Publication Publication Date Title
CN112068545B (zh) 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质
JP6982897B2 (ja) ライダ、レーダ及びカメラセンサのデータを使用する強化学習に基づく自律走行時の最適化されたリソース割当てのための方法及び装置
CN114715192A (zh) 一种解耦的自动驾驶车辆实时轨迹规划方法、装置及系统
US11392127B2 (en) Trajectory initialization
WO2023005223A1 (zh) 轨迹规划方法、装置、存储介质、设备及计算机程序产品
CN111830979B (zh) 一种轨迹优化方法和装置
CN112212872B (zh) 基于激光雷达和导航地图的端到端自动驾驶方法及系统
CN114234998A (zh) 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法
CN112539749B (zh) 机器人导航方法、机器人、终端设备及存储介质
WO2022257283A1 (zh) 车辆避障方法、装置、电子设备及存储介质
CN111006667B (zh) 高速场景下的自动驾驶轨迹生成系统
CN113721637A (zh) 智能车动态避障路径连续规划方法、系统及存储介质
CN111768647A (zh) 一种基于移动边缘计算的自主泊车方法和装置
CN106203341B (zh) 一种无人车的车道线识别方法及装置
CN113895463B (zh) 一种适用于自动驾驶车辆掉头的路径规划方法
CN111896004A (zh) 一种狭窄通道车辆轨迹规划方法及系统
CN114684111A (zh) 一种车头先入的泊车方法、装置和系统
CN115077553A (zh) 基于栅格搜索轨迹规划方法、系统、汽车、设备及介质
CN112665603B (zh) 一种基于带时间窗a*改进的多车路径规划方法
CN115840454B (zh) 非结构化道路冲突区域的多车轨迹协同规划方法及装置
CN117109620A (zh) 基于采样的车辆行为与环境交互的自动驾驶路径规划方法
JP2658056B2 (ja) 自律走行車両制御装置
CN116225026A (zh) 自动驾驶车辆操作方法、电子设备以及计算机可读介质
CN114187781B (zh) 一种分布式多车协同行为决策方法和系统
CN115416693A (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