特開 2024-174077 位置測定装置

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024174077

(43)【公開日】2024-12-13

(54)【発明の名称】位置測定装置

(51)【国際特許分類】

   G05D 1/246 20240101AFI20241206BHJP

   G05D 1/243 20240101ALI20241206BHJP

【ＦＩ】

G05D1/246

G05D1/243

【審査請求】有

【請求項の数】12

【出願形態】ＯＬ

(21)【出願番号】P 2024167769

(22)【出願日】2024-09-26

(62)【分割の表示】P 2021575599の分割

【原出願日】2020-09-11

(31)【優先権主張番号】P 2020019928

(32)【優先日】2020-02-07

(33)【優先権主張国・地域又は機関】JP

(71)【出願人】

【識別番号】314012076

【氏名又は名称】パナソニックＩＰマネジメント株式会社

(74)【代理人】

【識別番号】100132241

【弁理士】

【氏名又は名称】岡部 博史

(74)【代理人】

【識別番号】100135703

【弁理士】

【氏名又は名称】岡部 英隆

(74)【代理人】

【識別番号】100199314

【弁理士】

【氏名又は名称】竹内 寛

(74)【代理人】

【識別番号】100221556

【弁理士】

【氏名又は名称】金田 隆章

(72)【発明者】

【氏名】岡田 典

(57)【要約】

【課題】検出部の検出信号に含まれるバイアス誤差を従来技術より簡易に除去することができる位置測定装置を得る。
【解決手段】位置測定装置は、撮像部、検出部、バイアス補正部、位置算出部、及び静止判定部を備える。撮像部は、移動体に搭載され、移動体周囲の環境の撮像画像を取得する。検出部は、移動体に搭載され、移動体の動きを検出して検出結果を示す検出信号を出力する。バイアス補正部は、移動体の動きに依存せずに検出信号に含まれるバイアス誤差を補正する補正値を用いて、検出信号を処理する。位置算出部は、撮像部によって取得された撮像画像と、バイアス補正部によって処理された検出信号とに基づいて、移動体の位置を算出する。バイアス補正部は、静止判定部が移動体が静止していると判定したときに検出部によって出力された検出信号に基づいて、バイアス誤差の補正値を更新する。
【選択図】図４

【特許請求の範囲】

【請求項1】

移動体の位置を測定する位置測定装置であって、
前記移動体に搭載され、前記移動体の周囲の環境を撮像して撮像画像を取得する撮像部と、
前記移動体に搭載され、前記移動体の動きを検出して検出結果を示す検出信号を出力する検出部と、
前記移動体の動きに依存せずに前記検出信号に含まれるバイアス誤差を補正する補正値を用いて、前記検出信号を処理するバイアス補正部と、
前記撮像部によって取得された撮像画像と、前記バイアス補正部によって処理された検出信号とに基づいて、前記移動体の位置を算出する位置算出部と、
前記移動体が静止しているか否かを判定する静止判定部とを備え、
前記バイアス補正部は、前記静止判定部が前記移動体が静止していると判定したときに前記検出部によって出力された検出信号に基づいて、前記バイアス誤差の補正値を更新する、位置測定装置。

【請求項2】

前記撮像部によって取得された撮像画像から特徴点を抽出する特徴点抽出部を更に備え、
前記位置算出部は、前記撮像部が順次、取得する複数の撮像画像の間で、前記特徴点抽出部によって抽出された特徴点を追跡することによって、前記移動体の位置を算出する、請求項１に記載の位置測定装置。

【請求項3】

前記撮像部によって取得された撮像画像から特徴点を抽出する特徴点抽出部を更に備え、
前記バイアス補正部は、前記特徴点抽出部によって抽出された特徴点の数が所定の数未満である場合には、前記バイアス誤差を更新しない、請求項１又は２に記載の位置測定装置。

【請求項4】

前記静止判定部は、
前記位置算出部の算出結果に基づいて前記移動体の速さを算出し、
前記速さが、予め設定された閾値以下である状態が所定期間、継続した場合、前記移動体が静止していると判定する、請求項１～３のいずれか１項に記載の位置測定装置。

【請求項5】

前記静止判定部は、前記バイアス誤差が最後に更新されてから経過した時間が長いほど前記所定期間を短く及び／又は前記閾値を大きく設定して、前記移動体が静止しているか否かを判定する、請求項４に記載の位置測定装置。

【請求項6】

前記移動体の周囲の環境の温度を検知する温度センサを更に備え、
前記静止判定部は、前記温度センサによって検知された温度の変化が大きいほど前記所定期間を短く及び／又は前記閾値を大きく設定して、前記移動体が静止しているか否かを判定する、請求項４又は５に記載の位置測定装置。

【請求項7】

前記静止判定部は、予め設定された周期ごとに前記移動体が静止しているか否かを判定し、
前記周期は、前記バイアス誤差が最後に更新されてから経過した時間が長いほど短く設定される、請求項１～６のいずれか１項に記載の位置測定装置。

【請求項8】

前記移動体の周囲の環境の温度を検知する温度センサを更に備え、
前記静止判定部は、前記温度センサによって検知された温度の変化が大きいほど前記周期を短く設定して、前記移動体が静止しているか否かを判定する、請求項７に記載の位置測定装置。

【請求項9】

前記バイアス補正部は、前記検出部によって出力された前記検出信号の値の分散に基づいて、前記バイアス誤差を更新するか否かを決定する、請求項１～８のいずれか１項に記載の位置測定装置。

【請求項10】

前記検出部は、前記移動体の動きとして角速度を検出するジャイロセンサを含む、請求項１～９のいずれか１項に記載の位置測定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、移動体の位置を測定する位置測定装置に関する。

【背景技術】

【0002】

特許文献１は、ジャイロの零点オフセットを算出し、これをキャリブレーションできるキャリブレーション方法を開示している。特許文献１のキャリブレーション方法は、加速度センサ及び角速度センサの出力を用いるデッドレコニングを利用する。特許文献１に記載のキャリブレーション方法を実行するには、第１の地点及び第２の地点に、アンカーＩＤを発信する発信機であるアンカーを設置する必要がある。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１２－２１５５４７号公報

【発明の概要】

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

【0004】

本開示は、搭載された検出部の検出信号に含まれるバイアス誤差を従来技術より簡易に除去できる位置測定装置を提供する。

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

【0005】

