Abstract:
A method of determining an orientation of a body. The method includes the steps of measuring an acceleration of the body in a first direction, measuring an acceleration of the body in a second direction different from the first direction, the first direction and the second direction defining a plane. Measuring an acceleration perpendicular to the plane in a coordinate system rotating about an axis perpendicular to the plane, and inferring the orientation of the body from the three measurements.

Description:
FIELD AND BACKGROUND OF THE INVENTION 
     The present invention relates to direction finding and, more particularly, to a method and apparatus for determining the orientation of a body, such as a vehicle or a theodolite, relative to a local geographic coordinate system. 
     Many methods and devices are known for determining the geographic heading of a body such as a vehicle. Magnetic compasses, which are among the oldest of such devices, find the direction of magnetic north, rather than geographic north, so their readings must be corrected for the local deviation of magnetic north from geographic north. More modern methods and devices include gyrocompasses and methods based on signals received from external transmitters such as the GPS satellite network. Gyrocompassing performance (accuracy and duration) is very sensitive to a gyroscope&#39;s drift. The process is very long, usually lasting between 10 minutes and 40 minutes. For high accuracy, a very expensive gyroscope must be used. The methods based on external signals are, by definition, not autonomous. 
     The object of the present invention is more general than merely finding local north. The object of the present invention is the determination of the full three-dimensional orientation of a body with respect to local geographic coordinates. The local geographic Cartesian coordinate system is the “local level local north” (LLLN) coordinate system, in which the x-axis points towards local north, the y-axis points towards local east and the z-axis points down. The body itself has its own Cartesian coordinate system, referred to herein as the “body coordinate system”. For example, a typical Cartesian coordinate system for a vehicle is x-axis=forward, y-axis=right and z-axis=down. The orientation of the body is defined in terms of the Euler angles through which a Cartesian coordinate system, initially aligned with the LLLN coordinate system, must be rotated to align that Cartesian coordinate system with the body coordinate system. These Euler angles are azimuth Ψ, pitch Θ and roll Φ. The rotations that bring the coordinate system, that initially is aligned with the LLLN coordinate system, into alignment with the body coordinate system, are: 
     1. rotation of the x and y axes by azimuth Ψ about the z-axis; 
     2. rotation of the x and z axes by pitch Θ about the intermediate y-axis to bring the x-axis into coincidence with the body x-axis; and 
     3. rotation about the x-axis by roll Φ to bring the y and z axes into coincidence with the body y and z axes. 
     Note that azimuth Ψ also is the angle between geographic north and the projection of the body x-axis in the horizontal (x-y) LLLN plane. 
     SUMMARY OF THE INVENTION 
     According to the present invention there is provided a method of determining an orientation of a body, including the steps of: (a) measuring an acceleration of the body in a first direction; (b) measuring an acceleration of the body in a second direction different from the first direction, the first direction and the second direction defining a plane; (c) measuring an acceleration perpendicular to the plane in a coordinate system rotating about an axis perpendicular to the plane; and (d) inferring the orientation of the body from the three measurements. 
     According to the present invention there is provided a method of determining an azimuth of a body, the azimuth being defined in a certain plane, the method including the steps of: (a) measuring an acceleration, perpendicular to the plane, in a coordinate system rotating about an axis perpendicular to the plane; and (b) inferring the orientation of the body from the measured acceleration. 
     According to the present invention there is provided an apparatus for determining an orientation of a body, including: (a) a first accelerometer, fixedly mounted with respect to the body, for measuring an acceleration of the body in a first direction; (b) a second accelerometer, fixedly mounted with respect to the body, for measuring an acceleration of the body in a second direction, the first and second directions defining a plane; (c) a third accelerometer for measuring an acceleration in a direction perpendicular to the plane; and (d) a mechanism for revolving the third accelerometer about an axis of revolution perpendicular to the plane. 
     According to the present invention there is provided an apparatus for determining an azimuth of a body, the azimuth being defined in a certain plane, including: (a) a first accelerometer, for measuring an acceleration perpendicular to the plane; and (b) a mechanism for revolving the first accelerometer about an axis of revolution perpendicular to the plane. 
     The present invention exploits the fact that the LLLN coordinate system is rotating. Specifically, the LLLN coordinate system rotates, along with the Earth, around the Earth&#39;s rotational axis, at a uniform (vectorial) angular velocity {right arrow over (Ω)}. This fact is exploited to determine the azimuth angle of a body that is stationary with respect to the Earth. The principle of the present invention, in its most general form, is to measure acceleration in the body z-direction, using an accelerometer that is caused to revolve, at a uniform angular speed, around an axis of revolution, that itself points in the body z-direction, at a fixed distance from the axis of revolution. Because this accelerometer is moving with respect to the Earth, the acceleration measured by this accelerometer includes a Coriolis force component. Meanwhile, the components of the gravitational acceleration of the body in the body x- and y- directions are measured by two other accelerometers. Because the body is stationary with respect to the Earth, the accelerations in the x- and y- directions do not include a Coriolis component. The pitch and roll are inferred from the measured x- and y- accelerations. The azimuth is inferred from the pitch, the roll, and the phase of the time-dependent part of the acceleration in the z-direction. In the special case of zero pitch and zero roll, only the acceleration in the z-direction is measured, and the phase of the time-dependent part of the acceleration in the z-direction is identical to the azimuth. 
     Preferably, to reduce sensitivity to common mode noise sources such as vibration, two accelerometers, on opposite sides of the axis of revolution, are used to measure the acceleration in the z-direction. The signals of the two accelerometers are subtracted. 
     The method of the present invention for defining the azimuth is relatively insensitive to common accelerometer errors such as constant bias, scale factor error, and misalignment errors. 
     Unlike magnetic compasses, the present invention is independent of the Earth&#39;s magnetic field. Unlike methods that rely on external transmitters such as GPS satellites, the present invention is fully autonomous. A drawback of the present invention relative to gyrocompasses is that the present invention works only for a stationary body; but bodies oriented using gyrocompasses must themselves be stationary when gyrocompassing is performed. 
    
    
     BRIEF DESCRIPTION OF THE DRAWINGS 
     The invention is herein described, by way of example only, with reference to the accompanying drawings, wherein: 
     FIG. 1 is a schematic plan view of an apparatus of the present invention; 
     FIG. 2 is a partial schematic side view of the apparatus of FIG.  1 . 
    
    
     DESCRIPTION OF THE PREFERRED EMBODIMENTS 
     The present invention is of a method and apparatus which can be used to determine the geographic orientation of a body such as a vehicle. 
     The principles and operation of orientation determination according to the present invention may be better understood with reference to the drawings and the accompanying description. 
     Referring now to the drawings, FIG. 1 is a schematic plan view of an apparatus  10  of the present invention. Apparatus  10  includes four single-component accelerometers  12 ,  14 ,  16  and  16 ′ for measuring acceleration in the directions of the three axes of the body coordinate system. These three axes are shown in FIG. 1 as an x-axis pointing upward in the plane of FIG. 1, a y-axis pointing rightward in the plane of FIG. 1, and a z-axis pointing downward into the plane of FIG.  1 . Accelerometer  12  is mounted rigidly with respect to the body, either within the body or on the body, to measure acceleration of the body in the body x-direction. Accelerometer  14  also is mounted rigidly with respect to the body, either within the body or on the body, to measure acceleration of the body in the body y-direction. Although it is not strictly necessary that the components of acceleration in the body x-y plane that are measured by accelerometers  12  and  14  be orthogonal components, it is preferable that these components be mutually orthogonal, as illustrated. Accelerometers  16  and  16 ′ are mounted rigidly on a table  20  that rotates at a constant angular speed ω around an axis of rotation  22  that is coincident with the z-axis. Thus, as table  20  rotates, accelerometers  16  and  16 ′ revolve about axis  22 , so that axis  22  also is an axis of revolution for accelerometers  16  and  16 ′. Accelerometers  16  and  16 ′ are mounted at a radial distance R from axis  22 . Accelerometers  16  and  16 ′ measure acceleration in the body z-direction. Signals from accelerometers  12 ,  14 ,  16  and  16 ′ are conveyed by conventional data links, represented by dashed lines in FIG. 1, to a conventional microprocessor-based data processing system  18 . Data processing system  18  uses the algorithms presented below to infer the Euler angles from these signals. 
     FIG. 2 is a partial schematic side view of apparatus  10 . In FIG. 2, the body x-axis points to the right, the body y-axis points upward out of the plane of the Figure, and the body z-axis points downwards in the plane of the Figure. As in FIG. 1, accelerometers  16  and  16 ′ are shown mounted rigidly on table  20 , on opposite sides of axis  22 . Table  20  rotates about axis  22  above a platform  24  that is fixedly mounted within the body. On the surface of table  20  that faces platform  24 , below accelerometer  16 , is mounted a light source  26  such as a LED. On the surface of platform  24  that faces table  20 , at distance R from axis  22  in the positive x-direction, is a photodetector  28 . Signals from photodetector  28  are conveyed to data processing system  18  by a conventional data link represented in FIG. 2 by a dashed line. Thus, every time accelerometer  16  and LED  26  pass photodetector  28 , a synchronization signal is sent to data processing system  18 . 
     The mechanism for rotating table  20  at a constant angular speed is conventional and need not be elaborated here. Suffice it to say that the synchronization signal is used both to control the angular speed of table  20  and to infer the angular positions, as functions of time, of accelerometers  16  and  16 ′. 
     For clarity of exposition, the present invention first will be explained on the assumption that the pitch Θ and the roll Φ both are zero. In this case accelerometers  12  and  14  are not needed. For further clarity, only the acceleration measured by accelerometer  16  will be considered initially. Acceleration is measured as a function of time t, starting with accelerometer  16  and LED  26  aligned with photodetector  28  at time t=0. Subsequently, accelerometer  16  and LED  26  are aligned with photodetector  28  at times t=2Nπ/ω, where N is an integer. 
     In LLLN coordinates, the Earth&#39;s angular rotation vector has only two nonzero components: an x (north) component Ω LN  and a z (down) component Ω LD . 
     Now define a third Cartesian coordinate system, referred to herein as the “table” coordinate system. The body coordinate system and the table coordinate system coincide at time t=0. Subsequently, the table coordinate system rotates with respect to the body coordinate system, about the coincident z-axes of the two coordinate systems, at an angular speed ω. In other words, the table coordinate system is fixed in table  20  and rotates with table  20 . In table coordinates, the position vector {right arrow over (r)} of accelerometer  16  is a constant:                r   .     =     (         R           0           0         )             (   1   )                                
     The specific force {right arrow over (a)} experienced by body at position {right arrow over (r)} in a coordinate system rotating at angular velocity. {right arrow over (α)} is (Bernard Etkin,  Dynamics of Atmosphere Flight , John Wiley and Sons, 1972, p. 123 equation 5.1,7):          a   →     =       F   →     +              α   →            t       ×     r   →       +     2                   α   →     ×            r   →            t         +       α   →     ×     (       α   →     ×     r   →       )       +            2          r   →              t   2                                  
     where {right arrow over (F)} is the linear acceleration due to external forces on the body. In the present case, all time derivatives of {right arrow over (r)} are zero, and {right arrow over (F)} is due to gravity. 
     The transformation from LLLN coordinates to table coordinates is effected by multiplying a vector represented in LLLN coordinates by a direction cosine matrix. Specifically, let x L , Y L  and Z L  be x, y and z coordinates, respectively, in the LLLN coordinate system and let x T , Y T  and Z T  be x, y and z coordinates, respectively, in the table coordinate system. Then:                (           x   T               y   T               z   T           )     =         C   _     T   L          (           x   L               y   L               z   L           )               (   3   )                                
     where C T   L  is the appropriate direction cosine matrix. With zero pitch Θ and zero roll Φ, C T   L  is a function only of azimuth Ψ, time t and angular speed ω:                  C   _     T   L     =     (           cos        (     Ψ   +     ω                 t       )             sin        (     Ψ   +     ω                 t       )           0             -     sin        (     Ψ   +     ω                 t       )               cos        (     Ψ   +     ω                 t       )           0           0       0       1         )             (   4   )                                
     Letting {right arrow over (Ω)} L  represent the angular velocity vector of the Earth in LLLN coordinates,            Ω   →     L     =     (           Ω   LN             0             Ω   LD           )                            
     it follows that the total angular velocity of accelerometer  16  in table coordinates is          α   →     =           C   _     T   L            Ω   →     L       +     ω   →                              
     where {right arrow over (ω)} is a vector of magnitude ω pointing in the body (or table) z-direction. The time derivative of α comes exclusively from the time derivative of C T   L :               α          t       =                C   _     T   L            t              Ω   →     L                              
     In LLLN coordinates, the only non-zero component of {right arrow over (F)} is the z-component of {right arrow over (F)}, which is equal to the acceleration due to gravity, g. It follows that in table coordinates,          F   →     =         C   _     T   L          (         0           0           g         )                              
     Inserting equations (4) through (8) in equation (2) gives, for the z-component of {right arrow over (a)} which is the component of {right arrow over (a)} measured by accelerometer  16 : 
     
       
           a   z   =g+A  cos(ω t +Ψ)  (9) 
       
     
     where A=2{right arrow over (ω)}Ω LN R+2Ω LD Ω LN R is independent of time. It is evident that the azimuth Ψ is the phase of the time-dependent term of a z . 
     The preferred method for obtaining Ψ from a z  is to multiply a z  by cos ωt and by sin ωt and to integrate these products numerically over one period, t=0 to t=2π/ω, of the rotation of table  20 . Defining:                a   s     =         ∫   0     2                   π   /   ω                a   z        sin                 ω                 t           t         =       -     A   2          sin                 Ψ               (   10   )                                
     and                a   c     =         ∫   0     2                   π   /   ω                a   z        cos                 ω                 t           t         =       A   2        cos                 Ψ               (   11   )                                
     it follows that 
     
       
         Ψ=− arc  tan( a   s   /a   c )  (12) 
       
     
     Because the azimuth Ψ is the phase of the time-dependent term of a z , it is critical that the integrations in equations (10) run from the exact start of one rotational period of table  20  to the exact end of that rotational period. Successive synchronization signals from photodetector  28  to data processing system  18  define successive starts and ends of periods of integration. 
     As noted above, for common mode rejection, it is preferable to measure a z  using two accelerometers  16  and  16 ′. The value of a z  measured by accelerometer  16  is given by equation (9) above. Because accelerometer  16 ′ is 180° away from accelerometer  16  on table  20 , the value of a z  measured by accelerometer  16 ′ is: 
     
       
           a   z   =g−A  cos(ω t +Ψ)  (13) 
       
     
     Subtracting the two signals from accelerometers  16  and  16 ′ before integrating as in equations (10) and (11) provides values of a s  and a c  that are relatively immune to common mode noises such as vibrations. 
     In the general case of non-zero pitch Θ and roll Φ, the signals a x  and a y  from accelerometers  12  and  14  respectively are due exclusively to gravity: 
     
       
           a   x   =−g  sin Θ  (14) 
       
     
     
       
           a   y   =g  cos Θ sin Φ  (15) 
       
     
     It follows that: 
     
       
         Θ= arc  sin( a   x   /g )  (16) 
       
     
     and 
      Φ= arc  sin ( a   y   /g  cos Θ)  (17) 
     The acceleration measured by accelerometer  16  is: 
     
       
           a   z   =g  cos Θ cos Φ+ A  cos(ω t +β)  (18) 
       
     
     where 
     
       
           A =2ωΩ BX   R +2Ω BZ Ω BX   R   (19) 
       
     
     and β is the angle between the body coordinates x-axis and the projection of the Earth&#39;s angular rotation velocity vector {right arrow over (Ω)} in the body x-y plane. In equation (19), Ω BX  is the x-component of {right arrow over (Ω)} in body coordinates: 
     
       
         Ω BX =cos Θ cos ψΩ LN −sin ΘΩ LD   (20) 
       
     
     and Ω BZ  is the z-component of {right arrow over (Ω)} in body coordinates: 
     
       
         Ω BZ =cos Φ sin Θ cos ΨΩ LN +sin Φ sin ΨΩ LN +cos Φ cos ΘΩ LD   (21) 
       
     
     It also will be necessary shortly to have the y-component of {right arrow over (Ω)} in body coordinates, Ω BY . 
     
       
         Ω BY =sin Φ sin Θ cos ΨΩ LN −cos Φ sin ΨΩ LN +sin Φ cos ΘΩ LD   (22) 
       
     
     Numerical manipulation of a z  as described above in equations (10)-(12) yields β in the general case instead of Ψ. 
     To obtain Ψ from Θ, Φ and β, consider the rotation of the body coordinates about the z-axis that would be necessary to bring the projection of {right arrow over (Ω)} onto the body x-y plane into coincidence with the new body x-axis. The y-component of the projection of {right arrow over (Ω)} onto the body x-y plane in this rotated body coordinate system would be identically zero. It follows that 
     
       
         Ω BX  sin β+Ω BY  cos β=0  (23) 
       
     
     Combining equations (20), (22) and (23) gives: 
     
       
           B  cos Ψ+ C  sin Ψ+ D= 0  (24) 
       
     
     where: 
     
       
           B =sin β cos Θ+cos βsin Φ sin Θ  (25) 
       
     
     
       
           C =−cos β cos Φ  (26) 
       
     
     and 
     
       
           D =(Ω LD /Ω LN )[cos β sin Φ cos Θ−sin β sin Θ]  (27) 
       
     
     Note that Ω LD /Ω LN  is the negative arctangent of the geographic latitude of the body. Using the following trigonometric identities:                cos                 Ψ     =       1   -       tan   2          (     Ψ   /   2     )           1   +       tan   2          (     Ψ   /   2     )                   (   28   )                 sin                 Ψ     =       2                   tan        (     Ψ   /   2     )           1   +       tan   2          (     Ψ   /   2     )                   (   29   )                                
     gives the following quadratic equation for tan(Ψ/2): 
     
       
         ( D−B )tan 2 (Ψ/2)+2 C  tan(Ψ/2)+ B+D= 0  (30) 
       
     
     whose solution is                tan        (     Ψ   /   2     )       =         -   C     ±         B   2     +     C   2     -     D   2             D   -   B               (   31   )                                
     In the most usual practical case of |Θ|&lt;30° and |Φ|&lt;30°, the sign ambiguity in equation (31) is resolved by selecting the value of Ψ which is closer to β. 
     While the invention has been described with respect to a limited number of embodiments, it will be appreciated that many variations, modifications and other applications of the invention may be made.