特許 7561716 位置検出システムおよび位置検出方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-09-26

(45)【発行日】2024-10-04

(54)【発明の名称】位置検出システムおよび位置検出方法

(51)【国際特許分類】

   G05D 1/246 20240101AFI20240927BHJP

   G05D 1/242 20240101ALI20240927BHJP

   G05D 1/43 20240101ALI20240927BHJP

【ＦＩ】

G05D1/246

G05D1/242

G05D1/43

【請求項の数】 6

(21)【出願番号】P 2021148913

(22)【出願日】2021-09-13

(65)【公開番号】P2023041505

(43)【公開日】2023-03-24

【審査請求日】2023-10-26

(73)【特許権者】

【識別番号】502129933

【氏名又は名称】株式会社日立産機システム

(74)【代理人】

【識別番号】110001689

【氏名又は名称】青稜弁理士法人

(72)【発明者】

【氏名】槙 修一

【審査官】尾形 元

(56)【参考文献】

【文献】特開２０１９－２１５７７３（ＪＰ，Ａ）

【文献】特開２０２０－２４６１８（ＪＰ，Ａ）

【文献】特開２０２１－４７６７１（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／００－ １／８７

(57)【特許請求の範囲】

【請求項1】

距離センサで計測した測定データと地図情報とを照合することによって自身の位置を推定しながら移動する移動体の位置を検出する位置検出システムであって、
所定のタイミングで繰り返し、計測された前記測定データと、前記移動体の座標系である相対座標系における原点位置を含む地図情報とを照合し、前記相対座標系での前記移動体の位置および姿勢を検出する相対位置姿勢検出処理部と、
前記所定のタイミングのそれぞれにおいて、検出された前記移動体の位置および姿勢を前記地図情報にあらわした部分地図を作成する部分地図作成部と、
前記所定のタイミングで繰り返し得られた前記移動体の位置および姿勢の差に基づいて、前記部分地図における前記移動体の相対的な移動量を算出する相対移動量算出部と、
前記部分地図での前記移動体の位置および姿勢を、座標に関する所定のパラメータを用いて、全体地図上での前記移動体の位置および姿勢に変換し、当該変換に用いた前記所定のパラメータの補正量を算出する広域位置姿勢検出部と、
前記補正量の分だけ、前記相対座標系における原点位置を、前記全体地図の座標系である全体座標系における原点位置に変換する部分地図原点算出部と、
変換された前記全体座標系における原点位置と、前記移動体の相対的な移動量とを用いて、前記相対座標系での位置および姿勢を全体座標系に変換する座標変換部と、
前記全体座標系に変換された前記相対座標系での位置および姿勢と、当該位置および姿勢を検出したときの前記測定データを、前記移動体の表示部に表示する表示装置制御部と、を有し、
前記相対移動量算出部が、前記全体座標系における前記移動体の相対的な移動量が所定の閾値Ｄ以上でないと判定した場合、前記広域位置姿勢検出部、前記部分地図原点算出部、前記座標変換部は処理を行わず、前記表示装置制御部が前記移動体の表示部に原点特定失敗を通知する、
ことを特徴とする位置検出システム。

【請求項2】

請求項１に記載の位置検出システムにおいて、
前記相対位置姿勢検出処理部および前記部分地図作成部は、それぞれ、ＳＬＡＭ処理により、前記相対座標系での前記移動体の位置および姿勢を検出し、前記部分地図を作成する、
ことを特徴とする位置検出システム。

【請求項3】

請求項１に記載の位置検出システムにおいて、
前記広域位置姿勢検出部は、前記相対座標系での位置および姿勢を、前記全体座標系での前記移動体の位置および姿勢に一意に変換できない場合、過去の所定のタイミングで得られた、前記相対座標系での前記移動体の位置および姿勢と、前記部分地図とを用いて、前記全体座標系での前記移動体の位置および姿勢を特定する、
ことを特徴とする位置姿勢検出システム。

【請求項4】

コンピュータが、距離センサで計測した測定データと地図情報とを照合することによって自身の位置を推定しながら移動する移動体の位置を検出する位置検出方法であって、
相対位置姿勢検出処理部が、所定のタイミングで繰り返し、計測された前記測定データと、前記移動体の座標系である相対座標系における原点位置を含む地図情報とを照合し、前記相対座標系での前記移動体の位置および姿勢を検出し、
部分地図作成部が、前記所定のタイミングのそれぞれにおいて、検出された前記移動体の位置および姿勢を前記地図情報にあらわした部分地図を作成し、
相対移動量算出部が、前記所定のタイミングで繰り返し得られた前記移動体の位置および姿勢の差に基づいて、前記部分地図における前記移動体の相対的な移動量を算出し、
広域位置姿勢検出部が、前記部分地図での前記移動体の位置および姿勢を、座標に関する所定のパラメータを用いて、全体地図上での前記移動体の位置および姿勢に変換し、当該変換に用いた前記所定のパラメータの補正量を算出し、
部分地図原点算出部が、前記補正量の分だけ、前記相対座標系における原点位置を、前記全体地図の座標系である全体座標系における原点位置に変換し、
座標変換部が、変換された前記全体座標系における原点位置と、前記移動体の相対的な移動量とを用いて、前記相対座標系での位置および姿勢を全体座標系に変換し、
表示装置制御部が、前記全体座標系に変換された前記相対座標系での位置および姿勢と、当該位置および姿勢を検出したときの前記測定データを、前記移動体の表示部に表示する場合において、
前記相対移動量算出部が、前記全体座標系における前記移動体の相対的な移動量が所定の閾値Ｄ以上でないと判定した場合、前記広域位置姿勢検出部、前記部分地図原点算出部、前記座標変換部は処理を行わず、前記表示装置制御部が前記移動体の表示部に原点特定失敗を通知する、
ことを特徴とする位置検出方法。

【請求項5】

請求項４に記載の位置検出方法において、
前記相対位置姿勢検出処理部および前記部分地図作成部が、それぞれ、ＳＬＡＭ処理により、前記相対座標系での前記移動体の位置および姿勢を検出し、前記部分地図を作成する、
ことを特徴とする位置検出方法。

【請求項6】

請求項４に記載の位置検出方法において、
前記広域位置姿勢検出部が、前記相対座標系での位置および姿勢を、前記全体座標系での前記移動体の位置および姿勢に一意に変換できない場合、過去の所定のタイミングで得られた、前記相対座標系での前記移動体の位置および姿勢と、前記部分地図とを用いて、前記全体座標系での前記移動体の位置および姿勢を特定する、
ことを特徴とする位置姿勢検出方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、工場内で稼働するフォークリフト等の移動体の位置および姿勢を検出するための位置検出システム、位置検出方法に関する。

【背景技術】

【0002】

