JP7396618B2 - 移動体位置推定システム - Google Patents

Description

特許法第３０条第２項適用 展示日 ：令和１年５月２２日～５月２４日 展示会名：第６３回システム制御情報学会研究発表講演会（ＳＣＩ’１９） 開催場所：中央電気倶楽部（大阪市北区堂島浜２丁目１番２５号） 公開者 ：株式会社データ変換研究所

本発明は、空間内で移動する移動体の現在位置を推定するシステムに関する。

現実空間に存在する建築物や構造物、地形、空間内を移動する移動体（車両、歩行者、ロボット、航空機を含む飛翔体、船舶等）、または移動体が移動可能な通路（道路や橋梁等）の位置及び形状を精密にデータ化して蓄積しようとすると、データ量が膨大となる上、そのデータを利用して移動体からの距離や方向を把握するための処理が徒に複雑化し、計算量が増大して処理の遅延を招く懸念がある。

この問題を解決する手立てとして、本願発明者は既に、建築物、構造物、地形、移動体または通路に関する情報を軽量かつ利用容易なデータの形で表現する手法を特許出願している（下記特許文献１ないし４を参照）。具体的には、空間を一定の緯度及び一定の経度の大きさのグリッドに分割し、当該空間内に存在する物体と重なる一または複数のグリッドの位置の座標をデータベース化するというものである。さらに、各グリッドの位置の座標の情報に、当該グリッドと重なる物体の高さ寸法の情報を付加することで、空間内に存在する物体を一または複数の直方体状のキューブ（ｃｕｂｏｉｄ）の集合として表現することができる。

特開２０１７－０４９７３９号公報 特開２０１７－１０６９７８号公報 特開２０１７－１３８１６３号公報 特開２０１８－１１２８４５号公報

空間内で移動する車両やロボット等の移動体の自律制御、自動運転または遠隔操縦を行うためには、空間内における移動体の現在位置を把握する必要がある。

移動体の現在位置を知得する手段の典型として、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）やＱＺＳＳ（Ｑｕａｓｉ－Ｚｅｎｉｔｈ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）といった航法衛星システム（衛星測位システム）の航法信号受信装置が挙げられる。だが、ＧＰＳによる位置の測定では、数ｍないし数十ｍの誤差を生じることが間々ある。誤差を数ｃｍ以下に抑制できることが期待されているＱＺＳＳを用いるにしても、十数秒程度のタイムラグが見込まれ、リアルタイム処理が求められる用途には向かない。そして、当然これらは、航法信号を受信困難な場所では使用できなくなる。

移動体に搭載した加速度センサにより移動体の加速度を検出し、これを時間積分して移動体の移動速度及び移動量を求め、現在位置を推定することも可能である。また、いわゆるオドメトリ方式、即ち移動体の車輪の向き及び回転角度をセンサにより検出して移動速度及び移動量を求め、現在位置を推定する方式を採用することも考えられる。しかしながら、何れも、誤差が徐々に累積して増大してゆくきらいがある。加えて、オドメトリでは、車輪が滑らずに路面に接地することが前提となっており、車輪の空転や横滑りに起因する誤差の混入が不可避である。

本発明は、移動体の精確な移動量が分からなくとも、移動体の現在位置をできる限り精確に推定し、または誤差が混入する現在位置を補正できるようにすることを所期の目的としている。

本発明では、移動体が移動前位置から移動後位置に移動した際の当該移動後位置の座標を推定するものであって、移動体に搭載され、移動体の周囲に存在する複数の物体若しくは複数の物体の部分を検出し、移動体から各物体若しくは各部分までの距離、及び移動体から見て当該物体若しくは当該部分がどの方向に存在しているかを示す角度を計測する測距センサと、移動体が移動前位置にあるときに測距センサを介して検出した、または測距センサによらずに知得している、移動前位置及び移動後位置の付近に所在する既知の複数の物体若しくは複数の物体の部分の各々の位置の座標を格納する物体位置データ格納部と、移動体が移動後位置にあるときに測距センサを介して検出した複数の物体若しくは複数の物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定する位置推定部とを具備する移動体位置推定システムを構成した。

より具体的には、前記物体位置データ格納部が、空間を一定の大きさのグリッドに分割した場合における、当該空間内に所在している既知の物体と重なる一または複数のグリッドの各々に対応した座標を格納し、前記位置推定部が、移動後位置において前記測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかを照合するにあたり、移動後位置の座標を仮定した上、その座標の位置から測距センサにより計測して得た距離及び方向の角度の先が、既知の何れかの物体若しくは部分と重なるグリッド内にあるか否かを判断する。

移動後位置において前記測距センサにより計測して得た物体若しくは物体の部分の移動体からの距離及び方向の角度と、前記位置推定部により選定した同移動後位置の座標とを基に、同移動後位置において測距センサを介して検出した物体若しくは物体の部分の位置の座標を求めてこれを前記物体位置データ格納部に格納するとともに、同移動後位置を新たな移動前位置とし、そこから別の新たな移動後位置に向けて移動した際に、当該新たな移動後位置においても上記の処理を繰り返すものとすれば、移動体の移動の最中に、移動体の現在位置を精確に把握しながら、測距センサを介して検出した物体若しくは物体の部分の位置の座標のデータを物体位置データ格納部に順次蓄積してゆくことができ、既知の物体若しくは物体の部分の位置のデータベースを構築、拡充することができる。

前記位置推定部により、移動後位置において測距センサを介して検出した各物体若しくは各物体の部分が既知の何れの物体若しくは部分に該当するのかを照合するにあたり、物体位置データ格納部に多数の既知の物体若しくは部分の位置のデータが格納されていると、該当する物体若しくは部分の候補が膨大となり、照合に多大な計算量と長い時間とを要することになりかねない。そこで、前記位置推定部とは別の、移動体が移動後位置に移動した際の当該移動後位置の座標を推定する第二位置推定部を具備し、前記位置推定部が、前記第二位置推定部で推定した移動後位置の座標から所定の距離内にある既知の物体若しくは部分の位置の座標の情報を前記物体位置データ格納部から抽出し、その抽出した情報のみを用いるようにして、以て照合における処理の効率を高め高速化を図ることが好ましい。

前記測距センサが、光軸を鉛直軸回りに所定角度ずつ変位させながらレーザ光を照射し、物体若しくは物体の部分に当たって跳ね返る反射光を受光することを通じて、移動体から当該物体若しくは当該部分までの距離、及び移動体から見て当該物体若しくは当該部分がどの方向に存在しているかを示す角度を計測するものであり、空間を一定の大きさのグリッドに分割した場合において、測距センサがあるグリッドに対して光軸を所定角度ずつ変位させてその都度レーザ光を照射した結果、少なくとも一回以上または所定回数以上反射光を受光できたことを条件として、当該グリッドに物体若しくは物体の部分が存在していると判断する場合には、移動体からの距離が遠くなるほど、各グリッドの平面寸法を大きく設定することが好ましい。

処理をより高速化し、使用するメモリ領域をより小さくするためには、前記位置推定部が、移動体が移動後位置にあるときに前記測距センサを介して所定の上限数まで移動体の周囲に存在する物体若しくは物体の部分の当該移動体からの距離及び移動体から見た方向の角度を知得し、その上限数の物体若しくは物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して知得した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定することが好ましい。

本発明によれば、移動体の精確な移動量が分からなくとも、移動体の現在位置をできる限り精確に推定し、または誤差が混入する現在位置を補正することが可能となる。

本発明の一実施形態の移動体位置推定システムが有するハードウェア資源を示す図。 同実施形態の移動体位置推定システムの機能ブロック図。 同実施形態における移動体位置データ格納部が格納しているキューブデータにより規定される、空間内に存在する既知の物体若しくは物体の部分を模擬するキューブを示す斜視図。 同実施形態における物体位置データ格納部が格納しているキューブデータの内容を例示する図。 同実施形態における移動体に搭載された測距センサが走査し物体を検出できる範囲を模式的に示す平面図。 同実施形態における位置推定部が移動体の移動後位置の座標を推定する手順を説明する平面図。 同実施形態における位置推定部が移動体の移動後位置の座標を推定する手順を説明する平面図。 同実施形態における位置推定部が移動体の移動後位置の座標を推定する手順を説明する平面図。 同実施形態における位置推定部が移動体の移動後位置の座標を推定する手順を説明する平面図。 同実施形態における位置推定部が移動体の移動後位置の座標を推定する手順を説明する平面図。 同実施形態における位置推定部が移動体の移動後位置の座標を推定する手順を説明する平面図。 同実施形態における位置推定部が移動体の移動後位置の座標を推定する手順を説明する平面図。 同実施形態において移動体の周囲に設定する仮想的なグリッドを模式的に示す平面図。 同実施形態において移動体の周囲に設定する仮想的なグリッドを模式的に示す平面図。

本発明の一実施形態を、図面を参照して説明する。本実施形態の移動体位置推定システムは、コンピュータ１を主体として構成される。コンピュータ１は、汎用的なパーソナルコンピュータ１やワークステーション、携行可能なノートブック型コンピュータ１、タブレット型コンピュータ１、携帯電話端末その他の携帯情報端末であることがある。図１に示すように、コンピュータ１は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１ａ、メインメモリ１ｂ、補助記憶デバイス１ｃ、ビデオコーデック１ｄ、オーディオコーデック１ｅ、通信インタフェース１ｆ、入力デバイス１ｇ等のハードウェア資源を備え、これらがコントローラ（システムコントローラ、Ｉ／Ｏコントローラ等）１ｈにより制御されて連携動作するものである。

補助記憶デバイス１ｃは、フラッシュメモリ、ハードディスクドライブ、光学ディスクドライブ等である。ビデオコーデック１ｄは、ＣＰＵ１ａより受けた描画指示をもとに表示させるべき画面を生成しその画面信号をディスプレイ１ｉに向けて送出するＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、画面や画像のデータを一時的に格納しておくビデオメモリ等を要素とする。オーディオコーデック１ｅは、符号化されている音声データを復号化して音声出力デバイス１ｊから音声出力する。ビデオコーデック１ｄ及びオーディオコーデック１ｅはそれぞれ、ハードウェアでなくソフトウェアとして実装することも可能である。通信インタフェース１ｆは、当該コンピュータ１が外部の装置またはコンピュータと情報の授受を行うためのデバイスである。通信インタフェース１ｆは、インターネット、ＷＡＮ（Ｗｉｄｅ Ａｒｅａ Ｎｅｔｗｏｒｋ）、ＭＡＮ（Ｍｅｒｔｏｐｏｌｉｔａｎ Ａｒｅａ Ｎｅｔｗｏｒｋ）、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、携帯電話網、ＷｉＭＡＸ網、その他の電気通信回線を介した情報通信を実施するための有線接続インタフェースまたは無線トランシーバを含む。入力デバイス１ｇは、ユーザが手指で操作するタッチパネル（ディスプレイ１ｉに重ね合わされたものであることがある）、押下ボタン、マウス、キーボード等、またはユーザが肉声により指令を入力する音声入力デバイスである。

本移動体位置推定システムが、車両、ロボット、飛翔体（特に、遠隔操縦されまたは自律制御により飛行する無人航空機であるドローン。多くのドローンは、複数のプロペラを備えたマルチコプタ（マルチロータヘリコプタ、マルチロータ）である）やその他の移動体Ｍに搭載されて運用されることもあり得る。その場合、本移動体位置推定システムを構成するコンピュータ１が、移動体Ｍの運転制御を司る可能性がある。

移動体Ｍには、走査型の光波測距儀（光波距離計、レーザスキャナ）である測距センサ（測域センサと呼ばれることもある）１ｓが実装される。測距センサ１ｓは、移動体Ｍの周囲に存在する物体の外表面Ｓの移動体Ｍからの距離Ｄ、及び移動体Ｍから見た方向の角度を測定することのできるものである。換言すれば、測距センサ１ｓは、移動体Ｍの周囲に存在する物体または空間の形状を検出してその情報を出力することが可能である。図５に示すように、測距センサ１ｓは、例えば、移動体Ｍ及び当該測距センサ１ｓが向く正面方向Ｆを基準方向として、鉛直軸回りに左右それぞれ１３５°、計２７０°の範囲Ｒを光波（電磁波）特にレーザ光Ｌにより走査し、物体の外表面Ｓに当たって跳ね返った反射光を受信することを通じて、当該範囲Ｒ内に存在している物体の外表面Ｓの移動体Ｍからの距離Ｄ、及び移動体Ｍから見た当該物体の外表面Ｓの所在する方向と基準方向Ｆとがなす水平角を計測する。補足すると、測距センサ１ｓは、物体を検出可能な角度の範囲Ｒにおいて、レーザ光Ｌの光軸を鉛直軸回りに所定のステップ角、例えば０．２５°ずつ変位させながら、レーザ光Ｌを照射しその反射光を受光する走査を行う。即ち、測距センサ１ｓは、物体を検出可能な角度の範囲Ｒを１０８０点の粒度に分解し、それらの各方向に沿った移動体Ｍから物体の外表面Ｓまでの距離Ｄを計測する。測距センサ１ｓは、移動体Ｍの周囲２７０°の範囲Ｒを一秒間に二十回以上走査し、同頻度で物体までの距離Ｄ及び物体の所在する方向を計測する。

加えて、移動体Ｍには、ＧＰＳ、ＱＺＳＳ等の航法信号受信装置１ｎや、電子コンパス（地磁気センサ）、加速度センサ（ジャイロセンサ）等が実装されることがある。電子コンパスは、移動体Ｍ及び測距センサ１ｓ（の基準方向Ｆ）が現在どの方角を向いているのか、その絶対的な方位角を計測する。因みに、方位角０°（及び、３６０°）を真東とする右手系の場合、方位角９０°が真北、方位角１８０°が真西、方位角２７０°が真南である。方位角０°（及び、３６０°）を真北とする左手系の場合、方位角９０°が真東、方位角１８０°が真南、方位角２７０°が真西である。加速度センサは、移動体Ｍの移動の加速度や、移動体Ｍが水平軸（または、地面）に対して傾斜している角度である仰俯角を計測する。電子コンパスである三軸地磁気センサを加速度センサと組み合わせて用いれば、移動体Ｍ及び測距センサｍの基準方向Ｆが向く方位角をより精確に知得することが可能となる。

さらに、自動車やロボット等の走行する移動体Ｍには、走行用の車輪の向きを変化させることのできる既製の操舵装置１ｋ、車輪を回転駆動させる駆動源１ｌとなる既知のエンジン若しくはモータ、車輪の向き及び回転角度を検出するセンサ等が実装されることがある。

コンピュータ１は、移動体Ｍに搭載されている測距センサ１ｓ、航法信号受信装置１ｎその他各種センサが取得した情報を有線通信または無線通信により受信することができる。並びに、コンピュータ１は、移動体Ｍに搭載されている操舵装置１ｋや駆動源１ｌに対し、これらを制御する制御信号を有線通信または無線通信により送信することもできる。

コンピュータ１にあって、ＣＰＵ１ａにより実行されるべきプログラムは補助記憶デバイス１ｃに格納されており、プログラムの実行の際には補助記憶デバイス１ｃからメインメモリ１ｂに読み込まれ、ＣＰＵ１ａによって解読される。コンピュータ１は、プログラムに従い上記ハードウェア資源を作動させ、図２に示す物体位置データ格納部１０１、位置推定部１０２及び第二位置推定部１０３としての機能を発揮する。

物体位置データ格納部１０１は、メインメモリ１ｂ若しくは補助記憶デバイス１ｃの所要の記憶領域を利用し、移動体が移動前位置にあるときに測距センサ１ｓを介して検出した、または測距センサ１ｓによらずに知得している、移動前位置及び移動後位置の付近に所在する既知の複数の物体若しくは複数の物体の部分の各々の位置の緯度及び経度を格納する。

図３に例示するように、本実施形態では、空間内に存在する種々の物体を、各物体毎に、一つのキューブＣまたは複数のキューブＣの集合であると仮想する。キューブＣは、物体の位置、形状及び寸法の抽象化表現である。前提として、本実施形態では、移動体Ｍ１を運用する現実の空間を、一定の平面寸法（即ち、一定の幅寸法及び一定の奥行寸法）を有する仮想的なグリッドに分割する。例えば、一定の緯度及び一定の経度間隔の格子によって区画される、一定の緯度及び一定の経度の大きさの仮想的なグリッドに分割する。このとき、一つのグリッドは、緯度方向及び経度方向にそれぞれ所定の寸法を有した方形（四辺形。略長方形状をなすが、厳密には長方形ではない）となる。そして、一つのグリッドを表すために、一つのグリッドに対して少なくとも一つの位置座標を付与する。この位置座標は、対象のグリッド内の何れかの地点、例えば当該グリッドの中央や角（北東角、南東角、南西角または北西角）等の地点の緯度及び経度である。それら緯度及び経度はそれぞれ、所要の桁数の値とする。例えば、緯度及び経度の有効桁数を小数点以下６桁までとする場合、即ちあるグリッドを表すデータとして当該グリッド（の中央または何れかの角等）の位置座標［緯度，経度］を
［３５．６７５１２６°Ｎ，１３９．７５２４７２°Ｅ］
のように与える場合において、当該グリッドの北に隣接するグリッドに与えるべき（そのグリッドの中央または何れかの角等の）位置座標は、北緯に０．０００００１°を加算した
［３５．６７５１２７°Ｎ，１３９．７５２４７２°Ｅ］
となり、当該グリッドの西に隣接するグリッドに与えるべき（そのグリッドの中央または何れかの角等の）位置座標は、東経から０．０００００１°を減算した
［３５．６７５１２６°Ｎ，１３９．７５２４７１°Ｅ］
となる。

位置座標の緯度及び経度の有効桁数を小数点以下６桁までとする場合、一つのグリッドは緯度方向及び経度方向にそれぞれ０．０００００１°分の寸法を有した方形となる。その具体的な大きさは、緯度及び経度の影響を受けるため、あらゆるグリッドが均一な平面寸法を有するわけではない。即ち、北極または南極に近づくほど、単位経度あたりの経度（東西）方向の距離は顕著に小さくなり、また単位緯度あたりの緯度（南北）方向の寸法は若干ではあるが大きくなる。緯度及び経度の有効桁数を小数点以下６桁までとする場合、日本国内では、一つのグリッドの緯度方向の寸法は南北に約０．１１ｍ、経度方向の寸法は東西に約０．０９ｍとなる。尤も、日本列島は南北に伸びているので、所在地の緯度による寸法の伸縮を完全に無視することは必ずしも妥当ではない。

位置座標の緯度及び経度の有効桁数を小数点以下５桁までとする場合には、一つのグリッドの緯度方向の寸法は約１．１ｍ、経度方向の寸法は約０．９ｍとなる。有効桁数を小数点以下４桁までとすれば、一つのグリッドの緯度方向の寸法は約１１ｍ、経度方向の寸法は約９ｍとなる。要するに、緯度及び経度の有効桁数は、そのグリッドの平面寸法即ち緯度方向の寸法及び経度方向の寸法を規定する。緯度及び経度の有効桁数は、用途に応じて、また各物体の寸法に応じて、任意に設定することができる。大型のビル等の広い面積を有している物体にできるだけ少ない数のグリッドを当てはめようとするならば、そのグリッドに係る緯度及び経度の有効桁数を少なく設定することが有効である。

その上で、本実施形態では、図３に示しているように、グリッドから直立する直方体状のキューブＣを観念し、現実空間に存在する既知の種々の物体をそれぞれ一または複数のキューブＣによって模擬することとしている。そして、図４に例示するように、物体位置データ格納部１０１は、現実空間に存在する既知の物体若しくは物体の部分を模擬する個々のキューブＣ毎に、少なくとも、当該キューブＣが占めるグリッドの位置座標である緯度及び経度、並びにその有効桁数を、当該キューブＣを識別する識別子に関連づけてキューブデータとして格納する。既に述べた通り、キューブＣの位置の緯度及び経度の有効桁数は、キューブＣの平面寸法を間接的に指示する。即ち、キューブＣは、当該キューブＣが占有するグリッドの緯度及び経度の有効桁数に応じて定まるｘ軸方向寸法（緯度方向寸法）及びｙ軸方向寸法（経度方向寸法）を有する。

図４に示しているように、個々のキューブＣに対して、その高さの情報や、地上高の情報、当該キューブＣが所在しているグリッドの地表高の情報を付与することもある。キューブＣの高さは、現実空間において当該グリッドに存在している物体若しくは物体の部分、即ちキューブＣが模擬している物体若しくは物体の部分の上面の高さ位置（標高または海抜であることがある）を表す。キューブＣの地上高は、現実空間において当該グリッドに存在している物体または物体の部分の下面の高さ位置（標高または海抜であることがある）を表す。並びに、グリッドの地表高は、当該グリッド自体の高さ位置（標高または海抜であることがある）を表す。橋桁や梁、門等のように、移動体Ｍの存在する地面または床面から浮いている物を表現する際には、キューブＣの地上高に、グリッドの地表高よりも大きな正の数値を与える。地面または床面に接地している多くの物体については、地上高の値と地表高の値とが等しくなる。地面または床面よりも低い位置に存在する物体、例えば溝や穴、トンネル、河川や湖、海等を表現する際には、キューブＣの高さ及び地上高の値に、グリッドの地表高よりも小さな数値を与える。その数値は、負値となることもあり得る。物体位置データ格納部１０１は、各キューブＣの高さ、地上高、及び同キューブＣが占めるグリッドの地表高を、当該キューブＣを識別する識別子に関連づけてキューブデータとして格納する。

加えて、図４に示しているように、個々のキューブＣに対して、キャラクタシンボルの情報を付与することがある。物体位置データ格納部１０１に格納したキューブデータは、予め与えられていることもあれば、移動体Ｍに搭載された測距センサ１ｓにより移動体Ｍの周囲に存在する物体を検出することを通じて獲得されたものであることもある。キャラクタシンボルは、後者の場合において、移動体Ｍから見た当該キューブＣの大きさを表すものである。具体的には、移動体Ｍに搭載された測距センサ１ｓから出射するレーザ光が、当該キューブＣによって模される物体若しくは物体の部分に何箇所当たったか、その数の多寡を示している。

図５に示すように、移動体Ｍ及び測距センサ１ｓが、一つのキューブＣによって模される物体若しくは物体の部分に対し、レーザＬの光軸が当該物体若しくは物体の部分の外表面Ｓと直交するように正対している状況を想定する。緯度及び経度の有効桁数が小数点以下５桁のキューブＣが占めるグリッドの平面寸法は、一辺が約１ｍの方形である。このようなキューブＣに対応する物体若しくは物体の部分の外表面Ｓと、移動体の測距センサ１ｓとの距離Ｄが１０ｍであるとき、当該物体若しくは部分の外表面Ｓの幅即ち約１ｍの横幅は、測距センサ１ｓから見て約５．７４°の角度の幅となる。測距センサ１ｓが、レーザＬの光軸をステップ角０．２５°ずつ変位させて計測を行うものであるとすると、当該物体若しくは部分の外表面Ｓ上の約２２．９６個の箇所にレーザ光Ｌが当たり、約２２．９６個の箇所の移動体Ｍからの距離Ｄ及び移動体Ｍから見た方向の角度を計測できることになる。物体若しくは物体の部分の外表面Ｓと測距センサ１ｓとの距離Ｄが７０ｍであるならば、約１ｍの横幅は測距センサ１ｓから見て約０．８２°の角度の幅となり、当該物体若しくは部分の外表面Ｓ上の約３．２７個の箇所にレーザ光Ｌが当たり、約３．２７個の箇所の移動体Ｍからの距離Ｄ及び移動体Ｍから見た方向の角度を計測できる。

緯度及び経度の有効桁数が小数点以下６桁のキューブＣが占めるグリッドの平面寸法は、一辺が約０．１ｍの方形である。このようなキューブＣに対応する物体若しくは物体の部分のが表面Ｓと、移動体の測距センサ１ｓとの距離Ｄが１ｍであるとき、当該物体若しくは部分の外表面Ｓの幅即ち約０．１ｍの横幅は、測距センサ１ｓから見て約５．７４°の角度の幅となる。測距センサ１ｓが、レーザＬの光軸をステップ角０．２５°ずつ変位させて計測を行うものであるとすると、当該物体若しくは部分の外表面Ｓ上の約２２．９６個の箇所にレーザ光Ｌが当たり、約２２．９６個の箇所の移動体Ｍからの距離Ｄ及び移動体Ｍから見た方向の角度を計測できることになる。物体若しくは物体の部分の外表面Ｓと測距センサ１ｓとの距離Ｄが７ｍであるならば、約０．１ｍの横幅は測距センサ１ｓから見て約０．８２°の角度の幅となり、当該物体若しくは部分の外表面Ｓ上の約３．２７個の箇所にレーザ光Ｌが当たり、約３．２７個の箇所の移動体Ｍからの距離Ｄ及び移動体Ｍから見た方向の角度を計測できる。

あるキューブＣについてのキャラクタシンボルが３ないし９の数値であることは、当該キューブＣに対応する物体若しくは物体の部分を測距センサ１ｓにより検出したときに、当該物体若しくは物体の部分の外表面Ｓ上の当該数値と同数の箇所にレーザ光Ｌが当たり、それらの箇所を計測したことを意味する。あるキューブＣについてのキャラクタシンボルがＡであることは、当該キューブＣに対応する物体若しくは物体の部分を測距センサ１ｓにより検出したときに、当該物体若しくは物体の部分の外表面Ｓ上の十箇所ないし十九箇所にレーザ光Ｌが当たり、その十箇所ないし十九箇所を計測したことを意味する。同様に、キャラクタシンボルがＢであることは二十箇所ないし二十九箇所を計測したことを意味し、キャラクタシンボルがＣであることは三十箇所ないし三十九箇所を計測したことを意味し、……、キャラクタシンボルがＥであることは五十箇所ないし五十九箇所を計測したことを意味している。

物体位置データ格納部１０１は、各キューブＣのキャラクタシンボルを、当該キューブＣを識別する識別子に関連づけてキューブデータとして格納する。キャラクタシンボルは一定不変ではなく、移動体ＭとキューブＣが模擬する物体若しくは物体の部分との距離Ｄが拡縮することにより変動する。つまり、移動体Ｍが空間内を移動すると変動する。

一または複数のキューブＣにより模擬する対象となる物体は、例えば、移動体Ｍが移動する空間内に存在する建築物、構造物、中央分離帯、街灯、樹木等の障害物や、移動体Ｍが移動可能な道路や橋梁等、あるいは地形である。これら物体はそれぞれ、当該物体が空間内で占有している領域に略等しい一または複数のキューブＣの形で表現することができる。つまり、当該物体を象るキューブＣの位置座標及び高さ、地上高等の値の集合として、当該物体をデータ化することができる。全長１００ｍ、幅３ｍの通路は、キューブＣの位置座標の緯度及び経度の有効桁数を小数点以下６桁とすれば、（当該通路の向きにもよるが）約３万個のキューブＣの集合体として捉えることができ、同数の位置座標及び高さの値の集合としてデータ化できる。また、建物面積４００ｍ²の建物は、キューブＣの位置座標の緯度及び経度の有効桁数を小数点以下５桁とすれば、４００個程度のキューブＣの集合体として捉えることができ、同数の位置座標及び高さの値の集合としてデータ化できる。

位置推定部１０２は、測距センサ１ｓの機能を利用して、移動体Ｍが移動前位置から移動後位置に移動した際の当該移動後位置の緯度及び経度を推定する。以降、位置推定部１０２による移動後位置の推定方法に関して詳述する。

前提として、コンピュータ１は、移動体Ｍの移動の出発地点である移動前位置の精確な緯度及び経度を知得しており、これをメインメモリ１ｂ若しくは補助記憶デバイス１ｃに記憶保持している。並びに、コンピュータ１は、移動前位置にある移動体Ｍの周囲に存在している既知の複数の物体若しくは複数の物体の部分の各々の位置の精確な緯度及び経度を知得しており、それらをメインメモリ１ｂ若しくは補助記憶デバイス１ｃに記憶保持している。即ち、図４に示しているように、既知の物体若しくは物体の部分を模擬する複数のキューブＣ（が占めているグリッド）の各々の位置の緯度及び経度を、キューブデータとして物体位置データ格納部１０１に格納している。

移動体Ｍが移動前位置から移動後位置に移動したとき、コンピュータ１は、測距センサ１ｓにより、移動後位置にある移動体Ｍの周囲に存在している複数の物体若しくは物体の部分を検出する。そして、検出した複数の物体若しくは複数の部分のそれぞれの位置を示すベクトル、即ち移動体Ｍからの距離、及び移動体Ｍから見て当該物体若しくは当該部分がどの方向に存在しているかを示す水平角の値の組を得る。

しかして、コンピュータ１は、測距センサ１ｓを介して検出した、移動後位置の周囲にある複数の物体若しくは複数の物体の部分が、物体位置データ格納部１０１にデータを格納している既知の複数の物体若しくは複数の物体の部分の何れに該当するのかの照合を試みる。

一例として、図６に示すように、移動体Ｍが移動前位置にあるときに、複数の物体若しくは複数の物体の部分α、β、γ、δ、εを測距センサ１ｓにより検出し、検出した各物体若しくは各部分α、β、γ、δ、εが属するグリッドの緯度及び経度をキューブデータとして物体位置データ格納部１０１に格納して保持しているとする。尤も、キューブデータは、測距センサ１ｓによる物体の検出の結果としてではなく、予め与えられているものであってもよい。その後、図７に示すように、移動体Ｍが移動前位置から移動後位置に移動したときに、当該移動後位置の周囲に存在する複数の物体若しくは複数の物体の部分β、γ、δ、ε、ζを測距センサ１ｓにより検出し、移動後位置にある移動体Ｍから見た各物体若しくは各部分β、γ、δ、ε、ζの位置を示すベクトルＶ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５を得る。図７中、移動前位置を一点鎖線で表している。

移動体Ｍが移動後位置に到着した段階で、コンピュータ１は、既知の複数の物体若しくは複数の物体の部分α、β、γ、δ、εのそれぞれが属するグリッドの緯度及び経度を得ている。並びに、コンピュータ１は、移動後位置において検出した複数の物体若しくは複数の物体の部分β、γ、δ、ε、ζを指し示すベクトルＶ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５、即ち検出した各物体若しくは各部分β、γ、δ、ε、ζの移動体Ｍからの距離及び移動体Ｍから見た方向の水平角を得ている。

しかしながら、移動後位置（に到達した移動体Ｍ）の緯度及び経度は不明であり、各ベクトルＶ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５がそれぞれ既知のどの物体若しくは部分α、β、γ、δ、εに該当しているのかは不明である。移動前位置において検出した既知の何れかの物体若しくは物体の部分αを、移動後位置において検出できない可能性がある。並びに、移動前位置においてこれまで検出していない未知の物体若しくは物体の部分ζ、換言すれば物体位置データ格納部１０１にその緯度及び経度等のデータを格納していない物体若しくは物体の部分ζを、移動後位置においてはじめて検出する可能性もある。また、移動後位置において検出した物体β、γ、δ、ε、ζの移動体Ｍから見た方向の水平角は、移動体Ｍ及び測距センサ１ｓの正面方向を基準方向Ｆとした角度、当該基準方向Ｆからの変位量であり、絶対的な方角を示す方位角ではない。

位置推定部１０２として働くコンピュータ１は、まず、図８に示すように、移動後位置において検出した、移動後位置の周囲に存在する何れか一つの物体若しくは物体の部分の移動体Ｍからの距離及び移動体Ｍから見た方向のベクトルＶ１が、既知の物体若しくは物体の部分の一つαを指し示しているものと仮定し、物体位置データ格納部１０１に格納しているその物体若しくは部分αの緯度及び経度、並びに測距センサ１ｓにより検出した物体若しくは部分に至るベクトルＶ１の距離及び角度から、移動後位置にある現在の移動体Ｍの緯度及び経度を逆算する。緯度及び経度、二点間の距離である測地線長、方位角の計算には、例えばＶｉｎｃｅｎｔｙの式を用いる。図８中、その仮説的な移動後位置を一点鎖線で表している。そして、その逆算した移動後位置の緯度及び経度から、測距センサ１ｓにより検出した他の物体若しくは部分に至るベクトルＶ２、Ｖ３、Ｖ４、Ｖ５の各々が指し示している位置の緯度及び経度を算出し、それらの緯度及び経度が既知の他の物体若しくは物体の部分β、γ、δ、εの占めるグリッド内に属しているか否かを判断する。ベクトルＶ２、Ｖ３、Ｖ４、Ｖ５の一つが既知の物体若しくは物体の部分β、γ、δ、εの一つの占めるグリッド内に属しているならば、当該ベクトルＶ２、Ｖ３、Ｖ４、Ｖ５が既知の当該物体若しくは当該部分β、γ、δ、εを指し示しているものであると推認できる。

図８に示しているように、ベクトルＶ１が既知の物体若しくは部分αを指し示しているとの仮説の下では、ベクトルＶ３が既知の物体若しくは部分β、γ、δ、εの何れかを指し示していると判断できるが、他のベクトルＶ２、Ｖ４、Ｖ５は何れの既知の物体若しくは部分β、γ、δ、εも指し示していない。つまり、移動後位置において測距センサ１ｓを介して検出した各物体若しくは各部分Ｖ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５が、既知の何れの物体若しくは部分α、β、γ、δ、εに該当するのかを照合した結果、物体若しくは部分Ｖ１が物体若しくは部分αに合致し、物体若しくは部分Ｖ３が物体若しくは部分βに合致したが、それ以外のものは合致しなかった。この仮説の下で合致した物体若しくは部分の数は、二である。

次いで、図７に示すように、移動後位置において検出した、移動後位置の周囲に存在する何れか一つの物体若しくは物体の部分の移動体Ｍからの距離及び移動体Ｍから見た方向のベクトルＶ１が、既知の物体若しくは物体の部分の一つβを指し示しているものと仮定し、物体位置データ格納部１０１に格納しているその物体若しくは部分βの緯度及び経度、並びに測距センサ１ｓにより検出した物体若しくは部分に至るベクトルＶ１の距離及び角度から、移動後位置にある現在の移動体Ｍの緯度及び経度を逆算する。そして、その逆算した移動後位置の緯度及び経度から、測距センサ１ｓにより検出した他の物体若しくは部分に至るベクトルＶ２、Ｖ３、Ｖ４、Ｖ５の各々が指し示している位置の緯度及び経度を算出し、それらの緯度及び経度が既知の他の物体若しくは物体の部分α、γ、δ、εの占めるグリッド内に属しているか否かを判断する。ベクトルＶ２、Ｖ３、Ｖ４、Ｖ５の一つが既知の物体若しくは物体の部分α、γ、δ、εの一つの占めるグリッド内に属しているならば、当該ベクトルＶ２、Ｖ３、Ｖ４、Ｖ５が既知の当該物体若しくは当該部分α、γ、δ、εを指し示しているものであると推認できる。

図７に示しているように、ベクトルＶ１が既知の物体若しくは部分βを指し示しているとの仮説の下では、ベクトルＶ２、Ｖ３、Ｖ４がそれぞれ既知の物体若しくは部分γ、δ、εの何れかを指し示していると判断できるが、他のベクトルＶ５は何れの既知の物体若しくは部分β、γ、δ、εも指し示していない。つまり、移動後位置において測距センサ１ｓを介して検出した各物体若しくは各部分Ｖ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５が、既知の何れの物体若しくは部分α、β、γ、δ、εに該当するのかを照合した結果、物体若しくは部分Ｖ１が物体若しくは部分βに合致し、物体若しくは部分Ｖ２が物体若しくは部分γに合致し、物体若しくは部分Ｖ３が物体若しくは部分δに合致し、物体若しくは部分Ｖ４が物体若しくは部分εに合致したが、物体若しくは部分Ｖ５は合致しなかった。この仮説の下で合致した物体若しくは部分の数は、四である。ここで、ベクトルＶ５が合致する物体若しくは部分ζは、移動前位置において測距センサ１ｓを介して検出できておらず、物体位置データ１０１格納部にその緯度及び経度を格納している既知の物体若しくは部分ではない。

引き続き、図９に示すように、移動後位置において検出した、移動後位置の周囲に存在する何れか一つの物体若しくは物体の部分の移動体Ｍからの距離及び移動体Ｍから見た方向のベクトルＶ１が、既知の物体若しくは物体の部分の一つγを指し示しているものと仮定し、物体位置データ格納部１０１に格納しているその物体若しくは部分γの緯度及び経度、並びに測距センサ１ｓにより検出した物体若しくは部分に至るベクトルＶ１の距離及び角度から、移動後位置にある現在の移動体Ｍの緯度及び経度を逆算する。図９中、その仮説的な移動後位置を一点鎖線で表している。そして、その逆算した移動後位置の緯度及び経度から、測距センサ１ｓにより検出した他の物体若しくは部分に至るベクトルＶ２、Ｖ３、Ｖ４、Ｖ５の各々が指し示している位置の緯度及び経度を算出し、それらの緯度及び経度が既知の他の物体若しくは物体の部分α、β、δ、εの占めるグリッド内に属しているか否かを判断する。ベクトルＶ２、Ｖ３、Ｖ４、Ｖ５の一つが既知の物体若しくは物体の部分α、β、δ、εの一つの占めるグリッド内に属しているならば、当該ベクトルＶ２、Ｖ３、Ｖ４、Ｖ５が既知の当該物体若しくは当該部分α、β、δ、εを指し示しているものであると推認できる。

図９に示しているように、ベクトルＶ１が既知の物体若しくは部分γを指し示しているとの仮説の下では、ベクトルＶ２、Ｖ４が既知の物体若しくは部分α、β、δ、εの何れかを指し示していると判断できるが、他のベクトルＶ３、Ｖ５は何れの既知の物体若しくは部分α、β、δ、εも指し示していない。つまり、移動後位置において測距センサ１ｓを介して検出した各物体若しくは各部分Ｖ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５が、既知の何れの物体若しくは部分α、β、γ、δ、εに該当するのかを照合した結果、物体若しくは部分Ｖ１が物体若しくは部分γに合致し、物体若しくは部分Ｖ２が物体若しくは部分δに合致し、物体若しくは部分Ｖ４が物体若しくは部分εに合致したが、それ以外のものは合致しなかった。この仮説の下で合致した物体若しくは部分の数は、三である。

以後、同様にして、図１０に示すようにベクトルＶ１が既知の物体若しくは部分δを指し示しているとの仮説の下での照合を行い、合致した物体若しくは部分の数が二であることを確認する。さらに、図１１に示すようにベクトルＶ１が既知の物体若しくは部分εを指し示しているとの仮説の下での照合を行い、合致した物体若しくは部分の数が二であることを確認する。

最終的に、位置推定部１０２として働くコンピュータ１は、上掲の仮説のうち、合致した物体若しくは部分の数が最も大きいものを採用する。即ち、コンピュータ１は、ベクトルＶ１が既知の物体若しくは部分βを指し示していると推断し、既知の物体若しくは部分β、γ、δ、εの緯度及び経度と、これを指し示すベクトルＶ１、Ｖ２、Ｖ３、Ｖ４の距離及び角度とを基に、移動体Ｍの移動後位置の緯度及び経度を算出する。併せて、移動後位置において新たに検出した物体若しくは部分ζの緯度及び経度を算出し、これが属するグリッドの緯度及び経度をキューブデータとして物体位置データ格納部１０１に格納する。

なお、移動後位置において検出した物体若しくは部分を指し示すベクトルＶ１を、既知の何れかの物体若しくは部分α、β、γ、δ、εに当てはめる場合において、ベクトルＶ１を規定する方向の角度は移動体Ｍの正面方向を基準方向Ｆとした水平角であり、絶対的な方位角を示すものではない。従って、移動体Ｍの移動後位置の緯度及び経度を仮定するとしても、図１２に示すように、既知の物体若しくは部分α、β、γ、δ、εから、ベクトルＶ１を規定する距離を半径とした鉛直軸回りの円周Ｐ上の任意の位置に、移動体Ｍの移動後位置を設定することができてしまう。

移動体Ｍに、当該移動体Ｍの基準方向Ｆが現在どの方角を向いているのか、その絶対的な方位角を計測することのできる電子コンパスが搭載されているならば、移動後位置において周辺に存在する物体β、γ、δ、ε、ζを検出したときの移動体Ｍの基準方向Ｆを知得できる。基準方向Ｆの方位角が判明したならば、移動体Ｍの移動後位置の緯度及び経度が円周Ｐ上のどこにあるのかを一意に特定することができる。

移動体Ｍに電子コンパスが搭載されていないならば、仮説を設定して合致した物体若しくは部分の数を計数するにあたり、円周Ｐに沿って移動体Ｍの移動後位置を所定角度ずつ変位させ、同時に移動体Ｍの基準方向ＦをベクトルＶ１が仮説の対象の物体若しくは部分α、β、γ、δ、εを指向するように変化させながら、多数の移動後位置の緯度及び経度の組を設定し、それら各組のそれぞれについてベクトルＶ２、Ｖ３、Ｖ４、Ｖ５と既知の物体若しくは部分β、γ、δ、εの位置との当てはめを実行して、合致した物体若しくは部分の数を計数し、その数が最大となるような移動後位置の緯度及び経度を選定する必要がある。

また、物体位置データ格納部１０１に多数の既知の物体若しくは物体の部分の位置を示す緯度及び経度が格納されていると、移動後位置において検出した物体若しくは物体の部分を指し示すベクトルＶ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５を当てはめる対象となる既知の物体若しくは部分の候補が膨大となり、その分だけ移動後位置の緯度及び経度の確定に時間を要することになる。そこで、移動体Ｍに搭載されている測距センサ１ｓ以外の他の手段、例えば航法信号受信装置１ｎや、加速度センサ、車輪の向き及び回転角度を検出するセンサ等を利用して移動体Ｍの現在位置の緯度及び経度を推定する第二位置推定部１０３により、移動後位置に到達した移動体Ｍの緯度及び経度を仮に求める。そして、位置推定部１０２により移動後位置の精確な緯度及び経度を推定するにあたり、第二位置推定部１０３で推定した仮の移動後位置の緯度及び経度から所定の距離内にあるグリッドに位置する既知の物体若しくは物体の部分のみを、測距センサ１ｓにより検出した物体若しくは物体の部分のベクトルＶ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５の当てはめの対象とすることが好ましい。

位置推定部１０２による移動後位置の推定を高速化する手法について追記する。移動後位置の推定にあたっては、図１３に示すように、移動体Ｍの周囲の空間を、所定数の仮想的なグリッドに区分する。図示例では、移動体Ｍの正面方向Ｆに平行な前後方向に沿って二十一マス、移動体Ｍの正面方向Ｆと直交する左右方向に沿って二十一マスのグリッドを敷き詰めている。移動体Ｍの測距センサ１ｓは、それらグリッド群の中央のグリッドに位置している。つまり、移動体Ｍの前方に十マス、後方に十マス、左方に十マス、右方にも十マスのグリッドが並んでいることになる。

図５に示したように、移動体Ｍに搭載された測距センサ１ｓは、レーザ光Ｌの光軸を鉛直軸回りにステップ角０．２５°ずつ変位させながら、レーザ光Ｌを照射しその反射光を受光する走査を行う。図１３に示しているように、レーザ光Ｌの走査方向は常に一定である。レーザ光Ｌの光軸は、物体を検出可能な角度の範囲Ｒを、測距センサ１ｓを中心として平面視反時計回りに、移動体Ｍの右後方から右前方、正面、左前方そして左後方へと向かうように回動する。無論、レーザ光Ｌの走査方向、光軸の回動する方向は、これとは逆の平面視時計回りであっても構わない。

何れにせよ、レーザ光Ｌの光軸を０．２５°ずつ変位させ、その都度レーザ光Ｌを照射して物体の外表面Ｓに当たった反射光を受光しようとすることから、移動体Ｍから物体までの距離Ｄが大きくなるほど、測距センサ１ｓによる物体検出の粒度または解像度が粗くなる。

そこで、移動後位置に到達した移動体Ｍの周囲に存在する物体若しくは物体の部分の検出、及び検出した物体若しくは物体の部分に基づく移動後位置の推定に際して、図１４に示すように、移動体Ｍからの距離が遠くなるほど、物体若しくは物体の部分を模擬するキューブＣが占めるグリッドの平面寸法を大きく設定することが好ましい。

一つのグリッドの一辺の寸法は、例えば、１０ｃｍ（または、約１０ｃｍ）、２０ｃｍ（または、約２０ｃｍ）、５０ｃｍ（または、約５０ｃｍ）、１ｍ（または、約１ｍ）、２ｍ（または、約２ｍ）、５ｍ（または、約５ｍ）の六段階とする。一つのグリッドの一辺の寸法を１０ｃｍとすると、二十一×二十一マスのグリッド群は、移動体Ｍを中心として２．１ｍ四方の区域を捕捉する。一つのグリッドの一辺の寸法を５０ｃｍとすると、上記のグリッド群は、移動体Ｍを中心として１０．５ｍ四方の区域を捕捉する。一つのグリッドの一辺の寸法を５ｍとすると、上記のグリッド群は、移動体Ｍを中心として１０５ｍ四方の区域を捕捉する。

各区域内にあるグリッドは、マス位置［ｘ，ｙ］により指定できる。移動体Ｍ及び測距センサ１ｓが位置するグリッドのマス位置を［０，０］とおくと、整数値ｘ、ｙについて、－１０≦ｘ≦１０、－１０≦ｙ≦１０である。また、一つのグリッドの一辺の寸法を指定する識別子ｒ、換言すれば六段階の区域の何れに属するグリッドであるのかを識別する識別子ｒを付加したマス位置［ｘ，ｙ，ｒ］により、グリッドの一辺の寸法即ちグリッドの属する区域をも特定して一つのグリッドを指定できる。

位置推定部１０２として機能するコンピュータ１は、測距センサ１ｓを介して移動体Ｍの周囲に存在する物体若しくは物体の部分を検出するべく、レーザ光Ｌにより物体若しくは物体の部分を走査する。そして、物体の外表面Ｓに当たって反射した反射光を受光した事実に基づき、反射光を受光したときのレーザ光Ｌの光軸が指向する方向から、レーザ光Ｌが当たった箇所（当該箇所に、物体若しくは物体の部分が存在する可能性があると推認される）の所在する方向の水平角を求め、同時に移動体Ｍからレーザ光Ｌが当たった箇所までの距離Ｄを求める。しかして、それら距離Ｄ及び水平角の組を、メインメモリ１ｂ若しくは補助記憶デバイス１ｃに一時的に記憶保持する。

その上で、反射光を受光したときの距離Ｄ及び水平角から、レーザ光Ｌが当たった箇所が、どの区域に属するどのグリッド内にあるのかを特定する。例えば、距離Ｄが１ｍであるならば、レーザ光Ｌが当たった箇所は、一辺１０ｃｍのグリッドが敷かれた２．１ｍ四方の区域内に属している。そして、距離Ｄ及び方位角から、レーザ光Ｌが当たった箇所が、当該区域内の一辺１０ｃｍの何れのグリッドに位置するのかを選定する。また、距離Ｄが４．５ｍであるならば、レーザ光Ｌが当たった箇所は、一辺５０ｃｍのグリッドが敷かれた１０．５ｍ四方の区域内に属している。そして、距離Ｄ及び方位角から、レーザ光Ｌが当たった箇所が、当該区域内の一辺５０ｃｍの何れのグリッドに位置するのかを選定する。

レーザ光Ｌの光軸を変化させながらレーザ光を照射する結果、同一のグリッド内の複数の箇所にレーザ光が当たり、それら箇所から跳ね返る反射光を受光して複数の距離Ｄ及び方位角の組を取得することも当然にあり得る。位置推定部１０２は、各区域の各グリッド毎に、当該グリッド内の何箇所でレーザ光Ｌが当たったのかを計数し、その箇所の数を、当該グリッドのマス位置［ｘ，ｙ，ｒ］に関連づけて、メインメモリ１ｂ若しくは補助記憶デバイス１ｃに記憶する。

コンピュータ１は、各区域の各グリッドについて、少なくとも一回即ち一箇所レーザ光Ｌが当たったことを条件として、または、所定回数以上即ち所定数以上の箇所でレーザ光Ｌが当たったことを条件として、当該グリッドに物体若しくは物体の部分が存在していると判断する。

コンピュータ１は、物体若しくは物体の部分が存在していると判断したグリッドの数が所定の上限数、例えば三十に到達したら、それ以後に測距センサ１ｓを介して得られる距離Ｄ及び方位角の組を棄却する（移動体Ｍの移動後位置の推定の目的には使用しない）。つまり、移動体Ｍの右後方から平面視反時計回りにレーザ光Ｌによる走査を開始し、物体若しくは物体の部分が存在するグリッドを三十抽出したら、それら三十のグリッドの位置を基に、移動体Ｍの移動後位置の推定の実行に着手する。これにより、移動体Ｍから見て左方に所在する物体またはグリッドは、移動後位置の推定に用いる確率が相対的に低くなる。

さらに、抽出した三十のグリッドを、そのグリッド内でレーザ光Ｌが当たった箇所の数が多い順にソートし、上位から順に所定の上限数、例えば十のグリッドを“ランドマーク”として選抜し、その選抜した十のグリッドのみを、移動後位置の推定に用いるようにしてもよい。

移動体Ｍの周囲において物体若しくは物体の部分が存在しているグリッドの判断、三十のグリッドの抽出、及び十のグリッドの選抜は、移動前位置及び移動後位置のそれぞれで行う。移動前位置で抽出した三十のグリッドの位置座標（距離Ｄ及び方位角、または緯度及び経度）や、移動前位置で選抜したグリッドの位置座標（距離Ｄ及び方位角、または緯度及び経度）は、物体位置データ格納部１０１に格納して保持するデータの一部をなす。

その後の処理は、図６ないし図１３に示したものと同様となる。コンピュータ１は、移動後位置の周囲にある、物体若しくは複数の物体の部分と重なり合う三十または十のグリッドが、物体位置データ格納部１０１にデータを格納している、移動前位置にて検出した既知の物体若しくは物体の部分と重なり合う三十または十のグリッドの何れに該当するのかの照合を試みる。移動後位置の周囲の三十または十のグリッドの各々を指し示すベクトルＶ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５は、各グリッドの中央や角等の位置座標（距離Ｄ及び方位角、または緯度及び経度）を指すものとしてもよいし、各グリッドについて最初にまたは最後にレーザ光Ｌが当たった箇所の位置座標（距離Ｄ及び方位角、または緯度及び経度）を指すものとしてもよい。あるいは、同一のグリッド内の複数箇所にレーザ光Ｌが当たった場合における中央値、例えば十箇所にレーザ光Ｌが当たった場合に五番目または六番目にレーザ光Ｌが当たった箇所の位置座標（距離Ｄ及び方位角、または緯度及び経度）を指すものとしてもよい。

照合するグリッドの数を三十または十に絞り込むことにより、移動後位置の推定の処理を簡略化、高速化することができ、消費するメインメモリ１ｂの量も削減することができる。

本実施形態では、移動体Ｍが移動前位置から移動後位置に移動した際の当該移動後位置の座標を推定するものであって、移動体Ｍに搭載され、移動体Ｍの周囲に存在する複数の物体若しくは複数の物体の部分を検出し、移動体Ｍから各物体若しくは各部分までの距離、及び移動体Ｍから見て当該物体若しくは当該部分がどの方向に存在しているかを示す角度を計測する測距センサ１ｓと、移動体Ｍが移動前位置にあるときに測距センサ１ｓを介して検出した、または測距センサ１ｓによらずに知得している、移動前位置及び移動後位置の付近に所在する既知の複数の物体若しくは複数の物体の部分の各々の位置の座標を格納する物体位置データ格納部１０１と、移動体Ｍが移動後位置にあるときに測距センサ１ｓを介して検出した複数の物体若しくは複数の物体の部分の移動体Ｍからの距離及び移動体から見た方向の角度と、物体位置データ格納部１０１に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサ１ｓを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定する位置推定部１０２とを具備する移動体位置推定システムを構成した。

本実施形態によれば、移動体Ｍの精確な移動量が分からなくとも、移動体Ｍの現在位置をできる限り精確に推定し、または誤差が混入する現在位置を補正することが可能となる。

本実施形態では、前記物体位置データ格納部１０１が、空間を一定の大きさのグリッドに分割した場合における、当該空間内に所在している既知の物体と重なる一または複数のグリッドの各々に対応した座標を格納し、前記位置推定部１０２が、移動後位置において前記測距センサ１ｓを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかを照合するにあたり、移動後位置の座標を仮定した上、その座標の位置から測距センサ１ｓにより計測して得た距離及び方向の角度の先が、既知の何れかの物体若しくは部分と重なるグリッド内にあるか否かを判断する。このような手法により、物体位置データ格納部１０１に格納するデータの量を削減し、並びに移動後位置にある移動体Ｍの座標の推定における計算量を軽減することが可能となる。

移動後位置において前記測距センサ１ｓにより計測して得た物体若しくは物体の部分の移動体Ｍからの距離及び方向の角度と、前記位置推定部１０２により選定した同移動後位置の座標とを基に、同移動後位置において測距センサ１ｓを介して検出した物体若しくは物体の部分の位置の座標を求めてこれを前記物体位置データ格納部１０１に格納するとともに、同移動後位置を新たな移動前位置とし、そこから別の新たな移動後位置に向けて移動した際に、当該新たな移動後位置においても上記の処理を繰り返すものとすれば、移動体Ｍの移動の最中に、移動体Ｍの現在位置を精確に把握しながら、測距センサ１ｓを介して検出した物体若しくは物体の部分の位置の座標のデータを物体位置データ格納部１０１に順次蓄積してゆくことができ、既知の物体若しくは物体の部分の位置のデータベースを構築、拡充することができる。

加えて、前記位置推定部１０２とは別の、移動体Ｍが移動後位置に移動した際の当該移動後位置の座標を推定する第二位置推定部１０３を具備し、前記位置推定部１０２が、前記第二位置推定部１０３で推定した移動後位置の座標から所定の距離内にある既知の物体若しくは部分の位置の座標の情報を前記物体位置データ格納部１０１から抽出し、その抽出した情報のみを用いるようにすれば、照合における処理の効率を高め高速化を図ることができる。

前記測距センサ１ｓが、光軸を鉛直軸回りに所定角度ずつ変位させながらレーザ光Ｌを照射し、物体若しくは物体の部分に当たって跳ね返る反射光を受光することを通じて、移動体Ｍから当該物体若しくは当該部分までの距離、及び移動体から見て当該物体若しくは当該部分がどの方向に存在しているかを示す角度を計測するものであり、空間を一定の大きさのグリッドに分割した場合において、測距センサ１ｓがあるグリッドに対して光軸を所定角度ずつ変位させてその都度レーザ光を照射した結果、少なくとも一回以上または所定回数以上反射光を受光できたことを条件として、当該グリッドに物体若しくは物体の部分が存在していると判断する場合には、移動体Ｍからの距離が遠くなるほど、各グリッドの平面寸法を大きく設定する。

加えて、前記位置推定部１０２が、移動体Ｍが移動後位置にあるときに前記測距センサ１ｓを介して所定の上限数（例えば、三十または十）まで移動体Ｍの周囲に存在する物体若しくは物体の部分の当該移動体からの距離及び移動体から見た方向の角度を知得し、その上限数の物体若しくは物体の部分の移動体Ｍからの距離及び移動体から見た方向の角度と、物体位置データ格納部１０１に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサ１ｓを介して知得した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定することとしているため、処理をより高速化し、使用するメモリ領域をより小さくすることが可能となっている。

なお、本発明は以上に詳述した実施形態に限られるものではない。各部の具体的な構成や処理の手順等は、本発明の趣旨を逸脱しない範囲で種々変形が可能である。

１…コンピュータ
１ｓ…測距センサ
１０１…物体位置データ格納部
１０２…位置推定部
１０３…第二位置推定部
Ｃ…グリッド上に存在する、既知の物体若しくはその部分を模したキューブ
Ｖ１、Ｖ２、Ｖ３、Ｖ４、Ｖ５…移動後位置にあるときに測距センサを介して検出した複数の物体若しくは複数の物体の部分の移動体からの距離及び移動体から見た方向の角度
α、β、γ、δ、ε…既知の複数の物体若しくは複数の物体の部分の位置の座標

Claims (7)

1. 移動体が移動前位置から移動後位置に移動した際の当該移動後位置の座標を推定するものであって、
   移動体に搭載され、移動体の周囲に存在する複数の物体若しくは複数の物体の部分を検出し、移動体から各物体若しくは各部分までの距離、及び移動体から見て当該物体若しくは当該部分がどの方向に存在しているかを示す角度を計測する測距センサと、
   移動体が移動前位置にあるときに測距センサを介して検出した、または測距センサによらずに知得している、移動前位置及び移動後位置の付近に所在する既知の複数の物体若しくは複数の物体の部分の各々の位置の座標を格納する物体位置データ格納部と、
   移動体が移動後位置にあるときに測距センサを介して検出した複数の物体若しくは複数の物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定する位置推定部と
   を具備し、
   移動後位置において前記測距センサにより計測して得た物体若しくは物体の部分の移動体からの距離及び方向の角度と、前記位置推定部により選定した同移動後位置の座標とを基に、同移動後位置において測距センサを介して検出した物体若しくは物体の部分の位置の座標を求めてこれを前記物体位置データ格納部に格納するとともに、同移動後位置を新たな移動前位置とし、そこから別の新たな移動後位置に向けて移動した際に、当該新たな移動後位置においても上記の処理を繰り返す移動体位置推定システム。
2. 前記位置推定部とは別の、移動体が移動後位置に移動した際の当該移動後位置の座標を推定する第二位置推定部を具備し、
   前記位置推定部は、移動後位置において前記測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかを照合するにあたり、前記第二位置推定部で推定した移動後位置の座標から所定の距離内にある既知の物体若しくは部分の位置の座標の情報を前記物体位置データ格納部から抽出する請求項１記載の移動体位置推定システム。
3. 移動体が移動前位置から移動後位置に移動した際の当該移動後位置の座標を推定するものであって、
   移動体に搭載され、移動体の周囲に存在する複数の物体若しくは複数の物体の部分を検出し、移動体から各物体若しくは各部分までの距離、及び移動体から見て当該物体若しくは当該部分がどの方向に存在しているかを示す角度を計測する測距センサと、
   移動体が移動前位置にあるときに測距センサを介して検出した、または測距センサによらずに知得している、移動前位置及び移動後位置の付近に所在する既知の複数の物体若しくは複数の物体の部分の各々の位置の座標を格納する物体位置データ格納部と、
   移動体が移動後位置にあるときに測距センサを介して検出した複数の物体若しくは複数の物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定する位置推定部と
   を具備し、
   前記位置推定部は、移動体が移動後位置にあるときに前記測距センサを介して所定の上限数まで移動体の周囲に存在する物体若しくは物体の部分の当該移動体からの距離及び移動体から見た方向の角度を知得し、その上限数の物体若しくは物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して知得した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定する移動体位置推定システム。
4. 前記物体位置データ格納部は、空間を一定の大きさのグリッドに分割した場合における、当該空間内に所在している既知の物体と重なる一または複数のグリッドの各々に対応した座標を格納し、
   前記位置推定部は、移動後位置において前記測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかを照合するにあたり、移動後位置の座標を仮定した上、その座標の位置から測距センサにより計測して得た距離及び方向の角度の先が、既知の何れかの物体若しくは部分と重なるグリッド内にあるか否かを判断する請求項１、２または３記載の移動体位置推定システム。
5. 請求項１記載の移動体位置推定システムを構成するために用いられるものであって、コンピュータを、
   移動体が移動前位置にあるときに移動体に搭載された測距センサを介して検出した、または測距センサによらずに知得している、移動前位置及び移動後位置の付近に所在する既知の複数の物体若しくは複数の物体の部分の各々の位置の座標を格納する物体位置データ格納部、並びに、
   移動体が移動後位置にあるときに移動体に搭載された測距センサを介して検出した複数の物体若しくは複数の物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定する位置推定部
   として機能させ、
   移動後位置において前記測距センサにより計測して得た物体若しくは物体の部分の移動体からの距離及び方向の角度と、前記位置推定部により選定した同移動後位置の座標とを基に、同移動後位置において測距センサを介して検出した物体若しくは物体の部分の位置の座標を求めてこれを前記物体位置データ格納部に格納するとともに、同移動後位置を新たな移動前位置とし、そこから別の新たな移動後位置に向けて移動した際に、当該新たな移動後位置においても上記の処理を繰り返すプログラム。
6. コンピュータを、前記位置推定部とは別の、移動体が移動後位置に移動した際の当該移動後位置の座標を推定する第二位置推定部としても機能させ、
   前記位置推定部は、移動後位置において前記測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかを照合するにあたり、前記第二位置推定部で推定した移動後位置の座標から所定の距離内にある既知の物体若しくは部分の位置の座標の情報を前記物体位置データ格納部から抽出する請求項５記載のプログラム。
7. 請求項３記載の移動体位置推定システムを構成するために用いられるものであって、コンピュータを、
   移動体が移動前位置にあるときに移動体に搭載された測距センサを介して検出した、または測距センサによらずに知得している、移動前位置及び移動後位置の付近に所在する既知の複数の物体若しくは複数の物体の部分の各々の位置の座標を格納する物体位置データ格納部、並びに、
   移動体が移動後位置にあるときに移動体に搭載された測距センサを介して検出した複数の物体若しくは複数の物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して検出した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定する位置推定部
   として機能させ、
   前記位置推定部は、移動体が移動後位置にあるときに前記測距センサを介して所定の上限数まで移動体の周囲に存在する物体若しくは物体の部分の当該移動体からの距離及び移動体から見た方向の角度を知得し、その上限数の物体若しくは物体の部分の移動体からの距離及び移動体から見た方向の角度と、物体位置データ格納部に格納している既知の複数の物体若しくは複数の物体の部分の位置の座標とを基に、測距センサを介して知得した各物体若しくは各部分が既知の何れの物体若しくは部分に該当するのかの照合を試み、前者の物体若しくは部分と後者の物体若しくは部分とが合致する数が最も大きくなるような移動後位置の座標を選定するプログラム。

Images