CN117873115A - 一种基于slam的自主导航避障路径规划方法 - Google Patents
一种基于slam的自主导航避障路径规划方法 Download PDFInfo
- Publication number
- CN117873115A CN117873115A CN202410135356.XA CN202410135356A CN117873115A CN 117873115 A CN117873115 A CN 117873115A CN 202410135356 A CN202410135356 A CN 202410135356A CN 117873115 A CN117873115 A CN 117873115A
- Authority
- CN
- China
- Prior art keywords
- path
- point
- obstacle
- planning
- obstacle avoidance
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000006073 displacement reaction Methods 0.000 claims abstract description 18
- 238000005457 optimization Methods 0.000 claims abstract description 17
- 239000013598 vector Substances 0.000 claims abstract description 14
- 238000011478 gradient descent method Methods 0.000 claims abstract description 6
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000035484 reaction time Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Classifications
-
- 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
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及无人驾驶的自主导航避障路径规划技术领域公开一种基于SLAM的自主导航避障路径规划方法,方法中引入了障碍物距离影响系数ρ,在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响,判断是否开始避障路径规划;在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径,优化方式以一定的迭代次数通过梯度下降法求出每次优化N点路径中第二个路径点的位移矢量来约束N点路径的平滑度,接着去除掉每次优化N点路径中的第一个路径点,加入第N+1个路径点重新构成新的N点路径,循环遍历所有的路径点,直到构不成新的N点路径时停止。该方法能够优化路径平滑度,更符合现实环境中机器人的实际应用。
Description
技术领域
本发明涉及无人驾驶的自主导航避障路径规划技术领域。
背景技术
路径规划是实现无人车自主导航的核心,根据对环境信息掌握的不同可分为全局和局部路径规划,其中全局路径规划是根据已知的地图环境信息给出一个从起点到目标点的最优路经,局部路径规划是当局部环境信息发生变化时,如动态障碍物的加入,让机器人具有优良的避障性能,能够安全可靠的运行。
D*算法是现有的一种路径规划算法,是一种动态环境下的启发式路径搜索算法,能够在环境发生变化时,最大限度利用前一次的搜索结果,从而快速进行第二次规划,即D*算法能够实现动态避障,遇障碍物能够利用前一次的路径搜索信息,无需重复规划,提高搜索效率,但是现有的D*算法在动态避障时,是到障碍物面前才开始进行新的路径规划,这种方法虽然充分利用了第一阶段A*算法的快速性,保留了在遇到障碍物之前的所有路径。但这种方法也有一定的局限性,由于在动态障碍物的面前才开始新的路径规划,这种情况下可能会由于规划的不及时,出现路径增大、重复路径、耗时增长的情况,可见现有的D*算法在使用时容易出现效率低下和路径不平滑问题,其在路径平滑度上未做优化,存在大量转折,增大了路径距离也不符合现实环境中机器人的实际应用。
发明内容
本发明的目的在于提供一种能够优化路径平滑度,更符合现实环境中机器人的实际应用的一种基于SLAM的自主导航避障路径规划方法。
为实现上述目的,本发明的技术方案是:一种基于SLAM的自主导航避障路径规划方法,方法中引入了障碍物距离影响系数ρ,在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响,判断是否开始避障路径规划;在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径,优化的方式以一定的迭代次数通过梯度下降法求出每次优化N点路径中第二个路径点的位移矢量来约束N点路径的平滑度,接着去除掉每次优化N点路径中的第一个路径点,加入第N+1个路径点重新构成新的N点路径,如此依次循环下去,遍历所有的路径点,直到构不成新的N点路径时停止。
所述N点路径为4个连续的路径点的路径。
所述障碍物距离影响系数ρ,表达式为:
式中λ为配置系数,D为预判距离函数,
预判距离函数是指当前搜索点距离障碍物的距离,用欧式距离表达即为:
式中X表示当前搜索点的横纵坐标xi、yi,O表示障碍物的初始坐标Ox、Oy;
路径规划中在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响,表达式为,
式中ρ=0,表示搜索点距离障碍物的距离D为无限大,该障碍物不在路径搜索的范围内,路径规划时不予考虑;ρ=+∞表示障碍物与搜索点的距离D为零,障碍物已经在规划的路径上,根据给出的合理预判距离D开始避障路径规划。
在避障路径规划中的优化计算过程是这样的,定义D*算法规划后的初始栅格路径序列点为Pi=(xi+yi),i∈[1,n],n为总路径点数;
优化约束N点路径的路径平滑度的目标函数为:
路径序列点Pi的位置发生位移时,路径平滑度fs也会发生变化,在满足约束条件的情况下,使路径平滑度fs取最小值,定义路径序列点Pi点的位移方向为梯度方向,采用N点路径法对Pi求导得到梯度:
将位移矢量Δpi=pi-pi-1代入得:
得到位移矢量之后新的路径点为:
其中λ为步长,取值为[0,1],对Pi进行多次迭代同时满足对障碍物距离的约束条件:dis(min)<D(pi,o)<dis(max)。
通过采用上述技术方案,本发明的有益效果是:导航的核心在于反应迅速、路径平滑高效、同时能有效避开障碍物,本发明方法在此原则的基础上,对D*算法进行改进,其对于原有的D*算法引入了障碍物距离影响系数,通过障碍物距离影响系数和多点式的梯度下降法对D*算法进行了优化改进将全局与局部路径规划算法进行融合导航,从而达到本发明的上述能够优化路径平滑度,更符合现实环境中机器人的实际应用的目的。
附图说明
图1是本发明涉及的一种基于SLAM的自主导航避障路径规划方法中4点路径的两相邻序列点之间的位移矢量示意图。
图2是本发明涉及的一种基于SLAM的自主导航避障路径规划方法的流程示意图。
图3是本发明涉及的一种基于SLAM的自主导航避障路径规划方法中D*算法的优化计算流程图。
具体实施方式
为了进一步解释本发明的技术方案,下面通过具体实施例来对本发明进行详细阐述。
本实施例公开的一种基于SLAM的自主导航避障路径规划方法,方法中引入了障碍物距离影响系数ρ,在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响,判断是否开始避障路径规划,具体如下:
所述障碍物距离影响系数ρ,表达式为:
式中λ为配置系数,D为预判距离函数,
预判距离函数是指当前搜索点距离障碍物的距离,用欧式距离表达即为:
式中X表示当前搜索点的横纵坐标xi、yi,O表示障碍物的初始坐标Ox、Oy;
路径规划中在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响,表达式为,
式中ρ=0,表示搜索点距离障碍物的距离D为无限大,该障碍物不在路径搜索的范围内,路径规划时不予考虑;ρ=+∞表示障碍物与搜索点的距离D为零,障碍物已经在规划的路径上,此时才开始避障与传统的D*算法一样,不能解决上述背景技术中存在的问题。本文的目的是想搜索点距离障碍物一定距离的时候已经开始进行避障规划,留有一定的反应时间,而不是搜索到障碍物跟前才开始。因此要根据实际环境信息,给出合理预判距离D开始避障路径规划。
通过上述引入的影响系数的判断,当判断在搜索范围内遇到障碍物,导航***搜寻路径集合,去掉其中包含障碍物路径点的路径,并生成新的子集搜寻新的路径,搜寻到新的路径时进入下述的优化,即对最短路径进行平滑优化得到最优路径发布,否则***发布避障消息,如图2所示。
通过上述过程,已经可以得到初步改善的路径,但传统D*算法规划的路径节点是由各个栅格中点连接而成,这将导致规划的路径曲线存在转折点过多和曲线不平滑的问题,因此本发明对传统D*算法作出了如下改进,
在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径,如图1所示的4个连续的路径点的4点路径,优化的方式以一定的迭代次数通过梯度下降法求出每次优化4点路径中第二个路径点的位移矢量来约束4点路径的平滑度,接着去除掉每次优化4点路径中的第一个路径点,加入第5个路径点重新构成新的4点路径,如此依次循环下去,遍历所有的路径点,直到构不成新的4点路径时停止。优化计算过程是这样的,定义D*算法规划后的初始栅格路径序列点为Pi=(xi+yi),i∈[1,n],n为总路径点数;两相邻序列点之间的位移矢量如图1所示。
优化约束N点路径的路径平滑度的目标函数为:
结合图和路径平滑度fs可以看出路径序列点Pi的位置发生位移时,路径平滑度fs也会发生变化,在满足约束条件的情况下,如何尽可能使路径平滑度取得最小值则是解决问题的关键,定义路径序列点Pi点的位移方向为梯度方向,采用N点路径法对Pi求导得到梯度:
将位移矢量Δpi=pi-pi-1代入得:
得到位移矢量之后新的路径点为:
其中λ为步长,取值为[0,1],大于1将导致路径平滑失败,要得到平滑的路径曲线可以对Pi进行多次迭代同时满足对障碍物距离的约束条件:
dis(min)<D(pi,o)<dis(max)。
新得到的路径点Pi要满足距离障碍物的距离在规定的范围内,其中最大距离和最小距离根据实际情况给出。上述D*算法的优化计算流程如图3所示。
综上,本发明的上述方法对于传统的D*算法,引入了障碍物距离影响系数。通过障碍物距离影响系数和四点式的梯度下降法对D*算法进行了优化改进,运用改良的算法去规划一个全局路径,从而能够优化路径平滑度,更符合现实环境中机器人的实际应用。
上述实施例和图式并非限定本发明的产品形态和式样,任何所属技术领域的普通技术人员对其所做的适当变化或修饰,皆应视为不脱离本发明的专利范畴。
Claims (5)
1.一种基于SLAM的自主导航避障路径规划方法,其特征在于,方法中引入了障碍物距离影响系数ρ,在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响,判断是否开始避障路径规划;在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径,优化的方式以一定的迭代次数通过梯度下降法求出每次优化N点路径中第二个路径点的位移矢量来约束N点路径的平滑度,接着去除掉每次优化N点路径中的第一个路径点,加入第N+1个路径点重新构成新的N点路径,如此依次循环下去,遍历所有的路径点,直到构不成新的N点路径时停止。
2.如权利要求1所述的一种基于SLAM的自主导航避障路径规划方法,其特征在于,所述N点路径为4个连续的路径点的路径。
3.如权利要求1或2所述的一种基于SLAM的自主导航避障路径规划方法,其特征在于,所述障碍物距离影响系数ρ,表达式为:
式中λ为配置系数,D为预判距离函数,
预判距离函数是指当前搜索点距离障碍物的距离,用欧式距离表达即为:
式中X表示当前搜索点的横纵坐标xi、yi,O表示障碍物的初始坐标Ox、Oy;
路径规划中在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响,表达式为,
式中ρ=0,表示搜索点距离障碍物的距离D为无限大,该障碍物不在路径搜索的范围内,路径规划时不予考虑;ρ=+∞表示障碍物与搜索点的距离D为零,障碍物已经在规划的路径上,根据给出的合理预判距离D开始避障路径规划。
4.如权利要求1或2所述的一种基于SLAM的自主导航避障路径规划方法,其特征在于,在避障路径规划中的优化计算过程是这样的,定义D*算法规划后的初始栅格路径序列点为Pi=(xi+yi),i∈[1,n],n为总路径点数;
优化约束N点路径的路径平滑度的目标函数为:
路径序列点Pi的位置发生位移时,路径平滑度fs也会发生变化,在满足约束条件的情况下,使路径平滑度fs取最小值,定义路径序列点Pi点的位移方向为梯度方向,采用N点路径法对Pi求导得到梯度:
将位移矢量Δpi=pi-pi-1代入得:
得到位移矢量之后新的路径点为:
其中λ为步长,取值为[0,1],对Pi进行多次迭代同时满足对障碍物距离的约束条件:dis(min)<D(pi,o)<dis(max)。
5.如权利要求3所述的一种基于SLAM的自主导航避障路径规划方法,其特征在于,在避障路径规划中的优化计算过程是这样的,定义D*算法规划后的初始栅格路径序列点为Pi=(xi+yi),i∈[1,n],n为总路径点数;
优化约束N点路径的路径平滑度的目标函数为:
路径序列点Pi的位置发生位移时,路径平滑度fs也会发生变化,在满足约束条件的情况下,使路径平滑度fs取最小值,定义路径序列点Pi点的位移方向为梯度方向,采用N点路径法对Pi求导得到梯度:
将位移矢量Δpi=pi-pi-1代入得:
得到位移矢量之后新的路径点为:
其中λ为步长,取值为[0,1],对Pi进行多次迭代同时满足对障碍物距离的约束条件:dis(min)<D(pi,o)<dis(max)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410135356.XA CN117873115A (zh) | 2024-01-31 | 2024-01-31 | 一种基于slam的自主导航避障路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410135356.XA CN117873115A (zh) | 2024-01-31 | 2024-01-31 | 一种基于slam的自主导航避障路径规划方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117873115A true CN117873115A (zh) | 2024-04-12 |
Family
ID=90579222
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202410135356.XA Pending CN117873115A (zh) | 2024-01-31 | 2024-01-31 | 一种基于slam的自主导航避障路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117873115A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN118093492A (zh) * | 2024-04-24 | 2024-05-28 | 福建(泉州)先进制造技术研究院 | 一种机器人控制器的io接口电路 |
-
2024
- 2024-01-31 CN CN202410135356.XA patent/CN117873115A/zh active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN118093492A (zh) * | 2024-04-24 | 2024-05-28 | 福建(泉州)先进制造技术研究院 | 一种机器人控制器的io接口电路 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110487279B (zh) | 一种基于改进a*算法的路径规划方法 | |
CN108827335B (zh) | 一种基于单向搜索模型的最短路径规划方法 | |
CN110196602B (zh) | 目标导向集中优化的快速水下机器人三维路径规划方法 | |
CN109931943B (zh) | 无人船舶全局路径规划方法及电子设备 | |
CN113961004A (zh) | 海盗区域船舶航线规划方法、***、电子设备及存储介质 | |
CN114705196B (zh) | 一种用于机器人的自适应启发式全局路径规划方法与*** | |
CN110487290B (zh) | 基于变步长a星搜索的无人驾驶汽车局部路径规划方法 | |
CN113189988B (zh) | 一种基于Harris算法与RRT算法复合的自主路径规划方法 | |
CN113188555A (zh) | 一种移动机器人路径规划方法 | |
CN115167398A (zh) | 一种基于改进a星算法的无人船路径规划方法 | |
CN116952251B (zh) | 路径规划的方法、装置、终端设备及可读存储介质 | |
WO2022007227A1 (zh) | 一种自动泊车的方法和车辆 | |
CN116817947B (zh) | 一种基于变步长机制的任意时路径规划方法 | |
CN117873115A (zh) | 一种基于slam的自主导航避障路径规划方法 | |
CN112945254B (zh) | 一种基于快速拓展随机树的无人车曲率连续路径规划方法 | |
CN113358129A (zh) | 基于Voronoi图的避障最短路径规划方法 | |
Zheng et al. | Global Path Planning for USV Using A-Star Algorithm with Modified Heuristic Function | |
Liu et al. | Research on global path planning for driverless vehicles based on improved A* algorithm | |
Tang et al. | Research on Autonomous Mobile Robot Path Planning Based on M-RRT Algorithm | |
Zhang et al. | Reference Path Generation for Autonomous Driving in Off-road Environment | |
CN113985892B (zh) | 一种基于改进a*算法的智能船舶路径规划方法 | |
Wang et al. | An Improved A-Star Algorithm for Global Path Planning of Unmanned Surface Vehicle | |
Wang et al. | Mobile Robot Path Planning Method Based on an Improved A* Algorithm | |
CN115248604A (zh) | 基于固定翼无人机的多航点最短路径快速生成方法 | |
Guo et al. | Heuristic sparse sampling path planning algorithm for complex scenes |
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 |