(57)【要約】【課題】簡素で安価な構成でなおかつ精度の高い方位角計測装置を備えた移動体を提供する。【解決手段】駆動車輪の回転速度を計測する駆動輪回転速度計測手段と、前記回転角速度推定値ωＡ３１２を出力できるジャイロセンサと、前記駆動輪回転速度からオドメトリによって前記回転角速度推定値ωＢを計算する運動学演算手段と、前記回転角速度推定値ωＡ３１２と前記回転角速度推定値ωＢ４１２とを逐次選択する回転角速度推定値選択手段と、前記回転角速度推定値選択手段４０３の出力を積分する積分器４０４と、からなる移動体の方位角計測装置を構成し、前記回転角速度推定値選択手段４０３が常に信頼性の高い回転角速度推定値を出力するようにした。【選択図】図４
2006349399.tif 000002
【特許請求の範囲】
【請求項１】
路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、
前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、
前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、
前記走行手段に印加される駆動力に基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ωＡと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ωＢとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、
前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ωＡを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とする移動体。
【請求項２】
前記所定の期間は、
前記走行手段に印加される駆動力の大きさが所定の閾値ｃ１よりも大きい期間であることを特徴とする請求項１記載の移動体。
【請求項３】
前記所定の閾値ｃ１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とする請求項２記載の移動体。
【請求項４】
前記所定の期間は、
前記少なくとも１つ以上の走行手段に印加される駆動力について所定の変化が発生したときから、該駆動力計測手段の出力の大きさが所定の閾値ｃ２を超えるまでの期間であることを特徴とする請求項１または２いずれかに記載の移動体。
【請求項５】
前記所定の閾値ｃ２は、前記走行手段に印加される駆動力に含まれる定常ノイズの大きさよりも大きい値であることを特徴とする請求項４記載の移動体。
【請求項６】
前記所定の変化は、
前記駆動力の正負の変化、または前記駆動力が発生していない状態から発生する状態への変化若しくは発生している状態から発生しない状態への変化であることを特徴とする請求項４記載の移動体。
【請求項７】
路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、
前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、
前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、
前記駆動力発生手段回転角度と駆動力発生手段回転速度とに基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ωＡと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ωＢとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、
前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ωＡを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とする移動体。
【請求項８】
前記所定の期間は、
前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、
該時点からの駆動力発生手段回転角度の大きさが所定の閾値ｃ３を超えるまでの期間であることを特徴とする請求項７記載の移動体。
【請求項９】
前記所定の期間は、
前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、
該時点からの所定の方向への駆動力発生手段回転角度が所定の閾値ｃ３を超える期間、または０未満となるまでの期間であることを特徴とする請求項７記載の移動体。
【請求項１０】
前記所定の閾値ｃ３は、
前記走行手段と前記駆動力発生手段との間に存在するバックラッシュ分に相当する変位量よりも大きな値であることを特徴とする請求項８または９いずれかに記載の移動体。
【請求項１１】
前記所定の変化は、
前記駆動力発生手段回転速度の正負の変化、または前記駆動力発生手段回転速度が０である状態から０でない状態への変化若しくは０でない状態から０である状態への変化であることを特徴とする請求項８または９いずれかに記載の移動体。
【請求項１２】
前記所定の方向は、
前記所定の変化が発生した直後の前記駆動力発生手段回転速度の向き、または前記所定の変化が発生する直前の該駆動力発生手段回転速度の逆向きに等しいことを特徴とする請求項９または１０いずれかに記載の移動体。
【請求項１３】
前記所定の期間は、前記走行手段に印加される駆動力の大きさが所定の閾値ｃ１よりも大きい期間を含むことを特徴とする請求項７乃至１２いずれかに記載の移動体。
【請求項１４】
前記所定の閾値ｃ１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とする請求項１３記載の移動体。
【請求項１５】
前記回転角速度推定値選択手段は、
全ての前記駆動力発生手段回転速度が０である期間中、または、前記駆動力発生手段回転速度が所定の閾値ｃ０未満である期間中は、
前記回転角速度推定値ωＢを前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とする請求項１または７いずれかに記載の移動体。
【請求項１６】
前記所定の閾値ｃ０は、
前記駆動力発生手段回転速度に含まれるノイズの大きさよりも大きな値であることを特徴とする請求項１５記載の移動体。
【請求項１７】
前記走行手段に印加される駆動力に代えて、
前記走行手段に印加される駆動力の指令値とすることを特徴とする請求項１または７いずれかに記載の移動体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、路面上を走行する移動体に関し、特にその方位角計測装置に関する。
【背景技術】
【０００２】
従来、平面上を走行する移動体の方位角計測装置には、車輪を駆動するモータの回転速度から運動学関係式を用いて移動体の回転速度を求めて積分するもの（オドメトリ、技術［１］）、ジャイロセンサで移動体の回転角速度を計測して積分するもの（技術［２］）、があった。
【０００３】
また、上記技術［１］と［２］でそれぞれ求めた移動体回転角速度の差の大きさを監視し、差の大きさが一定の閾値内であれば技術［１］で求めた回転角速度を、閾値を超えたときは技術［２］で求めた回転角速度を採用して積分するというものもあった（技術［３］、非特許文献１）。
【０００４】
ここで、図１２は非特許文献１で開示されている移動体の方位角度算出方法を示す流れ図である。図においてΔθｇｙｒｏはジャイロセンサから得られる移動体の回転角速度推定値、Δθｏｄｏは駆動輪回転速度からオドメトリによって算出される移動体の回転角速度推定値、ΔＧ−Ｏは２つの回転角速度推定値の差、Δθｔｈｒｅｓは適当に決めた閾値、θは方位角推定値、Ｔはサンプリング時間であって、右下の添え字ｉは第ｉステップ目を表す。路面や車輪の条件がよければ、Δθｏｄｏは正確に得られるが、段差乗り越えなどで車輪が一時空転したりすると正しい値から大きく乖離する。ジャイロセンサはノイズや出力値のドリフト現象の影響を受けるためΔθｇｙｒｏの定量的な信頼性はオドメトリに比べると低いが、駆動輪の状態にかかわらず移動体の動作を反映する。この性質を利用して、ΔＧ−Ｏの大きさがある値Δθｔｈｒｅｓを超えたときはΔθｇｙｒｏを、それ以外のときはΔθｏｄｏを計算に用いることで、より正確な方位角推定値を得ることができる。
【０００５】
また、特許文献１では、技術［２］で求めた姿勢角の変化に対して不感帯を設け、特に移動体静止時の出力値の振動を抑制したものが開示されている（技術［４］）。図１３は特許文献１で開示されている姿勢角検出装置の全体構成を示した図である。図において２６ａは検出対象が単位時間に運動した角度である運動角を計算する運動角演算装置、２６ｂは仮の姿勢角度である静止角を計算する静止角演算装置、２６ｃは２６ａと２６ｂで得られた運動角と静止角から仮の姿勢角を計算する姿勢角演算装置、２８は２６ｃで得られた仮の姿勢角の微小な出力変動を抑制して出力する不感帯処理装置である。不感帯幅として設定されたある一定幅以上の動きにのみ出力値を反応させることでノイズを抑制し、静止時の安定性に優れた姿勢角検出装置が得られる。
【０００６】
また、特許文献２では、ＧＰＳなどから得られる情報を用いてジャイロセンサに基づく角度推定値を補正するものも開示されている（技術［５］）。図１４は特許文献２で開示されている移動体姿勢角検出装置のブロック構成図である。図において１１は姿勢角推定器、１２は角速度を計測するための精角速度センサ、１３はＧＰＳなどの粗姿勢角検出手段、１４は角速度データメモリ、１５は粗姿勢角データメモリ、１６は精角速度と粗姿勢角それぞれのサンプリング時間を検出するデータサンプリング周期検出器、である。姿勢角推定器１１はメモリ１４、１５に蓄積された精角速度センサ１２および粗姿勢角検出手段１３の出力値とデータサンプリング周期検出器１６の出力値とを用いて姿勢角を推定するのであるが、その際に精角速度センサ１２の出力値を積分計算した値を粗姿勢角推定器１１の出力値とそのデータサンプリング周期の履歴を用いて補正することにより安定した精度で姿勢角を推定するものである。
また、特許文献３や特許文献４には、特許文献２と同様にＧＰＳなどのジャイロセンサ以外の機器を用いて角度推定値を補正する技術が開示されている。
【０００７】
【特許文献１】特開２００４−１１７１３９号公報（第９頁、図１）
【特許文献２】特開２０００−２９２１７０（第７頁、図１）
【特許文献３】特開平８−１４９２２号公報（第１８頁、図２）
【特許文献４】特開平１１−２９５０７２（第５頁、図１）
【非特許文献１】Proceedings of the 1996 IEEE International Conference on Robotics and Automation， Minneapolis，Apr. 22-28，1996，pp. 423-428.
【発明の開示】
【発明が解決しようとする課題】
【０００８】
従来の移動体の方位角計測装置に関する技術［１］から［５］には、それぞれ以下のような問題があった。
【０００９】
技術［１］については、車輪が滑って空回りすると出力結果に誤差を生じるという問題があった。また、車輪とモータとの間にしばしば存在するバックラッシュが存在すると、速度の正負が切り替わるときに誤差を生じる、といった問題もあった。
【００１０】
また、技術［２］については、ジャイロセンサの出力値に微小なバイアス誤差が含まれていたとき、時間とともに誤差が増大するという問題があった。そこで誤差をほとんど発生しない高精度のジャイロセンサを採用すると、ジャイロセンサが高額なので方位角計測装置を構成するために多額の資金を要するという問題もあった。
【００１１】
また、技術［３］については、閾値Δθｔｈｒｅｓを小さくするとジャイロセンサの出力値Δθｇｙｒｏに含まれる雑音によってΔＧ−ＯがΔθｔｈｒｅｓを超えることが多くなって誤差が蓄積しやすいという問題があった。一方で閾値Δθｔｈｒｅｓが大き過ぎると車輪の滑りやバックラッシュに伴うΔθｏｄｏに含まれる誤差を補正できなくなるという問題があった。
【００１２】
また、技術［４］については、ジャイロセンサ出力値に微小なバイアス誤差が含まれていると時間とともに仮の姿勢角に含まれる誤差が徐々に増大するため、微小変動に対する不感帯を設けても出力値中の誤差を除去することはできないという問題があった。
【００１３】
また、技術［５］については、オドメトリとジャイロセンサ以外の方位角検出手段を備える必要があるため、装備が大規模になるという問題があった。またＧＰＳの使用にはＧＰＳ衛星からの信号を受信可能であることが前提条件であるため装置の使用環境が限定されるという問題もあった。
【００１４】
本発明はこのような従来の移動体の方位角計測装置が有していた問題を解決しようとするものであり、簡素で安価な構成でなおかつ精度の高い方位角の計測が可能な移動体の方位角計測装置を提供することを目的とする。
【課題を解決するための手段】
【００１５】
本発明は上記目的を達成するために、次のように構成した。
請求項１に記載の発明は、路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、前記走行手段に印加される駆動力に基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ωＡと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ωＢとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ωＡを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とするものである。
請求項２に記載の発明は、前記所定の期間は、前記走行手段に印加される駆動力の大きさが所定の閾値ｃ１よりも大きい期間であることを特徴とするものである。
請求項３に記載の発明は、前記所定の閾値ｃ１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とするものである。
請求項４に記載の発明は、前記所定の期間は、前記少なくとも１つ以上の走行手段に印加される駆動力について所定の変化が発生したときから、該駆動力計測手段の出力の大きさが所定の閾値ｃ２を超えるまでの期間であることを特徴とするものである。
請求項５に記載の発明は、前記所定の閾値ｃ２は、前記走行手段に印加される駆動力に含まれる定常ノイズの大きさよりも大きい値であることを特徴とするものである。
請求項６に記載の発明は、前記所定の変化は、前記駆動力の正負の変化、または前記駆動力が発生していない状態から発生する状態への変化若しくは発生している状態から発生しない状態への変化であることを特徴とするものである。
請求項７に記載の発明は、路面を走行するための少なくとも１つ以上の走行手段と、前記走行手段を駆動する少なくとも１つ以上の駆動力発生手段と、を備えた移動体において、前記移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段と、前記走行手段回転速度に基づいて前記移動体自身の鉛直軸回り回転角速度推定値を計算する運動学演算手段と、前記駆動力発生手段回転角度と駆動力発生手段回転速度とに基づいて、前記回転角速度検出手段が検出した前記移動体自身の鉛直軸回り回転角速度推定値ωＡと前記運動学演算手段が計算した前記移動体自身の鉛直軸回り回転角速度推定値ωＢとのうちいずれかを、前記移動体自身の鉛直軸回り回転角速度推定値として選択する回転角速度推定値選択手段と、を備え、前記回転角速度推定値選択手段は、所定の期間中は、前記回転角速度推定値ωＡを、前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とするものである。
請求項８に記載の発明は、前記所定の期間は、前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、該時点からの駆動力発生手段回転角度の大きさが所定の閾値ｃ３を超えるまでの期間であることを特徴とするものである。
請求項９に記載の発明は、前記所定の期間は、前記少なくとも１つ以上の駆動力発生手段回転速度について所定の変化が発生した時点から、該時点からの所定の方向への駆動力発生手段回転角度が所定の閾値ｃ３を超える期間、または０未満となるまでの期間であることを特徴とするものである。
請求項１０に記載の発明は、前記所定の閾値ｃ３は、前記走行手段と前記駆動力発生手段との間に存在するバックラッシュ分に相当する変位量よりも大きな値であることを特徴とするものである。
請求項１１に記載の発明は、前記所定の変化は、前記駆動力発生手段回転速度の正負の変化、または前記駆動力発生手段回転速度が０である状態から０でない状態への変化若しくは０でない状態から０である状態への変化であることを特徴とするものである。
請求項１２に記載の発明は、前記所定の方向は、前記所定の変化が発生した直後の前記駆動力発生手段回転速度の向き、または前記所定の変化が発生する直前の該駆動力発生手段回転速度の逆向きに等しいことを特徴とするものである。
請求項１３に記載の発明は、前記所定の期間は、前記走行手段に印加される駆動力の大きさが所定の閾値ｃ１よりも大きい期間を含むことを特徴とするものである。
請求項１４に記載の発明は、前記所定の閾値ｃ１は、前記走行手段と前記路面との間の動摩擦力に拮抗し得る駆動力の大きさよりも小さい値であることを特徴とするものである。
請求項１５に記載の発明は、前記回転角速度推定値選択手段は、全ての前記駆動力発生手段回転速度が０である期間中、または、前記駆動力発生手段回転速度が所定の閾値ｃ０未満である期間中は、前記回転角速度推定値ωＢを前記移動体自身の鉛直軸回り回転角速度推定値として選択することを特徴とするものである。
請求項１６に記載の発明は、前記所定の閾値ｃ０は、前記駆動力発生手段回転速度に含まれるノイズの大きさよりも大きな値であることを特徴とするものである。
請求項１７に記載の発明は、前記走行手段に印加される駆動力に代えて、前記走行手段に印加される駆動力の指令値とすることを特徴とするものである。
【発明の効果】
【００１６】
請求項１に記載の発明によれば、方位角推定値計算に用いる回転角速度推定値を選択する際に走行手段に印加される駆動力の値を考慮することができ、精度の高い方位角推定値が簡単な構成で得られるようになる。
請求項２、３に記載の発明によれば、走行手段が滑りを生じる可能性が高い期間中はジャイロセンサに基づく回転角速度推定値を用いて方位角推定値を計算することで、滑りによる誤差の発生を回避できる。また、走行手段が滑りを生じていない期間中はオドメトリによる回転角速度推定値を用いることで、ジャイロセンサのバイアス誤差の蓄積を非常に少なくできる。
請求項４、５、６に記載の発明によれば、走行手段と駆動力発生手段との間に存在するバックラッシュの影響が大きい期間中はジャイロセンサに基づく回転角速度推定値を用いて方位角推定値を計算することで、バックラッシュによる誤差の発生を回避できる。また、バックラッシュの影響を受けない期間中はオドメトリによる回転角速度推定値を用いることで、ジャイロセンサのバイアス誤差の影響を非常に少なくできる。
請求項７に記載の発明によれば、単純な演算によって新たなセンサを用いることなく長期間にわたって安定した方位角推定値が得られるようになる。
請求項８、９、１０、１１、１２に記載の発明によれば、走行手段と駆動力発生手段との間に存在するバックラッシュによる誤差の増大を確実に避けることができる。
請求項１３、１４に記載の発明によれば、走行手段の滑りの影響を受けない長期間にわたって安定した方位角推定値が得られるようになる。
請求項１５、１６に記載の発明によれば、停止時にも安定した方位角推定値が得られるようになる。また、モータ回転速度信号ωＭにノイズが含まれていても移動体の停止状態を確実に検出することができる。
請求項１７に記載の発明によれば、別途駆動力計測手段等を設ける必要がなくなるので、構成がシンプルになり、信頼性が向上する。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００１８】
図１および図２は、それぞれ本発明の方位角計測装置を搭載した移動体の側面図および正面図を示している。
図において１０１は移動体、１０２は方位角計測装置本体、１０３Ｌと１０３Ｒはそれぞれ左右の走行手段であって、本実施例においては、駆動車輪である。１０４Ｌと１０４Ｒはそれぞれ左右の駆動力発生手段であって、本実施例においてはサーボモータである。１０５は減速器であり、１０６は回転角速度検出手段であって、本実施例においてはエンコーダである。１０７はサーボアンプ、１０８はキャスターである。１１１は駆動輪半径ｒで、１１２は移動体中心から駆動輪取り付け位置までの距離ｄである。なお、移動体１０１の鉛直軸は図１、図２ともに紙面上向きである。
【００１９】
サーボアンプ１０７に回転速度指令やトルク指令が送られると、サーボアンプ１０７に内蔵された制御回路によってサーボモータ１０４は指令通りの回転速度や回転トルクを発生する。サーボモータ１０４の回転速度は減速器１０５で減速され、一方で回転トルクは増幅される。そして、トルク（駆動力）が印加されて駆動車輪１０３が回転し、路面に対して推進力を発生することで移動体１０１が走行する。移動体１０１の向きを変えるには、左右のサーボモータ１０４の回転速度に差をつければよい。エンコーダ１０６の出力パルスやサーボモータ１０４の出力トルクはサーボアンプ１０７を通して取得する。
【００２０】
図３は移動体１０１の構成要素の接続関係と方位角計測装置本体１０２の構成を示した図である。図において３０１は移動体自身の鉛直軸回り回転角速度を検出する回転角速度検出手段であって、本実施例においてはジャイロセンサである。３０２はＡＤ変換器、３０３はパルスカウンタ、３０４はマイクロコンピュータ、３１１はアナログ電圧回転角速度推定値信号で、ＡＤ変換器３０２によってディジタル表現の回転角速度推定値ωＡ（３１２）に変換されてマイクロコンピュータ３０４に入力される。３１３はサーボアンプから出力されるエンコーダパルス信号で、パルスカウンタ３０３によってモータ回転量信号３１４に変換されてマイクロコンピュータ３０４に入力される。３１５はアナログ電圧モータトルク出力値で、ＡＤ変換器３０２でディジタル表現のモータトルク出力値（３１６）に変換されてマイクロコンピュータ３０４に入力される。マイクロコンピュータ３０４は入力信号をもとにして方位角度推定値３１７を計算し、出力する。
【００２１】
本実施例が非特許文献１と異なる部分は、移動体の駆動力発生手段で発生する駆動力を用いてジャイロセンサに基づく回転角速度推定値ωＡとオドメトリに基づく回転角速度推定値ωＢとを切り替えて方位角推定値計算に用いる点である。
【００２２】
図４はマイクロコンピュータ３０４内部の各処理を示すブロック図である。
図において４０１は微分器であり、モータ回転量信号３１４の変化量をサンプリング周期で割ってモータ回転速度信号ωＭ（４１１）を得る。４０２は運動学演算手段であり、ωＭから運動学関係式を用いて移動体自身の鉛直軸回り回転角速度推定値ωＢ（４１２）を計算する。
具体的には次の計算式を用いる。
【数１】
2006349399.tif 000003
ここで、文字Ｇｒは減速器１０５の減速比であって駆動車輪１回転あたりのモータ回転数を表し、ωＭＬとωＭRはそれぞれサーボモータ１０４Ｌ、１０４Ｒのモータ回転角速度信号、πは円周率である。なお、ωＭＬとωＭRの正の向きは、モータの出力軸に向かって右回りとしている。
【００２３】
４０５はローパスフィルタ処理であり、読み込んだモータトルク出力値をτＭ’（３１６）とするとき、フィルタ処理後のモータトルク信号τＭ（４１４）は次の計算式で求める。
【数２】
2006349399.tif 000004
ここで、［ｋ］は第ｋ回目の値であることを表し、［ｋ−１］は第ｋ−１回目の値であることを表している。また文字Δｔはサンプリング時間、Ｔはフィルタの時定数でモータの動作速度の変化よりも十分短い値を設定する。
【００２４】
４０３は回転角速度推定値選択手段であり、ωＡ、ωＢ、ωＭ、およびモータトルク出力値を読み取って、ωＡとωＢのどちらか一方を回転角速度推定値ω’（４１３）として出力する。４０４は積分器であり、回転角速度推定値ω’の時間積分を方位角度推定値３１７として出力する。
【００２５】
次に、回転角速度推定値選択手段４０３における処理について説明する。
図５は、回転角速度推定値選択手段４０３における処理の流れ図であり、その処理は以下のステップに従う。
まず、ステップＳ１で新しい入力値ωＡ、ωＢ、ωＭ、およびτＭを読み取る。
次に、ステップＳ２では、各サーボモータのωＭの大きさ｜ωＭ｜を閾値ｃ０と比較し、全てのモータについて｜ωＭ｜がｃ０を下回っていた場合はステップＳ６に進み、それ以外のときはステップＳ３に進む。閾値ｃ０の値は０に近い非負の値とする。この処理はωＭにノイズが含まれていても移動体の停止状態を確実に検出するためであり、バイアス誤差を生じないωＢを回転角速度推定値ω’として採用することで停止中の回転角度推定値を安定させるものである。すなわち、閾値ｃ０は、０に近い非負の値であって、サーボモータの回転速度に含まれるノイズの大きさよりも大きな値を設定する。
ステップＳ３では、比較手段によって得られた全てのモータトルク信号τＭの大きさ｜τＭ｜を閾値ｃ１と比較し、少なくとも１つ以上のモータについて｜τＭ｜が閾値ｃ１を上回っていた場合はステップＳ４に進み、それ以外のときはステップＳ５に進む。閾値ｃ１の大きさは、｜τＭ｜＝ｃ１となるときに駆動輪１０３が路面に対して発生する推進力が想定される駆動車輪１０３と路面との動摩擦力を超えないような値とする。駆動車輪と路面との間に滑りが生じる可能性が高い場合を確実に区別するためである。
ステップＳ４では、ωＡを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ１に戻る。
ステップＳ５では、モータトルク信号τＭの前ステップの値を記憶しておき、少なくとも１つ以上のモータについてτＭが前ステップの値からその正負を反転させたときあるいは前ステップで非零であった値が０となったときまたは０であった値が非零となったときはステップＳ７に、それ以外のときはステップＳ６に進む。
ステップＳ６では、ωＢを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ１に戻る。
【００２６】
ステップＳ７では、ωＡを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ８に進む。
ステップＳ８では、新しい入力値を読み取ってステップＳ９に進む。
ステップＳ９では、比較手段によって得られた全てのモータトルク信号τＭの大きさ｜τＭ｜を閾値ｃ２と比較し、少なくとも１つ以上のモータについて｜τＭ｜が閾値ｃ２を上回っていた場合はステップＳ１０に進み、それ以外のときはステップＳ７に進む。閾値ｃ２はτＭに定常的に含まれる微細なノイズの大きさよりも十分大きな値とする。回転角速度推定値の切り替えがノイズに影響されることを防ぐためである。
ステップＳ１０では、ωＢを回転角速度推定値ω’として出力後に該ステップの処理を終了しステップＳ１に戻る。
【００２７】
以上の処理手順を図６から図８に示した信号波形の例を用いて説明する。なお、本波形例は減速機１０５の減速比Ｇｒが小さく、駆動輪にかかる負荷がモータに与えるトルクが大きい場合を想定している。
図６は１つのモータに関するモータ回転速度ωＭを減速比Ｇｒで割った値ωＭ／Ｇｒを描いた波形の例、図７はそのモータによって駆動される駆動車輪の回転速度ωＷ、図８はモータトルク信号τＭの波形である。
図において、横軸は時間ｔである。図中６０１はωＭ／Ｇｒの波形、７０１はωＷの波形、８０１はτＭの波形であり、ｔ１はτＭの正負が切り替わった時刻、ｔ２はτＭが閾値ｃ２を上回った時刻、ｔ３はτＭが閾値ｃ１を上回った時刻、ｔ４はτＭが閾値ｃ１を下回った時刻である。またＴａは回転角速度推定値ω’としてωＡを出力する期間を、Ｔｂは回転角速度推定値ω’としてωＢを出力する期間を、それぞれ表している。路面と駆動車輪との間の滑りやモータと減速器との間のバックラッシュの影響がなければモータ回転量信号をもとに計算したωＢはノイズが少なくバイアス誤差が生じることもないため、これを回転角速度推定値ω’とする。モータトルク信号τＭが閾値ｃ１を超えたときには駆動車輪が路面に対して滑っている可能性が高くωＢの信頼性が低下するため、ノイズの影響が大きくとも滑りの影響を受けないωＡを回転角速度推定値ω’として採用する。この判断はステップＳ３で行われる。
【００２８】
モータトルクの正負が切り替わるとき、減速器に存在するバックラッシュによってモータ出力軸と駆動車輪とが機械的に離れる瞬間が存在する。このときモータの回転と駆動車輪の回転とは無関係になるため、ωＢの信頼性は著しく低下する。よってモータトルク信号τＭの正負が切り替わるか非零の値から０になったときからτＭの大きさが閾値ｃ２を超えるまでの期間はバックラッシュの影響を受けないωＡを回転角速度推定値ω’として採用する。この判断はステップＳ５およびＳ９で行われる。
【００２９】
以上のような処理手順によって回転角速度推定値選択手段４０３から出力された回転角速度推定値ω’に含まれる駆動車輪の滑りやバックラッシュおよびジャイロセンサのバイアス誤差による影響は非常に小さく抑えられるため、回転角速度推定値ω’を積分して得られる方位角推定値の精度を高めることができる。また閾値は路面と駆動車輪との摩擦力およびトルク信号に含まれるノイズの大きさを調べることによって得られ、細かく調整する必要はない。
【００３０】
なお、本実施例ではモータトルク出力値はサーボアンプから得るものとしたが、代わりにトルクセンサを設けたり、エンコーダ値を２階微分して加速度信号を求めたものを用いたりしてもよい。またサーボアンプをトルク指令によって駆動させるときは、与えたトルク指令をそのままモータトルク出力値として用いることもできる。
【実施例２】
【００３１】
続いて回転角速度推定値選択手段４０３の第２の構成例を説明する。図９は本実施例においてマイクロコンピュータ３０４内部で行う処理の概要を示したブロック図である。なお、本実施例はモータトルク信号を使用しない点およびモータ回転量信号３１４を回転角速度推定値選択手段４０３でも使用する点で第１実施例と異なっている。
【００３２】
また、本実施例が非特許文献１と異なる部分は、ωＡとωＢとの切り替えはモータ回転量信号に基づく演算結果から決定し、ωＡは使用しない点である。
【００３３】
次に、回転角速度推定値選択手段における処理について説明する。
図１０は本実施例の回転角速度推定値選択手段における処理手順を示した流れ図であり、その処理は以下のステップに従う。
まず、ステップＳ２１では、入力値としてモータ回転量信号３１４とωＡとωＢの計算結果を新たに読み込む。
ステップＳ２２では、各モータの回転速度を前のステップと比較し、１つ以上のモータの動作方向が切り替わったあるいは速度０の状態から動作を開始したときにはステップＳ２４に、それ以外のときはステップＳ２３に進む。
ステップＳ２３では、ωＢを回転角速度推定値ω’として出力した後に該ステップの処理を終了しステップＳ２１に戻る。
ステップＳ２４では、動作切り替わり直後のモータ回転方向を記憶してステップＳ２５に進む。動作切り替わり直後のモータ回転速度が０である場合は、切り替わり直前のモータ回転方向の逆向きを記憶してステップＳ２５に進む。
ステップＳ２５では、モータ回転角度θＭの値を０に初期化してからステップＳ２６に進む。
【００３４】
ステップＳ２６では、後述するステップＳ３１で得られたモータ回転量信号３１４をもとに、ステップＳ２４で記憶した方向を正の角度方向としてモータ回転角度θＭを再構成し、ステップＳ２７に進む。
ステップＳ２７では、モータ回転角度θＭを閾値ｃ３と比較し、θＭがｃ３より大きい場合は後述するステップＳ３０に進み、それ以外のときはステップＳ２８に進む。なお、閾値ｃ３はバックラッシュによって駆動車輪が自由回転する角度幅に減速比１／Ｇｒをかけたものよりも大きな値とする。
ステップＳ２８では、θＭの正負を調べ、θＭが０未満である場合はステップＳ３０に進み、それ以外のときはステップＳ２９に進む。
ステップＳ２９では、ωＡを回転角速度推定値ω’として出力し該ステップの処理を終了してステップＳ３１に進む。
ステップＳ３１では、入力値としてモータ回転量信号３１４と、ωＡと、ωＢの計算結果と、を新たに読み込み、ステップＳ２６に進む。
ステップＳ３０では、ωＢを回転角速度推定値ω’として出力し該ステップの処理を終了してステップＳ２１に戻る。
【００３５】
以上の処理手順を図１１に示した信号波形の例を用いて説明する。なお、本波形例は減速機１０５の減速比Ｇｒが大きく、駆動輪にかかる負荷がモータに与える影響が小さい場合を想定している。
図１１において点線６１１はωＭ／Ｇｒの波形、実線７１１はωＷの波形であり、ｔ１はωＭ／Ｇｒの正負が切り替わった時刻、ｔ２はθＭが閾値ｃ３を上回った時刻、ｔ３はωＭ／Ｇｒの正負が切り替わった時刻、ｔ４はθＭが０を下回った時刻である。また面積６１２はｔ１からｔ２までのモータ回転角度で閾値ｃ３を減速比Ｇｒで割った値ｃ３／Ｇｒに等しく、面積６１３はｔ３からｔ４までのモータ回転角度で閾値ｃ３／Ｇｒよりも小さな値である。
【００３６】
路面と駆動車輪との間の滑りやモータと減速器との間のバックラッシュの影響がなければモータ回転量信号をもとに計算したωＢはノイズが少なくバイアス誤差が生じることもないため、これを回転角速度推定値ω’とする。モータの動作方向が切り替わったときには、減速器に存在するバックラッシュによってモータ出力軸と駆動車輪とが機械的に離れた状態になる。このときモータの回転と駆動車輪の回転とは無関係になるため、ωＢの信頼性は著しく低下する。よってモータの動作方向が切り替わったあるいは速度０の状態から動作を開始したときからθＭの大きさが閾値ｃ３を超えるか０より小さくなるまでの期間は、バックラッシュの影響を受けないωＡを回転角速度推定値ω’として採用する。この判断はステップＳ２２、Ｓ２７およびＳ２８で行われる。
【００３７】
以上のような処理手順によって回転角速度推定値選択手段４０３から出力された回転角速度推定値ω’に含まれる駆動車輪とモータとの間に存在するバックラッシュおよびジャイロセンサのバイアス誤差による影響は非常に小さく抑えられるため、回転角速度推定値ω’を積分して得られる方位角推定値の精度を高めることができる。また閾値ｃ３は駆動車輪のバックラッシュによる自由回転角度幅と減速比から簡単に決めることができ、細かく調整する必要はない。
【００３８】
本実施例ではバックラッシュの影響を受ける領域だけに限定してステップＳ２９に進むために、ステップＳ２４からステップＳ２８までの一連の処理を行う構成とした。より簡素な構成とするためにはステップＳ２４を省略し、ステップＳ２７とＳ２８の処理を次のように置き換えても良い。すなわち、論理式
｜θＭ｜＞ｃ３        式（３）
が成立するときはステップＳ３０に、それ以外のときはステップＳ２９に進む。この処理はバックラッシュの影響を受けない領域でもステップＳ２９が実行される場合があるため、ジャイロセンサのバイアス誤差の影響がやや大きくなる。しかし、バックラッシュの影響は確実に排除でき、また回転速度方向を記憶する必要がないため処理が簡便である。
【００３９】
以上より、本発明は、特許文献１に開示されている姿勢角検出装置とは、ジャイロセンサから得られる回転角速度推定値ωＡとオドメトリに基づく回転角速度推定値ωＢとを積分する前に回転角速度推定値選択手段によって信頼性の高い回転角速度推定値ω’を構成する点が異なっており、ω’がジャイロセンサのバイアス誤差から受ける影響は少ないので、積分による誤差の蓄積を抑えることができる。
また、ジャイロは必ずしも高精度で高価なものである必要はないため、装置を安価に構成できる。
【００４０】
また、本発明は、特許文献２に開示されている姿勢角検出装置とは、ジャイロセンサの補正に必ずしもＧＰＳを必要としない点で異なっており、ＧＰＳ受信装置を組み込む必要がないため、装置を簡素かつ安価にできるうえ、ＧＰＳを受信できない環境で作業をする移動体にも適用可能である。
【００４１】
なお、上記実施例においては、移動体は駆動車輪を備えているが、これに限らず、無限軌道その他の任意の走行手段であってもよく、その場合でも容易に本発明を適用することができる。
【産業上の利用可能性】
【００４２】
平面上を移動する移動体の方位角計測装置として広く適用でき、建物内部で用いるナビゲーション機能付きカートなどに用いることができる。
【図面の簡単な説明】
【００４３】
【図１】本発明の方位角計測装置を搭載した移動体の側面図
【図２】本発明の方位角計測装置を搭載した移動体の正面図
【図３】本発明の方位角計測装置を搭載した移動体のブロック構成図
【図４】マイクロコンピュータで行う実施例１の処理を示したブロック図
【図５】第１実施例における回転角速度推定値選択手段での処理手順を示した流れ図
【図６】第２実施例におけるモータ回転速度を減速比で割った信号波形
【図７】駆動車輪の回転速度波形
【図８】モータ出力軸のトルク波形
【図９】マイクロコンピュータで行う実施例２の処理を示したブロック図
【図１０】実施例２の回転角速度推定値選択手段における処理手順を示した流れ図
【図１１】第２実施例におけるモータの回転速度を減速比で割った信号波形の例
【図１２】非特許文献１で開示されている移動体の方位角度算出方法を示した流れ図
【図１３】特許文献１で開示されている姿勢角検出装置のブロック構成図
【図１４】特許文献２で開示されている移動体姿勢角検出装置のブロック構成図
【符号の説明】
【００４４】
１０１  移動体
１０２  本発明の方位角計測装置本体
１０３Ｌ、１０３Ｒ  駆動車輪
１０４Ｌ、１０４Ｒ  サーボモータ
１０５  減速器
１０６  エンコーダ
１０７  サーボアンプ
１０８  キャスター
１１１  駆動車輪半径
１１２  移動体１０１の中心から駆動車輪１０３までの距離
３０１  ジャイロセンサ
３０２  ＡＤ変換器
３０３  パルスカウンタ
３０４  マイクロコンピュータ
３１１  アナログ電圧回転角速度推定値信号
３１２  回転角速度推定値ωＡ
３１３  エンコーダパルス信号
３１４  モータ回転量信号
３１５  アナログ電圧モータトルク出力値
３１６  モータトルク出力値
３１７  モータトルク信号τＭ
３１８  方位角度推定値
４０１  微分器
４０２  運動学演算手段
４０３  回転角速度推定値選択手段
４０４  積分器
４１１  モータ回転速度信号
４１２  回転角速度推定値ωＢ
４１３  回転角速度推定値ω’
６０１、６１１  モータ回転速度ωＭを減速比Ｇｒで割った値ωＭ／Ｇｒの信号波形
６１２  時間区間［ｔ１、ｔ２］における信号波形６１１の時間積分
６１３  時間区間［ｔ３、ｔ４］における信号波形６１１の時間積分
７０１、７１１  駆動車輪回転速度ωＷの波形
８０１  モータトルク信号τＭの波形
２６ａ  運動角演算装置
２６ｂ  静止角演算装置
２６ｃ  姿勢角演算装置
２８  不感帯処理装置
１１  姿勢角推定器
１２  精角速度センサ
１３  粗姿勢角検出手段
１４  角速度データメモリ
１５  粗姿勢角データメモリ
１６  データサンプリング周期検出器
【図１】
2006349399.tif 000005
【図２】
2006349399.tif 000006
【図３】
2006349399.tif 000007
【図４】
2006349399.tif 000008
【図５】
2006349399.tif 000009
【図６】
2006349399.tif 000010
【図７】
2006349399.tif 000011
【図８】
2006349399.tif 000012
【図９】
2006349399.tif 000013
【図１０】
2006349399.tif 000014
【図１１】
2006349399.tif 000015
【図１２】
2006349399.tif 000016
【図１３】
2006349399.tif 000017
【図１４】
2006349399.tif 000018