JP6149791B2 - 重心推定装置、及び重心推定方法 - Google Patents
重心推定装置、及び重心推定方法 Download PDFInfo
- Publication number
- JP6149791B2 JP6149791B2 JP2014089943A JP2014089943A JP6149791B2 JP 6149791 B2 JP6149791 B2 JP 6149791B2 JP 2014089943 A JP2014089943 A JP 2014089943A JP 2014089943 A JP2014089943 A JP 2014089943A JP 6149791 B2 JP6149791 B2 JP 6149791B2
- Authority
- JP
- Japan
- Prior art keywords
- robot
- center
- gravity
- angular velocity
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Manipulator (AREA)
Description
本発明は、このような問題点を解決するためになされたものであり、ロボットの重心位置及び/又は速度を高精度に推定できる重心推定装置、及び重心推定方法を提供することを主たる目的とする。
この一態様において、前記第1推定手段は、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいて、前記ロボットの重心位置及び速度を状態変数とした状態方程式、および、前記ロボットの重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用して前記ロボットの重心位置及び速度を推定し、前記状態方程式及び観測方程式は、前記ロボットの重心に対して並進方向及び回転方向に作用する力の運動方程式に基づいて導出されてもよい。
この一態様において、前記ロボットは、複数の前記関節と、該各関節を介して相互に連結された複数のリンクと、を有する多関節型ロボットであり、前記角度検出手段により検出された各関節の角度に基づいて各関節の角速度を算出する関節角速度算出手段を更に備え、前記角運動量算出手段は、前記角度検出手段により検出された各関節の角度と、前記関節角速度算出手段により算出された各関節の角速度と、前記胴体部の姿勢角及び角速度と、前記各リンクの質量、位置及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの各リンクの並進運動による角運動量と回転運動による角運動量とを夫々加算して前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記ロボットの胴体部の姿勢角のオフセット値を四元数を用いて定義し、該四元数及び前記角速度検出手段の角速度のオフセット値を状態変数としシステム雑音を含む状態方程式、および、前記胴体部の加速度を観測値とし観測雑音を含む観測方程式、に対してカルマンフィルタを適用して、前記状態変数を推定し、該推定した状態変数に基づいて、前記胴体部の姿勢角及び角速度を推定する第2推定手段を更に備え、前記角運動量算出手段は、前記第2推定手段により推定された胴体部の姿勢角及び角速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記角速度検出手段により検出されたロボットの胴体部の角速度から低周波ノイズを除去するハイパスフィルタと、前記加速度検出手段により検出されたロボットの胴体部の加速度から高周波ノイズを除去するローパスフィルタと、を更に備え、前記角運動量算出手段は、前記ハイパスフィルタからの胴体部の角速度と、前記ローパスフィルタからの胴体部の加速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出してもよい。
この一態様において、前記取得手段により取得された各接触点の姿勢角に基づいて、前記力検出手段から出力された該力検出手段の座標系の力をグローバル座標系の力に変換する変換手段を更に備えていてもよい。
この一態様において、前記力検出手段は、前記各接触点における、x軸方向、y軸方向、及びz軸方向の力と、x軸周り、y軸周り、及びz軸周りのモーメント力を検出してもよい。
上記目的を達成するための本発明の一態様は、環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出するステップと、前記ロボットの胴体部に作用する角速度を検出するステップと、前記ロボットの胴体部に作用する加速度を検出するステップと、前記ロボットの各関節の角度を検出するステップと、前記ロボットの各接触点の姿勢及び位置を取得するステップと、前記検出された角速度と、前記検出された加速度と、前記検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出するステップと、前記検出された各接触点の力と、前記取得された各接触点の姿勢及び位置と、前記算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定するステップと、を含む、ことを特徴とする重心推定方法であってもよい。
以下、図面を参照して本発明の実施の形態について説明する。
本発明の実施の形態1に係る重心推定装置は、ロボットの重心位置及び速度のうち少なくとも一方を高精度に推定できる。重心推定装置は、例えば、ロボットに搭載されている。ロボットは、重心推定装置により推定されたロボットの重心位置及び速度に基づいて、その手足先等と環境との各接触点に作用する接触力を正確に制御できる。
軌道生成部は、設定された一連のタスクを実行する際に環境とロボットの手、足等との接触点の位置及び姿勢(時系列の接触点情報)を算出する。例えば、図1に示すように、ロボット100が、壁に手を接触させつつ机の前まで移動し、机上に手を接触させつつ奥にあるペットボトルを把持するタスクを実行する。このとき、軌道生成部は、どの位置にロボット100のどのような姿勢で手、足等を接触させて胴体部を支持するか等を、動作計画ソフトウェア(プランナー)、あるいは予めユーザに設定された設定情報を用いて計画する。
軌道生成部は、上記計画した時系列の接触点情報に基づいて、現在を初期状態として所定時間経過後までの安定性が維持できるロボット100の重心軌道を生成する。同時に、軌道生成部は、各接触点で発生させる反力(接触力)を算出する。
制御部は、軌道生成部により算出された接触力が各接触点で発生するようにロボット100の各関節部の力制御を行う。
軌道生成部は、制御部の力制御の結果に基づいて重心軌道を修正する。制御部は、軌道生成部により修正された重心軌道に基づいて、ロボット100全身の姿勢及び各関節角度を算出し、ロボット100の制御を行う。
最後に、状態推定部は、ジャイロセンサ、加速度センサ等により検出された実際のセンサ値に基づいて算出した実機の重心軌道と、上記修正したモデルの重心軌道と、のずれを推定する。そして、状態推定部は、推定したずれに基づいて、モデルの重心軌道が実機の重心軌道に一致するように制御パラメータを設定する。
各エンコーダ3は、角度検出手段の一具体例であり、例えば、ロボット100の各関節部に設けられている。各エンコーダ3は、各関節部の関節角度θを検出する。各エンコーダは、検出した各関節部の関節角度θを角運動量算出部10及び微分演算部9に出力する。なお、ロボット100には例えば、N個の関節部が設けられているものとする。
6軸力算出部7は、変換した接触6軸力(fi、ni)を第1カルマンフィルタ11に出力する。
ここで、ロボット100の胴体部の姿勢角のオフセット(バイアス)値を示す行列を四元数(クォータニオン)を用いて下記式を用いて定義する。
微分演算部9は、関節角速度算出手段の一具体例であり、各エンコーダ3から出力された各関節部の関節角度θに対し微分演算を行い各関節部の関節角速度(θ上辺にドットをθ(・)とする)を算出する。微分演算部9は、算出した各関節部の関節角速度を角運動量算出部10に出力する。
各力センサ2は、ロボット100と環境との各接触点における接触6軸力(Fi、τi)を夫々検出し(ステップS101)、検出した接触6軸力を6軸力算出部7に出力する。
加速度センサ5は、ロボット100の胴体部の並進加速度αを検出し(ステップS104)、検出した胴体部の並進加速度αを第2カルマンフィルタ8に出力する。
本実施の形態2に係る重心推定装置20は、上記実施の形態1の第2カルマンフィルタ8の代わりに、ハイパスフィルタ21及びローパスフィルタ22を備える点を特徴とする。図7は、本実施の形態2に係る重心推定装置の概略的なシステム構成を示すブロック図である。
本実施の形態2において、上記実施の形態1と同一部分に同一符号を付して詳細な説明は省略する。
本発明は、例えば、図6に示す処理を、CPUにコンピュータプログラムを実行させることにより実現することも可能である。
Claims (8)
- 環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出する力検出手段と、
前記ロボットの胴体部に作用する角速度を検出する角速度検出手段と、
前記ロボットの胴体部に作用する加速度を検出する加速度検出手段と、
前記ロボットの各関節の角度を検出する角度検出手段と、
前記ロボットの各接触点の姿勢及び位置を取得する取得手段と、
前記角速度検出手段により検出された角速度と、前記加速度検出手段により検出された加速度と、前記角度検出手段により検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出する角運動量算出手段と、
前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定する第1推定手段と、を備える、ことを特徴とする重心推定装置。 - 請求項1記載の重心推定装置であって、
前記第1推定手段は、前記力検出手段により検出された各接触点の力と、前記取得手段により取得された各接触点の姿勢及び位置と、前記角運動量算出手段により算出された重心周りの角運動量と、に基づいて、前記ロボットの重心位置及び速度を状態変数とした状態方程式、および、前記ロボットの重心周りの角運動量を観測値とした観測方程式、に対してカルマンフィルタを適用して前記ロボットの重心位置及び速度を推定し、
前記状態方程式及び観測方程式は、前記ロボットの重心に対して並進方向及び回転方向に作用する力の運動方程式に基づいて導出される、ことを特徴とする重心推定装置。 - 請求項1又は2記載の重心推定装置であって、
前記ロボットは、複数の前記関節と、該各関節を介して相互に連結された複数のリンクと、を有する多関節型ロボットであり、
前記角度検出手段により検出された各関節の角度に基づいて各関節の角速度を算出する関節角速度算出手段を更に備え、
前記角運動量算出手段は、前記角度検出手段により検出された各関節の角度と、前記関節角速度算出手段により算出された各関節の角速度と、前記胴体部の姿勢角及び角速度と、前記各リンクの質量、位置及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの各リンクの並進運動による角運動量と回転運動による角運動量とを夫々加算して前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。 - 請求項1乃至3のうちいずれか1項記載の重心推定装置であって、
前記ロボットの胴体部の姿勢角のオフセット値を四元数を用いて定義し、該四元数及び前記角速度検出手段の角速度のオフセット値を状態変数としシステム雑音を含む状態方程式、および、前記胴体部の加速度を観測値とし観測雑音を含む観測方程式、に対してカルマンフィルタを適用して、前記状態変数を推定し、該推定した状態変数に基づいて、前記胴体部の姿勢角及び角速度を推定する第2推定手段を更に備え、
前記角運動量算出手段は、前記第2推定手段により推定された胴体部の姿勢角及び角速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。 - 請求項1乃至3のうちいずれか1項記載の重心推定装置であって、
前記角速度検出手段により検出されたロボットの胴体部の角速度から低周波ノイズを除去するハイパスフィルタと、
前記加速度検出手段により検出されたロボットの胴体部の加速度から高周波ノイズを除去するローパスフィルタと、を更に備え、
前記角運動量算出手段は、前記ハイパスフィルタからの胴体部の角速度と、前記ローパスフィルタからの胴体部の加速度と、前記角度検出手段により検出された各関節の角度と、前記各関節の角速度と、前記各リンクの質量及び慣性モーメントと、前記ロボットの重心位置と、に基づいて、前記ロボットの重心周りの角運動量を算出する、ことを特徴とする重心推定装置。 - 請求項1乃至5のうちいずれか1項記載の重心推定装置であって、
前記取得手段により取得された各接触点の姿勢角に基づいて、前記力検出手段から出力された該力検出手段の座標系の力をグローバル座標系の力に変換する変換手段を更に備える、ことを特徴とする重心推定装置。 - 請求項1乃至6のうちいずれか1項記載の重心推定装置であって、
前記力検出手段は、前記各接触点における、x軸方向、y軸方向、及びz軸方向の力と、x軸周り、y軸周り、及びz軸周りのモーメント力を検出する、ことを特徴とする重心推定装置。 - 環境と接触する多関節型のロボットの複数の接触点に作用する力を夫々検出するステップと、
前記ロボットの胴体部に作用する角速度を検出するステップと、
前記ロボットの胴体部に作用する加速度を検出するステップと、
前記ロボットの各関節の角度を検出するステップと、
前記ロボットの各接触点の姿勢及び位置を取得するステップと、
前記検出された角速度と、前記検出された加速度と、前記検出された各関節の角度と、に基づいて前記ロボットの重心周りの角運動量を算出するステップと、
前記検出された各接触点の力と、前記取得された各接触点の姿勢及び位置と、前記算出された重心周りの角運動量と、に基づいてカルマンフィルタを用いて前記ロボットの重心位置及び速度のうち少なくとも一方を推定するステップと、を含む、ことを特徴とする重心推定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2014089943A JP6149791B2 (ja) | 2014-04-24 | 2014-04-24 | 重心推定装置、及び重心推定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2014089943A JP6149791B2 (ja) | 2014-04-24 | 2014-04-24 | 重心推定装置、及び重心推定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2015208790A JP2015208790A (ja) | 2015-11-24 |
JP6149791B2 true JP6149791B2 (ja) | 2017-06-21 |
Family
ID=54611450
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2014089943A Active JP6149791B2 (ja) | 2014-04-24 | 2014-04-24 | 重心推定装置、及び重心推定方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6149791B2 (ja) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113433338B (zh) * | 2021-07-02 | 2024-07-12 | 乐聚(深圳)机器人技术有限公司 | 机器人质心速度计算方法、装置及介质 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH02298489A (ja) * | 1989-05-10 | 1990-12-10 | Hitachi Ltd | 移動ロボット操作装置 |
JP3672426B2 (ja) * | 1996-12-19 | 2005-07-20 | 本田技研工業株式会社 | 脚式移動ロボットの姿勢制御装置 |
JP2007007797A (ja) * | 2005-07-01 | 2007-01-18 | Toyota Motor Corp | 歩行ロボット |
JP4753068B2 (ja) * | 2005-09-15 | 2011-08-17 | 独立行政法人産業技術総合研究所 | 移動体の姿勢検出装置、及び、移動ロボット |
JP2009285816A (ja) * | 2008-05-30 | 2009-12-10 | Toyota Motor Corp | 脚式ロボット及びその制御方法 |
JP5540850B2 (ja) * | 2010-04-09 | 2014-07-02 | トヨタ自動車株式会社 | 姿勢推定装置、方法及びプログラム |
JP5569681B2 (ja) * | 2010-04-23 | 2014-08-13 | 国立大学法人 東京大学 | 慣性センサ,磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法 |
-
2014
- 2014-04-24 JP JP2014089943A patent/JP6149791B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2015208790A (ja) | 2015-11-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108621161B (zh) | 基于多传感器信息融合的足式机器人本体状态估计方法 | |
JP5509226B2 (ja) | 力制御関節を有する有脚ロボットのための運動学的および動力学的キャリブレーション方法およびシステム、並びに該ロボット | |
EP3751434B1 (en) | Information processing device, information processing method, and storage medium | |
CN110662635B (zh) | 机器人的碰撞处理 | |
JP6380828B2 (ja) | ロボット、ロボットシステム、制御装置、及び制御方法 | |
JP4876204B2 (ja) | 小型姿勢センサ | |
JP2015199155A (ja) | 情報処理装置および情報処理方法、プログラム | |
JP2007030054A (ja) | ロボット装置 | |
JP6795540B2 (ja) | ロボットを用いた負荷の重量及び重心位置を推定するための装置、方法及びプログラム | |
Kim et al. | On-line initialization and extrinsic calibration of an inertial navigation system with a relative preintegration method on manifold | |
JP2012137421A (ja) | 3軸力センサを用いて力制御をおこなうロボットの制御装置 | |
Danilov et al. | Zmp trajectory from human body locomotion dynamics evaluated by kinect-based motion capture system | |
CN109983299A (zh) | 工业机器人的测量***和方法 | |
KR20190022198A (ko) | 착용형 센서를 이용한 자세정보 보정 방법 및 이를 수행하는 기록매체 | |
JP2013018075A (ja) | ロボット、ロボット制御装置、ロボット制御方法、およびプログラム | |
JP2013013987A (ja) | ロボットシステム | |
JP6149791B2 (ja) | 重心推定装置、及び重心推定方法 | |
Hawley et al. | External force observer for medium-sized humanoid robots | |
JP2017120575A (ja) | 姿勢動作検出装置 | |
JP2014087922A (ja) | ロボット制御装置及び方法 | |
CN108874146B (zh) | 一种应用于虚拟现实***中的运动人体质心位移计算方法 | |
JP2011255500A (ja) | 脚式移動ロボットの制御装置 | |
JP2014200882A (ja) | 把持対象物の把持パターン検出方法およびロボット | |
JP2017144490A (ja) | 制御装置、制御システム、制御方法およびプログラム | |
JP2020160594A (ja) | 自己位置推定方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20160513 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20170222 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20170307 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20170403 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20170425 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20170508 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 6149791 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |