JP2008077349A - Vehicle status quantity estimating apparatus and vehicle steering control apparatus using the same - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a vehicle status quantity estimating apparatus for improving estimation accuracy of status quantity indicating an operation state of a vehicle. <P>SOLUTION: In the vehicle status quantity estimating apparatus having a sensor such as a GPS receiver 11 etc. for observing a predetermined status quantity indicating an operation state of a vehicle, and a Kalman filter 200 for estimating a status quantity indicating the operation state of the vehicle including the predetermined status quantity after inputting a predetermined status quantity observed by these sensors into a model of the operation state of the vehicle, a predetermined status quantity capable of being observed by each sensor is estimated by inputting the estimated status quantity into a model of each sensor, and a reflection degree of the predetermined status quantity in the Kalman filter 200 is set based on deviation between the estimated predetermined status quantity and a predetermined status quantity actually observed. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

  The present invention relates to a vehicle state quantity estimation device that estimates a state quantity that represents a motion state of a traveling vehicle. More specifically, the present invention relates to an actual vehicle state quantity by modeling a vehicle motion state and calculating the state quantity. The present invention relates to a vehicle state quantity estimation device. The present invention also relates to a vehicle steering control device using the vehicle state quantity estimation device.

  Conventionally, a vehicle operation system for operating a vehicle on a predetermined road by automatic steering control has been proposed. In this vehicle operation system, markers (magnetic markers) are installed at predetermined intervals along the vehicle traveling trajectory on the road surface, and a marker output from a marker sensor mounted on the vehicle each time the vehicle passes through each marker and the vehicle The lateral displacement of the vehicle from the vehicle running track is detected based on the detection signal reflecting the relative positional relationship between the vehicle and the vehicle. Steering control is performed so that the vehicle does not deviate from the vehicle travel path based on the lateral displacement detected each time the marker passes.

  In order to realize more accurate steering control in such a vehicle operation system, conventionally, a method for estimating a state quantity representing a state of a yaw motion and a lateral motion of a vehicle necessary for the steering control using a Kalman filter has been proposed. (For example, refer to Patent Document 1). In the prior art described in Patent Document 1, four state variables defined as state variables representing yaw motion and lateral motion of a vehicle using observed lateral displacement values obtained each time the vehicle passes over the marker ( Estimated values (yaw rate, yaw angle, lateral displacement speed, lateral displacement) are calculated by a Kalman filter.

The prior art described in Patent Document 1 includes a distance axis Kalman filter that obtains a lateral displacement and a yaw rate as observation values each time it passes through a magnetic marker, and calculates an estimated value of the state quantity of the vehicle using the observation values. A time axis Kalman filter that obtains a yaw rate as an observed value every predetermined period and uses an estimated value calculated last time and calculates an estimated value of a state quantity of the vehicle using the observed value. Every time an estimated value is obtained by the Kalman filter, an estimated value of the state quantity of the vehicle is calculated using the estimated value obtained by the distance axis Kalman filter instead of the previously calculated estimated value. In other words, the estimation value obtained by the distance axis Kalman filter at each magnetic marker is reflected in the estimation by the time axis Kalman filter, thereby improving the estimation accuracy of the vehicle state quantity even between magnetic markers where lateral displacement cannot be observed. We are going to plan.
JP 2001-34341 A

  However, in the conventional technique described in Patent Document 1 described above, the estimated value obtained by the distance axis Kalman filter when passing through the immediately preceding magnetic marker until the next magnetic marker is passed is estimated by the time axis Kalman filter. Therefore, when there is a large lateral movement of the vehicle between the magnetic markers, the estimation error of the state quantity between the magnetic markers tends to increase. That is, since lateral displacement cannot be observed between magnetic markers, errors due to construction of actual road shapes, yaw rate drift, vehicle modeling, etc. are likely to accumulate. When the period reflected in the Kalman filter becomes long (for example, when traveling at a very low speed in a curve with a large curvature), the estimation error due to the time-axis Kalman filter between the magnetic markers tends to increase.

  Accordingly, a first object of the present invention is to provide a vehicle state quantity estimating device that improves the estimation accuracy of a state quantity that represents the motion state of a vehicle. The second object of the present invention is to provide a vehicle steering control device using such a vehicle state quantity estimation device.

In order to achieve the first object, a vehicle state quantity estimating device according to the present invention includes:
Observing means for observing a predetermined amount of state representing the motion state of the vehicle;
A vehicle state quantity estimation device comprising state quantity estimation means for inputting a predetermined state quantity observed by the observation means to a model of a vehicle motion state and estimating a state quantity representing the motion state of the vehicle,
An observation amount estimation unit that inputs a state amount estimated by the state amount estimation unit to a model of the observation unit and estimates a predetermined state amount that can be observed by the observation unit;
The degree of reflection of the vehicle state of motion of the predetermined state quantity observed by the observation means is a deviation between the predetermined state quantity observed by the observation means and the predetermined state quantity estimated by the observation amount estimation means. It is set based on this.

  Thereby, since the degree of reflection of the predetermined state quantity observed by the observation means that is input to the model of the vehicle motion state can be changed by the deviation, the predetermined state quantity observed by the observation means is temporarily Even if it greatly changes, it is possible to improve the estimation accuracy of the state quantity representing the motion state of the vehicle. For example, when the deviation exceeds a predetermined reference value, a certain level of divergence has occurred between the predetermined state quantity observed by the observation means and the predetermined state quantity estimated by the observation amount estimation means. As described above, the degree of reflection of the input to the motion state model of the predetermined state quantity observed by the observation means can be set small to reduce the influence on estimation of the state quantity representing the motion state of the vehicle. As a result, it is possible to improve the estimation accuracy of the state quantity representing the motion state of the vehicle even if the predetermined state quantity observed by the observation means changes greatly.

  Further, in the vehicle state quantity estimation apparatus according to the present invention, the reflection degree of the predetermined state quantity whose deviation exceeds a predetermined value among the predetermined state quantities observed by the observation unit is smaller than the previous reflection degree. It may be set. That is, when the deviation exceeds a predetermined value, a state quantity that represents the motion state of the vehicle by setting the degree of reflection of the input of the predetermined state quantity exceeding the predetermined value to the motion state model is smaller than the previous time. It is possible to reduce the influence on the estimation.

  The reflection degree may be set to be smaller for a predetermined state quantity having a larger degree of deviation among the predetermined state quantities observed by the observation means. Thereby, the influence which it has on the estimation of the state quantity showing the motion state of the vehicle can be reduced as the predetermined state quantity has a larger degree of deviation than the other predetermined state quantities among the plurality of predetermined state quantities. In addition, since the comparison between a plurality of predetermined state quantities is performed according to the degree of the deviation, it is possible to compare even if the plurality of predetermined state quantities are different physical quantities.

  Here, in the vehicle state quantity estimation device according to the present invention, the state quantity estimation means inputs the predetermined state quantity observed for the N-1th time by the observation means to the model of the vehicle motion state and moves the vehicle. A state quantity representing a state is estimated, and the observation quantity estimation means can input the state quantity estimated by the state quantity estimation means to a model of the observation means and can be observed for the Nth time by the observation means. A predetermined state quantity is estimated, and the deviation is a difference between the predetermined state quantity observed at the Nth time by the observation means and the predetermined state quantity observable at the Nth time estimated by the observation quantity estimation means. (N is an integer of 2 or more).

  Moreover, in the vehicle state quantity estimation device according to the present invention, the reflection degree is further set based on an observation state determined by the observation means based on its own observation result, and the observation state has a large observation error. It is preferable that the reflection degree is set to be smaller as the value becomes larger. As a result, if the observation state determined based on the observation result of the observation means itself is in a state where the observation error is large, the degree of reflection to the motion state model can be suppressed. It is possible to estimate a state quantity that is more robust against a decrease in accuracy.

  The reflection degree is further set based on an observation environment when a predetermined state quantity is observed by the observation means, and the reflection degree is set to be smaller as the observation environment is an environment having a larger observation error. And preferred. As a result, if the observation environment that can be objectively grasped is an environment with a large observation error, the degree of reflection in the motion state model can be suppressed. Furthermore, robust state quantity estimation becomes possible.

  Further, the reflection degree is preferably set based on an elapsed time from the observation time of the predetermined state quantity by the observation means, and the reflection degree is preferably set to be smaller as the elapsed time becomes longer. As a result, the accuracy of the observation data, which decreases with the passage of time from the observation time, can be reflected in the vehicle state model, so that it is more robust against the deterioration of the observation accuracy of the observation means with the passage of time from the observation time. State quantity estimation is possible. For example, even if actual road construction errors and vehicle model errors accumulate from the time of observation, robust state quantity estimation is possible.

  Further, the reflection degree may be further set based on a travel distance from the observation time of the predetermined state quantity by the observation means, and the reflection degree may be set to be smaller as the travel distance becomes longer. As a result, the accuracy of the observation data, which decreases due to the distance traveled from the observation time, can be reflected in the vehicle state model, which further reduces the observation accuracy of the observation means due to the increase in travel distance from the observation time. Robust state quantity estimation is possible. For example, even if actual road construction errors and vehicle model errors accumulate from the time of observation, robust state quantity estimation is possible.

  Here, in the vehicle state quantity estimation device according to the present invention, the state quantity estimation means and the observation quantity estimation means are Kalman filters, and a term of a standard deviation of an observation error of a predetermined state quantity observed by the observation means. Is added to the observation equation of the Kalman filter, and the standard deviation is preferably increased as the reflection degree is set smaller.

In order to achieve the second object, a vehicle steering control device according to the present invention includes:
A vehicle state quantity estimating device according to the present invention;
Control signal generating means for generating a control signal for the steering actuator in the steering system based on the state quantity representing the motion state of the vehicle estimated by the vehicle state quantity estimating device.

  ADVANTAGE OF THE INVENTION According to this invention, the vehicle state quantity estimation apparatus which improves the estimation precision of the state quantity showing the movement state of a vehicle is realizable. In addition, a vehicle steering control device using such a vehicle state quantity estimation device can be realized.

  The best mode for carrying out the present invention will be described below with reference to the drawings. A vehicle state quantity estimation device according to an embodiment of the present invention uses a Kalman filter to estimate a state quantity of a vehicle, in particular, a state quantity representing a motion state (yaw motion and lateral motion) in the lateral direction of the vehicle. Specifically, for example, a yaw rate, a yaw angle, a lateral displacement speed, and a lateral position (lateral displacement) are estimated as state quantities representing the motion state in the lateral direction of the vehicle.

  The estimation method will be briefly described with reference to FIG. In FIG. 1, the vehicle 100 is steered so as to travel along a track on which magnetic markers are discretely installed, for example. The motion state in the lateral direction of the vehicle 100 is affected by factors such as a steering angle, a wheel speed, a road curvature, and a bank angle (kant). Among the state quantities (yaw rate, yaw angle, lateral displacement speed, lateral position) representing the motion state, the yaw rate is autonomously observed based on the detection signal from the yaw rate sensor, and the lateral position is detected by a marker sensor such as a magnetic sensor. Each time a magnetic marker is detected, it is observed based on the detection signal. At the same time, the lateral position is also observed autonomously based on a detection signal from a GPS receiver that can identify the position of the vehicle or a white line recognition device that can recognize a white line drawn along the track.

  The Kalman filter 200 models the motion state of the vehicle 100 affected by factors such as the steering angle, wheel speed, road curvature, bank angle, and the like, and the lateral position and yaw rate observed as described above. Further, the estimated values of the yaw rate, yaw angle, lateral displacement speed and lateral position are calculated using the previous estimated values. The actually observed yaw rate and lateral position constantly include noise components (measurement noise) of yaw rate sensors, marker sensors, white line recognition measurement and GPS measurement, and further, noise components (system noise) unique to the vehicle 100. Is also included. In contrast, by repeatedly calculating each estimated value of the state quantity (yaw rate, yaw angle, lateral displacement speed, and lateral position) by the Kalman filter 200, each estimated value of the state quantity is a true value from which the noise component has been removed. It converges to the value of.

  A vehicle steering control device using the vehicle state quantity estimation device using the Kalman filter as described above is configured as shown in FIG. 2, for example. In FIG. 2, a GPS receiver 11, a white line recognition device 12, a marker sensor 13, a yaw rate sensor 14, a G sensor 15, a wheel speed sensor 16 and a steering angle sensor 17 are connected to a control unit (ECU) 50. The GPS receiver 11 outputs a detection signal corresponding to coordinate information (for example, latitude and longitude) related to the position of the vehicle specified based on reception information from GPS satellites. The white line recognition device 12 outputs a detection signal corresponding to the relative positional relationship in the vehicle lateral direction with the white line drawn along the track. The marker sensor 13 outputs a detection signal corresponding to the magnitude of magnetism generated by the magnetic marker (lane marker). Therefore, the marker sensor 13 outputs a detection signal corresponding to the relative positional relationship in the vehicle lateral direction with respect to the magnetic marker when the vehicle passes over each magnetic marker installed discretely along the track. The yaw rate sensor 14 outputs a detection signal corresponding to the yaw rate of the traveling vehicle. The G sensor 15 outputs a detection signal corresponding to the lateral acceleration of the running vehicle. The wheel speed sensor 16 outputs a pulse signal corresponding to the rotational speed of the wheel of the traveling vehicle (corresponding to the vehicle speed) as a detection signal. The steering angle sensor 17 outputs a detection signal corresponding to the steering angle of the steered wheel (front wheel).

  Further, the memory unit 20 includes the shape of the road on which the vehicle travels (curvature, bank angle, number of lanes of the road, lane width, altitude, etc.) and surrounding road structures (houses, buildings, intersections, railroad crossings, parking lots, Map information relating to a toll road toll) is stored in advance together with the coordinate data, and the control unit 50 reads the map information from the memory unit 20 as necessary. The map information in the memory unit 20 may be updatable via vehicle-to-vehicle communication, road-to-vehicle communication, external communication such as a predetermined management center, or via a medium such as a CD or DVD.

  The control unit 50 has the function of the Kalman filter 200 and the function of vehicle steering control as described above. With the function of the Kalman filter 200, state calculation (yaw rate, yaw angle, lateral displacement speed, lateral position) representing the motion state in the lateral direction of the vehicle is performed, and the vehicle steering control unit 31 performs the estimation calculation. A steering angle is calculated based on the state quantity, and a steering control signal corresponding to the steering angle is output. Thus, the steering actuator 32 provided in the steering system is driven based on the steering control signal output from the control unit 50.

The Kalman filter 200 of the control unit 50 is
State variable x = (η, η ′, θ, γ) ^T
Influence factor (input amount) u = (δ, κ, α) ^T
System noise w = (wη, wη ′, wθ, wγ) ^T
Observed quantity y = (Dmag, Dgps, Dwl, γy) ^T
Observation error standard deviation σ = (σmag, σgps, σwl, σyγ) ^T
Is defined, the equation of state shown in equation (1) and the observation equation shown in equation (2) are formulated. “() ^T ” indicates a transposed matrix. Further, η ′ and “dot η” in the equation are synonymous.

ここで、式（１）における（ｄ／ｄｔ）は、時間ｔに関する微分演算子である。また、状態方程式（１）と観測方程式（２）における各変数は、
η：車両重心の横位置
η’：車両重心の横変位速度（車両重心の横位置微分）
θ：道路接線方向に対するヨー角
γ：ヨーレート
δ：操舵角（操舵角センサ１７により検出）
κ：道路曲率（地図情報から取得）
α：バンク角（地図情報から取得）
Ｄｍａｇ，Ｄｇｐｓ，Ｄｗｌ：横位置（観測値）
γｙ：ヨーレート（観測値）
ｗη，ｗη’，ｗθ，ｗγ：状態量のシステム雑音
Ｖ：車速（車輪速センサ１１により検出）
ｇ：重力加速度
ｍ：車両質量
Ｉ：ヨー慣性質量
Ｋｆ，Ｋｒ：前・後輪のコーナリングパワー
ｌｆ，ｌｒ：重心と前・後輪との間の距離
Ｌ：重心から車両前端までの距離
と定義される。なお、観測方程式（２）における観測誤差標準偏差σについての説明は後述する。

Here, (d / dt) in equation (1) is a differential operator with respect to time t. Each variable in the state equation (1) and the observation equation (2) is
η: lateral position of vehicle center of gravity η ′: lateral displacement speed of vehicle center of gravity (lateral position derivative of vehicle center of gravity)
θ: Yaw angle relative to road tangential direction γ: Yaw rate δ: Steering angle (detected by steering angle sensor 17)
κ: Road curvature (obtained from map information)
α: Bank angle (obtained from map information)
Dmag, Dgps, Dwl: Horizontal position (observed value)
γy: Yaw rate (observed value)
wη, wη ′, wθ, wγ: System noise of state quantity V: Vehicle speed (detected by wheel speed sensor 11)
g: Gravity acceleration m: Vehicle mass I: Yaw inertia mass Kf, Kr: Cornering power of front and rear wheels lf, lr: Distance between the center of gravity and front / rear wheels L: Definition of the distance from the center of gravity to the front end of the vehicle Is done. The observation error standard deviation σ in the observation equation (2) will be described later.

  Each of the above variables is defined in a coordinate system as shown in FIG. That is, the relationship between the vehicle 100 and the track R is set so that the center of gravity G of the vehicle 100 (two-wheel model) is positioned in a direction η perpendicular to the tangent ξ of the track R (target track) having the curvature κ. Then, distances lf and lr between the center of gravity G and the front and rear wheels are defined in a coordinate system xy in the vehicle longitudinal direction with the center of gravity G as the origin. Further, the yaw rate γ as the state quantity is defined as the yaw rate around the center of gravity G. The lateral position D is defined as the distance from the track R in the direction η to the marker sensor (installed at the front end of the vehicle). The yaw angle θ as a state quantity is defined as an angle formed between the tangential direction ξ of the track R and the vehicle longitudinal direction x.

  Based on the state equation (1) and the observation equation (2), each estimated value of the state quantity (yaw rate, yaw angle, lateral displacement speed, and lateral position) is repeatedly calculated by the Kalman filter 200. The estimated value converges to a true value from which the noise component is removed. In the process of repeatedly calculating the estimated value in the process of the Kalman filter 200, the estimated value obtained last time is used when solving the differential equation shown in the state equation (1).

Further, in the Kalman filter 200, in order to obtain the current estimated error covariance matrix P _{K (t / t)} using the previously estimated estimated error covariance matrix P _{K (t / t−1)} , the expression (3) Formulate the covariance equation shown in Then, the Kalman filter 200 solves the covariance equation (3) and determines the estimated error covariance matrix P _{K (t / t)} for this time. Further, in the Kalman filter 200, in order to predict the next estimated error covariance matrix P _{K (t + 1 / t)} using the estimated error covariance matrix P _{K (t / t)} determined this time, The covariance equation shown is solved to predict the next estimated error covariance matrix P _{K (t + 1 / t)} .

推定誤差共分散行列Ｐ_Ｋは、式（５）で示される。推定誤差共分散行列Ｐ_Ｋの各対角値は、カルマンフィルタ２００によって推定された横位置、横変位速度、ヨー角及びヨーレートに対する各推定誤差（ひいては、これらを推定するために用いた観測量や入力量の検出誤差に相当する）を示す。すなわち、推定誤差共分散行列Ｐ_Ｋの各対角値は、その値が大きいほど、その対角値に対応する状態量ｘの各値の推定精度（ひいては、これらを推定するために用いた観測量や入力量の検出精度）が低いこと（推定誤差が大きいこと）を示す。式（３）のＫ_Ｋ（ｔ）は、カルマンゲインであり、式（６）によって求められる。式（３）の行列Ｃは、式（２）の行列Ｃに相当する。式（４）の行列Ａは式（１）内の行列Ａに相当する。式（４）の行列Ｇは式（１）内の行列Ｇに相当する。式（４）の行列Ｑ_Ｋは、式（７）で表され、その対角値がシステム雑音の標準偏差である。行列Ｑ_Ｋの各値は、横位置η，横変位速度η’，ヨー角θ，ヨーレートγに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、外乱の変化などに応じて設定される可変値としてもよい。

The estimated error covariance matrix P _K is expressed by Equation (5). Each diagonal value of the estimated error covariance matrix P _K is calculated based on each estimated error for the lateral position, lateral displacement speed, yaw angle, and yaw rate estimated by the Kalman filter 200 (and thus the observed amount and input used to estimate them). Corresponding to the detection error of the quantity). That is, the larger the value of each diagonal value of the estimated error covariance matrix P _K is, the higher the estimated accuracy of each value of the state quantity x corresponding to the diagonal value (and the observation used to estimate them). This means that the detection accuracy of the quantity and the input quantity is low (the estimation error is large). _{KK (t) in} Equation (3) is a Kalman gain, and is obtained by Equation (6). The matrix C in Expression (3) corresponds to the matrix C in Expression (2). The matrix A in Expression (4) corresponds to the matrix A in Expression (1). The matrix G in Equation (4) corresponds to the matrix G in Equation (1). The matrix Q _{K in} Expression (4) is expressed by Expression (7), and the diagonal value is the standard deviation of system noise. Each value of the matrix Q _K is set corresponding to the lateral position η, the lateral displacement speed η ′, the yaw angle θ, and the yaw rate γ. Each value may be a fixed value set in advance, or may be a variable value set according to a change in disturbance.

式（５）の行列Ｐ_Ｋ内の・は、任意の値を示す。式（６）の行列Ｃは、式（２）の行列Ｃに相当する。式（６）の行列Ｒ_Ｋは式（８）で表され、その対角値が観測誤差標準偏差である。行列Ｒ_Ｋの各値は、横位置Ｄｍａｇ，横位置Ｄｇｐｓ，横位置Ｄｗｌ及びヨーレートγｙに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、後述のように各観測信号のレベルの変化などに応じて設定される可変値としてもよい。行列Ｑ_Ｋ及び行列Ｒ_Ｋと上記したシステム雑音ｗ及び観測誤差標準偏差σとは、式（９）の関係にある。式（９）のＥは、相関演算を意味する記号である。式（９）のδ_ｔτは、クロネッカーのデルタを意味する記号であり、ｔ＝τのときにはδ_ｔτ＝１であり、ｔ＝τではないときにはδ_ｔτ＝０である。

In the matrix P _K in the equation (5), “•” indicates an arbitrary value. The matrix C in Expression (6) corresponds to the matrix C in Expression (2). Matrix R _K of the formula (6) is represented by the formula (8), the diagonal values are the observation error standard deviation. Each value of the matrix _{R K} is the lateral position Dmag, lateral position Dgps, each value corresponds to a horizontal position Dwl and the yaw rate γy is set. Each value may be a fixed value set in advance, or may be a variable value set according to a change in the level of each observation signal, as will be described later. The matrix Q _K and the matrix _RK , the above-described system noise w, and the observation error standard deviation σ are in the relationship of Expression (9). E in Equation (9) is a symbol that means correlation calculation. Δ _{tτ in} equation (9) is a symbol that means the Kronecker delta, and δ _tτ = 1 when t = τ, and δ _tτ = 0 when t = τ.

