JP6398777B2 - ロボット制御装置、制御方法、及び制御プログラム - Google Patents

Description

本発明は、ロボットを制御するロボット制御装置、制御方法、及び制御プログラムに関するものである。

例えば、関節で接続された複数のリンクから成る木構造リンク系のポーズ及び動作を生成する方法が知られている（特許文献１参照）。

特開２００３−０５８９０７号公報

上記方法において、冗長系の逆運動力学が用いられている。したがって、この方法を用いてロボット制御を行なった場合、その計算時間が膨大となるため、制御周期が低速となり、タスクの追従性が悪化する虞がある。

本発明は、このような問題点を解決するためになされたものであり、高速な制御周期が実現できつつ、タスクの追従性が良好にできるロボット制御装置、制御方法、及び制御プログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、ロボット周囲の環境情報を取得する環境情報取得手段と、前記ロボットが達成するタスク情報を含むタスクリストを更新し、該更新したタスクリストと、前記ロボットの姿勢情報と、前記ロボットが実行するタスクの目標値と、に基づいて、等式制約条件を設定し、前記ロボットが環境と接触する候補を含む干渉候補リストを、前記ロボットの姿勢情報と、前記環境取得手段により取得された環境情報と、に基づいて更新し、該更新した干渉候補リストと、前記ロボットの姿勢情報と、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記ロボットの逆運動学の最適化問題を、第１周期で求解し、該最適化問題の最適解を算出する低速周期演算手段と、前記ロボットの姿勢情報と、前記タスクの目標値と、前記低速周期演算手段からのタスクリスト又は前回周期のタスクリストと、に基づいて等式制約条件を設定し、前記ロボットの姿勢情報と、前記低速周期演算手段からの干渉候補リスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記低速周期演算手段により算出された最適解又は前回周期の最適解を初期値として前記ロボットの逆運動学の最適化問題を、前記第１周期より高速の第２周期で求解し、前記最適化問題の最適解を算出し、該算出した最適解に基づいて前記ロボットの姿勢情報を算出する高速周期演算手段と、前記高速周期演算手段により算出された姿勢情報に基づいて前記ロボットを制御する制御手段と、を備えることを特徴とするロボット制御装置である。
この一態様において、前記低速周期演算手段は、前記求解した最適化問題の最適解と、対応する前記タスクリスト及び干渉候補リストと、を記憶する記憶部を有し、前記高速周期演算手段は、前記記憶部に前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、が存在するとき、前記記憶部から読み込んだ前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、に基づいて、前記最適化問題を求解し、前記記憶部に前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、が存在しないとき、前回周期の前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、に基づいて、前記最適化問題を求解してもよい。
この一態様において、前記高速周期演算手段は、ウォームスターティング付きのアクティブセット法を用いて、前記最適化問題を求解し、前記低速周期演算手段は、内点法を用いて前記最適化問題を求解してもよい。
この一態様において、前記ロボットの姿勢情報は、現状の前記ロボットのベースフレームの位置姿勢と、前記ロボットの関節角度と、を含んでいてもよい。
この一態様において、前記最適化問題の最適解は、前記ロボットの姿勢情報の微分値であってもよい。
上記目的を達成するための本発明の一態様は、ロボット周囲の環境情報を取得するステップと、前記ロボットが達成するタスク情報を含むタスクリストを更新し、該更新したタスクリストと、前記ロボットの姿勢情報と、前記ロボットが実行するタスクの目標値と、に基づいて、等式制約条件を設定し、前記ロボットが環境と接触する候補を含む干渉候補リストを、前記ロボットの姿勢情報と、前記取得された環境情報と、に基づいて更新し、該更新した干渉候補リストと、前記ロボットの姿勢情報と、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記ロボットの逆運動学の最適化問題を、第１周期で求解し、該最適化問題の最適解を算出するステップと、前記ロボットの姿勢情報と、前記タスクの目標値と、前記設定されたタスクリスト又は前回周期のタスクリストと、に基づいて等式制約条件を設定し、前記ロボットの姿勢情報と、前記設定された干渉候補リスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記算出された最適解又は前回周期の最適解を初期値として前記ロボットの逆運動学の最適化問題を、前記第１周期より高速の第２周期で求解し、前記最適化問題の最適解を算出し、該算出した最適解に基づいて前記ロボットの姿勢情報を算出するステップと、前記算出された姿勢情報に基づいて前記ロボットを制御するステップと、を含む、ことを特徴とする制御方法であってもよい。
上記目的を達成するための本発明の一態様は、ロボットが達成するタスク情報を含むタスクリストを更新し、該更新したタスクリストと、前記ロボットの姿勢情報と、前記ロボットが実行するタスクの目標値と、に基づいて、等式制約条件を設定し、前記ロボットが環境と接触する候補を含む干渉候補リストを、前記ロボットの姿勢情報と、取得された前記ロボット周囲の環境情報と、に基づいて更新し、該更新した干渉候補リストと、前記ロボットの姿勢情報と、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記ロボットの逆運動学の最適化問題を、第１周期で求解し、該最適化問題の最適解を算出する処理と、前記ロボットの姿勢情報と、前記タスクの目標値と、前記設定されたタスクリスト又は前回周期のタスクリストと、に基づいて等式制約条件を設定し、前記ロボットの姿勢情報と、前記設定された干渉候補リスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記算出された最適解又は前回周期の最適解を初期値として前記ロボットの逆運動学の最適化問題を、前記第１周期より高速の第２周期で求解し、前記最適化問題の最適解を算出し、該算出した最適解に基づいて前記ロボットの姿勢情報を算出する処理と、前記算出された姿勢情報に基づいて前記ロボットを制御する処理と、をコンピュータに実行させる、ことを特徴とする制御プログラムであってもよい。

