CN116147653A - 一种面向无人驾驶车辆的三维参考路径规划方法 - Google Patents

一种面向无人驾驶车辆的三维参考路径规划方法 Download PDF

Info

Publication number
CN116147653A
CN116147653A CN202310395142.1A CN202310395142A CN116147653A CN 116147653 A CN116147653 A CN 116147653A CN 202310395142 A CN202310395142 A CN 202310395142A CN 116147653 A CN116147653 A CN 116147653A
Authority
CN
China
Prior art keywords
node
path
nodes
dimensional
cost
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
CN202310395142.1A
Other languages
English (en)
Other versions
CN116147653B (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202310395142.1A priority Critical patent/CN116147653B/zh
Publication of CN116147653A publication Critical patent/CN116147653A/zh
Application granted granted Critical
Publication of CN116147653B publication Critical patent/CN116147653B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/3407—Route searching; Route guidance specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种面向无人驾驶车辆的三维参考路径规划方法，属于路径规划技术领域，解决了现有技术中规划路径的可通行性不高的问题。包括：采集越野环境的图像数据和雷达点云数据，建模得到分层地图，所述分层地图包括：障碍物层、高度层和路面语义层；采用改进的A‑star算法，根据分层地图，获取路径节点的动态扩展步长，并在代价函数中引入静态稳定性代价和路面语义代价，规划出初始路径；基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点；对优化后的路径节点拟合并投影到高度层，得到三维参考路径。实现了路径的安全与高效通行。

Description

一种面向无人驾驶车辆的三维参考路径规划方法
技术领域
本发明涉及路径规划技术领域，尤其涉及一种面向无人驾驶车辆的三维参考路径规划方法。
背景技术
越野环境下无人驾驶车辆的路径规划，对于保证车辆的稳定驾驶和无人作战意义重大。
路径规划应该能根据感知系统输入的越野环境信息，综合考虑环境 约束和车辆特性，生成从当前车辆位置到指定目标位置的无碰撞、车辆控制算法可跟踪、安全平稳的路径。然而，大部分基于搜索、采样和优化的路径规划方法多集中于结构化道路或路面条件良好的城市环境。
目前无人驾驶车辆常见的路径规划方法，基于平面二值栅格地图，仅考虑障碍物约束，避障的同时只追求路径长度或时间最短，没有考虑地形信息和车辆姿态，无法保证规划路径的可通行性。
发明内容
鉴于上述的分析，本发明实施例旨在提供一种面向无人驾驶车辆的三维参考路径规划方法，用以解决现有规划路径的可通行性不高的问题。
本发明实施例提供了一种面向无人驾驶车辆的三维参考路径规划方法，包括以下步骤：
采集越野环境的图像数据和雷达点云数据，建模得到分层地图，分层地图包括：障碍物层、高度层和路面语义层；
采用改进的
Figure SMS_1
算法，根据分层地图，获取路径节点的动态扩展步长，并在代价函数中引入静态稳定性代价和路面语义代价，规划出初始路径；
基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点；
对优化后的路径节点拟合并投影到高度层，得到三维参考路径。
基于上述方法的进一步改进，根据分层地图，获取路径节点的动态扩展步长，包括：
将当前路径节点作为父节点，根据父节点的位置和预置的步长范围，获取在障碍物层的最大扩展步长，根据最大扩展步长，生成父节点的二维子节点；
将二维子节点投影到高度层，得到三维子节点，根据三维子节点与父节点间的单位攀升高度，及障碍物层的最大扩展步长，得到父节点在各个三维子节点方向上的动态扩展步长。
基于上述方法的进一步改进，通过以下公式得到父节点在各个三维子节点方向上的动态扩展步长：
Figure SMS_2
，
其中，
Figure SMS_3
表示父节点向第j个三维子节点方向扩展的步长，/>
Figure SMS_4
表示父节点在障碍物层的最大扩展步长，/>
Figure SMS_5
表示第j个三维子节点与父节点间的单位攀升高度，/>
Figure SMS_6
表示预置的最大曲面曲率值。
基于上述方法的进一步改进，根据父节点的位置和预置的步长范围，获取障碍物层的最大扩展步长，包括：
基于障碍物层，根据父节点的位置，获取父节点离障碍物的最近距离；当最近距离在预置的步长范围内，则最近距离作为最大扩展步长；当最近距离小于预置的步长范围的最小步长，则取最小步长为最大扩展步长；当最近距离大于预置的步长范围的最大步长，则取最大步长为最大扩展步长。
基于上述方法的进一步改进，根据最大扩展步长，生成父节点的二维子节点，是在以父节点为圆心，以最大扩展步长为半径的圆周上，根据预置的间隔角度生成均匀分布的二维子节点。
基于上述方法的进一步改进，代价函数包括移动代价和启发项代价；其中移动代价包括父节点的移动代价、父子节点间的欧式距离、父子节点间的静态稳定性代价和子节点的路面语义代价；启发项代价是子节点与终点间的欧式距离；子节点是父节点根据动态扩展步长扩展的子节点且满足平面无碰撞和静态稳定性的约束条件。
基于上述方法的进一步改进，父子节点间的静态稳定性代价通过以下公式计算得到：
Figure SMS_7
，
其中，
Figure SMS_8
表示第i个子节点，/>
Figure SMS_9
表示父节点；/>
Figure SMS_10
表示第i个子节点与父节点间的俯仰角差值绝对值，/>
Figure SMS_11
表示第i个子节点与父节点间的侧倾角差值绝对值，/>
Figure SMS_12
表示第i个子节点与父节点间的单位攀升高度。
基于上述方法的进一步改进，子节点的路面语义代价是根据无人驾驶车辆在子节点处所占区域，计算出区域内各路面类型所占比例，分别与对应的权重相乘后再求和而得到；路面类型包括：土路、草地和灌木。
基于上述方法的进一步改进，基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点，包括：
在每次迭代优化中，将初始路径中除起点和终点以外的路径节点作为待优化节点，依次对每个待优化节点执行以下更新操作，获取更新后的路径长度：获取待优化节点在其前后相邻的路径节点连线上的投影点；从投影点至其对应的待优化节点的连线上根据地图最小分辨率生成等间距的多个待调整节点；依次将每个待调整节点投影到高度层，将第一个满足平面无碰撞和静态稳定性的约束条件的三维待调整节点更新为对应的待优化节点，如果不存在满足平面无碰撞和静态稳定性的约束条件的三维待调整节点，则不更新对应的待优化节点；
当更新后的路径长度小于前一次迭代优化的长度且长度缩短率小于优化阈值，或者达到最大迭代次数时，结束迭代优化。
基于上述方法的进一步改进，平面无碰撞的约束条件是路径节点离障碍物的最近距离大于无人驾驶车辆半径；静态稳定性的约束条件是无人驾驶车辆在路径节点处的俯仰角和侧倾角均小于对应的角度阈值。
与现有技术相比，本发明至少可实现如下有益效果之一：
1、在规划过程中引入分层地图，考虑环境中地形的高度和路面语义信息，将原有基于平面二值栅格地图的局部路径规划算法推广到三维空间，提升了路径规划所能获取的环境信息。
2、改进代价函数，引导规划过程尽量避开地形起伏和难以行驶的灌木区，同时考虑越野环境下的无碰撞约束和车辆的静态稳定性约束，提升了路径的可通行性，保证车辆的安全与高效通行。
3、引入无梯度迭代优化方法，减小了路径的松弛度。
本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件；
图1为本发明实施例中一种面向无人驾驶车辆的三维参考路径规划方法流程图；
图2为本发明实施例中分层地图示意图；
图3为本发明实施例中路径节点迭代优化过程示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
本发明的一个具体实施例，公开了一种面向无人驾驶车辆的三维参考路径规划方法，如图1所示，包括以下步骤：
S11、采集越野环境的图像数据和雷达点云数据，建模得到分层地图，分层地图包括：障碍物层、高度层和路面语义层。
需要说明的是，越野环境中场地道路不平，道路表面属性差异明显，环境复杂度高，示例性地，选择河北石家庄越野场地。
本实施例的无人驾驶车辆是指无人履带平台，车身四周布置有工业相机，车顶上安装有激光雷达、全球定位系统（GPS，Global Positioning System）和惯性导航系统（IMU，Inertial Measurement Unit）。
给定路径规划的起点、终点和拓扑路网，根据GPS和IMU的数据，计算车辆位置，并映射至拓扑路网中。采用 3 台工控机作为计算设备，其中一台配备 RTX 2080Ti 作为图像语义分割处理工控机，对采集的越野环形的图像数据进行处理，一台对采集的激光雷达点云做栅格化处理，匹配图像语义分割结果，生成分层地图，另外一台处理运动规划控制程序，控制车辆运行。
基于分层规划地图模型，对越野环境建模得到多层地图，包括：
根据采集的越野环境的图像数据，采用双通道卷积神经网络提取图像特征，进行图像像素级分类，识别出越野环境中的土路、草地、灌木和未知类型，得到图像语义；基于传感器坐标系转换关系将图像语义赋予采集的雷达点云数据，得到语义点云；根据语义点云构建路面语义层地图；
基于雷达点云数据，获取点云高度，构建高度层地图；根据相对高度差阈值进行二值建模，当点云间的高度差高于设定的相对高度差阈值，则为障碍物，反之为非障碍物，构建障碍物层地图。
各层地图采用占据栅格地图的存储形式，每一个栅格都可以通过二 维坐标快速索引。
在得到的分层地图中，障碍物层中像素值为0，则表示是障碍物，否则，表示非障碍物；高度层中每个像素值表示该处在实际环境中的地形高度，像素值越大，高度越大，像素值越小，高度越小；路面语义层中每个像素值表示该处在实际环境中所对应的路面类型，路面类型包括土路、草地、灌木和未知类型。
S12、采用改进的
Figure SMS_13
算法，根据分层地图，获取路径节点的动态扩展步长，并在代价函数中引入静态稳定性代价和路面语义代价，规划出初始路径。
需要说明的是，现有
Figure SMS_14
（A-star）算法根据平面二值栅格地图，初始以起点作为父节点，按照固定步长向周围扩展出子节点，根据代价函数取代价值最小的子节点作为下一个父节点，再向周围扩展出子节点，依次类推，直至终点；然后从终点开始逐步追踪父节点，直至起点，得到路径。
本实施例根据分层地图，考虑环境中地形的高度和路面语义信息，不仅将原有基于平面二值栅格地图的局部路径规划算法推广到三维空间，提升了路径规划所能获取的环境信息，还通过动态扩展步长，实现在比较平坦的地方以大步长扩展，而在地形面凸起或凹陷的地方谨慎使用小步长，比起定步长的方式具有更高的搜索构建效率。
具体来说，根据分层地图，获取路径节点的动态扩展步长，包括：
①将当前路径节点作为父节点，根据父节点的位置和预置的步长范围，获取在障碍物层的最大扩展步长，根据最大扩展步长，生成父节点的二维子节点。
需要说明的是，根据父节点的位置和预置的步长范围，获取障碍物层的最大扩展步长，包括：
基于障碍物层，根据父节点的位置，获取父节点离障碍物的最近距离；当最近距离在预置的步长范围内，则最近距离作为最大扩展步长；当最近距离小于预置的步长范围的最小步长，则取最小步长为最大扩展步长；当最近距离大于预置的步长范围的最大步长，则取最大步长为最大扩展步长，用以下公式表示：
Figure SMS_15
，
其中，
Figure SMS_16
表示父节点在障碍物层的最大扩展步长，/>
Figure SMS_17
表示父节点离障碍物的最近距离，/>
Figure SMS_18
表示预置的步长范围。示例性地，预置的步长范围为[1.2,3]，单位是米。
根据最大扩展步长，生成父节点的二维子节点，是在以父节点为圆心，以最大扩展步长为半径的圆周上，根据预置的间隔角度生成均匀分布的二维子节点。
示例性地，预置的间隔角度为10度，得到36个均匀分布的二维子节点。
②将二维子节点投影到高度层，得到三维子节点，根据三维子节点与父节点间的单位攀升高度，及障碍物层的最大扩展步长，得到父节点在各个三维子节点方向上的动态扩展步长。
需要说明的是，第j个三维子节点的坐标表示为
Figure SMS_19
，父节点坐标表示为/>
Figure SMS_20
，第j个三维子节点与父节点间的单位攀升高度
Figure SMS_21
，采用以下公式计算得到：
Figure SMS_22
，
通过以下公式计算得到父节点在各个三维子节点方向上的动态扩展步长：
Figure SMS_23
，
其中，
Figure SMS_24
表示父节点向第j个三维子节点方向扩展的步长，/>
Figure SMS_25
表示预置的最大曲面曲率值，根据实际的越野环境而设置。示例性地，设置为0.4。
进一步地，父节点按照各个三维子节点方向上的动态扩展步长得到扩展后的子节点，并从中选取满足平面无碰撞和静态稳定性的约束条件的子节点
Figure SMS_26
，结合分层地图，在代价函数中引入静态稳定性代价和路面语义代价，计算出代价值。取代价值最小的子节点作为下一个父节点，再根据上述步骤获取动态扩展步长，向周围扩展出子节点，依次类推，直至终点，规划出初始路径。
与现有技术相比，本实施例中只有同时满足平面无碰撞和静态稳定性的子节点才会被保留下来参与代价值的计算。通过这种方式，避免生成不可通行的路径点或候选路径，而且在代价值计算中再次考虑了可通行性，最终使得规划效率和生成路径的可通行性得到进一步提升。
需要说明的是，平面无碰撞的约束条件是路径节点离障碍物的最近距离大于无人驾驶车辆半径；静态稳定性的约束条件是无人驾驶车辆在路径节点处的俯仰角和侧倾角均小于对应的角度阈值。示例性地，俯仰角对应的角度阈值是20度，侧倾角对应的角度阈值是15度。
具体来说，当车辆以某一姿态静止于某个路径节点时，将车辆在高度层上的投影近似为一个矩形，计算出投影区域内数据点的均值和协方差矩阵，将协方差矩阵进行特征分解，其最小特征值对应的特征向量就是车辆位置处的曲面法向量，归一化后即可得到Z轴单位法向量。根据车辆的航向角和Z轴单位法向量，计算出沿着车辆X轴的单位方向向量，根据右手定则计算出Y轴的单位方向向量；根据X轴和Y轴单位方向向量和Z轴单位法向量，计算出车辆在路径节点的俯仰角和侧倾角。
进一步地，代价函数
Figure SMS_27
包括移动代价/>
Figure SMS_28
和启发项代价/>
Figure SMS_29
；其中移动代价包括父节点的移动代价/>
Figure SMS_30
、父子节点间的欧式距离/>
Figure SMS_31
、父子节点间的静态稳定性代价/>
Figure SMS_32
和子节点的路面语义代价/>
Figure SMS_33
；启发项代价是子节点与终点间的欧式距离。
具体来说，移动代价的计算公式如下所示：
Figure SMS_34
，
其中，
Figure SMS_35
表示第i个子节点，/>
Figure SMS_36
表示第i个子节点的父节点；初始父节点的移动代价/>
Figure SMS_37
为0，/>
Figure SMS_38
是衡量距离代价的权重，/>
Figure SMS_39
是权衡静态稳定性代价的权重，/>
Figure SMS_40
是衡量路面语义代价的权重。权重越小表示越侧重于对应项。比如，设置衡量距离代价的权重最小，则表示期望生成的路径长度最短。
父子节点间的欧式距离
Figure SMS_41
根据父节点和子节点的三维坐标，取2范数而得到；启发项代价是根据子节点和终点的三维坐标，取2范数而得到。
父子节点间的静态稳定性代价通过以下公式计算得到：
Figure SMS_42
，
其中，
Figure SMS_43
表示第i个子节点与父节点间的俯仰角差值绝对值，/>
Figure SMS_44
表示第i个子节点与父节点间的侧倾角差值绝对值，/>
Figure SMS_45
表示第i个子节点与父节点间的单位攀升高度，通过将第i个子节点和父节点的三维坐标代入公式(2)而得到。
子节点的路面语义代价是根据无人驾驶车辆在子节点处所占区域，计算出区域内各路面类型所占比例，分别与对应的权重相乘后再求和而得到，公式如下所示：
Figure SMS_46
，
其中，
Figure SMS_47
、/>
Figure SMS_48
和/>
Figure SMS_49
分别表示第i个子节点处土路、草地和灌木三种路面类型所占比例，/>
Figure SMS_50
、/>
Figure SMS_51
和/>
Figure SMS_52
分别表示土路、草地和灌木三种路面类型对应的权重，/>
Figure SMS_53
，表示期望车辆多走土路，少走草地，避免走灌木路面。
与现有技术相比，本实施例改进了代价函数，引导规划过程尽量避开地形起伏和难以行驶的灌木区，同时考虑越野环境下的无碰撞约束和车辆的静态稳定性约束，提升了路径的可通行性，保证车辆的安全与高效通行。
S13、基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点。
需要说明的是，在每次迭代优化中，将初始路径中除起点和终点以外的路径节点作为待优化节点，依次对每个待优化节点执行以下更新操作，获取更新后的路径长度：获取待优化节点在其前后相邻的路径节点连线上的投影点；从投影点至其对应的待优化节点的连线上根据地图最小分辨率生成等间距的多个待调整节点；依次将每个待调整节点投影到高度层，将第一个满足平面无碰撞和静态稳定性的约束条件的三维待调整节点更新为对应的待优化节点，如果不存在满足平面无碰撞和静态稳定性的约束条件的三维待调整节点，则不更新对应的待优化节点；
当更新后的路径长度小于前一次迭代优化的长度且长度缩短率小于优化阈值，或者达到最大迭代次数时，结束迭代优化。
具体来说，如图3所示，获取待优化节点在其前后相邻的路径节点连线上的投影点，即将待优化节点
Figure SMS_62
和其前后相邻的路径节点/>
Figure SMS_56
、/>
Figure SMS_58
构成三角形，获取/>
Figure SMS_57
在/>
Figure SMS_60
连线上的投影点/>
Figure SMS_64
；在投影点/>
Figure SMS_68
和/>
Figure SMS_65
之间的连线上，根据地图的最小分辨率，比如0.2米，生成多个待调整节点，放入待调整节点序列/>
Figure SMS_69
。按照序列顺序，首先将
Figure SMS_54
投影到高度层上得到三维待调整节点/>
Figure SMS_61
，判断/>
Figure SMS_66
是否同时满足平面无碰撞和静态稳定性的约束条件，如果满足，则将三维待调整节点/>
Figure SMS_70
作为优化后的/>
Figure SMS_72
，继续利用下一个待优化节点/>
Figure SMS_74
在/>
Figure SMS_63
和/>
Figure SMS_67
连线上的投影点，对下一个待优化节点/>
Figure SMS_71
执行更新操作；如果不满足，继续取出下一个待调整节点/>
Figure SMS_73
，重复上述判断，直至获取到第一个满足约束条件的三维待调整节点，如果对待调整节点序列遍历结束，未获取到满足约束条件三维待调整节点，则/>
Figure SMS_55
在本次迭代优化中不再被更新，继续对下一个待优化节点/>
Figure SMS_59
执行更新操作。当对每个待优化节点执行完更新操作，完成一次迭代优化，根据更新后的路径节点，将每相邻两个路径节点间距离累加，得到更新后的路径长度，当更新后的路径长度小于前一次迭代优化的长度且长度缩短率小于优化阈值，或者达到最大迭代次数时，结束迭代优化，否则对更新后的路径节点再次执行迭代优化，直至满足迭代结束条件，得到最终更新后的路径节点。
与现有技术相比，本实施例的迭代优化过程是无梯度的迭代平滑优化方法，减少了路径的松弛度。
S14、对优化后的路径节点拟合并投影到高度层，得到三维参考路径。
具体来说，采用K次B样条算法对优化后的路径节点进行拟合，将拟合的曲线投影到高度层，得到满足平面无碰撞和静态稳定性的约束条件的三维参考路径。优选地，采用三次B样条算法。
与现有技术相比，本实施例提供的一种面向无人驾驶车辆的三维参考路径规划方法在规划过程中引入分层地图，考虑环境中地形的高度和路面语义信息，将原有基于平面二值栅格地图的局部路径规划算法推广到三维空间，提升了路径规划所能获取的环境信息；改进代价函数，引导规划过程尽量避开地形起伏和难以行驶的灌木区，同时考虑越野环境下的无碰撞约束和车辆的静态稳定性约束，提升了路径的可通行性，保证车辆的安全与高效通行；引入无梯度迭代优化方法，减小了路径的松弛度。
本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种面向无人驾驶车辆的三维参考路径规划方法，其特征在于，包括以下步骤：
采集越野环境的图像数据和雷达点云数据，建模得到分层地图，所述分层地图包括：障碍物层、高度层和路面语义层；
采用改进的
Figure QLYQS_1
算法，根据分层地图，获取路径节点的动态扩展步长，并在代价函数中引入静态稳定性代价和路面语义代价，规划出初始路径；
基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点；
对优化后的路径节点拟合并投影到高度层，得到三维参考路径。
2.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述根据分层地图，获取路径节点的动态扩展步长，包括：
将当前路径节点作为父节点，根据父节点的位置和预置的步长范围，获取在障碍物层的最大扩展步长，根据所述最大扩展步长，生成父节点的二维子节点；
将二维子节点投影到高度层，得到三维子节点，根据三维子节点与父节点间的单位攀升高度，及障碍物层的最大扩展步长，得到父节点在各个三维子节点方向上的动态扩展步长。
3.根据权利要求2所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，通过以下公式得到父节点在各个三维子节点方向上的动态扩展步长：
Figure QLYQS_2
，
其中，
Figure QLYQS_3
表示父节点向第j个三维子节点方向扩展的步长，/>
Figure QLYQS_4
表示父节点在障碍物层的最大扩展步长，/>
Figure QLYQS_5
表示第j个三维子节点与父节点间的单位攀升高度，/>
Figure QLYQS_6
表示预置的最大曲面曲率值。
4.根据权利要求2所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述根据父节点的位置和预置的步长范围，获取障碍物层的最大扩展步长，包括：
基于障碍物层，根据父节点的位置，获取父节点离障碍物的最近距离；当最近距离在预置的步长范围内，则最近距离作为最大扩展步长；当最近距离小于预置的步长范围的最小步长，则取最小步长为最大扩展步长；当最近距离大于预置的步长范围的最大步长，则取最大步长为最大扩展步长。
5.根据权利要求2所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述根据所述最大扩展步长，生成父节点的二维子节点，是在以父节点为圆心，以最大扩展步长为半径的圆周上，根据预置的间隔角度生成均匀分布的二维子节点。
6.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述代价函数包括移动代价和启发项代价；其中移动代价包括父节点的移动代价、父子节点间的欧式距离、父子节点间的静态稳定性代价和子节点的路面语义代价；启发项代价是子节点与终点间的欧式距离；所述子节点是父节点根据动态扩展步长扩展的子节点且满足平面无碰撞和静态稳定性的约束条件。
7.根据权利要求6所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述父子节点间的静态稳定性代价通过以下公式计算得到：
Figure QLYQS_7
，
其中，
Figure QLYQS_8
表示第i个子节点，/>
Figure QLYQS_9
表示父节点；/>
Figure QLYQS_10
表示第i个子节点与父节点间的俯仰角差值绝对值，/>
Figure QLYQS_11
表示第i个子节点与父节点间的侧倾角差值绝对值，/>
Figure QLYQS_12
表示第i个子节点与父节点间的单位攀升高度。
8.根据权利要求6所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述子节点的路面语义代价是根据无人驾驶车辆在子节点处所占区域，计算出区域内各路面类型所占比例，分别与对应的权重相乘后再求和而得到；所述路面类型包括：土路、草地和灌木。
9.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述基于平面无碰撞和静态稳定性的约束条件，利用路径节点在其前后相邻的路径节点连线上的投影点，对初始路径进行迭代优化，得到优化后的路径节点，包括：
在每次迭代优化中，将初始路径中除起点和终点以外的路径节点作为待优化节点，依次对每个待优化节点执行以下更新操作，获取更新后的路径长度：获取待优化节点在其前后相邻的路径节点连线上的投影点；从投影点至其对应的待优化节点的连线上根据地图最小分辨率生成等间距的多个待调整节点；依次将每个待调整节点投影到高度层，将第一个满足平面无碰撞和静态稳定性的约束条件的三维待调整节点更新为对应的待优化节点，如果不存在满足平面无碰撞和静态稳定性的约束条件的三维待调整节点，则不更新对应的待优化节点；
当更新后的路径长度小于前一次迭代优化的长度且长度缩短率小于优化阈值，或者达到最大迭代次数时，结束迭代优化。
10.根据权利要求1所述的面向无人驾驶车辆的三维参考路径规划方法，其特征在于，所述平面无碰撞的约束条件是路径节点离障碍物的最近距离大于无人驾驶车辆半径；所述静态稳定性的约束条件是无人驾驶车辆在路径节点处的俯仰角和侧倾角均小于对应的角度阈值。
CN202310395142.1A 2023-04-14 2023-04-14 一种面向无人驾驶车辆的三维参考路径规划方法 Active CN116147653B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310395142.1A CN116147653B (zh) 2023-04-14 2023-04-14 一种面向无人驾驶车辆的三维参考路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310395142.1A CN116147653B (zh) 2023-04-14 2023-04-14 一种面向无人驾驶车辆的三维参考路径规划方法

Publications (2)

Publication Number Publication Date
CN116147653A true CN116147653A (zh) 2023-05-23
CN116147653B CN116147653B (zh) 2023-08-22

Family

ID=86350918

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310395142.1A Active CN116147653B (zh) 2023-04-14 2023-04-14 一种面向无人驾驶车辆的三维参考路径规划方法

Country Status (1)

Country Link
CN (1) CN116147653B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017173990A1 (zh) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 一种机器人避障中的最短路径规划方法
CN112325892A (zh) * 2020-10-10 2021-02-05 南京理工大学 一种基于改进a*算法的类三维路径规划方法
CN112762957A (zh) * 2020-12-29 2021-05-07 西北工业大学 一种基于多传感器融合的环境建模及路径规划方法
CN113044029A (zh) * 2021-03-19 2021-06-29 北京理工大学 一种保证无人车在三维地形上安全行驶的运动规划方法
WO2022099620A1 (zh) * 2020-11-13 2022-05-19 深圳市大疆创新科技有限公司 三维点云分割方法和装置、可移动平台
CN114706400A (zh) * 2022-04-12 2022-07-05 重庆文理学院 一种越野环境下基于改进的a*算法的路径规划方法
CN114812581A (zh) * 2022-06-23 2022-07-29 中国科学院合肥物质科学研究院 一种基于多传感器融合的越野环境导航方法
WO2022193584A1 (zh) * 2021-03-15 2022-09-22 西安交通大学 一种面向多场景的自动驾驶规划方法及系统
CN115451983A (zh) * 2022-08-09 2022-12-09 华中科技大学 一种复杂场景下的动态环境建图与路径规划方法及装置

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017173990A1 (zh) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 一种机器人避障中的最短路径规划方法
CN112325892A (zh) * 2020-10-10 2021-02-05 南京理工大学 一种基于改进a*算法的类三维路径规划方法
WO2022099620A1 (zh) * 2020-11-13 2022-05-19 深圳市大疆创新科技有限公司 三维点云分割方法和装置、可移动平台
CN112762957A (zh) * 2020-12-29 2021-05-07 西北工业大学 一种基于多传感器融合的环境建模及路径规划方法
WO2022193584A1 (zh) * 2021-03-15 2022-09-22 西安交通大学 一种面向多场景的自动驾驶规划方法及系统
CN113044029A (zh) * 2021-03-19 2021-06-29 北京理工大学 一种保证无人车在三维地形上安全行驶的运动规划方法
CN114706400A (zh) * 2022-04-12 2022-07-05 重庆文理学院 一种越野环境下基于改进的a*算法的路径规划方法
CN114812581A (zh) * 2022-06-23 2022-07-29 中国科学院合肥物质科学研究院 一种基于多传感器融合的越野环境导航方法
CN115451983A (zh) * 2022-08-09 2022-12-09 华中科技大学 一种复杂场景下的动态环境建图与路径规划方法及装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
刘龙龙等: "基于拓扑路网的多挡无人履带平台路径重规划", 兵工学报, vol. 44, no. 1 *
周梦如等: "越野环境下无人履带平台的道路可通行性分析", 兵工学报, vol. 43, no. 10 *
许伦辉;曹宇超;林培群;: "基于多影响因素RDMA~*算法的无人驾驶动态路径规划", 交通信息与安全, no. 02 *

Also Published As

Publication number Publication date
CN116147653B (zh) 2023-08-22

Similar Documents

Publication Publication Date Title
CN109509210B (zh) 障碍物跟踪方法和装置
CN108519094B (zh) 局部路径规划方法及云处理端
CN112639502A (zh) 机器人位姿估计
CN106599108A (zh) 一种三维环境中多模态环境地图构建方法
CN112762957A (zh) 一种基于多传感器融合的环境建模及路径规划方法
CN114119920A (zh) 三维点云地图构建方法、系统
CN113238251B (zh) 一种基于车载激光雷达的目标级语义定位方法
CN114140761A (zh) 点云配准方法、装置、计算机设备和存储介质
US11373328B2 (en) Method, device and storage medium for positioning object
CN115752474A (zh) 非平坦地面环境下的机器人导航规划方法、装置及机器人
CN115639823A (zh) 崎岖起伏地形下机器人地形感知与移动控制方法及系统
CN116482711A (zh) 一种用于着陆区自主选择的局部静态环境感知方法及装置
CN116136417B (zh) 一种面向越野环境的无人驾驶车辆局部路径规划方法
CN113515128B (zh) 一种无人车实时路径规划方法及存储介质
CN115218916A (zh) 一种安全性路径规划方法、装置
Wu et al. A Non-rigid hierarchical discrete grid structure and its application to UAVs conflict detection and path planning
CN117406771B (zh) 一种基于四旋翼无人机的高效自主探索方法、系统及设备
CN112132951B (zh) 一种基于视觉的网格语义地图的构建方法
CN116182859A (zh) 路径规划方法及装置、设备、存储介质
CN116147653B (zh) 一种面向无人驾驶车辆的三维参考路径规划方法
CN117289301A (zh) 一种未知越野场景下空地无人平台协同路径规划方法
CN116929363A (zh) 一种基于可通行地图的矿用车辆自主导航方法
EP4148599A1 (en) Systems and methods for providing and using confidence estimations for semantic labeling
CN114398455A (zh) 异构多机器人协同slam地图融合方法
CN113119995B (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
GR01 Patent grant
GR01 Patent grant