JP2006349399A - 方位角計測装置および移動体 - Google Patents

Abstract

【課題】簡素で安価な構成でなおかつ精度の高い方位角計測装置を備えた移動体を提供する。
【解決手段】駆動車輪の回転速度を計測する駆動輪回転速度計測手段と、前記回転角速度推定値ω_Ａ３１２を出力できるジャイロセンサと、前記駆動輪回転速度からオドメトリによって前記回転角速度推定値ω_Ｂを計算する運動学演算手段と、前記回転角速度推定値ω_Ａ３１２と前記回転角速度推定値ω_Ｂ４１２とを逐次選択する回転角速度推定値選択手段と、前記回転角速度推定値選択手段４０３の出力を積分する積分器４０４と、からなる移動体の方位角計測装置を構成し、前記回転角速度推定値選択手段４０３が常に信頼性の高い回転角速度推定値を出力するようにした。
【選択図】図４

Description

本発明は、路面上を走行する移動体に関し、特にその方位角計測装置に関する。

従来、平面上を走行する移動体の方位角計測装置には、車輪を駆動するモータの回転速度から運動学関係式を用いて移動体の回転速度を求めて積分するもの（オドメトリ、技術［１］）、ジャイロセンサで移動体の回転角速度を計測して積分するもの（技術［２］）、があった。

また、上記技術［１］と［２］でそれぞれ求めた移動体回転角速度の差の大きさを監視し、差の大きさが一定の閾値内であれば技術［１］で求めた回転角速度を、閾値を超えたときは技術［２］で求めた回転角速度を採用して積分するというものもあった（技術［３］、非特許文献１）。

ここで、図１２は非特許文献１で開示されている移動体の方位角度算出方法を示す流れ図である。図においてΔθ_ｇｙｒｏはジャイロセンサから得られる移動体の回転角速度推定値、Δθ_ｏｄｏは駆動輪回転速度からオドメトリによって算出される移動体の回転角速度推定値、Δ_Ｇ−Ｏは２つの回転角速度推定値の差、Δθ_{ｔｈｒｅｓ}は適当に決めた閾値、θは方位角推定値、Ｔはサンプリング時間であって、右下の添え字ｉは第ｉステップ目を表す。路面や車輪の条件がよければ、Δθ_ｏｄｏは正確に得られるが、段差乗り越えなどで車輪が一時空転したりすると正しい値から大きく乖離する。ジャイロセンサはノイズや出力値のドリフト現象の影響を受けるためΔθ_ｇｙｒｏの定量的な信頼性はオドメトリに比べると低いが、駆動輪の状態にかかわらず移動体の動作を反映する。この性質を利用して、Δ_Ｇ−Ｏの大きさがある値Δθ_{ｔｈｒｅｓ}を超えたときはΔθ_ｇｙｒｏを、それ以外のときはΔθ_ｏｄｏを計算に用いることで、より正確な方位角推定値を得ることができる。

また、特許文献１では、技術［２］で求めた姿勢角の変化に対して不感帯を設け、特に移動体静止時の出力値の振動を抑制したものが開示されている（技術［４］）。図１３は特許文献１で開示されている姿勢角検出装置の全体構成を示した図である。図において２６ａは検出対象が単位時間に運動した角度である運動角を計算する運動角演算装置、２６ｂは仮の姿勢角度である静止角を計算する静止角演算装置、２６ｃは２６ａと２６ｂで得られた運動角と静止角から仮の姿勢角を計算する姿勢角演算装置、２８は２６ｃで得られた仮の姿勢角の微小な出力変動を抑制して出力する不感帯処理装置である。不感帯幅として設定されたある一定幅以上の動きにのみ出力値を反応させることでノイズを抑制し、静止時の安定性に優れた姿勢角検出装置が得られる。

また、特許文献２では、ＧＰＳなどから得られる情報を用いてジャイロセンサに基づく角度推定値を補正するものも開示されている（技術［５］）。図１４は特許文献２で開示されている移動体姿勢角検出装置のブロック構成図である。図において１１は姿勢角推定器、１２は角速度を計測するための精角速度センサ、１３はＧＰＳなどの粗姿勢角検出手段、１４は角速度データメモリ、１５は粗姿勢角データメモリ、１６は精角速度と粗姿勢角それぞれのサンプリング時間を検出するデータサンプリング周期検出器、である。姿勢角推定器１１はメモリ１４、１５に蓄積された精角速度センサ１２および粗姿勢角検出手段１３の出力値とデータサンプリング周期検出器１６の出力値とを用いて姿勢角を推定するのであるが、その際に精角速度センサ１２の出力値を積分計算した値を粗姿勢角推定器１１の出力値とそのデータサンプリング周期の履歴を用いて補正することにより安定した精度で姿勢角を推定するものである。
また、特許文献３や特許文献４には、特許文献２と同様にＧＰＳなどのジャイロセンサ以外の機器を用いて角度推定値を補正する技術が開示されている。

特開２００４−１１７１３９号公報（第９頁、図１） 特開２０００−２９２１７０（第７頁、図１） 特開平８−１４９２２号公報（第１８頁、図２） 特開平１１−２９５０７２（第５頁、図１） Proceedings of the 1996 IEEE International Conference on Robotics and Automation， Minneapolis，Apr. 22-28，1996，pp. 423-428.

従来の移動体の方位角計測装置に関する技術［１］から［５］には、それぞれ以下のような問題があった。

技術［１］については、車輪が滑って空回りすると出力結果に誤差を生じるという問題があった。また、車輪とモータとの間にしばしば存在するバックラッシュが存在すると、速度の正負が切り替わるときに誤差を生じる、といった問題もあった。

また、技術［２］については、ジャイロセンサの出力値に微小なバイアス誤差が含まれていたとき、時間とともに誤差が増大するという問題があった。そこで誤差をほとんど発生しない高精度のジャイロセンサを採用すると、ジャイロセンサが高額なので方位角計測装置を構成するために多額の資金を要するという問題もあった。

また、技術［３］については、閾値Δθ_{ｔｈｒｅｓ}を小さくするとジャイロセンサの出力値Δθ_ｇｙｒｏに含まれる雑音によってΔ_Ｇ−ＯがΔθ_{ｔｈｒｅｓ}を超えることが多くなって誤差が蓄積しやすいという問題があった。一方で閾値Δθ_{ｔｈｒｅｓ}が大き過ぎると車輪の滑りやバックラッシュに伴うΔθ_ｏｄｏに含まれる誤差を補正できなくなるという問題があった。

また、技術［４］については、ジャイロセンサ出力値に微小なバイアス誤差が含まれていると時間とともに仮の姿勢角に含まれる誤差が徐々に増大するため、微小変動に対する不感帯を設けても出力値中の誤差を除去することはできないという問題があった。

また、技術［５］については、オドメトリとジャイロセンサ以外の方位角検出手段を備える必要があるため、装備が大規模になるという問題があった。またＧＰＳの使用にはＧＰＳ衛星からの信号を受信可能であることが前提条件であるため装置の使用環境が限定されるという問題もあった。

本発明はこのような従来の移動体の方位角計測装置が有していた問題を解決しようとするものであり、簡素で安価な構成でなおかつ精度の高い方位角の計測が可能な移動体の方位角計測装置を提供することを目的とする。

本発明は上記目的を達成するために、次のように構成した。
請求項１に記載の発明は、路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、前記走行手段に印加される駆動力に基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ａと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ｂとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ω_Ａを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とするものである。
請求項２に記載の発明は、前記所定の期間は、前記走行手段に印加される駆動力の大きさが所定の閾値ｃ_１よりも大きい期間であることを特徴とするものである。
請求項３に記載の発明は、前記所定の閾値ｃ_１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とするものである。
請求項４に記載の発明は、前記所定の期間は、前記少なくとも１つ以上の走行手段に印加される駆動力について所定の変化が発生したときから、該駆動力計測手段の出力の大きさが所定の閾値ｃ_２を超えるまでの期間であることを特徴とするものである。
請求項５に記載の発明は、前記所定の閾値ｃ_２は、前記走行手段に印加される駆動力に含まれる定常ノイズの大きさよりも大きい値であることを特徴とするものである。
請求項６に記載の発明は、前記所定の変化は、前記駆動力の正負の変化、または前記駆動力が発生していない状態から発生する状態への変化若しくは発生している状態から発生しない状態への変化であることを特徴とするものである。
請求項７に記載の発明は、路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、前記駆動力発生手段回転角度と駆動力発生手段回転速度とに基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ａと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ｂとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ω_Ａを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とするものである。
請求項８に記載の発明は、前記所定の期間は、前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、該時点からの駆動力発生手段回転角度の大きさが所定の閾値ｃ_３を超えるまでの期間であることを特徴とするものである。
請求項９に記載の発明は、前記所定の期間は、前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、該時点からの所定の方向への駆動力発生手段回転角度が所定の閾値ｃ_３を超える期間、または０未満となるまでの期間であることを特徴とするものである。
請求項１０に記載の発明は、前記所定の閾値ｃ_３は、前記走行手段と前記駆動力発生手段との間に存在するバックラッシュ分に相当する変位量よりも大きな値であることを特徴とするものである。
請求項１１に記載の発明は、前記所定の変化は、前記駆動力発生手段回転速度の正負の変化、または前記駆動力発生手段回転速度が０である状態から０でない状態への変化若しくは０でない状態から０である状態への変化であることを特徴とするものである。
請求項１２に記載の発明は、前記所定の方向は、前記所定の変化が発生した直後の前記駆動力発生手段回転速度の向き、または前記所定の変化が発生する直前の該駆動力発生手段回転速度の逆向きに等しいことを特徴とするものである。
請求項１３に記載の発明は、前記所定の期間は、前記走行手段に印加される駆動力の大きさが所定の閾値ｃ_１よりも大きい期間を含むことを特徴とするものである。
請求項１４に記載の発明は、前記所定の閾値ｃ_１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とするものである。
請求項１５に記載の発明は、前記回転角速度推定値選択手段は、全ての前記駆動力発生手段回転速度が０である期間中、または、前記駆動力発生手段回転速度が所定の閾値ｃ_０未満である期間中は、前記回転角速度推定値ω_Ｂを前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とするものである。
請求項１６に記載の発明は、前記所定の閾値ｃ_０は、前記駆動力発生手段回転速度に含まれるノイズの大きさよりも大きな値であることを特徴とするものである。
請求項１７に記載の発明は、前記走行手段に印加される駆動力に代えて、前記走行手段に印加される駆動力の指令値とすることを特徴とするものである。

請求項１に記載の発明によれば、方位角推定値計算に用いる回転角速度推定値を選択する際に走行手段に印加される駆動力の値を考慮することができ、精度の高い方位角推定値が簡単な構成で得られるようになる。
請求項２、３に記載の発明によれば、走行手段が滑りを生じる可能性が高い期間中はジャイロセンサに基づく回転角速度推定値を用いて方位角推定値を計算することで、滑りによる誤差の発生を回避できる。また、走行手段が滑りを生じていない期間中はオドメトリによる回転角速度推定値を用いることで、ジャイロセンサのバイアス誤差の蓄積を非常に少なくできる。
請求項４、５、６に記載の発明によれば、走行手段と駆動力発生手段との間に存在するバックラッシュの影響が大きい期間中はジャイロセンサに基づく回転角速度推定値を用いて方位角推定値を計算することで、バックラッシュによる誤差の発生を回避できる。また、バックラッシュの影響を受けない期間中はオドメトリによる回転角速度推定値を用いることで、ジャイロセンサのバイアス誤差の影響を非常に少なくできる。
請求項７に記載の発明によれば、単純な演算によって新たなセンサを用いることなく長期間にわたって安定した方位角推定値が得られるようになる。
請求項８、９、１０、１１、１２に記載の発明によれば、走行手段と駆動力発生手段との間に存在するバックラッシュによる誤差の増大を確実に避けることができる。
請求項１３、１４に記載の発明によれば、走行手段の滑りの影響を受けない長期間にわたって安定した方位角推定値が得られるようになる。
請求項１５、１６に記載の発明によれば、停止時にも安定した方位角推定値が得られるようになる。また、モータ回転速度信号ω_Ｍにノイズが含まれていても移動体の停止状態を確実に検出することができる。
請求項１７に記載の発明によれば、別途駆動力計測手段等を設ける必要がなくなるので、構成がシンプルになり、信頼性が向上する。

以下、本発明の実施の形態について図を参照して説明する。

図１および図２は、それぞれ本発明の方位角計測装置を搭載した移動体の側面図および正面図を示している。
図において１０１は移動体、１０２は方位角計測装置本体、１０３Ｌと１０３Ｒはそれぞれ左右の走行手段であって、本実施例においては、駆動車輪である。１０４Ｌと１０４Ｒはそれぞれ左右の駆動力発生手段であって、本実施例においてはサーボモータである。１０５は減速器であり、１０６は回転角速度検出手段であって、本実施例においてはエンコーダである。１０７はサーボアンプ、１０８はキャスターである。１１１は駆動輪半径ｒで、１１２は移動体中心から駆動輪取り付け位置までの距離ｄである。なお、移動体１０１の鉛直軸は図１、図２ともに紙面上向きである。

サーボアンプ１０７に回転速度指令やトルク指令が送られると、サーボアンプ１０７に内蔵された制御回路によってサーボモータ１０４は指令通りの回転速度や回転トルクを発生する。サーボモータ１０４の回転速度は減速器１０５で減速され、一方で回転トルクは増幅される。そして、トルク（駆動力）が印加されて駆動車輪１０３が回転し、路面に対して推進力を発生することで移動体１０１が走行する。移動体１０１の向きを変えるには、左右のサーボモータ１０４の回転速度に差をつければよい。エンコーダ１０６の出力パルスやサーボモータ１０４の出力トルクはサーボアンプ１０７を通して取得する。

図３は移動体１０１の構成要素の接続関係と方位角計測装置本体１０２の構成を示した図である。図において３０１は移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段であって、本実施例においてはジャイロセンサである。３０２はＡＤ変換器、３０３はパルスカウンタ、３０４はマイクロコンピュータ、３１１はアナログ電圧回転角速度推定値信号で、ＡＤ変換器３０２によってディジタル表現の回転角速度推定値ω_Ａ（３１２）に変換されてマイクロコンピュータ３０４に入力される。３１３はサーボアンプから出力されるエンコーダパルス信号で、パルスカウンタ３０３によってモータ回転量信号３１４に変換されてマイクロコンピュータ３０４に入力される。３１５はアナログ電圧モータトルク出力値で、ＡＤ変換器３０２でディジタル表現のモータトルク出力値（３１６）に変換されてマイクロコンピュータ３０４に入力される。マイクロコンピュータ３０４は入力信号をもとにして方位角度推定値３１７を計算し、出力する。

本実施例が非特許文献１と異なる部分は、移動体の駆動力発生手段で発生する駆動力を用いてジャイロセンサに基づく回転角速度推定値ω_Ａとオドメトリに基づく回転角速度推定値ω_Ｂとを切り替えて方位角推定値計算に用いる点である。

図４はマイクロコンピュータ３０４内部の各処理を示すブロック図である。
図において４０１は微分器であり、モータ回転量信号３１４の変化量をサンプリング周期で割ってモータ回転速度信号ω_Ｍ（４１１）を得る。４０２は運動学演算手段であり、ω_Ｍから運動学関係式を用いて移動体自身の鉛直軸回り回転角速度推定値ω_Ｂ（４１２）を計算する。
具体的には次の計算式を用いる。

ここで、文字Ｇ_ｒは減速器１０５の減速比であって駆動車輪１回転あたりのモータ回転数を表し、ω_ＭＬとω_ＭRはそれぞれサーボモータ１０４Ｌ、１０４Ｒのモータ回転角速度信号、πは円周率である。なお、ω_ＭＬとω_ＭRの正の向きは、モータの出力軸に向かって右回りとしている。

４０５はローパスフィルタ処理であり、読み込んだモータトルク出力値をτ_Ｍ’（３１６）とするとき、フィルタ処理後のモータトルク信号τ_Ｍ（４１４）は次の計算式で求める。

ここで、［ｋ］は第ｋ回目の値であることを表し、［ｋ−１］は第ｋ−１回目の値であることを表している。また文字Δｔはサンプリング時間、Ｔはフィルタの時定数でモータの動作速度の変化よりも十分短い値を設定する。

４０３は回転角速度推定値選択手段であり、ω_Ａ、ω_Ｂ、ω_Ｍ、およびモータトルク出力値を読み取って、ω_Ａとω_Ｂのどちらか一方を回転角速度推定値ω’（４１３）として出力する。４０４は積分器であり、回転角速度推定値ω’の時間積分を方位角度推定値３１７として出力する。

次に、回転角速度推定値選択手段４０３における処理について説明する。
図５は、回転角速度推定値選択手段４０３における処理の流れ図であり、その処理は以下のステップに従う。
まず、ステップＳ１で新しい入力値ω_Ａ、ω_Ｂ、ω_Ｍ、およびτ_Ｍを読み取る。
次に、ステップＳ２では、各サーボモータのω_Ｍの大きさ｜ω_Ｍ｜を閾値ｃ_０と比較し、全てのモータについて｜ω_Ｍ｜がｃ_０を下回っていた場合はステップＳ６に進み、それ以外のときはステップＳ３に進む。閾値ｃ_０の値は０に近い非負の値とする。この処理はω_Ｍにノイズが含まれていても移動体の停止状態を確実に検出するためであり、バイアス誤差を生じないω_Ｂを回転角速度推定値ω’として採用することで停止中の回転角度推定値を安定させるものである。すなわち、閾値ｃ_０は、０に近い非負の値であって、サーボモータの回転速度に含まれるノイズの大きさよりも大きな値を設定する。
ステップＳ３では、比較手段によって得られた全てのモータトルク信号τ_Ｍの大きさ｜τ_Ｍ｜を閾値ｃ_１と比較し、少なくとも１つ以上のモータについて｜τ_Ｍ｜が閾値ｃ_１を上回っていた場合はステップＳ４に進み、それ以外のときはステップＳ５に進む。閾値ｃ_１の大きさは、｜τ_Ｍ｜＝ｃ_１となるときに駆動輪１０３が路面に対して発生する推進力が想定される駆動車輪１０３と路面との動摩擦力を超えないような値とする。駆動車輪と路面との間に滑りが生じる可能性が高い場合を確実に区別するためである。
ステップＳ４では、ω_Ａを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ１に戻る。
ステップＳ５では、モータトルク信号τ_Ｍの前ステップの値を記憶しておき、少なくとも１つ以上のモータについてτ_Ｍが前ステップの値からその正負を反転させたときあるいは前ステップで非零であった値が０となったときまたは０であった値が非零となったときはステップＳ７に、それ以外のときはステップＳ６に進む。
ステップＳ６では、ω_Ｂを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ１に戻る。

ステップＳ７では、ω_Ａを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ８に進む。
ステップＳ８では、新しい入力値を読み取ってステップＳ９に進む。
ステップＳ９では、比較手段によって得られた全てのモータトルク信号τ_Ｍの大きさ｜τ_Ｍ｜を閾値ｃ_２と比較し、少なくとも１つ以上のモータについて｜τ_Ｍ｜が閾値ｃ_２を上回っていた場合はステップＳ１０に進み、それ以外のときはステップＳ７に進む。閾値ｃ_２はτ_Ｍに定常的に含まれる微細なノイズの大きさよりも十分大きな値とする。回転角速度推定値の切り替えがノイズに影響されることを防ぐためである。
ステップＳ１０では、ω_Ｂを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ１に戻る。

以上の処理手順を図６から図８に示した信号波形の例を用いて説明する。なお、本波形例は減速機１０５の減速比Ｇ_ｒが小さく、駆動輪にかかる負荷がモータに与えるトルクが大きい場合を想定している。
図６は１つのモータに関するモータ回転速度ω_Ｍを減速比Ｇ_ｒで割った値ω_Ｍ／Ｇ_ｒを描いた波形の例、図７はそのモータによって駆動される駆動車輪の回転速度ω_Ｗ、図８はモータトルク信号τ_Ｍの波形である。
図において、横軸は時間ｔである。図中６０１はω_Ｍ／Ｇ_ｒの波形、７０１はω_Ｗの波形、８０１はτ_Ｍの波形であり、ｔ_１はτ_Ｍの正負が切り替わった時刻、ｔ_２はτ_Ｍが閾値ｃ_２を上回った時刻、ｔ_３はτ_Ｍが閾値ｃ_１を上回った時刻、ｔ_４はτ_Ｍが閾値ｃ_１を下回った時刻である。またＴ_ａは回転角速度推定値ω’としてω_Ａを出力する期間を、Ｔ_ｂは回転角速度推定値ω’としてω_Ｂを出力する期間を、それぞれ表している。路面と駆動車輪との間の滑りやモータと減速器との間のバックラッシュの影響がなければモータ回転量信号をもとに計算したω_Ｂはノイズが少なくバイアス誤差が生じることもないため、これを回転角速度推定値ω’とする。モータトルク信号τ_Ｍが閾値ｃ_１を超えたときには駆動車輪が路面に対して滑っている可能性が高くω_Ｂの信頼性が低下するため、ノイズの影響が大きくとも滑りの影響を受けないω_Ａを回転角速度推定値ω’として採用する。この判断はステップＳ３で行われる。

モータトルクの正負が切り替わるとき、減速器に存在するバックラッシュによってモータ出力軸と駆動車輪とが機械的に離れる瞬間が存在する。このときモータの回転と駆動車輪の回転とは無関係になるため、ω_Ｂの信頼性は著しく低下する。よってモータトルク信号τ_Ｍの正負が切り替わるか非零の値から０になったときからτ_Ｍの大きさが閾値ｃ_２を超えるまでの期間はバックラッシュの影響を受けないω_Ａを回転角速度推定値ω’として採用する。この判断はステップＳ５およびＳ９で行われる。

以上のような処理手順によって回転角速度推定値選択手段４０３から出力された回転角速度推定値ω’に含まれる駆動車輪の滑りやバックラッシュおよびジャイロセンサのバイアス誤差による影響は非常に小さく抑えられるため、回転角速度推定値ω’を積分して得られる方位角推定値の精度を高めることができる。また閾値は路面と駆動車輪との摩擦力およびトルク信号に含まれるノイズの大きさを調べることによって得られ、細かく調整する必要はない。

なお、本実施例ではモータトルク出力値はサーボアンプから得るものとしたが、代わりにトルクセンサを設けたり、エンコーダ値を２階微分して加速度信号を求めたものを用いたりしてもよい。またサーボアンプをトルク指令によって駆動させるときは、与えたトルク指令をそのままモータトルク出力値として用いることもできる。

続いて回転角速度推定値選択手段４０３の第２の構成例を説明する。図９は本実施例においてマイクロコンピュータ３０４内部で行う処理の概要を示したブロック図である。なお、本実施例はモータトルク信号を使用しない点およびモータ回転量信号３１４を回転角速度推定値選択手段４０３でも使用する点で第１実施例と異なっている。

また、本実施例が非特許文献１と異なる部分は、ω_Ａとω_Ｂとの切り替えはモータ回転量信号に基づく演算結果から決定し、ω_Ａは使用しない点である。

次に、回転角速度推定値選択手段における処理について説明する。
図１０は本実施例の回転角速度推定値選択手段における処理手順を示した流れ図であり、その処理は以下のステップに従う。
まず、ステップＳ２１では、入力値としてモータ回転量信号３１４とω_Ａとω_Ｂの計算結果を新たに読み込む。
ステップＳ２２では、各モータの回転速度を前のステップと比較し、１つ以上のモータの動作方向が切り替わったあるいは速度０の状態から動作を開始したときにはステップＳ２４に、それ以外のときはステップＳ２３に進む。
ステップＳ２３では、ω_Ｂを回転角速度推定値ω’として出力した後に該ステップの処理を終了しステップＳ２１に戻る。
ステップＳ２４では、動作切り替わり直後のモータ回転方向を記憶してステップＳ２５に進む。動作切り替わり直後のモータ回転速度が０である場合は、切り替わり直前のモータ回転方向の逆向きを記憶してステップＳ２５に進む。
ステップＳ２５では、モータ回転角度θ_Ｍの値を０に初期化してからステップＳ２６に進む。

ステップＳ２６では、後述するステップＳ３１で得られたモータ回転量信号３１４をもとに、ステップＳ２４で記憶した方向を正の角度方向としてモータ回転角度θ_Ｍを再構成し、ステップＳ２７に進む。
ステップＳ２７では、モータ回転角度θ_Ｍを閾値ｃ_３と比較し、θ_Ｍがｃ_３より大きい場合は後述するステップＳ３０に進み、それ以外のときはステップＳ２８に進む。なお、閾値ｃ_３はバックラッシュによって駆動車輪が自由回転する角度幅に減速比１／Ｇ_ｒをかけたものよりも大きな値とする。
ステップＳ２８では、θ_Ｍの正負を調べ、θ_Ｍが０未満である場合はステップＳ３０に進み、それ以外のときはステップＳ２９に進む。
ステップＳ２９では、ω_Ａを回転角速度推定値ω’として出力し該ステップの処理を終了してステップＳ３１に進む。
ステップＳ３１では、入力値としてモータ回転量信号３１４と、ω_Ａと、ω_Ｂの計算結果と、を新たに読み込み、ステップＳ２６に進む。
ステップＳ３０では、ω_Ｂを回転角速度推定値ω’として出力し該ステップの処理を終了してステップＳ２１に戻る。

以上の処理手順を図１１に示した信号波形の例を用いて説明する。なお、本波形例は減速機１０５の減速比Ｇ_ｒが大きく、駆動輪にかかる負荷がモータに与える影響が小さい場合を想定している。
図１１において点線６１１はω_Ｍ／Ｇ_ｒの波形、実線７１１はω_Ｗの波形であり、ｔ_１はω_Ｍ／Ｇ_ｒの正負が切り替わった時刻、ｔ_２はθ_Ｍが閾値ｃ_３を上回った時刻、ｔ_３はω_Ｍ／Ｇ_ｒの正負が切り替わった時刻、ｔ_４はθ_Ｍが０を下回った時刻である。また面積６１２はｔ_１からｔ_２までのモータ回転角度で閾値ｃ_３を減速比Ｇ_ｒで割った値ｃ_３／Ｇ_ｒに等しく、面積６１３はｔ_３からｔ_４までのモータ回転角度で閾値ｃ_３／Ｇ_ｒよりも小さな値である。

路面と駆動車輪との間の滑りやモータと減速器との間のバックラッシュの影響がなければモータ回転量信号をもとに計算したω_Ｂはノイズが少なくバイアス誤差が生じることもないため、これを回転角速度推定値ω’とする。モータの動作方向が切り替わったときには、減速器に存在するバックラッシュによってモータ出力軸と駆動車輪とが機械的に離れた状態になる。このときモータの回転と駆動車輪の回転とは無関係になるため、ω_Ｂの信頼性は著しく低下する。よってモータの動作方向が切り替わったあるいは速度０の状態から動作を開始したときからθ_Ｍの大きさが閾値ｃ_３を超えるか０より小さくなるまでの期間は、バックラッシュの影響を受けないω_Ａを回転角速度推定値ω’として採用する。この判断はステップＳ２２、Ｓ２７およびＳ２８で行われる。

