Patent Application
20230271727
The present invention relates to the field of attitude estimation, and more particularly, to an attitude estimation method, a terminal, a system and a computer-readable storage medium.
A key problem of dynamic attitude estimation is to describe the rotational motion of a rigid body relative to a given frame of reference. This problem has been paid much attention in many fields, such as navigation of autonomous or manually guided vehicles, tactical missile guidance, alignment of satellites intended to face the Earth, design of filtering methods used to stabilize offshore platforms, etc. Since the development of engineering works in the 60s of the 20th century promoted space exploration, research on attitude estimation has been growing systematically.
The reason for the continuing development of attitude estimation solutions lies not only in the pursuit of implementation of lightweight computational algorithms, but also in the constant need to overcome well-known topological obstacles, the inherent limitations of low-cost strap-down sensors, and/or the need to deal with situations where a particular sensor is considered unreliable, such as the global positioning system (GPS) in underwater areas, or magnetometers in environments with strong magnetic signatures. Aware of these deficiencies, the scientific community has been actively exploring solutions by means of single vector observations in an attempt to simplify the design, and to add operational redundancy. Furthermore, in order to avoid accumulated errors over long periods of time, it is crucial to be able to determine the sensor biases, in particular with respect to gyroscopes, which may affect the feasibility of the solution if they cannot be resolved.
The emergence of advanced sensors such as fiber optic gyroscopes (FOGs) prompted the study of new attitude observers that involve higher precision. However, most of the attitude estimation solutions require explicit measurements of at least two constant inertial reference vectors.
The technical problem to be solved by the present invention is to provide an attitude estimation method, a terminal, a system and a computer-readable storage medium, which reduce the complexity of attitude estimation and improve the accuracy of attitude estimation.
In order to solve the technical problem, the invention adopts a technical solution of an attitude estimation method comprising:
In order to solve the technical problem, the invention adopts another technical solution of an attitude estimation terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor, which when executed implements the steps of the above-mentioned attitude estimation method.
In order to solve the technical problem, the invention adopts another technical solution of an attitude estimation system comprising a Kalman filter and an attitude observer established on the special orthogonal group of order three;
{dot over ({circumflex over (R)})}(t)={circumflex over (R)}(t)S[ωm(t)−{circumflex over (b)}ω(t)−{circumflex over (R)}T(t)IωE+kω
In order to solve the technical problem, the invention adopts another technical solution of a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the above attitude estimation method.
The advantageous effect of the present invention is that by means of single observations of a constant inertial reference vector, e.g., direction of gravitational acceleration, and body-fixed measurements of angular velocity of a rigid body, an output comprising an estimated bias-corrected inertial reference vector expressed in body-fixed coordinates, an estimation of the Earth angular velocity expressed in body-fixed coordinates, and two bias offsets associated with the two sensors involved can be obtained from a preset Kalman filter; then the rotation matrix of the rigid body can be determined according to the estimated bias-corrected body-fixed inertial reference vector, the estimation of the Earth angular velocity expressed in body-fixed coordinates, the bias offset associated with the angular velocity sensor, and two preset inertial reference vectors; in the estimation process, only one constant inertial reference vector needs to be measured explicitly in body-fixed coordinates, and the geometric relationship between the inertial reference vector and the inertial representation of the Earth's angular velocity is used to greatly simplify the complexity of the attitude estimation algorithm. Meanwhile, the time-varying characteristics of the Kalman filter help to avoid tedious empirical gain adjustment processes often relying on sets of piecewise constant gains, improve the convergence speed of the algorithm, and ensure the accuracy of the attitude estimates.
The above-mentioned attitude estimation method, terminal, system and computer-readable storage medium of the present invention can be applied to application scenarios requiring attitude estimation, such as navigation of underwater vehicles, offshore platform stabilization, Earth satellite alignment, etc., and the following is described by way of specific embodiments:
S1, collecting body-fixed measurements of the angular velocity of a rigid body and collecting body-fixed measurements of a constant inertial reference vector regarding the same rigid body;
S2, taking the measured values of the angular velocity and the inertial reference vector as inputs of a preset Kalman filter, and obtaining an output comprising an estimated bias-corrected body-fixed inertial reference vector, an estimation of the Earth angular velocity expressed in body-fixed coordinates, and two bias offsets associated with the two collectors involved when collecting, in some embodiments, the collectors are sensors;
S3, determining the rotation matrix of the rigid body according to the estimated bias-corrected body-fixed inertial reference vector, the estimation of the Earth angular velocity expressed in body-fixed coordinates, the two bias offsets and two preset inertial reference vectors.
In a specific implementation, firstly, three reference frames are preset, the first one is an inertial reference frame, which is denoted as {I}, and the second one is a reference frame fixed to the body of the robotic platform, which is denoted as {B}, and it is assumed that R(t) represents a rotation matrix from {B} to {I}, R(t) ε SO(3), SO(3):={X ε 3×3; XXT=XTX=I, det(X)=1} represents the special orthogonal group of order three, I represents the identity matrix, T represents the transpose operator, and
3×3 represents the set of 3-by-3 real matrices;
A main object of the embodiment is to determine R(t) using the angular velocity measured by a set of high-grade rate gyroscopes, and using the measurements of some constant inertial reference vector obtained from a three-axis inertial sensor.
The third frame of reference is a navigation coordinate system, denoted as {T}, which follows the NED convention, i.e., the right-hand rule, similar to {T}, the coordinate reference frames {I} and {B} also follow the NED convention, the positive direction of their z-axis corresponding to the downward direction, and
the inertial reference frame {I} should be such that Tvhas only a downward component when expressed in {I}, i.e.,
I
v=[0,0,ν]T
The derivative with respect to time t of R(t)is:
{dot over (R)}(t)=R(t)S[ω(t)] (2)
ωm(t)=ω(t)+ωE(t)+bω+nω(t) (3)
m(t)=v(t)+bm+nm(t) (4)
IωE×Iv≠0
∥bm∥<<∥v(t)∥=∥═v∥=ν
IωE=TIR∥IωE∥[cos (φ),0,−sin (φ)]T
ωE(t)=ωE,xy(t)+ωE,z(t) (6)
ωE,z(t)−αv(t)−α(m(t)−bm), (8)
0={dot over (R)}(t)(m(t)−bm)+R(t){dot over (v)}(t), (9)
{dot over (v)}(t)=−S[ωm(t)−ωE,xy(t)−bω](m(t)−bm) (10)
{dot over (ω)}E,xy(t)=−S[ωm(t)−ωE,z(t)−bω]ωE,xy(t)
{dot over (ω)}E,xy9′t)=−S[ωm(t)−α(m(t)−bm)−bω]ωE,xy(t) (11)
{dot over (v)}(t)=−S[ωm9t)](m(t)−bm)+S[ωE,xy(t)+bω](m(t)−bm), (12)
{dot over (v)}(t)≈−S[ωm(t)−ΩE,xy(t)+bω]m(t)+S[ωm(t)]bm, (13)
{dot over (ω)}E,xy(t)≈−S[ωm(t)−αm(t)]ωE,xy(t) (14)
finally, a constraint on the Kalman filter is determined by the inner product of Equation (8) and ωE,xy(t), i.e.,
0=ωE,xyT(t)(m(t)−bm)≈ωE,xyT(t)m(t) (15)
x(t):=[vT(t),ωE,xy(t),bmT(t),bωT(t)]Tε12 (16)
To sum up, a linear time-varying system is designed;
{circumflex over (x)}(t):=[{circumflex over (v)}T(t),{circumflex over (ω)}E,xyT(t),{circumflex over (b)}mT(t),{circumflex over (b)}ωT(t)]Tε12 (18)
{circumflex over (ω)}E(t)={circumflex over (ω)}E,xy(t)+α{circumflex over (v)}(t) (20)
In further alternative embodiments, a rotational matrix observer is considered as follows:
{dot over ({circumflex over (R)})}(t)={circumflex over (R)}(t)S[ωm(t)−{circumflex over (b)}ω(t)−{circumflex over (R)}T(t)IωE+kω
It is proved theoretically that the designed attitude observer is locally input state stable (LISS) and almost globally asymptotically stable (AGAS).
In an embodiment, a real scene is simulated within the range of robotic platform attitude estimation, considering a robotic platform that describes a rotational motion in three-dimensional space, the angular velocity of which is represented as:
Further assuming that the robotic platform is located at latitude φ=38.777816°, longitude ψ=9.097570°, and at sea level, the corresponding norm of the inertial angular velocity of the planet is ∥TωE∥=7.2921159×10−5 (rad/s), taking into account the length of time of the sun, about 15 (deg/hr); its vector representation in {I} can be given by Equation (6).
Assume that the robotic platform is equipped with a commercial KVH Fiber Optic Gyroscope (FOG) 1775 Inertial Measurement Unit (IMU), which has a three-axis accelerometer that can collect vector measurements of the gravitational acceleration, according to the sea level and latitude shown above, and according to the 1980 International Gravity Formula, the components of the gravitational induced inertial reference acceleration are as follows:
T
v=[0,0,9.800611]T(m/s2)
To simulate the worst-case specifications of the KVH1775 with respect to its three-axis accelerometer, which is characterized by a random walk of velocity of 0.12 (mg/Hz), a zero-mean
Gaussian white noise sequence with a standard deviation of 0.0059 (m/s2) is added to all simulated accelerometer data assuming a sampling frequency of 25 Hz.
Angular velocity readings collected from the advanced rate gyro of the KVH 1775 were corrupted by angular random walk noise of 0.7 deg/hr/√{square root over (Hz)}. For a sampling frequency of 25 Hz, the angular random walk noise translates approximately to a standard deviation of 0.972 milli-degrees per second given a rate integration configuration.
According also to the worst-case specifications of the KVH 1775 manufacturer, the two constant bias offsets are set to:
{dot over (x)}(t0)=[mT(t0),0,0,0]T
The graphs in
In general, the convergence time was quite fast, and all error sequences reached steady state behavior around 5 minutes. This duration is extremely important because the rotation matrix observer is driven by the estimates of the Kalman filter (Equation 19). Table 1 provides further statistical observations of the given Kalman filter. For 25 (min) ≤t≤30 (min), the mean and standard deviation of each error variable across the three-dimensional Euclidean space is calculated and then averaged over the coordinates x, y and z. In addition, according to Equation (20), Table 1 also lists the average norm of the estimated value of the Earth angular velocity, and it is observed that the average norm of the estimated value of the Earth angular velocity is approximately 14.797 deg/hr (equivalent to a deviation of 1.353%), thereby confirming the high accuracy of the Kalman filter (Equation 19).
In order to verify the robustness of the provided attitude estimation, an error {tilde over (R)}(t) between R(t) and {circumflex over (R)}(t) is represented in polar coordinates, {tilde over (θ)}(t) ε D:=[0,π]and {tilde over (μ)}(t) ε S(2) are assumed to constitute the Euler angle-axis pair of {tilde over (R)}(t), and the simulation is carried out when {tilde over (θ)}(t0) is very large, with t0 representing the initial time of the simulation;
Assuming a randomly generated unit vector {tilde over (μ)}(t), the initial angular deviation is set to {tilde over (θ)}(t0)=175 degrees, and the corresponding initial rotation matrix estimate is calculated accordingly. Table 2 shows the observer gains, which are set in a piecewise manner. As the Kalman filter adjusts, the observer gains kω
The graphs of
In an alternative embodiment, to verify the effectiveness of the cascaded attitude estimation solution, experiments were performed on the designed cascaded structure using a three-axis high precision FOG IMU KVH 1775 mounted on an Ideal Aerosmith Model 2103HT Three-Axis Positioning and Motion Rate Table (MRT) designed to provide accurate position, velocity and acceleration motion for IMU and inertial navigation system development and/or production testing and calibration. Ground-truth data from the MRT is characterized by a rate accuracy of 0.5%±0.0005 (deg/s) on the limited rotation axes (y and z), a rate accuracy of 0.01%±0.0005 (deg/s) on the unlimited rotation axis (x), and a position accuracy of 30 arc sec on all axes;
KVH 1775 provides three axis readings of angular velocity and acceleration. A slow rotational motion is considered to ensure that the magnitude of the gravitational field is a dominant acceleration term. At room temperature, the accelerometer of the device is characterized by a random walk of speed of 0.12 (mg/√{square root over (Hz)});
With respect to sensor biases, bm and bω are also obtained during a full calibration of KVH 1775.
The data acquired from the MRT was taken at 128 Hz and then appropriately down-sampled to 25 Hz to match the sampling frequency of the KVH 1775;
The tuning of the Kalman filter is as follows:
{circumflex over (x)}(t0)=[mT(t0),0,0,0]T
Q=diag(10−6I, 10−13I, 10−16I, 10−16I) and =diag(10−7I, 10−7);
On the other hand, the experimental estimated value of ωE,xy(t) is in good agreement with the simulation results.
The evolutions of the estimation error {tilde over (b)}m(t) and the estimation error {tilde over (b)}ω(t) are shown in
In general, the designed Kalman filter provides a consistent estimate of the bias of both sensors. Furthermore, the next step on the results of the rotation matrix estimation will further prove the effectiveness of the Kalman filter.
For completeness,
The initial rotation matrix is estimated so that the corresponding initial angular deviation thereof is about 128°, as shown in
In another alternative implementation, as shown in
In summary, the present invention provides an attitude estimation method, a terminal, a system and a computer-readable storage medium, and for the problem of estimating attitude and sensor biases, a cascading solution method is provided, wherein the first part of the cascade is a Kalman filter applied to an LTV system. The second part of the cascade is characterized by a nonlinear attitude observer based on SO(3). In addition to the estimates of the Kalman filter, the observer is also driven by angular velocity measurements provided by a set of three high-grade rate gyros. It is proved that the nonlinear attitude observer is LISS with respect to the Kalman filter estimates. The simulation results show the effectiveness of the algorithm. The experimental results further prove the effectiveness of the above attitude estimation algorithm, which can be applied to applications requiring high-efficiency and high-precision attitude data.
While the foregoing is directed to embodiments of the present invention, other and further embodiments of the invention may be devised without departing from the basic scope thereof, and the scope thereof is determined by the claims that follow.
