MATLAB计算卫星相对位置、速度和加速度

MATLAB计算卫星相对位置、速度和加速度

问题描述：
给定两个卫星的轨道根数，计算出追踪卫星B (chase satellite) 相对目标卫星A (target satellite) 的位置、速度和加速度。相对运动的参考坐标系(xyz)附在目标卫星A上，见下图。

计算步骤：

1. 根据给定的轨道要素，计算卫星A和卫星B的绝对位置矢量和绝对速度矢量（参考系为赤惯系）, ${\mathbf{r}}_{\mathbf{A}},{\mathbf{r}}_{\mathbf{B}},{\mathbf{v}}_{\mathbf{A}},{\mathbf{v}}_{\mathbf{B}}$。
2. 计算两个卫星的加速度矢量。$${\mathbf{a}}_{\mathbf{A}}=-\mu \frac{{\mathbf{r}}_{\mathbf{A}}}{||{\mathbf{r}}_{\mathbf{A}}|{|}^3}$$
   $${\mathbf{a}}_{\mathbf{B}}=-\mu \frac{{\mathbf{r}}_{\mathbf{B}}}{||{\mathbf{r}}_{\mathbf{B}}|{|}^3}$$
3. 计算相对参考坐标的单位矢量(unit vector)。
   $$\hat{\mathbf{i}}=\frac{{\mathbf{r}}_{\mathbf{A}}}{r_A}$$
   $$\hat{\mathbf{k}}=\frac{{\mathbf{r}}_{\mathbf{A}}\times {\mathbf{v}}_{\mathbf{A}}}{||{\mathbf{r}}_{\mathbf{A}}\times {\mathbf{v}}_{\mathbf{A}}||}$$
   $$\hat{\mathbf{j}}=\hat{\mathbf{k}}\times \hat{\mathbf{i}}$$
4. 计算相对参考坐标系的角速度。
   $$\mathbf{\Omega }=\frac{{\mathbf{r}}_{\mathbf{A}}\times {\mathbf{v}}_{\mathbf{A}}}{{r_A}^2}=\frac{{\mathbf{h}}_{\mathbf{A}}}{{r_A}^2}$$
5. 计算相对参考坐标系的角加速度。
   $$\dot{\mathbf{\Omega }}=-\frac{2({\mathbf{r}}_{\mathbf{A}}\cdot {\mathbf{v}}_{\mathbf{A}})}{{r_A}^2}\mathbf{\Omega }$$
6. 计算卫星的相对位置（相对于赤惯系）。
   $${\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}={\mathbf{r}}_{\mathbf{B}}-{\mathbf{r}}_{\mathbf{A}}$$
