WO2023037550A1

WO2023037550A1 - ロボット、ロボットシステム、制御方法及びプログラム

Info

Publication number: WO2023037550A1
Application number: PCT/JP2021/033565
Authority: WO
Inventors: 雄希松尾; 高志佐藤
Original assignee: 東京ロボティクス株式会社
Priority date: 2021-09-13
Filing date: 2021-09-13
Publication date: 2023-03-16

Abstract

複数の関節を備えたロボットであって、切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定部と、前記対象関節を連動制御する、制御部と、を備えた、ロボットが提供される。

Description

ロボット、ロボットシステム、制御方法及びプログラム

　この発明は、複数の関節を備えたロボット、ロボットを含むシステム、ロボットの制御方法、又は、制御プログラム等に関する。

　近年、人間類似の作業を可能とするため、産業界において、人間と同様に複数の関節を備えたロボットが注目されている。例えば、胴部と胴部の左右から延びる腕部を備える双腕ロボット等が注目されている（例えば、特許文献１）。

　複数の関節を備えたロボットの制御にあたっては、各関節に対して関節座標系において指令を与える方法と、カルテシアン座標系（直交座標系又はデカルト座標系とも呼ばれる）において、所定の目標点（例えば、手先位置やＴＣＰ（Ｔｏｏｌ　Ｃｅｎｔｅｒ　Ｐｏｉｎｔ）等）に対して基準座標に対する指令を与え、逆運動学計算等を行って、各関節を連動制御する方法が知られている。

特開２００６－１６７９０２号公報

　ところで、ロボットの持つ関節のうちどの関節に対して連動制御を適用するかについては、従前いくつかの方法論が存在した。より詳細には、従前の複数関節を備えたロボットにおいては、ハードウェア構成に応じて、その一部のみに連動制御が適用されることがあった。例えば、双腕ロボットにおいては、ロボットアームを単に本体部に取り付けたかのように、腕部と胴部とは別々に制御され、腕部に対してのみ連動制御が適用されることがあった。また、稀に、双腕ロボット等の多関節ロボットの備えるすべての関節に対して、連動制御が適用されることもあった。

　しかしながら、ロボットの一部の関節のみに対して連動制御が適用されると、作業範囲が限定的となる等により、作業性が低下するおそれがあった。例えば、双腕ロボットにおいて腕部に係る関節のみに対して連動制御が適用されると、胴部の関節等との連携が不十分となり、その結果、作業範囲が限定的となる等して、作業性が低下するおそれがあった。

　一方、ロボットのすべての関節に対して連動制御が適用されると、一部の関節のみを使用するタスク等においては、その他の関節に関する制御が冗長となるおそれがあった。例えば、双腕ロボットにおいて、片手でドアを開ける動作のような一部の関節のみを使用するタスクを実行する場合には、開閉に使用しない腕に対する制御が冗長となるおそれがあった。

　本発明は上述の技術的背景に鑑みてなされたものであり、その目的は、関節に対して連動制御を適用する場合に、作業性の低下又は冗長な制御を防止することが可能なロボット等を提供することにある。

　上述の技術的課題は、以下の構成を有するロボット等により解決することができる。

　すなわち、本発明に係るロボットは、複数の関節を備えたロボットであって、切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定部と、前記対象関節を連動制御する、制御部と、を備えている。

　このような構成によれば、連動制御の対象となる関節を切り替えることができるので、作業を行うロボットにおける冗長な制御や作業性の低下を防止することができる。

　前記連動制御は、カルテシアン座標系における目標点への指令に基づく制御であってもよい。

　このような構成によれば、目標点への指令から逆運動学等を解くことにより各関節を連動して制御することができる。

　前記複数の関節のうち、前記対象関節以外の関節に対しては、関節座標系における指令が行われる、ものであってもよい。

　このような構成によれば、一部の関節については連動制御させ、その他の関節については、関節座標系における指令により個別制御を行うことができる。

　前記目標点は、ＴＣＰであってもよい。

　このような構成によれば、ＴＣＰ（ツール・センター・ポイント）を自在に制御することができる。

　前記目標点は、前記ロボットから離間した仮想点であってもよい。

　このような構成によれば、ロボットの持つ物体の軌道を設定したり、両手動作の生成の容易化等を実現することができる。

　前記ロボットは、胴部と前記胴部の左右から延びる一対の腕部とを含む、ものであってもよい。

　このような構成によれば、双腕ロボットにおいて、冗長な制御や作業性の低下を防止することができる。

　前記対象関節は、前記腕部の一方に係る関節と、前記胴部に係る関節を含む、ものであってもよい。

　このような構成によれば、腕部と胴部とを連動させる制御を行うことができる。

　前記対象関節は、前記腕部の両方に係る関節と、前記胴部に係る関節を含む、ものであってもよい。

　このような構成によれば、全身を連動させる制御を行うことができる。

　前記切替信号により設定されるモードは、前記腕部の一方に係る関節と、前記胴部に係る関節を、前記対象関節とする第１のモードと、前記腕部の両方に係る関節と、前記胴部に係る関節を、前記対象関節とする第２のモードを含む、ものであってもよい。

　このような構成によれば、腕部と胴部とを連動させる制御と、全身連動制御とを切り替えることができる。

　前記対象関節は、複数の関節群を含む、ものであってもよい。

　このような構成によれば、関節群毎にそれぞれ連動制御させることができる。

　前記指令は、位置指令、速度指令、電流指令、力指令、又は、モーメント指令のいずれか１つを含む、ものであってもよい。

　このような構成によれば、目標点に対して様々な指令を適用することができる。

　前記切替信号は、前記ロボットのオペレータによって設定される、ものであってもよい。

　このような構成によれば、オペレータは、タスク等に応じて連動制御させる関節を切り替えることができる。

　前記切替信号は、上位システム又は外部システムにより提供される、ものであってもよい。

　このような構成によれば、上位システム又は外部システムの動作に応じて連動制御させる関節を切り替えることができる。

　前記切替信号は、前記ロボットの動作に関する条件判定を行うことにより生成される、ものであってもよい。

　このような構成によれば、ロボット動作に応じて連動制御する関節を切り替えることができる。

　前記条件判定は、前記関節の可動範囲を超えた指令を検出したか否かを含む、ものであってもよい。

