特許 6994706 移動体の位置計測装置

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

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

(11)【特許番号】

(24)【登録日】2021-12-16

(45)【発行日】2022-01-14

(54)【発明の名称】移動体の位置計測装置

(51)【国際特許分類】

   G01C 19/5776 20120101AFI20220106BHJP

   G01C 21/10 20060101ALI20220106BHJP

   G01P 21/00 20060101ALI20220106BHJP

【ＦＩ】

G01C19/5776

G01C21/10

G01P21/00

【請求項の数】 6

(21)【出願番号】P 2021148141

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

【審査請求日】2021-09-15

【早期審査対象出願】

(73)【特許権者】

【識別番号】513169641

【氏名又は名称】株式会社テクノコア

(73)【特許権者】

【識別番号】521401944

【氏名又は名称】株式会社チアリータウン

(74)【代理人】

【識別番号】100194180

【弁理士】

【氏名又は名称】西谷 豊博

(72)【発明者】

【氏名】須藤 康成

(72)【発明者】

【氏名】渡邉 敬治

【審査官】續山 浩二

(56)【参考文献】

【文献】特開平０５－１９６４７５（ＪＰ，Ａ）

【文献】国際公開第２０１５／１７７８４３（ＷＯ，Ａ１）

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

【文献】国際公開第２０１９／０８７７７８（ＷＯ，Ａ１）

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

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

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

Ｇ０１Ｃ １９／５７７６

Ｇ０１Ｃ ２１／１０

Ｇ０１Ｐ ２１／００

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

【請求項1】

ジャイロセンサ及び加速度センサを用いて移動体の位置を計測する移動体の位置計測装置であって、前記ジャイロセンサ及び加速度センサからの信号を所定時間サンプリングする手段と、前記ジャイロセンサ及び加速度センサからの信号を直接入力し、前記ジャイロセンサからのピッチ角及びピッチ角変化と前記加速度センサからのピッチ角との加重平均を出力する相補フィルタと、前記サンプリングしたデータに基づき、関数推測する手段と前記関数推測手段で推測された関数により求めた値と前記サンプリングした値との誤差の大きさの変化に相当した所定の周波数を設定する手段と前記相補フィルタの係数値を設定する手段とを備え、前記関数推測手段で近似されたサンプリングしたデータの予想値及び前記周波数設定手段で得られた周波数に基づき前記相補フィルタから出力されるデータを補正する手段を備えた、移動体の位置計測装置。

【請求項2】

前記サンプリングしたデータは、第２の相補フィルタを介して前記関数推測手段と前記周波数設定手段と前記相補フィルタの係数値を設定する手段に出力されることを特徴とする請求項１に記載の移動体の位置計測装置。

【請求項3】

前記関数推測手段は、前記サンプリングしたデータを最小二乗法又は２次関数により関数近似することを特徴とする請求項１に記載の移動体の位置計測装置。

【請求項4】

前記サンプリング手段と前記相補フィルタは、相互に切り換えられて前記ジャイロセンサ及び加速度センサからの信号が入力されることを特徴とする請求項１に記載の移動体の位置計測装置。

【請求項5】

前記周波数設定手段は、前記関数推測手段により近似された関数の値と前記サンプリングデータとの相違の程度に基づき、周波数を決定することを特徴とする請求項１に記載の移動体の位置計測装置。

【請求項6】

前記相補フィルタの係数値を設定する手段は、前記サンプリングしたデータのばらつきの程度によって係数値が決定されることを特徴とする請求項１に記載の移動体の位置計測装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ジャイロセンサ及び加速度センサを用いて、移動体の位置を高い精度でリアルタイムに計測することができるようにした、位置計測装置に関するものである。

【背景技術】

【0002】

移動体の位置計測装置は、例えば、建築物の地下地盤強化のための薬液注入孔の形成や、地下へのガス、電気等の配管のための削孔の形成に、計画経路に沿った掘削が行われるように、掘削装置の掘削用ロッド等の先端位置を把握する目的で使用されている。このような場合、精度よく先端位置を計測するためには、使用されているジャイロセンサや加速度センサから生じるドリフトや、地中に敷設した長いケーブル等から拾われた外乱要因によるノイズ誤差を少なくする必要がある。
従来、このようなセンサから生じる誤差を除去する方法として、移動体が、止まっているか又は動きが少なく安定状態にある場合の保存したセンサデータから、最小二乗法を使って、時間の関数としてドリフト成分等の誤差成分を算出し、センサから得られるデータから当該関数分を差し引くことで、ドリフト等の誤差成分を含まないデータを作成し、このデータを用いて移動体の位置の計算を行っていた（例えば、特許文献１参照）。

【0003】

また、特許文献２には、加速度検出装置において、車両が所定距離走行する毎に加速度センサの出力を平均化してドリフト量を算出し、このドリフト量に基づいてゼロ点補正が行われることが開示されている。すなわち、加速度センサの出力値とドリフト量の前回値との重み付けの和としてドリフト量を算出し、加速度センサの出力値からこのドリフト量を減算してセンサ出力値としている。ここで重み付け係数は所定の定数が用いられている。
このような方法では、将来のドリフト量を逐次計算して予測するものではないため、現実のドリフト量との誤差が大きくなる可能性がある。

【先行技術文献】

【特許文献】

【0004】

【文献】特開２０１７－１５６０５号公報

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

【発明の概要】

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

【0005】

解決しようとする問題点は、非動作中のジャイロセンサや加速センサからの誤差成分の解析結果を反映させていたり、補正値が過去の実績から予め定められているような構成となっているため、動作中にリアルタイムでの誤差の傾向が分析されておらず、実情に合った正確な移動体の位置計測が行われていない点である。

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

【0006】

物体にかかる動きや回転の情報を検出するデバイスとして、一般にジャイロセンサや加速度センサが用いられるが、姿勢位置の情報を得るためには、それぞれ長所、短所がある。ジャイロセンサでは、検出した角速度を積分するため、誤差成分累積してドリフトが生じ、加速度センサでは、動きがある場合に重力以外の成分も加わってしまう。これらの誤差成分を取り除くために、ジャイロセンサでは、ハイパスフィルタ（ＨＰＦ）を通し、加速度センサには、ローパスフィルタ（ＬＰＦ）を通すことで、解決できる。これらを組み合わせたフィルタとして、カルマンフィルタやＭａｄｇｗｉｃｋｆｉｌｔｅｒなどの相補フィルタが知られている。すなわち、カットオフ周波数ｆｃ以下ではジャイロセンサからのドリフトによるオフセット分が除去され、カットオフ周波数ｆｃ以上では加速度センサからの高周波成分を除去することができる。

【0007】

本発明は、ジャイロセンサや加速度センサからのノイズを、相補フィルタを用いて予めキャンセルしてから、移動体の移動中に発生する、ドリフト等の誤差の傾向を逐次予測しながら補正することで、移動体の位置を高い精度で、リアルタイムに計測することができるようにすることを、主要な特徴とする。

【発明の効果】

【0008】

本発明の移動体の位置計測装置は、移動中に逐次、センサからの信号を予め相補フィルタでノイズを除去してから、関数計測を行って、その関数でドリフト等の傾向を予測しながら補正するため、移動体の移動途中にドリフト等の発生する傾向が変わっても、対応する関数で予測ができ、位置の計測が正確に行われるという利点がある。

【図面の簡単な説明】

【0009】

【図1】本発明に係る移動体の位置計測装置の要部を示すブロック図である。

【図2】本発明に係る関数推測回路及び周波数設定回路の動作を説明する図である。

【図3】本発明に係る補正回路の出力タイミングを示す説明図である。

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

【0010】

移動体のセンサからのドリフト誤差の補正を、短時間に予測と補正を繰り返して行うことで、移動中の正確な位置の計測を実現した。

【実施例】

【0011】

以下、図面により本発明の位置計測装置を詳細に説明する。
図１は、本発明に係る移動体の位置計測装置の要部を示すブロック図であり、１はジャイロセンサ及び加速度センサからなるセンサ、２はセンサ１からの信号を所定の時間間隔でサンプリング回路３又は相補フィルタ８に交互に切り替えるゲート回路、３は所定のサンプリング時間によりサンプリングを行うサンプリング回路、４は相補フィルタ、５は関数推測回路、６は周波数設定回路、７は相補フィルタ８の係数を設定する回路、９は関数回路５から推測された関数及び周波数設定回路６により設定された周波数に基づき相補フィルタ８からの信号を補正する補正回路、１０は補正回路９で補正された信号から所定の演算により移動体の位置を計測処理する回路を、それぞれ示す。

【0012】

最初に、センサ１からの信号、すなわちジャイロセンサ及び加速度センサから検出された信号は、ゲート回路２を介して、サンプリング回路３に入力される。サンプリング回路３に接続されている時間は、１０秒前後であり、サンプリングに必要な任意の時間が設定される。なお、センサ１から出力された信号は、ゲート回路２により、サンプリング回路３と後述する相補フィルタ８に交互に切り替えて入力されるように構成されている。

【0013】

次に、サンプリング回路３でジャイロセンサ及び加速度センサの出力をサンプリングした後、サンプリングされたデータは、相補フィルタ４に入力され、ノイズが除去される。
相補フィルタ４により出力されるピッチ角θ（n＋１）は、後述する相補フィルタ８も同様であるが、ジャイロからのピッチ角変化をΔθｇ、加速度によるピッチ角θα、係数をｋとすると、
式 θ（n＋１）＝ｋ×（θ（n）＋Δθｇ）＋（１－ｋ）×θα（ｎ＋１）
により求められる。
係数ｋの値を大きくすると、ジャイロセンサ出力値のドリフト量に与える影響が加速度センサの出力値よりも大きくなる。
なお、相補フィルタ４の係数ｋは、デフォルトとして、０．９６が予め設定されている。

【0014】

相補フィルタ４の出力は、関数推測回路５、周波数設定回路６、係数設定回路７に入力される。図２は、本発明の関数推測回路及び周波数設定回路の動作を説明する図である。
関数推測回路５では、相補フィルタ４から出力されるサンプリングされたデータがサンプリング時間内（１０秒前後）に時間に対してどのような傾向にあるのか、関数推測される。最小二乗法、一次関数、二次関数などにより関数近似を行い、この関数を用いてドリフト等の誤差成分の将来生ずる誤差を推測する。したがって、短い時間内（１０秒程度）にドリフト等の誤差の傾向を逐次推測するため、誤差の傾向が変わっても、すなわち、近似関数が変わっても、傾向に追従する正確な推測が可能となる。

【0015】

周波数設定回路６では、関数推測回路５で推測された関数により求められた値とサンプリング値との誤差分（ゆらぎ）を、周波数として捉えて、所定の周波数を決定する。推測された関数に対してどれだけずれているのかを傾きの角度などから計算し、ずれの値が予め定めた値よりも大きい場合に、ゆらぎがあるものとして（図２中の波形部分）、所定の周波数を決定する。なお、ずれの大きさに応じて、いくつかの周波数から選択できるように構成してもよい。
図２では、推測と補正が10秒前後で交互に繰り返されている様子を示している。すなわち、時間ｔ２～ｔ３の間に推測された関数（一次関数）とゆらぎに対応する周波数で、次の時間ｔ３～ｔ４の間にセンサからの信号が補正されることになる。

【0016】

係数設定回路７では、相補フィルタ８の係数値を決定する。相補フィルタ８は、前述の相補フィルタ４と同様の構成であり、フィルタの出力を決定するために、前述の式で係数ｋの値を求める必要がある。相補フィルタ４からの出力データにばらつきが大きい場合には、係数ｋの値を小さくして、相補フィルタ８のピッチ角がジャイロによる寄与率を小さくする。デフォルトでは０．９６ぐらいであるが、０．７ぐらいまで下げるのが好ましい。ばらつきの検出には、所定の閾値を設けておき、ばらつきが大きい場合には、相補フィルタ８の係数ｋの値を変えられるように構成する。

【0017】

次に、ゲート回路２でセンサ１からの信号がサンプリング回路３から相補フィルタ８へ切り換えられると、相補フィルタ８を介してノイズ分の軽減がされた信号が出力されて、補正回路９で関数推測回路５と周波数決定回路６によって補正されることになる。すなわち、補正回路９では、関数推測回路５で推測された関数に基づく予想値が所定期間（10秒程度）適用されて、信号から差し引かれ、また、周波数設定回路６によって決定された周波数のローパスフィルタによりゆらぎ成分が除かれる。このようにして補正回路９の出力信号は、ドリフト等の誤差成分が除かれたものとなり、この出力信号をもとにして、計測処理回路１０で所定の計算がされ、移動体の位置が求められる。

【0018】

図３は、本発明に係る補正回路の出力タイミングを説明する説明図である。前述の補正回路９の出力タイミングは、図中の計測出力１のａで示すように、断続的に出力されることになるが、同様のセンサをもう一台設置して、補正するタイミングをずらして同様の回路で処理を行うと、もう一台のセンサでは、計測出力２のｂで示すタイミングで出力させることができる。このように２台のセンサで構成すれば、計測出力１と計測出力２とを合わせることで連続して位置情報が得られるようできるため、よりリアルタイムに位置の計測が可能となる。また、計測出力３は、同様のセンサを３台設置した場合の各補正回路の出力を示すものであるが、センサが１台又は２台の場合と比べて、出力ｃのタイミングが遅くなるため、サンプリングする時間が長くでき、多くのサンプリングデータから関数の近似やゆらぎの周波数が求められ、より正確な計測データの出力が得られるようになる。これらのタイミングは、各センサのゲート回路２により、設定が可能である。

【産業上の利用可能性】

【0019】

ジャイロセンサ及び加速度センサからの信号を、ドリフト等を除去した精度の高い情報としてリアルタイムで検出して測定できるため、例えば、掘削装置などを使用する土木事業において、発掘の計画経路が目視などで容易に確認できないような現場にとって、センサからの信号が長いケーブルを介した場合と無線で通信する場合で誤差の大きさが異なるような場合にも正確な位置の測定が可能である。また、センサと測定器の間に設ける付属部品又はセンサが一体となった部品に、本発明の技術を採用することで、既存の各種計測器の精度を高めることもできる。本発明は、当該分野およびリアルタイムで誤差の補正が要求されるような分野において、産業上の利用可能性が高い。

【符号の説明】

【0020】

１ センサ
２ ゲート回路
３ サンプリング回路
４ 相補フィルタ
５ 関数推測回路
６ 周波数設定回路
７ 係数設定回路
８ 相補フィルタ
９ 補正回路
１０ 計測処理回路

【要約】

【課題】移動中の移動体から逐次誤差ドリフトを予測しながら補正することで、移動体の位置がリアルタイムで精度よく計測できることを可能とする。
【解決手段】ジャイロセンサ及び加速度センサを用いて移動体の位置を計測する移動体の位置計測装置であって、前記ジャイロセンサ及び加速度センサからの信号を所定時間サンプリングする手段３と、前記ジャイロセンサ及び加速度センサからの信号を直接入力する相補フィルタ８と、前記サンプリングしたデータに基づき、関数推測する手段５と周波数を設定する手段６と前記相補フィルタの係数値を設定する手段７とを備え、前記関数推測手段で近似された予想値及び前記周波数設定手段で得られた周波数に基づき前記相補フィルタから出力されるデータを補正する手段９を備えた。
【選択図】図１

【図1】

【図2】

【図3】