CN105549606A - 针对失效卫星的超近距离最优防撞接近方法 - Google Patents

Abstract

本发明涉及一种航天器超近距离最优防撞接近方法，特别涉及一种针对失效卫星的超近距离最优防撞接近方法，属于航天器交会技术领域。首先将目标，即失效卫星设计为球和椭球组合形式的包络模型，以简化目标构型；进而考虑目标姿态翻滚，在动态的目标本体系下推导目标与追踪星的相对动力学模型，及追踪星的路径约束条件；同时考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域；最后基于高斯伪谱法规划安全防撞路径，并进行闭环反馈控制。本发明既满足超近距离接近中的空间限制，又能够保证安全无碰撞的任务要求。且能够突出近距离抵近段航天器的姿轨耦合特点，并能够直接判断出航天器之间的距离是否满足约束条件。

Description

针对失效卫星的超近距离最优防撞接近方法

技术领域

本发明涉及一种航天器超近距离最优防撞接近方法，特别涉及一种针对失效卫星的超近距离最优防撞接近方法，属于航天器交会技术领域。

背景技术

针对失效卫星的主动逼近是失效卫星抓捕清理操作任务中的重要环节，然而，由于失效卫星处于失控状态，其姿态往往处于高速旋转状态，且失效卫星多数具有复杂的构型，因此，在考虑目标的姿态不确定性、构型复杂性的条件下，其全自主逼近过程极具难度和挑战性。在近距离导引段，追踪器采用相对导航进行自主控制，轨迹设计须满足目标安全控制区域和接近走廊等约束条件，同时考虑交会敏感器的测量范围及精度指标。

针对目标的构型复杂性、姿态漂移特性的特点，已有学者对逼近轨迹规划问题进行了研究，设计并提出了空间碎片近距离逼近安全轨迹规划方法。R.Lampariello针对非合作翻滚目标，利用基于梯度的非线性优化方法解决了防撞路径规划问题。(Lampariello,R.:"MotionPlanningfortheOn-orbitGraspingofaNon-cooperativeTargetSatellitewithCollisionAvoidance",i-SAIRAS2010,Japan,2010.)

