JP5091047B2 - Vehicle attitude angle estimation device and sensor drift estimation device - Google Patents

Description

  The present invention relates to a vehicle attitude angle estimation device and a sensor drift estimation device, and in particular, a vehicle attitude angle estimation device that estimates a roll angle and a pitch angle, which are vehicle attitude angles with respect to a vertical axis as a vehicle physical quantity, by an observer, and a vehicle The present invention relates 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 exercise.

  Conventionally, in an attitude angle estimation device that estimates the attitude angle of a vehicle using an in-vehicle angular velocity 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 attitude angle. Therefore, it is necessary to compensate by successively estimating the zero point drift.

As a conventional technique for estimating the zero point drift of such a sensor, when the sensor output is within a threshold value for a certain time or more, it is determined that the object is stationary, and the sensor output value at this time is used as the sensor drift amount. A method (for example, Patent Document 1), a method for determining a straight traveling state when the left-right difference in wheel speed is small, and using the lateral acceleration at this time as the lateral acceleration sensor drift amount (for example, Patent Document 2) are known. ing.
Japanese Patent No. 3795498 Japanese Patent Laid-Open No. 7-40043

  However, in the techniques described in Patent Documents 1 and 2 described above, since the stationary or straight traveling state is determined and the sensor output at this time is calculated as the drift amount, the object on which the sensor is mounted is not stationary If the motion is not constant, the sensor drift amount cannot be estimated, and the posture angle cannot be estimated accurately.

  The present invention has been made to solve the above-described problems, and provides an attitude angle estimation device and a sensor drift amount estimation device that can reduce the influence of drift error and accurately estimate the attitude angle. For the purpose.

  In order to achieve the above object, a vehicle attitude angle estimation device according to a first aspect of the present invention includes a longitudinal speed estimation means for estimating a longitudinal vehicle body speed, which is a vehicle body speed in the vehicle longitudinal direction, based on the wheel speed of each wheel; The lateral velocity estimating means for estimating the lateral vehicle body speed, the pitch angular velocity estimating means for estimating the pitch angular velocity, and the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion, The sensor signal is corrected so as to decrease the absolute value of the detected value by a smaller one of the minimum value of the absolute value of the detected value from the start of estimation of a large amount and the posture angle to the present time. Correction means, sensor signals corresponding to detected values of longitudinal acceleration, lateral acceleration, vertical acceleration, and yaw angular velocity of the vehicle motion, and detection of the roll angular velocity corrected by the correction means. Posture angle estimation means for estimating a roll angle and a pitch angle, which are posture angles with respect to a vertical axis of the vehicle body, based on a sensor signal corresponding to the value, the estimated value of the front and rear vehicle body speed, and the estimated value of the pitch angular velocity. A vehicle attitude angle estimation device including: the pitch angular velocity estimation means includes an estimated value of the longitudinal vehicle body speed, an estimated value of the lateral vehicle body speed, a detected value of the vertical acceleration, and a corrected detected value of the roll angular velocity. The pitch angular velocity is estimated based on the previous estimated value of the roll angle estimated by the posture angle estimating means and the previous estimated value of the pitch angle estimated by the posture angle estimating means. is there.

  The vehicle attitude angle estimation device according to the first aspect of the invention is provided with a sensor that detects each of longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity, and roll angular velocity of a vehicle motion and outputs a sensor signal corresponding to the detected value. be able to.

  In the first invention, with respect to the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion, the absolute value of the detected value from the start of estimation of the predetermined sensor drift maximum amount and the attitude angle to the present time. The sensor signal is corrected so as to reduce the absolute value of the detected value by the smaller one of the minimum values. Also, sensor signals corresponding to the detected values of longitudinal acceleration, lateral acceleration, vertical acceleration, and yaw angular velocity of the vehicle motion, sensor signals corresponding to the detected value of the roll angular velocity corrected by the correcting means, estimated values of the longitudinal vehicle body velocity Based on the estimated value of the pitch angular velocity, the roll angle and the pitch angle, which are posture angles with respect to the vertical axis of the vehicle body, are estimated.

  The pitch angular velocity is based on the estimated value of front and rear vehicle speed, estimated value of lateral vehicle speed, detected value of vertical acceleration, corrected detected value of roll angular velocity, previous estimated value of roll angle, and previous estimated value of pitch angle. ,Presumed.

  In the first invention, the pitch angular velocity is estimated from the balance of the vertical movement of the vehicle body, and the roll angle and the pitch angle, which are posture angles with respect to the vertical axis of the vehicle body, are estimated using the estimated value of the pitch angular velocity. It is possible to accurately estimate the roll angle and the pitch angle even during bank running where the vertical acceleration increases. In addition, the sensor signal is corrected so as to reduce the absolute value of the detected value of the roll angular velocity so as to set the initial dead zone, so the influence of drift error is reduced and the roll angle and pitch angle are estimated accurately. can do.

  According to a second aspect of the present invention, there is provided a vehicle posture angle estimating device, a longitudinal vehicle speed estimating means for estimating a longitudinal vehicle body speed, which is a vehicle body velocity in a vehicle longitudinal direction, based on a wheel speed of each wheel, and a lateral vehicle body, which is a vehicle body speed in a vehicle lateral direction. A lateral velocity estimation means for estimating speed, a pitch angular speed estimation means for estimating a pitch angular speed, and a sensor signal corresponding to a detected value of vertical acceleration of the vehicle motion, and a predetermined sensor drift maximum amount and posture angle estimation. The absolute value of the difference between the detected value and the standard value is reduced by the smaller of the minimum absolute value of the difference between the detected value and the standard value from the start of Correction means for correcting the sensor signal, sensor signals corresponding to detected values of longitudinal acceleration, lateral acceleration, yaw angular velocity, and roll angular velocity of vehicle motion, and the vertical acceleration corrected by the correcting means A posture angle estimating means for estimating a roll angle and a pitch angle, which are posture angles with respect to a vertical axis of the vehicle body, based on a sensor signal corresponding to the detected value of the vehicle, an estimated value of the front and rear vehicle body speed, and an estimated value of the pitch angular velocity. The pitch angular velocity estimation means includes: an estimated value of the longitudinal vehicle body speed, an estimated value of the lateral vehicle body speed, a corrected detected value of the vertical acceleration, a roll angular velocity The pitch angular velocity is estimated based on a detected value, a previous estimated value of the roll angle estimated by the attitude angle estimating means, and a previous estimated value of the pitch angle estimated by the attitude angle estimating means. Is.

  The vehicle attitude angle estimation device of the second invention is provided with a sensor that detects each of the longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity, and roll angular velocity of the vehicle motion and outputs a sensor signal corresponding to the detected value. be able to.

  In the second invention, the sensor value corresponding to the detected value of the vertical acceleration of the vehicle motion is detected by the correcting means, and the detected value from the start of the estimation of the predetermined maximum sensor drift amount and the attitude angle to the present time. The sensor signal is corrected so as to reduce the absolute value of the difference between the detected value and the standard value by the smaller of the minimum absolute value of the difference between the detected value and the standard value. Also, sensor signals corresponding to detected values of longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of vehicle motion, sensor signals corresponding to detected values of vertical acceleration corrected by correcting means, and estimated values of longitudinal vehicle body speed Based on the estimated value of the pitch angular velocity, the roll angle and the pitch angle, which are posture angles with respect to the vertical axis of the vehicle body, are estimated.

  The pitch angular velocity is based on the estimated value of the front and rear vehicle speed, the estimated value of the horizontal vehicle speed, the detected value of the roll angular velocity, the corrected detected value of the vertical acceleration, the previous estimated value of the roll angle, and the previous estimated value of the pitch angle. ,Presumed.

  In the second invention, the pitch angular velocity is estimated from the balance of the vertical movement of the vehicle body, and the roll angle and the pitch angle, which are posture angles with respect to the vertical axis of the vehicle body, are estimated using the estimated value of the pitch angular velocity. It is possible to accurately estimate the roll angle and the pitch angle even during bank running where the vertical acceleration increases. In addition, the sensor signal is corrected so as to reduce the absolute value of the difference between the vertical acceleration detection value and the standard value so that the initial dead band is set. The angle and pitch angle can be estimated.

  According to a third aspect of the present invention, there is provided a vehicle attitude angle estimating device comprising: a longitudinal vehicle speed estimating means for estimating a longitudinal vehicle body speed, which is a vehicle body speed in the vehicle longitudinal direction, based on a wheel speed of each wheel; The lateral velocity estimation means for estimating the velocity, and the sensor signal corresponding to the detected value of the pitch angular velocity of the vehicle motion, the predetermined sensor drift maximum amount and the detected value from the start of estimation of the attitude angle to the present time Correction means for correcting the sensor signal so as to reduce the absolute value of the detected value by the smaller one of the absolute values of the absolute value, longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity of the vehicle motion And a sensor signal corresponding to each detected value of the roll angular velocity, a sensor signal corresponding to the detected value of the pitch angular velocity corrected by the correcting means, and an estimated value of the front and rear vehicle body speed. Zui and is configured to include a posture angle estimating means for estimating a roll angle and a pitch angle that are attitude angles with respect to a vertical axis of a vehicle body.

  A vehicle attitude angle estimation device according to a third aspect of the invention is a sensor that detects each of longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity, roll angular velocity, and pitch angular velocity of a vehicle motion and outputs a sensor signal corresponding to the detected value. Can be provided.

  In the third aspect of the invention, the sensor signal corresponding to the detected value of the pitch angular velocity of the vehicle motion is calculated based on the absolute value of the detected value from the start of estimation of the predetermined sensor drift maximum amount and attitude angle to the present time. The sensor signal is corrected so as to reduce the absolute value of the detected value by the smaller one of the minimum values. Further, a sensor signal corresponding to each detected value of the longitudinal acceleration, lateral acceleration, vertical acceleration, roll angular velocity, and yaw angular velocity of the vehicle motion, a sensor signal corresponding to the detected value of the pitch angular velocity corrected by the correcting means, and the front and rear vehicle bodies Based on the estimated value of speed, a roll angle and a pitch angle, which are posture angles with respect to the vertical axis of the vehicle body, are estimated.

  In the third aspect of the invention, the sensor signal is corrected so as to reduce the absolute value of the detected value of the pitch angular velocity so as to set the initial dead zone. Therefore, the influence of the drift error can be reduced, and the roll angle and the accuracy can be reduced. The pitch angle can be estimated.

  The correction means according to the second invention corrects the sensor signal corresponding to the detected value of the vertical acceleration, and also determines a predetermined sensor drift maximum amount and posture angle for the sensor signal corresponding to the detected value of the roll angular velocity. The sensor signal is corrected so as to decrease the absolute value of the detected value by the smaller one of the minimum absolute values of the detected value from the start of estimation to the present time. Estimated value of speed, estimated value of lateral vehicle body speed, corrected detected value of vertical acceleration, corrected detected value of roll angular velocity, previous estimated value of roll angle estimated by attitude angle estimating means, and attitude angle estimating means The pitch angular velocity is estimated based on the previous estimated value of the pitch angle estimated in step (4), and the posture angle estimating means is a sensor signal corresponding to the detected values of the longitudinal acceleration, lateral acceleration, and yaw angular velocity of the vehicle motion. Based on the sensor signal corresponding to the detected value of vertical acceleration corrected by the correcting means, the sensor signal corresponding to the detected value of roll angular velocity corrected by the correcting means, the estimated value of front and rear vehicle body speed, and the estimated value of pitch angular speed The roll angle and the pitch angle, which are attitude angles with respect to the vertical axis of the vehicle body, can be estimated. As a result, the sensor signal is corrected so as to reduce the absolute value of the detected value of the roll angular velocity so as to set the initial dead zone, and the absolute value of the difference from the standard value of the detected value of the vertical acceleration is reduced. Since the sensor signal is corrected, the influence of the drift error can be reduced and the roll angle and pitch angle can be estimated with high accuracy.

  The correcting means according to the third invention corrects the sensor signal corresponding to the detected value of the pitch angular velocity, and determines a predetermined sensor drift maximum amount and posture angle for the sensor signal corresponding to the detected value of the roll angular velocity. The sensor signal is corrected so as to reduce the absolute value of the detected value by the smaller of the minimum absolute values of the detected value from the start of the estimation to the present time. Sensor signal corresponding to detected values of longitudinal acceleration, lateral acceleration, vertical acceleration and yaw angular velocity, sensor signal corresponding to detected pitch angular velocity value corrected by correcting means, and detection of roll angular velocity corrected by correcting means The roll angle and the pitch angle, which are the attitude angles with respect to the vertical axis of the vehicle body, can be estimated based on the sensor signal corresponding to the value and the estimated value of the front and rear vehicle body speed.Thus, the sensor signal is corrected so as to reduce the absolute value of the detected value of the roll angular velocity so as to set the initial dead zone, and the sensor signal is corrected so as to reduce the absolute value of the detected value of the pitch angular velocity. Therefore, the influence of the drift error can be reduced and the roll angle and pitch angle can be estimated with high accuracy.

  The correction means for correcting the sensor signal corresponding to each detected value of the pitch angular velocity and the roll angular velocity corrects the sensor signal corresponding to the detected value of the pitch angular velocity and the sensor signal corresponding to the detected value of the roll angular velocity. For the sensor signal corresponding to the detected value of the vertical acceleration, a predetermined maximum sensor drift amount and the minimum absolute value of the difference between the detected value and the standard value from the start of the estimation of the posture angle The sensor signal is corrected so as to reduce the absolute value of the difference between the detected value and the standard value by the smaller one, and the attitude angle estimation means is used for each of the vehicle motion longitudinal acceleration, lateral acceleration, and yaw angular velocity. A sensor signal corresponding to the detected value, a sensor signal corresponding to the detected vertical acceleration value corrected by the correcting means, and a sensor signal corresponding to the detected pitch angular velocity value corrected by the correcting means. , The sensor signal corresponding to the detected value of the roll angular velocity corrected by the correcting means, and based on the estimated value of the longitudinal vehicle body speed, it is possible to estimate the roll angle and the pitch angle that are attitude angles with respect to a vertical axis of a vehicle body. Thereby, the sensor signal is corrected so as to reduce the absolute value of the detected value of the roll angular velocity so as to set the initial dead zone, and the sensor signal is corrected so as to reduce the absolute value of the detected value of the pitch angular velocity, Since the sensor signal is corrected so as to reduce the absolute value of the difference between the vertical acceleration detection value and the standard value, the influence of drift error can be reduced and the roll angle and pitch angle can be estimated accurately. .

  The correction means according to the third invention corrects the sensor signal corresponding to the detected value of the pitch angular velocity, and determines a predetermined sensor drift maximum amount and attitude angle for the sensor signal corresponding to the detected value of vertical acceleration. Sensor signal to decrease the absolute value of the difference between the detected value and the standard value by the smaller of the minimum absolute value of the difference between the detected value and the standard value from the start of estimation The posture angle estimating means is a sensor signal corresponding to each detected value of longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of the vehicle motion, and a sensor corresponding to the detected value of the pitch angular velocity corrected by the correcting means. Based on the signal, the sensor signal corresponding to the detection value of the vertical acceleration corrected by the correction means, and the estimated value of the front-rear vehicle body speed, the roll angle and the pitch, which are the attitude angles with respect to the vertical axis of the vehicle body. It is possible to estimate the corner. As a result, the sensor signal is corrected so as to reduce the absolute value of the detected value of the pitch angular velocity so as to set the initial dead zone, and the absolute value of the difference from the standard value of the detected value of the vertical acceleration is reduced. Since the sensor signal is corrected, the influence of the drift error can be reduced and the roll angle and pitch angle can be estimated with high accuracy.

  According to a fourth aspect of the present invention, there is provided a sensor drift amount estimation device that starts a presumption of estimation of a predetermined sensor drift maximum amount and an attitude angle for a sensor signal corresponding to a detected value of a roll angular velocity of vehicle motion. The sensor signal is corrected so as to decrease the absolute value of the detected value by the smaller one of the absolute values of the detected values, and the sensor signal corresponding to the detected value of the vertical acceleration is determined in advance. The detected sensor value and the standard value by the smaller of the absolute value of the absolute value of the difference between the detected value and the standard value from the start of estimation of the attitude angle Correction means for correcting the sensor signal so as to reduce the absolute value of the difference between the sensor signal, a sensor signal corresponding to each detected value of the yaw angular velocity, longitudinal acceleration, and lateral acceleration of the vehicle motion, Based on the sensor signal corresponding to each detected value of the roll angular velocity and the vertical acceleration corrected by the positive means, the respective differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, and the calculated roll Posture angle estimating means for estimating the roll angle and the pitch angle by integrating the differential amounts of the angle and the pitch angle, the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw Each of the roll angle and the pitch angle obtained from the motion equation of vehicle motion based on the sensor signal corresponding to each detected value of the angular velocity and the roll angle and the pitch angle estimated by the posture angle estimating means. And a roll calculated by the posture angle estimating means when a sensor drift amount of the sensor signal is taken into account. And the difference between each of the pitch angles and the 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 calculating means, And a drift amount estimating means for estimating a sensor drift amount of each of the sensor signal corresponding to the detected value of the acceleration and the sensor signal corresponding to the detected value of the roll angular velocity.

  According to the sensor drift amount estimating apparatus according to the fourth aspect of the invention, the correction means starts estimating a predetermined maximum sensor drift amount and an attitude angle for the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion. The sensor signal is corrected so as to reduce the absolute value of the detected value by the smaller of the absolute values of the detected value up to the present time and the sensor signal corresponding to the detected value of the vertical acceleration For the predetermined maximum sensor drift amount and the minimum value of the absolute value of the difference between the detected value and the standard value from the start of the estimation of the posture angle to the detected value, whichever is smaller The sensor signal is corrected so as to reduce the absolute value of the difference from the standard value.

  A sensor signal corresponding to each detected value of the yaw angular velocity, longitudinal acceleration and lateral acceleration of the vehicle motion by the attitude angle estimating means, and a sensor corresponding to each detected value of the roll angular velocity and the vertical acceleration corrected by the correcting means. Based on the signal, the differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, and the roll angle and the pitch angle are estimated by integrating the calculated differential amounts of the roll angle and the pitch angle. . Further, based on the sensor signal corresponding to the detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity by the calculating means, and the roll angle and pitch angle estimated by the attitude angle estimating means, The differential amounts of the roll angle and the pitch angle obtained from the motion equation of vehicle motion are calculated.

  Then, when the drift amount estimation means considers the sensor drift amount of the sensor signal, the respective differential amounts of the roll angle and the pitch angle calculated by the posture angle estimation means, and the roll angle and pitch calculated by the calculation means The sensor drift amount of each of the sensor signal corresponding to the detected value of the vertical acceleration and the sensor signal corresponding to the detected value of the roll angular velocity using the relationship in which the differential value of each angle becomes equal to the value considering the sensor drift amount. Is estimated.

  As described above, by estimating the sensor drift amount of the sensor that detects each of the vertical acceleration and the roll angular velocity, the drift of the sensor that stably detects each of the vertical acceleration and the roll angular velocity regardless of the state of the vehicle motion. The sensor signal is corrected so as to reduce the absolute value of the detected value of the roll angular velocity so as to set the initial dead zone, and the difference between the detected value of the vertical acceleration and the standard value can be estimated. Since the sensor signal is corrected so as to reduce the absolute value, the influence of the drift error can be reduced and the roll angle and pitch angle can be estimated with high accuracy.

  According to a fifth aspect of the present invention, there is provided a sensor drift amount estimation device that starts a presumption of estimation of a predetermined sensor drift maximum amount and an attitude angle for a sensor signal corresponding to a detected value of a roll angular velocity of vehicle motion. The sensor signal is corrected so as to decrease the absolute value of the detected value by the smaller one of the absolute values of the detected values, and the sensor signal corresponding to the detected value of the pitch angular velocity is determined in advance. The absolute value of the detected value is decreased by the smaller of the maximum sensor drift amount and the minimum value of the absolute value of the detected value from the start of the estimation of the posture angle to the present time. Correction means for correcting the signal, sensor signals corresponding to the detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, and yaw angular velocity of the vehicle motion, and the correction means The roll angular velocity and the pitch angular velocity are calculated on the basis of sensor values corresponding to the detected values of the roll angular velocity and the pitch angular velocity, and the roll angle and the pitch are calculated. Posture angle estimating means for estimating the roll angle and the pitch angle by integrating the differential amount of each angle, and a sensor signal corresponding to each detected value of the roll angular velocity, the yaw angular velocity, and the pitch angular velocity; Calculation means for calculating a differential amount of each of the roll angle and the pitch angle obtained from a motion equation of vehicle motion based on the roll angle and the pitch angle estimated by the posture angle estimation means; and the sensor The differential amount of each of the roll angle and the pitch angle calculated by the posture angle estimating means when the sensor drift amount of the signal is considered, and the calculation A sensor signal corresponding to the detected value of the pitch angular velocity and the roll angular velocity using a relationship in which a differential value of each of the roll angle and the pitch angle calculated by the stage is equal to a value considering the sensor drift amount. Drift amount estimating means for estimating the sensor drift amount of each sensor signal corresponding to the detected value.

  According to the sensor drift amount estimating apparatus according to the fifth aspect of the invention, the correction means starts estimating a predetermined maximum sensor drift amount and an attitude angle for the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion. The sensor signal is corrected so as to reduce the absolute value of the detected value by the smaller of the absolute values of the detected value up to the present time and the sensor signal corresponding to the detected value of the pitch angular velocity The absolute value of the detected value is reduced by the smaller of the predetermined maximum sensor drift amount and the minimum absolute value of the detected value from the start of the estimation of the posture angle to the present time. Correct the sensor signal.

  Then, by the attitude angle estimation means, the sensor signal corresponding to the detected values of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, and the yaw angular velocity of the vehicle motion, and the detected values of the roll angular velocity and the pitch angular velocity corrected by the correcting means. Based on the corresponding sensor signal, the differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, the differential amounts of the calculated roll angle and the pitch angle are integrated, and the roll angle and the pitch angle are integrated. Is estimated. Further, based on the sensor equation corresponding to each detected value of the roll angular velocity, the yaw angular velocity, and the pitch angular velocity by the calculating means and the roll angle and the pitch angle estimated by the posture angle estimating means, The differential amount of each roll angle and pitch angle obtained is calculated.

  Then, when the drift amount estimation means considers the sensor drift amount of the sensor signal, the respective differential amounts of the roll angle and the pitch angle calculated by the posture angle estimation means, and the roll angle and pitch calculated by the calculation means The sensor drift amount of each of the sensor signal corresponding to the detected value of the pitch angular velocity and the sensor signal corresponding to the detected value of the roll angular velocity using the relationship in which the differential value of each angle becomes equal to the value considering the sensor drift amount. Is estimated.

  Thus, by estimating the sensor drift amount of the sensor that detects each of the pitch angular velocity and the roll angular velocity, the drift of the sensor that stably detects each of the pitch angular velocity and the roll angular velocity regardless of the state of the vehicle motion. The sensor signal can be corrected to reduce the absolute value of the detected value of the roll angular velocity so as to set the initial dead zone, and the absolute value of the detected value of the pitch angular velocity can be reduced. Since the sensor signal is corrected, the influence of the drift error can be reduced and the roll angle and pitch angle can be estimated with high accuracy.

  According to a fourth aspect of the present invention, there is provided a sensor drift amount estimation device, comprising: a sensor signal corresponding to a detected value of vertical acceleration and a sensor signal corresponding to a detected value of roll angular velocity based on a sensor drift amount estimated by a drift amount estimating means. The posture angle estimation unit further includes a drift correction unit that corrects each of the sensor signal corresponding to the detected value of the yaw angular velocity, the roll angular velocity and the vertical acceleration corrected by the drift correction unit and corrected by the correction unit. Based on the sensor signal corresponding to each detection value, the roll angle and the pitch angle can be estimated.

  The sensor drift amount estimation device according to the fifth invention is based on the sensor drift amount estimated by the drift amount estimation means, the sensor signal corresponding to the detected value of the pitch angular velocity and the sensor signal corresponding to the detected value of the roll angular velocity. It further includes a drift correction unit that corrects each, and the attitude angle estimation unit is corrected by the sensor signal corresponding to the detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, and yaw angular velocity, and the drift correction unit. The roll angle and pitch angle can be estimated based on sensor signals corresponding to the detected values of the roll angular velocity and the pitch angular velocity corrected by the means.

  The present invention can be configured by a program that causes a computer to function as follows.

  The first program uses a computer to estimate a longitudinal vehicle speed that is a longitudinal vehicle speed in the vehicle longitudinal direction based on a wheel speed of each wheel, and a lateral vehicle speed that is a vehicle speed in the vehicle lateral direction. Lateral speed estimation means, pitch angular speed estimation means for estimating pitch angular speed, sensor signal corresponding to the detected value of roll angular speed of vehicle motion, and pre-determined maximum sensor drift and posture angle Correction means for correcting the sensor signal so as to reduce the absolute value of the detected value by the smaller one of the absolute values of the detected value up to and including the longitudinal acceleration, lateral acceleration of the vehicle motion, Sensor signal corresponding to each detected value of vertical acceleration and yaw angular velocity, sensor signal corresponding to the detected value of roll angular velocity corrected by the correcting means, A program for functioning as a posture angle estimating means for estimating a roll angle and a pitch angle, which are posture angles with respect to a vertical axis of a vehicle body, based on an estimated value of front and rear vehicle body speeds and an estimated value of the pitch angular velocity, The pitch angular velocity estimation means is an estimated value of the longitudinal vehicle body speed, an estimated value of the lateral vehicle body speed, a detected value of the vertical acceleration, a corrected detected value of the roll angular velocity, and the posture angle estimating means This is a program for estimating the pitch angular velocity based on the previous estimated value of the roll angle and the previous estimated value of the pitch angle estimated by the posture angle estimating means.

  The second program uses a computer to estimate a longitudinal vehicle speed that is a longitudinal vehicle speed in the vehicle longitudinal direction based on a wheel speed of each wheel, and a lateral vehicle speed that is a vehicle speed in the vehicle lateral direction. Lateral speed estimation means, pitch angular speed estimation means for estimating pitch angular speed, sensor signals corresponding to detected values of vertical acceleration of vehicle motion, and estimation of predetermined sensor drift maximum amount and posture angle The sensor signal is corrected so that the absolute value of the difference between the detected value and the standard value is reduced by the smaller of the minimum absolute value of the difference between the detected value and the standard value. Correction means, sensor signals corresponding to detected values of longitudinal acceleration, lateral acceleration, yaw angular velocity, and roll angular velocity of vehicle motion, and the vertical acceleration corrected by the correcting means Functions as posture angle estimation means for estimating the roll angle and pitch angle, which are posture angles with respect to the vertical axis of the vehicle body, based on the sensor signal corresponding to the detected value, the estimated value of the front and rear vehicle body speed, and the estimated value of the pitch angular velocity The pitch angular velocity estimation means includes: an estimated value of the longitudinal vehicle body speed, an estimated value of the lateral vehicle body speed, the corrected detected value of the vertical acceleration, the detected value of the roll angular velocity, The program estimates the pitch angular velocity based on the previous estimated value of the roll angle estimated by the attitude angle estimating means and the previous estimated value of the pitch angle estimated by the attitude angle estimating means.

  The third program uses a computer to estimate a longitudinal vehicle body speed that is a longitudinal vehicle body speed that is a longitudinal vehicle body speed that estimates a longitudinal vehicle body speed that is a longitudinal vehicle body speed based on a wheel speed of each wheel. For the sensor signal corresponding to the detected value of the lateral velocity estimating means and the pitch angular velocity of the vehicle motion, the predetermined maximum sensor drift amount and the minimum of the absolute value of the detected value from the start of the estimation of the posture angle to the present time Correction means for correcting the sensor signal so as to reduce the absolute value of the detected value by the smaller one of the values, and longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity, and roll angular velocity of the vehicle motion A sensor signal corresponding to each detected value, a sensor signal corresponding to the detected value of the pitch angular velocity corrected by the correcting means, and an estimated value of the front and rear vehicle body speed Zui and a program for functioning as a posture angle estimating means for estimating a roll angle and a pitch angle that are attitude angles with respect to a vertical axis of a vehicle body.

  The fourth program causes the computer to detect the detection value for the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion and the detected value from the start of estimation of the predetermined sensor drift maximum amount and attitude angle to the present time. The sensor signal is corrected so as to decrease the absolute value of the detected value by the smaller one of the minimum absolute values, and a predetermined sensor is used for the sensor signal corresponding to the detected value of vertical acceleration. The difference between the detected value and the standard value is the smaller of the absolute value of the absolute value of the difference between the detected value and the standard value from the start of estimation of the maximum drift amount and attitude angle. Correction means for correcting the sensor signal so as to reduce the absolute value of the difference, a sensor signal corresponding to each detected value of yaw angular velocity, longitudinal acceleration, and lateral acceleration of the vehicle motion, and the correcting hand Based on the sensor signal corresponding to each detected value of the roll angular velocity and the vertical acceleration corrected by the above, the differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, and the calculated roll angle and Attitude angle estimation means for estimating the roll angle and the pitch angle by integrating the differential amounts of the pitch angles, the vertical acceleration, the longitudinal acceleration, the lateral acceleration, the roll angular velocity, and the yaw angular velocity. Based on the sensor signal corresponding to the detected value 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 equation of motion of the vehicle motion. And the roll angle calculated by the posture angle estimating means when considering the sensor drift amount of the sensor signal The vertical acceleration is calculated using 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. It is a program for functioning as drift amount estimation means for estimating the sensor drift amount of each of the sensor signal corresponding to the detected value of the sensor and the sensor signal corresponding to the detected value of the roll angular velocity.

  The fifth program causes the computer to detect the detection value for the sensor signal corresponding to the detection value of the roll angular velocity of the vehicle motion and the detection value from the start of estimation of the predetermined sensor drift maximum amount and attitude angle to the present time. The sensor signal is corrected so as to decrease the absolute value of the detected value by the smaller one of the minimum absolute values, and a predetermined sensor is used for the sensor signal corresponding to the detected value of the pitch angular velocity. The sensor signal is set so that the absolute value of the detected value is reduced by the smaller one of the minimum value of the absolute value of the detected value from the start of estimation of the maximum drift amount and posture angle to the present time. Correction means for correcting, sensor signals corresponding to detected values of vertical acceleration, longitudinal acceleration, lateral acceleration, and yaw angular velocity of vehicle motion, and correction by the correction means On the basis of the detected roll angular velocity and the sensor signal corresponding to the detected value of the pitch angular velocity, the respective differential amounts of the roll angle and the pitch angle with respect to the vertical axis of the vehicle body are calculated, and the calculated roll angle and the pitch Attitude angle estimating means for estimating the roll angle and the pitch angle by integrating each differential amount of the angle, a sensor signal corresponding to each detected value of the roll angular velocity, the yaw angular velocity, and the pitch angular velocity, and Calculation means for calculating a differential amount of each of the roll angle and the pitch angle obtained from a motion equation of vehicle motion based on the roll angle and the pitch angle estimated by the posture angle estimation means, and the sensor signal When the sensor drift amount is taken into consideration, the respective differential amounts of the roll angle and the pitch angle calculated by the posture angle estimation means, and the calculation hand The sensor signal corresponding to the detected value of the pitch angular velocity and the roll angular velocity of the roll angular velocity using the relationship that the differential value of each of the roll angle and the pitch angle calculated by It is a program for functioning as drift amount estimation means for estimating the sensor drift amount of each sensor signal corresponding to the detected value.

  As described above, according to the present invention, the roll angular velocity, vertical acceleration, or pitch angular velocity sensor signal is corrected so as to set the initial dead zone. The effect that the angle and the pitch angle can be estimated is obtained.

  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 an attitude angle estimation device that estimates a pitch angle and a roll angle, which are attitude angles of a vehicle with respect to a vertical axis.

  As shown in FIG. 1, the attitude angle estimation device according to the first embodiment is based on lateral speed estimation means 10 that estimates a lateral vehicle body speed V that is a vehicle body speed in the lateral direction of the vehicle, and the wheel speed of each wheel. Thus, a longitudinal speed estimating means 12 for estimating a longitudinal vehicle body speed U which is a vehicle body speed in the longitudinal direction of the vehicle is provided.

  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.

  The wheel speed of each wheel can be detected by a wheel speed sensor provided corresponding to each wheel, and the front and rear vehicle body speed U is determined from the wheel speed of each wheel or the wheel speed of each wheel and the differential value of the wheel speed. Can be estimated. For example, the maximum value of the wheel speeds of four wheels can be output as the front and rear vehicle body speed U during braking, and the average value of the wheel speeds of the driven wheels can be output as the front and rear vehicle body speed U during driving.

  In addition, the posture angle estimation device of the present embodiment includes a vertical acceleration sensor 14, a lateral acceleration sensor 16 that detects vertical acceleration Gz, lateral acceleration Gy, and longitudinal acceleration Gx, which are xyz 3-axis accelerations of vehicle motion, and A longitudinal acceleration sensor 18 is provided. As shown in FIG. 2, the x axis corresponds to the vehicle longitudinal direction, the y axis corresponds to the vehicle width direction (lateral direction), and the z axis corresponds to the vehicle vertical direction.

  In addition, the posture angle estimation device of the present embodiment is provided with a roll angular velocity sensor 20 that detects a roll angular velocity P and a yaw angular velocity sensor 22 that detects a yaw angular velocity R.

  The vertical acceleration sensor 14 and the roll angular velocity sensor 20 are connected to an initial dead zone correction unit 23 that corrects each sensor signal corresponding to each detection value from the vertical acceleration sensor 14 and the roll angular velocity sensor 20 so as to provide an initial dead zone. Yes.

  The lateral speed estimation means 10, the longitudinal speed estimation means 12, and the initial dead zone correction means 23 are connected to a pitch angular speed estimation means 24 that estimates the pitch angular speed Q.

  The pitch angular velocity estimation means 24 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 observer 26 is further connected with a longitudinal speed estimation means 12, an initial dead zone correction means 23, a lateral acceleration sensor 16, a longitudinal acceleration sensor 18, and a yaw angular speed sensor 22.

  The posture angle observer 26 includes a sensor signal corresponding to each detected value of the longitudinal acceleration Gx, lateral acceleration Gy, and yaw angular velocity R of the vehicle motion, a sensor signal corresponding to the detected value of the corrected vertical acceleration Gz, and a corrected roll. Based on the sensor signal corresponding to the detected value of the angular velocity P, the estimated value Vso of the longitudinal vehicle body speed U, and the estimated value of the pitch angular velocity 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 pitch angular velocity estimating means 24 is connected so that the estimated values of the roll angle φ and the pitch angle θ are input to the pitch angular velocity estimating means 24 as previous values.

  The lateral speed estimation means 10, the longitudinal speed estimation means 12, the initial dead zone correction means 23, the pitch angular speed estimation means 24, and the attitude angle observer 26 are one or more computers that realize the functions of the respective means, or one or more. The electronic circuit can be configured.

  Next, the principle of correcting the sensor signal so as to provide an initial dead zone will be described.

  Due to the nature of automobiles in which the attitude angle does not continue to increase, the roll angular velocity and the pitch angular velocity, which are the basis for estimating the attitude angle, have the characteristic that they always return to the zero point in a short time. This means that the roll angular velocity and the pitch angular velocity are “at least the drift corresponding to the minimum value of the sensor signal is applied to the sensor signal unless returning to the zero point”. In the present embodiment, paying attention to such characteristics of the sensor signal, an initial dead zone corresponding to the sensor signal value history up to now is set for the input signal for posture angle estimation, and the sensor signal is corrected. This reduces the influence of the estimated posture angle drift error.

  The initial dead zone correction means 23 starts the estimation of the sensor drift maximum amount and the posture angle determined in advance for the roll angular velocity sensor 20 with respect to the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion, and the history up to the present time. The sensor signal from the roll angular velocity sensor 20 is set so that the smaller of the absolute value of the absolute value of the sensor signal at the lower limit is set as the width of the initial dead zone, and the absolute value of the detected value is reduced by the width of the initial dead zone. Correct.

  The initial dead zone correction means 23 sets the sensor drift maximum amount (the maximum value of the absolute value of the drift error) of the sensor signal obtained in advance as the initial value of the initial dead zone width. For example, when the sign of the sensor signal is positive, an error upper limit value is set, and when the sign of the sensor signal is negative, an error lower limit value is set. Further, the initial dead zone correcting means 23 sequentially corrects the width of the initial dead zone so as not to exceed the minimum value of the absolute value of the sensor signal in the history of sensor signals from the start of posture angle estimation to the present. As a result, as shown in FIG. 3, when the absolute value of the sensor signal value is larger than the sensor drift maximum amount, the sensor drift maximum amount is set as the width of the initial dead zone. When the absolute value of the sensor signal value is smaller than the maximum sensor drift amount, the absolute value of the sensor signal value is set as the width of the initial dead zone.

  As shown in FIG. 4, the area of the initial dead zone that is set corresponds to a correction value of the estimated posture angle value. For example, the sensor signal from the roll angular velocity sensor becomes a true drift error. When the upper limit value is applied, the detected value of the roll angular velocity is corrected most greatly in the negative direction. Even when the sensor signal is a true value and there is no drift error that does not need to be corrected, the detection value of the roll angular velocity is corrected by setting the initial dead band, and a value smaller than the true value is Although it is output as a detected value of angular velocity, the correction width is smaller than that of a signal to which an upper limit value of drift error is applied.

  Also, when the sign of the sensor signal and the sign of the applied drift are different and the sensor signal of the roll angular velocity sensor is a signal in which the lower limit value of the drift error (the error with the opposite sign to the upper limit value) is applied to the true value. In this case, the detected value of the roll angular velocity that should be corrected to the increase side is corrected to the decrease side, but the range of decrease by the correction is relatively small.

  In the situation where the lower limit of drift error is applied to the sensor signal as described above, when a large roll angular velocity is output, the sign of the sensor signal and the sign of the applied drift error are different. As shown in FIG. 5, the sign of the sensor signal of the roll angular velocity is always reversed at a relatively early timing except during rollover. The inversion of the sign means that the correction by the initial dead band compensation is completed, and the influence on the posture angle estimation that contributes as the time integration is small.

  On the other hand, under traveling conditions in which the true value of the roll angular velocity is almost zero, the ratio of the drift error to the signal is large, and there are situations where the sensor signal does not become zero due to drift error for a long time. In such a case, as shown in FIG. 6, since the sign of the roll angular velocity signal matches the sign of the drift error, the signal is appropriately corrected for a long time by the set initial dead band.

  Further, the initial dead zone correction means 23 starts the estimation of the sensor drift maximum amount and the posture angle predetermined for the vertical acceleration sensor 14 for the sensor signal corresponding to the detected value of the vertical acceleration of the vehicle motion until the current time. The minimum value of the absolute value of the difference from the standard value (9.8 m / s2) of the sensor signal in the history is set as the width of the initial dead band, and the standard value is set by the width of the initial dead band. The sensor signal from the vertical acceleration sensor 14 is corrected so that the absolute value of the difference between them is reduced.

  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 velocity, V: lateral velocity, W: vertical velocity, φ: roll Angle, θ: Pitch angle, g: Gravitational acceleration.

  When the vehicle is a rigid body in the present embodiment, the longitudinal acceleration Gx, the lateral acceleration Gy, and the yaw angular velocity R are detected by the longitudinal acceleration sensor 18, the lateral acceleration sensor 16, and the yaw angular velocity sensor 22, respectively. The vertical acceleration Gz and the roll angular velocity P are correction signals obtained by correcting the signals detected by the vertical acceleration sensor 14 and the roll angular velocity sensor 20 by the initial dead zone correction means 23. The lateral speed V can be estimated by the lateral speed estimating means 10 as described above, and the longitudinal speed U is estimated by the longitudinal speed estimating means 12 as described above based on the wheel speed of each wheel. Can do.

  In the present embodiment, as shown in FIG. 2, the coordinates are described in a right-hand system with the upward direction of the vehicle body as the positive direction of the z axis, and the angles are expressed by Euler angles.

  While the bank is running, the vertical acceleration Gz increases due to centrifugal force. The increase in the vertical acceleration Gz corresponds to an increase in -QU on the left side in the above equation (3). By utilizing this property, the change in the vertical velocity W is ignored in the equation (3) (W = 0) In addition, the estimated value of the pitch angular velocity is obtained by using the following values of the roll angle and the pitch angle, the estimated value of the lateral velocity, and the estimated value Vso of the longitudinal vehicle body speed as the longitudinal velocity U: Can be obtained from The pitch angular velocity estimating means 24 obtains an estimated value of the pitch angular velocity by executing the calculation of the following equation (6).

In the above equation (6), P represents a correction signal obtained by correcting the sensor signal of the roll angular velocity sensor 20 by the initial dead zone correction means 23, and G _z represents the sensor signal G _z of the vertical acceleration sensor 14 as an initial value. The correction signal corrected by the dead zone correction means 23 is shown.

  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, characteristics unique to automobile motion are used as physical quantities to be fed back as follows. In the following formulas, the previous estimated value is used as the roll angle and the pitch angle, the estimated value of the lateral vehicle body speed is used as the lateral speed, and the estimated value Vso of the longitudinal vehicle body speed is used as the longitudinal speed U.

  Differentiating the estimated value Vso of the front and rear vehicle body speed and substituting it into the above equation (1), and utilizing the property that “the wheel slip does not continue to increase for a long time” with respect to the front and rear direction of the vehicle body as a characteristic characteristic of the vehicle motion. From the above equation (1), the conditions shown in the following equation (7) used for feedback ignoring the change in vertical speed are obtained.

  The above equation (7) describes the relationship between the pitch angle θ and the difference between the acceleration estimated from the wheel speed (the differential value of the estimated value Vso of the longitudinal vehicle body speed) and the longitudinal acceleration Gx.

  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”. From the equation (2) used for feedback, the following (8) The condition shown in the equation is obtained.

  Furthermore, in consideration of a general road gradient, it can be regarded that “the acceleration in the vertical direction substantially matches the acceleration of gravity”. This condition can be described by the algebraic equation shown in the following equation (9) using the longitudinal acceleration Gx, the lateral acceleration Gy, the vertical acceleration Gz, the pitch angle θ, and the roll angle φ.

  Since the relations of the above equations (7) to (9) are all satisfied when a certain amount of time is taken into consideration, the feedback of the observer measurement amount includes the above (7) to (7) as described below. A value obtained by low-pass filter processing on both sides of the equation (9) is used.

  Next, the configuration of a basic nonlinear observer will be described. Here, the sensor signal output from the sensor, the correction signal corrected to set the initial dead zone, and the value u including the estimated value of the pitch angular velocity are expressed as the following equation (10).

  Further, if the target vehicle motion is expressed as the following equation (11) and the physical quantity that can be measured to constitute the observer is expressed as the equation (12), the nonlinear observer is expressed by the following equation (13) and It can be described by the nonlinear equation of motion of equation (14).

  However, the tilde of x and the tilde of y represent the estimated values of x and y, respectively, and k (tilde of x, u) represents the observer gain to be designed.

  Since the above equations (7) to (9) are all satisfied when a certain amount of time is taken into consideration, the following equation (15) to (17) are used for feedback of the measured amount of the observer ( Values obtained by performing low-pass filter processing on both sides of equations 7) to (9) with a low-pass filter are used.

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

  Next, the attitude angle observer 26 for the roll angle φ and the pitch angle θ according to the present embodiment that estimates the roll angle φ and the pitch angle θ using the above-described basic nonlinear observer will be described. Since the state equation regarding the angle in the above equations (4) and (5) does not include the speed state quantity, the roll angle φ and the pitch angle θ are estimated as angles separately from the speed estimation. Can do.

  For this reason, first, an observer for estimating the roll angle and the pitch angle is configured including the state quantity generated by the low-pass filter. In the present embodiment, the state quantity of the observer is expressed by the following equation (18):

フィードバックに用いるオブザーバ出力を下記（１９）式で表し、

The observer output used for feedback is expressed by the following equation (19):

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

ただし、

However,

である。

It is.

The numerator of the second term on the right side of the equation (21) is the product of the yaw angular velocity value R detected by the three-axis sensor and the estimated value of the lateral vehicle body velocity from the differential value of the estimated vehicle body velocity value _Vs0 . This is the deviation of the longitudinal acceleration state quantity obtained by subtracting the sum of the longitudinal acceleration value Gx detected by the triaxial sensor, and the numerator in the second term on the right side of the equation (22) is the yaw angular velocity value R detected by the triaxial sensor. This is the deviation of the lateral acceleration state quantity obtained by subtracting the lateral acceleration value Gy detected by the three-axis sensor from the product of the estimated value _Vs0 of the longitudinal vehicle body speed.

  As an example of the observer gain, in the present embodiment, in order to ensure the stability of the observer, the following equation (23) is used so that the diagonal component when linearization is performed has a negative coefficient.

However, a _{K φy, K φg, K θx} , K θg, K x, K y, K g is proper positive constant. Therefore, the nonlinear observer of this embodiment for estimating the roll angle and the pitch angle is described by the equation of motion shown in the following equation (24).

ただし、ｘのチルダは、以下の（２５）式で表される。

However, the tilde of x is represented by the following formula (25).

  The posture angle observer 26 calculates the tilde of the tilde differential amount dφ of the roll angle φ and the tilde of the tilde differential amount dθ of the pitch angle θ by using the above equation (24). And calculating the tilde of the roll angle φ and the tilde of the pitch angle θ by integrating each of the calculated tilde of the tilde of the roll angle φ and the tilde of the tilde differential amount dθ of the pitch angle θ. Can do.

  Information processing by a program that causes the computer to function as each means of the lateral speed estimating means 10, the longitudinal speed estimating means 12, the initial dead zone correcting means 23, the pitch angular speed estimating means 24, and the attitude angle observer 26 is shown in the flowchart of FIG. This can be realized by a posture angle estimation processing routine. The computer is composed of a CPU, a ROM, and a RAM connected to each other by a bus, and an HDD connected as necessary. These programs are 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 vertical acceleration sensor 14, the lateral acceleration sensor 16, the longitudinal acceleration sensor 18, the roll angular velocity sensor 20, and the yaw angular velocity sensor 22. get. At this time, the detection value of the sensor signal from the vertical acceleration sensor 14 is recorded as a history of the detection value of the sensor signal from the vertical acceleration sensor 14, and the detection value of the sensor signal from the roll angular velocity sensor 20 is recorded as the roll angular velocity sensor. Recorded as a history of detection values of sensor signals from 20.

  Then, in step 102, the sensor signal corresponding to the detection value of the vertical acceleration sensor 14 acquired in step 100 is detected from the history of detection values of the vertical acceleration sensor 14 obtained after starting the posture angle estimation. It is determined whether or not the sensor signal has the smallest absolute value of the difference between the value and the standard value. If the absolute value of the difference between the detected value and the standard value is not the minimum for the sensor signal corresponding to the detected value of the vertical acceleration sensor 14 acquired in step 100, the process proceeds to step 106. On the other hand, if the absolute value of the difference between the detected value and the standard value is minimum for the sensor signal corresponding to the detected value of the vertical acceleration sensor 14 acquired in step 100 in step 102, The detection value of the vertical acceleration sensor 14 acquired in step 100 is stored as the detection value of the sensor signal that minimizes the absolute value of the difference from the standard value.

  In the next step 108, detection is performed from the history of detection values of the roll angular velocity sensor 20 obtained after starting the attitude angle estimation for the sensor signal corresponding to the detection value of the roll angular velocity sensor 20 acquired in step 100. It is determined whether or not the sensor signal has a minimum absolute value. If the absolute value of the detected value of the sensor signal corresponding to the detected value of the roll angular velocity sensor 20 acquired in step 100 is not the minimum, the process proceeds to step 110. On the other hand, if the absolute value of the detected value is minimum for the sensor signal corresponding to the detected value of the roll angular velocity sensor 20 acquired in Step 100 in Step 106, the absolute value of the detected value is determined in Step 108. The detection value of the roll angular velocity sensor 20 acquired in step 100 is stored as the detection value of the sensor signal that is minimized.

  In step 110, estimation of a predetermined sensor drift maximum amount and posture angle of the vertical acceleration sensor 14 for the sensor signal corresponding to the detection value of the vertical acceleration sensor 14 acquired in step 100 is started. The sensor signal is corrected so as to reduce the absolute value of the difference between the detection value and the standard value by the smaller of the minimum absolute value of the difference from the standard value in the history up to the present time. Further, with respect to the sensor signal corresponding to the detection value of the roll angular velocity sensor 20 acquired in step 100, a history from the start of estimation of the predetermined sensor drift maximum amount of the roll angular velocity sensor 20 and the posture angle to the present time. The sensor signal is corrected so as to decrease the absolute value of the detected value by the smaller one of the minimum absolute values of the detected values.

  In the next step 112, the lateral vehicle body speed is estimated as described above, and in step 114, the front and rear vehicle body speed is estimated based on the wheel speed of each wheel.

  In step 116, it is determined whether or not this is the first calculation. In the case of the first calculation, since the previous values of the roll angle and the pitch angle do not exist, the previous values of the roll angle and the pitch angle are determined in advance in step 118. In step 122, the estimated value of the pitch angular velocity Q is calculated using the equation (6) as described above.

  On the other hand, in the second and subsequent calculations, the estimated values of the roll angle and pitch angle are calculated in step 126, which will be described later. In step 122, the estimated value of the pitch angular velocity Q is calculated using the above equation (6) as described above.

  In step 124, as described above, based on the sensor signal corresponding to the detected values of the longitudinal acceleration Gx, the lateral acceleration Gy, and the yaw angular velocity R of the vehicle motion, and the estimated value Vso of the longitudinal vehicle body speed U (21 ) And low-pass filter processing described by equation (22).

  In the next step 126, using the estimated value of the pitch angular velocity Q calculated in step 122 and the low-pass filter processing value calculated in step 124, the roll angle which is the attitude angle with respect to the vertical axis of the vehicle body according to the above equation (24). φ and pitch angle θ are estimated, the estimated values of roll angle φ and pitch angle θ are stored in the memory of the computer, and the process returns to step 100.

  In the above embodiment, the case where correction is performed so as to set the initial dead band for each sensor signal from the roll angular velocity sensor and the vertical acceleration sensor has been described as an example. However, the present invention is not limited to this. The initial dead band may be corrected only for one of the sensor signals of the roll angular velocity sensor and the vertical acceleration sensor.

  Next, a second embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  The second embodiment is different from the first embodiment in that a posture angle is estimated using a pitch angular velocity sensor in addition to various sensors.

  As shown in FIG. 8, the posture angle estimation apparatus according to the second embodiment includes a vertical acceleration sensor 14, a lateral acceleration sensor 16, a longitudinal acceleration sensor 18, a roll angular velocity sensor 20, a yaw angular velocity sensor 22, and a pitch angular velocity. A pitch angular velocity sensor 220 for detection is provided.

  The vertical acceleration sensor 14, the roll angular velocity sensor 20, and the pitch angular velocity sensor 220 are sensor signals corresponding to the detection values from the vertical acceleration sensor 14, the roll angular velocity sensor 20, and the pitch angular velocity sensor 220 so as to provide an initial dead zone. It is connected to the initial dead zone correction means 223 for correcting.

  The attitude angle observer 26 is connected to the lateral velocity estimating means 10, the longitudinal velocity estimating means 12, the initial dead zone correcting means 223, the lateral acceleration sensor 16, the longitudinal acceleration sensor 18, and the yaw angular velocity sensor 22.

  The initial dead zone correction means 223 sets an initial dead zone for the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion, as in the first embodiment, and decreases the absolute value by the width of the initial dead zone. Thus, the sensor signal from the roll angular velocity sensor 20 is corrected. Further, the initial dead zone correction means 223 sets an initial dead zone for the sensor signal corresponding to the detected value of the vertical acceleration of the vehicle motion, as in the first embodiment, and sets the standard value by the width of the initial dead zone. The sensor signal from the vertical acceleration sensor 14 is corrected so as to reduce the absolute value of the difference between the vertical acceleration sensor 14 and the vertical acceleration sensor 14.

  The initial dead zone correction means 223 starts the estimation of the sensor drift maximum amount and the posture angle determined in advance for the pitch angular velocity sensor 220 for the sensor signal corresponding to the detected value of the pitch angular velocity of the vehicle motion until the current time. The sensor signal from the pitch angular velocity sensor 220 is corrected so that the absolute value is reduced by the width of the initial dead zone, whichever is smaller of the minimum absolute value of the sensor signal at To do.

The posture angle observer 26 is detected by the longitudinal acceleration sensor 18, the lateral acceleration sensor 16, and the yaw angular velocity sensor 22 by replacing the equation (24) described in the first embodiment with the following equation (26). Longitudinal acceleration Gx, lateral acceleration Gy, yaw angular velocity R, vertical acceleration Gz corrected by the initial dead zone correcting means 23, roll angular velocity P, pitch angular velocity Q, and lateral velocity V estimated by the lateral velocity estimating means 10. Using the tilde and the longitudinal speed V _s0 estimated by the longitudinal speed estimating means 12, a tilde of the roll angle differential amount dφ and a tilde of the pitch angle differential amount dθ, which are attitude angles with respect to the vehicle body vertical direction, are calculated. . Further, the attitude angle observer 26 calculates the tilde of the roll angle φ and the tilde of the pitch angle θ by integrating the tilde of the calculated roll angle derivative dφ and the tilde of the pitch angle derivative dθ.

  In the above-described embodiment, the case where correction is performed so as to set the initial dead band for each sensor signal from the roll angular velocity sensor, the pitch angular velocity sensor, and the vertical acceleration sensor has been described as an example. However, the initial dead zone may be corrected only for the sensor signal from the pitch angular velocity sensor. Moreover, you may correct | amend so that an initial dead zone may be set with respect to each sensor signal from the combination of arbitrary two sensors among a roll angular velocity sensor, a pitch angular velocity sensor, and a vertical acceleration sensor. For example, each sensor signal from the roll angular velocity sensor and the pitch angular velocity sensor may be corrected so as to set an initial dead band, and each sensor signal from the pitch angular velocity sensor and the vertical acceleration sensor may be corrected to an initial value. You may correct | amend so that a dead zone may be set.

Next, a third embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted. In the third embodiment,
The third embodiment is mainly different from the first embodiment in that the correction is made so as to set the initial dead zone after correcting the sensor drift amount.

  As shown in FIG. 9, a posture angle estimation apparatus 310 according to the third embodiment includes a vertical acceleration sensor 14, a lateral acceleration sensor 16, a longitudinal acceleration sensor 18, a roll angular velocity sensor 20, and a yaw angular velocity sensor 22. ing.

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

  The drift amount correcting means 322 is connected to the initial dead zone correcting means 23. The initial dead zone correction means 23 is connected to a posture angle observer 324 that estimates a roll angle φ and a pitch angle θ, which are posture angles with respect to the vertical axis of the vehicle body.

  The vertical acceleration sensor 14, the roll angular velocity sensor 20, the yaw angular velocity sensor 22, and the attitude angle observer 324 are motion equation differential amount calculation means 326 that calculates the differential amounts of the roll angle and the pitch angle obtained from the motion equation of vehicle motion. It is connected to the.

  The attitude angle observer 324 and the equation of motion differential amount calculation means 326 are connected to a drift amount estimation means 328 that estimates the sensor drift amounts of the vertical acceleration sensor 14 and the roll angular velocity sensor 20.

  The drift amount correcting means 322, the initial dead zone correcting means 23, the attitude angle observer 324, the equation of motion differential amount calculating means 326, and the drift amount estimating means 328 are one or a plurality of computers that realize the function of each means, or one Alternatively, it can be composed of a plurality of electronic circuits.

The attitude angle observer 324 estimates the lateral vehicle body speed V, which is the vehicle speed in the lateral direction of the vehicle, and estimates the longitudinal vehicle body speed U, which is the vehicle speed in the vehicle longitudinal direction, based on the wheel speed of each wheel. Furthermore, the attitude angle observer 324 estimates the lateral vehicle body velocity V, the estimated value, the drift amount correcting means 322 corrected detected value of the vertical acceleration G _z by the initial dead zone correcting unit 23 after being corrected by the longitudinal vehicle body velocity U The pitch angular velocity Q is estimated on the basis of the correction signal corresponding to the above and the correction signal corresponding to the detected value of the roll angular velocity P corrected by the initial dead zone correcting means 23 after being corrected by the drift amount correcting means 322. 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.

The posture angle observer 324 includes a correction signal corresponding to each detected value of the vertical acceleration _Gz and the roll angular velocity P of the vehicle motion corrected by the initial dead zone correcting unit 23 after being corrected by the drift amount correcting unit 322, and the vehicle motion. Based on sensor signals corresponding to detected values of longitudinal acceleration G _x , lateral acceleration G _y , and yaw angular velocity R, an estimated value V _so of longitudinal vehicle body speed U, and an estimated value of pitch angular velocity Q, A roll angle φ and a pitch angle θ, which are posture angles with respect to the vertical axis, are estimated.

  The attitude angle observer 324 obtains a tilde of the estimated pitch angular velocity Q by replacing the equation (6) described in the first embodiment with the following equation (27).

In the above equation (27), P-P _dr represents a correction signal corrected so as to set the initial dead zone after correcting the drift amount P _dr of the sensor signal P of the roll angular velocity sensor 20, and G _z -G _zdr represents a correction signal corrected so as to set an initial dead band after correcting the drift amount G _zdr of the sensor signal G _z of the vertical acceleration sensor 14.

  When the pitch angular velocity is estimated as described above, the pitch angle can also be estimated by the attitude angle observer 324 simultaneously with the roll angle. In the present embodiment, the conditions shown in the above equation (9) in consideration of a general road gradient as the constraint condition of the motion inherent to the vehicle when the attitude angle observer 324 is used are expressed by the following equation (28): Replace with the conditions shown.

  Next, the configuration of a basic nonlinear observer will be described. Here, the above equation (10) is replaced with the following equation (29), the correction signal corrected to set the initial dead zone after correcting the drift amount, the sensor signal, and the estimated value of the pitch angular velocity Represents a value u including

  The above formulas (15) to (17), which indicate the low-pass filter processing used for feedback of the measured amount of the observer, are replaced with the following formulas (30) to (32).

  Next, the attitude angle observer 324 of the present embodiment that estimates the roll angle φ and the pitch angle θ using the basic nonlinear observer will be described.

  Further, 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 (33).

ただし、ｘのチルダは、以下の（３４）式で表される。

However, the tilde of x is represented by the following formula (34).

  The attitude angle observer 324 calculates the tilde of the roll angle differential amount dφ and the pitch angle differential amount dθ tilde, which are the attitude angles with respect to the vehicle body vertical direction, by using the equation (33), and calculates the calculated roll angle. By integrating each of the tilde of the differential amount dφ and the tilde of the differential amount dθ of the pitch angle, the tilde of the roll angle φ and the tilde of the pitch angle θ can be calculated.

Next, respective calculation methods of the differential amount dφ _m of the roll angle φ and the differential amount dθ _m of the pitch angle θ obtained from the motion equation of the vehicle motion will be described.

The above formula (6) is substituted into the above formulas (1) to (5), the vertical vehicle body speed is assumed to be 0, and the sensor signals G _x , G _y , G _z , P, R are predetermined. _Assuming that the drift errors G _xdr , G _ydr , G _zdr , P _dr , and R _dr are superimposed, the following equations (35) to (38) are obtained.

However, estimated values of the roll angle φ and the pitch angle θ estimated by the posture angle observer 324 are used for the roll angle φ and the pitch angle θ. Further, each of the vertical acceleration G _z -G _zdr and the roll angular velocity P-P _dr is a correction signal corrected to remove the sensor drift amount from the sensor signals detected by the vertical acceleration sensor 14 and the roll angular velocity sensor 20. is there. Further, the longitudinal acceleration G _x -G _xdr , the lateral acceleration G _y -G _ydr , and the yaw angular velocity R-R _dr were detected by the longitudinal acceleration sensor 18, the lateral acceleration sensor 16, and the yaw angular velocity sensor 22, respectively. The correction signal is corrected so as to remove the sensor drift amount with respect to each sensor signal.

  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. .

The differential amount dφ _m of the roll angle φ and the differential amount dθ _m of the pitch angle θ obtained from the equation of motion of the vehicle motion are expressed by the following equation (39) ignoring the drift amount of the above equations (37) and (38): It is calculated by equation (40).

Further, the following (41) exists between the dot of φ in the above expression (37), the dot of θ in the above expression (38), and dφ _{m in} the above expression (39) and dθ _{m in the} above expression (40). ) And the relationship represented by the formula (42) exist.

  The motion equation differential amount calculation means 326 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 (39) and (40).

  In this embodiment, the drift amount estimation means 328 compares the posture angle differential amount calculated by the posture angle observer 324 with the posture angle differential amount calculated by the motion equation differential amount calculation means 326, thereby detecting the sensor drift. Estimate the amount.

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

_Assuming that the predetermined 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, the above equation (41), Assuming that the true value of the differential amount represented by the right side of the equation (42) is known by the attitude angle observer 324, the relationships represented by the following equations (39) and (40) are established.

  Here, dφ and dθ are the differential amount of the roll angle and the differential amount of the pitch angle as the observer internal calculation values obtained by the above equation (33).

  The above formulas (43) and (44) are calculated based on the differential amounts of the roll angle and the pitch angle calculated by the attitude angle observer 324, the roll angle obtained from the motion equation, and the sensor drift amount of the sensor signal. This represents a relationship in which the differential value of the pitch angle is equal to the value considering the sensor drift amount.

Here, since G _x and G _y are not included in the above equations (43) and (44) and the coefficient of R is relatively small (assuming that the posture angle is small), the vertical acceleration sensor 14 and the sensor drift amount related to the vertical acceleration G _z and the roll angular velocity P using the correction signal corrected by the initial dead zone correction unit 23 after the sensor signal detected by the roll angular velocity sensor 20 is corrected by the drift amount correction unit 322. Can be estimated.

  By the way, regarding the lateral acceleration, the relationship expressed by the following equation (45) can be used from the condition that “the slip angle does not stay in the nonlinear region for a long time”.

However, the tau _y, a filter time constant for considering only prolonged exercise, c _f, c _r is a cornering power of the front and rear wheels, l _f, l _r is longitudinal axis - the distance between the centers of gravity It is. Further, l is a wheel base, m is vehicle mass, the [delta] _f is the front wheel steer angle.

  Regarding the vertical acceleration, the relationship expressed by the following equation (46) can be used from the condition that “the acceleration in the vertical direction is the gravitational acceleration”.

However, Dgf and Egf are expressed by the following equations. Also, τ _f is a filter time constant for considering only long-time movement.

  Here, the following equation (47) can be described from the above equations (43), (44), and (46).

ただし、ｄＤ１、ｄＥ１、ｄＥ２、ｄＥ３は、以下の（４８）式〜（５１）式で表される。

However, dD1, dE1, dE2, and dE3 are represented by the following formulas (48) to (51).

  Then, by integrating the coefficient matrix on the left side of the above equation (47) and the vector on the right side for a fixed time, the following equation (52) is obtained.

ただし、Ｄ１、Ｅ１、Ｅ２、Ｅ３は、以下の（５３）式〜（５６）式で表される。

However, D1, E1, E2, and E3 are represented by the following formulas (53) to (56).

  By solving the equation (52), the following equation (57) can be derived.

ただし、Ｄ^＋は、Ｄの擬似逆行列である。

Here, D ⁺ is a pseudo inverse matrix of D.

  The drift amount estimating means 328 calculates the differential amounts of the roll angle and the pitch angle calculated by the posture angle observer 324 and the roll angle and pitch angle calculated by the motion equation differential amount calculating means 326 according to the above equation (57). The sensor drift amount of the vertical acceleration sensor 14 and the sensor drift amount of the roll angular velocity sensor 20 can be estimated based on the respective differential amounts.

  In addition, in the drift amount estimating means 328 according to the present embodiment, in order to stabilize the calculation, the previous estimated value is used and the following equation (58) and equation (59) are used. The estimated value of the sensor drift amount obtained from the calculation result is smoothed.

However, the tilde of G _{zdr and} the tilde of P _dr are estimated values of the sensor drift amount after smoothing, and λ1 is a forgetting factor.

  The initial dead zone correction unit 23 starts estimation of a sensor drift maximum amount and a posture angle predetermined for the roll angular velocity sensor 20 with respect to the sensor signal corresponding to the detected value of the roll angular velocity corrected by the drift amount correction unit 322. The minimum value of the absolute value of the sensor signal corrected by the drift amount correcting means 322 in the history from the current time to the current time is set as the initial dead band width, and the absolute value is set by the initial dead band width. The sensor signal from the roll angular velocity sensor 20 corrected by the drift amount correcting means 322 is corrected so as to reduce the.

  In addition, the initial dead zone correction unit 23 estimates a sensor drift maximum amount and a posture angle that are set in advance for the vertical acceleration sensor 14 for the sensor signal corresponding to the detected value of the vertical acceleration corrected by the drift amount correction unit 322. In the history from the start to the present time, the smallest value of the absolute value of the difference from the standard value of the sensor signal corrected by the drift amount correcting means 322 is set as the width of the initial dead zone. The sensor signal from the vertical acceleration sensor 14 corrected by the drift amount correcting means 322 is corrected so that the absolute value of the difference from the standard value is reduced by the width of

  Next, the estimation result by the method of correcting the sensor signal so as to set the initial dead zone of the present embodiment will be described. In order to confirm the effect of the method of correcting the sensor signal, the posture angle was estimated by applying a drift error to steady circular turning data of about 0.3 G. In addition, only the drift error estimation between the roll angular velocity sensor 20 and the vertical acceleration sensor 14 is performed, and when the correction from the initial dead zone compensation is performed on the signals from both sensors, and when the correction by the initial dead zone compensation is not performed. The angle was estimated.

  First, the posture angle was estimated in a state where a drift error of 3 deg / s was applied to the sensor signal from the roll angular velocity sensor. As shown in FIGS. 10A and 10B, when the correction by the initial dead band compensation is not performed, the estimated value of the roll angle is oscillating and the estimated value of the pitch angle is a true value ( Estimated value when no drift is applied). On the other hand, when correction by initial dead zone compensation is performed, an estimated value of the roll angle close to the true value is calculated, and an estimated value of the pitch angle close to the true value is calculated.

  Further, as shown in FIG. 11B, when a drift error of 3 deg / s is applied to the sensor signal from the roll angular velocity sensor, the initial value is set so as to compensate for the delay in estimating the sensor drift amount of the roll angular velocity. It can be seen that correction by deadband compensation is performed.

  Further, the posture angle was estimated in a state where a drift error of −3 deg / s was applied to the sensor signal from the roll angular velocity sensor. As shown in FIGS. 12A and 12B, when correction by the initial dead band compensation is not performed, each of the estimated value of the roll angle and the estimated value of the pitch angle is a true value (when no drift is applied). It has deviated greatly from the estimated value.

  Further, as shown in FIG. 13B, when a drift error of −3 deg / s is applied to the sensor signal from the roll angular velocity sensor, so as to compensate for the delay in estimating the sensor drift amount of the roll angular velocity, It can be seen that correction by the initial dead band compensation is performed.

  In the third embodiment, the computer functions as each unit of the drift amount correcting unit 322, the initial dead band correcting unit 23, the attitude angle observer 324, the motion equation differential amount calculating unit 326, and the drift amount estimating unit 328. Information processing by the program can be realized by a 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.

  The posture angle estimation processing routine will be described below. In addition, about the process similar to 1st Embodiment, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

  First, in step 100, sensor signals corresponding to the detected values are acquired from the longitudinal acceleration sensor 18, the lateral acceleration sensor 16, the vertical acceleration sensor 14, the roll angular velocity sensor 20, and the yaw angular velocity sensor 22.

  In step 350, the vertical acceleration sensor acquired in step 100 is obtained using the sensor drift amount of the vertical acceleration sensor 14 and the sensor drift amount of the roll angular velocity sensor 20 obtained in step 366, which is one calculation cycle before. 14 and the sensor signal output from the roll angular velocity sensor 20 are corrected.

  Then, in step 352, the history of corrected detection values of the vertical acceleration sensor 14 obtained after starting the estimation of the attitude angle for the sensor signal corresponding to the detection values of the vertical acceleration sensor 14 corrected in step 350 above. Among these, it is determined whether or not the absolute value of the difference between the detected value and the standard value is minimized. If the absolute value of the difference between the detected value and the standard value of the sensor signal corresponding to the detected value of the vertical acceleration sensor 14 corrected in step 350 is not minimized, the process proceeds to step 356. On the other hand, if the absolute value of the difference between the detected value and the standard value is minimum for the sensor signal corresponding to the detected value of the vertical acceleration sensor 14 corrected in step 350 in step 352, in step 354, The detection value of the sensor signal corresponding to the detection value of the vertical acceleration sensor 14 corrected in step 350 is stored as the minimum absolute value of the difference from the standard value.

  In the next step 356, the history of the corrected detection value of the roll angular velocity sensor 20 obtained after starting the estimation of the posture angle for the sensor signal corresponding to the detection value of the roll angular velocity sensor 20 corrected in the above step 350. Of these, it is determined whether or not the absolute value of the detected value is minimized. If the absolute value of the detected value of the sensor signal corresponding to the detected value of the roll angular velocity sensor 20 corrected in step 350 is not minimized, the process proceeds to step 360. On the other hand, if the absolute value of the detected value is minimum for the sensor signal corresponding to the detected value of the roll angular velocity sensor 20 corrected in step 350 in step 356, the absolute value of the detected value is determined in step 358. As the minimum value, the detection value of the sensor signal from the roll angular velocity sensor 20 corrected in step 350 is stored.

  In step 360, the sensor signal corresponding to the detection value of the vertical acceleration sensor 14 corrected in step 350 is recorded in the history from the start of estimation of the predetermined maximum sensor drift amount and posture angle to the present time. The sensor signal is further corrected so as to reduce the absolute value of the difference between the detected value and the standard value by the smaller of the minimum absolute value of the difference from the standard value. In addition, with respect to the sensor signal corresponding to the detection value of the roll angular velocity sensor 20 corrected in step 350, the absolute value of the detection value in the history from the start of estimation of the predetermined maximum sensor drift amount and posture angle to the present time. The sensor signal is further corrected so as to decrease the absolute value of the detected value by the smaller one of the maximum values.

  In step 362, the estimated posture angle obtained in step 368, which will be described later, the lateral vehicle body speed estimated by using a Kalman filter, etc., and the front and rear estimated based on the wheel speed of each wheel. Using the vehicle body speed, as described above, the differential amounts of the roll angle and the pitch angle calculated by the attitude angle observer 324 to estimate the roll angle and the pitch angle are calculated.

  Then, in step 364, as described above, using the estimated attitude angle value obtained in step 368, which will be described later, and the estimated lateral vehicle body speed and front / rear vehicle body speed, the above step 362 is performed. Using the relationship that the differential value of each of the roll angle and the pitch angle calculated in the above and the differential value of each of the roll angle and the pitch angle obtained from the equation of motion of the vehicle are equal to the value considering the sensor drift amount. The sensor drift amount of the vertical acceleration sensor 14 and the sensor drift amount of the roll angular velocity sensor 20 are estimated.

  In the next step 366, the sensor drift amount of the vertical acceleration sensor 14 and the sensor drift amount of the roll angular velocity sensor 20 estimated in step 364 are smoothed using the estimated value of the previous sensor drift amount.

  In the next step 368, the roll angle and the pitch angle are estimated and integrated by integrating the differential amounts of the roll angle and the pitch angle calculated in step 362, and the process returns to step 100.

  Next, a fourth embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment-3rd Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  In the fourth embodiment, a posture angle is estimated using a pitch angular velocity sensor in addition to various sensors, a sensor drift amount of the pitch angular velocity sensor is estimated, a pitch angular velocity sensor The point that it correct | amends so that an initial dead zone may be set with respect to a sensor signal is mainly different from 3rd Embodiment.

  As shown in FIG. 15, the posture angle estimation apparatus 410 according to the fourth embodiment includes a vertical acceleration sensor 14, a lateral acceleration sensor 16, a longitudinal acceleration sensor 18, a roll angular velocity sensor 20, a yaw angular velocity sensor 22, and a pitch angular velocity. A sensor 220 is provided.

  The pitch angular velocity sensor 220 and the roll angular velocity sensor 20 are connected to a drift amount correcting unit 422 that corrects a sensor signal from each sensor based on a sensor drift amount estimated by a drift amount estimating unit 428 described later. The drift amount correcting means 422 is connected to the drift amount estimating means 428.

  The drift amount correcting unit 422 corrects each sensor signal corresponding to each detected value from the pitch angular velocity sensor 220 and the roll angular velocity sensor 20 corrected by the drift amount correcting unit 422 so as to provide an initial dead zone. Connected to means 423. The initial dead zone correction means 423 is connected to a posture angle observer 424 that estimates a roll angle φ and a pitch angle θ, which are posture angles with respect to the vertical axis of the vehicle body.

  The pitch angular velocity sensor 220, the roll angular velocity sensor 20, the yaw angular velocity sensor 22, and the attitude angle observer 324 are motion equation differential amount calculation means 426 that calculates the differential amounts of the roll angle and the pitch angle obtained from the motion equation of vehicle motion. It is connected to the.

  Connected to the attitude angle observer 424 are the initial dead zone correction means 423, the vertical acceleration sensor 14, the lateral acceleration sensor 16, the longitudinal acceleration sensor 18, and the yaw angular velocity sensor 22.

  The initial dead zone correction unit 423 starts estimation of a sensor drift maximum amount and a posture angle predetermined for the roll angular velocity sensor 20 with respect to a sensor signal corresponding to the detected value of the roll angular velocity corrected by the drift amount correction unit 422. The minimum value of the absolute value of the sensor signal corrected by the drift amount correcting means 422 in the history from the current time to the current time is set as the width of the initial dead zone, and the absolute value by the width of the initial dead zone So that the sensor signal from the roll angular velocity sensor 20 corrected by the drift amount correcting means 422 is corrected.

  Further, the initial dead zone correction unit 423 estimates the sensor drift maximum amount and the posture angle that are predetermined for the pitch angular velocity sensor 220 for the sensor signal corresponding to the detected value of the pitch angular velocity corrected by the drift amount correcting unit 422. The smaller one of the absolute values of the absolute values of the sensor signals corrected by the drift amount correcting means 422 in the history from the start to the present time is set as the width of the initial dead zone. The sensor signal from the pitch angular velocity sensor 220 corrected by the drift amount correcting means 422 is corrected so as to reduce the absolute value.

  The attitude angle observer 424 estimates the lateral vehicle body speed V that is the vehicle speed in the lateral direction of the vehicle, and estimates the longitudinal vehicle body speed U that is the vehicle speed in the vehicle longitudinal direction based on the wheel speed of each wheel.

Further, the attitude angle observer 424 replaces the expression (33) described in the third embodiment with the following expression (60), and is corrected by the drift amount correcting means 322 and then corrected by the initial dead band correcting means 423. Correction signals Q-Q _dr and P-P _dr corresponding to the corrected detected values of the pitch angular velocity Q and the roll angular velocity P of the vehicle motion, the longitudinal acceleration G _x , the lateral acceleration G _y , and the vertical acceleration G of the vehicle motion. _Using a sensor signal corresponding to each detected value of _z and yaw angular velocity R and an estimated value V _so of the front and rear vehicle body speed U, a tilde and a pitch angle of a differential dφ of a roll angle that is an attitude angle with respect to the vertical direction of the vehicle body And the tilde of the differential amount dθ. Further, the posture angle observer 424 calculates the tilde of the roll angle φ and the tilde of the pitch angle θ by integrating each of the calculated tilde of the roll angle derivative dφ and the tilde of the pitch angle derivative dθ. Can do.

  The motion equation differential amount calculating means 426 replaces the above equations (39) and (40) with the following equations (61) and (62), and roll angle φ and pitch angle obtained from the motion equation of vehicle motion. Calculate the derivative of θ.

  However, the estimated values of the roll angle φ and the pitch angle θ estimated by the attitude angle observer 424 are used for the roll angle φ and the pitch angle θ.

  The drift amount estimating means 428 estimates the sensor drift amount from a comparison between the posture angle differential amount calculated by the posture angle observer 424 and the posture angle differential amount calculated by the motion equation differential amount calculating means 426.

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

In the above formulas (4) and (5), if the predetermined drift errors G _xdr , G _ydr , P _dr , Q _dr , R _dr are superimposed on the sensor signals G _x , G _y , P, Q, R. Assuming that, the following equations (63) and (64) are obtained.

  Assuming that the true value is known by the attitude angle observer 424 for the differential amounts represented by the above formulas (63) and (64), the relationships represented by the following formulas (65) and (66) are obtained. To establish.

  Here, dφ and dθ are the differential amount of the roll angle and the differential amount of the pitch angle as the observer internal calculation values obtained by the above equation (60).

  The above formulas (65) and (66) are the differential amounts of the roll angle and the pitch angle calculated by the attitude angle observer 424 when the sensor drift amounts of the sensor signals of the roll angular velocity sensor 20 and the pitch angular velocity sensor 220 are taken into account. And the differential value of the roll angle and the pitch angle obtained from the equation of motion are equal to the value considering the sensor drift amount.

  Here, since the coefficient of the yaw angular velocity drift in the equations (65) and (66) is relatively small (assuming that the attitude angle is small) and the influence can be ignored, the yaw angular velocity drift term is ignored and the following Organize as shown in equation (67).

  The roll angular velocity drift and the pitch angular velocity drift can be obtained by the following equation (69) derived by solving the following equation (68) obtained by integrating the coefficient matrix on the left side of equation (67) and the vector on the right side for a certain period of time. it can.

  The drift amount estimation means 428 calculates the differential amounts of the roll angle and the pitch angle calculated by the posture angle observer 424 and the roll angle and pitch angle calculated by the motion equation differential amount calculation means 426 according to the above equation (69). The sensor drift amount of the pitch angular velocity sensor 220 and the sensor drift amount of the roll angular velocity sensor 20 can be estimated on the basis of the respective differential amounts.

  In addition, in the drift amount estimating means 428 according to the present embodiment, in order to stabilize the calculation, the above estimated value is used according to the following equations (70) and (71), and the above equation (69) is used. The estimated value of the sensor drift amount obtained from the calculation result is smoothed.

However, the tilde of Q _{dr and} the tilde of P _dr are estimated values of the sensor drift amount after smoothing, and λ1 is a forgetting factor.

  In the above embodiment, the case where the pitch angular velocity, the roll angle, and the pitch angle are estimated using the previous value of each estimated value has been described as an example. The pitch angular velocity, the roll angle, and the pitch angle may be estimated by using the previous value.

It is a block diagram which shows the 1st Embodiment of this invention. It is explanatory drawing which shows the coordinate system of this Embodiment. It is a figure which shows the relationship between the sensor signal from a roll angular velocity sensor, and an initial dead zone. When the drift error upper limit is applied to the sensor signal from the roll angular velocity sensor, when the sensor signal is a true value, and when the drift error lower limit is applied to the sensor signal, the sensor signal and the initial value It is a figure which shows the relationship with a dead zone. When the drift error upper limit is applied to the sensor signal from the roll angular velocity sensor, when the sensor signal is a true value, and when the drift error lower limit is applied to the sensor signal, the sensor signal and the initial value It is a figure which shows the relationship with a dead zone. When the drift error upper limit is applied to the sensor signal from the roll angular velocity sensor, when the sensor signal is a true value, and when the drift error lower limit is applied to the sensor signal, the sensor signal and the initial value It is a figure which shows the relationship with a dead zone. It is a flowchart which shows the process of the 1st Embodiment of this invention. It is a block diagram which shows the 2nd Embodiment of this invention. It is a block diagram which shows the 3rd Embodiment of this invention. It is a diagram which shows the estimation result of the roll angle and pitch angle in 3rd Embodiment. It is a diagram which shows the sensor drift amount estimated value with respect to the sensor signal of a vertical acceleration and roll angular velocity in 3rd Embodiment, and an initial dead zone. It is a diagram which shows the estimation result of the roll angle and pitch angle in 3rd Embodiment. It is a diagram which shows the sensor drift amount estimated value with respect to the sensor signal of the vertical acceleration and roll angular velocity in 3rd Embodiment, and an initial dead zone. It is a flowchart which shows the process of the 3rd Embodiment of this invention. It is a block diagram which shows the 4th Embodiment of this invention.

Explanation of symbols

10 lateral velocity estimation means 12 longitudinal velocity estimation means 14 vertical acceleration sensor 16 lateral acceleration sensor 18 longitudinal acceleration sensor 20 roll angular velocity sensor 22 yaw angular velocity sensors 23, 223, 423 initial dead zone correction means 24 pitch angular velocity estimation means 26, 324, 424 posture Angular observer 220 Pitch angular velocity sensors 310, 410 Attitude angle estimating devices 322, 422 Drift amount correcting means 326, 426 Equation of motion differential amount calculating means 328, 428 Drift amount estimating means

Claims (11)

Front-rear speed estimation means for estimating front-rear vehicle body speed, which is a vehicle body speed in the vehicle front-rear direction, based on the wheel speed of each wheel;
A lateral speed estimating means for estimating a lateral vehicle body speed which is a vehicle body speed in a lateral direction of the vehicle;
Pitch angular velocity estimation means for estimating the pitch angular velocity;
For the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion, either the predetermined maximum sensor drift amount or the minimum absolute value of the detected value from the start of the estimation of the posture angle to the present time is smaller. Correction means for correcting the sensor signal so as to reduce the absolute value of the detection value by the amount of
Sensor signal corresponding to detected values of longitudinal acceleration, lateral acceleration, vertical acceleration and yaw angular velocity of vehicle motion, sensor signal corresponding to detected value of roll angular velocity corrected by the correcting means, estimation of front and rear vehicle body velocity A posture angle estimating means for estimating a roll angle and a pitch angle, which are posture angles with respect to a vertical axis of the vehicle body, based on the value and the estimated value of the pitch angular velocity;
A vehicle attitude angle estimation device including:
The pitch angular velocity estimation means is estimated by the front and rear vehicle body speed estimated value, the lateral vehicle body speed estimated value, the vertical acceleration detected value, the corrected roll angular velocity detected value, and the posture angle estimating means A vehicle attitude angle estimation device that estimates the pitch angular velocity based on a previous estimated value of the roll angle and a previous estimated value of the pitch angle estimated by the attitude angle estimating means. Front-rear speed estimation means for estimating front-rear vehicle body speed, which is a vehicle body speed in the vehicle front-rear direction, based on the wheel speed of each wheel;
A lateral speed estimating means for estimating a lateral vehicle body speed which is a vehicle body speed in a lateral direction of the vehicle;
Pitch angular velocity estimation means for estimating the pitch angular velocity;
For the sensor signal corresponding to the detected value of the vertical acceleration of the vehicle motion, the absolute value of the difference between the detected value and the standard value from the start of estimation of the predetermined sensor drift maximum amount and attitude angle to the present time Correction means for correcting the sensor signal so as to reduce the absolute value of the difference between the detected value and the standard value by the smaller one of the minimum values;
Sensor signal corresponding to detected values of longitudinal acceleration, lateral acceleration, yaw angular velocity, and roll angular velocity of vehicle motion, sensor signal corresponding to the detected value of vertical acceleration corrected by the correcting means, and estimation of the longitudinal vehicle body speed A posture angle estimating means for estimating a roll angle and a pitch angle, which are posture angles with respect to a vertical axis of the vehicle body, based on the value and the estimated value of the pitch angular velocity;
A vehicle attitude angle estimation device including:
The pitch angular velocity estimating means is estimated by the front and rear vehicle body speed estimated value, the lateral vehicle body speed estimated value, the corrected vertical acceleration detected value, the roll angular velocity detected value, and the posture angle estimating means. A vehicle attitude angle estimation device that estimates the pitch angular velocity based on a previous estimated value of the roll angle and a previous estimated value of the pitch angle estimated by the attitude angle estimating means. Front-rear speed estimation means for estimating front-rear vehicle body speed, which is a vehicle body speed in the vehicle front-rear direction, based on the wheel speed of each wheel;
A lateral speed estimating means for estimating a lateral vehicle body speed which is a vehicle body speed in a lateral direction of the vehicle;
For the sensor signal corresponding to the detected value of the pitch angular velocity of the vehicle motion, either a predetermined maximum sensor drift amount or the minimum absolute value of the detected value from the start of the estimation of the attitude angle to the present time is smaller. Correction means for correcting the sensor signal so as to reduce the absolute value of the detection value by the amount of
Sensor signals corresponding to the detected values of longitudinal acceleration, lateral acceleration, vertical acceleration, yaw angular velocity, and roll angular velocity of the vehicle motion, sensor signals corresponding to the detected value of the pitch angular velocity corrected by the correcting means, and the longitudinal Posture angle estimation means for estimating a roll angle and a pitch angle, which are posture angles with respect to the vertical axis of the vehicle body, based on the estimated value of the vehicle body speed;
A vehicle attitude angle estimation device including: The correction means corrects the sensor signal corresponding to the detected value of vertical acceleration and starts estimating a predetermined sensor drift maximum amount and posture angle for the sensor signal corresponding to the detected value of the roll angular velocity. The sensor signal is corrected so as to reduce the absolute value of the detected value by the smaller one of the minimum absolute values of the detected value from the current time to the current time,
The pitch angular velocity estimating means includes an estimated value of the longitudinal vehicle body speed, an estimated value of the lateral vehicle body speed, the corrected detected value of the vertical acceleration, the corrected detected value of the roll angular velocity, and the attitude angle estimating means. The pitch angular velocity is estimated based on the previous estimated value of the roll angle estimated in step (b) and the previous estimated value of the pitch angle estimated by the posture angle estimating means,
The posture angle estimating means includes a sensor signal corresponding to the detected values of longitudinal acceleration, lateral acceleration, and yaw angular velocity of the vehicle motion, a sensor signal corresponding to the detected value of the vertical acceleration corrected by the correcting means, and the correction. A roll angle and a pitch angle which are attitude angles with respect to a vertical axis of the vehicle body based on the sensor signal corresponding to the detected value of the roll angular velocity corrected by the means, the estimated value of the front and rear vehicle body speed, and the estimated value of the pitch angular velocity. The vehicle attitude angle estimation device according to claim 2, wherein The correction means corrects the sensor signal corresponding to the detected value of the pitch angular velocity, and starts estimating a predetermined sensor drift maximum amount and posture angle for the sensor signal corresponding to the detected value of the roll angular velocity. The sensor signal is corrected so as to reduce the absolute value of the detected value by the smaller one of the minimum absolute values of the detected value from the current time to the current time,
The posture angle estimating means is a sensor signal corresponding to the detected values of longitudinal acceleration, lateral acceleration, vertical acceleration and yaw angular velocity of the vehicle motion, and a sensor signal corresponding to the detected value of the pitch angular velocity corrected by the correcting means. A roll angle and a pitch angle, which are posture angles with respect to a vertical axis of the vehicle body, are estimated based on a sensor signal corresponding to the detected value of the roll angular velocity corrected by the correcting unit and an estimated value of the front and rear vehicle body speed. Item 4. The vehicle attitude angle estimation device according to Item 3. The correction means corrects the sensor signal corresponding to the detected value of the pitch angular velocity and the sensor signal corresponding to the detected value of the roll angular velocity, and at the same time, determines a predetermined sensor for the sensor signal corresponding to the detected value of the vertical acceleration. The difference between the detected value and the standard value is the smaller of the absolute value of the absolute value of the difference between the detected value and the standard value from the start of estimation of the maximum drift amount and attitude angle. Correct the sensor signal to reduce the absolute value of the difference,
The posture angle estimating means includes a sensor signal corresponding to the detected values of longitudinal acceleration, lateral acceleration, and yaw angular velocity of the vehicle motion, a sensor signal corresponding to the detected value of the vertical acceleration corrected by the correcting means, and the correction. Based on the sensor signal corresponding to the detected value of the pitch angular velocity corrected by the means, the sensor signal corresponding to the detected value of the roll angular velocity corrected by the correcting means, and the estimated value of the front and rear vehicle body speed, 6. The vehicle attitude angle estimation device according to claim 5, wherein a roll angle and a pitch angle, which are attitude angles with respect to the vertical axis, are estimated. The correction means corrects the sensor signal corresponding to the detected value of the pitch angular velocity, and starts estimating a predetermined sensor drift maximum amount and posture angle for the sensor signal corresponding to the detected value of the vertical acceleration. The sensor signal so as to reduce the absolute value of the difference between the detected value and the standard value by the smaller of the minimum absolute value of the difference between the detected value and the standard value from the current time to the current time. To correct
The posture angle estimation means is a sensor signal corresponding to the detected values of the longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity of the vehicle motion, and a sensor signal corresponding to the detected value of the pitch angular velocity corrected by the correcting means. A roll angle and a pitch angle, which are posture angles with respect to a vertical axis of the vehicle body, are estimated based on a sensor signal corresponding to the detected value of the vertical acceleration corrected by the correction unit and an estimated value of the longitudinal vehicle body speed. Item 4. The vehicle attitude angle estimation device according to Item 3. For the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion, either the predetermined maximum sensor drift amount or the minimum absolute value of the detected value from the start of the estimation of the posture angle to the present time is smaller. The sensor signal is corrected so that the absolute value of the detected value is reduced by the amount of the detected value, and a predetermined sensor drift maximum amount and posture angle are estimated for the sensor signal corresponding to the detected value of vertical acceleration. The absolute value of the difference between the detected value and the standard value is reduced by the smaller of the minimum absolute value of the difference between the detected value and the standard value from the start of Correction means for correcting the sensor signal;
Based on sensor signals corresponding to detected values of yaw angular velocity, longitudinal acceleration, and lateral acceleration of vehicle motion, and sensor signals corresponding to detected values of roll angular velocity and vertical acceleration corrected by the correcting means. The attitude for calculating the roll angle and the pitch angle with respect to the vertical axis of the vehicle body and estimating the roll angle and the pitch angle by integrating the calculated differential amounts of the roll angle and the pitch angle. Angle estimation means;
Based on sensor signals corresponding to detected values of the vertical acceleration, longitudinal acceleration, lateral acceleration, roll angular velocity, and yaw angular velocity, and the roll angle and pitch angle estimated by the posture angle estimating means. Calculating means for calculating a differential amount of each of the roll angle and the pitch angle obtained from a motion equation of vehicle motion;
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 sensor of the sensor signal corresponding to the detected value of the vertical acceleration and the sensor signal corresponding to the detected value of the roll angular velocity is used by using the relationship in which each differential amount is equal to the value considering the sensor drift amount. Drift amount estimation means for estimating the drift amount;
Sensor drift amount estimation apparatus including For the sensor signal corresponding to the detected value of the roll angular velocity of the vehicle motion, either the predetermined maximum sensor drift amount or the minimum absolute value of the detected value from the start of the estimation of the posture angle to the present time is smaller. The sensor signal is corrected so as to decrease the absolute value of the detected value by the amount of the detected value, and a predetermined sensor drift maximum amount and posture angle are estimated for the sensor signal corresponding to the detected value of the pitch angular velocity. Correction means for correcting the sensor signal so as to reduce the absolute value of the detected value by the smaller one of the minimum absolute values of the detected value from the start to the present time;
A sensor signal corresponding to each detected value of vertical acceleration, longitudinal acceleration, lateral acceleration, and yaw angular velocity of the vehicle motion, and a sensor signal corresponding to each detected value of the roll angular velocity and the pitch angular velocity corrected by the correcting means; The roll angle and the pitch angle are calculated with respect to the vertical axis of the vehicle body, and the roll angle and the pitch angle are integrated to calculate the roll angle and the pitch angle. Posture angle estimating means for estimating;
Based on a motion equation of vehicle motion based on a sensor signal corresponding to each detected value of the roll angular velocity, the yaw angular velocity, and the pitch angular velocity, and the roll angle and the pitch angle estimated by the posture angle estimating means. A calculating means for calculating a differential amount of each of the roll angle and the pitch angle obtained;
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 The sensor signal corresponding to the detected value of the pitch angular velocity and the sensor signal corresponding to the detected value of the roll angular velocity using the relationship in which the differential amount of each is equal to the value considering the sensor drift amount Drift amount estimation means for estimating the drift amount;
Sensor drift amount estimation apparatus including Drift correction means for correcting each of the sensor signal corresponding to the detected value of the vertical acceleration and the sensor signal corresponding to the detected value of the roll angular velocity based on the sensor drift amount estimated by the drift amount estimating means. Including
The posture angle estimating means is responsive to a sensor signal corresponding to the detected value of the yaw angular velocity, and each detected value of the roll angular velocity and the vertical acceleration corrected by the drift correcting means and corrected by the correcting means. The sensor drift amount estimation apparatus according to claim 8, wherein the roll angle and the pitch angle are estimated based on a sensor signal. Drift correction means for correcting each of the sensor signal corresponding to the detected value of the pitch angular velocity and the sensor signal corresponding to the detected value of the roll angular velocity based on the sensor drift amount estimated by the drift amount estimating means. Including
The posture angle estimating means is corrected by the sensor signal corresponding to each detected value of the vertical acceleration, the longitudinal acceleration, the lateral acceleration, and the yaw angular velocity, and corrected by the drift correcting means, and corrected by the correcting means. The sensor drift amount estimation apparatus according to claim 9, wherein the roll angle and the pitch angle are estimated based on a sensor signal corresponding to each detected value of the roll angular velocity and the pitch angular velocity.

Images