Now, processing actually performed by the Kalman filter 200 will be described. In the Kalman filter 200, in order to solve the state equation (1) and the observation equation (2), a filter equation expressed by the equation (10) is established. However, “hat x” in the formula indicates an estimated value of x. The Kalman filter 200 solves the filter equation (10) using the current observed quantity y _(t) , the previously predicted state quantity x _{(t / t-1),} and the current input quantity u _(t) , and the current state. The quantity x _{(t / t)} is determined. Further, in the Kalman filter 200, in order to predict the next state quantity x _{(t + 1 / t)} using the state quantity x _{(t / t)} determined this time and the current input quantity u _(t) , Equation (11) The filter equation shown in Fig. 1 is established. Then, the Kalman filter 200 solves the filter equation (11 ₎ and predicts the next state quantity x _{(t + 1 / t)} . The next state quantity x _{(t + 1 / t)} is used when determining the next state quantity x. Note that x _{(a / b)} indicates a state quantity (estimated value) at the time of the a-th calculation predicted at the time of the b-th calculation.

フィルタ方程式（１０）のＫ_Ｋ（ｔ）は、今回のカルマンゲインである。カルマンフィルタ２００では、観測方程式（２）の行列Ｃ、式（８）の行列Ｒ_Ｋ及び前回予測された推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、式（６）から今回のカルマンゲインＫ_Ｋ（ｔ）を求める。フィルタ方程式（１０）の行列Ｃは式（２）の行列Ｃに相当する。フィルタ方程式（１１）の行列Ａは式（１）内の行列Ａに相当する。フィルタ方程式（１１）の行列Ｂは式（１）内の行列Ｂに相当する。

_{KK (t} ) in the filter equation (10) is the current Kalman gain. In the Kalman filter 200, the matrix C of the observation equation (2), using the matrix _{R K} and the previous predicted estimated error covariance matrix _{P K} of the formula _{(8) (t / t-} 1), the current from the equation (6) Kalman gain KK _(t) is obtained. The matrix C in the filter equation (10) corresponds to the matrix C in the expression (2). The matrix A of the filter equation (11) corresponds to the matrix A in the equation (1). The matrix B of the filter equation (11) corresponds to the matrix B in the equation (1).

Further, the Kalman filter 200 solves the covariance equation (3) using the previously estimated estimated error covariance matrix P _{K (t / t−1)} , and this estimated error covariance matrix P _{K (t / t). )} . Further, the Kalman filter 200 solves the covariance equation (4) using the estimated error covariance matrix P _{K (t / t)} determined this time, and obtains the next estimated error covariance matrix P _{K (t + 1 / t)} . Predict. Thus, the Kalman filter 200 obtains the current state quantity x as the vehicle state quantity and the estimated error covariance matrix P _K. Specifically, the estimated values of the lateral position η, the lateral displacement speed η ′, the yaw angle θ, and the yaw rate γ as the vehicle state quantities are obtained.

  By the way, the Kalman filter is designed on the assumption of a range of variation in the observation amount due to noise or the like to some extent. Therefore, as long as the observation amount varies within the range, the Kalman filter can remove the variation error and accurately estimate the state amount. However, as shown in FIG. 7, when an observation amount having an unexpectedly large error (for example, an error due to a failure or a momentary inferior environment) is input to the Kalman filter, the state is affected by the influence. The error of the estimated value of the quantity becomes large.

  Therefore, the vehicle state quantity estimation apparatus according to the present embodiment calculates the “estimated value of the observed quantity y” and observes the state quantity estimated by the Kalman filter 200 based on the deviation between the actual observed quantity y and the estimated value. The degree to which the amount y is reflected is set. That is, the vehicle state quantity estimation apparatus according to the present embodiment performs actual observation in which the deviation from the estimated value by the Kalman filter 200 exceeds a predetermined reference value so that an observed value having unexpected variation does not adversely affect the state estimation. For the amount y, the state amount is estimated after executing processing that is regarded as an abnormal value.

  In order to calculate the “estimated value of the observation amount y”, the observation equation (2) may be used. The estimated value of the observed quantity y is calculated by an estimated observed quantity equation (12) obtained by modifying the observed equation (2). Note that “hat y” in the equation indicates an estimated value of y.

