JP6149791B2

JP6149791B2 - 重心推定装置、及び重心推定方法

Info

Publication number: JP6149791B2
Application number: JP2014089943A
Authority: JP
Inventors: 将弘土井
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2014-04-24
Filing date: 2014-04-24
Publication date: 2017-06-21
Anticipated expiration: 2034-04-24
Also published as: JP2015208790A

Description

本発明は、ロボットの重心位置及び／又は速度を高精度に推定できる重心推定装置、及び重心推定方法に関するものである。

環境とロボットとが接触する接触力を検出し、該検出した接触力に基づいてＺＭＰ（ZeroMomentPoint）方程式等により幾何学的にロボットの重心位置や速度を推定するロボットが知られている（例えば、特許文献１参照）。

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

しかしながら、上記ロボットにおいては、幾何学的な方法を用いているため、関節部の撓みなどによる関節角度の誤差の影響を受け、高精度にロボットの重心位置や速度を推定するのが困難となる。
本発明は、このような問題点を解決するためになされたものであり、ロボットの重心位置及び／又は速度を高精度に推定できる重心推定装置、及び重心推定方法を提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出する力検出手段と、前記ロボットの胴体部に作用する角速度を検出する角速度検出手段と、前記ロボットの胴体部に作用する加速度を検出する加速度検出手段と、前記ロボットの各関節の角度を検出する角度検出手段と、前記ロボットの各接触点の姿勢及び位置を取得する取得手段と、前記角速度検出手段により検出された角速度と、前記加速度検出手段により検出された加速度と、前記角度検出手段により検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出する角運動量算出手段と、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定する第１推定手段と、を備える、ことを特徴とする重心推定装置である。
この一態様において、前記第１推定手段は、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいて、前記ロボットの重心位置及び速度を状態変数とした状態方程式、および、前記ロボットの重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用して前記ロボットの重心位置及び速度を推定し、前記状態方程式及び観測方程式は、前記ロボットの重心に対して並進方向及び回転方向に作用する力の運動方程式に基づいて導出されてもよい。
この一態様において、前記ロボットは、複数の前記関節と、該各関節を介して相互に連結された複数のリンクと、を有する多関節型ロボットであり、前記角度検出手段により検出された各関節の角度に基づいて各関節の角速度を算出する関節角速度算出手段を更に備え、前記角運動量算出手段は、前記角度検出手段により検出された各関節の角度と、前記関節角速度算出手段により算出された各関節の角速度と、前記胴体部の姿勢角及び角速度と、前記各リンクの質量、位置及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの各リンクの並進運動による角運動量と回転運動による角運動量とを夫々加算して前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記ロボットの胴体部の姿勢角のオフセット値を四元数を用いて定義し、該四元数及び前記角速度検出手段の角速度のオフセット値を状態変数としシステム雑音を含む状態方程式、および、前記胴体部の加速度を観測値とし観測雑音を含む観測方程式、に対してカルマンフィルタを適用して、前記状態変数を推定し、該推定した状態変数に基づいて、前記胴体部の姿勢角及び角速度を推定する第２推定手段を更に備え、前記角運動量算出手段は、前記第２推定手段により推定された胴体部の姿勢角及び角速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記角速度検出手段により検出されたロボットの胴体部の角速度から低周波ノイズを除去するハイパスフィルタと、前記加速度検出手段により検出されたロボットの胴体部の加速度から高周波ノイズを除去するローパスフィルタと、を更に備え、前記角運動量算出手段は、前記ハイパスフィルタからの胴体部の角速度と、前記ローパスフィルタからの胴体部の加速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記取得手段により取得された各接触点の姿勢角に基づいて、前記力検出手段から出力された該力検出手段の座標系の力をグローバル座標系の力に変換する変換手段を更に備えていてもよい。
この一態様において、前記力検出手段は、前記各接触点における、ｘ軸方向、ｙ軸方向、及びｚ軸方向の力と、ｘ軸周り、ｙ軸周り、及びｚ軸周りのモーメント力を検出してもよい。
上記目的を達成するための本発明の一態様は、環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出するステップと、前記ロボットの胴体部に作用する角速度を検出するステップと、前記ロボットの胴体部に作用する加速度を検出するステップと、前記ロボットの各関節の角度を検出するステップと、前記ロボットの各接触点の姿勢及び位置を取得するステップと、前記検出された角速度と、前記検出された加速度と、前記検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出するステップと、前記検出された各接触点の力と、前記取得された各接触点の姿勢及び位置と、前記算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定するステップと、を含む、ことを特徴とする重心推定方法であってもよい。

本発明によれば、ロボットの重心位置及び／又は速度を高精度に推定できる重心推定装置、及び重心推定方法を提供することができる。

多点接触動作を行う際のロボット制御系のフレームワークを説明するための図である。環境と各接触点で接触するロボットの２次元モデルを概略的に示す図である。実施の形態１に係る重心推定装置の概略的なシステム構成を示すブロック図である。実施の形態１に係る多関節型ロボットに搭載された各センサを示す図である。ロボットの重心周りの角運動量を説明するための図である。実施の形態１に係る重心推定方法の処理フローを示すフローチャートである。実施の形態２に係る重心推定装置の概略的なシステム構成を示すブロック図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。
本発明の実施の形態１に係る重心推定装置は、ロボットの重心位置及び速度のうち少なくとも一方を高精度に推定できる。重心推定装置は、例えば、ロボットに搭載されている。ロボットは、重心推定装置により推定されたロボットの重心位置及び速度に基づいて、その手足先等と環境との各接触点に作用する接触力を正確に制御できる。

ここで、ロボットが環境と複数の接触点で接触して動作（多点接触動作）を行う際の、ロボット制御系のフレームワークについて説明する。ロボットは、腕部、脚部、胴体部等で構成された、例えば、多関節型のヒューマノイドロボットとして構成されている。ロボットは、ロボットの重心軌道を生成する軌道生成部と、ロボットの力制御を行う制御部と、実機とモデルとのずれを推定する状態推定部と、を備えている。

（１）接触点計画
軌道生成部は、設定された一連のタスクを実行する際に環境とロボットの手、足等との接触点の位置及び姿勢（時系列の接触点情報）を算出する。例えば、図１に示すように、ロボット１００が、壁に手を接触させつつ机の前まで移動し、机上に手を接触させつつ奥にあるペットボトルを把持するタスクを実行する。このとき、軌道生成部は、どの位置にロボット１００のどのような姿勢で手、足等を接触させて胴体部を支持するか等を、動作計画ソフトウェア（プランナー）、あるいは予めユーザに設定された設定情報を用いて計画する。

（２）重心軌道生成
軌道生成部は、上記計画した時系列の接触点情報に基づいて、現在を初期状態として所定時間経過後までの安定性が維持できるロボット１００の重心軌道を生成する。同時に、軌道生成部は、各接触点で発生させる反力（接触力）を算出する。

（３）接触力制御
制御部は、軌道生成部により算出された接触力が各接触点で発生するようにロボット１００の各関節部の力制御を行う。

（４）全身協調
軌道生成部は、制御部の力制御の結果に基づいて重心軌道を修正する。制御部は、軌道生成部により修正された重心軌道に基づいて、ロボット１００全身の姿勢及び各関節角度を算出し、ロボット１００の制御を行う。

（５）状態推定
最後に、状態推定部は、ジャイロセンサ、加速度センサ等により検出された実際のセンサ値に基づいて算出した実機の重心軌道と、上記修正したモデルの重心軌道と、のずれを推定する。そして、状態推定部は、推定したずれに基づいて、モデルの重心軌道が実機の重心軌道に一致するように制御パラメータを設定する。

ロボット１００は、上述した（２）から（５）のプロセスを高速周期で実行することで、安定的な多点接触動作を実現する。本実施の形態１に係る重心推定装置は、上述した（５）状態推定に相当する機能を有している。

