[发明专利]一种双向动态生长的Informed-RRT*路径规划方法

权利要求书

1.一种双向动态生长的Informed-RRT*路径规划方法，其特征在于，包括以下步骤：

S1、根据障碍物地图建模并二值化，确定起始点和目标点，设定初始参数；

S2、根据障碍物地图初始化两棵随机树；

S3、利用双向搜索融合P概率扇形约束的采样策略进行采样生成采样点；

S4、利用生长转角偏置策略来确定节点扩展时的方向；

S5、利用变步长生长策略来确定节点扩展时的动态扩展步长，产生最新的扩展节点；

S6、进行碰撞检测并更新主、副树图集，输出初始路径；

S7、确定动态椭圆采样范围；

S8、在动态椭圆区域利用动态生长策略来产生新的扩展节点；

S9、在新的扩展节点规定范围内重新寻找父节点和重新连接，寻找代价最小的路径。

2.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S1包括：

障碍物地图包括地图中的障碍物和空白区域，并对两者进行二值化处理，确定初始点q_init和目标点q_goal。

3.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S2包括：

根据障碍物地图初始化两棵随机树中的顶点集V₁、V₂和边集E₁、E₂，并分别把顶点集V₁、V₂和边集E₁、E₂加入到主树和副树的图集G₁、G₂中。

4.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S3包括：

S31、生成P概率扇形约束采样随机值并和规定阈值B比较，若是小于阈值B则采用扇形采样，否则选择普通随机采样。产生采样点q_rand；

S311、扇形采样是指连接上一次更新节点q_new(i-1)和目标点q_goal作为扇形的中线，若是初始采样即以初始点q_init和目标点q_goal作为扇形的中线，以2θ作为扇形夹角，扇形区域可由中线和扇形夹角所确定，扇形采样的采样区即可由扇形区域和地图边界所交区域确定；

S312、普通随机采样的采样区域即由地图边界所确定；

S32、判断树Tree1顶点集V₁中顶点的个数numv1是否小于等于树Tree2顶点集V₂中顶点的个数numv2，若是则把Tree1作为主树，Tree2作为副树。寻找主树上离采样点最近节点q_nearst，通过生长转角偏置和变步长生长完成新的扩展节点q_new的扩展。

5.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S4包括：

生长转角偏置策略是为了改变目标节点扩展的方向，夹角α为直线最近节点q_nearst和目标节点q_goal所得直线L(q_nearst,q_goal)与q_near1和采样节点q_rand形成的直线L(q_nearst,q_rand)之间的夹角，若是没有生长方向偏置则原则生长的节点的在线L(q_nearst,q_goal)上，通过添加可调偏置系数μ(0μ1)变可改变偏置方向。