AdrienEscande等通过离线方式，利用组合拼接的方法设计了一个包含目标几何外形的凸状多面体，基于V－clip或任何其他算法得到底层多面体的近似特征，继而在保证梯度连续性条件下计算接近距离，得到最优接近路径。(Escande,Adrien,Miossec,Sylvain,Kheddar,Abderrahmaneetal.Continuousgradientproximitydistanceforhumanoidsfree-collisionoptimized-postures[C].//；Pittsburgh,PA,USA.2007:188-195.)

StephenJacobsen利用两种方法规划了自由飞行机器人接近失控旋转卫星的安全轨迹。一是利用启发式方法，在二维平面得到满足约束的无碰撞路径；二是采用一般的数值优化方法，使代价函数最小化，规划出一个安全的空间轨迹，且后者可以得到更安全的接近轨迹。(StephenJacobsen,ChristopherLee,ChiZhuetal.PLANNINGOFSAFEKINEMATICTRAJECTORIESFORFREEFLYINGROBOTSAPPROACHINGANUNCONTROLLEDSPINNINGSATELLITE[C].//27thBiennialMechanismsandRoboticsConferencept.B.2002:1145-1151.)

在空间近距离操作任务中，由于地面参与少，安全性受到更多的关注。尤其在面向非合作目标跟踪、接近时，我们需结合目标的构型和运动状态规划出具备避碰能力的接近轨迹。对于不同的目标器外形，通过设置不同的安全禁区，以确定轨迹设计的约束条件。值得注意的是，由于目标姿态连续翻滚，安全控制区域和接近走廊随目标姿态运动不断变换，因此是动态时变的。对此我们展开了深入研究，以期实现对构型复杂、姿态翻滚的失效卫星的最优防撞安全抵近控制。

发明内容

本发明的目的是提供一种针对失效卫星的超近距离最优防撞接近方法，该方法能够有效地结合目标的构型、运动状态进行防撞策略设计。

本发明的方法是通过下述技术方案实现的。

针对失效卫星的超近距离最优防撞接近方法，首先将目标，即失效卫星设计为球和椭球组合形式的包络模型，以简化目标构型；进而考虑目标姿态翻滚，在动态的目标本体系下推导目标与追踪星的相对动力学模型，及追踪星的路径约束条件；同时考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域；最后基于高斯伪谱法规划安全防撞路径，并进行闭环反馈控制。

所述追踪星在沿规划轨迹逼近目标时，还将对目标进行视线跟踪，即调整姿态以使其视线轴实时指向目标质心。

针对失效卫星的超近距离最优防撞接近方法，具体步骤如下：

步骤一、确定航天器构型及其最小包络体。

根据目标，即失效卫星的失效状态不同，分三种情况：

情况一、目标受损严重，不带有太阳帆板，只考虑星本体。利用一个立方体简化目标构型，采用星本体的球型包络体作为目标最小包络体，即“球”模型，以描述追踪星的禁飞区域。包络体主轴坐标系与目标的本体系重合。

情况二、目标完好，带有成对的太阳帆板，考虑星本体和双侧的太阳帆板。考虑到目标是一个带有成对太阳帆板的卫星，带有太阳帆板的方向的尺寸要远大于其他两个方向，采用太阳帆板的椭球包络体和星本体的球型包络体组合的形式作为目标最小包络体，即“球+椭球”模型，以描述追踪星的禁飞区域。两个包络体中心重合，包络体主轴坐标系与椭球主轴坐标系重合，且与目标的本体系重合。

情况三、目标受损，带有非成对的太阳帆板，考虑星本体和单侧的太阳帆板。考虑到目标是一个单侧带有非成对太阳帆板的卫星，采用太阳帆板的半椭球包络体和星本体的球型包络体组合的形式作为目标最小包络体，即“球+半椭球”模型，以描述追踪星的禁飞区域。两个包络体球心重合，包络体主轴坐标系与半椭球主轴坐标系重合，且与目标的本体系重合。

追踪星则由一个简化的球形包络体模型代替。包络体主轴坐标系与追踪星的本体系重合。

步骤二、在目标本体坐标系下建立两航天器间的相对动力学模型。

近距离接近时，由于航天器的轨道与姿态耦合，需在轨道控制中引入姿态信息，则将两个航天器的相对位置矢量ρ投影到目标本体系中，得到相对轨道动力学方程模型的矢量形式

$\stackrel{\·\·}{\mathrm{\ρ}}=-{\stackrel{\·}{\mathrm{\ω}}}_{tb}\×\mathrm{\ρ}-2{\mathrm{\ω}}_{tb}\×\stackrel{\·}{\mathrm{\ρ}}-{\mathrm{\ω}}_{tb}\×({\mathrm{\ω}}_{tb}\×\mathrm{\ρ})-\frac{\mathrm{\μ}}{r_t^3}\[\mathrm{\ρ}-3\frac{{r_{tb}}^T{\mathrm{\ρr}}_{tb}}{r_t^2}\]+f---\left(1\right)$

其中，μ是地球引力常量；f是轨控推力；ω_tb、是目标本体相对惯性空间的旋转角速度和角加速度；r_t是航天器轨道高度；r_tb是航天器绝对位置矢量在目标本体系中的投影，记为

$r_{tb}=C_{tb}^o{\left[\begin{array}{ccc}0& 0& r_t\end{array}\right]}^T---\left(2\right)$

这里是轨道坐标系到目标本体系的转换矩阵。

为便于实现最优控制，将式(1)进一步整理为状态空间的形式

$\stackrel{\·}{X}=A\left(X\right)+BU---\left(3\right)$

式中

$\begin{array}{c}X=\left[\begin{array}{c}\mathrm{\ρ}\\ \stackrel{\·}{\mathrm{\ρ}}\end{array}\right]={\left[\begin{array}{cccccc}x& y& z& \stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right]}^T,A\left(X\right)=\left[\begin{array}{cc}{0}_3& I_3\\ -{{\stackrel{\·}{\mathrm{\ω}}}_{tb}}^{\×}-{{\mathrm{\ω}}_{tb}}^{\×}{{\mathrm{\ω}}_{tb}}^{\×}-\frac{\mathrm{\μ}}{r_t^3}S\left(C_{tb}^o\right)& -2{{\mathrm{\ω}}_{tb}}^{\×}\end{array}\right]X,\\ B=\left[\begin{array}{c}{0}_3\\ \frac{1}{m_3}{I}_3\end{array}\right],U={\left[\begin{array}{ccc}{u}_x& u_y& u_z\end{array}\right]}^T\end{array}---\left(4\right)$

其中，x,y,z和分别为相对位置矢量在目标本体系下的三轴分量；I₃是单位矩阵；m_c是追踪星质量；u_x,u_y,u_z分别为轨控推力在目标本体系下的三轴分量；记作

$S\left(C_{tb}^o\right)=\left[\begin{array}{ccc}1-3{C_{13}}^2& -3C_{13}{C}_{23}& -3C_{13}{C}_{33}\\ -3C_{13}{C}_{23}& 1-3{C_{23}}^2& -3C_{23}{C}_{33}\\ -3C_{13}{C}_{33}& -3C_{23}{C}_{33}& 1-3{C_{33}}^2\end{array}\right]---\left(5\right)$

而C₁₃,C₂₃,C₃₃是转换矩阵的元。

另外，由于追踪星推力器输出受限，控制力U需满足条件u_min≤U≤u_max。

步骤三、根据航天器的结构、几何构型设计安全飞行区域，以有效避开星体上可能发生碰撞的部位(包括星本体、太阳帆板、天线等)。

安全区域约束是为保证超近距离接近过程的安全性所设定的一个飞行区域，要求两个航天器的质心距离大于二者半径之和，表示为

S＝{M|r_c∈M,|r_c(t)-r_t(t)|≥D_min}(6)

这里r_c和r_t分别表示两个航天器的位置矢量，D_min即允许的最小安全距离。根据所接近的目标，即失效卫星的状态不同(如步骤一所示)，D_min的计算方法不同。

情况一、对于步骤一中情况一所给出的目标“球”包络体模型，显然追踪星与目标之间的最小安全距离是一个固定值

D_min＝d_s＝a_tb+a_c(7)

其中a_tb和a_c分别为两个包络球的半径。

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式

h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_s(8)

式中[x,y,z]是两个航天器的相对位置矢量。

情况二、对于步骤一中情况二所给出的目标“球+椭球”包络体模型，追踪星与目标星本体之间的最小安全距离为D_min＝d_s(式(7))；然而由于帆板包络体的非球形特性，追踪星与目标帆板之间的最小安全距离与二者的相对方位有关。对此，本发明将两个航天器的三维构型投影到二维平面，通过求解平面圆与椭圆相切问题得到二者之间的最小距离。

投影平面由两航天器质心位置确定。记追踪星质心在目标本体系中的坐标为[x_c,y_c,z_c]，则该平面定义为

$\frac{z_c}{y_c}y-z=0---\left(9\right)$

记目标帆板椭球包络体的半长轴为a,b,c，该椭球投影到上述平面所得的椭圆半长轴为a′,b′，表示为

$a^{\′}=a,b^{\′}=\sqrt{\frac{1+{\left(\frac{z_c}{y_c}\right)}^2}{\frac{1}{b^2}+{\left(\frac{z_c}{{\mathrm{cy}}_c}\right)}^2}}---\left(10\right)$

结合数学几何知识，推导出平面圆与椭圆圆心之间最小距离为

$d_e=\sqrt{\frac{q^2-1}{\mathrm{\δ}}{(a_c+\frac{b(1+\mathrm{\δ})}{q})}^2+(1-\frac{q^2-1}{\mathrm{\δ}}){(a_c+\frac{b}{q})}^2}---\left(11\right)$

这里δ与帆板椭球包络体尺寸有关，记为

$\mathrm{\δ}=\frac{a^{\′2}}{b^{\′2}}-1---\left(12\right)$

q是额外引入的变量，定义为

$q=\sqrt{(1+{\mathrm{\δsin}}^2\mathrm{\ψ})}---\left(13\right)$

式中ψ定义为椭圆上切点的外法线方向向量与椭圆长轴方向向量之间夹角。

d_e也就是追踪星球形包络体和目标帆板椭球包络体之间最小距离，至此就得到追踪星与目标帆板之间的最小安全距离D_min＝d_e。

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式

h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_i(i＝s,e)(14)

即要求两航天器之间的相对距离同时大于追踪星与目标星本体之间的最小安全距离和追踪星与目标帆板之间的最小安全距离。

情况三、对于步骤一中情况三所给出的目标“球+半椭球”包络体模型，追踪星与目标星本体之间的最小安全距离为D_min＝d_s(式(7))；追踪星与目标的单侧帆板之间的最小安全距离与情况二同理，记为D_min＝d_e。

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式。若目标只有+x方向帆板，则追踪星路径约束条件为

x≥0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_i(i＝s,e)

(15)

x＜0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_s

若目标只有-x方向帆板，则追踪星路径约束条件为

x≤0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_i(i＝s,e)

(16)

x＞0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_s

另外，为避开目标，即失效卫星上的天线(长为l)，增加一项路径约束条件，即

a_tb≤z≤a_tb+l时，x²+y²＞a_c ²

(17)

a_tb+l＜z≤a_tb+l+a_c时，x²+y²+(z-a_tb-l)²＞a_c ²

步骤四、考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域。

在近距离交会任务中，导航测量误差是不可忽视的一个要素。为此，在步骤三的基础上，又引入了误差椭球来表示位置不确定性，用以定义更加安全的飞行区域。

目标航天器的位置不确定性矩阵记为

$Cov=\left[\begin{array}{ccc}{{\mathrm{\σ}}^2}_x& & \\ & {{\mathrm{\σ}}^2}_y& \\ & & {{\mathrm{\σ}}^2}_z\end{array}\right]---\left(18\right)$

其中σ_i(i＝x,y,z)为坐标轴向的方差。

在位置测量中，通常假定目标位置坐标是服从正态分布的。如果用Δr表示航天器上某点的位置增量，那么该点位置在三维空间内的正态分布概率密度为

$pdf=\frac{\exp \[-\frac{1}{2}\left({\mathrm{\Δr}}^T{\mathrm{Cov}}^{-1}\mathrm{\Δ}r\right)\]}{{\left(2\mathrm{\π}\right)}^{\frac{3}{2}}\sqrt{\left|Cov\right|}}---\left(19\right)$

由此找到三维正态分布空间内概率密度相等的点，即

Δr^TCov^-1Δr＝k²(20)

其中k是放大因子。上式也是一个相似椭球族表达式，又可写作

$\frac{u^2}{{{\mathrm{\σ}}_x}^2}+\frac{v^2}{{{\mathrm{\σ}}_y}^2}+\frac{w^2}{{{\mathrm{\σ}}_z}^2}=k^2---\left(21\right)$

显然，每一个椭球对应一种概率。某点存在于误差椭球E_k内的概率可写作

$P=\∫\∫{\∫}_{E_k}\frac{\exp \[-\frac{1}{2}(\frac{u^2}{{{\mathrm{\σ}}_x}^2}+\frac{v^2}{{{\mathrm{\σ}}_y}^2}+\frac{w^2}{{{\mathrm{\σ}}_z}^2})\]}{{\left(2\mathrm{\π}\right)}^{\frac{3}{2}}{\mathrm{\σ}}_x{\mathrm{\σ}}_y{\mathrm{\σ}}_z}dudvdw---\left(22\right)$

设

可得

把上式中的指数函数展开成马克劳林级数后再将上式积分，得

$P=\frac{4}{\sqrt{2\mathrm{\π}}}(\frac{k^3}{6}-\frac{k^5}{20}+\frac{k^7}{112}-\frac{k^9}{864}+...)---\left(25\right)$

这样，根据允许的碰撞概率P_c,确定一个概率为P＝1-P_c的误差椭球,即确定放大因子k，就得到E_k椭球表面处于两航天器质心连线上的点到球心的距离

其中

$\mathrm{\θ}=\arg tan\frac{y_c}{x_c}$

d_p就是考虑导航测量误差造成的位置不确定性时所需的最小安全距离的增量，用以进一步扩大追踪星的禁飞区域。

结合式(8)，(14)，(15)，(16)和(25)就得到了三种情况下考虑位置不确定性的追踪星路径约束条件。

情况一、目标，即失效卫星不带帆板，追踪星路径约束条件为

h＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y_,z),d_s(a_tb,a_c)))≥0(27)

情况二、目标带有成对帆板，追踪星路径约束条件为

h₁＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

(28)

h₂＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_e(a,b,c,a_c,x,y,z)))≥0

情况三、目标带有不成对帆板，若只有+x方向帆板，追踪星路径约束条件为

x≥0时,h₁＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

h₂＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_e(a,b,c,a_c,x,y,z)))≥0(29)

x＜0时,h＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

若只有-x方向帆板，追踪星路径约束条件为

x≤0时,h₁＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

h₂＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_e(a,b,c,a_c,x,y,z)))≥0(30)

x＞0时,h＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

步骤五、基于高斯伪谱法，根据追踪星路径约束条件在安全区域内规划防撞接近轨迹。

轨迹规划问题就是根据航天器当前的位姿，运用合适的数学参考模型规划出一条满足航天器动力学特性以及其他约束条件的路径，以驱动航天器到达期望的位姿。因此，轨迹规划问题将转化为一个最优控制问题：根据步骤三和四所得到的追踪星路径约束条件，生成一条燃料最优的安全路径。

本发明采用基于高斯伪谱法的最优控制数值计算方法，将有限时间t_f内的连续优化控制问题转化为离散非线性规划问题进行求解。

下面将连续模型，包括步骤二中的动力学模型和步骤三、四中的约束条件，以及优化的性能指标等写成离散形式。

A.高斯点上的状态变量和控制变量为

X_1N,X_2N,X_3N,X_4N,X_5N,X_6N∈R^N,U_1N,U_2N,U_3N∈R^N

B.应用微分近似矩阵D∈R^N×N得到状态方程的积分形式

$\left\{\begin{array}{c}{\mathrm{DX}}_{1N}=\frac{t_f-t_0}{2}\·X_{4N}\\ {\mathrm{DX}}_{2N}=\frac{t_f-t_0}{2}\·X_{5N}\\ {\mathrm{DX}}_{3N}=\frac{t_f-t_0}{2}\·X_{6N}\\ {\mathrm{DX}}_{4N}=\frac{t_f-t_0}{2}\·(A_{X_{1N}}+1/m_c{U}_{1N})\\ {\mathrm{DX}}_{5N}=\frac{t_f-t_0}{2}\·(A_{X_{2N}}+1/m_c{U}_{2N})\\ {\mathrm{DX}}_{6N}=\frac{t_f-t_0}{2}\·(A_{X_{3N}}+1/m_c{U}_{3N})\end{array}\right.---\left(31\right)$

C.各高斯点i上的控制力约束条件为

u_1,min≤U_1Ni≤u_1,max

u_2,min≤U_2Ni≤u_2,max

u_3,min≤U_3Ni≤u_3,max

D.各高斯点i上的路径约束条件为

情况一、目标，即失效卫星不带帆板，路径约束条件为

h(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0(32)

情况二、目标带有成对帆板，路径约束条件为

h₁(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

h₂(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-(33)

D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_e(a,b,c,a_c,X_1Ni,X_2Ni,X_3Ni))≥0

情况三、目标带有不成对帆板，若只有+x方向帆板，路径约束条件为

X_1Ni≥0时,h₁(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

h₂(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-

(34)

D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_e(a,b,c,a_c,X_1Ni,X_2Ni,X_3Ni))≥0

X_1Ni＜0时,h(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

若只有-x方向帆板，路径约束条件为

X_1Ni≤0时,h₁(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

h₂(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-

(35)

D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_e(a,b,c,a_c,X_1Ni,X_2Ni,X_3Ni))≥0

X_1Ni＞0时,h(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

另外，每种情况下都有一项附加的路径约束条件，即

a_tb≤X_3Ni≤a_tb+l时，X² _1Ni+X2_2Ni＞a_c ²

(36)

a_tb+l＜X_3Ni≤a_tb+l+a_c时，X² _1Ni+X² _2Ni+(X_3Ni-a_tb-l)²＞a_c ²

E.由高斯求积公式得到末端约束条件

$\left\{\begin{array}{c}{X}_{1f}=X_{10}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{4N}\\ X_{2f}=X_{20}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{5N}\\ X_{3f}=X_{30}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{6N}\\ X_{4f}=X_{40}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{1N}}+1/m_c{U}_{1N})\\ X_{5f}=X_{50}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{2N}}+1/m_c{U}_{2N})\\ X_{6f}=X_{60}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{3N}}+1/m_c{U}_{3N})\end{array}\right.---\left(37\right)$

其中ω∈R^N是高斯积。

F.用高斯求积公式近似逼近性能函数

$J=\min \left(\frac{1}{2}{u}^{T}u\right)---\left(38\right)$

得

$J=\frac{t_f-t_0}{2}{\mathrm{\ω}}^T\·\underset{i=1}{\overset{N}{\Σ}}(U_{1N,i}^2+U_{2N,i}^2+U_{3N,i}^2)---\left(39\right)$

步骤六、设计闭环控制器，得到满足真实动力学关系的最优防撞接近轨迹。

根据步骤五所提出的规划方法，基于高斯伪谱法求解出了一系列满足约束条件和动力学特性的离散点，但是在点与点之间的轨迹并不满足动力学要求，因而设计了“最优控制+位置补偿”控制器，即将优化的控制量作为动力学系统输入项，得到满足动力学特性的运动轨迹。再将实际轨迹与优化所得的标称轨迹之差作为控制变量，设计为PID补偿控制器，同时作用于星体上，得到满足真实动力学关系的最优轨迹。

有益效果

1、本发明的一种针对失效卫星的超近距离最优防撞接近方法，考虑了目标的姿态翻滚特性，在动态坐标系中建立相对动力学模型、推导路径约束条件，能够突出近距离抵近段航天器的姿轨耦合特点，并能够直接判断出航天器之间的距离是否满足约束条件。

2、本发明的一种针对失效卫星的超近距离最优防撞接近方法，考虑了目标的构型复杂性和导航测量结果的不确定性，结合数学几何知识和碰撞概率问题对安全飞行区域进行定义，既满足超近距离接近中的空间限制，又能够保证安全无碰撞的任务要求。

3、本发明的一种针对失效卫星的超近距离最优防撞接近方法，基于高斯伪谱法展开开环的路径规划，进而设计闭环控制器得到满足真实动力学关系的最优轨迹。

附图说明

图1为接近任务中的追踪星与失效卫星的构型及其最小包络体；

图2为两个航天器之间最小安全距离的求解模型；

图3为航天器与误差椭球之间最小安全距离求解模型；

图4为实施例1中接近过程追踪星轨迹在目标本体系的三维示意图；

图5为实施例1中接近过程追踪星轨迹在目标本体系的二维示意图；

图6为实施例1中接近过程追踪星与失效卫星轨迹在惯性系的三维示意图；

图7为实施例1中接近过程航天器相对速度的时间历程；

图8为实施例1中接近过程追踪星轨道控制力的时间历程；

图9为实施例1中追踪星闭环控制回路；

图10为实施例1中追踪星开环规划轨迹和闭环跟踪轨迹；

图11为实施例1中接近过程追踪星姿态变化轨迹；

图12为实施例2中接近过程追踪星轨迹在目标本体系的三维示意图；

图13为实施例3中接近过程追踪星轨迹在目标本体系的三维示意图。

具体实施方式

下面结合附图与实施例对本发明做进一步说明。

实施例1

本发明的一种针对失效卫星的超近距离最优防撞接近方法，以某个带有一对太阳帆板、单个天线等附件的姿态翻滚的失效卫星为研究对象。该目标初始角速度为ω_t＝[0,0,0.05]^Trad/s，构型参数如下：

表1目标构型参数

如图1所示，追踪星首先到达停泊点X₀＝[-20,10,5]^Tm，并在该点使追踪星上的相对测量敏感器对准目标，即失效卫星。追踪星从此处启动逼近控制过程。该过程中，追踪星沿规划的安全防撞路径机动，最终在有限时间50s内到达距离目标对接抓捕部位一定距离的停泊位置X_f＝[0,-4,0]^Tm；同时姿态时刻变化，实现视线轴始终对准目标。

所述的针对失效卫星的超近距离最优防撞接近方法，具体步骤如下：

步骤一、确定航天器构型及其最小包络体。

采用太阳帆板的椭球包络体和星本体的球型包络体组合的形式作为目标最小包络体，即“球+椭球”模型，以描述追踪星的禁飞区域。两个包络体中心重合，包络体主轴坐标系与椭球主轴坐标系重合，且与目标的本体系重合。

在目标本体坐标系下，目标星本体包络球的数学模型为

$\frac{x^2}{3{\left(\frac{x_a}{2}\right)}^2}+\frac{y^2}{3{\left(\frac{x_a}{2}\right)}^2}+\frac{z^2}{3{\left(\frac{x_a}{2}\right)}^2}=1---\left(40\right)$

其中x_a＝2m是目标星本体简化的立方体构型的边长。目标星本体包络球的半径为 $a_{tb}=\frac{\sqrt{3}{x}_a}{2}=\sqrt{3}m.$

在目标本体坐标系下，目标太阳帆板包络椭球的数学模型为

$\frac{x^2}{3{\left(\frac{x_b}{2}\right)}^2}+\frac{y^2}{3{\left(\frac{y_b}{2}\right)}^2}+\frac{z^2}{3{\left(\frac{z_b}{2}\right)}^2}=1---\left(41\right)$

其中x_b＝(4×2+2)m,y_b＝0.1m,z_b＝1m是目标太阳帆板简化的长方体构型的长、宽、高。目标帆板包络椭球的半长轴为

$a=\frac{\sqrt{3}{x}_b}{2}=5\sqrt{3}m,b=\frac{\sqrt{3}{y}_b}{2}=\frac{\sqrt{3}}{20}m,c=\frac{\sqrt{3}{z}_b}{2}=\frac{\sqrt{3}}{2}m$

追踪星由半径为a_c＝0.75m的球形包络体表示。

步骤二、在目标本体坐标系下建立两航天器间的相对动力学模型。

近距离接近时，由于航天器的轨道与姿态耦合，需在轨道控制中引入姿态信息，则将两个航天器的相对位置矢量ρ投影到目标本体系中，得到相对轨道动力学方程模型的矢量形式

$\stackrel{\·\·}{\mathrm{\ρ}}=-{\stackrel{\·}{\mathrm{\ω}}}_{tb}\×\mathrm{\ρ}-2{\mathrm{\ω}}_{tb}\×\stackrel{\·}{\mathrm{\ρ}}-{\mathrm{\ω}}_{tb}\×({\mathrm{\ω}}_{tb}\×\mathrm{\ρ})-\frac{\mathrm{\μ}}{r_t^3}\[\mathrm{\ρ}-3\frac{{r_{tb}}^T{\mathrm{\ρr}}_{tb}}{r_t^2}\]+f---\left(42\right)$

其中，μ是地球引力常量；f是轨控推力；ω_tb、是目标本体相对惯性空间的旋转角速度和角加速度；r_t是航天器轨道高度；r_tb是航天器绝对位置矢量在目标本体系中的投影，记为

$r_{tb}=C_{tb}^o{\left[\begin{array}{ccc}0& 0& r_t\end{array}\right]}^T---\left(43\right)$

这里是轨道坐标系到目标本体系的转换矩阵。

为便于实现最优控制，将式(42)进一步整理为状态空间的形式

$\stackrel{\·}{X}=A\left(X\right)+BU---\left(44\right)$

式中

$\begin{array}{c}X=\left[\begin{array}{c}\mathrm{\ρ}\\ \stackrel{\·}{\mathrm{\ρ}}\end{array}\right]={\left[\begin{array}{cccccc}x& y& z& \stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right]}^T,A\left(X\right)=\left[\begin{array}{cc}{0}_3& I_3\\ -{{\stackrel{\·}{\mathrm{\ω}}}_{tb}}^{\×}-{{\mathrm{\ω}}_{tb}}^{\×}{{\mathrm{\ω}}_{tb}}^{\×}-\frac{\mathrm{\μ}}{r_t^3}S\left(C_{tb}^o\right)& -2{{\mathrm{\ω}}_{tb}}^{\×}\end{array}\right]X,\\ B=\left[\begin{array}{c}{0}_3\\ \frac{1}{m_3}{I}_3\end{array}\right],U={\left[\begin{array}{ccc}{u}_x& u_y& u_z\end{array}\right]}^T\end{array}---\left(45\right)$

其中，x,y,z和分别为相对位置矢量在目标本体系下的三轴分量；I₃是单位矩阵；m_c是追踪星质量；u_x,u_y,u_z分别为轨控推力在目标本体系下的三轴分量；记作

$S\left(C_{tb}^o\right)=\left[\begin{array}{ccc}1-3{C_{13}}^2& -3C_{13}{C}_{23}& -3C_{13}{C}_{33}\\ -3C_{13}{C}_{23}& 1-3{C_{23}}^2& -3C_{23}{C}_{33}\\ -3C_{13}{C}_{33}& -3C_{23}{C}_{33}& 1-3{C_{33}}^2\end{array}\right]---\left(46\right)$

而C₁₃,C₂₃,C₃₃是转换矩阵的元。

另外，由于追踪星推力器输出受限，控制力U需满足条件

-1≤u_x≤1,

-1≤u_y≤1,

-1≤u_z≤1

步骤三、根据航天器的结构、几何构型设计安全飞行区域，以有效避开星体上可能发生碰撞的部位(包括星本体、太阳帆板、天线等)。

安全区域约束是为保证超近距离接近过程的安全性所设定的一个飞行区域，要求两个航天器的质心距离大于二者半径之和，表示为

S＝{M|r_c∈M_,|r_c(t)-r_t(t)|≥D_min}(47)

这里r_c和r_t分别表示两个航天器的位置矢量，D_min即允许的最小安全距离。

本例中所考虑的目标，即失效卫星带有一对太阳帆板和单个天线，D_min的计算方法如下。

如图2所示，针对目标“球+椭球”包络体模型，追踪星与目标星本体之间的最小安全距离为是一个固定值

$D_{\min }=d_s=a_{tb}+a_c=(\sqrt{3}+0.75)m---\left(48\right)$

由于帆板包络体的非球形特性，追踪星与目标帆板之间的最小安全距离与二者的相对方位有关。将两个航天器的三维构型投影到二维平面，通过求解平面圆与椭圆相切问题得到二者之间的最小距离。

投影平面由两航天器质心位置确定。记追踪星质心在目标本体系中的坐标为[x_c,y_c,z_c]，则该平面定义为

$\frac{z_c}{y_c}y-z=0---\left(49\right)$

由目标帆板椭球包络体的半长轴为a,b,c得到该椭球投影到上述平面所得的椭圆半长轴为a′,b′，表示为

$a^{\′}=a=5\sqrt{3}m,b^{\′}=\sqrt{\frac{1+{\left(\frac{z_c}{y_c}\right)}^2}{\frac{1}{b^2}+{\left(\frac{z_c}{{\mathrm{cy}}_c}\right)}^2}}=\sqrt{\frac{1+{\left(\frac{z_c}{y_c}\right)}^2}{\frac{400}{3}+{\left(\frac{z_c}{{\mathrm{cy}}_c}\right)}^2}}m---\left(50\right)$

结合数学几何知识，推导出平面圆与椭圆圆心之间最小距离为

$d_e=\sqrt{\frac{q^2-1}{\mathrm{\δ}}{(a_c+\frac{b(1+\mathrm{\δ})}{q})}^2+(1-\frac{q^2-1}{\mathrm{\δ}}){(a_c+\frac{b}{q})}^2=}\sqrt{\frac{q^2-1}{\mathrm{\δ}}{(0.75+\frac{\sqrt{3}(1+\mathrm{\δ})}{20q})}^2+(1-\frac{q^2-1}{\mathrm{\δ}}){(0.75+\frac{\sqrt{3}}{20q})}^2}m---\left(51\right)$

这里δ与帆板椭球包络体尺寸有关，记为

$\mathrm{\δ}=\frac{a^{\′2}}{b^{\′2}}-1---\left(52\right)$

q是额外引入的变量，定义为

$q=\sqrt{(1+{\mathrm{\δsin}}^2\mathrm{\ψ})}---\left(13\right)$

式中ψ定义为椭圆上切点的外法线方向向量与椭圆长轴方向向量之间夹角。

d_e也就是追踪星球形包络体和目标帆板椭球包络体之间最小距离，至此就得到追踪星与目标帆板之间的最小安全距离D_min＝d_e。

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式

$\begin{array}{cc}{h}_1=(x^2+y^2+z^2-{D_{\min }}^2)\≥0,& D_{\min }=(\sqrt{3}+0.75)m\\ h_2=(x^2+y^2+z^2-{D_{\min }}^2)\≥0,& D_{\min }=d_e\end{array}---\left(54\right)$

即要求两航天器之间的相对距离同时大于追踪星与目标星本体之间的最小安全距离和追踪星与目标帆板之间的最小安全距离。

另外，为避开目标，即失效卫星上的天线(长为l＝2)，有一项附加的路径约束条件为

步骤四、考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域。

考虑近距离交会任务中的导航测量误差，在步骤三的基础上，又引入了误差椭球来表示位置不确定性，用以定义更加安全的飞行区域，如图3所示。

目标航天器的位置不确定性矩阵记为

$Cov=\left[\begin{array}{ccc}0.4& & \\ & 0.3& \\ & & 0.1\end{array}\right]---\left(56\right)$

在位置测量中，通常假定目标位置坐标是服从正态分布的。如果用Δr表示航天器上某点的位置增量，那么该点位置在三维空间内的正态分布概率密度为

$pdf=\frac{\exp \[-\frac{1}{2}\left({\mathrm{\Δr}}^T{\mathrm{Cov}}^{-1}\mathrm{\Δ}r\right)\]}{{\left(2\mathrm{\π}\right)}^{\frac{3}{2}}\sqrt{\left|Cov\right|}}---\left(57\right)$

由此找到三维正态分布空间内概率密度相等的点，即

Δr^TCov^-1Δr＝k²(58)

其中k是放大因子。上式也是一个相似椭球族表达式，又可写作

$\frac{u^2}{{{\mathrm{\σ}}_x}^2}+\frac{v^2}{{{\mathrm{\σ}}_y}^2}+\frac{w^2}{{{\mathrm{\σ}}_z}^2}=k^2---\left(59\right)$

显然，每一个椭球对应一种概率。某点存在于误差椭球E_k内的概率可写作

$P=\∫\∫{\∫}_{E_k}\frac{\exp \[-\frac{1}{2}(\frac{u^2}{{{\mathrm{\σ}}_x}^2}+\frac{v^2}{{{\mathrm{\σ}}_y}^2}+\frac{w^2}{{{\mathrm{\σ}}_z}^2})\]}{{\left(2\mathrm{\π}\right)}^{\frac{3}{2}}{\mathrm{\σ}}_x{\mathrm{\σ}}_y{\mathrm{\σ}}_z}dudvdw---\left(60\right)$

设

可得

把上式中的指数函数展开成马克劳林级数后再将上式积分，得

$P=\frac{4}{\sqrt{2\mathrm{\π}}}(\frac{k^3}{6}-\frac{k^5}{20}+\frac{k^7}{112}-\frac{k^9}{864}+...)---\left(63\right)$

这样，根据允许的最大碰撞概率P_c＝3％,确定一个概率为P＝1-P_c的误差椭球,即确定放大因子k＝3，就得到E_k椭球表面处于两航天器质心连线上的点到球心的距离

其中

$\mathrm{\θ}=\arg \tan \frac{y_c}{x_c}$

d_p就是考虑导航测量误差造成的位置不确定性时所需的最小安全距离的增量，用以进一步扩大追踪星的禁飞区域。

结合式(54)和(64)就得到了考虑位置不确定性的追踪星路径约束条件，即

$\begin{array}{cc}{h}_1=(x^2+y^2+z^2-{D_{\min }}^2)\≥0,& D_{\min }=\left(d_p\right(x,y,z)+\sqrt{3}+0.75)m\\ h_2=(x^2+y^2+z^2-{D_{\min }}^2)\≥0,& D_{\min }=d_p(x,y,z)+d_e(x,y,z)\end{array}---\left(65\right)$

步骤五、基于高斯伪谱法，根据追踪星路径约束条件在安全区域内规划防撞接近轨迹。

采用基于高斯伪谱法的最优控制数值计算方法，将连续优化控制问题转化为离散非线性规划问题进行求解，即将连续模型，包括步骤二中的动力学模型和步骤三、四中的约束条件，以及优化的性能指标等写成离散形式，再进行数值计算，以得到有限时间t_f＝50s内满足约束条件的燃料最优的安全路径。

A.高斯点上的状态变量和控制变量为

X_1N，X_2N,X_3N,X_4N,X_5N,X_6N∈R^N,U_1N,U_2N,U_3N∈R^N

B.应用微分近似矩阵D∈R^N×N得到状态方程的积分形式

$\left\{\begin{array}{c}{\mathrm{DX}}_{1N}=\frac{t_f-t_0}{2}\·X_{4N}\\ {\mathrm{DX}}_{2N}=\frac{t_f-t_0}{2}\·X_{5N}\\ {\mathrm{DX}}_{3N}=\frac{t_f-t_0}{2}\·X_{6N}\\ {\mathrm{DX}}_{4N}=\frac{t_f-t_0}{2}\·(A_{X_{1N}}+1/m_c{U}_{1N})\\ {\mathrm{DX}}_{5N}=\frac{t_f-t_0}{2}\·(A_{X_{2N}}+1/m_c{U}_{2N})\\ {\mathrm{DX}}_{6N}=\frac{t_f-t_0}{2}\·(A_{X_{3N}}+1/m_c{U}_{3N})\end{array}\right.---\left(66\right)$

C.各高斯点i上的控制力约束条件为

-1≤U_1Ni≤1

-1≤U_2Ni≤1

-1≤U_3Ni≤1

D.各高斯点i上的路径约束条件为

$\begin{array}{cc}{h}_1\left(X_{Ni}\right)={X^2}_{1Ni}+{X^2}_{2Ni}+{X^2}_{3Ni}-{D_{\min }}^2\≥0,& D_{\min }=\left(d_p\right(X_{1Ni},X_{2Ni},X_{3Ni})+\sqrt{3}+0.75)m\\ h_2\left(X_{Ni}\right)={X^2}_{1Ni}+{X^2}_{2Ni}+{X^2}_{3Ni}-{D_{\min }}^2\≥0,& D_{\min }=d_p(X_{1Ni},X_{2Ni},X_{3Ni})+d_e(X_{1Ni},X_{2Ni},X_{3Ni})\end{array}---\left(67\right)$

E.由高斯求积公式得到末端约束条件

$\left\{\begin{array}{c}{X}_{1f}=X_{10}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{4N}=0\\ X_{2f}=X_{20}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{5N}=-4\\ X_{3f}=X_{30}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{6N}=0\\ X_{4f}=X_{40}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{1N}}+1/m_c{U}_{1N})=0\\ X_{5f}=X_{50}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{2N}}+1/m_c{U}_{2N})=0\\ X_{6f}=X_{60}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{3N}}+1/m_c{U}_{3N})=0\end{array}\right.---\left(37\right)$

