(57)【要約】【課題】ロボットの３次元軌道の生成において、少ない演算量で演算の高速化を可能とする、ロボットの３次元軌道生成方法を提供すること。【解決手段】本発明にかかるロボット１の３次元軌道生成方法は、ロボット１が移動する軌道上に描かれるロボットの軌跡２０の始端１２及び終端１３と、始端１２から終端１３に至るまでの軌跡の経由点２１、２２を決定し、経由点２１、２２を通過する通過時刻を設定し、通過時刻に基づいて各経由点の間を通過する各時間区間で区分された軌跡２０を表す区分多項式の係数を決定する際、空間を表すための３次元要素に関する全ての係数を含む係数行列を設定し、係数行列により係数を計算し、係数を用いた区分多項式により軌跡を計算し、区分多項式のパラメータに始端１２及び終端１３における境界条件を満たす関数を適用して形成される合成関数により軌道を生成する、処理をコンピュータに実行させる。【選択図】図１
2015100867.tif 000002
【特許請求の範囲】
【請求項１】
ロボットが移動する軌道上に描かれる前記ロボットの軌跡の始端及び終端と、前記始端から前記終端に至るまでの軌跡の経由点を決定し、
前記経由点を通過する通過時刻を設定し、
前記通過時刻に基づいて各前記経由点の間を通過する各時間区間で区分された前記軌跡を表す区分多項式の係数を決定する際、空間を表すための３次元要素に関する全ての係数を含む係数行列を設定し、
前記係数行列により前記係数を計算し、
前記係数を用いた前記区分多項式により前記軌跡を計算し、
前記区分多項式のパラメータに前記始端及び前記終端における境界条件を満たす関数を適用して形成される合成関数により前記軌道を生成する、
処理をコンピュータに実行させるロボットの３次元軌道生成方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボットの３次元軌道生成方法に関し、特にロボットの手や足の制御時に用いる３次元軌道の生成方法に関する。
【背景技術】
【０００２】
ロボットが人の住空間で移動を行う際、手や足を地面に接触して身体を支えながら移動を行う。この時、ロボットの制御において、ある接触点に着目すると、接触平面から力を受けて（接触平面を押すことによって）身体を支える期間と、現在の接触平面から次の接触平面に接触点を移動させる期間が存在する。通常、ロボットの制御は、移動中はこの２つの期間が繰り返される。具体的には、例えば歩行中の足と地面の関係を思い浮かべればよい。
【０００３】
ここで、ロボットの制御方法は、接触点が接触平面間を移動する処理に着目すると、この区間では始端及び終端において接触面に垂直な方向に接触点が運動することが望ましく、また、軌道は静止状態（速度及び加速度０）から静止状態までをつなぎ、位置、速度及び加速度までの連続性を有することが望ましい。加えて、制御方法は障害物等を回避するために、経由点を自由に指定できると有用性が高い。また、ロボットはオンラインで繰り返しの軌道の修正を行うため、制御方法は少ない演算量で高速に軌道の生成が行えることが望ましい。軌道を生成する技術として例えば、特許文献１に記載された方法がある。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１１−１４５７９７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１の方法によると、３次元軌道を生成する場合において３連クロソイド曲線を用いている。そして、該方法は予め与えた通過点を通過するよう３次元軌道を生成している。しかし、クロソイド曲線の計算には時間を要するため、ロボットがオンラインで繰り返しの軌道修正を行う場合に高速で処理できない場合があるという課題がある。
【０００６】
本発明は、上述の点に鑑みてなされたものであり、ロボットの３次元軌道の生成において、少ない演算量で演算の高速化を可能とする、ロボットの３次元軌道生成方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
一態様にかかるロボットの３次元軌道生成方法は、ロボットが移動する軌道上に描かれる前記ロボットの軌跡の始端及び終端と、前記始端から前記終端に至るまでの軌跡の経由点を決定し、前記経由点を通過する通過時刻を設定し、前記通過時刻に基づいて各前記経由点の間を通過する各時間区間で区分された前記軌跡を表す区分多項式の係数を決定する際、空間を表すための３次元要素に関する全ての係数を含む係数行列を設定し、前記係数行列により前記係数を計算し、前記係数を用いた前記区分多項式により前記軌跡を計算し、前記区分多項式のパラメータに前記始端及び前記終端における境界条件を満たす関数を適用して形成される合成関数により前記軌道を生成する、処理をコンピュータに実行させる。
【発明の効果】
【０００８】
本発明によると、ロボットの３次元軌道の生成において、少ない演算量で演算の高速化を可能とする、ロボットの３次元軌道生成方法を提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の実施の形態１にかかるロボット１の各部の動作における３次元軌道を示した概要図である。
【図２】本発明の実施の形態１にかかるロボット１の軌道の１例を示した概要図である。
【図３】本発明の実施の形態１にかかるロボット１の３次元軌道生成方法に用いる記号の定義を示した図である。
【図４】本発明の実施の形態１にかかる関数ｇに用いた３次多項式を示す図である。
【図５】本発明の実施の形態１にかかる係数行列（補間点数４の場合）を示した図である。
【図６】本発明の実施の形態１にかかるロボット１の３次元軌道生成の演算処理方法を示したフローチャートである。
【図７】本発明の実施の形態２にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道を示す図である。
【図８】本発明の実施の形態２にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道の３次元位置を示す図である。
【図９】本発明の実施の形態２にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道の速度を示す図である。
【図１０】本発明の実施の形態２にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道の加速度を示す図である。
【図１１】本発明の実施の形態３にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道を示す図である。
【図１２】本発明の実施の形態３にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道の３次元位置を示す図である。
【図１３】本発明の実施の形態３にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道の速度を示す図である。
【図１４】本発明の実施の形態３にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道の加速度を示す図である。
【図１５】本発明の実施の形態１にかかるロボット１の概要を示したブロック図である。
【発明を実施するための形態】
【００１０】
実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１５は、本発明の実施の形態１にかかるロボット１の概要を示したブロック図である。ロボット１は軌道演算部５０を備える。軌道演算部５０は、ロボット１の手や足の軌道を演算する。軌道演算部５０は、以下に示す軌道計算の演算処理をコンピュータに実行させることにより軌道計算を実現する。
【００１１】
図１は、本発明の実施の形態１にかかるロボット１の各部の動作における３次元軌道を示した概要図である。本実施の形態においてロボット１は、人型ロボットを例に用いて説明する。以下の説明において、空間上に幾何学的に描かれるパスを軌跡や経路、軌跡に時間が伴って動いていく過程を軌道と記載する。
【００１２】
図１は、ロボット１がある瞬間に左手を第１の面１０から机３上の第２の面１１に移動させ、左手を机３上に着き直し、両脚を机３の前に着いて、ペットボトル２に右手を伸ばすというシーンである。この時、図中の矢印で示された左手の軌跡に着目すると、壁上の接触面である第１の面１０から机３上の接触面である第２の面１１に移動する経路になっている。
【００１３】
このような一連の動作を安定的に行うためには、経路の両端（始端と終端）で接触面に対して垂直方向に手を動作させることが望ましい。これは、歩行制御におけるロボット１の足の制御と類似している。図１に示すように、ロボット１の右足の軌跡に注目すると、ロボット」１は右足が地面に擦らないように（必要のない摩擦を発生させないように）一旦足を鉛直方向に持ち上げる制御に類似している。
【００１４】
即ち、ロボット１の手や足など周囲の環境と接触する部分を制御するためには、接触面において不要な摩擦を発生させないように、経路の両端において接触面に対して垂直方向のベクトルを持った動作を行う必要がある。そして、中間の経路におけるロボット１の制御は、ロボット１の手や足などが机３や周囲の環境と接触しないような経由点を通過するような軌跡を演算する必要がある。即ち、ロボット１の制御において、始端・終端の方向（３次元ベクトル）を指定し、軌道の途中に存在する経由点を通過するよう軌道を演算する、軌道補間法が必要である。
【００１５】
図２は、本発明の実施の形態１にかかるロボット１の軌道の１例を示した概要図である。図２は、例えば、ロボット１の手先や足先の軌道の軌跡である。図２において、始端１２を含む第１の面１０と、終端１３を含む第２の面１１が異なる角度で存在している。始端１２からの軌道の３次元ベクトル１４の方向は第１の面１０に対して垂直方向である。終端１３からの軌道の３次元ベクトル１５の方向は第２の面１１に対して垂直方向である。軌道の軌跡２０は、演算により終端１２と終端１３とを結び、経由点２１、２２を経由するよう生成される。
【００１６】
軌道の演算において、ロボット１の手先や足先の軌道の軌跡２０は、最終的にロボット１の関節軌道に変換されるので、端点も含めて始端１２から終端１３まで位置・速度・加速度が連続であることが望ましい。本実施の形態における軌道の軌跡２０は、接触面（第１の面１０）から接触面（第２の面１１）までの軌跡であり、第１の面１０上の始端１２位置で速度０、加速度０の状態から、第２の面１１上の終端１３位置で速度０、加速度０の状態で停止するまでの軌道として演算される。
【００１７】
上記より、本実施の形態において、ロボット１の軌道を演算する場合に必要な条件は以下のようにまとめられる。
（１）始端及び終端において速度及び加速度が０である。
（２）始端から終端までの間で位置、速度及び加速度が連続していること。
（３）与えられた始端、終端及び全経由点を通過すること。
（４）始端と終端における軌道の方向（３次元ベクトル）を指定可能であること。
これらはロボット１の軌道を演算する時の境界条件となる。
【００１８】
これより、本発明の実施の形態１にかかるロボットの３次元軌道生成方法は、端点（始端及び終端）における法線ベクトルに合わせた境界条件を用いてｘ、ｙ、ｚの３方向を同時にスプライン補間して軌跡を計算し、軌跡を表す関数のパラメータに始端及び終端における境界条件を満たす関数を適用して形成される合成関数により軌道を生成する。
【００１９】
以下、図面と数式を用いて本発明の実施の形態１にかかるロボットの３次元軌道生成方法を説明する。図３は、以下の数式の説明に用いる記号の定義を示した図である。本実施の形態において、補間点が４点の場合を示しているが、補間点は４点より多数存在していてもよい。
【００２０】
（ａ）経由点の通過時刻の設定
演算部５０は、まず経由点を通過する際の時刻を決定する。本実施の形態では、演算部５０は、簡単のために補間点間の距離に比例させて通過時刻を設定する。即ち、
【数１】
2015100867.tif 000003
となる。
【００２１】
（ｂ）３次元スプライン補間の係数行列設定
上記の条件（２）、（３）を満たすための式は、通常のスプライン補間と同様の定式化を行えばよい。（ａ）で決定した経由点の通過時刻を用いて、ｘ、ｙ、ｚの軌道が下記の区分３次多項式で表されるとする。
【数２】
2015100867.tif 000004
【００２２】
以下、ｘ方向についてのみ展開式を示す。ｊ-1番目とｊ番目について考えると、
【数３】
2015100867.tif 000005
と表される。ここで、ｔ＝ｔｊにおける位置、速度及び加速度の連続性の条件
【００２３】
【数４】
2015100867.tif 000006
から、次式が導かれる。
【数５】
2015100867.tif 000007
【００２４】
上記３つの式において、第３式を第１及び第２式に代入すると、
【数６】
2015100867.tif 000008
となる。式（３）は次の区間のｔ＝ｔｊ＋１においても同様の式が成り立つ必要があるので、
【００２５】
【数７】
2015100867.tif 000009
となる。式（３）、式（５）を式（４）に代入すれば、
【数８】
2015100867.tif 000010
となる。また、
【００２６】
【数９】
2015100867.tif 000011
であるから、ｙ、ｚとともに行列にまとめれば、
【数１０】
2015100867.tif 000012
と表すことができる。
【００２７】
次に、上記の（４）の条件について考える。端点において軌跡が垂線と一致するためには、垂線以外の方向の１階微分の成分が０になればよい。即ち、
【数１１】
2015100867.tif 000013
が成り立てばよい。
【００２８】
また、垂線方向に関しては、境界の２階微分が０（一般に自由端と言われている境界条件）を用いて、
【数１２】
2015100867.tif 000014
と表される。式（８）、式（９）を整理し、行列にまとめれば、
【００２９】
【数１３】
2015100867.tif 000015
【数１４】
2015100867.tif 000016
【数１５】
2015100867.tif 000017
と表される。
【００３０】
最終的に式（７）、式（１０）をまとめて、係数行列は、
【数１６】
2015100867.tif 000018
となる。
式（１１）の４、５行目に示す様に、係数行列は空間を表すためのｘ、ｙ、ｚの各方向、即ち３次元要素に関する全ての係数が含まれる。これにより、係数行列は、始終点が任意の平面に対して垂直であるためのｘ、ｙ、ｚの各方向に関する係数を同時に扱うことができ、接触平面に対して垂直に離着地する補間軌道の生成を可能とする。
図５は、本発明の実施の形態１にかかる係数行列（補間点数４の場合）を示した図である。
【００３１】
（ｃ）補間多項式の係数計算
まず、式（１１）を解くことによって各区間の係数ｂを求めることができる。
【数１７】
2015100867.tif 000019
係数ｂを用いて係数ａ、ｃについても算出が可能である。即ち、式（２）、式（３）より、
【数１８】
2015100867.tif 000020
となる。
【００３２】
また、係数ｄについては前述のように、
【数１９】
2015100867.tif 000021
である。
上記の係数計算式はｘ方向のみ記載した。この係数計算式はｙ及びｚ方向についても全く同様の式が成り立つので、ｂｘ、ｂｚを用いて計算すればよい。
【００３３】
（ｄ）入力フィルタ関数の設定
式（１２）、式（１３）、式（１４）によって区分多項式（１）の係数が全て決定したので、３次元空間上の軌跡（例えば図２）を決定できる。次に、この軌跡上をどのように移動するかという自由度を決定する。本実施の形態において、（ｃ）までで求めたスプライン関数で用いていたｔを以下のような条件を満たす（両端で速度及び加速度０を満たし、０からＴまで単調増加する）関数を用いてロボット１を駆動させるよう演算する。
【００３４】
【数２０】
2015100867.tif 000022
即ち、（ｃ）まで求めた３次元空間上の軌跡と上記関数ｇとの合成関数として軌道を生成する。３次元空間上の軌道は次式
【数２１】
2015100867.tif 000023
で表される。（ただし式（１）のスプライン補間関数を０〜Ｔの範囲で改めてｆｘ（ｔ）、ｆｙ（ｔ）、ｆｚ（ｔ）と記述し直した）。
【００３５】
軌道の速度及び加速度を計算すると、
【数２２】
2015100867.tif 000024
となるので、式（１５）を用いれば、
【数２３】
2015100867.tif 000025
が成り立つことが分かる。図４は、本発明の実施の形態１にかかる関数ｇに用いた３次多項式を示す図である。図４に示すように関数ｇは、時間ｔの範囲によって異なる３次多項式を用いている。この関数ｇとしては、５次関数や区分多項式等、様々な関数を用いてもよい。
【００３６】
次に、軌道演算部５０が上記の演算処理を行う流れを図を用いて説明する。図６は、本発明の実施の形態１にかかるロボット１の３次元軌道生成の演算処理方法を示したフローチャートである。（ａ）軌道演算部５０は、ロボット１が移動する軌道上に描かれるロボット１の軌跡の始端１２及び終端１３と、始端１２から終端１３に至るまでの軌跡の経由点２１、２２を決定し、経由点２１、２２を通過する通過時刻を設定する（Ｓ１００）。（ｂ）軌道演算部５０は、（ａ）で決定した軌跡の経由点の通過時刻に基づいて各経由点の間を通過する各時間区間で区分された軌跡２０を表す区分多項式の係数を決定する際、空間を表すための３次元要素に関する全ての係数を含む係数行列を設定する（Ｓ１０１）。
【００３７】
（ｃ）軌道演算部５０は、係数行列により係数を計算する（Ｓ１０２）。（ｄ）軌道演算部５０は、係数を用いた区分多項式により軌跡を計算し、区分多項式のパラメータに始端１２及び終端１３における境界条件を満たす関数を適用して形成される合成関数により軌道を生成する（Ｓ１０３）。
【００３８】
上記の様に本発明の実施の形態１にかかるロボット１の３次元軌道生成方法によると、軌道の演算においてスプライン補間を用いているため、計算コストを低くすることが可能となる。また、該方法は係数行列を用いており、そのサイズは補間点数の３倍程度であるため、高速な軌道の演算が可能となる。さらにまた、該方法は係数行列に関しては図５のように決まった形であり、不要な箇所の前進消去を省略する等、計算を工夫することによってさらなる高速化も可能となる。そして、該方法は、係数行列が空間を表すための３次元要素に関する全ての係数を含んだ一つの行列としてまとめられることにより、始終点の平面がどのような向きであっても、平面に対して垂直になるような軌跡を形成することができる。そのため、該方法は接触平面に対して垂直に離着地する補間軌道の生成が可能となる。
【００３９】
実施の形態２
次に、本発明のロボット１の３次元軌道生成方法について、具体的な数値を用いた例について説明する。図７〜図１０は、本発明の実施の形態２にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道を示す図である。各経由点及び接触平面等における数値の設定は以下の通りである。
【数２４】
2015100867.tif 000026
【００４０】
図７は、ロボット１の制御部分の始端から終端までの軌道が示されている。図８は、ロボット１の制御部分の３次元座標の時間的変化が示されている。図９は、ロボット１の制御部分の速度の時間的変化が示されている。図１０は、ロボット１の制御部分の加速度の時間的変化が示されている。これらの図から、本発明のロボット１の３次元軌道生成方法によって、端点において接触平面に垂直な軌跡であること、全経由点を通過していること、端点において速度及び加速度が０であること、という条件を満たす３次元軌道が生成されていることがわかる。
【００４１】
実施の形態３
実施の形態２と同様に実施の形態２と異なる条件で軌道を生成した例を以下に示す。図１１〜図１４は、本発明の実施の形態３にかかるロボット１の３次元軌道生成方法の具体的な数値を用いた軌道を示す図である。本実施の形態は、実施の形態２と条件が異なるのみなので、重複する説明は省略する。各経由点及び接触平面等における数値の設定は以下の通りである。
【数２５】
2015100867.tif 000027
【００４２】
以上、本発明を上記実施形態および実施例に即して説明したが、上記実施形態および実施例の構成にのみ限定されるものではなく、本願特許請求の範囲の請求項の発明の範囲内で当業者であればなし得る各種変形、修正、組み合わせを含むことは勿論である。
【符号の説明】
【００４３】
１  ロボット
２  ペットボトル
３  机
１０  第１の面
１１  第２の面
１２  始端
１３  終端
１４  ３次元ベクトル
１５  ３次元ベクトル
２０  軌道
２１  経由点
２２  経由点
５０  軌道演算部
【図１】
2015100867.tif 000028
【図２】
2015100867.tif 000029
【図３】
2015100867.tif 000030
【図４】
2015100867.tif 000031
【図５】
2015100867.tif 000032
【図６】
2015100867.tif 000033
【図７】
2015100867.tif 000034
【図８】
2015100867.tif 000035
【図９】
2015100867.tif 000036
【図１０】
2015100867.tif 000037
【図１１】
2015100867.tif 000038
【図１２】
2015100867.tif 000039
【図１３】
2015100867.tif 000040
【図１４】
2015100867.tif 000041
【図１５】
2015100867.tif 000042