(57)【要約】【課題】鉛直方向を検出することのできるロボットの制御装置を提供する。【解決手段】ロボット１０は、第４軸線Ｊ４の方向に延びるとともに、第４軸線Ｊ４を中心として回転可能に支持された第２上アーム１６Ｂと、第４軸線Ｊ４に直交する第５軸線Ｊ５から離れる方向へ延びるとともに、第２上アーム１６Ｂにより第５軸線Ｊ５を中心として回転可能に支持された手首部１７とを備える。第４軸線Ｊ４を中心として第２上アーム１６Ｂを重力により自由回転させ、且つ第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた場合に、手首部１７の先端が指す方向を鉛直方向として検出する。【選択図】      図１
2011212835.tif 000002
【特許請求の範囲】
【請求項１】
ｂ軸線の方向に延びるとともに、前記ｂ軸線を中心として回転可能に支持されたｂ回転部と、
前記ｂ軸線に直交するａ軸線から離れる方向へ延びるとともに、前記ｂ回転部により前記ａ軸線を中心として回転可能に支持されたａ回転部と、
を備えるロボットに適用され、
前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させ、且つ前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出する鉛直方向検出手段を備えることを特徴とするロボットの制御装置。
【請求項２】
ｂ軸線の方向に延びるとともに前記ｂ軸線を中心として回転可能に支持されたｂ回転部と、
前記ｂ軸線に直交するａ軸線から離れる方向へ延びるとともに、前記ｂ回転部により前記ａ軸線を中心として回転可能に支持されたａ回転部と、
を備えるロボットに適用され、
前記ａ回転部の延びる方向が前記ｂ軸線の方向とずれるように、前記ａ軸線を中心として前記ａ回転部を回転させる予備動作手段と、
前記予備動作手段により前記ａ軸線を中心として前記ａ回転部を回転させた後に、前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させ、その後に前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出する鉛直方向検出手段と、
を備えることを特徴とするロボットの制御装置。
【請求項３】
前記予備動作手段は、前記ａ軸線を中心として前記ａ回転部を回転させることが可能な範囲内において、前記ａ回転部の先端が前記ｂ軸線から最も離れる位置に前記ａ回転部を回転させることを特徴とする請求項２に記載のロボットの制御装置。
【請求項４】
前記ｂ軸線を中心として前記ｂ回転部がｂ範囲よりも回転することを規制するｂ規制手段を備え、
前記鉛直方向検出手段は、前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させた際に、前記ｂ規制手段により前記ｂ回転部の回転が規制された場合には、前記ｂ軸線を中心として前記ｂ範囲の中央位置まで前記ｂ回転部を回転させた後に再度、前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させることを特徴とする請求項１〜３のいずれか1項に記載のロボットの制御装置。
【請求項５】
前記ａ軸線を中心として前記ａ回転部がａ範囲よりも回転することを規制するａ規制手段を備え、
前記鉛直方向検出手段は、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた際に、前記ａ規制手段により前記ａ回転部の回転が規制された場合には、重力により自由回転させられる方向と反対方向へ前記ａ回転部を回転させるとともに、その位置から前記ａ軸線を中心として前記ａ回転部を重力により自由回転させることを繰返して、その自由回転時に前記ａ回転部が停止状態で維持される位置を検出し、その位置に前記ａ回転部を停止させた場合に、前記ａ回転部の先端が指す方向の反対方向を鉛直方向として検出することを特徴とする請求項１〜４のいずれか1項に記載のロボットの制御装置。
【請求項６】
前記ｂ回転部はｃ軸線を中心として回転可能に支持されており、
前記ａ回転部がａ範囲よりも回転することを規制するａ規制手段を備え、
前記鉛直方向検出手段は、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた際に、前記ａ規制手段により前記ａ回転部の回転が規制された場合には、前記ｃ軸線を中心として前記ｂ回転部を回転させ、前記ａ規制手段による前記ａ回転部の回転規制が解除された場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出することを特徴とする請求項１〜４のいずれか1項に記載のロボットの制御装置。
【請求項７】
前記鉛直方向検出手段は、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた時に、前記ａ回転部が停止状態のまま維持された場合には、前記ａ軸線を中心として前記ａ回転部を回転させることが可能な範囲の中央側へ前記ａ回転部を回転させた後に再度、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出することを特徴とする請求項１〜４のいずれか1項に記載のロボットの制御装置。
【請求項８】
前記ａ回転部の先端の中央には、所定質量の錘が取り付けられていることを特徴とする請求項１〜７のいずれか1項に記載のロボットの制御装置。
【請求項９】
前記ロボットは、
第１軸線の方向に延びるとともに、前記第１軸線を中心として回転可能に支持された第１回転部と、
前記第１軸線に直交する平面に含まれる第２軸線から離れる方向へ延びるとともに、前記第１回転部により前記第２軸線を中心として回転可能に支持された第２回転部と、
前記第２軸線に平行な第３軸線から離れる方向へ延びるとともに、前記第２回転部により前記第３軸線を中心として回転可能に支持された第３回転部と、
前記第３軸線に直交する平面に含まれる第４軸線の方向に延びるとともに、前記第３回転部により前記第４軸線を中心として回転可能に支持された第４回転部と、
前記第４軸線に直交する第５軸線から離れる方向へ延びるとともに、前記第４回転部により前記第５軸線を中心として回転可能に支持された第５回転部と、
前記第５軸線に直交する第６軸線の方向に延びるとともに、前記第５回転部により前記第６軸線を中心として回転可能に支持された第６回転部と、
を備える６軸ロボットであって、
前記ｂ軸線及び前記ｂ回転部は、それぞれ前記第４軸線及び前記第４回転部であり、
前記ａ軸線及び前記ａ回転部は、それぞれ前記第５軸線及び前記第５回転部であることを特徴とする請求項１〜８のいずれか1項に記載のロボットの制御装置。
【請求項１０】
前記鉛直方向検出手段により検出された鉛直方向に基づいて、前記ロボットにおける所定方向を基準方向とするロボット座標系を変換して、前記基準方向を前記鉛直方向に一致させたユーザ座標系を設定するユーザ座標系設定手段を備えることを特徴とする請求項１〜９のいずれか1項に記載のロボットの制御装置。
【請求項１１】
前記ロボット座標系及び前記ユーザ座標系は、ｘ軸，ｙ軸，ｚ軸を有する三次元座標系であり、
前記ユーザ座標系設定手段は、変換前の前記ロボット座標系のｘｙ平面に投影した前記鉛直方向のベクトルと前記変換前のロボット座標系のｙ軸方向のベクトルとを平行にするのに必要な角度だけ、前記ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸を回転させるとともに、変換前の前記ロボット座標系のｘｙ平面上において前記投影された前記鉛直方向のベクトルに垂直なベクトルを中心として、前記鉛直方向のベクトルと前記ロボット座標系のｚ軸方向のベクトルとを平行にするのに必要な角度だけ、前記ロボット座標系のｘ軸，ｙ軸，ｚ軸を回転させることにより、前記ユーザ座標系を設定することを特徴とする請求項１０に記載のロボットの制御装置。
【請求項１２】
前記ロボット座標系及び前記ユーザ座標系は、ｘ軸，ｙ軸，ｚ軸を有する三次元座標系であり、
前記ユーザ座標系設定手段は、前記ロボット座標系のｘｙ平面に投影した前記鉛直方向のベクトルと前記ロボット座標系のｙ軸方向のベクトルとを平行にするように、前記ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸を回転させた後、前記鉛直方向のベクトルと前記回転後のロボット座標系のｚ軸方向のベクトルとを平行にするように、前記回転後のロボット座標系においてｘ軸を中心としてｙ軸及びｚ軸を回転させることにより、前記ユーザ座標系を設定することを特徴とする請求項１０に記載のロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の関節を備えるロボットの制御装置に関する。
【背景技術】
【０００２】
この種の制御装置として、角度計測器により計測されたロボットの関節角度を元に、各関節に作用する重力トルクを算出し、この重力トルクをモータの制御系に補償するものがある（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００３−２１１３７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、特許文献１に記載のものでは、直交する３軸で構成された３次元の座標系を設定し、ロボットの関節角度を座標系の位置データに変換している。すなわち、設定された座標系の基準方向とロボットの基準方向との関係に基づいて、各関節に作用する重力トルクを算出している。そして、座標系の基準方向と鉛直方向とが一致する場合において、重力トルクを補償して位置ずれを抑制している。
【０００５】
しかしながら、ロボットの基準方向と鉛直方向との関係がずれている場合には、重力トルクの補償を適切に行うことができず、ロボットの制御精度が低下するおそれがある。
【０００６】
本発明は、こうした実情に鑑みてなされたものであり、鉛直方向を検出することのできるロボットの制御装置を提供することを主たる目的とするものである。
【課題を解決するための手段】
【０００７】
本発明は、上記課題を解決するために、以下の手段を採用した。
【０００８】
第１の発明は、ｂ軸線の方向に延びるとともに、前記ｂ軸線を中心として回転可能に支持されたｂ回転部と、前記ｂ軸線に直交するａ軸線から離れる方向へ延びるとともに、前記ｂ回転部により前記ａ軸線を中心として回転可能に支持されたａ回転部と、を備えるロボットに適用され、前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させ、且つ前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出する鉛直方向検出手段を備えることを特徴とする。
【０００９】
上記構成によれば、ｂ軸線の方向に延びるｂ回転部（例えばロボットのアーム）を、ｂ軸線を中心として回転させることができる。また、ｂ回転部により回転可能に支持されるとともに、ｂ軸線に直交するａ軸線から離れる方向へ延びるａ回転部（例えばロボットの手首部）を、ａ軸線を中心として回転させることができる。
【００１０】
ここで、ａ回転部の延びる方向とｂ軸線の方向とがずれている場合、すなわちａ軸線を中心としてａ回転部がｂ軸線上からずれた位置へ回転させられている場合には、ａ回転部及びｂ回転部を併せた重心は、ｂ軸線上からａ回転部の先端側へずれることとなる。このため、ｂ軸線を中心としてｂ回転部が重力により自由回転させられた場合には、ａ回転部の先端が最下端に位置するようにｂ回転部が回転させられる。また、ａ軸線を中心としてａ回転部が重力により自由回転させられた場合には、ａ回転部の先端が最下端に位置するようにａ回転部が回転させられる。
【００１１】
したがって、これらが共に行われた場合には、ａ回転部の先端が鉛直方向を指すこととなる。よって、ａ回転部の先端が指す方向を鉛直方向として検出することができ、ひいてはロボットの基準方向と鉛直方向との関係を補正することができる。
【００１２】
なお、ｂ軸線を中心としてｂ回転部を重力により自由回転させた場合に、その釣合い状態において、ｂ回転部の回転を規制してもよいし、ｂ回転部を回転可能なままの状態としてもよい。同様にして、ａ軸線を中心としてａ回転部を重力により自由回転させた場合に、その釣合い状態において、ａ回転部の回転を規制してもよいし、ａ回転部を回転可能なままの状態としてもよい。
【００１３】
また、ｂ軸線を中心としてｂ回転部を重力により自由回転させることと、ａ軸線を中心としてａ回転部を重力により自由回転させることとを、同時に行ってもよいし、いずれか一方を先に行った後に他方を行うようにしてもよい。
【００１４】
第２の発明は、ｂ軸線の方向に延びるとともに前記ｂ軸線を中心として回転可能に支持されたｂ回転部と、前記ｂ軸線に直交するａ軸線から離れる方向へ延びるとともに、前記ｂ回転部により前記ａ軸線を中心として回転可能に支持されたａ回転部と、を備えるロボットに適用され、前記ａ回転部の延びる方向が前記ｂ軸線の方向とずれるように、前記ａ軸線を中心として前記ａ回転部を回転させる予備動作手段と、前記予備動作手段により前記ａ軸線を中心として前記ａ回転部を回転させた後に、前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させ、その後に前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出する鉛直方向検出手段と、を備えることを特徴とする。
【００１５】
上記構成によれば、鉛直方向を検出する予備動作として、ａ回転部の延びる方向がｂ軸線の方向とずれるように、ａ軸線を中心としてａ回転部が回転させられる。このため、ａ回転部及びｂ回転部を併せた重心を、ｂ軸線上からａ回転部の先端側へ意図的にずらすことができる。
【００１６】
その後に、ｂ軸線を中心としてｂ回転部が重力により自由回転させられるため、ａ回転部の先端が最下端に位置するようにｂ回転部が回転させられる。そして、その後にａ軸線を中心としてａ回転部が重力により自由回転させられた場合には、ａ回転部の先端が最下端に位置するようにａ回転部が回転させられる。
【００１７】
したがって、ａ回転部の先端が鉛直方向を指すこととなり、ａ回転部の先端が指す方向を鉛直方向として検出することができる。
【００１８】
なお、上記の場合も、ａ軸線，ｂ軸線を中心として、それぞれａ回転部，ｂ回転部を重力により自由回転させた場合に、その釣合い状態において、ａ回転部やｂ回転部の回転を規制してもよいし、ａ回転部やｂ回転部を回転可能なままの状態としてもよい。
【００１９】
第３の発明では、前記予備動作手段は、前記ａ軸線を中心として前記ａ回転部を回転させることが可能な範囲内において、前記ａ回転部の先端が前記ｂ軸線から最も離れる位置に前記ａ回転部を回転させることを特徴とする。
【００２０】
上記構成によれば、鉛直方向を検出する予備動作として、回転可能な範囲内において、ａ回転部の先端がｂ軸線から最も離れる位置にａ回転部が回転させられる。このため、ａ回転部及びｂ回転部を併せた重心を、ｂ軸線上からａ回転部の先端側へ最大限ずらすことができる。したがって、ｂ軸線を中心としてｂ回転部を重力により自由回転させた場合に、ａ回転部の先端をより確実に最下端に位置させることができる。
【００２１】
第４の発明では、前記ｂ軸線を中心として前記ｂ回転部がｂ範囲よりも回転することを規制するｂ規制手段を備え、前記鉛直方向検出手段は、前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させた際に、前記ｂ規制手段により前記ｂ回転部の回転が規制された場合には、前記ｂ軸線を中心として前記ｂ範囲の中央位置まで前記ｂ回転部を回転させた後に再度、前記ｂ軸線を中心として前記ｂ回転部を重力により自由回転させることを特徴とする。
【００２２】
上記構成によれば、ｂ軸線を中心としてｂ回転部がｂ範囲よりも回転することが、ｂ規制手段により規制される。例えば、ｂ軸線の方向に延びるとともに、ｂ軸線を中心として回転させられるｂ回転部では、ｂ範囲が略３６０度に設定されることが多い。このため、ｂ軸線を中心としてｂ回転部を重力により自由回転させた場合に、ａ回転部の先端が最下端よりも高い位置にある状態で、ｂ回転部の回転がｂ規制手段により規制されるおそれがある。
【００２３】
ここで、ｂ軸線を中心としてｂ回転部を重力により自由回転させた際に、ｂ規制手段によりｂ回転部の回転が規制された場合には、ｂ軸線を中心としてｂ範囲の中央位置までｂ回転部が回転させられる。このため、ｂ回転部の回転が規制された位置と反対回転側に、ａ回転部の先端が最下端となる位置が存在する場合には、この中央位置までにａ回転部の先端が最上端を越えることとなる。したがって、その後に再度、ｂ軸線を中心としてｂ回転部を重力により自由回転させた場合には、ａ回転部の先端が最下端に位置するようにｂ回転部を回転させることができる。
【００２４】
そして、上述したように、ａ軸線を中心としてａ回転部が重力により自由回転させられた場合に、ａ回転部の先端が指す方向が鉛直方向として検出される。よって、ｂ軸線を中心としてｂ回転部がｂ範囲よりも回転することを規制するｂ規制手段を備える場合であっても、鉛直方向を正確に検出することができる。
【００２５】
なお、ｂ軸線を中心としてｂ範囲の中央位置までｂ回転部を回転させる際には、少なくともｂ範囲の中央位置までｂ回転部を回転させればよく、ｂ範囲の中央位置を越えた位置までｂ回転部を回転させてもよい。
【００２６】
また、ｂ規制手段は、ｂ回転部の回転を、制御的（ソフト的）に規制するものであってもよいし、物理的（機械的）に規制するものであってもよい。
【００２７】
第５の発明では、前記ａ軸線を中心として前記ａ回転部がａ範囲よりも回転することを規制するａ規制手段を備え、前記鉛直方向検出手段は、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた際に、前記ａ規制手段により前記ａ回転部の回転が規制された場合には、重力により自由回転させられる方向と反対方向へ前記ａ回転部を回転させるとともに、その位置から前記ａ軸線を中心として前記ａ回転部を重力により自由回転させることを繰返して、その自由回転時に前記ａ回転部が停止状態で維持される位置を検出し、その位置に前記ａ回転部を停止させた場合に、前記ａ回転部の先端が指す方向の反対方向を鉛直方向として検出することを特徴とする。
【００２８】
上記構成によれば、ａ軸線を中心としてａ回転部がａ範囲よりも回転することが、ａ規制手段により規制される。例えば、ｂ軸線に直交するａ軸線から離れる方向へ延びるとともに、ａ軸線を中心として回転させられるａ回転部では、ａ範囲が３６０度よりも狭く設定されることが多く、詳しくは略１８０度に設定されることが多い。このため、ａ軸線を中心としてａ回転部を重力により自由回転させた場合に、ａ回転部の先端が最下端よりも高い位置にある状態で、ａ回転部の回転がａ規制手段により規制されるおそれがある。
【００２９】
ここで、ａ軸線を中心としてａ回転部を重力により自由回転させた際に、ａ規制手段によりａ回転部の回転が規制された場合には、重力により自由回転させられる方向と反対方向へａ回転部を回転させるとともに、その位置からａ軸線を中心としてａ回転部を重力により自由回転させることが繰返される。このため、ａ回転部の先端が徐々に上昇するようにａ軸線を中心としてａ回転部が回転させられ、その都度ａ軸線を中心としてａ回転部が重力により自由回転させられる。
【００３０】
したがって、ａ回転部の先端が最上端に到達するまでは、ａ軸線を中心としてａ回転部を重力により自由回転させた時に、重力の作用によりａ回転部が回転を開始する。そして、ａ回転部の先端が最上端に到達した状態では、ａ軸線を中心としてａ回転部を重力により自由回転させた時に、ａ回転部が停止状態で維持される。よって、ａ回転部が停止状態で維持される位置にａ回転部を停止させた場合に、ａ回転部の先端が指す方向の反対方向を鉛直方向として検出することができる。
【００３１】
なお、ａ規制手段は、ａ回転部の回転を、制御的（ソフト的）に規制するものであってもよいし、物理的（機械的）に規制するものであってもよい。
【００３２】
第６の発明では、前記ｂ回転部はｃ軸線を中心として回転可能に支持されており、前記ａ回転部がａ範囲よりも回転することを規制するａ規制手段を備え、前記鉛直方向検出手段は、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた際に、前記ａ規制手段により前記ａ回転部の回転が規制された場合には、前記ｃ軸線を中心として前記ｂ回転部を回転させ、前記ａ規制手段による前記ａ回転部の回転規制が解除された場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出することを特徴とする。
【００３３】
上記構成によれば、ｃ軸線を中心としてｂ回転部を回転させることができる。このとき、ｃ軸線を中心としてｂ回転部を回転させることにより、水平面に対するｂ回転部の傾きを変更することができる場合がある。
【００３４】
そして、ａ軸線を中心としてａ回転部を重力により自由回転させた際に、ａ規制手段によりａ回転部の回転が規制された場合には、ａ回転部の先端が最下端よりも高い位置にある。この場合には、ａ回転部側が高くなるように、ｂ回転部が水平面に対して傾いた状態となっている。
【００３５】
一方、ａ軸線を中心としてａ回転部を重力により自由回転させた際に、ａ規制手段によりａ回転部の回転が規制されていない場合には、ａ回転部の先端が最下端に位置している。この場合には、ａ回転部側が低くなるように、ｂ回転部が水平面に対して傾いた状態となっている。
【００３６】
したがって、ｃ軸線を中心としてｂ回転部を回転させ、ａ規制手段によるａ回転部の回転規制が解除された場合には、ａ回転部の先端が鉛直方向を指す状態となる。よって、この場合にａ回転部の先端が指す方向を、鉛直方向として検出することができる。
【００３７】
第７の発明では、前記鉛直方向検出手段は、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた時に、前記ａ回転部が停止状態のまま維持された場合には、前記ａ軸線を中心として前記ａ回転部を回転させることが可能な範囲の中央側へ前記ａ回転部を回転させた後に再度、前記ａ軸線を中心として前記ａ回転部を重力により自由回転させた場合に、前記ａ回転部の先端が指す方向を鉛直方向として検出することを特徴とする。
【００３８】
ａ軸線を中心としてａ回転部を重力により自由回転させた時に、ａ回転部が停止状態のまま維持された場合には、ａ回転部の先端が鉛直方向を指す状態、又はａ回転部の先端が鉛直方向と反対方向を指す状態にある。
【００３９】
この点、上記構成によれば、そのような場合には、ａ軸線を中心としてａ回転部を回転させることが可能な範囲の中央側へａ回転部が回転させられる。このため、ａ回転部の先端が鉛直方向を指す状態、又はａ回転部の先端が鉛直方向と反対方向を指す状態から、ａ回転部の角度がずれることとなる。
【００４０】
その後に再度、ａ軸線を中心としてａ回転部が重力により自由回転させられた場合には、ａ回転部の先端が鉛直方向を指す状態となる。よって、ａ回転部の先端が指す方向を鉛直方向として検出することができる。
【００４１】
ａ回転部やｂ回転部の重心が、それぞれの中心線からずれている場合には、ａ回転部及びｂ回転部を重力により自由回転させた際に、ａ回転部の先端が指す方向と鉛直方向とがずれるおそれがある。
【００４２】
この点、第８の発明では、前記ａ回転部の先端の中央には、所定質量の錘が取り付けられているといった構成を採用している。このため、ａ回転部及び錘を併せた重心を、ａ回転部の中心線に近付けることができる。また、ａ回転部とｂ回転部と錘とを併せた重心を、ａ軸線に垂直でｂ軸線を含む平面に近付けることができる。
【００４３】
したがって、ｂ軸線を中心としてｂ回転部を重力により自由回転させた場合に、ａ回転部の先端がより最下端に近付くようにｂ回転部を回転させることができる。また、ａ軸線を中心としてａ回転部を重力により自由回転させた場合に、ａ回転部の先端が指す方向をより鉛直方向に近付けることができる。その結果、鉛直方向の検出精度を向上させることができる。
【００４４】
第９の発明では、前記ロボットは、第１軸線の方向に延びるとともに、前記第１軸線を中心として回転可能に支持された第１回転部と、前記第１軸線に直交する平面に含まれる第２軸線から離れる方向へ延びるとともに、前記第１回転部により前記第２軸線を中心として回転可能に支持された第２回転部と、前記第２軸線に平行な第３軸線から離れる方向へ延びるとともに、前記第２回転部により前記第３軸線を中心として回転可能に支持された第３回転部と、前記第３軸線に直交する平面に含まれる第４軸線の方向に延びるとともに、前記第３回転部により前記第４軸線を中心として回転可能に支持された第４回転部と、前記第４軸線に直交する第５軸線から離れる方向へ延びるとともに、前記第４回転部により前記第５軸線を中心として回転可能に支持された第５回転部と、前記第５軸線に直交する第６軸線の方向に延びるとともに、前記第５回転部により前記第６軸線を中心として回転可能に支持された第６回転部と、を備える６軸ロボットであって、前記ｂ軸線及び前記ｂ回転部は、それぞれ前記第４軸線及び前記第４回転部であり、前記ａ軸線及び前記ａ回転部は、それぞれ前記第５軸線及び前記第５回転部であることを特徴とする。
【００４５】
上記構成によれば、６軸ロボットにおいて、第４軸線を中心として回転する第４回転部と、第５軸線を中心として回転する第５回転部とを用いて、鉛直方向を検出することができる。
【００４６】
ここで、第４回転部は第４軸線の方向に延びているため、第４軸線を中心として第４回転部を回転させた場合に、第４回転部は第４軸線上で回転することとなり、第４回転部が周囲に振り回されることがない。また、一般に、第５回転部の長さは、第２回転部や第３回転部の長さよりも短く設定されている。このため、第５軸線から離れる方向へ延びる第５回転部が、第５軸線を中心として回転させられたとしても、第５回転部が周囲に振り回される範囲は比較的狭くなる。
【００４７】
したがって、鉛直方向を検出する際に、ロボットが周囲の設備等に接触することを抑制することができる。
【００４８】
ところで、一般にティーチング等では、ロボットを基準として設定しているロボット座標系（ベース座標系）を用いて、ロボットの座標をユーザが入力している。
【００４９】
しかしながら、作業効率化や設備の影響を受けて、ロボットが壁や天井、さらには傾いた壁等に配置されることがある。その場合に、ユーザが床面（地面）を基準としたユーザ座標系（ワールド座標系）ではなく、ロボットを基準としたロボット座標系を用いてロボットの座標を入力するためには、ユーザ座標系での座標をロボット座標系での座標に変換する必要がある。その結果、意図した座標へロボットを動作させることができなかったり、変換作業に手間がかかったりするおそれがある。
【００５０】
この点、第１０の発明では、前記鉛直方向検出手段により検出された鉛直方向に基づいて、前記ロボットにおける所定方向を基準方向とするロボット座標系を変換して、前記基準方向を前記鉛直方向に一致させたユーザ座標系を設定するユーザ座標系設定手段を備えている。
【００５１】
上記構成によれば、検出された鉛直方向に基づいて、ロボットにおける所定方向を基準方向とするロボット座標系を変換して、その基準方向を鉛直方向に一致させたユーザ座標系が設定される。すなわち、ロボットの基準方向と検出された鉛直方向との関係を用いて、ロボット座標系とユーザ座標系との関係が求められる。このため、ロボットの制御装置は、一方の座標系での座標を他方の座標系での座標に変換することが可能となる。したがって、ユーザはユーザ座標系を用いてロボットの座標を入力することができ、変換作業の手間を減らすことができるとともに、意図した座標へロボットを動作させることが容易となる。
【００５２】
具体的には、第１１の発明では、前記ロボット座標系及び前記ユーザ座標系は、ｘ軸，ｙ軸，ｚ軸を有する三次元座標系であり、前記ユーザ座標系設定手段は、変換前の前記ロボット座標系のｘｙ平面に投影した前記鉛直方向のベクトルと前記変換前のロボット座標系のｙ軸方向のベクトルとを平行にするのに必要な角度だけ、前記ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸を回転させるとともに、変換前の前記ロボット座標系のｘｙ平面上において前記投影された前記鉛直方向のベクトルに垂直なベクトルを中心として、前記鉛直方向のベクトルと前記ロボット座標系のｚ軸方向のベクトルとを平行にするのに必要な角度だけ、前記ロボット座標系のｘ軸，ｙ軸，ｚ軸を回転させることにより、前記ユーザ座標系を設定するといった構成を採用している。
【００５３】
上記構成によれば、変換前のロボット座標系のｘｙ平面に投影した上記鉛直方向のベクトルと変換前のロボット座標系のｙ軸方向のベクトルとを平行にするのに必要な角度だけ、ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸が回転させられる（第１制御）。これにより、ロボット座標系のｘ軸及びｙ軸が、ユーザ座標系のｘ軸及びｙ軸に対して、ｚ軸を中心として回転してずれていることを補正することができる。また、変換前のロボット座標系のｘｙ平面上において投影された上記鉛直方向のベクトルに垂直なベクトルを中心として、鉛直方向のベクトルとロボット座標系のｚ軸方向のベクトルとを平行にするのに必要な角度だけ、ロボット座標系のｘ軸，ｙ軸，ｚ軸が回転させられる（第２制御）。これにより、ロボット座標系のｚ軸が、ユーザ座標系のｚ軸に対して傾いていることを補正することができる。その結果、ロボット座標系のｘ軸，ｙ軸，ｚ軸を、それぞれユーザ座標系のｘ軸，ｙ軸，ｚ軸に変換することができる。なお、上記では、ロボットを固定した面に沿った水平方向を、ユーザ座標系のｘ軸に設定している。また、上記第１制御及び第２制御は、いずれを先に実行してもよい。
【００５４】
より具体的には、第１２の発明では、前記ロボット座標系及び前記ユーザ座標系は、ｘ軸，ｙ軸，ｚ軸を有する三次元座標系であり、前記ユーザ座標系設定手段は、前記ロボット座標系のｘｙ平面に投影した前記鉛直方向のベクトルと前記ロボット座標系のｙ軸方向のベクトルとを平行にするように、前記ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸を回転させた後、前記鉛直方向のベクトルと前記回転後のロボット座標系のｚ軸方向のベクトルとを平行にするように、前記回転後のロボット座標系においてｘ軸を中心としてｙ軸及びｚ軸を回転させることにより、前記ユーザ座標系を設定するといった構成を採用している。
【００５５】
上記構成によれば、ロボット座標系のｘｙ平面に投影した鉛直方向のベクトルとロボット座標系のｙ軸方向のベクトルとを平行にするように、ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸が回転させられる。これにより、ロボット座標系のｘ軸が、ユーザ座標系のｘ軸、すなわちロボットを固定した面に沿った水平方向と一致する。その後、鉛直方向のベクトルと回転後のロボット座標系のｚ軸方向のベクトルとを平行にするように、回転後のロボット座標系においてｘ軸を中心としてｙ軸及びｚ軸が回転させられる。これにより、ロボット座標系のｚ軸が、ユーザ座標系のｚ軸に対して傾いていることを補正することができる。その結果、ロボット座標系のｘ軸，ｙ軸，ｚ軸を、それぞれユーザ座標系のｘ軸，ｙ軸，ｚ軸に変換することができる。
【図面の簡単な説明】
【００５６】
【図１】ロボットの概要を示す正面図。
【図２】ロボットの制御装置及びその周辺構成を示す回路図。
【図３】鉛直方向検出制御の処理手順を示すフローチャート。
【図４】第２上アームを駆動して回転させる処理の手順を示すフローチャート。
【図５】第２上アーム及び手首部を第４軸線の方向からみた模式図。
【図６】手首部を駆動して回転させる処理の手順を示すフローチャート。
【図７】第２上アーム及び手首部を第５軸線の方向からみた模式図。
【図８】第２上アーム及び手首部を第５軸線の方向からみた模式図。
【図９】手首部を駆動して回転させる処理の手順を示すフローチャート。
【図１０】第２上アーム及び手首部を第５軸線の方向からみた模式図。
【図１１】第２上アーム及び手首部を第５軸線の方向からみた模式図。
【図１２】鉛直方向検出制御の変形例を示す斜視図。
【図１３】ユーザ座標系を設定する処理の手順を示すフローチャート。
【図１４】ユーザ座標系を設定する態様を示す模式図。
【発明を実施するための形態】
【００５７】
以下、垂直多関節型ロボットに具体化した一実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。
【００５８】
はじめに、ロボット１０の概要を図１に基づいて説明する。
【００５９】
同図に示すように、ロボット１０は、各関節の回転中心軸線として、第１軸線Ｊ１、第２軸線Ｊ２、第３軸線Ｊ３、第４軸線Ｊ４、第５軸線Ｊ５、第６軸線Ｊ６を有する６軸ロボットである。これら各軸線における各部の動作角度は、それぞれサーボモータ等からなる駆動源の駆動、及び減速機等による減速を通じて調整される。サーボモータは、いずれも正逆両方向の回転が可能であり、モータ駆動により原点位置を基準として各部が動作する。なお、各関節には、各軸線を中心として各部を滑らかに回転可能とする軸受け（軸受け手段）が設けられている。
【００６０】
ロボット１０の設置箇所としては、主に床、天井、壁の３通りがあり、いずれの場合であっても適用可能である。ここでは、ロボット１０が床に設置されており、第１軸線Ｊ１が基本的に鉛直方向へ延びている場合を例として説明する。すなわち、図１の下方向が基本的に鉛直方向を示すものとする。
【００６１】
ロボット１０において、基台１１は、床等に固定される固定部１２と、その固定部１２の上方に設けられる回転部１３（第１回転部）とを有しており、回転部１３が第１軸線Ｊ１を回転中心として水平方向に回転可能になっている。すなわち、回転部１３は、第１軸線Ｊ１の方向に延びるとともに、固定部１２により第１軸線Ｊ１を中心として回転可能に支持されている。
【００６２】
下アーム１５（第２回転部）が、水平方向に延びる第２軸線Ｊ２を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、下アーム１５は、第１軸線Ｊ１に直交する平面に含まれる第２軸線Ｊ２から離れる方向へ延びるとともに、回転部１３により第２軸線Ｊ２を中心として回転可能に支持されている。このため、下アーム１５の重心は、第２軸線Ｊ２から離れた位置にある。下アーム１５は、基本姿勢として鉛直方向に延びる向きに設けられている。
【００６３】
下アーム１５の上端部には、上アーム１６が、水平方向に延びる第３軸線Ｊ３（ｃ軸線）を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、上アーム１６は、第２軸線Ｊ２に平行な第３軸線Ｊ３から離れる方向へ延びるとともに、下アーム１５により第３軸線Ｊ３を中心として回転可能に支持されている。このため、上アーム１６の重心は、第３軸線Ｊ３から離れた位置にある。上アーム１６は、基本姿勢として水平方向に延びる向きに設けられている。
【００６４】
上アーム１６は、基端側と先端側とで２つのアーム部に分割されて構成されており、基端側は第１上アーム１６Ａ（第３回転部）、先端側は第２上アーム１６Ｂ（第４回転部、ｂ回転部）となっている。第２上アーム１６Ｂは、上アーム１６の長手方向に延びる第４軸線Ｊ４（ｂ軸線）を回転中心として、第１上アーム１６Ａに対してねじり方向に回転可能になっている。すなわち、第２上アーム１６Ｂは、第３軸線Ｊ３に直交する平面に含まれる第４軸線Ｊ４の方向に延びるとともに、第１上アーム１６Ａにより第４軸線Ｊ４を中心として回転可能に支持されている。
【００６５】
上アーム１６（詳しくは第２上アーム１６Ｂ）の先端部には、手首部１７（第５回転部、ａ回転部）が設けられている。手首部１７は、水平方向に延びる第５軸線Ｊ５（ａ軸線）を回転中心として、第２上アーム１６Ｂに対して回転可能になっている。すなわち、手首部１７は、第４軸線Ｊ４に直交する第５軸線Ｊ５から離れる方向へ延びるとともに、第２上アーム１６Ｂにより第５軸線Ｊ５を中心として回転可能に支持されている。このため、手首部１７の重心は、第５軸線Ｊ５から離れた位置にある。
【００６６】
手首部１７の先端部には、ワークやツール等を取り付けるためのハンド部１８（第６回転部）が設けられている。ハンド部１８は、その中心線である第６軸線Ｊ６を回転中心として、ねじり方向に回転可能になっている。すなわち、ハンド部１８は、第５軸線Ｊ５に直交する第６軸線Ｊ６の方向に延びるとともに、手首部１７により第６軸線Ｊ６を中心として回転可能に支持されている。
【００６７】
ロボット１０の各部は、その延びる方向の中心線に関して略対称に形成されており、略中心線上に重心が位置している。具体的には、回転部１３は、その延びる方向の中心線（第１軸線Ｊ１）に関して略対称に形成されており、略第１軸線Ｊ１上に重心が位置している。下アーム１５は、その延びる方向の中心線（基本姿勢では第１軸線Ｊ１）に関して略対称に形成されており、略その延びる方向の中心線上に重心が位置している。第２上アーム１６Ｂは、その延びる方向の中心線（第４軸線Ｊ４）に関して略対称に形成されており、略第４軸線Ｊ４上に重心が位置している。手首部１７は、その延びる方向の中心線（基本姿勢では第４軸線Ｊ４）に関して略対称に形成されており、略その延びる方向の中心線上に重心が位置している。ハンド部１８は、その延びる方向の中心線（第６軸線Ｊ６）に関して略対称に形成されており、略第６軸線Ｊ６上に重心が位置している。
【００６８】
図２に、ロボット１０の制御装置２０及びその周辺構成を示す。
【００６９】
制御装置２０は、ロボット１０の動作を制御するものであり、ＣＰＵ２１（鉛直方向検出手段、予備動作手段）、ＲＯＭ２２、ＲＡＭ２３、駆動回路２５、及び位置検出回路２６等を備えている。これらは互いに電気的に接続されている。ＲＯＭ２２は、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭ２３は、これらのプログラムを実行する際にパラメータの値等を記憶する。ＣＰＵ２１は、これらのＲＯＭ２２やＲＡＭ２３の記憶内容に基づいて、各種プログラムを実行することにより、ロボット１０の動作を制御する。
【００７０】
ロボット１０において、各軸線Ｊ１〜Ｊ６を回転中心軸とした各関節には、それぞれ前段部側にサーボモータ３１が設けられている。ＣＰＵ２１は、駆動回路２５を通じて、各サーボモータ３１の駆動を行う。これにより、各関節において、前段部に設けられたサーボモータ３１の駆動により、後段部がそれぞれ回転動作する。
【００７１】
各サーボモータ３１には、その出力軸を制動する非励磁作動型の電磁ブレーキ３３と、出力軸の回転位置に応じたパルス信号を出力するエンコーダ３２とがそれぞれ設けられている。
【００７２】
各電磁ブレーキ３３は、ばね等の弾性力に基づきサーボモータ３１の出力軸の制動を行い、励磁コイルへの電力供給に基づき出力軸の制動を解除する。ＣＰＵ２１は、駆動回路２５を通じて、各電磁ブレーキ３３の駆動を行う。ＣＰＵ２１は、各電磁ブレーキ３３による出力軸の制動を解除した状態において各サーボモータ３１の駆動を行う。
【００７３】
各エンコーダ３２は、所定パターンに形成された回転子の回転を磁気的又は光学的に検出する検出素子と、その検出素子の信号を処理するＩＣとを有している。位置検出回路２６には、各エンコーダ３２の検出信号がそれぞれ入力される。位置検出回路２６は、各エンコーダ３２の検出信号に基づいて、回転部１３、下アーム１５、第１上アーム１６Ａ、第２上アーム１６Ｂ、手首部１７、及びハンド部１８の各位置（各角度）を検出する。
【００７４】
具体的には、位置検出回路２６は、固定部１２に対する回転部１３の回転角度、回転部１３に対する下アーム１５の回転角度、下アーム１５に対する第１上アーム１６Ａの回転角度、第１上アーム１６Ａに対する第２上アーム１６Ｂの回転角度、第２上アーム１６Ｂに対する手首部１７の回転角度、手首部１７に対するハンド部１８の回転角度をそれぞれ検出し、それら検出した位置情報をＣＰＵ２１に出力する。位置検出回路２６は、上記の各回転角度を０．１°単位で検出可能である。
【００７５】
そして、ＣＰＵ２１は、動作プログラムを実行することにより、位置検出回路２６から入力される位置情報に基づいて、ロボット１０の上記各部の動作をフィードバック制御する。また、ＣＰＵ２１は、後述する鉛直方向検出制御を実行する。
【００７６】
ロボット１０の各関節では、前段部に対する後段部の動作可能範囲がそれぞれ設定されている。この動作可能範囲（ａ範囲、ｂ範囲）は、ＣＰＵ２１（ａ規制手段、ｂ規制手段）による制御においてソフト的に各部の回転を規制するソフトリミットと、機械的（物理的）に各部の回転を規制するメカリミットとによって設定されている。また、各動作可能範囲には、基準位置としての原点位置がそれぞれ設定されている。
【００７７】
具体的には、前段部に対する後段部の動作可能範囲は以下の通りである。すなわち、回転部１３，第２上アーム１６Ｂ，ハンド部１８では、原点位置０°、ソフトリミット位置−１８０．５°，＋１８０．５°、メカリミット位置−１８１．０°，＋１８１．０°である。下アーム１５，第１上アーム１６Ａ，手首部１７では、原点位置０°、ソフトリミット位置−９０．５°，＋９０．５°、メカリミット位置−９１．０°，＋９１．０°である。ＣＰＵ２１は、上記各部が上記ソフトリミット位置に到達した場合には、そのソフトリミット位置を越えて回転しないように各部の回転を規制する。なお、上記各部がそれぞれの原点位置に位置している状態を、各部の基本姿勢としている。
【００７８】
また、ＣＰＵ２１は、第１軸線Ｊ１の一方向を基準方向とした３次元の座標系を設定し、ロボット１０の各関節の角度を座標系の位置データに変換する。そして、第１軸線Ｊ１と鉛直方向との関係、及び各関節の角度や各部（アーム等）の質量に基づいて、各関節に作用する重力トルクを算出する。
【００７９】
ＣＰＵ２１は、ハンド部１８に持った部品を製品に挿入する場合等に、部品を押す力を弱める力制限制御を実行する。このとき、重力に対抗して各部を支持する力が弱くなることから、重力により各部が下がることを抑制するために重力補償制御を実行する。具体的には、上記のように算出した重力トルクと反対向きで大きさの等しいトルクを、ロボット１０の各部の駆動トルクに上乗せする。
【００８０】
こうした重力補償制御では、ロボット１０の基準方向（第１軸線Ｊ１の一方向）と、鉛直方向との関係を正確に把握する必要がある。また、その関係がずれている場合には、その関係を補正する必要がある。
【００８１】
例えば、第１軸線Ｊ１において回転部１３から固定部１２へ向かう方向を基準方向とする。ここで、固定部１２が床に固定されていれば、基準方向と鉛直方向とが一致することとなるが、固定部１２が天井や壁に固定されている場合には、基準方向と鉛直方向とがずれることとなる。
【００８２】
このようにロボット１０の基準方向と鉛直方向との関係がずれている場合に、基準方向が鉛直方向に一致していると想定して重力補償制御を実行すると、重力トルクを適切に補償することができない。例えば、ロボット１０が天井に設置されている場合に、基準方向が鉛直方向に一致している、すなわちロボット１０が床に設置されていると想定して重力補償制御を実行すると、重力トルクと同じ向きのトルクを駆動トルクに上乗せすることとなる。
【００８３】
なお、ロボット１０の固定部１２を床や壁に固定する角度がずれている場合や、床や壁そのものが水平や垂直からずれている場合にも、同様の問題は生じ得る。
【００８４】
次に、ロボット１０の第２上アーム１６Ｂ及び手首部１７を動作させることにより、鉛直方向を検出する制御について説明する。
【００８５】
図３は、この鉛直方向検出制御の処理手順を示すフローチャートである。この処理は、工場へのロボット１０の設置時に、ＣＰＵ２１によって１回のみ実行される。なお、この処理は、ロボット１０の設置箇所が、床、天井、壁のいずれの場合であっても有効であるが、ここではロボット１０が床に設置されている場合を例として説明する。
【００８６】
まず、この処理が開始される前提として、ロボット１０が作動されており、ロボット１０の各関節において、各電磁ブレーキ３３による各サーボモータ３１の出力軸の制動が解除され、各サーボモータ３１が駆動状態（通電状態）となっている。この状態では、各サーボモータ３１の駆動トルクにより、各関節の角度が目標角度に維持されている。
【００８７】
そして、手首部１７の現在の角度が０．０°よりも小さいか否か判定し（Ｓ１１）、０．０°よりも小さいと判定された場合には（Ｓ１１：ＹＥＳ）、第５軸線Ｊ５を中心として手首部１７を−９０°．０まで回転させ、０．０°よりも小さくないと判定された場合には（Ｓ１１：ＮＯ）、第５軸線Ｊ５を中心として手首部１７を＋９０．０°まで回転させる。すなわち、手首部１７の延びる方向が第４軸線Ｊ４の方向とずれるように、詳しくは、手首部１７の動作可能範囲内において手首部１７の先端が第４軸線Ｊ４から最も離れる位置（−９０．０°、＋９０．０°）まで、第５軸線Ｊ５を中心として手首部１７を回転させる。その際、手首部１７の現在の位置に基づいて、手首部１７の先端が第４軸線Ｊ４から最も離れる２つの位置のうち、近い方の位置へ手首部１７を回転させる。
【００８８】
なお、Ｓ１１〜Ｓ１３の処理が、予備動作手段としての処理に相当する。
【００８９】
ここで、Ｓ１１〜Ｓ１３の処理の作用効果を説明する。図１に示すように、手首部１７の延びる方向と第４軸線Ｊ４の方向とが一致している場合には（基本姿勢）、ハンド部１８、手首部１７、及び第２上アーム１６Ｂを併せた重心は、略第４軸線Ｊ４上に位置している。したがって、この状態において、第４軸線Ｊ４を中心として、第２上アーム１６Ｂを重力により自由回転させたとしても、第２上アーム１６Ｂは回転しない。
【００９０】
これに対して、手首部１７の延びる方向を第４軸線Ｊ４の方向からずらすように、第５軸線Ｊ５を中心として手首部１７を回転させるため、ハンド部１８、手首部１７、及び第２上アーム１６Ｂを併せた重心を、第４軸線Ｊ４上から手首部１７の先端側（ハンド部１８側）へずらすことができる。このとき、手首部１７の先端を第４軸線Ｊ４から離すほど、第４軸線Ｊ４上から手首部１７の先端側へ上記重心をずらすことができる。
【００９１】
この点、手首部１７の動作可能範囲（−９０．５°〜＋９０．５°）内において手首部１７の先端が第４軸線Ｊ４から最も離れる位置（−９０．０°位置、＋９０．０°位置）まで、第５軸線Ｊ５を中心として手首部１７を回転させるため、第４軸線Ｊ４上から手首部１７の先端側へ上記重心を最大限ずらすことができる。なお、その際に、手首部１７の現在の位置に基づいて、手首部１７の先端が第４軸線Ｊ４から最も離れる２つの位置のうち、近い方の位置へ手首部１７を回転させるため、手首部１７の回転に要する時間を短縮することができる。
【００９２】
図３に戻り、続いて、第４軸線Ｊ４を中心として第２上アーム１６Ｂを重力により自由回転させる（Ｓ１４）。具体的には、第４軸線Ｊ４を回転中心軸とする関節において、電磁ブレーキ３３によるサーボモータ３１の出力軸の制動を解除するとともに、サーボモータ３１への通電を停止させる、すなわちサーボモータ３１の駆動を停止させる。
【００９３】
続いて、第２上アーム１６Ｂが所定時間停止状態で維持されたか否か判定する（Ｓ１５）。具体的には、位置検出回路２６により検出される第２上アーム１６Ｂの角度が、数秒間（詳しくは５秒間）変化していないか否か判定する。第２上アーム１６Ｂが所定時間停止状態で維持されていないと判定された場合には（Ｓ１５：ＮＯ）、第２上アーム１６Ｂが所定時間停止状態で維持されたと判定されるまで待つ。
【００９４】
そして、第２上アーム１６Ｂが所定時間停止状態で維持されたと判定された場合には（Ｓ１５：ＹＥＳ）、第２上アーム１６Ｂがソフトリミット位置に到達したか否か判定する（Ｓ１６）。具体的には、位置検出回路２６により検出される第２上アーム１６Ｂの角度が、−１８０．５°又は＋１８０．５°となったか否か判定する。なお、上述したように、ＣＰＵ２１は、第２上アーム１６Ｂがソフトリミット位置に到達した場合には、そのソフトリミット位置を越えて回転しないように第２上アーム１６Ｂの回転を規制する。
【００９５】
上記判定において、第２上アーム１６Ｂがソフトリミット位置に到達していないと判定された場合には（Ｓ１６：ＮＯ）、第２上アーム１６Ｂを駆動状態（通電状態）にして、第１上アーム１６Ａに対する第２上アーム１６Ｂの現在の角度を維持する（Ｓ１７）。なおこのとき、第２上アーム１６Ｂを駆動するサーボモータ３１の出力軸は、電磁ブレーキ３３による制動が解除された状態となっているが、この出力軸を電磁ブレーキ３３により制動した状態にしてもよい。
【００９６】
一方、上記判定において、第２上アーム１６Ｂがソフトリミット位置に到達していると判定された場合には（Ｓ１６：ＹＥＳ）、１Ａに進み、図４に示すように、第２上アーム１６Ｂを駆動状態（通電状態）にして（Ｓ３１）、そのソフトリミット位置から、０．０°を越えた位置（０．０°±α°）まで第２上アーム１６Ｂを回転させる（Ｓ３２）。なお、０．０＜α＜１８０．０であり、例えばαは２．０〜３．０に設定される。すなわち、第４軸線Ｊ４を中心として、ソフトリミット位置（−１８０．５°又は＋１８０．５°）から、動作可能範囲（−１８０．５°〜＋１８０．５°）の少なくとも中央位置（０．０°）まで第２上アーム１６Ｂを回転させる。その後、１Ｂに進み、図３に示すように、再度Ｓ１４の処理から実行する。
【００９７】
ここで、Ｓ１４〜Ｓ１６、及びＳ３１〜Ｓ３２の処理の作用効果を説明する。上述したＳ１１〜Ｓ１３の処理によって、ハンド部１８、手首部１７、及び第２上アーム１６Ｂを併せた重心は、第４軸線Ｊ４上から手首部１７の先端側へずらされた状態となっている。この状態において、第４軸線Ｊ４を中心として第２上アーム１６Ｂを重力により自由回転させると、第４軸線Ｊ４を中心とした周方向において最下端に上記重心が位置するように、第２上アーム１６Ｂが回転しようとする。すなわち、第４軸線Ｊ４を中心とした周方向において、その最下端に手首部１７の先端が位置するように、第２上アーム１６Ｂが回転しようとする。
【００９８】
このとき、ロボット１０の設置状態により、第２軸線Ｊ２及び第３軸線Ｊ３が水平方向から傾いている場合がある。具体的には、例えばロボット１０が壁に設置されている場合や、第１軸線Ｊ１が鉛直方向から傾いている場合等である。図５は、第２上アーム１６Ｂ及び手首部１７を、第４軸線Ｊ４の方向からみた模式図である。
【００９９】
同図に示すように、第４軸線Ｊ４を中心として第２上アーム１６Ｂを自由回転させる処理の開始時に、第２上アーム１６Ｂの角度θが−１８０．５°≦θ＜−３０．０°である場合には、第２上アーム１６Ｂの自由回転により実線で示す状態となる。この場合には、第４軸線Ｊ４を中心とした周方向の最下端まで手首部１７の先端が移動するよりも前に、ソフトリミットにより第２上アーム１６Ｂの回転が規制されることとなる。
【０１００】
そして、ソフトリミットにより第２上アーム１６Ｂの回転が規制された場合には、第２上アーム１６Ｂが所定時間停止状態で維持されたと判定され、第２上アーム１６Ｂがソフトリミット位置に到達していると判定される。このため、そのソフトリミット位置から、二点鎖線で示す０°を越えた位置まで第２上アーム１６Ｂが回転させられる。このとき、第４軸線Ｊ４を中心とした周方向の最上端（−３０．０°位置）を手首部１７の先端が越える位置まで、第４軸線Ｊ４を中心として第２上アーム１６Ｂが回転する。
【０１０１】
その後、再度第４軸線Ｊ４を中心として第２上アーム１６Ｂが重力により自由回転させさられ、破線で示すように、第４軸線Ｊ４を中心とした周方向において、その最下端に手首部１７の先端が位置するように、第２上アーム１６Ｂが回転する。すなわち、第５軸線Ｊ５の方向が水平方向と一致した状態となり、鉛直方向に沿った垂直な平面上で手首部１７が回転する状態となる。
【０１０２】
また、第４軸線Ｊ４を中心として第２上アーム１６Ｂを自由回転させる処理の開始時に、第２上アーム１６Ｂの角度θが−３０°＜θ≦＋１８０．５°である場合には、第２上アーム１６Ｂの自由回転により破線で示す状態となる。すなわち、この場合には、ソフトリミットにより第２上アーム１６Ｂの回転が規制されることなく、重力による第２上アーム１６Ｂの自由回転により、鉛直方向に沿った垂直な平面上で手首部１７が回転する状態となる。
【０１０３】
なお、第４軸線Ｊ４を中心として第２上アーム１６Ｂが自由回転する際には、上記サーボモータ３１や減速機等が回転することとなるため、第２上アーム１６Ｂの回転を抑制するように摩擦力等が生じる。この点、ハンド部１８、手首部１７、及び第２上アーム１６Ｂを併せた重心が、第４軸線Ｊ４上から手首部１７の先端側へ最大限ずらされているため、第２上アーム１６Ｂを回転させるように作用する重力トルクを最大にすることができる。したがって、第２上アーム１６Ｂの回転を抑制する摩擦力等が生じたとしても、より確実に、手首部１７の先端が最下端に位置するように、すなわち鉛直方向に沿った垂直な平面上で手首部１７が回転する状態となるように、第４軸線Ｊ４を中心として第２上アーム１６Ｂを回転させることができる。
【０１０４】
ちなみに、上述したように、第４軸線Ｊ４を回転中心軸とした関節には、第４軸線Ｊ４を中心として第２上アーム１６Ｂを滑らかに回転可能とする軸受けが設けられている。同様に、第５軸線Ｊ５を回転中心軸とした関節には、第５軸線Ｊ５を中心として手首部１７を滑らかに回転可能とする軸受けが設けられている。
【０１０５】
図３に戻り、第１上アーム１６Ａに対する第２上アーム１６Ｂの現在の角度を維持した後（Ｓ１７）、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させる（Ｓ１８）。具体的には、第５軸線Ｊ５を回転中心軸とする関節において、電磁ブレーキ３３によるサーボモータ３１の出力軸の制動を解除するとともに、サーボモータ３１への通電を停止させる、すなわちサーボモータ３１の駆動を停止させる。
【０１０６】
続いて、手首部１７が所定時間停止状態で維持されたか否か判定する（Ｓ１９）。具体的には、位置検出回路２６により検出される手首部１７の角度が、数秒間（詳しくは５秒間）変化していないか否か判定する。手首部１７が所定時間停止状態で維持されていないと判定された場合には（Ｓ１９：ＮＯ）、手首部１７が所定時間停止状態で維持されたと判定されるまで待つ。
【０１０７】
そして、手首部１７が所定時間停止状態で維持されたと判定された場合には（Ｓ１９：ＹＥＳ）、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた際に、手首部１７が回転したか否か判定する（Ｓ２０）。具体的には、位置検出回路２６により検出される手首部１７の角度が、−９０．０°又は＋９０．０°から変化したか否か判定する。このとき、位置検出回路２６は、手首部１７の回転角度を０．１°単位で検出可能であり、例えば手首部１７の角度が＋９０．０°から＋９０．５°（ソフトリミット位置）まで変化した場合も、手首部１７が回転したと判定する。
【０１０８】
上記判定において、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた際に、手首部１７が回転していないと判定された場合には（Ｓ２０：ＮＯ）、２Ａに進み、図６に示すように、手首部１７を駆動状態（通電状態）にして（Ｓ４１）、現在の角度（−９０．０°又は＋９０．０°）から＋β°又は−β°だけ手首部１７を回転させる（Ｓ４２）。このとき、手首部１７の現在の角度が−９０．０°であれば、手首部１７を＋β°回転させ、手首部１７の現在の角度が＋９０．０°であれば、手首部１７を−β°回転させる。なお、−９０．０＜β＜＋９０．０であり、例えばβは３０．０に設定される。すなわち、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた時に、手首部１７が停止状態のまま維持された場合には、第５軸線Ｊ５を中心として手首部１７を回転させることが可能な範囲（−９０．５°〜＋９０．５°）の中央（０．０°）側へ、所定角度βだけ手首部１７部を回転させる。
【０１０９】
その後、Ｓ１８〜Ｓ１９と同様にして、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させ（Ｓ４３）、手首部１７が所定時間停止状態で維持されたか否か判定する（Ｓ４４）。そして、手首部１７が所定時間停止状態で維持されたと判定された場合には（Ｓ４４：ＹＥＳ）、２Ｂへ進み、図３に示すように、その停止状態において手首部１７の先端が指す方向を鉛直方向として検出する（Ｓ２２）。具体的には、手首部１７の現在の角度方向において手首部１７の先端側へ向かう方向、すなわち第５軸線Ｊ５から手首部１７の現在の角度へ向かう方向を鉛直方向として検出する。なおこのとき、手首部１７を駆動するサーボモータ３１の出力軸は、電磁ブレーキ３３による制動が解除された状態となっているが、この出力軸を電磁ブレーキ３３により制動した状態にしてもよい。その後、この一連の処理を終了する（ＥＮＤ）。
【０１１０】
ここで、Ｓ１８〜Ｓ２０、Ｓ４１〜Ｓ４４、及びＳ２２の処理の作用効果を説明する。上述したＳ１４〜Ｓ１７の処理によって、手首部１７の先端が最下端に位置するように、すなわち鉛直方向に沿った垂直な平面上で手首部１７が回転する状態となるように、第４軸線Ｊ４を中心として第２上アーム１６Ｂが回転した状態となっている。この状態において、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させると、第５軸線Ｊ５を中心とした周方向において最下端に手首部１７の先端が位置するように、手首部１７が回転しようとする。
【０１１１】
図７は、第２上アーム１６Ｂ及び手首部１７を第５軸線Ｊ５の方向からみた模式図である。このとき、同図に実線で示すように、手首部１７の先端が鉛直方向の反対方向（鉛直上方）を指している場合と、破線で示すように、手首部１７の先端が鉛直方向（鉛直下方）を指している場合とのみ、その状態で手首部１７に作用する重力が釣合い、手首部１７が回転しないこととなる。換言すれば、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた際に、手首部１７が回転していないと判定された場合には、手首部１７の先端は鉛直上方又は鉛直下方のいずれかを指す状態となっている。
【０１１２】
そして、例えば実線で示すように、＋９０．０°の角度において手首部１７の先端が鉛直上方を指している状態から、２点鎖線で示すように第５軸線Ｊ５を中心として手首部１７が−３０．０°回転させられる。その後に、手首部１７が重力により自由回転させられ、破線で示すように、手首部１７の先端が鉛直下方を指す角度（−９０．０°）まで手首部１７が回転する。なお、＋９０．０°の角度において手首部１７の先端が鉛直下方を指している場合には、第５軸線Ｊ５を中心として手首部１７が−３０．０°回転させられ、手首部１７の先端が上昇する。その後に、手首部１７が重力により自由回転させられ、手首部１７の先端が下降して、鉛直下方を指す角度（＋９０．０°）まで手首部１７が回転する。
【０１１３】
したがって、その後に、手首部１７が所定時間停止状態で維持されたと判定された場合に、その停止状態において手首部１７の先端が指す方向を鉛直方向として検出することができる。
【０１１４】
図３に戻り、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた際に、手首部１７が回転したと判定された場合には（Ｓ２０：ＹＥＳ）、手首部１７がソフトリミット位置に到達したか否か判定する（Ｓ２１）。具体的には、位置検出回路２６により検出される手首部１７の角度が、−９０．５°又は＋９０．５°となったか否か判定する。なお、上述したように、ＣＰＵ２１は、手首部１７がソフトリミット位置に到達した場合には、そのソフトリミット位置を越えて回転しないように手首部１７の回転を規制する。
【０１１５】
上記判定において、手首部１７がソフトリミット位置に到達していないと判定された場合には（Ｓ２１：ＮＯ）、上述したように、その停止状態において手首部１７の先端が指す方向を鉛直方向として検出し（Ｓ２２）、この一連の処理を終了する（ＥＮＤ）。
【０１１６】
ここで、Ｓ２０〜Ｓ２２の処理の作用効果を説明する。上述したように、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させると、第５軸線Ｊ５を中心とした周方向において最下端に手首部１７の先端が位置するように、手首部１７が回転しようとする。このため、手首部１７が回転したと判定され、且つ手首部１７がソフトリミット位置に到達していないと判定された場合には、手首部１７の先端が鉛直方向を指す角度まで、手首部１７が回転した状態となっている。
【０１１７】
図８は、第２上アーム１６Ｂ及び手首部１７を、第５軸線Ｊ５の方向からみた模式図である。同図に示すように、ロボット１０の動作状態や設置状態により、第２上アーム１６Ｂの先端が下方へ傾斜している場合がある。具体的には、第２軸線Ｊ２を中心として下アーム１５が回転している場合や、第３軸線Ｊ３を中心として上アーム１６が回転している場合、ロボット１０が壁に設置されており、第１軸線Ｊ１を中心として回転部１３が回転している場合等である。
【０１１８】
このように、第２上アーム１６Ｂの先端が下方へ傾斜していた場合には、第５軸線Ｊ５を中心として手首部１７が重力により自由回転させられた際に、手首部１７は実線で示す−９０．０°の角度から破線で示す角度まで回転することとなる。なお、２点鎖線で示すように＋９０．０°の角度から、手首部１７が重力により自由回転させられた場合も、手首部１７は破線で示す角度まで回転することとなる。したがって、手首部１７の先端が指す方向を鉛直方向として検出することができる。
【０１１９】
図３に戻り、手首部１７がソフトリミット位置に到達したと判定された場合には（Ｓ２１：ＹＥＳ）、３Ａに進み、図９に示すように、初期設定としてｘ＝０、ｙ＝１とする（Ｓ５１）。続いて、手首部１７を駆動状態（通電状態）にして（Ｓ５１）、現在の角度（−９０．５°又は＋９０．５°）からｘ°まで手首部１７を回転させ（Ｓ５３）、手首部１７が所定時間停止状態で維持されたか否か判定する（Ｓ５５）。
【０１２０】
そして、手首部１７が所定時間停止状態で維持されていないと判定された場合には（Ｓ５５：ＮＯ）、手首部１７が正方向へ回転したか否か判定する（Ｓ５６）。具体的には、位置検出回路２６により検出される第２上アーム１６Ｂの角度が増加したか否か判定する。
【０１２１】
この判定において、手首部１７が正方向へ回転していないと判定された場合には（Ｓ５６：ＮＯ）、ｘ＝ｘ＋９０／２ｙとし、手首部１７が正方向へ回転したと判定された場合には（Ｓ５６：ＹＥＳ）、ｘ＝ｘ−９０／２ｙとする。そして、ｙ＝ｙ＋１とする（Ｓ５９）。すなわち、次回Ｓ５３の処理において、重力により自由回転させられた方向と反対方向へ、手首部１７が特定角度回転させられるようにｘの値を更新する。その際に、２分探索法を用いて、手首部１７を重力により自由回転させる時に、手首部１７が停止状態のまま維持される角度（手首部１７の先端が鉛直上方を指す角度）を探索する。
【０１２２】
そして、手首部１７が所定時間停止状態で維持されと判定された場合には（Ｓ５５：ＹＥＳ）、その停止状態において手首部１７の先端が指す方向の反対方向を鉛直方向（鉛直下方）として検出し（Ｓ２２）、この一連の処理を終了する（ＥＮＤ）。
【０１２３】
ここで、Ｓ５１〜Ｓ６０の処理の作用効果を説明する。上述したように、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させると、第５軸線Ｊ５を中心とした周方向において最下端に手首部１７の先端が位置するように、手首部１７が回転しようとする。
【０１２４】
このとき、上述したように、ロボット１０の動作状態や設置状態により、第２上アーム１６Ｂの先端が上方へ傾斜している場合がある。図１０は、第２上アーム１６Ｂ及び手首部１７を、第５軸線Ｊ５の方向からみた模式図である。
【０１２５】
例えば、同図に実線で示すように、第２上アーム１６Ｂの先端が上方へ傾斜していた場合には、第５軸線Ｊ５を中心として手首部１７が重力により自由回転させられた際に、手首部１７は実線で示す−９０．５°の角度まで回転して、ソフトリミットによりその回転が規制されることとなる。
【０１２６】
そして、矢印Ｍ１で示すように、第５軸線Ｊ５を中心として手首部１７が、二点差線で示す０．０°まで回転させられる。その後に、矢印Ｍ２で示すように、手首部１７が重力により自由回転させられ、手首部１７が負方向へ回転する。このため、ｘ＝０＋９０／２１に更新され、矢印Ｍ３で示す位置まで手首部１７が回転させられる。その後に、矢印Ｍ４で示すように、手首部１７が重力により自由回転させられ、手首部１７が負方向へ回転する。したがって、ｘ＝４５＋９０／２２に更新される。
【０１２７】
このように２分探索法を用いた処理を繰返すことにより、破線で示すように手首部１７が垂直な状態となると、手首部１７に作用する重力が釣合う状態となる。このため、手首部１７を重力により自由回転させた時に、手首部１７が回転することなく、停止状態のままで所定時間維持されることとなる。このとき、手首部１７の先端が鉛直上方を指す状態となるため、手首部１７の先端が指す方向の反対方向を鉛直方向（鉛直下方）として検出することができる。なお、手首部１７が＋９０．５°の角度まで回転して、ソフトリミットによりその回転が規制されていた場合も、同様の処理により鉛直方向を検出することができる。
【０１２８】
上述したように鉛直方向を検出することにより、第１軸線Ｊ１において回転部１３から固定部１２へ向かう方向（基準方向）と、手首部１７の先端が指す方向（鉛直方向）との関係を把握することができる。このため、ロボット１０の基準方向と鉛直方向との関係がずれている場合には、その関係を補正することができる。したがって、ロボット１０の基準方向と鉛直方向との関係、及び各関節の角度や各部（アーム等）の質量に基づいて、各関節に作用する重力トルクを正確に算出することができ、ひいては重力補償制御を適切に実行することができる。
【０１２９】
以上詳述した本実施形態は以下の利点を有する。
【０１３０】
・第４軸線Ｊ４を中心として第２上アーム１６Ｂが重力により自由回転させられた場合には、手首部１７の先端が最下端に位置するように第２上アーム１６Ｂが回転させられる。また、第５軸線Ｊ５を中心として手首部１７が重力により自由回転させられた場合には、手首部１７の先端が最下端に位置するように手首部１７が回転させられる。
【０１３１】
したがって、これらを共に行うことにより、手首部１７の先端が鉛直方向を指すようにすることができる。よって、手首部１７の先端が指す方向を鉛直方向として検出することができ、ひいてはロボット１０の基準方向と鉛直方向との関係を補正することができる。
【０１３２】
・鉛直方向を検出する予備動作として、手首部１７の延びる方向が第４軸線Ｊ４の方向とずれるように、第５軸線Ｊ５を中心として手首部１７が回転させられる。このため、ハンド部１８、手首部１７、及び第２上アーム１６Ｂを併せた重心を、第４軸線Ｊ４上から手首部１７の先端側へ意図的にずらすことができる。
【０１３３】
・さらに、鉛直方向を検出する予備動作として、手首部１７の動作可能範囲（−９０．５°〜＋９０．５°）内において、手首部１７の先端が第４軸線Ｊ４から最も離れる位置（−９０．０°又は＋９０．０°）に手首部１７が回転させられる。このため、第４軸線Ｊ４上から手首部１７の先端側へ、上記重心を最大限ずらすことができる。したがって、第４軸線Ｊ４を中心として第２上アーム１６Ｂを重力により自由回転させた場合に、手首部１７の先端をより確実に最下端に位置させることができる。
【０１３４】
・第４軸線Ｊ４を中心として第２上アーム１６Ｂを重力により自由回転させた際に、ソフトリミットにより第２上アーム１６Ｂの回転が規制された場合には、第４軸線Ｊ４を中心として動作可能範囲（−１８０．５°〜＋１８０．５°）の中央位置（０．０°）を越える位置まで、第２上アーム１６Ｂが回転させられる。このため、第２上アーム１６Ｂの回転が規制された位置と反対回転側に、手首部１７の先端が最下端となる位置が存在する場合には、この中央位置までに手首部１７の先端が最上端を越えることとなる。したがって、その後に再度、第４軸線Ｊ４を中心として第２上アーム１６Ｂを重力により自由回転させた場合には、手首部１７の先端が最下端に位置するように第２上アーム１６Ｂを回転させることができる。
【０１３５】
・第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた際に、ソフトリミットにより手首部１７の回転が規制された場合には、重力により自由回転させられる方向と反対方向へ手首部１７を回転させるとともに、その位置から第５軸線Ｊ５を中心として手首部１７を重力により自由回転させることが繰返される。このため、手首部１７の先端が徐々に上昇するように第５軸線Ｊ５を中心として手首部１７が回転させられ、その都度第５軸線Ｊ５を中心として手首部１７が重力により自由回転させられる。
【０１３６】
そして、手首部１７の先端が最上端に到達した状態では、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた時に、手首部１７が停止状態のまま維持される。よって、手首部１７が停止状態で維持される位置に手首部１７を停止させた場合に、手首部１７の先端が指す方向の反対方向を鉛直方向として検出することができる。
【０１３７】
・第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた時に、手首部１７が回転することなく、停止状態のまま維持された場合には、手首部１７の先端が鉛直方向を指す状態、又は手首部１７の先端が鉛直方向と反対方向を指す状態にある。そのような場合には、第５軸線Ｊ５を中心として手首部１７を回転させることが可能な範囲の中央側へ、所定角度βだけ手首部１７が回転させられる。このため、手首部１７の先端が鉛直方向を指す状態、又は手首部１７の先端が鉛直方向と反対方向を指す状態から、手首部１７の角度がずれることとなる。
【０１３８】
その後に再度、第５軸線Ｊ５を中心として手首部１７が重力により自由回転させられた場合には、手首部１７の先端が鉛直方向を指す状態となる。よって、手首部１７の先端が指す方向を鉛直方向として検出することができる。
【０１３９】
・６軸ロボット１０において、第４軸線Ｊ４を中心として回転する第２上アーム１６Ｂと、第５軸線Ｊ５を中心として回転する手首部１７とを用いて、鉛直方向を検出している。ここで、第２上アーム１６Ｂは第４軸線Ｊ４の方向に延びているため、第４軸線Ｊ４を中心として第２上アーム１６Ｂを回転させた場合に、第２上アーム１６Ｂは第４軸線Ｊ４上で回転することとなり、第２上アーム１６Ｂが周囲に振り回されることがない。また、手首部１７の長さは、下アーム１５や上アーム１６の長さよりも短く設定されている。このため、第５軸線Ｊ５から離れる方向へ延びる手首部１７が、第５軸線Ｊ５を中心として回転させられたとしても、手首部１７が周囲に振り回される範囲は比較的狭くなる。したがって、鉛直方向を検出する際に、ロボット１０が周囲の設備等に接触することを抑制することができる。
【０１４０】
上記実施形態に限定されず、例えば次のように実施することもできる。
【０１４１】
・上記実施形態では、鉛直方向検出制御を、工場へのロボット１０の設置時に１回のみ行うようにしたが、この制御を所定の期間毎に行ってもよい。
【０１４２】
・Ｓ１１〜Ｓ１３において、手首部１７の現在の位置に基づいて、手首部１７の先端が第４軸線Ｊ４から最も離れる２つの位置（±９０．０°）のうち、近い方の位置へ手首部１７を回転させるようにしたが、常に一方の位置（例えば＋９０．０°）へ回転させるようにしてもよい。
【０１４３】
また、手首部１７の先端が第４軸線Ｊ４から最も離れる２つの位置（±９０．０°）に限らず、それらの位置よりも手前の位置（例えば±８０．０°）等へ、手首部１７を回転させてもよい。ただし、ハンド部１８、手首部１７、及び第２上アーム１６Ｂを併せた重心を、第４軸線Ｊ４上から手首部１７のできるだけ先端側へずらすためには、手首部１７の先端が第４軸線Ｊ４から最も離れる２つの位置へ回転させることが望ましい。
【０１４４】
・図３のＳ２１において、手首部１７がソフトリミット位置に到達したと判定された場合に、第３軸線Ｊ３（ｃ軸線）を中心として第２上アーム１６Ｂ（上アーム１６）を回転させ、ソフトリミットによる手首部１７の回転規制が解除された場合に、手首部１７の先端が指す方向を鉛直方向として検出することもできる。
【０１４５】
すなわち、図１１に実線で示すように、手首部１７を重力により自由回転させた際にソフトリミット位置（−９０．５°）で回転が規制された場合に、第３軸線Ｊ３を中心として第２上アーム１６Ｂを回転させることにより、水平面に対する第２上アーム１６Ｂの傾きを変更することができる。ここで、上述したように、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた際に、ソフトリミットにより手首部１７の回転が規制されていない場合には、手首部１７の先端が最下端に位置していることとなる。
【０１４６】
したがって、第３軸線Ｊ３を中心として第２上アーム１６Ｂを回転させ、ソフトリミットによる手首部１７の回転規制が解除された場合には、手首部１７の先端が鉛直方向を指す状態となる。よって、この場合に手首部１７の先端が指す方向を鉛直方向として検出することができる。なお、第３軸線Ｊ３を中心として第２上アーム１６Ｂを回転させることに代えて、第２軸線Ｊ２を中心として第２上アーム１６Ｂ（下アーム１５及び上アーム１６）を回転させてもよい。
【０１４７】
・手首部１７の延びる方向が第４軸線Ｊ４の方向とずれるように、第５軸線Ｊ５を中心として手首部１７が回転させる予備動作（Ｓ１１〜Ｓ１３）を省略して、第２上アーム１６Ｂの重力による自由回転と、手首部１７の重力による自由回転とを、同時に実行することもできる。こうした構成であっても、第２上アーム１６Ｂ及び手首部１７が、ソフトリミットにより規制されていない場合に、手首部１７の先端が指す方向を鉛直方向として検出することができる。
【０１４８】
・上記実施形態及びその変形例では、各関節における各部の動作範囲を、ＣＰＵ２１を通じた制御（ソフトリミット）により規制したが、このソフトリミットを停止させて、機械的（物理的）な規制（メカリミット）により規制してもよい。そして、Ｓ１６において第２上アーム１６Ｂがメカリミット位置に到達したか否か判定するとともに、Ｓ２１において手首部１７がメカリミット位置に到達したか否か判定するようにしてもよい。
【０１４９】
また、第２上アーム１６Ｂや手首部１７の動作可能範囲が規制されていない場合には、これらが規制位置に到達したか否かの判定処理、及び規制位置に到達していると判定された後の処理を省略することができる。
【０１５０】
・手首部１７や第２上アーム１６Ｂの重心が、それぞれの中心線からずれている場合には、手首部１７及び第２上アーム１６Ｂを重力により自由回転させた際に、手首部１７の先端が指す方向と鉛直方向とがずれるおそれがある。
【０１５１】
この点、図１２に示すように、手首部１７（ハンド部１８）の先端の中央には、所定質量の円柱状の錘４１が取り付けられているといった構成を採用することにより、ハンド部１８、手首部１７、及び錘４１を併せた重心を、手首部１７の中心線Ｃに近付けることができる。また、ハンド部１８、手首部１７、第２上アーム１６Ｂ、及び錘４１を併せた重心を、第５軸線Ｊ５に垂直で第４軸線Ｊ４を含む仮想平面Ｓに近付けることができる。
【０１５２】
したがって、第４軸線Ｊ４を中心として第２上アーム１６Ｂを重力により自由回転させた場合に、手首部１７の先端がより最下端に近付くように第２上アーム１６Ｂを回転させることができる。また、第５軸線Ｊ５を中心として手首部１７を重力により自由回転させた場合に、手首部１７の先端が指す方向をより鉛直方向に近付けることができる。その結果、鉛直方向の検出精度を向上させることができる。
【０１５３】
・第１軸線Ｊ１（ｂ軸線）を中心として回転部１３（ｂ回転部）を回転させ、第３軸線Ｊ３（ａ軸線）を中心として上アーム１６（ａ回転部）を回転させて、鉛直方向検出制御を実行することもできる。ただし、鉛直方向を検出する際に、ロボット１０が周囲の設備等に接触することを抑制するためには、第４軸線Ｊ４を中心として回転する第２上アーム１６Ｂと、第５軸線Ｊ５を中心として回転する手首部１７とを用いることが望ましい。
【０１５４】
要するに、ｂ軸線の方向に延びるとともに、ｂ軸線を中心として回転可能に支持されたｂ回転部と、ｂ軸線に直交するａ軸線から離れる方向へ延びるとともに、ｂ回転部によりａ軸線を中心として回転可能に支持されたａ回転部とを備えるロボットであれば、上述した鉛直方向検出制御を適用することができる。そのようなロボットであれば、６軸ロボットに限らず、５軸ロボットや、４軸ロボットを採用することもできる。
【０１５５】
また、上記実施形態の応用として、ロボット１０の基準方向と検出された鉛直方向との関係を用いて、ロボット座標系（ワールド座標系）とユーザ座標系（ベース座標系）との関係を求めることができる。そして、ロボットの制御装置２０は、一方の座標系での座標を他方の座標系での座標に変換することが可能となる。
【０１５６】
ここで、一般にティーチング等では、ロボット１０を基準として設定しているロボット座標系を用いて、ロボット１０のアーム１６の座標をユーザが入力している。しかしながら、ロボット１０の基台１１が壁や天井、さらには傾いた壁等に配置された場合には、ロボット１０を基準としたロボット座標系を用いてアーム１６の座標を入力することは容易ではない。すなわち、ユーザが床面（地面）を基準としたユーザ座標系ではなく、ロボット１０を基準としたロボット座標系を用いてアーム１６の座標を入力するためには、ユーザ座標系での座標をロボット座標系での座標に変換する必要がある。その結果、意図した座標へロボット１０のアーム１６を動作させることができなかったり、変換作業に手間がかかったりするおそれがある。
【０１５７】
そこで以下に、検出された鉛直方向に基づいて、ロボット１０における所定方向を基準方向とするロボット座標系を変換して、基準方向を鉛直方向に一致させたユーザ座標系を設定する制御の処理手順を説明する。なお、上記基準方向として、第１軸線Ｊ１において回転部１３から固定部１２へ向かう方向を設定しており、この基準方向はロボット座標系におけるｚ軸の負方向である。
【０１５８】
図１３は、ユーザ座標系を設定する処理の手順を示すフローチャートである。この処理は、鉛直方向が検出された後に、ＣＰＵ２１（ユーザ座標系設定手段）によって１回のみ実行される。
【０１５９】
まず、ユーザ座標系を設定する機能が有効であるか否か判定する（Ｓ７１）。この機能の有効及び無効は、ロボット１０の制御装置２０において適宜変更することができる。この判定において、ユーザ座標系を設定する機能が有効でないと判定した場合には（Ｓ７１：ＮＯ）、この一連の処理を終了する（ＥＮＤ）。
【０１６０】
一方、上記判定において、ユーザ座標系を設定する機能が有効であると判定した場合には（Ｓ７１：ＹＥＳ）、検出された鉛直方向とロボット座標系のｚ軸とが平行であるか否か判定する（Ｓ７２）。すなわち、ロボット１０の基台１１が、床又は天井に配置されているか否か判定する。この判定において、検出された鉛直方向とロボット座標系のｚ軸とが平行であると判定した場合には（Ｓ７２：ＹＥＳ）、この一連の処理を終了する（ＥＮＤ）。すなわち、ロボット座標系とユーザ座標系とが分かり易く対応しているため、ロボット座標系の変換を行わない。
【０１６１】
一方、上記判定において、検出された鉛直方向とロボット座標系のｚ軸とが平行でないと判定した場合には（Ｓ７２：ＮＯ）、検出された鉛直方向が前回（初期の鉛直方向）から変化しているか否か判定する（Ｓ７３）。この判定において、検出された鉛直方向が前回から変化していないと判定した場合には（Ｓ７３：ＮＯ）、この一連の処理を終了する（ＥＮＤ）。すなわち、ロボット１０の配置が変更されていないと判定して、ロボット座標系の変換を行わない。
【０１６２】
一方、上記判定において、検出された鉛直方向が前回から変化していると判定した場合には（Ｓ７３：ＹＥＳ）、検出された鉛直方向のベクトルをロボット座標系のｘｙ平面に投影する（Ｓ７４）。具体的には、鉛直方向のベクトルｎｇにおいてｚ軸成分を０にした投影ベクトルｓｇを求める。
【０１６３】
ここで、図１４は、ユーザ座標系を設定する態様を示す模式図である。同図（ａ）に示すように、例えばロボット座標系のｘｙ平面が水平に対して傾いており、上記実施形態において鉛直方向のベクトルｎｇが検出されているものとする。なお、ｘｙ平面は、ロボット１０の基台１１が固定される壁面と一致しており、ユーザはこの壁面に沿った水平方向をユーザ座標系のｘ軸方向と認識している。
【０１６４】
同図（ｂ）に示すように、ロボット座標系のｘ軸方向のベクトルｘとユーザ座標系のｘ軸方向のベクトルｘｕとは一致しておらず、ロボット座標系のｘ軸及びｙ軸が、ユーザ座標系のｘ軸及びｙ軸に対して、ｚ軸を中心として回転してずれた状態となっている。このとき、投影ベクトルｓｇとロボット座標系のｙ軸方向のベクトルｙとのなす角度がθｙであるとすると、ロボット座標系のｘ軸方向のベクトルｘとユーザ座標系のｘ軸方向のベクトルｘｕとのなす角度もθｙとなる。
【０１６５】
図１３に戻り、続いて、上記投影ベクトルｓｇとロボット座標系のｙ軸方向のベクトルｙとのなす角度θｙを算出する（Ｓ７５）。具体的には、投影ベクトルｓｇ及びｙ軸方向のベクトルｙに対して三角関数（逆関数）を適用して角度θｙを算出する。
【０１６６】
続いて、ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸を角度θｙ回転させる（Ｓ７６）。具体的には、三角関数を用いた変換式に基づいて、回転後のｘ軸方向のベクトルｘ、及び回転後のｙ軸方向のベクトルｙを算出する。これにより、図１４（ｃ）に示すように、ロボット座標系のｘ軸方向のベクトルｘが、ユーザ座標系のｘ軸方向のベクトルｘｕに重なる。すなわち、同図（ｄ）に示すように、回転後のｘ軸は上記壁面に沿った水平方向と一致する。
【０１６７】
図１３に戻り、続いて、鉛直方向のベクトルｎｇとｚ軸の負方向のベクトル−ｚとのなす角度θｚを算出する（Ｓ７７）。具体的には、鉛直方向のベクトルｎｇ及びｚ軸の負方向のベクトル−ｚに対して三角関数（逆関数）を適用して角度θｚを算出する。図１４（ｅ）に示すように、角度θｚは、上記壁面（回転後のｘｙ平面）の法線ベクトル（ベクトル−ｚ）と鉛直方向のベクトルｎｇとのなす角度となっている。
【０１６８】
図１３に戻り、続いて、上記回転後のロボット座標系においてｘ軸を中心としてｙ軸及びｚ軸を角度θｚ回転させる（Ｓ７８）。具体的には、三角関数を用いた変換式に基づいて、回転後のｙ軸方向のベクトルｙ、及び回転後のｚ軸方向のベクトルｚを算出する。これにより、図１４（ｆ），（ｇ）に示すように、ロボット座標系のｚ軸の正方向のベクトルｚが、鉛直方向のベクトルｎｇと反対方向（真上方向）となる。また、回転後のｙ軸は、ｘ軸と直交する水平方向となる。
【０１６９】
図１３に戻り、続いて、変換されたロボット座標系をユーザ座標系として設定する（Ｓ７９）。具体的には、上記のように回転されたロボット座標系のｘ軸，ｙ軸，ｚ軸を、それぞれユーザ座標系のｘ軸，ｙ軸，ｚ軸としてユーザ座標系を設定する。その結果、この設定されたユーザ座標系は、ユーザが認識しているユーザ座標系と一致することとなる。そして、この一連の処理を終了する（ＥＮＤ）。
【０１７０】
ここで、ロボット１０の制御装置２０は、ロボット座標系とユーザ座標系との関係を把握することとなるため、一方の座標系での座標を他方の座標系での座標に変換（逆変換）することが可能となる。したがって、ユーザはユーザ座標系を用いてロボット１０のアーム１６の座標を入力することができ、変換作業の手間を減らすことができるとともに、意図した座標へロボット１０のアーム１６を動作させることが容易となる。
【０１７１】
上述したユーザ座標系設定処理は以下の利点を有する。
【０１７２】
・鉛直方向のベクトルｎｇを変換前のロボット座標系のｘｙ平面に投影した投影ベクトルｓｇと、変換前のロボット座標系のｙ軸方向のベクトルｙとを平行にするのに必要な角度θｙだけ、ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸が回転させられる（第１制御）。これにより、ロボット座標系のｘ軸及びｙ軸が、ユーザ座標系のｘ軸及びｙ軸に対して、ｚ軸を中心として回転してずれていることを補正することができる。また、変換前のロボット座標系のｘｙ平面上において投影ベクトルｓｇに垂直なベクトル（回転後のｘ軸方向のベクトルｘ）を中心として、鉛直方向のベクトルｎｇとロボット座標系のｚ軸方向のベクトルｚとを平行にするのに必要な角度だけ、ロボット座標系のｘ軸，ｙ軸，ｚ軸が回転させられる（第２制御）。これにより、ロボット座標系のｚ軸が、ユーザ座標系のｚ軸に対して傾いていることを補正することができる。その結果、ロボット座標系のｘ軸，ｙ軸，ｚ軸を、それぞれユーザ座標系のｘ軸，ｙ軸，ｚ軸に変換することができる。なお、上記第１制御及び第２制御は、いずれを先に実行してもよい。
【０１７３】
・鉛直方向のベクトルｎｇをロボット座標系のｘｙ平面に投影した投影ベクトルｓｇとロボット座標系のｙ軸方向のベクトルｙとを平行にするように、ロボット座標系においてｚ軸を中心としてｘ軸及びｙ軸が回転させられる。これにより、ロボット座標系のｘ軸が、ユーザ座標系のｘ軸、すなわちロボット１０の基台１１を固定した壁面に沿った水平方向と一致する。その後、鉛直方向のベクトルｎｇと回転後のロボット座標系のｚ軸方向のベクトルｚとを平行にするように、回転後のロボット座標系においてｘ軸を中心としてｙ軸及びｚ軸が回転させられる。これにより、ロボット座標系のｚ軸が、ユーザ座標系のｚ軸に対して傾いていることを補正することができる。
【符号の説明】
【０１７４】
１０…ロボット、１６…上アーム、１６Ｂ…第２上アーム（ｂ回転部）、１７…手首部（ａ回転部）、２０…制御装置、２１…ＣＰＵ（鉛直方向検出手段、予備動作手段、ユーザ座標系設定手段）。
【図１】
2011212835.tif 000003
【図２】
2011212835.tif 000004
【図３】
2011212835.tif 000005
【図４】
2011212835.tif 000006
【図５】
2011212835.tif 000007
【図６】
2011212835.tif 000008
【図７】
2011212835.tif 000009
【図８】
2011212835.tif 000010
【図９】
2011212835.tif 000011
【図１０】
2011212835.tif 000012
【図１１】
2011212835.tif 000013
【図１２】
2011212835.tif 000014
【図１３】
2011212835.tif 000015
【図１４】
2011212835.tif 000016