基于快速最优随机树的巡飞弹动态路径规划方法

本发明涉及动态环境下的巡飞弹路径规划，具体涉及一种基于快速最优随机树的巡飞弹动态路径规划方法。

背景技术：

1、巡飞弹是通过遥控或自动控制系统来操纵，能够在没有人类操纵的情况下执行各种任务的不载人飞机。巡飞弹具有的高效性、机动性以及低成本，使得巡飞弹广泛应用于各个领域，例如农业、城市管理等领。巡飞弹的使用不仅可以降低人员的风险和安全隐患，还可以提高工作效率，促进社会生产力。考虑到巡飞弹通常在动态环境下执行任务，因此，研究巡飞弹的动态路径规划是一个非常重要的课题。对于动态路径规划，比较受欢迎的是基于网格的算法和基于采样的路径规划算法。然而，基于网格的算法由于其分辨率的限制，难以同时保持精确度和规划速度。基于采样的算法，例如rrt算法，因其搜索能力强，在高维环境中可以快速获取规划的路径，所以被广泛使用。然而，目前的rrt算法及其变体算法，大多集中在研究环境中存在动态障碍物的情况，或者环境中目标可变的情况。对于环境中既存在可变起止点，又存在动态障碍物的情况，目前的研究无法提供一种有力的支持。

技术实现思路

1、本发明的目的在于提供一种基于快速最优随机树的巡飞弹动态路径规划方法。

2、实现本发明目的的技术解决方案为：一种基于快速最优随机树的巡飞弹动态路径规划方法，包括以下步骤：

3、步骤1：构建巡飞弹所在的环境地图，包括已知的障碍物信息，巡飞弹的起始点和目标点；

4、步骤2：初始化随机树；

5、步骤3：利用区域存储策略，预存储障碍物，并区域化存储树的节点；

6、步骤4：更新障碍物位置，更新巡飞弹当前位置，更新树的根节点以及目标节点；

7、步骤5：对树中的k步路径或者可行路径进行判断，判断路径是否被破坏，若破坏，转到步骤6，否则，转到步骤7；

8、步骤6：基于局部连接策略，对路径进行修复；

9、步骤7：对树进行搜索，判断是否找到可行路径，如果找到，转步骤8，否则，转到步骤9；

10、步骤8：基于平衡策略，对可行路径上的节点进行节点重连；

11、步骤9：在空间内采样新节点，并对新节点进行碰撞检测，若无碰撞，对新节点进行重连操作，否则，转到步骤10；

12、步骤10：对根节点进行重连操作，并沿着k步路径或者可行路径移动巡飞弹；

13、步骤11：判断巡飞弹是否到达目标点，若到达了，则结束，否则，转到步骤4。

14、优选的，步骤1中，通过激光雷达，扫描环境并获取点云数据，处理点云数据。生成环境的二维地图；生成的地图用表示，是障碍物区域，是无障碍物的自由区域。巡飞弹的初始点用表示，，巡飞弹的目标点用表示，。

15、优选的，步骤2中，初始化随机树，树包含节点和边，其中，，，以及。

16、优选的，步骤3中，利用区域存储策略，预存储障碍物，并区域化存储树的节点的具体方法为：

17、首先将环境区域化，然后在每个区域预存储障碍物。与此同时，在树的生长过程中，区域化存储树中节点，从而减少花费在搜索最近邻节点以及邻域内节点的时间。

18、优选的，步骤4中，将检测到的新的障碍物或者移动的障碍物的位置进行更新，从而更新整个地图；同时，需要更新巡飞弹在随机树中的位置；最后，更新树的根节点，以及目标节点。

19、优选的，步骤5中，实现过程为：

20、对树中的k步路径或者可行路径进行判断，其中k步路径是在树中寻找距离目标点最近的k个点组成的路径；判断路径是否被障碍物阻挡，即与障碍物相交，若破坏，转到步骤6，否则，转到步骤7；

21、优选的，步骤6中，将步骤5 被破坏的路径分为两段：路径前段，以及路径后段；通过依次对这两段路径上的节点进行直连，并进行碰撞检测，从而进行重新连接。

22、优选的，步骤7中，在树中寻找一条从巡飞弹当前位置到目标点的可行路径，如果存在一条无障碍的路径，转到步骤8，否则，转到步骤9。

23、优选的，步骤8中，基于平衡策略，对可行路径上的节点进行节点重连。平衡公式如下：

24、                       式(1)

25、其中，表示可行路径上已经进行过邻域节点重连的节点个数占可行路径上节点总个数的比例。当随机数小于时，对可行路径上的节点进行重连操作，从而优化路径。

26、优选的，步骤9中，让目标点以预设的概率成为采样点，否则进行均匀采样获取，当获得采样点后，寻找树中最近邻节点，从开始以固定步长向生长，从而进一步产生新节点。对进行碰撞检测，若无碰撞，说明此次采样成功，进行父节点重选以及节点重连的操作。然后，将新节点加入到树中，同时将边加入到中。否则，转到步骤10。

27、优选的，步骤10中，对根节点进行重连操作，以根节点为圆心，以固定长度r为半径，寻找邻域内的节点。对邻域内的节点进行重选父节点操作。在树中寻找一条从巡飞弹当前位置到目标点的可行路径，如果没有可行路径，则寻找距离目标节点最近的节点，并以此回溯父节点，直到找到从根节点开始的k个节点，组成k步路径。将巡飞弹沿着k步路径或者可行路径前进。

28、优选的，步骤11中，判断巡飞弹是否到达目标点，若到达了，则结束路径的搜索。否则，需要继续生长，重复步骤4-步骤11。

29、与现有技术相比，本发明的显著优点为：提出了一种应用于动态环境的区域存储策略，通过区域化存储障碍物，减少颇为耗时的碰撞检测次数，同时区域化存储树，降低节点的空间搜索时间；增加了动态障碍物下的局部连接方法，从而兼顾目标可变以及动态障碍物场景下的实时路径规划；设计了一种探索和利用的平衡策略，实现采样新节点和利用旧树可行路径段上的节点的平衡，提高旧树节点的利用率。

30、下面结合附图对本发明作进一步详细描述。

技术特征：

1.一种基于快速最优随机树的巡飞弹动态路径规划方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的方法，其特征在于，步骤1中，通过激光雷达，扫描环境并获取点云数据，处理点云数据；生成环境的二维地图；生成的地图用表示，是障碍物区域，是无障碍物的自由区域；巡飞弹的初始点用表示，，巡飞弹的目标点用表示，。

3.根据权利要求2所述的方法，其特征在于，步骤2中，初始化随机树，树包含节点和边，其中，，，以及，其中内存储的是可行路径上的节点。

4.根据权利要求1所述的方法，其特征在于，步骤3中，利用区域存储策略，预存储障碍物，并区域化存储树的节点的具体方法为：

5.根据权利要求1所述的方法，其特征在于，步骤4中，将检测到的新的障碍物或者移动的障碍物的位置进行更新，从而更新整个地图；同时，需要更新巡飞弹在随机树中的位置；最后，更新树的根节点，以及目标节点。

6.根据权利要求1所述的方法，其特征在于，步骤5中，对树中的k步路径或者可行路径进行判断，其中k步路径是在树中寻找距离目标点最近的k个点组成的路径。

7.根据权利要求1所述的方法，其特征在于，步骤6中，将步骤5 被破坏的路径分为两段：路径前段，以及路径后段；通过依次对这两段路径上的节点进行直连，并进行碰撞检测，从而进行重新连接。

8.根据权利要求1所述的方法，其特征在于，步骤8中，基于平衡策略，对可行路径上的节点进行节点重连，平衡公式如下：

9.根据权利要求1所述的方法，其特征在于，步骤9中，让目标点以预设的概率成为采样点，否则进行均匀采样获取，当获得采样点后，寻找树中最近邻节点，从开始以固定步长向生长，从而产生新节点；对进行碰撞检测，若无碰撞，则此次采样成功，进行父节点重选以及节点重连的操作；然后，将新节点加入到树中，同时将边加入到中。

10.根据权利要求1所述的方法，其特征在于，步骤10中，对根节点进行重连操作，以根节点为圆心，以固定长度r为半径，寻找邻域内的节点；对邻域内的节点进行重选父节点操作；在树中寻找一条从巡飞弹当前位置到目标点的可行路径，如果没有可行路径，则寻找距离目标节点最近的节点，并以此回溯父节点，直到找到从根节点开始的k个节点，组成k步路径；将巡飞弹沿着k步路径或者可行路径前进。

技术总结
本发明公开了一种基于快速最优随机树的巡飞弹动态路径规划方法，可以有效解决存在动态障碍物，以及存在可变起止点的路径规划问题；采用区域存储策略，通过区域化存储障碍物以及节点，从而加快了算法的收敛；增加了动态障碍物下的局部连接方法，基于局部连接方法，利用有效路径段，快速修复阻塞路径；设计了一种探索和利用的平衡策略，实现采样新节点和利用旧树可行路径段上的节点的平衡，提高旧树节点的利用率。本发明的方法在静态环境下性能依旧优越，在既存在动态障碍物，又存在可变起止点的动态环境下具备快速的能力，并且可以规划出更加平滑的路径，能够更好地满足巡飞弹的飞行需求。

技术研发人员：郭健,高阳,钱晨,吴益飞,李胜,黄卓,吴潇瑞,陈庆伟
受保护的技术使用者：南京理工大学
技术研发日：
技术公布日：2024/11/26