CN113566826B - 一种基于二次规划的无人机路径规划方法 - Google Patents
一种基于二次规划的无人机路径规划方法 Download PDFInfo
- Publication number
- CN113566826B CN113566826B CN202110775762.9A CN202110775762A CN113566826B CN 113566826 B CN113566826 B CN 113566826B CN 202110775762 A CN202110775762 A CN 202110775762A CN 113566826 B CN113566826 B CN 113566826B
- Authority
- CN
- China
- Prior art keywords
- constraint
- model
- aerial vehicle
- rank
- unmanned aerial
- 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 52
- 238000013178 mathematical model Methods 0.000 claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 36
- 238000006467 substitution reaction Methods 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 229920006395 saturated elastomer Polymers 0.000 claims description 4
- 238000013329 compounding Methods 0.000 claims description 3
- 230000002040 relaxant effect Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Business, Economics & Management (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Human Resources & Organizations (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Economics (AREA)
- Strategic Management (AREA)
- Quality & Reliability (AREA)
- Tourism & Hospitality (AREA)
- Operations Research (AREA)
- General Business, Economics & Management (AREA)
- Marketing (AREA)
- Theoretical Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Entrepreneurship & Innovation (AREA)
- Game Theory and Decision Science (AREA)
- Development Economics (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明涉及一种基于二次规划的无人机路径规划方法,给出包括无人机运动模型,禁飞区约束,饱和约束,初末状态约束以及时间最优性能函数的无人机路径规划数学模型;对模型进行转化和离散处理,得到具有非凸二次约束的二次规划模型;将二次规划模型转化为齐次形式,并转化为带有秩约束的半定松弛模型;采用罚函数方法将秩约束转化到代价函数中,得到无秩约束的半定松弛模型;给出逐次迭代算法,得到无人机飞行的最优路径。本发明不需要提供初始数据,且所得到的路径为最优飞行路径;同时,本发明便于进行约束的扩展和扩充。
Description
技术领域
本发明涉及无人机路径规划技术领域,尤其涉及一种基于二次规划的无人机路径规划方法。
背景技术
无人机以其成本低,机动性强,安全成本低等优点成为了近年来的研究热点,目前在防灾救灾、医疗服务、农业、商业等领域都得到了广泛的应用。而路径规划问题是无人机任务中值得注意的重要问题,路径规划是指使无人机从初始位置飞行至目标位置。在路径规划任务中,常常存在多种约束,例如在军事活动中需要避免雷达的探测或在城市中飞行时存在需要避开的建筑物,这导致了无人机的禁飞区约束;无人机飞行的航向角变化率可能受到执行机构的限制从而存在着最大值的限制,这导致了饱和约束。
目前常见的无人机路径规划算法包括Dijkstra算法、A*算法、蚁群算法等方法,但是这些方法无法同时考虑无人机飞行过程中所存在的多种约束,所得到的路径是可行路径而不是最优路径。本发明不仅能够考虑多种约束,且所得到路径为时间最优路径。
发明内容
本发明技术解决问题:克服现有技术的不足,提供一种基于二次规划的无人机路径规划方法,考虑了运动学约束,禁飞区约束,饱和约束和初末状态约束;且性能函数为时间最优,得到的结果为最优路径;且便于增加约束数目和种类。
本发明技术解决方案:一种基于二次规划的无人机路径规划方法,包括如下步骤:
S1:将二维无人机运动模型,无人机飞行中所存在的禁飞区约束数学模型、饱和约束数学模型、初末状态约束的数学模型及时间最优的性能函数复合,得到无人机路径规划的数学模型;
S2:基于步骤S1中得到的模型,对其进行转化和离散化处理,得到具有非凸二次约束的二次规划模型;
S3:基于步骤S2中的二次规划模型,将其转化为齐次形式,采用半定松弛的方法转化为带有秩约束的半定规划模型;
S4:采用罚函数方法对步骤S3中存在的秩约束进行松弛,得到可以用半定规划方法求解的半定规划模型;
S5:采用逐次迭代的求解策略求解步骤S4中得到的半定规划模型,得到无人机的最优飞行路径。
所述步骤S1具体实现为:
(1)二维无人机运动模型为:
其中,x表示无人机的横坐标,y表示无人机的纵坐标,η表示无人机的航向角,V表示无人机的飞行速度,u表示控制输入,且u的方向与无人机的速度方向垂直;
(2)禁飞区约束的数学模型为:
无人机的禁飞区为圆形或者椭圆形,其数学模型为二次形式,且其为非凸的:
其中(xe,ye)为椭圆或圆的圆心,若禁飞区为圆,则ae=be为圆的半径;若禁飞区为椭圆,则ae,be分别为椭圆的半长轴和半短轴。
(3)饱和约束即控制输入存在饱和,其数学模型为:
|u|≤umax (3)
其中umax为航向角最大变化率。
(4)无人机的初末位置约束可以描述为:
其中,t0表示初始时刻,tf表示到达终点的时刻,(x0,y0)表示无人机的初始位置,(xf,yf)表示无人机的期望位置。
(5)性能函数为基于时间最优的性能函数,其数学描述为:
公式(5)表示对时间进行积分,1代表数字1,数字1无物理意义;
无人机路径规划数学模型为以上五个模型之和。
所述步骤S2中,
(1)对于步骤S1中的模型进行转化处理:
由于存在航向角的三角函数,故采用变量代换的方法对无人机运动模型进行转化:
其中u1=cosη,u2=sinη,并且引入了新的等式约束:
饱和约束(4)转化为:
-u3-u1umax≤0;u3-u1umax≤0 (8)
(2)对转化后的模型进行离散化:
性能函数为:
J1=minN·Δt (9)
约束为:
xi+1-xi=Vu1iΔt;yi+1-yi=Vu2iΔt;u2i-u2(i+1)=u3iΔt (10)
x1=x0,xN+1=xf,y1=y0,yN+1=yf (14)
其中N+1表示所设置的离散时间节点,表示离散时间步长;J1代表离散后的性能函数;公式中带有下标的变量表示的是第i个离散时间节点时该变量的值,变量的物理意义与上文相同。
该模型为具有非凸二次约束的二次规划模型。
所述步骤S3中,
(1)二次约束二次规划的一般模型:
经过步骤S2,无人机路径规划模型已经转化为二次约束二次规划模型,二次约束二次规划的一般模型为:
性能函数:
约束:
其中J0代表一般形式的性能函数,x∈Rm为列向量,Q0,Qi,Qj∈Rm×m为对称矩阵,b0,bi,bj∈Rm为列向量,ci,cj∈R为常数。若性能函数或约束为线性则对称矩阵Ql,l=0,i,j为零矩阵,若向量bl,l=0,i,j为零向量,则性能函数或约束为齐次方程。
(2)将二次约束二次规划模型转化为齐次形式:
性能函数:
约束:
α2=1 (21)
其中α为常数变量。
(3)采用半定松弛对齐次二次约束二次规划模型进行松弛,得到半定规划模型:
性能函数:
J0=mintr(Q’0X) (22)
约束:
tr(Q’iX)≤ci,i=1,...,p (23)
tr(Q'jX)=cj,j=1,...,q,q+1 (24)
其中tr(·)代表矩阵的迹,X∈Rn(n=m+1)为新引入的变量。其中约束(22)α2=1被包含在了等式约束(25)中,为第q+1个等式约束。
约束为非凸且非线性约束,其中α2=1,等价于:
X≥0 (26)
rank(X)=1 (27)
其中rank(·)代表矩阵的秩,(·)≥0代表矩阵为半正定矩阵。
对等价性进行证明,如果则显然X≥0且其秩为1;如果公式(26)和(27)被满足,那么意味着X=MMT,其中M∈Rn为列向量。由于α2=1,则X(n,n)=1,可以得到M=[M′,±1],其中M′∈Rn。综上可以得到x=±M′/α,即/>
通过上述转化,齐次的二次约束二次规划模型转化为了带有秩约束的半定规划模型。
所述步骤S4中,
由于秩约束(27)仍然为非凸约束,属于NP-困难问题,所以采用罚函数的方法对其进行转化:
性能函数:
J′0=mintr(Q’0X)+γ·rank(X) (28)
约束:
tr(Q’iX)≤ci,i=1,...,p (29)
tr(Q'jX)=cj,j=1,...,q,q+1 (30)
X≥0 (31)
其中J′0代表加入了秩惩罚后的性能函数,γ为惩罚系数,其范围为[100,+∞)。
对于半正定矩阵,其特征值大于零或等于0,其秩为其大于零的特征值的数目。引入函数ρ(z)=1-e-z/σ,其中σ为极小的常数。当z=0时,ρ(z)=0;当z>0时,ρ(z)=1。则矩阵X的秩可以近似地表示为其中λi,i=1...n为矩阵X的特征值。
但由于rank′(X)仍为非凸函数,需要对其进行线性化,给出其梯度为:
其中U∈Rn×n为矩阵X的特征值所对应的特征向量所组成的矩阵。
由于其非凸性,可以得到:
rank′(X)≤rank′(Xk)+tr(rank′(Xk)·(X-Xk))=r(X,Xk) (33)
其中Xk为第k次迭代的解。
故性能函数转化为:
J′=mintr(Q'0X)+γ·r(X,Xk) (34)
所述步骤S5中,给出逐次迭代策略:
首先给出两个模型:
模型1:J0=mintr(Q’0X)
tr(Q’iX)≤ci,i=1,...p,tr(Q'jX)=cj,j=1,...,q,q+1,X≥0
模型2:J′=mintr(Q'0X)+γ·r(X,Xk)
tr(Q’iX)≤ci,i=1,...p,tr(Q'jX)=cj,j=1,...,q,q+1,X≥0
再设置初始的惩罚系数和替代系数γ=γ0,σ=σ0,求解不考虑秩约束且性能函数为J0=mintr(Q’0X)的模型1得到无人机的第一条飞行路径;接着求解性能函数为J′=mintr(Q'0X)+γ·r(X,Xk)的模型2,判断两次迭代得到的解的差的F范数,直到其小于所设置的值ζ1,ζ1为常数且取值范围为[0.001,0.01],若不小于所设置的值则再次求解模型2,进行循环;设置σ=σ/κ1,其中κ1为常数且取值范围为[4,8],再次进行上述循环,比较前后两个σ时得到的解的差的F范数,直到其小于所设置的值ζ2,ζ2为常数且取值范围为[0.001,0.01];完成上述两组循环之后,判断所得到解的秩,当其大于1时,增大惩罚项的值使得γ=κ2·γ,其中κ2为常数且取值范围为[2,5],再次进行循环,直到所得到的解的秩为1,即得到无人机的最优飞行路径。
本发明与现有技术相比的有益效果在于:
(1)现有方法,例如势函数,快速随机搜索树,A*等方法无法同时考虑本文中所考虑的约束,并且只能得到无人机飞行的可行路径而不是最优路径。本发明考虑了运动学约束,禁飞区约束,饱和约束和初末状态约束;且性能函数为时间最优,得不需要提供初始路径即可开始迭代,且可以保证所得到的路径为飞行时间最优。
(2)现有方法所能考虑的约束种类和数目都受到限制,本发明方法,便于增加约束的数目和种类。
附图说明
图1为本发明实施例的基于二次规划的无人机路径规划方法流程图;
图2为本发明实施例的无人机路径规划示意图;
图3为本发明是实施的无人机路径规划方法的无人机二维路径示意图。
具体实施方式
下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。
如图1所示,本发明实施例的基于二次规划的无人机路径规划方法,包括如下步骤:
S1:将二维无人机运动模型,无人机飞行中所存在的禁飞区约束数学模型、饱和约束数学模型、初末状态约束的数学模型及时间最优的性能函数复合,得到无人机路径规划的数学模型。具体过程如下:
(1)二维无人机运动模型为:
其中,x表示无人机的横坐标,y表示无人机的纵坐标,η表示无人机的航向角,V表示无人机的飞行速度,u表示控制输入,且u的方向与无人机的速度方向垂直;
(2)禁飞区约束可以描述为:
无人机的禁飞区为圆形或者椭圆形,其数学模型为二次形式,且其为非凸的:
其中(xe,ye)为椭圆或圆的圆心,若禁飞区为圆,则ae=be为圆的半径;若禁飞区为椭圆,则ae,be分别为椭圆的半长轴和半短轴。
(3)饱和约束即控制输入存在饱和,其数学模型为:
|u|≤umax (3)
其中umax为航向角最大变化率。
(4)无人机的初末位置约束可以描述为:
其中,t0表示初始时刻,tf表示到达终点的时刻,(x0,y0)表示无人机的初始位置,(xf,yf)表示无人机的期望位置。
(5)性能函数为基于时间最优的性能函数,其数学描述为:
公式(5)表示对时间进行积分,1代表数字1,数字1无物理意义;
无人机路径规划数学模型为以上五个模型之和。
S2:将步骤S1中得到的无人机路径规划的数学模型进行转化和离散化处理,得到具有非凸二次约束的二次规划模型。具体过程如下:
(1)对于步骤S1中的模型进行转化处理:
由于存在航向角的三角函数,且三角函数为强非凸非线性的,故采用变量代换的方法对无人机运动模型进行转化:
其中u1=cosη,u2=sinη,且由于变量代换,引入了新的等式约束:
根据运动学方程(1),可以得到
故饱和约束(4)转化为如下形式:
-u3-u1umax≤0;u3-u1umax≤0 (8)
(2)对转化后的模型进行离散化:
性能函数为:
J1=minN·Δt (9)
约束为:
xi+1-xi=Vu1iΔt;yi+1-yi=Vu2iΔt;u2i-u2(i+1)=u3iΔt (10)
x1=x0,xN+1=xf,y1=y0,yN+1=yf (14)
其中N+1表示所设置的离散时间节点,表示离散时间步长;J1代表离散后的性能函数;公式中带有下标的变量表示的是第i个离散时间节点时该变量的值,变量的物理意义与上文相同。
该模型为具有非凸二次约束的二次规划模型。
S3:基于步骤S2中的二次规划模型,将其转化为齐次形式,并采用半定松弛的方法转化为带有秩约束的半定规划模型。具体过程如下:
(1)二次约束二次规划的一般模型:
经过步骤S2,无人机路径规划模型已经转化为二次约束二次规划形式,设二次约束二次规划的一般形式为:
性能函数:
约束:
其中J0代表一般形式的性能函数,x∈Rm为列向量,Q0,Qi,Qj∈Rm×m为对称矩阵,b0,bi,bj∈Rm为列向量,ci,cj∈R为常数。若性能函数或约束为线性则对称矩阵Ql,l=0,i,j为零矩阵,若向量bl,l=0,i,j为零向量,则性能函数或约束为齐次方程。
(2)引入常数变量α∈R,将二次约束二次规划模型转化为齐次形式:
性能函数:
约束:
α2=1 (21)
其中α为常数变量。
(3)采用半定松弛对齐次二次约束二次规划模型进行松弛,得到半定规划模型:
性能函数:
J0=mintr(Q’0X) (22)
约束:
tr(Q’iX)≤ci,i=1,...,p (23)
tr(Q'jX)=cj,j=1,...,q,q+1 (24)
其中tr(·)代表矩阵的迹,X∈Rn(n=m+1)为新引入的变量。其中约束(21)α2=1被包含在了等式约束(25)中,为第q+1个等式约束。
约束为非凸且非线性约束,其等价于:
X≥0 (26)
rank(X)=1 (27)
其中rank(·)代表矩阵的秩,(·)≥0代表矩阵为半正定矩阵。
下面对等价性进行证明,因为则显然有X≥0且其秩为1;如果公式(26)和(27)被满足,那么意味着X=MMT,其中M∈Rn。考虑到约束(25)中包括了X(n,n)=1,可以得到M=[M′,±1],其中M′∈Rn。综上可以得到x=±M′/α,进一步可以得到,
通过上述转化,齐次的二次约束二次规划模型转化为了带有秩约束的半定规划模型。
S4:采用罚函数方法对步骤S3中存在的秩约束进行松弛,得到可以用半定规划方法求解的半定规划模型。具体实现如下:
由于秩约束(27)仍然为非凸约束,采用罚函数的方法对其进行转化:
性能函数:
J′0=mintr(Q’0X)+γ·rank(X) (28)
约束:
tr(Q’iX)≤ci,i=1,...,p (29)
tr(Q'jX)=cj,j=1,...,q,q+1 (30)
X≥0 (31)
其中J′0代表加入了秩惩罚后的性能函数,γ为惩罚系数,其范围为[100,+∞)。
对于半正定矩阵,其特征值大于零或等于0,其秩为其大于零的特征值的数目。为了使得带有秩约束的问题能够求解,秩约束需要被写成更为清晰可解的形式,可以采用某些函数对秩约束进行近似。
引入函数ρ(z)=1-e-z/σ,其中σ为极小的常数。当z=0时,ρ(z)=0;当z>0时,ρ(z)=1。则矩阵X的秩可以近似地表示为其中λi,i=1...n为矩阵X的特征值。
但由于rank′(X)仍为非凸函数,需要对其进行线性化,给出其梯度为:
其中U∈Rn×n为矩阵X的特征值所对应的特征向量所组成的矩阵。
由于其非凸性,可以得到:
rank′(X)≤rank′(Xk)+tr(rank′(Xk)·(X-Xk))=r(X,Xk) (33)
其中Xk为第k次迭代的解。故可以采用r(X,Xk)来逼近rank′(X)。
则性能函数转化为下面的形式:
J′=mintr(Q'0X)+γ·r(X,Xk) (34)
S5:采用逐次迭代的求解策略求解步骤S4中得到的半定规划模型,得到无人机的最优飞行路径。具体实现如下:
首先给出两个模型:
模型1:J0=mintr(Q’0X)
tr(Q’iX)≤ci,i=1,...p,tr(Q'jX)=cj,j=1,...,q,q+1,X≥0
模型2:J′=mintr(Q'0X)+γ·r(X,Xk)
tr(Q’iX)≤ci,i=1,...p,tr(Q'jX)=cj,j=1,...,q,q+1,X≥0
再设置初始的惩罚系数和替代系数γ=γ0,σ=σ0,求解不考虑秩约束且性能函数为J0=mintr(Q’0X)的模型1得到无人机的第一条飞行路径;接着求解性能函数为J′=mintr(Q'0X)+γ·r(X,Xk)的模型2,判断两次迭代得到的解的差的F范数,直到其小于所设置的值ζ1,ζ1为常数且取值范围为[0.001,0.01],若不小于所设置的值则再次求解模型2,进行循环;设置σ=σ/κ1使得替代系数减小,其中κ1为常数且取值范围为[4,8],再次进行上述循环,比较前后两个σ时得到的解的差的F范数,直到其小于所设置的值ζ2,ζ2为常数且取值范围为[0.001,0.01];完成上述两组循环之后,判断所得到解的秩,当其大于1时,增大罚函数的值使得γ=κ2·γ,其中κ2为常数且取值范围为[2,5],再次进行循环,直到所得到的解的秩为1,即得到无人机的最优飞行路径。
本发明中所有变量上标“·”都是该变量的导数,除非该变量的导数有实际物理含义。
下面以考虑了五个禁飞区约束的某无人机路径规划场景为例,说明本发明所提出的方法的有效性。定义无人机的速度为100m/s,无人机初始位置为(0,0)m,目标位置为(1200,1500)m。五个禁飞区圆心及长短轴分别为(650,800,150,180)m,(100,200,150,150)m,(500,200,250,150)m,(900,1200,200,150)m,(1000,600,190,190)m。饱和约束的最大值为0.5。规划得到的无人机路径如图3所示,可以看出无人机在成功抵达终点的同时避开了五个禁飞区。最小飞行时间即性能函数的数值为19.6909s。
综上,本发明所提出的基于二次规划的无人机路径规划方法可以在满足约束的情况下得到无人机的时间最优路径,且该方法便于增加约束的数目和种类。
Claims (6)
1.一种基于二次规划的无人机路径规划方法,其特征在于,包括如下步骤:
S1:将二维无人机运动模型,无人机飞行中所存在的禁飞区约束数学模型、饱和约束数学模型、初末状态约束的数学模型及时间最优的性能函数复合,得到无人机路径规划的数学模型;
S2:将步骤S1中得到的无人机路径规划的数学模型进行转化和离散化处理,得到具有非凸二次约束的二次规划模型;
S3:将步骤S2中得到的具有非凸二次约束的二次规划模型转化为齐次形式,采用半定松弛的方法转化为带有秩约束的半定规划模型;
S4:采用罚函数方法对步骤S3中存在的秩约束进行松弛,得到用半定规划方法求解的半定规划模型;
S5:采用逐次迭代的求解策略求解步骤S4中得到的半定规划模型,得到无人机的最优飞行路径。
2.根据权利要求1所述的方法,其特征在于:所述步骤S1具体实现为:
(1)二维无人机运动模型为:
其中,x表示无人机的横坐标,y表示无人机的纵坐标,η表示无人机的航向角,V表示无人机的飞行速度,u表示控制输入,且u的方向与无人机的速度方向垂直;
(2)禁飞区约束的数学模型为:
无人机的禁飞区为圆形或者椭圆形,其数学模型为非凸的二次模型:
其中(xe,ye)为椭圆或圆的圆心,若禁飞区为圆,则ae=be为圆的半径;若禁飞区为椭圆,则ae,be分别为椭圆的半长轴和半短轴;
(3)饱和约束即控制输入存在饱和,其数学模型为:
|u|≤umax (3)
其中umax为航向角最大变化率;
(4)无人机的初末状态约束为:
其中,t0表示初始时刻,tf表示到达终点的时刻,(x0,y0)表示无人机的初始位置,(xf,yf)表示无人机的期望位置;
(5)时间最优的性能函数J的数学描述为:
公式(5)表示对时间进行积分,1代表数字1,数字1无物理意义;
无人机路径规划的数学模型为以上五个模型之和。
3.根据权利要求2所述的方法,其特征在于:所述步骤S2中,
(1)对于步骤S1中的模型进行转化处理:
采用变量代换的方法对无人机运动模型进行转化:
其中u1,u2,u3为新引入的变量,u1=cosη,u2=sinη,并且引入了新的等式约束:
饱和约束(4)转化为:
-u3-u1umax≤0;u3-u1umax≤0 (8)(2)对转化后的模型进行离散化:
性能函数为:
J1=minN·Δt (9)
约束为:
xi+1-xi=Vu1iΔt;yi+1-yi=Vu2iΔt;u2i-u2(i+1)=u3iΔt (10)
x1=x0,xN+1=xf,y1=y0,yN+1=yf (14)
其中N+1表示所设置的离散时间节点,表示离散时间步长;J1代表离散后的性能函数;公式中带有下标的变量表示的是第i个离散时间节点时该变量的值,变量的物理意义与上文相同;
该模型为具有非凸二次约束的二次规划模型。
4.根据权利要求3所述的方法,其特征在于:所述步骤S3中,
(1)二次约束二次规划的一般模型:
经过步骤S2,无人机路径规划模型已经转化为二次约束二次规划模型,二次约束二次规划的一般模型为:
性能函数:
约束:
其中J0代表一般形式的性能函数,x∈Rm为列向量,Q0,Qi,Qj∈Rm×m为对称矩阵,b0,bi,bj∈Rm为列向量,ci,cj∈R为常数,若性能函数或约束为线性则对称矩阵Ql,l=0,i,j为零矩阵,若向量bl,l=0,i,j为零向量,则性能函数或约束为齐次方程;p表示不等式约束的数目,q表示等式约束的数目;
(2)将二次约束二次规划模型转化为齐次形式:
性能函数:
约束:
α2=1 (21)
其中α为常数变量;
(3)采用半定松弛法对齐次二次约束二次规划模型进行松弛,得到半定规划模型:
给出性能函数:
J0=mintr(Q′0X) (22)
约束:
tr(Q′iX)≤ci,i=1,...,p (23)
tr(Q′jX)=cj,j=1,...,q,q+1 (24)
其中tr(·)代表矩阵的迹,X∈Rn,n=m+1为新引入的变量; 均为对称矩阵;
约束为非凸且非线性约束,其等价于:
X≥0 (26)
rank(X)=1 (27)
其中rank(·)代表矩阵的秩,(·)≥0代表矩阵为半正定矩阵;
通过上述转化,齐次的二次约束二次规划模型转化为了带有秩约束的半定规划模型。
5.根据权利要求4所述的方法,其特征在于,所述步骤S4中,
采用罚函数的方法对其进行转化:
性能函数:
J′0=mintr(Q′0X)+γ·rank(X) (28)
约束:
tr(Q′iX)≤ci,i=1,...,p (29)
tr(Q′jX)=cj,j=1,...,q,q+1 (30)
X≥0 (31)
其中J′0代表加入了秩惩罚后的性能函数,γ为惩罚系数,其范围为[100,+∞);
对于半正定矩阵,其特征值大于零或等于0,其秩为其大于零的特征值的数目;引入函数ρ(z)=1-e-z/σ,其中σ为替代系数,其取值范围为(0,0.5],z为变量,当z=0时,ρ(z)=0;当z>0时,ρ(z)=1,则矩阵X的秩近似地表示为其中λi,i=1...n为矩阵X的特征值;
rank′(X)仍为非凸函数,其梯度为:
其中U∈Rn×n为矩阵X的特征向量所组成的矩阵;
由于其非凸性,得到:
rank′(X)≤rank′(Xk)+tr(rank′(Xk)·(X-Xk))=r(X,Xk) (33)
其中Xk为第k次迭代的解,r(X,Xk)所采用的秩惩罚函数;
故性能函数转化为:
J′=mintr(Q′0X)+γ·r(X,Xk) (34)。
6.根据权利要求5所述的方法,其特征在于,所述步骤S5中具体实现如下:
首先给出两个模型:
模型1:J0=mintr(Q′0X)
tr(Q′iX)≤ci,i=1,...p,tr(Q′jX)=cj,j=1,...,q,q+1,X≥0
模型2:J′=mintr(Q′0X)+γ·r(X,Xk)
tr(Q′iX)≤ci,i=1,...p,tr(Q′jX)=cj,j=1,...,q,q+1,X≥0
再设置初始的惩罚系数和替代系数γ=γ0,σ=σ0,求解不考虑秩约束且性能函数为J0=mintr(Q′0X)的模型1得到无人机的第一条飞行路径;接着求解性能函数为J′=mintr(Q′0X)+γ·r(X,Xk)的模型2,判断两次迭代得到的解的差的F范数,直到其小于所设置的值ζ1,ζ1为常数且取值范围为[0.001,0.01],若不小于所设置的值则再次求解模型2,进行循环;设置σ=σ/κ1,其中κ1为常数且取值范围为[4,8],再次进行上述循环,比较前后两个σ时得到的解的差的F范数,直到其小于所设置的值ζ2,ζ2为常数且取值范围为[0.001,0.01];完成上述两组循环之后,判断所得到解的秩,当其大于1时,增大罚函数的值使得γ=κ2·γ,其中κ2为常数且取值范围为[2,5],再次进行循环,直到所得到的解的秩为1,即得到无人机的最优飞行路径。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110775762.9A CN113566826B (zh) | 2021-07-08 | 2021-07-08 | 一种基于二次规划的无人机路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110775762.9A CN113566826B (zh) | 2021-07-08 | 2021-07-08 | 一种基于二次规划的无人机路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113566826A CN113566826A (zh) | 2021-10-29 |
CN113566826B true CN113566826B (zh) | 2023-11-07 |
Family
ID=78164215
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110775762.9A Active CN113566826B (zh) | 2021-07-08 | 2021-07-08 | 一种基于二次规划的无人机路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113566826B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106017472A (zh) * | 2016-05-17 | 2016-10-12 | 成都通甲优博科技有限责任公司 | 全局路线规划方法、全局路线规划系统及无人机 |
CN108120442A (zh) * | 2017-12-12 | 2018-06-05 | 北京理工大学 | 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法 |
CN109357678A (zh) * | 2018-10-16 | 2019-02-19 | 北京航空航天大学 | 一种基于异质化鸽群优化算法的多无人机路径规划方法 |
WO2021001768A1 (fr) * | 2019-07-01 | 2021-01-07 | Uavia | Procédé de détermination de trajet d'un appareil aérien non habité et autres procédés associés |
CN112596549A (zh) * | 2020-12-29 | 2021-04-02 | 中山大学 | 基于连续凸规则的多无人机编队控制方法、装置及介质 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10816985B2 (en) * | 2018-04-17 | 2020-10-27 | Baidu Usa Llc | Method on moving obstacle representation for trajectory planning |
-
2021
- 2021-07-08 CN CN202110775762.9A patent/CN113566826B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106017472A (zh) * | 2016-05-17 | 2016-10-12 | 成都通甲优博科技有限责任公司 | 全局路线规划方法、全局路线规划系统及无人机 |
CN108120442A (zh) * | 2017-12-12 | 2018-06-05 | 北京理工大学 | 一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法 |
CN109357678A (zh) * | 2018-10-16 | 2019-02-19 | 北京航空航天大学 | 一种基于异质化鸽群优化算法的多无人机路径规划方法 |
WO2021001768A1 (fr) * | 2019-07-01 | 2021-01-07 | Uavia | Procédé de détermination de trajet d'un appareil aérien non habité et autres procédés associés |
CN112596549A (zh) * | 2020-12-29 | 2021-04-02 | 中山大学 | 基于连续凸规则的多无人机编队控制方法、装置及介质 |
Non-Patent Citations (4)
Title |
---|
Iterative Learning Optimization for UAV Path Planning with Avoidance Zones;You Sixiong et al.;《2019 AMERICAN CONTROL CONFERENCE (ACC)》;2759-2764 * |
基于改进A~*算法的无人机快速轨迹规划方法;刘永琦;徐丹;程归;陈刚;;飞行力学(第02期);43-49 * |
无人机航迹规划问题的非凸优化算法研究;张哲;《中国博士学位论文全文数据库 (基础科学辑)》;A002-75 * |
求解二次规划逆问题的信赖域共轭梯度法;陈曦;《中国优秀硕士学位论文全文数据库 (基础科学辑)》;A002-154 * |
Also Published As
Publication number | Publication date |
---|---|
CN113566826A (zh) | 2021-10-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US6244536B1 (en) | Air to air homing missile guidance | |
Kaminer et al. | Path following for small unmanned aerial vehicles using L1 adaptive augmentation of commercial autopilots | |
Palumbo et al. | Modern homing missile guidance theory and techniques | |
Abbasi et al. | Formation control of aerial robots using virtual structure and new fuzzy-based self-tuning synchronization | |
Shin et al. | Adaptive support vector regression for UAV flight control | |
Oktay et al. | Simultaneous small UAV and autopilot system design | |
Zhai et al. | Coverage-based interception algorithm of multiple interceptors against the target involving decoys | |
Luo et al. | On decoupling trajectory tracking control of unmanned powered parafoil using ADRC-based coupling analysis and dynamic feedforward compensation | |
Liang et al. | Vector field guidance for three-dimensional curved path following with fixed-wing UAVs | |
Wang et al. | Disturbance observer-based sliding mode backstepping control for a re-entry vehicle with input constraint and external disturbance | |
Lazim et al. | Disturbance observer-based formation tracking control of multiple quadrotors in the presence of disturbances | |
Shah et al. | Lateral track control of UAVs using the sliding mode approach: from design to flight testing | |
Kokolakis et al. | Coordinated standoff tracking of a ground moving target and the phase separation problem | |
Wael et al. | Robust hybrid control for ballistic missile longitudinal autopilot | |
Lin et al. | Estimating evasive acceleration for ballistic targets using an extended state observer | |
Bian et al. | A fitness sharing based ant clustering method for multimodal optimization of the aircraft longitudinal automatic carrier landing system | |
CN113566826B (zh) | 一种基于二次规划的无人机路径规划方法 | |
Wang et al. | Path following for unmanned combat aerial vehicles using three-dimensional nonlinear guidance | |
Arneberg et al. | Guidance laws for partially-observable interception based on linear covariance analysis | |
Mo et al. | Study on control method of a rotor UAV transportation with slung-load | |
Weiss et al. | Linear quadratic minimum-effort, specify-performance approach to missile guidance | |
Zhu et al. | An adaptive interactive multiple-model algorithm based on end-to-end learning | |
CN113625776B (zh) | 一种基于中值偏差的多无人机滑模编队控制方法 | |
Souanef | Adaptive guidance and control of small unmanned aerial vehicles | |
Xiong et al. | Tracking flight control of UAV based on multiple regions pole assignment method |
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 |