図２は、環境と複数の接触点で接触するロボットの２次元モデルを概略的に示す図である。なお、図２に示すロボットにおいて、説明の簡略化のために各接触点でモーメント力は作用しないものとする。すなわち、各接触点は夫々点接触、もしくは線接触しているものとする。

図２（１）に示す如く、ロボットの各接触点には環境から水平方向の反力（接触力）ｆ１及び鉛直方向の反力ｆ２が夫々作用している。そして、図２（２）に示す如く、この各接触点の水平方向及び鉛直方向の接触力の合力ｆ３、ｆ４がロボットの重心Ｇに作用する。したがって、このロボットの重心Ｇに作用する合力ｆ３、ｆ４から重心Ｇに作用する重力ｍｇを減算した値が、ロボットの重心Ｇの並進力となり、この並進力をロボットの質量で除算した値がロボットの重心Ｇの並進加速度となる。このように、各接触点に作用する接触力を正確に計測できれば、ロボットの重心Ｇの並進加速度を正確に算出できる。そして、算出したロボットの重心Ｇの並進加速度を逐次積分することで重心位置及び速度を推定することができる。

しかしながら、力センサなどにより計測される各接触点の接触力は一般にノイズや誤差を含んでいる。したがって、このノイズや誤差を含む各接触点の接触力を用いて算出した重心Ｇの並進加速度も誤差を含むこととなる。さらに、この誤差を含む重心Ｇの並進加速度を積分することで誤差が累積し重心位置及び速度は真値から外れた推定値となる虞がある。

ところで、図２（３）に示す如く、ロボットの重心Ｇには上述した並進力と共にモーメント力Ｍも作用している。これは、ロボットの重心Ｇから各接触点までの距離に各接触力を乗算し、各乗算結果を加算した合モーメント力として表される。

そこで、本実施の形態１に係る重心推定装置は、ロボットの重心Ｇに作用する並進力に基づいた重心の推定に、重心周りのモーメントの関係式を組合せると共に、重心周り角速運動量を観測値としてカルマンフィルタを構成している。これにより、ロボットのセンサによるノイズや誤差を除去し、ロボットの重心位置及び速度を高精度かつリアルタイムに推定できる。

図３は、本実施の形態１に係る重心推定装置の概略的なシステム構成を示すブロック図である。本実施の形態１に係る重心推定装置１は、複数の力センサ２と、複数のエンコーダ３と、ジャイロセンサ４と、加速度センサ５と、データ取得部６と、６軸力算出部７と、第２カルマンフィルタ８と、微分演算部９と、角運動量算出部１０と、第１カルマンフィルタ１１と、を備えている。

重心推定装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。

ロボット１００は、例えば、複数の関節部と、各関節部を介して相互に連結された複数のリンクと、を有する多関節型ロボットとして構成されている（図４）。ロボット１００は、二足歩行ロボットなどのヒューマノイドロボットとして構成されているが、これに限らず、複数の関節部及びリンクを有し、環境と複数点で接触する任意の多関節型ロボットに適用可能である。

各力センサ２は、力検出手段の一具体例であり、例えば、ロボット１００の手先、足先、臀部、膝部、肘部などの路面、壁面などの環境と接触する部分に夫々設けられている。各力センサ２は、ロボット１００と環境とが接触する接触点における、ｘ軸方向、ｙ軸方向、及びｚ軸方向の力Ｆ_ｉと、ｘ軸周り、ｙ軸周り、及びｚ軸周りのモーメントτ_ｉ（接触６軸力）を夫々検出する。各力センサ２は、検出した接触６軸力（Ｆ_ｉ、τ_ｉ）を６軸力算出部７に出力する。

なお、ロボット１００には、例えば、Ｍ個の力センサ２が設けれており、ｉ番目の力センサ２の接触６軸力を（Ｆ_ｉ、τ_ｉ）とする。
各エンコーダ３は、角度検出手段の一具体例であり、例えば、ロボット１００の各関節部に設けられている。各エンコーダ３は、各関節部の関節角度θを検出する。各エンコーダは、検出した各関節部の関節角度θを角運動量算出部１０及び微分演算部９に出力する。なお、ロボット１００には例えば、Ｎ個の関節部が設けられているものとする。

ジャイロセンサ４は、角速度検出手段の一具体例であり、例えば、ロボット１００の胴体部に設けれている。ジャイロセンサ４は、ロボット１００の胴体部の角速度ωを検出する。ジャイロセンサ４は、検出した胴体部の角速度ωを第２カルマンフィルタ８に出力する。

加速度センサ５は、加速度検出手段の一具体例であり、例えば、ロボット１００の胴体部に設けられている。加速度センサ５は、胴体部の並進方向の加速度（並進加速度）αを検出する。加速度センサ５は、検出した胴体部の並進加速度αを第２カルマンフィルタ８に出力する。

データ取得部６は、取得手段の一具体例であり、各接触点の姿勢を示す回転行列（以下、姿勢行列Ｒ^ｃ _ｉ）及び各接触点の位置を示すグローバル座標系の座標（以下、位置座標ｐ_ｉ）を上記メモリなどから取得する。例えば、ロボット１００は環境上の予め決められた位置に手や足を接触させて運動するものとし、メモリには、そのときの各接触点の姿勢及び位置が記憶されている。

データ取得部６は、取得した各接触点の姿勢行列Ｒ^ｃ _ｉを６軸力算出部７に出力し、取得した各接触点の位置座標ｐ_ｉを第１カルマンフィルタ１１に出力する。なお、データ取得部６は、例えば、ロボット１００に設けられたセンサを用いて、各接触点の姿勢行列Ｒ^ｃ _ｉ及び／又は位置座標ｐ_ｉを幾何学的に算出してもよい。また、データ取得部６は、取得した各接触点の姿勢行列Ｒ^ｃ _ｉ及び／又は位置座標ｐ_ｉを、ロボット１００の外界情報を検出する外界センサ（レーザレンジファインダ、カメラなど）を用いて幾何学的に修正してもよい。これにより、各接触点の姿勢行列Ｒ^ｃ _ｉ及び位置座標ｐ_ｉをより高精度に求めることができる。

６軸力算出部７は、変換手段の一具体例であり、データ取得部６から出力された各接触点ｉの姿勢行列Ｒ^ｃ _ｉに基づいて、各力センサ２から出力された力センサ座標系の接触６軸力（Ｆ_ｉ、τ_ｉ）をグローバル座標系の接触６軸力（ｆ_ｉ、ｎ_ｉ）に下記式を用いて変換する。

なお、上記式において、ｉ番目の接触点の姿勢行列をＲ^ｃ _ｉとし位置座標をｐ_ｉとしている。
６軸力算出部７は、変換した接触６軸力（ｆ_ｉ、ｎ_ｉ）を第１カルマンフィルタ１１に出力する。

第２カルマンフィルタ８は、第２推定手段の一具体例であり、ジャイロセンサ４から出力されたロボット１００の胴体部の角速度ωと、加速度センサ５から出力された胴体部の並進加速度αと、に基づいて、周知のカルマンフィルタを用いて、ロボット１００の胴体部の姿勢角（行列）Ｒ_ｂｏｄｙおよび角速度（ベクトル）ω_ｄを高精度に算出する。
ここで、ロボット１００の胴体部の姿勢角のオフセット（バイアス）値を示す行列を四元数（クォータニオン）を用いて下記式を用いて定義する。

ロボット１００の胴体部の角速度のオフセット値を示すベクトルを下記式を用いて定義する。但し、βドット（βの微分値）＝０とする。

状態変数ηを下記式で定義する。

次に、四元数の時間微分の関係式は、下記式のようになる。

上記式を用いて下記式が導出される。

上記式を離散化することで下記状態方程式（１）が導出される。