　このような構成によれば、ロボットの関節の可動範囲を超えた指令があったことを検出して、適応的に連動制御の対象となる関節を切り替えることができる。

　前記ロボットが動作しているか否かを検出する、動作検出部と、前記ロボットが動作していると判定される場合には、前記切替信号を無効化する、無効化処理部と、をさらに備えるものであってもよい。

　このような構成によれば、動作中はモードの切替を行うことができないので、安全性を向上させることができる。

　前記対象関節に対してヤコビ行列を算出する、ヤコビ行列算出部をさらに備え、前記制御部は、前記ヤコビ行列を利用して、前記対象関節を連動制御する、ものであってもよい。

　このような構成によれば、切り替えられた対象関節に対して算出されたヤコビ行列を利用して、対象関節を連動制御させることができる。

　本発明はシステムとして観念することもできる。本発明に係るシステムは、複数の関節を備えたロボットを含むシステムであって、切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定部と、前記対象関節を連動制御する、制御部と、を備えている。

　本発明は方法として観念することもできる。本発明に係る方法は、複数の関節を備えたロボットの制御方法であって、切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定ステップと、前記対象関節を連動制御する、制御ステップと、を備えている。

　本発明はコンピュータプログラムとして観念することもできる。すなわち、本発明に係るプログラムは、複数の関節を備えたロボットの制御プログラムであって、切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定ステップと、前記対象関節を連動制御する、制御ステップと、を備えている。

　本発明によれば、連動制御の対象となる関節を切り替えることができるので、作業を行うロボットにおける冗長な制御や作業性の低下を防止することができる。

図１は、ロボットシステムの全体構成図である。図２は、双腕ロボットの関節配置に関する説明図である。図３は、ロボットシステムの機能ブロック図である。図４は、ロボットシステムの動作に関するゼネラルフローチャートである。図５は、第１のモードに関する詳細フローチャートである。図６は、第１のモードに係るキネマティックチェーンの概念図である。図７は、第３のモードに関する詳細フローチャートである。図８は、第３のモードに係るキネマティックチェーンの概念図である。図９は、第４のモードに関する詳細フローチャートである。図１０は、第４のモードに係るキネマティックチェーンの概念図である。図１１は、仮想点への指令に関する説明図である。

　以下、本発明の好適な実施の形態について添付の図面を参照しつつ詳細に説明する。

　（１．第１の実施形態）
第１の実施形態として、本発明を双腕ロボット１を含むロボットシステム３００に対して適用した例について説明する。なお、本発明の適用対象は、双腕ロボット１又はそれを含むシステムに限定されない。従って、多関節ロボット等の他のロボット又はそれを含むシステムに対しても適用することができる。

　（１．１　ロボットシステムの構成）
図１は、本実施形態に係るロボットシステム３００の全体構成図である。同図から明らかな通り、本実施形態において、双腕ロボット１は、情報処理装置５と有線又は無線により接続されてロボットシステム３００を構成している。

　なお、本実施形態においては、双腕ロボット１と情報処理装置５とから成る構成について説明するものの、本発明はこのような構成に限定されない。従って、例えば、情報処理装置５の機能を双腕ロボット１内に配置してもよいし、情報処理装置５の機能をさらに分割して複数の装置としてもよい。また、情報処理装置５にさらに上位システム又は外部システム等を設けて双腕ロボット１を制御してもよい。

　図２は、双腕ロボット１の関節配置に関する説明図である。同図から明らかな通り、同図中央には、接地面から鉛直上方に延びる胴部１１が配置されている。胴部１１の上端には、首部１８を介して頭部１９が連結されている。また、胴部１１右側面の上端には、右腕部１２が連結され、一方、胴部１１左側面の上端には、左腕部１４が連結されている。

　頭部１９には、目の位置に一対のカメラ、額の位置に距離センサが備えられ、周囲の環境を認識可能に構成されている。首部１８には、頭部１９を左右又は上下に向けるように２つの自由度が設けられている。

　胴部１１には、胴部１１の上部（上半身）を左右に向けるよう旋回させる胴部第１回関節（Ｔ＿Ｊ１）と、胴部１１の上部を前傾又は後傾させる胴部第２関節（Ｔ＿Ｊ２）が設けられている。

　右腕部１２は、７自由度を備えている。すなわち、右腕部１２の胴部１１への付け根から側方に水平に延びた軸周りに右腕部１２を回動させる右腕部第１回関節（Ｒ＿Ｊ１）と、右腕部１２の肩の中心付近から前方へと延びる軸周りに回動させる右腕部第２関節（Ｒ＿Ｊ２）と、右腕部第２関節の直下の上腕部にあり肘以下を長手方向の中心軸周りに回動させる右腕部第３関節（Ｒ＿Ｊ３）と、を備えている。

　また、右腕部１２は、右腕部１２の肘部の中央付近から前方へと延びる軸周りに前腕部を回動させる右腕部第４関節部（Ｒ＿Ｊ４）と、右腕部第４関節の直下の前腕部にあり手先を長手方向の中心軸周りに回動させる右腕部第５関節（Ｒ＿Ｊ５）と、右腕部第５関節から手先位置寄りに配置され長手方向に直交する軸周りに回動させる右腕部第６関節（Ｒ＿Ｊ６）と、手先中央付近から前方へと延びる軸周りに回動させる第７関節（Ｒ＿Ｊ７）と、を備えている。

　左腕部１４は、右腕部１２と左右対称に構成され、同様に７つの自由度を備えている。

　図３は、ロボットシステム３００の機能ブロック図である。同図から明らかな通り、情報処理装置５は、制御部５０１と、記憶部５０２と、入力受付部５０３と、通信部５０６と、表示部５０７を備えている。

　制御部５０１は、ＣＰＵ等の演算処理部であり、読み込まれたプログラムを実行する。記憶部５０２は、ＲＯＭ／ＲＡＭ、ハードディスク、フラッシュメモリ等の記憶装置であり、プログラムや後述の種々のデータ、例えば、各モードの動作に必要な関節情報、設定情報を記憶する。入力受付部５０３は、オペレータ、管理者又はユーザ等による入力を受け付ける処理を行う。通信部５０６は、通信ユニットであり、双腕ロボット１との通信を行う。表示部５０７は、図示しないディスプレイ等に画像情報を表示する処理を行う。

　双腕ロボット１は、記憶部１０１、制御部１０２、通信部１０３を備えると共に、双腕ロボット１の各関節を駆動する複数の関節駆動部１０５（Ｔ＿Ｊ１、Ｔ＿Ｊ２、Ｌ＿Ｊ１～Ｌ＿Ｊ７、Ｒ＿Ｊ１～Ｒ＿Ｊ７）を備えている。また、図示しないものの、双腕ロボット１の各関節には、各関節の角度、角速度、角加速度、電流値及びトルク値等の各種の情報を取得するセンサが設けられ、それらのセンサからの情報は制御部１０２へと提供される。

　記憶部１０１と制御部１０２は、マイコンを構成し、記憶部１０１は、ＲＯＭ／ＲＡＭ等の記憶装置であり、制御部１０２は、ＣＰＵ等の演算処理装置である。通信部１０３は、情報処理装置５の通信部５０６との間で情報の授受を行う通信ユニットである。各関節駆動部１０５は、制御部１０２によって制御され、各関節を駆動する。

　（１．２　ロボットシステムの動作）
図４は、ロボットシステム３００の動作に関するゼネラルフローチャートである。同図から明らかな通り、処理が開始すると、制御部５０１は、表示部５０７を介して、モードを受け付ける表示を行う処理を実行する（Ｓ１）。このモード受付表示には、オペレータ等に対していずれかのモードを選択させる表示が含まれる。その後、入力受付部５０３を介して入力されたモード選択信号（又はモード切替信号）を受信するまで、情報処理装置５は待機状態となる（Ｓ２ＮＯ）。

　この状態において、入力受付部５０３を介して、モード選択信号が入力されると（Ｓ２ＹＥＳ）、各モードの開始動作が実行される。すなわち、検出されたモード選択信号に応じて対応するモードを開始する処理を行う（Ｓ３）。

　具体的には、制御部５０１は、第１のモードを選択する信号が検出された場合には、第１のモード開始処理を実行する（Ｓ５）。第２のモードを選択する信号が検出された場合には、第２のモード開始処理を実行する（Ｓ６）。第３のモードを選択する信号が検出された場合には、第３のモード開始処理を実行する（Ｓ７）。第４のモードを選択する信号が検出された場合には、第４のモード開始処理を実行する（Ｓ９）。これらのモード開始処理により、後述の各処理が実行される。

　モード開始処理の後、制御部５０１は、再び、一連の動作を繰り返す（Ｓ１～Ｓ９）。

　（１．２．１　第１のモード）
図５は、第１のモード開始処理（Ｓ５）により実行される第１のモードに関する詳細フローチャートである。

　同図から明らかな通り、処理が開始すると、制御部５０１は、記憶部５０２から第１のモードに関する関節情報を読み出す処理を行う（Ｓ５０１）。関節情報とはどの関節に対してどのような制御を適用するかを表す情報である。

　より詳細には、第１のモードにおいては、右腕部関節（Ｒ＿Ｊ１～Ｒ＿Ｊ７）と胴部関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）をカルテシアン座標系において連動制御する関節群とし、対応する関節変数ベクトル（ｑ）が読み出される。また、左腕部関節（Ｌ＿Ｊ１～Ｌ＿Ｊ７）については、関節座標系において各関節に所望の指令値を与えて位置制御を行うものとし、対応する関節変数ベクトル（ｑ）が読み出される。

　関節情報の読出処理の後、制御部５０１は、記憶部５０２から第１のモードに対応する設定情報を読み出す処理を実行する（Ｓ５０２）。この設定情報には、双腕ロボット１の各部のリンク長やリンク間の関係等を表すリンクパラメータ（Ｌ）等の双腕ロボット１のハードウェア情報、原点に関する情報（Ｏ、О_Ｌ'）及び制御対象となる目標点である左右のＴＣＰ（Ｔｏｏｌ　Ｃｅｎｔｅｒ　Ｐｏｉｎｔ）に関する情報（ＴＣＰ（Ｌ）、ＴＣＰ（Ｒ））が含まれる。なお、本実施形態において、原点Ｏは、胴部１１の中央の接地面に設定され、ＴＣＰ（Ｒ）は右腕部１２の手先付近に設定される。また、原点О_Ｌ'は、左腕部１４の付け根に設定され、ＴＣＰ（Ｌ）は左腕部１４の手先付近に設定される。

　なお、関節情報と設定情報をそれぞれ読み出すことで、連動制御を行う関節群に対するキネマティックチェーン（Ｋｉｎｅｍａｔｉｃ　Ｃｈａｉｎ）を特定することができる。本実施形態において、キネマティックチェーンとは、原点ＯからＴＣＰに至るまでに存在する連動する一連の関節を言う。このキネマティックチェーンは、関節変数ベクトル（ｑ）とリンクパラメータ（Ｌ）を特定することにより、具体的に特定される。

　図６は、第１のモードに係るキネマティックチェーンの概念図である。同図においては、原点Ｏから、胴部関節（Ｔ＿Ｊ１→Ｔ＿Ｊ２）、右腕部関節（Ｒ＿Ｊ１→Ｒ＿Ｊ２→・・・→Ｒ＿Ｊ７）を順に経て、ＴＣＰ（Ｒ）へと至る矢印が示されている。この矢印に沿って、原点ＯからＴＣＰ（Ｒ）に至るまでに存在する一連の関節が胴部１１及び右腕部１２の連動制御に用いられるキネマティックチェーンとして特定される。

　また、同図においては、原点Ｏから原点О_Ｌ'、原点О_Ｌ'から左腕部関節（Ｌ＿Ｊ１→Ｌ＿Ｊ２→・・・→Ｌ＿Ｊ７）を順に経て、ＴＣＰ（Ｌ）へと至る矢印が示されている。この矢印に沿って、原点ＯからＴＣＰ（Ｌ）に至るまでに存在する一連の関節が左腕部１４の制御に用いられるキネマティックチェーンとして特定される。なお、この左腕部１４に係るキネマティックチェーンは、重力補償等のダイナミクス計算に用いられる。

