METHOD AND SYSTEM FOR SOLVING RIGID BODY ATTITUDE BASED ON FUNCTIONAL ITERATIVE INTEGRATION

Abstract

A method for solving rigid body attitude based on functional iterative integration includes: fitting a Chebyshev polynomial function for an angular velocity according to gyro measurement values in a time interval; iteratively calculating a Chebyshev polynomial coefficient for a Rodrigues vector by using the obtained Chebyshev polynomial coefficient for the angular velocity and a Rodrigues vector integral equation, and performing polynomial truncation on the result at each iterative calculation according to a preset order; and calculating the Rodrigues vector according to the Chebyshev polynomial coefficient for the Rodrigues vector and the corresponding Chebyshev polynomial, so as to obtain attitude profile in the time interval in a form of quaternion. A system for solving rigid body attitude based on functional iterative integration includes a fitting module, an iterating module, and an attitude computation module.

Description

TECHNICAL FIELD

The present invention relates to the field of test and measurement technology, and more particularly, to a method and system for solving rigid body attitude based on functional iterative integration.

BACKGROUND

Calculation or estimation of rigid body motion in three-dimensional space is a core issue in many fields such as physics, robotic, navigation, machinery, computer vision and so on. Unlike the translational motion such as velocity and position, attitude cannot be measured directly and can only be obtained by indirect methods such as angular velocity integration or vector matching. Attitude calculation by angular velocity integration is completely autonomous, which does not need assistance from external information, so it is preferable in many applications, such as those where satellite navigation systems cannot work.

Recently, researchers in the art have proposed several attitude computation methods with high accuracy. The Chinese patent application No. 201710273489.3 proposes a method for solving rigid body attitude based on functional iterative integration, which includes: fitting a polynomial function for an angular velocity based on gyro measurement values in a time interval; iteratively calculating a Rodrigues vector by using the fitted polynomial function for the angular velocity and a Rodrigues vector integral equation; and then obtaining attitude profile in the time interval in a form of quaternion according to the iteration result. The method has an advantage of high calculation accuracy, but it does not make full use of characteristics of Chebyshev polynomial in the iterative process, and the order of the polynomial for the Rodrigues vector increases rapidly as the iterative process advances and thus the amount of calculation is huge, which is difficult to fulfill the real-time applications. For example, in a case that the polynomial for the angular velocity is fitted by using eight gyro measurement values, the order of the polynomial for the Rodrigues vector would be more than one thousand at the 7^th iteration. In fact, due to errors in angular velocity measurement, it is unnecessary to adopt such a high-order polynomial for the Rodrigues vector.

SUMMARY

To solve the above technical problems in the prior art, the present invention aims to provide a method and system for solving rigid body attitude based on functional iterative integration.

The present invention provides a method for solving rigid body attitude based on functional iterative integration, including:

fitting step: fitting a Chebyshev polynomial function for an angular velocity according to gyro measurement values in a time interval;

iteration step: iteratively calculating a Chebyshev polynomial coefficient for a Rodrigues vector by using the obtained Chebyshev polynomial coefficient for the angular velocity and a Rodrigues vector integral equation, and performing polynomial truncation on a result at each iterative calculation according to a preset order; and

attitude computation step: calculating the Rodrigues vector according to the Chebyshev polynomial coefficient for the Rodrigues vector and a corresponding Chebyshev polynomial, so as to obtain attitude profile in the time interval in a form of quaternion.

Preferably, the gyro measurement values include angular velocity measurement values or angular increment measurement values.

Preferably, the fitting step specifically includes:

with respect to N angular velocity measurement values {tilde over (ω)}_tk or angular increment measurement values Δ{tilde over (θ)}_tk at time t_k, k=1, 2, . . . N, let

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

mapping an original time interval to [−1 1]; and approximately fitting the angular velocity with a Chebyshev polynomial of an order of no greater than N−1:

$\hat{\omega }=\sum _{i=0}^nc_iF_i\left(\tau \right),n\le N-1,$

where, n denotes the order of the Chebyshev polynomial for the angular velocity, c_i denotes a coefficient vector of an i^th order Chebyshev polynomial, F_i(τ) denotes an i^th order Chebyshev polynomial of a first type, and τ denotes a mapped time variable.

Preferably, the iterating step specifically includes:

at an l^th iterative calculation, adopting the following equation to calculate the Chebyshev polynomial for the Rodrigues vector:

${\hat{g}}_l=\sum _{i=0}^{n_T}b_{l,i}F_i\left(\tau \right),$

where, n_T is a preset truncation order, b_l,i is a coefficient of the i^th order Chebyshev polynomial at the l^th iterative calculation, and g_l=0 when l=0.

Preferably, the Chebyshev polynomial coefficient for the Rodrigues vector is iteratively calculated as follows:

${\hat{g}}_{l+1}=\frac{t_N}{2}\left(\begin{array}{c}\sum _{i=0}^nc_iG_{i,\left[-1\tau \right]}+\frac{1}{4}\sum _{i=0}^{n_T}\sum _{j=0}^nb_{l,i}\times c_j\left(G_{i+j,\left[-1\tau \right]}+G_{i-j,\left[-1\tau \right]}\right)+\\ \frac{1}{16}\sum _{i=0}^{n_T}\sum _{j=0}^{n_T}\sum _{k=0}^nb_{l,i}b_{l,j}^Tc_k\left(G_{i+j+k,\left[-1\tau \right]}+G_{i+j-k,\left[-1\tau \right]}+G_{i-j+k,\left[-1\tau \right]}+G_{i-j-k,\left[-1\tau \right]}\right)\end{array}\right),\stackrel{\Delta }{=}\sum _{i=0}^{2n_T+n+1}b_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{n_T}b_{l+1,i}F_i\left(\tau \right)$

iterative calculation is performed until a convergence condition is satisfied or a preset maximum number of iterations is reached; as an approximation accuracy of the polynomial for the angular velocity does not exceed the order of N−1, a truncation order is set as n_T≥N.

A system for solving rigid body attitude based on functional iterative integration according to the present invention, including:

a fitting module, wherein the fitting module is configured to fit a Chebyshev polynomial function for an angular velocity according to gyro measurement values in a time interval;

an iterating module, wherein the iterating module is configured to iteratively calculate a Chebyshev polynomial coefficient for a Rodrigues vector by using the obtained Chebyshev polynomial coefficient for the angular velocity and a Rodrigues vector integral equation, and perform polynomial truncation on a result at each iterative calculation according to a preset order; and

an attitude computation module, wherein the attitude computation module is configured to calculate the Rodrigues vector according to the Chebyshev polynomial coefficient for the Rodrigues vector and a corresponding Chebyshev polynomial, so as to obtain attitude profile in the time interval in a form of quaternion.

Preferably, the gyro measurement values include angular velocity measurement values or angular increment measurement values.

Preferably, with respect to N angular velocity measurement values {tilde over (ω)}_tk or angular increment measurement values Δ{tilde over (θ)}_tk at time t_k, k=1, 2, . . . N, let

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

the fitting module is specifically configured to map an original time interval to [−1 1] and approximately fit the angular velocity with a Chebyshev polynomial of an order of no greater than N−1:

$\hat{\omega }=\sum _{i=0}^nc_iF_i\left(\tau \right),n\le N-1,$

where, n denotes the order of the Chebyshev polynomial for the angular velocity, c_i denotes a coefficient vector of an i^th order Chebyshev polynomial, F_i(τ) denotes an i^th order Chebyshev polynomial of a first type, and τ denotes a mapped time variable.

Preferably, the iteration module is specifically configured to adopt the following equation to calculate the Chebyshev polynomial for the Rodrigues vector at an l^th iterative calculation:

${\hat{g}}_l=\sum _{i=0}^{n_T}b_{l,i}F_i\left(\tau \right),$

where, n_T denotes a preset truncation order, b_l,i denotes a coefficient of the i^th order Chebyshev polynomial at the l^th iterative calculation, and g_l=0 when l=0.

Preferably, the Chebyshev polynomial coefficient for the Rodrigues vector is iteratively calculated as follows:

${\hat{g}}_{l+1}=\frac{t_N}{2}\left(\begin{array}{c}\sum _{i=0}^nc_iG_{i,\left[-1\tau \right]}+\frac{1}{4}\sum _{i=0}^{n_T}\sum _{j=0}^nb_{l,i}\times c_j\left(G_{i+j,\left[-1\tau \right]}+G_{i-j,\left[-1\tau \right]}\right)+\\ \frac{1}{16}\sum _{i=0}^{n_T}\sum _{j=0}^{n_T}\sum _{k=0}^nb_{l,i}b_{l,j}^Tc_k\left(G_{i+j+k,\left[-1\tau \right]}+G_{i+j-k,\left[-1\tau \right]}+G_{i-j+k,\left[-1\tau \right]}+G_{i-j-k,\left[-1\tau \right]}\right)\end{array}\right),\stackrel{\Delta }{=}\sum _{i=0}^{2n_T+n+1}b_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{n_T}b_{l+1,i}F_i\left(\tau \right)$

iterative calculation is performed until a convergence condition is satisfied or a preset maximum number of iterations is reached; as an approximation accuracy of the polynomial for the angular velocity does not exceed the order of N−1, a truncation order is set as n_T≥N.

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

The present invention is provided based on the technique of function iterative integration, which uses the Rodrigues vector to realize fast attitude reconstruction from gyro measurement. Attitude reconstruction based on gyro measurement adopts the Chebyshev polynomial with good numerical characteristics, transforms the iterative integration of Rodrigues vector into the iterative calculation of the corresponding Chebyshev polynomial coefficients, and then adopts an order truncation method to increase the calculation speed without significantly reducing the calculation accuracy.

BRIEF DESCRIPTION OF THE DRAWING

Other features, objects, and advantages of the present invention will become more apparent, upon reading the detailed description of the non-limiting embodiments with reference to the following drawings.

FIGURE is a flowchart illustrating the present invention.

DETAILED DESCRIPTION OF THE EMBODIMENTS

The present invention will be described in detail hereinafter with reference to specific embodiments. The following embodiments are described for facilitating those skilled in the art to further understand the present invention, but do not intend to limit the present invention in any way. It should be noted that, for those having ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present invention, and such variations and improvements shall fall within the scope of protection of the present invention.

As shown in FIGURE, the method for solving rigid body attitude based on functional iterative integration includes:

1) fitting step: a Chebyshev polynomial function for an angular velocity is fitted according to gyro measurement values in a time interval;

2) iterating step: a Chebyshev polynomial coefficient for a Rodrigues vector is iteratively calculated by using the obtained Chebyshev polynomial coefficient for the angular velocity and a Rodrigues vector integral equation, and polynomial truncation is performed on the result obtained at each iterative calculation according to a preset order; and

3) attitude computation step: the Rodrigues vector is calculated according to the Chebyshev polynomial coefficient for the Rodrigues vector and the corresponding Chebyshev polynomial, so as to obtain attitude profile in the time interval in a form of quaternion.

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

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

where, F_i(x) denotes the i^th order Chebyshev polynomial of the first type.

In step 1), the Chebyshev polynomial function for the angular velocity is fitted according to gyro measurement values in the time interval.

With respect to N angular velocity measurement values {tilde over (ω)}_tk or angular increment measurement values Δ{tilde over (θ)}_tk at time t_k, k=1, 2, . . . N, let

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

the original time interval is mapped to [−1 1]. The angular velocity is approximately fitted by a Chebyshev polynomial with an order of no greater than N−1:

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

where, n denotes the order of the Chebyshev polynomial for the angular velocity, c_i denotes a coefficient vector of the i^th order Chebyshev polynomial, F_i(τ) denotes the i^th order Chebyshev polynomial of the first type, and τ denotes the mapped time variable.

In case of angular velocity measurement, the coefficient c_i is determined by solving the following equation:

$\begin{array}{cc}{A}_{\omega }\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]\stackrel{\Delta }{=}\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[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=\left[\begin{array}{c}{\hat{\omega }}_{t_1}^T\\ {\hat{\omega }}_{t_2}^T\\ ⋮\\ {\hat{\omega }}_{t_N}^T\end{array}\right],\hspace{1em}\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=A_{\omega }^{-1}\left[\begin{array}{c}{\hat{\omega }}_{t_1}^T\\ {\hat{\omega }}_{t_2}^T\\ ⋮\\ {\hat{\omega }}_{t_N}^T\end{array}\right]& \left(2\right)\end{array}$

In case of angular increment measurement, the coefficient c_i is determined by solving the following equation:

$\begin{array}{cc}{A}_{\theta }\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]\stackrel{\Delta }{=}\left[\begin{array}{cccc}{G}_{0,\left[{\tau }_0{\tau }_1\right]}& G_{1,\left[{\tau }_0{\tau }_1\right]}& \dots & G_{n,\left[{\tau }_0{\tau }_1\right]}\\ G_{0,\left[{\tau }_1{\tau }_2\right]}& G_{1,\left[{\tau }_1{\tau }_2\right]}& \dots & G_{n,\left[{\tau }_1{\tau }_2\right]}\\ ⋮& ⋮& ⋮& ⋮\\ G_{0,\left[{\tau }_{N-1}{\tau }_N\right]}& G_{1,\left[{\tau }_{N-1}{\tau }_N\right]}& \dots & G_{n,\left[{\tau }_{N-1}{\tau }_N\right]}\end{array}\right]\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=\frac{2}{t_N}\left[\begin{array}{c}\Delta {\tilde{\theta }}_{t_1}^T\\ \Delta {\tilde{\theta }}_{t_2}^T\\ ⋮\\ \Delta {\tilde{\theta }}_{t_N}^T\end{array}\right],\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=\frac{2}{t_N}A_{\theta }^{-1}\left[\begin{array}{c}\Delta {\tilde{\theta }}_{t_1}^T\\ \Delta {\tilde{\theta }}_{t_2}^T\\ ⋮\\ \Delta {\tilde{\theta }}_{t_N}^T\end{array}\right]& \left(3\right)\end{array}$

where, T denotes the operation of vector transpose or matrix transpose, and the function G_{i,[τk−1τk]} is defined as follows:

$\begin{array}{cc}{G}_{i,\left[{\tau }_{k-1}{\tau }_k\right]}=\{\begin{array}{cc}\left(\frac{{\mathrm{iF}}_{i+1}\left({\tau }_k\right)}{i^2-1}-\frac{{\tau }_kF_i\left({\tau }_k\right)}{i-1}\right)-\left(\frac{{\mathrm{iF}}_{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(4\right)\end{array}$

In step 2), the Chebyshev polynomial coefficient for the Rodrigues vector is iteratively calculated by using the obtained Chebyshev polynomial coefficient for the angular velocity and the Rodrigues vector integral equation, and polynomial truncation is performed on the result obtained at each iterative calculation according to a preset truncation order.

At the l^th iterative calculation, the Chebyshev polynomial for the Rodrigues vector is expressed as follows:

$\begin{array}{cc}{\hat{g}}_l=\sum _{i=0}^{n_T}b_{l,i}F_i\left(\tau \right)& \left(5\right)\end{array}$

where, n_T denotes a preset truncation order, b_i,j denotes a coefficient of the i^th order Chebyshev polynomial at the l^th iterative calculation, and g_l=0 when l=0. The Chebyshev polynomial coefficient for the Rodrigues vector is iteratively calculated as follows:

$\begin{array}{cc}{\hat{g}}_{l+1}=\frac{t_N}{2}\left(\begin{array}{c}\sum _{i=0}^nc_iG_{i,\left[-1\tau \right]}+\frac{1}{4}\sum _{i=0}^{n_T}\sum _{j=0}^nb_{l,i}\times c_j\left(G_{i+j,\left[-1\tau \right]}+G_{i-j,\left[-1\tau \right]}\right)+\\ \frac{1}{16}\sum _{i=0}^{n_T}\sum _{j=0}^{n_T}\sum _{k=0}^nb_{l,i}b_{l,j}^Tc_k\left(G_{i+j+k,\left[-1\tau \right]}+G_{i+j-k,\left[-1\tau \right]}+G_{i-j+k,\left[-1\tau \right]}+G_{i-j-k,\left[-1\tau \right]}\right)\end{array}\right)\stackrel{\Delta }{=}\sum _{i=0}^{2n_T+n+1}b_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{n_T}b_{l+1,i}F_i\left(\tau \right)& \left(6\right)\end{array}$

The iterative calculation is performed until a convergence condition is satisfied or a preset maximum number of iterations is reached. In the above equation, × denotes the vector cross multiplication. According to equation (1), as the approximation accuracy of the polynomial for the angular velocity does not exceed the order of N−1, the truncation order is set as n_T≥N.

In step 3), the Rodrigues vector is calculated according to the Chebyshev polynomial coefficient for the Rodrigues vector and the corresponding Chebyshev polynomial, so as to obtain the attitude quaternion with respect to the start of the time interval.

The Rodrigues vector is calculated with reference to equation (5) according to the Chebyshev polynomial coefficient for the Rodrigues vector and the corresponding Chebyshev polynomial, so as to obtain the attitude quaternion with respect to the start of the time interval.

$\begin{array}{cc}q=\frac{2+g}{\sqrt{4+{g}^2}}& \left(7\right)\end{array}$

As for attitude calculation in a large time interval, the large time interval may be divided into several small time intervals and the calculation may be performed for such small time intervals sequentially.

In principle, if a certain accuracy loss can be accepted, the method of fast attitude computation presented in the present invention is also applicable to other three-dimensional attitude parameters, such as the rotation vector. In this case, it is necessary to make corresponding adjustments to the equation (6) in step 2) and equation (7) in step 3) as follows.

In step 2), the Chebyshev polynomial coefficient for the rotation vector can be iteratively calculated as follows:

$\begin{array}{cc}{\hat{g}}_{l+1}=\frac{t_N}{2}\left(\begin{array}{c}\sum _{i=0}^nc_iG_{i,\left[-1\tau \right]}+\frac{1}{2}\sum _{i=0}^{n_T}\sum _{j=0}^nb_{l,i}\times c_j\left(G_{i+j,\left[-1\tau \right]}+G_{i-j,\left[-1\tau \right]}\right)+\\ \frac{1}{12}\sum _{i=0}^{n_T}\sum _{j=0}^{n_T}\sum _{k=0}^n\left[b_{l,i}\times \left(b_{l,j}\times c_k\right)\right]\left(G_{i+j+k,\left[-1\tau \right]}+G_{i+j-k,\left[-1\tau \right]}+G_{i-j+k,\left[-1\tau \right]}+G_{i-j-k,\left[-1\tau \right]}\right)\end{array}\right)\stackrel{\Delta }{=}\sum _{i=0}^{2n_T+n+1}b_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{n_T}b_{l+1,i}F_i\left(\tau \right)& \left(8\right)\end{array}$

In step 3), the rotation vector is calculated according to the Chebyshev polynomial coefficient for the rotation vector and the corresponding Chebyshev polynomial, so as to obtain an attitude quaternion with respect to the start of the time interval.

$\begin{array}{cc}q=\cos \frac{g}{2}+\frac{g}{g}\sin \frac{g}{2}& \left(9\right)\end{array}$

In addition to the rigid body attitude computation based on functional iterative integration, the present invention further provides a system for solving rigid body attitude based on functional iterative integration, including:

a fitting module, wherein the fitting module is configured to fit a Chebyshev polynomial function for an angular velocity according to gyro measurement values in a time interval;

an iterating module, wherein the iterating module is configured to iteratively calculate a Chebyshev polynomial coefficient for a Rodrigues vector by using the obtained Chebyshev polynomial coefficient for the angular velocity and a Rodrigues vector integral equation, and perform polynomial truncation on the result obtained at each iterative calculation according to a preset order; and

an attitude computation module, wherein the attitude computation module is configured to calculate the Rodrigues vector according to the Chebyshev polynomial coefficient for the Rodrigues vector and the corresponding Chebyshev polynomial, so as to obtain attitude profile in the time interval in a form of quaternion.

Specifically, with respect to N angular velocity measurement values {tilde over (ω)}_tk or angular increment measurement values Δ{tilde over (θ)}_tk at time t_k, k=1, 2, . . . N, let

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

the fitting module is configured to map the original time interval to [−1 1] and approximately fit the angular velocity with a Chebyshev polynomial of an order of no greater than N−1:

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

where, n denotes the order of the Chebyshev polynomial for the angular velocity, c_i denotes the coefficient vector of the i^th order Chebyshev polynomial, F_i(τ) denotes an order Chebyshev polynomial of the first type, and τ denotes the mapped time variable.

In case of angular velocity measurement, the coefficient c_i is determined by solving the following equation:

$\begin{array}{cc}{A}_{\omega }\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]\stackrel{\Delta }{=}\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[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=\left[\begin{array}{c}{\hat{\omega }}_{t_1}^T\\ {\hat{\omega }}_{t_2}^T\\ ⋮\\ {\hat{\omega }}_{t_N}^T\end{array}\right],\hspace{1em}\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=A_{\omega }^{-1}\left[\begin{array}{c}{\hat{\omega }}_{t_1}^T\\ {\hat{\omega }}_{t_2}^T\\ ⋮\\ {\hat{\omega }}_{t_N}^T\end{array}\right]& \left(11\right)\end{array}$

In case of angular increment measurement, the coefficient c_i is determined by solving the following equation:

$\begin{array}{cc}{A}_{\theta }\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]\stackrel{\Delta }{=}\left[\begin{array}{cccc}{G}_{0,\left[{\tau }_0{\tau }_1\right]}& G_{1,\left[{\tau }_0{\tau }_1\right]}& \dots & G_{n,\left[{\tau }_0{\tau }_1\right]}\\ G_{0,\left[{\tau }_1{\tau }_2\right]}& G_{1,\left[{\tau }_1{\tau }_2\right]}& \dots & G_{n,\left[{\tau }_1{\tau }_2\right]}\\ ⋮& ⋮& ⋮& ⋮\\ G_{0,\left[{\tau }_{N-1}{\tau }_N\right]}& G_{1,\left[{\tau }_{N-1}{\tau }_N\right]}& \dots & G_{n,\left[{\tau }_{N-1}{\tau }_N\right]}\end{array}\right]\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=\frac{2}{t_N}\left[\begin{array}{c}\Delta {\tilde{\theta }}_{t_1}^T\\ \Delta {\tilde{\theta }}_{t_2}^T\\ ⋮\\ \Delta {\tilde{\theta }}_{t_N}^T\end{array}\right],\left[\begin{array}{c}{c}_0^T\\ c_1^T\\ ⋮\\ c_n^T\end{array}\right]=\frac{2}{t_N}A_{\theta }^{-1}\left[\begin{array}{c}\Delta {\tilde{\theta }}_{t_1}^T\\ \Delta {\tilde{\theta }}_{t_2}^T\\ ⋮\\ \Delta {\tilde{\theta }}_{t_N}^T\end{array}\right]& \left(12\right)\end{array}$

where, T denotes the operation of vector transpose or matrix transpose, and the function G_{i,[τk−1τk]} is defined as follows:

$\begin{array}{cc}{G}_{i,\left[{\tau }_{k-1}{\tau }_k\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(13\right)\end{array}$

Specifically, the iterating module iteratively calculates the Chebyshev polynomial for Rodrigues vector as follows.

At the l^th iterative calculation, the Chebyshev polynomial for the Rodrigues vector is expressed as follows:

$\begin{array}{cc}{\hat{g}}_l=\sum _{i=0}^{n_T}b_{l,i}F_i\left(\tau \right)& \left(14\right)\end{array}$

where, n_T denotes a preset truncation order, b_l,i denotes a coefficient of the i^th order Chebyshev polynomial at the l^th iterative calculation, and g_l=0 when l=0.

The Chebyshev polynomial coefficient for the Rodrigues vector is iteratively calculated as follows:

$\begin{array}{cc}\begin{array}{c}{\hat{g}}_{l+1}=\frac{t_N}{2}\left(\begin{array}{c}\sum _{i=0}^nc_iG_{i,\left[-1\tau \right]}+\frac{1}{4}\sum _{i=0}^{n_T}\sum _{j=0}^nb_{l,i}\times c_j\left(G_{i+j,\left[-1\tau \right]}+G_{i-j,\left[-1\tau \right]}\right)+\\ \frac{1}{16}\sum _{i=0}^{n_T}\sum _{j=0}^{n_T}\sum _{k=0}^nb_{l,i}b_{l,j}^Tc_k\left(G_{i+j+k,\left[-1\tau \right]}+G_{i+j-k,\left[-1\tau \right]}+G_{i-j+k,\left[-1\tau \right]}+G_{i-j-k,\left[-1\tau \right]}\right)\end{array}\right)\\ \stackrel{△}{=}\sum _{i=0}^{2n_T+n+1}b_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{n_T}b_{l+1,i}F_i\left(\tau \right)\end{array}& \left(15\right)\end{array}$

The iterative calculation is performed until a convergence condition is satisfied or a preset maximum number of iterations is reached. In the above equation, × denotes the vector cross multiplication. According to equation (10), as the approximation accuracy of the polynomial for the angular velocity does not exceed an order of N−1, the truncation order is set as n_T≥N.

The attitude computation module is configured to calculate the Rodrigues vector according to the Chebyshev polynomial coefficient for the Rodrigues vector and the corresponding Chebyshev polynomial, so as to obtain the attitude quaternion with respect to the start of the time interval.

The Rodrigues vector is calculated with reference to equation (14) according to the Chebyshev polynomial coefficient for the Rodrigues vector and the corresponding Chebyshev polynomial, so as to obtain the attitude quaternion with respect to the start of the time interval.

$\begin{array}{cc}q=\frac{2+g}{\sqrt{4+{g}^2}}& \left(16\right)\end{array}$

As for attitude calculation in a large time interval, the large time interval may be divided into several small time intervals and the calculation may be performed for such small time intervals sequentially.

In principle, if a certain accuracy loss can be accepted, the system of fast attitude computation presented in the present invention is also applicable to other three-dimensional attitude parameters, such as the rotation vector. In this case, it is necessary to make corresponding adjustments to the equations (15) and (16) as follows.

The iterating module iteratively calculates the Chebyshev polynomial coefficients for the rotation vector as follows:

$\begin{array}{cc}\begin{array}{c}{\hat{g}}_{l+1}=\frac{t_N}{2}\left(\begin{array}{c}\sum _{i=0}^nc_iG_{i,\left[-1\tau \right]}+\frac{1}{2}\sum _{i=0}^{n_T}\sum _{j=0}^nb_{l,i}\times c_j\left(G_{i+j,\left[-1\tau \right]}+G_{i-j,\left[-1\tau \right]}\right)+\\ \frac{1}{12}\sum _{i=0}^{n_T}\sum _{j=0}^{n_T}\sum _{k=0}^n\left[b_{l,i}\times \left(b_{l,j}\times c_k\right)\right]\left(G_{i+j+k,\left[-1\tau \right]}+G_{i+j-k,\left[-1\tau \right]}+G_{i-j+k,\left[-1\tau \right]}+G_{i-j-k,\left[-1\tau \right]}\right)\end{array}\right)\\ \stackrel{△}{=}\sum _{i=0}^{2n_T+n+1}b_{l+1,i}F_i\left(\tau \right)\stackrel{\mathrm{polynomial}\mathrm{truncation}}{\approx }\sum _{i=0}^{n_T}b_{l+1,i}F_i\left(\tau \right)\end{array}& \left(17\right)\end{array}$

The attitude computation module calculates the rotation vector according to the Chebyshev polynomial coefficient for the rotation vector and the corresponding Chebyshev polynomial, so as to obtain an attitude quaternion with respect to the start of the time interval.

$\begin{array}{cc}q=\cos \frac{g}{2}+\frac{g}{g}\sin \frac{g}{2}& \left(18\right)\end{array}$

Those skilled in the art shall understand that, in addition to implementing the system and its various devices, modules and units according to the present invention in a pure computer-readable program code, they may also be implemented to realize the same functions in 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 and its various devices, modules and units provided by the present invention may be regarded as hardware components, and the devices, modules and units included therein for realizing various functions can also be regarded as structures within the hardware component. Besides, the devices, the modules and the units for realizing various functions can also be regarded as software modules, or structures within 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 above specific embodiments, and those skilled in the art may make various variations or modifications within the scope of the appended claims, which does not affect the essence of the present invention. The embodiments of the present application and the features in the embodiments can be arbitrarily combined with each other, as long as they are not conflict with each other.

Claims

1. A method for solving rigid body attitude based on functional iterative integration, comprising: fitting step: fitting a Chebyshev polynomial function for an angular velocity according to gyro measurement values in a time interval to obtain a Chebyshev polynomial coefficient for the angular velocity;iterating step: iteratively calculating a Chebyshev polynomial coefficient for a Rodrigues vector by using the Chebyshev polynomial coefficient for the angular velocity and a Rodrigues vector integral equation, and performing polynomial truncation on a result obtained at each iterative calculation according to a preset order; andattitude computation step: calculating the Rodrigues vector according to the Chebyshev polynomial coefficient for the Rodrigues vector and a Chebyshev polynomial of a first type to obtain attitude profile in the time interval in a form of quaternion.

2. The method for solving rigid body attitude based on functional iterative integration of claim 1, wherein, the gyro measurement values comprise angular velocity measurement values or angular increment measurement values.

3. The method for solving rigid body attitude based on functional iterative integration of claim 1, wherein, the fitting step specifically comprises: with respect to N angular velocity measurement values {tilde over (ω)}tk or angular increment measurement values Δ{tilde over (θ)}tk at time tk, k=1, 2, . . . N, let

4. The method for solving rigid body attitude based on functional iterative integration of claim 3, wherein, the iterating step specifically comprises: at an lth iterative calculation, adopting the following equation to calculate the Chebyshev polynomial for the Rodrigues vector:

5. The method for solving rigid body attitude based on functional iterative integration of claim 4, wherein, the Chebyshev polynomial coefficient for the Rodrigues vector is iteratively calculated as follows:

6. A system for solving rigid body attitude based on functional iterative integration, comprising: a fitting module, wherein the fitting module is configured to fit a Chebyshev polynomial function for an angular velocity according to gyro measurement values in a time interval to obtain a Chebyshev polynomial coefficient for the angular velocity;an iterating module, wherein the iterating module is configured to iteratively calculate a Chebyshev polynomial coefficient for a Rodrigues vector by using the Chebyshev polynomial coefficient for the angular velocity and a Rodrigues vector integral equation, and perform polynomial truncation on a result at each iterative calculation according to a preset order; andan attitude computation module, wherein the attitude computation module is configured to calculate the Rodrigues vector according to the Chebyshev polynomial coefficient for the Rodrigues vector and a Chebyshev polynomial corresponding to the Rodrigues vector to obtain attitude profile in the time interval in a form of quaternion.

7. The system for solving rigid body attitude based on functional iterative integration according to claim 6, wherein, the gyro measurement values comprise angular velocity measurement values or angular increment measurement values.

8. The system for solving rigid body attitude based on functional iterative integration according to claim 6, wherein, with respect to N angular velocity measurement values {tilde over (ω)}tk or angular increment measurement values Δ{tilde over (θ)}tk at time tk, k=1, 2, . . . N, let

9. The system for solving rigid body attitude based on functional iterative integration according to claim 8, wherein, the iteration module is specifically configured to adopt the following equation to calculate the Chebyshev polynomial for the Rodrigues vector at an lth iterative calculation:

10. The system for solving rigid body attitude based on functional iterative integration according to claim 9, wherein, the Chebyshev polynomial coefficient for the Rodrigues vector is iteratively calculated as follows: