JP6981459B2

JP6981459B2 - Sensor error correction device

Info

Publication number: JP6981459B2
Application number: JP2019190566A
Authority: JP
Inventors: 大輝森; 徳祥鈴木; 義和服部; 豪軌杉浦
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2019-10-17
Filing date: 2019-10-17
Publication date: 2021-12-15
Anticipated expiration: 2039-10-17
Also published as: JP2021067472A

Description

本発明は、センサ誤差補正装置に係り、特に、自車両の位置推定に用いるセンサの誤差を修正するセンサ誤差補正装置に関する。 The present invention relates to a sensor error correction device, and more particularly to a sensor error correction device that corrects an error of a sensor used for position estimation of a own vehicle.

車両の姿勢角及び位置に係る情報は、運転支援技術及び予防安全技術にとって最も重要な情報である。例えば、車体のロール角及びピッチ角は、重力加速度成分を補償する制御入力（操舵角、アクセル及びブレーキ）を決定する際に活用でき、より安定した車両の制御が可能になる。さらに、位置推定は車両の位置制御には必要不可欠である。 Information on the attitude angle and position of the vehicle is the most important information for driving assistance technology and preventive safety technology. For example, the roll angle and pitch angle of the vehicle body can be utilized when determining the control input (steering angle, accelerator and brake) for compensating the gravitational acceleration component, and more stable vehicle control becomes possible. Furthermore, position estimation is indispensable for vehicle position control.

車両の位置推定においては、ＧＰＳ（Global Positioning System）装置等により、現在位置を直接推定するのみならず、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能なＩＭＵ（Inertial Measurement Unit：慣性計測装置）、車速センサ及び操舵角センサ等の車両自身の状態を検知する各種センサで検出した自車両の移動の蓄積に基づいて自車両の位置を推定する場合がある。 In vehicle position estimation, not only the current position is directly estimated by a GPS (Global Positioning System) device, but also three-axis angular speeds (pitch rate, roll rate, yaw rate) and 3 that indicate the behavior of the vehicle during driving. Self detected by various sensors that detect the state of the vehicle itself, such as IMU (Inertial Measurement Unit) that can detect axial acceleration (front-back acceleration, lateral acceleration, vertical acceleration), vehicle speed sensor, steering angle sensor, etc. The position of the own vehicle may be estimated based on the accumulation of vehicle movement.

ＩＭＵは、いわゆるゼロ点誤差であるゼロ点バイアスが補正されないと、角速度及び加速度を正確に検出することができない。航空機の分野では、ＧＰＳによって検出した位置情報により、ＩＭＵのゼロ点バイアスを補正する技術が存在する。しかしながら、遮蔽物が存在しない空中を飛翔する航空機と異なり、地上を走行する車両は、大きな建造物の近く又はトンネル等のＧＰＳ衛星からの電波が遮蔽される場所を通過する場合があり、かかる場合にはＩＭＵのゼロ点バイアスの補正が困難となる。また、ＧＰＳによってＩＭＵのゼロ点バイアスを精度よく補正するには、事前にＧＰＳアンテナ及びＩＭＵの各々のキャリブレーションが精度よく行われていることを要する。 The IMU cannot accurately detect the angular velocity and acceleration unless the zero point bias, which is a so-called zero point error, is corrected. In the field of aircraft, there is a technique for correcting the zero point bias of the IMU based on the position information detected by GPS. However, unlike aircraft that fly in the air where there is no obstruction, vehicles traveling on the ground may pass near large buildings or in places where radio waves from GPS satellites such as tunnels are obstructed. It becomes difficult to correct the zero point bias of the IMU. Further, in order to accurately correct the zero point bias of the IMU by GPS, it is necessary that the GPS antenna and the IMU are calibrated with high accuracy in advance.

特許文献１には、車速センサ及びヨーレートセンサの補正をＧＰＳで検出したドップラー速度で補正する速度推定装置の発明が開示されている。 Patent Document 1 discloses an invention of a speed estimation device that corrects a vehicle speed sensor and a yaw rate sensor by a Doppler speed detected by GPS.

特許文献２には、ＧＰＳとＩＭＵとの出力差より、各々の誤差量を推定し、自車両の位置および自車両の方位の推定精度を向上させる慣性航法の方法の発明が開示されている。 Patent Document 2 discloses an invention of an inertial navigation method that estimates the amount of each error from the output difference between GPS and IMU and improves the estimation accuracy of the position of the own vehicle and the orientation of the own vehicle.

特開２０１３−１１３７８９号公報Japanese Unexamined Patent Publication No. 2013-113789 特開平６−３１７４２８号公報Japanese Unexamined Patent Publication No. 6-317428

しかしながら、特許文献１に記載の発明は、使用可能な場合がＧＰＳ衛星からの電波が受信可能な地理的条件下に限定される上に、ヨーレート及び車速の推定が主であり、慣性センサのピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の５つの情報は補正できないという問題があった。 However, the invention described in Patent Document 1 is limited to geographical conditions in which radio waves from GPS satellites can be received when it can be used, and mainly estimates yaw rate and vehicle speed, and pitch of an inertial sensor. There is a problem that the five information of rate, roll rate, front-back acceleration, lateral acceleration and vertical acceleration cannot be corrected.

特許文献２に記載の発明は、ＩＭＵの誤差推定を高精度で行うには、高精度なＧＰＳ情報が必要だが、現在はそのようなサービスは少ないという問題があった。また、使用するＩＭＵは、航空機や船舶に用いられる高精度ＩＭＵが想定されており、ＭＥＭＳ（Micro Electro Mechanical Systems）によって量産されたＩＭＵでは同じアルゴリズムを用いても同等の性能が得られず、位置の推定値が不安定になるという問題があった。 The invention described in Patent Document 2 requires highly accurate GPS information in order to perform IMU error estimation with high accuracy, but there is a problem that such services are currently scarce. In addition, the IMU to be used is assumed to be a high-precision IMU used for aircraft and ships, and the IMU mass-produced by MEMS (Micro Electro Mechanical Systems) cannot obtain the same performance even if the same algorithm is used, and the position. There was a problem that the estimated value of was unstable.

本発明は、上記問題に鑑みてなされたものであり、ＩＭＵが検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定可能なセンサ誤差補正装置を実現することを目的とする。 The present invention has been made in view of the above problems, and realizes a sensor error correction device capable of estimating each error of pitch rate, roll rate, front-back acceleration, lateral acceleration, and vertical acceleration detected by the IMU. The purpose.

上記目的を達成するために、請求項1に記載のセンサ誤差補正装置は、走行時の車両の挙動を示すピッチレート、ロールレート及びヨーレートの３軸の角速度と、前後加速度、横加速度及び上下加速度の３軸の加速度とを検出可能な慣性計測装置と、前記３軸の角速度を用いた姿勢角の変化率に関する運動方程式と、前記３軸の角速度を用いた前記３軸の加速度に関する運動方程式とに基づき、前記車両の運動パターンに応じて、前記慣性計測装置が検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定する誤差推定部と、を含んでいる。 In order to achieve the above object, the sensor error correction device according to claim 1 has three axial angular velocities of pitch rate, roll rate and yaw rate, which indicate the behavior of the vehicle during traveling, and front-back acceleration, lateral acceleration and vertical acceleration. An inertial measuring device that can detect the acceleration of the three axes, a motion equation related to the rate of change of the attitude angle using the angular velocity of the three axes, and a motion equation related to the acceleration of the three axes using the angular velocity of the three axes. Based on the above, an error estimation unit for estimating each error of the pitch rate, the roll rate, the front-back acceleration, the lateral acceleration, and the vertical acceleration detected by the inertial measurement device according to the motion pattern of the vehicle is included.

また、請求項２に記載のセンサ誤差補正装置は、前記車両の運動パターンは前記車両のヨーレートであり、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定し、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する。 Further, in the sensor error correction device according to claim 2, the motion pattern of the vehicle is the yaw rate of the vehicle, and the error estimation unit uses the pitch detected by the inertial measurement unit when the yaw rate of the vehicle is small. Each error of the rate and the roll rate is estimated, and when the yaw rate of the vehicle is large, each error of the front-rear acceleration, the lateral acceleration and the vertical acceleration detected by the inertial measurement unit is estimated.

また、請求項３に記載のセンサ誤差補正装置では、前記誤差補正部は、前後速度を検出する車速検出部と、外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレート及びピッチレートが、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定する。 Further, in the sensor error correction device according to claim 3, the error correction unit includes a vehicle speed detection unit that detects the front-rear speed, an error in the front-rear speed detected by the vehicle speed detection unit based on external information, and the above-mentioned. When the yaw rate of the vehicle is small, the correction unit that corrects the yaw rate error detected by the inertial measurement unit and the error estimation unit measure the roll rate and pitch rate of the vehicle before and after the inertial measurement unit detects it. The error of each of the pitch rate and the roll rate detected by the inertial measurement unit is estimated based on the differential values of the acceleration and the lateral acceleration and the front-back speed and the yaw rate corrected by the correction unit.

また、請求項４に記載のセンサ誤差補正装置では、前後速度を検出する車速検出部と、外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度、横加速度及び上下加速度が、前記車両のロールレート及びピッチレートと、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する。 Further, in the sensor error correction device according to claim 4, the vehicle speed detection unit that detects the front-rear speed, the error of the front-rear speed detected by the vehicle speed detection unit based on external information, and the inertial measurement unit detect it. When the yaw rate of the vehicle is large, the correction unit that corrects the yaw rate error and the error estimation unit determine the front-rear acceleration, the lateral acceleration, and the vertical acceleration, the roll rate and pitch rate of the vehicle, and the inertial measurement unit. The front-back acceleration, the lateral acceleration, and the vertical acceleration detected by the inertial measurement unit based on the differential values of the front-back acceleration and the lateral acceleration detected by the inertial measurement unit and the front-back speed and the yaw rate corrected by the correction unit. Estimate each error of.

また、請求項５に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレートが、前記慣性計測装置が検出した横加速度の微分値から前記補正部により補正された前後速度と前記補正されたヨーレートの微分値との積を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したロールレートの誤差を推定する。 Further, in the sensor error correction device according to claim 5, when the yaw rate of the vehicle is small, the error estimation unit determines that the roll rate of the vehicle is the derivative value of the lateral acceleration detected by the inertial measurement unit. The roll rate detected by the inertial measurement unit based on the quotient of the gravitational acceleration and the value obtained by subtracting the product of the front-rear velocity corrected by the correction unit and the differential value of the corrected yaw rate. Estimate the error.

また、請求項６に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のピッチレートが、前記慣性計測装置が検出した前後加速度の微分値から前記補正部により補正された前後速度の２階微分値を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したピッチレートの誤差を推定する。 Further, in the sensor error correction device according to claim 6, when the yaw rate of the vehicle is small, the error estimation unit determines that the pitch rate of the vehicle is the differential value of the front-rear acceleration detected by the inertial measurement unit. The pitch rate error detected by the inertial measurement unit is estimated based on the quotient of the gravitational acceleration and the value obtained by subtracting the second-order differential value of the front-rear velocity corrected by the correction unit.

また、請求項７に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度が、ロールレートと重力加速度との積から、前記慣性計測装置が検出した横加速度の微分値を減算し、前記補正部により補正されたヨーレートの微分値と前記補正された前後速度との積を加算し、前記補正されたヨーレートと前記補正された前後速度の微分値との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した前後加速度の誤差を推定し、横加速度が、ピッチレートと重力加速度との積から、前記慣性計測装置が検出した前後加速度の微分値を減算し、前記補正された前後速度の２階微分値を加算し、前記補正されたヨーレートの二乗と前記補正された前後速度との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した横加速度の誤差を推定し、上下加速度が、重力加速度からロールレートと前記補正された前後速度との積を減算して得た値で表されることに基づき前記慣性計測装置が検出した上下加速度の誤差を推定する。 Further, in the sensor error correction device according to claim 7, the error estimation unit detects the front-rear acceleration from the product of the roll rate and the gravity acceleration when the yaw rate of the vehicle is large. The differential value of the lateral acceleration is subtracted, the product of the yaw rate corrected by the correction unit and the corrected front-rear velocity is added, and the corrected yaw rate and the corrected front-rear velocity are added to each other. The error of the longitudinal acceleration detected by the inertial measuring device is estimated based on the quotient of the value obtained by adding the products of From the product of, the differential value of the anteroposterior acceleration detected by the inertial measuring device is subtracted, the second-order differential value of the corrected anteroposterior velocity is added, and the square of the corrected yaw rate and the corrected anteroposterior velocity The error of the lateral acceleration detected by the inertial measuring device is estimated based on the quotient of the value obtained by adding the products of the above and the corrected yaw rate, and the vertical acceleration is the roll rate from the gravity acceleration. The error of the vertical acceleration detected by the inertial measuring device is estimated based on the value obtained by subtracting the product of the corrected front-rear velocity.

また、請求項８に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したロールレートの誤差及びピッチレートの誤差を含む状態量の予測値を算出し、ロールレートの誤差及びピッチレートの誤差を仮定した、前記慣性計測装置が検出したロールレート及びピッチレートの観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第１事前推定部と、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第１事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第１事前推定部によって算出した前記状態量の予測値を補正する第１状態推定部と、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を含む状態量の予測値を算出し、前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を仮定した、前記慣性計測装置が検出した前後加速度、横加速度、及び上下加速度の観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第２事前推定部と、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第２事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第２事前推定部によって算出した前記状態量の予測値を補正する第２状態推定部と、を含んでいる。 Further, in the sensor error correction device according to claim 8, when the yaw rate of the vehicle is small, the error estimation unit has a state quantity including a roll rate error and a pitch rate error detected by the inertial measurement unit. The predicted value is calculated, and the roll rate error and the pitch rate error are assumed. The first pre-estimation unit that calculates the predicted value of the observed value detected by the inertial measurement unit, the observed value detected and output by the inertial measurement unit when the yaw rate of the vehicle is small, and the first pre-estimation. When the yaw rate of the vehicle is large and the first state estimation unit that corrects the predicted value of the state quantity calculated by the first pre-estimation unit based on the difference from the predicted value of the observed value calculated by the unit. , Calculate the predicted value of the state quantity including the error of the front-back acceleration, the error of the lateral acceleration, and the error of the vertical acceleration detected by the inertial measurement unit, and calculate the error of the front-back acceleration, the error of the lateral acceleration, and the error of the vertical acceleration. Using the assumed observation equations for the observed values of longitudinal acceleration, lateral acceleration, and vertical acceleration detected by the inertial measurement unit, the predicted value of the observed value detected by the inertial measurement unit is calculated from the predicted value of the state quantity. The second pre-estimation unit to be calculated, the observed value detected and output by the inertial measurement unit when the yaw rate of the vehicle is large, and the predicted value of the observed value calculated by the second pre-estimated unit. It includes a second state estimation unit that corrects the predicted value of the state quantity calculated by the second pre-estimation unit based on the difference.

本発明によれば、ＩＭＵが検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定可能なセンサ誤差補正装置を実現できるという効果を奏する。 According to the present invention, there is an effect that it is possible to realize a sensor error correction device capable of estimating each error of the pitch rate, the roll rate, the front-back acceleration, the lateral acceleration and the vertical acceleration detected by the IMU.

本発明の実施の形態に係るセンサ誤差補正装置の一例を示したブロック図である。It is a block diagram which showed an example of the sensor error correction apparatus which concerns on embodiment of this invention. 本発明の実施の形態における座標系を示した概略図である。It is a schematic diagram which showed the coordinate system in embodiment of this invention. 本発明の実施の形態における変数の一例を示した説明図である。It is explanatory drawing which showed an example of the variable in embodiment of this invention. 本発明乗実施の形態に係る演算装置の一例を示した概略図である。It is a schematic diagram which showed an example of the arithmetic unit which concerns on embodiment of this invention. 本発明の実施の形態に係る演算装置の第１推定部の機能ブロック図の一例である。It is an example of the functional block diagram of the 1st estimation part of the arithmetic unit which concerns on embodiment of this invention. 事前推定部の機能ブロック図の一例である。This is an example of a functional block diagram of the pre-estimation unit. フィルタリング部の機能ブロック図の一例である。This is an example of a functional block diagram of the filtering unit.

以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、本実施の形態に係るセンサ誤差補正装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、車両が備えたセンシング装置及びＧＰＳ等によって取得した車両周辺の情報から車両の現在位置及び現在のヨー角（方位角）を算出する位置計測装置２０と、位置計測装置２０が算出した車両の現在位置及び現在のヨー角、車速センサ２４が検出した車両前後速度、ＩＭＵ２６が検出した車両の方位角の角速度及び加速度、並びに操舵角センサが検出した車両の操舵角が入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両位置の推定の演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算された車両の位置等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. As shown in FIG. 1, the sensor error correction device 10 according to the present embodiment includes a storage device 18 for storing data necessary for the calculation of the calculation device 14 described later and a calculation result by the calculation device 14, and a vehicle. The position measuring device 20 that calculates the current position and current yaw angle (azimuth angle) of the vehicle from the information around the vehicle acquired by the sensing device and GPS, and the current position and current yaw of the vehicle calculated by the position measuring device 20. Input from the input device 12 and the input device 12 for inputting the angle, the vehicle front-rear speed detected by the vehicle speed sensor 24, the angular speed and acceleration of the vehicle azimuth angle detected by the IMU 26, and the steering angle of the vehicle detected by the steering angle sensor. A calculation device 14 composed of a computer or the like that calculates a vehicle position estimation based on the input data and the data stored in the storage device 18, and a CRT that displays the position of the vehicle calculated by the calculation device 14. Alternatively, it is composed of a display device 16 composed of an LCD or the like.

本実施の形態に係る位置計測装置２０のセンシング装置は、一例として、車載カメラ等の撮像装置、ＬＩＤＡＲ（Light Detection and Ranging）及びソナーのいずれかである。車載カメラ等の撮像装置を車載のセンシング装置とした場合は、一例として、当該撮像装置で取得した車両周辺の画像情報を解析して道路の白線等を検出する。ＬＩＤＡＲを車載のセンシング装置とした場合は、一例として、車両周辺に照射したパルス状のレーザの散乱光から道路の白線等を検出する。ソナーを車載のセンシング装置とした場合は、一例として、アスファルトの路面とペイントされた白線との、超音波の反射率の差を利用して、当該白線を識別する。また、位置計測装置２０はＧＰＳにより、車両２００の緯度及び経度の各々の方向の速度情報を含む測位情報を取得可能に構成されている。 As an example, the sensing device of the position measuring device 20 according to the present embodiment is any of an imaging device such as an in-vehicle camera, LIDAR (Light Detection and Ranging), and sonar. When an image pickup device such as an in-vehicle camera is used as an in-vehicle sensing device, as an example, the image information around the vehicle acquired by the image pickup device is analyzed to detect a white line or the like on the road. When LIDAR is used as an in-vehicle sensing device, as an example, white lines on the road are detected from scattered light of a pulsed laser radiated around the vehicle. When the sonar is used as an in-vehicle sensing device, as an example, the white line is identified by using the difference in the reflectance of ultrasonic waves between the asphalt road surface and the painted white line. Further, the position measuring device 20 is configured to be able to acquire positioning information including speed information in each direction of the latitude and longitude of the vehicle 200 by GPS.

続いて、車両２００の挙動に係る座標系を図２に示したように定義する。地球座標系２０４は地球平面を基準として重力加速度方向とｚ_eとが平行で、ｙ_eが北方向を向いている座標系である。路面座標系２０６は、ｚ_rが車両２００の重心を通り路面に垂直な方向に向き、ｘ_rは車両進行方向に向いている座標系である。車体座標系２０８は車体バネ上に固定された座標系で、ｚ_vは車体鉛直上方向、ｘ_vは車体進行方向を向いている。従って、車両２００の前後方向は、車体座標系２０８のｘ軸に平行な方向となる。本実施の形態では、車体座標系２０８の基準点を車両２００の重心ではなく、車両２００の後輪の車軸の車幅方向の中心とする。 Subsequently, the coordinate system related to the behavior of the vehicle 200 is defined as shown in FIG. The earth coordinate system 204 is a coordinate system in which the gravitational acceleration direction and z _e are parallel to each other with _{respect to the earth plane, and y e} points in the north direction. The road surface coordinate system 206 is a coordinate system in which z _r passes through the center of gravity of the vehicle 200 and is oriented in the direction perpendicular to the road surface, and x _r is oriented in the vehicle traveling direction. The vehicle body coordinate system 208 is a coordinate system fixed on the vehicle body spring, z _v faces the vehicle body vertically upward direction, and x _v faces the vehicle body traveling direction. Therefore, the front-rear direction of the vehicle 200 is a direction parallel to the x-axis of the vehicle body coordinate system 208. In the present embodiment, the reference point of the vehicle body coordinate system 208 is not the center of gravity of the vehicle 200, but the center of the axle of the rear wheel of the vehicle 200 in the vehicle width direction.

また、オイラー姿勢角であるロール角φ、ピッチ角θ及びヨー角ψは、地球座標系２０４に対して、図２に示したように定義される。例えば、ロール角φはｘ軸まわりの回転角であり、ピッチ角θは、y軸まわりの回転角であり、ヨー角ψは、z軸まわりの回転角である。また、ロール角φ、ピッチ角θ及びヨー角ψの各々は、右ネジの方向（図２では、各々の矢印方向）の回転で正の値を示す。本実施の形態では、便宜上、後述するヨー角偏差は基準座標系を路面座標系２０６とし、さらに、本来は地球座標系２０４に対して定義されるオイラー姿勢角を、車体座標系２０８に対して、ロール角φ_v、ピッチ角θ_v及びヨー角ψ_vと定義する。以後、単に、ロール角φ、ピッチ角θ及びヨー角ψと記した場合は、基本的に、車体座標系２０８に対して定義された姿勢角であるとする。 Further, the roll angle φ, the pitch angle θ, and the yaw angle ψ, which are Euler attitude angles, are defined as shown in FIG. 2 with respect to the earth coordinate system 204. For example, the roll angle φ is the rotation angle around the x-axis, the pitch angle θ is the rotation angle around the y-axis, and the yaw angle ψ is the rotation angle around the z-axis. Further, each of the roll angle φ, the pitch angle θ, and the yaw angle ψ shows positive values when rotated in the direction of the right-handed screw (in the direction of each arrow in FIG. 2). In the present embodiment, for convenience, the yaw angle deviation described later uses the road surface coordinate system 206 as the reference coordinate system, and further, the Euler attitude angle originally defined for the earth coordinate system 204 is set with respect to the vehicle body coordinate system 208. , Roll angle φ _v , pitch angle θ _v and yaw angle ψ _v . Hereinafter, when the roll angle φ, the pitch angle θ, and the yaw angle ψ are simply described, it is basically assumed that the posture angle is defined with respect to the vehicle body coordinate system 208.

図３は、本実施の形態における変数の一例を示した説明図である。本実施の形態では、車両２００の前後速度Ｕ、車両２００の横速度Ｖ及び車両２００の上下速度Ｗの各々を定義する。Ｕはｘ軸、Ｖはｙ軸及びＷはｚ軸に各々平行する。 FIG. 3 is an explanatory diagram showing an example of variables in the present embodiment. In this embodiment, each of the front-rear speed U of the vehicle 200, the lateral speed V of the vehicle 200, and the vertical speed W of the vehicle 200 is defined. U is parallel to the x-axis, V is parallel to the y-axis, and W is parallel to the z-axis.

また、車両２００のロール角φ、ピッチ角θ、ヨー角ψに対応するＩＭＵ２６の出力値は、角速度であるロールレートＰ、ピッチレートＱ、ヨーレートＲと定義する。 Further, the output values of the IMU 26 corresponding to the roll angle φ, the pitch angle θ, and the yaw angle ψ of the vehicle 200 are defined as the roll rate P, the pitch rate Q, and the yaw rate R, which are the angular velocities.

従来はＩＭＵ２６、ジャイロセンサ等の精度が不十分であったこともあり、ヨーレートＲの推定にも車両運動モデルを活用していた。しかし近年、安価なＩＭＵ２６に用いられるＭＥＭＳジャイロの精度が向上していると共に、車速センサ及び操舵角センサ等の複数センサが車両２００に搭載されることで特にヨーレートＲについては補正が容易になった。 In the past, the accuracy of the IMU26, gyro sensor, etc. was insufficient, so the vehicle motion model was also used to estimate the yaw rate R. However, in recent years, the accuracy of the MEMS gyro used in the inexpensive IMU26 has been improved, and the vehicle 200 is equipped with a plurality of sensors such as a vehicle speed sensor and a steering angle sensor, which makes it easier to correct the yaw rate R in particular. ..

本実施の形態では、車両運動モデルのヨーレートは使用せず、ＩＭＵ２６が検出したヨーレートＲの値を後述するように補正して使用する。 In the present embodiment, the yaw rate of the vehicle motion model is not used, but the value of the yaw rate R detected by the IMU 26 is corrected and used as described later.

図４は、演算装置１４の一例を示した概略図である。本実施の形態に係る演算装置１４は、ＧＰＳの測位情報又はセンシング装置が検出した車両２０の位置情報によりＩＭＵ２６が検出したヨーレートＲ及び車速センサ２４が検出した車両２００の前後速度Ｕを補正するＧＰＳ補正部４２と、ＩＭＵ２６が検出したロールレートＰ及びピッチレートＱの各々の誤差を推定する第１推定部４４と、ＩＭＵ２６が検出した車両２００の進行方向（ｘ軸方向）の前後加速度、車両２００の横方向（ｙ軸方向）の横加速度及び車両２００の上下方向の上下加速度の各々の誤差を推定する第２推定部４６と、を含む。演算装置１４は、車両２００の位置推定の演算を行うが、本実施の形態では、位置推定の演算に用いるＩＭＵ２６の検出値のゼロ点誤差を推定する場合に特化した説明を行い、位置推定の演算について詳細な説明は省略する。 FIG. 4 is a schematic view showing an example of the arithmetic unit 14. The arithmetic unit 14 according to the present embodiment is a GPS that corrects the yaw rate R detected by the IMU 26 and the front-rear speed U of the vehicle 200 detected by the vehicle speed sensor 24 based on the GPS positioning information or the position information of the vehicle 20 detected by the sensing device. The correction unit 42, the first estimation unit 44 for estimating the respective errors of the roll rate P and the pitch rate Q detected by the IMU 26, the front-back acceleration of the vehicle 200 detected by the IMU 26 in the traveling direction (x-axis direction), and the vehicle 200. Includes a second estimation unit 46 for estimating the respective errors of the lateral acceleration in the lateral direction (y-axis direction) and the vertical acceleration in the vertical direction of the vehicle 200. The arithmetic unit 14 performs the calculation of the position estimation of the vehicle 200, but in the present embodiment, the description specialized in the case of estimating the zero point error of the detected value of the IMU 26 used for the calculation of the position estimation is given, and the position estimation is performed. A detailed description of the operation of is omitted.

ヨーレートＲは、鉛直軸であるｚ軸周りの回転運動の角速度であるから、ｘ軸及びｙ軸を含む平面座標系での回転運動の角速度である。また、車両２００の前後速度Ｕも、ヨーレートＲと同様にｘ軸及びｙ軸を含む平面座標系での運動に係る変化量である。ＧＰＳを用いると、平面座標系での運動による車両２００の位置の変化を精度よく検出できるので、ＧＰＳ補正部４２では、ＧＰＳによって得た測位情報に基づいて車速センサ２４が検出した前後速度Ｕ及びＩＭＵ２６が検出したヨーレートＲを補正する。衛星からの電波が遮蔽される等によりＧＰＳによる測位情報を得られない場合、ＧＰＳ補正部４２では、車載カメラ等の撮像装置、ＬＩＤＡＲ（Light Detection and Ranging）及びソナー等のセンシング装置によって得た車両２００の位置情報に基づいて車速センサ２４が検出した前後速度Ｕ及びＩＭＵ２６が検出したヨーレートＲを補正する。 Since the yaw rate R is the angular velocity of the rotational motion around the z-axis which is the vertical axis, it is the angular velocity of the rotational motion in the plane coordinate system including the x-axis and the y-axis. Further, the front-rear speed U of the vehicle 200 is also a change amount related to the motion in the plane coordinate system including the x-axis and the y-axis, similarly to the yaw rate R. Since GPS can accurately detect changes in the position of the vehicle 200 due to movement in the plane coordinate system, the GPS correction unit 42 can detect the front-rear speed U and the front-rear speed U detected by the vehicle speed sensor 24 based on the positioning information obtained by GPS. The yaw rate R detected by the IMU 26 is corrected. When the GPS positioning information cannot be obtained due to the shielding of radio waves from the satellite, the GPS correction unit 42 uses an image pickup device such as an in-vehicle camera, a vehicle obtained by a sensing device such as LIDAR (Light Detection and Ranging), and a sonar. The front-rear speed U detected by the vehicle speed sensor 24 and the yaw rate R detected by the IMU 26 are corrected based on the position information of the 200.

第１推定部４４では、状態方程式ｆ（ｘ）を用いてＩＭＵ２６が検出したピッチレート、ロールレートの各々の誤差を推定する。第２推定部４６では、第１推定部４４とは別の状態方程式ｆ（ｘ）を用いてＩＭＵ２６が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する。状態方程式ｆ（ｘ）を用いた第１推定部４４及び第２推定部４６での処理は、後述する。 The first estimation unit 44 estimates each error of the pitch rate and the roll rate detected by the IMU 26 using the equation of state f (x). The second estimation unit 46 estimates the errors of the front-back acceleration, the lateral acceleration, and the vertical acceleration detected by the IMU 26 using the equation of state f (x) different from that of the first estimation unit 44. The processing in the first estimation unit 44 and the second estimation unit 46 using the equation of state f (x) will be described later.

続いて、姿勢角推定部４０における処理について説明する。本実施の形態では、下記の式（１）により、車両２００の姿勢角の変化率である姿勢角の角速度を定義する。式（１）は、ピッチレートＰ_v、ロールレートＱ_v及びヨーレートＲ_vの３軸の角速度に係る方程式である。

Subsequently, the processing in the posture angle estimation unit 40 will be described. In the present embodiment, the angular velocity of the posture angle, which is the rate of change of the posture angle of the vehicle 200, is defined by the following equation (1). Equation (1) is an equation relating to the three-axis angular velocities of _{pitch rate P v} , roll rate Q _v, and yaw rate R _v.

さらに上記式（１）及び重力加速度ｇとに基づくと、ＩＭＵ２６による各軸の加速度は下記の式で表される。なお、下記の式（２）は、ロールレートＰ_v、ピッチレートＱ_v、ヨーレートＲ_v、前後速度Ｕ_v、横速度Ｖ_v及び上下速度Ｗ_vの各変数を含む、前後加速度Ａ_x、横加速度Ａ_y及び上下加速度Ａ_zの３軸の加速度に係る方程式である。上記の式（１）と下記の式（２）とを用いることにより、前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_z、ピッチレートＰ_v、ロールレートＱ_v及びヨーレートＲ_vに係る６自由度の車両の平面運動を記述できる。

Further, based on the above equation (1) and the gravitational acceleration g, the acceleration of each axis by the IMU 26 is expressed by the following equation. Incidentally, the following equation (2) is roll rate P _v, pitch rate Q _v, the yaw rate R _v, longitudinal velocity U _v, including the variables of the lateral velocity V _v and vertical velocity W _v, the longitudinal acceleration A _x, horizontal It is an equation concerning the acceleration of three axes of acceleration A _y and vertical acceleration A _z. By using the above equation (1) and the following equation (2), 6 relating _{to the longitudinal acceleration A x} , the lateral acceleration A _y , the vertical acceleration A _z , the pitch rate P _v , the roll rate Q _v and the yaw rate R _v. Can describe the plane motion of a vehicle with a degree of freedom.

上記の式（１）、（２）が成立するという条件の下、ＩＭＵ２６の出力のゼロ点誤差の推定を行う。ただし、車両２００が常に路面上を走行することを鑑みると、式（２）において、横速度Ｖ_v及び上下速度Ｗ_vの各々の長時間での平均値は０になると仮定できる。従って、横速度Ｖ_v及び上下速度Ｗ_v、並びに横速度Ｖ_vの微分値及び上下速度Ｗ_vの微分値を各々０とみなすと、上記の式（２）は、下記の式（３）のようになる。

Under the condition that the above equations (1) and (2) are satisfied, the zero point error of the output of the IMU 26 is estimated. However, considering that the vehicle 200 always travels on the road surface, it can be assumed that the average value of each of _{the lateral speed V v} and the vertical speed W _{v in the long time becomes 0 in the equation (2).} Therefore, assuming that the lateral velocity V _v and the vertical velocity W _v , and the differential value of the lateral velocity V _{v and} the differential value of the vertical velocity W _v are 0, respectively, the above equation (2) is the following equation (3). Will be.

また、車両姿勢角であるロール角φ_v及びピッチ角θ_vの各々が十分に小さいと仮定すると、sinφ_v≒φ_v、cosφ_v≒１、sinθ_v≒θ_v、cosθ_v≒１とみなすことが可能なので、式（１）、（３）は、下記の式（４）のように縮退される。

_{Assuming that each of the roll angle φ v} and the pitch angle θ _v , which are the vehicle attitude angles, is sufficiently small, it should be regarded as sin φ _v ≈ φ _v , cos _{φ v} ≈ 1, sin θ _v ≈ θ _v , cos θ _v ≈ 1. Therefore, the equations (1) and (3) are degenerated as in the following equation (4).

前述のように、車両２００の運動パターンであるヨーレートＲ_vは、ＧＰＳ補正部４２で補正されるので、補正済みの既知情報として扱える。また、ＩＭＵ２６による前後加速度Ａ_xの微分値、及び横加速度Ａ_yの微分値の各々はゼロ点誤差を含まず既知情報として扱えるので、上記の式（４）から、下記の式（５）、（６）、（７）が導かれる。式（５）、（６）、（７）において、左辺は既知な変数のみを含み、右辺は未知の変数を含む。

As described above, the yaw rate R _v is a motion pattern of the vehicle 200, since it is corrected by the GPS correction unit 42, treated as corrected known information. Further, since each of the differential value of the front-rear acceleration A _{x and} the differential value of the lateral acceleration A _y by the IMU 26 can be treated as known information without including the zero point error, the following equation (5), from the above equation (4), can be treated. (6) and (7) are derived. In equations (5), (6), and (7), the left side contains only known variables, and the right side contains unknown variables.

上記の式（５）、（６）、（７）において、未知の変数はロールレートＰ_v、ピッチレートＱ_v、前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zの５つである。３つの線形方程式に対して未知の変数が５つ存在するので、式（５）、（６）、（７）から一意に５つの変数を算出することはできない。従って、車両２００の走行条件によって推定する変数を変化させる。例えば、ヨーレートＲ_vが十分に小さい場合を想定し、Ｒ_v≒０とすると、上記の式（５）、（６）、（７）は、下記の式（８）、（９）のようになる。

In the above equations (5), (6), and (7), the five unknown variables are roll rate P _v , pitch rate Q _v , longitudinal acceleration A _x , lateral acceleration A _y , and vertical acceleration A _z . Since there are five unknown variables for the three linear equations, it is not possible to uniquely calculate the five variables from the equations (5), (6), and (7). Therefore, the variable to be estimated is changed according to the traveling conditions of the vehicle 200. For example, assuming that the yaw rate R _v is sufficiently small and R _v ≈ 0, the above equations (5), (6) and (7) are as shown in the following equations (8) and (9). Become.

式（８）、（９）では未知の変数は角速度であるロールレートＰ_v及びピッチレートＱ_vのみになるので、ロールレートＰ_v及びピッチレートＱ_vの各々を推定することができ、ロールレートＰ_ｓ及びピッチレートＱ_ｓの各々のゼロ点誤差を推定することが可能になる。角速度のゼロ点誤差の推定が完了すると、式（５）、（６）、（７）における未知の変数は加速度のみになるので、ヨーレートＲ_vが大きい走行区間を想定して、前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zの各々の値を算出する。 In equations (8) and (9), the only unknown variables are the angular velocity roll rate P _v and pitch rate Q _v , so each of roll rate P _v and pitch rate Q _v can be estimated, and the roll rate can be estimated. It becomes possible to estimate the zero point error of each of P _s and pitch rate Q _s. When the estimation of the zero-point error of the angular velocity is complete, equation (5), (6), since the unknown variables comprising only the acceleration in (7), the yaw rate R _v assumes a large travel section, longitudinal acceleration A _x , Lateral acceleration A _y , Vertical acceleration A _z are calculated.

以上の計算プロセスをまとめると下記のようになる。まず、ヨーレートＲ_v≒０の走行条件において、ロールレートＰ_v及びピッチレートＱ_vの各々を下記の式（１０）を用いて推定し、推定されたロールレートＰ_v及びピッチレートＱ_vと、ＩＭＵ２６が検出したピッチレートＰｓ、ロールレートＱｓとの差分を、ロールレートＰ_ｓ及びピッチレートＱ_ｓの各々のゼロ点誤差として推定する。式（１０）は、加速度センサであるＩＭＵ２６の出力の微分値と、補正済みの車両２００の前後速度Ｕ_vと、ゼロ点誤差を補正済みのヨーレートＲ_vとで表される。より具体的には、ロールレートＰ_vは、ＩＭＵ２６が検出した横加速度Ａ_yの微分値から補正済みの前後速度Ｕ_vとゼロ点誤差を補正済みのヨーレートＲ_vの微分値との積を減算して得た値と重力加速度ｇとの商で表されることを示す。また、ピッチレートＱ_vは、ＩＭＵ２６が検出した前後加速度Ａ_xの微分値から補正済みの前後速度Ｕ_vの２階微分値を減算して得た値と重力加速度ｇとの商で表される。

The above calculation process can be summarized as follows. First, the yaw rate R _v in the running condition of the ≒ 0, the roll rate P _v and the pitch rate Q _v each was estimated using the equation (10) below the roll rate estimated P _v and the pitch rate Q _v, pitch rate Ps of IMU26 detects a difference between the roll rate Qs, estimated as zero point error of each of the roll rate _{P s} and the pitch rate _{Q s.} Equation (10) is expressed by the differential value of the output of the IMU 26, which is an acceleration sensor, the corrected front-rear speed U _v of the vehicle 200, and the corrected yaw rate R _v . More specifically, the roll rate P _v is obtained by subtracting the product of the _{corrected front-rear velocity U v} and the corrected yaw rate R _v differential value from the differential value of _{the lateral acceleration A y} detected by the IMU 26. It is shown that it is expressed by the quotient of the value obtained in the above process and the gravitational acceleration g. Further, the pitch rate Q _v is expressed by the quotient of the value obtained by subtracting the second-order differential value of the corrected front-rear velocity U _v from the differential value of the front-back acceleration A _{x detected by the IMU 26 and the gravitational acceleration g.} ..

次いで、ヨーレートＲ_vが大きい区間で前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zの各々の値を下記の式（１１）を用いて算出し、推定された前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zと、ＩＭＵ２６が検出した前後加速度Ａ_ｓ、横加速度Ａ_ｓ、上下加速度Ａ_ｓとの差分を、前後加速度Ａ_ｓ、横加速度Ａ_ｓ、上下加速度Ａ_ｓの各々のゼロ点誤差として推定する。式（１０）は、上記の式（１０）等に基づいて補正されたロールレートＰ_v及びピッチレートＱ_vと、加速度センサであるＩＭＵ２６の出力の微分値と、補正済みの前後速度Ｕ_vと、ゼロ点誤差を補正済みのヨーレートＲ_vとで表される。

Next, in the _{section where the yaw rate R v} is large, the values of the longitudinal acceleration A _x , the lateral acceleration A _y , and the _{longitudinal acceleration A z} are calculated using the following equation (11), and the estimated longitudinal acceleration A _x and lateral acceleration are calculated. a _y, the vertical acceleration a _z and, IMU26 is detected longitudinal acceleration _{a s,} lateral acceleration _{a s,} the vertical acceleration of the difference between _{a s,} longitudinal acceleration _{a s,} lateral acceleration _{a s,} the zero of each vertical acceleration _{a s} Estimated as a point error. _{Equation (10) includes a roll rate P v} and a pitch rate Q _v corrected based on the above equation (10) and the like, a differential value of the output of the IMU 26 which is an acceleration sensor, and a corrected front-rear velocity U _v . , The zero point error is represented by the _{corrected yaw rate R v.}

上記の式（１１）は、より具体的には、前後加速度Ａ_xは、誤差を補正したロールレートＰ_vと重力加速度ｇとの積から、ＩＭＵ２６が検出した横加速度Ａ_yの微分値を減算し、誤差を補正したヨーレートＲ_vの微分値と補正済みの前後速度Ｕ_vとの積を加算し、誤差を補正したヨーレートＲ_vと補正済みの前後速度Ｕ_vの微分値との積を加算して得た値と誤差を補正したヨーレートＲ_vとの商で表されることを示している。また、上記の式（１１）は、横加速度Ａ_yは、誤差を補正したピッチレートＱ_vと重力加速度ｇとの積から、補正済みの前後加速度Ａ_xの微分値を減算し、補正済みの前後速度Ｕ_vの２階微分値を加算し、誤差を補正したヨーレートＲ_vの二乗と補正済みの前後速度Ｕ_vとの積を加算して得た値と誤差を補正したヨーレートＲ_vとの商で表されることを示している。また、上記の式（１１）は、上下加速度Ａ_zは、重力加速度ｇから誤差を補正したロールレートＲ_vと補正済みの前後速度Ｕ_vとの積を減算して得た値で表されることを示している。 In the above equation (11), more specifically, the longitudinal acceleration A _x is obtained by subtracting the differential value of _{the lateral acceleration A y} detected by the IMU 26 from the product of the roll rate P _{v corrected for the error and the gravity acceleration g.} and adds the product of the differential value of the yaw rate R _v obtained by correcting an error and the corrected longitudinal velocity U _v, adding the product of the differential value of the yaw rate R _v and corrected longitudinal velocity U _v corrected errors It is shown that it is expressed by the quotient of the value obtained in the above process and the yaw rate R _{v corrected for the error.} Further, in the above equation (11), the lateral acceleration A _y is corrected by subtracting the differential value of the corrected front-rear acceleration A _x from the product of the pitch rate Q _{v corrected for the error and the gravitational acceleration g.} adding second-order differential value of the longitudinal velocity U _v, the yaw rate R _v of the value and the error obtained by adding the product of the square and the corrected longitudinal velocity U _v of the yaw rate R _v obtained by correcting the error has been corrected Shows that it is represented by quotient. Further, in the above equation (11), the vertical acceleration _Az is expressed by a value obtained by subtracting the product of _{the roll rate R v} corrected for the error and the corrected front-rear speed U _{v from the gravitational acceleration g.} It is shown that.

式（１０）を用いた処理は第１推定部４４で、式（１１）を用いた処理は第２推定部で、各々実行される処理に相当する。
変形例では、状態量である変数ｘと状態方程式ｆ（ｘ）とに基づき、非線形方程式を扱うことが可能なアンセンティッドカルマンフィルタ（以下、「ＵＫＦ」と略記）を用いた演算により、上記のような方程式の縮退を行わずに、ＩＭＵ２６のゼロ点誤差の推定を行う。 The process using the equation (10) is the first estimation unit 44, and the process using the equation (11) is the second estimation unit, which correspond to the processes executed respectively.
In the modified example, based on the variable x which is a state quantity and the equation of state f (x), an operation using an uncentied Kalman filter (hereinafter abbreviated as “UKF”) capable of handling a nonlinear equation is performed as described above. The zero point error of the IMU26 is estimated without shrinking the equation.

以下に、変形例における、第１推定部４４で行われるＵＫＦを用いた角速度誤差推定について説明する。ＵＫＦを用いる場合も、式（１０）を用いる場合と同様に、第１推定部４４は、ヨーレートＲ_vが十分に小さい走行条件において、ＩＭＵ２６が検出したロールレートＰ_v及びピッチレートＱ_vのゼロ点誤差を推定する。ＵＫＦを用いた角速度誤差推定に係る状態変数ｘ_gを下記の式（１２）のように定義する。下記の式（１２）中のε₁はロールレートＰ_vのゼロ点誤差、ε₂はピッチレートＱ_vのゼロ点誤差である。

Hereinafter, the angular velocity error estimation using the UKF performed by the first estimation unit 44 in the modified example will be described. When using UKF, as in the case of using equation (10), the first estimation unit 44 has zero _{roll rate P v} and pitch rate Q _v detected by IMU 26 under running conditions where _{the yaw rate R v is sufficiently small.} Estimate the point error. _{The state variable x g} related to the estimation of the angular velocity error using UKF is defined by the following equation (12). In the following equation (12), ε ₁ is the zero point error of the roll rate P _v _{, and ε 2} is the zero point error of the pitch rate Q _v.

上記の状態量ｘ_gに対して、ｄｘ/ｄｔ＝ｆ_g（ｘ_g）の関係が成り立つ状態方程式ｆ_g（ｘ_g）を下記の式（１３）、（１４）、（１５）のように立式する。式（１４）、（１５）は、上述の式（１）に含まれる状態方程式である。

With respect to the state quantity _{x g, dx / dt = f} g (x g) relationship holds state equation f _g (x _g) the following equation (13), (14), as in (15) Stand up. Equations (14) and (15) are equations of state included in the above equation (1).

また、各状態量ｘ_gに対するシステムノイズＱ_gを下記のように定義する。本実施の形態では、Ｕ_vの微分、Ｐ_v、Ｑ_v、Ｒ_v、ε₁、ε₂については、白色ノイズによって駆動されるランダムウォークモデルとし、1次のマルコフモデルを採用してもよい。

Further, the system noise Q _g for each state quantity x _g is defined as follows. In the present embodiment, the U _v derivative, P _v , Q _v , R _v , ε ₁ , and ε ₂ are random walk models driven by white noise, and a first-order Markov model may be adopted. ..

次に観測量を下記のように観測行列として定義する。式（１６）の右辺のＵ_sは車速センサ２４が検出した車速の値であり、Ｐ_ｓ、Ｑ_s、Ｒ_sの各々は角速度センサとして機能するＩＭＵ２６が検出したロールレート、ピッチレート、ヨーレートの各々の値である。また、Ａ_x、Ａ_yの各々は加速度センサとして機能するＩＭＵ２６が検出した前後加速度、横加速度の各々の値である。

Next, the observable is defined as an observation matrix as shown below. A vehicle speed value detected by U _s is the vehicle speed sensor 24 of the right side of equation _{(16), P s, Q} s, R s of each roll rate detected is IMU26 functioning as an angular velocity sensor, the pitch rate, yaw rate Each value. Further, _{each of A x} and A _y is a value of each of the longitudinal acceleration and the lateral acceleration detected by the IMU 26 functioning as an acceleration sensor.

本実施の形態では、ＩＭＵ２６が検出したヨーレートＲ_sはＧＰＳ等によってゼロ点誤差が補正可能なので、ロールレートＰ_ｓとピッチレートＱ_sとにゼロ点誤差が存在すると仮定する。従って、観測値であるロールレートＰ_ｓとピッチレートＱ_sとに対する観測方程式ｈ_g（ｘ_g）は、下記の式（１７）、（１８）のようになる。前述のように、ε₁はロールレートＰ_ｓのゼロ点誤差、ε₂はピッチレートＱ_sのゼロ点誤差である。

In the present embodiment, since the yaw rate R _s detected by the IMU 26 can correct the zero point error by GPS or the like, _{it is assumed that the roll rate P s} and the pitch rate Q _s have a zero point error. Therefore, the observation _{equations h g} (x _g ) for the observed _{roll rate P s} and the pitch rate Q _s are as shown in the following equations (17) and (18). As described above, ε ₁ is the zero point error of the roll rate P _s _{, and ε 2} is the zero point error of the pitch rate Q _s.

ただし、各観測量に対する観測ノイズＲ_gを下記のように定義する。

However, the observed noise R _g for each observed amount is defined as follows.

ＵＫＦでは、式（１３）、（１４）、（１５）で示した状態方程式ｆ_g（ｘ_g）の各々を離散化して使用する。従って、関数ｆ_g（ｘ_g）の入出力関係を、時間ｔ＝ｋ、ｋ−１に対して、ｘ_g（ｋ）＝ｆ_g（ｘ_g（ｋ−１））となる形で使用する。 _{In UKF, each of the equations of state f g} (x _g ) shown in the equations (13), (14), and (15) is discretized and used. Therefore, the input / output relationship of the function f _g (x _g ) is used in such a form that _{x g} (k) = f _g (x _g (k-1)) with respect to time t = k and k-1. ..

図５は、本実施の形態に係る演算装置１４の第１推定部４４の機能ブロック図の一例である。図５に示したように、第１推定部４４は、状態方程式ｆ_g（ｘ_g）と観測方程式ｈ_g（ｘ_g）とを用いて、ＩＭＵ２６の検出値を推定する事前推定部１０２と、事前推定部１０２が出力した事前推定値を、ＩＭＵ２６の検出値を用いて補正するフィルタリング部１０４と、を含む。 FIG. 5 is an example of a functional block diagram of the first estimation unit 44 of the arithmetic unit 14 according to the present embodiment. As shown in FIG. 5, the first estimation unit 44 includes a pre-estimation unit 102 that estimates the detected value of the IMU 26 using the _{equation of state f g} (x _g ) and the observation equation h _g (x _g). It includes a filtering unit 104 that corrects the pre-estimated value output by the pre-estimating unit 102 by using the detected value of the IMU 26.

カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆ_g（ｘ_g）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆ_g（ｘ_g）についての線形化を要さず、かつ計算負荷が比較的小さいＵＫＦを一例として採用する。状態方程式ｆ_g（ｘ_g）が線形の場合は、線形カルマンフィルタを用いてもよいし、ＵＫＦ以外の非線形カルマンフィルタを用いてもよい。 Various methods including linear and non-linear Kalman filters have been proposed. In this embodiment, an example of utilizing a non-linear Kalman filter is shown based on the fact that the _{equation of state f g} (x _{g) is non-linear as described above.} Among them, UKF, which does not require linearization of the equation of state f _g (x _g ) and has a relatively small computational load, is adopted as an example. When the equation of state f _g (x _g ) is linear, a linear Kalman filter may be used, or a non-linear Kalman filter other than UKF may be used.

図６は、事前推定部１０２の機能ブロック図の一例である。図６に示したように、事前推定部１０２は、第１アンセンティッド変換部１０２Ａと第２アンセンティッド変換部１０２Ｂとを含む。 FIG. 6 is an example of a functional block diagram of the pre-estimation unit 102. As shown in FIG. 6, the pre-estimation unit 102 includes a first unscented conversion unit 102A and a second unscented conversion unit 102B.

第１アンセンティッド変換部１０２Ａは、状態方程式ｆ_g（ｘ_g）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘ_gの平均及びｘ_gの共分散行列を出力する。 First Ann Sentiddo converting unit 102A performs state equation f _{_g} (x _g) 1 st Ann Sentiddo transformation to update the state quantity based on the (Unscented transfer), the average and x _g of the covariance matrix of x _g Is output.

第２アンセンティッド変換部１０２Ｂは、第１アンセンティッド変換部１０２Ａが出力した状態量ｘ_gの平均及び状態量ｘ_gの共分散行列を用いて、観測方程式ｈ_g（ｘ_g）に従って、対応する観測量ｙ_gに変換する２回目のアンセンティッド変換を行う。 The second uncented conversion unit 102B corresponds according to the observation equation h _g (x _g ) using the average of the state quantity x _{g and} _{the covariance matrix of the state quantity x g} output by the first uncentied conversion unit 102A. Perform the second unscented conversion to convert to the observed quantity y _g.

なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び共分散行列を精度よく求めることにある。

The purpose of the unscented transformation is to accurately obtain the average and covariance matrix of the following observed observables y in the transformation by a certain nonlinear function y = f (x).

従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。 Therefore, the present embodiment is characterized in that the probability density function is approximated by using 2n + 1 samples (sigma points) corresponding to the mean value and the standard deviation.

図６に示したように、第１アンセンティッド変換部１０２Ａは、シグマポイント重み係数部１０２Ａ１と、関数変換部１０２Ａ２と、Ｕ変換部１０２Ａ３と、を含む。また、第２アンセンティッド変換部１０２Ｂは、シグマポイント重み係数部１０２Ｂ１と、関数変換部１０２Ｂ２と、Ｕ変換部１０２Ｂ３と、を含む。 As shown in FIG. 6, the first unscented conversion unit 102A includes a sigma point weighting coefficient unit 102A1, a function conversion unit 102A2, and a U conversion unit 102A3. Further, the second unsented conversion unit 102B includes a sigma point weighting coefficient unit 102B1, a function conversion unit 102B2, and a U conversion unit 102B3.

第１アンセンティッド変換部１０２Ａのシグマポイント重み係数部１０２Ａ１及び第２アンセンティッド変換部１０２Ｂのシグマポイント重み係数部１０２Ｂ１では、シグマポイントＸ_i：ｉ＝０、１、２、…２ｎが、下記のように選択される。

In the sigma point weighting coefficient unit 102A1 of the first unscented conversion unit 102A and the sigma point weighting coefficient unit 102B1 of the second unscented conversion unit 102B, the sigma points X _i : i = 0, 1, 2, ... 2n are as follows. Is selected.

ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。

However, the scaling factor κ is selected so that κ ≧ 0. The weight for the sigma point is defined as follows.

第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２での状態方程式ｆ_g（ｘ）による変換だが、第２アンセンティッド変換部１０２Ｂの関数変換部１０２Ｂ２での観測方程式ｈ_g（ｘ）を用いた変換では、観測値が得られる。

The conversion of each sigma point by the nonlinear function f (x) in the function conversion unit 102A2 of the first unscented conversion unit 102A is as follows. _{The following is the conversion by the equation of state f g} (x) in the function conversion unit 102A2 of the first uncentid conversion unit 102A _{, but the observation equation h g} (x) in the function conversion unit 102B2 of the second uncentied conversion unit 102B is The transformation used gives observations.

