FUNCTIONAL ITERATIVE INTEGRATION-BASED METHOD AND SYSTEM FOR INERTIAL NAVIGATION SOLUTION

Abstract

A functional iterative integration-based method for an inertial navigation solution includes: fitting a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval; iteratively calculating Chebyshev polynomial coefficients of an attitude quaternion by using the obtained Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and performing polynomial truncation on a result obtained from each iterative calculation according to a preset order; iteratively calculating Chebyshev polynomial coefficients of a velocity/position by using the obtained Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and performing polynomial truncation on a result obtained from each iterative calculation according to a preset order; and obtaining attitude/velocity/position information on the corresponding time interval.

Description

TECHNICAL FIELD

The present invention relates to the technical fields of inertial navigation and robotics, and more particularly, to a functional iterative integration-based method and system for inertial navigation solution.

BACKGROUND

The accuracy in calculation or estimation of rigid body motion in three-dimensional space is a critical problem in numerous fields such as physics, robotics, navigation and guidance, machinery, and computer vision. In inertial navigation, information on angular velocity and acceleration is measured by gyroscopes and accelerometers, and then integrated to obtain information on attitude, velocity and position. Inertial navigation has the advantage of being rich in motion information and having a fully autonomous work process without the aid of external information. Inertial navigation is favored especially in applications where satellite navigation signals are interrupted or deceived caused by interference, and typically used as the core navigation information source for various motion platforms.

At present, the algorithms for a majority of inertial navigation systems have two significant shortcomings, 1) principle defect, associated with the inertial navigation algorithm uses an approximation to the theoretical equation, for example, the attitude algorithm uses a simplified rotation vector, and the velocity algorithm uses the first-order approximation of attitude; 2) algorithmic defect associated with the inertial navigation algorithm parameters are designed in special motion situations, such as coning motion or swing motion, and thus cannot ensure the optimality in actual motion.

In view of the above-mentioned problems of the attitude algorithms, Chinese Patent CN 201710273489.3 proposes a Functional Iterative Integration-Based Method For Rigid Body Attitude Solution, which includes: fitting a polynomial function of an angular velocity according to gyroscope-measured values on a time interval; iteratively calculating a Rodrigues vector by using the fitted polynomial function of the angular velocity and an integral equation of the Rodrigues vector; and acquiring an attitude change in a form of quaternion on the time interval according to a result obtained from the iterative calculation. This method has the advantage of high calculation accuracy, but does not fully exploit the good properties of the Chebyshev polynomials in the iterative calculation process. Moreover, the polynomial order of the Rodrigues vector increases sharply along with the iterative calculation process, causes a large computational burden, which is difficult to meet real-time applications. In this regard, Chinese Patent CN 201810236436.9 proposes a Functional Iterative Integration-Based Fast Calculation Method And System For Rigid Body Attitude Solution. This fast algorithm uses the fitted Chebyshev polynomial coefficients of the angular velocity and the Rodrigues vector integral equation to iteratively calculate the Chebyshev polynomial coefficients of the Rodrigues vector, and performs polynomial truncation on the result obtained from each iterative calculation according to the preset order, which significantly improves the calculation efficiency. The aforementioned two invention patents have satisfactorily solved the problem of high-accuracy attitude solution in the inertial navigation solution algorithm.

However, an effective approach to solving the above-mentioned shortcomings in the velocity and position solution remains absent in the prior art.

SUMMARY

In view of the above-mentioned shortcomings in the prior art, an objective of the present invention is to provide a functional iterative integration-based method and system for inertial navigation solution.

According to the present invention, a functional iterative integration-based method for inertial navigation solution includes:

a fitting step: fitting a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval;

an attitude iterative solution step: iteratively calculating Chebyshev polynomial coefficients of an attitude quaternion by using the obtained Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and performing polynomial truncation on a result obtained from each iterative calculation according to a preset order;

a velocity/position iterative solution step: iteratively calculating Chebyshev polynomial coefficients of a velocity/position by using the obtained Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and performing polynomial truncation on a result obtained from each iterative calculation according to a preset order;

and an attitude/velocity/position acquisition step: obtaining attitude/velocity/position information on the corresponding time interval according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

