近年来人类航天事业蓬勃发展，对空间机器人的感知能力提出更高的要求。目前世界各国执行的空间任务主要限于合作目标之间^[1]。而非合作目标由于无法信息交互、缺少人工标识、表面结构未知的特点，给废弃航天器的清理、失效卫星的在轨维修等空间任务带来挑战^[2-3]，在非合作目标的空间任务中，目标星旋转，动力学参数的有效估计是后续操作的基础，具有重要的实际意义^[4]。

视觉传感器具有体积小、功耗低、信息量大的优势，是空间观测的重要方式。近年来国内外学者对于非合作目标参数的视觉估计进行广泛的研究。文[5-9]假设已知目标三维模型，通过最近迭代法算法或神经网络等方法估计相对位姿；文[10]针对太阳帆板三角形几何特征进行基于双目相机的位姿观测；文[11]利用圆形转接环，通过特征提取和匹配，求出目标实时位姿；文[12]利用模型对称性消除冗余信息，降低参数估计算法计算量。以上方法均需要已知目标的三维模型或一定的外形特点，对用于形状未知或不规则的航天器有所局限。

为执行接触式空间任务，除角速度等旋转运动学参数外，也需要估计出非合作目标动力学参数。文[13]采用双目视觉追踪目标的一系列点，在多种模态下采用迭代的扩展Kalman滤波估计惯量和位姿参数。文[14]通过观测目标黏附物体前后运动状态的变化，结合Kalman滤波法，估计惯量参数。文[15]以单目相机观测的目标表面特征像素位置作为输入量，通过扩展Kalman滤波估计目标运动状态与惯量比，前提是保持对图像中多个特征点的稳定跟踪。

截至目前，利用视觉传感器估计非合作目标运动学和动力学参数依旧是一个研究难点，实际情况中目标星形状多是未知的，且随着光照条件变化以及目标星的旋转，特征点的稳定识别与跟踪难以维持。对于动力学参数的估计，常用的扩展Kalman滤波等迭代方法有计算量小的优点，但需要对动力学方程进行线性化处理，且缺少全局优化，影响算法收敛效率和结果精度。在实验方面，已有研究主要采用数值仿真或图片仿真，其中数值仿真使用目标星上一组特征点的投影位置数值作为输入量，此理想条件在实际中难以实现；图片仿真多采用匀速自转卫星模型图片作为输入量，没有考虑卫星复杂的自由旋转。

实际上，卫星等空间目标如NASA的探险者号、哈勃望远镜等多为柱状，其横向惯量有对称性，可以对动力学模型进行简化，使得参数估计更快速、更准确。实时定位与建图(simultaneous localization and mapping, SLAM)是一种估计运动相机位姿同时建立环境地图的视觉技术，相比运动恢复结构(structure from motion, SFM)，它侧重于位姿的高精度估计和算法的实时性，更符合卫星参数估计的需求。目前较成熟的视觉SLAM方法多应用于街道、走廊等大型场景内运动，利用回环检测减小误差^[16]。而对于旋转卫星，SLAM位姿估计精度很难满足航天任务需求，须利用动力学模型等手段进行优化。

针对惯量对称且无动力失效目标的状态估计问题，本文首先根据无力矩动力学方程建立双锥体几何模型，提出几何参数与卫星参数间的解析关系；然后利用实时定位与建图的前端算法，得到完全未知卫星与相机的相对位姿序列，对其中包含的高频噪声问题，采用双层滤波法获得平滑角速度序列；最后用奇异值分解法做角速度向量的双锥体拟合，求出卫星旋转运动学参数、动力学参数。由于运动学几何模型与参数表达式是解析形式的，可获得全局最优参数，因此可以实现更快的收敛速度和更高的精度。仿真实验中设置多组不同惯量值，利用物理仿真软件获得自由旋转运动对应的图片序列，实验结果验证了该算法的有效性和鲁棒性。

1 相对运动描述

为描述星载相机与目标星之间的相对运动，首先定义以下坐标系并列出相应假设，如图 1所示。

地心惯性坐标系E：原点在地球质心，z_E轴指向地球北极，x_E轴指向春分点。

目标星本体惯性坐标系A：原点在目标星质心，x、y、z轴分别与3个惯量主轴重合，其中z轴对应的惯量最大。

相机坐标系B：原点在相机光心，z_B轴沿光轴方向，x_B和y_B轴平行于成像平面。

轨道坐标系C：原点固连在目标星刚体上，x_C轴指向地球质心，y_C轴沿轨道切线方向。

假设1：相对于轨道周期，卫星观测任务时间很短，可近似认为相机和目标在同方向上匀速运动。

假设2：在卫星近距离观测阶段，相机与目标近似处于同一轨道，且相对速度很小。因此以相机坐标系B为参考系，近似认为目标星质心静止。

假设3：失效卫星受其他星球引力较小，在短时间内对运动状态的影响可以忽略。因此在相机参考系下，目标星作定点无力矩旋转。

基于以上假设，以目标星本体坐标系A为参考系，相机在以目标星质心为球心的球面上运动，如图 2所示。根据相对运动关系，相机瞬时角速度为轨道惯性参考系下目标星瞬时角速度的相反矢量。

2 目标星参数模型

本节根据惯量对称刚体的动力学方程和双锥体模型，建立旋转运动学参数、双锥体模型几何参数、动力学参数3组等价参数的关系式。

2.1 自由旋转动力学模型

设I_x、I_y、I_z为目标星主惯量，ω_x、ω_y、ω_z为目标星相对轨道参考系的角速度在本体坐标系下分量，已知外力矩M=0。对于横向惯量相等的目标星，有I_x=I_y=I_s，其中I_s表示横向惯量。Euler动力学方程可简化为

$ \left\{ {\begin{array}{*{20}{l}} {{{\dot \omega }_x} + {\mathit{\Omega }_0}{\omega _y} = 0, }\\ {{{\dot \omega }_y} - {\mathit{\Omega }_0}{\omega _x} = 0,}\\ {{{\dot \omega }_z} = 0}. \end{array}} \right. $

(1)

得到角速度解析解为

$ \mathit{\boldsymbol{\omega }} = \left[ {\begin{array}{*{20}{c}} {{\omega _x}}\\ {{\omega _y}}\\ {{\omega _z}} \end{array}} \right] = \left[ {\begin{array}{*{20}{l}} {{\omega _0}\cos \left( {{\mathit{\Omega }_0}t + \varphi } \right)}\\ {{\omega _0}\sin \left( {{\mathit{\Omega }_0}t + \varphi } \right)}\\ {{\omega _1}} \end{array}} \right]. $

(2)

其中：Ω₀=(I_z/I_s－1)ω₁, ω₀、φ、ω₁分别为角速度横向分量初始值、角速度纵向分量初始值、进动相位角初始值。可知本体坐标系下角速度向量在一个圆锥体上，称为本体锥^[17]，如图 3所示。

根据刚体角动量公式，角速度可表示为

$ \mathit{\boldsymbol{\omega }} = \mathit{\boldsymbol{H}}/{I_{\rm{s}}} - {\mathit{\Omega }_0}{\mathit{\boldsymbol{e}}_z}. $

(3)

因此角动量H、角速度ω、z轴单位向量e_z位于同一平面内，横向惯量对称的目标星的运动可以分解为H方向上的进动与e_z上的自转运动，H与e_z夹角为章动角，设为α。在轨道惯性坐标系下，目标星的角速度向量也形成一个锥体，称为空间锥，本体锥与空间锥相切，本体锥沿空间锥表面作无滑滚动^[17]。双锥体模型如图 4所示。设置参数为锥体斜边长即角速度向量模长、空间锥锥角α^space、空间锥高h^space、本体锥锥角α^self。该几何模型的参数可以唯一确定目标星的运动状态。

2.2 目标星运动学与动力学参数表达式

本节建立几何模型参数与目标星参数的解析关系。首先根据双锥体模型和角速度向量分解，可以求出目标星旋转运动参数：

$ \alpha=\left\{\begin{array}{ll} \frac{\left(\alpha^{\text {self }}+\alpha^{\text {space }}\right)}{2}, & I_{z} \leqslant I_{\mathrm{s}} ; \\ {\rm{ \mathsf{ π} }}-\frac{\left(\alpha^{\text {self }}-\alpha^{\text {space }}\right)}{2}, & I_{z}>I_{\mathrm{s}} ; \end{array}\right. $

(4)

$ \mathit{\Omega } = \left\{ {\begin{array}{*{20}{l}} {l \cdot \frac{{\sin \left( {\frac{{{\alpha ^{{\rm{self }}}}}}{2}} \right)}}{{\sin \left( {\frac{{{\alpha ^{{\rm{self }}}} + {\alpha ^{{\rm{space }}}}}}{2}} \right)}}, }&{{I_z} \le {I_s};}\\ {l \cdot \frac{{\sin \left( {\frac{{{\rm{ \mathsf{ π} }} - {\alpha ^{{\rm{self }}}}}}{2}} \right)}}{{\sin \left( {\frac{{{\alpha ^{{\rm{self }}}} - {\alpha ^{{\rm{ppace }}}}}}{2}} \right)}}, }&{{I_z} > {I_s};} \end{array}} \right. $

(5)

$ \omega=\left\{\begin{array}{ll} l \cdot \frac{\sin \left(\frac{\alpha^{\text {space }}}{2}\right)}{\sin \left(\frac{\alpha^{\text {self }}+\alpha^{\text {space }}}{2}\right)}, & I_{z} \leqslant I_{\mathrm{s}} ; \\ l \cdot \frac{\sin \left(\frac{\alpha^{\text {space }}}{2}\right)}{\sin \left(\frac{\alpha^{\text {self }}-\alpha^{\text {space }}}{2}\right)}, & I_{z}>I_{\mathrm{s}} . \end{array}\right. $

(6)

以上3个运动学参数与几何模型参数是等价的，可以独立描述目标星的运动学、动力学信息。

关于惯量参数，由于缺少外力矩作用，理论上只能确定惯量之比，无法得到独立的惯量值。根据式(7)中刚体动力学方程的积分形式即角动量守恒和转动动能守恒方程，可以得到一种惯量参数表示形式。

$ \left\{\begin{array}{l} I_{\mathrm{s}}^{2}\left(\omega_{x}^{2}+\omega_{y}^{2}\right)+I_{z}^{2} \omega_{z}^{2}=H^{2}, \\ I_{\mathrm{s}}\left(\omega_{x}^{2}+\omega_{y}^{2}\right)+I_{z}^{2} \omega_{z}^{2}=2 T. \end{array}\right. $

(7)

其中：H表示角动量大小，T表示转动动能，均为常量。设横向惯量与角动量之比r_s=I_s/H，纵向惯量与角动量之比r_z=I_z/H，这2个参数是独立的，没有物理约束条件。设转动动能与角动量之比r_T=T/H，3个动力学参数同样可以完全描述目标星的无力矩旋转运动。

根据几何模型，ω与H的单位向量点积等于h^space。而根据动能守恒公式，ω与H点积为二倍转动动能。于是有

$ \begin{array}{c} \boldsymbol{H} \cdot \boldsymbol{\omega}=\left[I_{x} \omega_{x}, I_{y} \omega_{y}, I_{z} \omega_{z}\right] \cdot\left[\omega_{x}, \omega_{y}, \omega_{z}\right]^{\mathrm{T}}= \\ 2 T=H h^{\text {space }} . \end{array} $

(8)

可求出参数r_T为

$ r_{\mathrm{T}}=\frac{h^{\mathrm{space}}}{2}=l \frac{\cos \left(\frac{\alpha^{\mathrm{space}}}{2}\right)}{2} . $

(9)

根据模型可知角速度分量为

$ \left\{\begin{array}{l} \omega_{x}^{2}+\omega_{y}^{2}=l^{2} \cos ^{2}\left(\frac{\alpha^{\text {self }}}{2}\right), \\ \omega_{z}^{2}=l^{2} \sin ^{2}\left(\frac{\alpha^{\text {self }}}{2}\right). \end{array}\right. $

(10)

将式(9)、(10)代入式(7)，解方程组可得惯量参数的表达式：

$ r_{\mathrm{s}}=\left\{\begin{array}{ll} \frac{\sin \left(\frac{\alpha^{\text {self }}+\alpha^{\text {space }}}{2}\right)}{l \sin \left(\frac{\alpha^{\text {self }}}{2}\right)}, & I_{z} \leqslant I_{\mathrm{s}} ; \\ \frac{\sin \left(\frac{\alpha^{\text {self }}-\alpha^{\text {space }}}{2}\right)}{l \sin \left(\frac{\alpha^{\text {self }}}{2}\right)}, & I_{z}>I_{\mathrm{s}} ; \end{array}\right. $

(11)

$ r_{z}=\left\{\begin{array}{ll} \frac{\cos \left(\frac{\alpha^{\text {self }}+\alpha^{\text {space }}}{2}\right)}{l \cos \left(\frac{\alpha^{\text {self }}}{2}\right)}, & I_{z} \leqslant I_{\mathrm{s}} ; \\ \frac{\cos \left(\frac{\alpha^{\text {self }}-\alpha^{\text {space }}}{2}\right)}{l \cos \left(\frac{\alpha^{\text {self }}}{2}\right)}, & I_{z}>I_{\mathrm{s}} . \end{array}\right. $

(12)

式(9)、(11)、(12)为目标动力学参数表达式。

3 参数估计算法

本节给出未知目标的参数估计算法，算法流程如图 5所示。

步骤1采用开源的SLAM前端算法(本文采用ORB_SLAM算法^[18])输出图片对应的的相对姿态序列。根据2节的假设条件和相对运动情况，相机相对于目标作球面绕飞运动。相机方向相对于卫星轨道是静止的，因此只需对输出的旋转矩阵取逆就能得到目标星相对于相机的相对姿态。

SLAM得到带有时间戳t_n的四元数序列Q_n，参考系为第1帧图片的相机坐标系。姿态序列含有噪声，不能直接用于参数估计。

步骤2对姿态序列进行Savitzky-Golay滤波处理以初步减小Q_n所含高频噪声对后续角速度计算的影响。

由于Q_n分量含有较多断点，且各分量物理意义不同，不便于平滑处理，将Q_n转为Euler角序列E_n。分别对E_n的3个分量进行Savitzky-Golay滤波，窗口大小设为40。因为后续步骤要通过差分求角速度，所以采用这种局部窗口式的多项式滤波。将滤波结果转换为相机相对于目标星旋转矩阵序列R_0n，求逆得到目标星相对于轨道坐标系的旋转矩阵R_n=R_0n^-1。图 6展示Euler角分量的滤波效果，图 7展示滤波对于所求角速度的影响。

步骤3差分法求出本体坐标系和轨道坐标系下的角速度序列并用Gauss滤波法处理。

已知角速度公式：

$ \frac{\mathrm{d} \boldsymbol{R}(t)}{\mathrm{d} t}=\boldsymbol{S}(\boldsymbol{\omega}) \cdot \boldsymbol{R}(t) $

(13)

其中$ \boldsymbol{S}(\boldsymbol{\omega})=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{array}\right]$。

利用差分法可求出轨道惯性坐标系下目标星角速度向量ω_1n:

$ \frac{\left(\boldsymbol{R}_{n+1}-\boldsymbol{R}_{n}\right)\left(\boldsymbol{R}_{n}^{-1}+\boldsymbol{R}_{n+1}^{-1}\right)}{2}=\boldsymbol{S}\left(\boldsymbol{\omega}_{1 n}\right). $

(14)

左乘旋转矩阵的逆得到本体坐标系下的目标星角速度向量ω_2n:

$ \begin{equation}\boldsymbol{\omega}_{2 n}=\boldsymbol{R}_{n}^{-1} \cdot \boldsymbol{\omega}_{1 n}\end{equation}. $

(15)

为进一步提高随后的圆锥拟合精度，采用Gauss滤波法对角速度分量做平滑处理，如图 8所示。因为理想情况的角速度分量是三角函数，所以采用Gauss滤波法对其进行全局的滤波处理。

步骤4拟合双锥体模型，估计几何参数。

以空间锥为例，设目标星角速度向量序列坐标为(x_n, y_n, z_n)，设平均坐标为(x, y, z)，没有误差的情况下角速度向量均处于空间锥底面上，设该平面为

$ \begin{equation}a(x-\bar{x})+b(y-\bar{y})+c(z-\bar{z})=0\end{equation}. $

(16)

设列向量N=[a b c]^T，为空间锥底面的平面方程系数。

设矩阵$\boldsymbol{A}=\left[\begin{array}{ccc} x_{1}-\bar{x} & y_{1}-\bar{y} & z_{1}-\bar{z} \\ x_{2}-\bar{x} & y_{2}-\bar{y} & z_{2}-\bar{z} \\ \vdots & \vdots & \vdots \\ x_{n}-\bar{x} & y_{n}-\bar{y} & z_{n}-\bar{z} \end{array}\right]. $

则目标函数为min ‖AN‖²，即所有点到平面距离的平方和，约束条件为‖N‖=1。

对A做奇异值分解A=UDV^T，其中U和V为酉矩阵，D为对角阵，则有

$ \|\boldsymbol{A} \boldsymbol{N}\|=\left\|\boldsymbol{U} \boldsymbol{D} \boldsymbol{V}^{\mathrm{T}} \boldsymbol{N}\right\|=\left\|\boldsymbol{D} \boldsymbol{V}^{\mathrm{T}} \boldsymbol{N}\right\|. $

(17)

不妨设D的最后一个对角元素为最小奇异值，目标函数取得最小值当且仅当

$ \boldsymbol{V}^{\mathrm{T}} \boldsymbol{N}=\left[\begin{array}{llll} 0 & 0 & 0 & 1 \end{array}\right]^{\mathrm{T}} . $

(18)

因此，最小二乘意义下的平面系数为

$ \boldsymbol{N}=[\boldsymbol{V}(\mathit{n}, 1) \quad \boldsymbol{V}(\mathit{n} , 2) \quad \boldsymbol{V}(\mathit{n}, 3)]^{\mathrm{T}} . $

(19)

以原点到平面距离为空间锥高，以角速度向量长度均值为母线长，即可求出空间锥各参数，本体锥同理。实际求得的角速度向量点与平面拟合情况如图 9所示。

步骤5求目标星参数。

根据两锥体轴之间夹角和锥角可以判定两锥体内切或外切关系，由此判断I_z和I_s大小关系。将步骤4的几何参数结果代入式(4)、(5)、(6)、(9)、(11)、(12)得到目标星运动学和动力学参数估计结果。

4 仿真实验验证

本节利用物理仿真软件Unity3D模拟空间卫星的旋转运动和光照条件，生成图片数据集，验证参数估计算法的性能。

首先设置13组不同的进动角速度、自转角速度以及章动角，根据2节可以推导出运动学参数与动力学参数的关系式，从而求出对应的目标星动力学参数真实值。编写Unity3D的C#脚本控制目标星做指定运动，建立平行光环境，渲染出仿真图片序列。相机参数中，焦距设为40 mm，传感器尺寸设为70 mm×70 mm，图像分辨率设为720×720像素，采集频率设为30 Hz。图 10展示不同位姿的仿真图片，实验目标星运动学和动力学参数真实值分别如表 1和2所示。

将图片序列输入ORB_SLAM算法的前端，可得相机相对目标的原始姿态序列，如图 11所示。

根据本文提出的算法，利用n帧图片的姿态序列估计模型几何参数，在n∈[3, 2 000]内测试算法。测试硬件平台为Intel Core i7-8565U处理器，8 GB RAM，软件平台为MATLAB和C++，其中ORB_SLAM计算一帧的时间约为0.03 s；参数估计算法处理1 000和2 000帧数据分别耗时0.016和0.061 s，主要计算量为步骤4中奇异值分解，算法整体计算量较小满足空间应用的实时性要求。

13组实验的估计值相对误差随帧数变化趋势如图 12、13、14所示。空间锥锥角，本体锥锥角相对误差在1 000帧(图片采集过程第33 s)时均快速收敛至5%以内，在2 000帧时收敛至0.5%以内；公共母线长度相对误差在1 000帧时均收敛至0.1%以内，在2 000帧时收敛至0.05%以内。收敛时间短于扩展Kalman滤波等方法的数百秒^[14-15]，证明了本算法的有效性和快速性。

用收敛后的锥体几何参数求出各运动学和动力学参数，相对误差见表 3，数据表明各项参数的估计误差均在0.9%以内，展示了参数估计算法的准确性和鲁棒性。

5 结论

本文提出一种惯量对称的非合作目标参数的单目估计方法。首先针对惯量对称的特点，建立目标的动力学双锥体模型；其次推导出该模型几何参数与目标运动学、动力学参数的关系式；最后设计基于视觉SLAM的参数估计算法，针对SLAM前端运行结果的噪声设计了进行多重滤波方法，并通过奇异值分解法拟合双锥体模型，得到全局最优的各参数估计值。利用Unity3D采集仿真图片，在C++和MATLAB平台验证算法可行性，结果表明该算法可以高效、精确地估计目标的各个参数，误差均在0.9%以内。

本文算法适用于外观完全未知的惯量对称目标，其中多重滤波方法有效地降低了SLAM位姿输出的一阶、二阶导数误差；双锥体模型的拟合从全局上进行优化，进一步降低估计误差，同时保证较低的计算量；参数估计精度可以满足一般空间任务的要求，对空间任务的感知观测环节具有一定的借鉴意义。

实验过程中发现，光照条件的变化对结果有一定影响，如何减小光照影响有待研究；同时，对于非惯量对称物体的动力学建模和参数估计也有待进一步探讨。