　図５に戻り、設定情報の読出処理の後、制御部５０１は、双腕ロボット１の各関節に関する現在値の取得処理を行う（Ｓ５０３）。関節に関する現在値とは、双腕ロボット１に備えられた所定のセンサにより取得される各関節の角度、角速度、角加速度、電流値、トルク値等の現在値である。

　現在値の読出処理の後、胴部１１及び右腕部１２に関する制御と、左腕部１４に関する制御が並列的に実行される。

　胴部１１及び右腕部１２に関する制御として、制御部５０１は、順運動学に係る関数の算出処理を行う（Ｓ５０５）。すなわち、当業者に広く知られるように、関節変数ベクトル（ｑ）とリンクパラメータ（Ｌ）に基づいて、各関節角度からＴＣＰ（Ｒ）の位置及び姿勢を算出する非線形ベクトル関数ｆ（ｑ，Ｌ）を算出する。

　順運動学に係る関数の算出処理の後、制御部５０１は、順運動学に係る関数に基づいてヤコビ行列Ｊの算出処理を行う（Ｓ５０６）。具体的には非線形ベクトル関数ｆ（ｑ，Ｌ）を関節変数ベクトルｑで偏微分することによってヤコビ行列Ｊ（＝∂ｆ（ｑ，Ｌ）／∂ｑ）を算出する。

　ヤコビ行列の算出処理の後、制御部５０１は、ＴＣＰ（Ｒ）への指令値の取得処理を行う（Ｓ５０７）。本実施形態においては、指令値は、記憶部５０２から読み出すことにより取得する。なお、指令値は、本実施形態に示されるものに限定されず、例えば、カメラ等の各種のセンサ情報に応じて指令値を生成する等してもよい。

　指令値の取得後、情報処理装置５と双腕ロボット１は連携して、ＴＣＰ（Ｒ）に対するカルテシアン座標系での指令値と、ヤコビ行列に基づき、逆運動学を解くことにより、胴部関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）と右腕部関節（Ｒ＿Ｊ１、Ｒ＿Ｊ２、・・・、Ｒ＿Ｊ７）の連動制御処理、例えば、各関節の位置制御を行う（Ｓ５０８）。その後、処理は、終了判定処理（Ｓ５１０）へと進む。

　一方、関節に関する現在値の取得処理（Ｓ５０３）の後、情報処理装置５と双腕ロボット１は連携して、左腕部１４に対する制御を行う。まず、制御部５０１は、左腕部１４に関して、原点ＯからＴＣＰ（Ｌ）に至るキネマティックチェーンについて順運動学に係る関数の算出処理を行う（Ｓ５１１）。すなわち、当業者に広く知られるように、関節変数ベクトル（ｑ）とリンクパラメータ（Ｌ）に基づいて、各関節角度からＴＣＰ（Ｌ）の位置及び姿勢を算出する非線形ベクトル関数ｆ（ｑ，Ｌ）を算出する。

　順運動学に係る関数の算出処理の後、制御部５０１は、順運動学に係る関数に基づいてヤコビ行列Ｊの算出処理を行う（Ｓ５１２）。具体的には非線形ベクトル関数ｆ（ｑ，Ｌ）を関節変数ベクトルｑで偏微分することによってヤコビ行列Ｊ（＝∂ｆ（ｑ，Ｌ）／∂ｑ）を算出する。

　ヤコビ行列の算出処理の後、制御部５０１は、左腕部関節に対する指令値の取得処理を行う（Ｓ５１３）。本実施形態においては、指令値は、記憶部５０２から読み出すことにより取得する。なお、指令値は、本実施形態に示されるものに限定されず、例えば、カメラ等の各種のセンサ情報に応じて指令値を生成する等してもよい。

　指令値の取得後、情報処理装置５と双腕ロボット１は連携して、各関節に対する関節座標系での指令値に基づいて所望の位置制御を行う。例えば、左腕部１４の各関節を保持するような制御を行ってもよい。

　なお、本実施形態においては、左腕部関節に対して関節座標系における位置指令を与えたものの、本発明はこのような構成に限定されず、他の態様で指令を行ってもよい。従って、例えば、関節座標系において、速度指令、電流指令、モーメント指令を行ってもよい。

　また、本実施形態においては、ＴＣＰ（Ｒ）に対してカルテシアン座標系において位置指令を行ったものの、本発明はこのような構成に限定されず、他の態様で指令を行ってもよい。従って、例えば、ＴＣＰに対してカルテシアン座標系における速度指令、電流指令、力指令、又は、モーメント指令を行ってもよい。また、ＴＣＰ以外の点、例えば、任意の手先位置等を目標点としてもよい。

　さらに、本実施形態においては、連動制御にあたって、逆運動学計算を行うものとしたが、本発明はそのような構成に限定されない。従って、逆運動学の計算を行わずに、例えば、インピーダンス制御等を行ってもよい。また、本実施形態においては、ヤコビ行列を用いるものとしたが、本発明はそのような構成に限定されず、ヤコビ行列を利用することなく制御計算を行ってもよい。

　各関節に対する制御処理は、所定の終了信号を受信するまで繰り返し実行される（Ｓ５１０ＮＯ）。所定の終了信号を受信した場合（Ｓ５１０ＹＥＳ）、処理は終了する。

　このような構成によれば、胴部１１と右腕部１２のみを連動させることができるので、右腕部１２のみを連動制御するよりも広い作業範囲を確保することができ、双腕ロボット１の作業性を向上させることができる。また、左腕部１４については連動させず、関節座標系で所望の制御を行うので、制御の冗長性を抑制することができる。

　（１．２．２　第２のモード）
第２のモード開始処理（Ｓ６）により第２のモードが実行される。

　第２のモードは、第１のモードとは左右対称に、左腕部関節（Ｌ＿Ｊ１～Ｌ＿Ｊ７）と胴部関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）とを連動制御の対象としてカルテシアン座標系において指令を行い、一方、右腕部関節については関節座標系で所望の制御を行うものである。従って、第１のモードとは制御が左右対称である点において相違するのみであって、動作は同様であるので、詳細な説明は省略する。

