JP5821288B2

JP5821288B2 - Road shape prediction device

Info

Publication number: JP5821288B2
Application number: JP2011122600A
Authority: JP
Inventors: 太一溝尻
Original assignee: Nissan Motor Co Ltd
Current assignee: Nissan Motor Co Ltd
Priority date: 2011-05-31
Filing date: 2011-05-31
Publication date: 2015-11-24
Anticipated expiration: 2031-05-31
Also published as: JP2012252406A

Description

本発明は、車載された車両内部情報によって自車両の進行方向前方の道路形状を予測する道路形状予測技術に関する。 The present invention relates to a road shape prediction technique for predicting a road shape ahead of a traveling direction of a host vehicle based on vehicle internal information.

特許文献１には、自車両の車速、操舵角、ヨーレートを検出し、それらの検出値をカルマンフィルタに通して、車両状態として道路曲率および曲率変化を推定する道路曲率推定装置が記載されている。
具体的には、特許文献１では、車速およびヨーレートを観測して、第１のカルマンフィルタを用いて加速度および角加速度を推定し、これらの観測値及び推定値（車速推定値、ヨーレート推定値、加速度推定値、角加速度推定値）を用いて、曲率および曲率変化率を算出している。 Patent Document 1 describes a road curvature estimation apparatus that detects a vehicle speed, a steering angle, and a yaw rate of a host vehicle, passes the detected values through a Kalman filter, and estimates a road curvature and a curvature change as a vehicle state.
Specifically, in Patent Document 1, vehicle speed and yaw rate are observed, acceleration and angular acceleration are estimated using a first Kalman filter, and these observed values and estimated values (vehicle speed estimated value, yaw rate estimated value, acceleration) are estimated. The curvature and the curvature change rate are calculated using the estimated value and the angular acceleration estimated value).

また、上記算出した曲率および曲率変化を第２のカルマンフィルタに入力して、曲率および曲率変化の推定値を算出する構成となっている。
そして、上記各カルマンフィルタの計算過程で、車速推定値、加速度推定値および計算更新周期を用いた距離発展の差分方程式に変換して算出を実施している。 In addition, the calculated curvature and curvature change are input to the second Kalman filter, and the estimated value of the curvature and curvature change is calculated.
In the calculation process of each Kalman filter, the calculation is performed by converting into a difference equation of distance development using the vehicle speed estimation value, the acceleration estimation value, and the calculation update period.

特表２００７−５１６９０６号公報Special table 2007-516906 gazette

一般に上記ヨーレートセンサの観測値には、観測誤差を補正するオフセット補正が重畳される。しかし、カルマンフィルタは、ノイズが平均値０、分散σ²の正規分布に従うという仮定の下で、最小分散不偏推定量を与えるため、この正規分布の仮定に合わないオフセット補正分は除去できない。そのためカルマンフィルタ用として、特別にオフセット補正する仕組みがなければオフセット分の誤差が重畳して推定誤差が悪化する。
本発明は、上記のような点に着目したもので、車載された車両内部情報に基づき、より精度良く進行方向前方の道路形状を予測することを目的とする。 In general, an offset correction for correcting an observation error is superimposed on the observation value of the yaw rate sensor. However, since the Kalman filter gives the minimum variance unbiased estimator under the assumption that the noise follows a normal distribution with an average value of 0 and a variance σ ^2, an offset correction that does not match the normal distribution assumption cannot be removed. Therefore, if there is no special offset correction mechanism for the Kalman filter, an error for the offset is superimposed and the estimation error is deteriorated.
The present invention pays attention to the above points, and an object of the present invention is to predict a road shape ahead in the traveling direction with higher accuracy based on vehicle internal information mounted on a vehicle.

上記課題を解決するために、本発明は、車速と操舵角とに基づき、車両状態量として、車両が通過した軌跡の接線方向である車両姿勢角と、車両が通過した軌跡の曲率である車両軌跡曲率とを算出し、操舵角と、その距離微分値基づき、予め定めた演算式に従って、上記車両軌跡曲率の変化である車両軌跡曲率変化を算出する。また、本発明は、上記算出した車両状態量と車両軌跡曲率変化に基づき、予め定めた演算式に従って、現在地の道路形状の推定値として、車両が走行している道路を曲線で表したときのその曲線の接線方向である道路曲線接線方向と、車両が走行している道路を曲線で表したときのその曲線の曲率である道路曲率と、その道路曲率の変化である道路曲率変化とを推定する。そして、本発明は、上記推定した道路形状の推定値から車両進行方向前方の道路形状を予測する。 In order to solve the above problems, the present invention is based on the vehicle speed and the steering angle, and as a vehicle state quantity , a vehicle attitude angle that is a tangential direction of a trajectory passed by the vehicle and a curvature of the trajectory passed by the vehicle. calculating a trajectory curvature, the steering angle,-out the distance differential value group Dzu, according to a predetermined arithmetic expression to calculate the vehicle trajectory curvature change is the change in the vehicle trajectory curvature. In addition, the present invention is based on the calculated vehicle state quantity and the change in the vehicle trajectory curvature according to a predetermined arithmetic expression when the road on which the vehicle is traveling is represented by a curve as an estimated value of the road shape of the current location . estimation and road curves tangentially a tangential direction of the curve, and the road curvature is the curvature of the curve when representing the road on which the vehicle is traveling a curve, the road curvature change is a change in the road curvature To do. And this invention estimates the road shape ahead of a vehicle advancing direction from the estimated value of the said estimated road shape.

本発明によれば、車輪速と操舵角の情報を用いて車両状態量と車両軌跡曲率変化を算出し、これに基づき道路形状を予測し、前方道路形状を予測する。このため、本発明によれば、精度良く前方道路の形状を予測することが可能となる。 According to the present invention, the vehicle state quantity and the vehicle trajectory curvature change are calculated using the information on the wheel speed and the steering angle, the road shape is predicted based on this, and the forward road shape is predicted. For this reason, according to the present invention, the shape of the road ahead can be predicted with high accuracy.

本発明に基づく実施形態に係る車両の構成を示す図である。It is a figure which shows the structure of the vehicle which concerns on embodiment based on this invention. 道路形状予測ＥＣＵの構成を示す図である。It is a figure which shows the structure of road shape prediction ECU. 道路形状予測ＥＣＵでの本実施形態に係る処理を示すフローチャートである。It is a flowchart which shows the process which concerns on this embodiment in road shape prediction ECU. 車両状態量算出部の処理を示すフローチャートである。It is a flowchart which shows the process of a vehicle state quantity calculation part. 誤差処理部の処理を示すフローチャートである。It is a flowchart which shows the process of an error process part. 本発明に基づく実施形態に係るブロック図と対応する数式を示す図である。It is a figure which shows the numerical formula corresponding to the block diagram which concerns on embodiment based on this invention. 本発明に基づく実施形態に係るカントがついた直進路を示す図である。It is a figure which shows the straight path | route with the cant which concerns on embodiment based on this invention. 本発明に基づく実施形態に係る道路形状推定部のカルマンフィルタによる現在値の推定ブロックを示す図である。It is a figure which shows the estimation block of the present value by the Kalman filter of the road shape estimation part which concerns on embodiment based on this invention. 本発明に基づく実施形態に係る道路形状予測部のカルマンフィルタによる未来値の予測ブロックを示す図である。It is a figure which shows the prediction block of the future value by the Kalman filter of the road shape estimation part which concerns on embodiment based on this invention. 横ずれ量と車線逸脱との関係を示す図である。It is a figure which shows the relationship between lateral deviation | shift amount and lane departure. 一定舵角で車線維持できる車両軌道を説明する図である。It is a figure explaining the vehicle track which can maintain a lane with a fixed rudder angle. 車線維持可能な軌道の曲率分布を示す図である。It is a figure which shows the curvature distribution of the track | truck which can maintain a lane. カルマンフィルタをゲインスケジュールする場合のブロック構成を示す図である。It is a figure which shows the block structure in the case of carrying out the gain schedule of a Kalman filter. 道路形状予測を示す概念図である。It is a conceptual diagram which shows road shape prediction.

次に本発明の実施形態について図面を参照しつつ説明する。
（構成）
本実施形態の車両は、運転者の運転操作に応じた状態に車両挙動が制御される。車両は、制駆動制御などによって自動走行させる構成を備えていても良い。
車両は、駆動制御装置、及び制動制御装置を備える。駆動制御装置は、運転者が操作するアクセルペダルのアクセル開度に応じた目標駆動力となるように、エンジンやモータなどの駆動源を制御する。制動制御装置は、運転者が操作するブレーキペダルのブレーキ開度に応じた目標制動力となるように、各輪の制動装置を制御する。 Next, embodiments of the present invention will be described with reference to the drawings.
(Constitution)
In the vehicle of this embodiment, the vehicle behavior is controlled in a state corresponding to the driving operation of the driver. The vehicle may have a configuration for automatically traveling by braking / driving control or the like.
The vehicle includes a drive control device and a braking control device. The drive control device controls a drive source such as an engine or a motor so as to obtain a target drive force according to the accelerator opening of the accelerator pedal operated by the driver. The braking control device controls the braking device of each wheel so as to obtain a target braking force according to the brake opening degree of the brake pedal operated by the driver.

また、車両は、図１に示すように、運転者が操作するステアリングホイール１の操舵角に応じて、操舵装置２が操向輪３（図１では前輪）を転舵する。
車両は、図１に示すように、操舵角センサ４、車輪速センサ５、道路形状予測ＥＣＵ６を備える。
操舵角センサ４は、ステアリングホイール１やステアリング軸の回転角を検出することで操舵角情報を検出する。検出した操舵角情報は道路形状予測ＥＣＵ６に出力される。 Further, as shown in FIG. 1, in the vehicle, the steering device 2 steers the steered wheel 3 (the front wheel in FIG. 1) according to the steering angle of the steering wheel 1 operated by the driver.
As shown in FIG. 1, the vehicle includes a steering angle sensor 4, a wheel speed sensor 5, and a road shape prediction ECU 6.
The steering angle sensor 4 detects steering angle information by detecting the rotation angle of the steering wheel 1 and the steering shaft. The detected steering angle information is output to the road shape prediction ECU 6.

車輪速センサ５は、車輪速情報を検出し、その検出値を道路形状予測ＥＣＵ６に出力する。車輪速センサ５は、車輪の回転に応じて、予め設定した回転角、つまりその回転角に対応する車両の移動距離毎に発生するパルス情報に基づく車両の走行距離から車速を検出する。
道路形状予測ＥＣＵ６は、マイクロコンピュータおよびその周辺回路を備えるコントローラである。この道路形状予測ＥＣＵ６は、図２に示すように、機能的には、少なくとも車両状態量算出部１０、道路形状推定部１１、道路形状予測部１２を備える。 The wheel speed sensor 5 detects wheel speed information and outputs the detected value to the road shape prediction ECU 6. The wheel speed sensor 5 detects the vehicle speed from the travel distance of the vehicle based on the preset rotation angle, that is, the pulse information generated for each vehicle travel distance corresponding to the rotation angle, according to the rotation of the wheel.
The road shape prediction ECU 6 is a controller including a microcomputer and its peripheral circuits. As shown in FIG. 2, the road shape prediction ECU 6 functionally includes at least a vehicle state quantity calculation unit 10, a road shape estimation unit 11, and a road shape prediction unit 12.

車両状態量算出部１０は、車速と操舵角の情報に基づき車両状態量を算出する。その算出する車両状態量は、車両姿勢角、車両軌跡曲率、及び上記操舵角の距離微分に基づく車両軌跡曲率変化である。この上記車両状態量算出部１０は、車両状態量算出本体部１０Ａ、車両軌跡曲率変化算出部１０Ｂ、及び誤差補正部１０Ｃを備える。
車両状態量算出本体部１０Ａは、車輪速センサ５が検出した車輪速情報と、操舵センサが検出した操舵情報とに基づき、車両姿勢角と車両軌跡曲率を算出する。本実施形態では、車両姿勢角として、車両軌跡接線方向を算出する。この車両軌跡接線方向は、車両姿勢角を表している。 The vehicle state quantity calculation unit 10 calculates the vehicle state quantity based on information on the vehicle speed and the steering angle. The calculated vehicle state quantity is a vehicle trajectory curvature change based on a vehicle attitude angle, a vehicle trajectory curvature, and a distance derivative of the steering angle. The vehicle state quantity calculation unit 10 includes a vehicle state quantity calculation main body 10A, a vehicle trajectory curvature change calculation unit 10B, and an error correction unit 10C.
The vehicle state quantity calculation main body 10A calculates the vehicle attitude angle and the vehicle trajectory curvature based on the wheel speed information detected by the wheel speed sensor 5 and the steering information detected by the steering sensor. In this embodiment, the vehicle locus tangent direction is calculated as the vehicle attitude angle. This vehicle locus tangent direction represents the vehicle attitude angle.

車両軌跡曲率変化算出部１０Ｂは、操舵角センサ４が検出した操舵角と、操舵角を距離微分した値とを用いて車両軌跡曲率変化を算出する。
誤差補正部１０Ｃは、路面カントやうねりなどの平坦路に対する走行路面形状変化や、スリップや車輪空転などの路面に対する車輪の滑り状態からなる、車両状態算出誤差の情報を検出し、その検出した車両状態算出誤差が予め設定した補正閾値以上と判定すると、その検出した車両状態算出誤差の情報に基づき、車両状態量算出本体部１０Ａ及び車両軌跡曲率変化算出部１０Ｂが算出する車両状態量を補正する。 The vehicle trajectory curvature change calculation unit 10B calculates the vehicle trajectory curvature change using the steering angle detected by the steering angle sensor 4 and the value obtained by differentiating the steering angle.
The error correction unit 10C detects vehicle state calculation error information including a change in traveling road surface shape with respect to a flat road such as a road surface cant and undulation, and a slip state of a wheel with respect to a road surface such as slip and wheel idling, and the detected vehicle When it is determined that the state calculation error is equal to or greater than a preset correction threshold, the vehicle state amount calculated by the vehicle state amount calculation main body 10A and the vehicle locus curvature change calculation unit 10B is corrected based on the detected vehicle state calculation error information. .

このとき、誤差補正部１０Ｃは、上記検出した車両状態算出誤差が、上記補正閾値よりも大きな値として予め設定した補正限界閾値を越えたと判定した場合には、補正限界を超えた事を示す補正限界状態フラグをＯＮとすると共に、上記補正処理を実施しない。なお、補正限界状態フラグは、初期値がＯＦＦとなっている。
道路形状推定部１１は、車両状態量算出部１０で算出した車両状態量である、車両姿勢角、車両軌跡曲率、及び車両軌跡曲率変化から、現在走行中の道路の道路形状の推定値を推定する。上記道路形状の推定値は、道路曲線接線方向、道路曲率、及び道路曲率変化である。 At this time, if the error correction unit 10C determines that the detected vehicle state calculation error has exceeded a correction limit threshold set in advance as a value larger than the correction threshold, a correction indicating that the correction limit has been exceeded. While the limit state flag is turned ON, the above correction process is not performed. The initial value of the correction limit state flag is OFF.
The road shape estimation unit 11 estimates the estimated value of the road shape of the currently traveling road from the vehicle attitude angle, the vehicle trajectory curvature, and the vehicle trajectory curvature change, which are the vehicle state quantities calculated by the vehicle state quantity calculation unit 10. To do. The estimated value of the road shape is the road curve tangent direction, the road curvature, and the road curvature change.

道路形状予測部１２は、道路形状推定部１１が推定した道路形状の推定値から、予め設定した距離前方までの車両進行方向前方の道路形状を予測する。車両進行方向前方の道路形状の予測値は、現在位置から予め設定した前方距離若しくは当該前方距離までの道路形状に対する、車両進行方向前方の道路の道路曲線接線方向、道路曲率、道路曲率変化である。 The road shape prediction unit 12 predicts a road shape ahead of the vehicle traveling direction up to a preset distance from the estimated value of the road shape estimated by the road shape estimation unit 11. The predicted value of the road shape ahead of the vehicle traveling direction is the road curve tangent direction, the road curvature, and the road curvature change of the road ahead of the vehicle traveling direction with respect to the preset forward distance from the current position or the road shape up to the front distance. .

次に道路形状予測ＥＣＵ６における道路形状予測の処理を、図３を参照して説明する。
先ずステップＳ１０にて操舵角及び車輪速パルスを検出する。
次に、ステップＳ２０にて、車両状態量算出部１０が、車両状態量を算出する。
次に、ステップＳ３０にて、道路形状推定部１１が、車両状態量算出部１０が算出した車両状態量に基づき道路形状の推定値を求める。
次に、ステップＳ４０にて、道路形状予測部１２が、道路形状推定部１１が推定した道路形状の推定値に基づき、予め設定した前方距離までの道路形状を予測する。その後復帰する。 Next, road shape prediction processing in the road shape prediction ECU 6 will be described with reference to FIG.
First, in step S10, a steering angle and a wheel speed pulse are detected.
Next, in step S20, the vehicle state quantity calculator 10 calculates the vehicle state quantity.
Next, in step S <b> 30, the road shape estimation unit 11 obtains an estimated value of the road shape based on the vehicle state quantity calculated by the vehicle state quantity calculation unit 10.
Next, in step S40, the road shape prediction unit 12 predicts the road shape up to a preset forward distance based on the estimated value of the road shape estimated by the road shape estimation unit 11. Then return.

次に、車両状態量算出部１０の処理について図４を参照して説明する。
まずステップＳ１００にて、車輪速センサ５からのパルス信号に基づき車両の移動距離を算出する。
次に、ステップＳ１１０にて、車両状態量算出本体部１０Ａが、ステップＳ１００にて求めた移動距離に基づき、車両軌跡接線方向及び曲率を算出する。
次に、ステップＳ１２０にて、操舵角の時間微分を算出する。
次に、ステップＳ１３０にて、車両軌跡曲率変化算出部１０Ｂが、ステップＳ１２０にて算出した操舵角の時間微分に基づき車両軌跡曲率変化を算出する。その後復帰する。 Next, the process of the vehicle state quantity calculation unit 10 will be described with reference to FIG.
First, in step S100, the moving distance of the vehicle is calculated based on the pulse signal from the wheel speed sensor 5.
Next, in step S110, the vehicle state quantity calculation main body 10A calculates the vehicle locus tangent direction and the curvature based on the movement distance obtained in step S100.
Next, in step S120, a time derivative of the steering angle is calculated.
Next, in step S130, the vehicle trajectory curvature change calculating unit 10B calculates a vehicle trajectory curvature change based on the time derivative of the steering angle calculated in step S120. Then return.

次に、誤差補正部１０Ｃの処理について図５を参照して説明する。
まずステップＳ２００にて、車両状態算出誤差となる道路形状を検出する。
次に、ステップＳ２１０にて、車輪のスリップに起因する車両状態算出誤差の要因を検出する。
次に、ステップＳ２２０にて、予め設定した補正閾値以上の車両状態算出誤差があると判定すると補正項を算出する。後述のように、例えば評価関数値Ｊを演算し、その評価関数値Ｊが補正閾値Ｔｈ１以上の場合に補正項を算出する。 Next, the processing of the error correction unit 10C will be described with reference to FIG.
First, in step S200, a road shape that causes a vehicle state calculation error is detected.
Next, in step S210, a factor of a vehicle state calculation error caused by wheel slip is detected.
Next, when it is determined in step S220 that there is a vehicle state calculation error equal to or greater than a preset correction threshold, a correction term is calculated. As will be described later, for example, the evaluation function value J is calculated, and the correction term is calculated when the evaluation function value J is equal to or greater than the correction threshold Th1.

次に、ステップＳ２３０にて、予め設定した補正限界閾値Ｔｈ２（＞Ｔｈ１）を超える誤差があるか否かを判定する。条件を満足する場合には、ステップＳ２５０に移行する。条件を満足しない場合にはステップＳ２４０に移行する。
ステップＳ２４０では、補正閾値Ｔｈ１以上の誤差がある場合には、補正項で車両状態量を補正する。その後復帰する。
ステップＳ２５０では、補正限界状態フラグをＯＮにする。その後復帰する。補正限界状態フラグがＯＮの場合には、例えば、過去の情報をクリアして、道路形状予測のための計算をし直す。 Next, in step S230, it is determined whether there is an error exceeding a preset correction limit threshold Th2 (> Th1). If the condition is satisfied, the process proceeds to step S250. If the condition is not satisfied, the process proceeds to step S240.
In step S240, if there is an error greater than or equal to the correction threshold Th1, the vehicle state quantity is corrected using the correction term. Then return.
In step S250, the correction limit state flag is turned ON. Then return. When the correction limit state flag is ON, for example, past information is cleared and calculation for road shape prediction is performed again.

以下、上述の各処理について詳説する。
図６は、本実施形態における処理のブロック図と対応する数式を示す図である。
（車両状態量算出部１０での車両状態量の算出について）
次に、車両状態量算出部１０で算出する車両軌跡接線方向（車両姿勢角）、車両軌跡曲率、及び車両軌跡曲率変化の算出について説明する。 Hereinafter, each process described above will be described in detail.
FIG. 6 is a diagram showing mathematical expressions corresponding to the block diagram of the processing in the present embodiment.
(About the calculation of the vehicle state quantity in the vehicle state quantity calculation unit 10)
Next, calculation of the vehicle trajectory tangent direction (vehicle attitude angle), the vehicle trajectory curvature, and the vehicle trajectory curvature change calculated by the vehicle state quantity calculation unit 10 will be described.

車体速Ｖは、下記（１）式に基づき、左右の後輪車輪速Ｖｌ、Ｖｒから求めることが出来る。
Ｖ＝（Ｖｌ＋Ｖｒ）／２・・・（１）
ここで、（１）式における左右の後輪車輪速Ｖｌ、Ｖｒは、タイヤ径に基づき車輪速を車体速に換算した値である。 The vehicle body speed V can be obtained from the left and right rear wheel speeds Vl and Vr based on the following equation (1).
V = (Vl + Vr) / 2 (1)
Here, the left and right rear wheel speeds Vl and Vr in the equation (1) are values obtained by converting the wheel speed into the vehicle body speed based on the tire diameter.

また、ヨーレートωは、左右の後輪車輪速Ｖｌ、Ｖｒを用いて式（２）で表すことが出来る。Ｔｒｄ［ｍ］は、後輪トレッド長をトレッド値である。
ω ＝（Ｖｌ−Ｖｒ）／Ｔｒｄ・・・（２）
さらに、曲率ρ＝ω／Ｖと表すことができる。 Further, the yaw rate ω can be expressed by Expression (2) using the left and right rear wheel speeds Vl and Vr. Trd [m] is the tread value of the rear wheel tread length.
ω = (Vl−Vr) / Trd (2)
Furthermore, it can be expressed as curvature ρ = ω / V.

これらに基づき、車両軌跡の曲率は、（３）式によって求めることが出来る。
ρ ＝２（Ｖｌ−Ｖｒ）／｛Ｔｒｄ・（Ｖｌ＋Ｖｒ）｝・・・（３）
また、Ｖ＝（ｄＬ／ｄｔ）と表せることがから、（１）式は（４）式のように表現できる。
ｄＬ＝（ｄＬｌ＋ｄＬｒ）／２・・・（４）
ここでｄＬは車体の移動距離、ｄＬｌ、ｄＬｒは左右の後輪車輪移動距離を表している。ｄＬｌ、ｄＬｒは後輪車輪速センサ５の情報で検知出来る。 Based on these, the curvature of the vehicle trajectory can be obtained by equation (3).
ρ = 2 (Vl−Vr) / {Trd · (Vl + Vr)} (3)
Since V = (dL / dt) can be expressed, equation (1) can be expressed as equation (4).
dL = (dLl + dLr) / 2 (4)
Here, dL represents the moving distance of the vehicle body, and dLl and dLr represent the moving distances of the left and right rear wheels. dLl and dLr can be detected by information from the rear wheel speed sensor 5.

また、（３）式は、操舵角に基づき（５）式で表すことが出来る。
ρ ＝２（ｄＬｌ−ｄＬｒ）／｛Ｔｒｄ・（ｄＬｌ＋ｄＬｒ）｝・・・（５）
また、（６）式に基づき、操舵角δから車両軌跡の曲率ρを算出することが出来る。
ρ ＝ｔａｎ（δ／Ｇｒ）／Ｌｗ・・・・（６）
ここで、
Ｌｗ［ｍ］：ホイールベース長
Ｇｒ：ステアリングホイールとタイヤの転舵角間のギア比
である。
ここで、（６）式を距離微分すると（７）式が得られる。 Moreover, (3) Formula can be represented by (5) Formula based on a steering angle.
ρ = 2 (dLl−dLr) / {Trd · (dLl + dLr)} (5)
Further, the curvature ρ of the vehicle trajectory can be calculated from the steering angle δ based on the equation (6).
ρ = tan (δ / G r ) / Lw (6)
here,
Lw [m]: Wheel base length Gr: Gear ratio between the steering wheel and the turning angle of the tire.
Here, the equation (7) is obtained by differentiating the equation (6) by distance.

（４）式を用いて、（５）式及び（７）式を、オイラー法により近似して差分方程式で表現すると、下記（８）式、（９）式、（１０）式が得られる。 Using the equation (4), when the equations (5) and (7) are approximated by the Euler method and expressed as a difference equation, the following equations (8), (9), and (10) are obtained.

ここで、
θｎ：車両軌跡接線方向
ρｎ：車両軌跡曲率
ζｎ：車両軌跡曲率変化
を表している。また、添え字ｎは、車両移動距離ｄＬ毎に増加する。
そして、車両状態量算出部１０は、（１０）式を算出して、上記（８）式及び（９）式を車両移動距離ｄＬ毎に数値積分することにより、車両状態量を算出する。 here,
θn: vehicle track tangent direction ρn: vehicle track curvature ζn: vehicle track curvature change Further, the subscript n increases for each vehicle movement distance dL.
And the vehicle state quantity calculation part 10 calculates a vehicle state quantity by calculating (10) Formula and carrying out numerical integration of the said (8) Formula and (9) Formula for every vehicle moving distance dL.

以上の結果に基づき、車両状態量算出部１０は、（８）式〜（１０）式によって、車両移動距離ｄＬ毎に、車両状態量として、車両軌跡接線方向、車両軌跡曲率、車両軌跡曲率変化を求める。
ここで、上記車両状態量を算出する際のモデルは、車両が平坦な舗装路面を走行し、且つ、横滑りなどの車輪の滑りが発生していないという条件でのモデルである。
このため、上記車両状態量の算出に影響を与えるだけの走行路面形状変化が存在する場合、若しくは上記車両状態量の算出に影響を与えるだけの走行路面に対する車輪の滑りを検出すると、誤差補正部１０Ｃは、車両状態量を補正する。 Based on the above results, the vehicle state quantity calculation unit 10 calculates the vehicle state tangent direction, the vehicle locus curvature, and the vehicle locus curvature change as the vehicle state amount for each vehicle movement distance dL using the equations (8) to (10). Ask for.
Here, the model for calculating the vehicle state quantity is a model under the condition that the vehicle travels on a flat paved road surface and no wheel slip such as a side slip occurs.
Therefore, if there is a change in the shape of the traveling road surface that affects the calculation of the vehicle state quantity, or if a slip of the wheel with respect to the traveling road surface that affects the calculation of the vehicle state quantity is detected, an error correction unit 10C corrects the vehicle state quantity.

車両状態算出誤差になりうる走行路面形状として路面カントやうねりなどが存在する。また、車両状態算出誤差になりうる走行路面に対する車輪の滑りとなりうるものとして、横滑りや片流れ、スリップなどの現象がある。そして、誤差補正部１０Ｃは、このようなことで発生する車両状態算出誤差が予め設定した補正閾値以上となったことを検出すると、例えば（８）式〜（１０）式で示される上記算出した車両状態量、若しくは観測値としての車速値や操舵角を補正する。 There are road surface cants and swells as traveling road surface shapes that may cause vehicle state calculation errors. Further, there are phenomena such as side slip, one-sided flow, slip, etc. that may cause the wheel to slip on the road surface that may cause a vehicle state calculation error. Then, when the error correction unit 10C detects that the vehicle state calculation error caused by such a situation is equal to or greater than a preset correction threshold, the above-described calculation represented by, for example, Expressions (8) to (10) is performed. The vehicle state quantity or the vehicle speed value or the steering angle as an observed value is corrected.

上記誤差補正部１０Ｃの処理について補足説明する。
路面カントやうねり等の車両状態算出誤差要因の有無を検出する。予め設定した平坦路に対する、予め設定した以上の路面形状変動が存在すると判定すると、車両状態算出誤差要因があると判定する。
車両状態算出誤差は、例えば次のようにして検出する。
すなわち、後輪車輪速センサ５から算出した車両軌跡状態量（（５）式に基づく後輪車輪速センサ５から求めた曲率ρ_w）と前輪舵角から算出した車両軌跡状態量（（６）式に基づく操舵角から求めた曲率ρ_s）とを算出し、その偏差（ρ_w-ρ_s）に基づき、路面カントやうねりなどの車両状態算出誤差を検出する。 A supplementary description will be given of the processing of the error correction unit 10C.
The presence or absence of a vehicle state calculation error factor such as a road surface cant or swell is detected. If it is determined that there is more road surface shape variation than the preset flat road, it is determined that there is a vehicle state calculation error factor.
The vehicle state calculation error is detected as follows, for example.
That is, the vehicle trajectory state quantity calculated from the rear wheel speed sensor 5 (curvature ρ _w obtained from the rear wheel speed sensor 5 based on the formula (5)) and the vehicle trajectory state quantity calculated from the front wheel steering angle ((6) The curvature ρ _s ) obtained from the steering angle based on the equation is calculated, and vehicle state calculation errors such as road surface cant and swell are detected based on the deviation (ρ _w −ρ _s ).

誤差補正部１０Ｃは、例えば、予め設定した一定走行距離L[ｍ]毎に、逐次、下記（１１）式に基づき、車両状態算出誤差として評価関数値Jを評価する、そして、その評価関数値Ｊが予め設定した補正閾値Ｔｈ１以上となったと判定したら、上記車両状態量を補正する。 The error correction unit 10C, for example, sequentially evaluates the evaluation function value J as a vehicle state calculation error based on the following equation (11) for each predetermined constant travel distance L [m], and the evaluation function value If it is determined that J is equal to or greater than the preset correction threshold Th1, the vehicle state quantity is corrected.

また、誤差補正部１０Ｃは、上記式で示した評価関数値Jが、上記補正閾値Ｔｈ１よりも大きい値として予め設定した補正限界閾値Ｔｈ２を超えた場合に、補正限界と判定する。
ここで、路面カントによる路面形状誤差要因を例にして補正の一例を説明する。
ここで、図７に示すような路面カントがついた直進路を走行した場合について考える。図７の路面では、紙面上側の路面高さが高く且つ紙面下側が低いとする。 Further, the error correction unit 10C determines that the evaluation limit is the correction limit when the evaluation function value J indicated by the above formula exceeds the correction limit threshold Th2 set in advance as a value larger than the correction threshold Th1.
Here, an example of correction will be described by taking a road surface shape error factor caused by a road surface cant as an example.
Here, consider the case where the vehicle travels on a straight road with a road surface cant as shown in FIG. In the road surface of FIG. 7, it is assumed that the road surface height on the upper side of the paper is high and the lower side of the paper is low.

この路面を、左から右に走行する際に、ハンドルを中立状態のままにして走行すると、路面の高低差の影響により車両軌跡は実線の軌跡をたどる。従って、この路面を直進するためには、左側に補舵を加える必要がある。ここで、適切な補舵を加えた結果、実線で示す直進軌跡が得られたとする。この場合、舵角を用いて算出した車両軌跡は、一点鎖線の軌跡となり、後輪車輪速センサ５を用いて算出した車両軌跡は実線の軌跡となる。つまり、両者の軌跡の曲率が異なる事から、この差分を補正項として、上記（８）式〜（１０）式で表される算出した車両状態量を補正する。 When the vehicle is traveling from left to right with the steering wheel in a neutral state, the vehicle track follows a solid track due to the difference in level of the road surface. Therefore, in order to go straight on this road surface, it is necessary to add a steering to the left side. Here, it is assumed that a straight traveling locus indicated by a solid line is obtained as a result of adding appropriate steering. In this case, the vehicle trajectory calculated using the rudder angle is a one-dot chain trajectory, and the vehicle trajectory calculated using the rear wheel speed sensor 5 is a solid trajectory. That is, since the curvatures of the two trajectories are different, the calculated vehicle state quantity expressed by the above equations (8) to (10) is corrected using this difference as a correction term.

なお、通常走行状態では、横滑りが発生するほどのカントやうねりは通常の舗装道路上には存在しないので、車体は横滑りしないと仮定する。上記車両状態を求めるためのモデルは、平坦路面を仮定したモデルを用いているため、路面カントやうねりがある場合に、先に示したような推定誤差が生じる。
また通常、横滑りは車速が大きい（概ね５０ｋｍ／ｈ以上）状態で旋回した場合に、タイヤのグリップ力が飽和することにより発生する。 In the normal running state, it is assumed that the body does not skid because there is no cant or undulation that causes skidding on the ordinary paved road. Since the model for obtaining the vehicle state uses a model assuming a flat road surface, an estimation error as described above occurs when there is a road surface cant or undulation.
In general, skidding occurs when the tire grip force is saturated when the vehicle turns at a high vehicle speed (approximately 50 km / h or more).

