ホーム > 特許ランキング > 本田技研工業株式会社
知財求人 - 知財ポータルサイト「IP Force」
▶ 本田技研工業株式会社の特許一覧 ▶ 学校法人明治大学の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-09-09
(45)【発行日】2024-09-18
(54)【発明の名称】ロボット制御方法、およびロボット制御システム
(51)【国際特許分類】
B25J 5/00 20060101AFI20240910BHJP
B25J 13/00 20060101ALI20240910BHJP
【ＦＩ】
B25J5/00 E
B25J5/00 F
B25J13/00 Z
【請求項の数】
8
(21)【出願番号】P 2021165016
(22)【出願日】2021-10-06
(65)【公開番号】P2023055542
(43)【公開日】2023-04-18
【審査請求日】2023-10-23
(73)【特許権者】
【識別番号】000005326
【氏名又は名称】本田技研工業株式会社
(73)【特許権者】
【識別番号】801000027
【氏名又は名称】学校法人明治大学
(74)【代理人】
【識別番号】100165179
【弁理士】
【氏名又は名称】田▲崎▼ 聡
(74)【代理人】
【識別番号】100126664
【弁理士】
【氏名又は名称】鈴木 慎吾
(74)【代理人】
【識別番号】100154852
【弁理士】
【氏名又は名称】酒井 太一
(74)【代理人】
【識別番号】100194087
【弁理士】
【氏名又は名称】渡辺 伸一
(72)【発明者】
【氏名】上岡 拓未
(72)【発明者】
【氏名】小澤 隆太
【審査官】樋口 幸太郎
(56)【参考文献】
【文献】特開２０１６－１９８８７３（ＪＰ，Ａ）
【文献】特開２０１８－１８５７４７（ＪＰ，Ａ）
【文献】特開２００６－０７５９４５（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ２５Ｊ ５／００
Ｂ２５Ｊ １３／００
(57)【特許請求の範囲】
【請求項1】
重心補償力算出部が、受動性に基づく多点接触立位制御をロボットに行う際、前記ロボットとの接触点の幾何学的関係より重心補償力を計算し、拡張部が、前記重心補償力（ｖｆｆ）を制御入力（ｖ）におけるフィード・フォワード項として追加して制御入力（ｖ）を拡張し、
前記フィード・フォワード項は、水平力の項（ｖｆｆｈ）と垂直力の項（ｖｆｆｖ）に分けられ、
制御装置が、前記拡張された前記制御入力（ｖ）を用いて、前記ロボットの立位制御を行う、
ロボット制御方法。
【請求項2】
前記制御装置は、パワー保存変換（ｕ＝ＱＰ
Ｔｖ）を用いて、前記制御入力（ｖ）から実現可能な制御入力（ｕ）をフィードバック項と前記フィード・フォワード項を用いて生成し、前記パワー保存変換は各項で独立に行う、請求項１に記載のロボット制御方法。
【請求項3】
前記重心補償力算出部は、絶対座標系からみたｆｌｏａｔｉｎｇ ｂａｓｅの位置を、前記ロボットの重心位置（ｘｃｏｍ）に変換して重心補償力を計算する、請求項１に記載のロボット制御方法。
【請求項4】
切替部が、前記ロボットの接触状態に基づいて、前記フィード・フォワード項を前記水平力の項（ｖｆｆｈ）と前記垂直力の項（ｖｆｆｖ）のうちのいずれか１つに切り替えて使用する、請求項３に記載のロボット制御方法。
【請求項5】
前記水平力の項（ｖｆｆｈ）は、前記ロボットが備えるハンドの力を全身に作用する力にヤコビ行列によって変換して表され、前記垂直力の項（ｖｆｆｖ）は、重力による関数で表される、
請求項３または請求項４に記載のロボット制御方法。
【請求項6】
前記制御装置は、前記フィード・フォワード項の並進力はハンドと足、鉛直力は足、前記フィードバック項はハンドと足で発生させるように制御する、請求項２に記載のロボット制御方法。
【請求項7】
前記ロボットは、ボディに２つ以上の２自由度以上のアームまたは足を持つ、請求項１から請求項６のうちのいずれか１項に記載のロボット制御方法。
【請求項8】
ロボットの状態を検出するセンサと、受動性に基づく多点接触立位制御をロボットに行う際、前記ロボットとの接触点の幾何学的関係より重心補償力を計算する重心補償力算出部と、
前記重心補償力（ｖｆｆ）を制御入力（ｖ）におけるフィード・フォワード項として追加して制御入力（ｖ）を拡張する拡張部と、
前記拡張された前記制御入力（ｖ）を用いて、前記ロボットの立位制御を行う制御装置と、
前記制御装置の制御に応じて制御される２自由度以上のアームまたは足と、
を備え、
前記フィード・フォワード項は、水平力の項（ｖｆｆｈ）と垂直力の項（ｖｆｆｖ）に分けられるロボット制御システム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ロボット制御方法、およびロボット制御システムに関する。
【背景技術】
【0002】
ヒューマノイドの多点接触制御は、手先足先位置を拘束しながら、ロボットの姿勢を制御する接触力を同一平面上ではない複数の接触点に分配する問題である。この場合は、接触点が同一平面上ではないため、分配方法は一意には求まらない。これに対して、接触力を最適化問題として解いた後，各関節に分配する手法が提案されている（例えば非特許文献１参照）。
【先行技術文献】
【非特許文献】
【0003】
【文献】Bernd Henze, Maximo A. Roa, Christian Ott, “Passivity-based whole-body balancing for torque-controlled humanoid robots in multi-contact scenarios”, International Journal of Robotics Research, 35(12), 2016, p1522-1543
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、従来技術の最適化よる手法では計算負荷が高いという課題があった。
【0005】
本発明は、上記の問題点に鑑みてなされたものであって、ロボット制御において重心安定化を実現できるロボット制御方法、およびロボット制御システムを少ない計算量で提供することを目的とする。
【課題を解決するための手段】
【0006】
（１）上記目的を達成するため、本発明の一態様に係るロボット制御方法は、重心補償力算出部が、受動性に基づく多点接触立位制御をロボットに行う際、前記ロボットとの接触点の幾何学的関係より重心補償力を計算し、拡張部が、前記重心補償力（ｖｆｆ）を制御入力（ｖ）におけるフィード・フォワード項として追加して制御入力（ｖ）を拡張し、前記フィード・フォワード項は、水平力の項（ｖｆｆｈ）と垂直力の項（ｖｆｆｖ）に分けられ、制御装置が、前記拡張された前記制御入力（ｖ）を用いて、前記ロボットの立位制御を行う。
【0007】
（２）また、本発明の一態様に係る多点接触制御方法において、前記制御装置は、パワー保存変換（ｕ＝ＱＰ
Ｔｖ）を用いて、前記制御入力（ｖ）から実現可能な制御入力（ｕ）をフィードバック項と前記フィード・フォワード項を用いて生成し、前記パワー保存変換は各項で独立に行うようにしてもよい。
【0008】
（３）また、本発明の一態様に係る多点接触制御方法において、前記重心補償力算出部は、絶対座標系からみたｆｌｏａｔｉｎｇ ｂａｓｅの位置を、前記ロボットの重心位置（ｘｃｏｍ）に変換して重心補償力を計算するようにしてもよい。
【0009】
（４）また、本発明の一態様に係る多点接触制御方法において、切替部が、前記ロボットの接触状態に基づいて、前記フィード・フォワード項を前記水平力の項（ｖｆｆｈ）と前記垂直力の項（ｖｆｆｖ）のうちのいずれか１つに切り替えて使用するようにしてもよい。
【0010】
（５）また、本発明の一態様に係る多点接触制御方法において、前記水平力の項（ｖｆｆｈ）は、前記ロボットが備えるハンドの力を全身に作用する力にヤコビ行列によって変換して表され、前記垂直力の項（ｖｆｆｖ）は、重力による関数で表されるようにしてもよい。
【0011】
（６）また、本発明の一態様に係る多点接触制御方法において、前記制御装置は、パワー保存変換（ｕ＝ＱＰ
Ｔｖ）を用いて、前記制御入力（ｖ）から実現可能な制御入力（ｕ）をフィードバック項と前記フィード・フォワード項を用いて生成するようにしてもよい。
【0012】
（７）また、本発明の一態様に係る多点接触制御方法において、前記制御装置は、前記フィード・フォワード項の並進力はハンドと足、鉛直力は足、前記フィードバック項はハンドと足で発生させるように制御するようにしてもよい。この際、前記パワー保存変換は各項において独立に計算されてもよい。
【0013】
（８）また、本発明の一態様に係る多点接触制御方法において、前記ロボットは、ボディに２つ以上の２自由度以上のアームまたは足を持つようにしてもよい。
【0014】
（９）上記目的を達成するため、本発明の一態様に係るロボット制御システムは、ロボットの状態を検出するセンサと、受動性に基づく多点接触立位制御をロボットに行う際、前記ロボットとの接触点の幾何学的関係より重心補償力を計算する重心補償力算出部と、前記重心補償力（ｖｆｆ）を制御入力（ｖ）におけるフィード・フォワード項として追加して制御入力（ｖ）を拡張する拡張部と、前記拡張された前記制御入力（ｖ）を用いて、前記ロボットの立位制御を行う制御装置と、前記制御装置の制御に応じて制御される２自由度以上のアームまたは足と、を備え、前記フィード・フォワード項は、水平力の項（ｖｆｆｈ）と垂直力の項（ｖｆｆｖ）に分けられる。
【発明の効果】
【0015】
（１）～（９）によれば、ロボット制御において重心安定化を実現できる。（１）～（９）によれば、複数の接触制約を満たしながら、重心安定化を実現できるようになる。（１）～（９）によれば、受動性における証明により制御系として安定となるので、ロバストである。（１）～（９）によれば、計算は正則なヤコビ行列ｆｌｏａｔｉｎｇ ｂａｓｅに関する正方成分の逆行列を求めるのみという小さい計算コストで可能となる。（１）～（９）によれば、パラメタは比例、微分ゲインのみで、最適化問題で扱う重みのような無次元係数ではなく、物理的意味から直感的にチューニングが可能である。
また、（６）によれば、実現可能な制御入力（ｕ）を用いてロボットを制御することができる。
また、（６）によれば、実現可能な制御入力（ｕ）を用いてロボットを制御することができる。
【図面の簡単な説明】
【0016】
【図1】実施形態に係るロボット制御システムの構成例を示す図である。
【図2】ロボットの構成を示す図である。
【図3】４本の手足で環境に接する３Ｄヒューマノイドモデルを示す図である。
【図4】立っている状態から支えている状態への移行の一例を示す図である。
【図5】立っている状態から支えている状態への移行の一例を示す図である。
【図6】実施形態に係る制御系の構成例を示す図である。
【図7】力コントロールゲインの切り替え例を示す図である。
【図8】足で立っている状態から、手と足で体を支える状態への移行についてのシミュレーション結果における時刻毎の状態を示す図である。
【図9】壁に接触するシミュレーションを行った場合のＣＯＧのｘ方向の位置とｚ方向の力を示す図である。
【図10】実施形態に係るロボット制御システムの処理手順のフローチャートである。
【発明を実施するための形態】
【0017】
以下、本発明の実施の形態について図面を参照しながら説明する。なお、以下の説明に用いる図面では、各部材を認識可能な大きさとするため、各部材の縮尺を適宜変更している。なお、本実施形態では、ロボットの一例としてヒューマノイドロボットを例に説明する。
【0018】
＜概要＞
まず、実施形態の概要を説明する。実施形態では、ヒューマノイドロボットの立位制御を多点接触問題へ拡張する。実施形態では、手足の幾何学的関係より重心補償力を計算し、立位時の重力補償項を３次元に拡張することで、３次元の重心補償力をフィード・フォワード項として追加する。この際、実施形態では、水平力と垂直力を独立した項として定義する。また、実施形態では、手を壁等に着く、離すなどの接触点の切り替わりについて、実際の接触に応じてフィード・フォワード項を変化させることで実現する。
まず、実施形態の概要を説明する。実施形態では、ヒューマノイドロボットの立位制御を多点接触問題へ拡張する。実施形態では、手足の幾何学的関係より重心補償力を計算し、立位時の重力補償項を３次元に拡張することで、３次元の重心補償力をフィード・フォワード項として追加する。この際、実施形態では、水平力と垂直力を独立した項として定義する。また、実施形態では、手を壁等に着く、離すなどの接触点の切り替わりについて、実際の接触に応じてフィード・フォワード項を変化させることで実現する。
【0019】
＜ロボット制御システムの構成例＞
次に、ロボット制御システム１の構成例を説明する。
図１は、本実施形態に係るロボット制御システムの構成例を示す図である。図１のように、ロボット制御システム１は、センサ７、制御装置８、および少なくとも１つの関節アクチュエータ９（９－１，９－２，…，９－ｎ）（ｎは１以上の整数）を備える。
センサ７は、例えば、傾斜を検出する傾斜センサ７１、加速度や傾き等を検出する６軸センサ７２、アクチュエータの回転角度等を検出するエンコーダ７３等を備える。
制御装置８は、例えば、取得部８１、重心補償力算出部８２、拡張部８３、切替部８４、駆動部８５、および制御部８６を備える。
次に、ロボット制御システム１の構成例を説明する。
図１は、本実施形態に係るロボット制御システムの構成例を示す図である。図１のように、ロボット制御システム１は、センサ７、制御装置８、および少なくとも１つの関節アクチュエータ９（９－１，９－２，…，９－ｎ）（ｎは１以上の整数）を備える。
センサ７は、例えば、傾斜を検出する傾斜センサ７１、加速度や傾き等を検出する６軸センサ７２、アクチュエータの回転角度等を検出するエンコーダ７３等を備える。
制御装置８は、例えば、取得部８１、重心補償力算出部８２、拡張部８３、切替部８４、駆動部８５、および制御部８６を備える。
【0020】
ロボット制御システム１は、ロボット６（図２参照）の動作を制御する。ロボット６は、例えば、頭部、ボディ、２つの手と、２つの足を備えるヒューマノイドロボットである。ロボット６の構成例は、後述する。
【0021】
センサ７は、例えば、頭部、ボディ、２つの手と、２つの足、各関節等に取り付けられている。なお、本実施形態では、足首から先の「足」も、胴から下の部分の「脚」も「足」という。
【0022】
関節アクチュエータ９は、例えば電動モータあるいは油圧アクチュエータにより構成されたものであり、例えば関節毎に備えられている。この場合、各関節アクチュエータ９による各関節の駆動機構は周知の構造のものでよい。また、関節アクチュエータ９は、回転型のアクチュエータに限らず、直動型のアクチュエータであってもよい。
【0023】
制御装置８は、センサ７が検出した値、制御値等に基づいて、ロボット６の動作を制御する。制御装置８は、例えば、ＣＰＵ（中央演算装置）、ＲＡＭ（Read only memory）、ＲＯＭ（Read Only Memory）、インターフェース回路等を含む電子回路ユニットにより構成されていてもよい。
【0024】
取得部８１は、センサ７が検出した検出値を取得する。
【0025】
重心補償力算出部８２は、取得部８１が取得した検出値を用いて、手足の幾何学的関係より重心補償力を計算する。重心補償力算出部８２は、絶対座標系からみたロボット６の重心位置ｘｃｏｍを支える接触力を重心補償力として計算する。なお、重心補償力算出部８２の算出方法等については、後述する。
【0026】
拡張部８３は、重心補償力算出部８２が算出した重心補償力に対して、立位時の重力補償項を３次元に拡張する。なお、拡張部８３の処理方法等については、後述する。
【0027】
切替部８４は、手を着く、手を離すなどの接触点の切り替わりについて、実際の接触に応じてフィード・フォワード項（水平力の項と垂直力の項）を切り替える。なお、切替部８４の切り替え方法等については、後述する。
【0028】
駆動部８５は、センサ７が検出した検出値、拡張部８３が拡張した結果、切替部８４が切り替えた結果に基づいて、駆動信号を生成して関節アクチュエータ９を駆動する。
【0029】
制御部８６は、センサ７が検出した検出値を用いて、ロボット６の状態（例えばＰｈａｓｅ１～４）を検出する。なお、ロボット６の状態については後述する。制御部８６は、検出したロボット６の状態に応じて、例えば、制御モードを切り替えるように制御する。
【0030】
＜ロボットの構成例＞
ここで、ロボット６の構成例を説明する。
図２は、ロボットの構成を示す図である。図２に示したロボット６は、基体２と、基体２から延設された複数の可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒと、頭部５とを備える人型の脚式移動ロボットである。
ここで、ロボット６の構成例を説明する。
図２は、ロボットの構成を示す図である。図２に示したロボット６は、基体２と、基体２から延設された複数の可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒと、頭部５とを備える人型の脚式移動ロボットである。
【0031】
４つの可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの可動リンク機構３Ｌ，３Ｒは、左右一対の脚リンク機構に相当するリンク機構、可動リンク機構４Ｌ，４Ｒは、左右一対の腕リンク機構に相当するリンク機構である。
【0032】
なお、図示の参照符号中の「Ｌ」、「Ｒ」は、それぞれロボット６の左側の構成要素、右側の構成要素を示す符号である。ただし、以降の説明では、左右を区別する必要が無いときは、符号「Ｌ」，「Ｒ」を省略する。また、可動リンク機構３Ｌ，３Ｒを脚リンク機構３Ｌ，３Ｒ（又は脚リンク機構３）と称し、可動リンク機構４Ｌ，４Ｒを腕リンク機構４Ｌ，４Ｒ（又は腕リンク機構４）と称する場合がある。
【0033】
また、実施形態の以下の説明では、ヨー方向、ピッチ方向、ロール方向は、それぞれ図１に示すようにロボット６が直立姿勢で起立した状態（基体２、各可動リンク３Ｒ，３Ｌ，４Ｒ，４Ｌをほぼ上下方向に伸ばした状態）でのロボット６の上下方向の軸（図２に示すＺ軸）周りの方向、左右方向の軸（図２に示すＹ軸）周りの方向、前後方向の軸（図２ではＸ軸）周りの方向を意味する。なお、図２に示すＸ軸、Ｙ軸、Ｚ軸は、３軸直交座標系の各座標軸である。
【0034】
基体２は、ロボット６の上体に相当するリンク機構である。この基体２は、例えば、下側基体１０と、この下側基体１０の上方に配置された上側基体１１との２つの要素リンクと、該下側基体１０及び上側基体１１を連結する関節機構１２とから構成されている。図２のロボット６では、関節機構１２は、例えばピッチ方向（Ｙ軸周り方向）の回転自由度を有する関節１２ａにより構成されている。
【0035】
可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの脚リンク機構３Ｌ，３Ｒは、互いに同じ構造のものである。具体的には、各脚リンク機構３は、下側基体１０に股関節機構１３を介して連結された大腿部１４と、この大腿部１４に膝関節機構１５を介して連結された下腿部１６と、この下腿部１６に足首関節機構１７を介して連結された足部１８とを、脚リンク３機構を構成する要素リンクとして備える。足部１８は、脚リンク機構３の先端部を構成する要素リンクである。
【0036】
図２のロボット６では、股関節機構１３は、ヨー方向（Ｚ軸周り方向）、ピッチ方向（Ｙ軸周り方向）及びロール方向（Ｘ軸周り方向）の回転自由度をそれぞれ有する３つの関節１９，２０，２１により構成されている。また、膝関節機構１５は、ピッチ方向の回転自由度を有する関節２２により構成されている。また、足首関節機構１７は、ピッチ方向及びロール方向の回転自由度をそれぞれ有する２つの関節２３，２４により構成されている。従って、図２のロボット６では、各脚リンク３機構が、その先端部（足部１８）が下側基体１０に対して、６自由度の運動自由度を有するように構成されている。
【0037】
可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの腕リンク機構４Ｒ，４Ｌは、互いに同じ構造のものである。具体的には、各腕リンク機構４は、上側基体１１に肩関節機構２５を介して連結された上腕部２６と、この上腕部２６に肘関節機構２７を介して連結された前腕部２８と、この前腕部２８に手首関節機構２９を介して連結されたハンド部３０とを、該腕リンク機構４を構成する要素リンクとして備える。ハンド部３０は、腕リンク機構４の先端部を構成する要素リンクである。
【0038】
図２のロボット６では、肩関節機構２５は、ピッチ方向、ロール方向、及びヨー方向の回転自由度をそれぞれ有する３つの関節３１，３２，３３により構成されている。また、肘関節機構２７は、ピッチ方向（又はロール方向）の回転自由度を有する関節３４により構成されている。また、手首関節機構２９は、ヨー方向、ピッチ方向及びロール方向の回転自由度をそれぞれ有する２つの関節３５，３６，３７により構成されている。従って、図２のロボット６では、各腕リンク機構４が、その先端部（ハンド部３０）が上側基体１１に対して、７自由度の運動自由度を有するように構成されている。
【0039】
なお、ハンド部３０は、ハンド部３０による作業用の開閉機構、あるいは、複数の屈伸可能な指機構等を備えていてもよい。
【0040】
頭部５は、上側基体１１の上方に配置され、上側基体１１に固定されている。ただし、頭部５は、例えばチルト動作あるいはパン動作等を行い得るように、上側基体１１に対して関節機構を介して連結されていてもよい。
【0041】
本実施形態のロボット６は、基本的には、左右の脚リンク機構３Ｌ，３Ｒの動作（各足部１８の空中移動及びそれに続く接地を繰り返す２脚歩容の動作）によって移動する。ロボット６は、手と足とを制御して移動や作業の動作を制御する。
【0042】
＜受動性に基づくヒューマノイドロボットの制御の概要＞
次に本発明の基礎となる受動性に基づくヒューマノイドロボットの制御について、説明する（詳細は参考文献１を参照）。
次に本発明の基礎となる受動性に基づくヒューマノイドロボットの制御について、説明する（詳細は参考文献１を参照）。
【0043】
参考文献１；Ryuta Ozawa and Jun’ya Ishizaki, “Passivity-Based Symmetric Posture Control and the Effects of Mass Distribution and Internal Impedance on Balance”, 2012 12th IEEE-RAS International Conference on Humanoid Robots Nov.29-Dec.1, 2012. Business Innovation Center Osaka, Japan, IEEE, p171-176,2012
【0044】
＜ロボットの運動方程式＞
次に、ロボットの座標系として次式（１）のものを考える。
次に、ロボットの座標系として次式（１）のものを考える。
【0045】
【数1】
【0046】
式（１）において、ｘＣＯＭ、θｂａｓｅは絶対座標系からみた重心位置と上体姿勢角であり、これらを合わせた状態をｙ∈Ｒ６（Ｒは二重線文字、実施全体の集合）とし、ｑｂｂは背骨の関節角である。ｑｒａは右腕の関節角、ｑｌａは左腕の関節角、ｑｒｌは右脚の関節角、ｑｌｌは左脚の関節角である。背骨と各四股の関節角をまとめたベクトルをｑ∈ＲＮとする。
ここで、多リンクが接地している状況を考える。接触面において、環境から各リンクに作用する力点ＰＯＡｉ（ｉ＝１，２，…，ｎ）における拘束条件は次式（２）のように表現できる。
ここで、多リンクが接地している状況を考える。接触面において、環境から各リンクに作用する力点ＰＯＡｉ（ｉ＝１，２，…，ｎ）における拘束条件は次式（２）のように表現できる。
【0047】
【数2】
【0048】
式（２）において、ｘＰＯＡｉは第ｉ接触点における位置、θＰＯＡｉは第ｉ接触点における姿勢、ｙＰＯＡｉは第ｉ接触点におけるそれらを合わせたベクトル、ｃｉは接触している環境の位置姿勢である。また、０（太字）ｋは、ｋ次元のゼロベクトルである。
このため、拘束全体は次式（３）として表現できる。
このため、拘束全体は次式（３）として表現できる。
【0049】
【数3】
【0050】
拘束条件の微分は次式（４）となる。なお、上付き１つドットは微分を表す。
【0051】
【数4】
【0052】
式（４）において、Ｊｃ（∈Ｒ６×（６＋Ｎ））（Ｒは二重線文字）はｚから接触点へのヤコビ行列であり、ＡｃはＪｃのｙに関する６×６の部分行列であり、Ｂｃはｑに関する６×Ｎの部分行列である。ロボットのラグランジュアンを次式（５）とする。
【0053】
【数5】
【0054】
式（５）において、Ｈ（ｚ）は慣性行列、Ｕ（ｚ）は重力によるポテンシャルエネルギとする。Ｔは転置を表す。この運動方程式は次式（６）となる。
【0055】
【数6】
【0056】
式（６）において、Ｓは歪対称行列、λ∈Ｒ６ｎは拘束力であり，ｕ∈Ｒ６ｎは制御入力である。なお、上付き２つドットは二階微分を表す。また、ＥＴは、伝達マトリックスであり、次式（７）である。ｇ（ｚ）は、重力項であり、次式（８）である。なお、Ｉは単位行列である。
【0057】
【数7】
【0058】
【数8】
【0059】
このように、このロボットは上体に制御入力が直接は入らないため、劣駆動系となっている。
【0060】
＜受動性に基づく状態量のフィードバック制御の構成例＞
受動性に基づく状態量のフィードバック制御を説明する。
まず、受動性について説明する。電気回路では、回路が受動要素のみで構成されるならば、そのポートから流入するエネルギーは保存または消費されるので、回路の安定化に繋がる。この概念は、線形システムの伝達関数の正実性と等化となることが知られている。受動性に基づくロボット制御は、この概念がハミルトニアンで記述される非線形なロボット制御系に広がる（例えば参考文献２参照）。ロボットにおける受動性に基づく制御方法は、モデルに基づく制御系と並ぶトルク制御の一手法である。
受動性に基づく状態量のフィードバック制御を説明する。
まず、受動性について説明する。電気回路では、回路が受動要素のみで構成されるならば、そのポートから流入するエネルギーは保存または消費されるので、回路の安定化に繋がる。この概念は、線形システムの伝達関数の正実性と等化となることが知られている。受動性に基づくロボット制御は、この概念がハミルトニアンで記述される非線形なロボット制御系に広がる（例えば参考文献２参照）。ロボットにおける受動性に基づく制御方法は、モデルに基づく制御系と並ぶトルク制御の一手法である。
【0061】
参考文献２；Morikazu Takegaki, Suguru Arimoto, “A New Feedback Method for Dynamic Control of Manipulators”、ASME Journal of Dynamic Systems, Measurement, and Contorol, Vol. 102, pp.119-125, 1981
【0062】
一般にロボットの目標状態は、ｚの関数ｐｃｔｒｌ（ｚ）として与えられる。この時、目標姿勢ｐｃｔｒｌ
ｄに対して、誤差が次式（９）により与えられる。
【0063】
【数9】
【0064】
式（９）の微分は、次式（１０）で与えられる。
【0065】
【数10】
【0066】
通常、フローティングベースへの直接的な制御入力は無い。しかしながら、このフローティングベースへの制御入力ｖｂ∈Ｒ６があると仮定し、制御入力ｖを次式（１１）のように二つの項から構成する。
【0067】
【数11】
【0068】
式（１１）において、ｖｑ∈ＲＮは関節に直接作用する制御入力となる。ｖｆｂはフィードバック項であり、次式（１２）のように定義される。
【0069】
【数12】
【0070】
式（１２）において、Ｋ，Ｄ，Ｃは正定対角行列である。フィード・フォワード項ｖｆｆは、用いる状況によって形を変えるため、Ｋ、Ｄ、Ｃの正定対角行列を使用する際に導入する。
【0071】
なお、式（８）をフィード・フォワード項ｖｆｆとし、式（１１）に式（８）と（１２）を代入すると、制御入力ｖは次式（１３）のようにも表せる。
【0072】
【数13】
【0073】
なお、式（１３）において、Ｊｓはヤコビアン行列であり、ｓは任意の制御状態であり、ｑは関節角度のベクトルである。
式（１２）と（１３）の関係は、Ｋ＝Ｋｐ、Ｄ＝Ｋｖ、Δｐｃｔｒｌ＝Δｓ、Δｐ・ ｃｔｒｌ＝Δｓ・、Ｃ＝［０ Ｄ］Ｔ、ｚ・＝ｑ・である。
式（１２）と（１３）の関係は、Ｋ＝Ｋｐ、Ｄ＝Ｋｖ、Δｐｃｔｒｌ＝Δｓ、Δｐ・ ｃｔｒｌ＝Δｓ・、Ｃ＝［０ Ｄ］Ｔ、ｚ・＝ｑ・である。
【0074】
＜実現可能な制御入力ｕへの変換＞
制御入力の式（１２）は、システムｚｃｔｒｌを安定化することができるが、実際には劣駆動系であり、ｖｂを生成できないため実現が困難である。このため、接触点における拘束力が仕事をしないことを利用したパワー保存変換により、制御入力の式（１３）を実現可能な制御入力ｕに変換する。
制御入力の式（１２）は、システムｚｃｔｒｌを安定化することができるが、実際には劣駆動系であり、ｖｂを生成できないため実現が困難である。このため、接触点における拘束力が仕事をしないことを利用したパワー保存変換により、制御入力の式（１３）を実現可能な制御入力ｕに変換する。
【0075】
図３は、４本の手足で環境に接する３Ｄヒューマノイドモデルを示す図である。図３のように。手足の４点で床や壁に接触する可能性があるとする。図３において、基準点は床に設定されている。ｘｌａは基準点からみた左腕の位置であり、ｘｒａは基準点から右腕の位置であり、ｘｌｌは基準点から左足の位置であり、ｘｒｌは基準点から右足の位置である。この時、φｒａ、φｌａ、φｒｌ、φｌｌを各接触点における拘束条件とすると、式（３）は次式（１４）に書き直すことができる。
【0076】
【数14】
【0077】
これらの拘束条件から，制御方向を選択するための行列Ｓｉ（ｉ∈Ｓｌｉｍｂ=｛ｒａ、ｌａ、ｒｌ、ｌｌ｝）を導入し、それらを並べたものを次式（１５）のようにＳｐとする。
【0078】
【数15】
【0079】
パワー保存変換では、接触点における拘束条件φｃから構成された関数は次式（１６）となる。
【0080】
【数16】
【0081】
この場合、Ｓｒａ、Ｓｌａ、Ｓｒｌ、Ｓｌｌの各対角要素は零以上であり、次式（１７）を満足する。
【0082】
【数17】
【0083】
この時間微分は、次式（１８）となる。
【0084】
【数18】
【0085】
式（１８）において、Ａｐは正則な行列である。この場合、次式（１９）が成り立つ。
【0086】
【数19】
【0087】
これにより、各拘束方向の力をどの接触点で分配するかの記述ができるようになる。この拘束条件を用いて仮想的な全制御入力（式（１２）、式（１３））を変換し、制御入力ｕ∈ＲＮを次式（２０）のように構成する。
【0088】
【数20】
【0089】
式（２０）において、Ｑｐ
Ｔは、次式（２１）である。
【0090】
【数21】
【0091】
これを用いることで、ｖと等価の効力を持つ制御入力ｕを構成できる。
【0092】
＜多点接触による重心の移動についての縮退モデルの力学＞
次に、前記手法を多点接触に拡張した本発明について説明する。
図４、図５は、立っている状態から支えている状態への移行の一例を示す図である。図４、５に示すように、立位から壁接触へ映る際の遷移の制御モードの変化は、例えば以下の４つのモードから構成されている。
Ｐｈａｓｅ１（図４の画像ｇ１０１） 足で立ち、立位安定化する。この場合は、足が床に接している。重力Ｍｇがかかるので、足で支えるため、重力Ｍｇと足の反力λＰＯＡ１とが釣り合う。
Ｐｈａｓｅ２（図４の画像ｇ１０２） 手を前に伸ばし壁との接触を探し、環境と接触する。なお、手は壁に接触しているのみで、力は発生していない。画像ｇ１０２のように、重心の水平位置は、足の略中心あたりに移動する。
Ｐｈａｓｅ３（図５の画像ｇ１０３） 手が壁に接触後、重心を前に移動する。このため重心の水平位置は爪先よりも後方にある。
Ｐｈａｓｅ４（図５の画像ｇ１０４） Ｐｈａｓｅ３からさらに重心を前に移動する。この場合は、画像ｇ１０４のように、重力Ｍｇの向きと足の反力の向きが白抜き矢印のようにずれているので、重心位置に回転する力が発生する。この回転する分を手で壁に対して水平方向に力を発生させて補償する。
次に、前記手法を多点接触に拡張した本発明について説明する。
図４、図５は、立っている状態から支えている状態への移行の一例を示す図である。図４、５に示すように、立位から壁接触へ映る際の遷移の制御モードの変化は、例えば以下の４つのモードから構成されている。
Ｐｈａｓｅ１（図４の画像ｇ１０１） 足で立ち、立位安定化する。この場合は、足が床に接している。重力Ｍｇがかかるので、足で支えるため、重力Ｍｇと足の反力λＰＯＡ１とが釣り合う。
Ｐｈａｓｅ２（図４の画像ｇ１０２） 手を前に伸ばし壁との接触を探し、環境と接触する。なお、手は壁に接触しているのみで、力は発生していない。画像ｇ１０２のように、重心の水平位置は、足の略中心あたりに移動する。
Ｐｈａｓｅ３（図５の画像ｇ１０３） 手が壁に接触後、重心を前に移動する。このため重心の水平位置は爪先よりも後方にある。
Ｐｈａｓｅ４（図５の画像ｇ１０４） Ｐｈａｓｅ３からさらに重心を前に移動する。この場合は、画像ｇ１０４のように、重力Ｍｇの向きと足の反力の向きが白抜き矢印のようにずれているので、重心位置に回転する力が発生する。この回転する分を手で壁に対して水平方向に力を発生させて補償する。
【0093】
体重は両足に均等にかけるものとする。この場合は、選択行列を次式（２２）とする。
【0094】
【数22】
【0095】
冗長関節を持たない両足が地面に接地されている時、この座標系に対して、足の関節角ｑｌｌ、ｑｒｌとｆｌｏａｔｉｎｇ ｂａｓｅの位置姿勢ｙは独立に選択することができない。この場合は、冗長がない状態として、次式（２３）のような状態ｚｃｔｒｌを独立に制御する。
【0096】
【数23】
【0097】
一方、Ｐｈａｓｅ３とＰｈａｓｅ４では、接触に関する選択行列ＳｐをＰｈａｓｅ２とスムーズにつなぐために変更する必要がある。このため、本実施形態では、縮退モデルを用いて重心補償力を考える。
Ｐｈａｓｅ３（図３の画像ｇ１０３）のように、矢状面においてロボットが多点接触した状況を考える。この場合、足は水平な地面と接触し、手は垂直に反り立った壁に接触している。Ｐｈａｓｅ４では、重心が足と壁の間に移動している。このため、Ｐｈａｓｅ３では、手の接触が無い限りバランスを保つことができない。
Ｐｈａｓｅ３（図３の画像ｇ１０３）のように、矢状面においてロボットが多点接触した状況を考える。この場合、足は水平な地面と接触し、手は垂直に反り立った壁に接触している。Ｐｈａｓｅ４では、重心が足と壁の間に移動している。このため、Ｐｈａｓｅ３では、手の接触が無い限りバランスを保つことができない。
【0098】
重心の位置をｘＣＯＭ＝（ｘｃ,ｚｃ）とし，その目標位置をｘＣＯＭｄとする。接触している面に力点を設定し、この位置をｘＰＯＡｉ＝（ｘｉ,ｚｉ）とし、そこからロボットに作用する反力をλＰＯＡｉ＝（λｘｉ,λｚｉ）とする。この接触により平衡状態を保てているならば，次式（２４）～（２６）の力条件を満足しなければならない。
【0099】
【数24】
【0100】
【数25】
【0101】
【数26】
【0102】
ここで、壁からの反力λｈで押す状況を考える。式（２４）から、各位置ＰＯＡｉでの水平方向の拘束力は次式（２７）となる。λｚ１は足の水平の力であり、λｚ２は手の水平力である。
【0103】
【数27】
【0104】
また、全身にかかる重力を足で支えるとすると、式（２５）から次式（２８）となる。式（２８）は、足の垂直力λｚ１と重力とが釣り合い、手の垂直力λｚ２が０であることを表している。
【0105】
【数28】
【0106】
この時、式（２６）は、次式（２９）のような条件に書き換えられる。
【0107】
【数29】
【0108】
Ｐｈａｓｅ３のように、床反力と重心に作用する重力し、この力により半時計回りに回転を始める。式（３１）は、手に相当する位置ＰＯＡ２で、次式（３０）で表される並進力によりこの回転力をキャンセルできることを意味している。なお、この反力λｈは、式（２７）のように、λｈ＝λｚ２＝－λｚ１である。
【0109】
【数30】
【0110】
位置ＰＯＡｉは定数であるため、法線力λｈは重心位置の水平位置ｘｃの関数となる。ここで、動的な力や重心高さｚｃ－ｚ１には依存しな。なお、接触点の高さｚ２－ｚ１には依存する。
【0111】
＜拡張＞
手をついた状態で重心を足の支持多角形の前に出し、バランスを取るためには式（３０）のような力を手で発生させる必要がある。この力は重心に作用する力であり、全身に作用する力に変換するには次式（３１）のように水平力の項（重心による回転を補償するフィード・フォワード項）ｖｆｆｈを発生させる必要がある。また、実施形態では、ｖｆｆｖを式（８）とする。
手をついた状態で重心を足の支持多角形の前に出し、バランスを取るためには式（３０）のような力を手で発生させる必要がある。この力は重心に作用する力であり、全身に作用する力に変換するには次式（３１）のように水平力の項（重心による回転を補償するフィード・フォワード項）ｖｆｆｈを発生させる必要がある。また、実施形態では、ｖｆｆｖを式（８）とする。
【0112】
【数31】
【0113】
並進力は手のみ、鉛直力は足のみ，フィードバック項は手足で発生させるものとする。
Ｐｈａｓｅ２では、フィード・フォワードの入力として安定した力を加えるようにするため、式（１１）を次式（３２）のように拡張する。
Ｐｈａｓｅ２では、フィード・フォワードの入力として安定した力を加えるようにするため、式（１１）を次式（３２）のように拡張する。
【0114】
【数32】
【0115】
さらに、制御入力ｕは、次式（３３）となる。
【0116】
【数33】
【0117】
Ｑｐｉは、それぞれの接触状態に合わせた式（２１）であり、それぞれに対応する式（１９）の選択行列ＳＰｉは、次式（３４）～（３６）である。
【0118】
【数34】
【0119】
【数35】
【0120】
【数36】
【0121】
すなわち、フィードバック項は手と足に分配し、水平フィードフォード項は式（２７）に基づき手と足で逆の符号に分配し、鉛直フィード・フォワード項は式（２８）に基づいて足のみに分配する。なお、このパワー保存変換は、各項において独立に計算されてもよい。また、式（１８）において、Ａｐ、Ｂｐは接触点へのヤコビ行列のＪｐの部分行列であったが、Ｐｈａｓｅ２では各足の２点接触からＰｈａｓｅ３にかけて両手両足の４点接触へ変化する。Ｑｐｉの不連続な変化を避けるため、図７に示すような力制御ゲインαｉ（ｉ∈［１，４］）を各手足に設定し重みづけする。
また、Ｑｐｉ Ｔは次式（３７）であり、［Ａｐｉ Ｂｐｉ］＝Ｊｐｉである。
また、Ｑｐｉ Ｔは次式（３７）であり、［Ａｐｉ Ｂｐｉ］＝Ｊｐｉである。
【0122】
【数37】
【0123】
また、状態量ｐｃｔｒｌを次式（３８）とする。
【0124】
【数38】
【0125】
＜制御系の構成例＞
次に制御系の構成例を説明する。図６は、本実施形態に係る制御系の構成例を示す図である。図６のアルゴリズムの実行方法をまとめると、以下のようになる。
Ｉ．フィードバック誤差情報量Δｐｃｔｒｌ＝ｐｃｔｒｌ（ｚ）－ｐｃｔｒｌ ｄ（式（９））を決定し、そのヤコビ行列ＪｃｔｒｌをΔｐ・ ｃｔｒｌ＝ｐ・ ｃｔｒｌ＝Ｊｃｔｒｌ（ｚ）ｚ・（式（１０））から求める。
ＩＩ．理想的な（実現不能な）制御入力ｖを式（１１）により構成する（ｇ２０１）。
ＩＩＩ．力点での拘束条件φｐ＝Ｓｐφｃ＝０（０は太字）（式（１７））を求め、パワー保存変換に用いるヤコビ行列ＡＰ，ＢＰを式（２０）から求める。
ＩＶ．パワー保存変換（式（２１））により、ｖから実現可能な制御入力ｕを求める（ｇ２０２）。
次に制御系の構成例を説明する。図６は、本実施形態に係る制御系の構成例を示す図である。図６のアルゴリズムの実行方法をまとめると、以下のようになる。
Ｉ．フィードバック誤差情報量Δｐｃｔｒｌ＝ｐｃｔｒｌ（ｚ）－ｐｃｔｒｌ ｄ（式（９））を決定し、そのヤコビ行列ＪｃｔｒｌをΔｐ・ ｃｔｒｌ＝ｐ・ ｃｔｒｌ＝Ｊｃｔｒｌ（ｚ）ｚ・（式（１０））から求める。
ＩＩ．理想的な（実現不能な）制御入力ｖを式（１１）により構成する（ｇ２０１）。
ＩＩＩ．力点での拘束条件φｐ＝Ｓｐφｃ＝０（０は太字）（式（１７））を求め、パワー保存変換に用いるヤコビ行列ＡＰ，ＢＰを式（２０）から求める。
ＩＶ．パワー保存変換（式（２１））により、ｖから実現可能な制御入力ｕを求める（ｇ２０２）。
【0126】
＜接触に伴うゲインの切り替え＞
図７は、力コントロールゲインの切り替え例を示す図である。図７において、信号ｇ３０１は制御信号であり、信号ｇ３０２は力コントロールゲインの結果である。また、図７において、横軸は時刻である。
信号ｇ３０１において、縦軸は制御オン状態とオフ状態を表している。
信号ｇ３０２において、縦軸は力コントロールゲインである。力コントロールゲインは、“１”のとき接触可能な状態であり、“０”のとき接触不可能な状態である。
本実施形態では、制御信号（ｇ３０１）に対して、力コントロールゲインの結果（ｇ３０２）は、急激に切り替えるのではなく、図７のように時刻ｔ１～ｔ２またはｔ３～ｔ４の期間の所定時間を要してスムーズに切り替えるようにしてもよい。所定時間は、例えば０．２（ｓ）である。
図７は、力コントロールゲインの切り替え例を示す図である。図７において、信号ｇ３０１は制御信号であり、信号ｇ３０２は力コントロールゲインの結果である。また、図７において、横軸は時刻である。
信号ｇ３０１において、縦軸は制御オン状態とオフ状態を表している。
信号ｇ３０２において、縦軸は力コントロールゲインである。力コントロールゲインは、“１”のとき接触可能な状態であり、“０”のとき接触不可能な状態である。
本実施形態では、制御信号（ｇ３０１）に対して、力コントロールゲインの結果（ｇ３０２）は、急激に切り替えるのではなく、図７のように時刻ｔ１～ｔ２またはｔ３～ｔ４の期間の所定時間を要してスムーズに切り替えるようにしてもよい。所定時間は、例えば０．２（ｓ）である。
【0127】
＜シミュレーション結果＞
次に、足で立っている状態から、手と足で体を支える状態への移行についてのシミュレーション結果の一例を説明する。図８は、足で立っている状態から、手と足で体を支える状態への移行についてのシミュレーション結果における時刻毎の状態を示す図である。画像ｇ４０１～ｇ４０９は、０．１（ｓ）、８．４（ｓ）、８．９（ｓ）、１８．２（ｓ）、２２．０（ｓ）、２９．２（ｓ）、４３．６（ｓ）、４５．８（ｓ）、４７．５（ｓ）の状態の画像である。図８のように、シミュレーションでは、立位から始め、壁に接触し、接触が安定化した後、壁にもたれかかるように重心を前に移動した。そしてシミュレーションでは、その後、元の立脚姿勢に戻ることができた。
次に、足で立っている状態から、手と足で体を支える状態への移行についてのシミュレーション結果の一例を説明する。図８は、足で立っている状態から、手と足で体を支える状態への移行についてのシミュレーション結果における時刻毎の状態を示す図である。画像ｇ４０１～ｇ４０９は、０．１（ｓ）、８．４（ｓ）、８．９（ｓ）、１８．２（ｓ）、２２．０（ｓ）、２９．２（ｓ）、４３．６（ｓ）、４５．８（ｓ）、４７．５（ｓ）の状態の画像である。図８のように、シミュレーションでは、立位から始め、壁に接触し、接触が安定化した後、壁にもたれかかるように重心を前に移動した。そしてシミュレーションでは、その後、元の立脚姿勢に戻ることができた。
【0128】
制御モードを変更する冗長コントロールモードでは、ｐｃｔｒｌは次式（３９）である。また、制御入力ｖは式（１３）である。
【0129】
【数39】
【0130】
例えば、画像ｇ４０１（０．１（ｓ））の状態は、スタンディングモードであり、上半身のコントロールが弱いため、ｐｃｔｒｌは次式（４０）となる。
【0131】
【数40】
【0132】
画像ｇ４０４（１８．２（ｓ））の状態は、サポートモードであり、手の位置を制御するため、ｐｃｔｒｌは次式（４１）となる。
【0133】
【数41】
【0134】
図９は、壁に接触するシミュレーションを行った場合のｘ方向の重心位置とｙ方向の重心位置と、ｘ方向の接触力とｚ軸方向の接触力を示す図である。グラフｇ５００はｘ方向の重心位置の変化を表し、グラフｇ５１０はｙ方向の重心位置の変化を表す。グラフｇ５２０はｘ方向の接触力の変化を表し、グラフｇ５３０はｙ方向の接触力の変化を表す。また、グラフｇ５００～ｇ５３０において、横軸は時刻（ｓ）であり、グラフｇ５１０とｇ５２０の縦軸は位置であり、グラフｇ５２０とｇ５４０の縦軸は接触力（Ｎ）である。
【0135】
グラフｇ５００の線ｇ５０１と、グラフｇ５１０の線５１１は、目標値である。グラフｇ５００の線ｇ５０２と、グラフｇ５１０の線５１２は、実際値である。グラフｇ５００とｇ５１０のように、本実施形態によれば、ｘ方向とｚ方向の重心位置の実測値（シミュレーション結果）は、目標値に近い値である。
グラフｇ５２０の線ｇ５２１と、グラフｇ５３０の線ｇ５３１は、足の接触力である。グラフｇ５２０の線ｇ５２２と、グラフｇ５３０の線ｇ５３２は、手の接触力である。グラフｇ５２０のように、ｘ方向では、手と足とにかかる力が互いに正負反対方向に働いているので、式（２９）と一致する。また、グラフｇ５３０のように、ｚ方向では、手にほとんど力がかかっていず、体重が掛かっているので足に力がかかっている。
グラフｇ５２０の線ｇ５２１と、グラフｇ５３０の線ｇ５３１は、足の接触力である。グラフｇ５２０の線ｇ５２２と、グラフｇ５３０の線ｇ５３２は、手の接触力である。グラフｇ５２０のように、ｘ方向では、手と足とにかかる力が互いに正負反対方向に働いているので、式（２９）と一致する。また、グラフｇ５３０のように、ｚ方向では、手にほとんど力がかかっていず、体重が掛かっているので足に力がかかっている。
【0136】
【0137】
（ステップＳ１）取得部８１は、センサ７が検出した検出値を取得する。
【0138】
（ステップＳ２）制御部８６は、センサ７が検出した検出値を用いて、ロボット６の状態（例えばＰｈａｓｅ１～４）を検出する。
【0139】
（ステップＳ３）制御部８６は、検出したロボット６の状態に応じて、例えば、制御モード、使用するフィード・フォワード項を切り替えるように制御する。
【0140】
（ステップＳ４）制御部８６は、ロボット６の状態がＰｈａｓｅ４であるか、Ｐｈａｓｅ１～３であるかを判別する。制御部８６は、Ｐｈａｓｅ４であると判別した場合（ステップＳ４；Ｐｈａｓｅ４）、ステップＳ８の処理に進める。制御部８６は、Ｐｈａｓｅ１～３であると判別した場合（ステップＳ４；Ｐｈａｓｅ１～３）、ステップＳ５の処理に進める。
【0141】
（ステップＳ５）制御部８６は、重心のポテンシャルに合わせてフィード・フォワード項ｖｆｆを式（２３）に設定する。制御部８６は、処理後、ステップＳ６の処理に進める。
【0142】
（ステップＳ６）拡張部８３は、分離型パワー保存則に基づいて実行可能な制御入力ｕに置き換える。拡張部８３は、処理後、ステップＳ７の処理に進める。
【0143】
（ステップＳ７）制御部８６は、式（２２）で関節トルクｕを求めて、目標関節トルクを発揮するように関節アクチュエータ９を制御する。制御部８６は、処理後、ステップＳ１の処理に戻す。
【0144】
（ステップＳ８）重心補償力算出部８２は、重心の位置をｘＣＯＭ＝（ｘｃ,ｚｃ）とし，その目標位置をｘＣＯＭｄとする。接触している面に力点を設定する。
【0145】
（ステップＳ９）重心補償力算出部８２は、並進力λｈを、式（３２）を用いて算出する。
【0146】
（ステップＳ１０）重心補償力算出部８２は、水平力のフィード・フォワード項ｖｆｆｈを、式（３３）を用いて算出し、垂直力のフィード・フォワード項ｖｆｆｖを、式（３５）を用いて算出する。
【0147】
（ステップＳ１１）拡張部８３は、水平力のフィード・フォワード項ｖｆｆｈを、式（３３）を用いて算出し、垂直力のフィード・フォワード項ｖｆｆｖを用いて、制御入力ｖを拡張する。
【0148】
（ステップＳ１２）拡張部８３は、分離型パワー保存則に基づいて実行可能な制御入力ｕに置き換える。
【0149】
（ステップＳ１３）制御部８６は、式（２２）で関節トルクｕを求めて、目標関節トルクを発揮するように関節アクチュエータ９を制御する。御部８６は、処理後、ステップＳ１の処理に戻す。
【0150】
以上のように、本実施形態では、受動性に基づく制御をマルチコンタクトタスクに適用するための理論と戦略をたて制御するようにした。これにより、本実施形態によれば、シミュレーションによって、マルチコンタクトの安定化問題を実現できることを確認した。
【0151】
そして、本実施形態では、手足の幾何学的関係より重心補償力を計算し、立位時の重力補償項を３次元に拡張することで、３次元の重心補償力をフィード・フォワード項として追加するようにした。本実施形態では、この際、水平力と垂直力は独立した項として定義するようにした。また、本実施形態では、手を着く、離すなどの接触点の切り替わりについては実際の接触に応じてフィード・フォワード項を変化させることで実現した。
【0152】
これにより、本実施形態によれば、複数の接触制約を満たしながら，重心安定化を実現できるようになる。本実施形態によれば、受動性における証明により制御系として安定となるので、ロバストである。本実施形態によれば、計算はヤコビ行列の正則な部分行列の逆行列を求めるのみという小さい計算コストで可能となる。本実施形態によれば、パラメタは比例、微分ゲインのみで、最適化問題で扱う重みのような無次元係数ではなく、物理的意味から直感的にチューニングが可能である。
【0153】
なお、上述した例では、ロボット６が図２のようなボディ、頭部、２つのアーム、２つの足等を備える例を説明したが、これに限らない。本実施形態の制御方法は、２次元空間を移動可能なボディに２つ以上の２自由度以上のアーム（もしくは足）を持つロボットの制御に適用可能である。本実施形態の制御方法は、このようなロボットが非平面上に接触しながらボディを支える状況の制御に適用可能である。
【0154】
また、上述した例では、３次元で説明したが、水平力のフィード・フォワード項を適用できる平面の２次元にも適用可能である。この場合は、２次元空間で任意の方向に力を発生させる必要があるため、アームも２自由度必要である。また、アーム一本ではボディを支えることができないので、この場合は、アームも２本以上必要である。
【0155】
なお、本発明における制御装置８の機能の全てまたは一部を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより制御装置８が行う処理の全てまたは一部を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータシステム」は、ホームページ提供環境（あるいは表示環境）を備えたＷＷＷシステムも含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ－ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（ＲＡＭ）のように、一定時間プログラムを保持しているものも含むものとする。
【0156】
また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
【0157】
以上、本発明を実施するための形態について実施形態を用いて説明したが、本発明はこうした実施形態に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形および置換を加えることができる。
【符号の説明】
【0158】
１…ロボット制御システム、７…センサ、８…制御装置、９（９－１，９－２，…，９－ｎ）…関節アクチュエータ、７１…傾斜センサ、７２…６軸センサ、７３…エンコーダ、８１…取得部、８２…重心補償力算出部、８３…拡張部、８４…切替部、８５…駆動部、８６…制御部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】