JP7346342B2 - 計測装置、計測方法およびプログラム - Google Patents

Description

本開示は、計測装置、計測方法およびプログラムに関する。

特許文献１には、携帯端末が有する動きセンサを用いた位置の推定値を、絶対的な位置が決まっている送信機から送信された無線信号を用いて調整する位置推定方法が開示されている。

特表２０１７－５３４０４７号公報

しかしながら、特許文献１に記載の位置推定方法では、無線信号の受信環境によっては精度良く位置を推定することができない場合があるという課題があった。

本開示は、上記課題を解決するためになされたものであって、精度良く位置を推定することができる計測装置、計測方法およびプログラムを提供することを目的とする。

上記課題を解決するために、本開示に係る計測装置は、撮影した画像に基づき位置と移動方向を推定する第１推定部と、動きに応じて電気信号を出力する動きセンサの出力に基づき位置と移動方向を推定する第２推定部と、前記第１推定部が推定した位置または前記第２推定部が推定した位置のいずれかを選択する制御部とを備え、前記制御部が、前記第１推定部が位置を推定できなくなった場合に、前記第１推定部が位置を推定できなくなる前に推定した位置と移動方向を初期値として前記第２推定部に位置と移動方向とを推定させる。

本開示に係る計測方法は、撮影した画像に基づき位置と移動方向を推定する第１の推定ステップと、動きに応じて電気信号を出力する動きセンサの出力に基づき位置と移動方向を推定する第２の推定ステップと、前記第１の推定ステップで推定した位置または前記第２の推定ステップで推定した位置のいずれかを選択する選択ステップと、を有し、前記第１の推定ステップで位置を推定できなくなった場合に、前記第１の推定ステップで位置を推定できなくなる前に推定した位置と移動方向とを初期値として前記第２の推定ステップにおいて位置と移動方向とを推定させる。

本開示に係るプログラムは、計測装置のコンピュータに、撮影した画像に基づき位置と移動方向を推定する第１の推定ステップと、動きに応じて電気信号を出力する動きセンサの出力に基づき位置と移動方向を推定する第２の推定ステップと、前記第１の推定ステップで推定した位置または前記第２の推定ステップで推定した位置のいずれかを選択する選択ステップと、を実行させ、前記第１の推定ステップで位置を推定できなくなった場合に、前記第１の推定ステップで位置を推定できなくなる前に推定した位置と移動方向とを初期値として前記第２の推定ステップにおいて位置と移動方向とを推定させる。

本開示の計測装置、計測方法およびプログラムによれば、精度良く位置を推定することができる。

本開示の実施形態に係る計測装置の構成例を示すブロック図である。 図１に示すカメラ２１の設置例を示す模式図である。 図１に示すＳＬＡＭ制御部５２の動作例を示すフローチャートである。 図１に示す計測装置１００の動作例を説明するための模式図である。 図１に示す計測装置１００の動作例を説明するための模式図である。 図１に示す計測装置１００の動作例を説明するための模式図である。 図１に示す計測装置１００の動作例を示すフローチャートである。 図１に示す計測装置１００の動作例を示すタイミングチャートである。 図１に示す計測装置１００の他の動作例を示すフローチャートである。 少なくとも１つの実施形態に係るコンピュータの構成を示す概略ブロック図である。

＜第一実施形態＞
以下、本開示の第一実施形態に係る計測装置について、図１～図８を参照して説明する。

（計測装置の構成）
図１は、本開示の実施形態に係る計測装置の構成例を示すブロック図である。図１に示す計測装置１００は、例えば、ウェアラブルコンピュータ、タブレット端末、スマートフォン等の携帯型コンピュータ等のコンピュータとその周辺装置等から構成することができる。また、図１に示す計測装置１００は、コンピュータ、その周辺装置等のハードウェアと、コンピュータが実行するプログラム等のソフトウェアとの組み合わせで構成される機能的構成として、ＰＤＲ情報取得部１と、画像情報取得部２と、出力部３と、記憶部４と、位置推定制御部５を備える。なお、図１では、ＰＤＲ情報取得部１、画像情報取得部２、出力部３、記憶部４、および位置推定制御部５を、計測装置１００を表すブロック内に示しているが、各部が含む（あるいは各部が利用する）センサ、カメラ等の一部のハードウェアは、計測装置１００の本体（コンピュータを備える筐体）に対して有線または無線で接続され、別体として構成されていてもよい。また、位置推定制御部５内の一部の構成は、計測装置１００と例えば無線で接続された図示していないサーバ等の他のコンピュータ内に含まれていてもよい。

ＰＤＲ情報取得部１は、ＰＤＲ（Ｐｅｄｅｓｔｒｉａｎ Ｄｅａｄ－Ｒｅｃｋｏｎｉｎｇ；歩行者自律航法）技術による測位に用いる情報（ＰＤＲ情報とする）を取得する。ＰＤＲ情報取得部１は、加速度センサ１１、角速度センサ１２等を含み、計測装置１００（あるいはＰＤＲ情報取得部１）に生じた加速度（例えば３軸加速度）、角速度（例えば３軸角速度）等を検知して、検知した結果を示す信号を所定の周期で繰り返し出力する。ＰＤＲ情報取得部１が出力したＰＤＲ情報は例えば記憶部４に一定時間記憶される。なお、ＰＤＲ情報は、例えば、加速度を表す情報、角速度を表す情報等、動きに応じて電気信号を出力する動きセンサの出力値を表す情報である。本実施形態において、加速度センサ１１および角速度センサ１２は、加速度センサ１１または角速度センサ１２に発生した動き（加速度または角速度）に応じて電気信号を出力する動きセンサの一例である。また、ＰＤＲ情報取得部１は、他の動きセンサとして、磁気センサ、気圧センサ等を含んでいてもよい。

画像情報取得部２は、カメラ２１を含み、カメラ２１が撮影した画像（動画像）を表す映像信号（画像情報）を出力する。画像情報取得部２が出力した画像情報は例えば記憶部４に一定時間記憶される。カメラ２１は、例えば、図２に示すように、作業者１０１がかぶるヘルメット２０１に取り付けられていて、矢印２０２で示す作業者（被計測者）１０１の前方の画像を撮影する。図２は、カメラ２１の設置例を示す模式図である。なお、作業者１０１は、計測装置１００を携帯している。

出力部３は、計測装置１００が備える図示していない表示装置、通信装置等を用いて、ＰＤＲ情報取得部１が取得したＰＤＲ情報、画像情報取得部２が取得した画像情報、位置推定制御部５が推定した位置推定結果等を、表示したり、外部の図示していないサーバ等へ送信したりする。

記憶部４は、ＰＤＲ情報取得部１が取得したＰＤＲ情報、画像情報取得部２が取得した画像情報、位置推定制御部５が推定した位置推定結果、位置を推定する際に処理した中間情報、後述する地図の情報等を記憶する。

位置推定制御部５は、制御部５１と、ＳＬＡＭ制御部５２と、ＰＤＲ制御部５３と、動作判定制御部５４と、補正誤差制御部５５とを含む。

制御部５１は、ＳＬＡＭ制御部５２と、ＰＤＲ制御部５３と、動作判定制御部５４と、補正誤差制御部５５を制御する。

ＳＬＡＭ制御部５２は、画像情報取得部２が取得した画像情報を基に、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）技術を用いて自己位置推定と地図生成を行い、位置と移動方向を推定する。ここで、図３を参照して、ＳＬＡＭ制御部５２の動作の概要について説明する。図３は、図１に示すＳＬＡＭ制御部５２の動作例を示すフローチャートである。ＳＬＡＭ制御部５２は、まず、Ｉｎｉｔｉａｌｉｚａｔｉｏｉｎ（初期化）処理として、座標系における初期位置とマップの取得および決定を行う（ステップＳ０）。次に、ＳＬＡＭ制御部５２は、Ｔｒａｃｋｉｎｇ（トラッキング）処理として、１フレーム前の画像と現在の画像との比較による位置推定を行う（ステップＳ１）。また、ＳＬＡＭ制御部５２は、Ｔｒａｃｋｉｎｇ処理と並行し、Ｍａｐｐｉｎｇ（マッピング）処理として、Ｔｒａｃｋｉｎｇ処理の際に画像から検出された現在の画像特徴点を座標系へマッピングする（ステップＳ２）。

また、ＳＬＡＭ制御部５２は、Ｔｒａｃｋｉｎｇ処理においてトラッキングが失敗した場合（ロストした場合）、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ（リローカライゼーション）処理として、通過点の画像群と現在の画像の特徴点比較による位置の再推定を行った後（ステップＳ３）、Ｔｒａｃｋｉｎｇ処理を再度実行する（ステップＳ１）。

また、ＳＬＡＭ制御部５２は、Ｔｒａｃｋｉｎｇ処理においてループを検知した場合、Ｌｏｏｐ Ｃｌｏｓｕｒｅ（ループ閉じ込み）処理として、同一地点（ループ）の検知を起点とした軌跡の最適化処理を行った後（ステップＳ４）、Ｔｒａｃｋｉｎｇ処理を再度実行する（ステップＳ１）。ここで、ループ検知とは、同一地点に戻ったことを知ったことである。

ＳＬＡＭ制御部５２は、所定の周期で繰り返し画像情報を取り込み、例えば図３に示す処理を実行し、自己位置推定と地図生成を行う。ＳＬＡＭ制御部５２は、Ｔｒａｃｋｉｎｇ処理を行っている場合、推定した位置および移動方向と作成した地図を表す情報を随時、記憶部４に記憶したり、出力部３に出力したりする。また、ＳＬＡＭ制御部５２は、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理を行っている場合、ロストしていることを示す信号を制御部５１へ出力する。

