orbitalfuns.f90

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

!*************************************
! Simplicial package v1.3            *
! Functions for 3d orbital transfer  *
! Author: Pierre Martinon            *
! Original formulation from the code *
! mfmax v1.0 by Thomas Haberkorn     *
! ENSEEIHT-IRIT, UMR CNRS 5505       *
! Date: 09/2006                      *
!*************************************

! Three different formulations are used, according to the "longitude" flag

! Time formulation (longitude = 0)
!---------------------------------
! State (dim 8): [ Pa  ex  ey  hx  hy  L  m  tf ]
! Costate (id):  [ pPa pex pey phx phy pL pm ptf]
! dynamics according to Hamiltonian system
! autonomous formulation (independant variable is t)
! IVP unknown z: [ tf  Ppa  Pex  Pey  Phx  Phy  PL  Pm ]
!                   8   9    10   11   12   13  14  15

! Longitude formulation (longitude = 1)
!--------------------------------------
! State (dim 8): [ Pa  ex  ey  hx  hy *s* m  tf ]       s: reduced time in [0,1]
! Costate (id):  [ pPa pex pey phx phy pL pm ptf]
! variable change: x(6) <- L inside Control and Dynamics
! to compute dynamics according to "time" Hamiltonian cf above
! non autonomous formulation (independant variable is L)
! IVP unknown z: [ tf  Ppa  Pex  Pey  Phx  Phy  PL  Pm ] 
!                   8   9    10   11   12   13  14  15

! For these two formulations, final longitude is fixed and final time free
! Final time is then fixed by usual transformation t = tf . s, s in [0,1]
! (slight optimisation in Dynamics: multiplication by tf is done later in PostProcess)
! (in order to avoid multiplying then dividing by tf in longitude formulation) 

! Full longitude formulation (longitude = 2)
!-------------------------------------------
! State (dim 8): [ Pa  ex  ey  hx  hy  L  m] tf s       s: reduced time in [0,1]
! Costate (id):  [ pPa pex pey phx phy pL pm] ptf ps
! dynamics according to LONGITUDE Hamiltonian system
! optimal control is obtained via numerical minimisation of H
! autonomous formulation (independant variable is L)
! IVP unknown z: tf [Ppa  Pex  Pey  Phx  Phy  PL  Pm ] Ps
!                 9   10   11   12   13   14  15  16   18
! Final longitude is fixed, final time is ignored as time is not part of state
! (time can still be integrated as second component in subroutine Objective)


Module auxmod
  implicit none

  real (kind=8), save :: co, si, Z, A, B, XX, H, normu
  real (kind=8), save :: Pmu0, Pmu0Z, Pmu03, Hu3

end Module auxmod


Module SpecFuns
  use shootdefs
  implicit none

  integer, save :: longitude
  real(kind=8), save :: beta, mu0, Tmax, Hamilton0

