Inertial navigation system

Abstract

An inertial measurement system for a longitudinal projectile comprising: a first, roll gyro to be oriented substantially parallel to the longitudinal axis of the projectile; a second gyro and a third gyro with axes arranged with respect to the roll gyro such that they define a three dimensional coordinate system; a controller, arranged to: compute a current projectile attitude from the outputs of the first, second and third gyros, the computed attitude comprising a roll angle, a pitch angle and a yaw angle; compare the computed pitch and yaw angles with expected values for the pitch and yaw angles; calculate a roll angle error and a roll scale factor error based on the difference between the computed pitch and yaw angles and the expected pitch and yaw angles; and apply the calculated roll angle error and roll scale factor error to the output of the roll gyro.

Claims

1. An inertial measurement system for a longitudinal projectile comprising: a first, roll gyro to be oriented substantially parallel to the longitudinal axis of the projectile; a second gyro and a third gyro with axes arranged with respect to the roll gyro such that they define a three dimensional coordinate system; a controller, arranged to: compute a current projectile attitude from the outputs of the first, second and third gyros, the computed attitude comprising a roll angle, a pitch angle and a yaw angle; compare the computed pitch and yaw angles with expected values for the pitch and yaw angles; calculate a roll angle error and a roll scale factor error based on the difference between the computed pitch and yaw angles and the expected pitch and yaw angles; and apply the calculated roll angle error and roll scale factor error to the output of the roll gyro.

2. An inertial measurement system as claimed in claim 1, wherein the roll gyro is a MEMS gyro.

3. An inertial measurement system as claimed in claim 1, wherein the expected values for pitch and yaw angles as a function of flight time correspond to those expected from planar ballistic flight.

4. An inertial measurement system as claimed in claim 3, wherein the roll angle error is calculated as the angle whose tangent is the ratio of the rate of change of the calculated yaw angle to the rate of change of the calculated pitch angle.

5. An inertial measurement system as claimed in claim 1, wherein the expected values for pitch and yaw angles as a function of flight time are taken from a pre-computed flight trajectory which may be non-planar.

6. An inertial measurement system as claimed in claim 1 wherein the roll angle error is multiplied by a roll angle error gain factor before being applied to the output of the roll gyro.

7. An inertial measurement system as claimed in claim 1, wherein the roll scale factor error is multiplied by a roll scale factor error gain factor before being applied to the output of the roll gyro.

8. An inertial measurement system as claimed in claim 6, wherein the gain factor is a fixed value.

9. An inertial measurement system as claimed in claim 7, wherein the roll scale factor error gain is proportional to the reciprocal of the instantaneous roll rate.

10. An inertial measurement system as claimed in claim 1, wherein the roll angle error and roll scale factor error are only applied prior to any guidance action being initiated.

11. An inertial measurement system as claimed in claim 1, wherein the controller is arranged such that a period of time after launch it performs an attitude reset function, said attitude reset function comprising: setting the current projectile yaw equal to a corrected yaw value; and setting the current projectile pitch equal to a corrected pitch value.

12. An inertial measurement system as claimed in claim 11, further comprising: body mounted accelerometers for measuring an acceleration vector; and a velocity calculation unit that calculates a velocity vector from the acceleration vector; and wherein the corrected yaw value is equal to a current track angle of the velocity vector and the corrected pitch value is equal to a current climb angle of the velocity vector.

13. An inertial measurement system as claimed in claim 11, wherein the period of time is a time at which the measured roll rate decreases below a threshold roll rate.

14. An inertial measurement system as claimed in claim 11, wherein the period of time is a set time after launch.

15. A method of correcting roll angle in an inertial measurement system for a longitudinal projectile, comprising: computing a current projectile attitude comprising a roll angle, a pitch angle and a yaw angle; comparing the computed pitch angle and computed yaw angle with an expected pitch angle and an expected yaw angle; calculating a roll angle error and a roll scale factor error based on the difference between the computed pitch and yaw angles and the expected pitch and yaw angles; and applying the calculated roll angle error and roll scale factor error to the output of a roll gyro.

Description

BRIEF DESCRIPTION OF DRAWINGS

(1) One or more non-limiting examples will now be described, by way of example only, and with reference to the accompanying figures in which:

(2) FIG. 1a shows a projectile in flight;

(3) FIG. 1b illustrates the attitude of the projectile;

(4) FIG. 2 shows an inertial navigation system;

(5) FIG. 3 shows an attitude reset function; and

(6) FIG. 4 shows a roll control process.

(7) FIG. 1a illustrates a rocket 100 in flight. Its trajectory 110 is shown in broken line. The rocket 100 has a nose 101, tail 102 with fins 103. Fins 103 may be static or they may be movable so as to provide directional control so as to alter the flight path of the rocket 100.

(8) In inertial navigation terminology, the orientation of a body/platform is described in terms of the three Euler angles heading, elevation and bank. The equivalent terms yaw, pitch and roll are also in use. The body orientation is generally referred to using the term attitude. Although strictly speaking the term attitude refers only to the elevation and bank angles, in this document, the more general definition is used. Therefore in this document, the term attitude refers to all three of heading, elevation and bank (or equivalently yaw, pitch and roll).

(9) A quaternion is a 4-element hyper-complex number used in inertial navigation as a convenient method of maintaining and propagating heading/attitude angles. Use of quaternions avoids the numeric singularities that can occur when Euler angles are manipulated directly. The use of quaternions in navigation systems is well known and is not described further here.

(10) In FIG. 1b the attitude 125 of the rocket 100 is shown with respect to the axes 120 of the navigation frame of reference. The projection of the attitude 125 onto the horizontal plane is shown by dotted line 126. The heading or yaw angle of the rocket is shown by the angle 121, the elevation or pitch angle of the rocket is shown by the angle 122 and the bank or roll angle of the rocket about its longitudinal axis is indicated by arrow 123. The rocket 100 is rotating anti-clockwise when viewed from behind as shown.

(11) FIG. 2 illustrates how the roll correction system of the examples described here fits into a standard inertial navigation system.

(12) This example is described below in terms of its use in a surface-to-surface rocket application. However, the technique may be applied to any projectile/munition which travels on a predictable trajectory once the initial launch event/rocket motor burn phase is complete, and prior to any guidance actions being initiated. This period of flight may be referred to as the ballistic phase. During ballistic flight, the platform will travel on a heading (yaw) which is either fixed/constant, or a heading which changes predictably over time. Similarly, the platform will exhibit an elevation (pitch) angle profile which develops predictably during flight. According to the preferred examples described here, any deviation from these expected profiles can be used to compute and correct the prevailing roll angle error, which in turn allows gyro scale factor error to be inferred. Scale factor corrections can then be applied at source to prevent further roll angle errors developing.

(13) The following text describes a two-step process involving firstly an attitude reset, followed by a continuous process of roll angle error determination and correction. Again, this approach is focussed on the surface rocket application, where this specific sequence of events is appropriate. In other applications the attitude reset process may not be required, or it may be replaced by some equivalent but different process. For example, in applications involving guided artillery shells there is no opportunity for initialising the inertial navigation process on the ground. Instead the process must be initiated in-flight. A common approach is to initialise using data in a flight-path-aiding table (FPAT), and this circumvents the need for an attitude reset process.

(14) The Roll Control process is indicated in FIG. 2 by the dashed lines and boxes with dashed borders. The conventional strapdown inertial data flow is indicated by solid lines and boxes with solid borders. The process is particularly suited to operate as part of the navigation function of a MEMS IMU based system used to guide a surface launched rocket. The normal sequence of events for this example is: Initialise and align the navigation system on the ground prior to launch Launch the rocket, at which point high roll rotation rates are experienced (e.g. 10 Hz to 20 Hz) During this flight phase Bank (roll) angle error rapidly increases (e.g. >10/s) As the flight progresses, the roll rate may reduce and an orderly flight trajectory develops At this point an Attitude Reset function restores the attitude quaternion accuracy as described below. A Roll Control function then operates to maintain Bank (roll) angle accuracy This process is intended to restrict the inertial attitude error growth during the unguided flight phase and prior to any control action, to allow GPS-aided navigation to operate.

(15) As shown in FIG. 2, the raw inputs of the inertial navigation system 200 are the roll, pitch and yaw rates from the body mounted gyroscopes indicated at 210, 211, 212 respectively, and the specific force vector f.sub.b from the body mounted accelerometers indicated at 220.

(16) In a standard navigation system (ignoring for the moment the additional roll control functions of this example), the gyroscope outputs 210, 211, 212 are integrated by the attitude integration function 225. The attitude integration function also takes into account other corrections such as the Earth Rate Correction 230, Transport Rate Correction 235, Coriolis & Centripetal Correction 240 and Gravity Correction 245. Each of these corrections are based on the position and velocity data derived from the accelerometers 220. The acceleration vector {dot over (v)}.sub.n 222 is integrated by first integration module 223 to produce velocity vector v.sub.n 224 which is integrated by second integration module 225 to produce position vector R 226.

(17) The Earth Rate Correction 230, Transport Rate Correction 235, Coriolis & Centripetal Correction 240 and Gravity Correction 245 and the associated transformations between reference frames are all well-known and understood and will therefore not be described further here. However, to aid understanding, the symbols in FIGS. 2, 3 and 4 have the following meanings: f.sub.b Specific force in the body reference frame f.sub.n Specific force in the navigation reference frame {dot over (v)}.sub.n n Acceleration in the navigation frame of reference v.sub.n Velocity in the navigation frame of reference C.sub.b.sup.n Transformation matrixbody to navigation reference frames .sub.en.sup.n Navigation frame rotation with respect to the Earth frame in the navigation frame of reference C.sub.e.sup.n Transformation MatrixEarth to navigation reference frames .sub.ib.sup.b Body rotation rate in the body frame of reference .sub.in.sup.n Navigation frame rotation rate in the navigation frame of reference .sub.ie.sup.n Earth rotation rate in the navigation frame of reference .sub.ie.sup.e Earth rotation rate in the Earth frame of reference Inertially derived Elevation (pitch) angle Inertially derived Bank (roll) angle Inertially derived Heading (yaw) angle .sub.error Bank (roll) angle error estimate .sub.est Elevation angle estimate, based on Climb angle .sub.est Heading angle estimate, based on Track angle {dot over ()} Inertially measured roll rate

(18) $\begin{array}{cc}\frac{}{t}& \mathrm{Inertially}\mathrm{derived}\mathrm{Elevation}\left(\mathrm{pitch}\right)\mathrm{angle}\mathrm{rate}\\ \frac{}{t}& \mathrm{Inertially}\mathrm{derived}\mathrm{Heading}\left(\mathrm{yaw}\right)\mathrm{angle}\mathrm{rate}\end{array}$

(19) The roll correction process of this example is shown at 250. It receives inputs from the roll gyroscope 210 as well as the current velocity v.sub.n and the current calculated attitude C.sub.b.sup.n. The roll correction process 250 outputs an attitude reset output 252, a roll angle error output 254 and a roll scale factor error output 256.

(20) The attitude reset function 300 of the roll control process 250 will be described first with reference to FIG. 3.

(21) The Attitude Reset function 300 operates as a single event, triggered by a pre-defined condition, such as time from launch or a roll rate threshold as described further below.

(22) When triggered the function re-computes the attitude quaternion based on the following calculations:

(23) The computed velocity vector v.sub.n 224 is fed into the Track & Climb angle module 305. In Track & Climb angle module 305, the heading (yaw) component is output as an estimate of the yaw angle .sub.est and the elevation (pitch) component is output as an estimate of the pitch angle .sub.est as indicated at 310.

(24) Bank (roll angle) is derived as a correction to the existing Bank (roll) estimate, the correction being the ratio between the Heading (yaw) rate and the Elevation (pitch) rate as calculated from the attitude integration module 225. In other words, the Bank angle error is the angle having the ratio of Heading (yaw) rate to Elevation (pitch) Rate as its trigonometric tangent. This calculation is based on the assumption that the rocket trajectory is purely planar i.e. it maintains the same fixed heading throughout the flight. The elevation angle in such flight is also expected to decrease at a constant, predictable rate. This calculation is based on the assumption that any deviation in Heading (yaw) angle must have arisen through errors in the Bank (roll) angle calculation. This technique may however be extended to applications involving non-planar trajectories (e.g. spinning artillery shells) using prior knowledge of the expected heading profile. In such cases, a similar calculation can be made using the deviation of the calculated Heading (yaw) angle compared with the expected/predicted Heading (yaw) angle and assuming that any such deviations have arisen through errors in the Bank (roll) calculation.

(25) The Heading (yaw) and Elevation (pitch) rates are continuously calculated as the time derivative of low-pass filtered forms of the Heading (yaw) and Elevation (pitch) angles from the attitude integration module 225. The low pass filter gain is varied according to the roll rate, to accommodate the effect of roll rate on the characteristics of unwanted modulations in the heading and elevation angles. The frequency and magnitude of the unwanted modulations are related to roll rate, therefore the low pass filter performs better if adjusted by the roll rate.

(26) Referring to FIG. 3, the Heading (yaw, psi) and Elevation (pitch, theta) angles are extracted from the attitude integration module 225 and fed into low pass filter 315 which filters out unwanted high frequency components. Time derivatives of Elevation (pitch) angle,

(27) $\frac{}{t}$
and Heading (yaw) angle,

(28) $\frac{}{t}$
are calculated by time-derivative module 320 and the ratio of these time derivatives is calculated in Ratio module 325. The ratio is output as the roll angle error (.sub.error) 330.

(29) Attitude quaternion computation module 340 receives the current calculated roll angle phi 335 from the attitude integration module 225 along with the roll angle error .sub.error 330 and the new estimated values of pitch and yaw (.sub.est and .sub.est) 310 as well as the trigger command 345. Attitude quaternion computation module 340 then outputs the new attitude reset values (.sub.est, .sub.est and .sub.error) to the attitude integration module 225 to reset the current attitude.

(30) As mentioned above, the trigger for the attitude correction function 300 can be based on either or both of a time threshold or a roll rate threshold. A time threshold can be used to trigger the attitude correction function a predetermined time after launch. For example, the predetermined time may be a time at which stable, ballistic flight is expected to have been achieved. As a purely illustrative example, this may be a few seconds after launch once the rocket motor has ceased. Another possibility is to trigger the attitude reset function 300 when the roll rate drops below a predetermined threshold. The roll rate will typically be highest shortly after launch (e.g. due to firing with a rifled gun barrel) and will then decrease throughout the remainder of the flight. Stable flight may therefore readily be associated with a roll rate lower than a predetermined threshold value.

(31) Combinations of time and roll rate may also be used as a trigger for the attitude reset function 300. For example, the attitude reset may be triggered at the earliest to occur of the two thresholds, i.e. the earliest of a time threshold and a roll rate threshold. Alternatively, the attitude reset may be triggered a predetermined time after the predetermined roll rate has been achieved. It will be appreciated that other triggers for the attitude reset function may also be contemplated.

(32) After the attitude reset has taken place, the roll control process 400 is also initiated. This process 400 continuously monitors deviations to the Heading (yaw) and Elevation (pitch) of the projectile 100 and uses these to adjust the roll angle error and roll scale factor values and thereby continuously corrects the computed roll angle.

(33) Following the Attitude Reset, and up to the point that GPS aiding allows a different navigation regime to operate, the Roll Control function 400 maintains Bank (roll) angle accuracy using a closed-loop control process as illustrated in FIG. 4.

(34) A low-pass filter 315 is applied to the Heading (yaw) and Elevation (pitch) angles, and the ratio of their time derivatives (which may be calculated as an angle as described above in relation to the attitude reset function) is used to compute corrections to both roll angle and roll rate scale factor.

(35) The roll angle error .sub.error 330 is calculated in the same manner as described above in relation to the attitude reset function 300. It will be appreciated that the roll control process 400 may be implemented separately from the attitude reset function 300. Indeed, in some examples, the attitude reset function may not be used or required. However, where both the attitude reset function 400 and the roll control process 300 are both implemented as described here, the low pass filter 315, time derivative module 320 and ratio module 325 may be shared by the two processes 300, 400.

(36) The calculated roll angle error .sub.error 330 is used to calculate both the roll angle error 405 and the roll scale factor error 410. The output of roll gyroscope 210 is first multiplied by the roll scale factor error 410 as indicated at 415 and then the roll angle error 405 is added as indicated at 420.

(37) The roll angle error 405 and the roll scale factor error 410 are preferably produced from the calculated roll angle error .sub.error 330 by multiplying the calculated roll angle error .sub.error 330 by a gain or weighting factor, K1 425 for the roll angle error 405 and K2 430 for the scale factor error 410.

(38) Fixed gains (terms K1 and K2) have been found to provide sufficient stability, although a variable gain or Kalman filter implementation may also be employed in other examples. In yet further examples, the scale factor correction may also be factored by the reciprocal of the instantaneous roll rate.

(39) The roll correction process described above can be readily adapted to other types of projectiles. For example, projectiles which are not ground initialised (e.g. gun-launched) will not have self-derived velocity parameters available to compute the elevation and heading components of the attitude reset, in which case these may be substituted with either pre-defined or GPS measured velocity data.

(40) If the physical characteristics of the projectile result in non-planar trajectories where

(41) ${\tan }^{-1}\left(\frac{\frac{}{t}}{\frac{-}{t}}\right)$

(42) is non zero, a pre-defined offset may be used as an expected value to derive the Bank angle error correction. For example, in some examples it may be necessary to implement an offset that varies according to elapsed flight time.

(43) Note that the negative sign in the denominator of the above formula arises from the geometry and the direction in which angles are considered positive. An alternative, but equally useful definition would yield no negative sign.

(44) As the roll correction process 400 is a closed loop, feedback system with the correction being derived from and fed back to the roll gyroscope output, providing the gain factors K1 and K2 are set appropriately, the system is able to converge on a stable state with a highly accurate attitude output, while still allowing inexpensive components to be used for the gyroscopes.

(45) The above examples make use of closed-loop control, i.e. the controller maintains a system output at some desired level by monitoring the difference between the actual output and the desired output. The difference between the two (the error) is continuously computed and some proportion of this is used in setting the level of control action required to drive the error towards zero. The proportion of the error that is used (the loop gain) is determined during system design using techniques based on control theory. Many control regimes exist, and they variously involve fixed or time-varying gain terms.

(46) In the above examples, the desired output of the closed-loop control system is a heading angle equal to the predicted heading angle, e.g. a rate of change of heading angle equal to zero in the case of an expected planar flight path.