本開示における位置測定装置は、移動体の位置を測定する位置測定装置である。位置測定装置は、撮像部と、検出部と、バイアス補正部と、位置算出部と、静止判定部とを備える。撮像部は、移動体に搭載され、移動体の周囲の環境を撮像して撮像画像を取得する。検出部は、移動体に搭載され、移動体の動きを検出して検出結果を示す検出信号を出力する。バイアス補正部は、移動体の動きに依存せずに検出信号に含まれるバイアス誤差を補正する補正値を用いて、検出信号を処理する。位置算出部は、撮像部によって取得された撮像画像と、バイアス補正部によって処理された検出信号とに基づいて、移動体の位置を算出する。静止判定部は、移動体が静止しているか否かを判定する。バイアス補正部は、静止判定部が移動体が静止していると判定したときに検出部によって出力された検出信号に基づいて、バイアス誤差の補正値を更新する。

【発明の効果】

【0006】

本開示の位置測定装置によると、搭載された検出部の検出信号に含まれるバイアス誤差を従来技術より簡易に除去することができる。

【図面の簡単な説明】

【0007】

【図1】第１実施形態に係る位置測定装置を搭載した移動体の構成を例示する模式図

【図2】第１実施形態に係る位置測定装置の構成を示すブロック図

【図3】第１実施形態に係る位置測定装置による測定結果の履歴を示す位置履歴データを例示する模式図

【図4】第１実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャート

【図5】図４に示した静止判定処理の一例を示すフローチャート

【図6】第２実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャート

【図7】第２実施形態に係る位置測定装置の動作を説明するためのグラフ

【図8】第２実施形態に係る位置測定装置によって実行されるバイアス更新処理の変形例を示すフローチャート

【図9】第３実施形態に係る位置測定装置の構成を示すブロック図

【図10】第３実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャート

【図11】第４実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャート

【図12】第５実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャート

【図13】第５実施形態に係る位置測定装置のＩＭＵの出力信号の一例である角速度信号の時間変化を示すグラフ

【図14】第６実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャート

【図15】他の実施の形態に係る位置測定装置によって実行される静止判定処理を例示するフローチャート

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

【0008】

（本開示に至った経緯）
例えば、フォークリフト等の有人の荷役自動車、無人搬送車（Automated guided vehicle、ＡＧＶ）及び自律移動型の荷物運搬ロボット等の移動体に搭載され、カメラによって撮像された撮像画像を用いて移動体の位置を測定する位置測定装置が知られている。このような位置測定装置を構成する技術として、例えば逐次撮像される画像に基づいて自己位置の測定と地図情報の生成とを実行するＶｉｓｕａｌ－ＳＬＡＭ（Simultaneous Localization and Mapping）技術が知られている。

【0009】

本願発明者は、慣性計測装置（Inertial measurement unit、以下、「ＩＭＵ」という。）を更に用いて、Ｖｉｓｕａｌ－ＳＬＡＭ技術による移動体の位置の測定精度を向上する技術について検討した。ここで、ＩＭＵによって出力される検出値は、移動体の動きに依存せずに出力されるバイアス誤差を含む。例えば、ＩＭＵの一例である、角速度を検出するジャイロセンサは、入力値がない零点、即ち回転していない場合であっても、零でない検出値を出力する。この零でない検出値がバイアス誤差の一例である。バイアス誤差は、例えば、零点バイアス誤差、ヌルバイアス誤差、零点オフセット、及びヌルオフセットとも呼ばれる。

【0010】

特許文献１は、ジャイロの零点オフセットをキャリブレーションするキャリブレーション方法を開示する。このキャリブレーション方法は、移動体が第１の地点から位置または角度の誤差が特定量となる第２の地点へ移動した場合に、前記加速度センサ及び前記角速度センサの出力を用いたデッドレコニングにより前記第２の地点で得られる位置または角度と前記特定量との差分を特定する処理と、特定結果として得られた差分から、当該移動体が静止状態である場合に前記角速度センサによって出力されるセンサ値である零点オフセットを算出する処理とを含む。しかしながら、第１の地点と第２の地点には、アンカーＩＤを発信する発信機であるアンカー設置する必要があり、移動体端末はこのアンカーと無線通信を行う必要がある。そのため、アンカーを設置しない場合にはキャリブレーションを実行できない。アンカーを設置した場合には、その設置のための費用がかかり、移動体にアンカーとの無線通信のためのデバイスを備える必要があるため、構成が複雑になる。

【0011】

こうした従来技術の問題点も含めて、本願発明者は、移動体の位置測定におけるバイアス誤差の影響について鋭意研究し、本開示に係る位置測定装置を考案するに至った。本開示に係る位置測定装置は、自装置によって測定された移動体の位置を利用して静止判定を行い、静止時のバイアス誤差を取得してバイアス更新処理を実行する。これにより、ＩＭＵの検出信号に含まれるバイアス誤差を精度良く除去することができ、ＩＭＵの検出結果を用いて、精度良く移動体の位置を測定することができる。さらに、本開示によると、従来技術と異なり、バイアス更新処理を実行するためにアンカー等の外部機器を移動体の移動経路に設置する必要がない位置測定装置が得られる。

【0012】

以下、適宜図面を参照しながら、実施の形態を詳細に説明する。但し、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明や実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になるのを避け、当業者の理解を容易にするためである。

【0013】

なお、出願人は、当業者が本開示を十分に理解するために添付図面および以下の説明を提供するのであって、これらによって特許請求の範囲に記載の主題を限定することを意図するものではない。

【0014】

（第１実施形態）
１．構成
図１は、移動体１の構成を例示する模式図である。本開示の第１実施形態に係る位置測定装置１００は、例えば、フォークリフト等の有人の荷役自動車、ＡＧＶ及び自律移動型の荷物運搬ロボット等の移動体１に搭載され、移動体１の位置を測定する。

【0015】

移動体１は、例えば、荷物を搭載する荷台１ａを備える。移動体１には、本実施形態に係る位置測定装置１００が搭載されている。位置測定装置１００は、移動体１の周囲を撮像するカメラ２と、ＩＭＵ３とを備える。

【0016】

位置測定装置１００には、例えば、逐次撮像される画像に基づいて自己位置の測定と地図情報の生成とを実行するＶｉｓｕａｌ－ＳＬＡＭ技術を適用することができる。位置測定装置１００は、カメラ２によって撮像された撮像画像だけでなく、ＩＭＵ３の検出結果である角速度信号をも利用することによって、移動体１の位置を精度良く計測する。

【0017】

図２は、位置測定装置１００の構成を示すブロック図である。位置測定装置１００は、カメラ２、ＩＭＵ３、制御部４、記憶部５、通信インタフェース（Ｉ／Ｆ）７及び駆動部８を備える。

【0018】

カメラ２は、本開示の撮像部の一例である。カメラ２は、移動体１に搭載され、移動体１の周囲を撮像し、色画像データ及び距離画像データを生成する。カメラ２は、ＲＧＢ－Ｄカメラ、ステレオカメラ等のデプスセンサを含んでもよい。また、カメラ２は、色画像を撮像するＲＧＢカメラと、距離画像を撮像するＴｏＦ（Time of Flight）センサとによって構成されてもよい。

【0019】

ＩＭＵ３は、本開示の検出部の一例である。ＩＭＵ３は、移動体１に搭載され、例えば、移動体１の動きを示す動き情報として、移動体１の角速度を検出し、検出結果を示す検出信号を出力する。

【0020】

制御部４は、ソフトウェアと協働して所定の機能を実現するＣＰＵ又はＭＰＵのような汎用プロセッサを含む。制御部４は、記憶部５に格納されたプログラムを読み込んで実行することによって位置測定部４１、ＩＭＵ処理部４２等の各種の機能を実現し、位置測定装置１００の全体動作を制御する。位置測定部４１は、特徴点抽出部４１１と、位置算出部４１４と、地図管理部４１５と、静止判定部４１６とを含む。ＩＭＵ処理部４２は、バイアス補正部４２１と、姿勢算出部４２４とを含む。バイアス補正部４２１は、バイアス除去部４２２とバイアス更新部４２３とを含む。

【0021】

例えば、制御部４は、本実施形態の位置測定方法を実現するプログラム又はＳＬＡＭアルゴリズムを実現するプログラムを実行する。制御部４は、ハードウェアとソフトウェアの協働により所定の機能を実現するものに限定されず、所定の機能を実現するための専用回路として設計されたＦＰＧＡ、ＡＳＩＣ、ＤＳＰ等のハードウェア回路で構成されてもよい。

【0022】

記憶部５は、位置測定装置１００の機能を実現するために必要なプログラム及びデータを含む種々の情報を記録する記録媒体である。記憶部５には、例えば地図情報５１や画像データが格納される。記憶部５は、例えば、フラッシュメモリ、ＳＳＤなどの半導体メモリ装置、ハードディスク等の磁気記憶装置、その他の記憶デバイス単独で又はそれらを適宜組み合わせて実現される。記憶部５は、種々の情報を一時的に記憶する高速動作可能なＳＲＡＭ、ＤＲＡＭなどの揮発性メモリを含んでもよい。揮発性メモリは、例えば制御部４の作業領域や画像データをフレーム毎に一時的に記憶するフレームメモリとして動作する。

【0023】

通信Ｉ／Ｆ７は、ネットワークを介して、位置測定装置１００と外部サーバ等の外部機器との通信接続を可能とするためのインタフェース回路である。通信Ｉ／Ｆ７は、ＩＥＥＥ８０２．３、ＩＥＥＥ８０２．１１等の規格に従って通信を行う。

【0024】

駆動部８は、制御部４からの指示に従って移動体１を移動させる機構である。例えば、駆動部８は、移動体１のタイヤに接続されたエンジンの駆動回路、ステアリング回路及びブレーキ回路を含む。

【0025】

２．動作
以上のように構成される位置測定装置１００の動作について、以下説明する。

【0026】

２－１．位置測定処理
図２を参照して、位置測定装置１００の基本的な動作として位置測定処理の一例を説明する。位置測定処理は、位置測定部４１として動作する制御部４によって実行される。

【0027】

まず、制御部４は、カメラ２から、一定のフレームレートで撮像された複数の撮像画像のデータを取得する。ここで、撮像画像は、カメラ２によって撮像された移動体１の周囲の環境の画像データである。

【0028】

次に、特徴点抽出部４１１として動作する制御部４は、撮像画像を解析して特徴点を抽出する。制御部４は、輝度値又は色が周囲の画素又は画素群と区別できる画素又は画素群を特徴点として抽出する。撮像画像から特徴点を検出するために、例えば公知のＦＡＳＴ（Features from Accelerated Segment Test）技術が使用されてもよい。

【0029】

なお、制御部４は、移動体１の位置の算出処理の他、地図情報５１の作成処理も行う。地図情報５１は、特徴点の２次元位置、３次元位置、又はその両方の情報を含む。地図管理部４１５として動作する制御部４は、特徴点の撮像画像上の座標を世界座標に変換し、撮像画像上の特徴点に対応するマップポイントを世界座標空間に登録することによって、地図情報５１を作成する。地図情報５１には、撮像画像上の特徴点に対応するマップポイントとともに、撮像画像を示すカメラフレームと、当該撮像画像を撮像した際のカメラ２の位置及び向き（以下、「カメラポーズ」という。）と、が記録される。作成された地図情報５１は、記憶部５に格納される。制御部４は、例えば移動体１の移動中に、所定の時間間隔毎に撮像画像を取得して特徴点を登録することにより地図情報５１を生成することができる。

【0030】

位置算出部４１４として動作する制御部４は、特徴点抽出部４１１によって抽出された撮像画像上の特徴点の情報と、記憶部５に格納された地図情報５１とを用いて、カメラ２の位置ひいては移動体１の位置を算出する。例えば、位置算出部４１４として動作する制御部４は、撮像画像中の特徴点と、地図情報５１中のマップポイントとを対応させる特徴点マッチング処理を行い、撮像画像を撮像したカメラ２のカメラポーズを算出する。あるいは、制御部４は、特徴点マッチング処理として、例えば、公知のＫＬＴ（Kanade-Lucas-Tomasi）トラッカ技術を用いて、前フレーム内の特徴点と、前フレームの次に取得された現フレーム内の特徴点と、を対応させてもよい。

【0031】

特徴点マッチング処理は、例えば、特徴点が有する特徴量に基づいて、現フレーム内の特徴点と、地図情報５１中のマップポイント又は前フレーム内の特徴点とが対応するか否かを判断する処理である。特徴点マッチング処理により、位置算出部４１４として動作する制御部４は、カメラ２が順次、取得する複数の撮像画像の間で、対応する特徴点を追跡することができる。特徴点の特徴量は、例えば、ＳＵＲＦ（Speeded-Up Robust Features）技術によって得られるＳＵＲＦ特徴量、ＳＩＦＴ（Scale-Invariant Feature Transform）技術によって得られるＳＩＦＴ特徴量、又は、ＯＲＢ（Oriented FAST and Rotated BRIEF）技術によって得られるＯＲＢ特徴量である。特徴点の特徴量は、例えば、１以上の次元を有するベクトルで表される。例えば、ＳＵＲＦ特徴量は６４次元のベクトルで表され、ＳＩＦＴ特徴量は１２８次元のベクトルで表される。特徴量の類似度は、例えば、特徴量間のユークリッド距離等の距離として算出される。

【0032】

次に、位置算出部４１４として動作する制御部４は、現フレームに対応するカメラポーズを算出する。現フレームに対応するカメラポーズは、例えば、前フレーム内の特徴点と現フレーム内の特徴点との幾何学的位置関係に基づいて算出される。現フレームに対応するカメラポーズの算出精度及び／又は算出効率を向上させるために、例えば、ＩＭＵ処理部４２の姿勢算出部４２４によって算出された移動体１の姿勢を利用してもよい。姿勢算出部４２４は、位置算出部４１４から前フレームに対応するカメラポーズを取得し、これとＩＭＵ３によって検出された角速度とに基づいて、現フレームに対応する移動体１の推測姿勢を算出することができる。位置算出部４１４は、姿勢算出部４２４によって算出された現フレームに対応する移動体１の推測姿勢を取得し、これを現フレームに対応するカメラポーズに変換する。

【0033】

このように、ＩＭＵ３によって検出された角速度を移動体１の姿勢の推測に利用することにより、位置測定装置１００は、移動体１が加速や回転をした場合でも、効率的に特徴点マッチングを行うことができる。

【0034】

本実施形態の位置測定処理では、ＩＭＵ処理部４２において姿勢算出部４２４が、ＩＭＵ３からの検出信号がバイアス補正部４２１によって処理されたバイアス補正済みの検出信号に基づき動作する。バイアス補正部４２１において、バイアス除去部４２２は、ＩＭＵ３によって出力される検出信号からバイアス誤差の補正値を減算することによって、バイアス誤差を除去する処理を行う。バイアス誤差の補正値は、検出信号に含まれる誤差を相殺するための信号値として予め設定される。バイアス誤差の補正値は、零点オフセット、及びヌルオフセットとも呼ばれる。バイアス更新部４２３は、後述するバイアス更新処理においてバイアス誤差の補正値をバイアス除去部４２２に設定することにより、バイアス更新を行う。

【0035】

以上の位置測定処理によって得られる移動体１の位置データは、例えば外部サーバ又は位置測定装置１００の内部メモリ等に蓄積され、移動体１の位置の履歴を示す位置履歴データを構成する。こうした位置履歴データは、例えば移動体１が環境内で移動した軌跡に関する各種のデータ管理及びデータ解析に採用可能である（図３参照）。本実施形態の位置測定装置１００により、移動体１の軌跡を精度良く測定するように位置履歴データが得られると、例えば外部サーバにおける上記のデータ管理及び解析を高精度にすることができる。

【0036】

２－２．バイアス更新処理
２－２－１．動作の概要
本実施形態の位置測定装置１００は、以上のような位置測定処理の実施中にバイアス更新処理を繰り返して、バイアス誤差の補正値をリアルタイムに更新する。以下、バイアス更新処理の概要について、図３を用いて説明する、

【0037】

位置測定装置を搭載した移動体が、２つの進入禁止区域の間に延びる通路のみを移動した時の位置測定装置による移動履歴データについて、図３（ａ）及び図３（ｂ）で説明する。図３（ａ）は、長時間バイアス更新を行わずに動作させた位置測定装置による位置履歴データを例示する模式図である。位置履歴データは、例えば外部サーバ等に格納される。図３は、２つの進入禁止区域１０と、進入禁止区域１０の間に延びる通路２０とを上方から見た２次元地図データを示している。図３（ａ）に示すように、長時間バイアス更新を行わずに位置測定装置を動作させた場合、位置履歴データは、あたかも移動体が、破線で囲んだ領域Ｒの中に誤って進み、進入禁止区域１０内に進入したような誤測定の軌跡を含んでいる。こうした誤測定が位置履歴データに混入すると、例えば上述した外部サーバにおけるデータ管理及び解析の精度を悪化させてしまう。

【0038】

本願発明者の鋭意研究によると、上記のような誤測定は、例えば移動体１の軌跡にわたる位置測定中に、ＩＭＵ３のバイアス誤差が変動することに起因すると判明した。ＩＭＵ３の起動直後におけるバイアス誤差は、例えば起動から数秒の間に取得された静止状態におけるセンサ値の平均値として取得できる。バイアス補正は、その後の測定値から起動直後におけるバイアス誤差を減算することによって行うことができる。

【0039】

しかしながら、バイアス誤差は、温度等の外部要因によって変化する。したがって、ＩＭＵ３の起動直後にバイアス補正を行っただけでは、ＩＭＵ３を動作させ続けた場合にバイアス誤差が変動し、ＩＭＵ３の検出値に含まれる誤差が大きくなる。ＩＭＵ３の検出値に含まれる誤差が大きくなると、位置測定装置１００においてカメラポーズの算出にＩＭＵ３の検出結果を用いた場合、カメラポーズを誤算出する結果となる。

【0040】

こうした問題に対して、本願発明者は鋭意研究を重ね、起動直後においてバイアス誤差を取得するだけではなく、その後の動作継続中にも適切なタイミングにおいて検出信号から除去すべきバイアス誤差を更新するバイアス更新処理を想到するに至った。

【0041】

図３（ｂ）は、本実施形態のバイアス更新処理を適用した場合における、位置測定装置１００による移動体１の測定結果の履歴を示す模式図である。図３（ａ）と比較すると、図３（ｂ）の位置履歴データは、バイアス更新処理の結果、移動体１が破線で囲んだ領域Ｒの中に誤って進むことなく、矢印で示した通路２０に沿った正しい方向に移動した軌跡を示している。以下、本実施形態に係るバイアス更新処理の詳細を説明する。

【0042】

２－２－２．動作の詳細
図４は、本実施形態に係る位置測定装置１００によって実行されるバイアス更新処理の一例を示すフローチャートである。バイアス更新処理は、制御部４によって繰り返し実行される。

【0043】

まず、制御部４は、最後にバイアス更新処理が完了してから所定周期ａが経過したか否かを判定する（Ｓ１）。所定周期ａが経過していない場合（Ｓ１でＮｏ）、制御部４は、ステップＳ２以降に進まずに図４のバイアス更新処理を終了する。所定周期ａが経過すると（Ｓ１でＹｅｓ）、静止判定部４１６として動作する制御部４は、移動体１が静止しているか否かを判定する静止判定処理を行う（Ｓ２）。例えば、所定周期ａは１（ｓ）である。

【0044】

本実施形態の静止判定処理Ｓ２は、Ｖｉｓｕａｌ－ＳＬＡＭ技術に基づく位置測定部４１の測定結果を利用して、移動体１が充分に小さい速さを有する期間、即ち静止期間を検知することによって行われる。制御部４は、移動体１が静止していると判定した場合には、判定結果を示す静止フラグをＯＮにし、移動体１が静止していると判定しなかった場合には静止フラグをＯＦＦにする。静止判定処理Ｓ２の詳細については後述する。

【0045】

次に、バイアス更新部４２３として動作する制御部４は、静止フラグがＯＮであるか否かを判定する（Ｓ３）。静止フラグがＯＦＦである、即ち静止フラグがＯＮでない場合（Ｓ３でＮｏ）、制御部４は、特にバイアス誤差を補正せずに図４のバイアス更新処理を終了する。

【0046】

一方、静止フラグがＯＮである場合（Ｓ３でＹｅｓ）、バイアス更新部４２３として動作する制御部４は、例えば静止期間中にＩＭＵ３によって出力された検出信号を取得し（Ｓ４）、その信号値に基づきバイアス誤差の補正値を設定することによってバイアス誤差を更新する（Ｓ５）。バイアス誤差の補正値は、例えば静止時の数秒の間に取得されたＩＭＵ３の検出信号値の平均値として算出される。あるいは、ステップＳ４において、制御部４は、静止フラグがＯＮとなった静止判定時から所定の時間Ｓだけ遡った時点との間にＩＭＵ３から出力された検出信号を取得し、その検出信号値の平均値をバイアス誤差の補正値とする。

【0047】

以上のようなバイアス更新処理によりバイアス誤差が更新される。バイアス除去部４２２は、バイアス更新部４２３から更新されたバイアス誤差を取得し、例えば、その後の測定値から当該更新されたバイアス誤差を減算することによってバイアス補正を行う。

【0048】

このように、位置測定装置１００は、位置測定部４１による移動体１の位置の測定結果を利用して静止判定処理Ｓ２を行い、リアルタイムにバイアス誤差を更新する（Ｓ５）ようにバイアス更新処理を実行することができる。したがって、起動直後のバイアス誤差を補正するだけではなく、その後の動作継続中においてもバイアス更新を行ってバイアス補正を行うことができる。そのため、位置測定装置１００によると、ＩＭＵ３の検出信号に含まれるバイアス誤差を精度良く除去することができる。位置測定装置１００は、ＩＭＵ３の検出結果を用いて、精度良く移動体１の位置を測定することができる。

【0049】

上記の静止判定処理Ｓ２によると、移動体１が静止していると判定されたとき（Ｓ３でＹｅｓ）に得られるＩＭＵ３の検出信号の出力値（Ｓ４）は、移動体１の角速度に起因せずバイアス誤差に対応するものと考えられる。よって、こうしたタイミングが検知されることにより、バイアス誤差の更新（Ｓ５）を精度良く行うことができる。以下、本実施形態における静止判定処理Ｓ２の詳細について説明する。

【0050】

２－２－３．静止判定処理
図５は、図４に示した静止判定処理Ｓ２の一例を示すフローチャートである。まず、静止判定部４１６として動作する制御部４は、静止期間ｐを計測する（Ｓ２１）。静止期間ｐの計測開始時には、静止判定部４１６は、静止期間ｐをリセットした後にカウントアップを始める。図５に示した例では、静止期間ｐは、移動体１の速さｖが予め設定された所定の閾値ｖｔｈ以下である期間である。例えば、閾値ｖｔｈは０．０７５（ｍ／ｓ）である。

【0051】

例えばステップＳ２１の次に、静止判定部４１６は、位置算出部４１４から出力された移動体１の位置データに基づいて、移動体１の速さｖを算出する（Ｓ２２）。例えば、静止判定部４１６は、一定の時間間隔で位置算出部４１４から出力された移動体１の位置データを取得し、移動体１の位置の単位時間当たりの変化量の大きさとして速さｖを算出する。

【0052】

次に、静止判定部４１６は、算出された移動体１の速さｖが閾値ｖｔｈより大きいか否かを判定する（Ｓ２３）。閾値ｖｔｈは、例えば移動体１が動いていると見なせる基準の速さを示すように、予め設定される。算出された移動体１の速さｖが閾値ｖｔｈより大きい場合（Ｓ２３でＹｅｓ）、静止判定部４１６は、静止フラグをＯＦＦにし（Ｓ２６）、静止判定処理Ｓ２を終える。この場合、移動体１は静止せずに動いていると考えられ、図４のステップＳ３でＮＯに進むことによりバイアス誤差の更新（Ｓ５）は行われない。この場合、静止期間ｐは、例えば次のループにおけるステップＳ２１においてリセットされる。

【0053】

算出された移動体１の速さｖが閾値ｖｔｈ以下である場合（Ｓ２３でＮｏ）、静止判定部４１６は、静止期間ｐが、例えば所定の閾値ｐｔｈより長いか否かを判定する（Ｓ２４）。閾値ｐｔｈは、例えば移動体１が安定して静止すると考えられる基準の期間を示すように、予め設定される。なお、静止期間ｐの計測（Ｓ２１）は、ステップＳ２３でＮｏに進んだときに行われてもよい。

【0054】

静止判定部４１６は、計測中の静止期間ｐが閾値ｐｔｈ以下であると判定した場合（Ｓ２４でＮｏ）、例えば静止フラグをＯＦＦにし（Ｓ２６）、静止判定処理Ｓ２を終える。この場合、ステップＳ２３でＮｏに進んだ場合と異なり、静止期間ｐはリセットされない。一方、静止期間ｐが閾値ｐｔｈより長いと判定した場合（Ｓ２４でＹｅｓ）、静止判定部４１６は、静止フラグをＯＮにし（Ｓ２５）、静止判定処理Ｓ２を終える。この場合、移動体１は安定的に静止していると考えられる。このことから、図４のステップＳ３でＹＥＳに進み、例えば静止期間ｐ中のＩＭＵ３の検出信号を用いてバイアス誤差が更新される（Ｓ４，Ｓ５）。例えば、閾値ｐｔｈは５（ｓ）である。

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

（第４実施形態）
以下、図１１を用いて第４実施形態を説明する。第２実施形態では、バイアスの未更新期間が長くなり過ぎると静止判定処理Ｓ２の判定基準を緩和した。本実施形態では、静止判定処理Ｓ２の判定基準の代わりに、バイアス更新処理の周期ａを短くする位置測定装置について説明する。

【0086】

図１１は、本開示の第４実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャートである。図１１のバイアス更新処理は、図４に示した第１実施形態に係る位置測定装置１００によって実行されるバイアス更新処理に加えて、ステップＳ４０１～Ｓ４０３を更に含む。

【0087】

図４と異なり、図１１では、ステップＳ３において静止フラグがＯＦＦであると判定した場合（Ｓ３でＮｏ）、制御部４は、例えば第２実施形態のステップＳ２０１と同様にバイアスの未更新期間が長いか否かを判定する（Ｓ４０１）。

【0088】

バイアスの未更新期間が長くないと判定した場合（Ｓ４０１でＮｏ）、制御部４は、周期ａを通常値に設定する（Ｓ４０２）。一方、バイアスの未更新期間が長いと判定した場合（Ｓ４０１でＹｅｓ）、制御部４は、周期ａを通常値より短い値に設定し（Ｓ４０３）、図１１の処理を終える。図１１のバイアス更新処理は、制御部４によって繰り返し実行される。したがって、ステップＳ４０２で周期ａを短い値に設定した次のループにおけるステップＳ１では、制御部４は、最後にバイアス更新処理が完了してから、再設定されて短くなった周期ａが経過したか否かを判定する。

【0089】

これにより、例えば、初期設定では周期ａが５分であり、５分に１回静止判定ステップＳ２が実行されていたものの、長期間バイアスが更新されない場合（Ｓ４０１でＹｅｓ）には、周期ａを１分に再設定して短くし（Ｓ４０２）、その後は１分に１回のペースで静止判定ステップＳ２が実行されるようになる。

【0090】

以上のように、静止判定部４１６は、予め設定された周期ａごとに移動体１が静止しているか否かを判定する。周期ａは、バイアス誤差が最後に更新されてから経過した時間が長いほど短く設定される（Ｓ４０２）。

【0091】

静止判定が行われたとすると静止していると判定される状態であったとしても、最後に静止判定が行われてから周期ａが経過していない限り、静止判定が行われず、静止フラグはＯＮにならない。本実施形態では、最後にバイアス誤差が更新されてから経過した時間が長い場合に、周期ａを短くして静止判定を行う頻度を増加させる。そのため、静止していると判定される機会が増加し、バイアス誤差が変動しているにもかかわらずバイアス更新が実行されない事態を早期に解消することができる。

【0092】

（第５実施形態）
以下、図１２～１３を用いて第５実施形態を説明する。本実施形態では、バイアス更新の有無の決定に、ＩＭＵ３の検出信号のばらつきを用いる位置測定装置について説明する。

【0093】

図１２は、本開示の第５実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャートである。図１２のバイアス更新処理は、図４に示した第１実施形態に係る位置測定装置１００によって実行されるバイアス更新処理に加えて、ステップＳ５００を更に含む。

【0094】

ステップＳ５００は、ステップＳ４とステップＳ５との間で、例えばバイアス補正部４２１のバイアス更新部４２３として動作する制御部４によって実行される。ステップＳ５００では、制御部４は、ＩＭＵ３の出力信号の分散Ｖが、例えば所定の閾値Ｖｔｈより大きいか否かを判定する。分散Ｖが大きいと判定した場合（Ｓ５００でＹｅｓ）、バイアス更新部４２３として動作する制御部４は、バイアス更新ステップＳ５を実行せずに処理を終了する。分散Ｖが大きいと判定しなかった場合（Ｓ５００でＮｏ）、バイアス更新部４２３として動作する制御部４は、バイアス更新ステップＳ５を実行する。

【0095】

図１３は、ＩＭＵ３の出力信号の一例である角速度信号の時間変化を示すグラフである。図１２のフローでは、ステップＳ３において静止フラグがＯＮであると判定した場合、バイアス更新部４２３として動作する制御部４は、ＩＭＵ３によって出力された検出信号を取得する（Ｓ４）。次に、制御部４は、静止フラグがＯＮとなった静止判定時から所定の時間Ｓだけ遡った時点との間にＩＭＵ３から出力された検出信号を取得し、その検出信号値の分散Ｖを算出する。ステップＳ５００では、制御部４は、例えば、算出された分散Ｖが所定の閾値Ｖｔｈより大きいか否かを判定する。

【0096】

図１３（ａ）は、静止判定時から所定の時間Ｓだけ遡った時点との間にＩＭＵ３から出力された角速度信号の分散がＶ１であることを示し、図１３（ｂ）は、この分散がＶ２であることを示している。ここで、Ｖ１＜Ｖｔｈであり、Ｖ２＞Ｖｔｈであるとする。図１３（ａ）のように分散Ｖ１が所定の閾値Ｖｔｈより小さい場合、ステップＳ５００でＮｏに進み、バイアス更新部４２３として動作する制御部４は、バイアス更新ステップＳ５を実行する。一方、図１３（ｂ）のように分散Ｖ２が所定の閾値Ｖｔｈより大きい場合、ステップＳ５００でＹｅｓに進み、バイアス更新部４２３として動作する制御部４は、バイアス更新ステップＳ５を実行しない。

【0097】

以上のように、バイアス補正部４２１は、ＩＭＵ３によって出力された検出信号の値の分散Ｖに基づいて、バイアス誤差を更新するか否かを決定する（Ｓ５００）。例えば、バイアス補正部４２１は、ＩＭＵ３によって出力された検出信号の値の分散Ｖが所定の閾値Ｖｔｈより大きい場合、バイアス誤差を更新しないと決定する。

【0098】

これにより、分散Ｖが小さく、信頼性のあるＩＭＵ３の検出信号の値を用いてバイアス誤差を算出してバイアス更新を行うことができるため、より正確にバイアス誤差を補正することができる。

【0099】

（第６実施形態）
以下、図１４を用いて第６実施形態を説明する。本実施形態では、バイアス更新の有無の決定に、位置算出における特徴点を用いる位置測定装置について説明する。

【0100】

図１４は、本開示の第６実施形態に係る位置測定装置によって実行されるバイアス更新処理の一例を示すフローチャートである。図１４のバイアス更新処理は、図４に示した第１実施形態に係る位置測定装置１００によって実行されるバイアス更新処理に加えて、ステップＳ１とステップＳ２との間にステップＳ６０１，Ｓ６０２を更に含む。

【0101】

ステップＳ６０１では、例えば静止判定部４１６は、特徴点抽出部４１１から特徴点データを取得する。特徴点データは、例えば、撮像画像中のすべての特徴点の座標を示すデータである。静止判定部４１６は、特徴点データに基づいて、撮像画像中に存在する特徴点の数を算出する。あるいは、静止判定部４１６は、特徴点データとして、特徴点の数を特徴点抽出部４１１から取得してもよい。

【0102】

ステップＳ６０１の次のステップＳ６０２では、静止判定部４１６は、撮像画像中の特徴点の数が充分に多いか否かを判定する。例えば、静止判定部４１６は、撮像画像中の特徴点の数が所定の閾値以上である場合に、撮像画像中の特徴点の数が充分に多いと判定する。特徴点の数が充分に多いと判定した場合（Ｓ６０２でＹｅｓ）、静止判定処理Ｓ２に進む。特徴点の数が充分に多いと判定しなかった場合（Ｓ６０２でＮｏ）、制御部４は、図１４のバイアス更新処理を終え、バイアス更新ステップＳ５を実行しない。以降のステップは第１実施形態と同様であるため、説明を省略する。

【0103】

特徴点データを取得するステップＳ６０１と特徴点の数が充分に多いか否かを判定するＳ６０２とを、ステップＳ１とステップＳ２との間に実行する例について説明したが、本実施形態はこれに限定されない。例えば、ステップＳ６０１及びステップＳ６０２は、バイアス更新ステップＳ５の直前に実行されてもよい。

【0104】

以上のように、位置測定装置は、カメラ２によって取得された撮像画像から特徴点を抽出する特徴点抽出部４１１を更に備えてもよい。バイアス補正部４２１は、特徴点抽出部によって抽出された特徴点の数が所定の数未満である場合（Ｓ６０２でＮｏ）には、バイアス誤差を更新しない。

【0105】

特徴点の数が充分に多くない場合、位置算出部４１４による移動体１の位置の算出の精度、及び算出された移動体１の位置に基づく、静止判定部４１６による静止判定の精度等が高くなく、これらの処理の信頼度が担保されない。本実施形態では、このように位置算出処理、静止判定処理等の信頼度が担保されない場合にはバイアス更新ステップＳ５を実行せず、バイアス誤差が信頼度の低い値に更新されることを防止することができる。

【0106】

（他の実施形態）
以上のように、本出願において開示する技術の例示として、第１～第６の実施形態を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略などを行った実施の形態にも適用可能である。また、第１～第６の実施形態で説明した各構成要素を組み合わせて、新たな実施の形態とすることも可能である。

【0107】

そこで、以下、他の実施の形態を例示する。図５に示した第１実施形態の静止判定処理（Ｓ２）では、移動体１の位置に基づいて移動体１の速さｖを算出するステップＳ２２を説明し、算出された速さｖが閾値ｖｔｈより大きいか否かに基づいて静止フラグをＯＮにするかＯＦＦにするかを決定する処理について説明した。本開示に係る位置測定装置の静止判定部４１６は、移動体１が静止しているか否かを判定するものであればよく、移動体１の位置に基づいて移動体１の速さｖを算出するものに限定されない。例えば、静止判定部４１６は、前フレーム内の特徴点の位置と、この特徴点に対応する現フレーム内の特徴点の位置と、を比較して前フレームから現フレームへの特徴点の移動量を算出し、各特徴点についての移動量の平均値である平均移動量ｄが所定の閾値ｄｔｈ以下である場合に移動体１が静止していると判定する。即ち、前フレームと現フレームとの画像の差が小さい場合に、カメラ２ひいては移動体１が静止していると判断する。これにより、移動体１の速さｖを算出することなく静止判定を行うことができるため、制御部４の処理負荷を低減することができ、静止判定を高速度で行うことができる。

【0108】

図１５は、このような他の実施の形態に係る位置測定装置によって実行される静止判定処理を例示するフローチャートである。図１５の静止判定処理は、図５示した第１実施形態の静止判定処理と比較して、ステップＳ２２に代えてステップＳ７０１を含み、ステップＳ２３に代えてステップＳ７０２を含む。

【0109】

ステップＳ７０１では、静止判定部４１６は、前フレームから現フレームへの特徴点の平均移動量ｄを算出する。次に、ステップＳ７０２において、静止判定部４１６は、平均移動量ｄが所定の閾値ｄｔｈより大きいか否かを判定する。平均移動量ｄが所定の閾値ｄｔｈより大きい場合（Ｓ７０２でＹｅｓ）、静止判定部４１６は、静止フラグをＯＦＦにし（Ｓ２６）、静止判定処理を終える。平均移動量ｄが所定の閾値ｄｔｈ以下である場合（Ｓ７０２でＮｏ）、ステップＳ２４に進む。以降のステップは第１実施形態と同様であるため、説明を省略する。

【0110】

上記の実施形態では、検出部の一例であるＩＭＵ３について、移動体１の角速度を検出し、検出結果を示す検出信号を出力する例を説明した。ＩＭＵ３は、移動体１の動きを示す動き情報を検出するものであればよく、例えば加速度センサであってもよい。例えば、バイアス更新部４２３として動作する制御部４は、静止フラグがＯＮであるとき（図４のＳ３でＹｅｓ）に加速度センサによって出力された検出信号を取得し（Ｓ４）、その信号値に基づきバイアス誤差の補正値を設定することによってバイアス誤差を更新する（Ｓ５）。例えば、加速度センサに鉛直方向下向き（－ｚ方向）に１ｇ（ｍ／ｓ^２）の重力加速度が印加されているものとしてバイアス誤差の補正値を算出する。この場合、制御部４は、加速度センサによって出力された検出信号と、鉛直方向下向きの１ｇ（ｍ／ｓ^２）の加速度との差を算出し、算出結果をバイアス誤差の補正値としてもよい。また、この場合、制御部４は、鉛直方向に直交する水平方向（ｘ方向及びｙ方向）の加速度成分は零であるものとして、水平方向成分についてのバイアス誤差の補正値を算出してもよい。

【0111】

上記の実施形態では、ＩＭＵの検出結果が、カメラポーズの算出の精度を上げるための補助手段として使用される例を説明した。本実施形態において、ＩＭＵの検出結果の使用方法はこれに限定されず、例えば、ロストが発生した場合にＩＭＵの検出結果のみからカメラポーズを算出することに使用されてもよい。ここで、ロストとは、特徴点マッチングに成功した特徴点の数が所定の閾値以上存在しないこと、又は、位置測定装置が１つ前のフレーム以前のフレーム内の特徴点と、現フレーム内の特徴点と、の幾何学的位置関係に基づいてカメラポーズを算出することができないことを意味する。

【0112】

上記の実施形態では、移動体１の位置履歴データを蓄積する用途に位置測定装置１００を適用する用途について説明した。本実施形態では、位置測定装置１００の用途は上記に限らず、例えば移動体１の運転制御に適用されてもよい。こうした用途において、上記の実施形態と同様にバイアス誤差をリアルタイムに更新することにより、例えば移動体１の運転制御の精度を向上することができる。

【0113】

以上のように、本開示における技術の例示として、実施の形態を説明した。そのために、添付図面および詳細な説明を提供した。

【0114】

したがって、添付図面および詳細な説明に記載された構成要素の中には、課題解決のために必須な構成要素だけでなく、上記技術を例示するために、課題解決のためには必須でない構成要素も含まれ得る。そのため、それらの必須ではない構成要素が添付図面や詳細な説明に記載されていることをもって、直ちに、それらの必須ではない構成要素が必須であるとの認定をするべきではない。

【0115】

また、上述の実施の形態は、本開示における技術を例示するためのものであるから、特許請求の範囲またはその均等の範囲において、種々の変更、置換、付加、省略などを行うことができる。

【産業上の利用可能性】

【0116】

本開示は、移動体の位置を測定する位置測定装置に適用可能である。

【符号の説明】

【0117】

１ 移動体
２ カメラ
４ 制御部
４１ 位置測定部
４１１ 特徴点抽出部
４１４ 位置算出部
４１５ 地図管理部
４１６ 静止判定部
４２ 処理部
４２１ バイアス補正部
４２２ バイアス除去部
４２３ バイアス更新部
４２４ 姿勢算出部
５ 記憶部
５１ 地図情報
７ 通信Ｉ／Ｆ
８ 駆動部
９ 温度センサ
１０ 進入禁止区域
２０ 通路
１００ 位置測定装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【手続補正書】

【提出日】2024-10-09

【手続補正1】

【補正対象書類名】特許請求の範囲

【補正対象項目名】全文

【補正方法】変更

【補正の内容】

【特許請求の範囲】

【請求項1】

情報処理装置であって、
移動体に搭載され、前記移動体の周囲の環境を撮像して撮像画像を取得する撮像装置から、前記撮像画像を受信し、
前記撮像画像に基づいて、ＳＬＡＭ（Simultaneous Localization and Mapping）を用いて前記移動体の位置を算出し、
前記算出された前記移動体の位置の信頼度を判断し、
前記信頼度に基づいて、前記移動体に対する制御の内容を判断する、
情報処理装置。

【請求項2】

前記撮像画像から特徴点を抽出し、
前記抽出された特徴点の数に基づいて、前記信頼度を判断する、
請求項１に記載の情報処理装置。

【請求項3】

前記抽出された特徴点の数が所定の数未満である場合には、ロストが発生したと判断する、請求項２に記載の情報処理装置。

【請求項4】

前記移動体の動きの検出結果を示す検出信号を受信し、
前記撮像画像と前記検出信号とに基づいて、前記移動体の位置を算出する、
請求項１に記載の情報処理装置。

【請求項5】

前記移動体の動きの検出結果を示す検出信号を受信し、
前記撮像画像と前記検出信号とに基づいて、前記信頼度を判断する、
請求項１記載の情報処理装置。

【請求項6】

ＩＭＵ、ジャイロセンサ、及び加速度センサのうちの少なくとも一つから前記検出信号を受信する、請求項４又は５に記載の情報処理装置。

【請求項7】

前記検出信号と、前記算出された前記移動体の位置とに基づいて、前記移動体の姿勢を算出する、請求項４又は５に記載の情報処理装置。

【請求項8】

前記撮像装置は、ＲＧＢ－Ｄカメラ、ステレオカメラ、及びデプスセンサのうちの少なくとも一つを含む、請求項１又は２に記載の情報処理装置。

【請求項9】

前記撮像画像から特徴点を抽出し、
前記撮像装置が順次、取得する複数の撮像画像の間で、前記抽出された特徴点を追跡することによって、前記移動体の位置を算出する、請求項１に記載の情報処理装置。

【請求項10】

前記情報処理装置からの指示に従って移動体を移動させる駆動部を備える、請求項１又は２に記載の情報処理装置。

【請求項11】

前記制御の内容は、前記移動体を静止させる指示を含む、請求項１又は２に記載の情報処理装置。

【請求項12】

移動体に搭載され、前記移動体の周囲の環境を撮像して撮像画像を取得する撮像装置から、前記撮像画像を受信し、
前記撮像画像に基づいて、ＳＬＡＭ（Simultaneous Localization and Mapping）を用いて前記移動体の位置を算出し、
前記算出された前記移動体の位置の信頼度を判断し、
前記算出された前記移動体の位置と、前記信頼度とをサーバに送信し、
前記サーバから、前記移動体に対する制御の内容を受信する、
情報処理装置。