JP5267324B2 - 移動体、補正値の算出方法、及びプログラム - Google Patents

Description

本発明は、移動体、補正値の算出方法、及びプログラムに関する。

近年、ロボット技術の進展を背景として、ヒトの空間移動に用いられるロボットに係る技術も飛躍的に高度化している。移動体として活用されるロボットの筐体には、各種センサ、マイコン、電源、及びリンク機構等の様々な電気／機械コンポーネントが内蔵される。

このような移動型ロボットでは、移動時の安定性／安全性が強く要求される。ロボットの移動時の安全性／安定性を確保するために、各種センサの入力に基づいて、ロボットの姿勢制御を高精度に実行することが強く要求される。一般的に、移動体の姿勢制御には、角速度センサ（以下、ジャイロセンサと呼ぶこともある）及び加速度センサが用いられる。移動体の低コスト化を図るためには安価なジャイロセンサを活用することが要求される。

なお、安価なジャイロセンサでは、ＭＥＭＳ(Micro-Electro Mechanical Systems)が活用されている。具体的には、半導体プロセス技術を活用して、半導体、ガラス等の基板上に３次元構造を高精細に形成する。このようなジャイロセンサでは、一般的に、振動体の空間的な変位を電気的に検出することによって、ジャイロセンサが受けるコリオリの力を電気的に検出する。例えば、振動体を振動状態とし、振動体と検出電極間の容量値の変化を検出することによって、コリオリの力を電気的に検出する。なお、ジャイロセンサの具体的な構造、コリオリの力の検出方法は様々なタイプのものが開発されている。

特許文献１には、倒立車輪型の走行体が開示されており、特に、倒立車輪型の走行体の車体を後方に大きく傾斜させないでも急制動させる技術が開示されている。なお、特許文献２には、車椅子に関して、急傾斜地域や不整地でも安全且つ快適な姿勢を確保する技術が開示されている。

特開２００７−２０３９６５号公報 特開２０００−１１６７１８号公報

上述のように、移動体の低価格化を図るためには、安価なジャイロセンサを採用することが要求される。しかしながら、安価なジャイロセンサでは、その出力値が経時的に劣化してしまう場合がある。例えば、ジャイロセンサ内に組み込まれた振動体の振動の影響によって、ジャイロセンサ内の部品同士が部分的に磨耗し、ジャイロセンサの出力値に誤差が生じてしまうおそれがある。また、ロボットの筐体内に組み込まれた他の部品（例えば、熱源等）の影響によって、ジャイロセンサの出力値に誤差が生じてしまう場合がある。ジャイロセンサの出力値が劣化すると、移動体ロボットの姿勢制御を高精度に実現することが阻害されてしまう。

上述の説明から明らかなように、加速度センサの出力値に生じうる誤差の影響を低減して、移動体を高精度に制御することが強く要求されている。

本発明に係る移動体は、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体であって、前記本体部の角速度を検出する検出部と、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部と、前記検出部及び前記計測部の各出力に基づいて予め算出された補正値に応じて前記検出部の出力値に含まれる誤差を補正した条件で、前記車輪部と前記本体部間の相対位置を制御するための指令を生成する指令生成部と、を備える。この構成を採用することによって、加速度検出部の出力値に生じうる誤差の影響を低減して、移動体を高精度に制御することができる。

前記計測部の前記出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記検出部の前記出力値の変動を評価して前記補正値を算出する補正値算出部を更に備える、と良い。

前記補正値算出部は、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に取得した前記検出部及び前記計測部の各出力に基づいて前記補正値を算出する、と良い。

前記補正値算出部は、少なくとも、前記本体部の慣性モーメント、前記車輪部と前記本体部間を相対的に移動させる駆動源の特性値、前記駆動源により生じるトルク、及び前記計測部の出力値から定まる角度値に基づいて、前記本体部に与えられる外力を推定する、と良い。

前記補正値算出部は、算出した前記外力の値が零になるタイミングを前記本体部が鉛直方向上に位置するタイミングとして検出する、と良い。

前記検出部は、前記本体部の加速度も検出し、前記指令生成部は、前記センサの出力に基づいて、前記本体部の姿勢を評価して前記本体部の姿勢角度を算出する第１演算部と、前記第１演算部及び前記計測部の各出力に基づいて、前記指令を生成する第２演算部と、を備える、と良い。

前記補正値算出部は、算出した前記タイミングに基づいて、前記第１演算部の出力値の変動を評価して前記補正値を算出する、と良い。

前記検出部は、コリオリの力を検出する機構を備える、と良い。

前記補正値を保持する補正値保持部を更に備える、と良い。

前記本体部は、実質的に一定の速度で前記車輪部を回転中心として回転する、と良い。

本発明に係る移動体は、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体であって、前記本体部の角速度を検出する検出部と、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部と、前記計測部の出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記検出部の出力値の変動を評価して、前記検出部の出力値に含まれる誤差を補正するための補正値を算出する補正値算出部と、を備える。

本発明に係る補正値の算出方法は、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれた角速度センサ部の出力値を補正するための補正値の算出方法であって、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に前記角速度センサ部の出力を取得し、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力を取得し、前記計測部の出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して、前記角速度センサ部の出力値を補正するための補正値を算出する。

本発明に係るプログラムは、車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれる角速度センサ部の出力値を補正するための補正値をコンピュータに算出させるプログラムであって、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力値に基づいて、前記本体部が鉛直方向上に位置するタイミングを算出させ、算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して前記補正値を算出させる。

本発明によれば、加速度センサの出力値に生じうる誤差の影響を低減して、移動体をより高精度に制御することができる。

本発明の第１実施形態にかかる移動体の概略的な模式図である。 本発明の第１実施形態にかかる移動体の概略的なブロック図である。 本発明の第１実施形態にかかる移動体の通常動作を示す説明図である。 本発明の第１実施形態にかかる移動体の姿勢制御を示す説明図である。 本発明の第１実施形態にかかる移動体のオフセット値算出動作を示す概略的なフローチャートである。 本発明の第１実施形態にかかる移動体のオフセット値算出動作時に用いられる治具の概略的な斜視図である。 本発明の第１実施形態にかかる移動体のオフセット値算出動作を示す概略的な模式図である。 本発明の第１実施形態にかかる移動体のエンコーダ部の出力値の時間変動を示す模式図である。 本発明の第１実施形態にかかる移動体が算出した外力値の時間変動を示す模式図である。 本発明の第１実施形態にかかる移動体が算出した姿勢角度の時間変動を示す模式図である。 本発明の第１実施形態にかかる移動体の補正値の算出方法を示す概略的な模式図である。

以下、図面を参照しつつ、本発明の実施の形態について説明する。なお、各実施の形態は、説明の便宜上、簡略化されている。図面は簡略的なものであるから、図面の記載を根拠として本発明の技術的範囲を狭く解釈してはならない。図面は、もっぱら技術的事項の説明のためのものであり、図面に示された要素の正確な大きさ等は反映していない。同一の要素には、同一の符号を付し、重複する説明は省略するものとする。上下左右といった方向を示す言葉は、図面を正面視した場合を前提として用いるものとする。

［第１実施形態］
以下、図１乃至図１１を参照して本発明の第１実施形態について説明する。図１は、移動体の概略的な模式図である。図２は、移動体の概略的なブロック図である。図３は、移動体の通常動作を示す説明図である。図４は、移動体の姿勢制御を示す説明図である。図５は、オフセット値算出動作を示す概略的なフローチャートである。図６は、オフセット値算出動作時に用いられる治具の概略的な斜視図である。図７は、オフセット値算出動作を示す概略的な模式図である。図８は、エンコーダ部の出力値の時間変動を示す模式図である。図９は、移動体が算出した外力の時間変動を示す模式図である。図１０は、移動体が算出した姿勢角度の時間変動を示す模式図である。図１１は、補正値の算出方法を示す概略的な模式図である。

図１に示すように、倒立二輪型ロボット（以下、単にロボットと呼ぶこともある）１００は、ヒトが搭乗する移動体であり、本体部５０、及び一組の車輪（車輪部）５１を有する。本体部５０は、起立部５０ａ、着座部５０ｂ、及び足台５０ｃを有する。一組の車輪５１は、車輪５１ａ、及び車輪５１ｂからなる。

本体部５０には、各種センサ、マイコン、電源、及びリンク機構等の様々な電気／機械コンポーネントが内蔵される。着座部５０ｂは、搭乗者が着座する部分である。起立部５０ａは、搭乗者の背中を受け止める部分である。足台５０ｃは、搭乗者の足が置かれる部分である。車輪５１ａ、車輪５１ｂは、共通する軸線ＡＸ上に配置される。車輪５１ａの回転軸は、軸線ＡＸに一致する。車輪５１ｂの回転軸は、軸線ＡＸに一致する。

図１に模式的に示すように、ロボット１００は、モータ部５３、コントローラ５４、電源５５、及び入力部５６を有する。モータ部５３は、車輪５１ａ、車輪５１ｂに対応して、モータ５３ａ、及びモータ５３ｂを有する。モータ５３ａ、５３ｂの回転軸は、軸線ＡＸに一致するように配置されている。電源５５の出力は、コントローラ５４に供給される。また、電源５５の出力は、必要に応じて、入力部５６に供給される。コントローラ５４の出力は、モータ部５３に接続される。

コントローラ５４は、入力部５６からの入力値に応じて算出した指令値をモータ部５３に供給する。モータ５３ａは、コントローラ５４から供給される指令値に応じて、車輪５１ａを回転させる。モータ５３ｂも、同様に、コントローラ５４から供給される指令値に応じて、車輪５１ｂを回転させる。

なお、入力部５６は、角速度センサ、加速度センサ、操作子等の入力装置に相当する。また、モータ５３ａ、モータ５３ｂは、コントローラ５４によって個別に制御される。車輪５１ａ、５１ｂの回転を個別に制御可能とすることによって、操縦者は、倒立二輪型ロボット１００に搭乗した状態で空間内を任意の方向へ移動することができる。

倒立二輪型ロボット１００の具体的な構成は任意である。車輪５１は複数である必要はない。本体部５０と車輪５１間の連結方法は任意である。モータ５３ａ、５３ｂの配置位置は任意である。

図２を参照して、ロボット１００の概略的なブロック図を示す。図２に示すように、ロボット１００は、センサ部（検出部）１０、エンコーダ部（計測部）１５、指令値算出部（指令生成部）２０、オフセット値算出部（補正値算出部）３０、オフセット値格納部（補正値格納部）３５、及び駆動部４０を有する。なお、センサ部１０、及びエンコーダ部１５は、図１の入力部５６に含まれる。駆動部４０は、図１のモータ部５３に相当する。指令値算出部２０は、図１のコントローラ５４に含まれる。なお、指令値算出部２０、及びオフセット値算出部３０は、ハードウェア／ソフトウェアのいずれでも実現可能である。より好適には、ハードディスク等のストレージに格納したプログラムをＣＰＵ(Central Processing Unit)で実行することによって、指令値算出部２０及びオフセット値算出部３０の機能を実現すると良い。

センサ部１０は、角速度センサ部１１、及び加速度センサ部１２を有する。エンコーダ部１５は、右輪エンコーダ１６、及び左輪エンコーダ１７を有する。指令値算出部２０は、姿勢角度演算部（第１演算部）２１、及び倒立制御演算部（第２演算部）２２を有する。オフセット値算出部３０は、外力推定実行部３１、及びオフセット値演算実行部３２を有する。駆動部４０は、右輪駆動部４１、及び左輪駆動部４２を有する。なお、角速度センサ部１１は、ジャイロセンサと呼ばれることもある。

はじめに、上述の各要素の接続関係について説明する。角速度センサ部１１の出力は、姿勢角度演算部２１に接続される。加速度センサ部１２の出力は、姿勢角度演算部２１に接続される。姿勢角度演算部２１の出力は、倒立制御演算部２２に接続される。倒立制御演算部２２の出力は、右輪駆動部４１及び左輪駆動部４２に接続される。右輪エンコーダ１６の出力は、倒立制御演算部２２に接続され、また、外力推定実行部３１に接続される。左輪エンコーダ１７の出力は、倒立制御演算部２２に接続され、また、外力推定実行部３１に接続される。外力推定実行部３１の出力は、オフセット値演算実行部３２に接続される。また、オフセット値演算実行部３２には、姿勢角度演算部２１の出力も接続される。オフセット値演算実行部３２の出力は、オフセット値格納部３５に接続される。オフセット値格納部３５の出力は、姿勢角度演算部２１に接続される。

図３に示すように、ロボット１００は、センシング（ｓ１０１）、指令生成（ｓ１０２）、ドライブ（ｓ１０３）といった動作を順次実行する。具体的には、次のとおりである。

センサ部１０は、所定のタイミングでセンシングする（ｓ１０１）。角速度センサ部１１は、角速度を取得する。加速度センサ部１２は、加速度を取得する。エンコーダ部１５は、車輪５１の回転量を取得する。次に、指令値算出部２０は、角速度センサ部１１、加速度センサ部１２、及びエンコーダ部１５の出力値に応じて指令値を算出する（ｓ１０２）。次に、駆動部４０は、指令値算出部２０からの出力値に応じて、車輪５１ａ、５１ｂを個別に回転制御する（ｓ１０３）。ドライブ後のロボット１００は、上述の説明と同様にして、センシング、指令生成、ドライブといった各動作を実行する。

このようなフィードバック動作を繰り返すことによって、ロボット１００は、例えば、倒立状態を維持することができる。図４に模式的に示すように、ロボット１００は、倒立状態を維持するために、ロボット１００の固定軸ＡＸ２が鉛直方向に沿う軸線になす角度θ１の値を零（θ１＝０°）となるように指令値を生成する。なお、操縦者が入力した方向指示を考慮して指令値を算出すれば、操縦者の意図した位置へロボット１００を移動させることもできる。

本実施形態では、角速度センサ部１１は、ＭＥＭＳ(Micro-Electro Mechanical Systems)を活用したセンサである。具体的には、角速度センサ部１１は、半導体プロセス技術を活用して、半導体、ガラス等の基板上に３次元構造を高精細に形成して製造される。角速度センサ部１１は、基板上に配置された振動体の空間的な変位を電気的に検出することによって、ジャイロセンサが受けるコリオリの力を電気的に検出する。例えば、角速度センサ部１１は、振動体を振動状態とし、振動体と検出電極間の容量値の変化を検出することによって、コリオリの力を電気的に検出する。なお、ジャイロセンサの具体的な構造、コリオリの力の検出方法は任意である。

ロボット１００の高精度な姿勢制御を実現するためには、ロボット１００の固有軸ＡＸ２が鉛直方向に沿う軸線に一致したとき、角速度センサ部１１の出力値は零に示すことが要求される。しかしながら、様々な要因によって、角速度センサ部１１の出力値が経時的に劣化してしまう場合がある。角速度センサ部１１の出力値に誤差が含まれると、ロボット１００が鉛直方向に起立していると判断しているとき、実際には、鉛直方向上の位置からみて本体部５０が斜め前又は斜め後ろに傾いた状態にあるおそれがある。このような場合、搭乗者は、ロボット１００に搭乗して移動する際に、身体的なストレスを感じたり、自身の安定性を維持すること自体が難しくなってしまったりするおそれがある。また、加速度センサ部１２の出力値が経時的に劣化することによって、ロボット１００を定期的にメンテナンスする必要性が高まってしまうおそれがある。角速度センサ部１１の交換が前提になると、ロボット１００の管理コストが増大してしまうおそれがある。

本実施形態では、これらの点に鑑みて、角速度センサ部１１の出力値を補正するための補正値を算出する。そして、算出した補正値に基づいて、角速度センサ部１１の出力値に含まれる誤差の影響を低減する。具体的には、角速度センサ部１１の出力値が補正値によって補正された条件で、駆動部４０に伝達される指令値を算出する。これによって、角速度センサ部１１の出力値に含まれる誤差によってロボット１００の姿勢制御の精度が劣化してしまうことを抑制することができる。以下、上述の点を踏まえ、ロボット１００の構成及び動作についてより具体的に説明する。

角速度センサ部１１は、上述のように、ＭＥＭＳを活用したセンサであり、角速度を検出して、これに応じた値を出力する。なお、角速度センサ部１１は、アナログ値をデジタル値に変換してからデジタル値を出力するものとする。また、角速度センサ部１１は、ロール、ピッチ、ヨーの３軸方向の回転を角速度として検出するものとする。

加速度センサ部１２は、角速度センサ部１１と同様に、ＭＥＭＳを活用したセンサであり、加速度を検出して、これに応じた値を出力する。なお、加速度センサ部１２は、アナログ値をデジタル値に変換し、Ａ／Ｄ変換により生成したデジタル値を出力する。また、加速度センサ部１２は、ロール、ピッチ、ヨーの３軸方向の加速度を検出するものとする。加速度センサ部１２の原理は当業者にとって周知であり、説明は省略する。

姿勢角度演算部２１は、角速度センサ部１１及び加速度センサ部１２から伝達される角速度値及び加速度値に基づいて姿勢角度を演算する。周知のように、加速度値の積分によって角度は算出される。加速度値の積分によって速度は算出される。ロボット１００の進行方向は、加速度の値から算出される。姿勢角度演算部２１は、これらの値から姿勢角度を算出する。姿勢角度演算部２１によって現時点の姿勢角度が出力される点を考慮すると、センサ部１０及び姿勢角度演算部２１を包括してジャイロセンサユニットと把握することもできる。

なお、姿勢角度とは、鉛直方向に沿う軸線に対してロボット１００の固有軸がなす角度を示し、図４に示すθ１に相当する。なお、図４では、ｙ軸は鉛直方向に沿う軸線であり、軸線ＡＸ２がロボット１００の固有軸に相当する。ロボット１００は、固有軸に対応するフレーム棒５２を有する。なお、姿勢角度の定義方法は任意であり、これを他の方法によって定義しても良い。

本実施形態では、姿勢角度演算部２１は、オフセット値格納部３５に格納されたオフセット値に応じて、角速度センサ部１１の出力値に含まれる誤差を補正した条件で、姿勢角度を算出する。例えば、角速度センサ部１１の出力値を積分して算出した角度値からオフセット値を減算する。これによって、角速度センサ部１１の出力値に生じる誤差によって、ロボット１００の姿勢制御の精度が劣化することを抑制することができる。なお、オフセット値格納部３５にオフセット値が格納されていないとき、姿勢角度演算部２１は、オフセット値を考慮せずに、上述のように角速度値及び加速度値に基づいて姿勢角度を演算する。

倒立制御演算部２２は、姿勢角度演算部２１で算出した姿勢角度、及びエンコーダ部１５からの出力値に基づいて、ロボット１００を制御するための指令値を演算する。なお、倒立制御演算部２２は、操縦者による操作子の出力値に応じて、ロボット１００を前進等させることもできる。

倒立制御演算部２２は、姿勢角度演算部２１の出力に基づいて、ロボット１００の現在の姿勢を検出する。倒立制御演算部２２は、エンコーダ部１５の出力に基づいて、車輪５１の回転方向における本体部５０と車輪５１間の相対的な位置変化を検出する。

倒立制御演算部２２の具体的な演算手順は任意である。例えば、倒立制御演算部２２は、予め設定された目標値にセンサ部１０の出力値が近付くように指令値を生成する。同様に、倒立制御演算部２２は、予め設定された目標値にエンコーダ部１５の出力値が近付くように指令値を生成する。具体例を挙げると、静止して倒立状態を維持するため、倒立制御演算部２２は、角速度センサ部１１の出力値＝０、右輪エンコーダ１６、左輪エンコーダ１７の出力値＝０となるように指令値を生成する。また、姿勢角度演算部２１で算出した現在の姿勢角度が、図４に示すような場合、倒立制御演算部２２は、車輪５１ａ、５１ｂを後ろに回転させる制御を実行する。また、倒立して前方に進行するためには、倒立制御演算部２２は、角速度センサ部１１の出力値＝０、右輪エンコーダ１６、左輪エンコーダ１７の出力値＝整数となるように指令値を生成する。なお、上述のように倒立制御演算部２２の具体的な演算方法は任意であり、例えば、倒立制御演算部２２は、姿勢角度演算部２１、エンコーダ部１５からの入力値を所定の関係式に入力することによって指令値を算出する。

右輪駆動部４１は、倒立制御演算部２２から伝達する指令値に応じて、任意の方向に任意の回転量だけ右車を回転させる。左輪駆動部４２も、右輪駆動部４１と同様に、倒立制御演算部２２から伝達する指令値に応じて、任意の方向に任意の回転量だけ左輪を回転させる。

右輪エンコーダ１６は、右輪の相対的な回転量を検出し、回転量に応じたカウント値を出力する。右輪エンコーダ１６の具体的な構成は任意である。例えば、光学式又は磁気式エンコーダのいずれかを採用しても良い。光学式エンコーダの場合、右輪エンコーダ１６は、車輪に設けられた回転体の外周領域に形成されたスリットの数を光学的に検出し、スリット数に応じたカウント値を出力する。磁気式エンコーダの場合、右輪エンコーダ１６は、上述の回転体の外周領域に順次配置された磁石の個数を磁気的に検出し、磁石数に応じたカウント値を出力しても良い。

左輪エンコーダ１７は、右輪エンコーダ１６と同様である。ただし、右輪の回転量に変えて、左輪の回転量を検出する。

オフセット値算出部３０は、後述の説明から明らかなように、エンコーダ部１５から出力されるカウント値に基づいて本体部５０が受ける外力の変化を推定し、この外力が零になるタイミングで姿勢角度演算部２１の出力の変動を評価してオフセット値を算出する。オフセット値格納部３５は、オフセット値算出部３０が算出したオフセット値を格納する。オフセット値格納部３５は、レジスタ等の一般的な記憶素子である。

次に、図５乃至図１１を参照して、オフセット値の算出方法について説明する。これらの説明からオフセット値算出部３０の動作は明らかになる。

なお、上述のように、安価な角速度センサ部１１では、経時的に出力値に誤差が生じてしまうおそれがある。オフセット値は、角速度センサ部１１の出力値の誤差による悪影響を回避するために算出される。従って、通常、オフセット値を算出する時点は、ロボット１００の出荷後、相当期間が経過した後になる。但し、ロボット１００の製造直後にオフセット値を求め、ロボット１００の初期不良の有無を検査しても良い。つまり、オフセット値を算出する時点は任意である。

図５に示すように、まず初期設定をする（ｓ２０１）。具体的には、図６に示す治具６０によって図７に模式的に示すようにロボット１００を固定する。

図６に示すように、治具６０は、ベース板６１、柱６２、及び凸部６３を有する。治具６０は、例えば、金属等の成形により製造される。

図７に模式的に示すように、一対の治具６０によって、ロボット１００の車輪５１を固定する。なお、車輪５１には、凸部６３を受け入れる凹部が設けられているものとする。なお、凸部６３を角柱状に形成し、これに対応する凹部を車輪５１に設けることによって、治具６０と車輪５１とを嵌め合いにより位置決めさせても良い。この場合には、車輪５１を確実に固定状態にすることができる。

次に、回転指示をする（ｓ２０２）。具体的には、例えばタイミングコントローラ等の指示に応じて、倒立制御演算部２２は指令を生成し、駆動部４０にトルクを生じさせる。この状態で、ヒトが本体部５０から手を離すと、本体部５０は、図７に模式的に示すように、最も後方に傾斜した位置から最も前方に傾斜した位置へ車輪５１を回転中心として回転する。なお、本実施形態では、本体部５０の回転速度が一定となるように、倒立制御演算部２２は指令を生成するものとする。

次に、センシングをする（ｓ２０３）。具体的には、回転指示の直後、タイミングコントローラの指示に応じて、角速度センサ部１１は、センシングを開始する。加速度センサ部１２も同様である。右輪エンコーダ１６、及び左輪エンコーダ１７も同様である。

次にオフセット値の算出処理を実行する（ｓ２０４）。具体的には、次のとおりである。

まず、外力推定実行部３１は、右輪エンコーダ１６及び左輪エンコーダ１７の一方または両方から出力されるカウント値に基づいて、次の式（１）により外力を推定する。ただし、各符号は、次のとおりである。τｅは、駆動部４０に発生するトルクである。Ｉは、本体部５０の慣性モーメントである。ｑは、エンコーダ部１５の出力値から算出された角度値である。Ｄは、モータの粘性係数（駆動部４０の固有特性値）である。τａは、外力（外力トルク）である。

エンコーダ部１５から出力されるカウント値は、図８に示すように時間軸に沿って一定の割合で増加する。これは、上述のように、センシング期間、車輪５１を回転中心として本体部５０は一定の速度で回転するためである。各時点におけるカウント値から上述の角度値ｑが定まる。なお、角度値は、相対的な角度情報である。

駆動部に発生するトルクτｅは、倒立制御演算部２２で生成される指令値から算出される。本体部５０の慣性モーメントＩ、モータの粘性係数Ｄは、予め既知である。

上述の値を式（１）に代入して算出処理することによって、外力τａが算出される。なお、カウント値は、時系列順に式（１）に代入するものとする。これによって外力τａの時間変化を計算することができる。つまり、図８に示すカウント値の時間変化に基づいて、図９に示す外力の時間変化を検出することができる。

外力の推定後、オフセット値を算出する。上述の外力の推定処理によって、図９に示す外力の時間変化を検出することができる。外力の値が零のとき、本体部５０は、鉛直方向に沿う軸線上に位置することが判定できる。

他方、上述のセンシング時、図１０に示すように角速度センサ部１１及び加速度センサ部１２の出力値に基づいて、姿勢角度演算部２１は、姿勢角度を算出している。そして、姿勢角度演算部２１の出力値は、オフセット値演算実行部３２に供給される。

オフセット値演算実行部３２は、図１１に模式的に示すように、外力推定実行部３１の出力値から外力が零になる時刻ｔ１を算出し、また、時刻ｔ１時の姿勢角度演算部２１の出力値を算出する。オフセット値演算実行部３２は、算出した時刻ｔ１時の姿勢角度演算部２１の出力値（図１１では、−５度）をオフセット値とする。このようにして、エンコーダ部１５及びセンサ部１０の各出力値に基づいて、オフセット値が算出される。なお、オフセット値の算出方法は任意である。外力τａが零になる時刻と姿勢角度が０度になる時刻間の差分の算出に基づいてオフセット値を算出しても良い。

次に、オフセット値を格納する（ｓ２０５）。具体的には、オフセット値演算実行部３２は、算出したオフセット値をオフセット値格納部３５へ転送する。オフセット値格納部３５は、オフセット値演算実行部３２から伝達したオフセット値を保持する。オフセット値格納部３５にオフセット値が格納されることによって、姿勢角度演算部２１は、上述のように、オフセット値によって角速度センサ部１１の出力値に含まれる誤差を補正した条件で姿勢角度を算出する。

上述の説明から明らかなように、本実施形態では、角速度センサ部１１の出力値が経時的に劣化するおそれがあることに鑑みて、角速度センサ部１１に含まれる誤差を補正した条件で指令を生成して駆動部を駆動する。これによって、安価な角速度センサ部１１を採用することができ、ロボット１００の低コスト化を図ることができる。

また、本実施形態では、鉛直方向に沿う軸線上に本体部５０が位置するときに外力の値が零に近付くことに鑑みて、エンコーダ部１５の出力値に基づいて外力を推定し、外力が零になる時刻に基づいて姿勢角度の時間変化を評価してオフセット値を算出する。これによって、既存のエンコーダ部１５を活用して、オフセット値を算出することができ、別途新たなハードウェアを用意する必要性を解消することができる。この点は、コンパクト性も要求される倒立二輪型の移動体においては特に有利である。なお、姿勢角度が角速度に基づいて定まる点を考慮すれば、外力が零になる時刻に基づいて角速度センサ部１１の出力値の変動を評価してオフセット値を算出する、とも把握することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。角速度センサ部１１の具体的な構造は任意である。加速度センサ部１２の具体的な構造も任意である。指令値算出部２０の具体的な算出方法は任意である。オフセット値算出部３０の具体的な算出方法は任意である。オフセット値格納部３５は、着脱可能なメモリデバイスであっても良い。オフセット値算出部３０をロボット１００に組み込む必要はなく、それを外部のパソコンに格納し、外部のパソコンで実行しても良い。本体部５０と車輪５１間は、複数関節のリンク機構を介して接続しても良い。タイミングコントローラの搭載は任意である。オフセット値の算出動作を複数回実行し、これらの平均値を算出し、これをオフセット値格納部３５に格納しても良い。また、上述の説明によれば、補正値をコンピュータに算出させるプログラムの内容も自明である。

１００ 倒立二輪型ロボット

１０ センサ部
１１ 角速度センサ部
１２ 加速度センサ部

１５ エンコーダ部
１６ 右輪エンコーダ
１７ 左輪エンコーダ

２０ 指令値算出部
２１ 姿勢角度演算部
２２ 倒立制御演算部

３０ オフセット値算出部
３１ 外力推定実行部
３２ オフセット値演算実行部

３５ オフセット値格納部

４０ 駆動部
４１ 右輪駆動部
４２ 左輪駆動部

５０ 本体部
５０ａ 起立部
５０ｂ 着座部
５０ｃ 足台

５１ 車輪
５１ａ 車輪
５１ｂ 車輪

５３ モータ部
５４ コントローラ
５５ 電源
５６ 入力部

６０ 治具
６１ ベース板
６２ 柱
６３ 凸部

Claims (14)

1. 車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体であって、
   前記本体部の角速度を検出する検出部と、
   前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部と、
   前記計測部の出力値に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、算出した当該タイミングに基づいて、前記検出部の出力値の変動を評価して、前記検出部の出力値にふくまれる誤差を補正するための補正値を算出する補正値算出部と、
   を備える移動体。
2. 前記補正値に応じて前記検出部の出力値に含まれる誤差を補正した条件で、前記車輪部と前記本体部間の相対位置を制御するための指令を生成する指令生成部を更に備えることを特徴とする請求項１に記載の移動体。
3. 前記補正値算出部は、固定状態の前記車輪部を回転中心として前記本体部が回転する期間に取得した前記検出部及び前記計測部の各出力に基づいて前記補正値を算出することを特徴とする請求項１又は２に記載の移動体。
4. 前記補正値算出部は、少なくとも、前記本体部の慣性モーメント、前記車輪部と前記本体部間を相対的に移動させる駆動源の特性値、前記駆動源により生じるトルク、及び前記計測部の出力値から定まる角度値に基づいて、前記本体部に与えられる外力を推定することを特徴とする請求項３に記載の移動体。
5. 前記補正値算出部は、算出した前記外力の値が零になるタイミングを前記本体部が鉛直方向上に位置するタイミングとして検出することを特徴とする請求項４に記載の移動体。
6. 前記検出部は、前記本体部の加速度も検出し、
   前記指令生成部は、前記検出部の出力値に基づいて、前記本体部の姿勢を評価して前記本体部の姿勢角度を算出する第１演算部と、前記第１演算部及び前記計測部の各出力に基づいて、前記指令を生成する第２演算部と、を備えることを特徴とする請求項２に記載の移動体。
7. 前記補正値算出部は、算出した前記タイミングに基づいて、前記第１演算部の出力値の
   変動を評価して前記補正値を算出することを特徴とすることを特徴とする請求項６に記載の移動体。
8. 前記検出部は、コリオリの力を検出する機構を備えることを特徴とする請求項１乃至７のいずれか一項に記載の移動体。
9. 前記補正値を保持する補正値保持部を更に備えることを特徴とする請求項１乃至８のいずれか一項に記載の移動体。
10. 前記本体部は、実質的に一定の速度で前記車輪部を回転中心として回転することを特徴とする請求項１乃至９のいずれか一項に記載の移動体。
11. 車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれた角速度センサ部の出力値を補正するための補正値の算出方法であって、
    固定状態の前記車輪部を回転中心として前記本体部が回転する期間に前記角速度センサ部の出力を取得し、
    固定状態の前記車輪部を回転中心として前記本体部が回転する期間に、前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力を取得し、
    前記計測部の出力に基づいて前記本体部が鉛直方向上に位置するタイミングを算出し、
    算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して、前記角速度センサ部の出力値を補正するための補正値を算出する、補正値の算出方法。
12. 前記本体部の慣性モーメント、前記車輪部と前記本体部間を相対的に移動させる駆動源の特性値、前記駆動源により生じるトルク、及び前記計測部の出力値から定まる角度値に基づいて前記本体部に与えられる外力を推定することを特徴とする請求項１１に記載の補正値の算出方法。
13. 前記角速度センサ部は、コリオリの力を検出する機構を備えることを特徴とする請求項１１又は１２に記載の補正値の算出方法。
14. 車輪部に対して本体部の相対位置が制御された状態で空間移動する移動体に組み込まれる角速度センサ部の出力値を補正するための補正値をコンピュータに算出させるプログラムであって、
    前記車輪部の回転方向における前記本体部と前記車輪部との間の相対位置の変化を計測する計測部の出力値に基づいて、前記本体部が鉛直方向上に位置するタイミングを算出させ、
    算出した当該タイミングに基づいて、前記角速度センサ部の出力値の変動を評価して前記補正値を算出させる、プログラム。

Images