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









![YOLOv11融合[CVPR2024]Starnet中的star block取模块](https://i-blog.csdnimg.cn/direct/1049d59124754d98a2b377cecb161b4f.png)








