JPWO2020141579A1 - 制御装置、制御方法、及びプログラム - Google Patents

Abstract

始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、を備える、制御装置。

Description

本開示は、制御装置、制御方法、及びプログラムに関する。

一般的に、ロボット装置に備えられるマニュピュレータ等（以下では、機械要素とも称する）では、該マニュピュレータ等の位置及び姿勢を指示する動作軌道に基づいて動作が制御される。

このような機械要素の動作を適切に制御するためには、動作の始点から終点までをつなぐ動作軌道において、機械要素の位置及び姿勢を適切に決定することが重要となる。ただし、動作軌道を計画する際に機械要素の位置及び姿勢を同時に探索することは、探索の自由度が高くなり（合計６自由度）、探索に時間が掛かってしまうため、好ましくない。

そこで、まず、動作軌道上に経由点を定めて、経由点における機械要素の位置（３自由度）を探索し、次に、探索した機械要素の位置に基づいて機械要素の姿勢（３自由度）を決定することが検討されている。

例えば、下記の特許文献１には、軌道上の位置データと、マニュピュレータの姿勢データとを対応付けした姿勢データテーブルをあらかじめ記憶し、マニュピュレータの位置データに基づいてマニュピュレータの姿勢を決定することが記載されている。

特開平１０−３１５１６９号公報

しかし、経由点における機械要素の位置に基づいて機械要素の姿勢を決定した場合、決定された機械要素の姿勢は、機械要素の機構的特徴を考慮した適切な姿勢となっていないことがあり得る。そのため、動作軌道の始点から終点までの間に存在する経由点における機械要素の姿勢をより適切に導出することが可能な制御装置、制御方法、及びプログラムが求められていた。

本開示によれば、始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、を備える、制御装置が提供される。

また、本開示によれば、演算装置によって、始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、前記機械要素の前記経由点における最適姿勢を導出することと、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出することと、を含む、制御方法が提供される。

また、本開示によれば、コンピュータを始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、として機能させる、プログラムが提供される。

始点における機械要素の位置及び姿勢の一例を示す模式図である。 終点における機械要素の位置及び姿勢の一例を示す模式図である。 経由点における機械要素の位置及び姿勢の一例を示す模式図である。 経由点における機械要素の位置及び姿勢の他の例を示す模式図である。 本開示の一実施形態に係る制御装置の機能構成を示すブロック図である。 機械要素の姿勢を導出する際の制御装置の動作例を説明するフローチャート図である。 クォータニオンによる球面線形補間を説明する説明図である。 始点及び経由点の間のユークリッド距離と、経由点及び終点の間のユークリッド距離とを説明する説明図である。 クォータニオンの球面線形補正を用いた、経由点における機械要素の補間姿勢の導出を説明する説明図である。 始点、経由点及び終点を通る二次ベジエ曲線を説明する説明図である。 機械要素の機構を考慮した、経由点における機械要素の最適姿勢の導出を説明する説明図である。 補間姿勢及び最適姿勢の内分比を制御する関数の一例を示すグラフ図である。 補間姿勢及び最適姿勢の内分比を制御する関数の他の例を示すグラフ図である。 補間姿勢及び最適姿勢の内分比を制御する関数の他の例を示すグラフ図である。 本開示の一実施形態に係る制御装置のハードウェア構成の一例を示すブロック図である。

以下に添付図面を参照しながら、本開示の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。

なお、説明は以下の順序で行うものとする。
１．本開示に係る技術の概要
２．制御装置の構成例
３．制御装置の動作例
４．ハードウェア構成例
５．まとめ

＜１．本開示に係る技術の概要＞
まず、図１Ａ〜図２Ｂを参照して、本開示に係る技術の概要について説明する。

例えば、始点Ｐ_ｓから終点Ｐ_ｅまで、経由点Ｐ_ｖを介してロボット装置１０のロボットアーム等の機械要素２００を動作させる場合について考える。具体的には、図１Ａに示す始点Ｐ_ｓにおける機械要素２００の位置及び姿勢から、図１Ｂに示す終点Ｐ_ｅにおける機械要素２００の位置及び姿勢までロボット装置１０を動作させる場合について考える。なお、説明を簡単にするため、図１Ａ及び図１Ｂに示すように、始点Ｐ_ｓ及び終点Ｐ_ｅにおける機械要素２００の姿勢は、同一とした。

このとき、経由点Ｐ_ｖにおける機械要素２００の姿勢は、例えば、始点Ｐ_ｓ及び終点Ｐ_ｅにおける機械要素２００の姿勢から導出することができる。具体的には、経由点Ｐ_ｖにおける機械要素２００の姿勢は、始点Ｐ_ｓ及び終点Ｐ_ｅの各々における機械要素２００の姿勢を足し合わせることで導出することができる。例えば、図１Ａに示す始点Ｐ_ｓにおける機械要素２００の姿勢は、図１Ｂに示す終点Ｐ_ｅにおける機械要素２００の姿勢と同一である。そのため、経由点Ｐ_ｖにおける機械要素２００の姿勢は、始点Ｐ_ｓ及び終点Ｐ_ｅにおける機械要素２００の姿勢と同一として導出することができる。

