(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2023-12-11
(45)【発行日】2023-12-19
(54)【発明の名称】制御装置、制御方法、及びプログラム
(51)【国際特許分類】
B25J 9/10 20060101AFI20231212BHJP
G05B 19/4103 20060101ALI20231212BHJP
【FI】
B25J9/10 A
G05B19/4103 E
(21)【出願番号】P 2020563859
(86)(22)【出願日】2019-11-18
(86)【国際出願番号】 JP2019044987
(87)【国際公開番号】W WO2020141579
(87)【国際公開日】2020-07-09
【審査請求日】2022-10-18
(31)【優先権主張番号】P 2019000180
(32)【優先日】2019-01-04
(33)【優先権主張国・地域又は機関】JP
(73)【特許権者】
【識別番号】000002185
【氏名又は名称】ソニーグループ株式会社
(74)【代理人】
【識別番号】110002147
【氏名又は名称】弁理士法人酒井国際特許事務所
(72)【発明者】
【氏名】寺澤 良
(72)【発明者】
【氏名】松田 康宏
(72)【発明者】
【氏名】杉本 克文
【審査官】樋口 幸太郎
(56)【参考文献】
【文献】特開2000-006067(JP,A)
【文献】特開平06-051824(JP,A)
【文献】特開平07-334228(JP,A)
【文献】特開平01-230105(JP,A)
(58)【調査した分野】(Int.Cl.,DB名)
B25J 9/10
G05B 19/4103
(57)【特許請求の範囲】
【請求項1】
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢
の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
を備え
、
前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか1つ以上に基づき、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、制御装置。
【請求項2】
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢に基づいて、前記補間姿勢を導出する、請求項1に記載の制御装置。
【請求項3】
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢の線形補間によって、前記補間姿勢を導出する、請求項2に記載の制御装置。
【請求項4】
前記線形補間は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づいて行われる、請求項3に記載の制御装置。
【請求項5】
前記曲線は、ベジエ曲線、スプライン曲線、B-スプライン曲線、又はラグランジュ補間による曲線のいずれかである、請求項
1に記載の制御装置。
【請求項6】
前記最適姿勢導出部は、複数の姿勢を重み付けして
足し合わせることで、前記最適姿勢を導出する、請求項
1に記載の制御装置。
【請求項7】
前記姿勢導出部は、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する、請求項1に記載の制御装置。
【請求項8】
前記内分の割合は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づく関数に基づいて決定される、請求項
7に記載の制御装置。
【請求項9】
前記関数は、連続関数である、請求項
8に記載の制御装置。
【請求項10】
前記関数は、前記経由点が前記始点又は前記終点に一致する場合、前記経由点にて前記機械要素が制御される姿を前記補間姿勢とする関数である、請求項
8に記載の制御装置。
【請求項11】
前記関数は、環境情報、又は前記機械要素の自機情報に基づいて変更される、請求項
8に記載の制御装置。
【請求項12】
演算装置によって、
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、
前記機械要素の前記経由点における最適姿勢を導出することと、
前記補間姿勢及び前記最適姿勢
の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出することと、
を含
み、
前記最適姿勢を導出することは、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか1つ以上に基づき、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、制御方法。
【請求項13】
コンピュータを
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢
の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
として機能させ
、
前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか1つ以上に基づき、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、制御装置、制御方法、及びプログラムに関する。
【背景技術】
【0002】
一般的に、ロボット装置に備えられるマニュピュレータ等(以下では、機械要素とも称する)では、該マニュピュレータ等の位置及び姿勢を指示する動作軌道に基づいて動作が制御される。
【0003】
このような機械要素の動作を適切に制御するためには、動作の始点から終点までをつなぐ動作軌道において、機械要素の位置及び姿勢を適切に決定することが重要となる。ただし、動作軌道を計画する際に機械要素の位置及び姿勢を同時に探索することは、探索の自由度が高くなり(合計6自由度)、探索に時間が掛かってしまうため、好ましくない。
【0004】
そこで、まず、動作軌道上に経由点を定めて、経由点における機械要素の位置(3自由度)を探索し、次に、探索した機械要素の位置に基づいて機械要素の姿勢(3自由度)を決定することが検討されている。
【0005】
例えば、下記の特許文献1には、軌道上の位置データと、マニュピュレータの姿勢データとを対応付けした姿勢データテーブルをあらかじめ記憶し、マニュピュレータの位置データに基づいてマニュピュレータの姿勢を決定することが記載されている。
【先行技術文献】
【特許文献】
【0006】
【発明の概要】
【発明が解決しようとする課題】
【0007】
しかし、経由点における機械要素の位置に基づいて機械要素の姿勢を決定した場合、決定された機械要素の姿勢は、機械要素の機構的特徴を考慮した適切な姿勢となっていないことがあり得る。そのため、動作軌道の始点から終点までの間に存在する経由点における機械要素の姿勢をより適切に導出することが可能な制御装置、制御方法、及びプログラムが求められていた。
【課題を解決するための手段】
【0008】
本開示によれば、始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、を備え、前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか1つ以上に基づき、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、制御装置が提供される。
【0009】
また、本開示によれば、演算装置によって、始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、前記機械要素の前記経由点における最適姿勢を導出することと、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出することと、を含み、前記最適姿勢を導出することは、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか1つ以上に基づき、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、制御方法が提供される。
【0010】
また、本開示によれば、コンピュータを始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、として機能させ、前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか1つ以上に基づき、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、プログラムが提供される。
【図面の簡単な説明】
【0011】
【
図1A】始点における機械要素の位置及び姿勢の一例を示す模式図である。
【
図1B】終点における機械要素の位置及び姿勢の一例を示す模式図である。
【
図2A】経由点における機械要素の位置及び姿勢の一例を示す模式図である。
【
図2B】経由点における機械要素の位置及び姿勢の他の例を示す模式図である。
【
図3】本開示の一実施形態に係る制御装置の機能構成を示すブロック図である。
【
図4】機械要素の姿勢を導出する際の制御装置の動作例を説明するフローチャート図である。
【
図5】クォータニオンによる球面線形補間を説明する説明図である。
【
図6】始点及び経由点の間のユークリッド距離と、経由点及び終点の間のユークリッド距離とを説明する説明図である。
【
図7】クォータニオンの球面線形補正を用いた、経由点における機械要素の補間姿勢の導出を説明する説明図である。
【
図8】始点、経由点及び終点を通る二次ベジエ曲線を説明する説明図である。
【
図9】機械要素の機構を考慮した、経由点における機械要素の最適姿勢の導出を説明する説明図である。
【
図10A】補間姿勢及び最適姿勢の内分比を制御する関数の一例を示すグラフ図である。
【
図10B】補間姿勢及び最適姿勢の内分比を制御する関数の他の例を示すグラフ図である。
【
図10C】補間姿勢及び最適姿勢の内分比を制御する関数の他の例を示すグラフ図である。
【
図11】本開示の一実施形態に係る制御装置のハードウェア構成の一例を示すブロック図である。
【発明を実施するための形態】
【0012】
以下に添付図面を参照しながら、本開示の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。
【0013】
なお、説明は以下の順序で行うものとする。
1.本開示に係る技術の概要
2.制御装置の構成例
3.制御装置の動作例
4.ハードウェア構成例
5.まとめ
【0014】
<1.本開示に係る技術の概要>
まず、
図1A~
図2Bを参照して、本開示に係る技術の概要について説明する。
【0015】
例えば、始点P
sから終点P
eまで、経由点P
vを介してロボット装置10のロボットアーム等の機械要素200を動作させる場合について考える。具体的には、
図1Aに示す始点P
sにおける機械要素200の位置及び姿勢から、
図1Bに示す終点P
eにおける機械要素200の位置及び姿勢までロボット装置10を動作させる場合について考える。なお、説明を簡単にするため、
図1A及び
図1Bに示すように、始点P
s及び終点P
eにおける機械要素200の姿勢は、同一とした。
【0016】
このとき、経由点P
vにおける機械要素200の姿勢は、例えば、始点P
s及び終点P
eにおける機械要素200の姿勢から導出することができる。具体的には、経由点P
vにおける機械要素200の姿勢は、始点P
s及び終点P
eの各々における機械要素200の姿勢を足し合わせることで導出することができる。例えば、
図1Aに示す始点P
sにおける機械要素200の姿勢は、
図1Bに示す終点P
eにおける機械要素200の姿勢と同一である。そのため、経由点P
vにおける機械要素200の姿勢は、始点P
s及び終点P
eにおける機械要素200の姿勢と同一として導出することができる。
【0017】
しかしながら、このような方法で経由点P
vにおける機械要素200の姿勢を導出する場合、経由点P
vの位置によっては、
図2Aに示すように、機械要素200の姿勢が適切となる解が存在しないことがあり得る。また、経由点P
vの位置によっては、機械要素200の姿勢が不自然又は非効率な姿勢となってしまうことがあり得る。
【0018】
一方、経由点Pvにおける機械要素200の姿勢は、始点Ps及び終点Peにおける機械要素200の姿勢を考慮せずに、例えば、機械要素200の機械的特徴から導出することもできる。具体的には、経由点Pvにおける機械要素200の姿勢は、経由点Pvにおける機械要素200の各部位に掛かるトルクが小さくなるように、又は機械要素200の可操作性が高くなるように導出することができる。
【0019】
しかしながら、このような方法で経由点P
vにおける機械要素200の姿勢を導出した場合、
図2Bに示すように、機械要素200の姿勢が始点P
s又は終点P
eにおける姿勢から大きく変化したものとなり、機械要素200の動作効率が低下してしまうことがあり得る。
【0020】
したがって、経由点Pvにおける機械要素200の姿勢は、互いに異なる方法にて導出された複数の姿勢を考慮して導出されることが望ましい。本開示に係る技術は、上述した事情を鑑みて想到されたものである。具体的には、本開示に係る技術は、経由点Pvにおける機械要素200の姿勢を補間姿勢及び最適姿勢に基づいて導出するものである。
【0021】
ここで、補間姿勢とは、始点Psにおける機械要素200の姿勢、及び終点Peにおける機械要素200の姿勢の間を補間することで導出される姿勢である。また、最適姿勢とは、機械要素200の機構的特徴に基づいて導出される姿勢である。本開示に係る技術によれば、補間姿勢及び最適姿勢を用いて機械要素200の姿勢を導出することによって、経由点Pvにおける機械要素200の姿勢をより適切に制御することが可能である。
【0022】
以下では、上記で概要を説明した本開示に係る技術を実現する制御装置について詳述する。
【0023】
<2.制御装置の構成例>
続いて、
図3を参照して、本開示の一実施形態に係る制御装置100の具体的な構成例について説明する。
図3は、本実施形態に係る制御装置100の機能構成を示すブロック図である。
【0024】
図3に示すように、制御装置100は、認識部110と、軌道生成部120と、補間姿勢導出部130と、最適姿勢導出部140と、姿勢導出部150と、制御部160と、を備える。
【0025】
例えば、制御装置100は、センサ部210にて取得された情報に基づいて、機械要素200の駆動部220を制御することで、機械要素200の位置及び姿勢を制御するものである。制御装置100は、機械要素200を備えるロボット装置10の内部に設けられてもよく、機械要素200を備えるロボット装置10の外部に設けられてもよい。
【0026】
ここで、機械要素200とは、ロボット装置10に備えられる部位のうち機械的機構によって位置及び姿勢を制御可能な部位を表す。例えば、機械要素200は、ロボット装置10のロボットアーム又は該ロボットアームの先端のエンドエフェクタであってもよく、ロボット装置10の脚部又は該脚部の先端の接地部であってもよい。
【0027】
センサ部210は、ロボット装置10の周囲の環境情報を取得するセンサと、ロボット装置10の自機情報を取得するセンサと、を備える。例えば、センサ部210は、ロボット装置10の周囲の環境情報を取得するセンサとして、RGBカメラ、グレースケールカメラ、ステレオカメラ、デプスカメラ、赤外線カメラ又はToF(Time of Flight)カメラ等の各種カメラを備えてもよい。センサ部210は、LIDAR(Laser Imaging Detection and Ranging)センサ又はRADAR(Radio Detecting and Ranging)センサなどの各種測距センサを備えてもよい。センサ部210は、マイクロフォン、照度計、温度計又は湿度計などの環境センサを備えてもよい。また、センサ部210は、ロボット装置10の自機情報を取得するセンサとして、例えば、エンコーダ、電圧計、電流計、歪みゲージ、圧力計、又はIMU(Inertial Measurement Unit)等を備えてもよい。
【0028】
ただし、センサ部210は、ロボット装置10の周囲の環境情報、又はロボット装置10の自機情報を取得することができれば、上述したセンサ以外の公知のセンサを備えてもよいことは言うまでもない。
【0029】
認識部110は、センサ部210が取得した情報に基づいて、ロボット装置10の周囲の環境状態、又はロボット装置10の自機状態を認識する。具体的には、認識部110は、センサ部210が取得した環境情報に基づいて、障害物認識、形状認識、物体認識、マーカ認識、文字認識、白線認識、車線認識、又は音声認識を行うことで、ロボット装置10の周囲の環境状態を認識してもよい。また、認識部110は、センサ部210が取得した自機情報に基づいて、位置認識、運動状態(速度、加速度又はジャーク等)認識、又は機体状態(電源残量、温度又は関節角等)認識を行うことで、ロボット装置10の自機状態を認識してもよい。
【0030】
認識部110による上記の認識は、いずれも公知の認識技術を用いることで行うことができる。認識部110による認識は、例えば、所定のルールに基づいて行われてもよく、機械学習アルゴリズムに基づいて行われてもよい。
【0031】
軌道生成部120は、認識部110による認識結果に基づいて、ロボット装置10の機械要素200を動作させる軌道を生成する。具体的には、まず、軌道生成部120は、認識部110にて認識されたロボット装置10の周囲の環境状態、及びロボット装置10の自機状態に基づいて、機械要素200を動作させる始点Ps及び終点Peを決定する。次に、軌道生成部120は、決定した始点Ps及び終点Peを結ぶ機械要素200の軌道を生成し、軌道上の機械要素200の位置と、該位置における機械要素200の姿勢とを決定する。制御装置100は、生成された軌道に基づいて駆動部220の駆動を制御することで、機械要素200を生成された軌道に沿って動作させることができる。
【0032】
また、軌道生成部120は、軌道を生成する際に、始点Psから終点Peまでを結ぶ軌道が通過する経由点Pvを設定し、経由点Pvにおける機械要素200の位置を決定する。軌道生成部120は、例えば、軌道の探索を容易にするための中間点として経由点Pvを設定してもよく、障害物を回避するために経由点Pvを設定してもよく、ユーザからの指示によって経由点Pvを設定してもよい。また、軌道生成部120は、軌道上に複数の経由点Pvを設定してもよい。なお、軌道生成部120によって経由点Pvが設定される場合、軌道生成部120は、公知の探索アルゴリズムを用いて経由点Pvにおける機械要素200の位置を設定してもよい。
【0033】
ここで、本実施形態に係る制御装置100では、経由点Pvにおける機械要素200の姿勢は、後述する姿勢導出部150にて導出される。具体的には、経由点Pvにおける機械要素200の姿勢は、補間姿勢導出部130にて導出される補間姿勢と、最適姿勢導出部140にて導出される最適姿勢とに基づいて導出される。
【0034】
これによれば、制御装置100は、始点Psから終点Peまでの経路を探索する際に、経由点Pvにおける機械要素200の位置及び姿勢の両方ではなく、機械要素200の位置だけを探索すればよくなる。したがって、制御装置100は、探索アルゴリズムを用いて経由点Pvを設定する際に、機械要素200の位置及び姿勢の6自由度ではなく、機械要素200の位置の3自由度で探索を行えばよいため、探索の負荷及び時間を低減することができる。
【0035】
補間姿勢導出部130は、始点Psにおける機械要素200の姿勢、及び終点Peにおける機械要素200の姿勢に基づいて、経由点Pvにおける補間姿勢を導出する。
【0036】
具体的には、補間姿勢導出部130は、始点Psにおける機械要素200の姿勢、及び終点Peにおける機械要素200の姿勢の線形補間によって、経由点Pvにおける補間姿勢を導出してもよい。例えば、補間姿勢導出部130は、始点Psから経由点Pvまでの距離と、終点Peから経由点Pvまでの距離との比を用いて、始点Psにおける姿勢と、終点Peにおける姿勢との間を線形補間することで、経由点Pvにおける補間姿勢を導出してもよい。すなわち、補間姿勢導出部130は、始点Psから終点Peまでの機械要素200の姿勢遷移の途中段階の一姿勢を補間姿勢として導出してもよい。
【0037】
経由点Pvにおける補間姿勢は、後述する姿勢導出部150にて、経由点Pvにおいて機械要素200が実際に制御される姿勢を導出するために用いられる。制御装置100は、補間姿勢を用いて、機械要素200が実際に制御される姿勢を導出することによって、経由点Pvにおける機械要素200の姿勢を始点Ps又は終点Peにおける機械要素200の姿勢から大きく変化しないようにすることができる。
【0038】
最適姿勢導出部140は、機械要素200の機構的特徴に基づいて、経由点Pvにおける最適姿勢を導出する。具体的には、最適姿勢導出部140は、機械要素200の可操作度、機械要素200に掛かるトルク、又は機械要素200の特異点の少なくともいずれかに基づいて、経由点Pvにおける最適姿勢を導出してもよい。例えば、最適姿勢導出部140は、機械要素200の可操作度が高くなるように最適姿勢を導出してもよく、機械要素200に掛かるトルクが小さくなるように最適姿勢を導出してもよく、機械要素200の特異点を避けるように最適姿勢を導出してもよい。すなわち、最適姿勢導出部140は、経由点Pvにおいて機械要素200の機構が最適となる姿勢を最適姿勢として導出してもよい。
【0039】
または、最適姿勢導出部140は、経由点Pvを通過する軌道の接線方向と、経由点Pvにおける機械要素200の進入方向とが略一致するように、経由点Pvにおける最適姿勢を導出してもよい。
【0040】
経由点Pvにおける補間姿勢は、後述する姿勢導出部150にて、経由点Pvにおいて機械要素200が実際に制御される姿勢を導出するために用いられる。制御装置100は、最適姿勢を用いて、機械要素200が実際に制御される姿勢を導出することによって、経由点Pvにおける機械要素200の姿勢をより負荷の小さい姿勢にすることができる。また、制御装置100は、最適姿勢を用いて、機械要素200が実際に制御される姿勢を導出することによって、経由点Pvにおける機械要素200の姿勢を機械要素200の動作がより適切となる姿勢にすることができる。
【0041】
姿勢導出部150は、補間姿勢及び最適姿勢に基づいて、経由点Pvにおいて機械要素200が実際に制御される姿勢を導出する。具体的には、姿勢導出部150は、補間姿勢及び最適姿勢の内分によって、経由点Pvにおいて機械要素200が実際に制御される姿勢を導出してもよい。なお、補間姿勢及び最適姿勢の内分は、例えば、始点Ps及び終点Peにおいて補間姿勢の割合が100%となるような連続関数に基づいて行うことができる。これによれば、姿勢導出部150は、経由点Pvにおける機械要素200の姿勢をより適切に導出することができる。
【0042】
制御部160は、軌道生成部120にて生成された軌道に基づいて、駆動部220の駆動を制御する。具体的には、まず、制御部160は、認識部110にて認識されたロボット装置10の自機状態から機械要素200の位置及び姿勢を把握する。次に、制御部160は、機械要素200の位置及び姿勢と、軌道生成部120にて生成された軌道の位置及び姿勢との差が縮小するように、機械要素200の駆動部220の駆動を制御する。これによれば、制御装置100は、軌道生成部120にて生成された軌道に基づいて、機械要素200を始点Psから終点Peまで動作させることができる。
【0043】
駆動部220は、制御部160からの制御に基づいて、ロボット装置10の機械要素200の各部を駆動させる。例えば、駆動部220は、機械要素200の関節を駆動させるアクチュエータ等であってもよい。
【0044】
<3.制御装置の動作例>
続いて、
図4~
図10Cを参照して、機械要素200の姿勢を導出する際の制御装置100の動作例について説明する。
図4は、機械要素200の姿勢を導出する際の制御装置100の動作例を説明するフローチャート図である。
【0045】
まず、制御装置100は、始点Ps及び経由点Pvの間のユークリッド距離dsと、経由点Pv及び終点Peの間のユークリッド距離dvとを算出する(S100)。次に、補間姿勢導出部130は、機械要素200の姿勢を表すクォータニオンを球面線形補正することによって補間姿勢を導出する(S110)。
【0046】
球面線形補間は、2つの独立したクォータニオンq
A、q
Bを補間する方法であり、パラメータt(ただし、0≦t≦1)を用いた以下の式1にて行うことができる。これにより、
図5に示すように、クォータニオンq
A、q
Bから補間クォータニオンq(t)を導出することができる。
【0047】
【0048】
なお、クォータニオンでは、共役なクォータニオンは、同じ姿勢を表す。したがって、式1において、θの絶対値がπ/2を超える場合には、qA又はqBのいずれかを、符号が反転した共役クォータニオンに置き換えて演算すればよい。
【0049】
また、機械要素200の姿勢がクォータニオンではなくオイラー角で表される場合、補間姿勢導出部130は、オイラー角のロール角φ、ピッチ角θ及びヨー角ψの各成分についてそれぞれ線形補間を行うことで、補間姿勢を導出することができる。例えば、ロール角φの線形補間は、パラメータt(ただし、0≦t≦1)を用いて、以下の式2にて行うことができる。なお、ピッチ角θ、及びヨー角ψについても同様に線形補間を行うことができる。
【0050】
【0051】
このように、補間姿勢導出部130は、クォータニオンの球面線形補正を用いて、
図6及び
図7に示すように経由点P
vにおける機械要素200の補間姿勢を導出することができる。具体的には、まず、補間姿勢導出部130は、
図6に示すように、始点P
s及び経由点P
vの間のユークリッド距離d
sと、経由点P
v及び終点P
eの間のユークリッド距離d
vとを算出する。次に、以下の式3に示すように、算出したユークリッド距離d
s、d
vの比を計算することで、球面線形補正のパラメータt
iを決定する。これにより、補間姿勢導出部130は、
図7に示すように、経由点P
vにおける機械要素200の補間姿勢を以下の式4にて導出することができる。
【0052】
【0053】
続いて、最適姿勢導出部140は、始点Ps、経由点Pv、及び終点Peを滑らかに結ぶ曲線を導出し、該曲線の経由点Pvでの接線ベクトルLを導出する(S120)。始点Ps、経由点Pv、及び終点Peを滑らかに結ぶ曲線としては、例えば、ベジエ曲線、スプライン曲線、B-スプライン曲線、又はラグランジュ補間による曲線を例示することができる。
【0054】
図8に示すように、ベジエ曲線の一例である二次ベジエ曲線は、3つの制御点P
0、P
1、及びP
2によって定義される曲線である。二次ベジエ曲線上の点Pは、3つの制御点P
0、P
1、及びP
2の位置、及びパラメータs(ただし、0≦s≦1)を用いて、以下の式5にて表すことができる。
【0055】
【0056】
したがって、最適姿勢導出部140は、P0を始点Psとし、P2を終点Peとし、P1を適切に制御することで、始点Ps、経由点Pv、及び終点Peを滑らかに結ぶ曲線を導出することができる。P1は、例えば、始点Ps及び経由点Pvの間のユークリッド距離dsと、経由点Pv及び終点Peの間のユークリッド距離dvとの比s0をパラメータsに代入した際の点が経由点Pvとなるように決定すればよい。これにより、最終的に導出される経由点Pvにおける接線ベクトルLは、以下の式6のようになる。
【0057】
【0058】
次に、最適姿勢導出部140は、曲線の経由点Pvでの接線ベクトルLと、機械要素200の経由点Pvへの進入方向とが略一致するように機械要素200の最適姿勢を導出する(S130)。
【0059】
ただし、この方法では、機械要素200の経由点Pvへの進入方向の軸方向の姿勢は定まらない。軸方向の機械要素200の姿勢については、補間姿勢導出部130と同様に、始点Ps及び終点Peの機械要素200の姿勢を線形補間することで導出することができる。
【0060】
なお、上記の変形例として、最適姿勢導出部140は、曲線の経由点Pvでの接線ベクトルLに替えて、始点Psと経由点Pvとを通る直線ベクトル、及び経由点Pvと終点Peとを通る直線ベクトルの二等分線方向ベクトルを用いることも可能である。このような場合、最適姿勢導出部140は、該二等分線方向ベクトルと、機械要素200の経由点Pvへの進入方向とが略一致するように機械要素200の最適姿勢を導出することができる。二等分線方向L2は、例えば、以下の式7で表すことができる。
【0061】
【0062】
また、最適姿勢導出部140は、
図9に示すように、機械要素200の機構を考慮することで、最適姿勢q
oを導出してもよい。例えば、最適姿勢導出部140は、機械要素200が取り得る位置の各々について最適姿勢を導出したルックアップテーブルをあらかじめ用意していてもよい。ルックアップテーブルに記憶された最適姿勢は、例えば、可操作度が最大となる機械要素200の姿勢、機械要素200の各関節に掛かるトルク二乗和が最小となる機械要素200の姿勢、又は特定のタスクに対応した機械要素200の所定の姿勢であってもよい。
【0063】
続いて、姿勢導出部150は、補間姿勢及び最適姿勢を内分する関数を決定する(S140)。その後、姿勢導出部150は、決定された関数f(pv)に基づいて、補間姿勢及び最適姿勢を内分することで、経由点Pvにおいて機械要素200が実際に制御される姿勢を導出する(S150)。
【0064】
例えば、経由点Pvにおいて機械要素200が実際に制御される姿勢qvは、補間姿勢qi及び最適姿勢qoを上述した関数に基づくu:1-u(ただし、0≦u≦1)にて内分することで、以下の式8に基づいて導出されてもよい。なお、姿勢qv、補間姿勢qi及び最適姿勢qoは、それぞれクォータニオンである。
【0065】
【0066】
ここで、経由点Pvが始点Ps又は終点Peと一致する場合、機械要素200が実際に制御される姿勢qvは、補間姿勢qi(すなわち、始点Ps又は終点Peにおける機械要素200の姿勢)となるため、内分比uは、0に制御されることが望ましい。また、機械要素200の姿勢の連続性を考慮すると、内分比uは連続的に変化することが望ましい。したがって、補間姿勢及び最適姿勢の内分比uは、経由点Pvの位置pvの関数f(pv)であり、以下の条件(1)~(3)を満たすように決定されてもよい。なお、psは、始点Psの位置を表し、peは、終点Peの位置を表す。
(1)0≦f(pv)≦1
(2)f(ps)=f(pe)=0
(3)pvの定義域において、f(pv)は連続
【0067】
例えば、始点P
s及び経由点P
vの間のユークリッド距離d
sと、経由点P
v及び終点P
eの間のユークリッド距離d
vとの比rを以下の式9で定義する場合、uを制御する関数f(p
v)は、
図10A~
図10Cにてグラフを示す関数g(r)となってもよい。
図10Aにてグラフを示す関数g(r)は、B-スプライン基底関数であり、
図10Bにてグラフを示す関数g(r)は、放物線を描く二次関数であり、
図10Cにてグラフを示す関数g(r)は、上記の条件(1)~(3)を満たす任意の関数である。
【0068】
【0069】
以上の動作によれば、補間姿勢導出部130、最適姿勢導出部140、及び姿勢導出部150は、経由点Pvにおける機械要素200の適切な姿勢を導出することができる。これにより、制御装置100は、導出された姿勢に基づいて、機械要素200の経由点Pvにおける姿勢を制御することができる(S160)。
【0070】
なお、上記で説明した姿勢導出の方法は、あくまで一例であり、本開示に係る技術は、上記で説明した姿勢導出の方法に限定されるわけではない。
【0071】
例えば、最適姿勢導出部140は、姿勢導出部150にて用いられる最適姿勢として、複数の観点からそれぞれ導出した複数の最適姿勢を重み付けして足し合わせたものを導出してもよい。また、姿勢導出部150は、補間姿勢及び最適姿勢の内分比をロボット装置10の周囲の環境情報(例えば、障害物情報)、又は機械要素200の可操作度若しくは特異点に関する情報に基づいて動的に制御してもよい。さらに、機械要素200が取り得る姿勢に拘束条件が存在する場合、最適姿勢導出部140は、拘束条件を満たすように最適姿勢を制御してもよく、姿勢導出部150は、拘束条件を満たすように補間姿勢と最適姿勢との内分比を制御してもよい。
【0072】
<4.ハードウェア構成例>
続いて、
図11を参照して、本実施形態に係る制御装置100のハードウェア構成の一例について説明する。
図11は、本実施形態に係る制御装置100のハードウェア構成の一例を示すブロック図である。
【0073】
図11に示すように、制御装置100は、CPU(Central Processing Unit)901、ROM(Read Only Memory)902、RAM(Random Access Memory)903、ホストバス905、ブリッジ907、外部バス906、インタフェース908、入力装置911、出力装置912、ストレージ装置913、ドライブ914、接続ポート915、及び通信装置916を備える。制御装置100は、CPU901に替えて、又はこれと共に、電気回路、DSP若しくはASIC等の処理回路を備えてもよい。
【0074】
CPU901は、演算処理装置および制御装置として機能し、各種プログラムに従って制御装置100内の動作全般を制御する。また、CPU901は、マイクロプロセッサであってもよい。ROM902は、CPU901が使用するプログラム及び演算パラメータ等を記憶する。RAM903は、CPU901の実行において使用するプログラム、及びその実行において適宜変化するパラメータ等を一時記憶する。CPU901は、例えば、認識部110、軌道生成部120、補間姿勢導出部130、最適姿勢導出部140、姿勢導出部150、及び制御部160の機能を実行してもよい。
【0075】
CPU901、ROM902及びRAM903は、CPUバスなどを含むホストバス905により相互に接続されている。ホストバス905は、ブリッジ907を介して、PCI(Peripheral Component Interconnect/Interface)バスなどの外部バス906に接続されている。なお、ホストバス905、ブリッジ907、及び外部バス906は、必ずしも分離構成されなくともよく、1つのバスにこれらの機能が実装されてもよい。
【0076】
入力装置911は、例えば、マウス、キーボード、タッチパネル、ボタン、マイクロフォン、スイッチ又はレバー等のユーザによって情報が入力される装置である。または、入力装置911は、赤外線又はその他の電波を利用したリモートコントロール装置であってもよく、制御装置100の操作に対応した携帯電話又はPDA等の外部接続機器であってもよい。さらに、入力装置911は、例えば、上記の入力手段を用いてユーザにより入力された情報に基づいて入力信号を生成する入力制御回路などを含んでもよい。
【0077】
出力装置912は、情報をユーザに対して視覚的又は聴覚的に通知することが可能な装置である。出力装置912は、例えば、CRT(Cathode Ray Tube)ディスプレイ装置、液晶ディスプレイ装置、プラズマディスプレイ装置、EL(ElectroLuminescence)ディスプレイ装置、レーザープロジェクタ、LED(Light Emitting Diode)プロジェクタ又はランプ等の表示装置であってもよく、スピーカ又はヘッドホン等の音声出力装置等であってもよい。
【0078】
出力装置912は、例えば、制御装置100による各種処理にて得られた結果を出力してもよい。具体的には、出力装置912は、制御装置100による各種処理にて得られた結果を、テキスト、イメージ、表、又はグラフ等の様々な形式で視覚的に表示してもよい。または、出力装置912は、音声データ又は音響データ等のオーディオ信号をアナログ信号に変換して聴覚的に出力してもよい。
【0079】
ストレージ装置913は、制御装置100の記憶部の一例として形成されたデータ格納用の装置である。ストレージ装置913は、例えば、HDD(Hard Disk Drive)等の磁気記憶デバイス、半導体記憶デバイス、光記憶デバイス又は光磁気記憶デバイス等により実現されてもよい。例えば、ストレージ装置913は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置、及び記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置913は、CPU901が実行するプログラム、各種データ及び外部から取得した各種のデータ等を格納してもよい。
【0080】
ドライブ914は、記憶媒体用リーダライタであり、制御装置100に内蔵又は外付けされる。ドライブ914は、装着されている磁気ディスク、光ディスク、光磁気ディスク、又は半導体メモリ等のリムーバブル記憶媒体に記録されている情報を読み出して、RAM903に出力する。また、ドライブ914は、リムーバブル記憶媒体に情報を書き込むことも可能である。
【0081】
接続ポート915は、外部機器と接続されるインタフェースである。接続ポート915は、外部機器とのデータ伝送可能な接続口であり、例えばUSB(Universal Serial Bus)であってもよい。
【0082】
通信装置916は、例えば、ネットワーク920に接続するための通信デバイス等で形成されたインタフェースである。通信装置916は、例えば、有線若しくは無線LAN(Local Area Network)、LTE(Long Term Evolution)、Bluetooth(登録商標)又はWUSB(Wireless USB)用の通信カード等であってもよい。また、通信装置916は、光通信用のルータ、ADSL(Asymmetric Digital Subscriber Line)用のルータ又は各種通信用のモデム等であってもよい。通信装置916は、例えば、インターネット又は他の通信機器との間で、例えばTCP/IP等の所定のプロトコルに則して信号等を送受信することができる。
【0083】
なお、ネットワーク920は、情報の有線又は無線の伝送路である。例えば、ネットワーク920は、インターネット、電話回線網若しくは衛星通信網などの公衆回線網、Ethernet(登録商標)を含む各種のLAN(Local Area Network)、又はWAN(Wide Area Network)などを含んでもよい。また、ネットワーク920は、IP-VPN(Internet Protocol-Virtual Private Network)などの専用回線網を含んでもよい。
【0084】
なお、制御装置100に内蔵されるCPU、ROMおよびRAMなどのハードウェアに対して、上述した本実施形態に係る制御装置の各構成と同等の機能を発揮させるためのコンピュータプログラムも作成可能である。また、該コンピュータプログラムを記憶させた記憶媒体も提供することが可能である。
【0085】
<5.まとめ>
以上にて説明した本実施形態に係る制御装置100は、経由点Pvにおける機械要素200の姿勢を、始点Ps及び終点Peにおける機械要素200の姿勢の単純補間のみではなく、経由点Pvにおける機械要素200の最適姿勢を考慮して導出することができる。
【0086】
これによれば、制御装置100は、経由点Pvにおける機械要素200の姿勢をより適切な姿勢とすることができるため、機械要素200の到達可能範囲を広げることができる。
【0087】
また、制御装置100は、機械要素200に対する要求又は拘束条件を満たすように最適姿勢を設定することで、経由点Pvにおける機械要素200の姿勢を要求又は拘束条件に対応して制御することができる。
【0088】
さらに、制御装置100は、経由点Pvにおける機械要素200の姿勢を位置から導出することができるため、教示によって軌道生成する場合に、機械要素200の姿勢を教示せずとも、機械要素200の位置のみを教示すればよくなる。これによれば、制御装置100は、教示の効率を向上させることができる。
【0089】
加えて、制御装置100は、軌道の探索時に探索空間を機械要素200の位置及び姿勢の6次元空間から機械要素200の位置のみの3次元空間に次元数を減少させることができるため、より効率的に探索を行うことができる。
【0090】
以上、添付図面を参照しながら本開示の好適な実施形態について詳細に説明したが、本開示の技術的範囲はかかる例に限定されない。本開示の技術分野における通常の知識を有する者であれば、請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本開示の技術的範囲に属するものと了解される。
【0091】
また、本明細書に記載された効果は、あくまで説明的または例示的なものであって限定的ではない。つまり、本開示に係る技術は、上記の効果とともに、または上記の効果に代えて、本明細書の記載から当業者には明らかな他の効果を奏しうる。
【0092】
なお、以下のような構成も本開示の技術的範囲に属する。
(1)
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
を備える、制御装置。
(2)
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢に基づいて、前記補間姿勢を導出する、前記(1)に記載の制御装置。
(3)
前記補間姿勢導出部は、前記始点における前記機械要素の姿勢、及び前記終点における前記機械要素の姿勢の線形補間によって、前記補間姿勢を導出する、前記(2)に記載の制御装置。
(4)
前記線形補間は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づいて行われる、前記(3)に記載の制御装置。
(5)
前記最適姿勢導出部は、前記機械要素の機構的特徴に基づいて、前記最適姿勢を導出する、前記(1)~(4)のいずれか一項に記載の制御装置。
(6)
前記最適姿勢導出部は、前記機械要素の可操作度、前記機械要素に掛かるトルク、又は前記機械要素の特異点の少なくともいずれか1つ以上に基づいて、前記最適姿勢を導出する、前記(5)に記載の制御装置。
(7)
前記最適姿勢導出部は、前記始点、前記経由点、及び前記終点を順に通過する連続した曲線の接線ベクトルと、前記経由点への前記機械要素の接近方向とを略一致させる前記最適姿勢を導出する、前記(5)に記載の制御装置。
(8)
前記曲線は、ベジエ曲線、スプライン曲線、B-スプライン曲線、又はラグランジュ補間による曲線のいずれかである、前記(7)に記載の制御装置。
(9)
前記最適姿勢導出部は、複数の姿勢を重み付けして重ね合わせることで、前記最適姿勢を導出する、前記(5)~(8)のいずれか一項に記載の制御装置。
(10)
前記姿勢導出部は、前記補間姿勢及び前記最適姿勢の内分によって、前記経由点にて前記機械要素が制御される姿勢を導出する、前記(1)~(9)のいずれか一項に記載の制御装置。
(11)
前記内分の割合は、前記始点から前記経由点までの距離と、前記終点から前記経由点までの距離との比に基づく関数に基づいて決定される、前記(10)に記載の制御装置。
(12)
前記関数は、連続関数である、前記(11)に記載の制御装置。
(13)
前記関数は、前記経由点が前記始点又は前記終点に一致する場合、前記経由点にて前記機械要素が制御される姿を前記補間姿勢とする関数である、前記(11)又は(12)に記載の制御装置。
(14)
前記関数は、環境情報、又は前記機械要素の自機情報に基づいて変更される、前記(11)~(13)のいずれか一項に記載の制御装置。
(15)
演算装置によって、
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出することと、
前記機械要素の前記経由点における最適姿勢を導出することと、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出することと、
を含む、制御方法。
(16)
コンピュータを
始点から経由点を介して終点まで到達する軌道を運動する機械要素の前記経由点における補間姿勢を導出する補間姿勢導出部と、
前記機械要素の前記経由点における最適姿勢を導出する最適姿勢導出部と、
前記補間姿勢及び前記最適姿勢に基づいて、前記経由点にて前記機械要素が制御される姿勢を導出する姿勢導出部と、
として機能させる、プログラム。
【符号の説明】
【0093】
10 ロボット装置
100 制御装置
110 認識部
120 軌道生成部
130 補間姿勢導出部
140 最適姿勢導出部
150 姿勢導出部
160 制御部
200 機械要素
210 センサ部
220 駆動部