[发明专利]一种基于快速扩展随机树的无人机航路规划方法

权利要求书

1.一种基于快速扩展随机树的无人机航路路径规划方法，其特征在于步骤如下：

步骤1：障碍物的位置和尺寸已知的无人机航路规划环境Map，起始点x_start，目标区域X_goal，环境X＝X_obs∪X_free，其中X_obs为障碍物所在区域，X_free为无障碍物区域，因此有X_obs∩X_free＝φ；同时对参数进行初始化，包括：最大循环次数N_max，循环计数变量n_count＝1；局部重构半径D_rewire；随机树G＝(V,E)，其中V←{x_start}为随机树的节点列表，E←φ则表示节点之间的扩展关系；

步骤2：判断循环次数n_count是否超过最大循环次数N_max：当循环次数n_count超过设定的最大循环次数N_max时，则跳转至步骤15；当循环次数n_count小于等于最大循环次数N_max时，进入步骤3；

步骤3：循环次数n_count增加1，然后进入步骤4；

步骤4：在环境Map中无障碍物区域随机生成新增节点x_new；节点生成后，进入步骤5；

步骤5：在已知随机树包含的节点列表V中寻找与新节点x_new空间距离最近的节点，并将其记为x_nearest；进入步骤6；

步骤6：判别点x_nearest到x_new之间直线路径上是否含有障碍物，若存在障碍物，则返回步骤2；若不存在障碍物，则进入步骤7；

步骤7：沿着x_nearest所在支线向随机树的根节点进行逐个检测，找到一个与x_new之间的局部路径安全且其父节点与x_new之间存在障碍物的节点，如果将其记为x_best，其父节点记为Parent(x_best)，它们需要满足：{x_best-x_new}∈X_free且找到x_best后，利用二分法在x_best-Parent(x_best)线段上到一个点x_allow，使得x_allow到x_new之间无障碍物，即{x_allow-x_new}∈X_free；然后再次使用二分法在x_allow-x_new线段上找到一个点x_new-parent，使其满足{Parent(x_best)-x_new-parent}∈X_free，从而可以使得x_new通过x_new-parent连接到Parent(x_best)；

步骤8：判断x_new-parent是否创建成功，如果x_new-parent≠φ且x_new-parent≠x_best，则表示x_new-parent创建成功，进入步骤9；否则，进入步骤13；

步骤9：将x_new-parent加入到随机树G上，使其父节点为Parent(x_best)，即将x_new-parent加入到树的节点列表V，并将连线{Parent(x_best)-x_new-parent}加入E；然后进入步骤10；

步骤10：将x_new加入到随机树G上，使其父节点为x_new-parent，即将x_new加入到树的节点列表V，并将连线{x_new-parent-x_new}加入E；然后进入步骤11；

步骤11：在以x_new为圆心，D_rewire为半径的区域内对随机树进行局部重构，即对于该区域内的任一节点x_near，记x_near沿着树到起始点x_start的路径长度为Cost(x_near)，x_new沿着树到起始点x_start的路径长度为Cost(x_new)，如果Cost(x_near)＞Cost(x_new)+Distance(x_near,x_new)，则将x_near的父节点更改为x_new；然后进入步骤12；

步骤12：判别新增节点x_new是否等于目标区域X_goal内，若x_new∈X_goal，则存在可行路径，进入步骤14；若则返回步骤2；

步骤13：将x_new加入到随机树G上，使其父节点为x_best，即将x_new加入到树的节点列表V，并将连线{x_best-x_new}加入E；然后进入步骤11；

步骤14：按照随机树G节点之间的关系生成可行路径path，然后进入步骤15；

步骤15：若无人机航路路径规划成功，则输出无人机航路路径path；若失败，则返回失败。