特許 7391616 移動体の姿勢推定装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-11-27

(45)【発行日】2023-12-05

(54)【発明の名称】移動体の姿勢推定装置

(51)【国際特許分類】

   G05D 1/08 20060101AFI20231128BHJP

   B25J 5/00 20060101ALI20231128BHJP

   G05B 11/36 20060101ALI20231128BHJP

【ＦＩ】

G05D1/08 Z

B25J5/00 Z

G05B11/36 G

【請求項の数】 1

(21)【出願番号】P 2019200621

(22)【出願日】2019-11-05

(65)【公開番号】P2021076901

(43)【公開日】2021-05-20

【審査請求日】2021-11-26

(73)【特許権者】

【識別番号】000005326

【氏名又は名称】本田技研工業株式会社

(74)【代理人】

【識別番号】110000800

【氏名又は名称】デロイトトーマツ弁理士法人

(72)【発明者】

【氏名】黒須 智博

【審査官】影山 直洋

(56)【参考文献】

【文献】特許第４１８１１１３（ＪＰ，Ｂ２）

【文献】特開２００７－１８３１３８（ＪＰ，Ａ）

【文献】特開平０９－３１８３８２（ＪＰ，Ａ）

【文献】特開２０１１－０５９０２０（ＪＰ，Ａ）

【文献】特開平０６－１７４４８７（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／０８

Ｂ２５Ｊ ５／００

Ｇ０５Ｂ １１／３６

(57)【特許請求の範囲】

【請求項1】

移動体の所定の部位の姿勢を推定する装置であって、
前記所定の部位に搭載された角速度センサの出力から該所定の部位の角速度の推定値を生成する角速度推定部と、
前記所定の部位に搭載された加速度センサの出力から該加速度センサの搭載位置での並進加速度の推定値であって、重力加速度成分を含む推定値を生成する並進加速度推定部と、
前記所定の部位の運動に起因して前記加速度センサの搭載位置で発生する運動加速度の推定値を、前記所定の部位の運動に関する目標値を用いずに、該運動加速度と相関性を有する前記移動体の運動状態量の観測値から生成する運動加速度推定部と、
前記角速度の推定値と前記並進加速度の推定値と前記運動加速度の推定値とから前記所定の部位の姿勢の推定値を生成する姿勢推定部とを備えており、
前記運動加速度推定部は、前記加速度センサの搭載位置で発生する並進運動加速度及び向心加速度を含む運動加速度の推定値を生成するように構成され、
前記姿勢推定部は、前記並進加速度の推定値と前記運動加速度の推定値との差に基づいて、前記所定の部位の基準姿勢を推定する基準姿勢推定部と、生成済の前記所定の部位の最新の姿勢の推定値と前記基準姿勢の推定値との差を該姿勢の推定値の誤差として推定する誤差推定部と、該誤差の推定値に応じて、所定の周期で該誤差をゼロに近づけるようにフィードバック制御則によりフィードバック操作量を生成するフィードバック制御部と、該フィードバック操作量により前記角速度の推定値を補正してなる入力を積分することにより前記所定の部位の姿勢の推定値を生成する積分処理部とを備え、
前記運動状態量の観測値は、前記加速度センサにより検出される並進加速度以外の運動状態量であって、前記所定の周期で前記移動体の運動に応じて観測される運動状態量の観測値であることを特徴とする移動体の姿勢推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体の所定の部位の姿勢を推定する装置に関する。

【背景技術】

【0002】

従来、この種の技術としては、加速度センサ及び角速読センサを含む慣性センサを用いた慣性航法の手法が一般に知られている。

【0003】

また、例えば移動ロボットでは、特許文献１に見られるように、慣性航法の手法に移動ロボットの目標歩容を組み込んだ手法により移動ロボットの上体の姿勢を推定する手法が提案されている。

【先行技術文献】

【特許文献】

【0004】

【文献】特許第４１８１１１３号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

人や荷物を搭乗させる移動体や、公共施設等で人を誘導案内する移動体は、様々な障害物を避けつつ、フレキシブルに移動を行う必要がある。このため、このような移動体は、運動状態の変化が生じやすいと共に、事前に目標運動を設定しておくことが難しい。

【0006】

そして、従来の慣性航法の手法では、移動体の運動状態の変化に応じて姿勢の推定対象の部位の運動加速度が変動すると、該部位の姿勢を安定して精度よく推定することが困難である。また、目標運動を事前に設定しておくことが難しい移動体では、前記特許文献１に見られる技術を適用することができない。

【0007】

本発明はかかる背景に鑑みてなされたものであり、運動状態の変化が生じやすい移動体であっても、その所定の部位の姿勢を安定に精度よく推定することを可能とする姿勢推定装置を提供することを目的とする。

【課題を解決するための手段】

【0008】

本発明の移動体の姿勢推定装置は、上記の目的を達成するために、移動体の所定の部位の姿勢を推定する装置であって、
前記所定の部位に搭載された角速度センサの出力から該所定の部位の角速度の推定値を生成する角速度推定部と、
前記所定の部位に搭載された加速度センサの出力から該加速度センサの搭載位置での並進加速度の推定値であって、重力加速度成分を含む推定値を生成する並進加速度推定部と、
前記所定の部位の運動に起因して前記加速度センサの搭載位置で発生する運動加速度の推定値を、前記所定の部位の運動に関する目標値を用いずに、該運動加速度と相関性を有する前記移動体の運動状態量の観測値から生成する運動加速度推定部と、
前記角速度の推定値と前記並進加速度の推定値と前記運動加速度の推定値とから前記所定の部位の姿勢の推定値を生成する姿勢推定部とを備えており、
前記運動加速度推定部は、前記加速度センサの搭載位置で発生する並進運動加速度及び向心加速度を含む運動加速度の推定値を生成するように構成され、
前記姿勢推定部は、前記並進加速度の推定値と前記運動加速度の推定値との差に基づいて、前記所定の部位の基準姿勢を推定する基準姿勢推定部と、生成済の前記所定の部位の最新の姿勢の推定値と前記基準姿勢の推定値との差を該姿勢の推定値の誤差として推定す
る誤差推定部と、該誤差の推定値に応じて、所定の周期で該誤差をゼロに近づけるようにフィードバック制御則によりフィードバック操作量を生成するフィードバック制御部と、該フィードバック操作量により前記角速度の推定値を補正してなる入力を積分することにより前記所定の部位の姿勢の推定値を生成する積分処理部とを備え、
前記運動状態量の観測値は、前記加速度センサにより検出される並進加速度以外の運動状態量であって、前記所定の周期で前記移動体の運動に応じて観測される運動状態量の観測値であることを特徴とする。

【0009】

かかる本発明によれば、前記加速度センサの出力に基づく並進加速度の推定値には、重力加速度の成分に加えて、移動体の運動に起因する運動加速度の成分が含まれるものの、該運動加速度は、移動体の運動状態量（加速度センサにより検出される並進加速度以外の運動状態量）の観測値から前記運動加速度推定部により推定される。また、該運動加速度は、前記所定の部位の運動に関する目標値を用いずに生成される。

【0010】

なお、運動状態量の「観測値」は、適宜のセンサによる該運動状態量の検出値、又は該運動状態量と一定の相関関係を有する他の１つ以上の状態量の検出値から、該相関関係に基づいて推定してなる推定値を意味する。

【0011】

そして、前記並進加速度の推定値と前記運動加速度の推定値との差は、主に重力加速度に依存したものとなる。このため、当該差に基づいて、前記所定の部位の基準姿勢を推定することで、前記並進加速度の推定値に含まれる重力加速度の方向が正しい方向（鉛直方向）になるように基準姿勢の推定値を決定できる。従って、生成済の前記所定の部位の最新の姿勢の推定値と前記基準姿勢の推定値との差を該姿勢の推定値の誤差として推定することで、信頼性の高い当該誤差を推定することが可能となる。さらに、この誤差をゼロに近づけるようにフィードバック制御則により生成したフィードバック操作量により、前記角速度センサの出力に基づく前記角速度推定値を補正してなる入力を積分することで、上記誤差がゼロに近づくように、移動体の所定の部位の姿勢が推定されることになる。

【0012】

この結果、本発明によれば、運動状態の変化が生じやすい移動体であっても、その所定の部位の姿勢を安定に精度よく推定することを可能となる。

【0014】

更に、前記並進運動加速度と、前記向心加速度とが前記運動加速度として推定されるので、移動体の運動状態によらずに、移動体の所定の部位の姿勢の推定値の精度を高めることができる。

【図面の簡単な説明】

【0015】

【図1】本発明の第１実施形態における移動体の模式的構成を示す図。

【図2】移動体に備えた演算処理装置の機能を示すブロック図。

【図3】本発明の第２実施形態における移動体の模式的構成を示す図。

【発明を実施するための形態】

【0016】

［第１実施形態］
本発明の第１実施形態を図１及び図２を参照して説明する。図１を参照して本実施形態における移動体１Ａは、例えば倒立振子型の移動体である。詳細な図示は省略するが、この移動体１Ａは、床面上をアクチュエータ（図示せず）の駆動力により回転しながら移動する車輪状又は球体状の移動動作部２と、該移動動作部２に傾動可能に支持された基体３とを備え、倒立振子の如く、基体３の重心Ｇ１のバランスを保つように移動動作部２の移動制御が行われる移動体である。なお、重心Ｇ１は、基体３に人、荷物等の物体が搭載される場合には、該物体を含めた重心である。

【0017】

このような移動体１Ａは公知のものでよい。例えば、特開２０１５－０９３６５１号公報等に見られるように車輪状の移動動作部を備える構造のものを、移動体１Ａとして採用し得る。また、例えば、特開平４－２０１７９３号公報等に見られるように球体状の移動動作部を備える構造のものを移動体１Ａとして採用することもできる。さらに、例えば本願出願人が、特開２０１９－１６６８６３号公報にて提案した構造のもの（球体状の移動動作部を、これに接触させた複数のローラを介して駆動する構造の移動体）を、移動体１Ａとして採用することもできる。

【0018】

移動体１Ａの基体３には、３軸の（３次元の）加速度を検出可能な加速度センサ６と３軸の（３次元の）角速度を検出可能な角速度センサ７とを含む慣性センサ８と、様々な演算処理を行い得る演算処理装置１０とが搭載されている。

【0019】

演算処理装置１０は、例えば、マイクロコンピュータもしくはプロセッサ、メモリ（ＲＡＭ、ＲＯＭ等）、インターフェース回路等を含む電子回路ユニットにより構成され、慣性センサ８の検出出力を含めて、移動体１Ａに備えられた様々なセンサの検出出力が入力される。

【0020】

そして、演算処理装置１０は、実装されたハードウェア構成及びプログラム（ソフトウェア）により実現される機能として、本実施形態では、姿勢推定装置としての機能を含み、この機能により、移動体１Ａの所定の部位としての基体３の実際の姿勢を逐次推定することが可能である。ここで、基体３の「姿勢」はより詳しくは、水平もしくはほぼ水平な軸周りの方向での基体３の傾斜姿勢（傾き）を意味する。該傾斜姿勢は、例えば、オイラー角により表現される。以降、基体３の姿勢を表すオイラー角を姿勢角という。

【0021】

次に、図２を参照して演算処理装置１０が実行する基体３の姿勢の推定処理を具体的に説明する。ここで、該推定処理に関して補足すると、本実施形態では、図１に示すように、Ｚ軸が上下方向（鉛直方向又はほぼ鉛直な方向）の軸、これに直交するＸ軸及びＹ軸のうちのＸ軸が移動体１Ａの前後方向（図１では紙面の横方向）の軸、Ｙ軸が移動体１Ａの左右方向（図１では紙面に垂直な方向）の軸として移動体１Ａの移動環境に設定（定義）された３軸直交座標系が、基体３の姿勢等の運動状態量を表現する座標系（以降、基準座標系という）として用いられる。この場合、基準座標系のＸ軸周りの方向、Ｙ軸周りの方向、Ｚ軸周りの方向がそれぞれ、移動体１Ａのロール方向、ピッチ方向、ヨー方向に相当する。

【0022】

以降、Ｘ軸、Ｙ軸、Ｚ軸は、特にことわらない限り、基準座標系の座標軸を意味する。また、以降の説明では、角速度、加速度等の任意の状態量のＸ軸方向（又はＸ軸周り方向）の成分、Ｙ軸方向（又はＹ軸周り方向）の成分、Ｚ軸方向（又はＺ軸周り方向）の成分を表す場合に、該状態量の参照符号に添え字「_x」、「_y」、「_z」を付する。例えば状態量ＱのＸ軸方向（又はＸ軸周り方向）の成分をＱ_xと表記する。さらに、任意の状態量Ｑの特定の２つの座標成分の組、例えばＸ軸方向の成分及びＹ軸方向の成分の組（又はＸ軸周り方向の成分及びＹ軸周り方向の成分の組）を表す場合には、当該座標成分の組をＱ_x,yというように表記する。また、以降の説明では、任意の状態量Ｑの推定値を単に、状態量推定値Ｑというように表記する場合がある。

【0023】

図２を参照して、演算処理装置１０は、基体３の姿勢の推定処理のための機能として、基準座標系で見た基体３の角速度を推定する角速度推定部１１、基体３の加速度センサ６の搭載位置（以降、単にセンサ位置という）での並進加速度（基準座標系で見た並進加速度）を推定する並進加速度推定部１２と、移動体１Ａの運動に起因して基体３のセンサ位置で発生する加速度である運動加速度（基準座標系で見た運動加速度）を推定する運動加速度推定部１３と、上記角速度、並進加速度及び運動加速度のそれぞれの推定値を用いて基体３の姿勢を表す姿勢角を推定する姿勢推定部１４とを備えており、これらの各機能部の処理を所定の演算処理周期で逐次実行する。なお、姿勢推定部１４が推定する姿勢角は、Ｘ軸周り方向（ロール方向）の姿勢角と、Ｙ軸周り方向（ピッチ方向）の姿勢角との組である。

【0024】

角速度推定部１１には、角速度センサ７の検出出力が入力される。そして、角速度推定部１１は、入力された検出出力により示される角速度検出値（角速度センサ７に対して設定されたセンサ座標系で見た角速度の検出値）を、姿勢推定部１４により既に生成された基体３の姿勢角推定値θb_x,yの最新値（前回の演算処理周期での推定値。以降、前回姿勢角推定値θb_p_x,yという）を用いて座標変換することで、基準座標系で見た基体３の角速度推定値ωb（ωb_x，ωb_y，ωb_zの組）を生成して出力する。

【0025】

並進加速度推定部１２には、加速度センサ６の検出出力が入力される。そして、並進加速度推定部１２は、入力された検出出力により示される並進加速度検出値（加速度センサ６に対して設定されたセンサ座標系で見た並進加速度の検出値）を、姿勢推定部１４により既に生成された基体３の前記前回姿勢角推定値θb_p_x,yを用いて座標変換することで、基準座標系で見た基体３のセンサ位置での並進加速度推定値Ａb（Ａb_x，Ａb_y，Ａb_zの組）を生成して出力する。なお、加速度センサ６が検出する並進加速度には、重力加速度の成分が含まれるので、上記並進加速度推定値Ａbにも重力加速度の成分が含まれる。

【0026】

運動加速度推定部１３には、基体３の運動加速度に関連する（相関性を有する）移動体１Ａの運動状態量の観測値（検出値又は推定値）が入力される。この運動状態量の観測値には、加速度センサ６が検出する並進加速度の検出値や、並進加速度推定部１２が生成した並進加速度推定値Ａbは含まれない。

【0027】

ここで、本実施形態では、運動加速度推定部１３が推定する運動加速度は、詳しくは、移動体１Ａの運動によって基体３のセンサ位置で発生する水平方向（Ｘ軸方向及びＹ軸方向）の並進運動加速度と、移動体１Ａの旋回運動によって基体３に発生する水平方向（Ｘ軸方向及びＹ軸方向）の向心加速度との２種類の運動加速度である。

【0028】

そして、これらの並進運動加速度及び向心加速度を推定するために、運動加速度推定部１３には、角速度推定部１１で生成された角速度推定値ωb（ωb_x，ωb_y，ωb_zの組）と、移動体１Ａの車輪状又は球体状の移動動作部２のロール方向（Ｘ軸周り方向）及びピッチ方向（Ｙ軸周り方向）の回転角速度推定値ωw_x,yとが入力される。

【0029】

この場合、移動動作部２の回転角速度は、移動体１Ａが、例えば特開２０１５－０９３６５１号公報等に見られる如き車輪状の移動動作部を有する移動体である場合、車輪状の移動動作部２を駆動する各モータ（図示せず）の出力軸の回転角速度の観測値から推定することができる。この場合、各モータの出力軸の回転角速度は、例えばロータリーエンコーダ、レゾルバ等を用いて検出し得る。

【0030】

また、移動体１Ａが、例えば特開平４－２０１７９３号公報等に見られる如き球体状の移動動作部を有する移動体（又は特開２０１９－１６６８６３号公報に見られる構造の移動体）である場合、球体状の移動動作部２を駆動する各モータ（図示せず）の出力軸の回転角速度の観測値と、基体３の姿勢角の観測値とから移動動作部２の回転角速度推定値ωw_x,yを推定することができる。この場合、各モータの出力軸の回転角速度は、例えばロータリーエンコーダ、レゾルバ等を用いて検出し得る。また、基体３の姿勢角の観測値としては、姿勢推定部１４により求められた前回姿勢角推定値θb_p_x,yを用いることができる。

【0031】

本実施形態では、上記のように、基体３の角速度推定値ωbと、車輪状又は球体状の移動動作部２の回転角速度推定値ωw_x,yとが運動状態量の観測値として運動加速度推定部１３に入力される。

【0032】

運動加速度推定部１３は、運動加速度のうちの向心加速度を推定するために、入力された運動状態量のうち、基体３のロール方向及びピッチ方向の角速度推定値ωb_x,yと、移動動作部２の回転角速度推定値ωw_x,yとから次式（１ａ），（１ｂ）に示す運動学モデルの演算により、基体３の重心Ｇ１の並進速度推定値Ｖbg_x,yを算出する。なお、本明細書では、「＊」は乗算を意味する算術記号である。

【0033】

Ｖbg_x＝ｒ1＊(ωw_y＋ωb_y)＋ｈ1＊ωb_y ……（１ａ）
Ｖbg_y＝－ｒ1＊(ωw_x＋ωb_x)－ｈ1＊ωb_x ……（１ｂ）

【0034】

ここで、図１を参照して、式（１ａ），（１ｂ）におけるｒ1は、移動動作部２の回転半径、ｈ1は、移動動作部２の回転軸線上に位置する基体３の傾動支点Ｃ１から重心Ｇ１までの距離であり、これらの値は、あらかじめ定められた設定値である。なお、移動動作部２が、特開２０１５－０９３６５１号公報等に見られる如く車輪状の移動動作部である場合、上記ｒ1，ｈ1のそれぞれの値は、式（１ａ）と式（１ｂ）とで互いに異なる値に設定され得る。

【0035】

そして、式（１ａ），（１ｂ）の右辺の第１項は、移動動作部２の回転と、基体３の傾動とに起因して発生する移動動作部２の並進速度（Ｘ軸方向及びＹ軸方向のそれぞれの並進速度）を意味し、式（１ａ），（１ｂ）の右辺の第２項は、移動動作部２に対する基体３の相対的な傾動に起因して、移動動作部２に対して相対的に発生する重心Ｇ１の並進速度（Ｘ軸方向及びＹ軸方向のそれぞれの並進速度）を意味する。

【0036】

運動加速度推定部１３は、上記の如く求めた基体３の重心Ｇ１の並進速度推定値Ｖbg_x,yと、入力された運動状態量のうちの基体３のヨー方向の角速度推定値ωb_zとから、向心加速度推定値Ａbmr_x,yを求める。具体的は、並進速度推定値Ｖbg_x,y及び角速度推定値ωb_zから次式（２ａ），（２ｂ）により向心加速度推定値Ａbmr_x,yが算出される。

【0037】

Ａbmr_x＝－ωb_z＊Ｖbg_y ……（２ａ）
Ａbmr_y＝ωb_z＊Ｖbg_x ……（２ｂ）

なお、向心加速度推定値Ａbmr_x,yは、基体３の全体に発生する向心加速度推定値であるので、基体３のセンサ位置での向心加速度の推定値としての意味も持つ。

【0038】

また、運動加速度推定部１３は、運動加速度のうち並進運動加速度を推定するために、入力された運動状態量のうち、基体３の角速度推定値ωb（ωb_x，ωb_y，ωb_zの組）と、移動動作部２の回転角速度推定値ωw_x,yとから次式（３ａ），（３ｂ）により示される運動学モデルの演算により、基体３のセンサ位置での並進運動加速度推定値Ａbmt _x,yを算出する。なお、本明細書では、「×」はベクトル積演算（外積演算）を意味する算術記号である。

【0039】

Ａbmt_x＝ｒ1＊(ωw_dot_y＋ωb_dot_y)＋(↑ωb_dot×↑ｐ1)_x ……（３ａ）
Ａbmt_y＝－ｒ1＊(ωw_dot_x＋ωb_dot_x)＋(↑ωb_dot×↑ｐ1)_y ……（３ｂ）

【0040】

ここで、式（３ａ），（３ｂ）におけるｒ1は、前記式（１ａ），（１ｂ）に示したものと同じであり、ωw_dot_x、ωw_dot_yはそれぞれ回転角速度推定値ωw_x、ωw_yの１階微分値（時間的変化率）である。また、↑ωb_dotは、角速度推定値ωbのベクトル↑ωb（ωb_x，ωb_y，ωb_zを３つの成分とするベクトル）の１階微分値（時間的変化率）、↑ｐ1は、基体３の傾動支点Ｃ１に対するセンサ位置（加速度センサ６の搭載位置）の位置ベクトル（図１を参照）である。そして、(↑ωb_dot×↑ｐ1)_xは、ベクトル積(↑ωb_dot×↑ｐ1)のＸ軸方向成分、(↑ωb_dot×↑ｐ1)_yは、ベクトル積(↑ωb_dot×↑ｐ1)のＹ軸方向成分である。

【0041】

本実施形態では、運動加速度推定部１３は、以上の如く、向心加速度推定値Ａbmr_x,yと、並進運動加速度推定値Ａbmt _x,yとを、移動体１Ａの運動に起因して、基体３のセンサ位置で発生する運動加速度として求める。

【0042】

次に、姿勢推定部１４には、上記角速度推定部１１で求められた角速度推定値ωbのうちのロール方向及びピッチ方向の角速度推定値ωb_x,yと、並進加速度推定部１２で求められた並進加速度推定値Ａb（Ａb_x，Ａb_y，Ａb_zの組）と、運動加速度推定部１３で求められた向心加速度推定値Ａbmr_x,y及び並進運動加速度推定値Ａbmt_x,yとが入力される。そして、姿勢推定部１４は、これらの入力値から以下に説明する如く、基体３の姿勢角推定値θb_x,yを求める。

【0043】

姿勢推定部１４は、まず、入力された並進加速度推定値Ａbと、運動加速度推定部１３で求められた向心加速度推定値Ａbmr_x,y及び並進運動加速度推定値Ａbmt _x,yとから、基体３の基準姿勢角を推定する処理を基準姿勢角推定部１４ａにより実行する。該基準姿勢角推定部１４ａは、本発明における基準姿勢推定部に相当する。

【0044】

ここで、前記並進加速度推定部１２で、並進加速度推定値Ａbを求めるために使用した前回姿勢角推定値θb_p_x,yに誤差があると、求めた並進加速度推定値Ａbに含まれる重力加速度の成分の方向と、基準座標系のＺ軸方向との間にずれが生じる。

【0045】

そこで、基準姿勢角推定部１４ａでは、今回の（現在の）演算処理周期で姿勢推定部１４が新たに求める姿勢角推定値θb_xy（以降、今回姿勢角推定値θb_x,yという）を、上記ずれを抑制すべく補正するための基準とする基体３の姿勢角を基準姿勢角推定値θbb_x,yとして求める。

【0046】

具体的には、基準姿勢角推定部１４ａは、次式（４ａ），（４ｂ）で示す如く、入力された並進加速度推定値ＡbうちのＸ軸方向及びＹ軸方向の並進加速度推定値Ａb_x,yから、向心加速度推定値Ａbmr_x,y及び並進運動加速度推定値Ａbmt _x,yのそれぞれを差し引くことにより、基準座標系で見た、重力加速度ベクトルのＸ軸方向成分ｇ_x及びＹ軸方向成分ｇ_yを求める。

【0047】

ｇ_x＝Ａb_x－Ａbmr_x－Ａbmt _x ……（４ａ）
ｇ_y＝Ａb_y－Ａbmr_y－Ａbmt _y ……（４ｂ）

【0048】

これらの式（４ａ），（４ｂ）により求められるｇ_x，ｇ_yは、それぞれ、基準座標系で見た重力加速度ベクトルのＸ軸方向の成分、Ｙ軸方向の成分に相当するものとなる。そして、基準姿勢角推定値θbb_x,yは、近似的に次式（５ａ），（５ｂ）により推定され得る。

【0049】

θbb_ x＝－ｇ_y／ｇc ……（５ａ）
θbb_y＝ｇ_x／ｇc ……（５ｂ）

【0050】

ここで、θbb_ xは、ロール方向（Ｘ軸周り方向）の基準姿勢角、θbb_ yは、ピッチ方向（Ｙ軸周り方向）の基準姿勢角、ｇcは、重力加速度定数である。上記式（５ａ），（５ｂ）により、基準座標系で見た重力加速度ベクトルに基づく、基体３の姿勢角推定値としての基準姿勢角推定値θbb_x,yが求められる。これにより前記並進加速度推定値Ａbに含まれる重力加速度ベクトルの方向が正しい方向（鉛直方向）になるように基準姿勢角推定値をθbb_x,yを決定できる。

【0051】

姿勢推定部１４は、次に、演算部１４ｂで基体３の前回姿勢角推定値θb_x，θb_ yのそれぞれから、上記の如く求めた基準姿勢角推定値θbb_x，θbb_yのそれぞれを減算することにより、前回姿勢角推定値θb_p_xyの誤差（以降、姿勢角誤差という）の推定値としての姿勢角誤差推定値θb_err_x,yを算出する。該演算部１４ｂは、本発明における誤差推定部に相当する。そして、この姿勢角誤差推定値θb_err_x,yを、フィードバック制御部１４ｃに入力し、該フィードバック制御部１４ｃにより、姿勢角誤差推定値θb_err_x，θb_err_yのそれぞれをフィードバック制御則によりゼロに近づけるように、基体３の角速度推定値ωb_x，ωb_xのそれぞれの補正用のフィードバック操作量δω_x，δω_yを求める。

【0052】

フィードバック制御部１４ｃでは、フィードバック制御則として、例えば、ＰＩ則（比例・積分則）を使用し得る。この場合には、フィードバック操作量δω_x，δω_yは、次式（６ａ），（６ｂ）により算出される。

【0053】

δω_x＝k1_x＊θb_err_x＋k2_x＊∫θb_err_x dt ……（６ａ）
δω_y＝k1_y＊θb_err_y＋k2_y＊∫θb_err_y dt ……（６ｂ）

【0054】

これらの式（６ａ），（６ｂ）におけるk1_x，k2_x，k1_y，k2_yは所定値のゲインである。上記式（６ａ），（６ｂ）により、基体３の角速度推定値ωb_x，ωb_xのオフセットや、移動体１Ａの基体３に設定されるローカル座標系と座標軸の向きの誤差等に起因して生じる姿勢角誤差を低減し得るように、フィードバック操作量δω_x，δω_yが算出される。補足すると、フィードバック制御部１４ｃで使用するフィードバック制御則は、ＰＩ則に限らず、例えばＰＩＤ則（比例・微分・積分則）等を使用してもよい。

【0055】

姿勢推定部１４は、次に、演算部１４ｄにおいて、角速度推定部１１により求められた基体３の角速度推定値ωb_x，ωb_xのそれぞれから、フィードバック制御部１４ｃで求めたフィードバック操作量δω_x，δω_yのそれぞれを減算することにより、角速度推定値ωb_x，ωb_xのそれぞれを補正する。そして、この補正後の角速度推定値を積分器１４ｅにより積分することにより、基体３の今回姿勢角推定値θb_x,yを求める。なお、積分器１４ｅは、本発明における積分処理部に相当する。

【0056】

本実施形態では、演算処理装置１０は、以上説明した如く、基体３の姿勢指定処理を所定の演算処理周期で逐次実行する。かかる本実施形態によれば、加速度センサ６の検出出出力に基づく並進加速度推定値Ａbから、移動体１Ａの実際の運動状態量から推定した基体３の向心加速度推定値Ａbmr_x,yと、基体３のセンサ位置での並進運動加速度推定値Ａbmt_x,yを差し引くことにより、移動体１Ａの様々な運動状態において、信頼性の高い重力加速度ベクトル（基準座標系で見た重力加速度ベクトル）を求めることができる。ひいてはこの重力加速度ベクトルの方向と基準座標系のＺ軸方向とのずれ量を示すものとしての、重力加速度ベクトルのＸ軸方向成分ｇ_x及びＹ軸方向成分ｇ_yから、信頼性の高い基準姿勢角推定値θbb_x,yを求めることができる。

【0057】

そして、この基準姿勢角推定値θbb_x,yと前回姿勢角指定値θb_p_x,yとの偏差として求められる姿勢角誤差推定値θb_err_x,yをゼロに収束させるようにフィードバック制御則により求めたフィードバック操作量δω_x，δω_yにより、角速度センサ７の検出出力に基づく角速度推定値ωb_x,yを補正した上で、その補正後の角速度推定値を積分することによって基体３の姿勢角推定値θb_x,yが算出される。これにより、移動体１Ａの様々な運動状態において、継続的に信頼性の高い姿勢角推定値θb_x,yを求めることができる。

【0058】

［第２実施形態］
次に、図３を参照して本発明の第２実施形態を説明する。なお、本実施形態では、第１実施形態と同一の事項については詳細な説明を省略する。本実施形態では、移動体１Ｂの構成が第１実施形態の移動体１Ａと相違しており、複数の車輪２２と、これらの車輪２２に図示しないサスペンション機構を介して支持された基体（車体）２３とを備える。車輪２２は、例えば、前後に一輪ずつ配置された前輪２２ｆ及び後輪２２ｒと、これらの前輪２２ｆ及び後輪２２ｒの間の中間位置（移動体１Ｂの前後方向での中間位置）で左右方向（図３では紙面に垂直な方向）に間隔を存して配置された２つの中間車輪２２ｍとを備える。そして、例えば、前輪２２ｆ及び後輪２２ｒが従動輪、中間車輪２２ｍが駆動輪である。なお、各車輪２２は、通常の自動車の車輪と同様のものでもよいが、例えば、特開２０１５－０９３６５１号公報等に見られるように車輪状の移動動作部と同様の構造のものであってもよい。

【0059】

そして、基体２３が、中間車輪２２ｍの回転軸上の点Ｃ２を支点として、ロール方向及びピッチ方向に傾動し得るように、基体２３と複数の車輪２２との間のサスペンション機構が構成されている。

【0060】

また、基体２３に第１実形態とものと同様に構成された慣性センサ８（加速度センサ６及び角速度センサ７を含む）と、演算処理装置１０とが搭載されている。演算処理装置１０は、第１実施形態のものと同様に、姿勢推定装置としての機能を含み、この機能により、移動体１Ｂの所定の部位としての基体２３の実際の姿勢を逐次推定することが可能である。

【0061】

次に、本実施形態の演算処理装置１０が実行する基体２３の姿勢の推定処理を具体的に説明する。なお、本実施形態の演算処理装置１０の処理では、第１実施形態と同様に設定される３軸直交座標系（図３に示す）が、基体３の姿勢等の運動状態量を表現する基準座標系として用いられる。

【0062】

本実施形態の演算処理装置１０の処理は、第１実施形態と同様に、図２に示した角速度推定部１１、並進加速度推定部１２、運動加速度推定部１３及び姿勢推定部１４を備える。この場合、本実施形態では、運動加速度推定部１３の一部の処理だけが相違し、その他の処理は、第１実施形態と同じである。そこで、以下に運動加速度推定部１３の処理を、第１実施形態と相違する事項を中心に説明する。

【0063】

本実施形態では、運動加速度推定部１３には、角速度推定部１１で生成された角速度推定値ωb（ωb_x，ωb_y，ωb_zの組）と、移動体１Ｂの車輪２２のうちの例えば中間車輪２２ｍの回転角速度推定値ωw_x,yとが、移動体１Ｂの運動状態量の観測値として入力される。該回転角速度推定値ωw_x,yは、例えば中間車輪２２ｍの回転を検出可能なロータリエンコーダ、レゾルバ等の検出出力から推定され得る。

【0064】

なお、中間車輪２２ｍがピッチ方向にだけ回転し得る車輪である場合には、ロール方向の回転角速度推定値ωw_ｘはゼロでよい。

【0065】

運動加速度推定部１３は、運動加速度のうちの向心加速度を推定するために、入力された運動状態量のうち、基体２３のロール方向及びピッチ方向の角速度推定値ωb_x,yと、中間車輪２２ｍの回転角速度推定値ωw_x,yとから次式（１１ａ），（１１ｂ）に示す運動学モデルの演算により、基体２３の重心Ｇ２の並進速度推定値Ｖbg_x,yを算出する。なお、式（１１ａ），（１１ｂ）では、２つの中間車輪２２ｍの回転角速度推定値ωw_x,yの平均値をωw_x,yの値として用いてもよい。

【0066】

Ｖbg_x＝ｒ2＊ωw_y＋ｈ2＊ωb_y ……（１１ａ）
Ｖbg_y＝－ｒ2＊ωw_x－ｈ2＊ωb_x ……（１１ｂ）

【0067】

ここで、図２を参照して、式（１１ａ），（１１ｂ）におけるｒ2は、中間車輪２２ｍの回転半径、ｈ2は、基体２３の傾動支点Ｃ２から重心Ｇ２までの距離であり、これらの値は、あらかじめ定められた設定値である。なお、中間車輪２２ｍが、特開２０１５－０９３６５１号公報等に見られる如く車輪状の移動動作部である場合、上記ｒ2，ｈ2のそれぞれの値は、式（１１ａ）と式（１１ｂ）とで互いに異なる値に設定され得る。

【0068】

そして、式（１１ａ），（１１ｂ）の右辺の第１項は、中間車輪２２ｍの中心の並進速度（Ｘ軸方向及びＹ軸方向のそれぞれの並進速度）を意味し、式（１１ａ），（１１ｂ）の右辺の第２項は、中間車輪２２ｍに対する基体２３の相対的な傾動に起因して、中間車輪２２ｍに対して相対的に発生する重心Ｇ２の並進速度（Ｘ軸方向及びＹ軸方向のそれぞれの並進速度）を意味する。

【0069】

運動加速度推定部１３は、上記の如く求めた基体３の重心Ｇ１の並進速度推定値Ｖbg_x,yと、入力された運動状態量のうちの基体３のヨー方向の角速度推定値ωb_zとから、第１実施形態と同様に、前記式（２ａ），（２ｂ）により向心加速度推定値Ａbmr_x,yを算出する。

【0070】

また、運動加速度推定部１３は、運動加速度のうち並進運動加速度を推定するために、入力された運動状態量のうち、基体２３の角速度推定値ωb（ωb_x，ωb_y，ωb_zの組）と、中間車輪２２ｍの回転角速度推定値ωw_x,yとから次式（２３ａ），（２３ｂ）により示される運動学モデルの演算により、基体２３のセンサ位置での並進運動加速度推定値Ａbmt _x,yを算出する。

【0071】

Ａbmt_x＝ｒ2＊ωw_dot_y＋(↑ωb_dot×↑ｐ2)_x ……（２３ａ）
Ａbmt_y＝－ｒ2＊ωw_dot_x＋(↑ωb_dot×↑ｐ2)_y ……（２３ｂ）

【0072】

ここで、式（２３ａ），（２３ｂ）におけるｒ2は、前記式（２１ａ），（２１ｂ）に示したものと同じであり、ωw_dot_x、ωw_dot_yはそれぞれ回転角速度推定値ωw_x、ωw_yの１階微分値（時間的変化率）である。また、↑ωb_dotは、角速度推定値ωbのベクトル↑ωb（ωb_x，ωb_y，ωb_zを３つの成分とするベクトル）の１階微分値（時間的変化率）、↑ｐ2は、基体２３の傾動支点Ｃ２に対するセンサ位置（加速度センサ６の搭載位置）の位置ベクトル（図３を参照）である。そして、(↑ωb_dot×↑ｐ2)_xは、ベクトル積(↑ωb_dot×↑ｐ2)のＸ軸方向成分、(↑ωb_dot×↑ｐ2)_yは、ベクトル積(↑ωb_dot×↑ｐ2)のＹ軸方向成分である。

【0073】

本実施形態では、運動加速度推定部１３は、以上の如く、向心加速度推定値Ａbmr_x,yと、並進運動加速度推定値Ａbmt _x,yとを、移動体１Ｂの運動に起因して、基体２３のセンサ位置で発生する運動加速度として求める。以上説明した事項以外の演算処理装置１０の処理は、第１実施形態と同じである。かかる本実施形態では、第１実施形態と同様の効果を奏することができる。

【0074】

なお、本実施形態では、中間車輪２２ｍを備える移動体１Ｂについて説明したが、中間者車輪２２ｍを備えない移動体、例えば、前輪及び後輪をそれぞれ２ずつ備える４輪の移動体、あるいは、前輪及び後輪の一方を１つ、他方を２つ備える３輪の移動体であってもよい。

【符号の説明】

【0075】

１Ａ，１Ｂ…移動体、３，２３…基体（所定の部位）、６…加速度センサ、７…角速度センサ、１０…演算処理装置（姿勢推定装置）１１…角速度推定部、１２…並進加速度推定部、１３…運動加速度推定部、１４…姿勢推定部、１４ａ…基準姿勢角推定部（基準姿勢推定部）、１４ｂ…演算部（誤差推定部）、１４ｃ…フィードバック制御部、１４ｅ…積分器（積分処理部）。

【図1】

【図2】

【図3】