Failure satellite close-range relative pose adaptive cubature kalman filter estimation method

Abstract

The present application relates to a kind of failure satellite close relative pose adaptive cubature Kalman filtering estimation method, belong to the relative navigation field of failure satellite.The method is realized with failure satellite in observation phase relative navigation using monocular camera.Firstly, the relative position equation set between satellites, i.e.C-W equation is used to establish the translational kinematic model between tracking spacecraft and the center of mass of failure satellite;Then, it is assumed that failure satellite does spin motion with constant angular velocity without perturbation, uses quaternion to represent the attitude transformation between its current attitude and initial attitude, and uses the differential equation of attitude transformation matrix as its rotational kinematic model.Finally, cubature Kalman filtering algorithm is used as the filtering algorithm of nonlinear system, and the adaptive measurement noise R is realized using fuzzy algorithm.Compared with the conventional cubature Kalman filtering method, the accuracy is improved while the stability of filtering program is ensured.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of relative navigation of failed satellites, in particular to a failed satellite close-range relative pose adaptive cubature Kalman filter estimation method. BACKGROUND

[0002] The progress of human science and technology constantly promotes the exploration and utilization of outer space, and the number of spacecraft launched by countries around the world is constantly setting new records. However, so far some of these on-orbit spacecraft have been unable to work normally, and it is of considerable practical significance to space capture for these failed satellites with complete structures that match the current level of technological development. Failed satellites usually perform complex tumbling motion under the action of the Earth's gravity and space perturbation forces. In fact, when the spacecraft runs out of fuel, and the elastic vibration energy of the flexible accessories on the spacecraft is completely dissipated, the spacecraft becomes a non-acting free-floating rigid body and performs single-axis rotation or tumbling motion. In the process of tracking, approaching and capturing failed satellites in the on-orbit mission of the spacecraft, the relative position and attitude information between the two is particularly important.

[0003] Since the failed satellite cannot use inter-satellite links for communication, its own sensitive devices such as gyroscopes and accelerometers cannot provide attitude and position information. Optical detection methods have the advantages of non-contact measurement, high measurement accuracy, etc., and are currently the mainstream way of relative pose measurement of failed satellites. Optical measurement methods mainly include monocular vision cameras, binocular vision cameras, depth cameras, and laser radars, among which monocular cameras attract the attention of many researchers due to their simple equipment, low cost, and ease of implementation. SUMMARY

[0004] The purpose of the present application is to address the practical problem of obtaining real-time accurate relative pose between failed satellites and tracking spacecraft in the close-range observation stage in the failed satellite tracking and capturing or docking mission using monocular vision cameras, and to propose a failed satellite close-range relative pose adaptive cubature Kalman filter estimation method. This method is based on the real-time pose estimation filter of the cubature Kalman filter algorithm, and uses the square root filter method to improve the convergence problem and stability problem in the filtering process, so that the filter can also converge under a large initial error. At the same time, a fuzzy control method is used to realize the self-adaptation of the measurement noise R in the filtering process, and to improve the accuracy of the filter.

[0005] The above-mentioned purpose is achieved by the following technical solutions:

[0006] The failed satellite close-range relative pose adaptive cubature Kalman filter estimation method comprises the following steps:

[0007] (1) The relative position between the tracking spacecraft and the failed satellite is represented by the inter-satellite relative orbit dynamics equation, the relative attitude of the failed satellite is represented by the attitude quaternion, and the failed satellite is assumed to do uniform rotation with constant angular velocity in space. The relative kinematics model between the tracking spacecraft and the failed satellite is established and used as the system equation. The relative position, relative attitude, relative velocity and relative angular velocity between the tracking spacecraft and the failed satellite are used as the state variables.

[0008] (2) The sequence image data collected by the monocular camera carried by the tracking spacecraft is used as the measurement of the system. The position information of the camera relative to the center of mass of the tracking spacecraft, the attitude information of the camera coordinate system relative to the tracking spacecraft coordinate system, and the mapping model of the camera are used to establish the measurement function of the system.

[0009] (3) The initial state value of the filtering system is given, and the relative kinematics model established in step (1) is used as the system equation to predict the state variables between the tracking spacecraft and the failed satellite at the next time.

[0010] (4) The two-dimensional coordinates of the feature points of the failed satellite in the current image are used as the measurement, the fuzzy adaptive logic controller is used to correct the noise parameters, and the predicted state quantity in step (3) is corrected. The state variables are output as the filtering results.

[0011] Further, the specific method of step (1) is:

[0012] First, define the relevant coordinate systems. Assuming that the failed satellite runs on a circular orbit, the tracking spacecraft estimates its relative position and attitude through the camera, then define the following coordinate systems:

[0013] O I X I Y I Z I is the equatorial inertial coordinate system, the origin is the center of the earth, X I axis is in the equatorial plane, pointing to the star, Z I axis along the direction of the earth's rotation axis, Y I axis, X I axis and Z I axis form a right-handed coordinate system.

[0014] O C X C Y C Z C is the tracking spacecraft orbit coordinate system, denoted as coordinate system F c , the origin O C is located at the center of mass of the tracking spacecraft, X C axis is the center of the earth pointing to the center of mass of the tracking spacecraft vector direction, YC axis in the orbit plane of the tracking spacecraft, perpendicular to X C axis, and the angle between the axis and the velocity of the tracking spacecraft is an acute angle, Z C axis is determined according to the right-hand rule;

[0015] O T X T Y T Z T is an orbit coordinate system of the failed satellite, denoted as coordinate system F T , the origin O T is located at the center of mass of the failed satellite, X T axis is a vector pointing from the center of the Earth to the center of mass of the failed satellite, Y T axis in the orbit plane of the failed satellite, perpendicular to X T axis, and the angle between the axis and the flight velocity of the failed satellite is an acute angle, Z T axis is determined according to the right-hand rule;

[0016] O B X B Y B Z B is a carrier coordinate system of the failed satellite, the origin of the coordinate system coincides with the center of mass of the failed satellite, the coordinate axes are fixed on the failed satellite, and the coordinate axes point in the same direction as the tracking spacecraft orbit coordinate system at the initial time;

[0017] O0X0Y0Z0 is a carrier coordinate system of the tracking spacecraft, the origin of the coordinate system coincides with the center of mass of the tracking spacecraft, the coordinate axes are fixed on the spacecraft, and it is assumed that the tracking spacecraft is spin-stabilized, that is, the tracking spacecraft carrier coordinate system O0X0Y0Z0 coincides with the tracking spacecraft orbit coordinate system O C X C Y C Z C ;

[0018] O ca X ca Y ca Z ca is a camera coordinate system, the origin O ca of the coordinate system is located at the optical center of the monocular camera, Z ca axis is in the direction of the optical axis and perpendicular to the imaging plane, X ca axis is parallel to the upper and lower edges of the imaging plane and points to the left, Y ca axis is parallel to the left and right edges of the imaging plane and points upward;

[0019] O p X p Y p is an image coordinate system, which is a plane coordinate system. The plane of the coordinate system is the imaging plane of the camera, the origin is the principal point of the camera imaging, X pThe axis is parallel to the lower edge of the imaging plane and points to the left, Y p The axis is parallel to the left edge of the imaging plane and points upward.

[0020] The relative kinematics model of the orbit is established by C-W equation, which is expressed as:

[0021]

[0022] Where s represents the position of the failed satellite mass center in the coordinate system F c X C axis component; y represents the position of the failed satellite mass center in the coordinate system F c Y C axis component; z represents the position of the failed satellite mass center in the coordinate system F c Z C axis component; represents the velocity of the failed satellite mass center in the coordinate system F c X C axis component; represents the velocity of the failed satellite mass center in the coordinate system F c Y C axis component; represents the velocity of the failed satellite mass center in the coordinate system F c Z C axis component, and the superscript T represents the transpose of the matrix; represents the acceleration of the failed satellite mass center in the coordinate system F c X C axis component; represents the acceleration of the failed satellite mass center in the coordinate system F c Y C axis component; represents the acceleration of the failed satellite mass center in the coordinate system F c Z C axis component; n represents the average angular velocity of the tracking spacecraft orbit, and μ is the coefficient of the earth's gravity, and a is the semi-major axis of the tracking spacecraft orbit;

[0023] The relative attitude quaternion q = [q0 q1 q2 q3] is defined. T is the attitude transformation quaternion between the current failed satellite feature point set and the initial feature point set, where q0, q1, q2, and q3 are four parameters of the attitude quaternion, and the corresponding attitude transformation matrix R T is:

[0024]

[0025] The relative attitude kinematics equation represented by the attitude transformation matrix is:

[0026]

[0027] wherein, is the derivative of the attitude transformation matrix R T , w(t) is the angular velocity vector of the failed satellite at time t; w(t) x is the skew-symmetric matrix corresponding to the angular velocity vector of the failed satellite, R T (0) is the attitude transformation matrix at time 0, the value of which is R T0 is the initial attitude matrix, the value of which can be calculated by the PNP algorithm using the initial three-dimensional point cloud and the two-dimensional feature points of the first image;

[0028] The system state variable is selected as:

[0029]

[0030] Q k = [q 0，k , q 1，k , q 2，k , q 3，k ] T , ω k = [ω x，k , ω y，k , ω z，k ] T

[0031] wherein, wherein T k is the velocity position quantity at time k, wherein s k represents the component of the position of the center of mass of the failed satellite at time k in the X c axis of the coordinate system F C ; y k represents the component of the position of the center of mass of the failed satellite at time k in the Y c axis of the coordinate system F C ; z k represents the component of the position of the center of mass of the failed satellite at time k in the Z c axis of the coordinate system F C ; represents the component of the velocity of the center of mass of the failed satellite at time k in the X c axis of the coordinate system F C ; represents the component of the velocity of the center of mass of the failed satellite at time k in the Y c axis of the coordinate system F C ; represents the component of the velocity of the center of mass of the failed satellite at time k in the Z c axis of the coordinate system F C ; Q k is the attitude quantity at time k, q 0，k , q 1，k , q2，k , q 3，k are four parameters of the quaternion Q k of the k-th time attitude; ω k is the angular velocity of the failed satellite at the k-th time, ω x，k , ω y，k , ω z，k are three components of ω k .

[0032] The state equation F(.) is:

[0033]

[0034] T k+1 = A k T k

[0035]

[0036] where T k+1 is the k+1-th time velocity position vector. A k is the k-th time position vector state transition matrix, and T is the time interval.

[0037] The attitude vector state transition mode is:

[0038]

[0039] Δθ k = ω k T

[0040] Δθ k is the equivalent rotation vector between the k-th time and the k+1-th time, and ||Δθ k || is the modulus of the equivalent rotation vector. is the corresponding four-dimensional skew-symmetric matrix of Δθ k .

[0041] Δθ k = [Δθ x，k Δθ y，k Δθ z，k ] T

[0042]

[0043] Δθ x，k , Δθ y，k , Δθ z，k are three components of the equivalent rotation vector.

[0044] The angular velocity vector and the failed satellite body coordinate vector state update are:

[0045] ω k+1 = ωk

[0046] ω k+1 is the angular velocity of the target satellite at time k.

[0047] Further, the specific method of step (2) is:

[0048] Select the measurement z k at time k.

[0049] z k = [x 1，k y 1，k x 2，k y 2，k …x j，k y j，k ] T , j = 1, 2…14

[0050] where x j，k represents the horizontal coordinate of the jth feature point detected in the image at time k, y j，k represents the vertical coordinate of the jth feature point detected in the image at time k, and the corresponding measurement function is:

[0051]

[0052] where, is the predicted image coordinate of the jth feature point in the image coordinate system at time k, and f, is the focal length of the camera, x j，c，k is the coordinate of the jth feature point in the tracking spacecraft orbit coordinate system at time k, x j，c，k (1), x j，c，k (2), and x j，c，k (3) are the 1st, 2nd, and 3rd components of x j，c，k , respectively.

[0053]

[0054] where rotm fix is the attitude transformation matrix of the camera coordinate system relative to the tracking spacecraft carrier coordinate system, C T is the coordinate column vector of the camera optical center in the tracking spacecraft carrier coordinate system, X j , Y j , and Z j represent the coordinate of the jth feature point in the failed satellite carrier coordinate system, j = 1, 2…14.

[0055] Further, the specific method of step (3) is:

[0056] First, the parameters and states of the filter are initialized, the initial state X0and the initial covariance matrix P0are set, the system noise Q k , the initial measurement noise R0, and then the sample point set X

[0057]

[0058] P k-1|k-1 = S k-1|k-1 (S k-1|k-1 ) T

[0059]

[0060] where X i，k-1|k-1 is the generated sample point set, P k-1|k-1 is the covariance matrix at time k-1, and P0is the initial covariance matrix at time 0. S k-1|k-1 is the Cholesky decomposition of P k-1|k-1 , and the resulting covariance matrix square root is ξ i is the cubature point set, and m is a defined intermediate variable, and m = 2n x , n x is the dimension of the state vector, and n x is the nx-dimensional unit vector e = [1, 0,..., 0] T , and the symbol [1] represents the point set generated by the full permutation and element sign change of the nx-dimensional unit vector e, and [1] i represents the i-th point in the point set [1] is the state estimate at time k-1, and X0is the initial state at time 0;

[0061] Substitute the sample point set X i，k-1|k-1 into the state equation F(.), perform state transition, and obtain the cubature points corresponding to the state prediction value at time k

[0062]

[0063] get one-step prediction value and one-step prediction mean square error S k，k-1 :

[0064]

[0065]

[0066] where Tria(B) represents the transpose of the upper triangular matrix obtained by orthogonal triangular decomposition of the matrix B T , is the square root of the system noise matrix Qk, that is, is an intermediate variable defined as The calculation formula is

[0067]

[0068] Construct new sampling points and calculate the measurement prediction volume points by the measurement function h()

[0069]

[0070]

[0071] Calculate the measurement prediction value Square root of the prediction variance Theoretical innovation covariance and cross covariance

[0072]

[0073]

[0074]

[0075]

[0076] wherein is the measurement noise matrix R k Square root, that is and X k，k-1 is an intermediate variable in the calculation process, and the calculation formula is

[0077]

[0078]

[0079] Further, the specific method of step (4) is:

[0080] According to the relationship between the innovation variance matrix and the theoretical variance matrix, the measurement noise matrix R k at time k is adjusted by using a fuzzy control algorithm:

[0081]

[0082]

[0083]

[0084] wherein v k , v d are innovation vectors, vk is the innovation vector at time k, v d is the innovation vector at time d. z k is the measurement vector at time k, M is the width of the sliding window, generally taken as 4, tr() is the trace of a matrix, and if the ratio α k is less than 0.95 or greater than 1.05, the ratio can be brought back to between 0.95 and 1.05 by adjusting R k . is the actual innovation variance matrix monitored.

[0085] R k = λ k R k-1

[0086] where R k-1 is the measurement noise at time k-1, λ k is the adjustment factor generated by the fuzzy inference system, which consists of three parts, namely the fuzzification process, the fuzzy control rule generation process and the defuzzification process. Let L represent the fuzzy set "small", E represent the fuzzy set "equal", and Mor represent the fuzzy set "large". According to actual requirements, the fuzzy control rules are set as follows:

[0087] If α k ∈ L, then λ k ∈ L

[0088] If α k ∈ E, then λ k ∈ E

[0089] If α k ∈ Mor, then λ k ∈ Mor

[0090] The method of defuzzification finally adopted is the barycenter method, the essence of which is weighted average, that is, the weighted coefficient is taken as the membership degree of the corresponding element, and the output is taken as the barycenter horizontal coordinate λ k of the area surrounded by the membership function curve.

[0091] Finally, the measurement update is performed:

[0092]

[0093]

[0094]

[0095] where K K is the filter gain matrix, is the state estimate at time k, S k is the square root of the covariance matrix at time k.

[0096] Beneficial effects: the square root cubature Kalman filtering algorithm ensures the accuracy and stability of the nonlinear filtering process, and the fuzzy control adaptive algorithm realizes the adaptive ability to the measurement noise. BRIEF DESCRIPTION OF DRAWINGS

[0097] Figure 1 is a flowchart of the present application;

[0098] Figure 2 is the membership function of the fuzzy inference system input α k ;

[0099] Figure 3 is the membership function of the fuzzy inference system output λ k ;

[0100] Figure 4 is the relative position error and relative angle error diagram of the proposed filtering algorithm (FSRCKF) and the standard cubature Kalman filter (CKF), wherein (a) is the x relative position error / m, (b) is the y relative position error / m, (c) is the z relative position error / m, and (d) is the relative angle error / deg. DETAILED DESCRIPTION

[0101] The technical solutions of the present application will be described in detail below in combination with the drawings.

[0102] As shown in Figure 1 , the present application provides a failure satellite close-range relative pose adaptive cubature Kalman filtering estimation method, which comprises the following steps:

[0103] (1) The relative position between the tracking spacecraft and the failure satellite is characterized by using the relative orbit dynamics equation between the satellites, the relative attitude of the failure satellite is characterized by using the attitude quaternion, and it is assumed that the failure satellite does uniform rotation in space with constant angular velocity, the relative kinematics model between the tracking spacecraft and the failure satellite is established and used as the system equation, and the relative position, relative attitude, relative velocity and relative angular velocity between the tracking spacecraft and the failure satellite are used as the state variables;

[0104] (2) The sequence image data collected by the monocular camera carried by the tracking spacecraft is used as the measurement of the system, the position information of the camera relative to the center of mass of the tracking spacecraft is used, the attitude information of the camera coordinate system relative to the tracking spacecraft carrier system is used, and the mapping model of the camera is used to establish the measurement function of the system;

[0105] (3) The initial state value of the filtering system is given, and the relative kinematics model established in step (1) is used as the system equation to predict the state variables of the tracking spacecraft and the failure satellite at the next time;

[0106] (4) Using the two-dimensional coordinates of the feature points of the failed satellite collected in step (2) in the current time image as a measurement, a fuzzy adaptive logic controller is used to correct the noise parameters, and the predicted state quantity in step (3) is corrected, and the state variable is output as a filtering result.

[0107] Further, the specific method of step (1) is:

[0108] First, define the relevant coordinate system. Assuming that the failed satellite runs on a circular orbit, the tracking spacecraft estimates the relative pose through the camera, and the following coordinate system is defined:

[0109] O I X I Y I Z I is the equatorial inertial coordinate system, the origin is the center of the earth, X I axis in the equatorial plane, pointing to the star, Z I axis along the direction of the earth's rotation axis, Y I axis, X I axis and Z I axis constitute a right-handed coordinate system;

[0110] O C X C Y C Z C is the tracking spacecraft orbit coordinate system, denoted as coordinate system F c , the origin O C is located at the center of mass of the tracking spacecraft, X C axis is the center of the earth pointing to the center of mass of the tracking spacecraft vector direction, Y C axis in the tracking spacecraft orbit plane, perpendicular to X C axis, and the included angle with the tracking spacecraft speed is an acute angle, Z C axis is determined according to the right-hand rule;

[0111] O T X T Y T Z T is the failed satellite orbit coordinate system, denoted as coordinate system F T , the origin O T is located at the center of mass of the failed satellite, X T axis is the center of the earth pointing to the center of mass of the failed satellite vector direction, Y T axis in the failed satellite orbit plane, perpendicular to X T axis, and the included angle with the failed satellite flight speed is an acute angle, Z T axis is determined according to the right-hand rule.

[0112] O B X B Y B ZB The coordinate system is the coordinate system of the failed satellite carrier. The origin of the coordinate system coincides with the center of mass of the failed satellite. The coordinate axes are fixed to the failed satellite. The coordinate axes point to coincide with the orbital coordinate system of the tracking spacecraft at the initial moment.

[0113] O0X0Y0Z0 is the coordinate system of the tracking spacecraft carrier, with its origin coinciding with the center of mass of the tracking spacecraft. The coordinate axes are fixed to the spacecraft. It is assumed that the tracking spacecraft achieves spin stability, i.e., the coordinate system O0X0Y0Z0 of the tracking spacecraft carrier coincides with the center of mass of the tracking spacecraft. C X C Y C Z C coincide;

[0114] O ca X ca Y ca Z ca Let O be the camera coordinate system. ca Located at the optical center of the monocular camera, Z ca The axial direction is along the optical axis and perpendicular to the imaging plane, X ca The axis is parallel to the top and bottom edges of the imaging plane and points to the left. ca The axis is parallel to the left and right edges of the imaging plane and points upwards;

[0115] O p X p Y p The image coordinate system is a planar coordinate system. The coordinate system plane is the camera imaging plane, with the origin being the principal point of the camera image. X p The axis is parallel to the top and bottom edges of the imaging plane and points to the left. p The axis is parallel to the left and right edges of the imaging plane and points upwards;

[0116] By C - The W equation establishes the orbital relative kinematics model, C - The W equation is expressed as:

[0117]

[0118] Where s represents the position of the centroid of the failed satellite in coordinate system F c Next X C The axial component; y represents the position of the failed satellite's center of mass in coordinate system F. c Next Y C The axial component; z represents the centroid of the failed satellite in coordinate system F. c Z C Axial component; The velocity of the center of mass of the failed satellite in coordinate system F c Next X C Axial component; the velocity of the failed satellite's center of mass in the coordinate system F c down Y C the axial component; the velocity of the failed satellite's center of mass in the coordinate system F c down Z C the axial component, the superscript T represents the transpose of the matrix; the acceleration of the failed satellite's center of mass in the coordinate system F c down X C the axial component; the acceleration of the failed satellite's center of mass in the coordinate system F c down Y C the axial component; the acceleration of the failed satellite's center of mass in the coordinate system F c down Z C the axial component; n represents the average angular velocity of the orbit of the tracking spacecraft, and μ is the coefficient of the earth's gravity, and a is the semi-major axis of the orbit of the tracking spacecraft;

[0119] Define the relative attitude quaternion q = [q0 q1 q2 q3] T as the attitude transformation quaternion between the current feature point set of the failed satellite and the initial feature point set, wherein q0, q1, q2, q3 are four parameters of the attitude quaternion, and the corresponding attitude transformation matrix R T is:

[0120]

[0121] The relative attitude kinematics equation represented by the attitude transformation matrix is:

[0122]

[0123] wherein, is the derivative of the attitude transformation matrix R T , w(t) is the angular velocity vector of the failed satellite at time t; w(t) x is the skew-symmetric matrix corresponding to the angular velocity vector of the failed satellite, and R T (0) is the attitude transformation matrix at time 0, the value of which is R T0 is the initial attitude matrix, the value of which can be calculated by the PNP algorithm using the initial three-dimensional point cloud and the two-dimensional feature points of the first image;

[0124] The system state variable is selected as:

[0125]

[0126] Q k = [q 0，k , q 1，k , q2，k , q 3，k ] T , ω k = [ω x，k , ω y，k , ω z，k ] T

[0127] wherein, wherein T k is the velocity position quantity at time k, wherein s k represents the component of the position of the center of mass of the failed satellite at time k in the X c axis of the coordinate system F C ; y k represents the component of the position of the center of mass of the failed satellite at time k in the Y c axis of the coordinate system F C ; z k represents the component of the position of the center of mass of the failed satellite at time k in the Z c axis of the coordinate system F C ; represents the component of the velocity of the center of mass of the failed satellite at time k in the X c axis of the coordinate system F C ; represents the component of the velocity of the center of mass of the failed satellite at time k in the Y c axis of the coordinate system F C ; represents the component of the velocity of the center of mass of the failed satellite at time k in the Z c axis of the coordinate system F C ; Q k is the attitude quantity at time k, q 0，k , q 1，k , q 2，k , q 3，k are the four parameters of the attitude quaternion Q k at time k; ω k is the angular velocity quantity of the failed satellite at time k, ω x，k , ω y，k , ω z，k are the three components of ω k .

[0128] The state equation F(.) is:

[0129]

[0130] T k+1 = A k T k

[0131]

[0132] wherein T k+1is the position quantity at time k + 1. A k is the position quantity state transition matrix at time k, and T is the time interval.

[0133] The attitude quantity state transition mode is:

[0134]

[0135] Δθ k = ω k T

[0136] Δθ k is the equivalent rotation vector between time k and time k + 1, and ||Δθ k || is the modulus of the equivalent rotation vector. is Δθ k corresponds to a four-dimensional skew-symmetric matrix.

[0137] The angular velocity quantity and the failed satellite carrier body coordinate quantity state update are:

[0138] ω k+1 = ω k

[0139] ω k+1 is the angular velocity quantity at time k + 1.

[0140] Further, the specific method of step (2) is:

[0141] The measurement z k is selected at time k.

[0142] z k = [x 1，k y 1，k x 2，k y 2，k …x j，k y j，k ] T , j = 1, 2…14

[0143] wherein x j，k represents the horizontal coordinate of the jth feature point detected in the image at time k, y j，k represents the vertical coordinate of the jth feature point detected in the image at time k, and the corresponding measurement function is:

[0144]

[0145] wherein, is the predicted image coordinate of the jth feature point in the image coordinate system at time k, and f is the focal length of the camera, x j，c，k is the coordinate of the jth feature point in the tracking spacecraft orbit coordinate system at time k, and xj，c，k (1), x j，c，k (2), x j，c，k (3) are the first, second and third components, respectively; j，c，k

[0146]

[0147] where rotm fix is the attitude transformation matrix of the camera coordinate system relative to the tracking spacecraft carrier coordinate system, C T is the coordinate column vector of the camera optical center in the tracking spacecraft carrier coordinate system, X j , Y j , Z j represent the coordinates of the jth feature point in the failed satellite carrier coordinate system, j = 1, 2…14.

[0148] Further, the specific method of step (3) is:

[0149] First, the parameters and state of the filter are initialized, the initial state X0and the initial covariance matrix P0are set, the system noise Q k , the initial measurement noise R0, and then the initial covariance matrix and the initial state estimate are generated to generate a sample point set,

[0150]

[0151] P k-1|k-1 = S k-1|k-1 (S k-1|k-1 ) T

[0152]

[0153] where X i，k-1|k-1 is the generated sample point set, P k-1|k-1 is the covariance matrix at time k-1, and 0 is the initial covariance matrix P0. S k-1|k-1 is the Cholesky decomposition of P k-1|k-1 , and the covariance matrix square root is obtained, ξ i is the volume point set, and m is a defined intermediate variable, and m = 2n x , n x is the dimension of the state vector, and n x dimension unit vector is e = [1, 0, …, 0] T , the symbol [1] represents the point set generated by the full permutation and element sign change of the n x dimension unit vector e, [1] i represents the ith point in the point set [1] is the state estimate at time k-1, and 0 is the initial state X0;​

[0154] The set of sampling points X i，k-1|k-1 is substituted into the state equation F(.), and the state transition is performed to obtain the volume points corresponding to the state prediction values at k time points

[0155]

[0156] The one-step prediction value is obtained and the one-step prediction mean square error S k，k-1 :

[0157]

[0158]

[0159] wherein Tria(B) represents the transpose of the upper triangular matrix obtained after the orthogonal triangular decomposition of the matrix B T , and is the square root of the system noise matrix Qk, that is, is a defined intermediate variable, and is defined as:

[0160]

[0161] The new sampling points are constructed and the measurement prediction volume points are calculated by the measurement function h()

[0162]

[0163]

[0164] The measurement prediction value is calculated The prediction variance square root The theoretical innovation covariance and the cross covariance

[0165]

[0166]

[0167]

[0168]

[0169] wherein is the square root of the measurement noise matrix R k , that is, and X k，k-1 are intermediate variables in the calculation process, and the calculation formula is:

[0170]

[0171]

[0172] Further, the specific method of step (4) is:

[0173] According to the relationship between the innovation variance matrix and the theoretical variance matrix, the fuzzy control algorithm is used to adjust the measurement noise matrix R k at k time

[0174]

[0175]

[0176]

[0177] wherein v k , v d are innovation vectors, v k is the innovation vector at k time, v d is the innovation vector at d time. z k is the measurement vector obtained at k time, M is the sliding window width, taking 4, tr() is the trace of the matrix, if the ratio α k is less than 0.95 or greater than 1.05, the ratio can be brought back to between 0.95 and 1.05 by adjusting R k , is the actual innovation variance matrix monitored.

[0178] R k = λ k R k-1

[0179] wherein R k-1 is the measurement noise at k-1 time, λ k is the adjustment factor generated by the fuzzy inference system, which consists of three parts, namely the fuzzification process, the fuzzy control rule generation process and the defuzzification process. Let L represent the fuzzy set "small", E represent the fuzzy set "equal", and Mor represent the fuzzy set "large". According to the actual demand, the fuzzy control rule is set as:

[0180] If α k ∈L, then λ k ∈L

[0181] If α k ∈E, then λ k ∈E

[0182] If α k ∈Mor, then λk ∈Mor

[0183] The last defuzzification method takes the barycenter method, the essence of which is weighted average, that is, the weighted coefficient is taken as the membership degree of the corresponding element, and the output is taken as the barycenter horizontal coordinate λ of the area surrounded by the membership function curve k ;

[0184] Finally, the measurement is updated:

[0185]

[0186]

[0187]

[0188] where K K is the filter gain matrix, is the state estimation at time k, S k is the square root of the covariance matrix at time k.

[0189] In step (3) of the embodiment, a data set of 1000 seconds is generated by simulation, the sampling frequency of the data is 5Hz, and the relative position between the mass center of the failed satellite and the tracking satellite, the rotation attitude of the failed satellite, and the pixel coordinates of 14 feature points in the monocular camera after rotation and translation are generated at the same time. The image size is 512x512.

[0190] The relative position error is used to describe the estimation performance of the filter system on the relative position. The relative position error is defined as the difference between the true value of the relative position and the estimated value of the filter algorithm. The relative angle error is used to describe the estimation performance of the filter system on the attitude of the failed satellite. The relative angle error is defined as the modulus φ of the misalignment angle The error quaternion δQ is defined as the product of the true value of the attitude quaternion Q and the conjugate of the estimated value of the attitude quaternion The relationship between the error quaternion and the misalignment angle is The data update rate of the filter system is the number of output data per second. The higher the update rate, the better the real-time performance.

[0191] Simulation experiment:

[0192] The platform of the embodiment is Windows 11 operating system, and the development environment is Matlab R2020b. The CPU used in the experiment is an Inteli5 processor. The specific experimental steps are as follows:

[0193] (1) The initial parameter setting is that the filter initial state quantity is the true value, and the initial covariance matrix is set as:

[0194] P0=diag([I3,1*10 -4*I3, 1*10 -1 *I4, 1*10 -4 *I3])

[0195] (2) Filter result analysis: relative position error and relative angle error evaluation of monocular vision failure satellite pose estimation filter algorithm based on cubature Kalman filter. The smaller the relative position error and relative angle error, the higher the accuracy of the pose estimation. Figure 4 For the comparison of three-axis relative position error and relative angle error of ordinary cubature Kalman filter (CKF) and fuzzy square root cubature Kalman filter (FSRCKF) proposed in the application in the filtering process.

[0196] It can be seen that after the filtering process is stable, the relative position error can be kept less than 0.4m, which is reduced by 2m compared with the ordinary CKF position error; the relative angle error is less than 1°, which is reduced by 1.2° compared with the ordinary CKF angle error.

Claims

1. A method for estimating the near-range relative pose of a failed satellite using adaptive capacitive Kalman filtering, characterized in that, The method comprises the following steps: (1) using the relative orbit dynamics equation between satellites to represent the relative position between the tracking spacecraft and the failed satellite, using the attitude quaternion to represent the relative attitude of the failed satellite, and assuming that the failed satellite does uniform rotation in space with constant angular velocity, a relative kinematic model between the tracking spacecraft and the failed satellite is established and taken as a system equation; the relative position, relative attitude, relative velocity and relative angular velocity between the tracking spacecraft and the failed satellite are taken as state variables; Defining a relative pose quaternion is the pose transformation quaternion between the initial feature point set and the current failed satellite feature point set, where are the four parameters of the pose quaternion, then the corresponding pose transformation matrix is: ； The relative attitude kinematic equation represented by the attitude transformation matrix is: ； wherein, is the derivative of the attitude transformation matrix , is the angular velocity vector of the failed satellite at time t, is the angular velocity vector of the failed satellite at time t, is the skew-symmetric matrix corresponding to the angular velocity vector of the failed satellite, is the attitude transformation matrix at time t = 0, whose value is the initial attitude matrix, whose value can be calculated by the PNP algorithm using the initial three-dimensional point cloud and the two-dimensional feature points of the first image. The system state variables are selected as: ； ； wherein is the velocity position quantity at time k, is the tracking spacecraft orbital coordinate system, , , represents the kth moment of the failed satellite center of mass position in the coordinate system under , , the axial component; , , represents the kth moment of the failed satellite center of mass velocity in the coordinate system under , , the axial component; is the attitude quantity at time k, is the four parameters of the attitude quaternion at time k ; is the angular velocity quantity of the failed satellite at time k, is the three components; Equation of state is: ； ； ； wherein is the velocity position quantity at time instant k + 1, is the position quantity state transition matrix at time instant k, is the time interval; The attitude state transition mode is: ； is the equivalent rotation vector between time instant k and k+1, is the norm of the equivalent rotation vector, is the corresponding four-dimensional skew-symmetric matrix; The angular velocity and the failed satellite body coordinate state update are: ； for the k+1 time instant angular velocity quantity; (2) using the sequence image data collected by the monocular camera carried by the tracking spacecraft as the measurement of the system, using the position information of the camera calibrated on the ground relative to the center of mass of the tracking spacecraft, the attitude information of the camera coordinate system relative to the tracking spacecraft body coordinate system and the mapping model of the camera to establish the measurement function of the system; (3) giving the initial state value of the filtering system, using the relative kinematic model established in step (1) as the system equation to predict the state variables between the tracking spacecraft and the failed satellite at the next time; (4) using the two-dimensional coordinates of the feature points of the failed satellite collected in step (2) in the current image as the measurement, using the fuzzy adaptive logic controller to correct the noise parameters, and correcting the state variables predicted in step (3), and outputting the state variables as the filtering result.

2. The method of claim 1, wherein, The specific method of step (1) is: Firstly, define the related coordinate system, assume that the failed satellite runs on a circular orbit, and the tracking spacecraft estimates the relative position and posture of the failed satellite through the camera, and define the following coordinate systems: is the equatorial inertial coordinate system, the origin is the center of the earth, the axis is in the equatorial plane, pointing to the stars, the axis is along the direction of the earth's rotation axis, the axis, the axis and the axes constitute a right-handed coordinate system; To track the spacecraft, an orbit coordinate system is defined, denoted as coordinate system , with its origin located at the center of mass of the tracking spacecraft, its axis in the direction of the vector from the center of the Earth to the center of mass of the tracking spacecraft, its axis in the plane of the orbit of the tracking spacecraft, perpendicular to its axis, and making an acute angle with the velocity of the tracking spacecraft, its axis determined according to the right-hand rule. The coordinate system of the failed satellite orbit is denoted as the coordinate system , the origin of which is located at the center of mass of the failed satellite, the axis of which is directed to the center of mass of the failed satellite, the axis of which is in the plane of the orbit of the failed satellite and is perpendicular to the axis of which makes an acute angle with the flight velocity of the failed satellite, and the axis of which is determined according to the right-hand rule. is the coordinate system of the failed satellite, the origin of the coordinate system coincides with the center of mass of the failed satellite, the coordinate axes are fixed on the failed satellite, and the coordinate axes are directed to coincide with the orbit coordinate system of the tracking space vehicle at the initial time; To track the spacecraft carrier coordinate system, the coordinate system origin coincides with the tracking spacecraft center of mass, and the coordinate axis is fixed on the spacecraft. It is assumed that the tracking spacecraft achieves spin stabilization, that is, the tracking spacecraft carrier coordinate system coincides with the tracking spacecraft orbit coordinate system coincides; is a camera coordinate system, and the origin of the coordinate system is located at the optical center of the monocular camera, the axis direction is along the optical axis direction and perpendicular to the imaging plane, the axis is parallel to the left edge of the imaging plane and points to the left, the axis is parallel to the upper edge of the imaging plane and points to the upper side. is an image coordinate system, the image coordinate system is a plane coordinate system, a coordinate system plane is a camera imaging plane, an origin is a camera imaging principal point, an axis is parallel to an upper edge of the imaging plane and points to the left, an axis is parallel to a left edge of the imaging plane and points upward. The relative orbit kinematic model is established by the C-W equation, and the C-W equation is represented as: ； where s, , represents the components of the position of the failed satellite's center of mass in the coordinate system , , in the axial direction; , , represents the components of the velocity of the failed satellite's center of mass in the coordinate system , , in the axial direction; the superscript T denotes the transpose of the matrix; , , represents the components of the acceleration of the failed satellite's center of mass in the coordinate system , , in the axial direction; n denotes the average angular velocity of the orbit of the tracking spacecraft and , is the gravitational coefficient of the Earth, is the semi-major axis of the orbit of the tracking spacecraft.​​​ 3. The method of claim 1, wherein, The specific method of step (2) is: Selecting kth measurement To ； wherein represents the horizontal coordinate of the jth feature point detected in the image at time k, represents the vertical coordinate of the jth feature point detected in the image at time k, the corresponding measurement function of which is ； wherein, is the predicted image coordinate of the jth feature point at time k in the image coordinate system, and , is the focal length of the camera, is the coordinate of the jth feature point at time k in the tracking spacecraft orbital coordinate system, are the 1st, 2nd, and 3rd components of , respectively. ； wherein, is the attitude transformation matrix of the camera coordinate system with respect to the tracking space vehicle body coordinate system, C T is the coordinate column vector of the camera optical center in the tracking space vehicle body coordinate system, represents the coordinate of the jth feature point in the failed satellite body coordinate system, .

4. The method of claim 1, wherein, The specific method of step (3) is: First, the parameters and states of the filter are initialized, setting the initial state and the initial covariance matrix , the system noise , the initial measurement noise , and then generating a set of sampling points according to the initial covariance matrix and the initial state estimate, ； ； ； in, For the generated set of sampling points, for The covariance matrix at time 0, where time 0 is the initial covariance matrix. , To Performing Joliski decomposition, the square root of the covariance matrix is ​​obtained. Let m be the set of volume points, and m be a defined intermediate variable. , Let be the dimension of the state vector, denoted as . A unit vector of dimension is ,symbol Indicates to 1D unit vector The set of points generated by performing full permutations and changing the signs of the elements. Represents a point set The first in One point, for State estimation at time 0, with the initial state at time 0. ; The set of sampling points is obtained Substitute the state equation , and the state transition is performed to obtain the volume points corresponding to the state prediction values at k time points : ； one-step-ahead prediction value mean square error of one-step-ahead prediction : ； ； where Tria(B) represents the transpose of the upper triangular matrix obtained by performing the orthogonal triangular decomposition on the matrix , is the square root of the system noise matrix , i.e. has , the formula for ； Constructing new sample points , and computing the measurement prediction volume points by the measurement function h(.) ； ； Computational measurement prediction , predicted root mean square error , theoretical innovation covariance and cross covariance : ； ； ； ； where is the measurement noise matrix at time k is the square root of , and are intermediate variables in the computation process, whose formulas are ； 。 5. The method of claim 1, wherein, The specific method of step (4) is to adjust the measurement noise matrix at time k by using a fuzzy control algorithm according to the relationship between the innovation variance matrix and the theoretical variance matrix : ； ； ； wherein, are innovation vectors, is the innovation vector at time k, is the innovation vector at time d, is the measurement vector obtained at time k, M is the length of the sliding window, and is taken as 4, is the trace of a matrix, and if the ratio is less than 0.95 or greater than 1.05, the ratio is brought back to between 0.95 and 1.05 by adjusting , the ratio is brought back to between 0.95 and 1.05 by adjusting is the actual innovation variance matrix monitored; ； wherein is the measurement noise at the moment, is an adjustment factor generated by a fuzzy inference system, which is composed of three parts, i.e. a fuzzification process, a fuzzy control rule generation process and a defuzzification process, and is set as represents the fuzzy set "smaller", represents the fuzzy set "equal", represents the fuzzy set "larger", and according to actual requirements, the fuzzy control rule is set as ； Finally, the method of defuzzification takes the barycenter method, whose essence is weighted average, that is, the weighted coefficient takes the membership degree of the corresponding element, and the output takes the barycenter horizontal coordinate of the area surrounded by the membership function curve ; Finally, the measurement is updated: ； ； ； wherein is a filter gain matrix, is a state estimate at time k, is a square root of the covariance matrix at time k.

Images