7. 计算卫星的相对速度（相对于赤惯系）。
   $${\mathbf{v}}_{\mathbf{B}}={\mathbf{v}}_{\mathbf{A}}+\mathbf{\Omega }\times {\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}+{\mathbf{v}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$$
   $${\mathbf{v}}_{\mathbf{r}\mathbf{e}\mathbf{l}}={\mathbf{v}}_{\mathbf{B}}-{\mathbf{v}}_{\mathbf{A}}-\mathbf{\Omega }\times {\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$$
8. 计算卫星的相对加速度（相对于赤惯系）。
   $${\mathbf{a}}_{\mathbf{B}}={\mathbf{a}}_{\mathbf{A}}+\dot{\mathbf{\Omega }}\times {\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}+\mathbf{\Omega }\times (\mathbf{\Omega }\times {\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}})+2\mathbf{\Omega }\times {\mathbf{v}}_{\mathbf{r}\mathbf{e}\mathbf{l}}+{\mathbf{a}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$$
   $${\mathbf{a}}_{\mathbf{r}\mathbf{e}\mathbf{l}}={\mathbf{a}}_{\mathbf{B}}-{\mathbf{a}}_{\mathbf{A}}-\dot{\mathbf{\Omega }}\times {\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}-\mathbf{\Omega }\times (\mathbf{\Omega }\times {\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}})-2\mathbf{\Omega }\times {\mathbf{v}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$$
9. 计算卫星从赤惯系转换到移动坐标系(xyz)的变换矩阵。
   $$[\mathbf{Q}{]}_{Xx}=\left[\begin{array}{c}\hat{\mathbf{i}}\\ \hat{\mathbf{j}}\\ \hat{\mathbf{k}}\end{array}\right]$$
10. 计算卫星的相对位置、速度和加速度（参考系为移动坐标xyz）。
    $${\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}{)}_{xyz}=[\mathbf{Q}{]}_{Xx}\cdot {\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$$
    $${\mathbf{v}}_{\mathbf{r}\mathbf{e}\mathbf{l}}{)}_{xyz}=[\mathbf{Q}{]}_{Xx}\cdot {\mathbf{v}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$$
    $${\mathbf{a}}_{\mathbf{r}\mathbf{e}\mathbf{l}}{)}_{xyz}=[\mathbf{Q}{]}_{Xx}\cdot {\mathbf{a}}_{\mathbf{r}\mathbf{e}\mathbf{l}}$$
    

算例：
卫星A：$a=6803.6km,e=0.02,i=60°,\Omega =40°,\omega =30°,\theta =40°$
卫星B：$a=6878.9km,e=0.005,i=50°,\Omega =40°,\omega =120°,\theta =40°$

Matlab 代码：

% Satellite relative motion in orbit
% Calculate the position, velocity and acceleration of spacecraft B relative to spacecraft A, measured along the xyz axes of the co-moving coordinate system of spacecraft A

% Input:Six orbital elements
% Output: Relative position, velocity and acceleration

clc
clear all
mu = 3.9860*1e5;
a1 = 6803.6;
a2 = 6878.9;
e1 = 0.02;
e2 = 0.005;
i1 = 60;
i2 = 70;
raan1 = 40;
raan2 = 40;
perigee1 = 30;
perigee2 = 120;
true1 = 40;
true2 = 40;

% Keplerian_RV() convert orbital elements into position and velocity vectors
[rA, vA] = Keplerian_RV(a1,e1,raan1,i1,perigee1,true1);
[rB, vB] = Keplerian_RV(a2,e2,raan2,i2,perigee2,true2);

% accelerations of two spacecraft
aA = -mu*rA/(norm(rA)^3);
aB = -mu*rB/(norm(rB)^3);

% momentum
hA = cross(rA,vA);

% co-moving frame of reference
I = rA/norm(rA);
K = hA/norm(hA);
J = cross(K,I);

% angular velocity
angV = hA/(norm(rA)^2);

% angular acceleration
angA = -2*(dot(rA,vA))/(norm(rA)^2)*angV;

% relative position
rRel = rB - rA;

% relative velocity: vB-vA-angV x rRel
vRel = vB-vA- cross(angV,rRel);

% relative acceleration: 
aRel = aB-aA-cross(angA,rRel)-cross(angV,cross(angV,rRel))-cross(2*angV,vRel);

% transformation matrix
Q = [I';J';K'];

% relative components along the axes of the co-moving xyz frame of spacecraft A
rRelXYZ = Q*rRel;
vRelXYZ = Q*vRel;
aRelXYZ = Q*aRel;

计算结果：
${\mathbf{r}}_{\mathbf{r}\mathbf{e}\mathbf{l}}{)}_{xyz}=\left[\begin{array}{c}-6730\\ 6840.1\\ 406.96\end{array}\right](km);$

${\mathbf{v}}_{\mathbf{r}\mathbf{e}\mathbf{l}}{)}_{xyz}=\left[\begin{array}{c}0.30306\\ 0.10057\\ -1.24546\end{array}\right](km/s);$

${\mathbf{a}}_{\mathbf{r}\mathbf{e}\mathbf{l}}{)}_{xyz}=\left[\begin{array}{c}-0.000139\\ -0.000189\\ -0.000504\end{array}\right](km/s^2)$

参考文献：
Curtis, Howard D. Orbital mechanics for engineering students. Butterworth-Heinemann, 2013.

注：本文为作者的一篇学习笔记，记录了计算卫星相对位置、速度和加速度的算法和程序代码。

附Keplerian_RV代码：

% This is to convert keplerian orbital elements to position and velocity
% vectors
% Inputs: [a,eccen,raan,inclin,arg_perigee,true_anomaly]
% Outputs: rijk, vijk
% Example: [rijk,vijk] = Keplerian_RV(7437147.167,0.0017234,99.850837,312.921873,119.8701972,155.0849921)

function [rijk, vijk] = Keplerian_RV(a,eccen,inclin,raan,arg_perigee,true_anomaly)
    mu = 3.9860*1e14;  % mu = 3.9860*1e5 (Choose the correct measurement unit)
    h = sqrt(mu*a*(1-eccen*eccen));
    R_x = h*h/mu*(1/(1+eccen*cosd(true_anomaly)))*[cosd(true_anomaly), sind(true_anomaly), 0];
    V_x = (mu/h)*[-sind(true_anomaly), eccen+cosd(true_anomaly), 0];

    Q1 = [cosd(arg_perigee), sind(arg_perigee), 0;-sind(arg_perigee), cosd(arg_perigee),0;0,0,1];
    Q2 = [1,0,0;0,cosd(inclin), sind(inclin);0,-sind(inclin),cosd(inclin)];
    Q3 = [cosd(raan), sind(raan),0; -sind(raan),cosd(raan),0; 0,0,1];
    Qx = Q1*Q2*Q3;
    Qxt = Qx';
    rijk = Qxt * R_x';
    vijk = Qxt * V_x';
end