JPWO2017145541A1

JPWO2017145541A1 - 移動体

Info

Publication number: JPWO2017145541A1
Application number: JP2018501027A
Authority: JP
Inventors: アレックス益男金子; 山本　健次郎; 健次郎山本
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 2016-02-23
Filing date: 2017-01-12
Publication date: 2018-11-08
Anticipated expiration: 2037-01-12
Also published as: EP3422293A1; WO2017145541A1; JP6708730B2; EP3422293B1; US20190066312A1; EP3422293A4; US10740908B2

Abstract

カメラ画像に静止もしくは移動する立体物が映っている場合でも、低処理負荷、かつ高精度な移動量算出が可能な移動体を提供することを目的とする。移動体は、路面の画像を撮像する撮像装置と、撮像装置が撮像した画像に基づいて、移動体の移動量を算出する画像処理部と、を備え、該画像処理部は、第１のタイミングで撮像された第１の画像から第１の特徴点を複数抽出し、第１のタイミングより後の第２のタイミングで撮像された第２の画像から第２の特徴点を複数抽出し、複数の第１の特徴点の各々を複数の第２の特徴点の各々にトラッキングし、複数の第２の特徴点の各々の移動量と速度を算出し、複数の第２の特徴点のうち、その速度が所定範囲内の特徴点に基づいて、移動体の移動量を算出する。

Description

本発明は、ロボットや自動車などの移動体に関する。

ロボットや自動車などの移動体において、安全性や利便性向上のため、周囲環境の情報を検出し、状況に応じた走行制御を行う自律走行技術及び運転支援技術が開発されている。

移動体の移動量を算出する例として、移動体のカメラで撮影したカメラ画像に基づいて所定の微小時間間隔Δｔにおける移動量を表す変位ベクトルΔＳを算出する技術がある（特許文献１参照）。該文献では、ベクトルΔＳを算出するために、車道にある特徴点をなるべく多く抽出し、各特徴点の移動量の平均値を移動体の移動量としている。

特開２００３−１７８３０９号公報

しかし、特許文献１のように、多数の特徴点の移動量の平均値をとっても、精度を高められるとは限らない。例えば、カメラ画像の半分以上に立体物が映っていれば、立体物の高さや移動量の影響で、特徴点のトラッキングの失敗もしくは、移動体の移動量を誤る場合がある。

そこで、本発明の目的は、カメラ画像に静止もしくは移動する立体物が映っている場合でも、低処理負荷、かつ高精度な移動量算出が可能な移動体を提供することにある。

上記課題を解決するために、代表的な本発明の移動体は、路面の画像を撮像する撮像装置と、撮像装置が撮像した画像に基づいて、移動体の移動量を算出する画像処理部と、を備え、該画像処理部は、第１のタイミングで撮像された第１の画像から第１の特徴点を複数抽出し、第１のタイミングより後の第２のタイミングで撮像された第２の画像から第２の特徴点を複数抽出し、複数の第１の特徴点の各々を複数の第２の特徴点の各々にトラッキングし、複数の第２の特徴点の各々の移動量と速度を算出し、複数の第２の特徴点のうち、その速度が所定範囲内の特徴点に基づいて、移動体の移動量を算出する。

本発明によれば、カメラ画像に静止もしくは移動する立体物が映っている場合でも、低処理負荷、かつ高精度な移動量算出が可能な移動体を提供することができる。

移動体の構成図。画像処理部のフローチャート。速度フィルタリングの詳細図。道路での移動量算出の説明図。立体物の高さ及び距離算出の説明図。

以下、図面を用いて実施例を説明する。図１は、移動体の構成図である。

移動体１は、周囲環境の画像を撮像する撮像装置２、撮像装置２が撮像した画像を処理して移動体１の移動量を算出し、算出結果に応じた表示あるいは制御用の信号を出力する処理装置３により構成されている。

処理装置３は、計算機システム等により構成され、撮像装置２が撮像した画像を処理する画像処理部４、該処理された画像に基づき種々の制御を行う制御部（ＣＰＵ）５、移動量算出等のために制御部で使用する各種データを格納するメモリ６、制御部５の演算結果等を出力する表示部７、及び、これら構成要素を相互に接続するバス８を備えている。

撮像装置２は、例えば、移動体１の前方に向けられて設置された単眼カメラやステレオカメラである。撮像装置２が単眼カメラの場合、路面が平らであれば、画像上のピクセル位置と実際の位置関係（ｘ,ｙ）が一定になるため、特徴点を幾何学的に計算できる。撮像装置２がステレオカメラの場合、特徴点までの距離をより正確に計測できる。以下では説明を簡単にするため、１台の標準カメラを採用した事例について説明するが、走行中に特徴点が抽出可能な視野角を有するものであれば、標準カメラでも広角カメラでもステレオカメラでもよい。どのカメラも最終的に１枚の画像を生成するものであり、複数台のカメラを組み合わせて撮像装置２を構成してもよい。

撮像装置２は、制御部５から指令が入力された時、もしくは一定の時間間隔で画像を取得し、取得した画像と取得時間を、メモリ６を介して画像処理部４に出力する。取得した画像の原画と取得時間はメモリ６に格納された上で、画像処理部４において中間的な加工画像が作成される。これらの中間画像も適宜必要に応じてメモリ６に格納され、制御部５などの判断や処理に利用される。尚、制御部５での処理に用いられた結果データなども適宜メモリ６に格納される。

各ブロック間のデータの伝送を行うバス８は、ＩＥＢＵＳ（ＩｎｔｅｒＥｑｕｉｐｍｅｎｔＢｕｓ）やＬＩＮ（ＬｏｃａｌＩｎｔｅｒｃｏｎｎｅｃｔＮｅｔｗｏｒｋ）やＣＡＮ（ＣｏｎｔｒｏｌｌｅｒＡｒｅａＮｅｔｗｏｒｋ）などで構成できる。
画像処理部４は、移動体１の走行中に撮像装置２が撮像画像に基づいて、移動量を算出する。まず、撮像装置２から伝送された画像上の特徴点を抽出する。更に、次に伝送された画像上の特徴点も抽出する。そして、先に（前回）抽出された特徴点と次に（今回）抽出された特徴点をトラッキング（特徴点同士を対応付ける処理：図４参照）し、移動体の移動量を算出し、その結果を制御部５に出力する。

制御部５は、画像処理部４で算出した移動量に基づいて、移動体１の位置を算出し、将来の移動方向や速度を決定し、移動体１を制御する。そして、必要な検知結果を表示部７に表示することで、移動体１の運転員に情報提供を行う。

図２は、画像処理部４のフローチャートを示す図である。

画像処理部４は、撮像装置２が撮像した画像をメモリ６から取得する（ステップ２０）。

次に、ステップ２０で取得した画像上に、特徴点を抽出するための関心領域を設定する（ステップ２１）。例えば単眼カメラの場合、幾何学的に画像のピクセル位置から道路路面のメートル位置に精度良く変換可能な走行路面を関心領域に設定する。

次に、ステップ２１で設定した関心領域の中から特徴点を抽出する（ステップ２２）。特徴点は、画像上のエッジやコーナーなどであり、Ｃａｎｎｙ、Ｓｏｂｅｌ、ＦＡＳＴ、Ｈｅｓｓｉａｎ、Ｇａｕｓｓｉａｎなどの技術を用いる。

次に、第1のタイミングで撮像された画像（前フレーム）、即ち、第2のタイミングで撮像された画像（現フレーム）よりも前に撮像された画像から抽出した特徴点を、今回撮像された画像上でトラッキングする（ステップ２３）。トラッキングにはＬｕｃａｓ−Ｋａｎａｄｅ法やＳｈｉ−Ｔｏｍａｓｉ法などの技術を用いる。

