CN113091754B - Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method - Google Patents

Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method Download PDF

Info

Publication number
CN113091754B
CN113091754B CN202110341892.1A CN202110341892A CN113091754B CN 113091754 B CN113091754 B CN 113091754B CN 202110341892 A CN202110341892 A CN 202110341892A CN 113091754 B CN113091754 B CN 113091754B
Authority
CN
China
Prior art keywords
dual
time
state
matrix
spacecraft
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110341892.1A
Other languages
Chinese (zh)
Other versions
CN113091754A (en
Inventor
胡庆雷
陈航
郑建英
郭雷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202110341892.1A priority Critical patent/CN113091754B/en
Publication of CN113091754A publication Critical patent/CN113091754A/en
Application granted granted Critical
Publication of CN113091754B publication Critical patent/CN113091754B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Abstract

The invention relates to a method for integrally estimating pose and determining inertial parameters of a non-cooperative spacecraft, and provides a method for estimating pose and inertial parameters of the non-cooperative spacecraft based on dual quaternion aiming at the problem that measurement information of relative angular velocity and relative linear velocity of a target spacecraft is lost in a visual navigation process; the invention comprises the following steps: establishing a six-degree-of-freedom attitude kinematics and a dynamics model of the space rigid-body spacecraft under the description of a dual quaternion framework for accurately describing the inherent coupling relationship between the relative attitude motion and the relative position motion; then selecting error quantities of the target spacecraft dual quaternion vector part, the dual angular velocity vector part and the rotational inertia ratio as state variables, and determining a state equation and an observation equation; and finally, a state correction method of the error dual quaternion is given, a multiplicative extended Kalman filter is designed, the real-time pose and inertial parameters of the target spacecraft are estimated on line, and the execution precision of the on-orbit task is improved.

Description

Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method
Technical Field
The invention relates to a non-cooperative spacecraft pose integrated estimation and inertial parameter determination method, which is mainly applied to the spacecraft pose estimation problem under the condition of considering the measurement information loss of the relative angular velocity and the relative linear velocity of a target spacecraft in the visual navigation process and belongs to the field of spacecraft navigation.
Background
Close-proximity operation requires a space vehicle to be carefully maintained in relatively close proximity and at a specified relative attitude with another space vehicle. The accurate acquisition of complete six-degree-of-freedom pose information of the target spacecraft is obviously a prerequisite for the success of the approaching operation. Under the background of a relative visual navigation task, the characteristics of the observed target of the camera are all positioned on the surface of the spacecraft deviating from the centroid, so that the estimation deviation caused by neglecting the coupling relation between poses is particularly great, and the error cannot be represented by a traditional pose discrete motion model. In addition, the tracking spacecraft does not have the capability of directly measuring the relative angular velocity and the relative linear velocity between the tracking spacecraft and the target spacecraft by only depending on the navigation camera, which causes the accuracy of pose estimation to be reduced and influences the successful execution of the on-orbit task. Therefore, under the framework of integrating the attitude and position coupling, the six-degree-of-freedom attitude and position precise estimation method is designed for the target spacecraft which lacks the measurement information of the relative angular velocity and the relative linear velocity, and has great theoretical and engineering significance.
Aiming at the existing spacecraft pose estimation method considering the pose coupling relationship, a patent 201710178159.6 utilizes a gyroscope and accelerometer error measurement model and combines GPS measurement information to build an inertia/GPS combined Kalman filter based on dual quaternion. However, the method is not suitable for pose estimation under the condition of relative angular velocity and relative linear velocity measurement information loss because the gyroscope and the accelerometer are respectively adopted to measure the target angular velocity and the linear velocity. Similarly, patent 201810872927.2 proposes a deep learning-based attitude and orbit integration parameter estimation method for a space non-cooperative target, but the method does not give a pose estimation method when the angular velocity and linear velocity of the target are always not available. In the actual situation that a navigation camera is adopted to observe a target spacecraft, the accuracy of pose estimation is directly reduced due to the fact that angular velocity and linear velocity cannot be obtained.
Aiming at the research result of the existing spacecraft pose integrated estimation problem, most of the existing estimation methods are based on the assumption that the angular velocity and linear velocity of a target spacecraft can be accurately obtained, and the target relative velocity information which cannot be directly measured under the visual navigation background is ignored. Under the condition, the accuracy of the pose estimation algorithm is greatly reduced, and the smooth execution of the adjacent operation task is influenced.
Disclosure of Invention
The technical problem of the invention is solved: the method solves the problem of reduced pose estimation precision caused by lack of relative angular velocity and relative linear velocity measurement information in a spacecraft approach operation visual navigation task, provides a non-cooperative spacecraft pose integral estimation and inertial parameter determination method, solves the problem of reduced pose estimation precision under the condition of relative measurement information loss by establishing a space rigid spacecraft six-degree-of-freedom pose kinematics and a dynamics model under the condition of dual quaternion frame description, then selecting error quantities of a dual quaternion vector part, a dual angular velocity vector part and a rotational inertia ratio as state variables, deducing a state equation and an observation equation, and designing a novel multiplicative extended Kalman filter, thereby solving the problem of reduced pose estimation precision under the condition of relative measurement information loss, realizing the on-line estimation of real-time pose and inertial parameters of a target spacecraft, and improving the precision of on-orbit task execution.
The technical solution of the invention is as follows: a non-cooperative spacecraft pose integral estimation and inertial parameter determination method comprises the following steps:
s1: considering that the traditional pose discrete model cannot accurately describe the inherent coupling relation between the relative attitude motion and the relative position motion, establishing a six-degree-of-freedom kinematics and pose dynamics model of the space rigid-body spacecraft under the description of a dual quaternion frame;
s2: based on the spacecraft six-degree-of-freedom kinematics and attitude dynamics model established in the step S1, selecting error quantities of a target spacecraft dual quaternion vector part, a dual angular velocity vector part and a rotational inertia ratio as state variables, and deriving a state equation and an observation equation;
s3: and (3) providing a state correction method of the error dual quaternion, designing a novel multiplicative extended Kalman filter based on the state equation and the observation equation deduced in the step S2, and realizing the on-line estimation of the real-time pose and the inertial parameters of the target spacecraft.
The concrete implementation steps are as follows:
the first step is to establish a six-degree-of-freedom kinematics and attitude dynamics model of the space rigid body spacecraft under the description of a dual quaternion frame as follows:
Figure BDA0002999398200000031
Figure BDA0002999398200000032
in the formula (I), the compound is shown in the specification,
Figure BDA0002999398200000033
is a dual quaternion of the target spacecraft system relative to the inertial system,
Figure BDA0002999398200000034
is its derivative with respect to time;
Figure BDA0002999398200000035
the dual angular velocity of the system of the target spacecraft relative to the inertial system is defined under the inertial system;
Figure BDA0002999398200000036
defining the linear velocity of the target spacecraft relative to the inertial system under the inertial system; the arithmetic on the right side of the equal sign of the formula (1) is dual quaternion multiplication;
Figure BDA0002999398200000037
defining the three-axis rotation angular velocity of the target spacecraft body system relative to the inertial coordinate system under the inertial system;
Figure BDA0002999398200000038
is composed of
Figure BDA0002999398200000039
A derivative with respect to time; j is a rotational inertia matrix, and under the background of visual navigation, the inertia product is not zero:
Figure BDA00029993982000000310
wherein, J xx ，J yy ，J zz Is the moment of inertia of the main shaft, J xy ，J xz ，J yz Is the product of inertia; τ is the external disturbance moment, which can be modeled as white gaussian noise. In addition, in a task scene of visual navigation, the true value of the target spacecraft rotational inertia cannot be obtained, and only the relative ratio of the rotational inertia can be estimated. Selecting J in a rotational inertia matrix xx As a reference, the formula (2) is rewritten as follows:
Figure BDA00029993982000000311
wherein the relative ratio of the moments of inertia is defined as:
Figure BDA0002999398200000041
the remaining element vector J of the moment of inertia matrix is added 5 ＝[J yy ,J zz ,J xy ,J xz ,J yz ]Relative J xx The ratio of (2) is also included into the state vector, and is estimated together with the six-degree-of-freedom pose of the target spacecraft.
And secondly, deducing a state equation and an observation equation. Are respectively provided with
Figure BDA0002999398200000042
And
Figure BDA0002999398200000043
represent
Figure BDA0002999398200000044
And an estimate of J. Defining an error dual quaternion, an error dual angular velocity and an error rotational inertia matrix:
Figure BDA0002999398200000045
Figure BDA0002999398200000046
Figure BDA0002999398200000047
substituting formula (1) into formula (7) in consideration of six-degree-of-freedom kinematics to obtain
Figure BDA0002999398200000048
Wherein
Figure BDA0002999398200000049
q r And q is d Are respectively dual quaternions
Figure BDA00029993982000000416
Real part and dual part of, and
Figure BDA00029993982000000410
q 0 and q is v Scalar and vector parts, I, of quaternions q, respectively 3×3 Is an identity matrix. A second term approximation of the result obtained by equation (9):
Figure BDA00029993982000000411
ignoring the second order fractional quantity, equation (9) can be written as:
Figure BDA00029993982000000412
there are two constraints due to dual quaternions:
Figure BDA00029993982000000413
and
Figure BDA00029993982000000414
wherein
Figure BDA00029993982000000415
Is q is r The number of the conjugated quaternion of (a),
Figure BDA0002999398200000051
is composed of
Figure BDA0002999398200000052
Conjugate dual quaternion of (1). Only six parameters are independent of each other. To avoid covariance matrix singularity, from the original state quantity
Figure BDA0002999398200000053
Deleting two scalar elements. The vector part is taken for equation (11):
Figure BDA0002999398200000054
in the formula (I), the compound is shown in the specification,
Figure BDA0002999398200000055
is composed of
Figure BDA0002999398200000056
The vector portion of (1); for any purpose
Figure BDA0002999398200000057
The cross-multiplication matrix is defined as
Figure BDA0002999398200000058
Substituting equation (2) into equation (8) in consideration of the rigid body spacecraft attitude dynamics, and obtaining:
Figure BDA0002999398200000059
wherein
Figure BDA00029993982000000510
Then derive respectively
Figure BDA00029993982000000511
To pair
Figure BDA00029993982000000512
δ τ and δ J 5 The jacobian matrix of.
Figure BDA00029993982000000513
For is to
Figure BDA00029993982000000514
The partial derivative of (c) is calculated as follows:
Figure BDA00029993982000000515
similarly:
Figure BDA00029993982000000516
Figure BDA00029993982000000517
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA00029993982000000518
a is an arbitrary three-dimensional vector.
The discrete state equation and the observation equation of the linearized system are:
Figure BDA00029993982000000519
Z(t k )＝H(t k )x(t k )+v(t k ) (18)
wherein, t k Represents the k time; x (t) k ) Is a state variable at the moment k; f (t) k ) Is a state matrix at time k; w (t) k ) Process noise at time k; g (t) k ) A process noise matrix at time k; z (t) k ) The measurement vector at the k moment; h (t) k ) A measurement matrix at the time k; v (t) k ) Is the measurement noise at time k. Then selecting the state quantity
Figure BDA0002999398200000061
The equation of state is obtained as follows:
Figure BDA0002999398200000062
in the formula (I), the compound is shown in the specification,
Figure BDA0002999398200000063
Figure BDA0002999398200000064
Figure BDA0002999398200000065
Figure BDA0002999398200000066
because the relative pose of the target spacecraft can be directly measured, the obtained observation matrix is as follows:
H(t k )＝[I 6×6 0 6×6 0 6×5 ] (24)
and thirdly, designing a multiplicative extended Kalman filter. The multiplicative extended Kalman filter is divided into four parts of state one-step prediction, measurement updating, filtering updating and state correction:
(1) State one-step prediction
Figure BDA0002999398200000067
Figure BDA0002999398200000068
Figure BDA0002999398200000069
Wherein, the first and the second end of the pipe are connected with each other,
Figure BDA00029993982000000610
an estimated value of a dual quaternion vector part at the time k;
Figure BDA00029993982000000611
is an estimated value of the dual quaternion vector part at the time of k-1;
Figure BDA00029993982000000612
the estimated value of the time derivative of the dual quaternion vector part at the moment k-1 is obtained by calculation of the formula (1); Δ t is the time interval;
Figure BDA00029993982000000613
the estimated value of the dual angular velocity at the moment k is obtained;
Figure BDA00029993982000000614
is an estimated value of the dual angular velocity at the time of k-1;
Figure BDA0002999398200000071
the real part of the estimated value of the time derivative of the dual angular velocity at the time k-1 is calculated by an equation (2), and the dual part of the estimated value of the time derivative of the dual angular velocity at the time k-1 is calculated by an equation (19);
Figure BDA0002999398200000072
is an estimated value of the linear velocity at the moment k;
Figure BDA0002999398200000073
is an estimated value of the linear velocity at the moment of k-1;
Figure BDA0002999398200000074
is an estimate of the time derivative of the velocity of the line at k-1.
(2) Measurement update
Calculating error dual quaternion from observed value and one-step predicted value of dual quaternion
Figure BDA0002999398200000075
(3) Filter update
Figure BDA0002999398200000076
P k|k-1 ＝ΦP k-1 Φ T +Q k-1 (30)
K k ＝P k|k-1 H T (HP k|k-1 H T +R k ) -1 (31)
Figure BDA0002999398200000077
x k ＝x k-1 +K k (δq v,k -Hx k-1 ) (33)
Wherein phi k State transition matrix at k time; p k|k-1 Predicting an estimation error covariance matrix for the k moment; p k-1 Estimating an error covariance matrix for the k time; k is k Is a gain matrix at time k; q k-1 A system noise variance matrix at the moment k; r k To measure the variance matrix.
(4) State correction
Using the two constraints of the dual quaternion to calculate the complete error dual quaternion from the error dual quaternion vector as the state quantity
Figure BDA0002999398200000078
The above formula is suitable for
Figure BDA0002999398200000079
In the case of
Figure BDA00029993982000000710
Then, the state is corrected using the following equation:
Figure BDA00029993982000000711
wherein
Figure BDA00029993982000000712
Are respectively as
Figure BDA00029993982000000713
The real part and the dual part of (c).
The six-degree-of-freedom pose estimation and inertial parameter determination of the target spacecraft can be completed under the condition of lacking relative angular velocity and linear velocity measurement information through the steps.
Compared with the prior art, the invention has the advantages that:
(1) Compared with the existing pose discrete motion model, the coupled relative pose motion model is established, the target angular velocity and linear velocity information are uniformly described in a dual quaternion mode, the pose coupling relation is described, and the high-precision estimation of the relative position and the relative pose is realized.
(2) Compared with the existing spacecraft six-degree-of-freedom pose estimation method, the pose estimation method for the target angular velocity and linear velocity which are always not measurable is provided, the limitation of a corresponding sensor is avoided, and the method has practical significance for executing an on-orbit task of approaching operation by adopting a navigation camera.
(3) Compared with the existing deep learning-based method, the method has outstanding real-time performance and calculation efficiency, has the capability of resolving the pose parameters of the target spacecraft in real time, reduces the requirements on huge hardware calculation capability and data training amount, and has strong engineering realizability.
Drawings
FIG. 1 is a schematic block diagram of a dual quaternion based relative navigation system of the present invention;
FIG. 2 is a flow chart of a non-cooperative spacecraft pose integrated estimation and inertial parameter estimation method of the present invention.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and examples.
FIG. 1 is a schematic block diagram of a dual quaternion-based relative navigation system according to the present invention, which comprises a navigation camera, a pose solution module, and a navigation filter algorithm. Firstly, a three-dimensional model of a target spacecraft is known, a navigation camera shoots an on-orbit picture of the target spacecraft in real time, and the on-orbit picture is removed from a background and denoised; then sending the pose to a pose resolving module, and calculating rough relative pose and relative position by a pose resolving method based on a model; and finally, calculating accurate pose and inertia parameters by recursion of a navigation filter, and updating the pose parameters into a pose calculation module to realize real-time tracking.
As shown in FIG. 2, the invention relates to a non-cooperative spacecraft pose integrated estimation and inertial parameter determination method, which comprises the following steps: establishing a six-degree-of-freedom attitude kinematics and a dynamics model of the space rigid-body spacecraft under the description of a dual quaternion framework for accurately describing the inherent coupling relationship between the relative attitude motion and the relative position motion; then selecting error quantities of the target spacecraft dual quaternion vector part, the dual angular velocity vector part and the rotational inertia ratio as state variables, and determining a state equation and an observation equation; and finally, a state correction method of the error dual quaternion is given, a multiplicative extended Kalman filter is designed, the real-time pose and inertial parameters of the target spacecraft are estimated on line, and the execution precision of the on-orbit task is improved.
The specific operation steps are as follows:
firstly, establishing a six-degree-of-freedom kinematics and attitude dynamics model of the space rigid-body spacecraft under the description of a dual quaternion frame as follows:
Figure BDA0002999398200000091
Figure BDA0002999398200000092
in the formula (I), the compound is shown in the specification,
Figure BDA0002999398200000093
is a dual quaternion of the target spacecraft system relative to the inertial system,
Figure BDA0002999398200000094
is its derivative with respect to time;
Figure BDA0002999398200000095
the dual angular velocity of the system of the target spacecraft relative to the inertial system is defined under the inertial system;
Figure BDA0002999398200000096
defining the linear velocity of the target spacecraft relative to the inertial system under the inertial system; the right side operation of the equation equal sign is dual quaternion multiplication;
Figure BDA0002999398200000097
defining the three-axis rotation angular velocity of the target spacecraft body system relative to the inertial coordinate system under the inertial system;
Figure BDA0002999398200000098
is composed of
Figure BDA0002999398200000099
Derivative with respect to time. The initial relative attitude of the target spacecraft is selected as
Figure BDA00029993982000000910
The initial relative angular velocity is selected as
Figure BDA00029993982000000911
The target spacecraft centroid is located at the tracking spacecraft distance vector of
Figure BDA00029993982000000912
Wherein the initial relative linear velocity of the tracked spacecraft is selected as
Figure BDA00029993982000000913
J is a rotational inertia matrix, and under the background of visual navigation, the inertia product is not zero:
Figure BDA00029993982000000914
wherein, J xx ，J yy ，J zz Is the moment of inertia of the main shaft, J xy ，J xz ，J yz Is the product of inertia; τ is the external disturbance torque, which can be modeled as white gaussian noise. In addition, in a task scene of visual navigation, the true value of the target spacecraft rotational inertia cannot be obtained, and only the relative ratio of the rotational inertia can be estimated. Selecting J in a rotational inertia matrix xx As a reference, the rewritten formula (2) is as follows:
Figure BDA0002999398200000101
wherein the relative ratio of the moments of inertia is defined as:
Figure BDA0002999398200000102
the remaining element vector J of the moment of inertia matrix is added 5 ＝[J yy ,J zz ,J xy ,J xz ,J yz ]Relative J xx The ratio of (2) is also included into the state vector, and is estimated together with the six-degree-of-freedom pose of the target spacecraft. The relative ratio of the moments of inertia is set as:
Figure BDA0002999398200000103
and secondly, deducing a state equation and an observation equation. Are respectively provided with
Figure BDA0002999398200000104
And
Figure BDA0002999398200000105
to represent
Figure BDA0002999398200000106
And an estimate of J. Defining an error dual quaternion, an error dual angular velocity and an error rotational inertia matrix:
Figure BDA0002999398200000107
Figure BDA0002999398200000108
Figure BDA0002999398200000109
substituting equation (1) into equation (7) in consideration of six-degree-of-freedom kinematics, yields:
Figure BDA00029993982000001010
wherein
Figure BDA00029993982000001011
q r And q is d Are respectively dual quaternions
Figure BDA00029993982000001012
The real part and the dual part of
Figure BDA00029993982000001013
q 0 And q is v Scalar and vector parts, I, of quaternions q, respectively 3×3 Is an identity matrix. And (4) carrying out second term approximation processing on the result obtained by the equation (44):
Figure BDA0002999398200000111
ignoring the second order fractional quantity, equation (44) can be written as:
Figure BDA0002999398200000112
there are two constraints due to dual quaternions:
Figure BDA0002999398200000113
wherein
Figure BDA0002999398200000114
Is q is r The number of the conjugate quaternion of (c),
Figure BDA0002999398200000115
is composed of
Figure BDA0002999398200000116
Conjugate dual quaternion of (1). Only six parameters are independent of each other. To avoid covariance matrix singularity, from the original state quantity
Figure BDA0002999398200000117
Deleting two scalar elements. The vector part is taken for equation (46):
Figure BDA0002999398200000118
in the formula (I), the compound is shown in the specification,
Figure BDA0002999398200000119
is composed of
Figure BDA00029993982000001110
The vector portion of (1); for any purpose
Figure BDA00029993982000001111
The cross multiplication matrix is defined as
Figure BDA00029993982000001112
Substituting equation (2) into equation (8) in consideration of rigid body spacecraft attitude dynamics, yields:
Figure BDA00029993982000001113
wherein
Figure BDA00029993982000001114
Then respectively derive
Figure BDA00029993982000001115
To pair
Figure BDA00029993982000001116
δ τ and δ J 5 The jacobian matrix of.
Figure BDA00029993982000001117
To pair
Figure BDA00029993982000001118
The partial derivative of (c) is calculated as follows:
Figure BDA00029993982000001119
similarly:
Figure BDA00029993982000001120
Figure BDA0002999398200000121
wherein the content of the first and second substances,
Figure BDA0002999398200000122
a is an arbitrary three-dimensional vector.
The discrete state equation and the observation equation of the linearized system are:
Figure BDA0002999398200000123
Z(t k )＝H(t k )x(t k )+v(t k ) (53)
wherein, t k Represents the k time; x (t) k ) Is composed ofA state variable at time k; f (t) k ) State matrix at time k; w (t) k ) Process noise at time k; g (t) k ) A process noise matrix at time k; z (t) k ) The measurement vector at the k moment; h (t) k ) A measurement matrix at the time k; v (t) k ) Is the measurement noise at time k. Then selecting the state quantity
Figure BDA0002999398200000124
The equation of state is obtained as follows:
Figure BDA0002999398200000125
in the formula (I), the compound is shown in the specification,
Figure BDA0002999398200000126
Figure BDA0002999398200000127
Figure BDA0002999398200000128
Figure BDA0002999398200000129
because the relative pose of the target spacecraft can be directly measured, the obtained observation matrix is as follows:
H(t k )＝[I 6×6 0 6×6 0 6×5 ] (59)
and thirdly, designing a multiplicative extended Kalman filter. The multiplicative extended Kalman filter is divided into four parts of state one-step prediction, measurement updating, filtering updating and state correction:
(1) State one-step prediction
Figure BDA0002999398200000131
Figure BDA0002999398200000132
Figure BDA0002999398200000133
Wherein the content of the first and second substances,
Figure BDA0002999398200000134
an estimated value of a dual quaternion vector part at the time k;
Figure BDA0002999398200000135
the estimated value of the dual quaternion vector part at the time k-1;
Figure BDA0002999398200000136
the estimated value of the time derivative of the dual quaternion vector part at the moment k-1 is obtained by calculation of the formula (1); (ii) a
Figure BDA0002999398200000137
Is an estimated value of the dual angular velocity at the moment k;
Figure BDA0002999398200000138
is an estimated value of the dual angular velocity at the moment k-1;
Figure BDA0002999398200000139
the real number part of the estimated value of the time derivative of the dual angular velocity at the moment k-1 is calculated by the formula (2), and the dual part of the estimated value is calculated by the formula (19);
Figure BDA00029993982000001310
the linear velocity at the moment k is an estimated value;
Figure BDA00029993982000001311
is an estimated value of the linear velocity at the moment of k-1;
Figure BDA00029993982000001312
an estimate of the time derivative of the k-1 time line velocity; the sampling frequency is 2Hz, so the time interval is set to Δ t =0.5s.
(2) Measurement update
Calculating error dual quaternion from observed value and one-step predicted value of dual quaternion
Figure BDA00029993982000001313
(3) Filter update
Figure BDA00029993982000001314
P k|k-1 ＝ΦP k-1 Φ T +Q k-1 (65)
K k ＝P k|k-1 H T (HP k|k-1 H T +R k ) -1 (66)
Figure BDA00029993982000001315
x k ＝x k-1 +K k (δq v,k -Hx k-1 ) (68)
Wherein phi is k State transition matrix at k time; p k|k-1 Predicting an estimation error covariance matrix for the k moment; p k-1 Estimating an error covariance matrix for the k time; k k Is a gain matrix at time k; q k-1 A system noise variance matrix at the moment k; r is k To measure the variance matrix. The initial parameter of the filter is selected as P 17×17 (0)＝diag(1×10 -9 I 3×3 ,1×10 -6 I 3×3 ,1×10 -7 I 3×3 ,1×10 - 6 I 3×3 ,1×10 -4 I 5×5 )，Q 11×11 ＝1×10 -6 I 11×11 ，R 6×6 ＝diag(q noise 2 ×I 3×3 ,p noise 2 ×I 3×3 ). Wherein the attitude measurement noise q noise = π/90, relative distance measurement noise p noise ＝0.5。
(4) State correction
Using the two constraints of the dual quaternion to calculate the complete error dual quaternion from the error dual quaternion vector as the state quantity
Figure BDA0002999398200000141
The above formula is suitable for
Figure BDA0002999398200000142
In the case of
Figure BDA0002999398200000143
Then, the state is corrected using the following equation:
Figure BDA0002999398200000144
wherein
Figure BDA0002999398200000145
Are respectively as
Figure BDA0002999398200000146
The real part and the dual part of (a).
The six-degree-of-freedom pose estimation and inertial parameter determination of the target spacecraft can be completed under the condition of lacking relative angular velocity and linear velocity measurement information through the steps.
Those skilled in the art will appreciate that the invention may be practiced without these specific details.

Claims (2)

1. A non-cooperative spacecraft pose integrated estimation and inertial parameter determination method is characterized by comprising the following steps:
s1: establishing a six-degree-of-freedom kinematics and attitude dynamics model of the space rigid-body spacecraft under the description of a dual quaternion frame;
s2: based on the six-degree-of-freedom kinematics and attitude dynamics model of the space rigid-body spacecraft established in the step S1, selecting an error quantity of a target spacecraft dual quaternion vector part, a dual angular velocity vector part and a rotational inertia ratio as a state variable, and determining a state equation and an observation equation;
s3: designing a new multiplicative extended Kalman filter by adopting a state correction method of error dual quaternion and based on the state equation and the observation equation determined in the step S2, and realizing the on-line estimation of the real-time pose and the inertial parameters of the target spacecraft;
in the step S1, the six-degree-of-freedom kinematics and attitude dynamics model of the space rigid body spacecraft is established as follows:
Figure FDA0003885456220000011
Figure FDA0003885456220000012
in the formula (I), the compound is shown in the specification,
Figure FDA0003885456220000013
is a dual quaternion of the target spacecraft system relative to the inertial system,
Figure FDA0003885456220000014
is its derivative with respect to time;
Figure FDA0003885456220000015
the dual angular velocity of the system of the target spacecraft relative to the inertial system is defined under the inertial system;
Figure FDA0003885456220000016
defining the linear velocity of the target spacecraft relative to the inertial system under the inertial system; the arithmetic on the right side of the equal sign of the formula (1) is dual quaternion multiplication;
Figure FDA0003885456220000017
defining the triaxial rotation angular velocity of the target spacecraft body system relative to the inertial coordinate system under the inertial coordinate system;
Figure FDA0003885456220000018
is composed of
Figure FDA0003885456220000019
A derivative with respect to time; tau is external interference torque, and is modeled as Gaussian white noise; j' is defined as the relative ratio of the moments of inertia:
Figure FDA0003885456220000021
in a task scene of visual navigation, the true value of the rotational inertia of the target spacecraft cannot be obtained, only the relative ratio of the rotational inertia can be estimated, and J in a rotational inertia matrix is selected xx As a reference; j is a rotational inertia matrix, and under the background of visual navigation, the inertia product is not zero:
Figure FDA0003885456220000022
wherein, J xx ，J yy ，J zz Is the moment of inertia of the main shaft, J xy ，J xz ，J yz As the product of inertia, the remaining element vector J of the rotational inertia matrix is 5 ＝[J yy ,J zz ,J xy ,J xz ,J yz ]Relative J xx The ratio of the target space vehicle to the target space vehicle is also included in the state vector, and is estimated together with the six-degree-of-freedom pose of the target space vehicle;
in step S2, the determined state equation and observation equation are:
Figure FDA0003885456220000023
Z(t k )＝H(t k )x(t k )+v(t k ) (6)
wherein, t k Represents the k time; x (t) k ) Is a state variable at the moment k; f (t) k ) State matrix at time k; w (t) k ) Process noise at time k; g (t) k ) A process noise matrix at time k; z (t) k ) The measurement vector at the k moment; h (t) k ) A measurement matrix at the time k; v (t) k ) Measurement noise at time k; a quantity of state being
Figure FDA0003885456220000024
The matrix of the error dual quaternion, the error dual angular velocity and the error rotational inertia is as follows:
Figure FDA0003885456220000025
Figure FDA0003885456220000026
Figure FDA0003885456220000027
wherein
Figure FDA0003885456220000028
And
Figure FDA0003885456220000029
represent
Figure FDA00038854562200000210
And the estimated value of J is calculated,
Figure FDA00038854562200000211
is composed of
Figure FDA00038854562200000212
Conjugation of (1); the state matrix is:
Figure FDA0003885456220000031
in the formula (I), the compound is shown in the specification,
Figure FDA0003885456220000032
Figure FDA0003885456220000033
Figure FDA0003885456220000034
Figure FDA0003885456220000035
wherein the content of the first and second substances,
Figure FDA0003885456220000036
a is any three-dimensional vector;
Figure FDA0003885456220000037
Figure FDA0003885456220000038
is any dual quaternion, q r And q is d Are respectively as
Figure FDA0003885456220000039
The real part and the dual part of (a),
Figure FDA00038854562200000310
representing the vectorial part of the quaternion, process noise
Figure FDA00038854562200000311
The process noise matrix is:
Figure FDA00038854562200000312
because the relative position and attitude of the target spacecraft are measured and solved by the camera, the measurement matrix is as follows:
H(t k )＝[I 6×6 0 6×6 0 6×5 ] (16)。
2. the method of claim 1, wherein: in step S3, the multiplicative extended kalman filter is divided into four parts, namely, state one-step prediction, measurement update, filtering update, and state correction:
(1) State one-step prediction
Figure FDA00038854562200000313
Figure FDA0003885456220000041
Figure FDA0003885456220000042
Wherein the content of the first and second substances,
Figure FDA0003885456220000043
an estimated value of a dual quaternion vector part at the time k;
Figure FDA0003885456220000044
the estimated value of the dual quaternion vector part at the time k-1;
Figure FDA0003885456220000045
the estimated value of the time derivative of the dual quaternion vector part at the moment k-1 is obtained by calculation of the formula (1); Δ t is the time interval;
Figure FDA0003885456220000046
is an estimated value of the dual angular velocity at the moment k;
Figure FDA0003885456220000047
is an estimated value of the dual angular velocity at the time of k-1;
Figure FDA0003885456220000048
the real number part of the estimated value of the time derivative of the dual angular velocity at the moment k-1 is calculated by the formula (2), and the dual part of the estimated value is calculated by the formula (19);
Figure FDA0003885456220000049
is an estimated value of the linear velocity at the moment k;
Figure FDA00038854562200000410
is an estimated value of the linear velocity at the moment of k-1;
Figure FDA00038854562200000411
an estimate of the time derivative of the k-1 time line velocity;
(2) Measurement update
Calculating error dual quaternion from observed value and one-step predicted value of dual quaternion
Figure FDA00038854562200000412
(3) Filter update
Figure FDA00038854562200000413
P k|k-1 ＝ΦP k-1 Φ T +Q k-1 (22)
K k ＝P k|k-1 H T (HP k|k-1 H T +R k ) -1 (23)
Figure FDA00038854562200000414
x k ＝x k-1 +K k (δq v,k -Hx k-1 ) (25)
Wherein phi is k State transition matrix at the moment k; p k|k-1 Predicting an estimation error covariance matrix for the k moment; p is k-1 Estimating an error covariance matrix for the k time; k k Is a gain matrix at time k; q k-1 A system noise variance matrix at the moment k; r k Measuring a variance matrix;
(4) State correction
Two constraints (q) using dual quaternions r ) * q r =1 and
Figure FDA00038854562200000415
calculating the complete error dual quaternion by using the error dual quaternion vector as the state quantity
Figure FDA0003885456220000051
The above formula is suitable for
Figure FDA0003885456220000052
The case (2);
when the temperature is higher than the set temperature
Figure FDA0003885456220000053
Then, the state is corrected using the following equation:
Figure FDA0003885456220000054
wherein
Figure FDA0003885456220000055
Are respectively as
Figure FDA0003885456220000056
The real part and the dual part of (a).
CN202110341892.1A 2021-03-30 2021-03-30 Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method Active CN113091754B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110341892.1A CN113091754B (en) 2021-03-30 2021-03-30 Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110341892.1A CN113091754B (en) 2021-03-30 2021-03-30 Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method

Publications (2)

Publication Number Publication Date
CN113091754A CN113091754A (en) 2021-07-09
CN113091754B true CN113091754B (en) 2023-02-28

Family

ID=76670981

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110341892.1A Active CN113091754B (en) 2021-03-30 2021-03-30 Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method

Country Status (1)

Country Link
CN (1) CN113091754B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114012507B (en) * 2021-12-09 2023-06-02 天津工业大学 Identification method for position independent errors of double rotating shafts of cradle type five-axis machine tool
CN114543797A (en) * 2022-02-18 2022-05-27 北京市商汤科技开发有限公司 Pose prediction method and apparatus, device, and medium

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767797B (en) * 2017-03-23 2020-03-17 南京航空航天大学 inertial/GPS combined navigation method based on dual quaternion
CN110470297A (en) * 2019-03-11 2019-11-19 北京空间飞行器总体设计部 A kind of attitude motion of space non-cooperative target and inertial parameter estimation method
CN110567461B (en) * 2019-08-01 2021-07-02 北京航空航天大学 Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope

Also Published As

Publication number Publication date
CN113091754A (en) 2021-07-09

Similar Documents

Publication Publication Date Title
CN109376785B (en) Navigation method based on iterative extended Kalman filtering fusion inertia and monocular vision
Ye et al. Tightly coupled 3d lidar inertial odometry and mapping
CN110398257B (en) GPS-assisted SINS system quick-acting base initial alignment method
CN108731670B (en) Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN110030994B (en) Monocular-based robust visual inertia tight coupling positioning method
CN113091754B (en) Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method
CN110986939B (en) Visual inertia odometer method based on IMU (inertial measurement Unit) pre-integration
CN110567461B (en) Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope
CN107607113B (en) Method for measuring inclination angles of two-axis attitude
CN109931957B (en) Self-alignment method of SINS strapdown inertial navigation system based on LGMKF
CN106767797B (en) inertial/GPS combined navigation method based on dual quaternion
CN110455309B (en) MSCKF-based visual inertial odometer with online time calibration
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN109959374B (en) Full-time and full-range reverse smooth filtering method for pedestrian inertial navigation
CN111707261A (en) High-speed sensing and positioning method for micro unmanned aerial vehicle
CN110929402A (en) Probabilistic terrain estimation method based on uncertain analysis
CN115046545A (en) Positioning method combining deep network and filtering
CN115540860A (en) Multi-sensor fusion pose estimation algorithm
Bai et al. Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory
CN114046800B (en) High-precision mileage estimation method based on double-layer filtering frame
CN112489176B (en) Tightly-coupled graph building method fusing ESKF, g2o and point cloud matching
CN115344033A (en) Monocular camera/IMU/DVL tight coupling-based unmanned ship navigation and positioning method
CN111637894B (en) Navigation filtering method for constant coefficient landmark image
CN115014346A (en) Consistent efficient filtering algorithm based on map and oriented to visual inertial positioning
CN115856974B (en) GNSS, INS and visual tight combination navigation positioning method based on invariant filtering

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant