TH方程学习（7）

一、内容介绍

TH存在广泛应用，在下面案例中，将介绍几种相对运动模型，斜滑接近模型，本节学习斜滑接近制导方法能够对接近时间、接近方向以及自主接近过程的相对速度进行控制。施加脉冲时刻追踪器的位置连线可构成一条直线，即理想轨道，实际接近轨道和理想接近轨道在脉冲施加时刻相交。脉冲施加的次数越多，则实际轨道偏离理想轨道越少；脉冲施加次数趋近于无穷大时，实际交会轨道将会和理想轨道重合，接近一条直线，从大范围看，多脉冲斜滑接近整个过程基本是沿直线运动的。

设追踪器在接近段的初始时刻，相对运动状态为 $\boldsymbol{r}_0$ 和 $\dot{\boldsymbol{r}_0}$ ，接近段终端时刻 $T$ 的相对状态为 $\boldsymbol{r}_T$ 和 $\dot{\boldsymbol{r}}_T$ 。设追踪器沿准直线完成对目标器的接近，由 $\boldsymbol{r}_T$ 指向 $\boldsymbol{r}_0$ 的直线矢量 $\boldsymbol{\rho}$ 为规划轨迹。所以，在任意时刻 $t$ 有

$\boldsymbol{\rho}(t)=\boldsymbol{r}_c(t)-\boldsymbol{r}_T$

其中， $\boldsymbol{r}_c(t)$ 为追踪器在该直线上某点处的位置矢量。

矢量 $\boldsymbol{\rho}$ 的单位矢量为

$\boldsymbol{u}_{\rho}=[\cos\alpha,\cos\beta,\cos\gamma]^{T}$

其中， $\alpha,\beta,\gamma$ 表示矢量 $\boldsymbol{\rho}(T)$ 在VVLH坐标系中投影与三个坐标轴的夹角，从而决定了接近方向。矢量可以表示为

$\boldsymbol{\rho}=\left | \rho \right |\boldsymbol{u}_{\boldsymbol{\rho}}$

在接近过程中，可根据接近轨迹快速性和安全性等多种需求来确定 $\rho,\dot{\rho}$ 的变化关系，即设计理想的交会轨迹。典型的相对运动速度变化模式有指数型，这里采用指数型， $\rho$ 和 $\dot{\rho}$ 为线性关系

$\dot{\rho}=a\rho+\dot{\rho}_T\\\rho(t)=\rho_0\exp(at)+\frac{\dot{\rho}_T}{a}(\exp at)-1)$

上式中 $a$ 为斜率。则整个接近段的转移时间T为

$T=\frac{1}{a}\ln\frac{\dot{\rho}_T}{\dot{\rho}_0}$

设接近段采用多脉冲分段控制，作用 $N$ 次速度脉冲使绕飞卫星在时间T内从初始位置 $\boldsymbol{r}_0$ 转移到终点位置 $\boldsymbol{r}_T$ 。任意控制段的两次脉冲作用的时间间隔是相同的，即 $\Delta t=T/N$ 。

在时刻 $t_m=m\Delta t(m=0,1,..,N-1)$ ，经过第m次速度脉冲后，绕飞卫星从 $\boldsymbol{r}_m$ 转移到 $\boldsymbol{r}_{m+1}$ ，有

$\boldsymbol{r}_m=\boldsymbol{r}_T+\rho_m \boldsymbol{u}_{\rho}\\ \rho_m=\rho_0\exp(at_m)+\frac{\dot{\rho}_T}{a}(\exp(at_m)-1)$

每个阶段相当于一次双脉冲轨道转移，可通过如下编程

% 本节旨在利用TH方程实现椭圆的轨道的滑行制导
clc;clear
% 初始化条件
Ecc    =  0.1;
Perigee=  500;
TA     =  45;
N      =  5  ;    %施加脉冲次数  
r_i    =  [1;1;1];
v_i    =  [0.01;0.01;0.01];
% 期望末端条件
r_f    =  [0.1;0;0];
v_f    =  [0;0;0];
% 求出初始相对距离
rho0   =  norm(r_i-r_f);
rhof   =  0;

%设计初始和结束沿着rho方向的速度
drho0  =  -0.005;
drhof  =  -0.0001;
a            =  (drho0-drhof)/rho0;
t            =  1/a *log(drhof/drho0);
% 求出单位矢量
u_rho        =  (r_i-r_f)/rho0;


rho1_vec     =  zeros(3,N);
rho1_vec(:,1)=  r_i;
% 记录每次施加脉冲前的速度
vvff         =   zeros(3,N);
vvff(:,1)    =  v_i;
% 记录每次施加脉冲后的速度
vvrr         =  zeros(3,N);
vvrr(:,N)    =  v_f; 
delta_t      =  t/(N-1);
% 脉冲希望到达的位置
for i=1:N-1
    t1            =  i*t/(N-1);
    rho1          =  rho0*exp(a*t1)+drhof/a*(exp(a*t1)-1);
    rho1_vec(:,i+1) = r_f+rho1*u_rho;    
    [v,Phi,vv]    =  TH_solver(Ecc,Perigee,TA,rho1_vec(:,i),vvff(:,i),t/(N-1));
    Phiall{i}     =  Phi;
    Phi_rr        =  Phi(1:3,1:3);
    Phi_rv        =  Phi(1:3,4:6);
    vvrr(:,i)     =  inv(Phi_rv)*(rho1_vec(:,i+1)-Phi_rr*rho1_vec(:,i));
    [x,Phi0,xx]   =  TH_solver(Ecc,Perigee,TA,rho1_vec(:,i),vvrr(:,i),t/(N-1));
    
    yy(:,i)       =  x(1:3);
    vvff(:,i+1)   =  x(4:6);
end
dv=vvrr-vvff;
Phiall{4}*[rho1_vec(:,4);vvrr(:,4)];
tt            =  linspace(0,delta_t,1000);
for ss=1:N
    for j=1:length(tt)
        [mm,Phi00,zz0]=  TH_solver(Ecc,Perigee,TA,rho1_vec(:,ss),vvrr(:,ss),tt(j));
        tarx((ss-1)*1000+j)       =  mm(1);
        tary((ss-1)*1000+j)       =  mm(2);
        tarz((ss-1)*1000+j)       =  mm(3);
    end
end
% 使用STK验证VVLH坐标系
uiApplication = actxGetRunningServer('STK12.application');
root = uiApplication.Personality2;
checkempty = root.Children.Count;
if checkempty ~= 0
    root.CurrentScenario.Unload
    root.CloseScenario;
end
root.NewScenario('VVLH');
StartTime = '26 Jan 2024 04:00:00.000';    % 场景开始时间
StopTime = '10 Feb 2024 04:00:00.000';     % 场景结束时间
root.ExecuteCommand(['SetAnalysisTimePeriod * "',StartTime,'" "',StopTime,'"']);
root.ExecuteCommand(' Animate * Reset');
SatName = 'Target';       %  SAR_   GX_   Sat_  GX_1_  SAR_1_
satellite = root.CurrentScenario.Children.New('eSatellite', SatName);
satellite.SetPropagatorType('ePropagatorAstrogator');   %  不设置的时候默认为二体模型  ePropagatorJ4Perturbation
satellite.Propagator;
% 目标星初始状态
Perigee = 500;
T       = 60;
% 追踪星在VVLH坐下的相对位置
delta_r = [1;1;1];
delta_v = [0.01;0.01;0.01];
Perige  = 6378.137+Perigee;
ecc     = 0.1;
sma     = Perige/(1-ecc);
Inc     = 30;
w       = 0;
RAAN    = 0;
TA      = 45;
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList Initial_State Propagate']);
InitialState=satellite.Propagator.MainSequence.Item(0);
%% 初始化卫星参数
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.CoordinateType Modified Keplerian']);
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.InitialState.Epoch ',StartTime,' UTCG']);
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.InitialState.Keplerian.sma ',num2str(sma),' km']);
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.InitialState.Keplerian.ecc ',num2str(ecc)]);
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.InitialState.Keplerian.inc ',num2str(Inc),' deg']);
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.InitialState.Keplerian.w ',num2str(w),' deg']);
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.InitialState.Keplerian.RAAN ',num2str(RAAN),' deg']);
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' SetValue MainSequence.SegmentList.Initial_State.InitialState.Keplerian.TA ',num2str(TA),' deg']);
%% 二体传播
Propagate=satellite.Propagator.MainSequence.Item(1);
Propagate.PropagatorName='Earth Point Mass';
root.ExecuteCommand(['Astrogator */Satellite/',SatName,' RunMCS']);

% 插入目标星
SatName2 = 'Chaser';       
satellite2 = root.CurrentScenario.Children.New('eSatellite', SatName2);
satellite2.SetPropagatorType('ePropagatorAstrogator');   %  不设置的时候默认为二体模型  ePropagatorJ4Perturbation
satellite2.Propagator;
InitialState2=satellite2.Propagator.MainSequence.Item(0);
InitialState2.CoordSystemName='Satellite/Target VVLH';
InitialState2.Element.X=delta_r(1);
InitialState2.Element.Y=delta_r(2);
InitialState2.Element.Z=delta_r(3);
InitialState2.Element.Vx=delta_v(1);
InitialState2.Element.Vy=delta_v(2);
InitialState2.Element.Vz=delta_v(3);
Propagate2=satellite2.Propagator.MainSequence.Item(1);
Propagate2.PropagatorName='Earth Point Mass';
for j=1:N
    ManeuverName=['Maneuver',num2str(j)];
    PropagateName=['Propagate',num2str(j)];
    satellite2.Propagator.MainSequence.Insert('eVASegmentTypeManeuver',ManeuverName,'Propagate');
    Maneuver=satellite2.Propagator.MainSequence.Item(ManeuverName);
    root.ExecuteCommand(['Astrogator */Satellite/',SatName2,' SetValue MainSequence.SegmentList.',ManeuverName,'.ImpulsiveMnvr.AttitudeControl Thrust Vector']);
    Maneuver.Maneuver.AttitudeControl.ThrustAxesName='Satellite VVLH.Axes';
    Maneuver.Maneuver.AttitudeControl.X=dv(1,j)*1000;
    Maneuver.Maneuver.AttitudeControl.Y=dv(2,j)*1000;
    Maneuver.Maneuver.AttitudeControl.Z=dv(3,j)*1000;
    
    satellite2.Propagator.MainSequence.Insert('eVASegmentTypePropagate',PropagateName,'Propagate');
    Propagate3=satellite2.Propagator.MainSequence.Item(PropagateName);
    Propagate3.PropagatorName='Earth Point Mass';
    Propagate3.Properties.Color=255;
    Propagate3.StoppingConditions.Item(0).Properties.Trip = delta_t;
end
satellite2.Propagator.MainSequence.Cut('Propagate')
root.ExecuteCommand(['Astrogator */Satellite/',SatName2,' RunMCS']);
% 报告二颗卫星的三维关系
satellite.VO.OrbitSystems.InertialByWindow.IsVisible=0;
satellite2.VO.OrbitSystems.InertialByWindow.IsVisible=0;
satellite2.VO.OrbitSystems.Add('Satellite/Target VVLH System')
satellite.VO.Vector.RefCrdns.Item(2).Visible=1;

targetdata=root.ExecuteCommand(['Report_RM */Satellite/Target  Style "VVLH" TimePeriod "26 Jan 2024 04:00:00.000" "26 Jan 2024 4:30:00.000" TimeStep 1']);
Num=targetdata.Count;
root.ExecuteCommand('Astrogator */Satellite/Target ClearDWCGraphics');
root.ExecuteCommand('Astrogator */Satellite/Chaser ClearDWCGraphics');
for j=1:Num-2
    struct=regexp(targetdata.Item(j),',','split');
    Tar_x(j)=str2double(struct{2});
    Tar_y(j)=str2double(struct{3});
    Tar_z(j)=str2double(struct{4});
end

figure(1)
plot3(Tar_x(1:floor(t)),Tar_y(1:floor(t)),Tar_z(1:floor(t)),'LineWidth',1);
hold on
plot3(tarx,tary,tarz,'LineWidth',1)
axis([-1.5 1.5 -1.5 1.5 -1.5 1.5])
set(gca,'XDir','reverse');
set(gca,'YDir','reverse');
set(gca,'ZDir','reverse');
xlabel('X axis(km)','FontName','Times New Roman')
ylabel('Y axis(km)','FontName','Times New Roman')
zlabel('Z axis(km)','FontName','Times New Roman')
title('e=0.1,Perigee=500km','FontName','Times New Roman')
grid on

plot3(rho1_vec(1,:),rho1_vec(2,:),rho1_vec(3,:),'g.')
plot3(delta_r(1),delta_r(2),delta_r(3),'r.')

legend('transfer trajectory(STK)','trasnfer trajecoty(TH)','Impulsive Point','Original','Location','Northeast')

得到最终的结果，滑移的曲线如图所示