特許 7581163 情報処理装置、移動体、情報処理方法及びコンピュータプログラム

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

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

(11)【特許番号】

(24)【登録日】2024-11-01

(45)【発行日】2024-11-12

(54)【発明の名称】情報処理装置、移動体、情報処理方法及びコンピュータプログラム

(51)【国際特許分類】

   G05D 1/43 20240101AFI20241105BHJP

【ＦＩ】

G05D1/43

【請求項の数】 13

(21)【出願番号】P 2021146513

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

(65)【公開番号】P2023039368

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

【審査請求日】2023-06-27

(73)【特許権者】

【識別番号】000001007

【氏名又は名称】キヤノン株式会社

(74)【代理人】

【識別番号】100114775

【弁理士】

【氏名又は名称】高岡 亮一

(74)【代理人】

【識別番号】100121511

【弁理士】

【氏名又は名称】小田 直

(74)【代理人】

【識別番号】100208580

【弁理士】

【氏名又は名称】三好 玲奈

(72)【発明者】

【氏名】山本 圭一

(72)【発明者】

【氏名】冨岡 誠

【審査官】大古 健一

(56)【参考文献】

【文献】米国特許出願公開第２０１７／０１５３６４６（ＵＳ，Ａ１）

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

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

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

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

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

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

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

【請求項1】

移動体に搭載した慣性計測センサと、
前記慣性計測センサとは別に前記移動体に設けられ、前記移動体の位置姿勢を推定する位置姿勢センサと、
前記慣性計測センサの出力を用いて前記移動体の位置姿勢の第１の推定処理を行った第１の推定結果と、前記位置姿勢センサの出力を用いて前記移動体の位置姿勢の第２の推定処理を行った第２の推定結果と、を統合して、前記移動体の位置姿勢を推定する位置姿勢推定手段と、
前記移動体の移動状態を取得する移動状態取得手段と、
前記移動状態取得手段により取得された前記移動状態を静止状態とみなす場合、前記第１の推定結果を統合する第１の重みを、前記移動状態を静止状態とみなさない場合より小さく決定する第１の重み決定手段と、を備えることを特徴とする情報処理装置。

【請求項2】

前記位置姿勢センサは被写体を撮像する撮像手段、被写体の距離を計測する距離計測手段、ＧＰＳ、ＬｉＤＡＲの少なくとも１つを含むことを特徴とする請求項１に記載の情報処理装置。

【請求項3】

前記移動状態取得手段と前記位置姿勢センサは同じ撮像手段を用いることを特徴とする請求項２に記載の情報処理装置。

【請求項4】

前記移動状態取得手段は撮像手段、距離計測装置、エンコーダ、ＧＰＳ、駆動源に対する駆動命令の少なくとも１つを含むことを特徴とする請求項１又は２に記載の情報処理装置。

【請求項5】

前記移動状態取得手段は、前記移動状態として、前記移動体の移動量が所定の閾値以下か否か、もしくは前記移動体の加速度が所定の閾値以下か否か、若しくは前記移動体の角速度が所定の閾値以下か否か、の少なくとも１つを取得することを特徴とする請求項１に記載の情報処理装置。

【請求項6】

前記第１の重み決定手段は、前記移動体の移動量が所定の閾値以下、前記移動体の加速度が所定の閾値以下、前記移動体の角速度が所定の閾値以下の少なくとも１つの場合に、前記慣性計測センサの出力情報に対する前記重みを所定の値よりも低くすることを特徴とする請求項１に記載の情報処理装置。

【請求項7】

前記位置姿勢センサで計測した前記位置姿勢の信頼度を取得する信頼度取得手段と、
前記信頼度に基づいて、前記位置姿勢センサの出力情報に対する第２の重みを決定する第２の重み決定手段と、を有し、
前記位置姿勢推定手段は、前記位置姿勢センサの出力と、前記慣性計測センサの出力と、前記第１の重みと、前記第２の重みとに基づき前記推定処理を行うことを特徴とする請求項１に記載の情報処理装置。

【請求項8】

前記信頼度取得手段は、前記位置姿勢センサの出力の画像から検出した特徴点の数又は分布、前記画像の輝度、前記輝度の変動、前記画像の中の移動物体の数、前記画像の中の所定の被写体の有無、ＧＰＳの電波の受信状態のうち少なくとも１つに基づいて前記信頼度を算出することを特徴とする請求項７に記載の情報処理装置。

【請求項9】

前記位置姿勢センサで計測した前記位置姿勢の信頼度を取得する信頼度取得手段を有し、
前記信頼度取得手段が取得した前記信頼度が所定の閾値以下である場合に、前記信頼度を改善するように前記移動体を移動又は姿勢変更を制御する制御手段を有することを特徴とする請求項１に記載の情報処理装置。

【請求項10】

前記第１の重み決定手段が決定した重みを前記移動体の現在位置と共に表示する表示手段を有することを特徴とする請求項１～９のいずれか１項に記載の情報処理装置。

【請求項11】

請求項１～１０のいずれか１項に記載の情報処理装置と、
前記位置姿勢推定手段により推定された前記位置姿勢に基づき前記移動体の移動を制御する移動制御手段と、を有する移動体。

【請求項12】

移動体に搭載した位置姿勢センサの出力を用いて前記移動体の位置姿勢を推定する位置姿勢工程と、
位置姿勢センサとは別に前記移動体に搭載した慣性計測センサの出力を用いて前記移動体の位置姿勢の第１の推定処理を行った第１の推定結果と、前記位置姿勢センサの出力を用いて前記移動体の位置姿勢の第２の推定処理を行った第２の推定結果と、を統合して、前記移動体の位置姿勢を推定する位置姿勢推定工程と、
前記移動体の移動状態を取得する移動状態取得工程と、
前記移動状態取得工程により取得された前記移動状態を静止状態とみなす場合、前記第１の推定結果を統合する第１の重みを、前記移動状態を静止状態とみなさない場合より小さく決定する第１の重み決定工程と、を備えることを特徴とする情報処理方法。

【請求項13】

請求項１～１０のいずれか１項に記載の情報処理装置又は請求項１１に記載の移動体の各手段をコンピュータにより制御するためのコンピュータプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体の位置姿勢の推定が可能な情報処理装置、移動体、情報処理方法及びコンピュータプログラム等に関する。

【背景技術】

【0002】

搬送車両（例えばＡＧＶ（ＡｕｔｏｍａｔｅｄＧｕｉｄｅｄＶｅｈｉｃｌｅ））などの移動体を、工場や物流倉庫といった環境内において自動で移動させるために、位置姿勢算出技術が提案されている。移動体を自律走行させる場合、複数種類のセンサを利用して、総合的に位置姿勢を算出することで、位置姿勢算出の精度を高める方法がある。例えば、特許文献１では慣性計測センサを用いて算出した並進ベクトルと回転行列を、撮像装置を用いて算出した並進ベクトルと回転行列との誤差に基づいて補正する。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１１－１４１２２１号公報

【発明の概要】

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

【0004】

しかしながら、特許文献１の方法では移動体の加速度が低い時などに慣性計測センサのノイズが無視できないものとなり、位置姿勢推定の精度が低下するという課題があった。
本発明は上記問題に鑑みてなされたものであり、移動体の位置姿勢推定の精度低下を軽減することができる情報処理装置を提供することを目的とする。

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

【0005】

本発明の１つの側面に係る情報処理装置は、
移動体に搭載した慣性計測センサと、
前記慣性計測センサとは別に前記移動体に設けられ、前記移動体の位置姿勢を推定する位置姿勢センサと、
前記慣性計測センサの出力を用いて前記移動体の位置姿勢の第１の推定処理を行った第１の推定結果と、前記位置姿勢センサの出力を用いて前記移動体の位置姿勢の第２の推定処理を行った第２の推定結果と、を統合して、前記移動体の位置姿勢を推定する位置姿勢推定手段と、
前記移動体の移動状態を取得する移動状態取得手段と、
前記移動状態取得手段により取得された前記移動状態を静止状態とみなす場合、前記第１の推定結果を統合する第１の重みを、前記移動状態を静止状態とみなさない場合より小さく決定する第１の重み決定手段と、を備えることを特徴とする。

【発明の効果】

【0006】

本発明によれば、移動体の位置姿勢推定の精度低下を軽減することができる情報処理装置を実現できる。

【図面の簡単な説明】

【0007】

【図1】実施例１における情報処理装置等の構成例を示す機能ブロック図である。

【図2】実施例１の情報処理装置１００のハードウェア構成例を示すブロック図である。

【図3】実施例１における情報処理装置が実行する処理を示すフローチャートである。

【図4】実施例２における情報処理装置等の構成例を示す機能ブロック図である。

【図5】実施例２における情報処理装置が実行する処理を示すフローチャートである。

【図6】実施例３における情報処理装置が実行する処理を示すフローチャートである。

【図7】図６のステップＳ３０１の処理を示すフローチャートである。

【図8】実施例４において表示部２１６に表示されるＧＵＩの例を示す図である。

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

【0008】

以下、添付図面を参照して、本発明の好適な実施の形態について実施例を用いて説明する。尚、各図において、同一の部材ないし要素については同一の参照番号を付し、重複する説明は省略ないし簡略化する。
又、実施例においては、移動体としてＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）又は、ＡＭＲ（自律走行ロボット装置：Ａｕｔｏｎｏｍｏｕｓ Ｍｏｂｉｌｅ Ｒｏｂｏｔ）の形態を有するが、自動車、列車、飛行機、バイク、ドローン、ロボットなど移動可能なものを含む。

【実施例1】

【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】

具体的には、前述のバンドル調整時に、慣性計測センサ１０４の出力情報を用いて位置と姿勢を補正して、位置姿勢の推定を実施する。又は、Ｗｉｆｉのような無線通信における電波強度計測に基づく方法でも良いし、ＧＰＳのような衛星測位や、磁気式のトラッキングセンサ、多数の光センサで空間中を走査する光を感知する時間差を用いる方法でも良い。

【実施例2】

【0045】

実施例２では、位置姿勢センサ１０５として撮像装置（撮像部）又は距離計測装置（距離計測部）等を用いた場合に、それらの出力情報に対する信頼度に基づいて第２の重みを決定する。そして、撮像装置又は距離計測装置と、慣性計測センサ１０４の出力情報、及び第１の重みと第２の重みに基づいて移動体の位置姿勢を推定する。
特に、位置姿勢センサ１０５としての撮像装置又は距離計測装置の出力情報に対する信頼度が低い場合に第２の重みを低く設定することで、不正確なセンサの出力情報の利用割合が下がり、位置姿勢推定の精度低下を軽減することができる。

【0046】

図４は、実施例２における情報処理装置等の構成例を示す機能ブロック図である。
図１に対して、信頼度算出部２０１と、信頼度取得部２０２、第２の重み決定部２０３を設けた点と位置姿勢推定部１０７における処理が異なる。ここでは、実施例１と異なる部分についてのみ説明する。

【0047】

信頼度算出部２０１は、移動体の位置姿勢計測に関わる位置姿勢センサ１０５の出力情報に対する信頼度を算出する。実施例２においては、位置姿勢センサ１０５として利用する例えばステレオカメラ等の撮像装置の出力情報に基づいて信頼度を算出する。
具体的には、例えば、位置姿勢センサ１０５として利用するステレオカメラ等の撮像装置で撮像した画像内の特徴点の数が少ない場合に信頼度の値を低くする。

【0048】

信頼度取得部２０２は、信頼度算出部２０１が算出した信頼度を取得する。
第２の重み決定部２０３は、信頼度取得部２０２により取得された信頼度に基づいて、第２の重みを決定する。例えば、信頼度が低い場合に第２の重みも低くなるように決定する。
位置姿勢推定部１０７は、第１の重みと、第２の重みと、センサ情報取得部１０６が取得した慣性計測センサ１０４と位置姿勢センサ１０５の出力情報に基づいて移動体の位置姿勢を推定する。

【0049】

尚、第２の重み決定部２０３は、位置姿勢センサ１０５の出力情報に対して直接的に掛けられる重みを決定するのではなく、慣性計測センサ１０４の出力に対する位置姿勢センサ１０５の出力の相対的な重みを決定するものを含む。即ち、第２の重み決定部は、慣性計測センサ１０４の出力に対する位置姿勢センサ１０５の出力情報の重みを、相対的に決定するものを含む。

【0050】

図５は、実施例２の情報処理装置が実行する処理を示すフローチャートである。図３のフローチャートと同じステップについては説明を省略し、図３のフローチャートと異なる処理のみを説明する。
ステップＳ２０１（信頼度取得工程）では、信頼度取得部２０２が信頼度算出部２０１にて算出された信頼度を取得する。

【0051】

ステップＳ２０２（第２の重み決定工程）では、第２の重み決定部２０３が信頼度取得部２０２にて取得した信頼度に基づいて第２の重みを決定する。第２の重みはステップＳ１０１において初期値が予め設定されているが、ユーザ操作により別の値に設定されていても良い。初期値としては、例えば１を設定しておく。
位置姿勢センサ１０５の出力情報に掛けられる第２の重みが１であれば、位置姿勢センサ１０５の出力情報が移動体の位置姿勢の推定に１００％利用されることになる。

【0052】

本実施例においては、信頼度が予め設定した閾値以下の場合に第２の重みを０、もしくは信頼度が閾値以下でない場合の値より低い値に設定する。第２の重みが０であれば、位置姿勢センサ１０５の出力情報に０が掛けられ、移動体の位置姿勢の推定には利用されないことになる。又、第２の重みが、信頼度が閾値以下でない場合の値より低い値であれば、移動体の位置姿勢の推定での位置姿勢センサ１０５の出力情報の利用割合を下げる。

【0053】

ステップＳ１０５では、位置姿勢推定部１０７が、第１の重みと、第２の重みと、慣性計測センサ１０４の出力情報と、位置姿勢センサ１０５の出力情報とに基づいて移動体の位置姿勢を推定する。本実施例においては位置姿勢センサ１０５としてのステレオカメラの出力情報に第２の重みを掛けたものと、慣性計測センサ１０４の出力情報に第１の重みを掛けたものとに基づいて移動体の位置姿勢を推定する。位置姿勢の推定方法については実施例１と同様であるため、説明は省略する。

【0054】

本実施例によれば、位置姿勢センサ１０５としての撮像装置又は距離計測装置等の出力情報に対する信頼度が低い場合に第２の重みを低く設定することで、不正確なセンサの出力情報の利用割合が下がり、位置姿勢推定の精度低下を軽減することができる。

【0055】

実施例２において、信頼度算出部２０１による位置姿勢センサ１０５の出力情報に対する信頼度の算出方法として、位置姿勢センサ１０５としてのステレオカメラ等の撮像装置で撮像した画像内の特徴点の数が少ない場合に信頼度を低くしたが、これに限らない。例えば、位置姿勢センサ１０５としてのステレオカメラ等の撮像装置で撮像した画像内における特徴点の分布に偏りがある場合に信頼度を低くしても良い。具体的には、ユーザが指定した又はシステムで予め設定された分割方法に応じて画面を複数の区間に分割し、区画毎に所定の閾値以上の数の特徴点が存在するか否かを判定する。そして、所定の閾値以上の数の特徴点が存在する区画数の総和が所定の閾値以下である場合に、偏りがあると判断する。

【0056】

又、画像内の平均輝度、もしくは外部のセンサにより計測した移動体の周囲の空間の明るさが明る過ぎる状態（画像の白飛び状態）、反対に暗過ぎる状態（画像の黒潰れ状態）において信頼度を低くしても良い。具体的には、画像内の画素値の平均が所定の閾値より大きい場合に明る過ぎると判断し、所定の閾値より小さい場合に暗過ぎると判断する。又は、照度センサを利用し、照度センサの値が閾値より大きい場合に明る過ぎると判断し、所定の閾値より小さい場合に暗すぎると判断しても良い。明る過ぎると判断するための閾値は、暗すぎると判断するための閾値より大きい値とする。

【0057】

或いは、急激に明るさが変化した場合や、照明変動が繰り返される場合など、輝度の変動が大きい場合にも信頼度を低くしても良い。具体的には、時系列に連続する画像を取得し、時間の進行に伴って画像内の平均輝度が所定の閾値以上変化した場合に急激に明るさが変化したと判断し、その状態が繰り返される場合に照明変動が繰り返されていると判断する。

【0058】

或いは、移動物体が多いことを検出した場合は、正確な地図情報が作成できないため、信頼度を低くしても良い。具体的には、時系列画像を機械学習等による物体認識を行い、時間の進行に伴って画像内の位置が移動している物体の数が多い場合に信頼度を低くする。もしくは、背景差分を用いた動体検知技術等を用いて、移動物体を検出しても良い。

【0059】

或いは、機械学習等による物体認識を行い、鏡面、光を透過する窓などが存在すると判断された場合も、特徴点が正確に取得できないため、信頼度を低くしても良い。又、似たような景色が連続するような場合も、現在地が判断できなくなるため、信頼度を低くしても良い。又、機械学習等による物体認識を行い、ステレオカメラの視界の大部分を占めるような大きな扉が存在すると判断された場合も、扉の開閉によって特徴点が変化してしまうため、信頼度を低くしても良い。

【0060】

即ち、画像の中の所定の被写体の有無に応じて信頼度を算出しても良い。又、ステレオカメラに限らず、同様に特徴点により位置姿勢を推定できる距離計測装置を用いても良い。更に、位置姿勢センサ１０５にＧＰＳを利用する場合は、建物の中など電波の受信状態が悪い場合などに信頼度を低くしても良い。これらの複数の信頼度の算出方法の少なくとも１つに基づいて信頼度を算出すれば良い。勿論、複数を組み合わせても良い。

【0061】

又、実施例２において、第２の重み決定部２０３による第２の重み決定方法として、信頼度が予め設定した閾値以下である場合に第２の重みを０、もしくは信頼度が閾値以下でない場合の値より低い値に設定するとしたが、これに限らない。例えば、信頼度の値が大きいほど第２の重みの値も段階的に大きくするようにしても良い。

【0062】

又、実施例２において、位置姿勢の推定は、位置姿勢センサ１０５の出力情報に第２の重みを掛けたものと、慣性計測センサ１０４の出力情報に第１の重みを掛けたものとに基づいて移動体の位置姿勢を推定するとしたがこれに限らない。例えば、第２の重みが所定の閾値以下である場合には、位置姿勢センサ１０５の出力情報を使用しないようにしても良い。

【0063】

又、位置姿勢センサ１０５の出力情報に第２の重みを掛けたものと、慣性計測センサ１０４の出力情報に第１の重み掛けたものの夫々で位置姿勢を推定して、双方の推定結果である位置姿勢を合成しても良い。
又は、位置姿勢センサ１０５の出力情報から位置姿勢を推定したものに第２の重みを掛けたものと、慣性計測センサ１０４の出力情報から位置姿勢を推定したものに第１の重みを掛けたものを合成しても良い。

【0064】

位置姿勢センサ１０５と慣性計測センサ１０４夫々での位置姿勢の推定方法や、その合成方法等は、実施例１と同様であるため、説明は省略する。又、第１の重みと第２の重みの双方が所定の閾値以下になるなど、低い値となる場合は、どちらのセンサの出力情報も信頼できないため、移動体を停止させるようにしても良い。又は、移動体を減速させるようにしても良い。又は、表示部２１６にてユーザに対する注意喚起の警告表示を行っても良い。又は、警告音を鳴らしても良い。

【実施例3】

【0065】

実施例３では、移動体の移動状態と、位置姿勢推定結果に基づいて、慣性計測センサの出力情報のノイズの影響が軽減するように、移動体の移動や旋回等の姿勢の変更を行う。
特に、移動体の移動状態が静止状態の場合に、移動や旋回等の姿勢の変更を行い、慣性計測センサの出力情報の値を大きくすることで、ノイズの影響を軽減し、位置姿勢推定の精度低下を軽減する例を説明する。

【0066】

実施例３の機能ブロックは、図１に示した機能ブロックと同一であるため説明は省略する。但し、実施例１とは、制御部１０８における処理が異なる。即ち、実施例３の制御部１０８は、移動状態取得部１０２にて取得した移動状態と、位置姿勢推定部１０７にて推定された位置姿勢に基づいて、慣性計測センサ１０４の出力情報のノイズ影響が軽減されるように、移動体の移動及び姿勢の変更を行う。

【0067】

図６は、実施例３における情報処理装置が実行する処理を示すフローチャートである。図３のフローチャートと同じステップについては説明を省略し、実施例１と異なる処理についてのみ説明する。
ステップＳ３０１では、移動状態取得部１０２にて取得した移動状態と、位置姿勢推定部１０７にて推定された位置姿勢に基づいて、慣性計測センサ１０４の出力情報のノイズ影響が軽減されるように、移動体の移動や旋回などの姿勢の変更を行う。ステップＳ３０１の処理の詳細については図７を用いて説明する。

【0068】

図７は、図６のステップＳ３０１の処理を示すフローチャートである。
ステップＳ４０１では、移動状態取得部１０２にて取得した移動状態が静止状態か否かを判定する。静止状態の場合はステップＳ４０２に進み、静止状態でない場合は図７のフローを終了し、図６のフローチャートのステップＳ１０６に進む。

【0069】

