JP6961669B2

JP6961669B2 - ロボット重心位置調整方法、装置、ロボット及びデータ記憶媒体

Info

Publication number: JP6961669B2
Application number: JP2019230711A
Authority: JP
Inventors: 友軍熊; 岳嵩王; 明国趙
Original assignee: Ubtech Robotics Corp
Current assignee: Ubtech Robotics Corp
Priority date: 2018-12-29
Filing date: 2019-12-20
Publication date: 2021-11-05
Anticipated expiration: 2039-12-20
Also published as: KR20200083259A; CN109703645B; US11353887B2; US20200209890A1; KR102268286B1; CN109703645A; JP2020108915A

Description

本発明はロボット制御分野に関し、具体的には、ロボット重心位置調整方法、装置、ロボット及びデータ記憶媒体に関する。

通常の二足ロボット制御分野において、ロボットの走行時の実際の重心位置を計算するとき、通常３つの質量ブロックモデルを用いてロボット構造を簡素化し、即ち、ロボットを腰、左脚及び右脚のみを有する３つの剛体のモデルに簡素化する。該過程では簡素化モデルと類似モデルを使用するため、３つの質量ブロックモデルにより計算して得られた重心実際位置は、予め設定パラメーターにより計画されたロボット重心位置と比べると大きい差が存在してしまう。例えば、身長１．５メートル、体重が５０キログラム前後のロボットに対して、該重心位置の誤差は約３〜５センチメートル前後である。該誤差によって、ロボットが走行する際に、その平衡制御と高速前進に対する能力は影響を受けている。本願優先日において、３つの質量ブロックモデルを用いて重心位置を調整する公知文献は見つかりませんので、二足ロボットの重心位置を調整する先行技術文献を以下に開示します。
＜先行技術文献＞
特許文献１：中国公開１０３２１７９０３Ａ号公報

上述した問題を鑑みて、本発明は大幅に前記重心誤差値を低減させて、ロボットの走行時の平衡制御能力を大幅に増強するロボット重心位置調整方法、装置、ロボット及びデータ記憶媒体を提供することである。

ロボット重心位置調整方法であって、
３つの質量ブロックモデルと重心位置計画値により腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得するステップと、
重心位置初期値と重心位置計画値により、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を取得するステップと、
腰姿勢初期値、腰速度調整値、左脚姿勢初期値と右脚姿勢初期値により、重心位置現在値を取得するステップと、
重心位置現在値が重心位置計画値に等しいか否かを比較し、等しくない場合、重心位置現在値を取得して次の重心位置初期値とし、かつ重心位置現在値が重心位置計画値に等しくなるまで、重心位置初期値と重心位置計画値によるステップに戻って継続して実行し、等しい場合、前記重心位置現在値により目標制御システム司令を生成するステップを含む。

１つ実施例において、３つの質量ブロックモデルと重心位置計画値により対応する腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得するステップの前に、
線形倒立振子モデルとロボットの予め設定パラメーターにより、重心位置計画値を生成するステップを更に含む。

１つ実施例において、３つの質量ブロックモデルと重心位置計画値により対応する腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得するステップは、
３つの質量ブロックモデルと重心位置計画値により、腰姿勢初期値と両脚姿勢初期値を取得するステップと、
腰姿勢初期値及び前記両脚姿勢初期値により、重心位置初期値を計算して取得するステップを含む。

１つ実施例において、腰姿勢初期値、腰速度調整値と両脚姿勢初期値により、重心位置現在値を取得するステップは、
前記腰姿勢初期値、前記腰速度調整値と前記両脚姿勢初期値により、腰姿勢逆運動学アルゴリズムを採用して、各関節の関節角度を計算して取得するステップと、
前記腰姿勢初期値、前記腰速度調整値と前記各関節の関節角度により、重心位置逆運動学アルゴリズムを採用して、重心位置現在値を計算して取得するステップを含む。

さらに、ロボット重心位置調整装置を提供し、
３つの質量ブロックモデルと重心位置計画値により腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得する初期値取得ユニットと、
重心位置初期値と重心位置計画値により、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を取得する腰速度調整値取得ユニットと、
腰姿勢初期値、腰速度調整値と前記両脚姿勢初期値により、重心位置現在値を計算して取得する重心位置現在値取得ユニットと、
重心位置現在値が重心位置計画値に等しいか否かを比較し、等しくない場合、循環実行ユニットに進む判断ユニットと、
重心位置現在値を取得して次の重心位置初期値にし、かつ重心位置現在値が重心位置計画値に等しくなるまで、腰速度調整値取得ユニットにフィードバックして実行され、等しい場合、司令生成ユニットに進む循環実行ユニットと、
前記重心位置現在値により目標制御システム司令を生成する司令生成ユニットを含む。

１つ実施例において、前記装置は、
線形倒立振子モデルとロボットの予め設定パラメーターにより、重心位置計画値を生成する計画ユニットを更に含む。

１つ実施例において、初期値取得ユニットは、
３つの質量ブロックモデルと重心位置計画値により、腰姿勢初期値と両脚姿勢初期値を取得する第１サブユニットと、
腰姿勢初期値、左脚姿勢初期値及び右脚姿勢初期値により、重心位置初期値を計算して取得する第２サブユニットを含む。

１つ実施例において、重心位置現在値取得ユニットは、
前記腰姿勢初期値、前記腰速度調整値と前記両脚姿勢初期値により、腰姿勢逆運動学アルゴリズムを採用して、各関節の関節角度を計算して取得する関節角度取得サブユニットと、
前記腰姿勢初期値、前記腰速度調整値と前記各関節の関節角度により、重心位置逆運動学アルゴリズムを採用して、重心位置現在値を計算して取得する重心位置現在値取得サブユニットを含む。

さらに、ロボットを提供し、記憶装置及びプロセッサーを含み、記憶装置はコンピュータープログラムの記憶に用いられ、プロセッサーがコンピュータープログラムを実行することにより、ロボットが前記ロボット重心位置調整方法を実行する。

さらに、データ記憶媒体を提供し、前記ロボットにより使用されるコンピュータープログラムが記憶されている。

前記ロボット重心位置調整方法、装置、ロボット及びデータ記憶媒体は、先ず、３つの質量ブロックモデルと重心位置計画値を用いて、腰姿勢初期値、両脚姿勢初期値と重心位置初期値を含むロボット関連初期パラメータを取得する。その後、重心位置初期値と重心位置計画値により、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を取得し、更に腰姿勢初期値、腰速度調整値と両脚姿勢初期値を通じて重心位置現在値を計算して取得する。最後に、重心位置現在値が重心位置計画値に等しいか否かを比較し、等しくない場合、重心位置現在値を取得して次の重心位置初期値にし、重心位置現在値が重心位置計画値に等しくなるまで、重心位置初期値と重心位置計画値によるステップに戻って実行する。等しい場合、重心位置現在値により目標制御システム司令を生成する。ロボットの目標制御システム司令周期内での両脚姿勢が変更しない状況で、腰姿勢変化と重心位置変化の対応する線形正の相関を巧妙に利用し、腰姿勢変化量を通じて重心位置に対してフィードバック調整を行い、複数の反復計算を行うことで、ロボット重心計画位置と３つの質量ブロックモデルにより計算されたロボット重心実際位置との間の誤差を大幅に低減し、効果的にロボットの走行時の外乱を減少し、ロボットの走行の安定を高め、平衡制御能力を増加する。

１つ実施例におけるロボット重心位置調整方法のフローチャートである。１つ実施例における腰姿勢変化と重心位置変化の関係を示す図である。１つ実施例におけるロボット重心位置調整効果を示す図である。他の１つ実施例におけるロボット重心位置調整方法のフローチャートである。１つ実施例における腰姿勢初期値、両脚姿勢初期値及び重心位置初期値の取得方法のフローチャートである。１つ実施例におけるロボット構造と座標系を示す図である。１つ実施例における初期小腿部前揺動角度を示す図である。１つ実施例における重心位置現在値を計算するフローチャートである。１つ実施例におけるロボット重心位置調整装置の構造模式図である。１つ実施例におけるロボット重心位置調整装置の構造模式図である。１つ実施例における初期値取得ユニットの構造模式図である。１つ実施例における重心位置現在値取得ユニットの構造模式図である。

以下では、各実施例について詳細に説明する。本公開は複数の実施例を有し、かつ調整や変更をすることができる。本公開の複数の実施例は本明細書に公開される特定実施例を制限するものではなく、本公開が本明細書により開示される複数の実施例の精神と範囲内での全ての調整、同等物及び／または選択可能手段を全て含むと理解しなければならない。

下記では、本明細書に開示される複数の実施例により使用される技術用語「含む」または「含んでもよい」は開示されている機能、操作または部材の存在、かつ１つまたは多くの機能、操作または部材増加を制限しない。それ以外に、本明細書に開示される技術用語「含む」、「有する」及び同じまたは類似用語は、特定特徴、数字、ステップ、操作、部材、ユニットまたは前述用語の組合せを示し、かつ１つまたは複数の他の特徴、数字、ステップ、操作、部材、ユニットまたは前述用語の組合せの存在、または１つまたは複数の特徴、数字、ステップ、操作、部材、ユニットまたは前述用語の組合せを追加する可能性を排除できないことを理解するべきである。