近年、工場内の物流の増加に伴い、フォークリフト等にレーザスキャナ等の距離センサを搭載し、SLAM（Simultaneous Localization And Mapping）技術を適用することで位置および姿勢を検出するための技術が開発されている。本技術分野における先行技術文献として特許文献１がある。特許文献１では、カメラを使用したいわゆるVisual SLAMを用いて説明している。カメラで撮像した画像データを、ネットワークを介してサーバに転送し、サーバ上で処理負荷の高い位置推定処理を実行する技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０２１－５６６２９号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、特許文献１では通信回線を介して位置推定を実行するため、通信の遅れ等によってリアルタイムに位置を知る必要がある有人フォークリフト向けのサービスへの適用が難しい。例えば、特許文献１では、フォークリフトの初期位置同定や原点位置の計算をサーバ側で行っているため、その結果をフォークリフトに送信し、フォークリフトに搭載されたディスプレイ等の表示装置に表示するまでに時間がかかり、操縦者が、自ら操縦するフォークリフトの位置をリアルタイムに確認することが難しい。

【0005】

本発明は、操縦者が、自ら操縦するフォークリフトの位置をリアルタイムに確認することが可能な位置検出システム、位置検出方法を提供することを目的とする。

【課題を解決するための手段】

【0006】

本発明にかかる位置検出システムは、距離センサで計測した測定データと地図情報とを照合することによって自身の位置を推定しながら移動する移動体の位置を検出する位置検出システムであって、所定のタイミングで繰り返し、計測された前記測定データと、前記移動体の座標系である相対座標系における原点位置を含む地図情報とを照合し、前記相対座標系での前記移動体の位置および姿勢を検出する相対位置姿勢検出処理部と、前記所定のタイミングのそれぞれにおいて、検出された前記移動体の位置および姿勢を前記地図情報にあらわした部分地図を作成する部分地図作成部と、前記所定のタイミングで繰り返し得られた前記移動体の位置および姿勢の差に基づいて、前記部分地図における前記移動体の相対的な移動量を算出する相対移動量算出部と、前記部分地図での前記移動体の位置および姿勢を、座標に関する所定のパラメータを用いて、全体地図上での前記移動体の位置および姿勢に変換し、当該変換に用いた前記所定のパラメータの補正量を算出する広域位置姿勢検出部と、前記補正量の分だけ、前記相対座標系における原点位置を、前記全体地図の座標系である全体座標系における原点位置に変換する部分地図原点算出部と、変換された前記全体座標系における原点位置と、前記移動体の相対的な移動量とを用いて、前記相対座標系での位置および姿勢を全体座標系に変換する座標変換部と、前記全体座標系に変換された前記相対座標系での位置および姿勢と、当該位置および姿勢を検出したときの前記測定データを、前記移動体の表示部に表示する表示装置制御部と、を有することを特徴とする位置検出システムとして構成される。

【発明の効果】

【0007】

本発明によれば、操縦者が、自ら操縦するフォークリフトの位置をリアルタイムに確認することができる。

【図面の簡単な説明】

【0008】

【図1】システム構成例

【図2】サーバ装置を実現するための構成例

【図3】エッジ位置検出装置を実現するための構成例

【図4】移動体が環境を移動しながら距離センサで測定している例

【図5】サーバ側で広域探索を実行した際に、解が複数ある例

【図6】エッジ位置検出装置で部分地図用のデータ測定を実施している例

【図7】測定したデータから部分地図を作成する例

【図8】サーバで広域探索を実行した際に解が複数ある例

【図9A】複数ある解に対して過去のデータのマッチングを行った例

【図9B】複数ある解に対して過去のデータのマッチングを行った例

【図10】過去のデータを利用することで正解を特定できた例

【図11】エッジ位置検出装置上での処理フローチャート

【図12】サーバ装置上での処理フローチャート

【図13A】データ構成例

【図13B】データ構成例

【図14】コンピュータ概略図

【発明を実施するための形態】

【0009】

以下、図面を参照して本発明の実施形態を説明する。以下の記載および図面は、本発明を説明するための例示であって、説明の明確化のため、適宜、省略および簡略化がなされている。本発明は、他の種々の形態でも実施する事が可能である。特に限定しない限り、各構成要素は単数でも複数でも構わない。

【0010】

図面において示す各構成要素の位置、大きさ、形状、範囲などは、発明の理解を容易にするため、実際の位置、大きさ、形状、範囲などを表していない場合がある。このため、本発明は、必ずしも、図面に開示された位置、大きさ、形状、範囲などに限定されない。

【0011】

以下の説明では、「データベース」、「テーブル」、「リスト」等の表現にて各種情報を説明することがあるが、各種情報は、これら以外のデータ構造で表現されていてもよい。データ構造に依存しないことを示すために「ＸＸテーブル」、「ＸＸリスト」等を「ＸＸ情報」と呼ぶことがある。識別情報について説明する際に、「識別情報」、「識別子」、「名」、「ＩＤ」、「番号」等の表現を用いた場合、これらについてはお互いに置換が可能である。

【0012】

同一あるいは同様な機能を有する構成要素が複数ある場合には、同一の符号に異なる添字を付して説明する場合がある。ただし、これらの複数の構成要素を区別する必要がない場合には、添字を省略して説明する場合がある。

【0013】

また、以下の説明では、プログラムを実行して行う処理を説明する場合があるが、プログラムは、プロセッサ（例えばＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ））によって実行されることで、定められた処理を、適宜に記憶資源（例えばメモリ）および／またはインターフェースデバイス（例えば通信ポート）等を用いながら行うため、処理の主体がプロセッサとされてもよい。同様に、プログラムを実行して行う処理の主体が、プロセッサを有するコントローラ、装置、システム、計算機、ノードであってもよい。プログラムを実行して行う処理の主体は、演算部や演算装置であれば良く、特定の処理を行う専用回路（例えばＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、ＣＰＬＤ（Ｃｏｍｐｌｅｘ Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）やＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ））を含んでいてもよい。

【0014】

プログラムは、プログラムソースから計算機のような装置にインストールされてもよい。プログラムソースは、例えば、プログラム配布サーバまたは計算機が読み取り可能な記憶メディアであってもよい。プログラムソースがプログラム配布サーバの場合、プログラム配布サーバはプロセッサと配布対象のプログラムを記憶する記憶資源を含み、プログラム配布サーバのプロセッサが配布対象のプログラムを他の計算機に配布してもよい。また、以下の説明において、２以上のプログラムが１つのプログラムとして実現されてもよいし、１つのプログラムが２以上のプログラムとして実現されてもよい。

【0015】

本実施例においては、本発明にかかる位置検出システムを、フォークリフトが工場内で荷役作業を行っている環境下に適用した場合を例示しているが、これに限らず、フォークリフトと同種の移動体について適用してよい。

【0016】

図１は、位置検出システム１０００の全体の実施例を構成するシステム構成図である。システム構成としては大きく分けてサーバ装置１０とフォークリフト１１がある。サーバ装置１０には、フォークリフト１１と通信するための通信装置１０２が搭載されており、その通信装置１０２を制御するための通信装置制御部１０５が搭載されている。フォークリフト１１との通信を行う通信装置１０２は、無線通信を行うための装置である。本実施例においては、公衆回線を用いた通信装置を想定した説明を行うが、工場内にアクセスポイントを配置して、ローカルで通信を行う無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）でも良い。

【0017】

サーバ装置１０には、広域位置姿勢検出部１０３が搭載されている。広域位置姿勢検出部１０３は、フォークリフト１１が、後述する全体地図や部分地図などの地図データ上において、どの位置でどの方向を向いているかを検出するためにフォークリフト１１から送信された距離センサ１１３の測定データと上記地図データを広い範囲で照合するための機能ブロックである。

【0018】

また、サーバ装置１０には、部分地図原点算出部１０４が搭載されている。これはエッジ位置検出装置１１１で検出する、部分地図の座標系である相対座標系での位置および姿勢の検出結果を、全体地図の座標系である全体座標系に変換するための補正量を定めるパラメータを算出するための機能ブロックである。相対座標系は、複数存在するそれぞれのエッジ検出装置１１１（あるいはフォークリフト１１）での座標系であり、全体座標系は、複数存在するすべてのエッジ検出装置１１１（あるいはフォークリフト１１）に共通する座標系であるといえる。

【0019】

サーバ装置１０に搭載されている運行管理部１０１は、上記位置および姿勢の検出結果からフォークリフト１１の移動距離等を算出する機能が備わっている機能ブロックである。システムの管理者等が移動距離等を参照することで、メンテナンスの時期等を正確に知ることが可能となる。

【0020】

フォークリフト１１には、表示装置１１８、通信装置１１２、距離センサ１１３、エッジ位置検出装置１１１が搭載されている。表示装置１１８は、フォークリフト１１の位置および姿勢、全体地図を表示し、運転手が現在位置を確認するために用いる。通信装置１１２は、サーバ装置１０と通信する際に用いる。

【0021】

エッジ位置検出装置１１１は、サーバ装置１０と連携し、フォークリフト１１の位置および姿勢を検出するための装置であり、主に距離センサ制御部１１６、部分地図生成部１１４、通信装置制御部１１５、相対位置姿勢検出部１１７、座標変換部１１９によって構成される。

【0022】

距離センサ制御部１１６は、距離センサ１１３を制御するために用いる。距離センサ１１３は、周囲環境にある物体との距離と測定方向を計測することが可能なセンサである。本実施例においては典型的な距離センサとして、二次元のレーザスキャナを用いて説明を行う。二次元のレーザスキャナは、水平方向にレーザのパルスを照射して、乱反射したレーザ光が返ってくるまでの時間を計測することで距離を算出する。二次元のレーザスキャナは、センサ内部に備わっているモータによってレーザパルスの照射方向を回転させ、照射方向と、距離を取得することで、周囲の形状データを取得することができる。

【0023】

レーザスキャナの計測範囲は、例えば照射範囲２７０度、角度分解能（パルスの照射間隔）を０．２５度として、１０８１点の測定データを２５ｍｓｅｃ周期で取得することが可能なスキャナなどがある。ただし、これに限らず、照射範囲や角度分解能が異なっていても、複数の測定データと照射方向の組を取得できるセンサであれば適用することが可能である。また、二次元のレーザスキャナに限らず三次元で周囲の構造物との距離を計測できる三次元のレーザスキャナやステレオカメラ等、周囲にある構造物との距離を、異なる角度方向に対して複数取得できるものであれば適用することが可能である。

【0024】

次に、図２を用いてサーバ処理装置１０の具体的な実現方法について説明する。サーバ処理装置１０は、演算処理を行うためのプロセッサ２０１、プログラムを実行する際の一時記憶装置であるメモリ２０２、補助記憶装置２０８、通信Ｉ／Ｆ（インターフェース）２０９および通信Ｉ／Ｆ２０９に接続された通信装置１０２によって構成される。

【0025】

サーバ装置１０の稼働時においては、メモリ２０２上に、通信装置制御プログラム２０３、広域位置姿勢検出プログラム２０４、地図作成プログラム２０５、運行管理プログラム２１０、部分地図原点算出プログラム２１３、相対移動量算出プログラム２１５、補助記憶装置２０８に記憶された全体地図２０６をはじめとする本システムで用いられる各種データが展開される。これらのプログラムをプロセッサ２０１で処理することで、全体の機能を実現している。プロセッサ２０１はプログラムを実行するための演算装置である。

【0026】

通信制御プログラム２０３は、通信装置制御部１０５の機能を実現するためのプログラムである。通信制御プログラム２０３は、通信装置１０２を制御し、フォークリフト１１に搭載されているエッジ位置検出装置１１１と通信を行い、距離センサ１１３の測定データ等を受信しメモリ２０２に格納する。広域位置姿勢検出プログラム２０４は、広域位置姿勢検出部１０３の機能を実現するプログラムである。広域位置姿勢検出プログラム２０４は、全体地図２０６とエッジ位置検出装置１１１から送られてくる距離センサ１１３の測定データとを照合し、全体地図２０６上で距離センサ１１３が計測した位置および姿勢を特定するためのプログラムである。また、部分地図原点算出プログラム２１３は、検出した全体地図２０６上の位置および姿勢と、エッジ位置検出装置１１１から送られてきたエッジ位置検出装置１１１にある部分地図３１３上での位置および姿勢を比較することで、エッジ位置検出装置１１１で持っている相対座標系に対する補正量を算出するためのプログラムである。

【0027】

運行管理プログラム２１０は、運行管理部１０１の機能を実現するためのプログラムである。地図作成プログラム２０５は、全体地図の地図データを作成する機能を実現するためのプログラムである。エッジ位置検出装置１１１の原点情報２１２は、全体地図における、エッジ位置検出装置１１１の原点の座標情報であり、あらかじめ定められている。部分地図原点算出プログラム２１３は、部分地図原点算出部１０４の機能を実現するためのプログラムである。補助記憶装置２０８に記憶される探索初期位置２１４は、フォークリフト１１の初期位置同定前に探索されたフォークリフト１１の初期位置である。相対移動量算出プログラム２１５は、フォークリフト１１の相対的な移動量を算出するプログラムである。

【0028】

さらに、サーバ装置１０の補助記憶装置２０８は、履歴情報２１１を記憶する。履歴情報２１１は、フォークリフト１１から受信した各種データを蓄積した情報である。図２に示すように、履歴情報２１１には、位置姿勢検出結果履歴２１１ａ、距離センサデータ２１１ｂ、測定時刻履歴２１１ｃが含まれる。位置姿勢検出結果履歴２１１ａは、エッジ位置検出装置１１１が検出したフォークリフト１１の相対座標系上での位置および姿勢の検出結果である相対位置姿勢検出結果を時系列に蓄積した情報である。距離センサデータ２１１ｂは、距離センサ１１３が測定したデータを時系列に蓄積した情報である。測定時刻履歴２１１ｃは、距離センサ１１３が測定した時刻を時系列に蓄積した情報である。位置姿勢検出結果履歴２１１ａ、距離センサデータ２１１ｂ、測定時刻履歴２１１ｃは、互いに測定時刻をキーにして互いに対応付けて記憶され、１つのテーブルとして記憶されていてもよい。

【0029】

例えば、位置姿勢検出結果履歴２１１ａは、図１３Ａに示す「相対位置、姿勢」項目の内容を時系列に蓄積した情報である。また、距離センサデータ２１１ｂは、図１３Ａに示す「距離センサの測定データ」項目の内容を時系列に蓄積した情報である。また、測定時刻履歴２１１ｃは、図１３Ａに示す「測定時刻」項目の内容を時系列に蓄積した情報である。

【0030】

これらの情報に対応付けて、補助記憶装置２０８には、さらに、エッジ位置検出装置１１１から受信した測定データと全体地図２０６あるいは全体地図２０６と同様の座標系を有した表示用地図３１８をマッチングした際の一致度を示す内容、当該マッチングして一致度を算出したときの解のバラツキを示す「解の精度」項目の内容が、それぞれ時系列に蓄積されている。上記「解の精度」項目には、最も一致度が高い解以外にも、一致度を判定した際に得られた所定の閾値以上の他の解が記憶されている。

【0031】

補助記憶装置２０８は、電源が落とされている場合にも各種のデータを保存可能とするための記憶装置であり、不揮発性メモリなどによって実現される。

【0032】

図３を用いて、エッジ位置検出装置１１１の具体的な機能について説明する。エッジ位置検出装置１１１は、プロセッサ３０１、メモリ３０２、補助記憶装置３０９、通信Ｉ／Ｆ３１０を有する。通信Ｉ／Ｆに対しては、通信装置１１２、表示装置１１８、距離センサ１１３が接続されている。

【0033】

動作時において、電源が投入された後、メモリ３０２上に、距離センサ制御プログラム３０３、位置姿勢検出プログラム３０４、部分地図作成プログラム３０５、相対移動量算出プログラム３０６、表示装置制御プログラム３１１、通信装置制御プログラム３１２、補助記憶装置２０８に記憶された部分地図３１３をはじめとする本システムで用いられる各種データが展開される。

【0034】

距離センサ制御プログラム３０３は、距離センサ制御部１１６の機能を実現するためのプログラムである。距離センサ制御プログラム３０３は、通信Ｉ／Ｆを介して、距離センサ１１３を制御し、距離センサ１１３から測定データ３１４を取得し、メモリ３０２に格納する。測定データ３１４は、距離センサ１１３が計測した物体との距離と測定方向とを含むデータである。具体的には後述するが、測定データ３１４には、所定のタイミング（例えば、２５ｍｓ周期）における測定データが蓄積されている。

【0035】

位置姿勢検出プログラム３０４は、相対位置姿勢検出部１１７の機能を実現するためのプログラムである。位置姿勢検出プログラム３０４は、測定データ３１４と部分地図３１３を照合し、フォークリフト１１の位置および姿勢を検出し、その測定時刻とともに位置姿勢検出結果３０８を履歴でメモリ３０２に格納する。

【0036】

部分地図作成プログラム３０５は、部分地図作成部１１４の機能を実現するためのプログラムである。部分地図作成プログラム３０５は、部分地図と測定データとを用いて、フォークリフト１１の位置および姿勢を含む最新の部分地図を作成する。具体的な作成方法については、図７を用いて後述する。

【0037】

相対移動量算出プログラム３０６は、異なる時刻間の位置姿勢検出結果３０８からフォークリフト１１の相対的な移動量を算出し、算出した都度、その移動量を、位置姿勢検出結果３０８、測定データ３１４とともにサーバ装置１０に送信する。

【0038】

座標変換プログラム３１７は、座標変換部１１９の機能を実現するためのプログラムである。座標変換プログラム３１７は、サーバ装置１０で算出し、送信されたエッジ位置検出装置１１１の原点情報３１６を用いて、位置姿勢検出結果３０８を全体地図２０６で定義されている全体座標系に変換するためのプログラムである。これら座標系の明確な定義については後述する。

【0039】

表示装置制御プログラム３１１は、変換された位置姿勢検出結果３０８を、通信Ｉ／Ｆ３１０を介して、表示装置１１８に出力する。当該位置姿勢検出結果３０８は、補助記憶装置３０９に記憶された、表示装置１１９に表示するための全体地図２０６と同様の座標系を有した地図である表示用地図３１８と共に表示される。これにより、フォークリフト１１の操縦者は、自ら操縦するフォークリフト１１の全体地図２０６における現在位置および姿勢をリアルタイムに確認することが可能である。

【0040】

上述したサーバ１０、エッジ位置検出装置１１１は、例えば、図１４（コンピュータ概略図）に示すような、ＣＰＵ１６０１と、メモリ１６０２と、ＨＤＤ(Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ)等の外部記憶装置１６０３と、ＣＤ(Ｃｏｍｐａｃｔ Ｄｉｓｋ)やＵＳＢメモリ等の可搬性を有する記憶媒体１６０８に対して情報を読み書きする読書装置１６０７と、キーボードやマウス等の入力装置１６０６と、ディスプレイ等の出力装置１６０５と、通信ネットワークに接続するためのＮＩＣ(Ｎｅｔｗｏｒｋ Ｉｎｔｅｒｆａｃｅ Ｃａｒｄ)等の通信装置１６０４と、これらを連結するシステムバス等の内部通信線(システムバスという)１６０９と、を備えた一般的なコンピュータ１６００により実現できる。

【0041】

サーバ１０やエッジ位置検出装置１１１に記憶され、あるいは処理に用いられる様々なデータは、ＣＰＵ１６０１がメモリ１６０２または外部記憶装置１６０３から読み出して利用することにより実現可能である。また、サーバ１０やエッジ位置検出装置１１１が有する各機能部（例えば、サーバ１０の広域位置姿勢検出部１０３、部分地図原点算出部１０４等、あるいはエッジ位置検出装置１１１の部分地図生成部１１４、相対位置姿勢検出部１１７等）は、ＣＰＵ１６０１が外部記憶装置１６０３に記憶されている所定のプログラムをメモリ１６０２にロードして実行することにより実現可能である。

【0042】

上述した所定のプログラムは、読書装置１６０７を介して記憶媒体１６０８から、あるいは、通信装置１６０４を介してネットワークから、外部記憶装置１６０３に記憶(ダウンロード)され、それから、メモリ１６０２上にロードされて、ＣＰＵ１６０１により実行されるようにしてもよい。また、読書装置１６０７を介して、記憶媒体１６０８から、あるいは通信装置１６０４を介してネットワークから、メモリ１６０２上に直接ロードされ、ＣＰＵ１６０１により実行されるようにしてもよい。

【0043】

以下では、サーバ１０が、ある１つのコンピュータにより構成される場合を例示するが、これらの機能の全部または一部が、クラウドのような１または複数のコンピュータに分散して設けられ、ネットワークを介して互いに通信することにより同様の機能を実現してもよい。

【0044】

図４は、フォークリフトが移動しながら距離センサで測定している様子を表した図である。図４では、相対座標系での位置および姿勢の検出結果を、所定のパラメータにより定められた補正量だけ補正して、全体座標系での位置および姿勢に変換した後の状態をあらわしている。

【0045】

まず、始業時に、フォークリフト１１は、位置４０１において電源投入され、エッジ位置検出装置１１１が起動する。フォークリフト１１は、起動後に距離センサ１１３で周囲を計測し、測定データ４０６を取得する。一般にレーザスキャナの測定距離には限界があり、フォークリフト１１の動作範囲における一部の構造物との距離しか取得することができない。図４では、測定データ４０７として得られる上記測定距離の限界の範囲内で測定された構造物の部分を太線で表している。この時点では、フォークリフト１１は全体地図２０６上における自己の位置および姿勢の情報を保持していないため、サーバ装置１０に測定データ４０６を送信する。

【0046】

図５は、サーバ処理装置１０において、測定データ４０６を受信した際の広域位置姿勢検出処理の結果を表している。広域位置姿勢検出処理は、フォークリフト１１から受信した距離センサ１１３の測定データ４０６と全体地図２０６の形状を照合する処理である。この形状マッチングの処理に関しては距離センサ１１３の測定データ４０６と全体地図２０６を照合する処理であるＩＣＰ（Iterative Closest Point Search）アルゴリズムやＮＤＴ（Normal Distribution Transform）アルゴリズムといったスキャンマッチング等の手法を用いて行うことを想定しているが、これに限らず、互いの形状を比較し、照合位置を特定できる技術であればよく、深層学習等のAI技術を用いても良い。当該処理は、広域位置姿勢検出プログラム２０４により行われる。

【0047】

ＩＣＰアルゴリズムやＮＤＴアルゴリズムは、照合処理を行う際に初期の位置および姿勢を与える必要がある。広域位置姿勢検出プログラム２０４は、この初期の位置および姿勢としては、全体地図２０６に対して、フォークリフト１１が存在する可能性がある範囲を一定の区間となる領域ごとに区切り、初期の位置および姿勢を生成する。例えば、全体地図２０６を格子上に分割した後に、フォークリフト１１の通路の情報と重なる範囲のうち、最も早い時刻に存在する可能性のある位置を初期位置とし、当該初期位置における姿勢を初期位置の姿勢とするなどの方法で生成する。姿勢に関しては、通路の方向を考慮するなどで設定を行えば良い。

【0048】

図５において、広域位置姿勢検出処理の実行結果として、フォークリフト１１の複数の位置および姿勢５０１、５０５が得られる。１フレーム分の測定データ４０６を用いた場合、計測範囲が限定される。このため、複数個所で良く全体地図２０６（あるいは全体地図２０６と同様の座標系を有した表示用地図３１８）と測定データ４０６とが一致する箇所が現れる場合がある。図５では、１フレーム内におけるある時点でのフォークリフト１１の位置および姿勢５０１、１フレーム内における別の時点でのフォークリフト１１の位置および姿勢５０５が得られた例を示している。この場合はフォークリフト１１の位置を一意に特定することができていない。以下では、これを回避し、フォークリフト１１の位置および姿勢を特定するための方法について説明する。

【0049】

図６は、フォークリフト１１が移動しながら距離センサ１１３の測定データ４０６を計測している様子を表している。このとき、起動した時点でのフォークリフト１１の位置をエッジ位置検出装置１１１上における初期位置、すなわち、相対座標における初期位置とし、フォークリフト１１の進行方向にx軸を取ることとする。図６では、位置４０１にあるフォークリフト１１の中心位置が、相対座標における初期位置であり、図の左から右に向かってフォークリフト１１が進行することを示している。

【0050】

本実施例においては、距離センサ１１３の位置はフォークリフト１１の中心とし、距離センサ１１３の正面０度方向はフォークリフト１１の進行方向と一致しているとして説明する。しかし、距離センサ１１３の取付け位置、方向を変更したとしても、フォークリフト１１の中心、進行方向に対する取付け位置、方向の関係から、適切な座標変換を行えば上記と同様となる。

【0051】

フォークリフト１１は、起動位置である位置４０１（初期位置）から前進を始め、位置６０１、６０２へと進んで行く。この時、距離センサ１１３は２５ｍｓ周期で測定データを測定し続けている。図６では、次の周期の位置６０１で測定される測定データ６０１Ｓとして得られる測定距離の限界、さらにその次の周期の位置６０２で測定される測定データ６０２Ｓとして得られる測定距離の限界が示されている。

【0052】

図７は、エッジ位置検出装置１１１上における相対座標系上での位置および姿勢の検出である相対位置姿勢検出と、部分地図生成の方法について説明している。

【0053】

はじめに、何も記載されていない初期状態の地図情報である地図データ７０３がある。地図データ７０３には当該地図データ上における原点Ｏが設定されている。フォークリフト１１が起動した位置４０１である初期位置における距離センサ１１３の測定データ４０６は、起動時の位置および姿勢で計測したデータである。このため、部分地図作成プログラム３０５は、このデータを基準として部分地図の作成を始める。

【0054】

初期状態の地図データ７０３に対して、距離センサ１１３の測定データ４０６によるフォークリフト１１の位置および姿勢の照合処理を行ったとしても、照合先となるフォークリフト１１の位置および姿勢が存在しないことにより、一致する位置および姿勢が無い。このため、位置姿勢検出プログラム３０４は、この場合は、位置４０１における位置および姿勢（0,0,0）を基準として、距離センサ１１３の測定データ４０６を取得する位置姿勢検出処理７０１を実行する。さらに、部分地図作成プログラム３０５は、位置姿勢検出処理７０１が取得した測定データ４０６をその初期状態の地図データ７０３に書き込む部分地図作成処理７０２を実行し、その結果を部分地図である地図データ７０４とする。

【0055】

次に、位置姿勢検出プログラム３０４は、地図データ７０４を作成したときの測定データ４０６から得られた位置および姿勢を、図６に示した位置６０１における測定データ６０１Ｓから得られる位置および姿勢に対する前回位置および姿勢としてスキャンマッチングの手法で照合する。これにより、位置姿勢検出プログラム３０４は、フォークリフト１１が起動した位置４０１に対する相対的な移動量を算出することができ、部分地図作成プログラム３０５は、相対的に移動した後のフォークリフト１１の位置および姿勢を含む新たな部分地図である地図データ７０５を作成することができる。同様に、次の周期の位置６０２では、位置姿勢検出プログラム３０４は、位置６０１における測定データ６０１Ｓから得られる位置および姿勢を、位置６０２における測定データ６０２Ｓから得られる位置および姿勢に対する前回位置および姿勢としてスキャンマッチングの手法で照合する。これにより、位置姿勢検出プログラム３０４は、フォークリフト１１が起動して進んだ後の位置６０１に対する相対的な移動量を算出することができ、部分地図作成プログラム３０５は、相対的に移動した後のフォークリフト１１の位置および姿勢を含む新たな部分地図である地図データ７０６を作成することができる。

【0056】

位置姿勢検出プログラム３０４および部分地図作成プログラム３０５は、これを繰り返すことによって、フォークリフト１１が起動した位置および姿勢から、現在に至るまでのフォークリフト１１の相対的な移動量を算出し、当該移動量だけ移動した後のフォークリフト１１の位置および姿勢を含む新たな部分地図を作成することができる。通信制御部１１５は、この相対的な移動量と距離センサ１１３の測定データをサーバ装置１０に送信する。また、通信制御部１１５は、相対的な移動量を算出したときの部分地図、すなわち初期位置から直近位置までの部分地図をサーバ装置１０に送信する。

