(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023176361
(43)【公開日】2023-12-13
(54)【発明の名称】移動体制御システムおよび移動体制御方法
(51)【国際特許分類】
G05D 1/02 20200101AFI20231206BHJP
【ＦＩ】
G05D1/02 H
【審査請求】未請求
【請求項の数】15
【出願形態】ＯＬ
(21)【出願番号】P 2022088603
(22)【出願日】2022-05-31
(71)【出願人】
【識別番号】507250427
【氏名又は名称】日立ＧＥニュークリア・エナジー株式会社
(74)【代理人】
【識別番号】110001807
【氏名又は名称】弁理士法人磯野国際特許商標事務所
(72)【発明者】
【氏名】藤 大樹
(72)【発明者】
【氏名】木村 宣隆
(72)【発明者】
【氏名】伊藤 潔人
【テーマコード（参考）】
5H301
【Ｆターム（参考）】
5H301AA01
5H301AA10
5H301GG08
5H301GG09
(57)【要約】
【課題】不整地を走行する移動体に対し、高性能な自己位置推定と環境地図作成を可能とする。
【解決手段】 移動体制御システム１００は、取得されたフレームの点群とオンライン環境地図１３１との対応付けを行い、移動体２００の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するデータ対応付け部１１２、移動体２００の移動量を算出する移動量算出部１１３と、所定のタイミングで取得されたフレームをキーフレーム生成元として検出するキーフレーム生成元検出部１１４と、当該タイミングの過去または前後に取得され、フレーム指標が所定の指標値を満たすフレームを選択するフレーム選択部１１５と、選択されたフレームを重ね合わせてキーフレームを基に移動体２００の移動量を補正する移動量補正部１１７と、を備える。
【選択図】図３
【解決手段】 移動体制御システム１００は、取得されたフレームの点群とオンライン環境地図１３１との対応付けを行い、移動体２００の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するデータ対応付け部１１２、移動体２００の移動量を算出する移動量算出部１１３と、所定のタイミングで取得されたフレームをキーフレーム生成元として検出するキーフレーム生成元検出部１１４と、当該タイミングの過去または前後に取得され、フレーム指標が所定の指標値を満たすフレームを選択するフレーム選択部１１５と、選択されたフレームを重ね合わせてキーフレームを基に移動体２００の移動量を補正する移動量補正部１１７と、を備える。
【選択図】図３
【特許請求の範囲】
【請求項1】
移動体の周囲環境にある物体を示す点群データであるフレームを取得するデータ取得部と、
前記フレームの点群を基に前記周囲環境の地図であるオンライン環境地図を生成する環境地図作成部と、
前記データ取得部が取得したフレームの点群と生成済みの前記オンライン環境地図との対応付けを行い、前記周囲環境における前記移動体の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するデータ対応付け部と、
前記位置姿勢情報を基に前記移動体の移動量を算出する移動量算出部と、
所定のタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出するキーフレーム生成元検出部と、
前記キーフレーム生成元を検出したタイミングより過去に取得されたフレーム、または当該タイミングの前後に取得されたフレームであって、前記フレーム指標が所定の指標値を満たすフレームを選択するフレーム選択部と、
前記フレーム選択部が選択したフレームを重ね合わせてキーフレームを生成するキーフレーム生成部と、
前記キーフレームを基に前記移動体の移動量を補正する移動量補正部と、を備える
移動体制御システム。
前記フレームの点群を基に前記周囲環境の地図であるオンライン環境地図を生成する環境地図作成部と、
前記データ取得部が取得したフレームの点群と生成済みの前記オンライン環境地図との対応付けを行い、前記周囲環境における前記移動体の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するデータ対応付け部と、
前記位置姿勢情報を基に前記移動体の移動量を算出する移動量算出部と、
所定のタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出するキーフレーム生成元検出部と、
前記キーフレーム生成元を検出したタイミングより過去に取得されたフレーム、または当該タイミングの前後に取得されたフレームであって、前記フレーム指標が所定の指標値を満たすフレームを選択するフレーム選択部と、
前記フレーム選択部が選択したフレームを重ね合わせてキーフレームを生成するキーフレーム生成部と、
前記キーフレームを基に前記移動体の移動量を補正する移動量補正部と、を備える
移動体制御システム。
【請求項2】
前記環境地図作成部は、
補正された移動量を基にオフライン環境地図を生成し、当該オフライン環境地図を基に前記オンライン環境地図を更新する
請求項１に記載の移動体制御システム。
補正された移動量を基にオフライン環境地図を生成し、当該オフライン環境地図を基に前記オンライン環境地図を更新する
請求項１に記載の移動体制御システム。
【請求項3】
前記フレーム指標は、
前記データ対応付け部が行う前記フレームの点群と前記オンライン環境地図との対応付けの良否の程度である
請求項１に記載の移動体制御システム。
前記データ対応付け部が行う前記フレームの点群と前記オンライン環境地図との対応付けの良否の程度である
請求項１に記載の移動体制御システム。
【請求項4】
前記フレーム指標は、
前記フレームの点群から検出される前記周囲環境にあるランドマークの有無、または、ランドマークとなる物体の数である
請求項１に記載の移動体制御システム。
前記フレームの点群から検出される前記周囲環境にあるランドマークの有無、または、ランドマークとなる物体の数である
請求項１に記載の移動体制御システム。
【請求項5】
前記データ取得部は、
前記移動体の位置姿勢の変化速度または姿勢を示すモーション値を取得し、
前記フレームのフレーム指標は、
当該フレームを取得した時点の前記モーション値であり、
前記フレーム指標が前記所定の指標値を満たすフレームとは、
前記フレーム指標が前記所定の指標値以内のフレームである
請求項１に記載の移動体制御システム。
前記移動体の位置姿勢の変化速度または姿勢を示すモーション値を取得し、
前記フレームのフレーム指標は、
当該フレームを取得した時点の前記モーション値であり、
前記フレーム指標が前記所定の指標値を満たすフレームとは、
前記フレーム指標が前記所定の指標値以内のフレームである
請求項１に記載の移動体制御システム。
【請求項6】
前記データ取得部は、
前記移動体の周囲環境の画像を取得し、
前記フレームのフレーム指標は、
当該フレームを取得した時点の前記画像から検出される前記周囲環境にあるランドマークとなる物体の数、
当該フレームを取得した時点の前記画像から検出される特徴点の検出スコア、
当該フレームを取得した時点の前記画像から検出される特徴点の数、および、
当該フレームを取得した時点の前記画像から検出されるテクスチャの有無の、何れか少なくとも１つを基に算出される
請求項１に記載の移動体制御システム。
前記移動体の周囲環境の画像を取得し、
前記フレームのフレーム指標は、
当該フレームを取得した時点の前記画像から検出される前記周囲環境にあるランドマークとなる物体の数、
当該フレームを取得した時点の前記画像から検出される特徴点の検出スコア、
当該フレームを取得した時点の前記画像から検出される特徴点の数、および、
当該フレームを取得した時点の前記画像から検出されるテクスチャの有無の、何れか少なくとも１つを基に算出される
請求項１に記載の移動体制御システム。
【請求項7】
初期の前記所定の指標値は、
前記移動体の初期位置におけるフレーム指標であり、
所定数の前記フレームが取得された後の前記所定の指標値は、
前記フレームの取得時点より過去に取得されたフレームのフレーム指標の平均値、
当該平均値との差が所定値以内の値、
前記フレームの取得時点前後の所定時間内に取得されたフレームのフレーム指標の平均値、または、
当該平均値との差が所定値以内の値である
請求項１に記載の移動体制御システム。
前記移動体の初期位置におけるフレーム指標であり、
所定数の前記フレームが取得された後の前記所定の指標値は、
前記フレームの取得時点より過去に取得されたフレームのフレーム指標の平均値、
当該平均値との差が所定値以内の値、
前記フレームの取得時点前後の所定時間内に取得されたフレームのフレーム指標の平均値、または、
当該平均値との差が所定値以内の値である
請求項１に記載の移動体制御システム。
【請求項8】
前記所定の指標値は、
説明変数が前記点群データであり、目的変数が当該点群データのフレーム指標である学習データを用いて生成された機械学習モデルに、前記点群データを説明変数として入力して得られるフレーム指標、または、
当該フレーム指標との差が所定値以内の値である
請求項１に記載の移動体制御システム。
説明変数が前記点群データであり、目的変数が当該点群データのフレーム指標である学習データを用いて生成された機械学習モデルに、前記点群データを説明変数として入力して得られるフレーム指標、または、
当該フレーム指標との差が所定値以内の値である
請求項１に記載の移動体制御システム。
【請求項9】
前記位置姿勢情報と、当該位置姿勢情報に対応する地点で過去に取得されたフレームのフレーム指標との対応表を記憶する記憶部を備え、
前記所定の指標値は、
前記対応表において前記フレームの取得された地点に対応するフレーム指標の平均値、または、
当該平均値との差が所定値以内の値である
請求項１に記載の移動体制御システム。
前記所定の指標値は、
前記対応表において前記フレームの取得された地点に対応するフレーム指標の平均値、または、
当該平均値との差が所定値以内の値である
請求項１に記載の移動体制御システム。
【請求項10】
前記移動体の位置姿勢を制御する移動体制御部をさらに備え、
前記移動体制御部は、
前記キーフレーム生成元検出部がキーフレーム生成元を検出した地点で前記移動体が静止または概ね静止するように制御し、
前記フレーム選択部は、
静止または概ね静止中に取得されたフレームを選択し、
前記キーフレーム生成部は、
選択されたフレームを重ね合わせてキーフレームを生成する
請求項１に記載の移動体制御システム。
前記移動体制御部は、
前記キーフレーム生成元検出部がキーフレーム生成元を検出した地点で前記移動体が静止または概ね静止するように制御し、
前記フレーム選択部は、
静止または概ね静止中に取得されたフレームを選択し、
前記キーフレーム生成部は、
選択されたフレームを重ね合わせてキーフレームを生成する
請求項１に記載の移動体制御システム。
【請求項11】
前記移動体の位置姿勢を制御する移動体制御部をさらに備え、
前記移動体制御部は、
前記所定の指標値を満たすフレームが取得された地点において、前記移動体が静止または概ね静止するように制御し、
前記フレーム選択部は、
静止または概ね静止中に取得されたフレームを選択し、
前記キーフレーム生成部は、
選択されたフレームを重ね合わせてキーフレームを生成する
請求項１に記載の移動体制御システム。
前記移動体制御部は、
前記所定の指標値を満たすフレームが取得された地点において、前記移動体が静止または概ね静止するように制御し、
前記フレーム選択部は、
静止または概ね静止中に取得されたフレームを選択し、
前記キーフレーム生成部は、
選択されたフレームを重ね合わせてキーフレームを生成する
請求項１に記載の移動体制御システム。
【請求項12】
前記移動体の位置姿勢を制御する移動体制御部をさらに備え、
前記移動体制御部は、
初期地点において、前記データ取得部が複数の前記フレームを取得するように前記移動体を制御する
請求項１に記載の移動体制御システム。
前記移動体制御部は、
初期地点において、前記データ取得部が複数の前記フレームを取得するように前記移動体を制御する
請求項１に記載の移動体制御システム。
【請求項13】
前記移動体の位置姿勢を制御する移動体制御部をさらに備え、
前記移動体制御部は、
前記フレームの点群に３次元形状の特徴点が検出される地点を複数回訪れるように前記移動体を制御する
請求項１に記載の移動体制御システム。
前記移動体制御部は、
前記フレームの点群に３次元形状の特徴点が検出される地点を複数回訪れるように前記移動体を制御する
請求項１に記載の移動体制御システム。
【請求項14】
前記所定のタイミングは、
累積の移動量が所定の移動量に達したタイミング、または所定時間経過したタイミングである
請求項１に記載の移動体制御システム。
累積の移動量が所定の移動量に達したタイミング、または所定時間経過したタイミングである
請求項１に記載の移動体制御システム。
【請求項15】
移動体を制御する移動体制御システムが、
前記移動体の周囲環境にある物体を示す点群データであるフレームを取得するステップと、
前記フレームの点群を基に前記周囲環境の地図であるオンライン環境地図を生成するステップと、
取得されたフレームの点群と生成済みの前記オンライン環境地図との対応付けを行い、前記周囲環境における前記移動体の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するステップと、
前記位置姿勢情報を基に前記移動体の移動量を算出するステップと、
所定のタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出するステップと、
前記キーフレーム生成元を検出したタイミングより過去に取得されたフレーム、または当該タイミングの前後に取得されたフレームであって、前記フレーム指標が所定の指標値を満たすフレームを選択するステップと、
選択されたフレームを重ね合わせてキーフレームを生成するステップと、
前記キーフレームを基に前記移動体の移動量を補正するステップと、を実行する
移動体制御方法。
前記移動体の周囲環境にある物体を示す点群データであるフレームを取得するステップと、
前記フレームの点群を基に前記周囲環境の地図であるオンライン環境地図を生成するステップと、
取得されたフレームの点群と生成済みの前記オンライン環境地図との対応付けを行い、前記周囲環境における前記移動体の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するステップと、
前記位置姿勢情報を基に前記移動体の移動量を算出するステップと、
所定のタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出するステップと、
前記キーフレーム生成元を検出したタイミングより過去に取得されたフレーム、または当該タイミングの前後に取得されたフレームであって、前記フレーム指標が所定の指標値を満たすフレームを選択するステップと、
選択されたフレームを重ね合わせてキーフレームを生成するステップと、
前記キーフレームを基に前記移動体の移動量を補正するステップと、を実行する
移動体制御方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の自己位置推定と環境地図作成を同時に行う移動体制御システムおよび移動体制御方法に関する。
【背景技術】
【0002】
傾斜のある床面、瓦礫や路面の大きな起伏のある不整地などを自律移動する移動ロボットや無人搬送車（ＡＧＶ：Automatic Guided Vehicle）などの移動体は、路面の起伏や障害物などを含む周囲環境を３次元計測する必要がある。このような環境計測技術として、周囲環境の計測情報を基に、環境地図作成と自己位置推定を同時に行うＳＬＡＭ（Simultaneous Localization and Mapping）技術が知られている。
【0003】
例えば非特許文献１には、測距センサと画像センサとを併用したＳＬＡＭ技術が提案されている。一方、特許文献について特許文献１には、測距センサと画像センサとを併用し、計算効率の高いＳＬＡＭ技術が記載されている。また特許文献１には、センサから得られるデータ（フレーム）の点群が疎でフレーム間の位置合わせが困難な場合、一定移動量の検出時や一定時間の経過時などのタイミングにおいて、過去の複数フレームの点群を重ね合わせて統合し、新たにフレーム（統合フレーム）を生成する手法が記載されている。この手法により、分解能が高い統合フレームの点群を構築し、この統合フレームの点群同士で位置合わせを行えば、位置合わせが行いやすくなるという。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開２０１７－１１７３８６号公報
【非特許文献】
【0005】
【非特許文献1】J. Zhang and S. Singh, "Visual-lidar Odometry and Mapping, Low-drift, Robust, and Fast," ICRA 2015, pp.2174-2181, 2015.
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかしながら、不整地を走行する移動体の位置姿勢は随時変動するため、過去の複数フレームを単純に重ねて統合しただけでは、統合フレームの点群に位置合わせ精度の低い点群が含まれる。このため、統合フレーム間の位置合わせにおける位置合わせ精度の劣化につながり、正確な環境地図の作成が難しくなる。また不整地における移動では、路面の起伏の大きさを考慮した経路を計画し、それに基づいて移動するため、３次元的かつ分解能高い路面情報の認識が必要となる。
本発明は、このような背景を鑑みてなされたものであり、不整地を走行する移動体に対し、高性能な自己位置推定を可能とする移動体制御システムおよび移動体制御方法を提供することを課題とする。
本発明は、このような背景を鑑みてなされたものであり、不整地を走行する移動体に対し、高性能な自己位置推定を可能とする移動体制御システムおよび移動体制御方法を提供することを課題とする。
【課題を解決するための手段】
【0007】
上記した課題を解決するため、本発明に係る移動体制御システムは、移動体の周囲環境にある物体を示す点群データであるフレームを取得するデータ取得部と、前記フレームの点群を基に前記周囲環境の地図であるオンライン環境地図を生成する環境地図作成部と、前記データ取得部が取得したフレームの点群と生成済みの前記オンライン環境地図との対応付けを行い、前記周囲環境における前記移動体の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するデータ対応付け部と、前記位置姿勢情報を基に前記移動体の移動量を算出する移動量算出部と、所定のタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出するキーフレーム生成元検出部と、前記キーフレーム生成元を検出したタイミングより過去に取得されたフレーム、または当該タイミングの前後に取得されたフレームであって、前記フレーム指標が所定の指標値を満たすフレームを選択するフレーム選択部と、前記フレーム選択部が選択したフレームを重ね合わせてキーフレームを生成するキーフレーム生成部と、前記キーフレームを基に前記移動体の移動量を補正する移動量補正部と、を備える。
【発明の効果】
【0008】
本発明によれば、不整地を走行する移動体に対し、高性能な自己位置推定を可能とする移動体制御システムおよび移動体制御方法を提供することができる。上記した以外の課題、構成および効果は、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【0009】
【図1】第１実施形態に係る移動体の構成図である。
【図2】第１実施形態に係る移動体の構成図である。
【図3】第１実施形態に係る移動体に備わる移動体制御システムの機能ブロック図である。
【図4】第１実施形態に係るループ閉じ込め補正手法を説明するための図である。
【図5】第１実施形態に係るオンライン環境地図作成処理のフローチャートである。
【図6】第１実施形態に係るオフライン環境地図作成処理のフローチャートである。
【図7】第１実施形態に係る初期フレームの点群を示す図である。
【図8】第１実施形態に係る初期位置で取得された複数フレームの点群を重ね合わせて生成された点群を示す図である。
【図9】第２実施形態に係る移動体に備わる移動体制御システムの機能ブロック図である。
【図10】第３実施形態に係る移動体に備わる移動体制御システムの機能ブロック図である。
【発明を実施するための形態】
【0010】
≪移動体制御システムの概要≫
以下に本発明を実施するための形態（実施形態）における移動体制御システムについて説明する。移動体制御システムは、測距センサや移動のためのアクチュエータを備える移動体に搭載される。移動体制御システムは、移動体の周囲にある物体の点群データ（フレームとも記す）を測距センサから周期的に取得して、取得時刻が異なる点群同士をマッチングして移動量を算出するとともに、環境地図（後記するオンライン環境地図）を作成する。なお移動体の移動量を算出することは、移動体の位置姿勢情報を算出（推定）する自己位置推定を行うことになる。このため移動体制御システムは、ＳＬＡＭを行うシステムとなる。
以下に本発明を実施するための形態（実施形態）における移動体制御システムについて説明する。移動体制御システムは、測距センサや移動のためのアクチュエータを備える移動体に搭載される。移動体制御システムは、移動体の周囲にある物体の点群データ（フレームとも記す）を測距センサから周期的に取得して、取得時刻が異なる点群同士をマッチングして移動量を算出するとともに、環境地図（後記するオンライン環境地図）を作成する。なお移動体の移動量を算出することは、移動体の位置姿勢情報を算出（推定）する自己位置推定を行うことになる。このため移動体制御システムは、ＳＬＡＭを行うシステムとなる。
【0011】
また移動体制御システムは、移動量や時間を基に算出される所定のタイミングより過去、または当該タイミングの前後に取得されたフレームであって、所定の指標を満たすフレームの点群を重ね合わせて点群データ（キーフレーム）を生成する。指標（後記するフレーム指標）は点群の位置精度を示す。指標の例としてフレームの点群と環境地図との位置合わせのスコアがあり、移動体制御システムは当該スコアが所定値を満たすフレームの点群を重ね合わせてキーフレームを生成する。
【0012】
移動体制御システムは、キーフレームの点群同士マッチングして移動量を補正し、さらに環境地図を更新する。所定の指標を満たすフレーム（点群データ）の精度は、他のフレームより高いことが期待できるため、移動体制御システムは高精度の移動量を算出（自己位置推定）でき、高精度の環境地図を作成することができるようになる。
【0013】
≪移動体の構成≫
図１は、第１実施形態に係る移動体２００の構成図である。移動体２００は、多脚型のロボットであって、舗装された通路や道路の他、凸凹のある道や物が散乱した不整地を問わず、また屋内、屋外問わず様々な環境（周囲環境とも記す）を移動する。移動体２００の移動手段は、脚以外であってもよい。図２は、第１実施形態に係る移動体２００の構成図である。図２に示すように、移動手段はキャタピラ（無限軌道）であってもよいし、車輪であってもよい。移動体２００は、自律的に移動してもよいし、人手による遠隔操作によって移動してもよい。
図１は、第１実施形態に係る移動体２００の構成図である。移動体２００は、多脚型のロボットであって、舗装された通路や道路の他、凸凹のある道や物が散乱した不整地を問わず、また屋内、屋外問わず様々な環境（周囲環境とも記す）を移動する。移動体２００の移動手段は、脚以外であってもよい。図２は、第１実施形態に係る移動体２００の構成図である。図２に示すように、移動手段はキャタピラ（無限軌道）であってもよいし、車輪であってもよい。移動体２００は、自律的に移動してもよいし、人手による遠隔操作によって移動してもよい。
【0014】
移動体２００には、測距センサ２２０、外部装置と通信データを送受信する通信部２８０、アクチュエータ２７０がさらに備える。アクチュエータ２７０は、キャタピラや多脚の他に、ドローンなどの飛行体の駆動装置を対象とする。移動体２００にアームを取り付けて作業を行える作業ロボットとしてもよく、移動体の構造については、作業目的に応じて変更可能である。
【0015】
測距センサ２２０は、周囲環境の３次元点群が計測可能であり、例えばＬｉＤＡＲ（Light Detection and Ranging）センサ、ＴＯＦ（Time of Flight）カメラ、ステレオカメラなどである。他に画像センサを用いて取得した画像を基に、Lucas-Kanade法などの特徴点検出で特徴点を検出し、オプティカルフローによってその特徴点追跡を行う手法、ＳＩＦＴ（Scale-Invariant Feature Transform）やＯＲＢ（Oriented FAST and Rotated BRIEF）の特徴量に基づく特徴点照合手法を用いて、単眼の移動に基づいた三角測量を行うことにより、３次元点群を取得してもよい。
【0016】
≪移動体制御システムの構成≫
図３は、第１実施形態に係る移動体２００に備わる移動体制御システム１００の機能ブロック図である。移動体制御システム１００は例えばコンピュータであり、演算部１１０、および記憶部１３０を備える。演算部１１０は、ＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphics Processing Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＡＳＩＣ（Application Specific Integrated Circuit）などのマイクロプロセッサを含んで構成される。演算部１１０には、データ取得部１１１、データ対応付け部１１２、移動量算出部１１３、キーフレーム生成元検出部１１４、フレーム選択部１１５、キーフレーム生成部１１６、移動量補正部１１７、環境地図作成部１１８、および移動体制御部１１９が備わる。
移動体制御部１１９は、アクチュエータ２７０を制御することで、移動体２００の位置姿勢を制御する。移動体制御部１１９以外の演算部１１０に備わる機能部の詳細は後記する。
図３は、第１実施形態に係る移動体２００に備わる移動体制御システム１００の機能ブロック図である。移動体制御システム１００は例えばコンピュータであり、演算部１１０、および記憶部１３０を備える。演算部１１０は、ＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphics Processing Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＡＳＩＣ（Application Specific Integrated Circuit）などのマイクロプロセッサを含んで構成される。演算部１１０には、データ取得部１１１、データ対応付け部１１２、移動量算出部１１３、キーフレーム生成元検出部１１４、フレーム選択部１１５、キーフレーム生成部１１６、移動量補正部１１７、環境地図作成部１１８、および移動体制御部１１９が備わる。
移動体制御部１１９は、アクチュエータ２７０を制御することで、移動体２００の位置姿勢を制御する。移動体制御部１１９以外の演算部１１０に備わる機能部の詳細は後記する。
【0017】
以上に説明したように移動体制御システム１００は、移動体２００の位置姿勢を制御する移動体制御部１１９を備える。
【0018】
≪記憶部≫
記憶部１３０は、ＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）、フラッシュメモリなどの記憶機器を含んで構成される。記憶部１３０には、フレーム情報データベース１４０、キーフレームデータベース１５０、オンライン環境地図１３１、オフライン環境地図１３２、およびプログラム１３８が記憶される。
記憶部１３０は、ＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）、フラッシュメモリなどの記憶機器を含んで構成される。記憶部１３０には、フレーム情報データベース１４０、キーフレームデータベース１５０、オンライン環境地図１３１、オフライン環境地図１３２、およびプログラム１３８が記憶される。
【0019】
フレーム情報データベース１４０には、フレーム（点群データ）の他にフレームに係るデータが当該フレームに関連付けられて記憶される。キーフレームデータベース１５０には、後記されるキーフレームに係るデータが記憶される。オンライン環境地図１３１は、移動体２００の周囲環境にある物体（障害物を含む）を示す地図であり、点群データを基に作成される。オフライン環境地図１３２は、後記されるキーフレームを基に作成される移動体２００の周囲環境にある物体を示す地図である。プログラム１３８は、演算部１１０の構成要素である各機能部の処理手順の記述を含む。
【0020】
≪演算部：データ取得部、データ対応付け部、移動量算出部、環境地図作成部≫
データ取得部１１１は、測距センサ２２０から点群データ（フレーム）を周期的に取得して、フレーム情報データベース１４０に格納する。
データ取得部１１１は、測距センサ２２０から点群データ（フレーム）を周期的に取得して、フレーム情報データベース１４０に格納する。
【0021】
以上に説明したように移動体制御システム１００は、移動体２００の周囲環境にある物体を示す点群データであるフレームを取得するデータ取得部１１１を備える。
【0022】
データ対応付け部１１２は、フレームの点群をオンライン環境地図１３１に対応付ける。対応付け（位置合わせ）の手法として、例えば、ＩＣＰ（Iterative Closest Point）やＮＤＴ（Normal Distributions Transform）などがある。データ対応付け部１１２は、対応付けにともない算出される対応付け（位置合わせ）の良否の程度を示すスコア（対応付けスコア、位置合わせスコア）や移動体２００の位置姿勢情報を、フレームに関連付けてフレーム情報データベース１４０に格納する。対応付けスコアは、フレーム指標の１つの例である。
【0023】
ＩＣＰやＮＤＴは一例であり、スコアが算出可能な手法であれば、データ対応付け部１１２は他の対応付け手法を用いてもよい。データ対応付け部１１２の対応付けにより、オンライン環境地図１３１における移動体２００の位置（位置姿勢情報）が算出でき、自己位置推定が可能となる。
【0024】
以上に説明したように移動体制御システム１００は、データ取得部１１１が取得したフレームの点群と生成済み（生成途中）のオンライン環境地図１３１との対応付けを行い、周囲環境における移動体２００の位置姿勢情報、および当該点群の精度を示すフレーム指標を算出するデータ対応付け部１１２を備える。
【0025】
移動量算出部１１３は、データ対応付け部１１２が算出した位置姿勢情報を基に、移動体２００の位置姿勢の変動量（移動量、オドメトリ）を算出し、フレームに関連付けてフレーム情報データベース１４０に格納する。移動量算出部１１３は、例えば１つ前のフレーム取得時点からの移動量を算出する。
【0026】
以上に説明したように移動体制御システム１００は、位置姿勢情報を基に移動体２００の移動量を算出する移動量算出部１１３を備える。
【0027】
環境地図作成部１１８は、移動量算出部１１３が算出した移動量を基に、オンライン環境地図１３１を更新しながら作成する。詳しくは、環境地図作成部１１８は、フレームごとに算出された移動量を基に、フレームが示す点群に３次元変換処理を施し、既存のオンライン環境地図１３１に重ねて統合していくことで、徐々にオンライン環境地図１３１を作成する。
【0028】
ＩＣＰやＮＤＴなどの対応付け（位置合わせ）手法は、一般に対応付けを行う点群同士の距離が近い必要がある。フレームの取得間隔が長くなるほど、移動量が大きくなる可能性があるため、対応付けの際に局所解に陥るなど対応付けの誤差の要因となる。このため、オンライン環境地図１３１の作成では、測距センサ２２０のフレーム取得周期内で処理可能な対応付け手法を用いて高速に対応付け処理を行うことが望ましい。測距センサ２２０のフレームレートが２０Ｈｚであれば、４０ｍｓ以内に処理可能な対応付け手法を用いるとよい。取得したフレームの点群が粗い場合であっても、対応付け先のオンライン環境地図１３１の分解能が高いため、位置合わせを行うことが可能であるが、逐次的に対応付けを行うため、誤差が蓄積していく傾向にある。
【0029】
以上に説明したように移動体制御システム１００は、フレームの点群を基に周囲環境の地図であるオンライン環境地図１３１を生成する環境地図作成部１１８を備える。
【0030】
≪演算部：キーフレーム生成元検出部、フレーム選択部、キーフレーム生成部、移動量補正部、環境地図作成部≫
キーフレーム生成元検出部１１４は、移動量算出部１１３が算出した移動量を基に移動量の累積値が所定値に達したタイミング、または所定の経過時間に達したタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出し、フレーム選択部１１５に出力する。移動量の累積値、および経過時間とは、移動量算出部１１３がキーフレームを前回検出してからの移動量の累積値、および経過時間である。換言すればキーフレーム生成元を検出することは、前回のキーフレーム生成元の検出から所定距離移動した、または所定時間が経過したということである。キーフレーム生成元検出部１１４は、他のタイミングでキーフレームを検出してもよい。
キーフレーム生成元検出部１１４は、移動量算出部１１３が算出した移動量を基に移動量の累積値が所定値に達したタイミング、または所定の経過時間に達したタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出し、フレーム選択部１１５に出力する。移動量の累積値、および経過時間とは、移動量算出部１１３がキーフレームを前回検出してからの移動量の累積値、および経過時間である。換言すればキーフレーム生成元を検出することは、前回のキーフレーム生成元の検出から所定距離移動した、または所定時間が経過したということである。キーフレーム生成元検出部１１４は、他のタイミングでキーフレームを検出してもよい。
【0031】
以上に説明したように移動体制御システム１００は、所定のタイミングで取得されたフレームをキーフレーム生成元として繰り返し検出するキーフレーム生成元検出部１１４を備える。
所定のタイミングは、累積の移動量が所定の移動量に達したタイミング、または所定時間経過したタイミングである。
所定のタイミングは、累積の移動量が所定の移動量に達したタイミング、または所定時間経過したタイミングである。
【0032】
フレーム選択部１１５は、キーフレーム生成元を含めキーフレームより過去または前後に取得されたフレームのなかで、フレーム指標が所定値を満たすフレームを所定数選択する。フレーム指標は、例えばデータ対応付け部１１２が算出する対応付けスコア（位置合わせスコア）である。後記するように所定値（閾値、所定の指標値）は、固定値とは限らず、変動してもよい。なおフレーム指標が所定値を満たすフレームは、点群の位置合わせの精度が高く、点群の位置の精度が高いと期待できる。
【0033】
以上に説明したように移動体制御システム１００は、キーフレーム生成元を検出したタイミングより過去に取得されたフレーム、または当該タイミングの前後に取得されたフレームであって、フレーム指標が所定の指標値（所定値、閾値）を満たすフレームを選択するフレーム選択部１１５を備える。
フレーム指標は、データ対応付け部１１２が行うフレームの点群とオンライン環境地図１３１との対応付けの良否の程度である。
フレーム指標は、データ対応付け部１１２が行うフレームの点群とオンライン環境地図１３１との対応付けの良否の程度である。
【0034】
キーフレーム生成部１１６は、フレーム選択部１１５が選択したフレームの点群を重ね合わせてキーフレームを生成して、キーフレームデータベース１５０に格納する。キーフレーム生成に用いられるフレームはフレーム指標が所定値を満たすフレームであるため、キーフレームにある点群の位置の精度も高いと考えられる。
【0035】
以上に説明したように移動体制御システム１００は、フレーム選択部１１５が選択したフレームを重ね合わせてキーフレームを生成するキーフレーム生成部１１６を備える。
【0036】
移動量補正部１１７は、キーフレームデータベース１５０にあるキーフレームを基に移動量を補正する。例えば移動量補正部１１７は、ループ閉じ込め補正手法を用いる。詳しくは、移動量補正部１１７はキーフレームに対応する移動体２００の位置姿勢が近い２つのキーフレーム（ループ辺の両端となるキーフレーム、以下固定キーフレームとも記す）を検出する。移動量補正部１１７は、固定キーフレームの点群の対応付け（位置合わせ）により相対位置姿勢情報を算出し、一方の固定キーフレームを固定して他方の固定キーフレームを補正して、さらにループ辺の他のキーフレームを補正する。
【0037】
図４は、第１実施形態に係るループ閉じ込め補正手法を説明するための図である。実線の丸は補正前のキーフレームに対応する位置姿勢を示し、矢印は移動体２００の移動（移動量）を示す。移動体２００は、位置姿勢２９１で示される位置から反時計回りに移動して、位置姿勢２９２で示される位置に移動する。位置姿勢２９１，２９２は近く、位置姿勢２９１，２９２に対応するキーフレームが固定キーフレームとなる。
点線の丸は補正後のキーフレームに対応する位置姿勢を示す。位置姿勢２９２が位置姿勢２９２Ａに補正されたことにともない、位置姿勢２９１，２９２に対応するキーフレームに間にあるキーフレームも補正される。
点線の丸は補正後のキーフレームに対応する位置姿勢を示す。位置姿勢２９２が位置姿勢２９２Ａに補正されたことにともない、位置姿勢２９１，２９２に対応するキーフレームに間にあるキーフレームも補正される。
【0038】
なおループ閉じ込め補正手法に用いる地点数（固定フレーム数）は必ずしも２つではなく、３つ以上であってもよい。近年では、キーフレームにおける位置姿勢情報をノードとし、その相対位置姿勢をエッジとする、グラフ最適化技術を用いた手法が提案されている。移動量補正部１１７は、このような技術を用いて補正をしてもよい。但しループ閉じ込みに用いる地点について、２地点のキーフレームの点群が粗い場合には、対応付けが困難となる。このため複数の地点が近いだけでなく、それらの地点における点群の分解能が高いことが望ましい。
【0039】
以上に説明したように移動体制御システム１００は、キーフレームを基に移動体２００の移動量を補正する移動量補正部１１７を備える。
【0040】
環境地図作成部１１８は、移動量補正部１１７が補正したキーフレームを基に、オフライン環境地図１３２を更新しながら作成する。詳しくは、環境地図作成部１１８は、キーフレームごとに算出された補正された移動量を基に、キーフレームが示す点群に３次元変換処理を施し、既存のオフライン環境地図１３２に重ねて統合していくことで、徐々にオフライン環境地図１３２を作成する。さらに環境地図作成部１１８は、オフライン環境地図１３２を基にオンライン環境地図１３１を更新して作成する。
【0041】
【0042】
≪オンライン環境地図作成処理≫
図５は、第１実施形態に係るオンライン環境地図作成処理のフローチャートである。図５を参照しながら、オンライン環境地図１３１の作成処理を説明する。
ステップＳ１１において環境地図作成部１１８は、周囲環境の計測終了までステップＳ１２～Ｓ１５を繰り返す処理を開始する。計測終了の判断は、例えば作成済みのオンライン環境地図１３１やオフライン環境地図１３２を参照して移動体の運用者が行う終了の指示に従った判断であってもよい。
図５は、第１実施形態に係るオンライン環境地図作成処理のフローチャートである。図５を参照しながら、オンライン環境地図１３１の作成処理を説明する。
ステップＳ１１において環境地図作成部１１８は、周囲環境の計測終了までステップＳ１２～Ｓ１５を繰り返す処理を開始する。計測終了の判断は、例えば作成済みのオンライン環境地図１３１やオフライン環境地図１３２を参照して移動体の運用者が行う終了の指示に従った判断であってもよい。
【0043】
ステップＳ１２においてデータ取得部１１１は、測距センサ２２０からフレーム（点群データ）を取得して、フレーム情報データベース１４０に格納する。
ステップＳ１３においてデータ対応付け部１１２は、ステップＳ１２で取得したフレームの点群をオンライン環境地図１３１に対応付ける。さらにデータ対応付け部１１２は、対応付けにともない算出された位置姿勢情報や位置合わせスコアを、フレームに関連付けてフレーム情報データベース１４０に格納する。
ステップＳ１３においてデータ対応付け部１１２は、ステップＳ１２で取得したフレームの点群をオンライン環境地図１３１に対応付ける。さらにデータ対応付け部１１２は、対応付けにともない算出された位置姿勢情報や位置合わせスコアを、フレームに関連付けてフレーム情報データベース１４０に格納する。
【0044】
ステップＳ１４において移動量算出部１１３は、ステップＳ１３で算出された位置姿勢情報を基に、移動体２００の位置姿勢の変動量（移動量、オドメトリ）を算出する。
ステップＳ１５において環境地図作成部１１８は、ステップＳ１４で算出された移動量を基に、オンライン環境地図１３１を更新して作成する。
ステップＳ１５において環境地図作成部１１８は、ステップＳ１４で算出された移動量を基に、オンライン環境地図１３１を更新して作成する。
【0045】
≪オフライン環境地図作成処理≫
図６は、第１実施形態に係るオフライン環境地図作成処理のフローチャートである。図６を参照しながら、オフライン環境地図１３２の作成処理を説明する。なお、図５記載のオンライン環境地図作成処理とオフライン環境地図作成処理とは、同時並行に処理される。
ステップＳ２１において環境地図作成部１１８は、周囲環境の計測終了までステップＳ２２～Ｓ２８を繰り返す処理を開始する。
図６は、第１実施形態に係るオフライン環境地図作成処理のフローチャートである。図６を参照しながら、オフライン環境地図１３２の作成処理を説明する。なお、図５記載のオンライン環境地図作成処理とオフライン環境地図作成処理とは、同時並行に処理される。
ステップＳ２１において環境地図作成部１１８は、周囲環境の計測終了までステップＳ２２～Ｓ２８を繰り返す処理を開始する。
【0046】
ステップＳ２２においてキーフレーム生成元検出部１１４は、キーフレーム生成元を検出すれば（ステップＳ２２→ＹＥＳ）ステップＳ２３に進み、未検出ならば（ステップＳ２２→ＮＯ）ステップＳ２２に戻る。
ステップＳ２３においてフレーム選択部１１５は、キーフレーム生成元を含め、キーフレーム生成元より過去または前後に取得されたフレームのなかで、フレーム指標が所定値を満たすフレームを所定数選択する。
ステップＳ２４においてキーフレーム生成部１１６は、ステップＳ２３で選択されたフレームの点群を重ね合わせてキーフレームを生成して、キーフレームデータベース１５０に格納する。
ステップＳ２３においてフレーム選択部１１５は、キーフレーム生成元を含め、キーフレーム生成元より過去または前後に取得されたフレームのなかで、フレーム指標が所定値を満たすフレームを所定数選択する。
ステップＳ２４においてキーフレーム生成部１１６は、ステップＳ２３で選択されたフレームの点群を重ね合わせてキーフレームを生成して、キーフレームデータベース１５０に格納する。
【0047】
ステップＳ２５において移動量補正部１１７は、固定キーフレームを検出すれば（ステップＳ２５→ＹＥＳ）ステップＳ２６に進み、未検出ならば（ステップＳ２５→ＮＯ）ステップＳ２２に戻る。
ステップＳ２６において移動量補正部１１７は、キーフレームを補正する。
ステップＳ２７において環境地図作成部１１８は、ステップＳ２６で補正されたキーフレームを基に、オフライン環境地図１３２を更新する。
ステップＳ２８において環境地図作成部１１８は、ステップＳ２７で更新されたオフライン環境地図１３２を基にオンライン環境地図１３１を更新する。
ステップＳ２６において移動量補正部１１７は、キーフレームを補正する。
ステップＳ２７において環境地図作成部１１８は、ステップＳ２６で補正されたキーフレームを基に、オフライン環境地図１３２を更新する。
ステップＳ２８において環境地図作成部１１８は、ステップＳ２７で更新されたオフライン環境地図１３２を基にオンライン環境地図１３１を更新する。
【0048】
≪移動体制御システムの特徴≫
移動体制御システム１００は、移動体２００の移動量や経過時間を参照して、繰り返しキーフレームを生成する。また移動体制御システム１００は、キーフレームの点群同士をマッチングして移動量を補正し、オフライン環境地図１３２を更新する。キーフレームは、フレーム指標が所定値を満たすフレームを重ね合わせて生成されるので、点群データの精度が高いことが期待でき、高精度のオフライン環境地図１３２が作成される。延いては高分解能で高精度のオンライン環境地図１３１が作成され、高精度の自己位置推定（移動体２００の位置姿勢情報の算出）が可能になる。
移動体制御システム１００は、移動体２００の移動量や経過時間を参照して、繰り返しキーフレームを生成する。また移動体制御システム１００は、キーフレームの点群同士をマッチングして移動量を補正し、オフライン環境地図１３２を更新する。キーフレームは、フレーム指標が所定値を満たすフレームを重ね合わせて生成されるので、点群データの精度が高いことが期待でき、高精度のオフライン環境地図１３２が作成される。延いては高分解能で高精度のオンライン環境地図１３１が作成され、高精度の自己位置推定（移動体２００の位置姿勢情報の算出）が可能になる。
【0049】
≪変形例：初期地点での環境地図≫
データ対応付け部１１２は、フレームの点群をオンライン環境地図１３１に対応付けて移動体２００の位置姿勢情報を算出（自己位置推定）しており、あらかじめオンライン環境地図１３１を保有しておく必要がある。未知／新規の周囲環境などオンライン環境地図１３１がない場合には、初期フレーム（初期位置（計測開始位置）で最初に取得したフレーム）に対して逐次的にデータ対応付け部１１２が対応付けていく。換言すれば、初期フレームを初期のオンライン環境地図１３１とする。
データ対応付け部１１２は、フレームの点群をオンライン環境地図１３１に対応付けて移動体２００の位置姿勢情報を算出（自己位置推定）しており、あらかじめオンライン環境地図１３１を保有しておく必要がある。未知／新規の周囲環境などオンライン環境地図１３１がない場合には、初期フレーム（初期位置（計測開始位置）で最初に取得したフレーム）に対して逐次的にデータ対応付け部１１２が対応付けていく。換言すれば、初期フレームを初期のオンライン環境地図１３１とする。
【0050】
しかしながら、測距センサ２２０から取得した点群データの分解能が低い（粗い）場合（後記する図７参照）には位置合わせは一般に困難となる。このため、データ取得部１１１は移動体制御部１１９に指示して移動体２００を静止または概ね静止して、初期位置で複数フレームを取得し、環境地図作成部１１８がこの複数フレームの点群を重ねることで分解能が高いオンライン環境地図１３１とすることができるようになる。
【0051】
図７は、第１実施形態に係る初期フレームの点群を示す図である。図８は、第１実施形態に係る初期位置で取得された複数フレームの点群を重ね合わせて生成された点群を示す図である。図８に示される点群を初期のオンライン環境地図１３１とすることで、以降のフレームの位置合わせや移動量算出の精度を上げることができるようになる。
【0052】
以上に説明したように移動体制御部１１９は、初期地点において、データ取得部１１１が複数のフレームを取得するように移動体２００を制御する。
【0053】
≪変形例：フレーム指標≫
上記した実施形態において、第１実施形態におけるフレーム指標は、データ対応付け部１１２が算出する位置合わせスコアである。その他に、点群のランドマークを基にしたフレーム指標を用いてもよい。ランドマークとは、周囲環境内にある目印となる特有の構造物や物体である。例えば周囲環境が建屋内だとすると、柱や階段、設置されている設備などがランドマークとなり得る。他に凹凸がある物体がランドマークになり得る。このようなランドマークの検出の有無や検出されたランドマークの個数をフレーム指標としてもよい。
上記した実施形態において、第１実施形態におけるフレーム指標は、データ対応付け部１１２が算出する位置合わせスコアである。その他に、点群のランドマークを基にしたフレーム指標を用いてもよい。ランドマークとは、周囲環境内にある目印となる特有の構造物や物体である。例えば周囲環境が建屋内だとすると、柱や階段、設置されている設備などがランドマークとなり得る。他に凹凸がある物体がランドマークになり得る。このようなランドマークの検出の有無や検出されたランドマークの個数をフレーム指標としてもよい。
【0054】
ランドマークの検出には、点群ベースの物体認識技術の手法を用いる。物体認識技術分野では、深層学習ベースの手法が主流であるが、例えばＰＦＨ（Point Feature Histogram）やＳＨＯＴ（Signature of Histograms of OrienTations）、ＰＰＦ（Point Pair Feature）といった点群の３次元局所特徴量を使った３次元物体認識・検出技術によるランドマーク検出技術を用いてもよい。周囲環境特有の物体の事前情報がない場合は、物体認識・検出技術を用いて検出されたものをその場でランドマークとして追加してもよい。
【0055】
ランドマークをフレーム指標に用いる場合には、点群データから検出されるランドマークの他に、画像センサで取得した画像から検出されるランドマークと組み合わせてフレーム指標とすることが望ましい。また、単にランドマークの有無をフレーム指標とすると、ランドマークが存在しない周囲環境においては、フレーム選択部１１５がフレームを選択しないため、位置合わせスコアのフレーム指標と併用する形でランドマークによるフレーム指標を用いることが望ましい。
【0056】
以上に説明したようにフレーム指標は、フレームの点群から検出される周囲環境にあるランドマークの有無、または、ランドマークとなる物体の数である。
【0057】
≪フレーム指標の所定値（閾値）：固定値≫
フレーム選択部１１５がフレームを選択する際の所定値（閾値）については、周囲環境に応じて所定の固定値を経験的に設定してもよい。他に閾値として、初期位置での環境地図と測距センサ２２０から取得した点群との対応付け後に算出されるフレーム指標（例えば位置合わせスコア）列の平均値であってもよい。閾値は当該平均値にバッファを持たせた閾値（当該平均値との差が所定値以内の値）としてもよい。
フレーム選択部１１５がフレームを選択する際の所定値（閾値）については、周囲環境に応じて所定の固定値を経験的に設定してもよい。他に閾値として、初期位置での環境地図と測距センサ２２０から取得した点群との対応付け後に算出されるフレーム指標（例えば位置合わせスコア）列の平均値であってもよい。閾値は当該平均値にバッファを持たせた閾値（当該平均値との差が所定値以内の値）としてもよい。
【0058】
また周囲環境を移動し、点群の対応付けを行いながら一定期間計測した後は、それまでの一連のフレーム指標の平均値または当該平均値にバッファを持たせた閾値を用いてもよい。但し、フレーム指標には、大きく平均値から外れる外れ値が含まれることがあるため、中央値またはその中央値にバッファを持たせた閾値や、外れ値を除去したフレーム指標の平均値または当該平均値にバッファを持たせた閾値を用いてもよい。
【0059】
≪フレーム指標の所定値（閾値）：移動平均≫
閾値は、移動や周囲環境の変化に応じて変動させてもよい。例えば、計測直後の閾値については、初期位置を基準としたフレーム指標の初期値を設定し、一定フレーム数移動して計測した後は、取得したフレームの過去または前後における連続した所定数の複数フレームについて逐次的にフレーム指標の平均値を算出して、その平均値または当該平均値にバッファを持たせた変動型の閾値を用いてもよい。
閾値は、移動や周囲環境の変化に応じて変動させてもよい。例えば、計測直後の閾値については、初期位置を基準としたフレーム指標の初期値を設定し、一定フレーム数移動して計測した後は、取得したフレームの過去または前後における連続した所定数の複数フレームについて逐次的にフレーム指標の平均値を算出して、その平均値または当該平均値にバッファを持たせた変動型の閾値を用いてもよい。
【0060】
以上に説明したように初期の所定の指標値は、移動体２００の初期位置におけるフレーム指標である。所定数のフレームが取得された後の所定の指標値は、フレームの取得時点より過去に取得されたフレームのフレーム指標の平均値、当該平均値との差が所定値以内の値、フレームの取得時点前後の所定時間内に取得されたフレームのフレーム指標の平均値、または、当該平均値との差が所定値以内の値である。
【0061】
≪フレーム指標の所定値（閾値）：スコアポテンシャルマップ≫
自己位置推定の結果を基に、位置姿勢情報とフレーム指標（例えば位置合わせスコア）とのポテンシャルマップ（対応表）を生成し、データ対応付け部１１２が算出した位置姿勢情報に応じた位置合わせスコアまたは当該スコアにバッファを持たせた値を閾値としてもよい。このような閾値は、同じ周囲環境の計測を複数回行う場合に有効である。周囲環境の計測を繰り返す度に、位置合わせスコアのポテンシャルスコアの精度が高くなり、フレーム選択部１１５がより高品質なフレームを選択することでキーフレーム生成部１１６が高分解能かつ正確なキーフレームを生成することができるようになる。
自己位置推定の結果を基に、位置姿勢情報とフレーム指標（例えば位置合わせスコア）とのポテンシャルマップ（対応表）を生成し、データ対応付け部１１２が算出した位置姿勢情報に応じた位置合わせスコアまたは当該スコアにバッファを持たせた値を閾値としてもよい。このような閾値は、同じ周囲環境の計測を複数回行う場合に有効である。周囲環境の計測を繰り返す度に、位置合わせスコアのポテンシャルスコアの精度が高くなり、フレーム選択部１１５がより高品質なフレームを選択することでキーフレーム生成部１１６が高分解能かつ正確なキーフレームを生成することができるようになる。
【0062】
以上に説明したように移動体制御システム１００は、位置姿勢情報と、当該位置姿勢情報に対応する地点で過去に取得されたフレームのフレーム指標との対応表（ポテンシャルマップ）を記憶する記憶部１３０を備え、所定の指標値は、対応表においてフレームの取得された地点（位置姿勢情報）に対応するフレーム指標の平均値、または、当該平均値との差が所定値以内の値である。
【0063】
≪フレーム指標の所定値（閾値）：機械学習≫
機械学習技術を用いてフレーム指標の所定値（閾値）を決めてもよい。例えば、点群、位置姿勢情報を説明変数とし、当該位置姿勢情報に示される地点で取得されたフレームのフレーム指標を目的変数とする機械学習モデルを用いて、所定値（閾値）を算出してもよい。所定値は、機械学習モデルを用いて算出された値にバッファを持たせた値であってもよい。このような閾値は、同じ周囲環境の計測を複数回行う場合に用いられ、学習（機械学習モデルの生成）を繰り返す。フレーム選択部１１５は、最新の機械学習モデルを用いて所定値（閾値）を算出して、フレームを選択する。
機械学習技術を用いてフレーム指標の所定値（閾値）を決めてもよい。例えば、点群、位置姿勢情報を説明変数とし、当該位置姿勢情報に示される地点で取得されたフレームのフレーム指標を目的変数とする機械学習モデルを用いて、所定値（閾値）を算出してもよい。所定値は、機械学習モデルを用いて算出された値にバッファを持たせた値であってもよい。このような閾値は、同じ周囲環境の計測を複数回行う場合に用いられ、学習（機械学習モデルの生成）を繰り返す。フレーム選択部１１５は、最新の機械学習モデルを用いて所定値（閾値）を算出して、フレームを選択する。
【0064】
以上に説明したように所定の指標値は、説明変数が点群データであり、目的変数が当該点群データのフレーム指標である学習データを用いて生成された機械学習モデルに、フレーム（点群データ）を説明変数として入力して得られるフレーム指標、または、当該フレーム指標との差が所定値以内の値である。
【0065】
≪変形例：計測環境・条件≫
点群同士の位置合わせでは、３次元形状に特徴のある周囲環境で計測したフレームが望ましく、フレーム内に凹凸などの３次元的な形状特徴のある構造物や物体が１つ以上含まれる地点で複数フレームを取得するのが望ましい。また、オンライン環境地図１３１は、初期位置を基準とする場合が多いため、初期位置で複数フレーム計測するとよい。またループ閉じ込み手法を用いて補正する場合には、移動量補正部１１７は初期位置や周囲環境のランドマークを基準に補正する。このため移動体制御部１１９は、周囲環境の計測時に初期位置やランドマーク地点を移動体２００が複数回訪れるようにアクチュエータ２７０を制御するのが望ましい。
点群同士の位置合わせでは、３次元形状に特徴のある周囲環境で計測したフレームが望ましく、フレーム内に凹凸などの３次元的な形状特徴のある構造物や物体が１つ以上含まれる地点で複数フレームを取得するのが望ましい。また、オンライン環境地図１３１は、初期位置を基準とする場合が多いため、初期位置で複数フレーム計測するとよい。またループ閉じ込み手法を用いて補正する場合には、移動量補正部１１７は初期位置や周囲環境のランドマークを基準に補正する。このため移動体制御部１１９は、周囲環境の計測時に初期位置やランドマーク地点を移動体２００が複数回訪れるようにアクチュエータ２７０を制御するのが望ましい。
【0066】
以上に説明したように移動体制御部１１９は、フレームの点群に３次元形状の特徴点が検出される地点を複数回訪れるように移動体２００を制御する。
【0067】
≪変形例：移動体制御によるキーフレーム作成≫
移動体２００が移動しながら計測する場合、対応付け処理が発生するため、オンライン環境地図１３１の誤差を完全に無くすことは困難である。移動体制御部１１９による移動体２００の移動制御を行うことで高品質なキーフレームを生成するようによい。例えば、キーフレーム生成元検出部１１４がキーフレーム生成元を検出した地点で移動体制御部１１９に指示して移動体２００を静止または概ね静止し、その位置姿勢で複数フレームを計測してもよい。キーフレーム生成部１１６は当該複数フレームの点群を重ね合わせることで高分解かつ正確なキーフレームを生成できるようになる。
移動体２００が移動しながら計測する場合、対応付け処理が発生するため、オンライン環境地図１３１の誤差を完全に無くすことは困難である。移動体制御部１１９による移動体２００の移動制御を行うことで高品質なキーフレームを生成するようによい。例えば、キーフレーム生成元検出部１１４がキーフレーム生成元を検出した地点で移動体制御部１１９に指示して移動体２００を静止または概ね静止し、その位置姿勢で複数フレームを計測してもよい。キーフレーム生成部１１６は当該複数フレームの点群を重ね合わせることで高分解かつ正確なキーフレームを生成できるようになる。
【0068】
このようなキーフレームを複数含めることで、よりオフライン環境地図１３２の精度を向上させることができ、延いてはオンライン環境地図１３１の精度や分解能も向上する。キーフレームを生成するフレームを取得する地点は、どの位置で取得してもよいが、上記したように３次元的に特徴のある周囲環境で計測することが望ましい。なお移動体制御部１１９は静止に限らず、移動体２００が位置姿勢の変化速度が所定値より小さくなる（概ね静止）ように制御してもよい。
【0069】
以上に説明したように移動体制御部１１９は、キーフレーム生成元検出部１１４がキーフレーム生成元を検出した地点で移動体２００が静止または概ね静止するように制御する。フレーム選択部１１５は、静止または概ね静止中に取得されたフレームを選択する。キーフレーム生成部１１６は、選択されたフレームを重ね合わせてキーフレームを生成する。
【0070】
≪第２実施形態≫
第１実施形態では、測距センサ２２０が計測した点群データ（フレーム）に含まれる点群の位置合わせスコアをフレーム指標としている。他のセンサが計測したデータを基にしたフレーム指標を用いてもよい。
第１実施形態では、測距センサ２２０が計測した点群データ（フレーム）に含まれる点群の位置合わせスコアをフレーム指標としている。他のセンサが計測したデータを基にしたフレーム指標を用いてもよい。
【0071】
測距センサ２２０は、一般に水平方向に対して垂直方向の分解能が低く、視野も狭いものが多い。例えばＬｉＤＡＲセンサについては、水平方向の計測範囲は３６０度計測可能なセンサも存在するほど広範囲の計測が可能であるが、垂直方向は数十度程度のセンサが多い。また広く普及しているマルチレイヤー方式のＬｉＤＡＲは垂直方向の情報量の抜けがある。したがって、移動体の位置姿勢の変動において、上下並進、ピッチ回転、ロール回転に対する点群の対応付け精度が悪化しやすい。またフレームレートも数１０Ｈｚ程度のため、不整地を移動する際に生じやすい急激な位置姿勢変動が起きると、点群の対応付け処理において対応付ける点群同士の距離が遠くなり、位置合わせに失敗する場合がある。
【0072】
第２実施形態では、モーションセンサを併用することで、対応付けが困難になりやすい急激な位置姿勢変動や姿勢角の点群をキーフレームの生成に用いないようにする。結果として、正確なキーフレームの生成が可能となる。また、モーションセンサを用いたセンサ融合により、情報量の不足を補い、また一般に高周波（計測が高頻度）である特徴を活用してオンライン環境地図１３１や自己位置推定の精度劣化を軽減するようにする。
【0073】
図９は、第２実施形態に係る移動体２００Ａに備わる移動体制御システム１００Ａの機能ブロック図である。移動体２００Ａに備わるモーションセンサ２３０は、３軸加速度センサと３軸ジャイロセンサの６軸センサである慣性センサである。モーションセンサ２３０は、さらに３軸地磁気センサを組み合わせた９軸センサであってもよい。測距センサ２２０とモーションセンサ２３０との相対位置姿勢に関するキャリブレーションは実行済みであるとし、相互に位置姿勢を変換可能であるとする。
【0074】
データ取得部１１１Ａは、測距センサ２２０からフレーム（点群データ）を取得しながら、モーションセンサ２３０の計測結果を取得して、移動体２００の速度や角速度などをフレーム情報データベース１４０Ａに関連付けて格納する。モーションセンサ２３０は、測距センサ２２０より高周波であるため、１つのフレーム（点群データ）に対して、前後数ｍｓ内に取得された複数のモーションセンサ２３０の計測結果が、当該フレーム対応付けられて格納される。
フレーム選択部１１５Ａは、以下に説明するモーションセンサ２３０の計測値を基にしたフレーム指標に基づいてフレームを選択する。
フレーム選択部１１５Ａは、以下に説明するモーションセンサ２３０の計測値を基にしたフレーム指標に基づいてフレームを選択する。
【0075】
≪フレーム指標：速度、角速度≫
モーションセンサ２３０の計測値を基にするフレーム指標として、速度と角速度（モーション値）がある。フレーム取得時における速度と角速度とが所定値以内であれば、フレーム選択部１１５Ａは当該フレームを選択する。所定値は、測距センサ２２０のフレームレート、測定可能範囲、および分解能に応じて決定すればよい。なお速度と角速度とは、データ取得部１１１Ａが算出してフレーム情報データベース１４０Ａに格納してもよいし、フレーム選択部１１５Ａが算出してもよい。
モーションセンサ２３０の計測値を基にするフレーム指標として、速度と角速度（モーション値）がある。フレーム取得時における速度と角速度とが所定値以内であれば、フレーム選択部１１５Ａは当該フレームを選択する。所定値は、測距センサ２２０のフレームレート、測定可能範囲、および分解能に応じて決定すればよい。なお速度と角速度とは、データ取得部１１１Ａが算出してフレーム情報データベース１４０Ａに格納してもよいし、フレーム選択部１１５Ａが算出してもよい。
【0076】
≪フレーム指標：角度（姿勢角）≫
モーションセンサ２３０の計測値を基にするフレーム指標として角度がある。角度（移動体２００の姿勢角、モーション値）が所定値以内であれば、フレーム選択部１１５Ａは当該フレームを選択する。所定値は、用いる測距センサ２２０の測定可能範囲、および分解能に応じて決定すればよい。なお角度については、データ取得部１１１Ａが算出してフレーム情報データベース１４０Ａに格納してもよいし、フレーム選択部１１５Ａが算出してもよい。
モーションセンサ２３０の計測値を基にするフレーム指標として角度がある。角度（移動体２００の姿勢角、モーション値）が所定値以内であれば、フレーム選択部１１５Ａは当該フレームを選択する。所定値は、用いる測距センサ２２０の測定可能範囲、および分解能に応じて決定すればよい。なお角度については、データ取得部１１１Ａが算出してフレーム情報データベース１４０Ａに格納してもよいし、フレーム選択部１１５Ａが算出してもよい。
【0077】
以上に説明したようにデータ取得部１１１Ａは、移動体２００の位置姿勢の変化速度または姿勢（角度）を示すモーション値を取得する。フレームのフレーム指標は、当該フレームを取得した時点のモーション値である。フレーム指標が所定の指標値を満たすフレームとは、フレーム指標が所定の指標値以内のフレームである。
【0078】
≪第２実施形態の特徴≫
モーションセンサ２３０を用いたフレーム指標を用いることで、段差や傾斜などがなく移動体２００の位置姿勢の変動が小さい平らな地点でのフレームが選択されて、正確なキーフレームが生成されるようになる。換言すれば、測距センサ２２０のみでは自己位置推定が不正確になりやすい位置姿勢変動時のフレームを含めないため、正確なキーフレームを生成することができる。
モーションセンサ２３０を用いたフレーム指標を用いることで、段差や傾斜などがなく移動体２００の位置姿勢の変動が小さい平らな地点でのフレームが選択されて、正確なキーフレームが生成されるようになる。換言すれば、測距センサ２２０のみでは自己位置推定が不正確になりやすい位置姿勢変動時のフレームを含めないため、正確なキーフレームを生成することができる。
【0079】
≪変形例：移動体制御によるキーフレーム作成≫
移動体２００の位置姿勢の変動が小さい地点で周囲環境を計測して複数フレームを取得するようにしてもよい。このような地点とは、例えば段差や傾斜がない平らな地点である。データ取得部１１１Ａは、算出した位置姿勢の変動が所定値以内であれば、移動体制御部１１９に指示して移動体２００を静止し、その位置姿勢で複数フレームを計測してもよい。キーフレーム生成部１１６は当該複数フレームの点群を重ね合わせることで高分解かつ正確なキーフレームを生成することができる。このようなキーフレームを複数含めることで、よりオフライン環境地図１３２の精度を向上させることができ、延いてはオンライン環境地図１３１の精度も向上する。なお移動体制御部１１９は静止に限らず、移動体２００が位置姿勢の変化速度が小さくなるように制御してもよい。
移動体２００の位置姿勢の変動が小さい地点で周囲環境を計測して複数フレームを取得するようにしてもよい。このような地点とは、例えば段差や傾斜がない平らな地点である。データ取得部１１１Ａは、算出した位置姿勢の変動が所定値以内であれば、移動体制御部１１９に指示して移動体２００を静止し、その位置姿勢で複数フレームを計測してもよい。キーフレーム生成部１１６は当該複数フレームの点群を重ね合わせることで高分解かつ正確なキーフレームを生成することができる。このようなキーフレームを複数含めることで、よりオフライン環境地図１３２の精度を向上させることができ、延いてはオンライン環境地図１３１の精度も向上する。なお移動体制御部１１９は静止に限らず、移動体２００が位置姿勢の変化速度が小さくなるように制御してもよい。
【0080】
≪変形例：センサ融合処理≫
移動体制御システム１００Ａは、測距センサ２２０とモーションセンサ２３０のセンサ融合処理（センサフュージョン）を行う融合処理部１２０Ａ（図９参照）を備えてもよい。センサ融合処理を行うことで、測距センサ２２０のフレームレートではとらえきれない位置姿勢変動にも追従でき、自己位置推定の精度が向上する。このためフレーム指標の閾値を、センサ融合を用いない場合と比べて緩めることができる。センサ融合処理の手法としては、拡張カルマンフィルタやパーティクルフィルタなどのベイズフィルタ、因子グラフなどを用いることができる。
移動体制御システム１００Ａは、測距センサ２２０とモーションセンサ２３０のセンサ融合処理（センサフュージョン）を行う融合処理部１２０Ａ（図９参照）を備えてもよい。センサ融合処理を行うことで、測距センサ２２０のフレームレートではとらえきれない位置姿勢変動にも追従でき、自己位置推定の精度が向上する。このためフレーム指標の閾値を、センサ融合を用いない場合と比べて緩めることができる。センサ融合処理の手法としては、拡張カルマンフィルタやパーティクルフィルタなどのベイズフィルタ、因子グラフなどを用いることができる。
【0081】
≪第３実施形態≫
センサとして、測距センサ２２０と画像センサとを組み合わせてもよい。図１０は、第３実施形態に係る移動体２００Ｂに備わる移動体制御システム１００Ｂの機能ブロック図である。移動体２００Ｂに備わる画像センサ２４０は、画素ごとにＲＧＢ情報の明るさを検出して２次元画像を取得できる一般的なＲＧＢカメラである。ただし、必ずしもＲＧＢ情報を取得できるカメラでなくてもよく、モノクロのカメラであってもよい。また画像センサ２４０は、赤外線カメラなどの可視光以外を検知するカメラであってもよい。さらに測距センサ２２０と一体化した形状のセンサであってもよい。取得する画像は、カラー画像であってもよいし、モノクロ画像であってもよい。画像センサ２４０のフレームレートは、測距センサ２２０のフレームレートと同等かそれ以上のフレームレートであることが望ましく、周期は移動体２００Ｂの速度に応じて決定すればよい。
センサとして、測距センサ２２０と画像センサとを組み合わせてもよい。図１０は、第３実施形態に係る移動体２００Ｂに備わる移動体制御システム１００Ｂの機能ブロック図である。移動体２００Ｂに備わる画像センサ２４０は、画素ごとにＲＧＢ情報の明るさを検出して２次元画像を取得できる一般的なＲＧＢカメラである。ただし、必ずしもＲＧＢ情報を取得できるカメラでなくてもよく、モノクロのカメラであってもよい。また画像センサ２４０は、赤外線カメラなどの可視光以外を検知するカメラであってもよい。さらに測距センサ２２０と一体化した形状のセンサであってもよい。取得する画像は、カラー画像であってもよいし、モノクロ画像であってもよい。画像センサ２４０のフレームレートは、測距センサ２２０のフレームレートと同等かそれ以上のフレームレートであることが望ましく、周期は移動体２００Ｂの速度に応じて決定すればよい。
【0082】
画像センサ２４０は、測距センサ２２０の撮像範囲の一部あるいは全てを含むように、移動体２００Ｂに取り付けられている。測距センサ２２０と画像センサ２４０のキャリブレーションは行われているものとし、測距センサ２２０が取得する点群の点と画像センサ２４０の画素との対応関係がとれているものとする。
【0083】
測距センサ２２０で取得するフレームのタイミングと画像センサ２４０で撮影された画像のタイミングとは、必ずしも同期をとる必要はないが、測距センサ２２０の該当フレームに応じた画像がどの画像となるかを判断できるよう、時系列的な対応がとられているものとする。具体的には、１つのフレーム（点群データ）に対して、前後数ｍｓ内に取得された複数の画像センサ２４０の計測結果を対応付けるとよい。
【0084】
データ取得部１１１Ｂは、測距センサ２２０からフレーム（点群データ）を取得しながら、画像センサ２４０が撮影した画像を取得して、フレーム情報データベース１４０Ｂに関連付けて格納する。換言すればデータ取得部１１１Ｂは、測距センサ２２０のフレームの前後数ｍｓ以内に取得された数個の画像を対応付けてフレーム情報データベース１４０Ｂに格納する。
フレーム選択部１１５Ｂは、以下に説明する画像センサ２４０の計測値を基にしたフレーム指標に基づいてフレームを選択する。
フレーム選択部１１５Ｂは、以下に説明する画像センサ２４０の計測値を基にしたフレーム指標に基づいてフレームを選択する。
【0085】
≪フレーム指標：目印となる物体・構造物（ランドマーク）≫
画像センサ２４０が撮影した画像に含まれる目印となる物体・構造物（ランドマーク）の検出には、物体認識技術分野の手法を用いる。物体認識技術分野では、深層学習ベースの手法が主流であり、深層学習を用いた物体認識・検出技術を適用する。例えば、Ｒ－ＣＮＮ（Region Based Convolutional Neural Networks）やＹＯＬＯ（You Only Look Once）などの物体認識・検出技術を用いると良い。またはif-thenルールに従ったルールベースの物体認識・検出技術の適用を行ってもよい。
画像センサ２４０が撮影した画像に含まれる目印となる物体・構造物（ランドマーク）の検出には、物体認識技術分野の手法を用いる。物体認識技術分野では、深層学習ベースの手法が主流であり、深層学習を用いた物体認識・検出技術を適用する。例えば、Ｒ－ＣＮＮ（Region Based Convolutional Neural Networks）やＹＯＬＯ（You Only Look Once）などの物体認識・検出技術を用いると良い。またはif-thenルールに従ったルールベースの物体認識・検出技術の適用を行ってもよい。
【0086】
物体は、２次元画像中では当該物体のバウンディングボックスである２次元矩形として検出される。より詳細に物体の２次元形状を切り出すような手法を用いてもよい。適用先の周囲環境に応じて目印となる物体・構造物は適宜変更してもよい。環境特有の物体の事前情報がない場合は、物体認識・検出技術を用いて検出されたものをその場で目印となる物体・構造物として追加してもよい。
【0087】
目印となる物体・構造物をフレーム指標に用いる場合は、画像センサ２４０で取得した画像内に目印となる物体・構造物が検出される当否をフレーム指標としてもよいし、検出された物体・構造物の数を指標としてもよい。
【0088】
単に画像センサ２４０が撮影した画像中の目印となる物体・構造物の有無をフレーム指標とすると、目印となる物体・構造物が存在しない周囲環境においては、どんなフレームも選択されない。このため、測距センサ２２０のフレーム指標で選択されなかった場合に画像中の目印となる物体・構造物によるフレーム指標を用いるとよい。これにより、本来なら選択すべきだが選択されなかったフレームの数が軽減できる。
【0089】
≪フレーム指標：特徴点≫
画像の特徴点とは、２次元画像中のテクスチャのある領域やコーナのような局所的に輝度の変化が大きい領域の中心点である。特徴点の事前情報がない場合は、ＳＩＦＴやＯＲＢといった特徴点検出技術を用いてその場で特徴点に追加してもよい。
特徴点をフレーム指標に用いる場合は、照合できた特徴点の照合スコア、または特徴点数をフレーム指標とする。照合スコアの場合は、使用する画像センサ２４０の性能に応じてフレーム指標の閾値を決定すればよい。特徴点数については、画像センサの性能に加え、環境に応じて取得できる特徴点数は異なるため、画像センサ２４０の性能と周囲環境に応じて閾値を決定する。
画像の特徴点とは、２次元画像中のテクスチャのある領域やコーナのような局所的に輝度の変化が大きい領域の中心点である。特徴点の事前情報がない場合は、ＳＩＦＴやＯＲＢといった特徴点検出技術を用いてその場で特徴点に追加してもよい。
特徴点をフレーム指標に用いる場合は、照合できた特徴点の照合スコア、または特徴点数をフレーム指標とする。照合スコアの場合は、使用する画像センサ２４０の性能に応じてフレーム指標の閾値を決定すればよい。特徴点数については、画像センサの性能に加え、環境に応じて取得できる特徴点数は異なるため、画像センサ２４０の性能と周囲環境に応じて閾値を決定する。
【0090】
≪フレーム指標：テクスチャ平面≫
テクスチャ平面とは、周囲環境の壁などの平面を含む構造物において、何かしらの模様が描かれた平面領域である。テクスチャ平面の検出は、例えば、物体認識技術やテンプレートマッチング、特徴点検出を用いた平面領域検出などを用いる。適用先の環境に応じてテクスチャ平面は適宜変更するとよい。周囲環境特有の面の事前情報がない場合には、特徴点が多く検出され、かつ平面であると認識された領域の２次元矩形をその場でテクスチャ平面として追加してもよい。
テクスチャ平面とは、周囲環境の壁などの平面を含む構造物において、何かしらの模様が描かれた平面領域である。テクスチャ平面の検出は、例えば、物体認識技術やテンプレートマッチング、特徴点検出を用いた平面領域検出などを用いる。適用先の環境に応じてテクスチャ平面は適宜変更するとよい。周囲環境特有の面の事前情報がない場合には、特徴点が多く検出され、かつ平面であると認識された領域の２次元矩形をその場でテクスチャ平面として追加してもよい。
【0091】
テクスチャ平面をフレーム指標に用いる場合は、ランドマークをフレーム指標に用いる場合と同様に、画像センサ２４０で取得した画像内でのテクスチャ平面検出の当否をフレーム指標とする。ただし、単にテクスチャ平面検出の有無をフレーム指標とすると、テクスチャ平面が存在しない周囲環境においては、どんなフレームも選択されない。このため、測距センサ２２０のフレーム指標で選択されなかった場合にテクスチャ平面によるフレーム指標を用いるとよい。
画像に係るフレーム指標としてランドマーム、特徴点、テクスチャ平面に係る指標を説明したが、これらを組み合わせた指標であってもよい。
画像に係るフレーム指標としてランドマーム、特徴点、テクスチャ平面に係る指標を説明したが、これらを組み合わせた指標であってもよい。
【0092】
以上に説明したようにデータ取得部１１１Ｂは、移動体２００の周囲環境の画像を取得する。フレームのフレーム指標は、当該フレームを取得した時点の画像から検出される周囲環境にあるランドマークとなる物体の数、当該フレームを取得した時点の画像から検出される特徴点の検出スコア、フレームを取得した時点の画像から検出される特徴点の数、および、当該フレームを取得した時点の画像から検出されるテクスチャの有無の、何れか少なくとも１つを基に算出される。
【0093】
≪第３実施形態の特徴≫
画像センサ２４０を用いたフレーム指標を用いることで、テクスチャに特徴のある環境で取得されたフレーム、物が乱雑になっている環境で取得されたフレーム、目印となる物体・構造物が多く映り込む地点で取得されたフレームが選択されて、正確なキーフレームが生成されるようになる。換言すれば、位置合わせ信頼度の高いランドマークが含まれたフレームが選択され、当該フレームの点群をキーフレームの生成に利用できるため、正確なキーフレームを生成することができる。
画像センサ２４０を用いたフレーム指標を用いることで、テクスチャに特徴のある環境で取得されたフレーム、物が乱雑になっている環境で取得されたフレーム、目印となる物体・構造物が多く映り込む地点で取得されたフレームが選択されて、正確なキーフレームが生成されるようになる。換言すれば、位置合わせ信頼度の高いランドマークが含まれたフレームが選択され、当該フレームの点群をキーフレームの生成に利用できるため、正確なキーフレームを生成することができる。
【0094】
≪変形例：センサ融合処理≫
移動体制御システム１００Ｂは、測距センサ２２０を用いた自己位置推定と画像センサ２４０を用いた自己位置推定との融合処理を行う融合処理部１２０Ｂ（図１０参照）を備えてもよい。センサ融合処理を行うことで、測距センサ２２０のフレームレートではとらえきれない位置姿勢の変動への追従や自己位置ロストの発生の軽減ができる。延いては、自己位置推定やオンライン環境地図１３１の精度が向上する。センサ融合処理の手法としては、拡張カルマンフィルタやパーティクルフィルタなどのベイズフィルタ、因子グラフなどを用いることができる。
移動体制御システム１００Ｂは、測距センサ２２０を用いた自己位置推定と画像センサ２４０を用いた自己位置推定との融合処理を行う融合処理部１２０Ｂ（図１０参照）を備えてもよい。センサ融合処理を行うことで、測距センサ２２０のフレームレートではとらえきれない位置姿勢の変動への追従や自己位置ロストの発生の軽減ができる。延いては、自己位置推定やオンライン環境地図１３１の精度が向上する。センサ融合処理の手法としては、拡張カルマンフィルタやパーティクルフィルタなどのベイズフィルタ、因子グラフなどを用いることができる。
【0095】
画像センサ２４０を用いた自己位置推定（とオンライン環境地図１３１の作成）については、オブジェクトＳＬＡＭやＶｉｓｕａｌ ＳＬＡＭを用いるとよい。オブジェクトＳＬＡＭとは、画像中の物体の見えから、直方体や球などで物体の３次元位置姿勢推定し、その相対位置姿勢から自己位置推定を行う手法である。一方Ｖｉｓｕａｌ ＳＬＡＭは、深層学習や特徴点をベースに単眼でビジュアルオドメトリを求め、自己位置推定を行う手法である。
【0096】
≪変形例：複数のセンサ≫
第２実施形態の移動体２００Ａは測距センサ２２０とモーションセンサ２３０とを備え、第３実施形態の移動体２００Ｂは測距センサ２２０と画像センサ２４０とを備える。測距センサ２２０とモーションセンサ２３０と画像センサ２４０とを備える移動体であってもよい。複数のセンサがある場合にフレーム選択部は、それぞれのセンサに係るフレーム指標を組み合わせて、フレームを選択してもよい。例えば、何れか１つのセンサに係るフレーム指標が所定値を満たすフレームを選択してもよいし、測距センサ２２０に係るフレーム指標が所定値を満たし、さらにその他１つのセンサに係るフレーム指標が所定値を満たすフレームを選択してもよい。または所定値を満たす測距センサ２２０に係るフレーム指標のフレームがない（少ない）場合に、他のセンサに係るフレーム指標が所定値を満たすフレームを選択してもよい。
第２実施形態の移動体２００Ａは測距センサ２２０とモーションセンサ２３０とを備え、第３実施形態の移動体２００Ｂは測距センサ２２０と画像センサ２４０とを備える。測距センサ２２０とモーションセンサ２３０と画像センサ２４０とを備える移動体であってもよい。複数のセンサがある場合にフレーム選択部は、それぞれのセンサに係るフレーム指標を組み合わせて、フレームを選択してもよい。例えば、何れか１つのセンサに係るフレーム指標が所定値を満たすフレームを選択してもよいし、測距センサ２２０に係るフレーム指標が所定値を満たし、さらにその他１つのセンサに係るフレーム指標が所定値を満たすフレームを選択してもよい。または所定値を満たす測距センサ２２０に係るフレーム指標のフレームがない（少ない）場合に、他のセンサに係るフレーム指標が所定値を満たすフレームを選択してもよい。
【0097】
測距センサ２２０のフレーム指標が所定値を満たし、かつ画像センサ２４０のフレーム指標も所定値を満たすフレームに関しては、フレーム指標を複数満たすフレームとして、整合信頼度の高いフレームの点群としてフレーム情報データベースに格納してもよい。キーフレーム生成時にキーフレーム生成部は、整合信頼度の高いフレームの点群を優先して重ね合わせてもよい。
【0098】
何れかのセンサに係るフレーム指標が所定値を満たすタイミング（地点）や、複数のフレーム指標が所定値を満たすタイミングで移動体制御部１１９は、移動体２００を静止または概ね静止するように制御し、データ取得部１１１は複数フレームを取得する。フレーム選択部１１５は、当該複数フレームを選択し、キーフレーム生成部１１６は当該複数フレームの点群を重ね合わせることで高分解かつ正確なキーフレームを生成できるようになる。
【0099】
またキーフレーム生成元検出部１１４は、何れかのセンサに係るフレーム指標が所定値を満たすタイミング（地点）や、複数のフレーム指標が所定値を満たすタイミングのフレームをキーフレーム生成元としてもよい。高分解かつ正確なキーフレームが生成されるようになる。
【0100】
以上に説明したように移動体制御部１１９は、所定の指標値を満たすフレームが取得された地点において、移動体２００が静止または概ね静止するように制御する。フレーム選択部１１５は、静止または概ね静止中に取得されたフレームを選択する。キーフレーム生成部１１６は、選択されたフレームを重ね合わせてキーフレームを生成する。
【0101】
≪変形例：複数の移動体≫
上記した実施形態において移動体制御システム１００，１００Ａ，１００Ｂは、１つの移動体２００，２００Ａ，２００Ｂに搭載されているが、これに限定されるものではない。例えばオンライン環境地図１３１とオフライン環境地図１３２とは、サーバ上にあり複数の移動体で共有されてもよい。この場合に環境地図作成部１１８はサーバ上にあってもよい。さらにキーフレームデータベース１５０も複数の移動体２００，２００Ａ，２００Ｂで共有され、移動量補正部１１７はサーバで動作してもよい。このようにすることで、複数の移動体２００，２００Ａ，２００Ｂが協働して、高精度なオンライン環境地図１３１やオフライン環境地図１３２を短時間で生成することができるようになる。
また移動体が１つであっても、演算部１１０に備わる一部の機能部や記憶部１３０にある一部のデータがサーバ（別装置）にあってもよい。
上記した実施形態において移動体制御システム１００，１００Ａ，１００Ｂは、１つの移動体２００，２００Ａ，２００Ｂに搭載されているが、これに限定されるものではない。例えばオンライン環境地図１３１とオフライン環境地図１３２とは、サーバ上にあり複数の移動体で共有されてもよい。この場合に環境地図作成部１１８はサーバ上にあってもよい。さらにキーフレームデータベース１５０も複数の移動体２００，２００Ａ，２００Ｂで共有され、移動量補正部１１７はサーバで動作してもよい。このようにすることで、複数の移動体２００，２００Ａ，２００Ｂが協働して、高精度なオンライン環境地図１３１やオフライン環境地図１３２を短時間で生成することができるようになる。
また移動体が１つであっても、演算部１１０に備わる一部の機能部や記憶部１３０にある一部のデータがサーバ（別装置）にあってもよい。
【0102】
≪その他変形例≫
以上、本発明のいくつかの実施形態について説明したが、これらの実施形態は、例示に過ぎず、本発明の技術的範囲を限定するものではない。本発明はその他の様々な実施形態をとることが可能であり、さらに、本発明の要旨を逸脱しない範囲で、省略や置換等種々の変更を行うことができる。これら実施形態やその変形は、本明細書等に記載された発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
以上、本発明のいくつかの実施形態について説明したが、これらの実施形態は、例示に過ぎず、本発明の技術的範囲を限定するものではない。本発明はその他の様々な実施形態をとることが可能であり、さらに、本発明の要旨を逸脱しない範囲で、省略や置換等種々の変更を行うことができる。これら実施形態やその変形は、本明細書等に記載された発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【0103】
１００，１００Ａ，１００Ｂ 移動体制御システム
１１１，１１１Ａ，１１１Ｂ データ取得部
１１２ データ対応付け部
１１３ 移動量算出部
１１４ キーフレーム生成元検出部
１１５，１１５Ａ，１１５Ｂ フレーム選択部
１１６ キーフレーム生成部
１１７ 移動量補正部
１１８ 環境地図作成部
１１９ 移動体制御部
１２０Ａ，１２０Ｂ 融合処理部
１３１ オンライン環境地図
１３２ オフライン環境地図
１３８ プログラム
１４０，１４０Ａ，１４０Ｂ フレーム情報データベース
１５０ キーフレームデータベース
２００，２００Ａ，２００Ｂ 移動体
２２０ 測距センサ
２３０ モーションセンサ
２４０ 画像センサ
１１１，１１１Ａ，１１１Ｂ データ取得部
１１２ データ対応付け部
１１３ 移動量算出部
１１４ キーフレーム生成元検出部
１１５，１１５Ａ，１１５Ｂ フレーム選択部
１１６ キーフレーム生成部
１１７ 移動量補正部
１１８ 環境地図作成部
１１９ 移動体制御部
１２０Ａ，１２０Ｂ 融合処理部
１３１ オンライン環境地図
１３２ オフライン環境地図
１３８ プログラム
１４０，１４０Ａ，１４０Ｂ フレーム情報データベース
１５０ キーフレームデータベース
２００，２００Ａ，２００Ｂ 移動体
２２０ 測距センサ
２３０ モーションセンサ
２４０ 画像センサ
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】