第１アンセンティッド変換部１０２ＡのＵ変換部１０２Ａ３では、関数ｆ_g（ｘ_g）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘ_gの平均値と状態量ｘ_gの共分散行列を算出する。なお、下記式中のＱ_gはシステムノイズである。

In the U conversion unit 102A3 of the first uncentied conversion unit 102A, the average value and the state _{of the state quantity x g} are used as shown below by using the value converted by _{the function f g} (x _{g) and the above-mentioned weighting coefficient.} Calculate the covariance matrix of the quantity x _g. In addition, Q _g in the following equation is system noise.

第２アンセンティッド変換部１０２ＢのＵ変換部１０２Ｂ３では、下記の計算を行う。

The U conversion unit 102B3 of the second unscented conversion unit 102B performs the following calculation.

図７は、フィルタリング部１０４の機能ブロック図の一例である。フィルタリング部１０４では、Ｕ変換部１０２Ｂ３で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。 FIG. 7 is an example of a functional block diagram of the filtering unit 104. The filtering unit 104 compares the difference between the observed value corresponding to the pre-predicted value of the state quantity calculated by the U conversion unit 102B3 and the actually observed observed value, and corrects the predicted value of the state quantity. I do.

状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲ_gは観測ノイズである。

The process of feeding back the predicted value of the state quantity with the actually observed value is called Kalman Gain, and it is calculated by the following equation. In addition, R _g of the following equation is an observation noise.

次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。

Next, using this Kalman gain, the process of correcting the pre-predicted value of the state quantity is performed as follows.

以上の事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、ロールレートＰ_v及びピッチレートＱ_vの推定値を算出することにより、ＩＭＵ２６が検出したロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を推定することができる。 By repeating the above processing of the pre-estimation unit 102 and the filtering unit 104 for each time step, _{the estimated values of the roll rate P v} and the pitch rate Q _v are calculated, and the roll rate P _s detected by the IMU 26 is zero. The point error ε ₁ and the zero point error ε ₂ of the pitch rate Q _s can be estimated.

ＩＭＵ２６が検出したロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を推定する場合、フィルタリング部１０４が出力したｘ_g（ｋ）、Ｐ_xg（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値を用いて補正される。フィルタリング部１０４が出力したｘ_g（ｋ）、Ｐ_xg（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ_g（ｋ）、Ｐ_xg（ｋ）を前回値として事前推定部１０２に入力する。 If IMU26 to estimate the zero point error epsilon ₂ of the zero point error epsilon ₁ and the pitch rate Q _s of the roll rate _{P s} detected, x _g of the filtering unit 104 is output (k), the previous value of P _xg (k) Is input to the pre-estimation unit 102, and the two-step unscented conversion is performed as described above to calculate the pre-predicted value of the state quantity and the corresponding observed value. The calculated predicted value of the state quantity is corrected by using the Kalman gain and the latest observed value actually observed by the filtering unit 104. _{The x g} (k) and P _xg (k) output by the filtering unit 104 are temporarily held in the storage device 18, and the x _g (k) and P _xg (k) held in the storage device 18 are set as the previous values. Input to the pre-estimation unit 102.

第１推定部４４は、ヨーレートＲ_vが十分に小さい走行条件において、この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、ＩＭＵ２６が検出したロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を推定する。ＵＫＦを用いることにより、非線形の方程式を近似によって縮退することを要しないので、ロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を精度よく推定できる。 The first estimation unit 44 repeats the calculation of the pre-predicted value of this state quantity and the correction with the latest actually observed value under the driving condition where _{the yaw rate Rv is sufficiently small, and the roll rate detected by the IMU 26.} estimating the zero error epsilon ₂ of the zero point error epsilon ₁ and the pitch rate Q _s of P _s. By using UKF, since not necessary to degenerate by approximation equations nonlinear, the zero point error epsilon ₂ of the roll rate P zero error epsilon ₁ and pitch rate _s Q _s can be accurately estimated.

続いて、第２推定部４６におけるＵＫＦを用いた加速度誤差推定について説明する。ＵＫＦを用いる場合も、式(１１)を用いる場合と同様に、第２推定部４６は、車両２００が旋回中等のヨーレートＲ_vが比較的大きい走行条件において、ＩＭＵ２６が検出した前後加速度Ａ_x及び横加速度Ａ_yのゼロ点誤差を推定する。 Subsequently, the acceleration error estimation using UKF in the second estimation unit 46 will be described. In the case of using the UKF as well as the case of using the equation (11), the second estimation unit 46 has _{the front-rear acceleration A x} and the front-rear acceleration A x detected by the IMU 26 under the traveling condition where _{the yaw rate R v is relatively large such as when the vehicle 200 is turning.} Estimate the zero point error of the lateral acceleration A _y.

ＵＫＦを用いた角速度誤差推定に係る状態変数ｘ_aを下記の式（１９）のように定義する。下記の式（１９）中のε₃は前後加速度Ａ_xのゼロ点誤差、ε₄は横加速度Ａ_yのゼロ点誤差、ε₅は上下加速度Ａ_zのゼロ点誤差である。

_{The state variable x a} related to the estimation of the angular velocity error using UKF is defined by the following equation (19). In the following equation (19), ε ₃ is the zero point error of the longitudinal acceleration A _x _{, ε 4} is the zero point error of the lateral acceleration A _y , and ε ₅ is the zero point error of the vertical acceleration A _z.

上記の状態量ｘ_aに対して、ｄｘ/ｄｔ＝ｆ_a（ｘ_a）の関係が成り立つ状態方程式ｆ_a（ｘ_a）を下記の式（２０）、（２１）、（２２）のように立式する。式（２１）、（２２）は、上述の式（１）に含まれる状態方程式である。

With respect to the state quantity _{x a, dx / dt = f} a (x a) relationship holds state equation f _a (x _a) to the following equation (20), (21), as in (22) Stand up. Equations (21) and (22) are equations of state included in the above equation (1).

また、各状態量ｘ_aに対するシステムノイズＱ_aを下記のように定義する。本実施の形態では、Ｕ_vの微分、Ｐ_v、Ｑ_v、Ｒ_v、ε₃、ε_4、ε₅については、白色ノイズによって駆動されるランダムウォークモデルとし、1次のマルコフモデルを採用してもよい。

Further, the system noise Q _a for each state quantity x _a is defined as follows. In this embodiment, the derivative of _{U v} _{, P v} , Q _v , R _v , ε ₃ , ε _{4, and} ε ₅ are random walk models driven by white noise, and a first-order Markov model is adopted. You may.

次に観測量を下記のように観測行列として定義する。式（２３）の右辺のＵ_sは車速センサ２４が検出した車速の値であり、Ｐ_ｓ、Ｑ_s、Ｒ_sの各々は角速度センサとして機能するＩＭＵ２６が検出したロールレート、ピッチレート、ヨーレートの各々の値である。また、Ａ_x、Ａ_yの各々は加速度センサとして機能するＩＭＵ２６が検出した前後加速度、横加速度の各々の値である。

Next, the observable is defined as an observation matrix as shown below. A vehicle speed value detected by U _s is the vehicle speed sensor 24 of the right side of equation _{(23), P s, Q} s, R s of each roll rate detected is IMU26 functioning as an angular velocity sensor, the pitch rate, yaw rate Each value. Further, _{each of A x} and A _y is a value of each of the longitudinal acceleration and the lateral acceleration detected by the IMU 26 functioning as an acceleration sensor.

観測値である前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zに対する観測方程式ｈ_a（ｘ_a）は、下記の式（２４）、（２５）、（２６）のようになる。前述のように、ε₃は前後加速度Ａ_xのゼロ点誤差、ε₄は横加速度Ａ_yのゼロ点誤差、ε₅は上下加速度Ａ_zのゼロ点誤差である。

The observation equations h _a (x _a ) for the observed values of longitudinal acceleration A _x , lateral acceleration A _y , and vertical acceleration A _z are as shown in the following equations (24), (25), and (26). As described above, ε ₃ is the zero point error of the longitudinal acceleration A _x _{, ε 4} is the zero point error of the lateral acceleration A _y , and ε ₅ is the zero point error of the vertical acceleration A _z.

ただし、各観測量に対する観測ノイズＲ_aを下記のように定義する。

However, the observed noise R _a for each observed amount is defined as follows.

以上の状態方程式である式（２０）、（２１）、（２２）、観測方程式である式（２４）、（２５）、（２６）を用いて、上述の角速度誤差推定の場合と同様にＵＫＦにより加速度誤差推定を行う。
第２推定部４６は、ヨーレートＲ_vが比較的大きい走行条件において、この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、ＩＭＵ２６が検出した前後加速度Ａ_xのゼロ点誤差ε_３、横加速度Ａ_yのゼロ点誤差ε_４、上下加速度Ａ_zのゼロ点誤差ε_５を推定する。ＵＫＦによる演算は、上述の角速度誤差推定の場合と同様なので、詳細な説明は省略する。ＵＫＦを用いることにより、非線形の方程式を近似によって縮退することを要しないので、角速度の誤差及び加速度の誤差を精度よく推定できる。 Using the equations (20), (21), (22), which are the above equations of state, and the equations (24), (25), and (26), which are the observation equations, the UKF is the same as in the case of the above-mentioned angular velocity error estimation. Acceleration error is estimated by.
Second estimation unit 46, a relatively large driving conditions yaw rate R _v is, by repeating the correction in the actually observed latest value and the calculation of the pre-predicted value of the state quantity, longitudinal IMU26 detects acceleration zero error epsilon ₃ of a _{_x,} lateral acceleration a _y zero point error epsilon _{4 of} estimating the zero error epsilon ₅ of the vertical acceleration a _z. Since the calculation by UKF is the same as the case of the above-mentioned angular velocity error estimation, detailed description thereof will be omitted. By using UKF, it is not necessary to degenerate the nonlinear equation by approximation, so that the error of the angular velocity and the error of the acceleration can be estimated accurately.

