JP6939759B2 - Vehicle condition estimation device - Google Patents
Vehicle condition estimation device Download PDFInfo
- Publication number
- JP6939759B2 JP6939759B2 JP2018234756A JP2018234756A JP6939759B2 JP 6939759 B2 JP6939759 B2 JP 6939759B2 JP 2018234756 A JP2018234756 A JP 2018234756A JP 2018234756 A JP2018234756 A JP 2018234756A JP 6939759 B2 JP6939759 B2 JP 6939759B2
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- state
- speed
- relationship
- lateral
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Steering Control In Accordance With Driving Conditions (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Description
本発明は、車両状態推定装置に係り、特に、走行路と横滑りを含む車両の平面運動の関係に基づき、車両の横速度を推定する車両状態推定装置に関する。 The present invention relates to a vehicle state estimation device, and more particularly to a vehicle state estimation device that estimates the lateral speed of a vehicle based on the relationship between the traveling path and the plane motion of the vehicle including skidding.
車両の挙動、特に車両のコーナリング時の挙動における重要な要素として、車両横速度(以下、「横速度」と略記)がある。車体の横方向の運動が計測可能になる事により、例えば車両の自己位置推定が高精度に行えるようになる。従って、高精度な自己位置推定により、車両の運動を計測する他の各種センサの正確な補正も可能となる。また、車両運動モデルをオンラインで補正する事が可能になり、車両運動モデルによって算出される車両制御パラメータのオンラインチューニングにも活用できる。 A vehicle lateral speed (hereinafter abbreviated as "lateral speed") is an important factor in the behavior of the vehicle, particularly the behavior when the vehicle is cornering. By making it possible to measure the lateral motion of the vehicle body, for example, the self-position estimation of the vehicle can be performed with high accuracy. Therefore, the highly accurate self-position estimation enables accurate correction of various other sensors that measure the motion of the vehicle. In addition, the vehicle motion model can be corrected online, and can be used for online tuning of vehicle control parameters calculated by the vehicle motion model.
横速度の測定には、走行中の車両からレーザ光を路面に照射し、路面で反射されたレーザ光を受信して横速度を検出する対地車速計が用いられてきたが、対地車速計は、量産車両に車載されていないため、車両運動モデルを用いた推定手法が提案されている。 A ground vehicle speed meter has been used to measure the lateral speed by irradiating the road surface with a laser beam from a moving vehicle and receiving the laser light reflected on the road surface to detect the lateral speed. Since it is not mounted on a mass-produced vehicle, an estimation method using a vehicle motion model has been proposed.
特許文献1には、車両の状態方程式を使用して横速度を算出する車両物理量推定装置の発明が開示されている。
しかしながら、特許文献1に記載の発明は、車両の状態方程式を使用するので、環境に応じたパラメータの設定が必要であり、パラメータの設定が適切でない場合は、誤差が生じるおそれがある。
However, since the invention described in
本発明は、上記問題に鑑みてなされたものであり、既存の状態方程式に依存せずに、撮像装置が取得した画像及び車速から車両の横速度を算出可能な車両状態推定装置を実現することを目的とする。 The present invention has been made in view of the above problems, and realizes a vehicle state estimation device capable of calculating the lateral speed of a vehicle from an image acquired by an image pickup device and a vehicle speed without depending on an existing equation of state. With the goal.
上記目的を達成するために、請求項1に記載の車両状態推定装置は、 車両周辺の画像情報を取得する撮像装置と、前記撮像装置が取得した画像情報から、目標経路と車両との横位置偏差、前記目標経路と車両のヨー角偏差、及び前記目標経路の曲率を算出する画像情報処理部と、前記目標経路上を前記車両が横滑りなしの状態で走行した場合の位置である目標点の移動と、横滑りを含む前記車両の運動と、の関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車速センサから得られる車両前後速度を含む実測値から、前記車両のヨーレートを推定する演算部と、を含んでいる。
In order to achieve the above object, the vehicle state estimation device according to
また、請求項2に記載の車両状態推定装置は、請求項1に記載の車両状態推定装置において、前記演算部は、更に、前記車両の横速度を推定し、前記関係を表す式は、前記目標点の移動速度の、車両前後方向に対する垂直成分から、前記横位置偏差の変化量と、前記車両のヨー運動に伴う車両重心位置に対する前記撮像装置の横速度とを減算した結果と、前記車両の横速度とが等しい関係を表す式を含む。
Further, the vehicle state estimation device according to claim 2 is the vehicle state estimation device according to
また、請求項3に記載の車両状態推定装置は、請求項1又は2記載の車両状態推定装置において、前記関係を表す式は、前記目標点の移動速度と前記曲率との積から、前記ヨー角偏差の変化量を減算した結果と、前記車両のヨーレートとが等しい関係を表す式を含む。
Further, the vehicle state estimation device according to claim 3 is the vehicle state estimation device according to
請求項4に記載の車両状態推定装置は、請求項1〜請求項3の何れか1項記載の車両状態推定装置において、前記関係を表す式は、前記車両前後速度から、前記車両のヨーレートと前記横位置偏差との積を減算したものを、前記ヨー角偏差を用いて補正した結果と、前記目標点の移動速度とが等しい関係を表す式を含む。
The vehicle state estimation device according to claim 4 is the vehicle state estimation device according to any one of
また、請求項5に記載の車両状態推定装置は、請求項1〜請求項4の何れか1項記載の車両状態推定装置において、前記演算部は、前記関係を表す式に基づいて、前回推定された、前記車両のヨーレートを含む前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車両前後速度と、それぞれの前記実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を補正する更新部と、を含む。
Further, the vehicle state estimation device according to claim 5 is the vehicle state estimation device according to any one of
本発明によれば、既存の状態方程式に依存せずに、撮像装置が取得した画像及び車速から車両の横速度を算出可能な車両状態推定装置を実現することができるという効果を奏する。 According to the present invention, it is possible to realize a vehicle state estimation device capable of calculating the lateral speed of the vehicle from the image acquired by the image pickup device and the vehicle speed without depending on the existing equation of state.
[第1の実施の形態]
以下、図面を参照して本発明の第1の実施の形態を詳細に説明する。図1に示すように、本実施の形態に係る車両状態推定装置10は、後述する演算装置14の演算に必要なデータ及び演算装置14による演算結果を記憶する記憶装置18と、撮像装置22が取得した画像情報から目標経路と車両との横位置偏差、車両のヨー角偏差及び目標経路の曲率を算出する画像情報処理部20と、画像情報処理部20が算出した横位置偏差、ヨー角偏差、曲率、車速センサ(図示せず)が検出した車両前後速度及び慣性センサ(図示せず)が検出したヨーレートが入力される入力装置12と、入力装置12から入力された入力データ及び記憶装置18に記憶されたデータに基づいて車両の横速度の演算を行なうコンピュータ等で構成された演算装置14と、演算装置14で演算された横速度を表示するCRT又はLCD等で構成された表示装置16と、で構成されている。
[First Embodiment]
Hereinafter, the first embodiment of the present invention will be described in detail with reference to the drawings. As shown in FIG. 1, the vehicle
続いて、車両200の挙動に係る座標系を図2に示したように定義する。地球座標系204は地球平面を基準として重力加速度方向とzeとが平行で、yeが北方向を向いている座標系である。路面座標系206は、zrが車両200の重心を通り路面に垂直な方向に向き、xrは車両進行方向に向いている座標系である。車両座標系208は車体バネ上に固定された座標系で、zvは車体鉛直上方向、xvは車体進行方向を向いている。
Subsequently, the coordinate system related to the behavior of the
本実施の形態は、撮像装置22で取得した画像情報を用いて車両200の横速度を算出する。本実施の形態では横速度の算出に際して、図3にあるように、目標点Ptにおける目標経路220である走行車線の曲率ρに加え、センサ位置Psを基準とした目標点Ptに対する横位置偏差e1とヨー角偏差e2とを想定する。なお、センサ位置Psは車両重心Prから車両200の前方方向に距離(車両重心Prとセンサ位置Psとのオフセット距離)lsで隔てられた位置にあり、車載カメラ等の撮像装置22が設けられている。図3に示したように、センサ位置Psを基準とした車両200の進行方向の移動量はxSであり、センサ位置Psを基準とした車両200の横方向の移動量はysである。
In this embodiment, the lateral speed of the
次に、車両200の横速度を推定する際に用いる、横滑りを含む車両200の運動と目標経路220との関係を図4に示す。一般に車両200は路面に対して横滑りをしており、横速度を有する運動を伴っている。車両200の挙動に対し、走行車線上の目標位置Ptは前後方向の速度のみを有し、横速度を有さない運動をしている。この特徴を利用し、車両200の運動と目標位置Ptの運動との差分を導出する事で、車両200の横速度を推定するのに必要な条件式を導出する事が可能になる。
Next, FIG. 4 shows the relationship between the motion of the
本実施の形態では、図3、4のように各変数を設定する。目標経路220上の目標位置Ptは、接線方向の速度成分Utのみを持つとして、目標位置Ptの速度ベクトルをVt=[Ut 0]T、目標位置PtをPt=[xt yt ]T、地球座標系のx軸方向に対する接線方向のヨー角をψtと定義する。さらに車両200は前後方向のみならず横速度を有するとして(車体スリップ角を有する)、路面座標系206の原点をPr=[xr yr ]T、ヨー角をψr、車両200の各軸方向の速度成分を図4に示したように、νr=[Ur Vr ]Tとする。本実施の形態では、後に車両座標系208を基準に計測されたセンサ情報との関係を容易にするため、偏差の基準座標系を路面座標系206とし、ヨー角偏差としてψe=ψt−ψrを定義する。
In this embodiment, each variable is set as shown in FIGS. 3 and 4. Assuming that the target position P t on the
路面座標系206から見た目標位置Ptの偏差ベクトルをPe=[xe ye ]とすると、当該偏差ベクトルPeの微分は下記の2式で表される。
Assuming that the deviation vector of the target position P t seen from the road
車載カメラ等の撮像装置22が取得した車両200周辺の画像情報を、画像情報処理部20において既知の手法によって画像解析することにより、目標点Ptにおける走行車線の曲率ρに加え、センサ位置Psを基準とした目標点Ptに対する横位置偏差e1とヨー角偏差e2とが抽出される。撮像装置22はセンサの一種であるから、当該撮像装置22で取得した画像情報から抽出した曲率ρ、横位置偏差e1及びヨー角偏差e2をセンサで取得されるセンサ情報とする。さらに、車速センサで検出する車両200の前後速度Ur(以下、「車両前後速度Ur」と略記)、及び慣性センサで検出する車両200のヨーレートRrをセンサ情報として、上記式を変形すると、下記の3式が得られる。
By analyzing the image information around the
さらに車体の横速度VrとヨーレートRrとについて上記3式を整理すると、下記の式(1)、(2)、(3)が得られる。
Further, by rearranging the above three equations for the lateral speed V r and the yaw rate R r of the vehicle body, the following equations (1), (2), and (3) can be obtained.
上記式(1)は、車両200の横速度Vrは、車両前後速度Urの目標経路220に対する垂直成分、言い換えれば、車両前後速度Urの車両座標系208のy座標成分であるUtsine2から、目標経路との横位置偏差e1の変化量(e1の微分値)と、車両200のヨー運動に伴う車両重心位置Prに対する撮像装置22の横速度Rrlsを引いたものと一致することを示している。なお、上記式(1)は、目標点の移動速度の、車両前後方向に対する垂直成分から、横位置偏差の変化量と、車両のヨー運動に伴う車両重心位置に対する撮像装置の横速度とを減算した結果と、車両の横速度とが等しい関係を表す式の一例である。
In the above equation (1), the lateral speed V r of the
上記式(2)は、車両200のヨーレートRrは、目標点Ptの移動速度Utと曲率ρとの積であり、目標経路220上の目標点Ptの接線方向の変化率ρUtから、接線方向と車両進行方向とのヨー角偏差e2の変化量(e2の微分値)を引いたものと一致することを示している。なお、上記式(2)は、目標点の移動速度と曲率との積から、ヨー角偏差の変化量を減算した結果と、車両のヨーレートとが等しい関係を表す式の一例である。
The formula (2) is the yaw rate R r of the
上記式(3)は、目標経路220上の目標点Ptの移動速度Utは、車両前後速度Urから車両200のヨーレートRrと横位置偏差e1を乗算したRre1を減算することで旋回半径の補正を行い、さらにそれをヨー角偏差e2の余弦cose2で除算することで求められることを示している。なお、上記式(3)は、車両前後速度から、車両ヨーレートと横位置偏差との積を減算したものを、ヨー角偏差を用いて補正した結果と、目標点の移動速度とが等しい関係を表す式の一例である。
In the above equation (3), the moving speed U t of the target point P t on the
上記式(1)〜(3)を用いて、撮像装置22、慣性センサ及び車速センサによって取得されたデータに対して最もよくあてはまる未知変数を推定することにより、車両200の横速度Vrの推定が可能になる。
Estimating the lateral speed V r of the
上記式(1)〜(3)はいずれも運動の近似をしておらず、非線形な形を取っているが、上記式(1)〜(3)を直接用いて非線形カルマンフィルタに適用することにより、車両200が目標経路220から大きく逸脱している場合でも正確な推定が可能となる。また、状態変数に関して局所線形化する事により、拡張カルマンフィルタを適用してもよい。
None of the above equations (1) to (3) are approximations of motion and take a non-linear form. However, by directly using the above equations (1) to (3) and applying them to a non-linear Kalman filter, Even when the
本実施の形態では、目標経路220と車両200の運動との関係を用いて、状態方程式f(x)を定義し、演算装置14における事前推定とフィルタリングのステップとで活用する。事前推定及びフィルタリングのステップの詳細については、後述する。
In the present embodiment, the equation of state f (x) is defined by using the relationship between the
まず、撮像装置22より得られる目標経路220に対する横位置偏差e1、ヨー角偏差e2及び目標経路220の曲率ρの情報を用いて、車両200のヨーレートRrを推定する場合の状態方程式f(x)は、以下のように定義する。
First, the equation of state f when estimating the yaw rate R r of the
かかる場合の状態方程式f(x)は、下記のような、時間tにおける状態量x(t)に対して、下記の式(4)の関係が成り立つ。
In the equation of state f (x) in such a case, the relationship of the following equation (4) holds with respect to the state quantity x (t) at time t as described below.
上記の状態量x(t)に対して、上記式(4)の関係が成り立つ状態方程式f(x)を下記のように立式する。
For the above state quantity x (t), the equation of state f (x) in which the relationship of the above equation (4) holds is formulated as follows.
式(5)は、目標経路220と車両200との運動の関係を示した微分方程式である。
Equation (5) is a differential equation showing the relationship between the motion of the
また、本実施の形態では、各状態に対するシステムノイズを下記のように定義する。
Further, in the present embodiment, the system noise for each state is defined as follows.
次に観測量を下記のように定義する。
Next, the observable is defined as follows.
y=h(x)を満たす観測方程式h(x)は、下記の各式のように、状態量(右辺)と観測量(左辺)との対応を定義したものである。
The observation equation h (x) satisfying y = h (x) defines the correspondence between the state quantity (right side) and the observation quantity (left side) as shown in the following equations.
ただし、各観測量に対する観測ノイズを下記のように定義する。
However, the observable noise for each observed amount is defined as follows.
カルマンフィルタでは、式(5)を含む状態方程式f(x)の各々を離散化して使用する。従って、関数f(x)の入出力関係を、時間t=k、k−1に対して、x(k)=f(x(k−1))となる形で使用する。 In the Kalman filter, each of the equations of state f (x) including the equation (5) is discretized and used. Therefore, the input / output relationship of the function f (x) is used in such a form that x (k) = f (x (k-1)) with respect to time t = k and k-1.
図5は、本実施の形態に係る車両状態推定装置10の演算装置14の機能ブロック図の一例である。図5に示したように、演算装置14は、状態方程式f(x)を用いて、目標経路220である走行車線と横滑りを含む車両200の平面運動との関係に基づく状態の推定を行うと共に、観測方程式h(x)を用いて、対応するセンサ情報を推定する事前推定部102と、事前推定部102が出力した事前推定値を、センサ情報を用いて補正するフィルタリング部104と、を含む。
FIG. 5 is an example of a functional block diagram of the
カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式f(x)が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式f(x)についての線形化を要さず、かつ計算負荷が比較的小さいアンセンティッドカルマンフィルタを一例として採用する。状態方程式f(x)が線形の場合は、線形カルマンフィルタを用いてもよいし、アンセンティッドカルマンフィルタ以外の非線形カルマンフィルタを用いてもよい。 Various methods have been proposed for the Kalman filter, including linear and non-linear. In this embodiment, based on the fact that the equation of state f (x) is non-linear as described above, an example of utilizing a non-linear Kalman filter is shown. Among them, an unscented Kalman filter that does not require linearization of the equation of state f (x) and has a relatively small calculation load is adopted as an example. When the equation of state f (x) is linear, a linear Kalman filter may be used, or a non-linear Kalman filter other than the uncentric Kalman filter may be used.
図6は、事前推定部102の機能ブロック図の一例である。図6に示したように、事前推定部102は、第1アンセンティッド変換部102Aと第2アンセンティッド変換部102Bとを含む。
FIG. 6 is an example of a functional block diagram of the
第1アンセンティッド変換部102Aは、状態方程式f(x)に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、xの平均及びxの共分散行列を出力する。
The first
第2アンセンティッド変換部102Bは、第1アンセンティッド変換部102Aが出力した状態量xの平均及び状態量xの共分散行列を用いて、観測方程式h(x)に従って、対応する観測量に変換する2回目のアンセンティッド変換を行う。
The second
なお、アンセンティッド変換の目的は、ある非線形関数y=f(x)による変換において、下記の観測量yの平均及び共分散行列を精度よく求めることにある。
The purpose of the unscented transformation is to accurately obtain the average and covariance matrix of the following observables y in the transformation by a certain nonlinear function y = f (x).
従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント)を用いて、確率密度関数を近似することを特徴とする。 Therefore, the present embodiment is characterized in that the probability density function is approximated by using 2n + 1 samples (sigma points) corresponding to the mean value and the standard deviation.
図6に示したように、第1アンセンティッド変換部102Aは、シグマポイント重み係数部102A1と、関数変換部102A2と、U変換部102A3と、を含む。また、第2アンセンティッド変換部102Bは、シグマポイント重み係数部102B1と、関数変換部102B2と、U変換部102B3と、を含む。
As shown in FIG. 6, the first
第1アンセンティッド変換部102Aのシグマポイント重み係数部102A1及び第2アンセンティッド変換部102Bのシグマポイント重み係数部102B1では、シグマポイントXi:i=0、1、2、…2nが、下記のように選択される。
In the sigma point weighting coefficient unit 102A1 of the first
ただし、スケーリングファクタκは、κ≧0となるように選択する。また、シグマポイントに対する重みは下記のように定義する。
However, the scaling factor κ is selected so that κ ≧ 0. The weight for the sigma point is defined as follows.
第1アンセンティッド変換部102Aの関数変換部102A2における、非線形関数f(x)による各シグマポイントの変換は下記のようになる。下記は、第1アンセンティッド変換部102Aの関数変換部102A2での状態方程式f(x)による変換だが、第2アンセンティッド変換部102Bの関数変換部102B2での観測方程式h(x)を用いた変換では、観測値が得られる。
The conversion of each sigma point by the non-linear function f (x) in the function conversion unit 102A2 of the first
第1アンセンティッド変換部102AのU変換部102A3では、関数f(x)によって変換された値と、前述の重み係数とを用いて、下記のように状態量xの平均値と状態量xの共分散行列を算出する。なお、下記式中のQnはシステムノイズである。
In the U conversion unit 102A3 of the first
第2アンセンティッド変換部102BのU変換部102B3では、下記の計算を行う。
The U conversion unit 102B3 of the second
図7は、フィルタリング部104の機能ブロック図の一例である。フィルタリング部104では、U変換部102B3で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。
FIG. 7 is an example of a functional block diagram of the
状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のRnは観測ノイズである。
The process of feeding back the predicted value of the state quantity with the actually observed value is called Kalman Gain, and it is calculated by the following equation. R n in the following equation is the observed noise.
次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。
Next, using this Kalman gain, the process of correcting the pre-predicted value of the state quantity is performed as follows.
以上の事前推定部102及びフィルタリング部104の処理を各タイムステップごとに繰り返すことにより、車両200のヨーレートRrを推定する。
By repeating the above processes of the
車両200のヨーレートRrを推定する場合、フィルタリング部104が出力したx(k)、Px(k)を前回値として事前推定部102に入力し、上述のように2段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部104でカルマンゲイン及び実際に観測された最新の観測値を用いて補正される。フィルタリング部104が出力したx(k)、Px(k)は、記憶装置18に一時的にホールドし、記憶装置18にホールドしたx(k)、Px(k)を前回値として事前推定部102に入力する。
When estimating the yaw rate R r of the vehicle 200, the x (k) and P x (k) output by the
この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、車両の横速度Vr及び車両200のヨーレートRrを精度よく推定できる。本実施の形態に係る車両状態推定装置は、既存の状態方程式に依存せず、かつ車両200の運動について近似をしていないので、車両200が目標経路220から大きく逸脱した場合でも正確にヨーレートRrの推定が可能となる。
By repeating the calculation of the pre-predicted value of the state quantity and the correction with the latest value actually observed, the lateral velocity V r of the vehicle and the yaw rate R r of the vehicle 200 can be estimated accurately. Since the vehicle state estimation device according to the present embodiment does not depend on the existing equation of state and does not approximate the motion of the
以上説明したように、本実施の形態に係る車両状態推定装置によれば、既存の状態方程式に依存せずに、撮像装置22の映像及び車速から車両200のヨーレートRrを算出できる。
As described above, according to the vehicle state estimation device according to the present embodiment, the yaw rate R r of the vehicle 200 can be calculated from the image and the vehicle speed of the
[第2の実施の形態]
続いて本発明の第2の実施の形態について説明する。本実施の形態では、車両200の横速度Urを算出するが、使用する状態方程式f(x)が第1の実施の形態と相違するものの、車両状態推定装置10の構成は、第1の実施の形態と同一なので、詳細な説明は省略する。
[Second Embodiment]
Subsequently, a second embodiment of the present invention will be described. In the present embodiment, to calculate the transverse velocity U r of the
まず、撮像装置22より得られる目標経路220に対する横位置偏差e1、ヨー角偏差e2及び目標経路220の曲率ρの情報を用いて、車両200の横速度Urを推定する場合の状態方程式f(x)を定義する。
First, the equation of state when the transverse position e 1 relative to the
かかる場合の状態方程式f(x)は、下記のような、時間tにおける状態量x(t)に対して、下記の式(4)の関係が成り立つ。
In the equation of state f (x) in such a case, the relationship of the following equation (4) holds with respect to the state quantity x (t) at time t as described below.
上記の状態量x(t)に対して、上記の式(4)の関係が成り立つ状態方程式f(x)を下記のように立式する。
For the above state quantity x (t), the equation of state f (x) in which the relationship of the above equation (4) holds is formulated as follows.
式(6)、(7)は、目標経路220と車両200との運動の関係を示す式を用いて定義される微分方程式である。
Equations (6) and (7) are differential equations defined using equations showing the relationship between the motion of the
また、本実施の形態では、各状態に対するシステムノイズを下記のように定義する。
Further, in the present embodiment, the system noise for each state is defined as follows.
次に観測量を第1の実施の形態と同様に定義する。
Next, the observed amount is defined in the same manner as in the first embodiment.
y=h(x)を満たす観測方程式h(x)は、第1の実施の形態と同様に、下記の各式のように、状態量(右辺)と観測量(左辺)との対応を定義したものである。
The observation equation h (x) satisfying y = h (x) defines the correspondence between the state quantity (right side) and the observation quantity (left side) as in the following equations, as in the first embodiment. It was done.
ただし、各観測量に対する観測ノイズを下記のように定義する。
However, the observable noise for each observed amount is defined as follows.
本実施の形態では、上記のf(x)の入出力関係を、第1の実施の形態と同様に、時間t=k、k−1に対して、x(k)=f(x(k−1))となる形で使用する。以下、第1の実施の形態と同様に、事前推定部102及びフィルタリング部104の処理を各タイムステップごとに繰り返すことにより、車両200の横速度Urを推定する。
In the present embodiment, the input / output relationship of the above f (x) is set to x (k) = f (x (k) with respect to the time t = k and k-1 as in the first embodiment. -1))) is used. Hereinafter, similarly to the first embodiment, by repeating the process of
車両200の横速度Vrを推定する場合も、フィルタリング部104が出力したx(k)、Px(k)を前回値として事前推定部102に入力し、上述のように2段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部104でカルマンゲイン及び実際に観測された最新の観測値で補正される。フィルタリング部104が出力したx(k)、Px(k)は、記憶装置18に一時的にホールドし、記憶装置18にホールドしたx(k)、Px(k)を前回値として事前推定部102に入力する。
Also when estimating the lateral speed V r of the vehicle 200, the x (k) and P x (k) output by the
この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、車両200の横速度Vrを精度よく推定できる。本実施の形態に係る車両状態推定装置は、既存の状態方程式に依存せず、かつ車両200の運動について近似をしていないので、車両200が目標経路220から大きく逸脱した場合でも正確に横速度Vrの推定が可能となる。
By repeating the calculation of the pre-predicted value of the state quantity and the correction with the latest value actually observed, the lateral velocity V r of the
以上説明したように、本実施の形態に係る車両状態推定装置によれば、既存の状態方程式に依存せずに、撮像装置22の映像及び車速から車両200の横速度Vrを算出できる。
As described above, according to the vehicle state estimation device according to the present embodiment, the lateral speed V r of the
10 車両状態推定装置
12 入力装置
14 演算装置
16 表示装置
18 記憶装置
20 画像情報処理部
22 撮像装置
102 事前推定部
102A 第1アンセンティッド変換部
102A1 シグマポイント重み係数部
102A2 関数変換部
102A3 U変換部
102B 第2アンセンティッド変換部
102B1 シグマポイント重み係数部
102B2 関数変換部
102B3 U変換部
104 フィルタリング部
200 車両
204 地球座標系
206 路面座標系
208 車両座標系
220 目標経路
10 Vehicle
Claims (5)
前記撮像装置が取得した画像情報から、目標経路と車両との横位置偏差、前記目標経路と車両のヨー角偏差、及び前記目標経路の曲率を算出する画像情報処理部と、
前記目標経路上を前記車両が横滑りなしの状態で走行した場合の位置である目標点の移動と、横滑りを含む前記車両の運動と、の関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車速センサから得られる車両前後速度を含む実測値から、前記車両のヨーレートを推定する演算部と、
を含む車両状態推定装置。 An imaging device that acquires image information around the vehicle,
An image information processing unit that calculates the lateral position deviation between the target route and the vehicle, the yaw angle deviation between the target route and the vehicle, and the curvature of the target route from the image information acquired by the imaging device.
By using the movement of the target point is the position when the upper the target path the vehicle has traveled in a state without a sideslip, motion and the vehicle including skid, an expression representing the relationship, the transverse position, the A calculation unit that estimates the yaw rate of the vehicle from actual measurement values including the yaw angle deviation, the curvature, and the vehicle front-rear speed obtained from the vehicle speed sensor.
Vehicle condition estimation device including.
前記関係を表す式は、前記目標点の移動速度の、車両前後方向に対する垂直成分から、前記横位置偏差の変化量と、前記車両のヨー運動に伴う車両重心位置に対する前記撮像装置の横速度とを減算した結果と、前記車両の横速度とが等しい関係を表す式を含む請求項1に記載の車両状態推定装置。 The calculation unit further estimates the lateral speed of the vehicle.
The formula expressing the relationship is the amount of change in the lateral position deviation from the vertical component of the moving speed of the target point with respect to the vehicle front-rear direction, and the lateral speed of the imaging device with respect to the position of the vehicle center of gravity accompanying the yaw movement of the vehicle. The vehicle state estimation device according to claim 1, further comprising an expression representing a relationship in which the result of subtracting the above is equal to the lateral speed of the vehicle.
予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車両前後速度と、それぞれの前記実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を補正する更新部と、
を含む請求項1〜請求項4の何れか1項記載の車両状態推定装置。
The calculation unit includes a prediction unit that predicts the state amount of the vehicle at the next time from the state amount of the vehicle including the yaw rate of the vehicle estimated last time based on the formula expressing the relationship.
The lateral position deviation, the yaw angle deviation, the curvature, and the vehicle front-rear speed, which correspond to the state quantity of the vehicle at the predicted next time, calculated using a predetermined observation equation, respectively. An update unit that corrects the state amount of the vehicle predicted by the prediction unit based on the difference from the measured value and the state amount of the vehicle estimated last time.
The vehicle state estimation device according to any one of claims 1 to 4.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018234756A JP6939759B2 (en) | 2018-12-14 | 2018-12-14 | Vehicle condition estimation device |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018234756A JP6939759B2 (en) | 2018-12-14 | 2018-12-14 | Vehicle condition estimation device |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2020094989A JP2020094989A (en) | 2020-06-18 |
JP6939759B2 true JP6939759B2 (en) | 2021-09-22 |
Family
ID=71085583
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2018234756A Active JP6939759B2 (en) | 2018-12-14 | 2018-12-14 | Vehicle condition estimation device |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6939759B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2020097316A (en) * | 2018-12-18 | 2020-06-25 | 株式会社豊田中央研究所 | Vehicle body posture angle estimation device |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP7396308B2 (en) * | 2021-01-28 | 2023-12-12 | トヨタ自動車株式会社 | Travel trajectory estimation system, travel trajectory estimation program, and travel trajectory estimation method |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8160780B2 (en) * | 2007-12-13 | 2012-04-17 | Hyundai Motor Japan R&D Center, Inc. | System and method for keeping a vehicle in a lane |
JP5402244B2 (en) * | 2009-05-26 | 2014-01-29 | トヨタ自動車株式会社 | Vehicle physical quantity estimation device |
JP2016011060A (en) * | 2014-06-30 | 2016-01-21 | マツダ株式会社 | Lane keeping support system |
-
2018
- 2018-12-14 JP JP2018234756A patent/JP6939759B2/en active Active
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2020097316A (en) * | 2018-12-18 | 2020-06-25 | 株式会社豊田中央研究所 | Vehicle body posture angle estimation device |
JP7234617B2 (en) | 2018-12-18 | 2023-03-08 | 株式会社豊田中央研究所 | Body attitude angle estimation device |
Also Published As
Publication number | Publication date |
---|---|
JP2020094989A (en) | 2020-06-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Reina et al. | Vehicle dynamics estimation via augmented extended Kalman filtering | |
Reina et al. | Vehicle parameter estimation using a model-based estimator | |
CN110036353B (en) | Method and system for controlling lateral movement of a ground vehicle | |
KR102432116B1 (en) | A navigation system | |
JP5173854B2 (en) | Sensor drift estimation device | |
JP7036080B2 (en) | Inertial navigation system | |
JP5029442B2 (en) | Vehicle attitude angle estimation device and program | |
US10668928B2 (en) | Method and device for estimating the friction values of a wheel of a vehicle against a substrate | |
JP2017531597A (en) | Method for estimating vehicle side slip angle, computer program for implementing the method, control unit reading the computer program, and vehicle equipped with the control unit | |
JP6939759B2 (en) | Vehicle condition estimation device | |
JP7206883B2 (en) | Yaw rate corrector | |
CN115303265A (en) | Vehicle obstacle avoidance control method and device and vehicle | |
JP7028223B2 (en) | Self-position estimator | |
CN112955715B (en) | Method for determining a movement vector of a motor vehicle, method for determining a speed of a vehicle and related vehicle | |
KR101954510B1 (en) | Lane keeping control method and apparatus | |
JP6981459B2 (en) | Sensor error correction device | |
CN112577512A (en) | State quantity error determination method based on wheel speed fusion and vehicle-mounted terminal | |
Eising et al. | 2.5 D vehicle odometry estimation | |
CN112577513A (en) | State quantity error determination method and vehicle-mounted terminal | |
JP7234617B2 (en) | Body attitude angle estimation device | |
JP5091047B2 (en) | Vehicle attitude angle estimation device and sensor drift estimation device | |
WO2022203026A1 (en) | Driving control device | |
JP7206875B2 (en) | Vehicle lateral speed estimator | |
CN115826583A (en) | Automatic driving vehicle formation method based on point cloud map | |
JP4541839B2 (en) | Road surface inclination judgment system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20200422 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210217 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20210302 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20210326 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20210803 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20210816 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6939759 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |