(57)【要約】【課題】静止状態の情報を必要とすることなく、また、得られる角度が離散的にすることなく、角速度を積分して角度を求める際に誤差の蓄積を抑制する方法、誤差蓄積抑制装置、姿勢制御装置、及び台車を提供する。【解決手段】角速度ωｔ  を取得する（Ｓ１３）都度、測定の各順毎に定められる補正量Ｂｔ  を用いて角速度の補正値（ωｔ  −Ｂｔ  ）を計算し（Ｓ１４）、（ωｔ  −Ｂｔ  ）を積分して角度の計算値θｔ  を計算し（Ｓ１５）、角度測定値φｔ  を取得し（Ｓ１６）、角度測定値φｔ  と角度の計算値θｔ  との差の値に応じて、次順で用いる補正量Ｂｔ＋１  を決定し（Ｓ１７）、この処理を繰り返して、角速度を測定する都度、動的に角速度の値を補正して誤差の蓄積を抑制する。【選択図】        図４
2004251671.tif 000002
【特許請求の範囲】
【請求項１】
時系列的に測定された微分測定値を積分して物理量を求める際の誤差蓄積を抑制する方法において、
各順で測定された前記微分測定値に対して各順毎に定められる補正量を用いて補正を行った値を積分して前記物理量の計算値を計算し、
前記物理量の測定値と前記計算値との差の値を計算し、
前記差の値に応じて、次順に係る前記補正量を定める
ことを特徴とする誤差蓄積抑制方法。
【請求項２】
時系列的に測定された角速度を積分して角度を求める際の誤差蓄積を抑制する方法において、
各順で測定された角速度に対して各順毎に定められる補正量を加減算した値を積分して角度の計算値を計算し、
角度が測定された角度測定値と前記計算値との差の値を計算し、
前記差の値に応じて、次順に係る前記補正量を決定する
ことを特徴とする誤差蓄積抑制方法。
【請求項３】
前記補正量は、測定される角速度の最小分解能の整数倍に定めることを特徴とする請求項２に記載の誤差蓄積抑制方法。
【請求項４】
時系列的に測定された角速度を積分して角度を求める際の誤差蓄積を抑制する装置において、
各順で測定された角速度に対して各順毎に定められる補正量を加減算した値を積分して角度の計算値を計算する角度計算手段と、
角度が測定された角度測定値と前記計算値との差の値を計算する手段と、
前記差の値に応じて、次順に係る前記補正量を決定する決定手段と
を備えることを特徴とする誤差蓄積抑制装置。
【請求項５】
前記決定手段は、前記補正量を、測定される角速度の最小分解能の整数倍に定めるべくなしてあることを特徴とする請求項４に記載の誤差蓄積抑制装置。
【請求項６】
姿勢を変更可能な物体の姿勢がなす角度に基づいて前記物体の姿勢を制御する装置において、
請求項４又は５に記載の誤差蓄積抑制装置と、
前記角度計算手段が計算した角度の計算値に基づいて姿勢を制御する手段と
を備えることを特徴とする姿勢制御装置。
【請求項７】
台車本体に車輪を備えてなり、該車輪の支持により前記台車本体の倒立を行う手段を備える台車において、
請求項４又は５に記載の誤差蓄積抑制装置と、
倒立を行っている場合に、前記角度計算手段が計算した角度の計算値を一定に保つべく前記台車本体の姿勢を制御する手段と
を備えることを特徴とする台車。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ジャイロセンサ等の角速度センサを用いて求めた角度の誤差蓄積を抑制する方法、誤差蓄積抑制装置、誤差の蓄積を抑制しながら求めた角度に基づいて姿勢を制御する姿勢制御装置、及び前記誤差蓄積抑制装置を用いた台車に関する。
【０００２】
【従来の技術】
物体の姿勢制御に利用するために、物体の姿勢角度を検出する方法として、従来、角度を直接に測定する角度センサを用いる方法、及びジャイロセンサ等の角速度センサにより測定した角速度を積分して角度を求める方法がある。角度センサを用いる方法では、姿勢角度の絶対角を測定することが可能であるが、一般的に応答性が悪いという問題がある。また、角速度センサを用いる方法は、応答性が良いものの、絶対角がわからず、角速度を測定する際の誤差が積分によって蓄積するという問題がある。そこで、積分による蓄積誤差を除去し、角速度センサを用いて求められた角度を補正する技術が開発されている。特許文献１では、角速度センサが静止状態である条件の下で、角速度センサの測定値に含まれるドリフト成分量を算出し、算出したドリフト成分量を更新可能に保存し、保存しているドリフト成分量を角速度センサの測定値から逐次除去することにより角速度を補正して、積分による蓄積誤差を回避する技術が開示されている。また、特許文献２では、移動体の自立航法装置において、角速度センサとは別に方位検出手段を備え、角速度の積分によって求めた方位角と方位検出手段による方位角の検出値との比較から、方位角の修正量を算出し、方位角を較正する技術が開示されている。
【０００３】
【特許文献１】
特許第３１６５０４５号公報
【特許文献２】
特許第２６６９８８９号公報
【特許文献３】
特許第３０１８４９７号公報
【特許文献４】
特許第２７０２６００号公報
【特許文献５】
特許第２６８０９００号公報
【０００４】
【発明が解決しようとする課題】
前輪と後輪とを備える台車が、溝を越える際などに前輪または後輪で倒立する際の姿勢制御をするためには、台車の姿勢角度または角速度を常に変化させながら、台車の姿勢角度または角速度の変化に基づいたフィードバック制御を行う必要がある。このような静止状態がない状態での姿勢制御では、特許文献１に開示された技術は、静止状態でのドリフト成分量が必要であるため、適用できないという問題がある。また、特許文献２に開示された技術は、方位角の修正によって、得られる方位角が修正の前後で離散的になるため、方位角の変化に基づいたフィードバック制御が不可能であるという問題がある。
【０００５】
本発明は、斯かる事情に鑑みてなされたものであって、その目的とするところは、角速度の積分値と角度の測定値との比較により角速度を測定する都度角速度の修正量を定めて角速度を修正することにより、角速度センサの静止状態がない場合においても角速度の積分による誤差の蓄積を抑制し、また、得られる角度を離散的にすることなく角速度の積分による誤差の蓄積を抑制する方法、誤差蓄積抑制装置、誤差の蓄積を抑制しながら求めた角度に基づいて姿勢を制御する姿勢制御装置、及び前記誤差蓄積抑制装置を用いた台車を提供することにある。
【０００６】
【課題を解決するための手段】
第１発明に係る誤差蓄積抑制方法は、時系列的に測定された微分測定値を積分して物理量を求める際の誤差蓄積を抑制する方法において、各順で測定された前記微分測定値に対して各順毎に定められる補正量を用いて補正を行った値を積分して前記物理量の計算値を計算し、前記物理量の測定値と前記計算値との差の値を計算し、前記差の値に応じて、次順に係る前記補正量を定めることを特徴とする。
【０００７】
第１発明においては、時系列的に測定した微分測定値に対して各順毎に定められる補正量を用いて補正した値を積分して物理量の計算値を計算し、物理量の計算値と測定値との間の差の値に応じて次順での補正量を計算して、微分測定値の値を測定する都度動的に補正することにより、微分測定値の誤差が積分により蓄積することを可及的に抑制する。
【０００８】
第２発明に係る誤差蓄積抑制方法は、時系列的に測定された角速度を積分して角度を求める際の誤差蓄積を抑制する方法において、各順で測定された角速度に対して各順毎に定められる補正量を加減算した値を積分して角度の計算値を計算し、角度が測定された角度測定値と前記計算値との差の値を計算し、前記差の値に応じて、次順に係る前記補正量を決定することを特徴とする。
【０００９】
第３発明に係る誤差蓄積抑制方法は、前記補正量は、測定される角速度の最小分解能の整数倍に定めることを特徴とする。
【００１０】
第４発明に係る誤差蓄積抑制装置は、時系列的に測定された角速度を積分して角度を求める際の誤差蓄積を抑制する装置において、各順で測定された角速度に対して各順毎に定められる補正量を加減算した値を積分して角度の計算値を計算する角度計算手段と、角度が測定された角度測定値と前記計算値との差の値を計算する手段と、前記差の値に応じて、次順に係る前記補正量を決定する決定手段とを備えることを特徴とする。
【００１１】
第５発明に係る誤差蓄積抑制装置は、前記決定手段は、前記補正量を、測定される角速度の最小分解能の整数倍に定めるべくなしてあることを特徴とする。
【００１２】
第２及び第４発明においては、時系列的に測定した角速度に対して各順毎に定められる補正量を加減算した値を積分して角度の計算値を計算し、角度の計算値と角度測定値との間の差の値に応じて次順での補正量を計算して、角速度の値を測定する都度動的に補正することにより、角速度の誤差が積分により蓄積することを可及的に抑制する。
【００１３】
第３及び第５発明においては、補正量を角速度の最小分解能の整数倍とすることにより、角速度の補正によって最小分解能以下の誤差が発生することを排除する。
【００１４】
第６発明に係る姿勢制御装置は、姿勢を変更可能な物体の姿勢がなす角度に基づいて前記物体の姿勢を制御する装置において、第４又は第５発明に係る誤差蓄積抑制装置と、前記角度計算手段が計算した角度の計算値に基づいて姿勢を制御する手段とを備えることを特徴とする。
【００１５】
第６発明においては、測定の都度、動的に補正した角速度を積分した角度の計算値に基づいて物体の姿勢を制御する。
【００１６】
第７発明に係る台車は、台車本体に車輪を備えてなり、該車輪の支持により前記台車本体の倒立を行う手段を備える台車において、第４又は第５発明に係る誤差蓄積抑制装置と、倒立を行っている場合に、前記角度計算手段が計算した角度の計算値を一定に保つべく前記台車本体の姿勢を制御する手段とを備えることを特徴とする。
【００１７】
第７発明においては、車輪の支持による倒立が可能な台車にて、測定の都度動的に補正した角速度を積分した角度の計算値を一定に保つべく、静止状態がない倒立状態の台車の姿勢をフィードバック制御する。
【００１８】
【発明の実施の形態】
以下本発明をその実施の形態を示す図面に基づき具体的に説明する。
図１は、本発明の台車を示す側面図である。本発明の台車は、自身を制御する本発明の姿勢制御装置１を搭載した台車本体３０と、台車本体３０の下部に設けられた脚部３２と、脚部３２を中途部で保持しつつ台車本体３０を支持する本体支持部３１とを備えている。脚部３２は、台車の前後方向に伸びた形状であり、回転自在な一対の前輪４１，４１及び一対の後輪４２，４２を備えており、台車は４輪に支えられて移動する構成となっている。前輪４１，４１及び後輪４２，４２は、各別に回転するように構成されており、前輪４１及び後輪４２の夫々の回転軸上には、前輪４１及び後輪４２の夫々を回転させるモータ４１ｍ及び４２ｍ、並びに前輪４１及び後輪４２の夫々の回転角度を検出する車輪エンコーダ４１ｓ及び４２ｓを備えている。本体支持部３１は、回動軸３３を介して脚部３２を中途部で保持しており、回動軸３３上には本体支持部３１を回動軸３３周りに回動させる図示しないモータを備えている。本体支持部３１が回動軸３３周りに回動することにより、台車本体３０も回動し、前方へ回動することにより台車の重心が前方へ移動し、後方へ回動することにより台車の重心が後方へ移動する。台車本体３０は、ジャイロセンサを用いた角速度センサ２１、及び台車本体３０の傾斜角度を測定する角度センサ２２を備えており、角速度センサ２１及び角度センサ２２は、時系列的に台車本体３０の角速度および姿勢角度を測定する構成となっている。
【００１９】
台車は、段差を乗り越える場合、又は溝を越える場合などに、前輪４１又は後輪４２を浮かした倒立を行うことが可能な構成となっている。図２は、倒立中の本発明の台車を示す側面図であり、前輪４１を浮かした倒立の場合を示している。台車本体３０を回動軸３３の周りに後方へ回動させることによって台車の重心を後方へ移動させ、更に、モータ４２ｍによって後輪４２，４２に後進方向へのトルクを与えることにより、台車は倒立する。台車の重心が後輪４２，４２の車軸の真上に来るように重心を制御することで、台車は見かけ上静止して倒立状態を保つことができる。
【００２０】
図３は、本発明の姿勢制御装置１及び誤差蓄積抑制装置１１の内部の構成を示すブロック図である。姿勢制御装置１は、角速度センサ２１が測定する角速度の誤差蓄積を抑制しながら角度の計算値を計算する本発明の誤差蓄積抑制装置１１と、ＣＰＵ等から構成されており、角度の計算値に基づいて台車の姿勢を制御する制御部１２とを備えている。誤差蓄積抑制装置１１は、演算を行うＣＰＵ１１１と、演算に伴う一時的な情報、及びＣＰＵ１１１の計算結果を記憶するＲＡＭ１１２と、ＣＰＵ１１１に本発明の誤差蓄積抑制方法を実行させるためのプログラムを記憶しているＲＯＭ１１３とを備えており、ＣＰＵ１１１は、ＲＯＭ１１３からＲＡＭ１１２へロードしたプログラム、又はＲＯＭ１１３に記憶しているプログラムに従って、本発明の誤差蓄積抑制方法を実行する。ＣＰＵ１１１には、また、角速度センサ２１及び角度センサ２２が接続されたＡ／Ｄコンバータ１１４が接続されており、Ａ／Ｄコンバータ１１４は、角速度センサ２１が測定する角速度、及び角度センサ２２が測定する台車本体３０の傾斜角度を受け付けてＡ／Ｄ変換し、ＣＰＵ１１１へ入力する。ＣＰＵ１１１には、更に、出力インタフェース１１５が接続されており、出力インタフェース１１５は、ＲＡＭ１１２に記憶している角速度、及び角度の計算値を誤差蓄積抑制装置１１の外部へ出力する。制御部１２は、出力インタフェース１１５に接続されており、出力インタフェース１１５から出力された角速度および角度の計算値を受け付ける。また、制御部１２は、後輪４２，４２の回転角度を測定する車輪エンコーダ４２ｓと、後輪４２，４２を回転させるモータ４２ｍとが接続されており、車輪エンコーダ４２ｓが測定する後輪４２，４２の回転角度を受け付け、台車本体３０の角速度および角度の計算値と後輪４２，４２の回転角度とから、後輪４２，４２へ与えるべきトルクを計算し、計算したトルクを後輪４２，４２へ与えるべくモータ４２ｍを駆動させる。更に、制御部１２は、台車が倒立状態である場合に、倒立状態を保つように、後輪４２，４２へ与えるべきトルクを計算してモータ４２ｍを制御する。
【００２１】
次に、フローチャートを用いて本発明の誤差蓄積抑制方法の手順を説明する。図４は、本発明の誤差蓄積抑制装置１１が行う処理の手順を示すフローチャートである。ＲＯＭ１１３は、誤差蓄積抑制方法の処理に用いるための、角速度の補正量Ｂｔ  の初期値、｜ｎ１  ｜＞｜ｎ２  ｜である整数ｎ１  ，ｎ２  、角度の許容誤差Ｅｒｒ、及び角速度センサ２１の最小分解能Ｒｅｚを予め記憶している。まず、誤差蓄積抑制装置１１は、角度センサ２２が測定した台車本体３０の傾斜角度をＡ／Ｄコンバータ１１４にて取得し（Ｓ１１）、ＣＰＵ１１１は、受け付けた傾斜角度を角度測定値の初期値φ０  として、計算すべき角度の計算値θｔ  をθ０  ＝φ０  に初期化してＲＡＭ１１２に記憶する（Ｓ１２）。ＣＰＵ１１１は、次に、角速度センサ２１が時系列的に測定している角速度ωｔ  をＡ／Ｄコンバータ１１４にて取得し（Ｓ１３）、ＲＯＭ１１３に記憶しているプログラムに従って、ＲＲＡＭ１１２に記憶している補正量Ｂｔ  を用いて、角速度の補正値（ωｔ  −Ｂｔ  ）を計算してＲＡＭ１１２に記憶する（Ｓ１４）。なお、角速度の測定が１順目であって補正量Ｂｔ  がＲＡＭ１１２に記憶されていない場合は、ＲＯＭ１１３に記憶している補正量Ｂｔ  の初期値を用いて角速度の補正値を計算する。ＣＰＵ１１１は、次に、ＲＯＭ１１３に記憶しているプログラムに従って、角度の計算値θｔ  を、角速度センサ２１から角速度を取得する周期をＴ、及び角度を計算する現在の順目をｍとした以下の式、
【００２２】
【数１】
2004251671.tif 000003
【００２３】
に従って計算し、ＲＡＭ１１２に記憶する（Ｓ１５）。なお、このとき、前回の順目での角度の計算値θｔ−１  を用いた式、θｔ  ＝Ｔ（ωｔ  −Ｂｔ  ）＋θｔ−１  により角度の計算値θｔ  を計算しても良い。ＣＰＵ１１１は、次に、角度センサ２２が測定した台車本体３０の傾斜角である角度測定値φｔ  をＡ／Ｄコンバータ１１４にて取得し（Ｓ１６）、ＲＯＭ１１３に記憶しているプログラムに従って、次順での角速度の補正に用いる補正量Ｂｔ＋１  を決定する次順の補正量Ｂｔ＋１  決定処理を行う（Ｓ１７）。
【００２４】
図５は、ステップＳ１７における次順の補正量決定処理のサブルーチンの手順を示すフローチャートである。ＣＰＵ１１１は、ＲＯＭ１１３に記憶しているプログラムに従って、角度測定値φｔ  と角度の計算値θｔ  との差の値の絶対値｜φｔ  −θｔ  ｜が、予め定めてＲＯＭ１１３に記憶してある許容誤差Ｅｒｒより大きいか否かを判定する（Ｓ１７１）。｜φｔ  −θｔ  ｜がＥｒｒより大きいと判定された場合は（Ｓ１７１：ＹＥＳ）、ＣＰＵ１１１は、ＲＯＭ１１３に記憶しているプログラムに従って、角度測定値φｔ  と角度の計算値θｔ  との差の値（φｔ  −θｔ  ）が０より大きいか否かを判定し（Ｓ１７２）、０より大きい場合は（Ｓ１７２：ＹＥＳ）、次順の補正量Ｂｔ＋１  を、Ｂｔ＋１  ＝−｜ｎ１  ｜×Ｒｅｚと決定して（Ｓ１７３）、ステップＳ１７のサブルーチンを終了して処理をメインへ戻し、（φｔ  −θｔ  ）が０よりも大きくなかった場合は（Ｓ１７２：ＮＯ）、次順の補正量Ｂｔ＋１  を、Ｂｔ＋１  ＝｜ｎ１  ｜×Ｒｅｚと決定して（Ｓ１７４）、ステップＳ１７のサブルーチンを終了して処理をメインへ戻す。ステップＳ１７１にて｜φｔ  −θｔ  ｜がＥｒｒより大きくないと判定された場合は（Ｓ１７１：ＮＯ）、ＣＰＵ１１１は、ＲＯＭ１１３に記憶しているプログラムに従って、角度測定値φｔ  と角度の計算値θｔ  との差の値（φｔ  −θｔ  ）が０より大きいか否かを判定し（Ｓ１７５）、０より大きい場合は（Ｓ１７６：ＹＥＳ）、次順の補正量Ｂｔ＋１  を、Ｂｔ＋１  ＝−｜ｎ２  ｜×Ｒｅｚと決定して（Ｓ１７６）、ステップＳ１７のサブルーチンを終了して処理をメインへ戻し、（φｔ  −θｔ  ）が０よりも大きくなかった場合は（Ｓ１７５：ＮＯ）、次順の補正量Ｂｔ＋１  を、Ｂｔ＋１  ＝｜ｎ２  ｜×Ｒｅｚと決定して（Ｓ１７７）、ステップＳ１７のサブルーチンを終了して処理をメインへ戻す。
【００２５】
ステップＳ１７のサブルーチンで用いる許容誤差Ｅｒｒ及び整数ｎ１  ，ｎ２  は、角度測定値φｔ  と角度の計算値θｔ  との差が可及的に小さくなるように、予め実験的に定められており、ｎ２  ＝０としても良い。なお、補正量Ｂｔ＋１  の絶対値を２段階に定めるのではなく、（φｔ  −θｔ  ）の値の範囲と多数の整数ｎ１  ，ｎ２  ，…の夫々とを対応させてＲＯＭ１１３に記憶したテーブルを参照する等して、（φｔ  −θｔ  ）の値に応じてより多段階に補正量Ｂｔ＋１  を決定する処理を用いてもよい。
【００２６】
ＣＰＵ１１１は、次に、ＲＯＭ１１３に記憶しているプログラムに従って、決定した次順の補正量Ｂｔ＋１  をＲＡＭ１１２に記憶し（Ｓ１８）、誤差蓄積抑制方法の処理を終了するか否かを判定し（Ｓ１９）、終了する場合は（Ｓ１９：ＹＥＳ）、処理を終了し、終了しない場合は（Ｓ１９：ＮＯ）、処理をステップＳ１３へ戻して、次順の計算を行う。ステップＳ１３からステップＳ１９までの処理は、角速度センサ２１から角速度を取得する周期Ｔの時間間隔で行われ、時間間隔Ｔ毎に、角度の計算値θｔ  が得られ、得られた角度の計算値θｔ  を用いて台車の姿勢制御が行われる。
【００２７】
次に、角度の計算値θｔ  を用いて姿勢制御装置１が行う台車の倒立制御の手順を説明する。図２に示す如く、台車の重心が後輪４２，４２の車軸の真上に来て台車が見かけ上静止して倒立している場合に、回動軸３３を固定し、倒立状態を保つように、後輪４２，４２へ与えるべきトルクを計算してモータ４２ｍを制御する。このとき、姿勢制御装置１は、倒立を行っている状態を初期値０とした台車本体３０の傾斜角度を０に保つべく制御すればよい。
【００２８】
図６は、本発明の姿勢制御装置１が行う倒立制御の手順を示すフローチャートである。制御部１２は、誤差蓄積抑制装置１１のＲＡＭ１１２に記憶されている角速度の補正値（ωｔ  −Ｂｔ  ）、及び角度の計算値θｔ  を、出力インタフェース１１５を介して読み出し（Ｓ２１）、車輪エンコーダ４２ｓが測定している後輪４２，４２の回転角度ψを取得し（Ｓ２２）、取得した回転角度ψの差分を、取得する周期で割ることにより、後輪４２，４２の角速度ｄψ／ｄｔを計算し（Ｓ２３）、（ωｔ  −Ｂｔ  ）、θｔ  、ψ、ｄψ／ｄｔ、並びに倒立の平衡位置を与える後輪４２，４２の目標回転角度ψｒｅｆ  及び目標角速度（ｄψ／ｄｔ）ｒｅｆ  を用いて、台車本体３０の傾斜角度および角速度を０に保って倒立状態を保つべく後輪４２，４２へ与えるトルクＦを計算する（Ｓ２４）。トルクＦは、フィードバック制御により、
ｘ＝（θｔ  ，ψ，（ωｔ  −Ｂｔ  ），ｄψ／ｄｔ）
ｘｒｅｆ  ＝（０，ψｒｅｆ  ，０，（ｄψ／ｄｔ）ｒｅｆ  ）
とし、台車の構成で定められる関数Ｋ（ｘ）を用いて、
Ｆ＝Ｋ（ｘ−ｘｒｅｆ  ）
で計算される。倒立の平衡位置を与える目標回転角度ψｒｅｆ  及び目標角速度（ｄψ／ｄｔ）ｒｅｆ  は、予め実験的に定められる。制御部１２は、次に、計算したトルクＦを後輪４２，４２に与えるべくモータ４２ｍを制御し（Ｓ２５）、倒立制御の処理を終了するか否かを判定し（Ｓ２６）、終了する場合は（Ｓ２６：ＹＥＳ）、処理を終了し、終了しない場合は（Ｓ２６：ＮＯ）、処理をステップＳ２１へ戻して、倒立制御を続行する。ステップＳ２１からステップＳ２６までの処理は、誤差蓄積抑制装置１１から（ωｔ  −Ｂｔ  ），θｔ  を読み出す周期、又は車輪エンコーダ４２ｓからψを取得する周期の時間間隔で行われ、該時間間隔毎に、後輪４２，４２に与えられるトルクが制御されて、台車の倒立状態が維持される。姿勢制御装置１が倒立制御を終了した場合は、台車は前輪４１，４１を下ろして、倒立状態から通常の状態へ復帰する。
【００２９】
以上詳述した如く、本発明の誤差蓄積抑制方法では、角速度と角度とを測定し、角速度を積分した角度の計算値と角度の測定値との差の値に応じて角速度の補正量を決定し、決定した補正量を用いて角速度を補正する。角速度を測定する都度、動的に補正量を決定して角速度を補正するため、角速度の積分によって角速度の誤差が蓄積することを可及的に防止し、また、静止状態で得られる情報を必要とせず、角度の計算値を用いて、台車の倒立制御などの静止状態がない物体の姿勢制御を行うことができる。また、角速度を積分した角度に含まれる誤差を補正するのではなく、角速度の値を補正して積分するため、角度の計算値が離散的に得られることがなく、フィードバック制御の条件を保って台車の倒立制御などの姿勢制御を行うことが可能となる。また、角速度の補正量は角速度の最小分解能の整数倍にするため、角速度の補正によって最小分解能以下の誤差が発生することを排除し、より効果的に角速度の誤差が蓄積することを防止できる。
【００３０】
なお、本実施の形態においては、台車が倒立する際に後輪４２，４２で倒立する場合の倒立制御を示したが、これに限るものではなく、本発明により、前輪４１，４１で倒立する場合の倒立制御を行うことも可能であり、また、倒立状態で移動する場合の倒立制御を行うことも可能である。また、本実施の形態においては、四輪の台車が行う倒立制御について示したが、オートバイ等の二輪の台車についても、本発明を用いて倒立制御を行うことが可能である。また、安定状態から倒立状態へ移行できる形態の台車について示したが、本発明に係る台車は、一輪または同軸の二輪で支持される等、倒立状態を常態として使用される台車であってもよく、このような台車についても本発明を用いて倒立制御を行うことが可能である。また、本実施の形態においては、台車の倒立制御に対する適用例のみを示したが、これに限るものではなく、台車の水平維持制御、ロボットの姿勢制御、またはつり下げ物体の揺れ抑制制御など、物体の姿勢角度を用いたその他の姿勢制御についても、本発明を適用することができる。
【００３１】
更に、本実施の形態においては、物理量として角度、微分測定値として角速度を測定する例について説明したが、物理量として速度、微分測定値として加速度など、角度以外の物理量を測定する場合においても本発明の誤差蓄積抑制方法を適用することが可能である。
【００３２】
【発明の効果】
第１発明においては、時系列的に測定した微分測定値に対して各順毎に定められる補正量を用いて補正した値を積分して物理量の計算値を計算し、物理量の計算値と測定値との間の差の値に応じて次順での補正量を計算して、微分測定値の値を測定する都度動的に補正することにより、微分測定値の誤差が積分により蓄積することが可及的に抑制される。
【００３３】
第２及び第４発明においては、時系列的に測定した角速度に対して各順毎に定められる補正量を加減算した値を積分して角度の計算値を計算し、角度の計算値と角度測定値との間の差の値に応じて次順での補正量を計算して、角速度の値を測定する都度動的に補正することにより、角速度の誤差が積分により蓄積することが可及的に抑制され、また、静止状態がない場合においても誤差の蓄積を抑制することができ、さらに、得られる角度が離散的になることがない。
【００３４】
第３及び第５発明においては、補正量を角速度の最小分解能の整数倍とすることにより、角速度の補正によって最小分解能以下の誤差が発生することを排除すし、より効果的に角速度の誤差が蓄積することを防止できる。
【００３５】
第６発明においては、静止状態がない物体の姿勢制御、及び姿勢角度のフィードバック制御の条件を保った物体の姿勢制御を行うことが可能となる。
【００３６】
第７発明においては、車輪の支持による倒立が可能な台車にて、静止状態がない倒立状態の姿勢を制御する倒立制御を、傾斜角度のフィードバック制御の条件を保ちながら行うことができる等、本発明は優れた効果を奏する。
【図面の簡単な説明】
【図１】本発明の台車を示す側面図である。
【図２】倒立中の本発明の台車を示す側面図である。
【図３】本発明の姿勢制御装置及び誤差蓄積抑制装置の内部の構成を示すブロック図である。
【図４】本発明の誤差蓄積抑制装置が行う処理の手順を示すフローチャートである。
【図５】ステップＳ１７における次順の補正量決定処理のサブルーチンの手順を示すフローチャートである。
【図６】本発明の姿勢制御装置が行う倒立制御の手順を示すフローチャートである。
【符号の説明】
１  姿勢制御装置
１１  誤差蓄積抑制装置
１１１  ＣＰＵ
１２  制御部
２１  角速度センサ
２２  角度センサ
３０  台車本体
４１  前輪（車輪）
４２  後輪（車輪）
４１ｓ、４２ｓ  車輪エンコーダ
４１ｍ、４２ｍ  モータ
【図１】
2004251671.tif 000004
【図２】
2004251671.tif 000005
【図３】
2004251671.tif 000006
【図４】
2004251671.tif 000007
【図５】
2004251671.tif 000008
【図６】
2004251671.tif 000009