以上説明したように、本実施の形態に係るセンサ誤差補正装置によれば、ロールレートＰ_ｓのゼロ点誤差ε₁、ピッチレートＱ_sのゼロ点誤差ε₂、前後加速度Ａ_xのゼロ点誤差ε₃、横加速度Ａ_yのゼロ点誤差ε₄及び上下加速度Ａ_zのゼロ点誤差ε₅を精度よく推定できる。ゼロ点誤差を補正したロールレートＰ_v、ピッチレートＱ_v、ヨーレートＲ_v、前後加速度Ａ_x、横加速度Ａ_y及び上下加速度Ａ_zを用いることにより、車両２００の位置推定の精度を向上させることが可能となる。 As described above, according to the sensor error correction apparatus according to this embodiment, the zero point error epsilon ₁ of the roll rate P _s, the zero point error epsilon ₂ of the pitch rate Q _s, the zero point error of the longitudinal acceleration A _x epsilon _3, the zero point error epsilon ₅ of the lateral acceleration a zero point error of _y epsilon ₄ and vertical acceleration a _z can be accurately estimated. To improve the accuracy of position estimation of the vehicle 200 by using _{the roll rate P v} , pitch rate Q _v , yaw rate R _v , longitudinal acceleration A _x , lateral acceleration A _y, and vertical acceleration A _z corrected for zero point error. Is possible.

また、本実施の形態に係るセンサ誤差補正装置１０は、非線形モデルであるＵＫＦを用いるので、ＩＭＵ２６が検出した値の直接微分又は近似を用いることを要しない。従って、本実施の形態に係るセンサ誤差補正装置１０は、ＩＭＵ２６が検出した角速度及び加速度のゼロ点誤差を高精度で推定することができる。 Further, since the sensor error correction device 10 according to the present embodiment uses the UKF which is a nonlinear model, it is not necessary to use the direct differentiation or approximation of the value detected by the IMU 26. Therefore, the sensor error correction device 10 according to the present embodiment can estimate the zero point error of the angular velocity and the acceleration detected by the IMU 26 with high accuracy.

１０センサ誤差補正装置
１２入力装置
１４演算装置
１６表示装置
１８記憶装置
２０位置計測装置
２４車速センサ
４０姿勢角推定部
４２ＧＰＳ補正部
４４第１推定部
４６第２推定部
１０２事前推定部
１０４フィルタリング部
２００車両 10 Sensor error correction device 12 Input device 14 Arithmetic device 16 Display device 18 Storage device 20 Position measurement device 24 Vehicle speed sensor 40 Posture angle estimation unit 42 GPS correction unit 44 First estimation unit 46 Second estimation unit 102 Pre-estimation unit 104 Filtering unit 200 vehicles

Claims

An inertial measurement unit that can detect the three-axis angular velocities of pitch rate, roll rate, and yaw rate, which indicate the behavior of the vehicle during running, and the three-axis accelerations of longitudinal acceleration, lateral acceleration, and vertical acceleration.
Based on the equation of motion related to the rate of change of the attitude angle using the angular velocity of the three axes and the equation of motion related to the acceleration of the three axes using the angular velocity of the three axes, the inertial measurement is performed according to the motion pattern of the vehicle. An error estimation unit that estimates the errors of each of the pitch rate, roll rate, front-back acceleration, lateral acceleration, and vertical acceleration detected by the device, and
Sensor error correction device including.

The motion pattern of the vehicle is the yaw rate of the vehicle.
The error estimation unit estimates each error of the pitch rate and the roll rate detected by the inertial measurement unit when the yaw rate of the vehicle is small, and the inertial measurement unit detects when the yaw rate of the vehicle is large. The sensor error correction device according to claim 1, wherein each error of the front-back acceleration, the lateral acceleration, and the vertical acceleration is estimated.

A vehicle speed detector that detects the front-rear speed and
A correction unit that corrects the error in the front-rear speed detected by the vehicle speed detection unit and the error in the yaw rate detected by the inertial measurement unit based on external information.
When the yaw rate of the vehicle is small, the error estimation unit corrects the roll rate and pitch rate of the vehicle with the differential values of the front-rear acceleration and the lateral acceleration detected by the inertial measurement unit and the correction unit. The sensor error correction device according to claim 2, wherein the error of each of the pitch rate and the roll rate detected by the inertial measurement unit is estimated based on the front-back speed and the yaw rate.

A vehicle speed detector that detects the front-rear speed and
A correction unit that corrects the error in the front-rear speed detected by the vehicle speed detection unit and the error in the yaw rate detected by the inertial measurement unit based on external information.
In the error estimation unit, when the yaw rate of the vehicle is large, the longitudinal acceleration, the lateral acceleration and the longitudinal acceleration are the roll rate and the pitch rate of the vehicle, and the longitudinal acceleration and the lateral acceleration detected by the inertial measurement unit, respectively. 2. 3. The sensor error correction device according to 3.

When the yaw rate of the vehicle is small, the error estimation unit determines that the roll rate of the vehicle is the front-rear speed corrected by the correction unit from the differential value of the lateral acceleration detected by the inertial measurement unit and the corrected yaw rate. The sensor error according to claim 3 or 4, wherein the error of the roll rate detected by the inertial measurement unit is estimated based on the quotient of the value obtained by subtracting the product of the derivative value and the gravitational acceleration. Compensator.

When the yaw rate of the vehicle is small, the error estimation unit determines that the pitch rate of the vehicle is the second-order differential value of the front-rear speed corrected by the correction unit from the differential value of the front-rear acceleration detected by the inertial measurement unit. The sensor error correction device according to claim 3 or 4, which estimates an error in the pitch rate detected by the inertial measurement unit based on the quotient of the value obtained by subtraction and the gravitational acceleration.

When the yaw rate of the vehicle is large, the error estimation unit subtracts the differential value of the lateral acceleration detected by the inertial measurement device from the product of the roll rate and the gravity acceleration, and corrects the front-rear acceleration by the correction unit. The value obtained by adding the product of the differential value of the adjusted yaw rate and the corrected front-rear speed and the product of the corrected yaw rate and the corrected front-rear speed and the corrected value. The error of the longitudinal acceleration detected by the inertial measuring device is estimated based on the quotient with the yaw rate, and the lateral acceleration is the longitudinal acceleration detected by the inertial measuring device from the product of the pitch rate and the gravity acceleration. The value obtained by subtracting the differential value, adding the second-order differential value of the corrected front-rear speed, adding the product of the square of the corrected yaw rate and the corrected front-rear speed, and the corrected value. The error of the lateral acceleration detected by the inertial measuring device is estimated based on the quotient with the yaw rate, and the vertical acceleration is obtained by subtracting the product of the roll rate and the corrected front-rear velocity from the gravity acceleration. The sensor error correction device according to claim 4, wherein the error of the vertical acceleration detected by the inertial measuring device is estimated based on the value represented by the above value.

The error estimation unit is
When the yaw rate of the vehicle is small, the predicted value of the state quantity including the roll rate error and the pitch rate error detected by the inertial measurement unit is calculated, and the roll rate error and the pitch rate error are assumed. With the first pre-estimation unit that calculates the predicted value of the observed value detected by the inertial measurement unit from the predicted value of the state quantity using the observation equation for the observed value of the roll rate and the pitch rate detected by the inertial measurement unit. ,
When the yaw rate of the vehicle is small, the first advance is based on the difference between the observation value detected and output by the inertial measurement unit and the predicted value of the observation value calculated by the first advance estimation unit. The first state estimation unit that corrects the predicted value of the state quantity calculated by the estimation unit, and
When the yaw rate of the vehicle is large, the predicted value of the state quantity including the error of the longitudinal acceleration, the error of the lateral acceleration, and the error of the longitudinal acceleration detected by the inertial measurement unit is calculated, and the error of the longitudinal acceleration and the lateral acceleration are calculated. Using the observation equations for the observed values of the longitudinal acceleration, lateral acceleration, and longitudinal acceleration detected by the inertial measurement unit, assuming the error and the error of the vertical acceleration, the inertial measurement unit can be used from the predicted value of the state quantity. The second pre-estimation unit that calculates the predicted value of the detected observed value, and
When the yaw rate of the vehicle is large, the second advance is based on the difference between the observation value detected and output by the inertial measurement unit and the predicted value of the observation value calculated by the second advance estimation unit. A second state estimation unit that corrects the predicted value of the state quantity calculated by the estimation unit, and
The sensor error correction device according to claim 1.