すなわち、観測誤差標準偏差σを除いた観測方程式（２）において、状態量ｘの部分に前回の演算時に予測した今回の状態量ｘ_{（ｔ／ｔ−１）}を代入することによって、今回の観測量ｙの推定値を演算することができる。

In other words, in the observation equation (2) excluding the observation error standard deviation σ, the current state quantity x _{(t / t−1)} predicted at the previous calculation is substituted for the state quantity x, thereby obtaining the current observation. An estimate of the quantity y can be calculated.

The observation amount y is observed at a predetermined calculation cycle. In the previous calculation, the Kalman filter 200 is the actual observation amount y _(t−1) at the previous calculation, the state quantity x _{(t−1 / t−2)} at the previous calculation predicted at the previous calculation, and the previous calculation. By solving the filter equation (10) using the input quantity u _(t−1) , the state quantity x _{(t−1 / t−1)} at the previous calculation predicted at the previous calculation is determined. At the same time, the Kalman filter 200 calculates the filter equation (11) using the state quantity x _{(t−1 / t−1)} determined in the previous calculation and the input quantity u _(t−1) in the previous calculation. By solving, the state quantity x _{(t / t-1)} at the time of the current calculation is predicted. The Kalman filter 200 calculates the state quantity x _{(t / t−1)} at the time of the current calculation predicted at the time of the previous calculation and the input quantity u _(t) at the time of the current calculation at the time of the current calculation. By substituting into, the estimated observation amount of y _(t) at the time of the current calculation is calculated.

When the deviation between the actual current observation amount y _(t) and the estimated value of the current observation amount y _(t) calculated by the Kalman filter 200 exceeds a predetermined reference value, the control unit 50 This observation amount y _(t) is regarded as an abnormal value, and the observation amount regarded as the abnormal value is excluded from the vehicle motion state model formulated by the state equation (1) and the observation equation (2). Switch the controlled model to the selected model. For example, when the horizontal position Dwl observed by the white line recognition device 12 in the observation amount y exceeds the reference range for the horizontal position D and the horizontal position Dwl is regarded as an abnormal value, the observation equation is expressed as From 2), change to the expression (13) in which the row at the horizontal position Dwl is deleted.

図８は、観測量ｙの異常値を状態量推定から排除する一方法のフローである。本フローは、所定の演算周期で繰り返される。カルマンフィルタ２００は、前回演算時に予測された今回演算時の状態量ｘ_{（ｔ／ｔ−１）}と今回演算時の入力量ｕ_（ｔ）を推定観測量方程式（１２）に代入することによって、今回演算時の観測量ｙ_（ｔ）の推定観測量を演算する（ステップ２）。カルマンフィルタ２００は、今回演算時の観測量ｙ_（ｔ）とステップ２で演算された今回演算時の観測量ｙ_（ｔ）の推定観測量との差の絶対値が所定の基準値を超えるか否かを観測量ｙ_（ｔ）の要素毎に判断する（ステップ４）。その差の絶対値が所定の基準値を超えている場合には（ステップ４；Ｙｅｓ）、その基準値を超えた今回演算時の観測量ｙ_（ｔ）の要素を観測方程式（２）から削除する（ステップ６）。例えば、基準値を超えた今回演算時の観測量ｙ_（ｔ）の要素が白線認識装置１２によって観測された横位置Ｄｗｌであるならば、観測方程式は式（１３）のように変形する。したがって、カルマンフィルタ２００は、状態方程式（１）と変形後の観測方程式（１３）の運動状態モデルに対して、フィルタ方程式（１０）を解くことで、今回演算時の状態量ｘ_（ｔ）を推定するとともに、フィルタ方程式（１１）を解くことで、次回演算時の状態量ｘ_{（ｔ＋１）}を予測する。一方、今回演算時の観測量ｙ_（ｔ）とステップ２で演算された今回演算時の観測量ｙ_（ｔ）の推定観測量との差の絶対値が所定の基準値を超えていない場合には（ステップ４；Ｎｏ）、観測方程式（２）の変形は行われない（ステップ８）。したがって、カルマンフィルタ２００は、状態方程式（１）と観測方程式（２）の運動状態モデルに対して、フィルタ方程式（１０）を解くことで、今回演算時の状態量ｘ_（ｔ）を推定するとともに、フィルタ方程式（１１）を解くことで、次回演算時の状態量ｘ_{（ｔ＋１）}を予測する。なお、ステップ６において観測方程式（２）の変形が行われたとしても、本フローが繰り返されて、今回演算時の観測量ｙ_（ｔ）とステップ２で演算された今回演算時の観測量ｙ_（ｔ）の推定観測量との差の絶対値が所定の基準値を超えないようになった場合には、式（２）に示される変形前の観測方程式に戻る。