　このような構成によれば、胴部１１と左腕部１４のみを連動させることができるので、左腕部１４のみを連動制御するよりも広い作業範囲を確保することができ、双腕ロボット１の作業性を向上させることができる。また、右腕部１２については連動させず、関節座標系で所望の制御を行うのみであるので、制御の冗長性を抑制することができる。

　（１．２．３　第３のモード）
図７は、第３のモード開始処理（Ｓ７）により実行される第３のモードに関する詳細フローチャートである。

　同図から明らかな通り、処理が開始すると、制御部５０１は、記憶部５０２から第３のモードに対応する関節情報を読み出す処理を行う（Ｓ７０１）。より詳細には、本実施形態においては、右腕部関節（Ｒ＿Ｊ１～Ｒ＿Ｊ７）と胴部関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）と左腕部関節（Ｌ＿Ｊ１～Ｌ＿Ｊ７）、すなわち、全身の関節をまとめて連動制御の対象として、それに対応する関節変数ベクトル（ｑ）を読み出す。

　関節情報の読み出しの後、第３のモードに対応する設定情報の読出処理が行われる（Ｓ７０２）。この設定情報には、双腕ロボット１の各部のリンク長やリンク間の関係等を表すリンクパラメータ（Ｌ）等の双腕ロボット１のハードウェア情報、及び、原点Ｏ及び制御対象となる目標点であるＴＣＰ（Ｌ）、ＴＣＰ（Ｒ）に関する情報が含まれる。なお、本実施形態において、原点Ｏは、胴部１１の中央の接地面に配置される。また、ＴＣＰは各腕部１２、１４の手先付近に設定される。

　なお、関節情報と設定情報をそれぞれ読み出すことで、連動制御を行う関節群に対するキネマティックチェーンを特定することができる。

　図８は、第３のモードに係るキネマティックチェーンの概念図である。同図においては、原点Ｏから、胴部関節（Ｔ＿Ｊ１→Ｔ＿Ｊ２）、右腕部関節（Ｒ＿Ｊ１→Ｒ＿Ｊ２→・・・→Ｒ＿Ｊ７）を順に経て、右腕部１２のＴＣＰ（Ｒ）へと至る矢印と、原点Ｏから、胴部関節（Ｔ＿Ｊ１→Ｔ＿Ｊ２）、左腕部関節（Ｌ＿Ｊ１→Ｌ＿Ｊ２→・・・→Ｌ＿Ｊ７）を順に経て、左腕部１４のＴＣＰ（Ｌ）へと至る矢印が示されている。これらの矢印に沿って、原点Ｏから左右のＴＣＰ（Ｒ）、ＴＣＰ（Ｌ）に至るまでに存在する連動する一連の関節が本実施形態において特定されるキネマティックチェーンである。

　図７に戻り、設定情報の読出処理の後、制御部５０１は、各関節に関する現在値の取得処理を行う（Ｓ７０３）。関節に関する現在値とは、双腕ロボット１に備えられた所定のセンサにより取得される各関節の角度、角速度、角加速度、電流値、トルク値等の現在値である。

　各関節に関する現在値の取得処理の後、制御部５０１は、順運動学に係る関数の算出処理を行う（Ｓ７０４）。すなわち、当業者に広く知られるように、関節変数ベクトル（ｑ）とリンクパラメータ（Ｌ）に基づいて、各関節角度から各ＴＣＰの位置及び姿勢を算出する非線形ベクトル関数ｆ（ｑ，Ｌ）を算出する。

　順運動学に係る関数の算出処理の後、制御部５０１は、順運動学に係る関数に基づいてヤコビ行列Ｊの算出処理を行う（Ｓ７０５）。具体的には非線形ベクトル関数ｆ（ｑ，Ｌ）を関節変数ベクトルｑで偏微分することによってヤコビ行列Ｊ（＝∂ｆ（ｑ，Ｌ）／∂ｑ）を算出する。

　ヤコビ行列の算出処理の後、制御部５０１は、ＴＣＰ（Ｒ）及びＴＣＰ（Ｌ）への指令値の取得処理を行う（Ｓ７０７）。本実施形態においては、指令値は、記憶部５０２から読み出すことにより取得する。なお、指令値は、本実施形態に示されるものに限定されず、例えば、カメラ等の各種のセンサ情報に応じて指令値を生成する等してもよい。

　指令値の取得処理の後、情報処理装置５と双腕ロボット１は連携し、算出したヤコビ行列Jを利用して逆運動学を解くことにより、右腕部関節（Ｒ＿Ｊ１～Ｒ＿Ｊ７）、左腕部関節（Ｌ＿Ｊ１～Ｌ＿Ｊ７）及び胴部関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）の連動制御を行う（Ｓ７０８）。具体的には、右腕部１２に係るＴＣＰ（Ｒ）と、左腕部１４に係るＴＣＰ（Ｌ）に対してカルテシアン座標系における所望の位置指令を行い各関節の制御を行う。

　なお、本実施形態においては、各ＴＣＰに対してカルテシアン座標系における位置指令を行ったものの、本発明はこのような構成に限定されず、他の態様で指令を行ってもよい。従って、例えば、各ＴＣＰに対してカルテシアン座標系において速度指令、電流指令、力指令、又は、モーメント指令を行ってもよい。また、ＴＣＰ以外の点、例えば、任意の手先位置等を目標点としてもよい。

　各関節に対する制御処理（Ｓ７０３～Ｓ７０８）は、所定の終了信号を受信するまで繰り返し実行される（Ｓ７０９ＮＯ）。一方、所定の終了信号を受信した場合（Ｓ７０９ＹＥＳ）、処理は終了する。

　このような構成によれば、全身の関節を連動させて制御させることができるので、双腕ロボット１の作業性を向上させることができる。

　（１．２．４　第４のモード）
図９は、第４のモード開始処理（Ｓ９）により実行される第４のモードに関する詳細フローチャートである。

　同図から明らかな通り、処理が開始すると、制御部５０１は、記憶部５０２から第４のモードに対応する関節情報を読み出す処理を行う（Ｓ９０１）。より詳細には、第４のモードにおいては、複数の関節群、すなわち、右腕部関節（Ｒ＿Ｊ１～Ｒ＿Ｊ７）と、左腕部関節（Ｌ＿Ｊ１～Ｌ＿Ｊ７）が、それぞれ、連動制御の対象関節群として特定され、それに対応する関節変数ベクトル（ｑ）が読み出される。また、胴部関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）については、関節座標系において所望の位置制御等を行うものとして、対応する関節変数ベクトル（ｑ）が読み出される。

　関節情報の読み出しの後、第４のモードに対応する設定情報の読出処理が行われる（Ｓ９０２）。この設定情報には、双腕ロボット１の各部のリンク長やリンク間の関係等を表すリンクパラメータ（Ｌ）等の双腕ロボット１のハードウェア情報、及び、原点Ｏ、左右の腕部用の原点Ｏ_Ｌ'、Ｏ_Ｒ'及び制御対象となる目標点である左右のＴＣＰ（Ｌ）、ＴＣＰ（Ｒ）に関する情報が含まれる。なお、本実施形態において、原点Ｏは、胴部１１中央の接地面に配置される。また、左右の腕部用の原点Ｏ_Ｌ'、Ｏ_Ｒ'は、それぞれ左右の腕部１２、１４の付け根近傍に配置される。さらに、左右のＴＣＰ（Ｒ）、ＴＣＰ（Ｌ）は各腕部１２、１４の手先付近に設定される。

　図９に戻り、設定情報の読出処理の後、制御部５０１は、各関節に関する現在値の取得処理を行う（Ｓ９０３）。関節に関する現在値とは、双腕ロボット１に備えられた所定のセンサにより取得される各関節の角度、角速度、角加速度、電流値、トルク値等の現在値である。

　関節に関する現在値の取得処理の後、制御部５０１は、胴部関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）に関して、順運動学に係る関数の算出処理を行う（Ｓ９０４）。すなわち、当業者に広く知られるように、関節変数ベクトル（ｑ）とリンクパラメータ（Ｌ）に基づいて、各関節角度から各原点Ｏ_Ｌ'、Ｏ_Ｒ'の位置及び姿勢を算出する非線形ベクトル関数ｆ（ｑ，Ｌ）を算出する。

　順運動学に係る関数の算出処理の後、制御部５０１は、算出された順運動学に係る関数に基づいてヤコビ行列Ｊの算出処理を行う（Ｓ９０５）。具体的には非線形ベクトル関数ｆ（ｑ，Ｌ）を関節変数ベクトルｑで偏微分することによってヤコビ行列Ｊ（＝∂ｆ（ｑ，Ｌ）／∂ｑ）を算出する。

　胴部関節に関する順運動学計算、ヤコビ行列計算の後、右腕部１２に関する制御、左腕部１４に関する制御、及び、胴部１１に関する制御が並列的に実行される。

　右腕部１２に関する制御として、制御部５０１は、右腕部原点Ｏ_Ｒ'からＴＣＰ（Ｒ）へと至るキネマティックチェーンについて、順運動学に係る関数の算出処理を行う（Ｓ９０６）。すなわち、関節変数ベクトル（ｑ）とリンクパラメータ（Ｌ）に基づいて、各関節角度からＴＣＰ（Ｒ）の位置及び姿勢を算出する非線形ベクトル関数ｆ（ｑ，Ｌ）を算出する。

　順運動学に係る関数の算出処理の後、制御部５０１は、算出された順運動学に係る関数に基づいてヤコビ行列Ｊの算出処理を行う（Ｓ９０７）。具体的には非線形ベクトル関数ｆ（ｑ，Ｌ）を関節変数ベクトルｑで偏微分することによってヤコビ行列Ｊ（＝∂ｆ（ｑ，Ｌ）／∂ｑ）を算出する。

　ヤコビ行列の算出処理の後、制御部５０１は、ＴＣＰ（Ｒ）への指令値の取得処理を行う（Ｓ９０８）。本実施形態においては、指令値は、記憶部５０２から読み出すことにより取得する。なお、指令値は、本実施形態に示されるものに限定されず、例えば、カメラ等の各種のセンサ情報に応じて指令値を生成する等してもよい。

　指令値の取得処理の後、情報処理装置５と双腕ロボット１は連携し、算出したヤコビ行列Jを利用して逆運動学を解くことにより、右腕部関節（Ｒ＿Ｊ１～Ｒ＿Ｊ７）の連動制御を行う（Ｓ９０９）。具体的には、ＴＣＰ（Ｒ）に対してカルテシアン座標系における所望の位置指令を行い連動制御を行う。

　なお、本実施形態においては、ＴＣＰ（Ｒ）に対してカルテシアン座標系における位置指令を行ったものの、本発明はこのような構成に限定されず、他の態様で指令を行ってもよい。従って、例えば、ＴＣＰ（Ｒ）に対してカルテシアン座標系において速度指令、電流指令、力指令、又は、モーメント指令を行ってもよい。また、ＴＣＰ（Ｒ）以外の点、例えば、任意の手先位置等を目標点としてもよい。

　右腕部関節の連動制御処理は、所定の終了信号を受信するまで繰り返し実行される（Ｓ９１０ＮＯ）。一方、所定の終了信号を受信した場合（Ｓ９１０ＹＥＳ）、処理は終了する。

　また、左腕部１４に関する制御として、制御部５０１は、左腕部原点Ｏ_Ｌ'からＴＣＰ（Ｌ）へと至るキネマティックチェーンについて、順運動学に係る関数の算出処理を行う（Ｓ９１１）。すなわち、関節変数ベクトル（ｑ）とリンクパラメータ（Ｌ）に基づいて、各関節角度からＴＣＰ（Ｌ）の位置及び姿勢を算出する非線形ベクトル関数ｆ（ｑ，Ｌ）を算出する。

　順運動学に係る関数の算出処理の後、制御部５０１は、算出された順運動学に係る関数に基づいてヤコビ行列Ｊの算出処理を行う（Ｓ９１２）。具体的には非線形ベクトル関数ｆ（ｑ，Ｌ）を関節変数ベクトルｑで偏微分することによってヤコビ行列Ｊ（＝∂ｆ（ｑ，Ｌ）／∂ｑ）を算出する。

　ヤコビ行列の算出処理の後、制御部５０１は、ＴＣＰ（Ｌ）への指令値の取得処理を行う（Ｓ９１３）。本実施形態においては、指令値は、記憶部５０２から読み出すことにより取得する。なお、指令値は、本実施形態に示されるものに限定されず、例えば、カメラ等の各種のセンサ情報に応じて指令値を生成する等してもよい。

　指令値の取得処理の後、情報処理装置５と双腕ロボット１が連携し、算出したヤコビ行列Jを利用して逆運動学を解くことにより、左腕部関節（Ｌ＿Ｊ１～Ｌ＿Ｊ７）の連動制御を行う（Ｓ９１５）。具体的には、ＴＣＰ（Ｌ）に対してカルテシアン座標系において所望の位置指令を行い制御を行う。

　なお、本実施形態においては、ＴＣＰ（Ｌ）に対してカルテシアン座標系における位置指令を行ったものの、本発明はこのような構成に限定されず、他の態様で指令を行ってもよい。従って、例えば、ＴＣＰ（Ｌ）に対してカルテシアン座標系における速度指令、電流指令、力指令、又は、モーメント指令を行ってもよい。また、ＴＣＰ（Ｌ）以外の点、例えば、任意の手先位置等を目標点としてもよい。

　左腕部関節の連動制御処理は、所定の終了信号を受信するまで繰り返し実行される（Ｓ９１６ＮＯ）。一方、所定の終了信号を受信した場合（Ｓ９１６ＹＥＳ）、処理は終了する。

　さらに、胴部１１に関する制御として、制御部５０１は、胴部１１の各関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）への関節座標系における指令値の取得処理を行う（Ｓ９２１）。本実施形態においては、指令値は、記憶部５０２から読み出すことにより取得する。なお、指令値は、本実施形態に示されるものに限定されず、例えば、カメラ等の各種のセンサ情報に応じて指令値を生成する等してもよい。

　指令値の取得処理の後、制御部５０１は、関節座標系における指令値に基づいて、胴部１１の各関節（Ｔ＿Ｊ１、Ｔ＿Ｊ２）について所望の位置制御を行う（Ｓ９２２）。

　胴部１１の制御は、所定の終了信号を受信するまで繰り返し実行される（Ｓ９２３ＮＯ）。一方、所定の終了信号を受信した場合（Ｓ９２３ＹＥＳ）、処理は終了する。

　なお、本実施形態においては、胴部関節に対して関節座標系における位置指令を与えたものの、本発明はこのような構成に限定されず、他の態様で指令を行ってもよい。従って、例えば、関節座標系において、速度指令、電流指令、又はモーメント指令を行ってもよい。

　図１０は、第４のモードに係るキネマティックチェーンの概念図である。第４のモードに係るキネマティックチェーンの特定にあたっては、まず、原点Оから矢印に沿って胴部関節（Ｔ＿Ｊ１→Ｔ＿Ｊ２）を経て左右の腕部の付け根にある左右の腕部原点О_Ｒ'、Ｏ_Ｌ'までのキネマティックチェーンが特定される。

　その後、右腕部１２のＴＣＰ（Ｒ）に向けては、右腕部原点Ｏ_Ｒ'から矢印に沿って右腕部関節（Ｒ＿Ｊ１→Ｒ＿Ｊ２→・・・→Ｒ＿Ｊ７）を順に経て、右腕部１２のＴＣＰ（Ｒ）へと至る経路上に存在する一連の関節が、右腕部１２の連動制御用のキネマティックチェーンとなる。

　一方、左腕部１４のＴＣＰ（Ｌ）に向けては、左腕部原点Ｏ_Ｌ'から矢印に沿って左腕部関節（Ｌ＿Ｊ１→Ｌ＿Ｊ２→・・・→Ｌ＿Ｊ７）を順に経て、左腕部１４のＴＣＰ（Ｌ）へと至る経路上に存在する一連の関節が、左腕部１４の連動制御用のキネマティックチェーンとなる。

　このような構成によれば、右腕部１２、左腕部１４及び胴部１１について独立に制御しつつ、所望の関節、すなわち、右腕部関節（Ｒ＿Ｊ１～Ｒ＿Ｊ７）、及び、左腕部関節（Ｌ＿Ｊ１～Ｌ＿Ｊ７）については連動制御を適用することができる。これにより、双腕ロボット１の作業性を向上させることができる。

　以上の構成によれば、モードを切り替えることにより連動制御の対象となる関節を変更することができるので、ロボットにおける冗長な制御や作業性の低下を回避することができる。

　（２．変形例）
本発明は、様々に変形して実施することができる。

　上述の実施形態においては、指令を行う目標点としてＴＣＰを採用したが、本発明はそのような構成に限定されない。従って、例えば、ＴＣＰ以外の点をロボットの目標点としてもよく、また、ロボットの筐体外の仮想点を目標点として制御してもよい。

　図１１は、仮想点への指令に関する説明図である。同図は、第１の実施形態に記載の双腕ロボット１が第３のモード、すなわち、全身連動モードにあるときに、直方体の荷物２１を持ち上げる場合を想定している。同図において、位置等の指令を行う目標点となる仮想点は、右腕部１２のＴＣＰ（Ｒ）と左腕部１４のＴＣＰ（Ｌ）の中間点（Ｖ）に配置されている。この中間点（Ｖ）に対して指令を行うことで全身連動制御を行うことができる。

　具体的には、仮想点に対する指令を検出した場合には、それを右腕部１２のＴＣＰ（Ｒ）と左腕部１４のＴＣＰ（Ｌ）の指令へと変換する。その後の処理は第１の実施形態で示したものと同一であるので詳細な説明は省略する。

　このように、ロボットの筐体外の仮想点を目標点とすることで、例えば、物体（オブジェクト）の軌道を設定したり、両手動作の生成の容易化等を実現することができる。

　なお、本変形例では、双腕ロボット１が第３のモードにあるものとして説明したが、他のモードで実行してもよい。

　上述の実施形態においては、タイミングに関する制限なく、第１～第４のモードを自在に切り替えられる構成としたが（図１参照）、本発明はそのような構成に限定されない。従って、例えば、ロボットに備えられた各種のセンサにより得られた情報からロボットが動作中であるか否かを検出する検出部を設け、動作中であると検出された場合には、モードの切り替えを無効化処理する無効化処理部、又は、視覚的又は聴覚的にオペレータ等に対して警告を行う警告処理部をさらに設けてもよい。

　このような構成によれば、モードの切り替えに伴う急激な動作等を防止することができる。

　上述の実施形態においては、切り替えは、入力受付部５０３への入力に基づいて行うものとして説明したが、本発明はそのような構成に限定されない。従って、入力受付部５０３からの入力信号以外にも、上位システムや外部システム等からの指令や、所定の条件判定に応じてモードを自動的に切り替えてもよい。また、直接モードの切り替えを行うのではなく、視覚的又は聴覚的に切り替えの提案を行ってもよい。

　例えば、双腕ロボット１の制御中において、双腕ロボット１の腕部の可動域を超えるような動作指令を検出した場合に、情報処理装置５において、胴部１１の連動を伴う第１～第３のモードへの切り替えを促す注意喚起のための視覚的又は聴覚的な警告を行ってもよい。また、警告と共に又は単独で、胴部１１の連動を伴う第１～第３のいずれかのモードへの切り替え処理を行ってもよい。

　このような構成によれば、ハードウェア的な制約により適切にアクセスできない物体に対して、モードを切り替えることにより連動させる関節を変更して、適切にアクセスすること等を実現できる。例えば、腕部のリーチ外の物体に対して胴部１１を連動させることで適切にアクセスすること等ができる。

　上述の実施形態においては、胴部１１、右腕部１２又は左腕部１４といった単位で制御単位としているものの、このような構成に限定されない。従って、例えば、各部分を跨ぐように制御単位を割り当ててもよいし、各部の一部の関節群に対して制御単位を割り当ててもよい。

　以上、本発明の実施形態について説明したが、上記実施形態は本発明の適用例の一部を示したに過ぎず、本発明の技術的範囲を上記実施形態の具体的構成に限定する趣旨ではない。また、上記の実施形態は、矛盾が生じない範囲で適宜組み合わせ可能である。

　本発明は、ロボット等を製造する産業において利用可能である。

　１　双腕ロボット
　１１　胴部
　１２　右腕部
　１４　左腕部
　１８　首部
　１９　頭部
　１０１　記憶部
　１０２　制御部
　１０３　通信部
　１０５　関節駆動部
　５　情報処理装置
　５０１　制御部
　５０２　記憶部
　５０３　入力受付部
　５０６　通信部
　５０７　表示部
　３００　ロボットシステム

Claims

　複数の関節を備えたロボットであって、
　切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定部と、
　前記対象関節を連動制御する、制御部と、
を備えた、ロボット。
　前記連動制御は、カルテシアン座標系における目標点への指令に基づく制御である、請求項１に記載のロボット。
　前記複数の関節のうち、前記対象関節以外の関節に対しては、関節座標系における指令が行われる、請求項２に記載のロボット。
　前記目標点は、ＴＣＰである、請求項２に記載のロボット。
　前記目標点は、前記ロボットから離間した仮想点である、請求項２に記載のロボット。
　前記ロボットは、胴部と前記胴部の左右から延びる一対の腕部とを含む、請求項１～５のいずれ１項に記載のロボット。
　前記対象関節は、前記腕部の一方に係る関節と、前記胴部に係る関節を含む、請求項６に記載のロボット。
　前記対象関節は、前記腕部の両方に係る関節と、前記胴部に係る関節を含む、請求項６に記載のロボット。
　前記切替信号により設定されるモードは、
　前記腕部の一方に係る関節と、前記胴部に係る関節を、前記対象関節とする第１のモードと、
　前記腕部の両方に係る関節と、前記胴部に係る関節を、前記対象関節とする第２のモードを含む、請求項６に記載のロボット。
　前記対象関節は、複数の関節群を含む、請求項１に記載のロボット。
　前記指令は、位置指令、速度指令、電流指令、力指令、又は、モーメント指令のいずれか１つを含む、請求項２又は３に記載のロボット。
　複数の関節を備えたロボットであって、
　切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定部と、
　前記対象関節を連動制御する、制御部と、を備えたロボットであって、
　前記連動制御は、カルテシアン座標系における目標点への第１の指令に基づく制御であり、
　前記複数の関節のうち、前記対象関節以外の関節に対しては、関節座標系における第２の指令が行われ、
　前記ロボットは、胴部と前記胴部の左右から延びる一対の腕部とを含み、
　前記対象関節は、一方の前記腕部に係る関節と、前記胴部に係る関節を含み、
　前記第１の指令は、位置指令、速度指令、電流指令、力指令、又は、モーメント指令のいずれか１つを含み、
　前記第２の指令は、位置指令、速度指令、電流指令、力指令、又は、モーメント指令のいずれか１つを含む、ロボット。
　前記切替信号は、前記ロボットのオペレータによって設定される、請求項１～１２のいずれか１項に記載のロボット。
　前記切替信号は、上位システム又は外部システムにより提供される、請求項１～１２のいずれか１項に記載のロボット。
　前記切替信号は、前記ロボットの動作に関する条件判定を行うことにより生成される、請求項１～１２のいずれか１項に記載のロボット。
　前記条件判定は、前記関節の可動範囲を超えた指令を検出したか否かを含む、請求項１５に記載のロボット。
　前記ロボットが動作しているか否かを検出する、動作検出部と、
　前記ロボットが動作していると判定される場合には、前記切替信号を無効化する、無効化処理部と、をさらに備える、請求項１～１６のいずれか１項に記載のロボット。
　前記対象関節に対してヤコビ行列を算出する、ヤコビ行列算出部をさらに備え、
　前記制御部は、前記ヤコビ行列を利用して、前記対象関節を連動制御する、請求項１～１７のいずれか１項に記載のロボット。
　複数の関節を備えたロボットを含むシステムであって、
　切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定部と、
　前記対象関節を連動制御する、制御部と、
を備えた、システム。
　複数の関節を備えたロボットの制御方法であって、
　切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定ステップと、
　前記対象関節を連動制御する、制御ステップと、
を備えた、制御方法。
　複数の関節を備えたロボットの制御プログラムであって、
　切替信号に応じて、前記複数の関節のうちの一部又は全ての関節を連動制御の対象関節として設定する、設定ステップと、
　前記対象関節を連動制御する、制御ステップと、
を備えた、プログラム。