一种基于二次规划的无人机路径规划方法与流程

1.本发明涉及无人机路径规划技术领域，尤其涉及一种基于二次规划的无人机路径规划方法。


背景技术：

2.无人机以其成本低，机动性强，安全成本低等优点成为了近年来的研究热点，目前在防灾救灾、医疗服务、农业、商业等领域都得到了广泛的应用。而路径规划问题是无人机任务中值得注意的重要问题，路径规划是指使无人机从初始位置飞行至目标位置。在路径规划任务中，常常存在多种约束，例如在军事活动中需要避免雷达的探测或在城市中飞行时存在需要避开的建筑物，这导致了无人机的禁飞区约束；无人机飞行的航向角变化率可能受到执行机构的限制从而存在着最大值的限制，这导致了饱和约束。
3.目前常见的无人机路径规划算法包括dijkstra算法、a*算法、蚁群算法等方法，但是这些方法无法同时考虑无人机飞行过程中所存在的多种约束，所得到的路径是可行路径而不是最优路径。本发明不仅能够考虑多种约束，且所得到路径为时间最优路径。


技术实现要素：

4.本发明技术解决问题：克服现有技术的不足，提供一种基于二次规划的无人机路径规划方法，考虑了运动学约束，禁飞区约束，饱和约束和初末状态约束；且性能函数为时间最优，得到的结果为最优路径；且便于增加约束数目和种类。
5.本发明技术解决方案：一种基于二次规划的无人机路径规划方法，包括如下步骤：
6.s1：将二维无人机运动模型，无人机飞行中所存在的禁飞区约束数学模型、饱和约束数学模型、初末状态约束的数学模型及时间最优的性能函数复合，得到无人机路径规划的数学模型；
7.s2：基于步骤s1中得到的模型，对其进行转化和离散化处理，得到具有非凸二次约束的二次规划模型；
8.s3：基于步骤s2中的二次规划模型，将其转化为齐次形式，采用半定松弛的方法转化为带有秩约束的半定规划模型；
9.s4：采用罚函数方法对步骤s3中存在的秩约束进行松弛，得到可以用半定规划方法求解的半定规划模型；
10.s5：采用逐次迭代的求解策略求解步骤s4中得到的半定规划模型，得到无人机的最优飞行路径。
11.所述步骤s1具体实现为：
12.(1)二维无人机运动模型为：
13.14.其中，x表示无人机的横坐标，y表示无人机的纵坐标，η表示无人机的航向角，v表示无人机的飞行速度，u表示控制输入，且u的方向与无人机的速度方向垂直；
15.(2)禁飞区约束的数学模型为：
16.无人机的禁飞区为圆形或者椭圆形，其数学模型为二次形式，且其为非凸的：
[0017][0018]
其中(x
e
,y
e
)为椭圆或圆的圆心，若禁飞区为圆，则a
e
＝b
e
为圆的半径；若禁飞区为椭圆，则a
e
,b
e
分别为椭圆的半长轴和半短轴。
[0019]
(3)饱和约束即控制输入存在饱和，其数学模型为：
[0020]
|u|≤u
max
ꢀꢀꢀ
(3)
[0021]
其中u
max
为航向角最大变化率。
[0022]
(4)无人机的初末位置约束可以描述为：
[0023][0024]
其中，t0表示初始时刻，t
f
表示到达终点的时刻，(x0,y0)表示无人机的初始位置，(x
f
,y
f
)表示无人机的期望位置。
[0025]
(5)性能函数为基于时间最优的性能函数，其数学描述为：
[0026][0027]
公式(5)表示对时间进行积分，1代表数字1，数字1无物理意义；
[0028]
无人机路径规划数学模型为以上五个模型之和。
[0029]
所述步骤s2中，
[0030]
(1)对于步骤s1中的模型进行转化处理：
[0031]
由于存在航向角的三角函数，故采用变量代换的方法对无人机运动模型进行转化：
[0032][0033]
其中u1＝cosη,u2＝sinη，并且引入了新的等式约束：
[0034][0035]
饱和约束(4)转化为：
[0036]
‑
u3‑
u1u
max
≤0；u3‑
u1u
max
≤0
ꢀꢀꢀ
(8)
[0037]
(2)对转化后的模型进行离散化：
[0038]
性能函数为：
[0039]
j1＝minn
·
δt
ꢀꢀꢀ
(9)
[0040]
约束为：
[0041]
x
i 1
‑
x
i
＝vu
1i
δt；y
i 1
‑
y
i
＝vu
2i
δt；u
2i
‑
u
2(i 1)
＝u
3i
δt
ꢀꢀꢀ
(10)
[0042][0043][0044][0045]
x1＝x0,x
n 1
＝x
f
,y1＝y0,y
n 1
＝y
f
ꢀꢀꢀ
(14)
[0046]
其中n 1表示所设置的离散时间节点，表示离散时间步长；j1代表离散后的性能函数；公式中带有下标的变量表示的是第i个离散时间节点时该变量的值，变量的物理意义与上文相同。
[0047]
该模型为具有非凸二次约束的二次规划模型。
[0048]
所述步骤s3中，
[0049]
(1)二次约束二次规划的一般模型：
[0050]
经过步骤s2，无人机路径规划模型已经转化为二次约束二次规划模型，二次约束二次规划的一般模型为：
[0051]
性能函数：
[0052][0053]
约束：
[0054][0055][0056]
其中j0代表一般形式的性能函数，x∈r
m
为列向量,q0,q
i
,q
j
∈r
m
×
m
为对称矩阵，b0,b
i
,b
j
∈r
m
为列向量，c
i
,c
j
∈r为常数。若性能函数或约束为线性则对称矩阵q
l
,l＝0,i,j为零矩阵，若向量b
l
,l＝0,i,j为零向量，则性能函数或约束为齐次方程。
[0057]
(2)将二次约束二次规划模型转化为齐次形式：
[0058]
性能函数：
[0059][0060]
约束：
[0061][0062][0063]
α2＝1
ꢀꢀꢀ
(21)
[0064]
其中α为常数变量。
[0065]
(3)采用半定松弛对齐次二次约束二次规划模型进行松弛，得到半定规划模型：
[0066]
性能函数：
[0067]
j0＝mintr(q
’0x)
ꢀꢀꢀ
(22)
[0068]
约束：
[0069]
tr(q’i
x)≤c
i
,i＝1,...,p
ꢀꢀꢀ
(23)
[0070]
tr(q'
j
x)＝c
j
,j＝1,...,q,q 1
ꢀꢀꢀ
(24)
[0071][0072]
其中tr(
·
)代表矩阵的迹，x∈r
n
(n＝m 1)为新引入的变量。其中约束(22)α2＝1被包含在了等式约束(25)中，为第q 1个等式约束。
[0073]
约束为非凸且非线性约束，其中α2＝1，等价于：
[0074]
x≥0
ꢀꢀꢀ
(26)
[0075]
rank(x)＝1
ꢀꢀꢀ
(27)
[0076]
其中rank(
·
)代表矩阵的秩，(
·
)≥0代表矩阵为半正定矩阵。
[0077]
对等价性进行证明，如果则显然x≥0且其秩为1；如果公式(26)和(27)被满足，那么意味着x＝mm
t
，其中m∈r
n
为列向量。由于α2＝1，则x(n,n)＝1，可以得到m＝[m
′
,
±
1]，其中m
′
∈r
n
。综上可以得到x＝
±
m
′
/α，即
[0078]
通过上述转化，齐次的二次约束二次规划模型转化为了带有秩约束的半定规划模型。
[0079]
所述步骤s4中，
[0080]
由于秩约束(27)仍然为非凸约束，属于np
‑
困难问题，所以采用罚函数的方法对其进行转化：
[0081]
性能函数：
[0082]
j
′0＝mintr(q
’0x) γ
·
rank(x)
ꢀꢀꢀ
(28)
[0083]
约束：
[0084]
tr(q’i
x)≤c
i
,i＝1,...,p
ꢀꢀꢀ
(29)
[0085]
tr(q'
j
x)＝c
j
,j＝1,...,q,q 1
ꢀꢀꢀ
(30)
[0086]
x≥0
ꢀꢀꢀ
(31)
[0087]
其中j
′0代表加入了秩惩罚后的性能函数，γ为惩罚系数，其范围为[100， ∞)。
[0088]
对于半正定矩阵，其特征值大于零或等于0，其秩为其大于零的特征值的数目。引入函数ρ(z)＝1
‑
e
‑
z/σ
，其中σ为极小的常数。当z＝0时，ρ(z)＝0；当z＞0时，ρ(z)＝1。则矩阵x的秩可以近似地表示为其中λ
i
,i＝1...n为矩阵x的特征值。
[0089]
但由于rank
′
(x)仍为非凸函数，需要对其进行线性化，给出其梯度为：
[0090][0091]
其中u∈r
n
×
n
为矩阵x的特征值所对应的特征向量所组成的矩阵。
[0092]
由于其非凸性，可以得到：
[0093]
rank
′
(x)≤rank
′
(x
k
) tr(rank
′
(x
k
)
·
(x
‑
x
k
))＝r(x,x
k
)
ꢀꢀꢀ
(33)
[0094]
其中x
k
为第k次迭代的解。
[0095]
故性能函数转化为：
[0096]
j
′
＝mintr(q'0x) γ
·
r(x,x
k
)
ꢀꢀꢀ
(34)
[0097]
所述步骤s5中，给出逐次迭代策略：
[0098]
首先给出两个模型：
[0099]
模型1：j0＝mintr(q
’0x)
[0100]
tr(q’i
x)≤c
i
,i＝1,...p，tr(q'
j
x)＝c
j
,j＝1,...,q,q 1，x≥0
[0101]
模型2：j
′
＝mintr(q'0x) γ
·
r(x,x
k
)
[0102]
tr(q’i
x)≤c
i
,i＝1,...p，tr(q'
j
x)＝c
j
,j＝1,...,q,q 1，x≥0
[0103]
再设置初始的惩罚系数和替代系数γ＝γ0,σ＝σ0，求解不考虑秩约束且性能函数为j0＝mintr(q
’0x)的模型1得到无人机的第一条飞行路径；接着求解性能函数为j
′
＝mintr(q'0x) γ
·
r(x,x
k
)的模型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，即得到无人机的最优飞行路径。
[0104]
本发明与现有技术相比的有益效果在于：
[0105]
(1)现有方法，例如势函数，快速随机搜索树，a*等方法无法同时考虑本文中所考虑的约束，并且只能得到无人机飞行的可行路径而不是最优路径。本发明考虑了运动学约束，禁飞区约束，饱和约束和初末状态约束；且性能函数为时间最优，得不需要提供初始路径即可开始迭代，且可以保证所得到的路径为飞行时间最优。
[0106]
(2)现有方法所能考虑的约束种类和数目都受到限制，本发明方法，便于增加约束的数目和种类。
附图说明
[0107]
图1为本发明实施例的基于二次规划的无人机路径规划方法流程图；
[0108]
图2为本发明实施例的无人机路径规划示意图；
[0109]
图3为本发明是实施的无人机路径规划方法的无人机二维路径示意图。
具体实施方式
[0110]
下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
[0111]
如图1所示，本发明实施例的基于二次规划的无人机路径规划方法，包括如下步骤：
[0112]
s1：将二维无人机运动模型，无人机飞行中所存在的禁飞区约束数学模型、饱和约束数学模型、初末状态约束的数学模型及时间最优的性能函数复合，得到无人机路径规划的数学模型。具体过程如下：
[0113]
(1)二维无人机运动模型为：
[0114][0115]
其中，x表示无人机的横坐标，y表示无人机的纵坐标，η表示无人机的航向角，v表示无人机的飞行速度，u表示控制输入，且u的方向与无人机的速度方向垂直；
[0116]
(2)禁飞区约束可以描述为：
[0117]
无人机的禁飞区为圆形或者椭圆形，其数学模型为二次形式，且其为非凸的：
[0118][0119]
其中(x
e
,y
e
)为椭圆或圆的圆心，若禁飞区为圆，则a
e
＝b
e
为圆的半径；若禁飞区为椭圆，则a
e
,b
e
分别为椭圆的半长轴和半短轴。
[0120]
(3)饱和约束即控制输入存在饱和，其数学模型为：
[0121]
|u|≤u
max
ꢀꢀꢀ
(3)
[0122]
其中u
max
为航向角最大变化率。
[0123]
(4)无人机的初末位置约束可以描述为：
[0124][0125]
其中，t0表示初始时刻，t
f
表示到达终点的时刻，(x0,y0)表示无人机的初始位置，(x
f
,y
f
)表示无人机的期望位置。
[0126]
(5)性能函数为基于时间最优的性能函数，其数学描述为：
[0127][0128]
公式(5)表示对时间进行积分，1代表数字1，数字1无物理意义；
[0129]
无人机路径规划数学模型为以上五个模型之和。
[0130]
s2：将步骤s1中得到的无人机路径规划的数学模型进行转化和离散化处理，得到具有非凸二次约束的二次规划模型。具体过程如下：
[0131]
(1)对于步骤s1中的模型进行转化处理：
[0132]
由于存在航向角的三角函数，且三角函数为强非凸非线性的，故采用变量代换的方法对无人机运动模型进行转化：
[0133][0134]
其中u1＝cosη,u2＝sinη，且由于变量代换，引入了新的等式约束：
[0135][0136]
根据运动学方程(1)，可以得到
[0137][0138]
故饱和约束(4)转化为如下形式：
[0139]
‑
u3‑
u1u
max
≤0；u3‑
u1u
max
≤0
ꢀꢀꢀ
(8)
[0140]
(2)对转化后的模型进行离散化：
[0141]
性能函数为：
[0142]
j1＝minn
·
δt
ꢀꢀꢀ
(9)
[0143]
约束为：
[0144]
x
i 1
‑
x
i
＝vu
1i
δt；y
i 1
‑
y
i
＝vu
2i
δt；u
2i
‑
u
2(i 1)
＝u
3i
δt
ꢀꢀꢀ
(10)
[0145][0146][0147][0148]
x1＝x0,x
n 1
＝x
f
,y1＝y0,y
n 1
＝y
f
ꢀꢀꢀ
(14)
[0149]
其中n 1表示所设置的离散时间节点，表示离散时间步长；j1代表离散后的性能函数；公式中带有下标的变量表示的是第i个离散时间节点时该变量的值，变量的物理意义与上文相同。
[0150]
该模型为具有非凸二次约束的二次规划模型。
[0151]
s3：基于步骤s2中的二次规划模型，将其转化为齐次形式，并采用半定松弛的方法转化为带有秩约束的半定规划模型。具体过程如下：
[0152]
(1)二次约束二次规划的一般模型：
[0153]
经过步骤s2，无人机路径规划模型已经转化为二次约束二次规划形式，设二次约束二次规划的一般形式为：
[0154]
性能函数：
[0155][0156]
约束：
[0157][0158][0159]
其中j0代表一般形式的性能函数，x∈r
m
为列向量,q0,q
i
,q
j
∈r
m
×
m
为对称矩阵，b0,
b
i
,b
j
∈r
m
为列向量，c
i
,c
j
∈r为常数。若性能函数或约束为线性则对称矩阵q
l
,l＝0,i,j为零矩阵，若向量b
l
,l＝0,i,j为零向量，则性能函数或约束为齐次方程。
[0160]
(2)引入常数变量α∈r，将二次约束二次规划模型转化为齐次形式：
[0161]
性能函数：
[0162][0163]
约束：
[0164][0165][0166]
α2＝1
ꢀꢀꢀ
(21)
[0167]
其中α为常数变量。
[0168]
(3)采用半定松弛对齐次二次约束二次规划模型进行松弛，得到半定规划模型：
[0169]
性能函数：
[0170]
j0＝mintr(q
’0x)
ꢀꢀꢀ
(22)
[0171]
约束：
[0172]
tr(q’i
x)≤c
i
,i＝1,...,p
ꢀꢀꢀ
(23)
[0173]
tr(q'
j
x)＝c
j
,j＝1,...,q,q 1
ꢀꢀꢀ
(24)
[0174][0175]
其中tr(
·
)代表矩阵的迹，x∈r
n
(n＝m 1)为新引入的变量。其中约束(21)α2＝1被包含在了等式约束(25)中，为第q 1个等式约束。
[0176]
约束为非凸且非线性约束，其等价于：
[0177]
x≥0
ꢀꢀꢀ
(26)
[0178]
rank(x)＝1
ꢀꢀꢀ
(27)
[0179]
其中rank(
·
)代表矩阵的秩，(
·
)≥0代表矩阵为半正定矩阵。
[0180]
下面对等价性进行证明，因为则显然有x≥0且其秩为1；如果公式(26)和(27)被满足，那么意味着x＝mm
t
，其中m∈r
n
。考虑到约束(25)中包括了x(n,n)＝1，可以得到m＝[m
′
,
±
1]，其中m
′
∈r
n
。综上可以得到x＝
±
m
′
/α，进一步可以得到，
[0181]
通过上述转化，齐次的二次约束二次规划模型转化为了带有秩约束的半定规划模型。
[0182]
s4：采用罚函数方法对步骤s3中存在的秩约束进行松弛，得到可以用半定规划方法求解的半定规划模型。具体实现如下：
[0183]
由于秩约束(27)仍然为非凸约束，采用罚函数的方法对其进行转化：
[0184]
性能函数：
[0185]
j
′0＝mintr(q
’0x) γ
·
rank(x)
ꢀꢀꢀ
(28)
[0186]
约束：
[0187]
tr(q’i
x)≤c
i
,i＝1,...,p
ꢀꢀꢀ
(29)
[0188]
tr(q'
j
x)＝c
j
,j＝1,...,q,q 1
ꢀꢀꢀ
(30)
[0189]
x≥0
ꢀꢀꢀ
(31)
[0190]
其中j
′0代表加入了秩惩罚后的性能函数，γ为惩罚系数，其范围为[100， ∞)。
[0191]
对于半正定矩阵，其特征值大于零或等于0，其秩为其大于零的特征值的数目。为了使得带有秩约束的问题能够求解，秩约束需要被写成更为清晰可解的形式，可以采用某些函数对秩约束进行近似。
[0192]
引入函数ρ(z)＝1
‑
e
‑
z/σ
，其中σ为极小的常数。当z＝0时，ρ(z)＝0；当z＞0时，ρ(z)＝1。则矩阵x的秩可以近似地表示为其中λ
i
,i＝1...n为矩阵x的特征值。
[0193]
但由于rank
′
(x)仍为非凸函数，需要对其进行线性化，给出其梯度为：
[0194][0195]
其中u∈r
n
×
n
为矩阵x的特征值所对应的特征向量所组成的矩阵。
[0196]
由于其非凸性，可以得到：
[0197]
rank
′
(x)≤rank
′
(x
k
) tr(rank
′
(x
k
)
·
(x
‑
x
k
))＝r(x,x
k
)
ꢀꢀꢀ
(33)
[0198]
其中x
k
为第k次迭代的解。故可以采用r(x,x
k
)来逼近rank
′
(x)。
[0199]
则性能函数转化为下面的形式：
[0200]
j
′
＝mintr(q'0x) γ
·
r(x,x
k
)
ꢀꢀꢀ
(34)
[0201]
s5：采用逐次迭代的求解策略求解步骤s4中得到的半定规划模型，得到无人机的最优飞行路径。具体实现如下：
[0202]
首先给出两个模型：
[0203]
模型1：j0＝mintr(q
’0x)
[0204]
tr(q’i
x)≤c
i
,i＝1,...p，tr(q'
j
x)＝c
j
,j＝1,...,q,q 1，x≥0
[0205]
模型2：j
′
＝mintr(q'0x) γ
·
r(x,x
k
)
[0206]
tr(q’i
x)≤c
i
,i＝1,...p，tr(q'
j
x)＝c
j
,j＝1,...,q,q 1，x≥0
[0207]
再设置初始的惩罚系数和替代系数γ＝γ0,σ＝σ0，求解不考虑秩约束且性能函数
为j0＝mintr(q
’0x)的模型1得到无人机的第一条飞行路径；接着求解性能函数为j
′
＝mintr(q'0x) γ
·
r(x,x
k
)的模型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，即得到无人机的最优飞行路径。
[0208]
本发明中所有变量上标“·”都是该变量的导数，除非该变量的导数有实际物理含义。
[0209]
下面以考虑了五个禁飞区约束的某无人机路径规划场景为例，说明本发明所提出的方法的有效性。定义无人机的速度为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。
[0210]
综上，本发明所提出的基于二次规划的无人机路径规划方法可以在满足约束的情况下得到无人机的时间最优路径，且该方法便于增加约束的数目和种类。