CN114764250A - 一种基于拓展引导的非均匀采样运动规划方法 - Google Patents

一种基于拓展引导的非均匀采样运动规划方法 Download PDF

Info

Publication number
CN114764250A
CN114764250A CN202210455016.6A CN202210455016A CN114764250A CN 114764250 A CN114764250 A CN 114764250A CN 202210455016 A CN202210455016 A CN 202210455016A CN 114764250 A CN114764250 A CN 114764250A
Authority
CN
China
Prior art keywords
expansion
sampling
point
sample
exp
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
CN202210455016.6A
Other languages
English (en)
Other versions
CN114764250B (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.)
Fuzhou University
Original Assignee
Fuzhou University
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 Fuzhou University filed Critical Fuzhou University
Priority to CN202210455016.6A priority Critical patent/CN114764250B/zh
Publication of CN114764250A publication Critical patent/CN114764250A/zh
Application granted granted Critical
Publication of CN114764250B publication Critical patent/CN114764250B/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
    • 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem

Landscapes

  • Engineering & Computer Science (AREA)
  • Business, Economics & Management (AREA)
  • Physics & Mathematics (AREA)
  • Human Resources & Organizations (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Economics (AREA)
  • Remote Sensing (AREA)
  • Strategic Management (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Operations Research (AREA)
  • Theoretical Computer Science (AREA)
  • Marketing (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Game Theory and Decision Science (AREA)
  • Development Economics (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明提供了一种基于拓展引导的非均匀采样运动规划方法，包括如下步骤：步骤S1、狭窄通道拓展起点选择：利用混合高斯采样，得到点集
Figure 130428DEST_PATH_IMAGE001
和点集
Figure 74333DEST_PATH_IMAGE002
，从点集
Figure 791753DEST_PATH_IMAGE002
中筛选狭窄通道拓展起点，组成集合
Figure 124645DEST_PATH_IMAGE003
；步骤S2、导向阶段初始方向选择；步骤S3、局部采样：将当前拓展点更新为
Figure 375236DEST_PATH_IMAGE004
，在
Figure 306021DEST_PATH_IMAGE005
为中心，半径为
Figure 776316DEST_PATH_IMAGE006
，轴线方向为
Figure 417294DEST_PATH_IMAGE007
的半球域内进行均匀采样，采样点集合为
Figure 23856DEST_PATH_IMAGE008
；步骤S4、判断是否仍位于狭窄通道内：应用本技术方案可实现在狭窄通道环境中能快速得到最优解，并且无狭窄通道的环境中仍具有较高的性能。

Description

一种基于拓展引导的非均匀采样运动规划方法
技术领域
本发明涉及移动机器人运动规划技术领域，特别是一种基于拓展引导的非均匀采样运动规划方法。
背景技术
随着现代科学的飞速发展，自动化、智能化水平不断提高，机器人技术也愈发成熟，在工业、服务、医疗等领域得到了广泛应用。但随着应用场景的拓宽，机器所面对的环境也日趋复杂多变。如何在充满不确定性的环境中使机器人安全高效地运作成为了十分重要的课题。运动规划作为机器人应对各类复杂环境完成智能自主作业的关键也因此受到的广泛的关注与研究。
运动规划在移动机器人、工业机械臂、自动驾驶等领域具有重要的地位。它主要根据给定的起始状态和目标状态，在位形空间中得到一条连接起始状态和目标状态的可行路径，进而得到一组不与障碍物发生碰撞的机器人位形序列。目前常见的运动规划方法包括基于搜索的规划方法、基于势场的规划方法和基于采样的规划方法。
为满足一些任务要求，机器人有时需要在狭窄通道中工作。例如，移动机器人在狭长的走廊中通行，机械臂在零件的腔体内部进行焊接工作等。在狭窄通道中规划一条安全可靠的路径对运动规划算法是一个巨大挑战。基于搜索的规划算法主要依赖于对规划空间的网格细分，当环境中存在狭窄通道时，需要增加细分网格的数量以精确地估计位形空间的连通性，但这也将大大增加计算负担。基于势场的规划方法在存在狭窄通道的环境中可能无法找到最优解，需要针对环境设计更复杂的势函数避免陷入局部最优解并实现最优规划，增加了规划问题的解决难度。而基于采样的规划算法不依赖于网格细分并且能够收敛到最优解，在存在狭窄通道的规划问题中可以规避上述问题，有助于提高运动规划的效率。
然而，对于基于采样的规划算法，由于在狭窄通道内采样概率低，狭窄通道区域探索困难等原因，在狭窄通道环境中的规划效率将明显降低。为解决该问题，学界提出了许多改进方法。其中一类方法是识别并探索狭窄通道，该类方法包括Spark PRM和RRV等。SparkPRM通过图的大小和图之间的连通性识别狭窄通道，通过在狭窄通道中构建树以连接不同的图，但所构建的树缺乏拓展引导，效率不高，并且通过狭窄通道后的树需要进行裁剪，这意味着部分构建树的计算在之后的规划当中是无用的，增加了额外计算，不利于算法的后续迭代。RRV通过在拓展失效的采样点附近采样并执行PCA算法识别狭窄通道并进行拓展，但为了探索到探索狭窄通道的入口，需要在非狭窄通道区域额外增加较多采样点，在无狭窄通道的环境会降低算法效率。并且，RRV探索入口时需要采用更小的拓展步距，降低了拓展的效率。总体而言，由于狭窄通道限制了树的拓展，使用该类方法需要增加较多额外计算，并且该类算法主要修改规划算法的拓展阶段，考虑到各类基于采样的规划算法在拓展和重布线等环节间的差异，不易于简便地进行整合。
另一类加快狭窄通道探索速度的方法为偏向采样的方法，该类方法包括高斯采样和桥式测量法等方法。高斯采样通过在无效采样点附近依据高斯分布重新进行采样以提升狭窄通道内采样点密度，但同时也会增加障碍物外侧的采样点密度。桥式测量法相较高斯采样能够使采样点更集中于狭窄通道中，但需要进行更多的碰撞检测，在障碍较多的规划空间内将增加大量计算。偏向采样方法仅更改采样点分布，易于整合入各类基于采样的规划算法中，但由于缺少对狭窄通道的探索，难以使采样点集中在期望区域，往往需要在开阔区域获得较多采样点才能使狭窄通道内采样点密度达到期望数值，使得规划路径通过狭窄通道，不利于规划算法的收敛。
此外，批采样知情集算法(IBP,Informed batch-sampling planner)作为提高采样规划算法收敛速度的改进方法，也能够有效地加快狭窄通道环境中算法的收敛速度。IBPs包括BIT*与IAFMT*等，通过得到初始路径后将采样与拓展限制于能够得到更优路径的超椭球区域内，以避免许多无用的采样与拓展，能够使对位形空间的探索更集中于所期望的狭窄通道区域内。但当狭窄通道外的路径的均远大于狭窄通道内部路径的成本时，知情集算法将无法实现逐步收敛，收敛时间间隔增大，知情集收敛前后大小差异大，规划效率难以得到提升。Meng等人将BIT*与高斯采样算法进行结合，提出了BIT-H*算法以解决狭窄通道等复杂环境的规划问题，但该方法由于将采样点集中于当前迭代路径附近的复杂区域中，缺少对狭窄通道区域足够充分的探索，在一些特定环境中会对采样与拓展起到误导作用。
发明内容
有鉴于此，本发明的目的在于提供一种基于拓展引导的非均匀采样运动规划方法，实现在狭窄通道环境中能快速得到最优解，并且在无狭窄通道的环境中仍具有较高的性能。
为实现上述目的，本发明采用如下技术方案：一种基于拓展引导的非均匀采样运动规划方法，包括如下步骤：
步骤S1、狭窄通道拓展起点选择：利用混合高斯采样，得到点集Vuni和点集Vgau，从点集Vgau中筛选狭窄通道拓展起点，组成集合Vstart；
步骤S2、导向阶段初始方向选择：选择未经过拓展的起始点xstart∈Vstart，进行局部采样并构建向量集合选择出拓展方向q1，q2，以xstart作为当前拓展点xexp，q1作为当前拓展方向q；
步骤S3、局部采样：将当前拓展点更新为xexp←xexp+q，在xexp为中心，半径为rsample，轴线方向为q的半球域内进行均匀采样，采样点集合为 Xsample；
步骤S4、判断是否仍位于狭窄通道内：利用Xsample，通过PCA算法判断当前区域是否仍为狭窄通道区域，若是则执行步骤S5，否则执行步骤S8；
步骤S5、链拓展：以Xsample与xexp构建向量集合Q，从Q中选择出拓展方向q；
步骤S6、判断当前拓展方向q的模长||q||是否大于设定步距lmin，若是则执行步骤S3，否则执行步骤S7；
步骤S7、记录链端点：将当前拓展点xexp加入链端点集合Vend中；
步骤S8、判断当前拓展起点xstart是否已完成两个方向的拓展，若是则执行步骤S9，否则以xstart作为当前拓展点xexp，q2作为当前拓展方向q，返回步骤S3继续；
步骤S9、判断是否所有xstart∈Vstart均已完成拓展，是则执行步骤S10，否则返回步骤S2继续；
步骤S10、转向阶段初始方向选择：选择未经过拓展的链端点xend∈Vend，进行局部采样并构建向量集合选择出拓展方向q作为当前拓展方向，以xend作为当前拓展点xexp；
步骤S11、局部采样：将当前拓展点更新为xexp←xexp+q，在xexp为中心，半径为rsample，轴线方向为q的半球域内进行均匀采样，采样点集合为 Xsample；
步骤S12、判断是否仍位于狭窄通道内：利用Xsample，通过PCA算法判断当前区域是否仍为狭窄通道区域，若是则执行步骤S13，否则执行步骤 S15；
步骤S13、链拓展：以Xsample与xexp构建向量集合Q，从Q中选择出拓展方向q；
步骤S14、判断当前拓展方向q的模长||q||是否大于设定步距lmin，若是则返回步骤S11继续，否则执行步骤S15；
步骤S15、判断是否所有xend∈Vend均已完成拓展，若是则输出包含拓展过程中的所有采样点的集合Vsample，否则返回步骤S10继续；
步骤S16、采样规划算法根据所得到的采样点集合Vsample进行路径规划；
步骤S17、机器人接收到算法规划的可行解，控制机器人运动到目标位置，运动规划结束。
在一较佳的实施例中，所述步骤S1的具体实现方式为：在状态空间C 中进行均匀采样，并其中位于无碰撞空间Cfree中的采样点加入集合Vuni，对于位于障碍物空间Cobs的采样点，依高斯分布进行重新采样，将重新采样得到的采样点加入集合Vgau；对每个点xgau∈Vgau，以xgau为中心，半径rn搜索 Vuni中点，组成集合Xnear；设定阈值η，若点Xnear中点的数目小于等于η，则将对应的xgau加入拓展起始点集合Vstart。
在一较佳的实施例中，所述步骤S2的具体实现方式为：在以xstart为中心，rsample为半径的球域内进行均匀采样，采样点集合为Xsample，用该集合中的点作为终点，xstart作为起点构建向量集合Q；令σ(x,q)表示起点为x，终点为x+q的线段；则所选方向q1,q2定义为：
Figure RE-GDA0003666797700000061
Figure RE-GDA0003666797700000062
在一较佳的实施例中，所述步骤S4及步骤S12的具体实现方式为：使用PCA算法分析Xsample中采样点，λ1、λ2分别为最大和次大的特征值，设定阈值ηPCA，若λ2/λ1<ηPCA，则判断当前探索区域仍为狭窄通道区域，否则判断当前探索区域不为狭窄通道区域。
在一较佳的实施例中，所述步骤S5及步骤S13的具体实现方式为：以 Xsample中的点作为终点，xexp作为起点构建向量集合Q；令σ(x,q)表示起点为x，终点为x+q的线段；则所选方向q可根据公式(1)得到。
在一较佳的实施例中，所述步骤S10的具体实现方式为：令xend.parent 为链端点xend的父节点，引导向量qend＝xend-xend.parent；在xexp为中心，半径为rsample，轴线方向为q的半球域H内进行均匀采样，根据公式(1)而选择出拓展方向qpos，若沿该方向拓展的距离大于等于最小步距lmin，则选择qpos作为拓展方向q，此时拓展方向延续导向拓展阶段的拓展方向，避免了导向拓展阶段因采样点较少，采样点位置不理想导致的拓展中断；若沿 qpos拓展的距离小于最小步距lmin，则在与H相对的半球域H'内进行均匀采样，根据公式(1)而选择出拓展方向qneg；然后判断xend+qneg与xend父节点之间的连线σ(xend+qneg,xend.parent)是否与障碍物空间发生碰撞；若不发生碰撞，则说明沿该拓展方向拓展可能并未绕过“弯道”，而是产生了折返，此时不再进行链拓展；若产生碰撞，则说明沿该方向拓展可以绕过“弯道”，则选qneg作为拓展方向q。
与现有技术相比，本发明具有以下有益效果：
(1)本发明提出一种结合狭窄通道探索与偏置采样的非均匀采样算法，作为一种元算法，可作为采样方法结合任何基于采样的规划算法，提升其收敛性能，适用性强，是具有完备性和渐进最优性的算法；
(2)提高狭窄通道等采样困难区域采样点密度的效率是高斯采样和障碍物引导采样等偏置采样算法的主要性能，本发明通过链状探索将样本重点布置在狭窄通道所在区域，能够快速提高在狭窄通道内采样点密度，让算法快速的从中捕获最优路径解；
(3)对于大多偏置采样算法而言，识别并探索采样困难区域较为困难且耗时较长，环境越复杂算法效率越低，且偏置采样时易于同时增加开阔区域的采样点密度，不利于算法的快速收敛。本发明通过利用向量引导链拓展探索狭窄通道区域，并基于探索信息进行偏置采样，可以提高算法计算效率。
附图说明
图1是本发明优选实施例的流程图；
图2是本发明优选实施例的算法原理说明图；
图3是本发明优选实施例的链式拓展探索图；
图4是本发明优选实施例的转向阶段初始方向选择图；
图5是本发明优选实施例的静态场景一仿真结果图；
图6是本发明优选实施例的静态场景二仿真结果图；
图7是本发明优选实施例的静态场景三仿真结果图；
图8是本发明优选实施例的静态真实实验图。
具体实施方式
下面结合附图及实施例对本发明做进一步说明。
应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式；如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
一种基于拓展引导的非均匀采样运动规划方法，参考图1-8，包括如下步骤：
步骤S1、狭窄通道拓展起点选择：利用混合高斯采样，得到点集Vuni和点集Vgau，从点集Vgau中筛选狭窄通道拓展起点，组成集合Vstart，如图2 最左边图所示；
步骤S2、导向阶段初始方向选择：选择未经过拓展的起始点xstart∈Vstart，进行局部采样并构建向量集合选择出拓展方向q1，q2，以xstart作为当前拓展点xexp，q1作为当前拓展方向q；
步骤S3、局部采样：将当前拓展点更新为xexp←xexp+q，在xexp为中心，半径为rsample，轴线方向为q的半球域内进行均匀采样，采样点集合为 Xsample；
步骤S4、判断是否仍位于狭窄通道内：利用Xsample，通过PCA算法判断当前区域是否仍为狭窄通道区域，若是则执行步骤S5，否则执行步骤S8；
步骤S5、链拓展：以Xsample与xexp构建向量集合Q，从Q中选择出拓展方向q；
步骤S6、判断当前拓展方向q的模长||q||是否大于设定步距lmin，若是则执行步骤S3，否则执行步骤S7；
步骤S7、记录链端点：将当前拓展点xexp加入链端点集合Vend中；
步骤S8、判断当前拓展起点xstart是否已完成两个方向的拓展，若是则执行步骤S9，否则以xstart作为当前拓展点xexp，q2作为当前拓展方向q，返回步骤S3继续；
步骤S9、判断是否所有xstart∈Vstart均已完成拓展，若已完成拓展，如图2中间图所示，则执行步骤S10，否则返回步骤S2继续；
步骤S10、转向阶段初始方向选择：选择未经过拓展的链端点xend∈Vend，进行局部采样并构建向量集合选择出拓展方向q作为当前拓展方向，以xend作为当前拓展点xexp；
步骤S11、局部采样：将当前拓展点更新为xexp←xexp+q，在xexp为中心，半径为rsample，轴线方向为q的半球域内进行均匀采样，采样点集合为 Xsample；
步骤S12、判断是否仍位于狭窄通道内：利用Xsample，通过PCA算法判断当前区域是否仍为狭窄通道区域，若是则执行步骤S13，否则执行步骤 S15；
步骤S13、链拓展：以Xsample与xexp构建向量集合Q，从Q中选择出拓展方向q；
步骤S14、判断当前拓展方向q的模长||q||是否大于设定步距lmin，若是则返回步骤S11继续，否则执行步骤S15；
步骤S15、判断是否所有xend∈Vend均已完成拓展，若已完成拓展，如图2最右边图所示，则输出包含拓展过程中的所有采样点的集合Vsample，否则返回步骤S10继续；
步骤S16、采样规划算法根据所得到的采样点集合Vsample进行路径规划；
步骤S17、机器人接收到算法规划的可行解，控制机器人运动到目标位置，运动规划结束。
本发明的具体流程图如图1所示。
所述步骤S1的具体实现方式为：在状态空间C中进行均匀采样，并其中位于无碰撞空间Cfree中的采样点加入集合Vuni，对于位于障碍物空间Cobs的采样点依高斯分布进行重新采样，将重新采样得到的采样点加入集合Vgau；对每个点xgau∈Vgau，以xgau为中心，rn为半径搜索Vuni中点，组成集合Xnear；设定阈值η，若点Xnear中点的数目小于等于η，则将对应的xgau加入拓展起始点集合Vstart。
所述步骤S2的具体实现方式为：在以xstart为中心，rsample为半径的球域内进行均匀采样，采样点集合为Xsample，用该集合中的点作为终点，xstart作为起点构建向量集合Q；令σ(x,q)表示起点为x，终点为x+q的线段；则所选方向q1,q2定义为：
Figure RE-GDA0003666797700000111
Figure RE-GDA0003666797700000112
所述步骤S4及步骤S12的具体实现方式为：使用PCA算法分析Xsample中采样点，λ1、λ2分别为最大和次大的特征值，设定阈值ηPCA，若λ2/λ1<ηPCA，则判断当前探索区域仍为狭窄通道区域，否则判断当前探索区域不为狭窄通道区域。
所述步骤S5及步骤S13的具体实现方式为：以Xsample中的点作为终点， xexp作为起点构建向量集合Q；令σ(x,q)表示起点为x，终点为x+q的线段；则所选方向q可根据公式(1)得到，如图3所示。
所述步骤S10的具体实现方式为：令xend.parent为链端点xend的父节点，引导向量qend＝xend-xend.parent；在xexp为中心，半径为rsample，轴线方向为q的半球域H内进行均匀采样，根据公式(1)而选择出拓展方向qpos，若沿该方向拓展的距离大于等于最小步距lmin，则选择qpos作为拓展方向q，此时拓展方向延续导向拓展阶段的拓展方向，避免了导向拓展阶段因采样点较少，采样点位置不理想导致的拓展中断；若沿qpos拓展的距离小于最小步距lmin，则在与H相对的半球域H'内进行均匀采样，根据公式(1)而选择出拓展方向qneg；然后判断xend+qneg与xend父节点之间的连线σ(xend+ qneg,xend.parent)是否与障碍物空间发生碰撞；若不发生碰撞，则说明沿该拓展方向拓展可能并未绕过“弯道”，而是产生了折返，此时不再进行链拓展，如图4(a)所示；若产生碰撞，则说明沿该方向拓展可以绕过“弯道”，则选qneg作为拓展方向q，如图4(b)所示。
采用的算法的完整伪代码，具体如下：
Figure RE-GDA0003666797700000121
以下用具体的实验对本发明的实施方式进行详细说明，本发明提供一种完备且渐进最优的运动规划算法，主要通过仿真实验和真实实验来验证其有效性。具体实验设置如下：
仿真实验：
仿真实验在MATLAB R2020b软件中进行。
(1)静态场景一
仿真静态场景地图大小为X＝Y＝200m，所有障碍物皆为静态障碍物。起始点为(10，100)，目标点为(190，100)。如图5所示，为静态场景一的仿真地图，S点表示起始点，G表示目标点，浅色线表示已探索的路径，深色线表示规划时间为15秒时本发明提出的算法规划的路径。
(2)静态场景二
仿真静态场景地图大小为X＝Y＝200m，所有障碍物皆为静态障碍物。起始点为(10，100)，目标点为(190，100)。如图6所示，为静态场景二的仿真地图，S点表示起始点，G表示目标点，浅色线表示已探索的路径，深色线表示规划时间为20秒时本发明提出的算法规划的路径。
(3)静态场景三
仿真静态场景地图大小为X＝Y＝200m，所有障碍物皆为静态障碍物。起始点为(20，170)，目标点为(170，40)。如图7所示，为静态场景三的仿真地图，S点表示起始点，G表示目标点，浅色表示已探索的路径，深色线表示规划时间为2秒时本发明提出的算法规划的路径。
真实实验：
本发明可以直接用于移动机器人的路径规划、机械臂的运动规划还有无人机的轨迹规划中，在Matlab中编写新的采样模块，执行运动规划算法时调用此采样模块进行采样即可。
静态真实实验环境为Microsoft Windows 10、Matlab 2021b，使用Intelrealsense d345i深度相机，搭载STM32的差动智能小车及Intel Core i5-7300HQ，内存为16GB的联想笔记本电脑。场景地图大小为X＝Y＝300cm，所有障碍物皆为已知静态障碍物。起始点为(150，275)，目标点为(150， 275)。如图8所示，为静态真实实验过程，左侧图为实验过程实拍，右侧图为实验过程中本发明提出的算法规划路径时Matlab可视化界面，深色线表示规划得到的最优路径。
以上所述仅为本发明的较佳实施例，对于本领域的普通技术人员而言，根据本发明的教导，设计出不同形式的运动规划算法并不需要创造性的劳动，在不脱离本发明的原理和精神的情况下凡依本发明申请专利范围所做的均等变化、修改、替换和变型，皆应属本发明的涵盖范围。

Claims (6)

1.一种基于拓展引导的非均匀采样运动规划方法，其特征在于，包括如下步骤：
步骤S1、狭窄通道拓展起点选择：利用混合高斯采样，得到点集Vuni和点集Vgau，从点集Vgau中筛选狭窄通道拓展起点，组成集合Vstart；
步骤S2、导向阶段初始方向选择：选择未经过拓展的起始点xstart∈Vstart，进行局部采样并构建向量集合选择出拓展方向q1，q2，以xstart作为当前拓展点xexp，q1作为当前拓展方向q；
步骤S3、局部采样：将当前拓展点更新为xexp←xexp+q，在xexp为中心，半径为rsample，轴线方向为q的半球域内进行均匀采样，采样点集合为Xsample；
步骤S4、判断是否仍位于狭窄通道内：利用Xsample，通过PCA算法判断当前区域是否仍为狭窄通道区域，若是则执行步骤S5，否则执行步骤S8；
步骤S5、链拓展：以Xsample与xexp构建向量集合Q，从Q中选择出拓展方向q；
步骤S6、判断当前拓展方向q的模长||q||是否大于设定步距lmin，若是则执行步骤S3，否则执行步骤S7；
步骤S7、记录链端点：将当前拓展点xexp加入链端点集合Vend中；
步骤S8、判断当前拓展起点xstart是否已完成两个方向的拓展，若是则执行步骤S9，否则以xstart作为当前拓展点xexp，q2作为当前拓展方向q，返回步骤S3继续；
步骤S9、判断是否所有xstart∈Vstart均已完成拓展，是则执行步骤S10，否则返回步骤S2继续；
步骤S10、转向阶段初始方向选择：选择未经过拓展的链端点xend∈Vend，进行局部采样并构建向量集合选择出拓展方向q作为当前拓展方向，以xend作为当前拓展点xexp；
步骤S11、局部采样：将当前拓展点更新为xexp←xexp+q，在xexp为中心，半径为rsample，轴线方向为q的半球域内进行均匀采样，采样点集合为Xsample；
步骤S12、判断是否仍位于狭窄通道内：利用Xsample，通过PCA算法判断当前区域是否仍为狭窄通道区域，若是则执行步骤S13，否则执行步骤S15；
步骤S13、链拓展：以Xsample与xexp构建向量集合Q，从Q中选择出拓展方向q；
步骤S14、判断当前拓展方向q的模长||q||是否大于设定步距lmin，若是则返回步骤S11继续，否则执行步骤S15；
步骤S15、判断是否所有xend∈Vend均已完成拓展，若是则输出包含拓展过程中的所有采样点的集合Vsample，否则返回步骤S10继续；
步骤S16、采样规划算法根据所得到的采样点集合Vsample进行路径规划；
步骤S17、机器人接收到算法规划的可行解，控制机器人运动到目标位置，运动规划结束。
2.根据权利要求1所述的一种基于拓展引导的非均匀采样运动规划方法，其特征在于，所述步骤S1的具体实现方式为：在状态空间C中进行均匀采样，并将其中位于无碰撞空间Cfree中的采样点加入集合Vuni，对于位于障碍物空间Cobs的采样点，依高斯分布进行重新采样，将重新采样得到的采样点加入集合Vgau；对每个点xgau∈Vgau，以xgau为中心，rn为半径搜索Vuni中点，组成集合Xnear；设定阈值η，若点Xnear中点的数目小于等于η，则将对应的xgau加入拓展起始点集合Vstart。
3.根据权利要求1所述的一种基于拓展引导的非均匀采样运动规划方法，其特征在于，所述步骤S2的具体实现方式为：在以xstart为中心，rsample为半径的球域内进行均匀采样，采样点集合为Xsample，用该集合中的点作为终点，xstart作为起点构建向量集合Q；令σ(x,q)表示起点为x，终点为x+q的线段；则所选方向q1,q2定义为：
Figure FDA0003618448050000031
Figure FDA0003618448050000032
4.根据权利要求1所述的一种基于拓展引导的非均匀采样运动规划方法，其特征在于，所述步骤S4及步骤S12的具体实现方式为：使用PCA算法分析Xsample中采样点，λ1、λ2分别为最大和次大的特征值，设定阈值ηPCA，若λ2/λ1＜ηPCA，则判断当前探索区域仍为狭窄通道区域，否则判断当前探索区域不为狭窄通道区域。
5.根据权利要求1所述的一种基于拓展引导的非均匀采样运动规划方法，其特征在于，所述步骤S5及步骤S13的具体实现方式为：以Xsample中的点作为终点，xexp作为起点构建向量集合Q；令σ(x,q)表示起点为x，终点为x+q的线段；则所选方向q可根据公式(1)得到。
6.根据权利要求1所述的一种基于拓展引导的非均匀采样运动规划方法，其特征在于，所述步骤S10的具体实现方式为：令xend.parent为链端点xend的父节点，引导向量qend＝xend-xend.parent；在xexp为中心，半径为rsample，轴线方向为q的半球域H内进行均匀采样，根据公式(1)而选择出拓展方向qpos，若沿该方向拓展的距离大于等于最小步距lmin，则选择qpos作为拓展方向q，此时拓展方向延续导向拓展阶段的拓展方向，避免了导向拓展阶段因采样点较少，采样点位置不理想导致的拓展中断；若沿qpos拓展的距离小于最小步距lmin，则在与H相对的半球域H′内进行均匀采样，根据公式(1)而选择出拓展方向qneg；然后判断xend+qneg与xend父节点之间的连线σ(xend+qneg,xend.parent)是否与障碍物空间发生碰撞；若不发生碰撞，则说明沿该拓展方向拓展可能并未绕过“弯道”，而是产生了折返，此时不再进行链拓展；若产生碰撞，则说明沿该方向拓展可以绕过“弯道”，则选qneg作为拓展方向q。
CN202210455016.6A 2022-04-27 2022-04-27 一种基于拓展引导的非均匀采样运动规划方法 Active CN114764250B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210455016.6A CN114764250B (zh) 2022-04-27 2022-04-27 一种基于拓展引导的非均匀采样运动规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210455016.6A CN114764250B (zh) 2022-04-27 2022-04-27 一种基于拓展引导的非均匀采样运动规划方法

Publications (2)

Publication Number Publication Date
CN114764250A true CN114764250A (zh) 2022-07-19
CN114764250B CN114764250B (zh) 2024-06-07

Family

ID=82364586

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210455016.6A Active CN114764250B (zh) 2022-04-27 2022-04-27 一种基于拓展引导的非均匀采样运动规划方法

Country Status (1)

Country Link
CN (1) CN114764250B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110035087A1 (en) * 2009-08-10 2011-02-10 Samsung Electronics Co., Ltd. Method and apparatus to plan motion path of robot
CN112286202A (zh) * 2020-11-11 2021-01-29 福州大学 一种非均匀采样fmt*的移动机器人路径规划方法
CN114161416A (zh) * 2021-12-06 2022-03-11 贵州大学 基于势函数的机器人路径规划方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110035087A1 (en) * 2009-08-10 2011-02-10 Samsung Electronics Co., Ltd. Method and apparatus to plan motion path of robot
CN112286202A (zh) * 2020-11-11 2021-01-29 福州大学 一种非均匀采样fmt*的移动机器人路径规划方法
CN114161416A (zh) * 2021-12-06 2022-03-11 贵州大学 基于势函数的机器人路径规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
闫富玉;朱晓军;彭飞;: "基于RRTConCon算法的船舶装配拆卸高斯采样路径规划", 中国舰船研究, no. 05, 15 October 2011 (2011-10-15), pages 112 - 116 *

Also Published As

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

Similar Documents

Publication Publication Date Title
CN113219998B (zh) 一种基于改进双向informed-RRT*的车辆路径规划方法
CN106569496B (zh) 一种运动路径的规划方法
CN110609547B (zh) 一种基于可视图引导的移动机器人规划方法
CN112902971B (zh) 基于高斯采样和目标偏向引导的快速扩展随机树算法的机器人运动轨迹计算方法、电子设备及存储介质
CN112286202B (zh) 一种非均匀采样fmt*的移动机器人路径规划方法
CN111610786A (zh) 基于改进rrt算法的移动机器人路径规划方法
Balan et al. Optimal trajectory planning for multiple waypoint path planning using tabu search
Wang et al. Dual-objective collision-free path optimization of arc welding robot
CN112857370A (zh) 一种基于时序信息建模的机器人无地图导航方法
CN115167398A (zh) 一种基于改进a星算法的无人船路径规划方法
Benelmir et al. An efficient autonomous vehicle navigation scheme based on LiDAR sensor in vehicular network
CN110705803A (zh) 基于三角形内心引导rrt算法的路径规划方法
Zhu et al. Online state-time trajectory planning using timed-esdf in highly dynamic environments
Liu et al. NT-ARS-RRT: A novel non-threshold adaptive region sampling RRT algorithm for path planning
CN114764250A (zh) 一种基于拓展引导的非均匀采样运动规划方法
Vonásek et al. Searching multiple approximate solutions in configuration space to guide sampling-based motion planning
Hsu Obstacle avoidance path scheme of snake robot based on bidirectional fast expanding random tree algorithm
Dang Autonomous mobile robot path planning based on enhanced A* algorithm integrating with time elastic band
Abiyev et al. Improved path-finding algorithm for robot soccers
Zhao et al. A compound path planning algorithm for mobile robots
Huang et al. Adaptive Informed RRT*: Asymptotically Optimal Path Planning With Elliptical Sampling Pools in Narrow Passages
Xu et al. Hybrid frontier detection strategy for autonomous exploration in multi-obstacles environment
Suwoyo et al. An Integrated RRT* SMART-A* Algorithm for solving the Global Path Planning Problem in a Static Environment
Vonásek et al. Computation of approximate solutions for guided sampling-based motion planning of 3d objects
Dovgopolik et al. Fast and memory-efficient planning in c-space: Modified bi-directional rrt* algorithm for humanoid robots

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