[发明专利]基于RRT的动态障碍规避算法

摘要

本发明公开了一种基于RRT的动态障碍规避算法，通过计算无人机的规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径。本发明规避算法能够充分利用原有航路信息，以人工势场法计算得到的避障点作为RRT的航路指引点，克服了RRT原有的随机性强的问题，其规划时间、航迹长度和安全程度等方面都得到了很大的提高。

主权项

基于RRT的动态障碍规避算法，其特征在于，通过计算无人机的规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径；其中，规避点的生成方法具体为：步骤一、根据t时刻本机位置ptar(t)，目标点位置p(t)以及本机速度vtar(t)以及目标点速度v(t)，计算出无人机的引力场；步骤二、再分别计算无人机指向目标单位的方向向量nRT的应用力Fatt1(p)和无人机相对于目标速度的单位方向nVRT的方向力Fatt2(v)；步骤三、计算无人机的斥力场；步骤四、计算无人机指向障碍物方向nRO的斥力Frep1，以及无人机垂直于障碍物方向nRO⊥的斥力Frep2；步骤五、根据式(8)求出合力F，其中，F＝Frep(p，v)+Fatt(p，v) (8)；其中，Frep(p,v)是斥力的合力，Fatt(p,v)是引力的合力；步骤六、根据步骤五中求出的合力的方向，移动目标点的位置p(t)至指定的所述合力方向上，从而重新规划出新航点P(x,y,z)。