(57)【特許請求の範囲】
【請求項１】
移動体に設けられる角速度検出装置であって、
ジャイロセンサと、
ジャイロセンサ以外のセンサからなる移動体運動計測センサと、
前記ジャイロセンサの零点の自己相関性を一次モデルで表した式を状態方程式とし、前記ジャイロセンサの出力から求まる角速度と前記移動体運動計測センサの出力から求まる角速度との差分を観測量として、該零点の最尤推定値を求め、該推定値に応じて零点較正した後の前記ジャイロセンサの出力を角速度検出値として出力する補正手段と、
を具備し、
前記状態方程式は、ωbiasを角速度の次元を有する零点バイアス、εを角加速度の次元を有する値、ωεをホワイトノイズとして、
ｄωbias／ｄｔ＝ε
ｄε／ｄｔ＝−αε＋ωε
と表せる、角速度検出装置。
【請求項２】
前記補正手段は、ある時刻における該差分が所定の閾値より大きい場合に角速度計測不能と判断する、請求項１に記載の角速度検出装置。
【請求項３】
前記移動体運動計測センサが温度依存性を有しない、請求項１に記載の角速度検出装置。
【請求項４】
ジャイロセンサとジャイロセンサ以外のセンサからなる移動体運動計測センサとが設けられた移動体の角速度を検出する方法であって、
前記ジャイロセンサの出力と前記移動体運動計測センサの出力とを入力する入力ステップと、
前記ジャイロセンサの零点の自己相関性を一次モデルで表した式を状態方程式とし、前記ジャイロセンサの出力から求まる角速度と前記移動体運動計測センサの出力から求まる角速度との差分を観測量として、該零点の最尤推定値を求め、該推定値に応じて零点較正した後の前記ジャイロセンサの出力を角速度検出値として出力する補正ステップと、
を具備し、
前記状態方程式は、ωbiasを角速度の次元を有する零点バイアス、εを角加速度の次元を有する値、ωεをホワイトノイズとして、
ｄωbias／ｄｔ＝ε
ｄε／ｄｔ＝−αε＋ωε
と表せる、角速度検出方法。
【請求項５】
ジャイロセンサとジャイロセンサ以外のセンサからなる移動体運動計測センサとが設けられた移動体の角速度を検出するために、コンピュータに、
前記ジャイロセンサの出力と前記移動体運動計測センサの出力とを入力する入力ステップと、
前記ジャイロセンサの零点の自己相関性を一次モデルで表した式を状態方程式とし、前記ジャイロセンサの出力から求まる角速度と前記移動体運動計測センサの出力から求まる角速度との差分を観測量として、該零点の最尤推定値を求め、該推定値に応じて零点較正した後の前記ジャイロセンサの出力を角速度検出値として出力する補正ステップであって、
前記状態方程式は、ωbiasを角速度の次元を有する零点バイアス、εを角加速度の次元を有する値、ωεをホワイトノイズとして、
ｄωbias／ｄｔ＝ε
ｄε／ｄｔ＝−αε＋ωε
と表せる、補正ステップと、
を実行させるための角速度検出プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体に設けられる角速度検出装置及びその角速度検出方法に関する。
【背景技術】
【０００２】
移動ロボット、自動車等の移動体では、自己の位置を推定するために、ジャイロセンサを使用して角速度を検出している。角速度が大きく変化する移動体の運動を計測する場合、ダイナミックレンジの広いジャイロセンサ（例えば、約±３００度／秒）を用いて角速度が計測される。
【０００３】
一方、製品コストを抑えるため、ジャイロセンサ出力信号のＡＤ変換分解能は１０ビット程度が一般的であり、これが制約条件となっている。低分解能のＡＤ変換器でダイナミックレンジの広いジャイロセンサ出力信号をサンプリングすると、ジャイロセンサの 1 LSB（Least Significant Bit）以下の零点オフセットが求められないため、計測される角速度にバイアス誤差が発生する。この誤差を含む角速度を積分して移動体の姿勢を求めると、バイアスドリフトが生じ、移動体の自己位置推定の精度に影響を与える。
【０００４】
また、移動体の走行する環境温度が変化すると、ジャイロセンサの零点が変動するため、補正が必要となる（例えば、下記特許文献１〜５参照）。
【０００５】
【特許文献１】特公平７−５８２９５号公報
【特許文献２】特開昭６１−４８７１４号公報
【特許文献３】特許第２５２３９１２号公報
【特許文献４】特開平１０−３３２３８２号公報
【特許文献５】特開２０００−５５６７１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
本発明は、上述した問題点に鑑みてなされたものであり、その目的は、ジャイロセンサの零点を動的に較正することができる角速度検出装置を提供することにより、例えば、それを搭載する移動体における高精度な自己位置推定を実現することにある。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、本発明の一つの面によれば、移動体に設けられる角速度検出装置であって、ジャイロセンサと、ジャイロセンサ以外のセンサからなる移動体運動計測センサと、前記ジャイロセンサの零点の自己相関性を一次モデルで表した式を状態方程式とし、前記ジャイロセンサの出力から求まる角速度と前記移動体運動計測センサの出力から求まる角速度との差分を観測量として、該零点の最尤推定値を求め、該推定値に応じて零点較正した後の前記ジャイロセンサの出力を角速度検出値として出力する補正手段と、を具備し、前記状態方程式は、ωbiasを角速度の次元を有する零点バイアス、εを角加速度の次元を有する値、ωεをホワイトノイズとして、
ｄωbias／ｄｔ＝ε
ｄε／ｄｔ＝−αε＋ωε
と表せる、角速度検出装置が提供される。
【０００８】
さらに、本発明の他の面によれば、上述の角速度検出装置で実行される方法と、コンピュータにその方法を実行させるためのプログラムと、が提供される。
【発明の効果】
【０００９】
ジャイロセンサの零点は一定ではなく、時間とともに変動するが、開示の角速度検出装置によれば、ジャイロセンサ以外のセンサからなる移動体運動計測センサが設けられ、ジャイロセンサの零点が推定され、その推定値に応じてジャイロセンサの出力について零点較正が動的に行われるため、検出される角速度の精度が向上する。
【発明を実施するための最良の形態】
【００１０】
以下、添付図面を参照して本発明の実施形態について説明する。図１は、本発明の一実施形態に係る角速度検出装置１００の構成を示すブロック図である。この角速度検出装置１００は、例えば、二輪速度差方式の移動ロボットに搭載されるもので、図１に示されるように、ジャイロセンサ１０２と、ＡＤ変換器（analogue-to-digital converter）１０４と、ジャイロセンサ以外のセンサからなる移動体運動計測センサ１０６と、低域通過フィルタ（ＬＰＦ：low-pass filter）１０８と、角速度補正処理部１１０と、を備える。
【００１１】
ジャイロセンサ１０２は、移動体の回転角速度を検出するもので、例えば、光ファイバジャイロである。ジャイロセンサ１０２の零点（実際の角速度が零のときのセンサ出力）は、一定でなく、環境の温度変化の影響を受けて時間とともに変動する。
【００１２】
ＡＤ変換器１０４は、ジャイロセンサ１０２の出力をサンプリングしてディジタル信号に変換（量子化）した後、その出力を角速度補正処理部１１０に供給する。かかる量子化に伴い、計測される角速度にバイアス誤差が発生する。
【００１３】
移動体運動計測センサ１０６は、例えば、移動体（移動ロボット）の駆動輪の回転数を計測するロータリエンコーダ（オドメータ）である。移動体運動計測センサ１０６は、その出力から角速度を演算することができるものであればよく、回転中心からオフセットを付けて設置した加速度センサ等でもよい。
【００１４】
低域通過フィルタ１０８は、移動体運動計測センサ１０６の出力から高調波成分を除去した後、その出力を角速度補正処理部１１０に供給する。
【００１５】
角速度補正処理部１１０は、ジャイロセンサ１０２の出力から求まる角速度と移動体運動計測センサ１０６の出力から求まる角速度とに基づいて、ジャイロセンサ１０２の零点を推定し、その推定値に応じてジャイロセンサ１０２の零点較正を動的に行い、較正後のジャイロセンサ１０２の出力を角速度検出値として出力する。
【００１６】
角速度補正処理部１１０は、プロセッサ、メモリ等（図示せず）を備えるコンピュータとして構成される。そして、プロセッサが、メモリにロードされたプログラムに従って動作することにより、その機能が実現される。以下、角速度補正処理部１１０の処理について説明する。
【００１７】
ジャイロセンサ１０２の出力から求まる角速度 ωg 及び移動体運動計測センサ（オドメータ）１０６の出力から求まる角速度 ωo は、それぞれ、次式のように表される。
ωg＝ωtrue＋ωbias＋ωg noise                 （１）
ωo＝ωtrue＋ωo noise                         （２）
ここで、ωtrueは真の角速度、ωbiasはジャイロセンサ１０２の零点バイアスによる角速度、をそれぞれ表す。また、ωg noise及びωo noiseは、それぞれ、ジャイロセンサ１０２及びオドメータ１０６の出力に含まれるホワイトノイズ（白色雑音）である。
【００１８】
ジャイロセンサ１０２の出力から求まる角速度ωgとオドメータ１０６の出力から求まる角速度ωoとの差分δを観測量とすると、δは次式に示されるものとなる。
δ＝ωg−ωo                                   （３）
＝ωbias＋ωg noise−ωo noise               （４）
＝ωbias＋ωobs noise                        （５）
ここで、ωg noise及びωo noiseが共にホワイトノイズであるため、それらの差分である観測ノイズωobs noiseもホワイトノイズとなる。
【００１９】
前述のように、ジャイロセンサ１０２の零点は一定ではなく、時間とともに変動するため、角速度の差分δにはこの変動が含まれる。ある時刻におけるジャイロセンサ１０２の零点の値に対しては、その時刻に近い時刻における零点の値の方が、その時刻から離れた時刻における零点の値よりも近い値となり、すなわち相関性が高い。
【００２０】
角速度補正処理部１１０は、次式に示されるように、この自己相関性を一次モデルで表したものを状態方程式とし、角速度の差分δを観測量としてカルマンフィルタによる最尤推定を行う。
ｄωbias／ｄｔ＝ε                             （６）
ｄε／ｄｔ＝−αε＋ωε                       （７）
ここで、εは角加速度を表し、ωεはホワイトノイズである。
【００２１】
式（６）及び式（７）は、ベクトル及び行列を用いると、次式となる。
ｄｘ(ｔ)／ｄｔ＝Ｆｘ(ｔ)＋Ｇｗ(ｔ)             （８）
ここで、ｘは、次の二次元列ベクトルである（［］Tは転置を示す）。
ｘ＝［ωbias  ε］T                            （９）
Ｆ＝（ｆij）は、次の各要素を有する２×２行列である。
ｆ11＝０      ｆ12＝１
ｆ21＝０      ｆ22＝−α                      （１０）
Ｇは、次の二次元列ベクトルである。
Ｇ＝［０  １］T                               （１１）
【００２２】
式（８）で表される連続時間状態方程式を離散化することにより、次の離散時間状態方程式が得られる。
ｘ(ｋ)＝Ａｘ(ｋ−１)＋ｕ(ｋ)                      （１２）
ここで、Ａ＝ｅFΔt＝（ａij）は、次の各要素を有する２×２行列である。Δｔは、サンプリング周期を表す。ｕ(ｋ) は、プロセスノイズである。
ａ11＝１      ａ12＝(１／α)(１−ｅ-αΔt)
ａ21＝０      ａ22＝ｅ-αΔt                  （１３）
【００２３】
角速度補正処理部１１０は、上記の式（１２）に示される状態方程式及び式（５）に示される観測方程式に基づき、カルマンフィルタを使用して角速度のバイアス成分ωbiasを推定し、零点較正を動的に行う。
【００２４】
図２は、図１に示される角速度検出装置１００を備える二輪速度差方式移動ロボットの動作を示すフローチャートである。図２に示される一連のブロックは、所定のサンプリング周期ごとに実行される。
【００２５】
まず、角速度補正処理部１１０は、ＡＤ変換器１０４によってサンプリング及びＡＤ変換されたジャイロセンサ１０２の出力 ωg を入力する（ブロック２０２）。
【００２６】
次いで、角速度補正処理部１１０は、移動体運動計測センサ（オドメータ）１０６及び低域通過フィルタ（ＬＰＦ）１０８を介して得られるデータから角速度 ωo を求める（ブロック２０４）。
【００２７】
次いで、角速度補正処理部１１０は、ジャイロセンサ１０２による角速度ωgとオドメータ１０６による角速度ωoとの差分δを算出する（ブロック２０６）。前述の式（３）、（４）及び（５）に示されるように、差分δは、ジャイロセンサ１０２の零点バイアスによる角速度ωbiasと観測ノイズωobs noiseとの和になっている。
【００２８】
次いで、角速度補正処理部１１０は、得られた角速度差分δが所定の閾値よりも小さい否かを判定する（ブロック２０８）。ジャイロセンサ以外の角速度計測可能な移動体運動計測センサとしてオドメータを使用する場合、例えば、スリップが発生すると正しい角速度を計測することができない。そこで、あるサンプリング時刻における角速度差分δが所定の閾値より大きい場合、外乱による誤差が含まれていると判断してジャイロの零点推定値には状態方程式から求めた予測値を使用することにより、零点のバイアス以外の誤差をキャンセルする（ブロック２１０）。
【００２９】
次いで、角速度補正処理部１１０は、前述のように、カルマンフィルタを使用して角速度のバイアス成分ωbiasを推定する（ブロック２１２）。カルマンフィルタは、前回のサンプリング時刻において予測された今回のサンプリング時刻における事前状態推定値を今回のサンプリング時刻における観測結果により訂正して事後状態推定値を得るとともに、次回のサンプリング時刻における事前状態推定値を予測する。
【００３０】
次いで、角速度補正処理部１１０は、ジャイロ零点オフセット量ωbiasの推定値でジャイロセンサ１０２による観測量ωgを補正（零点較正）して、補正後の角速度を出力する（ブロック２１４）。
【００３１】
最後に、移動ロボットの制御部（図示せず）は、零点較正されたジャイロセンサ出力すなわち角速度を使用して、ロボットの自己位置推定を行う（ブロック２１６）。以上の処理がサンプリング時刻ごとに実行される。
【００３２】
移動体が温度の異なる領域を跨いで移動する場合、ジャイロセンサ１０２の零点は環境の温度変化の影響を受けて変動する。移動体運動計測センサ１０６として温度依存性を有しないセンサを使用すると、移動体運動計測センサ１０６が、移動体が回転してないことを検知するとともに、同じ期間にジャイロセンサ１０２が角速度を検知した場合に、温度によりジャイロの零点が変化したことを検知することができる。
【００３３】
従来技術として、高域通過フィルタ（ＨＰＦ：high-pass filter）によりジャイロセンサ出力から低い周波数成分を除去することで零点の変動の影響をキャンセルする技術がある。しかし、この技術ではゆっくり回転する移動体の角速度を検出することができない。一方、上述した実施形態による手法では、このような問題は生じない。また、上記特許文献１には移動体の停止状態で零点を調整する方法が開示されているが、本実施形態によれば移動中に零点が変化した場合でも零点を調整することができる。
【００３４】
以上の実施形態に関し、以下の付記を開示する。
【００３５】
（付記１）  移動体に設けられる角速度検出装置であって、
ジャイロセンサと、
ジャイロセンサ以外のセンサからなる移動体運動計測センサと、
前記ジャイロセンサの零点の自己相関性を一次モデルで表した式を状態方程式とし、前記ジャイロセンサの出力から求まる角速度と前記移動体運動計測センサの出力から求まる角速度との差分を観測量として、該零点の最尤推定値を求め、該推定値に応じて零点較正した後の前記ジャイロセンサの出力を角速度検出値として出力する補正手段と、
を具備する角速度検出装置。
【００３６】
（付記２）  前記補正手段は、ある時刻における該差分が所定の閾値より大きい場合に角速度計測不能と判断する、付記１に記載の角速度検出装置。
【００３７】
（付記３）  前記移動体運動計測センサが温度依存性を有しない、付記１に記載の角速度検出装置。
【００３８】
（付記４）  ジャイロセンサとジャイロセンサ以外のセンサからなる移動体運動計測センサとが設けられた移動体の角速度を検出する方法であって、
前記ジャイロセンサの出力と前記移動体運動計測センサの出力とを入力する入力ステップと、
前記ジャイロセンサの零点の自己相関性を一次モデルで表した式を状態方程式とし、前記ジャイロセンサの出力から求まる角速度と前記移動体運動計測センサの出力から求まる角速度との差分を観測量として、該零点の最尤推定値を求め、該推定値に応じて零点較正した後の前記ジャイロセンサの出力を角速度検出値として出力する補正ステップと、
を具備する角速度検出方法。
【００３９】
（付記５）  前記補正ステップは、ある時刻における該差分が所定の閾値より大きい場合に角速度計測不能と判断する、付記４に記載の角速度検出方法。
【００４０】
（付記６）  前記移動体運動計測センサが温度依存性を有しない、付記４に記載の角速度検出方法。
【００４１】
（付記７）  ジャイロセンサとジャイロセンサ以外のセンサからなる移動体運動計測センサとが設けられた移動体の角速度を検出するために、コンピュータに、
前記ジャイロセンサの出力と前記移動体運動計測センサの出力とを入力する入力ステップと、
前記ジャイロセンサの零点の自己相関性を一次モデルで表した式を状態方程式とし、前記ジャイロセンサの出力から求まる角速度と前記移動体運動計測センサの出力から求まる角速度との差分を観測量として、該零点の最尤推定値を求め、該推定値に応じて零点較正した後の前記ジャイロセンサの出力を角速度検出値として出力する補正ステップと、
を実行させるための角速度検出プログラム。
【００４２】
（付記８）  前記補正ステップは、ある時刻における該差分が所定の閾値より大きい場合に角速度計測不能と判断する、付記７に記載の角速度検出プログラム。
【００４３】
（付記９）  前記移動体運動計測センサが温度依存性を有しない、付記７に記載の角速度検出プログラム。
【図面の簡単な説明】
【００４４】
【図１】本発明の一実施形態に係る角速度検出装置の構成を示すブロック図である。
【図２】図１に示される角速度検出装置を備える移動ロボットの動作を示すフローチャートである。
【符号の説明】
【００４５】
１００    角速度検出装置
１０２    ジャイロセンサ
１０４    ＡＤ変換器
１０６    移動体運動計測センサ
１０８    低域通過フィルタ
１１０    角速度補正処理部
【図１】
0005245531.tif 000002
【図２】
0005245531.tif 000003