しかしながら、このような方法で経由点Ｐ_ｖにおける機械要素２００の姿勢を導出する場合、経由点Ｐ_ｖの位置によっては、図２Ａに示すように、機械要素２００の姿勢が適切となる解が存在しないことがあり得る。また、経由点Ｐ_ｖの位置によっては、機械要素２００の姿勢が不自然又は非効率な姿勢となってしまうことがあり得る。

一方、経由点Ｐ_ｖにおける機械要素２００の姿勢は、始点Ｐ_ｓ及び終点Ｐ_ｅにおける機械要素２００の姿勢を考慮せずに、例えば、機械要素２００の機械的特徴から導出することもできる。具体的には、経由点Ｐ_ｖにおける機械要素２００の姿勢は、経由点Ｐ_ｖにおける機械要素２００の各部位に掛かるトルクが小さくなるように、又は機械要素２００の可操作性が高くなるように導出することができる。

しかしながら、このような方法で経由点Ｐ_ｖにおける機械要素２００の姿勢を導出した場合、図２Ｂに示すように、機械要素２００の姿勢が始点Ｐ_ｓ又は終点Ｐ_ｅにおける姿勢から大きく変化したものとなり、機械要素２００の動作効率が低下してしまうことがあり得る。

したがって、経由点Ｐ_ｖにおける機械要素２００の姿勢は、互いに異なる方法にて導出された複数の姿勢を考慮して導出されることが望ましい。本開示に係る技術は、上述した事情を鑑みて想到されたものである。具体的には、本開示に係る技術は、経由点Ｐ_ｖにおける機械要素２００の姿勢を補間姿勢及び最適姿勢に基づいて導出するものである。

ここで、補間姿勢とは、始点Ｐ_ｓにおける機械要素２００の姿勢、及び終点Ｐ_ｅにおける機械要素２００の姿勢の間を補間することで導出される姿勢である。また、最適姿勢とは、機械要素２００の機構的特徴に基づいて導出される姿勢である。本開示に係る技術によれば、補間姿勢及び最適姿勢を用いて機械要素２００の姿勢を導出することによって、経由点Ｐ_ｖにおける機械要素２００の姿勢をより適切に制御することが可能である。

以下では、上記で概要を説明した本開示に係る技術を実現する制御装置について詳述する。

＜２．制御装置の構成例＞
続いて、図３を参照して、本開示の一実施形態に係る制御装置１００の具体的な構成例について説明する。図３は、本実施形態に係る制御装置１００の機能構成を示すブロック図である。

図３に示すように、制御装置１００は、認識部１１０と、軌道生成部１２０と、補間姿勢導出部１３０と、最適姿勢導出部１４０と、姿勢導出部１５０と、制御部１６０と、を備える。

例えば、制御装置１００は、センサ部２１０にて取得された情報に基づいて、機械要素２００の駆動部２２０を制御することで、機械要素２００の位置及び姿勢を制御するものである。制御装置１００は、機械要素２００を備えるロボット装置１０の内部に設けられてもよく、機械要素２００を備えるロボット装置１０の外部に設けられてもよい。

ここで、機械要素２００とは、ロボット装置１０に備えられる部位のうち機械的機構によって位置及び姿勢を制御可能な部位を表す。例えば、機械要素２００は、ロボット装置１０のロボットアーム又は該ロボットアームの先端のエンドエフェクタであってもよく、ロボット装置１０の脚部又は該脚部の先端の接地部であってもよい。

センサ部２１０は、ロボット装置１０の周囲の環境情報を取得するセンサと、ロボット装置１０の自機情報を取得するセンサと、を備える。例えば、センサ部２１０は、ロボット装置１０の周囲の環境情報を取得するセンサとして、ＲＧＢカメラ、グレースケールカメラ、ステレオカメラ、デプスカメラ、赤外線カメラ又はＴｏＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）カメラ等の各種カメラを備えてもよい。センサ部２１０は、ＬＩＤＡＲ（Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）センサ又はＲＡＤＡＲ（Ｒａｄｉｏ Ｄｅｔｅｃｔｉｎｇ ａｎｄ Ｒａｎｇｉｎｇ）センサなどの各種測距センサを備えてもよい。センサ部２１０は、マイクロフォン、照度計、温度計又は湿度計などの環境センサを備えてもよい。また、センサ部２１０は、ロボット装置１０の自機情報を取得するセンサとして、例えば、エンコーダ、電圧計、電流計、歪みゲージ、圧力計、又はＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）等を備えてもよい。

ただし、センサ部２１０は、ロボット装置１０の周囲の環境情報、又はロボット装置１０の自機情報を取得することができれば、上述したセンサ以外の公知のセンサを備えてもよいことは言うまでもない。

認識部１１０は、センサ部２１０が取得した情報に基づいて、ロボット装置１０の周囲の環境状態、又はロボット装置１０の自機状態を認識する。具体的には、認識部１１０は、センサ部２１０が取得した環境情報に基づいて、障害物認識、形状認識、物体認識、マーカ認識、文字認識、白線認識、車線認識、又は音声認識を行うことで、ロボット装置１０の周囲の環境状態を認識してもよい。また、認識部１１０は、センサ部２１０が取得した自機情報に基づいて、位置認識、運動状態（速度、加速度又はジャーク等）認識、又は機体状態（電源残量、温度又は関節角等）認識を行うことで、ロボット装置１０の自機状態を認識してもよい。

認識部１１０による上記の認識は、いずれも公知の認識技術を用いることで行うことができる。認識部１１０による認識は、例えば、所定のルールに基づいて行われてもよく、機械学習アルゴリズムに基づいて行われてもよい。

軌道生成部１２０は、認識部１１０による認識結果に基づいて、ロボット装置１０の機械要素２００を動作させる軌道を生成する。具体的には、まず、軌道生成部１２０は、認識部１１０にて認識されたロボット装置１０の周囲の環境状態、及びロボット装置１０の自機状態に基づいて、機械要素２００を動作させる始点Ｐ_ｓ及び終点Ｐ_ｅを決定する。次に、軌道生成部１２０は、決定した始点Ｐ_ｓ及び終点Ｐ_ｅを結ぶ機械要素２００の軌道を生成し、軌道上の機械要素２００の位置と、該位置における機械要素２００の姿勢とを決定する。制御装置１００は、生成された軌道に基づいて駆動部２２０の駆動を制御することで、機械要素２００を生成された軌道に沿って動作させることができる。

また、軌道生成部１２０は、軌道を生成する際に、始点Ｐ_ｓから終点Ｐ_ｅまでを結ぶ軌道が通過する経由点Ｐ_ｖを設定し、経由点Ｐ_ｖにおける機械要素２００の位置を決定する。軌道生成部１２０は、例えば、軌道の探索を容易にするための中間点として経由点Ｐ_ｖを設定してもよく、障害物を回避するために経由点Ｐ_ｖを設定してもよく、ユーザからの指示によって経由点Ｐ_ｖを設定してもよい。また、軌道生成部１２０は、軌道上に複数の経由点Ｐ_ｖを設定してもよい。なお、軌道生成部１２０によって経由点Ｐ_ｖが設定される場合、軌道生成部１２０は、公知の探索アルゴリズムを用いて経由点Ｐ_ｖにおける機械要素２００の位置を設定してもよい。

ここで、本実施形態に係る制御装置１００では、経由点Ｐ_ｖにおける機械要素２００の姿勢は、後述する姿勢導出部１５０にて導出される。具体的には、経由点Ｐ_ｖにおける機械要素２００の姿勢は、補間姿勢導出部１３０にて導出される補間姿勢と、最適姿勢導出部１４０にて導出される最適姿勢とに基づいて導出される。

これによれば、制御装置１００は、始点Ｐ_ｓから終点Ｐ_ｅまでの経路を探索する際に、経由点Ｐ_ｖにおける機械要素２００の位置及び姿勢の両方ではなく、機械要素２００の位置だけを探索すればよくなる。したがって、制御装置１００は、探索アルゴリズムを用いて経由点Ｐ_ｖを設定する際に、機械要素２００の位置及び姿勢の６自由度ではなく、機械要素２００の位置の３自由度で探索を行えばよいため、探索の負荷及び時間を低減することができる。

補間姿勢導出部１３０は、始点Ｐ_ｓにおける機械要素２００の姿勢、及び終点Ｐ_ｅにおける機械要素２００の姿勢に基づいて、経由点Ｐ_ｖにおける補間姿勢を導出する。

具体的には、補間姿勢導出部１３０は、始点Ｐ_ｓにおける機械要素２００の姿勢、及び終点Ｐ_ｅにおける機械要素２００の姿勢の線形補間によって、経由点Ｐ_ｖにおける補間姿勢を導出してもよい。例えば、補間姿勢導出部１３０は、始点Ｐ_ｓから経由点Ｐ_ｖまでの距離と、終点Ｐ_ｅから経由点Ｐ_ｖまでの距離との比を用いて、始点Ｐ_ｓにおける姿勢と、終点Ｐ_ｅにおける姿勢との間を線形補間することで、経由点Ｐ_ｖにおける補間姿勢を導出してもよい。すなわち、補間姿勢導出部１３０は、始点Ｐ_ｓから終点Ｐ_ｅまでの機械要素２００の姿勢遷移の途中段階の一姿勢を補間姿勢として導出してもよい。

経由点Ｐ_ｖにおける補間姿勢は、後述する姿勢導出部１５０にて、経由点Ｐ_ｖにおいて機械要素２００が実際に制御される姿勢を導出するために用いられる。制御装置１００は、補間姿勢を用いて、機械要素２００が実際に制御される姿勢を導出することによって、経由点Ｐ_ｖにおける機械要素２００の姿勢を始点Ｐ_ｓ又は終点Ｐ_ｅにおける機械要素２００の姿勢から大きく変化しないようにすることができる。

最適姿勢導出部１４０は、機械要素２００の機構的特徴に基づいて、経由点Ｐ_ｖにおける最適姿勢を導出する。具体的には、最適姿勢導出部１４０は、機械要素２００の可操作度、機械要素２００に掛かるトルク、又は機械要素２００の特異点の少なくともいずれかに基づいて、経由点Ｐ_ｖにおける最適姿勢を導出してもよい。例えば、最適姿勢導出部１４０は、機械要素２００の可操作度が高くなるように最適姿勢を導出してもよく、機械要素２００に掛かるトルクが小さくなるように最適姿勢を導出してもよく、機械要素２００の特異点を避けるように最適姿勢を導出してもよい。すなわち、最適姿勢導出部１４０は、経由点Ｐ_ｖにおいて機械要素２００の機構が最適となる姿勢を最適姿勢として導出してもよい。

または、最適姿勢導出部１４０は、経由点Ｐ_ｖを通過する軌道の接線方向と、経由点Ｐ_ｖにおける機械要素２００の進入方向とが略一致するように、経由点Ｐ_ｖにおける最適姿勢を導出してもよい。

経由点Ｐ_ｖにおける補間姿勢は、後述する姿勢導出部１５０にて、経由点Ｐ_ｖにおいて機械要素２００が実際に制御される姿勢を導出するために用いられる。制御装置１００は、最適姿勢を用いて、機械要素２００が実際に制御される姿勢を導出することによって、経由点Ｐ_ｖにおける機械要素２００の姿勢をより負荷の小さい姿勢にすることができる。また、制御装置１００は、最適姿勢を用いて、機械要素２００が実際に制御される姿勢を導出することによって、経由点Ｐ_ｖにおける機械要素２００の姿勢を機械要素２００の動作がより適切となる姿勢にすることができる。

姿勢導出部１５０は、補間姿勢及び最適姿勢に基づいて、経由点Ｐ_ｖにおいて機械要素２００が実際に制御される姿勢を導出する。具体的には、姿勢導出部１５０は、補間姿勢及び最適姿勢の内分によって、経由点Ｐ_ｖにおいて機械要素２００が実際に制御される姿勢を導出してもよい。なお、補間姿勢及び最適姿勢の内分は、例えば、始点Ｐ_ｓ及び終点Ｐ_ｅにおいて補間姿勢の割合が１００％となるような連続関数に基づいて行うことができる。これによれば、姿勢導出部１５０は、経由点Ｐ_ｖにおける機械要素２００の姿勢をより適切に導出することができる。

制御部１６０は、軌道生成部１２０にて生成された軌道に基づいて、駆動部２２０の駆動を制御する。具体的には、まず、制御部１６０は、認識部１１０にて認識されたロボット装置１０の自機状態から機械要素２００の位置及び姿勢を把握する。次に、制御部１６０は、機械要素２００の位置及び姿勢と、軌道生成部１２０にて生成された軌道の位置及び姿勢との差が縮小するように、機械要素２００の駆動部２２０の駆動を制御する。これによれば、制御装置１００は、軌道生成部１２０にて生成された軌道に基づいて、機械要素２００を始点Ｐ_ｓから終点Ｐ_ｅまで動作させることができる。

駆動部２２０は、制御部１６０からの制御に基づいて、ロボット装置１０の機械要素２００の各部を駆動させる。例えば、駆動部２２０は、機械要素２００の関節を駆動させるアクチュエータ等であってもよい。

＜３．制御装置の動作例＞
続いて、図４〜図１０Ｃを参照して、機械要素２００の姿勢を導出する際の制御装置１００の動作例について説明する。図４は、機械要素２００の姿勢を導出する際の制御装置１００の動作例を説明するフローチャート図である。

まず、制御装置１００は、始点Ｐ_ｓ及び経由点Ｐ_ｖの間のユークリッド距離ｄ_ｓと、経由点Ｐ_ｖ及び終点Ｐ_ｅの間のユークリッド距離ｄ_ｖとを算出する（Ｓ１００）。次に、補間姿勢導出部１３０は、機械要素２００の姿勢を表すクォータニオンを球面線形補正することによって補間姿勢を導出する（Ｓ１１０）。

球面線形補間は、２つの独立したクォータニオンｑ_Ａ、ｑ_Ｂを補間する方法であり、パラメータｔ（ただし、０≦ｔ≦１）を用いた以下の式１にて行うことができる。これにより、図５に示すように、クォータニオンｑ_Ａ、ｑ_Ｂから補間クォータニオンｑ（ｔ）を導出することができる。

なお、クォータニオンでは、共役なクォータニオンは、同じ姿勢を表す。したがって、式１において、θの絶対値がπ／２を超える場合には、ｑ_Ａ又はｑ_Ｂのいずれかを、符号が反転した共役クォータニオンに置き換えて演算すればよい。

また、機械要素２００の姿勢がクォータニオンではなくオイラー角で表される場合、補間姿勢導出部１３０は、オイラー角のロール角φ、ピッチ角θ及びヨー角ψの各成分についてそれぞれ線形補間を行うことで、補間姿勢を導出することができる。例えば、ロール角φの線形補間は、パラメータｔ（ただし、０≦ｔ≦１）を用いて、以下の式２にて行うことができる。なお、ピッチ角θ、及びヨー角ψについても同様に線形補間を行うことができる。

このように、補間姿勢導出部１３０は、クォータニオンの球面線形補正を用いて、図６及び図７に示すように経由点Ｐ_ｖにおける機械要素２００の補間姿勢を導出することができる。具体的には、まず、補間姿勢導出部１３０は、図６に示すように、始点Ｐ_ｓ及び経由点Ｐ_ｖの間のユークリッド距離ｄ_ｓと、経由点Ｐ_ｖ及び終点Ｐ_ｅの間のユークリッド距離ｄ_ｖとを算出する。次に、以下の式３に示すように、算出したユークリッド距離ｄ_ｓ、ｄ_ｖの比を計算することで、球面線形補正のパラメータｔ_ｉを決定する。これにより、補間姿勢導出部１３０は、図７に示すように、経由点Ｐ_ｖにおける機械要素２００の補間姿勢を以下の式４にて導出することができる。

続いて、最適姿勢導出部１４０は、始点Ｐ_ｓ、経由点Ｐ_ｖ、及び終点Ｐ_ｅを滑らかに結ぶ曲線を導出し、該曲線の経由点Ｐ_ｖでの接線ベクトルＬを導出する（Ｓ１２０）。始点Ｐ_ｓ、経由点Ｐ_ｖ、及び終点Ｐ_ｅを滑らかに結ぶ曲線としては、例えば、ベジエ曲線、スプライン曲線、Ｂ−スプライン曲線、又はラグランジュ補間による曲線を例示することができる。

図８に示すように、ベジエ曲線の一例である二次ベジエ曲線は、３つの制御点Ｐ_０、Ｐ_１、及びＰ_２によって定義される曲線である。二次ベジエ曲線上の点Ｐは、３つの制御点Ｐ_０、Ｐ_１、及びＰ_２の位置、及びパラメータｓ（ただし、０≦ｓ≦１）を用いて、以下の式５にて表すことができる。

したがって、最適姿勢導出部１４０は、Ｐ_０を始点Ｐ_ｓとし、Ｐ_２を終点Ｐ_ｅとし、Ｐ_１を適切に制御することで、始点Ｐ_ｓ、経由点Ｐ_ｖ、及び終点Ｐ_ｅを滑らかに結ぶ曲線を導出することができる。Ｐ_１は、例えば、始点Ｐ_ｓ及び経由点Ｐ_ｖの間のユークリッド距離ｄ_ｓと、経由点Ｐ_ｖ及び終点Ｐ_ｅの間のユークリッド距離ｄ_ｖとの比ｓ_０をパラメータｓに代入した際の点が経由点Ｐ_ｖとなるように決定すればよい。これにより、最終的に導出される経由点Ｐ_ｖにおける接線ベクトルＬは、以下の式６のようになる。

次に、最適姿勢導出部１４０は、曲線の経由点Ｐ_ｖでの接線ベクトルＬと、機械要素２００の経由点Ｐ_ｖへの進入方向とが略一致するように機械要素２００の最適姿勢を導出する（Ｓ１３０）。

ただし、この方法では、機械要素２００の経由点Ｐ_ｖへの進入方向の軸方向の姿勢は定まらない。軸方向の機械要素２００の姿勢については、補間姿勢導出部１３０と同様に、始点Ｐ_ｓ及び終点Ｐ_ｅの機械要素２００の姿勢を線形補間することで導出することができる。

なお、上記の変形例として、最適姿勢導出部１４０は、曲線の経由点Ｐ_ｖでの接線ベクトルＬに替えて、始点Ｐ_ｓと経由点Ｐ_ｖとを通る直線ベクトル、及び経由点Ｐ_ｖと終点Ｐ_ｅとを通る直線ベクトルの二等分線方向ベクトルを用いることも可能である。このような場合、最適姿勢導出部１４０は、該二等分線方向ベクトルと、機械要素２００の経由点Ｐ_ｖへの進入方向とが略一致するように機械要素２００の最適姿勢を導出することができる。二等分線方向Ｌ_２は、例えば、以下の式７で表すことができる。

また、最適姿勢導出部１４０は、図９に示すように、機械要素２００の機構を考慮することで、最適姿勢ｑ_ｏを導出してもよい。例えば、最適姿勢導出部１４０は、機械要素２００が取り得る位置の各々について最適姿勢を導出したルックアップテーブルをあらかじめ用意していてもよい。ルックアップテーブルに記憶された最適姿勢は、例えば、可操作度が最大となる機械要素２００の姿勢、機械要素２００の各関節に掛かるトルク二乗和が最小となる機械要素２００の姿勢、又は特定のタスクに対応した機械要素２００の所定の姿勢であってもよい。

続いて、姿勢導出部１５０は、補間姿勢及び最適姿勢を内分する関数を決定する（Ｓ１４０）。その後、姿勢導出部１５０は、決定された関数ｆ（ｐ_ｖ）に基づいて、補間姿勢及び最適姿勢を内分することで、経由点Ｐ_ｖにおいて機械要素２００が実際に制御される姿勢を導出する（Ｓ１５０）。

例えば、経由点Ｐ_ｖにおいて機械要素２００が実際に制御される姿勢ｑ_ｖは、補間姿勢ｑ_ｉ及び最適姿勢ｑ_ｏを上述した関数に基づくｕ：１−ｕ（ただし、０≦ｕ≦１）にて内分することで、以下の式８に基づいて導出されてもよい。なお、姿勢ｑ_ｖ、補間姿勢ｑ_ｉ及び最適姿勢ｑ_ｏは、それぞれクォータニオンである。

ここで、経由点Ｐ_ｖが始点Ｐ_ｓ又は終点Ｐ_ｅと一致する場合、機械要素２００が実際に制御される姿勢ｑ_ｖは、補間姿勢ｑ_ｉ（すなわち、始点Ｐ_ｓ又は終点Ｐ_ｅにおける機械要素２００の姿勢）となるため、内分比ｕは、０に制御されることが望ましい。また、機械要素２００の姿勢の連続性を考慮すると、内分比ｕは連続的に変化することが望ましい。したがって、補間姿勢及び最適姿勢の内分比ｕは、経由点Ｐ_ｖの位置ｐ_ｖの関数ｆ（ｐ_ｖ）であり、以下の条件（１）〜（３）を満たすように決定されてもよい。なお、ｐ_ｓは、始点Ｐ_ｓの位置を表し、ｐ_ｅは、終点Ｐ_ｅの位置を表す。
（１）０≦ｆ（ｐ_ｖ）≦１
（２）ｆ（ｐ_ｓ）＝ｆ（ｐ_ｅ）＝０
（３）ｐ_ｖの定義域において、ｆ（ｐ_ｖ）は連続

例えば、始点Ｐ_ｓ及び経由点Ｐ_ｖの間のユークリッド距離ｄ_ｓと、経由点Ｐ_ｖ及び終点Ｐ_ｅの間のユークリッド距離ｄ_ｖとの比ｒを以下の式９で定義する場合、ｕを制御する関数ｆ（ｐ_ｖ）は、図１０Ａ〜図１０Ｃにてグラフを示す関数ｇ（ｒ）となってもよい。図１０Ａにてグラフを示す関数ｇ（ｒ）は、Ｂ−スプライン基底関数であり、図１０Ｂにてグラフを示す関数ｇ（ｒ）は、放物線を描く二次関数であり、図１０Ｃにてグラフを示す関数ｇ（ｒ）は、上記の条件（１）〜（３）を満たす任意の関数である。

以上の動作によれば、補間姿勢導出部１３０、最適姿勢導出部１４０、及び姿勢導出部１５０は、経由点Ｐ_ｖにおける機械要素２００の適切な姿勢を導出することができる。これにより、制御装置１００は、導出された姿勢に基づいて、機械要素２００の経由点Ｐ_ｖにおける姿勢を制御することができる（Ｓ１６０）。

なお、上記で説明した姿勢導出の方法は、あくまで一例であり、本開示に係る技術は、上記で説明した姿勢導出の方法に限定されるわけではない。

例えば、最適姿勢導出部１４０は、姿勢導出部１５０にて用いられる最適姿勢として、複数の観点からそれぞれ導出した複数の最適姿勢を重み付けして足し合わせたものを導出してもよい。また、姿勢導出部１５０は、補間姿勢及び最適姿勢の内分比をロボット装置１０の周囲の環境情報（例えば、障害物情報）、又は機械要素２００の可操作度若しくは特異点に関する情報に基づいて動的に制御してもよい。さらに、機械要素２００が取り得る姿勢に拘束条件が存在する場合、最適姿勢導出部１４０は、拘束条件を満たすように最適姿勢を制御してもよく、姿勢導出部１５０は、拘束条件を満たすように補間姿勢と最適姿勢との内分比を制御してもよい。

＜４．ハードウェア構成例＞
続いて、図１１を参照して、本実施形態に係る制御装置１００のハードウェア構成の一例について説明する。図１１は、本実施形態に係る制御装置１００のハードウェア構成の一例を示すブロック図である。

図１１に示すように、制御装置１００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）９０１、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）９０２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）９０３、ホストバス９０５、ブリッジ９０７、外部バス９０６、インタフェース９０８、入力装置９１１、出力装置９１２、ストレージ装置９１３、ドライブ９１４、接続ポート９１５、及び通信装置９１６を備える。制御装置１００は、ＣＰＵ９０１に替えて、又はこれと共に、電気回路、ＤＳＰ若しくはＡＳＩＣ等の処理回路を備えてもよい。

ＣＰＵ９０１は、演算処理装置および制御装置として機能し、各種プログラムに従って制御装置１００内の動作全般を制御する。また、ＣＰＵ９０１は、マイクロプロセッサであってもよい。ＲＯＭ９０２は、ＣＰＵ９０１が使用するプログラム及び演算パラメータ等を記憶する。ＲＡＭ９０３は、ＣＰＵ９０１の実行において使用するプログラム、及びその実行において適宜変化するパラメータ等を一時記憶する。ＣＰＵ９０１は、例えば、認識部１１０、軌道生成部１２０、補間姿勢導出部１３０、最適姿勢導出部１４０、姿勢導出部１５０、及び制御部１６０の機能を実行してもよい。

ＣＰＵ９０１、ＲＯＭ９０２及びＲＡＭ９０３は、ＣＰＵバスなどを含むホストバス９０５により相互に接続されている。ホストバス９０５は、ブリッジ９０７を介して、ＰＣＩ（Ｐｅｒｉｐｈｅｒａｌ Ｃｏｍｐｏｎｅｎｔ Ｉｎｔｅｒｃｏｎｎｅｃｔ／Ｉｎｔｅｒｆａｃｅ）バスなどの外部バス９０６に接続されている。なお、ホストバス９０５、ブリッジ９０７、及び外部バス９０６は、必ずしも分離構成されなくともよく、１つのバスにこれらの機能が実装されてもよい。

入力装置９１１は、例えば、マウス、キーボード、タッチパネル、ボタン、マイクロフォン、スイッチ又はレバー等のユーザによって情報が入力される装置である。または、入力装置９１１は、赤外線又はその他の電波を利用したリモートコントロール装置であってもよく、制御装置１００の操作に対応した携帯電話又はＰＤＡ等の外部接続機器であってもよい。さらに、入力装置９１１は、例えば、上記の入力手段を用いてユーザにより入力された情報に基づいて入力信号を生成する入力制御回路などを含んでもよい。