FIG. 8 is a flow of one method for eliminating an abnormal value of the observation amount y from the state amount estimation. This flow is repeated at a predetermined calculation cycle. The Kalman filter 200 substitutes the state quantity x _{(t / t-1)} at the time of the current calculation predicted at the time of the previous calculation and the input quantity u _(t) at the time of the current calculation into the estimated observable equation (12). An estimated observation amount of the observation amount y _(t) at the time of calculation is calculated (step 2). Kalman 200, whether the absolute value of the difference between the estimated observed amount of current operation when the observation quantity y _(t) and the observed quantity y of the current operation time calculated in step 2 _(t) exceeds a predetermined reference value Is determined for each element of the observation amount y _(t) (step 4). When the absolute value of the difference exceeds a predetermined reference value (step 4; Yes), the element of the observation amount y _(t) at the time of the current calculation exceeding the reference value is deleted from the observation equation (2). (Step 6). For example, if the element of the observation amount y _(t) at the time of the current calculation exceeding the reference value is the horizontal position Dwl observed by the white line recognition device 12, the observation equation is transformed as shown in Expression (13). Therefore, the Kalman filter 200 estimates the state quantity x _(t) at the time of the current calculation by solving the filter equation (10) for the motion state model of the state equation (1) and the observed observation equation (13). At the same time, the state quantity x _{(t + 1)} at the next calculation is predicted by solving the filter equation (11). On the other hand, when the absolute value of the difference between the estimated observed amount of current operation when the observation quantity y _(t) and the observed amount of current operation time calculated in step 2 y _(t) does not exceed the predetermined reference value (Step 4; No), the observation equation (2) is not transformed (Step 8). Therefore, the Kalman filter 200 estimates the state quantity x _(t) at the time of the current calculation by solving the filter equation (10) for the motion state model of the state equation (1) and the observation equation (2), By solving the filter equation (11), the state quantity x _{(t + 1)} at the next calculation is predicted. Even if the observation equation (2) is modified in step 6, this flow is repeated, and the observation amount y _(t) at the time of the current calculation and the observation amount y at the time of the current calculation calculated in step 2 _When the absolute value of the difference from the estimated observation amount in _(t) does not exceed the predetermined reference value, the process returns to the observation equation before deformation shown in Expression (2).

Therefore, even if an observed value y _(t) is observed as an abnormal value exceeding the reference value at the time of this calculation, by removing the element of the observed value y _(t) corresponding to the abnormal value from the observation equation, It is possible to prevent the outlier from affecting the quantity estimation calculation. As a result, even if an observation amount having an unexpectedly large error is observed, the state amount estimation accuracy can be improved without affecting the state amount estimation accuracy. As long as the elements of the observation equation do not become zero due to the above deformation, the state quantity can be estimated by the Kalman filter.

  Incidentally, the observation error standard deviation σ may be set to an arbitrary value in advance by simulation or the like, but in order to further improve the accuracy of the estimated value of the state quantity x, “reliability” and “ The observation error standard deviation σ should be calculated using “freshness”.

  For example, the control unit 50 executes processing in the procedure shown in FIG. The processing in the procedure shown in FIG. 4 is based on the estimation calculation of the Kalman filter 200 based on the state equation (1) and the observation equation (2) at a predetermined calculation cycle that can be calculated a plurality of times between adjacent magnetic markers. The estimated value of the state quantity of the vehicle is calculated.

  In FIG. 4, the detection signals of the sensors as shown in FIG. 2 are acquired while the vehicle is traveling (step 10). The lateral position D of the vehicle by each sensor is redundantly calculated based on the acquired detection signal. That is, the lateral position Dgps of the vehicle is calculated based on the detection signal from the GPS receiver 11, the lateral position Dwl of the vehicle is calculated based on the detection signal from the white line recognition device 12, and the detection signal from the marker sensor 13 is Based on this, the lateral position Dmag of the vehicle is calculated. Further, the detected yaw rate γy is acquired based on the detection signal from the yaw rate sensor 14. The estimated value of the state quantity of the vehicle is calculated by the Kalman filter 200 using each lateral position D and the detected yaw rate γy thus obtained.

  In this step 10, as a premise for calculating the observation error standard deviation σ in the observation equation (2), the detection signals of the respective sensors related to the lateral position D, the detected yaw rate γy, etc. are acquired, and Single unit reliability is acquired.

  The single unit reliability of the sensor indicates the degree of accuracy that the sensor itself has subjectively determined for the detection signal output by itself. That is, each sensor can subjectively determine how accurate its detection result is in its detection status. Therefore, each sensor transmits the accuracy subjectively determined with respect to its own detection result to the control unit 50 as a single unit reliability. For example, each sensor determines that the single unit reliability is low if the detection status grasped by the sensor is a status in which the detection error tends to increase. The unit reliability is quantified to a value between 0 and 100, for example. The single unit reliability of each sensor will be described below.

  The single unit reliability of the GPS receiver 11 is set according to, for example, a position accuracy degradation degree PDOP (Position Dilution of Precision) used in GPS. PDOP is an index that expresses the relationship between the error in the observation point position and the error in the satellite position. The smaller the numerical value, the better the position accuracy. The GPS receiver 11 calculates the unit reliability higher as the PDOP is smaller, and outputs the calculated unit reliability to the control unit 50 together with the detection signal. For example, the GPS receiver 11 sets the unit reliability according to the arithmetic expression “single unit reliability = 100−PDOP × 10 (10 when the PDOP is 10 or more)”.

  The single line reliability of the white line recognition device 12 is set according to its own white line recognition result such as a luminance difference, white line parallelism, and tracking degree. The white line recognition device 12 calculates the unit reliability higher as the luminance difference is larger, calculates the unit reliability higher as the white line parallelism is higher, and calculates the unit reliability higher as the white line tracking degree is higher. The calculated unit reliability is output to the control unit 50 together with the detection signal.

  The single unit reliability of the marker sensor (magnetic sensor) 13 is set according to the magnetic density distribution shape, the magnetic strength, and the like. The marker sensor 13 calculates the unit reliability higher as the detected magnetic density distribution shape is closer to the ideal distribution, calculates the unit reliability higher as the magnetic force strength is larger, and calculates the calculated unit reliability together with the detection signal. Output to the control unit 50.

  The single unit reliability of the yaw rate sensor 14 is set according to, for example, the spectral intensity of the frequency component of the detection signal. The yaw rate sensor 14 calculates the single unit reliability as the frequency component higher than the high frequency component (for example, 10 Hz) that is unlikely to be a vehicle movement increases, and outputs the calculated single unit reliability to the control unit 50 together with the detection signal. To do.

  The single unit reliability of the G sensor 15 is set according to the spectral intensity of the frequency component of the detection signal, for example. The G sensor 15 calculates the single unit reliability as the frequency component higher than the high frequency component (10 Hz) that is unlikely to be a vehicle motion increases, and outputs the calculated single unit reliability to the control unit 50 together with the detection signal.

  The single unit reliability of the wheel speed sensor 16 is set according to the shape of the output pulse of the detection signal, for example. When the elastic change of the tire due to puncture or other abnormality occurs, the spectrum distribution of the output pulse changes. Therefore, the wheel speed sensor 16 uses pattern matching or the like so that the shape of the output pulse is close to the ideal pulse shape at the wheel speed. The higher the unit reliability is calculated, and the calculated unit reliability is output to the control unit 50 together with the detection signal.

  Further, as described above, the single unit reliability of each sensor is acquired in step 10 of FIG. 4, while the driving environment reliability of each sensor is calculated (step 20).

  The driving environment reliability of the sensor indicates the degree of accuracy that the control unit 50 has objectively determined for the detection signal acquired from the sensor. That is, the control unit 50 can objectively determine how accurate the detection result of the sensor is in the detection state. This is based on the idea that the detection result of the sensor is not necessarily an accurate value depending on the detection situation. Therefore, the control unit 50 calculates the accuracy determined objectively with respect to the detection result of the sensor as the travel environment reliability. For example, the control unit 50 calculates that the traveling environment reliability is low if the detection environment when the sensor performs a detection operation is a situation in which a detection error tends to increase. The traveling environment reliability is quantified to a value between 0 and 100, for example. The travel environment reliability of each sensor will be described below.

  The driving environment reliability of the GPS receiver 11 is set according to the state of the building around the vehicle (for example, its shape and arrangement), for example. Based on the map information in the memory unit 20, the control unit 50 calculates the driving environment reliability low when a vehicle is present in a place where radio waves from GPS satellites are difficult to reach such as a tunnel or a building street.

  The driving environment reliability of the white line recognition device 12 is set according to the weather, sunshine, road surface material, and the like. The control unit 50 sets the driving environment reliability to be lower in the detection situation where the white line is difficult to recognize. In case of rain or cloudy, the driving environment reliability is calculated lower than in the case of clear weather, and in the case of night, the driving environment reliability is calculated lower than in the daytime. The driving environment reliability is calculated low.

  The traveling environment reliability of the marker sensor 13 is set according to magnet embedding error, geomagnetism, road structure material, and the like. The control unit 50 calculates the driving environment reliability lower in the place where the magnet embedding error is large due to the construction than in the small place, and calculates the driving environment reliability lower in the place where the geomagnetism is strong than in the weak place. The driving environment reliability is calculated lower in places where the body is used than in places where the body is not used.

  The traveling environment reliability of the yaw rate sensor 14, the G sensor 15, and the wheel speed sensor 16 is set according to road surface roughness, road surface undulation, road surface material, and the like. The control unit 50 calculates the driving environment reliability lower in a place where the road surface roughness and road undulation are large than in a small place.

  Common elements for setting the driving environment reliability of each sensor include vehicle shape (vehicle height, vehicle width, total length), vehicle type, body material, and the like. For example, when the vehicle on which the sensor is mounted is a bus or a truck, the control unit 50 is more likely to transmit the vibration of the vehicle to the sensor than in the case of an ordinary passenger vehicle, and therefore calculates the driving environment reliability low.

  As described above, when the unit reliability of each sensor is acquired in step 10 of FIG. 4 and the travel environment reliability of each sensor is calculated in step 20, the total reliability of each sensor is calculated (step 30).