ステップＳ４０２では、位置姿勢推定部１０７にて推定された移動体の現在の位置、姿勢を考慮して、移動や姿勢の変更を行っても支障がないか判断する。移動体の移動や姿勢の変更を行っても支障がないか否かは、移動体の周囲、特に移動方向や旋回方向に障害物がなく、現在位置・向きと経路の位置・進行方向との差分が所定の閾値以下である場合に、移動や姿勢変更に支障がないと判断する。

【0070】

ステップＳ４０２にて移動や姿勢変更に支障がないと判断された場合にはステップＳ４０３に進む。ステップＳ４０３では、例えば移動体の位置が経路の位置・進行方向に対して所定の閾値以上ずれない範囲で移動体の移動量と進行方向を決定し、移動体の移動や姿勢の変更をさせる。

【0071】

このように、実施例３では、移動体の移動状態が静止状態の場合に、移動体の移動や姿勢の変更に支障がなければ移動や姿勢の変更を行う。それによって、慣性計測センサ１０４の出力情報の値を大きくし、慣性計測センサ１０４のノイズの影響を軽減し、位置姿勢推定の精度低下を軽減することができる。

【0072】

尚、実施例３において、移動体の移動状態が静止状態（又は動きが所定の閾値以下の状態）の場合に、移動体の移動等に支障がなければ移動体の移動を行うとしたが、これに限らない。例えば、移動体の加速度が所定の閾値以下であり、加速度が低い状態と決定され、ステップＳ４０２と同様に移動体の移動等に支障がないと判断された場合に、移動体の加減速を行うようにしても良い。

【0073】

又は、移動体の角速度が所定の閾値以下であり、角速度が低い状態と決定され、ステップＳ４０２と同様に移動体の移動等に支障がないと判断された場合に、移動体の移動や姿勢の変更を行うようにしても良い。又、これらの条件を複数組み合わせて判断をして移動を行っても良い。

【0074】

又、実施例３において、実施例２の位置姿勢センサ１０５の出力情報に対する信頼度を改善するように移動させる処理を加えても良い。例えば、位置姿勢センサ１０５としてステレオカメラや距離計測装置を利用している場合、画像の中の特徴点の数が少ない、もしくは特徴点の分布に偏りがある場合に、取得した画像内で特徴点がより多い場所に向かって移動するように移動体を移動させても良い。

【0075】

又、実施例２にて説明したような鏡面や、光を透過する窓、大きな扉などの信頼度を低くするような物体が画像内に存在する場合は、ステレオカメラや距離計測装置の画角外にその物体が出るように移動体を移動させても良い。更に、位置姿勢センサ１０５にＧＰＳを利用する場合は、建物の中や、障害物の影に入って電波状況が悪くなった場合に、建物や障害物の影から出て電波状態が良くなる位置まで移動させても良い。

【実施例4】

【0076】

実施例４では、慣性計測センサ１０４の出力情報に対する第１の重みを可視化することで、センサの利用状態が悪い場所をユーザに示し、経路の変更や、慣性計測センサ１０４のノイズ影響を軽減させるための移動操作を促すようにする。
本実施例の機能ブロックは、図１とほぼ同じであるが、画像生成部が存在する点が異なる。ここでは、実施例１と異なる部分についてのみ説明する。

【0077】

画像生成部は、第１の重み決定部１０３にて決定された第１の重みを表示した画像を生成し、表示部２１６に送信することで表示部において画面等にその画像を表示する。
図８は、実施例４において表示部２１６に表示されるＧＵＩの例を示す図である。
Ｇ１００は、表示部２１６に表示される、情報処理装置１００が出力した表示内容であり、画面全体に表示されても良いし、画面の一部分に表示されても良い。

【0078】

Ｇ１１０は、移動体の移動経路について移動空間を上から見た時の水平面を示す。本図の黒い部分は壁などであり、白い部分が通路を示している。移動体は、白い部分である通路を移動する。点線の矢印が走行経路を示しており、Ｇ１１１は移動体の現在位置を示すアイコンである。

【0079】

Ｇ１２０は、慣性計測センサ１０４の出力値に対する第１の重みを「利用度」として表示したものである。
尚、表示方法はこれに限らない。本図の例では、移動体が等速直線運動しているため、加速度がほぼ０になっている。このため、第１の重みが０．１となり、慣性計測センサ１０４の出力情報が０．１倍されている状態を示している。これにより、ユーザは慣性計測センサ１０４の出力情報の利用割合が低いことを把握できる。

【0080】