出力装置９１２は、情報をユーザに対して視覚的又は聴覚的に通知することが可能な装置である。出力装置９１２は、例えば、ＣＲＴ（Ｃａｔｈｏｄｅ Ｒａｙ Ｔｕｂｅ）ディスプレイ装置、液晶ディスプレイ装置、プラズマディスプレイ装置、ＥＬ（ＥｌｅｃｔｒｏＬｕｍｉｎｅｓｃｅｎｃｅ）ディスプレイ装置、レーザープロジェクタ、ＬＥＤ（Ｌｉｇｈｔ Ｅｍｉｔｔｉｎｇ Ｄｉｏｄｅ）プロジェクタ又はランプ等の表示装置であってもよく、スピーカ又はヘッドホン等の音声出力装置等であってもよい。

出力装置９１２は、例えば、制御装置１００による各種処理にて得られた結果を出力してもよい。具体的には、出力装置９１２は、制御装置１００による各種処理にて得られた結果を、テキスト、イメージ、表、又はグラフ等の様々な形式で視覚的に表示してもよい。または、出力装置９１２は、音声データ又は音響データ等のオーディオ信号をアナログ信号に変換して聴覚的に出力してもよい。

ストレージ装置９１３は、制御装置１００の記憶部の一例として形成されたデータ格納用の装置である。ストレージ装置９１３は、例えば、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の磁気記憶デバイス、半導体記憶デバイス、光記憶デバイス又は光磁気記憶デバイス等により実現されてもよい。例えば、ストレージ装置９１３は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置、及び記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置９１３は、ＣＰＵ９０１が実行するプログラム、各種データ及び外部から取得した各種のデータ等を格納してもよい。

ドライブ９１４は、記憶媒体用リーダライタであり、制御装置１００に内蔵又は外付けされる。ドライブ９１４は、装着されている磁気ディスク、光ディスク、光磁気ディスク、又は半導体メモリ等のリムーバブル記憶媒体に記録されている情報を読み出して、ＲＡＭ９０３に出力する。また、ドライブ９１４は、リムーバブル記憶媒体に情報を書き込むことも可能である。

接続ポート９１５は、外部機器と接続されるインタフェースである。接続ポート９１５は、外部機器とのデータ伝送可能な接続口であり、例えばＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）であってもよい。

通信装置９１６は、例えば、ネットワーク９２０に接続するための通信デバイス等で形成されたインタフェースである。通信装置９１６は、例えば、有線若しくは無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、ＬＴＥ（Ｌｏｎｇ Ｔｅｒｍ Ｅｖｏｌｕｔｉｏｎ）、Ｂｌｕｅｔｏｏｔｈ（登録商標）又はＷＵＳＢ（Ｗｉｒｅｌｅｓｓ ＵＳＢ）用の通信カード等であってもよい。また、通信装置９１６は、光通信用のルータ、ＡＤＳＬ（Ａｓｙｍｍｅｔｒｉｃ Ｄｉｇｉｔａｌ Ｓｕｂｓｃｒｉｂｅｒ Ｌｉｎｅ）用のルータ又は各種通信用のモデム等であってもよい。通信装置９１６は、例えば、インターネット又は他の通信機器との間で、例えばＴＣＰ／ＩＰ等の所定のプロトコルに則して信号等を送受信することができる。

なお、ネットワーク９２０は、情報の有線又は無線の伝送路である。例えば、ネットワーク９２０は、インターネット、電話回線網若しくは衛星通信網などの公衆回線網、Ｅｔｈｅｒｎｅｔ（登録商標）を含む各種のＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、又はＷＡＮ（Ｗｉｄｅ Ａｒｅａ Ｎｅｔｗｏｒｋ）などを含んでもよい。また、ネットワーク９２０は、ＩＰ−ＶＰＮ（Ｉｎｔｅｒｎｅｔ Ｐｒｏｔｏｃｏｌ−Ｖｉｒｔｕａｌ Ｐｒｉｖａｔｅ Ｎｅｔｗｏｒｋ）などの専用回線網を含んでもよい。

なお、制御装置１００に内蔵されるＣＰＵ、ＲＯＭおよびＲＡＭなどのハードウェアに対して、上述した本実施形態に係る制御装置の各構成と同等の機能を発揮させるためのコンピュータプログラムも作成可能である。また、該コンピュータプログラムを記憶させた記憶媒体も提供することが可能である。

＜５．まとめ＞
以上にて説明した本実施形態に係る制御装置１００は、経由点Ｐ_ｖにおける機械要素２００の姿勢を、始点Ｐ_ｓ及び終点Ｐ_ｅにおける機械要素２００の姿勢の単純補間のみではなく、経由点Ｐ_ｖにおける機械要素２００の最適姿勢を考慮して導出することができる。

これによれば、制御装置１００は、経由点Ｐ_ｖにおける機械要素２００の姿勢をより適切な姿勢とすることができるため、機械要素２００の到達可能範囲を広げることができる。

また、制御装置１００は、機械要素２００に対する要求又は拘束条件を満たすように最適姿勢を設定することで、経由点Ｐ_ｖにおける機械要素２００の姿勢を要求又は拘束条件に対応して制御することができる。

