goddardfuns.f90

一个基于打靶法的最优控制求解软件 求解过程中采用参数延续算法

!!*********************************
!!* Shoot package v1.0            *
!!* Functions for Goddard problem *
!!* Author: Pierre Martinon       *
!!* INRIA FUTURS, team COMMANDS   *
!!* CMAP, ECOLE POLYTECHNIQUE     * 
!!* 11/2007                       *
!!*********************************


!! Homotopies
!! 1: Max mf: Quadratic perturbation, critere Min int_0^tf |u| + (1-lambda) u^2
!! 2: Max mf: Atmosphere introduction, Kd = lambda Kd_max, critere Min int_0^tf |u|+u^2
!! 3: Max mf: Quadratic perturbation, critere Min -mf + int_0^tf (1-lambda) u^2
!! 4: Max mf: Atmosphere introduction, Kd = lambda Kd_max, critere Min -mf + int_0^tf u^2



!! State:          [ r  v  m  ] dim 7 (r and v in R^3)
!! Costate:        [ Pr Pv Pm ] dim 7 (idem)
!! IVP unknown z:  [ Pr Pv Pm ] dim 7   

!! with free final time
!! State:          [  r  v  m  tf ] dim 8 (r and v in R^3)
!! Costate:        [ Pr Pv Pm Ptf ] dim 8 (idem)
!! IVP unknown z:  [ tf Pr Pv Pm ] dim 8   
!! indices            8  9 to 15

!! Choix des unites (cf article Oberle)
!! distance: exprimee en rayon terrestre, ie r=1 correspond a 6378 km
!! vitesse: exprimee en vitesse de satellisation (1ere vitesse cosmique), ie v=1 correspond a environ 7900 m.s-1
!! temps: du coup, le temps est exprime en une unite a la noix, ie t=1 correspond a environ 804s
!! la gravite g0 est alors renormalisee a 1

!! Parameters
!! [1 2 3]: Tmax, beta, Kdmax
!4: freetf (free final time formulation)
!0: fixed final time
!1: free final time as state with associated costate
!! 5: controlcorr 
!! - 0 for exact singular control expression from psi'' = 0
!! - 1 for exact singular control with numerical correction
!! - 2 for alternate singular control from argmin psi^2 + psi'^2

Module auxmod
  implicit none

  real(kind=8), save :: pm, mass, cg, ex, Kex, normv, normr, normpv
  real(kind=8), save :: ra,rb,rc,va,vb,vc,pra,prb,prc,pva,pvb,pvc
  real(kind=8), save :: grava,gravb,gravc,draga,dragb,dragc

end Module auxmod


Module SpecFuns
  use shootdefs
  implicit none

  integer, save :: freetf, fullthrust

  real(kind=8), parameter :: g0 = 1d0 !9.81d0 / 6378d3 * 804**2
  real(kind=8), save :: beta, Tmax, Kd, Kdmax



contains

  !!*******************
  !!* Initializations *
  !!*******************
  Subroutine InitPar(mode,z,lambda)
    implicit none

    integer, intent(in) :: mode     
    real(kind=8), intent(in) :: lambda  
    real(kind=8), dimension(n), intent(in) :: z  

    !!mode = 0: first call
    !!mode = 1: homotopy call

    !discshoot = 0

    if (mode == 0) then

       Tmax = par(1)
       beta = par(2)
       Kdmax = par(3)
       Kd = Kdmax

       lower(1) = par(4)
       upper(1) = par(5)

       freetf = 1

    else

       select case (homotop)
       case (2,4)
          !atmospher homotopy
          Kd = Kdmax * lambda
       end select

       !free final time formulation (freetf)
       !0: fixed final time
       !1: free final time as state with associated costate

       select case (freetf)
       case (0)
          tff = tf
       case (1)
          tff = max(z(1) / zscal(1),0d0)
       case default
          write(0,*) 'ERROR: InitPar >>> Unknown freetf value...',freetf
          stop
       end select

    end if


  end subroutine InitPar


  Subroutine AuxVars(lambda,x,p)
    use auxmod
    implicit none

    real(kind=8), intent(in) ::  lambda                 
    real(kind=8), intent(in), dimension(ns) :: x         
    real(kind=8), intent(in), dimension(nc) :: p  
      
    !Auxiliary variables

    if (freetf == 1) then
       mass = x(ns-1)
       pm = p(nc-1)
    else
       mass = x(ns)
       pm = p(nc)
    end if

    ra = x(1)
    va = x(m+1)
    pra = p(1)
    pva = p(m+1)
    
    if (m > 1) then
       rb = x(2)
       vb = x(m+2)
       prb = p(2)
       pvb = p(m+2)
    else
       rb = 0d0
       vb = 0d0
       prb = 0d0
       pvb = 0d0
    end if

    if (m > 2) then
       rc = x(3)
       vc = x(m+3)
       prc = p(3)
       pvc = p(m+3)
    else
       rc = 0d0
       vc = 0d0
       prc= 0d0
       pvc = 0d0
    end if

    normr = sqrt(ra**2 + rb**2 + rc**2)
    normv = sqrt(va**2 + vb**2 + vc**2)
    normpv = sqrt(pva**2 + pvb**2 + pvc**2)

    cg = ra**2 + rb**2 + rc**2

    ex = exp(-500d0 * normr + 500d0)
    Kex = Kd * ex

    !D(1:3) = kd * normv * exp(-500d0 * (h - 1d0)) * v(1:3)
    draga = Kex * normv  * va
    dragb = Kex * normv  * vb
    dragc = Kex * normv  * vc

    !g(1:3) =  g0 / normr**3 * x(1:3)
    grava = g0 / normr**3 * ra
    gravb = g0 / normr**3 * rb
    gravc = g0 / normr**3 * rc


  end Subroutine AuxVars


  !!*****************************
  !!* Compute Optimal control u *
  !!*****************************
  Subroutine Control(lambda,x,p,u)
    use auxmod
    implicit none

    real(kind=8), intent(in) ::  lambda                 
    real(kind=8), intent(in), dimension(ns) :: x         
    real(kind=8), intent(in), dimension(nc) :: p        
    real(kind=8), intent(out), dimension(m) :: u         


    !Local
    real(kind=8) :: coeffc, A, B, alpha, norm_comp, coeff
    real(kind=8) :: comp(m)

    !controlmode
    !0: compute usual control via Hamiltonian minimization (Control)
    !1: compute bang-bang control (Control)
    !2: compute exact singular control (Control)



    !out init
    u = 0d0

    !set aux vars 
    call AuxVars(lambda,x,p)

    if (homotop == 1 .or. homotop == 2) then
       coeff = 1d0 - beta * Tmax * pm
    else
       coeff = - beta * Tmax * pm
    end if

    comp(1:m) = p(m+1:2*m) * Tmax / mass
    norm_comp =  sqrt(sum(comp(1:m)*comp(1:m)))

    select case (homotop)

    case (1,3)

       !Set coefficient for u^2 term
       coeffc = 2d0 * (1d0 - lambda)
 
       select case (controlmode)

       case (0) !usual control

          if (lambda < 1d0) then

             if (norm_comp <= coeff) then
                u(1:m) = 0d0
             elseif (norm_comp <= coeff + coeffc) then
                u(1:m) = - comp(1:m) * (norm_comp-coeff) / (coeffc*norm_comp)
             else
                u(1:m) = - comp(1:m) / norm_comp
             end if

          else

             if (coeff > norm_comp) then
                u(1:m) = 0d0
             else        
                u(1:m) = - comp(1:m) / norm_comp
             end if

          end if

       case (2) !singular control

          if (lambda < 1d0) then

             if (norm_comp <= coeff) then
                u(1:m) = 0d0
             elseif (norm_comp <= coeff + coeffc) then
                u(1:m) = - comp(1:m) * (norm_comp-coeff) / (coeffc*norm_comp)
             else
                u(1:m) = - comp(1:m) / norm_comp
             end if

          else 

             A = -2d0 * ((normpv * beta * (pva ** 2 + pvc ** 2 + pvb ** 2)& 
                  &* normv**3 * normv ** 2 + (va * pva + vb * pvb + vc *&
                  & pvc) * ((vc ** 2 + va ** 2 / 2d0 + vb ** 2 / 2d0) * pvc ** 2& 
                  &+ vc * (va * pva + vb * pvb) * pvc + (va ** 2 + vb ** 2 / 2d0 +& 
                  &vc ** 2 / 2d0) * pva ** 2 + pva * va * vb * pvb + (2d0 * vb **&
                  & 2 + va ** 2 + vc ** 2) * pvb ** 2 / 2d0) * normv - 2d0 * (pvc&
                  & ** 3 * vc + (va * pva + vb * pvb - normpv * beta * (2d0 * vc **&
                  & 2 + va ** 2 + vb ** 2) / 2d0) * pvc ** 2 + (pva ** 2 - beta * n&
                  &ormpv * pva * va - pvb * (-pvb + beta * normpv * vb)) * vc * pvc +&
                  & pva ** 3 * va + (vb * pvb - normpv * beta * (va ** 2 + vb ** 2 /& 
                  &2d0 + vc ** 2 / 2d0)) * pva ** 2 - va * pvb * (-pvb + beta * n&
                  &ormpv * vb) * pva - pvb ** 2 * (-2d0 * vb * pvb + normpv * beta& 
                  &* (2d0 * vb ** 2 + va ** 2 + vc ** 2)) / 2d0) * normv**3) * Kex&
                  & - normpv * beta * normv * ((2d0 * beta * dragc *& 
                  &normpv + prc * mass) * pvc + (2d0 * beta * draga * normpv + pra& 
                  &* mass) * pva + pvb * (2d0 * beta * dragb * normpv + prb * mass)&
                  &) * normv**3 / 2d0) * Tmax ** 2 * normv**(-3) / normv / normpv ** 2 / mass ** 3


             B = -beta * (pra * normv * mass - 2d0 * (pva * va ** 2 + (vb * pv&
                  &b / 2d0 + vc * pvc / 2d0) * va + pva * (vb ** 2 + vc ** 2) / 0&
                  &.2D1) * Kd * ex) * draga / normv / mass - 2d0 * pva * beta * ((0.25&
                  &0D3 * mass * rc * vc + ex * Kd * normv * normr + 0.250D3 * mass * (ra *& 
                  &va + rb * vb)) * draga + ex * Kd * normv * normr * grava * mass) / no&
                  &rmr / mass + (-prb * normv * mass + ex * Kd * (2d0 * pvb * vb ** 2 + (&
                  &va * pva + vc * pvc) * vb + pvb * (va ** 2 + vc ** 2))) * beta * d&
                  &ragb / normv / mass - 2d0 * ((0.250D3 * mass * rc * vc + ex * Kd * nor&
                  &mv * normr + 0.250D3 * mass * (ra * va + rb * vb)) * dragb + ex * Kd& 
                  &* normv * normr * gravb * mass) * pvb * beta / normr / mass + (2d0 * K&
                  &d * pvc * ex * vc ** 2 + ex * Kd * (vb * pvb + va * pva) * vc - pr&
                  &c * normv * mass + Kd * pvc * ex * (vb ** 2 + va ** 2)) * beta * drag&
                  &c / normv / mass - 2d0 * ((0.250D3 * mass * rc * vc + ex * Kd * normv& 
                  &* normr + 0.250D3 * mass * (ra * va + rb * vb)) * dragc + ex * Kd * n&
                  &ormv * normr * gravc * mass) * pvc * beta / normr / mass + (-(vc ** 2 *& 
                  &(pva ** 2 + pvb ** 2 + 2d0 * pvc ** 2) + 2d0 * pvc * (vb * pvb&
                  & + va * pva) * vc + (pvc ** 2 + pvb ** 2 + 2d0 * pva ** 2) * va& 
                  &** 2 + 2d0 * pva * va * vb * pvb + vb ** 2 * (pvc ** 2 + pva **& 
                  &2 + 2d0 * pvb ** 2)) * Kd * normv ** 2 * normr * ((vc * gravc +& 
                  &va * grava + vb * gravb) * mass + vc * dragc + va * draga + vb * drag&
                  &b) * ex * (va ** 2 + vb ** 2 + vc ** 2) ** (-3d0 / 2d0) - 0.2D&
                  &1 * ((-ra ** 2 / 2d0 - rb ** 2 / 2d0 + rc ** 2) * pvc ** 2 + 0&
                  &.3D1 * rc * (pva * ra + rb * pvb) * pvc + (ra ** 2 - rc ** 2 / 0.2&
                  &D1 - rb ** 2 / 2d0) * pva ** 2 + 3d0 * pva * rb * pvb * ra - p&
                  &vb ** 2 * (-2d0 * rb ** 2 + ra ** 2 + rc ** 2) / 2d0) * normv& 
                  &** 2 * g0 * normr * mass ** 2 * cg ** (-0.5D1 / 2d0) - normr * norm&
                  &v ** 2 * (pra ** 2 + prc ** 2 + prb ** 2) * mass ** 2 + 0.500D3 * Kd& 
                  &* normv * ex * (rc * (pva ** 2 + pvb ** 2 + 2d0 * pvc ** 2) * vc&
                  & ** 3 + ((2d0 * ra * pvc ** 2 + 2d0 * pva * rc * pvc + ra * (p&
                  &va ** 2 + pvb ** 2)) * va + (2d0 * rb * pvc ** 2 + 2d0 * pvb *&
                  & rc * pvc + rb * (pva ** 2 + pvb ** 2)) * vb + 3d0 / 0.500D3 * n&
                  &ormr * (pra * pva + 2d0 * prc * pvc + prb * pvb)) * vc ** 2 + ((&
                  &rc * pvb ** 2 + 2d0 * ra * pva * pvc + 2d0 * rc * pva ** 2 + r&
                  &c * pvc ** 2) * va ** 2 + (((2d0 * ra * pvb + 2d0 * pva * rb)& 
                  &* pvc + 2d0 * rc * pva * pvb) * vb + 3d0 / 0.500D3 * normr * (&
                  &prc * pva + pvc * pra)) * va + (rc * pvc ** 2 + 2d0 * rb * pvb *&
                  & pvc + rc * (pva ** 2 + 2d0 * pvb ** 2)) * vb ** 2 + 3d0 / 0.5&
                  &00D3 * normr * (pvb * prc + pvc * prb) * vb - pvc * (rc * pvc + rb&
                  & * pvb + pva * ra) * normv ** 2 + (2d0 * pvc ** 2 * gravc + (gra&
                  &vb * pvb + grava * pva) * pvc + gravc * (pva ** 2 + pvb ** 2)) * n&
                  &ormr / 0.250D3) * vc + 2d0 * (pvc ** 2 / 2d0 + pvb ** 2 / 0.2D&
                  &1 + pva ** 2) * ra * va ** 3 + ((2d0 * rb * pva ** 2 + pvb ** 2& 
                  &* rb + rb * pvc ** 2 + 2d0 * pvb * ra * pva) * vb + 3d0 / 0.25&
                  &0D3 * (pra * pva + prb * pvb / 2d0 + prc * pvc / 2d0) * normr)&
                  & * va ** 2 + ((2d0 * ra * pvb ** 2 + ra * pvc ** 2 + 2d0 * rb& 
                  &* pva * pvb + pva ** 2 * ra) * vb ** 2 + 3d0 / 0.500D3 * normr *&
                  & (prb * pva + pvb * pra) * vb - pva * (rc * pvc + rb * pvb + pva *&
                  & ra) * normv ** 2 + normr * (grava * pvc ** 2 / 2d0 + pva ** 2 *&
                  & grava + pva * pvc * gravc / 2d0 + grava * pvb ** 2 / 2d0 + pv&
                  &a * pvb * gravb / 2d0) / 0.125D3) * va + vb * (0.250D3 * rb * (p&
                  &vc ** 2 + pva ** 2 + 2d0 * pvb ** 2) * vb ** 2 + 3d0 / 2d0 *&
                  & normr * (prc * pvc + 2d0 * prb * pvb + pra * pva) * vb - 0.250D&
                  &3 * pvb * (rc * pvc + rb * pvb + pva * ra) * normv ** 2 + normr *& 
                  &(gravb * pvc ** 2 + pva * pvb * grava + gravb * pva ** 2 + 2d0 *&
                  & pvb ** 2 * gravb + pvb * pvc * gravc)) / 0.250D3) * mass - 2d0 * (&
                  &Kd * ex * (0.4D1 * pvc ** 2 + pvb ** 2 + pva ** 2) * vc ** 4 + 0.6&
                  &D1 * Kd * ex * pvc * (vb * pvb + va * pva) * vc ** 3 + 0.5D1 * Kd& 
                  &* ((pva ** 2 + pvc ** 2 + 2d0 / 0.5D1 * pvb ** 2) * va ** 2 + 0.&
                  &6D1 / 0.5D1 * pva * va * vb * pvb + 2d0 / 0.5D1 * vb ** 2 * (0.5&
                  &D1 / 2d0 * pvb ** 2 + pva ** 2 + 0.5D1 / 2d0 * pvc ** 2)) * ex&
                  & * vc ** 2 + (0.6D1 * Kd * ex * pva * va ** 3 * pvc + 0.6D1 * Kd *&
                  & ex * pvb * va ** 2 * vb * pvc + 0.6D1 * Kd * ex * pva * va * vb *&
                  &* 2 * pvc + 0.6D1 * Kd * ex * pvb * vb ** 3 * pvc - (2d0 * pvc *&
                  &* 2 * dragc + (dragb * pvb + draga * pva) * pvc + dragc * (pva **& 
                  &2 + pvb ** 2)) * normv) * vc + 0.4D1 * Kd * ex * (pvb ** 2 / 0.4D1&
                  & + pvc ** 2 / 0.4D1 + pva ** 2) * va ** 4 + 0.6D1 * Kd * ex * pva& 
                  &* va ** 3 * vb * pvb + 0.5D1 * (2d0 / 0.5D1 * pvc ** 2 + pva **& 
                  &2 + pvb ** 2) * Kd * vb ** 2 * ex * va ** 2 + (0.6D1 * Kd * ex * p&
                  &va * vb ** 3 * pvb - 2d0 * (pva ** 2 * draga + pva * pvb * dragb&
                  & / 2d0 + pvb ** 2 * draga / 2d0 + pva * pvc * dragc / 2d0 +& 
                  &pvc ** 2 * draga / 2d0) * normv) * va - (-Kd * ex * (pva ** 2 +& 
                  &0.4D1 * pvb ** 2 + pvc ** 2) * vb ** 3 + normv * (pvb * pvc * drag&
                  &c + 2d0 * pvb ** 2 * dragb + pvc ** 2 * dragb + pva ** 2 * dragb&
                  & + pva * pvb * draga)) * vb) * Kd * normr * ex) / mass / normr / norm&
                  &v ** 2 / normpv + (pvb ** 2 + pva ** 2 + pvc ** 2) ** (-3d0 / 0.&
                  &2D1) * (2d0 * (pva ** 2 / 2d0 + pvc ** 2 + pvb ** 2 / 2d0) *&
                  & ex * Kd * vc ** 2 + 2d0 * ex * Kd * pvc * (vb * pvb + va * pva)&
                  & * vc + Kd * ex * (pvc ** 2 + pvb ** 2 + 2d0 * pva ** 2) * va **&
                  & 2 + 2d0 * Kd * ex * pva * va * vb * pvb + Kd * ex * (pvc ** 2 +&
                  & pva ** 2 + 2d0 * pvb ** 2) * vb ** 2 - mass * (pra * pva + prb * p&
                  &vb + prc * pvc) * normv) ** 2 / normv ** 2 / mass

             B = Tmax / mass**2 * B

             alpha = - B / A

             !Check necessary condition for local optimality
             if (A > 0 .and. outmode > 0) write(0,*) 'WARNING: Control >>> Local optimality condition not verified...',A

             !Check bounds
             if (alpha < lower(1)) then
                alpha = lower(1)
                if (outmode > 0) write(0,*) 'Singular control below lower bound !'