一方、ＰＤＲ制御部５３は、ＰＤＲ情報取得部１が取得したＰＤＲ情報に基づき、作業者１０１の移動速度や移動方向（進行方向）を推定し、推定した移動速度と移動方向に基づき位置を推定する。その際、ＰＤＲ制御部５３は、ＳＬＡＭ制御部５２が推定した位置と移動方向を初期値として位置と移動方向を推定する。ＰＤＲ制御部５３は、例えば、加速度センサ１１の値を用いて推定される作業者１０１の歩数と歩幅から移動距離を推定したり、加速度センサ１１の値から移動速度を推定し、移動速度を時間積分することで移動距離を推定したりする。

ＰＤＲ制御部５３は、ＰＤＲ情報を取り込み、取り込んだＰＤＲ情報に基づき、所定の周期で繰り返し、位置と移動方向を推定し、推定した位置および移動方向を表す情報を随時、記憶部４に記憶したり、出力部３に出力したりする。なお、ＳＬＡＭ制御部５２はロストしている場合は位置と移動方向を推定することはできないが、ＰＤＲ制御部５３は、初期化後は、常時、位置と移動方向を表す情報を所定の周期で推定することができる。

動作判定制御部５４は、ＰＤＲ情報取得部１が取得したＰＤＲ情報と画像情報取得部２が取得した画像情報を基に、作業者１０１の動作（静止しているのか、作業しているのか、または、歩行しているのか）を判定し、歩行していないと判定した場合（静止または作業であると判定した場合）、歩行していると判定したときに最後に推定された位置を、位置の推定結果とする。

その際、動作判定制御部５４は、次のようにしてＰＤＲ情報に基づき歩行か否かを判定することができる。すなわち、例えば図４に示すように加速度センサ１１の出力値が歩行時に発生する一定の周期的パターンで変化している場合、動作判定制御部５４は、作業者１０１が歩行していると判定することができる。あるいは、例えば加速度センサ１１の出力値の大きさ（例えば振幅）が一定の値を超えていない場合、動作判定制御部５４は、作業者１０１が静止していると判定することができる。なお、図４は、図１に示す計測装置１００の動作例を説明するための模式図であり、横軸を時刻とし、縦軸を加速度センサ値として、歩行時における加速度センサ１１の出力値の例を模式的に示す。

また、動作判定制御部５４は、次のようにして画像情報に基づき歩行か否かを判定することができる。すなわち、動作判定制御部５４は、例えば図５に示すように異なる複数フレーム（例えば連続する２フレーム）の画像３０１および３０２から、画像認識によって、複数の画像特徴点ＦＰを抽出し、画像３０１および３０２において対応する（同一の）画像特徴点同士（例えば、画像特徴点３０１ａと画像特徴点３０２ａ、画像特徴点３０１ｂと画像特徴点３０２ｂ）のフレーム間の変化に基づいて歩行か否かを判定することができる。ここで、フレーム間の変化は、例えば、画像特徴点の画像上の位置（座標値）の変化、複数の画像特徴点間の距離や角度の変化等とすることができる。あるいは、動作判定制御部５４は、撮影された画像に所定の物体が認識された場合（例えば、作業設備や作業工具が認識された場合）に、作業状態である（非歩行状態である）と判定してもよい。なお、図５は、図１に示す計測装置１００の動作例を説明するための模式図であり、連続する２フレームの画像３０１と画像３０２の例を模式的に示す。

あるいは、動作判定制御部５４は、例えば、次のようにして、特徴量変化に基づき歩行か否かを判定することができる。すなわち、時刻ｔ（ｎ）におけるフレームｆ（ｔ（ｎ））の画像特徴量と、時刻ｔ（ｎ＋１）におけるフレームｆ（ｔ（ｎ＋１））における画像特徴量を比較し、画像特徴量の変化量Δｄを導出する。そして、変化量Δｄが閾値Ｔｈよりも大きければ歩行として判定し、小さければ歩行ではないと判定する。ただし、必ずしも一時刻前のフレームｆ（ｔ（ｎ））との比較にする必要はなく、例えば、比較する時間窓を定義し、比較するフレームをｔ（ｎ）よりも前のフレームとしてもよい。作業時は、基本的には、作業位置が変わらないこと、視点が大きく変化しないこと（キョロキョロしながら作業できない）から、変化量Δｄは小さくなる。あるいは、動作判定制御部５４は、作業時の画像特徴量を学習データとして与えることで、機械学習的に作業しているか否かを判定するようにしてもよい。

また、補正誤差制御部５５は、ＳＬＡＭ制御部５２が推定した位置および移動方向とＰＤＲ制御部５３が推定した位置および移動方向との誤差を、時間に応じて推定する関数（モデル）を用いて、ＰＤＲ制御部５３が推定した位置と移動方向を補正する。ＰＤＲ制御部５３による測位では、加速度センサ１１と角速度センサ１２を用いて、相対的に作業者１０１の移動方向と移動距離を求めるが、これらセンサでは経時的にドリフト誤差が累積してしまう。そこで、本実施形態では、精度高く取得可能なＳＬＡＭ制御部５２による推定結果を真値とみなし、ＰＤＲ制御部５３による推定結果と比較することで、加速度センサ１１と角速度センサ１２のドリフトにより起因して経時的に生じる誤差を、機械学習して推定するモデル（ＰＤＲ累積誤差モデルＦ（ｔ））を用いて、ＰＤＲ制御部５３単独による推定時の精度を向上させている。図６は、図１に示す計測装置１００の動作例を説明するための模式図であり、横軸を動作時間とし、縦軸をＳＬＡＭ制御部５２による推定結果とＰＤＲ制御部５３による推定結果との誤差として、動作時間と誤差計測値とＰＤＲ累積誤差モデルＦ（ｔ）の値との関係を示す。