さらに、制御装置１００は、経由点Ｐ_ｖにおける機械要素２００の姿勢を位置から導出することができるため、教示によって軌道生成する場合に、機械要素２００の姿勢を教示せずとも、機械要素２００の位置のみを教示すればよくなる。これによれば、制御装置１００は、教示の効率を向上させることができる。

加えて、制御装置１００は、軌道の探索時に探索空間を機械要素２００の位置及び姿勢の６次元空間から機械要素２００の位置のみの３次元空間に次元数を減少させることができるため、より効率的に探索を行うことができる。

以上、添付図面を参照しながら本開示の好適な実施形態について詳細に説明したが、本開示の技術的範囲はかかる例に限定されない。本開示の技術分野における通常の知識を有する者であれば、請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本開示の技術的範囲に属するものと了解される。

また、本明細書に記載された効果は、あくまで説明的または例示的なものであって限定的ではない。つまり、本開示に係る技術は、上記の効果とともに、または上記の効果に代えて、本明細書の記載から当業者には明らかな他の効果を奏しうる。

なお、以下のような構成も本開示の技術的範囲に属する。
（１）
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
を備える、制御装置。
（２）
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢に基づいて、前記補間姿勢を導出する、前記（１）に記載の制御装置。
（３）
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢の線形補間によって、前記補間姿勢を導出する、前記（２）に記載の制御装置。
（４）
前記線形補間は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づいて行われる、前記（３）に記載の制御装置。
（５）
前記最適姿勢導出部は、前記機械要素の機構的特徴に基づいて、前記最適姿勢を導出する、前記（１）〜（４）のいずれか一項に記載の制御装置。
（６）
前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか１つ以上に基づいて、前記最適姿勢を導出する、前記（５）に記載の制御装置。
（７）
前記最適姿勢導出部は、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、前記（５）に記載の制御装置。
（８）
前記曲線は、ベジエ曲線、スプライン曲線、Ｂ−スプライン曲線、又はラグランジュ補間による曲線のいずれかである、前記（７）に記載の制御装置。
（９）
前記最適姿勢導出部は、複数の姿勢を重み付けして重ね合わせることで、前記最適姿勢を導出する、前記（５）〜（８）のいずれか一項に記載の制御装置。
（１０）
前記姿勢導出部は、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する、前記（１）〜（９）のいずれか一項に記載の制御装置。
（１１）
前記内分の割合は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づく関数に基づいて決定される、前記（１０）に記載の制御装置。
（１２）
前記関数は、連続関数である、前記（１１）に記載の制御装置。
（１３）
前記関数は、前記経由点が前記始点又は前記終点に一致する場合、前記経由点にて前記機械要素が制御される姿を前記補間姿勢とする関数である、前記（１１）又は（１２）に記載の制御装置。
（１４）
前記関数は、環境情報、又は前記機械要素の自機情報に基づいて変更される、前記（１１）〜（１３）のいずれか一項に記載の制御装置。
（１５）
演算装置によって、
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、
前記機械要素の前記経由点における最適姿勢を導出することと、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出することと、
を含む、制御方法。
（１６）
コンピュータを
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
として機能させる、プログラム。

１０ ロボット装置
１００ 制御装置
１１０ 認識部
１２０ 軌道生成部
１３０ 補間姿勢導出部
１４０ 最適姿勢導出部
１５０ 姿勢導出部
１６０ 制御部
２００ 機械要素
２１０ センサ部
２２０ 駆動部

Claims (16)

1. 始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
   前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
   前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
   を備える、制御装置。
2. 前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢に基づいて、前記補間姿勢を導出する、請求項１に記載の制御装置。
3. 前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢の線形補間によって、前記補間姿勢を導出する、請求項２に記載の制御装置。
4. 前記線形補間は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づいて行われる、請求項３に記載の制御装置。
5. 前記最適姿勢導出部は、前記機械要素の機構的特徴に基づいて、前記最適姿勢を導出する、請求項１に記載の制御装置。
6. 前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか１つ以上に基づいて、前記最適姿勢を導出する、請求項５に記載の制御装置。
7. 前記最適姿勢導出部は、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、請求項５に記載の制御装置。
8. 前記曲線は、ベジエ曲線、スプライン曲線、Ｂ−スプライン曲線、又はラグランジュ補間による曲線のいずれかである、請求項７に記載の制御装置。
9. 前記最適姿勢導出部は、複数の姿勢を重み付けして重ね合わせることで、前記最適姿勢を導出する、請求項５に記載の制御装置。
10. 前記姿勢導出部は、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する、請求項１に記載の制御装置。
11. 前記内分の割合は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づく関数に基づいて決定される、請求項１０に記載の制御装置。
12. 前記関数は、連続関数である、請求項１１に記載の制御装置。
13. 前記関数は、前記経由点が前記始点又は前記終点に一致する場合、前記経由点にて前記機械要素が制御される姿を前記補間姿勢とする関数である、請求項１１に記載の制御装置。
14. 前記関数は、環境情報、又は前記機械要素の自機情報に基づいて変更される、請求項１１に記載の制御装置。
15. 演算装置によって、
    始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、
    前記機械要素の前記経由点における最適姿勢を導出することと、
    前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出することと、
    を含む、制御方法。
16. コンピュータを
    始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
    前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
    前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
    として機能させる、プログラム。

Images