(57)【要約】【課題】異なる場所から検出した特徴点の見え方が各検出場所で変化する場合であっても、移動体の位置を推定することが可能な、移動体位置推定装置及び移動体位置推定方法を提供する。【解決手段】撮像部２が撮像する周囲画像の撮像範囲内へ投光した線状光が屈曲している部分である折曲部において折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出する立ち上がり線算出部１６と、撮像部２が撮像する周囲画像の撮像範囲の変化に応じて、立ち上がり線算出部１６が算出した立ち上がり線と、撮像部２から特徴点までの距離に基づいて、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する特徴点照合部２０と、特徴点照合部２０が照合した撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部との位置の変位量に基づいて、車両Ｖの位置を推定する移動体位置推定部２２を備える。【選択図】図２
2012122734.tif 000002
【特許請求の範囲】
【請求項１】
移動体の周囲画像を連続的に撮像する撮像部と、
前記撮像部が撮像する周囲画像の撮像範囲内へ線状光を投光する投光手段と、
前記投光手段が線状光を投光した状態で、前記撮像部が撮像した周囲画像から、前記撮像範囲が変化しても追従可能な点である特徴点を検出する特徴点検出手段と、
前記撮像部から前記特徴点までの距離を算出する距離算出手段と、
前記特徴点検出手段が検出した特徴点及びその周囲から、前記投光手段が投光した線状光が屈曲している部分である折曲部を検出する折曲部検出手段と、
前記折曲部検出手段が検出した折曲部の屈曲角度である折曲角を算出する折曲角算出手段と、
前記折曲角算出手段が算出した折曲角から前記折曲部が存在する平面である折曲部平面を定義し、さらに、前記折曲部において前記折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出する立ち上がり線算出手段と、
前記撮像範囲の変化に応じて、前記立ち上がり線算出手段が算出した立ち上がり線と前記距離検出手段が算出した距離に基づいて、前記撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する特徴点照合手段と、
前記特徴点照合手段が照合した前記撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部との位置の変位量に基づいて、前記移動体の位置を推定する移動体位置推定手段と、を備えることを特徴とする移動体位置推定装置。
【請求項２】
前記立ち上がり線算出手段は、前記折曲部平面から垂直方向に立ち上がる法線ベクトルに基づいて、前記立ち上がり線を算出することを特徴とする請求項１に記載した移動体位置推定装置。
【請求項３】
前記特徴点照合手段は、前記撮像部が撮像する周囲画像に対するマッチング処理により、前記撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合することを特徴とする請求項１または２に記載した移動体位置推定装置。
【請求項４】
撮像部で連続的に撮像した移動体の周囲画像の撮像範囲内へ線状光を投光し、
前記線状光を投光した状態で、前記撮像した周囲画像から、前記撮像範囲が変化しても追従可能な点である特徴点を検出し、
前記撮像部から前記特徴点までの距離を算出し、
前記検出した特徴点及びその周囲から、前記投光した線状光が屈曲している部分である折曲部を検出し、
前記検出した折曲部の屈曲角度である折曲角を算出し、
前記算出した折曲角から前記折曲部が存在する平面である折曲部平面を定義し、さらに、前記折曲部において前記折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出し、
前記撮像範囲の変化に応じて、前記算出した立ち上がり線と前記算出した距離に基づいて、前記撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合し、
前記照合した前記撮像範囲が変化する前の折曲部と前記撮像範囲が変化した後の折曲部との位置の変位量に基づいて、前記移動体の位置を推定することを特徴とする移動体位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、車両等、移動体の位置を推定する移動体位置推定装置及び移動体位置推定方法に関する。
【背景技術】
【０００２】
車両等、移動体の位置（自己位置）を推定する装置としては、例えば、特許文献１に記載されている装置がある。
特許文献１に記載されている移動体位置推定装置は、移動体の前方視野の輝度画像を取得し、取得した輝度画像のうち連続する少なくとも二つのフレームに対して、輝度画像間における特徴点の位置の変位量に基づいて、移動体の位置を算出する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００２‐０４８５１３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１に記載の移動体位置推定装置では、輝度画像内の特徴点を検出し、この検出した特徴点と輝度画像を取得したカメラとの相対距離を計測する手順を有する。しかしながら、特徴点の見え方は、特徴点を検出する検出位置に依存するため、異なる場所から特徴点を検出すると、それぞれの場所において、特徴点の見え方が変化することとなる。このような、特徴点の見え方が変化は、特に、特徴点が、カメラ中心からの放射線方向と平行に近い角度で配置されている平面上の特徴点である場合に顕著となる。そのため、道路脇の建物壁面等に検出された特徴点等は、特徴点を検出する場所が少しずれてしまうだけでも検出が困難となる。
【０００５】
このため、異なる場所から検出した特徴点は、位置の算出に用いることが不適切なものとなり、位置の推定が困難となるという問題が発生するおそれがある。
本発明は、上記のような問題点に着目してなされたもので、異なる場所から検出した特徴点の見え方が、各検出場所で変化する場合であっても、移動体の位置を推定することが可能な、移動体位置推定装置及び移動体位置推定方法を提供することを課題とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するために、本発明は、撮像部で連続的に撮像した移動体の周囲画像の撮像範囲内へ線状光を投光した状態で、撮像した周囲画像から、撮像範囲が変化しても追従可能な点である特徴点を検出する。そして、撮像部から特徴点までの距離を算出し、さらに、特徴点及びその周囲から検出した線状光が屈曲している部分である折曲部の、屈曲角度である折曲角を算出し、この折曲角から、折曲部が存在する平面である折曲部平面を定義する。これに加え、折曲部において折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出し、撮像範囲の変化に応じて、撮像部と折曲部との距離と立ち上がり線とに基づいて、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する。さらに、照合した両折曲部の位置の変位量に基づいて、移動体の位置を推定する。
【発明の効果】
【０００７】
本発明によれば、折曲部が存在する折曲部平面と、折曲部平面から立ち上がる立ち上がり線に基づいて、移動体の位置を推定することが可能となる。このため、移動体の移動に伴う特徴点の検出位置の変化によって、特徴点の見え方が各検出位置で変化しても、移動体の位置を推定することが可能となる。
【図面の簡単な説明】
【０００８】
【図１】本発明の第一実施形態の移動体位置推定装置を備えた車両の概略構成を示す図である。
【図２】本発明の第一実施形態の移動体位置推定装置の概略構成を示す図である。
【図３】撮像部と、投光部と、投光平面との位置関係を示す図である。
【図４】撮像画像と実世界との関係を示す図である。
【図５】法線方向と、法線方向の許容される範囲を示す図である。
【図６】移動体位置推定装置が行う動作の一部を示すフローチャートである。
【図７】移動体位置推定装置が行う動作の一部を示すフローチャートである。
【発明を実施するための形態】
【０００９】
以下、本発明の実施形態について図面を参照しつつ説明する。
（第一実施形態）
以下、本発明の第一実施形態（以下、本実施形態と記載する）について、図面を参照しつつ説明する。
（構成）
図１は、本実施形態の移動体位置推定装置１を備えた車両Ｖの概略構成を示す図である。
図１中に示すように、移動体位置推定装置１は、移動体である車両Ｖが備える。すなわち、本実施形態では、移動体位置推定装置１を備える対象を、車両（自動車）とする。
図２は、移動体位置推定装置１の概略構成を示す図である。
図２中に示すように、移動体位置推定装置１は、撮像部２と、投光部４と、演算処理部６を備える。
【００１０】
撮像部２は、例えば、赤外線カメラ等のカメラを用いて形成し、予め設定した時間間隔ｔ［ｓ］で、車両Ｖ（移動体）の周囲画像を連続的に撮像する。
また、撮像部２は、撮像した周囲画像を含む情報信号を、演算処理部６へ出力する。具体的には、撮像した周囲画像を含む情報信号を、後述する特徴点検出部８と、距離算出部１０と、特徴点照合部２０及び移動体位置推定部２２へ出力する。
【００１１】
本実施形態では、撮像部２の構成を、車両Ｖの周囲画像のうち、車両Ｖの前方の画像を連続的に撮像する構成とする。したがって、本実施形態では、撮像部２を、例えば、車両Ｖの車室内におけるフロントウインド付近等、車両Ｖの車両前後方向前方の画像を撮像可能な位置に配置する場合について説明する。
投光部４は、例えば、レーザー照射機等、直線状の光を投光可能な装置を用いて形成し、撮像部２の撮像範囲内に、線状光（ライン状の光）を投光する。
【００１２】
また、投光部４は、線状光を投光している状態を含む情報信号を、演算処理部６へ出力する。具体的には、線状光を投光している状態を含む情報信号を、後述する折曲部検出部１２へ出力する。
本実施形態では、撮像部２の構成を、車両Ｖの前方の画像を連続的に撮像する構成とするため、これに伴い、投光部４の構成を、車両Ｖの前方へ線状光を投光する構成とする。したがって、本実施形態では、投光部４を、撮像部２の付近、例えば、車両Ｖの車室内におけるフロントウインド付近等、車両Ｖの車両前後方向前方へ線状光を投光可能な位置に配置する場合について説明する。
【００１３】
ここで、投光部４から線状光を投光すると、線状光が通過する領域は、平面（以下、「投光平面」と記載する場合がある）となる。
本実施形態では、図２及び３中に示すように、撮像部２と投光平面との位置関係を、予め設定した距離ｄ１で離間する位置関係に設定する。上述したように、本実施形態では、撮像部２及び投光部４の構成を、車両Ｖの前方の画像を連続的に撮像する構成とするため、これに伴い、撮像部２と投光部４との距離ｄ１を、車幅方向の距離とする。なお、図３は、撮像部２と、投光部４と、投光平面との位置関係を示す図である。
【００１４】
また、本実施形態では、撮像部２の取り付け角度θを、一例として、撮像部２から車両Ｖの前方に存在する物体までの距離を、２〜１０［ｍ］までの範囲内において、誤差３［％］で計測することが求められる場合に、−７０〜−１３０［°］の範囲内とする。
これは、投光平面上で、撮像部２から車両Ｖの前方に存在する物体までの距離に対する３［％］の距離変化が、１［ｐｉｘｅｌ］以上の画像の変化となって現れる条件が、投光平面からの距離が１．０［ｍ］以上となるためである。この条件は、撮像部２から２［ｍ］の位置及び１０［ｍ］の位置がそれぞれ撮像される場合に適用する。
【００１５】
演算処理部６は、特徴点検出部８と、距離算出部１０と、折曲部検出部１２と、折曲角算出部１４と、立ち上がり線算出部１６と、マップ保存部１８と、特徴点照合部２０と、移動体位置推定部２２を備える。
特徴点検出部８は、撮像部２が撮像した周囲画像（車両Ｖの前方の画像）から特徴点を検出する。ここで、特徴点は、例えば、車両Ｖの移動等に伴って撮像範囲が移動した際に、追従可能な点である。また、特徴点検出部８は、検出した特徴点を含む情報信号を、折曲部検出部１２へ出力する。
【００１６】
ここで、特徴点は、一般的には、撮像部２が撮像した周囲画像内において、他の点と比較して縦方向及び横方向に輝度差がある点である。また、特徴点の検出方法としては、公知のＳＵＲＦ（Ｓｐｅｅｄｅｄ  Ｕｐ  Ｒｏｂｕｓｔ  Ｆｅａｔｕｒｅｓ）や、ＳＩＦＴ（Ｓｃａｌｅ‐Ｉｎｖａｒｉａｎｔ  Ｆｅａｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ）等を用いることが可能である。
また、特徴点の発生要因は、車両Ｖの移動等に伴って特徴点の観測位置が変化したときの、特徴点の見え方の変化に大きな影響を与える。このため、本実施形態では、観測位置が変化したときの特徴点を観測し、この観測した特徴点を、マップ保存部１８に保存しておく。
また、特徴点には、平面上のテクスチャが観測されるテクスチャ特徴点と、異なる距離に存在する物体の境界に発生する構造特徴点がある。
【００１７】
検出した特徴点が、テクスチャ特徴点であるか構造特徴点であるかを判定する際には、特徴点及びその周囲の幾何学的構造を検出する。ここで、幾何学的構造とは、観測位置（撮像部２）から観測される特徴点の発生要因を決定する要素である。
特徴点の幾何学的構造を検出する方法は、まず、撮像部２が撮像した周囲画像内における、投光部４から投光された線状光の高さを計測することで、撮像部２から撮像対象までの距離を計測する。そして、撮像部２と撮像対象とを結ぶ直線の連続性を確認することで、特徴点及びその周囲の幾何学的構造を確認する。
【００１８】
この場合、撮像部２と撮像対象とを結ぶ直線の連続性が保たれていれば、その特徴点及びその周囲は連続的な平面である。したがって、撮像部２と撮像対象とを結ぶ直線の連続性が保たれている場合、その特徴点は、平面上のテクスチャ特徴点である。
一方、撮像部２と撮像対象とを結ぶ直線の連続性が保たれておらず、屈曲している場合や切断されている場合は、その特徴点及びその周囲は連続的な平面ではない。したがって、撮像部２と撮像対象とを結ぶ直線の連続性が保たれていない場合、その特徴点は、異なる距離に存在する物体の境界にできた構造特徴点である。
【００１９】
距離算出部１０は、撮像部２から特徴点までの距離を算出し、この算出した距離を示す情報信号を、特徴点照合部２０へ出力する。
本実施形態では、距離算出部１０が、投光部４から投光した線状光が、撮像部２で撮像した画像のどこに写っているのかを観測することによって、撮像部２から線状光が投光されている場所までの距離を計測する場合について説明する。しかしながら、撮像部２から特徴点までの距離を算出する方法は、これに限定するものではない。
以下、撮像部２から特徴点までの距離を算出する際の具体的な手順を説明する。
線状光の撮像画像内における位置は、図３中に示すように、線状光が通過する範囲（投光平面π＝（０  ０  １  ０）T）に対して、投光部４との距離ｄ１として撮像部２を設置した場合、距離ｄ１に応じて以下の式（１）により変化する。
【００２０】
【数１】
2012122734.tif 000003
【００２１】
ここで、上記の式（１）における「ｘ」は、ｘ＝（ｘ  ｙ  ｗ）Tで示される値であり、撮像画像上の点をＨｏｍｏｇｒａｐｈｙ座標で表したものである。
また、上記の式（１）における「Ｘ」は、Ｘ＝（Ｘ  Ｙ  Ｚ  １）Tで示される値であり、三次元空間上の点をＨｏｍｏｇｒａｐｈｙ座標で表したものである。
また、上記の式（１）における「ｋ」は、キャリブレーションマトリックスであり、以下の式（２）で表される。
【００２２】
【数２】
2012122734.tif 000004
【００２３】
ここで、上記の式（２）における「ｆ」は、撮像画像上の焦点距離であり、「ｕｘ」及び「ｕｙ」は、撮像画像中心のオフセットである。
また、上記の式（２）における「ＭL」は、投光平面を基準としたカメラマトリックスであり、以下の式（３）で表される。
【００２４】
【数３】
2012122734.tif 000005
【００２５】
ここで、上記の式（３）における「Ｒ」は、世界座標系での撮像部２の姿勢を表す回転行列である。
また、上記の式（３）における「Ｃ」は、撮像部２の位置（座標）を表す値である。
ここで、撮像部２の回転方向を、投光平面に対するＺ方向への変位とｘ軸周りの回転に限定すると、上記の「Ｒ」と「Ｃ」は、以下の式（４）及び（５）で表される。
【００２６】
【数４】
2012122734.tif 000006
【００２７】
ここで、投光された線状光が照射される条件は、常にＺ＝０であるので、投光平面上の点は、Ｘ＝（Ｘ  Ｙ  ０  １）と表すことができる。このとき、撮像画像上の位置ｘ＝（ｘ  ｙ  ｗ）は、次式（６）で表す値となる。
【００２８】
【数５】
2012122734.tif 000007
【００２９】
ここで、上記の式（６）における「ｘ」は、撮像画像上の線状光の位置であり、「ｆ」は、撮像画像上の焦点距離である。なお、上記の式（６）における「ｘ」及び「ｆ」は、共に、既知または観測可能な値である。
また、上記の式（６）における「ｕｘ」及び「ｕｙ」は、撮像画像のオフセット（撮像画像は、一般的に左上を原点とするため、オフセットを設定する必要がある）であり、「ｄ２」は、投光平面から撮像部２までの距離である。なお、上記の式（６）における「ｕｘ」、「ｕｙ」及び「ｄ２」は、共に既知または観測可能な値である。
【００３０】
したがって、撮像画像上の点のｙ（縦方向の位置）は、カメラからの距離Ｙによってのみ決定されるため、線状光の撮像画像上の位置を計測することによって、線状光が照射された物体と撮像部２との距離を確認することが可能となる。これにより、撮像部２から特徴点までの距離を算出することが可能となる。
なお、上述した説明では、撮像部２から特徴点までの距離を算出する方法として、投光による直接的な計測を記載したが、撮像部２から特徴点までの距離を算出する方法は、これに限定するものではない。すなわち、例えば、異なる位置から撮影された二枚以上の画像を用いて、これらの画像中にある特徴点の画面内における位置の変化から、実空間上の位置を特定する、「Ｍｏｔｉｏｎ  Ｓｔｅｒｅｏ」と呼ばれる手法を用いてもよい。
【００３１】
以上により、本実施形態では、上述したように、撮像部２と投光部４との距離ｄ１を設定しておくことによって、撮像部２から車両Ｖの前方に存在する物体までの距離を計測することが可能となる。
折曲部検出部１２は、撮像部２が撮像した周囲画像における、特徴点検出部８が検出した特徴点及びその周囲から、投光部４が投光した線状光が屈曲している部分である折曲部を検出する。また、折曲部検出部１２は、検出した折曲部を含む情報信号を、折曲角算出部１４及び距離算出部１０へ出力する。
【００３２】
以下、特徴点検出部８が検出した特徴点及びその周囲から折曲部を検出する際の、具体的な手順を説明する。
折曲部検出部１２は、特徴点検出部８において確認した、撮像部２と撮像対象とを結ぶ直線の連続性が保たれておらず、特徴点が構造特徴点である場合には、処理を終了する。
一方、折曲部検出部１２は、特徴点検出部８において確認した、撮像部２と撮像対象とを結ぶ直線の連続性が保たれており、特徴点が平面上のテクスチャ特徴点である場合は、以下の処理を行う。
【００３３】
具体的には、折曲部検出部１２は、特徴点が平面上のテクスチャ特徴点である場合、線状光の直線の傾きを求める。
ここで、線状光の直線の傾きとは、図４中に示すように、線分（線状光の直線）の両端に存在する折曲部であり、撮像画像内の座標ｘ1と座標ｘ2である。なお、図４は、撮像画像と実世界との関係を示す図である。また、図４中では、撮像画像を符号「Ｐ」で示し、特徴点及びその周囲を表す領域を符合「Ｅ」で示している。
折曲角算出部１４は、折曲部検出部１２が検出した折曲部の屈曲角度である折曲角を算出し、この算出した折曲角を含む情報信号を、立ち上がり線算出部１６へ出力する。ここで、折曲角の算出は、例えば、撮像部２が撮像した周囲画像から、折曲部及びその周辺部分を抽出して行う。
【００３４】
立ち上がり線算出部１６は、折曲角算出部１４が算出した折曲角から、折曲部が存在する平面である折曲部平面を定義する。これに加え、立ち上がり線算出部１６は、折曲部において折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出する。また、立ち上がり線算出部１６は、算出した立ち上がり線を含む情報信号を、特徴点照合部２０へ出力する。
本実施形態では、一例として、立ち上がり線算出部１６の構成を、折曲部平面から垂直方向に立ち上がる法線ベクトルに基づいて、立ち上がり線を算出する構成とした場合について説明する。
【００３５】
以下、折曲部平面の法線方向を算出する手順について説明する。
立ち上がり線算出部１６は、特徴点検出部８において確認した特徴点が、平面上のテクスチャ特徴点である場合、その特徴点が存在する平面（折曲部平面）の法線方向を求める。
ここで、例えば、図５中に示すように、特徴点を検出する範囲βを８０［°］とすると、折曲部平面の法線方向に関して許容される範囲αは、前後５［°］の誤差までとなる。なお、図５は、法線方向と、法線方向の許容される範囲を示す図である。また、図５中では、図４中と同様、撮像画像を符号「Ｐ」で示し、特徴点及びその周囲を表す領域を符合「Ｅ」で示している。
【００３６】
まず、撮像画像上における、実世界における線分の両端の点Ｘ1及びＸ2までの奥行きであるＹ1及びＹ2を、上記の式（６）を用いて計算する。なお、奥行きＹ1及びＹ2は、折曲部の点ｘ1＝（ｘ1  ｙ1  ｗ）と、ｘ1の縦方向の位置ｙ1と、ｘ2＝（ｘ2  ｙ2  １）と、ｘ2の縦方向の位置ｙ1とｙ2から、上記の式（６）を用いて計算する。
さらに、上記の式（６）を用いて計算したＹ1及びＹ2と、上記の式（６）の一行目の式を用いて、点Ｘ1及びＸ2を求める。
【００３７】
また、点Ｘ1及びＸ2は、投光平面上の点であり、投光平面πは、Ｚ＝０に広がる平面（図３参照）であるため、点Ｘ1に対応するＺ1（図示せず）は「０」となり、点Ｘ2に対応するＺ2（図示せず）は「０」となる。さらに、特徴点が存在する平面が、Ｚ軸に対して平行であるという仮定を用いれば、特に図示しないが、実世界における点Ｘ3＝（Ｘ１  Ｙ１  ａ  １）とすることが可能となる。ここで、「ａ」は、任意の値である。
以上により、特徴点が存在する平面の法線方向Ｄcは、上述した三つの点、すなわち、Ｘ１と、Ｘ２及びＸ３によって規定される平面の法線方向となるため、次式（７）により計算することが可能である。
【００３８】
【数６】
2012122734.tif 000008
【００３９】
マップ保存部１８は、特徴点検出部８が検出した特徴点と、距離算出部１０が算出した撮像部２と特徴点との距離と、折曲部検出部１２が検出した折曲部を用いて特徴点マップを形成し、その形成した特徴点マップを保存する。また、マップ保存部１８は、上記のように形成した特徴点マップをマップ保存部１８内に保存するとともに、特徴点マップを含む情報信号を特徴点照合部２０へ出力する。
【００４０】
以下、特徴点マップを形成する際の具体的な手順について説明する。
距離算出部１０が算出した撮像部２と特徴点との距離や、立ち上がり線算出部１６が算出した法線ベクトル（立ち上がり線）は、カメラ座標系を基準としている。しかしながら、車両Ｖの走行時には、カメラ（撮像部２）の位置は常に変化するため、カメラ座標系を基準とした値を用いると、カメラ（撮像部２）が移動したときに、特徴点等の位置が不定となってしまう。
【００４１】
したがって、特徴点マップをマップ保存部１８内に保存するためには、距離算出部１０が算出した撮像部２と特徴点との距離や、立ち上がり線算出部１６が算出した法線ベクトルを、カメラ座標系から世界座標系に変換して保存する必要がある。
上述した撮像部２と特徴点との距離や法線ベクトルを、カメラ座標系から世界座標系に変換するためには、世界座標系における撮像部２の位置を、特徴点マップの生成と同時に算出する必要がある。
【００４２】
また、撮像部２と特徴点との距離や法線ベクトルを、カメラ座標系から世界座標系に変換するためには、カメラ座標系への変換行列を算出する必要がある。
特徴点マップの生成と、撮像部２の位置の算出を同時に行うとともに、変換行列を算出する手法としては、例えば、公知のＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｐｉｎｇ）を用いることが可能である。ここで、撮像部２（カメラ）を使用したＳＬＡＭとしては、例えば、Ｍｏｎｏ‐ＳＬＡＭやＰＴＡＭ等がある。
【００４３】
なお、撮像部２の位置の算出を行う方法としては、上述したＳＬＡＭ以外に、例えば、公知の車輪速センサが検出した車輪速度パルスを用いて、車両Ｖの移動量を積算していく方法を用いてもよい。
そして、上記の変換行列を算出した後、世界座標系からカメラ座標系への変換行列の逆数を用いて、撮像部２と特徴点との距離ＹWを、次式（８）を用いて設定する。
【００４４】
【数７】
2012122734.tif 000009
【００４５】
さらに、特徴点及びその周囲の法線ベクトルＤwを、次式（９）を用いて設定する。
【００４６】
【数８】
2012122734.tif 000010
【００４７】
撮像部２と特徴点との距離ＹWと特徴点及びその周囲の法線ベクトルＤwを設定した後、これらの設定した値と特徴点のテクスチャ画像Ｉfとを合わせて特徴点マップを形成し、この形成した特徴点マップをマップ保存部１８内に保存する。
特徴点照合部２０は、撮像範囲の変化に応じて、立ち上がり線算出部１６が算出した立ち上がり線と、距離算出部１０が算出した距離に基づいて、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する。また、特徴点照合部２０は、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合した結果を含む情報信号を、移動体位置推定部２２へ出力する。
【００４８】
本実施形態では、一例として、特徴点照合部２０の構成を、撮像部２が撮像する周囲画像に対するマッチング処理により、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する構成とした場合について説明する。
以下、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する際に、特徴点照合部２０が行う処理を説明する。
【００４９】
特徴点照合部２０は、まず、撮像範囲が変化する前の折曲部を算出する処理として、走行前における車両Ｖの事前位置を推定する処理（事前位置推定）を行う。
ここで、事前位置推定時には、マップ保存部１８に保持した特徴点マップ内の特徴点と、撮像部２が撮像した画像内で検出された特徴点とを照合する。
具体的には、事前知識（以前の位置推定処理における位置推定結果や、停車中の車両Ｖが走行を開始する位置や、各種センサによる位置の計測結果等）を用いて、事前位置推定を行う。なお、上記の「各種センサ」とは、例えば、ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）等である。
【００５０】
そして、事前位置推定により、特徴点マップ内における位置と、世界座標系での撮像部２の姿勢を検出した後、撮像部２が撮像した画像内に写っていると予測される特徴点を、特徴点マップ内から選別する。ここで、撮像部２が撮像した画像内に写っていると予測される特徴点は、特徴点マップ内における特徴点の位置と、世界座標系での撮像部２の姿勢を用いて、特徴点マップ内に保存されている特徴点の中からマップ内から選別する。
撮像部２が撮像した画像内に写っていると予測される特徴点を、特徴点マップ内から選別する際には、特徴点マップ内における特徴点の位置に対して、撮像部２による周囲画像上での位置を、次式（１０）を用いて計算する。
【００５１】
【数９】
2012122734.tif 000011
【００５２】
ここで、上記の式（１０）における「Ｘp」は、特徴点マップに保存されている特徴点の世界座標系による位置である。
また、上記の式（１０）における「ＭW」は、世界座標系を基準とした撮像部２の位置であり、車両Ｖの位置に応じた事前推定値ＭWである。この事前推定値ＭWは、車両Ｖの詳細な事前位置推定を行うための初期解を与えるものである。これは、事前位置推定は、推定に伴う計算負荷が大きいため、初期解を与えることにより、その処理を簡略化するために行う。
【００５３】
ここで、上記の式（１０）における「ｘ」が、周囲画像の撮像範囲内であれば、特徴点マップ内における特徴点が、撮像部２が撮像した画像内に写る可能性が高いと判断し、その場所を保持する。
そして、撮像部２が撮像した周囲画像内に写っていると予測される特徴点を選別した後、この選別した特徴点の周囲画像内での位置を推定し、その推定した位置を中心として、周囲画像内で、特徴点の実際の位置を検索する。これにより、事前推定値から想定される周囲画像内での位置に基づく撮像範囲が変化する前の折曲部と、実際に検出された画像内位置に基づく撮像範囲が変化した後の折曲部とを照合する。
【００５４】
このとき、特徴点検出部８により特徴点を検出した位置と、車両Ｖの現在位置が大きく異なる場合では、特徴点の見え方が大きく異なる。このため、上記のような特徴点同士のマッチング処理では、特徴点の実際の位置を検索できない場合がある。
これに対し、本実施形態では、上述したように、折曲部が存在する平面である折曲部平面から垂直方向に立ち上がる法線ベクトルに基づいて、立ち上がり線を算出することにより、特徴点の見え方を推定する。そして、この推定した特徴点の見え方に基づいて、特徴点の実際の位置を検索する。
ここで、特徴点がテクスチャ特徴点であり、その法線方向が保存されている場合は、その法線方向Ｄを、撮像部２の座標（位置）Ｃ'に合わせ、次式（１１）を用いて変換しておく。
【００５５】
【数１０】
2012122734.tif 000012
【００５６】
周囲画像内で、特徴点の実際の位置を検索する際に用いる方法としては、以下に記載する方法を用いることが可能である。
すなわち、特徴点の実際の位置を検索する際に用いる方法は、まず、上記のように推定した位置（ｘ）を中心として、ある大きさの矩形範囲を探索領域として設定する。そして、設定した探索領域の中で、ＳＳＤ（Ｓｏｕｒｃｅ  Ｓｋｉｎ  Ｄｉｓｔａｎｃｅ）やＳＡＤ（Ｓｏｕｒｃｅ  Ａｘｉｓ  Ｄｉｓｔａｎｃｅ）等により算出した画像一致度を、１［ｐｉｘｅｌ］毎に変化させて計算する。さらに、最も画像一致度が高い場所を探索する。
【００５７】
このとき、特徴点がテクスチャ特徴点であり、テクスチャ平面の法線方向が保存されている場合は、撮像部２の位置からの見え方に合わせて、テクスチャを変換する。
テクスチャを変換する場合、テクスチャは平面に存在するので、二次元のＨｏｍｏｇｒａｐｈｙ変換を実施する。ここで、保存されているテクスチャ画像Ｉfの各Ｐｉｘｅｌ位置を、Ｐｉ＝（Ｉｘ  Ｉｙ  １）とすると、テクスチャを変換した後の特徴点の位置Ｐｔ’＝（Ｉ’ｘ  Ｉ’ｙ  １）は、次式（１２）として表される。
【００５８】
【数１１】
2012122734.tif 000013
【００５９】
上記の式（１２）において、「Ｈ」は３×３の変換行列であり、法線ベクトルと撮像部２の位置から、次式（１３）を用いて算出する。
【００６０】
【数１２】
2012122734.tif 000014
【００６１】
ここで、上記の式（１３）における「Ｒ」は、撮像範囲が変化する前の折曲部を撮像したときの撮像部２の位置Ｃから、撮像範囲が変化した後の折曲部を撮像したときの撮像部２の位置Ｃ'への回転行列である。
また、上記の式（１３）における「ｔ」は、撮像範囲が変化する前の折曲部を撮像したときの撮像部２の位置Ｃから、撮像範囲が変化した後の折曲部を撮像したときの撮像部２の位置Ｃ'への移動ベクトルである。
また、上記の式（１３）における「Ｄｃ'」は、撮像部２から見たときの、テクスチャ特徴点の法線ベクトルである。
【００６２】
また、上記の式（１３）における「ｄ３」は、撮像部２から特徴点までの距離である。
移動体位置推定部２２は、特徴点照合部２０が照合した、撮像範囲が変化する前の折曲部と、撮像範囲が変化した後の折曲部を用いて、両者の位置の変位量を算出する。そして、算出した変位量に基づいて、車両Ｖの位置を推定（以下、「事後推定」と記載する場合がある）する。
【００６３】
また、移動体位置推定部２２による車両Ｖの位置の推定結果は、例えば、特に図示しないが、表示装置（ディスプレイ等）に、地図上における座標等として表示する。
なお、本実施形態では、特徴点の位置が実際の計測位置に合うように事前推定の結果を調整することによって、事後推定を行う。これは、特徴点検出部８が検出した特徴点の位置が正しい場合、特徴点照合部２０が照合した特徴点同士の位置の差は、事前推定値の誤差として考えられるためである。したがって、特徴点照合部２０が照合した特徴点同士の位置の差を最小化することによって事前推定値を補正し、事後推定値を得る。
なお、特徴点照合部２０が照合した特徴点同士の位置の差を最小化するための方法としては、例えば、公知のモンテカルロ法等、複数の事後推定候補に対して、改めて特徴点同士の位置の差を計算する方法を用いてもよい。
【００６４】
（動作）
以下、図１から図５を参照しつつ、図６及び図７を用いて、移動体位置推定装置１が行う動作を説明する。
図６は、移動体位置推定装置１が行う動作の一部を示すフローチャートである。
図６中に示すように、移動体位置推定装置１が動作を開始（ＳＴＡＲＴ）すると、まず、ステップＳ１０において、撮像部２が、予め設定した時間間隔ｔ［ｓ］で、車両Ｖの周囲画像を連続的に撮像する。このとき、投光部４により、撮像部２の撮像範囲内に、線状光を投光する。ステップＳ１０において、車両Ｖの周囲画像を連続的に撮像すると、移動体位置推定装置１が行う処理は、ステップＳ１２へ移行する。
【００６５】
ステップＳ１２では、特徴点検出部８が、撮像部２が撮像した周囲画像から特徴点を検出する。ステップＳ１２において、特徴点を検出すると、移動体位置推定装置１が行う処理は、ステップＳ１４へ移行する。
ステップＳ１４では、距離算出部１０が、撮像部２から特徴点までの距離を算出する。ステップＳ１４において、撮像部２から特徴点までの距離を算出すると、移動体位置推定装置１が行う処理は、ステップＳ１６へ移行する。
【００６６】
ステップＳ１６では、折曲部検出部１２が、特徴点検出部８が検出した特徴点及びその周囲から折曲部を検出する。ステップＳ１６において、折曲部を検出すると、移動体位置推定装置１が行う処理は、ステップＳ１８へ移行する。
ステップＳ１８では、立ち上がり線算出部１６が、折曲角算出部１４が算出した折曲角から折曲部平面を定義し、さらに、折曲部において折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出する。ステップＳ１８において、立ち上がり線を算出すると、移動体位置推定装置１が行う処理は、ステップＳ２０へ移行する。
【００６７】
ステップＳ２０では、マップ保存部１８が、特徴点マップを形成し、その形成した特徴点マップを保存する。このとき、特徴点マップの保存に伴い、特徴点検出部８が検出した特徴点を消去する。ステップＳ２０において、特徴点マップを保存すると、移動体位置推定装置１が行う処理は、ステップＳ１０へ復帰する。
上述したステップＳ１０からＳ２０の処理によって、マップ保存部１８に特徴点マップを保存した後、移動体位置推定装置１は、図７中に示す動作を行う。なお、図７は、移動体位置推定装置１が行う動作の一部を示すフローチャートである。
【００６８】
図７中に示すように、移動体位置推定装置１が動作を開始（ＳＴＡＲＴ）すると、まず、ステップＳ３０において、特徴点照合部２０が、車両Ｖに対する事前位置推定を行う。ステップＳ３０において、事前位置推定を行うと、移動体位置推定装置１が行う処理は、ステップＳ３２へ移行する。
ステップＳ３２では、ステップＳ３０で行った事前位置推定が、正常に終了したか否かを判定する。そして、ステップＳ３０で行った事前位置推定が正常に終了していると判定した場合は、移動体位置推定装置１が行う処理は、ステップＳ３４へ移行する。
【００６９】
一方、ステップＳ３０で行った事前位置推定が正常に終了していないと判定した場合は、移動体位置推定装置１が行う処理は終了（ＥＮＤ）する。
ここで、ステップＳ３２の処理を行う理由は、ステップＳ３０で行う事前位置推定が、何らかの原因で失敗することがあるためであり、失敗した場合には、事前位置推定に用いるパラメータをリセットした状態で、再び事前位置推定を行う必要があるためである。
【００７０】
ステップＳ３４では、折曲部が存在する平面である折曲部平面から垂直方向に立ち上がる法線ベクトルに基づいて、立ち上がり線を算出することにより、特徴点の見え方を推定する。ステップＳ３４において、特徴点の見え方を推定すると、移動体位置推定装置１が行う処理は、ステップＳ３６へ移行する。
ステップＳ３６では、撮像部２が撮像した車両Ｖの周囲画像を取得する。ここで、ステップＳ３６において取得する周囲画像は、上述したステップＳ１０において取得した周囲画像よりも時間が経過して車両Ｖが移動している状態の画像である。ステップＳ３６において、車両Ｖの周囲画像を取得すると、移動体位置推定装置１が行う処理は、ステップＳ３８へ移行する。
【００７１】
ステップＳ３８では、特徴点照合部２０が、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する。ステップＳ３８において、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合すると、移動体位置推定装置１が行う処理は、ステップＳ４０へ移行する。
ステップＳ４０では、移動体位置推定部２２が、特徴点照合部２０が照合した、撮像範囲が変化する前の折曲部と、撮像範囲が変化した後の折曲部を用いて、両者の位置の変位量を算出する。ステップＳ４０において、変位量を算出すると、移動体位置推定装置１が行う処理は、ステップＳ４２へ移行する。
【００７２】
ステップＳ４２では、移動体位置推定部２２が、ステップＳ４０で算出した変位量に基づいて、車両Ｖの位置に対する事後推定を行う。ステップＳ４２において、事後推定を行うと、移動体位置推定装置１が行う処理は、ステップＳ４４へ移行する。
ステップＳ４４では、ステップＳ４２で行った事後推定が、正常に終了したか否かを判定する。そして、ステップＳ４２で行った事後推定が正常に終了していると判定した場合は、移動体位置推定装置１が行う処理は、ステップＳ４６へ移行する。
【００７３】
一方、ステップＳ４２で行った事後推定が正常に終了していないと判定した場合は、移動体位置推定装置１が行う処理は終了（ＥＮＤ）する。
ここで、ステップＳ４４の処理を行う理由は、ステップＳ４２で行う事後推定が、何らかの原因で失敗することがあるためであり、失敗した場合には、事後推定に用いるパラメータをリセットした状態で、再び事後推定を行う必要があるためである。
【００７４】
ステップＳ４６では、移動体位置推定部２２が行った移動体の位置の推定が、正常に終了したか否かを判定する。そして、移動体位置推定部２２が行った移動体の位置の推定が正常に終了していると判定した場合は、移動体位置推定装置１が行う処理は、ステップＳ３０へ移行する。
一方、移動体位置推定部２２が行った移動体の位置の推定が正常に終了していないと判定した場合は、移動体位置推定装置１が行う処理は終了（ＥＮＤ）する。
【００７５】
なお、上述したように、本実施形態の移動体位置推定装置１の動作で実施する移動体位置推定方法は、撮像範囲が変化する前の折曲部の位置と撮像範囲が変化した後の折曲部の位置との変位量に基づいて、車両Ｖの位置を推定する方法である。
ここで、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部との照合は、撮像範囲の変化に応じて、折曲部平面から予め設定した角度で立ち上がる立ち上がり線と撮像部２から特徴点までの距離に基づいて行う。
【００７６】
なお、立ち上がり線の算出は、まず、撮像部２で連続的に撮像した車両Ｖの周囲画像の撮像範囲内へ線状光を投光した状態で、撮像部２で撮像した周囲画像から、撮像範囲が変化しても追従可能な点である特徴点を検出する。そして、検出した特徴点及びその周囲から、撮像部２で連続的に撮像した車両Ｖの周囲画像の撮像範囲内へ投光した線状光が屈曲している部分である折曲部を検出する。これにより、検出した折曲部の屈曲角度である折曲角から折曲部が存在する平面である折曲部平面を定義して、立ち上がり線を算出する。
【００７７】
以上により、投光部４は、投光手段に対応する。また、特徴点検出部８は、特徴点検出手段に対応する。また、距離算出部１０は、距離算出手段に対応する。また、折曲部検出部１２は、折曲部検出手段に対応する。また、折曲角算出部１４は、折曲角算出手段に対応する。また、立ち上がり線算出部１６は、立ち上がり線算出手段に対応する。また、特徴点照合部２０は、特徴点照合手段に対応する。また、移動体位置推定部２２は、移動体位置推定手段に対応する。
【００７８】
（第一実施形態の効果）
（１）特徴点検出部８は、投光部４が線状光を投光した状態で、撮像部２が撮像した周囲画像から、撮像範囲が変化しても追従可能な点である特徴点を検出し、折曲部検出部１２は、特徴点検出部８が検出した特徴点及びその周囲から折曲部を検出する。また、折曲角算出部１４は、折曲部の屈曲角度である折曲角を算出し、立ち上がり線算出部１６は、折曲部が存在する平面である折曲部平面を定義し、さらに、折曲部において折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出する。
【００７９】
これに加え、特徴点照合部２０は、撮像範囲の変化に応じて、立ち上がり線算出部１６が算出した立ち上がり線と、距離算出部１０が算出した撮像部２から特徴点までの距離に基づいて、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する。さらに、移動体位置推定部２２が、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部との位置の変位量に基づいて、車両Ｖの位置を推定する。
このため、折曲部が存在する折曲部平面と、折曲部平面から立ち上がる立ち上がり線に基づいて、車両Ｖの位置を推定することが可能となる。
その結果、異なる場所から検出した特徴点の見え方が、各検出場所で変化する場合であっても車両Ｖの位置を推定することが可能となり、車両Ｖの位置を推定可能な条件を増加させることが可能となる。
【００８０】
（２）立ち上がり線算出部１６が、折曲部が存在する折曲部平面から垂直方向に立ち上がる法線ベクトルに基づいて、立ち上がり線を算出する。
このため、車両Ｖの位置の事後推定を行う際に、特徴点が存在する平面の法線ベクトルと、特徴点の特徴点マップ上における位置と、撮像部２の位置の事前推定値を用いて、事前推定した撮像部２の位置からの、特徴点の見え方を推定することが可能となる。
その結果、検出した角度により見え方が大きく変化する特徴点に対し、その特徴点の検出精度を向上させることが可能となる。
【００８１】
（３）特徴点照合部２０が、撮像部２が撮像する周囲画像に対するマッチング処理により、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する。
その結果、マッチング処理以外の処理を用いて、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する場合と比較して、シンプルな処理で照合が可能になるため、車両Ｖの位置を推定する処理を簡略化することが可能となる。
【００８２】
（４）本実施形態の移動体位置推定方法では、撮像部２が撮像した周囲画像の撮像範囲内へ線状光を投光した状態で、撮像範囲が変化しても追従可能な点である特徴点を検出し、この検出した特徴点及びその周囲から折曲部を検出する。また、折曲部の屈曲角度である折曲角を算出し、折曲部が存在する平面である折曲部平面を定義し、さらに、折曲部において折曲部平面から予め設定した角度で立ち上がる立ち上がり線を算出する。
【００８３】
これに加え、撮像範囲の変化に応じて、立ち上がり線と、撮像部２から特徴点までの距離に基づいて、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する。さらに、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部との位置の変位量に基づいて、車両Ｖの位置を推定する。
このため、折曲部が存在する折曲部平面と、折曲部平面から立ち上がる立ち上がり線に基づいて、車両Ｖの位置を推定することが可能となる。
その結果、異なる場所から検出した特徴点の見え方が、各検出場所で変化する場合であっても車両Ｖの位置を推定することが可能となり、車両Ｖの位置を推定可能な条件を増加させることが可能となる。
【００８４】
（変形例）
（１）本実施形態の移動体位置推定装置１では、立ち上がり線算出部１６が、折曲部が存在する折曲部平面から垂直方向に立ち上がる法線ベクトルに基づいて、立ち上がり線を算出するが、立ち上がり線算出部１６の構成は、これに限定するものではない。すなわち、立ち上がり線算出部１６の構成を、法線ベクトル以外に基づいて、立ち上がり線を算出する構成としてもよい。
【００８５】
（２）本実施形態の移動体位置推定装置１では、特徴点照合部２０が、撮像部２が撮像する周囲画像に対するマッチング処理により、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する。しかしながら、特徴点照合部２０の構成は、これに限定するものではなく、撮像部２が撮像する周囲画像に対するマッチング処理以外の処理により、撮像範囲が変化する前の折曲部と撮像範囲が変化した後の折曲部とを照合する構成としてもよい。
（３）本実施形態の移動体位置推定装置１では、移動体位置推定装置１を備える対象を車両としたが、これに限定するものではなく、例えば、作業機械や電車等を、移動体位置推定装置１を備える対象としてもよい。
【符号の説明】
【００８６】
１    移動体位置推定装置
２    撮像部
４    投光部（投光手段）
６    演算処理部
８    特徴点検出部（特徴点検出手段）
１０  距離算出部（距離算出手段）
１２  折曲部検出部（折曲部検出手段）
１４  折曲角算出部（折曲角算出手段）
１６  立ち上がり線算出部（立ち上がり線算出手段）
１８  マップ保存部
２０  特徴点照合部（特徴点照合手段）
２２  移動体位置推定部（移動体位置推定手段）
Ｖ    車両
Ｐ    撮像画像
Ｅ    特徴点及びその周囲を表す領域
【図１】
2012122734.tif 000015
【図２】
2012122734.tif 000016
【図３】
2012122734.tif 000017
【図４】
2012122734.tif 000018
【図５】
2012122734.tif 000019
【図６】
2012122734.tif 000020
【図７】
2012122734.tif 000021