本実施形態では、ＰＤＲ制御部５３による推定計算時に、機械学習により推定されたＰＤＲ累積誤差モデルＦ（ｔ）を考慮して、位置推定することにより、加速度センサ１１と角速度センサ１２によるドリフトを軽減でき、精度を向上させることができる。この場合、例えば、推定位置Ｐは、以下の式により求めることができる。

［推定位置］Ｐ＝［ＰＤＲ推定位置］ＰＰＤＲ＋［累積誤差］Ｆ（ｔ）

ここで、［ＰＤＲ推定位置］ＰＰＤＲは、ＰＤＲ制御部５３が動作時間ｔにおいて推定した位置である。なお、ＰＤＲ累積誤差モデルＦ（ｔ）は、位置の誤差を推定するモデルと、方向の誤差を推定するモデルを別々に用意しておくことができる。また、位置の誤差を推定するモデルと方向の誤差を推定するモデルは、センサの検出軸ごとに複数（例えば２つのセンサの３軸分の６個）用意しておくことができる。

また、ＰＤＲ累積誤差モデルＦ（ｔ）は、例えば以下のようにして構築することができる。まず、ＳＬＡＭ制御部５２で計測できた区間における、「ＳＬＡＭ制御部５２による位置推定結果と方位推定結果（Ｖ－ＳＬＡＭデータ）」と「ＰＤＲ制御部５３による位置推定結果と方位推定結果（ＰＤＲデータ）」とを比較する。この際、Ｖ－ＳＬＡＭデータを真値として、ＰＤＲデータと真値との位置推定誤差ＰｏｓｉｔｉｏｎＥｒｒｏｒ、方位推定誤差ＰｏｓｅＥｒｒｏｒを取得していく。そして、取得したＰｏｓｉｔｉｏｎＥｒｒｏｒとＰｏｓｅＥｒｒｏｒを統計分析し、時系列に依存する位置誤差推定モデルＰｏｓｉｔｉｏｎＥｒｒｏｒＥｓｔｉｍａｔｅ（ｔ）、方位誤差推定モデルＰｏｓｅＥｒｒｏｒＥｓｔｉｍａｔｅ（ｔ）を決定する。

なお、ＰＤＲ累積誤差モデルＦ（ｔ）は、予め実測値やシミュレーション結果等に基づいて作成されたものであってもよいし、実際の（現場での）動作時に取得した実測値に基づき学習することで作成されたものであってもよいし、予め実測値やシミュレーション結果に基づいて作成されたものを実測値に基づいて学習することで更新されたものであってもよい。

（計測装置の動作例）
次に、図７と図８を参照して、図１に示す計測装置１００の動作例について説明する。図７は、図１に示す計測装置１００の動作例を示すフローチャートである。図８は、図１に示す計測装置１００の動作例を示すタイミングチャートである。

図７に示す処理は、ＰＤＲ情報や画像情報のサンプリング周期に合わせて繰り返し実行される。なお、ＳＬＡＭ制御部５２は、図７の処理とは別に所定の周期で位置と移動方向を推定し、例えば制御部５１からの要求に応じて、推定結果を示す情報またはロストしていることを示す信号を制御部５１に対して出力する。また、ＰＤＲ制御部５３は、図７の処理とは別に所定の周期で位置と移動方向を推定し、例えば制御部５１からの要求に応じて、推定結果を示す情報を制御部５１に対して出力する。

図７に示す処理が開始されると、制御部５１が、ＰＤＲ情報取得部１、画像情報取得部２または記憶部４からＰＤＲ情報（加速度と角速度）と画像情報（画像）を取得する（ステップＳ１０１）。次に、制御部５１が、ＳＬＡＭ制御部５２による位置推定が可能か否かを判定する（ステップＳ１０２）。位置推定が可能な場合（ロストしていない場合）（ステップ１０２で「推定可」の場合）、制御部５１は、推定可フラグを１（＝推定可）に設定し（ステップＳ１０３）、ＳＬＡＭ制御部５２による推定結果を選択し（ステップＳ１０４）、選択した推定結果を出力部４から出力し（ステップＳ１１６）、図７に示す処理を終了する。なお、推定可フラグは、１で推定可、０で推定不可を表す変数で、ＳＬＡＭ制御部５２がロストした後、ＰＤＲ制御部５３の位置を初期化する回数を１回に限定するために用いられる。

