知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024048605
(43)【公開日】2024-04-09
(54)【発明の名称】地図利用装置、方法及びプログラム並びに移動制御方法
(51)【国際特許分類】
G01C 21/30 20060101AFI20240402BHJP
G05D 1/43 20240101ALI20240402BHJP
G08G 1/00 20060101ALI20240402BHJP
【ＦＩ】
G01C21/30
G05D1/02 K
G08G1/00 X
【審査請求】未請求
【請求項の数】11
【出願形態】ＯＬ
(21)【出願番号】P 2022154612
(22)【出願日】2022-09-28
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．ＪＡＶＡＳＣＲＩＰＴ
(71)【出願人】
【識別番号】000208891
【氏名又は名称】ＫＤＤＩ株式会社
(74)【代理人】
【識別番号】100092772
【弁理士】
【氏名又は名称】阪本 清孝
(74)【代理人】
【識別番号】100119688
【弁理士】
【氏名又は名称】田邉 壽二
(72)【発明者】
【氏名】高橋 裕章
(72)【発明者】
【氏名】稗圃 泰彦
(72)【発明者】
【氏名】後藤 崇行
(72)【発明者】
【氏名】神谷 尚保
【テーマコード（参考）】
2F129
5H181
5H301
【Ｆターム（参考）】
2F129AA01
2F129BB33
2F129BB49
2F129BB56
2F129CC15
2F129CC16
2F129CC17
2F129DD13
2F129DD14
2F129DD15
2F129DD62
2F129EE25
2F129EE26
2F129EE78
2F129EE79
2F129EE80
2F129FF15
2F129FF32
2F129FF63
2F129FF64
2F129FF65
2F129FF73
2F129HH04
2F129HH12
5H181AA27
5H181BB04
5H181CC04
5H181CC12
5H181CC14
5H181FF04
5H181FF07
5H181FF10
5H181FF13
5H181FF14
5H181FF17
5H181FF23
5H181FF27
5H181FF32
5H181FF35
5H181LL01
5H181LL02
5H181LL09
5H301AA01
5H301AA10
5H301CC03
5H301CC06
5H301CC10
5H301EE31
5H301GG09
5H301HH01
5H301KK02
5H301KK08
(57)【要約】
【課題】移動体の事前走行を必須とせずに、画像に基づく自己位置推定精度が高い経路を設定することができる地図利用装置を提供する。
【解決手段】フィールドの３次元地図から、移動体が移動しうる範囲について２次元地図を作成する第１処理31と、前記２次元地図において、出発地から目的地までの経路を、前記移動体がカメラ撮影する画像を前記３次元地図と照合することによって前記移動体の自己位置推定を行う際の精度が高いと判定される複数のウェイポイントを辿るものとして設定する第２処理32と、を実行する。
【選択図】図２
【解決手段】フィールドの３次元地図から、移動体が移動しうる範囲について２次元地図を作成する第１処理31と、前記２次元地図において、出発地から目的地までの経路を、前記移動体がカメラ撮影する画像を前記３次元地図と照合することによって前記移動体の自己位置推定を行う際の精度が高いと判定される複数のウェイポイントを辿るものとして設定する第２処理32と、を実行する。
【選択図】図２
【特許請求の範囲】
【請求項1】
フィールドの３次元地図から、移動体が移動しうる範囲について２次元地図を作成する第１処理と、
前記２次元地図において、出発地から目的地までの経路を、前記移動体がカメラ撮影する画像を前記３次元地図と照合することによって前記移動体の自己位置推定を行う際の精度が高いと判定される複数のウェイポイントを辿るものとして設定する第２処理と、を実行することを特徴とする地図利用装置。
前記２次元地図において、出発地から目的地までの経路を、前記移動体がカメラ撮影する画像を前記３次元地図と照合することによって前記移動体の自己位置推定を行う際の精度が高いと判定される複数のウェイポイントを辿るものとして設定する第２処理と、を実行することを特徴とする地図利用装置。
【請求項2】
前記第１処理では、前記３次元地図より、前記移動体の移動面の高さから前記移動体の高さまでの範囲を抽出して、当該範囲内において前記移動体が移動可能か移動不可能かの区別を与えることで、前記２次元地図を作成することを特徴とする請求項１に記載の地図利用装置。
【請求項3】
前記第１処理では、前記２次元地図の領域を分割した単位セルの各々について、前記移動体が移動可能であるかの区別の情報と、移動不可能である場合には物体が存在する信頼度の情報の情報を与えることで、前記２次元地図を作成することを特徴とする請求項１に記載の地図利用装置。
【請求項4】
前記第２処理では、前記２次元地図の単位セルの各々のうち、前記移動可能として区別されているものについて、当該単位セルの位置において、カメラ方向ごとに、前記移動体のカメラ視野範囲内に含まれる単位セルの、前記信頼度の情報を合算することで、前記自己位置推定の精度が高いと判定される単位セル及びカメラ方向を定めることを特徴とする請求項３に記載の地図利用装置。
【請求項5】
前記第２処理では、最適経路問題を解くことにより、前記２次元地図において移動可能な領域を辿ることで前記出発地から前記目的地まで到達する最適経路を定めたうえで、当該最適経路上の点に位置が近いと判定される、前記自己推定精度が高いと判定される単位セルを、前記最適経路の進行に沿った順番に並べたうえで、前記ウェイポイントとして定めることを特徴とする請求項４に記載の地図利用装置。
【請求項6】
前記第２処理ではさらに、前記最適経路上の点に位置が近く、且つ、当該点における前記最適経路の方向にカメラ方向が近いと判定される、前記自己推定精度が高いと判定される単位セル及びカメラ方向を、前記最適経路の進行に沿った順番に並べたうえで、前記ウェイポイントとして定めることを特徴とする請求項５に記載の地図利用装置。
【請求項7】
前記出発地から前記目的地まで、前記ウェイポイントを順番に経由しながら、前記移動体を移動させるように制御する第３処理をさらに実行することを特徴とする請求項１に記載の地図利用装置。
【請求項8】
前記第３処理では、
前記移動体がリアルタイムでカメラ撮影した画像を前記３次元地図と照合することで前記移動体の自己位置をリアルタイムで推定し、
当該リアルタイムで推定される自己位置が、前記ウェイポイントを順番に経由するように、前記移動体を移動させる指示を生成することを特徴とする請求項７に記載の地図利用装置。
前記移動体がリアルタイムでカメラ撮影した画像を前記３次元地図と照合することで前記移動体の自己位置をリアルタイムで推定し、
当該リアルタイムで推定される自己位置が、前記ウェイポイントを順番に経由するように、前記移動体を移動させる指示を生成することを特徴とする請求項７に記載の地図利用装置。
【請求項9】
請求項１に記載の地図利用装置によって予め実行される前記第１処理及び前記第２処理のもとで予め定まっている前記ウェイポイントを順番に経由しながら、前記移動体を移動させるようにリアルタイムでコンピュータが制御する移動制御方法であって、
前記移動体がリアルタイムでカメラ撮影した画像を前記３次元地図と照合することで前記移動体の自己位置をリアルタイムで推定し、
当該リアルタイムで推定される自己位置が、前記ウェイポイントを順番に経由するように、前記移動体を移動させる指示を生成することを特徴とする移動制御方法。
前記移動体がリアルタイムでカメラ撮影した画像を前記３次元地図と照合することで前記移動体の自己位置をリアルタイムで推定し、
当該リアルタイムで推定される自己位置が、前記ウェイポイントを順番に経由するように、前記移動体を移動させる指示を生成することを特徴とする移動制御方法。
【請求項10】
フィールドの３次元地図から、移動体が移動しうる範囲について２次元地図を作成する第１手順と、
前記２次元地図において、出発地から目的地までの経路を、前記移動体がカメラ撮影する画像を前記３次元地図と照合することによって前記移動体の自己位置推定を行う際の精度が高いと判定される複数のウェイポイントを辿るものとして設定する第２手順と、をコンピュータが実行することを特徴とする地図利用方法。
前記２次元地図において、出発地から目的地までの経路を、前記移動体がカメラ撮影する画像を前記３次元地図と照合することによって前記移動体の自己位置推定を行う際の精度が高いと判定される複数のウェイポイントを辿るものとして設定する第２手順と、をコンピュータが実行することを特徴とする地図利用方法。
【請求項11】
コンピュータを請求項１ないし８に記載の地図利用装置として機能させることを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体がVPSによる自己位置推定を行って移動するための、地図利用装置、方法及びプログラム並びに移動制御方法に関する。
【背景技術】
【0002】
ロボット等の移動体による自律移動サービスを導入するに際して、地図を参照して自律走行を行うこととなるが、自律走行時の自己位置推定の精度が高いことが望ましい。自己位置推定の精度が低いと、走行予定の経路から外れる恐れがあるためである。これに関し、自己位置推定の精度の高さに応じて走行経路を計画する技術として例えば特許文献１～３が検討されている。
【0003】
特許文献１では、適切な移動体の走行支援を行うプログラムが開示されている。カメラ画像が取得された位置で自己位置推定の精度を評価し、精度の高さに基づいて、自己位置の推定結果を走行支援に用いるか否かを切り替えることができる。ここで、カメラ画像による自己位置推定の精度が低いと判断された場合は、オドメトリによる自己位置推定に切り替える。
【0004】
特許文献２では、移動体が目的地に到達するまでの時間を短縮する装置が開示されている。レーザセンサの検出結果による自己位置推定部は自己位置推定精度が低下したと評価し、自己位置推定精度が低いと評価された低評価領域は経路として選択される頻度が下がる。これにより、移動体が低評価領域への移動を抑制するため、不要な時間を削減できる。
【0005】
特許文献３では、レーザセンサによる移動体の自己位置推定の精度を簡易に評価することができる自己位置推定精度評価方法が開示されている。移動体の走行環境地図を示す模式図を生成し、自己位置推定の精度が低下しやすい位置を着色することで、ユーザが視覚的に判断することができる。
【先行技術文献】
【特許文献】
【0006】
【特許文献1】特開２０２１－１０５８３２号公報
【特許文献2】特開２０２１－１１７８９３号公報
【特許文献3】特開２０２２－０３７７３６号公報
【非特許文献】
【0007】
【非特許文献1】「小森田ら, "画像に基づく測位技術の研究とシステムの開発実証", 映像情報メディア学会, 2021.」
【発明の概要】
【発明が解決しようとする課題】
【0008】
しかしながら、特許文献１～３のいずれの技術も、自己位置推定の精度を評価するためには、地図の生成対象となるフィールドを少なくとも事前にロボットが走行することを必須とし、当該走行させる手間が発生するという課題があった。
【0009】
なお、フィールドを事前にロボットが走行して地図生成を行うための計測等をロボット自身において行うことが必須とはならない手法として、非特許文献１のVPS（Visual Positioning System）では、人などが撮影した環境のカメラ映像（すなわち、実際に移動するロボットのカメラ視点とは必ずしも一致しないカメラ視点による映像）を画像処理することで地図を生成できるので、実際のロボットの事前走行を必須としない。
【0010】
この点に関して、特許文献１では、画像撮影による地図で自己位置推定しているが、ロボット自身が撮影した画像から地図を生成する必要があるため、VPSのように地図生成時の撮影手段を実際のロボットに限定する必要がないという利点を享受することができない。また、特許文献１では、実際にロボットが走行して自己位置推定を行う段階において初めて、自己位置推定精度の評価が可能となり、走行前において精度評価を行うことができない。
【0011】
また、特許文献２，３では前述の通り、少なくとも事前にロボット走行が必要であるほか、自己位置推定にレーザセンサ計測を用いており、VPSのように画像に基づく自己位置推定とは異なるものであった。
【0012】
上記従来技術の課題に鑑み、本発明は、移動体の事前走行を必須とせずに、画像に基づく自己位置推定精度が高い経路を設定することができる地図利用装置、方法及びプログラムを提供することを第１の目的とする。
【0013】
また、当該第１の目的を達成する地図利用装置が設定した経路において移動体の移動制御を行う移動制御方法を提供することを第２の目的とする。
【課題を解決するための手段】
【0014】
前記第１の目的を達成するため、本発明は地図利用装置であって、フィールドの３次元地図から、移動体が移動しうる範囲について２次元地図を作成する第１処理と、前記２次元地図において、出発地から目的地までの経路を、前記移動体がカメラ撮影する画像を前記３次元地図と照合することによって前記移動体の自己位置推定を行う際の精度が高いと判定される複数のウェイポイントを辿るものとして設定する第２処理と、を実行することを第１の特徴とする。また、当該装置に対応する方法及びプログラムであることを特徴とする。
【0015】
前記第２の目的を達成するため、本発明は前記地図利用装置によって予め実行される前記第１処理及び前記第２処理のもとで予め定まっている前記ウェイポイントを順番に経由しながら、前記移動体を移動させるようにリアルタイムでコンピュータが制御する移動制御方法であって、前記移動体がリアルタイムでカメラ撮影した画像を前記３次元地図と照合することで前記移動体の自己位置をリアルタイムで推定し、当該リアルタイムで推定される自己位置が、前記ウェイポイントを順番に経由するように、前記移動体を移動させる指示を生成することを第２の特徴とする。
【発明の効果】
【0016】
前記第１の特徴によれば、移動体の事前走行を必須とせずに、画像に基づく自己位置推定精度が高い経路を設定することができる。前記第２の特徴によれば、前記地図利用装置が設定した経路において移動体の移動制御を行うことができる。
【図面の簡単な説明】
【0017】
【図1】一実施形態に係る地図利用システムの構成図である。
【図2】一実施形態に係る地図利用システムの各装置の機能ブロック図である。
【図3】一実施形態に係る地図利用システムの全体概要的な動作のフローチャートである。
【図4】環境の撮影映像からVPSの手法で生成される３次元点群としての３次元地図の例を示す図である。
【図6】３次元地図を加工（ロボットが移動しうる高さ範囲のみ抽出することによる加工）した様子の例を示す図である。
【図8】ポリゴン生成のための高さZ軸方向での長さ変換（縮小）の模式例を示す図である。
【図9】３つの閾値の設定値の例と、閾値判定による着色ルールの例を表で示す図である。
【図14】例EX1,EX2として、経路生成部の各処理の模式的な例を示す図である。
【図15】例EX3,EX4として、経路生成部の各処理の模式的な例を示す図である。
【図16】角度θごとに、セル(NX,NY)において定まるカメラ視野の模式例を示す図である。
【図17】JSON形式で記録する例を示す図である。
【図18】ウェイポイントの模式例を示す図である。
【図20】例EX11～EX14として、ロボットの移動制御の説明例を示す図である。
【図21】一般的なコンピュータにおけるハードウェア構成を示す図である。
【発明を実施するための形態】
【0018】
図１は、一実施形態に係る地図利用システム100の構成図である。地図利用システム100は、カメラとしての環境撮影装置1と、サーバ装置としての地図利用装置3及びVPS装置6と、移動体としてのロボット4とを備え、これら各要素は、インターネットやLAN（ローカルエリアネットワーク）等のネットワークNWを介して相互に通信可能である。ここで、地図利用システム100が地図を作成する対象となる範囲としての、屋内及び／又は屋外で構成されるフィールドF内を、環境撮影装置1及びロボット4が移動可能である。
【0019】
なお、図１の装置構成のもとで以下の説明を行うが、この構成は種々の変形も可能である。例えば、図１では地図利用装置3及びVPS装置6はロボット4とは別途の構成としているが、ロボット4内に地図利用装置3及び／又はVPS装置6が搭載（ロボット4に備わるプロセッサ等で地図利用装置3及び／又はVPS装置6が実現）されていてもよい。
【0020】
図２は、一実施形態に係る地図利用システム100の各装置の機能ブロック図である。図示するように、地図利用装置3は、３次元地図加工部311、ポリゴン生成部312、セル生成部313及び２次元地図記憶部314を含む２次元地図作成部31と、出発地・目的地設定部321、経路生成部322及び経路記憶部323を含む経路設定部32と、画像切出部331、位置確認部332及び動作計画部333を含む走行制御部33を備える。
【0021】
ロボット4は、センサ部41、地図生成部42、環境撮影部43及び動作制御部44を備える。VPS装置6は、３次元地図作成部21、３次元地図記憶部22及び位置推定部51を備える。なお、図示されるようにVPS装置6において、３次元地図生成部21及び３次元地図記憶部22は３次元地図を作成する３次元地図作成装置2としての役割を果たし、３次元地図記憶部22及び位置推定部51はロボット4の自己位置推定を行う自己位置推定装置5としての役割を果たす。
【0022】
【0023】
ステップS1では、環境撮影装置1でフィールドF内を網羅的に撮影した画像を入力として、３次元地図作成装置2でフィールドFの３次元地図を作成する。ステップS2では、ステップS1で作成された３次元地図を入力として２次元地図作成部31がフィールドFの２次元地図を作成する。
【0024】
ステップS3では、例えば商品等の物品配送を行うロボット4に関して、フィールドF内での出発地及び目的地の入力を受け付けたうえで、移動体であるロボット4がフィールドF内で出発地から目的地まで移動するための経路を経路設定部32が設定する。
【0025】
ステップS4では、ステップS3で設定された経路（大局的経路）上を、ロボット4が画像撮影を行いながら、この画像を自己位置推定装置5で解析してロボット4の現在位置を推定し、この現在位置に応じたロボット4の移動経路（局所的経路）の決定と当該決定に即したロボット4の走行制御処理を走行制御部33が行い、当該走行制御に従ってロボット4が移動を行う。こうして、ロボット4は各時刻においてフィールドFの撮影を行い、その結果から推定される自己位置を走行制御に従う走行による移動で更新しながら、局所的経路上を順次、移動していくことにより、フィールドF内で出発地から目的地まで到達することができる。
【0026】
以下ではさらに、ステップS1～S4の詳細を説明する。
【0027】
＜３次元地図の作成…ステップS1＞
ステップS1では、３次元地図生成部2が３次元地図を作成し、３次元地図記憶部22に３次元地図を保存する。本実施形態では前掲の非特許文献１に開示されるVPSの手法により３次元地図を作成する場合を例として説明する。
ステップS1では、３次元地図生成部2が３次元地図を作成し、３次元地図記憶部22に３次元地図を保存する。本実施形態では前掲の非特許文献１に開示されるVPSの手法により３次元地図を作成する場合を例として説明する。
【0028】
はじめに、環境撮影装置1を利用して、人手により環境（フィールドF）を撮影する。環境撮影装置1を構成するカメラとしては例えばRICOH THETA（登録商標、商品名）などの360°カメラを持ち、環境を移動した映像を撮影する。この時、VPSの自己位置推定精度を高めるため、360°カメラはロボットの映像撮像部と同じ高さを維持し、曲がり角は直角の移動を避けて緩やかに移動し、輪を描くように一周することが望ましい。
【0029】
なお、画角が超広角となる360°カメラを用いることで、環境内を移動しながら撮影を行う際に、カメラを様々な方向に向ける手間を低減することができるが、画角が前方周辺のみに限定される通常のカメラを用いて、環境内を移動しつつ、様々な方向を撮影するようにしてもよい。
【0030】
次に、撮影した映像をVPS装置6の３次元地図作成部21に入力し３次元地図を作成して３次元地図記憶部22に保存する。映像内の各フレーム画像に対して、人などの動体はノイズとなるため物体認識処理等を用いて除去したうえで、ロボット4が移動する際に移動不能となる障害物としての、あるいは移動のための目印となる物体としての、壁等の静止物に関して、特徴点を抽出し、異なる画像間で特徴点のマッチングを行い、ステレオ視差等の原理を用いて、環境内の静止物の３次元点群の再構築を行うことができる。この際、別途、測量機で地図の一部の長さを計測しておき、それに合わせて点群をスケールさせる物量変換を行うようにしてもよい。
【0031】
以上により、図４に例示されるような点群と座標（３次元点群を構成する各点の３次元座標）を保持したものとしての３次元地図を、VPSの手法により生成することができる。図４の例では、環境（フィールドF）は会議室等の室内であり、３次元空間座標(X,Y,Z)においてX軸とY軸が水平方向、Z軸が垂直方向である。図４では点群を構成する各点を黒のドットで示し、垂直方向Z軸に近い向きから点群の分布を見た状態を示しているため、結果的に、壁等の表面上において垂直方向に分布する点群が紙面上で高密度となって濃く描かれ、逆に、天井や床等の表面上において水平方向に分布する点群が紙面上で低密度となって淡く描かれる状態となっている。図４に示される原点(X,Y,Z)=(0,0,0)は、点群生成の際に適宜、定義することができる。なお、以降の説明において例示で用いる各図も、図４の例と同様の室内環境に関するものである。
【0032】
＜２次元地図の作成…ステップS2＞
ステップS2として、２次元地図作成部31の各部311～315がこの順番で以下の処理を行うことにより、２次元地図を作成する。
ステップS2として、２次元地図作成部31の各部311～315がこの順番で以下の処理を行うことにより、２次元地図を作成する。
【0033】
３次元地図加工部311は、３次元地図から、環境の特徴を保持したまま２次元地図に加工する。ポリゴン生成部312は、当該環境の特徴が保持されたままの２次元地図の空間を区切るポリゴンを生成し、各ポリゴンの属性（移動体の通行可能性についての属性）を決定する。セル生成部313は、当該ポリゴンで区切られた２次元地図に、２次元のグリッド座標で単位格子（以下、セル）及び各セルの属性を決定することで、最終的に作成された２次元地図を得る。２次元地図記憶部314は当該作成された２次元地図を保存して、経路設定部32の処理の際に参照に供する。
【0034】
図５は、図３のステップS2の詳細として、２次元地図作成部31による２次元地図の作成処理を示すフローチャートである。なお、図５の各ステップS101～S120のうち、ステップS101～S106が３次元地図加工部311の処理であり、ステップS107～S118がポリゴン生成部312の処理であり、ステップS119がセル生成部313の処理であり、ステップS120が２次元地図保存部314の処理である。従って、以下の各ステップの説明においては、動作主体である各部311～315の言及は省略し、処理内容のみを説明する。
【0035】
はじめに、ステップS101,S102,S103ではそれぞれ、３次元地図と、床面の高さ（Hf>0[m]）とロボットの高さ方向の全長（Hr>0[m]）とを入力として受け取る。床面高さHfは３次元地図を目視して数値（床面の高度として床面のz軸座標の値であり、正負に応じて±Hfとなる）を確認したユーザ入力の値を用いてよい。また、ロボット全長Hrはロボット4のカタログ値を確認したユーザ入力の値を用いてよい。
【0036】
次に、ステップS104～S106では、上記入力された高さHfと全長Hrとの間に位置する高さzの全ての値（例えば後述する図６の位置関係では-Hf≦z≦Hr-Hf）について、ステップS105において当該高さzにある３次元地図（点群）を抽出し、抽出されなかった点群（座標(x,y,z)のうち高さzがz<-Hfまたはz>Hr-Hfの範囲にある全ての点群）は３次元地図から削除することにより、３次元地図を加工する。なお、ステップS104,S106は、当該ステップS104,S106で囲まれるステップS105を、当該高さ範囲z（-Hf≦z≦Hr-Hf）にある全てのzについて繰り返すことを表す形式上のステップであり、以降の説明でも同様の形式の繰り返しステップが現れる。
【0037】
図６に、３次元地図を加工した様子の例を示す。３次元地図で、ロボットが通過しうる空間領域である、床面高さz=-Hfからロボット4の高さz=Hr-Hfまでの、高さ方向zで長さHrの範囲の領域R2のみを残し、この領域R2以外の領域R1,R3を、ロボットが通過しない領域として削除する。この処理の意義は、点群として生成された３次元地図の全データをそのまま用いるのではなく、ロボット4が環境を通過しうる領域のデータのみに限定することで、ロボット4の走行可否が判定しやすいためである。
【0038】
具体的に、Unity（非特許文献２）などの３次元データを編集できるアプリケーションを使い、ロボットが通過する高さz（-Hf≦z≦Hr-Hf）にある点群を残し、それら以外の点群（図６中の削除する領域）を削除する。
[非特許文献２] 「"リアルタイムコンテンツ制作を担う、世界をリードするプラットフォーム, https://unity.com/ja, (2022-08-01 アクセス).」
[非特許文献２] 「"リアルタイムコンテンツ制作を担う、世界をリードするプラットフォーム, https://unity.com/ja, (2022-08-01 アクセス).」
【0039】
なお、図６の例では、原点の高さz=0が、負の値を取る床面z=-Hf<0の高さと、正の値を取る床面に立つロボットの最上部Hr-Hf>0との間に位置するものとなっている。一般には、図７に示すように、正負を問わない床面高さ座標値z=Hfと、この床面高さz=Hfにロボット高さHr>0を加算したロボット高さ座標z=Hf+Hrと、の間の高さにある点群のみを抽出すればよい。
【0040】
なお、以上のようなステップS104～S106の抽出が可能となる前提として、本実施形態では、２次元地図が作成される対象である、ロボット4が車輪等の移動機構により移動するフィールドF内の地面や床面は、高度が概ね一定であって急峻な坂道などの大きな傾斜箇所が存在せず、概ね水平面とみなせるものとしている。
【0041】
次に、ステップS107では、３次元点群としての３次元地図を２次元的に区切る単位格子であり、当該単位格子内の点群数をカウントするためのポリゴンを生成する。すなわち、３次元地図を、原点から横Xと縦Yを１辺dP [m]で区分けを行う。また高さZは、図８に模式的に示すように点群を保持したまま、Hr[m]からdP [m]に変換し、横Xと縦Yの単位サイズと同様の１辺dP[m]のポリゴンを生成する。図８では、サイズの関係Hr＞dpである（通常はこの関係となる）ことから、点群の位置関係が保たれたままで、且つ、横Xと縦Yの各位置での１辺dP [m]の正方形格子内での点群の数が維持されたままで、長さHrが単位サイズdpに縮小されることで、Z軸方向に縮小された状態となっている。
【0042】
本実施形態ではポリゴンとして、1辺dP[m]のサイズが10[cm]=0.01[m]である立方体を想定して説明するが、これに限らず環境（フィールドF）内でのロボット4の移動に要求される精度等に合わせて変更するようにしてよい。例えば、要求される自己位置推定の精度は、ロボット4が走行する環境によって変化することも考えられる。例えば、屋外の広い場所では、自己位置推定の精度が荒くても許容できる一方で、マンション内等における狭い通路の通過では、高い自己位置推定の精度が要求されることが想定される。このため、ポリゴンのサイズはロボットが走行する環境によって、動的に変化させてもよい。
【0043】
１辺のサイズdP[m]の設定のもとで、横方向のポリゴンの数NXmax個は以下の式(1)の通り、地図のX軸方向の距離dmapx[m]とポリゴンの１辺dP[m]の商である。なお、10[cm]満たない端数が存在する場合は、端数を切り上げて10[cm]と見なして処理してよい。同様に、縦方向のポリゴンの数NYmax個は以下の式(2)の通り、地図のY軸方向の距離dmapy[m]とポリゴンの１辺dP[m]の商である。
【0044】
【数1】
【0045】
次に、ステップS110～S118では、各ポリゴンに含まれる点群の数をカウントする。このためまず、処理対象となるポリゴン位置(X,Y)を格子位置として整数で表す変数(Nx,Ny)をステップS108,S109において、初期値(NX,NY)=(1,1)に設定する。
【0046】
なお、ステップS110及びS118は、これが囲むステップS111～S117を変数Nxの全ての値であるNX=1,2,…,NXmaxについて繰り返し行う第１ループを示す。同様に、ステップS111及びS116は、これが囲むステップS112～S115を変数NYの全ての値であるNY=1,2,…,NYmaxについて繰り返し行う第２ループを示す。こうして、第１ループ及び第２ループの２重ループ処理を伴うステップS110～S118において、ステップS112,S113,S114の処理が、２次元的に配置される全てのポリゴン位置{(Nx,Ny)| Nx=1,2,…,NXmax, NY=1,2,…,NYmax }についてラスタスキャン順で網羅的に実施されることとなる。
【0047】
なお、ステップS115でNYを次の値NY+1に更新し、ステップS117でNXを次の値NX+1に更新し、且つ、NYを初期値NY=1に再設定する処理により、上記ラスタスキャン順の処理が実現される。同様に、ステップS116では、現時点でのNYの値がNYmax+1であればステップS117へ進み、そうでない（NY≦NYmaxである）場合にはステップS111に戻ることで、また、ステップS118では、現時点でのNXの値がNXmax+1であればステップS119へ進み、そうでない（NX≦NXmaxである）場合にはステップS110に戻ることで、上記ラスタスキャン順の処理が実現される。
【0048】
以下、上記のような２重ループ処理によって繰り返される実体的な処理としての、ステップS112,S113,S114について説明する。
【0049】
ステップS112では、当該処理対象となっている位置(Nx,Ny)のポリゴンについて、当該ポリゴン内に含まれる点群の数をカウントしてから、ステップS113へと進む。説明のため、当該カウントされたポリゴン数をnum(Nx,Ny)と表記する。
【0050】
点群にはノイズが含まれる可能性があるため、ステップS113では、点群の数num(Nx,Ny)に応じてノイズと特徴量の多さを判定する。具体的に、点群の数num(Nx,Ny)に閾値Th1を設け、閾値Th1に満たない点群の数を持つポリゴン(Nx,Ny)はノイズと判断する。すなわち、ステップS113から（ステップS114をスキップして）ステップS115へ至ることで、図９で後述するように、閾値Th1に満たない点群の数を持つポリゴン(Nx,Ny)は着色を行わず、初期値の色として色なし（白色）として扱われる。
【0051】
一方、閾値Th1を超えた点群の数num(Nx,Ny)を持つポリゴン(Nx,Ny)について、ステップS113からステップS114へと進み、ステップS114において着色を行う。ここで、点群の数num(Nx,Ny)に応じてさらに２つ閾値Th2,Th3を設け、３つの閾値により４段階で色の濃さ（及び色の有無）を調整する。（なお、当該設定する３つの閾値について、大小関係はTh1<Th2<Th3である。）これを全てのポリゴン(Nx,Ny)で実施する。
【0052】
図９は、３つの閾値Th1<Th2<Th3の設定値の例と、閾値判定による着色ルールと、着色が意味するポリゴン属性の例を表で示す図であり、図１０は、色の濃さを図９の表の着色ルールに従って着色した場合のポリゴン群の例である。
【0053】
なお、図９では示される通り、３つの閾値Th1=20,Th2=25,Th3=40で４段階に当該ポリゴン(Nx,Ny)位置の２次元地図としての属性を評価しており、点群数num(Nx,Ny)が最小の閾値Th1以下である場合には、当該ポリゴン位置には物体は存在せず、ロボット4が移動可能な通路であると評価する。
【0054】
一方で、点群数num(Nx,Ny)が最小の閾値Th1より多い場合には、当該ポリゴン位置に、ロボット4が移動不可能な障害物としての物体（壁など）が存在するものと評価するが、その信頼度を３段階に分けるようにしている。すなわち、Th2>Th1となる第２の閾値により点群数num(Nx,Ny)がTh1より多くTh2以下である場合は、物体に該当し、その信頼度は低いものとする。また、Th3>Th2となる第３の閾値により点群数num(Nx,Ny)がTh2より多くTh3以下である場合は、物体に該当し、その信頼度は中程度とする。さらに、点群数num(Nx,Ny)がTh3より多い場合は、物体に該当し、その信頼度は高いものとする。
【0055】
図９に例示される、色なし（白色）と、色ありの場合の赤色についての３段階の濃淡の設定は、図９に示される通りのポリゴン属性を区別して２次元地図上において可視化するための例であるが、以下では、この色の違いがポリゴン属性を表現するものとして、図９の例を前提に説明するものとする。
【0056】
なお、色ありの場合には当該ポリゴン位置に物体があるものと推定されるが、この物体は前述の通り、ロボット4が移動不能な障害物を構成しうるものであり、同時に、ロボット4が通路を移動する際の、画像による自己位置推定を行うための目印としての役割を果たしうるものである。
【0057】
なお、図９では色あり（物体あり）の場合を３段階に分けているが、１段階以上の任意の段階に分けるようにしてよい。２段階以上に分ける場合は、点群がより多いほど、物体に該当する信頼度がより高いものとしてポリゴン属性を評価し、適宜の色分け等で区別するようにすればよい。
【0058】
以上、ステップS110～S118での全ポリゴン(Nx,Ny)に対する着色を説明した。次に、ステップS119では、当該着色された色を保持したまま、ポリゴンの高さ方向の情報を0[cm]に圧縮することでセルを生成することにより、図１１のような２次元地図を生成する。図１１の２次元地図は、図１０のポリゴン高さZ軸方向を0[cm]に圧縮したものであり、整数値による２次元のグリッド座標(Nx,Ny)で各々の位置が指定され、１辺のサイズがdp[m]の正方形となる単位格子（セル）及びその着色による属性で構成されるものとなる。ステップS120では、ステップS119で生成された２次元地図を２次元地図記憶部314に保存し、図５のフロー（２次元地図作成部31の処理）を終了する。
【0059】
＜経路の設定…ステップS3＞
ステップS3として、経路設定部32の各部321～323がこの順番で以下の処理を行うことにより、経路の設定を実施し、設定された経路を経路記憶部323に保存する。ここで、経路は自己位置推定の精度を低い場所を避けた、目的地に到達する経路を導出する。
ステップS3として、経路設定部32の各部321～323がこの順番で以下の処理を行うことにより、経路の設定を実施し、設定された経路を経路記憶部323に保存する。ここで、経路は自己位置推定の精度を低い場所を避けた、目的地に到達する経路を導出する。
【0060】
図１２は、図３のステップS3の詳細として、一実施形態に係る経路設定処理のフローチャートである。なお、図１２のフローの全ステップS201～S222のうち、最初のステップS201,S202が出発地・目的地設定部321の処理であり、これより後のステップS203～S222が経路生成部322の処理であるため、以下において各ステップの説明を行うに際して、動作主体への言及は省略する。
【0061】
【0062】
初めにステップS201では、２次元地図記憶部314に記憶されている２次元地図の入力を受け付け、ステップS202において、出発地と目的地との指定をユーザ入力による指定として受け付けることで設定する。ここで、図１１でも示したとおりの２次元地図上のセル位置(NX,NY)として出発地と目的地の入力を受け付ける。これを出発地(SX,SY)と目的地(GX,GY)として表す。（すなわち、出発地はセル位置(NX,NY)=(SX,SY)であり、目的地はセル位置(NX,NY)=(GX,GY)であるものとする。）
【0063】
次に、ステップS203では、２次元地図から物体の判定を行う。なお、ステップS203での当該物体判定を行うことの意義は、この物体は環境の特徴であるため、この特徴を用いて自己位置推定の精度を予測できるようにするためである。
【0064】
具体的に、図１３のポイント表に応じて、２次元地図のうち白色ではなく、着色されているセルの色に応じたポイントの合算値を算出する。なお、図１３は、赤色に関して図９の３通りの濃淡に応じたポイント値として、事前設定しておくポイント表の例を示すものである。なお、図１３の例に限らず、通路に該当するのではなく、物体が存在すると判定されたセルについて、元の点群のカウント数がより多いことで物体に該当する信頼度がより高いと推定されるほど、ポイントの値も高くなるような設定を用いてよい。
【0065】
上記の通り、着色セルの全てについてポイント表からポイントを定めたうえで、さらに、２次元地図上で着色セルが連結しており、且つ、当該連結する着色セルのポイント合算値が閾値以上となる集合を物体とみなすことで、ステップS203の物体判定を行うことができる。すなわち、着色セルのうち当該条件に該当するものが物体と判定され、着色セルであっても当該条件に該当しないものと、着色されていない（白色の）セルは物体ではないものとして判定される。
【0066】
例えば、ポイント合算値に対する閾値を10とした場合、図１４の例EX1に示すように、４つの着色セル連結箇所C1～C4についてそれぞれ、物体判定を行うことができる。なお、閾値は10に限らず、環境に合わせて適切な値を管理者等が指定することにより、変更できるものとする。
【0067】
図１４の例EX1では、４つの着色セル連結箇所C1～C4についてそれぞれ以下の通りポイント合算値sum(C1)～sum(C4)を算出し、閾値10との比較で物体とみなす／みなさないの判定を行うことができる。
【0068】
●連結箇所C1は２個の濃い赤のセルの連結領域として構成され、
sum(C1)=2×4=8<10であるため、連結箇所C1は物体とはみなさない。
●連結箇所C2も２個の濃い赤のセルの連結領域として構成され、
sum(C2)=2×4=8<10であるため、連結箇所C2も物体とはみなさない。
●連結箇所C3は２個の薄い赤のセルと２個の赤のセルと１個の濃い赤のセルの連結領域として構成され、
sum(C3)=2×1+2×2+1×4=10≧10であるため、連結箇所C3は物体とみなす。
●連結箇所C4は３個の濃い赤のセルの連結領域として構成され、
sum(C4)=3×4=12≧10であるため、連結箇所C4は物体とみなす。
sum(C1)=2×4=8<10であるため、連結箇所C1は物体とはみなさない。
●連結箇所C2も２個の濃い赤のセルの連結領域として構成され、
sum(C2)=2×4=8<10であるため、連結箇所C2も物体とはみなさない。
●連結箇所C3は２個の薄い赤のセルと２個の赤のセルと１個の濃い赤のセルの連結領域として構成され、
sum(C3)=2×1+2×2+1×4=10≧10であるため、連結箇所C3は物体とみなす。
●連結箇所C4は３個の濃い赤のセルの連結領域として構成され、
sum(C4)=3×4=12≧10であるため、連結箇所C4は物体とみなす。
【0069】
【0070】
ステップS204では、カメラ情報（画角、焦点距離）を入力し、この値からカメラ視野を生成する。このカメラ情報は、ロボット4の環境撮影部43を構成するカメラについての情報として、ユーザ入力等により取得すればよい。
【0071】
次いで、ステップS205～S219では、３重ループ処理により、ステップS211,S212,S213の処理を、2次元地図を網羅するように、横NX縦NY向きθの全てのパラメータについて実施する。
このためまず、ステップS205において、処理対象となるグリッド位置を表す整数変数(Nx,Ny)に関し初期値(NX,NY)=(1,1)に設定する。ステップS205ではまた、向きθに初期値θ=0°を設定する。
このためまず、ステップS205において、処理対象となるグリッド位置を表す整数変数(Nx,Ny)に関し初期値(NX,NY)=(1,1)に設定する。ステップS205ではまた、向きθに初期値θ=0°を設定する。
【0072】
ステップS206及びS219は、これが囲むステップS207～S218を変数Nxの全ての値であるNX=1,2,…,NXmaxについて繰り返し行う第１ループを示す。同様に、ステップS207及びS218は、これが囲むステップS208～S217を変数NYの全ての値であるNY=1,2,…,NYmaxについて繰り返し行う第２ループを示す。こうして、第１ループ及び第２ループの２重ループ処理を伴うステップS206～S219において、ステップS209～S215の処理が、２次元的に配置される全てのポリゴン位置{(Nx,Ny)| Nx=1,2,…,NXmax, NY=1,2,…,NYmax }についてラスタスキャン順で網羅的に実施されることとなる。
【0073】
なお、ステップS216でNYを次の値NY+1に更新し、ステップS218でNXを次の値NX+1に更新し、且つ、NYを初期値NY=1に再設定する処理により、上記ラスタスキャン順の処理が実現される。同様に、ステップS217では、現時点でのNYの値がNYmax+1であればステップS218へ進み、そうでない（NY≦NYmaxである）場合にはステップS207に戻ることで、また、ステップS219では、現時点でのNXの値がNXmax+1であればステップS220へ進み、そうでない（NX≦NXmaxである）場合にはステップS206に戻ることで、上記ラスタスキャン順の処理が実現される。
【0074】
以下、上記のような２重ループ処理によって繰り返される実体的な処理としての、ステップS208～S215について説明する。（なお、当該ステップS208～S215内にはさらに、着色セルを対象として向きθに関して網羅的に繰り返す処理があるため、全体として３重ループ処理が行われる。）
【0075】
ステップS208では、ステップS204で入力されたカメラ情報に即して、当該セル(NX,NY)に当該カメラ情報で画角や視野が定まるカメラが位置しているものとして、カメラ視野の情報を用意する。このカメラ視野は、２次元地図上において扇型や三角形等の形状の領域として定まるものとなる。なお、カメラ視野における距離については、無限遠までとするのではなく、カメラ位置から一定範囲内に制限するよう、設定しておけばよい。
【0076】
なお、ステップS208ではカメラ視野の領域形状の情報を用意したうえで、後述するステップS210において、当該セル(NX,NY)にカメラが存在し、且つ、カメラが角度θの方向（２次元地図上での0°≦θ<360°の方向であり、当該θに関するループ処理の繰り返しにおいて設定されている角度θ）を向いているものとして、２次元地図上にカメラ視野を重畳する処理が行われる。
【0077】
ステップS209では、当該セル(NX,NY)が着色されているか否かを判定し、着色されていなければ（当該セル(NX,NY)は白色であり、通路に該当するのであれば）処理対象としてステップS210へ進み、着色されている（当該セル(NX,NY)は物体に該当し移動不可能な箇所である可能性が高い）のであれば、処理対象からスキップすべく、ステップS216へ進む。
【0078】
ステップS210～S215では、移動可能な通路に該当し処理対象とされた当該非着色セル(NX,NY)において、ステップS208のカメラ視野の向きθを0°～360°で網羅的に設定し、各θの値について処理S211,S212,S213の処理を繰り返す。本実施形態では、当該繰り返す際の角度のインクリメント幅をステップS214において10°としているが、その他の幅を設定してよい。すなわち、図１２のステップS210～S215では、処理対象とされた当該非着色セル(NX,NY)に関して、10°のインクリメント幅の36個の各角度θ=0°,10°,20°,…,340°,350°で網羅的に、ステップS211,S212,S213の処理を行う。（このため、ステップS215では、角度θについて、ステップS214で設定されるインクリメント幅のもとで0°≦θ<360°の全てについて処理が完了しているかを判定し、完了していればステップS216へ進み、完了していなければステップS210に戻って繰り返す。）
【0079】
図１６に、角度θごとに、セル(NX,NY)において定まるカメラ視野の模式例を示す。図１６の例では、左側に示す7×7セル範囲の中央にあるセルについて、角度のインクリメント幅45°によって360°範囲を網羅的にカバーしたものとして、8つの各角度θ=0°,45°,90°,…,270°,315°の方向をそれぞれ向く扇型状のカメラ視野CV0～CV7が、右側に示されている。
【0080】
図１６の例のようなカメラ視野方向θごとの網羅的な処理を、図１２のステップS211,S212,S213において行う。このため、ステップS210では、方向θを向くカメラ視野を示す扇形状領域を2次元地図に重畳したうえで、移動可能な通路に該当し処理対象とされた非着色セル(NX,NY)のうち自己位置推定の精度が高い場所（セル）を特定すべく、ステップS211へ進む。自己位置推定の精度が高いと判定するセルは、一定量の特徴を把握して判定する。
【0081】
具体的に本実施形態では、ステップS211又はステップS212の２つの条件判定の少なくとも片方に該当した場合に、ステップS213へと進み、当該セル(NX,NY)及び方向θが、自己位置推定精度が高いものである旨の情報を保存する。ステップS211及びステップS212の２つの条件判定のいずれにも該当しない場合には、ステップS213はスキップされ、当該セル(NX,NY)及び方向θが、自己位置推定精度が高いものである旨の情報は保存されない。
【0082】
１つ目の判定としてステップS211では、当該セル位置(NX,NY)での方向θのカメラ視野に含まれる物体数が閾値Tho以上である場合に、自己位置推定精度が高いものと判定する。
【0083】
図１４の例EX2は当該１つ目の判定の例であり、閾値Tho=2の設定において、セル位置(NX,NY)=(6,8)で角度θ=90°のカメラ視野CV(6,8,90°)内の物体数が2であり、閾値以上となっているため、セル位置(NX,NY)=(6,8)での角度θ=90°は自己位置推定の精度が高いと判定される。ここで、カメラ視野CV(6,8,90°)内には、物体と判定された連結領域C3の一部と、物体と判定された連結領域C4の全部と、が含まれるため、物体数は2となる。
【0084】
なお、この例EX2の連結領域C3に示されるように、物体と判定された連結領域の一部のみが含まれる場合、当該連結領域の一定割合以上（例えば半分以上）が、セル位置(NX,NY)の角度θのカメラ視野に含まれていれば、１つの物体数としてカウントし、一定割合未満であれば、物体数としてカウントしないようにすればよい。
【0085】
【0086】
２つ目の判定方法としてステップS212では、当該セル位置(NX,NY)での方向θのカメラ視野に含まれるセルのポイント合計値が閾値Thp以上である場合に、自己位置推定精度が高いものと判定する。
【0087】
図１５の例EX3は当該２つ目の判定の例であり、閾値Thp=20の設定において、セル位置(NX,NY)=(2,6)で角度θ=90°のカメラ視野CV(2,6,90°)内のポイント合計値が以下の通り、22である。このポイント合計値は閾値以上であるため、セル位置(NX,NY)=(2,6)で角度θ=90°は自己位置推定の精度が高いと判定される。
【0088】
すなわち、カメラ視野CV(2,6,90°)内には薄い赤のセルが２個、赤のセルが１個、濃い赤のセルが４個存在するため、ポイント合計値を以下の通り計算できる。
2×1+1×2+4×4=22
2×1+1×2+4×4=22
【0089】
なお、例EX3のカメラ視野CV(2,6,90°)における位置(4,4)の赤いセルのように、カメラ視野内にセルの全部ではなく一部分のみが含まれるセルは、ポイント計算で用いる対象からは除外するようにすればよい。あるいは、このセルのように、一部分のみが視野内に含まれるセルに関して、セルの一定面積割合以上が視野内に含まれる場合に、ポイント計算の対象に用いるようにしてもよい。
【0090】
以上のステップS211,S212の２つの判定のいずれにも該当しないセル位置(NX,NY)及び角度θは、ステップS213がスキップされることで自己位置推定精度が高い旨の情報が記録されないことにより、自己位置推定精度が低いものとして扱うことが可能となる。
【0091】
図１５の例EX4は、当該２つの判定のいずれにも該当しない例であり、セル位置(NX,NY)=(8,8)で角度θ=90°のカメラ視野CV(8,8,90°)について、含まれる物体数は、物体判定された連結領域C4の１個のみ（物体判定された連結領域C3は一定割合以下の一部分しか視野内に含まれないので物体数カウントから除外する）であるため、閾値Tho=2の設定により閾値未満の物体数１個であり、１つ目の判定が不成立となる。
【0092】
同様に、このカメラ視野CV(8,8,90°)について、領域内のポイント合計値は以下の計算の通り12であり、閾値Thp=20の設定により閾値未満のポイント合計値であるため、２つ目の判定も不成立となる。すなわち、カメラ視野CV(8,8,90°)内には濃い赤のセルが３個存在する（一部分のみが当該視野内に含まれる２個の赤いセルは前述の手法によりポイント計算対象から除外する）ため、ポイント合計値を以下の通り計算できる。
3×4=12
3×4=12
【0093】
以上の通り、例EX4のカメラ視野CV(8,8,90°)は、２つの判定条件のいずれにも該当しないため、自己位置推定精度が高いものとしての情報が保存されないことにより、自己位置推定精度が低いものとして扱うことができる。
【0094】
以上、ステップS211,S212,S213では、セル位置(NX,NY)及び角度θの全ての組み合わせについて、自己位置推定精度が高いと判定された場合に、その旨の情報を記録することができる。当該自己位置推定精度が高いと判定された座標{(NX,NY),θ}の記録は例えば、図１７の例D1に例示されるように、周知のデータ記法であるJSON（JavaScript Object Notation）形式等で記録しておくことができる。
【0095】
なお、図１７は、経路設定部32において扱うデータ例をJSON形式の例によりD1～D3として示す図であり、以下の説明でも適宜、参照する。
【0096】
次に、ステップS220では経路計画を行う。経路計画は、２次元地図の白色セル（非着色セル）をロボットが移動可能な領域と見立て、出発地と目的地からランダムサンプリングベース（非特許文献３）による最適経路を算出する。これにより、JSON形式で例えば図１７の例D2のような経路計画の座標が得られる。
[非特許文献３] 「平瀬祐貴ら, 動的環境を考慮した移動ロボットの経路計画", ロボティクス・メカトロニクス 講演会, 2019.」
[非特許文献３] 「平瀬祐貴ら, 動的環境を考慮した移動ロボットの経路計画", ロボティクス・メカトロニクス 講演会, 2019.」
【0097】
この経路計画の情報は、例えば以下のように、２次元地図上で隣接する白色セルを、出発地から目的地まで辿るものとして得るようにしてよい。あるいは、以下のようなセル座標によるではなく、メートル単位等の２次元地図上での環境内の物理的な位置座標による経路として得るようにしてもよい。
(1,1)→(1,2)→(1,3)→(1,4)→(2,4)→(3,4)→…
(1,1)→(1,2)→(1,3)→(1,4)→(2,4)→(3,4)→…
【0098】
なお、最適経路の算出には、非特許文献３のような任意の既存の最適経路問題を解くアルゴリズム等を用いることができる。深層学習等に基づく手法を用いてもよい。
【0099】
次に、最短経路と自己位置推定の精度が高いセルの座標をマッチング（突合）する。両者の座標間の距離dを計算する。距離dは座標間のユークリッド距離とし、以下の式で計算される。
【0100】
【数2】
【0101】
上記の式において、(xpp,ypp,θpp)が最適経路上の座標であり、このうち(xpp,ypp)が２次元座標位置であり、θppは、最適経路に沿って位置(xpp,ypp)を進んでいる時点での経路上の２次元方向（経路に曲線をあてはめた際の位置(xpp,ypp)での接線方向）である。また、(xhl,yhl,θhl)は自己位置推定精度が高いと判定された座標である。なお、セル座標に関して、(Nx,Ny)は格子点位置を示す整数座標（無次元）であり、(xhl,yhl)はメートル単位等の、２次元地図上での実際の物理的距離を表す座標である。(xhl,yhl)=(dp×Nx,dp×Ny)等により相互に変換することが可能である。
【0102】
そして、以下の式のように、最適経路上の座標への距離d（最短距離）が予め設定する閾値Thd以内の距離にある、自己位置推定精度が高いと判定された座標（セル）をウェイポイントと判定する。
d<Thd
d<Thd
【0103】
例えば、経路計画の座標id=1と自己位置推定の精度が高い座標id=1、経路計画の座標id=2と自己位置推定の精度が高い座標id=2、がウェイポイントと判定された場合、ウェイポイントは出発地（Sx,Sy）と目的地（Gx,Gy）と共に、経路記憶部にて保存する。以上により、JSON形式で図１７のデータ例D3のようなウェイポイントの座標が得られる。
【0104】
こうして、ウェイポイント集合を、当該集合の要素であるウェイポイントの各々について、最適経路に沿って移動することに対応した順番の情報も付与される形で、最適経路に近いと判定された、自己位置推定精度が高いと判定された点の集合として得ることができる。
【0105】
図１８は、ウェイポイントの模式例を示す図であり、出発地(Sx,Sy)から目的地(Gx,Gy)へ至る最適経路BPの近傍に、最適経路BP上の移動順番に沿って、８つのウェイポイントwp1～wp8が得られている。ウェイポイントは、場合によっては最適経路BP上の点に一致するものとして得られることもありうる。図１８の例では、８つのウェイポイントwp1～wp8のうち、２つのウェイポイントwp5,wp8が、最適経路BP上の点として得られている。（なお、当該例示する一致は、地図上の２次元座標としての一致である。前述の通り、ウェイポイント及び最適経路の点はいずれも、地図上の２次元座標の他に、方向成分も有している。方向成分まで含めて一致する場合もありうる。）
【0106】
なお、後述するようにウェイポイントは、ロボット4が順に辿ることで移動を実現するものであるため、隣接セルに渡って空間的に密集して設定されるのではなく、図１８の模式例に示されるように、空間的にある程度まばらに設定されることが望ましい。このため、ステップS221の判定において、閾値Thdを小さめの値に設定するようにしてもよいし、あるいは、後述するステップS311（図１９）の判定において、閾値xの値を大きめに設定し、ウェイポイントが隣接セルに渡って密集していても、実質的に１つのウェイポイントとして扱えるようにしてもよい。
【0107】
＜走行制御と地図生成…ステップS4＞
ステップS4として、走行制御と地図生成を実施する。ロボット4に対して走行制御を行いつつ、ロボット内部の地図を生成する。図１９は、図３のステップS4の詳細として、一実施形態に係る走行制御と地図生成の処理を示すフローチャートである。
ステップS4として、走行制御と地図生成を実施する。ロボット4に対して走行制御を行いつつ、ロボット内部の地図を生成する。図１９は、図３のステップS4の詳細として、一実施形態に係る走行制御と地図生成の処理を示すフローチャートである。
【0108】
はじめにステップS301では、ロボット4内部でセンサ部41を稼働させてこの出力を地図生成部42が処理することによって地図生成を実行し、ステップS302へ進む。地図生成はロボット4にセンサ部41として搭載されたLiDARセンサにより環境をセンシングすることで地図を生成するLiDAR-SLAM（非特許文献４）を実行するようにしてよいが、これに限らず地図を生成するアルゴリズムを用いてよい。
[非特許文献４] 「友納ら, "SLAM の現状と今後の展望", システム制御情報学会, 2020.」
[非特許文献４] 「友納ら, "SLAM の現状と今後の展望", システム制御情報学会, 2020.」
【0109】
なお、ステップS301では地図作成を開始し、以降のステップS302～S312において本実施形態の手法により出発地から目的地までロボット4が移動し、ロボット4は、当該移動している最中にリアルタイムで継続的に、センサ部41によるセンサデータ取得と、この取得データを用いた地図生成部42による地図生成（３次元点群地図の生成）が行われ、目的地に到達した際に、ステップS313において、センサ部41及び地図生成部42による地図作成を終了する。
【0110】
なお、このステップS301で作成する地図は、ロボット4が環境を移動する機会が得られたことを利用して作成する地図であるため、本実施形態の３次元地図記憶部314に保存させることが必須の地図ではない。また、ステップS302～S312でロボット4が移動する際に参照する情報は、事前に作成され、経路記憶部323に記憶される経路となる。
【0111】
次に、ステップS302では、経路記憶部323から動作記憶部323がウェイポイントを順次取得し、走行経路を把握する。なお、図１８で例示の通り、ウェイポイントの各々には、最適経路（走行経路とは異なる）に沿った順番が付与されている。
【0112】
次に、ステップS303では、ロボット4に搭載されたカメラとしての環境撮影部43から映像を取得し、さらにステップS304において、画像切出部331により映像からフレーム単位の画像を切り出す。これはVPS（非特許文献１）における自己位置推定を用いて、画像と３次元地図とのマッチングを行うためである。
【0113】
次に、VPS装置6の自己位置推定装置5における自己位置推定を行う。ステップS305において、位置確認部332が、位置推定部51に対して、リアルタイムでの画像等のデータ送受信の手法として例えばREST API（非特許文献５）を用いて画像を送信する。
[非特許文献５] 「"REST API とは", https://www.redhat.com/ja/topics/api/what-is-a-rest-api, (2022-07-26 アクセス).」
[非特許文献５] 「"REST API とは", https://www.redhat.com/ja/topics/api/what-is-a-rest-api, (2022-07-26 アクセス).」
【0114】
ステップS306にて、位置推定部51は受信した画像から動体などのノイズの除去、特徴量の抽出を行う。その後、位置推定部51は画像の特徴量と３次元地図（点群地図）間をマッチングするPnP問題（非特許文献６）を解くことで、環境撮影部43のカメラが映像を撮影した位置と姿勢（X,Y,θ）を算出する。すなわち、位置推定部51は画像と３次元地図とを照合して自己位置（X,Y,θ）を推定する。その後、自己位置（X,Y,θ）はVPS側の位置推定部5におけるREST APIの応答により、位置確認部332で受信する。
[非特許文献６] 「PE Sarlinら, "From Coarse to Fine: Robust Hierarchical Localization at Large Scale", CVPR, 2019.」
[非特許文献６] 「PE Sarlinら, "From Coarse to Fine: Robust Hierarchical Localization at Large Scale", CVPR, 2019.」
【0115】
次に、ステップS307及びS308にて、動作制御部44が自己位置と、ステップS302で動作計画部333が取得して動作制御部44に通知されているウェイポイントとの位置差分を幾何学的に計算する。この目的は、ロボット4を走行させて、位置差分をなくすことにある。なお、ロボット4は車輪等の移動機構を有し、前進と、後退と、位置を変えないままでの回転（平面上での向きを変えること）とが可能であるものとする。
【0116】
具体的に、ステップS307及びS308ではそれぞれ、図２０の例EX11のように現在の姿勢（２次元地図平面上でロボット4が前進したとする際に移動する向きベクトルvr）と現在のウェイポイントW1とのなす角度θと、ウェイポイントW1と現在の位置（X, Y）間の距離rを計算する。ここで、前記向きベクトルvrと、ロボット4の現在位置(X,Y)を始点としてウェイポイントW1を終点とするベクトルvwを用いて、当該なす角度θは以下により計算される。
【0117】
【数3】
【0118】
なお、図２０は例EX11～EX14として、ロボット4の移動制御の説明例を示す図であり、以下でもさらに参照する。
【0119】
次に、ステップS309では、図２０の例EX11の状態から例EX12の状態に変化させるように、θ=0となるまでロボット4を回転させるよう、動作制御部44が制御を行う。次に、ステップS310では、図２０の例EX12の状態から例EX13の状態に変化させるように、ロボット4を前進させるよう、動作制御部44が制御を行う。
【0120】
この移動の途中では、ロボット4の位置Pr=(X,Y)とウェイポイントの位置Pw=(Xw,Yw)の距離rを以下によりリアルタイムに計算する。なお、リアルタイムでの映像を位置推定部51で解析して推定しているため、ロボット4の位置Pr=(X,Y)もリアルタイムに算出可能である。
r=|Pr-Pw|
r=|Pr-Pw|
【0121】
これら（リアルタイムで距離rや角度θを計算しながらロボット4を前進移動させること）を繰り返し実行し、ステップS311では、図２０の例EX14のようにrが所定の閾値x以内（r≦x）の範囲に収まった状態で、ロボット4がウェイポイントW1に到着したと判定する。
【0122】
ステップS311では、上記閾値判定r≦xにより現在のウェイポイントに到着していると判定されれば、ステップS312へ進み、到達していない判定である場合は、ステップS303に戻って繰り返すことで、リアルタイムで距離rや角度θを計算しながらロボット4をウェイポイントに向けて前進移動させる処理が実現される。
【0123】
ステップS312では、ロボット4が目的地に到達したか否かを判定し、到達していればステップS313に進んで地図（ステップS310で作成開始され、ロボット4の移動に沿って地図作成部42で継続して作成され続けている、LiDAR-SLAM等の手法による地図）を保存し、図１９のフローを終了する。なお、ステップS312における目的地への到着判定は例えば、目的地(Gx,Gy)を形式上、順番が最後に該当するウェイポイントとして扱うことによってステップS311の判定を行い、ステップS311で到着判定したウェイポイントが当該最後の目的地(Gx,Gy)であった場合に、ステップS312の判定が成立するものとして扱えばよい。
【0124】
ステップS312でロボット4が目的地に到達していない判定の場合、ステップS302に戻り、次のウェイポイント（図２０の例で、現在到達しているウェイポイントがW1であれば、次のウェイポイントW2）を取得し、以上の処理を繰り返す。こうして、図１８の例であれば、出発地(SX,SY)→wp1→wp2→…→wp7→wp8→目的地(GX,GY)のように、出発地から目的地までウェイポイントを順番に経由しながら、ロボット4が移動することが可能である。
【0125】
ここで、ウェイポイントは２次元位置及びカメラ方向に関して、自己位置推定精度が高いものとして予め推定されている。従って、本実施形態により、ロボット4を予め環境内を移動させる手間を必須とすることなく、VPSの手法により、自己位置推定の精度の低下でロボット4が移動途中で走行不可になるリスクを回避し、ロボット4が目的地への到達することができる。さらに、地図生成部42の処理により、ロボット4が目的地に到達すると、ロボット4の地図も完成する。
【0126】
以下、種々の補足例、追加例、代替例等について説明する。
【0127】
本発明の実施形態の応用例として、何らかの作業現場等である環境においてロボット4を活用した作業等を遠隔制御（遠隔の管理者等が地図利用装置3を利用することによる集中管理）によってより円滑に行うことが可能となる。すなわち、本実施形態によれば、ロボット4を実環境で事前に稼働させることを必須とせず、VPS手法での自己位置推定精度が高い移動経路を設定できるので、準備の手間等が低減され、遠隔作業等をより円滑に行うことが可能となる。
【0128】
これにより、当該作業現場等に業務担当者等が実際に赴くことを必須とせず、人物移動に必要となるエネルギー資源を節約することで二酸化炭素排出量を抑制できることから、国連が主導する持続可能な開発目標（ＳＤＧｓ）の目標１３「気候変動とその影響に立ち向かうため、緊急対策を取る」に貢献することが可能となる。
【0129】
図２１は、一般的なコンピュータ装置70におけるハードウェア構成の例を示す図である。地図利用システム100を構成する環境撮影装置1、VPS装置6、地図利用装置3及びロボット4の各々は、このような構成を有する１台以上のコンピュータ装置70として実現可能である。なお、２台以上のコンピュータ装置70で地図利用システム100の部分構成を実現する場合、ネットワーク経由で処理に必要な情報の送受を行うようにしてよい。コンピュータ装置70は、所定命令を実行するCPU（中央演算装置）71、CPU71の実行命令の一部又は全部をCPU71に代わって又はCPU71と連携して実行する専用プロセッサとしてのGPU（グラフィックス演算装置）72、CPU71（及びGPU72）にワークエリアを提供する主記憶装置としてのRAM73、補助記憶装置としてのROM74、通信インタフェース75、ディスプレイ76、マウス、キーボード、タッチパネル等によりユーザ入力を受け付ける入力インタフェース77、ロボット4としての自己位置推定機能に用いられるカメラ81及びセンサ82、ロボット4としての自律移動機能を提供する、モータ等のアクチュエータ83及びこのアクチュエータ83によって駆動される車輪等の移動機構84（車軸やギヤ等の変速機構も含む）と、これらの間でデータを授受するためのバスBSと、を備える。前述のステップS102,S103（図５）やステップS202,S204（図１２）等における入力をユーザ入力として受け付ける場合、入力インタフェース77を介して受け付けるようにしてよい。
【0130】
地図利用システム100の各機能部は、各部の機能に対応する所定のプログラムをROM74から読み込んで実行するCPU71及び／又はGPU72によって実現することができる。なお、CPU71及びGPU72は共に、演算装置（プロセッサ）の一種である。ここで、表示関連の処理が行われる場合にはさらに、液晶等のモニタあるいはLED等の点灯装置として構成されるディスプレイ76が連動して動作し、データ送受信に関する通信関連の処理が行われる場合にはさらに通信インタフェース75が連動して動作する。
【0131】
ロボット4において、環境撮影部4及びセンサ部41はカメラ81及びセンサ82からの入力を用いて実現し、動作制御部44からの制御出力によってモータ等のアクチュエータ83が駆動され、車輪等の移動機構84が動作することにより、ロボット4は移動することが可能となる。
【符号の説明】
【0132】
100…地図利用システム、1…環境撮影装置、2…３次元地図作成装置、3…地図利用装置、4…ロボット、5…自己位置推定装置、6…VPS装置
21…３次元地図作成部、22…３次元地図記憶部、31…２次元地図作成部、32…経路設定部、33…走行制御部、41…センサ部、42…地図生成部、43…環境撮影部、44…動作制御部、51…位置推定部
311…３次元地図加工部、312…ポリゴン生成部、313…セル生成部、314…２次元地図記憶部、321…出発地・目的地設定部、322…経路生成部、323…経路記憶部、331…画像切出部、332…位置確認部、333…動作計画部
21…３次元地図作成部、22…３次元地図記憶部、31…２次元地図作成部、32…経路設定部、33…走行制御部、41…センサ部、42…地図生成部、43…環境撮影部、44…動作制御部、51…位置推定部
311…３次元地図加工部、312…ポリゴン生成部、313…セル生成部、314…２次元地図記憶部、321…出発地・目的地設定部、322…経路生成部、323…経路記憶部、331…画像切出部、332…位置確認部、333…動作計画部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】