# Patent

## Title
Navigation apparatus

## Abstract

A navigation apparatus mounted on a vehicle is equipped with a first gyro sensor and a second gyro sensor arranged in symmetry with respect to a first axis in a travel direction of the vehicle and to a second axis that passes through the center of gravity of the vehicle, the first and second axes being perpendicular to each other, so that the first and second gyro sensors lean by the same angle from the second axis in opposing directions with respect to a third axis that is perpendicular to the first and second axes. A sensor output circuit is connected to each of the first and second gyro sensors, to output an output signal. A calculator calculates a yaw angular velocity of the vehicle based on the output signal.

## Background
CROSS-REFERENCE TO RELATED APPLICATIONS
This application is based on and claims the benefit of priority from the prior Japanese Patent Application No. 2004-287764 filed on Sep. 30, 2004, the entire content of which is incorporated herein by reference.
BACKGROUND OF THE INVENTION
The present invention relates to navigation apparatus to be mounted on vehicles such as automobiles and motorcycles. Particularly, this invention relates to a yaw angular-velocity sensor and calculator to be used for a navigation apparatus to detect and calculate a yaw angular velocity of a vehicle with an aid of a gyro sensor provided in an inertial positioning system.
A navigation apparatus to be mounted on a vehicle is generally equipped with an inertial positioning system. The system employs a gyro sensor (or an azimuth angle sensor) to detect yawing of a vehicle and a controller, such as, a micro computer, to calculate a yaw rate through a specific algorithm, the yaw rate being used as basic data for position estimation.
SUMMARY OF THE INVENTION
A purpose of the present invention is to provide a navigation apparatus equipped with a yaw angular-velocity sensor and calculator that has a simple structure but can precisely detect and calculate a yaw angular velocity of a vehicle.
The present invention provides a navigation apparatus mounted on a vehicle comprising: a first gyro sensor and a second gyro sensor arranged in symmetry with respect to a first axis in a travel direction of the vehicle and to a second axis that passes through the center of gravity of the vehicle, the first and second axes being perpendicular to each other, so that the first and second gyro sensors lean by the same angle from the second axis in opposing directions with respect to a third axis that is perpendicular to the first and second axes; a sensor output circuit connected to each of the first and second gyro sensors, to output an output signal; and a calculator to calculate a yaw angular velocity of the vehicle based on the output signal.
Moreover, the present invention provides a method of arranging gyro sensors in a vehicle comprising the steps of: providing a first gyro sensor and a second gyro sensor in symmetry with respect to a first axis in a travel direction of the vehicle and to a second axis that passes through the center of gravity of the vehicle, the first and second axes being perpendicular to each other; and fixing the first and second gyro sensors as leaning by the same angle from the second axis in opposing directions with respect to a third axis that is perpendicular to the first and second axes.
Furthermore, the present invention provides a method of calculating a yaw angular velocity of a vehicle comprising the steps of: obtaining variations X1 and X2 in sensor output voltages obtained from a first gyro sensor and a second gyro sensor, respectively: X1=A×ω×cos(θ−θ1), X2=A×ω×cos(θ+θ2) where A is constant and ω is a roll angle of the vehicle, the first and second gyro sensors being arranged in symmetry with respect to a first axis in a travel direction of the vehicle and to a second axis that passes through the center of gravity of the vehicle, the first and second axes being perpendicular to each other, so that the first and second gyro sensors lean by the same lean angles θ1 and θ2, respectively, from the second axis in opposing directions with respect to a third axis that is perpendicular to the first and second axes; obtaining a roll angle θ of the vehicle based on the variations X1 and X2 as follows: θ=atan((X2×cos θ1−X1×cos θ2)/(X2×sin θ1+X1×sin θ2))=atan((X2−X1)×cos θ0/((X2+X1)×sin θ0)) where θ1=θ2=θ0, and the sign “atan” means arctangent; and obtaining a yaw angular velocity ω of the vehicle based on the variations X1 and X2 as follows: ω=sqrt((X2−X1×cos(θ1+θ2))^2/(A^2×sin(θ1+θ2)^2)+X1^2)/A^2)=sqrt((X2−X1×cos(2×θ0))^2/(A^2×sin(2×θ0)^2)+X1^2/A^2) where the signs “sqrt” and “A^2” mean the square root and two squared, respectively.



BRIEF DESCRIPTION OF DRAWINGS
FIG. 1 is a perspective view illustrating movement of a vehicle;
FIG. 2 is a schematic view illustrating a principle of a gyro sensor;
FIG. 3 shows a block diagram of an embodiment of a navigation apparatus according to the present invention;
FIG. 4 is a schematic view illustrating an arrangement of two gyro sensors in a yaw angular-velocity sensor and calculator according to the present invention; and
FIG. 5 shows a block diagram of a sensor output circuit for the gyro sensors of the navigation apparatus according to the present invention.



DETAILED DESCRIPTION OF PREFERRED EMBODIMENT
Explained first are two basic techniques in yaw-angular velocity sensing and calculation.
[Technique I]
Illustrated in FIG. 1 (a perspective view) is movement of a vehicle (an automobile) 1, with a Y-axis along which the vehicle 1 is traveling, an X-axis that is a vertical axis passing through the center of gravity of the vehicle 1 and perpendicular to the Y-axis, and a Z-axis perpendicular to the X- and Y-axes.
An angel of the vehicle 1 that rotates about the X-axis is termed as a yaw angle. Moreover, an angel of the vehicle 1 that rotates about the Y-axis (leaning of the vehicle 1 in the lateral direction) is termed as a roll angle.
Here, the following expression is given:

X=A×ω×cos θ(A=constant)  (1)

where ω is a yaw angular velocity at which the vehicle 1 rotates about the X-axis, X is a variation of output from a gyro sensor while detecting the yaw angular velocity ω, and θ is a lean angle (roll angel) of the vehicle 1 in the lateral direction with respect to the Y-axis along which the vehicle 1 is traveling.

The yaw angular velocity ω is obtained with the variation X as follows:

X≅B×ω(B=constant)  (2)

where the sign “≅” means near-equal.

This is because, generally, when an instantaneous yaw angular velocity ω is obtained, the roll angle θ can be treated as almost constant (or θ=0) with almost no variation, thus cos θ=constant.
As illustrated in FIG. 2, a gyro sensor 2 is equipped with piezoelectric ceramic elements 3a and 3b that vibrate like a diapason.
When the gyro sensor 2 rotates about the X-axis at an angular velocity ω, Coriolis force appears perpendicularly to the vibration of the piezoelectric ceramic elements 3a and 3b. The force strains the elements 3a and 3b, which is electrically detected. The detected electric signal is output through a sensor output circuit as a sensor-output voltage which is in proportion to the angular velocity ω.
[Technique II]
For more precise yaw-angular velocity sensing and calculation, two gyro sensors may be provided for the vehicle 1 (FIG. 1): one dedicated to detection of leaning in the lateral direction (roll angle); and the other dedicated to detection of yawing (yaw angle).
The first one is provided along the Y-axis around which leaning occurs whereas the second one the X-axis around which yawing occurs. Each gyro sensor is provided at 90 degrees to the corresponding axis.
Here, the following expressions are given:

X=A×ω×cos θ(A=constant)  (3)

Y=B×cos θ)(B=constant)  (4)

where X is a variation of output from the gyro sensor dedicated to detection of yawing, Y is a variation of output from the gyro sensor dedicated to detection of leaning, ω is a yaw angular velocity, and θ is leaning (roll angel) of the vehicle 1 in the lateral direction.

Leaning (roll angel) θ of the vehicle 1 in the lateral direction is obtained by the expression (4).
The yaw angular velocity ω is obtained by the following expression (5) which is given by the expressions (3) and (4).

X=C×ω×Y(C=constant)  (5)

The techniques explained as above have problems as follows:
In detail, “cos θ=constant” cannot be applied to the expression (1) or variation of cos θ cannot be ignored, which results in change in X based on ω and cos θ (the expression (2) is useless), when the technique I is used in yaw-angular velocity sensing and calculation for a vehicle, such as, a motorcycle. Because a motorcycle leans very much when it turns right or left and the lean angle is always different. Thus, a precise yaw angular velocity ω cannot be obtained based on a gyro output variation X without a lean angle (roll angel) θ.
The technique II using two gyro sensors dedicated to leaning and yawing, respectively, requires processing and training signals in learning processing (filtering, correction, etc.) for each gyro output, resulting in a complex and expensive system configuration.
A navigation apparatus equipped with a global positioning system (GPS) and a map database for map matching uses outputs of the system and database as training signals for learning (sensor correction) in the yaw direction in an inertial positioning system. However, if the technique II using two dedicated gyro sensors is applied to such apparatus employing map matching, it requires roll data for yaw calculation and also a learning system dedicated to learning processing to the output of a roll-dedicated gyro sensor.
An embodiment of a navigation apparatus according to the present invention will now be explained in detail.
FIG. 3 shows a block diagram of an embodiment of a navigation apparatus according to the present invention. FIG. 4 illustrates an arrangement of two gyro sensors according to the present invention. FIG. 5 shows a block diagram of a sensor output circuit for gyro sensors of the navigation apparatus according to the present invention.
A navigation apparatus 10 according to the present invention is mounted on the vehicle 1 (FIG. 1) such as an automobile or a motorcycle.
The navigation apparatus 10 is equipped with: a GPS receiver 11 (a major component in a global positioning system); a geomagnetic sensor 17, and gyro sensors 2A and 2B (the sensors 17, and 2A and 2B being azimuth sensors in an inertial positioning system); a vehicle speed sensor 16 for obtaining a distance signal in accordance with a travel distance of the vehicle 1, based on the wheel rotation number; a map database 12 (DVD-ROM, HDD or CD-ROM) for map matching and guidance map displaying; a display 13 (LED, etc.) for displaying guidance maps; an operation unit 14 with an operation switch, voice recognition equipment, etc.; and a controller 15 with a micro computer for computation in the global and inertial positioning systems and entire control of the navigation apparatus 10.
Preferably, the gyro sensors 2A and 2B have the substantially identical characteristics on, for example, sensitivity to angular velocity. The output voltage of a gyro sensor varies when the sensor detects angular velocity. Sensitivity [mV/(deg/sec)] indicates how much the output voltage of a gyro sensor varies when the sensor rotates at a given angular velocity [deg/sec)].
The controller 15 is equipped with a yaw angular-velocity sensor and calculator 18 for sensing and calculating a yaw angular velocity of the vehicle 1.
The yaw angular-velocity sensor and calculator 18 is equipped with sensor output circuits 9 (FIG. 5) each for picking up a varying electric signal from the corresponding gyro sensor 2A or 2B, and a calculator in which a CPU of the micro computer (controller 15) calculates a yaw angular velocity ω of the vehicle 1 through a specific algorithm pre-programmed in memory.
The gyro sensors 2A and 2B are arranged as illustrated in FIG. 4. In detail, the sensors 2A and 2B are arranged in symmetry with respect to the X- and Y-axes (FIG. 1) so that they lean by the same angles θ1 and θ2 (θ1=θ2=θ0), on the horizontal plane H (corresponding to the Z-axis) of the vehicle 1, from Xa- and Xb-axes, respectively, which are parallel to the X-axis and perpendicular to the horizontal plane H.
The gyro sensors 2A or 2B may be equipped with piezoelectric ceramic elements such as shown in FIG. 2. Or, they may be gas-rate or optical-fiber gyro sensors.
As shown in FIG. 5, each sensor output circuit 9 includes: an oscillator 4 for applying vibration to the corresponding gyro sensor 2A or 2B; a strain amplifier 6 for amplifying an output voltage from the sensor 2A or 2B, corresponding to the vibration; a synchronous detector 5 for detecting a slight electric signal from the amplified voltage; a low-pass filter (LPF) 7 for filtering out noises from the electric signal; and a DC amplifier 8 for amplifying the filtered electric signal as a sensor output voltage.
The present invention with the specific arrangements of the gyro sensors 2A and 2B shown in FIG. 4 achieves simple calculation of a lean angle (roll angel) θ of the vehicle 1 in the lateral direction with a specific algorithm disclosed below.
FIG. 4 depicts an X-Z plane between the X- and Z-axes (FIG. 1). In this figure, each of the angles θ1 and θ2 (θ1=θ2=θ0) of the gyro sensors 2A and 2B, respectively, is a lean angle (fixed angle) with respect to the X-axis. Each gyro sensor is provided as having a lean angle of 0 degrees with respect to the X-axis in an X-Y plane between the X- and Y-axes (FIG. 1) when the vehicle 1 is viewed along the travel direction (Y-axis).
Suppose that the vehicle 1 turns right or left, so that the gyro sensors 2A and 2B lean by a roll angle θ with respect to the X-axis, thus a yaw angular velocity ω is generated.
Variations X1 and X2 in sensor output voltages of the sensor output circuits 9 for the gyro sensors 2A and 2B having the fixed angles θ1 and θ2 (FIG. 4), respectively, are given as follows:

X1=A×ω×cos(θ−θ1)(A=constant)  (6)

X2=A×ω×cos(θ+θ2)(A=constant)  (7)

The expressions (6) and (7) give a roll angle θ as below when θ1=θ2=θ0:




  
    
      
        
          
            
              θ
              =
              
                atan
                ⁡
                
                  (
                  
                    
                      (
                      
                        
                          X
                          ⁢
                          
                            
                          
                          ⁢
                          2
                          ×
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ1
                        
                        -
                        
                          X
                          ⁢
                          
                            
                          
                          ⁢
                          1
                          ×
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ2
                        
                      
                      )
                    
                    /
                    
                      (
                      
                        
                          X
                          ⁢
                          
                            
                          
                          ⁢
                          2
                          ×
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          θ1
                        
                        +
                        
                          X
                          ⁢
                          
                            
                          
                          ⁢
                          1
                          ×
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          θ2
                        
                      
                      )
                    
                  
                  )
                
              
            
          
        
        
          
            
              =
              
                atan
                ⁡
                
                  (
                  
                    
                      (
                      
                        
                          X
                          ⁢
                          
                            
                          
                          ⁢
                          2
                        
                        -
                        
                          X
                          ⁢
                          
                            
                          
                          ⁢
                          1
                        
                      
                      )
                    
                    ×
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    
                      θ0
                      /
                      
                        (
                        
                          
                            (
                            
                              
                                X
                                ⁢
                                
                                  
                                
                                ⁢
                                2
                              
                              +
                              
                                X
                                ⁢
                                
                                  
                                
                                ⁢
                                1
                              
                            
                            )
                          
                          ×
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          θ0
                        
                        )
                      
                    
                  
                  )
                
              
            
          
        
      
    
    
      
        (
        8
        )
      
    
  




where the sign “atan” means arctangent.

Likewise, a yaw angular velocity ω is given as follows:




  
    
      
        
          
            
              
                ω
                =
                
                  
                    
                      sqrt
                      ⁡
                      
                        (
                        
                          
                            
                              
                                (
                                
                                  
                                    X
                                    ⁢
                                    
                                      
                                    
                                    ⁢
                                    2
                                  
                                  -
                                  
                                    X
                                    ⁢
                                    
                                      
                                    
                                    ⁢
                                    1
                                    ×
                                    
                                      cos
                                      ⁡
                                      
                                        (
                                        
                                          θ1
                                          +
                                          θ2
                                        
                                        )
                                      
                                    
                                  
                                
                                )
                              
                              ⋀
                            
                            ⁢
                            
                              2
                              /
                              
                                (
                                
                                  
                                    A
                                    ⋀
                                  
                                  ⁢
                                  2
                                  ×
                                  
                                    
                                      sin
                                      ⁡
                                      
                                        (
                                        
                                          θ1
                                          +
                                          θ2
                                        
                                        )
                                      
                                    
                                    ⋀
                                  
                                  ⁢
                                  2
                                
                                )
                              
                            
                          
                          +
                          
                            X
                            ⁢
                            
                              
                            
                            ⁢
                            
                              1
                              ⋀
                            
                            ⁢
                            2
                          
                        
                        )
                      
                    
                    /
                    
                      A
                      ⋀
                    
                  
                  ⁢
                  2
                
              
              )
            
          
        
        
          
            
              =
              
                sqrt
                (
                
                  
                    
                      
                        (
                        
                          
                            X
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                          -
                          
                            X
                            ⁢
                            
                              
                            
                            ⁢
                            1
                            ×
                            
                              cos
                              (
                              
                                2
                                ×
                                θ0
                              
                              )
                            
                          
                        
                        )
                      
                      ⋀
                    
                    ⁢
                    
                      2
                      /
                      
                        (
                        
                          
                            A
                            ⋀
                          
                          ⁢
                          2
                          ×
                          
                            
                              sin
                              ⁡
                              
                                (
                                
                                  2
                                  ×
                                  θ0
                                
                                )
                              
                            
                            ⋀
                          
                          ⁢
                          2
                        
                        )
                      
                    
                  
                  +
                  
                    X
                    ⁢
                    
                      
                    
                    ⁢
                    
                      1
                      ⋀
                    
                    ⁢
                    
                      2
                      /
                      
                        A
                        ⋀
                      
                    
                    ⁢
                    2
                  
                
                )
              
            
          
        
      
    
    
      
        (
        9
        )
      
    
  




where the signs “sqrt” and “^2” mean the square root and two squared, respectively.

As disclosed above, a roll angle θ and a yaw angular velocity ω are given with simple calculation employing the expressions (8) and (9), respectively, using the variations X1 and X2 in sensor output voltages of the sensor output circuits 9 for the gyro sensors 2A and 2B.
The algorithm disclosed above is programmed in a memory such as ROM in the micro computer of the controller 15 (FIG. 3).
As disclosed above, the present invention gives a roll angle θ and a yaw angular velocity ω with simple calculation and achieves high accuracy for an inertial positioning system.
Moreover, the present invention employs two gyro sensors, each for both leaning and yawing, i.e., not dedicated to leaning or yawing, thus achieving a simple system configuration with cost down.
The two gyro sensors are arranged in symmetry with the same lean angle, thus the present invention readily achieving a learning function for gyro-sensor output processing (filtering, correction, etc.).
Furthermore, not only automobiles, the present invention is applicable to navigation apparatus for motorcycles, personal watercrafts, motorboats, etc., which turn at a larger roll angle.

## Claims

1. A navigation apparatus mounted on a vehicle having a center of gravity comprising:
a first gyro sensor and a second gyro sensor arranged in symmetry with respect to a first axis in a travel direction of the vehicle and to a second axis that passes through the center of gravity of the vehicle, the first and second axes being perpendicular to each other, so that the first and second gyro sensors lean by the same angle from the second axis in opposing directions with respect to a third axis that is perpendicular to the first and second axes;
a sensor output circuit connected to each of the first and second gyro sensors, to output an output signal; and
a calculator to calculate a yaw angular velocity of the vehicle based on the output signal.

2. The navigation apparatus according to claim 1, wherein the calculator:
obtains variations X1 and X2 in output signals from the first and second gyro sensors, respectively, via the sensor output circuit, as follows:


X1 =A×ω×cos(θ−θ1)



X2 =A×ω×cos(θ+θ2)



where A is constant, ωis a roll angle of the vehicle, and θ1 and θ2 are angles by which the first and second gyro sensors lean from the second axis in the opposing directions, respectively;
obtains a roll angle θ of the vehicle based on the variations X1 and X2 as follows:





  θ
  =
  
    
      atan
      ⁡
      
        (
        
          
            (
            
              
                X
                ⁢
                
                  
                
                ⁢
                2
                ×
                cos
                ⁢
                
                  
                
                ⁢
                θ1
              
              -
              
                X
                ⁢
                
                  
                
                ⁢
                1
                ×
                cos
                ⁢
                
                  
                
                ⁢
                θ2
              
            
            )
          
          /
          
            (
            
              
                X
                ⁢
                
                  
                
                ⁢
                2
                ×
                sin
                ⁢
                
                  
                
                ⁢
                θ1
              
              +
              
                X
                ⁢
                
                  
                
                ⁢
                1
                ×
                sin
                ⁢
                
                  
                
                ⁢
                θ2
              
            
            )
          
        
        )
      
    
    ⁢
    
      

    
    ⁢
    
      
    
    =
    
      atan
      ⁡
      
        (
        
          
            (
            
              
                X
                ⁢
                
                  
                
                ⁢
                2
              
              -
              
                X
                ⁢
                
                  
                
                ⁢
                1
              
            
            )
          
          ×
          cos
          ⁢
          
            
          
          ⁢
          
            θ0
            /
            
              (
              
                
                  (
                  
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                    +
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      1
                    
                  
                  )
                
                ×
                sin
                ⁢
                
                  
                
                ⁢
                θ0
              
              )
            
          
        
        )
      
    
  




where θ1 =θ2 =θ0, and the sign “atan” means arctangent; and
obtains a yaw angular velocity w of the vehicle based on the variations X1 and X2 as follows:





  
    
      
        
          ω
          =
          
            sqrt
            ⁢
            
              
                (
                
                  
                    
                      
                        (
                        
                          
                            X
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                          -
                          
                            X
                            ⁢
                            
                              
                            
                            ⁢
                            1
                            ×
                            
                              cos
                              ⁡
                              
                                (
                                
                                  θ1
                                  +
                                  θ2
                                
                                )
                              
                            
                          
                        
                        )
                      
                      ⋀
                    
                    ⁢
                    
                      2
                      /
                      
                        (
                        
                          
                            A
                            ⋀
                          
                          ⁢
                          2
                          ×
                          
                            
                              sin
                              ⁡
                              
                                (
                                
                                  θ1
                                  +
                                  θ2
                                
                                )
                              
                            
                            ⋀
                          
                          ⁢
                          2
                        
                        )
                      
                    
                  
                  +
                  
                    X
                    ⁢
                    
                      
                    
                    ⁢
                    
                      1
                      ⋀
                    
                    ⁢
                    2
                  
                
                )
              
              /
              
                A
                ⋀
              
            
            ⁢
            2
          
        
        )
      
    
  
  
    
      
        =
        
          sqrt
          (
          
            
              
                
                  (
                  
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                    -
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      1
                      ×
                      
                        cos
                        (
                        
                          2
                          ×
                          θ0
                        
                        )
                      
                    
                  
                  )
                
                ⋀
              
              ⁢
              
                2
                /
                
                  (
                  
                    
                      A
                      ⋀
                    
                    ⁢
                    2
                    ×
                    
                      
                        sin
                        ⁡
                        
                          (
                          
                            2
                            ×
                            θ0
                          
                          )
                        
                      
                      ⋀
                    
                    ⁢
                    2
                  
                  )
                
              
            
            +
            
              X
              ⁢
              
                
              
              ⁢
              
                1
                ⋀
              
              ⁢
              
                2
                /
                
                  A
                  ⋀
                
              
              ⁢
              2
            
          
          )
        
      
    
  




where the signs “sqrt” and “^2” mean the square root and two squared, respectively.

3. A computer implemented method of calculating a yaw angular velocity of a vehicle comprising the steps of:
obtaining variations X1 and X2 in sensor output voltages obtained from a first gyro sensor and a second gyro sensor, respectively:


X1 =A×ω×cos(θ−θ1)



X2 =A×ω×cos(θ+θ2)



where A is constant and ω is a roll angle of the vehicle, the first and second gyro sensors being arranged in symmetry with respect to a first axis in a travel direction of the vehicle and to a second axis that passes through the center of gravity of the vehicle, the first and second axes being perpendicular to each other, so that the first and second gyro sensors lean by the same lean angles θ1 and θ2 respectively, from the second axis in opposing directions with respect to a third axis that is perpendicular to the first and second axes;
obtaining a roll angle e of the vehicle based on the variations X1 and X2 as follows:





  θ
  =
  
    
      atan
      ⁡
      
        (
        
          
            (
            
              
                X
                ⁢
                
                  
                
                ⁢
                2
                ×
                cos
                ⁢
                
                  
                
                ⁢
                θ1
              
              -
              
                X
                ⁢
                
                  
                
                ⁢
                1
                ×
                cos
                ⁢
                
                  
                
                ⁢
                θ2
              
            
            )
          
          /
          
            (
            
              
                X
                ⁢
                
                  
                
                ⁢
                2
                ×
                sin
                ⁢
                
                  
                
                ⁢
                θ1
              
              +
              
                X
                ⁢
                
                  
                
                ⁢
                1
                ×
                sin
                ⁢
                
                  
                
                ⁢
                θ2
              
            
            )
          
        
        )
      
    
    ⁢
    
      

    
    ⁢
    
      
    
    =
    
      atan
      ⁡
      
        (
        
          
            (
            
              
                X
                ⁢
                
                  
                
                ⁢
                2
              
              -
              
                X
                ⁢
                
                  
                
                ⁢
                1
              
            
            )
          
          ×
          cos
          ⁢
          
            
          
          ⁢
          
            θ0
            /
            
              (
              
                
                  (
                  
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                    +
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      1
                    
                  
                  )
                
                ×
                sin
                ⁢
                
                  
                
                ⁢
                θ0
              
              )
            
          
        
        )
      
    
  




where θ1 =θ2 =θ , and the sign “atan” means arctangent; and
obtaining a yaw angular velocity ω of the vehicle based on the variations X1 and X2 as follows:





  
    
      
        
          ω
          =
          
            sqrt
            ⁢
            
              
                (
                
                  
                    
                      
                        (
                        
                          
                            X
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                          -
                          
                            X
                            ⁢
                            
                              
                            
                            ⁢
                            1
                            ×
                            
                              cos
                              ⁡
                              
                                (
                                
                                  θ1
                                  +
                                  θ2
                                
                                )
                              
                            
                          
                        
                        )
                      
                      ⋀
                    
                    ⁢
                    
                      2
                      /
                      
                        (
                        
                          
                            A
                            ⋀
                          
                          ⁢
                          2
                          ×
                          
                            
                              sin
                              ⁡
                              
                                (
                                
                                  θ1
                                  +
                                  θ2
                                
                                )
                              
                            
                            ⋀
                          
                          ⁢
                          2
                        
                        )
                      
                    
                  
                  +
                  
                    X
                    ⁢
                    
                      
                    
                    ⁢
                    
                      1
                      ⋀
                    
                    ⁢
                    2
                  
                
                )
              
              /
              
                A
                ⋀
              
            
            ⁢
            2
          
        
        )
      
    
  
  
    
      
        =
        
          sqrt
          (
          
            
              
                
                  (
                  
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                    -
                    
                      X
                      ⁢
                      
                        
                      
                      ⁢
                      1
                      ×
                      
                        cos
                        (
                        
                          2
                          ×
                          θ0
                        
                        )
                      
                    
                  
                  )
                
                ⋀
              
              ⁢
              
                2
                /
                
                  (
                  
                    
                      A
                      ⋀
                    
                    ⁢
                    2
                    ×
                    
                      
                        sin
                        ⁡
                        
                          (
                          
                            2
                            ×
                            θ0
                          
                          )
                        
                      
                      ⋀
                    
                    ⁢
                    2
                  
                  )
                
              
            
            +
            
              X
              ⁢
              
                
              
              ⁢
              
                1
                ⋀
              
              ⁢
              
                2
                /
                
                  A
                  ⋀
                
              
              ⁢
              2
            
          
          )
        
      
    
  




where the signs “sqrt” and “^2” mean the square root and two squared, respectively.