Preferably, the gyroscope-measured values include a measured value of the angular velocity or a measured value of an angular increment, and the accelerometer-measured values include a measured value of the specific force or a measured value of a velocity increment.

Preferably, in the fitting step, the time interval is divided into at least two sub-time intervals to perform sequential calculations.

Preferably, in the attitude iterative solution step or the velocity/position iterative solution step, the iterative calculation is performed until the maximum number of iterative calculations is reached or a predetermined convergence condition is satisfied.

Preferably, the method further includes:

a navigation step: performing a navigation according to the attitude/velocity/position information on the corresponding time interval.

A functional iterative integration-based system for inertial navigation solution according to the present invention includes:

a fitting module, configured to fit a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval;

an attitude iterative solution module, configured to iteratively calculate Chebyshev polynomial coefficients of an attitude quaternion by using the obtained Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and perform polynomial truncation on a result obtained from each iterative calculation according to a preset order;

a velocity/position iterative solution module, configured to iteratively calculate Chebyshev polynomial coefficients of a velocity/position by using the obtained Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and perform polynomial truncation on a result obtained from each iterative calculation according to a preset order; and

an attitude/velocity/position acquisition module, configured to obtain attitude/velocity/position information on the corresponding time interval according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

Preferably, the gyroscope-measured values include a measured value of the angular velocity or a measured value of an angular increment, and the accelerometer-measured values include a measured value of the specific force or a measured value of a velocity increment.

Preferably, the fitting module divides the time interval into at least two sub-time intervals to perform sequential calculations.

Preferably, the attitude iterative solution module and the velocity/position iterative solution module perform the iterative calculation until the maximum number of iterative calculations is reached or a predetermined convergence condition is satisfied.

Preferably, the system further includes:

a navigation module, configured to perform a navigation according to the attitude/velocity/position information on the corresponding time interval.

Compared with the prior art, the present invention has the following advantages:

Based on the functional iterative integration and Chebyshev polynomial approximation techniques, the angular velocity and the specific force are reconstructed by means of fitting according to the gyroscope-measured information and accelerometer-measured information, and precise integral calculation of kinematics equations with respect to attitude, velocity and position is performed by a functional iterative integration approach. The iterative calculation process employs the Chebyshev polynomials with good numerical characteristics, whereby the iterative integrals of attitude, velocity and position are converted into iterative calculations of the corresponding Chebyshev polynomial coefficients. The calculation speed is increased by an order truncation method without significantly reducing the calculation accuracy, and the noncommutative errors caused by motion, such as an attitude coning error, a velocity swing error and a position scrolling error, are effectively eliminated.

BRIEF DESCRIPTION OF THE DRAWING

Other features, objectives and advantages of the present invention will be clearly described with reference to the detailed description of the non-restrictive embodiments and the drawing.

FIGURE is a solution flow chart of the present invention.

DETAILED DESCRIPTION OF THE EMBODIMENTS

The present invention will be described in detail below with reference to the specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but should not be construed as limiting the present invention in any form. It should be noted that for those having ordinary skill in the art, several changes and improvements can be made without departing from the concept of the present invention. These changes and improvements shall fall within the scope of protection of the present invention.

The functional iterative integration-based method for inertial navigation solution provided by the present invention can select different calculation reference coordinate systems. In consideration of the computational burden, the present invention uses the earth coordinate system as the calculation reference coordinate system for illustration.

As shown in FIGURE, the present invention provides a function iterative integration-based method and system for inertial navigation solution. The method includes:

A fitting step: A Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force are fitted according to gyroscope-measured values and accelerometer-measured values on a time interval. The gyroscope-measured values include a measured value of the angular velocity or a measured value of an angular increment. The accelerometer-measured values include a measured value of the specific force (non-gravitational acceleration) or a measured value of a velocity increment.

An attitude iterative solution step: Chebyshev polynomial coefficients of an attitude quaternion is iteratively calculated by using Chebyshev polynomial coefficients of the angular velocity obtained in the fitting step and an integral equation of the attitude quaternion, and polynomial truncation is performed on a result obtained from each iterative calculation according to a preset order. The attitude iterative process continues until the maximum number of iterative calculations is reached or the predetermined convergence condition is satisfied.

A velocity/position iterative solution step: Chebyshev polynomial coefficients of a velocity/position is iteratively calculated by using the Chebyshev polynomial coefficients of the specific force obtained in the fitting step, the Chebyshev polynomial coefficients of the attitude quaternion obtained in the attitude iterative solution step and an integral equation of the velocity/position, and polynomial truncation is performed on a result obtained from each iterative calculation according to a preset order. The velocity/position iterative process continues until the maximum number of iterative calculations is reached or the predetermined convergence condition is satisfied.

An attitude/velocity/position acquisition step: Attitude/velocity/position information on the corresponding time interval is obtained according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

A Chebyshev polynomial of a first type is defined on an interval [−1 1] and given by the following iterative relationship:

F ₀(x)=1,F₁(x)=x,F_i+1(x)=2×F_i(x)−F_i−1(x);

• • wherein F_i(x) represents the i^th order Chebyshev polynomial of the first type.

Fitting step: The Chebyshev polynomial function of the angular velocity and the Chebyshev polynomial function of the specific force are fitted according to the gyroscope-measured values and the accelerometer-measured values on the time interval.

For the measured value {tilde over (ω)}_tk of the angular velocity or the measured value Δ{tilde over (θ)}_tk of the angular increment obtained by N gyroscopes at t_k, and the measured value {tilde over (f)}_tk of the specific force or the measured value Δ{tilde over (v)}_tk of the velocity increment obtained by N accelerometers, k=1, 2, . . . N, let

$t=\frac{t_N}{2}\left(1+\tau \right),$

wherein τ represents a mapped time variable, and the original time interval is mapped to [−1 1]. The angular velocity is approximately fitted by the following Chebyshev polynomials not exceeding N−1 order:

$\begin{array}{cc}{\hat{\omega }}^b=\sum _{i=0}^{n_{\omega }}c_iF_i\left(\tau \right),n_{\omega }\le N-1;& \left(1\right)\end{array}$

the specific force is approximately fitted by the following Chebyshev polynomials not exceeding N−1 order:

$\begin{array}{cc}{\hat{f}}^b=\sum _{i=0}^{n_f}d_iF_i\left(\tau \right),n_f\le N-1;& \left(2\right)\end{array}$

wherein n_ω and n_f represent the order of the Chebyshev polynomial of the angular velocity and the order of the Chebyshev polynomial of the specific force, respectively; c_i and d_i represent a coefficient vector of the i^th order Chebyshev polynomial of the angular velocity and a coefficient vector of the i^th order Chebyshev polynomial of the specific force, respectively; and r represents the mapped time variable.

For the angular velocity/specific force measurement, the coefficients c_i and d_i are determined by solving the following equations:

$\begin{array}{cc}{\Gamma \left(n_{\omega }\right)\left[\begin{array}{cccc}{c}_0& c_1& \dots & c_{n_{\omega }}\end{array}\right]}^T={\left[\begin{array}{cccc}{\tilde{\omega }}_{t_1}& {\tilde{\omega }}_{t_2}& \dots & {\tilde{\omega }}_{t_N}\end{array}\right]}^T{\Gamma \left(n_f\right)\left[\begin{array}{cccc}{d}_0& d_1& \dots & d_{n_f}\end{array}\right]}^T={\left[\begin{array}{cccc}{\tilde{f}}_{t_1}& {\tilde{f}}_{t_2}& \dots & {\tilde{f}}_{t_N}\end{array}\right]}^T;& \left(3\right)\end{array}$

for the angular increment/velocity increment measurement, the coefficients c_i and d_i are determined by solving the following equations:

$\begin{array}{cc}{\Theta \left(n_{\omega }\right)\left[\begin{array}{cccc}{c}_0& c_1& \dots & c_{n_{\omega }}\end{array}\right]}^T={\left[\begin{array}{cccc}\Delta {\tilde{\theta }}_{t_1}& \Delta {\tilde{\theta }}_{t_2}& \dots & \Delta {\tilde{\theta }}_{t_N}\end{array}\right]}^T{\Theta \left(n_f\right)\left[\begin{array}{cccc}{d}_0& d_1& \dots & d_{n_f}\end{array}\right]}^T={\left[\begin{array}{cccc}\Delta {\tilde{v}}_{t_1}& \Delta {\tilde{v}}_{t_2}& \dots & \Delta {\tilde{v}}_{t_N}\end{array}\right]}^T;& \left(3\right)\end{array}$

wherein the superscript T represents an operation of vector transpose or matrix transpose, and the matrices Γ and Θ are defined as follows:

$\begin{array}{cc}\Gamma \left(n\right)=\left[\begin{array}{cccc}1& F_1\left({\tau }_1\right)& \dots & F_n\left({\tau }_1\right)\\ 1& F_1\left({\tau }_2\right)& \dots & F_n\left({\tau }_2\right)\\ ⋮& ⋮& ⋮& ⋮\\ 1& F_1\left({\tau }_N\right)& \dots & F_n\left({\tau }_N\right)\end{array}\right],& \left(5\right)\\ \Theta \left(n\right)=\left[\begin{array}{cccc}{G}_{0,\left[\begin{array}{cc}{\tau }_0& {\tau }_1\end{array}\right]}& G_{1,\left[\begin{array}{cc}{\tau }_0& {\tau }_1\end{array}\right]}& \dots & G_{n,\left[\begin{array}{cc}{\tau }_0& {\tau }_1\end{array}\right]}\\ G_{0,\left[\begin{array}{cc}{\tau }_1& {\tau }_2\end{array}\right]}& G_{1,\left[\begin{array}{cc}{\tau }_1& {\tau }_2\end{array}\right]}& \dots & G_{n,\left[\begin{array}{cc}{\tau }_1& {\tau }_2\end{array}\right]}\\ ⋮& ⋮& ⋮& ⋮\\ G_{0,\left[\begin{array}{cc}{\tau }_{N-1}& {\tau }_N\end{array}\right]}& G_{1,\left[\begin{array}{cc}{\tau }_{N-1}& {\tau }_N\end{array}\right]}& \dots & G_{n,\left[\begin{array}{cc}{\tau }_{N-1}& {\tau }_N\end{array}\right]}\end{array}\right];& \end{array}$

wherein function G_{i,[τk−1τk]} is defined as follows:

$\begin{array}{cc}{G}_{i,\left[\begin{array}{cc}{\tau }_{k-1}& {\tau }_k\end{array}\right]}=\{\begin{array}{cc}\left(\frac{iF_{i+1}\left({\tau }_k\right)}{i^2-1}-\frac{{\tau }_kF_i\left({\tau }_k\right)}{i-1}\right)-\left(\frac{iF_{i+1}\left({\tau }_{k-1}\right)}{i^2-1}-\frac{{\tau }_{k-1}F_i\left({\tau }_{k-1}\right)}{i-1}\right),& i\ne 1\\ \frac{{\tau }_k^2-{\tau }_{k-1}^2}{2},& i=1\end{array}.& \left(6\right)\end{array}$

Attitude iterative solution step: The Chebyshev polynomial coefficients of the attitude quaternion is iteratively calculated by using the Chebyshev polynomial coefficients of the angular velocity obtained in the fitting step and an integral equation of the attitude quaternion, and polynomial truncation is performed on the result obtained from each iterative calculation according to a preset order.

Assuming that in the l^th iterative calculation, the Chebyshev polynomial of the attitude quaternion is expressed as:

$\begin{array}{cc}{q}_l=\sum _{i=0}^{m_q}b_{l,i}F_i\left(\tau \right);& \left(7\right)\end{array}$

wherein m_q represents the predetermined truncation order, and b_l,i represents the coefficient of the i^th Chebyshev polynomial in the l^th iterative calculation. When l=0, q₀(t)≡q(0), wherein q(0) represents the initial attitude quaternion. The Chebyshev polynomial coefficients of the attitude quaternion can be calculated iteratively as follows:

$\begin{array}{cc}{q}_{l+1}=q\left(0\right)+\frac{t_N}{8}\left(\sum _{i=0}^{m_q}\sum _{j=0}^{n_{\omega }}b_{l,i}\circ c_j\left(G_{i+j,\left[\begin{array}{cc}-1& \tau \end{array}\right]}+G_{i-j,\left[\begin{array}{cc}-1& \tau \end{array}\right]}\right)-2\sum _{i=0}^{m_q}{\omega }_e\circ b_{l,i}G_{i,\left[\begin{array}{cc}-1& \tau \end{array}\right]}\right)=\sum _{i=0}^{m_q+m_{\omega }+1}b_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{m_q}b_{l+1,i}F_i\left(\tau \right),& \left(8\right)\end{array}$

until the convergence condition is satisfied or the preset maximum number of iterative calculations is reached. The convergence condition is set according to the Euclidean distance deviation between the Chebyshev polynomial coefficients of two iterative calculations, i.e.,

${\left(\sum _{i=0}^{m_q}{b_{l+1,i}-b_{l,i}}^2\right)}^{1/2}.$

The final result obtained from iterative calculations of the attitude is expressed as

$q=\sum _{i=0}^{m_q}b_iF_i\left(\tau \right),$

which is used as the input for the subsequent velocity and position calculation.

Velocity/position iterative solution step: The Chebyshev polynomial coefficients of the velocity/position is iteratively calculated by using the Chebyshev polynomial coefficients of the specific force obtained in the fitting step, the Chebyshev polynomial coefficients of the attitude quaternion obtained in the attitude iterative solution step and an integral equation of the velocity/position, and polynomial truncation is performed on a result obtained from each iterative calculation according to a preset order.

According to the Chebyshev polynomial of the specific force and the Chebyshev polynomial of the attitude quaternion, the integral of the specific force transformation is expressed as follows:

$\begin{array}{cc}{I}_f\left(\tau \right)=\frac{1}{4}\sum _{i=0}^{m_q}\sum _{j=0}^{n_f}\sum _{k=0}^{m_q}b_i\circ d_j\circ b_k^{*}\left(G_{i+j+k,\left[\begin{array}{cc}-1& \tau \end{array}\right]}+G_{i+j-k,\left[\begin{array}{cc}-1& \tau \end{array}\right]}+G_{i+j-k,\left[\begin{array}{cc}-1& \tau \end{array}\right]}+G_{i-j-k,\left[\begin{array}{cc}-1& \tau \end{array}\right]}\right).& \left(9\right)\end{array}$

Assuming that in the l^th iterative calculation, the Chebyshev polynomial of the velocity/position is expressed as follows:

$\begin{array}{cc}{v}_l^e=\sum _{i=0}^{m_v}s_{l,i}F_i\left(\tau \right),p_l^e=\sum _{i=0}^{m_p}{\rho }_{l,i}F_i\left(\tau \right);& \left(10\right)\end{array}$

wherein m_v and m_p represent the predetermined truncation order of the velocity and the predetermined truncation order of the position, respectively; s_l,i and ρ_l,i represent a coefficient of the i^th Chebyshev polynomial of the velocity and a coefficient of the i^th Chebyshev polynomial of the position in the l^th iterative calculation, respectively. When l=0, v₀^e(t)≡v^e(0), p₀^e(t)≡p^e(0), wherein v^e(0) and p^e(0) represent an initial velocity and an initial position, respectively.

Considering that the gravity model is generally given in the geographic coordinate system and gravity is a position-correlated nonlinear function, the gravity vector in the earth coordinate system is approximated by the Chebyshev polynomials as follows:

$\begin{array}{cc}{g}^e\left(p_l^e\right)=C_n^eg^n\left(\mathrm{ecef}2\mathrm{lla}\left(\sum _{i=0}^{m_p}{\rho }_{l,i}F_i\left(\tau \right)\right)\right)\approx \sum _{i=0}^{m_g}{\gamma }_{l,i}F_i\left(\tau \right);& \left(11\right)\end{array}$

wherein C_n^e represents an attitude matrix (also a position-correlated nonlinear function) of the earth coordinate system relative to the geographic coordinate system, m_g represents the maximum order of the Chebyshev polynomial of gravity, and ecef2lla(⋅) represents a transformation from rectangular coordinates of the earth to geographic coordinates (longitude, latitude and altitude). The Chebyshev polynomial coefficient γ_l,i of the gravity vector is calculated as follows:

$\begin{array}{cc}{\gamma }_{l,i}\approx \frac{2-{\delta }_{0i}}{P}\sum _{k=0}^{P-1}\cos \frac{i\left(k+1/2\right)\pi }{P}C_n^eg^n\left(\mathrm{ecef}2\mathrm{lla}\left(\sum _{i=0}^{m_p}{\rho }_{l,i}F_i\left(\cos \frac{\left(k+1/2\right)\pi }{P}\right)\right)\right);& \left(12\right)\end{array}$

wherein δ_0i represents the Kronecker delta function (when i=1, output 1; otherwise, output 0). The accuracy of approximation of gravity increases as the number P of terms increases.

Formulas (9) and (12) are substituted to iteratively calculate the Chebyshev polynomial coefficients of the velocity/position as follows:

$\begin{array}{cc}\begin{array}{c}{v}_{l+1}^e=v^e\left(0\right)+\frac{t_N}{2}\left(I_f-2\sum _{i=0}^{m_v}{\omega }_e\times s_{l,i}G_{i,\left[\begin{array}{cc}-1& \tau \end{array}\right]}+\sum _{i=0}^{m_g}{\gamma }_{l,i}G_{i,\left[\begin{array}{cc}-1& \tau \end{array}\right]}\right)\\ =\sum _{i=0}^{\mathrm{ma}x\left\{2m_q+n_f,m_v,m_g\right\}+1}s_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{m_v}s_{l+1,i}F_i\left(\tau \right)\end{array}\begin{array}{c}p_{l+1}^e=p^e\left(0\right)+\frac{t_N}{2}\sum _{i=0}^{m_v}s_{l,i}G_{i,\left[\begin{array}{cc}-1& \tau \end{array}\right]}\\ =\sum _{i=0}^{m_v+1}{\rho }_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{m_p}{\rho }_{l+1,i}F_i\left(\tau \right),\end{array}& \left(13\right)\end{array}$

until the convergence condition is satisfied or the preset maximum number of iterative calculations is reached.

The sequence of the velocity and the position in formula (13) can be reversed, and the velocity (position) result obtained from the same iterative calculation can be directly used in the subsequent position (velocity) calculation. For example, formula (13) can be calculated in the following sequence:

$\begin{array}{cc}\begin{array}{c}p_{l+1}^e=p^e\left(0\right)+\frac{t_N}{2}\sum _{i=0}^{m_v}s_{l,i}G_{i,\left[\begin{array}{cc}-1& \tau \end{array}\right]}\\ =\sum _{i=0}^{m_v+1}{\rho }_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{m_p}{\rho }_{l+1,i}F_i\left(\tau \right),\end{array}\mathrm{and}& \left(14\right)\\ \begin{array}{c}{v}_{l+1}^e=v^e\left(0\right)+\frac{t_N}{2}\left(I_f-2\sum _{i=0}^{m_v}{\omega }_e\times s_{l,i}G_{i,\left[\begin{array}{cc}-1& \tau \end{array}\right]}+\sum _{i=0}^{m_g}{\gamma }_{l,i}G_{i,\left[\begin{array}{cc}-1& \tau \end{array}\right]}\right)\\ =\sum _{i=0}^{\mathrm{ma}x\left\{2m_q+n_f,m_v,m_g\right\}+1}s_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{m_v}s_{l+1,i}F_i\left(\tau \right),\end{array}& \left(15\right)\end{array}$

It should be noted that the gravity vector of formula (15) approximately uses position p_l+1^e (corresponding to γ_l+1,i) calculated by formula (14) instead of p_l^e in the previous iterative calculation.

Attitude/velocity/position acquisition step: The attitude/velocity/position information on the corresponding time interval is determined according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

With respect to the inertial navigation solution on a long time interval, the time interval can be divided into a plurality of sub-time intervals to perform sequential calculations.

Navigation step: Navigation is performed according to the attitude/velocity/position information on the corresponding time interval.

According to the above-mentioned functional iterative integration-based method for inertial navigation solution, the present invention further provides a functional iterative integration-based system for inertial navigation solution, which includes:

a fitting module, configured to fit a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval;

an attitude iterative solution module, configured to iteratively calculate Chebyshev polynomial coefficients of an attitude quaternion by using the obtained Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and perform polynomial truncation on a result obtained from each iterative calculation according to a preset order;

a velocity/position iterative solution module, configured to iteratively calculate Chebyshev polynomial coefficients of a velocity/position by using the obtained Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and perform polynomial truncation on a result obtained from each iterative calculation according to a preset order; and

an attitude/velocity/position acquisition module, configured to obtain attitude/velocity/position information on the corresponding time interval according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

Those skilled in the art shall understand that, in addition to implementing the system provided by the present invention and its various devices, modules, and units in a pure computer-readable program code manner, they may also be implemented to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, embedded microcontrollers, and the like, by performing logic programming on the steps of the method. Therefore, the system provided by the present invention and its various devices, modules and units can be regarded as hardware components, and the devices, modules, and units included in the system for implementing various functions can also be regarded as structures within the hardware components. Besides, the devices, modules, and units for realizing various functions can also be regarded as not only software modules but also the structures within the hardware components for implementing the method.

The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the foregoing specific embodiments. Those skilled in the art can make various changes or modifications within the scope of the claims without affecting the essence of the present invention. When not in conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A functional iterative integration-based method for an inertial navigation solution, comprising: a fitting step: fitting a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval to obtain Chebyshev polynomial coefficients of the angular velocity and Chebyshev polynomial coefficients of the specific force;an attitude iterative solution step: iteratively calculating Chebyshev polynomial coefficients of an attitude quaternion by using the Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and performing polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the attitude quaternion according to a first preset order;a velocity/position iterative solution step: iteratively calculating Chebyshev polynomial coefficients of a velocity/position by using the Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and performing polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the velocity/position according to a second preset order; andan attitude/velocity/position acquisition step: obtaining attitude/velocity/position information on the time interval according to the Chebyshev polynomial coefficients of the velocity/position, the Chebyshev polynomial coefficients of the attitude quaternion and a Chebyshev polynomial corresponding to the velocity/position and the attitude quaternion.

2. The functional iterative integration-based method according to claim 1, wherein, the gyroscope-measured values comprise a measured value of the angular velocity or a measured value of an angular increment, and the accelerometer-measured values comprise a measured value of the specific force or a measured value of a velocity increment.

3. The functional iterative integration-based method according to claim 1, wherein, in the fitting step, the time interval is divided into at least two sub-time intervals to perform sequential calculations.

4. The functional iterative integration-based method according to claim 1, wherein, in the attitude iterative solution step or the velocity/position iterative solution step, the Chebyshev polynomial coefficients of the attitude quaternion or the Chebyshev polynomial coefficients of the velocity/position is iteratively calculated until a maximum number of iterative calculations is reached or a predetermined convergence condition is satisfied.

5. The functional iterative integration-based method according to claim 1, further comprising: a navigation step: performing a navigation according to the attitude/velocity/position information on the time interval.

6. A functional iterative integration-based system for an inertial navigation solution, comprising: a fitting module, wherein the fitting module is configured to fit a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval to obtain Chebyshev polynomial coefficients of the angular velocity and Chebyshev polynomial coefficients of the specific force;an attitude iterative solution module, wherein the attitude iterative solution module is configured to iteratively calculate Chebyshev polynomial coefficients of an attitude quaternion by using the Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and perform polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the attitude quaternion according to a first preset order;a velocity/position iterative solution module, wherein the velocity/position iterative solution module is configured to iteratively calculate Chebyshev polynomial coefficients of a velocity/position by using the Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and perform polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the velocity/position according to a second preset order; andan attitude/velocity/position acquisition module, wherein the attitude/velocity/position acquisition module is configured to obtain attitude/velocity/position information on the time interval according to the Chebyshev polynomial coefficients of the velocity/position, the Chebyshev polynomial coefficients of the attitude quaternion and a Chebyshev polynomial corresponding to the velocity/position and the attitude quaternion.

7. The functional iterative integration-based system according to claim 6, wherein, the gyroscope-measured values comprise a measured value of the angular velocity or a measured value of an angular increment, and the accelerometer-measured values comprise a measured value of the specific force or a measured value of a velocity increment.

8. The functional iterative integration-based system according to claim 6, wherein, the fitting module divides the time interval into at least two sub-time intervals to perform sequential calculations.

9. The functional iterative integration-based system according to claim 6, wherein, the attitude iterative solution module and the velocity/position iterative solution module perform the each iterative calculation until a maximum number of iterative calculations is reached or a predetermined convergence condition is satisfied.

10. The functional iterative integration-based system according to claim 6, further comprising: a navigation module, wherein the navigation module is configured to perform a navigation according to the attitude/velocity/position information on the time interval.