JP2013116684A

JP2013116684A - 倒立移動体及び角速度センサの出力値補正方法

Info

Publication number: JP2013116684A
Application number: JP2011265109A
Authority: JP
Inventors: Susumu Oikawa; 晋及川
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2011-12-02
Filing date: 2011-12-02
Publication date: 2013-06-13
Anticipated expiration: 2031-12-02
Also published as: US20140330507A1; CN103975224A; US9321498B2; WO2013080040A1; JP5724855B2

Abstract

【課題】ピッチ軸角速度センサとロール軸角速度センサの、倒立移動体本体に対する取り付け角度の誤差を推定する。
【解決手段】（１）倒立二輪車本体２の基準ヨー軸Zを鉛直に対して平行にして静止させた状態としての静止状態におけるバイアス値を取得する。（２）倒立二輪車本体２の基準ヨー軸Zを鉛直に対して平行にした状態のままで倒立二輪車１を所定の旋回角速度で旋回させた旋回状態におけるバイアス値を取得する（３）静止状態における各センサのバイアス値と、旋回状態における各センサのバイアス値と、所定の旋回角速度と、に基づいて、ピッチ軸角速度センサとロール軸角速度センサの、倒立二輪車本体２に対する取り付け角度誤差を推定する。
【選択図】図５

Description

本発明は、取り付け角度誤差の推定方法に関する。

この種の技術として、特許文献１は、角速度センサ（ジャイロセンサ）と加速度センサを用いた倒立二輪型ロボットの姿勢制御を開示している。角速度センサの出力値は経時的に劣化するものであるから、特許文献１では、倒立二輪型ロボットを特殊な治具を用いて支持した上で、倒立二輪型ロボットを、最も後方に傾斜した位置から最も前方に傾斜した位置へと一定の回転速度で回転させるなどして、角速度センサの補正値を求めることとしている。

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

ところで、倒立二輪車の倒立制御を行うためのピッチ軸角速度センサの、機体の基準軸に対する、取り付け角度に誤差があると、旋回（ヨー軸回りの旋回）を行ったとき、その旋回成分がピッチ軸角速度センサにより検出されてしまい、倒立基準角度が徐々にズレてしまうことがある。この結果、倒立二輪車は、旋回を停止したとき前後に傾いた状態で倒立してしまい、乗員に対して不快感を与えてしまう。

また、同様に、ロール軸角速度センサの、機体の基準軸に対する、取り付け角度に誤差があると、旋回を行ったとき、倒立二輪車の前後の傾きがゆらゆらと変化してしまい、同様に乗員に対して不快感を与えてしまう。

この問題に対し、従来では、各角速度センサの、機体の基準軸に対する、取り付け角度を可及的にゼロにする対策を講じてきた。この対策では、角速度センサと水平の基準軸となる傾斜センサとの間の取り付け精度や、角速度と傾斜センサを含むセンサブロックと倒立二輪車の車体との間の取り付け精度、といった各種の精度を補償するための高精度なフレームが必要になるなど、倒立二輪車の車体の小型軽量化の観点から問題があった。

上述した取り付け誤差に起因する問題を更に説明すると、例えば、倒立二輪車の倒立制御を行うピッチ軸角速度センサの、旋回面（つまりヨー旋回面）に対する取り付け誤差がロール軸周りにβ０［ｒａｄ］であったとし、角速度γドットで旋回したとする。この場合、ピッチ軸角速度センサからは、γドット×ｓｉｎ（β０）［ｒａｄ／ｓｅｃ］が誤差として出力されることになる。具体的には、ピッチ軸角速度センサの、旋回軸に対する取り付け誤差β０が０．５［ｄｅｇ］であったとすると、３６０［ｄｅｇ］の旋回により、累積としてピッチ軸回りにあたかも約３［ｄｅｇ］回転したこととして認識されてしまう。

このような旋回によるピッチ角速度の誤差は比較的低周波帯域で発生するため、ローパスフィルタによって誤差をキャンセルする手法が考えられる。しかしながら、例えば旋回方向を細かく左右に切り替えた場合はローパスフィルタが全く役に立たず、ピッチ角速度を積分してピッチ角度を求める演算過程において誤差が蓄積してしまい、ピッチ角度が際限なくズレていってしまう可能性がある。

本願発明の目的は、ピッチ軸角速度センサとロール軸角速度センサの、倒立移動体本体に対する取り付け角度の誤差を推定する技術を提供することにある。

本願発明の観点によれば、倒立移動体本体と、前記倒立移動体本体に取り付けられたピッチ軸角速度センサとロール軸角速度センサを備えた倒立移動体における、前記ピッチ軸角速度センサと前記ロール軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差の推定方法であって、前記倒立移動体本体の基準ヨー軸を鉛直方向で静止させた状態としての静止状態における、前記ピッチ軸角速度センサと前記ロール軸角速度センサの出力を取得することで、前記静止状態における前記ピッチ軸角速度センサと前記ロール軸角速度センサのバイアス値を取得するステップと、前記倒立移動体本体の前記基準ヨー軸を鉛直に対して平行にした状態のままで前記倒立移動体を所定の旋回角速度で旋回させた旋回状態における、前記ピッチ軸角速度センサと前記ロール軸角速度センサの出力を取得することで、前記旋回状態における前記ピッチ軸角速度センサと前記ロール軸角速度センサのバイアス値を取得するステップと、前記静止状態における各センサのバイアス値と、前記旋回状態における各センサのバイアス値と、前記所定の旋回角速度と、に基づいて、前記ピッチ軸角速度センサと前記ロール軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差を推定するステップと、を含む、取り付け角度誤差の推定方法が提供される。
好ましくは、前記旋回状態では、前記移動体本体を水平に保持し、その場で旋回させる。
好ましくは、前記倒立移動体を倒立状態で超信地旋回させる。
好ましくは、前記倒立移動体を支持する支持台と、高さを調整可能であって前記支持台に取り付けられた複数の支持脚と、前記支持台に対して取り付けられる第１の部材と、前記倒立移動体本体に対して取り付けられる第２の部材と、を有し、前記第２の部材に対して前記第１の部材が前記支持台に対して平行に回転可能に前記第１の部材と前記第２の部材が連結された回転治具と、を備えた旋回装置を用いて、前記倒立移動体を超信地旋回させる。
本願発明の他の観点によれば、倒立移動体を支持する支持台と、高さを調整可能であって前記支持台に取り付けられた複数の支持脚と、前記支持台に対して取り付けられる第１の部材と、前記倒立移動体本体に対して取り付けられる第２の部材と、を有し、前記第２の部材に対して前記第１の部材が前記支持台に対して平行に回転可能に前記第１の部材と前記第２の部材が連結された回転治具と、を備えた、旋回装置が提供される。
上記の旋回装置は、取り付け角度誤差の推定方法に用いられる。

本願発明によれば、前記ピッチ軸角速度センサと前記ロール軸角速度センサの、前記倒立移動体本体に対する取り付け角度の誤差を推定することができる。

図１は、倒立二輪車の外観斜視図である。（第１実施形態）図２は、倒立二輪車の機能ブロック図である。（第１実施形態）図３は、水平旋回装置の斜視図である。（第１実施形態）図４は、水平旋回装置に倒立二輪車を取り付けた状態を示す斜視図である。（第１実施形態）図５は、調整モードの制御フローである。（第１実施形態）図６は、調整モードの制御フローである。（第２実施形態）

（第１実施形態）
以下、図１〜図５を参照しつつ、本願発明の第１実施形態を説明する。

図１に示す倒立二輪車１（倒立移動体）は、倒立二輪車１に搭乗した人員（搭乗者）を所望の方向へ搬送するための乗り物である。倒立二輪車１は、倒立二輪車１に搭乗した搭乗者による操作に応じて移動する。

倒立二輪車１は、倒立二輪車本体２と、倒立二輪車本体２に回転自在に取り付けられた一対の車輪としての右輪３及び左輪４と、操作入力部としてのハンドル体５と、によって構成されている。

倒立二輪車本体２は、バッテリー等を搭載した本体下部６と、本体下部６上に設けられた一対の足置き７と、本体下部６上に設けられた制御装置８と、によって構成されている。倒立二輪車本体２の本体下部６には、ハンドル体５が取り付けられている。ハンドル体５は、倒立二輪車本体２の本体下部６の主たる走行方向側に取り付けられている。ハンドル体５の上端には、搭乗者が把持するグリップ９が設けられている。右輪３には、右輪用モータ３ａが設けられている。左輪４には、左輪用モータ４ａが設けられている。

倒立二輪車本体２の基準となるヨー軸としての基準ヨー軸Zは、例えば、足置き７の足置き面７ａに対して直交するように定義付けられる。同様に、倒立二輪車本体２の基準となるピッチ軸としての基準ピッチ軸Yは、例えば、倒立二輪車本体２の本体下部６に回転可能に取り付けられている一対の右輪３及び左輪４の共通する回転軸として定義付けられる。また、倒立二輪車本体２の基準となるロール軸としての基準ロール軸Xは、例えば、基準ヨー軸Zに対して直交し、基準ピッチ軸Yに対して直交する軸として定義付けられる。

図２に示すように、制御装置８には、センサ部１０、指令値算出部１１、駆動部１２、誤差補正部１３、入力部１４が設けられている。

センサ部１０は、角速度センサ１５と、加速度センサ１６によって構成されている。角速度センサ１５は、例えばジャイロセンサにより構成される。即ち、角速度センサ１５は、倒立二輪車本体２のヨー角速度値を出力するヨー軸角速度センサと、倒立二輪車本体２のピッチ角速度値を出力するピッチ軸角速度センサと、倒立二輪車本体２のロール角速度値を出力するロール軸角速度センサと、によって構成されている。また、加速度センサ１６は、倒立二輪車本体２の３軸加速度値を出力する。

入力部１４は、ハンドル部１７によって構成されている。ハンドル部１７は、図１のハンドル体５の初期取り付け状態からの傾斜角度値を出力する。即ち、搭乗員は、ハンドル部１７を所望の方向に傾かせることで、倒立二輪車１の動作を制御する。

指令値算出部１１は、姿勢角度演算部１８と倒立制御演算部１９によって構成されている。

姿勢角度演算部１８は、角速度センサ１５から出力された各軸角速度値と、加速度センサ１６から出力された各軸加速度値と、をカルマンフィルタ等を用いてセンサフュージョンさせることで、現在の倒立二輪車本体２の姿勢角度値を演算する部分である。

倒立制御演算部１９は、姿勢角度演算部１８によって演算された倒立二輪車本体２の姿勢角度値に基づいて、倒立二輪車１が倒立した状態を維持するために必要な制御情報を演算して生成する部分である。また、倒立制御演算部１９は、姿勢角度演算部１８によって演算された倒立二輪車本体２の姿勢角度値と、入力部１４のハンドル部１７から受信した入力情報と、に基づいて、倒立二輪車１の倒立状態を維持しつつ、倒立二輪車１に所望の動作をさせるのに必要な制御情報を演算して生成する。

駆動部１２は、右輪駆動部２０と左輪駆動部２１によって構成されている。右輪駆動部２０及び左輪駆動部２１は、指令値算出部１１の倒立制御演算部１９から受信した制御情報に基づいて、右輪用モータ３ａ及び左輪用モータ４ａを夫々駆動する。

誤差補正部１３は、角速度記憶部２２と、取り付け角度誤差演算部２３と、取り付け角度誤差記憶部２４と、角速度補正部２５と、によって構成されている。角速度記憶部２２は、角速度センサ１５から出力された各軸角速度値を記憶する。取り付け角度誤差演算部２３は、角速度記憶部２２に記憶されている各軸角速度値に基づいて、倒立二輪車本体２に対する角速度センサ１５の取り付け角度の誤差値を演算する。取り付け角度誤差記憶部２４は、取り付け角度誤差演算部２３によって演算された取り付け角度の誤差値を記憶する。角速度補正部２５は、取り付け角度誤差記憶部２４に記憶されている取り付け角度の誤差値に基づいて、角速度センサ１５から出力された各軸角速度を補正し、補正後の各軸角速度を姿勢角度演算部１８に出力する。

上述した制御装置８は、CPU（Central Processing Unit）やRAM（Random Access Memory）、ROM（Read Only Memory）を備えている。ROMに記憶されている制御プログラムがCPUによって読み出され、CPU上で実行されることで、制御プログラムは、CPU等のハードウェアを、指令値算出部１１や誤差補正部１３等として機能させている。

図３には、本実施形態において倒立二輪車本体２に対する角速度センサ１５の取り付け角度の誤差を推定する際に用いられる水平旋回装置３０（旋回装置）を示している。水平旋回装置３０は、支持台３１と、高さを調整可能であって支持台３１に取り付けられた４本の支持脚３２と、回転治具３３と、によって構成されている。４本の支持脚３２の高さを調整することで、支持台３１は、略水平な状態に調整される。回転治具３３は、支持台３１に対して取り付けられる下治具３４（第１の部材）と、倒立二輪車本体２に対して取り付けられる上治具３５（第２の部材）と、を有し、下治具３４に対して上治具３５が支持台３１に対して水平に回転可能に下治具３４と上治具３５が連結されて構成されている。

図４には、水平旋回装置３０に取り付けられた倒立二輪車１を示している。図４に示す状態で、倒立二輪車１の右輪３及び左輪４は、水平旋回装置３０の支持台３１に対して適度な圧力で接している。この状態で、右輪３と左輪４を逆方向に回転させると、倒立二輪車１は、所定の旋回角速度で超信地旋回（２つの車輪を同じ速度で反対方向へ回転させることによる旋回）する。

次に、図５に基づいて、倒立二輪車１の制御装置８の制御フローについて説明する。倒立二輪車１の制御装置８は、搭乗者を搬送する通常走行モードの他に、図５に示す調整モードを有している。調整モードは、倒立二輪車１の倒立二輪車本体２に対する角速度センサ１５の取り付け角度の誤差を推定するためのモードである。

先ず、この調整モードの背景にある導出理論について説明し、次に、実際にその理論を用いて誤差を推定する図５の制御フローの説明に移る。

倒立二輪車の姿勢を計測するためにはピッチ軸角度、ロール軸角度を計測する傾斜センサ（加速度センサ１６に相当。）と、ピッチ角速度、ロール角速度、ヨー角速度を計測する角速度センサ（角速度センサ１５に相当。）が用いられる。傾斜センサは、一般的には３軸加速度センサを利用して下記の演算を行い、ピッチ軸角度α_0accとロール軸角度β_0accを求める。ピッチ軸角度α_0accは、本体に対する３軸加速度センサの取り付け角度のピッチ軸回りの誤差である。ロール軸角度β_0accは、本体に対する３軸加速度センサの取り付け角度のロール軸回りの誤差である。しかしながら、倒立二輪車への搭載に適した小型の３軸加速度センサは一般的にはノイズが大きく、このままでは倒立制御には適さない。ノイズを低減するためにローパスフィルタを用いるとフィルタの位相遅れにより倒立制御が不安定になってしまうためである。

これに対し、角速度センサであるジャイロスコープ（ジャイロセンサと呼ばれることもある）は角度を求めるために積分を行う必要があり、長時間の倒立制御ではドリフトが発生し、基準角度が次第にずれていってしまう。これはジャイロセンサのゼロ点が厳密には求められないこと、温度によってゼロ点が変化してしまうことなどの理由が挙げられる。

従って、これら二種類のセンサについてセンサフュージョンを行い、互いのセンサの不得意な部分を補いあい、好適な特性をもつセンサユニットを得る。センサフュージョンは、例えばカルマンフィルタなどが利用されている。カルマンフィルタを利用すると、ジャイロセンサのゼロ点誤差、即ちバイアス値が推定できる。バイアス推定値はジャイロセンサを積分した角度が加速度センサにて計測した角度に収束するように自動的に変化する。

ここで、３軸加速度センサX（ロール軸方向）,Y（ピッチ軸方向）、Z（ヨー軸方向）と、ロール、ピッチ、ヨー角速度センサの取り付け誤差について考える。

加速度センサの測定値のズレは各軸センサのゼロ点オフセットX0、Y0、Z0であり、加速度センサが倒立二輪車の車体基準軸（例えば倒立二輪車１の倒立二輪車本体２の基準ヨー軸Z）からの取り付け角度ズレはα0acc（倒立二輪車１の倒立二輪車本体２の基準ロール軸X回りのズレに相当。）、β0acc（倒立二輪車１の倒立二輪車本体２の基準ピッチ軸Y回りのズレに相当。）、γ0accの合計６つが考えられる。この中で、倒立制御を行うために必要なのは、α0accとβ0accのみである。倒立制御状態においては加速度センサZ軸の出力はおよそ１Gであり、ゼロ点のオフセットZ0は無視できる。旋回方向の取り付け角度ズレγ0accも倒立制御には利用しない。X0とY0はα0accとβ0accで代表して考えることができる。この２つの角度を利用した下記式（１）及び（２）の回転行列は、加速度センサ基準座標系（添字ｓ）と車体基準座標系（添字ｂ）の変換行列となっている。

上記式（２）においてβ0accとα0accともにゼロに近い小さな値であることを考慮すると、上記式（２）は、下記式（３）のように変形できる。

同様に、ジャイロセンサの取り付けズレは、（αはロール軸を意味し、βはピッチ軸を、γはヨー軸を示す。）ロール軸ジャイロの取り付けピッチ角度ズレα０β、ヨー角度ズレα０γ、ピッチ軸ジャイロの取り付けロール角度ズレβ０α、ヨー角度ズレβ０γ、ヨー軸ジャイロの取り付けロール角度ズレγ０α、ピッチ角度ズレγ０βの合計６つが考えられる。しかし、倒立二輪車は、ピッチ軸回りに回転を続けることはあり得ないため、α０γとγ０αには誤差が存在しても問題にならず、従って、ゼロと仮定しても差し支えない。同様に、倒立二輪車は、ロール軸回りに回転を続けることはあり得ないため、β０γとγ０βもゼロと仮定しても差し支えない。従って、考慮すべき角度ズレは、α０βとβ０αのみとなる。α０βとβ０αを用いてジャイロセンサ基準座標系（添字ｓ）を車体基準座標系（添字ｂ）に回転する変換行列は下記式（４）及び（５）となる。下記式（４）等で、αｓドットは、ロール軸角速度センサの出力値である。βｓドットは、ピッチ軸角速度センサの出力値である。γｓドットは、ヨー軸角速度センサの出力値である。αｂドットは、倒立二輪車のロール角速度である。βｂドットは、倒立二輪車のピッチ角速度である。γｂドットは、倒立二輪車のヨー角速度である。

上記式（５）において、α０βとβ０αともにゼロに近い小さい値であることを考慮すると、上記式（５）は下記式（６）に変形できる。

以上から、取り付け誤差を補正するために必要なパラメータは、加速度センサとボディ基準軸のロール軸回りの取り付け角度ズレα0accと、ピッチ軸回りの取り付け角度ズレβ0acc、ロール軸ジャイロの計測軸とボディ基準ロール軸のピッチ軸回りの取り付け角度ズレα０βと、ピッチ軸ジャイロの計測軸とボディ基準ピッチ軸のロール軸回りの取り付け角度ズレβ０αと、の４つのズレが精度よく計測できていれば、例え各センサの取り付けがずれていても単に逆回転の演算を行うことでボディ基準座標系における傾斜角度、回転角速度を精度よく求めることができる。裏を返せば、上記式（６）において、取り付けズレを放置した状態でヨー軸回りの旋回を行うと、ロール軸ジャイロのロール角速度αｓドットやピッチ軸ジャイロのピッチ角速度βｓドットが変化し、その積分値であるロール角度αやピッチ角度βに影響を及ぼす、ということになる。

以上に、調整モードの背景にある導出理論について説明した。

以下、図５の制御フローを順に説明する。

先ず、図４に示すように倒立二輪車１を水平旋回装置３０に取り付けて、倒立二輪車本体２の基準ヨー軸を鉛直に対して平行にして静止させた状態としての静止状態とする。次に、制御装置８の制御モードを、停止モードから調整モードへと切り替える（S300）。そして、制御装置８は、上記の静止状態において、加速度センサ１６の出力値X0、Y0を取得する（S301）。このとき、出力値X0、Y0は、十分長い時間かけて平均化させることで、ノイズを除去した精確な値とする。同様に、制御装置８は、上記の静止状態において、角速度センサ１５の出力値α０ドット（ロール軸角速度センサの出力値）、β０ドット（ピッチ軸角速度センサの出力値）、γ０ドットを取得する（S301）。各出力値は誤差補正部１３の角速度記憶部２２に記憶される。このとき、出力値α０ドット、β０ドット、γ０ドットは、十分長い時間かけて平均化させることで、ノイズを除去した精確な値とする。制御装置８は、取得した出力値α０ドット、β０ドット、γ０ドットを、誤差補正部１３の角速度記憶部２２に記憶させる。なお、上記の静止状態では、出力値α０ドット、β０ドット、γ０ドットは、本来であれば当然にゼロとなるべきものであるが、角速度センサのドリフト問題により、実際にはゼロではない値となっている。この意味で、出力値β０ドットは、静止状態における角速度センサ１５のピッチ軸角速度センサのバイアス値である。同様に、出力値α０ドットは、静止状態における角速度センサ１５のロール軸角速度センサのバイアス値である。添字０は、静止状態を示している。

次に、倒立二輪車１を水平旋回装置３０上で反時計回りに超信地旋回させる（S320）。詳しく言えば、倒立二輪車１の倒立二輪車本体２の基準ヨー軸を鉛直に対して平行にした状態で倒立二輪車１を所定の旋回角速度γ１ドットでその場で旋回させることで(S320)、倒立二輪車１を旋回状態とする。次に、制御装置８は、倒立二輪車１の旋回状態において、ピッチ軸角速度センサとロール軸角速度センサの出力を取得する（S330）。倒立二輪車１の旋回状態におけるピッチ軸角速度センサの出力値はβ１ドットとし、同じくロール軸角速度センサの出力値はα１ドットする。添字"１"は、旋回状態を示している。好ましくは、α１ドット及びβ１ドットは、十分な時間をかけて平均化するなどしてノイズを除去した精確な値とする。α１ドットとβ１ドットは、ヨー軸回りの旋回成分を拾うことで、それぞれ、α０ドットやβ０ドットとは若干異なる値となる。β１ドットは、旋回状態におけるピッチ軸角速度センサのバイアス値に相当し、同様にα１ドットは、旋回状態におけるロール軸角速度センサのバイアス値に相当している（S330）。

そして、取り付け角度誤差演算部２３は、静止状態における各センサのバイアス値α０ドットβ０ドットと、旋回状態における各センサのバイアス値α１ドット及びβ１ドットと、所定の旋回角速度γ１ドットと、に基づいて、ピッチ軸角速度センサとロール軸角速度センサの、倒立二輪車本体２に対する取り付け角度誤差を推定する（S340）。上記の推定には、上記式（６）から導出された下記式（７）及び（８）が用いられる。

また、倒立二輪車本体２に対する加速度センサ１６の取り付け角度の誤差については、下記式（９）及び（１０）が用いられる（S340）。ただし、下記式（９）及び（１０）において変数ｇは重力加速度である。

次に、制御装置８は、倒立二輪車１の超信地旋回を停止させた上で（S350）、今度は、倒立二輪車１を水平旋回装置３０上で時計回りに超信地旋回させる（S360）。

S370〜S390の制御フローは、S330〜S350の制御フローと同じであるからその説明を割愛する。

そして、最後に、取り付け角度誤差記憶部２４は、S340で算出した各値と、S380で算出した各値と、の平均値を求め（S400）、その平均値を、例えば図２の取り付け角度誤差記憶部２４を含む制御装置８のRAMに記憶し（S410)、制御装置８は、調整モードを完了させ（S420）、調整モードから通常走行モードへと制御を復帰させる。

通常走行モードへと復帰したら、以後、図２の誤差補正部１３の角速度補正部２５は、取り付け角度誤差記憶部２４から各値を取り出し、角速度センサ１５からの出力値を適宜に補正した上で姿勢角度演算部１８に出力する。これにより、倒立二輪車本体２に対する角速度センサ１５の取り付け角度に誤差が存在していても、その誤差を安価な計算で見かけ上なくすことができる。

同様に、上記式（９）〜（１１）で求めた各値によって、倒立二輪車本体２に対する加速度センサ１６の取り付け角度の誤差を安価な計算で見かけ上なくすことができる。

以上に本願発明の第１実施形態を説明したが、上記第１実施形態の特長は以下の通りである。

即ち、倒立二輪車本体２と、倒立二輪車本体２に取り付けられた角速度センサ１５（ピッチ軸角速度センサとロール軸角速度センサ）を備えた倒立二輪車１における、ピッチ軸角速度センサとロール軸角速度センサの、倒立二輪車本体２に対する取り付け角度誤差の推定は、以下のステップを含む方法によって行われる。（１）倒立二輪車本体２の基準ヨー軸Zを鉛直に対して平行にして静止させた状態としての静止状態における、ピッチ軸角速度センサとロール軸角速度センサの出力を取得することで、静止状態におけるピッチ軸角速度センサとロール軸角速度センサのバイアス値を取得するステップ。（２）倒立二輪車本体２の基準ヨー軸Zを鉛直に対して平行にした状態のままで倒立二輪車１を所定の旋回角速度γ１ドットで旋回させた旋回状態における、ピッチ軸角速度センサとロール軸角速度センサの出力を取得することで、旋回状態におけるピッチ軸角速度センサとロール軸角速度センサのバイアス値を取得するステップ。（３）静止状態における各センサのバイアス値と、旋回状態における各センサのバイアス値と、所定の旋回角速度γ１ドットと、に基づいて、ピッチ軸角速度センサとロール軸角速度センサの、倒立二輪車本体２に対する取り付け角度誤差を推定するステップ。以上の方法によれば、ピッチ軸角速度センサとロール軸角速度センサの、倒立二輪車本体２に対する取り付け角度の誤差を推定することができる。

（第２実施形態）
次に、図６を参照しつつ、本願発明の第２実施形態を説明する。ここでは、本実施形態が上記第１実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。

本実施形態では、水平旋回装置３０を用いる代わりに、簡単な水準器を用いて静止状態を作り出し、その後、人員が実際に倒立二輪車１に搭乗して、搭乗員によるハンドル体５の操作によって倒立二輪車１を超信地旋回させることとする。図６に示す制御フローを用いて説明すると、S310では、水準器を用いて静止状態を作り出している。そして、S315で人員が倒立二輪車１に搭乗している。S350とS360、S390も搭乗員によるハンドル体５の操作によって行われ、S415で搭乗員は倒立二輪車１から降車している。

１倒立二輪車
２倒立二輪車本体
３右輪
４左輪
１８姿勢角度演算部
１９倒立制御演算部
２２角速度記憶部
２３取り付け角度誤差演算部
２４取り付け角度誤差記憶部
２５角速度補正部

本願発明は、倒立移動体及び角速度センサの出力値補正方法に関する。

本願発明の第１の観点によれば、倒立移動体本体と、前記倒立移動体本体に取り付けられたピッチ軸角速度センサ及びロール軸角速度センサと、を備えた倒立移動体であって、前記倒立移動体本体は、制御装置を有し、前記制御装置は、前記倒立移動体本体の基準ヨー軸を鉛直方向で静止させた状態としての静止状態における、前記ロール軸角速度センサの出力を取得することで、前記静止状態における前記ロール軸角速度センサのバイアス値を取得し、前記倒立移動体本体の前記基準ヨー軸を鉛直に対して平行にした状態のままで前記倒立移動体を所定の旋回角速度で旋回させた旋回状態における、前記ロール軸角速度センサの出力を取得することで、前記旋回状態における前記ロール軸角速度センサのバイアス値を取得し、前記静止状態における前記ロール軸角速度センサのバイアス値と、前記旋回状態における前記ロール軸角速度センサのバイアス値と、前記所定の旋回角速度と、に基づいて、前記ピッチ軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差を演算し、前記ピッチ軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差に基づいて、前記ロール軸角速度センサの出力値を補正する、倒立移動体が提供される。
好ましくは、前記制御装置は、前記静止状態における前記ロール軸角速度センサのバイアス値と前記旋回状態における前記ロール軸角速度センサのバイアス値との差分と、前記所定の旋回角速度と、に基づいて、前記ピッチ軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差を演算する。
本願発明の第２の観点によれば、倒立移動体本体と、前記倒立移動体本体に取り付けられたピッチ軸角速度センサ及びロール軸角速度センサと、を備えた倒立移動体であって、前記倒立移動体本体は、制御装置を有し、前記制御装置は、前記倒立移動体本体の基準ヨー軸を鉛直方向で静止させた状態としての静止状態における、前記ピッチ軸角速度センサの出力を取得することで、前記静止状態における前記ピッチ軸角速度センサのバイアス値を取得し、前記倒立移動体本体の前記基準ヨー軸を鉛直に対して平行にした状態のままで前記倒立移動体を所定の旋回角速度で旋回させた旋回状態における、前記ピッチ軸角速度センサの出力を取得することで、前記旋回状態における前記ピッチ軸角速度センサのバイアス値を取得し、前記静止状態における前記ピッチ軸角速度センサのバイアス値と、前記旋回状態における前記ピッチ軸角速度センサのバイアス値と、前記所定の旋回角速度と、に基づいて、前記ロール軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差を演算し、前記ロール軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差に基づいて、前記ピッチ軸角速度センサの出力値を補正する、倒立移動体が提供される。
好ましくは、前記制御装置は、前記静止状態における前記ピッチ軸角速度センサのバイアス値と前記旋回状態における前記ピッチ軸角速度センサのバイアス値との差分と、前記所定の旋回角速度と、に基づいて、前記ロール軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差を演算する。
本願発明の第３の観点によれば、倒立移動体本体と、前記倒立移動体本体に取り付けられたピッチ軸角速度センサとロール軸角速度センサを備えた倒立移動体における、前記ロール軸角速度センサの出力値の補正方法であって、前記倒立移動体本体の基準ヨー軸を鉛直方向で静止させた状態としての静止状態における、前記ロール軸角速度センサの出力を取得することで、前記静止状態における前記ロール軸角速度センサのバイアス値を取得するステップと、前記倒立移動体本体の前記基準ヨー軸を鉛直に対して平行にした状態のままで前記倒立移動体を所定の旋回角速度で旋回させた旋回状態における、前記ロール軸角速度センサの出力を取得することで、前記旋回状態における前記ロール軸角速度センサのバイアス値を取得するステップと、前記静止状態における前記ロール軸角速度センサのバイアス値と、前記旋回状態における前記ロール軸角速度センサのバイアス値と、前記所定の旋回角速度と、に基づいて、前記ピッチ軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差を演算するステップと、前記ピッチ軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差に基づいて、前記ロール軸角速度センサの前記出力値を補正するステップと、を含む、角速度センサの出力値補正方法が提供される。
好ましくは、前記静止状態における前記ロール軸角速度センサのバイアス値と前記旋回状態における前記ロール軸角速度センサのバイアス値との差分と、前記所定の旋回角速度と、に基づいて、前記ピッチ軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差を演算する。
本願発明の第４の観点によれば、倒立移動体本体と、前記倒立移動体本体に取り付けられたピッチ軸角速度センサとロール軸角速度センサを備えた倒立移動体における、前記ピッチ軸角速度センサの出力値の補正方法であって、前記倒立移動体本体の基準ヨー軸を鉛直方向で静止させた状態としての静止状態における、前記ピッチ軸角速度センサの出力を取得することで、前記静止状態における前記ピッチ軸角速度センサのバイアス値を取得するステップと、前記倒立移動体本体の前記基準ヨー軸を鉛直に対して平行にした状態のままで前記倒立移動体を所定の旋回角速度で旋回させた旋回状態における、前記ピッチ軸角速度センサの出力を取得することで、前記旋回状態における前記ピッチ軸角速度センサのバイアス値を取得するステップと、前記静止状態における前記ピッチ軸角速度センサのバイアス値と、前記旋回状態における前記ピッチ角速度センサのバイアス値と、前記所定の旋回角速度と、に基づいて、前記ロール軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差を演算するステップと、前記ロール軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差に基づいて、前記ピッチ軸角速度センサの前記出力値を補正するステップと、を含む、角速度センサの出力値補正方法が提供される。
好ましくは、前記静止状態における前記ピッチ軸角速度センサのバイアス値と前記旋回状態における前記ピッチ軸角速度センサのバイアス値との差分と、前記所定の旋回角速度と、に基づいて、前記ロール軸角速度センサの、前記倒立移動体本体に対する前記取り付け角度誤差を演算する。

