CN116907532A - 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 - Google Patents
无人驾驶车辆窄路三段调头路径规划方法、装置及设备 Download PDFInfo
- Publication number
- CN116907532A CN116907532A CN202311169700.9A CN202311169700A CN116907532A CN 116907532 A CN116907532 A CN 116907532A CN 202311169700 A CN202311169700 A CN 202311169700A CN 116907532 A CN116907532 A CN 116907532A
- Authority
- CN
- China
- Prior art keywords
- path
- turning
- arc
- section
- circular arc
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 82
- 238000013439 planning Methods 0.000 title claims abstract description 50
- 230000006870 function Effects 0.000 claims description 25
- 238000004590 computer program Methods 0.000 claims description 7
- 238000005516 engineering process Methods 0.000 abstract description 11
- 238000004364 calculation method Methods 0.000 abstract description 9
- 238000004422 calculation algorithm Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 12
- 230000033001 locomotion Effects 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 238000005457 optimization Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000004140 cleaning Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000009499 grossing Methods 0.000 description 3
- 238000007726 management method Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 241001417527 Pempheridae Species 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000003542 behavioural effect Effects 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000007599 discharging Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000000474 nursing effect Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 231100000817 safety factor Toxicity 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000012384 transportation and delivery Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种无人驾驶车辆窄路三段调头路径规划方法、装置及设备，以车辆感知、定位、底盘数据为基础，通过生成驶入圆弧路径，生成三段圆弧路径，生成驶出圆弧路径，路径拼接等步骤，提供了一套实用、方便的调头路径生成流程，包含全套流程的数学表达、步骤和实现方法。本发明适用于不同车辆的调头路径生成，特别是在狭窄路宽生成调头路径，可解决现有调头路径生成方法中需要路宽过大、平滑计算耗时大、路径生成结果不符合驾驶逻辑的问题，为自动驾驶车辆在调头场景提供了一套可靠、实用的解决方案，促进自动驾驶技术的进一步落地与应用。
Description
技术领域
本发明涉及自动驾驶技术领域，尤其涉及一种无人驾驶车辆窄路三段调头路径规划方法、装置及设备。
背景技术
近年来，自动驾驶技术得到发展和落地，它被认为是智慧交通建设、城市交通服务水平提升的关键环节。在自动化和数字化技术的背景下，自动驾驶车辆开始承担起出行接驳、包裹配送、街道清洁和植被护理等城市服务功能。自动驾驶系统的关键技术主要由四个部分组成，分别为：环境感知、行为决策、路径规划和运动控制。基于周边环境信息，路径规划会实现车身位置到目标位置的规划，生成安全、舒适、可控的路径，是自动驾驶系统中承上启下的一环。
自动驾驶车辆经常会遇到在封闭或者半封闭的道路中调头的需求，因此车辆调头路径方法对于自动驾驶的应用落地有重要意义。调头场景路径规划的难点包括但不限于以下几点：
1、空间限制：调头场景中车辆需要倒车并转弯，需要考虑周围环境的限制，如路宽、路堤、路缘石等。例如，在环卫行业中，环卫车通常在狭窄街道或非机动车道进行作业，且需要频繁进行调头操作以完成清扫任务，要求无人清扫车具备在较小路宽狭完成调头的能力；
2、平滑性：调头路径往往需要车辆进行较大的连续转向，如果调头路径平滑性不好、甚至超出车辆控制极限，会使控制模块会无法跟上，导致车辆连续点刹甚至停车，对车辆、乘客及周围交通造成影响；
3、安全性：调头过程中需要考虑安全因素，如避免与其他车辆或行人发生碰撞等；
4、实时性：调头过程需要在实时性的要求下完成，避免对交通造成影响；
5、适用性：调头路径的生成需适用于不同车辆类型以及不同场景的需求。
现有技术一中，申请号为： CN202211193749.3，专利名称为《车辆调头控制方法、装置、设备及计算机可读存储介质》的现有技术，请参阅图1，其主要是按照初始调头路径生成下一时刻的目标行驶状态信息，基于目标行驶状态信息，控制车辆在行驶区域内进行调头行驶；该技术主要在控制层面保证调头路径的可控性，并未提及如何在规划层面生成初始调头路径。且该技术中的初始调头路径为“U”型调头路径，是一种在道路上进行全方位转向的操作，使车辆回到原始行驶方向的相反方向，对比三段调头路径，U型调头只有前进段、没有倒车段，完成调头所需的路宽更大，不适用于路宽较小的场地。
现有技术二中，申请号为： CN202010474326.3，专利名称为《自动驾驶车辆三点转弯》的现有技术，请参阅图2，其在开放空间模式下执行三点转弯调头，生成向前和倒车运动。该技术使用Hybrid A*算法规划得出第一轨迹，在第一轨迹的基础上执行两次QP（Quadratic Programming，二次规划）优化，在满足阈值的情况下，找到最优调头路径；Hybrid A* 算法是一种基于图搜索的路径规划算法，它结合了传统A* 算法和车辆动力学模型，用于生成较平滑的路径。但混合A*算法需要在连续状态空间中搜索最优路径，算法时间复杂度高。另一方面，尽管该算法考虑了车辆动力学模型，但生成的路径平滑性通常不能满足控制的需求，需要进一步平滑。对混合A*的路径平滑方法较在原理上较复杂，涉及到线性或非线性的数学优化问题求解，且求解结果的平滑性依赖于优化问题构造是否合理。现有技术二通过两次 QP优化，对混合A*路径进行平滑，引入了较为复杂的数学约束，进一步增加了程序的复杂度。
现有技术三中，申请号为： CN201880008824.X，专利名称为《 基于枚举的自动驾驶车辆的三点转弯规划》的现有技术，请参阅图3，其基于车辆最大前向转弯角生成一组前向转弯路径，基于与车辆最大后向转弯角生成一组后向转弯路径，基于前向转弯路径和后向转弯路径生成一组三点转弯路径候选项。对于每条候选路径，使用预定成本函数计算路径成本，并选择路径成本最低的路径作为最终的三点转弯路径；其在不超过最大向前/向后转弯角的前提下生成的路径，是以不同转弯半径生成的两段圆弧路径（请继续参阅图3中的401与402），可满足控制的要求。但向后路径（402）和新的向前路径（403）的连接处，未对车头朝向的一致性做限制，即在402段和403段的交点处，车头朝向可能不一样，在实际调头场景中不被允许。另一方面，该技术在判断路径的可行性时，仅检查了车辆前进方向与目标车道方向是否差距过大，未考虑路宽、障碍物等其他环境因素的限制，生成路径可能欠缺安全性与可执行性。
现有技术四中，申请号为： CN202011231532.8，专利名称为《一种农机无人驾驶调头方法》的现有技术，请参阅图4，其调头方法分为5个操作段：S1：前进作业直线段1：直线行驶，行驶方向与道路方向平行；S2：前进圆弧段2；S3：倒车直线段3；S4：前进圆弧段4；S5：前进作业直线段5。其中直线段采用欧拉螺旋进行规划，可保证直线与圆弧连接处的曲率连续性，圆弧段则保持固定转弯半径行驶，倒车段垂直与直线段；但是，其技术方案同样不能保证前进段与倒车段交点处车头的一致性，因为该技术未在交点处做车头角度的限制。另一方面，该技术方案在倒车段（3）为直线段（把方向盘打正倒车），而人操作车辆进行倒车时会将方向盘打至一定角度后倒车，倒车为直线段不符合常规驾驶逻辑、且可能会增加调头路宽；另外，倒车段需要与直线段（1、5段）垂直，灵活性欠缺。
因此，现有技术还有待提高。
发明内容
鉴于上述现有技术的不足，本发明的目的在于提供一种无人驾驶车辆窄路三段调头路径规划方法、装置及设备，旨在实现简单实用、安全可执行、平滑、多方面权衡最优的调头路径。
本发明的技术方案如下：
一种无人驾驶车辆窄路三段调头路径规划方法，所述方法包括：
生成驶入圆弧路径；所述驶入圆弧路径包括从车辆当前位置到调头区域起点的路径；
生成三段圆弧路径；所述三段圆弧路径包括第一前进段圆弧路径、倒车段圆弧路径、第二前进段圆弧路径；
生成驶出圆弧路径；所述驶出圆弧路径包括从所述前进段圆弧路径终点到参考线的路径；
路径拼接；将所述驶入圆弧路径、三段圆弧路径以及驶出圆弧路径拼接得到掉头路径。
在一个实施例中，所述驶入圆弧路径与所述驶出圆弧路径均由五次多项式样条曲线进行生成。
在一个实施例中，所述生成驶入圆弧路径与所述驶出圆弧路径具体还包括：
构造一个以多项式二阶导和三阶导为最小值的代价函数的二次规划问题；
设置起点角度约束以及终点角度约束，所述角度为当前车辆位置的航向角；
设置起点位置约束以及终点位置约束，以确保所述驶入圆弧路径通过调头区域的起点；
基于所述代价函数、角度约束以及位置约束，使用求解器求得最优路径作为车辆由当前位置行驶到调头区域起点的最终路径。
在一个实施例中，所述生成三段圆弧路径具体还包括：
通过圆弧差值法得到圆弧路径点；
多个所述圆弧路径点形成圆弧路径；
限制不同段的所述圆弧路径的偏航角。
在一个实施例中，所述起点角度约束以及终点角度约束具体包括：
所述五次多项式样条曲线的起点角度和终点角度分别与车辆当前位置的角度、调头区域起点的角度方向相同。
在一个实施例中，所述路径拼接的方法还包括：
约束所述五次多项式样条曲线与圆弧段连接处的曲率连续性。
在一个实施例中，所述路径拼接之后，所述方法还包括：
判断最优路径；找到所述掉头路径中成本最小的掉头路径，作为最终的最优路径。
相应的，本发明实施例公开了一种无人驾驶车辆窄路三段调头路径规划方法控制装置，所述装置包括：
第一生成单元，用于生成驶入圆弧路径；
第二生成单元，用于生成三段圆弧路径；
第三生成单元，用于生成驶出圆弧路径；
拼接单元，用于将所述驶入圆弧路径、三段圆弧路径以及驶出圆弧路径拼接得到掉头路径；
判断单元，用于判断最优路径；找到所述掉头路径中成本最小的掉头路径，作为最终的最优路径。
相应的，本发明实施例还提供了一种电子设备，包括：处理器、存储器及存储在所述存储器上并能够在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述所述无人驾驶车辆窄路三段调头路径规划方法的步骤。
相应的，本发明实施例还提供了一种计算机存储介质，所述存储介质中存储有至少一条程序指令，所述至少一条程序指令用于被处理器加载并执行以实现一方面上述所述无人驾驶车辆窄路三段调头路径规划方法的步骤。
综上所述：本文提出的一种无人驾驶车辆窄路三段调头路径规划方法，以车辆感知、定位、底盘数据为基础，提供了一套实用、方便的调头路径生成流程，包含全套流程的数学表达、步骤和实现方法。本发明适用于不同车辆的调头路径生成，特别是在狭窄路宽生成调头路径，可解决现有调头路径生成方法中需要路宽过大、平滑计算耗时大、路径生成结果不符合驾驶逻辑的问题，为自动驾驶车辆在调头场景提供了一套可靠、实用的解决方案，促进自动驾驶技术的进一步落地与应用。
与现有技术相比，本发明的有益效果如下：
1、可在路宽较小时生成调头路径，满足特殊作业场景的需求；
自动驾驶车辆在进行调头任务时，某些道路或转弯区域的路宽可能较窄，不足以容纳车辆进行标准的U型转向操作，如无人清扫车在清扫非机动车道时路宽较小，在这种情况下，自动驾驶系统需要具备能够适应狭窄道路的能力，以确保车辆能够安全地完成调头操作。本发明提供了调头路径路宽计算方法，并把调头路宽作为路径成本的一项，用于确定最优路径。通过加大路径成本中的路宽权重，可保证最优调头路径路宽较小，满足小路宽场景的作业需求。
2、提供了一种简单、通用性较高的调头路径生成方法；
简易性：本发明采用采用了简洁直观的几何曲线生成调头路径，即样条曲线通过OSQP等求解器解出，圆弧曲线则通过差值方法得出，相比与Hybrid A*算法生成的调头路径，计算简便、复杂度低且避免了后续复杂的路径平滑过程。
灵活性：本发明中找到了三段圆弧转角之和与驶入、驶出圆弧yaw角的关系，车辆不再需要在开始调头、结束调头位置车头朝向角度相反，减少了路径规划的限制，同时更符合实际道路的调头环境。
通用性：本发明生成调头路径时，用到与车辆相关的参数主要为最小转弯半径；对于不同控制模型，只需重新计算最小转弯半径，即可复用本发明中的方法生成调头路径，而无需对具体控制模型进行复杂的调整。
3、得到了安全可执行、平滑、多方面权衡最优的调头路径；
本发明在调头路径生成时，通过考虑路径是否碰撞和是否超出地图边界，确保行驶路径的安全性与可通行性。另一方面，本发明生成的调头路径前进段与倒车段切换处车头朝向一致、不会突变，使得生成路径符合驾驶逻辑；各路径点之间曲率连续，保证了路径的平滑性。此外，通过计算不同方面的路径成本选择最优路径，权衡了曲率、障碍物、地图边界等多方面因素，同时可根据实际需求调整不同项的权重，生成合适的路径。
附图说明
下面将结合附图及实施例对本发明作进一步说明，附图中：
图1为现有技术一的技术方案示意图；
图2为现有技术二的技术方案示意图；
图3为现有技术三的技术方案示意图；
图4为现有技术四的技术方案示意图；
图5为本发明实施例提供的无人驾驶车辆窄路三段调头路径规划方法的流程图；
图6为本发明实施例提供的无人驾驶车辆窄路三段调头路径规划方法的生成驶入圆弧路径与驶出圆弧路径的流程图；
图7为本发明实施例提供的无人驾驶车辆窄路三段调头路径规划方法的生成三段圆弧路径的流程图；
图8为本发明实施例提供的无人驾驶车辆窄路三段调头路径规划方法的车辆切换掉头场景的流程图；
图9为本发明实施例提供的无人驾驶车辆窄路三段调头路径规划方法的调头路径的生成的流程图;
图10为本发明实施例提供的无人驾驶车辆窄路三段调头路径规划方法的三段圆弧路径示意图；
图11为本发明实施例提供的无人驾驶车辆窄路三段调头路径规划方法的控制装置的结构示意图;
图12为本发明实施例提供的一种电子设备的结构示意图；
图13为本发明实施实例提供的各模块系统交互图。
实施方式
为使本发明的目的、技术方案及效果更加清楚、明确，以下对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。以下结合附图对本发明实施例进行介绍。
为使本发明内容表达清楚明白，将本发明中涉及到的缩略语以及关键术语定义解释如下：
二次规划（Quadratic Programming，简称QP）：是一种数学优化问题，其目标是在给定一组线性等式和不等式约束条件下，找到一个变量向量的最优值，使得一个二次目标函数取得最小值或最大值。
转弯半径：最小转弯半径是指当转向盘转到极限位置，汽车以最低稳定车速转向行驶时，外侧转向轮的中心在支承平面上滚过的轨迹圆半径。它在很大程度上表征了汽车能够通过狭窄弯曲地带或绕过不可越过的障碍物的能力。转弯半径越小，汽车的机动性能越好。
五次多项式样条曲线：五次多项式样条曲线是一种用于插值和平滑曲线的方法。它由一系列相连的五次多项式组成，用于在给定的离散数据点之间生成连续的曲线。
欧式距离：在欧几里德空间中，两个点之间的直线距离。
SL坐标系：自动驾驶中用于描述车辆在道路上的位置和运动的坐标系。SL坐标系基于道路的几何形状，并结合车辆的纵向运动和横向位置来定义坐标。SL坐标系中的S轴表示沿道路中心线的累积弧长，从道路的某个参考点开始计算，L轴表示车辆相对于道路中心线的横向偏移量。
偏航角（yaw角，heading）：车辆绕垂直轴旋转的角度，用于描述车辆的航向方向，用于定位、导航和航向控制等方面的计算和决策。在实际工程中，车辆的yaw角通常以弧度（radians）为单位表示，取值范围为[-π, π] 或 (-180°, 180°]，当车辆朝向参考方向的正方向时，yaw角为0；当车辆逆时针旋转时，yaw角为正值，反之为负值。
曲率：描述曲线弯曲程度的物理量。在数学上，曲率是指曲线在某一点处的弯曲程度，通常用曲率半径来描述。曲率半径是指曲线在某一点处所描述的圆的半径，这个圆与该点处的切线相切。曲率半径越小，曲线的弯曲程度就越大。
本实施例提供的无人驾驶车辆窄路三段调头路径规划方法，请阅图5-12，该方法具体包括如下步骤：
S100、生成驶入圆弧路径；所述驶入圆弧路径包括从车辆当前位置到调头区域起点的路径；
本实施例步骤中，若车辆即将进入调头区域，可规划一条从车辆当前位置到调头区域起点的路径，将车辆由当前位置行驶到调头区域起点。调头区域起点由高精地图中的道路属性确定。
本实施例中，所述驶入圆弧路径由五次多项式样条曲线进行生成；所述五次多项式样条曲线的关键特点是在相邻数据点处保持曲线的一阶导数和二阶导数的连续性。这种连续性使得所述五次多项式样条曲线在插值数据点之间呈现出平滑的形状，避免了不稳定的振荡。
本发明实施例中，所述五次多项式样条参数方程如下：
；
其中为五次多项式系数，在本步骤中为待求解系数；s为自变量，在本步骤中为各个路径点到路径起点的欧式距离除以路径总长；x, y为因变量，为预测点坐标；i表示多段样条曲线时段数序号。
根据多项式函数，可简化出函数预测点(x, y)的计算方式为：
；
其中：
；
本发明实施例中，所述步骤S100、生成驶入圆弧路径具体还包括：
S101、构造一个以多项式二阶导和三阶导为最小值的代价函数的二次规划问题；
S102、设置起点角度约束以及终点角度约束，所述角度为当前车辆位置的航向角；
S103、设置起点位置约束以及终点位置约束，以确保所述驶入圆弧路径通过调头区域的起点；
S104、基于所述代价函数、角度约束以及位置约束，使用求解器求得最优路径作为车辆由当前位置行驶到调头区域起点的最终路径。
具体的，为使路径曲率及曲率变化率最小、达到更好的平滑性，本发明构造了一个以多项式二阶导和三阶导最小为的代价函数的二次规划（QP）问题，求解一条从车辆当前位置到调头区域起点的样条曲线，样条曲线分段数为1。所述二次规划（QP）问题的代价函数如下所示（分别为二阶导和三阶导的权重）：
；
本发明实施例中，所述二次规划（QP）问题还需设计以下约束：
（1）设置起点角度约束以及终点角度约束；
所述角度为当前车辆位置的航向角，在五次多项式中角度。设定角度约束是为了确保路径在指定位置的角度有连续性和可控性。
同时，本发明实施例中，为了避免样条曲线的突变和不连续的转向，所述五次多项式样条曲线的起点角度和终点角度需分别与车辆当前位置的角度、调头区域起点的角度方向相同。方向一致的约束可由以下两方面的约束保证：
a.L轴分量为0，可保证方向相同或相反；
；
b.验证方向相同；
角度的单位向量为，如果/>，则表明方向一致。
（2）设置起点位置约束以及终点位置约束；
为确保所述五次多项式样条曲线经过特定位置，还需设置位置约束。在本发明场景中，为避免生成路径与实际车辆位置直接发生突变、确保路径通过调头区域起点，所述五次多项式样条曲线设置了起点的位置约束与终点的位置约束。所述位置约束可通过在S、L轴分别设置不等式实现，具体如下：
所述五次多项式样条曲线在L轴、S轴的投影系数为分别为：
；
实际路径点与所述五次多项式样条曲线的预测点在L、S轴的差值则表示了样条曲线结果在L、S轴的允许移动范围。本发明实施例中，起终点的差值设置为0，确保所述五次多项式样条曲线的拟合结果严格进过指定起终点，如下所示（dlateral, dlongitudinal为路径的在L、S轴的实际值）：
；
基于以上代价函数和约束，可构造二次规划（QP）问题，并使用OSQP等求解器得的最优路径，可作为车辆由当前位置行驶到调头区域起点的最终路径。
S200、生成三段圆弧路径；所述三段圆弧路径包括第一前进段圆弧路径、倒车段圆弧路径、第二前进段圆弧路径；
请参阅图10，图10为本发明实施例中的路径示意图，本发明实施例主要通过插值的方法生成三段圆弧路径。所述三段圆弧路径依次由第一前进段圆弧路径（）、倒车段圆弧路径（/>）、第二前进段圆弧（/>）组成，令/>分别为各圆弧段对应的圆弧角，/>分别为驶入、驶出圆弧偏航角（yaw角）；
A点为调头区域起点，坐标、yaw角（）由高精地图确定；D点为调头区域终点，坐标不确定，yaw角（/>）由高精地图确定。令/>为三段圆弧转角之和，即/>，其具体值可通过的几何关系计算得出：
；
由以上关系可知，各段圆弧的转角之和,由确定/>。确定/>，可确定/>，确定三段圆弧路径。因此遍历不同/>的值，可得到不同的圆弧路径，作为备选路径。
本发明实施例中，所述步骤S200、生成三段圆弧路径具体还包括：
S201、通过圆弧差值法得到圆弧路径点；
本步骤中，可根据行驶时方向盘往左还是往右打，将圆弧类型分为左圆弧和右圆弧，请继续参阅图10，三段路径依次为：第一前进段右圆弧路径（），倒车段左圆弧路径（），第二前进段右圆弧路径（/>）。
S202、多个所述圆弧路径点形成圆弧路径；
各段圆弧路径由多个路径点表示出来，将圆弧参数方程按一定间隔差值，可得到圆弧路径点。左右圆弧参数方程如下所示：
左圆弧：
；
；
右圆弧：
；
其中为圆弧起点坐标，/>为起点yaw角；自变量/>为路径点距圆弧起点的弧长距离；/>为路径方向，向前为1，向后（倒车）为-1；/>为圆弧固定半径，由车辆前轮或后轮最小转弯半径/>估算得出（可令/>，或可根据实际情况令/>略大于/>）：
；
其中为前后轴距离，/>为转向轮可打的最大转角。圆弧半径/>可根据实际情况，使用/>或使用对/>放大后的值。
S203、限制不同段的所述圆弧路径的偏航角；
本步骤实施例中，生成的圆弧路径除了满足最小转弯半径要求外，还需保证在不同段连接处车头朝向的一致性，确保yaw角在路径交点处不会突变，提高路径的平滑性。
在与/>的交点B处，需保证/>段最后一个点的yaw角与/>段第一个点的yaw角相同，/>与/>的交点C处同理。在圆弧段的起点A处，/>需保证与驶入圆弧路径最后一点的yaw角一致，圆弧终点D处，/>需要与地图中调头区域终点的yaw角一致（/>，/>之和不需要相加为180°）。
本发明实施例中，所述方法步骤还包括：
S300、生成驶出圆弧路径；所述驶出圆弧路径包括从所述前进段圆弧路径终点到参考线的路径；
所述三段圆弧路径生成完成后，还需回到参考线继续执行自动驾驶任务，从圆弧终最后一个点D回到参考线的路径，同样可由五次多项式样条曲线生成，通过构造QP问题求解最优曲线，具体数学原理与步骤S100、生成驶入圆弧路径相同，故此处不再赘述。
S400、路径拼接；将所述驶入圆弧路径、三段圆弧路径以及驶出圆弧路径拼接得到掉头路径。
通过上述方法生成了驶入圆弧路径、三段圆弧路径、驶出圆弧路径，将各段路径拼接得到调头路径。
在路径拼接的过程中，用上述五次多项式样条曲线的方法生成的所述驶入圆弧路径与所述驶出圆弧路径与所述三段圆弧路径直接拼接，尽管保证了yaw角的连续性，但未保证连接点（A、D）曲率的连续性。为保证曲率的连续性，还需对上述五次多项式样条曲线生成添加额外约束。
路径点处的曲率计算公式如下（/>为一阶导、二阶导）：
；
上述公式表明路径点的曲率大小由其一、二阶导确定，为保证连接处A、D点曲率的连续性，只要保证样条曲线在A、D点的曲率与圆弧段一致，保证曲率一致又可转化为保证一、二阶导一致。圆弧段一阶导计算方法如下：
；
其中为圆弧点的yaw角；dt为圆弧插值相邻路径点间的角度差值，为固定值，可根据实际情况指定其大小。因此/>为固定值，则圆弧段二阶导/>。
根据五次多项式样条曲线参数方程，其一、二阶导 如下所示：
；
对五次多项式样条曲线的起点和终点都添加以下约束，则可约束五次多项式样条曲线与圆弧段连接处的曲率连续性：
；
S500、判断最优路径；找到所述掉头路径中成本最小的掉头路径，作为最终的最优路径。
若所述调头路径无碰撞风险、不超出地图边界范围，则保存该路径作为备选路径，用于后续最优路径的评判。
进一步的，得到不同备选路径后，本发明中使用了加权成本来评价不同调头路径，选取成本最小的路径为最终调头路径。成本主要由曲率、距障碍物距离、与地图边界距离、调头路径路宽等方面考虑：
；
其中，为曲率项权重，/>为路径的最大曲率。对于三段圆弧路径，其曲率固定，为/>，不考虑曲率成本。由五次多项式样条曲线差值得到的驶入、驶出圆弧路径，计算不同路径点的曲率，选取最大曲率为/>。
为障碍物权重，/>为每一段路径距障碍物的最小距离，可由路径中每个点到障碍物距离计算得出。
为地图边界权重，/>调头路径距地图边界最小距离，可由路径中每个点到地图边界距离计算得出。
为路宽权重，/>为路径路宽，由调头路径做坐标转换计算得到。原调头路径坐标系与地图坐标系相同，即坐标原点为地图原点，xy轴同样与地图xy坐标系相同。以调头路径的起点start为坐标原点，其yaw角方向为y轴，垂直于yaw角方向为x轴，进行坐标转换，新坐标系下各路径点坐标为：
；/>
其中为调头路径各路径点在新坐标系下的坐标；/>为调头路径各路径点在地图坐标系下的坐标；/>为调头路径起点在地图坐标系下的坐标，为其yaw角。
调头路径的宽度范围为新x轴上的范围即为调头路径的宽度，即。另外，上述步骤是将路径点做坐标转换计算路宽，而实际路宽是基于车身边界覆盖范围计算得到的；若需计算车身边界对应的路宽，可基于路径点，推出车身边界，将车身边界做坐标转换得到实际路宽。
综上，本发明通过生成驶入圆弧路径，生成三段圆弧路径，生成驶出圆弧路径，路径拼接等步骤，以车辆感知、定位、底盘数据为基础，提供了一套实用、方便的调头路径生成流程，包含全套流程的数学表达、步骤和实现方法。本发明适用于不同车辆的调头路径生成，可解决现有调头路径生成方法中需要路宽过大、平滑计算耗时大、路径生成结果不符合驾驶逻辑的问题，为自动驾驶车辆在调头场景提供了一套可靠、实用的解决方案，促进自动驾驶技术的进一步落地与应用。
为了更好地实施以上方法，本申请实施例还提供一种无人驾驶车辆窄路三段调头路径规划控制装置，该窄路三段调头路径规划控制装置可以集成在计算机设备，比如车载终端等计算机设备中。
例如，请参阅图11，该窄路三段调头路径规划控制装置可以包括第一生成单元301、第二生成单元302、第三生成单元303和拼接单元304。
第一生成单元301，用于生成驶入圆弧路径；
第二生成单元302，用于生成三段圆弧路径；
第三生成单元303，用于生成驶出圆弧路径；
拼接单元304，用于将所述驶入圆弧路径、三段圆弧路径以及驶出圆弧路径拼接得到掉头路径；
判断单元305，用于判断最优路径；找到所述掉头路径中成本最小的掉头路径，作为最终的最优路径。
综上所述，本实施例提供的无人驾驶车辆窄路三段调头路径规划控制装置，通过利用前述路沿识别方法显著提升路沿识别的算法鲁棒性、精度和推理效率。所提供的无人驾驶车辆窄路三段调头路径规划控制装置的具体实施过程可以参见图1及图3所示的实施例提供的无人驾驶车辆窄路三段调头路径规划方法的具体实施过程，在此不再一一赘述。
此外，本实施例还提供了一种电子设备，包括存储器以及处理器，所述存储器存储有计算程序，所述计算程序在所述处理器上运行时执行图5和图7所示的无人驾驶车辆窄路三段调头路径规划方法。
另外，本公开实施例还提供了一种计算机可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行图5和图7所示的无人驾驶车辆窄路三段调头路径规划方法。
请参阅图12，本申请实施例还提供一种计算机设备，含核心处理器110、计算机可读存储介质的存储器120、电源130、输入单元140等部件。处理器110是该计算机设备的控制中心，利用各种接口和线路连接整个计算机设备的各个部分，通过运行或执行存储在存储器120内的软件程序和/或模块，以及调用存储在存储器120内的数据，执行计算机设备的各种功能和处理数据，从而对计算机设备进行整体监控。处理器110可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、用户界面和应用程序等，调制解调处理器主要处理无线通信。存储器120可用于存储软件程序以及模块，处理器110通过运行存储在存储器120的软件程序以及模块，从而执行各种功能应用以及车辆调头控制。存储器120可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据计算机设备的使用所创建的数据等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。相应地，存储器120还可以包括存储器控制器，以提供处理器对存储器的访问。电源130可以通过电源管理系统与处理器逻辑相连，从而通过电源管理系统实现管理充电、放电、以及功耗管理等功能。电源还可以包括一个或一个以上的直流或交流电源、再充电系统、电源故障检测电路、电源转换器或者逆变器、电源状态指示器等任意组件。输入单元140可用于接收输入的数字或字符信息，以及产生与用户设置以及功能控制有关的键盘、鼠标、操作杆、光学或者轨迹球信号输入。尽管未示出，计算机设备还可以包括显示单元等，在此不再赘述。
具体在本实施例中，计算机设备中的处理器会按照如下的指令，将一个或一个以上的应用程序的进程对应的可执行文件加载到存储器中，并由处理器来运行存储在存储器中的应用程序，从而实现各种功能：判断车辆当前位置是否即将进入调头区域。若车辆刚进入调头区域、没有生成好的调头路径，便生成调头路径。有调头路径后，车辆沿调头路径行驶，当车辆走完调头路径后，便切换会基础的车辆行驶模式、结束调头模式。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是智能手机、个人计算机、服务器、或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
请参阅图13，本发明提供的无人驾驶车辆窄路三段路径规划算法与定位、感知、控制和底盘模块紧密交互，以实现高效且安全的调头操作。定位模块210提供了车辆当前位置和姿态信息，为路径规划提供准确的起始点。感知模块220通过相机、雷达、激光雷达等传感器获取环境信息，如障碍物、交通流量和道路限制等，为路径规划算法提供关键的场景感知。控制模块240负责执行路径规划算法生成的路径，并实施转向、加速和制动等操作，确保车辆按照规划的路径进行调头操作。调头路径规划算法属于规划决策模块230，其与控制模块之间的紧密协作确保路径的可执行性和平滑性。底盘模块250负责处理车辆底盘动作和运动控制，包括转向系统、驱动系统和制动系统。
调头路径的生成还需从高精地图中做决策规划。通过决策规划系统与高精地图，可以判断车辆当前位置是否即将进入调头区域。若车辆刚进入调头区域、没有生成好的调头路径，便生成调头路径。有调头路径后，车辆沿调头路径行驶，当车辆走完调头路径后，便切换会基础的车辆行驶模式、结束调头模式。
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。
Claims (10)
1.一种无人驾驶车辆窄路三段调头路径规划方法，其特征在于，所述方法包括：
生成驶入圆弧路径；所述驶入圆弧路径包括从车辆当前位置到调头区域起点的路径；
生成三段圆弧路径；所述三段圆弧路径包括第一前进段圆弧路径、倒车段圆弧路径、第二前进段圆弧路径；
生成驶出圆弧路径；所述驶出圆弧路径包括从所述前进段圆弧路径终点到参考线的路径；
路径拼接；将所述驶入圆弧路径、三段圆弧路径以及驶出圆弧路径拼接得到掉头路径。
2.根据权利要求1所述的无人驾驶车辆窄路三段调头路径规划方法，其特征在于，所述驶入圆弧路径与所述驶出圆弧路径均由五次多项式样条曲线进行生成。
3.根据权利要求2所述的无人驾驶车辆窄路三段调头路径规划方法，其特征在于，所述生成驶入圆弧路径与所述驶出圆弧路径具体还包括：
构造一个以多项式二阶导和三阶导为最小值的代价函数的二次规划问题；
设置起点角度约束以及终点角度约束，所述角度为当前车辆位置的航向角；
设置起点位置约束以及终点位置约束，以确保所述驶入圆弧路径通过调头区域的起点；
基于所述代价函数、角度约束以及位置约束，使用求解器求得最优路径作为车辆由当前位置行驶到调头区域起点的最终路径。
4.根据权利要求1所述的无人驾驶车辆窄路三段调头路径规划方法，其特征在于，所述生成三段圆弧路径具体还包括：
通过圆弧差值法得到圆弧路径点；
多个所述圆弧路径点形成圆弧路径；
限制不同段的所述圆弧路径的偏航角。
5.根据权利要求3所述的无人驾驶车辆窄路三段调头路径规划方法，其特征在于，所述起点角度约束以及终点角度约束具体包括：
所述五次多项式样条曲线的起点角度和终点角度分别与车辆当前位置的角度、调头区域起点的角度方向相同。
6.根据权利要求2所述的无人驾驶车辆窄路三段调头路径规划方法，其特征在于，所述路径拼接的方法还包括：
约束所述五次多项式样条曲线与圆弧段连接处的曲率连续性。
7.根据权利要求1所述的无人驾驶车辆窄路三段调头路径规划方法，其特征在于，所述路径拼接之后，所述方法还包括：
判断最优路径；找到所述掉头路径中成本最小的掉头路径，作为最终的最优路径。
8.一种无人驾驶车辆窄路三段调头路径规划方法控制装置，其特征在于，所述装置包括：
第一生成单元，用于生成驶入圆弧路径；
第二生成单元，用于生成三段圆弧路径；
第三生成单元，用于生成驶出圆弧路径；
拼接单元，用于将所述驶入圆弧路径、三段圆弧路径以及驶出圆弧路径拼接得到掉头路径；
判断单元，用于判断最优路径；找到所述掉头路径中成本最小的掉头路径，作为最终的最优路径。
9.一种电子设备，其特征在于，包括：处理器、存储器及存储在所述存储器上并能够在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1-7中任一项所述的无人驾驶车辆窄路三段调头路径规划方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现如权利要求1-7中任一项所述的无人驾驶车辆窄路三段调头路径规划方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311169700.9A CN116907532B (zh) | 2023-09-12 | 2023-09-12 | 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311169700.9A CN116907532B (zh) | 2023-09-12 | 2023-09-12 | 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116907532A true CN116907532A (zh) | 2023-10-20 |
| CN116907532B CN116907532B (zh) | 2023-11-21 |
Family
ID=88360615
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311169700.9A Active CN116907532B (zh) | 2023-09-12 | 2023-09-12 | 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116907532B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107315411A (zh) * | 2017-07-04 | 2017-11-03 | 合肥工业大学 | 一种基于车车协同下无人驾驶车辆的换道轨迹规划方法 |
| US20210123750A1 (en) * | 2019-10-29 | 2021-04-29 | Hyundai Motor Company | Autonomous vehicle and method for planning u-turn path thereof |
| WO2022088380A1 (zh) * | 2020-10-28 | 2022-05-05 | 重庆长安汽车股份有限公司 | 车辆自动变道横向路径规划方法、系统、车辆及存储介质 |
| CN114834486A (zh) * | 2022-06-08 | 2022-08-02 | 云控智行科技有限公司 | 一种规划车辆掉头路径的方法、装置及设备 |
| CN115097826A (zh) * | 2022-06-20 | 2022-09-23 | 合众新能源汽车有限公司 | 一种车辆掉头轨迹规划方法及装置 |
| CN116048081A (zh) * | 2023-01-10 | 2023-05-02 | 吉林大学 | 一种考虑安全边界约束的自动驾驶车辆决策与规控方法 |
-
2023
- 2023-09-12 CN CN202311169700.9A patent/CN116907532B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107315411A (zh) * | 2017-07-04 | 2017-11-03 | 合肥工业大学 | 一种基于车车协同下无人驾驶车辆的换道轨迹规划方法 |
| US20210123750A1 (en) * | 2019-10-29 | 2021-04-29 | Hyundai Motor Company | Autonomous vehicle and method for planning u-turn path thereof |
| WO2022088380A1 (zh) * | 2020-10-28 | 2022-05-05 | 重庆长安汽车股份有限公司 | 车辆自动变道横向路径规划方法、系统、车辆及存储介质 |
| CN114834486A (zh) * | 2022-06-08 | 2022-08-02 | 云控智行科技有限公司 | 一种规划车辆掉头路径的方法、装置及设备 |
| CN115097826A (zh) * | 2022-06-20 | 2022-09-23 | 合众新能源汽车有限公司 | 一种车辆掉头轨迹规划方法及装置 |
| CN116048081A (zh) * | 2023-01-10 | 2023-05-02 | 吉林大学 | 一种考虑安全边界约束的自动驾驶车辆决策与规控方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116907532B (zh) | 2023-11-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11460311B2 (en) | Path planning method, system and device for autonomous driving | |
| Li et al. | Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles | |
| CN111806467B (zh) | 一种基于车辆行驶规律的变速动态换道轨迹规划方法 | |
| CN110361013B (zh) | 一种用于车辆模型的路径规划系统及方法 | |
| Rastelli et al. | Fuzzy logic steering control of autonomous vehicles inside roundabouts | |
| Li et al. | Precise trajectory optimization for articulated wheeled vehicles in cluttered environments | |
| Wang et al. | Action annotated trajectory generation for autonomous maneuvers on structured road networks | |
| Suzuki et al. | Automatic two-lane path generation for autonomous vehicles using quartic B-spline curves | |
| Sharma et al. | A survey on smooth path generation techniques for nonholonomic autonomous vehicle systems | |
| CN113848927A (zh) | 一种自主规划路径的自动驾驶系统 | |
| EP4347356A1 (en) | An adaptive path following algorithm for heavy-duty vehicles | |
| Wu et al. | Research on unmanned electric shovel autonomous driving path tracking control based on improved pure tracking and fuzzy control | |
| Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
| Yang et al. | A trajectory planning and fuzzy control for autonomous intelligent parking system | |
| CN116907532B (zh) | 无人驾驶车辆窄路三段调头路径规划方法、装置及设备 | |
| Kim et al. | Vehicle path tracking control using pure pursuit with MPC-based look-ahead distance optimization | |
| Meng et al. | A Path Planning and Model Predictive Control for Automatic Parking System | |
| Guo et al. | Global trajectory generation for nonholonomic robots in dynamic environments | |
| Qian et al. | Research on automatic parking system based on linear quadratic regulator | |
| CN114670876A (zh) | 无人车实现车道内调头的方法、装置及电子设备 | |
| CN114103957A (zh) | 变道控制方法、装置、电子设备和存储介质 | |
| Liu et al. | Trajectory Planning of Autonomous Vehicles Based on Parameterized Control Optimization for Three-Degree-of-Freedom Vehicle Dynamics Model | |
| Liu et al. | An integration planning and control method of intelligent vehicles based on the iterative linear quadratic regulator | |
| Hu et al. | Trajectory planning and optimal tracking for an industrial mobile robot | |
| Patel et al. | Simulation-based analysis of highway trajectory planning using high-order polynomial for highly automated driving function |
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 |