JP2001264424A - 高度推定装置及び高度推定方法 - Google Patents

Abstract

(57)【要約】 【課題】 道路監視用のセンサと走行車両を用いて道路
高を自動的に推定することにより、道路を占有すること
なくかつ人手をかけずに短時間で道路高データを得る高
度推定装置およびその方法を提供する。 【解決手段】 基準の平面に対する高低差を推定する高
度推定装置であって、移動体の画像を観測する画像観測
手段１と、画像観測手段により観測された移動体の画像
の注目位置を仮定された複数の道路高の候補に基づき２
次元直交座標に変換する座標変換手段２と、前サンプリ
ング時に観測された移動体の２次元直交座標上の位置に
基づき移動体の予測位置を算出する予測手段３と、予測
手段によって算出された予測位置と座標変換手段によっ
て変換された複数の道路高の候補に基づく２次元直交座
標上の移動体の観測位置から道路の高低差を算出する道
路高算出４手段とを備える。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、高度推定装置お
よび推定方法に関し、特に例えば道路の高度を道路上を
移動する人、自転車、車両などの移動位置から推定する
ものに関する。

【０００２】

【従来の技術】図１３は、例えば、”やさしいＧＰＳ測
量”、土屋・辻著、(社)日本測量学会、１９９１年に示
された従来の測量による水準儀を用いた高低差測定方法
を示す構成図であり、図において、２０は水準儀、２１
は第一の標尺、２２は第二の標尺である。

【０００３】次に高低差の測定方法について説明する。
高低差を測定したい道路上の２点にそれぞれ第一の標尺
２１と第二の標尺２２を垂直にセットする。水準儀２０
を第一の標尺２１と第二の標尺２２の中間に水平を保っ
て設置し、第一の標尺２１と第二の標尺２２の目盛りを
読む。これらの目盛りの値をそれぞれ、ａ１およびａ２
とすると、所望の高低差ｈはａ１−ａ２となる。

【０００４】上記のようにして、ある基準位置からの水
平距離と高低差を順次計測することにより、所望の道路
区間の相対高度データを取得することができる。

【０００５】

【発明が解決しようとする課題】従来の基準の平面に対
する道路の高低差(以後、道路高と呼ぶ)を計測する方法
は以上のように水準儀と標尺を用いて人手によりに行う
ので、計測には多大な労力を必要とした。また、計測の
際に道路を通行止めにするなど道路を占有する必要があ
った。

【０００６】この発明は上記のような課題を解決するた
めになされたもので、道路監視用のセンサと走行車両を
用いて道路高を自動的に推定することにより、道路を占
有することなくかつ人手をかけずに短時間で道路高デー
タを得ることのできる道路高推定装置およびその方法、
またひいては道路に限られることのない一般の高度推定
装置およびその方法を提供することを目的とする。

【０００７】

【課題を解決するための手段】上記の目的に鑑み、この
発明は、基準の平面に対する高低差を推定する高度推定
装置であって、対象領域上に移動体を走らせ、観測され
た移動体の位置を仮定された複数の高さの候補に基づき
２次元直交座標に変換し、この２次元直交座標上のその
前に観測された移動体の位置に基づく移動体の予測位置
と上記観測位置から道路の高低差を算出することを特徴
とする高度推定装置にある。

【０００８】また、上記高度推定装置が道路高推定装置
であって、移動体の画像を観測する画像観測手段と、画
像観測手段により観測された移動体の画像の注目位置を
仮定された複数の道路高の候補に基づき２次元直交座標
に変換する座標変換手段と、前サンプリング時に観測さ
れた移動体の２次元直交座標上の位置に基づき移動体の
予測位置を算出する予測手段と、予測手段によって算出
された予測位置と座標変換手段によって変換された複数
の道路高の候補に基づく２次元直交座標上の移動体の観
測位置から道路の高低差を算出する道路高算出手段と、
を備えたことを特徴とする請求項１に記載の高度推定装
置にある。

【０００９】また、上記高度推定装置が道路高推定装置
であって、移動体の距離と方位角を観測する観測手段
と、観測手段により観測された移動体の距離と方位角を
仮定された複数の道路高の候補に基づき２次元直交座標
に変換する座標変換手段と、前サンプリング時に観測さ
れた移動体の２次元直交座標上の位置に基づき移動体の
予測位置を算出する予測手段と、予測手段によって算出
された予測位置と座標変換手段によって変換された複数
の道路高の候補に基づく２次元直交座標上の移動体の観
測位置から道路の高低差を算出する道路高算出手段と、
を備えたことを特徴とする請求項１に記載の高度推定装
置にある。

【００１０】また、道路高算出手段によって算出された
道路高に基づく移動体の２次元直交座標上の位置と予測
手段により算出された前サンプリング時の予測位置から
移動体の位置の平滑値を算出する平滑手段を設け、予測
手段は平滑手段の算出する平滑値によって予測位置を算
出することを特徴とする請求項２または３に記載の高度
推定装置にある。

【００１１】また、移動体の距離変化率を観測する距離
変化率観測手段と、距離変化率観測手段により観測され
た移動体の距離変化率を道路高算出手段によって算出さ
れた道路高に基づく移動体の速度に変換する速度算出手
段とを設け、平滑手段は道路高算出手段によって算出さ
れた道路高に基づく移動体の２次元直交座標上の位置と
速度算出手段によって算出された移動体の速度と予測手
段により算出された前サンプリング時の予測位置から移
動体の位置の平滑値を算出することを特徴とする請求項
４に記載の高度推定装置にある。

【００１２】また、基準の平面に対する高低差を推定す
る高度推定方法であって、対象領域上に移動体を走ら
せ、観測された移動体の位置を仮定された複数の高さの
候補に基づき２次元直交座標に変換し、この２次元直交
座標上のその前に観測された移動体の位置に基づく移動
体の予測位置と上記観測位置から道路の高低差を算出す
ることを特徴とする高度推定方法にある。

【００１３】また、上記高度推定方法が道路高推定方法
であって、移動体の画像を観測する画像観測ステップ
と、画像観測ステップにより観測された移動体の画像の
注目位置を仮定された複数の道路高の候補に基づき２次
元直交座標に変換する座標変換ステップと、前サンプリ
ング時に観測された移動体の２次元直交座標上の位置に
基づき移動体の予測位置を算出する予測ステップと、予
測ステップによって算出された予測位置と座標変換ステ
ップによって変換された複数の道路高の候補に基づく２
次元直交座標上の移動体の観測位置から道路の高低差を
算出する道路高算出ステップと、を設けたことを特徴と
する請求項６に記載の高度推定方法にある。

【００１４】また、上記高度推定方法が道路高推定方法
であって、移動体の距離と方位角を観測する観測ステッ
プと、観測ステップにより観測された移動体の距離と方
位角を仮定された複数の道路高の候補に基づき２次元直
交座標に変換する座標変換ステップと、前サンプリング
時に観測された移動体の２次元直交座標上の位置に基づ
き移動体の予測位置を算出する予測ステップと、予測ス
テップによって算出された予測位置と座標変換ステップ
によって変換された複数の道路高の候補に基づく２次元
直交座標上の移動体の観測位置から道路の高低差を算出
する道路高算出ステップと、を設けたことを特徴とする
請求項６に記載の高度推定方法にある。

【００１５】また、道路高算出ステップによって算出さ
れた道路高に基づく移動体の２次元直交座標上の位置と
予測ステップにより算出された前サンプリング時の予測
位置から移動体の位置の平滑値を算出する平滑ステップ
を設け、予測ステップは平滑ステップの算出する平滑値
によって予測位置を算出することを特徴とする請求項７
または８に記載の高度推定方法にある。

【００１６】また、移動体の距離変化率を観測する距離
変化率観測ステップと、距離変化率観測ステップにより
観測された移動体の距離変化率を道路高算出ステップに
よって算出された道路高に基づく移動体の速度に変換す
る速度算出ステップとを設け、平滑ステップは道路高算
出ステップによって算出された道路高に基づく移動体の
２次元直交座標上の位置と速度算出ステップによって算
出された移動体の速度と予測ステップにより算出された
前サンプリング時の予測位置から移動体の位置の平滑値
を算出することを特徴とする請求項９に記載の高度推定
方法にある。

【００１７】

【発明の実施の形態】以下、この発明の高度推定装置お
よびその方法の各実施の形態を、道路高推定装置の場合
を例に挙げて図面に従って説明する。 実施の形態１．図１はこの発明の実施の形態１による道
路高推定装置を示す構成図であり、図において、１は画
像センサ観測座標系で車両の位置(スクリーン上の位置)
を検出する画像センサ装置、２は画像センサ装置１で観
測された画像センサ観測座標における車両の位置と、道
路高設定器５で設定された複数道路高を用いて、各道路
高を仮定した場合の道路座標の位置観測ベクトルを算出
するスクリーン座標変換器である。

【００１８】３は平滑器９で算出した平滑ベクトルから
等速直線運動モデルに基づき予測ベクトルを算出する予
測器、４は予測器３で算出した予測ベクトルと、スクリ
ーン座標変換器２で算出した複数道路高を仮定した場合
の道路座標の位置観測ベクトルとを用いて予測位置を算
出し、予測位置とその前後の位置観測ベクトルとを用い
て道路高を算出する道路高算出器、５は道路高算出器４
で算出した道路高を基準に等間隔に複数の道路高を設定
する道路高設定器、６は道路高設定器５で設定された道
路高を１サンプリング時間だけ遅延する第１の遅延回路
である。

【００１９】７は平滑誤差評価器１１で算出された平滑
誤差共分散行列と、予め設定された駆動雑音共分散行列
とを用いて予測誤差共分散行列を算出する予測誤差評価
器、８は予測誤差評価器７で算出された予測誤差共分散
行列と、予め設定された画像センサ装置１の観測諸元の
観測雑音共分散行列とを用いて、ゲイン行列を算出する
ゲイン行列算出器である。

【００２０】９はスクリーン座標変換器２で算出された
推定道路高における道路座標の位置観測ベクトルと、ゲ
イン行列算出器８で算出されたゲイン行列と、予測器３
で算出された予測ベクトルを用いて平滑ベクトルを算出
する平滑器、１０は平滑器９で算出された平滑ベクトル
を１サンプリング時間だけ遅延する第２の遅延回路、１
１はゲイン行列算出器８で算出されたゲイン行列と、予
測誤差評価器７で算出された予測誤差共分散行列を用い
て平滑誤差共分散行列を算出する平滑誤差評価器、そし
て１２は平滑誤差評価器１１で算出した平滑誤差共分散
行列を１サンプリング時間だけ遅延する第３の遅延回路
である。

【００２１】また、図２はこの発明の実施の形態１によ
る道路高推定装置の動作を示すフローチャートである。

【００２２】次に動作について説明する。最初に、この
実施の形態１による道路高推定装置の動作原理を説明す
る。画像センサ装置１は道路上に設置され、道路を走行
する車両を観測する。この様子を図３に示す。センサ設
置地点付近の道路脇を原点とし、道路交差方向をｘ軸、
道路に沿った方向をｚ軸、鉛直上向きをｙ軸の正とする
ｏ−ｘｙｚ直交座標系を基準座標系とする。基準座標系
における画像センサ装置１の取り付け位置は(ｘｃ，ｈ
ｃ，ｚｃ)とする。

【００２３】図４及び図５に示すように、画像センサ装
置１の焦点を原点ｏ^(c)とし、画像センサ装置１の前方
をｚ^(c)軸正の方向、左手をｘ^(c)軸正の方向、鉛直上方
をｙ ^(c)軸正の方向とする座標系ｏ^(c)−ｘ^(c)ｙ^(c)ｚ
^(c)を画像センサ座標系とする。画像センサ装置１はｙ
軸回りにα(ｚ軸正の方向より反時計回りを正とする)だ
け傾き、更にｘ^(c)軸回りにβ(ｙ軸正の方向より下方を
負とする)だけ傾いているものとする。

【００２４】図６に示すように、画像センサ装置１はス
クリーン面上に定義された２次元直交座標系ｏ^(s)−ｘ
^(s)ｙ^(s)にて車両を観測する。これを画像センサ観測座
標系とする。この際、画像センサ装置１の焦点位置はｏ
^(c)であり、フォーカス距離をｆ(＞０)とすると、画像
センサ観測座標系ｏ^(s)−ｘ^(s)ｙ^(s)は画像センサ座標
系のｘ^(c)−ｙ^(c)平面をｚ^(c)軸の正の方向へｆだけ平
行移動した座標系となる。ピクセルサイズをｋとする
と、画像センサ観測座標系と画像センサ座標系の関係は
式(１)で表される。従って、カメラの倍率をｓとする
と、画像センサ座標系における車両の位置は式(２)とな
る。また、これを基準座標系で表すと式(３)となる。

【００２５】

【数１】

【００２６】ここで、車両の位置における道路の高さを
ｈとすると、式(４)とおけるので、倍率ｓは式(５)で表
される。式(５)を式(３)に代入して式(６)及び式(７)を
得る。すなわち、画像センサ観測座標系で観測された車
両の画像の検出位置は、道路の高さｈを用いて、式(６)
及び式(７)によりｘ−ｚ平面(以後、道路座標と呼ぶ)に
変換される。

【００２７】

【数２】

【００２８】次に、車両が短いサンプリング時間間隔に
おいて等速直線運動をしているものとして、車両の運動
モデルを式(８)に示す。ここで、ｘ _kは時刻ｔ_k(以後、
時刻ｔ_kと呼ぶ)における車両の運動諸元の真値を表す状
態ベクトルであり、道路座標における車両の位置ベクト
ルを式(９)、速度ベクトルを式(１０)とすると、車両の
状態ベクトルは式(１１)で表される。なお、Ａ ^Tはベク
トルＡの転置ベクトルを表す。

【００２９】

【数３】

【００３０】ここで、Φ_k-1は時刻ｔ_k-1よりｔ_kへの状
態ベクトルｘ _kの推移行列であり、式(１２)で表され
る。また、ｗ _kは時刻ｔ_kにおける駆動雑音ベクトルであ
り、Γ₁(ｋ)は時刻ｔ_kにおける駆動雑音ベクトルの変換
行列であり、車両の運動モデルを等速直線運動と仮定す
ることによる打ち切り誤差項をΓ₁(ｋ−１)ｗ _k-1とみれ
ば、ｗ _kは加速度ベクトル相当であり、Γ₁(ｋ−１)は式
(１３)で表される。なお、Ｉ_nはｎ行ｎ列の単位行列で
ある。

【００３１】

【数４】

【００３２】また、Ｅを平均を表す記号とすると、ｗ _k
は平均０の２次元正規分布白色雑音であり、式(１４)及
び(１５)とする。ただし、０は零ベクトルであり、Ｑ_k
は時刻ｔ_kにおける駆動雑音共分散行列である。

【００３３】

【数５】

【００３４】次に、車両の位置ベクトルが時刻ｔ_kに画
像センサ装置１より観測される場合の画像センサ装置１
の観測モデルを式(１６)とする。ここで、ｚ _kは式(６)
及び式(７)を用いて変換した時刻ｔ_kにおける画像セン
サ装置１の道路座標における観測ベクトルであり式(１
７)で表される。Ｈ₁は時刻ｔ_kにおける画像センサ装置
１の観測行列であり式(１８)で表される。

【００３５】また、ｖ _k ¹は時刻ｔ_kにおける画像センサ
装置１の観測雑音ベクトルであり、平均０の２次元正規
分布白色雑音で、式(１９)及び(２０)で表される。な
お、Ｒ _k ¹は時刻ｔ_kにおける画像センサ装置１の観測諸
元の観測雑音共分散行列である。また、Γ₂ ¹(ｋ)は画像
センサ観測座標系より道路座標系への観測雑音ベクトル
の変換行列であり、式(２１)で表される。

【００３６】

【数６】

【００３７】次に、画像センサ装置１により観測された
時刻ｔ_k-1までの観測ベクトルＺ_k-1が得られているとき
の、時刻ｔ_kにおけるｘ _kの予測ベクトルをｘ _k「ハッ
ト」(−)、予測誤差共分散行列をＰ_k(−)とすると、そ
れぞれ条件付平均ベクトル、条件付共分散行列で定義さ
れ、式(２２)及び式(２３)で表される。また、予測位置
ｚ _k(−)は式(２４)で表される。ここで、ｘ _k「ハット」
(＋)及びＰ_k(＋)はそれぞれ前時刻ｔ_k-1の平滑ベクトル
及び平滑誤差共分散行列である。

【００３８】

【数７】

【００３９】次に、時刻ｔ_kにおいて、画像センサ装置
１により観測された車両の位置ベクトルを用いて車両の
平滑ベクトルを算出する平滑処理について述べる。ゲイ
ン行列Ｋ_k、平滑ベクトルｘ _k「ハット」(＋)及び平滑誤
差共分散行列Ｐ_k(＋)はカルマンフィルタの理論によ
り、式(２５)〜(２７)で与えられる。

【００４０】

【数８】

【００４１】次に、道路を走行している車両を画像セン
サ装置１で観測することにより、道路高を推定する方法
について述べる。図７は、水平道路面を走行している状
態から下り坂にさしかかった１台の車両を画像センサで
観測している様子を側面から見た図である。いま、前サ
ンプリング時刻ｔ_k-1に推定した道路高ｈ「ハット」(ｋ
−１)を基準に等間隔に設定された複数の道路高をｈ
₁(ｋ−１)，ｈ₂(ｋ−１)，・・・，ｈ_j-1(ｋ−１)，ｈ_j
(ｋ−１)，ｈ_j+1(ｋ−１)，・・・とする。

【００４２】時刻ｔ_kにおける真の車両の位置をＵ_k、予
測位置をＵ'_kとする。時刻ｔ_kにおける車両の予測位置
Ｕ'_kの道路座標ｚ _k(−)は式(２４)により算出できる。
下り坂はなだらかで、サンプル時刻間隔ｔ_k−ｔ_k-1は十
分短いものとすると、式(２８)とみなすことができる。

【００４３】

【数９】

【００４４】従って、真の車両の位置Ｕ_kと予測位置Ｕ'
_kの道路座標は等しく、ｚ _k(−)であるとみなすことがで
きる。複数の道路高各々を仮定した場合の道路座標にお
ける車両の位置観測ベクトルと予測位置ｚ _k(−)を比較
した結果、図７に示すようにｚ _k(−)がｚ _k[ｈ＝ｈ_j(ｋ
−１)]とｚ _k[ｈ＝ｈ_j+1(ｋ−１)]の間にあるとする。こ
こで、ｚ _k[ｈ＝ｈ_i(ｋ)] (i=1,2,・・・)は道路高ｈ
_i(ｋ)における車両の位置観測ベクトルである。

【００４５】時刻ｔ_kにおける道路高の推定値をｈ「ハ
ット」(ｋ)とし、また、式(２９)〜式(３２)とおくと、
幾何学的関係から式(３３)が成り立つ。従って、時刻ｔ
_kにおける道路高の推定値ｈ「ハット」(ｋ)は式(３４)
にて算出できる。これは、道路が上り坂になっている場
合も同様である。なお、次のサンプリング時刻における
複数道路高は現サンプリング時刻の推定値ｈ「ハット」
(ｋ)を基準に等間隔に設定し直す。このようにして、道
路座標における車両の位置と基準平面に対する道路高を
逐次推定する。

【００４６】

【数１０】

【００４７】次にこの発明の道路高推定装置の動作を具
体的に説明する。なお、カルマンフィルタを目標追尾装
置に通常適用する場合と同様にして、初期値ｘ ₀「ハッ
ト」(＋)、Ｐ₀(＋)は別途定まっているとする。

【００４８】画像センサ装置１では、画像センサ観測座
標系で車両の位置(スクリーン上の位置)を検出し、その
結果をスクリーン座標変換器２に出力する(ステップＳ
Ｔ１)。スクリーン座標変換器２では、画像センサ装置
１から車両のスクリーン上の位置と、道路高設定器５の
出力する複数道路高を第１の遅延回路６を介して入力
し、各道路高を仮定した場合の道路座標の位置観測ベク
トルｚ _k[ｈ＝ｈ_i(ｋ−１)](i=1,2,・・・)を式(６)およ
び式(７)に従い算出する(ステップＳＴ２)。

【００４９】予測器３では、平滑器９の出力する平滑ベ
クトルｘ _k「ハット」(＋)を第２の遅延回路１０を介し
て入力し、予測ベクトルｘ _k「ハット」(−)を式(２２)
に従い算出して平滑器９に出力し、また、予測位置ｚ _k
「ハット」(−)を式(２４)に従い算出して道路高算出器
４に出力する(ステップＳＴ３)。道路高算出器４では、
予測器３の出力する予測位置ｚ _k「ハット」(−)と、ス
クリーン座標変換器２の出力する複数道路高を仮定した
場合の道路座標の位置観測ベクトルｚ _k[ｈ＝ｈ_i(ｋ−
１)] (i=1,2,・・・)を入力して、道路高の推定値ｈ
「ハット」(ｋ)を予測位置ｚ _k「ハット」(−)と、その
前後の位置観測ベクトルとを用いて式(３４)に従い算出
する(ステップＳＴ４)。

【００５０】道路高設定器５では、道路高算出器４の出
力する道路高の推定値ｈ「ハット」(ｋ)を基準とする複
数の道路高ｈ_i(ｋ) (i=1,2,・・・)を等間隔に設定する
(ステップＳＴ５)。道路高算出器４が道路高の推定値ｈ
「ハット」(ｋ)を算出すると、スクリーン座標変換器２
では、道路高算出器４から道路高の推定値ｈ「ハット」
(ｋ)を入力し、画像センサ装置１より入力した車両のス
クリーン上の位置を道路高の推定値ｈ「ハット」(ｋ)を
道路高とした場合の道路座標の位置観測ベクトルｚ _k[ｈ
＝ｈ「ハット」(ｋ)]に変換して平滑器９へ出力する(ス
テップＳＴ６)。

【００５１】予測誤差評価器７では、平滑誤差評価器１
１の出力する平滑誤差共分散行列Ｐ _k(＋)を第３の遅延
回路１２を介して入力し、予め設定された駆動雑音共分
散行列Ｑ_kを用いて、予測誤差共分散行列Ｐ_k(−)を式
(２３)に従い算出する(ステップＳＴ７)。ゲイン行列算
出器８では、予測誤差評価器７の出力する予測誤差共分
散行列Ｐ _k(−)と、予め設定された画像センサ装置１の
観測雑音共分散行列Ｒ_k ¹を用いて、ゲイン行列Ｋ_kを式
(２５)に従い算出する(ステップＳＴ８)。

【００５２】平滑器９では、スクリーン座標変換器２の
出力する道路高推定値ｈ「ハット」(ｋ)における位置観
測ベクトルｚ _k[ｈ＝ｈ「ハット」(ｋ)]と、ゲイン行列
算出器８の出力するゲイン行列Ｋ_kと、予測器３の出力
する予測ベクトルｘ _k「ハット」(−)を入力し、平滑ベ
クトルｘ _k「ハット」(＋)を式(２６)に従い算出する(ス
テップＳＴ９)。平滑誤差評価器１１では、ゲイン行列
算出器８の出力するゲイン行列Ｋ_kと、予測誤差評価器
７の出力する予測誤差共分散行列Ｐ_k(−)を入力し、平
滑誤差共分散行列Ｐ_k(＋)を式(２７)に従い算出する(ス
テップＳＴ１０)。

【００５３】以上、所望の道路範囲の道路高が推定され
るまでこの一連の処理をくり返す。

【００５４】以上で明らかなように、この実施の形態１
によれば、画像センサ装置にて走行する車両を観測する
ことにより逐次的に道路高を推定することができるた
め、道路高を短時間で推定することができる。また、測
量で道路高を測定する場合のように、道路を占有する必
要がない。

【００５５】なお、この発明は一般の道路以外の地表の
高度を推定する場合にも適用可能である。また、各サン
プリング間の車両の速度ベクトルＶ _kが既知の場合に
は、予測誤差評価器７から第３の遅延回路１２までの処
理は行わずに、予測器３において、車両の速度ベクトル
Ｖ _kを用いて式(３５)により予測位置を算出するように
してもよい。

【００５６】

【数１１】

【００５７】実施の形態２．図８はこの発明の実施の形
態２による道路高推定装置を示す構成図であり、図にお
いて、３〜７、９〜１２は実施の形態１と同じである。
１３は車両の距離及び方位角を観測するレーダ装置、１
４はレーダ装置１３で観測された車両の距離及び方位角
と、道路高設定器５で設定された複数道路高を用いて、
各道路高を仮定した場合の道路座標の位置観測ベクトル
を算出する極座標変換器、１５は予測誤差評価器７で算
出された予測誤差共分散行列と、予め設定されたレーダ
装置１３の観測諸元の観測雑音共分散行列とを用いて、
ゲイン行列を算出するレーダゲイン行列算出器である。
なお、この実施の形態２による道路高推定装置の動作を
示すフローチャートは省略するが、概略的流れは図２に
示した実施の形態１のものに同じである。

【００５８】次に動作について説明する。最初に、この
実施の形態２による道路高推定装置の動作原理を説明す
る。レーダ装置１３は実施の形態１の画像センサ装置と
同様に道路上に設置され、道路を走行する車両を観測す
る。図３のｏ−ｘｙｚ直交座標系を基準座標系とし、ｏ
−ｘｙ直交座標系を道路座標系とする。基準座標系にお
けるレーダ装置１３の取り付け位置は(ｘｒ，ｈｒ，ｚ
ｒ)とする。

【００５９】図９に示すように、レーダを原点とするレ
ーダ座標系をｏ^(r)−ｘ^(r)ｙ^(r)ｚ^{( r)}とする。レーダの
アンテナはｙ軸回りにγ(ｚ軸の方向より反時計回りを
正とする)だけ傾いて設置されているものとする。レー
ダは、車両の方位角Ａz(ｚ^(r)軸の方向より反時計回り
を正とする)、距離Ｒを観測するものとする。レーダ観
測座標を図１０に示す。図９および図１０より、式(３
６)及び式(３７)の関係がある。ここで、ｈは車両位置
における道路面の高さである。これらをｘおよびｚにつ
いて解くと式(３８)及び式(３９)を得る。

【００６０】

【数１２】

【００６１】車両の運動モデルは、実施の形態１と同様
に、式(８)で表されるものとする。

【００６２】次に、時刻ｔ_kに車両の位置ベクトルがレ
ーダ装置１３より観測される場合の観測モデルを式(４
０)とする。ここで、ｚ _kは式(３８)及び式(３９)を用い
て変換した、時刻ｔ_kにおけるレーダ装置１３の道路座
標における車両の位置観測ベクトルであり式(４１)で表
される。Ｈ₂はレーダ装置１３の観測行列であり、式(４
２)で表される。

【００６３】また、ｖ _k ²は時刻ｔ_kにおけるレーダ装置
１３の観測雑音ベクトルであり、平均０の２次元正規分
布白色雑音で、式(４３)及び(４４)で表される。なお、
Ｒ_k ²は時刻ｔ_kにおけるレーダ装置１３の観測諸元の観
測雑音共分散行列である。また、Γ₂ ²(ｋ)はレーダ観測
座標系より道路座標系への観測雑音ベクトルの変換行列
であり、式(４５)で表される。

【００６４】

【数１３】

【００６５】次に、レーダ装置１３により観測された時
刻ｔ_k-1までの観測ベクトルＺ_k-1 ²が得られているとき
の、時刻ｔ_kにおけるｘ _kの予測ベクトルをｘ _k ²「ハッ
ト」(−)、予測誤差共分散行列をＰ_k ²(−)とすると、そ
れぞれ条件付平均ベクトル、条件付共分散行列で定義さ
れ、式(４６)及び式(４７)で表される。また、観測ベク
トルの予測位置をｚ _k ²(−)とすると式(４８)で表され
る。ここで、ｘ _k ²「ハット」(＋)及びＰ_k ²(＋)はそれぞ
れ前時刻ｔ_k-1の平滑ベクトル及び平滑誤差共分散行列
である。

【００６６】

【数１４】

【００６７】次に、時刻ｔ_kにおいてレーダ装置１３に
より観測された車両の位置ベクトルを用いて車両の平滑
ベクトルを算出する平滑処理について述べる。ゲイン行
列Ｋ _k ²、平滑ベクトルｘ _k ²「ハット」(＋)及び平滑誤差
共分散行列Ｐ_k ²(＋)はカルマンフィルタの理論により、
式(４９)〜(５１)で与えられる。

【００６８】

【数１５】

【００６９】道路を走行している車両をレーダ装置１３
で観測することにより、道路高を推定する方法は実施の
形態１と同様である。ただし、時刻ｔ_kにおける車両の
予測位置Ｕ'ｋの道路座標はｚ _k ²(−)であり、ｚ _k ²(−)
はｚ _k ²[ｈ＝ｈ_j(ｋ−１)]とｚ _k ²[ｈ＝ｈ_j+1(ｋ−１)]の
間にあるとする。ここで、ｚ _k ²[ｈ＝ｈ_i(ｋ)] (i=1,2,
・・・)は道路高ｈ_i(ｋ)における車両の位置観測ベクト
ルである。従って、時刻ｔ_kにおける道路高の推定値ｈ
「ハット」(ｋ)は式(５２)を用いて算出する。

【００７０】

【数１６】

【００７１】次にこの発明の道路高推定装置の動作を具
体的に説明する。なお、カルマンフィルタを目標追尾装
置に通常適用する場合と同様にして、初期値値ｘ ₀ ²「ハ
ット」(＋)、Ｐ₀ ²(＋)は別途定まっているとする。

【００７２】レーダ装置１３では、レーダ観測座標系で
車両の距離及び方位角を検出し、その結果を極座標変換
器１４に出力する。極座標変換器１４では、レーダ装置
１３から車両の距離及び方位角を入力し、道路高設定器
５で設定した複数道路高を第１の遅延回路６を介して入
力し、各道路高を仮定した場合の道路座標の位置観測ベ
クトルｚ _k ²[ｈ＝ｈ_i(ｋ−１)] (i=1,2,・・・)を式(３
８)および式(３９)に従い算出する。

【００７３】予測器３では、平滑器９の出力する平滑ベ
クトルｘ _k ²「ハット」(＋)を第２の遅延回路１０を介し
て入力し、予測ベクトルｘ _k ²「ハット」(−)を式(４６)
に従い算出して平滑器９に出力し、また、予測位置ｚ _k ²
「ハット」(−)を式(４８)に従い算出して道路高算出器
４に出力する。道路高算出器４では、予測器３の出力す
る予測位置ｚ _k ²「ハット」(−)と、極座標変換器１４の
出力する複数道路高を仮定した場合の道路座標の位置観
測ベクトルｚ _k ²[ｈ＝ｈ_i(ｋ−１)] (i=1,2,・・・)を入
力して、道路高の推定値ｈ「ハット」(ｋ)を予測位置ｚ
_k ²「ハット」(−)と、その前後の位置観測ベクトルとを
用いて式(５２)に従い算出する。

【００７４】道路高設定器５では、道路高算出器４の出
力する道路高の推定値ｈ「ハット」(ｋ)を基準とする複
数の道路高ｈ_i(ｋ) (i=1,2,・・・)を等間隔に設定す
る。道路高算出器４が道路高の推定値ｈ「ハット」(ｋ)
を算出すると、極座標変換器１４では、道路高算出器４
から道路高の推定値ｈ「ハット」(ｋ)を入力し、前回レ
ーダ装置１３より入力した車両の距離及び方位角を道路
高の推定値ｈ「ハット」(ｋ)を道路高とした場合の道路
座標の位置観測ベクトルｚ _k ²[ｈ＝ｈ「ハット」(ｋ)]に
変換して平滑器９へ出力する。

【００７５】予測誤差評価器７では、平滑誤差評価器１
１の出力する平滑誤差共分散行列Ｐ _k ²(＋)を第３の遅延
回路１２を介して入力し、予め設定された駆動雑音共分
散行列Ｑ_kを用いて、予測誤差共分散行列Ｐ_k ²(−)を式
(４７)に従い算出する。レーダゲイン行列算出器１５で
は、予測誤差評価器７の出力する予測誤差共分散行列Ｐ
_k ²(−)と、予め設定されたレーダ装置１３の観測諸元の
観測雑音共分散行列Ｒ_k ²を用いて、ゲイン行列Ｋ_k ²を式
(４９)に従い算出する。

【００７６】平滑器９では、極座標変換器１４の出力す
る道路高推定値ｈ「ハット」(ｋ)における位置観測ベク
トルｚ _k ²[ｈ＝ｈ「ハット」(ｋ)]と、レーダゲイン行列
算出器１５の出力するゲイン行列Ｋ_k ²と、予測器３の出
力する予測ベクトルｘ _k ²「ハット」(−)を入力し、平滑
ベクトルｘ _k ²「ハット」(＋)を式(５０)に従い算出す
る。平滑誤差評価器１１では、ゲイン行列算出器８の出
力するゲイン行列Ｋ_k ²と、予測誤差評価器７の出力する
予測誤差共分散行列Ｐ_k ²(−)を入力し、平滑誤差共分散
行列Ｐ_k ²(＋)を式(５１)に従い算出する。

【００７７】以上、所望の道路範囲の道路高が推定され
るまでこの一連の処理をくり返す。

【００７８】以上で明らかなように、この実施の形態２
によれば、レーダ装置にて走行する車両を観測すること
により逐次的に道路高を推定することができるため、道
路高を短時間で推定することができる。また、測量で道
路高を測定する場合のように、道路を占有する必要がな
い。

【００７９】なお、この発明は一般の道路以外の地表の
高度を推定する場合にも適用可能である。また、レーダ
装置の代わりにレーザー装置などの位置計測センサを用
いても同様の効果が得られる。また、各サンプリング間
の車両の速度ベクトルＶ _kが既知の場合には、予測誤差
評価器７から第３の遅延回路１２までの処理は行わず
に、予測器３において、車両の速度ベクトルＶ _kを用い
て式(３５)と同じように予測位置を算出するようにして
もよい。

【００８０】実施の形態３．図１１はこの発明の実施の
形態３による道路高推定装置を示す構成図であり、図に
おいて、１〜７、１０〜１２は実施の形態１と同じであ
る。１６は車両の距離、方位角及び距離変化率を観測す
る測速度レーダ装置、１７は測速度レーダ装置１６の出
力する距離、方位角及び距離変化率と、道路高算出器４
の出力する推定道路高を用いて推定道路高における車両
の速度を算出する速度算出器である。

【００８１】１８は予測誤差評価器７で算出された予測
誤差共分散行列と、予め設定された画像センサ装置１の
観測雑音共分散行列と、予め設定された測速度レーダの
観測諸元の距離変化率の観測誤差共分散行列を用いて、
ゲイン行列を算出する位置と速度によるゲイン行列算出
器、１９は座標変換器２で算出された推定道路高におけ
る道路座標の位置観測ベクトルと、速度算出器１７で算
出された推定道路高における車両の速度と、位置と速度
によるゲイン行列算出器１８で算出されたゲイン行列
と、予測器３で算出された予測ベクトルを用いて平滑ベ
クトルを算出する位置と速度による平滑器である。な
お、この実施の形態３による道路高推定装置の動作を示
すフローチャートは省略するが、概略的流れは図２に示
したものに類似する。

【００８２】次に動作について説明する。最初に、この
実施の形態３による道路高推定装置の動作原理を説明す
る。画像センサ装置１及び測速度レーダ装置１６は道路
上に設置され(図３および図１２参照)、道路を走行する
車両を観測する。実施の形態１と同様に、図３のｏ−ｘ
ｙｚ直交座標系を基準座標系とし、ｏ−ｘｙ直交座標系
を道路座標系とする。基準座標系における画像センサ装
置１および測速度レーダ装置１６の取り付け位置は、そ
れぞれ(ｘｃ，ｈｃ，ｚｃ)および(ｘｒ，ｈｒ，ｚｒ)と
する。画像センサ装置１の観測座標系と道路座標との関
係は、実施の形態１と同様に、式(６)及び式(７)により
得られる。

【００８３】測速度レーダ装置１６の観測座標系は実施
の形態２におけるレーダ装置１３と同様に、図９及び図
１０に示すように配置され、車両の方位角Ａz(ｚ^(r)軸
の方向より反時計回りを正とする)、距離Ｒ、距離変化
率Ｒ「ドット」を観測する。時刻ｔ_kにおける車両の方
位角をＡz_k、距離距離変化率をそれぞれＲ_k、Ｒ_k「ドッ
ト」、道路高をｈとし、車両は道路をｚ軸方向に速度ｖ
_kで直進しているものとすると、速度ｖ_kは図９と図１２
の関係から式(５３)で表される。

【００８４】

【数１７】

【００８５】車両の運動モデルは、実施の形態１と同様
に、式(８)で表されるものとする。

【００８６】次に、時刻ｔ_kに車両の位置ベクトルが画
像センサ装置１より観測され、同時刻に車両の速度が測
速度レーダ装置１６より観測される場合の観測モデルを
式(５４)とする。ここで、ｚ _k ³は式(６)及び式(７)を用
いて変換した画像センサ装置１による道路座標における
車両の観測位置(ｘ_o(ｋ)，ｚ_o(ｋ))と、式(５３)によっ
て変換した測速度レーダ装置１６による車両の観測速度
ｖ_o(ｋ)から成るベクトルであり式(５５)で表される。
Ｈ₃は画像センサ装置１および測速度レーダ装置１６の
観測行列であり、式(５６)で表される。

【００８７】また、ｖ _k ³は時刻ｔ_kにおける画像センサ
装置１の観測諸元と測速度レーダ装置１６で観測される
距離変化率の観測雑音ベクトルであり、平均０の３次元
正規分布白色雑音で、式(５７)及び(５８)で表される。
なお、Ｒ_k ³は時刻ｔ_kにおける画像センサ装置１の観測
諸元と測速度レーダ装置１６で観測される距離変化率の
観測雑音共分散行列である。また、Γ₂ ³(ｋ)は画像セン
サ観測座標系とレーダ観測座標系より道路座標系への観
測雑音ベクトルの変換行列であり、式(５９)で表され
る。

【００８８】

【数１８】

【００８９】次に、画像センサ装置１と測速度レーダ装
置１６により観測された時刻ｔ_k-1までの観測ベクトル
Ｚ_k-1 ³が得られているときの、時刻ｔ_kにおけるｘ _kの予
測ベクトルをｘ _k ³「ハット」(−)、予測誤差共分散行列
をＰ_k ³(−)とすると、それぞれ条件付平均ベクトル、条
件付共分散行列で定義され、式(６０)及び式(６１)で表
される。また、観測ベクトルの予測値をｚ _k ³(−)とする
と式(６２)で表される。ここで、ｘ _k ³「ハット」(＋)及
びＰ_k ³(＋)はそれぞれ前時刻ｔ_k-1の平滑ベクトル及び
平滑誤差共分散行列である。

【００９０】

【数１９】

【００９１】次に、時刻ｔ_kにおいて、画像センサ装置
１と測速度レーダ装置１６により観測された車両の位置
及び速度を用いて車両の平滑ベクトルを算出する平滑処
理について述べる。ゲイン行列Ｋ_k ³、平滑ベクトルｘ _k ³
「ハット」(＋)及び平滑誤差共分散行列Ｐ_k ³(＋)はカル
マンフィルタの理論により、式(６３)〜(６５)で与えら
れる。

【００９２】

【数２０】

【００９３】道路を直進している車両を画像センサ装置
１および測速度レーダ装置１６で観測することにより、
道路高を推定する方法は実施の形態１と同様である。た
だし、時刻ｔ_kにおける車両の予測位置Ｕ'ｋの道路座標
はｚ _k ⁴(−)であり、ｚ _k ⁴(−)はｚ _k ⁴[ｈ＝ｈ_j(ｋ−１)]
とｚ _k ⁴[ｈ＝ｈ_j+1(ｋ−１)]の間にあるとする。ここ
で、ｚ _k ⁴(−)は観測ベクトルの予測値ｚ _k ³(−)の位置の
要素を取り出した位置予測ベクトルであり、式(６６)よ
り算出する。同様に、ｚ _k ⁴は車両の観測ベクトルｚ _k ³の
位置の要素を取り出した位置観測ベクトルであり、式
(６７)より算出する。また、ｚ _k ⁴[ｈ＝ｈ_i(ｋ)] (i=1,
2,・・・)は道路高ｈ_i(ｋ)における車両の位置観測ベク
トルである。従って、時刻ｔ_kにおける道路高の推定値
ｈ「ハット」(ｋ)は式(６８)を用いて算出する。

【００９４】

【数２１】

【００９５】次にこの発明の道路高推定装置の動作を具
体的に説明する。なお、カルマンフィルタを目標追尾装
置に通常適用する場合と同様にして、初期値ｘ ₀ ³「ハッ
ト」(＋)、Ｐ₀ ³(＋)は別途定まっているとする。

【００９６】画像センサ装置１では、画像センサ観測座
標系で車両の位置(スクリーン上の位置)を検出し、その
結果を座標変換器２に出力する。座標変換器２では、画
像センサ装置１から車両のスクリーン上の位置と、道路
高設定器５の出力する複数道路高を第１の遅延回路６を
介して入力し、各道路高を仮定した場合の道路座標の位
置観測ベクトルｚ _k ⁴[ｈ＝ｈ_i(ｋ)] (i=1,2,・・・)を式
(６)及び式(７)に従い算出する。

【００９７】一方、測速度レーダ装置１６では、レーダ
観測座標系で車両の距離、方位角、距離変化率を検出
し、その結果を速度算出器１７に出力する。

【００９８】予測器３では、位置と速度による平滑器１
９の出力する平滑ベクトルｘ _k ³「ハット」(＋)を第２の
遅延回路１０を介して入力し、予測ベクトルｘ _k ³「ハッ
ト」(−)を式(６０)に従い算出して位置と速度による平
滑器１９に出力し、また、予測位置ｚ _k ⁴(−)を式(６２)
及び式(６６)に従い算出し、道路高算出器４に出力す
る。道路高算出器４では、予測器３の出力する予測位置
ｚ _k ³(−)と、座標変換器２の出力する複数道路高を仮定
した場合の道路座標の位置観測ベクトルｚ _k ⁴[ｈ＝ｈ
_i(ｋ−１)] (i=1,2,・・・)を入力して、道路高の推定
値ｈ「ハット」(ｋ)を予測位置ｚ _k ⁴(−)と、その前後の
位置観測ベクトルとを用いて式(６８)に従い算出する。

【００９９】道路高設定器５では、道路高算出器４の出
力する道路高の推定値ｈ「ハット」(ｋ)を基準とする複
数の道路高ｈ_i(ｋ) (i=1,2,・・・)を等間隔に設定す
る。道路高算出器４が道路高の推定値ｈ「ハット」(ｋ)
を算出すると、座標変換器２では、道路高算出器４から
道路高の推定値ｈ「ハット」(ｋ)を入力し、前回入力し
た画像センサ装置１の車両のスクリーン上の位置に対し
て、道路高の推定値ｈ「ハット」(ｋ)を道路高とした場
合の道路座標の位置観測ベクトルを算出し、位置と速度
による平滑器１９へ出力する。

【０１００】速度算出器１７では、道路高算出器４の出
力する道路高の推定値ｈ「ハット」(ｋ)と、測速度レー
ダ装置１６の出力する車両の距離、方位角及び距離変化
率を入力し、車両の速度ｖ_kを式(５３)に従い算出す
る。ここで、座標変換器２の出力する道路高の推定値ｈ
「ハット」(ｋ)を道路高とした場合の道路座標の位置観
測ベクトルと、速度算出器１７の出力する車両の速度ｖ
_kを合わせて観測ベクトルｚ _k ³[ｈ＝ｈ「ハット」(ｋ)]
と表記される。

【０１０１】予測誤差評価器７では、平滑誤差評価器１
１の出力する平滑誤差共分散行列Ｐ _k ³(＋)を第３の遅延
回路１２を介して入力し、予め設定された駆動雑音共分
散行列Ｑ_kを用いて、予測誤差共分散行列Ｐ_k ³(−)を式
(６１)に従い算出する。位置と速度によるゲイン行列算
出器１８では、予測誤差評価器７の出力する予測誤差共
分散行列Ｐ_k ³(−)と、予め設定された画像センサ装置１
と測速度レーダ装置１６の観測雑音共分散行列Ｒ_k ³を用
いて、ゲイン行列Ｋ_k ³を式(６３)に従い算出する。

【０１０２】位置と速度による平滑器１９では、座標変
換器２の出力する道路高推定値ｈ「ハット」(ｋ)におけ
る位置観測ベクトルと速度算出器１７の出力する車両の
速度ｖ_kを合わせた観測ベクトルｚ _k ³[ｈ＝ｈ「ハット」
(ｋ)]と、位置と速度によるゲイン行列算出器１８の出
力するゲイン行列Ｋ_k ³と、予測器３の出力する予測ベク
トルｘ _k ³「ハット」(−)を入力し、平滑ベクトルｘ
_k ³「ハット」(＋)を式(６４)に従い算出する。平滑誤差
評価器１１では、位置と速度によるゲイン行列算出器１
８の出力するゲイン行列Ｋ_k ³と、予測誤差評価器７の出
力する予測誤差共分散行列Ｐ_k ³(−)を入力し、平滑誤差
共分散行列Ｐ_k ³(＋)を式(６５)に従い算出する。

【０１０３】以上、所望の道路範囲の道路高が推定され
るまでこの一連の処理をくり返す。

【０１０４】以上で明らかなように、この実施の形態３
によれば、画像センサ装置にて直進する車両を観測する
ことにより道路高を自動的に推定することができるた
め、道路高を短時間で推定することができる。また、測
量で道路高を測定する場合のように、道路を占有する必
要がない。さらに、測速度レーダ装置にて車両の速度を
計測し、位置と速度によって車両の位置を推定するた
め、車両が一定速度で走行していない場合でも道路高の
推定精度の劣化が小さい。

【０１０５】なお、この発明は一般の道路以外の地表の
高度を推定する場合にも適用可能である。また、車両位
置の観測に画像センサを使わずに、測速度レーダ装置で
観測される距離及び方位角を使用しても同様の効果が得
られる。その場合の位置観測ベクトルの扱いは、実施の
形態２のレーダ装置と同様である。

【０１０６】

【発明の効果】以上のようにこの発明によれば、基準の
平面に対する高低差を推定する高度推定装置であって、
対象領域上に移動体を走らせ、観測された移動体の位置
を仮定された複数の高さの候補に基づき２次元直交座標
に変換し、この２次元直交座標上のその前に観測された
移動体の位置に基づく移動体の予測位置と上記観測位置
から道路の高低差を算出することを特徴とする高度推定
装置およびその方法としたので、対象領域上に移動体を
走らせこれの位置を測定することにより、高度を自動的
に推定することができる。

【０１０７】また、上記高度推定装置が道路高推定装置
であって、移動体の画像を観測する画像観測手段と、画
像観測手段により観測された移動体の画像の注目位置を
仮定された複数の道路高の候補に基づき２次元直交座標
に変換する座標変換手段と、前サンプリング時に観測さ
れた移動体の２次元直交座標上の位置に基づき移動体の
予測位置を算出する予測手段と、予測手段によって算出
された予測位置と座標変換手段によって変換された複数
の道路高の候補に基づく２次元直交座標上の移動体の観
測位置から道路の高低差を算出する道路高算出手段と、
を備えたことを特徴とする上記高度推定装置およびこれ
に基づく方法としたので、画像センサ装置等の画像観測
装置にて走行する車両を観測することにより逐次的に道
路高を推定することができるため、道路高を短時間で推
定することができ、また測量で道路高を測定する場合の
ように道路を占有する必要がない。

【０１０８】また、上記高度推定装置が道路高推定装置
であって、移動体の距離と方位角を観測する観測手段
と、観測手段により観測された移動体の距離と方位角を
仮定された複数の道路高の候補に基づき２次元直交座標
に変換する座標変換手段と、前サンプリング時に観測さ
れた移動体の２次元直交座標上の位置に基づき移動体の
予測位置を算出する予測手段と、予測手段によって算出
された予測位置と座標変換手段によって変換された複数
の道路高の候補に基づく２次元直交座標上の移動体の観
測位置から道路の高低差を算出する道路高算出手段と、
を備えたことを特徴とする上記高度推定装置およびこれ
に基づく方法としたので、レーダ装置等の移動体の距離
と方位角を観測する観測手段にて走行する車両を観測す
ることにより逐次的に道路高を推定することができるた
め、道路高を短時間で推定することができ、また測量で
道路高を測定する場合のように道路を占有する必要がな
い。

【０１０９】また、道路高算出手段によって算出された
道路高に基づく移動体の２次元直交座標上の位置と予測
手段により算出された前サンプリング時の予測位置から
移動体の位置の平滑値を算出する平滑手段を設け、予測
手段は平滑手段の算出する平滑値によって予測位置を算
出するようにしたので、より正確に道路高を推定するこ
とができる。

【０１１０】また、移動体の距離変化率を観測する距離
変化率観測手段と、距離変化率観測手段により観測され
た移動体の距離変化率を道路高算出手段によって算出さ
れた道路高に基づく移動体の速度に変換する速度算出手
段とを設け、平滑手段は道路高算出手段によって算出さ
れた道路高に基づく移動体の２次元直交座標上の位置と
速度算出手段によって算出された移動体の速度と予測手
段により算出された前サンプリング時の予測位置から移
動体の位置の平滑値を算出するようにしたので、上記効
果に加えてさらに、距離変化率観測手段および速度算出
手段にて車両の速度を計測し、位置と速度によって車両
の位置を推定するため、車両が一定速度で走行していな
い場合でも道路高の推定精度の劣化が小さい。

【図面の簡単な説明】

【図１】 この発明の実施の形態１による道路高推定装
置を示す構成図である。

【図２】 この発明の実施の形態１による道路高推定装
置の動作を示すフローチャート図である。

【図３】 この発明の実施の形態１による道路高推定装
置の原理を説明するための図である。

【図４】 この発明の実施の形態１による道路高推定装
置の原理を説明するための図である。

