CN102508818B - Arbitrary-step orthogonal series output method of space motion state of rigid body - Google Patents

Abstract

The invention discloses an arbitrary-step orthogonal series output method of the space motion state of a rigid body. In the method, a ternary number is defined so that the three velocity components of a body axis system and the ternary number form linear simultaneous differential equations; the rolling angular velocity p, pitching angular velocity q and yawing angular velocity r are subjected to similar approximation description by use of an arbitrary-step orthogonal polynomial; and a state transition matrix of the system can be solved according to the mode of an arbitrary-order holder so as to obtain an expression of a motion discrete state equation of the rigid body, avoid the problem of singularity of an attitude equation and obtain the major motion state of the rigid body; and in the invention, the state transition matrix becomes a block upper triangular matrix by introducing the ternary number, and the state transition matrix can be solved by reducing order, thereby greatly reducing the computational complexity and facilitating the engineering application.

Description

A kind of any step-length orthogonal series output model modeling method of rigid body space motion state

Technical field

The present invention relates to spatial movement rigid model, particularly aircraft high maneuver state of flight exports problem.

Background technology

The axis system rigid motion differential equation is the fundamental equation describing the spatial movements such as aircraft, torpedo, spacecraft.Usually, in the application such as data processing, the state variable of body axle system mainly comprises the X of 3 speed components, three Eulerian angle and earth axes _e, Y _e, Z _edeng, due to Z _ebe defined as vertical ground and point to earth center, therefore Z _eactual is negative flying height; X _e, Y _eusually mainly rely on GPS, GNSS, the Big Dipper etc. directly to provide; Eulerian angle represent rigid space motion attitude, and the differential equation portraying rigid-body attitude is core wherein, usually describe with three Eulerian angle and pitching, rolling and crab angle.When the angle of pitch of rigid body is ± 90 °, roll angle and crab angle cannot definite values, and it is excessive that the region simultaneously closing on this singular point solves error, causes intolerable error in engineering and can not use; In order to avoid this problem, first people adopt the method for restriction angle of pitch span, and this makes equation degenerate, can not attitude work entirely, are thus difficult to be widely used in engineering practice.Along with the research of flying to aircraft limit, people in succession have employed again direction cosine method, Rotation Vector, Quaternion Method etc. and calculate rigid motion attitude.

Direction cosine method avoids " unusual " phenomenon of Eulerian angle describing method, and calculating attitude matrix with direction cosine method does not have equation degenerate problem, can attitude work entirely, but need to solve 9 differential equations, calculated amount is comparatively large, and real-time is poor, cannot meet engineering practice requirement.Rotation Vector is as list sample recursion, Shuangzi sample gyration vector, three increment gyration vectors and four increment rotating vector law and various correction algorithm on this basis and recursive algorithm etc.When studying rotating vector in document, be all export as the algorithm of angle increment based on rate gyro.But in Practical Project, the output of some gyros is angle rate signals, as optical fibre gyro, dynamic tuned gyroscope etc.When rate gyro exports as angle rate signal, the Algorithm Error of rotating vector law obviously increases.Quaternion Method is that the function of definition 4 Eulerian angle is to calculate boat appearance, effectively can make up the singularity of Eulerian angle describing method, as long as separate 4 differential equation of first order formula groups, there is obvious minimizing than direction cosine attitude matrix differential equation calculated amount, the requirement to real-time in engineering practice can be met.Its conventional computing method have complete card approximatioss, second order, fourth-order Runge-Kutta method and three rank Taylor expansions etc.Finishing card approximatioss essence is list sample algorithm, and what cause restricted rotational movement can not compensate by exchange error, and the algorithm drift under high current intelligence in attitude algorithm can be very serious.Adopt fourth-order Runge-Kutta method when solving quaternion differential equation, along with the continuous accumulation of integral error, there will be trigonometric function value to exceed the ± phenomenon of 1, thus cause calculating to be dispersed; Taylor expansion is also restricted because of the deficiency of computational accuracy.When rigid body high maneuver, angular speed causes more greatly the error of said method larger; Moreover, the error of Attitude estimation usually can cause speed 4 components, highly export error sharply increase.

Summary of the invention

