JP2014240266A - Sensor drift amount estimation device and program - Google Patents

Abstract

A drift amount of a sensor can be stably estimated regardless of a vehicle motion state. When a sensor drift amount of a sensor signal is taken into consideration, a sensor drift is calculated by a posture angle derivative calculated by a posture angle observer and a posture angle differential calculated by a motion equation derivative calculating unit. A value in which the sensor drift amount is taken into account in the relation that the value considering the amount becomes equal, the differential amount of the vehicle speed calculated by the slip angle observer 28, and the differential amount of the vehicle speed calculated by the motion equation differential amount calculating means 30 And the body yaw angle calculated by the vehicle body yaw angle calculating means 44 and the differential amount of the vehicle body yaw angle calculated by the motion equation differential amount calculating means 30 are integrated with a value considering the sensor drift amount. The sensor drift amount of the sensor signal is estimated by the drift amount estimating means 32 using the relationship in which the values are equal. [Selection] Figure 1

Description

  The present invention relates to a sensor drift amount estimation device, and more particularly to a sensor drift amount estimation device that estimates a sensor drift amount of a sensor signal according to a detected value of a motion state amount of vehicle motion.

  Conventionally, in a posture angle estimation device that estimates the posture angle of a vehicle using an in-vehicle acceleration sensor and an acceleration sensor, the zero point drift of the in-vehicle angular velocity sensor and acceleration sensor has a large influence on the estimation of the posture angle. Therefore, it is necessary to compensate by successively estimating the zero point drift.

  As a conventional technique for estimating such sensor zero-point drift, an inertial navigation system has been proposed that reduces angular velocity and accelerometer drift / bias errors when used in a moving body that travels for long periods of time with a stable attitude. (Patent Document 1).

  Also, a method for estimating the amount of sensor drift using signals from GPS satellites has been proposed (Non-patent Documents 2 and 3). In Non-Patent Document 2, the turning angle of the vehicle is obtained from the GPS receiver, the output data of the angular velocity meter is integrated to obtain the yaw angle of the vehicle. A technique for correcting the output of the yaw angular velocity has been proposed. Further, in Patent Document 3, the sensor drift amount of the pitch angular velocity is obtained by obtaining the average value of the pitch angular velocity signals during the period when the differential value of the yaw angle obtained from the yaw angular velocity meter or GPS receiver is equal to or less than a predetermined value. The estimation method is described.

  Further, as a sensor drift amount estimation method capable of stably estimating the sensor drift amount regardless of the state of the vehicle motion, a posture angle (roll) with respect to the vehicle body vertical axis based on the biaxial angular velocity and the triaxial acceleration sensor signal. A method for estimating the sensor drift amount from the difference between the attitude angle estimation observer for estimating the angle and the pitch angle) and the equation of motion considering the output from the observer and the sensor drift amount has been proposed (Patent Document 4). In this method, in order to estimate the sensor drift amount of the yaw angular velocity and the lateral acceleration from one equation, the sensor drift amount of the yaw angular velocity and the lateral acceleration is distributed using the change in the vehicle speed.

  A technique for correcting only the zero point drift of the yaw angular velocity sensor of the in-vehicle gyro sensor using the yaw angular velocity estimated from the in-vehicle camera has also been proposed (Patent Documents 5, 6, and 7). The methods of Patent Literature 5 and Patent Literature 6 are methods for correcting the drift of the gyro sensor by determining whether or not the vehicle is traveling straight using a camera. The method of Patent Document 7 is a method of correcting the drift of the gyro sensor by estimating the yaw rate based on the vanishing point at the time of turning and the vanishing point at the time of straight traveling obtained from the image.

JP 2007-15584 A JP-A-6-26865 JP 2011-191243 A JP 2010-70184 A JP 2006-199224 A JP 2012-233852 A JP 2011-169728 A

  In order to improve the accuracy of the vehicle motion control technology, there is a problem that it is necessary to stably estimate the sensor drift amount regardless of the state of the vehicle motion.

  In the technique described in Patent Document 1 described above, a state in which the posture of the moving body is almost constant and stable is determined, and the drift amount is estimated. If not, there is a problem that the sensor drift amount cannot be estimated. In addition, since the techniques described in Patent Documents 2 and 3 do not take into account the vehicle attitude angle (roll angle, pitch angle) or slip angle, the accuracy in traveling where the cant road and the slip angle are large. There is a problem that the deterioration of

  In the above-mentioned Patent Document 4, a technique for estimating the sensor drift amount is proposed for the above problem regardless of the state of the vehicle motion. However, since the change in the vehicle speed is used to estimate the sensor drift amount of the yaw angular velocity and the lateral acceleration, there is a problem that there is a concern that the estimation accuracy is deteriorated under a traveling condition with a small change in the vehicle speed.

  The present invention has been made to solve the above problems, and provides a sensor drift amount estimation apparatus and program capable of stably estimating the drift amount of a sensor accurately regardless of the state of vehicle motion. The purpose is to do.

  In order to achieve the above object, a sensor drift amount estimation device according to a first aspect of the present invention calculates a differential amount of a posture angle with respect to a vertical axis of a vehicle body based on a sensor signal corresponding to a detected value of a motion state amount of vehicle motion. And calculating the differential amount of the vehicle speed based on the sensor signal based on the attitude angle estimating means for estimating the attitude angle by integrating the calculated differential amount of the attitude angle. Is integrated to estimate the vehicle speed, and based on the estimated vehicle speed, slip angle estimation means for estimating a slip angle, and the attitude angle estimated by the sensor signal and the attitude angle estimation means Based on the equation of motion of the vehicle motion, a differential amount of the posture angle obtained from the equation of motion of the vehicle motion, and based on the vehicle speed estimated by the sensor signal and the slip angle estimation means The vehicle motion is calculated based on the sensor signal, the posture angle estimated by the posture angle estimating means, and the vehicle speed estimated by the slip angle estimating means. Calculating means for calculating the differential amount of the rotation angle about the vehicle body up-down direction axis obtained from the equation of motion, information on each position of the information transmission source transmitted from each of the plurality of information transmission sources, the information Acquisition means for acquiring transmission source information including information on the distance between each of the transmission sources and the vehicle, and information on the relative speed of the vehicle with respect to each of the information transmission sources, and the transmission source acquired by the acquisition means Vehicle speed vector estimating means for estimating the speed vector of the vehicle based on the information; the posture angle estimated by the posture angle estimating means; Vehicle body rotation angle calculation means for calculating the rotation angle based on the slip angle estimated by the pop angle estimation means and the vehicle speed vector estimated by the vehicle speed vector estimation means, and the sensor signal When the sensor drift amount is taken into account, the posture angle differential amount calculated by the posture angle estimation unit and the posture angle differential amount calculated by the calculation unit include the sensor drift amount in consideration. A relationship in which the differential amount of the vehicle speed calculated by the slip angle estimating unit is equal to a value in which the sensor drift amount is considered in the differential amount of the vehicle speed calculated by the calculation unit, and The sensor drift is calculated based on the rotation angle calculated by the vehicle body rotation angle calculation means and the differential amount of the rotation angle calculated by the calculation means. It includes drift amount estimation means for estimating the sensor drift amount of the sensor signal using a relationship in which a value obtained by integrating a value considering the amount becomes equal.

  A program according to a second invention calculates a differential amount of a posture angle with respect to a vertical axis of a vehicle body based on a sensor signal corresponding to a detected value of a motion state amount of vehicle motion, Posture angle estimation means for integrating the differential amount to estimate the posture angle, calculating the differential amount of the vehicle speed based on the sensor signal, and integrating the differential amount of the vehicle speed to estimate the vehicle speed Then, based on the estimated vehicle speed, a slip angle estimating means for estimating a slip angle, the sensor signal and the posture angle estimated by the posture angle estimating means are obtained from a motion equation of vehicle motion. The vehicle speed obtained from the equation of motion of the vehicle motion based on the sensor signal and the vehicle speed estimated by the slip angle estimating means A vehicle body obtained from a motion equation of vehicle motion based on the sensor signal, the posture angle estimated by the posture angle estimating means, and the vehicle speed estimated by the slip angle estimating means. Calculation means for calculating the differential amount of the rotation angle about the vertical axis, information on the position of each of the information transmission sources transmitted from each of a plurality of information transmission sources, each of the information transmission sources and the vehicle Based on the transmission source information acquired by the acquisition means for acquiring transmission source information including information on distance and information on the relative speed of the vehicle with respect to each of the information transmission sources, the speed vector of the vehicle Vehicle speed vector estimation means for estimating the attitude angle estimated by the attitude angle estimation means, estimated by the slip angle estimation means The vehicle body rotation angle calculation means for calculating the rotation angle based on the slip angle and the vehicle speed vector estimated by the vehicle speed vector estimation means, and when the sensor drift amount of the sensor signal is considered, The relationship between the differential amount of the posture angle calculated by the posture angle estimating means and the value obtained by considering the sensor drift amount to the differential amount of the posture angle calculated by the calculating means, and the slip angle estimating means The calculated differential amount of the vehicle speed and the relationship between the differential amount of the vehicle speed calculated by the calculating unit and the value considering the sensor drift amount are equal to each other, and the vehicle body rotation angle calculating unit calculates The rotation angle is equal to a value obtained by integrating a differential amount of the rotation angle calculated by the calculation unit with a value considering the sensor drift amount. Is a program for functioning as drift amount estimation means for estimating the sensor drift amount of the sensor signal.

  According to a third aspect of the present invention, there is provided a sensor drift amount estimation device that rolls relative to a vertical axis of a vehicle body based on sensor signals corresponding to detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of vehicle motion. Attitude angle estimating means for calculating the differential amount of each of the angle and the pitch angle, integrating the calculated differential amounts of the roll angle and the pitch angle, and estimating the roll angle and the pitch angle, and each detection Based on the sensor signal according to the value, the differential amounts of the longitudinal speed and the lateral speed are calculated, and the differential quantities of the longitudinal speed and the lateral speed are integrated to estimate the longitudinal speed and the lateral speed. A slip angle estimating means for estimating a slip angle based on the estimated longitudinal speed and lateral speed, the sensor signal, and the roll angle estimated by the attitude angle estimating means. And a differential amount of each of the roll angle and the pitch angle obtained from a motion equation of vehicle motion based on the pitch angle, and the front-rear speed estimated by the sensor signal and the slip angle estimating means And a differential amount of each of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion based on the lateral speed, and the sensor signal, the roll angle estimated by the attitude angle estimating means, and Calculation for calculating a derivative of the vehicle body yaw angle around the vehicle body vertical axis obtained from the equation of motion of the vehicle motion based on the pitch angle and the longitudinal speed and the lateral speed estimated by the slip angle estimating means. Means, information on the position of each of the information transmission sources transmitted from each of the plurality of information transmission sources, and the distance between each of the information transmission sources and the vehicle Acquisition means for acquiring transmission source information including information related to the information and relative information on the relative speed of the vehicle with respect to each of the information transmission sources, and a speed vector of the vehicle based on the transmission source information acquired by the acquisition means Vehicle speed vector estimation means for estimating the attitude angle estimated by the attitude angle estimation means, the slip angle estimated by the slip angle estimation means, and the vehicle speed estimated by the vehicle speed vector estimation means Based on the velocity vector, the vehicle body yaw angle calculation means for calculating the vehicle body yaw angle, and the roll angle and the pitch angle calculated by the posture angle estimation means when the sensor drift amount of the sensor signal is taken into account. Each differential amount and each differential amount of the roll angle and the pitch angle calculated by the calculating means are The relationship in which the value considering the sensor drift amount is equal, the differential amounts of the longitudinal speed and the lateral speed calculated by the slip angle estimating means, the longitudinal speed and the lateral speed calculated by the calculating means The differential value of the vehicle body yaw angle calculated by the vehicle body yaw angle calculation means and the differential of the vehicle body yaw angle calculated by the calculation means In accordance with each detected value of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity using a relationship in which the value obtained by integrating the value considering the sensor drift amount is equal to the amount. And drift amount estimating means for estimating the sensor drift amount of the sensor signal.

  According to a fourth aspect of the invention, there is provided a program for causing a computer to detect a roll angle with respect to a vertical axis of a vehicle body based on sensor signals corresponding to detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of vehicle motion. And a differential value of each of the pitch angles, and a posture angle estimating means for estimating the roll angle and the pitch angle by integrating the calculated differential amounts of the roll angle and the pitch angle, Based on the corresponding sensor signal, the respective differential amounts of the longitudinal speed and the lateral speed are calculated, the differential amounts of the longitudinal speed and the lateral speed are integrated, and the longitudinal speed and the lateral speed are estimated, Based on the estimated longitudinal speed and lateral speed, slip angle estimating means for estimating a slip angle, the sensor signal, and the roll angle estimated by the posture angle estimating means. Based on the pitch angle, a differential amount of each of the roll angle and the pitch angle obtained from a motion equation of vehicle motion is calculated, and the front-rear speed estimated by the sensor signal and the slip angle estimating means and Based on the lateral speed, a differential amount of each of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion is calculated, and the roll angle estimated by the sensor signal, the posture angle estimating means, and the Calculation means for calculating a differential amount of the vehicle body yaw angle about the vehicle body vertical axis obtained from the equation of motion of the vehicle motion based on the pitch angle and the longitudinal speed and the lateral speed estimated by the slip angle estimation means. , Information regarding the position of each of the information transmission sources transmitted from each of the plurality of information transmission sources, and the distance between each of the information transmission sources and the vehicle. Acquisition means for acquiring transmission source information including information on the relative speed of the vehicle with respect to each of the information transmission sources, and the vehicle speed vector based on the transmission source information acquired by the acquisition means. Vehicle speed vector estimation means to estimate, the attitude angle estimated by the attitude angle estimation means, the slip angle estimated by the slip angle estimation means, and the vehicle speed vector estimated by the vehicle speed vector estimation means Each of the roll angle and the pitch angle calculated by the attitude angle estimating means when the vehicle yaw angle calculating means for calculating the vehicle body yaw angle and the sensor drift amount of the sensor signal are considered. The sensor includes a differential amount and a differential amount of each of the roll angle and the pitch angle calculated by the calculation unit. The relationship between the value considering the drift amount is equal, the differential amounts of the longitudinal speed and the lateral speed calculated by the slip angle estimating means, and the longitudinal speed and the lateral speed calculated by the calculating means. A relationship in which each differential amount is equal to a value considering the sensor drift amount, the vehicle body yaw angle calculated by the vehicle body yaw angle calculating unit, and a differential amount of the vehicle body yaw angle calculated by the calculating unit A sensor corresponding to each detected value of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity using a relationship in which a value obtained by integrating the value considering the sensor drift amount is equal to It is a program for functioning as drift amount estimation means for estimating the sensor drift amount of a signal.

  A sensor drift amount estimation device according to a fifth aspect of the present invention calculates a differential amount of a posture angle with respect to a vertical axis of a vehicle body based on a sensor signal corresponding to a detected value of a motion state amount of vehicle motion, and calculates the calculated posture angle. And a posture angle estimating means for estimating the posture angle, calculating a differential amount of the vehicle speed based on the sensor signal, integrating the differential amount of the vehicle speed, and calculating the vehicle speed. A slip angle estimating means for estimating a slip angle based on the estimated vehicle speed, and a vehicle motion based on the posture angle estimated by the sensor signal and the posture angle estimating means. The vehicle speed obtained from the motion equation of the vehicle motion is calculated based on the vehicle speed estimated by the sensor signal and the slip angle estimating means. Is obtained from an equation of motion of vehicle motion based on the sensor signal, the posture angle estimated by the posture angle estimating means, and the vehicle speed estimated by the slip angle estimating means. A calculation means for calculating a differential amount of a rotation angle around the vehicle body vertical axis, and extracting a plurality of feature points from each of a plurality of images obtained by imaging the outside of the vehicle body, and based on the extracted feature points, The motion estimation means for estimating the motion of the vehicle when the plurality of images are captured, and calculating the differential amount of the rotation angle based on the estimated motion of the vehicle, and the sensor drift amount of the sensor signal is taken into consideration The posture angle differential amount calculated by the posture angle estimation means, and the posture angle differential amount calculated by the calculation means and the value considering the sensor drift amount are equal to each other. A relationship in which the differential amount of the vehicle speed calculated by the slip angle estimation unit is equal to a value in which the sensor drift amount is considered in the differential amount of the vehicle speed calculated by the calculation unit, and The sensor signal is calculated using a relationship in which a differential amount of the rotation angle calculated by the motion estimation unit is equal to a value obtained by considering the sensor drift amount with the differential amount of the rotation angle calculated by the calculation unit. Drift amount estimating means for estimating the sensor drift amount.

  A program according to a sixth aspect of the invention calculates a differential amount of a posture angle with respect to a vertical axis of a vehicle body based on a sensor signal corresponding to a detected value of a movement state amount of vehicle motion, Posture angle estimation means for integrating the differential amount to estimate the posture angle, calculating the differential amount of the vehicle speed based on the sensor signal, and integrating the differential amount of the vehicle speed to estimate the vehicle speed Then, based on the estimated vehicle speed, a slip angle estimating means for estimating a slip angle, the sensor signal and the posture angle estimated by the posture angle estimating means are obtained from a motion equation of vehicle motion. The vehicle speed obtained from the equation of motion of the vehicle motion based on the sensor signal and the vehicle speed estimated by the slip angle estimating means A vehicle body obtained from a motion equation of vehicle motion based on the sensor signal, the posture angle estimated by the posture angle estimating means, and the vehicle speed estimated by the slip angle estimating means. A calculating means for calculating a differential amount of a rotation angle about the vertical axis, extracting a plurality of feature points from each of a plurality of images obtained by imaging the outside of the vehicle body, and based on the extracted plurality of feature points, the plurality of feature points When estimating the motion of the vehicle when the image of the image is taken, and considering the amount of sensor drift of the sensor signal, the motion estimation means for calculating the differential amount of the rotation angle based on the estimated motion of the vehicle The posture angle differential amount calculated by the posture angle estimating means is equal to the posture angle differential amount calculated by the calculating means in consideration of the sensor drift amount. A relationship in which the differential amount of the vehicle speed calculated by the slip angle estimating means is equal to a value in which the sensor drift amount is considered in the differential amount of the vehicle speed calculated by the calculation means, and The relationship between the differential amount of the rotation angle calculated by the motion estimation means and the value obtained by taking the sensor drift amount into consideration with the differential amount of the rotation angle calculated by the calculation means is equal. A program for functioning as drift amount estimation means for estimating the sensor drift amount.

  According to a seventh aspect of the present invention, there is provided a sensor drift amount estimation device, wherein a roll relative to a vertical axis of a vehicle body is based on sensor signals corresponding to detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of vehicle motion. Attitude angle estimating means for calculating the differential amount of each of the angle and the pitch angle, integrating the calculated differential amounts of the roll angle and the pitch angle, and estimating the roll angle and the pitch angle, and each detection Based on the sensor signal according to the value, the differential amounts of the longitudinal speed and the lateral speed are calculated, and the differential quantities of the longitudinal speed and the lateral speed are integrated to estimate the longitudinal speed and the lateral speed. A slip angle estimating means for estimating a slip angle based on the estimated longitudinal speed and lateral speed, the sensor signal, and the roll angle estimated by the attitude angle estimating means. And a differential amount of each of the roll angle and the pitch angle obtained from a motion equation of vehicle motion based on the pitch angle, and the front-rear speed estimated by the sensor signal and the slip angle estimating means And a differential amount of each of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion based on the lateral speed, and the sensor signal, the roll angle estimated by the attitude angle estimating means, and Calculation for calculating a derivative of the vehicle body yaw angle around the vehicle body vertical axis obtained from the equation of motion of the vehicle motion based on the pitch angle and the longitudinal speed and the lateral speed estimated by the slip angle estimating means. And a plurality of feature points are extracted from each of the plurality of images obtained by imaging the outside of the vehicle body and the plurality of images based on the extracted plurality of feature points. When estimating the motion of the vehicle when imaged, and taking into account the sensor drift amount of the sensor signal, the motion estimation means for calculating the differential amount of the vehicle body yaw angle based on the estimated motion of the vehicle, The sensor drift amount is considered in the differential amounts of the roll angle and the pitch angle calculated by the posture angle estimation unit and the differential amounts of the roll angle and the pitch angle calculated by the calculation unit. And the differential amounts of the longitudinal speed and the lateral speed calculated by the slip angle estimating means, and the differential amounts of the longitudinal speed and the lateral speed calculated by the calculating means. The relationship in which the value considering the sensor drift amount is equal, the differential amount of the vehicle body yaw angle calculated by the motion estimation unit, and the calculation unit Using the relationship in which the differential value of the vehicle body yaw angle is equal to the value considering the sensor drift amount, the detected values of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity are used. And a drift amount estimating means for estimating the sensor drift amount of the corresponding sensor signal.

  According to an eighth aspect of the invention, there is provided a program for causing a computer to detect a roll angle with respect to a vertical axis of a vehicle body based on sensor signals corresponding to detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of vehicle motion. And a differential value of each of the pitch angles, and a posture angle estimating means for estimating the roll angle and the pitch angle by integrating the calculated differential amounts of the roll angle and the pitch angle, Based on the corresponding sensor signal, the respective differential amounts of the longitudinal speed and the lateral speed are calculated, the differential amounts of the longitudinal speed and the lateral speed are integrated, and the longitudinal speed and the lateral speed are estimated, Based on the estimated longitudinal speed and lateral speed, slip angle estimating means for estimating a slip angle, the sensor signal, and the roll angle estimated by the posture angle estimating means. Based on the pitch angle, a differential amount of each of the roll angle and the pitch angle obtained from a motion equation of vehicle motion is calculated, and the front-rear speed estimated by the sensor signal and the slip angle estimating means and Based on the lateral speed, a differential amount of each of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion is calculated, and the roll angle estimated by the sensor signal, the posture angle estimating means, and the Calculation means for calculating a differential amount of the vehicle body yaw angle about the vehicle body vertical axis obtained from the equation of motion of the vehicle motion based on the pitch angle and the longitudinal speed and the lateral speed estimated by the slip angle estimation means. Extracting a plurality of feature points from each of a plurality of images obtained by imaging the outside of the vehicle body, and capturing the plurality of images based on the extracted plurality of feature points. When estimating the motion of the vehicle at the time, taking into account the sensor drift amount of the sensor signal, the motion estimation means for calculating the derivative of the vehicle body yaw angle based on the estimated motion of the vehicle, A value obtained by taking the sensor drift amount into consideration for the respective differential amounts of the roll angle and the pitch angle calculated by the posture angle estimating means and the differential amounts of the roll angle and the pitch angle calculated by the calculating means. And the differential amounts of the longitudinal speed and the lateral speed calculated by the slip angle estimating means, and the differential quantities of the longitudinal speed and the lateral speed calculated by the calculating means. The relationship in which the sensor drift amount is considered to be equal, the differential amount of the vehicle body yaw angle calculated by the motion estimation means, and the previous value calculated by the calculation means Using the relationship in which the differential value of the vehicle body yaw angle is equal to the value considering the sensor drift amount, the detected values of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity are used. It is a program for functioning as drift amount estimation means for estimating the sensor drift amount of the corresponding sensor signal.

  The present invention can also be configured as a recording medium storing the above program.

  As described above, according to the sensor drift amount estimation device and the program of the present invention, the posture angle differential amount calculated for estimating the posture angle and the posture angle differential amount obtained from the motion equation of the vehicle motion , The relationship between the differential amount of the vehicle speed calculated to estimate the slip angle and the differential amount of the vehicle speed obtained from the equation of motion of the vehicle motion, and the rotation angle calculated based on the source information, By estimating the sensor drift amount using the relationship with the value obtained by integrating the differential amount of the rotation angle obtained from the equation of motion of the vehicle motion, the drift amount of the sensor can be estimated stably regardless of the vehicle motion state. The effect that it can do is acquired.

  Further, according to the sensor drift amount estimation apparatus and program of the present invention, the relationship between the posture angle differential amount calculated for estimating the posture angle and the posture angle differential amount obtained from the motion equation of the vehicle motion, slip The relationship between the differential amount of the vehicle speed calculated for estimating the angle and the differential amount of the vehicle speed obtained from the equation of motion of the vehicle, and the rotation angle calculated based on a plurality of images obtained by imaging the outside of the vehicle body By estimating the sensor drift amount using the relationship between the differential amount of the motor and the rotational angle derivative obtained from the equation of motion of the vehicle motion, the sensor drift amount can be stably stabilized regardless of the state of the vehicle motion. The effect that it can estimate is acquired.

It is the schematic which shows the structure of the attitude | position angle slip angle estimation apparatus which concerns on the 1st Embodiment of this invention. It is a figure which shows the direction of each axis | shaft explaining vehicle motion. It is a figure for demonstrating an angle. It is a figure for demonstrating the estimation method of the drift amount in a prior art. It is a block diagram which shows the structure of a vehicle speed vector estimation means. It is a figure for demonstrating a vehicle body yaw angle. It is a flowchart which shows the content of the attitude angle estimation process routine in the attitude angle slip angle estimation apparatus which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the content of the vehicle body yaw angle estimation process routine in the attitude | position angle slip angle estimation apparatus which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the content of the own vehicle speed estimation process in the attitude angle slip angle estimation apparatus which concerns on the 1st Embodiment of this invention. It is a figure for demonstrating the update method of the estimated value of the drift amount in 2nd Embodiment. It is a graph which shows the calculation result of the yaw angle error in the state which applied the drift, the estimation result of a roll angle estimated value, the estimation result of the sensor drift amount of a yaw angular velocity, and continuous supplementary time. It is the schematic which shows the structure of the attitude | position angle slip angle estimation apparatus which concerns on the 4th Embodiment of this invention. It is a figure which shows the example of the coordinate system of the vehicle which carries out a plane motion. It is the schematic which shows the structure of the motion estimation means which concerns on the 4th Embodiment of this invention. It is a flowchart which shows the content of the attitude angle estimation process routine in the attitude angle slip angle estimation apparatus which concerns on the 4th Embodiment of this invention. It is a flowchart which shows the content of the vehicle body yaw angle time differential value estimation process routine in the attitude | position angle slip angle estimation apparatus which concerns on the 4th Embodiment of this invention. It is a flowchart which shows the content of the motion estimation process routine in the attitude | position angle slip angle estimation apparatus which concerns on the 4th Embodiment of this invention. It is a figure which shows the example of a noise countermeasure.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the present embodiment, the present invention is applied to a posture angle slip angle estimating device that estimates a pitch angle and a roll angle, which are vehicle posture angles with respect to a vertical axis, and estimates a vehicle slip angle.

  First, as shown in FIG. 2, rotation about each axis when the vehicle motion axis is the x axis from the center of gravity toward the front of the vehicle, the y axis toward the left of the vehicle, and the z axis upward in the vertical direction. The angular velocities are defined as roll angular velocity, pitch angular velocity, and yaw angular velocity, respectively.

As shown in FIG. 1, the posture angle slip angle estimation device 10 according to the first embodiment includes each of a longitudinal acceleration G _x , a lateral acceleration G _y , and a vertical acceleration G _z that are xyz three-axis accelerations of a vehicle motion. And the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the vertical acceleration sensor 16 that output a sensor signal corresponding to the detected value, and the roll angular velocity that detects the roll angular velocity P and outputs a sensor signal corresponding to the detected value. The sensor 18 includes a yaw angular velocity sensor 20 that detects a yaw angular velocity R and outputs a sensor signal corresponding to the detected value.

  The vertical acceleration sensor 16, the roll angular velocity sensor 18, the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the yaw angular velocity sensor 20 are sensors from each sensor based on sensor drift amounts estimated by a drift amount estimation means 32 described later. It is connected to drift amount correction means 22 for correcting the signal. The drift amount correcting means 22 is connected to the drift amount estimating means 32.

  The drift amount correcting means 22 is connected to a posture angle observer 26 that estimates a roll angle φ and a pitch angle θ, which are posture angles with respect to the vertical axis of the vehicle body.

  The drift amount correcting means 22 is connected to a slip angle observer 28 that estimates the vehicle body slip angle.

  Further, the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the yaw angular velocity sensor 20 have the sensor drift amounts of the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the yaw angular velocity sensor 20 estimated by the drift amount estimating means 32. Based on the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the yaw angular velocity sensor 20, the drift amount correction unit 22 that corrects sensor signals is connected.

  The longitudinal acceleration sensor 12, the lateral acceleration sensor 14, the vertical acceleration sensor 16, the roll angular velocity sensor 18, the yaw angular velocity sensor 20, the attitude angle observer 26, and the slip angle observer 28 are a roll angle and a pitch angle obtained by a motion equation of vehicle motion. , Connected to a motion equation differential amount calculating means 30 for calculating the differential amounts of the front and rear vehicle speed, the lateral vehicle speed, and the vehicle body yaw angle.

  The attitude angle observer 26, the slip angle observer 28, and the equation of motion differential amount calculation means 30 are sensor drifts of the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, the vertical acceleration sensor 16, the roll angular velocity sensor 18, and the yaw angular velocity sensor 20, respectively. It is connected to drift amount estimation means 32 for estimating the amount.

  The drift amount correcting means 22, the attitude angle observer 26, the slip angle observer 28, the equation of motion differential amount calculating means 30, and the drift amount estimating means 32 are one or more computers that realize the function of each means, or one or It can be composed of a plurality of electronic circuits.

Attitude angle observer 26, the estimated estimated value of the lateral vehicle body speed V at the slip angle observer 28, correction corresponding to the estimated value, the detection value of the corrected vertical acceleration G _z in the drift amount correcting means 22 of the longitudinal vehicle body velocity U Based on the signal and the correction signal corresponding to the detected value of the roll angular velocity P, the pitch angular velocity Q is estimated. The attitude angle observer 26 corrects according to the detected values of the longitudinal acceleration G _x , lateral acceleration G _y , vertical acceleration G _z , yaw angular velocity R, and roll angular velocity P of the vehicle motion corrected by the drift amount correcting means 22. Based on the signal, the estimated value V _so of the front and rear vehicle body speed U, and the estimated value of the pitch angular speed Q, the roll angle φ and the pitch angle θ, which are attitude angles with respect to the vertical axis of the vehicle body, are estimated.

  The vertical acceleration sensor 16 and the roll angular velocity sensor 18 are connected to a drift amount correcting unit 22 that corrects a sensor signal from each sensor based on a sensor drift amount estimated by a drift amount estimating unit 32 described later. The drift amount correcting means 22 is connected to the drift amount estimating means 32.

  The drift amount correcting means 22 is connected to a posture angle observer 26 that estimates a roll angle φ and a pitch angle θ, which are posture angles with respect to the vertical axis of the vehicle body.

  The attitude angle slip angle estimation device 10 further includes GPS receiving means 40 for receiving radio waves from GPS satellites.

  The GPS receiving means 40 is connected to a vehicle speed vector estimating means 42 for estimating a vehicle speed vector. The GPS receiving means 40 receives radio waves from a plurality of GPS satellites, and from the received signals from all received GPS satellites, as GPS satellite information, the satellite number of the GPS satellite, orbit information (ephemeris) of the GPS satellite The time when the GPS satellite transmits the radio wave, the intensity of the received signal, the frequency, and the like are acquired and output to the vehicle speed vector estimating means 42. A GPS satellite is an example of an information transmission source.

  The vehicle speed vector estimation unit 42 is connected to a vehicle body yaw angle calculation unit 44 that calculates a vehicle body yaw angle. The vehicle body yaw angle calculation means 44 is connected to the attitude angle observer 26, the slip angle observer 28, and the vehicle speed vector estimation means 42, and outputs the calculated vehicle body yaw angle to the drift amount estimation means 32.

  Next, estimation of the pitch angular velocity will be described. The equation of motion of the rigid body that expresses the relationship between the sensor signal output from the three-axis sensor that detects the three-axis acceleration and the three-axis angular velocity fixed to the rigid body and the motion state quantity can be described as follows.

However, _Gx : longitudinal acceleration, _Gy : lateral acceleration, _Gz : vertical acceleration, P: roll angular velocity, Q: pitch angular velocity, R: yaw angular velocity, U: longitudinal vehicle velocity, V: lateral vehicle velocity, W: vertical velocity Vehicle speed, φ: roll angle, θ: pitch angle, g: gravitational acceleration. As shown in FIG. 2, the coordinate system is a vehicle body fixed coordinate system, and is described by a right-hand system in which the upward direction of the vehicle body is the positive direction of the Z axis. As shown in FIG. 3, the angle is expressed as an Euler angle obtained by rotating the axis in the order of Z axis → Y axis → X axis.

When the vehicle is a rigid body in the present embodiment, each of the three-axis speeds of the longitudinal acceleration G _x , the lateral acceleration G _y , and the vertical acceleration G _z and the two-axis angular speeds of the roll angular velocity P and the yaw angular velocity R is the longitudinal acceleration. Sensor signals detected by the sensor 12, the lateral acceleration sensor 14, the vertical acceleration sensor 16, the roll angular velocity sensor 18, and the yaw angular velocity sensor 20 are signals in which the drift amount is corrected by the drift amount correcting means 22, and the lateral vehicle body speed V Can be estimated by the slip angle observer 28, and the front and rear vehicle body speed U can be estimated based on the wheel speed of each wheel.

In the present embodiment, it is assumed that the change in the vertical vehicle body speed W can be ignored (W = 0) in the above equation (3), and the previously estimated values of the roll angle φ and the pitch angle θ are ~ φ, ~ θ By using the estimated value V _so of the longitudinal vehicle body speed V and the estimated value V _so of the longitudinal vehicle body speed U as the longitudinal vehicle body speed U, the estimated value ~ Q of the pitch angular velocity Q can be obtained from the following equation (6). it can. The posture angle observer 26 obtains the estimated value ~ Q of the pitch angular velocity Q by executing the calculation of the following equation (6).

When the pitch angular velocity is estimated as described above, the pitch angle can also be estimated by the attitude angle observer 26 simultaneously with the roll angle. A description will be given of a constraint condition of the motion unique to the automobile when the attitude angle observer 26 is used. When the posture angle observer 26 is configured using the above equation of motion, feedback of a physical quantity that can be measured is necessary so that the state quantities of the speed and angle estimated by the integral calculation do not diverge. In the present embodiment, the characteristic unique to automobile motion is used as a physical quantity to be fed back as follows. In the following equations, the previous estimated values ~ φ, ~ θ, the estimated value ~ V of the lateral vehicle speed as the lateral vehicle speed ~ V, and the estimated longitudinal vehicle speed as the longitudinal vehicle speed U, respectively, as the roll angle and the pitch angle. Each value V _so is used.

Differentiating the estimated value V _so of the front and rear vehicle speed and substituting it into the above equation (1), and using the characteristic that “the wheel slip does not continue to increase for a long time” in the longitudinal direction of the vehicle body as a feature unique to automobile motion Then, the condition shown in the following formula (7) used for feedback is obtained, ignoring the change in the vertical vehicle body speed from the formula (1).

Here, V _S0 is the vehicle body longitudinal speed estimated from the wheel speed, and the difference between the acceleration estimated from the wheel speed (the differential value of the estimated value V _so of the longitudinal vehicle speed) and the longitudinal acceleration G _x , and the pitch angle Describes the relationship with θ. Further, the vehicle body lateral velocity V is expressed as ~ V because it is obtained by the following equation (8) using the estimated value ~ β of the slip angle.

  Further, regarding the lateral direction of the vehicle body, the roll angular velocity and the lateral acceleration can be ignored due to the property that “the slip angle does not continue to increase for a long time”, so the following obtained from the above equation (2) for use in feedback: The condition shown in equation (9) is obtained.

Furthermore, considering a general road gradient, it is considered that “the acceleration in the vertical axis direction of the fixed ground coordinate system is equal to the acceleration of gravity”. This condition can be described by the following algebraic equation (10) using longitudinal acceleration G _x , lateral acceleration G _y , vertical acceleration G _z , pitch angle θ, and roll angle φ.

  Next, the attitude angle observer will be described.

  Since the relationship of the above equations (7), (9), and (10) is a condition that is satisfied when a change in a long time is taken into account, the following (11) A value obtained by performing low-pass filter processing on both sides of the above formulas (7) to (9) shown in formula (13) is used.

However, τ _x , τ _y , and τ _g respectively represent time constants of several seconds to several tens of seconds or more of the low-pass filter considered in the above _equations (7), (9), and (10).

  Next, in this embodiment, the state quantity ~ x of the observer is expressed by the following equation (14).

  The observer output used for feedback is expressed by the following equation (15).

  Further, the vehicle output calculated from the sensor signal or the like is expressed by the following equation (16), and an appropriate observer gain k (˜x, u) is set to estimate the roll angle and the pitch angle. Is configured.

  As an example of the observer gain, in the present embodiment, in order to ensure the stability of the observer, the diagonal component when linearization is performed is expressed as the following equation (19) so as to have a negative coefficient.

_{K x,} K y, K _g is a positive constant, K φg _(G y), and _{K θg (G x), K} φy, K θx is the lateral acceleration, longitudinal estimated value varies according to the pitch angle estimated value Variable gain.

  Therefore, the nonlinear observer of the present embodiment for estimating the roll angle and the pitch angle is described by an equation of motion represented by the following equation (20).

  The attitude angle observer 26 uses the above equation (20) to calculate the roll angle ~ φ differential amount ~ dφ and the pitch angle ~ θ differential quantity ~ dθ, which are attitude angles with respect to the vehicle body vertical direction. The roll angle ~ φ and the pitch angle ~ θ can be calculated by integrating each of the roll angle ~ φ derivative amount ~ dφ and the pitch angle ~ θ derivative amount ~ dθ.

The slip angle observer 28 estimates the lateral vehicle body speed V which is the vehicle body speed in the lateral direction of the vehicle, estimates the longitudinal vehicle body speed U which is the vehicle body speed in the vehicle longitudinal direction based on the wheel speed of each wheel, estimate of vehicle speed V, according to the estimated value, the correction signal corresponding to the detected value of the longitudinal acceleration G _x, the correction signal corresponding to the detected value of the lateral acceleration G _y, and the detection value of the yaw angular velocity R of the longitudinal vehicle body velocity U Based on the corrected signal, the longitudinal vehicle body speed U and the lateral vehicle body speed V are estimated. Further, the slip angle observer 28 estimates the vehicle body slip angle β based on the estimated value of the longitudinal vehicle body speed U and the estimated value of the lateral vehicle body speed V.

  Next, estimation of the longitudinal vehicle body speed and the lateral vehicle body speed will be described.

  Conventionally, the equation (1), which ignores dynamic characteristics, includes a term of the horizontal vehicle body speed V, and an observer can be configured by using this horizontal vehicle body speed. Here, considering the yaw angular velocity as an independent parameter, the above formulas (1) and (2) are arranged as the following formula (21). Note that the vertical vehicle body speed W is assumed to be 0 as in the case of the attitude angle observer 26.

  Further, assuming that the output y of the system is represented by the longitudinal vehicle body speed U, that is, the following equation (22), equations (21) and (22) are observable systems.

  The front-rear vehicle body speed U is estimated based on the wheel speed of each wheel, as described above. For example, the observer gain K is expressed by the following equation (23) including a value obtained from the absolute value of the yaw angular velocity.

  When the observer gain is expressed as in the above equation (23), the observer is described by an equation of motion expressed by the following equation (24).

  However, ~ x is the state quantity of the observer and is expressed by the following equation (25).

The slip angle observer 28 according to the present embodiment estimates the longitudinal vehicle body speed and the lateral vehicle body speed using the above equation (24). In the present embodiment, the vehicle body attitude angle, that is, the roll angle φ and the pitch angle θ of the vehicle body with respect to the vertical axis are assumed to be known (for example, the roll angle and the pitch angle are set to 0), And the lateral body speed ~ V is the state quantity of the observer, the deviation obtained by subtracting the estimated value ~ U (observer output) of the front and rear body speed from the calculated value U of the front and rear body speed, the known roll angle φ and pitch angle θ, and drift By using the longitudinal acceleration G _x -G _xdr , lateral acceleration G _y -G _ydr , and yaw angular velocity R-R _dr corrected by the amount correction means 22, the longitudinal vehicle body speed ~ the derivative of U ~ dU and the lateral vehicle body speed ~ The derivative amount V of dV is calculated. Further, the slip angle observer 28 estimates the longitudinal vehicle body speed and the lateral vehicle body speed by integrating the differential amounts of the longitudinal vehicle body speed and the lateral vehicle body speed.

  The slip angle observer 28 estimates the vehicle body slip angle ~ β based on the following equation (26) from the relationship between the longitudinal vehicle body speed U and the lateral vehicle body speed V.

  Next, the correction of the drift amount will be described.

  When a sensor signal in which sensor drift is superimposed is input to the attitude angle observer 26 and the slip angle observer 28, an error is superimposed on the attitude angle and the vehicle body speed estimation value, and the estimation error increases. Therefore, it is necessary to subtract the drift amount estimated value calculated by the drift amount estimating means 32 from the sensor signal and use it as an input of the observer.

  The drift amount correction means 22 subtracts the estimated vertical acceleration drift amount from the sensor signal from the vertical acceleration sensor 16 and outputs the result to the attitude angle observer 26. Further, the drift amount correcting means 22 subtracts the roll angular velocity drift amount estimated value from the sensor signal from the roll angular velocity sensor 18 and outputs the result to the attitude angle observer 26.

  The drift amount correcting unit 22 subtracts the estimated longitudinal acceleration drift amount from the sensor signal from the longitudinal acceleration sensor 12 and outputs the result to the slip angle observer 28. The drift amount correcting means 22 subtracts the estimated lateral acceleration drift amount from the sensor signal from the lateral acceleration sensor 14 and outputs the result to the slip angle observer 28. The drift amount correction means 22 subtracts the estimated yaw angular velocity drift amount from the sensor signal from the yaw angular velocity sensor 20 and outputs the result to the slip angle observer 28.

  Next, a drift amount estimation method using the vehicle body yaw angle will be described.

  The drift amount estimation means described in Patent Document 4 uses the fact that the observer has the effect of reducing the influence of the sensor error, and estimates the sensor zero point drift by comparing the simple integration of the sensor signal with the output of the observer. (See FIG. 4).

  In the conventional method, since the sensor drift of the lateral acceleration and the yaw angular velocity is estimated from one equation based on the above concept, there is a concern that the accuracy of the yaw angular velocity drift estimated value is deteriorated due to a change in the vehicle speed.

  In the present embodiment, the estimation accuracy of the yaw angular velocity drift can be improved by using the GPS signal and newly adding the following balance formula relating to the vehicle body yaw direction.

  Here, ψ is the vehicle body yaw angle.

  Next, a method for calculating the vehicle body yaw angle using the vehicle body velocity vector obtained from the GPS satellite signal will be described.

  A signal obtained from a GPS satellite is composed of a carrier wave and a code given by 0.1 on the carrier wave. A signal obtained by measuring a frequency change caused by the topler effect of the carrier wave is called a GPS Doppler. A highly accurate vehicle speed vector can be obtained by a method of estimating the speed of the vehicle body relative to the earth center coordinate system from the GPS Doppler and the satellite speed.

  The vehicle speed vector estimation means 42 estimates the position of the moving body using a vehicle body speed vector by GPS Doppler, as in JP 2011-209268 A.

  When the vehicle speed vector estimation means 42 is expressed in functional blocks according to the own vehicle speed estimation processing routine described below, as shown in FIG. 5, the GPS satellites are obtained for all GPS satellites that have received radio waves from the GPS reception means 40. And GPS information acquisition means 50 for calculating and acquiring GPS pseudorange data, Doppler frequency, and GPS satellite position coordinates, and for each GPS satellite based on the acquired Doppler frequency of each GPS satellite. Relative speed calculation means 52 for calculating the relative speed of the host vehicle, satellite speed calculation means 54 for calculating the speed vector of each GPS satellite based on the acquired time series data of the position coordinates of each GPS satellite, and each acquired Based on the GPS pseudorange data of the GPS satellite, the vehicle position calculation means 56 for calculating the position of the vehicle, and the calculation Satellite direction calculation means 58 for calculating the direction (angle relationship) of each GPS satellite based on the position of the own vehicle and the position coordinates of each GPS satellite, the calculated relative velocity, the velocity vector of each GPS satellite, And a satellite direction own vehicle speed calculation means 60 for calculating the speed of the own vehicle in each GPS satellite direction based on the direction of each GPS satellite, and an own vehicle speed based on the calculated own vehicle speeds in the plurality of GPS satellite directions. The vehicle speed calculation means 62 for calculating the vehicle speed vector can be used.

  The GPS information acquisition unit 50 acquires GPS satellite information for all GPS satellites that have received radio waves from the GPS reception unit 40, and at the time when the GPS satellites transmit radio waves and the time at which the vehicle receives radio waves. Based on this, GPS pseudorange data is calculated. Further, the GPS information acquisition means 50 determines the Doppler frequency of the received signal from each GPS satellite based on the known frequency of the signal transmitted from each GPS satellite and the frequency of the received signal received from each GPS satellite. calculate. The Doppler frequency is obtained by observing the Doppler shift amount of the carrier frequency due to the relative speed between the GPS satellite and the own vehicle. Further, the GPS information acquisition means 50 calculates the position coordinates of the GPS satellites based on the orbit information of the GPS satellites and the time when the GPS satellites transmitted the radio waves.

  The relative speed calculation means 52 calculates the relative speed of the host vehicle with respect to each GPS satellite from the Doppler frequency of the received signal from each GPS satellite according to the following equation (28) representing the relationship between the Doppler frequency and the relative speed with respect to the GPS satellite. calculate.

Here, v _j is a relative velocity with respect to the GPS satellite j, and D1 _j is a Doppler frequency (a Doppler shift amount) obtained from the GPS satellite j. C is the speed of light, and F ₁ is a known L1 frequency of a signal transmitted from a GPS satellite.

The satellite velocity calculation means 54 calculates the velocity vector (three-dimensional velocity VX _j , VY _j , VZ _j ) of each GPS satellite from the acquired time series data of the position coordinates of each GPS satellite using the differentiation of Kepler's equation. calculate. For example, using the method described in a non-patent document (Pratap Misra and Per Eng, the original Japanese Navigational Society GPS Study Group translation: “Sophisticated GPS Basic Concept / Positioning Principle / Signal and Receiver”, Shoyo Bunko, 2004.) The velocity vector of each GPS satellite can be calculated.

  The own vehicle position calculation means 56 calculates the position of the own vehicle using the GPS pseudorange data of each GPS satellite acquired by the GPS information acquisition means 50 as follows.

  In positioning using GPS, the position of the host vehicle is estimated according to the principle of triangulation based on the known position coordinates of GPS satellites and the pseudo distance that is the propagation distance of the received signal received from each GPS satellite. The

Here, the true distance r _j to the GPS satellite is expressed by the following equation (29), and the pseudorange ρ _j observed by GPS is expressed by the following equation (30).

However, (X _j , Y _j , Z _j ) is the position coordinate of the GPS satellite j, and (x, y, z) is the position coordinate of the host vehicle. s is a distance error due to a clock error of the GPS receiving means 40.

  By solving simultaneous equations of the following equation (31) obtained from the GPS pseudorange data of four or more GPS satellites from the above equations (29) and (30), the position of the host vehicle (x, y, z ) Is calculated.

  In the present embodiment, the position of the host vehicle is calculated in order to obtain the direction of the GPS satellite (the angle between the GPS satellite and the host vehicle), but is the direction of the GPS satellite existing in the distance. The positioning may be rough, and the position determination using the pseudo distance may not be performed. Although the influence of time is small and depends on the estimation accuracy allowed by the system etc., if the positioning error of the host vehicle is in the range of several hundred meters, the speed estimation error is about 1 m / sec or less and there is no major problem. For example, the position may be determined from a map or the like, or the position of the host vehicle may be determined from information such as a past measurement history or a beacon.

Based on the calculated position of the own vehicle and the position coordinates of each GPS satellite, the satellite direction calculating means 58 determines the angular relationship between the position of each GPS satellite j and the position of the own vehicle (elevation angle θ _{j with} respect to the horizontal direction, north direction) the phi _j) azimuth angle to be calculated as the direction of each GPS satellite.

The satellite direction own vehicle speed calculation means 60 calculates the relative speed v _j of the own vehicle with respect to each calculated GPS satellite, the velocity vector (VX _j , VY _j , VZ _j ) of each GPS satellite, and the direction R _j ( Based on θ _j , φ _j ), the speed Vv _j of the host vehicle in the direction of each GPS satellite _j is calculated according to the following equation (32).

v _j is a relative speed of the host vehicle with respect to the GPS satellite j (relative speed with respect to the GPS satellite in the satellite direction). Vs _j is the speed of the GPS satellite j in the direction of the vehicle, and is obtained by Vs _j = R _j [VX _j , VY _j , VZ _j ] ^T. Vv _j is the vehicle speed in the direction of the GPS satellite j, and vCb is a clock bias fluctuation.

  As described above, the own vehicle speed in the direction of the GPS satellite is calculated not only by the three-dimensional position of the GPS satellite position but only by the orientation relationship with the GPS satellite. The GPS satellite is far away, and the angle change per minute is 0.5 degrees because it makes two rounds of the earth in one day. Usually, since the clock error between the GPS satellite and the GPS receiver is usually 1 msec or less, the azimuth relationship with the GPS satellite is not greatly affected. Similarly, since the GPS satellite is far away, even if an error of about several hundred meters occurs in determining the position of the own vehicle, the orientation relationship with the GPS satellite is not greatly affected. For this reason, even if it is a situation where an error is likely to ride on the pseudo distance, the vehicle speed in the direction of the GPS satellite can be accurately calculated as compared with the pseudo distance.

  The own vehicle speed calculation means 62 performs optimum estimation of the speed vector of the own vehicle, as will be described below.

First, when the velocity vector of the vehicle and (Vx, Vy, Vz), the relationship between the speed Vv _j of the GPS satellites direction of the vehicle is expressed by the following equation (33).

  From the above equation (33) obtained for each GPS satellite j, simultaneous equations represented by the following equation (34) with Vx, Vy, Vz, and Cb as estimated values are obtained.

  When the number of GPS satellites that have received radio waves is four or more, the optimum value of the speed vector (Vx, Vy, Vz) of the host vehicle is calculated by solving the simultaneous equation (34).

As described above, the velocity vector of the vehicle body calculated based on the GPS Doppler is obtained as the velocity with respect to the earth center coordinate system as shown in FIG. When the vehicle body speed vector with respect to the ground fixed coordinate system is converted into the speed vector of the ground fixed coordinate system O-XYZ with an arbitrary position as the origin, the angle ψ formed between the ground fixed coordinate system O-XYZ and the vehicle body speed vector by the GPS Doppler _GPS is obtained. Hereinafter, the [psi _GPS and GPS azimuth. When considering only the motion in the XY plane, the relationship between ψ _GPS , the vehicle body slip angle β, and the vehicle body yaw angle ψ is expressed by the following equation (35).

In order to deal with vehicle motion in consideration of the roll angle and pitch angle of the vehicle body, the relationship of equation (35) is expanded to a shape that takes into account vehicle motion in three dimensions. The vehicle body attitude angle is an Euler angle obtained by rotating an axis in the order of Z axis → Y axis → X axis as defined above. Therefore, the rotation angle around the Z axis between the coordinate system OX ₁ Y ₁ Z _{1 obtained} by returning the vehicle body coordinate system in the order of the X axis to the Y axis and the ground fixed coordinate system is the vehicle body yaw angle. At this time, if the velocity vector of the vehicle body is expressed as ^OX ₁ Y ₁ Z ₁ and the vector formed from the X ₁ direction component and the Y ₁ direction component is V ^X1Y1Z1 , the X ₁ axis and V ^X1Y1Z1 form. The angle is expressed by the following equation (36).

  That is, when the three-dimensional motion of the vehicle is taken into consideration, the expression (35) is corrected to the following expression (37) using β ′.

  By the above equation (37), the vehicle body yaw angle considering the three-dimensional motion of the vehicle can be obtained from the vehicle body velocity vector and the vehicle body slip angle obtained from the GPS signal.

The vehicle body yaw angle calculation means 44 is estimated by the GPS azimuth angle ψ _GPS obtained from the vehicle speed vector estimated by the vehicle speed vector estimation means 42, the slip angle β estimated by the slip angle observer 28, and the attitude angle observer 26. Based on the roll angle φ and the pitch angle θ, the vehicle body yaw angle ψ is calculated according to the above equations (36) and (37).

  Next, a method for calculating each differential amount obtained from the motion equation of vehicle motion will be described.

  Assuming that the vertical vehicle body speed W is 0 with respect to the above expressions (1), (2), (4), and (5), and substituting the above expression (6), the following expressions (38) to Equation (41) is obtained.

However, estimated values of the roll angle φ and the pitch angle θ estimated by the attitude angle observer 26 are used for the roll angle φ and the pitch angle θ. The longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the vertical acceleration sensor 16 are respectively the three-axis speeds of the longitudinal acceleration G _x , the lateral acceleration G _y , and the vertical acceleration G _z and the two-axis angular speeds of the roll angular velocity P and the yaw angular velocity R. , Sensor signals detected by the roll angular velocity sensor 18 and the yaw angular velocity sensor 20. The lateral vehicle body speed V can be estimated using a Kalman filter or the like, or can be estimated from the detection value of the lateral acceleration sensor, and the longitudinal vehicle body speed U can be estimated based on the wheel speed of each wheel. .

In addition, it is assumed that constant drift errors G _xdr , G _ydr , G _zdr , P _dr , R _dr are superimposed on the sensor signals G _X , G _y , G _z , P, R, and the right side of the above equation is Assuming that the true value is known by the observer, the following relationships (42) to (45) are established.

  Here, dφ and dθ are internal values of the posture angle observer 26 corresponding to the right side of the equations (40) and (41). DU and dV correspond to the right sides of the equations (38) and (39), and are observer internal values obtained from the vehicle body longitudinal and lateral velocity estimation methods.

  The above equation (27) representing the relational expression between the vehicle body yaw angle and the angular velocity sensor signal is assumed to have a constant drift error superimposed on the sensor signal. .

  Here, the estimated value of equation (6) was used for the pitch angular velocity Q.

  The motion equation differential amount calculating means 30 calculates the differential amounts of the roll angle φ and the pitch angle θ obtained from the motion equation of vehicle motion according to the above equations (44) and (45).

  The motion equation differential amount calculation means 30 calculates the differential amounts of the longitudinal vehicle body speed U and the lateral vehicle body speed V obtained from the motion equation of vehicle motion according to the above equations (42) and (43).

  The motion equation differential amount calculation means 30 calculates the differential amount of the vehicle body yaw angle ψ obtained from the motion equation of vehicle motion according to the formula in parentheses {} on the right side of the formula (46).

  Next, a sensor drift amount estimation method will be described.

  When the equations (42), (43), (44), (45), and (46) are arranged into equations related to drift, they are expressed as the following equations (47) to (51).

  However, dU and dV are the differential amounts of the front and rear vehicle body speeds and the lateral vehicle body speeds as the observer internal calculation values obtained by the above equation (24), and dφ and dθ are obtained by the above equation (20). It is the differential amount of the roll angle and the differential amount of the pitch angle as the internal calculation value of the observer.

  The above formulas (49) and (50) are calculated based on the differential amounts of the roll angle and the pitch angle calculated by the attitude angle observer 26, the roll angle obtained from the equation of motion, This represents a relationship in which the differential value of the pitch angle is equal to the value considering the sensor drift amount. Further, the above equations (47) and (48) are obtained by calculating the differential amounts of the front and rear vehicle body speeds and the lateral vehicle body speeds calculated by the slip angle observer 28 and the differential amounts of the front and rear vehicle body speeds and the lateral vehicle body speeds obtained from the equations of motion. This represents a relationship in which the value considering the sensor drift amount becomes equal. In addition, in the above equation (51), the vehicle body yaw angle calculated by the vehicle body yaw angle calculation means 44 is equal to the value obtained by integrating the differential amount of the vehicle body yaw angle obtained from the equation of motion with the value considering the sensor drift amount. Represents the relationship.

  Further, the sensor drift estimated value is obtained by solving the simultaneous equation with the sensor drift as an unknown in consideration of the condition that “the acceleration in the vertical direction of the ground fixed coordinate system is the gravitational acceleration” in the equation (10). Here, since the equation (10) is an assumption regarding long-time movement, the following equation (52) after filtering is used.

  The above equation (52) is rewritten as the following equation (53).

However, D _gf and E _gf are expressed by the following formulas (54) and (55). Also, τ _f is a filter time constant for considering only long-time movement.

Further, u _gf is a signal in which an upper limit is set for g _v in order to prevent deterioration in estimation accuracy due to an increase in vertical acceleration, and the range is set using g _m .

  Moreover, it can describe like the following (56) Formula from (47), (48), (49), (50), (51), (53) Formula.

Here, t _smp is a sampling time used for the calculation of the entire system. When the coefficient matrix on the left side of equations (57) and (58) and the vector on the right side are integrated for a fixed time, the following equation (59) is obtained.

Therefore, G _zdr , P _dr , R _dr , G _ydr , G _xdr can be derived by solving the following equation (61).

Here, D ⁺ is a pseudo inverse matrix.

  The drift amount estimating means 32 calculates the differential amounts of the roll angle and the pitch angle calculated by the attitude angle observer 26 and the longitudinal vehicle body speed and the lateral vehicle body speed calculated by the slip angle observer 28 according to the above equation (61). Each differential amount, the vehicle yaw angle calculated by the vehicle body yaw angle calculation means 44, the roll angle, the pitch angle, the front and rear vehicle body speed, the lateral vehicle body speed, and the vehicle body yaw angle calculated by the motion equation differential amount calculation means 30. On the basis of the respective differential amounts, the sensor drift amount of the longitudinal acceleration sensor 12, the sensor drift amount of the lateral acceleration sensor 14, the sensor drift amount of the vertical acceleration sensor 16, the sensor drift amount of the roll angular velocity sensor 18, and the yaw angular velocity sensor. Twenty sensor drift amounts can be estimated.

  Further, in the drift amount estimating means 32 according to the present embodiment, in order to stabilize the calculation, the previous value is used for the calculation result of the above equation to smooth the following equation (62) to (66). The converted value is used as the drift estimation value.

Here, ~ G _zdr , ~ P _dr , ~ G _ydr , ~ R _dr , ~ G _xdr are estimated values of the sensor drift after smoothing, and λ ₁ , λ ₂ , and λ ₃ are forgetting factors. Equations (64), (65), and (66) represent the absolute value of the posture angle when updating the translational acceleration drift estimated value in order to suppress an increase in the posture angle estimated value in the balanced state of the translational acceleration and the posture angle. This is a logic for updating the drift estimation value only when the direction is changed in a decreasing direction.

In other words, as shown in the above equation (64), when the estimated value ~ φ of the roll angle is positive, the newly calculated estimated value G _ydr of the drift error is _obtained from the previous value ~ G _ydr (i). If it is larger, the absolute value of the estimated roll angle value can be reduced by updating the drift error, so the drift error learning value is updated. If the estimated roll angle value ~ φ is negative, and the newly calculated drift error estimated value G _ydr is smaller than the previous value ~ G _ydr (i), the roll angle is estimated by updating the drift error. Since the absolute value of the value can be reduced, the drift error learning value is updated.

As shown in the above equation (65), when the estimated value ~ φ of the roll angle is positive, if the estimated value R _{dr of} newly calculated drift error is smaller than the previous value ~ R _dr (i), the drift Since the absolute value of the estimated roll angle value can be reduced by updating the error, the drift error learning value is updated. If the estimated roll angle value ~ φ is negative and the newly calculated drift error estimated value R _dr is greater than the previous value ~ R _dr (i), the roll angle is estimated by updating the drift error. Since the absolute value of the value can be reduced, the drift error learning value is updated.

As shown in the above equation (66), when the estimated pitch angle value ~ θ is positive, if the newly calculated estimated drift error value G _xdr is smaller than the previous value ~ G _xdr (i), drift Since the absolute value of the estimated value of the pitch angle can be reduced by updating the error, the drift error learning value is updated. When the estimated pitch angle value ~ θ is negative, if the newly calculated drift error estimated value G _xdr is larger than the previous value ~ G _xdr (i), the pitch angle is estimated by updating the drift error. Since the absolute value of the value can be reduced, the drift error learning value is updated.

  In the first embodiment, a program that causes a computer to function as each means of the drift amount correcting means 22, the posture angle observer 26, the slip angle observer 28, the motion equation differential amount calculating means 30, and the drift amount estimating means 32. The information processing by can be realized by the posture angle estimation processing routine shown in the flowchart of FIG. The computer includes a CPU, a ROM, and a RAM connected to each other by a bus, and an HDD connected as necessary. These programs are recorded on a recording medium such as a ROM or an HDD connected to the CPU of the computer. To be recorded.

  This posture angle estimation processing routine will be described. In step 100, sensor signals corresponding to the detected values are obtained from the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, the vertical acceleration sensor 16, the roll angular velocity sensor 18, and the yaw angular velocity sensor 20. get.

  In step 102, the sensor drift amount of the longitudinal acceleration sensor 12, the sensor drift amount of the lateral acceleration sensor 14, the sensor drift amount of the vertical acceleration sensor 16, and the roll angular velocity sensor 18 obtained one calculation cycle before in step 110 described later. And the sensor drift amount of the yaw angular velocity sensor 20, the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, the vertical acceleration sensor 16, the roll angular velocity sensor 18, and the yaw angular velocity sensor 20 acquired in step 100 above. The sensor signal output from is corrected.

  Then, in step 104, the posture angle estimated value obtained in the later-described step 114 in the later-described step 114 and the vehicle body speed estimated value obtained in the later-described step 112 in the later-described step 112 are described above. As described above, the differential amounts of the roll angle and the pitch angle calculated for estimating the roll angle and the pitch angle by the posture angle observer 26 are calculated.

  In step 106, as described above, using the attitude angle estimation value obtained in step 114 one operation cycle before and the vehicle body speed estimation value obtained in step 112 one operation cycle, as described above, The differential amounts of the front and rear vehicle body speeds and the lateral vehicle body speeds calculated to estimate the lateral vehicle body speeds are calculated.

  Then, in step 108, the estimated attitude angle value obtained one step before the calculation cycle in step 112, the estimated vehicle body speed value obtained one step before the calculation cycle in step 114, and one calculation cycle before in step 124 described later. As described above, based on the vehicle body yaw angle, the vehicle longitudinal speed and vehicle obtained from the differential amounts of the vehicle longitudinal speed and the vehicle lateral speed calculated in step 106, and the motion equation of the vehicle motion, as described above. The relationship in which the differential amount of the lateral velocity is equal to the value in consideration of the sensor drift amount, the differential amounts of the roll angle and the pitch angle calculated in the above step 104, and the roll angle obtained from the motion equation of the vehicle motion Further, the relationship between the differential value of each of the pitch angle and the value considering the sensor drift amount, and the vehicle body yaw angle calculated in step 124 and the vehicle operation The sensor drift amount of the longitudinal acceleration sensor 12 and the sensor drift amount of the lateral acceleration sensor 14 are obtained by using a relationship in which the differential value of the vehicle yaw angle obtained from the equation of motion is equal to the value obtained by integrating the value considering the sensor drift amount. The sensor drift amount of the vertical acceleration sensor 16, the sensor drift amount of the roll angular velocity sensor 18, and the sensor drift amount of the yaw angular velocity sensor 20 are estimated.

  In the next step 110, the sensor drift amount of the longitudinal acceleration sensor 12 estimated in step 108, the sensor drift amount of the lateral acceleration sensor 14, the sensor drift amount of the vertical acceleration sensor 16, the sensor drift amount of the roll angular velocity sensor 18, and The sensor drift amount of the yaw angular velocity sensor 20 is smoothed using the estimated value of the previous sensor drift amount.

  In the next step 112, the roll angle and pitch angle are estimated and output by integrating the differential amounts of the roll angle and pitch angle calculated in step 104 above.

  In step 114, the front and rear vehicle body speeds and the lateral vehicle body speeds are estimated by integrating the differential amounts of the front and rear vehicle body speeds and the lateral vehicle body speeds calculated in step 106, and the estimated front and rear vehicle body speeds and lateral vehicle body speeds are estimated. The vehicle body slip angle is estimated and output based on the vehicle body speed, and the process returns to step 100.

  Information processing by a program that causes a computer to function as each means of the vehicle speed vector estimation means 42 and the vehicle body yaw angle calculation means 44 can be realized by a vehicle body yaw angle estimation processing routine shown in the flowchart of FIG.

  In step 120, information on a plurality of GPS satellites is acquired from the GPS receiving means 40, and GPS pseudorange data, Doppler frequency, and GPS satellite position coordinates of the plurality of GPS satellites are calculated and acquired.

  Next, in step 122, the host vehicle speed estimation process described later is executed to calculate the speed vector of the host vehicle. Next, in step 124, the speed vector calculated in step 122 and the estimation in step 112 are performed. The vehicle body yaw angle is calculated based on the slip angle and the attitude angle estimated in step 114, and the process returns to step 120.

  Next, the host vehicle speed estimation processing routine will be described with reference to FIG.

In step 130, the relative speed vj of the _host vehicle with respect to each GPS satellite is calculated from the Doppler frequency of the received signal from each GPS satellite according to the above equation (28).

Next, in step 132, the velocity vector (VX _j , VY _j , VZ _j ) of each GPS satellite is calculated from the obtained time series data of the position coordinates of each GPS satellite using the differential of Kepler's equation.

  Next, in step 134, the position of the host vehicle is calculated according to the above equations (29) to (31) using the GPS pseudorange data of each GPS satellite. Here, the position of the own vehicle is calculated in order to obtain the direction of the GPS satellite (the angle between the GPS satellite and the own vehicle), and it is sufficient that a rough position can be determined as the position of the own vehicle. The position may be determined from the above information, or the position of the host vehicle may be determined from information such as past position measurement history and beacons.

Next, in step 136, based on the position of the host vehicle calculated in step 134 and the acquired position coordinates of each GPS satellite, the angular relationship R _j (the position of each GPS satellite j and the position of the host vehicle). The elevation angle θ _{j with} respect to the horizontal direction and the azimuth angle φ _{j with} respect to the north direction are calculated as the directions of the respective GPS satellites.

Next, in step 138, the relative speed v _j of the host vehicle with respect to each GPS satellite calculated in step 130, and the velocity vector V _j (VX _j , VY _j , VZ _{j of} each GPS satellite calculated in step 132 above. ) And the direction R _j (θ _j , φ _j ) of each GPS satellite calculated in step 136, the speed Vv _j of the host vehicle in the direction of each GPS satellite _j is calculated according to the above equation (32). To do. The velocity Vs _j of the GPS satellite j in the direction of the vehicle in the equation (32) is calculated by Vs _j = R _j [VX _j , VY _j , VZ _j ] ^T.

  Next, in step 140, the optimal value of the speed vector (Vx, Vy, Vz) of the host vehicle is calculated according to the above equations (33) and (34), and the process returns.

  As described above, according to the posture angle slip angle estimating apparatus according to the first embodiment, a method for estimating the posture angle with respect to the vehicle body vertical axis, a method for estimating the vehicle longitudinal speed and the vehicle body slip angle, and Using the vehicle body yaw angle obtained from the GPS satellite, the roll angle and pitch angle obtained from the differential values of the roll angle and pitch angle calculated to estimate the roll angle and pitch angle and the motion equation of vehicle motion are calculated. The relationship between each differential amount and the value considering the sensor drift amount is equal, the front and rear vehicle body speeds obtained from the differential amounts of the front and rear vehicle body speeds and lateral vehicle body speeds calculated by the slip angle observer and the motion equations of the vehicle motion, and The relationship between the differential value of the horizontal vehicle body speed and the value considering the sensor drift amount, and the vehicle body yaw angle calculated based on GPS Doppler Using the relationship in which the differential value of the vehicle body yaw angle obtained from the equations of motion of both motions and the value obtained by integrating the value taking into account the sensor drift amount are equal, longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity, and roll angular velocity By estimating the sensor drift amount of the sensor that detects each of the above, the sensor that detects each of the longitudinal acceleration, the lateral acceleration, the vertical acceleration, the yaw angular velocity, and the roll angular velocity stably regardless of the state of the vehicle motion. The amount of sensor drift can be estimated.

  Further, by reflecting the estimated values of the posture angle and the slip angle on the vehicle body yaw angle calculation means, the vehicle body yaw angle considering the three-dimensional motion of the vehicle can be calculated. Further, even when the vehicle speed change is small, the sensor drift amount of the yaw angular velocity with high accuracy can be estimated by using the information from the GPS satellite.

  Further, by applying the attitude angle / slip angle estimation values obtained from the attitude angle observer and the slip angle observer to the vehicle velocity vector estimation value for the earth coordinate system using signals from GPS satellites, the vehicle 3 Focusing on the fact that the vehicle body yaw angle can be obtained in consideration of dimensional motion, the drift amount of the sensor signal is estimated from the relationship between the equation of motion when the vehicle state amount and the drift amount are considered. Since the vehicle body yaw angle signal obtained from the GPS satellite is superimposed with noise depending on the satellite reception environment and vehicle speed, the noise is removed and the drift amount is stably estimated.

  In addition, the roll angle and the pitch angle as the posture angle can be accurately estimated based on the sensor signal corrected by the estimated sensor drift amount.

  Further, the front and rear vehicle body speed and the lateral vehicle body speed can be accurately estimated based on the sensor signal corrected by the estimated sensor drift amount, and the vehicle body slip angle can be accurately estimated.

  Next, a second embodiment will be described. Note that the configuration of the posture angle slip angle estimation apparatus of the second embodiment is the same as that of the first embodiment, and therefore the same reference numerals are given and description thereof is omitted.

  The second embodiment is different from the first embodiment in that the forgetting factor is changed according to the vehicle speed when the drift estimation value is smoothed by the forgetting factor.

  First, the principle of estimating the sensor drift amount will be described.

  Since the vehicle body yaw angle obtained from the GPS signal is obtained from the relative speed between the satellite and the vehicle, the relative speed decreases when the vehicle speed is low, and the influence of noise in the GPS signal increases. Therefore, noise is superimposed on the estimated value of the vehicle body yaw angle obtained from the GPS signal, and there is a concern about the influence on the estimation accuracy of the drift amount of the yaw angular velocity. Therefore, when smoothing the drift estimation value based on the forgetting factor, the forgetting factor is changed according to the vehicle speed to reduce the influence of noise due to the reduction in the vehicle speed. That is, the above expression (65) is changed to the following expression (67).

The case where the vehicle speed range is divided into four vehicle speed ranges of U ₀ , 2U ₀ , 3U ₀ , 3U ₀ or more is shown in the above equation (67). Here, U _min is the minimum value of the vehicle speed that can be calculated from the GPS signal, and U ₀ is an appropriate value larger than U _min . The forgetting factor is λ ₂₁ > λ ₂₂ > λ ₂₃ > λ ₂₄ . U is the front-rear vehicle body speed calculated by the slip angle observer 28.

  In the drift amount estimating means 32 according to the present embodiment, in order to stabilize the calculation, the above values (62) to (64) are used for the calculation result of the equation (61) using the previous value and the longitudinal vehicle body speed. ), (66), and a smoothed value as in (67) are used as drift estimation values.

  In addition, about the other structure and effect | action of the attitude | position angle slip angle estimation apparatus which concern on 2nd Embodiment, since it is the same as that of 1st Embodiment, description is abbreviate | omitted.

  As described above, when smoothing the drift estimated value by the forgetting factor, the GPS signal is reduced by increasing the forgetting factor and reducing the influence degree of the drift estimating value in updating the sensor drift amount as the vehicle speed decreases. The influence of the noise can be suppressed, and the posture angle and the slip angle can be estimated with higher accuracy.

  Next, a third embodiment will be described. Note that the configuration of the posture angle slip angle estimation apparatus of the third embodiment is the same as that of the first embodiment, and therefore the same reference numerals are given and description thereof is omitted.

  The third embodiment differs from the first embodiment in that the maximum drift amount compensated by the sensor is used to reduce the influence of noise.

  First, the principle of estimating the sensor drift amount will be described.

When the number of captured satellites is not stable, or when GPS signals cannot be received constantly due to buildings, trees, etc., the noise of the GPS signals increases. Therefore, the influence of noise is reduced by using the relationship of the above equation (46) and the maximum drift amount compensated by the sensor. That is, the detected value of the vertical acceleration sensor 16, the detected value of the roll angular velocity sensor 18, the detected value of the yaw angular velocity sensor 20, the estimated roll angle by the attitude angle observer 26, the estimated yaw angle, and the longitudinal vehicle speed estimated by the slip angle observer 28. Value, the vehicle body yaw angle calculated from the lateral vehicle speed estimation value (the vehicle body yaw angle calculated from the expression obtained by _removing the drift amounts G _zdr , P _dr , and R _dr from the right side of the above equation (46)) and the vehicle body yaw angle calculation When the error from the vehicle body yaw angle obtained from the means 44 is larger than the maximum drift amount compensated by the sensor, the yaw angular velocity drift estimated value is not updated.

  The drift amount estimating means 32 according to the present embodiment uses the previous value and the front and rear vehicle body speeds for the calculation result of the above equation (61), as in the above equations (62) to (64) and (66). The smoothed value is taken as the drift estimation value.

Further, when updating the drift estimated value of the yaw angular velocity sensor 20, the drift amount estimating means 32 is an equation obtained by _removing the drift amounts G _zdr , P _dr , R _dr from the right side of the equation (46) (integral value of Δt seconds). The difference between the vehicle body yaw angle calculated from the vehicle body yaw angle and the vehicle body yaw angle calculation means 44 is obtained for N points before the drift estimation value is updated, and the average H _ψ for the N points before the update is calculated. (See FIG. 10). When the maximum zero point drift value compensated by the yaw angular velocity sensor 20 is R _drmax , the drift amount estimating means 32 does not update the estimated value of the yaw angular velocity drift amount when the following equation (68) is satisfied. On the other hand, when the following equation (68) is not satisfied, the estimated value of the yaw angular velocity drift amount is updated according to the above equation (65).

  Next, an estimation result obtained by the sensor drift amount estimation method according to the third embodiment will be described. In order to confirm the effect of the sensor drift amount estimation method, the drift of the yaw angular velocity sensor signal was simulated, the sensor drift amount was estimated, and the posture angle was estimated using this estimated value. .

  FIG. 11 shows the estimation result. Here, the continuous satellite capturing time is a time when GPS satellites are continuously captured without interruption. That is, it means that the signal from the satellite is interrupted at the timing when the continuous satellite acquisition time becomes 0 s. From this result, it can be seen that the drift estimation accuracy is good even when the satellite cannot be continuously acquired.

  In this way, the maximum value of the drift amount compensated by the sensor is used, and when the error of the vehicle body yaw angle exceeds the maximum value of the drift amount, the estimated value of the yaw angular velocity drift amount is not updated. Therefore, the influence of noise can be suppressed, and the posture angle and the slip angle can be estimated with higher accuracy.

  Next, a fourth embodiment will be described. In addition, about the structure and effect | action similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  The fourth embodiment is different from the first embodiment in that the drift amount is estimated using the time differential value of the vehicle body yaw angle obtained using the in-vehicle camera.