車体の横滑り成分は、他のセンサ情報（例えば、加速度センサやヨーレートセンサ）の情報を用いて横滑り成分を推定すれば良い。そして、推定した横すべり成分を補正項として補正する。
すなわち、誤差補正部１０Ｃは、横滑り成分を検出し、その検出した横滑り分を補正項として、上記（８）式〜（１０）式で表される算出した車両状態量を補正する。 The side slip component of the vehicle body may be estimated using information from other sensor information (for example, an acceleration sensor or a yaw rate sensor). Then, the estimated side slip component is corrected as a correction term.
That is, the error correction unit 10C detects a skid component, and corrects the calculated vehicle state amount expressed by the above equations (8) to (10) using the detected skid amount as a correction term.

（道路形状推定部１１及び道路形状予測部１２について）
次に、道路形状推定部１１における道路曲線接線方向と道路曲率と道路曲率変化の推定方法について説明する。
道路形状推定部１１では、下記の（１１）式、（１２）式で表されるダイナミクスに基づき、（１３）式で表すカルマンフィルタを用いて、上記車両状態量算出部１０が算出した車両軌跡接線方向推定値θn［ｒａｄ／ｍ］、車両軌跡曲率ρn ［ｒａｄ／ｍ］、車両軌跡曲率変化ζn［ｒａｄ／ｍ2］から、道路曲線接線方向推定値＾θn［ｒａｄ／ｍ］、道路曲線曲率＾ρn［ｒａｄ／ｍ］、道路曲線曲率変化＾ζn［ｒａｄ／ｍ2］を推定する。
ここで、Ｆｎは車両線型モデル、Ｈｎは観測モデル、Ｑｎ、Ｒｎが共分散行列、ｗｎ、ｖｎが雑音、Ｐが誤差の共分散行列、Ｋｎがカルマンゲインである。 (About the road shape estimation unit 11 and the road shape prediction unit 12)
Next, a method of estimating the road curve tangent direction, the road curvature, and the road curvature change in the road shape estimation unit 11 will be described.
The road shape estimation unit 11 uses the Kalman filter represented by the equation (13) based on the dynamics represented by the following equations (11) and (12) to calculate the vehicle trajectory calculated by the vehicle state quantity calculation unit 10. From the tangential direction estimated value θn [rad / m], the vehicle trajectory curvature ρn [rad / m], and the vehicle trajectory curvature change ζn [rad / m2], the road curve tangent direction estimated value ^ θn [rad / m], the road curve curvature ^ Ρn [rad / m] and road curve curvature change ^ ζn [rad / m2] are estimated.
Here, Fn is a vehicle linear model, Hn is an observation model, Qn and Rn are covariance matrices, wn and vn are noises, P is an error covariance matrix, and Kn is a Kalman gain.

次に、道路形状予測部１２における前方道路形状予測方法について説明する。
道路形状予測部１２では、上記道路形状推定部１１で推定した道路曲線接線方向推定値＾θ_n［ｒａｄ／ｍ］、道路曲線曲率＾ρ_n［ｒａｄ／ｍ］、道路曲線曲率変化＾ζ_n［ｒａｄ／ｍ²］と、これらの推定値を数値積分することで、未来の予測誤差を算出し、その予測誤差としての誤差共分散行列Ｐから（１４）式の第１式、第２式を用いて算出することで、進行方向前方の形状予測を行う。 Next, a front road shape prediction method in the road shape prediction unit 12 will be described.
The road shape prediction unit 12 estimates the road curve tangent direction ^ θ _n [rad / m], road curve curvature ^ ρ _n [rad / m], and road curve curvature change ^ ζ _n estimated by the road shape estimation unit 11. [Rad / m ² ] and these estimated values are numerically integrated to calculate a future prediction error, and from the error covariance matrix P as the prediction error, the first expression and the second expression of Expression (14) By calculating using, shape prediction in the forward direction is performed.

すなわち、道路形状推定部１１では、（１３）式に基づき予測及び更新をして推定することとなるが、道路形状予測部１２では、（１４）式の第１式、第２式を用いて予測する。
更に、上記道路形状推定部１１、道路形状予測部１２の処理について補足説明する。
上述のように、車両状態量算出部１０は、操舵角δおよび後輪車輪速パルスVpr、Vplを観測量として、下記（１５）式で表される車両状態量を算出する。 That is, the road shape estimation unit 11 estimates and estimates based on the equation (1 3 ), but the road shape prediction unit 12 uses the first and second equations of the equation (14). Predict.
Further, a supplementary description will be given of the processing of the road shape estimation unit 11 and the road shape prediction unit 12.
As described above, the vehicle state quantity calculation unit 10 calculates the vehicle state quantity represented by the following equation (15) using the steering angle δ and the rear wheel speed pulses Vpr and Vpl as observation quantities.

そして、道路形状推定部１１は、車両状態量算出部１０で算出した上記状態量Ｘを入力とし、カルマンフィルタにて予測及び観測更新の処理を行って、現在値の道路形状の推定を行う。さらに、道路形状予測部１２が、その推定値を入力として、カルマンフィルタにて、進行方向前方に位置する道路形状の予測を行う。 The road shape estimation unit 11 receives the state quantity X calculated by the vehicle state quantity calculation unit 10 as input, performs prediction and observation update processing by the Kalman filter, and estimates the road shape of the current value. Further, the road shape prediction unit 12 receives the estimated value as input and predicts the road shape located forward in the traveling direction by the Kalman filter.

次に、道路形状推定部１１が用いるカルマンフィルタの構成について説明する。
図８は、現在値推定に使用する道路形状推定部１１のカルマンフィルタの構成を示す。この図８から分かるように、カルマンフィルタの演算処理は２つの機能ブロックで構成される。一つ目の機能ブロックは、「予測」処理を行い、２つ目の機能ブロックは「観測更新」処理を行う。 Next, the configuration of the Kalman filter used by the road shape estimation unit 11 will be described.
FIG. 8 shows the configuration of the Kalman filter of the road shape estimation unit 11 used for the current value estimation. As can be seen from FIG. 8, the Kalman filter calculation process is composed of two functional blocks. The first functional block performs “prediction” processing, and the second functional block performs “observation update” processing.

上記予測ブロックでは、ｎステップ目の推定値＾Ｘ_nと推定誤差共分散＾Ｐ_nを基に、ｎ＋１ステップ目の予測値￣Ｘ_n+1と予測誤差共分散￣Ｐ_n+1を算出する。そして、２つ目の機能ブロックでの観測更新で、ｎ＋１ステップ目の予測値￣Ｘ_n+1とｎ＋１ステップ目の観測値Y_n+1をカルマンゲインKで重み付けしてブレンドすることによりｎ＋１ステップ目の推定値＾Ｘ_n+1と推定誤差共分散＾Ｐ_n+1を算出する。
以上の演算処理を演算ステップ毎に逐次繰り返す。ここで、これらの演算ステップは予め設定した走行距離ｄｌ[ｍ]毎に処理を実行する。 In the prediction block, based on n-th step of the estimated value ^ X _n and the estimated error covariance ^ P _n, calculates n + 1-th step predicted value ¯X n _{+ 1} and a prediction error covariance ¯P n _{+ 1} . Then, by updating the observation in the second function block, the prediction value ￣X _{n + 1} at the n + 1 step and the observation value Y _{n + 1} at the n + 1 step are weighted by the Kalman gain K and blended to make an n + 1 step. An eye estimated value ^ _{Xn + 1} and an estimated error covariance ^ _{Pn + 1} are calculated.
The above calculation process is sequentially repeated for each calculation step. Here, these calculation steps execute processing for each preset travel distance dl [m].

次に、道路形状予測部１２が用いる、つまり未来値の予測に用いているカルマンフィルタの構成について説明する。
図９は、未来値の予測に使用する道路形状予測部１２のカルマンフィルタの構成を示す。
道路形状予測部１２が使用するカルマンフィルタの構成は、図９に示すように、上記道路形状推定部１１のカルマンフィルタの構成と基本的な構成は同じある。ただし、未来値の予測では、予測の機能ブロックのみが動作する。すなわち、未来値は観測できないため、観測更新の機能ブロックは使用しない。 Next, the configuration of the Kalman filter used by the road shape prediction unit 12, that is, used for prediction of future values will be described.
FIG. 9 shows the configuration of the Kalman filter of the road shape prediction unit 12 used for prediction of future values.
The configuration of the Kalman filter used by the road shape prediction unit 12 is the same as the configuration of the Kalman filter of the road shape estimation unit 11 as shown in FIG. However, in the prediction of future values, only the functional block for prediction operates. That is, since future values cannot be observed, the observation update function block is not used.

そして、予測の演算処理はｎステップ目の推定値に基づき、所望の予測長に至るまで予測の機能ブロックの演算処理を演算ステップ毎に逐次繰り返し実施する。これらの演算ステップは予め設定した走行距離ｄｌ[ｍ]毎に処理を実行する。
例えば、上記走行距離ｄｌ＝０．１[ｍ]とし、現在から進行方向前方の道路形状予測位置を、予測長１０[ｍ]に設定したとする。この場合には、ｎステップ目の推定値を初期値として１００回ステップ分の予測演算処理を実行することにより、ｎステップ目における（ｎ＋１００）ステップ目までの予測値が算出できる事になる。言い換えると、現在位置から１０[ｍ]前方までの予測値を算出できる事になる。 Then, the calculation process of prediction is repeatedly performed sequentially for each calculation step, based on the estimated value of the nth step, until the desired prediction length is reached. These calculation steps are executed for each preset travel distance dl [m].
For example, it is assumed that the travel distance dl = 0.1 [m], and the road shape predicted position ahead of the traveling direction from the present is set to the predicted length 10 [m]. In this case, the predicted value up to the (n + 10 0 ) th step in the nth step can be calculated by executing the prediction calculation process for 100 steps using the estimated value in the nth step as an initial value. In other words, a predicted value from the current position to 10 [m] ahead can be calculated.

次に、上記道路形状推定部１１において、カルマンゲインをゲインスケジュールしながら車両の走行距離毎に道路形状を推定する場合について説明する。
道路と車両の幾何モデルを用いた車線維持可能な曲率分散値の解析として
まず、現実の道路は、道路曲率が連続して変化するというように仮定できるため、局所的には曲率一定であるとみなして解析する。 Next, a description will be given of a case where the road shape estimation unit 11 estimates a road shape for each travel distance of the vehicle while gain scheduling the Kalman gain.
As an analysis of the curvature dispersion value that can maintain the lane using the geometric model of the road and the vehicle First, it can be assumed that the road curvature changes continuously, so the curvature is locally constant. Consider it and analyze it.

いま、横滑りが発生しない低速域において、曲率半径Ｒ[ｍ]、車線幅４[ｍ]の道路での走行を考える。例えば、Ｒ５０以上の道路において車両全長５[ｍ]、全幅１．９[ｍ]とすると車線逸脱することなく走行するためには、後輪車軸中心の車線中心からの横ずれ量ｄが±０．９[ｍ]以内であれば良い。この様子を図１０に示す。
次に、曲率半径Ｒ[ｍ]、車線幅４[ｍ]の道路に対して、一定舵角で車線維持できる車両軌道を考える。図１１に一定舵角で車線維持できる車両軌道を示す。最大曲率Ｒｍaxとなる軌道をＬ４の線、最小曲率半径Ｒｍinとなる軌道をＬ５の線で示す。 Now, let us consider traveling on a road having a radius of curvature R [m] and a lane width 4 [m] in a low speed region where no side slip occurs. For example, if the vehicle has a total length of 5 [m] and a total width of 1.9 [m] on a road of R50 or higher, the lateral deviation amount d from the lane center of the rear wheel axle center is ± 0. It may be within 9 [m]. This is shown in FIG.
Next, consider a vehicle trajectory that can maintain a lane at a constant steering angle with respect to a road having a curvature radius R [m] and a lane width 4 [m]. FIG. 11 shows a vehicle track that can maintain a lane at a constant rudder angle. A trajectory having the maximum curvature Rmax is indicated by a line L4, and a trajectory having the minimum curvature radius Rmin is indicated by a line L5.

ここで、点Ｐ、Ｑを通る円の方程式からＲｍaxが得られる。また、点Ｓ、Ｒvを通る円の方程式からＲｍinが得られる。（１６）式、（１７）式にこれらの導出式を示す。
Ｒｍaxの導出：
（ｘ＋ａ）²＋（ｙ＋ａ）²＝Ｒ² _maxにＰ（Ｒ＋ｄ、０）
及びＱ（（Ｒ-ｄ）／√２、（Ｒ-ｄ）／√２）を代入して、
（ｘ＋ｄ＋ａ）²＋ａ² ＝Ｒ² _max
←→Ｒ²＋２Ｒｄ＋ｄ²＋２ａ（Ｒ＋ｄ）＋２ａ²＝Ｒ² _max
２（（Ｒ-ｄ）／√２）²＝Ｒ² _max
←→２｛１／２（Ｒ²＋２Ｒｄ＋ｄ²）＋（２／√２）ａ（Ｒ＋ｄ）＋２ａ²｝＝Ｒ² _max
・・・（１６） Here, Rmax is obtained from an equation of a circle passing through points P and Q. Rmin is obtained from the equation of a circle passing through the points S and Rv. These derivation formulas are shown in formulas (16) and (17).
Derivation of Rmax:
(X + a) ² + (y + a) ² = R ² _max to P (R + d, 0)
And Q ((Rd) / √2, (Rd) / √2) are substituted,
(X + d + a) ² + a ² = R ² _max
← → R ² + 2Rd + d ² + 2a (R + d) + 2a ² = R ² _max
2 ((R-d) / √2) ² = R ² _max
← → 2 {1/2 (R ² + 2Rd + d ² ) + (2 / √2) a (R + d) + 2a ² } = R ² _max
... (16)

辺々引いて
ａ＝２Ｒｄ／（（（√２）−１）Ｒ −（（√２）＋１）ｄ）
よって、Ｒ_max ＝√２ａ＋Ｒ−ｄとなる。
Ｒｍinの導出：
（ｘ−ａ）²＋（ｙ−ａ）²＝Ｒ² _maxにＰ（Ｒ−ｄ、０）
及びＱ（（Ｒ＋ｄ）／√２、（Ｒ＋ｄ）／√２）を代入して、
（ｘ−ｄ−ａ）²＋ａ² ＝Ｒ² _min
←→Ｒ²−２Ｒｄ＋ｄ²−２ａ（Ｒ−ｄ）＋２ａ²＝Ｒ² _min
２（（Ｒ＋ｄ）／√２）²＝Ｒ² _max
←→２｛１／２（Ｒ²＋２Ｒｄ＋ｄ²）−（２／√２）ａ（Ｒ＋ｄ）＋２ａ²｝＝Ｒ² _min
・・・（１７） A = 2Rd / (((√2) −1) R − ((√2) +1) d)
Therefore, R _max = √2a + R−d.
Derivation of Rmin:
(X−a) ² + (ya) ² = R ² _max and P (R−d, 0)
And Q ((R + d) / √2, (R + d) / √2) are substituted,
(Xda) ² + a ² = R ² _min
^{^{← → R 2 -2Rd + d 2}} -2a (R-d) + 2a 2 = R 2 min
2 ((R + d) / √2) ² = R ² _max
← → 2 {1/2 (R ² + 2Rd + d ² ) − (2 / √2) a (R + d) + 2a ² } = R ² _min
... (17)

辺々引いて
ａ＝−２Ｒｄ／（（（√２）−１）Ｒ −（（√２）＋１）ｄ）
よって、Ｒ_max ＝−√２ａ＋Ｒ＋ｄとなる。
そして、（１６）式、（１７）式から、最大曲率ρ_maxおよび最小曲率ρ_minは（１８）式で表せる． A = −2Rd / (((√2) −1) R − ((√2) +1) d)
Therefore, R _max = −√2a + R + d.
From the equations (16) and (17), the maximum curvature ρ _max and the minimum curvature ρ _min can be expressed by the equation (18).

つまり、曲率半径Ｒ、逸脱限界幅ｄに対する曲率ρのとりうる値は
ρ_min≦ ρ ≦ ρ_maxとなる事が分かる。
ここで、曲率半径Ｒの車線に対して、車線維持可能な曲率分布が図１２に示すような正規分布に従うと仮定すると、その分散σρ²は（１８）式から算出できる。
ここで、道路曲率に応じてカルマンゲインをゲインスケジュールするために、現在走行中の車線の曲率半径Rを操舵角δを用いて推定する。前出の（式６）とR=1/ρの関係からRを算出し（１８）式に代入すれば、ρ_minおよびρ_maxが算出できる．例えば、下記式のようにおけば曲率分散値σρ²が算出できる。 That is, it can be seen that the possible values of the curvature ρ for the curvature radius R and the deviation limit width d are ρ _min ≦ ρ ≦ ρ _max .
Assuming that the curvature distribution capable of maintaining the lane follows a normal distribution as shown in FIG. 12 with respect to the lane having the curvature radius R, the variance σρ ² can be calculated from the equation (18).
Here, in order to schedule the Kalman gain according to the road curvature, the curvature radius R of the currently traveling lane is estimated using the steering angle δ. If R is calculated from the relationship of (Equation 6) and R = 1 / ρ and is substituted into Equation (18), ρ _min and ρ _max can be calculated. For example, the curvature dispersion value σρ ² can be calculated by the following equation.

図１３は、カルマンゲインをゲインスケジュールしながら車両の走行距離毎に道路形状を推定する際のロジックの構成を示す図である。
すなわち、曲率分散値算出部１３を備える。そして、曲率分散値算出部１３は、検出した操舵角から道路曲率を算出し、先に述べた曲率分散値σρ²を算出する。
そして、道路形状推定部１１は、この曲率分散値算出部１３が算出した曲率分散σρ²の算出値を所定の値に調整する。すなわち、現在の状態推定に用いているカルマンゲインをゲインスケジュールする構成とする。 FIG. 13 is a diagram illustrating a logic configuration when a road shape is estimated for each travel distance of the vehicle while gain scheduling of the Kalman gain.
That is, a curvature dispersion value calculation unit 13 is provided. Then, the curvature dispersion value calculation unit 13 calculates the road curvature from the detected steering angle, and calculates the curvature dispersion value σρ ² described above.
Then, the road shape estimation unit 11 adjusts the calculated value of the curvature variance σρ ² calculated by the curvature variance value calculation unit 13 to a predetermined value. In other words, a gain schedule is used for the Kalman gain used for the current state estimation.

（動作その他）
本実施形態では、車両状態量算出部１０が、車輪速と操舵角情報の情報から車両状態量と車両軌跡曲率変化を算出し、算出した車両状態量と車両軌跡曲率変化から、道路形状推定部１１が現在地の道路形状を推定し、更に、道路形状予測部１２が、道路形状推定部１１が算出した推定値を基に、進行方向前方の道路形状を予測する。 (Operation other)
In this embodiment, the vehicle state quantity calculation unit 10 calculates a vehicle state quantity and a vehicle trajectory curvature change from information on wheel speed and steering angle information, and a road shape estimation unit from the calculated vehicle state quantity and the vehicle trajectory curvature change. 11 estimates the road shape of the current location, and the road shape prediction unit 12 predicts the road shape ahead of the traveling direction based on the estimated value calculated by the road shape estimation unit 11.

図１４に、道路形状予測の概念図を示す。図１４に示すように、車両の走行軌跡から現在の道路形状を推定し、その推定値から未来の道路形状の予測値を求める。
このように、カメラなどの外界認識センサによって進行方向前方の道路の観測を行うことなく、つまり高価なセンサを使用することなく、通常車両に搭載されているセンサからの情報だけで、進行方向前方の道路形状を精度良く予測可能となる。 FIG. 14 shows a conceptual diagram of road shape prediction. As shown in FIG. 14, the current road shape is estimated from the travel locus of the vehicle, and the predicted value of the future road shape is obtained from the estimated value.
As described above, the road ahead in the traveling direction is not observed by an external recognition sensor such as a camera, that is, without using an expensive sensor, only the information from the sensor normally mounted on the vehicle is used. It is possible to accurately predict the road shape.

ここで、車輪速センサ５が車速検出部を構成する。操舵角センサ４が操舵角検出部を構成する。誤差補正部１０Ｃが第１補正部及び第２補正部を構成する。ステップＳ２３０が補正限界判定部を構成する。車両状態量算出部１０は、車両状態量算出部のほか車両軌跡曲率変化算出部を兼ねる。 Here, the wheel speed sensor 5 constitutes a vehicle speed detection unit. The steering angle sensor 4 constitutes a steering angle detector. The error correction unit 10C constitutes a first correction unit and a second correction unit. Step S230 constitutes a correction limit determination unit. The vehicle state quantity calculation unit 10 also serves as a vehicle locus curvature change calculation unit in addition to the vehicle state quantity calculation unit.

（本実施形態の効果）
（１）車速検出部が車両の車速を検出する。操舵角検出部が車両に設けられた操舵装置２の操舵角を検出する。車両状態量算出部１０は、上記車速検出部が検出した車速と上記操舵角検出部が検出した操舵角とに基づき車両状態量として、車両姿勢角、車両軌跡曲率、及び上記操舵角の距離微分に基づく車両軌跡曲率変化を算出する。道路形状推定部１１は、車両状態量算出部１０が算出した車両状態量に基づき、道路形状の推定値として道路曲線接線方向、道路曲率、及び道路曲率変化を推定する。道路形状予測部１２は、上記道路形状推定部１１が推定した道路形状の推定値から車両進行方向前方の道路形状を予測する。
車輪速と操舵角情報を用いて車両状態量と車両軌跡曲率変化を算出し、これに基づき道路形状を予測し、前方道路形状を予測する。このため、精度良く進行方向前方の道路形状予測が実現できる。 (Effect of this embodiment)
(1) The vehicle speed detection unit detects the vehicle speed of the vehicle. A steering angle detection part detects the steering angle of the steering apparatus 2 provided in the vehicle. The vehicle state quantity calculation unit 10 calculates a vehicle state angle, a vehicle locus curvature, and a distance derivative of the steering angle as a vehicle state quantity based on the vehicle speed detected by the vehicle speed detection unit and the steering angle detected by the steering angle detection unit. The vehicle locus curvature change based on is calculated. The road shape estimation unit 11 estimates the road curve tangent direction, the road curvature, and the road curvature change as the estimated value of the road shape based on the vehicle state quantity calculated by the vehicle state quantity calculation unit 10. The road shape prediction unit 12 predicts the road shape ahead of the vehicle traveling direction from the estimated value of the road shape estimated by the road shape estimation unit 11.
The vehicle state quantity and the vehicle trajectory curvature change are calculated using the wheel speed and the steering angle information, the road shape is predicted based on this, and the forward road shape is predicted. For this reason, it is possible to accurately predict the road shape ahead of the traveling direction.

（２）上記車速検出部は、車輪の回転に応じて予め設定した距離間隔毎に発生するパルス情報に基づく車両の走行距離から車速を検出する。上記車両状態量算出部１０は、予め設定した走行距離毎に操舵装置の操舵角を検出し、その検出した操舵角の距離微分に基づき上記車両軌跡曲率変化を算出する。
車両の走行距離毎に車両状態を算出し、道路形状を推定、前方道路形状を予測する。この結果、低車速時にも誤差が重畳されにくく、高精度な道路形状予測が実現できる。 (2) The vehicle speed detection unit detects the vehicle speed from the travel distance of the vehicle based on pulse information generated at each predetermined distance interval according to the rotation of the wheel. The vehicle state quantity calculation unit 10 detects the steering angle of the steering device for each preset travel distance, and calculates the vehicle locus curvature change based on the distance derivative of the detected steering angle.
The vehicle state is calculated for each travel distance of the vehicle, the road shape is estimated, and the forward road shape is predicted. As a result, errors are not easily superimposed even at low vehicle speeds, and highly accurate road shape prediction can be realized.

（３）上記道路形状推定部１１は、カルマンゲインを含むカルマンフィルタを有し、カルマンゲインは、操舵角に基づき算出した曲率分散値からなる共分散行列を含み、この曲率分散値を所定の値に調整しながら、予め設定した車両の走行距離毎に道路形状を推定する。
車両の走行距離毎に車両状態を算出し、道路形状を推定、前方道路形状を予測する。この結果、低車速時にも誤差が重畳されにくく、高精度な道路形状予測が実現できる。
また、操舵角を用いて道路形状推定に用いる曲率分散値を調整するため色々な道路形状に対しても精度良く道路形状の予測が実現できる。 (3) The road shape estimation unit 11 includes a Kalman filter including a Kalman gain, and the Kalman gain includes a covariance matrix including a curvature dispersion value calculated based on a steering angle, and the curvature dispersion value is set to a predetermined value. While adjusting, the road shape is estimated for each preset travel distance of the vehicle.
The vehicle state is calculated for each travel distance of the vehicle, the road shape is estimated, and the forward road shape is predicted. As a result, errors are not easily superimposed even at low vehicle speeds, and highly accurate road shape prediction can be realized.
Further, since the curvature dispersion value used for the road shape estimation is adjusted using the steering angle, the road shape can be accurately predicted for various road shapes.

（４）上記車両状態量算出部１０は、車両状態量の算出に対して誤差要因となる道路形状を検出し、その検出した誤差要因となる道路形状に基づき車両状態量を補正する第１補正部を備える。
路面カントやうねりなどの道路形状による誤差を補正することで、路面状況などによらず安定して道路形状の予測が実現できる。
（５）上記第１補正部が検出する道路形状が補正可能な否かを判定する補正限界判定部を備える。
これによって、予測した道路形状予測の精度が悪くなったことを検出可能となる。 (4) The vehicle state quantity calculation unit 10 detects a road shape that is an error factor in calculating the vehicle state quantity, and corrects the vehicle state quantity based on the detected road shape that is an error factor. A part.
By correcting errors due to road shapes such as road surface cants and swells, it is possible to stably predict road shapes regardless of road surface conditions.
(5) A correction limit determination unit that determines whether or not the road shape detected by the first correction unit can be corrected is provided.
This makes it possible to detect that the accuracy of the predicted road shape prediction has deteriorated.

（６）車両状態量算出部１０は、路面に対する車輪の滑りを検出し、検出した車輪の滑りに基づき車両状態量を補正する第２補正部を備える。特に、横すべりに対する補正をすることが好ましい。
車輪のスリップや車輪空転などによる誤差を補正することで、路面状況などによらず安定して道路形状の予測が実現できる。
（７）上記第２補正部が検出する滑りが補正可能な否かを判定する補正限界判定部を備える。
これによって、予測した道路形状予測の精度が悪くなったことを検出可能となる。 (6) The vehicle state quantity calculation unit 10 includes a second correction unit that detects slippage of the wheel with respect to the road surface and corrects the vehicle state quantity based on the detected wheel slippage. In particular, it is preferable to correct the side slip.
By correcting errors due to wheel slip or wheel slipping, it is possible to stably predict the road shape regardless of road conditions.
(7) A correction limit determination unit that determines whether or not the slip detected by the second correction unit can be corrected is provided.
This makes it possible to detect that the accuracy of the predicted road shape prediction has deteriorated.

２操舵装置
３操向輪
４操舵角センサ
５車輪速センサ
６道路形状予測ＥＣＵ
１０車両状態量算出部
１０Ａ車両状態量算出本体部
１０Ｂ車両軌跡曲率変化算出部
１０Ｃ誤差補正部
１１道路形状推定部
１２道路形状予測部
１３曲率分散値算出部 2 Steering device 3 Steering wheel 4 Steering angle sensor 5 Wheel speed sensor 6 Road shape prediction ECU
DESCRIPTION OF SYMBOLS 10 Vehicle state quantity calculation part 10A Vehicle state quantity calculation main-body part 10B Vehicle locus curvature change calculation part 10C Error correction part 11 Road shape estimation part 12 Road shape prediction part 13 Curvature dispersion value calculation part

Claims

車両の車速を検出する車速検出部と、
車両に設けられた操舵装置の操舵角を検出する操舵角検出部と、
上記車速検出部が検出した車速と上記操舵角検出部が検出した操舵角とに基づき、車両状態量として、車両が通過した軌跡の接線方向である車両姿勢角と、車両が通過した軌跡の曲率である車両軌跡曲率とを算出する車両状態量算出部と、
上記操舵角検出部が検出した操舵角と、その距離微分値とに基づき、前記車両軌跡曲率を求める差分方程式を距離で微分してなる予め定めた演算式に従って、上記車両軌跡曲率の変化である車両軌跡曲率変化を算出する車両軌跡曲率変化算出部と、
車両状態量算出部が算出した車両状態量と車両軌跡曲率変化算出部が算出した車両軌跡曲率変化とに基づき、予め定めた演算式であるカルマンフィルタに従って、現在地の道路形状の推定値として、車両が走行している道路を曲線で表したときのその曲線の接線方向である道路曲線接線方向と、車両が走行している道路を曲線で表したときのその曲線の曲率である道路曲率と、その道路曲率の変化である道路曲率変化とを推定する道路形状推定部と、
上記道路形状推定部が推定した道路形状の推定値を積分して車両進行方向前方の道路形状を予測する道路形状予測部と、
を備えることを特徴とする道路形状予測装置。 A vehicle speed detector for detecting the vehicle speed;
A steering angle detector for detecting a steering angle of a steering device provided in the vehicle;
Based on the vehicle speed detected by the vehicle speed detection unit and the steering angle detected by the steering angle detection unit , as a vehicle state quantity , a vehicle attitude angle that is a tangential direction of the trajectory passed by the vehicle, and a curvature of the trajectory passed by the vehicle. A vehicle state quantity calculation unit for calculating a vehicle trajectory curvature that is,
A steering angle the steering angle detection unit detects,-out based on its distance differential value, according to a predetermined arithmetic expression composed by differentiating the difference equation for obtaining the vehicle trajectory curvature at a distance, the change in the vehicle path curvature and the vehicle trajectory curvature change calculating unit which calculates a vehicle path curvature change is,
Based on the vehicle state amount calculated by the vehicle state amount calculation unit and the vehicle locus curvature change calculated by the vehicle locus curvature change calculation unit , the vehicle is estimated as the estimated value of the road shape of the current location according to a Kalman filter that is a predetermined arithmetic expression. road curves tangentially running to that road is a tangential direction of the curve when expressed by a curve, the road curvature is the curvature of the curve when representing the road on which the vehicle is traveling a curve, the and the road shape estimation unit which estimates a road curvature change is a change in road curvature,
A road shape prediction unit that predicts the road shape ahead of the vehicle traveling direction by integrating the estimated value of the road shape estimated by the road shape estimation unit;
A road shape prediction apparatus comprising:

上記車速検出部は、車輪の回転に応じて予め設定した距離間隔毎に発生するパルス情報に基づく車両の走行距離から車速を検出し、
上記車両軌跡曲率変化算出部は、予め設定した走行距離毎に操舵装置の操舵角を検出し、その検出した操舵角の距離微分に基づき、予め定めた演算式に従って、上記車両軌跡曲率変化を算出することを特徴とする請求項１に記載した道路形状予測装置。 The vehicle speed detection unit detects the vehicle speed from the travel distance of the vehicle based on pulse information generated at each predetermined distance interval according to the rotation of the wheel,
The vehicle trajectory curvature change calculation unit detects a steering angle of the steering device for each preset travel distance, and calculates the vehicle trajectory curvature change according to a predetermined arithmetic expression based on a distance derivative of the detected steering angle. The road shape prediction apparatus according to claim 1, wherein

上記道路形状推定部は、上記カルマンフィルタのカルマンゲインは、上記操舵角に基づいて算出した曲率分散値からなる共分散行列を含み、この曲率分散値を予め設定した所定の値に調整しながら、予め設定した車両の走行距離毎に道路形状を推定することを特徴とする請求項１又は請求項２に記載した道路形状予測装置。 The road shape estimation unit Kalman gain of the Kalman filter includes a covariance matrix of curvature dispersion value calculated based on the steering angle, while adjusting to a predetermined value set this curvature variance advance The road shape prediction apparatus according to claim 1, wherein the road shape is estimated for each predetermined travel distance of the vehicle.

上記車両状態量算出部は、車両状態量の算出に対して誤差要因となる道路形状を検出し、その検出した誤差要因となる道路形状に基づき車両状態量を補正する第１補正部を備えることを特徴とする請求項１〜請求項３のいずれか１項に記載した道路形状予測装置。 The vehicle state quantity calculation unit includes a first correction unit that detects a road shape that is an error factor in calculating the vehicle state quantity and corrects the vehicle state quantity based on the detected road shape that is an error factor. The road shape prediction apparatus according to any one of claims 1 to 3, wherein:

上記第１補正部が検出する道路形状が補正可能な否かを判定する補正限界判定部を備えることを特徴とする請求項４に記載した道路形状予測装置。 The road shape prediction apparatus according to claim 4, further comprising a correction limit determination unit that determines whether or not the road shape detected by the first correction unit can be corrected.

車両状態量算出部は、路面に対する車輪の滑りを検出し、検出した車輪の滑りに基づき車両状態量を補正する第２補正部を備えることを特徴とする請求項１〜請求項５のいずれか１項に記載した道路形状予測装置。 6. The vehicle state quantity calculation unit includes a second correction unit that detects a slip of a wheel with respect to a road surface, and corrects the vehicle state quantity based on the detected wheel slip. The road shape prediction apparatus described in item 1.

上記第２補正部が検出する滑りが補正可能な否かを判定する補正限界判定部を備えることを特徴とする請求項６に記載した道路形状予測装置。 The road shape prediction apparatus according to claim 6, further comprising a correction limit determination unit that determines whether or not the slip detected by the second correction unit can be corrected.