JP7469482B2

JP7469482B2 - ロボット制御装置

Info

Publication number: JP7469482B2
Application number: JP2022544008A
Authority: JP
Inventors: 悦来王; 康広内藤
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 2020-08-21
Filing date: 2021-08-20
Publication date: 2024-04-16
Anticipated expiration: 2041-08-20
Also published as: WO2022039245A1; JPWO2022039245A1; US20230256598A1; CN115943019A; DE112021003148T5

Description

本発明は、ロボット制御装置に関するものである。

従来、複数の関節を協調させて動作させることによってロボットの所定の部位を所定の連続軌道に沿って移動させる多関節のロボットの制御方法が知られている（例えば、特許文献１～４参照。）。例えば、第１位置および第２位置が教示され、第１位置から第２位置までの直線軌道上をＴＣＰ（ｔｏｏｌｃｅｎｔｅｒｐｏｉｎｔ）のような制御点が直線移動するように複数の関節が制御される。

特開２０１８－１１１１５５号公報国際公開第２０１６/１３５８６１号国際公開第２０１７/００２２０８号特開２０１５－０６６６６８号公報

連続軌道に沿って動作しているとき、ロボットの姿勢が特異点に近付き、少なくとも１つの関節が急激に動作することがある。このような場合、ロボット制御装置は、エラーが発生したと判定し、ロボットの動作を停止させる等の処置を取る。したがって、エラーが発生する度に、別の軌道を再教示してロボットを再動作させる等の追加の作業が必要となる。

本開示の一態様は、多関節のロボットを制御するロボット制御装置であって、前記ロボットが、手首部の位置を変化させる複数の基本関節と、前記手首部または該手首部に接続されたエンドエフェクタに設定された制御点の位置および姿勢を変化させる複数の手首関節とを有し、前記複数の基本関節および前記複数の手首関節を制御することによって、前記制御点を第１位置から第２位置まで移動させる制御部と、前記第２位置の位置と前記制御点が所定の姿勢で前記第２位置に位置するときの前記複数の手首関節の各々の回転角度とを含む位置情報を記録する位置情報記録部と、を備え、前記制御部は、前記第２位置において前記制御点が前記所定の姿勢に配置されるように前記位置情報に基づいて前記制御点を前記第２位置まで移動させ、前記制御点が前記第２位置に位置するときの前記手首部の姿勢が特異点である場合、前記複数の手首関節を相互に協調させずに各軸動作させることによって、前記制御点を前記第２位置まで移動させる、ロボット制御装置である。

ロボット制御装置によって制御されるロボットの構成図である。ロボット制御装置のブロック図である。第１位置から第２位置までの制御点の軌道の他の例を示す図である。

以下に、一実施形態に係るロボット制御装置について図面を参照して説明する。
図１および図２に示されるように、ロボット制御装置１は、手首部３を備える多関節のロボット２に接続され、ロボット２を制御するものである。
ロボット２は、手首部３の位置を３次元的に変化させる複数の基本関節Ｊ１，Ｊ２，Ｊ３と、制御点Ｐｃの位置（Ｘ，Ｙ，Ｚ）および姿勢（Ｗ，Ｐ，Ｒ）を３次元的に変化させる複数の手首関節Ｊ４，Ｊ５，Ｊ６とを備える。制御点Ｐｃは、手首部３の所定の位置に設定された点、または、手首部３に接続されたエンドエフェクタ８の所定の位置に設定された点である。例えば、制御点Ｐｃは、手首部３の先端面３ａの中心点、または、ＴＣＰ（ｔｏｏｌｃｅｎｔｅｒｐｏｉｎｔ）である。

本実施形態において、ロボット２は、６つの関節Ｊ１～Ｊ６を有する６軸の垂直多関節ロボットである。ロボット２は、床に固定されるベース４と、ベース４上に配置された旋回胴５と、旋回胴５と連結された第１アーム６と、第１アーム６の先端に連結された第２アーム７とを備える。手首部３は、第２アーム７の先端に連結され、ハンドまたはツール等のエンドエフェクタ８を取り付け可能な先端面３ａを有する。
各関節Ｊ１～Ｊ６には、ロボット制御装置１からの制御指令に従って各関節Ｊ１～Ｊ６を駆動するサーボモータ、および、各関節Ｊ１～Ｊ６の回転角度を検出するエンコーダが設けられている。

基本関節は、第１関節Ｊ１、第２関節Ｊ２および第３関節Ｊ３である。第１関節Ｊ１は、旋回胴５を鉛直方向の第１軸線Ａ１回りにベース４に対して回転させる。第２関節Ｊ２は、第１アーム６を水平な第２軸線Ａ２回りに旋回胴５に対して回転させる。第３関節Ｊ３は、第２アーム７を水平な第３軸線Ａ３回りに第１アーム６に対して回転させる。

手首関節は、第４関節Ｊ４、第５関節Ｊ５および第６関節Ｊ６である。第４関節Ｊ４は、第２アーム７を第４軸線Ａ４回りに第１アーム６に対して回転させる。第５関節Ｊ５は、手首部３を第５軸線Ａ５回りに第２アーム７に対して回転させる。第６関節Ｊ６は、手首部３の先端面３ａを第６軸線Ａ６回りに回転させる。第４軸線Ａ４は第２アーム７の長手軸に沿って延び、第５軸線Ａ５は第４軸線Ａ４に直交し、第６軸線Ａ６は第５軸線Ａ５に直交する。

図２に示されるように、ロボット制御装置１は、記憶部１１と、制御部１２とを備える。
記憶部１１は、ＲＡＭ（random access memory）、ＲＯＭ（read-only memory）およびＨＤＤ（hard disc drive）等の記憶装置を有する。ロボット制御装置１には、中央演算処理装置のような少なくとも１つのプロセッサが設けられている。記憶部１１には、制御部１２の後述の処理をプロセッサに実行させるための制御プログラムが記憶されている。すなわち、制御部１２は、少なくとも１つのプロセッサによって実現される。

制御部１２は、第１位置Ｐ１と第２位置Ｐ２との間を直線補間することによって、第１位置Ｐ１から第２位置Ｐ２まで真っすぐに延びる直線軌道Ｔを算出する。第１位置Ｐ１および第２位置Ｐ２は、操作者によって設定される任意の３次元位置であり、例えば、ダイレクトティーチ等の教示作業において教示される教示点である。
制御部１２は、関節Ｊ１～Ｊ６を制御することによって、制御点Ｐｃを第１位置Ｐ１から第２位置Ｐ２まで移動させる。

制御点Ｐｃの移動に先立ち、制御部１２は、制御点Ｐｃが設定された姿勢で第２位置Ｐ２に位置するときの手首関節Ｊ４，Ｊ５，Ｊ６の各々の回転角度を逆運動学によって算出する。制御点Ｐｃの姿勢（先端面３ａの姿勢）は、第１位置Ｐ１および第２位置Ｐ２と同様に、操作者によって設定される。算出された手首関節Ｊ４，Ｊ５，Ｊ６の各々の回転角度と、第２位置Ｐ２の３次元の位置（Ｘ２，Ｙ２，Ｚ２）とを含む位置情報は、記憶部（位置情報記録部）１１に記憶（記録）される。

制御部１２は、制御点Ｐｃが第２位置Ｐ２に位置するときの手首部３の姿勢が特異点または特異点近傍であるか否かを判定する。次に、制御部１２は、判定結果に応じて関節Ｊ１～Ｊ６を直線動作または各軸動作させることによって、制御点Ｐｃを第１位置Ｐ１から第２位置まで移動させる。制御部１２による関節Ｊ１～Ｊ６の制御方法については、後で詳述する。

参照する図面において、領域Ｓは、手首部３の姿勢が特異点または特異点近傍に配置される特異点領域を示している。本実施形態において、特異点は、第４軸線Ａ４と第６軸線Ａ６とが一直線上に配置される姿勢である。手首部３の姿勢が特異点であるとき、手首部３の動作を制御することができなくなる。
例えば、制御部１２は、記憶部１１に記憶されている位置情報に基づいて、制御点Ｐｃが第２位置Ｐ２に位置するときの第４軸線Ａ４と第６軸線Ａ６との間の回転角度θを算出する。そして、制御部１２は、回転角度θの大きさが所定の閾値以下、例えば５°以下であるとき、制御点Ｐｃが第２位置Ｐ２に位置するときの手首部３の姿勢が特異点または特異点近傍であると判定する。

次に、ロボット制御装置１の作用について説明する。
例えば、制御点Ｐｃの軌道をロボット制御装置１に教示するとき、操作者は、第１位置Ｐ１（Ｘ１，Ｙ１，Ｚ１）および第２位置Ｐ２（Ｘ２，Ｙ２，Ｚ２）を教示位置として設定し、制御点Ｐｃの姿勢（Ｗ１，Ｐ１，Ｒ１）を教示姿勢として設定する。第１位置Ｐ１（Ｘ１，Ｙ１，Ｚ１）、第２位置Ｐ２（Ｘ２，Ｙ２，Ｚ２）および姿勢（Ｗ１，Ｐ１，Ｒ１）は、記憶部１１に記憶される。

次に、制御部１２によって、制御点Ｐｃが設定された姿勢（Ｗ１，Ｐ１，Ｒ１）で第２位置Ｐ２に位置するときの手首部３の姿勢が特異点であるか否かが判定される。次に、制御部１２によって関節Ｊ１～Ｊ６が制御され、関節Ｊ１～Ｊ６の直線動作または各軸動作によって制御点Ｐｃが第１位置Ｐ１から第２位置Ｐ２まで移動する。

制御点Ｐｃが第２位置Ｐ２に位置するときの手首部３の姿勢が特異点でない場合、制御部１２は、関節Ｊ１～Ｊ６を相互に協調させて直線動作させることによって、制御点Ｐｃを、姿勢（Ｗ１，Ｐ１，Ｒ１）を保持しながら第１位置Ｐ１（Ｘ１，Ｙ１，Ｚ１）から第２位置Ｐ２（Ｘ２，Ｙ２，Ｚ２）まで直線軌道Ｔに沿って直線移動させる。具体的には、制御部１２は、制御点Ｐｃが設定された姿勢（Ｗ１，Ｐ１，Ｒ１）で直線軌道Ｔ上の各位置に位置するときの各関節Ｊ１～Ｊ６の回転角度を逆運動学によって算出し、制御点Ｐｃを第１位置Ｐ１から第２位置Ｐ２まで姿勢（Ｗ１，Ｐ１，Ｒ１）を保持しながら直線移動させるための回転角度の時系列のデータを得る。次に、制御部１２は、算出された角度の時系列のデータに従って関節Ｊ１～Ｊ６を制御する。

一方、制御点Ｐｃが第２位置Ｐ２に位置するときの手首部３の姿勢が特異点である場合、制御部１２は、基本関節Ｊ１，Ｊ２，Ｊ３を直線動作させながら手首関節Ｊ４，Ｊ５，Ｊ６を各軸動作させることによって、制御点Ｐｃを第１位置Ｐ１から第２位置Ｐ２まで移動させる。基本関節Ｊ１，Ｊ２，Ｊ３の直線動作は、前述の方法で得られた各関節Ｊ１，Ｊ２，Ｊ３の回転角度の時系列のデータに従って関節Ｊ１，Ｊ２，Ｊ３を制御することによって行われる。

各軸動作において、制御部１２は、制御点Ｐｃが設定された姿勢（Ｗ１，Ｐ１，Ｒ１）で第１位置Ｐ１に位置するときの各手首関節Ｊ４，Ｊ５，Ｊ６の回転角度と、制御点Ｐｃが設定された姿勢（Ｗ１，Ｐ１，Ｒ１）で第２位置Ｐ２に位置するときの各手首関節Ｊ４，Ｊ５，Ｊ６の回転角度との差分を算出する。次に、制御部１２は、各手首関節Ｊ４，Ｊ５，Ｊ６を、算出された差分だけ個別に、すなわち相互に協調させずに動作させる。各軸動作において、制御部１２は、手首関節Ｊ４，Ｊ５，Ｊ６を順番に動作させてもよく、同時に動作させてもよい。

基本関節Ｊ１，Ｊ２，Ｊ３の直線動作によって、手首中心点（ＷＣＰ）は直線軌道Ｔに平行な軌道Ｔ’に沿って直線移動する。ＷＣＰは、手首関節Ｊ４，Ｊ５，Ｊ６の軸線Ａ４，Ａ５，Ａ６が相互に交差する点である。また、手首関節Ｊ４，Ｊ５，Ｊ６の各軸動作によって、制御点Ｐｃは、姿勢（Ｗ，Ｐ，Ｒ）を変化させながら第１位置Ｐ１から第２位置Ｐ２まで移動し、設定された姿勢（Ｗ１，Ｐ１，Ｒ１）で第２位置Ｐ２に配置される。

このように、本実施形態によれば、第２位置Ｐ２において手首部３が特異点または特異点近傍に配置される場合、手首関節Ｊ４，Ｊ５，Ｊ６の各軸動作によって制御点Ｐｃが第２位置Ｐ２まで移動する。各軸動作において手首関節Ｊ４，Ｊ５，Ｊ６は相互に協調せずに動作するので、制御点Ｐｃは、姿勢を変化させながら移動する。つまり、制御点Ｐｃが第１位置Ｐ１から第２位置Ｐ２まで移動する間、手首部３は特異点とは異なる姿勢に配置される。したがって、手首部３が特異点または特異点近傍に配置されることを防止しながら、制御点Ｐｃを第１位置Ｐ１から第２位置Ｐ２まで移動させることができる。

操作者にとって、動作しているロボット２の特異点を認識することは難しく、ロボット２が特異点に配置されてしまうことがある。ロボット２が特異点に配置されると、特定の関節の急激な動作によってエラーが生じ、ロボット２が停止してしまう。この場合、操作者は、ロボット２が特異点を通らないように第２位置Ｐ２を再設定しなければならない。このような作業は、操作者にとって煩わしく、作業効率が悪い。本実施形態によれば、手首部３が特異点に配置されるか否かが自動的に判定され、手首部３の姿勢が特異点または特異点近傍に配置されることを自動的に回避しながら制御点Ｐｃが第１位置Ｐ１から第２位置Ｐ２まで移動する。したがって、エラーの発生を防止し、ロボット２の教示をスムーズに行うことができる。

また、制御点Ｐｃが第２位置Ｐ２まで移動している間、基本関節Ｊ１，Ｊ２，Ｊ３の直線動作によってＷＣＰは直線移動する。したがって、手首関節Ｊ４，Ｊ５，Ｊ６が各軸動作を行ったとしても、制御点Ｐｃの位置が直線軌道Ｔから大きく外れることはなく、制御点Ｐｃの軌道は、直線軌道Ｔに近い軌道となる。したがって、操作者が意図する直線軌道Ｔと大体同じ軌道に沿って制御点Ｐｃを移動させることができる。

本実施形態において、ロボット制御装置１は、直線軌道Ｔから制御点Ｐｃまでの距離が所定の閾値を超えたときにその旨を報知する報知部１３をさらに備えていてもよい。操作者は、制御点Ｐｃの位置が閾値を超えて直線軌道Ｔから外れたことを、報知部１３による報知に基づいて認識し、必要に応じて、第１位置Ｐ１および第２位置Ｐ２を他の位置へ再設定することができる。

例えば、制御部１２は、手首関節Ｊ４，Ｊ５，Ｊ６を各軸動作させている間、制御点Ｐｃの３次元の位置をエンコーダによって検出された各関節Ｊ１～Ｊ６の回転角度から計算し、制御点Ｐｃの直線軌道Ｔからの距離を計算する。算出される距離が閾値を超えたとき、報知部１３は、例えば警告音を発することによって、距離が閾値を超えたことを操作者に報知する。制御部１２は、距離が閾値を超えたとき、ロボット２の動作を停止させてもよい。

本実施形態において、制御点Ｐｃが第２位置Ｐ２に位置するときの手首部３の姿勢が特異点である場合、制御部１２は、制御点Ｐｃの位置を直線軌道Ｔ上に保持しながら手首関節Ｊ４、Ｊ５，Ｊ６を各軸動作させることによって、制御点Ｐｃの姿勢を変化させながら制御点Ｐｃを第２位置Ｐ２まで直線軌道Ｔ上で移動させてもよい。
この構成によれば、手首部３が特異点に配置されることを防ぎつつ、直線動作のときと同じ直線軌道Ｔに沿って制御点Ｐｃを移動させることができる。

本実施形態において、制御点Ｐｃが第２位置Ｐ２に位置するときの手首部３の姿勢が特異点である場合、制御部１２が、基本関節Ｊ１，Ｊ２，Ｊ３を直線動作させることとしたが、第１位置Ｐ１から第２位置Ｐ２までの制御点Ｐｃの軌道が直線軌道Ｔから大きく外れても問題ない場合、基本関節Ｊ１，Ｊ２，Ｊ３も各軸動作させてもよい。
本実施形態において、ロボット２が６軸の垂直多関節ロボットであることとしたが、ロボット２は他の軸数の垂直多関節ロボットまたは他の関節構成のロボットであってもよい。

１ロボット制御装置
２ロボット
３手首部
８エンドエフェクタ
１１記憶部（位置情報記録部）
１２制御部
１３報知部
Ｊ１，Ｊ２，Ｊ３基本関節
Ｊ４，Ｊ５，Ｊ６手首関節
Ｐ１第１位置
Ｐ２第２位置
Ｐｃ制御点
Ｓ特異点領域
Ｔ直線軌道

Claims

多関節のロボットを制御するロボット制御装置であって、前記ロボットが、手首部の位置を変化させる複数の基本関節と、前記手首部または該手首部に接続されたエンドエフェクタに設定された制御点の位置および姿勢を変化させる複数の手首関節とを有し、
前記複数の基本関節および前記複数の手首関節を制御することによって前記制御点を第１位置から第２位置まで移動させる制御部と、
前記第２位置の位置と、前記制御点が所定の姿勢で前記第２位置に位置するときの前記複数の手首関節の各々の回転角度と、を含む位置情報を記録する位置情報記録部と、を備え、
前記制御部は、
前記第２位置において前記制御点が前記所定の姿勢に配置されるように前記位置情報に基づいて前記制御点を前記第２位置まで移動させ、
前記制御点が前記第２位置に位置するときの前記手首部の姿勢が特異点である場合、前記複数の手首関節を相互に協調させずに各軸動作させることによって、前記制御点を前記第２位置まで移動させる、ロボット制御装置。
前記制御点が前記第２位置に位置するときの前記手首部の姿勢が特異点である場合、前記制御部は、前記制御点の位置を前記第１位置と前記第２位置とを結ぶ直線軌道上に保持しながら前記複数の手首関節の各軸動作させることによって、前記制御点を前記直線軌道上で前記第２位置まで移動させる、請求項１に記載のロボット制御装置。
前記第１位置と前記第２位置とを結ぶ直線軌道からの前記制御点の距離が所定の閾値を超えたときにその旨を報知する報知部を備える請求項１または請求項２に記載のロボット制御装置。
複数の手首関節を有するロボットを制御する制御装置であって、
制御プログラムを記憶可能な１又は複数のメモリと、
１又は複数のプロセッサと、を備え、
前記１又は複数のプロセッサは、
前記制御プログラムに基づいて前記ロボットの先端の制御点を第１位置から第２位置に移動させる際に、前記制御点が所定の姿勢に配置されるように前記制御点を前記第２位置まで移動させ、
前記制御点が前記第２位置に位置するときの前記先端の姿勢が特異点であるかを判定し、
前記先端の姿勢が特異点である場合、複数の手首関節を相互に協調させずに各軸動作させる、制御装置。