# Patent

## Title
Vehicular behavior determination device and vehicular behavior determination method

## Abstract

According to a vehicular behavior determination device (100) of the present invention, an angular velocity vector ω(k) which represents an angular velocity around each of 3 axes of a vehicle (1) can be determined in high accuracy on the basis of a temporal variation mode of a posture vector psti(k) (i=x, y, z) which represents a posture of the vehicle (1) in a global coordinate system in an attempt to curtail a manufacture cost and size of the vehicle (1) by avoiding the usage of a 3-axis gyro sensor which is relatively expensive in price and large in size.

## Background
BACKGROUND OF THE INVENTION
1. Field of the Invention
The present invention relates to a vehicular behavior determination device and a vehicular behavior determination method.
2. Description of the Related Art
There has been disclosed, for example, in Japanese Patent Publication No. 9-42979, a technology for displaying accurately a vehicular position of a vehicle on a map in a navigation device, which attempts to improve a determination accuracy of the vehicular position by getting rid of effects caused by a road inclination through calculating an inclined angle of the road where the vehicle is running on the basis of outputs from a vehicular velocity sensor and a 3-axis acceleration sensor, and thereafter correcting an output from a gyro sensor on the basis of the inclined angle calculated.
However, in order to determine accurately a behavior of the vehicle, it is necessary to determine an angular velocity and an angular acceleration around each of a roll axis and a pitch axis in addition to the angular velocity and the angular acceleration around a yaw axis. On the other hand, a 3-axis gyro sensor which is used to determine the angular velocity and the like around each of the 3 axes is relatively expensive in price and large in size. Therefore, it is disadvantageous to mount on the vehicle the relatively expensive and large-sized 3-axis gyro sensor from the viewpoint of curtailing the manufacture cost and reducing the size of the vehicle.
SUMMARY OF THE INVENTION
The present invention has been accomplished in view of the aforementioned problem, and it is therefore an objective of the present invention to provide a vehicular behavior determination device and a vehicular behavior determination method which can determine in high accuracy an angular velocity and the like around each of 3 axes of a vehicle in an attempt to curtail a manufacture cost and size of the vehicle by avoiding the usage of a 3-axis gyro sensor which is relatively expensive in price and large in size.
The vehicular behavior determination device which determines a behavior of a vehicle according to a first aspect of the present invention includes: a first arithmetic processor, which determines a parameter representing the behavior of the vehicle; a second arithmetic processor, which calculates a posture vector representing a posture of the vehicle in a global coordinate system on the basis of a determination result determined by the first arithmetic processor; and a third arithmetic processor, which calculates an angular velocity vector representing an angular velocity of the vehicle around each of 3 orthogonal axes in a vehicular coordinate system on the basis of a temporal variation mode of the posture vector calculated by the second arithmetic processor.
According to the vehicular behavior determination device of the first aspect of the present invention, the angular velocity vector which represents the angular velocity of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system is determined on the basis of the temporal variation mode of the posture vector which represents the vehicular posture in the global coordinate system. Therefore, the angular velocity around each of the 3 orthogonal axes of the vehicle can be determined in high accuracy in an attempt to curtail a manufacture cost and size of the vehicle by avoiding the usage of a 3-axis gyro sensor which is relatively expensive in price and large in size.
The vehicular behavior determination device according to a second aspect of the present invention is dependent on the first aspect of the present invention, wherein the second arithmetic processor calculates an ith posture vector (i=1, 2, 3) which represents a posture of each of the 3 orthogonal axes in the global coordinate system as the posture vector; and the third arithmetic processor obtains a rotational main axis vector which represents a direction of a rotational main axis in the global coordinate system by combining 3 rotational axis vectors for making a previous ith posture vector match with a current ith posture vector, calculates a first-order angular velocity vector which represents an angular velocity of the vehicle around each of the 3 orthogonal axes in the global coordinate system on the basis of the rotational main axis vector, the previous ith posture vector and the current ith posture vector, and thereafter calculates a second-order angular velocity vector which represents the angular velocity of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system as the angular velocity vector on the basis of the first-order angular velocity vector and the ith posture vector.
According to the vehicular behavior determination device of the second aspect of the present invention, due to the consideration of the rotational main axis for matching the previous posture of the vehicular coordinate system to the current posture of the vehicular coordinate system in the global coordinate system, it is possible to determine in high accuracy the angular velocity of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system.
The vehicular behavior determination device according to a third aspect of the present invention is dependent on the first aspect or the second aspect of the present invention, wherein the third arithmetic processor calculates an angular acceleration vector which represents an angular acceleration of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system on the basis of the temporal variation mode of the angular velocity vector.
According to the vehicular behavior determination device of the third aspect of the present invention, it is possible to determine in high accuracy the angular acceleration around each of the 3 orthogonal axes of the vehicle on the basis of the angular velocity around each of the 3 orthogonal axes of the vehicle determined in high accuracy without using the 3-axis gyro sensor as aforementioned.
The vehicular behavior determination method according to a fourth aspect of the present invention includes steps of: a first arithmetic processing for determining a parameter representing the behavior of the vehicle; a second arithmetic processing for calculating a posture vector representing a posture of the vehicle in a global coordinate system on the basis of a determination result determined by the first arithmetic processing; and a third arithmetic processing for calculating an angular velocity vector representing an angular velocity of the vehicle around each of 3 orthogonal axes in a vehicular coordinate system on the basis of a temporal variation mode of the posture vector calculated by the second arithmetic processing.
According to the vehicular behavior determination method of the fourth aspect of the present invention, similar to the vehicular behavior determination device of the first aspect of the present invention, it is possible to determine in high accuracy the angular velocity around each of the 3 orthogonal axes of the vehicle without using the 3-axis gyro sensor.



BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 is an explanatory diagram illustrating a configuration of a vehicular behavior determination device.
FIG. 2 is a block chart illustrating a configuration of the vehicular behavior determination device.
FIG. 3 is a flow chart illustrating a vehicular behavior determination method.
FIG. 4 is flow chart illustrating the vehicular behavior determination method.
FIGS. 5(a) to 5(c) are explanatory diagrams illustrating a relation between a global coordinate system and a vehicular coordinate system, respectively.
FIG. 6 is an explanatory diagram illustrating a correction of a yaw rate.
FIGS. 7(a) and 7(b) are explanatory diagrams illustrating a calculation method of an angular velocity vector.



DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
Hereinafter, an embodiment of a vehicular behavior determination device and a vehicular behavior determination method of the present invention will be described with reference to the drawings. In the description below, a parameter with a subscript “g” denotes a parameter in a global coordinate system; similarly, a parameter with a subscript “c” denotes a parameter in a vehicular coordinate system.
Explanations will be given firstly to the vehicular behavior determination device of a first embodiment. As illustrated in FIGS. 1 and 2, the vehicular behavior determination device 100 is constituted as a part of a vehicular control apparatus 10 which is mounted in a vehicle 1. The vehicular control apparatus 10 is constituted from a computer or ECU (Electronic Control Unit, which is constituted of a CPU, a ROM and a RAM which are connected by a bus line or the like). The vehicle 1 is further mounted with a velocity sensor 101, a 2-axis acceleration sensor 102, and a 1-axis gyro sensor 103 (hereinafter, it will be referred to as a yaw rate sensor where appropriately). The velocity sensor 101 outputs an output signal according to a roll axial velocity vxc(k) in the roll axis direction (xc direction). The 2-axis acceleration sensor 102 outputs an output signal according to a roll axial component αxc(k) and a pitch axial component αyc(k) (a component in yc direction) of an acceleration α of the vehicle 1. The 1-axis gyro sensor 103 outputs an output signal according an angular velocity ωzc(k) around a yaw axis (zc axis) of the vehicle 1.
The vehicular control apparatus 10 controls a behavior of the vehicle 1 on the basis of a determination result by the vehicular behavior determination device 100 which includes a first arithmetic processor (operation processor) 110, a second arithmetic processor (operation processor) 120 and a third arithmetic processor (operation processor) 130. The first arithmetic processor 110 determines the roll axial velocity vxc(k), the roll axial component αxc(k) and the pitch axial component αyc(k) of the acceleration α, and the angular velocity ωzc(k) around the yaw axis of the vehicle 1, on the basis of the respective output signal from the velocity sensor 101, the 2-axis acceleration sensor 102 and the 1-axis gyro sensor 103. The second arithmetic processor 120 calculates a posture vector representing a posture of the vehicle 1 in the global coordinate system according to a posture operator on the basis of the determination result by the first arithmetic processor 110. The third arithmetic processor 130 calculates the angular velocity and the angular acceleration around the 3 axes of the vehicle 1, namely the roll axis, the pitch axis and the yaw axis, on the basis of the posture vector calculated by the second arithmetic processor 120 and the temporal variation of the posture vector. Each arithmetic processor is constituted from an arithmetic circuit, or a memory, and an arithmetic processing unit (operation processor, such as a CPU) which retrieves a program stored in the memory and performs an arithmetic processing (operation processing) according to the program. The determination result by the first arithmetic processor 110, the calculation result by the second arithmetic processor 120, and the calculation result by the third arithmetic processor 130 are stored respectively in a storing device such as a memory or the like.
Hereinafter, descriptions will be given to a vehicular behavior determination method performed by the vehicular behavior determination device 100 with the aforementioned configuration of the vehicle 1 in the first embodiment.
Firstly, the first arithmetic processor 110 performs a first processing which determines a parameter representing a behavior of the vehicle 1. Specifically, the first arithmetic processor 110 determines the roll axial velocity vxc(k) of the vehicle 1 at a timing k on the basis of the output signal from the velocity sensor 101 (FIG. 3/S012). The first arithmetic processor 110 determines the roll axial component αxc(k) of the acceleration α of the vehicle 1 at the timing k (FIG. 3/S014) and the pitch axial component αyc(k) of the acceleration α of the vehicle 1 at the timing k (FIG. 3/S016) on the basis of the output signal from the 2-axis acceleration sensor 102. The first arithmetic processor 110, on the basis of the output signal from the 1-axis gyro sensor 103, determines the angular velocity ωzc(k) around the yaw axis of the vehicle 1 at the timing k (FIG. 3/S018).
Subsequently, the second arithmetic processor 120, on the basis of the determination results obtained by the first arithmetic processor 110, performs a second processing which calculates a posture vector representing the posture of the vehicle 1 in the global coordinate system. Specifically, the second arithmetic processor 120 calculates an inclined angle θ(k) in the xc direction of the vehicular coordinate system with respect to the zg direction of the global coordinate system at the timing k (FIG. 3/S021). The inclined angle θ(k) is calculated according to the equation (101) on the basis of the roll axial component αxc(k) of the acceleration α of the vehicle 1 at the timing k, a temporal variation rate v′xc(k) of the roll axial velocity vxc(k) of the vehicle 1 at the timing k, a distance ly in y direction from a pivoting point to the 1-axis gyro sensor 103, and a temporal variation rate (angular acceleration) ω′zc(k) of the angular velocity (yaw rate) ωzc(k) around the yaw axis at the timing k. There is expressed in the equation (101) that the xc directional component αxc(k) of the acceleration α of the vehicle 1, as illustrated in FIG. 5(a), is equal to a sum of a xc directional component of the gravity, the centrifugal force, and the inertial force and the pivoting force due to acceleration and deceleration, respectively (herein, the xc directional component of the centrifugal force is zero).

θ(k)=π/2−arcsin((αxc(k)−vxc′(k)−lyωzc′(k))/g)

vxc′(k)=(vxc(k)−vxc(k−1))/Δt 

ωzc′(k)=(ωzc(k)−ωzc(k−1))/Δt  (101)

Thereafter, the second arithmetic processor 120 calculates an inclined angle φ(k) in the yc direction in the vehicular coordinate system with respect to the zg direction in the global coordinate system at the timing k (FIG. 3/S022). The inclined angle φ(k) is calculated according to the equation (102) on the basis of the y directional component αyc(k) of the acceleration α of the vehicle 1 at the timing k, the x directional component vxc(k) of the vehicle 1 at the timing k, a distance lx in the x direction from the pivoting point to the yaw rate sensor 103, the yaw rate ωzc(k) at the timing k, and a temporal variation rate ω′zc(k) of the yaw rate ωzc(k) at the timing k. There is expressed in the equation (102) that the yc directional component αyc(k) of the acceleration α of the vehicle 1, as illustrated in FIG. 5(b), is equal to a sum of a yc directional component of the gravity, the centrifugal force, and the inertial force and the pivoting force due to acceleration and deceleration, respectively (herein, the yc directional component of the inertial force due to the acceleration and deceleration is zero).

φ(k)=π/2−arcsin((αyc(k)−vxc(k)ωzc(k)−lxωzc′(k))/g)  (102)

Next, the second arithmetic processor 120 calculates an inclined angle ψ(k) in the zc direction in the vehicular coordinate system with respect to the zg direction in the global coordinate system at the timing k (FIG. 3/S023). The inclined angle ψ(k) is calculated on the basis of a geometrical relationship illustrated in FIG. 5(c). Specifically, the zc directional inclined angle ψ(k) is calculated according to the equation (103), on the basis of the xc directional inclined angle θ(k) and the yc directional inclined angle φ(k) of the vehicular coordinate system with respect to the zg direction in the global coordinate system at the timing k.

ψ(k)=arcsin √{square root over (cos2 θ(k)+cos2 φ(k))}{square root over (cos2 θ(k)+cos2 φ(k))}  (103)

Subsequently, the second arithmetic processor 120 calculates a first-order posture vector (represented by a quaternion) pst1i(k) (i=x, y, z) of the vehicle 1 according to the equation (104), on the basis of the above calculation results based on the determination results of the first arithmetic processor 110 and a first quaternion qt1(k) serving as a first rotational operator (FIG. 3/S024).

pst1i(k)≡qt1*(k)·psti(0)·qt1(k),

pstx(0)=(1,0,0,0),psty(0)=(0,1,0,0),pstz(0)=(0,0,1,0)  (104)

The first quaternion qt1(k) represents the rotation of the global coordinate system for matching the zg axis in the global coordinate system with the zc axis in the vehicular coordinate system. The rotation rotates around a unit normal vector n1=(n1x, n1y, n1z, 0) of a plane containing the zg axis of the global coordinate system and the zc axis of the vehicular coordinate system. The first quaternion qt1(k) is expressed with the equation (105) on the basis of the inclined angle ψ(k) in the zc direction of the vehicular coordinate system with respect to the zg direction of the global coordinate system at the timing k.

qt1(k)≡(qt1x(k),qt1y(k),qt1z(k),qt1w(k)),

qt1x(k)=n1x(k)sin(ψ(k)/2),qt1y(k)=n1y(k)sin(ψ(k)/2),

qt1z(k)=n1z(k)sin(ψ(k)/2),qt1w(k)=cos(ψ(k)/2)  (105)

Further, the second arithmetic processor 120 calculates a yaw rate ωzg(k) around the zg axis of the vehicle 1 in the global coordinate system (FIG. 3/S025). As illustrated in FIG. 6, if the yaw axis or the zc direction of the vehicular coordinate system is inclined at the inclined angle ψ(k) from the zg direction of the global coordinate system, the sensitivity of the yaw rate sensor 103 is reduced by cos ψ(k). Thereby, the (original) yaw rate ωzg(k) around the zg axis of the vehicle 1 in the global coordinate system is calculated by correcting the yaw rate ωzc(k) around the zc axis based on the output of the yaw rate sensor 103 on the basis of the inclined angle ψ(k) in the zc direction in the vehicular coordinate system with respect to the zg direction in the global coordinate system according to the equation (106).

ωzg(k)=ωzc(k)/cos ψ(k)  (106)

Thereafter, the second arithmetic processor 120 calculates an inclined angle ξ(k) in the xc direction in the vehicular coordinate system with respect to the xg direction in the global coordinate system at the timing k (FIG. 3/S026). The inclined angle ξ(k) is calculated according to the equation (107), on the basis of the inclined angle ξ(k−1) at a previous timing “k−1” (ξ(0)=0), and the angular velocity ωgz(k) of the vehicle 1 at the timing k.

ξ(k)=ξ(k−1)+ωgz(k)×Δt  (107)

Subsequently, the second arithmetic processor 120 calculates a second-order posture vector (represented by a quaternion) pst2i(k) (i=x, y, z) as an ith posture vector psti(k) representing the posture of the vehicle 1 in the global coordinate system according to the equation (108), on the basis of the above calculation results of the first-order posture vector pst1i(k) and the like based on the determination result of the first arithmetic processor 110, and a second quaternion qt2(k) as a second rotational operator (FIG. 3/S027). The ith posture vector psti(k) corresponds to a unit vector in the global coordinate system which represents a direction of an i axis in the vehicular coordinate system. Note that it is acceptable to calculate the ith posture vector psti(k) at one time according to a composite quaternion qt(k)=qt1(k)*qt2(k), instead of calculating the first-order posture vector pst1i(k) and the second vector pst2i(k) sequentially.




  
    
      
        
          
            
              
                
                  
                    pst
                    
                      2
                      ⁢
                      
                        
                      
                      ⁢
                      i
                    
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ≡
                
                ⁢
                
                  
                    
                      qt
                      2
                      *
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ·
                  
                    
                      pst
                      
                        1
                        ⁢
                        
                          
                        
                        ⁢
                        i
                      
                    
                    ⁡
                    
                      (
                      0
                      )
                    
                  
                  ·
                  
                    
                      qt
                      2
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                
              
            
          
          
            
              
                =
                
                ⁢
                
                  
                    
                      qt
                      2
                      *
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ·
                  
                    
                      qt
                      1
                      *
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ·
                  
                    
                      pst
                      i
                    
                    ⁡
                    
                      (
                      0
                      )
                    
                  
                  ·
                  
                    
                      qt
                      1
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ·
                  
                    
                      qt
                      2
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                
              
            
          
        
        ⁢
        
          

        
        ⁢
        
          (
          
            
              i
              =
              x
            
            ,
            y
            ,
            z
          
          )
        
      
    
    
      
        (
        108
        )
      
    
  




The second quaternion qt2(k) represents a rotation of the global coordinate system for matching the xg axis and the yg axis in the global coordinate system with the xc axis and yc axis in the vehicular coordinate system, respectively. The rotation rotates around a unit vector (0, 0, 1, 0) in the zg direction of the global coordinate system. The second quaternion qt2(k) is expressed by the equation (109) on the basis of the inclined angle ξ(k) in the xc direction of the vehicular coordinate system with respect to the xg direction of the global coordinate system at the timing k.

qt2(k)≡(qt2x(k),qt2y(k),qt2z(k),qt2w(k)),

qt2x(k)=0, qt2y(k)=0,

qt2z(k)=sin(ξ(k)/2), qt2w(k)=cos(ξ(k)/2)  (109)

Further, the third arithmetic processor 130 performs a third processing which determines an angular velocity and the like of the vehicle 1 around each of the 3 orthogonal axes in the vehicular coordinate system, on the basis of the calculation results by the second arithmetic processor 120. Firstly, the third arithmetic processor 130 calculates a first-order angular velocity vector ω1(k) which represents the angular velocity of the vehicle 1 around each of the 3 orthogonal axes in the global coordinate system on the basis of the temporal variation mode of the ith posture vector psti(k) (FIG. 4/S032). Specifically, a rotational main axis vector n2(k) is calculated according to the equation (110). The rotational main axis vector n2(k) shows a direction of a rotational main axis in the global coordinate system and causes a previous posture vector pst1i(k−1) at the timing t=k−1 to match with a current posture vector psti(k) at the timing t=k.

n2(k)=Σi=x,y,z(psti(k−1)×psti(k))/|psti(k−1)×psti(k)|  (110)

Based on the above, the first-order angular velocity vector ω1(k) is calculated according to the equation (111), on the basis of a rotation angle Θ1(k) of the posture vector psti(k) around the rotational main axis n2(k), as illustrated in FIG. 7(a).




  
    
      
        
          
            
              
                
                  ω
                  1
                
                ⁡
                
                  (
                  k
                  )
                
              
              =
              
              ⁢
              
                (
                
                  
                    
                      ω
                      
                        1
                        ⁢
                        
                          
                        
                        ⁢
                        x
                      
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ,
                  
                    
                      ω
                      
                        1
                        ⁢
                        
                          
                        
                        ⁢
                        y
                      
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ,
                  
                    
                      ω
                      
                        1
                        ⁢
                        
                          
                        
                        ⁢
                        z
                      
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ,
                  0
                
                )
              
            
          
        
        
          
            
              
                =
                
                ⁢
                
                  
                    
                      ω
                      n
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ⁢
                  
                    (
                    
                      
                        
                          n
                          
                            2
                            ⁢
                            
                              
                            
                            ⁢
                            x
                          
                        
                        ⁡
                        
                          (
                          k
                          )
                        
                      
                      ,
                      
                        
                          n
                          
                            2
                            ⁢
                            
                              
                            
                            ⁢
                            y
                          
                        
                        ⁡
                        
                          (
                          k
                          )
                        
                      
                      ,
                      
                        
                          n
                          
                            2
                            ⁢
                            
                              
                            
                            ⁢
                            z
                          
                        
                        ⁡
                        
                          (
                          k
                          )
                        
                      
                      ,
                      0
                    
                    )
                  
                
              
              ,
            
          
        
        
          
            
              
                
                  
                    ω
                    n
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                =
                
                ⁢
                
                  
                    
                      Θ
                      ⁡
                      
                        (
                        k
                        )
                      
                    
                    /
                    Δ
                  
                  ⁢
                  
                    
                  
                  ⁢
                  t
                
              
              ,
            
          
        
        
          
            
              
                
                  Θ
                  ⁡
                  
                    (
                    k
                    )
                  
                
                =
                
                ⁢
                
                  avg
                  ⁡
                  
                    (
                    
                      
                        Θ
                        i
                      
                      ⁡
                      
                        (
                        k
                        )
                      
                    
                    )
                  
                
              
              ,
              
                
                  max
                  ⁡
                  
                    (
                    
                      
                        Θ
                        i
                      
                      ⁡
                      
                        (
                        k
                        )
                      
                    
                    )
                  
                
                ⁢
                
                  
                
                ⁢
                or
                ⁢
                
                  
                
                ⁢
                
                  min
                  ⁡
                  
                    (
                    
                      
                        Θ
                        i
                      
                      ⁡
                      
                        (
                        k
                        )
                      
                    
                    )
                  
                
              
              ,
            
          
        
        
          
            
              
                
                  
                    Θ
                    i
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                =
                
                ⁢
                
                  arc
                  ⁢
                  
                    
                  
                  ⁢
                  cos
                  ⁢
                  
                    
                      {
                      
                        
                          
                            n
                            2
                          
                          ⁡
                          
                            (
                            k
                            )
                          
                        
                        ×
                        
                          
                            pst
                            i
                          
                          ⁡
                          
                            (
                            
                              k
                              -
                              1
                            
                            )
                          
                        
                      
                      )
                    
                    ·
                    
                      (
                      
                        
                          
                            n
                            2
                          
                          ⁡
                          
                            (
                            k
                            )
                          
                        
                        ×
                        
                          
                            pst
                            i
                          
                          ⁡
                          
                            (
                            k
                            )
                          
                        
                      
                      )
                    
                  
                
              
              }
            
          
        
      
    
    
      
        (
        111
        )
      
    
  




Wherein, “avg(Θi(k))” refers to an average value of 2 or 3 rotation angles (“0” is excluded) among the rotation angle Θi(k) around the rotational main axis of the ith posture vector psti(k) (i=x(=1), y(=2), z(=3)); “max(Θi(k))” refers to the maximum value of the three rotation angle Θi(k); and “min(Θi(k))” refers to the minimum value of the three rotation angle Θi(k) (“0” is excluded).
Thereafter, the third arithmetic processor 130 calculates a second-order angular velocity vector ω2(k) which represents an angular velocity of the vehicle 1 around each of the 3 orthogonal axes in the vehicular coordinate system as the angular velocity vector ω(k) (FIG. 4/S034) according to the equation (112), on the basis of the first-order angular velocity vector ω1(k) and the ith posture vector psti(k) which represents the posture of the vehicle 1 in the global coordinate system.




  
    
      
        
          
            
              
                
                  ω
                  2
                
                ⁡
                
                  (
                  k
                  )
                
              
              =
              
              ⁢
              
                (
                
                  
                    
                      ω
                      
                        2
                        ⁢
                        
                          
                        
                        ⁢
                        x
                      
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ,
                  
                    
                      ω
                      
                        2
                        ⁢
                        
                          
                        
                        ⁢
                        y
                      
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ,
                  
                    
                      ω
                      
                        2
                        ⁢
                        
                          
                        
                        ⁢
                        z
                      
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ,
                  0
                
                )
              
            
          
        
        
          
            
              
                
                  ω
                  
                    2
                    ⁢
                    
                      
                    
                    ⁢
                    i
                  
                
                ⁡
                
                  (
                  k
                  )
                
              
              =
              
              ⁢
              
                
                  
                    pst
                    i
                    *
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ⁢
                
                  
                    ω
                    1
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ⁢
                
                  
                    pst
                    i
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
              
            
          
        
        
          
            
              =
              
              ⁢
              
                
                  
                    qt
                    2
                    *
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ⁢
                
                  
                    qt
                    1
                    *
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ⁢
                
                  
                    ω
                    1
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ⁢
                
                  
                    qt
                    1
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ⁢
                
                  
                    qt
                    2
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
              
            
          
        
      
    
    
      
        (
        112
        )
      
    
  




The composite quaternion qt1(k) qt2(k) of the first quaternion qt1(k) and the second quaternion qt2(k) used in calculating the angular velocity vector ω(k) represent a rotation or coordinate transfer of the global coordinate system for making the global coordinate system and the vehicular coordinate system match each other, as mentioned above.
Further, the third arithmetic processor 130 calculates an angular acceleration vector ω′(k) according to the equation (114) on the basis of the previous angular velocity vector ω(k−1) at timing t=k−1 and the current angular velocity vector ω(k) at timing t=k.

ω′(k)=(ω(k)−ω(k−1))/Δt  (114)

The aforementioned process is repeated, thereby the angular velocity vector ω(k) and the angular acceleration vector ω′(k) around each of the 3 orthogonal axes, namely the roll axis, the pitch axis and the yaw axis is calculated or determined sequentially.
Hereinafter, descriptions will be given regarding a vehicular behavior determination device 100 of a second embodiment. The vehicular behavior determination device 100 of the second embodiment has the same configuration in hardware as the vehicular behavior determination device 100 of the first embodiment (refer to FIGS. 1 and 2), however differs in contents of the second arithmetic processing and the third arithmetic processing. In detail, firstly, the first arithmetic processor 110 performs the first arithmetic processing, which is equivalent to that in the first embodiment (refer to FIG. 3/S012 to S018).
Next, similar to that in the first embodiment, the second arithmetic processor 120 calculates the inclined angles θ(k), φ(k) and ψ(k) of the xc direction, yc direction and zc direction in the vehicular coordinate system with respect to the zg direction, respectively, (refer to FIG. 3/S021 to S023). Thereafter, the second arithmetic processor 120 calculates the first-order posture vector pst1(k) according to the equation (204) on the basis of the first quaternion qt1(k) (refer to the equation (105) and FIG. 3/S024). Although the coefficient ci may be set arbitrarily, it is set herein under a constraint condition that the first-order posture vector pst1(k) becomes a unit vector.

pst1(k)≡Σi=x,y,zciqt1*(k)psti(0)qt1(k)  (204)

Subsequently, the second arithmetic processor 120 calculates the second-order posture vector pst2(k) as the posture vector pst(k) according to the equation (208) by using the second quaternion qt2(k), on the basis of the first-order posture vector pst1(k) (refer to FIG. 3/S025 to S027).




  
    
      
        
          
            
              
                
                  
                    pst
                    
                      2
                      ⁢
                      
                        
                      
                      ⁢
                      i
                    
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                ≡
                
                ⁢
                
                  
                    
                      qt
                      2
                      *
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ⁢
                  
                    
                      pst
                      
                        1
                        ⁢
                        
                          
                        
                        ⁢
                        i
                      
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ⁢
                  
                    
                      qt
                      2
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                
              
            
          
          
            
              
                =
                
                ⁢
                
                  
                    
                      qt
                      2
                      *
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ⁢
                  
                    
                      qt
                      1
                      *
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ⁢
                  
                    
                      pst
                      i
                    
                    ⁡
                    
                      (
                      0
                      )
                    
                  
                  ⁢
                  
                    
                      qt
                      1
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                  ⁢
                  
                    
                      qt
                      2
                    
                    ⁡
                    
                      (
                      k
                      )
                    
                  
                
              
            
          
        
        ⁢
        
          

        
        ⁢
        
          (
          
            
              i
              =
              x
            
            ,
            y
            ,
            z
          
          )
        
      
    
    
      
        (
        108
        )
      
    
  




Further, the third arithmetic processor 130 calculates a preliminary angular velocity ω0(k) which represents the angular velocity around the rotational main axis of the posture vector pst(k) (refer to FIG. 4/S032). Specifically, as illustrated in FIG. 7(b), the preliminary angular velocity ω0(k), which represents the angular velocity around the rotational main axis of the posture vector pst(k), is calculated according to the equation (210) on the basis of a variation rate θ(k)/Δt of the angle θ(k) formed between the posture vector pst (k−1) at a timing “k−1” and the posture vector pst(k) at another different timing k. The preliminary angular velocity ω(k), as illustrated in FIG. 7(b), is defined as a 3 dimensional vector in a rotation coordinate system having the posture vector pst(k−1) at the timing “k−1” as a unit toward the positive direction in xn axis and a normal line of a plane including the posture vector pst(k−1) and pst(k) at two different timings “k−1” and k respectively as the zn axis (rotational main axis)




  
    
      
        
          
            
              
                
                  
                    ω
                    0
                  
                  ⁡
                  
                    (
                    k
                    )
                  
                
                =
                
                ⁢
                
                  (
                  
                    0
                    ,
                    0
                    ,
                    
                      
                        
                          θ
                          ⁡
                          
                            (
                            k
                            )
                          
                        
                        /
                        Δ
                      
                      ⁢
                      
                        
                      
                      ⁢
                      t
                    
                    ,
                    0
                  
                  )
                
              
              ,
            
          
        
        
          
            
              
                θ
                ⁡
                
                  (
                  k
                  )
                
              
              =
              
              ⁢
              
                arc
                ⁢
                
                  
                
                ⁢
                
                  cos
                  ⁡
                  
                    (
                    
                      
                        pst
                        ⁡
                        
                          (
                          
                            k
                            -
                            1
                          
                          )
                        
                      
                      ·
                      
                        pst
                        ⁡
                        
                          (
                          
                            k
                            +
                            1
                          
                          )
                        
                      
                    
                    )
                  
                
              
            
          
        
        
          
            
              =
              
              ⁢
              
                arc
                ⁢
                
                  
                
                ⁢
                
                  cos
                  (
                  
                    
                      ∑
                      
                        
                          i
                          =
                          x
                        
                        ,
                        y
                        ,
                        z
                      
                    
                    ⁢
                    
                      
                        
                          pst
                          i
                        
                        ⁡
                        
                          (
                          
                            k
                            -
                            1
                          
                          )
                        
                      
                      ·
                      
                        
                          pst
                          i
                        
                        ⁡
                        
                          (
                          k
                          )
                        
                      
                    
                  
                  )
                
              
            
          
        
      
    
    
      
        (
        210
        )
      
    
  




The third arithmetic processor 130 calculates the first angular velocity ω1(k) representing the angular velocity around each of the 3 axes of the posture vector in the global coordinate system according to the equation (211) on the basis of the preliminary angular velocity vector ω0(k) and the posture in the global coordinate system in a plane including the two posture vectors pst(k−1) and pst(k).

ω1(k)≡qt3*(k)ω0(k)qt3(k)  (211)

The third quaternion qt3(k) represents the rotation of zn axis in the rotation coordinate system for causing the zn axis in the rotation coordinate system to match with the zg axis in the global coordinate system. The rotation rotates around a unit normal vector n3=(n3x, n3y, n3z) of a plane containing the zn axis of the rotation coordinate system and the zg axis of the global coordinate system. The third quaternion qt3(k) is expressed in the equation (212) on the basis of the inclined angle η(k) in the zc direction of the vehicular coordinate system with respect to the zg direction of the global coordinate system at the timing k.

qt3(k)≡(qt3x(k),qt3y(k),qt3z(k),qt3w(k)),

qt3x(k)=n3x(k)sin(η(k)/2), qt3y(k)=n3y(k)sin(η(k)/2),

qt3z(k)=n3z(k)sin(η(k)/2), qt3w(k)=cos(η(k)/2) (212). Thereafter, the third arithmetic processor 130, similar to the first embodiment, calculates the angular velocity vector ω(k) and the angular acceleration vector ω′(k) representing the angular velocity around each of the 3 axes of the posture vector pst(k) in the vehicular coordinate system (refer to the equations (112) and (114), FIG. 4/S036 and). By repeating the aforementioned processing, the angular velocity and the angular acceleration around each of the 3 axes, namely the roll axis, the pitch axis and the yaw axis of the vehicle 1 are calculated or determined sequentially.
According to the vehicular behavior determination device performing the above-mentioned functions, the angular velocity vector ω(k) representing the angular velocity around each of the 3 axes of the vehicle 1 is determined on the basis of the temporal variation mode of the ith posture vector psti(k) (the first embodiment) or the posture vector pst(k) (the second embodiment) representing the posture of the vehicle 1 in the global coordinate system without using a 3-axis gyro sensor which is relatively expensive in price and large in size so as to curtail the manufacture cost and size of the vehicle 1 (refer to FIG. 4/S032 and S034, FIGS. 7(a) and 7(b)). Moreover, the angular acceleration vector ω′(k) around each of the 3 axes of the vehicle 1 may be determined in high accuracy on the basis of the angular velocity vector ω(k) (refer to FIG. 4/S036).
Further, it is acceptable to express the posture vector and the angular velocity vector which represent the posture of the vehicle 1 in the global coordinate system with a 3 dimensional vector which is mathematically equivalent to the quaternion as an alternate to the quaternion. It is also acceptable to use a matrix which is mathematically equivalent to the quaternion as the posture operator as an alternate to the quaternion in determining the posture of the vehicle 1. Specifically, a posture vector P(k) of the vehicle 1 at the timing k is estimated according to the equation (132) by using a first rotation matrix Q1(k) and a second rotation matrix Q2(k).

P(k)=(Px(k),Py(k),Pz(k))=Q2(k)·Q1(k)·P(0),

Pi(k)=t(pi1(k),pi2(k),pi3(k)),

Px(0)=t(1, 0, 0), Py(0)=t(0, 1, 0), Pz(k)=t(0, 0, 1) (132) (Wherein, a matrix with a subscript “t” refers to a transposed matrix.)
The first rotation matrix Q1(k) represents a rotation around the yg axis in the global coordinate system for causing the zg axis in the global coordinate system to match with the zc axis in the vehicular coordinate system. The first rotation matrix Q1(k) is expressed by the equation (134).

Q1(k)≡(Q11(k),Q12(k),Q13(k)),

Q11=t(1−2(qt1y2+qt1z2),2(qt1xqt1y−qt1zqt1w),2(qt1zqt1x+qt1wqt1y)),

Q12=t(2(qt1xqt1y+qt1zqt1w),1−2(qt1z2+qt1x2),2(qt1yqt1x−qt1wqt1x)),

Q13=t(2(qt1zqt1x−qt1wqt1y),2(qt1yqt1z+qt1wqt1x),1-2(qt1y2+qt1z2))  (134)

The second rotation matrix Q2(k) represents a rotation around the zg axis in the global coordinate system for causing the xg axis and yg axis in the global coordinate system to match with the xc axis and yc axis in the vehicular coordinate system, respectively. The second rotation matrix Q2(k) is expressed by the equation (136) on the basis of the inclined angle ξ(k) in the xc direction in the vehicular coordinate system with respect to the xg direction in the global coordinate system at the timing k.

Q2(k)≡(Q21(k),Q22(k),Q23(k)),

Q21=t(cos ξ(k),−sin ξ(k),0),

Q22=t(sin ξ(k),cos ξ(k),0),

Q23=t(0,0,1)  (136)

According to the present embodiment, similar to that in the previous embodiment, the posture of the vehicle 1 can be estimated in high accuracy without using a 3-axis gyro sensor which is relatively expensive in price and large in size in an attempt to curtail the manufacture cost and size of the vehicle 1.
Although the present invention has been explained in relation to the preferred embodiments and drawings but not limited, it should be noted that other possible modifications and variations made without departing from the gist and scope of the invention will be comprised in the present invention. Therefore, the appended claims encompass all such changes and modifications as falling within the gist and scope of the present invention.

## Claims

1. A vehicular behavior determination device which determines a behavior of a vehicle, comprising:
a first arithmetic processor, which determines a parameter representing the behavior of the vehicle;
a second arithmetic processor, which calculates a posture vector representing a posture of the vehicle in a global coordinate system on the basis of a determination result determined by the first arithmetic processor; and
a third arithmetic processor, which calculates an angular velocity vector representing an angular velocity of the vehicle around each of 3 orthogonal axes in a vehicular coordinate system on the basis of a temporal variation mode of the posture vector calculated by the second arithmetic processor;
wherein the second arithmetic processor calculates an ith posture vector (i=1, 2, 3) which represents a posture of each of the 3 orthogonal axes of the vehicular coordinate system in the global coordinate system as the posture vector; and
the third arithmetic processor obtains a rotational main axis vector which represents a direction of a rotational main axis in the global coordinate system by combining 3 rotational axis vectors for making a previous ith posture vector match with a current ith posture vector, calculates a first-order angular velocity vector which represents an angular velocity of the vehicle around each of the 3 orthogonal axes in the global coordinate system on the basis of the obtained rotational main axis vector, the previous ith posture vector and the current ith posture vector, and thereafter calculates a second-order angular velocity vector which represents the angular velocity of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system as the angular velocity vector on the basis of the first-order angular velocity vector and the ith posture vector.

2. The vehicular behavior determination device according to claim 1, wherein the third arithmetic processor calculates an angular acceleration vector which represents an angular acceleration of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system on the basis of the temporal variation mode of the angular velocity vector.

3. The vehicular behavior determination device according to claim 1, wherein the third arithmetic processor calculates an angular acceleration vector which represents an angular acceleration of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system on the basis of the temporal variation mode of the angular velocity vector.

4. A vehicular behavior determination method of determining a behavior of a vehicle, comprising steps of:
a first arithmetic processing for determining a parameter representing the behavior of the vehicle;
a second arithmetic processing for calculating a posture vector representing a posture of the vehicle in a global coordinate system on the basis of a determination result determined by the first arithmetic processing; and
a third arithmetic processing for calculating an angular velocity vector representing an angular velocity of the vehicle around each of 3 orthogonal axes in a vehicular coordinate system on the basis of a temporal variation mode of the posture vector calculated by the second arithmetic processing;
wherein the second arithmetic processing comprises calculating an ith posture vector (i=1, 2, 3) which represents a posture of each of the 3 orthogonal axes of the vehicular coordinate system in the global coordinate system as the posture vector; and
the third arithmetic processing comprises obtaining a rotational main axis vector which represents a direction of a rotational main axis in the global coordinate system by combining 3 rotational axis vectors for making a previous ith posture vector match with a current ith posture vector, calculating a first-order angular velocity vector which represents an angular velocity of the vehicle around each of the 3 orthogonal axes in the global coordinate system on the basis of the obtained rotational main axis vector, the previous ith posture vector and the current ith posture vector, and thereafter calculating a second-order angular velocity vector which represents the angular velocity of the vehicle around each of the 3 orthogonal axes in the vehicular coordinate system as the angular velocity vector on the basis of the first-order angular velocity vector and the ith posture vector.

5. The vehicular behavior determination device according to claim 1, wherein the parameter is one or more of a roll axis velocity in the roll axis direction, a roll axial component of acceleration of the vehicle, a pitch axial component of acceleration of the vehicle, and an angular velocity around the yaw axis of the vehicle.