CN113655810B - 一种基于速度势场的无人机避障方法和系统 - Google Patents
一种基于速度势场的无人机避障方法和系统 Download PDFInfo
- Publication number
- CN113655810B CN113655810B CN202110963231.2A CN202110963231A CN113655810B CN 113655810 B CN113655810 B CN 113655810B CN 202110963231 A CN202110963231 A CN 202110963231A CN 113655810 B CN113655810 B CN 113655810B
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- obstacle
- speed
- vector
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 64
- 239000013598 vector Substances 0.000 claims abstract description 236
- 238000001514 detection method Methods 0.000 claims description 28
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000004088 simulation Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 9
- 230000008569 process Effects 0.000 description 8
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000012617 force field calculation Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 230000002940 repellent Effects 0.000 description 1
- 239000005871 repellent Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请提供了一种基于速度势场的无人机避障方法和系统,用于控制无人机有效地规避高速移动障碍物,该方法包括:根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力;根据所述引力、改进的斥力场对无人机产生的斥力和速度势场力,计算无人机第i步所受合力,其中,i取值为正整数;根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物。
Description
技术领域
本申请涉及无人机航迹规划技术领域,特别涉及一种基于速度势场的无人机避障方法和系统。
背景技术
无人机(Unmanned Aerial Vehicle,简称UAV)是一种依靠自主操控或受无线遥控的不载人飞行器。近年来,随着无人机相关技术的发展,无人机逐渐从军事领域进入民用领域,特别在农业植保、民用航拍、编队表演等场合发挥了重要作用。
无人机作为一种具备极高机动性、可自动化和智能化飞行的设备,在飞行过程中需要对其运动轨迹进行规划,以保障无人机规避障碍物,同时高效到达目标点。完善的航迹规划和自动避障能够很大程度上减少无人机因碰撞造成损毁,同时提高无人机在复杂环境下执行任务的成功率。
传统无人机避障算法包括Voronoi图法、A星算法、Dijkstra算法、D星算法、人工势场法等等。其中,人工势场法由于结构简单,易于建模和应用,实时性能良好的显著优点,在无人机避障规划中应用广泛。
人工势场法的基本思想是将无人机的飞行运动看作是在障碍物对其产生的斥力与目标点对其产生的引力所组成的合力驱动下的空间运动,其中,障碍物对无人机产生的斥力指引无人机避开障碍物,目标点对无人机产生的引力指引无人机向目标点运动。
目前,人工势场法能够解决无人机飞行过程中实时路径规划和避开低速运动障碍物,无法应对实时、高速移动的障碍物,特别是当障碍物速度接近甚至大于无人机飞行速度时障碍物规避问题。
因此,需要提供一种针对上述现有技术不足的改进技术方案。
发明内容
本申请的目的在于提供一种基于速度势场的无人机避障方法和系统,以解决或缓解上述现有技术中存在的问题。
为了实现上述目的,本申请提供如下技术方案:
本申请提供了一种基于速度势场的无人机避障方法,包括:
步骤S1、根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;
步骤S2、若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力,其中,所述引力场是目标点对无人机产生引力的势场,方向从无人机位置指向目标点,使无人机向目标点运动,所述斥力场是障碍物对无人机产生斥力的势场,控制无人机规避障碍物,速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场,对无人机产生速度势场力,控制无人机规避运动障碍物;
步骤S3、根据步骤S20所述引力、斥力和速度势场力,计算无人机第i步所受合力,其中,i取值为正整数;
步骤S4、根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物。
优选的,在步骤S1中,按照碰撞锥检测条件,对所述无人机和障碍物的碰撞可能性进行检测;所述碰撞锥检测条件为:
其中,vr为无人机与障碍物的相对速度矢量,由vr=v-vob计算得到,v为无人机速度矢量,vob为所述障碍物的速度矢量,Lob为所述无人机到所述障碍物的距离矢量,由Lob=pob-p计算得到,其中pob为所述障碍物的位置,p为所述无人机的位置,dsafe为所述无人机和所述障碍物需要保持的最小安全距离,dsafe=rob+r,其中rob为所述障碍物半径,r为所述无人机的最小安全半径。
优选的,在步骤S2中,按照公式:
计算得到所述速度势场;
其中,Urep,move表示速度势场,ηv为速度势场系数,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,df为当无人机速度矢量v和所在障碍物速度矢量vob不平行时二者所在直线的最短距离矢量,dl为当无人机速度矢量v和所在障碍物速度矢量vob平行时二者所在直线的最短距离矢量,nv为无人机速度矢量的单位方向矢量,nvob为障碍物速度矢量的单位方向矢量,ε是常量,取值范围(0,1),dsafe为无人机和障碍物需要保持的最小安全距离。
优选的,在步骤S2中,根据所述速度势场,按照公式:
计算得到速度势场力;
式中:Frep,move为速度势场力,为梯度符号,Urep,move为速度势场,ηv为速度势场系数,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,ε是常量,取值范围(0,1),dsafe为无人机和障碍物需要保持的最小安全距离。
优选的,根据碰撞锥检测条件,按照公式:
计算得到所述改进的斥力场对无人机产生的斥力;
式中:Frep,cone为所述改进的斥力场对无人机产生的斥力;为梯度符号;Urep,att为根据无人机与目标点之间的位置关系,基于传统人工势场法,得到的改进的斥力场:
式中:Urep,att为改进的斥力场,η为障碍物斥力因子,p为无人机位置,pob为障碍物位置,pgoal为目标点位置,ρ0为障碍物对无人机运动产生影响的最大距离,ρ(p,pob)表示障碍物到无人机的距离,ρ(p,pob)=||pob-p||,ρ(p,pgoal)表示目标点到无人机的距离;
λcone为根据所述碰撞锥检测条件得到的碰撞锥判断系数;
按照公式:
确定碰撞锥判断系数λcone;
其中,vr为无人机与障碍物的相对速度矢量,由vr=v-vob计算得到,v为无人机速度矢量,vob为所述障碍物的速度矢量,Lob为所述无人机到所述障碍物的距离矢量,由Lob=pob-p计算得到,dsafe为所述无人机和所述障碍物需要保持的最小安全距离,dsafe=rob+r,其中rob为所述障碍物半径,r为所述无人机的最小安全半径。
优选的,在步骤S2中,按照公式:
计算得到所述引力场;
式中:Uatt为引力场,ξ为目标点引力因子,pob为所述障碍物的位置,p为所述无人机的位置,ρ(p,pgoal)为无人机到目标点的距离,ρ(p,pgoal)=||pgoal-p||。
优选的,在步骤S2中,根据所述引力场,按照公式:
计算得引力场对无人机产生的引力;
式中:Fatt为引力场对无人机产生的引力;Uatt为引力场;ξ为目标点引力因子;pob为所述障碍物的位置,p为所述无人机的位置,(pgoal-p)为无人机到目标点的距离,(pgoal-p)=||pgoal-p||。
优选的,在步骤S3中,所述无人机第i步所受合力为引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力、速度势场对无人机产生的速度势场力之和,计算公式如下:
Ftotal,move(i)=Fatt(i)+Frep,cone(i)+Frep,move(i)
其中,Ftotal,move(i)为无人机第i步所受到的合力;Fatt(i)为无人机第i步引力场对无人机产生的引力;Frep,cone(i)为无人机第i步受到的所述改进的斥力场对无人机产生的斥力;Frep,move(i)为无人机第i步速度势场对无人机产生的速度势场力。
优选的,在步骤S4中,所述根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物,包括:
步骤S41、根据无人机第i步所受合力,计算得到无人机第i步的方向单位角;
根据无人机第i步所受合力,按照公式:
nftotal,cone(i)=Ftotal,move(i)/||Ftotal,move(i)||
计算得到无人机第i步的方向单位角;
其中,nftotal,cone(i)为所述无人机第i步的方向单位角,Ftotal,move(i)为无人机第i步所受到的合力;
步骤S42、根据所述无人机第i步的方向单位角,根据动态飞行步长,计算无人机下一步的期望位置;其中,所述动态飞行步长表征根据防抖动调节系数对无人机的飞行步长进行动态规划的步长;
按照公式:
pc(i+1)=p(i)+λs·l·(2·(1-λs)nftotal,cone(i-1)+λs·nftotal,cone(i))
计算得到无人机下一步的期望位置;
其中,pc(i+1)无人机下一步的期望位置,p(i)为无人机第i步所在位置,nftotal,cone(i-1)为无人机第i-1步方向单位角,nftotal,cone(i)为无人机第i步方向单位角,l为无人机飞行步长,l=Vxun·Δt,Vxun无人机巡航速度大小,Δt为仿真时间步长,λs为防抖动调节系数:
其中,为预设的调节系数,Δθi为第i-1步与第i步合力方向变化值,nftotal,cone(i-1)·nftotal,cone(i)=cosΔθi,θmax是无人机最大转弯角度,当θmax<Δθi<π时表明无人机的飞行姿态出现抖动;
步骤S43、根据所述无人机第i步所在位置和无人机下一步的期望位置pc(i+1),计算无人机下一步运动的速度矢量;
按照公式:
vc=Δp/Δt=(pc(i+1)-p(i))/Δt
计算得到无人机下一步运动的速度矢量;
其中,vc为无人机下一步运动的速度矢量,Δp为无人机下一步期望位置与无人机第i步所在位置之差,Δp=pc(i+1)-p(i),Δt为仿真时间步长;
步骤S44、根据所述无人机下一步运动的速度矢量,计算所述俯仰角和所述偏航角;
根据所述无人机下一步运动的速度矢量,按照公式:
计算得到所述偏航角;
其中,γc表示所述偏航角,Vc表示表示无人机下一步运动的速度矢量的模,Vc=||vc||;
按照公式:
计算得到所述俯仰角;
其中,χc为所述俯仰角,x,y,z为惯性坐标系中的x,y,z轴,Vcx表示Vc在x轴方向上的分量,Vcy表示Vc在y轴方向上的分量;
步骤S45、根据所述无人机下一步运动的速度矢量、所述俯仰角和所述偏航角,控制无人机避开障碍物。
本申请实施例还提供一种基于速度势场的无人机避障系统,包括:
碰撞检测单元,配置为:根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;
受力计算单元,配置为:若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力,其中,所述引力场是目标点对无人机产生引力的势场,方向从无人机位置指向目标点,使无人机向目标点运动,所述斥力场是障碍物对无人机产生斥力的势场,控制无人机规避障碍物,速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场,对无人机产生速度势场力,控制无人机规避运动障碍物;
合力计算单元,配置为:根据所述引力、斥力和速度势场力,计算无人机第i步所受合力,其中,i取值为正整数;
避障控制单元,配置为:根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物。
与最接近的现有技术相比,本申请实施例的技术方案具有如下有益效果:
本申请实施例中,根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力;根据所述引力、斥力和速度势场力,计算无人机第i步所受合力;根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物。
通过本申请提供的方法,根据碰撞可能性检测,解决无人机飞行过程中出现的非必要规避路径的问题;通过速度势场,有效控制无人机避开高速移动的障碍物;通过动态飞行步长,解决无人机因规避障碍物导致无人机的飞行姿态出现抖动的问题,使无人机的飞行姿态更加平稳。综上所述,本申请提供的无人机避障方法可以得到更有效的航迹路径,缩短无人机到达目标点的时间,降低无人机的飞行成本。此外,本申请还通过在斥力场计算中加入目标点对无人机的影响,得到改进的斥力场,解决了传统人工势场法存在的目标不可达问题。
附图说明
构成本申请的一部分的说明书附图用来提供对本申请的进一步理解,本申请的示意性实施例及其说明用于解释本申请,并不构成对本申请的不当限定。其中:
图1为根据本申请的一些实施例提供的一种基于速度势场的无人机避障方法的流程示意图;
图2为本申请实施例提供的碰撞锥检测条件示意图;
图3为根据本申请的实施例提供的无人机与障碍物的速度关系图;
图4为根据本申请的实施例提供的无人机与障碍物速度矢量方向不平行时的位置关系图;
图5为根据本申请的实施例提供的无人机与障碍物速度矢量方向平行时的位置关系图;
图6为根据本申请的实施例提供的计算无人机下一步运动的速度矢量、俯仰角和偏航角流程图;
图7为根据本申请的一些实施例提供的一种基于速度势场的无人机避障系统的结构示意图。
具体实施方式
下面将对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员所获得的所有其他实施例,都属于本发明保护的范围。
示例性方法
图1为根据本申请的一些实施例提供的一种基于速度势场的无人机避障方法的流程示意图;如图1所示,该无人机避障方法包括:
步骤S1、根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;
根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性,能够解决无人机飞行过程中出现的非必要规避路径的问题。所述非必要规避路径的问题发生的场景为:无人机若按原规划航线进行飞行,与所述障碍物并不会产生碰撞;其中,所述原规划航线是所述无人机自带的飞行控制系统提供的原有飞行路径。由于无人机的障碍物感知系统对障碍物的感知范围比无人机和障碍物需要保持的最小安全距离大,飞行控制系统根据感知到的障碍物所产生的斥力场,控制无人机进行非必要规避,也就是说,无人机沿着规划航线飞行并不会与所述障碍物产生碰撞,该规避路径是不必要的,导致无人机的飞行路径复杂化,增大无人机的飞行成本。
为了解决上述非必要规避路径的问题,根据无人机飞行路径上的障碍物所产生的斥力场,增加碰撞锥检测条件,当满足碰撞锥检测条件,所述障碍物的斥力场才对无人机的飞行路径产生影响,即所述障碍物的斥力场对无人机产生的斥力不为0,否则所述障碍物的斥力场对无人机产生斥力为0。
图2为本申请实施例提供的碰撞锥检测条件示意图;如图2所示,p表示所述无人机的位置,pob表示所述障碍物的位置,Lob表示所述无人机到所述障碍物的距离矢量,计算公式为:
Lob=pob-p
所述无人机和所述障碍物需要保持的最小安全距离为:
dsafe=rob+r
其中,dsafe表示所述无人机和所述障碍物需要保持的最小安全距离,rob是所述障碍物的半径,r是所述无人机的最小安全半径,以所述障碍物的位置pob为圆心、dsafe为半径画一个球,即可得到满足所述碰撞锥检测条件的碰撞区域P。
在检测无人机和障碍物的碰撞可能性过程中,需要考虑所述无人机和所述障碍物的速度矢量,用v表示所述无人机的速度矢量,vob表示所述障碍物的速度矢量,则无人机与障碍物的相对速度矢量vr计算公式为:
vr=v-vob
定义无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量为dc,dc的物理含义为:所述无人机和所述障碍物按照原运动速度和运动方向匀速前进情况下二者最接近时的距离。
设α为所述相对速度矢量vr和所述无人机到所述障碍物的距离矢量Lob的夹角,由图2中可以看出,当所述无人机和所述障碍物按照原运动速度和运动方向匀速前进时,二者之间的距离将会不变或减小,并且,当所述相对速度矢量vr的方向射线从所述无人机的位置p出发会穿过所述碰撞区域P时,即dc<dsafe时,所述无人机与所述障碍物将由可能发生碰撞,需要所述无人机进行障碍物规避,反之则认为所述障碍物对所述无人机的飞行路径没有影响,所述无人机继续沿原规划航线飞行。
由此可见,所述碰撞锥检测条件可表示为:
dc<dsafe
上式表示,当所述无人机到所述障碍物的距离矢量Lob与所述相对速度矢量vr夹角范围在且所述相对速度矢量vr和所述障碍物的位置pob的距离dc小于所述最小安全距离dsafe时,即可判定所述无人机与所述障碍物发生碰撞。
dc由公式:
计算得到。将上式代入所述碰撞锥检测条件:
其中,vr为无人机与障碍物的相对速度矢量,由vr=v-vob计算得到,v为无人机速度矢量,vob为所述障碍物的速度矢量,Lob为所述无人机到所述障碍物的距离矢量,由Lob=pob-p计算得到,其中pob为所述障碍物的位置,p为所述无人机的位置,dsafe为所述无人机和所述障碍物需要保持的最小安全距离,dsafe=rob+r,其中rob为所述障碍物半径,r为所述无人机的最小安全半径。
当上述不等式成立,所述无人机与所述障碍物沿原路线前进有发生碰撞的可能,需要所述无人机进行障碍物规避,反之则认为所述障碍物对所述无人机的飞行路径没有影响,所述无人机继续沿原规划航线飞行。
步骤S2、若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力,其中,所述引力场是目标点对无人机产生引力的势场,方向从无人机位置指向目标点,使无人机向目标点运动,所述改进的斥力场是根据无人机与目标点之间的位置关系,基于传统人工势场法进行改进,得到的障碍物对无人机产生斥力的势场,控制无人机规避障碍物,速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场,对无人机产生速度势场力,控制无人机规避运动障碍物。
1、计算引力场对无人机产生的引力。
所述引力场是目标点对无人机产生引力的势场,方向从无人机位置指向目标点,使无人机向目标点运动。
按照公式:
计算得到所述引力场。
式中:Uatt为引力场,ξ为目标点引力因子,pob为所述障碍物的位置,p为所述无人机的位置,ρ(p,pgoal)=||pgoal-p||为无人机到目标点的距离。
所述引力是引力场的负梯度,按照公式:
计算得引力场对无人机产生的引力;
式中:Fatt为引力场对无人机产生的引力;Uatt为引力场;ξ为目标点引力因子;pob为所述障碍物的位置,p为所述无人机的位置,(pgoal-p)为无人机到目标点的距离,(pgoal-p)=||pgoal-p||。
2、计算改进的斥力场对无人机产生的斥力。
所述改进的斥力场是根据无人机与目标点之间的位置关系,基于传统人工势场法进行改进,得到的障碍物对无人机产生斥力的势场,控制无人机规避障碍物。
传统人工势场法中,斥力场计算公式如下:
式中,Uobs为传统人工势场法中的斥力场,η为障碍物斥力因子,p为无人机位置,pob为障碍物位置,pgoal为目标点位置,ρ0为障碍物对无人机运动产生影响的最大距离,ρ(p,pob)=||pob-p||,表示障碍物到无人机的距离。
由于传统人工势场法中,对斥力场的计算只考虑所述障碍物和所述无人机之间的位置关系,没有考虑到所述目标点与所述无人机之间的位置关系,容易出现目标不可达问题。所述目标不可达问题,具体为:所述无人机行进到距离所述目标点较近位置时,由于所述目标点附近存在一个或者多个所述障碍物,并且所述目标点和所述无人机同时处在所述障碍物的影响范围之内时,所产生的所述无人机无法到达所述目标点的现象。
根据无人机与目标点之间的位置关系,基于传统人工势场法,按照公式:
得到所述改进的斥力场;
式中:Urep,att为改进的斥力场,η为障碍物斥力因子,p为无人机位置,pob为障碍物位置,pgoal为目标点位置,ρ0为障碍物对无人机运动产生影响的最大距离,ρ(p,pob)=||pob-p||,表示障碍物到无人机的距离,ρ(p,pgoal)为目标点到无人机的距离。
上面所述改进的斥力场在构建过程中,在传统人工势场法的斥力场的基础上增加目标点到无人机的距离的变量ρ(p,pgoal),以解决传统人工势场法中存在的目标不可达问题。
基于所述改进的斥力场,按照公式:
计算得到第一斥力,所述第一改进斥力具体为:未考虑碰撞检测条件时,所述改进的斥力场对无人机产生的斥力;
式中:Frep,att为第一斥力,为障碍物指向无人机的梯度,/>为目标点指向无人机的梯度。
由碰撞锥检测条件可知,当满足碰撞锥检测条件时,所述第一斥力对无人机的飞行路径产生影响,即所述第一斥力不为0,否则所述第一斥力为0。
因此,根据所述碰撞锥检测条件,按照公式:
计算得到所述改进的斥力场对无人机产生的斥力,Urep,att为所述改进的斥力场,λcone为根据所述碰撞锥检测条件得到的碰撞锥判断系数:
其中,vr为无人机与障碍物的相对速度矢量,由vr=v-vob计算得到,v为无人机速度矢量,vob为所述障碍物的速度矢量,Lob为所述无人机到所述障碍物的距离矢量,由Lob=pob-p计算得到,dsafe为所述无人机和所述障碍物需要保持的最小安全距离,dsafe=rob+r,其中rob为所述障碍物半径,r为所述无人机的最小安全半径。
3、计算速度势场对无人机产生的速度势场力。
速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场,对无人机产生速度势场力,控制无人机规避运动障碍物。
图3为根据本申请的实施例提供的无人机与障碍物速度关系图。如图3所示,所述无人机的速度矢量与所述障碍物的速度矢量之间的关系有两种场景。
场景一:所述无人机的速度矢量v与所述障碍物的速度矢量vob不同向,即二者方向不平行。如图3中(a)所示,所述障碍物的速度矢量vob与所述无人机的速度矢量为v不同向,即无人机与障碍物之间的运动方向不相同,且通过步骤S1所述碰撞锥检测条件,判定所述无人机与所述障碍物存在发生碰撞的可能性,需要控制无人机避开障碍物。
场景二:所述无人机的速度矢量v与所述障碍物的速度矢量vob同向,即二者方向平行。如图3中(b)所示,障碍物的位置在无人机位置的后方,形成障碍物追赶无人机的态势,当所述无人机的速度矢量与所述障碍物的速度矢量同向,且所述障碍物的速度矢量vob大于所述无人机的速度矢量v时,需要控制无人机避开障碍物。
上述两种场景中,所述障碍物的速度矢量vob大于所述无人机的速度矢量v时,仅仅根据所述无人机的位置和障碍物的位置构建势力场,将无法有效控制无人机避开障碍物。
本申请的实施例中,为了能够有效控制无人机避开高速移动的障碍物,在构建势力场时,不仅考虑所述障碍物的位置和所述无人机的位置,而且将所述障碍物的速度矢量和所述无人机的速度矢量加入到势力场中。具体为:
对于高速移动的障碍物,最有效的规避方法是控制无人机下一步的飞行方向与所述障碍物的速度矢量方向、所述无人机的速度矢量方向均垂直,即添加速度势场,使得所述速度势场对无人机产生的速度势场力的方向与所述障碍物的速度矢量方向、所述无人机的速度矢量方向均垂直。所述速度势场按照公式:
计算得到;
其中,Urep,move表示速度势场,ηv为速度势场系数,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,dsafe为无人机和障碍物需要保持的最小安全距离。
在本申请实施例中,根据无人机速度矢量与障碍物速度矢量的相互关系,分为二者不平行和二者平行两种情况,分别求解上式中的参数dc。
(1)、当无人机速度矢量与障碍物速度矢量不平行时:
图4为根据本申请的实施例提供的无人机与障碍物速度矢量方向不平行时位置关系图。如图4所示,df表示无人机速度矢量v和所在障碍物速度矢量vob不平行时二者所在直线的最短距离矢量,方向由vob所在直线指向v所在直线。v和vob的单位方向矢量分别为nv和nvob,因为v和vob不平行,则nv×nvob≠0。在三维空间中,当无人机速度矢量v和障碍物速度矢量vob不平行时,有如下关系:
||dc||=dc=||df||=df
ndc=ndf
||df||=|Lob·(nv×nvob)
其中,||df||为无人机速度矢量与障碍物速度矢量不平行时二者所在直线的最短距离矢量的模,无方向,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,ndc表示dc的单位方向矢量,ndf为df的单位方向矢量。
根据无人机速度矢量的单位方向矢量nv与障碍物速度矢量的单位方向矢量nvob,按照公式:
计算得到df的单位方向矢量ndf。
dc≤dsafe时,将上述结果代入速度势场Urep,move公式:
计算得到无人机速度矢量与障碍物速度矢量不平行时的速度势场;
其中Urep,move1表示无人机速度矢量与障碍物速度矢量不平行时的速度势场。
当||df||=0时,v和vob共面,表示无人机与障碍物处于同一个二维平面,且无人机速度矢量与障碍物速度矢量不平行时二者所在直线的最短距离矢量的模等于0,此时有df趋向于0,dc趋向于0,即df→0,dc→0;
从而得到:
Urep,move1→∞;
Urep,move1→∞,表示无人机速度矢量与障碍物速度矢量不平行时的速度势场对无人机产生的速度势场力趋向于无穷大,从而导致无人机的飞行控制系统失效或者无人机出现飞行震荡的危险。因此,引入常数ε,其意义在于:当小于ε时,Urep,move1达到数值上限,方向不变。此时,Urep,move1表示为:
由此可得,当无人机速度矢量与障碍物速度矢量不平行时的速度势场可表示为下列分段函数:
(2)、当无人机速度矢量与障碍物速度矢量平行时:
图5为根据本申请的实施例提供的无人机与障碍物速度矢量方向平行时位置关系图。如图5所示,dl是当无人机速度矢量与障碍物速度矢量平行时,无人机速度矢量v和障碍物速度矢量vob所在直线的距离矢量,方向由vob所在直线指向v所在直线。
Lob在v方向的分量Lob,v为:
Lob,v=(Lob·nv)·nv
Lob在垂直于v方向的分量为dl的反向矢量,因此,按照公式:
dl=Lob,v-Lob
计算得到dl。
当||dl||=0时,v和vob共线,||dl||=dl为无人机速度矢量v和障碍物速度矢量vob二者所在直线最短距离矢量的模,无方向。
当||dl||=0时,dl的单位方向矢量无法确定,导致dc的单位方向矢量也无法确定。由于无人机通常不进行垂直运动,所以无人机速度矢量的方向不会与垂直方向重合,即nv×e3=nv×[0 0 1]T≠0。在本申请实施例中,设此时的速度势场方向与无人机速度矢量方向、垂直方向同时垂直,即||dl||=0时,ndl=nv×e3,即:
||dl||=||Lob,v-Lob||
其中,ndl表示dl的单位方向矢量,Lob,v为Lob在v方向的分量,nv为无人机速度矢量的单位方向矢量,e3=[0 0 1]T。
当无人机速度矢量与障碍物速度矢量平行时,由图5所示的几何关系可知:
dc=dl
||dc||<dsafe
当无人机速度矢量与障碍物速度矢量平行时的速度势场可表示为:
其中,Urep,move2表示无人机速度矢量与障碍物速度矢量平行时的速度势场。
当||dl||=0时,无人机速度矢量v和障碍物速度矢量vob所在直线趋向于0,出现导致Urep,move2→∞,即Urep,move2产生的速度势场力过大导致无人机飞行控制系统失效或者无人机出现飞行震荡的危险。因此,引入常数ε,其意义在于:当/>小于ε时,Urep,move2达到数值上限,方向不变。此时,Urep,move2表示为:
由此可得,当无人机速度矢量与障碍物速度矢量平行时的速度势场表示为下列分段函数:
综合无人机速度矢量与障碍物速度矢量的相互关系中,二者不平行时的速度势场Urep,move1和二者平行时的速度势场Urep,move2两种情况,速度势场的通用公式表示为:
其中,Urep,move表示速度势场,ηv为速度势场系数,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,df为无人机速度矢量v和所在障碍物速度矢量vob不平行时二者所在直线的最短距离矢量,dl为无人机速度矢量v和所在障碍物速度矢量vob平行时二者所在直线的最短距离矢量,nv为无人机速度矢量的单位方向矢量,nvob为障碍物速度矢量的单位方向矢量,ε是常量,取值范围(0,1),dsafe为无人机和障碍物需要保持的最小安全距离。
根据所述速度势场,按照公式:
计算得到速度势场力;
式中:Frep,move为速度势场力,为梯度符号,Urep,move为速度势场,ηv为速度势场系数,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,ε是常量,取值范围(0,1),dsafe为无人机和障碍物需要保持的最小安全距离。
步骤S3、根据步骤S2所述引力、改进的斥力场对无人机产生的斥力和速度势场力,计算无人机第i步所受合力,其中,i取值为整数。
所述无人机第i步所受合力为引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力、速度势场对无人机产生的速度势场力之和,计算公式如下:
Ftotal,move(i)=Fatt(i)+Frep,cone(i)+Frep,move(i)
其中,Ftotal,move(i)为无人机第i步所受到的合力;Fatt(i)为无人机第i步引力场对无人机产生的引力;Frep,cone(i)为无人机第i步受到的所述改进的斥力场对无人机产生的斥力;Frep,move(i)为无人机第i步速度势场对无人机产生的速度势场力。
步骤S4、根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物,如图6所示。
步骤S41、根据无人机第i步所受合力,计算得到无人机第i步的方向单位角。
所述无人机第i步的方向单位角为:
nftotal,cone(i)=Ftotal,move(i)/||Ftotal,move(i)||
其中,nftotal,cone(i)为所述无人机第i步的方向单位角,Ftotal,move(i)为无人机第i步所受到的合力。
步骤S42、根据所述无人机第i步的方向单位角,计算无人机下一步的期望位置。
无人机下一步的期望位置为:
pc(i+1)=p(i)+nftotal,cone(i)·l
其中,pc(i+1)无人机下一步的期望位置,p(i)为无人机第i步所在位置,l为无人机飞行步长。
根据碰撞锥检测条件,所述改进的斥力场对无人机产生的斥力由碰撞锥判断系数λcone进行确定,当λcone为1时,表示满足碰撞锥检测条件,所述障碍物的改进的斥力场对无人机产生的斥力不为0,当λcone为0时,所述障碍物的斥力场对无人机产生斥力为0。λcone在0和1之间切换的瞬间,所述斥力场从无到有进行突然切换,此时如果改进的斥力场对无人机产生的斥力过大,将会导致无人机的飞行方向随之产生突然变向,从而导致无人机的飞行姿态出现抖动。在本申请实施例中,通过动态飞行步长解决上述问题。所述动态飞行步长表示当无人机的飞行姿态出现抖动时,无人机下一步的期望位置不再根据无人机飞行控制系统所规定的无人机飞行步长确定,而是根据防抖动调节系数进行调节,采用动态规划步长,具体为:
当检测到第i步出现抖动时,无人机下一步的期望位置pc(i+1)由公式:
pc(i+1)=p(i)+λs·l·(2·(1-λs)nftotal,cone(i-1)+λs·nftotal,cone(i))
计算得到;
其中,pc(i+1)无人机下一步的期望位置,p(i)为无人机第i步所在位置,nftotal,cone(i-1)为无人机第i-1步方向单位角,nftotal,cone(i)为无人机第i步方向单位角,l为无人机飞行步长,l=Vxun·Δt,Vxun无人机巡航速度大小,Δt为仿真时间步长,λs为防抖动调节系数:
其中,为预设的调节系数,nftotal,cone(i-1)·nftotal,cone(i)=cosΔθi,Δθi为第i-1步与第i步合力方向变化值,θmax是无人机最大转弯角度,当θmax<Δθi<π时表明无人机的飞行姿态出现抖动。
优选的,时,无人机下一步的期望位置pc(i+1)为:
步骤S43、根据所述无人机第i步所在位置和无人机下一步的期望位置pc(i+1),计算无人机下一步运动的速度矢量。
根据所述无人机第i步所在位置和无人机下一步的期望位置pc(i+1),按照公式:
vc=Δp/Δt=(pc(i+1)-p(i))/Δt
计算得到无人机下一步运动的速度矢量;
其中,vc为无人机下一步运动的速度矢量,Δp为无人机下一步期望位置与无人机第i步所在位置之差,Δp=pc(i+1)-p(i),Δt为仿真时间步长;
步骤S44、根据所述无人机下一步运动的速度矢量,计算所述俯仰角和所述偏航角。
根据所述无人机下一步运动的速度矢量,按照公式:
计算得到所述偏航角;
其中,γc表示所述偏航角,Vc表示表示无人机下一步运动的速度矢量的模,Vc=||vc||;
根据所述无人机下一步运动的速度矢量,按照公式:
计算得到所述俯仰角;
其中,χc为所述俯仰角,x,y,z为惯性坐标系中x,y,z轴,Vcx表示Vc在x轴方向上的分量,Vcy表示Vc在y轴方向上的分量;
步骤S45、根据所述无人机下一步运动的速度矢量、所述俯仰角和所述偏航角,控制无人机避开障碍物。
本申请实施例中,根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力;根据所述引力、斥力和速度势场力,计算无人机第i步所受合力;根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物。
通过本申请提供的方法,根据碰撞可能性检测,解决无人机飞行过程中出现的非必要规避路径的问题;通过速度势场,有效控制无人机避开高速移动的障碍物;通过动态飞行步长,解决无人机因规避障碍物导致无人机的飞行姿态出现抖动的问题,使无人机的飞行姿态更加平稳。综上所述,通过本申请实施例中所提供的方法,可以有效避开速度大于无人机速度的障碍物,使得无人机对障碍物的规避更灵活、无人机的飞行姿态更稳定。
示例性系统
图7为根据本申请的一些实施例提供的一种基于速度势场的无人机避障系统的结构示意图;如图7所示,该基于速度势场的无人机避障系统包括:
碰撞检测单元601,配置为:根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;
受力计算单元602,配置为:若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力,其中,所述引力场是目标点对无人机产生引力的势场,方向从无人机位置指向目标点,使无人机向目标点运动,所述改进的斥力场是障碍物对无人机产生斥力的势场,控制无人机规避障碍物,速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场,对无人机产生速度势场力,控制无人机规避运动障碍物;
合力计算单元603,配置为:根据所述引力、改进的斥力场对无人机产生的斥力和速度势场力,计算无人机第i步所受合力,其中,i取值为正整数;
避障控制单元604,配置为:根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物。
本申请实施例提供的一种基于速度势场的无人机避障系统能够实现上述任一基于速度势场的无人机避障方法实施例的步骤、流程,并达到相同的技术效果,在此不再一一赘述。
以上所述仅为本申请的优选实施例,并不用于限制本申请,对于本领域的技术人员来说,本申请可以有各种更改和变化。凡在本申请的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本申请的保护范围之内。
Claims (10)
1.一种基于速度势场的无人机避障方法,其特征在于,包括:
步骤S1、根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;
步骤S2、若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力;其中,所述引力场是目标点对无人机产生引力的势场,方向从无人机位置指向目标点,使无人机向目标点运动;所述改进的斥力场是根据无人机与目标点之间的位置关系,基于传统人工势场法进行改进,得到的障碍物对无人机产生斥力的势场,控制无人机规避障碍物;速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场,对无人机产生速度势场力,控制无人机规避运动障碍物;
步骤S3、根据步骤S20所述引力、改进的斥力场对无人机产生的斥力和速度势场力,计算无人机第i步所受合力,其中,i取值为正整数;
步骤S4、根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物;
其中,根据无人机第i步所受合力,按照公式:
nftotal,cone(i)=Ftotal,move(i)/‖Ftotal,move(i)‖
计算得到无人机第i步方向单位角;其中,nftotal,cone(i)为所述无人机第i步的方向单位角,Ftotal,move(i)为无人机第i步所受到的合力。
2.根据权利要求1所述的方法,其特征在于,在步骤S1中,按照碰撞锥检测条件,对所述无人机和障碍物的碰撞可能性进行检测;所述碰撞锥检测条件为:
其中,vr为无人机与障碍物的相对速度矢量,由vr=v-vob计算得到,v为无人机速度矢量,vob为所述障碍物的速度矢量,Lob为所述无人机到所述障碍物的距离矢量,由Lob=pob-p计算得到,其中pob为所述障碍物的位置,p为所述无人机的位置,dsafe为所述无人机和所述障碍物需要保持的最小安全距离,dsafe=rob+r,其中rob为所述障碍物半径,r为所述无人机的最小安全半径。
3.根据权利要求2所述的方法,其特征在于,根据碰撞锥检测条件,按照公式:
Frep,cone=-λcone▽Urep,att
计算得到所述改进的斥力场对无人机产生的斥力;
式中:Frep,cone为所述改进的斥力场对无人机产生的斥力;▽为梯度符号;Urep,att为根据无人机与目标点之间的位置关系,基于传统人工势场法,得到的改进的斥力场:
式中:Urep,att为改进的斥力场,η为障碍物斥力因子,p为无人机位置,pob为障碍物位置,pgoal为目标点位置,ρ0为障碍物对无人机运动产生影响的最大距离,ρ(p,pob)表示障碍物到无人机的距离,ρ(p,pob)=||pob-p||,ρ(p,pgoal)表示目标点到无人机的距离;
λcone为根据所述碰撞锥检测条件得到的碰撞锥判断系数;
按照公式:
确定碰撞锥判断系数λcone;
其中,vr为无人机与障碍物的相对速度矢量,由vr=v-vob计算得到,v为无人机速度矢量,vob为所述障碍物的速度矢量,Lob为所述无人机到所述障碍物的距离矢量,由Lob=pob-p计算得到,dsafe为所述无人机和所述障碍物需要保持的最小安全距离,dsafe=rob+r,其中rob为所述障碍物半径,r为所述无人机的最小安全半径。
4.根据权利要求1所述的方法,其特征在于,在步骤S2中,按照公式:
计算得到所述速度势场;
其中,Urep,move表示速度势场,ηv为速度势场系数,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,df为当无人机速度矢量v和所在障碍物速度矢量vob不平行时二者所在直线的最短距离矢量,dl为当无人机速度矢量v和所在障碍物速度矢量vob平行时二者所在直线的最短距离矢量,nv为无人机速度矢量的单位方向矢量,nvob为障碍物速度矢量的单位方向矢量,ε是常量,取值范围(0,1),dsafe为无人机和障碍物需要保持的最小安全距离。
5.根据权利要求4所述的方法,其特征在于,在步骤S2中,根据所述速度势场,按照公式:
计算得到速度势场力;
式中:Frep,move为速度势场力,▽为梯度符号,Urep,move为速度势场,ηv为速度势场系数,dc为无人机速度矢量v和障碍物速度矢量vob所在直线的最短距离矢量,ε是常量,取值范围(0,1),dsafe为无人机和障碍物需要保持的最小安全距离。
6.根据权利要求1所述的方法,其特征在于,在步骤S2中,按照公式:
计算得到所述引力场;
式中:Uatt为引力场,ξ为目标点引力因子,pob为所述障碍物的位置,p为所述无人机的位置,ρ(p,pgoal)为无人机到目标点的距离,ρ(p,pgoal)=||pgoal-p||。
7.根据权利要求1所述的方法,其特征在于,在步骤S2中,根据所述引力场,按照公式:
Fatt=-▽Uatt=ξ(pgoal-p)
计算得引力场对无人机产生的引力;
式中:Fatt为引力场对无人机产生的引力;Uatt为引力场;ξ为目标点引力因子;pob为所述障碍物的位置,p为所述无人机的位置,(pgoal-p)为无人机到目标点的距离,(pgoal-p)=||pgoal-p||。
8.根据权利要求1所述的方法,其特征在于,在步骤S3中,所述无人机第i步所受合力为:引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力、速度势场对无人机产生的速度势场力之和,计算公式如下:
Ftotal,move(i)=Fatt(i)+Frep,cone(i)+Frep,move(i)
其中,Ftotal,move(i)为无人机第i步所受到的合力;Fatt(i)为无人机第i步引力场对无人机产生的引力;Frep,cone(i)为无人机第i步受到的所述改进的斥力场对无人机产生的斥力;Frep,move(i)为无人机第i步速度势场对无人机产生的速度势场力。
9.根据权利要求1所述的方法,其特征在于,在步骤S4中,所述根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物,包括:
步骤S41、根据无人机第i步所受合力,计算得到无人机第i步方向单位角;
步骤S42、根据所述无人机第i步的方向单位角,根据动态飞行步长,计算无人机下一步的期望位置;其中,所述动态飞行步长表征根据防抖动调节系数对无人机的飞行步长进行动态规划的步长;
按照公式:
pc(i+1)=p(i)+λs·l·(2·(1-λs)nftotal,cone(i-1)+λs·nftotal,cone(i))
计算得到无人机下一步的期望位置;
其中,pc(i+1)无人机下一步的期望位置,p(i)为无人机第i步所在位置,nftotal,cone(i-1)为无人机第i-1步方向单位角,nftotal,cone(i)为无人机第i步方向单位角,l为无人机飞行步长,l=Vxun·Δt,Vxun无人机巡航速度大小,Δt为仿真时间步长,λs为防抖动调节系数:
其中,ζs∈(0,1)为预设的调节系数,Δθi为第i-1步与第i步合力方向变化值,nftotal,cone(i-1)·nftotal,cone(i)=cosΔθi;
θmax是无人机最大转弯角度,当θmax<Δθi<π时表明无人机的飞行姿态出现抖动;
步骤S43、根据所述无人机第i步所在位置和无人机下一步的期望位置pc(i+1),计算无人机下一步运动的速度矢量;
按照公式:
vc=Δp/Δt=(pc(i+1)-p(i))/Δt
计算得到无人机下一步运动的速度矢量;
其中,vc为无人机下一步运动的速度矢量,Δp为无人机下一步期望位置与无人机第i步所在位置之差,Δp=pc(i+1)-p(i),Δt为仿真时间步长;
步骤S44、根据所述无人机下一步运动的速度矢量,计算所述俯仰角和所述偏航角;
根据所述无人机下一步运动的速度矢量,按照公式:
计算得到所述偏航角;
其中,γc表示所述偏航角,Vc表示表示无人机下一步运动的速度矢量的模,Vc=||vc||;
按照公式:
计算得到所述俯仰角;
其中,χc为所述俯仰角,x,y,z为惯性坐标系中的x,y,z轴,Vcx表示Vc在x轴方向上的分量,Vcy表示Vc在y轴方向上的分量;
步骤S45、根据所述无人机下一步运动的速度矢量、所述俯仰角和所述偏航角,控制无人机避开障碍物。
10.一种基于速度势场的无人机避障系统,其特征在于,包括:
碰撞检测单元,配置为:根据无人机的位置和速度、障碍物的位置和速度,检测无人机和障碍物的碰撞可能性;
受力计算单元,配置为:若无人机与障碍物存在碰撞可能性,计算引力场对无人机产生的引力、改进的斥力场对无人机产生的斥力以及速度势场对无人机产生的速度势场力,其中,所述引力场是目标点对无人机产生引力的势场,方向从无人机位置指向目标点,使无人机向目标点运动,所述改进的斥力场是根据无人机与目标点之间的位置关系,基于传统人工势场法进行改进,得到的障碍物对无人机产生斥力的势场,控制无人机规避障碍物,速度势场为与无人机速度矢量的方向、障碍物速度矢量的方向均垂直的势场,对无人机产生速度势场力,控制无人机规避运动障碍物;
合力计算单元,配置为:根据所述引力、改进的斥力场对无人机产生的斥力和速度势场力,计算无人机第i步所受合力,其中,i取值为正整数;
避障控制单元,配置为:根据无人机第i步所受合力,计算得到无人机第i步的方向单位角,并根据方向单位角计算无人机下一步运动的速度矢量、俯仰角和偏航角,控制无人机避开障碍物;
其中,根据无人机第i步所受合力,按照公式:
nftotal,cone(i)=Ftotal,move(i)/‖Ftotal,move(i)‖
计算得到无人机第i步方向单位角;其中,nftotal,cone(i)为所述无人机第i步的方向单位角,Ftotal,move(i)为无人机第i步所受到的合力。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110963231.2A CN113655810B (zh) | 2021-08-20 | 2021-08-20 | 一种基于速度势场的无人机避障方法和系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110963231.2A CN113655810B (zh) | 2021-08-20 | 2021-08-20 | 一种基于速度势场的无人机避障方法和系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113655810A CN113655810A (zh) | 2021-11-16 |
CN113655810B true CN113655810B (zh) | 2024-04-16 |
Family
ID=78491887
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110963231.2A Active CN113655810B (zh) | 2021-08-20 | 2021-08-20 | 一种基于速度势场的无人机避障方法和系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113655810B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114527783A (zh) * | 2021-12-17 | 2022-05-24 | 浙江华飞智能科技有限公司 | 无人机的控制方法、装置、设备及计算机存储介质 |
CN114519472B (zh) * | 2022-04-20 | 2022-07-22 | 南京信息工程大学 | 一种基于三维模型的应急管理监测方法 |
CN115167528B (zh) * | 2022-09-05 | 2022-12-02 | 北京航空航天大学 | 一种基于人工势场法的空间协同制导方法及装置 |
CN116400739B (zh) * | 2023-06-06 | 2023-09-08 | 成都时代星光科技有限公司 | 基于外力策略的智能无人机移动引导方法、系统和介质 |
CN117631689B (zh) * | 2024-01-25 | 2024-04-16 | 成都航空职业技术学院 | 一种无人机避障飞行方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20130071580A (ko) * | 2011-12-21 | 2013-07-01 | 한국해양과학기술원 | 다중선박 충돌위험도 식별 지원 시스템 |
CN105717942A (zh) * | 2016-01-31 | 2016-06-29 | 中国人民解放军海军航空工程学院 | 一种无人飞行器空间避障方法及相关路径在线规划方法 |
CN109521794A (zh) * | 2018-12-07 | 2019-03-26 | 南京航空航天大学 | 一种多无人机航路规划及动态避障方法 |
CN110850873A (zh) * | 2019-10-31 | 2020-02-28 | 五邑大学 | 无人船路径规划方法、装置、设备和存储介质 |
CN110879615A (zh) * | 2019-12-25 | 2020-03-13 | 上海市南电力(集团)有限公司 | 基于多旋翼无人机自动飞行的牵引放线系统及方法 |
CN111123984A (zh) * | 2019-11-05 | 2020-05-08 | 东莞理工学院 | 一种基于位置与速度人工势场法的无人机路径规划方法 |
CN111506068A (zh) * | 2020-04-20 | 2020-08-07 | 哈尔滨工程大学 | 一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法 |
CN111781948A (zh) * | 2020-06-18 | 2020-10-16 | 南京非空航空科技有限公司 | 一种无人机编队队形变换与在线动态避障方法 |
CN112965471A (zh) * | 2021-02-10 | 2021-06-15 | 大连理工大学 | 一种考虑角速度约束和改进斥力场的人工势场路径规划方法 |
-
2021
- 2021-08-20 CN CN202110963231.2A patent/CN113655810B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20130071580A (ko) * | 2011-12-21 | 2013-07-01 | 한국해양과학기술원 | 다중선박 충돌위험도 식별 지원 시스템 |
CN105717942A (zh) * | 2016-01-31 | 2016-06-29 | 中国人民解放军海军航空工程学院 | 一种无人飞行器空间避障方法及相关路径在线规划方法 |
CN109521794A (zh) * | 2018-12-07 | 2019-03-26 | 南京航空航天大学 | 一种多无人机航路规划及动态避障方法 |
CN110850873A (zh) * | 2019-10-31 | 2020-02-28 | 五邑大学 | 无人船路径规划方法、装置、设备和存储介质 |
CN111123984A (zh) * | 2019-11-05 | 2020-05-08 | 东莞理工学院 | 一种基于位置与速度人工势场法的无人机路径规划方法 |
CN110879615A (zh) * | 2019-12-25 | 2020-03-13 | 上海市南电力(集团)有限公司 | 基于多旋翼无人机自动飞行的牵引放线系统及方法 |
CN111506068A (zh) * | 2020-04-20 | 2020-08-07 | 哈尔滨工程大学 | 一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法 |
CN111781948A (zh) * | 2020-06-18 | 2020-10-16 | 南京非空航空科技有限公司 | 一种无人机编队队形变换与在线动态避障方法 |
CN112965471A (zh) * | 2021-02-10 | 2021-06-15 | 大连理工大学 | 一种考虑角速度约束和改进斥力场的人工势场路径规划方法 |
Non-Patent Citations (5)
Title |
---|
Yao, Min and Zhao, Min.Unmanned aerial vehicle dynamic path planning in an uncertain environment.2015,第33卷(第3期),611-621. * |
四旋翼运输编队的环形耦合同步控制器设计;范琦涵,胡士强;《电光与控制》;28(6);57-63 * |
基于改进人工势场法的机器人避障及路径规划研究;徐飞;《计算机科学》;第43卷(第12期);293-296 * |
基于改进人工势场的无人机预设航线避障研究;马娅婕, 刘国庆, 胡轶等;《高技术通讯》;第30卷(第1期);64-70 * |
针对非合作型动态障碍的无人机自主防碰撞;韩统, 周欢, 李霞等;《电光与控制》;第24卷(第2期);6-12 * |
Also Published As
Publication number | Publication date |
---|---|
CN113655810A (zh) | 2021-11-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113655810B (zh) | 一种基于速度势场的无人机避障方法和系统 | |
Breivik et al. | Guidance laws for autonomous underwater vehicles | |
Breivik et al. | Guidance laws for planar motion control | |
Zhou et al. | Vector field following for quadrotors using differential flatness | |
Frew et al. | Cooperative stand-off tracking of moving targets by a team of autonomous aircraft | |
CN111290429A (zh) | 一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法 | |
CN113126644B (zh) | 基于自适应视线法的无人机三维航迹跟踪方法 | |
CN116047909B (zh) | 面向海事平行搜寻的无人机-船协同鲁棒自适应控制方法 | |
Xu et al. | Vision-based autonomous landing of unmanned aerial vehicle on a motional unmanned surface vessel | |
Mills et al. | Vision based control for fixed wing UAVs inspecting locally linear infrastructure using skid-to-turn maneuvers | |
CN113759936A (zh) | 一种适用于动态目标跟踪的比例导引法与人工势场法相结合的路径规划方法 | |
Panyakeow et al. | Deconfliction algorithms for a pair of constant speed unmanned aerial vehicles | |
KR20120129002A (ko) | 수중로봇과 그 제어방법 | |
Serres et al. | Biomimetic autopilot based on minimalistic motion vision for navigating along corridors comprising U-shaped and S-shaped turns | |
Karras et al. | Target tracking with multi-rotor aerial vehicles based on a robust visual servo controller with prescribed performance | |
Rezende et al. | Autonomous system for a racing quadcopter | |
CN114564015B (zh) | 一种拒止环境下的欠驱动无人艇分布式编队控制方法 | |
Li et al. | Output feedback image-based visual servoing of rotorcrafts | |
CN113031643B (zh) | 一种无人机编队与避障方法 | |
WO2021238743A1 (zh) | 一种无人机飞行控制方法、装置及无人机 | |
Roussos et al. | Decentralised navigation and collision avoidance for aircraft in 3D space | |
Wang et al. | Shortest path planning of UAV for target tracking and obstacle avoidance in 3D environment | |
CN112947570A (zh) | 一种无人机避障的方法、装置和存储介质 | |
Guo et al. | Disturbance perception based quadrotor UAV maneuvering formation against unknown external disturbance | |
Zhang et al. | Nonlinear cooperative control of double drone-bar transportation system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |