CN103884340B

CN103884340B - A kind of information fusion air navigation aid of survey of deep space fixed point soft landing process

Info

Publication number: CN103884340B
Application number: CN201410126454.3A
Authority: CN
Inventors: 张晓文; 王大轶; 黄翔宇; 褚永辉
Original assignee: Beijing Institute of Control Engineering
Current assignee: Beijing Institute of Control Engineering
Priority date: 2014-03-31
Filing date: 2014-03-31
Publication date: 2016-08-17
Anticipated expiration: 2034-03-31
Also published as: CN103884340A

Abstract

An information fusion navigation method for the fixed-point soft landing process of deep space exploration. On the basis of the lander's autonomous navigation system equipped with navigation sensors such as an inertial measurement unit, a ranging sensor, a speed sensor, and an optical imaging sensor, the soft landing The angular velocity and non-gravitational acceleration of the lander are measured by the inertial measurement unit throughout the process, and the position and velocity information of the lander is provided to the navigation system after integration and extrapolation. The slant distance information output by the ranging sensor, the relative velocity information output by the speed sensor, and the pixel coordinate information of feature points output by the optical imaging sensor are all processed by independent Kalman sub-filters, and the state and error covariance are given. local estimate. The local estimation results are then fused by the covariance intersection method to obtain a unified position and velocity estimation value of the current lander, which is fed back to the sub-filter for the next navigation update. The navigation method has high position and speed estimation accuracy, good real-time performance, fault tolerance and easy implementation.

Description

An Information Fusion Navigation Method for Fixed-point Soft Landing in Deep Space Exploration

技术领域technical field

本发明属于导航领域，涉及一种基于信息融合的航天器自主导航方法。The invention belongs to the field of navigation and relates to an autonomous navigation method for spacecraft based on information fusion.

背景技术Background technique

深空探测定点软着陆过程时间短，姿态轨道动态变化大，环境未知性强，并且在大天体软着陆时一旦启动就难以终止，这就对导航系统在实时性、精度、容错能力提出了极高的要求。The fixed-point soft landing process of deep space exploration takes a short time, the dynamic changes in attitude and orbit are large, the environment is highly unknown, and it is difficult to stop once it starts soft landing on a large celestial body. high demands.

传统的基于地面测控网的导航方式存在着深空通讯延时大，确定相对位置、速度的精度低，不能提供着陆场详细地理信息的缺点。因此，着陆器必须依靠自主导航系统，通过各类敏感器获取测量信息，再由着陆器上的导航算法处理，独立实时的估计自身的姿态、位置、速度并且识别着陆场障碍。由于要估计的状态参数多，因此着陆器自主导航系统的敏感器配置相对丰富，典型方案包括惯性测量单元(含陀螺和加速度计)、测距敏感器、测速敏感器、光学成像敏感器。惯性测量单元测量着陆器的角速度和非引力加速度，通过积分可给出着陆器姿态位置速度的基础信息。测距敏感器测量波束方向的斜距，从而可确定着陆器的实时高度。测速敏感器只要有3束不共面的波束就可确定着陆器的相对速度。光学成像敏感器对天体表面已知的特征点成像，经图像处理提取特征点的像素坐标，从而对着陆点附近的障碍或是危险区域进行成像识别。上述敏感器在均有效的前提下，可以为导航系统提供的测量信息维数大于着陆器待估计的状态量维数，因此不同敏感器之间实际上形成了异构备份。在工程实践中，为了进一步保证安全，还可能增加冗余备份，例如配备2个测距波束等。处理如此多源的非独立测量信息，导航算法除了保证必要的导航精度之外，还必须具备良好的信息融合能力、容错能力，并且易于星上实现。The traditional navigation method based on the ground measurement and control network has the disadvantages of large delay in deep space communication, low accuracy in determining relative position and velocity, and inability to provide detailed geographic information of the landing site. Therefore, the lander must rely on the autonomous navigation system to obtain measurement information through various sensors, and then processed by the navigation algorithm on the lander to estimate its attitude, position, and speed independently and in real time and identify obstacles on the landing site. Due to the large number of state parameters to be estimated, the sensor configuration of the autonomous navigation system of the lander is relatively rich. Typical solutions include inertial measurement units (including gyroscopes and accelerometers), ranging sensors, speed sensors, and optical imaging sensors. The inertial measurement unit measures the angular velocity and non-gravitational acceleration of the lander, and the basic information of the attitude, position, and velocity of the lander can be given by integration. The range sensor measures the slant range in the direction of the beam, allowing the real-time altitude of the lander to be determined. The speed sensor can determine the relative speed of the lander as long as there are three non-coplanar beams. The optical imaging sensor images the known feature points on the surface of the celestial body, and extracts the pixel coordinates of the feature points through image processing, so as to image and identify obstacles or dangerous areas near the landing site. Under the premise that the above sensors are all effective, the dimension of measurement information provided by the navigation system is larger than the dimension of the state quantity to be estimated by the lander, so different sensors actually form a heterogeneous backup. In engineering practice, in order to further ensure safety, it is also possible to add redundant backups, such as equipped with 2 ranging beams, etc. To deal with so many sources of non-independent measurement information, the navigation algorithm must not only ensure the necessary navigation accuracy, but also have good information fusion capabilities, fault tolerance, and be easy to implement on-board.

惯性导航加辅助测量修正的导航方法是一种较为稳妥简单的软着陆导航方法。该方法视惯性导航结果为主导值，当斜距、速度、特征点像素坐标等辅助测量有效时，逐一的或是分组的将其与惯性导航给出的该量估计值相比较，其差值乘以定常的或是拟合的系数得到对惯性导航结果的修正量。由于辅助测量的修正系数独立设计且没有耦合关系，因此该方法易于设计和应用，即使单个敏感器故障也不影响其它敏感器测量的使用。但该方法的不足之处在于牺牲了部分导航精度，给出的估计不是最优值。The navigation method of inertial navigation plus auxiliary measurement correction is a relatively safe and simple soft landing navigation method. This method regards the results of inertial navigation as the dominant value. When auxiliary measurements such as slope distance, velocity, and pixel coordinates of feature points are valid, they are compared with the estimated value given by inertial navigation one by one or in groups, and the difference is Multiply by the constant or fitted coefficients to get the corrections to the inertial navigation results. Since the correction coefficients of auxiliary measurements are independently designed and have no coupling relationship, the method is easy to design and apply, even if a single sensor fails, it will not affect the use of other sensor measurements. However, the disadvantage of this method is that part of the navigation accuracy is sacrificed, and the estimation given is not the optimal value.

目前在导航领域广泛应用的扩展卡尔曼滤波(Extended Kalman Filter)算法是一种集中式滤波方法，能够对所有导航测量信息做最优融合处理，但是该方法需要将不同敏感器的测量统一写为一个方程。这样的方程其维数很高，因而计算负担非常重，在当前星载计算机性能条件下不利于滤波的实时运行。而且该方法的容错性差，不利于故障诊断，也不利于处理不同敏感器的时间同步性问题。The Extended Kalman Filter (Extended Kalman Filter) algorithm, which is widely used in the field of navigation, is a centralized filtering method, which can perform optimal fusion processing on all navigation measurement information, but this method needs to write the measurements of different sensors uniformly as an equation. The dimension of such equations is very high, so the calculation burden is very heavy, which is not conducive to the real-time operation of filtering under the performance conditions of the current spaceborne computer. Moreover, the fault tolerance of this method is poor, which is not conducive to fault diagnosis, and is also not conducive to dealing with the time synchronization problem of different sensors.

为克服集中式滤波的缺点诞生了分布式滤波，该技术至今已经发展了30多年，在众多的分布式滤波方法中，Carlson提出的联邦滤波(Federated Filter)只对子滤波器的估计信息进行合成。子滤波器是平行结构形式。各子滤波器算法采用卡尔曼滤波算法，处理对应传感器的测量信息。联邦滤波由于计算量小、信息分配方式灵活、信息容错性能好而广受重视。但是联邦滤波需要局部状态估计相互独立或者是已知其相关性，而精确计算局部状态估计相关性一般十分困难。In order to overcome the shortcomings of centralized filtering, distributed filtering was born. This technology has been developed for more than 30 years. Among the many distributed filtering methods, the Federated Filter proposed by Carlson only synthesizes the estimated information of the sub-filters. . The sub-filters are in the form of a parallel structure. Each sub-filter algorithm adopts the Kalman filter algorithm to process the measurement information of the corresponding sensor. Federated filtering has been widely valued due to its small amount of computation, flexible information distribution, and good information fault tolerance. However, federated filtering requires local state estimates to be independent of each other or their correlations are known, and it is generally very difficult to accurately calculate the correlations of local state estimates.

发明内容Contents of the invention

本发明的技术解决问题是：克服现有技术的不足，提供了一种深空探测定点软着陆过程的信息融合导航方法，其位置速度估计精度高、实时性好、具备容错能力且容易实现。The technical solution of the present invention is to overcome the deficiencies of the prior art and provide an information fusion navigation method for the fixed-point soft landing process of deep space exploration, which has high position and velocity estimation accuracy, good real-time performance, fault tolerance and is easy to implement.

本发明的技术解决方案是：一种深空探测定点软着陆过程的信息融合导航方法，包括如下步骤：The technical solution of the present invention is: an information fusion navigation method for a fixed-point soft landing process of deep space exploration, comprising the following steps:

(1)将着陆器在天体中心惯性系的位置r和速度v作为状态变量X， $X = [\begin{matrix} r \\ v \end{matrix}],$ 其中X的估计初值及对应的误差协方差初值P₀在着陆开始时刻由地面测控系统轨道确定结果给出；(1) The position r and velocity v of the lander in the inertial system of the center of the celestial body are used as the state variable X, $x = [\begin{matrix} r \\ v \end{matrix}],$ where the estimated initial value of X And the corresponding initial value of error covariance P ₀ is given by the orbit determination result of the ground measurement and control system at the moment of landing;

(2)利用着陆器上安装的惯性导航系统上一时刻的状态估计外推预测当前时刻着陆器的状态以及对应的误差协方差，(2) Use the state estimation and extrapolation of the inertial navigation system installed on the lander at the last moment to predict the state of the lander at the current moment and the corresponding error covariance,

${\overset{^^}{X x}}_{k k / / k k - - 11} = = {\overset{^^}{X x}}_{k k - - 11} + + [\begin{matrix} {\overset{^^}{v v}}_{k k - - 11} Δ Δ t t + + \frac{11}{22} {C C}_{i i b b,, k k} {\overset{^^}{a a}}_{k k} {Δt Δt}^{22} - - \frac{11}{22} \frac{μ μ}{{(({\overset{^^}{r r}}_{k k - - 11}))}^{33}} {\overset{^^}{r r}}_{k k - - 11} {Δt Δt}^{22} \\ {C C}_{i i b b,, k k} {\overset{^^}{a a}}_{k k} Δ Δ t t - - \frac{μ μ}{{(({\overset{^^}{r r}}_{k k - - 11}))}^{33}} {\overset{^^}{r r}}_{k k - - 11} Δ Δ t t \end{matrix}]$

P_k/k-1＝Φ_k,k-1P_k-1(Φ_k,k-1)^T+QP _k/k-1 ＝Φ _k,k-1 P _k-1 (Φ _k,k-1 ) ^T +Q

${Φ Φ}_{k k,, k k - - 11} = = {I I}_{66} + + [\begin{matrix} 00_{33} & {I I}_{33} \\ {\frac{\partial \partial ((- - μ μ r r / / {r r}^{33}))}{\partial \partial {r r}^{T T}} | |}_{r r = = {\overset{^^}{r r}}_{k k / / k k - - 11}} & 00_{33} \end{matrix}] Δ Δ t t$

其中为k时刻着陆器的状态预测，为k-1时刻着陆器的状态估计，和分别为k-1时刻着陆器的位置估计和速度估计， ${\hat{X}}_{k - 1} = [\begin{matrix} {\hat{r}}_{k - 1} \\ {\hat{v}}_{k - 1} \end{matrix}],$ 为的模，Δt为导航更新周期，C_ib,k为k时刻着陆器本体系到着陆天体中心惯性系的方向余弦阵，为k时刻惯性单元中加速度计输出的加速度，μ为着陆天体引力常数，I₆为6阶单位矩阵，I₃为3阶单位矩阵，0₃为3阶零矩阵，符号T表示矩阵转置，r为r的模，为k时刻着陆器的位置状态预测，P_k/k-1为k时刻的误差协方差预测，Q是系统噪声方差阵；in is the state prediction of the lander at time k, is the state estimation of the lander at time k-1, and are the position estimate and velocity estimate of the lander at time k-1, respectively, ${\hat{x}}_{k - 1} = [\begin{matrix} {\hat{r}}_{k - 1} \\ {\hat{v}}_{k - 1} \end{matrix}],$ for Δt is the navigation update cycle, C _ib,k is the cosine array of the direction from the lander system to the center inertial system of the landing celestial body at time k, is the acceleration output by the accelerometer in the inertial unit at time k, μ is the gravitational constant of the landing celestial body, I ₆ is the 6th-order identity matrix, I ₃ is the 3rd-order identity matrix, 0 ₃ is the 3rd-order zero matrix, and the symbol T represents matrix transposition, r is the modulus of r, is the position and state prediction of the lander at time k, P _k/k-1 is the error covariance prediction at time k, and Q is the system noise variance matrix;

(3)判断着陆器上安装的测距敏感器、测速敏感器、光学成像敏感器是否输出了新的有效数据，记新的有效数据的个数为n，如果n＝0，则直接将步骤(2)的结果作为当前时刻的导航结果输出，结束并退出；如果n不为0，则进入步骤(4)；(3) Judging whether the ranging sensor, velocity measuring sensor, and optical imaging sensor installed on the lander have output new valid data, record the number of new valid data as n, if n=0, then directly replace the step The result of (2) is output as the navigation result of the current moment, ends and exits; If n is not 0, then enter step (4);

(4)针对每一个新的有效数据，均单独建立一个子卡尔曼滤波器，并求取每一个子卡尔曼滤波器的状态估计以及对应的误差协方差 (4) For each new effective data, a sub-Kalman filter is established separately, and the state estimation of each sub-Kalman filter is obtained and the corresponding error covariance

${\overset{^^}{X x}}_{k k}^{i i} = = {\overset{^^}{X x}}_{k k / / k k - - 11} + + {K K}_{k k}^{i i} [[{Z Z}_{k k}^{i i} - - {h h}^{i i} (({\overset{^^}{X x}}_{k k / / k k - - 11}))]]$

${P P}_{k k}^{i i} = = ((I I - - {K K}_{k k}^{i i} {H h}_{k k}^{i i})) {P P}_{k k / / k k - - 11}$

${K K}_{k k}^{i i} = = {P P}_{k k / / k k - - 11} {(({H h}_{k k}^{i i}))}^{T T} {[[{H h}_{k k}^{i i} {P P}_{k k / / k k - - 11} {(({H h}_{k k}^{i i}))}^{T T} + + {R R}^{i i}]]}^{- - 11}$

${H h}_{k k}^{i i} = = {\frac{\partial \partial {h h}^{i i} ((X x))}{\partial \partial X x} | |}_{X x = = {\overset{^^}{X x}}_{k k,, k k - - 11}}$

其中hⁱ(X)为第i个新的有效数据，i＝1,…,n，Rⁱ为第i个新的有效数据对应敏感器的测量噪声方差阵，为第i个新的有效数据对应的敏感器在k时刻的输出，符号-1表示求矩阵的逆；Among them, h ⁱ (X) is the i-th new effective data, i=1,...,n, R ⁱ is the measurement noise variance matrix of the i-th new effective data corresponding to the sensor, is the output of the sensor corresponding to the i-th new valid data at time k, and the symbol -1 means to find the inverse of the matrix;

(5)对步骤(4)中每一个子滤波器的输出进行信息融合，得到当前时刻的导航结果并输出，(5) Carry out information fusion to the output of each sub-filter in step (4), obtain the navigation result of current moment and output,

${\overset{^^}{X x}}_{k k} = = {P P}_{k k} [[{Σ Σ}_{i i = = 11}^{n no} {ω ω}_{i i} {(({P P}_{k k}^{i i}))}^{- - 11} {\overset{^^}{X x}}_{k k}^{i i}]]$

${P P}_{k k} = = {[[{Σ Σ}_{i i = = 11}^{n no} {(({P P}_{k k}^{i i}))}^{- - 11} {ω ω}_{i i}]]}^{- - 11}$

其中tr( )表示求矩阵的迹，非负加权系数且满足ω₁+…+ω_n＝1。in tr( ) means to find the trace of the matrix, non-negative weighting coefficient And satisfies ω ₁ + . . . +ω _n =1.

本发明与现有技术相比的优点在于：The advantage of the present invention compared with prior art is:

(1)本发明提出的深空探测定点软着陆过程的信息融合导航方法，与惯性导航加辅助测量修正的导航方法相比，由于对斜距、速度、特征点坐标等测量信息进行了统一的信息融合，因此其获得的位置速度估计精度更高；(1) The information fusion navigation method of the fixed-point soft landing process of deep space exploration proposed by the present invention, compared with the navigation method of inertial navigation plus auxiliary measurement correction, due to the unified measurement information such as slope distance, speed, and feature point coordinates Information fusion, so the position and speed estimation accuracy obtained by it is higher;

(2)本发明提出的深空探测定点软着陆过程的信息融合导航方法，与基于扩展卡尔曼滤波的导航方法相比，属于分布式结构，单个子滤波器的方程维数低，易于星载计算机实时计算，在部分测量无效的情况下不影响其余测量的信息融合，因此其容错能力强，鲁棒性好，提高了着陆器导航系统的可靠性；(2) The information fusion navigation method of the fixed-point soft landing process of deep space exploration proposed by the present invention, compared with the navigation method based on extended Kalman filter, belongs to the distributed structure, the equation dimension of a single sub-filter is low, and it is easy to carry on the space Computer real-time calculation does not affect the information fusion of other measurements when some measurements are invalid, so it has strong fault tolerance and good robustness, which improves the reliability of the lander's navigation system;

(3)本发明提出的深空探测定点软着陆过程的信息融合导航方法，采用类似联邦滤波的分布式结构以及卡尔曼滤波算法，利用协方差交叉法(CovarianceIntersection)对测量数据进行融合给出全局最优估计。但与基于联邦滤波的导航方法相比，不需要计算不同测量信息给出的多个局部估计误差之间的相关性，只是增加了一个信息矩阵放大过程和一个非负加权系数的优化计算过程，其融合后的估计量在空间某方向的精度至少不低于参与融合的估计量在此方向上可提供的最低精度，方程简单，易于工程实现。(3) The information fusion navigation method of the fixed-point soft landing process of deep space exploration proposed by the present invention adopts a distributed structure similar to federated filtering and a Kalman filtering algorithm, and utilizes the covariance intersection method (CovarianceIntersection) to fuse the measurement data to give a global best estimate. However, compared with the navigation method based on federated filtering, it does not need to calculate the correlation between multiple local estimation errors given by different measurement information, but only adds an information matrix amplification process and an optimization calculation process of non-negative weighting coefficients. The accuracy of the fused estimator in a certain direction of space is at least not lower than the minimum accuracy that can be provided by the estimator participating in the fusion in this direction. The equation is simple and easy to implement in engineering.

附图说明Description of drawings

图1为本发明分布式信息融合的结构原理图；Fig. 1 is the structural schematic diagram of the distributed information fusion of the present invention;

图2为本发明方法的流程框图；Fig. 2 is the block flow diagram of the inventive method;

图3为本发明实施例中的导航位置估计误差示意图；FIG. 3 is a schematic diagram of a navigation position estimation error in an embodiment of the present invention;

图4为本发明实施例中的导航速度估计误差示意图。Fig. 4 is a schematic diagram of navigation speed estimation error in an embodiment of the present invention.

具体实施方式detailed description

本发明提出的信息融合导航方法采用分布式滤波器结构，如图1所示。深空探测定点软着陆任务着陆器自主导航系统配备惯性测量单元(含陀螺和加速度计)、测距敏感器、测速敏感器、光学成像敏感器等导航敏感器。在软着陆全程，惯性测量单元测量着陆器的角速度和非引力加速度，经积分外推后为导航系统提供着陆器的位置速度基础信息，该过程即为惯性导航。待着陆器的高度、姿态等达到各敏感器的工作条件后，测距敏感器输出斜距信息，测速敏感器输出相对速度信息，光学成像敏感器输出特征点像素坐标信息。这些信息均由相互独立的卡尔曼子滤波器处理，给出状态及误差协方差的局部估计。每个导航周期，卡尔曼子滤波器的数目随着有效测量的数目动态调整。局部估计结果再由协方差交叉法进行信息融合，得出统一的着陆器当前位置速度全局估计值。这样就完成一轮导航更新。The information fusion navigation method proposed by the present invention adopts a distributed filter structure, as shown in FIG. 1 . The lander's autonomous navigation system for fixed-point soft landing missions in deep space exploration is equipped with navigation sensors such as inertial measurement units (including gyroscopes and accelerometers), ranging sensors, speed sensors, and optical imaging sensors. In the whole process of soft landing, the inertial measurement unit measures the angular velocity and non-gravitational acceleration of the lander, and provides the basic information of the position and velocity of the lander for the navigation system after integral extrapolation. This process is called inertial navigation. After the height and attitude of the lander reach the working conditions of each sensor, the ranging sensor outputs slant distance information, the speed sensor outputs relative velocity information, and the optical imaging sensor outputs pixel coordinate information of feature points. This information is processed by independent Kalman sub-filters, giving local estimates of state and error covariance. Each navigation cycle, the number of Kalman sub-filters is dynamically adjusted with the number of valid measurements. The local estimation results are then fused by the covariance intersection method to obtain a unified global estimate of the current position and velocity of the lander. This completes a round of navigation updates.

本发明方法的流程如图2所示，具体步骤如下：The flow process of the inventive method is as shown in Figure 2, and concrete steps are as follows:

1、初始化：1. Initialization:

导航算法的状态变量X取为着陆器在天体中心惯性系(原点位于天体中心，三轴与J2000地心赤道惯性系平行)的位置r和速度v，即：The state variable X of the navigation algorithm is taken as the position r and velocity v of the lander in the inertial system at the center of the celestial body (the origin is at the center of the celestial body, and the three axes are parallel to the J2000 geocentric equatorial inertial system), namely:

$X x = = [\begin{matrix} r r \\ v v \end{matrix}] - - - - - - ((11))$

在着陆开始时刻，由地面测控系统轨道确定结果给出着陆器的状态变量X估计初值及对应的误差协方差初值P₀。At the beginning of landing, the estimated initial value of the state variable X of the lander is given by the orbit determination result of the ground measurement and control system And the corresponding error covariance initial value P ₀ .

2、时间更新：2. Time update:

${\overset{^^}{X x}}_{k k / / k k - - 11} = = {\overset{^^}{X x}}_{k k - - 11} + + [\begin{matrix} {\overset{^^}{v v}}_{k k - - 11} Δ Δ t t + + \frac{11}{22} {C C}_{i i b b,, k k} {\overset{^^}{a a}}_{k k} {Δt Δt}^{22} - - \frac{11}{22} \frac{μ μ}{{(({\overset{^^}{r r}}_{k k - - 11}))}^{33}} {\overset{^^}{r r}}_{k k - - 11} {Δt Δt}^{22} \\ {C C}_{i i b b,, k k} {\overset{^^}{a a}}_{k k} Δ Δ t t - - \frac{μ μ}{{(({\overset{^^}{r r}}_{k k - - 11}))}^{33}} {\overset{^^}{r r}}_{k k - - 11} Δ Δ t t \end{matrix}] - - - - - - ((22))$

其中为k时刻的状态预测，为k-1时刻的状态估计，和分别为k-1时刻着陆器的位置估计和速度估计，即 ${\hat{X}}_{k - 1} = [\begin{matrix} {\hat{r}}_{k - 1} \\ {\hat{v}}_{k - 1} \end{matrix}],$ 为的模，Δt为导航更新周期，C_ib,k为k时刻着陆器本体系(原点位于着陆器质心，三轴与着陆器惯量主轴平行)到天体中心惯性系的方向余弦阵，为k时刻惯性单元中加速度计输出的加速度，μ为着陆天体引力常数。in is the state prediction at time k, is the state estimate at time k-1, and are the position estimate and velocity estimate of the lander at time k-1, respectively, that is, ${\hat{x}}_{k - 1} = [\begin{matrix} {\hat{r}}_{k - 1} \\ {\hat{v}}_{k - 1} \end{matrix}],$ for Δt is the navigation update period, C _ib,k is the direction cosine array of the lander's own system (the origin is at the center of mass of the lander, and the three axes are parallel to the main axis of inertia of the lander) to the inertial system of the center of the celestial body at time k, is the acceleration output by the accelerometer in the inertial unit at time k, and μ is the gravitational constant of the landing celestial body.

C_ib,k是利用对惯性测量单元中陀螺输出的角速度积分来实现更新的，其计算方法属于惯性导航的常规内容，具体公式可参考章仁为编著的《卫星轨道姿态动力学与控制》中第五章相关内容。C _ib,k is updated by integrating the angular velocity output by the gyroscope in the inertial measurement unit. Its calculation method belongs to the routine content of inertial navigation. For the specific formula, please refer to No. Five chapters related content.

公式(2)的含义是利用惯性导航由上一时刻的状态估计外推预测当前时刻的状态。The meaning of formula (2) is to use inertial navigation to extrapolate and predict the state of the current moment from the state estimation at the previous moment.

深空软着陆过程的动力学方程为非线性的，因此对其做泰勒级数展开取线性项来计算状态转移矩阵如下：The dynamic equation of the soft landing process in deep space is nonlinear, so Taylor series expansion and linear terms are used to calculate the state transition matrix as follows:

${Φ Φ}_{k k,, k k - - 11} = = {I I}_{66} + + [\begin{matrix} 00_{33} & {I I}_{33} \\ {\frac{\partial \partial ((- - μ μ r r / / {r r}^{33}))}{\partial \partial {r r}^{T T}} | |}_{r r = = {\overset{^^}{r r}}_{k k / / k k - - 11}} & 00_{33} \end{matrix}] Δ Δ t t - - - - - - ((33))$

其中I₆为6阶单位矩阵，I₃为3阶单位矩阵，0₃为3阶零矩阵，符号T表示矩阵转置，r为r的模，为k时刻的状态预测，即中前3个元素。Among them, I ₆ is the 6th-order identity matrix, I ₃ is the 3rd-order identity matrix, 0 ₃ is the 3rd-order zero matrix, the symbol T represents matrix transposition, r is the modulus of r, is the state prediction at time k, namely in the first 3 elements.

利用状态转移矩阵对误差协方差进行时间更新：Time updates of the error covariance using the state transition matrix:

P_k/k-1＝Φ_k,k-1P_k-1(Φ_k,k-1)^T+Q (4)P _k/k-1 ＝Φ _k,k-1 P _k-1 (Φ _k,k-1 ) ^T +Q (4)

其中P_k/k-1为k时刻的误差协方差预测；Q是系统噪声方差阵，属于已知参数，这里的系统噪声是指除中心天体球形引力和推力器推力外其余作用于着陆器力的合成加速度。Among them, P _k/k-1 is the error covariance prediction at time k; Q is the system noise variance matrix, which belongs to known parameters. The system noise here refers to the forces acting on the lander except the spherical gravity of the central celestial body and the thrust of the thruster. synthetic acceleration.

3、测量有效性判断：3. Judgment of measurement validity:

首先判断敏感器是否输出了最新数据，其次判断该数据是否在有效范围内，例如利用最大最小阈值来判断。只有有效的测量才进入下面的子滤波器处理。若有n个测量有效则后面将建立n个子滤波器，用上标i来标注，i＝1,…,n。Firstly, it is judged whether the sensor has output the latest data, and secondly, it is judged whether the data is within the valid range, for example, by using the maximum and minimum thresholds. Only valid measurements enter the following sub-filter processing. If n measurements are valid, n sub-filters will be established later, marked with superscript i, i=1,...,n.

4、惯性导航输出：4. Inertial navigation output:

如果在第3步判断中，斜距、相对速度、特征点像素坐标测量中至少有一个有效，则跳过本步骤，直接执行第5步。如果测量全部无效则将第2步中惯性导航给出的状态预测和误差协方差预测P_k/k-1直接输出作为本轮导航计算结果，并跳过第5和6步直接结束本轮导航计算，等待下一轮导航计算时间到来，重新从第3步开始计算。If at least one of the slope distance, relative velocity, and feature point pixel coordinate measurement is valid in the third step, skip this step and directly execute step 5. If all measurements are invalid, the state prediction given by inertial navigation in step 2 And the error covariance prediction P _k/k-1 is directly output as the result of the current round of navigation calculation, and skip steps 5 and 6 to directly end the current round of navigation calculation, wait for the arrival of the next round of navigation calculation time, and start from step 3 again calculate.

惯性导航输出的公式如下The formula for the inertial navigation output is as follows

${\overset{^^}{X x}}_{k k} = = {\overset{^^}{X x}}_{k k / / k k - - 11} - - - - - - ((55))$

P_k＝P_k/k-1 (6) _Pk = _Pk/k-1 (6)

5、测量更新：5. Measurement update:

记第i个有效测量的测量方程为hⁱ(X)。Denote the measurement equation of the i-th effective measurement as h ⁱ (X).

当有效测量为斜距时，hⁱ(X)的表达式如下：When the effective measurement is slant distance, the expression of h ⁱ (X) is as follows:

${h h}^{i i} ((X x)) = = {s the s}_{11} [[((r r - - R R)) / / ((- - \frac{r r}{r r} {C C}_{i i b b,, k k} {A A}_{11})) + + {b b}_{11}]] - - - - - - ((77))$

其中s_l为测距敏感器的标度因数，r为位置r的模，R为天体的参考半径，A_l为测距敏感器在着陆器本体坐标系的安装矩阵，b_l为测距敏感器的常值偏差。where s _l is the scaling factor of the ranging sensor, r is the modulus of the position r, R is the reference radius of the celestial body, A _l is the installation matrix of the ranging sensor in the lander body coordinate system, b _l is the ranging sensor The constant value deviation of the instrument.

当有效测量为相对速度时，hⁱ(X)的表达式如下：When the effective measurement is relative velocity, the expression of h ⁱ (X) is as follows:

${h h}^{i i} ((X x)) = = {s the s}_{u u} [[{A A}_{u u} {C C}_{i i b b,, k k}^{T T} ((v v - - ω ω \times \times r r)) + + {b b}_{u u}]] - - - - - - ((88))$

其中s_u为测速敏感器的标度因数，A_u为测距敏感器在着陆器本体坐标系的安装矩阵，ω为天体的自转角速度，b_u为测速敏感器的常值偏差。Among them, _u is the scaling factor of the velocity sensor, A _u is the installation matrix of the ranging sensor in the lander body coordinate system, ω is the rotation angular velocity of the celestial body, and _bu is the constant deviation of the velocity sensor.

当有效测量为特征点像素坐标时，hⁱ(X)的表达式如下：When the effective measurement is the pixel coordinates of feature points, the expression of h ⁱ (X) is as follows:

${h h}^{i i} ((X x)) = = {K K}_{o o} {A A}_{o o} {C C}_{i i b b,, k k}^{T T} \frac{r r - - {r r}_{a a}}{| | | | r r - - {r r}_{a a} | | | |} - - - - - - ((99))$

其中K_o为光学成像敏感器像素坐标转换系数，A_o为光学成像敏感器在着陆器本体坐标系下的安装矩阵，r_a为天体表面特征点的位置，符号|| ||表示求矢量的模。Where K _o is the pixel coordinate conversion coefficient of the optical imaging sensor, A _o is the installation matrix of the optical imaging sensor in the lander body coordinate system, r _a is the position of the feature point on the surface of the celestial body, and the symbol || || mold.

由于hⁱ(X)为状态变量的非线性表达式，因此对其求偏导得到线性的k时刻的测量矩阵如下：Since h ⁱ (X) is a nonlinear expression of the state variable, the partial derivative of it is obtained to obtain the linear measurement matrix at time k as follows:

${H h}_{k k}^{i i} = = {\frac{\partial \partial {h h}^{i i} ((X x))}{\partial \partial X x} | |}_{X x = = {\overset{^^}{X x}}_{k k,, k k - - 11}} - - - - - - ((1010))$

由误差协方差和测量矩阵计算k时刻的第i个子滤波器的滤波增益矩阵如下：Calculate the filter gain matrix of the i-th sub-filter at time k from the error covariance and measurement matrix as follows:

${K K}_{k k}^{i i} = = {P P}_{k k / / k k - - 11} {(({H h}_{k k}^{i i}))}^{T T} {[[{H h}_{k k}^{i i} {P P}_{k k / / k k - - 11} {(({H h}_{k k}^{i i}))}^{T T} + + {R R}^{i i}]]}^{- - 11} - - - - - - ((1111))$

其中Rⁱ为第i个测量的测量噪声方差阵，属于已知参数，符号-1表示求矩阵的逆。Among them, R ⁱ is the measurement noise variance matrix of the i-th measurement, which is a known parameter, and the symbol -1 means to find the inverse of the matrix.

由k时刻导航敏感器输出的测量计算子滤波器的状态估计如下：Measurements output by navigation sensors at time k Compute the state estimates of the sub-filters as follows:

${\overset{^^}{X x}}_{k k}^{i i} = = {\overset{^^}{X x}}_{k k / / k k - - 11} + + {K K}_{k k}^{i i} [[{Z Z}_{k k}^{i i} - - {h h}^{i i} (({\overset{^^}{X x}}_{k k / / k k - - 11}))]] - - - - - - ((1212))$

公式(12)中部分称为测量残差，该值左乘滤波增益矩阵得到状态修正。In formula (12) part is called the measurement residual, and this value is left multiplied by the filter gain matrix Get status fixes.

由滤波增益矩阵和测量矩阵计算k时刻子滤波器的误差协方差估计如下：The error covariance estimation of the sub-filter at time k is estimated from the filter gain matrix and the measurement matrix as follows:

${P P}_{k k}^{i i} = = ((I I - - {K K}_{k k}^{i i} {H h}_{k k}^{i i})) {P P}_{k k / / k k - - 11} - - - - - - ((1313))$

6、估计融合：6. Estimate fusion:

计算子滤波器的误差协方差阵的迹：Compute the trace of the error covariance matrix for a subfilter:

${ϵ ϵ}_{i i} = = t t r r (({P P}_{k k}^{i i})) - - - - - - ((1414))$

其中tr( )表示求矩阵的迹。Among them, tr( ) means to find the trace of the matrix.

计算子滤波器的非负加权系数：Compute nonnegative weighting coefficients for subfilters:

${ω ω}_{i i} = = \frac{11 / / {ϵ ϵ}_{i i}}{{Σ Σ}_{j j = = 11}^{n no} 11 / / {ϵ ϵ}_{j j}} - - - - - - ((1515))$

其中非负加权系数ω_i满足ω₁+…+ω_n＝1。Wherein the non-negative weighting coefficient ω _i satisfies ω ₁ + . . . +ω _n =1.

计算k时刻信息融合后的误差协方差：Calculate the error covariance after information fusion at time k:

${P P}_{k k} = = {[[{Σ Σ}_{i i = = 11}^{n no} {(({P P}_{k k}^{i i}))}^{- - 11} {ω ω}_{i i}]]}^{- - 11} - - - - - - ((1616))$

计算k时刻信息融合后的状态估计：Calculate the state estimate after information fusion at time k:

${\overset{^^}{X x}}_{k k} = = {P P}_{k k} [[{Σ Σ}_{i i = = 11}^{n no} {ω ω}_{i i} {(({P P}_{k k}^{i i}))}^{- - 11} {\overset{^^}{X x}}_{k k}^{i i}]] - - - - - - ((1717))$

该步骤为本发明的重点，即用协方差交叉法实现信息融合。即为导航系统的在k时刻的输出结果。This step is the key point of the present invention, that is, information fusion is realized by covariance intersection method. That is, the output result of the navigation system at time k.

到下一次导航更新时重复上述步骤即可，直到导航任务结束。Repeat the above steps until the next navigation update until the navigation task ends.

实施例Example

深空定点软着陆的初始轨道高度取15km。导航初始位置误差为径向1000m，法向-1000m，前向1000m；速度误差为径向1m、法向-1m、前向1m。测距标度因数误差0.1％，常值偏差0.1m，测量噪声2％。测速标度因数误差0.13％，常值偏差0.01m/s，测量噪声0.16m/s。路标像素坐标误差0.1像素。陀螺随机漂移0.2°/h，常值漂移0.2°/h。加速度计测量噪声0.002m/s²。光学导航敏感器从动力下降起工作，采样周期取20s。测距测速敏感器在10km高度处引入。The initial orbit height of fixed-point soft landing in deep space is 15km. The initial navigation position error is 1000m in the radial direction, -1000m in the normal direction, and 1000m in the forward direction; the speed error is 1m in the radial direction, -1m in the normal direction, and 1m in the forward direction. The ranging scale factor error is 0.1%, the constant value deviation is 0.1m, and the measurement noise is 2%. The speed measurement scale factor error is 0.13%, the constant value deviation is 0.01m/s, and the measurement noise is 0.16m/s. The road sign pixel coordinate error is 0.1 pixel. The random drift of the gyro is 0.2°/h, and the constant drift is 0.2°/h. Accelerometer measurement noise 0.002m/s ² . The optical navigation sensor works from power down, and the sampling period is 20s. The ranging speed sensor is introduced at an altitude of 10km.

软着陆过程在天东北三个方向的位置和速度导航估计误差分别如图3和图4所示。从图3中可以看出，引入路标像素坐标测量信息后位置估计误差相比初始误差有了显著修正，不过高度估计误差仍然较大。从335s开始引入测距修正，高度估计误差逐渐下降，到软着陆末期导航位置精度满足任务要求。从图4可以看出，速度估计误差首先随着惯性导航漂移不断增大，到335s开始引入测速起得到修正，经过短暂的震荡很快收敛到较高精度，到软着陆末期导航速度精度满足任务要求。该仿真结果验证了本发明方法的有效性。The position and velocity navigation estimation errors in the three directions of Tiandongbei during the soft landing process are shown in Figure 3 and Figure 4, respectively. It can be seen from Figure 3 that after the introduction of landmark pixel coordinate measurement information, the position estimation error has been significantly corrected compared with the initial error, but the height estimation error is still relatively large. Starting from 335s, the ranging correction was introduced, and the altitude estimation error gradually decreased, and the navigation position accuracy met the mission requirements at the end of the soft landing. It can be seen from Figure 4 that the speed estimation error first increases with the inertial navigation drift, and it is corrected after the introduction of speed measurement at 335s. After a short period of oscillation, it quickly converges to a higher accuracy, and the navigation speed accuracy meets the task at the end of the soft landing. Require. The simulation results verify the effectiveness of the method of the present invention.

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。The content that is not described in detail in the description of the present invention belongs to the well-known technology of those skilled in the art.

Claims

1. An information fusion navigation method of a fixed-point soft landing process for deep space exploration, characterized in that it comprises the steps:

(1) The position r and velocity v of the lander in the inertial system of the center of the celestial body are used as the state variable X,

x = [\begin{matrix} r \\ v \end{matrix}],

where the estimated initial value of X And the corresponding initial value of error covariance P ₀ is given by the orbit determination result of the ground measurement and control system at the moment of landing;

(2) Use the state estimation and extrapolation of the inertial navigation system installed on the lander at the last moment to predict the state of the lander at the current moment and the corresponding error covariance,

{\overset{^^}{X x}}_{k k / / k k - - 11} = = {\overset{^^}{X x}}_{k k - - 11} + + [\begin{matrix} {\overset{^^}{v v}}_{k k - - 11} Δ Δ t t + + \frac{11}{22} {C C}_{i i b b,, k k} {\overset{^^}{a a}}_{k k} {Δt Δt}^{22} - - \frac{11}{22} \frac{μ μ}{{(({\overset{^^}{r r}}_{k k - - 11}))}^{33}} {\overset{^^}{r r}}_{k k - - 11} {Δt Δt}^{22} \\ {C C}_{i i b b,, k k} {\overset{^^}{a a}}_{k k} Δ Δ t t - - \frac{μ μ}{{(({\overset{^^}{r r}}_{k k - - 11}))}^{33}} {\overset{^^}{r r}}_{k k - - 11} Δ Δ t t \end{matrix}]

P _k/k-1 ＝Φ _k,k-1 P _k-1 (Φ _k,k-1 ) ^T +Q

{Φ Φ}_{k k,, k k - - 11} = = {I I}_{66} + + [\begin{matrix} 00_{33} & {I I}_{33} \\ {\frac{\partial \partial ((- - μ μ r r / / {r r}^{33}))}{\partial \partial {r r}^{T T}} | |}_{r r = = {\overset{^^}{r r}}_{k k / / k k - - 11}} & 00_{33} \end{matrix}] Δ Δ t t

in is the state prediction of the lander at time k, is the state estimation of the lander at time k-1, and are the position estimate and velocity estimate of the lander at time k-1, respectively,

{\hat{x}}_{k - 1} = [\begin{matrix} {\hat{r}}_{k - 1} \\ {\hat{v}}_{k - 1} \end{matrix}],

for Δt is the navigation update cycle, C _ib,k is the cosine array of the direction from the lander system to the center inertial system of the landing celestial body at time k, is the acceleration output by the accelerometer in the inertial unit at time k, μ is the gravitational constant of the landing celestial body, I ₆ is the 6th-order identity matrix, I ₃ is the 3rd-order identity matrix, 0 ₃ is the 3rd-order zero matrix, and the symbol T represents matrix transposition, r is the modulus of r, is the position and state prediction of the lander at time k, P _k/k-1 is the error covariance prediction at time k, and Q is the system noise variance matrix;

(3) Judging whether the ranging sensor, velocity measuring sensor, and optical imaging sensor installed on the lander have output new valid data, record the number of new valid data as n, if n=0, then directly step The result of (2) is output as the navigation result of the current moment, ends and exits; If n is not 0, then enter step (4);

(4) For each new effective data, a sub-Kalman filter is established separately, and the state estimation of each sub-Kalman filter is obtained and the corresponding error covariance

{\overset{^^}{X x}}_{k k}^{i i} = = {\overset{^^}{X x}}_{k k / / k k - - 11} + + {K K}_{k k}^{i i} [[{Z Z}_{k k}^{i i} - - {h h}^{i i} (({\overset{^^}{X x}}_{k k / / k k - - 11}))]]

{P P}_{k k}^{i i} = = ((I I - - {K K}_{k k}^{i i} {H h}_{k k}^{i i})) {P P}_{k k / / k k - - 11}

{K K}_{k k}^{i i} = = {P P}_{k k / / k k - - 11} {(({H h}_{k k}^{i i}))}^{T T} {[[{H h}_{k k}^{i i} {P P}_{k k / / k k - - 11} {(({H h}_{k k}^{i i}))}^{T T} + + {R R}^{i i}]]}^{- - 11}

{H h}_{k k}^{i i} = = {\frac{\partial \partial {h h}^{i i} ((X x))}{\partial \partial X x} | |}_{X x = = {\overset{^^}{X x}}_{k k,, k k - - 11}}

Among them, h ⁱ (X) is the i-th new effective data, i=1,...,n, R ⁱ is the measurement noise variance matrix of the i-th new effective data corresponding to the sensor, is the output of the sensor corresponding to the i-th new valid data at time k, and the symbol -1 means to find the inverse of the matrix;

(5) Carry out information fusion to the output of each sub-filter in step (4), obtain the navigation result of current moment and output,

{\overset{^^}{X x}}_{k k} = = {P P}_{k k} [[{Σ Σ}_{i i = = 11}^{n no} {ω ω}_{i i} {(({P P}_{k k}^{i i}))}^{- - 11} {\overset{^^}{X x}}_{k k}^{i i}]]

{P P}_{k k} = = {[[{Σ Σ}_{i i = = 11}^{n no} {(({P P}_{k k}^{i i}))}^{- - 11} {ω ω}_{i i}]]}^{- - 11}

where the non-negative weighting coefficient And satisfy ω ₁ +...+ω _n =1, tr( ) means to find the trace of the matrix.