U.S. patent application number 10/438915 was filed with the patent office on 2003-11-20 for attitude sensing apparatus for determining the attitude of a mobile unit.
Invention is credited to Fukuda, Masaru, Toda, Hiroyuki.
Application Number | 20030216864 10/438915 |
Document ID | / |
Family ID | 19194567 |
Filed Date | 2003-11-20 |
United States Patent
Application |
20030216864 |
Kind Code |
A1 |
Fukuda, Masaru ; et
al. |
November 20, 2003 |
Attitude sensing apparatus for determining the attitude of a mobile
unit
Abstract
The invention provides an attitude sensing apparatus for
determining the attitude of a mobile unit that can reliably
estimate an alignment angle between a GPS antenna coordinate system
and an IMU coordinate system with good accuracy regardless of the
magnitude of the alignment angle. Based on observation of the
difference between a GPS angular velocity and an IMU angular
velocity, an alignment angle estimating section estimates an
alignment angle and sensor errors. An alignment angle adder and a
sensor error adder cumulatively add and update the estimated
alignment angle and sensor errors, respectively. The estimated
alignment angle is fed back to an inertia data converter while the
estimated sensor errors are fed back to an inertia data correcting
section. The apparatus repeatedly performs estimation until the
estimated alignment angle gradually approaches a true alignment
angle by successively feeding back estimated values to a flow of
alignment angle estimation process.
Inventors: |
Fukuda, Masaru;
(Nishinomiya-shi, JP) ; Toda, Hiroyuki;
(Nishinomiya-shi, JP) |
Correspondence
Address: |
BIRCH STEWART KOLASCH & BIRCH
PO BOX 747
FALLS CHURCH
VA
22040-0747
US
|
Family ID: |
19194567 |
Appl. No.: |
10/438915 |
Filed: |
May 16, 2003 |
Current U.S.
Class: |
701/472 ;
342/357.31 |
Current CPC
Class: |
G01S 19/21 20130101;
G01C 21/165 20130101; G01S 19/26 20130101; G01S 19/49 20130101;
G01S 19/53 20130101; G01S 5/0247 20130101 |
Class at
Publication: |
701/216 ;
342/357.11; 342/357.14 |
International
Class: |
G01C 021/26; G01S
005/14 |
Foreign Application Data
Date |
Code |
Application Number |
May 16, 2002 |
JP |
2002-141576 |
Claims
What is claimed is:
1. An attitude sensing apparatus for determining the attitude of a
mobile unit by successively performing a process of estimating an
alignment angle between an antenna coordinate system of a GPS
attitude sensing system which determines the attitude of the mobile
unit in the antenna coordinate system and an IMU coordinate system
of an IMU attitude sensing system which determines the attitude of
the mobile unit in the IMU coordinate system and thereby
integrating the attitudes of the mobile unit determined in the
individual coordinate systems, said attitude sensing apparatus
comprising: an alignment angle estimator for successively
estimating an alignment angle to be used in a succeeding
calculation process based on the difference between inertia data
calculated from observations by said GPS attitude sensing system
and inertia data observed by said IMU attitude sensing system; and
an alignment angle adder for generating an updated alignment angle
by cumulatively adding the successively estimated alignment angle
and thereby updating the estimated alignment angle in sequence and
for outputting the updated alignment angle to said alignment angle
estimator; wherein the estimated alignment angle is successively
fed back for use in the alignment angle estimation process.
2. The attitude sensing apparatus according to claim 1, wherein
individual components .theta..sub.x, .theta..sub.y, .theta..sub.z
of the alignment angle satisfy the following conditions:
-85.degree..ltoreq..theta..sub.x.- ltoreq.85.degree.,
-85.degree..ltoreq..theta..sub.y.ltoreq.85.degree.,
-85.degree..ltoreq..theta..sub.z.ltoreq.90.degree.and the estimated
alignment angle is successively fed back together with the updated
alignment angle for use in the alignment angle estimation
process.
3. The attitude sensing apparatus according to claim 1 or 2,
wherein said alignment angle estimator successively estimates a
sensor error caused by an inertia sensor of said IMU attitude
sensing system from the estimated alignment angle, said attitude
sensing apparatus further comprising: a sensor error adder for
generating an updated sensor error by cumulatively adding the
successively estimated sensor error and thereby updating the
estimated sensor error in sequence and for outputting the updated
sensor error to said alignment angle estimator; wherein the updated
sensor error is successively fed back for use in the alignment
angle estimation process.
4. The attitude sensing apparatus according to one of claims 1 to 3
further comprising: a setter for setting a provisional alignment
angle upon installation of a GPS antenna of said GPS attitude
sensing system and an inertia sensor of said IMU attitude sensing
system; wherein initial conditions for said alignment angle
estimator are set by using the provisional alignment angle.
5. The attitude sensing apparatus according to one of claims 1 to
4, wherein the alignment angle estimation process is performed
until the alignment angle is finally determined.
Description
BACKGROUND OF THE INVENTION
[0001] 1. Field of the Invention
[0002] The present invention relates to an integrated GPS/IMU
attitude sensing apparatus for determining the attitude of a mobile
unit by integrating attitude data derived from the global
positioning system (GPS) and attitude data derived from an inertial
measurement unit (IMU). The GPS-derived attitude and the
IMU-derived attitude are hereinafter referred to as the GPS
attitude and the IMU attitude, respectively. More particularly, the
invention is concerned with an integrated GPS/IMU attitude sensing
apparatus designed to reliably estimate an alignment angle for
correcting misalignment between an antenna coordinate system and an
IMU coordinate system.
[0003] 2. Description of the Related Art
[0004] A GPS attitude sensing system is a known example of a system
for determining the heading and attitude of a mobile unit. The
conventional GPS attitude sensing system uses at least three GPS
antennas which are installed on a rigid mobile unit and are not
arranged in a line. The system receives radio signals from GPS
satellites through the individual GPS antennas of which positions
are known in a 3-axis Cartesian coordinate system, and observes
carrier phase differences between the radio signals received by the
individual antennas. The system then establishes an antenna
coordinate system by calculating relative positions of the GPS
antennas from observables of the carrier phase differences and
determines the heading and attitude of the mobile unit in a
specific reference coordinate system.
[0005] The conventional GPS attitude sensing system of this kind
can determine the attitude of the mobile unit by receiving a radio
signal from a GPS satellite. The GPS attitude sensing system
however has a problem that, if the radio signal from the GPS
satellite is interrupted or a cycle slip in carrier phase
observation occurs, it becomes impossible to observe carrier phase
differences, resulting in an inability to determine the attitude of
the mobile unit.
[0006] One known approach to the solution of this problem is
GPS/IMU integration technology, in which an inertial attitude
sensing system observes motion of a mobile unit by use of inertia
sensors (IMUs), such as angular velocity sensors or acceleration
sensors, and the amount of rotation of the coordinate system, or an
alignment angle for correcting misalignment between the attitude of
the mobile unit obtained from inertial observations and the
attitude of the mobile unit obtained by the GPS attitude sensing
system, is estimated to determine the correct attitude of the
mobile unit.
[0007] This conventional integration approach integrates attitude
observations obtained by the inertial attitude sensing system and
the GPS attitude sensing system to give high-precision attitude
measurements in a stable fashion. To achieve this, the conventional
integration approach involves a process of estimating an alignment
angle between the attitude of the mobile unit represented in an
inertial sensor coordinate system (IMU coordinate system) which is
obtained by the inertia sensors mounted on the individual axes of a
3-axis Cartesian coordinate system and the attitude of the mobile
unit represented in an antenna coordinate system which is obtained
by the GPS attitude sensing system.
[0008] Using this conventional approach, it is possible to observe
the motion of the mobile unit by the inertia sensors and
uninterruptedly outputs data on the attitude of the mobile unit
even when the radio signals from the GPS satellites are
interrupted, because attitude observations, if any interrupted due
to a loss of the radio signals, can be interpolated by the attitude
obtained by the inertia sensors.
[0009] The aforementioned conventional GPS/IMU integration approach
still has a problem to be solved, however, which is explained in
the following.
[0010] In the conventional GPS/IMU integration approach, it is
necessary to determine the amount of coordinate system rotation,
that is, an inherent alignment angle for correcting misalignment
between the antenna coordinate system and the IMU coordinate
system, at the time of installation of the GPS antennas and the
inertia sensors.
[0011] Conventionally, estimation of alignment angles is made by
one of the following methods.
[0012] A first method of alignment angle estimation is such that
multiple GPS antennas are installed while visually ensuring, for
instance, that one reference direction (axis) of the IMU coordinate
system of an inertial attitude sensing system including multiple
inertia sensors matches one reference direction (axis) of the
antenna coordinate system defined by the multiple GPS antennas.
Then, disregarding misalignment which may occur between the two
coordinate systems at installation, it is assumed that the two
coordinate systems have been exactly matched.
[0013] In this first method, misalignment of a few degree could
frequently occur between the two coordinate systems, so that
attitude observations are inaccurate and unstable even when the
GPS/IMU integration technology is used.
[0014] A second method of alignment angle estimation involves a
process of estimating the alignment angle by the following method
after setting the alignment angle by the aforementioned first
method.
[0015] It is assumed in the following explanation that the inertial
attitude sensing system employs angular velocity sensors, for
example.
[0016] An angular velocity (hereinafter referred to as the GPS
angular velocity) .omega..sub.g1 is calculated from the attitude of
the mobile unit obtained by the GPS attitude sensing system while,
at the same time, an angular velocity (hereinafter referred to as
the IMU angular velocity) (oil is determined by the angular
velocity sensors. By taking a difference between the GPS angular
velocity .omega..sub.g1 and the IMU angular velocity
.omega..sub.i1, a difference value .DELTA..sub.z1 is obtained and
an alignment angle .theta..sub.i1 is estimated from the difference
value .DELTA..sub.z1. The alignment angle .theta..sub.i1 thus
calculated is used to correct an IMU angular velocity
.omega..sub.i2 obtained in a succeeding measurement cycle. Then,
taking a difference between the IMU angular velocity .omega..sub.i2
and a GPS angular velocity .omega..sub.g2 obtained at the same
time, a new difference value .DELTA..sub.z2 is calculated, and from
the difference value .DELTA..sub.z2 thus obtained, a new alignment
angle .theta..sub.i2 is estimated and used for correcting an IMU
angular velocity obtained in a succeeding measurement cycle. This
calculation cycle is repeated thereafter such that the alignment
angle .theta..sub.i converges to a specific value, whereby a true
alignment angle is obtained.
[0017] The alignment angle does not converge due to nonlinear
property unless the alignment angle calculated as shown above is a
small angle of a few degrees. Therefore, the alignment angle needs
to be a small angle as an initial condition if the aforementioned
method of alignment angle estimation is to be used.
[0018] If the GPS antennas are to be installed onboard by a user,
for instance, the user must determine a GPS coordinate system
(antenna coordinate system) on site and install the GPS antennas at
precise positions in such a fashion that the GPS coordinate system
substantially matches the IMU coordinate system. From a practical
viewpoint, however, it is extremely difficult for the unskilled
user to make sure that the GPS antennas are installed with a minor
alignment angle between the GPS coordinate system and the IMU
coordinate system. Furthermore, if the user can not visually check
the locations of inertia sensors from installation sites of the GPS
antennas, it is impossible to align the GPS coordinate system with
the IMU coordinate system, so that it is absolutely difficult to
minimize the alignment angle.
SUMMARY OF THE INVENTION
[0019] In light of the foregoing problems of the prior art, it is
an object of the invention to provide an attitude sensing apparatus
for determining the attitude of a mobile unit that can reliably
estimate an alignment angle between a GPS antenna coordinate system
and an IMU coordinate system with good accuracy regardless of the
magnitude of the alignment angle.
[0020] According to a principal feature of the invention, an
attitude sensing apparatus for determining the attitude of a mobile
unit is provided with an alignment angle estimator and an alignment
angle adder. While cumulatively adding an alignment angle estimated
at specific intervals and thereby updating the estimated alignment
angle in sequence, the attitude sensing apparatus feeds back the
estimated alignment angle for use in an alignment angle estimation
process.
[0021] The alignment angle estimator including an inertia data
converter and an alignment angle estimating section converts
inertia data obtained by IMU inertia sensors from an IMU coordinate
system to an antenna coordinate system.
[0022] The alignment angle estimating section estimates the
alignment angle from the difference between the
coordinate-converted inertia data and inertia data calculated from
observations by a GPS attitude sensing system (hereinafter referred
to as GPS inertia data) at the specific intervals.
[0023] The estimated alignment angle is cumulatively added and
updated by the alignment angle adder at the aforementioned
intervals and output to the inertia data converter. The inertia
data converter coordinate-converts the inertia data using the
updated alignment angle obtained at a particular point in time.
[0024] The alignment angle estimating section successively converts
the inertia data using an alignment angle estimated from a
difference value obtained at a particular point in time. Then,
taking a difference between the inertia data thus converted and the
GPS inertia data obtained at the same point in time as the
converted inertia data, the alignment angle estimating section
estimates a new alignment angle.
[0025] The estimated alignment angle at a particular point in time
is estimated from the difference between the inertia data converted
by using the alignment angle updated in a preceding estimation
cycle and the GPS inertia data obtained at the same point in time
by repeatedly performing the aforementioned feedback operation.
Therefore, the value of the alignment angle estimated by the
alignment angle estimating section gradually decreases and
eventually approaches zero. At the same time, the updated alignment
angle produced by the alignment angle adder gradually approaches
its true value.
[0026] By using the alignment angle obtained by repeatedly
performed estimation and cumulative adding operations in the
aforementioned fashion, the attitude sensing apparatus of the
invention integrates the attitude of the mobile unit determined in
the antenna coordinate system and the attitude of the mobile unit
determined in the IMU coordinate system and gives high-precision
attitude measurements in a stable fashion.
[0027] According to the invention, GPS antennas are installed in
such a manner that individual components .theta..sub.x,
.theta..sub.y, .theta..sub.z of the alignment angle satisfy the
conditions -85.degree..ltoreq..theta..sub.x.ltoreq.85.degree.,
-85.degree..ltoreq..theta..sub.y.ltoreq.85.degree. and
-85.degree..ltoreq..theta..sub.z.ltoreq.90.degree., and both the
updated alignment angle and the estimated alignment angle are fed
back for use in the alignment angle estimation process.
[0028] By installing the GPS antennas such that the individual
components of the alignment angle fall within specific ranges and
using both the updated alignment angle and the estimated alignment
angle in the alignment angle estimation process in this way, it is
possible to simplify algorithm of the alignment angle estimation
process and increase processing speed for alignment angle
estimation, without C.sup.g.sub.i as initial values.
[0029] The attitude sensing apparatus of the invention further
includes a sensor error adder and an inertia data correcting
section to compensate for sensor errors contained in the inertia
data output from the IMU inertia sensors.
[0030] The alignment angle estimator estimates the sensor errors
from the difference in inertia data between the two attitude
sensing systems and outputs the sensor errors to the sensor error
adder. In the sensor error adder, the sensor errors are
cumulatively added and updated at the specific intervals like the
estimated alignment angle. The updated sensor errors are output to
the inertia data correcting section, which corrects inertia data
obtained in a succeeding measurement cycle by using the updated
sensor errors.
[0031] According to the invention, it is also possible estimate an
approximate alignment angle by visual observation, for instance,
before execution of the aforementioned alignment angle estimation
process and, using the alignment angle thus estimated, perform the
alignment angle estimation process after setting initial values of
a transformation matrix used by the alignment angle estimator.
[0032] Furthermore, since the alignment angle estimation process is
performed until the alignment angle approaches a correct estimated
value in this invention, it is possible to estimate the alignment
angle in a reliable fashion.
[0033] These and other objects, features and advantages of the
invention will become more apparent upon reading the following
detailed description along with the accompanying drawings.
BRIEF DESCRIPTION OF THE DRAWINGS
[0034] FIG. 1 is a diagram showing a relationship between an
antenna coordinate system and an IMU coordinate system;
[0035] FIG. 2 is a block diagram of an attitude sensing apparatus
according to a first embodiment of the invention particularly
showing its alignment angle estimation process flow;
[0036] FIG. 3 is a graphical representation of the result of
simulation of alignment angle estimation;
[0037] FIG. 4 is a graphical representation of the result of
simulation of alignment angle estimation performed on an actual
vessel; and
[0038] FIG. 5 is a block diagram of an attitude sensing apparatus
according to a second embodiment of the invention particularly
showing its alignment angle estimation process flow.
DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS OF THE
INVENTION
First Embodiment
[0039] An attitude sensing apparatus for determining the attitude
of a mobile unit according to a first embodiment of the invention
is now described with reference to FIGS. 1 and 2, of which FIG. 1
is a diagram showing a relationship between an antenna coordinate
system of a GPS attitude sensing system and an IMU coordinate
system of an IMU attitude sensing system, and FIG. 2 is a block
diagram of the attitude sensing apparatus of the first embodiment
particularly showing its alignment angle estimation process
flow.
[0040] Referring to FIG. 1, ANT0, ANT1 and ANT2 designate GPS
antennas, S.sub.x, S.sub.y and S.sub.z designate angular velocity
sensors which are used as inertia sensors, x.sup.g, y.sup.g and
z.sup.g designate the antenna coordinate system, x.sup.i, y.sup.i
and z.sup.i designate the IMU coordinate system, and C.sup.g.sub.i
is a transformation matrix used for converting coordinates in the
IMU coordinate system to corresponding coordinates in the antenna
coordinate system.
[0041] Referring to FIG. 2, the reference numeral 101 designates an
IMU angular velocity calculating section of the IMU attitude
sensing system, the reference numeral 102 designates an inertia
data correcting section, the reference numeral 103 designates an
inertia data converter, the reference numeral 104 designates a GPS
attitude calculating section of the GPS attitude sensing system,
the reference numeral 105 designates a GPS angular velocity
calculating section of the GPS attitude sensing system, the
reference numeral 106 designates an alignment angle estimating
section, the reference numeral 107 designates an alignment angle
adder, and the reference numeral 108 designates a sensor error
adder. The inertia data correcting section 102, the inertia data
converter 103 and the alignment angle estimating section 106
together constitute an alignment angle estimator mentioned in the
claims of this invention.
[0042] The three GPS antennas ANT0, ANT1, ANT2 are installed on the
mobile unit in a manner that they are not arranged in a straight
line as shown in FIG. 1. In the illustrated example, the GPS
antenna ANT0 is located at the origin of the antenna coordinate
system and the other two GPS antennas ANT1 and ANT2 are located at
coordinates (x.sub.1, y.sub.1, z.sub.1) and (x.sub.2, Y.sub.2,
z.sub.2), respectively. The angular velocity sensors S.sub.x,
S.sub.y, S.sub.z are mounted on the individual axes x.sup.g,
y.sup.g, z.sup.g of the IMU coordinate system, respectively.
[0043] As depicted FIG. 1, the antenna coordinate system is rotated
by a specific angle from the IMU coordinate system. Assuming that
the coordinate system has been rotated about the z-, y- and x-axes
in this order, and expressing Euler angles by .theta..sub.x,
.theta..sub.y and .theta..sub.z, the transformation matrix
C.sup.g.sub.i is expressed as follows: 1 C i g = [ cos y sin z cos
y cos z - sin y sin x sin y cos z - cos x sin z sin x sin y cos z +
cos x cos z sin x cos y cos x sin y cos z + sin x sin z cos x sin y
sin z - sin x cos z cos x cos y ] ( 1 )
[0044] where .theta..sub.x, .theta..sub.y and .theta..sub.z in
equation (1) above are x, y and z components of an alignment angle
(hereinafter referred to as simply as the alignment angles of the
individual axes).
[0045] The antenna coordinate system and the IMU coordinate system
can be correlated with each other, or integrated, by estimating
these alignment angles through calculation.
[0046] A method of alignment angle estimation is now described in
detail with reference to FIG. 2. The angular velocity sensors
S.sub.x, S.sub.y, S.sub.z are used as inertia sensors as already
mentioned, and angular velocities measured by the angular velocity
sensors S.sub.x, S.sub.y, S.sub.z (or IMU angular velocities) are
used as inertia data in the following discussion of the
embodiment.
[0047] The IMU angular velocity calculating section 101 includes
the three angular velocity sensors S.sub.x, S.sub.y, S.sub.z
mounted on the three axes x.sup.g, y.sup.g, z.sup.g of the 3-axis
Cartesian IMU coordinate system shown in FIG. 1. Each of these
angular velocity sensors S.sub.x, S.sub.y, S.sub.z outputs IMU
angular velocity .omega..sub.im referenced to the IMU coordinate
system. An IMU attitude angle calculator (not shown) determines an
IMU attitude from the IMU angular velocity .omega..sub.im using a
known method.
[0048] Each of these angular velocity sensors S.sub.x, S.sub.y,
S.sub.z has as their inherent error factors a bias error
.DELTA..omega..sub.i and a scale factor error .DELTA.K.sub.s.
Therefore, the true value .omega..sub.i of the IMU angular velocity
.omega..sub.im is given by equation (2) below:
.omega..sub.im=.omega..sub.i+.DELTA..omega..sub.i+(.omega..sub.i+.DELTA..o-
mega..sub.i).DELTA.K.sub.s (2)
[0049] Assuming that the values of the terms of the second and
higher power of the aforementioned error are negligible, the true
value .omega..sub.i of the IMU angular velocity .omega..sub.im can
be expressed as follows:
.omega..sub.im.apprxeq..omega..sub.i+.DELTA..omega..sub.i+.omega..sub.i.DE-
LTA.K.sub.s (2')
[0050] Expressing the alignment angle for correcting misalignment
of the IMU coordinate system with respect to the antenna coordinate
system by .DELTA..theta..sup.i.sub.gi, a transformation matrix
C'.sup.g.sub.i for correcting the misalignment is given by equation
(3) below:
C'.sup.g.sub.i.apprxeq.[I-S(.DELTA..theta..sup.i.sub.gi)]C.sup.g.sub.i
(3)
[0051] where C.sup.g.sub.i is the true transformation matrix shown
in FIG. 1 and equation (1), .DELTA..theta..sup.i.sub.gi is a vector
of which x, y and z components are (.DELTA..theta..sub.x,
.DELTA..theta..sub.y, .DELTA..theta..sub.z), and
S(.DELTA..theta..sup.i.sub.gi) is an alternating matrix of the
alignment angle .DELTA..theta..sup.i.sub.gi and expressed as
follows: 2 S ( gi i ) = [ 0 - giz i giy i giz i 0 - gix i - giy i
gix i 0 ] ( 4 )
[0052] The inertia data converter 103 converts the IMU angular
velocity .omega..sub.im from the IMU coordinate system to the
antenna coordinate system using the aforementioned equation
(1).
[0053] Disregarding the terms of the second and higher power of the
error, IMU/GPS angular velocity .omega.'.sup.g.sub.i obtained by
converting the IMU angular velocity .omega..sub.im from the IMU
coordinate system to the antenna coordinate system is expressed as
follows from equations (2') and (3): 3 i 'g = i 'g im = [ I - S (
gi i ) ] C i g ( i + i + i k s ) C i g i + C i g i + C i g i k s -
S ( gi i ) C i g i ( 5 )
[0054] On the other hand, the GPS attitude calculating section 104
receives radio signals from GPS satellites through the GPS antennas
ANT0, ANT1, ANT2 shown in FIG. 1 and outputs a GPS attitude using a
known method. Using this GPS attitude, the GPS angular velocity
calculating section 105 calculates and outputs a GPS angular
velocity .omega..sub.gm. Since the actually observed GPS angular
velocity .omega..sub.gm contains an error .DELTA..sub.g, the true
value .omega..sub.g of the GPS angular velocity .omega..sub.gm is
given by equation (6) below:
.omega..sub.gm=.omega..sub.g+.DELTA..omega..sub.g (6)
[0055] Here, there is a relationship expressed by equation (7)
below between the true value .omega..sub.g of the GPS angular
velocity .omega..sub.gm and the true value .omega..sub.i of the IMU
angular velocity .omega..sub.im:
.omega..sub.g=C.sup.g.sub.i.omega..sub.i (7)
[0056] From equations (5), (6) and (7), the difference .DELTA.z
between the IMU/GPS angular velocity .omega.'.sup.g.sub.i and the
GPS angular velocity .omega..sub.gm is expressed by equation (8)
below: 4 z = gm - i 'g - i 'g gi 1 - C i g i - i 'g k s + g = H X +
v ( 8 )
[0057] where 5 H = [ 0 i z 'g i y 'g - C i g ( 1 , 1 ) - C i g ( 1
, 2 ) - C i g ( 1 , 3 ) - i x 'g 0 0 - i z 'g 0 i x 'g - C i g ( 2
, 1 ) - C i g ( 2 , 2 ) - C i g ( 2 , 3 ) 0 - i y 'g 0 i y 'g - i x
'g 0 - C i g ( 3 , 1 ) - C i g ( 3 , 2 ) - C i g ( 3 , 3 ) 0 0 - i
z 'g ] and ( 9 ) X = [ gix i giy i giz i x ' y ' ' z K sx ' K sy '
K sz ' ] ( 10 )
[0058] where .DELTA..theta..sup.i.sub.gix,
.DELTA..theta..sup.i.sub.giy and .DELTA..theta..sup.i.sub.giz are
alignment angles, .DELTA..omega.'.sub.x, .DELTA..omega.'.sub.y and
.DELTA..omega.'.sub.z are estimated bias errors of the angular
velocities measured by the angular velocity sensors S.sub.x,
S.sub.y, S.sub.z mounted on the z-, y- and x-axes of the IMU
coordinate system, .DELTA.K'.sub.sx, .DELTA.K'.sub.sy and
.DELTA.K'.sub.sz are estimated scale factor errors of the angular
velocities measured by the angular velocity sensors S.sub.x,
S.sub.y, S.sub.z mounted on the z-, y- and x-axes of the IMU
coordinate system, and .nu. is an observation error of the
difference .DELTA.z between the IMU/GPS angular velocity
.omega.'.sup.g.sub.i and the GPS angular velocity .omega..sub.gm,
respectively.
[0059] The IMU/GPS angular velocity .omega.'.sup.g.sub.i and the
GPS angular velocity .omega..sub.gm are individually sampled at
intervals of Tg and processed in synchronism with each other such
that the IMU/GPS angular velocity .omega.'.sup.g.sub.i and the GPS
angular velocity .omega..sub.gm observed at the same time are
processed together.
[0060] The alignment angle estimating section 106 receives the
difference .DELTA.z between the IMU/GPS angular velocity
.omega.'.sup.g.sub.i and the GPS angular velocity .omega..sub.gm
and estimates state variables of equation (10) above.
[0061] For example, the alignment angle estimating section 106
estimates the individual state variables during each successive
sampling period Tg by using a Kalman filter represented by equation
(11) below:
X(k+1)=.PHI.X(k)+w.sub.k (11)
[0062] where .PHI. is a state transition matrix, and w.sub.k=(0, 0,
0, .eta.x, .eta.y, .eta.z, 0, 0, 0).sup.T represents observation
noise.
[0063] The Kalman filter calculates estimated errors of a current
estimation cycle from those of a preceding estimation cycle at
specific intervals in such a manner that the mean square error of
the estimated errors is minimized. The Kalman filter repeatedly
performs this operation to determine a desired output.
[0064] Provided that the estimated bias error .DELTA..omega.'.sub.i
is .delta..DELTA..omega.'.sub.i and the estimated scale factor
error .DELTA.K'.sub.s is .delta..DELTA.K'.sub.s at a given point in
time, the estimated bias error .delta..DELTA..omega.'.sub.i and the
estimated scale factor error .delta..DELTA.K'.sub.s are input to
the sensor error adder 108. The sensor error adder 108 then adds
the estimated bias error .delta..DELTA..omega.'.sub.i and the
estimated scale factor error .delta..DELTA.K'.sub.s to the
estimated bias error .DELTA..omega.'.sub.i and the estimated scale
factor error .DELTA.K'.sub.s of the preceding estimation cycle as
shown by equations (12) below:
.DELTA..omega.'.sub.i=.DELTA..omega.'.sub.i+.delta..DELTA..omega.'.sub.i
.DELTA.K'.sub.s=.DELTA.K'.sub.s+.delta..DELTA.K'.sub.s (12)
[0065] The aforementioned mathematical operation is performed at
the intervals of Tg, each time .delta..DELTA..omega.'.sub.i and
.delta..DELTA.K'.sub.s are estimated. Both the estimated bias error
.DELTA..omega.'.sub.i and the estimated scale factor error
.DELTA.K'.sub.s are updated by cumulatively adding their values
over the successive sampling periods Tg.
[0066] The updated estimated bias error .DELTA..omega.'.sub.i and
estimated scale factor error .DELTA.K'.sub.s are output to the
inertia data correcting section 102. Then, the inertia data
correcting section 102 corrects the IMU angular velocity
.omega..sub.im obtained in a succeeding measurement cycle using the
updated estimated bias error .DELTA..omega.'.sub.i and estimated
scale factor error .DELTA.K'.sub.s.
[0067] By feeding back the estimated bias error
.DELTA..omega.'.sub.i and the estimated scale factor error
.DELTA.K'.sub.s in the aforementioned fashion, sensor errors
.delta..DELTA..omega.'.sub.i and .delta..DELTA.K'.sub.s estimated
by the alignment angle estimating section 106 at a particular point
in time are determined from the IMU angular velocity corrected by
the estimated value of a preceding estimation cycle and the GPS
angular velocity of a current estimation cycle. As a consequence,
the sensor errors estimated by the alignment angle estimating
section 106 gradually decrease each time they are updated and
eventually approach zero. On the other hand, the sensor error adder
108 cumulatively adds the sensor errors which are estimated
time-sequentially so that the sensor errors gradually approach
their true values.
[0068] The sensor errors gradually approach the true values as they
are repeatedly estimated in the aforementioned manner. The IMU
angular velocity is corrected by using such sensor errors to
gradually exclude the influence of the sensor errors with respect
to IMU angular velocity measurement.
[0069] The alignment angle adder 107 cumulatively adds the
alignment angle .DELTA..theta..sup.i.sub.gi estimated by the
alignment angle estimating section 106 over the successive sampling
periods Tg and generates an updated alignment angle
.theta..sup.i.sub.gi as shown by equation (13) below:
.theta..sup.i.sub.gi=.theta..sup.i.sub.gi+.DELTA..theta..sup.i.sub.gi
(13)
[0070] The updated alignment angle .theta..sup.i.sub.gi is output
to the inertia data converter 103, which sequentially calculates
and updates the transformation matrix C.sup.g.sub.i shown in
equation (1) using the updated alignment angle
.theta..sup.i.sub.gi.
[0071] By feeding back the updated alignment angle
.theta..sup.i.sub.gi in this fashion, the alignment angle
.theta..sup.i.sub.gi estimated at a particular point in time is
determined from the difference between the IMU angular velocity
coordinate-converted by using the updated alignment angle
.theta..sup.i.sub.gi of a preceding estimation cycle and the GPS
angular velocity obtained in the same estimation cycle. As a
consequence, the alignment angle .theta..sup.i.sub.gi estimated by
the alignment angle estimating section 106 gradually decreases and
eventually approach zero, and the estimated alignment angle
.DELTA..theta..sup.i.sub.gi gradually approaches its true
value.
[0072] FIG. 3 shows the result of simulation of alignment angle
estimation.
[0073] For the purpose of simulation, alignment angle estimation
was made on condition that the individual components of the
alignment angle between the antenna coordinate system and the IMU
coordinate system corresponded to roll angle, pitch angle and yaw
angle of the mobile unit, which were assumed to be 30.degree.,
50.degree.and 100.degree., respectively, their initial values of
estimation being 0.degree., and white noise was superimposed. Also,
the amplitudes and periods of the roll angle, pitch angle and yaw
angle, which were used as conditions for estimating the alignment
angle here, were set as shown in Table 1 below.
1TABLE 1 Component of alignment angle Amplitude Period Roll angle
4.degree. 4 sec Pitch angle 4.degree. 4 sec Yaw angle 30.degree. 15
sec
[0074] Although the individual components of the alignment angle
oscillate in an initial stage of estimation due to the influence of
the white noise, for instance, the oscillation gradually diminish
and the components of the alignment angle approach their true
values as shown in FIG. 3.
[0075] FIG. 4 shows the result of estimation of alignment angles
derived from angular velocities obtained by the angular velocity
sensors S.sub.x, S.sub.y, S.sub.z and the GPS antennas ANT0, ANT1,
ANT2 installed on a swing motion testing facility. In this
experimental testing of estimation, two coordinate systems (for the
IMU attitude sensing system and the GPS attitude sensing system)
were set up such that the roll angle was -90.degree. and the yaw
angle and pitch angle were 0.degree.. Also, yawing and pitching
were started 180 seconds after the beginning of testing, and
rolling was started 500 seconds after the beginning of testing as
swinging conditions of IMU unit.
[0076] As shown in FIG. 4, the individual components of the
alignment angle approach to values within a range of errors of
approximately 1.degree.. This indicates that the alignment angles
can be estimated in a reliable fashion by using the aforementioned
alignment angle estimation method no matter how large the alignment
angles may be.
[0077] The alignment angles for correcting misalignment between the
antenna coordinate system and the IMU coordinate system can be
precisely determined as seen above by the aforementioned method.
This means that the attitude of the mobile unit determined by the
GPS attitude sensing system and the attitude of the mobile unit
determined by the IMU attitude sensing system can be correlated
with each other, or integrated, with high precision by the
invention. In short, the invention makes it possible to
continuously determine the attitude of the mobile unit with high
precision in a manner unaffected by external conditions.
[0078] In the aforementioned simulation, the initial values of the
individual state variables in the inertia data correcting section
102, the inertia data converter 103, the alignment angle adder 107
and the sensor error adder 108 are set to all zeroes and the
initial value of the transformation matrix C.sup.g.sub.i is assumed
to be a unit matrix.
[0079] While estimation of the individual state variables are done
by using the Kalman filter in the present embodiment, it is also
possible to store as many differences .DELTA.z as necessary for
calculating the individual state variables and calculate the state
variables from these differences .DELTA.z using the least squares
method. In this case, however, it is to be noted that update
intervals of the individual state variables become equal to the
sampling intervals Tg multiplied by the number of the differences
.DELTA.z necessary for calculating the state variables.
[0080] In addition, although the alignment angles are estimated
taking into account the sensor errors and the scale factor error in
the foregoing embodiment, it is also possible to estimate the
alignment angles by using high-precision angular velocity sensors
or, depending on required accuracy of the alignment angle
estimation, by neglecting the aforementioned state variables.
Second Embodiment
[0081] An attitude sensing apparatus for determining the attitude
of a mobile unit according to a second embodiment of the invention
is now described with reference to FIG. 5.
[0082] FIG. 5 is a block diagram of the attitude sensing apparatus
of the second embodiment particularly showing its alignment angle
estimation process flow.
[0083] Although the attitude sensing apparatus of FIG. 5 has the
same configuration including the same circuit elements as the
attitude sensing apparatus of FIG. 2, the inertia data converter
103 of FIG. 5 performs a different mathematical operation compared
to the attitude sensing apparatus of FIG. 2 in converting IMU
angular velocities referenced to the IMU coordinate system to GPS
angular velocities referenced to the antenna coordinate system.
Specifically, the estimated alignment angle
.DELTA..theta..sup.i.sub.gi is fed back to the inertia data
converter 103 together with the updated alignment angle
.theta..sup.i.sub.gi.
[0084] Transformation matrix C.sup.g.sub.i must be approximated by
a unit matrix when the transformation matrix C.sup.g.sub.i is
unknown, because the transformation matrix C.sup.g.sub.i is
necessary to be known to use the equation (8) based on the equation
(3). For this purpose, the individual components .theta..sub.x,
.theta..sub.y, .theta..sub.z of the alignment angle constituting
individual elements of the transformation matrix C.sup.g.sub.i are
set to satisfy the following conditions:
-85.degree..ltoreq..theta..sup.i.sub.gix.ltoreq.85.degree.,
-85.degree..ltoreq..theta..sup.i.sub.giy.ltoreq.85.degree.,
-85.degree..ltoreq..theta..sup.i.sub.giz.ltoreq.90.degree.,
(14)
[0085] These conditions can be easily met by visually checking the
arrangement of the IMU coordinate system and the antenna coordinate
system.
[0086] It is possible to approximate the transformation matrix
C.sup.g.sub.i shown in equations (8) and (9) by a unit matrix with
the alignment angles in the aforementioned ranges, whereby
equations (8) and (9) are expressed by the following equations,
respectively: 6 z = gm - i 'g - i 'g gi 1 - i - i 'g k s + g = H X
+ v ( 15 ) H = [ 0 i z 'g i y 'g - 1 0 0 - i x 'g 0 0 - i z 'g 0 i
x 'g 0 - 1 0 0 - i y 'g 0 i y 'g - i x 'g 0 0 0 - 1 0 0 - i z 'g ]
( 16 )
[0087] When the conditions expressed by inequalities (14) above are
applied to the above equations, a maximum of only one element of
the actual transformation matrix C'.sup.g.sub.i calculated with the
equation (3) by C.sup.g.sub.i as the unit matrix has a sign (plus
or minus) differing from the corresponding element of the true
transformation matrix C.sup.g.sub.i among their all elements.
[0088] When the alignment angle is converted from the IMU
coordinate system to the antenna coordinate system by using the
actual transformation matrix C'.sup.g.sub.i, the magnitude of the
IMU/GPS angular velocity .omega.'.sup.g.sub.i would change.
However, a change in the plus/minus sign occurs in only one of x, y
and z components .omega.'.sup.g.sub.ix, .omega.'.sup.g.sub.iy,
.omega.'.sup.g.sub.iz of the IMU/GPS angular velocity
.omega.'.sup.g.sub.i.
[0089] In case that two components of angle velocity
.omega.'.sup.g.sub.i have unreversed plus/minus signs, the
estimated alignment angle .DELTA..theta..sup.i.sub.gi is calculated
in such a way that it approaches a true value. If the estimated
alignment angle .DELTA..theta..sup.i.sub.gi thus calculated is
cumulatively added in sequence and fed back for the conversion of
the angular velocity, the plus/minus sign of the one element of the
transformation matrix C'.sup.g.sub.i having the reversed plus/minus
sign is reversed again so that the estimated alignment angle
.DELTA..theta..sup.i.sub.gi approaches its true value. Since the
elements are corrected in this fashion, the true transformation
matrix C.sup.g.sub.i can be substituted for the transformation
matrix C'.sup.g.sub.i, making it possible to exactly estimate the
alignment angle.
[0090] Since the transformation matrix C.sup.g.sub.i can be
approximated by the unit matrix as stated above by setting the
elements of the transformation matrix C.sup.g.sub.i to satisfy the
conditions expressed by inequalities (14), it is possible to
simplify algorithm of mathematical operation and reduce the time
required for estimation.
[0091] When the alignment angle is considerably large not to be
satisfied with the inequalities (14), it is possible to reduce the
estimation time by presetting initial values C.sup.g.sub.i(1) of
the transformation matrix C.sup.g.sub.i of the inertia data
converter 103 to satisfy the conditions of inequalities (14) as
shown in FIG. 5.
[0092] In the foregoing embodiments of the invention, operation for
estimating the alignment angle is performed until the alignment
angle approaches a correct estimated value, so that the alignment
angle can be estimated in a reliable fashion according to the
invention.
Additional Features
[0093] According to the invention, an estimated alignment angle is
cumulatively added and updated at specific intervals of estimation
and the updated alignment angle is fed back for use in alignment
angle estimation process. Since a new alignment angle is estimated
at the same estimation intervals from inertia data converted by the
alignment angle which was updated in a preceding estimation cycle,
the alignment angle successively estimated at the estimation
intervals gradually decreases and eventually approaches zero. By
performing such feedback operation in the alignment angle
estimation process, it is possible to reliably estimate an accurate
alignment angle regardless particularly of the magnitude of an
initial alignment angle
[0094] According to the invention, it is possible to simplify
algorithm of the alignment angle estimation process and reduce the
time required for alignment angle estimation by setting an initial
value of the alignment angle falling within a specific range.
[0095] According to the invention, it is also possible to exclude
sensor errors contained in the estimated alignment angle by feeding
back the successively estimated and cumulatively added sensor
errors. With this operation, it is possible to cause the updated
alignment angle obtained by cumulatively adding the alignment angle
estimated over the successive estimation intervals to approach a
true value with yet higher accuracy.
[0096] Furthermore, the invention makes it possible to reliably
estimate the alignment angle in the alignment angle estimation
process and further reduce the time required for alignment angle
estimation by setting an initial value of the alignment angle
obtained by visual observation, for instance, before execution of
the alignment angle estimation process.
[0097] Moreover, since the alignment angle estimation process is
performed until the alignment angle approaches a correct estimated
value in this invention, it is possible to estimate the alignment
angle in a reliable fashion.
* * * * *