移動体のキャリブレーション装置およびこれを搭載した移動体並びにキャリブレーション方法
【課題】簡単な構成の装置で移動体の運動学パラメターを精度よく求めることができ、かつキャリブレーション作業に要する手間と空間を大幅に削減する移動体のキャリブレーション装置と方法および自己キャリブレーション機能を有する移動体を提供する。
【解決手段】モータ回転速度計測部と、移動体の回転速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値とから前記移動体の運動学パラメターを得る演算処理部と、を備え、前記モータ回転速度指令部はキャリブレーションにあたって正負対称の定常波指令を一定期間出力するようにした。
【解決手段】モータ回転速度計測部と、移動体の回転速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値とから前記移動体の運動学パラメターを得る演算処理部と、を備え、前記モータ回転速度指令部はキャリブレーションにあたって正負対称の定常波指令を一定期間出力するようにした。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置およびこれを搭載した移動体並びにキャリブレーション方法に関する。
【背景技術】
【０００２】
移動体の並進位置や回転角度を得るためには、移動体の運動学パラメター（例えば車輪半径、車輪接地点半径など）を用いてモータ回転速度から移動体の並進速度・回転速度を求めて積分する「オドメトリ」という方法が広く用いられている。
【０００３】
オドメトリについて、図６に示した２輪型移動体の例を用いて説明する。図において、６０１が２輪型移動体、６０２が右車輪、６０３が左車輪、６０４が右モータ、６０５が左モータ、６０６が減速器である。
移動体６０１の並進速度ｖをモータ回転速度（ωｍＬ、ωｍＲ）から求める式は式（１）である。
【数１】
移動体６０１の路面に垂直な軸回りの回転速度をモータ回転速度から求める式は式（２）である。
【数２】
式中、Ｇｒは減速器６０６の減速比、ｒＷＲは右車輪６０２の半径である右車輪半径、ｒＷＬは左車輪６０３の半径である左車輪半径、Ｒは車軸中心に対する車輪接地点半径であって、ここではｐｔＬ、ｐｔＲ、ｐｒＬ、およびｐｒＲを運動学パラメターと定義する。
なお、パラメターｐｔＬ、ｐｔＲ、ｐｒＬ、およびｐｒＲの間には、式（３）の関係がある。
【数３】
モータ回転速度（ωｍＬ、ωｍＲ）はモータに内蔵されたエンコーダやタコメータなどのセンサで計測できる。よって上記２つの式を用いて移動体の並進・回転速度を逐次求め、それを積分していくことで移動体の初期位置に対する移動量を求めることができる。これを式で表すと、式（４）のようになる。
【数４】
ここでｔは現在時刻、ｔ０はオドメトリの開始時刻、θ０、ｘ０、ｙ０はそれぞれ移動体初期姿勢、初期ｘ座標、初期ｙ座標を表す。移動体の初期位置を定めれば、後は式（４）の積分計算によって逐次移動体位置・姿勢（ｘ(ｔ) 、ｙ(ｔ) 、θ(ｔ)）を推定できるのである。これがオドメトリと呼ばれる手法である。
【０００４】
オドメトリは、図６のような２輪型移動体に限らず、多くの移動体に適用可能な手法である。ここでは更に、図１０を用いて３輪を有する全方向移動体にオドメトリを実施する場合を説明する。図１０は前記全方向移動体の上視図である。図１０において、１００１が全方向移動体、１００２−１〜１００２−３がそれぞれ第１モータ〜第３モータである。また、１００３がエンコーダ、１００４が減速器、１００５−１〜１００５−３がそれぞれ第１オムニホイル〜第３オムニホイルである。
第１車輪１〜第３車輪は、その回転軸が移動体１００１の中心で互いに交わるように、かつ互いのなす角が１２０°になるように設置する。
【０００５】
全方向移動体１００１の移動体固定座標系における並進および回転の速度をモータ回転速度（ωｍ１、ωｍ２、ωｍ３）から求める式は 、式（５）である。
【数５】
ここで、ｖｘｖ、ｖｙｖはそれぞれ移動体に固定された座標系で表現した移動体１００１の並進速度、ｖθｖは移動体に固定された座標系で表現した移動体１００１の回転速度である。式中Ｇｒ’は減速器１００４の減速比、ｒｗ１、ｒｗ２、ｒｗ３、はそれぞれ車輪１−３の半径である車輪半径１−３、また、Ｒ１、Ｒ２、Ｒ３はそれぞれ車軸中心に対する車輪１−３の車輪接地点半径１−３である。ここではｐ１１、ｐ１２、ｐ１３、ｐ２１、ｐ２２、ｐ２３、ｐ３１、ｐ３２、およびｐ３３を運動学パラメターと定義する。モータ回転速度（ωｍ１、ωｍ２、ωｍ３）はモータに内蔵されたエンコーダやタコメータなどのセンサで計測できる。よって式（５）を用いて移動体の並進・回転速度を逐次求め、それを積分していくことで移動体の初期位置に対する移動量を求めることができる。これを式で表すと、式（６）のようになる。
【数６】
このように３輪を有する全方向移動体であっても、モータ回転速度と移動体の並進および回転速度とを対応付ける関係式を導けば、オドメトリ手法を適用することができる。
【０００６】
従来、運動学パラメターを取得するには、設計値を用いる（従来技術１）か、実際に物差で測定（従来技術２）していた。あるいは、移動体に正方形状の目標軌道を与え、実際の到達位置を測定してオドメトリの結果と比較することにより運動学パラメターを修正する方法（従来技術３、非特許文献１参照）もあった。
【非特許文献１】J.Borenstein and L.Feng、 “Measurement and Correction of Systematic Odometry Errors in Mobile Robots”、 IEEE Trans. Robotics and Automation、 Vol.12、 No.6、 1996
【０００７】
従来技術３における移動体のキャリブレーション方法を以下で説明する。
その手順は以下のとおりである。
１）運動学パラメターとして設計値を設定する。
２）基準壁に対する移動体の位置・姿勢を計測し、計測結果を移動体のオドメトリ初期値に設定する。
３）移動体に一辺が４ｍの正方形経路上を時計回りに走行する指令を与える。正方形の各頂点では逐一移動体を停止させ、時計回りに90度回転させる。滑りを生じないように移動体をゆっくり走らせる。
４）走行後の基準壁に対する位置を計測する。
５）オドメトリの結果と実際の位置計測結果を比較する。
６）ステップ２）から５）を合計5回以上繰り返す。
７）ステップ２）から６）を、走行の向きを時計回りから反時計回りに変更して実施する。
８）以上のステップで得られた計測結果を利用して修正運動学パラメターを求め、設定する（運動学パラメターの修正に関する数式については、非特許文献１を参照されたい。）。
【０００８】
図１１は従来技術３のキャリブレーションにあたって移動体が走行する経路の例を示した図である。図１１において１１０１が移動体、１１０２が基準壁、１１０３が移動体の移動開始点、１１０４が移動完了点、実線１１０５が移動体に与える事前にプログラムされた一辺が４ｍの正方形の経路、 点線１１０６は移動体１１０１が実際に走行した経路である。なお、図１１は移動体を反時計回りに走行させた場合である。
従来技術３では、移動体に同じ経路を時計回りに走行させた場合と反時計回りに走行させた場合とを比較することで、回転に関する運動学パラメターの誤差の影響と、並進に関する運動学パラメターの誤差の影響とを分離している。
また、試行を5回以上行うことで施行毎に生じるノイズの影響を軽減している。
したがって、以上に示したようなキャリブレーションを行うことによって、より真値に近い運動学パラメターを取得することができるのである。
【発明の開示】
【発明が解決しようとする課題】
【０００９】
従来技術１では加工や組立によって生じる運動学パラメターの設計値と真値との相違に対応できないため、オドメトリの結果に誤差を生じる可能性が高いという問題があった。
従来技術２は計測に手間がかかる上、移動体の構造によっては計測そのものが困難であるという問題があった。また、計測は多くの場合目測に頼らざるを得ないため、測定誤差を生じる可能性も高かった。
従来技術３においても、移動体の初期位置や到達位置を実測する必要があり、手間がかかった。また、移動体を走行させるための広い空間を必要とするという問題もあった。
【００１０】
本発明はこのような問題点に鑑みてなされたものであり、簡単な構成の装置で移動体の運動学パラメターを精度よく求めることができ、かつキャリブレーション作業に要する手間と空間を大幅に削減する移動体のキャリブレーション装置およびこれを搭載した移動体並びにそのキャリブレーション方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項２記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項３記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項４記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項５記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と、前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項６記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項７記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項８記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
【００１２】
請求項９記載の発明は、請求項１、５、又は７記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の回転速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とするものである。
請求項１０記載の発明は、請求項２、６、又は８記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の回転加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とするものである。
請求項１１記載の発明は、請求項４、５又は６記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の並進加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする。
請求項１２記載の発明は、請求項３、７又は８記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の並進速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とするものである。
請求項１３記載の発明は、請求項９〜１２のいずれか１項記載の移動体のキャリブレーション装置において、前記モータ回転速度指令部が出力するのは、一定角周波数の正弦波状の指令であることを特徴とするものである。
【００１３】
請求項１４記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記振幅値取得部が、その入力信号の周波数解析を行って前記一定角周波数の振動振幅値を抽出する周波数解析部からなることを特徴とするものである。
請求項１５記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の回転速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
請求項１６記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の回転加速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
請求項１７記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の並進加速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
請求項１８記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の並進速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
【００１４】
請求項１９記載の発明は、請求項９記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２０記載の発明は、請求項１０記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２１記載の発明は、請求項１１記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２２記載の発明は、請求項１２記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２３記載の発明は、請求項９記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の回転速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２４記載の発明は、請求項１０記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の回転加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２５記載の発明は、請求項１１記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の並進加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２６記載の発明は、請求項１２記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の並進速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２７記載の発明は、請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置において、前記モータ回転速度が、前記モータ回転速度指令で代替されることを特徴とするものである。
請求項２８記載の発明は、移動体に係り、請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置を搭載したことを特徴とするものである。
請求項２９記載の発明は、移動体のキャリブレーション方法に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うためのキャリブレーション方法であって、はじめにモータ回転速度とセンサから得られる前記移動体の回転速度とを用いて移動体の回転に関する第１の運動学パラメターを求め、次いで、求まった第1の運動学パラメターと、モータ回転速度と、センサから得られる前記移動体の並進加速度あるいは並進速度と、を用いて移動体の並進に関する第２の運動学パラメターを求めることを特徴とするものである。
【発明の効果】
【００１５】
請求項１記載の発明によると、移動体の寸法や回転角度を測定する手間をかけることなく移動体の回転に関する運動学パラメターを自動的に求めることができる。
請求項２記載の発明によると、移動体の回転速度計測部が手に入らない場合でも同等の効果を有する移動体のキャリブレーション装置を構成できる。
請求項３および４記載の発明によると、移動体の寸法や並進移動量を測定する手間をかけることなく、移動体の並進に関する運動学パラメターを求めることができる。
請求項５〜８記載の発明によると、移動体の寸法や回転移動量および並進移動量を測定する手間をかけることなく、移動体の回転および並進に関する運動学パラメターを自動的に求めることができる。
請求項９〜１２記載の発明によると、移動体の回転速度計測部や回転加速度計測部、並進速度計測部、あるいは並進加速度計測部の出力にバイアス誤差が含まれている場合でも移動体の運動学パラメターを自動的に精度良く求めることができる。また、キャリブレーション時の移動体移動量を少なくできるため、キャリブレーションのために広大な空間を必要としないという利点もある。
請求項１３記載の発明によると、単純な信号発生装置で前記モータ回転速度指令部を構成でき、なおかつセンサの出力値に含まれるバイアス誤差にかかわらず精度良く移動体の運動学パラメターを求めることができる。
【００１６】
請求項１４〜１８記載の発明によると、センサの出力値に含まれるノイズ成分を効果的に除去でき、精度良く移動体の運動学パラメターを求めることができる。
請求項１９〜２２記載の発明によると、単純な演算によって素早く移動体の運動学パラメターを求めることができる。
請求項２３〜２６記載の発明によると、センサの出力値に含まれるノイズ成分の影響を減少させ、精度良く移動体の運動学パラメターを求めることができる。
請求項２７記載の発明によれば、より単純な構成で移動体の運動学パラメターを求めることができる。
請求項２８記載の発明によれば、キャリブレーションにあたって別途装置を用意する必要がなくなり、作業の迅速化が可能となる。
請求項２９記載の発明によると、移動体の回転および並進に関する運動学パラメターを少ない手順で効果的に求めることができる。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００１８】
図１は、図６に示した２輪型移動体のキャリブレーション装置の構成を示した図である。図において、破線１０１内部が移動体のキャリブレーション装置、点線１０２内部が移動体、１０３がマイクロコンピュータ、１０４が移動体の回転速度を計測するジャイロセンサ、１０５が加速度センサ、１０６がＡＤ変換器、１０７がＤＡ変換器、１０８がサーボアンプ、１０９がエンコーダおよびモータ、１１０が減速器および車輪である。本実施例においてキャリブレーションの対象とする移動体は2輪型なので、サーボアンプ１０８、エンコーダおよびモータ１０９、減速器および車輪１１０はそれぞれ２つずつ設置されている。ジャイロセンサ１０４および加速度センサ１０５の出力は、それぞれ移動体の回転速度、前後方向加速度に比例したアナログ電圧であるから、ＡＤ変換器１０６を通してディジタル信号に変換される。変換後の値はバス接続されたマイクロコンピュータ１０３から利用可能となっている。マイクロコンピュータ１０３はモータ回転速度指令を計算し、バス接続されたＤＡ変換器１０７を通してサーボアンプ１０８に出力する。サーボアンプ１０８はモータ１０９を前記モータ回転速度指令に追従するように制御するとともに、エンコーダ信号をマイクロコンピュータ１０３に出力する。モータ１０９の回転は減速器で減速され、車輪１１０を駆動する。その結果移動体１０２は路面に対して移動する。移動によって生じる回転速度はジャイロセンサ１０４で、前後方向加速度は加速度センサ１０５で計測される。なお、本実施例は図６のような平面上を走行する2輪型移動体を対象としたキャリブレーション装置であるので、ジャイロセンサ１０４は走行面に垂直な軸回りの回転速度を計測できるように、加速度センサ１０５は移動体前後方向の加速度を計測できるように、それぞれ設置する。
続いて、マイクロコンピュータ１０３内部で実行される処理を説明する。図２は、マイクロコンピュータ内部で実行されるタスクと信号の流れを示した図である。図２において、破線２０１がマイクロコンピュータ内部であり、２０２はモータ回転速度指令出力タスク、２０３はジャイロセンサ出力読み取りタスク、２０４は加速度センサ出力読み取りタスク、２０５はエンコーダ信号読み取りおよび差分タスク、２０６は運動学パラメター演算タスク、２０７はマイクロコンピュータ１０３が内蔵するメモリ、２１１はモータ回転速度指令信号、２１２はサーボアンプから得られるエンコーダ信号、２１３はジャイロセンサから得られる移動体１０２の回転速度信号、２１４は加速度センサから得られる移動体１０２の前後方向加速度信号、２１５はキャリブレーション結果として出力される運動学パラメター信号である。モータ回転速度指令出力タスク２０２の処理内容は後述する。
ジャイロセンサ出力読み取りタスク２０３は、一定周期毎に回転速度信号２１３を読み取り、時系列データとしてメモリ２０７に保存するタスクである。
加速度センサ出力読み取りタスク２０４は、一定周期毎に前後方向加速度信号２１４を読み取り、時系列データとしてメモリ２０７に保存するタスクである。
エンコーダ信号読み取りおよび差分タスク２０５は、エンコーダ信号２１２からモータ回転角度を取得するとともに、一定周期毎にモータ回転角度の差分値を計算してモータ回転速度を求め、時系列データとしてメモリ２０７に保存するタスクである。なお、運動学パラメター演算タスク２０６の処理内容は後述する。
メモリ２０７には、各タスクで生成する時系列データに加え、速度指令生成に用いるパラメターをあらかじめ保存しておく。
本実施例では、求めるべき運動学パラメターは、前記のｐｔＬ，ｐｔＲ，ｐｒＬ，およびｐｒＲの４つである。
このうち、移動体の回転速度とモータ回転速度とを対応付けるのはｐｒＬおよびｐｒＲであるから、これらを回転に関する運動学パラメターと呼ぶ。
残るｐｔＬおよびｐｔＲは、移動体の並進速度とモータ回転速度とを対応付けるパラメターであるから、これらを並進に関する運動学パラメターと呼ぶ。
【００１９】
図３はキャリブレーションの手順に関するフロー図である。
図３において、３０１は回転に関する運動学パラメターを求めるステップ、３０２は求めた回転に関する運動学パラメターを保存するステップ、３０３は並進に関する運動学パラメターを求めるステップ、３０４は求めた並進に関する運動学パラメターを保存するステップである。
モータ回転速度指令出力タスク２０２の処理内容および運動学パラメター演算タスク２０６の処理内容は、キャリブレーションの対象とする運動学パラメターの種類によって切り替える。したがって、以下では図３に示した運動学パラメターのキャリブレーション手順に沿って各タスクの処理内容を説明する。
最初に実行するのは、回転に関する運動学パラメターを求めるステップ３０１である。ステップ３０１では、パラメターｐｒＬ、を求める処理、パラメターｐｒＲ、を求める処理を順に行う。
【００２０】
まず、パラメターｐｒＬを求める処理について以下で説明する。
モータ回転速度指令出力タスク２０２は、左モータに対して振幅Ａｒｅｆ、位相角速度
の正弦波を、右モータに対して０を、モータ回転速度指令信号２１１として一定期間にわたって出力する。すなわち、時間［０、Ｔ］において、式（７）とする。
【数７】
ここで、Ａｒｅｆ，αは、Ａｒｅｆの値がモータの制限回転速度を超えないように、かつαＡｒｅｆの値がモータの制限回転角加速度を超えないように適当に設定する定数である。
また、指令出力時間であるＴには、正弦波の周期２π／αのｎ倍に適当な正数Ｔ０を加えた値よりも十分大きな値を設定する。すなわち、
Ｔ＞ＴＳ＋ｎ×２π／α
とする。ここでｎは十分大きな正整数である。
正数ＴＳを設定するのは、移動体の運動がモータ回転指令出力直後から定常状態に移行するまでの時間を使用するデータから除外するためである。
また、正整数ｎを設定するのは、複数周期にわって移動体を動作させることで各種計測値に含まれるノイズの影響を抑えるためである。
時刻ｔは、マイクロコンピュータ１０３が内蔵する図示しないタイマーから取得する。
以上が、モータ回転速度指令出力タスク２０２がパラメターｐｒＬを求めるために実行する処理である。
【００２１】
式（７）の指令を与えたときのモータ回転速度は、一般に
（ωｍＬ，ωｍＲ）＝（Ａｓｉｎ（αｔ＋δservo），０）
となる。
【数８】
モータ回転速度（ωｍＬ，ωｍＲ）の値は、時間［ＴＳ，ＴＳ＋ｎ×２π／α］にわたってエンコーダ信号読み取りおよび差分タスクで取得し、メモリ２０７に保存する。
このとき、移動体の路面に垂直な軸回りの回転速度は
ω＝−ｐｒＬωｍＬ＝−PｒＬＡｓｉｎ（αｔ＋δservo）
となる。
この結果、移動体は初期位置付近で振動的に運動するため、移動体の走行する領域は限定される。すなわち、移動体の走行のために広大な領域を確保する必要はない。ωの計測値ωｏｂｓには一般にバイアス誤差ｄＤＣとホワイトノイズｄＷが含まれる。
ωｏｂｓ＝ω＋ｄＤＣ＋ｄＷ＝−pｒＬＡｓｉｎ（αｔ＋δservo）ｄＤＣ＋ｄＷ
しかし、ωｏｂｓの振幅にはバイアス誤差ｄＤＣは影響せず、ホワイトノイズｄＷのみが影響する。ωｏｂｓは回転速度信号２１３に相当し、その値を時間［ＴＳ，ＴＳ＋ｎ×２π／α］にわたってジャイロセンサ出力読み取りタスク２０３によってメモリ２０７に保存する。
【００２２】
図４にωとωｏｂｓの波形例を示す。
図４において、４０１は時間軸、４０２は回転速度軸、４０３は移動体回転速度ωの波形、４０４はジャイロセンサ１０４で計測した移動体回転速度ωｏｂｓの波形、４０５は左モータ回転速度指令ωｒｅｆＬとして出力した正弦波の1周期に相当する時間を表している。
パラメターpｒＬを求めるための運動学パラメター演算タスク２０６の処理を以下で説明する。運動学パラメター演算タスク２０６の処理は、指令出力および各種データの保存が完了した後に開始する。まず、メモリ２０７に保存された左モータ回転速度ωｍＬの時系列データからその振幅Ａの値を求める。振幅Ａの値は、ωｍＬの時系列データを1周期毎に分割して得られるｎ個の波形の平均値を取ることで得られる1周期分の平均波形について最大値と最小値とを求め、その差を１／２倍することで求める。これはωｍＬに含まれるホワイトノイズの影響を抑えるためである。
次いで、メモリ２０７に保存されたωｏｂｓの時系列データからその振幅Ｂの値を求める。振幅Ｂの値も、振幅Ａの値と同様に１周期分の平均波形から求めることで、ωｏｂｓに含まれるホワイトノイズの影響を抑える。なお、ジャイロセンサ１０４の持つバイアス誤差は、振幅Ｂには全く影響しない。
【００２３】
図５は、１周期分の平均波形から振幅値を求めることの効果を示した図である。
図５において、５０１−１は分割したωｏｂｓの時系列データの1番目の波形、５０１−ｎは分割したωｏｂｓの時系列データのｎ番目の波形、５０２はｎ個の波形から求めた前記1周期分の平均波形、５０３は前記平均波形５０２の最大値と最小値との差を１／２倍したものであって、振幅Ｂである。
平均波形５０２を求めることで、波形に含まれるホワイトノイズの影響を効果的に抑制できる。
更に、振幅を求めるためωｏｂｓに含まれるバイアス誤差ｄＤＣの影響は皆無である。
得られるωｏｂｓの振幅ＢとωｍＬの振幅Ａとの間には、
Ｂ＝ＰｒＬＡ
なる関係があるため、パラメターＰｒＬは式（８）から求める。
ＰｒＬ ＝Ｂ／Ａ ・・・・式（８）
以上が、ＰｒＬを求めるための運動学パラメター演算タスク２０６の処理である。
本実施例では、振幅ＡおよびＢを求めるためにｎ個の波形の平均をとってその最大値と最小値の差を１／２倍したが、ωｍＬおよびωｏｂｓの周波数スペクトル解析を行って位相角速度αに対応する成分の振幅を抽出するようにしてもよい。
あるいは、計測したωｍＬおよびωｏｂｓを位相角速度α付近の振動成分のみを通過するバンドパスフィルタに通してから用いるような構成とすれば、不要なノイズ成分を効果的に除去できる。
【００２４】
次に、パラメターＰｒＲを求める処理を実行する。
処理の内容はＰｒＬを求める処理とほぼ同じである。すなわち、モータ回転速度指令出力タスク２０２は、右モータに対して振幅Ａｒｅｆ、位相角速度αの正弦波を、右モータに対して０を、モータ回転速度指令信号２１１として一定期間にわたって出力する。すなわち、時間［０、Ｔ］において、式（９）とする。
【数９】
以下の手順はＰｒＬの場合と同様であるため割愛する。
ここでも移動体は初期位置付近で振動的に運動するため、移動体の走行のために広大な領域を確保する必要はない。
回転に関する運動学パラメターを求めるステップ３０１が完了したら、求めた回転に関する運動学パラメターを保存するステップ３０２に移行する。運動学パラメター演算タスク２０６は、外部から参照できるように結果をメモリ２０７に保存する。
ステップ３０２の次は、並進に関する運動学パラメターを求めるステップ３０３に進む。本ステップで求めるのはｐｔＬ，ｐｔＲである。
以下でその方法を説明する。
ステップ３０３では、モータ回転速度指令タスク２０２は時間［０、Ｔ］にわたって以下のようなモータ回転速度指令を出力する。
【数１０】
このときのモータ回転速度は、
(ωmL、ωmR)=(A´sin(αt+δservo´)、-A´sin(αt+δservo´))
のようになり、移動体の並進速度は式（１）から
ｖ＝（ｐｔＬ＋ｐｔＲ）Ａ´ｓｉｎ（αｔ＋δservo´）
となる。
ここでも、移動体は初期位置付近で振動的に運動するため、移動体の走行のために広大な領域を確保する必要はない。
【００２５】
移動体の並進加速度は、
ｄｖ／ｄｔ＝α（ｐｔＬ＋ｐｔＲ）Ａ´ｃｏｓ（αｔ＋δservo´）
となる。
加速度センサ１０５で計測される加速度ａｏｂｓは、実際の移動体加速度に加速度センサのバイアス誤差ｄＤＣ´、ホワイトノイズｄＷ´が加わった値となる。
ａｏｂｓ＝α（ｐｔＬ＋ｐｔＲ）Ａ´ｃｏｓ（αｔ＋δservo´）＋ｄＤＣ´＋ｄＷ´
ａｏｂｓは前後方向加速度信号２１４に相当する。
【００２６】
モータ回転速度指令の出力完了後、運動学パラメター演算タスク２０６の処理を開始する。
まず、メモリ２０７に保存された左モータ回転速度ωｍＬまたは右モータ回転速度ωｍＲの時系列データから、その振幅Ａ´の値を求める。振幅を求める方法はステップ３０１の方法と同じである。
次いで、加速度センサ出力読み取りタスクによって保存された時系列データから、ａｏｂｓの振幅Ｂ´を求める。振幅を求める方法はステップ３０１の方法と同じである。ステップ３０１の場合と同様、値に含まれるバイアス誤差の影響は皆無であり、ホワイトノイズの影響も小さく抑えられる。
得られる振幅Ｂ´と振幅Ａ´との間には
Ｂ´＝α（ｐｔＬ＋ｐｔＲ）Ａ´
なる関係がある。また、式（３）から
ｐｔＬ＝（ｐｒＬ／ｐｒＲ）×ｐｔＲ
であるから、上記２式により並進に関する運動学パラメターｐｔＬ、ｐｔＲは、
ｐｔＬ＝ｐｒＬＢ´／（αＡ´（ｐｒＬ＋ｐｒＲ））
ｐｔＲ＝ｐｒＲＢ´／（αＡ´（ｐｒＬ＋ｐｒＲ））
のように求まるのである。
以上でステップ３０３の処理は完了し、ステップ３０４に移行する。
ステップ３０４では、求めた並進に関する運動学パラメターｐｔＬ、ｐｔＲを外部から参照できるようにメモリ２０７に保存する。
【００２７】
本発明は従来の手法とは全く異なり、移動体の回転速度や加速度を直接計測できるセンサを運動学パラメターのキャリブレーションに用いている。そのため、移動体の移動量を計測する手間を必要としない。
更に、正負対称の定常波状のモータ回転速度指令を与えることによってホワイトノイズの影響を軽減し、パラメター計算に振幅を用いることによってセンサ出力値に含まれるバイアス誤差の影響を排除している。
【００２８】
実施例１では、モータ回転速度をエンコーダ信号２１２から求めて保存し、運動学パラメター演算に用いる構成とした。しかし、モータ回転速度制御の性能が十分高い場合はモータ回転速度指令信号２１１をそのままモータ回転速度としてパラメター演算に用いても良い。
また、実施例１では移動体の回転速度を計測するジャイロセンサと移動体の並進加速度を計測する加速度センサとをそれぞれ備えた構成としたが、これは本発明の実施形態をこれに限定するものではない。
【００２９】
以下、２輪型移動体６０１に適用する移動体のキャリブレーション装置について、実施例２では移動体の回転加速度および並進加速度を計測するセンサユニットを備えた例を、実施例３では移動体の回転速度および並進速度を計測するセンサユニットを備えた例を、それぞれ説明する。
【実施例２】
【００３０】
〈移動体の回転加速度および並進加速度を計測するセンサユニットを備えた例〉
実施例２では、移動体の回転加速度および並進加速度を計測するセンサユニットを備えた２輪型移動体のキャリブレーション装置について述べる。
本実施例における移動体のキャリブレーション装置の構成は図１におけるジャイロセンサ１０４と加速度センサ１０５とを図７に示したセンサユニットに置き換えたものである。
図７は、移動体の回転加速度および並進加速度を計測するセンサユニット１の上視図である。図において、７０１がセンサユニット１、７０２−１が第１の１軸加速度センサ、７０２−２が第２の１軸加速度センサ、７０３−１が第１の１軸加速度センサ７０２−１が計測する加速度の正方向、７０３−２が第２の１軸加速度センサ７０２−２が計測する加速度の正方向、７０４がセンサユニット１の中心線、７０５がセンサユニット１の移動体正面に対する取り付け方向である。
１軸加速度センサ７０２−１および７０２−２は、センサユニット１の中心線７０４について対称な位置に、中心線７０４から距離各ｒaだけ離れた点における加速度を計測するように設置する。また、センサユニット７０１は中心線７０４が移動体の中心線と一致するように２輪型移動体６０１（図６）に水平に取り付ける。２つの加速度センサ、７０２−１、７０２−２で計測する加速度の向きは、それぞれ矢印７０３−１、矢印７０３−２に沿う向きである。計測された加速度をそれぞれａ1obs 、ａ2obsと表現する。
実施例２におけるマイクロコンピュータ１０３内部で実行されるタスクと信号の流れを図８に示す。図において８０１はセンサユニット１出力読み取り・処理タスクである。８１１−１および８１１−２はそれぞれ１軸加速度センサ７０２−１および７０２−２から得られる加速度信号であって、ａ1obsとａ2obsに相当する。
センサユニット１出力読み取り・処理タスク８０１は加速度信号ａ1obsとａ2obsを逐次読み取って処理し、移動体の路面に垂直な軸回りの回転加速度および並進加速度に変換したものを時系列データとしてメモリ２０７に保存するタスクである。
得られる移動体の回転加速度をξobs 、移動体の並進加速度をａobs’と表現すると、変換式は以下の通りである。
ξobs ＝（ａ1obs＋ａ2obs）/（２ｒａ）
ａobs’ ＝（ａ1obs−ａ2obs）/２
実施例２におけるキャリブレーションの手順とステップ２，４における処理内容は実施例１と同様である。
また、各ステップにおけるモータ回転速度指令出力タスク２０２の処理も、実施例１と同様である。ただし、ステップ１において、運動学パラメター演算タスク２０６が回転に関する運動学パラメターＰｒＬ，およびＰｒＲを求める処理内容は実施例１と異なるため、以下で説明する。
【００３１】
ステップ１では、モータ回転速度指令力出力タスク２０２は時間［０，Ｔ］にわたって式（７）のようなモータ回転速度指令信号を出力する。このときのモータ回転加速度は
（ξｍＬ，ξｍＲ）=（−αAcos（αt＋δservo ）
となる。実際の移動体の回転加速度ξは
ξ＝−ＰｒＬωｍＬ＝ＰｒＬαAcos（αt＋δservo ）
となる。
センサユニット１出力読み取り・処理タスク８０１で得られる移動体の回転加速度ξobnには一般にバイアス誤差ｄDC2とホワイトノイズｄW２が含まれる。
ξobs ＝ξ＋ｄDC2＋ｄW２＝ＰｒＬαAsin（αt＋δservo）+ｄDC2＋ｄW２
しかし、ξobsの振幅にはバイアス誤差ｄDC2は影響せず、ホワイトノイズｄW２のみが影響する。センサユニット１出力読み取り・処理タスク８０１は、ξobs の値を時間［ＴＳ，ＴＳ＋ｎ×２π／α］にわたってメモリ２０７に保存する。
運動学パラメター演算タスク２０６の処理は、指令出力および各種データの保存が完了した後に開始する。まず、メモリ２０７に保存された左モータ回転速度ωｍＬの時系列データからその振幅Ａの値を求める。
次いで、メモリ２０７に保存されたξobsの時系列データからその振幅Ｂ２の値を求める。振幅を求める方法は実施例１と同様である。振幅にはバイアス誤差の影響が皆無であり、複数周期にわたる時系列データを利用するためホワイトノイズの影響も小さく抑えられる。
得られるξobsの振幅Ｂ２とωｍＬの振幅Ａとの間には、
Ｂ２＝ＰｒＬαA
なる関係があるため、パラメターＰｒＬは
ＰｒＬ＝Ｂ２/（αA） ・・・式（Ａ）
によって求める。
以上が、実施例２においてＰｒLを求めるための運動学パラメター演算タスク２０６の処理である。パラメターＰｒRは、モータ回転速度指令信号２１１を式（９）のように与え、ＰｒＬの場合と同様にして求める。
以上で実施例２におけるステップ１の処理は完了である。
ステップ３において並進に関する運動学パラメターを求める処理は、実施例１においてａobsをａobs’に置き換えたものであるので、説明は割愛する。
【実施例３】
【００３２】
〈移動体の回転速度および並進速度を計測するセンサユニットを備えた例〉
実施例３では、移動体の回転速度および並進速度を計測するセンサユニットを備えた２輪型移動体のキャリブレーション装置について述べる。本実施例における移動体のキャリブレーション装置の構成は図１におけるジャイロセンサ１０４と加速度センサ１０５とをビジョンセンサと画像処理装置を組み合わせたセンサユニット２に置き換えたものである。
センサユニット２は、一定周期毎に移動体６０１（図６）の周辺環境の画像を取り込み、前回の画像との比較差分をとることで移動体６０１の並進速度および回転速度を計算し出力する。計測された移動体の並進速度ｖ３obs，回転速度をω３obsと表現する。
実施例３におけるマイクロコンピュータ１０３内部で実行されるタスクと信号の流れを図９に示す。図において９０１はセンサユニット２出力読み取りタスクである。９１１−１および９１１−２はそれぞれセンサユニット２から得られる移動体の並進速度信号、回転速度信号であって、ｖ３obs，ω３obsに相当する。
センサユニット２出力読み取り・処理タスク９０１は移動体の並進速度信号ｖ３obsおよび回転速度信号ω３obsを逐次読み取り、時系列データとしてメモリ２０７に保存するタスクである。ジャイロセンサ１０４（図１）や加速度センサ１０５（図１）とは異なり、このセンサユニット２はビジョンセンサと画像処理装置を組み合わせた構成であるためバイアスノイズは信号中にほとんど含まれない。しかし、一般に信号へのホワイトノイズの重畳は避け難い。
【００３３】
実施例３におけるキャリブレーションの手順とステップ２，４における処理内容は実施例１と同様である。また、各ステップにおけるモータ回転速度指令出力タスク２０２の処理も、実施例１と同様である。
ステップ１において回転に関する運動学パラメターを求める処理は、実施例１においてωobsをω3obsに置き換えたものであるので、説明は割愛する。
ステップ３において運動学パラメター演算タスク２０６が並進に関する運動学パラメターＰｒＬおよびＰｔＲを求める処理内容は実施例１，２と異なるため、以下で説明する。
ステップ３では、モータ回転速度指令タスク２０２は時間［０，Ｔ］にわたって式（１０）のようなモータ回転速度指令を出力する。このときの移動体の並進速度は前述の通りであるが、その計測値であるｖ３obsにはホワイトノイズｄ3wが含まれる。
ｖ3obs＝ｖ＋ｄ3w＝（ｐｔＬ＋ｐｔＲ）Ａ’ｓｉｎ（αｔ＋δservo’）＋ｄ3w
センサユニット２出力読み取り・処理タスク８０１は、ｖ3obs の値を時間［ＴS，ＴS＋ｎ×２π／α］にわたってメモリ２０７に保存する。
運動学パラメター演算タスク２０６の処理は、指令出力および各種データの保存が完了した後に開始する。
まず、メモリ２０７に保存された左モータ回転速度ωｍＬまたは右モータ回転速度ωｍＲの時系列データから、その振幅Ａ’の値を求める。振幅を求める方法は実施例１と同様である。
次いで、メモリ２０７に保存されたｖ3obsの時系列データから、その振幅Ｂ３’を求める。振幅を求める方法は実施例１と同様である。ここでｖ3obsに含まれるホワイトノイズｄ3wの影響は小さく抑えられる。
得られる振幅Ｂ３’と振幅Ａ’との間には
Ｂ３’＝（ｐｔＬ＋ｐｔＲ）Ａ’
なる関係がある。また、式（３）から
ｐｔＬ＝（ｐｒＬ／ｐｒＲ）×ｐｔＲ
であるから、上記２式により並進に関する運動学パラメターｐｔＬ、ｐｔＲは、
ｐｔＬ＝ｐｒＬＢ３’／（Ａ’（ｐｒＬ＋ｐｒＲ））
ｐｔＲ＝ｐｒＲＢ３’／（Ａ’（ｐｒＬ＋ｐｒＲ））
のように求まる。以上で実施例３におけるステップ３０３の処理は完了である。
以上に示したように、本発明による移動体のキャリブレーション装置はさまざまな種類のセンサを用いて実現することができるのである。キャリブレーションにあたっては、何れの場合も移動体は初期位置付近で振動的に運動するため、移動体の走行する領域は限定される。よってキャリブレーションのために広大な領域を確保する必要はない。
【実施例４】
【００３４】
実施例１は２輪型の移動体をキャリブレーション対象としていたが、本発明はオドメトリによって位置推定を行っている移動体に広く適用できるものである。
本発明の実施例４として、図１０に示した全方向移動体のキャリブレーション装置についても説明する。
図１０において、１００２−１〜１００２−３はそれぞれ第１モータ〜第３モータ、１００３はエンコーダ、１００４は減速器、１００５−１〜３はそれぞれ第１オムニホイル〜第３オムニホイルである。第１車輪１〜第３車輪は、その回転軸が移動体１００１の中心で互いに交わるように、かつ互いのなす角が１２０°になるように設置する。
実施例４においても装置の基本構成は図１と同じである。ただし、加速度センサ１０５（図１）は図１０では全方向移動体１００１に固定した座標系におけるＸ軸方向およびＹ軸方向の並進加速度を計測できる２軸加速度センサとする。
計測される移動体加速度をａｘｏｂｓ、ａｙｏｂｓのように表す。また、キャリブレーションの手順は図３と異なるが、単純であるため図示はしない。
【００３５】
パラメターｐ１１を求める処理について以下で説明する。
モータ回転速度指令出力タスク２０２（図２）は、第１モータ１００２−１に対して振幅Ａｒｅｆ、位相角速度αの正弦波を、第２モータ１００２−２、第３モータ１００２−３に対して０を、モータ回転速度指令信号２１１（図２）として一定期間にわたって出力する。すなわち、時間［０、Ｔ］において、式（１１）とする。
【数１１】
ここでＡｒｅｆ，α，Ｔの各設定基準は、実施例１と同様である。
このときのモータ１００２−１，１００２−２，および１００２−３の回転速度は、式（１２）である。
【数１２】
このときの全方向移動体１００１（図１０）の並進および回転速度は、式（１３）となる。
【数１３】
ここでも移動体の走行領域は限定されており、広大な領域を確保する必要はない。移動体のｘ方向並進加速度は式（１４）であって、
【数１４】
その計測値ａxobsvには、一般に、バイアス誤差ｄＤＣｘとホワイトノイズｄｗｘが含まれる。
【数１５】
ａvxobsは加速度信号２１４（図２）に相当し、その値を加速度センサ出力読み取りタスク２０４（図２）によって時間［ＴＳ、ＴＳ＋ｎ×２π／α］にわたってメモリ２０７（図２）に保存する。
また、モータ回転速度指令出力タスク２０２（図２）、ジャイロセンサ出力読み取りタスク２０３（図２）、加速度センサ出力読み取りタスク２０４（図２）、エンコーダ信号読み取りおよび差分タスク２０５（図２）の処理内容は実施例１と同じである。
【００３６】
モータ回転速度指令の出力完了後、運動学パラメター演算タスク２０６（図２）の処理を開始する。
まず、メモリ２０７に保存されたモータ１回転速度ωｍ１の時系列データから、その振幅Ａ”の値を求める。振幅を求める方法は実施例１と同様である。
次いで、加速度センサ出力読み取りタスク２０４によって保存されたＸ方向加速度信号の時系列データから、ａｏｂｓの振幅Ｂ”を求める。振幅を求める方法は実施例１と同じである。
得られる振幅Ｂ”と振幅Ａ”との間には
Ｂ”＝αｐ１１Ａ”
なる関係があるため、
ｐ１１＝Ｂ”／（αＡ”）
の演算によってパラメターｐ１１を求める。
他の運動学パラメターも同様の手順で求めることができる。
このように、本発明の移動体のキャリブレーション装置は、オドメトリを適用する移動体の運動学パラメターを精度良く求めることができる。
【００３７】
また、本発明のキャリブレーション装置を構成する要素は全て移動体に搭載可能であって、キャリブレーション実施にあたって外部環境に別途設備を用意する必要はない。本キャリブレーション装置を予め搭載した自己キャリブレーション機能を有する移動体を提供することもできる。
【００３８】
簡単な構成の装置で移動体の運動学パラメターを精度よく求めることができ、かつキャリブレーション作業に要する手間と空間を大幅に削減できるため、新たに製造された移動ロボットや長期間使用したために運動学パラメターが変化してしまった移動ロボットなどの調整作業に効果的に用いることができる。
【図面の簡単な説明】
【００３９】
【図１】本発明の実施例１示す移動体のキャリブレーション装置の構成ブロック図である。
【図２】実施例１示す移動体のキャリブレーション装置におけるマイクロコンピュータ内部の処理タスクと信号の流れを示した図である。
【図３】実施例１おける移動体のキャリブレーション方法の手順を示した流れ図である。
【図４】移動体の回転速度とそのジャイロセンサによる計測値の波形例である。
【図５】平均波形から振幅値を求めることの効果を示した図である。
【図６】実施例１〜３が対象とする２輪型移動体を示した図である。
【図７】本発明の実施例２示す移動体のキャリブレーション装置におけるセンサユニット１の上視図である。
【図８】実施例２示す移動体のキャリブレーション装置におけるマイクロコンピュータ内部の処理タスクと信号の流れを示した図である。
【図９】実施例３示す移動体のキャリブレーション装置におけるマイクロコンピュータ内部の処理タスクと信号の流れを示した図である。
【図１０】実施例４が対象とする全方向移動体を示した図である。
【図１１】従来技術３において移動体が走行する経路の例を示した図である。
【符号の説明】
【００４０】
１０１ 移動体のキャリブレーション装置
１０２ 移動体
１０３ マイクロコンピュータ
１０４ ジャイロセンサ
１０５ 加速度センサ
１０６ ＡＤ変換器
１０７ ＤＡ変換器
１０８ サーボアンプ
１０９ エンコーダおよびモータ
１１０ 減速器および車輪
２０１ マイクロコンピュータ内部
２０２ モータ回転速度指令出力タスク
２０３ ジャイロセンサ出力読み取りタスク
２０４ 加速度センサ出力読み取りタスク
２０５ エンコーダ信号読み取りおよび差分タスク
２０６ 運動学パラメター演算タスク
２０７ マイクロコンピュータ１０３が内蔵するメモリ
２１１ モータ回転速度指令信号
２１２ サーボアンプから得られるエンコーダ信号
２１３ 移動体１０２の回転速度信号
２１４ 移動体１０２の前後方向加速度信号
２１５ 運動学パラメター信号
３０１ 回転に関する運動学パラメターを求めるステップ
３０２ 求めた回転に関する運動学パラメターを保存するステップ
３０３ 並進に関する運動学パラメターを求めるステップ
３０４ 求めた並進に関する運動学パラメターを保存するステップ
４０１ 時間軸
４０２ 回転速度軸
４０３ 移動体回転速度の波形
４０４ ジャイロセンサ１０４で計測した移動体回転速度の波形
４０５ 左モータ回転速度指令として出力した正弦波の1周期
５０１−１ ｎ分割したωｏｂｓの時系列データの1番目の波形
５０１−ｎ ｎ分割したωｏｂｓの時系列データのｎ番目の波形
５０２ ｎ個の波形から求めた1周期分の平均波形
５０３ 平均波形５０２の最大値と最小値との差を１／２倍(振幅)
６０１ ２輪型移動体
６０２ 右車輪
６０３ 左車輪
６０４ 右モータ
６０５ 左モータ
６０６ 減速器
７０１ センサユニット１
７０２−１ 第１の１軸加速度センサ
７０２−２ 第２の１軸加速度センサ
７０３−１ 第１の１軸加速度センサ７０２−１が計測する加速度の正方向
７０３−２ 第２の１軸加速度センサ７０２−２が計測する加速度の正方向
７０４ センサユニット１の中心線
７０５ センサユニット１の移動体正面に対する取り付け方向
８０１ センサユニット１出力読み取り・処理タスク
８１１−１ １軸加速度センサ７０２−１から得られる加速度信号
８１１−２ １軸加速度センサ７０２−２から得られる加速度信号
９０１ センサユニット２出力読み取りタスク
９１１−１ センサユニット２から得られる移動体の並進速度信号
９１１−２ センサユニット２から得られる移動体の回転速度信号
１００１ 全方向移動体
１００２−１ モータ１
１００２−２ モータ２
１００２−３ モータ３
１００３ エンコーダ
１００４ 減速器
１００５−１ オムニホイル１
１００５−２ オムニホイル２
１００５−３ オムニホイル３、
１１０１ 移動体
１１０２ 基準壁
１１０３ 移動体の移動開始点
１１０４ 移動完了点
１１０５ 事前にプログラムされた一辺が４ｍの正方形の経路
１１０６ 移動体１１０１が実際に走行した経路
【技術分野】
【０００１】
本発明は、移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置およびこれを搭載した移動体並びにキャリブレーション方法に関する。
【背景技術】
【０００２】
移動体の並進位置や回転角度を得るためには、移動体の運動学パラメター（例えば車輪半径、車輪接地点半径など）を用いてモータ回転速度から移動体の並進速度・回転速度を求めて積分する「オドメトリ」という方法が広く用いられている。
【０００３】
オドメトリについて、図６に示した２輪型移動体の例を用いて説明する。図において、６０１が２輪型移動体、６０２が右車輪、６０３が左車輪、６０４が右モータ、６０５が左モータ、６０６が減速器である。
移動体６０１の並進速度ｖをモータ回転速度（ωｍＬ、ωｍＲ）から求める式は式（１）である。
【数１】
移動体６０１の路面に垂直な軸回りの回転速度をモータ回転速度から求める式は式（２）である。
【数２】
式中、Ｇｒは減速器６０６の減速比、ｒＷＲは右車輪６０２の半径である右車輪半径、ｒＷＬは左車輪６０３の半径である左車輪半径、Ｒは車軸中心に対する車輪接地点半径であって、ここではｐｔＬ、ｐｔＲ、ｐｒＬ、およびｐｒＲを運動学パラメターと定義する。
なお、パラメターｐｔＬ、ｐｔＲ、ｐｒＬ、およびｐｒＲの間には、式（３）の関係がある。
【数３】
モータ回転速度（ωｍＬ、ωｍＲ）はモータに内蔵されたエンコーダやタコメータなどのセンサで計測できる。よって上記２つの式を用いて移動体の並進・回転速度を逐次求め、それを積分していくことで移動体の初期位置に対する移動量を求めることができる。これを式で表すと、式（４）のようになる。
【数４】
ここでｔは現在時刻、ｔ０はオドメトリの開始時刻、θ０、ｘ０、ｙ０はそれぞれ移動体初期姿勢、初期ｘ座標、初期ｙ座標を表す。移動体の初期位置を定めれば、後は式（４）の積分計算によって逐次移動体位置・姿勢（ｘ(ｔ) 、ｙ(ｔ) 、θ(ｔ)）を推定できるのである。これがオドメトリと呼ばれる手法である。
【０００４】
オドメトリは、図６のような２輪型移動体に限らず、多くの移動体に適用可能な手法である。ここでは更に、図１０を用いて３輪を有する全方向移動体にオドメトリを実施する場合を説明する。図１０は前記全方向移動体の上視図である。図１０において、１００１が全方向移動体、１００２−１〜１００２−３がそれぞれ第１モータ〜第３モータである。また、１００３がエンコーダ、１００４が減速器、１００５−１〜１００５−３がそれぞれ第１オムニホイル〜第３オムニホイルである。
第１車輪１〜第３車輪は、その回転軸が移動体１００１の中心で互いに交わるように、かつ互いのなす角が１２０°になるように設置する。
【０００５】
全方向移動体１００１の移動体固定座標系における並進および回転の速度をモータ回転速度（ωｍ１、ωｍ２、ωｍ３）から求める式は 、式（５）である。
【数５】
ここで、ｖｘｖ、ｖｙｖはそれぞれ移動体に固定された座標系で表現した移動体１００１の並進速度、ｖθｖは移動体に固定された座標系で表現した移動体１００１の回転速度である。式中Ｇｒ’は減速器１００４の減速比、ｒｗ１、ｒｗ２、ｒｗ３、はそれぞれ車輪１−３の半径である車輪半径１−３、また、Ｒ１、Ｒ２、Ｒ３はそれぞれ車軸中心に対する車輪１−３の車輪接地点半径１−３である。ここではｐ１１、ｐ１２、ｐ１３、ｐ２１、ｐ２２、ｐ２３、ｐ３１、ｐ３２、およびｐ３３を運動学パラメターと定義する。モータ回転速度（ωｍ１、ωｍ２、ωｍ３）はモータに内蔵されたエンコーダやタコメータなどのセンサで計測できる。よって式（５）を用いて移動体の並進・回転速度を逐次求め、それを積分していくことで移動体の初期位置に対する移動量を求めることができる。これを式で表すと、式（６）のようになる。
【数６】
このように３輪を有する全方向移動体であっても、モータ回転速度と移動体の並進および回転速度とを対応付ける関係式を導けば、オドメトリ手法を適用することができる。
【０００６】
従来、運動学パラメターを取得するには、設計値を用いる（従来技術１）か、実際に物差で測定（従来技術２）していた。あるいは、移動体に正方形状の目標軌道を与え、実際の到達位置を測定してオドメトリの結果と比較することにより運動学パラメターを修正する方法（従来技術３、非特許文献１参照）もあった。
【非特許文献１】J.Borenstein and L.Feng、 “Measurement and Correction of Systematic Odometry Errors in Mobile Robots”、 IEEE Trans. Robotics and Automation、 Vol.12、 No.6、 1996
【０００７】
従来技術３における移動体のキャリブレーション方法を以下で説明する。
その手順は以下のとおりである。
１）運動学パラメターとして設計値を設定する。
２）基準壁に対する移動体の位置・姿勢を計測し、計測結果を移動体のオドメトリ初期値に設定する。
３）移動体に一辺が４ｍの正方形経路上を時計回りに走行する指令を与える。正方形の各頂点では逐一移動体を停止させ、時計回りに90度回転させる。滑りを生じないように移動体をゆっくり走らせる。
４）走行後の基準壁に対する位置を計測する。
５）オドメトリの結果と実際の位置計測結果を比較する。
６）ステップ２）から５）を合計5回以上繰り返す。
７）ステップ２）から６）を、走行の向きを時計回りから反時計回りに変更して実施する。
８）以上のステップで得られた計測結果を利用して修正運動学パラメターを求め、設定する（運動学パラメターの修正に関する数式については、非特許文献１を参照されたい。）。
【０００８】
図１１は従来技術３のキャリブレーションにあたって移動体が走行する経路の例を示した図である。図１１において１１０１が移動体、１１０２が基準壁、１１０３が移動体の移動開始点、１１０４が移動完了点、実線１１０５が移動体に与える事前にプログラムされた一辺が４ｍの正方形の経路、 点線１１０６は移動体１１０１が実際に走行した経路である。なお、図１１は移動体を反時計回りに走行させた場合である。
従来技術３では、移動体に同じ経路を時計回りに走行させた場合と反時計回りに走行させた場合とを比較することで、回転に関する運動学パラメターの誤差の影響と、並進に関する運動学パラメターの誤差の影響とを分離している。
また、試行を5回以上行うことで施行毎に生じるノイズの影響を軽減している。
したがって、以上に示したようなキャリブレーションを行うことによって、より真値に近い運動学パラメターを取得することができるのである。
【発明の開示】
【発明が解決しようとする課題】
【０００９】
従来技術１では加工や組立によって生じる運動学パラメターの設計値と真値との相違に対応できないため、オドメトリの結果に誤差を生じる可能性が高いという問題があった。
従来技術２は計測に手間がかかる上、移動体の構造によっては計測そのものが困難であるという問題があった。また、計測は多くの場合目測に頼らざるを得ないため、測定誤差を生じる可能性も高かった。
従来技術３においても、移動体の初期位置や到達位置を実測する必要があり、手間がかかった。また、移動体を走行させるための広い空間を必要とするという問題もあった。
【００１０】
本発明はこのような問題点に鑑みてなされたものであり、簡単な構成の装置で移動体の運動学パラメターを精度よく求めることができ、かつキャリブレーション作業に要する手間と空間を大幅に削減する移動体のキャリブレーション装置およびこれを搭載した移動体並びにそのキャリブレーション方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項２記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項３記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項４記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項５記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と、前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項６記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項７記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
請求項８記載の発明は、移動体のキャリブレーション装置に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とするものである。
【００１２】
請求項９記載の発明は、請求項１、５、又は７記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の回転速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とするものである。
請求項１０記載の発明は、請求項２、６、又は８記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の回転加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とするものである。
請求項１１記載の発明は、請求項４、５又は６記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の並進加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする。
請求項１２記載の発明は、請求項３、７又は８記載の移動体のキャリブレーション装置において、前記演算処理部が、前記移動体の並進速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とするものである。
請求項１３記載の発明は、請求項９〜１２のいずれか１項記載の移動体のキャリブレーション装置において、前記モータ回転速度指令部が出力するのは、一定角周波数の正弦波状の指令であることを特徴とするものである。
【００１３】
請求項１４記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記振幅値取得部が、その入力信号の周波数解析を行って前記一定角周波数の振動振幅値を抽出する周波数解析部からなることを特徴とするものである。
請求項１５記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の回転速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
請求項１６記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の回転加速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
請求項１７記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の並進加速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
請求項１８記載の発明は、請求項１３記載の移動体のキャリブレーション装置において、前記移動体の並進速度計測部が、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とするものである。
【００１４】
請求項１９記載の発明は、請求項９記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２０記載の発明は、請求項１０記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２１記載の発明は、請求項１１記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２２記載の発明は、請求項１２記載のキャリブレーション装置において、前記振幅値取得部が、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とするものである。
請求項２３記載の発明は、請求項９記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の回転速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２４記載の発明は、請求項１０記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の回転加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２５記載の発明は、請求項１１記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の並進加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２６記載の発明は、請求項１２記載の移動体のキャリブレーション装置において、前記振幅値取得部が、前記移動体の並進速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とするものである。
請求項２７記載の発明は、請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置において、前記モータ回転速度が、前記モータ回転速度指令で代替されることを特徴とするものである。
請求項２８記載の発明は、移動体に係り、請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置を搭載したことを特徴とするものである。
請求項２９記載の発明は、移動体のキャリブレーション方法に係り、１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うためのキャリブレーション方法であって、はじめにモータ回転速度とセンサから得られる前記移動体の回転速度とを用いて移動体の回転に関する第１の運動学パラメターを求め、次いで、求まった第1の運動学パラメターと、モータ回転速度と、センサから得られる前記移動体の並進加速度あるいは並進速度と、を用いて移動体の並進に関する第２の運動学パラメターを求めることを特徴とするものである。
【発明の効果】
【００１５】
請求項１記載の発明によると、移動体の寸法や回転角度を測定する手間をかけることなく移動体の回転に関する運動学パラメターを自動的に求めることができる。
請求項２記載の発明によると、移動体の回転速度計測部が手に入らない場合でも同等の効果を有する移動体のキャリブレーション装置を構成できる。
請求項３および４記載の発明によると、移動体の寸法や並進移動量を測定する手間をかけることなく、移動体の並進に関する運動学パラメターを求めることができる。
請求項５〜８記載の発明によると、移動体の寸法や回転移動量および並進移動量を測定する手間をかけることなく、移動体の回転および並進に関する運動学パラメターを自動的に求めることができる。
請求項９〜１２記載の発明によると、移動体の回転速度計測部や回転加速度計測部、並進速度計測部、あるいは並進加速度計測部の出力にバイアス誤差が含まれている場合でも移動体の運動学パラメターを自動的に精度良く求めることができる。また、キャリブレーション時の移動体移動量を少なくできるため、キャリブレーションのために広大な空間を必要としないという利点もある。
請求項１３記載の発明によると、単純な信号発生装置で前記モータ回転速度指令部を構成でき、なおかつセンサの出力値に含まれるバイアス誤差にかかわらず精度良く移動体の運動学パラメターを求めることができる。
【００１６】
請求項１４〜１８記載の発明によると、センサの出力値に含まれるノイズ成分を効果的に除去でき、精度良く移動体の運動学パラメターを求めることができる。
請求項１９〜２２記載の発明によると、単純な演算によって素早く移動体の運動学パラメターを求めることができる。
請求項２３〜２６記載の発明によると、センサの出力値に含まれるノイズ成分の影響を減少させ、精度良く移動体の運動学パラメターを求めることができる。
請求項２７記載の発明によれば、より単純な構成で移動体の運動学パラメターを求めることができる。
請求項２８記載の発明によれば、キャリブレーションにあたって別途装置を用意する必要がなくなり、作業の迅速化が可能となる。
請求項２９記載の発明によると、移動体の回転および並進に関する運動学パラメターを少ない手順で効果的に求めることができる。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００１８】
図１は、図６に示した２輪型移動体のキャリブレーション装置の構成を示した図である。図において、破線１０１内部が移動体のキャリブレーション装置、点線１０２内部が移動体、１０３がマイクロコンピュータ、１０４が移動体の回転速度を計測するジャイロセンサ、１０５が加速度センサ、１０６がＡＤ変換器、１０７がＤＡ変換器、１０８がサーボアンプ、１０９がエンコーダおよびモータ、１１０が減速器および車輪である。本実施例においてキャリブレーションの対象とする移動体は2輪型なので、サーボアンプ１０８、エンコーダおよびモータ１０９、減速器および車輪１１０はそれぞれ２つずつ設置されている。ジャイロセンサ１０４および加速度センサ１０５の出力は、それぞれ移動体の回転速度、前後方向加速度に比例したアナログ電圧であるから、ＡＤ変換器１０６を通してディジタル信号に変換される。変換後の値はバス接続されたマイクロコンピュータ１０３から利用可能となっている。マイクロコンピュータ１０３はモータ回転速度指令を計算し、バス接続されたＤＡ変換器１０７を通してサーボアンプ１０８に出力する。サーボアンプ１０８はモータ１０９を前記モータ回転速度指令に追従するように制御するとともに、エンコーダ信号をマイクロコンピュータ１０３に出力する。モータ１０９の回転は減速器で減速され、車輪１１０を駆動する。その結果移動体１０２は路面に対して移動する。移動によって生じる回転速度はジャイロセンサ１０４で、前後方向加速度は加速度センサ１０５で計測される。なお、本実施例は図６のような平面上を走行する2輪型移動体を対象としたキャリブレーション装置であるので、ジャイロセンサ１０４は走行面に垂直な軸回りの回転速度を計測できるように、加速度センサ１０５は移動体前後方向の加速度を計測できるように、それぞれ設置する。
続いて、マイクロコンピュータ１０３内部で実行される処理を説明する。図２は、マイクロコンピュータ内部で実行されるタスクと信号の流れを示した図である。図２において、破線２０１がマイクロコンピュータ内部であり、２０２はモータ回転速度指令出力タスク、２０３はジャイロセンサ出力読み取りタスク、２０４は加速度センサ出力読み取りタスク、２０５はエンコーダ信号読み取りおよび差分タスク、２０６は運動学パラメター演算タスク、２０７はマイクロコンピュータ１０３が内蔵するメモリ、２１１はモータ回転速度指令信号、２１２はサーボアンプから得られるエンコーダ信号、２１３はジャイロセンサから得られる移動体１０２の回転速度信号、２１４は加速度センサから得られる移動体１０２の前後方向加速度信号、２１５はキャリブレーション結果として出力される運動学パラメター信号である。モータ回転速度指令出力タスク２０２の処理内容は後述する。
ジャイロセンサ出力読み取りタスク２０３は、一定周期毎に回転速度信号２１３を読み取り、時系列データとしてメモリ２０７に保存するタスクである。
加速度センサ出力読み取りタスク２０４は、一定周期毎に前後方向加速度信号２１４を読み取り、時系列データとしてメモリ２０７に保存するタスクである。
エンコーダ信号読み取りおよび差分タスク２０５は、エンコーダ信号２１２からモータ回転角度を取得するとともに、一定周期毎にモータ回転角度の差分値を計算してモータ回転速度を求め、時系列データとしてメモリ２０７に保存するタスクである。なお、運動学パラメター演算タスク２０６の処理内容は後述する。
メモリ２０７には、各タスクで生成する時系列データに加え、速度指令生成に用いるパラメターをあらかじめ保存しておく。
本実施例では、求めるべき運動学パラメターは、前記のｐｔＬ，ｐｔＲ，ｐｒＬ，およびｐｒＲの４つである。
このうち、移動体の回転速度とモータ回転速度とを対応付けるのはｐｒＬおよびｐｒＲであるから、これらを回転に関する運動学パラメターと呼ぶ。
残るｐｔＬおよびｐｔＲは、移動体の並進速度とモータ回転速度とを対応付けるパラメターであるから、これらを並進に関する運動学パラメターと呼ぶ。
【００１９】
図３はキャリブレーションの手順に関するフロー図である。
図３において、３０１は回転に関する運動学パラメターを求めるステップ、３０２は求めた回転に関する運動学パラメターを保存するステップ、３０３は並進に関する運動学パラメターを求めるステップ、３０４は求めた並進に関する運動学パラメターを保存するステップである。
モータ回転速度指令出力タスク２０２の処理内容および運動学パラメター演算タスク２０６の処理内容は、キャリブレーションの対象とする運動学パラメターの種類によって切り替える。したがって、以下では図３に示した運動学パラメターのキャリブレーション手順に沿って各タスクの処理内容を説明する。
最初に実行するのは、回転に関する運動学パラメターを求めるステップ３０１である。ステップ３０１では、パラメターｐｒＬ、を求める処理、パラメターｐｒＲ、を求める処理を順に行う。
【００２０】
まず、パラメターｐｒＬを求める処理について以下で説明する。
モータ回転速度指令出力タスク２０２は、左モータに対して振幅Ａｒｅｆ、位相角速度
の正弦波を、右モータに対して０を、モータ回転速度指令信号２１１として一定期間にわたって出力する。すなわち、時間［０、Ｔ］において、式（７）とする。
【数７】
ここで、Ａｒｅｆ，αは、Ａｒｅｆの値がモータの制限回転速度を超えないように、かつαＡｒｅｆの値がモータの制限回転角加速度を超えないように適当に設定する定数である。
また、指令出力時間であるＴには、正弦波の周期２π／αのｎ倍に適当な正数Ｔ０を加えた値よりも十分大きな値を設定する。すなわち、
Ｔ＞ＴＳ＋ｎ×２π／α
とする。ここでｎは十分大きな正整数である。
正数ＴＳを設定するのは、移動体の運動がモータ回転指令出力直後から定常状態に移行するまでの時間を使用するデータから除外するためである。
また、正整数ｎを設定するのは、複数周期にわって移動体を動作させることで各種計測値に含まれるノイズの影響を抑えるためである。
時刻ｔは、マイクロコンピュータ１０３が内蔵する図示しないタイマーから取得する。
以上が、モータ回転速度指令出力タスク２０２がパラメターｐｒＬを求めるために実行する処理である。
【００２１】
式（７）の指令を与えたときのモータ回転速度は、一般に
（ωｍＬ，ωｍＲ）＝（Ａｓｉｎ（αｔ＋δservo），０）
となる。
【数８】
モータ回転速度（ωｍＬ，ωｍＲ）の値は、時間［ＴＳ，ＴＳ＋ｎ×２π／α］にわたってエンコーダ信号読み取りおよび差分タスクで取得し、メモリ２０７に保存する。
このとき、移動体の路面に垂直な軸回りの回転速度は
ω＝−ｐｒＬωｍＬ＝−PｒＬＡｓｉｎ（αｔ＋δservo）
となる。
この結果、移動体は初期位置付近で振動的に運動するため、移動体の走行する領域は限定される。すなわち、移動体の走行のために広大な領域を確保する必要はない。ωの計測値ωｏｂｓには一般にバイアス誤差ｄＤＣとホワイトノイズｄＷが含まれる。
ωｏｂｓ＝ω＋ｄＤＣ＋ｄＷ＝−pｒＬＡｓｉｎ（αｔ＋δservo）ｄＤＣ＋ｄＷ
しかし、ωｏｂｓの振幅にはバイアス誤差ｄＤＣは影響せず、ホワイトノイズｄＷのみが影響する。ωｏｂｓは回転速度信号２１３に相当し、その値を時間［ＴＳ，ＴＳ＋ｎ×２π／α］にわたってジャイロセンサ出力読み取りタスク２０３によってメモリ２０７に保存する。
【００２２】
図４にωとωｏｂｓの波形例を示す。
図４において、４０１は時間軸、４０２は回転速度軸、４０３は移動体回転速度ωの波形、４０４はジャイロセンサ１０４で計測した移動体回転速度ωｏｂｓの波形、４０５は左モータ回転速度指令ωｒｅｆＬとして出力した正弦波の1周期に相当する時間を表している。
パラメターpｒＬを求めるための運動学パラメター演算タスク２０６の処理を以下で説明する。運動学パラメター演算タスク２０６の処理は、指令出力および各種データの保存が完了した後に開始する。まず、メモリ２０７に保存された左モータ回転速度ωｍＬの時系列データからその振幅Ａの値を求める。振幅Ａの値は、ωｍＬの時系列データを1周期毎に分割して得られるｎ個の波形の平均値を取ることで得られる1周期分の平均波形について最大値と最小値とを求め、その差を１／２倍することで求める。これはωｍＬに含まれるホワイトノイズの影響を抑えるためである。
次いで、メモリ２０７に保存されたωｏｂｓの時系列データからその振幅Ｂの値を求める。振幅Ｂの値も、振幅Ａの値と同様に１周期分の平均波形から求めることで、ωｏｂｓに含まれるホワイトノイズの影響を抑える。なお、ジャイロセンサ１０４の持つバイアス誤差は、振幅Ｂには全く影響しない。
【００２３】
図５は、１周期分の平均波形から振幅値を求めることの効果を示した図である。
図５において、５０１−１は分割したωｏｂｓの時系列データの1番目の波形、５０１−ｎは分割したωｏｂｓの時系列データのｎ番目の波形、５０２はｎ個の波形から求めた前記1周期分の平均波形、５０３は前記平均波形５０２の最大値と最小値との差を１／２倍したものであって、振幅Ｂである。
平均波形５０２を求めることで、波形に含まれるホワイトノイズの影響を効果的に抑制できる。
更に、振幅を求めるためωｏｂｓに含まれるバイアス誤差ｄＤＣの影響は皆無である。
得られるωｏｂｓの振幅ＢとωｍＬの振幅Ａとの間には、
Ｂ＝ＰｒＬＡ
なる関係があるため、パラメターＰｒＬは式（８）から求める。
ＰｒＬ ＝Ｂ／Ａ ・・・・式（８）
以上が、ＰｒＬを求めるための運動学パラメター演算タスク２０６の処理である。
本実施例では、振幅ＡおよびＢを求めるためにｎ個の波形の平均をとってその最大値と最小値の差を１／２倍したが、ωｍＬおよびωｏｂｓの周波数スペクトル解析を行って位相角速度αに対応する成分の振幅を抽出するようにしてもよい。
あるいは、計測したωｍＬおよびωｏｂｓを位相角速度α付近の振動成分のみを通過するバンドパスフィルタに通してから用いるような構成とすれば、不要なノイズ成分を効果的に除去できる。
【００２４】
次に、パラメターＰｒＲを求める処理を実行する。
処理の内容はＰｒＬを求める処理とほぼ同じである。すなわち、モータ回転速度指令出力タスク２０２は、右モータに対して振幅Ａｒｅｆ、位相角速度αの正弦波を、右モータに対して０を、モータ回転速度指令信号２１１として一定期間にわたって出力する。すなわち、時間［０、Ｔ］において、式（９）とする。
【数９】
以下の手順はＰｒＬの場合と同様であるため割愛する。
ここでも移動体は初期位置付近で振動的に運動するため、移動体の走行のために広大な領域を確保する必要はない。
回転に関する運動学パラメターを求めるステップ３０１が完了したら、求めた回転に関する運動学パラメターを保存するステップ３０２に移行する。運動学パラメター演算タスク２０６は、外部から参照できるように結果をメモリ２０７に保存する。
ステップ３０２の次は、並進に関する運動学パラメターを求めるステップ３０３に進む。本ステップで求めるのはｐｔＬ，ｐｔＲである。
以下でその方法を説明する。
ステップ３０３では、モータ回転速度指令タスク２０２は時間［０、Ｔ］にわたって以下のようなモータ回転速度指令を出力する。
【数１０】
このときのモータ回転速度は、
(ωmL、ωmR)=(A´sin(αt+δservo´)、-A´sin(αt+δservo´))
のようになり、移動体の並進速度は式（１）から
ｖ＝（ｐｔＬ＋ｐｔＲ）Ａ´ｓｉｎ（αｔ＋δservo´）
となる。
ここでも、移動体は初期位置付近で振動的に運動するため、移動体の走行のために広大な領域を確保する必要はない。
【００２５】
移動体の並進加速度は、
ｄｖ／ｄｔ＝α（ｐｔＬ＋ｐｔＲ）Ａ´ｃｏｓ（αｔ＋δservo´）
となる。
加速度センサ１０５で計測される加速度ａｏｂｓは、実際の移動体加速度に加速度センサのバイアス誤差ｄＤＣ´、ホワイトノイズｄＷ´が加わった値となる。
ａｏｂｓ＝α（ｐｔＬ＋ｐｔＲ）Ａ´ｃｏｓ（αｔ＋δservo´）＋ｄＤＣ´＋ｄＷ´
ａｏｂｓは前後方向加速度信号２１４に相当する。
【００２６】
モータ回転速度指令の出力完了後、運動学パラメター演算タスク２０６の処理を開始する。
まず、メモリ２０７に保存された左モータ回転速度ωｍＬまたは右モータ回転速度ωｍＲの時系列データから、その振幅Ａ´の値を求める。振幅を求める方法はステップ３０１の方法と同じである。
次いで、加速度センサ出力読み取りタスクによって保存された時系列データから、ａｏｂｓの振幅Ｂ´を求める。振幅を求める方法はステップ３０１の方法と同じである。ステップ３０１の場合と同様、値に含まれるバイアス誤差の影響は皆無であり、ホワイトノイズの影響も小さく抑えられる。
得られる振幅Ｂ´と振幅Ａ´との間には
Ｂ´＝α（ｐｔＬ＋ｐｔＲ）Ａ´
なる関係がある。また、式（３）から
ｐｔＬ＝（ｐｒＬ／ｐｒＲ）×ｐｔＲ
であるから、上記２式により並進に関する運動学パラメターｐｔＬ、ｐｔＲは、
ｐｔＬ＝ｐｒＬＢ´／（αＡ´（ｐｒＬ＋ｐｒＲ））
ｐｔＲ＝ｐｒＲＢ´／（αＡ´（ｐｒＬ＋ｐｒＲ））
のように求まるのである。
以上でステップ３０３の処理は完了し、ステップ３０４に移行する。
ステップ３０４では、求めた並進に関する運動学パラメターｐｔＬ、ｐｔＲを外部から参照できるようにメモリ２０７に保存する。
【００２７】
本発明は従来の手法とは全く異なり、移動体の回転速度や加速度を直接計測できるセンサを運動学パラメターのキャリブレーションに用いている。そのため、移動体の移動量を計測する手間を必要としない。
更に、正負対称の定常波状のモータ回転速度指令を与えることによってホワイトノイズの影響を軽減し、パラメター計算に振幅を用いることによってセンサ出力値に含まれるバイアス誤差の影響を排除している。
【００２８】
実施例１では、モータ回転速度をエンコーダ信号２１２から求めて保存し、運動学パラメター演算に用いる構成とした。しかし、モータ回転速度制御の性能が十分高い場合はモータ回転速度指令信号２１１をそのままモータ回転速度としてパラメター演算に用いても良い。
また、実施例１では移動体の回転速度を計測するジャイロセンサと移動体の並進加速度を計測する加速度センサとをそれぞれ備えた構成としたが、これは本発明の実施形態をこれに限定するものではない。
【００２９】
以下、２輪型移動体６０１に適用する移動体のキャリブレーション装置について、実施例２では移動体の回転加速度および並進加速度を計測するセンサユニットを備えた例を、実施例３では移動体の回転速度および並進速度を計測するセンサユニットを備えた例を、それぞれ説明する。
【実施例２】
【００３０】
〈移動体の回転加速度および並進加速度を計測するセンサユニットを備えた例〉
実施例２では、移動体の回転加速度および並進加速度を計測するセンサユニットを備えた２輪型移動体のキャリブレーション装置について述べる。
本実施例における移動体のキャリブレーション装置の構成は図１におけるジャイロセンサ１０４と加速度センサ１０５とを図７に示したセンサユニットに置き換えたものである。
図７は、移動体の回転加速度および並進加速度を計測するセンサユニット１の上視図である。図において、７０１がセンサユニット１、７０２−１が第１の１軸加速度センサ、７０２−２が第２の１軸加速度センサ、７０３−１が第１の１軸加速度センサ７０２−１が計測する加速度の正方向、７０３−２が第２の１軸加速度センサ７０２−２が計測する加速度の正方向、７０４がセンサユニット１の中心線、７０５がセンサユニット１の移動体正面に対する取り付け方向である。
１軸加速度センサ７０２−１および７０２−２は、センサユニット１の中心線７０４について対称な位置に、中心線７０４から距離各ｒaだけ離れた点における加速度を計測するように設置する。また、センサユニット７０１は中心線７０４が移動体の中心線と一致するように２輪型移動体６０１（図６）に水平に取り付ける。２つの加速度センサ、７０２−１、７０２−２で計測する加速度の向きは、それぞれ矢印７０３−１、矢印７０３−２に沿う向きである。計測された加速度をそれぞれａ1obs 、ａ2obsと表現する。
実施例２におけるマイクロコンピュータ１０３内部で実行されるタスクと信号の流れを図８に示す。図において８０１はセンサユニット１出力読み取り・処理タスクである。８１１−１および８１１−２はそれぞれ１軸加速度センサ７０２−１および７０２−２から得られる加速度信号であって、ａ1obsとａ2obsに相当する。
センサユニット１出力読み取り・処理タスク８０１は加速度信号ａ1obsとａ2obsを逐次読み取って処理し、移動体の路面に垂直な軸回りの回転加速度および並進加速度に変換したものを時系列データとしてメモリ２０７に保存するタスクである。
得られる移動体の回転加速度をξobs 、移動体の並進加速度をａobs’と表現すると、変換式は以下の通りである。
ξobs ＝（ａ1obs＋ａ2obs）/（２ｒａ）
ａobs’ ＝（ａ1obs−ａ2obs）/２
実施例２におけるキャリブレーションの手順とステップ２，４における処理内容は実施例１と同様である。
また、各ステップにおけるモータ回転速度指令出力タスク２０２の処理も、実施例１と同様である。ただし、ステップ１において、運動学パラメター演算タスク２０６が回転に関する運動学パラメターＰｒＬ，およびＰｒＲを求める処理内容は実施例１と異なるため、以下で説明する。
【００３１】
ステップ１では、モータ回転速度指令力出力タスク２０２は時間［０，Ｔ］にわたって式（７）のようなモータ回転速度指令信号を出力する。このときのモータ回転加速度は
（ξｍＬ，ξｍＲ）=（−αAcos（αt＋δservo ）
となる。実際の移動体の回転加速度ξは
ξ＝−ＰｒＬωｍＬ＝ＰｒＬαAcos（αt＋δservo ）
となる。
センサユニット１出力読み取り・処理タスク８０１で得られる移動体の回転加速度ξobnには一般にバイアス誤差ｄDC2とホワイトノイズｄW２が含まれる。
ξobs ＝ξ＋ｄDC2＋ｄW２＝ＰｒＬαAsin（αt＋δservo）+ｄDC2＋ｄW２
しかし、ξobsの振幅にはバイアス誤差ｄDC2は影響せず、ホワイトノイズｄW２のみが影響する。センサユニット１出力読み取り・処理タスク８０１は、ξobs の値を時間［ＴＳ，ＴＳ＋ｎ×２π／α］にわたってメモリ２０７に保存する。
運動学パラメター演算タスク２０６の処理は、指令出力および各種データの保存が完了した後に開始する。まず、メモリ２０７に保存された左モータ回転速度ωｍＬの時系列データからその振幅Ａの値を求める。
次いで、メモリ２０７に保存されたξobsの時系列データからその振幅Ｂ２の値を求める。振幅を求める方法は実施例１と同様である。振幅にはバイアス誤差の影響が皆無であり、複数周期にわたる時系列データを利用するためホワイトノイズの影響も小さく抑えられる。
得られるξobsの振幅Ｂ２とωｍＬの振幅Ａとの間には、
Ｂ２＝ＰｒＬαA
なる関係があるため、パラメターＰｒＬは
ＰｒＬ＝Ｂ２/（αA） ・・・式（Ａ）
によって求める。
以上が、実施例２においてＰｒLを求めるための運動学パラメター演算タスク２０６の処理である。パラメターＰｒRは、モータ回転速度指令信号２１１を式（９）のように与え、ＰｒＬの場合と同様にして求める。
以上で実施例２におけるステップ１の処理は完了である。
ステップ３において並進に関する運動学パラメターを求める処理は、実施例１においてａobsをａobs’に置き換えたものであるので、説明は割愛する。
【実施例３】
【００３２】
〈移動体の回転速度および並進速度を計測するセンサユニットを備えた例〉
実施例３では、移動体の回転速度および並進速度を計測するセンサユニットを備えた２輪型移動体のキャリブレーション装置について述べる。本実施例における移動体のキャリブレーション装置の構成は図１におけるジャイロセンサ１０４と加速度センサ１０５とをビジョンセンサと画像処理装置を組み合わせたセンサユニット２に置き換えたものである。
センサユニット２は、一定周期毎に移動体６０１（図６）の周辺環境の画像を取り込み、前回の画像との比較差分をとることで移動体６０１の並進速度および回転速度を計算し出力する。計測された移動体の並進速度ｖ３obs，回転速度をω３obsと表現する。
実施例３におけるマイクロコンピュータ１０３内部で実行されるタスクと信号の流れを図９に示す。図において９０１はセンサユニット２出力読み取りタスクである。９１１−１および９１１−２はそれぞれセンサユニット２から得られる移動体の並進速度信号、回転速度信号であって、ｖ３obs，ω３obsに相当する。
センサユニット２出力読み取り・処理タスク９０１は移動体の並進速度信号ｖ３obsおよび回転速度信号ω３obsを逐次読み取り、時系列データとしてメモリ２０７に保存するタスクである。ジャイロセンサ１０４（図１）や加速度センサ１０５（図１）とは異なり、このセンサユニット２はビジョンセンサと画像処理装置を組み合わせた構成であるためバイアスノイズは信号中にほとんど含まれない。しかし、一般に信号へのホワイトノイズの重畳は避け難い。
【００３３】
実施例３におけるキャリブレーションの手順とステップ２，４における処理内容は実施例１と同様である。また、各ステップにおけるモータ回転速度指令出力タスク２０２の処理も、実施例１と同様である。
ステップ１において回転に関する運動学パラメターを求める処理は、実施例１においてωobsをω3obsに置き換えたものであるので、説明は割愛する。
ステップ３において運動学パラメター演算タスク２０６が並進に関する運動学パラメターＰｒＬおよびＰｔＲを求める処理内容は実施例１，２と異なるため、以下で説明する。
ステップ３では、モータ回転速度指令タスク２０２は時間［０，Ｔ］にわたって式（１０）のようなモータ回転速度指令を出力する。このときの移動体の並進速度は前述の通りであるが、その計測値であるｖ３obsにはホワイトノイズｄ3wが含まれる。
ｖ3obs＝ｖ＋ｄ3w＝（ｐｔＬ＋ｐｔＲ）Ａ’ｓｉｎ（αｔ＋δservo’）＋ｄ3w
センサユニット２出力読み取り・処理タスク８０１は、ｖ3obs の値を時間［ＴS，ＴS＋ｎ×２π／α］にわたってメモリ２０７に保存する。
運動学パラメター演算タスク２０６の処理は、指令出力および各種データの保存が完了した後に開始する。
まず、メモリ２０７に保存された左モータ回転速度ωｍＬまたは右モータ回転速度ωｍＲの時系列データから、その振幅Ａ’の値を求める。振幅を求める方法は実施例１と同様である。
次いで、メモリ２０７に保存されたｖ3obsの時系列データから、その振幅Ｂ３’を求める。振幅を求める方法は実施例１と同様である。ここでｖ3obsに含まれるホワイトノイズｄ3wの影響は小さく抑えられる。
得られる振幅Ｂ３’と振幅Ａ’との間には
Ｂ３’＝（ｐｔＬ＋ｐｔＲ）Ａ’
なる関係がある。また、式（３）から
ｐｔＬ＝（ｐｒＬ／ｐｒＲ）×ｐｔＲ
であるから、上記２式により並進に関する運動学パラメターｐｔＬ、ｐｔＲは、
ｐｔＬ＝ｐｒＬＢ３’／（Ａ’（ｐｒＬ＋ｐｒＲ））
ｐｔＲ＝ｐｒＲＢ３’／（Ａ’（ｐｒＬ＋ｐｒＲ））
のように求まる。以上で実施例３におけるステップ３０３の処理は完了である。
以上に示したように、本発明による移動体のキャリブレーション装置はさまざまな種類のセンサを用いて実現することができるのである。キャリブレーションにあたっては、何れの場合も移動体は初期位置付近で振動的に運動するため、移動体の走行する領域は限定される。よってキャリブレーションのために広大な領域を確保する必要はない。
【実施例４】
【００３４】
実施例１は２輪型の移動体をキャリブレーション対象としていたが、本発明はオドメトリによって位置推定を行っている移動体に広く適用できるものである。
本発明の実施例４として、図１０に示した全方向移動体のキャリブレーション装置についても説明する。
図１０において、１００２−１〜１００２−３はそれぞれ第１モータ〜第３モータ、１００３はエンコーダ、１００４は減速器、１００５−１〜３はそれぞれ第１オムニホイル〜第３オムニホイルである。第１車輪１〜第３車輪は、その回転軸が移動体１００１の中心で互いに交わるように、かつ互いのなす角が１２０°になるように設置する。
実施例４においても装置の基本構成は図１と同じである。ただし、加速度センサ１０５（図１）は図１０では全方向移動体１００１に固定した座標系におけるＸ軸方向およびＹ軸方向の並進加速度を計測できる２軸加速度センサとする。
計測される移動体加速度をａｘｏｂｓ、ａｙｏｂｓのように表す。また、キャリブレーションの手順は図３と異なるが、単純であるため図示はしない。
【００３５】
パラメターｐ１１を求める処理について以下で説明する。
モータ回転速度指令出力タスク２０２（図２）は、第１モータ１００２−１に対して振幅Ａｒｅｆ、位相角速度αの正弦波を、第２モータ１００２−２、第３モータ１００２−３に対して０を、モータ回転速度指令信号２１１（図２）として一定期間にわたって出力する。すなわち、時間［０、Ｔ］において、式（１１）とする。
【数１１】
ここでＡｒｅｆ，α，Ｔの各設定基準は、実施例１と同様である。
このときのモータ１００２−１，１００２−２，および１００２−３の回転速度は、式（１２）である。
【数１２】
このときの全方向移動体１００１（図１０）の並進および回転速度は、式（１３）となる。
【数１３】
ここでも移動体の走行領域は限定されており、広大な領域を確保する必要はない。移動体のｘ方向並進加速度は式（１４）であって、
【数１４】
その計測値ａxobsvには、一般に、バイアス誤差ｄＤＣｘとホワイトノイズｄｗｘが含まれる。
【数１５】
ａvxobsは加速度信号２１４（図２）に相当し、その値を加速度センサ出力読み取りタスク２０４（図２）によって時間［ＴＳ、ＴＳ＋ｎ×２π／α］にわたってメモリ２０７（図２）に保存する。
また、モータ回転速度指令出力タスク２０２（図２）、ジャイロセンサ出力読み取りタスク２０３（図２）、加速度センサ出力読み取りタスク２０４（図２）、エンコーダ信号読み取りおよび差分タスク２０５（図２）の処理内容は実施例１と同じである。
【００３６】
モータ回転速度指令の出力完了後、運動学パラメター演算タスク２０６（図２）の処理を開始する。
まず、メモリ２０７に保存されたモータ１回転速度ωｍ１の時系列データから、その振幅Ａ”の値を求める。振幅を求める方法は実施例１と同様である。
次いで、加速度センサ出力読み取りタスク２０４によって保存されたＸ方向加速度信号の時系列データから、ａｏｂｓの振幅Ｂ”を求める。振幅を求める方法は実施例１と同じである。
得られる振幅Ｂ”と振幅Ａ”との間には
Ｂ”＝αｐ１１Ａ”
なる関係があるため、
ｐ１１＝Ｂ”／（αＡ”）
の演算によってパラメターｐ１１を求める。
他の運動学パラメターも同様の手順で求めることができる。
このように、本発明の移動体のキャリブレーション装置は、オドメトリを適用する移動体の運動学パラメターを精度良く求めることができる。
【００３７】
また、本発明のキャリブレーション装置を構成する要素は全て移動体に搭載可能であって、キャリブレーション実施にあたって外部環境に別途設備を用意する必要はない。本キャリブレーション装置を予め搭載した自己キャリブレーション機能を有する移動体を提供することもできる。
【００３８】
簡単な構成の装置で移動体の運動学パラメターを精度よく求めることができ、かつキャリブレーション作業に要する手間と空間を大幅に削減できるため、新たに製造された移動ロボットや長期間使用したために運動学パラメターが変化してしまった移動ロボットなどの調整作業に効果的に用いることができる。
【図面の簡単な説明】
【００３９】
【図１】本発明の実施例１示す移動体のキャリブレーション装置の構成ブロック図である。
【図２】実施例１示す移動体のキャリブレーション装置におけるマイクロコンピュータ内部の処理タスクと信号の流れを示した図である。
【図３】実施例１おける移動体のキャリブレーション方法の手順を示した流れ図である。
【図４】移動体の回転速度とそのジャイロセンサによる計測値の波形例である。
【図５】平均波形から振幅値を求めることの効果を示した図である。
【図６】実施例１〜３が対象とする２輪型移動体を示した図である。
【図７】本発明の実施例２示す移動体のキャリブレーション装置におけるセンサユニット１の上視図である。
【図８】実施例２示す移動体のキャリブレーション装置におけるマイクロコンピュータ内部の処理タスクと信号の流れを示した図である。
【図９】実施例３示す移動体のキャリブレーション装置におけるマイクロコンピュータ内部の処理タスクと信号の流れを示した図である。
【図１０】実施例４が対象とする全方向移動体を示した図である。
【図１１】従来技術３において移動体が走行する経路の例を示した図である。
【符号の説明】
【００４０】
１０１ 移動体のキャリブレーション装置
１０２ 移動体
１０３ マイクロコンピュータ
１０４ ジャイロセンサ
１０５ 加速度センサ
１０６ ＡＤ変換器
１０７ ＤＡ変換器
１０８ サーボアンプ
１０９ エンコーダおよびモータ
１１０ 減速器および車輪
２０１ マイクロコンピュータ内部
２０２ モータ回転速度指令出力タスク
２０３ ジャイロセンサ出力読み取りタスク
２０４ 加速度センサ出力読み取りタスク
２０５ エンコーダ信号読み取りおよび差分タスク
２０６ 運動学パラメター演算タスク
２０７ マイクロコンピュータ１０３が内蔵するメモリ
２１１ モータ回転速度指令信号
２１２ サーボアンプから得られるエンコーダ信号
２１３ 移動体１０２の回転速度信号
２１４ 移動体１０２の前後方向加速度信号
２１５ 運動学パラメター信号
３０１ 回転に関する運動学パラメターを求めるステップ
３０２ 求めた回転に関する運動学パラメターを保存するステップ
３０３ 並進に関する運動学パラメターを求めるステップ
３０４ 求めた並進に関する運動学パラメターを保存するステップ
４０１ 時間軸
４０２ 回転速度軸
４０３ 移動体回転速度の波形
４０４ ジャイロセンサ１０４で計測した移動体回転速度の波形
４０５ 左モータ回転速度指令として出力した正弦波の1周期
５０１−１ ｎ分割したωｏｂｓの時系列データの1番目の波形
５０１−ｎ ｎ分割したωｏｂｓの時系列データのｎ番目の波形
５０２ ｎ個の波形から求めた1周期分の平均波形
５０３ 平均波形５０２の最大値と最小値との差を１／２倍(振幅)
６０１ ２輪型移動体
６０２ 右車輪
６０３ 左車輪
６０４ 右モータ
６０５ 左モータ
６０６ 減速器
７０１ センサユニット１
７０２−１ 第１の１軸加速度センサ
７０２−２ 第２の１軸加速度センサ
７０３−１ 第１の１軸加速度センサ７０２−１が計測する加速度の正方向
７０３−２ 第２の１軸加速度センサ７０２−２が計測する加速度の正方向
７０４ センサユニット１の中心線
７０５ センサユニット１の移動体正面に対する取り付け方向
８０１ センサユニット１出力読み取り・処理タスク
８１１−１ １軸加速度センサ７０２−１から得られる加速度信号
８１１−２ １軸加速度センサ７０２−２から得られる加速度信号
９０１ センサユニット２出力読み取りタスク
９１１−１ センサユニット２から得られる移動体の並進速度信号
９１１−２ センサユニット２から得られる移動体の回転速度信号
１００１ 全方向移動体
１００２−１ モータ１
１００２−２ モータ２
１００２−３ モータ３
１００３ エンコーダ
１００４ 減速器
１００５−１ オムニホイル１
１００５−２ オムニホイル２
１００５−３ オムニホイル３、
１１０１ 移動体
１１０２ 基準壁
１１０３ 移動体の移動開始点
１１０４ 移動完了点
１１０５ 事前にプログラムされた一辺が４ｍの正方形の経路
１１０６ 移動体１１０１が実際に走行した経路
【特許請求の範囲】
【請求項１】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項２】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項３】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項４】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項５】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項６】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項７】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項８】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項９】
前記演算処理部は、前記移動体の回転速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項１、５、又は７記載の移動体のキャリブレーション装置。
【請求項１０】
前記演算処理部は、前記移動体の回転加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項２、６又は８記載の移動体のキャリブレーション装置。
【請求項１１】
前記演算処理部は、前記移動体の並進加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項４、５又は６記載の移動体のキャリブレーション装置。
【請求項１２】
前記演算処理部は、前記移動体の並進速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項３、７又は８記載の移動体のキャリブレーション装置。
【請求項１３】
前記モータ回転速度指令部が出力するのは、一定角周波数の正弦波状の指令であることを特徴とする請求項９〜１２のいずれか１項記載の移動体のキャリブレーション装置。
【請求項１４】
前記振幅値取得部は、その入力信号の周波数解析を行って前記一定角周波数の振動振幅値を抽出する周波数解析部からなることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１５】
前記移動体の回転速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１６】
前記移動体の回転加速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１７】
前記移動体の並進加速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１８】
前記移動体の並進速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１９】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項９記載のキャリブレーション装置。
【請求項２０】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項１０記載のキャリブレーション装置。
【請求項２１】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項１１記載のキャリブレーション装置。
【請求項２２】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項１２記載のキャリブレーション装置。
【請求項２３】
前記振幅値取得部は、前記移動体の回転速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項９記載の移動体のキャリブレーション装置。
【請求項２４】
前記振幅値取得部は、前記移動体の回転加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項１０記載の移動体のキャリブレーション装置。
【請求項２５】
前記振幅値取得部は、前記移動体の並進加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項１１記載の移動体のキャリブレーション装置。
【請求項２６】
前記振幅値取得部は、前記移動体の並進速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項１２記載の移動体のキャリブレーション装置。
【請求項２７】
前記モータ回転速度は、前記モータ回転速度指令で代替されることを特徴とする請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置。
【請求項２８】
請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置を搭載したことを特徴とする移動体。
【請求項２９】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うためのキャリブレーション方法であって、
はじめにモータ回転速度とセンサから得られる前記移動体の回転速度とを用いて移動体の回転に関する第１の運動学パラメターを求め、
次いで、求まった第1の運動学パラメターと、モータ回転速度と、センサから得られる前記移動体の並進加速度あるいは並進速度と、を用いて移動体の並進に関する第２の運動学パラメターを求めること
を特徴とする移動体のキャリブレーション方法。
【請求項１】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項２】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項３】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項４】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項５】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項６】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進加速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進加速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項７】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項８】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うキャリブレーション装置であって、モータ回転速度計測部と、移動体の回転加速度計測部と、移動体の並進速度計測部と、車輪を駆動するモータの回転速度指令を出力するモータ回転速度指令部と、前記モータ回転速度計測部の出力値と前記移動体の回転加速度計測部の出力値と前記移動体の並進速度計測部の出力値とから前記移動体の運動学パラメターを演算する演算処理部と、を備えたことを特徴とする移動体のキャリブレーション装置。
【請求項９】
前記演算処理部は、前記移動体の回転速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項１、５、又は７記載の移動体のキャリブレーション装置。
【請求項１０】
前記演算処理部は、前記移動体の回転加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項２、６又は８記載の移動体のキャリブレーション装置。
【請求項１１】
前記演算処理部は、前記移動体の並進加速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項４、５又は６記載の移動体のキャリブレーション装置。
【請求項１２】
前記演算処理部は、前記移動体の並進速度計測部の出力値の振幅値を得る振幅値取得部を備え、前記モータ回転速度指令部は正負対称の定常波指令を一定期間にわたって出力し、前記演算処理部は前記モータ回転速度の振幅値と前記振幅値取得部で得られた振幅値との比から前記移動体の運動学パラメターを計算することを特徴とする請求項３、７又は８記載の移動体のキャリブレーション装置。
【請求項１３】
前記モータ回転速度指令部が出力するのは、一定角周波数の正弦波状の指令であることを特徴とする請求項９〜１２のいずれか１項記載の移動体のキャリブレーション装置。
【請求項１４】
前記振幅値取得部は、その入力信号の周波数解析を行って前記一定角周波数の振動振幅値を抽出する周波数解析部からなることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１５】
前記移動体の回転速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１６】
前記移動体の回転加速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１７】
前記移動体の並進加速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１８】
前記移動体の並進速度計測部は、前記一定角周波数に近い振動成分のみを通過するバンドパスフィルタを有していることを特徴とする請求項１３記載の移動体のキャリブレーション装置。
【請求項１９】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項９記載のキャリブレーション装置。
【請求項２０】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の回転加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項１０記載のキャリブレーション装置。
【請求項２１】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進加速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項１１記載のキャリブレーション装置。
【請求項２２】
前記振幅値取得部は、前記定常波指令の1周期に相当する時間にわたって、前記移動体の並進速度の最大値から最小値を減じた値を１／２倍した値である1周期振幅値を出力することを特徴とする請求項１２記載のキャリブレーション装置。
【請求項２３】
前記振幅値取得部は、前記移動体の回転速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項９記載の移動体のキャリブレーション装置。
【請求項２４】
前記振幅値取得部は、前記移動体の回転加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項１０記載の移動体のキャリブレーション装置。
【請求項２５】
前記振幅値取得部は、前記移動体の並進加速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項１１記載の移動体のキャリブレーション装置。
【請求項２６】
前記振幅値取得部は、前記移動体の並進速度計測部の複数周期にわたる所定時間内の平均的な波形値を求め、その最大値から最小値を減じた値を１／２倍した値を出力することを特徴とする請求項１２記載の移動体のキャリブレーション装置。
【請求項２７】
前記モータ回転速度は、前記モータ回転速度指令で代替されることを特徴とする請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置。
【請求項２８】
請求項１〜８のいずれか１項記載の移動体のキャリブレーション装置を搭載したことを特徴とする移動体。
【請求項２９】
１つ以上のモータによって駆動される車輪によって走行する移動体の運動学パラメターのキャリブレーションを行うためのキャリブレーション方法であって、
はじめにモータ回転速度とセンサから得られる前記移動体の回転速度とを用いて移動体の回転に関する第１の運動学パラメターを求め、
次いで、求まった第1の運動学パラメターと、モータ回転速度と、センサから得られる前記移動体の並進加速度あるいは並進速度と、を用いて移動体の並進に関する第２の運動学パラメターを求めること
を特徴とする移動体のキャリブレーション方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２００８−２８４６（Ｐ２００８−２８４６Ａ）
【公開日】平成２０年１月１０日（２００８．１．１０）
【国際特許分類】
【出願番号】特願２００６−１７０３６７（Ｐ２００６−１７０３６７）
【出願日】平成１８年６月２０日（２００６．６．２０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２０年１月１０日（２００８．１．１０）
【国際特許分類】
【出願日】平成１８年６月２０日（２００６．６．２０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]