CN105509703A - Calculation method for roll angel of spinning projectile - Google Patents

Abstract

The invention provides a calculation method for a roll angle of a spinning projectile. The calculation method comprises steps as follows: a projectile coordination system and a navigation coordination system are established; a projectile body of the spinning projectile is controlled to rotate at a preset projectile attitude angle according to the projectile coordination system and the navigation coordination system; a rotating matrix is established according to the projectile attitude angle of the rotating projectile body, and a spinning projectile attitude matrix of the spinning projectile is calculated according to the rotating matrix; the projection of a geomagnetism in a reference system is set, and the roll angel is calculated according to the projection of the geomagnetism in the reference system, the rotating matrix and the spinning projectile attitude matrix. With the adoption of the method for calculating the roll angle through the geomagnetism, the measurement range can be larger, drifting cannot be generated, and the measurement precision is improved.

Description

For the computing method of the roll angle of rotating missile

Technical field

The present invention relates to rotating missile technical field, particularly a kind of computing method of the roll angle for rotating missile.

Background technology

Rotating missile rotates around bullet axle in the air, and the roll angle how measuring rotating missile is the current technical issues that need to address.This is because current gyrostatic measurement range is lower, rotating missile is difficult to adapt to.In addition, due to the drift of gyroscope itself, considerable influence can be produced to attitude after long-term accumulated, error is caused.

Summary of the invention

Object of the present invention is intended at least solve one of described technological deficiency.

For this reason, the object of the invention is to the computing method proposing a kind of roll angle for rotating missile, calculate the method for roll angle with earth magnetism, measurement range can be made comparatively large, can not drift be produced, improve measuring accuracy.

To achieve these goals, embodiments of the invention provide a kind of computing method of the roll angle for rotating missile, comprise the steps:

Step S1, sets up missile coordinate system and navigational coordinate system;

Step S2, the body controlling rotating missile rotates with default body attitude angle according to described missile coordinate system and navigational coordinate system;

Step S3, sets up rotation matrix according to the body attitude angle that described body rotates, and calculates the rotating missile attitude matrix of described rotating missile according to described rotation matrix;

Step S4, arranges the projection of earth magnetism in reference frame, calculates roll angle according to the projection of described earth magnetism in reference frame, rotation matrix and rotating missile attitude matrix.

Further, described missile coordinate system is defined as: X-axis represents roll axle, and Y-axis represents pitch axis, and Z axis represents driftage; Described navigational coordinate system is defined as east northeast ground coordinate system.

Further, in described step S2, described body attitude angle is the angle of body when rotating in the following order: the angle ψ rotated around described Z axis, around the angle θ that described Y-axis is rotated, around the angle Φ that described X-axis is rotated.

Further, in described step S3, described rotation matrix is (c1, c2, c3),

Rotate ψ angle according to around z-axis, calculate $c1=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right];$

Rotate θ angle according to around y-axis, calculate $c2=\left[\begin{array}{ccc}cos\mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& cos\mathrm{\θ}\end{array}\right];$

Rotate Φ angle according to around x-axis, calculate $c3=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\φ}& sin\mathrm{\φ}\\ 0& -sin\mathrm{\φ}& cos\mathrm{\φ}\end{array}\right].$

Further, in described step S3, calculate the rotating missile attitude matrix of described rotating missile according to described rotation matrix, comprise the steps:

Carrier system is changed to reference to system according to described rotation matrix,

Then described rotating missile attitude matrix is calculated for:

$\begin{array}{c}{C}_b^n=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]=\\ \left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& -\cos \mathrm{\Φ}\sin \mathrm{\ψ}+\sin \mathrm{\Φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\Φ}\sin \mathrm{\ψ}+\cos \mathrm{\Φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\Φ}\cos \mathrm{\ψ}+\sin \mathrm{\Φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\Φ}\cos \mathrm{\ψ}+\cos \mathrm{\Φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\Φ}\cos \mathrm{\θ}& \cos \mathrm{\Φ}\cos \mathrm{\ψ}\end{array}\right]\end{array}$

Wherein,

$\mathrm{\Φ}=arctan\[\frac{c_{32}}{c_{33}}\]$

$\mathrm{\θ}g=arcsin\[-c_{31}\]$

$\mathrm{\ψ}=arctan\[\frac{c_{21}}{c_{11}}\].$

Further, in described step S4, calculating roll angle Φ d according to the projection of described earth magnetism in reference frame, rotation matrix and rotating missile attitude matrix is:

Φd＝arctan(yb/zb)，

Wherein,

$\begin{array}{c}\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=c_2{c}_1\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]=\\ \left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right],\end{array}$

Wherein, $\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right]$ For the projection of earth magnetism in reference frame.

Further, in step s 4 which, calculate rotating missile roll angle according to earth magnetism, comprise the steps: Φ 1=Φ _{_ 1}-k × (Φ g-Φ d), wherein, Φ 1 is revised roll angle, Φ _{_ 1}roll angle Φ 1, the Φ g calculated for the last time is that body rotates roll angle around X-axis, and Φ g=(Φ, θ, ψ), k are predetermined coefficient.

Further, the span of k is 0.4 ~ 0.6.

According to the computing method of the roll angle for rotating missile of the embodiment of the present invention, according to the computing method of the roll angle for rotating missile of the embodiment of the present invention, calculate the method for roll angle with earth magnetism, measurement range can be made larger.Because earth magnetism is vector, therefore can not produce drift, improve measuring accuracy.And the cost of earth magnetism is lower than gyroscope, therefore calculates the method for roll angle with earth magnetism, the production cost of whole product can be reduced.

The aspect that the present invention adds and advantage will part provide in the following description, and part will become obvious from the following description, or be recognized by practice of the present invention.

Accompanying drawing explanation

Above-mentioned and/or additional aspect of the present invention and advantage will become obvious and easy understand from accompanying drawing below combining to the description of embodiment, wherein:

Fig. 1 is the process flow diagram of the computing method of the roll angle for rotating missile according to the embodiment of the present invention.

Embodiment

Be described below in detail embodiments of the invention, the example of embodiment is shown in the drawings, and wherein same or similar label represents same or similar element or has element that is identical or similar functions from start to finish.Be exemplary below by the embodiment be described with reference to the drawings, be intended to for explaining the present invention, and can not limitation of the present invention be interpreted as.

As shown in Figure 1, the computing method of the roll angle for rotating missile of the embodiment of the present invention, comprise the steps:

Step S1, sets up missile coordinate system and navigational coordinate system.

Particularly, establishment of coordinate system is as follows: missile coordinate system is defined as: X-axis represents roll axle, corresponding front; Y-axis represents pitch axis, corresponding right; Z axis represents driftage, corresponding below.Navigational coordinate system is defined as east northeast ground coordinate system.

Step S2, the body controlling rotating missile rotates with default body attitude angle according to missile coordinate system and navigational coordinate system.

Body attitude angle is the angle of body when rotating in the following order: the angle ψ rotated around Z axis, around the angle θ that Y-axis is rotated, around the angle Φ that X-axis is rotated.

Step S3, sets up rotation matrix according to the body attitude angle that body rotates, and calculates the rotating missile attitude matrix of rotating missile according to rotation matrix.

Body rotates and rotates ψ according to around yaw axis (Z axis), and pitch axis (Y-axis) rotates θ, and when roll axle (X-axis) rotates the order rotation of Φ, rotation matrix is (c1, c2, c3),

Rotate ψ angle according to around z-axis, calculate $c1=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right];---\left(1\right)$

Rotate θ angle according to around y-axis, calculate $c2=\left[\begin{array}{ccc}cos\mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& cos\mathrm{\θ}\end{array}\right];---\left(2\right)$

Rotate Φ angle according to around x-axis, calculate $c3=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\φ}& sin\mathrm{\φ}\\ 0& -sin\mathrm{\φ}& cos\mathrm{\φ}\end{array}\right];---\left(3\right)$

Based on this, calculate the rotating missile attitude matrix of rotating missile according to rotation matrix, comprise the steps:

First, be changed to carrier system according to rotation matrix with reference to system,

$c_n^b=c3c2c1;---\left(4\right)$

Then, rotating missile attitude matrix is calculated for:

$\begin{array}{c}{C}_b^n=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]=\\ \left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& -\cos \mathrm{\Φ}\sin \mathrm{\ψ}+\sin \mathrm{\Φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\Φ}\sin \mathrm{\ψ}+\cos \mathrm{\Φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\Φ}\cos \mathrm{\ψ}+\sin \mathrm{\Φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\Φ}\cos \mathrm{\ψ}+\cos \mathrm{\Φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\Φ}\cos \mathrm{\θ}& \cos \mathrm{\Φ}\cos \mathrm{\ψ}\end{array}\right]\\ =\left[\begin{array}{ccc}{a}^2+b^2-c^2-d^2& 2(bc-ad)& 2(bd+ac)\\ 2(bc+ad)& (a^2-b^2+c^2-d^2)& 2(cd-ab)\\ 2(bd-ac)& 2(cd+ab)& (a^2-b^2-c^2+d^2)\end{array}\right]\end{array},---\left(5\right)$

Further, can calculate body attitude angle according to above-mentioned rotating missile attitude matrix is:

$\mathrm{\Φ}=arctan\[\frac{c_{32}}{c_{33}}\]---\left(6\right)$

$\mathrm{\θ}=arcsin\[-c_{31}\]---\left(7\right)$

$\mathrm{\ψ}=arctan\[\frac{c_{21}}{c_{11}}\]---\left(8\right)$

Step S4, arranges the projection of earth magnetism in reference frame, calculates roll angle according to the projection of earth magnetism in reference frame, rotation matrix and rotating missile attitude matrix.

Calculating roll angle Φ d according to the projection of earth magnetism in reference frame, rotation matrix and rotating missile attitude matrix is:

Φd＝arctan(yb/zb)，

Wherein,

$\begin{array}{c}\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=c_2{c}_1\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]=\\ \left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right],\end{array}$

Wherein, $\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right]$ For the projection of earth magnetism in reference frame.

Further, in step s 4 which, calculate rotating missile roll angle according to earth magnetism, comprise the steps: Φ 1=Φ _{_ 1}-k × (Φ g-Φ d), wherein, Φ 1 is revised roll angle, Φ _{_ 1}roll angle Φ 1, the Φ g calculated for the last time is that body rotates roll angle around X-axis, and Φ g=(Φ, θ, ψ), k are predetermined coefficient.

Further, the span of k is 0.4 ~ 0.6.

According to the computing method of the roll angle for rotating missile of the embodiment of the present invention, calculate the method for roll angle with earth magnetism, measurement range can be made larger.Because earth magnetism is vector, therefore can not produce drift, improve measuring accuracy.And the cost of earth magnetism is lower than gyroscope, therefore calculates the method for roll angle with earth magnetism, the production cost of whole product can be reduced.

In the description of this instructions, specific features, structure, material or feature that the description of reference term " embodiment ", " some embodiments ", " example ", " concrete example " or " some examples " etc. means to describe in conjunction with this embodiment or example are contained at least one embodiment of the present invention or example.In this manual, identical embodiment or example are not necessarily referred to the schematic representation of above-mentioned term.And the specific features of description, structure, material or feature can combine in an appropriate manner in any one or more embodiment or example.

Although illustrate and describe embodiments of the invention above, be understandable that, above-described embodiment is exemplary, can not be interpreted as limitation of the present invention, those of ordinary skill in the art can change above-described embodiment within the scope of the invention when not departing from principle of the present invention and aim, revising, replacing and modification.Scope of the present invention is by claims extremely equivalency.

Claims (8)

1. for computing method for the roll angle of rotating missile, it is characterized in that, comprise the steps:

Step S1, sets up missile coordinate system and navigational coordinate system;

Step S2, the body controlling rotating missile rotates with default body attitude angle according to described missile coordinate system and navigational coordinate system;

Step S3, sets up rotation matrix according to the body attitude angle that described body rotates, and calculates the rotating missile attitude matrix of described rotating missile according to described rotation matrix;

Step S4, arranges the projection of earth magnetism in reference frame, calculates roll angle according to the projection of described earth magnetism in reference frame, rotation matrix and rotating missile attitude matrix.

2., as claimed in claim 1 for the computing method of the roll angle of rotating missile, it is characterized in that, described missile coordinate system is defined as: X-axis represents roll axle, and Y-axis represents pitch axis, and Z axis represents driftage; Described navigational coordinate system is defined as east northeast ground coordinate system.

3. as claimed in claim 1 for the computing method of the roll angle of rotating missile, it is characterized in that, in described step S2, described body attitude angle is the angle of body when rotating in the following order: the angle ψ rotated around described Z axis, around the angle θ that described Y-axis is rotated, around the angle Φ that described X-axis is rotated.

4., as claimed in claim 1 for the computing method of the roll angle of rotating missile, it is characterized in that, in described step S3, described rotation matrix is (c1, c2, c3),

Rotate ψ angle according to around z-axis, calculate $c1=\left[\begin{array}{ccc}cos\mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& cos\mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right];$

Rotate θ angle according to around y-axis, calculate $c2=\left[\begin{array}{ccc}cos\mathrm{\θ}& 0& -sin\mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& cos\mathrm{\θ}\end{array}\right];$

Rotate Φ angle according to around x-axis, calculate $c3=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -sin\mathrm{\φ}& cos\mathrm{\φ}\end{array}\right].$

5., as claimed in claim 4 for the computing method of the roll angle of rotating missile, it is characterized in that, in described step S3, calculate the rotating missile attitude matrix of described rotating missile according to described rotation matrix, comprise the steps:

Carrier system is changed to reference to system, c according to described rotation matrix _n ^b=c3c2c1;

Then described rotating missile attitude matrix C is calculated _b ⁿfor:

$\begin{array}{c}{C}_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{11}\\ C_{21}& C_{\mathrm{22}}& C_{\mathrm{23}}\\ C_{31}& C_{\mathrm{32}}& C_{\mathrm{33}}\end{array}\right]=\\ \left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& -\cos \mathrm{\Φ}\sin \mathrm{\ψ}+\sin \mathrm{\Φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\Φ}\sin \mathrm{\ψ}+\cos \mathrm{\Φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\Φ}\cos \mathrm{\ψ}+\sin \mathrm{\Φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\Φ}\cos \mathrm{\ψ}+\cos \mathrm{\Φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\Φ}\cos \mathrm{\θ}& \cos \mathrm{\Φ}\cos \mathrm{\ψ}\end{array}\right]\end{array},$

Wherein,

$\mathrm{\Φ}=arctan\[\frac{c_{32}}{c_{33}}\]$

$\mathrm{\θ}=arcsin\[-c_{31}\]$

$\mathrm{\ψ}=arctan\[\frac{c_{21}}{c_{11}}\].$

6. as claimed in claim 5 for the computing method of the roll angle of rotating missile, it is characterized in that, in described step S4, calculating roll angle Φ d according to the projection of described earth magnetism in reference frame, rotation matrix and rotating missile attitude matrix is:

Φd＝arctan(yb/zb)，

Wherein,

$\begin{array}{c}\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=c_2{c}_1\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]=\\ \left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right],\end{array}$

Wherein, $\left[\begin{array}{c}{x}_b^{11}\\ y_b^{11}\\ z_b^{11}\end{array}\right]$ For the projection of earth magnetism in reference frame.

7. as claimed in claim 6 for the computing method of the roll angle of rotating missile, it is characterized in that, in described step S4, calculate rotating missile roll angle according to earth magnetism, comprise the steps: Φ 1=Φ _{_ 1}-k × (Φ g-Φ d), wherein, Φ 1 is revised roll angle, Φ _{_ 1}roll angle Φ 1, the Φ g calculated for the last time is that body rotates roll angle around X-axis, and Φ g=(Φ, θ, ψ), k are predetermined coefficient.

8., as claimed in claim 7 for the machine calculation method of the roll angle of rotating missile, it is characterized in that, the span of k is 0.4 ~ 0.6.