First, the attitude angle slip angle estimation apparatus 100 according to the fourth embodiment will be described. As shown in FIG. 12, the posture angle slip angle estimation device 100 according to the fourth embodiment includes a longitudinal acceleration G _x , a lateral acceleration G _y , and a vertical acceleration G _z that are xyz 3-axis accelerations of the vehicle motion. And the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the vertical acceleration sensor 16 that output a sensor signal corresponding to the detected value, and the roll angular velocity that detects the roll angular velocity P and outputs a sensor signal corresponding to the detected value. The sensor 18 includes a yaw angular velocity sensor 20 that detects a yaw angular velocity R and outputs a sensor signal corresponding to the detected value.

  The vertical acceleration sensor 16, the roll angular velocity sensor 18, the longitudinal acceleration sensor 12, the lateral acceleration sensor 14, and the yaw angular velocity sensor 20 are sensors from each sensor based on sensor drift amounts estimated by a drift amount estimation means 32 described later. It is connected to drift amount correction means 22 for correcting the signal. The drift amount correcting means 22 is connected to the drift amount estimating means 140.

  The imaging device 110 captures an image of the front or rear of the vehicle and generates an image signal of the image (not shown), and an A / D conversion unit (not shown) that A / D converts the image signal generated by the imaging unit. And an image memory (not shown) for temporarily storing the A / D converted image signal. The imaging device 110 outputs the A / D converted image signal to the motion estimation unit 120. In the present embodiment, an in-vehicle camera is used as the imaging device. In the present embodiment, there is one imaging device, but two imaging devices may be used.

  The motion estimation unit 120 calculates the time differential value of the vehicle body yaw angle by estimating the motion of the host vehicle, and outputs the calculated time differential value of the vehicle body yaw angle to the drift amount estimation unit 140. The time differential value of the vehicle body yaw angle is an example of the differential amount of the rotation angle and the differential amount of the vehicle body yaw angle.

  Next, the principle of estimating the drift amount using the time differential value of the vehicle body yaw angle will be described.

  In the present embodiment, the estimation accuracy of the yaw angular velocity drift can be improved by using the imaging device 110 (on-vehicle camera) and newly adding the balance equation (27) relating to the vehicle body yaw direction.

  Next, a method for calculating the time differential value of the vehicle body yaw angle using the image signal obtained by the imaging device 110 will be described.

The motion estimation unit 120 uses the image based on the image signal input from the imaging device 110 to extract a plurality of feature points that are easy to track, and how the feature point is in the image based on the image signal at the next time. By searching for the change, the motion of the host vehicle is estimated while determining the reliability of the feature point, and the time differential value of the vehicle body yaw angle is calculated from the estimated motion. Specifically, as shown in FIG. 13, given the coordinates of a vehicle planar motion, the vehicle body yaw angle from the time t ₁ at time t ₂ Assuming that change [Delta] [phi], the time derivative of the vehicle body yaw angle ,

It becomes. In an actual vehicle, unlike FIG. 13, rolls and pitches are also accompanied between time t ₁ and time t ₂ , but in the method of estimating motion from feature points, yaw angle time differentiation after separating these motions. The signal can be estimated. In FIG. 13, the roll motion and the pitch motion are not considered.

  When the motion estimation means 120 is expressed in functional blocks according to a motion estimation processing routine described below, as shown in FIG. 14, an image acquisition unit 122, a feature point extraction unit 124, a feature point feature amount calculation unit 126, This can be represented by a configuration including a corresponding point search unit 128, a feature vector generation unit 130, a learning result storage unit 132, a determination unit 134, and a motion estimation unit 136.

  The image acquisition unit 122 acquires an image captured from the imaging device 110.

  The feature point extraction unit 124 extracts a plurality of feature points from each of a plurality of captured images obtained from the imaging device 110. The feature points are automatically extracted using, for example, DoG (Difference of Gaussian), Harris-Laplace, or the like.

  The feature point feature amount calculation unit 126 calculates the feature amount of each feature point extracted by the feature point extraction unit 124.

  The corresponding point search unit 128 uses the feature amount of each feature point obtained from the feature point feature amount calculation unit 126 to associate feature points between a plurality of images, as will be described below. A feature point corresponding to a feature point of one image is searched from other images.

When obtaining the feature point of the image I ′ with respect to the feature point f _i of the image I in the two images I and I ′ that have been subjected to feature point extraction and feature amount calculation, the feature points extracted from the image I ′ Then, the feature point f _j ′ whose feature amount is closest to the feature amount of the feature point f _i is obtained. In this case, the ratio of a distance d ₂ between the _'distance d ₁ and the feature quantity of the feature point f _i and the feature amount f _k second closest to the feature amount _of' the feature quantity of the feature point f _i and the feature point f _j When d ₁ / d ₂ is smaller than a predetermined threshold value, the feature point f _i of the image I and the feature point f _j ′ of the image I ′ are associated with each other.

  The feature vector generation unit 130 generates a feature vector for each of the feature points extracted by the feature point extraction unit 124 using a histogram that is the number distribution of feature amounts of the feature point of interest and the feature points existing around the feature point. To do.

  The learning result storage unit 132 stores a relationship between a feature vector obtained as a learning result by a learning process described below and labels indicating inliers and outliers.

  In the learning process, first, feature points are extracted, feature points are calculated, and corresponding feature points are searched for a plurality of learning images prepared in the same manner as described above. Correspondences between points, feature amounts, and feature points between images are obtained. Then, out of the feature points extracted from the learning image by the operator's operation, feature points that have an incorrect correspondence between the feature points between the images and feature points that exist on the moving object are labeled as outliers. And label feature points that are not so as inliers. Next, a feature vector is generated for the labeled feature point in the same manner as the feature vector generating unit 130, and the feature vector, the feature vector of the feature point, and the label are input as inputs. A learning process for determining inliers and outliers is performed on the images, and the relationship between the feature vectors and the labels indicating the inliers and outliers is learned. Note that the inlier or outlier labeled for the feature point indicates whether or not the correspondence of the feature point can be trusted at the time of motion estimation, and corresponds to whether or not the feature point can be trusted.

  Based on the feature vector of each feature point generated by the feature vector generation unit 130 and the relationship between the feature vector and the label as the learning result stored in the learning result storage unit 132, the determination unit 134 The inlier and outlier are judged.

  The motion estimation unit 136 uses the correspondence between the feature points determined to be inliers by the determination unit 134 to determine the relative positions and orientations of the plurality of images when the plurality of images are captured. Estimated as vehicle motion.

  When calculating changes in position and orientation (movement amount and rotation amount in three-dimensional space) when two images are captured from the coordinates of corresponding feature points in the two images, changes in the position and orientation to be calculated Is a six-degree-of-freedom motion composed of a rotation matrix R and a translation vector t from the first image to the second image.

Here, a calculation method of the rotation matrix R and the translation vector t from the first image to the second image will be described. Regarding the image coordinates I _i of the corresponding points of the n points in the first image and the image coordinates I ′ _i of the corresponding points of the n points in the second image (n ≧ 8), the correspondence between the corresponding points is correct and there is an error. Otherwise, there is a 3 × 3 matrix F that satisfies the following expression (70) as a matrix indicating the change in position and orientation.

However, I _i = (u _i , v _i , 1) ^T , I ′ _i = (u ′ _i , v ′ _i , 1) ^T and image coordinates (u _i , v _i ) in the first image The image coordinates of the point in the second image corresponding to the point are (u ′ _i , v ′ _i ).

Here, the matrix F satisfying the above equation (70) has a constant multiple indefiniteness. That is, when F _s satisfies the above equation (70), α F _s also satisfies the above equation (70) (where α is a real number). Therefore, the matrix F can be expressed as the following equation (71).

  Moreover, the following (72) Formula is obtained from the said (70) Formula and (71) Formula.

Here, if there are eight or more pairs of corresponding points I _i and I ′ _i , at least eight of the above-described expression (72) can be obtained, so that eight variables f _{11 to} f ₃₂ can be obtained. That is, if there are eight or more feature points determined as inliers by the determination unit 134, the matrix F can be calculated.

  In addition, since mis-corresponding feature points and feature points on moving objects may include points that are erroneously determined as inliers, among the feature points determined as inliers, the correspondence relationship is 70) A matrix F having the largest number of feature points satisfying equation 70 is obtained.

  When the correspondence between the feature points is obtained in the real image, the corresponding image coordinates include an error, and thus the above equation (70) is not strictly satisfied even with the correct correspondence. Therefore, when the matrix F is given, whether or not the correspondence between the feature points satisfies the above equation (70) is determined by whether or not the following equation (73) is satisfied.

  However, d (a, b) represents the distance between a and b, and τ is a preset threshold value.

  Further, when the calibration matrix K of the imaging device 110 is known, the rotation matrix R and the translation vector according to the following equations (74) and (75) based on the matrix F specified as described above. t is calculated and output as a motion (change in position and orientation) of the imaging device 110 when each of the first image and the second image is captured.

With the above method, the motion estimation unit 136 performs the first operation based on the image coordinates of the plurality of sets of corresponding feature points in the first image and the second image, and the inlier and outlier determination results of each feature point. Changes in the position and orientation of the imaging device 110 when the first image and the second image are captured are calculated. Motion estimation unit 136, of the change in the attitude of the imaging device 110 between the first image and the time t ₁ which is captured and t ₂ the second time the image is captured, a change in the vehicle body yaw angle Based on the amount Δψ, the time differential value of the vehicle body yaw angle is calculated according to the above equation (69) and output to the drift amount estimating means 140.

  Next, a method for calculating each differential amount obtained from the motion equation of vehicle motion will be described.

  In the present embodiment, assuming that a constant drift error is superimposed on the sensor signal with respect to the equation (27) representing the relational expression between the vehicle body yaw angle and the angular velocity sensor signal, the equation (46) It is replaced with equation (76). In the following equation (76), dψ is a time differential value of the vehicle body yaw angle calculated by the motion estimation unit 136 according to the equation (69).

  Here, the estimated value of equation (6) was used for the pitch angular velocity Q.

  The motion equation differential amount calculating means 30 calculates the differential amounts of the roll angle φ and the pitch angle θ obtained from the motion equation of vehicle motion according to the above equations (44) and (45).

  The motion equation differential amount calculation means 30 calculates the differential amounts of the longitudinal vehicle body speed U and the lateral vehicle body speed V obtained from the motion equation of vehicle motion according to the above equations (42) and (43).

  The motion equation differential amount calculating means 30 calculates the differential amount of the vehicle body yaw angle ψ obtained from the vehicle motion equation according to the right side of the equation (76).

  Next, a sensor drift amount estimation method will be described.

  When the above (42), (43), (44), and (45) are arranged into equations relating to drift, they are expressed as the above equations (47) to (50). Further, when the above equation (76) is rearranged into an equation relating to drift, it is expressed as the following equation (77).

  The above expression (77) represents a relationship in which the time differential value of the vehicle body yaw angle calculated by the motion estimation unit 120 is equal to the value obtained by taking the sensor drift amount into the differential value of the vehicle body yaw angle obtained from the motion equation. ing.

  In the present embodiment, the equation (47) to (50), (77), and (53) can be described as the above equation (56). However, in the present embodiment, the condition represented by the above equation (58) is replaced by the following equation (78).

Here, t _smp is a sampling time used for the calculation of the entire system. When the coefficient matrix on the left side of equations (57) and (78) and the vector on the right side are integrated for a fixed time, the above equation (59) is obtained.

  The drift amount estimating means 140 calculates the differential amounts of the roll angle and the pitch angle calculated by the attitude angle observer 26 and the longitudinal vehicle body speed and the lateral vehicle body speed calculated by the slip angle observer 28 according to the above equation (61). Each differential amount, a time differential value of the vehicle body yaw angle calculated by the motion estimation means 120, a roll angle, a pitch angle, a front and rear vehicle body speed, a lateral vehicle body speed, and a vehicle body speed calculated by the motion equation differential amount calculation means 30 Based on the differential amount of each yaw angle, the sensor drift amount of the longitudinal acceleration sensor 12, the sensor drift amount of the lateral acceleration sensor 14, the sensor drift amount of the vertical acceleration sensor 16, the sensor drift amount of the roll angular velocity sensor 18, and the yaw amount The sensor drift amount of the angular velocity sensor 20 can be estimated.

  In the fourth embodiment, a program that causes a computer to function as each of the drift amount correcting means 22, the attitude angle observer 26, the slip angle observer 28, the motion equation differential amount calculating means 30, and the drift amount estimating means 32. The information processing by can be realized by the posture angle estimation processing routine shown in the flowchart of FIG. The computer includes a CPU, a ROM, and a RAM connected to each other by a bus, and an HDD connected as necessary. These programs are recorded on a recording medium such as a ROM or an HDD connected to the CPU of the computer. To be recorded.

  This posture angle estimation processing routine will be described. In step 200, the posture angle estimated value obtained in step 112 before one calculation cycle, the vehicle body speed estimation value obtained in step 114 before one calculation cycle, and a later-described step. Based on the time differential value of the vehicle body yaw angle calculated in step 404 one calculation cycle before, as described above, the differential amounts of the vehicle longitudinal speed and the vehicle lateral speed calculated in step 106 are as follows. , The relationship in which the differential values of the vehicle longitudinal speed and the vehicle lateral speed obtained from the equation of motion of the vehicle are equal to the values considering the sensor drift amount, each of the roll angle and pitch angle calculated in step 104 above The relationship in which the differential amount is equal to the differential value of each of the roll angle and pitch angle obtained from the equation of motion of the vehicle motion, taking into account the sensor drift amount, and The longitudinal acceleration sensor 12 is calculated using the relationship in which the time derivative value of the vehicle body yaw angle calculated in step 404 is equal to the derivative value of the vehicle yaw angle obtained from the equation of motion of the vehicle and the value considering the sensor drift amount. Sensor drift amount, lateral acceleration sensor 14 sensor drift amount, vertical acceleration sensor 16 sensor drift amount, roll angular velocity sensor 18 sensor drift amount, and yaw angular velocity sensor 20 sensor drift amount are estimated.

  Information processing by a program that causes a computer to function as the motion estimation means 120 can be realized by a time differential value estimation processing routine of the vehicle body yaw angle shown in the flowchart of FIG.

  Next, the vehicle body yaw angle time differential value estimation processing routine shown in FIG. 16 will be described. In step 400, image signals of a first image and a second image continuously captured by the imaging device 110 are acquired.

  Next, in step 402, a motion estimation process described later is executed to estimate the motion of the host vehicle. Next, in step 404, based on the motion of the host vehicle estimated in step 402, the vehicle body yaw angle is determined. Is calculated, and the process returns to step 400.

  Next, the motion estimation processing routine will be described with reference to FIG.

  In step 500, a plurality of feature points are extracted from the first image and the second image acquired in step 400. In step 502, a feature amount is calculated for each of the plurality of feature points extracted from the first image and the second image in step 500.

  In the next step 504, based on the feature amount calculated in step 502, feature points corresponding to each of the plurality of feature points on the first image extracted in step 500 are displayed on the second image. Search and track with.

  In step 506, a feature vector is generated for each feature point on the first image. In step 508, the relationship between the learned feature vector stored in the learning result storage unit 132 and the label, and Based on the feature vector of each feature point generated in step 506, it is determined whether each feature point on the first image is an inlier or an outlier.

  In the next step 510, the matrix F is calculated based on an arbitrary combination of the correspondences between the feature points determined to be inliers in step 508 and the corresponding feature points, and the correspondence relationship is expressed by the above equation (73). A matrix F having the largest number of feature points satisfying is obtained. Then, based on the obtained matrix F, the first position and the first posture of the imaging device 110 when the first image is captured according to the above formulas (74) and (75), and the second As a change in the second position and the second posture of the image pickup apparatus 110 when the image is picked up, the movement amount of the movement of the image pickup apparatus 110 in the XYZ axis direction and the rotation amount based on the XYZ axis are calculated, and the image pickup is performed. The motion estimation processing routine of the host vehicle on which the device 110 is mounted is output to the external device as the motion estimation result during the two hours.

  As described above, according to the posture angle slip angle estimating apparatus according to the fourth embodiment, a method for estimating the posture angle with respect to the vehicle body vertical axis, a method for estimating the vehicle longitudinal velocity and estimating the vehicle body slip angle, and The roll angle obtained from the differential value of each roll angle and pitch angle calculated to estimate the roll angle and pitch angle and the motion equation of vehicle motion, using the time differential value of the vehicle body yaw angle obtained from the imaging device And the differential value of each of the pitch angles is equal to the value considering the sensor drift amount, the differential values of the front and rear vehicle body speeds and the lateral vehicle body speeds calculated by the slip angle observer and the equation of motion of the vehicle motion. The relationship in which the differential values of the front and rear vehicle body speeds and the lateral vehicle body speeds are equal to the values considering the sensor drift amount, and the vehicle body yaw angle calculated using the imaging device Using the relationship in which the differential value of the vehicle yaw angle obtained from the time differential value and the equation of motion of the vehicle is equal to the value considering the sensor drift amount, longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity, and roll angular velocity By estimating the sensor drift amount of the sensor that detects each of the above, the sensor that detects each of the longitudinal acceleration, the lateral acceleration, the vertical acceleration, the yaw angular velocity, and the roll angular velocity stably regardless of the state of the vehicle motion. The amount of sensor drift can be estimated.

  In addition, by estimating the movement of the vehicle from the image of the camera mounted on the vehicle and obtaining the time differential value of the vehicle body yaw angle that is not affected by the vehicle body slip angle, the vehicle state amount and the drift amount are taken into account. The drift amount of the sensor signal can be estimated from the relationship of the equation of motion.

  Next, a fifth embodiment will be described. In addition, about the structure and effect | action of the attitude angle slip angle estimation apparatus of 5th Embodiment, about the part which becomes the same structure as 1st-4th Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted. .

  The fifth embodiment is different from the first to fourth embodiments in that the maximum drift amount compensated by the sensor is used to reduce the influence of noise.

  First, the principle of estimating the sensor drift amount will be described.

When the surrounding environment is extremely bright or dark, or when the vehicle continues to move and the same image continues, the time derivative of the yaw angle estimated from the camera will be used. Large noise is easily superimposed. Therefore, the influence of noise is reduced by using the relationship of the above equation (76) and the maximum drift amount guaranteed by the sensor. That is, the detected value of the vertical acceleration sensor 16, the detected value of the roll angular velocity sensor 18, the detected value of the yaw angular velocity sensor 20, the estimated roll angle by the attitude angle observer 26, the estimated yaw angle, and the longitudinal vehicle speed estimated by the slip angle observer 28. Vehicle body yaw angle derivative calculated from the value and lateral vehicle speed estimated value (the vehicle body yaw angle derivative calculated from the expression obtained by removing the drift amounts G _zdr , P _dr , and R _dr from the right side of the above equation (76)) ) And the time differential value of the vehicle body yaw angle obtained from the motion estimation means 120 is larger than the maximum drift amount compensated by the sensor, the yaw angular velocity drift estimated value is not updated.

  The drift amount estimating means 32 according to the present embodiment uses the previous value and the front and rear vehicle body speeds for the calculation result of the above equation (61), as in the above equations (62) to (64) and (66). The smoothed value is taken as the drift estimation value.

Further, when the drift amount estimation means 140 updates the drift estimation value of the yaw angular velocity sensor 20, the vehicle body yaw angle calculated from the equation obtained by removing the drift amounts G _zdr , P _dr , and R _dr from the right side of the equation (76). (N) before updating the drift estimation value, and calculating an average H _ψ for the N point before the update (the difference between the differential amount and the time differential value of the vehicle body yaw angle obtained from the motion estimation means 120). (See FIG. 18). When the maximum zero point drift value compensated by the yaw angular velocity sensor 20 is R _drmax , the drift amount estimating means 140 does not update the estimated value of the yaw angular velocity drift amount when the following equation (79) is satisfied. On the other hand, when the following expression (79) is not satisfied, the estimated value of the yaw angular velocity drift amount is updated according to the above expression (65).

  As described above, according to the posture angle slip angle estimating apparatus according to the fifth embodiment, the maximum value of the drift amount compensated by the sensor is used, and the error of the time differential value of the vehicle body yaw angle is the drift amount. If the estimated value of the yaw angular velocity drift amount is not updated when the maximum value is exceeded, the influence of noise can be suppressed, and the attitude angle and slip angle can be estimated more accurately. .

  Further, since the time derivative signal of the vehicle body yaw angle obtained from the image is superimposed with noise depending on the shooting environment, this noise can be removed and the drift amount can be estimated stably.

DESCRIPTION OF SYMBOLS 10 Attitude angle slip angle estimation apparatus 12 Longitudinal acceleration sensor 14 Lateral acceleration sensor 16 Vertical acceleration sensor 18 Roll angular velocity sensor 20 Yaw angular velocity sensor 22 Drift amount correction means 26 Attitude angle observer 28 Slip angle observer 30 Motion equation differential amount calculation means 32 Drift amount Estimating means 40 GPS receiving means 42 Vehicle speed vector estimating means 44 Vehicle body yaw angle calculating means 100 Attitude angle slip angle estimating apparatus 110 Imaging apparatus 120 Motion estimating means 140 Drift amount estimating means

Claims (21)

Based on the sensor signal corresponding to the detected value of the motion state quantity of the vehicle motion, the differential amount of the posture angle with respect to the vertical axis of the vehicle body is calculated, and the posture angle is estimated by integrating the calculated differential amount of the posture angle. Posture angle estimating means for performing,
Based on the sensor signal, a differential amount of the vehicle speed is calculated, the differential amount of the vehicle speed is integrated, the vehicle speed is estimated, and a slip angle is estimated based on the estimated vehicle speed. Slip angle estimating means;
Based on the posture angle estimated by the sensor signal and the posture angle estimating means, a differential amount of the posture angle obtained from a motion equation of vehicle motion is calculated, and is estimated by the sensor signal and the slip angle estimating means. Based on the vehicle speed, a differential amount of the vehicle speed obtained from a motion equation of vehicle motion is calculated, and the sensor signal, the posture angle estimated by the posture angle estimating means, and the slip angle estimating means are used. A calculating means for calculating a differential amount of a rotation angle about a vehicle body vertical axis obtained from a motion equation of vehicle motion based on the estimated vehicle speed;
Information regarding the position of each of the information transmission sources, information regarding the distance between each of the information transmission sources and the vehicle, and the relative speed of the vehicle with respect to each of the information transmission sources, transmitted from each of a plurality of information transmission sources. Obtaining means for obtaining source information including information on
Vehicle speed vector estimation means for estimating a speed vector of the vehicle based on the transmission source information acquired by the acquisition means;
Based on the posture angle estimated by the posture angle estimation means, the slip angle estimated by the slip angle estimation means, and the vehicle speed vector estimated by the vehicle speed vector estimation means, the rotation angle is calculated. Vehicle body rotation angle calculating means for calculating;
When the sensor drift amount of the sensor signal is taken into consideration, the sensor drift amount is considered in the differential amount of the posture angle calculated by the posture angle estimation unit and the differential amount of the posture angle calculated by the calculation unit. The differential value of the vehicle speed calculated by the slip angle estimating means is equal to the value of the differential value of the vehicle speed calculated by the calculating means in consideration of the sensor drift amount. And the relationship in which the rotation angle calculated by the vehicle body rotation angle calculation means is equal to the value obtained by integrating the differential amount of the rotation angle calculated by the calculation means with the value considering the sensor drift amount. Drift amount estimation means for estimating the sensor drift amount of the sensor signal using
Sensor drift amount estimation apparatus including Further comprising correction means for correcting the sensor signal based on the sensor drift amount estimated by the drift amount estimation means,
The posture angle estimation means estimates the posture angle based on the sensor signal corrected by the correction means,
The sensor drift amount estimating apparatus according to claim 1, wherein the slip angle estimating means estimates the slip angle based on the sensor signal corrected by the correcting means. Based on sensor signals corresponding to the detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of the vehicle motion, the differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, Attitude angle estimating means for integrating the calculated differential amounts of the roll angle and the pitch angle to estimate the roll angle and the pitch angle;
Based on the sensor signal corresponding to each detected value, the respective differential amounts of the longitudinal speed and the lateral speed are calculated, and the differential amounts of the longitudinal speed and the lateral speed are integrated to obtain the longitudinal speed and the lateral speed. Slip angle estimation means for estimating a slip angle based on the estimated longitudinal speed and lateral speed,
Based on the sensor signal and the roll angle and the pitch angle estimated by the posture angle estimating means, the differential amounts of the roll angle and the pitch angle obtained from the motion equation of vehicle motion are calculated, Based on the sensor signal and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, the differential amounts of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion are calculated, Based on the sensor signal, the roll angle and the pitch angle estimated by the posture angle estimating means, and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, obtained from a motion equation of vehicle motion. Calculating means for calculating the differential amount of the vehicle body yaw angle around the vehicle body vertical axis;
Information regarding the position of each of the information transmission sources, information regarding the distance between each of the information transmission sources and the vehicle, and the relative speed of the vehicle with respect to each of the information transmission sources, transmitted from each of a plurality of information transmission sources. Obtaining means for obtaining source information including information on
Vehicle speed vector estimation means for estimating a speed vector of the vehicle based on the transmission source information acquired by the acquisition means;
Based on the roll angle and pitch angle estimated by the posture angle estimation means, the slip angle estimated by the slip angle estimation means, and the vehicle speed vector estimated by the vehicle speed vector estimation means, Vehicle body yaw angle calculating means for calculating the vehicle body yaw angle;
When the sensor drift amount of the sensor signal is taken into account, the respective differential amounts of the roll angle and the pitch angle calculated by the posture angle estimation unit, and the roll angle and the pitch angle calculated by the calculation unit Each differential amount is equal to a value in consideration of the sensor drift amount, each differential amount of the longitudinal speed and the lateral speed calculated by the slip angle estimating means, and calculated by the calculating means Calculated by the calculation means, the relationship in which the differential value of each of the longitudinal speed and the lateral speed is equal to the value considering the sensor drift amount, the vehicle body yaw angle calculated by the vehicle body yaw angle calculation means, and Further, the vertical acceleration, the front-rear direction, and the front-rear acceleration are obtained by using a relationship in which a differential value of the vehicle body yaw angle is equal to a value obtained by integrating a value in consideration of the sensor drift amount Speed, and the lateral acceleration, drift amount estimating means for estimating the sensor drift amount of the roll angular velocity, and sensor signals corresponding to the respective detected values of the yaw angular velocity,
Sensor drift amount estimation apparatus including   The drift amount estimation means estimates a sensor drift amount of a sensor signal corresponding to each detected value of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity, and the estimated sensor The sensor drift amount estimation apparatus according to claim 3, wherein the sensor drift amount is updated based on a drift amount and the previously estimated sensor drift amount.   The drift amount estimating means updates the sensor drift amount by using a sensor drift amount of a sensor signal corresponding to the estimated yaw angular velocity, thereby obtaining an absolute value of the roll angle estimated by the posture angle estimating means. Only when the value is smaller, the smaller the estimated longitudinal speed is, the smaller the degree of influence of the sensor drift amount of the sensor signal according to the estimated yaw angular velocity in the update of the sensor drift amount is, The sensor drift amount estimation apparatus according to claim 4, wherein a sensor drift amount of a sensor signal corresponding to the yaw angular velocity is updated.   The drift amount estimation means has an absolute value of a difference between the vehicle body yaw angle calculated by the vehicle body yaw angle calculation means and a value obtained by integrating the differential amount of the vehicle body yaw angle calculated by the calculation means in advance. Only when it is less than a predetermined value, based on the sensor drift amount of the sensor signal corresponding to the estimated yaw angular velocity and the sensor drift amount of the sensor signal corresponding to the yaw angular velocity estimated last time, The sensor drift amount estimation apparatus according to claim 4, wherein the sensor drift amount of the sensor signal corresponding to the angular velocity is updated.   The drift amount estimating means updates the sensor drift amount using a sensor drift amount of a sensor signal corresponding to the estimated lateral acceleration, thereby obtaining an absolute value of the roll angle estimated by the posture angle estimating means. Only when the value is small, based on the sensor drift amount of the sensor signal according to the estimated lateral acceleration and the sensor drift amount of the sensor signal according to the previously estimated lateral acceleration, according to the lateral acceleration. The sensor drift amount estimation apparatus according to claim 4, wherein the sensor drift amount of the sensor signal is updated.   The drift amount estimating means updates the sensor drift amount using a sensor drift amount of a sensor signal corresponding to the estimated longitudinal acceleration, thereby obtaining an absolute value of the pitch angle estimated by the posture angle estimating means. Only in the case where the value is small, based on the sensor drift amount of the sensor signal corresponding to the estimated longitudinal acceleration and the sensor drift amount of the sensor signal corresponding to the previously estimated longitudinal acceleration, according to the longitudinal acceleration. The sensor drift amount estimation apparatus according to claim 4, wherein the sensor drift amount of the sensor signal is updated. Based on the sensor drift amount estimated by the drift amount estimating means, further includes a correcting means for correcting a sensor signal corresponding to each detected value,
The posture angle estimation means estimates the roll angle and the pitch angle based on a sensor signal corresponding to each detection value corrected by the correction means,
The slip angle estimating means estimates the longitudinal speed and the lateral speed based on a sensor signal corresponding to each detected value corrected by the correcting means, and based on the estimated longitudinal speed and the lateral speed. The sensor drift amount estimation apparatus according to any one of claims 4 to 8, which estimates a slip angle. Computer
Based on the sensor signal corresponding to the detected value of the motion state quantity of the vehicle motion, the differential amount of the posture angle with respect to the vertical axis of the vehicle body is calculated, and the posture angle is estimated by integrating the calculated differential amount of the posture angle. Posture angle estimating means to perform,
Based on the sensor signal, a differential amount of the vehicle speed is calculated, the differential amount of the vehicle speed is integrated, the vehicle speed is estimated, and a slip angle is estimated based on the estimated vehicle speed. Slip angle estimating means,
Based on the posture angle estimated by the sensor signal and the posture angle estimating means, a differential amount of the posture angle obtained from a motion equation of vehicle motion is calculated, and is estimated by the sensor signal and the slip angle estimating means. Based on the vehicle speed, a differential amount of the vehicle speed obtained from a motion equation of vehicle motion is calculated, and the sensor signal, the posture angle estimated by the posture angle estimating means, and the slip angle estimating means are used. A calculating means for calculating a differential amount of a rotation angle about a vehicle body vertical axis obtained from a motion equation of vehicle motion based on the estimated vehicle speed;
Information regarding the position of each of the information transmission sources, information regarding the distance between each of the information transmission sources and the vehicle, and the relative speed of the vehicle with respect to each of the information transmission sources, transmitted from each of a plurality of information transmission sources. Acquisition means for acquiring transmission source information including information on,
Vehicle speed vector estimation means for estimating a speed vector of the vehicle based on the transmission source information acquired by the acquisition means;
Based on the posture angle estimated by the posture angle estimation means, the slip angle estimated by the slip angle estimation means, and the vehicle speed vector estimated by the vehicle speed vector estimation means, the rotation angle is calculated. Vehicle body rotation angle calculating means for calculating,
When the sensor drift amount of the sensor signal is taken into consideration, the sensor drift amount is considered in the differential amount of the posture angle calculated by the posture angle estimation unit and the differential amount of the posture angle calculated by the calculation unit. The differential value of the vehicle speed calculated by the slip angle estimating means is equal to the value of the differential value of the vehicle speed calculated by the calculating means in consideration of the sensor drift amount. And the relationship in which the rotation angle calculated by the vehicle body rotation angle calculation means is equal to the value obtained by integrating the differential amount of the rotation angle calculated by the calculation means with the value considering the sensor drift amount. A program for functioning as a drift amount estimating means for estimating a sensor drift amount of the sensor signal. Computer
Based on sensor signals corresponding to the detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of the vehicle motion, the differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, Attitude angle estimation means for integrating the calculated differential amounts of the roll angle and the pitch angle to estimate the roll angle and the pitch angle;
Based on the sensor signal corresponding to each detected value, the respective differential amounts of the longitudinal speed and the lateral speed are calculated, and the differential amounts of the longitudinal speed and the lateral speed are integrated to obtain the longitudinal speed and the lateral speed. Slip angle estimating means for estimating a slip angle based on the estimated longitudinal speed and lateral speed,
Based on the sensor signal and the roll angle and the pitch angle estimated by the posture angle estimating means, the differential amounts of the roll angle and the pitch angle obtained from the motion equation of vehicle motion are calculated, Based on the sensor signal and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, the differential amounts of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion are calculated, Based on the sensor signal, the roll angle and the pitch angle estimated by the posture angle estimating means, and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, obtained from a motion equation of vehicle motion. Calculating means for calculating a differential amount of the vehicle body yaw angle around the vehicle body vertical axis;
Information regarding the position of each of the information transmission sources, information regarding the distance between each of the information transmission sources and the vehicle, and the relative speed of the vehicle with respect to each of the information transmission sources, transmitted from each of a plurality of information transmission sources. Acquisition means for acquiring transmission source information including information on,
Vehicle speed vector estimation means for estimating a speed vector of the vehicle based on the transmission source information acquired by the acquisition means;
Based on the roll angle and pitch angle estimated by the posture angle estimation means, the slip angle estimated by the slip angle estimation means, and the vehicle speed vector estimated by the vehicle speed vector estimation means, A vehicle body yaw angle calculating means for calculating the vehicle body yaw angle, and a differential amount of each of the roll angle and the pitch angle calculated by the attitude angle estimating means when considering a sensor drift amount of the sensor signal, A relationship in which a differential value of each of the roll angle and the pitch angle calculated by the calculation unit is equal to a value considering the sensor drift amount, the longitudinal speed and the lateral speed calculated by the slip angle estimation unit And the differential amount of each of the longitudinal speed and the lateral speed calculated by the calculating means. The sensor drift amount is considered in the relationship in which the value considering the sensor drift amount is equal, the vehicle body yaw angle calculated by the vehicle body yaw angle calculating means, and the differential amount of the vehicle body yaw angle calculated by the calculating means. The sensor drift amount of the sensor signal is estimated according to the detected values of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity using a relationship in which the integrated value becomes equal. A program for functioning as drift amount estimation means. Based on the sensor signal corresponding to the detected value of the motion state quantity of the vehicle motion, the differential amount of the posture angle with respect to the vertical axis of the vehicle body is calculated, and the posture angle is estimated by integrating the calculated differential amount of the posture angle. Posture angle estimating means for performing,
Based on the sensor signal, a differential amount of the vehicle speed is calculated, the differential amount of the vehicle speed is integrated, the vehicle speed is estimated, and a slip angle is estimated based on the estimated vehicle speed. Slip angle estimating means;
Based on the posture angle estimated by the sensor signal and the posture angle estimating means, a differential amount of the posture angle obtained from a motion equation of vehicle motion is calculated, and is estimated by the sensor signal and the slip angle estimating means. Based on the vehicle speed, a differential amount of the vehicle speed obtained from a motion equation of vehicle motion is calculated, and the sensor signal, the posture angle estimated by the posture angle estimating means, and the slip angle estimating means are used. A calculating means for calculating a differential amount of a rotation angle about a vehicle body vertical axis obtained from a motion equation of vehicle motion based on the estimated vehicle speed;
A plurality of feature points are extracted from each of a plurality of images obtained by imaging the outside of the vehicle body, and based on the extracted plurality of feature points, a motion of the vehicle when the plurality of images are captured is estimated, and the estimation Motion estimation means for calculating a differential amount of the rotation angle based on the motion of the vehicle,
When the sensor drift amount of the sensor signal is taken into consideration, the sensor drift amount is considered in the differential amount of the posture angle calculated by the posture angle estimation unit and the differential amount of the posture angle calculated by the calculation unit. The differential value of the vehicle speed calculated by the slip angle estimating means is equal to the value of the differential value of the vehicle speed calculated by the calculating means in consideration of the sensor drift amount. And the relationship in which the differential amount of the rotation angle calculated by the motion estimation unit is equal to the value of the differential amount of the rotation angle calculated by the calculation unit in consideration of the sensor drift amount. , Drift amount estimating means for estimating a sensor drift amount of the sensor signal;
Sensor drift amount estimation apparatus including Further comprising correction means for correcting the sensor signal based on the sensor drift amount estimated by the drift amount estimation means,
The posture angle estimation means estimates the posture angle based on the sensor signal corrected by the correction means,
The sensor drift amount estimating device according to claim 12, wherein the slip angle estimating means estimates the slip angle based on the sensor signal corrected by the correcting means. Based on sensor signals corresponding to the detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of the vehicle motion, the differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, Attitude angle estimating means for integrating the calculated differential amounts of the roll angle and the pitch angle to estimate the roll angle and the pitch angle;
Based on the sensor signal corresponding to each detected value, the respective differential amounts of the longitudinal speed and the lateral speed are calculated, and the differential amounts of the longitudinal speed and the lateral speed are integrated to obtain the longitudinal speed and the lateral speed. Slip angle estimation means for estimating a slip angle based on the estimated longitudinal speed and lateral speed,
Based on the sensor signal and the roll angle and the pitch angle estimated by the posture angle estimating means, the differential amounts of the roll angle and the pitch angle obtained from the motion equation of vehicle motion are calculated, Based on the sensor signal and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, the differential amounts of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion are calculated, Based on the sensor signal, the roll angle and the pitch angle estimated by the posture angle estimating means, and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, obtained from a motion equation of vehicle motion. Calculating means for calculating the differential amount of the vehicle body yaw angle around the vehicle body vertical axis;
A plurality of feature points are extracted from each of a plurality of images obtained by imaging the outside of the vehicle body, and based on the extracted plurality of feature points, a motion of the vehicle when the plurality of images are captured is estimated, and the estimation Motion estimation means for calculating a differential amount of the vehicle body yaw angle based on the motion of the vehicle,
When the sensor drift amount of the sensor signal is taken into account, the respective differential amounts of the roll angle and the pitch angle calculated by the posture angle estimation unit, and the roll angle and the pitch angle calculated by the calculation unit Each differential amount is equal to a value in consideration of the sensor drift amount, each differential amount of the longitudinal speed and the lateral speed calculated by the slip angle estimating means, and calculated by the calculating means The relationship between the differential amount of each of the longitudinal speed and the lateral speed is equal to the value considering the sensor drift amount, and the differential amount of the vehicle body yaw angle calculated by the motion estimation means, and the calculation means The vertical acceleration, the longitudinal acceleration, the front acceleration, the front acceleration, the longitudinal acceleration, and the front And the lateral acceleration, drift amount estimating means for estimating the sensor drift amount of the roll angular velocity, and the sensor signals corresponding to the respective detected values of the yaw angular velocity,
Sensor drift amount estimation apparatus including   The drift amount estimation means estimates a sensor drift amount of a sensor signal corresponding to each detected value of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity, and the estimated sensor The sensor drift amount estimation device according to claim 14, wherein the sensor drift amount is updated based on a drift amount and the sensor drift amount estimated last time.   The drift amount estimation means has a predetermined absolute value of a difference between the differential amount of the vehicle body yaw angle calculated by the motion estimation means and the differential amount of the vehicle body yaw angle calculated by the calculation means. Only when it is less than the value, based on the sensor drift amount of the sensor signal according to the estimated yaw angular velocity and the sensor drift amount of the sensor signal according to the yaw angular velocity estimated last time, according to the yaw angular velocity. The sensor drift amount estimation apparatus according to claim 15, wherein the sensor drift amount of the detected sensor signal is updated.   The drift amount estimating means updates the sensor drift amount using a sensor drift amount of a sensor signal corresponding to the estimated lateral acceleration, thereby obtaining an absolute value of the roll angle estimated by the posture angle estimating means. Only when the value is small, based on the sensor drift amount of the sensor signal according to the estimated lateral acceleration and the sensor drift amount of the sensor signal according to the previously estimated lateral acceleration, according to the lateral acceleration. The sensor drift amount estimation apparatus according to any one of claims 15 to 16, wherein the sensor drift amount of the sensor signal is updated.   The drift amount estimating means updates the sensor drift amount using a sensor drift amount of a sensor signal corresponding to the estimated longitudinal acceleration, thereby obtaining an absolute value of the pitch angle estimated by the posture angle estimating means. Only in the case where the value is small, based on the sensor drift amount of the sensor signal corresponding to the estimated longitudinal acceleration and the sensor drift amount of the sensor signal corresponding to the previously estimated longitudinal acceleration, according to the longitudinal acceleration. The sensor drift amount estimation apparatus according to any one of claims 15 to 17, wherein the sensor drift amount of the sensor signal is updated. Based on the sensor drift amount estimated by the drift amount estimating means, further includes a correcting means for correcting a sensor signal corresponding to each detected value,
The posture angle estimation means estimates the roll angle and the pitch angle based on a sensor signal corresponding to each detection value corrected by the correction means,
The slip angle estimating means estimates the longitudinal speed and the lateral speed based on a sensor signal corresponding to each detected value corrected by the correcting means, and based on the estimated longitudinal speed and the lateral speed. The sensor drift amount estimation apparatus according to any one of claims 15 to 18, which estimates a slip angle. The computer calculates a differential amount of the posture angle with respect to the vertical axis of the vehicle body based on the sensor signal corresponding to the detected value of the movement state amount of the vehicle motion, integrates the calculated differential amount of the posture angle, and calculates the posture angle. Posture angle estimation means for estimating
Based on the sensor signal, a differential amount of the vehicle speed is calculated, the differential amount of the vehicle speed is integrated, the vehicle speed is estimated, and a slip angle is estimated based on the estimated vehicle speed. Slip angle estimating means,
Based on the posture angle estimated by the sensor signal and the posture angle estimating means, a differential amount of the posture angle obtained from a motion equation of vehicle motion is calculated, and is estimated by the sensor signal and the slip angle estimating means. Based on the vehicle speed, a differential amount of the vehicle speed obtained from a motion equation of vehicle motion is calculated, and the sensor signal, the posture angle estimated by the posture angle estimating means, and the slip angle estimating means are used. A calculating means for calculating a differential amount of a rotation angle about a vehicle body vertical axis obtained from a motion equation of vehicle motion based on the estimated vehicle speed;
A plurality of feature points are extracted from each of a plurality of images obtained by imaging the outside of the vehicle body, and based on the extracted plurality of feature points, a motion of the vehicle when the plurality of images are captured is estimated, and the estimation A motion estimation unit that calculates a differential amount of the rotation angle based on the movement of the vehicle, and a differential of the posture angle calculated by the posture angle estimation unit when a sensor drift amount of the sensor signal is taken into consideration A relationship in which the amount and the differential value of the posture angle calculated by the calculating unit are equal to a value considering the sensor drift amount, the differential amount of the vehicle speed calculated by the slip angle estimating unit, and the calculation A relationship between the differential amount of the vehicle speed calculated by the means and a value considering the sensor drift amount, and the differential amount of the rotation angle calculated by the motion estimation means, Drift amount estimation means for estimating the sensor drift amount of the sensor signal using a relationship in which the differential amount of the rotation angle calculated by the calculation means is equal to a value in consideration of the sensor drift amount;
Program to function as. Computer
Based on sensor signals corresponding to the detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of the vehicle motion, the differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, Attitude angle estimation means for integrating the calculated differential amounts of the roll angle and the pitch angle to estimate the roll angle and the pitch angle;
Based on the sensor signal corresponding to each detected value, the respective differential amounts of the longitudinal speed and the lateral speed are calculated, and the differential amounts of the longitudinal speed and the lateral speed are integrated to obtain the longitudinal speed and the lateral speed. Slip angle estimating means for estimating a slip angle based on the estimated longitudinal speed and lateral speed,
Based on the sensor signal and the roll angle and the pitch angle estimated by the posture angle estimating means, the differential amounts of the roll angle and the pitch angle obtained from the motion equation of vehicle motion are calculated, Based on the sensor signal and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, the differential amounts of the longitudinal speed and the lateral speed obtained from a motion equation of vehicle motion are calculated, Based on the sensor signal, the roll angle and the pitch angle estimated by the posture angle estimating means, and the longitudinal speed and the lateral speed estimated by the slip angle estimating means, obtained from a motion equation of vehicle motion. Calculating means for calculating a differential amount of the vehicle body yaw angle around the vehicle body vertical axis;
A plurality of feature points are extracted from each of a plurality of images obtained by imaging the outside of the vehicle body, and based on the extracted plurality of feature points, a motion of the vehicle when the plurality of images are captured is estimated, and the estimation A motion estimation unit that calculates a differential amount of the vehicle body yaw angle based on the motion of the vehicle, and the roll angle calculated by the posture angle estimation unit when the sensor drift amount of the sensor signal is considered; A relationship in which each differential amount of the pitch angle is equal to a value obtained by taking the sensor drift amount into the differential amount of each of the roll angle and the pitch angle calculated by the calculating unit, the slip angle estimating unit The sensor drift amount is added to the calculated differential amounts of the longitudinal velocity and the lateral velocity, and the differential amounts of the longitudinal velocity and the lateral velocity calculated by the calculating means. A relationship in which the considered value is equal, and a differential amount of the vehicle body yaw angle calculated by the motion estimation unit, and a value in which the sensor drift amount is considered in the differential amount of the vehicle body yaw angle calculated by the calculation unit Drift amount estimating means for estimating a sensor drift amount of a sensor signal corresponding to each detected value of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity using a relationship in which
Program to function as.