(57)【要約】【課題】位置・加速度制約を満たし、かつ位置・速度・加速度が連続な軌道を高速生成する。【解決手段】サンプリング点の時間と始点・終端点の位置・速度・加速度を用いて、軌道生成方法では、始点・終端点を通過し、かつ始点・終端点での位置・速度・加速度が連続な軌道となる等式制約条件を計算する。進入禁止領域の存在範囲外に位置するように設定される各サンプリング点での制約位置と、所定の加速度の制限範囲と、を用いて、各サンプリング点での軌道上の位置が、始点から終端点へと至る経路上に存在する侵入禁止領域を回避する位置となり、かつ、各サンプリング点での軌道上の加速度が、所定の加速度の制限範囲内になる軌道となる不等式制約条件を計算する。等式制約条件、不等式制約条件、目的関数、からなる線形計画問題を構成して求解し、その結果を用いて多項式補間の補間係数を計算する。【選択図】図３
2013184233.tif 000002
【特許請求の範囲】
【請求項１】
ロボットの部位を始点から終端点へと移動させるための軌道を多項式補間を用いて生成する軌道生成方法であって、
前記始点および前記終端点を通過し、かつ、前記始点および前記終端点それぞれでの位置・速度・加速度が連続な軌道となることを制約する条件を等式制約条件として設定する等式制約条件設定ステップと、
前記始点から前記終端点に到達する時間をサンプリング時間単位で分割して、サンプリング点を設定するサンプリング点設定ステップと、
前記サンプリング点の時間と、設定される前記始点および前記終端点それぞれでの位置・速度・加速度と、を用いて、前記等式制約条件を計算する等式制約条件計算ステップと、
各前記サンプリング点での軌道上の位置が、前記始点から前記終端点へと至る経路上に存在する侵入禁止領域を回避する位置となり、かつ、各前記サンプリング点での軌道上の加速度が、所定の加速度の制限範囲内になる軌道となることを制約する条件を不等式制約条件として設定する不等式制約条件設定ステップと、
前記進入禁止領域の存在範囲外に位置するように設定される各前記サンプリング点での制約位置と、設定される前記所定の加速度の制限範囲と、を用いて、前記不等式制約条件を計算する不等式制約条件計算ステップと、
生成される軌道と目的軌道の間での評価関数を目的関数として設定する目的関数設定ステップと、
計算した前記等式制約条件と、計算した前記不等式制約条件と、前記目的関数と、を用いて線形計画問題を構成し、当該線形計画問題を求解する線形計画問題求解ステップと、
前記線形計画問題での求解結果を用いて、前記多項式補間の補間係数を計算する補間係数計算ステップと、
を備えるロボットの軌道生成方法。
【請求項２】
前記始点から前記終端点へと向かう方向をｘ軸方向とし、当該ｘ軸方向に垂直な方向をｙ軸方向とし、
前記ｘ軸方向において前記始点から前記終端点へと至る位置・速度・加速度が連続な軌道を設定するｘ軸方向軌道設定ステップを更に備え、
前記等式制約条件計算ステップでは、前記サンプリング点の時間と、設定される前記始点および前記終端点それぞれでの前記ｙ軸方向における位置・速度・加速度と、を用いて、前記等式制約条件を計算し、
前記不等式制約条件計算ステップでは、前記進入禁止領域の存在範囲外に位置するように設定される各前記サンプリング点での前記ｙ軸方向における制約位置と、設定される前記所定の加速度の前記ｙ軸方向における制限範囲と、を用いて、前記不等式制約条件を計算する、
請求項１に記載のロボットの軌道生成方法。
【請求項３】
各前記サンプリング点での前記ｘ軸上の値と、設定される前記進入禁止領域の存在範囲と、を用いて、前記サンプリング点での前記ｙ軸方向における制約位置を設定する制約位置設定ステップを更に備える、
請求項２に記載のロボットの軌道生成方法。
【請求項４】
前記目的関数設定ステップでは、生成される軌道と前記始点から前記終端点を結ぶ直線軌道の間での評価関数を目的関数として設定する、
請求項１〜３いずれか１項に記載のロボットの軌道生成方法。
【請求項５】
前記ロボットは、アームを備えるアームロボットであり、当該アームの手先位置の軌道を生成する、
請求項１〜４いずれか１項に記載のロボットの軌道生成方法。
【請求項６】
前記請求項１〜５のいずれか１項のロボットの軌道生成方法を実行するコントローラを備えるロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボットおよびその軌道生成方法に関し、特に、位置および加速度の制約を満たし、かつ、位置・速度・加速度が連続な軌道を高速に生成することができるロボットおよびその軌道生成方法に関する。
【背景技術】
【０００２】
ロボットの軌道生成手法として様々な手法が提案されている。位置制約（障害物回避・自己干渉回避・特異点回避など）を満たす軌道を生成する手法としては、例えばポテンシャル法がよく知られている。ポテンシャル法では、例えば始点から終端点までの軌道を生成するような場合には、始点と終端点を結んだ直線に近づくようなポテンシャルと、位置制約境界から遠ざかるようなポテンシャルとを設定して軌道計画を行い、これにより、位置制約を満たす軌道を生成する。また、ポテンシャル法以外にも、確率的ロードマップ法やＲＲＴなどを用いた探索的な手法が数多く知られている。
【０００３】
ところで、ロボットがサービスロボットである場合には、加速度制約を満たす軌道の生成が必要とされることがある。例えば、サービスロボットの手先等の軌道を生成する場合には、手先の加速度を制約範囲内に抑えた軌道を生成することが求められることがある。以下、図１を参照してこの状況を具体的に説明する。
【０００４】
図１に例示するようにロボット１０の手先１１を動かす場合、その手先軌道が満たすべき最低限の条件は、「位置、速度、加速度が連続な軌道となる」ことである。このような軌道は、例えば多項式補間を用いて始点と終端点を結ぶことで生成できる（同図で、点線を用いて示す軌道）。ここで例えば、ロボット１０が運んでいる対象物体が水の入ったコップ１２であり、さらに、コップ１２を移動させるテーブル１３の上には障害物１４が置いてあったとものする。このような状況では、手先軌道が満たすべき条件として、さらに、（１）障害物にぶつからないように位置の制約を満たすこと、（２）加速度が過大にならないように制約内に収めること、という２つの条件が加わる。１つ目の条件は、障害物を回避するように設定される手先位置の制約を満たすという条件であり、図１で例えば実線を用いて示すような軌道を描くことになる。また、２つ目の条件は、加速度を制約することを意味しており、これはお盆１５の上の水がこぼれないようにすることを目的とするものである。
【０００５】
さらに、本発明に関連する技術として、例えば特許文献１がある。特許文献１は、位置決め制御点を通過するように連続した関数を用いて位置の補間を行い、この補間位置に基づいて軌道を生成するものである。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平０９−２６５３１１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上述した探索的な手法では、ロボットの全リンクの干渉回避等といったより複雑な環境での軌道生成に拡張できるという長所を有するものの、計算コストが高いという問題がある。また、上述した探索的な手法に対して加速度制約を加えた場合には、軌道生成の求解にさらに多くの時間を必要とすることになり、オンラインでの高速な軌道生成には不向きであるという問題がある。
【０００８】
サービスロボットが実環境で遭遇する干渉回避の場面としては、「周りに障害物を囲まれて全リンクを精密に制御していかないと干渉してしまう」というような場面はそれほど多くなく、「テーブルの上の物体を避けながら他の物体を取るあるいは置く」などのより単純化された干渉回避の場面であることが多い。このような単純な場面では、複雑な干渉回避を探索的に計算することよりも、むしろ、オンラインで高速かつ信頼性高い軌道を求解できる軌道生成手法が求められる。また、例えばオンライン動作中に障害物の位置が動いたり、ビジュアルフィードバックで手先の位置を修正したりするような場合には、現在の位置・速度・加速度から連続するような軌道を新たに作り直して接続する必要がある。このため、位置・速度・加速度が連続な干渉回避軌道を、高速に生成可能な軌道生成手法が強く求められている。
【０００９】
一方で、例えば特許文献１の軌道生成手法によれば、高速に軌道生成は可能であるものの、生成された軌道を用いると加速度の変化が大きくなってしまうことがある。このため、加速度制約を満たすことができずに、例えば物を運搬するアームなどの軌道としては用いることができないという問題がある。
【００１０】
本発明は、上述した問題点を解決するためになされたものであり、線形計画法とスプライン補間法を組み合わせることで、位置および加速度の制約を満たし、かつ、位置・速度・加速度が連続な軌道を高速に生成することができるロボットおよびその軌道生成方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明に係るロボットの軌道生成方法は、ロボットの部位を始点から終端点へと移動させるための軌道を多項式補間を用いて生成する軌道生成方法であって、前記始点および前記終端点を通過し、かつ、前記始点および前記終端点それぞれでの位置・速度・加速度が連続な軌道となることを制約する条件を等式制約条件として設定する等式制約条件設定ステップと、前記始点から前記終端点に到達する時間をサンプリング時間単位で分割して、サンプリング点を設定するサンプリング点設定ステップと、前記サンプリング点の時間と、設定される前記始点および前記終端点それぞれでの位置・速度・加速度と、を用いて、前記等式制約条件を計算する等式制約条件計算ステップと、各前記サンプリング点での軌道上の位置が、前記始点から前記終端点へと至る経路上に存在する侵入禁止領域を回避する位置となり、かつ、各前記サンプリング点での軌道上の加速度が、所定の加速度の制限範囲内になる軌道となることを制約する条件を不等式制約条件として設定する不等式制約条件設定ステップと、前記進入禁止領域の存在範囲外に位置するように設定される各前記サンプリング点での制約位置と、設定される前記所定の加速度の制限範囲と、を用いて、前記不等式制約条件を計算する不等式制約条件計算ステップと、生成される軌道と目的軌道の間での評価関数を目的関数として設定する目的関数設定ステップと、計算した前記等式制約条件と、計算した前記不等式制約条件と、前記目的関数と、を用いて線形計画問題を構成し、当該線形計画問題を求解する線形計画問題求解ステップと、前記線形計画問題での求解結果を用いて、前記多項式補間の補間係数を計算する補間係数計算ステップと、
を備えるものである。これにより、位置および加速度の制約を満たし、かつ、位置・速度・加速度が連続な軌道を高速に生成することができる。
【００１２】
また、前記始点から前記終端点へと向かう方向をｘ軸方向とし、当該ｘ軸方向に垂直な方向をｙ軸方向とし、前記ｘ軸方向において前記始点から前記終端点へと至る位置・速度・加速度が連続な軌道を設定するｘ軸方向軌道設定ステップを更に備え、前記等式制約条件計算ステップでは、前記サンプリング点の時間と、設定される前記始点および前記終端点それぞれでの前記ｙ軸方向における位置・速度・加速度と、を用いて、前記等式制約条件を計算し、前記不等式制約条件計算ステップでは、前記進入禁止領域の存在範囲外に位置するように設定される各前記サンプリング点での前記ｙ軸方向における制約位置と、設定される前記所定の加速度の前記ｙ軸方向における制限範囲と、を用いて、前記不等式制約条件を計算するとよい。
【００１３】
さらにまた、各前記サンプリング点での前記ｘ軸上の値と、設定される前記進入禁止領域の存在範囲と、を用いて、前記サンプリング点での前記ｙ軸方向における制約位置を設定する制約位置設定ステップを更に備えるとよい。
【００１４】
また、前記目的関数設定ステップでは、生成される軌道と前記始点から前記終端点を結ぶ直線軌道の間での評価関数を目的関数として設定するとよい。
【００１５】
さらにまた、前記ロボットは、アームを備えるアームロボットであり、当該アームの手先位置の軌道を生成するとよい。
【００１６】
本発明に係るロボットは、上述のロボットの軌道生成方法を実行するコントローラを備えるものである。これにより、位置および加速度の制約を満たし、かつ、位置・速度・加速度が連続な軌道を高速に生成することができる。
【発明の効果】
【００１７】
本発明によれば、本発明はロボットおよびその軌道生成方法に関し、特に、位置および加速度の制約を満たし、かつ、位置・速度・加速度が連続な軌道を高速に生成することができるロボットおよびその軌道生成方法を提供することができる。
【図面の簡単な説明】
【００１８】
【図１】本発明の課題を説明するための図である。
【図２】実施の形態１に係るアームロボットの模式図である。
【図３】実施の形態１に係る軌道生成処理を示すフローチャートである。
【図４】実施の形態１に係るｘ方向の軌道の例を説明するためのグラフである。
【図５】実施の形態１に係る障害物を説明するための図である。
【図６】実施の形態１に係るｙ方向の制約を説明するための図である。
【図７】実施の形態１に係るｙ方向の位置の制約を説明するための図である。
【図８】単純なスプライン補間を用いた場合の位置軌道の例を説明するグラフ。
【図９】図８に示した軌道での加速度の変化を説明するグラフである。
【図１０】実施の形態１に係る位置軌道の例を説明するグラフである。
【図１１】図１０に示した軌道での速度軌道の例を説明するグラフである。
【図１２】図１０に示した軌道での加速度軌道の例を説明するグラフである。
【図１３】実施の形態１に係る位置軌道の例を説明するグラフである。
【図１４】図１３に示した軌道での速度軌道の例を説明するグラフである。
【図１５】図１３に示した軌道での加速度軌道の例を説明するグラフである。
【図１６】実施の形態１に係る位置軌道の例を説明するグラフである。
【図１７】図１６に示した軌道での速度軌道の例を説明するグラフである。
【図１８】図１６に示した軌道での加速度軌道の例を説明するグラフである。
【発明を実施するための形態】
【００１９】
以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一又は対応する要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。
【００２０】
＜発明の実施の形態１＞
図２は、本実施の形態に係るロボットとしてのアームロボット２０の模式的な構成例を示している。以下では、アームロボット２０を単にロボット２０と称する。ロボット２０はコントローラ３０を備えている。また、ロボット２０はアーム４０を備えており、アーム４０の手先５０を動かして、ユーザに対するサービスを行う。アームの各関節には、関節角を制御するアクチュエータと、関節角を検出するエンコーダとが内蔵されている。
【００２１】
コントローラ３０は、アーム４０の手先５０の軌道データに基づいて、アーム４０の各関節を制御する。すなわち、コントローラ３０は、軌道データから得られる各関節の目標角度に追従するように、アクチュエータを制御する。より具体的には、コントローラ３０は、軌道データを用いて各関節の目標角の時系列データを算出し、算出した目標角に追従するように、各関節を制御する。軌道を用いて関節目標角を生成する手法については、従来から知られている手法を用いればよいため、ここではその詳細な説明を省略する。
【００２２】
なお、コントローラ３０は、主要なハードウェア構成として、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される制御プログラムと、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを用いて構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスによって相互に接続されている。
【００２３】
本実施の形態では、図２に例示するように、２次元平面ｘｙ内において、所定の設定時間内に、始点Ｓから終端点Ｇまで手先５０を移動させるための軌道を生成する。ｘｙ座標は、始点Ｓの位置を原点とし、始点Ｓから終端点Ｇへ向かう方向をｘ方向とし、ｘ方向に垂直な方向をｙ方向とする座標系である。本実施の形態では、ロボット２０自らが現在の手先５０の位置情報を取得し、取得した手先５０の位置を始点Ｓの位置として設定することができる。また、終端点Ｇの位置はユーザ、あるいはコントローラ３０に指令を与える上位コントローラによって設定すればよい。なお、始点Ｓの位置の設定に関しても、ユーザ、あるいは上位コントローラの指令によって設定してもよい。
【００２４】
図２に例示した環境では、障害物６０が存在しない場合には、例えば、始点Ｓと終端点Ｇを直線で結んだ経路上（ｘ軸上）を滑らかに移動させるような軌道を生成すればよい。コントローラ３０は、この経路上に障害物６０が存在するものとし、障害物６０を回避し（位置制約）、かつ、加速度が過大にならないような（加速度制約）軌道を、高速に生成する。
【００２５】
図３は、コントローラ３０が実行する軌道生成処理のフローチャートを示す。コントローラ３０は、ロボット３０のアーム４０の手先５０を始点から終端点へと移動させるための軌道を、多項式補間を用いて生成する。
【００２６】
まず、コントローラ３０は、生成軌道に関する等式制約条件として設定する（Ｓ３０１）。等式制約条件とは、始点および終端点を通過し、かつ、始点および終端点それぞれでの位置・速度・加速度が連続な軌道となることを制約する条件である。コントローラ３０は、始点から終端点に到達する時間をサンプリング時間単位で分割して、サンプリング点を設定する（Ｓ３０２）。コントローラ３０は、サンプリング点の時間と、設定される始点および終端点それぞれでの位置・速度・加速度と、を用いて、等式制約条件を計算する（Ｓ３０３）。
【００２７】
また、コントローラ３０は、生成軌道に関する不等式制約条件として設定する（Ｓ３０４）。不等式制約条件とは、各サンプリング点での軌道上の位置が、始点から終端点へと至る経路上に存在する侵入禁止領域を回避する位置となり、かつ、各サンプリング点での軌道上の加速度が、所定の加速度の制限範囲内になる軌道となることを制約する条件である。コントローラ３０は、進入禁止領域の存在範囲外に位置するように設定される各サンプリング点での制約位置と、設定される所定の加速度の制限範囲と、を用いて、不等式制約条件を計算する（Ｓ３０５）。
【００２８】
また、コントローラ３０は、生成される軌道と目的軌道の間での評価関数を目的関数として設定する（Ｓ３０６）。
【００２９】
そして、コントローラ３０は、計算した等式制約条件と、計算した不等式制約条件と、目的関数と、を用いて線形計画問題を構成し、その線形計画問題を求解する（Ｓ３０７）。コントローラ３０は、線形計画問題での求解結果を用いて、多項式補間の補間係数を計算する（Ｓ３０８）。コントローラ３０は、計算した補間係数を用いて軌道を生成する。
【００３０】
以下、Ｓ３０１〜Ｓ３０８での処理を具体的に説明する。なお、説明の都合上、Ｓ３０１〜Ｓ３０８での処理の説明をまとめて行う。
【００３１】
まず、コントローラ３０は、始点での位置・速度・加速度と、終端点での位置・速度・加速度をそれぞれ設定する。始点および終端点それぞれでのｘ方向位置・速度・加速度を、以下の式（１）に示すように設定する。以下の設定では、アーム４０の手先５０の軌道が、静止状態から開始して静止状態へと至ることを目標とするものである。なお、以下の式において、変数の上部に配置される"・"は一階微分を示し、"・・"は二階微分を示している。
【数１】
2013184233.tif 000003
【００３２】
ｙ方向に関しては、始点および終端点それぞれでのｘ方向位置・速度・加速度を、以下の式（２）に示すように設定する。
【数２】
2013184233.tif 000004
【００３３】
なお、これら設定値のうち、始点での位置・速度・加速度に関しては、ロボット２０自らが必要な情報を取得して設定することができる。また、終端点での位置・速度・加速度に関しては、ユーザ、あるいはコントローラ３０に指令を与える上位コントローラによって設定すればよい。なお、始点での位置・速度・加速度の設定に関しても、ユーザ、あるいは上位コントローラの指令によって設定してもよい。
【００３４】
コントローラ３０は、ｘ方向において、始点から終端点まで位置・速度・加速度が連続となる軌道を生成する。ｘ方向の軌道は、始点での位置・速度・加速度から終端点での位置・速度・加速度まで連続となるように補間軌道を生成する。ここでは、多項式補間を用いて補間軌道を生成するが、滑らかな軌道を生成できるのであれば、他の公知の補間手法を用いて軌道生成するものとしてもよい。生成したｘ方向の軌道を図４に示す。なお、本実施の形態では、終端点への到達時刻をＴ＝２．０［ｓ］に設定した。また、終端点での位置・速度・加速度に関しては、ユーザ、あるいはコントローラ３０に指令を与える上位コントローラによって設定すればよい。
【００３５】
本実施の形態では、図５に例示する環境（図２に対応する環境）において、始点から終端点へと至る軌道を生成する。同図では、始点から終端点へと至る経路（波線を用いて示す）上に障害物６０が存在し、障害物６０を含む範囲が進入禁止領域となる。本実施の形態では、この進入禁止領域を回避する軌道を生成する。
【００３６】
このような障害物６０を迂回する軌道を生成するため、まず、コントローラ３０は、始点から終端点に到達する時間を時間方向に一定刻みのサンプリング間隔で分割して、サンプリング点を設定する（Ｓ３０２）。サンプリング点の設定は、ユーザ、あるいはコントローラ３０に指令を与える上位コントローラによって設定すればよい。設定したサンプリング点ｓｔを、以下の式（３）に示す。
【数３】
2013184233.tif 000005
【００３７】
次に、図４に例示したｘ方向軌道を、以下の式（４）に示すように定める。
【数４】
2013184233.tif 000006
ｓｘは、以下の式（５）に示すようにして計算することができる。
【数５】
2013184233.tif 000007
【００３８】
次に、コントローラ３０は、障害物６０と衝突しないように、サンプリング点におけるｙ方向位置の制約値を計算する。より具体的には、コントローラ３０は、各サンプリング点におけるｙ方向位置が障害物６０の存在範囲に含まれるか否かを判定し、含まれないと判定した場合には、そのサンプリング点におけるｙ方向位置の制約値を始点と終端点を結んだ直線上の値とし（すなわち制約値は０）、含まれると判定した場合には、そのサンプリング点におけるｙ方向位置の制約値を障害物６０の存在範囲の外側に位置する値とする。なお、障害物６０の存在範囲の外側に位置する値は、障害物６０の存在範囲の外縁から所定のマージンを持たせた値とするとよい。また、障害物６０の存在範囲は、ロボット２０自らが障害物６０が存在する範囲情報を取得することもできるし、ユーザ、あるいはコントローラ３０に指令を与える上位コントローラがコントローラ３０に対して設定するようにしてもよい。
【００３９】
計算したサンプリング点におけるｙ方向位置の制約値ｓｕｙを、以下の式（６）に示すように定める。
【数６】
2013184233.tif 000008
【００４０】
図６に、計算したｙ方向の制約値を結んだ経路を示す。図６に示すように、各サンプリング点において、障害物６０と衝突する可能性がある範囲では障害物６０の外側に、障害物６０と衝突する可能性がない範囲では始点と終端点を結ぶ直線上にｙの制約値をとる。なお、図６では、障害物６０の存在範囲の外側の値としてｙ軸方向マイナス側の値をとる例を説明したが、ｙ軸方向プラス側の値をとるようにしてもよい。
【００４１】
生成しようとする軌道のサンプリング点におけるｙ方向の値ｓｙを、以下の式（７）に示すように定める。
【数７】
2013184233.tif 000009
【００４２】
すると、生成しようとする軌道のｙ方向の位置制約条件は、以下の式（８）により示される。
【数８】
2013184233.tif 000010
【００４３】
図７に、生成しようとする軌道のｙ方向の位置制約条件を示す。同図は、各サンプリング点を横軸、そのサンプリング点ｙ方向における制約値を縦軸に取ったグラフである。すなわち、各サンプリング点において、そのｙ方向における値が制約値以下の範囲となる軌道が、障害物６０と衝突しない軌道となる。なお、以下では、サンプリング点ｓｔと、そのサンプリング点におけるｙ方向の制約値ｓｕｙと、からなる点を、「制御点」と称する。
【００４４】
また、サンプリング点におけるｙ方向のアーム４０の手先５０の加速度（ｓｙの二階微分）を以下の式（９）に示すように定める。
【数９】
2013184233.tif 000011
【００４５】
そして、サンプリング点におけるｙ方向の手先５０の加速度の上限制約値（ｓｕｙの二階微分）および下限制約値（ｓｌｙの二階微分）を、それぞれ以下の式（１０）に示すように定める。
【数１０】
2013184233.tif 000012
【００４６】
すると、生成したいｙ方向の軌道は、この加速度の上下限制約を満たし、かつ、以下の式（１１）により示すｙ方向の加速度制約条件を満たす必要がある。
【数１１】
2013184233.tif 000013
【００４７】
なお、加速度の上限制約値および下限制約値は、ロボット２０に実現させたい動作に応じて、適切な値を予め設定すればよい。例えば、物体を運搬するような場合には、その物体に応じた値を、ユーザ、あるいはコントローラ３０に指令を与える上位コントローラが設定すればよい。また、設定する加速度の上限制約値および下限制約値は、例えば全て同じ値に設定してもよいし、それぞれ異なる値を設定するものとしてもよい。
【００４８】
以上に説明した内容をまとめると、生成したい軌道とは、上述した位置の制約条件である式（８）と加速度の制約条件である式（１１）を満たす軌道であり、さらに、始点から終端点にかけて位置・速度・加速度が連続な軌道である。
【００４９】
続いて、位置・速度・加速度が連続となる軌道を生成するために用いる手法を説明する。まず、本実施の形態で用いる手法の説明に先立って、単純な３次スプライン補間を用いて軌道を生成する手法について説明する。
【００５０】
例えば、３次スプライン補間を用いて、図７に示した全ての制御点を通る軌道を生成する場合を考える。ここで、３次スプライン補間は、各補間点間をそれぞれ別の区間として設定し、各区間で独立な３次多項式を生成した上でそれを繋いでいくことで、全ての補間点を通り、かつ、位置・速度・加速度が連続な軌道を得る手法である。
【００５１】
まず、第ｊ区間、第ｊ＋１区間における３次多項式を以下の式（１２）に示すように定める。
【数１２】
2013184233.tif 000014
【００５２】
すると、時刻ｔｊ＋１において、第ｊ区間から第ｊ＋１区間にかけて位置・速度・加速度が連続となる条件は、以下の式（１３）によって示される。なお、式（１３）において、ａｊ、ｂｊ、ｃｊ、ｄｊは、スプライン多項式の係数である。
【数１３】
2013184233.tif 000015
【００５３】
式（１３）は、時刻ｔｊにおいても成り立つため、以下の式（１４）を得ることができる。
【数１４】
2013184233.tif 000016
【００５４】
上述した式（１３）および式（１４）を整理すると、最終的に以下の式（１５）を得ることができる。
【数１５】
2013184233.tif 000017
【００５５】
また、上述した式（１３）および式（１４）を用いると、以下の式（１６）を得ることができる。この式（１６）は、始点および終端点の境界条件の一つであり、速度の境界条件となる。
【数１６】
2013184233.tif 000018
【００５６】
上述した式（１４）〜（１６）を行列表現形式としてまとめると、以下の式（１７）を得ることができる。
【数１７】
2013184233.tif 000019
【００５７】
上述した式（１７）において、行列Ａｓｐｌは、以下の式（１８）により示される。
【数１８】
2013184233.tif 000020
【００５８】
また、上述した式（１７）において、行列ｂｓｐｌは、以下の式（１９）により示される。
【数１９】
2013184233.tif 000021
【００５９】
また、上述した式（１７）において、行列ｑは、以下の式（２０）により示される。
【数２０】
2013184233.tif 000022
【００６０】
さらに、上述した式（１７）を変形して、以下の式（２１）を得る。
【数２１】
2013184233.tif 000023
【００６１】
式（２２）において、制御点の時刻ｔ１，・・・，ｔｎと、制御点の制約値ｙｕ１，・・・，ｙｕｎを入力し、行列ｑについて解くことで、スプライン多項式の係数ｂｊを得ることができる。そして、この得たスプライン多項式の係数ｂｊと、制御点の時刻ｔ１，・・・，ｔｎと、制御点の制約値ｙｕ１，・・・，ｙｕｎと、を以下の式（２２）に入力することで、スプライン多項式の係数ａｊとｃｊを得ることができる。
【数２２】
2013184233.tif 000024
【００６２】
図８に、上述したようにして生成した軌道を示す。図９は、図８に示した軌道での加速度の変化を示すグラフである。図８では、３次スプライン補間のみを単純に用いて生成した軌道を示している。図８に示したように、制御点の数を十分に取ることで、単純なスプライン補間を用いた場合であっても、障害物６０を回避する軌道を簡単に作ることは可能である。しかしながら、図９から明らかなように、図８の軌道では、加速度の変化が非常に大きなものとなっており、この軌道に従うように手先５０を動かした場合には、手先５０により運搬する物体を落とす、または水をこぼすなどの問題が発生する。
【００６３】
そこで本実施の形態に係る軌道生成処理では、制御点を必ず通過するという等式条件を不等式条件へと拡張し、さらに、線形計画法を用いて多項式補間の係数を求めることで、加速度に制約を設けた軌道を生成することを可能にする。
【００６４】
まず、変数ｂｊに関する上述した式（２０）を拡大して、以下に示す式（２３）を定める。すなわち、行列ｑに対して変数ｄｊをも含めるようにする。
【数２３】
2013184233.tif 000025
【００６５】
そして、この式（２３）を用いて、上述した式（１７）を、以下に示す式（２４）として書き換える。この式（２４）は、式（１７）と全く同等な式であり、等式制約条件となる。
【数２４】
2013184233.tif 000026
【００６６】
上述した式（２４）において、行列Ａｓｐｌｂは、以下の式（２５）により示される。
【数２５】
2013184233.tif 000027
【００６７】
また、上述した式（２４）において、行列ｂｓｐｌは、以下の式（２６）により示される。
【数２６】
2013184233.tif 000028
【００６８】
また、上述した式（２４）において、行列Ａｓｐｌｄは、以下の式（２７）により示される。
【数２７】
2013184233.tif 000029
【００６９】
次いで、上述した式（２４）に示す等式制約条件に関して、始点および終端点での位置および加速度が連続となるための条件を考える。上述した式（２４）における行列ｑは、変数ｄｊを追加したものとなっている。この変数ｄｊの追加を反映させるため、始点および終端点での位置および加速度の境界条件として、以下に示す式（２８）を定める。なお、この式（２８）は式（１２）から求めることができる。
【数２８】
2013184233.tif 000030
【００７０】
上述した式（２８）は、行列表現形式としてまとめると、以下に示す式（２９）として定めることができる。
【数２９】
2013184233.tif 000031
【００７１】
上述した式（２９）において、行列Ａｓｇは、以下の式（３０）により示される。
【数３０】
2013184233.tif 000032
【００７２】
また、上述した式（２９）において、行列ｂｓｇは、以下の式（３１）により示される。
【数３１】
2013184233.tif 000033
【００７３】
上述した式（２４）および式（２９）をまとめることで、以下に示す式（３２）を得る。この式（３２）が、始点・終端点および始点から終端点までの全領域での位置・速度・加速度が連続となることに関する、等式制約条件となる。コントローラ３０は、この等式制約条件を設定する（Ｓ３０１）。さらに、コントローラ３０は、サンプリング点の時間と、設定される始点および終端点それぞれでの位置・速度・加速度と、を用いて、この等式制約条件を計算する（Ｓ３０３）。
【数３２】
2013184233.tif 000034
【００７４】
次に、上述した式（８）および式（１１）を書き換えることで、以下の式（３３）および式（３４）に示す不等式制約条件を得ることができる。すなわち、生成軌道のｙ方向の位置制約条件である式（８）を書き換えることで、式（３３）を得る。また、生成軌道のｙ方向の加速度の制約条件である式（１１）を書き換えることで、式（３４）を得る。
【数３３】
2013184233.tif 000035
【数３４】
2013184233.tif 000036
【００７５】
上述した式（３３）および式（３４）をまとめることで、以下に示す式（３５）を得る。この式（３５）が、生成軌道が位置および加速度の制約を満たすことに関する、不等式制約条件となる。コントローラ３０は、この不等式制約条件を設定する（Ｓ３０４）。さらに、コントローラ３０は、進入禁止領域の存在範囲外に位置するように設定される各サンプリング点での制約位置と、設定される所定の加速度の制限範囲と、を用いて、この不等式制約条件を計算する（Ｓ３０５）。
【数３５】
2013184233.tif 000037
【００７６】
最後に、線形計画で用いる目的関数を定める。コントローラ３０は、生成される軌道と目的軌道の間での評価関数を目的関数として設定する（Ｓ３０６）。本実施の形態では、生成軌道を可能な限り始点と終端点を結ぶ直線経路に近づけるようにして生成する。このため、目的関数ｇＴｑについて、以下に示す式（３６）を定める。すなわち、式（３６）では、変数ｄｊの総和が最小となるような軌道を求めるものであり、生成軌道の位置の不等式条件を考慮して、生成軌道の位置を可能な限り制約値に近づけるものである。
【数３６】
2013184233.tif 000038
【００７７】
上述した式（３６）において、行列ｇＴは、以下の式（３７）により示される。
【数３７】
2013184233.tif 000039
【００７８】
上述した内容をまとめると、本実施の形態で扱う線形計画問題は、以下に示す式（３８）により構成することができる。式（３８）において、目的関数は式（３６）であり、始点および終端点での位置・速度・加速度の連続に関する等式制約条件は式（３２）であり、生成軌道の位置および加速度の制約に関する不等式制約条件は式（３５）である。
【数３８】
2013184233.tif 000040
【００７９】
本実施の形態では、上述したように、位置・速度・加速度が連続となる軌道を生成可能な３次スプライン補間問題を線形計画問題に変換することに加えて、位置および加速度に関する制約条件を設定可能にし、さらに、高速に求めたい軌道の生成を実行するものである。線形計画問題はその歴史が非常に長く、シンプレックス法や内点法を用いることで、非常に高速かつ信頼性高く求解できることが知られている。また、実用的なソルバーが既にいくつも商用になっている。
【００８０】
そこで、コントローラ３０は、上述した式（３８）に示す線形計画問題をソルバーを用いて求解する（Ｓ３０７）。そして、コントローラ３０は、求解結果である多項式補間の係数ｄｊ（行列ｑ）を得て、この得た行列ｑを以下に示す式（２９）に入力することで、他の係数ａｊ、ｂｊ、ｃｊを計算する（Ｓ３０８）。なお、式（２９）において、ｑ（ｊ）は行列ｑのｊ番目の要素であることを示している。最終的に、コントローラ３０は、多項式補間の全ての係数ａｊ、ｂｊ、ｃｊ、ｄｊを得ることで、軌道を生成する。
【数３９】
2013184233.tif 000041
【００８１】
図１０〜図１８に、本実施の形態による軌道生成処理による処理結果を例示する。図１０〜図１２と、図１３〜図１５と、図１６〜図１８と、で、それぞれ終端点への到達時刻Ｔを変更した場合を示している。図１０、図１３、図１６は、それぞれ位置の軌道を示している。図１１、図１４、図１７は、それぞれ加速度の軌道を示している。図１２、図１５、図１８は、それぞれ加速度の軌道を示している。始点から終端点へと至る手先５０の軌道を示している。図１０、図１１、図１３、図１４、図１６、図１７から明らかなように、位置および加速度の制約（図に示す例では、加速度の制約±６［ｍ／ｓ２］ とした）範囲内で、軌道が生成できていることが分かる。また、図１２、図１５、図１８から明らかなように、加速度制約を満たしつつも、障害物４０を回避して滑らかに移動可能な軌道が生成されたことが確認できる。なお、図１３〜１５では、終端時刻Ｔを２[ｓ]から１［ｓ］へと短縮した場合での結果を示している。また、図１６〜１８では、終端時刻Ｔを３[s]へと延長した場合での結果を示している。いずれの場合においても、位置および加速度の制約を満たしながら、滑らかな軌道が生成されていることが分かる。
【００８２】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述した実施の形態では、ロボット３０がアーム４０を備えたアームロボットであり、アーム４０の手先５０の軌道を生成する場合を例に説明したが、本発明はこれに限られるものではない。すなわち、ロボット３０の部位として、他の部位の軌道を生成するものとしてもよい。
【符号の説明】
【００８３】
１０  ロボット、１１  手先、１２  コップ、
１３  テーブル、１４  障害物、１５  お盆、
２０  ロボット、３０  コントローラ、４０  アーム、５０  手先、６０  障害物、
Ｓ  始点、Ｇ  終端点
【図１】
2013184233.tif 000042
【図２】
2013184233.tif 000043
【図３】
2013184233.tif 000044
【図４】
2013184233.tif 000045
【図５】
2013184233.tif 000046
【図６】
2013184233.tif 000047
【図７】
2013184233.tif 000048
【図８】
2013184233.tif 000049
【図９】
2013184233.tif 000050
【図１０】
2013184233.tif 000051
【図１１】
2013184233.tif 000052
【図１２】
2013184233.tif 000053
【図１３】
2013184233.tif 000054
【図１４】
2013184233.tif 000055
【図１５】
2013184233.tif 000056
【図１６】
2013184233.tif 000057
【図１７】
2013184233.tif 000058
【図１８】
2013184233.tif 000059