JP2019148456A - 算出装置、自己位置算出方法、およびプログラム - Google Patents

Abstract

【課題】移動体の自己位置の推定精度を向上させる技術を提供する。【解決手段】算出装置１０は、移動体に搭載される装置であり、第１取得部１１０、第２取得部１２０、第３取得部１３０、および算出部１４０を有する。第１取得部１１０は、移動体周辺の他の移動体から、当該他の移動体の位置を示す位置情報及び位置情報の精度を示す精度情報を取得する。第２取得部１２０は、移動体と他の移動体との相対位置を示す相対位置情報を取得する。第３取得部１３０は、移動体の位置の精度を示す自己位置精度情報を取得する。算出部１４０は、第１取得部１１０により取得された精度情報の精度が、第３取得部１３０により取得された自己位置精度情報の精度より高い場合、第２取得部１２０により取得された相対位置情報及び第１取得部１１０により取得された他の移動体の位置情報に基づき、移動体の位置を算出する。【選択図】図２

Description

本発明は、算出装置、自己位置算出方法、およびプログラムに関する。

自己位置を推定する技術は、例えば移動体の自律航行制御システムやナビゲーションシステムなど、様々なシステムで利用されている。

自己位置を推定する技術の一例が、下記特許文献１に開示されている。特許文献１には、（１）移動体に搭載されたステレオカメラにより生成された画像を解析することにより、当該移動体の周囲で観測されたランドマークを特定し、（２）三角法などを用いてステレオカメラの設置位置からランドマークまでの距離を算出し、（３）ランドマークまでの距離と当該ランドマークの観測可能範囲とに基づいて、移動体の存在可能範囲（自己位置）を推定する技術が開示されている。

特開２００９−２００１４号公報

特許文献１に開示される技術では、移動体の周囲の情報を検出するために利用される装置の初期性能、当該装置の性能劣化、移動体の周囲の環境（例えば、降雨、降雪、積雪、地吹雪、霧、人工光、自然光）などによって、移動体の自己位置の推定精度が変動し得る。

本発明は、上述の課題に鑑みてなされたものであり、移動体の自己位置の推定精度を向上させる技術を提供することを一つの目的とする。

請求項１に記載の発明は、
移動体周辺の他の移動体から、当該他の移動体の位置を示す位置情報及び前記位置情報の精度を示す精度情報を取得する第１取得部と、
前記移動体と前記他の移動体との相対位置を示す相対位置情報を取得する第２取得部と、
前記移動体の位置の精度を検出する第３取得部と、
前記取得された精度情報が前記検出された精度より高い場合、前記取得された相対位置情報及び前記他の移動体の位置情報に基づき、前記移動体の位置を算出する算出部と、
を備える算出装置である。

請求項７に記載の発明は、
コンピュータが、
移動体周辺の他の移動体から、当該他の移動体の位置を示す位置情報及び前記位置情報の精度を示す精度情報を取得し、
前記移動体と前記他の移動体との相対位置を示す相対位置情報を取得し、
前記移動体の位置の精度を検出し、
前記取得された精度情報が前記検出された精度より高い場合、前記取得された相対位置情報及び前記他の移動体の位置情報に基づき、前記移動体の位置を算出する、
ことを含む自己位置算出方法である。

請求項８に記載の発明は、
コンピュータに、請求項７に記載の自己位置算出方法を実行させるプログラムである。

本発明に係る算出装置の動作を概念的に示す図である。 第１実施形態における算出装置の機能構成の例を概念的に示すブロック図である。 第１実施形態での算出部の動作を概念的に示す図である。 算出装置のハードウエア構成を例示するブロック図である。 第２実施形態の算出装置により実行される処理の流れを例示するフローチャートである。 第２実施形態での算出部の動作を概念的に示す図である。 第３実施形態での算出部の動作を概念的に示す図である。 第４実施形態での算出部の動作の一例を概念的に示す図である。 第４実施形態での算出部の動作の他の一例を概念的に示す図である。

以下、本発明の実施の形態について、図面を用いて説明する。尚、すべての図面において、同様な構成要素には同様の符号を付し、適宜説明を省略する。

［第１実施形態］
〔概要〕
まず、第１実施形態に係る算出装置の概要について説明する。図１は、本発明に係る算出装置１０の動作を概念的に示す図である。なお、図１は、算出装置１０の動作の理解を容易にすることを目的とする単なる例示である。よって、算出装置１０の動作は図１によって何ら限定されない。例えば、図１では移動体の一例として車両が描かれているが、移動体は車両に限定されない。移動体は、飛行機、船舶、または、自律航行が可能なロボット（例：ドローン）などであってもよい。また、図１では移動体Ｖ１および他の移動体Ｖ２が同じ方向に進んでいる様子が描かれているが、移動体Ｖ１および他の移動体Ｖ２の進行方向は異なっていてもよい。また、移動体Ｖ１は複数のセンシングデバイスＳＤを有していてもよい。その場合、算出装置１０は、各センシングデバイスＳＤからの出力信号を受信することができるように、複数のセンシングデバイスＳＤそれぞれと通信可能に接続される。ここで、センシングデバイスＳＤは、例えば、ＬｉＤＡＲ（Light Detection and Ranging）、ミリ波レーダー、深度情報を取得可能なカメラなどである。また、移動体Ｖ１の周りに複数の他の移動体Ｖ２が存在し、当該複数の他の移動体Ｖ２それぞれについて、後述の処理が実行されてもよい。

算出装置１０は移動体に搭載される装置である。なお、図１の例では、算出装置１０は移動体Ｖ１に搭載されている。ここで「移動体に搭載」とは、移動体の部品として当該移動体に組み込まれることのみならず、別途組み立てられた単体の装置が移動体の室内または移動体の室外に設置されることを含む。例えば、算出装置１０は、移動体を構成する部品として当該移動体に組み込まれたＥＣＵ（Electronic Control Unit）であってもよい。また例えば、算出装置１０は、移動体のインスツルメントパネル内に埋め込まれたナビゲーション装置であってもよい。また例えば、算出装置１０は、運転者により携帯され、移動体の内部に持ち込まれた携帯端末やコンピュータであってもよい。また例えば、算出装置１０は、移動体の周囲の状況を示す情報を取得するためのセンサ類を備え、移動体の窓や外周部などに取り付け可能な装置であってもよい。

算出装置１０は、移動体Ｖ１に備えられたセンサ類の出力を用いて自己位置を算出する第１の自己位置算出処理と、図１に例示されるように、他の車両Ｖ２の位置情報を基準として自己位置を算出する第２の自己位置算出処理と、を実行可能に構成される。

＜第１の自己位置算出処理＞
算出装置１０は、例えば、移動体Ｖ１に搭載された測位衛星システム（例えば、ＧＰＳ（Global Positioning System）など）の受信機（図示せず）等を用いて取得される位置情報や、当該測位衛星システムの受信機等から取得した位置情報および移動体Ｖ１に搭載されたセンシングデバイスＳＤを用いて検出された周囲環境を示す情報に基づく地図データとのマッピングにより算出した位置情報などを用いて、移動体Ｖ１の自己位置情報Ｐ１を算出することができる（Ｓ１−１）。また、算出装置１０は、自己位置情報Ｐ１の算出結果（自己位置の確率密度分布など）を用いて自己位置情報Ｐ１の精度を算出し、自己位置精度情報Ａ１を生成することができる（Ｓ１−２）。算出装置１０により算出された移動体Ｖ１の自己位置情報Ｐ１および当該自己位置情報Ｐ１の精度を示す自己位置精度情報Ａ１は、例えば、算出装置１０のメモリやストレージデバイスなどに記憶される。これら処理は、例えば、任意の間隔で繰り返し実行され、算出装置１０のメモリやストレージデバイスに記憶される自己位置情報Ｐ１および自己位置精度情報Ａ１は、上述の処理が実行される度に更新される。

＜第２の自己位置算出処理＞
算出装置１０は、移動体Ｖ１の周辺に存在する他の移動体Ｖ２の位置情報Ｐ２、および、当該他の移動体Ｖ２の位置情報Ｐ２の精度を示す精度情報Ａ２を取得する（Ｓ２−１）。算出装置１０は、例えば、他の移動体Ｖ２に搭載される図示しない機器と無線通信（車車間通信）を行うことによって、他の移動体Ｖ２に記憶されている、当該他の移動体Ｖ２の位置情報Ｐ２および当該位置情報Ｐ２の精度を示す精度情報Ａ２を取得することができる。また、移動体Ｖ１の走行路の近傍に路車間通信用の路側機（図示せず）が存在する場合、算出装置１０は、当該路側機を介して、他の移動体Ｖ２の位置情報Ｐ２および当該位置情報Ｐ２の精度を示す精度情報Ａ２を取得してもよい。

また、算出装置１０は、移動体Ｖ１と他の移動体Ｖ２との相対位置を示す相対位置情報を測定する（Ｓ２−２）。算出装置１０は、移動体Ｖ１に搭載されたセンシングデバイスＳＤを用いて、移動体Ｖ１から見た他の移動体Ｖ２の位置（相対位置）を測定することができる。

そして、算出装置１０は、自己位置精度情報Ａ１の精度（移動体Ｖ１の自己位置情報Ｐ１の精度）が精度情報Ａ２の精度（他の移動体Ｖ２の位置情報Ｐ２の精度）よりも低い場合、移動体Ｖ１と他の移動体Ｖ２との相対位置および当該他の移動体の位置情報Ｐ２を用いて、移動体Ｖ１の自己位置情報Ｐ１を更新する（Ｓ２−３）。算出装置１０は、メモリやストレージデバイスなどから自己位置精度情報Ａ１を読み出して精度情報Ａ２と比較することによって、移動体Ｖ１の自己位置情報Ｐ１の精度が他の移動体Ｖ２の位置情報Ｐ２の精度よりも低いか否かを判定できる。移動体Ｖ１の自己位置情報Ｐ１の精度が他の移動体Ｖ２の位置情報Ｐ２の精度よりも低い場合、算出装置１０は、他の車両Ｖ２の位置情報Ｐ２および移動体Ｖ１と他の移動体Ｖ２との相対位置を用いて、移動体Ｖ１が存在し得る範囲（自己位置情報Ｐ１）を算出する。

〔機能構成例〕
図２は、第１実施形態における算出装置１０の機能構成の例を概念的に示すブロック図である。図２に示されるように、算出装置１０は、第１取得部１１０、第２取得部１２０、第３取得部１３０、および算出部１４０を有する。

第１取得部１１０は、移動体Ｖ１の周辺に存在する他の移動体Ｖ２から、当該他の移動体Ｖ２の位置を示す位置情報及びその位置情報の精度を示す精度情報を取得する。ここで第１取得部１１０が取得する位置情報および精度情報は、他の移動体Ｖ２において算出されたものである。つまり、第１取得部１１０が取得する位置情報の精度は、他の移動体Ｖ２に備えられる位置推定用装置（センサ）の性能や、他の移動体Ｖ２での位置測定時の環境などに応じて決まることになる。また、第１取得部１１０は、他の移動体Ｖ２で算出された精度情報を取得するように構成されていてもよいし、他の移動体Ｖ２からは位置情報のみを取得し、その位置情報の特性（例えば、推定位置の確率密度分布など）に基づいて精度情報を独自に算出するように構成されていてもよい。

第２取得部１２０は、移動体Ｖ１と他の移動体Ｖ２との相対位置を示す相対位置情報を取得する。第２取得部１２０は、算出装置１０または移動体Ｖ１に搭載されるセンシングデバイスを用いて、相対位置情報を取得することができる。センシングデバイスとしては、例えば、光学式センサを利用して距離を測定可能なＬｉＤＡＲやカメラ（例えばステレオカメラなどの深度情報を取得可能なカメラ）のほか、ミリ波レーダーなどが利用され得る。

第３取得部１３０は、移動体Ｖ１の自己位置情報Ｐ１の精度を示す自己位置精度情報を取得する。一例として、第３取得部１３０は、移動体Ｖ１に搭載されるセンシングデバイスの出力に基づく自己位置情報Ｐ１の算出結果（例えば、位置情報の確率密度分布など）に基づいて自己位置情報Ｐ１の精度を算出することによって、自己位置精度情報を生成することができる。ここで、第３取得部１３０は、例えば第１取得部１１０によって他の移動体Ｖ２の位置情報および精度情報が取得されたことに応じて、移動体Ｖ１の位置の精度を算出してもよい。あるいは、第３取得部１３０は、自己位置精度情報を予め生成して算出装置１０のメモリやストレージデバイスなどに記憶しておき、当該自己位置精度情報を必要に応じて読み出してもよい。

算出部１４０は、第１取得部１１０により取得された精度情報の精度が、第３取得部１３０により取得された自己位置精度情報の精度よりも高い場合、第２取得部１２０により取得された相対位置情報及び第１取得部１１０により取得された他の移動体Ｖ２の位置情報を用いて、移動体Ｖ１の位置を算出する。以下、図を用いて算出部１４０の動作の具体例を説明する。なお、以下の説明はあくまで例示であり、算出部１４０の動作は以下で説明される内容に制限されない。

図３は、第１実施形態での算出部１４０の動作を概念的に示す図である。図３において、点線の矢印は、移動体Ｖ１と移動体Ｖ２との相対位置（ベクトル）を表している。また、図３において、点線の円は、算出部１４０の処理が実行される前の移動体Ｖ１の位置（位置座標）を表している。また、図３において、実線の円（斜めのハッチングあり）は、後述の算出部１４０の処理が実行された後の移動体Ｖ１の位置（位置座標）を表している。また、図３において、実線の円（ハッチングなし）は、他の移動体Ｖ２の位置（位置座標）を表している。また、図３において、各円の大きさは、位置の精度の高低を表している。すなわち、円が小さいほど位置の精度が高く、逆に円が大きいほど位置の精度が低いことを表している。図３の例では、他の移動体Ｖ２の位置の精度が、算出部１４０の処理が実行される前の移動体Ｖ１の位置の精度よりも高いことがわかる。なお、図示していないが、他の移動体Ｖ２の位置の精度が移動体Ｖ１の位置の精度よりも低い場合には、算出部１４０は以下で説明する処理を実行しない。尚、図３では、各移動体の位置が２次元座標上の位置として描かれているが、各移動体の位置は３次元座標上の位置でも構わない。例えば、各移動体が３次元座標の位置情報を保持していれば、算出部１４０は、３次元座標上で位置情報を扱うことができる。その場合、図３における円は球のような３次元形状となる。

図３に例示されるケースにおいて、算出部１４０は、他の移動体Ｖ２の位置座標を、移動体Ｖ１と他の移動体Ｖ２との相対位置（ベクトル）を用いて平行移動し、移動体Ｖ１の新たな位置（斜めのハッチングをした円）を算出する。そして、算出部１４０は、移動体Ｖ１の新たな位置を用いて、算出装置１０のメモリやストレージデバイスなどに記憶されている自己位置情報Ｐ１を更新する。

なお、算出部１４０は、移動体Ｖ１と移動体Ｖ２との相対位置（ベクトル）を、第２取得部１２０により取得された相対位置情報を用いて計算することができる。また、算出部１４０は、算出装置１０のメモリやストレージデバイスなどに記憶されている自己位置情報に基づいて、所定の座標系での移動体Ｖ１の位置を把握することができる。また、算出部１４０は、第１取得部１１０により取得された、他の移動体Ｖ２の位置情報に基づいて、所定の座標系での他の移動体Ｖ２の位置を把握することができる。なお、ここでは、移動体Ｖ１の自己位置情報の座標系と他の移動体Ｖ２の位置情報の座標系とが共通であることを前提としているが、移動体Ｖ１の自己位置情報の座標系と他の移動体Ｖ２の位置情報の座標系とが異なる場合もある。その場合、算出部１４０は、それぞれの位置情報が示す位置を共通の座標系に置き換えることで、移動体Ｖ１の位置を誤って算出することを防止できる。

〔ハードウエア構成例〕
算出装置１０の各機能構成部は、各機能構成部を実現するハードウエア（例：ハードワイヤードされた電子回路など）で実現されてもよいし、ハードウエアとソフトウエアとの組み合わせ（例：電子回路とそれを制御するプログラムの組み合わせなど）で実現されてもよい。以下、算出装置１０の各機能構成部がハードウエアとソフトウエアとの組み合わせで実現される場合について、さらに説明する。

図４は、算出装置１０のハードウエア構成を例示するブロック図である。本図の例において、算出装置１０は、集積回路４０を用いて実装されている。集積回路４０は、例えばＳｏＣ（System On Chip）である。

集積回路４０は、バス４０２、プロセッサ４０４、メモリ４０６、ストレージデバイス４０８、入出力インタフェース４１０、及びネットワークインタフェース４１２を有する。バス４０２は、プロセッサ４０４、メモリ４０６、ストレージデバイス４０８、入出力インタフェース４１０、及びネットワークインタフェース４１２が、相互にデータを送受信するためのデータ伝送路である。ただし、プロセッサ４０４などを互いに接続する方法は、バス接続に限定されない。プロセッサ４０４は、マイクロプロセッサなどを用いて実現される演算処理装置である。メモリ４０６は、ＲＡＭ（Random Access Memory）などを用いて実現される主記憶装置である。ストレージデバイス４０８は、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）、ＲＯＭ（Read Only Memory）やフラッシュメモリなどを用いて実現される補助記憶装置である。

ストレージデバイス４０８は、上述した、算出装置１０の各機能構成部（第１取得部１１０、第２取得部１２０、第３取得部１３０、および算出部１４０）を実現するためのプログラムモジュールを記憶している。プロセッサ４０４は、このプログラムモジュールをメモリ４０６に読み出して実行することで、算出装置１０の各機能を実現する。

入出力インタフェース４１０は、算出装置１０を周辺機器と接続するためのインタフェースである。入出力インタフェース４１０には、ＬｉＤＡＲ、ミリ波レーダー、ステレオカメラといったセンシングデバイスＳＤが少なくとも１つ接続される。また、入出力インタフェース４１０には、例えば、ＧＰＳ（Global Positioning System）などの測位衛星システムからの信号を受信するための受信モジュール４１０１や、移動体Ｖ１の角速度および加速度などを示す情報を生成するための慣性計測モジュール４１０２などが接続される。なお、慣性計測モジュール４１０２は、例えばジャイロセンサなどを用いて移動体Ｖ１の角速度および加速度を示す情報を生成することができる。算出装置１０は、これらのモジュールで生成された情報を、上述の第１の自己位置推定処理で利用することができる。また、入出力インタフェース４１０には、ユーザからの入力操作を受け付ける入力装置４１０３、各種情報を表示させるディスプレイ装置４１０４、又は、それらが一体となったタッチパネルなどが更に接続されてもよい。

ネットワークインタフェース４１２は、算出装置１０を通信網に接続するためのインタフェースである。この通信網は、例えばＷＡＮ（Wide Area Network）通信網やＣＡＮ（Controller Area Network）通信網である。ネットワークインタフェース４１２が通信網に接続する方法は、無線接続であってもよいし、有線接続であってもよい。例えば算出装置１０は、ネットワークインタフェース４１２を介して、周辺に位置する他の移動体Ｖ２と通信することができる。また、算出装置１０は、ネットワークインタフェース４１２を介して移動体Ｖ１の制御情報（アクセル開度、ブレーキ圧、ステアリング角度など）を取得し、上述の第１の自己位置推定処理に利用することができる。

なお、集積回路４０のハードウエア構成は本図に示した構成に限定されない。例えば、プログラムモジュールはメモリ４０６に格納されてもよい。この場合、集積回路４０は、ストレージデバイス４０８を備えていなくてもよい。

以上、本実施形態では、移動体Ｖ１の周辺に存在する他の移動体Ｖ２から、より精度の高い位置情報を取得できた場合、より精度の高い位置情報および移動体Ｖ１と他の移動体Ｖ２との相対位置を用いて、移動体Ｖ１の自己位置情報が算出される。これにより、移動体Ｖ１の位置情報の精度を向上させることができる。また、他の移動体Ｖ２から取得した位置情報の精度が、移動体Ｖ１の自己位置情報の精度よりも低い場合には、他の移動体Ｖ２の位置情報および移動体Ｖ１と他の移動体Ｖ２との相対位置を用いて移動体Ｖ１の自己位置情報を算出する処理を実行しない。これにより、移動体Ｖ１の位置の精度が、より精度の低い他の移動体Ｖ２の位置情報の影響を受けて低下することを防止できる。

［第２実施形態］
本実施形態は、以下で説明する点を除き、第１実施形態と同様の構成を有する。

〔機能構成〕
本実施形態の算出装置１０は、第１実施形態と同様の機能構成（例：図２）を有する。

本実施形態において、第１取得部１１０は、他の移動体Ｖ２の位置情報の算出基準を示す情報を更に取得する。特に限定されないが、一例として、「他の移動体Ｖ２の位置情報の算出基準を示す情報」は、「他の移動体Ｖ２の形状を示す情報」と「他の移動体Ｖ２における位置推定用装置（センサ）の位置」とを含む。「他の移動体Ｖ２の形状を示す情報」とは、例えば、他の移動体Ｖ２の３次元モデルデータなどである。

第１取得部１１０により取得された「他の移動体Ｖ２の位置情報の算出基準を示す情報」を更に用いることによって、算出部１４０は、移動体Ｖ１の位置をより高精度に算出することが可能となる。具体的には、第１実施形態では、算出部１４０は、他の移動体Ｖ２の位置情報および移動体Ｖ１と他の移動体Ｖ２との相対位置を用いて、移動体Ｖ１の位置を算出している。ここで、移動体Ｖ１と他の移動体Ｖ２との相対位置は、移動体Ｖ１のセンシングデバイスＳＤによって検出された他の移動体Ｖ２の表面を基準に決まる。そして、センシングデバイスＳＤによって検出された表面が、他の移動体Ｖ２が位置情報を算出する際の基準と一致しているとは限らない。例えば、他の移動体Ｖ２は、位置測定用装置（センサ）の設置位置を基準として自身の位置情報を算出することもあり得る。この場合、第１実施形態の手法では、センシングデバイスＳＤが検出した面と、他の移動体Ｖ２に備えられた位置測定用装置（センサ）との相対位置だけ、誤差が生じることになる。

そこで、本実施形態の算出部１４０は、第１取得部１１０により更に取得された「他の移動体Ｖ２の位置情報の算出基準を示す情報」を更に用いて、移動体Ｖ１の位置を算出する。なお、算出部１４０の具体的な動作については後述する。

〔処理の流れ〕
図５に、本実施形態の算出装置１０により実行される処理の流れを例示する。図５は、第２実施形態の算出装置１０により実行される処理の流れを例示するフローチャートである。

第１取得部１１０は、他の移動体Ｖ２の位置情報、当該位置情報の精度を示す精度情報、並びに、当該他の移動体Ｖ２の位置情報の算出基準を示す情報を取得する（Ｓ１０２）。本例では、第１取得部１１０は、「他の移動体Ｖ２の形状を示す情報」および「他の移動体Ｖ２における位置推定用装置（センサ）の位置」を、「他の移動体Ｖ２の位置情報の算出基準を示す情報」として取得するものとする。第２取得部１２０は、センシングデバイスＳＤを用いて、移動体Ｖ１と移動体Ｖ２との相対位置を示す相対位置情報を取得する（Ｓ１０４）。第３取得部１３０は、移動体Ｖ１の位置の精度を示す自己位置精度情報を取得する（Ｓ１０６）。自己位置精度情報は、例えば、メモリ４０６やストレージデバイス４０８などに記憶されている。算出部１４０は、Ｓ１０２の処理で取得された精度情報が示す精度と、Ｓ１０６で取得された自己位置精度情報が示す精度とを比較する（Ｓ１０８）。Ｓ１０２の処理で取得された精度情報が示す精度が、Ｓ１０６で取得された自己位置精度情報が示す精度よりも低い場合（Ｓ１０８：ＮＯ）、以降の処理は実行されない。一方、Ｓ１０２の処理で取得された精度情報が示す精度が、Ｓ１０６で取得された自己位置精度情報が示す精度よりも高い場合（Ｓ１０８：ＹＥＳ）、算出部１４０は、上述の各処理で取得された情報を用いて、移動体Ｖ１の位置情報を算出する（Ｓ１１０）。以下、図を用いて算出部１４０の動作の具体例を説明する。なお、以下の説明はあくまで例示であり、算出部１４０の動作は以下で説明される内容に制限されない。

図６は、第２実施形態での算出部１４０の動作を概念的に示す図である。図６において、点線の矢印は、移動体Ｖ１と移動体Ｖ２との相対位置（ベクトル）を表している。また、図６において、実線の矢印は、センシングデバイスＳＤが検出した他の移動体Ｖ２の部位と他の移動体Ｖ２に搭載されているセンサとの相対位置（ベクトル）を表している。また、図６において、点線の円は、算出部１４０の処理が実行される前の移動体Ｖ１の位置（位置座標）を表している。また、図６において、実線の円（斜めのハッチングあり）は、後述の算出部１４０の処理が実行された後の移動体Ｖ１の位置（位置座標）を表している。また、図６において、実線の円（ハッチングなし）は、他の移動体Ｖ２の位置（位置座標）を表している。また、図６において、各円の大きさは、位置の精度の高低を表している。すなわち、円が小さいほど位置の精度が高く、逆に円が大きいほど位置の精度が低いことを表している。図６の例では、他の移動体Ｖ２の位置の精度が、算出部１４０の処理が実行される前の移動体Ｖ１の位置の精度よりも高いことがわかる。なお、図示していないが、他の移動体Ｖ２の位置の精度が移動体Ｖ１の位置の精度よりも低い場合には、算出部１４０は以下で説明する処理を実行しない。尚、図６では、各移動体の位置が２次元座標上の位置として描かれているが、各移動体の位置は３次元座標上の位置でも構わない。例えば、各移動体が３次元座標の位置情報を保持していれば、算出部１４０は、３次元座標上で位置情報を扱うことができる。その場合、図６における円は球のような３次元形状となる。

図６に例示されるケースにおいて、算出部１４０は、他の移動体Ｖ２の位置座標を、移動体Ｖ１と他の移動体Ｖ２との相対位置（ベクトル）およびセンシングデバイスＳＤが検出した他の移動体Ｖ２の部位と他の移動体Ｖ２に搭載されているセンサとの相対位置（ベクトル）を用いて平行移動し、移動体Ｖ１の新たな位置（斜めのハッチングをした円）を算出する。そして、算出部１４０は、移動体Ｖ１の新たな位置を用いて、算出装置１０のメモリやストレージデバイスなどに記憶されている自己位置情報Ｐ１を更新する。

なお、算出部１４０は、移動体Ｖ１と移動体Ｖ２との相対位置（ベクトル）を、第２取得部１２０により取得された相対位置情報を用いて計算することができる。また、算出部１４０は、センシングデバイスＳＤからの出力と、第１取得部１１０により取得された「他の移動体Ｖ２の位置情報の算出基準を示す情報」に基づいて、センシングデバイスＳＤが検出した面と他の移動体Ｖ２のセンサとの相対位置（ベクトル）を計算することができる。具体的には、算出部１４０は、センシングデバイスＳＤの出力結果と他の移動体Ｖ２の形状を示す情報とを比較して、センシングデバイスＳＤが検出した他の移動体Ｖ２の部位の位置を特定することができる。一例として、算出部１４０は、第１取得部１１０が取得した、他の移動体Ｖ２の３次元モデルデータと、センシングデバイスＳＤによる他の移動体Ｖ２のセンシング結果（３次元的な形状を示すデータ）とを比較することによって、当該センシング結果に対応する部位を３次元モデルデータの中から特定することができる。さらに、算出部１４０は、他の移動体Ｖ２における位置推定用装置（センサ）の位置を用いて、特定した他の移動体Ｖ２の部位から位置測定用装置（センサ）の取り付け位置までの距離および方向（ベクトル）を算出することができる。また、算出部１４０は、算出装置１０のメモリやストレージデバイスなどに記憶されている自己位置情報に基づいて、所定の座標系での移動体Ｖ１の位置を把握することができる。また、算出部１４０は、第１取得部１１０により取得された、他の移動体Ｖ２の位置情報に基づいて、所定の座標系での他の移動体Ｖ２の位置を把握することができる。なお、ここでは、移動体Ｖ１の自己位置情報の座標系と他の移動体Ｖ２の位置情報の座標系とが共通であることを前提としているが、移動体Ｖ１の自己位置情報の座標系と他の移動体Ｖ２の位置情報の座標系とが異なる場合もある。その場合、算出部１４０は、それぞれの位置情報が示す位置を共通の座標系に置き換えることで、移動体Ｖ１の位置を誤って算出することを防止できる。

以上、他の移動体Ｖ２の位置情報の算出基準を示す情報を更に用いることにより、当該他の移動体Ｖ２の位置情報を基準として移動体Ｖ１の位置を算出する際、その算出精度を更に向上させることができる。

［第３実施形態］
本実施形態では、移動体Ｖ１の周囲に複数の他の移動体Ｖ２が存在する場合における算出装置１０の動作について説明する。

〔機能構成〕
本実施形態の算出装置１０は、第１実施形態と同様の機能構成（例：図２）を有する。

移動体Ｖ１の周囲に複数の他の移動体Ｖ２が存在した場合、第２取得部１２０は、当該複数の他の移動体Ｖ２それぞれについて、相対位置情報を取得することができる。ここで、複数の他の移動体Ｖ２の１台から、より精度の高い位置情報が取得できた場合を考える。この場合、第２取得部１２０により取得された複数の相対位置情報の中から、位置情報送信してきた他の移動体Ｖ２に対応する相対位置情報を選択する必要がある。相対位置情報を誤って選択した場合、算出部１４０が、誤った位置を移動体Ｖ１の位置として算出してしまうからである。

そこで、算出部１４０は、第１取得部１１０により取得された他の移動体Ｖ２の位置情報と、移動体Ｖ１がメモリ４０６やストレージデバイス４０８などに記憶している自己位置情報とを用いて、位置情報を送信した他の移動体Ｖ２に対応する相対位置情報を特定する。以下、図を用いて算出部１４０の動作の具体例を説明する。なお、以下の説明はあくまで例示であり、算出部１４０の動作は以下で説明される内容に制限されない。

図７は、第３実施形態での算出部１４０の動作を概念的に示す図である。図７の例では、移動体Ｖ１の周囲に、２台の他の移動体が存在している。ここでは、移動体Ｖ１の前方に位置する他の移動体Ｖ２_Ａを「第１の他の移動体」と、移動体Ｖ１の右斜め前方に位置する他の移動体Ｖ２_Ｂを「第２の他の移動体」と表記する。

図示されるような状況において第１取得部１１０が、周辺に位置する他の移動体のいずれかから位置情報を取得したとする。この段階では、算出装置１０は、その位置情報が第１の他の移動体Ｖ２_Ａおよび第２の他の移動体Ｖ２_Ｂのどちらが位置情報を送信したかを判別できない。ここで、第２取得部１２０は、センシングデバイスＳＤを用いて、移動体Ｖ１と第１の他の移動体Ｖ２_Ａとの相対位置ｄ_Ａを示す第１の相対位置情報、および、移動体Ｖ１と第２の他の移動体Ｖ２_Ｂとの相対位置ｄ_Ｂを示す第２の相対位置情報をそれぞれ取得している。また、移動体Ｖ１の自己位置情報が算出装置１０のメモリ４０６やストレージデバイス４０８などには記憶されており、移動体Ｖ１の大よその位置が既知となっている。そこで、算出部１４０は、第１取得部１１０により取得された位置情報と、移動体Ｖ１の自己位置情報とを用いて、仮の相対位置を示す仮相対位置情報を生成する。そして、算出部１４０は、算出した仮相対位置情報によって示される相対位置が、移動体Ｖ１と第１の他の移動体Ｖ２_Ａとの相対位置ｄ_Ａと、移動体Ｖ１と第２の他の移動体Ｖ２_Ｂとの相対位置ｄ_Ｂのどちらに近いかを判定する。具体的な例として、自己位置情報と、第１取得部１１０が取得した他の移動体の位置情報とを用いて算出された仮の相対位置が、相対位置ｄ_Ｂよりも相対位置ｄ_Ａに近かったとする。この場合、算出部１４０は、位置情報を送信した他の移動体に対応する相対位置情報として、移動体Ｖ１と第１の他の移動体Ｖ２_Ａとの相対位置ｄ_Ａを示す第１の相対位置情報を選択することができる。以降は、上述の各実施形態で説明したような、移動体Ｖ１の位置を算出する処理が実行される。

以上、本実施形態によれば、移動体Ｖ１の周辺に複数の他の移動体が存在した場合であっても、当該移動体Ｖ１の位置の精度を高める処理を実行することができる。

［第４実施形態］
移動体の周辺に位置する他の移動体から当該他の移動体の位置情報を取得するまで、わずかながらタイムラグが生じ得る。移動体および他の移動体は、そのタイムラグの間にも移動を続けているため、算出部１４０により算出される移動体の位置に、移動による誤差が生じる可能性がある。本実施形態は、この問題を解消する構成を更に備える。なお、本実施形態の算出装置１０は、その他の点については、上述の各実施形態と同様の構成を有する。

〔機能構成〕
本実施形態の算出装置１０は、第１実施形態と同様の機能構成（例：図２）を有する。

本実施形態において、算出部１４０は、（１）移動体Ｖ１を基準とした場合の計算、および、（２）他の移動体Ｖ２を基準とした場合の計算、の少なくともいずれかを実行する。

＜移動体Ｖ１を基準とした場合の計算＞
図を用いて算出部１４０の動作の具体例を説明する。なお、以下の説明はあくまで例示であり、算出部１４０の動作は以下で説明される内容に制限されない。図８は、第４実施形態での算出部１４０の動作の一例を概念的に示す図である。まず、第１取得部１１０が、他の移動体Ｖ２の位置情報を取得する。このとき、他の移動体Ｖ２の位置情報には時刻情報が関連付けられる。この時刻情報は、他の移動体Ｖ２の位置情報の生成タイミングに対応する時刻を示す。そして、算出部１４０は、例えば次のように動作する。まず、算出部１４０は、時刻情報と現在時刻との時間差を算出する（Ｓ３−１）。算出部１４０は、例えば、第１取得部１１０により他の移動体Ｖ２の位置情報が取得された際、算出装置１０で管理されるシステム時刻を参照することによって、現在時刻を取得することができる。そして、算出部１４０は、先で算出した時間差に他の移動体Ｖ２の速度を乗じた値、および、他の移動体Ｖ２の軌跡を用いて、他の移動体の位置情報を補正する（Ｓ３−２）。ここで、補正した後の他の移動体Ｖ１の位置情報が示す位置が、算出装置１０が当該他の移動体Ｖ１の移動体を受信した時点での位置となる。この処理により、他の移動体Ｖ２からの情報が算出装置１０に到達するまでのタイムラグによって生じる誤差を相殺することができる。なお、算出部１４０は、移動体Ｖ１の制御情報やセンシングデバイスＳＤからの出力を解析することによって、他の移動体Ｖ２の移動速度、並びに、他の移動体Ｖ２の軌跡を算出することができる。そして、算出部１４０は、補正後の他の移動体Ｖ２の位置情報、および、移動体Ｖ１と他の移動体Ｖ２との相対位置を示す相対位置情報を用いて、移動体Ｖ１の位置を算出する（Ｓ３−３）。

＜他の移動体Ｖ２を基準とした場合の計算＞
図を用いて算出部１４０の動作の具体例を説明する。なお、以下の説明はあくまで例示であり、算出部１４０の動作は以下で説明される内容に制限されない。図９は、第４実施形態での算出部１４０の動作の他の一例を概念的に示す図である。まず、第１取得部１１０が、他の移動体Ｖ２の位置情報を取得する。このとき、他の移動体Ｖ２の位置情報には時刻情報が関連付けられる。この時刻情報は、他の移動体Ｖ２の位置情報の生成タイミングに対応する時刻を示す。そして、算出部１４０は、例えば次のように動作する。まず、算出部１４０は、他の移動体Ｖ２の位置情報に関連付けられた時刻情報を用いて、その時刻情報が示す時点での移動体Ｖ１と他の移動体Ｖ２との相対位置を示す相対位置情報（過去の相対位置情報）を取得する（Ｓ４−１）。なお、この場合、算出装置１０は、移動体Ｖ１の位置情報および移動体Ｖ１と他の移動体Ｖ２との相対位置を示す相対位置情報の所定期間分の履歴（過去の情報）を、メモリ４０６またはストレージデバイス４０８などに記憶している。算出部１４０は、メモリ４０６またはストレージデバイス４０８に記憶されている履歴の中から、他の移動体Ｖ２の位置情報に関連付けられた時刻情報が示す時点における、移動体Ｖ１と他の移動体Ｖ２との相対位置を示す相対位置情報を読み出すことができる。そして、算出部１４０は、他の移動体の位置情報と、Ｓ４−１の処理で取得した過去の相対位置情報とを用いて、位置情報が送信された時点の移動体Ｖ１の位置（移動体Ｖ１の過去の位置）を算出する（Ｓ４−２）。この処理により、他の移動体Ｖ２からの情報が算出装置１０に到達するまでのタイムラグによって生じる誤差を相殺することができる。そして、算出部１４０は、時刻情報と現在時刻との時間差を算出する（Ｓ４−３）。算出部１４０は、例えば、第１取得部１１０により他の移動体Ｖ２の位置情報が取得された際、算出装置１０で管理されるシステム時刻を参照することによって、現在時刻を取得することができる。そして、算出部１４０は、Ｓ４０２の処理で算出した移動体Ｖ１の過去の位置、Ｓ４−３の処理で算出した時間差に移動体Ｖ１の速度を乗じた値、および、移動体Ｖ１の軌跡を用いて、移動体Ｖ１の現在の位置を算出する（Ｓ４−４）。なお、算出部１４０は、ＣＡＮ通信網を介して、移動体Ｖ１の速度や軌跡を示す制御情報を取得することができる。

以上、本実施形態では、他の移動体Ｖ２からの情報が算出装置１０に到達するまでのタイムラグによって生じる誤差を相殺する処理が実行される。これにより、本実施形態の算出装置１０によれば、移動体Ｖ１の自己位置の精度を更に高めることができる。

以上、図面を参照して実施形態及び実施例について述べたが、これらは本発明の例示であり、上記以外の様々な構成を採用することもできる。

以下、参考形態の例を付記する。
１．
移動体周辺の他の移動体から、当該他の移動体の位置を示す位置情報及び前記位置情報の精度を示す精度情報を取得する第１取得部と、
前記移動体と前記他の移動体との相対位置を示す相対位置情報を取得する第２取得部と、
前記移動体の位置の精度を示す自己位置精度情報を取得する第３取得部と、
前記精度情報の精度が前記自己位置精度情報の精度より高い場合、前記相対位置情報及び前記他の移動体の位置情報に基づき、前記移動体の位置を算出する算出部と、
を備える算出装置。
２．
前記第２取得部は、光学式センサを用いて、前記相対位置情報を取得する、
１．に記載の算出装置。
３．
前記第１取得部は、前記他の移動体の位置情報の算出基準を示す情報を更に取得し、
前記算出部は、前記算出基準を更に用いて前記移動体の位置を算出する、
１．または２．に記載の算出装置。
４．
前記算出部は、前記移動体の周辺に複数の他の移動体が存在し、複数の前記相対位置情報が前記第２取得部により取得された場合、前記第１取得部により取得された前記他の移動体の位置情報と、前記移動体が保持している当該移動体の位置とを用いて、当該位置情報を送信した他の移動体に対応する前記相対位置情報を特定する、
１．〜３．のいずれか１つに記載の算出装置。
５．
前記他の移動体の位置情報には時刻情報が関連付けられており、
前記算出部は、
前記時刻情報と現在時刻との時間差を算出し、
前記時間差に前記他の移動体の速度を乗じた値、および、前記他の移動体の軌跡を用いて、前記他の移動体の位置情報を補正し、
補正後の前記他の移動体の位置情報と前記相対位置情報とを用いて、前記移動体の位置を算出する、
１．〜４．のいずれか１つに記載の算出装置。
６．
前記他の移動体の位置情報には時刻情報が関連付けられており、
前記算出部は、
前記時刻情報が示す時点での前記移動体と前記他の移動体との過去の相対位置を取得し、
前記他の移動体の位置情報、および、前記移動体と前記他の移動体との過去の相対位置を用いて、前記移動体の過去の位置を算出し、
前記時刻情報と現在時刻との時間差を算出し、
前記移動体の過去の位置、前記時間差に前記移動体の速度を乗じた値、および、前記移動体の軌跡を用いて、前記移動体の現在の位置を算出する、
１．〜４．のいずれか１つに記載の算出装置。
７．
コンピュータが、
移動体周辺の他の移動体から、当該他の移動体の位置を示す位置情報及び前記位置情報の精度を示す精度情報を取得し、
前記移動体と前記他の移動体との相対位置を示す相対位置情報を取得し、
前記移動体の位置の精度を示す自己位置精度情報を取得し、
前記精度情報の精度が前記自己位置精度情報の精度より高い場合、前記相対位置情報及び前記他の移動体の位置情報に基づき、前記移動体の位置を算出する、
ことを含む自己位置算出方法。
８．
前記コンピュータが、
光学式センサを用いて、前記相対位置情報を取得する、
ことを含む７．に記載の自己位置算出方法。
９．
前記コンピュータが、
前記他の移動体の位置情報の算出基準を示す情報を更に取得し、
前記算出基準を更に用いて前記移動体の位置を算出する、
ことを含む７．または８．に記載の自己位置算出方法。
１０．
前記コンピュータが、
前記移動体の周辺に複数の他の移動体が存在し、複数の前記相対位置情報が取得された場合、取得された前記他の移動体の位置情報と、前記移動体が保持している当該移動体の位置とを用いて、当該位置情報を送信した他の移動体に対応する前記相対位置情報を特定する、
ことを含む７．〜９．のいずれか１つに記載の自己位置算出方法。
１１．
前記他の移動体の位置情報には時刻情報が関連付けられており、
前記コンピュータが、
前記時刻情報と現在時刻との時間差を算出し、
前記時間差に前記他の移動体の速度を乗じた値、および、前記他の移動体の軌跡を用いて、前記他の移動体の位置情報を補正し、
補正後の前記他の移動体の位置情報と前記相対位置情報とを用いて、前記移動体の位置を算出する、
ことを含む７．〜１０．のいずれか１つに記載の自己位置算出方法。
１２．
前記他の移動体の位置情報には時刻情報が関連付けられており、
前記コンピュータが、
前記時刻情報が示す時点での前記移動体と前記他の移動体との過去の相対位置を取得し、
前記他の移動体の位置情報、および、前記移動体と前記他の移動体との過去の相対位置を用いて、前記移動体の過去の位置を算出し、
前記時刻情報と現在時刻との時間差を算出し、
前記移動体の過去の位置、前記時間差に前記移動体の速度を乗じた値、および、前記移動体の軌跡を用いて、前記移動体の現在の位置を算出する、
ことを含む７．〜１０．のいずれか１つに記載の自己位置算出方法。
１３．
コンピュータに、７．〜１２．のいずれか１つに記載の自己位置算出方法を実行させるプログラム。

１０ 算出装置
１１０ 第１取得部
１２０ 第２取得部
１３０ 第３取得部
１４０ 算出部
４０ 集積回路
４０２ バス
４０４ プロセッサ
４０６ メモリ
４０８ ストレージデバイス
４１０ 入出力インタフェース
４１０１ 受信モジュール
４１０２ 慣性計測モジュール
４１０３ 入力装置
４１０４ ディスプレイ装置
４１２ ネットワークインタフェース
ＳＤ センシングデバイス

Claims (8)

1. 移動体周辺の他の移動体から、当該他の移動体の位置を示す位置情報及び前記位置情報の精度を示す精度情報を取得する第１取得部と、
   前記移動体と前記他の移動体との相対位置を示す相対位置情報を取得する第２取得部と、
   前記移動体の位置の精度を示す自己位置精度情報を取得する第３取得部と、
   前記精度情報の精度が前記自己位置精度情報の精度より高い場合、前記相対位置情報及び前記他の移動体の位置情報に基づき、前記移動体の位置を算出する算出部と、
   を備える算出装置。
2. 前記第２取得部は、光学式センサを用いて、前記相対位置情報を取得する、
   請求項１に記載の算出装置。
3. 前記第１取得部は、前記他の移動体の位置情報の算出基準を示す情報を更に取得し、
   前記算出部は、前記算出基準を更に用いて前記移動体の位置を算出する、
   請求項１または２に記載の算出装置。
4. 前記算出部は、前記移動体の周辺に複数の他の移動体が存在し、複数の前記相対位置情報が前記第２取得部により取得された場合、前記第１取得部により取得された前記他の移動体の位置情報と、前記移動体が保持している当該移動体の位置とを用いて、当該位置情報を送信した他の移動体に対応する前記相対位置情報を特定する、
   請求項１〜３のいずれか１項に記載の算出装置。
5. 前記他の移動体の位置情報には時刻情報が関連付けられており、
   前記算出部は、
   前記時刻情報と現在時刻との時間差を算出し、
   前記時間差に前記他の移動体の速度を乗じた値、および、前記他の移動体の軌跡を用いて、前記他の移動体の位置情報を補正し、
   補正後の前記他の移動体の位置情報と前記相対位置情報とを用いて、前記移動体の位置を算出する、
   請求項１〜４のいずれか１項に記載の算出装置。
6. 前記他の移動体の位置情報には時刻情報が関連付けられており、
   前記算出部は、
   前記時刻情報が示す時点での前記移動体と前記他の移動体との過去の相対位置を取得し、
   前記他の移動体の位置情報、および、前記移動体と前記他の移動体との過去の相対位置を用いて、前記移動体の過去の位置を算出し、
   前記時刻情報と現在時刻との時間差を算出し、
   前記移動体の過去の位置、前記時間差に前記移動体の速度を乗じた値、および、前記移動体の軌跡を用いて、前記移動体の現在の位置を算出する、
   請求項１〜４のいずれか１項に記載の算出装置。
7. コンピュータが、
   移動体周辺の他の移動体から、当該他の移動体の位置を示す位置情報及び前記位置情報の精度を示す精度情報を取得し、
   前記移動体と前記他の移動体との相対位置を示す相対位置情報を取得し、
   前記移動体の位置の精度を示す自己位置精度情報を取得し、
   前記精度情報の精度が前記自己位置精度情報の精度より高い場合、前記相対位置情報及び前記他の移動体の位置情報に基づき、前記移動体の位置を算出する、
   ことを含む自己位置算出方法。
8. コンピュータに、請求項７に記載の自己位置算出方法を実行させるプログラム。

Images