先ず、図４に示すように倒立二輪車１を水平旋回装置３０に取り付けて、倒立二輪車本体２の基準ヨー軸を鉛直に対して平行にして静止させた状態としての静止状態とする。次に、制御装置８の制御モードを、停止モードから調整モードへと切り替える（S300）。そして、制御装置８は、上記の静止状態において、加速度センサ１６の出力値X0、Y0を取得する（S310）。このとき、出力値X0、Y0は、十分長い時間かけて平均化させることで、ノイズを除去した精確な値とする。同様に、制御装置８は、上記の静止状態において、角速度センサ１５の出力値α０ドット（ロール軸角速度センサの出力値）、β０ドット（ピッチ軸角速度センサの出力値）、γ０ドットを取得する（S310）。各出力値は誤差補正部１３の角速度記憶部２２に記憶される。このとき、出力値α０ドット、β０ドット、γ０ドットは、十分長い時間かけて平均化させることで、ノイズを除去した精確な値とする。制御装置８は、取得した出力値α０ドット、β０ドット、γ０ドットを、誤差補正部１３の角速度記憶部２２に記憶させる。なお、上記の静止状態では、出力値α０ドット、β０ドット、γ０ドットは、本来であれば当然にゼロとなるべきものであるが、角速度センサのドリフト問題により、実際にはゼロではない値となっている。この意味で、出力値β０ドットは、静止状態における角速度センサ１５のピッチ軸角速度センサのバイアス値である。同様に、出力値α０ドットは、静止状態における角速度センサ１５のロール軸角速度センサのバイアス値である。添字０は、静止状態を示している。

同様に、上記式（９）及び（１０）で求めた各値によって、倒立二輪車本体２に対する加速度センサ１６の取り付け角度の誤差を安価な計算で見かけ上なくすことができる。

Claims

倒立移動体本体と、前記倒立移動体本体に取り付けられたピッチ軸角速度センサとロール軸角速度センサを備えた倒立移動体における、前記ピッチ軸角速度センサと前記ロール軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差の推定方法であって、
前記倒立移動体本体の基準ヨー軸を鉛直方向で静止させた状態としての静止状態における、前記ピッチ軸角速度センサと前記ロール軸角速度センサの出力を取得することで、前記静止状態における前記ピッチ軸角速度センサと前記ロール軸角速度センサのバイアス値を取得するステップと、
前記倒立移動体本体の前記基準ヨー軸を鉛直に対して平行にした状態のままで前記倒立移動体を所定の旋回角速度で旋回させた旋回状態における、前記ピッチ軸角速度センサと前記ロール軸角速度センサの出力を取得することで、前記旋回状態における前記ピッチ軸角速度センサと前記ロール軸角速度センサのバイアス値を取得するステップと、
前記静止状態における各センサのバイアス値と、前記旋回状態における各センサのバイアス値と、前記所定の旋回角速度と、に基づいて、前記ピッチ軸角速度センサと前記ロール軸角速度センサの、前記倒立移動体本体に対する取り付け角度誤差を推定するステップと、
を含む、取り付け角度誤差の推定方法。
請求項１に記載の取り付け角度誤差の推定方法であって、
前記旋回状態では、前記移動体本体を水平に保持し、その場で旋回させる、
取り付け角度誤差の推定方法。
請求項２に記載の取り付け角度誤差の推定方法であって、
前記倒立移動体を倒立状態で超信地旋回させる、
取り付け角度誤差の推定方法。
請求項２に記載の取り付け角度誤差の推定方法であって、
前記倒立移動体を支持する支持台と、
高さを調整可能であって前記支持台に取り付けられた複数の支持脚と、
前記支持台に対して取り付けられる第１の部材と、前記倒立移動体本体に対して取り付けられる第２の部材と、を有し、前記第２の部材に対して前記第１の部材が前記支持台に対して平行に回転可能に前記第１の部材と前記第２の部材が連結された回転治具と、
を備えた旋回装置を用いて、
前記倒立移動体を超信地旋回させる、
取り付け角度誤差の推定方法。
倒立移動体を支持する支持台と、
高さを調整可能であって前記支持台に取り付けられた複数の支持脚と、
前記支持台に対して取り付けられる第１の部材と、前記倒立移動体本体に対して取り付けられる第２の部材と、を有し、前記第２の部材に対して前記第１の部材が前記支持台に対して平行に回転可能に前記第１の部材と前記第２の部材が連結された回転治具と、
を備えた、
旋回装置。
請求項２に記載の取り付け角度誤差の推定方法に用いられる旋回装置であって、
倒立移動体を支持する支持台と、
高さを調整可能であって前記支持台に取り付けられた複数の支持脚と、
前記支持台に対して取り付けられる第１の部材と、前記倒立移動体本体に対して取り付けられる第２の部材と、を有し、前記第２の部材に対して前記第１の部材が前記支持台に対して平行に回転可能に前記第１の部材と前記第２の部材が連結された回転治具と、
を備える、
旋回装置。