CN113091754A - 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
CN113091754A
CN113091754A CN202110341892.1A CN202110341892A CN113091754A CN 113091754 A CN113091754 A CN 113091754A CN 202110341892 A CN202110341892 A CN 202110341892A CN 113091754 A CN113091754 A CN 113091754A
Authority
CN
China
Prior art keywords
dual
state
time
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.)
Granted
Application number
CN202110341892.1A
Other languages
Chinese (zh)
Other versions
CN113091754B (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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Astronomy & Astrophysics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

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, the 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 a gyroscope and an accelerometer are respectively adopted to measure the target angular velocity and the linear velocity. Similarly, patent 201810872927.2 proposes a method for estimating attitude and orbit integration parameters of a spatial non-cooperative target based on deep learning, but the method does not give a pose estimation method when the angular velocity and linear velocity of the target are not always available. In the actual situation that the target spacecraft is observed by the navigation camera, the accuracy of pose estimation is directly reduced due to the fact that the angular velocity and the linear velocity cannot be acquired.
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 integrated 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 (4) 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 online estimation of the real-time pose and the inertial parameters of the target spacecraft.
The method comprises the following concrete steps:
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,
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, Jxx，Jyy，JzzIs the moment of inertia of the main shaft, Jxy，Jxz，JyzIs 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 rotational inertia of the target spacecraft cannot be obtained, and only the relative ratio of the rotational inertia can be estimated. Selecting J in a rotational inertia matrixxxAs a reference, equation (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 added5＝[Jyy,Jzz,Jxy,Jxz,Jyz]Relative JxxThe 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
to 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
considering six-degree-of-freedom kinematics, the formula (1) is substituted into the formula (7) to obtain
Figure BDA0002999398200000048
Wherein
Figure BDA0002999398200000049
qrAnd q isdAre respectively dual quaternions
Figure BDA00029993982000000416
The real part and the dual part of
Figure BDA00029993982000000410
q0And q isvScalar and vector parts, I, of quaternions q, respectively3×3Is 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 qrThe number of the conjugate quaternion of (c),
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,
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 formula (2) into formula (8) in consideration of rigid body spacecraft attitude dynamics to obtain:
Figure BDA0002999398200000059
wherein
Figure BDA00029993982000000510
Then respectively derive
Figure BDA00029993982000000511
To pair
Figure BDA00029993982000000512
δ τ and δ J5The jacobian matrix of.
Figure BDA00029993982000000513
To pair
Figure BDA00029993982000000514
The partial derivative of (c) is calculated as follows:
Figure BDA00029993982000000515
similarly:
Figure BDA00029993982000000516
Figure BDA00029993982000000517
wherein,
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(tk)＝H(tk)x(tk)+v(tk) (18)
wherein, tkRepresents 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) 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,
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(tk)＝[I6×6 06×6 06×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,
Figure BDA00029993982000000610
an estimated value of a dual quaternion vector part at the time k;
Figure BDA00029993982000000611
the estimated value of the dual quaternion vector part at the time 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
is an estimated value of the dual angular velocity at the moment k;
Figure BDA00029993982000000614
is an estimated value of the dual angular velocity at the moment k-1;
Figure BDA0002999398200000071
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 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 k-1 time line velocity.
(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
Pk|k-1＝ΦPk-1ΦT+Qk-1 (30)
Kk＝Pk|k-1HT(HPk|k-1HT+Rk)-1 (31)
Figure BDA0002999398200000077
xk＝xk-1+Kk(δqv,k-Hxk-1) (33)
Wherein phikState transition matrix at the moment k; pk|k-1Predicting an estimation error covariance matrix for the k moment; pk-1Estimating an error covariance matrix for the k time; kkIs a gain matrix at time k; qk-1A system noise variance matrix at the moment k; rkTo 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 (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.
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 of 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,
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 formula equal sign right side operation 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, Jxx，Jyy，JzzIs the moment of inertia of the main shaft, Jxy，Jxz，JyzIs 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 rotational inertia of the target spacecraft cannot be obtained, and only the relative ratio of the rotational inertia can be estimated. Selecting J in a rotational inertia matrixxxAs 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 added5＝[Jyy,Jzz,Jxy,Jxz,Jyz]Relative JxxThe 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 formula (1) into formula (7) in consideration of six-degree-of-freedom kinematics, we obtain:
Figure BDA00029993982000001010
wherein
Figure BDA00029993982000001011
qrAnd q isdAre respectively dual quaternions
Figure BDA00029993982000001012
The real part and the dual part of
Figure BDA00029993982000001013
q0And q isvScalar and vector parts, I, of quaternions q, respectively3×3Is 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 qrThe 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,
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 formula (2) into formula (8) in consideration of rigid body spacecraft attitude dynamics to obtain:
Figure BDA00029993982000001113
wherein
Figure BDA00029993982000001114
Then respectively derive
Figure BDA00029993982000001115
To pair
Figure BDA00029993982000001116
δ τ and δ J5The 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,
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(tk)＝H(tk)x(tk)+v(tk) (53)
wherein, tkRepresents 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) 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,
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(tk)＝[I6×6 06×6 06×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,
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
is an estimated value of the linear velocity at the moment k;
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; since the sampling frequency is 2Hz, the time interval Δ t is set to 0.5 s.
(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
Pk|k-1＝ΦPk-1ΦT+Qk-1 (65)
Kk＝Pk|k-1HT(HPk|k-1HT+Rk)-1 (66)
Figure BDA00029993982000001315
xk＝xk-1+Kk(δqv,k-Hxk-1) (68)
Wherein phikState transition matrix at the moment k; pk|k-1Predicting an estimation error covariance matrix for the k moment; pk-1Estimating an error covariance matrix for the k time; kkIs a gain matrix at time k; qk-1A system noise variance matrix at the moment k; rkTo measure the variance matrix. The initial parameter of the filter is selected as P17×17(0)＝diag(1×10-9I3×3,1×10-6I3×3,1×10-7I3×3,1×10- 6I3×3,1×10-4I5×5)，Q11×11＝1×10-6I11×11，R6×6＝diag(qnoise 2×I3×3,pnoise 2×I3×3). Wherein the attitude measurement noise qnoisePi/90, relative distance measurement noise pnoise＝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 (4)

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 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 determining a state equation and an observation equation;
s3: and (4) designing a new multiplicative extended Kalman filter by adopting a state correction method of an 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.
2. The method of claim 1, wherein: in step S1, the six-degree-of-freedom kinematics and attitude dynamics model of the space rigid body spacecraft is established as follows:
Figure FDA0002999398190000011
Figure FDA0002999398190000012
in the formula,
Figure FDA0002999398190000013
is a dual quaternion of the target spacecraft system relative to the inertial system,
Figure FDA0002999398190000014
is its derivative with respect to time;
Figure FDA0002999398190000015
the dual angular velocity of the system of the target spacecraft relative to the inertial system is defined under the inertial system;
Figure FDA0002999398190000016
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 FDA0002999398190000017
defining the three-axis rotation angular velocity of the target spacecraft body system relative to the inertial coordinate system under the inertial system;
Figure FDA0002999398190000018
is composed of
Figure FDA0002999398190000019
A derivative with respect to time; tau is external disturbance moment and is modeled as Gaussian white noise; j' is defined as the relative ratio of moments of inertia:
Figure FDA0002999398190000021
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 selectedxxAs a reference; j is a rotational inertia matrix, and under the background of visual navigation, the inertia product is not zero:
Figure FDA0002999398190000022
wherein, Jxx，Jyy，JzzIs the moment of inertia of the main shaft, Jxy，Jxz，JyzAs the product of inertia, the remaining element vector J of the rotational inertia matrix is5＝[Jyy,Jzz,Jxy,Jxz,Jyz]Relative JxxThe 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.
3. The method of claim 1, wherein: in step S2, the determined state equation and observation equation are:
Figure FDA0002999398190000023
Z(tk)＝H(tk)x(tk)+v(tk) (6)
wherein, tkRepresents 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 FDA0002999398190000024
The matrix of the error dual quaternion, the error dual angular velocity and the error rotational inertia is as follows:
Figure FDA0002999398190000025
Figure FDA0002999398190000026
Figure FDA0002999398190000027
wherein
Figure FDA0002999398190000031
And
Figure FDA0002999398190000032
to represent
Figure FDA0002999398190000033
And the estimated value of J is calculated,
Figure FDA0002999398190000034
is composed of
Figure FDA0002999398190000035
Conjugation of (1);
the state matrix is:
Figure FDA0002999398190000036
in the formula,
Figure FDA0002999398190000037
Figure FDA0002999398190000038
Figure FDA0002999398190000039
Figure FDA00029993981900000310
wherein,
Figure FDA00029993981900000311
a is any three-dimensional vector;
Figure FDA00029993981900000312
Figure FDA00029993981900000313
is any dual quaternion, qrAnd q isdAre respectively as
Figure FDA00029993981900000314
The real part and the dual part of (a),
Figure FDA00029993981900000315
representing the vectorial part of the quaternion, process noise
Figure FDA00029993981900000316
The process noise matrix is:
Figure FDA00029993981900000317
because the relative position and attitude of the target spacecraft are measured and solved by the camera, the measurement matrix is as follows:
H(tk)＝[I6×6 06×6 06×5] (16)。
4. 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 FDA0002999398190000041
Figure FDA0002999398190000042
Figure FDA0002999398190000043
Wherein,
Figure FDA0002999398190000044
an estimated value of a dual quaternion vector part at the time k;
Figure FDA0002999398190000045
the estimated value of the dual quaternion vector part at the time k-1;
Figure FDA0002999398190000046
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 FDA0002999398190000047
is an estimated value of the dual angular velocity at the moment k;
Figure FDA0002999398190000048
is an estimated value of the dual angular velocity at the moment k-1;
Figure FDA0002999398190000049
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 FDA00029993981900000410
is an estimated value of the linear velocity at the moment k;
Figure FDA00029993981900000411
is an estimated value of the linear velocity at the moment of k-1;
Figure FDA00029993981900000412
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 FDA00029993981900000413
(3) Filter update
Figure FDA00029993981900000414
Pk|k-1＝ΦPk-1ΦT+Qk-1 (22)
Kk＝Pk|k-1HT(HPk|k-1HT+Rk)-1 (23)
Figure FDA00029993981900000415
xk＝xk-1+Kk(δqv,k-Hxk-1) (25)
Wherein phikState transition matrix at the moment k; pk|k-1Predicting an estimation error covariance matrix for the k moment; pk-1Estimating an error covariance matrix for the k time; kkIs a gain matrix at time k; qk-1A system noise variance matrix at the moment k; rkMeasuring a variance matrix;
(4) state correction
Two constraints (q) using dual quaternionsr)*qr1 and
Figure FDA00029993981900000416
calculating a complete error dual quaternion with an error dual quaternion vector as a state quantity
Figure FDA0002999398190000051
The above formula is suitable for
Figure FDA0002999398190000052
The case (2);
when in use
Figure FDA0002999398190000053
Then, the state is corrected using the following equation:
Figure FDA0002999398190000054
wherein
Figure FDA0002999398190000055
Are respectively as
Figure FDA0002999398190000056
Real part ofAnd a dual portion.
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 true CN113091754A (en) 2021-07-09
CN113091754B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114012507A (en) * 2021-12-09 2022-02-08 天津工业大学 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

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767797A (en) * 2017-03-23 2017-05-31 南京航空航天大学 A kind of inertia based on dual quaterion/GPS Combinated navigation methods
CN110470297A (en) * 2019-03-11 2019-11-19 北京空间飞行器总体设计部 A kind of attitude motion of space non-cooperative target and inertial parameter estimation method
CN110567461A (en) * 2019-08-01 2019-12-13 北京航空航天大学 Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106767797A (en) * 2017-03-23 2017-05-31 南京航空航天大学 A kind of inertia based on dual quaterion/GPS Combinated navigation methods
CN110470297A (en) * 2019-03-11 2019-11-19 北京空间飞行器总体设计部 A kind of attitude motion of space non-cooperative target and inertial parameter estimation method
CN110567461A (en) * 2019-08-01 2019-12-13 北京航空航天大学 Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114012507A (en) * 2021-12-09 2022-02-08 天津工业大学 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
CN114543797B (en) * 2022-02-18 2024-06-07 北京市商汤科技开发有限公司 Pose prediction method and device, equipment and medium

Also Published As

Publication number Publication date
CN113091754B (en) 2023-02-28

Similar Documents

Publication Publication Date Title
CN109991636B (en) Map construction method and system based on GPS, IMU and binocular vision
Ye et al. Tightly coupled 3d lidar inertial odometry and mapping
CN109376785B (en) Navigation method based on iterative extended Kalman filtering fusion inertia and monocular vision
CN110398257B (en) GPS-assisted SINS system quick-acting base initial alignment method
CN110030994B (en) Monocular-based robust visual inertia tight coupling positioning method
CN110986939B (en) Visual inertia odometer method based on IMU (inertial measurement Unit) pre-integration
Fleps et al. Optimization based IMU camera calibration
CN108731670A (en) Inertia/visual odometry combined navigation locating method based on measurement model optimization
CN110567461B (en) Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope
CN110455309B (en) MSCKF-based visual inertial odometer with online time calibration
CN113091754B (en) Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method
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
CN111707261A (en) High-speed sensing and positioning method for micro unmanned aerial vehicle
CN101949703A (en) Strapdown inertial/satellite combined navigation filtering method
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN113503872B (en) Low-speed unmanned aerial vehicle positioning method based on fusion of camera and consumption-level IMU
CN112985450B (en) Binocular vision inertial odometer method with synchronous time error estimation
CN105806363A (en) Alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter)
CN108827288A (en) A kind of dimensionality reduction strapdown inertial navigation system Initial Alignment Method and system based on dual quaterion
CN114046800A (en) High-precision mileage estimation method based on double-layer filtering framework
CN115046545A (en) Positioning method combining deep network and filtering
CN115014346A (en) Consistent efficient filtering algorithm based on map and oriented to visual inertial positioning
CN111912427A (en) Method and system for aligning motion base of strapdown inertial navigation assisted by Doppler radar
CN114764830A (en) Object pose estimation method based on quaternion EKF and uncalibrated hand-eye system

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