以上のような処理手順によって回転角速度推定値選択手段４０３から出力された回転角速度推定値ω’に含まれる駆動車輪とモータとの間に存在するバックラッシュおよびジャイロセンサのバイアス誤差による影響は非常に小さく抑えられるため、回転角速度推定値ω’を積分して得られる方位角推定値の精度を高めることができる。また閾値ｃ_３は駆動車輪のバックラッシュによる自由回転角度幅と減速比から簡単に決めることができ、細かく調整する必要はない。

本実施例ではバックラッシュの影響を受ける領域だけに限定してステップＳ２９に進むために、ステップＳ２４からステップＳ２８までの一連の処理を行う構成とした。より簡素な構成とするためにはステップＳ２４を省略し、ステップＳ２７とＳ２８の処理を次のように置き換えても良い。すなわち、論理式
｜θ_Ｍ｜＞ｃ_３ 式（３）
が成立するときはステップＳ３０に、それ以外のときはステップＳ２９に進む。この処理はバックラッシュの影響を受けない領域でもステップＳ２９が実行される場合があるため、ジャイロセンサのバイアス誤差の影響がやや大きくなる。しかし、バックラッシュの影響は確実に排除でき、また回転速度方向を記憶する必要がないため処理が簡便である。

以上より、本発明は、特許文献１に開示されている姿勢角検出装置とは、ジャイロセンサから得られる回転角速度推定値ω_Ａとオドメトリに基づく回転角速度推定値ω_Ｂとを積分する前に回転角速度推定値選択手段によって信頼性の高い回転角速度推定値ω’を構成する点が異なっており、ω’がジャイロセンサのバイアス誤差から受ける影響は少ないので、積分による誤差の蓄積を抑えることができる。
また、ジャイロは必ずしも高精度で高価なものである必要はないため、装置を安価に構成できる。

また、本発明は、特許文献２に開示されている姿勢角検出装置とは、ジャイロセンサの補正に必ずしもＧＰＳを必要としない点で異なっており、ＧＰＳ受信装置を組み込む必要がないため、装置を簡素かつ安価にできるうえ、ＧＰＳを受信できない環境で作業をする移動体にも適用可能である。

なお、上記実施例においては、移動体は駆動車輪を備えているが、これに限らず、無限軌道その他の任意の走行手段であってもよく、その場合でも容易に本発明を適用することができる。

平面上を移動する移動体の方位角計測装置として広く適用でき、建物内部で用いるナビゲーション機能付きカートなどに用いることができる。

本発明の方位角計測装置を搭載した移動体の側面図 本発明の方位角計測装置を搭載した移動体の正面図 本発明の方位角計測装置を搭載した移動体のブロック構成図 マイクロコンピュータで行う実施例１の処理を示したブロック図 第１実施例における回転角速度推定値選択手段での処理手順を示した流れ図 第２実施例におけるモータ回転速度を減速比で割った信号波形 駆動車輪の回転速度波形 モータ出力軸のトルク波形 マイクロコンピュータで行う実施例２の処理を示したブロック図 実施例２の回転角速度推定値選択手段における処理手順を示した流れ図 第２実施例におけるモータの回転速度を減速比で割った信号波形の例 非特許文献１で開示されている移動体の方位角度算出方法を示した流れ図 特許文献１で開示されている姿勢角検出装置のブロック構成図 特許文献２で開示されている移動体姿勢角検出装置のブロック構成図

符号の説明

