一种适用于矿井无人机巡检航迹规划方法

本发明涉及无人机航迹规划，更具体地说，涉及一种适用于矿井无人机巡检航迹规划方法。

背景技术：

1、煤炭是国民经济发展的重要支撑，随着人们日常生活和工业生产对煤炭的需求不断增长，对矿井的开采强度也随之增强。矿井开采环境恶劣，巷道内突发性瓦斯爆炸，水灾害等煤矿事故威胁着井下工作人员的安全。对煤矿井下巷道、采煤、掘进、运输等重要工作面环境的检测和设备的日常巡检，是确保井下工作人员及设备安全可靠运行进行高效生产的重要保障。随着无人机(unmanned aerial vehicle，简称uav)技术的发展，无人机具有体积小、质量轻和机动性好的优点，将无人机应用于矿井的日常巡检中，能减少大量的人力成本，并对煤矿的设备管理、维护和检修提供可靠的原始数据。由于矿井巷道狭长，存在无规则的障碍物，无人机在矿井环境中的航迹规划是其完成巡检任务的关键。

2、双向rrt方法虽然在解决高维空间航迹规划等问题上有很大进步，但由于该方法是在状态空间中进行随机采样，存在规划时间长、路径转角大、规划航迹与障碍物相切等问题，无人机在按照规划航迹运行时，存在很大的安全隐患，且需要消耗大量的时间在调整方向上，无人机巡检的效率不高。

技术实现思路

1、针对现有技术的问题和不足，本发明提供了一种适用于矿井无人机巡检航迹规划方法，本发明通过改进采样点生成策略、增加扩展步长函数和避障策略实现无人机的航迹规划。

2、为实现上述目的，本发明采用以下技术方案：

3、一种适用于矿井无人机巡检航迹规划方法，基于障碍物占比的采样点生成策略，通过增加扩展步长函数和避障策略，减少采样点的无效采样，同时增加转角约束和避障策略，加快航迹生产速率，实现无人机的航迹规划，具体包括以下步骤：

4、步骤1.无人机的飞行的状态空间为r，其中状态空间中障碍区域为robs；设置无人机航迹规划的起始点xstart、目标点xgoal；在状态空间r中，以xstart为根节点创建随机搜索树t1，以xgoal为根节点创建随机搜索树t2；t1和t2中均只包含了根节点xstart和xgoal，为两个节点定义索引值、父节点、父节点索引值；

5、步骤2.初始化参数，根据公式(1)计算障碍区域空间占比p0：

6、

7、计算两个根节点xstart和xgoal两节点连线的垂直平分线l⊥方程，初始化最大步长smax和最小步长smin，最大步长根据公式(2)确定：

8、

9、步骤3.首先对t1进行扩展，在状态空间中根据公式(3)随机获取采样点xrand,

10、

11、其中p为每次迭代随机生成的数值，p∈[0，1]；

12、步骤4.通过增加扩展步长函数和避障策略，以采样点xrand作为扩展方向对随机搜索树t1进行扩展，新生成节点xnew1加入随机搜索树t1节点中，并对xnew1进行重选父节点和重新布线；随机搜索树t2以新生成节点xnew1作为扩展方向，通过增加扩展步长函数和避障策略，生成新节点xnew2，并对xnew2进行重选父节点和重新布线；

13、步骤5.判断步骤4获取的新节点xnew2与t1中最近节点距离是否小于最小步长smin，如果小于smin，判断xnew2与t1中最近节点连线是否与障碍物发生碰撞，若不发生碰撞，连接两点完成航迹规划，若发生碰撞，则返回步骤3；如果大于smin，则返回步骤3。

14、进一步，利用b样条曲线优化所获取的航迹节点，以获得光滑的无人机航迹。

15、进一步，步骤4中，增加扩展步长函数和避障策略包括以下步骤：

16、步骤4.1.遍历ti找到距离扩展方向节点欧氏距离最近的节点xnear，其中i表示随机搜索树的序号，根据公式(4)计算节点xnear及其父节点xnear―parent和扩展方向节点x3夹角角度θ：

17、

18、其中，x2表示节点xnear，x1表示节点xnear―parent，lx1,x2表示x1和x2之间的距离，lx1,x3表示x1和x3之间的距离，lx2,x3表示x2和x3之间的距离；

19、如果θ>θmax，则放弃本次扩展，θmax设定为90°；

20、步骤4.2.计算xnear到l⊥的距离d，根据公式(5)计算本次扩展步长s：

21、

22、步骤4.3.xnear沿扩展方向节点方向扩展步长s的距离，生成新节点xnew，若xnew与障碍物发生碰撞，放弃本次扩展，若xnew未与障碍物发生碰撞，检测无人机与障碍物安全距离ds内是否存在障碍物，若不存在障碍物，则将xnew加入随机搜索树ti节点中，并对xnew进行重选父节点和重新布线；若存在障碍物，根据障碍物到xnew的距离dobs―new和障碍物相对xnew方向，xnew向相反方向移动ds―dobs―new的距离得到新的x′new，并将新的x′new加入随机搜索树ti节点中，并对x′new进行重选父节点和重新布线。

23、进一步，步骤5中，每次返回步骤3前，判断随机搜索树t1和t2中节点个数是否相同，如果节点数相同，则直接返回步骤3；如果节点数不同，将随机搜索树中节点信息进行互换，保持随机搜索树t1和t2节点数量相同。

24、进一步，节点信息进行互换，创建临时数组ttemp，将随机搜索树t1节点信息复制到ttemp并清空t1，再将随机搜索树t2节点信息复制到t1并清空t2，再将ttemp中节点信息复制到t2中。

25、综上所述，发明具有以下有益效果：

26、本发明针对双向rrt方法的不足，提出一种基于障碍物占比的采样点偏向生成策略的矿用无人机巡检航迹规划方法，通过对状态空间中障碍物占比进行计算，然后根据障碍物占比调整采样点选取概率加快收敛速度，然后根据障碍物占比调整最大步长，通过最近节点状态空间位置动态步长，减少采样点的无效采样，加快航迹生成速率；同时增加了转角约束和避障策略，使生成的航迹符合无人机动力学约束，并采用b样条曲线对生成的航迹进行平滑处理，航迹生成效率和质量大幅提升，提高路径规划速率，本发明便于实现，具有良好的可操作性，有利于提高无人机巡检效率。

技术特征：

1.一种适用于矿井无人机巡检航迹规划方法，其特征在于，基于障碍物占比的采样点生成策略，通过增加扩展步长函数和避障策略，减少采样点的无效采样，同时增加转角约束和避障策略，加快航迹生产速率，实现无人机的航迹规划，具体包括以下步骤：

2.根据权利要求1所述的适用于矿井无人机巡检航迹规划方法，其特征在于，利用b样条曲线优化所获取的航迹节点，以获得光滑的无人机航迹。

3.根据权利要求1所述的适用于矿井无人机巡检航迹规划方法，其特征在于，所述步骤4中，增加扩展步长函数和避障策略包括以下步骤：

4.根据权利要求1所述的适用于矿井无人机巡检航迹规划方法，其特征在于，所述步骤5中，每次返回步骤3前，判断随机搜索树t1和t2中节点个数是否相同，如果节点数相同，则直接返回步骤3；如果节点数不同，将随机搜索树中节点信息进行互换，保持随机搜索树t1和t2节点数量相同。

5.根据权利要求4所述的适用于矿井无人机巡检航迹规划方法，其特征在于，所述节点信息进行互换，创建临时数组ttemp，将随机搜索树t1节点信息复制到ttemp并清空t1，再将随机搜索树t2节点信息复制到t1并清空t2，再将ttemp中节点信息复制到t2中。

技术总结
本发明涉及无人机航迹规划技术领域，现有航迹规划方法存在规划时间长、路径转角大、规划航迹与障碍物相切等问题，存在很大的安全隐患，本发明提供一种适用于矿井无人机巡检航迹规划方法，通过对状态空间中障碍物占比进行计算，根据障碍物占比调整采样点选取概率加快收敛速度，然后根据障碍物占比调整最大步长，通过最近节点状态空间位置动态调整扩展步长，加速路径生成速度，并且增加转角约束和避障策略，使生成航迹符合动力学约束，采用B样条曲线对生成的航迹进行平滑处理，使规划的航迹平滑，本发明优化了航迹长度，提高路径规划速率；该方法便于实现，具有良好的可操作性。

技术研发人员：贾华宇,马楷文,李灯熬,赵菊敏
受保护的技术使用者：太原理工大学
技术研发日：
技术公布日：2024/2/1