基于RRT的动态障碍规避算法制造技术

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

【技术实现步骤摘要】
基于RRT的动态障碍规避算法
本专利技术属于无人机空域动态障碍物规避
，涉及一种基于RRT的动态障碍规避算法。
技术介绍
无人机的成本低、易控制，因此近年来广泛应用于军事与民用领域，但是除了少量的高空长航时等特种用途无人机外，大量的军、民用无人机的飞行高度在5000米以下，中小型无人机的导航精度较差，基本不具备障碍规避能力。为了提高无人机的自主障碍规避能力，全球各无人机研究单位都付出了大量资金与科研力量。无人机路径规划算法是无人机自主避障的核心技术，近年来也得到了广泛关注。按照面向对象的不同分类，无人机路径规划算法可以分为全局规划算法与局部规划算法。当预先具备完整精确的环境信息时，全局规划算法可一次性规划出一条自起点到终点的最优航迹，但是飞行过程若遇到突发威胁，全局规划算法的路径就无法满足避开威胁的要求，必须结合局部规划算法生成实时规避路径。局部路径规划算法要处理的突发威胁有两种：移动威胁与静态威胁。静态威胁指事先未探明的阻挡航路的地形、建筑物、禁飞区等固定目标，对于这些目标局部路径规划算法根据其与本机的位置信息即可生成规避路径。移动威胁指飞机、气球、飞鸟等在空中移动的目标，传统的飞机避障系统如TCAS系统采用的方式是发现移动障碍物立即进行规避动作，爬升、下降或改速转向等，这样的方式虽然能够有效避开障碍物但是当障碍物在规避过程中机动时，飞机又要重新规划避障路径并执行，能量耗费较大。
技术实现思路
本专利技术的目的是提供一种基于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)；步骤六、根据步骤五中求出的合力的方向，移动目标点的位置p(t)至指定的所述合力方向上，从而重新规划出新航点P(x,y,z)。进一步的，步骤一中计算引力场Uatt(p，v)的公式如下：Uatt(p，v)＝αp||ptar(t)-p(t)||m+αv||vtar(t)-v(t)||n(1)，其中，||ptar(t)-p(t)||是欧式距离，||vtar(t)-v(t)||是相对速度的幅值，αp与αv是尺度正参数，m与n是正常数；当αv＝0，m＝2时与传统的引力场的相同的。进一步的，步骤二中Fatt1(p)和Fatt2(v)分别根据下式(2)和(3)计算：Fatt1(p)＝mαp||ptar(t)-p(t)||m-1nRT(2)，Fatt2(v)＝nαv||vtar(t)-v(t)||n-1nVRT(3)，当ptar(t)≠p(t)且vtar(t)≠v(t)时，Fatt(p，v)＝Fatt1(p)+Fatt2(v)。进一步的，步骤三中计算斥力场Urep(p，v)的公式如下：其中，ρ0是单个障碍物影响的最大距离范围，当无人机与障碍物的距离大于ρ0时，斥力势力场对无人机的运动不再有影响；ρs(p，pobs)为本机与目标机之间的距离；vRO为目标与本机的相对距离；ρm(vRO)表示无人机从vRO下降到0所经过的距离，假设在时刻t，无人机向障碍物运动，无人机与障碍物之间的最小距离为ρs(p(t)，pobs(t))，最大负向加速度幅值为amax。进一步的，步骤四中计算斥力Frep1和斥力Frep2的方法如下：其中：Frep(p，v)＝-grad(Urep(p，v))，其中，vRO(t)＝[v(t)-vobs(t)]TnRO，vRO是指向障碍物的方向向量方向，vRO⊥是垂直于障碍物的方向向量，若vRO(t)≤0，即无人机向远离障碍物的方向移动，无需碰撞规避；若vRO(t)＞0即无人机在接近障碍物，则需要进行规避动作。进一步的，规避路径的生成方法具体为：以步骤六中得到的新航点P(x,y,z)为目标点Xgoal，从当前起点Xinit向目标点移动；将Xinit作为根结点，添加到航点集合T中；在无人机的可行域Xfree中取随机点Xrand；找出航点集合T中的与Xrand最近的节点Xnear，并用Xnear来扩展；根据公式(9)计算出临时节点如果临时节点在可行域Xfree中，则临时节点做为RRT的叶子节点，即令新节点则添加新节点Xnew到航点集合T中；否则，舍弃该临时节点重复寻找随机点，直到Xnew＝Xgoal或Xnew∈Area(Xgoal)，则确定了从Xinit到Xgoal的可行路径。本专利技术的有益效果是，本专利技术规避算法能够充分利用原有航路信息，以人工势场法计算得到的避障点作为RRT的航路指引点，克服了RRT原有的随机性强的问题，其规划时间、航迹长度和安全程度等方面都得到了很大的提高。具体实施方式下面对本专利技术进行详细说明。本专利技术提供了一种基于RRT的动态障碍规避算法，通过计算无人机的规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径；其中，规避点的生成方法具体为：步骤一、根据t时刻本机位置ptar(t)，目标点位置p(t)以及本机速度vtar(t)以及目标点速度v(t)，计算出无人机的引力场Uatt(p，v)；Uatt(p，v)＝αp||ptar(t)-p(t)||m+αv||vtar(t)-v(t)||n(1)，其中，||ptar(t)-p(t)|是欧式距离，||vtar(t)-v(t)||是相对速度的幅值，αp与αv是尺度正参数，m与n是正常数；当αv＝0，m＝2时与传统的引力场的相同的。步骤二、再分别计算无人机指向目标单位的方向向量nRT的应用力Fatt1(p)和无人机相对于速度的单位方向nVRT的方向力Fatt2(v)；Fatt1(p)＝mαp||ptar(t)-p(t)||m-1nRT(2)，Fatt2(v)＝nαv||vtar(t)-v(t)||n-1nVRT(3)，当ptar(t)≠p(t)且vtar(t)≠v(t)时，Fatt(p，v)＝Fatt1(p)+Fatt2(v)。步骤三、计算无人机的斥力场Urep(p，v)；其中，ρ0是单个障碍物影响的最大距离范围，当无人机与障碍物的距离大于ρ0时，斥力势力场对无人机的运动不再有影响；ρs(p，pobs)为本机与目标机之间的距离；vRO为目标与本机的相对距离；ρm(vRO)表示无人机从vRO下降到0所经过的距离，假设在时刻t，无人机向障碍物运动，无人机与障碍物之间的最小距离为ρs(p(t)，pobs(t))，最大负向加速度幅值为αmax。步骤四、计算无人机指向障碍物方向nRO的斥力Frep1，以及无人机垂直于障碍物方向nRO⊥的斥力Frep2本文档来自技高网...

【技术保护点】
基于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)；步骤六、根据步骤五中求出的合力的方向，移动目标点的位置p(t)至指定的所述合力方向上，从而重新规划出新航点P(x，y，z)。

【技术特征摘要】
1.基于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)。2.如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤一中计算引力场Uatt(p，v)的公式如下：Uatt(p，v)＝αp||ptar(t)-p(t)||m+αv||vtar(t)-v(t)||n(1)，其中，||ptar(t)-p(t)||是欧式距离，||vtar(t)-v(t)||是相对速度的幅值，αp与αv是尺度正参数，m与n是正常数；当αv＝0，m＝2时与传统的引力场是相同的。3.如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤二中Fatt1(p)和Fatt2(v)分别根据下式(2)和(3)计算：Fatt1(p)＝mαp||ptar(t)-p(t)||m-1nRT(2)，Fatt2(v)＝nαv||vtar(t)-v(t)||n-1nVRT(3)，当ptar(t)≠p(t)且vtar(t)≠v(t)时，Fatt(p，v)＝Fatt1(p)+Fatt2(v)；其中，||ptar(t)-p(t)||是欧式距离，||Vtar(t)-V(t)||是相对速度的幅值，αp与αv是尺度正参数，m与n是正常数。4.如权利要求1所述的基于RRT的动态障...

【专利技术属性】
技术研发人员：赵春晖，王晓华，朱海锋，梁彦，吕洋，潘泉，程承，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西;61