本発明によれば、高速な制御周期が実現できつつ、タスクの追従性が良好にできるロボット制御装置、制御方法、及び制御プログラムを提供することができる。

本発明の一実施形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。 ヒューマノイドロボットの一例を示す図である。 velocity damperを説明するための図である。 本発明の一実施形態に係る低速周期演算部及び高速周期演算部の演算処理を示すブロック図である。 本発明の一実施形態に係る低速周期演算部による低速周期の最適化演算方法のフローを示すフローチャートである。 本発明の一実施形態に係る高速周期演算部による高速周期の最適化演算方法のフローを示すフローチャートである。

以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施形態に係るロボット制御装置は、例えば、複数の関節部を介して連結された複数のリンクを有するヒューマノイドロボットなどのロボットの動作を制御する。各関節部には、例えば、サーボアンプと、各関節部を駆動するサーボモータなどのアクチュエータと、が設けられている。ロボット制御装置は、各関節部のサーボアンプを介して、各アクチュエータを制御することで、ロボットを制御する。

図１は、本発明の一実施形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。本実施形態に係るロボット制御装置１は、環境情報を取得する環境センサ２と、ロボットの最適化問題の最適解を低速周期で算出する低速周期演算部３と、ロボットの最適化問題の最適解を高速周期で算出する高速周期演算部４と、各関節部を駆動する複数のアクチュエータ５と、各アクチュエータ５を制御する制御部６と、を備えている。

ロボット制御装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。

環境センサ２は、環境情報取得手段の一具体例である。環境センサ２は、例えば、ロボット周囲に存在する障害物などの環境との間の距離情報、障害物の形状情報などの環境情報を取得する。環境センサ２は、例えば、カメラ、超音波センサ、ミリ波センサ、などの距離センサである。環境センサ２は、取得した環境情報を低速周期演算部３に出力する。

低速周期演算部３は、低速周期演算手段の一具体例である。低速周期演算部３は、ロボットが達成するタスク情報を含むタスクリストを更新する。そして、低速周期演算部３は、更新したタスクリストと、ロボットの姿勢情報と、ロボットが実行するタスクの目標値と、に基づいて、等式制約条件を設定する。また、低速周期演算部３は、ロボットが環境と接触する候補を含む干渉候補リストを、ロボットの姿勢情報と、環境センサ２から出力された環境情報と、に基づいて更新する。低速周期演算部３は、更新した干渉候補リストと、ロボットの姿勢情報と、に基づいて不等式制約条件を設定する。低速周期演算部３は、設定した等式制約条件及び不等式制約条件に基づいて、ロボットの逆運動学の最適化問題を、低速周期（低速の第１周期）で求解し、該最適化問題の最適解を算出する。

高速周期演算部４は、高速周期演算手段の一具体例である。高速周期演算部４は、ロボットの姿勢情報と、タスクの目標値と、低速周期演算部３からのタスクリスト又は前回周期のタスクリストと、に基づいて、等式制約条件を設定する。また、高速周期演算部４は、ロボットの姿勢情報と、タスクの目標値と、低速周期演算部３からのタスクリスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定する。高速周期演算部４は、設定した等式制約条件及び不等式制約条件に基づいて、低速周期演算部３により算出された最適解又は前回周期の最適解を初期値としてロボットの逆運動学の最適化問題を、高速周期（第１周期より高速の第２周期）で求解し、最適化問題の最適解を算出する。高速周期演算部４は、該算出した最適解に基づいてロボットの姿勢情報を算出し、算出した姿勢情報を制御部６に出力する。

制御部６は、制御手段の一具体例である。制御部６は、高速周期演算部４から出力された姿勢情報に基づいて、各関節部のアクチュエータ５を制御する。

ところで、例えば、ヒューマノイドロボットなどの多自由度ロボットの逆運動学問題では、関節制約や環境との干渉を回避しつつ目標タスクを達成するように構成した最適化ベースのアルゴリズムを用いている。このような最適化問題の求解は、汎用性及び拡張性が非常に高いというメリットを有している。一方で、従来、この最適化問題の求解は、多くの計算時間を要するため、制御周期が上げられず、目標タスクに対する追従性が悪いという、問題が生じていた。

これに対し、本実施形態に係るロボット制御装置１においては、上述の如く、低速周期演算部３が、タスクリスト及び干渉候補リストの更新を行う。低速周期演算部３は、更新したタスクリスト及び干渉候補リストに基づいて、最適化問題を低速周期で最適解を求解する。

同時に、高速周期演算部４は、低速周期演算部３からの又は前回周期のタスクリスト及び干渉候補リストに基づいて、低速周期演算部３からの又は前回周期の最適解を初期値として、最適化問題を高速周期で最適解を求解する。すなわち、低速周期演算部３は、タスクリスト及び干渉候補リストを更新して大域的にかつ低速周期で最適化問題を解く。同時に、高速周期演算部４はタスクリスト及び干渉候補リストを更新せずに低速周期演算部３の最適解または前回周期の最適解近傍から開始して効率的にかつ高速周期で最適化問題を解く。このように、低速周期演算部３及び高速周期演算部４が並列的に演算処理を行うことで、最適化ベースの逆運動学計算法で高速な制御周期を実現でき、タスクの追従性を良好にできる。