其中ω∈R^N是高斯积。

F.用高斯求积公式近似逼近性能函数

$J=\frac{t_f-t_0}{2}{\mathrm{\ω}}^T\·\underset{i=1}{\overset{N}{\Σ}}(U_{1N,i}^2+U_{2N,i}^2+U_{3N,i}^2)---\left(70\right)$

根据本发明所提出的最优规划方法，基于高斯伪谱法分别求解出有/无导航测量误差条件下，满足约束条件的离散轨迹。图4-6中的仿真结果表示，在该过程中，追踪星沿规划的安全防撞路径机动，可以有效地避开目标星体上可能发生碰撞的部位(包括星本体、太阳帆板、天线等)，最终能够安全地到达终止状态。

当考虑不确定性时，航天器需要消耗更多的燃料，并从稍远的位置绕过星体到达目的位置。该过程中最大的碰撞概率为P_c,max＝0.24％，满足任务要求。

图7说明两个航天器的相对速度最后能够控制到零，即保证追踪星与目标同步运动。这为下一步在轨操作做好准备。

图8说明追踪星上的控制力大小不超过推力器限制(-1,1)，满足任务要求。

步骤六、设计闭环控制器，得到满足真实动力学关系的最优防撞接近轨迹。

根据步骤五所提出的规划方法，基于高斯伪谱方法能够很好地解决航天器之间超近距离安全防撞接近路径规划问题，但由于离散的点与点之间的轨迹并不满足动力学要求，进一步地，设计了“最优控制+位置补偿”控制器(图9)，将优化的控制量作为动力学系统输入项，得到满足动力学特性的运动轨迹。再将实际轨迹与优化所得的标称轨迹之差作为控制变量，设计为PID补偿控制器，同时作用于星体上，得到满足真实动力学关系的最优轨迹。如图10所示，通过最优控制和位置补偿能够较好地跟踪规划轨迹，控制精度为10^-3，满足任务要求。

另外，在图11中，追踪星在沿规划轨迹进行轨道机动时，追踪星的姿态满足姿态视场指向约束，即追踪星的测量敏感器视场始终指向目标质心，能够保证相对位姿测量信息的连续性。

实施例2

本发明的一种针对失效卫星的超近距离最优防撞接近方法，以某个外形受损严重，不带有太阳帆板，仅携带天线等附件的姿态翻滚的失效卫星为研究对象。该目标初始角速度为ω_t＝[0,0,0.05]^Trad/s，构型参数如下：

表1目标构型参数

追踪星首先到达停泊点X₀＝[-20,10,5]^Tm，并在该点使追踪星上的相对测量敏感器对准目标，即失效卫星。追踪星从此处启动逼近控制过程。该过程中，追踪星沿规划的安全防撞路径机动，最终在有限时间50s内到达距离目标对接抓捕部位一定距离的停泊位置X_f＝[0,-4,0]^Tm；同时姿态时刻变化，实现视线轴始终对准目标。

所述的针对失效卫星的超近距离最优防撞接近方法，具体步骤如下：

步骤一、确定航天器构型及其最小包络体。

采用星本体的球型包络体作为目标最小包络体，即“球”模型，以描述追踪星的禁飞区域。包络体主轴坐标系与目标的本体系重合。

在目标本体坐标系下，目标星本体包络球的数学模型为

$\frac{x^2}{3{\left(\frac{x_a}{2}\right)}^2}+\frac{y^2}{3{\left(\frac{x_a}{2}\right)}^2}+\frac{z^2}{3{\left(\frac{x_a}{2}\right)}^2}=1---\left(71\right)$

其中x_a＝2m是目标星本体简化的立方体构型的边长。目标星本体包络球的半径为 $a_{tb}=\frac{\sqrt{3}{x}_a}{2}=\sqrt{3}m.$

追踪星由半径为a_c＝0.75m的球形包络体表示。

步骤二、在目标本体坐标系下建立两航天器间的相对动力学模型。

同实施例1。

步骤三、根据航天器的结构、几何构型设计安全飞行区域，以有效避开星体上可能发生碰撞的部位(包括星本体、天线等)。

安全区域约束是为保证超近距离接近过程的安全性所设定的一个飞行区域，要求两个航天器的质心距离大于二者半径之和，表示为

S＝{M|r_c∈M_,|r_c(t)-r_t(t)|≥D_min}(72)

这里r_c和r_t分别表示两个航天器的位置矢量，D_min即允许的最小安全距离。

本例中所考虑的目标，即失效卫星带有单个天线，D_min的计算方法如下。

针对目标“球”包络体模型，追踪星与目标星本体之间的最小安全距离为是一个固定值

$D_{\min }=d_s=a_{tb}+a_c=(\sqrt{3}+0.75)m---\left(73\right)$

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式

$h=(x^2+y^2+z^2-{D_{\min }}^2)\≥0,D_{\min }=(\sqrt{3}+0.75)m---\left(74\right)$

即要求两航天器之间的相对距离大于追踪星与目标星本体之间的最小安全距离。

另外，为避开目标，即失效卫星上的天线(长为l＝2)，有一项附加的路径约束条件为

步骤四、考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域。

同实施例1。

考虑位置不确定性的追踪星路径约束条件为

$h=(x^2+y^2+z^2-{D_{\min }}^2)\≥0,D_{\min }=\left(d_p\right(x,y,z)+\sqrt{3}+0.75)m---\left(76\right)$

步骤五、基于高斯伪谱法，根据追踪星路径约束条件在安全区域内规划防撞接近轨迹。

同实施例1。

各高斯点i上的路径约束条件为

$h\left(X_{Ni}\right)={X^2}_{1Ni}+{X^2}_{2Ni}+{X^2}_{3Ni}-{D_{\min }}^2\≥0,D_{\min }=\left(d_p\right(X_{1Ni},X_{2Ni},X_{3Ni})+\sqrt{3}+0.75)m---\left(77\right)$

根据本发明所提出的最优规划方法，基于高斯伪谱法求解出有导航测量误差时，满足约束条件的离散轨迹。图12中的仿真结果表示，在该过程中，追踪星沿规划的安全防撞路径机动，可以有效地避开目标星体上可能发生碰撞的部位(包括星本体、天线等)，最终能够安全地到达终止状态。

步骤六、设计闭环控制器，得到满足真实动力学关系的最优防撞接近轨迹。

同实施例1。

实施例3

本发明的一种针对失效卫星的超近距离最优防撞接近方法，以某个外形受损，仅单侧(+x方向)带有一个太阳帆板，并携带单个天线等附件的姿态翻滚的失效卫星为研究对象。该目标初始角速度为ω_t＝[0,0,0.05]^Trad/s，构型参数如下：

表1目标构型参数

追踪星首先到达停泊点X₀＝[-20,10,5]^Tm，并在该点使追踪星上的相对测量敏感器对准目标，即失效卫星。追踪星从此处启动逼近控制过程。该过程中，追踪星沿规划的安全防撞路径机动，最终在有限时间50s内到达距离目标对接抓捕部位一定距离的停泊位置X_f＝[0,-4,0]^Tm；同时姿态时刻变化，实现视线轴始终对准目标。

所述的针对失效卫星的超近距离最优防撞接近方法，具体步骤如下：

步骤一、确定航天器构型及其最小包络体。

采用太阳帆的半椭球包络体和星本体的球型包络体组合的形式作为目标最小包络体，即“球+半椭球”模型，以描述追踪星的禁飞区域。两个包络体球心重合，包络体主轴坐标系与半椭球主轴坐标系重合，且与目标的本体系重合。

在目标本体坐标系下，目标星本体包络球的数学模型为

$\frac{x^2}{3{\left(\frac{x_a}{2}\right)}^2}+\frac{y^2}{3{\left(\frac{x_a}{2}\right)}^2}+\frac{z^2}{3{\left(\frac{x_a}{2}\right)}^2}=1---\left(79\right)$

其中x_a＝2m是目标星本体简化的立方体构型的边长。目标星本体包络球的半径为 $a_{tb}=\frac{\sqrt{3}{x}_a}{2}=\sqrt{3}m.$

在目标本体坐标系下，目标太阳帆板包络半椭球的数学模型为

$\frac{x^2}{3{\left(x_b\right)}^2}+\frac{y^2}{3{\left(\frac{y_b}{2}\right)}^2}+\frac{z^2}{3{\left(\frac{z_b}{2}\right)}^2}=1,x>0---\left(80\right)$

其中x_b＝(4+2/2)m,y_b＝0.1m,z_b＝1m是目标太阳帆板简化的长方体构型的长、宽、高。目标帆板包络半椭球的半长轴为

$a=\sqrt{3}{x}_b=5\sqrt{3}m,b=\frac{\sqrt{3}{y}_b}{2}=\frac{\sqrt{3}}{20}m,c=\frac{\sqrt{3}{z}_b}{2}=\frac{\sqrt{3}}{2}m$

追踪星由半径为a_c＝0.75m的球形包络体表示。

步骤二、在目标本体坐标系下建立两航天器间的相对动力学模型。

同实施例1。

步骤三、根据航天器的结构、几何构型设计安全飞行区域，以有效避开星体上可能发生碰撞的部位(包括星本体、太阳帆板、天线等)。

安全区域约束是为保证超近距离接近过程的安全性所设定的一个飞行区域，要求两个航天器的质心距离大于二者半径之和，表示为

S＝{M|r_c∈M,|r_c(t)-r_t(t)|≥D_min}(81)

这里r_c和r_t分别表示两个航天器的位置矢量，D_min即允许的最小安全距离。

本例中所考虑的目标，即失效卫星在+x方向带有一个太阳帆板，并携带单个天线，D_min的计算方法如下。

针对目标“球+半椭球”包络体模型，追踪星与目标星本体之间的最小安全距离为是一个固定值

$D_{\min }=d_s=a_{tb}+a_c=(\sqrt{3}+0.75)m---\left(82\right)$

对于目标帆板，只需考虑+x方向追踪星与目标帆板之间的最小安全距离即可。也就是在+x方向，最小安全距离D_min求解方法同实施例1；在-x方向，最小安全距离D_min求解方法同实施例2。

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式

即要求两航天器之间的相对距离同时大于追踪星与目标星本体之间的最小安全距离和追踪星与目标帆板之间的最小安全距离。

另外，为避开目标，即失效卫星上的天线(长为l＝2)，有一项附加的路径约束条件为

步骤四、考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域。

同实施例1。

考虑位置不确定性的追踪星路径约束条件，即

步骤五、基于高斯伪谱法，根据追踪星路径约束条件在安全区域内规划防撞接近轨迹。

同实施例1。

各高斯点i上的路径约束条件为

根据本发明所提出的最优规划方法，基于高斯伪谱法分别求解出有导航测量误差时，满足约束条件的离散轨迹。图13中的仿真结果表示，在该过程中，追踪星沿规划的安全防撞路径机动，可以有效地避开目标星体上可能发生碰撞的部位(包括星本体、太阳帆板、天线等)，最终能够安全地到达终止状态。

步骤六、设计闭环控制器，得到满足真实动力学关系的最优防撞接近轨迹。

同实施例1。

Claims (3)

1.针对失效卫星的超近距离最优防撞接近方法，其特征在于：首先将目标，即失效卫星设计为球和椭球组合形式的包络模型，以简化目标构型；进而考虑目标姿态翻滚，在动态的目标本体系下推导目标与追踪星的相对动力学模型，及追踪星的路径约束条件；同时考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域；最后基于高斯伪谱法规划安全防撞路径，并进行闭环反馈控制。

2.如权利要求1所述的针对失效卫星的超近距离最优防撞接近方法，其特征在于：所述追踪星在沿规划轨迹逼近目标时，还将对目标进行视线跟踪，即调整姿态以使其视线轴实时指向目标质心。

3.针对失效卫星的超近距离最优防撞接近方法，具体步骤如下：

步骤一、确定航天器构型及其最小包络体；

根据目标，即失效卫星的失效状态不同，分三种情况：

情况一、目标受损严重，不带有太阳帆板，只考虑星本体；利用一个立方体简化目标构型，采用星本体的球型包络体作为目标最小包络体，即“球”模型，以描述追踪星的禁飞区域；包络体主轴坐标系与目标的本体系重合；

情况二、目标完好，带有成对的太阳帆板，考虑星本体和双侧的太阳帆板；考虑到目标是一个带有成对太阳帆板的卫星，带有太阳帆板的方向的尺寸要远大于其他两个方向，采用太阳帆板的椭球包络体和星本体的球型包络体组合的形式作为目标最小包络体，即“球+椭球”模型，以描述追踪星的禁飞区域；两个包络体中心重合，包络体主轴坐标系与椭球主轴坐标系重合，且与目标的本体系重合；

情况三、目标受损，带有非成对的太阳帆板，考虑星本体和单侧的太阳帆板；考虑到目标是一个单侧带有非成对太阳帆板的卫星，采用太阳帆板的半椭球包络体和星本体的球型包络体组合的形式作为目标最小包络体，即“球+半椭球”模型，以描述追踪星的禁飞区域；两个包络体球心重合，包络体主轴坐标系与半椭球主轴坐标系重合，且与目标的本体系重合；

追踪星则由一个简化的球形包络体模型代替；包络体主轴坐标系与追踪星的本体系重合；

步骤二、在目标本体坐标系下建立两航天器间的相对动力学模型；

近距离接近时，由于航天器的轨道与姿态耦合，需在轨道控制中引入姿态信息，则将两个航天器的相对位置矢量ρ投影到目标本体系中，得到相对轨道动力学方程模型的矢量形式

$\stackrel{\·\·}{\mathrm{\ρ}}=-{\stackrel{\·}{\mathrm{\ω}}}_{tb}\×\mathrm{\ρ}-2{\mathrm{\ω}}_{tb}\×\stackrel{\·}{\mathrm{\ρ}}-{\mathrm{\ω}}_{tb}\×({\mathrm{\ω}}_{tb}\×\mathrm{\ρ})-\frac{\mathrm{\μ}}{r_t^3}\[\mathrm{\ρ}-3\frac{{r_{tb}}^T{\mathrm{\ρr}}_{tb}}{r_t^2}\]+f---\left(1\right)$

其中，μ是地球引力常量；f是轨控推力；ω_tb、是目标本体相对惯性空间的旋转角速度和角加速度；r_t是航天器轨道高度；r_tb是航天器绝对位置矢量在目标本体系中的投影，记为

$r_{tb}=C_{tb}^o{\left[\begin{array}{ccc}0& 0& r_t\end{array}\right]}^T---\left(2\right)$

这里是轨道坐标系到目标本体系的转换矩阵；

为便于实现最优控制，将式(1)进一步整理为状态空间的形式

$\stackrel{\·}{X}=A\left(X\right)+BU---\left(3\right)$

式中

$X=\left[\begin{array}{c}\mathrm{\ρ}\\ \stackrel{\·}{\mathrm{\ρ}}\end{array}\right]={\left[\begin{array}{cccccc}x& y& z& \stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right]}^T,A\left(X\right)=\left[\begin{array}{cc}{0}_3& I_3\\ -{{\stackrel{\·}{\mathrm{\ω}}}_{tb}}^{\×}-{{\mathrm{\ω}}_{tb}}^{\×}{{\mathrm{\ω}}_{tb}}^{\×}-\frac{\mathrm{\μ}}{r_t^3}S\left(C_{tb}^o\right)& -2{{\mathrm{\ω}}_{tb}}^{\×}\end{array}\right]X,$

$B=\left[\begin{array}{c}{0}_3\\ \frac{1}{m_c}{I}_3\end{array}\right],U={\left[\begin{array}{ccc}{u}_x& u_y& u_z\end{array}\right]}^T$ (4)

其中，x,y,z和分别为相对位置矢量在目标本体系下的三轴分量；I₃是单位矩阵；m_c是追踪星质量；u_x,u_y,u_z分别为轨控推力在目标本体系下的三轴分量；记作

$S\left(C_{tb}^o\right)=\left[\begin{array}{ccc}1-3{C_{13}}^2& -3C_{13}{C}_{23}& -3C_{13}{C}_{33}\\ -3C_{13}{C}_{23}& 1-3{C_{23}}^2& -3C_{23}{C}_{33}\\ -3C_{13}{C}_{33}& -3C_{23}{C}_{33}& 1-3{C_{33}}^2\end{array}\right]---\left(5\right)$

而C₁₃,C₂₃,C₃₃是转换矩阵的元；

另外，由于追踪星推力器输出受限，控制力U需满足条件u_min≤U≤u_max；

步骤三、根据航天器的结构、几何构型设计安全飞行区域，以有效避开星体上可能发生碰撞的部位；

安全区域约束是为保证超近距离接近过程的安全性所设定的一个飞行区域，要求两个航天器的质心距离大于二者半径之和，表示为

S＝{M|r_c∈M,|r_c(t)-r_t(t)|≥D_min}(6)

这里r_c和r_t分别表示两个航天器的位置矢量，D_min即允许的最小安全距离；根据所接近的目标，即失效卫星的状态不同(如步骤一所示)，D_min的计算方法不同；

情况一、对于步骤一中情况一所给出的目标“球”包络体模型，显然追踪星与目标之间的最小安全距离是一个固定值

D_min＝d_s＝a_tb+a_c(7)

其中a_tb和a_c分别为两个包络球的半径；

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式

h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_s(8)

式中[x,y,z]是两个航天器的相对位置矢量；

情况二、对于步骤一中情况二所给出的目标“球+椭球”包络体模型，追踪星与目标星本体之间的最小安全距离为D_min＝d_s(式(7))；然而由于帆板包络体的非球形特性，追踪星与目标帆板之间的最小安全距离与二者的相对方位有关；对此，本发明将两个航天器的三维构型投影到二维平面，通过求解平面圆与椭圆相切问题得到二者之间的最小距离；

投影平面由两航天器质心位置确定；记追踪星质心在目标本体系中的坐标为[x_c,y_c,z_c]，则该平面定义为

$\frac{z_c}{y_c}y-z=0---\left(9\right)$

记目标帆板椭球包络体的半长轴为a,b,c，该椭球投影到上述平面所得的椭圆半长轴为a′,b′，表示为

$a^{\′}=a,b^{\′}=\sqrt{\frac{1+{\left(\frac{z_c}{y_c}\right)}^2}{\frac{1}{b^2}+{\left(\frac{z_c}{{\mathrm{cy}}_c}\right)}^2}}---\left(10\right)$

结合数学几何知识，推导出平面圆与椭圆圆心之间最小距离为

$d_e=\sqrt{\frac{q^2-1}{\mathrm{\δ}}{(a_c+\frac{b(1+\mathrm{\δ})}{q})}^2+(1-\frac{q^2-1}{\mathrm{\δ}}){(a_c+\frac{b}{q})}^2}---\left(11\right)$

这里δ与帆板椭球包络体尺寸有关，记为

$\mathrm{\δ}=\frac{a^{\′2}}{b^{\′2}}-1---\left(12\right)$

q是额外引入的变量，定义为

$q=\sqrt{(1+{\mathrm{\δsin}}^2\mathrm{\ψ})}---\left(13\right)$

式中ψ定义为椭圆上切点的外法线方向向量与椭圆长轴方向向量之间夹角；

d_e也就是追踪星球形包络体和目标帆板椭球包络体之间最小距离，至此就得到追踪星与目标帆板之间的最小安全距离D_min＝d_e；

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式

h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_i(i＝s,e)(14)

即要求两航天器之间的相对距离同时大于追踪星与目标星本体之间的最小安全距离和追踪星与目标帆板之间的最小安全距离；

情况三、对于步骤一中情况三所给出的目标“球+半椭球”包络体模型，追踪星与目标星本体之间的最小安全距离为D_min＝d_s(式(7))；追踪星与目标的单侧帆板之间的最小安全距离与情况二同理，记为D_min＝d_e；

为避免航天器之间发生碰撞，追踪星路径约束条件定义为如下形式；若目标只有+x方向帆板，则追踪星路径约束条件为

x≥0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_i(i＝s,e)

x＜0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_s(15)

若目标只有-x方向帆板，则追踪星路径约束条件为

x≤0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_i(i＝s,e)

x＞0时,h＝(x²+y²+z²-D_min ²)≥0,D_min＝d_s(16)

另外，为避开目标，即失效卫星上的天线(长为l)，增加一项路径约束条件，即

a_tb≤z≤a_tb+l时，x²+y²＞a_c ²

a_tb+l＜z≤a_tb+l+a_c时，x²+y²+(z-a_tb-l)²＞a_c ²(17)

步骤四、考虑导航测量误差造成的位置不确定性，结合碰撞概率问题进一步扩大追踪星的禁飞区域；

在近距离交会任务中，导航测量误差是不可忽视的一个要素；为此，在步骤三的基础上，又引入了误差椭球来表示位置不确定性，用以定义更加安全的飞行区域；

目标航天器的位置不确定性矩阵记为

$Cov=\left[\begin{array}{ccc}{{\mathrm{\σ}}^2}_x& & \\ & {{\mathrm{\σ}}^2}_y& \\ & & {{\mathrm{\σ}}^2}_z\end{array}\right]---\left(18\right)$

其中σ_i(i＝x,y,z)为坐标轴向的方差；

在位置测量中，通常假定目标位置坐标是服从正态分布的；如果用Δr表示航天器上某点的位置增量，那么该点位置在三维空间内的正态分布概率密度为

$pdf=\frac{\exp \[-\frac{1}{2}\left({\mathrm{\Δr}}^T{\mathrm{Cov}}^{-1}\mathrm{\Δ}r\right)\]}{{\left(2\mathrm{\π}\right)}^{\frac{3}{2}}\sqrt{\left|Cov\right|}}---\left(19\right)$

由此找到三维正态分布空间内概率密度相等的点，即

Δr^TCov^-1Δr＝k²(20)

其中k是放大因子；上式也是一个相似椭球族表达式，又可写作

$\frac{u^2}{{{\mathrm{\σ}}_x}^2}+\frac{v^2}{{{\mathrm{\σ}}_y}^2}+\frac{w^2}{{{\mathrm{\σ}}_z}^2}=k^2---\left(21\right)$

显然，每一个椭球对应一种概率；某点存在于误差椭球E_k内的概率可写作

$P=\∫\∫{\∫}_{E_k}\frac{exp\[-\frac{1}{2}\left(\frac{u^2}{{{\mathrm{\σ}}_x}^2}+\frac{v^2}{{{\mathrm{\σ}}_y}^2}+\frac{w^2}{{{\mathrm{\σ}}_z}^2}\right)\]}{{\left(2\mathrm{\π}\right)}^{\frac{3}{2}}{\mathrm{\σ}}_x{\mathrm{\σ}}_y{\mathrm{\σ}}_z}dudvdw---\left(22\right)$

设

可得

把上式中的指数函数展开成马克劳林级数后再将上式积分，得

$P=\frac{4}{\sqrt{2\mathrm{\π}}}(\frac{k^3}{6}-\frac{k^5}{20}+\frac{k^7}{112}-\frac{k^9}{864}+\mathrm{...})---\left(25\right)$

这样，根据允许的碰撞概率P_c,确定一个概率为P＝1-P_c的误差椭球,即确定放大因子k，就得到E_k椭球表面处于两航天器质心连线上的点到球心的距离

其中

$\mathrm{\θ}=\arg \tan \frac{y_c}{x_c}$

d_p就是考虑导航测量误差造成的位置不确定性时所需的最小安全距离的增量，用以进一步扩大追踪星的禁飞区域；

结合式(8)，(14)，(15)，(16)和(25)就得到了三种情况下考虑位置不确定性的追踪星路径约束条件；

情况一、目标，即失效卫星不带帆板，追踪星路径约束条件为

h＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0(27)

情况二、目标带有成对帆板，追踪星路径约束条件为

h₁＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

h₂＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_e(a,b,c,a_c,x,y,z)))≥0(28)

情况三、目标带有不成对帆板，若只有+x方向帆板，追踪星路径约束条件为

x≥0时,h₁＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

h₂＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_e(a,b,c,a_c,x,y,z)))≥0(29)

x＜0时,h＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

若只有-x方向帆板，追踪星路径约束条件为

x≤0时,h₁＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

h₂＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_e(a,b,c,a_c,x,y,z)))≥0(30)

x＞0时,h＝(x²+y²+z²-D_min ²(d_p(k,σ_x,σ_y,σ_z,x,y,z),d_s(a_tb,a_c)))≥0

步骤五、基于高斯伪谱法，根据追踪星路径约束条件在安全区域内规划防撞接近轨迹；

根据步骤三和四所得到的追踪星路径约束条件，生成一条燃料最优的安全路径；

本发明采用基于高斯伪谱法的最优控制数值计算方法，将有限时间tf内的连续优化控制问题转化为离散非线性规划问题进行求解；

下面将连续模型，包括步骤二中的动力学模型和步骤三、四中的约束条件，以及优化的性能指标等写成离散形式；

A.高斯点上的状态变量和控制变量为

X_1N,X_2N,X_3N,X_4N,X_5N,X_6N∈R^N,U_1N,U_2N,U_3N∈R^N

B.应用微分近似矩阵D∈R^N×N得到状态方程的积分形式

$\left\{\begin{array}{c}{\mathrm{DX}}_{1N}=\frac{t_f-t_0}{2}\·X_{4N}\\ {\mathrm{DX}}_{2N}=\frac{t_f-t_0}{2}\·X_{5N}\\ {\mathrm{DX}}_{3N}=\frac{t_f-t_0}{2}\·X_{6N}\\ {\mathrm{DX}}_{4N}=\frac{t_f-t_0}{2}\·(A_{X_{1N}}+1/m_c{U}_{1N})\\ {\mathrm{DX}}_{5N}=\frac{t_f-t_0}{2}\·(A_{X_{2N}}+1/m_c{U}_{2N})\\ {\mathrm{DX}}_{6N}=\frac{t_f-t_0}{2}\·(A_{X_{3N}}+1/m_c{U}_{3N})\end{array}\right.---\left(31\right)$

C.各高斯点i上的控制力约束条件为

u_1,min≤U_1Ni≤u_1,max

u_2,min≤U_2Ni≤u_2,max

u_3,min≤U_3Ni≤u_3,max

D.各高斯点i上的路径约束条件为

情况一、目标，即失效卫星不带帆板，路径约束条件为

h(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0(32)

情况二、目标带有成对帆板，路径约束条件为

h₁(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

h₂(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-(33)

D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_e(a,b,c,a_c,X_1Ni,X_2Ni,X_3Ni))≥0

情况三、目标带有不成对帆板，若只有+x方向帆板，路径约束条件为

X_1Ni≥0时,h₁(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

h₂(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-

D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_e(a,b,c,a_c,X_1Ni,X_2Ni,X_3Ni))≥0(34)

X_1Ni＜0时,h(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

若只有-x方向帆板，路径约束条件为

X_1Ni≤0时,h₁(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

h₂(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-

D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_e(a,b,c,a_c,X_1Ni,X_2Ni,X_3Ni))≥0(35)

X_1Ni＞0时,h(X_Ni)＝X² _1Ni+X² _2Ni+X² _3Ni-D_min ²(d_p(k,σ_x,σ_y,σ_z,X_1Ni,X_2Ni,X_3Ni),d_s(a_tb,a_c))≥0

另外，每种情况下都有一项附加的路径约束条件，即

a_tb≤X_3Ni≤a_tb+l时，X² _1Ni+X² _2Ni＞a_c ²

a_tb+l＜X_3Ni≤a_tb+l+a_c时，X² _1Ni+X² _2Ni+(X_3Ni-a_tb-l)²＞a_c ²(36)

E.由高斯求积公式得到末端约束条件

$\left\{\begin{array}{c}{X}_{1f}=X_{10}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{4N}\\ X_{2f}=X_{20}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{5N}\\ X_{3f}=X_{30}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·X_{6N}\\ X_{4f}=X_{40}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{1N}}+1/m_c{U}_{1N})\\ X_{5f}=X_{50}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{2N}}+1/m_c{U}_{2N})\\ X_{6f}=X_{60}+\frac{t_f-t_0}{2}\·{\mathrm{\ω}}^T\·(A_{X_{3N}}+1/m_c{U}_{3N})\end{array}\right.---\left(37\right)$

其中ω∈R^N是高斯积；

F.用高斯求积公式近似逼近性能函数

$J=min\left(\frac{1}{2}{u}^{T}u\right)---\left(38\right)$

得

$J=\frac{t_f-t_0}{2}{\mathrm{\ω}}^T\·\underset{i=1}{\overset{N}{\Σ}}(U_{1N,i}^2+U_{2N,i}^2+U_{3N,i}^2)---\left(39\right)$

步骤六、设计闭环控制器，得到满足真实动力学关系的最优防撞接近轨迹；

根据步骤五所提出的规划方法，基于高斯伪谱法求解出了一系列满足约束条件和动力学特性的离散点，但是在点与点之间的轨迹并不满足动力学要求，因而设计了“最优控制+位置补偿”控制器，即将优化的控制量作为动力学系统输入项，得到满足动力学特性的运动轨迹；再将实际轨迹与优化所得的标称轨迹之差作为控制变量，设计为PID补偿控制器，同时作用于星体上，得到满足真实动力学关系的最优轨迹。