１０１ 移動体
１０２ 本発明の方位角計測装置本体
１０３Ｌ、１０３Ｒ 駆動車輪
１０４Ｌ、１０４Ｒ サーボモータ
１０５ 減速器
１０６ エンコーダ
１０７ サーボアンプ
１０８ キャスター
１１１ 駆動車輪半径
１１２ 移動体１０１の中心から駆動車輪１０３までの距離
３０１ ジャイロセンサ
３０２ ＡＤ変換器
３０３ パルスカウンタ
３０４ マイクロコンピュータ
３１１ アナログ電圧回転角速度推定値信号
３１２ 回転角速度推定値ω_Ａ
３１３ エンコーダパルス信号
３１４ モータ回転量信号
３１５ アナログ電圧モータトルク出力値
３１６ モータトルク出力値
３１７ モータトルク信号τ_Ｍ
３１８ 方位角度推定値
４０１ 微分器
４０２ 運動学演算手段
４０３ 回転角速度推定値選択手段
４０４ 積分器
４１１ モータ回転速度信号
４１２ 回転角速度推定値ω_Ｂ
４１３ 回転角速度推定値ω’
６０１、６１１ モータ回転速度ω_Ｍを減速比Ｇ_ｒで割った値ω_Ｍ／Ｇ_ｒの信号波形
６１２ 時間区間［ｔ_１、ｔ_２］における信号波形６１１の時間積分
６１３ 時間区間［ｔ_３、ｔ_４］における信号波形６１１の時間積分
７０１、７１１ 駆動車輪回転速度ω_Ｗの波形
８０１ モータトルク信号τ_Ｍの波形
２６ａ 運動角演算装置
２６ｂ 静止角演算装置
２６ｃ 姿勢角演算装置
２８ 不感帯処理装置
１１ 姿勢角推定器
１２ 精角速度センサ
１３ 粗姿勢角検出手段
１４ 角速度データメモリ
１５ 粗姿勢角データメモリ
１６ データサンプリング周期検出器

Claims (17)

1. 路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、
   前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、
   前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、
   前記走行手段に印加される駆動力に基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ａと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ｂとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、
   前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ω_Ａを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とする移動体。
2. 前記所定の期間は、
   前記走行手段に印加される駆動力の大きさが所定の閾値ｃ_１よりも大きい期間であることを特徴とする請求項１記載の移動体。
3. 前記所定の閾値ｃ_１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とする請求項２記載の移動体。
4. 前記所定の期間は、
   前記少なくとも１つ以上の走行手段に印加される駆動力について所定の変化が発生したときから、該駆動力計測手段の出力の大きさが所定の閾値ｃ_２を超えるまでの期間であることを特徴とする請求項１または２いずれかに記載の移動体。
5. 前記所定の閾値ｃ_２は、前記走行手段に印加される駆動力に含まれる定常ノイズの大きさよりも大きい値であることを特徴とする請求項４記載の移動体。
6. 前記所定の変化は、
   前記駆動力の正負の変化、または前記駆動力が発生していない状態から発生する状態への変化若しくは発生している状態から発生しない状態への変化であることを特徴とする請求項４記載の移動体。
7. 路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、
   前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、
   前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、
   前記駆動力発生手段回転角度と駆動力発生手段回転速度とに基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ａと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ω_Ｂとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、
   前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ω_Ａを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とする移動体。
8. 前記所定の期間は、
   前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、
   該時点からの駆動力発生手段回転角度の大きさが所定の閾値ｃ_３を超えるまでの期間であることを特徴とする請求項７記載の移動体。
9. 前記所定の期間は、
   前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、
   該時点からの所定の方向への駆動力発生手段回転角度が所定の閾値ｃ_３を超える期間、または０未満となるまでの期間であることを特徴とする請求項７記載の移動体。
10. 前記所定の閾値ｃ_３は、
    前記走行手段と前記駆動力発生手段との間に存在するバックラッシュ分に相当する変位量よりも大きな値であることを特徴とする請求項８または９いずれかに記載の移動体。
11. 前記所定の変化は、
    前記駆動力発生手段回転速度の正負の変化、または前記駆動力発生手段回転速度が０である状態から０でない状態への変化若しくは０でない状態から０である状態への変化であることを特徴とする請求項８または９いずれかに記載の移動体。
12. 前記所定の方向は、
    前記所定の変化が発生した直後の前記駆動力発生手段回転速度の向き、または前記所定の変化が発生する直前の該駆動力発生手段回転速度の逆向きに等しいことを特徴とする請求項９または１０いずれかに記載の移動体。
13. 前記所定の期間は、前記走行手段に印加される駆動力の大きさが所定の閾値ｃ_１よりも大きい期間を含むことを特徴とする請求項７乃至１２いずれかに記載の移動体。
14. 前記所定の閾値ｃ_１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とする請求項１３記載の移動体。
15. 前記回転角速度推定値選択手段は、
    全ての前記駆動力発生手段回転速度が０である期間中、または、前記駆動力発生手段回転速度が所定の閾値ｃ_０未満である期間中は、
    前記回転角速度推定値ω_Ｂを前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とする請求項１または７いずれかに記載の移動体。
16. 前記所定の閾値ｃ_０は、
    前記駆動力発生手段回転速度に含まれるノイズの大きさよりも大きな値であることを特徴とする請求項１５記載の移動体。
17. 前記走行手段に印加される駆動力に代えて、
    前記走行手段に印加される駆動力の指令値とすることを特徴とする請求項１または７いずれかに記載の移動体。