(57)【要約】【課題】垂直多関節型ロボットにおいて、動作軌跡の形状に関係なく、また、軌道誤差や速度むらが現れることなく位置制御性を高める。【解決手段】第２のリンク８の回転中心側に、第３のリンク９を駆動するモータ１５を配設し、このモータ１５と歯車減速装置１６との間をベルト伝動機構２０により連結してなる垂直多関節型ロボットロボットにおいて、第３の回転関節Ｌｃ−３のモータ１５に対する位置指令と、当該モータ１５の回転角度を検出する角度センサにより検出される現在位置との差分を基に、第２の回転関節Ｌ−２のモータの補正量を算出し、当該補正量を第２の回転関節Ｌｃ−２のモータに対する次の位置指令に加算することにより、第３のリンク９の位置を補正する。【選択図】図１
2015178156.tif 000002
【特許請求の範囲】
【請求項１】
据え付け面に固定されるベースリンクに対し、ロボットアームを構成する第１〜第３の各リンクを、第１〜第３の各回転関節により順次連結してなる垂直多関節型ロボットロボットであって、前記第２のリンクが前記第１のリンクに前記第２の回転関節により上下方向に回転可能に連結されると共に、前記第３のリンクが前記第２のリンクの先端部に前記第３の回転関節により上下方向に回転可能に連結され、且つ、前記第３のリンクの駆動モータを前記第２のリンクに、当該第２のリンクの回転中心側に位置して設けてなる垂直多関節型ロボットにおいて、
前記第３のリンクの駆動モータに対する位置指令と、当該駆動モータの回転角度を検出する角度センサにより検出される現在位置との差分を位置誤差とし、この位置誤差を基に、前記第２のリンクの駆動モータの補正量を算出し、当該補正量を前記第２のリンクの駆動モータに対する次の位置指令に加算することにより、前記第３のリンクの位置を補正することを特徴とする垂直多関節型ロボットの位置誤差抑制方法。
【請求項２】
前記請求項１記載の垂直多関節型ロボットの位置誤差抑制方法において、
位置指令により前記モータを動作させたとき、前記第３のリンクの先端の速度から、当該第３のリンクの先端の位置指令から求められる移動方向以外の前記位置誤差を求め、この位置誤差から前記第２のリンクの駆動モータの補正量を算出し、当該補正量を第２のリンクの駆動モータに対する次の位置指令に加算して第２のリンクを動作させることを特徴とする垂直多関節型ロボットの位置誤差抑制方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は垂直多関節型ロボットにおいて、上下方向に回転動作する第３のリンクの位置誤差を抑制する方法に関する。
【背景技術】
【０００２】
例えば６軸の垂直多関節型ロボットは、床面に固定されるベース（ベースリンク）にショルダ（第１のリンク）を水平方向に回転可能に連結し、ショルダに下アーム（第２のリンク）を上下方向に回転可能に連結し、下アームの先端部に第１の上アーム（第３のリンク）を上下方向に回転可能に連結し、そして第１の上アームに第２の上アーム、手首アーム、フランジを順に回転可能に連結した構造になっている。
【０００３】
小形の６軸垂直多関節型ロボットにおいては、産業用ロボットとして使用された場合の生産性向上の観点から高速化が要求され、また作業性向上の観点からアーム幅の狭小化が望まれている。そのために、下アームを駆動する第２軸モータにかかる慣性モーメントを小さくすることにより高速化を図り、下アーム内に設けられる第３軸モータを小型にすることによりアーム幅の狭小化を図っている。
【０００４】
具体的には、重量物である第３軸モータを下アームの回転中心側（根元側である回転中心側）に配置し、第３軸モータの回転をベルト伝導機構によって第３軸に伝達する構造を採用することにより、下アームの慣性モーメントを小さくし、また、第３軸モータとして体積に対して大出力の小型モータを採用することにより、下アームの幅を狭小化するようにしている。
【０００５】
最大可搬質量が５Ｋｇ程度の小型の６軸垂直多関節型ロボットでは、第３軸モータとして出力１００Ｗクラスのサーボモータが使用されることが多い。一般的に産業用サーボモータの最小サイズは対角４０ｍｍ程度の角形で、出力１００Ｗ程度である。このクラスのモータは、小型・高出力を主眼とした設計がなされている。そのため、コギングトルクなどのトルク脈動が他クラスのモータに比べて大きく出る傾向にある。
【０００６】
また、第３軸モータの回転は、ベルトによって第３軸（第１の上アーム）に伝達されるため、モータに制御電流を印加してからモータ位置応答までの伝達特性には位相遅れが現れる。そのため、フィードバック制御系が不安定になり易く、フィードバックゲインを大きくすることができない。
【０００７】
以上により、小形の６軸垂直多関節型ロボットの第１の上アーム（第３軸）は、他と比べて外乱の影響を受け易いにもかかわらず、その外乱を制御によって抑制することが困難である（制御性が低い）という問題を生じていた。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】産業用多関節形ロボットアームの相互位置同期制御を図った高精度輪郭制御法  日本ロボット学会誌  Ｖｏｌ．１５  Ｎｏ．５，ｐ７３６−７４３，１９９７年発行
【非特許文献２】多関節ロボットアームにおける外乱対策を図った主軸従軸切替え位置同期制御法  電気学会論文誌Ｃ  Ｖｏｌ．１２１，Ｎｏ．１，ｐ８３−８９，２００１年発行
【発明の概要】
【発明が解決しようとする課題】
【０００９】
非特許文献１は、ロボットの直線軌道動作に対して、直線からのずれ成分を三次元位置座標上でフィードバックするというものである。しかしながら、この非特許文献１の制御法では、第３軸だけを対象にして特別に計算することはできない。また、ある程度の時間サンプル分は直線軌道動作をしなければ位置誤差の抑制ができないため、毎サンプル動作方向の変わる円弧軌道動作やスプライン軌道動作といった曲線状軌跡動作の場合には、この制御法を適用することはできない。つまり、特定の軸に対する位置ずれを補正する計算ができないのである。
【００１０】
非特許文献２は、主軸とした軸（本発明の場合の第３の回転関節の回転軸）の動作進捗具合に応じて、従軸とした別の軸（本発明の場合の第２の回転関節の回転軸）の動作指令を決定する方法である。この方法では、主軸がほとんど動かずに従軸が大きく動く場合に、主軸に僅かな振動が発生したとしても、従軸に大きく振動する指令値が送られる。主軸の動作がゼロの場合には、僅かな振動でも、この振動の振幅は無限大となり、異常な指令値としてシステムの停止を招く。また、振幅が無限大となる程でなくても、従軸を第２の回転関節の回転軸のみとした場合では、主軸および従軸以外とのバランスが崩れ、これによる軌跡誤差が発生する。更に、従軸を第３の回転関節の回転軸以外とした場合には、速度むらとして現れる。
【００１１】
本発明は上記した事情に鑑みてなされたもので、その目的は、動作軌跡の形状に関係なく、また、軌道誤差や速度むらが現れることなく制御性を高めることができる垂直多関節型ロボットの位置誤差抑制方法を提供することにある。
【課題を解決するための手段】
【００１２】
請求項１の発明は、第３のリンクの駆動モータに対する位置指令と、当該駆動モータの回転角度を検出する角度センサにより検出される現在位置との差分を位置誤差とし、この位置誤差を基に、第２のリンクの駆動モータの補正量を算出し、当該補正量を前記第２のリンクの駆動モータに対する次の位置指令に加算することにより、第３のリンクの位置を補正するので、どのような軌道形状であっても、第２のリンクの位置補正により第２のリンクの位置誤差を抑制できると共に、当該第２のリンクが大きく振動する恐れがなく、軌道誤差や速度むらが現れることを極力抑制できる。
【００１３】
請求項２の発明は、第３のリンクの先端の速度から、当該第３のリンクの先端の位置指令から求められる移動方向以外の位置誤差を求め、この位置誤差から前記第２のリンクの駆動モータの補正量を算出し、当該補正量を第２のリンクの駆動モータに対する次の位置指令に加算して第２のリンクを動作させるので、目標とする軌道から外れないようにしながら第３のリンクの位置誤差を抑制できる。
【図面の簡単な説明】
【００１４】
【図１】本発明の第１の実施形態を示す要部の概略的な側面図
【図２】アームのある瞬間での位置を示すスケルトン
【図３】ロボット装置の全体を示す斜視図
【図４】回転関節の概略構成を示す断面図
【図５】ロボットの座標系を示す斜視図
【図６】制御装置の電気的構成を示すブロック図
【発明を実施するための形態】
【００１５】
以下に本発明の実施形態を図面に基づいて説明する。
［第１の実施形態］
図面は本発明の第１の実施形態を示すもので、図３には、６軸の垂直多回転関節型ロボット１が示されている。このロボット１は、６軸垂直多関節型ロボット本体２と、ロボット本体２の動作を制御する制御装置（ロボット制御手段）３と、制御装置３に接続されてロボット本体２を手動操作によって動作させるためのティーチングペンダント（手動操作手段）４とを備えている。
【００１６】
ロボット本体２は、工場の床面などの据え付け面に固定されるベース（ベースリンク）５に対し、ロボットアーム６を構成するショルダ（第の１リンク）７、下アーム（第２のリンク）８、第１の上アーム（第３のリンク）９、第２の上アーム（第４のリンク）１０と、手首アーム（第５のリンク）１１、フランジ（第６のリンク）１２を第１〜第６の各回転関節Ｊ１〜Ｊ６（図３には図示せず）により順次連結して構成されている。
【００１７】
つまり、ショルダ７はベース５上に第１回転関節Ｊ１によって第１軸Ｌｃ−１を中心に水平方向に回転可能に連結され、下アーム８はショルダ７の先端部に第２回転関節Ｊ２によって第２軸Ｌｃ−２を中心に上下（鉛直）方向に回転可能に支持され、第１の上アーム９は下アーム８の先端部に第３回転関節Ｊ３によって第３軸Ｌｃ−３を中心に上下方向に回転可能に支持され、第２の上アーム１０は第１の上アーム９の先端部に第４回転関節Ｊ４によって第４軸Ｌｃ−４を中心に捻り回転可能に支持され、手首アーム１１は第２の上アーム１０の先端部に第５回転関節Ｊ５によって第５軸Ｌｃ−５を中心に上下方向に回転可能に支持され、フランジ１２は手首アーム１１に第６回転関節Ｊ６によって第６軸Ｌｃ−６を中心に捻り回転可能に支持されている。
【００１８】
前記第１〜第６の各回転関節Ｊ１〜Ｊ６は、図４にその概略が示されている。図４に示す通り、各リンク７〜１２の回転関節Ｊ１〜Ｊ６は、前段のリンクに軸受１３を介して回転可能に支持された回転軸１４からなり、この回転軸１４に次段のリンクが直接或いは継ぎ手材を介して連結される。回転軸１４は、前段のリンクに固定された駆動源としてサーボモータ１５に減速装置、例えば歯車減速装置１６を介して回転駆動され、この回転軸１４の回転によって次段のリンクがせんかい回転動作或いは捻り回転動作を行うようになっている。なお、前記第１軸Ｌｃ−１〜第６軸Ｌｃ−６は、第１〜第６の各回転関節Ｊ１〜Ｊ６の回転軸の回転中心線Ｌｃに相当するものである。
【００１９】
第３軸Ｌｃ−３を除き、サーボモータ１５と歯車減速装置１６とは一体化され、歯車減速装置１６が回転軸１４に直接連結されている。第３軸Ｌｃ−３については、図１に示すように、サーボモータ１５と歯車減速装置１６とは分離され、サーボモータ１５が下アーム８の回転中心側、つまり第２軸Ｌｃ−２側に位置して設けられ、歯車減速装置１６が第３軸Ｌｃ−３に連結されている。そして、サーボモータ１５と歯車減速装置１６との間が歯付きプーリ１７，１８間に歯付きベルト１９を掛け渡してなるベルト伝動機構２０により連結され、サーボモータ１５の回転がベルト伝動機構２０を介して歯車減速装置１６に伝達されるようになっている。
【００２０】
ショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２には、図５に示すように、それぞれ３次元の座標Ｒ１〜Ｒ６が規定されている。これらの座標Ｒ１〜Ｒ６の原点Ｏ１〜Ｏ６は、ショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転中心線である第１軸Ｌｃ−1〜第６軸Ｌｃ−６上の所定位置に定められ、各座標Ｒ１〜Ｒ６のＺ軸であるＺ１〜Ｚ６軸は第１軸Ｌｃ−1〜第６軸Ｌｃ−６と一致している。
【００２１】
ベース５およびロボットアーム６の先端であるフランジ１２の先端面にも座標Ｒ０およびＲｆが規定されている。上記座標Ｒ１〜Ｒ６がショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転に伴って位置と向きが変化するのに対して、ベース５の座標Ｒ０は不動であり、ロボット座標とされている。このロボット座標Ｒ０の原点Ｏ０は、ショルダ７の回転中心線である第１軸Ｌｃ−１上にあって本実施形態では座標Ｒ１の原点Ｏ１と一致した位置に定められている。また、Ｚ軸であるＺ０軸は第１軸Ｌｃ−１に一致し、Ｘ軸であるＸ０軸およびＹ軸であるＹ０軸は、ショルダ７が原点位置にあるときの当該ショルダ７の座標Ｒ１のＸ１軸およびＹ１軸に一致している。
【００２２】
フランジ１２の先端面の座標Ｒｆは手先座標とされ、その原点Ｏｆは、第６軸Ｌｃ−６とフランジ１２の先端面との交点、つまり、フランジ１２の円形をなす先端面の中心と一致するように定められている。そして、フランジ１２の先端面の中心がロボットアーム６の先端である手先に定められている。
【００２３】
以上の座標Ｒ０〜Ｒ６，Ｒｆの原点Ｏ０〜Ｏ６，Ｏｆは、本実施形態においては、座標Ｒ１のＸ１軸とＺ１軸を含む一つの平面（鉛直面）を仮定したとき、この一平面上に位置されている。
【００２４】
前記制御装置３は、図６に示すように、ＣＰＵ（制御手段）２１と、駆動回路（駆動手段）２２と、位置検出回路（位置検出手段）２３とを備えている。ＣＰＵ２１には、ロボット本体２全体のシステムプログラムや動作プログラムを作成するためのロボット言語やＤＨパラメータなどの各種データなどを記憶するＲＯＭ２４およびロボット本体２の動作プログラムを記憶するＲＡＭ２５が接続されていると共に、ティーチング作業などを行う際に使用する前記ティーチングペンダント４が接続されている。
【００２５】
位置検出回路２３には、各サーボモータ１５の回転軸（図示せず）に連結された回転センサとしてのロータリエンコーダ２６が接続されている。位置検出回路２３は、各ロータリーエンコーダ２６の回転角検出情報に基づいてショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角度を検出し、その回転角検出情報をＣＰＵ２１に与える。
【００２６】
ここで、ＣＰＵ２１は、ロボット座標Ｒ０上での手先の位置（手先座標Ｒｆの原点Ｏｆの位置）と姿勢（手先座標ＲｆのＸｆ軸、Ｙｆ軸の向き）が与えられると、手先がその位置と姿勢をとるようなショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角度を、ヤコビ行列を用いた逆運動学の周知の計算手法によって求める。また、逆に、ショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角度が与えられると、それら回転角度から座標変換を用いた順運動学の周知の計算手法によってロボット座標Ｒ０上での手先の位置と姿勢を求めることができるように構成されている。これらの逆運動学および順運動学の計算手法による演算には、周知のように各座標Ｒ０〜Ｒ６，Ｒｆの相互の関係を示す前記ＤＨパラメータが用いられる。
【００２７】
そして、ＣＰＵ２１は、動作プログラムから一定時間毎（サンプリングタイム）に手先の目標位置を取得し、その手先の目標位置からショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の目標動作角度を演算し、その目標動作角度を指令位置とする位置指令を駆動回路２２に与える。駆動回路２２は与えられた位置指令に基づいて各サーボモータ１５を駆動する。このとき、ＣＰＵ２１は、位置検出回路２３から与えられるショルダ７、下アーム８、第１の上アーム９、第２の上アーム１０、手首アーム１１およびフランジ１２の回転角検出情報をフィードバック情報として各サーボモータ１５をフィードバック制御する。
【００２８】
ところで、第３軸Ｌｃ‐３を駆動する駆動装置、即ちサーボモータ１５と歯車減速装置１６とは離間して設けられ、両者間はベルト伝動機構２０によって連結されている。以下の説明では、この第３軸Ｌｃ‐３のサーボモータ１５と歯車減速装置１６を他の軸のものと区別する場合、符号１５，１６の次に「‐３」を付け加え示すこととする。
比較的重量のあるサーボモータ１５‐３を下アーム８の回転中心側（第２軸Ｌｃ−２側）に配置した理由は、当該下アーム８の慣性モーメントを小さくし、下アーム８の高速化を図るためである。
【００２９】
サーボモータ１５‐３は下アーム８の慣性モーメントの更なる減少化を図るために小型軽量であることを要求される半面、第１の上アーム９以降のリンク９〜１２を１本のアームと見たとき、そのアームの動作の高速化を図るために高出力化が必要である。そこで、本実施形態のロボット本体２が最大可搬質量５Ｋｇ以下の小型ロボットであるとした場合、サーボモータ１５‐３としては、一般に、対角が４０ｍｍ、出力が１００Ｗクラスの角型モータが採用される。この小形高出力のサーボモータ１５‐３では、他クラスのモータに比べコギングなどのトルク脈動が大きく出る傾向にある。
【００３０】
一方、ベルト伝動機構２０の歯付きベルト１９は弾性を有し、伸縮する。このため、サーボモータ１５に電流が流されて当該サーボモータ１５が駆動力を発すると、歯付きベルト１９は伸び、直後に縮むという挙動を呈する。
このトルク脈動が大きく出る傾向にあること、および歯付きベルト１９が伸縮し易いことから、駆動回路２２が与えられた位置指令に基づいてサーボモータ１５‐３を駆動したとき、位置検出回路２３から与えられる第１の上アーム９の回転角検出情報は位相遅れが現れる。そのため、フィードバック制御が不安定になり易く、フィードバックゲインを大きくすることができない。
【００３１】
以上により、第１の上アーム９は外乱の影響を受け易い。本発明は、この外乱による第１の上アーム９の位置誤差を小さくしようとするものである。そのために本発明は、位置指令で出されて指令位置とロータリーエンコーダ２６により検出されるサーボモータ１５‐３の回転角度から求められる第３軸Ｌｃ−３の現在位置（角度）との差分を位置誤差とし、この位置誤差を基に、第２軸Ｌｃ−２の補正量を計算し、この補正量を第２軸Ｌｃ−２の位置指令（角度）に加算するものである。
【００３２】
下アーム８の補正量は以下の方法により導出される。
いま、ある位置指令によりモータ１５が動作したとする。このとき、図２に示すように、ロータリーエンコーダ２６により検出された下アーム８の現在角度がθ２、第１の上アーム９の現在角度がθ３であったとする。そして、指令位置に対する第１の上アーム９の位置誤差をｄ３、下アーム８の位置指令に対する補正量をｄ２とする。なお、補正量ｄ２は、下アーム８の次の位置指令に当該補正量ｄ２を加えれば第１の上アーム９の位置誤差ｄ３が０になる量をいう。
【００３３】
以下において、
下アーム８のヤコビアン：Ｊ２＝（Ｊ２ｘ，Ｊ２ｙ，Ｊ２ｚ）
第１の上アーム９のヤコビアン：Ｊ３＝（Ｊ３ｘ，Ｊ３ｙ，Ｊ３ｚ）
第１の上アーム９の先端Ｏ５の位置誤差：ｅ＝（ｅｘ，ｅｙ，ｅｚ）＝Ｊ２ｄ２＋Ｊ３ｄ３
第１の上アーム９の先端Ｏ５の位置誤差の絶対値：Ｅ
下アーム８のアーム長（Ｏ２からＯ３までの長さ）：Ｌ１
第１の上アーム９のアーム長（Ｏ３からＯ５までの長さ）：Ｌ２
とする。ここで、第２の上アーム１０は第１の上アーム９と上下方向の回転に関して一体に回転するので、第１の上アーム１０の先端をＯ５点と定めている。
【００３４】
位置誤差の絶対値Ｅを最小化するように第２軸Ｌｃ−２の位置指令を補正する。この場合の第２軸Ｌｃ−２の補正量ｄ２は以下のように求まる。
まずＯ５の位置誤差ｅは、ｅ＝（ｅｘ，ｅｙ，ｅｚ）＝Ｊ２ｄ２＋Ｊ３ｄ３
位置誤差の絶対値Ｅは、Ｅ＝｜ｅ｜＝（ｅｘ２＋ｅｙ２＋ｅｚ２）１／２、であるから、
Ｅ２＝（Ｊ２ｄ２＋Ｊ３ｄ３）（Ｊ２ｄ２＋Ｊ３ｄ３）Ｔ
＝（Ｊ２ｘ２＋Ｊ２ｙ２＋Ｊ２ｚ２）ｄ２２＋２（Ｊ２ｘＪ３ｘ＋Ｊ２ｙＪ３ｙ＋
Ｊ２ｚＪ３ｚ）ｄ２ｄ３＋（Ｊ３ｘ２＋Ｊ３ｙ２＋Ｊ３ｚ２）ｄ３２…（１）
この（１）式をｄ２について偏微分すると、次の（２）式となる。
∂Ｅ２／∂ｄ２＝２（Ｊ２ｘ２＋Ｊ２ｙ２＋Ｊ２ｚ２）ｄ２＋２（Ｊ２ｘＪ３ｘ＋
Ｊ２ｙＪ３ｙ＋Ｊ２ｚＪ３ｚ）ｄ３＝０…（２）
【００３５】
そして、この（２）式からｄ２を求める。
ｄ２＝−（Ｊ２ｘＪ３ｘ＋Ｊ２ｙＪ３ｙ＋Ｊ２ｚＪ３ｚ）ｄ３／（Ｊ２ｘ２
＋Ｊ２ｙ２＋Ｊ２ｚ２）…（３）
ここで、Ｊ２ｘは第２軸Ｌｃ−２が動作した際に、Ｏ５がＸ軸方向にどれだけ動くかを示す値であり、他のＪ２ｙ，Ｊ２ｚ，Ｊ３ｘ，Ｊ３ｙ，Ｊ３ｚも同様の値を意味している。式（３）の分母は第２軸Ｌｃ−３が１ラジアン回転した際のＯ５の移動距離の２乗を示している。
つまり、円弧の長さ＝円弧の半径×円弧の中心角度、であるから、分母は第２軸Ｌｃ−２の回転中心Ｏ２からＯ５までの、第２軸Ｌｃ−２を法線とする２次元平面における距離の２乗に等しく、その値は常に０より大きい。
【００３６】
ヤコビアンＪを各アーム８，９の角度θ２，θ３とアーム長Ｌ１，Ｌ２から導出し、（３）式に代入すると、ｄ２は次のようになる。
ｄ２＝−（Ｊ２ｘＪ３ｘ＋Ｊ２ｙＪ３ｙ＋Ｊ２ｚＪ３ｚ）ｄ３／（Ｊ２ｘ２＋Ｊ２ｙ２＋Ｊ２ｚ２）
＝−［｛Ｌ１ｃｏｓθ２＋Ｌ２ｃｏｓ（θ２＋θ３）｝Ｌ２ｃｏｓ（θ２＋θ３）
＋｛Ｌ１ｓｉｎθ２＋Ｌ２ｓｉｎ（θ２＋θ３）｝Ｌ２ｓｉｎ（θ２＋θ３）｝ｄ３］
／（Ｌ１２＋Ｌ２２＋２Ｌ１Ｌ２ｃｏｓθ３）
＝−｛Ｌ１Ｌ２ｃｏｓθ２ｃｏｓ（θ２＋θ３）＋Ｌ２２ｃｏｓ２（θ２＋θ３）
＋Ｌ１Ｌ２ｓｉｎθ２ｓｉｎ（θ２＋θ３）＋Ｌ２２ｓｉｎ２（θ２＋θ３）｝ｄ３
／（Ｌ１２＋Ｌ２２＋２Ｌ１Ｌ２ｃｏｓθ３）
＝−（Ｌ２２＋Ｌ１Ｌ２ｃｏｓθ３）ｄ３／（Ｌ１２＋Ｌ２２＋２Ｌ１Ｌ２ｃｏｓθ３）
…（４）
【００３７】
このように、第１の上アーム９の角度に応じて誤差に対する適切な補正量ｄ２が変化する。この補正量ｄ２を下アーム８の次のサンプリングタイム時の位置指令に加算し、この加算値を真の位置指令として駆動回路２２に出力する。これにより、第１の上アーム９が下アーム８により補正されてより正しい位置に動作する。
【００３８】
本実施形態によれば、外乱の影響を受けて制御し難い第１の上アーム９の位置誤差を、第１の上アーム９と同じ回転方向（上下方向）に回転する下アーム８の位置を補正することで抑制するので、比較的容易に第１の上アーム９をより正しい位置に動作させることができる。
【００３９】
この第１の上アーム９の位置誤差の抑制は、Ｏ５が動作する軌道の形状に関係なく適用可能で、直線軌道動作であっても曲線状軌道動作であっても、より正しい位置に制御できる。
【００４０】
また、第１の上アーム９が振動したとしても、その振動を抑制するために、下アーム８の位置指令にはＯ４の位置誤差を修正する程度の補正値が加算されるだけであるから、下アーム８に大きく振動する指令値が与えられることはなく、システムの停止を招く事態を生ずる恐れがない。しかも、下アーム８および第１の上アーム９以外の他のリンクとのバランスが崩れる恐れもなく、速度むらが発生するおそれもなく、正常に動作させることができる。
【００４１】
［第２の実施形態］
本発明の第２の実施形態は、アーム先端Ｏ５の移動速度から下アーム８の位置指令に対する補正値ｄ２を求めるものである。移動速度は、前回のサンプリングタイム時のＯ５の位置と今回のサンプリングタイム時のＯ５の位置からＯ５の移動距離が求められるので、この移動距離を前回のサンプリングタイムと今回のサンプリングタイムとの時間間隔で除すことにより求めることができる。
【００４２】
移動方向に対する誤差は、軌道精度には影響しない。そのため、移動方向以外の位置誤差の抑制は軌道精度の観点から効果的である。但し、移動方向への誤差は速度むらとなるので注意が必要である。
【００４３】
以下において、
下アーム８のヤコビアン：Ｊ２＝（Ｊ２ｘ，Ｊ２ｙ，Ｊ２ｚ）
第１の上アーム９のヤコビアン：Ｊ３＝（Ｊ３ｘ，Ｊ３ｙ，Ｊ３ｚ）
第１の上アーム９の先端Ｏ５の位置誤差：ｅ＝（ｅｘ，ｅｙ，ｅｚ）＝Ｊ２ｄ２＋Ｊ３ｄ３
第１の上アーム９の先端Ｏ５の位置誤差の絶対値：Ｅ
下アーム８のアーム長：Ｌ１
第１の上アーム９のアーム長；Ｌ２
第１の上アーム９の先端Ｏ５の速度ｖ＝［ｖｘ，ｖｙ，ｖｚ］
とする。
【００４４】
移動方向以外の位置誤差Ｄの２乗は、
Ｄ２＝｛（ｖｙ２＋ｖｚ２）ｅｘ２＋（ｖｚ２＋ｖｘ２）ｅｙ２＋（ｖｘ２＋ｖｙ２）ｅｚ２−２ｖｘｖｙｅｘｅｙ−２ｖｙｖｚｅｙｅｚ−２ｖｚｖｘｅｚｅｘ｝／（ｖｘ２＋ｖｙ２＋ｖｚ２）…（５）
【００４５】
この位置誤差Ｄを最小化する下アーム８の補正量ｄ２ａは次の（６）式となる。
ｄ２ａ＝−｛Ｊ２ｘＪ３ｘ（ｖｙ２＋ｖｚ２）＋Ｊ２ｙＪ３ｙ（ｖｚ２＋ｖｘ２）＋Ｊ２ｚＪ３ｚ（ｖｘ２＋ｖｙ２）−（Ｊ２ｘＪ３ｙ＋Ｊ２ｙＪ３ｘ）ｖｘｖｙ−（Ｊ２ｙＪ３ｚ＋Ｊ２ｚＪ３ｙ）ｖｙｖｚ−（Ｊ２ｚＪ３ｘ＋Ｊ２ｘＪ３ｙ）ｖｚｖｘ｝ｄ３／｛Ｊ２ｘ２（ｖｙ２＋ｖｚ２）＋Ｊ２ｙ２（ｖｚ２＋ｖｘ２）＋Ｊ２ｚ２（ｖｘ２＋ｖｙ２）−２Ｊ２ｘＪ２ｙｖｘｖｙ−２Ｊ２ｙＪ２ｚｖｙｖｚ−２Ｊ２ｚＪ２ｘｖｚｖｘ｝…（６）
【００４６】
このｄ２ａは速度が０、即ち停止中には計算できない。停止中には全方向において位置誤差Ｅが最小化するようにする。この場合、下アーム８の補正量ｄ２ｂは下の（９）式で求めることができる。
Ｅ２＝（Ｊ２ｄ２＋Ｊ３ｄ３）（Ｊ２ｄ２＋Ｊ３ｄ３）Ｔ＝Ｊ２Ｊ２Ｔｄ２２＋２Ｊ２Ｊ３Ｔｄ２ｄ３＋Ｊ３Ｊ３Ｔｄ３２＝（Ｊ２ｘ２＋Ｊ２ｙ２＋Ｊ２ｚ２）ｄ２２＋２（Ｊ２ｘＪ３ｘ＋Ｊ２ｙＪ３ｙ＋Ｊ２ｚＪ３ｚ）＋（Ｊ３ｘ２＋Ｊ３ｙ２＋Ｊ３ｚ２）ｄ３２…（７）
【００４７】
このＥ２をｄ２について偏微分すると、次の（８）式となる。
∂Ｅ２／∂ｄ２＝２（Ｊ２ｘ２＋Ｊ２ｙ２＋Ｊ２ｚ２）ｄ２＋２（Ｊ２ｘＪ３ｘ＋Ｊ２ｙＪ３ｙ＋Ｊ２ｚＪ３ｚ）ｄ３＝０…（８）
そして、（８）式からｄ２ｂを求めると、
ｄ２ｂ＝−（Ｊ２ｘＪ３ｘ＋Ｊ２ｙＪ３ｙ＋Ｊ２ｚＪ３ｚ）ｄ３
／（Ｊ２ｘ２＋Ｊ２ｙ２＋Ｊ２ｚ２）…（９）
【００４８】
停止中から移動する際、ｄ２ｂとｄ２ａを滑らかに接続するために、速度ｖ１以下（ｖ１は０を含む）の場合には、ｄ２ｂの補正量を用い、速度ｖ２以上のときは、ｄ２ａで補正し、その間はｄ２ｂとｄ２ａとの間を例えば線形補間するとすれば、速度をｖとすると、補正値ｄ２は
ｄ２＝ｄ２ａ＋（ｄ２ｂ−ｄ２ａ）（ｖ−ｖ１）／（ｖ２−ｖ１）
となる。
【００４９】
この方法によれば、動作中には移動方向以外の誤差を最小化するために、軌道精度がより改善される。また、移動方向の誤差を最小化している際には、第１の上アーム９の移動長に比例した速度むらが発生するが、第１の上アーム９の振動が速度に依存しないものであるとすれば、速度が高いほど速度むらの相対的な大きさは小さくなる。即ち、速度が高い領域のみ移動方向以外の誤差を最小化する本方法によれば、速度むらの小さなものとなり、問題とはならない。
【００５０】
なお、本発明の多関節型ロボットは６軸のものに限らない。少なくとも第１〜３軸のリンクからロボットアームを構成するものであれば良い。
【符号の説明】
【００５１】
図面中、２はロボット本体、５はベース（ベースリンク）、７はショルダ（第の１リンク）、８は下アーム（第２のリンク）、９は第１の上アーム（第３のリンク）、１０は第２の上アーム（第４のリンク）、１１は手首アーム（第５のリンク）、１２はフランジ（第６のリンク）、１５はサーボモータ、１６は歯車減速装置、１９は歯付きベルト、２０はベルト伝動機構を示す。
【図１】
2015178156.tif 000003
【図２】
2015178156.tif 000004
【図３】
2015178156.tif 000005
【図４】
2015178156.tif 000006
【図５】
2015178156.tif 000007
【図６】
2015178156.tif 000008