さらに、ロボット１００の胴体部の並進加速度を観測値に選ぶことで下記観測方程式（２）が導出される。但し、下記観測方程式（２）において、加速度センサ５が取り付けられたロボット１００の胴体部の並進加速度が十分に小さいものと仮定する。

上記観測方程式（２）において、回転変換の回転行列Ｒ（ｑ）を下記式で表すことができる。

上記導出した状態方程式（１）及び観測方程式（２）にシステム雑音ｂ_ηｖ_η[ｋ]及び観測雑音ｗ_Γ[ｋ]を導入することで、下記（３）式を導出することができる。

第２カルマンフィルタ８は、加速度センサ５からのロボット１００の胴体部の並進加速度αに基づいて、例えば、上記（３）式に拡張カルマンフィルタを適用することで、高精度に状態変数η（ｑ、β）を推定する。そして、第２カルマンフィルタ８は、推定した状態変数ηと、に基づいて下記式を用いて、ロボット１００の胴体部の姿勢角Ｒ_ｂｏｄｙおよび角速度ω_ｄを算出する。

上記第２カルマンフィルタを用いることで、ジャイロセンサ４の低周波ノイズ及び加速度センサ５の高周波ノイズを除去し、ロボット１００の胴体部の姿勢角および角速度を高精度に算出できる。

第２カルマンフィルタ８は、算出した胴体部の姿勢角Ｒ_ｂｏｄｙおよび角速度ω_ｄを角運動量算出部１０に出力する。
微分演算部９は、関節角速度算出手段の一具体例であり、各エンコーダ３から出力された各関節部の関節角度θに対し微分演算を行い各関節部の関節角速度（θ上辺にドットをθ^（・）とする）を算出する。微分演算部９は、算出した各関節部の関節角速度を角運動量算出部１０に出力する。

角運動量算出部１０は、角運動量算出手段の一具体例であり、第２カルマンフィルタ８から出力される胴体部の姿勢角Ｒ_ｂｏｄｙ及び角速度ω_ｄと、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ^（・）と、第１カルマンフィルタ１１から出力されるロボット１００の重心位置ｒと、に基づいて、ロボット１００の重心周りの角運動量Ｌを算出する。

ここで、ロボット１００の重心周りの角運動量Ｌは、ロボットの各リンクが重心周りに運動することによって発生する角運動量である。例えば、図５に示すように、ｊ番目のリンクの質量をｍ_ｊ、その位置をｃ_ｊとすると、このリンクが並進運動することによって生じる重心周りの角運動量は、下記式を用いて算出される。

さらに、上記式に各リンクの回転運動による角運動量を考慮して求めた式を、全リンクについて足し合わせることで下記ロボット１００の重心周りの角運動量Ｌの下記式を導出できる。但し、下記式において、ｊ番目のリンクの慣性モーメントをＩ_ｊとし、角速度をω_ｊとする。

角運動量算出部１０は、第２カルマンフィルタ８から出力される胴体部の姿勢角Ｒ_ｂｏｄｙ及び角速度ω_ｄと、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ^（・）と、に基づいて、周知の再帰的な演算を行い、各リンクの位置ｃ_ｊ及び角速度ω_ｊとを高速に算出する。

角運動量算出部１０は、算出した各リンクの位置ｃ_ｊ及び角速度ω_ｊと、第１カルマンフィルタ１１からフィードバックされたロボット１００の重心位置ｒと、に基づいて、上記式を用いてロボット１００の重心周りの角運動量Ｌを算出する。角運動量算出部１０は、算出した重心周りの角運動量Ｌを第１カルマンフィルタ１１に出力する。

第１カルマンフィルタ１１は、６軸力算出部７から出力された接触６軸力（ｆ_ｉ、ｎ_ｉ）と、角運動量算出部１０から出力された重心周りの角運動量Ｌと、ロボットの接触点の位置座標ｐ_ｉと、に基づいて、周知のカルマンフィルタを用いてロボット１００の重心位置ｒ及び速度（ｒ上辺にドットをｒ^（・）とする）を推定する。

ここで、ロボット１００の重心に対して並進方向及び回転方向に作用する力の運動方程式は、下記式で表される。但し、下記式において、Ｐは並進方向の運動量であり、ｇは重力加速度ベクトルである。

下記新しい変数ｓを導入する。

上記変数ｓの式に上記並進方向及び回転方向の運動方程式を代入することで下記式が導出される。

状態変数ｘを下記式で定義する。

上記式を離散化することで下記状態方程式（４）が導出される。

ロボット１００の重心周りの角運動量Ｌを観測値に選択することで下記観測方程式（５）が導出される。

上記状態方程式（４）及び観測方程式（５）にシステム雑音ｂ_ｘｖ_ｘ[ｋ]及び観測雑音ｗ_ｈ[ｋ]を導入することで下記式が導出される。

第１カルマンフィルタ１１は、第１推定手段の一具体例であり、６軸力算出部７から出力された接触６軸力（ｆ_ｉ、ｎ_ｉ）と、角運動量算出部１０から出力された重心周りの角運動量Ｌと、ロボット１００の接触点の位置座標ｐ_ｉと、に基づいて、上記導出した（６）式に拡張カルマンフィルタを適用し、状態変数ｘを高精度に推定する。

図６は、本発明の実施の形態１に係る重心推定方法の処理フローを示すフローチャートである。
各力センサ２は、ロボット１００と環境との各接触点における接触６軸力（Ｆ_ｉ、τ_ｉ）を夫々検出し（ステップＳ１０１）、検出した接触６軸力を６軸力算出部７に出力する。

各エンコーダ３は、ロボット１００の各関節部の関節角度θを検出し（ステップＳ１０２）、検出した各関節部の関節角度θを角運動量算出部１０及び微分演算部９に出力する。

ジャイロセンサ４は、ロボット１００の胴体部の角速度ωを検出し（ステップＳ１０３）、検出した胴体部の角速度ωを第２カルマンフィルタ８に出力する。
加速度センサ５は、ロボット１００の胴体部の並進加速度αを検出し（ステップＳ１０４）、検出した胴体部の並進加速度αを第２カルマンフィルタ８に出力する。

６軸力算出部７は、データ取得部６から出力された各接触点ｉの姿勢行列Ｒ^ｃ _ｉに基づいて、各力センサ２から出力された力センサ座標系の接触６軸力（Ｆ_ｉ、τ_ｉ）をグローバル座標系の接触６軸力（ｆ_ｉ、ｎ_ｉ）に変換し（ステップＳ１０５）、変換した接触６軸力を第１カルマンフィルタ１１に出力する。

第２カルマンフィルタ８は、ジャイロセンサ４から出力されたロボット１００の胴体部の角速度ωと、加速度センサ５から出力されたロボット１００の胴体部の並進加速度αと、に基づいて、カルマンフィルタを用いて、ロボット１００の胴体部の姿勢角Ｒ_ｂｏｄｙおよび角速度ω_ｄを算出する（ステップＳ１０６）。第２カルマンフィルタ８は、算出した胴体部の姿勢角Ｒ_ｂｏｄｙおよび角速度ω_ｄを角運動量算出部１０に出力する。

微分演算部９は、各エンコーダ３から出力された各関節部の関節角度θに対し微分演算を行い各関節部の関節角速度θ^（・）を算出し（ステップＳ１０７）、算出した各関節部の関節角速度を角運動量算出部１０に出力する。

角運動量算出部１０は、第２カルマンフィルタ８から出力されるロボット１００の胴体部の姿勢角及び角速度と、各エンコーダ３から出力される各関節部の関節角度と、微分演算部９から出力される各関節部の関節角速度θ^（・）と、フィードバックされるロボット１００の重心位置ｒと、に基づいて、ロボット１００の重心周りの角運動量Ｌを算出する（ステップＳ１０８）。角運動量算出部１０は、算出した重心周りの角運動量Ｌを第１カルマンフィルタ１１に出力する。

第１カルマンフィルタ１１は、６軸力算出器７から出力された接触６軸力（ｆ_ｉ、ｎ_ｉ）と、角運動量算出部１０から出力された重心周りの角運動量Ｌと、データ取得部６から出力されるロボット１００の接触点の位置座標ｐ_ｉと、に基づいて、カルマンフィルタを用いてロボットの重心位置ｒ及び速度ｒ^（・）を推定する（ステップＳ１０９）。

以上、本実施の形態１に係る重心推定装置１及び重心推定方法において、各力センサ２により検出された各接触点の接触６軸力と、各接触点の姿勢行列及び位置座標と、ロボット１００の重心周りの角運動量と、に基づいて、ロボット１００の重心位置及び速度を状態変数とした状態方程式、および、ロボット１００の重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用してロボット１００の重心位置及び速度を推定する。これにより、ロボット１００の力センサ２などによるノイズや誤差を除去し、ロボット１００の重心位置及び速度を高精度に推定できる。

実施の形態２
本実施の形態２に係る重心推定装置２０は、上記実施の形態１の第２カルマンフィルタ８の代わりに、ハイパスフィルタ２１及びローパスフィルタ２２を備える点を特徴とする。図７は、本実施の形態２に係る重心推定装置の概略的なシステム構成を示すブロック図である。

ジャイロセンサ４は、検出した胴体部の角速度ωをハイパスフィルタ２１に出力する。ハイパスフィルタ２１は、ジャイロセンサ４の角速度ωから低周波ノイズを除去する。ハイパスフィルタ２１は、低周波ノイズを除去した角速度を角運動量算出部１０に出力する。

加速度センサ５は、検出した胴体部の並進加速度αをローパスフィルタ２２に出力する。ローパスフィルタ２２は、加速度センサ５の並進加速度αから高周波ノイズを除去する。ローパスフィルタ２２は、高周波ノイズを除去した並進加速度を角運動量算出部１０に出力する。

角運動量算出部１０は、ハイパスフィルタ２１から出力される胴体部の角速度と、ローパスフィルタ２２から出力された胴体部の並進加速度と、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ^（・）と、に基づいて、周知の再帰的な演算を行い、各リンクの位置ｃ_ｊ及び角速度ω_ｊとを高速に算出する。

角運動量算出部１０は、算出した各リンクの位置ｃ_ｊ及び角速度ω_ｊと、ロボット１００の重心位置ｒと、に基づいて、上記式を用いてロボット１００の重心周りの角運動量Ｌを算出する。これにより、ジャイロセンサ及び加速度センサのノイズを除去し、高精度にロボット１００の重心周りの角運動量Ｌを算出できる。

なお、ジャイロセンサ４及び加速度センサ５の計測精度が高い場合、ジャイロセンサ４及び加速度センサ５は、夫々、ハイパスフィルタ及びローパスフィルタを介することなく、角運動量算出部１０に直接的に角速度ω及び並進加速度αを出力してもよい。

角運動量算出部１０は、ジャイロセンサ４から出力される胴体部の角速度ωと、加速度センサ５から出力された胴体部の並進加速度αと、各エンコーダ３から出力される各関節部の関節角度θと、微分演算部９から出力される各関節部の関節角速度θ^（・）と、に基づいて、各リンクの位置ｃ_ｊ及び角速度ω_ｊとを算出し、該算出した各リンクの位置ｃ_ｊ及び角速度ω_ｊと、ロボット１００の重心位置ｒと、に基づいて、上記式を用いてロボット１００の重心周りの角運動量Ｌを算出する。これにより、構成を簡略化しつつ、高精度にロボット１００の重心周りの角運動量Ｌを算出できる。
本実施の形態２において、上記実施の形態１と同一部分に同一符号を付して詳細な説明は省略する。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭを含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１重心推定装置、２力センサ、３エンコーダ、４ジャイロセンサ、５加速度センサ、６データ取得部、７６軸力算出部、８第２カルマンフィルタ、９微分演算部、１０角運動量算出部、１１第１カルマンフィルタ、２１ハイパスフィルタ、２２ローパスフィルタ、１００ロボット

Claims

環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出する力検出手段と、
前記ロボットの胴体部に作用する角速度を検出する角速度検出手段と、
前記ロボットの胴体部に作用する加速度を検出する加速度検出手段と、
前記ロボットの各関節の角度を検出する角度検出手段と、
前記ロボットの各接触点の姿勢及び位置を取得する取得手段と、
前記角速度検出手段により検出された角速度と、前記加速度検出手段により検出された加速度と、前記角度検出手段により検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出する角運動量算出手段と、
前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定する第１推定手段と、を備える、ことを特徴とする重心推定装置。
請求項１記載の重心推定装置であって、
前記第１推定手段は、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいて、前記ロボットの重心位置及び速度を状態変数とした状態方程式、および、前記ロボットの重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用して前記ロボットの重心位置及び速度を推定し、
前記状態方程式及び観測方程式は、前記ロボットの重心に対して並進方向及び回転方向に作用する力の運動方程式に基づいて導出される、ことを特徴とする重心推定装置。
請求項１又は２記載の重心推定装置であって、
前記ロボットは、複数の前記関節と、該各関節を介して相互に連結された複数のリンクと、を有する多関節型ロボットであり、
前記角度検出手段により検出された各関節の角度に基づいて各関節の角速度を算出する関節角速度算出手段を更に備え、
前記角運動量算出手段は、前記角度検出手段により検出された各関節の角度と、前記関節角速度算出手段により算出された各関節の角速度と、前記胴体部の姿勢角及び角速度と、前記各リンクの質量、位置及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの各リンクの並進運動による角運動量と回転運動による角運動量とを夫々加算して前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。
請求項１乃至３のうちいずれか１項記載の重心推定装置であって、
前記ロボットの胴体部の姿勢角のオフセット値を四元数を用いて定義し、該四元数及び前記角速度検出手段の角速度のオフセット値を状態変数としシステム雑音を含む状態方程式、および、前記胴体部の加速度を観測値とし観測雑音を含む観測方程式、に対してカルマンフィルタを適用して、前記状態変数を推定し、該推定した状態変数に基づいて、前記胴体部の姿勢角及び角速度を推定する第２推定手段を更に備え、
前記角運動量算出手段は、前記第２推定手段により推定された胴体部の姿勢角及び角速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。
請求項１乃至３のうちいずれか１項記載の重心推定装置であって、
前記角速度検出手段により検出されたロボットの胴体部の角速度から低周波ノイズを除去するハイパスフィルタと、
前記加速度検出手段により検出されたロボットの胴体部の加速度から高周波ノイズを除去するローパスフィルタと、を更に備え、
前記角運動量算出手段は、前記ハイパスフィルタからの胴体部の角速度と、前記ローパスフィルタからの胴体部の加速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。
請求項１乃至５のうちいずれか１項記載の重心推定装置であって、
前記取得手段により取得された各接触点の姿勢角に基づいて、前記力検出手段から出力された該力検出手段の座標系の力をグローバル座標系の力に変換する変換手段を更に備える、ことを特徴とする重心推定装置。
請求項１乃至６のうちいずれか１項記載の重心推定装置であって、
前記力検出手段は、前記各接触点における、ｘ軸方向、ｙ軸方向、及びｚ軸方向の力と、ｘ軸周り、ｙ軸周り、及びｚ軸周りのモーメント力を検出する、ことを特徴とする重心推定装置。
環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出するステップと、
前記ロボットの胴体部に作用する角速度を検出するステップと、
前記ロボットの胴体部に作用する加速度を検出するステップと、
前記ロボットの各関節の角度を検出するステップと、
前記ロボットの各接触点の姿勢及び位置を取得するステップと、
前記検出された角速度と、前記検出された加速度と、前記検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出するステップと、
前記検出された各接触点の力と、前記取得された各接触点の姿勢及び位置と、前記算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定するステップと、を含む、ことを特徴とする重心推定方法。