本明細書に開示されている複数の実施例において、Ａ及び／またはＢの少なくとも１つを「含む」ことは同時に記載された文字のいずれかの組合せまたは全ての組合せを示す。例えば、「ＡまたはＢ」または「Ａ及び／またはＢのいずれか１つ」はＡを含んでよく、Ｂを含んでもよく、またはＡとＢの両者を含んでもよい。

本明細書に開示されている複数の実施例で使用される「第１」、「第２」などは複数の実施例での複数の部材の組合せを示すが、この部材の組合せに制限されない。例えば、前述は前記部材の順番及び／または重要性を制限しない。上述は１つの部材と他の部材を区別する目的を果たすためである。例えば、第１ユーザ装置と第２ユーザ装置は異なるユーザ装置を示す。例えば、本明細書に開示される複数の実施例の範囲を離れない状況で、第１部材は第２部材としても称され、同じく、第２部材も第１部材としても称される。

注意すべきこととして、１つの構成部材を他の構成部材に「連結」するとき、第１構成部材を第２構成部材に直接連結してもよく、第１構成部材と第２構成部材の間に第３構成部材を「連結」してもよい。これに相反して、１つの構成部材を他の構成部材に「直接連結」する際には、第１構成部材と第２構成部材の間に第３構成部材が存在しないと理解する。

本明細書に開示される複数の実施例で使用される技術用語「ユーザ」は電子装置を使用する人または電子装置を使用する装置（例えば、人工知能電子装置）を示す。

本明細書に開示される複数の実施例で使用される技術用語は特定実施例を説明する目的であり、本明細書に開示される複数の実施例を制限するものではない。本明細書に使用される単数形式は、特別に説明されている以外は複数形式を含む。特別に説明されている以外、本明細書で使用される全ての用語（技術用語と科学用語を含む）は本明細書に開示される複数の実施例における当業者が理解できる意味と同じ意味を有する。前記用語（例えば、一般的に使用される辞書に限定される用語）は特別に説明されている以外、関連技術分野での用語意味と同じの意味、または正式な意味を有すると理解すべきである。

本発明の目的、技術手段及び特徴を明確に理解するために、以下では図面及び実施例に合わせて、本発明について詳細に説明し、以下の計算過程での座標系は世界座標系を採用する。

１つ実施例において、図１に示すように、ロボット重心位置調整方法を提供し、ステップＳ１１０乃至ステップＳ１６０を含む。
ステップＳ１１０において、３つの質量ブロックモデルと重心位置計画値により腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得する。

重心位置計画値は具体的状況により、ロボットに対して予め設定して得られ、通常の二足ヒューマノイドロボットについて、３つの質量ブロックモデルは通常ロボットを腰、左脚と右脚の３つのモデルに簡素化され、このような簡素化過程では、重心逆運動学アルゴリズムを採用することで、概略的に重心位置計画値により腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を計算して取得することができ、該過程での計算過程の精度が高くないため、後続に調整して計算する必要がある。

前記両脚姿勢初期値はロボットが目標制御システム司令の周期内で対応した初期値であり、即ち、ロボットが１つの制御システム司令周期内で対応した初期値であり、例えば、ロボット制御システムの司令周期は通常１ｍｓを単位とし、１ｍｓ内で、ロボットの両脚姿勢初期値（左脚姿勢初期値と右脚姿勢初期値を含む）はずっと変化しない。

前記重心計画値、腰姿勢初期値、両脚姿勢初期値及び重心位置初期値はそれぞれ世界座標系を用いて表示する。

ステップＳ１２０において、重心位置初期値と重心位置計画値により、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を得る。

腰姿勢変化と重心位置変化の２つ変数の変更方向は同じであり、ロボットの両脚姿勢が固定されたとき、ロボットの腰位置、ロボットの重心は同じ方向に向かって移動し、両者の変化は正の相関を満たし、さらに、腰姿勢と重心位置が類似線形関係（線形関係に類似として処理）を満たし、更に腰姿勢と重心位置が線形重ね合わせの原理を満たすため、腰姿勢変化と重心位置変化は対応する線形正の相関を満たす。

１つ実施例において、図２に示すように、ロボットの両脚が固定されて、右へロボットの腰を移動させると、ロボットの重心も応じて右へ移動し、かつこのような関係は線形（標準的な意味での線形関係ではないが、線形相関係数が高く、数学上では線形として処理してもよい）に近接し、例えば、腰が右へ６センチメートル移動すると、重心の変位はほぼ右へ３センチメートル移動し、重心の位置変化と腰姿勢変化に線形相関が生じ、数学上では線形として処理できる特徴を有する。そのため、前記腰姿勢変化と重心位置変化は、前記正の相関、及び線形として処理する数学特徴を組合せて、重心逆運動学アルゴリズムを通じてニュートン・ラフソンループ反復演算に基づいてロボットの対応する重心実際位置を計算して調整する。

１つ実施例において、下記の式により示すように、Ｖ_{ｗａｉｓｔ}は腰速度調整値を示し、ＣＯＭ_{ｄｅｉｓｒｅｄ}は重心位置計画値を示し、ＣＯＭ_{ａｃｔｕａｌ}は重心位置初期値を示し、（ＣＯＭ_{ｄｅｉｓｒｅｄ}-ＣＯＭ_{ａｃｔｕａｌ}）は重心位置変化量を示し、Ｋｐは線形調整係数を示す。
Ｖ_{ｗａｉｓｔ}＝Ｋｐ（ＣＯＭ_{ｄｅｉｓｒｅｄ}-ＣＯＭ_{ａｃｔｕａｌ}）
前記式において、Ｋｐの上限はロボットの質量の分布により決められている。例えば、重心が腰と脚の真中に位置する場合、Ｋｐの上限は通常２を超えず、ここで、Ｋｐの値は実際の状況によって調整して、重心位置変化と実際の腰姿勢変化との実際の対応関係を基準にして満たす必要があり、Ｖ_{ｗａｉｓｔ}は計算過程での仮想の腰速度調整値を示すものである。

ステップＳ１３０において、腰姿勢初期値、腰速度調整値と両脚姿勢初期値により、重心位置現在値を計算して取得する。

腰姿勢初期値と腰速度調整値により、数学の積分を通じて腰姿勢現在値を計算でき、更に、逆運動学アルゴリズムを通じて、腰姿勢現在値と両脚姿勢初期値により重心位置現在値を計算できる。

ステップＳ１４０において、重心位置現在値と重心位置計画値が等しいか否かを比較し、等しくない場合、ステップＳ１５０に進み、等しい場合、ステップＳ１６０に進む。

計算して取得された重心位置現在値は、計算・調整して得られた重心位置現在値であり、重心位置計画値と更に比較する必要があり、等しくない場合、再び反復計算調整を行う必要があって、ステップＳ１５０に進み、等しい場合、ステップＳ１６０に進む。

ステップＳ１５０において、重心位置現在値を取得して次の重心位置初期値にし、かつ重心位置現在値が重心位置計画値に等しくなるまでステップ１２０に戻って実行する。

現在制御システムの司令周期内で、重心位置計画値、腰姿勢初期値と両脚姿勢初期値のいずれも変更がない場合、ステップＳ１２０に戻って継続して実行する。前の計算過程で得られた重心位置現在値を次の計算過程での重心位置初期値にして反復計算を行い、最後に該過程で対応する重心位置現在値を取得し、その後重心位置計画値と継続して比較し、等しいか否かを判断し、等しい場合、ステップＳ１６０に進み、等しくない場合、ステップＳ１２０に再び戻って、前記計算過程を繰り返しし、得られた重心位置現在値が前記重心位置計画値に等しくなるまで、前記計算過程を繰り返し、その後ステップＳ１６０に進む。

ステップＳ１６０において、重心位置現在値により目標制御システム司令を生成する。

現在の制御システム司令周期内で、前記計算過程は複数行ってもよく、計算して得られた重心位置現在値が現在の制御システム司令周期内の対応する重心位置計画値に等しい場合、ロボットは重心位置現在値により目標制御システム司令を生成し、目標制御システム司令により対応する制御パラメータを得ることで、各関節の角度を調整し、ロボット重心の実際位置の調整を実現することができる。

図３は１つ実施例で前記ロボット重心位置調整方法を応用するロボット重心位置調整効果を示す図であり、ロボットは二足ロボットであり、前記ロボット重心位置調整方法（図３の新しい計算方法に対応）を用いて、計算して得られたロボット重心位置現在値と重心位置計画値の間の重心位置誤差をセンチメートルからミリレベルに低減させ、効果的にロボットの走行時の外乱を減少し、ロボットの走行時の安定性を高め、平衡制御能力を増加し、かつ前記調整方法のリアルタイム性が良好で、１２自由度ヒューマノイドロボットを例にすると、重心誤差を１ミリ以下に低減させることができ、０．３ミリ秒の計算のみ必要で、ロボットの対応する制御システム司令周期よりはるかに少なく、ロボットのリアルタイム制御システムに影響しない。

前記ロボット重心位置調整方法は、先ず、３つの質量ブロックモデルと重心位置計画値を用いて、腰姿勢初期値、両脚姿勢初期値と重心位置初期値を含むロボット関連初期パラメータを取得する。その後、重心位置初期値と重心位置計画値により、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を取得し、更に腰姿勢初期値、腰速度調整値と両脚姿勢初期値を通じて重心位置現在値を計算して取得する。最後に、重心位置現在値と重心位置計画値が等しいか否かを比較し、等しくない場合、重心位置現在値を取得して次の重心位置初期値にし、重心位置現在値が重心位置計画値に等しくなるまで、重心位置初期値と重心位置計画値によるステップに戻って実行する。等しい場合、重心位置現在値により目標制御システム司令を生成する。

前記ロボット重心位置調整方法はロボットの現在の制御システム司令周期内での両脚姿勢が変更しない状況で、腰姿勢変化と重心位置変化に対応する線形正の相関を巧妙に利用し、腰姿勢変化量を通じて重心位置に対してフィードバック調整を行い、複数の反復計算を行うことで、ロボット重心計画位置と３つの質量ブロックモデルにより計算されたロボット重心実際位置との間の誤差を低減し、効果的にロボットの走行時の外乱を減少し、ロボットの走行の安定性を高め、平衡制御能力を増加する。

１つ実施例において、図４に示すように、前記ロボット重心位置調整方法は、線形倒立振子モデルとロボットの予め設定パラメーターにより、重心位置計画値を生成するステップＳ１７０を更に含む。

線形倒立振子モデルを用いて、ロボット計画速度と計画歩行長さを合わせて、重心位置に関する時間軌跡を生成し、更に現在制御システム司令周期内の対応する重心位置計画値を取得することができる。

１つ実施例において、図５に示すように、ステップＳ１１０は、ステップＳ１１２とステップＳ１１４を含む。
ステップＳ１１２において、３つの質量ブロックモデルと重心位置計画値により、腰姿勢初期値と両脚姿勢初期値を取得する。

二足ロボットの制御領域で、ロボット走行時の重心実際位置を計算するとき、通常３つの質量ブロックモデルを用いてロボット構造を簡素化し、即ち、ロボットを腰、左脚及び右脚のみを有する３つの鋼体のモデルに簡素化する。現在制御システム司令周期内で、前記モデルを通じて、重心位置の時間軌跡を粗く腰姿勢と両脚姿勢の時間軌跡に転化し、更に現在制御システム司令周期内の対応する重心位置計画値を腰姿勢初期値と両脚姿勢初期値に転化させる。

ステップＳ１１４において、腰姿勢初期値と両脚姿勢初期値により、重心位置初期値を計算して取得する。

腰姿勢初期値と両脚姿勢初期値はそれぞれ世界座標系を採用し、逆運動学アルゴリズムを通じて重心位置初期値を計算して取得する。

１つ実施例において、図６に示すように、ロボットは二足ロボットであり、ロボットの各脚は６つのモータを有し、各脚の３つのモータの回転軸はロボットの各臀部を中心に交差する。例えば、左脚では、左脚と臀部の交差部（股関節部）には３つのモータが設けられ、膝部には１つのモータが設けられ、他の２つのモータの回転軸は足首部（くるぶし）で交差し、左脚は６つのモータを有する。理解しやすくするために、例えば第１モータが大腿部（太もも）の回転を行い、第２モータが大腿部の横揺動を行い、第３モータが大腿部の前揺動を行い、第４モータが小腿部（すね）の前揺動を行い、第５モータが足首部の前揺動を行い、第６モータが足首部の横揺動を行う。

図６において、全ての座標系においてＺ軸は上へ向かい、Ｘ軸は前へ向かい、Ｙ軸は（正面に向かって）左へ向かっている。更に理解しやすくするため、左脚と右脚は全体で対応する同様の座標表示を有し、（Ｘ０、Ｙ０、Ｚ０）は腰座標系であり、（Ｘ１、Ｙ１、Ｚ１）は臀部回転座標系を示し、（Ｘ２、Ｙ２、Ｚ２）は臀部横揺動座標系を示し、（Ｘ３、Ｙ３、Ｚ３）は臀部前揺動座標系を示し、（Ｘ４、Ｙ４、Ｚ４）小腿部前揺動座標系を示し、（Ｘ５、Ｙ５、Ｚ５）は足首部前揺動座標系を示し、（Ｘ６、Ｙ６、Ｚ６）は足首部外側横揺動座標系を示し、ｓｏｌｅは足底座標系（足首座標系と簡称）を示し、Ｚ１、Ｘ２、Ｙ３、Ｙ４、Ｙ５及びＸ６はそれぞれは第１乃至第６モータの各回転軸と平行になる。ＲｓｏｌｅとＬｓｏｌｅはそれぞれ左足首座標系と右足首座標系を示し、上記の他の座標系と同様のゼロ位置と方向を有する。

前記腰姿勢初期値と両脚姿勢初期値により、逆運動学アルゴリズムを用いて重心位置初期値を計算して取得する過程で、腰姿勢逆運動学アルゴリズムと重心位置逆運動学アルゴリズムの２つの大きいステップを含み、重要なのは、足首から股関節までのベクトルを取得して、足首座標系に表示する。

１つ実施例において、右脚をぶら下げたときを例にする場合、腰姿勢逆運動学を用いて右脚の６つの初期関節角度、θ_１、θ_２、θ_３、θ_４、θ_５とθ_６を計算し、右脚姿勢初期値と腰姿勢初期値により、右脚の腰に対する姿勢を計算でき、ここでは^ｗＴ_ｆとして表示し、右脚の６うの初期関節角度、θ_１、θ_２、θ_３、θ_４、θ_５とθ_６を計算でき、それぞれ初期大腿部回転角度、初期大腿部横揺動角度、初期大腿部前揺動角度、初期小腿部前揺動角度、初期足首部前揺動角度及び初期足首部横揺動角度を示す。

右脚は腰座標系の初期姿勢を^ｗＴ_ｆとして表示し、^６Ｔ_ｆは右脚姿勢初期値の右足首部外側揺動座標系（Ｘ６、Ｙ６、Ｚ６）の同次変換行列を示し、同次変換行列を利用する連鎖律は、以下の通りである。

足首から臀部関節ベクトルを足首座標系に示す。

その後、余弦定理によりθ_４を計算し、図７に示すように、膝は後ろのみへ向かって曲がるため、θ_４は初期小腿部前揺動角度（膝の曲げ角度）であり、その範囲は［-π,０］である。

臀部関節を足首座標系のｙ−ｚ平面に投影して、足首の横揺動θ_６を計算することができる。

θ_４，θ_６の情報を利用して、臀部関節を足首座標系のｘ−ｚ平面に投影して、足首の前揺動θ_５を計算する。

臀部関節部位で３つの回転軸は互いに直交し、それぞれθ_１，θ_２，θ_３に対応し、該３つの生じる姿勢はθ_４，θ_５，θ_６と^ｗＲ_ｆを用いて逆算を行うことができる。

さらに、重心位置アルゴリズムにより重心位置初期値を計算し、前記右脚の６つ初期関節角度、θ_１，θ_２、θ_３、θ_４、θ_５及びθ_６により、腰姿勢初期値に合わせて、順運動学の重心位置アルゴリズムを通じて重心位置初期値を得ることができ、その具体的過程は以下に示す。

重心位置初期値ｒ_ｃｏｍは各剛体の重心位置初期値ｒ_ｉのそれら各質量ｍ_ｉに対する加重平均であり、各剛体の重心位置初期値ｒ_ｉは、順運動学のアルゴリズムにより取得することができる。

各剛体の座標系の原点は世界座標系でその位置をｒ_ｏｉで示し、ｒ_ｏｉは同次変換行列の連鎖律を通じて連乗して得られ、各剛体の同次変換行列は前記右脚の６つの初期関節角度（θ_１、θ_２、θ_３、θ_４、θ_５及びθ_６）と腰姿勢初期値により計算して得られ、ｃｏｍ_ｉは各剛体の重心位置初期値から該剛体自身の座標系原点のベクトルであり、各剛体の世界座標系の姿勢は回転行列Ｒ_ｉで表示し、^ｉｃｏｍ_ｉは各剛体の重心位置初期値の剛体自身の座標系でのベクトル（通常は固定値であり、機械設計時にＳｏｌｉｄｗｏｒｋｓ等のソフトにより得られる）を示す。

１つ実施例において、図８に示すように、ステップＳ１３０はステップＳ１３２とステップＳ１３４を含む。
ステップＳ１３２において、前記腰姿勢初期値、前記腰速度調整値と前記両脚姿勢初期値により、腰姿勢逆運動学アルゴリズムを採用して、各関節の関節角度を計算して取得する。

腰姿勢初期値と腰速度調整値により、腰姿勢現在値を計算して取得でき、更に腰逆運動学アルゴリズムを通じて、腰姿勢現在値と両脚姿勢初期値により各関節の関節角度を計算して取得し、該計算方法はステップＳ１１０での各関節の初期関節角度を計算する計算方法とほぼ同じである。

ステップＳ１３４において、前記腰姿勢初期値、前記腰速度調整値と各関節の関節角度により、重心位置逆運動学アルゴリズムを採用して、重心位置現在値を計算して取得する。

該ステップでは、腰姿勢初期値と腰速度調整値により、腰姿勢現在値を計算して取得でき、更に重心位置逆運動学アルゴリズムを用いて、重心位置現在値を計算して取得し、該計算過程は前記ステップＳ１１０での重心位置初期値を計算する過程とほぼ同じである。

さらに、図９に示すようにロボット重心位置調整装置を提供し、
３つの質量ブロックモデルと重心位置計画値により腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得する初期値取得ユニット２１０と、
重心位置初期値と前記重心位置計画値により、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を取得する腰速度調整値取得ユニット２２０と、
腰姿勢初期値、腰速度調整値、左脚姿勢初期値と右脚姿勢初期値により、重心位置現在値を計算して取得する重心位置現在値取得ユニット２３０と、
重心位置現在値と重心位置計画値が等しいか否かを比較し、等しくない場合、循環実行ユニットに進む判断ユニット２４０と、
重心位置現在値を取得して次の重心位置初期値にし、かつ重心位置現在値が重心位置計画値に等しくなるまで、腰速度調整値取得ユニットにフィードバックして実行され、等しい場合、司令生成ユニットに進む循環実行ユニット２５０と、
前記重心位置現在値により目標制御システム司令を生成する司令生成ユニット２６０を含む。

１つ実施例において、図１０に示すように、前記装置は、線形倒立振子モデルとロボットの予め設定パラメーターにより、重心位置計画値を生成する計画ユニット２７０を更に含む。

１つ実施例において、図１１に示すように、初期値取得ユニット２１０は、３つの質量ブロックモデルと重心位置計画値により、腰姿勢初期値と両脚姿勢初期値を取得する第１サブユニット２１２と、腰姿勢初期値、左脚姿勢初期値及び右脚姿勢初期値により、重心位置初期値を計算して取得する第２サブユニット２１４を含む。

１つ実施例において、図１２に示すように、重心位置現在値取得ユニット２３０は、前記腰姿勢初期値、前記腰速度調整値と前記両脚姿勢初期値により、腰姿勢逆運動学アルゴリズムを採用して、各関節の関節角度を計算して取得する関節角度取得サブユニット２３２と、前記腰姿勢初期値、前記腰速度調整値と各関節の関節角度により、重心位置逆運動学アルゴリズムを採用して、重心位置現在値を計算して取得する重心位置現在値取得サブユニット２３４を含む。

以上は、本発明の具体的な実施形態に過ぎず、本発明の保護範囲はこれに限定されるものではなく、当業者が本発明が開示した技術範囲内において、容易に想到できるすべての変更又は変換は、本発明の保護範囲内に含まれるべきである。

Claims

少なくとも腰、左脚及び右脚を有し、前記左脚及び前記右脚は、それぞれ大腿部と小腿部と足首部を有し、それぞれ大腿部の回転、大腿部の横揺動、大腿部の前揺動、小腿部の前揺動、足首部の前揺動及び足首部の横揺動を行う複数の関節を有する二足ロボットの重心位置調整方法であって、
前記腰、左脚及び右脚を３つの質量ブロックモデルとみなし、前記３つの質量ブロックモデルと目標制御システム指令の周期内に対応する重心位置計画値により、前記目標制御システム指令の周期内の初期値である腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得するステップと、
前記重心位置初期値と対応する重心位置計画値とにより、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を取得するステップと、
前記腰姿勢初期値、腰速度調整値と両脚姿勢初期値により重心位置現在値を計算して取得するステップと、
前記重心位置現在値が対応する重心位置計画値に等しいか否かを比較し、等しくない場合、前記重心位置現在値を取得して次の重心位置初期値とし、かつ前記重心位置現在値が対応する重心位置計画値に等しくなるまで、前記重心位置初期値と対応する重心位置計画値とによるステップに戻って実行し、等しい場合、前記重心位置現在値により当該ロボットの重心位置を調整するための制御パラメータである前記複数の関節の関節角度を含む目標制御システム指令を生成するステップを含むことを特徴とするロボット重心位置調整方法。
前記３つの質量ブロックモデルと重心位置計画値により対応する腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得するステップの前に、
線形倒立振子モデルを用いて、ロボット計画速度と計画歩行長さを合わせて、重心位置に関する時間軌跡を生成し、前記重心位置計画値を生成するステップをさらに含むことを特徴とする請求項１に記載のロボット重心位置調整方法。
前記３つの質量ブロックモデルと重心位置計画値により対応する腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得するステップは、
前記３つの質量ブロックモデルと前記重心位置計画値により、重心位置の時間軌跡を腰姿勢と両脚姿勢の時間軌跡に転化し、腰姿勢初期値と両脚姿勢初期値を取得するステップと、
前記腰姿勢初期値及び前記両脚姿勢初期値により、重心位置初期値を計算して取得するステップを含むことを特徴とする請求項１に記載のロボット重心位置調整方法。
前記腰姿勢初期値、前記腰速度調整値と前記両脚姿勢初期値により、重心位置現在値を計算して取得するステップは、
前記腰姿勢初期値と前記腰速度調整値により、腰姿勢現在値を計算して取得し、前記腰姿勢現在値と前記両脚姿勢初期値により、腰姿勢逆運動学アルゴリズムを採用して、前記複数の関節の関節角度を計算して取得するステップと、
前記腰姿勢初期値、前記腰速度調整値と前記複数の関節の関節角度により、重心位置逆運動学アルゴリズムを採用して、重心位置現在値を計算して取得するステップを含むことを特徴とする請求項１に記載のロボット重心位置調整方法。
少なくとも腰、左脚及び右脚を有し、前記左脚及び前記右脚は、それぞれ大腿部と小腿部と足首部を有し、それぞれ大腿部の回転、大腿部の横揺動、大腿部の前揺動、小腿部の前揺動、足首部の前揺動及び足首部の横揺動を行う複数の関節を有する二足ロボットの重心位置調整装置であって、
前記腰、左脚及び右脚を３つの質量ブロックモデルとみなし、前記３つの質量ブロックモデルと目標制御システム指令の周期内に対応する重心位置計画値により、前記目標制御システム指令の周期内の初期値である腰姿勢初期値、両脚姿勢初期値及び重心位置初期値を取得する初期値取得ユニットと、
前記重心位置初期値と対応する重心位置計画値とにより、腰姿勢変化と重心位置変化に対応する線形正の相関を利用して、腰速度調整値を取得する腰速度調整値取得ユニットと、
前記腰姿勢初期値、前記腰速度調整値と前記両脚姿勢初期値により、重心位置現在値を計算して取得する重心位置現在値取得ユニットと、
前記重心位置現在値が対応する重心位置計画値に等しいか否かを比較し、等しくない場合、循環実行ユニットに進み、等しい場合、指令生成ユニットに進む判断ユニットと、
前記重心位置現在値を取得して次の重心位置初期値にし、かつ前記重心位置現在値が対応する重心位置計画値に等しくなるまで、前記腰速度調整値取得ユニットにフィードバックして実行される循環実行ユニットと、
前記重心位置現在値により当該ロボットの重心位置を調整するための制御パラメータである前記複数の関節の関節角度を含む目標制御システム指令を生成する指令生成ユニットと、を含むことを特徴とするロボット重心位置調整装置。
線形倒立振子モデルを用いて、ロボット計画速度と計画歩行長さを合わせて、重心位置に関する時間軌跡を生成し、前記重心位置計画値を生成する計画ユニットをさらに含むことを特徴とする請求項５に記載のロボット重心位置調整装置。
前記初期値取得ユニットは、
３つの質量ブロックモデルと重心位置計画値により、重心位置の時間軌跡を腰姿勢と両脚姿勢の時間軌跡に転化し、腰姿勢初期値と両脚姿勢初期値を取得する第１サブユニットと、
前記腰姿勢初期値及び前記両脚姿勢初期値により、重心位置初期値を計算して取得する第２サブユニットと、を含むことを特徴とする請求項５に記載のロボット重心位置調整装置。
前記重心位置現在値取得ユニットは、
前記腰姿勢初期値と前記腰速度調整値により、腰姿勢現在値を計算して取得し、前記腰姿勢現在値と前記両脚姿勢初期値により、腰姿勢逆運動学アルゴリズムを採用して、前記複数の関節の関節角度を計算して取得する関節角度取得サブユニットと、
前記腰姿勢初期値、前記腰速度調整値と前記複数の関節の関節角度により、重心位置逆運動学アルゴリズムを採用して、重心位置現在値を計算して取得する重心位置現在値取得サブユニットと、を含むことを特徴とする請求項５に記載のロボット重心位置調整装置。
二足ロボットであって、記憶装置及びプロセッサーを含み、前記記憶装置はコンピュータープログラムの記憶に用いられ、前記プロセッサーが前記コンピュータープログラムを実行することにより、前記ロボットが請求項１乃至請求項４のいずれか一項記載のロボット重心位置調整方法を実行することを特徴とするロボット。
データ記憶媒体であって、請求項９に記載のロボットにより使用されるコンピュータープログラムが記憶されていることを特徴とするデータ記憶媒体。