CN115951681A - 基于栅格化三维空间路径规划的路径搜索域构建方法 - Google Patents
基于栅格化三维空间路径规划的路径搜索域构建方法 Download PDFInfo
- Publication number
- CN115951681A CN115951681A CN202310032050.7A CN202310032050A CN115951681A CN 115951681 A CN115951681 A CN 115951681A CN 202310032050 A CN202310032050 A CN 202310032050A CN 115951681 A CN115951681 A CN 115951681A
- Authority
- CN
- China
- Prior art keywords
- grid
- adjacent
- free
- communication
- space
- 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
- 238000010276 construction Methods 0.000 title description 12
- 238000004891 communication Methods 0.000 claims abstract description 97
- 239000011159 matrix material Substances 0.000 claims abstract description 44
- 238000000034 method Methods 0.000 claims abstract description 28
- 230000000694 effects Effects 0.000 claims abstract description 19
- 238000012163 sequencing technique Methods 0.000 claims abstract description 4
- 230000004888 barrier function Effects 0.000 claims description 10
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 238000004364 calculation method Methods 0.000 abstract description 3
- 238000005457 optimization Methods 0.000 abstract description 3
- 230000000007 visual effect Effects 0.000 abstract description 3
- 238000004088 simulation Methods 0.000 description 17
- 238000010586 diagram Methods 0.000 description 11
- 238000002474 experimental method Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000002537 cosmetic Substances 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 229910052731 fluorine Inorganic materials 0.000 description 1
- 125000001153 fluoro group Chemical group F* 0.000 description 1
- 238000013508 migration Methods 0.000 description 1
- 230000005012 migration Effects 0.000 description 1
- 238000012552 review Methods 0.000 description 1
- 229940055221 silvera Drugs 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000008022 sublimation Effects 0.000 description 1
- 238000000859 sublimation Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Landscapes
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
基于栅格化三维空间路径规划的路径搜索域构建方法，包括以下步骤：离散化三维空间，进行栅格排序和空间定位；根据三维空间障碍物的分布情况，设障碍栅格活性值为Inf，自由栅格活性值为0，形成栅格地图信息矩阵field；初始化栅格联通信息矩阵N；拓展面联通邻接域，第一次更新栅格联通信息矩阵N；拓展棱联通邻接域，第二次更新栅格联通信息矩阵N；拓展点联通邻接域，第三次更新栅格联通信息矩阵N；根据栅格联通信息矩阵N，形成空间联通邻接搜索域集合D。本发明以一个栅格为中心，按照空间的连接关系向外围拓展搜索域，可以实现实质意义的空间搜索，扩大搜索视野，减小路径规划计算的压力，利于后续的路径优化。
Description
技术领域
本发明涉及路径规划技术领域，具体涉及一种基于栅格化三维空间路径规划的路径搜索域构建方法。
背景技术
机器人路径规划领域在路径探索时，需要正确的路径搜索域以供路径构造，大多数研究是以栅格法进行环境建模。正确的路径搜索域可以保证规划路径的安全性。目前已有的保障路径安全的方法有障碍物膨胀法，安全评估系数法。路径探索的方式有降维处理，维度转化等。
搜索域构建在路径规划问题解决中的重要性，如文献[1]:Zhang H Y,Lin W M,Chen A X.Path Planning for the Mobile Robot:A Review[J].Symmetry,2018,10(10).路径规划问题的解决流程，环境建模，构建搜素域，路径构造，路径优化，算法求解。栅格法环境建模，如文献[2]:KennyDaniel；AlexNash；SvenKoenig；ArielFelner.Theta*:Any-Angle Path Planning on Grids[J].Computer Science.2014.
维度转化，如文献[3]：Chagas,Caroline1；Zacarias,Eliakim2；de Lima Silva,LuísAlvaro3；Pignaton de Freitas,Edison1.Hierarchical and smoothed topographicpath planning for large-scale virtual simulation environments.[J].ExpertSystems with Applications.2022.
障碍物膨胀法，如文献[4]:张天瑞,吴宝库,周福强.面向机器人全局路径规划的改进蚁群算法研究[J].计算机工程与应用,2022,58(1):10.
降维处理，如文献[5]：韩忠华,冯兴浩,吕哲,等.一种改进的无人机路径规划环境建模方法[J].信息与控制,2018,47(3):8.
目前，就路径规划研究问题更多是二维规模，针对三维问题的研究方法基本是从二维情况迁移升华所得。并没有完全利用到三维空间宽泛的视野。且仅靠栅格活性来筛选搜索域，会出现“穿越障碍物”，“碰触障碍物”的错误路径情况，如图1和图2所示的情况。
发明内容
为解决上述技术问题，本发明提供一种基于栅格化三维空间路径规划搜索的空间安全联通邻接搜索域构建方法，该方法以一个自由栅格体为中心，通过三种邻接拓展规则向外围拓展正确联通的自由栅格体构建为搜索域。三种邻接拓展规则：面邻接拓展规则、棱邻接拓展规则和点邻接拓展规则；每种邻接拓展规则包含两个判断准则；邻接位置判断准则和邻接联通关系判断准则。正确的栅格联通关系下，路径结果不会出现上述碰触障碍物的问题，可达到空间安全避障的目的。
本发明采取的技术方案为：
基于栅格化三维空间路径规划的路径搜索域构建方法，包括以下步骤：
步骤一：离散化三维空间，进行栅格排序和空间定位；
步骤二：根据三维空间障碍物的分布情况，设障碍栅格活性值为Inf，自由栅格活性值为0，形成栅格地图信息矩阵field；
步骤三：初始化栅格联通信息矩阵N，该矩阵包含每对栅格是否安全联通的判断信息；
步骤四：拓展面联通邻接域，第一次更新栅格联通信息矩阵N；
步骤五：拓展棱联通邻接域，第二次更新栅格联通信息矩阵N；
步骤六：拓展点联通邻接域，第三次更新栅格联通信息矩阵N；
步骤七：根据步骤四、步骤五、步骤六更新后的栅格联通信息矩阵N，形成空间联通邻接搜索域集合D。
所述步骤一中，将三维空间栅格离散化，根据路径规划的精度，确立栅格的粒度和尺寸。基于空间直角坐标系，以每个栅格的中心为空间定位点，记为(xi,yi,zi)，xi,yi,zi表示序号为i的栅格的空间位置基于空间直角坐标系的投影参数，先X轴增量方向，然后Y轴增量方向、最后Z轴增量方向进行栅格排序，记为i，i表示序号为i的栅格。
所述步骤二中，根据三维空间障碍物的分布情况，表达栅格活性值，当栅格i为自由栅格时，则栅格活性值为0；当栅格i为障碍栅格时，则栅格活性值为1。
所述步骤四中，确定中心栅格中心自由栅格体u(xu,yu,zu)，xu,yu,zu表示序号为u的栅格的空间位置基于空间直角坐标系的投影参数，根据面邻接位置关系判断准则和面邻接联通关系判断准则，拓展面联通邻接域，第一次更新栅格联通信息矩阵N。
所述步骤四中，面邻接位置关系判断准则和面邻接联通关系判断准则，具体如下：
S4.1：面邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)，满足
xu-xv|+|yu-yv|+|zu-zv|＝1，则自由栅格体v处于自由栅格体u的面邻接位置处；
xv,yv,zv表示序号为v的栅格的空间位置基于空间直角坐标系的三个坐标轴的投影参数；
S4.2：面邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的面邻接位置处时，若field(u)＝0且field(v)＝0，field(u)、field(v)分别表示u、v处的栅格地图信息，则存在一个辅助栅格体w，该辅助栅格体w即为中心自由栅格体u(xu,yu,zu)自身。
当NumberF-w(u,v)＝1时，则N(u,v)＝1，栅格联通信息矩阵N，N(u,v)表示u到v的联通关系，为“1”联通成立，NumberF-w(u,v)表示面联通辅助栅格个数；
未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)的面邻接联通关系成立。
所述步骤五中，确定中心自由栅格u(xu,yu,zu)，根据棱邻接位置关系判断准则和棱邻接联通关系判断准则，拓展棱联通邻接域，第二次更新栅格联通信息矩阵N。
所述步骤五中，棱位置关系判断准则和棱邻接联通关系判断准则，具体如下：
S5.1:棱邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)邻接，满足：
条件一：|xu-xv|＝1∧|yu-yv|＝1∧|zu-zv|＝0；
条件二：|xu-xv|＝1∧|yu-yv|＝0∧|zu-zv|＝1；
条件三：|xu-xv|＝0∧|yu-yv|＝1∧|zu-zv|＝1，之一，
则未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的棱邻接位置处。
S5.2:棱邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)的棱拓展栅格位置处时，若满足栅格体w同时是中心自由栅格体u(xu,yu,zu)和未知联通自由栅格体v(xv,yv,zv)的面邻接栅格，则栅格体w是一个辅助栅格体。NumberE-w(u,v)表示棱联通辅助栅格个数；当NumberE-w(u,v)＝2，则N(u,v)＝1，未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的棱邻接联通关系成立。
所述步骤六中，确定中心栅格中心自由栅格体u(xu,yu,zu)，根据点邻接位置关系判断准则和点邻接联通关系判断准则，拓展点联通邻接域，第三次更新栅格联通信息矩阵N。
所述步骤六中，点邻接位置关系判断准则和点邻接联通关系判断准则具体如下：
S6.1:点邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)，满足
xu-xv|＝1∧|yu-yv|＝1∧|zu-zv|＝1,则未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的点邻接位置处；
S6.2:点邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)的点邻接位置处时。若满足栅格体w同时是中心自由栅格体u(xu,yu,zu)的棱邻接栅格和未知联通自由栅格体v(xv,yv,zv)的面邻接栅格，则栅格体w是一个辅助栅格体。NumberP-w(u,v)表示点联通辅助栅格个数；当NumberP-w(u,v)＝3时，N(u,v)＝1，未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的点邻接联通关系成立。
所述步骤七中，汇总步骤四、步骤五、步骤六更新后的栅格联通信息矩阵N，形成空间联通邻接搜索域集合D。
本发明一种基于栅格化三维空间路径规划的路径搜索域构建方法，技术效果如下：
1)本发明基于空间安全联通邻接搜索域构建方法，运用于路径规划问题路径构造环节，保证空间安全联通的思路，可以筛选出无碰撞静态障碍物的自由搜索空间。
2)本发明以一个栅格为中心，按照空间的连接关系向外围拓展搜索域，可以实现实质意义的空间搜索，扩大搜索视野，减小路径规划计算的压力，利于后续的路径优化。
附图说明
下面结合附图和实施例对本发明作进一步说明：
图1为背景技术中所述的“穿越”型联通错误示意图。
图2为背景技术中所述的“碰触”型联通错误示意图。
图3为无障碍影响下，应用面拓展规则所得空间安全联通邻接搜索域的仿真结果图。
图4为无障碍影响下，应用棱拓展规则所得空间安全联通邻接搜索域的仿真结果图。
图5为无障碍影响下，应用点拓展规则所得空间安全联通邻接搜索域的仿真结果图。
图6为无障碍影响下，应用面-棱-点拓展规则所得空间安全联通邻接搜索域的仿真结果图。
图7为本发明的流程示意图。
图8为栅格化实验用仿真环境示意图。
具体实施方式
如图7所示，基于栅格化三维空间路径规划的路径搜索域构建方法，包括以下步骤：
步骤一：离散化三维空间，进行栅格排序和空间定位；
步骤二：根据三维空间障碍物的分布情况，设障碍栅格活性值为Inf，自由栅格活性值为0，形成栅格地图信息矩阵field；
步骤三：初始化栅格联通信息矩阵N，该矩阵包含每对栅格是否安全联通的判断信息；
步骤四：拓展面联通邻接域，第一次更新栅格联通信息矩阵N；
步骤五：拓展棱联通邻接域，第二次更新栅格联通信息矩阵N；
步骤六：拓展点联通邻接域，第三次更新栅格联通信息矩阵N；
步骤七：根据步骤四、步骤五、步骤六更新后的栅格联通信息矩阵N，形成空间联通邻接搜索域集合D。
步骤一中：将水下工作空间B×E×F用单个栅格尺寸进行离散化，形成b*e*f的立方栅格域。其中，b行数，e列数，f层数，式(1)中，ceil(·)为向上取整操作。
令栅格序号i等于代表该位置通行情况矩阵参数的索引值。序号i与栅格中心点坐标(xi,yi,zi)的转化公式，如式(2)所示：
式(2)中：i为栅格体序号；xi,yi,zi是其三维空间直角坐标系参数；Mod(·)是取余操作；Floor(·)是向下取整操作。
步骤二中：汇总障碍物分布情况，设障碍栅格活性值为Inf，自由栅格活性值为0。形成栅格地图信息矩阵field。
步骤三中：初始化栅格联通信息矩阵N，矩阵包含每对栅格是否安全联通的判断信息；
步骤四中：确定中心栅格中心自由栅格体u(xu,yu,zu)，xu,yu,zu分别表示序号为u的栅格的空间位置基于空间直角坐标系的三个坐标轴的投影参数，根据面邻接位置关系判断准则和面邻接联通关系判断准则，拓展面联通邻接域，第一次更新栅格联通信息矩阵N。
所述步骤四中，面邻接位置关系判断准则和面邻接联通关系判断准则，具体如下：
S4.1：面邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)，满足|xu-xv|+|yu-yv|+|zu-zv|＝1，则自由栅格体v处于自由栅格体u的面邻接位置处；
xv,yv,zv表示序号为v的栅格的空间位置基于空间直角坐标系的三个坐标轴的投影参数；
S4.2：面邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的面邻接位置处时，若field(u)＝0且field(v)＝0，field(u)、field(v)分别表示u、v处的栅格地图信息；则存在一个辅助栅格体w，该辅助栅格体w即为中心自由栅格体u(xu,yu,zu)自身。
NumberF-w(u,v)表示面联通辅助栅格个数，N(u,v)代表u到v的联通关系，为“1”联通成立；当NumberF-w(u,v)＝1时，则N(u,v)＝1；未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)的面邻接联通关系成立。面邻接联通判断辅助点个数记为NumberF-w(u,v)。
图3为无障碍影响下，应用面拓展规则所得空间安全联通邻接搜索域的仿真结果图。图3中，理想条件下以红色栅格“14”为中心栅格，应用面拓展规则拓展出的安全联通的各面邻接栅格构成的空间联通域。空间定位准则在实现了空间搜索路径的视野，空间联通准则校验了空间联通的安全性，从图3中可看出产生了6个方向选择。
所述步骤五中，确定中心自由栅格u(xu,yu,zu)，根据棱邻接位置关系判断准则和棱邻接联通关系判断准则，拓展棱联通邻接域，第二次更新栅格联通信息矩阵N。
所述步骤五中，棱位置关系判断准则和棱邻接联通关系判断准则，具体如下：
S5.1:棱邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)邻接，满足：
条件一：|xu-xv|＝1∧|yu-yv|＝1∧|zu-zv|＝0；
条件二：|xu-xv|＝1∧|yu-yv|＝0∧|zu-zv|＝1；
条件三：|xu-xv|＝0∧|yu-yv|＝1∧|zu-zv|＝1，之一，
则未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的棱邻接位置处。
S5.2:棱邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)的棱拓展栅格位置处时，若满足栅格体w同时是中心自由栅格体u(xu,yu,zu)和未知联通自由栅格体v(xv,yv,zv)的面邻接栅格，则栅格体w是一个辅助栅格体。
N(u,v)代表u到v的联通关系，为“1”联通成立；
当NumberE-w(u,v)＝2，则N(u,v)＝1，未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的棱邻接联通关系成立。棱邻接联通判断辅助点个数NumberE-w(u,v)。
图4为无障碍影响下，应用棱拓展规则所得空间安全联通邻接搜索域的仿真结果图。图4中，理想条件下以红色栅格“14”为中心栅格，应用棱拓展规则拓展出的安全联通的各棱邻接栅格构成的空间联通域。空间定位准则在实现了空间搜索路径的视野，空间联通准则校验了空间联通的安全性，从图4中可看出产生了12个方向选择。
所述步骤六中，确定中心栅格中心自由栅格体u(xu,yu,zu)，根据点邻接位置关系判断准则和点邻接联通关系判断准则，拓展点联通邻接域，第三次更新栅格联通信息矩阵N。
所述步骤六中，点邻接位置关系判断准则和点邻接联通关系判断准则具体如下：
S6.1:点邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)，满足
xu-xv|＝1∧|yu-yv|＝1∧|zu-zv|＝1,则未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的点邻接位置处；
S6.2:点邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)的点邻接位置处时。若满足栅格体w同时是中心自由栅格体u(xu,yu,zu)的棱邻接栅格和未知联通自由栅格体v(xv,yv,zv)的面邻接栅格，则栅格体w是一个辅助栅格体。NumberP-w(u,v)表示点联通辅助栅格个数，N(u,v)代表u到v的联通关系，为“1”联通成立；当NumberP-w(u,v)＝3时，N(u,v)＝1，未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的点邻接联通关系成立。点邻接联通判断辅助点个数NumberP-w(u,v)。
图5为无障碍影响下，应用点拓展规则所得空间安全联通邻接搜索域的仿真结果图。图5中，理想条件下以红色栅格“14”为中心栅格，应用点拓展规则拓展出的安全联通的各点邻接栅格构成的空间联通域。空间定位准则在实现了空间搜索路径的视野，空间联通准则校验了空间联通的安全性，从图5中可看出产生了8个方向选择。
所述步骤七中，汇总步骤四、步骤五、步骤六更新后的栅格联通信息矩阵N，形成空间联通邻接搜索域集合D。
图6为无障碍影响下，应用面-棱-点拓展规则所得空间安全联通邻接搜索域的仿真结果图。图6中，理想条件下以红色栅格“14”为中心栅格，应用面-棱-点拓展规则拓展出的安全联通的所有邻接栅格构成的空间联通域。空间定位准则在实现了空间搜索路径的视野，空间联通准则校验了空间联通的安全性，从图6中可看出产生了26个方向选择。
实施例：
该方法的具体实施依靠计算机的数学计算，选择Matlabb2017为运行环境。在连接关系判断中嵌套了辅助栅格点的位置关系判断和连接关系判断。因此为避免计算的繁冗，该方法的实施分为三个阶段：第一阶段-面拓展阶段，第二阶段-点拓展阶段和第三阶段-棱拓展阶段。上一阶段的结果也是下一阶段的输入。
实施流程：
设：中心自由栅格体u(xu,yu,zu)，未知联通自由栅格体v(xv,yv,zv)，N(u,v)记录两栅格体的联通关系，MM是栅格总数。
第一阶段-面拓展阶段的实施步骤如下：
步骤1-1.根据面拓展阶段位置关系判断准则，标记中心自由栅格体u(xu,yu,zu)的面拓展栅格位置处未知联通自由栅格体v(xv,yv,zv)；
步骤1-2.根据面拓展阶段连接关系判断准则，判断未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的连接关系，更新,；
步骤1-3.判断,的值是否为0；
步骤1-4.若,不为0。连接关系成立，计算未知联通自由栅格体v(xv,yv,zv)到中心自由栅格体u(xu,yu,zu)的距离，保存至,。否则到步骤1-5；
步骤1-5.令v＝v+1，如果v<＝MM,则转至步骤1-2，否则到步骤1-6；
步骤1-6.令＝+1，如果<＝MM,则转至步骤1-1，否则本阶段结束。
第二阶段-棱拓展阶段的实施步骤如下：
步骤2-1.根据棱拓展阶段位置关系判断准则，标记中心自由栅格体u(xu,yu,zu)的棱拓展栅格位置处未知联通自由栅格体v(xv,yv,zv)；
步骤2-2.根据棱拓展阶段连接关系判断准则，判断未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的连接关系，更新,；
步骤2-3.判断,的值是否为0；
步骤2-4.若,不为0。连接关系成立，计算未知联通自由栅格体v(xv,yv,zv)到中心自由栅格体u(xu,yu,zu)的距离，保存至,。否则到步骤2-5；
步骤2-5.令v＝v+1，如果v<＝MM,则转至步骤2-2，否则到步骤2-6；
步骤2-6.令＝+1，如果<＝MM,则转至步骤2-1，否则本阶段结束。
第三阶段-点拓展阶段的实施步骤如下：
步骤3-1.根据点拓展阶段位置关系判断准则，标记中心自由栅格体u(xu,yu,zu)的点拓展栅格位置处未知联通自由栅格体v(xv,yv,zv)；
步骤3-2.根据点拓展阶段连接关系判断准则，判断未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的连接关系，更新,；
步骤3-3.判断,的值是否为0；
步骤3-4.若,不为0。连接关系成立，计算未知联通自由栅格体v(xv,yv,zv)到中心自由栅格体u(xu,yu,zu)的距离，保存至,。否则到步骤3-5；
步骤3-5.令v＝v+1，如果v<＝MM,则转至步骤3-2，否则到步骤3-6；
步骤3-6.令＝+1，如果<＝MM,则转至步骤3-1，否则本阶段结束。
验证分析：
栅格化实验用三维水下环境，环境中有1000个路径遍历点，环境规模为10*10*10,如图8所示。基于仿真环境，应用基本蚁群算法做两例仿真实验：
仿真实验I组：实施膨胀物障碍处理分平面搜索；
仿真实验Ⅱ组：本发明构建的搜索域。
表1仿真实验I组、Ⅱ组仿真结果对比表
| 对比指标 | 实验I | 实验Ⅱ |
| 仿真地图 | 10*10*10 | 10*10*10 |
| 起始点 | (1000，1000，1000) | (1000，1000，1000) |
| 目标点 | (1，1，1) | (1，1，1) |
| 实际障碍区比例 | 25/1000 | 25/1000 |
| 膨胀处理添加障碍区比例 | 139/1000 | 0 |
| 路径长度(米) | 29.00 | 29.00 |
| 路径曲折度(度) | 1710.00 | 1350.00 |
| 转折次数(次) | 19 | 15 |
| 路径适应度 | 1.18 | 1.03 |
| 路径搜索成功比例 | 62％ | 65％ |
| 路径安全度 | 安全 | 安全 |
从上表1结果分析，两种策略的实施均满足了路径规划AUVA安全航行的基本要求。空间联通邻接域搜索策略，避免了环境建模中不必要的障碍物设置，保证了路径搜索可遍历空间的规模，为最优解创造了更大的解集空间，路径搜索成功比例提高了3％。两组实验路径长度结果差异不大，但实施本发明策略的实验结果，路径转折次数从19次直降到15次，路径曲折度减缓了21.05％，所得到的路径结果适应性明显优于对比组。可见本发明设计的基于栅格化三维空间路径规划搜索的空间安全联通邻接搜索域构建方法，具有明显的合理性和优越性，更助于符合AUV运动学特性的平滑路径规划。
Claims (10)
1.基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于包括以下步骤：
步骤一：离散化三维空间，进行栅格排序和空间定位；
步骤二：根据三维空间障碍物的分布情况，设障碍栅格活性值为Inf，自由栅格活性值为0，形成栅格地图信息矩阵；
步骤三：初始化栅格联通信息矩阵N，该矩阵包含每对栅格是否安全联通的判断信息；
步骤四：拓展面联通邻接域，第一次更新栅格联通信息矩阵N；
步骤五：拓展棱联通邻接域，第二次更新栅格联通信息矩阵N；
步骤六：拓展点联通邻接域，第三次更新栅格联通信息矩阵N；
步骤七：根据步骤四、步骤五、步骤六更新后的栅格联通信息矩阵N，形成空间联通邻接搜索域集合D。
2.根据权利要求1所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤一中，将三维空间栅格离散化，根据路径规划的精度，确立栅格的粒度和尺寸；基于空间直角坐标系，以每个栅格i的中心为空间定位点，记为(xi,yi,zi)，xi,yi,zi表示序号为i的栅格的空间位置基于空间直角坐标系的投影参数，先X轴增量方向，然后Y轴增量方向、最后Z轴增量方向进行栅格排序，记为i。
3.根据权利要求1所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤二中，根据三维空间障碍物的分布情况，表达栅格活性值，当栅格i为自由栅格时，则栅格活性值为0；当栅格i为障碍栅格时，则栅格活性值为1。
4.根据权利要求1所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤四中，确定中心栅格中心自由栅格体u(xu,yu,zu)，xu,yu,zu表示序号为u的栅格的空间位置基于空间直角坐标系的投影参数，根据面邻接位置关系判断准则和面邻接联通关系判断准则，拓展面联通邻接域，第一次更新栅格联通信息矩阵N。
5.根据权利要求4所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤四中，面邻接位置关系判断准则和面邻接联通关系判断准则，具体如下：
S4.1：面邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)，满足|xu-xv|+|yu-yv|+|zu-zv|＝1，则自由栅格体v处于自由栅格体u的面邻接位置处；
xv,yv,zv表示序号为v的栅格的空间位置基于空间直角坐标系的三个坐标轴的投影参数；
S4.2：面邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的面邻接位置处时，若field(u)＝0且field(v)＝0，field(u)、field(v)分别表示u、v处的栅格地图信息，则存在一个辅助栅格体w，该辅助栅格体w即为中心自由栅格体u(xu,yu,zu)自身；
当NumberF-w(u,v)＝1时，则N(u,v)＝1，未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)的面邻接联通关系成立。
6.根据权利要求1所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤五中，确定中心自由栅格u(xu,yu,zu)，根据棱邻接位置关系判断准则和棱邻接联通关系判断准则，拓展棱联通邻接域，第二次更新栅格联通信息矩阵N。
7.根据权利要求6所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤五中，棱位置关系判断准则和棱邻接联通关系判断准则，具体如下：
S5.1:棱邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)和中心自由栅格体u(xu,yu,zu)邻接，满足：
条件一：|xu-xv|＝1∧|yu-yv|＝1∧|zu-zv|＝0；
条件二：|xu-xv|＝1∧|yu-yv|＝0∧|zu-zv|＝1；
条件三：|xu-xv|＝0∧|yu-yv|＝1∧|zu-zv|＝1，之一，
则未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的棱邻接位置处；
S5.2:棱邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)的棱拓展栅格位置处时，若满足栅格体w同时是中心自由栅格体u(xu,yu,zu)和未知联通自由栅格体v(xv,yv,zv)的面邻接栅格，则栅格体w是一个辅助栅格体；NumberE-w(u,v)表示棱联通辅助栅格个数；当NumberE-w(u,v)＝2，则N(u,v)＝1，未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的棱邻接联通关系成立。
8.根据权利要求1所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤六中，确定中心栅格中心自由栅格体u(xu,yu,zu)，根据点邻接位置关系判断准则和点邻接联通关系判断准则，拓展点联通邻接域，第三次更新栅格联通信息矩阵N。
9.根据权利要求8所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤六中，点邻接位置关系判断准则和点邻接联通关系判断准则具体如下：
S6.1:点邻接位置关系判断准则：
若未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)，满足|xu-xv|＝1∧|yu-yv|＝1∧|zu-zv|＝1,则未知联通自由栅格体v(xv,yv,zv)处于中心自由栅格体u(xu,yu,zu)的点邻接位置处；
S6.2:点邻接联通关系判断准则：
未知联通自由栅格体v(xv,yv,zv)在中心自由栅格体u(xu,yu,zu)的点邻接位置处时；若满足栅格体w同时是中心自由栅格体u(xu,yu,zu)的棱邻接栅格和未知联通自由栅格体v(xv,yv,zv)的面邻接栅格，则栅格体w是一个辅助栅格体；NumberP-w(u,v)表示点联通辅助栅格个数；当NumberP-w(u,v)＝3时，N(u,v)＝1，未知联通自由栅格体v(xv,yv,zv)与中心自由栅格体u(xu,yu,zu)的点邻接联通关系成立。
10.根据权利要求8所述基于栅格化三维空间路径规划的路径搜索域构建方法，其特征在于：所述步骤七中，汇总步骤四、步骤五、步骤六更新后的栅格联通信息矩阵N，形成空间联通邻接搜索域集合D。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310032050.7A CN115951681B (zh) | 2023-01-10 | 2023-01-10 | 基于栅格化三维空间路径规划的路径搜索域构建方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310032050.7A CN115951681B (zh) | 2023-01-10 | 2023-01-10 | 基于栅格化三维空间路径规划的路径搜索域构建方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115951681A true CN115951681A (zh) | 2023-04-11 |
| CN115951681B CN115951681B (zh) | 2024-03-15 |
Family
ID=87282420
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310032050.7A Active CN115951681B (zh) | 2023-01-10 | 2023-01-10 | 基于栅格化三维空间路径规划的路径搜索域构建方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115951681B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118051035A (zh) * | 2024-04-15 | 2024-05-17 | 山东大学 | 一种基于局部距离视野强化学习的多agv调度方法 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107806877A (zh) * | 2017-10-11 | 2018-03-16 | 湖北工业大学 | 一种基于蚁群算法的四旋翼无人机的航迹规划优化方法 |
| CN108413963A (zh) * | 2018-02-12 | 2018-08-17 | 淮安信息职业技术学院 | 基于自学习蚁群算法的条形机器人路径规划方法 |
| WO2020134082A1 (zh) * | 2018-12-28 | 2020-07-02 | 歌尔股份有限公司 | 一种路径规划方法、装置和移动设备 |
| CN111707269A (zh) * | 2020-06-23 | 2020-09-25 | 东南大学 | 一种三维环境下的无人机路径规划方法 |
| CN111880534A (zh) * | 2020-07-17 | 2020-11-03 | 桂林电子科技大学 | 一种基于栅格地图的二次路径规划方法 |
| CN112034836A (zh) * | 2020-07-16 | 2020-12-04 | 北京信息科技大学 | 一种改进a*算法的移动机器人路径规划方法 |
| CN113110473A (zh) * | 2021-04-26 | 2021-07-13 | 珠海市一微半导体有限公司 | 基于连通性的区域判定方法、芯片及机器人 |
| CN113405552A (zh) * | 2021-06-17 | 2021-09-17 | 北京理工大学 | 一种飞行器路径规划方法及装置 |
| DE102020119954A1 (de) * | 2020-07-29 | 2022-02-03 | Valeo Schalter Und Sensoren Gmbh | Verfahren zum Erzeugen einer Belegungsgitterkarte für zumindest ein statisches Objekt, Computerprogrammprodukt, computerlesbares Speichermedium sowie Assistenzsystem |
| CN115167465A (zh) * | 2022-08-10 | 2022-10-11 | 哈尔滨理工大学 | 基于人工势场栅格法的无人潜器三维路径规划方法 |
-
2023
- 2023-01-10 CN CN202310032050.7A patent/CN115951681B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107806877A (zh) * | 2017-10-11 | 2018-03-16 | 湖北工业大学 | 一种基于蚁群算法的四旋翼无人机的航迹规划优化方法 |
| CN108413963A (zh) * | 2018-02-12 | 2018-08-17 | 淮安信息职业技术学院 | 基于自学习蚁群算法的条形机器人路径规划方法 |
| WO2020134082A1 (zh) * | 2018-12-28 | 2020-07-02 | 歌尔股份有限公司 | 一种路径规划方法、装置和移动设备 |
| CN111707269A (zh) * | 2020-06-23 | 2020-09-25 | 东南大学 | 一种三维环境下的无人机路径规划方法 |
| CN112034836A (zh) * | 2020-07-16 | 2020-12-04 | 北京信息科技大学 | 一种改进a*算法的移动机器人路径规划方法 |
| CN111880534A (zh) * | 2020-07-17 | 2020-11-03 | 桂林电子科技大学 | 一种基于栅格地图的二次路径规划方法 |
| DE102020119954A1 (de) * | 2020-07-29 | 2022-02-03 | Valeo Schalter Und Sensoren Gmbh | Verfahren zum Erzeugen einer Belegungsgitterkarte für zumindest ein statisches Objekt, Computerprogrammprodukt, computerlesbares Speichermedium sowie Assistenzsystem |
| CN113110473A (zh) * | 2021-04-26 | 2021-07-13 | 珠海市一微半导体有限公司 | 基于连通性的区域判定方法、芯片及机器人 |
| CN113405552A (zh) * | 2021-06-17 | 2021-09-17 | 北京理工大学 | 一种飞行器路径规划方法及装置 |
| CN115167465A (zh) * | 2022-08-10 | 2022-10-11 | 哈尔滨理工大学 | 基于人工势场栅格法的无人潜器三维路径规划方法 |
Non-Patent Citations (2)
| Title |
|---|
| PEI-CHENG SONG: "A parallel compact cuckoo search algorithm for three-dimensional", 《APPLIED SOFT COMPUTING JOURNAL》, pages 1 - 16 * |
| 游天: "一种面向室内导航的通行区域模型及其自动提取算法", 《武汉大学学报(信息科学版)》, vol. 44, no. 2, pages 177 - 184 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118051035A (zh) * | 2024-04-15 | 2024-05-17 | 山东大学 | 一种基于局部距离视野强化学习的多agv调度方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115951681B (zh) | 2024-03-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110487279B (zh) | 一种基于改进a＊算法的路径规划方法 | |
| CN110703762B (zh) | 一种复杂环境下水面无人艇混合路径规划方法 | |
| CN111679692A (zh) | 一种基于改进A-star算法的无人机路径规划方法 | |
| CN106441303A (zh) | 一种基于可搜索连续邻域a*算法的路径规划方法 | |
| Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
| CN105843234A (zh) | 一种uuv对圆形障碍物几何绕行的二维航路规划方法 | |
| CN115951681A (zh) | 基于栅格化三维空间路径规划的路径搜索域构建方法 | |
| CN111080786A (zh) | 基于bim的室内地图模型构建方法及装置 | |
| CN108827311A (zh) | 一种制造车间无人搬运系统路径规划方法 | |
| CN111998858B (zh) | 一种基于改进a*算法的无人机航路规划方法 | |
| CN113341998A (zh) | 一种改进蚁群算法的三维水下欠驱动auv路径规划方法 | |
| Zhang et al. | Beetle colony optimization algorithm and its application | |
| Gu et al. | Path planning for mobile robot in a 2.5‐dimensional grid‐based map | |
| Li et al. | Adaptive sampling-based motion planning with a non-conservatively defensive strategy for autonomous driving | |
| Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
| Jhang et al. | Human‐like motion planning for autonomous parking based on revised bidirectional rapidly‐exploring random tree* with Reeds‐Shepp curve | |
| Gonzalez et al. | Search-based planning with provable suboptimality bounds for continuous state spaces | |
| CN106020213A (zh) | 一种uuv对矩形障碍物几何绕行的二维航路规划方法 | |
| CN113110530B (zh) | 一种针对三维环境下的水下机器人路径规划方法 | |
| CN114815899A (zh) | 基于3d激光雷达传感器的无人机三维空间路径规划方法 | |
| Bulut | The optimal path of robot end effector based on hierarchical clustering and Bézier curve with three shape parameters | |
| Jia-ning | Improved Ant Colony Algorithm for AGV Path Planning | |
| Kui et al. | A Pathfinding Algorithm for Large-Scale Complex Terrain Environments in the Field | |
| CN118113047A (zh) | 基于stca*路径规划和仿射变换控制的多智能体自动巡航方法 | |
| Wang et al. | Efficient 3D Path Planning for Underwater Vehicle Based on Non-Uniformly Modeling |
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 |