特許 5829968 多関節ロボット、その関節協調制御装置および方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5829968

(24)【登録日】2015年10月30日

(45)【発行日】2015年12月9日

(54)【発明の名称】多関節ロボット、その関節協調制御装置および方法

(51)【国際特許分類】

   B25J 9/10 20060101AFI20151119BHJP

【ＦＩ】

   B25J9/10 A

【請求項の数】6

【全頁数】31

(21)【出願番号】特願2012-87028(P2012-87028)

(22)【出願日】2012年4月6日

(65)【公開番号】特開2013-215827(P2013-215827A)

(43)【公開日】2013年10月24日

【審査請求日】2014年5月12日

(73)【特許権者】

【識別番号】000003207

【氏名又は名称】トヨタ自動車株式会社

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(74)【代理人】

【識別番号】100103894

【弁理士】

【氏名又は名称】家入 健

(72)【発明者】

【氏名】森平 智久

(72)【発明者】

【氏名】但馬 竜介

【審査官】 鈴木 崇文

(56)【参考文献】

【文献】 特開２００９−０５３９２６（ＪＰ，Ａ）

【文献】 特開平０７−０５６６１７（ＪＰ，Ａ）

【文献】 特開２００８−２１４１０８（ＪＰ，Ａ）

【文献】 特開平０２−１８１２０１（ＪＰ，Ａ）

【文献】 Ni Yanbing ; Luo Fayang ; Zhong Xueyong ; Guo Lei，Trajectory Planning of a Parallel Robot for Wafer Transfer，Proc. of 2010 International Conference on E-Product E-Service and E-Entertainment (ICEEE)，２０１０年１１月，pp.1 - 4

【文献】 Piazzi, A. ; Visioli, A.，Global minimum-jerk trajectory planning of robot manipulators，IEEE Transactions on Industrial Electronics，２０００年 ２月，(Volume:47 , Issue: 1)，pp.140 - 149

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００−２１／０２

(57)【特許請求の範囲】

【請求項1】

多関節ロボットの関節を協調制御して対象部位を操作する多関節ロボットの関節協調制御装置であって、
現在時刻における前記関節の状態から開始して、前記関節の速度、加速度、躍度の設定上下限制約を全て満たしながら、所定の時間経過後に、設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達可能な前記関節への躍度入力の軌道パターンを決定し、当該決定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する躍度入力への上下限制約計算部と、
前記対象部位の躍度と前記関節への躍度入力の等式制約と、前記関節への躍度入力の上下限範囲の不等式としての制約と、を含む制約をタスクとし、前記等式制約に関する複数の設定タスクのうちで解が存在するタスクと、前記躍度入力への上下限制約計算部で計算された前記関節への躍度入力の上下限範囲と、を制約条件に含み、前記等式制約に関する複数の設定タスクのうちで解が存在しないタスクにおける前記関節への躍度入力の誤差を最適化の評価関数とし、前記制約条件の下で前記評価関数の値が最適となるように最適化問題を求解して、前記関節への躍度入力を計算する最適化問題求解部と、を備える、
多関節ロボットの関節協調制御装置。

【請求項2】

前記躍度入力への上下限制約計算部は、
前記軌道パターンとして、前記関節への躍度入力が設定最小値から設定最大値へと変化する第１の軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最大値へと変化する第２の軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、設定最大値へと変化する第３の軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、再び０を経て設定最大値へと変化する第４の軌道パターンと、を決定し、当該決定した第１〜第４の軌道パターンのうちで、いずれの軌道パターンを用いる場合に、前記所定の時間経過後に、前記設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達できるか否かを判定し、到達できると判定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する、
請求項１に記載の多関節ロボットの関節協調制御装置。

【請求項3】

前記多関節ロボットが、複数の自由度アームを有するロボットである、
請求項１または２に記載の多関節ロボットの関節協調制御装置。

【請求項4】

多関節ロボットの関節を協調制御して対象部位を操作する多関節ロボットの関節協調制御方法であって、
現在時刻における前記関節の状態から開始して、前記関節の速度、加速度、躍度の設定上下限制約を全て満たしながら、所定の時間経過後に、設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達可能な前記関節への躍度入力の軌道パターンを決定し、当該決定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する躍度入力への上下限制約計算工程と、
前記対象部位の躍度と前記関節への躍度入力の等式制約と、前記関節への躍度入力の上下限範囲の不等式としての制約と、を含む制約をタスクとし、前記等式制約に関する複数の設定タスクのうちで解が存在するタスクと、前記躍度入力への上下限制約計算工程で計算された前記関節への躍度入力の上下限範囲と、を制約条件に含み、前記等式制約に関する複数の設定タスクのうちで解が存在しないタスクにおける前記関節への躍度入力の誤差を最適化の評価関数とし、前記制約条件の下で前記評価関数の値が最適となるように最適化問題を求解して、前記関節への躍度入力を計算する最適化問題求解工程と、を備える、
多関節ロボットの関節協調制御方法。

【請求項5】

前記躍度入力への上下限制約計算工程は、
前記軌道パターンとして、前記関節への躍度入力が設定最小値から設定最大値へと変化する第１の軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最大値へと変化する第２の軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、設定最大値へと変化する第３の軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、再び０を経て設定最大値へと変化する第４の軌道パターンと、を決定し、当該決定した第１〜第４の軌道パターンのうちで、いずれの軌道パターンを用いる場合に、前記所定の時間経過後に、前記設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達できるか否かを判定し、到達できると判定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する、
請求項４に記載の多関節ロボットの関節協調制御方法。

【請求項6】

関節を協調制御して対象部位を操作する多関節ロボットであって、
現在時刻における前記関節の状態から開始して、前記関節の速度、加速度、躍度の設定上下限制約を全て満たしながら、所定の時間経過後に、設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達可能な前記関節への躍度入力の軌道パターンを決定し、当該決定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する躍度入力への上下限制約計算部と、
前記対象部位の躍度と前記関節への躍度入力の等式制約と、前記関節への躍度入力の上下限範囲の不等式としての制約と、を含む制約をタスクとし、前記等式制約に関する複数の設定タスクのうちで解が存在するタスクと、前記躍度入力への上下限制約計算部で計算された前記関節への躍度入力の上下限範囲と、を制約条件に含み、前記等式制約に関する複数の設定タスクのうちで解が存在しないタスクにおける前記関節への躍度入力の誤差を最適化の評価関数とし、前記制約条件の下で前記評価関数の値が最適となるように最適化問題を求解して、前記関節への躍度入力を計算する最適化問題求解部と、を備える、
多関節ロボット。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は多関節ロボットの関節協調制御装置および方法に関し、特に、関節への躍度入力を低減させると共に、関節の変位、速度、加速度の上下限制約を満たした関節入力を得ることが可能な多関節ロボット、その関節協調制御装置および方法に関する。

【背景技術】

【0002】

ロボットの関節を協調的に動作させて対象を操作する手法として、関節協調制御が知られている。ここで「関節」とは、主にロボットの駆動関節を指すが、ロボットが車輪型ロボットや人間型ロボットなどの地面に固定されていないものである場合には、その基準座標系の自由度も仮想的な関節として考えることができる。また、「対象」とは、操作したいロボットの部位を指す。例えば、ロボットがマニピュレータを備える場合には、対象は手先であり、対象の自由度は手先の位置、姿勢である。対象として、任意のリンク上の点やロボット全体の重心位置などを考えることもできる。

【0003】

また、従来、冗長自由度を持つ多関節ロボットを対象とした関節協調制御では、関節速度を入力とした手法がよく用いられている。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２００９−１８７５２１号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかしながら、関節速度を入力とした手法では、最も優先度の低いタスクの速度の二乗誤差を最小化することに起因して、（Ａ）速度が急変する軌道や振動的な軌道になることがあり、関節の加速度の変化量（以下、躍度と呼ぶ。）が大きなものとなる（つまり、加速度が不連続になる）、また、（Ｂ）タスクを切り替えた際に、関節の速度が不連続になる（つまり、関節の変位、速度、加速度の上下限制約を必ず満たした関節への入力を得ることができない）という問題がある。

【0006】

上記（Ａ）に関しては、速度の誤差の最小化に関して、関節の加速度については何ら制限を課していないことに起因して発生する。また、上記（Ｂ）に関しては、ステップごとにタスクの優先度が入れ替わる場合、または、タスクの追加や除外が行われた場合に発生し、このような場合には、最適化の対象となる目的関数の構造が全く異なるものに変更されることから、得られる関節の速度が不連続に変化してしまう。このような問題が起こると、得られた関節速度入力の軌道が滑らかなものにならず、ロボットにそのまま適用できないものになってしまう。

【0007】

従って、ロボットの関節協調制御主要に関して、（１）多関節ロボットにおいて、対象を操作するための関節の入力を得ることができる、（２）関節の変位、速度、加速度の上下限制約を必ず満たした関節の入力を得ることができる、（３）関節への入力は、躍度をできるだけ小さくし、加速度を滑らかなものにすることができる、ということが可能な技術が強く求められている。

【0008】

なお、本発明に関連する技術として、特許文献１に開示される技術がある。特許文献１に開示される技術では、関節の躍度を制限するものではあるが、速度パターンの頂点での躍度を単に制限するのみであり、上記（３）に示したような、できるだけ小さな躍度を実現するものではない。また、対象とするのは単軸のロボットであり、関節と対象とが一対一に対応するものであるため、上記（１）に関して考慮されたものではない。さらに、上記（２）に示した、関節の変位、速度、加速度の制約に関しても何ら考慮されていない。

【0009】

本発明は、上述した問題点を解決するためになされたものであり、関節への躍度入力を低減させると共に、関節の変位、速度、加速度の上下限制約を満たした関節入力を得ることが可能な多関節ロボット、その関節協調制御装置および方法を提供することを目的とする。

【課題を解決するための手段】

【0010】

本発明に係る多関節ロボットの関節協調制御装置は、多関節ロボットの関節を協調制御して対象部位を操作する多関節ロボットの関節協調制御装置であって、現在時刻における前記関節の状態から開始して、前記関節の速度、加速度、躍度の設定上下限制約を全て満たしながら、所定の時間経過後に、設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達可能な前記関節への躍度入力の軌道パターンを決定し、当該決定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する躍度入力への上下限制約計算部と、前記対象部位の躍度と前記関節への躍度入力の等式制約と、前記関節への躍度入力の上下限範囲の不等式制約と、を含む等式制約をタスクとし、前記等式制約に関する複数の設定タスクのうちで解が存在するタスクと、前記躍度入力への上下限制約計算部で計算された前記関節への躍度入力の上下限範囲と、を制約条件に含み、前記等式制約に関する複数の設定タスクのうちで解が存在しないタスクにおける前記関節への躍度入力の誤差を最適化の評価関数とし、前記制約条件の下で前記評価関数の値が最適となるように最適化問題を求解して、前記関節への躍度入力を計算する最適化問題求解部と、を備えるものである。これにより、関節への躍度入力を低減させると共に、関節の変位、速度、加速度の上下限制約を満たした関節入力を得ることができる。

【0011】

さらに、前記躍度入力への上下限制約計算部は、前記停止軌道パターンとして、前記関節への躍度入力が設定最小値から設定最大値へと変化する第１の停止軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最大値へと変化する第２の停止軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、設定最大値へと変化する第３の停止軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、再び０を経て設定最大値へと変化する第４の停止軌道パターンと、を決定し、当該決定した第１〜第４の停止軌道パターンのうちで、いずれの停止軌道パターンを用いる場合に、前記所定の時間経過後に、前記設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達できるか否かを判定し、到達できると判定した停止軌道パターン軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算するようにしてもよい。

【0012】

さらにまた、前記多関節ロボットが、複数の自由度アームを有するロボットであるようにしてもよい。

【0013】

本発明に係る多関節ロボットの関節協調制御方法は、多関節ロボットの関節を協調制御して対象部位を操作する多関節ロボットの関節協調制御方法であって、現在時刻における前記関節の状態から開始して、前記関節の速度、加速度、躍度の設定上下限制約を全て満たしながら、所定の時間経過後に、設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達可能な前記関節への躍度入力の軌道パターンを決定し、当該決定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する躍度入力への上下限制約計算工程と、前記対象部位の躍度と前記関節への躍度入力の等式制約と、前記関節への躍度入力の上下限範囲の不等式制約と、を含む等式制約をタスクとし、前記等式制約に関する複数の設定タスクのうちで解が存在するタスクと、前記躍度入力への上下限制約計算工程で計算された前記関節への躍度入力の上下限範囲と、を制約条件に含み、前記等式制約に関する複数の設定タスクのうちで解が存在しないタスクにおける前記関節への躍度入力の誤差を最適化の評価関数とし、前記制約条件の下で前記評価関数の値が最適となるように最適化問題を求解して、前記関節への躍度入力を計算する最適化問題求解工程と、を備えるものである。これにより、関節への躍度入力を低減させると共に、関節の変位、速度、加速度の上下限制約を満たした関節入力を得ることができる。

【0014】

さらにまた、前記躍度入力への上下限制約計算工程は、前記停止軌道パターンとして、前記関節への躍度入力が設定最小値から設定最大値へと変化する第１の停止軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最大値へと変化する第２の停止軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、設定最大値へと変化する第３の停止軌道パターンと、前記関節への躍度入力が設定最小値から０を経て設定最小値へと戻った後に、再び０を経て設定最大値へと変化する第４の停止軌道パターンと、を決定し、当該決定した第１〜第４の停止軌道パターンのうちで、いずれの停止軌道パターンを用いる場合に、前記所定の時間経過後に、前記設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達できるか否かを判定し、到達できると判定した停止軌道パターン軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算するようにしてもよい。

【0015】

本発明に係る多関節ロボットは、関節を協調制御して対象部位を操作する多関節ロボットであって、現在時刻における前記関節の状態から開始して、前記関節の速度、加速度、躍度の設定上下限制約を全て満たしながら、所定の時間経過後に、設定される前記関節の変位の上限または下限において前記関節の速度および加速度が０に到達可能な前記関節への躍度入力の軌道パターンを決定し、当該決定した軌道パターンから計算される前記関節への躍度入力の上下限値と、前記現在時刻における前記関節の状態、および前記関節の設定速度、加速度、躍度の上下限制約から計算される前記関節への躍度入力の上下限値と、を用いて前記関節への躍度入力の上下限範囲を計算する躍度入力への上下限制約計算部と、前記対象部位の躍度と前記関節への躍度入力の等式制約と、前記関節への躍度入力の上下限範囲の不等式制約と、を含む等式制約をタスクとし、前記等式制約に関する複数の設定タスクのうちで解が存在するタスクと、前記躍度入力への上下限制約計算部で計算された前記関節への躍度入力の上下限範囲と、を制約条件に含み、前記等式制約に関する複数の設定タスクのうちで解が存在しないタスクにおける前記関節への躍度入力の誤差を最適化の評価関数とし、前記制約条件の下で前記評価関数の値が最適となるように最適化問題を求解して、前記関節への躍度入力を計算する最適化問題求解部と、を備える、ものである。これにより、関節への躍度入力を低減させると共に、関節の変位、速度、加速度の上下限制約を満たした関節入力を得ることができる。

【発明の効果】

【0016】

本発明によれば、関節への躍度入力を低減させると共に、関節の変位、速度、加速度の上下限制約を満たした関節入力を得ることが可能な多関節ロボット、その関節協調制御装置および方法を提供することができる。

【図面の簡単な説明】

【0017】

【図1】実施の形態１に係る躍度入力への上下限制約計算処理を示すフローチャートである。

【図2】実施の形態１に係る軌道Ａを示すグラフである。

【図3】実施の形態１に係る軌道Ｂを示すグラフである。

【図4】実施の形態１に係る軌道Ｃを示すグラフである。

【図5】実施の形態１に係る軌道Ｄを示すグラフである。

【図6】実施の形態１に係る１自由度での軌道の例（変位の応答）を示すグラフである。

【図7】実施の形態１に係る１自由度での軌道の例（速度の応答）を示すグラフである。

【図8】実施の形態１に係る１自由度での軌道の例（加速度の応答）を示すグラフである。

【図9】実施の形態１に係る１自由度での軌道の例（躍度の入力）を示すグラフである。

【図10】実施の形態１に係るアームロボットの模式図である。

【図11】実施の形態１に係る３自由度での軌道の例（手先位置の応答（ｙ方向））を示すグラフである。

【図12】実施の形態１に係る３自由度での軌道の例（手先位置の応答（ｚ方向））を示すグラフである。

【図13】実施の形態１に係る３自由度での軌道の例（関節変位の応答）を示すグラフである。

【図14】実施の形態１に係る３自由度での軌道の例（関節速度の応答）を示すグラフである。

【図15】実施の形態１に係る３自由度での軌道の例（関節加速度の応答）を示すグラフである。

【図16】実施の形態１に係る３自由度での軌道の例（関節躍度の入力）を示すグラフである。

【図17】（ａ）本発明に係るロボット動作生成システム（ｂ）従来技術に係るロボット動作生成システムの概略構成図である。

【発明を実施するための形態】

【0018】

以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一又は対応する要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。

【0019】

まず、本発明の実施の形態の説明に先立ち、図１７を参照して本発明の概略的な構成を説明する。図１７（ａ）は、本発明に係るロボット動作生成システムの概略構成を示す図である。同図に示すように、ロボット動作生成システムは、動作入力装置１００と、全身協調計算部２００と、多関節ロボットとしてのロボット３００と、を備えている。全身協調計算部２００は、躍度入力への上下限制約計算部２１０と、最適化問題求解部２２０と、を備えている。

【0020】

動作入力装置１００は、ロボット３００に実現させたい位置・姿勢を含む動作データを取得する。全身協調計算部２００は、得られた動作データから、躍度入力への上下限を計算し、計算した躍度上下限を用いて最適化問題を求解する。ロボット３００は、最適化問題の求解結果（関節への躍度入力）に従って多関節を協調制御することで、ロボット３００の対象を操作する。

【0021】

全身協調計算部２００は、ロボット３００の関節を協調制御して、対象部位を操作する。躍度入力への上下限制約計算部２１０は、現在時刻における関節の状態から開始して、関節の速度、加速度、躍度について設定される上下限制約を全て満たしながら、関節への躍度入力の軌道パターンを決定する。この決定する軌道パターンは、所定の時間経過後に、設定される関節の変位の上限または下限において関節の速度および加速度が０に到達可能な軌道パターンである。さらに、躍度入力への上下限制約計算部２１０は、決定した軌道パターンから関節への躍度入力の上下限値を計算し、また、現在時刻における関節の状態と、設定される関節の速度、加速度、躍度の上下限制約と、を用いて関節への躍度入力の上下限値を計算し、これら計算した関節への躍度入力の上下限値を用いて、関節への躍度入力の上下限範囲を計算する。

【0022】

最適化問題求解部２２０は、制約条件の下で評価関数の値が最小となるように最適化問題を求解して、関節への躍度入力を計算する。ここで、タスクとは、対象部位の躍度と関節への躍度入力の等式制約と、関節への躍度入力の上下限範囲の不等式制約と、を含む等式制約を指す。また、ここで制約条件は、等式制約に関する複数の設定タスクのうちで解が存在するタスクと、躍度入力への上下限制約計算部２１０で計算された関節への躍度入力の上下限範囲と、を含む。また、評価関数として、等式制約に関する複数の設定タスクのうちで解が存在しないタスクにおける関節への躍度入力の誤差を用いる。

【0023】

本発明では、関節の躍度を入力として、その上下限制約を設定することで、躍度誤差の二乗和を最小化するような解（関節への躍度入力）を得る。これは、加速度の微分（躍度）をできるだけ小さくし、滑らかな軌道を実現することに相当する。この結果、加速度は不連続にはならず、タスクを切り替えても滑らかな軌道を得ることができ、従来の問題を解決した関節協調制御を実現することが可能となる。なお、軌道とは、対象または関節が従う、変位、速度、加速度、躍度などの時系列データのことを指す。

【0024】

本発明では、全身協調解を最終的に最適化問題として導出する。その過程において、最適化の制約条件として、関節の変位（可動範囲）、速度、加速度の制約を満たすような、関節への躍度入力の上下限制約の決定方法がポイントとなる。

【0025】

関節の全ての状態（ある時刻での関節の変位、速度、加速度）は、その目標変位で目標速度かつ目標加速度に最短で到達できるように設定する必要があり、これを実現するために、後述して例示する所定の軌道パターンを考慮する。なお、関節への躍度入力の下限に関しては上限を反転することで得ることができ、上限と同様の取り扱いをする。

【0026】

本発明ではこれら軌道パターンを考慮し、各条件によって適切な躍度の上下限を求める手法を提供し、求めた躍度の上下限制約を考慮した最適化計算によって、滑らかな関節軌道を生成する。さらに、この生成した関節軌道を用いることで、人環境において動作するロボット３００において加速度・速度が振動的になることを防止し、外界の環境に対しても安全な動作を提供することができる。

【0027】

＜発明の実施の形態１＞
以下、図１〜図１６を参照して、本実施の形態に係る多関節ロボットの関節協調制御装置および方法を具体的に説明する。本実施の形態に係る関節協調制御手法では、関節への入力を速度とした手法に代えて、速度から微分を進めて躍度とした手法を実現し、この制御手法を躍度分解制御と呼ぶ。躍度まで微分を進める理由は、加速度の変化量に上下限を設定し、加速度を滑らかに変化させたいためである。

【0028】

（躍度分解制御）
躍度分解制御を説明する。
対象の指令軌道ｙ_ｒ、ｙ_ｒのワンドット（数式中では上部に１つの「・」を有する変数であり、１階微分した変数である。）、ｙ_ｒのツードット（数式中では上部に２つの「・」を有する変数であり、２階微分した変数である。）、が与えられたとき、以下の式（１）に示す状態フィードバック則の躍度（ｙのスリードット（数式中では上部に３つの「・」を有する変数であり、３階微分した変数である。））を実現すれば、対象を目標軌道に追従させることができる。

【0029】

ただし、以下の式において、ｙ、ｙのワンドット、ｙのツードット、はそれぞれ現在時刻での対象の変位、速度、加速度を示すベクトルであり、ｙのスリートッドは現在時刻で実現すべき対象の躍度を示すベクトルであり、Ｋ_ｄ、Ｋ_ｖ、Ｋ_ａ、はそれぞれ変位、速度、加速度の偏差に対するフィードバックゲインである。このフィードバック則は三次系の応答になるため、適切なゲインを予め計算し設定することで安定性を保証しておく。

【0030】

【数1】

【0031】

対象の躍度（ｙのスリードット）と関節の躍度（ｘのスリードット）の関係は、対象速度と関節速度に関するヤコビ行列Ａを用いて、以下の式（２）に示される。ただし、以下の式において、ｘ、ｘのワンドット、ｘのツードット、はそれぞれ現在時刻での関節の変位、速度、加速度を示すベクトルであり、ｘのスリートッドは現在時刻で実現すべき関節の躍度を示すベクトルである。

【0032】

【数2】

【0033】

現在時刻での状態量ｘ、ｘのワンドット、ｘのツードット、は全て既知であるため、躍度のタスクは以下の式（３）に示す等式制約条件となる。詳細は後述するが、複数のタスクを設定する場合には、複数の等式制約条件を考慮することができる。また、実際のロボット３００においては関節の変位や速度の制約が存在する。このため、関節の変位や速度の制約についても考慮する必要がある。そのため、等式としてのタスクに加えて、不等式としての制約をもタスクとして加え、このタスクの下で、最適化問題として求解する手法が知られている（非特許文献：Bernard Faverjon and Pierre Tournassoud."A Local Based Approach for Path Planning of Manipulators", With a High Number of Degrees of Freedom. In In Procs. of IEEE In- ternational Conference on Robotics and Automation, pp. 1152-1159, 1987.）。

【0034】

【数3】

【0035】

ただし、式（３）のｂのスリードットは、以下の式（４）により示される。

【数4】

【0036】

複数のタスクがある場合には、複数の等式制約条件を考慮することができる。例えばタスクが３つある場合には、以下の式（５）に示す等式制約条件となる。

【数5】

【0037】

また、関節の躍度入力に関して、以下の式（６）に示すように、上下限制約が設定される。この関節への躍度入力の上下限の設定方法は、関節の変位の上下限（可動範囲）、速度の上下限、加速度の上下限を実現するものであるが、その計算方法の詳細については後述する。

【数6】

【0038】

また、従来手法と同様にタスクに優先度を設定し、解が存在するタスクまでを制約条件に含める。例えば、式（５）に示した３つのタスクのうちで、優先度の高い１番目の等式制約および２番目の等式制約までのタスクを実現する解は存在するが、３番目の等式制約を満たすタスクは存在しないものとする。この場合、解が存在しない３番目の等式制約のタスクの躍度の誤差を、最適化のための評価関数とする。すると、最適化問題の目的関数は、以下の式（７）に示す躍度誤差の二乗和からなる二次形式となる。

【0039】

【数7】

【0040】

つまり、式（５）に示した１番目の等式制約のタスクおよび２番目の等式制約のタスクを満たし、かつ、式（６）に示した関節の躍度入力の上下限制約を満たしながら、３番目の等式制約のタスクに関する躍度の誤差の二乗和により示される式であって、式（７）に示した目的関数を最小化する問題である。このような二次計画問題を求解することで、関節の躍度入力を得ることができる。

【0041】

従って、関節への躍度を入力としたことで、躍度誤差の二乗和を最小化するような解が得られる。これは、加速度の微分をできるだけ小さくし、滑らかな軌道を実現することに相当する。例えば、ステップごとにタスクの優先度を変更する、または、タスクの追加や削除を行った場合、不連続に変化するのは関節の躍度である。関節の躍度の上下限制約を設定することで、関節の加速度は(その躍度の上下限制約以上には) 不連続にはならず、その結果、タスクを切り替えても滑らかな軌道を得ることができる。

【0042】

ここで、関節への躍度入力に対する上下限制約をいかにして設定するか、が問題になる。まず、関節への躍度入力として、ステップごとに任意の上下限制約を与えることができるものの、実際に制限したいのは、ロボット３００の関節の躍度だけではなく、トルク、速度、変位（可動範囲）の制約である。トルクはロボット３００の慣性行列に依存するため、本実施の形態では、関節の加速度の制約を加える場合を例に説明する。

【0043】

また、関節への躍度入力は最適化計算の結果として得られるものであり、予め分かるものではない。保証されるのは、現在時刻における関節への躍度入力の上下限制約のみである。つまり、関節の変位、速度、加速度の上下限制約を満たすための関節への躍度入力の上下限制約を、どのようにして与えるかが問題となる。以下、具体的に説明する。

【0044】

（関節の躍度入力への上下限制約の決定方法：関節の上下限制約）
以下では、１つの関節について、変位、速度、加速度の上下限制約を満たすための、現在時刻での関節への躍度入力の上下限制約をどのようにして計算するか、を説明する。多関節の場合についても、それぞれ独立に考えることで同様に扱うことができる。

【0045】

ある関節の現在時刻での変位、速度、加速度を、それぞれｘ、ｘのワンドット、ｘのツードット、とし、関節への入力を躍度（ｘのスリードット）とする。全身協調制御において、躍度入力（ｘのスリードット）は、最適化問題の変数の一要素であり、ステップごとに設定された躍度（ｘのスリードット）の上下限制約は必ず満たされる。しかし、最適化の結果どのような値が入力されるかは前もって分からないことに注意する必要がある。

【0046】

離散時間Δｔで離散化された時刻ｋでの状態変数として、変位、速度、加速度を、それぞれｘ_ｋ、ｘ_ｋのワンドット、ｘ_ｋのツードット、とし、関節の躍度（ｘ_ｋのスリードット）を入力とする。ただし、ｋ＝０を現在時刻とする。時刻ｋ＋１の状態の更新式を、以下の式（８）に示す。

【数8】

【0047】

ここで、全ての時刻ｋにおいて、変位、速度、加速度、躍度の全てが、以下の式（９）に示す上下限制約を満たすようにしたい。

【数9】

【0048】

ただし、式（９）において、上下限の制約値ｘ_ｌ、ｘ_ｕ、ｘ_ｌのワンドット、ｘ_ｕのワンドット、ｘ_ｌのツードット、ｘ_ｕのツードット、ｘ_ｌのスリードット、ｘ_ｕのスリードット、はすべて定数であり、予め適切な値が設定される。また、ｘ_ｌのワンドット＜０＜ｘ_ｕのワンドット、ｘ_ｌのツードット＜０＜ｘ_ｕのツードット、ｘ_ｌのスリードット＜０＜ｘ_ｕのスリードット、であるとする。

【0049】

式（９）の全ての不等式を必ず満たすことが保証されるような、以下の式（１０）により示される、現在時刻での躍度の上下限制約（ｘ_０ｌのスリードット、ｘ_０ｕのスリードット）を求めることが問題になる。

【数10】

【0050】

以下では、関節への躍度入力の上限（ｘ_０ｕのスリードット）の計算方法を例に説明するが、上限と下限を入れ替えることで、関節への躍度入力の下限（ｘ_０ｌのスリードット）に関しても全く同様に考えることができる。

【0051】

（関節の躍度入力への上下限制約の決定方法：基本的な考え方）
上述したように、関節には変位、速度、加速度の上下限制約があるが、これらを現在時刻近辺において瞬間的に満たすだけでは不十分である。現在時刻近辺でこれらの上下限制約を満たしても、未来の時刻において同様に上下限制約を満たせるという保証は無い。また、変位の上限付近で振動するような軌道は、上限制約を満たすとしても望ましい軌道とはいえず、避けることが好ましい。

【0052】

いま、現在時刻において関節への躍度入力（ｘ_０のスリードット）にある値を代入した場合を想定する。さらに、この代入された関節への躍度入力の下で、現在時刻から所定の時間経過した時に、設定された速度、加速度、躍度の上下限制約を全て満たしながら、この関節が変位の上限で振動やオーバーシュートしないで停止する軌道（停止軌道）が存在するものと仮定する。ここで、「変位の上限で停止する」とは、目標位置である変位上限（ｘ＝ｘ_ｕ）において、目標速度が０（ｘのワンドット＝０）、目標加速度が０（ｘのツードット＝０）に到達することを指す。

【0053】

このとき、停止軌道を実現できると考えられる関節への躍度入力（ｘ_０のスリードット）で最大のものが、現在時刻における躍度入力の上限（ｘ_０ｕのスリードット）である。関節が変位上限で振動せずに停止できる軌道としては様々なものが考えられるが、例えば、最短時間で停止することを目的とする場合には、以下の４つの軌道パターン（軌道Ａ、Ｂ、Ｃ、Ｄ）のいずれかとなる。

【0054】

・軌道Ａ：「ｘ_ｌのスリードット→ｘ_ｕのスリードット」の躍度入力により停止
・軌道Ｂ：「ｘ_ｌのスリードット→０→ｘ_ｕのスリードット」の躍度入力により停止
・軌道Ｃ：「ｘ_ｌのスリードット→０→ｘ_ｌのスリードット→ｘ_ｕのスリードット」の躍度入力により停止
・軌道Ｄ：「ｘ_ｌのスリードット→０→ｘ_ｌのスリードット→０→ｘ_ｕのスリードット」の躍度入力により停止

【0055】

この４つの軌道パターンは、常に最大値または常に最小値の躍度を入力とする区間と、状態が速度上限または加速度上限にあり躍度の入力を０にする区間と、の組み合わせを用いることで、最短時間で停止する軌道を実現するものである。

【0056】

図１は、現在時刻の躍度入力への上下限制約を計算する処理を示すフローチャートである。躍度入力への上下限制約計算では、４つの軌道パターンＡ、Ｂ、Ｃ、Ｄの順に、いずれの停止軌道により停止が実現可能か否かを判定する（Ｓ１１０、Ｓ１３０、Ｓ１５０）。実現可能な停止軌道が分かると、その停止起動を実現するための躍度入力（ｘ_０のスリードット）を、現在時刻での関節への躍度入力の上限（ｘ_０ｕのスリードット）として設定する（Ｓ１２０、Ｓ１４０、Ｓ１６０）。なお、停止軌道の判定処理の詳細については、後述する。

【0057】

停止可能な初期状態(通常は停止状態)からこの処理に従って関節への躍度入力の範囲を実現していくことで、停止軌道を実現できる範囲で必ず動作し続けるため、変位、速度、加速度の制約内での動作が常に可能となる。以下、各軌道パターンＡ、Ｂ、Ｃ、Ｄの説明に先立って、軌道パターンＡ、Ｂ、Ｃ、Ｄに共通する処理を説明する。

【0058】

（関節の躍度入力への上下限制約の決定方法：全軌道パターンに共通の躍度制限の必要条件）
４つの軌道パターンＡ、Ｂ、Ｃ、Ｄに共通となる、躍度入力の必要条件を説明する。まず、躍度入力自体にパラメータとしての上下限制約（ｘ_ｌのスリードット、ｘ_ｕのスリードット）が予め設定されている。加速度、速度、変位の上下限値がｘ_ｌのツードット、ｘ_ｕのツードット、ｘ_ｌのワンドット、ｘ_ｕのワンドット、ｘ_ｌ、ｘ_ｕ、であるから、それぞれを超えないための躍度の上下限は、以下の式（１１）により示される。

【0059】

【数11】

【0060】

そして、以下の式（１２）により示される条件が、躍度の上下限の必要条件となる。

【数12】

【0061】

また、現在時刻からΔｔ後の時刻１においては、以下の式（１３）により示すように、加速度、速度、変位のそれぞれが上限制約に収まるようにする必要がある。

【数13】

【0062】

これらから、時刻０での関節への躍度入力の上限値を計算することができ、以下の式（１４）により示すように、それぞれ得ることができる。

【数14】

【0063】

（関節の躍度入力への上下限制約の決定方法：軌道パターンＡ）
軌道Ａ（「ｘ_ｌのスリードット→ｘ_ｕのスリードット」の躍度入力により停止）を説明する。

【0064】

まず、軌道Ａをどのようにして決定するかを説明する。図２に、軌道Ａを例示する。軌道Ａは、まず、最小値の躍度（ｘ_ｌのスリードット）を所定の時間ｔ_１入力し、その後、最大値の躍度（ｘ_ｕのスリードット）を一定時間ｔ_２与えて停止するパターンである。現在時刻を時刻０、ステップ時間Δｔ後を時刻１、そのｔ_１秒後を時刻２、そのｔ_２秒後を時刻３としている。現在時刻での状態変数をｘ_０、ｘ_０のワンドット、ｘ_０のツードット、とし、現在時刻の躍度入力を、ｘ_０のスリードットとする。

【0065】

時刻３では、以下の式（１５）に示す停止条件が必要である。変数をｘ_０のスリードット、ｔ_１、ｔ_２と置くと、式（１５）から変数の解を求めることができる。

【数15】

【0066】

まず変数ｔ_１については、以下の式（１６）に示す６次多項式の解となる。

【数16】

【0067】

ただし、式（１６）におけるａ_０〜ａ_６は、以下の式（１７）に示される。

【数17】

【0068】

上記の式（１６）に示した多項式の解は、ニュートン法などを用いた数値解法によって求めることができる。解が無い場合には、ｔ_１＝０とする。このｔ_１を用いて、以下の式（１８）の解として、ｔ_２を得ることができる。

【数18】

【0069】

ただし、式（１８）におけるｂ_０〜ｂ_２は、以下の式（１９）に示される。

【数19】

【0070】

以上のｔ_１、ｔ_２から、ｘ_０のスリードットを、以下の式（２０）に示すようにして、得ることができる。

【数20】

【0071】

次に、このようにして決定した停止軌道Ａが、速度と加速度の制約を満たしているか否かを判定する。すなわち、軌道Ａに従って停止可能か否かを判定する。
まず、時刻ｌａを、加速度が０（ｘ_ｌａのツードット＝０）になる時刻とすると、時刻１から時刻ｌａまでの時間ｔｌａは、以下の式（２１）により示される。

【数21】

【0072】

ここで、ｔｌａ＜０である場合には、この軌道では速度の極大値を取らないため、次の加速度の確認へと進む。

【0073】

一方で、ｔｌａ＞０である場合には、以下の式（２２）により示される、その時の速度（ｘ_ｌａのワンドット）を計算する。計算の結果、ｘ_ｌａのワンドット＞ｘ_ｕのワンドットである場合には、得られた軌道Ａは速度制約を満たさないと判定される。

【数22】

【0074】

次に、加速度の最小値（ｘ_２のツードット）を求める。ｘ_２のツードット＜ｘ_ｌのツードットである場合には、得られた軌道Ａは加速度制約を満たさないと判定される。

【0075】

以上の判定の結果、制約を全て満たすと判定された場合、軌道Ａが最短時間の停止軌道となる。このため、躍度入力の最大値（ｘ_０ｕのスリードット）は、以下の式（２３）に示すようにして得ることができる。

【数23】

【0076】

初期状態において軌道Ａが実現でき、かつ、軌道Ａ、Ｂ、Ｃ、Ｄのいずれかで停止できるように状態を遷移させていけば、軌道Ａ自体が実現できない状態に陥ることはない。

【0077】

（関節の躍度入力への上下限制約の決定方法：軌道パターンＢ）
次に、軌道Ｂ（「ｘ_ｌのスリードット→０→ｘ_ｕのスリードット」の躍度入力により停止）を説明する。まず、軌道Ｂをどのようにして決定するかを説明する。図３に、軌道Ｂを例示する。

【0078】

軌道Ｂでは、時刻４において、以下の式（２４）に示す停止条件が必要となる。

【数24】

【0079】

また、以下の式（２５）に示すように、時刻２から時刻３の間では加速度が最小値をとる。

【数25】

【0080】

そして、変数をｘ_０のスリードット、ｔ_１、ｔ_２、ｔ_３と置くと、上記式（２５）に示した停止条件と、上記式（２５）の１番目の条件と、から、変数の解を求めることができる。

【0081】

変数ｔ_３の解は、以下に示す式（２６）により得ることができる。

【数26】

【0082】

変数ｔ_１の解は、以下に示す式（２７）の解として得ることができる。

【数27】

【0083】

ただし、式（２７）におけるａ_０〜ａ_４は、以下の式（２８）に示される。

【数28】

【0084】

変数ｔ_２の解は、ｔ_１、ｔ_３から、以下に示す式（２９）により得ることができる。

【数29】

【0085】

変数（ｘ_０のスリードット）の解は、上述したようにして得た他の変数から、以下に示す式（３０）により得ることができる。

【数30】

【0086】

次に、このようにして決定した停止軌道Ｂが、速度の制約を満たしているか否かを判定する。すなわち、軌道Ｂに従って停止可能か否かを判定する。
まず、時刻ｔ１ｂを、軌道Ｂが速度の極大値をとる時刻とすると、時間ｔｌｂは、以下の式（３１）により示される。

【数31】

【0087】

ここで、ｔ１ｂ＜０である場合には、この軌道では速度の極大値を取らないため、軌道Ｂによって停止することができる。

【0088】

一方で、ｔ１ｂ＞０である場合には、以下の式（３２）により示される、その時の速度（ｘ_１ｂのワンドット）を計算する。計算の結果、ｘ_１ｂのワンドット＞ｘ_ｕのワンドットである場合には、得られた軌道Ｂは速度制約を満たさないと判定される。

【数32】

【0089】

以上の判定の結果、制約を満たすと判定された場合、軌道Ｂが最短時間の停止軌道となる。このため、躍度入力の最大値（ｘ_０ｕのスリードット）は、以下の式（３３）に示すようにして得ることができる。

【数33】

【0090】

（関節の躍度入力への上下限制約の決定方法：軌道パターンＣ）
次に、軌道Ｃ（「ｘ_ｌのスリードット→０→ｘ_ｌのスリードット→ｘ_ｕのスリードット」の躍度入力により停止）を説明する。まず、軌道Ｃをどのようにして決定するかを説明する。図４に、軌道Ｃを例示する。

【0091】

軌道Ｃでは、時刻５において、以下の式（３４）に示す停止条件が必要となる。

【数34】

【0092】

また、以下の式（３５）に示すように、時刻２から時刻３の間では速度が最大値をとる。

【数35】

【0093】

そして、変数をｘ_０のスリードット、ｔ_１、ｔ_２、ｔ_３、ｔ_４と置くと、上記式（３４）に示した停止条件と、上記式（３５）の１番目および２番目の条件と、から、変数の解を求めることができる。

【0094】

変数ｔ_４の解は、以下に示す式（３６）により得ることができる。

【数36】

【0095】

変数ｔ_３の解は、以下に示す式（３７）により得ることができる。

【数37】

【0096】

変数ｔ_１の解は、以下の式（３８）に示す二次方程式の解として得ることができる。

【数38】

【0097】

ただし、式（３８）におけるａ_０〜ａ_２は、以下の式（３９）に示される。

【数39】

【0098】

変数ｔ_２の解は、ｔ_１、ｔ_３、ｔ_４から、以下に示す式（４０）により得ることができる。

【数40】

【0099】

変数（ｘ_０のスリードット）の解は、以下に示す式（４１）により得ることができる。

【数41】

【0100】

次に、このようにして決定した停止軌道Ｃが、加速度の制約を満たしているか否かを判定する。すなわち、軌道Ｃに従って停止可能か否かを判定する。
軌道Ｃにおける制限は、時刻４での加速度（ｘ_４のツードット）が、加速度の下限（ｘ_ｌのツードット）を超えないことである。ｘ_４のツードット＜ｘ_ｌのツードットである場合には、軌道Ｃは制約を満たしていないと判定される。一方で、この制約を満たすと判定された場合には、軌道Ｃが最短時間の停止軌道となる。このため、躍度入力の最大値（ｘ_０ｕのスリードット）は、以下の式（４２）に示すようにして得ることができる。

【数42】

【0101】

（関節の躍度入力への上下限制約の決定方法：軌道パターンＤ）
次に、軌道Ｄ（「ｘ_ｌのスリードット→０→ｘ_ｌのスリードット→０→ｘ_ｕのスリードット」の躍度入力により停止）を説明する。まず、軌道Ｄをどのようにして決定するかを説明する。図５に、軌道Ｄを例示する。

【0102】

軌道Ｄでは、時刻６において、以下の式（４３）に示す停止条件が必要となる。

【数43】

【0103】

また、以下の式（４４）に示すように、時刻２から時刻３の間では速度が最大値をとる。

【数44】

【0104】

また、以下の式（４５）に示すように、時刻４から時刻５の間では加速度が最小値をとる。

【数45】

【0105】

そして、変数をｘ_０のスリードット、ｔ_１、ｔ_２、ｔ_３、ｔ_４、ｔ_５と置くと、上記式（４３）に示した停止条件と、上記式（４５）の１番目および２番目の条件と、上記式（４５）の１番目の条件と、から、変数の解を求めることができる。

【0106】

変数ｔ_３、ｔ_５、ｔ_４の解は、以下に示す式（４６）により得ることができる。

【数46】

【0107】

変数ｔ_１の解は、以下の式（４７）に示す二次方程式の解として得ることができる。

【数47】

【0108】

ただし、式（４７）におけるａ_０〜ａ_２は、以下の式（４８）に示される。

【数48】

【0109】

変数ｔ_２の解は、ｔ_１、ｔ_３、ｔ_４、ｔ_５から、以下に示す式（４９）により得ることができる。

【数49】

【0110】

変数（ｘ_０のスリードット）の解は、以下に示す式（５０）により得ることができる。

【数50】

【0111】

このようにして決定した停止軌道Ｄは制約がない。このため、躍度入力の最大値（ｘ_０ｕのスリードット）は、以下の式（５１）に示すようにして得ることができる。

【数51】

【0112】

（動作確認：１自由度での動作確認）
次に、上述した躍度制限を、１自由度変数ｘに対して実装し、その動作確認を行った。変数ｘには、変位、速度、加速度の上下限を設定する。関節の目標変位ｘ_ｒに対して、躍度入力（ｘのスリードット）を以下の式（５２）に示すようにして計算する。

【数52】

【0113】

ステップごとに図１に示した処理を行い躍度の上下限を計算し、関節の躍度入力（ｘのスリードット）を上下限によって制限する。ただし、上記式（５２）の各ゲインＫｄ、Ｋｖ、Ｋａは、この３次系システムの応答に基づいて予め適当な値を設計する。

【0114】

図６、７、８に、得られた軌道を例示する。図６は、変位の応答を示す。図７は、速度の応答を示す。図８は、加速度の応答を示す。図９は、躍度入力を示す。変位の上下限を±１、速度の上下限を±２、加速度の上下限を±１０、躍度の上下限を±１０００に設定している。また、目標変位ｘ_ｒは、時刻３まで１．１、時刻８まで−１．１、時刻１０まで０に設定している。目標速度と目標加速度は、共に０と設定している。これらの図から明らかなように、変位、速度、加速度の上下限（図において、Upper limit、Lower limitとして示す。）を全て満たしながら動作していることがわかる。

【0115】

（動作確認：３自由度での動作確認）
次に、躍度分解制御を用いた３自由度アームを有するロボットに対して動作確認を行い、その結果を説明する。図１０は、本実施の形態に係る多関節ロボット３００（以下、単にロボット３００と称する。）の模式的な構成例を示している。ここでは、ロボット３００が、３自由度アームを備えたアームロボットである場合を例に説明する。同図にこのロボット３００の３自由度アームの軸の配置を示す。

【0116】

ロボット３００はコントローラ３１０を備えている。また、ロボット３００はアーム３２０を備えており、アーム３２０の先端に設けられた手先３３０を対象として、この対象を操作する。アーム３２０は、複数の関節（関節３２１、３２２、３２３）と、各関節を接続するリンクと、を備えている。各関節には、関節角を制御するアクチュエータと、関節角を検出するエンコーダとが内蔵されている。なお、コントローラ３１０は、ロボット３００の内部に有する構成であってもよいし、外部に接続されてロボット３００に対して指令を送信する構成であってもよい。

【0117】

コントローラ３１０は、上述した全身協調計算部２００の機能を有している。すなわち、コントローラ３１０は、躍度入力への上下限制約計算部２１０、最適化問題求解部２２０の各機能を実現するプログラムを含み、各部での各処理を実行し、関節への躍度入力を計算する。そして、コントローラ３１０は、計算した関節への躍度入力に基づいて、各関節（関節３２１、３２２、３２３）を制御することで多関節を協調制御し、これによって、アーム３２０の手先３３０を移動させる。

【0118】

すなわち、コントローラ３１０は、計算された各関節の躍度の目標に追従するように、アクチュエータを制御する。より具体的には、コントローラ３１０は、各関節の目標躍度の時系列データを算出し、算出した目標躍度に追従するように各関節を制御することで、ロボット３００の対象を操作する。

【0119】

なお、コントローラ３１０は、主要なハードウェア構成として、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される制御プログラムと、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを用いて構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスによって相互に接続されている。

【0120】

また、図１０に示すロボット３００の関節（関節３２１、３２２、３２３）は、ｙ軸方向への回転関節である。同図に示す例では、ｘ−ｚ平面上の手先３３０の位置を指定して、この位置を３つの関節を用いて実現するために、冗長な問題となる。

【0121】

まず、ロボット３００は、初期状態において、図１０（ａ）に示す初期姿勢(全関節（関節３２１、３２２、３２３）角度＝０) にあるものとする。そして、タスクの設定に関して、最も優先度の高いタスクとして、図１０（ｂ）に示すように、手先３３０の目標変位（目標位置）を、座標（２．０，０．５）にステップ状に与えた。また、手先３３０の指令速度、指令加速度は０を与えた。次に優先度の高いタスクとして、各関節（関節３２１、３２２、３２３）の速度を０にする指令を与えている。さらに、関節３２１の変位の下限を−０．６５［ｒａｄ］に制限し、手先３３０の目標位置へ到達するためには、関節３２１がこの下限の変位を必ずとるようにした。

【0122】

図１１および図１２に、計算した関節の躍度入力に従って、ロボット３００を動作させた際の対象（手先３３０）の変位の応答を示す。同図に示すように、指令した変位へと収束していることが確認できる。

【0123】

また、図１３〜１５に、各関節（関節３２１、３２２、３２３）の変位、速度、加速度の応答を示す。なお、各図において、関節３２１を「Ｊｏｉｎｔ ０」、関節３２２を「Ｊｏｉｎｔ １」、関節３２３を「Ｊｏｉｎｔ ２」として示している。また、図１６に、関節（関節３２１、３２２、３２３）の躍度入力を示す。図１３〜１６に示すように、各関節（関節３２１、３２２、３２３）それぞれに設定された上下限制約を満たしながら動作していることが確認できる。

【0124】

また、関節（関節３２１、３２２、３２３）の躍度入力は常に上下限を満たしているため、滑らかな軌道が得られる。特に、図１３に示すように、関節３２１に関しては０．９秒付近で変位の下限にぶつかっているが、ロボット３００は冗長な自由度を持っているために、関節３２２および関節３２３が変位することで、手先３３０の目標位置を実現できている。

【0125】

以上に説明したように、複数の関節を協調させて対象を操作するロボットにおいて、関節の躍度入力を最小化すると共に、関節の変位、速度、加速度の上下限制約を必ず満たすことが可能な手法を実現した。

【0126】

図１７を参照して、本発明による効果を更に説明する。
近年、人の位置・姿勢を簡便に取得できるセンサが実用化されている。そのようなセンサを含む動作入力装置１００を用いて得られた動作データから、ロボット３００を駆動するような試みも多くなされている。図１７に、このような動作入力装置１００を用いた、ロボット３００の動作生成システムの概略構成を示す。図１７（ａ）は、本発明に係る全身協調計算部２００を含む動作生成システムの概略構成図である。図１７（ｂ）は、従来技術に係る全身協調計算部５００を含む動作生成システムの概略構成図である。

【0127】

図１７（ｂ）に示すように、従来の関節協調制御技術を用いた動作生成システムでは、協調制御処理の前段には、センサの誤差などによる不連続な入力値を補正する構成（入力値の不連続性補正処理４００）を必要とし、また、後段にも、過度な加速度の不連続点や速度超過を平滑化するための構成（指令値の平滑化処理６００）を必要としていた。

【0128】

これに対して、図１７（ａ）に示すように、本発明に係る全身協調計算部２００を用いた動作生成システムでは、全身協調計算部２００での全身協調制御に状態フィードバック則を内包しているために、その前段の処理が不要になる。さらに、関節躍度まで考慮して加速度のレベルでの連続性を保証していることから、その後段のフィルタ処理を設ける必要がなく、直接、関節駆動の指令値として使用することができる。従って、システムの大幅な簡素化を期待でき、計測した何らかの動作目標値を、そのままロボット３００に適用するようなダイレクトな系を構成することが可能となる。

【0129】

すなわち、本発明に係る全身協調計算部２００を用いた動作生成システムでは、システム単体で、不連続かつ粗悪なデータ入力からでもロボット３００の関節を滑らかに駆動するような軌道に修正して出力することができる。この結果、ロボット３００自体に対しても、外的環境（ヒトやモノ）に対しても、安心・安全な動作指令値を提供することができ、非常に有用なものである。

【0130】

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述した実施の形態では、ロボット３００が３自由度アームを備えたアームロボットである場合を例に説明したが、本発明はこれに限られるものではない。すなわち、ロボット３００として、人間型ロボット(関節数２０以上) や車輪付きアームロボット(関節数８以上)に対して本発明を適用するものとしてもよい。

【0131】

また、変位、速度、加速度の上下限制約を実現するための躍度上下限の計算方法は一般的なものであり、例えば、産業ロボットアームや自動車のモーションコントロールに対しても適用するものとしてもよい。

【0132】

また、上述した実施の形態では、ロボット３００が備えるコントローラ３１０が、上述した全身協調計算部２００の機能を有する構成として説明したが、本発明はこれに限定されず、全身協調計算部２００の機能を備える装置を関節協調制御装置として、ロボット３００とは独立して備える構成としてもよい。

【符号の説明】

【0133】

１００ 動作入力装置、
２００ 全身協調計算部、
２１０ 躍度入力への上下限制約計算部、
２２０ 最適化問題求解部、
３００ ロボット、
３１０ コントローラ、
３２０ アーム、
３３０ 手先、
３２１、３２２、３２３ 関節、
４００ 入力値の不連続性補正処理、
５００ 全身協調計算部、
６００ 指令値の平滑化処理、

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】