The total reliability of the sensor indicates the degree of accuracy of the detection signal of each sensor in consideration of the above-described single unit reliability and traveling environment reliability. The overall reliability Ra is
Total reliability Ra
= (K1 × unit reliability + k2 × running environment reliability) × freshness (14)
Freshness f (t)
= −K (κ, γ) × t / T + 1 (15)
Can be computed by Here, k1 and k2 in the equation (14) are coefficients satisfying “k1 + k2 = 1, 0 ≦ k1 ≦ 1, 0 ≦ k2 ≦ 1”. In Equation (15), T is the measurement cycle of the sensor, and t is the elapsed time after the lateral position is detected (or from the previous measurement time). K is a deterioration coefficient and depends on the road curvature κ and the yaw rate γ.

The deterioration coefficient K is, for example,
K (κ, γ) = j1 + j2 × | κ | + j3 × | γ | (16)
Can be defined. However, 0 ≦ K ≦ 1, j1, j2, and j3 are arbitrary coefficients.

  Here, the freshness of Expression (15) will be described with reference to FIG. Freshness is determined for each sensor. When each sensor observes data at a predetermined measurement cycle, the reliability of the observation data at the measurement time is high, but the reliability of the observation data at the measurement time decreases as time passes from the measurement time (that is, , The freshness of the observation data decreases).

  For example, the degree of change in the observed data from the time of measurement when driving on a curve or when the vehicle is not stable is when the vehicle is traveling on a straight road or when the vehicle is traveling stably. In comparison, it is considered that it is large (especially, it is highly possible that the horizontal position has changed significantly), so that the freshness of the observation data at that time may be considered to decrease as time passes from the time of measurement. it can.

  Therefore, as shown in Expression (15), by adding terms of road curvature κ and yaw rate γ, which are factors affecting the motion state in the lateral direction of the vehicle, the larger the road curvature κ, the larger the yaw rate γ. An arithmetic expression for calculating the freshness level is set so that the freshness level decreases, and as shown in formula (14), an arithmetic expression for calculating the total reliability level Ra so that the overall reliability level Ra decreases as the freshness level decreases. Is set. Depending on the sensor, the measurement may fail at the time of measurement. Therefore, as shown in FIG. 5, when the measurement fails, the freshness is reduced as it is and reflected in the calculation of the total reliability Ra.

Further, when the absolute value of the difference between the estimated observables observables current operation time y _(t) and the current operation when the observation quantity y _(t) exceeds a predetermined reference value, observed in the current operation time Assuming that the measurement of the quantity y _(t) has failed, the freshness is decreased according to the freshness f (t) shown in FIG. 5 and reflected in the calculation of the overall reliability Ra as described above. Good.

Note that, when the measurement interval is defined not by time but by distance (that is, when measurement is performed at each timing when the magnetic marker is installed at every predetermined interval), the elapsed time t is set as the elapsed distance s. The period T can be replaced with the magnetic marker installation distance interval S. This may be applied to the case of the marker sensor 13,
Freshness f (s) = − K (κ, γ) × s / S + 1 (17)
Can be defined. In this case, the horizontal axis in FIG. 5 is defined not by time but by travel distance. If the freshness degree for each sensor is calculated, the total reliability Ra of each sensor is digitized to a value between 0 and 100 according to Equation (14).

As described above, when the total reliability of each sensor is calculated in step 30 of FIG. 4, the total reliability is reflected in the observation error standard deviation σ in the observation equation (2) (step 40). The observation error standard deviation σ is an index indicating whether the observation data measured by the sensor is reliable. As the error in the observation data increases, the observation error standard deviation σ increases. The overall reliability Ra calculated as described above is reflected in this observation error standard deviation σ. As a way of reflecting the overall reliability Ra in the observation error standard deviation σ, the observation error standard deviation σ is set to a larger value as the overall reliability Ra becomes smaller. Therefore, as an example of a method for reflecting the overall reliability Ra to the observation error standard deviation σ, the standard deviation in the best measurement state is calculated in advance for each sensor by simulation or the like, and the standard deviation in the best measurement state is calculated as σmin. Then, the observation error standard deviation σ is
σ = (100 / Ra) × σmin (18)
It can be expressed as. Since the total reliability Ra assumes a value of 0 to 100 in this embodiment, the minimum value of the observation error standard deviation σ corresponds to σmin. Thus, the term of the observation error standard deviation σ in the observation equation (2) of the Kalman filter 200 can be changed according to the total reliability Ra of each sensor calculated as described above. In the observation equation (2), the observation error standard deviation of the GPS receiver 11 is σgps, the observation error standard deviation of the white line recognition device 12 is σwl, the observation error standard deviation of the marker sensor 13 is σmag, and the yaw rate sensor The observation error standard deviation of 14 is σyγ.

  Accordingly, the observation error standard deviation σ calculated as described above is substituted into the observation equation (2), and the state quantity (lateral position η, lateral displacement speed η ′, yaw angle θ) is estimated by the Kalman filter 200 estimation calculation described above. , Yaw rate γ) is calculated (step 50 in FIG. 4).

  Since the observation error standard deviation σ term exists in the observation equation (2) in this way, the Kalman filter 200 calculates that the observation data is unreliable when the observation error standard deviation σ becomes large. Become. As in the observation equation (2), the lateral position Dgps is observed based on the GPS receiver 11, the lateral position Dwl is observed based on the white line recognition device 12, and the lateral position Dmag is observed based on the marker sensor 13. The lateral position D is observed redundantly. Accordingly, among the redundantly observed lateral positions D, the observation data of the sensor having the small observation error standard deviation is more easily used for the calculation, and the observation data of the sensor having the large observation error standard deviation is less used for the calculation. Become.

When the estimated value of the state quantity is calculated by the process in the Kalman filter 200 as described above, the steering angle δ to be controlled using the estimated value is calculated according to the following equation (step 60 in FIG. 4). The steering angle δ is
δ = Kη · η + Kη ′ · η ′ + Kθ (θ−θ _TARGET ) + Kγ (γ−γ _TARGET ) + δf
... (19)
Can be calculated by In Equation (19), Kη, Kη ′, Kθ, and Kγ are constants, θ _TARGET is a target yaw angle, γ _TARGET is a target yaw rate, and δf is a feedforward steering angle. The feedforward steering angle δf is determined based on the vehicle model, road curvature, bank angle, vehicle speed, and the like. When the steering angle δ to be controlled is calculated, as shown in FIG. 2, a steering control signal based on the steering angle δ is output from the control unit 50 to the steering actuator 32, and the steering actuator according to the steering control signal. 32 is driven (step 70 in FIG. 4). Thereafter, the above process is repeatedly executed. By repeatedly executing the process in the Kalman filter 200 in this way, the estimated values of the state quantities such as the lateral position η and the yaw rate γ are successively converged to the value of the state quantity that should be originally obtained.

  By the way, in the above-described method for removing the abnormal value of the observed quantity y from the state quantity estimation, the single unit reliability, the driving environment reliability, and the freshness are used to calculate the standard error standard deviation σ. In order to calculate the standard deviation σ, the abnormal value of the observation amount y may be excluded from the state quantity estimation using the single unit reliability, the driving environment reliability, the continuity reliability, and the freshness. In this case, the control unit 50 executes processing according to the procedure shown in FIG. The process in the procedure shown in FIG. 9 is similar to that in FIG. 4 described above, with the above-described state equation (1) and observation equation (2) at a predetermined calculation cycle that can be calculated multiple times between adjacent magnetic markers. Based on the estimation calculation of the Kalman filter 200, the estimated value of the state quantity of the vehicle is calculated. The description of FIG. 9 in which step 25 is added to FIG. 4 will be omitted or simplified for steps other than step 25.

The continuity reliability shown in FIG. 9 is an index for evaluating the reliability of the observed value in view of the continuity of the observation amount of each sensor. The continuity reliability is calculated by using the estimated observation amount and the observation amount for the observation amount y _(t) used for the calculation of the state amount estimation by the Kalman filter 200. If the absolute value of the difference between the estimated observed amount of current operation when the observation quantity y _(t) and the current operation when the observation quantity y _(t) exceeds a predetermined reference value, the observed quantity y of this calculation time _(T) can be regarded as having low continuity reliability as compared to the observed quantity y _(t) when the reference value is not exceeded. Further, the continuity reliability is quantified to be a value between 0 and 100 according to the single unit reliability and the driving environment reliability.

のように、観測量ｙ_（ｔ）についての推定観測量と観測量との差の関数Ｌによって定義する。関数Ｌは、観測量ｙ_（ｔ）についての推定観測量と観測量との差が大きいほど連続性信頼度が小さくなるような関数とする。例えば、『関数Ｌ（Ｚ）＝ｌ／｜Ｚ｜』としてもよい。すなわち、関数Ｌは、Ｚの絶対値の逆数に比例する関数（ｌは定数）である。

As defined by a function L of the difference between the estimated observed quantity and the observed quantity for the observed quantity y _(t) . The function L is a function such that the continuity reliability decreases as the difference between the observed amount and the observed amount for the observed amount y _(t) increases. For example, “function L (Z) = 1 / | Z |” may be used. That is, the function L is a function proportional to the reciprocal of the absolute value of Z (l is a constant).

  According to FIG. 9 described above, the unit reliability of each sensor is acquired in step 10, the driving environment reliability of each sensor is calculated in step 20, and the continuity reliability of each sensor is calculated as described above. Then (step 25), the total reliability of each sensor is calculated (step 30).

The total reliability of the sensor in the case shown in FIG. 9 indicates the degree of accuracy of the detection signal of each sensor in consideration of the above-described single unit reliability, traveling environment reliability, and continuity reliability. The total reliability Ra in the case shown in FIG.
Total reliability Ra =
(K1 × unit reliability + k2 × running environment reliability + k3 × continuity reliability) × freshness
... (21)
Can be computed by The freshness in equation (21) is the same as equation (15) above. Here, k1, k2, and k3 in Expression (21) are coefficients that satisfy “k1 + k2 + k3 = 1, 0 ≦ k1 ≦ 1, 0 ≦ k2 ≦ 1, 0 ≦ k3 ≦ 1”.

  As described above, when the total reliability of each sensor is calculated in step 30 of FIG. 9, the total reliability is reflected in the observation error standard deviation σ in the observation equation (2) (step 40). The observation error standard deviation σ can be expressed in the same manner as Expression (18).

  Accordingly, the observation error standard deviation σ calculated as described above is substituted into the observation equation (2), and the state quantity (lateral position η, lateral displacement speed η ′, yaw angle θ) is estimated by the Kalman filter 200 estimation calculation described above. , Yaw rate γ) is calculated (step 50 in FIG. 9).

  Since the observation error standard deviation σ term exists in the observation equation (2) in this way, the Kalman filter 200 calculates that the observation data is unreliable when the observation error standard deviation σ becomes large. Become. As in the observation equation (2), the lateral position Dgps is observed based on the GPS receiver 11, the lateral position Dwl is observed based on the white line recognition device 12, and the lateral position Dmag is observed based on the marker sensor 13. The lateral position D is observed redundantly. Accordingly, among the redundantly observed lateral positions D, the observation data of the sensor having the small observation error standard deviation is more easily used for the calculation, and the observation data of the sensor having the large observation error standard deviation is less used for the calculation. Become.

  When the estimated value of the state quantity is calculated by the process in the Kalman filter 200 as described above, the steering angle δ to be controlled using the estimated value is calculated according to the equation (19) (step 60 in FIG. 9). . When the steering angle δ to be controlled is calculated, as shown in FIG. 2, a steering control signal based on the steering angle δ is output from the control unit 50 to the steering actuator 32, and the steering actuator according to the steering control signal. 32 is driven (step 70 in FIG. 9). Thereafter, the above process is repeatedly executed. By repeatedly executing the process in the Kalman filter 200 in this way, the estimated values of the state quantities such as the lateral position η and the yaw rate γ are successively converged to the value of the state quantity that should be originally obtained.

  Therefore, according to the present embodiment, the total reliability is calculated from the driving environment reliability in which the control unit 50 considers the advantages and disadvantages of each sensor according to the unit reliability and the measurement situation based on the processing result of each sensor ( Alternatively, the total reliability is calculated from the continuity reliability considering the continuity of the unit reliability and the driving environment reliability and the observation value of each sensor), and the elapsed time from the timing measured by each asynchronous sensor The degree of freshness that decreases according to (or elapsed distance) is calculated, and the Kalman filter is estimated by reflecting the calculated total reliability and freshness in the observation error standard deviation σ. In addition, the lateral displacement measured immediately before can be used to estimate the state of the Kalman filter, and the accuracy and reliability of state quantity estimation by the Kalman filter can be improved. That is, the accuracy of the estimated value of the state quantity between the magnetic markers can be maintained, and even if the period for observing the state quantity at a predetermined checkpoint where the magnetic marker is installed becomes longer, a more accurate vehicle It is possible to realize a vehicle state quantity estimating device that can estimate the state quantity of the vehicle. In addition, it is possible to realize a vehicle steering control device using such a vehicle state quantity estimation device that can perform steering control of the vehicle with higher accuracy.

  Further, according to the present embodiment, even if an observed amount having an unexpectedly large error (for example, an error due to a failure or a momentary inferior environment) is observed, the difference between the estimated observed amount and the actual observed amount is By exceeding a predetermined reference value, the reliability of continuity regarding the observed quantity is lowered, and the weight in the state estimation of the observed value can be lowered. That is, by temporarily lowering the reliability of the observed quantity indicating the abnormal value and eliminating the influence of the abnormal observed value on the state quantity estimation, the estimated value of the state quantity as shown by the dotted line in FIG. It is possible to suppress the error and prevent the estimation accuracy from deteriorating.

  The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the above-described embodiments, and various modifications and substitutions can be made to the above-described embodiments without departing from the scope of the present invention. Can be added.

  For example, the present invention can also be implemented in the configuration of the Kalman filter shown in FIG. The Kalman filter shown in FIG. 6 is a distance axis Kalman filter that calculates an estimated value of a state quantity (yaw rate, yaw angle, lateral displacement speed, lateral position) of the vehicle 100 according to the function of the Kalman filter 200a every time a magnetic marker is detected, A time-axis Kalman filter that calculates an estimated value of the state quantity (yaw rate, yaw angle, lateral displacement speed, lateral position) of the vehicle 100 according to the function of the Kalman filter 200b for each predetermined period is provided. The Kalman filter shown in FIG. 6 is an estimated value of the distance-axis Kalman filter 200a that estimates the state using the horizontal position and the yaw rate as an observed value when passing the magnetic marker, and the initial state quantity of the time-axis Kalman filter 200b that estimates the state using only the yaw rate as the observed value Thus, the estimation accuracy of the state quantity between the magnetic markers that cannot measure the lateral position is improved. For each of the distance axis Kalman filter 200a and the time axis Kalman filter 200b, a formula similar to the state equation (1) and the observation equation (2) in which the total reliability and freshness are reflected in the observation error standard deviation σ. If the calculation is performed, the accuracy and reliability of state quantity estimation are improved.

  In the above-described embodiment, the travel environment reliability is calculated for each vehicle. However, a travel environment reliability database may be constructed, and each vehicle may acquire the travel environment reliability from the database. . For example, each vehicle transmits measurement information at the same point, such as the measurement result of each sensor and its single unit reliability, and the traveling state, weather, and road surface condition at the time of measurement by each sensor, to a predetermined management center. On the other hand, the management center acquires the measurement information from each vehicle, and statistically constructs a reliability database of each sensor using the running status, weather, road surface status, and the like as indexes. And each vehicle should just acquire the traveling environment reliability in a reliability database via communication lines, such as road-to-vehicle communication, from a management center, and may utilize it for the above-mentioned state quantity estimation. As a result, it is possible to derive a driving environment reliability with higher objectivity than the driving environment reliability calculated for each vehicle.

  In the above-described embodiment, the Kalman filter is taken as an example. However, the estimator that inputs the predetermined state quantity observed by the sensor to the model of the vehicle movement state and estimates the state quantity representing the vehicle movement state is as follows. In particular, it is not limited to the Kalman filter. This estimator only needs to converge to a true value to be obtained by repeatedly calculating. In the estimation process, the estimator adds the reliability of the sensor to a numerical value (the observation error standard deviation σ in the case of the Kalman filter in this embodiment) that determines how much the measured value of the sensor is to be estimated. Reflect it.

It is a block diagram for demonstrating the method using a Kalman filter. It is a block diagram which shows the structure of the vehicle steering control apparatus with which the vehicle state quantity estimation apparatus which concerns on one Embodiment of this invention is applied. It is a figure which shows the definition of the coordinate system set with respect to the modeled vehicle, and each variable. 3 is a flowchart showing a first procedure of processing of a control unit 50. It is a figure for demonstrating calculation of a freshness degree. It is a block diagram for demonstrating the method using a Kalman filter. It is the figure which showed the relationship between an observed value and a state estimated value. It is a flow of one method for eliminating an abnormal value of an observation amount y from state quantity estimation. 7 is a flowchart showing a second procedure of processing of the control unit 50.

Explanation of symbols

DESCRIPTION OF SYMBOLS 11 GPS receiver 12 White line recognition apparatus 13 Marker sensor 14 Yaw rate sensor 15 G sensor 16 Wheel speed sensor 17 Steering angle sensor 20 Memory unit 31 Steering control part 32 Steering actuator 50 Control unit 100 Vehicle 200 Kalman filter

Claims (10)

Observing means for observing a predetermined amount of state representing the motion state of the vehicle;
A vehicle state quantity estimation device comprising state quantity estimation means for inputting a predetermined state quantity observed by the observation means to a model of a vehicle motion state and estimating a state quantity representing the motion state of the vehicle,
An observation amount estimation unit that inputs a state amount estimated by the state amount estimation unit to a model of the observation unit and estimates a predetermined state amount that can be observed by the observation unit;
The degree of reflection of the vehicle state of motion of the predetermined state quantity observed by the observation means is a deviation between the predetermined state quantity observed by the observation means and the predetermined state quantity estimated by the observation amount estimation means. A vehicle state quantity estimating device, which is set based on the above.   The vehicle state quantity estimation according to claim 1, wherein the reflection degree of the predetermined state quantity whose deviation exceeds a predetermined value among the predetermined state quantities observed by the observation unit is set to be smaller than the previous reflection degree. apparatus.   The vehicle state quantity estimation apparatus according to claim 1 or 2, wherein the degree of reflection is set to be smaller for a predetermined state quantity having a larger degree of deviation among the predetermined state quantities observed by the observation means. The state quantity estimating means estimates a state quantity representing the motion state of the vehicle by inputting the predetermined state quantity observed for the N-1th time by the observation means to the model of the motion state of the vehicle,
The observation amount estimation means is configured to input a state quantity estimated by the state quantity estimation means to a model of the observation means and estimate a predetermined state quantity observable for the Nth time by the observation means,
The deviation corresponds to a difference between a predetermined state quantity observed N times by the observation means and a predetermined state quantity observable N times by the observation amount estimation means (N is an integer of 2 or more). The vehicle state quantity estimation device according to any one of claims 1 to 3.   The reflection degree is further set based on an observation state determined by the observation means based on its own observation result, and the reflection degree is set to be smaller as the observation state has a larger observation error. Item 5. The vehicle state quantity estimation device according to any one of Items 1 to 4.   The reflection degree is further set based on an observation environment when a predetermined state quantity is observed by the observation unit, and the reflection degree is set to be smaller as the observation environment is an environment having a larger observation error. Item 6. The vehicle state quantity estimation device according to any one of Items 1 to 5.   The reflection degree is further set based on an elapsed time from the observation point of the predetermined state quantity by the observation means, and the reflection degree is set to be smaller as the elapsed time becomes longer. The vehicle state quantity estimation device according to claim 1.   The reflection degree is further set based on a travel distance from the observation point of the predetermined state quantity by the observation means, and the reflection degree is set to be smaller as the travel distance becomes longer. The vehicle state quantity estimation device according to claim 1. The state quantity estimating means and the observed quantity estimating means are Kalman filters,
A standard deviation term of the observation error of the predetermined state quantity observed by the observation means is added to the observation equation of the Kalman filter,
The vehicle state quantity estimation device according to any one of claims 1 to 8, wherein the standard deviation increases as the reflection degree is set smaller. The vehicle state quantity estimating device according to any one of claims 1 to 9,
A vehicle steering control device, comprising: a control signal generation unit configured to generate a control signal for a steering actuator in a steering system based on a state quantity representing a motion state of a vehicle estimated by the vehicle state quantity estimation device.

Images