【図５】 この発明の実施の形態１による道路高推定装
置の原理を説明するための図である。

【図６】 この発明の実施の形態１による道路高推定装
置の原理を説明するための図である。

【図７】 この発明の実施の形態１による道路高推定装
置の原理を説明するための図である。

【図８】 この発明の実施の形態２による道路高推定装
置を示す構成図である。

【図９】 この発明の実施の形態２による道路高推定装
置の原理を説明するための図である。

【図１０】 この発明の実施の形態２による道路高推定
装置の原理を説明するための図である。

【図１１】 この発明の実施の形態３による道路高推定
装置を示す構成図である。

【図１２】 この発明の実施の形態３による道路高推定
装置の原理を説明するための図である。

【図１３】 従来の高低差測定方法を示す図である。

【符号の説明】

１ 画像センサ装置、２ スクリーン座標変換器、３
予測器、４ 道路高算出器、５ 道路高設定器、６ 第
１の遅延回路、７ 予測誤差評価器、８ ゲイン行列算
出器、９ 平滑器、１０ 第２の遅延回路、１１ 平滑
誤差評価器、１２ 第３の遅延回路、１３ レーダ装
置、１４ 極座標変換器、１５ レーダゲイン行列算出
器、１６ 測速度レーダ装置、１７ 速度算出器、１８
位置と速度によるゲイン行列算出器、１９ 位置と速
度による平滑器。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 小菅 義夫 東京都千代田区丸の内二丁目２番３号 三 菱電機株式会社内 Ｆターム(参考） 5J070 AC01 AC03 AC06 AC13 AE01 AF01 AK22 BD08 5J084 AA04 AB01 AD05 BA03 CA65 EA04

Claims (10)

【特許請求の範囲】
1. 【請求項１】 基準の平面に対する高低差を推定する高
   度推定装置であって、対象領域上に移動体を走らせ、観
   測された移動体の位置を仮定された複数の高さの候補に
   基づき２次元直交座標に変換し、この２次元直交座標上
   のその前に観測された移動体の位置に基づく移動体の予
   測位置と上記観測位置から道路の高低差を算出すること
   を特徴とする高度推定装置。
2. 【請求項２】 上記高度推定装置が道路高推定装置であ
   って、 移動体の画像を観測する画像観測手段と、 画像観測手段により観測された移動体の画像の注目位置
   を仮定された複数の道路高の候補に基づき２次元直交座
   標に変換する座標変換手段と、 前サンプリング時に観測された移動体の２次元直交座標
   上の位置に基づき移動体の予測位置を算出する予測手段
   と、 予測手段によって算出された予測位置と座標変換手段に
   よって変換された複数の道路高の候補に基づく２次元直
   交座標上の移動体の観測位置から道路の高低差を算出す
   る道路高算出手段と、 を備えたことを特徴とする請求項１に記載の高度推定装
   置。
3. 【請求項３】 上記高度推定装置が道路高推定装置であ
   って、 移動体の距離と方位角を観測する観測手段と、 観測手段により観測された移動体の距離と方位角を仮定
   された複数の道路高の候補に基づき２次元直交座標に変
   換する座標変換手段と、 前サンプリング時に観測された移動体の２次元直交座標
   上の位置に基づき移動体の予測位置を算出する予測手段
   と、 予測手段によって算出された予測位置と座標変換手段に
   よって変換された複数の道路高の候補に基づく２次元直
   交座標上の移動体の観測位置から道路の高低差を算出す
   る道路高算出手段と、 を備えたことを特徴とする請求項１に記載の高度推定装
   置。
4. 【請求項４】 道路高算出手段によって算出された道路
   高に基づく移動体の２次元直交座標上の位置と予測手段
   により算出された前サンプリング時の予測位置から移動
   体の位置の平滑値を算出する平滑手段を設け、予測手段
   は平滑手段の算出する平滑値によって予測位置を算出す
   ることを特徴とする請求項２または３に記載の高度推定
   装置。
5. 【請求項５】 移動体の距離変化率を観測する距離変化
   率観測手段と、距離変化率観測手段により観測された移
   動体の距離変化率を道路高算出手段によって算出された
   道路高に基づく移動体の速度に変換する速度算出手段と
   を設け、平滑手段は道路高算出手段によって算出された
   道路高に基づく移動体の２次元直交座標上の位置と速度
   算出手段によって算出された移動体の速度と予測手段に
   より算出された前サンプリング時の予測位置から移動体
   の位置の平滑値を算出することを特徴とする請求項４に
   記載の高度推定装置。
6. 【請求項６】 基準の平面に対する高低差を推定する高
   度推定方法であって、対象領域上に移動体を走らせ、観
   測された移動体の位置を仮定された複数の高さの候補に
   基づき２次元直交座標に変換し、この２次元直交座標上
   のその前に観測された移動体の位置に基づく移動体の予
   測位置と上記観測位置から道路の高低差を算出すること
   を特徴とする高度推定方法。
7. 【請求項７】 上記高度推定方法が道路高推定方法であ
   って、 移動体の画像を観測する画像観測ステップと、 画像観測ステップにより観測された移動体の画像の注目
   位置を仮定された複数の道路高の候補に基づき２次元直
   交座標に変換する座標変換ステップと、 前サンプリング時に観測された移動体の２次元直交座標
   上の位置に基づき移動体の予測位置を算出する予測ステ
   ップと、 予測ステップによって算出された予測位置と座標変換ス
   テップによって変換された複数の道路高の候補に基づく
   ２次元直交座標上の移動体の観測位置から道路の高低差
   を算出する道路高算出ステップと、 を設けたことを特徴とする請求項６に記載の高度推定方
   法。
8. 【請求項８】 上記高度推定方法が道路高推定方法であ
   って、 移動体の距離と方位角を観測する観測ステップと、 観測ステップにより観測された移動体の距離と方位角を
   仮定された複数の道路高の候補に基づき２次元直交座標
   に変換する座標変換ステップと、 前サンプリング時に観測された移動体の２次元直交座標
   上の位置に基づき移動体の予測位置を算出する予測ステ
   ップと、 予測ステップによって算出された予測位置と座標変換ス
   テップによって変換された複数の道路高の候補に基づく
   ２次元直交座標上の移動体の観測位置から道路の高低差
   を算出する道路高算出ステップと、 を設けたことを特徴とする請求項６に記載の高度推定方
   法。
9. 【請求項９】 道路高算出ステップによって算出された
   道路高に基づく移動体の２次元直交座標上の位置と予測
   ステップにより算出された前サンプリング時の予測位置
   から移動体の位置の平滑値を算出する平滑ステップを設
   け、予測ステップは平滑ステップの算出する平滑値によ
   って予測位置を算出することを特徴とする請求項７また
   は８に記載の高度推定方法。
10. 【請求項１０】 移動体の距離変化率を観測する距離変
    化率観測ステップと、距離変化率観測ステップにより観
    測された移動体の距離変化率を道路高算出ステップによ
    って算出された道路高に基づく移動体の速度に変換する
    速度算出ステップとを設け、平滑ステップは道路高算出
    ステップによって算出された道路高に基づく移動体の２
    次元直交座標上の位置と速度算出ステップによって算出
    された移動体の速度と予測ステップにより算出された前
    サンプリング時の予測位置から移動体の位置の平滑値を
    算出することを特徴とする請求項９に記載の高度推定方
    法。