asp 网站管理系统,网络营销营销型网站建设,网站备案换公司吗,中文域名注册费用控制架构
文章继续采用的是 ULTRA-Extra无人机#xff0c;相关参数如下#xff1a; 这里用于guidance law的无人机运动学模型为#xff1a; { x ˙ p V a cos γ cos χ V w cos γ w cos χ w y ˙ p V a cos γ sin χ V w cos γ w sin χ…控制架构
文章继续采用的是 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˙pVacosγcosχVwcosγwcosχwy˙pVacosγsinχVwcosγwsinχwz˙pVasinγ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)xB_v(\rho)u x˙Av(ρ)xBv(ρ)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)^Tr-(x_p,y_p,z_p)^T e(xr−xp,yr−yp,zr−zp)Tr−(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)Tr˙−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˙eAe(ρ)xeBe(ρ)uce 其中 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,k1} A_e(\rho_k)x_{e,k}B_e(\rho_k)u_{e,k}c_{r,k} xe,k1Ae(ρk)xe,kBe(ρk)ue,kcr,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^2A_e(\rho_k)T_sI \\ 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^2B_e(\rho_k)T_s Ae(ρk)241Ae(ρk)4Ts461Ae3(ρk)Ts321Ae(ρk)2Ts2Ae(ρk)TsIBe(ρk)241Ae(ρk)3Be(ρk)Ts461Ae2(ρk)Be(ρk)Ts321Ae(ρk)Be(ρk)Ts2Be(ρ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}1x(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)]∫t0tf1x(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 QQ^T\geq 0,RR^T\geq 0 QQT≥0,RRT≥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∂H2RuBv(ρ)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 ] H1x^TQxu^TRu\lambda^T [A_v(\rho)xB_v(\rho)u] H1xTQxuTRuλT[Av(ρ)xBv(ρ)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)xB_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(ρ)xBv(ρ)u))x˙∂λ∂HAv(ρ)xBv(ρ)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迭代出最优轨迹和最优策略。