このように、実施例４では、慣性計測センサ１０４の出力情報に対する第１の重みを移動体の現在位置と共に可視化することで、慣性計測センサ１０４の利用状態が悪い場所をユーザに示すことができる。又、慣性計測センサ１０４のノイズ影響を軽減させるための経路の変更や、移動操作を促すことができる。

【0081】

尚、実施例４において、慣性計測センサ１０４の出力情報に対する第１の重みを可視化したが、位置姿勢センサ１０５の出力情報に対する第２の重みを可視化しても良い。又、両方を同時に可視化しても良い。尚、実施例４では重みを数値で表示したが、例えば、重みの値の大きさに応じて、アイコンＧ１１１の色や大きさを変えて表示しても良い。

【0082】

又、重みが所定の閾値以下になった場合に、アイコンＧ１１１を点滅させたり、色や大きさを変えたり、慣性計測センサ１０４の利用率低下の警告文を表示したりしても良い。複数のセンサに対して重みを設定している場合に、どちらのセンサに対する重みが低下したかを区別するために、アイコンＧ１１１の色分けを行っても良い。

【0083】

以上説明したように、実施例１では、移動体が静止状態の場合に第１の重みを低く設定することで、慣性計測センサ１０４の利用割合を下げ、慣性計測センサ１０４のノイズの影響を軽減することにより、位置姿勢推定の精度低下を軽減することができる。
又、実施例２では、撮像装置又は距離計測装置の出力情報に対する信頼度が低い場合に第２の重みを低く設定することで、不正確なセンサの出力情報の利用割合が下がり、位置姿勢推定の精度低下を軽減することができる。

【0084】

又、実施例３では、移動体の移動状態が静止状態の場合に移動を開始し、慣性計測センサ１０４の出力情報の値を大きくすることで、ノイズの影響を軽減し、位置姿勢推定の精度低下を軽減することができる。
実施例４では、慣性計測センサ１０４の出力情報に対する第１の重みを可視化することで、センサの利用状態が悪い場所をユーザに示すことができ、慣性計測センサ１０４のノイズ影響を軽減させるための経路の変更や、移動操作を促すことができる。

【0085】

尚、上記の実施例１～４の形態を適宜組み合わせて移動体の位置姿勢の推定処理を行っても良い。それによって、よりきめ細かに精度良く移動体の位置姿勢を推定することができる。
又、以上の実施例では慣性計測センサ１０４の他に位置姿勢センサ１０５を設けているが、実施例１、３、４は、位置姿勢センサ１０５がない構成においても適用できる。

【0086】

尚、以上の実施例において、移動体は、ＡＧＶやＡＭＲ（自律走行ロボット装置）の形態を有し、ＡＧＶやＡＭＲの移動（走行）を行うための移動用モータやエンジン等の駆動装置や、ＡＧＶやＡＭＲの移動方向を変更するための移動方向制御装置を有する。又、駆動装置の駆動量や移動方向制御装置の移動方向を制御する移動制御部を有する。

【0087】

移動制御部は内部にコンピュータとしてのＣＰＵと、コンピュータプログラムを記憶したメモリを内蔵しており、他の装置との通信を行う。それにより、例えば情報処理装置１００を制御すると共に、情報処理装置１００から位置姿勢情報や走行経路情報等を取得する。
移動体としてのＡＧＶやＡＭＲは、情報処理装置１００により生成された位置姿勢情報や走行経路情報等に基づき移動制御部によりＡＧＶやＡＭＲの移動方向や移動量や移動経路を制御するように構成されている。

【0088】

以上、本発明をその好適な実施例に基づいて詳述してきたが、本発明は上記実施例に限定されるものではなく、本発明の主旨に基づき種々の変形が可能であり、それらを本発明の範囲から除外するものではない。
尚、本実施例における制御の一部又は全部を上述した実施例の機能を実現するコンピュータプログラムをネットワーク又は各種記憶媒体を介して情報処理等に供給するようにしてもよい。そしてその情報処理等におけるコンピュータ（又はＣＰＵやＭＰＵ等）がプログラムを読み出して実行するようにしてもよい。その場合、そのプログラム、及び該プログラムを記憶した記憶媒体は本発明を構成することとなる。

【符号の説明】

【0089】

１００：情報処理装置
１０１：移動状態決定部
１０２：移動状態取得部
１０３：第１の重み決定部
１０４：慣性計測センサ
１０５：位置姿勢センサ
１０６：センサ情報取得部
１０７：位置姿勢推定部
１０８：制御部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】