一方、位置推定が可能でない場合（ロストしている場合）（ステップ１０２で「推定不可」の場合）、制御部５１は、推定可フラグが１であるか否かを判定する（ステップＳ１０５）。推定可フラグが１の場合（ステップＳ１０５で「ＹＥＳ」の場合）、制御部５１は、推定可フラグを０（＝推定不可）に設定し（ステップＳ１０６）、ＳＬＡＭ制御部５２がロストする前に推定した位置と移動方向を初期値として、ＰＤＲ制御部５３に位置と移動方向を推定させる（ステップＳ１０７）。

推定可フラグが１でない場合（ステップＳ１０５で「ＮＯ」の場合）、または、ステップＳ１０７の後、動作判定制御部５４が、加速度による歩行判定を行う（ステップＳ１０８）。歩行でないと判定した場合（ステップＳ１０８で「非歩行」の場合）、動作判定制御部５４は、作業者１０１が静止していると判断する（ステップＳ１０９）。他方、歩行であると判定した場合（ステップＳ１０８で「歩行」の場合）、動作判定制御部５４は、画像による歩行判定を行う（ステップＳ１１０）。歩行でないと判定した場合（ステップＳ１１０で「非歩行」の場合）、動作判定制御部５４は、作業者１０１が作業をしていると判断する（ステップＳ１１１）。他方、歩行であると判定した場合（ステップＳ１１０で「歩行」の場合）、動作判定制御部５４は、作業者１０１が移動していると判断する（ステップＳ１１２）。次に、制御部５１が、ＰＤＲ制御部５３による位置と移動方向の推定結果を選択し（ステップＳ１１３）、補正誤差制御部５５が、ＰＤＲ制御部５３による位置と移動方向の推定値をＰＤＲ累積誤差モデルＦ（ｔ）を用いて補正する（ステップＳ１１４）。次に、制御部５１が、選択して補正されたＰＤＲ制御部５３による推定結果を出力部４から出力し（ステップＳ１１６）、図７に示す処理を終了する。

一方、動作判定制御部５４は、ステップＳ１０９で静止と判断した場合またはステップＳ１１１で作業と判断した場合、ＰＤＲ制御部５３の位置と移動方向の推定値を、一時刻前の位置と移動方向と同じ値に設定する（ステップＳ１１５）。一時刻前の位置と移動方向の値は、加速度による歩行判定（ステップＳ１０８）と画像による歩行判定（ステップＳ１１０）が両方とも「歩行」であった期間の最後に推定された位置と移動方向の値またはステップＳ１０７での初期値である。次に、制御部５１は、一時刻前と同位置の値に設定されたＰＤＲ制御部５３による推定結果を出力部４から出力し（ステップＳ１１６）、図７に示す処理を終了する。

図７に示す動作例によれば、図７に示すように、ＳＬＡＭ制御部５２が位置を推定できる時間はＳＬＡＭ制御部５２が推定した位置と移動方向を選択し、ＳＬＡＭ制御部５２がロストしている時間はＰＤＲ制御部５３が推定した位置と移動方向を選択して、作業者１０１の位置と移動方向の推定値とすることができる。すなわち、本実施形態によれば、ＳＬＡＭでロストしている時間では、ＰＤＲベースで位置推定することにより補間することができる。

図８において、例えば、時刻ｔ０から時刻ｔ１までの時間は、ＳＬＡＭ制御部５２で取得した座標情報を位置推定結果としている。

また、時刻ｔ１でロストが発生した場合、ロストが発生する直前の時刻ｔ１－△ｔで取得した座標と進行方向Ｐｔ１（ｘ１、ｙ１、θ１）を初期位置、初期方位として、加速度センサ１１、角速度センサ１２を用いてＰＤＲ制御部５３による測位計算を行い、その結果を位置推定結果としている。これにより、ＰＤＲ情報を取得するセンサに累積している誤差をキャリブレーションすることができる。なお、△ｔは、図７に示す処理の繰り返し周期以下の時間である。

また、時刻ｔ２でＳＬＡＭ制御部５２におけるリローカライゼーション処理が完了した場合、リローカライゼーション処理完了以降はＳＬＡＭ制御部５２で取得した座標情報を位置推定結果としている。

（計測装置の作用効果）
ＰＤＲによる測位は、相対的な位置を推定する技術であり、与えられた位置と方向を初期値（基準値）として位置の変化と方向の変化を積算することで現在の位置と方向が推定される。位置と方向の初期値は、例えば、設置位置が既知のビーコンを用いて取得することができる。ただし、この場合、電波の受信環境によっては、初期値を精度良く取得できない場合がある。これに対し、本実施形態では、ＳＬＡＭによる測位と、ＰＤＲによる測位を組み合わせるとともに、ＳＬＡＭによる測位を基本として、ＳＬＡＭがロストした場合にＰＤＲによる測位結果を補間的に採用する。さらに、ＰＤＲによる測位では、ＳＬＡＭから切り替わる毎にＳＬＡＭで推定された位置と方向を初期値として、測位の推定を初期化することができる。よって、受信環境等に影響されず、精度良く位置と方向を推定することができる。また、本実施形態によれば、例えば、金属遮蔽環境においても、エリア判定が可能となり、現場改善につながるエリア分析が可能となった。また、例えば、作業者１０１に、首を振るとか、後ろを向くとか等の動きがあった場合、例えば画像の撮影が追いつかなくなることで、ＳＬＡＭによる測位がロストしてしまうことが考えられるが、本実施形態では、そのような場合でもＰＤＲによる測位で位置を推定することができる。

また、本実施形態によれば、加速度値に基づく歩行判定と画像認識に基づく歩行判定とを組み合わせた高度な判定処理を用いているので、歩行時の動きと、作業時または静止時の動きとを判別し、歩行をしていない場合の作業時または静止時の動きによって誤って位置が移動してしまうことを避けることができる。

また、本実施形態によれば、ＰＤＲ累積誤差モデルＦ（ｔ）（＝累積誤差推定関数）を用いることで、センサ値のドリフトによる影響を抑えることができるので、精度良く位置と方向を推定することができる。

＜第二実施形態＞
以下、本開示の第二実施形態に係る計測装置について、図９を参照して説明する。第二実施形態は、第一実施形態に係る計測装置１００において、図７を参照して説明した第一実施形態に係る計測装置１００の動作を一部変更したものである。図１～図６と図８を参照して説明した基本的な構成と動作については、第一実施形態と第二実施形態で共通である。以下、主に、第一実施形態と異なる点について説明する。図９は、図１に示す計測装置１００の他の動作例（第二実施形態の動作例）を示すフローチャートである。

図９に示す処理は、ＰＤＲ情報や画像情報のサンプリング周期に合わせて繰り返し実行される。なお、ＳＬＡＭ制御部５２は、図９の処理とは別に所定の周期で位置と移動方向を推定し、例えば制御部５１からの要求に応じて、推定結果を示す情報またはロストしていることを示す信号を制御部５１に対して出力する。また、ＰＤＲ制御部５３は、図９の処理とは別に所定の周期で位置と移動方向を推定し、例えば制御部５１からの要求に応じて、推定結果を示す情報を制御部５１に対して出力する。

図９に示す処理が開始されると、制御部５１が、ＰＤＲ情報取得部１、画像情報取得部２または記憶部４からＰＤＲ情報（加速度と角速度）と画像情報（画像）を取得する（ステップＳ２０１）。次に、動作判定制御部５４が、加速度による歩行判定を行う（ステップＳ２０２）。歩行でないと判定した場合（ステップＳ２０２で「非歩行」の場合）、動作判定制御部５４は、作業者１０１が静止していると判断する（ステップＳ２０３）。他方、歩行であると判定した場合（ステップＳ２０２で「歩行」の場合）、動作判定制御部５４は、画像による歩行判定を行う（ステップＳ２０４）。歩行でないと判定した場合（ステップＳ２０４で「非歩行」の場合）、動作判定制御部５４は、作業者１０１が作業をしていると判断する（ステップＳ２０５）。他方、歩行であると判定した場合（ステップＳ２０４で「歩行」の場合）、動作判定制御部５４は、作業者１０１が移動していると判断し（ステップＳ２０６）、次に、ＳＬＡＭ制御部５２による位置推定が可能か否かを判定する（ステップＳ２０７）。位置推定が可能な場合（ロストしていない場合）（ステップ２０７で「推定可」の場合）、制御部５１は、推定可フラグを１（＝推定可）に設定し（ステップＳ２０８）、ＳＬＡＭ制御部５２による推定結果を選択し（ステップＳ２０９）、選択した推定結果を出力部４から出力し（ステップＳ２１５）、図９に示す処理を終了する。

一方、動作判定制御部５４は、ステップＳ２０３で静止と判断した場合またはステップＳ２０５で作業と判断した場合、ＳＬＡＭ制御部５２またはＰＤＲ制御部５３の位置と移動方向の推定値を、一時刻前の位置と移動方向と同じ値に設定する（ステップＳ２１７）。一時刻前の位置と移動方向の値は、加速度による歩行判定（ステップＳ２０２）と画像による歩行判定（ステップＳ２０４）が両方とも「歩行」であった期間の最後に推定された位置と移動方向の値である。次に、制御部５１は、一時刻前と同位置の値とされた推定結果を出力部４から出力し（ステップＳ２１５）、図９に示す処理を終了する。

他方、位置推定が可能でない場合（ロストしている場合）（ステップ２０７で「推定不可」の場合）、制御部５１は、推定可フラグが１であるか否かを判定する（ステップＳ２１０）。推定可フラグが１の場合（ステップＳ２１０で「ＹＥＳ」の場合）、制御部５１は、推定可フラグを０（＝推定不可）に設定し（ステップＳ２１１）、ＳＬＡＭ制御部５２がロストする前に推定した位置と移動方向を初期値として、ＰＤＲ制御部５３に位置と移動方向を推定させる（ステップＳ２１２）。

推定可フラグが１でない場合（ステップＳ２１０で「ＮＯ」の場合）、または、ステップＳ２１２の後、制御部５１が、ＰＤＲ制御部５３による位置と移動方向の推定結果を選択し（ステップＳ２１３）、補正誤差制御部５５が、ＰＤＲ制御部５３による位置と移動方向の推定値をＰＤＲ累積誤差モデルＦ（ｔ）を用いて補正する（ステップＳ２１４）。次に、制御部５１が、選択して補正されたＰＤＲ制御部５３による推定結果を出力部４から出力し（ステップＳ２１５）、図９に示す処理を終了する。

第一実施形態では、ＳＬＡＭ制御部５２が位置を推定可能な場合、動作判定制御部５４による歩行判定の結果に関わらず、ＳＬＡＭ制御部５２が推定した位置と移動方向が選択され、出力される。一方、第二実施形態では、ＳＬＡＭ制御部５２が位置を推定可能であっても、動作判定制御部５４による歩行判定の結果が静止または作業である場合、一時刻前の位置と方向の値が、出力される。

（その他の実施形態）
以上、本開示の実施の形態について図面を参照して詳述したが、具体的な構成はこの実施の形態に限られるものではなく、本開示の要旨を逸脱しない範囲の設計変更等も含まれる。例えば、図２では、カメラ２１をヘルメット２０１に取り付けているが、例えばめがね型のウェアラブルコンピュータを用いて計測装置１００全体を作業者１０１がめがねとして装着するようにしてもよい。

〈コンピュータ構成〉
図１０は、少なくとも１つの実施形態に係るコンピュータの構成を示す概略ブロック図である。
コンピュータ９０は、プロセッサ９１、メインメモリ９２、ストレージ９３、インタフェース９４を備える。
上述の計測装置１００は、コンピュータ９０に実装される。そして、上述した各処理部の動作は、プログラムの形式でストレージ９３に記憶されている。プロセッサ９１は、プログラムをストレージ９３から読み出してメインメモリ９２に展開し、当該プログラムに従って上記処理を実行する。また、プロセッサ９１は、プログラムに従って、上述した各記憶部に対応する記憶領域をメインメモリ９２に確保する。

プログラムは、コンピュータ９０に発揮させる機能の一部を実現するためのものであってもよい。例えば、プログラムは、ストレージに既に記憶されている他のプログラムとの組み合わせ、または他の装置に実装された他のプログラムとの組み合わせによって機能を発揮させるものであってもよい。なお、他の実施形態においては、コンピュータは、上記構成に加えて、または上記構成に代えてＰＬＤ（Programmable Logic Device）などのカスタムＬＳＩ（Large Scale Integrated Circuit）を備えてもよい。ＰＬＤの例としては、ＰＡＬ(Programmable Array Logic)、ＧＡＬ(Generic Array Logic)、ＣＰＬＤ(Complex Programmable Logic Device)、ＦＰＧＡ（Field Programmable Gate Array）が挙げられる。この場合、プロセッサによって実現される機能の一部または全部が当該集積回路によって実現されてよい。

ストレージ９３の例としては、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）、磁気ディスク、光磁気ディスク、ＣＤ－ＲＯＭ（Compact Disc Read Only Memory）、ＤＶＤ－ＲＯＭ（Digital Versatile Disc Read Only Memory）、半導体メモリ等が挙げられる。ストレージ９３は、コンピュータ９０のバスに直接接続された内部メディアであってもよいし、インタフェース９４または通信回線を介してコンピュータ９０に接続される外部メディアであってもよい。また、このプログラムが通信回線によってコンピュータ９０に配信される場合、配信を受けたコンピュータ９０が当該プログラムをメインメモリ９２に展開し、上記処理を実行してもよい。少なくとも１つの実施形態において、ストレージ９３は、一時的でない有形の記憶媒体である。

＜付記＞
各実施形態に記載の計測装置１００は、例えば以下のように把握される。

（１）第１の態様に係る計測装置１００は、撮影した画像に基づき位置と移動方向を推定する第１推定部（ＳＬＡＭ制御部５２）と、動きに応じて電気信号を出力する動きセンサ（加速度センサ１１、角速度センサ１２）の出力に基づき位置と移動方向を推定する第２推定部（ＰＤＲ制御部５３）と、前記第１推定部が推定した位置または前記第２推定部が推定した位置のいずれかを選択する制御部５１とを備え、前記制御部５１が、前記第１推定部が位置を推定できなくなった場合に、前記第１推定部が位置を推定できなくなる前に推定した位置と移動方向を初期値として前記第２推定部に位置と移動方向を推定させる。この態様によれば、精度良く位置と方向を推定することができる。

（２）第２の態様に係る計測装置１００は、（１）の計測装置１００であって、前記動きセンサは加速度センサ１２を含み、前記加速度センサ１２の出力が歩行状態を示している否かを判定するとともに（Ｓ１０８）、前記画像が歩行状態を示しているか否かを判定し（Ｓ１１０）、両判定が歩行状態を示していなかった場合、前記両判定が歩行状態を示さなくなる前の位置を、前記第２推定部（ＰＤＲ制御部５３）が推定した位置とする（Ｓ１１５）動作判定部（動作判定制御部５４）をさらに備える。この態様によれば、歩行をしていない場合の作業時または静止時の動きによって誤って位置が移動してしまうことを避けることができる。

（３）第３の態様に係る計測装置１００は、（１）または（２）の計測装置１００であって、前記第１推定部（ＳＬＡＭ制御部５２）が推定した位置および移動方向と前記第２推定部（ＰＤＲ制御部５３）が推定した位置および移動方向との誤差を、時間に応じて推定する関数を用いて、前記第２推定部（ＰＤＲ制御部５３）が推定した位置と移動方向を補正する誤差補正部（補正誤差制御部５５）をさらに備える。この態様によれば、センサ値のドリフトによる位置推定への影響を抑えることができる。

１００ 計測装置
１ ＰＤＲ情報取得部
２ 画像情報取得部
３ 出力部
４ 記憶部
５ 位置推定制御部
５１ 制御部
５２ ＳＬＡＭ制御部（第１推定部）
５３ ＰＤＲ制御部（第２推定部）
５４ 動作判定制御部（動作判定部）
５５ 補正誤差制御部（誤差補正部）

Claims (5)

1. 撮影した画像に基づき位置と移動方向を推定する第１推定部と、
   動きに応じて電気信号を出力する動きセンサの出力に基づき位置と移動方向とを推定する第２推定部と、
   前記第１推定部が推定した位置または前記第２推定部が推定した位置のいずれかを選択する制御部と、
   を備え、
   前記制御部が、前記第１推定部が位置を推定できなくなった場合に、前記第１推定部が位置を推定できなくなる前に推定した位置と移動方向とを初期値として前記第２推定部に位置と移動方向を推定させ、
   前記動きセンサは加速度センサを含み、
   前記加速度センサの出力が歩行状態を示しているか否かを判定するとともに、前記画像が歩行状態を示しているか否かを判定し、両判定が歩行状態を示していなかった場合、前記両判定が歩行状態を示さなくなる前の位置を、前記第２推定部が推定した位置とする動作判定部をさらに備える、
   計測装置。
2. 前記第１推定部が推定した位置および移動方向と前記第２推定部が推定した位置および移動方向との誤差を、時間に応じて推定する関数を用いて、前記第２推定部が推定した位置と移動方向を補正する誤差補正部
   をさらに備える請求項１に記載の計測装置。
3. 撮影した画像に基づき位置と移動方向を推定する第１の推定ステップと、
   動きに応じて電気信号を出力する動きセンサの出力に基づき位置と移動方向を推定する第２の推定ステップと、
   前記第１の推定ステップで推定した位置または前記第２の推定ステップで推定した位置のいずれかを選択する選択ステップと、を有し、
   前記第１の推定ステップで位置を推定できなくなった場合に、前記第１の推定ステップで位置を推定できなくなる前に推定した位置と移動方向とを初期値として前記第２の推定ステップにおいて位置と移動方向とを推定させ、
   前記動きセンサは加速度センサを含み、
   前記加速度センサの出力が歩行状態を示しているか否かを判定するとともに、前記画像が歩行状態を示しているか否かを判定し、両判定が歩行状態を示していなかった場合、前記両判定が歩行状態を示さなくなる前の位置を、前記第２の推定ステップで推定した位置とする動作判定ステップをさらに有する、
   計測方法。
4. 計測装置のコンピュータに、
   撮影した画像に基づき位置と移動方向を推定する第１の推定ステップと、
   動きに応じて電気信号を出力する動きセンサの出力に基づき位置と移動方向を推定する第２の推定ステップと、
   前記第１の推定ステップで推定した位置または前記第２の推定ステップで推定した位置のいずれかを選択する選択ステップと、を実行させ、
   前記第１の推定ステップで位置を推定できなくなった場合に、前記第１の推定ステップで位置を推定できなくなる前に推定した位置と移動方向とを初期値として前記第２の推定ステップにおいて位置と移動方向とを推定させ、
   前記動きセンサは加速度センサを含み、
   前記加速度センサの出力が歩行状態を示しているか否かを判定するとともに、前記画像が歩行状態を示しているか否かを判定し、両判定が歩行状態を示していなかった場合、前記両判定が歩行状態を示さなくなる前の位置を、前記第２の推定ステップで推定した位置とする動作判定ステップをさらに実行させる、
   プログラム。
5. 撮影した画像に基づき位置と移動方向を推定する第１推定部と、
   動きに応じて電気信号を出力する動きセンサの出力に基づき位置と移動方向とを推定する第２推定部と、
   前記第１推定部が推定した位置または前記第２推定部が推定した位置のいずれかを選択する制御部と、
   を備え、
   前記制御部が、前記第１推定部が位置を推定できなくなった場合に、前記第１推定部が位置を推定できなくなる前に推定した位置と移動方向とを初期値として前記第２推定部に位置と移動方向を推定させ、
   前記第１推定部が推定した位置および移動方向と前記第２推定部が推定した位置および移動方向との誤差を、時間に応じて推定する関数を用いて、前記第２推定部が推定した位置と移動方向を補正する誤差補正部
   をさらに備える計測装置。

Images