CN111830979A - 一种轨迹优化方法和装置 - Google Patents

一种轨迹优化方法和装置 Download PDF

Info

Publication number
CN111830979A
CN111830979A CN202010670920.XA CN202010670920A CN111830979A CN 111830979 A CN111830979 A CN 111830979A CN 202010670920 A CN202010670920 A CN 202010670920A CN 111830979 A CN111830979 A CN 111830979A
Authority
CN
China
Prior art keywords
path
evaluation
determining
optimized
optimization
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
CN202010670920.XA
Other languages
English (en)
Other versions
CN111830979B (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.)
Guangzhou Xiaopeng Internet of Vehicle Technology Co Ltd
Original Assignee
Guangzhou Xiaopeng Internet of Vehicle Technology 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 Guangzhou Xiaopeng Internet of Vehicle Technology Co Ltd filed Critical Guangzhou Xiaopeng Internet of Vehicle Technology Co Ltd
Priority to CN202010670920.XA priority Critical patent/CN111830979B/zh
Publication of CN111830979A publication Critical patent/CN111830979A/zh
Application granted granted Critical
Publication of CN111830979B publication Critical patent/CN111830979B/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/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
    • 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to 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)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明实施例提供了一种轨迹优化方法和装置，所述的方法包括：确定原始路径；对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域，其中，所述最大可行驶区域采用线性约束条件描述；依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径；基于所述优化路径进行速度优化，得到速度优化曲线；将所述优化路径与速度优化曲线进行合并，得到优化轨迹；相对于计算两个凸多边形的距离而言，计算最大可行驶区域的复杂度更低，进而能够快速的实现路径优化；从而能够快速的进行轨迹优化，提高轨迹优化的效率。

Description