In order to overcome the large problem of existing rigid motion model output error, the invention provides a kind of any step-length orthogonal series output model modeling method of rigid body space motion state, the method is by definition Three-ary Number, axis system three speed components and Three-ary Number is made to form linear differential equation system, and adopt any step-length orthogonal polynomial to rolling, pitching, yaw rate p, q, r carries out close approximation description, can according to the state-transition matrix of the mode solving system of arbitrary order retainer, and then obtain the expression formula of rigid motion discrete state equations, avoid attitude equation singular problem, thus obtain rigid body main movement state.

The technical scheme that the present invention solves the employing of its technical matters is, a kind of any step-length orthogonal series output model modeling method of rigid body space motion state, and its feature comprises the following steps:

1, axis system three speed components export and are:

$\begin{array}{c}{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}+g{\mathrm{\Φ}}_v{[(k+1)T,\mathrm{kT}]\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}\\ +g{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}\end{array}$

Wherein: u, v, w are respectively along rigid body axis system x, y, the speed component of z-axis, n _x, n _y, n _zbe respectively along x, y, the overload of z-axis, g is acceleration of gravity, s ₁, s ₂, s ₃for the Three-ary Number of definition, and

${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s{[(k+1)T,\mathrm{kT}]\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

${\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_v\mathrm{\ζ}+{\mathrm{\Π}}_v\mathrm{\Ω}{\mathrm{\Π}}_v^T$

${\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_s\mathrm{\ζ}+{\mathrm{\Π}}_s\mathrm{\Ω}{\mathrm{\Π}}_s^T$

P, q, r are respectively rolling, pitching, yaw rate, and T is the sampling period;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],\mathrm{\ξ}\left(t\right)={\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_{n-1}\left(t\right)& {\mathrm{\ξ}}_n\left(t\right)\end{array}\right]}^T,$

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=\cos \left[a{\cos }^{-1}(1-2t/b)\right]\\ {\mathrm{\ξ}}_2\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right)\·{\mathrm{\ξ}}_1\left(t\right)-1\\ .\\ .\\ .\\ {\mathrm{\ξ}}_{i+1}\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right){\mathrm{\ξ}}_i\left(t\right)-{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}i=\mathrm{2,3},...,n-\mathrm{1,0}\≤t\≤\mathrm{NT},b=\mathrm{NT}$

For the recursive form of any step-length orthogonal polynomial obtained based on Chebyshev (Chebyshev) orthogonal polynomial, a is any real number, and the expansion of rolling, pitching, yaw rate p, q, r is respectively

p(t)＝[p ₀?p ₁?…?p _n-1?p _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

q(t)＝[q ₀?q ₁?…?q _n-1?q _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

r(t)＝[r ₀?r ₁?…?r _n-1?r _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

$\begin{array}{c}{\mathrm{\Π}}_v=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]\\ +\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\end{array}$

$\begin{array}{c}{\mathrm{\Π}}_s=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]\\ +\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\end{array}$

$\mathrm{\ζ}={\left[\begin{array}{cccc}{\mathrm{\ζ}}_0& {\mathrm{\ζ}}_1& ...& {\mathrm{\ζ}}_n\end{array}\right]}^T={\∫}_{\mathrm{kT}}^{(k+1)T}\mathrm{\ξ}\left(t\right)\mathrm{dt}$

$\begin{array}{c}{\mathrm{\ζ}}_i={\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\ξ}}_i\left(t\right)\mathrm{dt}={\∫}_{\mathrm{kT}}^{(k+1)T}\cos \left[\mathrm{ai}{\cos }^{-1}(1-2t/b)\right]\mathrm{dt}\\ =\frac{b}{4}\left\{\frac{1}{\mathrm{ai}-1}\cos \right[(\mathrm{ai}-1){\cos }^{-1}(1-2t/b)]\\ -\frac{1}{\mathrm{ai}+1}\cos \left[(\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\right]\}{|}_{\mathrm{kT}}^{(k+1)T}\end{array}$

$\mathrm{\Ω}={\left\{{\mathrm{\Ω}}_{\mathrm{ji}}\right\}}_{j=\mathrm{0,1},...,n;i=\mathrm{1,2},...,n}={\∫}_{\mathrm{kT}}^{(k+1)T}\mathrm{\ξ}\left(t\right){\∫}_{\mathrm{kT}}^T\mathrm{\ξ}\left(\mathrm{\τ}\right)\mathrm{d\τdt}$

$\begin{array}{c}{\mathrm{\Ω}}_{\mathrm{ji}}={\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\ξ}}_j\left(t\right){\∫}_{\mathrm{kT}}^t{\mathrm{\ξ}}_i\left(\mathrm{\τ}\right)\mathrm{d\τdt}\\ =\frac{b}{8}\left\{\frac{1}{\mathrm{ai}-1}{\∫}_{\mathrm{kT}}^{(k+1)T}\right\{\cos \left[(\mathrm{aj}-\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\right]+\cos \left[(\mathrm{aj}+\mathrm{ai}-1){\cos }^{-1}(1-2t/b)\right]\}\mathrm{dt}\\ -\frac{1}{\mathrm{ai}+1}{\∫}_{\mathrm{kT}}^{(k+1)T}\left\{\cos \right[(\mathrm{aj}-\mathrm{ai}-1){\cos }^{-1}(1-2t/b)]+\cos [(\mathrm{aj}+\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\left]\right\}\mathrm{dt}\}\\ -\frac{b}{4}\left\{\frac{1}{\mathrm{ai}-1}\cos \right[(\mathrm{ai}-1){\cos }^{-1}(1-2\mathrm{kT}/b)]\\ -\frac{1}{\mathrm{ai}+1}\cos \left[(\mathrm{ai}+1){\cos }^{-1}(1-2\mathrm{kT}/b)\right]\left\}{\∫}_{\mathrm{kT}}^{(k+1)T}\cos \right[\mathrm{aj}{\cos }^{-1}(1-2t/b)]\mathrm{dt}\end{array};$

2, highly output is:

$\stackrel{\·}{h}=\left[\begin{array}{ccc}u& v& w\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\\ s_3\end{array}\right]$

Wherein: h is height;

3, the output of attitude angle is:

$\mathrm{\θ}\left(t\right)=0.5\left\{{\sin }^{-1}\right[s_1\left(t\right)]+{\cos }^{-1}\sqrt{s_2^2\left(t\right)+s_3^2\left(t\right)}\}$

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t\frac{q{s}_2\left(t\right)+{\mathrm{rs}}_3\left(t\right)}{s_2^2\left(t\right)+s_3^2\left(t\right)}\mathrm{dt}$

Wherein: θ, ψ represent rolling, pitching, crab angle respectively, $\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]={\mathrm{\Φ}}_s(t,\mathrm{kT}){\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}.$

The invention has the beneficial effects as follows: make state-transition matrix be triangular form on piecemeal by introducing Three-ary Number, can depression of order solving state transition matrix, enormously simplify computation complexity, be convenient to engineering and use.

Below in conjunction with embodiment, the present invention is elaborated.

Embodiment

1, axis system three speed components export and are:

$\begin{array}{c}{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}+g{\mathrm{\Φ}}_v{[(k+1)T,\mathrm{kT}]\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}\\ +g{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}\end{array}$

Wherein: u, v, w are respectively along rigid body axis system x, y, the speed component of z-axis, n _x, n _y, n _zbe respectively along x, y, the overload of z-axis, g is acceleration of gravity, s ₁, s ₂, s ₃for the Three-ary Number of definition, and

${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s{[(k+1)T,\mathrm{kT}]\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

${\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_v\mathrm{\ζ}+{\mathrm{\Π}}_v\mathrm{\Ω}{\mathrm{\Π}}_v^T$

${\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_s\mathrm{\ζ}+{\mathrm{\Π}}_s\mathrm{\Ω}{\mathrm{\Π}}_s^T$

P, q, r are respectively rolling, pitching, yaw rate, and T is the sampling period;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],\mathrm{\ξ}\left(t\right)={\left[\begin{array}{ccccc}{\mathrm{\ξ}}_0\left(t\right)& {\mathrm{\ξ}}_1\left(t\right)& ...& {\mathrm{\ξ}}_{n-1}\left(t\right)& {\mathrm{\ξ}}_n\left(t\right)\end{array}\right]}^T,$

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=\cos \left[a{\cos }^{-1}(1-2t/b)\right]\\ {\mathrm{\ξ}}_2\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right)\·{\mathrm{\ξ}}_1\left(t\right)-1\\ .\\ .\\ .\\ {\mathrm{\ξ}}_{i+1}\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right){\mathrm{\ξ}}_i\left(t\right)-{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}i=\mathrm{2,3},...,n-\mathrm{1,0}\≤t\≤\mathrm{NT},b=\mathrm{NT}$

For the recursive form of any step-length orthogonal polynomial obtained based on Chebyshev (Chebyshev) orthogonal polynomial, a is any real number, and the expansion of rolling, pitching, yaw rate p, q, r is respectively

p(t)＝[p ₀?p ₁?…?p _n-1?p _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

q(t)＝[q ₀?q ₁?…?q _n-1?q _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

r(t)＝[r ₀?r ₁?…?r _n-1?r _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

$\begin{array}{c}{\mathrm{\Π}}_v=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]\\ +\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\end{array}$

$\begin{array}{c}{\mathrm{\Π}}_s=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& ...& p_{n-1}& p_n\end{array}\right]\\ +\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& ...& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& ...& r_{n-1}& r_n\end{array}\right]\end{array}$

$\mathrm{\ζ}={\left[\begin{array}{cccc}{\mathrm{\ζ}}_0& {\mathrm{\ζ}}_1& ...& {\mathrm{\ζ}}_n\end{array}\right]}^T={\∫}_{\mathrm{kT}}^{(k+1)T}\mathrm{\ξ}\left(t\right)\mathrm{dt}$

$\begin{array}{c}{\mathrm{\ζ}}_i={\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\ξ}}_i\left(t\right)\mathrm{dt}={\∫}_{\mathrm{kT}}^{(k+1)T}\cos \left[\mathrm{ai}{\cos }^{-1}(1-2t/b)\right]\mathrm{dt}\\ =\frac{b}{4}\left\{\frac{1}{\mathrm{ai}-1}\cos \right[(\mathrm{ai}-1){\cos }^{-1}(1-2t/b)]\\ -\frac{1}{\mathrm{ai}+1}\cos \left[(\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\right]\}{|}_{\mathrm{kT}}^{(k+1)T}\end{array}$

$\mathrm{\Ω}={\left\{{\mathrm{\Ω}}_{\mathrm{ji}}\right\}}_{j=\mathrm{0,1},...,n;i=\mathrm{1,2},...,n}={\∫}_{\mathrm{kT}}^{(k+1)T}\mathrm{\ξ}\left(t\right){\∫}_{\mathrm{kT}}^T\mathrm{\ξ}\left(\mathrm{\τ}\right)\mathrm{d\τdt}$

$\begin{array}{c}{\mathrm{\Ω}}_{\mathrm{ji}}={\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\ξ}}_j\left(t\right){\∫}_{\mathrm{kT}}^t{\mathrm{\ξ}}_i\left(\mathrm{\τ}\right)\mathrm{d\τdt}\\ =\frac{b}{8}\left\{\frac{1}{\mathrm{ai}-1}{\∫}_{\mathrm{kT}}^{(k+1)T}\right\{\cos \left[(\mathrm{aj}-\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\right]+\cos \left[(\mathrm{aj}+\mathrm{ai}-1){\cos }^{-1}(1-2t/b)\right]\}\mathrm{dt}\\ -\frac{1}{\mathrm{ai}+1}{\∫}_{\mathrm{kT}}^{(k+1)T}\left\{\cos \right[(\mathrm{aj}-\mathrm{ai}-1){\cos }^{-1}(1-2t/b)]+\cos [(\mathrm{aj}+\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\left]\right\}\mathrm{dt}\}\\ -\frac{b}{4}\left\{\frac{1}{\mathrm{ai}-1}\cos \right[(\mathrm{ai}-1){\cos }^{-1}(1-2\mathrm{kT}/b)]\\ -\frac{1}{\mathrm{ai}+1}\cos \left[(\mathrm{ai}+1){\cos }^{-1}(1-2\mathrm{kT}/b)\right]\left\}{\∫}_{\mathrm{kT}}^{(k+1)T}\cos \right[\mathrm{aj}{\cos }^{-1}(1-2t/b)]\mathrm{dt}\end{array};$

2, highly output is:

$\stackrel{\·}{h}=\left[\begin{array}{ccc}u& v& w\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\\ s_3\end{array}\right]$

Wherein: h is height;

3, the output of attitude angle is:.

$\mathrm{\θ}\left(\mathrm{kT}\right)=0.5\left\{{\sin }^{-1}\right[s_1\left(\mathrm{kT}\right)]+{\cos }^{-1}\sqrt{s_2^2\left(\mathrm{kT}\right)+s_3^2\left(\mathrm{kT}\right)}\}$

$\mathrm{\ψ}\left[(k+1)T\right]=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^{(k+1)T}\frac{q{s}_2\left(t\right)+{\mathrm{rs}}_3\left(t\right)}{s_2^2\left(t\right)+s_3^2\left(t\right)}\mathrm{dt}$

Wherein: θ, ψ represent rolling, pitching, crab angle respectively, ${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s{[(k+1)T,\mathrm{kT}]\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}.$

Claims (1)

1. any step-length orthogonal series output model modeling method of rigid body space motion state, its feature comprises the following steps:

(1) axis system three speed components export and are:

${\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}u\left(t\right)\\ v\left(t\right)\\ w\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}+g{\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\Φ}}_v[(k+1)T,\mathrm{\τ}]\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]\mathrm{d\τ}$

$+{\mathrm{g\Φ}}_v[(k+1)T,\mathrm{kT}]{\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

Wherein: u, v, w are respectively along rigid body axis system x, y, the speed component of z-axis, n _x, n _y, n _zbe respectively along x, y, the overload of z-axis, g is acceleration of gravity, s ₁, s ₂, s ₃for the Three-ary Number of definition, and

${\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=(k+1)T}={\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]{\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}$

${\mathrm{\Φ}}_v[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_v\mathrm{\ζ}+{\mathrm{\Π}}_v\mathrm{\Ω}{\mathrm{\Π}}_v^T$

${\mathrm{\Φ}}_s[(k+1)T,\mathrm{kT}]\≈I+{\mathrm{\Π}}_s\mathrm{\ζ}+{\mathrm{\Π}}_s\mathrm{\Ω}{\mathrm{\Π}}_s^T$

P, q, r are respectively rolling, pitching, yaw rate, and T is the sampling period;

$I=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ ξ(t)＝[ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T，

$\left.\begin{array}{c}{\mathrm{\ξ}}_0\left(t\right)=1\\ {\mathrm{\ξ}}_1\left(t\right)=\cos \left[a{\cos }^{-1}(1-2t/b)\right]\\ {\mathrm{\ξ}}_2\left(t\right)={2\mathrm{\ξ}}_1\left(t\right)\·{\mathrm{\ξ}}_1\left(t\right)-1\\ \·\\ \·\\ \·\\ {\mathrm{\ξ}}_{i+1}\left(t\right)=2{\mathrm{\ξ}}_1\left(t\right){\mathrm{\ξ}}_i\left(t\right)-{\mathrm{\ξ}}_{i-1}\left(t\right)\end{array}\right\}$ i＝2，3，…，n-1，0≤t≤NT，b＝NT

For the recursive form of any step-length orthogonal polynomial obtained based on Chebyshev (Chebyshev) orthogonal polynomial, a is any real number, and the expansion of rolling, pitching, yaw rate p, q, r is respectively

p(t)＝[p ₀?p ₁?…?p _n-1?p _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

q(t)＝[q ₀?q ₁?…?q _n-1?q _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

r(t)＝[r ₀?r ₁?…?r _n-1?r _n][ξ ₀(t)?ξ ₁(t)?…?ξ _n-1(t)?ξ _n(t)] ^T

${\mathrm{\Π}}_v=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

${\mathrm{\Π}}_s=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\left[\begin{array}{ccccc}{p}_0& p_1& \·\·\·& p_{n-1}& p_n\end{array}\right]$

$+\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{q}_0& q_1& \·\·\·& q_{n-1}& q_n\end{array}\right]+\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccc}{r}_0& r_1& \·\·\·& r_{n-1}& r_n\end{array}\right]$

$\mathrm{\ζ}={\left[\begin{array}{cccc}{\mathrm{\ζ}}_0& {\mathrm{\ζ}}_1& \·\·\·& {\mathrm{\ζ}}_n\end{array}\right]}^T={\∫}_{\mathrm{kT}}^{(k+1)T}\mathrm{\ξ}\left(t\right)\mathrm{dt}$

${\mathrm{\ξ}}_i={\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\ξ}}_i\left(t\right)\mathrm{dt}={\∫}_{\mathrm{kT}}^{(k+1)T}\cos \left[\mathrm{ai}{\cos }^{-1}(1-2t/b)\right]\mathrm{dt}$

$=\frac{b}{4}\left\{\frac{1}{\mathrm{ai}-1}\cos \right[(\mathrm{ai}-1){\cos }^{-1}(1-2t/b)]$

$-\frac{1}{\mathrm{ai}+1}\cos \left[(\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\right]\}{|}_{\mathrm{kT}}^{(k+1)T}$

$\mathrm{\Ω}={\left\{{\mathrm{\Ω}}_{\mathrm{ji}}\right\}}_{j=\mathrm{0,1},...,n;i=\mathrm{1,2},...,n}={\∫}_{\mathrm{kT}}^{(k+1)T}\mathrm{\ξ}\left(t\right){\∫}_{\mathrm{kT}}^T\mathrm{\ξ}\left(\mathrm{\τ}\right)\mathrm{d\τdt}$

${\mathrm{\Ω}}_{\mathrm{ji}}={\∫}_{\mathrm{kT}}^{(k+1)T}{\mathrm{\ξ}}_j\left(t\right){\∫}_{\mathrm{kT}}^t{\mathrm{\ξ}}_i\left(\mathrm{\τ}\right)\mathrm{d\τdt}$

$=\frac{b}{8}\left\{\frac{1}{\mathrm{ai}-1}{\∫}_{\mathrm{kT}}^{(k+1)T}\right\{\cos \left[(\mathrm{aj}-\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\right]+\cos \left[(\mathrm{aj}+\mathrm{ai}-1){\cos }^{-1}(1-2t/b)\right]\}\mathrm{dt}$

$-\frac{1}{\mathrm{ai}+1}{\∫}_{\mathrm{kT}}^{(k+1)T}\left\{\cos \right[(\mathrm{aj}-\mathrm{ai}-1){\cos }^{-1}(1-2t/b)]+\cos [(\mathrm{aj}+\mathrm{ai}+1){\cos }^{-1}(1-2t/b)\left]\right\}\mathrm{dt}\}$

$-\frac{b}{4}\left\{\frac{1}{\mathrm{ai}-1}\cos \right[(\mathrm{ai}-1){\cos }^{-1}(1-2\mathrm{kT}/b)]$

$-\frac{1}{\mathrm{ai}+1}\cos \left[(\mathrm{ai}+1){\cos }^{-1}(1-2\mathrm{kT}/b)\right]\left\}{\∫}_{\mathrm{kT}}^{(k+1)T}\cos \right[\mathrm{aj}{\cos }^{-1}(1-2t/b)]\mathrm{dt}$

(2) highly output is:

$\stackrel{\·}{h}=\left[\begin{array}{ccc}u& v& w\end{array}\right]\left[\begin{array}{c}{s}_1\\ s_2\\ s_3\end{array}\right]$

Wherein: h is height;

(3) output of attitude angle is:

$\mathrm{\θ}\left(t\right)=0.5\left\{{\sin }^{-1}\right[s_1\left(t\right)]+{\cos }^{-1}\sqrt{s_2^2\left(t\right)+s_3^2\left(t\right)}\}$

$\mathrm{\ψ}\left(t\right)=\mathrm{\ψ}\left(\mathrm{kT}\right)+{\∫}_{\mathrm{kT}}^t\frac{{\mathrm{qs}}_2\left(t\right)+{\mathrm{rs}}_3\left(t\right)}{s_2^2\left(t\right)+s_3^2\left(t\right)}\mathrm{dt}$

Wherein: represent rolling, pitching, crab angle respectively, $\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]={\mathrm{\Φ}}_s(t,\mathrm{kT}){\left[\begin{array}{c}{s}_1\left(t\right)\\ s_2\left(t\right)\\ s_3\left(t\right)\end{array}\right]}_{t=\mathrm{kT}}.$