次に、ステップ２３のトラッキングが成功したか否かをチェックする（ステップ２４）。該トラッキングに成功すればステップ２５に進み、失敗すれば処理を終了する。尚、トラッキングには異なるフレームで撮像した２枚の画像が必要であるため、移動体１を起動して最初に撮像した一枚目についてはステップ２３でトラッキングできず、ステップ２４の後、処理を終了する。

トラッキングに成功した場合、トラッキングできた各特徴点の移動量と速度を算出する（ステップ２５）。例えば、トラッキングできた特徴点の点数をｎとし、移動体１の移動量算出回数をｍとする。各特徴点１、…、ｎの移動量は、前フレームで抽出した特徴点の相対位置ｄ_（m-1)nと今回抽出した特徴点の相対位置ｄ_ｍｎの差分（Δｄ_ｍｎ＝ｄ_ｍｎ−ｄ_{（ｍ−１)ｎ})で算出する。前フレームと今回の撮像時刻の取得時間の差分Δｔ_ｍを用いて、各特徴点の相対速度ｖ_ｍｎ＝Δｄ_ｍｎ /Δｔ_ｍを算出する。

次に、ステップ２５で算出した各特徴点の相対速度ｖ_ｍｎと時系列に保存した移動体１の速度Ｖ_ｍと比較し、時系列に保存した移動体１の速度と前記相対速度ｖ_ｍｎの差分が定められた値より大きい特徴点をフィルタリング（除去）する（ステップ２６）。

次に、ステップ２６でフィルタリングされなかった特徴点を用い、移動体１の移動量ΔＤ_ｍを算出する（ステップ２７）。フィルタリングされなかった特徴点の点数をｏとし、前フレームで抽出した特徴点の相対位置ｄ_（m-1)ｏと今回抽出した特徴点の相対位置ｄ_ｍｏの差分（Δｄ_ｍｏ=ｄ_ｍｏ-ｄ_{（ｍ−１)ｏ})を算出し、複数のΔｄ_ｍｏを用いて移動体１の移動量ΔＤ_ｍを算出する。算出する方法には、例えばＲｉｇｉｄＢｏｄｙＴｒａｎｓｆｏｒｍａｔｉｏｎ、ＳｌｉｄｉｎｇＷｉｎｄｏｗ、最小二乗法、中央値フィルタなどを利用することができる。

次に、ステップ２６でフィルタリングされた特徴点があるか否かをチェックする（ステップ２８）。フィルタリングされた特徴点がある場合、ステップ２９に進み、フィルタリングされた特徴点がない場合、処理を終了する。

フィルタリングされた特徴点がある場合、ステップ２６でフィルタリングされた各特徴点とステップ２７で算出した移動体１の移動量ΔＤ_ｍを用い、立体物の高さ立体物までの距離、立体物の速度を算出する（ステップ２９）。

図３は、ステップ２６の速度フィルタリングの詳細を示す図である。

図３（Ａ）は、ステップ２７で算出した移動体１の移動量ΔＤ_ｍの時間推移を示す。各フレームで抽出した特徴点の点数によって、移動量算出の処理時間が異なるため、フレーム間の撮像時刻の差分Δｔ_ｍが不規則に変動し、差分Δｔ_ｍの間に算出した移動量ΔＤ_ｍも不規則に変化する。そのため、今回算出した移動量と前フレームで算出した移動量に相関関係はない。

一方、図３（Ｂ）は、ステップ２５で算出した移動体１の速度Ｖ_ｍを示す。図３（Ｂ）に記載しているデータ数と差分Δｔ_ｍは図３（Ａ）と同じであるが、不規則に変化する移動量ΔＤ_ｍをΔｔ_ｍで割ることで、規則的に変化する速度Ｖ_ｍに変換できる。物理的に、速度は短時間（ミリ秒単位）で大きく変わらないので、Ｖ_ｍ−１が既知であればＶ_ｍはＶ_ｍ−１を中心とする閾値Ｖ_{ｔｈｒｅｓｈｏｌｄ}以内にある。従って、ステップ２６では、ステップ２５で算出した各特徴点の相対速度ｖ_ｍｎと、時系列に保存した過去の移動体の速度Ｖ_ｍ−１、Ｖ_ｍ−２…とを比較することで特徴点のフィルタリングができる。

例えば、｜Ｖ_ｍ−１−ｖ_ｍｎ｜> Ｖ_{ｔｈｒｅｓｈｏｌｄ}の条件を設定すれば前フレームで算出した移動体１の速度と相対速度ｖ_ｍｎとの差分が大きい特徴点がフィルタリングされる。また、前フレームで算出した移動体１の速度Ｖ_ｍ−１だけではなく、過去に算出した速度の時系列情報をもとに現在速度により近い速度Ｖ_ｅ＝ｆ（Ｖ_ｍ−１、Ｖ_ｍ−２、Ｖ_ｍ−３…）を算出し、｜Ｖ_ｅ−ｖ_ｍｎ｜＞Ｖ_{ｔｈｒｅｓｈｏｌｄ}という条件を設定することで、ステップ２６のフィルタリングをより正確に行うことができる。移動体１の現在速度を算出するための関数ｆは、過去に算出した移動体の速度の時系列情報（Ｖ_ｍ−１、Ｖ_ｍ−２、Ｖ_ｍ−３…）の平均や現在速度を補間するための多項式で構成できる。

尚、Ｖ_{ｔｈｒｅｓｈｏｌｄ}は一定値ではなく、関数Ｖ_{ｔｈｒｅｓｈｏｌｄ}＝Ｆ（Ｖ_ｍ−１、Ｖ_ｍ−２、Ｖ_ｍ−３…）で算出してもよい。即ち、過去に算出した移動体１の速度の時系列情報をもとにＶ_{ｔｈｒｅｓｈｏｌｄ}を変動させてもよい。例えば、過去の速度の時系列情報をもとに算出したＶ_ｅの標準偏差σを算出し、Ｖ_{ｔｈｒｅｓｈｏｌｄ}＝σ_ｍ＊ｋ（０≦ｋ≦３）と設定することで統計学的にフィルタリングできる。このパラメータｋは周囲の環境や移動体１の状態に合わせて、適宜変更できる。
また、移動量算出装置の起動後、過去の移動体１の過去の速度情報はないため、抽出した特徴点のほとんどを用いて、現在速度を算出する。即ち、速度Ｖ_ｅの初期値を０とし、Ｖ_{ｔｈｒｅｓｈｏｌｄ}を大きく設定する。

また、ステップ２６の速度フィルタリングは、撮像装置２だけではなく、移動体１に搭載された他の速度センサで構成してもよい。例えば、現在位置ＧＰＳ＿ｐｏｓ_ｍと時間ＧＰＳ＿ｔ_ｍを算出できるＧＰＳが搭載されていれば、移動体１の速度ＧＰＳ＿Ｖ_ｍ＝（ＧＰＳ＿ｐｏｓ_ｍ−ＧＰＳ＿ｐｏｓ_ｍ−１）／ＧＰＳ＿ｔ_ｍを算出し、速度Ｖ_ｅを、Ｖ_ｅ＝ｆ（ＧＰＳ＿Ｖ_ｍ−１、ＧＰＳ＿Ｖ_ｍ−２、ＧＰＳ＿Ｖ_ｍ−３…）で算出してもよい。車輪エンコーダやレーダやレーザなどの速度センサにより算出した移動体１の速度Ｓｅｎｓｏｒ＿Ｖ_ｍをもとにＶ_ｅ＝ｆ（Ｓｅｎｓｏｒ＿Ｖ_ｍ−１、Ｓｅｎｓｏｒ＿Ｖ_ｍ−２、Ｓｅｎｓｏｒ＿Ｖ_ｍ−３…）を算出してもよい。

図４は、道路での移動量算出を示す図である。

フレーム４１は撮像装置２が撮像した画像である。（ｕ,ｖ）は画像上のピクセル単位の座標であり、（ｘ,ｙ）は撮像装置２の道路のメートル単位の座標である。また、道路４２は移動体１が走行する路面である。

特徴点４３ａは、フレームＡにおいてステップ２０〜２２で抽出した特徴点である。フレームＢにおいて特徴点４３ａをトラッキングした（ステップ２３）後、フレームＡにおける特徴点４３ａ（フレームＢの白丸）はフレームＢにおける特徴点４３ｂに移動する。そして、フレームＣにおいて特徴点４３ｂをトラッキングした（ステップ２３）後、特徴点４３ｂ（フレームＣの白丸）はフレームＣにおける特徴点４３ｃに移動する。

移動量４４ｂは特徴点４３ａと特徴点４３ｂの位置をもとにステップ２５で算出した相対的な移動量Δｄ_Ｂn（但し、ｎは特徴点の番号）であり、移動量４４ｃは特徴点４３ｂと特徴点４３ｃの位置をもとにステップ２５で算出した移動量Δｄ_Ｃｎである。フレームＢにおいて、フレーム間の時間をΔｔ_Ｂとすると、各特徴点４３ｂの相対速度ｖ_Ｂｎ＝Δｄ_Ｂｎ /Δｔ_Ｂが算出される。特徴点の相対的な移動量Δｄ_Ｂｎは全て同じ路面上にある特徴点４３ａの相対移動量として算出されるため、複数の特徴点４３ａ間で大きな差はない（旋回成分があるので僅かには異なる）。従って、ステップ２６の速度フィルタリングの条件｜Ｖ_ｅ−ｖ_Bｎ｜＞Ｖ_{ｔｈｒｅｓｈｏｌｄ}で特徴点４３ｂをフィルタリングし、フィルタリングされなかったｖ_Ｂｎを用いて、ステップ２７で移動量ΔＤ_ｍ＝ΔＤ_Ｂと速度Ｖ_Ｂ＝ΔＤ_Ｂ /ｔ_Ｂを算出する。また、移動量Δｄ_Ｃｎの道路座標上の分布から方向変化Δθ_Ｂを算出することも可能である。

立体物４５はフレームＢの画像に映っている動的立体物である。特徴点４６ｂはステップ２０〜２２で抽出したフレームＢにおける立体物４５の特徴点である。特徴点４６ｃはフレームＣにステップ２３で特徴点４６ｂをトラッキングした点である。

移動量４７はステップ２５で算出した特徴点４６ｂから４６ｃへの移動量である。フレームＣには、ステップ２０〜２４で算出した特徴点４３ｂから４３ｃへの移動量４４ｃと、立体物４５の特徴点４６ｂから４６ｃへの移動量４７がある。ステップ２５でフレーム間の時間Δｔ_Ｃ時間を用い、それぞれの特徴点の相対速度ｖ_Ｃｎを算出する。速度フィルタリングの条件｜Ｖ_ｅ−ｖ_Bｎ｜＞Ｖ_{ｔｈｒｅｓｈｏｌｄ}の速度Ｖ_ｅに、フレームＢで算出した移動体１の速度Ｖ_Ｂを用いる場合は、Ｖ_ＢをＶ_ｅに代入し、ステップ２６を実行する。立体物４５の特徴点４６ｂから４６ｃへの移動量４７は路面の特徴点４３ｂから４３ｃへの移動量４４ｃと異なるため、特徴点４６ｃはステップ２６の条件｜Ｖ_ｅ−ｖ_Ｃｎ｜>Ｖ_{ｔｈｒｅｓｈｏｌｄ}を満たし、フィルタリングされる。従って、残った路面上の特徴点４３ｂから４３ｃへの移動量４４ｃをもとに、ステップ２７で精度よく移動体１の移動量ΔＤ_Ｃを算出できる。

尚、立体物の影響で全ての特徴点がフィルタリングされた場合、移動体１の移動量はステップ２６の速度フィルタリングに用いたＶ_ｅとフレーム間の撮像時刻の差分Δｔ_ｍを用いて、ΔＤ_ｍ＝Ｖ_ｅ／Δｔ_ｍで近似値を算出する。

図５は、ステップ２９の詳細を示す図である。

まず、路面５１から高さＨにある撮像装置２が位置Ｐ１からＰ２へ移動する時、立体物が路面上に無い場合について説明する。位置Ｐ１でステップ２０〜２２を実行し、路面５１の上にある特徴点５３ａを抽出する。路面５１が平らであれば、画像上のピクセル位置（ｕ,ｖ）と実際の位置関係（ｘ,ｙ）が一定になるため、画像上の特徴点５３ａのピクセル位置（Ｕ,Ｖ）_{ｆｅａｔｕｒｅ}から実際の特徴点５３ａの位置（Ｘ,Ｙ）_{ｆｅａｔｕｒｅ}へ変換でき、位置Ｐ１から特徴点５３ａまでの距離ｄ１を算出できる。次に、位置Ｐ２でステップ２０〜２７を実行し、位置Ｐ２から特徴点５３ａまでの距離ｄ２を算出し、距離ｄ１とｄ２をもとに移動量Δｄを算出する（Δｄ＝ｄ１−ｄ２）。

次に、図５（Ｂ）の路面５１から高さｈの位置に立体物５５がある場合について説明する。位置Ｐ１において立体物５５の特徴点５３ｂが画像上のピクセル位置（Ｕ,Ｖ）_{ｆｅａｔｕｒｅ}に映っているとき、特徴点５３ｂについては、ピクセル（Ｕ,Ｖ）_{ｆｅａｔｕｒｅ}から誤った座標５３ａ’（Ｘ,Ｙ）_{ｆｅａｔｕｒｅ}に変換されるため、位置Ｐ１から特徴点５３ｂまでの距離ｄ３も誤った値ｄ１で算出される。同様に、撮像装置２が位置Ｐ２へ移動した時も位置Ｐ２から特徴点５３ｂまでの距離は誤った値ｄ２’と算出され、ステップ２０〜２７では移動体１の移動量をΔｄ’＝ｄ1−ｄ２’＝ Δｄｂ＋Δｄと誤って算出される。図５（Ｂ）において、Δｄｂは立体物５５の特徴点５３ｂをトラッキングしたことによって生じる移動量Δｄの誤差である。距離ｄｉｓｔは位置Ｐ２から立体物５５までの距離であり、距離ｌ１は立体物５５から地点５３ａ’までの距離、距離ｌ２は距離ｄ２’と距離ｄｉｓｔの差分である。ここで、式（１）と式（２）から式（３）および式（４）が得られる。‘式（３）および式（４）によると、立体物５５がある場合誤って算出した移動量Δｄ’はカメラの設置高さＨと立体物５５の高さｈで表せる。
ｈ／Ｈ＝ｌ２／ｄ２’＝ｌ１／ｄ１（１）
Δｄｂ＝ｌ１-ｌ２＝ｈ＊（ｄ１−ｄ２’）／Ｈ（２）
Δｄ’＝Δｄｂ+Δｄ＝ｄ１−ｄ２’
＝ｈ＊（ｄ１−ｄ２’)／Ｈ＋ Δｄ
＝ｈ＊Δｄ’／Ｈ＋ Δｄ
よって、Δｄ’＝Δｄ＊Ｈ／（Ｈ−ｈ）（３）
ｈ＝Ｈ＊（Δｄ’− Δｄ）／Δｄ’（４）
以上より、カメラの設置高さＨと立体物５５がある場合に誤って算出した移動量Δｄ’= Δｄｂ＋Δｄと、立体物５５がない場合に正しく算出した移動量Δｄを式（３）に代入することで、立体物の高さｈを算出できる。カメラの設置高さＨは固定で既知である。立体物５５がある場合の誤った移動量Δｄ’= Δｄｂ＋Δｄはステップ２０〜２５で算出でき、立体物がない場合の正しい移動量Δｄはステップ２６でフィルタリングされなかった特徴点を用いてステップ２７で算出できる。立体物の影響で全ての特徴点がフィルタリングされた場合、移動体１の移動量はステップ２６で算出したＶ_ｅとフレーム間の撮像時刻の差分Δｔ_ｍを用いて、Δｄ＝Ｖ_ｅ／Δｔ_ｍで算出する。ステップ２９では、移動量Δｄと移動量Δｄ’を式（４）に代入し、立体物５５の高さｈを算出する。式（４）に特徴点の相対移動量Δｄ’=Δｄ_ｍｎを代入すれば、それぞれの特徴点の高さを算出できる。しかし、全ての特徴点の高さを算出するには時間がかかるため、ステップ２６でフィルタリングされた特徴点の高さのみ算出すればよい。

次に、式（５）により幾何学的に位置Ｐ２から立体物５５までの距離を算出する。位置Ｐ２から特徴点５３ｂまでの算出した距離ｄ２’と、立体物５５の算出した高さｈを式（５）に代入することで、各特徴点までの距離ｄｉｓｔを算出できる。
ｄｉｓｔ＝ｄ２’−ｌ２＝ｄ２’−ｄ２’＊ｈ／Ｈ＝ｄ２’＊（Ｈ−ｈ）／Ｈ（５）
移動体１にＧＰＳや速度センサを搭載することでＧＰＳや速度センサで取得した情報と撮像装置２で取得した情報を組み合わせて立体物５５の高さｈや距離ｄｉｓｔを算出できる。例えば、ＧＰＳからより正確な移動量Δｄを算出した場合は、移動量Δｄを式（３）に代入することで、より正確な高さｈを算出できる。更に、高さｈを式（５）に代入することで立体物５５までの距離ｄｉｓｔもより正確に算出できる。速度センサを搭載する場合、速度センサで取得した移動体１の速度Ｖを用いて移動量Δｄ＝Ｖ＊Δｔ_ｍを算出し、式（４）より高さｈを算出し、式（５）より距離ｄｉｓｔを算出する。

図５（Ｃ）は、立体物５５がフレーム間で移動量Δｄ_objectの移動を伴う動的立体物である場合を示している。ここで、Δｄは移動体の移動量、ｄ１は移動体が地点Ｐ１にいたときの立体物までの距離換算値、ｄ５は移動体が地点Ｐ２にいたときの立体物までの距離換算値、Ｈはカメラの設置高さ、ｈは立体物の高さである。図の関係より、式（６）が得られる。

ｄ_object＝Δｄ＋ｄ4-ｄ3＝Δｄ＋（ｄ１-ｄ５）＊（Ｈ−ｈ）／Ｈ（６）
前述のように、Δｄ、ｄ１、ｄ５は既知であり、式（６）より、立体物の特徴点が２つ以上取得取得できると、立体物の移動量と高さが同時に算出できる。

１移動体
２撮像装置
３処理装置
４画像処理部
５制御部
６メモリ
７表示部
８バス

Claims

路面の画像を撮像する撮像装置と、
前記撮像装置が撮像した画像に基づいて、移動体の移動量を算出する画像処理部と、を備え、
前記画像処理部は、
第１のタイミングで撮像された第１の画像から第１の特徴点を複数抽出し、
前記第１のタイミングより後の第２のタイミングで撮像された第２の画像から第２の特徴点を複数抽出し、
前記複数の第１の特徴点の各々を前記複数の第２の特徴点の各々にトラッキングし、
前記複数の第２の特徴点の各々の移動量と速度を算出し、
前記複数の第２の特徴点のうち、その速度が所定の範囲内の特徴点に基づいて、前記移動体の移動量を算出する、移動体。
前記複数の第２の特徴点のうち、その速度が前記所定の範囲より大きい特徴点に基づいて、前記撮像装置により撮像された路面上の立体物の高さ、及び/または、前記移動体から該立体物までの距離、及び/または、該立体物の速度を算出する、請求項１記載の移動体。
前記移動量と、前記撮像装置で画像を取得した時刻に基づいて、前記移動体の速度を算出する、請求項１又は２に記載の移動体。