(57)【要約】  始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、を備える、制御装置。
2020141579.tif 000002
【特許請求の範囲】
【請求項１】
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
を備える、制御装置。
【請求項２】
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢に基づいて、前記補間姿勢を導出する、請求項１に記載の制御装置。
【請求項３】
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢の線形補間によって、前記補間姿勢を導出する、請求項２に記載の制御装置。
【請求項４】
前記線形補間は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づいて行われる、請求項３に記載の制御装置。
【請求項５】
前記最適姿勢導出部は、前記機械要素の機構的特徴に基づいて、前記最適姿勢を導出する、請求項１に記載の制御装置。
【請求項６】
前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか１つ以上に基づいて、前記最適姿勢を導出する、請求項５に記載の制御装置。
【請求項７】
前記最適姿勢導出部は、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、請求項５に記載の制御装置。
【請求項８】
前記曲線は、ベジエ曲線、スプライン曲線、Ｂ−スプライン曲線、又はラグランジュ補間による曲線のいずれかである、請求項７に記載の制御装置。
【請求項９】
前記最適姿勢導出部は、複数の姿勢を重み付けして重ね合わせることで、前記最適姿勢を導出する、請求項５に記載の制御装置。
【請求項１０】
前記姿勢導出部は、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する、請求項１に記載の制御装置。
【請求項１１】
前記内分の割合は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づく関数に基づいて決定される、請求項１０に記載の制御装置。
【請求項１２】
前記関数は、連続関数である、請求項１１に記載の制御装置。
【請求項１３】
前記関数は、前記経由点が前記始点又は前記終点に一致する場合、前記経由点にて前記機械要素が制御される姿を前記補間姿勢とする関数である、請求項１１に記載の制御装置。
【請求項１４】
前記関数は、環境情報、又は前記機械要素の自機情報に基づいて変更される、請求項１１に記載の制御装置。
【請求項１５】
演算装置によって、
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、
前記機械要素の前記経由点における最適姿勢を導出することと、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出することと、
を含む、制御方法。
【請求項１６】
コンピュータを
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
として機能させる、プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、制御装置、制御方法、及びプログラムに関する。
【背景技術】
【０００２】
一般的に、ロボット装置に備えられるマニュピュレータ等（以下では、機械要素とも称する）では、該マニュピュレータ等の位置及び姿勢を指示する動作軌道に基づいて動作が制御される。【０００３】
このような機械要素の動作を適切に制御するためには、動作の始点から終点までをつなぐ動作軌道において、機械要素の位置及び姿勢を適切に決定することが重要となる。ただし、動作軌道を計画する際に機械要素の位置及び姿勢を同時に探索することは、探索の自由度が高くなり（合計６自由度）、探索に時間が掛かってしまうため、好ましくない。【０００４】
そこで、まず、動作軌道上に経由点を定めて、経由点における機械要素の位置（３自由度）を探索し、次に、探索した機械要素の位置に基づいて機械要素の姿勢（３自由度）を決定することが検討されている。【０００５】
例えば、下記の特許文献１には、軌道上の位置データと、マニュピュレータの姿勢データとを対応付けした姿勢データテーブルをあらかじめ記憶し、マニュピュレータの位置データに基づいてマニュピュレータの姿勢を決定することが記載されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平１０−３１５１６９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかし、経由点における機械要素の位置に基づいて機械要素の姿勢を決定した場合、決定された機械要素の姿勢は、機械要素の機構的特徴を考慮した適切な姿勢となっていないことがあり得る。そのため、動作軌道の始点から終点までの間に存在する経由点における機械要素の姿勢をより適切に導出することが可能な制御装置、制御方法、及びプログラムが求められていた。
【課題を解決するための手段】
【０００８】
本開示によれば、始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、を備える、制御装置が提供される。【０００９】
また、本開示によれば、演算装置によって、始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、前記機械要素の前記経由点における最適姿勢を導出することと、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出することと、を含む、制御方法が提供される。【００１０】
また、本開示によれば、コンピュータを始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、として機能させる、プログラムが提供される。
【図面の簡単な説明】
【００１１】
【図１Ａ】始点における機械要素の位置及び姿勢の一例を示す模式図である。
【図１Ｂ】終点における機械要素の位置及び姿勢の一例を示す模式図である。
【図２Ａ】経由点における機械要素の位置及び姿勢の一例を示す模式図である。
【図２Ｂ】経由点における機械要素の位置及び姿勢の他の例を示す模式図である。
【図３】本開示の一実施形態に係る制御装置の機能構成を示すブロック図である。
【図４】機械要素の姿勢を導出する際の制御装置の動作例を説明するフローチャート図である。
【図５】クォータニオンによる球面線形補間を説明する説明図である。
【図６】始点及び経由点の間のユークリッド距離と、経由点及び終点の間のユークリッド距離とを説明する説明図である。
【図７】クォータニオンの球面線形補正を用いた、経由点における機械要素の補間姿勢の導出を説明する説明図である。
【図８】始点、経由点及び終点を通る二次ベジエ曲線を説明する説明図である。
【図９】機械要素の機構を考慮した、経由点における機械要素の最適姿勢の導出を説明する説明図である。
【図１０Ａ】補間姿勢及び最適姿勢の内分比を制御する関数の一例を示すグラフ図である。
【図１０Ｂ】補間姿勢及び最適姿勢の内分比を制御する関数の他の例を示すグラフ図である。
【図１０Ｃ】補間姿勢及び最適姿勢の内分比を制御する関数の他の例を示すグラフ図である。
【図１１】本開示の一実施形態に係る制御装置のハードウェア構成の一例を示すブロック図である。
【発明を実施するための形態】
【００１２】
以下に添付図面を参照しながら、本開示の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。
【００１３】
なお、説明は以下の順序で行うものとする。
１．本開示に係る技術の概要
２．制御装置の構成例
３．制御装置の動作例
４．ハードウェア構成例
５．まとめ
【００１４】
＜１．本開示に係る技術の概要＞
まず、図１Ａ〜図２Ｂを参照して、本開示に係る技術の概要について説明する。
【００１５】
例えば、始点Ｐｓから終点Ｐｅまで、経由点Ｐｖを介してロボット装置１０のロボットアーム等の機械要素２００を動作させる場合について考える。具体的には、図１Ａに示す始点Ｐｓにおける機械要素２００の位置及び姿勢から、図１Ｂに示す終点Ｐｅにおける機械要素２００の位置及び姿勢までロボット装置１０を動作させる場合について考える。なお、説明を簡単にするため、図１Ａ及び図１Ｂに示すように、始点Ｐｓ及び終点Ｐｅにおける機械要素２００の姿勢は、同一とした。
【００１６】
このとき、経由点Ｐｖにおける機械要素２００の姿勢は、例えば、始点Ｐｓ及び終点Ｐｅにおける機械要素２００の姿勢から導出することができる。具体的には、経由点Ｐｖにおける機械要素２００の姿勢は、始点Ｐｓ及び終点Ｐｅの各々における機械要素２００の姿勢を足し合わせることで導出することができる。例えば、図１Ａに示す始点Ｐｓにおける機械要素２００の姿勢は、図１Ｂに示す終点Ｐｅにおける機械要素２００の姿勢と同一である。そのため、経由点Ｐｖにおける機械要素２００の姿勢は、始点Ｐｓ及び終点Ｐｅにおける機械要素２００の姿勢と同一として導出することができる。
【００１７】
しかしながら、このような方法で経由点Ｐｖにおける機械要素２００の姿勢を導出する場合、経由点Ｐｖの位置によっては、図２Ａに示すように、機械要素２００の姿勢が適切となる解が存在しないことがあり得る。また、経由点Ｐｖの位置によっては、機械要素２００の姿勢が不自然又は非効率な姿勢となってしまうことがあり得る。
【００１８】
一方、経由点Ｐｖにおける機械要素２００の姿勢は、始点Ｐｓ及び終点Ｐｅにおける機械要素２００の姿勢を考慮せずに、例えば、機械要素２００の機械的特徴から導出することもできる。具体的には、経由点Ｐｖにおける機械要素２００の姿勢は、経由点Ｐｖにおける機械要素２００の各部位に掛かるトルクが小さくなるように、又は機械要素２００の可操作性が高くなるように導出することができる。
【００１９】
しかしながら、このような方法で経由点Ｐｖにおける機械要素２００の姿勢を導出した場合、図２Ｂに示すように、機械要素２００の姿勢が始点Ｐｓ又は終点Ｐｅにおける姿勢から大きく変化したものとなり、機械要素２００の動作効率が低下してしまうことがあり得る。
【００２０】
したがって、経由点Ｐｖにおける機械要素２００の姿勢は、互いに異なる方法にて導出された複数の姿勢を考慮して導出されることが望ましい。本開示に係る技術は、上述した事情を鑑みて想到されたものである。具体的には、本開示に係る技術は、経由点Ｐｖにおける機械要素２００の姿勢を補間姿勢及び最適姿勢に基づいて導出するものである。
【００２１】
ここで、補間姿勢とは、始点Ｐｓにおける機械要素２００の姿勢、及び終点Ｐｅにおける機械要素２００の姿勢の間を補間することで導出される姿勢である。また、最適姿勢とは、機械要素２００の機構的特徴に基づいて導出される姿勢である。本開示に係る技術によれば、補間姿勢及び最適姿勢を用いて機械要素２００の姿勢を導出することによって、経由点Ｐｖにおける機械要素２００の姿勢をより適切に制御することが可能である。
【００２２】
以下では、上記で概要を説明した本開示に係る技術を実現する制御装置について詳述する。
【００２３】
＜２．制御装置の構成例＞
続いて、図３を参照して、本開示の一実施形態に係る制御装置１００の具体的な構成例について説明する。図３は、本実施形態に係る制御装置１００の機能構成を示すブロック図である。
【００２４】
図３に示すように、制御装置１００は、認識部１１０と、軌道生成部１２０と、補間姿勢導出部１３０と、最適姿勢導出部１４０と、姿勢導出部１５０と、制御部１６０と、を備える。
【００２５】
例えば、制御装置１００は、センサ部２１０にて取得された情報に基づいて、機械要素２００の駆動部２２０を制御することで、機械要素２００の位置及び姿勢を制御するものである。制御装置１００は、機械要素２００を備えるロボット装置１０の内部に設けられてもよく、機械要素２００を備えるロボット装置１０の外部に設けられてもよい。
【００２６】
ここで、機械要素２００とは、ロボット装置１０に備えられる部位のうち機械的機構によって位置及び姿勢を制御可能な部位を表す。例えば、機械要素２００は、ロボット装置１０のロボットアーム又は該ロボットアームの先端のエンドエフェクタであってもよく、ロボット装置１０の脚部又は該脚部の先端の接地部であってもよい。
【００２７】
センサ部２１０は、ロボット装置１０の周囲の環境情報を取得するセンサと、ロボット装置１０の自機情報を取得するセンサと、を備える。例えば、センサ部２１０は、ロボット装置１０の周囲の環境情報を取得するセンサとして、ＲＧＢカメラ、グレースケールカメラ、ステレオカメラ、デプスカメラ、赤外線カメラ又はＴｏＦ（Ｔｉｍｅ  ｏｆ  Ｆｌｉｇｈｔ）カメラ等の各種カメラを備えてもよい。センサ部２１０は、ＬＩＤＡＲ（Ｌａｓｅｒ  Ｉｍａｇｉｎｇ  Ｄｅｔｅｃｔｉｏｎ  ａｎｄ  Ｒａｎｇｉｎｇ）センサ又はＲＡＤＡＲ（Ｒａｄｉｏ  Ｄｅｔｅｃｔｉｎｇ  ａｎｄ  Ｒａｎｇｉｎｇ）センサなどの各種測距センサを備えてもよい。センサ部２１０は、マイクロフォン、照度計、温度計又は湿度計などの環境センサを備えてもよい。また、センサ部２１０は、ロボット装置１０の自機情報を取得するセンサとして、例えば、エンコーダ、電圧計、電流計、歪みゲージ、圧力計、又はＩＭＵ（Ｉｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ）等を備えてもよい。
【００２８】
ただし、センサ部２１０は、ロボット装置１０の周囲の環境情報、又はロボット装置１０の自機情報を取得することができれば、上述したセンサ以外の公知のセンサを備えてもよいことは言うまでもない。
【００２９】
認識部１１０は、センサ部２１０が取得した情報に基づいて、ロボット装置１０の周囲の環境状態、又はロボット装置１０の自機状態を認識する。具体的には、認識部１１０は、センサ部２１０が取得した環境情報に基づいて、障害物認識、形状認識、物体認識、マーカ認識、文字認識、白線認識、車線認識、又は音声認識を行うことで、ロボット装置１０の周囲の環境状態を認識してもよい。また、認識部１１０は、センサ部２１０が取得した自機情報に基づいて、位置認識、運動状態（速度、加速度又はジャーク等）認識、又は機体状態（電源残量、温度又は関節角等）認識を行うことで、ロボット装置１０の自機状態を認識してもよい。
【００３０】
認識部１１０による上記の認識は、いずれも公知の認識技術を用いることで行うことができる。認識部１１０による認識は、例えば、所定のルールに基づいて行われてもよく、機械学習アルゴリズムに基づいて行われてもよい。
【００３１】
軌道生成部１２０は、認識部１１０による認識結果に基づいて、ロボット装置１０の機械要素２００を動作させる軌道を生成する。具体的には、まず、軌道生成部１２０は、認識部１１０にて認識されたロボット装置１０の周囲の環境状態、及びロボット装置１０の自機状態に基づいて、機械要素２００を動作させる始点Ｐｓ及び終点Ｐｅを決定する。次に、軌道生成部１２０は、決定した始点Ｐｓ及び終点Ｐｅを結ぶ機械要素２００の軌道を生成し、軌道上の機械要素２００の位置と、該位置における機械要素２００の姿勢とを決定する。制御装置１００は、生成された軌道に基づいて駆動部２２０の駆動を制御することで、機械要素２００を生成された軌道に沿って動作させることができる。
【００３２】
また、軌道生成部１２０は、軌道を生成する際に、始点Ｐｓから終点Ｐｅまでを結ぶ軌道が通過する経由点Ｐｖを設定し、経由点Ｐｖにおける機械要素２００の位置を決定する。軌道生成部１２０は、例えば、軌道の探索を容易にするための中間点として経由点Ｐｖを設定してもよく、障害物を回避するために経由点Ｐｖを設定してもよく、ユーザからの指示によって経由点Ｐｖを設定してもよい。また、軌道生成部１２０は、軌道上に複数の経由点Ｐｖを設定してもよい。なお、軌道生成部１２０によって経由点Ｐｖが設定される場合、軌道生成部１２０は、公知の探索アルゴリズムを用いて経由点Ｐｖにおける機械要素２００の位置を設定してもよい。
【００３３】
ここで、本実施形態に係る制御装置１００では、経由点Ｐｖにおける機械要素２００の姿勢は、後述する姿勢導出部１５０にて導出される。具体的には、経由点Ｐｖにおける機械要素２００の姿勢は、補間姿勢導出部１３０にて導出される補間姿勢と、最適姿勢導出部１４０にて導出される最適姿勢とに基づいて導出される。
【００３４】
これによれば、制御装置１００は、始点Ｐｓから終点Ｐｅまでの経路を探索する際に、経由点Ｐｖにおける機械要素２００の位置及び姿勢の両方ではなく、機械要素２００の位置だけを探索すればよくなる。したがって、制御装置１００は、探索アルゴリズムを用いて経由点Ｐｖを設定する際に、機械要素２００の位置及び姿勢の６自由度ではなく、機械要素２００の位置の３自由度で探索を行えばよいため、探索の負荷及び時間を低減することができる。
【００３５】
補間姿勢導出部１３０は、始点Ｐｓにおける機械要素２００の姿勢、及び終点Ｐｅにおける機械要素２００の姿勢に基づいて、経由点Ｐｖにおける補間姿勢を導出する。
【００３６】
具体的には、補間姿勢導出部１３０は、始点Ｐｓにおける機械要素２００の姿勢、及び終点Ｐｅにおける機械要素２００の姿勢の線形補間によって、経由点Ｐｖにおける補間姿勢を導出してもよい。例えば、補間姿勢導出部１３０は、始点Ｐｓから経由点Ｐｖまでの距離と、終点Ｐｅから経由点Ｐｖまでの距離との比を用いて、始点Ｐｓにおける姿勢と、終点Ｐｅにおける姿勢との間を線形補間することで、経由点Ｐｖにおける補間姿勢を導出してもよい。すなわち、補間姿勢導出部１３０は、始点Ｐｓから終点Ｐｅまでの機械要素２００の姿勢遷移の途中段階の一姿勢を補間姿勢として導出してもよい。
【００３７】
経由点Ｐｖにおける補間姿勢は、後述する姿勢導出部１５０にて、経由点Ｐｖにおいて機械要素２００が実際に制御される姿勢を導出するために用いられる。制御装置１００は、補間姿勢を用いて、機械要素２００が実際に制御される姿勢を導出することによって、経由点Ｐｖにおける機械要素２００の姿勢を始点Ｐｓ又は終点Ｐｅにおける機械要素２００の姿勢から大きく変化しないようにすることができる。
【００３８】
最適姿勢導出部１４０は、機械要素２００の機構的特徴に基づいて、経由点Ｐｖにおける最適姿勢を導出する。具体的には、最適姿勢導出部１４０は、機械要素２００の可操作度、機械要素２００に掛かるトルク、又は機械要素２００の特異点の少なくともいずれかに基づいて、経由点Ｐｖにおける最適姿勢を導出してもよい。例えば、最適姿勢導出部１４０は、機械要素２００の可操作度が高くなるように最適姿勢を導出してもよく、機械要素２００に掛かるトルクが小さくなるように最適姿勢を導出してもよく、機械要素２００の特異点を避けるように最適姿勢を導出してもよい。すなわち、最適姿勢導出部１４０は、経由点Ｐｖにおいて機械要素２００の機構が最適となる姿勢を最適姿勢として導出してもよい。
【００３９】
または、最適姿勢導出部１４０は、経由点Ｐｖを通過する軌道の接線方向と、経由点Ｐｖにおける機械要素２００の進入方向とが略一致するように、経由点Ｐｖにおける最適姿勢を導出してもよい。
【００４０】
経由点Ｐｖにおける補間姿勢は、後述する姿勢導出部１５０にて、経由点Ｐｖにおいて機械要素２００が実際に制御される姿勢を導出するために用いられる。制御装置１００は、最適姿勢を用いて、機械要素２００が実際に制御される姿勢を導出することによって、経由点Ｐｖにおける機械要素２００の姿勢をより負荷の小さい姿勢にすることができる。また、制御装置１００は、最適姿勢を用いて、機械要素２００が実際に制御される姿勢を導出することによって、経由点Ｐｖにおける機械要素２００の姿勢を機械要素２００の動作がより適切となる姿勢にすることができる。
【００４１】
姿勢導出部１５０は、補間姿勢及び最適姿勢に基づいて、経由点Ｐｖにおいて機械要素２００が実際に制御される姿勢を導出する。具体的には、姿勢導出部１５０は、補間姿勢及び最適姿勢の内分によって、経由点Ｐｖにおいて機械要素２００が実際に制御される姿勢を導出してもよい。なお、補間姿勢及び最適姿勢の内分は、例えば、始点Ｐｓ及び終点Ｐｅにおいて補間姿勢の割合が１００％となるような連続関数に基づいて行うことができる。これによれば、姿勢導出部１５０は、経由点Ｐｖにおける機械要素２００の姿勢をより適切に導出することができる。
【００４２】
制御部１６０は、軌道生成部１２０にて生成された軌道に基づいて、駆動部２２０の駆動を制御する。具体的には、まず、制御部１６０は、認識部１１０にて認識されたロボット装置１０の自機状態から機械要素２００の位置及び姿勢を把握する。次に、制御部１６０は、機械要素２００の位置及び姿勢と、軌道生成部１２０にて生成された軌道の位置及び姿勢との差が縮小するように、機械要素２００の駆動部２２０の駆動を制御する。これによれば、制御装置１００は、軌道生成部１２０にて生成された軌道に基づいて、機械要素２００を始点Ｐｓから終点Ｐｅまで動作させることができる。
【００４３】
駆動部２２０は、制御部１６０からの制御に基づいて、ロボット装置１０の機械要素２００の各部を駆動させる。例えば、駆動部２２０は、機械要素２００の関節を駆動させるアクチュエータ等であってもよい。
【００４４】
＜３．制御装置の動作例＞
続いて、図４〜図１０Ｃを参照して、機械要素２００の姿勢を導出する際の制御装置１００の動作例について説明する。図４は、機械要素２００の姿勢を導出する際の制御装置１００の動作例を説明するフローチャート図である。
【００４５】
まず、制御装置１００は、始点Ｐｓ及び経由点Ｐｖの間のユークリッド距離ｄｓと、経由点Ｐｖ及び終点Ｐｅの間のユークリッド距離ｄｖとを算出する（Ｓ１００）。次に、補間姿勢導出部１３０は、機械要素２００の姿勢を表すクォータニオンを球面線形補正することによって補間姿勢を導出する（Ｓ１１０）。
【００４６】
球面線形補間は、２つの独立したクォータニオンｑＡ、ｑＢを補間する方法であり、パラメータｔ（ただし、０≦ｔ≦１）を用いた以下の式１にて行うことができる。これにより、図５に示すように、クォータニオンｑＡ、ｑＢから補間クォータニオンｑ（ｔ）を導出することができる。
【００４７】
【数１】
2020141579.tif 000003
【００４８】
なお、クォータニオンでは、共役なクォータニオンは、同じ姿勢を表す。したがって、式１において、θの絶対値がπ／２を超える場合には、ｑＡ又はｑＢのいずれかを、符号が反転した共役クォータニオンに置き換えて演算すればよい。
【００４９】
また、機械要素２００の姿勢がクォータニオンではなくオイラー角で表される場合、補間姿勢導出部１３０は、オイラー角のロール角φ、ピッチ角θ及びヨー角ψの各成分についてそれぞれ線形補間を行うことで、補間姿勢を導出することができる。例えば、ロール角φの線形補間は、パラメータｔ（ただし、０≦ｔ≦１）を用いて、以下の式２にて行うことができる。なお、ピッチ角θ、及びヨー角ψについても同様に線形補間を行うことができる。
【００５０】
【数２】
2020141579.tif 000004
【００５１】
このように、補間姿勢導出部１３０は、クォータニオンの球面線形補正を用いて、図６及び図７に示すように経由点Ｐｖにおける機械要素２００の補間姿勢を導出することができる。具体的には、まず、補間姿勢導出部１３０は、図６に示すように、始点Ｐｓ及び経由点Ｐｖの間のユークリッド距離ｄｓと、経由点Ｐｖ及び終点Ｐｅの間のユークリッド距離ｄｖとを算出する。次に、以下の式３に示すように、算出したユークリッド距離ｄｓ、ｄｖの比を計算することで、球面線形補正のパラメータｔｉを決定する。これにより、補間姿勢導出部１３０は、図７に示すように、経由点Ｐｖにおける機械要素２００の補間姿勢を以下の式４にて導出することができる。
【００５２】
【数３】
2020141579.tif 000005
【００５３】
続いて、最適姿勢導出部１４０は、始点Ｐｓ、経由点Ｐｖ、及び終点Ｐｅを滑らかに結ぶ曲線を導出し、該曲線の経由点Ｐｖでの接線ベクトルＬを導出する（Ｓ１２０）。始点Ｐｓ、経由点Ｐｖ、及び終点Ｐｅを滑らかに結ぶ曲線としては、例えば、ベジエ曲線、スプライン曲線、Ｂ−スプライン曲線、又はラグランジュ補間による曲線を例示することができる。
【００５４】
図８に示すように、ベジエ曲線の一例である二次ベジエ曲線は、３つの制御点Ｐ０、Ｐ１、及びＰ２によって定義される曲線である。二次ベジエ曲線上の点Ｐは、３つの制御点Ｐ０、Ｐ１、及びＰ２の位置、及びパラメータｓ（ただし、０≦ｓ≦１）を用いて、以下の式５にて表すことができる。
【００５５】
【数４】
2020141579.tif 000006
【００５６】
したがって、最適姿勢導出部１４０は、Ｐ０を始点Ｐｓとし、Ｐ２を終点Ｐｅとし、Ｐ１を適切に制御することで、始点Ｐｓ、経由点Ｐｖ、及び終点Ｐｅを滑らかに結ぶ曲線を導出することができる。Ｐ１は、例えば、始点Ｐｓ及び経由点Ｐｖの間のユークリッド距離ｄｓと、経由点Ｐｖ及び終点Ｐｅの間のユークリッド距離ｄｖとの比ｓ０をパラメータｓに代入した際の点が経由点Ｐｖとなるように決定すればよい。これにより、最終的に導出される経由点Ｐｖにおける接線ベクトルＬは、以下の式６のようになる。
【００５７】
【数５】
2020141579.tif 000007
【００５８】
次に、最適姿勢導出部１４０は、曲線の経由点Ｐｖでの接線ベクトルＬと、機械要素２００の経由点Ｐｖへの進入方向とが略一致するように機械要素２００の最適姿勢を導出する（Ｓ１３０）。
【００５９】
ただし、この方法では、機械要素２００の経由点Ｐｖへの進入方向の軸方向の姿勢は定まらない。軸方向の機械要素２００の姿勢については、補間姿勢導出部１３０と同様に、始点Ｐｓ及び終点Ｐｅの機械要素２００の姿勢を線形補間することで導出することができる。
【００６０】
なお、上記の変形例として、最適姿勢導出部１４０は、曲線の経由点Ｐｖでの接線ベクトルＬに替えて、始点Ｐｓと経由点Ｐｖとを通る直線ベクトル、及び経由点Ｐｖと終点Ｐｅとを通る直線ベクトルの二等分線方向ベクトルを用いることも可能である。このような場合、最適姿勢導出部１４０は、該二等分線方向ベクトルと、機械要素２００の経由点Ｐｖへの進入方向とが略一致するように機械要素２００の最適姿勢を導出することができる。二等分線方向Ｌ２は、例えば、以下の式７で表すことができる。
【００６１】
【数６】
2020141579.tif 000008
【００６２】
また、最適姿勢導出部１４０は、図９に示すように、機械要素２００の機構を考慮することで、最適姿勢ｑｏを導出してもよい。例えば、最適姿勢導出部１４０は、機械要素２００が取り得る位置の各々について最適姿勢を導出したルックアップテーブルをあらかじめ用意していてもよい。ルックアップテーブルに記憶された最適姿勢は、例えば、可操作度が最大となる機械要素２００の姿勢、機械要素２００の各関節に掛かるトルク二乗和が最小となる機械要素２００の姿勢、又は特定のタスクに対応した機械要素２００の所定の姿勢であってもよい。
【００６３】
続いて、姿勢導出部１５０は、補間姿勢及び最適姿勢を内分する関数を決定する（Ｓ１４０）。その後、姿勢導出部１５０は、決定された関数ｆ（ｐｖ）に基づいて、補間姿勢及び最適姿勢を内分することで、経由点Ｐｖにおいて機械要素２００が実際に制御される姿勢を導出する（Ｓ１５０）。
【００６４】
例えば、経由点Ｐｖにおいて機械要素２００が実際に制御される姿勢ｑｖは、補間姿勢ｑｉ及び最適姿勢ｑｏを上述した関数に基づくｕ：１−ｕ（ただし、０≦ｕ≦１）にて内分することで、以下の式８に基づいて導出されてもよい。なお、姿勢ｑｖ、補間姿勢ｑｉ及び最適姿勢ｑｏは、それぞれクォータニオンである。
【００６５】
【数７】
2020141579.tif 000009
【００６６】
ここで、経由点Ｐｖが始点Ｐｓ又は終点Ｐｅと一致する場合、機械要素２００が実際に制御される姿勢ｑｖは、補間姿勢ｑｉ（すなわち、始点Ｐｓ又は終点Ｐｅにおける機械要素２００の姿勢）となるため、内分比ｕは、０に制御されることが望ましい。また、機械要素２００の姿勢の連続性を考慮すると、内分比ｕは連続的に変化することが望ましい。したがって、補間姿勢及び最適姿勢の内分比ｕは、経由点Ｐｖの位置ｐｖの関数ｆ（ｐｖ）であり、以下の条件（１）〜（３）を満たすように決定されてもよい。なお、ｐｓは、始点Ｐｓの位置を表し、ｐｅは、終点Ｐｅの位置を表す。
（１）０≦ｆ（ｐｖ）≦１
（２）ｆ（ｐｓ）＝ｆ（ｐｅ）＝０
（３）ｐｖの定義域において、ｆ（ｐｖ）は連続
【００６７】
例えば、始点Ｐｓ及び経由点Ｐｖの間のユークリッド距離ｄｓと、経由点Ｐｖ及び終点Ｐｅの間のユークリッド距離ｄｖとの比ｒを以下の式９で定義する場合、ｕを制御する関数ｆ（ｐｖ）は、図１０Ａ〜図１０Ｃにてグラフを示す関数ｇ（ｒ）となってもよい。図１０Ａにてグラフを示す関数ｇ（ｒ）は、Ｂ−スプライン基底関数であり、図１０Ｂにてグラフを示す関数ｇ（ｒ）は、放物線を描く二次関数であり、図１０Ｃにてグラフを示す関数ｇ（ｒ）は、上記の条件（１）〜（３）を満たす任意の関数である。
【００６８】
【数８】
2020141579.tif 000010
【００６９】
以上の動作によれば、補間姿勢導出部１３０、最適姿勢導出部１４０、及び姿勢導出部１５０は、経由点Ｐｖにおける機械要素２００の適切な姿勢を導出することができる。これにより、制御装置１００は、導出された姿勢に基づいて、機械要素２００の経由点Ｐｖにおける姿勢を制御することができる（Ｓ１６０）。
【００７０】
なお、上記で説明した姿勢導出の方法は、あくまで一例であり、本開示に係る技術は、上記で説明した姿勢導出の方法に限定されるわけではない。
【００７１】
例えば、最適姿勢導出部１４０は、姿勢導出部１５０にて用いられる最適姿勢として、複数の観点からそれぞれ導出した複数の最適姿勢を重み付けして足し合わせたものを導出してもよい。また、姿勢導出部１５０は、補間姿勢及び最適姿勢の内分比をロボット装置１０の周囲の環境情報（例えば、障害物情報）、又は機械要素２００の可操作度若しくは特異点に関する情報に基づいて動的に制御してもよい。さらに、機械要素２００が取り得る姿勢に拘束条件が存在する場合、最適姿勢導出部１４０は、拘束条件を満たすように最適姿勢を制御してもよく、姿勢導出部１５０は、拘束条件を満たすように補間姿勢と最適姿勢との内分比を制御してもよい。
【００７２】
＜４．ハードウェア構成例＞
続いて、図１１を参照して、本実施形態に係る制御装置１００のハードウェア構成の一例について説明する。図１１は、本実施形態に係る制御装置１００のハードウェア構成の一例を示すブロック図である。
【００７３】
図１１に示すように、制御装置１００は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）９０１、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）９０２、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）９０３、ホストバス９０５、ブリッジ９０７、外部バス９０６、インタフェース９０８、入力装置９１１、出力装置９１２、ストレージ装置９１３、ドライブ９１４、接続ポート９１５、及び通信装置９１６を備える。制御装置１００は、ＣＰＵ９０１に替えて、又はこれと共に、電気回路、ＤＳＰ若しくはＡＳＩＣ等の処理回路を備えてもよい。
【００７４】
ＣＰＵ９０１は、演算処理装置および制御装置として機能し、各種プログラムに従って制御装置１００内の動作全般を制御する。また、ＣＰＵ９０１は、マイクロプロセッサであってもよい。ＲＯＭ９０２は、ＣＰＵ９０１が使用するプログラム及び演算パラメータ等を記憶する。ＲＡＭ９０３は、ＣＰＵ９０１の実行において使用するプログラム、及びその実行において適宜変化するパラメータ等を一時記憶する。ＣＰＵ９０１は、例えば、認識部１１０、軌道生成部１２０、補間姿勢導出部１３０、最適姿勢導出部１４０、姿勢導出部１５０、及び制御部１６０の機能を実行してもよい。
【００７５】
ＣＰＵ９０１、ＲＯＭ９０２及びＲＡＭ９０３は、ＣＰＵバスなどを含むホストバス９０５により相互に接続されている。ホストバス９０５は、ブリッジ９０７を介して、ＰＣＩ（Ｐｅｒｉｐｈｅｒａｌ  Ｃｏｍｐｏｎｅｎｔ  Ｉｎｔｅｒｃｏｎｎｅｃｔ／Ｉｎｔｅｒｆａｃｅ）バスなどの外部バス９０６に接続されている。なお、ホストバス９０５、ブリッジ９０７、及び外部バス９０６は、必ずしも分離構成されなくともよく、１つのバスにこれらの機能が実装されてもよい。
【００７６】
入力装置９１１は、例えば、マウス、キーボード、タッチパネル、ボタン、マイクロフォン、スイッチ又はレバー等のユーザによって情報が入力される装置である。または、入力装置９１１は、赤外線又はその他の電波を利用したリモートコントロール装置であってもよく、制御装置１００の操作に対応した携帯電話又はＰＤＡ等の外部接続機器であってもよい。さらに、入力装置９１１は、例えば、上記の入力手段を用いてユーザにより入力された情報に基づいて入力信号を生成する入力制御回路などを含んでもよい。
【００７７】
出力装置９１２は、情報をユーザに対して視覚的又は聴覚的に通知することが可能な装置である。出力装置９１２は、例えば、ＣＲＴ（Ｃａｔｈｏｄｅ  Ｒａｙ  Ｔｕｂｅ）ディスプレイ装置、液晶ディスプレイ装置、プラズマディスプレイ装置、ＥＬ（ＥｌｅｃｔｒｏＬｕｍｉｎｅｓｃｅｎｃｅ）ディスプレイ装置、レーザープロジェクタ、ＬＥＤ（Ｌｉｇｈｔ  Ｅｍｉｔｔｉｎｇ  Ｄｉｏｄｅ）プロジェクタ又はランプ等の表示装置であってもよく、スピーカ又はヘッドホン等の音声出力装置等であってもよい。
【００７８】
出力装置９１２は、例えば、制御装置１００による各種処理にて得られた結果を出力してもよい。具体的には、出力装置９１２は、制御装置１００による各種処理にて得られた結果を、テキスト、イメージ、表、又はグラフ等の様々な形式で視覚的に表示してもよい。または、出力装置９１２は、音声データ又は音響データ等のオーディオ信号をアナログ信号に変換して聴覚的に出力してもよい。
【００７９】
ストレージ装置９１３は、制御装置１００の記憶部の一例として形成されたデータ格納用の装置である。ストレージ装置９１３は、例えば、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）等の磁気記憶デバイス、半導体記憶デバイス、光記憶デバイス又は光磁気記憶デバイス等により実現されてもよい。例えば、ストレージ装置９１３は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置、及び記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置９１３は、ＣＰＵ９０１が実行するプログラム、各種データ及び外部から取得した各種のデータ等を格納してもよい。
【００８０】
ドライブ９１４は、記憶媒体用リーダライタであり、制御装置１００に内蔵又は外付けされる。ドライブ９１４は、装着されている磁気ディスク、光ディスク、光磁気ディスク、又は半導体メモリ等のリムーバブル記憶媒体に記録されている情報を読み出して、ＲＡＭ９０３に出力する。また、ドライブ９１４は、リムーバブル記憶媒体に情報を書き込むことも可能である。
【００８１】
接続ポート９１５は、外部機器と接続されるインタフェースである。接続ポート９１５は、外部機器とのデータ伝送可能な接続口であり、例えばＵＳＢ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ）であってもよい。
【００８２】
通信装置９１６は、例えば、ネットワーク９２０に接続するための通信デバイス等で形成されたインタフェースである。通信装置９１６は、例えば、有線若しくは無線ＬＡＮ（Ｌｏｃａｌ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）、ＬＴＥ（Ｌｏｎｇ  Ｔｅｒｍ  Ｅｖｏｌｕｔｉｏｎ）、Ｂｌｕｅｔｏｏｔｈ（登録商標）又はＷＵＳＢ（Ｗｉｒｅｌｅｓｓ  ＵＳＢ）用の通信カード等であってもよい。また、通信装置９１６は、光通信用のルータ、ＡＤＳＬ（Ａｓｙｍｍｅｔｒｉｃ  Ｄｉｇｉｔａｌ  Ｓｕｂｓｃｒｉｂｅｒ  Ｌｉｎｅ）用のルータ又は各種通信用のモデム等であってもよい。通信装置９１６は、例えば、インターネット又は他の通信機器との間で、例えばＴＣＰ／ＩＰ等の所定のプロトコルに則して信号等を送受信することができる。
【００８３】
なお、ネットワーク９２０は、情報の有線又は無線の伝送路である。例えば、ネットワーク９２０は、インターネット、電話回線網若しくは衛星通信網などの公衆回線網、Ｅｔｈｅｒｎｅｔ（登録商標）を含む各種のＬＡＮ（Ｌｏｃａｌ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）、又はＷＡＮ（Ｗｉｄｅ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）などを含んでもよい。また、ネットワーク９２０は、ＩＰ−ＶＰＮ（Ｉｎｔｅｒｎｅｔ  Ｐｒｏｔｏｃｏｌ−Ｖｉｒｔｕａｌ  Ｐｒｉｖａｔｅ  Ｎｅｔｗｏｒｋ）などの専用回線網を含んでもよい。
【００８４】
なお、制御装置１００に内蔵されるＣＰＵ、ＲＯＭおよびＲＡＭなどのハードウェアに対して、上述した本実施形態に係る制御装置の各構成と同等の機能を発揮させるためのコンピュータプログラムも作成可能である。また、該コンピュータプログラムを記憶させた記憶媒体も提供することが可能である。
【００８５】
＜５．まとめ＞
以上にて説明した本実施形態に係る制御装置１００は、経由点Ｐｖにおける機械要素２００の姿勢を、始点Ｐｓ及び終点Ｐｅにおける機械要素２００の姿勢の単純補間のみではなく、経由点Ｐｖにおける機械要素２００の最適姿勢を考慮して導出することができる。
【００８６】
これによれば、制御装置１００は、経由点Ｐｖにおける機械要素２００の姿勢をより適切な姿勢とすることができるため、機械要素２００の到達可能範囲を広げることができる。
【００８７】
また、制御装置１００は、機械要素２００に対する要求又は拘束条件を満たすように最適姿勢を設定することで、経由点Ｐｖにおける機械要素２００の姿勢を要求又は拘束条件に対応して制御することができる。
【００８８】
さらに、制御装置１００は、経由点Ｐｖにおける機械要素２００の姿勢を位置から導出することができるため、教示によって軌道生成する場合に、機械要素２００の姿勢を教示せずとも、機械要素２００の位置のみを教示すればよくなる。これによれば、制御装置１００は、教示の効率を向上させることができる。
【００８９】
加えて、制御装置１００は、軌道の探索時に探索空間を機械要素２００の位置及び姿勢の６次元空間から機械要素２００の位置のみの３次元空間に次元数を減少させることができるため、より効率的に探索を行うことができる。
【００９０】
以上、添付図面を参照しながら本開示の好適な実施形態について詳細に説明したが、本開示の技術的範囲はかかる例に限定されない。本開示の技術分野における通常の知識を有する者であれば、請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本開示の技術的範囲に属するものと了解される。
【００９１】
また、本明細書に記載された効果は、あくまで説明的または例示的なものであって限定的ではない。つまり、本開示に係る技術は、上記の効果とともに、または上記の効果に代えて、本明細書の記載から当業者には明らかな他の効果を奏しうる。
【００９２】
なお、以下のような構成も本開示の技術的範囲に属する。
（１）
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
を備える、制御装置。
（２）
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢に基づいて、前記補間姿勢を導出する、前記（１）に記載の制御装置。
（３）
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢の線形補間によって、前記補間姿勢を導出する、前記（２）に記載の制御装置。
（４）
前記線形補間は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づいて行われる、前記（３）に記載の制御装置。
（５）
前記最適姿勢導出部は、前記機械要素の機構的特徴に基づいて、前記最適姿勢を導出する、前記（１）〜（４）のいずれか一項に記載の制御装置。
（６）
前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか１つ以上に基づいて、前記最適姿勢を導出する、前記（５）に記載の制御装置。
（７）
前記最適姿勢導出部は、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、前記（５）に記載の制御装置。
（８）
前記曲線は、ベジエ曲線、スプライン曲線、Ｂ−スプライン曲線、又はラグランジュ補間による曲線のいずれかである、前記（７）に記載の制御装置。
（９）
前記最適姿勢導出部は、複数の姿勢を重み付けして重ね合わせることで、前記最適姿勢を導出する、前記（５）〜（８）のいずれか一項に記載の制御装置。
（１０）
前記姿勢導出部は、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する、前記（１）〜（９）のいずれか一項に記載の制御装置。
（１１）
前記内分の割合は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づく関数に基づいて決定される、前記（１０）に記載の制御装置。
（１２）
前記関数は、連続関数である、前記（１１）に記載の制御装置。
（１３）
前記関数は、前記経由点が前記始点又は前記終点に一致する場合、前記経由点にて前記機械要素が制御される姿を前記補間姿勢とする関数である、前記（１１）又は（１２）に記載の制御装置。
（１４）
前記関数は、環境情報、又は前記機械要素の自機情報に基づいて変更される、前記（１１）〜（１３）のいずれか一項に記載の制御装置。
（１５）
演算装置によって、
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、
前記機械要素の前記経由点における最適姿勢を導出することと、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出することと、
を含む、制御方法。
（１６）
コンピュータを
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
として機能させる、プログラム。
【符号の説明】
【００９３】
１０      ロボット装置
１００    制御装置
１１０    認識部
１２０    軌道生成部
１３０    補間姿勢導出部
１４０    最適姿勢導出部
１５０    姿勢導出部
１６０    制御部
２００    機械要素
２１０    センサ部
２２０    駆動部
【図１Ａ】
2020141579.tif 000011
【図１Ｂ】
2020141579.tif 000012
【図２Ａ】
2020141579.tif 000013
【図２Ｂ】
2020141579.tif 000014
【図３】
2020141579.tif 000015
【図４】
2020141579.tif 000016
【図５】
2020141579.tif 000017
【図６】
2020141579.tif 000018
【図７】
2020141579.tif 000019
【図８】
2020141579.tif 000020
【図９】
2020141579.tif 000021
【図１０Ａ】
2020141579.tif 000022
【図１０Ｂ】
2020141579.tif 000023
【図１０Ｃ】
2020141579.tif 000024
【図１１】
2020141579.tif 000025
【国際調査報告】
2020141579.tif 000026
2020141579.tif 000027
2020141579.tif 000028
2020141579.tif 000029