基于多胞型线性微分包含的航天器相对姿态确定方法

摘要

本发明涉及一种基于多胞型线性微分包含的航天器相对姿态确定方法，属于航天控制技术领域。首先建立绕飞模型下，相对姿态确定系统的状态模型和测量模型；然后转化为PLDI系统描述模型，获取相对姿态估计的一步预测值以及相对姿态估计校正量；再利用相对姿态估计校正量对相对姿态的一步预测进行校正，获取相对姿态估计值。本方法将相对姿态确定非线性滤波器设计问题转换为线性鲁棒滤波器设计问题，不需要实时更新滤波增益，且在实现过程中不需要实时计算雅可比矩阵，极大地降低了计算量，有效地提高了相对姿态确定的实时性；适用于编队飞行、目标拦截、交会对接多工作模式航天器的相对姿态确定。

说明书

技术领域

本发明涉及一种基于多胞型线性微分包含的航天器相对姿态确定方法，属 于航天控制技术领域。

背景技术

相对导航是卫星编队飞行、空间交会对接、卫星在轨捕获及深空探测的关 键技术之一，目前用于相对导航的测量传感器主要有激光雷达、微波雷达、GPS、 类GPS导航和视觉测量敏感器等。激光雷达、微波雷达受到精度、带宽以及使 用范围的限制，并且雷达系统测量出的精确射程信息与解算相对导航的六自由 度信息有很大的不同，使其在相对导航中的运用受到限制。GPS是一种先进的 导航系统，能实现全球、全天候、连续和高精度星实时位置和姿态确定的，但 在相对位姿测量中，需要星间相互通讯，在非合作航天器相对位姿确定中的运 用具有一定的局限性。针对非合作航天器间的相对位姿确定问题，近年来提出 了立体视觉测量方法，通过提取目标结构上的一些特征点，基于点特征的视觉 测量技术实现相对位姿确定。

目前在姿态确定方面应用广泛的算法是扩展尔曼滤波算法(EKF)。EKF为迭 代算法，虽然简单易于实现，但由于在线性化过程中引入了模型误差，使得滤 波精度下降，甚至会出现滤波发散现象，且在滤波过程中需要实时求解雅克比 矩阵，计算复杂，尤其在高维非线性滤波求解中，很容易出现求解困难或错误 等问题。

微分包含理论通过采用全局线性化方法，将非线性系统转换为线性微分包 含系统(LDI)，原非线性系统为LDI的一个子集，虽具有一定的保守性，但由于 LDI的线性特性，为简化非线性系统控制及滤波算法的设计提供了新的思路。

发明内容

本发明的目的在于解决航天器绕飞过程中双目视觉测量相对姿态确定复杂 的问题，提出了一种基于多胞型微分包含(PLDI)的非线性相对姿态确定方法，该 方法将相对姿态确定的滤波误差系统采用PLDI系统模型来描述，从而将相对姿 态确定的非线性滤波问题转换为线性系统鲁棒滤波问题。

本发明的一种基于PLDI的航天器相对姿态确定方法，具体包括以下步骤：

步骤1，建立绕飞模型下，相对姿态确定系统的状态模型和测量模型；

绕飞模式下，从航天器的相对姿态动力学方程为：

$J_b\stackrel{·}{\omega }+J_b[\Omega \times ]\omega +J_b[v\times ]\omega +J_{b}W+[(\omega +\Omega +v)\times ]J_b(\omega +\Omega +v)=T_b---\left(1\right)$

其中，J_b∈R^3×3为从航天器惯性矩阵，T_b∈R³为作用于从航天器的控制力矩， [a×]表示以向量a=[a_x a_y a_z]^T生成的反对称矩阵：

$[a\times ]=\left(\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right)$

ω为在从航天器本体系B_b中，B_b相对于绕飞轨道坐标系F的角速度矢量（相对 姿态角速度）；Ω为在B_b中，F相对于相对运动参考系R的角速度矢量；v为在B_b中，R相对于惯性坐标系I的角速度矢量。

所述相对运动参考系R的原点为目标质心O_d、x_R轴由地心O指向目标质心 O_d，y_R在目标轨道平面内垂直于x_R轴并沿目标飞行方向；z_R轴满足右手定则。

式(1)中W的表达式如下所示：

$W=R\left(\sigma \right)[n_f\times ]R_z\left(\phi \right)R_x\left(\gamma \right)R_z\left(\psi \right)\left(\begin{array}{c}0\\ 0\\ n_d\end{array}\right)$

其中，σ表示由绕飞轨道坐标系F到从航天器本体系B_b姿态（相对姿态）的 修正罗德里格描述参数(MRP)，R(σ)的表达式为：

$R\left(\sigma \right)=I-\frac{4(1-{\left|\sigma \right|}^2)}{{(1+{\left|\sigma \right|}^2)}^2}[\sigma \times ]+\frac{8}{{(1+{\left|\sigma \right|}^2)}^2}{[\sigma \times ]}^2$

n_f、n_d分别为从航天器绕目标的平均轨道角速度以及目标的平均轨道角速度， (ψ,γ,φ)表示从相对运动参考系R到绕飞轨道坐标系F姿态按3-1-3旋转顺序描述 的欧拉角参数，R_z(ψ)、R_x(γ)、R_z(φ)分别表示三次旋转所对应的姿态转换矩阵， 表达式分别为：

$R_z\left(\psi \right)=\left(\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right),$$R_x\left(\gamma \right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \gamma & \sin \gamma \\ 0& -\sin \gamma & \cos \gamma \end{array}\right),$$R_z\left(\phi \right)=\left(\begin{array}{ccc}\cos \phi & \sin \phi & 0\\ -\sin \phi & \cos \phi & 0\\ 0& 0& 1\end{array}\right)$

相对姿态运动学方程为：

$\stackrel{·}{\sigma }=M\left(\sigma \right)\omega ---\left(2\right)$

其中，$M\left(\sigma \right)=\frac{(1-{\sigma }^T\sigma )I_3+2[\sigma \times ]+2{\mathrm{\sigma \sigma }}^T}{4}$

式(1)和(2)即为相对姿态确定系统的状态模型。

根据双目视觉测量原理，基于小孔成像和三角运算关系，建立双目视觉测 量方程为：

$z_i=\left(\begin{array}{c}{u}_{1i}\\ v_{1i}\\ u_{2i}\\ v_{2i}\end{array}\right)=\left(\begin{array}{c}\frac{f}{\mathrm{dx}}·\frac{y_{\mathrm{ci}}-\frac{b}{2}}{x_{\mathrm{ci}}}+u_0+n_{v1i}\\ \frac{f}{\mathrm{dy}}·\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}+v_0+n_{v2i}\\ \frac{f}{\mathrm{dx}}·\frac{y_{\mathrm{ci}}+\frac{b}{2}}{x_{\mathrm{ci}}}+u_0+n_{v3i}\\ \frac{f}{\mathrm{dy}}·\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}+v_0+n_{v4i}\end{array}\right)$

其中，z_i表示双目视觉测量系统观测到目标第i个特征点的测量信息，(u_1i,v_1i)、 (u_2i,v_2i)分别表示目标第i个特征点在左右摄像机像素坐标系中的坐标，(x_ci,y_ci,z_ci) 表示第i个特征点在从航天器本体系中的坐标，dx、dy分别表示每一像素在像 素平面坐标系X_u、Y_u轴上的物理尺寸，(u₀,v₀)表示像平面坐标原点在像素平面坐 标系中的坐标，b表示基线长度，f为摄像机的焦距，n_vi=[n_v1i n_v2i n_v3i n_v4i]^T表 示双目视觉测量系统的测量噪声。若观测到的目标特征点个数为N，则相对姿 态确定系统的测量模型为：

$z=\left(\begin{array}{c}{z}_1\\ z_2\\ .\\ .\\ .\\ z_N\end{array}\right)---\left(3\right)$

步骤2，建立相对姿态确定滤波误差系统的PLDI系统描述模型。

选取滤波误差系统的状态变量为Δx=[Δσ^T Δω^T]^T，其中，Δσ、Δω分别为 相对MRP和相对姿态角速度估计误差变量：

$\mathrm{\Delta \sigma }=\sigma \otimes {\hat{\sigma }}^{-1},$$\mathrm{\Delta \omega }=\omega -\hat{\omega }$

其中，分别表示相对MRP及相对姿态角速度的估计值。表示的逆， 等于其负值。表示MRP乘法。

采用泰勒级数展开方法得到相对姿态确定滤波误差系统的状态模型和测量 模型分别为：

$\Delta \stackrel{·}{x}=\left(\begin{array}{cc}-[\hat{\omega }\times ]& \frac{1}{4}{I}_{3\times 3}\\ 0_{3\times 3}& F_{\omega }\end{array}\right)\mathrm{\Delta x}+O_1\left(\mathrm{\Delta x}\right)+\left(\begin{array}{c}{0}_{3\times 3}\\ I_{3\times 3}\end{array}\right)n_{\omega }$

Δz=HΔx+O₂(Δx)+n

其中，$F_{\omega }=-J_b^{-1}[(\hat{\omega }+\hat{\Omega }+\hat{v})\times ]J_b+J_b^{-1}\left\{\right[J_b(\hat{\omega }+\hat{\Omega }+\hat{v})]\times \},$$n_{\omega }=J_b^{-1}(T_b-{\hat{T}}_b),$O₁(Δx)为泰 勒级数展开一阶以上量。分别表示在从航天器本体系中，绕飞轨道坐标系 相对于相对运动坐标系，以及目标轨道系相对于惯性系的角速度分量估计值；为控制力矩的估计值。O₁(Δx)、O₂(Δx)分别表示相对姿态确定滤波误差系统状态 模型和测量模型中的泰勒级数展开一阶以上量。$H={\left(\begin{array}{cccc}{H_1}^T& {H_2}^T& ...& {H_N}^T\end{array}\right)}^T,$$n={\left(\begin{array}{cccc}{n_{v1}}^T& {n_{v2}}^T& ...& {n_{\mathrm{vN}}}^T\end{array}\right)}^T,$H_i=H_1iH_2i，$H_{2i}=\left(\begin{array}{cc}4[{\hat{P}}_{\mathrm{ci}}\times ]& 0_{3\times 3}\end{array}\right),$表示第i个特征点在从 航天器本体系中的位置坐标估计值，i＝1,2,…,N。其中，H_1i的表达式为：

$H_{1i}=\left(\begin{array}{ccc}-\frac{f}{\mathrm{dx}}·\frac{{\hat{y}}_c-\frac{b}{2}}{{{\hat{x}}_c}^2}& \frac{f}{\mathrm{dx}}·\frac{1}{{\hat{x}}_c}& 0\\ -\frac{f}{\mathrm{dy}}·\frac{{\hat{z}}_c}{{{\hat{x}}_c}^2}& 0& \frac{f}{\mathrm{dy}}·\frac{1}{{\hat{x}}_c}\\ -\frac{f}{\mathrm{dx}}·\frac{{\hat{y}}_c+\frac{b}{2}}{{{\hat{x}}_c}^2}& \frac{f}{\mathrm{dx}}·\frac{1}{{\hat{x}}_c}& 0\\ -\frac{f}{\mathrm{dy}}·\frac{{\hat{z}}_c}{{{\hat{x}}_c}^2}& 0& \frac{f}{\mathrm{dy}}·\frac{1}{{\hat{x}}_c}\end{array}\right)$

采用参数边界值确定多胞型原理，得到离散化后的相对姿态确定滤波误差 系统的多胞型系统描述模型为：

Δx_k=AΔx_k-1+Bn_k，k-1                             (4)

Δz_k=CΔx_k+Dn_k，k-1

其中，Δx_k表示相对姿态确定滤波误差系统离散化后的k时刻状态。

$(A,C)\in \Omega \stackrel{\Delta }{=}\left\{(A,C)\right|(A,C)=\underset{j=1}{\overset{l}{\Sigma }}{\lambda }_j(A_j,C_j),0\le {\lambda }_j\le 1,\underset{j=1}{\overset{l}{\Sigma }}{\lambda }_j=1\},$(A_j,C_j)为多胞型系统的顶 点，l表示多胞型顶点的个数。$n_{k,k-1}={\left(\begin{array}{cc}{n_{\omega ,k-1}}^T& {n_k}^T\end{array}\right)}^T$表示离散化后的相对姿态确 定滤波误差系统的噪声，包含过程噪声和测量噪声，其中n_ω,k-1表示k-1时刻离 散化后的相对姿态确定滤波误差系统的过程噪声，n_k表示k时刻离散化后的相对 姿态确定滤波误差系统的测量噪声，$B=\left(\begin{array}{cc}{0}_{3\times 3}& 0_{3\times 4N}\\ I_{3\times 3}& 0_{3\times 4N}\end{array}\right),$D=[0_4N×3 I_4N×4N]。

步骤3，获取相对姿态估计的一步预测值。

根据步骤1得到的相对姿态确定系统状态模型，获取相对姿态估计的一步 预测值：

相对姿态角速度一步预测：

${\hat{\omega }}_{k/k-1}=\{{\hat{T}}_{b,k-1}-J_b^{-1}[({\hat{\omega }}_{k-1}+{\hat{\Omega }}_{k-1}+{\hat{v}}_{k-1})\times ]J_b({\hat{\omega }}_{k-1}+{\hat{\Omega }}_{k-1}+{\hat{v}}_{k-1})-[{\hat{\Omega }}_{k-1}\times ]{\hat{\omega }}_{k-1}-[\hat{v}\times ]{\omega }_{k-1}-W_{k-1}\}·\mathrm{\Delta T}+{\hat{\omega }}_{k-1}---\left(5\right)$

相对MRP一步预测：${\hat{\sigma }}_{k/k-1}=M\left({\hat{\sigma }}_{k-1}\right){\hat{\omega }}_{k-1}·\mathrm{\Delta T}+{\hat{\sigma }}_{k-1}---\left(6\right)$其中，分别为k时刻从航天器相对目标的相对角速度一步预测值， 以及相对MRP的一步预测值；为k-1时刻控制力矩估计值，实际中为控制 系统的力矩输出值，ΔT为采样时间间隔，为k-1时刻相对MRP估计值；分别为在航天器本体系中，k-1时刻的相对角速度、绕飞轨道坐标系相 对于相对运动坐标系的角速度分量估计值、目标轨道系相对于惯性系的角速度 分量估计值。

步骤4，获取相对姿态估计校正量。

根据步骤3得到的多胞型描述模型，利用基于LMI技术的鲁棒H2滤波原 理，得到相对姿态估计校正量为：

$\Delta {\hat{x}}_{\mathrm{m><mo>+</mo><mn>1</mn>}}=A_F\Delta {\hat{x}}_{\mathrm{m>}}+B_F\Delta z_k$$\left(7\right)$

$\Delta {\hat{x}}_k=C_F\Delta {\hat{x}}_{\mathrm{mk}}+D_F\Delta z_k$

其中，$\Delta {\hat{x}}_k={\left(\begin{array}{cc}\Delta {\hat{\sigma }}^T& \Delta {\hat{\omega }}^T\end{array}\right)}^T$为k时刻相对姿态估计校正量，为k时刻相对姿态 估计校正量的预测值，为求解的一个中间变量。滤波系数A_F、B_F、C_F、D_F的 计算公式为： $A_F=G_2^{-1}{S}_A,$$B_F=G_2^{-1}{S}_B,$C_F=S_C、D_F=S_D

其中，矩阵S_A、S_B、S_C、S_D、G₂通过求解下述优化问题得到：对于正定矩阵变 量P_11j、P_22i和矩阵变量P_12j、G₁₁、G₂₁、G₂、F₁₁、F₂₁、S_A、S_B、S_C、S_D，寻求滤 波估计误差协方差矩阵的最优上界Z，使得不等式(i)(ii)成立。

$\left(i\right)\left(\begin{array}{ccccc}{G}_{11}+G_{11}^T-P_{11j}& G_2+G_{21}^T-P_{12j}& {\psi }_{1j}& S_A-F_{21}^T& G_{11}{B}_j+S_B{D}_j\\ *& G_2+G_2^T-P_{22j}& {\psi }_{2j}& S_A-{\alpha }_2{G_2}^T& G_{21}{B}_j+S_B{D}_j\\ *& *& {\psi }_{3j}& {\psi }_{4j}& -F_{11}{B}_j-{\alpha }_1{S}_B{D}_j\\ *& *& *& P_{22i}-{\alpha }_2{S}_A-{\alpha }_2{S_A}^T& -F_{21}{B}_j-{\alpha }_2{S}_B{D}_j\\ *& *& *& *& I\end{array}\right)>0$

$\left(\mathrm{ii}\right)\left(\begin{array}{cccc}Z& I-S_D{C}_j& -S_C& -S_D{C}_j\\ *& P_{11j}& P_{12j}& 0\\ *& *& P_{22j}& 0\\ *& *& *& I\end{array}\right)>0$

其中，j＝1,2,…,l，松弛变量α₁、α₂为滤波器性能调节参数，

${\psi }_{1j}=G_{11}{A}_j+S_B{C}_j-F_{11}^T,$${\psi }_{2j}=G_{21}{A}_j+S_B{C}_j-{\alpha }_1{G_2}^T$

${\psi }_{3j}=P_{11j}-F_{11}{A}_j-{\alpha }_1{S}_B{C}_j-A_j^T{F}_{11}^T-{\alpha }_1{C}_j^T{S}_B^T$

${\psi }_{4j}=P_{12j}-{\alpha }_1{S}_A-A_j^T{F}_{21}^T-{\alpha }_2{C}_j^T{S}_B^T.$

步骤5，利用相对姿态估计校正量对相对姿态的一步预测进行校正，获取相 对姿态估计值。

相对角速度估计：${\hat{\omega }}_k={\hat{\omega }}_{k/k-1}+\Delta {\hat{\omega }}_k---\left(8\right)$

相对MRP估计：${\hat{\sigma }}_k=\Delta {\hat{\sigma }}_k\otimes {\hat{\sigma }}_{k/k-1}---\left(9\right)$

有益效果

本发明方法重点考虑绕飞模式下航天器相对姿态确定，基于PLDI技术，将 相对姿态确定滤波误差系统采用PLDIs模型来描述，从而将相对姿态确定非线 性滤波器设计问题转换为线性鲁棒滤波器设计问题，简化了相对姿态确定方法， 在此基础上，利用鲁棒H2滤波技术，获得相对姿态估计误差校正量，并对EKF 算法得到的相对姿态估计一步预测值进行校正，由于相对姿态估计误差校正量 的计算表达式中的滤波系数为常量，不需要实时更新滤波增益，且在实现过程 中不需要实时计算雅可比矩阵，极大地降低了计算量，有效地提高了相对姿态 确定的实时性；适用于编队飞行、目标拦截、交会对接多工作模式航天器的相 对姿态确定。

附图说明

图1为本发明的基于多胞型线性微分包含的航天器相对姿态确定方法流程 图；

图2为具体实施方式中地心赤道惯性坐标系；

图3为具体实施方式中相对运动坐标系；

图4为具体实施方式中绕飞平面坐标系；

图5为具体实施方式中绕飞轨道坐标系；

图6为具体实施方式中从航天器绕目标相对运动示意图；

图7为具体实施方式中相对运动坐标系到从航天器绕飞轨道坐标系的转换 关系示意图；

图8为具体实施方式中双目视觉测量原理图；

图9为具体实施方式中相对MRP估计及估计误差曲线；

图10为具体实施方式中相对姿态角速度估计及估计误差曲线；

图11为具体实施方式中稳态相对MRP估计误差曲线；

图12为具体实施方式中稳态相对姿态角速度估计误差曲线。

具体实施方式

为了更好地说明本发明的目的和优点，下面结合附图和实施例对本发明作 进一步说明。

首先定义坐标系

1、坐标系定义

(1)J2000地心赤道惯性坐标系I

简称为惯性坐标系或者惯性系，定义如下：原点位于地球质心O，x_I由地心 O指向某一历元的春分点γ；z_I垂直历元赤道面，由地心指向地球北极；y_I服从 右手定则，如图2所示。

(2)相对运动坐标系R

定义如下：原点为目标质心O_d、x_D轴由地心O指向目标质心O_d，y_R在目标 轨道平面内垂直于x_R轴并沿目标飞行方向；z_R轴满足右手定则，垂直轨道平面， 指向轨道正法线方向，如图3所示。

(3)绕飞平面坐标系P

定义如下：原点为目标质心O_d，x_P沿绕飞平面和相对运动参考系R的x_Ry_R平 面的交线，并指向相对运动参考系中z_R轴坐标由负变正的点；z_P为相对运动的 正法向；y_P服从右手定则，如图4所示。

(4)绕飞轨道坐标系F

定义如下：原点为从航天器质心O_b，x_F由拦截器质心O_b指向目标质心O_d；z_F为绕飞平面的正法向；y_F服从右手定则，如图5所示。

(5)本体坐标系B

航天器本体坐标系是与航天器本体固连的坐标系，航天器的姿态即本体系 相对于参考坐标系的方位或指向，本体系的定义与航天器需要完成的空间任务 和航天器本身的结构等有关。

2、相对姿态确定系统状态模型和测量模型

绕飞模式下，从航天器相对目标的运动如图6所示。被动绕飞模式下，无 外力作用时，从航天器相对目标的轨道动力学方程即Hill方程：

$\left(\begin{array}{c}\stackrel{··}{x}-2n_d\stackrel{·}{y}-3n_d^{2}x=0\\ \stackrel{··}{y}+2n_d\stackrel{·}{x}=0\\ \stackrel{··}{z}+n_d^{2}z=0\end{array}\right)---\left(10\right)$

其中，n_d为目标的平均轨道角速度。

Hill方程的解析解为：

$x\left(t\right)=\frac{{\stackrel{·}{x}}_0}{n_d}\sin \left(n_{d}t\right)-\frac{1}{n_d}(2{\stackrel{·}{y}}_0+3n_d{x}_0)\cos \left(n_{d}t\right)+(\frac{2{\stackrel{·}{y}}_0}{n_d}+4x_0)$

$y\left(t\right)=\frac{2{\stackrel{·}{x}}_0}{n_d}\cos \left(n_{d}t\right)+\frac{1}{n_d}(4{\stackrel{·}{y}}_0+6n_d{x}_0)\sin \left(n_{d}t\right)+(y_0-2\frac{{\stackrel{·}{x}}_0}{n_d})-(3{\stackrel{·}{y}}_0+6n_d{x}_0)t---\left(11\right)$

$z\left(t\right)=z_0\cos \left(n_{d}t\right)+\frac{{\stackrel{·}{z}}_0}{n_d}\sin \left(n_{d}t\right)$

并将轨道稳定条件式：

${\stackrel{·}{y}}_0=-2n_d{x}_0$

$y_0=\frac{2{\stackrel{·}{x}}_0}{n_d}---\left(12\right)$

代入式(11)，则有：

$x\left(t\right)=\frac{{\stackrel{·}{x}}_0}{n_d}\sin \left(n_{d}t\right)+x_0\cos \left(n_{d}t\right)$

$y\left(t\right)=\frac{2{\stackrel{·}{x}}_0}{n_d}\cos \left(n_{d}t\right)-2x_0\sin \left(n_{d}t\right)---\left(13\right)$

$z\left(t\right)=z_0\cos \left(n_{d}t\right)+\frac{{\stackrel{·}{z}}_0}{n_d}\sin \left(n_{d}t\right)$

这时相对运动轨迹是一个中心在相对运动参考系R原点的椭圆。联立（13） 式中的三个方程，并消除t，可得：

c₁x+c₂y+c₃z=0    (14)

其中，

$c_1=2n_d^2{x}_0{z}_0+2{\stackrel{·}{x}}_0{\stackrel{·}{z}}_0$

$c_2=-n_d{x}_0{\stackrel{·}{z}}_0+n_d{\stackrel{·}{x}}_0{z}_0$

$c_3=-2n_d^2{x_0}^2-2{\stackrel{·}{x}}_0^2$

相对运动坐标系R到从航天器绕飞轨道坐标系F之间的转换关系如图7所 示，相对运动坐标系按照3-1的旋转顺序依次逆时针旋转ψ、γ与绕飞平面坐标 系P重合，绕飞平面坐标系P绕z_P轴逆时针旋转φ即可与绕飞轨道坐标系F重合， 即相对运动坐标系R到从航天器绕飞轨道坐标系F的姿态转换矩阵R_FR为：

R_FR=R_FPR_PR    (15)

其中，R_FP、R_PR分别为绕飞平面坐标系到绕飞轨道坐标系、相对运动坐标系到绕 飞平面坐标系的姿态转换矩阵：

$R_{\mathrm{PR}}=R_x\left(\gamma \right)R_z\left(\psi \right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \gamma & \sin \gamma \\ 0& -\sin \gamma & \cos \gamma \end{array}\right)\left(\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right)---\left(16\right)$

$R_{\mathrm{FP}}=R_z\left(\phi \right)=\left(\begin{array}{ccc}\cos \phi & \sin \phi & 0\\ -\sin \phi & \cos \phi & 0\\ 0& 0& 1\end{array}\right)---\left(17\right)$

其中，γ和ψ的确定公式为：

$\left(\begin{array}{c}\sin \gamma =C_1/C_2\\ \cos \gamma =c_3/C_2\end{array}\right),$$\left(\begin{array}{c}\sin \psi =c_1/C_1\\ \cos \psi =-c_2/C_1\end{array}\right)---\left(18\right)$

其中，φ的表达式可由从航天器在绕飞平面内的x_P、 y_P轴坐标(x_BP,y_BP)计算得到：

$\sin \phi =-\frac{y_{\mathrm{BP}}}{\sqrt{{x_{\mathrm{BP}}}^2+{y_{\mathrm{BP}}}^2}}$

$\cos \phi =-\frac{x_P}{\sqrt{{x_{\mathrm{BP}}}^2+{y_{\mathrm{BP}}}^2}}$

从航天器的惯性姿态动力学方程如下所示：

$J_b{\stackrel{·}{\omega }}_{\mathrm{bi}}+[{\omega }_{\mathrm{bi}}\times ]J_b{\omega }_{\mathrm{bi}}=T_b---\left(20\right)$

其中，J_b∈R^3×3为从航天器惯性矩阵，T_b∈R³为作用于从航天器的控制力矩，[a×] 表示以向量a=[a_x a_y a_z]^T生成的反对称矩阵，定义如下：

$[a\times ]=\left(\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right)$

ω_bi∈R³为从航天器本体系相对于惯性系的角速度，可进行如下分解：

[ω_bi]_B=[ω_e]_B+[ω_fr]_B+[ω_ri]_B

其中，[ω_e]_B为从航天器本体系B_b相对于绕飞轨道坐标系的角速度矢量也即相 对姿态角速度在B_b中坐标表示；[ω_fr]_B为绕飞轨道坐标系F相对于相对运动参考 系R的角速度矢量在B_b中坐标表示；[ω_ri]_B为相对运动参考系R相对于惯性坐标 系I的角速度矢量在B_b中坐标表示。

记ω=ω_e，结合轨道信息计算可得：

ω_bi=ω+Ω+v    (21)

其中，

$\Omega =R\left(\sigma \right)\left(\begin{array}{c}0\\ 0\\ n_f\end{array}\right),$$v=R\left(\sigma \right)R_{\mathrm{FP}}{R}_{\mathrm{PR}}\left(\begin{array}{c}0\\ 0\\ n_d\end{array}\right)$

R(σ)、R_FP、R_PR分别为从航天器绕飞轨道坐标系到从航天器本体系、绕飞平面坐 标系到绕飞轨道坐标系、相对运动坐标系到绕飞平面坐标系的姿态转换矩阵。 n_f、n_d分别为从航天器绕目标的平均轨道角速度和目标的平均轨道角速度，其 中σ表示由绕飞轨道坐标系到从航天器本体系姿态的修正罗德里格参数 (MRP)，则R(σ)的计算表达式为：

$R\left(\sigma \right)=I-\frac{4(1-{\left|\sigma \right|}^2)}{{(1+{\left|\sigma \right|}^2)}^2}[\sigma \times ]+\frac{8}{{(1+{\left|\sigma \right|}^2)}^2}{[\sigma \times ]}^2$

将式(21)代入式(20)，可得绕飞模式下从航天器的相对姿态动力学方程为：

$J_b\stackrel{·}{\omega }+J_b[\Omega \times ]\omega +J_b[v\times ]\omega +J_{b}W+[(\omega +\Omega +v)\times ]J_b(\omega +\Omega +v)=T_b$

其中，

$W=R\left(\sigma \right)[n_f\times ]R_z\left(\phi \right)R_x\left(\gamma \right)R_z\left(\psi \right)\left(\begin{array}{c}0\\ 0\\ n_d\end{array}\right)$

相应地，相对姿态运动学方程为：

$\stackrel{·}{\sigma }=M\left(\sigma \right)\omega$

其中，

$M\left(\sigma \right)=\frac{(1-{\sigma }^T\sigma )I_3+2[\sigma \times ]+2{\mathrm{\sigma \sigma }}^T}{4}$

双目视觉测量原理如图8所示。根据小孔成像原理及三角运算关系可得像 素化后的双目视觉测量方程为：

$z_i=\left(\begin{array}{c}{u}_{1i}\\ v_{1i}\\ u_{2i}\\ v_{2i}\end{array}\right)=\left(\begin{array}{c}\frac{f}{\mathrm{dx}}·\frac{y_{\mathrm{ci}}-\frac{b}{2}}{x_{\mathrm{ci}}}+u_0+n_{v1i}\\ \frac{f}{\mathrm{dy}}·\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}+v_0+n_{v2i}\\ \frac{f}{\mathrm{dx}}·\frac{y_{\mathrm{ci}}+\frac{b}{2}}{x_{\mathrm{ci}}}+u_0+n_{v3i}\\ \frac{f}{\mathrm{dy}}·\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}+v_0+n_{v4i}\end{array}\right)---\left(22\right)$

其中，z_i表示双目视觉测量系统观测到目标第i个特征点的测量信息，(u_1i,v_1i)、 (u_2i,v_2i)分别表示目标第i个特征点在左右摄像机像素坐标系中的坐标表示， (x_ci，y_ci,z_ci)表示第i个特征点在从航天器本体系中的坐标表示，dx、dy分别表示 每一像素在像素平面坐标系X_u、Y_u轴上的物理尺寸，(u₀,v₀)表示像平面坐标原点 在像素平面坐标系中的坐标表示，b表示基线长度，f为摄像机的焦距， n_vi=[n_v1i n_v2i n_v3i n_v4i]^T表示双目视觉测量系统的测量噪声，假设为高斯噪声。

根据双目视觉测量值解算出相对姿态信息，至少需要从观测到的目标目标 图像中提取出三个特征点，为保证相对姿态测量信息的有效性和冗余性，本实 施例中选取识别四个特征点，N=4。

$z=\left(\begin{array}{c}{z}_1\\ z_2\\ z_3\\ z_4\end{array}\right)---\left(23\right)$

3、相对姿态确定算法

(1)相对姿态确定误差系统的PLDI模型描述

设分别为从航天器本体系相对于绕飞轨道坐标系的姿态和姿态角速度 的估计值，定义误差相对MRP为估计相对MRP到真实相对MRP的旋转

$R\left(\sigma \right)=R\left(\mathrm{\Delta \sigma }\right)R\left(\hat{\sigma }\right)---\left(24\right)$

$\sigma =\mathrm{\Delta \sigma }\otimes \hat{\sigma }---\left(25\right)$

式中R(Δσ)、分别为估计坐标系到从航天器本体系的转换矩阵和绕飞轨道坐 标系到估计坐标系的转换矩阵，表示MRP乘法。

定义误差相对姿态角速度

$\mathrm{\Delta \omega }=\omega -\hat{\omega }$

相对角速度和相对MRP估计满足：

${\hat{J}}_b\stackrel{·}{\hat{\omega }}+J_b[\hat{\Omega }\times ]\hat{\omega }+J_b[\hat{v}\times ]\hat{\omega }+J_b\hat{W}+[(\hat{\omega }+\hat{\Omega }+\hat{v})\times ]J_b{(\hat{\omega }+\hat{\Omega }+\hat{v})=\hat{T}}_b---\left(26\right)$

$\stackrel{\stackrel{·}{^}}{\sigma }=M\left(\hat{\sigma }\right)\hat{\omega }---\left(27\right)$

将式(26)、式(27)分别与式(1)、式(2)相减，并进行泰勒级数展开可得相对姿态确 定滤波误差系统的状态方程为：

$\Delta \stackrel{·}{x}=\mathrm{F\Delta x}+O_1\left(\mathrm{\Delta x}\right)+\left(\begin{array}{c}{0}_{3\times 3}\\ I_{3\times 3}\end{array}\right)n_{\omega }---\left(28\right)$

其中，O₁(Δx)为泰勒级数展开一阶以上量，

$F=\left(\begin{array}{cc}-[\hat{\omega }\times ]& \frac{1}{4}{I}_{3\times 3}\\ 0_{3\times 3}& F_{\omega }\end{array}\right),$$F_{\omega }=-J_b^{-1}[(\hat{\omega }+\hat{\Omega }+\hat{v})\times ]J_b+J_b^{-1}\left\{\right[J_b(\hat{\omega }+\hat{\Omega }+\hat{v})]\times \},$

双目视觉测量系统测量的预测方程为：

$z=\left(\begin{array}{c}{\hat{z}}_1\\ {\hat{z}}_2\\ {\hat{z}}_3\\ {\hat{z}}_4\end{array}\right)---\left(29\right)$

其中，

$z_i=\left(\begin{array}{c}{\hat{u}}_{1i}\\ {\hat{v}}_{1i}\\ {\hat{u}}_{2i}\\ {\hat{v}}_{2i}\end{array}\right)=\left(\begin{array}{c}\frac{f}{\mathrm{dx}}·\frac{{\hat{y}}_{\mathrm{ci}}-\frac{b}{2}}{{\hat{x}}_{\mathrm{ci}}}+u_0\\ \frac{f}{\mathrm{dy}}·\frac{{\hat{z}}_{\mathrm{ci}}}{{\hat{x}}_{\mathrm{ci}}}+v_0\\ \frac{f}{\mathrm{dx}}·\frac{{\hat{y}}_{\mathrm{ci}}+\frac{b}{2}}{{\hat{x}}_{\mathrm{ci}}}+u_0\\ \frac{f}{\mathrm{dy}}·\frac{{\hat{z}}_{\mathrm{ci}}}{{\hat{x}}_{\mathrm{ci}}}+v_0\end{array}\right),$$\left(\begin{array}{c}{\hat{x}}_{\mathrm{ci}}\\ {\hat{y}}_{\mathrm{ci}}\\ {\hat{z}}_{\mathrm{ci}}\end{array}\right)=R\left(\hat{\sigma }\right)R_{\mathrm{FP}}{R}_{\mathrm{PR}}\{\left(\begin{array}{c}{x}_{\mathrm{ri}}\\ y_{\mathrm{ri}}\\ z_{\mathrm{ri}}\end{array}\right)-\left(\begin{array}{c}x\\ y\\ z\end{array}\right)\},i=\mathrm{1,2,3,4}$

[x y z]^T为从航天器在相对运动坐标系中的位置坐标，由式(13)得到。

[x_ri y_ri z_ri]^T为第i（i＝1,2,3,4）个特征点在相对运动坐标系中的坐标。

双目视觉测量系统残差方程为：

$\mathrm{\Delta z}=z-\hat{z}$

将上式进行泰勒级数展开可得：

Δz＝HΔx+O₂(Δx)+n    (30)

其中，$\mathrm{\Delta z}={\left(\begin{array}{cccc}\Delta {z_1}^T& \Delta {z_2}^T& \Delta {z_3}^T& \Delta {z_4}^T\end{array}\right)}^T,$O₂(Δx)为测量残差方程的一阶以上量，n为 测量噪声$n={\left(\begin{array}{cccc}{n_{v1}}^T& {n_{v2}}^T& {n_{v3}}^T& {n_{v4}}^T\end{array}\right)}^T,$$H={\left(\begin{array}{cccc}{H_1}^T& {H_2}^T& {H_3}^T& {H_4}^T\end{array}\right)}^T,$其中H_i=H_1iH_2i，

$H_{2i}=\left(\begin{array}{cc}4[{\hat{P}}_{\mathrm{ci}}\times ]& 0_{3\times 3}\end{array}\right),$${\hat{P}}_{\mathrm{ci}}=\left(\begin{array}{c}{\hat{x}}_{\mathrm{ci}}\\ {\hat{y}}_{\mathrm{ci}}\\ {\hat{z}}_{\mathrm{ci}}\end{array}\right),$$H_{1i}=\left(\begin{array}{ccc}-\frac{f}{\mathrm{dx}}·\frac{{\hat{y}}_c-\frac{b}{2}}{{{\hat{x}}_c}^2}& \frac{f}{\mathrm{dx}}·\frac{1}{{\hat{x}}_c}& 0\\ -\frac{f}{\mathrm{dy}}·\frac{{\hat{z}}_c}{{{\hat{x}}_c}^2}& 0& \frac{f}{\mathrm{dy}}·\frac{1}{{\hat{x}}_c}\\ -\frac{f}{\mathrm{dx}}·\frac{{\hat{y}}_c+\frac{b}{2}}{{{\hat{x}}_c}^2}& \frac{f}{\mathrm{dx}}·\frac{1}{{\hat{x}}_c}& 0\\ -\frac{f}{\mathrm{dy}}·\frac{{\hat{z}}_c}{{{\hat{x}}_c}^2}& 0& \frac{f}{\mathrm{dy}}·\frac{1}{{\hat{x}}_c}\end{array}\right),i=\mathrm{1,2,3,4}$

由于相对姿态角速度和相对MRP为有限区间变化状态，故有[F^T G^T]^T∈Ω， 且Ω为实紧集，因此，由式(23)和式(29)描述的相对姿态确定滤波误差模型离散 化后，可采用下述PLDIs模型来描述，且离散化后的相对姿态确定误差系统被 包含在PLDIs中

Δx_k=Ax_k-1+Bn_k，k-1

Δz_k=Cx_k+Dn_k，k-1

其中，$(A,C)=\underset{j=1}{\overset{l}{\Sigma }}{\lambda }_j(A_j,C_j),$$\underset{j=1}{\overset{l}{\Sigma }}{\lambda }_j=1,$0≤λ_j≤1，$n_{k,k-1}={\left(\begin{array}{cc}{n_{\omega ,k-1}}^T& {n_k}^T\end{array}\right)}^T,$D=[0_16×3 I_16×16] $B=\left(\begin{array}{cc}{0}_{3\times 3}& 0_{3\times 16}\\ I_{3\times 3}& 0_{3\times 16}\end{array}\right).$

(2)相对姿态确定算法

针对离散化后的相对姿态确定滤波误差系统的PLDIs描述模型，结合离散 型线性鲁棒H2滤波理论，可得相对姿态估计误差校正量的动态方程为:

$\Delta {\hat{x}}_{\mathrm{m><mo>+</mo><mn>1</mn>}}=A_F\Delta {\hat{x}}_{\mathrm{m>}}+B_F\Delta z_k$

$\Delta {\hat{x}}_k=C_F\Delta {\hat{x}}_{\mathrm{mk}}+D_F\Delta z_k$

其中，$\Delta {\hat{x}}_k={\left(\begin{array}{cc}\Delta {\hat{\sigma }}^T& \Delta {\hat{\omega }}^T\end{array}\right)}^T$为k时刻相对姿态估计校正量，为k时刻相对姿态 估计校正量的预测值，为求解的一个中间变量。滤波系数A_F、B_F、C_F、D_F的 计算公式为： $A_F=G_2^{-1}{S}_A,$$B_F=G_2^{-1}{S}_B,$C_F=S_C、D_F=S_D

其中，S_A、S_B、S_C、S_D、G₂可通过求解下述优化问题得到：对于正定矩阵变量 P_11j、P_22i和矩阵变量P_12j、G₁₁、G₂₁、G₂、F₁₁、F₂₁、S_A、S_B、S_C、S_D，寻求滤波 估计误差协方差矩阵的最优上界Z，使得下述不等式成立。

$\left(i\right)\left(\begin{array}{ccccc}{G}_{11}+G_{11}^T-P_{11j}& G_2+G_{21}^T-P_{12j}& {\psi }_{1j}& S_A-F_{21}^T& G_{11}{B}_j+S_B{D}_j\\ *& G_2+G_2^T-P_{22j}& {\psi }_{2j}& S_A-{\alpha }_2{G_2}^T& G_{21}{B}_j+S_B{D}_j\\ *& *& {\psi }_{3j}& {\psi }_{4j}& -F_{11}{B}_j-{\alpha }_1{S}_B{D}_j\\ *& *& *& P_{22i}-{\alpha }_2{S}_A-{\alpha }_2{S_A}^T& -F_{21}{B}_j-{\alpha }_2{S}_B{D}_j\\ *& *& *& *& I\end{array}\right)>0$

$\left(\mathrm{ii}\right)\left(\begin{array}{cccc}Z& I-S_D{C}_j& -S_C& -S_D{C}_j\\ *& P_{11j}& P_{12j}& 0\\ *& *& P_{22j}& 0\\ *& *& *& I\end{array}\right)>0$

其中，j＝1,2,…,l，

${\psi }_{1j}=G_{11}{A}_j+S_B{C}_j-F_{11}^T,$${\psi }_{2j}=G_{21}{A}_j+S_B{C}_j-{\alpha }_1{G_2}^T$

${\psi }_{3j}=P_{11j}-F_{11}{A}_j-{\alpha }_1{S}_B{C}_j-A_j^T{F}_{11}^T-{\alpha }_1{C}_j^T{S}_B^T$

${\psi }_{4j}=P_{12j}-{\alpha }_1{S}_A-A_j^T{F}_{21}^T-{\alpha }_2{C}_j^T{S}_B^T$

松弛变量α₁、α₂为滤波器性能调节参数，实际求解过程中令α₁=α₂=1。

由此可得相对姿态确定滤波器方程为：

相对角速度一步预测：

${\hat{\omega }}_{k/k-1}=\{{\hat{T}}_{b,k-1}-J_b^{-1}[({\hat{\omega }}_{k-1}+{\hat{\Omega }}_{k-1}+{\hat{v}}_{k-1})\times ]J_b({\hat{\omega }}_{k-1}+{\hat{\Omega }}_{k-1}+{\hat{v}}_{k-1})-[{\hat{\Omega }}_{k-1}\times ]{\hat{\omega }}_{k-1}-[\hat{v}\times ]{\omega }_{k-1}-W_{k-1}\}·\mathrm{\Delta T}+{\hat{\omega }}_{k-1}$

相对MRP一步预测：${\hat{\sigma }}_{k/k-1}=M\left({\hat{\sigma }}_{k-1}\right){\hat{\omega }}_{k-1}·\mathrm{\Delta T}+{\hat{\sigma }}_{k-1}$

相对角速度估计：${\hat{\omega }}_k={\hat{\omega }}_{k/k-1}+\Delta {\hat{\omega }}_k$

相对MRP估计：${\hat{\sigma }}_k=\Delta {\hat{\sigma }}_k\otimes {\hat{\sigma }}_{k/k-1}$

其中，分别为k时刻航天器相对目标的相对角速度和相对MRP的一 步预测值，为控制力矩估计值，实际中取值为控制系统的力矩输出值，ΔT为 采样时间间隔，为k-1时刻相对MRP估计值，分别为k-1时 刻，相对角速度、绕飞轨道坐标系相对于相对运动坐标系和目标轨道系相对于 惯性系的角速度在航天器本体系中的分量的估计值，

${\hat{\Omega }}_{k-1}=R\left({\hat{\sigma }}_{k-1}\right)\left(\begin{array}{c}0\\ 0\\ n_f\end{array}\right),$${\hat{v}}_{k-1}=R\left({\hat{\sigma }}_{k-1}\right)R_{\mathrm{FP}}{R}_{\mathrm{PR}}\left(\begin{array}{c}0\\ 0\\ n_d\end{array}\right)$

相对姿态估计校正量${\Delta \hat{x}}_k={\left(\begin{array}{cc}\Delta {{\hat{\sigma }}_k}^T& \Delta {{\hat{\omega }}_k}^T\end{array}\right)}^T$由式(7)计算得到。

4、仿真实现

此实施例中，目标的轨道高度为6899807km，从航天器绕目标飞行的半径 为200m，初始相对姿态角为[2°3°5°]，初始相对角速度为[0 0 0]^Trad/s，相 对位置为[-100 0 -173.21]^Tm，相对速度为[0 0.22 0]^Tm/s。双目两摄像机的焦 距都为0.015m，基线长度为0.2m，像素大小为10μm×10μm，测量误差为0.2像 素。从航天器受到的干扰力矩标准差为10^-3N·m，采样时间为0.2秒。

本实施例得到的相对姿态MRP和相对姿态角速度的估计曲线如图9~图10 所示，按照从左到右从上到下的顺序，图9和图10中各子图的纵轴分别表示飞 行器滚动、俯仰、偏航轴的相对姿态MRP和相对姿态角速度的真实值，飞行器 滚动、俯仰、偏航轴的相对姿态MRP和相对姿态角速度的估计值，飞行器俯仰、 偏航轴的相对姿态MRP和相对姿态角速度的估计误差。相对姿态MRP和相对 姿态角速度的稳态估计曲线图11~图12所示。其中稳态相对MRP折合成3-2-1 欧拉角。按照从上到下的顺序，图11中各子图的纵轴分别表示飞行器的相对滚 动角、俯仰角、偏航角的稳态估计误差，图12中各子图的纵轴分别表示飞行器 的滚动轴、俯仰轴、偏航轴的相对姿态角速度稳态估计误差，相对姿态MRP和 相对姿态角速度估计误差曲线随时间逐渐收敛到0附近，说明了本方法的正确 性。稳态时相对姿态角、姿态角速度的最大绝对估计误差分别为：0.0285deg、 1.2323×10^-4deg/s，估计精度较高，满足一般工程需求，但由于相对姿态估计误 差校正量的计算公式中滤波系数为常值，且不需要实时计算雅可比矩阵，该方 法比基于EKF的相对姿态确定方法具有更好的实时性。