contains

  Subroutine InitPar(mode,z,lambda)
    implicit none

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

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

    if (lambda < 0d0) write(0,*) 'NOTE: InitPar >>> lambda is negative...',lambda

    if (mode == 0) then

       !set some parameters
       beta = par(3)
       mu0 = par(4)
       Tmax = par(1) * par(5)
       longitude = 1
       Hamilton0 = 666d0
       if (npar > 5) longitude = int(par(6))

       !choose time/longitude formulation
       select case (longitude)

       case (0)
          !time formulation
          !set correct initial and final longitude values
          ci(6) = t0
          cf(6) = (tf0 - t0) * par(2) + t0
          !set correct initial and final time (reduced time s in [0,1])
          t0 = 0d0
          tf = 1d0
          write(0,*) 'TIME FORMULATION, L0, Lf, t0, tf:',ci(6),cf(6),t0,tf
          write(0,*) 'check new fixed times structure'
          stop
          
       case (1)
          !default longitude formulation
          !apply multiplicative coefficient to final longitude
          tf = (tf0 - t0) * par(2) + t0
          fixedtimes(2) = tf

       case (2)
          !full longitude formulation
          !apply multiplicative coefficient to final longitude
          tf = (tf0 - t0) * par(2) + t0
          !set correct initial and final longitude values
          ci(6) = t0
          cf(6) = tf0
          write(0,*) 'FULL LONGITUDE FORMULATION'
          write(0,*) 'check new fixed times structure'
          stop
          
       case default
          write(0,*) 'ERROR: InitPar >>> Unknown value for longitude...',longitude
          stop
          
       end select

       !comment ca marchait sans ca avant ??
       tff = max(z(1) / zscal(1),0d0)




       !save initial and final conditions
       cf0 = cf
       ci0 = ci


    else
       
       if (lambda < 1d0 .or. ntry == 0) then
          discshoot = 0
       else
          discshoot = 1
       end if


    end if

  end subroutine InitPar



  !********************************
  ! Set some auxilliary variables *
  !********************************
  Subroutine AuxVars(x,p,u)
    use auxmod
    implicit none

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

    co = cos(x(6))
    si = sin(x(6))
    Z = 1.d0 + x(2)*co + x(3)*si
    A = x(2) + (1.d0+Z)*co
    B = x(3) + (1.d0+Z)*si
    XX = 1.d0 + x(4)**2+x(5)**2
    H = x(4)*si-x(5)*co
    
    Pmu0 = sqrt(x(1)/mu0)/x(7)
    Hu3 = H*u(3)
    Pmu0Z = Pmu0/Z*Tmax
    Pmu03 = sqrt(mu0/x(1)**3)*Z**2

    normu = sqrt(u(1)**2+u(2)**2+u(3)**2)

  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) :: comp(m), norm_comp, coeff, coeffc

    !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


    !switch value and successive derivatives
    norm_comp = 0d0
    call AuxVars(x,p,u)

    comp(1) = Tmax*(p(2)*Pmu0*si - p(3)*Pmu0*co)
    comp(2) = Tmax/Z*(2.d0*p(1)/x(7)*sqrt(x(1)**3/mu0)+p(2)*Pmu0*A + p(3)*Pmu0*B)
    comp(3) = Tmax/Z*(-Pmu0*p(2)*x(3)*H + Pmu0*p(3)*x(2)*H+p(4)*0.5d0*Pmu0*XX*co &
         &         + p(5)*0.5d0*Pmu0*XX*si + p(6)*Pmu0*H)

    norm_comp = sqrt(comp(1)**2 + comp(2)**2 + comp(3)**2)

    coeff = lambda - Beta*Tmax*p(7)
    coeffc = 2.d0*(1.d0-lambda)


    if (lambda < 1d0) then

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

    else !lambda = 1

       select case (controlmode)

       case (0)
          !usual control

          if ((norm_comp <= coeff).or.(norm_comp < macheps)) then
             u(:) = 0.d0  
          else
             u(:) = - comp(:)/norm_comp
          end if

       case (1)
          !bang-bang control

          select case (switchflag)
          case (0)
             !initialisation a t0
             if ((norm_comp <= coeff).or.(norm_comp < macheps)) then
                u(:) = 0.d0  
                switchflag = 1
             else
                u(:) = - comp(:)/norm_comp
                switchflag = -1
             end if
          case (1) 
             u(:) = 0.d0
          case (-1)
             u(:) = - comp(:)/norm_comp
          case default 
             write (0,*) 'ERROR: Control >>> Unknown value for switchflag...',switchflag
             stop
          end select


       case default
          write(0,*) 'ERROR: Control >>> Unknown homotop...',homotop
          stop

       end select !controlmode

    end if !lambda


  end subroutine Control


  Subroutine Switch(lambda,x,p,psi)
    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) :: psi

    !Local
    real(kind=8) :: norm_comp
    real(kind=8) :: comp(m), u(m)

    u = 0d0
    call AuxVars(x,p,u)

    comp(1) = Tmax*(p(2)*Pmu0*si - p(3)*Pmu0*co)
    comp(2) = Tmax/Z*(2.d0*p(1)/x(7)*sqrt(x(1)**3/mu0)+p(2)*Pmu0*A + p(3)*Pmu0*B)
    comp(3) = Tmax/Z*(-Pmu0*p(2)*x(3)*H + Pmu0*p(3)*x(2)*H+p(4)*0.5d0*Pmu0*XX*co &
         &         + p(5)*0.5d0*Pmu0*XX*si + p(6)*Pmu0*H)

    norm_comp = sqrt(comp(1)**2 + comp(2)**2 + comp(3)**2)

    psi = 1d0 - Beta*Tmax*p(7) - norm_comp


  end Subroutine Switch


  Subroutine Switchdot(lambda,x,p,u,psidot)
    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(in), dimension(m) :: u
    real(kind=8), intent(out) :: psidot

    psidot = 0d0
    write(0,*) 'Please complete switchdot...'
    stop
    
  end Subroutine Switchdot


  Subroutine SwitchGradient(lambda,y,gradpsi)
    implicit none
    real(kind=8), intent(in) :: lambda      
    real(kind=8), intent(in), dimension(ns+nc) :: y         
    real(kind=8), intent(out), dimension(ns+nc) :: gradpsi  
 
    !out init
    gradpsi = 0d0
    write(0,*) 'Please complete switchgradient...'
    stop

  end Subroutine SwitchGradient



  Subroutine InitAltcontrol(lambda,alpha,x,p,u)
    implicit none   
    real(kind=8), intent(in) :: lambda      
    real(kind=8), intent(inout) :: alpha  
    real(kind=8), intent(in), dimension(ns) :: x         
    real(kind=8), intent(in), dimension(nc) :: p 
    real(kind=8), intent(inout), dimension(m) :: u  

    !initialize control and alpha
    alpha = (lower(1) + upper(1)) / 2d0


  end Subroutine InitAltcontrol


  Subroutine UpdateAltcontrol(lambda,newalpha,x,p,u)
    implicit none   
    real(kind=8), intent(in) :: lambda      
    real(kind=8), intent(in) :: newalpha  
    real(kind=8), intent(in), dimension(ns) :: x         
    real(kind=8), intent(in), dimension(nc) :: p 
    real(kind=8), intent(inout), dimension(m) :: u  

    !update control with alpha
    u(1) = newalpha

  end Subroutine UpdateAltcontrol



  !*************************
  ! Compute state dynamics *
  !*************************
  Subroutine StateDynamics(dimphi,lambda,x,p,u,f)
    use auxmod
    implicit none

    integer, intent(in) :: dimphi
    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(in), dimension(m) :: u
    real(kind=8), intent(out), dimension(dimphi) :: f

    !out init
    f = 0d0

    !Dynamics of state wrt time t: dxdt
    f(1) = 2.d0*Pmu0Z*x(1)*u(2)                 !Pa
    f(2) = Pmu0Z*(Z*si*u(1)+A*u(2)-x(3)*Hu3)    !ex
    f(3) = Pmu0Z*(-Z*co*u(1)+B*u(2)+x(2)*Hu3)   !ey
    f(4) = 0.5d0*Pmu0Z*XX*co*u(3)               !hx
    f(5) = 0.5d0*Pmu0Z*XX*si*u(3)               !hy
    f(6) = (Pmu03+Pmu0Z*Hu3)                    !L
    f(7) = -Beta*Tmax*normu                     !m
    !if (longitude < 2) f(8) = 0.d0              !tf
    if (ns > 7) f(8) = 0d0                      !tf
    if (ns > 8) f(9) = 1d0 / x(8)               !s