ここで、一般的な逆運動学問題について詳細に説明する。例えば、ロボットの手先の位置・姿勢を表すベクトルｒ∈Ｒ^ｍと関節角度ベクトルθ∈Ｒ^ｎとにおいて、下記式が成立する。
ｒ＝ｆ（θ） （１）

逆運動学問題は、上記式の逆問題である下記式を求める問題である。すなわち、逆運動学問題とは、「手先の位置・姿勢を満たす関節角を求める問題」と換言できる。
θ＝ｆ^−１（ｒ） （２）

例えば、７軸マニュピュレータの手先位置・姿勢制御では、ヤコビ行列のサイズｍ＝６、ｎ＝７となる。このように、ｍ＜ｎが成立するロボットは、冗長マニュピュレータもしくは冗長ロボットと呼ばれる。ｒに対するθが一意に決まらない。このため、何らかの指標を用いて上記逆運動学問題を解くこととなる。

上記（２）式は、非線形性が強く、通常、解析的に解くことは困難である。このため、上記（２）式の両辺を微分した下記（３）式を用いて関節角速度を算出し、この算出した関節角速度を積分することで数値的に上記（２）式を解くことが一般的である。

但し、ヤコビ行列は、下記（４）式で表すことができる。

以上、説明したように、（２）式に示す逆運動学問題は、上記（３）式を満たすような関節角速度を求めるという問題に帰着できる。

上記逆運動学問題について、例えば、図２に示すヒューマノイドロボットの左手先及び右足先の位置姿勢と重心位置とにタスクが設定する場合を説明する。ベースフレーム（例えば、ロボット１００の腰フレーム）から見た左手先及び右足先のヤコビアンＪ_{ｌｈａｎｄ}、Ｊ_{ｒｆｏｏｔ}、重心ヤコビアンＪ_ｃｏｇは、下記（５）式で表すことができる。但し、下記（５）式において、Ｐ_{ｌｈａｎｄ}は左手先の位置・姿勢、Ｐ_{ｒｆｏｏｔ}は右足先の位置・姿勢、Ｐ_ｃｏｇは重心位置、とする。また、Ｐ（ドット）_ｂａｓｅはベースフレームの位置・姿勢、θ（ドット）は全関節角速度を縦に並べた関節角速度ベクトル、とする。

なお、上記（５）において、ｑは、ロボット１００の関節角度やベースフレームの位置・姿勢などを示す姿勢情報である。ｑ（ドット）はそのｑの微分値であり、以下同様に各パラメータを表現する
上記（５）式は、上記（２）式と異なり、解ベクトルがθ（ドット）からｑ（ドット）に拡張されている。これは、ヒューマノイドロボット等のように、ベースフレームが空間上に固定されていないロボットで一般的に行われる処理であり、関節角度と同時にベースフレームの位置・姿勢を求めることができる。上記ロボットは、フローティングベースのロボットと称し、一方、産業用ロボットマニュピュレータ等はベースフレームが地面等に固定されている。本実施形態においては、上記フローティングベースのロボットを一例にして説明を行うが、これに限定されない。 本実施形態に係るロボット制御装置１は、ベースフレームが固定されたロボットにも適用可能である。

上記（５）式は、下記（６）式のように表現できる。

ここで、例えば、ロボット１００の関節数を３０とすると、ヤコビ行列のサイズはｍ＝３＋６＋６＝１５、ｎ＝６＋３０＝３６となる。この場合、かなり冗長性の高い逆運動学問題となることが分かる。ロボット１００が達成するタスク情報を含むタスクリストをＬ_ｔと定義する。位置３自由度の指定をＰ^ｘｙｚ、姿勢３自由度の指定をＲ^ｒｐｙとすると、タスクリストＬ_ｔは下記（７）のように表現できる。

なお、上記タスクリストは一例であり、これに限定されない。例えば、タスクリストＬ_ｔは、上記と異なるフォーマット形式や実装であってもよい。
上記（６）式の等式制約条件は、上記（７）式のタスクリストＬ_ｔに依存しており、下記（８）式のように表現できる。但し、ｆは任意の関数とする。
Ａ＝ｆ_Ａ（Ｌ_ｔ、ｑ）
ｂ＝ｆ_ｂ（Ｌ_ｔ、ｔ） （８）

したがって、タスクリストＬ_ｔを固定した場合、上記（６）式の等式制約条件は、下記（９）式のように表現できる。
Ａ＝ｆ_Ａ、Ｌｔ（ｑ）
ｂ＝ｆ_ｂ、Ｌｔ（ｔ） （９）

次に、関節角速度の不等式制約条件について、説明する。ロボット１００の動作を実際に決める際、ロボット１００が実行可能な動作に決める必要がある。例えば、ロボット１００の各関節は、その動作可能範囲がハードウェア的に決まっている。このため、その動作可能範囲内に関節角度が収まるように逆運動学を解く必要がある。また、腕が胴体と接触しないように制限するなどの自己干渉回避や、ロボット１００の各部が環境の障害物と接触しないように制限するなどの外界干渉回避を行う必要がある。このような制限を関節角速度の不等式制約条件として設定する。なお、この不等式制約条件は、本出願人が提出した特開２０１１−１８３５２７号公報に詳細に開示されており、これを援用できるものとし、以下、概略のみ説明する。

