CN104406598B - 一种基于虚拟滑模控制的非合作航天器姿态估计方法 - Google Patents

Abstract

本发明公开了一种基于虚拟滑模控制的非合作航天器姿态估计方法，属于非合作航天器导航技术领域，包括一种基于Lyapunov原理的虚拟控制的滑模控制器；通过立体视觉系统得到的目标卫星绝对姿态作为控制目标；根据目标卫星的运动特点建立其虚拟卫星运动模型；将虚拟卫星的动力学模型作为控制对象，得到虚拟卫星的姿态参量；以虚拟卫星估计的姿态参量和立体视觉系统得到的目标卫星绝对姿态作为控制输入，经虚拟滑模控制器计算出虚拟转动力矩作用在虚拟卫星的运动模型上，实现利用虚拟控制的滑模控制器对目标卫星姿态参数的估计。本发明计算量少；在状态量初始误差较大或存在系统误差时依然能较快的收敛速度和较高精度，因而能够满足高性能导航系统的需求。

Description

一种基于虚拟滑模控制的非合作航天器姿态估计方法

技术领域

本发明涉及一种基于虚拟滑模控制的卫星姿态跟踪方法，属于非合作航天器导航技术领域。

背景技术

在很近的距离上，航天器相对非合作航天器自由、灵巧、可靠的飞行能力具有重大的科学和应用价值，已经引起世界各国的重视。

所谓非合作目标，即目标卫星本身未安装对接图案标识和对接口/捕获机钩，或姿轨不可控而在空间自由翻滚。不能提供有效位置、姿态等导航信息，且自身不能进行控制。空间非合作目标卫星的自主接近、悬停与捕获操作需要解决的一项关键技术就是非合作目标卫星的自主接近、悬停与在轨捕获实时的相对位置和相对姿态。利用小卫星对非合作卫星的在轨操作现已取得了初步进展，例如，美国的XSS-11试验项目和SUMO计划，德国DLR开展的DEOS项目以及欧洲的TECSAS项目。

人类航天活动的不断发展与深化，航天器的临近空间操作已经成为了未来发展趋势之一，所以航天器尤其是非合作航天器之间的相对导航即相对位置相对姿态的测量是实现该类任务的前提与关键。立体视觉技术的研究使得计算机具有通过一幅或多幅图像认识周围环境信息的能力。该系统仅通过对目标特征点的识别就能够提供主从星间的相对状态，期间不需要任何外界的实时动态信息。针对上述非合作航天器，立体视觉系统因其低能耗、高性价比等特性已经成为常用的3D传感器。

滑模变结构控制是一种特殊的非线性控制，具有快速响应、对参数变化及扰动不灵敏、无需系统在线辨识，物理实现简单等优点。由于不依赖于外部扰动和内部参数的摄动，滑模变结构控制在航天器的姿态控制上得到了广泛的应用。

目前，针对基于立体视觉的近距离非合作航天器的姿态估计国内一些学者取得了一些成果，在仅获得主星的外观特征和一些静态参数的情况下，能自主进行相对位置和姿态的确定，但也存在一些较理想的假设，比如推导的模型讨论的是目标卫星的本体系与轨道系重合的情况；而且经典的滤波方法对模型精确性要求都较高，在初始误差较大时，存在估计效果急剧下降和滤波器收敛速度缓慢的问题，且在迭代过程中都需更新状态协方差矩阵，计算量较大。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明提供一种基于虚拟滑模控制的非合作航天器姿态估计方法，该方法针对非合作目标特点，在滑模控制方法的基础上提出了一种虚拟控制的思想，将非合作卫星的姿态估计转换成虚拟控制问题，继承滑模控制的优势，在减少计算量的前提下，能够在对象存在系统误差时保证估计的鲁棒性，克服初始误差较大时传统方法的局限性，对非合作卫星在轨操作的可靠实现提供技术支持。

为实现上述目的，本发明采用的技术方案为：一种基于虚拟滑模控制的非合作航天器姿态估计方法，包括一种基于Lyapunov原理的虚拟控制的滑模控制器；将立体视觉系统输出的实时观测数据转化为目标卫星绝对姿态作为滑模控制器的控制目标；根据目标卫星是非合作卫星，而非合作卫星失控后最终近似围绕一个定轴做旋转的运动特点，建立虚拟卫星运动模型；滑模控制器将虚拟卫星的动力学模型作为控制对象，得到虚拟卫星的姿态参量；以虚拟卫星估计的姿态参量和立体视觉系统得到的目标卫星绝对姿态作为滑模控制器的控制输入，滑模控制器根据控制输入计算出虚拟转动力矩并作用在虚拟卫星运动模型上；使得虚拟卫星的姿态与观测姿态同步，实现利用虚拟控制的滑模控制器对目标卫星姿态参数的估计。

具体包括以下步骤：

步骤一、通过追踪星上的立体视觉系统获取目标卫星相对追踪星的相对位姿信息；然后根据该目标卫星相对追踪星的相对位姿信息结合目标卫星的结构信息，将目标卫星相对追踪星相对姿态信息转化为目标卫星相对惯性系的绝对姿态信息；然后通过目标卫星的绝对姿态信息差分推算出目标卫星的观测角速度；

步骤二，由于目标卫星是非合作卫星，根据非合作卫星最终近似围绕一个定轴做旋转的运动特点，建立用来描述虚拟卫星运动的虚拟卫星模型，估计虚拟卫星的姿态参数；所述虚拟卫星模型包括目标卫星姿态运动学模型和姿态动力学模型；所述虚拟卫星的姿态参量包括目标卫星的估计四元数和估计角速度；

步骤三、根据步骤一中得到的目标卫星的绝对姿态信息和步骤二中得到的目标卫星姿态的估计四元数，建立四元数误差模型，从而得到目标卫星的误差四元数；根据步骤一中得到的目标卫星的观测角速度、步骤二中得到的目标卫星姿态动力学模型以及目标卫星的误差四元数，建立角速度误差模型，从而得到角速度误差；

步骤四、将虚拟反馈思想引入滑模控制器，设计基于Lyapunov原理的虚拟滑模控制器，将步骤三得到的目标卫星的误差四元数和角速度误差代入到虚拟滑模控制器，计算出虚拟控制量转动力矩“作用”在姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，修正模型估计值，使得虚拟卫星的姿态与观测值同步，最终实现对目标卫星姿态参数的估计。

所述步骤一中通过立体视觉系统观测的数据求解得出目标卫星的绝对姿态信息的方法包括以下步骤：

步骤一一，通过追踪星上的立体视觉系统得到同一目标卫星的两幅图像数据，采用surf算法对两幅图像上的卫星表面特征点进行提取和匹配，并输出同一特征点在两幅图像上相应的像面坐标；

步骤一二，基于视差原理对像面上成功配对的特征点进行3D重构，利用平行式双目立体视觉深度恢复公式，获得特征点相对于追踪星本体坐标系的空间坐标P_ci；

步骤一三，从步骤一二获取的空间坐标中选取三个空间坐标S₁,S₂,S₃，建立特征点坐标系，以特征点S₂为坐标原点O_S，向量S₂S₁为Z_S轴，以垂直于特征点平面且背离目标卫星质心的方向为X_S轴，建立特征点坐标系O_S-X_SY_SZ_S，同时可以确定三个特征点在特征点坐标系的坐标P_si；

步骤一四，根据追踪星本体坐标系和特征点坐标系，建立特征点在特征点坐标系与追踪星本体系间的转换关系公式：P_ci＝RP_si+T，其中R和T分别为两坐标系的姿态转换矩阵和平移矢量；同时根据目标卫星的结构，确定目标卫星质心在特征点坐标系中的坐标，从而推算出目标卫星相对追踪星的相对姿态；

步骤一五，根据追踪星的姿态信息，将步骤一四得到的相对姿态转化为目标卫星的绝对姿态。

所述步骤二中目标卫星姿态的估计四元数的计算方法，包括以下步骤：

步骤二一，根据步骤二中根据非合作卫星最终近似围绕一个定轴做旋转的运动特点，建立以四元数描述的目标卫星姿态运动学模型和姿态动力学模型；

步骤二二，初始时，通过观测值计算的观测角速度代替估计角速度代入目标卫星姿态运动学模型，得到目标卫星姿态的估计四元数；其他时刻时，将估计角速度代入目标卫星姿态运动学模型，推算目标卫星姿态的估计四元数。

所述步骤四中通过虚拟滑模控制器来“控制”虚拟卫星的姿态与观测值同步，包括以下步骤：

步骤四一，滑模面设计：根据步骤三中得到的目标卫星的误差四元数和角速度误差定义切换函数，并校验滑模面滑模发生条件；

步骤四二，趋近律设计：根据滑模可达性条件，设计滑模趋近律；

步骤四三，反馈力矩求解：首先将步骤三中得到的目标卫星的误差四元数和角速度误差代入步骤四一中的切换函数，然后结合步骤四二设计的趋近律，计算出滑模控制器的虚拟滑模控制反馈值，该反馈值即为转动力矩，将该反馈值“作用”在姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，最终达到虚拟卫星的姿态与观测姿态跟踪同步。

所述步骤四一中的切换函数为：s＝ω_e+kq_e13；其中：s＝[s_x,s_y,s_z]^T，k＝diag(k₁,k₂,k₃),k_i>0，s_x,s_y,s_z分别为切换函数在三个轴的分量，ω_e为目标卫星估计角速度与观测角速度误差，q_e13为q_e的矢量部分，q_e为误差四元数。

所述滑模发生条件为

所述步骤四二中趋近律包括指数项-Ks和等速趋近项-εsgn(s)，即：

本发明提供的基于虚拟滑模控制的非合作航天器姿态估计方法，相比现有技术，具有以下有益效果：

1、在工程实践中不可能得到精确的模型参数，例如故障卫星的燃料经过消耗而改变了质量特性，因此很难得到精确的系统模型，而传统方法的估计精度严重依赖于模型的精度，本发明将虚拟反馈思想引入滑模控制器中用于姿态估计，继承了滑模控制对系统的不确定性的鲁棒性。

2、状态初值选取多为人为估计，当初始误差较大时，传统滤波器收敛效果依赖于协方差阵初值的选择，而本发明是虚拟控制器会迅速向虚拟卫星反馈较大的控制力矩对虚拟卫星姿态进行及时修正。

3、只需计算反馈力矩对系统模型进行调节，无需在迭代过程中新状态协方差矩阵，大大减少了计算量，提高计算效率。

综上所述，本发明与目前经典的估计方法相比大大减小了计算量，不仅在状态量初始误差较大时，有很快的收敛速度和很高的估计精度，在对象存在系统误差时也能保证估计的鲁棒性，能够满足不同工况下高性能导航系统的需求。

附图说明

图1为本发明的基于虚拟滑模控制的视觉导航总体框架示意图。

图2为本发明的惯性系、轨道系及本体系各坐标系之间的关系示意图。

图3为本发明的目标航天器和追踪航天器坐标系的相对关系示意图。

图4为本发明的立体视觉观测模型示意图。

图5为本发明的特征点坐标系的构建及其与相机坐标系的关系示意图。

图6为本发明的滑模变结构控制切换函数原理图。

图7为本发明的滑模变结构控制反馈力矩随时间变化图。

图8为本发明的经滑模变结构控制反馈后角速度误差在X、Y、Z三轴随上时间变化曲线。

图9为本发明的控制方法下最终姿态角误差在X、Y、Z三轴随上时间变化曲线。

具体实施方式

下面结合附图对本发明作更进一步的说明。

一种基于虚拟滑模控制的非合作航天器姿态估计方法，如图1所示，包括一种基于Lyapunov原理的虚拟控制的滑模控制器；将立体视觉系统输出的实时观测数据转化为目标卫星绝对姿态作为滑模控制器的控制目标；根据目标卫星是非合作卫星，而非合作卫星失控后最终近似围绕一个定轴做旋转的运动特点，建立虚拟卫星运动模型；滑模控制器将虚拟卫星的动力学模型作为控制对象，得到虚拟卫星的姿态参量；以虚拟卫星估计的姿态参量和立体视觉系统得到的目标卫星绝对姿态作为滑模控制器的控制输入，滑模控制器根据控制输入计算出虚拟转动力矩并作用在虚拟卫星运动模型上；使得虚拟卫星的姿态与观测姿态同步，实现利用虚拟控制的滑模控制器对目标卫星姿态参数的估计。

具体包括以下步骤：

步骤一、通过追踪星上的立体视觉系统获取目标卫星相对追踪星的相对位姿信息；然后根据该目标卫星相对追踪星的相对位姿信息结合目标卫星的结构信息，将目标卫星相对追踪星相对姿态信息转化为目标卫星相对惯性系的绝对姿态信息；然后通过目标卫星的绝对姿态信息差分推算出目标卫星的观测角速度。

所述步骤一中通过立体视觉系统观测的数据求解得出目标卫星的绝对姿态信息的方法包括以下步骤：

步骤一一，通过追踪星上的立体视觉系统得到同一目标卫星的两幅图像数据，采用surf算法对两幅图像上的卫星表面特征点进行提取和匹配，并输出同一特征点在两幅图像上相应的像面坐标；

步骤一二，基于视差原理对像面上成功配对的特征点进行3D重构，利用平行式双目立体视觉深度恢复公式，获得特征点相对于追踪星本体坐标系的空间坐标P_ci；

步骤一三，从步骤一二获取的空间坐标中选取三个空间坐标S₁,S₂,S₃，建立特征点坐标系，以特征点S₂为坐标原点O_S，向量S₂S₁为Z_S轴，以垂直于特征点平面且背离目标卫星质心的方向为X_S轴，建立特征点坐标系O_S-X_SY_SZ_S，同时可以确定三个特征点在特征点坐标系的坐标P_si；

步骤一四，根据追踪星本体坐标系和特征点坐标系，建立特征点在特征点坐标系与追踪星本体系间的转换关系公式：P_ci＝RP_si+T，其中R和T分别为两坐标系的姿态转换矩阵和平移矢量；同时根据目标卫星的结构，确定目标卫星质心在特征点坐标系中的坐标，从而推算出目标卫星相对追踪星的相对姿态；

步骤一五，根据追踪星的姿态信息，将步骤一四得到的相对姿态转化为目标卫星的绝对姿态。

步骤二，由于目标卫星是非合作卫星，根据非合作卫星最终近似围绕一个定轴做旋转的运动特点，建立用来描述虚拟卫星运动的虚拟卫星模型，估计虚拟卫星的姿态参数；所述虚拟卫星模型包括目标卫星姿态运动学模型和姿态动力学模型；所述虚拟卫星的姿态参量包括目标卫星的估计四元数和估计角速度。

所述步骤二中目标卫星姿态的估计四元数的计算方法，包括以下步骤：

步骤二一，根据步骤二中根据非合作卫星最终近似围绕一个定轴做旋转的运动特点，建立以四元数描述的目标卫星姿态运动学模型和姿态动力学模型；

步骤二二，初始时，通过观测值计算的观测角速度代替估计角速度代入目标卫星姿态运动学模型，得到目标卫星姿态的估计四元数；其他时刻时，将估计角速度代入目标卫星姿态运动学模型，推算目标卫星姿态的估计四元数。

步骤三、根据步骤一中得到的目标卫星的绝对姿态信息和步骤二中得到的目标卫星姿态的估计四元数，建立四元数误差模型，从而得到目标卫星的误差四元数；根据步骤一中得到的目标卫星的观测角速度、步骤二中得到的目标卫星姿态动力学模型以及目标卫星的误差四元数，建立角速度误差模型，从而得到角速度误差；

步骤四、将虚拟反馈思想引入滑模控制器，设计基于Lyapunov原理的虚拟滑模控制器，将步骤三得到的目标卫星的误差四元数和角速度误差代入到虚拟滑模控制器，计算出虚拟控制量转动力矩“作用”在姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，修正模型估计值，使得虚拟卫星的姿态与观测值同步，最终实现对目标卫星姿态参数的估计。

所述步骤四中通过虚拟滑模控制器来“控制”虚拟卫星的姿态与观测值同步，包括以下步骤：

步骤四一，滑模面设计：根据步骤三中得到的目标卫星的误差四元数和角速度误差定义切换函数，并校验滑模面滑模发生条件；

步骤四二，趋近律设计：根据滑模可达性条件，设计滑模趋近律；

步骤四三，反馈力矩求解：首先将步骤三中得到的目标卫星的误差四元数和角速度误差代入步骤四一中的切换函数，然后结合步骤四二设计的趋近律，计算出滑模控制器的虚拟滑模控制反馈值，该反馈值即为转动力矩，将该反馈值“作用”在姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，最终达到虚拟卫星的姿态与观测姿态跟踪同步。

所述步骤四一中的切换函数为：s＝ω_e+kq_e13；其中：s＝[s_x,s_y,s_z]^T，k＝diag(k₁,k₂,k₃),k_i>0，s_x,s_y,s_z分别为切换函数在三个轴的分量，ω_e为目标卫星估计角速度与观测角速度误差，q_e13为q_e的矢量部分，q_e为误差四元数。

所述滑模发生条件为

所述步骤四二中趋近律包括指数项-Ks和等速趋近项-εsgn(s)，即：

为了更好的说明本发明，对本发明的原理进行如下说明。

1、技术实施整体路线

本发明基于虚拟滑模控制的非合作航天器姿态估计的设计方法基本思路是：如图1所示，将立体视觉的测量姿态作为控制目标，设计滑模控制器，将目标卫星姿态运动学模型和姿态动力学模型作为一颗虚拟的数字卫星，控制该虚拟卫星的姿态与观测值同步。将卫星动力学方程以及由四元数描述的运动学方程，转化为由误差四元数和误差角速度描述的误差跟踪方程，以立体视觉测量的姿态与模型的估计姿态之差作为控制系统的输入，基于Lyapunov原理设计滑模变结构控制器，计算出虚拟控制量“作用”在姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，最终达到虚拟卫星的姿态与观测姿态跟踪同步。

2、坐标系建立及相机模型

①参考坐标系

目标卫星轨道坐标系原点O_t与目标卫星质心重合，轴的方向为卫星位置矢量r，的方向垂直于平面，的方向符合右手定则；同样，追踪星轨道系为轨道系与惯性系O-X_iY_iZ_i的关系如图2所示。

本体系：O_t-x_ty_tz_t为目标航天器本体坐标系，原点定义在目标航天器质心处；三轴分别沿目标航天器的3个惯量主轴，并构成右手坐标系；同样，追踪星本体系为O_c-x_cy_cz_c。

像平面坐标系O(U,V)，原点O定义为相机光轴与图像平面的交点，位于图像中心；采用直角坐标系，U轴和V轴分别与图像像素的列数和行数平行；

相机坐标系C-XYZ，原点C定义为投影透镜的中心，各轴指向与O_c-x_cy_cz_c指向一致，与像平面关系如图3所示；左右相机原点分别定义为C_L和C_R，与追踪星原点之间有确定的平移关系。

②相机模型

特征点在目标卫星本体系上的坐标为s(x_i,y_i,z_i)，经过一系列的旋转平移可得到特征点在相机坐标系中的坐标S(X_i,Y_i,Z_i)：

S＝Ms+t (1)

其中，M为目标卫星相对于相机坐标系的相对姿态四元数转换矩阵；t＝[t_x,t_y,t_z]^T表示两坐标系的平移矢量。另外，在已知相机参数的情况下，根据小孔成像原理，S(X_i,Y_i,Z_i)和特征点对应像平面的坐标(U_i,V_i)的转换关系如下：

U_i＝Z_if/Y_i；V_i＝X_if/Y_i (2)

3、目标卫星姿态模型

①姿态动力学模型

目标卫星是一种非合作卫星，而非合作卫星最终近似围绕一个定轴做旋转的运动特点，建立目标卫星姿态运动的姿态动力学模型，根据该姿态动力学模型估计出目标卫星的估计角速度。

因发明所研究的非合作卫星已经失去控制而在空间中做自由翻滚，角速度无法通过姿态敏感器获得，由于帆板等挠性设备的阻尼作用，一般卫星最终将近似围绕一个定轴做旋转，根据如下姿态动力学方程来模拟目标卫星的角速度解析模型：

其中，ω_t为目标卫星的角速度(目标卫星的估计角速度)，是ω_t的微分(以下公式中变量上加一点均表示变量的微分值)；I_t为转动惯量；M_t为转动力矩。对于符号[ξ×]均表示如下：

目标卫星姿态运动学模型：

其中，ω_t＝[ω_x,ω_y,ω_z]^T,ω_x,ω_y,ω_z分别为角速度在x,y,z三个轴上的分量；q_t＝[q_t0,q_t1,q_t2,q_t3]^T为目标卫星相对惯性系的姿态四元数。

式(4)中“ο”为四元数乘法，设有两四元数Λ＝[λ₀ λ₁ λ₂ λ₃]和P＝[p₀ p₁ p₂ p₃]，两者四元数乘法Q运算规则如下：

解算可得运动学模型的矢量部分：

其中，q_t13＝[q_t1,q_t2,q_t3]^T是目标卫星姿态四元数的矢量部分，E(q_t13)＝S(q_t13)+q_t0I_3×3；q_t0为目标卫星姿态四元数的标量部分，I_3×3为3×3的单位矩阵。

标量部分：

初始时，通过测量角速度代替估计角速度代入目标卫星姿态运动学模型(4)，得到目标卫星姿态的估计四元数；其他时刻时，将估计角速度代入目标卫星姿态运动学模型(4)，得到目标卫星姿态的估计四元数。

②姿态误差模型

根据目标卫星相对追踪星的绝对姿态信息和目标卫星姿态的估计四元数，建立四元数误差模型，从而得到目标卫星的四元数误差，又可称为误差四元数。

定义目标卫星姿态的估计四元数和误差四元数q_e，则q_e与目标值q_d之间的关系如下，其中由式(4)计算而来：

根据目标卫星的测量角速度、目标卫星姿态的估计四元数以及目标卫星本体的姿态四元数误差，建立角速度误差模型，从而得到角速度误差。

将式(7)的误差四元数公式带入式(5)，形成姿态误差微分方程：

其中，q_e13为q_e的矢量部分，为ω_tb的标称值，为的斜对称矩阵，ω_e为目标卫星估计角速度与观测角速度误差。

其中，ω_d为由观测数据推算的角速度，即目标卫星的测量角速度，为目标卫星本体相对惯性系的估计角速度，q_e为目标卫星本体的姿态四元数误差，其中A(q_e)可由下式算得：

将式两边求导可得：

代入式(3)，则等式左侧为：

等式右侧为：

右侧＝M_t-[A(q_e)^-1(ω_d-ω_e)×]I_t[A(q_e)^-1(ω_d-ω_e)] (12)

综合(11)(12)两式可得角速度误差模型：

4、基于立体视觉的相对姿态测量算法

通过追踪星上的立体视觉系统获取目标卫星相对追踪星的相对位姿信息；然后根据该目标卫星相对追踪星的相对位姿信息结合目标卫星的结构信息，将目标卫星相对追踪星相对姿态信息转化为目标卫星相对追踪星的绝对姿态信息；然后通过目标卫星相对追踪星的绝对姿态信息推算出目标卫星的测量角速度；

步骤一一，通过立体视觉系统得到同一目标卫星的两幅图像数据，采用surf算法对两幅图像上的卫星表面特征点进行提取和匹配，并输出同一特征点在两幅图像上相应的像面坐标。

立体视觉系统的观测模型，如图4所示，通过立体视觉系统得到同一目标卫星的两幅图像数据，采用surf算法对两幅图像上的卫星表面特征点进行提取和匹配，并输出同一特征点在两幅图像上相应的像面坐标，其中，O_c为追踪星质心；O_t为目标卫星质心；f为特征点，r_feature为目标卫星特征点相对目标卫星质心在目标卫星本体系下的位置矢量；C点为相机质心，r_camera为相机相对目标卫星在追踪星本体系下的位置矢量；追踪星本体系O_c-x_cy_cz_c作为测量坐标系，ρ为两星质心相对距离在惯性系下的投影；ρ′为相机所拍摄的特征点坐标(在相机坐标系下)。

由图4可得出追踪星、目标卫星及特征点间的相对关系如下，其中m表示轨道系：

其中，为相机坐标系相对追踪星本体系的坐标，和分别为目标卫星本体系和追踪星本体系相对于目标卫星轨道系的姿态转换矩阵；υ为立体视觉系统的观测噪声。

步骤一二，基于视差原理对像面上成功配对的特征点进行3D重构，利用平行式双目立体视觉深度恢复公式，获得特征点相对于追踪航天器本体坐标系的空间坐标P_ci。

非合作目标一般与追踪星间无主动的信息交流，特征点也是卫星表面的自然特征，但是如果目标卫星是己方研制的，可以认为其结构是已知的。通过对相机拍摄的图像进行识别可以提取5个特征点在像平面的坐标，然后通过3D重构可以获得五个特征点在相机坐标系下的三维坐标，利用其中三个特征点坐标建立特征点坐标系，即可得到目标卫星坐标系与追踪星坐标系间的转换矩阵。

步骤一三，从步骤一二获取的空间坐标中选取三个建立如图5的特征点坐标系，以特征点s₂为坐标原点，向量s₂s₁为z轴，以垂直于特征点平面且背离目标卫星质心的方向为x轴，建立特征点坐标系O_s-X_sY_sZ_s，同时可以确定三个特征点在O_s-X_sY_sZ_s的坐标P_si。

步骤一四，借助特征点在特征点坐标系与追踪星本体系间的转换关系公式，同时由于假设目标卫星是己方研制的，可以认为其结构是已知的，因此可直接确定目标卫星质心在特征点坐标系中的坐标，从而推算出目标卫星航天器相对追踪星的相对姿态。

步骤一五，由于追踪星航天器的姿态信息完全可测，可将步骤一四得到的相对姿态转化为目标卫星的绝对姿态，并以此作为虚拟滑模控制目标。

特征点在特征点坐标系与追踪星本体系间的转换关系如下：

P_c＝RP_s+T (15)

其中，P_s为特征点在特征点坐标系下的坐标，P_c为特征点在追踪星本体系下的坐标，R和T分别为两坐标系的姿态转换矩阵和平移矢量。

假设s_i在追踪星本体系的坐标为(x_ci,y_ci,z_ci)^T；在特征点坐标系的坐标为(x_si,y_si,z_si)^T即

①平移矩阵求解

因s₂在特征点坐标系中坐标为(0,0,0)^T，在追踪星本体系的坐标为(x_c2,y_c2,z_c2)^T代入式(16)中可得：

可得特征点坐标系O_s-X_sY_sZ_s相对于追踪星本体系O_c-x_cy_cz_c的平移矩阵T＝(x_c2,y_c2,z_c2)^T。

②转换矩阵求解

定义向量s₂s₁的方向为特征点坐标系的z轴，即特征点s₁在z轴上，因此s₁在特征点坐标系和追踪星本体系中的坐标分别为(0,0,z_s1)^T和(x_c1,y_c1,z_c1)^T，其中z_s1＝|s₂s₁|代入式(16)中：

可解得转换矩阵的最后一列：

又因为特征点s₃在特征点坐标系的O_s-X_sZ_s平面内，其坐标为(x_s3,0,z_s3)^T，在追踪星本体系的坐标为(x_c3,y_c3,z_c3)^T代入式(16)中可得：

其中，z_s3为向量s₂s₃在z轴方向的分量，x_s3＝s₂s₃-z_s3。

由R₃和R₁，可确定R₂为：

R₂＝R₁×R₃ (21)

最后可得姿态转换矩阵R＝[R₁ R₂ R₃]，根据姿态转换矩阵得出目标卫星姿态四元数，该目标卫星姿态四元数即为目标卫星的绝对姿态。将目标卫星姿态四元数作为观测输入，即虚拟控制系统的控制目标。

5、滑模控制器设计

根据目标卫星姿态误差模型来设计合适的滑模控制器，采用Lyapunov方法设计了滑模变结构控制器来“控制”虚拟卫星的姿态与观测值同步。其步骤如下：

①滑模面设计

为了跟踪误差向量，用标量方程s(t)＝0定义状态空间中的时变曲面。

定义切换函数：

s＝ω_e+kq_e13 (22)

其中，s＝[s₁,s₂,s₃]^T，k＝diag(k₁,k₂,k₃),k_i>0，s₁,s₂,s₃为特征点坐标，ω_e为目标卫星估计角速度与观测角速度误差，q_e13为q_e的矢量部分，q_e为四元数误差。

切换函数原理图如图6所示，系统运动点到达切换面附近时，当s→0⁺时，或当s→0^-时，滑动模态发生，即当时满足滑模发生条件。

②趋近律设计

根据滑模可达性条件，仅可以保证由状态空间任意位置运动点在有限时间内到达切换面的要求，对趋近的轨迹未作任何限制，而设计趋近律可以改善趋近运动的品质。因此设计指数趋近律，包括指数项-Ks和等速趋近项-εsgn(s)以改善趋近运动的品质。本发明选取指数趋近律：

其中，i＝1,2,3，ε>0，K＝diag(K₁,K₂,K₃),K_i>0为指数项，解为s＝s(0)e^-Kt。

指数项-Ks可以保证当s较大时，系统状态能以较大的速度趋近于滑动模态。趋近速度从较大值逐渐减小到0，不仅缩短了趋近时间，而且使运动点在到达切换面时速度较小。

但是单纯的指数趋近，运动点逼近切换面是一个渐近的过程，无法在有限时间内到达，切换面上就不会发生滑动模态，因此增加一个等速趋近当s→0时，趋近速度是ε而不是0，可保证在有限时间内到达切换面。

③反馈力矩推导

因为因此：

将式(23)代入(24)可得：

因此，滑模控制的反馈值为控制力矩M_t：

④稳定性证明

李雅普诺函数选取如下：

V＝s^Ts (27)

将式(25)代入(28)中：

因此，本发明设计的控制律可以满足滑模的稳定条件。

为了验证本发明所提出基于滑模控制姿态估计方法具有较快的收敛速度和较好的估计精度，采用本发明方法获取立体视觉观测数据，并根据目标卫星姿态模型，通过设计合理的滑模控制器的方法利用观测数据对模型估计值进行修正，并进行基于matlab的仿真验证。

从图7的反馈控制量图上看，除Y轴向在30s后收敛，其他两轴均在20s内快速收敛。50s后，姿态动力学模型反馈力矩X轴与Z轴上的均方差约0.0001(N·m)；Y轴上的实际控制量均方差也小于0.0001(N·m)。

从图8可以看出，经过滑模变结构控制后，角速度三轴也均可在50s后内收敛。50s后，角速度在X轴与Z轴两轴上的均方差约3×10^-5(°/s)，最大误差值不超过0.2(°/s)；Y轴上的实际控制量均方差也小于8×10^-5(°/s)，最大误差值不超过0.5(°/s)。

从图9的欧拉角收敛曲线可以看出，除Y轴向在150s内收敛，其他两轴均在50s内快速收敛。50s后，姿态动力学模型反馈力矩X轴与Z轴上的均方差约0.005°；150s后，Y轴收敛后的实际控制量均方差也小于0.002°。

由以上仿真曲线图可以发现，本发明所推导的相对姿态动力学模型设计准确有效，并通过滑模变结构控制可获得较高的姿态估计精度。

本发明首先根据目标卫星在空间中做自由翻滚运动给出卫星动力学方程和四元数描述的运动学方程，将该模型作为虚拟控制的对象；然后利用立体视觉观测的图像信息对特征点进行3D重构，得到目标卫星的姿态，将该姿态信息与估计姿态之差作为控制输入；采用Lyapunov方法设计滑模变结构控制律来“控制”虚拟卫星，计算出虚拟控制量“作用”在虚拟卫星即姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，最终达到虚拟卫星的姿态与观测姿态跟踪同步。

综上所述，本发明将滑模控制引入姿态估计中，继承滑模控制的优势，只需计算反馈力矩对系统模型进行调节,与目前经典的估计方法相比大大减小了计算量；从仿真结果可以看出本发明能有效提高收敛速度和精度，满足高性能导航系统的需求。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (8)

1.一种基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于：包括一种基于Lyapunov原理的虚拟控制的滑模控制器；将立体视觉系统输出的实时观测数据转化为目标卫星绝对姿态作为滑模控制器的控制目标；根据目标卫星是非合作卫星，而非合作卫星失控后最终近似围绕一个定轴做旋转的运动特点，建立虚拟卫星运动模型；滑模控制器将虚拟卫星的动力学模型作为控制对象，得到虚拟卫星的姿态参量；以虚拟卫星估计的姿态参量和立体视觉系统得到的目标卫星绝对姿态作为滑模控制器的控制输入，滑模控制器根据控制输入计算出虚拟转动力矩并作用在虚拟卫星运动模型上；使得虚拟卫星的姿态与观测姿态同步，实现利用虚拟控制的滑模控制器对目标卫星姿态参数的估计。

2.根据权利要求1所述的基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于，包括以下步骤：

步骤一、通过追踪星上的立体视觉系统获取目标卫星相对追踪星的相对位姿信息；然后根据该目标卫星相对追踪星的相对位姿信息结合目标卫星的结构信息，将目标卫星相对追踪星相对姿态信息转化为目标卫星相对惯性系的绝对姿态信息；然后通过目标卫星的绝对姿态信息差分推算出目标卫星的观测角速度；

步骤二，由于目标卫星是非合作卫星，根据非合作卫星最终近似围绕一个定轴做旋转的运动特点，建立用来描述虚拟卫星运动的虚拟卫星模型，估计虚拟卫星的姿态参数；所述虚拟卫星模型包括目标卫星姿态运动学模型和姿态动力学模型；所述虚拟卫星的姿态参量包括目标卫星的估计四元数和估计角速度；

步骤三、根据步骤一中得到的目标卫星的绝对姿态信息和步骤二中得到的目标卫星姿态的估计四元数，建立四元数误差模型，从而得到目标卫星的误差四元数；根据步骤一中得到的目标卫星的观测角速度、步骤二中得到的目标卫星姿态动力学模型以及目标卫星的误差四元数，建立角速度误差模型，从而得到角速度误差；

步骤四、将虚拟反馈思想引入滑模控制器，设计基于Lyapunov原理的虚拟滑模控制器，将步骤三得到的目标卫星的误差四元数和角速度误差代入到虚拟滑模控制器，计算出虚拟控制量转动力矩“作用”在姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，修正模型估计值，使得虚拟卫星的姿态与观测值同步，最终实现对目标卫星姿态参数的估计。

3.根据权利要求2所述的基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于：所述步骤一中通过立体视觉系统观测的数据求解得出目标卫星的绝对姿态信息的方法包括以下步骤：

步骤一一，通过追踪星上的立体视觉系统得到同一目标卫星的两幅图像数据，采用surf算法对两幅图像上的卫星表面特征点进行提取和匹配，并输出同一特征点在两幅图像上相应的像面坐标；

步骤一二，基于视差原理对像面上成功配对的特征点进行3D重构，利用平行式双目立体视觉深度恢复公式，获得特征点相对于追踪星本体坐标系的空间坐标P_ci；

步骤一三，从步骤一二获取的空间坐标中选取三个空间坐标S₁,S₂,S₃，建立特征点坐标系，以特征点S₂为坐标原点O_S，向量S₂S₁为Z_S轴，以垂直于特征点平面且背离目标卫星质心的方向为X_S轴，建立特征点坐标系O_S-X_SY_SZ_S，同时可以确定三个特征点在特征点坐标系的坐标P_si；

步骤一四，根据追踪星本体坐标系和特征点坐标系，建立特征点在特征点坐标系与追踪星本体系间的转换关系公式：P_ci＝RP_si+T，其中R和T分别为两坐标系的姿态转换矩阵和平移矢量；同时根据目标卫星的结构，确定目标卫星质心在特征点坐标系中的坐标，从而推算出目标卫星相对追踪星的相对姿态；

步骤一五，根据追踪星的姿态信息，将步骤一四得到的相对姿态转化为目标卫星的绝对姿态。

4.根据权利要求3所述的基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于：所述步骤二中目标卫星姿态的估计四元数的计算方法，包括以下步骤：

步骤二一，根据步骤二中根据非合作卫星最终近似围绕一个定轴做旋转的运动特点，建立以四元数描述的目标卫星姿态运动学模型和姿态动力学模型；

步骤二二，初始时，通过观测值计算的观测角速度代替估计角速度代入目标卫星姿态运动学模型，得到目标卫星姿态的估计四元数；其他时刻时，将估计角速度代入目标卫星姿态运动学模型，推算目标卫星姿态的估计四元数。

5.根据权利要求4所述的基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于：所述步骤四中通过虚拟滑模控制器来“控制”虚拟卫星的姿态与观测值同步，包括以下步骤：

步骤四一，滑模面设计：根据步骤三中得到的目标卫星的误差四元数和角速度误差定义切换函数，并校验滑模面滑模发生条件；

步骤四二，趋近律设计：根据滑模可达性条件，设计滑模趋近律；

步骤四三，反馈力矩求解：首先将步骤三中得到的目标卫星的误差四元数和角速度误差代入步骤四一中的切换函数，然后结合步骤四二设计的趋近律，计算出滑模控制器的虚拟滑模控制反馈值，该反馈值即为转动力矩，将该反馈值“作用”在姿态动力学模型上，以消除虚拟卫星姿态与测量姿态之间的偏差，最终达到虚拟卫星的姿态与观测姿态跟踪同步。

6.根据权利要求5所述的基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于：所述步骤四一中的切换函数为：s＝ω_e+kq_e13；其中：s＝[s_x,s_y,s_z]^T，k＝diag(k₁,k₂,k₃),k_i>0，s_x,s_y,s_z分别为切换函数在三个轴的分量，ω_e为目标卫星估计角速度与观测角速度误差，q_e13为q_e的矢量部分，q_e为误差四元数。

7.根据权利要求6所述的基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于：所述滑模发生条件为

8.根据权利要求7所述的基于虚拟滑模控制的非合作航天器姿态估计方法，其特征在于：所述步骤四二中趋近律包括指数项-Ks和等速趋近项-εsgn(s)，即：

$\begin{array}{cc}\stackrel{\·}{s}=-\mathrm{\ϵ}\mathrm{sgn}\left(s\right)-Ks& \mathrm{\ϵ}>0;\end{array}$

$\begin{array}{ccc}\mathrm{sgn}\left(s_i\right)=\left\{\begin{array}{c}1,s_i>0\\ 0,s_i=0\\ -1,s_i<0\end{array}\right.& i=1,2,3,& K=diag(K_1,K_2,K_3),K_i>0.\end{array}$