[PSOC-12] 예제 : 램버트 문제 (Lambert’s problem)

램버트 문제(Lambert's problem)는 이체문제(two-body problem)에서 유도된 기본 궤도 미분 방정식에 대한 2점 경계값 문제(TPBVP, two-point boundary value problem)이다.

 

\[ \begin{align} & \frac{d^2 \mathbf{r}}{dt^2 }+ \frac{ \mu}{ (\sqrt{\mathbf{r} \cdot \mathbf{r} })^3} \mathbf{r}=0 \tag{1} \\ \\ & \mathbf{r}(t_0 )= \mathbf{r}_0, \ \ \ \mathbf{r}(t_f )=\mathbf{r}_f, \ \ \ t_0, \ t_f \ \mbox{given} \end{align} \]

 

여기서 \(\mu\) 는 중력 파라미터, \(\mathbf{r}\) 은 관성좌표계의 원점에서 질점까지의 위치벡터다.

 

 

램버트 문제의 솔루션은 여러가지가 제안되어 있지만 여기서는 램버트 문제를 최적제어 문제로 변환시킨 후 로바토 유사 스펙트럴 방법(LPM)으로 풀어보고자 한다 (https://pasus.tistory.com/282).

 

 

램버트 문제를 최적제어 문제로 변환하면 다음과 같다 (https://pasus.tistory.com/268).

 

\[ \begin{align} \arg \min_{\mathbf{v}} J &= \int_{t_0}^{t_f} \left( \frac{1}{2} \mathbf{v} \cdot \mathbf{v}+ \frac{\mu}{ \sqrt{ \mathbf{r} \cdot \mathbf{r} } } \right) \ dt \tag{2} \\ \\ \mbox{subject to :} \ \ & \frac{d \mathbf{r}}{dt} = \mathbf{v} \tag{3} \\ \\ & \mathbf{r}(t_0 ) =\mathbf{r}_0, \ \ \ \mathbf{r}(t_f )= \mathbf{r}_f \tag{4} \end{align} \]

 

이 예제에서는 다음 수치를 사용한다.

 

\[ \begin{align} & \mathbf{r}_0= \begin{bmatrix} 5,000 & 10,000 & 2,100 \end{bmatrix}^T \ km \\ \\ & \mathbf{r}_f= \begin{bmatrix} -14,600 & 2,500 & 7,000 \end{bmatrix}^T \ km \\ \\ & t_f-t_0=3,600 \ sec⁡ \ \ (1 \ hr) \\ \\ & \mu =398,600 \ km^3 / s^2 \\ \\ & R= 6,378 \ km \ \ \mbox{(Earth radius)} \end{align} \]

 

그러면 구하고자 하는 램버트 문제의 정답은 다음과 같다.

 

\[ \begin{align} & \mathbf{v}_0= \begin{bmatrix} -5.99249 & 1.92536 & 3.24564 \end{bmatrix}^T \ km/s \\ \\ & \mathbf{v}_f= \begin{bmatrix} -3.31246 & -4.19662 & -0.38529 \end{bmatrix}^T \ km/s \end{align} \]

 

과연 어느 정도의 정확도로 LPM이 답을 계산하는지 알아보도록 하자.

먼저 본 예제에서 사용하는 거리와 속도, 시간의 단위를 그대로 최적화에 사용하기에는 수치의 범위가 너무 크므로 천문단위를 이용하여 무차원화 시킨다 (https://pasus.tistory.com/183).

 

% parameters
mu_orig=398600; % mu=GM
DU=6378;
TU=DU^(3/2)/sqrt(mu_orig);
DUTU=DU/TU;

mu=1;
t0=0;

% problem formulation
tf_orig=60*60;

rr0_0=[5000 10000  2100]';
rrf_0=[-14600  2500  7000]';

rr0 = rr0_0/DU;
rrf = rrf_0/DU;
tf = tf_orig/TU;

 

여기서는 \(N=12\) 개의 LGL 콜로케이션 포인트를 사용한다. LGL 콜로케이션 포인트와 보간점, LGL 쿼드래처 포인트의 가중치(weighting) \(w_j\), 미분행렬 \(D_{ji}\) 를 다음과 같이 계산한다.

 

% parameters for pseudospectral
N=12;  % number of collocation points
points=GaussPoints(N,'LGL'); % LGL collocation points (N)
col_point=points';
int_point=col_point; % interpolating points (N)
Dji=LagrangePolyDerivative(col_point,int_point); % Differentiation matrix
gweights=GaussianQuadrature(N,'LGL'); % LGL quadrature weights

 

LPM에서는 상태변수 \(\mathbf{r}(\tau)\) 와 제어변수 \(\mathbf{v}(\tau)\) 를 \((N-1)\) 차 라그랑지 다항식으로 근사화한다.

 

\[ \begin{align} & \mathbf{r}( \tau ) \approx \sum_{i=1}^N \mathbf{R}_i L_i (\tau) \\ \\ & \mathbf{v} (\tau) \approx \sum_{i=1}^N \mathbf{V}_i L_i (\tau) \end{align} \]

 

목적함수 (2)는 다음과 같이 \(N\) 개의 LGL 포인트를 이용한 가우시안 쿼드래처로 근사화 된다.

 

\[ J= \frac{(t_f-t_0 )}{2} \sum_{j=1}^N w_j \left( \frac{1}{2} \mathbf{V}_j^T \mathbf{V}_j+ \frac{ \mu}{ \sqrt{ \mathbf{R}_j^T \mathbf{R}_j } } \right) \]

 

rsum=0;
for ii=1:N
    rsum=rsum+ww(ii)/sqrt(rr(ii,1)*rr(ii,1)+ ...
         rr(ii,2)*rr(ii,2)+rr(ii,3)*rr(ii,3)+ep);
end

vsum=(ww.*vv(:,1))'*vv(:,1)+(ww.*vv(:,2))'*vv(:,2)+(ww.*vv(:,3))'*vv(:,3);
rcost = (tf-t0)/2*((0.5*vsum)+mu*rsum);

 

식 (4)의 경계조건은 다음과 같다.

 

\[ \begin{align} & \mathbf{R}_1-\mathbf{r}_0=0 \\ \\ & \mathbf{R}_N- \mathbf{r}_f=0 \end{align} \]

 

% equality constraints
ceq = [rr(1,1)-param.rr0(1); 
       rr(1,2)-param.rr0(2); 
       rr(1,3)-param.rr0(3); 
       rr(Nitp,1)-param.rrf(1); 
       rr(Nitp,2)-param.rrf(2); 
       rr(Nitp,3)-param.rrf(3)  ];

 

식 (3)의 운동 미분방정식은 LGL 콜로케이션 포인트에 배치시켜 다음 식을 만족시키도록 한다.

 

\[ \begin{bmatrix} D_{11} & \cdots & D_{1N} \\ \vdots & \ddots & \vdots \\ D_{N1} & \cdots & D_{NN} \end{bmatrix} \begin{bmatrix} \mathbf{R}_1^T \\ \vdots \\ \mathbf{R}_N^T \end{bmatrix} = \frac{(t_f-t_0 )}{2} \begin{bmatrix} \mathbf{V}_1^T \\ \vdots \\ \mathbf{V}_N^T \end{bmatrix} \]

 

% dynamic constraints  rr_dot=vv
F = vv;

for n=1:3 % state number
    ceq=[ceq;
         Dji*rr(:,n)-(tf-t0)/2*F(:,n)];
end

 

최적화 대상 변수는 다음과 같다.

 

\[ ( \mathbf{R}_1, \mathbf{R}_2, ... , \mathbf{R}_N ), \ (\mathbf{V}_1, \mathbf{V}_2, ... , \mathbf{V}_N ) \]

 

pinput_0 = [rr1_0; rr2_0; rr3_0; vv1_0; vv2_0; vv3_0];

 

 

 

이제 매트랩의 비선형 최적화 문제 솔버인 'fmincon' 함수를 이용하여 위 문제를 풀면 된다.

 

% Using MATLAB nonlinear optimization solved 'fmincon' 

Aineq = []; Bineq = []; % no linear inequality constraints
Aeq = []; Beq = []; % no equality constraints

LBx=-Inf*ones(3*Nitp,1);
UBx=Inf*ones(3*Nitp,1);

LBu=-Inf*ones(3*N,1);
UBu=Inf*ones(3*N,1);

LB = [LBx; LBu];  % lower bound on the unknown variables
UB = [UBx; UBu]; % upper bound on the unknown variables

options = optimset('display','iter','diffmaxchange',1.1*1e-7,'diffminchange',1e-7,'MaxFunEvals',50000);

[presult,optfval] = ...
    fmincon(@ExLambert_L_cost,pinput_0,Aineq,Bineq,Aeq,Beq,LB,UB,@ExLambert_L_cons,options,param);

 

 

결과는 다음과 같다.

 

\[ \begin{align} & \mathbf{V}_1= \begin{bmatrix} -5.99248 & 1.92535 & 3.24562 \end{bmatrix}^T \ km/s \\ \\ & \mathbf{V}_N= \begin{bmatrix} -3.31249 & -4.19662 & -0.38531 \end{bmatrix}^T \ km/s \end{align} \]

 

초기속도 오차의 크기는 \(0.0256 \ m/s\), 최종속도 오차의 크기는 \(0.0285 \ m/s\) 로서 해가 정확함을 알 수 있다.

아래 그림은 궤도와 위치벡터, 속도벡터, 에너지의 궤적을 그린 것이다. 에너지가 일정하게 나오므로 LPM 으로 푼 램버트 문제의 해가 안정적임을 보여준다.

 

 

전체 매트랩 코드는 다음과 같다.

 

 

 

ExLambert_L_main.m

 

%
% Standard Lambert problem
%
% main
% using LGL pseudospectral method
% J= Lambert 
%
% Needs GaussPoints.m  LagrangePolyDerivative.m  GaussianQuadrature.m
%
% coded by st.watermelon
%

clear all; 
close all; % starting with a clean slate

% parameters
mu_orig=398600; % mu=GM
DU=6378;
TU=DU^(3/2)/sqrt(mu_orig);
DUTU=DU/TU;

mu=1;
t0=0;

% problem formulation
tf_orig=60*60;

rr0_0=[5000 10000  2100]';
rrf_0=[-14600  2500  7000]';

%----------- ref value -----------------
v0_orig = [-5.992494639666394 1.925363415280893 3.245636528490489]'; 
vf_orig = [-3.312460310936790 -4.196617307926468 -0.385287617068105]';
E_0 =(vf_orig'*vf_orig)/2-mu_orig/(sqrt(rrf_0'*rrf_0)); % specified energy
%----------- ref value -----------------

rr0=rr0_0/DU;
rrf = rrf_0/DU;
tf = tf_orig/TU;

% parameters for pseudospectral
N=12;  % number of collocation points
points=GaussPoints(N,'LGL'); % LGL collocation points (N)
col_point=points';
int_point=col_point; % interpolating points (N)
Dji=LagrangePolyDerivative(col_point,int_point); % Differentiation matrix
gweights=GaussianQuadrature(N,'LGL'); % LGL quadrature weights

% collecting parameters into param
param.Dji=Dji;
param.gweights=gweights;
param.N=N;
param.rr0=rr0;
param.rrf=rrf;
param.mu=mu;
param.t0=t0;
param.tf=tf;

% initial guesses for optimization variables (X and U)
Nitp=N;
rr1_0=zeros(Nitp,1); rr1_0(1)=rr0(1);
rr2_0=zeros(Nitp,1); rr2_0(1)=rr0(2);
rr3_0=zeros(Nitp,1); rr3_0(1)=rr0(3);

vv1_0=zeros(N,1); 
vv2_0=zeros(N,1); 
vv3_0=zeros(N,1); 

tf0=10;

% arrange initial guessed for the unknowns into a single vector
pinput_0 = [rr1_0; rr2_0; rr3_0; vv1_0; vv2_0; vv3_0];


% Using MATLAB nonlinear optimization solved 'fmincon' 

Aineq = []; Bineq = []; % no linear inequality constraints
Aeq = []; Beq = []; % no equality constraints

LBx=-Inf*ones(3*Nitp,1);
UBx=Inf*ones(3*Nitp,1);

LBu=-Inf*ones(3*N,1);
UBu=Inf*ones(3*N,1);

LB = [LBx; LBu];  % lower bound on the unknown variables
UB = [UBx; UBu]; % upper bound on the unknown variables

options = optimset('display','iter','diffmaxchange',1.1*1e-7,'diffminchange',1e-7,'MaxFunEvals',50000);

[presult,optfval] = ...
    fmincon(@ExLambert_L_cost,pinput_0,Aineq,Bineq,Aeq,Beq,LB,UB,@ExLambert_L_cons,options,param);

% results
rr1_res=presult(1:Nitp);
rr2_res=presult(Nitp+1:2*Nitp);
rr3_res=presult(2*Nitp+1:3*Nitp);

vv1_res=presult(3*Nitp+1:4*Nitp);
vv2_res=presult(4*Nitp+1:5*Nitp);
vv3_res=presult(5*Nitp+1:6*Nitp);


% Lagrange interpolating polynomials
tau=linspace(-1,1,300)';
dataset_rr1=[int_point' rr1_res];
dataset_rr2=[int_point' rr2_res];
dataset_rr3=[int_point' rr3_res];
dataset_vv1=[col_point' vv1_res]; % no u at 1 (final point)
dataset_vv2=[col_point' vv2_res]; 
dataset_vv3=[col_point' vv3_res]; 

p_rr1=LagrangePoly(dataset_rr1,tau);
p_rr2=LagrangePoly(dataset_rr2,tau);
p_rr3=LagrangePoly(dataset_rr3,tau);
p_vv1=LagrangePoly(dataset_vv1,tau);
p_vv2=LagrangePoly(dataset_vv2,tau);
p_vv3=LagrangePoly(dataset_vv3,tau);

tt=((tf-t0)*tau+(tf+t0))/2*TU; % convert to t from tau

% plotting
figure,plot(((tf-t0)*dataset_rr1(:,1)+(tf+t0))/2*TU,dataset_rr1(:,2)*DU,'ro', ...
            ((tf-t0)*dataset_rr2(:,1)+(tf+t0))/2*TU,dataset_rr2(:,2)*DU,'go', ...
            ((tf-t0)*dataset_rr3(:,1)+(tf+t0))/2*TU,dataset_rr3(:,2)*DU,'bo',...
            tt,p_rr1*DU,'r', tt,p_rr2*DU,'g', tt,p_rr3*DU,'b')
legend('r1','r2','r3')
grid

figure,plot(((tf-t0)*dataset_vv1(:,1)+(tf+t0))/2*TU,dataset_vv1(:,2)*DUTU,'ro', ...
            ((tf-t0)*dataset_vv2(:,1)+(tf+t0))/2*TU,dataset_vv2(:,2)*DUTU,'go', ...
            ((tf-t0)*dataset_vv3(:,1)+(tf+t0))/2*TU,dataset_vv3(:,2)*DUTU,'bo', ...
            tt,p_vv1*DUTU,'r',tt,p_vv2*DUTU,'g', tt,p_vv3*DUTU,'b')
legend('v1','v2','v3')
grid
        
figure, plot3(dataset_rr1(:,2)*DU,dataset_rr2(:,2)*DU,dataset_rr3(:,2)*DU,'ro',...
              p_rr1*DU,p_rr2*DU,p_rr3*DU, 'r', 0,0,0,'go', ...
              dataset_rr1(1,2)*DU,dataset_rr2(1,2)*DU,dataset_rr3(1,2)*DU,'b*');
          xlabel('x');ylabel('y');zlabel('z'); grid

          
          
Ener = ((dataset_vv1(:,2)).^2+(dataset_vv2(:,2)).^2 + ...
          (dataset_vv3(:,2)).^2).*(DUTU)^2./2 -  ...
          mu_orig./(DU.*sqrt(((dataset_rr1(1:N,2)).^2+(dataset_rr2(1:N,2)).^2 + ...
          (dataset_rr3(1:N,2)).^2)));
figure, plot(((tf-t0)*dataset_vv1(:,1)+(tf+t0))/2*TU, Ener,'bo');
          xlabel('time(sec)');ylabel('Ener'); grid
          
format long

disp('ref vs computed initial velocity......................');
VV0=[dataset_vv1(1,2) dataset_vv2(1,2) dataset_vv3(1,2)]'*DUTU;
[v0_orig VV0]
disp('ref vf vs computed vf......................');
VVf = [dataset_vv1(end,2); dataset_vv2(end,2); dataset_vv3(end,2)]*DUTU;
[vf_orig VVf]
disp('errors of initial and final velocity......................');
norm(v0_orig-VV0)
norm(vf_orig-VVf)
disp('ref Energy vs computed Energy......................');
RR0=[dataset_rr1(1,2) dataset_rr2(1,2) dataset_rr3(1,2)]*DU;
[E_0 (norm(VV0))^2/2-mu_orig/norm(RR0)]

 

ExLambert_L_cost.m

 

%
% Standard Lambert problem
%
% Cost function
%
% coded by st.watermelon
%

function rcost = ExLambert_L_cost(pinput,param)

ww=param.gweights;
N=param.N;
mu=param.mu;
t0=param.t0;
tf=param.tf;

Nitp=N;

ep=1e-10;

rr(:,1)=pinput(1:Nitp);
rr(:,2)=pinput(Nitp+1:2*Nitp);
rr(:,3)=pinput(2*Nitp+1:3*Nitp);

vv(:,1)=pinput(3*Nitp+1:4*Nitp);
vv(:,2)=pinput(4*Nitp+1:5*Nitp);
vv(:,3)=pinput(5*Nitp+1:6*Nitp);

rsum=0;
for ii=1:N
    rsum=rsum+ww(ii)/sqrt(rr(ii,1)*rr(ii,1)+ ...
         rr(ii,2)*rr(ii,2)+rr(ii,3)*rr(ii,3)+ep);
end

vsum=(ww.*vv(:,1))'*vv(:,1)+(ww.*vv(:,2))'*vv(:,2)+(ww.*vv(:,3))'*vv(:,3);
rcost = (tf-t0)/2*((0.5*vsum)+mu*rsum);

 

ExLambert_L_cons.m

 

%
% Standard Lambert problem
%
% Constraints
%
% coded by st.watermelon
%

function [cineq,ceq] = ExLambert_L_cons(pinput,param)
%
% constraints
%
N=param.N;
Dji=param.Dji;
t0=param.t0;
tf=param.tf;

Nitp=N;

rr(:,1)=pinput(1:Nitp);
rr(:,2)=pinput(Nitp+1:2*Nitp);
rr(:,3)=pinput(2*Nitp+1:3*Nitp);

vv(:,1)=pinput(3*Nitp+1:4*Nitp);
vv(:,2)=pinput(4*Nitp+1:5*Nitp);
vv(:,3)=pinput(5*Nitp+1:6*Nitp);


% inequality constraints
cineq = []; 

% dynamic constraints  rr_dot=vv
F = vv;
      
% equality constraints
ceq = [rr(1,1)-param.rr0(1); 
       rr(1,2)-param.rr0(2); 
       rr(1,3)-param.rr0(3); 
       rr(Nitp,1)-param.rrf(1); 
       rr(Nitp,2)-param.rrf(2); 
       rr(Nitp,3)-param.rrf(3)  ];
      

for n=1:3 % state number
    ceq=[ceq;
         Dji*rr(:,n)-(tf-t0)/2*F(:,n)];
end