関節可動域（関節角度制限）と関節角速度制限は、下記（１０）式のように表現できる。但し、θ_{ｊ、ｍａｘ}はｊ関節の角度上限値、θ_{ｊ、ｍｉｎ}はｊ関節の角度下限値、θ（ドット）_{ｊ、ｍａｘ}はｊ関節の角速度上限値、θ（ドット）_{ｊ、ｍｉｎ}はｊ関節の角速度下限値、Ψ_{ｊ、ｍａｘ}はｊ関節の角度上限値で関節を止めるための猶予角度、Ψ_{ｊ、ｍｉｎ}はｊ関節の角度下限値で関節を止めるための猶予角度、である。

上記（１０）式は下記（１１）式のように表現できる。

上記、θ_{ｊ、ｍａｘ}、θ_{ｊ、ｍｉｎ}、θ（ドット）_{ｊ、ｍａｘ}、θ（ドット）_{ｊ、ｍｉｎ}、Ψ_{ｊ、ｍａｘ}、Ψ_{ｊ、ｍｉｎ}を固定すると、上記（１１）式の係数は下記（１２）式のように表現できる。
Ｃ_ｊｎｔ＝ｃｏｎｓｔ
ｇ_ｊｎｔ＝ｆ_ｇjnt（ｑ） （１２）

自己干渉回避および外界干渉回避について説明する。本実施形態において、例えば、Faverjonらによって提案されたvelocity damperを用いて上記干渉回避を行う。velocity damperは下記（１３）式に示すように、２つの物体間の距離に応じて速度に制約を設けるものである。下記（１３）式において、ｄは２つの物体間の最短距離、ｄ_ｉはvelocity damperが有効になり始める距離、ｄ_ｓは物体間に確保される最小距離、ζは減速度合いを調整する正パラメータ、である（図３）。

上記（１３）式に示すように、ｄがｄ_ｉより小さくなると徐々に減速し始め、ｄ＝ｄ_ｓのときに完全に停止するような拘束条件となっている。したがって、ｄがｄ_ｓより小さくなることは無く、干渉回避が達成される。

物体間の最近点をｐ_１、ｐ_２とし、最近点同士を結んだ方向の単位ベクトルをｎとすると上記（１３）式は、下記（１４）式のように表すことができる。但し、下記（１４）式において、Ｊ_ｐ１、Ｊ_ｐ２は、ｐ_１、ｐ_２のヤコビアンである。

以上のようにして、干渉回避の条件は、下記（１５）式に示す線形の不等式制約条件で表すことができる。

上記（１４）式を計算するためには、順運動学を行ってロボット１００の全リンクの位置・姿勢を更新した上で、各リンク間の最短距離を求めることとなる。このとき、全リンクを組合せて、夫々の最短距離の計算した場合、その計算コストが非常に高くなる。これに対し、本実施形態において、後述の如く、低速周期演算部３は、低速周期で、全リンクの組合せの最短距離を算出し、その干渉判定し回避候補を選定し、干渉候補リストＬ_ｃを更新する。そして、高速周期演算部４は、高速周期で、低速周期演算部３により更新された干渉候補リストＬ_ｃの回避候補についてのみ、干渉回避の計算を行う。

次に、干渉候補リストＬ_ｃをについて詳細に説明する。例えば、ロボット１００の右肘と胴体、左肘と環境（テーブル）が接近して干渉候補リストＬ_ｃに挙がったとすると、干渉候補リストＬ_ｃを下記（１６）式のように表すことができる。
Ｌ_ｃ＝［Ｓ_{ｒｅｌｂｏｗ}×Ｓ_ｂｏｄｙ、Ｓ_{ｌｅｌｂｏｗ}×Ｓ_ｂｏｄｙ］ （１６）

なお、干渉候補リストＬ_ｃは様々な方法で記述可能である。ここでは、Ｓは対象となるフレームのＩＤや干渉判定用のシェープ等、干渉判定に必要な情報を含んでいるものとする。上記（１６）式は、干渉候補リストＬ_ｃに格納されている要素ペアのみを干渉判定するため、上記（１６）式の各係数は、下記（１７）式のように表すことができる。

干渉候補リストＬ_ｃを固定した場合、上記（１６）式の各係数は、下記（１８）式のように表すことができる。

最終的に、上記（１２）式、（１７）式及び（１８）式をまとめて、１つの線形不等式制約条件として下記（１９）式及び（２０）式のように表すことができる。

なお、干渉候補リストＬ_ｃを固定した場合、上記（１９）式の係数Ｃ、ｇは、下記（２１）式のように表すことができる。

以上の定式化を用いて最適化問題を構成する。本実施形態に係る逆運動学問題は、上記（６）式および（１９）式に基づいて、下記（２２）式に示す凸２次計画問題に帰着することとなる。

図４は、本実施形態に係る低速周期演算部及び高速周期演算部の演算処理を示すブロック図である。高速周期演算部４は、低速周期演算部３で算出された最適解、あるいは、前回周期において算出された最適解を今回周期の初期値とする、所謂ウォームスターティングを用いて上記（２２）式の最適化問題を高速に解く。以下、そのウォームスターティングを用いる理由について説明する。

一般に目標軌道（例えば、Ｐ_ｃｏｇ、Ｐ_{ｌｈａｎｄ}、Ｐ_{ｒｆｏｏｔ}）は、滑らかな連続軌道となる。このため、制御周期が十分に短い場合、制御周期毎に求解する最適化問題（上記（１５）式）は、隣合うサンプリング間でそれ程大きく変化しない。すなわち、上記（９）式のｆ_ｂ、Ｌｔ（ｔ）は通常十分な滑らかさを有しており、サンプリング間隔が短ければ、ｂは漸次的にしか変化しない。

同様に、ロボット１００の関節角度やベースフレームの位置・姿勢を示すｑも、滑らかに動作する。このため、制御周期が十分に短い場合、隣合うサンプリング点で、その最適化問題の解は近い値となる。そして、干渉判定のシェープが適切に設定されていれば、上記（９）式及び（２１）式におけるｆ_Ａ、_Ｌｔ（ｑ）、ｆ_Ｃ、Ｌｃ（ｑ）、ｆ_ｇ、Ｌｃ（ｑ）は滑らかな関数となる。このため、Ａ、Ｃ、及びｇも漸次的にしか変化しない。

以上により、等式制約条件が上記（９）式で表される（タスクリストＬ_ｔが変更されない）、および、不等式制約条件が上記（２１）式で表される（干渉候補リストＬ_ｃが変更されない）限り、上記（２２）式に示す最適化問題は滑らかに少しずつしか変化しない、ことが分かる。この場合、前回周期において算出した最適解を初期値に用いる、あるいは、前回の最適解求解時に有効になっていた等式制約条件及び不等式制約条件の情報を用いる、などの既知の情報を利用する上記ウォームアップスターティングが有効となる。これにより、非常に高速に最適化問題の解を求めることができる。なお、上記有効になっている等式および不等式制約条件とは、等式および不等式制約条件のうちで、求められた最適解が制約にぶつかった等式及び不等式制約条件を指す。

したがって、本実施形態に係る高速周期演算部４は、タスクリストＬ_ｔおよび干渉候補リストＬ_ｃが変更されない場合、ウォームスターティングを用いて演算処理を行い、上記（２２）式の最適化問題を効率的に解くのが好ましい。なお、高速周期演算部４は、ウォームスターティングだけでなく、既知の情報を利用した、例えば、パラメトリック法等の他の手法を用いて演算処理を行ってもよい。

一方で、低速周期演算部３は、任意の初期値から実時間で収束解に至るような求解法を用いて最適化問題を解く。以下、その求解法を用いる理由について説明する。

タスクリストＬ_ｔや干渉候補リストＬ_ｃが変更された場合、等式制約条件が上記（８）式で表される（タスクリストＬ_ｔが変更される）、又は、不等式制約条件が上記（２０）式で表される（干渉候補リストＬ_ｃが変更される）場合、前回周期の最適解、有効な等式および不等式制約条件などの既知の情報を利用することができない。例えば、タスクリストＬ_ｔが変更された場合、サイズも含めて、上記（８）式のＡ及びｂは大きく変化する可能性がある。このため、タスクリストＬ_ｔの変更前に算出された最適解がタスクリストＬ_ｔの変更後に算出される最適解の近傍にあるという可能性は極めて低い。また、干渉候補リストＬ_ｃに関しても、環境センサ２で外界環境を観測した際に、環境センサ２の視野角や死角の関係で、今まで検出されなった壁や障害物などの物体等がロボット１００自身の移動によって新たに検出されることが起こり得る。例えば、ロボット１００自身の直ぐ隣にテーブルが新たに検出されるような場合、上記（２０）式のＣ及びｇは大きく変化する可能性がある。この場合も、前回周期で求解した最適解や有効な等式および不等式制約条件を、利用することはできない。

したがって、このようにタスクリストＬ_ｔや干渉候補リストＬ_ｃが変更された場合、前回周期で求解した最適解や有効な等式および不等式制約条件などの、既知の情報を用いずに、上記（２２）式の最適化問題を大域的に解く必要がある。この場合、最適化問題を解くのに多少の時間を要したとしても、確実に、任意の初期値から実時間で収束解に至るような求解法が好ましい。そのような求解法として、例えば、内点法が有効的である。なお、凸２次計画問題の求解法として、一般的に、「アクティブセット法（有効制約法）」と上記「内点法」が知られている。

以上により、本実施形態において、高速周期演算部４は、「既知の情報を用いた高速求解」を目的として、例えば、ウォームスターティング付きのアクティブセット法を用いて、高速周期の最適化演算を効率的に行う。同時に、低速周期演算部３は、「実時間で必ず収束解を得る」を目的として、例えば、「内点法（より詳細にはインフィージブル内点法）」を用いて、低速周期の最適化演算を行う。

次に、本実施形態に係る低速周期演算部による上記低速周期の最適化演算方法のフローについて詳細に説明する。図５は、本実施形態に係る低速周期演算部による低速周期の最適化演算方法のフローを示すフローチャートである。なお、低速周期演算部３は、図５に示す制御処理を所定の微小時間毎に繰返し実行する。

低速周期演算部３は、高速周期演算部４からフィードバックされる、現状の姿勢情報ｑを取得する（ステップＳ１０１）。

