控制架构
文章继续采用的是 ULTRA-Extra无人机,相关参数如下:
这里用于guidance law的无人机运动学模型为:
{
x
˙
p
=
V
a
cos
γ
cos
χ
+
V
w
cos
γ
w
cos
χ
w
y
˙
p
=
V
a
cos
γ
sin
χ
+
V
w
cos
γ
w
sin
χ
w
z
˙
p
=
V
a
sin
γ
+
V
w
sin
γ
w
χ
˙
=
g
tan
ϕ
/
V
a
γ
˙
=
g
(
n
z
cos
ϕ
−
cos
γ
)
/
V
a
\begin{cases} \dot{x}_p = V_a\cos\gamma\cos\chi + V_w\cos\gamma_w\cos\chi_w \\ \dot{y}_p = V_a\cos\gamma\sin\chi + V_w\cos\gamma_w\sin\chi_w \\ \dot{z}_p = V_a\sin\gamma + V_w\sin\gamma_w \\ \dot{\chi} = g\tan\phi/V_a \\ \dot{\gamma} = g(n_z\cos\phi-\cos\gamma)/V_a \end{cases}
⎩
⎨
⎧x˙p=Vacosγcosχ+Vwcosγwcosχwy˙p=Vacosγsinχ+Vwcosγwsinχwz˙p=Vasinγ+Vwsinγwχ˙=gtanϕ/Vaγ˙=g(nzcosϕ−cosγ)/Va
其中状态量为
(
x
p
,
y
p
,
z
p
,
γ
,
χ
)
(x_p,y_p,z_p,\gamma,\chi)
(xp,yp,zp,γ,χ),控制量为
(
V
a
,
n
z
,
ϕ
)
(V_a,n_z,\phi)
(Va,nz,ϕ)。在自动驾驶仪(Autopilot)中,采用 Successive-Loop-Closure (SLC)实现参考量
(
V
a
m
,
n
z
m
,
ϕ
m
)
(V_{a_m},n_{z_m},\phi_m)
(Vam,nzm,ϕm)的信号跟踪:
自动驾驶仪中依然采用横纵向通道的SLC实现控制,相应的控制逻辑如下:
Path Following 最优控制器
对运动学模型进行二阶求导可以得到:
(
x
˙
p
y
˙
p
z
˙
p
χ
˙
γ
˙
x
¨
p
y
¨
p
z
¨
p
χ
¨
γ
¨
V
˙
a
ϕ
˙
n
˙
z
)
=
(
O
5
×
5
I
5
O
5
×
3
−
V
a
cos
γ
sin
χ
−
V
a
sin
γ
cos
χ
V
a
cos
γ
cos
χ
−
V
a
sin
γ
sin
χ
O
5
×
5
O
5
×
3
0
V
a
cos
γ
O
5
×
3
0
0
0
g
sin
γ
V
a
O
3
×
13
)
(
x
p
y
p
z
p
χ
γ
x
˙
p
y
˙
p
z
˙
p
χ
˙
γ
˙
V
a
ϕ
n
z
)
+
(
O
5
×
3
cos
γ
cos
χ
0
0
cos
γ
sin
χ
0
0
sin
γ
0
0
−
g
tan
ϕ
V
a
2
g
V
a
cos
2
ϕ
0
g
(
cos
γ
−
n
z
cos
ϕ
)
V
a
2
−
g
n
z
sin
ϕ
V
a
g
cos
ϕ
V
a
I
3
)
(
V
˙
a
ϕ
˙
n
˙
z
)
\left( \begin{matrix} {{{\dot{x}}}_{p}} \\ {{{\dot{y}}}_{p}} \\ {{{\dot{z}}}_{p}} \\ {\dot{\chi }} \\ {\dot{\gamma }} \\ {{{\ddot{x}}}_{p}} \\ {{{\ddot{y}}}_{p}} \\ {{{\ddot{z}}}_{p}} \\ {\ddot{\chi }} \\ {\ddot{\gamma }} \\ \dot{V}_a\\ \dot{\phi} \\ \dot{n}_z\\ \end{matrix} \right)=\left( \begin{matrix} {{O}_{5\times 5}} & {} & {{I}_{5}} & {} & O_{5\times 3} \\ {} & {} & -{{V}_{a}}\cos \gamma \sin \chi & -{{V}_{a}}\sin \gamma \cos \chi \\ {} & {} & {{V}_{a}}\cos \gamma \cos \chi & -{{V}_{a}}\sin \gamma \sin \chi \\ {{O}_{5\times 5}} & {{O}_{5\times 3}} & 0 & {{V}_{a}}\cos \gamma & O_{5\times 3}\\ {} & {} & 0 & 0 \\ {} & {} & 0 & \frac{g\sin \gamma }{V_{a}^{{}}} \\ {} & {} & {} & O_{3 \times 13} \end{matrix} \right)\left( \begin{matrix} {{x}_{p}} \\ {{y}_{p}} \\ {{z}_{p}} \\ \chi \\ \gamma \\ {{{\dot{x}}}_{p}} \\ {{{\dot{y}}}_{p}} \\ {{{\dot{z}}}_{p}} \\ {\dot{\chi }} \\ {\dot{\gamma }} \\ V_a\\ \phi \\n_z \end{matrix} \right)+\left( \begin{matrix} {} & {{O}_{5\times 3}} & {} \\ \cos \gamma \cos \chi & 0 & 0 \\ \cos \gamma \sin \chi & 0 & 0 \\ \sin \gamma & 0 & 0 \\ -\frac{g\tan \phi }{V_{a}^{2}} & \frac{g}{{{V}_{a}}{{\cos }^{2}}\phi } & 0 \\ \frac{g(\cos \gamma -{{n}_{z}}\cos \phi )}{V_{a}^{2}} & -\frac{g{{n}_{z}}\sin \phi }{V_{a}^{{}}} & \frac{g\cos \phi }{V_{a}^{{}}} \\ & I_{3} &\\ \end{matrix} \right)\left( \begin{align} & {{{\dot{V}}}_{a}} \\ & {\dot{\phi }} \\ & {{{\dot{n}}}_{z}} \\ \end{align} \right)
x˙py˙pz˙pχ˙γ˙x¨py¨pz¨pχ¨γ¨V˙aϕ˙n˙z
=
O5×5O5×5O5×3I5−VacosγsinχVacosγcosχ000−Vasinγcosχ−VasinγsinχVacosγ0VagsinγO3×13O5×3O5×3
xpypzpχγx˙py˙pz˙pχ˙γ˙Vaϕnz
+
cosγcosχcosγsinχsinγ−Va2gtanϕVa2g(cosγ−nzcosϕ)O5×3000Vacos2ϕg−VagnzsinϕI30000Vagcosϕ
V˙aϕ˙n˙z
这里设
ρ
=
(
γ
,
χ
,
V
a
,
ϕ
,
n
z
)
T
\rho=(\gamma,\chi,V_a,\phi,n_z)^T
ρ=(γ,χ,Va,ϕ,nz)T,
x
=
(
x
p
,
y
p
,
z
p
,
χ
,
γ
,
x
˙
p
,
y
˙
p
,
z
˙
p
,
χ
˙
,
γ
˙
,
V
a
,
ϕ
,
n
z
)
T
x=(x_p,y_p,z_p,\chi,\gamma,\dot{x}_p,\dot{y}_p,\dot{z}_p,\dot{\chi},\dot{\gamma},V_a,\phi,n_z)^T
x=(xp,yp,zp,χ,γ,x˙p,y˙p,z˙p,χ˙,γ˙,Va,ϕ,nz)T,
u
=
(
V
˙
a
,
ϕ
˙
,
n
˙
z
)
T
u=(\dot{V}_a,\dot{\phi},\dot{n}_z)^T
u=(V˙a,ϕ˙,n˙z)T,得到:
x
˙
=
A
v
(
ρ
)
x
+
B
v
(
ρ
)
u
\dot{x}=A_v(\rho)x+B_v(\rho)u
x˙=Av(ρ)x+Bv(ρ)u
假设要跟踪的量为 r = ( x r , y r , z r ) T r=(x_r,y_r,z_r)^T r=(xr,yr,zr)T,构造跟踪向量 e = ( x r − x p , y r − y p , z r − z p ) T = r − ( x p , y p , z p ) T e=(x_r-x_p,y_r-y_p,z_r-z_p)^T=r-(x_p,y_p,z_p)^T e=(xr−xp,yr−yp,zr−zp)T=r−(xp,yp,zp)T, e ˙ = r ˙ − ( x ˙ p , y ˙ p , z ˙ p ) T = r ˙ − C x \dot{e} = \dot{r} - (\dot{x}_p,\dot{y}_p,\dot{z}_p)^T=\dot{r}-Cx e˙=r˙−(x˙p,y˙p,z˙p)T=r˙−Cx,有:
( x ˙ e ˙ ) = ( A v ( ρ ) O 13 × 3 − C O 3 × 3 ) ( x e ) + ( B v ( ρ ) O 3 × 3 ) u + ( O 13 × 1 r ˙ ) \begin{pmatrix} \dot{x} \\ \dot{e} \end{pmatrix} = \begin{pmatrix} A_v(\rho) &O_{13 \times 3} \\ -C & O_{3 \times 3} \end{pmatrix}\begin{pmatrix} x \\ e \end{pmatrix} +\begin{pmatrix} B_v(\rho)\\O_{3 \times 3} \end{pmatrix}u+\begin{pmatrix} O_{13\times 1} \\\dot{r} \end{pmatrix} (x˙e˙)=(Av(ρ)−CO13×3O3×3)(xe)+(Bv(ρ)O3×3)u+(O13×1r˙)
上市被描述为:
x ˙ e = A e ( ρ ) x e + B e ( ρ ) u + c e \dot{x}_{e}=A_e(\rho)x_e + B_e(\rho)u + c_e x˙e=Ae(ρ)xe+Be(ρ)u+ce
其中,
C = ( O 3 × 5 ∣ I 3 ∣ O 3 × 5 ) C=\begin{pmatrix} O_{3\times 5} | I_3 |O_{3\times 5} \end{pmatrix} C=(O3×5∣I3∣O3×5)
利用4阶Runge-Kutta法可以将上式可以离散化为一个LPV状态空间方程(linear parameter varying state-space representation):
x e , k + 1 = A e ( ρ k ) x e , k + B e ( ρ k ) u e , k + c r , k x_{e,k+1} = A_e(\rho_k)x_{e,k}+B_e(\rho_k)u_{e,k}+c_{r,k} xe,k+1=Ae(ρk)xe,k+Be(ρk)ue,k+cr,k
其中, T s T_s Ts是采样时间,
A e ( ρ k ) = 1 24 A e ( ρ k ) 4 T s 4 + 1 6 A e 3 ( ρ k ) T s 3 + 1 2 A e ( ρ k ) 2 T s 2 + A e ( ρ k ) T s + I B e ( ρ k ) = 1 24 A e ( ρ k ) 3 B e ( ρ k ) T s 4 + 1 6 A e 2 ( ρ k ) B e ( ρ k ) T s 3 + 1 2 A e ( ρ k ) B e ( ρ k ) T s 2 + B e ( ρ k ) T s A_e(\rho_k)=\frac{1}{24}A_e(\rho_k)^4T_s^4+\frac{1}{6}A^3_e(\rho_k)T_s^3+\frac{1}{2}A_e(\rho_k)^2T_s^2+A_e(\rho_k)T_s+I \\ B_e(\rho_k)=\frac{1}{24}A_e(\rho_k)^3B_e(\rho_k)T_s^4+\frac{1}{6}A^2_e(\rho_k)B_e(\rho_k)T_s^3+\frac{1}{2}A_e(\rho_k)B_e(\rho_k)T_s^2+B_e(\rho_k)T_s Ae(ρk)=241Ae(ρk)4Ts4+61Ae3(ρk)Ts3+21Ae(ρk)2Ts2+Ae(ρk)Ts+IBe(ρk)=241Ae(ρk)3Be(ρk)Ts4+61Ae2(ρk)Be(ρk)Ts3+21Ae(ρk)Be(ρk)Ts2+Be(ρk)Ts
上述轨迹跟踪问题可以转化为:
min
u
(
t
)
J
[
u
(
t
)
]
=
∫
t
0
t
f
1
+
x
(
t
)
T
Q
x
(
t
)
+
u
(
t
)
T
R
u
(
t
)
d
t
x
˙
(
t
)
=
A
v
(
ρ
)
x
(
t
)
+
B
v
(
ρ
)
u
(
t
)
x
(
t
0
)
=
x
0
,
E
x
(
t
f
)
=
(
x
r
,
y
r
,
z
r
)
T
d
min
≤
D
x
(
t
)
≤
d
max
\min_{u(t)}J[u(t)]=\int_{t_0}^{t_f}1+x(t)^TQx(t)+u(t)^TRu(t)dt \\ \dot{x}(t)=A_v(\rho)x(t) + B_v(\rho)u(t) \\ x(t_0)=x_0,Ex(t_f)=(x_r,y_r,z_r)^T\\ d_{\min} \leq Dx(t) \leq d_{\max}
u(t)minJ[u(t)]=∫t0tf1+x(t)TQx(t)+u(t)TRu(t)dtx˙(t)=Av(ρ)x(t)+Bv(ρ)u(t)x(t0)=x0,Ex(tf)=(xr,yr,zr)Tdmin≤Dx(t)≤dmax
其中:
E
=
(
I
3
,
O
3
×
10
)
E=(I_3,O_{3\times 10})
E=(I3,O3×10),
D
=
(
O
3
×
10
,
I
3
)
D = (O_{3\times 10},I_3)
D=(O3×10,I3),
Q
=
Q
T
≥
0
,
R
=
R
T
≥
0
Q=Q^T\geq 0,R=R^T\geq 0
Q=QT≥0,R=RT≥0,
d
min
=
(
V
a
min
,
ϕ
a
min
,
n
z
min
)
T
d_{\min}=(V_{a\min},\phi_{a\min},n_{z\min})^T
dmin=(Vamin,ϕamin,nzmin)T,
d
max
=
(
V
a
max
,
ϕ
a
max
,
n
z
max
)
T
d_{\max}=(V_{a\max},\phi_{a\max},n_{z\max})^T
dmax=(Vamax,ϕamax,nzmax)T。令
∂
H
∂
u
=
2
R
u
+
B
v
(
ρ
)
T
λ
=
0
\frac{\partial H}{\partial u}=2Ru + B_v(\rho)^T\lambda = 0
∂u∂H=2Ru+Bv(ρ)Tλ=0,得到:
u
=
−
1
2
R
−
1
B
v
(
ρ
)
T
λ
u = -\frac{1}{2}R^{-1}B_v(\rho)^T\lambda
u=−21R−1Bv(ρ)Tλ
构造Hamilton函数
H
=
1
+
x
T
Q
x
+
u
T
R
u
+
λ
T
[
A
v
(
ρ
)
x
+
B
v
(
ρ
)
u
]
H=1+x^TQx+u^TRu+\lambda^T [A_v(\rho)x+B_v(\rho)u]
H=1+xTQx+uTRu+λT[Av(ρ)x+Bv(ρ)u],令
ρ
=
x
\rho =x
ρ=x:
{
λ
˙
=
−
∂
H
∂
x
=
−
(
2
Q
x
+
λ
T
∂
∂
x
(
A
v
(
ρ
)
x
+
B
v
(
ρ
)
u
)
)
x
˙
=
∂
H
∂
λ
=
A
v
(
ρ
)
x
+
B
v
(
ρ
)
u
\begin{cases} \dot{\lambda}=-\frac{\partial H}{\partial x}=-(2Qx+\lambda^T\frac{\partial}{\partial x}(A_v(\rho)x+B_v(\rho)u)) \\ \dot{x} =\frac{\partial H}{\partial \lambda}= A_v(\rho)x + B_v(\rho)u \end{cases}
{λ˙=−∂x∂H=−(2Qx+λT∂x∂(Av(ρ)x+Bv(ρ)u))x˙=∂λ∂H=Av(ρ)x+Bv(ρ)u
其中,
∂
∂
x
[
A
v
(
ρ
)
x
]
=
?
∂
∂
x
[
B
v
(
ρ
)
u
]
=
−
1
2
∂
∂
x
[
B
v
(
ρ
)
R
−
1
B
v
(
ρ
)
T
λ
]
=
?
\frac{\partial}{\partial x}[A_v(\rho)x] = ?\\ \frac{\partial }{\partial x}[B_v(\rho)u] = -\frac{1}{2}\frac{\partial }{\partial x}[B_v(\rho)R^{-1}B_v(\rho)^T\lambda] = ?
∂x∂[Av(ρ)x]=?∂x∂[Bv(ρ)u]=−21∂x∂[Bv(ρ)R−1Bv(ρ)Tλ]=?
其中
H
(
t
f
)
=
0
H(t_f)=0
H(tf)=0,应该采用打靶法得到
t
f
t_f
tf和
λ
0
\lambda_0
λ0,能使得:
∣
∣
E
x
(
t
f
)
−
(
x
r
,
y
r
,
z
r
)
T
∣
∣
≤
ε
1
∣
∣
H
(
t
f
)
∣
∣
≤
ε
2
d
min
≤
D
x
(
t
)
≤
d
max
||Ex(t_f)-(x_r,y_r,z_r)^T|| \leq \varepsilon_1 \\ ||H(t_f)||\leq \varepsilon_2\\ d_{\min} \leq Dx(t) \leq d_{\max}
∣∣Ex(tf)−(xr,yr,zr)T∣∣≤ε1∣∣H(tf)∣∣≤ε2dmin≤Dx(t)≤dmax
获取上述的量后,如何就可以用Matlab的ode45
函数,或者直接采用bvp4c
将上述两点边值问题(BVP),迭代出最优轨迹和最优策略。