(57)【特許請求の範囲】
【請求項１】
互いに直行する三軸方向の加速度を測定する加速度センサを装着した物体を上下方向に移動させて、加速度信号を取得する工程と、
前記取得した加速度信号からノイズを除去する工程と、
前記ノイズを除去した加速度信号に対して、速度補正、揺れ補正、回転補正を行う工程と、
前記補正後の加速度信号に基づいて前記物体の位置及び姿勢を推定する工程と、
を含むことを特徴とする加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法。
【請求項２】
前記物体は、計測機器またはカメラであることを特徴とする請求項１に記載の加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法。
【請求項３】
互いに直行する三軸方向の加速度を測定する加速度センサを備えた計測手段と、
前記計測手段を吊り下げて、上下方向に移動させる移動手段と、
前記計測手段から受信した計測信号に基づいて、前記計測手段の位置及び姿勢を推定する推定手段と、を備え、
前記推定手段は、
前記受信した計測信号のノイズを除去するフィルタ部と、
前記フィルタ部によりフィルタ処理した計測信号に対して、速度補正、揺れ補正、回転補正を行う補正部と、を備えたことを特徴とする加速度信号を用いた吊り下げ移動物体の位置・姿勢推定装置。
【請求項４】
前記物体は、計測機器またはカメラであることを特徴とする請求項３に記載の加速度信号を用いた吊り下げ移動物体の位置・姿勢推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、計測機器やカメラ等の物体をワイヤーや紐等を用いて吊り下げて、上下に昇降させた際の位置や姿勢を、物体自身に取り付けた加速度センサの信号を用いて精度良く推定する方法及び装置に関するものである。
【背景技術】
【０００２】
従来より、物体や空間内面の形状を測定するための技術が種々提案されている（例えば、特許文献１、特許文献２、特許文献３参照）。
【０００３】
特許文献１に記載された技術は、複数台のカメラを備えた光学式モーションキャプチャと、９軸センサを備えたセンサモジュールとを備えた動作計測において、光学式マーカと同一体節上にセンサモジュールを設け、センサモジュールに対する光学式マーカの位置関係を記憶しておく。そして、世界座標系における隠れた光学式マーカの座標値を、センサモジュールに対する光学式マーカの位置関係と、世界座標系におけるセンサモジュールの座標値と、センサモジュールの９軸センサによって取得された回転行列を用いて計算されるセンサモジュール座標系と世界座標系間の変換行列とを用いて推定する。
【０００４】
特許文献２に記載された技術は、角速度に基づいて、演奏操作中のスティック部の回転軸方向を推定し、スティック部の長手方向の軸成分を除いた角速度に基づいて、スティック部の先端部の角速度を算出し、推定された直近の回転軸方向及び算出されたスティック部の先端部の直近の角速度に基づいて、所定時間後のスティック部のワールド座標系の位置を算出する。そして、加速度に基づいて、移動距離を算出し、算出された当該位置及び算出された当該移動距離に基づいて、所定時間後のスティック部のワールド座標系の位置を補正する。
【０００５】
特許文献３に記載された技術は、撮像手段の視点を変えて測定対象物の画像を入力し、第１視点で測定して入力した画像の複数の特徴点を抽出し、第２視点で測定して入力した画像の特徴点に相当する対応点を抽出する。そして、２視点における重力方向と重力回りの角度を入力し、２視点における重力方向と角度から各視点における撮像手段の姿勢を算出し、姿勢値と特徴点及び対応点から第１視点と第２視点における各特徴点と対応点を通る視線ベクトルを算出し、視線ベクトルと特徴点及び対応点に基づき第１視点から第２視点までの並進運動ベクトルを算出し、並進運動ベクトルと視線ベクトルにより測定対象物の３次元形状を算出する。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２０１６−６４１５号公報
【特許文献２】特開２０１３−２１３９４７号公報
【特許文献３】特開平１１−３７７３６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ところで、物体の加速度から位置を算出する場合には、加速度を二度積分する必要がある。このため、加速度信号を用いて物体の位置を推定する従来の手法では、得られた加速度を積算する際に、信号ノイズや姿勢・速度誤差等の影響が増幅されるため、高い精度で位置や姿勢を推定することが困難であった。
【０００８】
特許文献１〜特許文献３に記載された技術は、種々の手法を用いて物体の位置を推定するものであるが、信号ノイズや姿勢・速度誤差等の影響を排除することについては考慮されておらず、高い精度で位置や姿勢の推定を行うことができるとは言い難かった。
【０００９】
本発明は、上述した事情に鑑み提案されたもので、高い精度で位置や姿勢の推定を行うことが可能な加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法及び推定装置を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法及び推定装置は、上述した目的を達成するため、以下の特徴点を有している。すなわち、本発明に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法は、互いに直行する三軸方向の加速度を測定する加速度センサを装着した物体を上下方向に移動させて、加速度信号を取得する工程と、取得した加速度信号からノイズを除去する工程と、ノイズを除去した加速度信号に対して、速度補正、揺れ補正、回転補正を行う工程と、補正後の加速度信号に基づいて物体の位置及び姿勢を推定する工程とを含むことを特徴とするものである。
【００１１】
本発明に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定装置は、互いに直行する三軸方向の加速度を測定する加速度センサを備えた計測手段と、計測手段を吊り下げて、上下方向に移動させる移動手段と、計測手段から受信した計測信号に基づいて、計測手段の位置及び姿勢を推定する推定手段とを備えている。そして、推定手段は、受信した計測信号のノイズを除去するフィルタ部と、フィルタ部によりフィルタ処理した計測信号に対して、速度補正、揺れ補正、回転補正を行う補正部とを備えたことを特徴とするものである。
【００１２】
また、本発明に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法及び推定装置において、移動可能に吊り下げた物体は、計測機器またはカメラとすることが可能である。
【発明の効果】
【００１３】
本発明に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法及び推定装置によれば、三軸の加速度センサを用いて吊り下げ移動物体の位置及び姿勢の推定を行うが、この際、加速度センサから受信した加速度信号のノイズ除去処理及び速度、揺れ、回転の補正処理を行うことにより、高い精度で位置や姿勢の推定を行うことが可能となる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法の概略フローチャート。
【図２】本発明の実施形態に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定装置の概略ブロック図。
【図３】ローカル座標とグローバル座標の説明図。
【図４】補正前速度と補正後速度の一例を示す説明図。
【図５】鉛直方向移動量の推定結果と実際の計測結果の比較を示す説明図。
【図６】物体の揺れの補正例を示す説明図。
【図７】物体の回転の補正例を示す説明図。
【発明を実施するための形態】
【００１５】
以下、図面を参照して、本発明の実施形態に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法及び推定装置を説明する。図１〜図７は本発明の実施形態に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法及び推定装置を説明するもので、図１は位置・姿勢推定方法の概略フローチャート、図２は位置・姿勢推定装置の概略ブロック図、図３はローカル座標とグローバル座標の説明図、図４は補正前速度と補正後速度の一例を示す説明図、図５は鉛直方向移動量の推定結果と実際の計測結果の比較を示す説明図、図６は物体の揺れの補正例を示す説明図、図７は物体の回転の補正例を示す説明図である。
【００１６】
＜吊り下げ移動物体の位置・姿勢推定方法及び推定装置の概要＞
本発明の実施形態に係る吊り下げ移動物体の位置・姿勢推定方法及び推定装置は、加速度の信号ノイズに対してローパスフィルタやカルマンフィルタ等のフィルタ処理を施すとともに、物体の移動速度に対して誤差の補正を行う。また、物体をワイヤーや紐等を用いて吊り下げた場合、物体には揺れ（振り子運動）や回転（ヨーイング）が発生するため、加速度周期等から回転角、揺れの幅等を推定し補正する。これにより、物体の位置・姿勢を精度良く推定することが可能となる。
【００１７】
なお、加速度センサから出力される信号には、重力成分、並進加速度成分、回転運動の成分(遠心加速度成分、接線加速度成分)が混在しており、１つの加速度センサ信号だけではこれらを分離することができない。このため、本発明では三軸の加速度センサ（３Ｄモーションセンサ等）を用いることを前提としている。
【００１８】
＜吊り下げ移動物体の位置・姿勢推定方法の詳細＞
本発明の実施形態に係る吊り下げ移動物体の位置・姿勢推定方法は、図１に示すように、互いに直行する三軸方向の加速度を測定する加速度センサを装着した物体を上下方向に移動させて、加速度信号を受信（取得）する工程（Ｓ１）と、受信（取得）した加速度信号からノイズを除去する工程（Ｓ２）と、ノイズを除去した加速度信号に対して、速度補正、揺れ補正、回転補正を行う工程（Ｓ３、Ｓ４、Ｓ５）と、補正後の加速度信号に基づいて物体の位置及び姿勢を推定する工程（Ｓ６）とを含んでいる。
【００１９】
＜吊り下げ移動物体の位置・姿勢推定装置の概要＞
上述した吊り下げ移動物体の位置・姿勢推定方法に使用する位置・姿勢推定装置は、図２に示すように、計測手段１１と、移動手段１２と、推定手段１３とを備えており、推定手段１３は、フィルタ部１３ａと、補正部１３ｂとを備えている。
【００２０】
計測手段１１は、互いに直行する三軸方向の加速度を測定する加速度センサを備えており、吊り下げ移動物体に取り付けられている。移動手段１２は、計測手段１１を吊り下げて、上下方向に移動させるための装置で、例えば、計測手段１１を吊り下げるワイヤや紐等と、ワイヤや紐等の巻き取り及び巻き出しを行う装置とからなる。
【００２１】
推定手段１３は、計測手段１１から受信した計測信号に基づいて、フィルタ処理及び補正処理を行って、計測手段１１の位置及び姿勢を推定する。この推定手段１３は、受信した計測信号のノイズを除去するフィルタ部１３ａと、フィルタ部１３ａによりフィルタ処理した計測信号に対して、速度補正、揺れ補正、回転補正を行う補正部１３ｂとを備えている。フィルタ部１３ａは、加速度の信号ノイズに対してローパスフィルタやカルマンフィルタ等のフィルタ処理を施す。補正部１３ｂは、コンピュータ及びこれにインストールされたプログラムにより構成されており、所定の演算を行うことにより、速度補正、揺れ補正、回転補正を行う。
【００２２】
＜上下方向移動量の推定方法＞
図３に示すように、ある物体のローカル座標系をｘ-ｙ-ｚ、グローバル座標系をＸ-Ｙ-Ｚとする。この物体に加速度センサ（計測手段１１）を取り付けて移動させると、その移動に伴った加速度が発生する。なお、物体自体に加速度センサを取り付けることから、計測される加速度は物体のローカル座標系ｘ-ｙ-ｚの各軸方向の加速度αx(t)，αy(t)，αz(t)である。この物体のローカル座標系の各軸がグローバル座標に対してθx，θy，θz回転しているとすると、グローバル座標系における加速度αx(t)，αy(t)，αz(t)は、この物体の回転を考慮して下記式（１）のように求められる。
【００２３】
【数１】
0006677576.tif 000002
【００２４】
なお、計測される加速度信号αx(t)，αy(t)，αz(t)にはノイズが含まれるため、ローパスフィルタやカルマンフィルタ等のフィルタ処理を施す。ここで、この物体を上下に昇降させる場合を対象として、鉛直Ｚ方向の加速度αz(t)に着目する。また、物体は移動開始前後では停止していると考える。この場合、物体の鉛直Ｚ方向（グローバル座標系）の移動速度νz（ｔ）は、上記式（１）に示す鉛直Ｚ方向の加速度αz(t)を積分することにより、下記式（２）のように求められる。
【００２５】
【数２】
0006677576.tif 000003
【００２６】
上記式（２）において、Δtは加速度の計測間隔時間、ｇは重力加速度である。この物体の移動速度νz（ｔ）は、移動開始前後は物体が停止状態にあると考えることから、計測終了時（ここでは、ｔ＝ｔLと仮定する）では移動速度がゼロにならければならない。しかしながら、実際に移動開始前後に物体を停止させて計測した加速度から、上記式（１）および上記式（２）を用いて移動速度νz（ｔ）を算出すると、計測終了時（ｔ＝ｔL）の移動速度νz（ｔL）はゼロにならない（図４参照）。これは、上記式（２）に示した加速度を積算してゆく過程で、計測誤差が蓄積するためである。
【００２７】
そこで、この計測誤差が一次関数に従うと仮定し、上記式（２）により求めた計測終了時（ｔ＝ｔL）の移動速度νz（ｔL）を用いて、次のような補正関数Ｈ（t）を作成する（下記式（３））。下記式（３）は速度補正に関する式である。
【００２８】
【数３】
0006677576.tif 000004
【００２９】
この補正関数Ｈ（t）を用いて、計測終了時の移動速度がゼロになるよう、移動速度を下記式（４）のように補正する。図４に、補正前の移動速度と補正後の移動速度の一例を示す。
【００３０】
【数４】
0006677576.tif 000005
【００３１】
物体の鉛直方向の移動量Ｚ（ｔ）は、補正後の移動速度を用いて、下記式（５）のように積分することで求める。
【００３２】
【数５】
0006677576.tif 000006
【００３３】
図５は、加速度センサ（三次元モーションセンサ；３ＤＭＳ）で計測した加速度から求めた鉛直方向移動量の推定結果と、物体の鉛直方向移動量をトータルステーションで計測した結果を比較した例である。３ＤＭＳから推定した鉛直方向移動量は、トータルステーションの測定結果とほぼ一致しており、その誤差は約２ｍの引き上げ量（移動量）に対して１％未満である。
【００３４】
＜水平方向移動量の推定方法＞
ワイヤや紐等を用いて吊り下げた物体を上下に昇降させる場合、その物体の運動は単振り子運動であると捉えることができる。水平方向の移動量が、振り子の支点から物体までの長さＬ（ｔ）に対して微小であると考えると、ローカル座標系の水平方向移動量ｘ（ｔ），ｙ（ｔ）は、各軸の回転角θx，θyを用いて、下記式（６）により表わされる。
【００３５】
【数６】
0006677576.tif 000007
【００３６】
また、振り子の長さＬ（ｔ）は物体を鉛直方向に移動させることで変化し、計測開始前の支点から物体までの長さをＬiniとすれば、下記式（７）のように表わされる。
【００３７】
【数７】
0006677576.tif 000008
【００３８】
上記式（７）において、Ｚ（ｔ）は、式（４）に示す鉛直方向の移動量である。なお、物体をワイヤや紐等で吊り下げた場合、例えば風やコリオリ力等の影響により、移動開始前の物体には若干の揺れが生じる。そこで、各軸の回転角θx，θy，θzから移動開始前の振り子の周期Ｔiniを測定すれば、初期長さＬiniを周期Ｔiniから、下記式（８）で求めることができる。式（６）、式（７）、式（８）は、揺れ補正に関する式である。
【００３９】
【数８】
0006677576.tif 000009
【００４０】
物体をワイヤや紐等を用いて吊り下げた場合、このワイヤや紐等のよじれの影響により物体は回転するため、この回転量についても補正する必要がある。この時のｚ軸周りの回転（ヨーイング）角をθzとすると、グローバル座標系の水平方向移動量Ｘ（ｔ），Ｙ（ｔ）は、下記式（９）のように求められる。式（９）は、ヨーイング補正に関する式である。
【００４１】
【数９】
0006677576.tif 000010
【００４２】
図６及び図７は、計測装置に加速度センサを搭載し、それぞれ水平方向の移動量と回転（ヨーイング）を補正した事例である。図６及び図７から明らかなように、本発明によれば、物体の揺れ及び回転（回転）を適切に補正することができる。
【００４３】
＜計測手段＞
先に説明したが、計測手段１１は、移動物体に取り付ける加速度センサ（三次元モーションセンサ；３ＤＭＳS）部分と、計測した加速度を処理するパーソナルコンピュータ等から構成される。
【００４４】
＜適用例＞
本発明に係る加速度信号を用いた吊り下げ移動物体の位置・姿勢推定方法及び推定装置は、例えば、地下資源を採掘した後に放置された鉱山廃鉱の地下空洞や、廃棄された地下施設の空間の形状を迅速かつ正確に計測することにより、空洞充填工事を行う際に、充填材配合設計、充填材施工等を円滑に行う装置及び方法に適用することができる。
【００４５】
このような装置及び方法は、例えば、特開２０１５−２３０３０１号公報に記載されている。特開２０１５−２３０３０１号公報に記載された空間内面形状の計測装置は、測定手段と、位置検出手段と、収容手段と、移動手段と、形状演算手段とを備えている。この計測装置は、種々の態様からなる空間内面形状を計測するための装置であり、地盤内に存在する空洞、トンネル、貯水タンク、下水管等の空間内面形状を計測することができる。
【００４６】
また、特開２０１５−２３０３０１号公報に記載された空間内面形状の計測方法は、このような構成からなる空間内面形状の計測装置を用いて、計測対象となる空間内に、レーザー光の照射部と、広角レンズと、撮像素子と、位置検出手段とが一体に収容された収容手段を設置する。空間内面形状を計測するには、計測対象となる空間内で、収容手段を移動させるとともに、その位置を検出しながら、空間内面に対して円環状にレーザー光を照射し、空間内面に形成されたレーザー光像を広角レンズにより撮像面に結像させて画像データとして出力する。そして、出力された画像データと、当該画像データの検出位置データとに基づいて、演算処理を行うことにより空間内面の形状を求めるようになっている。
【００４７】
＜本発明の有利な効果＞
本発明では、物体自身に取り付けた加速度センサの信号のみを利用するため、人が入ることが出来ない場所（例えば、狭隘空間や危険個所等）や、ＧＮＳＳ等の電波を受信できない場所（例えば、屋内や地下等）であっても、遠隔操作で物体の位置・姿勢を推定することが可能である。また、高精度で位置・姿勢を推定することができるため、計測装置やカメラ等へ適用することにより、高い精度で計測を行うことができる。
【符号の説明】
【００４８】
１０  位置・姿勢推定装置
１１  計測手段
１２  移動手段
１３  推定手段
１３ａ  フィルタ部
１３ｂ  補正部
【図１】
0006677576.tif 000011
【図２】
0006677576.tif 000012
【図３】
0006677576.tif 000013
【図４】
0006677576.tif 000014
【図５】
0006677576.tif 000015
【図６】
0006677576.tif 000016
【図７】
0006677576.tif 000017