低速周期演算部３は、取得した姿勢情報ｑに基づいて、順運動学演算を行い全身フレーム情報（全フレームの位置・姿勢など）の更新ＦＫ（ｑ）を行う（ステップＳ１０２）。 環境センサ２は、ロボット１００周囲の環境情報（例えば、障害物の位置・姿勢、形状などの情報）を取得し（ステップＳ１０３）、低速周期演算部３に出力する。

低速周期演算部３は、上位コントローラから設定タスクを読み込み、タスクリストＬ_ｔを更新する（ステップＳ１０４）。低速周期演算部３は、更新したタスクリストＬ_ｔと、高速周期演算部４からフィードバックされる姿勢情報ｑと、上位コントローラから出力されるタスクの目標値Ｐ_ｔａｓｋ（ｔ）の微分値と、に基づいて、等式制約条件（Ａ^ｓ＝ｆ_Ａ（Ｌ_ｔ、ｑ）、ｂ^ｓ＝ｆ_ｂ（Ｌ_ｔ、ｔ＋Δｔ^ｓ））を設定する（ステップＳ１０５）。タスクの目標値は、例えば、低速周期演算部３の周期分だけ未来の目標値を用いる。また、上位コントローラは、例えば、ロボット１００の軌道を生成する軌道生成部である。

低速周期演算部３は、設定されている干渉候補リストＬ_ｃをリセットする。その後、低速周期演算部３は、更新した全身フレーム情報ＦＫ（ｑ）と、環境センサ２から出力される環境情報と、に基づいて、全リンクを組み合わせて夫々の最短距離を算出する。そして、低速周期演算部３は、例えば、算出した最短距離のうち閾値以下となる最短距離のリンクの組合せ（ロボット１００が環境と接触する候補）を、干渉候補リストＬ_ｃに加えることで、干渉候補リストＬ_ｃを更新する（ステップＳ１０６）。

低速周期演算部３は、更新した干渉候補リストＬ_ｃと、取得した姿勢情報ｑと、に基づいて、不等式制約条件（Ｃ^ｓ＝ｆ_ｃ（Ｌ_ｃ、ｑ）、ｇ^ｓ＝ｆ_ｇ（Ｌ_ｃ、ｑ））を設定する（ステップＳ１０７）。

低速周期演算部３は、設定した等式制約条件と不等式制約条件とに基づいて、内点法を用いて、凸２次計画問題を解き最適解ｑ（ドット）^ｓを算出する（ステップＳ１０８）。 低速周期演算部３は、算出した最適解ｑ（ドット）^ｓと、対応する干渉候補リストＬ_ｃ及びタスクリストＬ_ｔと、を後述のデータ授受を行うためのパイプ（記憶部の一具体例であり、メモリなど）に書き込む（ステップＳ１０９）。

ここで、上記低速周期演算部３で算出された最適解ｑ（ドット）^ｓは、関節角度指令値を算出するための逆運動学演算に直接使用されるのではなく、高速周期演算部４が行う最適化演算の初期値として使用される。上述の如く、高速周期演算部４において、「最適化問題が連続的にしか変化しない」という前提下で、ウォームスターティング手法を用いて高速な最適化演算を実現している。したがって、タスクリストＬ_ｔや干渉候補リストＬ_ｃが変更された場合、上記、「最適化問題が連続的にしか変化しない」という前提が成立しない。

したがって、本実施形態において、低速周期演算部３が干渉候補リストＬ_ｃ及びタスクリストＬ_ｔの更新を行い、凸２次計画問題の最適解を算出する。そして、高速周期演算部４は、低速周期演算部３で算出した 凸２次計画問題の最適解をウォームスターティングに利用する。

なお、低速周期演算部３は、上述の如く、全リンクを組合せて最短距離を計算するなどの計算コストの高い演算を行う必要がある。したがって、低速周期演算部３の周期は、これら計算コストの高い演算が完了できる程度に長く設定されている。また、低速周期演算部３は、環境センサ２からの環境情報に基づいて環境との干渉を回避するように不等式制約条件の設定を行っている。この為、低速周期演算部３は、環境センサ２の計測周期と同期している。これにより、常に最新の観測結果を干渉候補リストＬ_ｃに反映させることができる。このように、低速周期演算部３の周期は、例えば、全体の演算量、ＣＰＵのパワー、環境センサ２の計測周期などを考慮して決定される。

次に、本実施形態に係る高速周期演算部による上記高速周期の最適化演算方法のフローについて詳細に説明する。図６は、本実施形態に係る高速周期演算部による高速周期の最適化演算方法のフローを示すフローチャートである。なお、高速周期演算部４は、図６に示す制御処理を所定の微小時間毎に繰返し実行する。
高速周期演算部４は、低速周期演算部３のパイプを参照し、パイプに、最適解ｑ（ドット）^ｓと、対応する干渉候補リストＬ_ｃ及びタスクリストＬ_ｔと、の情報が存在するか否かを確認する（ステップＳ２０１）。

高速周期演算部４は、低速周期演算部３のパイプに情報が存在すると判断すると（ステップＳ２０１のＹＥＳ）、パイプから、その最適解ｑ（ドット）^ｓを読込み、読込んだ最適解を最適化問題の初期値ｑ（ドット）_０に設定する（ステップＳ２０２）。高速周期演算部４は、対応する干渉候補リストＬ_ｃ及びタスクリストＬ_ｔをパイプから読込み、高速周期側の干渉候補リストＬ_ｃ ^＊及びタスクリストＬ_ｔ ^＊に設定する（ステップＳ２０３）。

一方、高速周期演算部４は、低速周期演算部３のパイプに情報が存在しないと判断すると（ステップＳ２０１のＮＯ）、バッファ（メモリなど）から、前回周期の最適解、干渉候補リストＬ_ｃ及びタスクリストＬ_ｔを読み出す。そして、高速周期演算部４は、読み出した前回周期の最適解を、今回周期の最適化演算の初期値に設定する（ステップＳ２０４）。高速周期演算部４は、読み出した前回周期の干渉候補リストＬ_ｃ及びタスクリストＬ_ｔを今回周期最適化演算の干渉候補リストＬ_ｃ及びタスクリストＬ_ｔに設定する。

例えば、低速周期演算部３の制御周期が１５ｍｓであり、高速周期演算部４の制御周期が１ｍｓである場合、パイプには１５制御周期毎に情報が存在することとなる。この場合、高速周期演算部４は、１５制御周期のうち、１制御周期においてパイプの情報に基づいて最適化問題を解き、１４制御周期において前回制御周期の情報に基づいて最適化問題を解くこととなる。

高速周期演算部４は、設定したタスクリストＬ_ｔ ^＊と、フィードバックされた姿勢情報ｑと、上位コントローラから出力されるタスクの目標値Ｐ_ｔａｓｋ（ｔ）の微分値と、に基づいて、等式制約条件（Ａ^ｆ＝ｆ_{Ａ、Ｌｔ＊}（ｑ）、ｂ^ｆ＝ｆ_{ｂ、Ｌｔ＊}（ｔ））を設定する（ステップＳ２０５）。

高速周期演算部４は、設定した干渉候補リストＬ_ｃ ^＊と、フィードバックされた姿勢情報ｑと、に基づいて、不等式制約条件（Ｃ^ｆ＝ｆ_{ｃ、Ｌｃ＊}（ｑ）、ｇ^ｆ＝ｆ_{ｇ、Ｌｃ＊}（ｑ））を設定する（ステップＳ２０６）。

高速周期演算部４は、設定した等式制約条件と不等式制約条件とに基づいて、ウォームスターティング付きアクティブセット法を用いて、凸２次計画問題を解き最適解ｑ（ドット）^ｆを算出する（ステップＳ２０７）。

制御部６は、高速周期演算部４により算出された最適解ｑ（ドット）^ｆを積分し、各関節角度ｑを算出する。制御部６は、算出した関節角度ｑに基づいて、下記式（２３）式により、各関節部に対する関節角度指令値ｑ_ｒｅｆを算出する（ステップＳ２０８）。

制御部６を算出した関節角度指令値ｑ_ｒｅｆを各関節部のサーボアンプに出力する。サーボアンプは、制御部６からの関節角度指令値ｑ_ｒｅｆに応じた制御電流を各関節部のアクチュエータ５に出力することで、各関節部を駆動する。高速周期演算部４は、上記算出した最適解（収束解）ｑ（ドット）^ｆと対応する干渉候補リストＬ_ｃ及びタスクリストＬ_ｔをバッファに書き込む（ステップＳ２０９）。なお、低速周期演算部３及び高速周期演算部４は、上記凸２次計画問題の最適解ｑ（ドット）を求解しているが、これに限定されず、例えば、最適解ｑ（２ドット）を求解してもよい。

以上、本実施形態に係るロボット制御装置１において、低速周期演算部３は、更新したタスクリストとロボット１００の姿勢情報とタスクの目標値とに基づいて、等式制約条件を設定し、干渉候補リストを、ロボット１００の姿勢情報と環境センサ２からの環境情報とに基づいて更新し、該更新した干渉候補リストとロボット１００の姿勢情報とに基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、最適化問題を、低速周期で求解し最適解を算出する。同時に、高速周期演算部４は、ロボット１００の姿勢情報と、タスクの目標値と、低速周期演算部３からのタスクリスト又は前回周期のタスクリストと、に基づいて等式制約条件を設定し、ロボット１００の姿勢情報と、タスクの目標値と、低速周期演算部３からの干渉候補リスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、低速周期演算部３により算出された最適解又は前回周期の最適解を初期値として最適化問題を、高速周期で求解し、最適解を算出し、該算出した最適解に基づいてロボット１００の姿勢情報を算出する。制御部６は、高速周期演算部４により算出された姿勢情報に基づいてロボット１００を制御する。

これにより、低速周期演算部３は、タスクリスト及び干渉候補リストを更新して大域的にかつ低速周期で最適化問題を解くことができる。同時に、高速周期演算部４はタスクリスト及び干渉候補リストを更新せずに低速周期演算部３の最適解または前回周期の最適解近傍から開始して効率的にかつ高速周期で最適化問題を解くことができる。したがって、高速な制御周期が実現できつつ、タスクの追従性が良好にできる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図５及び図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ ロボット制御装置、２ 環境センサ、３ 低速周期演算部、４ 高速周期演算部、５ アクチュエータ、６ 制御部

Claims (7)

1. ロボット周囲の環境情報を取得する環境情報取得手段と、
   前記ロボットが達成するタスク情報を含むタスクリストを更新し、該更新したタスクリストと、前記ロボットの姿勢情報と、前記ロボットが実行するタスクの目標値と、に基づいて、等式制約条件を設定し、前記ロボットが環境と接触する候補を含む干渉候補リストを、前記ロボットの姿勢情報と、前記環境情報取得手段により取得された環境情報と、に基づいて更新し、該更新した干渉候補リストと、前記ロボットの姿勢情報と、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記ロボットの逆運動学の最適化問題を、第１周期で求解し、該最適化問題の最適解を算出する低速周期演算手段と、
   前記ロボットの姿勢情報と、前記タスクの目標値と、前記低速周期演算手段からのタスクリスト又は前回周期のタスクリストと、に基づいて等式制約条件を設定し、前記ロボットの姿勢情報と、前記低速周期演算手段からの干渉候補リスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記低速周期演算手段により算出された最適解又は前回周期の最適解を初期値として前記ロボットの逆運動学の最適化問題を、前記第１周期より高速の第２周期で求解し、前記最適化問題の最適解を算出し、該算出した最適解に基づいて前記ロボットの姿勢情報を算出する高速周期演算手段と、
   前記高速周期演算手段により算出された姿勢情報に基づいて前記ロボットを制御する制御手段と、
   を備えることを特徴とするロボット制御装置。
2. 請求項１記載のロボット制御装置であって、
   前記低速周期演算手段は、前記求解した最適化問題の最適解と、対応する前記タスクリスト及び干渉候補リストと、を記憶する記憶部を有し、
   前記高速周期演算手段は、
   前記記憶部に前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、が存在するとき、前記記憶部から読み込んだ前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、に基づいて、前記最適化問題を求解し、
   前記記憶部に前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、が存在しないとき、前回周期の前記最適化問題の最適解と、前記対応するタスクリスト及び干渉候補リストと、に基づいて、前記最適化問題を求解する、
   ことを特徴とするロボット制御装置。
3. 請求項１又は２記載のロボット制御装置であって、
   前記高速周期演算手段は、ウォームスターティング付きのアクティブセット法を用いて、前記最適化問題を求解し、
   前記低速周期演算手段は、内点法を用いて前記最適化問題を求解する、
   ことを特徴とするロボット制御装置。
4. 請求項１乃至３のうちいずれか１項記載のロボット制御装置であって、
   前記ロボットの姿勢情報は、現状の前記ロボットのベースフレームの位置姿勢と、前記ロボットの関節角度と、を含む、ことを特徴とするロボット制御装置。
5. 請求項１乃至４のうちいずれか１項記載のロボット制御装置であって、
   前記最適化問題の最適解は、前記ロボットの姿勢情報の微分値である、ことを特徴とするロボット制御装置。
6. ロボット周囲の環境情報を取得するステップと、
   前記ロボットが達成するタスク情報を含むタスクリストを更新し、該更新したタスクリストと、前記ロボットの姿勢情報と、前記ロボットが実行するタスクの目標値と、に基づいて、等式制約条件を設定し、前記ロボットが環境と接触する候補を含む干渉候補リストを、前記ロボットの姿勢情報と、前記取得された環境情報と、に基づいて更新し、該更新した干渉候補リストと、前記ロボットの姿勢情報と、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記ロボットの逆運動学の最適化問題を、第１周期で求解し、該最適化問題の最適解を算出するステップと、
   前記ロボットの姿勢情報と、前記タスクの目標値と、前記設定されたタスクリスト又は前回周期のタスクリストと、に基づいて等式制約条件を設定し、前記ロボットの姿勢情報と、前記設定された干渉候補リスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記算出された最適解又は前回周期の最適解を初期値として前記ロボットの逆運動学の最適化問題を、前記第１周期より高速の第２周期で求解し、前記最適化問題の最適解を算出し、該算出した最適解に基づいて前記ロボットの姿勢情報を算出するステップと、
   前記算出された姿勢情報に基づいて前記ロボットを制御するステップと、
   を含む、ことを特徴とする制御方法。
7. ロボットが達成するタスク情報を含むタスクリストを更新し、該更新したタスクリストと、前記ロボットの姿勢情報と、前記ロボットが実行するタスクの目標値と、に基づいて、等式制約条件を設定し、前記ロボットが環境と接触する候補を含む干渉候補リストを、前記ロボットの姿勢情報と、取得された前記ロボット周囲の環境情報と、に基づいて更新し、該更新した干渉候補リストと、前記ロボットの姿勢情報と、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記ロボットの逆運動学の最適化問題を、第１周期で求解し、該最適化問題の最適解を算出する処理と、
   前記ロボットの姿勢情報と、前記タスクの目標値と、前記設定されたタスクリスト又は前回周期のタスクリストと、に基づいて等式制約条件を設定し、前記ロボットの姿勢情報と、前記設定された干渉候補リスト又は前回周期の干渉候補リストと、に基づいて不等式制約条件を設定し、該設定した等式制約条件及び不等式制約条件に基づいて、前記算出された最適解又は前回周期の最適解を初期値として前記ロボットの逆運動学の最適化問題を、前記第１周期より高速の第２周期で求解し、前記最適化問題の最適解を算出し、該算出した最適解に基づいて前記ロボットの姿勢情報を算出する処理と、
   前記算出された姿勢情報に基づいて前記ロボットを制御する処理と、
   をコンピュータに実行させる、ことを特徴とする制御プログラム。

Images