【0057】

図８は、サーバ装置１０上に送信されてきたフォークリフト１１の位置および姿勢４０４における測定データ４０８（図４）を全体地図２０６（あるいは全体地図２０６と同様の座標系を有した表示用地図３１８。以下同様。）と照合した結果を示す図である。図５を用いて説明したように、１フレーム分の測定データ４０８では、図８に示すように、位置および姿勢を検出した測定データ８０１ｂ、８０１ｃ、８０１ｅが良く一致している複数の位置および姿勢として抽出されたとする。この状態においては、図５と同様に、全体地図２０６上での位置および姿勢を特定できない。したがって、広域位置姿勢検出プログラム２０４は、過去の周期で送信されたデータを利用して、広域での位置姿勢検出処理を行う。

【0058】

図９Ａ、９Ｂでは、照合の結果得られた、上記位置および姿勢を検出した測定データ８０１ｂに対して過去の周期でエッジ位置検出装置１１１から送られてきた位置および姿勢の履歴データ（履歴情報２１１として蓄積されている）を当てはめた結果（図９Ａ）と、上記位置および姿勢を検出した測定データ８０１ｅに対して過去の周期でエッジ位置検出装置１１１から送られてきた位置および姿勢の履歴データを当てはめた結果（図９Ｂ）について示している。図９Ａでは、直近の周期でエッジ位置検出装置１１１から受信した相対座標系上での位置および姿勢を検出した測定データ８０１ｂは、全体地図２０６や表示用地図３１８の形状とよく一致している。しかし、他の過去の周期でエッジ位置検出装置１１１から受信した相対座標系上での位置および姿勢を検出した測定データ（履歴データ８０１ｂ’、８０１ｂ”）は、全体地図２０６や表示用地図３１８の形状と一致していない範囲Ｒの箇所が一定数以上存在する。同様に、図９Ｂでは、直近の周期でエッジ位置検出装置１１１から受信した相対座標系上での位置および姿勢を検出した測定データ８０１ｅは、全体地図２０６や表示用地図３１８の形状とよく一致している一方、履歴データ８０１ｅ’、８０１ｅ”は、全体地図２０６や表示用地図３１８の形状と一致していない範囲Ｒの箇所が一定数以上存在する。図９Ａ、９Ｂでは、いずれも上記一致していない範囲Ｒが４箇所存在していることがわかる。

【0059】

このように、本システムでは、エッジ位置検出装置１１１から受信した、測定データを全体地図に当てはめることにより、当該エッジ位置検出装置１１１を搭載したフォークリフト１１の全体地図における位置および姿勢を推定する。

【0060】

図１０は、照合結果の候補８０１ｃに対して過去の履歴を当てはめた結果であり、図９Ａ、９Ｂに示したような全体地図２０６や表示用地図３１８の形状と一致していない範囲Ｒの箇所がなく、正解の位置および姿勢に対してはすべて良く一致していることがわかる。このような照合結果が、フォークリフト１１の表示装置１１８に表示される。

【0061】

図１０では、直近の周期でエッジ位置検出装置１１１から受信した相対座標系上での位置および姿勢を検出した測定データ８０１ｃ、過去の周期でエッジ位置検出装置１１１から受信した相対座標系上での位置および姿勢を検出した他の測定データ（履歴データ８０１ｃ’、８０１ｃ”）のいずれもが、全体地図２０６や表示用地図３１８の形状とよく一致している。このように、広域位置姿勢検出プログラム２０４は、直近の周期でエッジ位置検出装置１１１から受信した相対座標系上での位置および姿勢を検出した測定データだけでなく、過去の周期でエッジ位置検出装置１１１から受信した上記測定データも含めて、全体地図２０６と照合することで、初期位置同定を正確に行うことができる。当該初期位置同定は、エッジ位置検出装置１１１から送信された、初期位置から直近位置までの部分地図と、図７を用いて説明した相対的な移動量を用いて行うことができる。測定データを全体地図に当てはめる際の座標変換については後述する。

【0062】

次に、図１１のフローチャートを用いて、エッジ位置検出装置１１１上での処理の詳細を説明する。

【0063】

エッジ位置検出装置１１１の距離センサ制御プログラム３０３は、距離センサ１１３から測定データを取得する（１１０１）。

【0064】

エッジ位置検出装置１１１では、位置姿勢検出プログラム３０４が、取得された測定データと、現在の部分地図を示す地図データ、１フレーム前の位置および姿勢を初期値とした部分地図を示す地図データを用いて、図７で説明したように、エッジ位置検出装置１１１での位置姿勢検出処理を行う（１１０２）。

【0065】

次に、エッジ位置検出装置１１１は、ステップ１１０２の位置姿勢検出処理で得られた位置および姿勢の検出の結果に従って、図７で説明したように、エッジ位置検出装置１１１に記憶されている部分地図３１３を更新する（１１０３）。ステップ１１０２および１１０３で行われる処理は、例えば、ＳＬＡＭ（Simultaneous Localization And Mapping）技術を用いればよい。

【0066】

次に、エッジ位置検出装置１１１の通信装置制御プログラム３１２は、サーバ装置１０に、図１３Ａに示したような測定時刻、距離センサ１１３の測定データ、相対座標系での位置および姿勢から算出された相対的な移動量、解の精度をサーバ装置１０に送信する（１１０５）。

【0067】

エッジ位置検出装置１１１の座標変換プログラム３１７は、サーバ装置１０から、すでに広域位置姿勢検出処理が実行されて、成功している旨の通知を受けたか否かを判定する（１１０９）。すでに成功している旨の通知を受けた場合は（１１０９；Ｙｅｓ）、座標変換プログラム３１７は、サーバ装置１０からすでに送信されてきている原点情報２１２を用いて、相対座標系での位置および姿勢を、全体座標系に変換し（１１０６）、表示装置制御プログラム３１１は、全体座標系に変換された位置および姿勢を検出した測定データを、エッジ位置検出装置１１１の表示装置１１８に表示する（１１０７）。

【0068】

一方、サーバ装置１０から、すでに成功している旨の通知を受けていない場合は（１１０９；Ｎｏ）、エッジ位置検出装置１１１の座標変換プログラム３１７は、初期の位置および姿勢の探索が未完であること表示装置１１８に表示する（１１０８）。

【0069】

次に、図１２のフローチャートを用いて、サーバ装置１０上での処理の詳細を説明する。エッジ位置検出装置１１１から相対座標系での位置および姿勢、距離センサ１１３の測定データ、測定時刻、エッジ位置検出装置１１１での検出結果とその精度を受信する（１２０１）。

【0070】

図１３Ａは、エッジ位置検出装置１１１がサーバ装置１０に送信するデータの一例を示す図である。図１３Ａに示すように、エッジ位置検出装置１１１から送信されるデータは、「測定時刻」項目およびその内容（図１３Ａでは「yyyymmddmmssmmm」）、「相対位置、姿勢」項目およびその内容（図１３Ａでは「ｘ＿ｌ[mm]、ｙ＿ｌ[mm]、θ＿ｌ[mm]」）、「距離センサの測定データ」項目およびその内容（図１３Ａでは「d1,d2,…,d1081」）が互いに対応付けて記憶されている。さらに上記データには、これらの情報に対応付けて、位置および姿勢を検出した測定データと全体地図２０６や表示用地図３１８とが一致した割合を示す「一致度」項目とその内容（図１３Ａでは「R[%]」）、「一致度」が所定の閾値（例えば、９０パーセント以上）を満たしたときの測定データの精度である「解の精度」項目とその内容（図１３Ａでは現在の周期で得られた測定データの精度「ｃ＿１１、ｃ＿１２、ｃ＿１３」、および過去の２回の周期で得られた測定データの精度「ｃ＿２１、ｃ＿２２、ｃ＿２３」、「ｃ＿３１、ｃ＿３２、ｃ＿３３」）が含まれる。これらの３つは、例えば、図１０に示した測定データ８０１ｃ、履歴データ８０１ｃ’、８０１ｃ”が得られたときの精度である。

【0071】

相対移動量算出プログラム２１５は、ステップ１２０１で受信したデータと、サーバ装置１０に蓄積されている位置姿勢検出結果履歴２１１aを比較し、フォークリフト１１の全体座標系における相対的な移動量を算出する（１２１２）。全体座標系における相対的な移動量の算出は、相対座標系から全体座標系への座標変換を行ってから、相対座標系における相対的な移動量を算出した場合と同様に、異なる時刻間の位置姿勢検出結果から算出することができる（図９、１０）。

【0072】

相対移動量算出プログラム２１５は、算出された相対的な移動量が閾値Ｄ以上の移動量であるか否かを判定する（１２１３）。閾値Ｄは、前回のタイミングでフォークリフト１１から受信したときの上記相対的な移動量に対してどの程度変化したかを示す値である。つまり、前回の探索に用いた上記相対的な移動量が閾値Ｄ以上でないと判定された場合は（１２１３；Ｎｏ）、フォークリフト１１が移動していないことによる判定結果の変化も得られない可能性が高いため、以降の処理を行わず、エッジ位置検出装置１１１に原点特定失敗を通知する（１２１１）。

【0073】

ステップ１２１３において、フォークリフトが閾値Ｄ以上移動していると判定された場合（１２１３；Ｙｅｓ）、まず、相対移動量算出プログラム２１５は、受信データを履歴情報２１１に保存する（１２１４）。次に、広域位置姿勢検出プログラム２０４は、１ステップ１２０１で受信した直近の測定時刻、相対的な移動量、解の精度から探索範囲および探索初期値{(xi_1,yi_1,θi_1),…, (xi_N,yi_N,θi_N)}を決定する（１２０２）。この処理において、過去に全体地図における位置および姿勢の探索が成功している場合は、探索領域として全体地図の全体を探索することはなく、全体地図のなかでも現在位置周辺を探索することで演算量を削減することができるためである。

【0074】

広域位置姿勢検出プログラム２０４は、設定された探索初期値{(xi_1,yi_1,θi_1),…, (xi_N,yi_N,θi_N)}を初期位置として、位置姿勢検出処理を実行する（１２０３）。広域位置姿勢検出プログラム２０４は、それぞれの探索初期値に対する位置姿勢検出結果の評価を行い、位置および姿勢の検出が成功しているものの数Ｓを算出する（１２０４）。

【0075】

次に、広域位置姿勢検出プログラム２０４は、Ｓの値を評価し、条件分岐処理を実行する（１２０５）。

【0076】

Ｓ＝０である場合、初期位置および姿勢の探索に成功しているものが一つもないため、広域位置姿勢検出プログラム２０４は、エッジ位置検出装置１１１に原点特定失敗の通知を行う。

【0077】

Ｓ＝１である場合、位置および姿勢の検出が成功しているものが唯一であるから、広域位置姿勢検出プログラム２０４は成功とみなし、全体地図上での原点位置、当該原点位置を基準としたエッジ位置検出装置１１１の位置および姿勢を算出する（１２０７）。具体的には、広域位置姿勢検出プログラム２０４は、全体座標系上での位置および姿勢を｛（x_t_g,y_t_g,θ_t_g）｝、エッジ位置検出装置１１１での相対座標系での位置および姿勢を（x_t_l,y_t_l,θ_t_l）として、（数１）（数２）を用いて、相対座標系における原点情報（x_t_ｃ,y_t_ｃ,θ_t_ｃ）を算出する。なお、添え字「ｇ」は全体座標系、添え字「ｌ」は相対座標系を表している。最後に、広域位置姿勢検出プログラム２０４は、エッジ位置検出装置１１１の全体座標系における原点情報２１２をエッジ位置検出装置１１１に送信する。図１３Ｂは、サーバ装置１０からエッジ位置検出装置１１１に送信されるデータの例を示す図である。図１３Ｂに示すように、当該データには、図１３Ａに示した測定データに対応する測定時刻「yyyymmddmmssmmm」、相対座標系における原点情報「ｘ＿ｃ、ｙ＿ｃ、θ＿ｃ」が対応付けられている。

【0078】

（数１）θ＿ｃ＝θ＿ｇ―θ＿ｌ、
（数２）ｘ＿ｃ＝ｘ＿ｇ―ｘ＿ｌ×cosθ＿ｃ＋ｙ＿ｌ×sinθ＿ｃ、
ｙ＿ｃ＝ｙ＿ｇ―ｘ＿ｌ×sinθ＿ｃ―ｙ＿ｌ×cosθ＿ｃ

【0079】

ここで、（数１）（数２）にある変数の添え字として、時刻のインデックスｔを省略している。上記全体座標系上での位置および姿勢｛（x_t_g,y_t_g,θ_t_g）｝およびエッジ位置検出装置１１１の相対座標系での位置および姿勢（x_t_l,y_t_l,θ_t_l）の座標が、部分地図の座標系である相対座標系から、全体地図の座標系である全体座標系に変換するためのパラメータであり、これらのパラメータに上記数１、数２を適用することで、エッジ位置検出装置１１１の相対座標系での位置および姿勢から全体座標系上での位置および姿勢への補正量を求めることができる。さらに、相対座標系の原点に対して、当該補正量と同じ補正量で原点位置を補正することで、全体座標系における原点位置を得ることができる。そして、補正した原点位置を表示用地図３１８に適用し、全体座標系でのエッジ位置検出装置１１１の位置および姿勢を表示することができる。また、このような表示を繰り返し所定の周期で行うことで、常にフォークリフト１１の全体座標系における位置や姿勢を正しく表示することができるようになる。

【0080】

Ｓ＞１である場合、位置および姿勢の検出が成功しているものが複数あるので、これらを特定するために、広域位置姿勢検出プログラム２０４は、履歴情報２１１として蓄積されている履歴データを参照し、解を特定するための処理を実行する。

【0081】

具体的には、広域位置姿勢検出プログラム２０４は、時刻tおける距離センサ１１３の測定データに対して、成功と判定されたＭ個の位置姿勢検出結果を｛（x_t_１_g,y_t_１_g,θ_t_１_g）, …,（x_t_m_g,y_t_m_g,θ_t_m_g）,…,（x_t_M_g,y_t_M_g,θ_t_M_g）｝とする。広域位置姿勢検出プログラム２０４は、これらの位置および姿勢を基準として、過去の履歴データから、位置姿勢検出処理を実行するための初期値I_m＝{（x_t-1_m_g,y_t-1_m_g,θ_t-1_m_g）,…,（x_t-L_m_g,y_t-L_m_g,θ_t-L_m_g）}を算出する。これらの値は下記の（数３）（数４）の座標変換によって算出することが可能である。

【0082】

（数３）θ＿ｇ＝θ＿ｌ＋θ＿ｃ、
（数４）ｘ＿ｇ＝ｘ＿ｌ×cosθ＿ｃ－ｙ＿ｌ×sinθ＿ｃ＋ｘ＿ｃ、
ｙ＿ｇ＝ｘ＿ｌ×sinθ＿ｃ＋ｙ＿ｌ×cosθ＿ｃ＋ｙ＿ｃ
ここで（数３）（数４）にある変数の添え字として、時刻ｔ、初期値ｍを省略している。

【0083】

広域位置姿勢検出プログラム２０４は、I_1,…,I_m,…,I_Mに対して位置姿勢検出処理を行った結果、I_mの初期探索位置で成功した数R_mを算出し、R_mが閾値以上であるものの数Ｔを算出する。

【0084】

Ｔ＝１である場合、広域位置姿勢検出成功として、処理ブロック１２０７の処理を実行し、算出された原点の情報をエッジ位置検出装置１１１に送信する。Ｔ≠１である場合はエッジ位置検出装置１１１に広域位置姿勢探索の失敗を送信する。

【0085】

以上のように位置姿勢検出処理を、サーバ装置１０とエッジ位置検出装置１１１に分割することで、フォークリフト側で遅れの無い位置姿勢検出結果を表示装置１１８に表示することが可能となる。

【0086】

このように、本実施例では、距離センサで計測した測定データと地図情報とを照合することによって自身の位置を推定しながら移動する移動体（例えば、フォークリフト１１）の位置を検出する位置検出システム１０００において、所定のタイミングで繰り返し（例えば、２５ｍｓｅｃ周期で）、計測された上記測定データと、上記移動体の座標系である相対座標系における原点位置（例えば、図８に示した原点位置Ｏ）を含む地図情報（例えば、図８に示した地図データ７０３）とを照合し、上記相対座標系での上記移動体の位置および姿勢を検出する相対位置姿勢検出処理部（例えば、位置姿勢検出プログラム３０４）と、上記所定のタイミングのそれぞれにおいて、検出された上記移動体の位置および姿勢を上記地図情報にあらわした部分地図（例えば、図８に示した地図データ７０４）を作成する部分地図作成部（例えば、部分地図作成プログラム３０５）と、上記所定のタイミングで繰り返し得られた上記移動体の位置および姿勢の差に基づいて、上記部分地図における上記移動体の相対的な移動量を算出する相対移動量算出部（例えば、相対移動量算出プログラム２１５）と、上記部分地図での上記移動体の位置および姿勢を、座標に関する所定のパラメータ（例えば、相対座標系での位置および姿勢を示す座標）を用いて、全体地図上での上記移動体の位置および姿勢（例えば、全体座標系での位置および姿勢を示す座標）に変換し、当該変換に用いた上記所定のパラメータの補正量を算出する広域位置姿勢検出部（例えば、広域位置姿勢検出プログラム２０４）と、上記補正量の分だけ、上記相対座標系における原点位置を、上記全体地図の座標系である全体座標系における原点位置に変換する部分地図原点算出部（例えば、部分地図原点算出プログラム２１３）と、変換された上記全体座標系における原点位置と、上記移動体の相対的な移動量とを用いて、上記相対座標系での位置および姿勢を全体座標系に変換する座標変換部（例えば、座標変換プログラム３１７）と、上記全体座標系に変換された上記相対座標系での位置および姿勢と、当該位置および姿勢を検出したときの上記測定データ（例えば、図１０に示したような過去の履歴を当てはめた照合結果）を、上記移動体の表示部に表示する表示装置制御部（例えば、表示装置制御プログラム３１１）と、を有する。

【0087】

したがって、操縦者が、自ら操縦するフォークリフトの全体地図上での位置をリアルタイムに確認することができる。また、フォークリフト等の屋内外を動作する移動体の位置および姿勢を検出するためのシステムを低コストに提供することができる。

【0088】

また、上記位置検出システムにおいて、上記相対位置姿勢検出処理部および上記部分地図作成部は、それぞれ、ＳＬＡＭ処理により、上記相対座標系での上記移動体の位置および姿勢を検出し、上記部分地図を作成する。したがって、移動体が未知の環境下で地図データを作成しつつ、当該地図データを用いて障害物などを回避して特定のタスクを行うことができる。

【0089】

また、上記位置検出システムにおいて、上記広域位置姿勢検出部は、上記相対座標系での位置および姿勢を、上記全体座標系での上記移動体の位置および姿勢に一意に変換できない場合（例えば、図５や図８の場合）、図９Ａ、９Ｂ、１０で説明したように、過去の所定のタイミングで得られた、上記相対座標系での上記移動体の位置および姿勢と、上記部分地図とを用いて、上記全体座標系での上記移動体の位置および姿勢を特定する。したがって、全体座標系での移動体の位置を正確に特定することができる。

【0090】

また、上記位置検出システムにおいて、上記相対移動量算出部が、上記移動体の相対的な移動量が所定の閾値Ｄ以上でないと判定した場合、例えば、図１２の１２１３から１２１１で説明したように、上記広域位置姿勢検出部、上記部分地図原点算出部、上記座標変換部は処理を行わず、上記表示装置制御部が上記移動体の表示部に原点特定失敗を通知する。したがって、例えば、フォークリフト１１が移動していないことによる判定結果の変化が得られない可能性が高い場合には、そのような無駄な処理を行うことがなくなり、処理負荷を低減することができる。

【符号の説明】

【0091】

１０：サーバ装置、１０２：通信装置、１０５：通信制御部、１０３：広域位置姿勢検出部、１０４：部分地図原点算出部、１１：移動体、１１１：エッジ位置検出装置、１１３：距離センサ、１１２：通信装置、１１４：部分地図生成部、１１６：距離センサ制御部、１１５：通信制御部、１１７：位置姿勢検出部、２０１：プロセッサ（ＣＰＵ）、２０２：メモリ（主記憶装置）、２０８：補助記憶装置、２０９：インターフェース部（Ｉ／Ｆ）、２０６：全体地図、２０４：広域位置姿勢検出プログラム、２１３：部分地図原点算出プログラム、２１５：相対移動量算出プログラム、３０１：プロセッサ（ＣＰＵ）、３０２：メモリ（主記憶装置）、３０９：補助記憶装置、３１０：インターフェース部（Ｉ／Ｆ）、３０５：部分地図作成プログラム、３１７：座標変換プログラム、３０４：位置姿勢検出プログラム、３１１：表示装置制御プログラム。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9A】

【図9B】

【図10】

【図11】

【図12】

【図13A】

【図13B】

【図14】