一种轨迹优化方法和装置
技术领域
本发明涉及汽车技术领域，特别是涉及一种轨迹方法和装置。
背景技术
随着人工智能的发展，人工智能也被广泛的应用到各个领域，如车辆领域。车辆在很多方面应用了人工智能，如语音助手、自动驾驶(也称无人驾驶)等。自动驾驶通过将传感器、机器以及人工智能结合，实现控制车辆驾驶，以提升整个驾驶行为的安全和效率。
自动驾驶过程中，需要为车辆进行轨迹规划，然后按照规划的轨迹控制车辆行驶。目前，轨迹规划方法主要分为基于采样、搜索和最优控制。虽然基于最优控制的规划器通过设置约束，可实现采样和搜索方法无法比拟的连续性和舒适性要求，但该方法通常需要沉重的计算负担而限制了其在实时控制的应用。
因此通常先利用基于采样和搜索的快速找到粗略的轨迹，然后使用粗略的轨迹作为初始猜测值进行数值优化，可显著地提高优化速度。尽管快速搜索的初始猜测有助于数值求解过程，但是在制定的最优控制问题中仍然存在大量的非凸避免碰撞的约束，这使得在初始猜测未接近最优时优化速度变慢。另外，传统的碰撞检测中，车辆在每一个采样位置均需要与环境中所有障碍物进行碰撞检测，即使可以在采样位置处仅搜索附近的障碍物可减小碰撞检测的规模，但是避免碰撞在最优控制问题中通常被表达为两个凸多边形的最小距离大于设定的距离阈值，然而两个凸多边形的距离计算涉及复杂的数学运算而存在大量的时间消耗。
发明内容
本发明实施例提供一种轨迹优化方法，以提高轨迹优化的效率。
本发明实施例还提供了一种轨迹优化装置，以保证上述方法的实施。
为了解决上述问题，本发明公开了一种轨迹优化方法，包括：确定原始路径；对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域，其中，所述最大可行驶区域采用线性约束条件描述；依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径；基于所述优化路径进行速度优化，得到速度优化曲线；将所述优化路径与速度优化曲线进行合并，得到优化轨迹。
可选地，所述确定原始路径，包括：根据车辆当前位置确定车辆前方待进行轨迹规划的目标区域，并在所述目标区域中设置多条评估线；分别对各条所述评估线进行采样，确定各条所述评估线上的评估点；根据车辆当前位置和各条所述评估线上评估点的位置，生成相邻两条所述评估线之间的路径，所述相邻两条所述评估线之间包括多条路径；确定各相邻两条所述评估线之间的最优路径，基于各相邻两条所述评估线之间的最优路径确定原始路径。
可选地，所述在所述目标区域中设置多条评估线，包括：将目标区域中的障碍物映射至Frenet坐标系中，并在Frenet坐标系的目标区域中按照第一预设步长设置多条评估线；所述分别对各条所述评估线进行采样，确定各条所述评估线上的评估点，包括：基于Frenet坐标系中障碍物的位置和车辆宽度分别对各条评估线进行采样，确定各条所述评估线上的多个评估点。
可选地，所述基于Frenet坐标系中障碍物的位置和车辆宽度分别对各条所述评估线进行采样，确定各条所述评估线上的多个评估点，包括：针对一条评估线，基于Frenet坐标系中障碍物的位置和车辆宽度，确定所述评估线上可行驶部分的边界点；根据所述评估线上可行驶部分的边界点确定所述评估线上的中点；从所述评估线上可行驶部分的中点开始，按照第二预设步长向所述评估线的边界点进行采样，得到所述评估线上的多个评估点。
可选地，所述确定各相邻两条所述评估线之间的最优路径，包括：针对一对相邻两条所述评估线之间的一条路径，计算所述路径对应的路径成本、静态障碍物成本和动态障碍物成本；依据所述路径成本、静态障碍物成本和动态障碍物成本，确定所述路径对应的成本；将所述一对相邻两条所述评估线之间各条路径中成本最小的路径，确定为所述一对相邻两条所述评估线之间的最优路径。
可选地，所述确定各采样点对应的最大可行驶区域，包括：针对一个采样点，确定所述采样点处可包围车辆的多边形；按照第三预设步长将所述多边形的各边向对应的方向扩张，直到所述多边形的任一边与所述目标区域内的障碍物/道路边界碰撞为止，并将停止扩展时多边形对应的区域，确定为所述采样点对应的最大可行驶区域。
可选地，所述依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径，包括：确定目标路径函数，以及将各最大可行驶区域对应的线性约束条件作为所述目标路径函数的约束条件；以最小化目标路径函数为目标对目标路径函数进行优化，确定各最大可行驶区域内位置最优的节点；依据各最大可行驶区域内位置最优的节点，生成优化路径。
可选地，所述基于所述优化路径进行速度优化，得到速度优化曲线，包括：若所述优化路径的最后一个节点属于最后一个采样点对应的最大可行驶区域，则依据参考速度曲线进行速度优化，得到速度优化曲线；若所述优化路径的最后一个采样点不属于最后一个采样点对应的最大可行驶区域，则获取若车辆按照任一对相邻两个可行驶子区域之间的路径行驶的情况下，与车辆发生碰撞的目标动态障碍物；依据所述优化路径将所述目标动态障碍物投影至弧度-时间图，并基于投影后的目标态障碍物进行跟随的速度优化，得到速度优化曲线。
可选地，所述最大可行驶区域在笛卡尔坐标系下确定。
本发明实施例还提供了一种轨迹优化装置，包括：路径确定模块，用于确定原始路径；区域确定模块，用于对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域，其中，所述最大可行驶区域采用线性约束条件描述；路径优化模块，用于依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径；速度优化模块，用于基于所述优化路径进行速度优化，得到速度优化曲线；合并模块，用于将所述优化路径与速度优化曲线进行合并，得到优化轨迹。
可选地，所述路径确定模块，包括：评估线确定子模块，用于根据车辆当前位置确定车辆前方待进行轨迹规划的目标区域，并在所述目标区域中设置多条评估线；评估点确定子模块，用于分别对各条所述评估线进行采样，确定各条所述评估线上的评估点；路径生成子模块，用于根据车辆当前位置和各条所述评估线上评估点的位置，生成相邻两条所述评估线之间的路径，所述相邻两条所述评估线之间包括多条路径；原始路径确定子模块，用于确定各相邻两条所述评估线之间的最优路径，基于各相邻两条所述评估线之间的最优路径确定原始路径。
可选地，所述评估线确定子模块，用于将目标区域中的障碍物映射至Frenet坐标系中，并在Frenet坐标系的目标区域中按照第一预设步长设置多条评估线；所述评估点确定子模块，用于基于Frenet坐标系中障碍物的位置和车辆宽度分别对各条所述评估线进行采样，确定各条所述评估线上的多个评估点。
可选地，所述评估点确定子模块，具体用于针对一条评估线，基于Frenet坐标系中障碍物的位置和车辆宽度，确定所述评估线上可行驶部分的边界点；根据所述评估线上可行驶部分的边界点确定所述评估线上的中点；从所述评估线上可行驶部分的中点开始，按照第二预设步长向所述评估线的边界点进行采样，得到所述评估线上的多个评估点。
可选地，所述原始路径确定子模块，包括：成本计算单元，用于针对一对相邻两条所述评估线之间的一条路径，计算所述路径对应的路径成本、静态障碍物成本和动态障碍物成本；成本确定单元，用于依据所述路径成本、静态障碍物成本和动态障碍物成本，确定所述路径对应的成本；最优路径确定单元，用于将所述一对相邻两条所述评估线之间各条路径中成本最小的路径，确定为所述一对相邻两条所述评估线之间的最优路径。
可选地，所述区域确定模块，用于针对一个采样点，确定所述采样点处可包围车辆的多边形；按照第三预设步长将所述多边形的各边向对应的方向扩张，直到所述多边形的任一边与所述目标区域内的障碍物/道路边界碰撞为止，并将停止扩展时多边形对应的区域，确定为所述采样点对应的最大可行驶区域。
可选地，所述路径优化模块，包括：函数确定子模块，用于确定目标路径函数，以及将各最大可行驶区域对应的线性约束条件作为所述目标路径函数的约束条件；节点确定子模块，用于以最小化目标路径函数为目标对目标路径函数进行优化，确定各最大可行驶区域内位置最优的节点；优化路径生成子模块，用于依据各最大可行驶区域内位置最优的节点，生成优化路径。
可选地，所述速度优化模块，包括：第一速度优化曲线生成子模块，用于若所述优化路径的最后一个节点属于最后一个采样点对应的最大可行驶区域，则依据参考速度曲线进行速度优化，得到速度优化曲线；第二速度优化曲线生成子模块，用于若所述优化路径的最后一个采样点不属于最后一个采样点对应的最大可行驶区域，则获取若车辆按照任一对相邻两个可行驶子区域之间的路径行驶的情况下，与车辆发生碰撞的目标动态障碍物；依据所述优化路径将所述目标动态障碍物投影至弧度-时间图，并基于投影后的目标态障碍物进行跟随的速度优化，得到速度优化曲线。
可选地，所述最大可行驶区域在笛卡尔坐标系下确定。
本发明实施例还提供了一种可读存储介质，当所述存储介质中的指令由电子设备的处理器执行时，使得电子设备能够执行如本发明实施例任一所述的轨迹优化方法。
与现有技术相比，本发明实施例包括以下优点：
本发明实施例中，可以在确定原始路径后，对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域；然后依据用于描述最大可行驶区域的线性约束条件对所述原始路径进行路径优化，得到优化路径；相对于计算两个凸多边形的距离而言，计算最大可行驶区域的复杂度更低，进而能够快速的实现路径优化；再基于所述优化路径进行速度优化，得到速度优化曲线；并将所述优化路径与速度优化曲线进行合并，得到优化轨迹；进而能够快速的进行轨迹优化，提高轨迹优化的效率。且还能够在复杂环境下，实现高效的、鲁棒的轨迹优化。
附图说明
图1是本发明的一种轨迹优化方法实施例的步骤流程图；
图2是本发明的一种轨迹优化方法可选实施例的步骤流程图；
图3a是本发明实施例的一种笛卡尔坐标系下目标区域示意图；
图3b是本发明实施例的一种Frenet坐标系下目标区域示意图；
图3c是本发明实施例的一种相连两条评估线上评估点之间路径示意图；
图3d是本发明实施例的一种确定最大行驶区域方式示意图；
图4是本发明的一种轨迹优化装置实施例的结构框图；
图5是本发明的一种轨迹优化装置可选实施例的结构框图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
本发明实施例提供的一种轨迹方法，可以基于原始路径上各采样点的最大可行驶区域的线性约束条件，进行带碰撞约束的路径优化；相对于计算两个凸多边形的距离而言，计算最大可行驶区域的复杂度更低，进而能够快速的实现路径优化；然后基于优化后的路径进行速度优化，再将优化后的路径与速度进行合成，得到优化轨迹；进而快速的进行轨迹优化，提高轨迹优化的效率。且还能够在复杂环境下，实现高效的、鲁棒的轨迹优化。
参照图1，示出了本发明的一种轨迹优化方法实施例的步骤流程图。
步骤102、确定原始路径。
本发明实施例中，在自动驾驶过程中，控制车辆行驶至任一位置时，可以为车辆规划一条从当前位置到前方某一位置的路径；以便后续可以按照该路径控制车辆行驶至其前方的该位置。为了便于后续说明，可以将该路径称为原始路径。
其中，原始路径中任意一点的位置信息o1(s)一种表示方式可以为：
o1(s)＝[xo1(s),yo1(s),θo1(s),κo1(s)]
其中，(xo1(s),yo1(s))为该点在笛卡尔坐标系下的坐标信息，θo1(s)为该点在笛卡尔坐标系下的航向角，κo1(s)为该在笛卡尔坐标系下点曲率，s为该点在笛卡尔坐标系下，在道路参考线上对应投影点的弧长，道路参考线也就是道路中心线。
步骤104、对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域，其中，所述最大可行驶区域采用线性约束条件描述。
步骤106、依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径。
然后可以对原始路径进行离散采样，确定原始路径上的多个采样点。其中，一种采样方式可以是，在原始路径上每间隔设定长度抽取一个采样点。其中，所述设定长度可以按照需求设置，本发明实施例对此不作限制。
在确定原始路径上的多个采样点后，可以分别确定各采样点对应的最大可行驶区域；所述采样点对应的可行驶区域可以是指车辆在该采样点按照该采样点对应的航向角行驶时，不与障碍物碰撞且不压道路边界线的区域。
本发明的一个可选实施例中，为了保证车辆从一个采样点行驶到下一个采样点的过程中不与障碍物碰撞且不压道路边界线，所述设定长度不应过大；以及为了保证路径优化效率，所述设定长度不应过小。可选地，本发明实施例中，所述设定长度可以为至少一个车身的长度；一个示例中，所述设定长度可以为1个车身长度或1.5个车身长度。
其中，所述最大可行驶区域可以是一个多边形如四边形；而任一多边形均可以用线性约束条件进行描述，对应的，最大可行驶区域也可以采用线性约束条件描述。一个示例中，一种线性约束条件可以表示为：AX≤b，其中X可以表示采样点的优化变量如坐标信息；A和b为已知矩阵。
进而可以基于各采样点对应最大可行驶区域的线性约束条件对原始路径进行路径优化，确定优化路径；例如可以将该线性约束条件作为目标路径函数的约束条件，通过优化目标路径函数进行路径优化，确定优化路径。
一个示例中，优化路径中每个采样点的位置信息o2(s)可以表示为：
o2(s)＝[xo2(s),yo2(s),θo2(s),κo2(s)]
其中，(xo2(s),yo2(s))为该点在笛卡尔坐标系下的坐标信息，θo2(s)为该点在笛卡尔坐标系下的航向角，κo2(s)为该点在笛卡尔坐标系下的曲率，s为该点在笛卡尔坐标系下，在道路参考线上对应投影点对应的弧长。
步骤108、基于所述优化路径进行速度优化，得到速度优化曲线。
然后可以根据对原始路径进行优化得到的优化路径，进行速度优化，确定优化路径对应的速度优化曲线。
本发明的一个示例中，所述速度优化曲线中每个点的速度信息g(t)可以表示为：
g(t)＝[s,v(t),a(t),t]
其中，v(t)为该点的速度，a(t)为该点的加速度，t为该点的时间，s为该点在道路参考线上对应投影点对应的弧长。
步骤110、将所述优化路径与速度优化曲线进行合并，得到优化轨迹。
其中，可以基于弧长s将优化路径与速度优化曲线进行合并，可以得到优化轨迹。
本发明的一个示例中，所述优化轨迹中每个点的轨迹L(s)可以表示为：
L(s)＝[xL(s),yL(s),θL(s),kL(s),sL,vL(s),aL(s),tL]
其中，(xL(s),yL(s))为该点在笛卡尔坐标系下的坐标信息，θL(s)为该点在笛卡尔坐标系下的航向角，κL(s)为该点在笛卡尔坐标系下的曲率，s为该点在笛卡尔坐标系下，在道路参考线上对应投影点的弧长；vL(t)为该点的速度，aL(t)为该点的加速度，tL为该点的时间。
综上，本发明实施例中，可以在确定原始路径后，对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域；然后依据用于描述最大可行驶区域的线性约束条件对所述原始路径进行路径优化，得到优化路径；相对于计算两个凸多边形的距离而言，计算最大可行驶区域的复杂度更低，进而能够快速的实现路径优化；再基于所述优化路径进行速度优化，得到速度优化曲线；并将所述优化路径与速度优化曲线进行合并，得到优化轨迹；进而能够快速的进行轨迹优化，提高轨迹优化的效率。且还能够在复杂环境下，实现高效的、鲁棒的轨迹优化。
以下对如何确定各采样点对应的最大可行驶区域进行说明。
参照图2，示出了本发明的一种轨迹优化方法可选实施例的步骤流程图。
其中，步骤102中确定原始路径的一种实现方式，可以参照如下步骤202-步骤208：
步骤202、根据车辆当前位置确定车辆前方待进行轨迹规划的目标区域，并在所述目标区域中设置多条评估线。
本发明实施例中，可以预先设置轨迹规划区域规则，如规划车辆前方设定距离范围内的轨迹；所述设定距离可以按照需求设置，如80m，本发明实施例对此不作限制。
然后可以根据当前位置和预先设置的轨迹规划区域规则，确定车辆前方待进行轨迹规划的区域；其中，为了便于后续说明，可以将车辆前方待进行轨迹规划的区域成为目标区域。
然后可以在所述目标区域中设置多条评估线；其中一种实现方式可以是：将目标区域中的障碍物映射至Frenet坐标系中，并在Frenet坐标系的目标区域中按照第一预设步长设置多条评估线。
本发明实施例中，车辆的位置、目标区域、路径等都是在笛卡尔坐标下确定的。为了快速的确定原始路径，本发明实施例可以将目标区域中的障碍物由笛卡尔坐标系(如图3a所示)，映射到Frenet坐标系(如图3b所示)。其中，可以确定障碍物在笛卡尔坐标系中投影到道路参考线上投影点，然后确定该投影点切线的垂线；然后基于该投影点切线的垂线，将目标区域中障碍物由笛卡尔坐标系映射到Frenet坐标系。当然，也可以将车辆/车道线由笛卡尔坐标系映射到Frenet坐标系；车辆/车道线映射方式与障碍物的映射方式类似，在此不再赘述。所述目标区域中的障碍物可以包括静态障碍物和动态障碍物；其中，可以将目标区域中的静态障碍物由笛卡尔坐标系，映射到Frenet坐标系。
然后可以在Frenet坐标系的目标区域中，按照第一预设步长设置多条评估线(如图3b所示)。所述第一预设步长按照需求设置，如20m，本发明实施例对此不作限制；其中，图3b中设置了3条评估线。
步骤204、分别对各条评估线进行采样，确定各条评估线上的评估点。
其中，步骤204的一种实现方式可以是：基于Frenet坐标系中障碍物的位置和车辆宽度分别对各条评估线进行采样，确定所述评估线上的多个评估点；具体可以参照如下子步骤2042-子步骤2046：
子步骤2042、针对一条评估线，基于Frenet坐标系中障碍物的位置和车辆宽度，确定所述评估线上可行驶部分的边界点。
子步骤2044、根据所述评估线上可行驶部分的边界点确定所述评估线上的中点。
子步骤2046、从所述评估线上可行驶部分的中点开始，按照第二预设步长向所述评估线的边界点进行采样，得到所述评估线上的多个评估点。
其中，可以当车辆中心与障碍物的距离大于或等于车辆宽度的一半时，车辆不会与障碍物发生碰撞；以及当车辆中心与车道线的距离大于或等于车辆宽度的一半时，车辆也不会压在车道线上。因此针对一条评估线，可以在该条评估线上查找两个边界点；这两个边界点之间的距离大于或等于车辆宽度，其中一个边界点与障碍物的距离大于或等于车辆宽度的一半，另一个边界点与车道线的距离大于或等于车辆宽度的一半。这两个边界点之间即为所述评估线上的可行驶部分；然后可以将这两个边界点之间的中点，作为该评估线上可行驶部分的中点。再以该评估线上可行驶部分的中点为起点，按照第二预设步长分别向这两个边界点扩展进行采样；每按照第二预设步长向其中任意一个个边界点扩展一次，可以进行一次采样，可以得到该评估线上的一个评估点。其中，所述中点也可以作为评估点。所述第二预设步长可以按照需求设置，如0.2m，本发明实施例对此不作限制。
其中，每个评估点在Frenet坐标系的位置信息p(s,l)可以采用如下形式表示：
p(s,l)＝[xp(s,l),yp(s,l),θp(s,l),κp(s,l)]
其中，(xp(s,l),yp(s,l))为该评估点在Frenet坐标系下的坐标信息，θp(s,l)为该评估点在Frenet坐标系下的航向角，κp(s,l)为该评估点在Frenet坐标系下的曲率，s为该评估点在笛卡尔坐标系下，在道路参考线上对应投影点的弧长，l为该评估点在笛卡尔坐标系下，与对应投影点在投影点切线的垂线方向上的距离。
其中，道路参考线也就是道路中心线，在笛卡尔坐标系中道路参考线的每个点的位置信息r0(s)可以表示为：
r0(s)＝[xr0(s),yr0(s),θr0(s),κr0(s)]
其中，(xr0(s),yr0(s))为该评估点在笛卡尔坐标系下的坐标信息，θr0(s)为该评估点在笛卡尔坐标系下的航向角，κr0(s)为该评估点在笛卡尔坐标系下的曲率，s为该评估点在笛卡尔坐标系下，在道路参考线上对应投影点的弧长。
其中，Frenet坐标系下与笛卡尔坐标系下该评估点各个参数的对应关系如下：
xp(s,l)＝xr0(s)+l*cos(θr0(s)+π/2)
yp(s,l)＝yo0(s)+l*sin(θr0(s)+π/2)
θp(s,l)＝θr0(s)
Figure BDA0002582241420000111
步骤206、根据车辆当前位置和各条评估线上评估点的位置，生成相邻两条评估线之间的路径，所述相邻两条评估线之间包括多条路径。
其中，相邻两个评估线中，其中一条评估线上的任一评估点，可以与另一条评估线上的任一评估点连接，生成一条路径；对应的，相邻两条评估线之间包括多条路径。当然，任一评估线上的一个评估点，均可以与车辆当前位置对应的点连接，生成一条路径；对应的，任意一条评估线与车辆当前位置之间也可以包括多条路径。可以参照图3c所示，图3c中的一条评估线上仅示出了两个评估点；除最左侧的一辆车外，其它的一辆车对应一个评估点。
步骤208、确定各相邻两条评估线之间的最优路径，基于各相邻两条评估线之间的最优路径确定原始路径。
本发明实施例中，针对每一对相邻的两条评估线，可以确定这两条评估线之间最优路径；然后分别采用每一对相邻的两条评估线之间最优路径，生成目标区域内的最优路径，也就是原始路径。
其中，确定各相邻两条评估线之间的最优路径，可以参照如下子步骤S2082-子步骤S2086：
S2082、针对一对相邻两条评估线之间的一条路径，计算所述路径对应的路径成本、静态障碍物成本和动态障碍物成本。
S2084、依据所述路径成本、静态障碍物成本和动态障碍物成本，确定所述路径对应的成本。
S2086、将所述一对相邻两条评估线之间各条路径中成本最小的路径，确定为所述一对相邻两条评估线之间的最优路径。
其中，针对一对相邻两条评估线之间的一条路径，可以计算该路径对应的路径成本、静态障碍物成本和动态障碍物成本。
本发明一个实施例中，针对一对相邻两条评估线之间的一条路径，可以对该路径进行离散采样，得到对应评估采样点。
其中，路径成本的计算方式可以是：计算该路径上后一个评估采样点，到该评估采样点在车道参考线上投影点的侧向偏移，该路径的路径长度，该路径上后一个评估采样点的曲率和曲率导数；基于所述侧向偏移、路径长度、曲率和曲率导数，确定该路径对应的路径成本。当然还可以计算与路径相关的参数来确定路径成本，本发明实施例对此不作限制。
其中，静态障碍物成本的计算方式可以是：根据该路径上后一个评估采样点的位置和方向(航向角)，计算包围车辆的多边形与静态障碍物的最小距离；依据所述最小距离，确定该路径对应的静态障碍物成本。其中，若最小距离为0，则说明车辆会与静态障碍物发生碰撞，此时可以将该评估采样点之后的评估点丢弃，后续无需计算评估采样点之后的评估点与相邻评估线上的其他评估采样点之间路径的成本。
其中，动态障碍物成本的计算方式可以是：根据该路径上后一个评估采样点的位置，确定上一次确定的优化轨迹上，该路径上后一个评估采样点对应的第一目标位置和目标时间。根据动态障碍物的预测轨迹，确定动态障碍物在目标时间所到达的第二目标位置。然后根据第一目标位置和第一目标位置车辆的方向，计算包围车辆的多边形与动态障碍物的第二目标位置的最小距离。依据所述最小距离，确定该路径对应的动态障碍物成本。其中，若最小距离为0，则说明车辆会与动态障碍物发生碰撞，此时可以将该评估采样点之后的评估点丢弃，后续无需计算评估采样点之后的评估点与相邻评估线上的其他评估采样点之间路径的成本。以及，还可以记录会与车辆发生碰撞的动态障碍物。
然后可以将路径成本、静态障碍物成本和动态障碍物成本进行加权计算，确定该路径对应的成本；然后将所述一对相邻两条评估线之间各条路径中成本最小的路径，确定为所述一对相邻两条评估线之间的最优路径。进而采用各对相邻两条评估线之间最优路径，可以生成原始路径。
步骤210、对所述原始路径进行离散采样得到多个采样点。
由于原始路径上评估采样点之间的间距较小，不仅对路径优化的效果没有提升，而且还会增加路径优化的复杂度；因此本发明实施例确定原始路径后，可以对所述原始路径进行离散采样，得到多个采样点。其中，原始路径上相邻两个采样点之间的距离，大于上述相邻两条评估线上评估采样点之间的距离。
然后确定各采样点对应的最大可行驶区域，可以参照步骤212-步骤214。
步骤212、针对一个采样点，确定所述采样点处可包围车辆的多边形。
步骤214、按照第三预设步长将所述多边形的各边向对应的方向扩张，直到所述多边形的任一边与所述目标区域内的障碍物/道路边界碰撞为止，并将停止扩展的多边形对应的区域，确定为所述采样点对应的最大可行驶区域。
现以一个采样点为例，对如何确定该采样点对应的最大可行驶区域进行说明。
本发明实施例中，可以根据该采样点处的位置信息，确定该采样点处可包围车辆的多边形。其中，可以将该采样点的坐标信息作为车辆的中心，然后依据车辆的尺寸和该采样点对应的航向角，确定该车辆所覆盖的车辆区域。再基于车辆所述覆盖的车辆区域，确定该采样点处可包围车辆的多边形。
本发明的一个示例中，一个采样点处可包围车辆的多边形可参照图3d的1所示，该多边形可以是四边形。
然后可以基于该多边形，确定该采样点对应的最大可行驶区域。其中，可以按照第三预设步长，分别将所述多边形的各边向对应的方向扩张，直到所述多边形的任一边与所述目标区域内的障碍物/道路边界碰撞为止。其中，针对每个边，可以将多边形中心与该边的垂线方向，作为与该边对应的方向；扩张多边形各边的过程，可以参照图3d中的1→2→3所示。所述第三预设步长可以按照需求设置，本发明实施例对此不作限制。
其中，所述最大可行驶区域在笛卡尔坐标系下确定，所述多边形也在笛卡尔坐标系下确定。
在停止扩展该多边形后，可以将停止扩展时多边形对应的区域，确定为所述采样点对应的最大可行驶区域。其中，扩展后的多边形可以采用线性约束条件AX≤b描述，对应的，每个采样点对应的最大可行驶区域，也可以采用线性约束条件AX≤b描述；然后可以依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径，可以参照步骤216-步骤220：
步骤216、确定目标路径函数，以及将各最大可行驶区域对应的线性约束条件作为所述目标路径函数的约束条件。
步骤218、以最小化目标路径函数为目标对目标路径函数进行优化，确定各最大可行驶区域内位置最优的节点。
步骤220、依据各最大可行驶区域内位置最优的节点，生成优化路径。
本发明实施例中，当采样点的数量大于数量阈值时，可以按照需求确定目标路径函数；然后可以通过优化该目标路径函数对原始路径进行优化，确定对应的优化路径。当采样点的数量小于或等于数量阈值时，可以不对原始路径进行优化。其中，所述数量阈值可以按照需求设置，本发明实施例对此不作限制。
本发明的一个示例中，目标路径函数可以包括：采样点到参考点的距离项、路径长度项、路径平滑性项；当然目标路径还可以包括其他项，可以按照需求设置，本发明实施例对此不作限制。其中，所述参考点可以是指采样点到道路参考线上的投影点。
然后可以将各最大可行驶区域对应的线性约束条件作为所述目标路径函数的约束条件，以及还可以将车辆的当前位置作为约束条件；再以最小化目标路径函数为目标对目标路径函数进行优化，确定各最大可行驶区域内位置最优的节点。其中，每个最大可行驶区域内位置最优的节点可能是该最大可行驶区域对应的采样点。
然后采用各最大可行驶区域内位置最优的节点，生成优化路径；其中，可以对相邻两个节点进行路径拟合，生成相邻两个节点之间的路径，进而得到优化路径。
本发明实施例中，在生成优化路径后，可以基于所述优化路径进行速度优化，得到速度优化曲线；若未对原始路径进行优化，则可以基于所述原始路径进行速度优化，得到速度优化曲线。
以下以基于所述优化路径进行速度优化，得到速度优化曲线为例进行说明。
本发明实施例中，优化路径可能是完整的路径，也可能不是完整的路径。其中，所述完整的路径可以是指优化路径的最后一个节点属于最后一个采样点对应的最大可行驶区域。其中，优化路径是完整路径和不是完整路径时，对应的速度优化方法不同。可以参照步骤222-步骤224：
步骤222、若所述优化路径的最后一个节点属于最后一个采样点对应的最大可行驶区域，则依参考速度曲线进行速度优化，得到速度优化曲线。
若所述优化路径的最后一个节点属于最后一个采样点对应的最大可行驶区域，则可以确定道路参考线上的曲率对应的推荐速度、道路限速、道路推荐速度、以及根据车辆运动极限得到的极限速度；然后依据所述曲率对应的推荐速度、道路限速、道路推荐速度和极限速度，确定确定参考速度曲线。然后基于参考速度曲线进行速度优化，确定对应的速度优化曲线。
其中，当未对原始路径进行优化时，可以按照步骤222，基于原始路径进行优化，确定原始路径对应的速度优化曲线。
步骤224、若所述优化路径的最后一个采样点不属于最后一个采样点对应的最大可行驶区域，则获取若车辆按照任一对相邻两个可行驶子区域之间的路径行驶的情况下，与车辆发生碰撞的目标动态障碍物；依据所述优化路径将所述目标动态障碍物投影至弧度-时间图，并基于投影后的目标态障碍物进行跟随的速度优化，得到速度优化曲线。
若所述优化路径的最后一个采样点不属于最后一个采样点对应的最大可行驶区域，则可以获取若车辆按照任一对相邻两条评估线之间的路径行驶的情况下，与车辆发生碰撞的目标动态障碍物；也就是上述在计算对相邻两条评估线之间路径的动态障碍物成本时，确定的与车辆发生碰撞的动态障碍物。然后可以基于所述优化路径将所述目标动态障碍物投影至弧度-时间图中，再通过优化目标速度函数实现速度优化。一个示例中，所述目标速度函数可以包括：优化路径上节点到参考速度曲线的距离项、速度优化曲线的长度项、速度优化曲线的平滑性项；其中，速度优化曲线也就是进行速度优先所需得到的曲线。然后可以将该目标速度函数的约束条件被设置车辆当前位置的约束，以及优化路径中节点位置的约束；再以最小化目标速度函数为目标对目标速度函数进行优化，确定优化路径中每个节点对应的优化速度信息，进而可以得到速度优化曲线。
步骤226、将所述优化路径与速度优化曲线进行合并，得到优化轨迹。
其中，在确定优化路径和速度优化曲线后，可以将优化路径和速度优化曲线进行合并，得到优化轨迹。当然，当未对原始路径进行优化时，可以将原始路径和速度优化曲线进行合并，得到对应的优化轨迹。
综上，本发明实施例中，可以将目标区域中的障碍物映射至Frenet坐标系中，并在Frenet坐标系的目标区域中按照第一预设步长设置多条评估线；然后针对每一条评估线，基于Frenet坐标系中障碍物的位置和车辆宽度，确定所述评估线上的多个评估点；相对于在笛卡尔坐标系中确定评估线和评估点而言，本发明实施例进而通过将目标区域中的障碍物映射至Frenet坐标系确定评估线和评估点，能够降低确定评估线和评估点的难度，从而提高确定原始路径的效率。
其次，本发明实施例中，可以针对一对相邻两条评估线之间的一条路径，计算所述路径对应的路径成本、静态障碍物成本和动态障碍物成本，并依据所述路径成本、静态障碍物成本和动态障碍物成本，确定所述路径对应的成本；将所述一对相邻两条评估线之间各条路径中成本最小的路径，确定为所述一对相邻两条评估线之间的最优路径；然后基于各相邻两条评估线之间的最优路径确定原始路径，进而能够将成本最小的路径确定为原始路径，以提高路径优化效率。
进一步，本发明实施例中，针对一个采样点，确定所述采样点处可包围车辆的多边形，然后按照第三预设步长将所述多边形的各边向对应的方向扩张，直到所述多边形的任一边与所述目标区域内的障碍物/道路边界碰撞为止；并将停止扩展时的多边形对应的区域，确定为所述采样点对应的最大可行驶区域；进而能够快速的完成非凸的碰撞检测，提高路径优化效率。
再次，所述最大可行驶区域在笛卡尔坐标系下确定，能够避免将障碍物位置投影到Frenet坐标系下，而引起的严重变形而导致丢失大量的可行驶区域的问题。
需要说明的是，对于方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明实施例并不受所描述的动作顺序的限制，因为依据本发明实施例，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作并不一定是本发明实施例所必须的。
参照图4，示出了本发明一种轨迹优化装置实施例的结构框图，具体可以包括如下模块：
路径确定模块402，用于确定原始路径；
区域确定模块404，用于对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域，其中，所述最大可行驶区域采用线性约束条件描述；
路径优化模块406，用于依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径；
速度优化模块408，用于基于所述优化路径进行速度优化，得到速度优化曲线；
合并模块410，用于将所述优化路径与速度优化曲线进行合并，得到优化轨迹。
参照图5，示出了本发明一种轨迹优化装置可选实施例的结构框图。
本发明一个可选实施例中，所述路径确定模块402，包括：
评估线确定子模块4022，用于根据车辆当前位置确定车辆前方待进行轨迹规划的目标区域，并在所述目标区域中设置多条评估线；
评估点确定子模块4024，用于分别对各条评估线进行采样，确定各条评估线上的评估点；
路径生成子模块4026，用于根据车辆当前位置和各条评估线上评估点的位置，生成相邻两条评估线之间的路径，所述相邻两条评估线之间包括多条路径；
原始路径确定子模块4028，用于确定各相邻两条评估线之间的最优路径，基于各相邻两条评估线之间的最优路径确定原始路径。
本发明一个可选实施例中，所述评估线确定子模块4022，用于将目标区域中的障碍物映射至Frenet坐标系中，并在Frenet坐标系的目标区域中按照第一预设步长设置多条评估线；所述评估点确定子模块4024，用于基于Frenet坐标系中障碍物的位置和车辆宽度分别对各条评估线进行采样，确定所述评估线上的多个评估点。
本发明一个可选实施例中，所述评估点确定子模块4024，具体用于针对一条评估线，基于Frenet坐标系中障碍物的位置和车辆宽度，确定所述评估线上可行驶部分的边界点；根据所述评估线上可行驶部分的边界点确定所述评估线上的中点；从所述评估线上可行驶部分的中点开始，按照第二预设步长向所述评估线的边界点进行采样，得到所述评估线上的多个评估点。
本发明一个可选实施例中，所述原始路径确定子模块4028，包括：
成本计算单元40282，用于针对一对相邻两条评估线之间的一条路径，计算所述路径对应的路径成本、静态障碍物成本和动态障碍物成本；
成本确定单元40284，用于依据所述路径成本、静态障碍物成本和动态障碍物成本，确定所述路径对应的成本；
最优路径确定单元40286，用于将所述一对相邻两条评估线之间各条路径中成本最小的路径，确定为所述一对相邻两条评估线之间的最优路径。
本发明一个可选实施例中，所述区域确定模块404，用于针对一个采样点，确定所述采样点处可包围车辆的多边形；按照第三预设步长将所述多边形的各边向对应的方向扩张，直到所述多边形的任一边与所述目标区域内的障碍物/道路边界碰撞为止，并将停止扩展时多边形对应的区域，确定为所述采样点对应的最大可行驶区域。
本发明一个可选实施例中，所述路径优化模块406，包括：
函数确定子模块4062，用于确定目标路径函数，以及将各最大可行驶区域对应的线性约束条件作为所述目标路径函数的约束条件；
节点确定子模块4064，用于以最小化目标路径函数为目标对目标路径函数进行优化，确定各最大可行驶区域内位置最优的节点；
优化路径生成子模块4066，用于依据各最大可行驶区域内位置最优的节点，生成优化路径。
本发明一个可选实施例中，所述速度优化模块408，包括：
第一速度优化曲线生成子模块4082，用于若所述优化路径的最后一个节点属于最后一个采样点对应的最大可行驶区域，则依据参考速度曲线进行速度优化，得到速度优化曲线；
第二速度优化曲线生成子模块4084，用于若所述优化路径的最后一个采样点不属于最后一个采样点对应的最大可行驶区域，则获取若车辆按照任一对相邻两个可行驶子区域之间的路径行驶的情况下，与车辆发生碰撞的目标动态障碍物；依据所述优化路径将所述目标动态障碍物投影至弧度-时间图，并基于投影后的目标态障碍物进行跟随的速度优化，得到速度优化曲线。
本发明一个可选实施例中，所述最大可行驶区域在笛卡尔坐标系下确定。
综上，本发明实施例中，可以在确定原始路径后，对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域；然后依据用于描述最大可行驶区域的线性约束条件对所述原始路径进行路径优化，得到优化路径；相对于计算两个凸多边形的距离而言，计算最大可行驶区域的复杂度更低，进而能够快速的实现路径优化；再基于所述优化路径进行速度优化，得到速度优化曲线；并将所述优化路径与速度优化曲线进行合并，得到优化轨迹；进而能够快速的进行轨迹优化，提高轨迹优化的效率。
对于装置实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
本发明实施例还提供了一种可读存储介质，当所述存储介质中的指令由电子设备的处理器执行时，使得电子设备能够执行如本发明实施例任一所述的轨迹优化方法。
本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
本领域内的技术人员应明白，本发明实施例的实施例可提供为方法、装置、或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本发明实施例的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。
最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
以上对本发明所提供的一种轨迹优化方法和一种轨迹优化装置，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种轨迹优化方法，其特征在于，所述的方法包括：
确定原始路径；
对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域，其中，所述最大可行驶区域采用线性约束条件描述；
依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径；
基于所述优化路径进行速度优化，得到速度优化曲线；
将所述优化路径与速度优化曲线进行合并，得到优化轨迹。
2.根据权利要求1所述的方法，其特征在于，所述确定原始路径，包括：
根据车辆当前位置确定车辆前方待进行轨迹规划的目标区域，并在所述目标区域中设置多条评估线；
分别对各条所述评估线进行采样，确定各条所述评估线上的评估点；
根据车辆当前位置和各条所述评估线上评估点的位置，生成相邻两条所述评估线之间的路径，所述相邻两条所述评估线之间包括多条路径；
确定各相邻两条所述评估线之间的最优路径，基于各相邻两条所述评估线之间的最优路径确定原始路径。
3.根据权利要求2所述的方法，其特征在于，
所述在所述目标区域中设置多条评估线，包括：
将目标区域中的障碍物映射至Frenet坐标系中，并在Frenet坐标系的目标区域中按照第一预设步长设置多条评估线；
所述分别对各条所述评估线进行采样，确定各条所述评估线上的评估点，包括：
基于Frenet坐标系中障碍物的位置和车辆宽度分别对各条所述评估线进行采样，确定各条所述评估线上的多个评估点。
4.根据权利要求3所述的方法，其特征在于，所述基于Frenet坐标系中障碍物的位置和车辆宽度分别对各条所述评估线进行采样，确定各条所述评估线上的多个评估点，包括：
针对一条评估线，基于Frenet坐标系中障碍物的位置和车辆宽度，确定所述评估线上可行驶部分的边界点；
根据所述评估线上可行驶部分的边界点确定所述评估线上的中点；
从所述评估线上可行驶部分的中点开始，按照第二预设步长向所述评估线的边界点进行采样，得到所述评估线上的多个评估点。
5.根据权利要求2所述的方法，其特征在于，所述确定各相邻两条所述评估线之间的最优路径，包括：
针对一对相邻两条所述评估线之间的一条路径，计算所述路径对应的路径成本、静态障碍物成本和动态障碍物成本；
依据所述路径成本、静态障碍物成本和动态障碍物成本，确定所述路径对应的成本；
将所述一对相邻两条所述评估线之间各条路径中成本最小的路径，确定为所述一对相邻两条所述评估线之间的最优路径。
6.根据权利要求2所述的方法，其特征在于，所述确定各采样点对应的最大可行驶区域，包括：
针对一个采样点，确定所述采样点处可包围车辆的多边形；
按照第三预设步长将所述多边形的各边向对应的方向扩张，直到所述多边形的任一边与所述目标区域内的障碍物/道路边界碰撞为止，并将停止扩展时多边形对应的区域，确定为所述采样点对应的最大可行驶区域。
7.根据权利要求1所述的方法，其特征在于，所述依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径，包括：
确定目标路径函数，以及将各最大可行驶区域对应的线性约束条件作为所述目标路径函数的约束条件；
以最小化目标路径函数为目标对目标路径函数进行优化，确定各最大可行驶区域内位置最优的节点；
依据各最大可行驶区域内位置最优的节点，生成优化路径。
8.根据权利要求7所述的方法，其特征在于，所述基于所述优化路径进行速度优化，得到速度优化曲线，包括：
若所述优化路径的最后一个节点属于最后一个采样点对应的最大可行驶区域，则依据参考速度曲线进行速度优化，得到速度优化曲线；
若所述优化路径的最后一个采样点不属于最后一个采样点对应的最大可行驶区域，则获取若车辆按照任一对相邻两个可行驶子区域之间的路径行驶的情况下，与车辆发生碰撞的目标动态障碍物；依据所述优化路径将所述目标动态障碍物投影至弧度-时间图，并基于投影后的目标态障碍物进行跟随的速度优化，得到速度优化曲线。
9.一种轨迹优化装置，其特征在于，所述的装置包括：
路径确定模块，用于确定原始路径；
区域确定模块，用于对所述原始路径进行离散采样得到多个采样点，并确定各采样点对应的最大可行驶区域，其中，所述最大可行驶区域采用线性约束条件描述；
路径优化模块，用于依据所述线性约束条件对所述原始路径进行路径优化，得到优化路径；
速度优化模块，用于基于所述优化路径进行速度优化，得到速度优化曲线；
合并模块，用于将所述优化路径与速度优化曲线进行合并，得到优化轨迹。
10.一种可读存储介质，其特征在于，当所述存储介质中的指令由电子设备的处理器执行时，使得电子设备能够执行如方法权利要求1-8任一所述的轨迹优化方法。
CN202010670920.XA 2020-07-13 2020-07-13 一种轨迹优化方法和装置 Active CN111830979B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010670920.XA CN111830979B (zh) 2020-07-13 2020-07-13 一种轨迹优化方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010670920.XA CN111830979B (zh) 2020-07-13 2020-07-13 一种轨迹优化方法和装置

Publications (2)

Publication Number Publication Date
CN111830979A true CN111830979A (zh) 2020-10-27
CN111830979B CN111830979B (zh) 2024-06-07

Family

ID=72922708

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010670920.XA Active CN111830979B (zh) 2020-07-13 2020-07-13 一种轨迹优化方法和装置

Country Status (1)

Country Link
CN (1) CN111830979B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112632208A (zh) * 2020-12-25 2021-04-09 际络科技(上海)有限公司 一种交通流轨迹变形方法及装置
CN112859845A (zh) * 2021-01-04 2021-05-28 广州小鹏自动驾驶科技有限公司 一种轨迹的规划方法、装置和车辆
CN113551679A (zh) * 2021-07-23 2021-10-26 杭州海康威视数字技术股份有限公司 一种示教过程中的地图信息构建方法、构建装置
CN113561992A (zh) * 2021-07-30 2021-10-29 广州文远知行科技有限公司 自动驾驶车辆轨迹生成方法、装置、终端设备及介质
CN113650608A (zh) * 2021-09-14 2021-11-16 广州小鹏自动驾驶科技有限公司 一种基于自动驾驶的避障方法和装置
CN113654556A (zh) * 2021-07-05 2021-11-16 的卢技术有限公司 一种基于改进em算法的局部路径规划方法、介质及设备
CN114061606A (zh) * 2021-11-10 2022-02-18 京东鲲鹏(江苏)科技有限公司 路径规划方法、装置、电子设备及存储介质
CN114506343A (zh) * 2022-03-02 2022-05-17 阿波罗智能技术(北京)有限公司 轨迹规划方法、装置、设备、存储介质及自动驾驶车辆
WO2022199855A1 (en) * 2021-03-26 2022-09-29 Embotech Ag Method and system for controlling autonomous or semi-autonomous vehicle
US20220402527A1 (en) * 2021-06-22 2022-12-22 Waymo Llc Modeling positional uncertainty of moving objects using precomputed polygons
CN116974288A (zh) * 2023-09-20 2023-10-31 深圳鹏行智能研究有限公司 机器人控制方法以及机器人

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102207736A (zh) * 2010-03-31 2011-10-05 中国科学院自动化研究所 基于贝塞尔曲线的机器人路径规划方法及装置
CN108891412A (zh) * 2018-07-19 2018-11-27 吉林大学 一种平行泊车的路径规划方法
CN109976355A (zh) * 2019-04-26 2019-07-05 腾讯科技（深圳）有限公司 轨迹规划方法、系统、设备及存储介质
CN110648529A (zh) * 2019-09-16 2020-01-03 腾讯大地通途（北京）科技有限公司 一种车辆行驶轨迹的渲染方法、装置、设备及存储介质
CA3048689A1 (fr) * 2018-07-11 2020-01-11 Dassault Aviation Systeme de calcul de mission d'un aeronef par combinaison d'algorithmes et procede associe
CN111123927A (zh) * 2019-12-20 2020-05-08 北京三快在线科技有限公司 轨迹规划方法、装置、自动驾驶设备和存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102207736A (zh) * 2010-03-31 2011-10-05 中国科学院自动化研究所 基于贝塞尔曲线的机器人路径规划方法及装置
CA3048689A1 (fr) * 2018-07-11 2020-01-11 Dassault Aviation Systeme de calcul de mission d'un aeronef par combinaison d'algorithmes et procede associe
CN108891412A (zh) * 2018-07-19 2018-11-27 吉林大学 一种平行泊车的路径规划方法
CN109976355A (zh) * 2019-04-26 2019-07-05 腾讯科技（深圳）有限公司 轨迹规划方法、系统、设备及存储介质
CN110648529A (zh) * 2019-09-16 2020-01-03 腾讯大地通途（北京）科技有限公司 一种车辆行驶轨迹的渲染方法、装置、设备及存储介质
CN111123927A (zh) * 2019-12-20 2020-05-08 北京三快在线科技有限公司 轨迹规划方法、装置、自动驾驶设备和存储介质

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112632208A (zh) * 2020-12-25 2021-04-09 际络科技(上海)有限公司 一种交通流轨迹变形方法及装置
CN112859845A (zh) * 2021-01-04 2021-05-28 广州小鹏自动驾驶科技有限公司 一种轨迹的规划方法、装置和车辆
WO2022199855A1 (en) * 2021-03-26 2022-09-29 Embotech Ag Method and system for controlling autonomous or semi-autonomous vehicle
US11753045B2 (en) * 2021-06-22 2023-09-12 Waymo Llc Modeling positional uncertainty of moving objects using precomputed polygons
US20220402527A1 (en) * 2021-06-22 2022-12-22 Waymo Llc Modeling positional uncertainty of moving objects using precomputed polygons
CN113654556A (zh) * 2021-07-05 2021-11-16 的卢技术有限公司 一种基于改进em算法的局部路径规划方法、介质及设备
CN113551679A (zh) * 2021-07-23 2021-10-26 杭州海康威视数字技术股份有限公司 一种示教过程中的地图信息构建方法、构建装置
CN113561992A (zh) * 2021-07-30 2021-10-29 广州文远知行科技有限公司 自动驾驶车辆轨迹生成方法、装置、终端设备及介质
CN113561992B (zh) * 2021-07-30 2023-10-20 广州文远知行科技有限公司 自动驾驶车辆轨迹生成方法、装置、终端设备及介质
CN113650608A (zh) * 2021-09-14 2021-11-16 广州小鹏自动驾驶科技有限公司 一种基于自动驾驶的避障方法和装置
CN114061606A (zh) * 2021-11-10 2022-02-18 京东鲲鹏(江苏)科技有限公司 路径规划方法、装置、电子设备及存储介质
CN114061606B (zh) * 2021-11-10 2024-04-12 京东鲲鹏(江苏)科技有限公司 路径规划方法、装置、电子设备及存储介质
CN114506343A (zh) * 2022-03-02 2022-05-17 阿波罗智能技术(北京)有限公司 轨迹规划方法、装置、设备、存储介质及自动驾驶车辆
CN116974288A (zh) * 2023-09-20 2023-10-31 深圳鹏行智能研究有限公司 机器人控制方法以及机器人
CN116974288B (zh) * 2023-09-20 2024-03-15 深圳鹏行智能研究有限公司 机器人控制方法以及机器人

Also Published As

Publication number Publication date
CN111830979B (zh) 2024-06-07

Similar Documents

Publication Publication Date Title
CN111830979A (zh) 一种轨迹优化方法和装置
CN112068545B (zh) 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质
Stahl et al. Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios
EP3517893B1 (en) Path and speed optimization fallback mechanism for autonomous vehicles
EP3524934B1 (en) Systems and methods for determining a projection of an obstacle trajectory onto a reference line of an autonomous vehicle
US10816990B2 (en) Non-blocking boundary for autonomous vehicle planning
US10579062B2 (en) Scalable smooth reference path generator for autonomous driving vehicles
JP6622148B2 (ja) 周辺環境認識装置
US20220080961A1 (en) Control system and control method for sampling based planning of possible trajectories for motor vehicles
CN109955853B (zh) 用于操作自动驾驶车辆的方法、系统和存储介质
Petrich et al. Map-based long term motion prediction for vehicles in traffic environments
US10571916B2 (en) Control method for autonomous vehicles
US20190317509A1 (en) Novel method on moving obstacle representation for trajectory planning
CN112284393B (zh) 一种智能移动机器人全局路径规划方法和系统
JP7299976B2 (ja) 環境ターゲットの特徴点抽出方法及び装置
US11414096B2 (en) QP spline path and spiral path based reference line smoothing method for autonomous driving
JPWO2018047292A1 (ja) 車両の走行制御方法および走行制御装置
JP7125286B2 (ja) 行動予測装置及び自動運転装置
CN113670305A (zh) 泊车轨迹生成方法、装置、计算机设备和存储介质
CN115309163A (zh) 基于改进方向评价函数dwa算法的局部路径规划方法
CN115077553A (zh) 基于栅格搜索轨迹规划方法、系统、汽车、设备及介质
Park et al. Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles
KR20170070488A (ko) 주행경로 자동생성방법 및 장치
CN116476840A (zh) 变道行驶方法、装置、设备及存储介质
CN112644487B (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
CB02 Change of applicant information

Address after: Room 46, room 406, No.1, Yichuang street, Zhongxin knowledge city, Huangpu District, Guangzhou City, Guangdong Province 510000

Applicant after: Guangzhou Xiaopeng Automatic Driving Technology Co.,Ltd.

Address before: Room 46, room 406, No.1, Yichuang street, Zhongxin knowledge city, Huangpu District, Guangzhou City, Guangdong Province 510000

Applicant before: Guangzhou Xiaopeng Internet of vehicles Technology Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant