控制架构
文章继续采用的是 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），迭代出最优轨迹和最优策略。