(57)【特許請求の範囲】
【請求項１】
所定の動作命令に応じた駆動プロファイルを生成する駆動プロファイル生成手段と、
駆動源からの駆動力で駆動する駆動部材によって該駆動部材と被駆動部材との間に張架された被張架部材を張架方向へ移動させて該被駆動部材を駆動させる駆動装置に対し、前記駆動プロファイル生成手段が生成した駆動プロファイルに従って前記駆動源を制御することにより、前記被駆動部材を該所定の駆動プロファイルに応じた目標位置に駆動させ、又は、該所定の駆動プロファイルに応じた目標タイミングで該被駆動部材の駆動を開始させる駆動制御を実行する制御手段とを有する駆動制御装置であって、
前記被張架部材の張力を検知する張力検知手段を有し、
前記駆動プロファイル生成手段は、前記張力検知手段が検知した被張架部材の張力が規定値以上であるときは、前記駆動源が駆動を開始してから前記被駆動部材の駆動が開始されるまでの遅れ時間がゼロ又は予め決められた時間である基準駆動プロファイルを前記所定の駆動プロファイルとして生成し、前記張力検知手段が検知した被張架部材の張力が前記規定値を下回るときは、該張力検知手段が検知した該被張架部材の張力に応じた遅れ時間に応じて該基準駆動プロファイルを補正した補正駆動プロファイルを前記所定の駆動プロファイルとして生成することを特徴とする駆動制御装置。
【請求項２】
請求項１に記載の駆動制御装置において、
前記制御手段は、複数の駆動装置のそれぞれに対応する個別の駆動プロファイルに従って各駆動装置の駆動源を制御して前記駆動制御を実行するものであり、
前記張力検知手段は、各駆動装置における被張架部材の張力を個別に検知するものであり、
前記駆動プロファイル生成手段は、少なくとも１つの駆動装置について前記張力検知手段が検知した被張架部材の張力が規定値を下回る場合、前記張力検知手段が検知した被張架部材の張力が規定値を下回る低張力駆動装置よりも前記張力検知手段が検知した被張架部材の張力が高い高張力駆動装置に対する駆動プロファイルとして、該低張力駆動装置の該被張架部材の張力に応じた遅れ時間に応じて該高張力駆動装置の基準駆動プロファイルの駆動開始時点を遅らせた補正駆動プロファイルを生成することを特徴とする駆動制御装置。
【請求項３】
所定の動作命令に応じた駆動プロファイルを生成する駆動プロファイル生成手段と、
駆動源からの駆動力で駆動する駆動部材によって該駆動部材と被駆動部材との間に張架された被張架部材を張架方向へ移動させて該被駆動部材を駆動させる駆動装置に対し、前記駆動プロファイル生成手段が生成した駆動プロファイルに従って前記駆動源を制御することにより、前記被駆動部材を該所定の駆動プロファイルに応じた目標位置に駆動させ、又は、該所定の駆動プロファイルに応じた目標タイミングで該被駆動部材の駆動を開始させる駆動制御を実行する制御手段とを有する駆動制御装置であって、
前記被張架部材の張力を検知する張力検知手段を有し、
前記制御手段は、複数の駆動装置のそれぞれに対応する個別の駆動プロファイルに従って各駆動装置の駆動源を制御して前記駆動制御を実行するものであり、
前記張力検知手段は、各駆動装置における被張架部材の張力を個別に検知するものであり、
前記駆動プロファイル生成手段は、少なくとも１つの駆動装置について前記張力検知手段が検知した被張架部材の張力が規定値を下回る場合、前記張力検知手段が検知した被張架部材の張力が規定値を下回る低張力駆動装置よりも前記張力検知手段が検知した被張架部材の張力が高い高張力駆動装置に対する駆動プロファイルとして、該低張力駆動装置の該被張架部材の張力に応じた遅れ時間に応じて該高張力駆動装置の基準駆動プロファイルの駆動開始時点を遅らせた補正駆動プロファイルを生成することを特徴とする駆動制御装置。
【請求項４】
請求項１乃至３のいずれか１項に記載の駆動制御装置において、
前記補正駆動プロファイルは、その駆動終了時点が前記基準駆動プロファイルの駆動終了時点と一致するように補正されたものであることを特徴とする駆動制御装置。
【請求項５】
請求項１乃至４のいずれか１項に記載の駆動制御装置において、
前記補正駆動プロファイルは、前記基準駆動プロファイルにおける駆動開始時点から目標速度に達するまでの加速期間の終了時点で、当該補正駆動プロファイルにおける目標位置が該基準駆動プロファイルにおける目標位置と一致するように補正されたものであることを特徴とする駆動制御装置。
【請求項６】
請求項５に記載の駆動制御装置において、
前記制御手段は、前記補正駆動プロファイルにおける加速期間中の加速度が閾値以上であるときには、前記駆動源の駆動を開始しないことを特徴とする駆動制御装置。
【請求項７】
請求項１乃至６のいずれか１項に記載の駆動制御装置において、
前記駆動プロファイル生成手段は、所定の張力低下条件が満たされたときに、前記張力検知手段が検知した被張架部材の張力に基づいて前記所定の駆動プロファイルを生成することを特徴とする駆動制御装置。
【請求項８】
請求項７に記載の駆動制御装置において、
前記被張架部材の累積移動距離を計測する計測手段を有し、
前記所定の張力低下条件は、前記計測手段が計測した前記被張架部材の累積移動距離が規定距離以上であるという条件を含むことを特徴とする駆動制御装置。
【請求項９】
請求項７又は８に記載の駆動制御装置において、
当該駆動制御装置の稼働開始からの経過時間を計測する経時手段を有し、
前記所定の張力低下条件は、前記経時手段が計測した経過時間が規定時間以上であるという条件を含むことを特徴とする駆動制御装置。
【請求項１０】
請求項７乃至９のいずれか１項に記載の駆動制御装置において、
前記所定の駆動プロファイルに従って制御された前記駆動源における駆動結果の整定時間を計測する経時手段を有し、
前記所定の張力低下条件は、前記整定時間が規定時間以上であるという条件を含むことを特徴とする駆動制御装置。
【請求項１１】
所定の動作命令に応じた駆動プロファイルを生成する駆動プロファイル生成手段と、
駆動源からの駆動力で駆動する駆動部材によって該駆動部材と被駆動部材との間に張架された被張架部材を張架方向へ移動させて該被駆動部材を駆動させる駆動装置に対し、前記駆動プロファイル生成手段が生成した駆動プロファイルに従って前記駆動源を制御することにより、前記被駆動部材を該所定の駆動プロファイルに応じた目標位置に駆動させ、又は、該所定の駆動プロファイルに応じた目標タイミングで該被駆動部材の駆動を開始させる駆動制御を実行する制御手段とを有する駆動制御装置であって、
前記被張架部材の張力を検知する張力検知手段と、
前記被張架部材の累積移動距離を計測する計測手段とを有し、
前記駆動プロファイル生成手段は、前記計測手段が計測した前記被張架部材の累積移動距離が規定距離以上であるという条件を含む所定の張力低下条件が満たされたときに、前記張力検知手段が検知した被張架部材の張力に基づいて前記所定の駆動プロファイルを生成することを特徴とする駆動制御装置。
【請求項１２】
所定の動作命令に応じた駆動プロファイルを生成する駆動プロファイル生成手段と、
駆動源からの駆動力で駆動する駆動部材によって該駆動部材と被駆動部材との間に張架された被張架部材を張架方向へ移動させて該被駆動部材を駆動させる駆動装置に対し、前記駆動プロファイル生成手段が生成した駆動プロファイルに従って前記駆動源を制御することにより、前記被駆動部材を該所定の駆動プロファイルに応じた目標位置に駆動させ、又は、該所定の駆動プロファイルに応じた目標タイミングで該被駆動部材の駆動を開始させる駆動制御を実行する制御手段とを有する駆動制御装置であって、
前記被張架部材の張力を検知する張力検知手段と、
当該駆動制御装置の稼働開始からの経過時間を計測する経時手段とを有し、
前記駆動プロファイル生成手段は、前記経時手段が計測した経過時間が規定時間以上であるという条件を含む所定の張力低下条件が満たされたときに、前記張力検知手段が検知した被張架部材の張力に基づいて前記所定の駆動プロファイルを生成することを特徴とする駆動制御装置。
【請求項１３】
所定の動作命令に応じた駆動プロファイルを生成する駆動プロファイル生成手段と、
駆動源からの駆動力で駆動する駆動部材によって該駆動部材と被駆動部材との間に張架された被張架部材を張架方向へ移動させて該被駆動部材を駆動させる駆動装置に対し、前記駆動プロファイル生成手段が生成した駆動プロファイルに従って前記駆動源を制御することにより、前記被駆動部材を該所定の駆動プロファイルに応じた目標位置に駆動させ、又は、該所定の駆動プロファイルに応じた目標タイミングで該被駆動部材の駆動を開始させる駆動制御を実行する制御手段とを有する駆動制御装置であって、
前記被張架部材の張力を検知する張力検知手段と、
前記所定の駆動プロファイルに従って制御された前記駆動源における駆動結果の整定時間を計測する経時手段とを有し、
前記駆動プロファイル生成手段は、前記整定時間が規定時間以上であるという条件を含む所定の張力低下条件が満たされたときに、前記張力検知手段が検知した被張架部材の張力に基づいて前記所定の駆動プロファイルを生成することを特徴とする駆動制御装置。
【請求項１４】
請求項７乃至１３のいずれか１項に記載の駆動制御装置において、
前記所定の張力低下条件は、当該駆動制御装置に電源が投入されたという条件を含むことを特徴とする駆動制御装置。
【請求項１５】
請求項１乃至１４のいずれか１項に記載の駆動制御装置において、
前記制御手段は、前記駆動プロファイルに従って制御された前記駆動源又は前記駆動部材の駆動量と該駆動プロファイル上の目標量とのズレを低減するためのフィードバック制御又はフィードフォワード制御を実施して前記駆動制御を実行することを特徴とする駆動制御装置。
【請求項１６】
１又は２以上の関節部と、
前記関節部の動作に連動する被駆動部材と駆動部材との間に張架された被張架部材を介して該駆動部材の駆動力を該被駆動部材へ伝達して該関節部を駆動させる駆動装置と、
前記駆動装置の駆動源を制御して前記関節部の動作を制御する駆動制御装置とを有するマニピュレータ装置であって、
前記駆動制御装置として、請求項１乃至１５のいずれか１項に記載の駆動制御装置を用いることを特徴とするマニピュレータ装置。
【請求項１７】
請求項１６に記載のマニピュレータ装置において、
前記関節部を２以上有し、
前記駆動装置は関節部ごとに設けられており、
前記駆動制御装置として、請求項１乃至３のいずれか１項に記載の駆動制御装置を用い、
前記駆動制御装置における前記駆動プロファイル生成手段は、当該マニピュレータ装置の制御対象箇所を前記所定の動作命令で指示された移動経路に沿って該所定の動作命令で指示された目標位置まで移動させるための各駆動装置の駆動プロファイルを生成する第１処理モードと、前記所定の動作命令で指示された目標位置に基づいて該目標位置までの移動経路を算出し、当該マニピュレータ装置の制御対象箇所を該移動経路に沿って該目標位置まで移動させるための各駆動装置の駆動プロファイルを生成する第２処理モードとを有しており、
前記駆動プロファイル生成手段は、前記第１処理モードでは前記補正駆動プロファイルを生成するが、前記第２処理モードでは前記補正駆動プロファイルを生成せずに前記基準駆動プロファイルを生成することを特徴とするマニピュレータ装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、駆動制御装置及びマニピュレータ装置に関するものである。
【背景技術】
【０００２】
従来、マニピュレータ装置の関節部の駆動装置として、関節部の動作に連動する被駆動プーリ（被駆動部材）に対し、その被駆動プーリと駆動プーリ（駆動部材）との間に張架されたワイヤ（被張架部材）を介して回転駆動力を伝達する駆動装置が知られている。
【０００３】
例えば、特許文献１には、３個の関節部を有するロボットアームを駆動するワイヤ駆動方式のロボットアーム駆動装置が開示されている。特許文献１に開示の駆動装置では、医療、福祉などの分野においてロボットアームが人間と接触しても危害を加えないという理由から、ワイヤ駆動方式を採用している。この駆動装置は、駆動プーリと被駆動プーリとの間に張架されるワイヤが、テンションプーリやテンション調整機構等を介して張架されている。この駆動装置では、稼働中のアームが人間等の障害物に衝突した場合、その検知結果に基づいてテンション調整機構がワイヤのテンションを緩ませるように制御される。これにより、関節部がほぼフリーな状態となり、障害物に沿って関節部が動くことができて、障害物への衝撃を小さくすることができるとしている。
【発明の概要】
【発明が解決しようとする課題】
【０００４】
このようなマニピュレータ装置においては、近年、関節部の動作を正確に制御することが望まれる場面が多くなってきている。具体的には、関節部の動作開始タイミングを正確に制御したり、関節部の動作位置（関節部の回転角度等）を正確に制御したりすることが求められる場面が多くなってきている。マニピュレータ装置の関節部の動作を正確に制御するためには、駆動プーリ等の駆動部材の駆動に正確に追従するようにワイヤ等の被張架部材を移動させることにより、被駆動プーリ等の被駆動部材を駆動部材の駆動に正確に追従するように駆動させることが求められる。
【０００５】
ところが、被張架部材には、関節部に過大な駆動負荷が加わったり経時使用されたりするなどの様々な要因で伸びが発生する場合がある。このような被張架部材の伸びが原因で駆動部材と被駆動部材との間に張架されている被張架部材の張架部分で緩みが発生すると、駆動部材の駆動を開始しても被張架部材の緩みが解消されるまでは被駆動部材が駆動しない。したがって、被張架部材の移動位置が目標位置からズレたり、被張架部材の駆動開始タイミングが遅れたりするなどの不具合が生じる。このような不具合が生じると、マニピュレータ装置の関節部の動作を正確に制御できなくなる。
【課題を解決するための手段】
【０００６】
上述した課題を解決するために、本発明は、所定の動作命令に応じた駆動プロファイルを生成する駆動プロファイル生成手段と、駆動源からの駆動力で駆動する駆動部材によって該駆動部材と被駆動部材との間に張架された被張架部材を張架方向へ移動させて該被駆動部材を駆動させる駆動装置に対し、前記駆動プロファイル生成手段が生成した駆動プロファイルに従って前記駆動源を制御することにより、前記被駆動部材を該所定の駆動プロファイルに応じた目標位置に駆動させ、又は、該所定の駆動プロファイルに応じた目標タイミングで該被駆動部材の駆動を開始させる駆動制御を実行する制御手段とを有する駆動制御装置であって、前記被張架部材の張力を検知する張力検知手段を有し、前記駆動プロファイル生成手段は、前記張力検知手段が検知した被張架部材の張力が規定値以上であるときは、前記駆動源が駆動を開始してから前記被駆動部材の駆動が開始されるまでの遅れ時間がゼロ又は予め決められた時間である基準駆動プロファイルを前記所定の駆動プロファイルとして生成し、前記張力検知手段が検知した被張架部材の張力が前記規定値を下回るときは、該張力検知手段が検知した該被張架部材の張力に応じた遅れ時間に応じて該基準駆動プロファイルを補正した補正駆動プロファイルを前記所定の駆動プロファイルとして生成することを特徴とする。
【発明の効果】
【０００７】
本発明によれば、被張架部材に伸びが発生して駆動部材と被駆動部材との間に張架されている被張架部材の張架部分で緩みが発生しても、目標位置に対する被駆動部材の駆動位置ズレや目標タイミングに対する被駆動部材の駆動開始タイミングのズレによる弊害を軽減することが可能となるという優れた効果が奏される。
【図面の簡単な説明】
【０００８】
【図１】実施形態に係るマニピュレータ装置の一例を示す斜視図である。
【図２】同マニピュレータ装置における第一関節の動作に関連する、第一ワイヤを張架する第一駆動プーリ及び第一従動プーリの構成を示す斜視図である。
【図３】第一関節及び第二関節を動作させるマニピュレータ装置の制御ブロック図である。
【図４】変形例１に係るマニピュレータ装置を示す斜視図である。
【図５】変形例２に係るマニピュレータ装置における第一関節の駆動装置を示す斜視図である。
【図６】変形例２の第一関節の駆動装置を構成する部品の分解図である。
【図７】（ａ）は、変形例１において、第一ワイヤに伸びが発生していないときの第一モータエンコーダの出力信号と第一関節軸エンコーダの出力信号とを比較したグラフである。（ｂ）は、変形例１において、第一ワイヤに伸びが発生しているときの第一モータエンコーダの出力信号と第一関節軸エンコーダの出力信号とを比較したグラフである。
【図８】実施形態における駆動制御部による関節駆動制御に関わる制御ブロック図である。
【図９】張力検知手段の一例を説明するためのマニピュレータ装置の斜視図である。
【図１０】制御例１の関節駆動制御の流れの一例を示すフローチャートである。
【図１１】（ａ）は、第一モータを駆動するための基準駆動プロファイルの一例を示すグラフであり、（ｂ）は、第二モータを駆動するための基準駆動プロファイルの一例を示すグラフである。
【図１２】図１１（ａ）に示す基準駆動プロファイルを制御例１の補正処理によって補正した第一モータの補正駆動プロファイルを示すグラフである。
【図１３】図１１（ｂ）に示す基準駆動プロファイルを制御例１の補正処理によって補正した第二モータの補正駆動プロファイルを示すグラフである。
【図１４】（ａ）は、第二モータの基準駆動プロファイルを示すグラフであり、（ｂ）は、制御例２の補正処理によって補正した第二モータの補正駆動プロファイルを示すグラフである。
【図１５】制御例３の補正処理によって補正した第二モータの補正駆動プロファイルを示すグラフである。
【図１６】実施タイミング例１の制御の流れを示すフローチャートである。
【図１７】実施タイミング例５において、第二モータの駆動速度が、一定の加速度で加速した後、一定の定常速度で駆動し、その後、一定の加速度で減速して停止するという駆動プロファイルに従って第二モータを駆動させたときの第二モータエンコーダの出力結果を示すグラフである。
【図１８】実施タイミング例６において、第二関節軸エンコーダの出力値を実線で示し、第二モータエンコーダの出力値を破線で示したグラフである。
【発明を実施するための形態】
【０００９】
以下、本発明に係る駆動制御装置を、マニピュレータ装置の駆動装置を制御する駆動制御装置に適用した一実施形態について説明する。
図１は、本実施形態に係るマニピュレータ装置の一例を示す斜視図である。
本実施形態のマニピュレータ装置１００は、第一関節１８１および第二関節１８２の２自由度を備えた水平マニピュレータ装置である。このマニピュレータ装置は、固定配置された第一リンク１８３と、第一関節１８１を回転軸として回動する第二リンク１８４と、第二関節１８２を回転軸として回動する第三リンク１８５とを備えている。マニピュレータ装置は、第二リンク１８４及び第三リンク１８５が鉛直方向に延びる回転軸回りに回転するように設置される。第三リンク１８５の自由端部側には、使用目的に応じて、ピッキングハンドやエアーチャックなどのエンドエフェクタが取り付けられる。
【００１０】
第一リンク１８３上には、駆動源である第一モータ１８８が設置されている。第一モータ１８８には、エンコーダ１８９が同軸上に取り付けられており、エンコーダ１８９は、第一モータ１８８の回転動作に応じた回転検知信号を出力する。また、第一リンク１８３上には、第一モータ１８８の回転軸上に取り付けられた第一モータ減速機１９０が設置されており、この第一モータ減速機１９０には駆動部材としての第一駆動プーリ１９４が取り付けられている。第一リンク１８３は、第一関節１８１の箇所に固定軸を有しており、被駆動部材としての第一従動プーリ１９５が第一関節１８１に対して回動自在に取り付けられている。第一駆動プーリ１９４と第一従動プーリ１９５との間には、被張架部材としての第一ワイヤ１８６が張架されている。
【００１１】
図２は、第一関節１８１の動作に関連する、第一ワイヤ１８６を張架する第一駆動プーリ１９４及び第一従動プーリ１９５の構成を示す斜視図である。
第一駆動プーリ１９４及び第一従動プーリ１９５には、第一ワイヤ１８６の巻きつけを案内するための半円断面の溝が備えられている。第一ワイヤ１８６の両端部には、ワイヤ本体部分よりも断面が大きい固定部材１８６Ａ，１８６Ｂがカシメられている。これらの固定部材１８６Ａ，１８６Ｂを第一駆動プーリ１９４に形成された孔に嵌め込むことにより、第一駆動プーリ１９４に対して第一ワイヤ１８６が固定される。なお、他のプーリに対するワイヤの固定方法もこれと同様であるが、プーリに対するワイヤの固定方法はこれに限らず、広く公知の固定方法を採用することができる。
【００１２】
第一モータ１８８による駆動力は、第一モータ減速機１９０を介して第一駆動プーリ１９４に伝達され、第一駆動プーリ１９４が回転駆動する。第一駆動プーリ１９４の回転駆動により第一ワイヤ１８６が第一駆動プーリ１９４に巻き取られ又は第一駆動プーリ１９４から繰り出されることにより第一ワイヤ１８６が移動する。この第一ワイヤ１８６の移動に伴い、第一ワイヤ１８６が巻き付いている第一従動プーリ１９５が第一関節１８１の回りを回動する。第一従動プーリ１９５は、第二リンク１８４の一端部に固定されているので、第一従動プーリ１９５の回動に伴って第二リンク１８４も第一関節１８１の回りを回動する。
【００１３】
次に、第二関節１８２の動作に関連する構成について説明するが、その基本構成は、第一関節１８１と同様である。
第一リンク１８３上には、駆動源である第二モータ１９１が設置されている。第二モータ１９１には、エンコーダ１９２が同軸上に取り付けられており、エンコーダ１９２は、第二モータ１９１の回転動作に応じた回転検知信号を出力する。また、第一リンク１８３上には、第二モータ１９１の回転軸上に取り付けられた第二モータ減速機１９３が設置されており、この第二モータ減速機１９３には駆動部材としての第二駆動プーリ１９６が取り付けられている。
【００１４】
第一リンク１８３上の第一関節１８１の箇所に設けられる固定軸には、中継プーリ１９７が回動自在に取り付けられている。また、第二リンク１８４は、第二関節１８２の箇所に固定軸を有しており、被駆動部材としての第二従動プーリ１９８が第二関節１８２に対して回動自在に取り付けられている。第二駆動プーリ１９６と第二従動プーリ１９８との間には、中継プーリ１９７を介して、被張架部材としての第二ワイヤ１８７が張架されている。
【００１５】
第二モータ１９１による駆動力は、第二モータ減速機１９３を介して第二駆動プーリ１９６に伝達され、第二駆動プーリ１９６が回転駆動する。第二駆動プーリ１９６の回転駆動により第二ワイヤ１８７が第二駆動プーリ１９６に巻き取られ又は第二駆動プーリ１９６から繰り出されることにより第二ワイヤ１８７が移動する。この第二ワイヤ１８７の移動に伴い、第二ワイヤ１８７が巻き付いている中継プーリ１９７を回転させるととともに、第二ワイヤ１８７が巻き付いている第二従動プーリ１９８が第二関節１８２の回りを回動する。第二従動プーリ１９８は、第三リンク１８５の一端部に固定されているので、第二従動プーリ１９８の回動に伴って第三リンク１８５も第二関節１８２の回りを回動する。
【００１６】
図３は、第一関節１８１及び第二関節１８２を動作させるマニピュレータ装置１００の駆動制御装置の制御ブロック図である。
マニピュレータ装置１００は、主に、装置本体１１０、電源部５００、ホストコントローラ３００とから構成されている。例えば、マニピュレータ装置１００の制御に画像情報が必要な場合には、ホストコントローラ３００に画像入力装置４００が接続されることもある。なお、電源部５００、ホストコントローラ３００、画像入力装置４００は、装置本体１１０に対して別体であってもよいが、装置本体１１０内に実装されたスタンドアローン型の構成としてもよい。
【００１７】
次に、マニピュレータ装置１００に動作について説明する。
マニピュレータ装置の動作制御は、演算処理部であるＣＰＵ２１０によって行われる。ホストコントローラ３００とＣＰＵ２１０との間は通信網３１１によって接続されている。この通信網３１１からは、動作モード情報、各関節１８１，１８２の駆動制御等に必要な各種パラメータなどがＣＰＵ２１０へ送信される。また、ＣＰＵ２１０には、各種センサー２２０やスイッチ２２１からの信号も入力され、これらの信号をもとにマニピュレータ装置の稼動領域の制限、緊急停止等の処理が実行される。
【００１８】
マニピュレータ装置の各関節１８１，１８２の動作は、第一モータ１８８及び第二モータ１９１を駆動制御することによって実現される。第一モータ１８８及び第二モータ１９１の駆動情報は、第一モータエンコーダ１８９及び第二モータエンコーダ１９２からの信号が駆動情報検出部２３６，２３８に入力され、移動量、移動速度、移動加速度といった回転情報に変換されて、ＣＰＵ２１０内の駆動制御部２１３に入力される。駆動制御部２１３は、この回転情報及びホストコントローラ３００から指令される目標移動位置情報に基づいて目標駆動プロファイルを生成し、その目標駆動プロファイルに沿って第一モータ１８８及び第二モータ１９１が駆動されるように、モータドライバ２３５へ制御指令を出す。
【００１９】
〔変形例１〕
図４は、本実施形態に係るマニピュレータ装置の一変形例（以下、本変形例を「変形例１」という。）を示す斜視図である。
本変形例１のマニピュレータ装置１００は、上述した実施形態の構成と同様に、第一関節１８１および第二関節１８２の２自由度を備えているが、第二リンク１８４及び第三リンク１８５が水平方向に延びる回転軸回りに回転するように設置された垂直マニピュレータ装置である。
【００２０】
本変形例１においては、上述した実施形態の構成に加え、第一従動プーリ１９５に回転負荷を付与する負荷付与手段としてのトルクリミッタ１７１が第一関節１８１に取り付けられている。このようなトルクリミッタ１７１を設けることで、第一従動プーリ１９５にトルクリミッタ１７１による負荷を超える回転力が加わらない限り、第一従動プーリ１９５は回転せず、第一関節１８１は動作しない。トルクリミッタ１７１による負荷の大きさは、稼働中に作用し得る通常の外力が第一従動プーリ１９５に作用しても第一従動プーリ１９５を回転させない程度の大きさに設定されている。トルクリミッタ１７１は落下防止ブレーキとして機能する。また、本変形例１では、第一モータ１８８として、トルクリミッタ１７１による負荷を超える回転力で第一従動プーリ１９５を回転させるのに十分な駆動トルクが得られるものを用いている。
【００２１】
また、本変形例１においては、第一関節１８１に、回転検知手段としての光学式の第一関節軸エンコーダ１７３が設けられている。第一ワイヤ１８６が十分な張力で張架され、第一ワイヤ１８６の伸びが発生しなければ、第一モータエンコーダ１８９の検知結果と第一関節１８１の回転位置との関係は線形性を有するので、第一モータエンコーダ１８９の検知結果から第一関節１８１の回転位置を精度良く把握できる。しかしながら、第一ワイヤ１８６に伸びが発生し得る状況においては、その線形性が崩れ、第一モータエンコーダ１８９の検知結果から第一関節１８１の回転位置を精度良く把握できなくなるおそれがある。このような場合を想定して、第一関節１８１の回転位置を直接的に検知する第一関節軸エンコーダ１７３を設けている。
【００２２】
同様に、本変形例１では、第二関節１８２にもトルクリミッタ１７２が取り付けられており、第三リンク１８５が第二関節１８２に対して回転する際に負荷が生じるように構成されている。また、本変形例１においては、第二関節１８２にも、回転検知手段としての光学式の第二関節軸エンコーダ１７４が設けられている。
【００２３】
なお、本変形例１では、第一関節１８１や第二関節１８２に落下防止ブレーキとしてトルクリミッタ１７１，１７２を設置しているが、トルクリミッタを第一モータ１８８や第二モータ１９１に設置してもよい。また、トルクリミッタに代えて、電磁ブレーキなどの他の手段を設けても良い。
【００２４】
〔変形例２〕
図５は、本実施形態に係るマニピュレータ装置の他の変形例（以下、本変形例を「変形例２」という。）における第一関節１８１の駆動装置を示す斜視図である。
図６は、本変形例２の第一関節１８１の駆動装置を構成する部品の分解図である。
本変形例２のマニピュレータ装置１００は、前記変形例１と同様に垂直マニピュレータ装置であるが、少なくとも第一関節１８１については、第一ワイヤ１８６に代えて、個別のワイヤからなる一対のワイヤ１８６−１，１８６−２を用いている。具体的には、一対のワイヤ１８６−１，１８６−２は、それぞれ、第一駆動プーリ１９４上に設けられるプーリ部１９４Ａ，１９４Ｂと、第一従動プーリ１９５上に設けられるプーリ部１９５Ａ，１９５Ｂとに巻き付き、張架される。
【００２５】
第一駆動プーリ１９４が図中時計回り方向（第一方向）へ回転駆動すると、一対のワイヤのうちの第一方向駆動用ワイヤ１８６−１が第一駆動プーリ１９４に巻き取られ、これに伴って第一従動プーリ１９５も図中時計回り方向（第一方向）へ回転する。このとき、一対のワイヤのうちの第二方向駆動用ワイヤ１８６−２は、第一従動プーリ１９５に巻き取られつつ、第一駆動プーリ１９４から繰り出される。一方、第一駆動プーリ１９４が図中反時計回り方向（第二方向）へ回転駆動すると、第二方向駆動用ワイヤ１８６−２が第一駆動プーリ１９４に巻き取られ、これに伴って第一従動プーリ１９５も図中反時計回り方向（第二方向）へ回転する。このとき、第一方向駆動用ワイヤ１８６−１は、第一従動プーリ１９５に巻き取られつつ、第一駆動プーリ１９４から繰り出される。
【００２６】
次に、ワイヤの緩みについて説明する。
図７（ａ）は、上述した変形例１において、第一ワイヤ１８６に伸びが発生していないときの第一モータエンコーダ１８９の出力信号と第一関節軸エンコーダ１７３の出力信号とを比較したグラフである。
図７（ｂ）は、上述した変形例１において、第一ワイヤ１８６に伸びが発生しているときの第一モータエンコーダ１８９の出力信号と第一関節軸エンコーダ１７３の出力信号とを比較したグラフである。
なお、以下の説明では、第一関節１８１の駆動装置を例に挙げて説明するが、第二関節１８２についても同様である。
【００２７】
第一ワイヤ１８６は、通常使用の範囲内であれば実質的には伸びが生じない材料、構造のものを用いるのが好ましいが、そのような第一ワイヤ１８６であっても、耐久劣化や突発的に過大な負荷が加わった場合には、微小な伸びが発生し得る。第一ワイヤ１８６に伸びが発生すると、第一ワイヤ１８６の張力（テンション値）が低下してしまう場合がある。
【００２８】
第一ワイヤ１８６の張力が適正な範囲内であれば、図７（ａ）に示すように、第一モータ１８８の駆動開始に遅れることなく、第一関節１８１の動作が開始され、駆動制御部２１３が作成した目標駆動プロファイルに沿って第一関節１８１を遅れなく動作させることができる。しかしながら、第一ワイヤ１８６に伸びが発生して第一ワイヤ１８６が緩んでいると、図７（ｂ）に示すように、第一モータ１８８の駆動開始からΔｔ時間だけ遅れて第一関節１８１の動作が開始される。これは、第一モータ１８８の駆動開始と同時に第一駆動プーリ１９４は回転し始めるが、第一ワイヤ１８６の伸び分（緩み）が解消されて張力が適正範囲内となるまでは第一従動プーリ１９５が移動しないためである。そのため、駆動制御部２１３が作成した目標駆動プロファイルに沿って第一関節１８１を適切に動作させることができない。
【００２９】
また、耐久劣化や突発的に過大な負荷が加わって第一ワイヤ１８６に微小な伸びが発生した場合、第一ワイヤ１８６の伸び分だけ第一関節１８１が動いてしまう場合もある。この場合、第一ワイヤ１８６の張力（テンション値）は適正範囲内であれば、第一モータ１８８の駆動に遅れることなく、第一関節１８１を動作させることが可能である。しかしながら、第一モータ１８８の回転角度（第一駆動プーリ１９４の回転角度）と、第一関節１８１の回動角度（第一従動プーリ１９５の回転角度）との対応関係が崩れるため、第一モータ１８８の回転角度に対応する第一関節１８１の目標角度と第一関節１８１の実際の回動角度との間にズレが生じる。そのため、この場合も、駆動制御部２１３が作成した目標駆動プロファイルに沿って第一関節１８１を適切に動作させることができない。
【００３０】
ただし、本実施形態では、第一ワイヤ１８６が伸びても、トルクリミッタ１７１によって第一関節１８１が回転してしまうことはない。したがって、本実施形態においては、第一ワイヤ１８６が伸びた場合、第一関節１８１が動いてしまうことはなく、第一ワイヤ１８６の張力が低下することになる。よって、以下の説明では、第一ワイヤ１８６が伸びると、第一関節１８１が動かずに第一ワイヤ１８６の張力が低下する場合を前提に説明する。
【００３１】
図８は、ＣＰＵ２１０が果たす駆動制御部２１３による関節駆動制御に関わる制御ブロック図である。
駆動制御部２１３は、主に、駆動プロファイル生成手段としての指令値生成部２１５とコントローラ２１６とから構成され、駆動源（第一モータ１８８、第二モータ１９１）である制御対象の制御量（制御対象の位置、速度、加速度、加速度の微分値など）に基づいてフィードバック制御を実行する。なお、フィードバック制御以外にも、あらかじめ予想される外乱などに基づいてフィードフォワード制御を行っても良い。
【００３２】
指令値生成部２１５は、ホストコントローラ３００から指令される目標移動位置情報、駆動情報検出部２３６，２３８からの駆動情報（現在位置情報等）、各種センサ２２０からの情報などをインプット情報として、コントローラ２１６への指令値を生成する。指令値生成部２１５は、目標移動位置情報や現在位置情報などから、制御対象箇所（本実施形態では、エンドエフェクタが取り付けられる第三リンク１８５の自由端部）の軌道を計算し、その軌道上を当該制御対象箇所が移動していくように、第一モータ１８８及び第二モータ１９１の駆動プロファイルを生成して、各駆動プロファイルに基づく指令値をコントローラ２１６へ出力する。
【００３３】
本実施形態における指令値生成部２１５は、まずは、基準指令値生成部２１５Ａにおいて、ワイヤ１８６，１８７に緩みがなくワイヤ１８６，１８７の張力が適正範囲内であるときの基準駆動プロファイルを生成する。そして、基準指令値生成部２１５Ａは、各基準駆動プロファイルに基づく基準指令値を順次出力していく。
【００３４】
一方で、指令値生成部２１５には、張力検出部２３９で検出されたワイヤ１８６，１８７の張力の検出値が入力される。このワイヤ１８６，１８７の張力の検出値は、指令値生成部２１５の演算部２１５Ｂへ入力される。演算部２１５Ｂは、入力されるワイヤ１８６，１８７の張力が適正範囲から外れているときには所定の補正値を算出して出力する。この場合、基準指令値生成部２１５Ａから出力される基準指令値が演算部２１５Ｂから出力される補正値によって補正されて、コントローラ２１６への指令値が生成される。一方、入力されるワイヤ１８６，１８７の張力が適正範囲内であるときには、補正値を出力しない。この場合、基準指令値生成部２１５Ａから出力される基準指令値がそのままコントローラ２１６への指令値として生成される。
【００３５】
図９は、張力検知手段の一例を説明するためのマニピュレータ装置の斜視図である。
図９に示す例では、前記変形例２のマニピュレータ装置において、第一方向駆動用ワイヤ１８６−１の張架部分に、歪ゲージを用いた張力検出部が設けられている。この張力検出部は、第一方向駆動用ワイヤ１８６−１に対して上方から当接する検知用プーリ１３２と、検知用プーリ１３２の両側で第一方向駆動用ワイヤ１８６−１に対して下方から当接するアイドラプーリ１３３，１３４とを備えている。検知用プーリ１３２は、歪みゲージ１３１に取り付けられている。第一方向駆動用ワイヤ１８６−１の張力が変化すると、第一方向駆動用ワイヤ１８６−１が検知用プーリ１３２を押し上げる力が変化し、これにより、検知用プーリ１３２が取り付けられた歪みゲージ１３１に生じる歪み量が変化する。歪みゲージ１３１から出力される歪み量と第一方向駆動用ワイヤ１８６−１の張力との間には高い相関関係があることから、歪みゲージ１３１から出力値から第一方向駆動用ワイヤ１８６−１の張力を検知することができる。
【００３６】
なお、ここでは、張力検知手段として、歪ゲージを用いた張力検出部を採用した例であるが、これに限られるものではない。例えば、第一モータエンコーダ１８９の信号と第一関節軸エンコーダ１７３の信号とから測定される遅れ時間Δｔから、第一方向駆動用ワイヤ１８６−１の張力を検知（推定）する張力検知手段を採用することもできる。
【００３７】
〔制御例１〕
次に、本実施形態における関節駆動制御における一制御例（本制御例を「制御例１」という。）について説明する。
図１０は、本制御例１の関節駆動制御の流れの一例を示すフローチャートである。
本制御例１において、ＣＰＵ２１０は、ホストコントローラ３００から動作指令を受けたら（Ｓ１のＹｅｓ）、ホストコントローラ３００からの動作指令に含まれる目標移動位置情報等に基づいて制御対象箇所の軌道を計算する（Ｓ２）。そして、算出した軌道上を当該制御対象箇所が移動していくための第一モータ１８８及び第二モータ１９１の基準駆動プロファイルを生成する（Ｓ３）。
【００３８】
また、ＣＰＵ２１０は、張力検出部２３９が検出したワイヤ張力の検出値を取得する（Ｓ４）。いずれのワイヤの張力検出値も規定値を超えている場合には（Ｓ５のＮｏ）、各モータ１８８，１９１の駆動プロファイルとして、前記処理ステップＳ３で生成した基準駆動プロファイルを用い、その駆動プロファイルに従った指令値を各モータ１８８，１９１のモータドライバー２３５，２３７へ出力する（Ｓ８）。
【００３９】
一方、いずれかのワイヤの張力検出値が規定値以下である場合（Ｓ５のＹｅｓ）、例えば、第二ワイヤ１８７に伸びが発生して第二ワイヤ１８７の張力が規定値以下になった場合、まず、検出された第二ワイヤ１８７の張力検出値に基づき、第二モータ１９１の駆動開始から第二従動プーリ１９８の駆動が開始されるまでの遅れ時間Δｔを算出する（Ｓ６）。ワイヤ張力と遅れ時間Δｔとの間には高い相関関係があるため、ワイヤ張力の検出値から遅れ時間Δｔを高精度に算出することが可能である。そして、モータ１８８，１９１についての基準駆動プロファイルを補正する補正処理を実行し（Ｓ７）、その補正駆動プロファイルに従った指令値を各モータ１８８，１９１のモータドライバー２３５，２３７へ出力する（Ｓ８）。
【００４０】
図１１（ａ）は、第一モータ１８８を駆動するための基準駆動プロファイルの一例を示すグラフであり、図１１（ｂ）は、第二モータ１９１を駆動するための基準駆動プロファイルの一例を示すグラフである。
図１２は、図１１（ａ）に示す基準駆動プロファイルを本制御例１の補正処理によって補正した第一モータ１８８の補正駆動プロファイルを示すグラフである。
図１３は、図１１（ｂ）に示す基準駆動プロファイルを本制御例１の補正処理によって補正した第二モータ１９１の補正駆動プロファイルを示すグラフである。
【００４１】
本制御例１において、ワイヤ張力が規定値以下となった第二ワイヤ１８７については、図１１（ｂ）に示す第二モータ１９１の基準駆動プロファイルを、図１３に示す補正駆動プロファイルへ補正する補正処理を実行する。具体的には、処理ステップＳ６で算出した遅れ時間Δｔに基づき、本来であれば（第二ワイヤ１８７の張力が適正範囲内ならば）第二モータ１９１の駆動開始から遅れ時間Δｔが経過するまでに回転していたはずの第二従動プーリ１９８の回転量（以下「不足回転量」という。）を算出する。この不足回転量は、図１３に斜線で示すＳ２ａの面積から概算することができる。
【００４２】
このような遅れ時間Δｔが発生する場合、図１１（ｂ）に示す基準駆動プロファイルで第二モータ１９１を駆動すると、その駆動完了時（時刻Ｔ２Ｅ）における第二従動プーリ１９８の回転位置が目標回転位置からズレてしまう。そのズレ量は、上述した不足回転量に相当するので、本制御例１では、図１３に示すように、第二モータ１９１の基準駆動プロファイルの減速開始時刻Ｔ２ｂを時刻Ｔ２ｂ’まで遅らせた補正駆動プロファイルを生成する。時刻Ｔ２ｂ’は、第二従動プーリ１９８の回転位置が目標回転位置に一致するように不足回転量から算出される。言い換えると、図１３に斜線で示すＳ２ｂの面積がＳ２ａの面積に一致するように時刻Ｔ２ｂ’を算出する。このような補正駆動プロファイルを用いて第二モータ１９１を駆動制御することで、第二ワイヤ１８７に緩みが発生していても、第二従動プーリ１９８を目標回転位置へ駆動させることができ、第二関節を適切に動作させることができる。
【００４３】
一方、ワイヤ張力が規定値を超えている第一ワイヤ１８６については、図１１（ａ）に示す第一モータ１８８の基準駆動プロファイルを、図１２に示す補正駆動プロファイルへ補正する補正処理を実行する。具体的には、第一モータ１８８の基準駆動プロファイルの駆動開始時刻を、処理ステップＳ６で算出した遅れ時間Δｔの分だけ遅らせ、かつ、減速開始時刻Ｔ１ｂを時刻Ｔ１ｂ’まで遅らせた補正駆動プロファイルを生成する。このように駆動開始時刻を遅れ時間Δｔの分だけ遅らせることで、第二従動プーリ１９８の実際の動作開始タイミングに対する第一従動プーリ１９５の動作開始タイミングのズレをなくすことが可能となる。
【００４４】
また、減速開始時刻を時刻Ｔ１ｂ’に遅らせることで、駆動開始時刻を遅れ時間ΔＴの分だけ遅らせたことにより駆動完了時（時刻Ｔ１Ｅ）における第一従動プーリ１９５の回転位置が目標回転位置からズレてしまうのを抑制する。このズレ量は、駆動開始時刻を遅れ時間Δｔの分だけ遅らせなければ回転していたはずの第一従動プーリ１９５の不足回転量に相当する。この不足回転量は、図１２に斜線で示すＳ１ａの面積から概算することができる。時刻Ｔ１ｂ’は、第一従動プーリ１９５の回転位置が目標回転位置に一致するように、この不足回転量から算出される。言い換えると、図１２に斜線で示すＳ１ｂの面積がＳ１ａの面積に一致するように時刻Ｔ１ｂ’を算出する。これにより、駆動開始時刻を遅れ時間ΔＴの分だけ遅らせても、第一従動プーリ１９５を目標回転位置へ駆動させることができる。
【００４５】
以上のように、各モータ１８８，１９１の基準駆動プロファイルを補正し、その補正駆動プロファイルに従って各モータ１８８，１９１を駆動制御することにより、第二関節１８２の動作開始タイミングの遅れに合わせて第一関節１８１の動作開始タイミングも遅れる。これにより、両関節間における動作開始タイミングのズレがなくなる。また、いずれの関節１８１，１８２も目標動作位置まで動作することができる。その結果、制御対象箇所（第三リンク１８５の自由端部）は、動作指令に応じた軌道から大きく外れることなく目標位置まで移動することができる。
【００４６】
なお、関節部が３つ以上あるマニピュレータ装置において、そのうちの２つ以上の関節部において遅れ時間Δｔが発生するような事態が発生した場合には、例えば、遅れ時間Δｔが最も長い関節部に合わせて、他の関節部についての駆動開始時刻を遅らせるようにすればよい。
【００４７】
また、本制御例１では、両関節間の動作開始タイミングのズレと、各関節の動作位置のズレの両方を修正する例であったが、いずれか一方のズレだけを修正するようにしてもよい。例えば、各関節の動作位置が目標位置からズレるのを修正する場合には、ワイヤが緩んでいない第一モータ１８８の駆動プロファイルには図１１（ａ）に示したような基準駆動プロファイルを用い、ワイヤが緩んでいる第二モータ１９１の駆動プロファイルには図１３に示したような補正駆動プロファイルを用いてもよい。また、例えば、両関節間の動作開始タイミングがズレるのを修正する場合には、ワイヤが緩んでいない第一モータ１８８の駆動プロファイルには図１２に示したような補正駆動プロファイルを用い、ワイヤが緩んでいる第二モータ１９１の駆動プロファイルには図１１（ｂ）に示したような基準駆動プロファイルを用いてもよい。
【００４８】
〔制御例２〕
次に、本実施形態における関節駆動制御における他の制御例（本制御例を「制御例２」という。）について説明する。
本制御例２は、ワイヤの緩みが発生した第二モータ１９１の基準駆動プロファイルの補正処理の内容が異なる点を除いて、上述した制御例１と同様であることから、以下、上述した制御例１と異なる点を中心に説明する。
【００４９】
図１４（ａ）は、第二モータ１９１の基準駆動プロファイルを示すグラフであり、図１４（ｂ）は、本制御例２の補正処理によって補正した第二モータ１９１の補正駆動プロファイルを示すグラフである。なお、図１４（ａ）に示す基準駆動プロファイルは、図１１（ｂ）に示した基準駆動プロファイルと同じものである。
本制御例２において、ワイヤ張力が規定値以下となった第二ワイヤ１８７については、図１４（ａ）に示す第二モータ１９１の基準駆動プロファイルを、図１４（ｂ）に示す補正駆動プロファイルへ補正する補正処理を実行する。具体的には、駆動プロファイルの加速完了時を、基準駆動プロファイルの加速完了時である時刻Ｔ２ａから、図１４（ｂ）に示すように時刻Ｔ２ａ’まで早めた補正駆動プロファイルを生成する。
【００５０】
この時刻Ｔ２ａ’は、時刻Ｔ２ａの時点で、第二従動プーリ１９８の回転位置が基準駆動プロファイル上の回転位置に一致するように、遅れ時間Δｔから算出される。言い換えると、図１４（ｂ）に斜線で示すＳ２ｃ’の面積が図１４（ａ）に斜線で示すＳ２ｃの面積と一致するように、時刻Ｔ２ａ’を算出する。このような補正駆動プロファイルを用いて第二モータ１９１を駆動制御することで、第二ワイヤ１８７に緩みが発生していても、早期のうちに（時刻Ｔ２ａの時点で）、第二従動プーリ１９８を基準駆動プロファイル上の回転位置に追従させることが可能となる。
【００５１】
なお、本制御例２では、補正駆動プロファイルにより第二モータ１９１の加速期間における加速度が大きくなるが、この加速度が大きくなりすぎると、第二モータ１９１を適切に駆動させることができず、かえって第二モータ１９１の駆動制御が不適切なものとなるおそれがある。そのため、例えば、補正駆動プロファイル上の加速期間の加速度が所定の閾値を超える場合には、その補正駆動プロファイルに基づく指令値を出力せず、モータを停止状態に維持するなどのエラー処理するようにしてもよい。これにより、マニピュレータ装置の稼働中にモータのトルク不足等による不具合の発生を回避することができる。また、本制御例２に限らず、補正駆動プロファイルにおいて過度な加速度が設定される事態が発生した場合に、加速度と閾値とを比較してエラー処理を行うことができる。なお、ここでは、加速度と閾値とを比較してエラー処理を行うが、速度と閾値とを比較してエラー処理を行ってもよい。
【００５２】
〔制御例３〕
次に、本実施形態における関節駆動制御における更に他の制御例（本制御例を「制御例３」という。）について説明する。
本制御例３は、ワイヤの緩みが発生した第二モータ１９１の基準駆動プロファイルの補正処理の内容が異なる点を除いて、上述した制御例１と同様であることから、以下、上述した制御例１と異なる点を中心に説明する。
【００５３】
図１５は、本制御例３の補正処理によって補正した第二モータ１９１の補正駆動プロファイルを示すグラフである。なお、第二モータ１９１の基準駆動プロファイルは、図１１（ｂ）に示した基準駆動プロファイルと同じものである。
本制御例３において、ワイヤ張力が規定値以下となった第二ワイヤ１８７については、図１１（ｂ）に示す第二モータ１９１の基準駆動プロファイルを、図１５に示す補正駆動プロファイルへ補正する補正処理を実行する。具体的には、駆動プロファイルの定常速度を、基準駆動プロファイルの定常速度Ｖ２から、図１５に示すように定常速度Ｖ２’まで速めた補正駆動プロファイルを生成する。
【００５４】
この定常速度Ｖ２’は、減速開始時刻Ｔ２ｂの時点で、第二従動プーリ１９８の回転位置が基準駆動プロファイル上の回転位置に一致するように、遅れ時間Δｔから算出される。言い換えると、図１５において斜線で示すＳ２ｄ’の面積が斜線で示すＳ２ｄの面積と一致するように、定常速度Ｖ２’を算出する。このような補正駆動プロファイルを用いて第二モータ１９１を駆動制御することで、第二ワイヤ１８７に緩みが発生していても、時刻Ｔ２ｂの時点で、第二従動プーリ１９８を基準駆動プロファイル上の回転位置に追従させることが可能となる。
【００５５】
上述した制御例２や制御例３によれば、第二従動プーリ１９８の駆動完了時が基準駆動プロファイル上の駆動完了時（時刻Ｔ２Ｅ）と一致するので、第二関節１８２の動作完了時刻を遵守することができる。なお、第一モータ１８８については、上述した制御例１の補正駆動プロファイルを用いるため、第一従動プーリ１９５の駆動完了時（時刻Ｔ１Ｅ’）は基準駆動プロファイル上の駆動完了時（時刻Ｔ１Ｅ）からズレることになる。第一従動プーリ１９５の駆動完了時も基準駆動プロファイル上の駆動完了時（時刻Ｔ１Ｅ）に一致させる場合には、第一モータ１８８の補正駆動プロファイルも、駆動完了時が基準駆動プロファイル上の駆動完了時と一致するように、加速完了時時刻を早めたり、定常速度を速めたりしてもよい。
【００５６】
また、以上の各制御例では、第二ワイヤ１８７が緩んだ状態のまま、各モータ１８８，１９１の駆動を駆動プロファイルに従って開始させる例であったが、駆動プロファイルに従って駆動を開始する前に、緩んだ状態の第二ワイヤ１８７の張力を適正範囲内に復帰させる張力復帰処理を事前に実施し、その張力復帰処理に要した時間を遅れ時間Δｔとして、第二モータ１９１の基準駆動プロファイルを補正してもよい。このときの第二モータ１９１の補正駆動プロファイルとしては、例えば、図１２に示した第一モータ１８８の補正駆動プロファイルと同様の補正駆動プロファイルを用いることができる。
【００５７】
このときの張力復帰処理としては、例えば、第二従動プーリ１９８が動かないように第二モータ１９１を駆動し、第二ワイヤ１８７の張力検出値が適正範囲内になったら第二モータ１９１の駆動を停止させるという処理が挙げられる。また、第二モータ１９１を駆動させずに、第二ワイヤ１８７の張力を変更する張力変更手段を設け、その張力変更手段を用いて第二ワイヤ１８７の張力を適正範囲内に復帰させてもよい。
【００５８】
〔実施タイミング例１〕
次に、補正駆動プロファイルを生成する処理（基準駆動プロファイルの補正処理）を実施するタイミングの一例（本例を「実施タイミング例１」という。）について説明する。
本実施形態におけるマニピュレータ装置は、その動作モードとして、第１処理モードとしてのＣＰモードと第２処理モードとしてのＰＴＰモードを備えている。
【００５９】
ＣＰモードは、その制御対象箇所を所定の動作命令で指示された移動経路に沿って所定の動作命令で指示された目標位置まで移動させる動作モードである。すなわち、ＣＰモードの動作命令では、当該制御対象箇所の移動先の目標位置や必要に応じて姿勢が指定されているだけでなく、現在位置からの目標位置までの移動経路についても指定されている。一方、ＰＴＰモードは、その制御対象箇所を所定の動作命令で指示された目標位置まで移動させれば、その移動経路には任意である動作モードである。すなわち、ＰＴＰモードの動作命令では、当該制御対象箇所の移動先の目標位置や必要に応じて姿勢が指定され、現在位置からの目標位置までの移動経路については指定されていない。
【００６０】
ＰＴＰモードにおいては、移動経路が任意であるため、第二ワイヤに緩みがあって遅れ時間Δｔが発生している場合において、基準駆動プロファイルに従って各モータ１８８，１９１を駆動しても、動作指令に基づく動作を適正に行うことが可能である。具体的には、例えば、各関節１８１，１８２に上述した第一関節軸エンコーダ１７３や第二関節軸エンコーダ１７４を設け、これらのエンコーダの出力値がそれぞれ目標値に一致した時点で各モータ１８８，１９１を停止させるという制御を実施すればよい。
【００６１】
これに対し、ＣＰモードの場合には、このようなＰＴＰモードと同じ制御を実施すると、制御対象箇所の移動経路が動作指令で指定された移動経路から大きく外れたものとなる。そのため、各モータ１８８，１９１の駆動プロファイルを補正して、制御対象箇所の移動経路が動作指令で指定された移動経路から大きく外れないようにする補正処理が必要となる。
【００６２】
図１６は、本実施タイミング例１の制御の流れを示すフローチャートである。
本実施タイミング例１において、ＣＰＵ２１０は、ホストコントローラ３００から動作指令を受けたら（Ｓ１のＹｅｓ）、その動作指令がＣＰモードであるか否かを判断する（Ｓ１１）。このとき、ＣＰモードであると判断された場合には（Ｓ１１のＹｅｓ）、上述した各制御例１〜３の関節駆動制御を実施する（Ｓ２〜Ｓ８）。これにより、ワイヤに緩みが発生していても、制御対象箇所の移動経路が動作指令で指定された移動経路から大きく外れないように、各モータ１８８，１９１の基準駆動プロファイルの補正処理が実行され、動作指令に従った適切な動作が実現される。
【００６３】
一方、ＰＴＰモードであると判断された場合（Ｓ１１のＮｏ）、制御対象箇所が動作指令によって指定される目標位置へ移動するための基準駆動プロファイルを生成する（Ｓ１２）。そして、この基準駆動プロファイルに従った指令値を各モータ１８８，１９１のモータドライバー２３５，２３７へ出力する（Ｓ１３）。その後、各関節１８１，１８２に設けられた第一関節軸エンコーダ１７３及び第二関節軸エンコーダ１７４の出力値を観測し、これらのエンコーダ出力値がそれぞれ目標値に一致した時点で（Ｓ１４のＹｅｓ）、モータ１８８，１９１を停止させる（Ｓ１５）。これにより、ワイヤが緩んでいても、動作指令に従った目標位置に制御対象箇所を移動させることができ、適切なＰＴＰモードの動作が実現される。
【００６４】
〔実施タイミング例２〕
次に、補正駆動プロファイルを生成する処理（基準駆動プロファイルの補正処理）を実施するタイミングの他の例（本例を「実施タイミング例２」という。）について説明する。
上述した実施タイミング例１では、動作指令がＣＰモードであって、ワイヤ張力が規定値以下であるという条件を満たすと、補正駆動プロファイルを生成する処理が実施される例であったが、本実施タイミング例２では、本マニピュレータ装置の電源が投入された後の最初の動作指令を受けたという条件を満たすときに補正駆動プロファイルを生成する処理が実施される例である。このとき、更にワイヤ張力が規定値以下であるという条件を付加してもよい。本実施タイミング例２によれば、電源が投入されていない期間における何らかの原因でワイヤに緩みが発生していても、適切な動作を実現することができる。
【００６５】
〔実施タイミング例３〕
次に、補正駆動プロファイルを生成する処理（基準駆動プロファイルの補正処理）を実施するタイミングの更に他の例（本例を「実施タイミング例３」という。）について説明する。
本実施タイミング例３では、本マニピュレータ装置の関節１８１，１８２ごとに、各関節が駆動した総駆動時間（ワイヤの累積動作距離）を計測し、その総駆動時間が規定時間以上であるという条件を満たすときに補正駆動プロファイルを生成する処理が実施される例である。このとき、更にワイヤ張力が規定値以下であるという条件を付加してもよい。
【００６６】
関節の総駆動時間（ワイヤの累積動作距離）が短いうちは、未だ経時使用による伸びが発生しないことから、ワイヤが緩むような事態が生じる可能性が少ない。したがって、関節の総駆動時間が規定時間以上になるまでは、補正駆動プロファイルを生成する処理を実施しないことで、補正駆動プロファイルを生成する処理の実施頻度を下げて、マニピュレータ装置の生産性向上を図ることが可能となる。
【００６７】
〔実施タイミング例４〕
次に、補正駆動プロファイルを生成する処理（基準駆動プロファイルの補正処理）を実施するタイミングの更に他の例（本例を「実施タイミング例４」という。）について説明する。
本実施タイミング例４では、本マニピュレータ装置の稼働開始からの経過時間を計測し、その経過時間が規定時間以上であるという条件を満たすときに補正駆動プロファイルを生成する処理が実施される例である。このとき、更にワイヤ張力が規定値以下であるという条件を付加してもよい。
【００６８】
マニピュレータ装置の稼働開始からの経過時間が短いうちは、未だ経時使用による伸びが発生しないことから、ワイヤが緩むような事態が生じる可能性が少ない。したがって、マニピュレータ装置の稼働開始からの経過時間が規定時間以上になるまでは、補正駆動プロファイルを生成する処理を実施しないことで、補正駆動プロファイルを生成する処理の実施頻度を下げて、マニピュレータ装置の生産性向上を図ることが可能となる。
【００６９】
〔実施タイミング例５〕
次に、補正駆動プロファイルを生成する処理（基準駆動プロファイルの補正処理）を実施するタイミングの更に他の例（本例を「実施タイミング例５」という。）について説明する。
本実施タイミング例５では、所定の駆動プロファイルに従って制御されたモータ１８８，１９１における駆動結果の整定時間が規定時間以上であるという条件を満たすときに補正駆動プロファイルを生成する処理が実施される例である。このとき、更にワイヤ張力が規定値以下であるという条件を付加してもよい。
【００７０】
図１７は、第二モータ１９１の駆動速度が、一定の加速度で加速した後、一定の定常速度で駆動し、その後、一定の加速度で減速して停止するという駆動プロファイルに従って第二モータ１９１を駆動させたときの第二モータエンコーダ１９２の出力結果を示すグラフである。なお、このグラフには、駆動プロファイルが破線で示されている。また、第二モータエンコーダ１９２の出力結果ではなく、第二関節軸エンコーダ１７４の出力結果でもよい。
【００７１】
図１７に示すグラフにおける整定時間Ｔｓは、マニピュレータ装置の動作振動量を評価するための指標値になる。第二ワイヤ１８７の張力が適正範囲内である場合の基準駆動プロファイルは整定時間Ｔｓが短くなるように設計されており、振動が最小限になるように制御設計されている。したがって、この整定時間Ｔｓを計測し、本来よりも長い整定時間が計測されたときは、第二ワイヤ１８７に緩みが発生したことが推定できる。したがって、整定時間Ｔｓが規定時間以上であるという条件を満たしたときに補正駆動プロファイルを生成する処理を実施するようにすれば、補正駆動プロファイルの生成が必要な適切なタイミングで処理を実施できる。
【００７２】
〔実施タイミング例６〕
次に、補正駆動プロファイルを生成する処理（基準駆動プロファイルの補正処理）を実施するタイミングの更に他の例（本例を「実施タイミング例６」という。）について説明する。
本実施タイミング例６では、所定の駆動プロファイルに従って制御されたモータ１８８，１９１のモータエンコーダ１８９，１９２の出力値と、関節軸エンコーダ１７３，１７４の出力値との間に、所定量以上のズレが検出されたという条件を満たすときに補正駆動プロファイルを生成する処理が実施される例である。このとき、更にワイヤ張力が規定値以下であるという条件を付加してもよい。
【００７３】
図１８は、第二関節軸エンコーダ１７４の出力値を実線で示し、第二モータエンコーダ１９２の出力値を破線で示したグラフである。
本実施タイミング例６では、加速度期間中の特定速度に達するときの第二モータエンコーダ１９２に対する第二関節軸エンコーダ１７４の出力値の動作遅れを検出し、その動作遅れ量が規定量以上であるときに、補正駆動プロファイルを生成する処理を実施する。この動作遅れは、マニピュレータ装置の動作の軌道精度悪化等を評価するための指標値になる。第二ワイヤ１８７の張力が適正範囲内である場合の基準駆動プロファイルはこの動作遅れが短くなるように設計されており、軌道精度が上げるように制御設計されている。したがって、この動作遅れ量（動作遅れ時間）を計測し、本来よりも動作遅れ量が多いときは、第二ワイヤ１８７に緩みが発生したことが推定できる。したがって、動作遅れ量が規定量以上であるという条件を満たしたときに補正駆動プロファイルを生成する処理を実施するようにすれば、補正駆動プロファイルの生成が必要な適切なタイミングで処理を実施できる。
【００７４】
以上に説明したものは一例であり、本発明は、次の態様毎に特有の効果を奏する。
（態様Ａ）
所定の動作命令に応じた駆動プロファイルを生成する指令値生成部２１５等の駆動プロファイル生成手段と、第一モータ１８８や第二モータ１９１等の駆動源からの駆動力で駆動する第一駆動プーリ１９４や第二駆動プーリ１９６等の駆動部材によって該駆動部材と第一従動プーリ１９５や第二従動プーリ１９８等の被駆動部材との間に張架された第一ワイヤ１８６や第二ワイヤ１８７等の被張架部材を張架方向へ移動させて該被駆動部材を駆動させる駆動装置に対し、前記駆動プロファイル生成手段が生成した駆動プロファイルに従って前記駆動源を制御することにより、前記被駆動部材を該所定の駆動プロファイルに応じた目標位置に駆動させ、又は、該所定の駆動プロファイルに応じた目標タイミングで該被駆動部材の駆動を開始させる駆動制御を実行するコントローラ２１６等の制御手段とを有する駆動制御装置であって、前記被張架部材の張力を検知する張力検出部２３９等の張力検知手段を有し、前記駆動プロファイル生成手段は、前記張力検知手段が検知した被張架部材の張力に基づいて、補正駆動プロファイル等の前記所定の駆動プロファイルを生成することを特徴とする。
これによれば、駆動部材と被駆動部材との間に張架されている被張架部材の張架部分で緩みが発生していることを、張力検知手段の検知結果（被張架部材の張力）から把握することができる。被張架部材の張架部分で緩みが発生していると、駆動部材の駆動を開始しても被駆動部材の駆動が開始されるまでに遅れ時間が発生したり、その緩みを解消する処理を実施する場合にはその処理の実施時間分だけ被駆動部材の駆動開始が遅れたりする。本態様によれば、被張架部材の張力に基づいて駆動プロファイルを生成することにより、このような被駆動部材の駆動開始の遅れを考慮した駆動プロファイルを生成することができる。よって、被張架部材が緩んでも、目標位置に対する被駆動部材の駆動位置ズレや目標タイミングに対する被駆動部材の駆動開始タイミングのズレによる弊害を軽減することが可能となる。
【００７５】
（態様Ｂ）
前記態様Ａにおいて、前記駆動プロファイル生成手段は、前記張力検知手段が検知した被張架部材の張力が規定値以上であるときは、前記駆動源が駆動を開始してから前記被駆動部材の駆動が開始されるまでの遅れ時間がゼロ又は予め決められた時間である基準駆動プロファイルを前記所定の駆動プロファイルとして生成し、前記張力検知手段が検知した被張架部材の張力が前記規定値を下回るときは、該張力検知手段が検知した該被張架部材の張力に応じた遅れ時間Δｔに応じて該基準駆動プロファイルを補正した補正駆動プロファイルを前記所定の駆動プロファイルとして生成することを特徴とする。
これによれば、被張架部材の張力が規定値を下回るときに、その被張架部材の張力に応じた遅れ時間Δｔを考慮した補正駆動プロファイルを生成することで、遅れ時間Δｔによる目標位置に対する被駆動部材の駆動位置ズレや目標タイミングに対する被駆動部材の駆動開始タイミングのズレによる弊害を適切に軽減することができる。
【００７６】
（態様Ｃ）
前記態様Ａ又はＢにおいて、前記制御手段は、複数の駆動装置のそれぞれに対応する個別の駆動プロファイルに従って各駆動装置の駆動源を制御して前記駆動制御を実行するものであり、前記張力検知手段は、各駆動装置における被張架部材の張力を個別に検知するものであり、前記駆動プロファイル生成手段は、少なくとも１つの駆動装置について前記張力検知手段が検知した被張架部材の張力が規定値を下回る場合、前記張力検知手段が検知した被張架部材の張力が規定値を下回る第二関節１８２の駆動装置等の低張力駆動装置よりも前記張力検知手段が検知した被張架部材の張力が高い第一関節１８１の駆動装置等の高張力駆動装置に対する駆動プロファイルとして、該低張力駆動装置の該被張架部材の張力に応じた遅れ時間Δｔに応じて該高張力駆動装置の基準駆動プロファイルの駆動開始時点を遅らせた補正駆動プロファイルを生成することを特徴とする。
これによれば、低張力駆動装置の駆動源をその基準駆動プロファイルと駆動開始時点が同じである駆動プロファイルで駆動したとしても、低張力駆動装置と高張力駆動装置との間で被駆動部材の駆動開始タイミングがズレるのを抑制できる。
【００７７】
（態様Ｄ）
前記態様Ｂ又はＣにおいて、前記補正駆動プロファイルは、その駆動終了時点が前記基準駆動プロファイルの駆動終了時点と一致するように補正されたものであることを特徴とする。
これによれば、補正駆動プロファイルで駆動される駆動装置の被駆動部材の動作完了時刻が基準駆動プロファイル上の動作完了時刻と一致させることができるので、動作完了時刻のズレを抑制できる。
【００７８】
（態様Ｅ）
前記態様Ｂ〜Ｄのいずれかの態様において、前記補正駆動プロファイルは、前記基準駆動プロファイルにおける駆動開始時点から目標速度に達するまでの加速期間の終了時点Ｔ１ａ，Ｔ２ａで、当該補正駆動プロファイルにおける目標位置が該基準駆動プロファイルにおける目標位置と一致するように補正されたものであることを特徴とする。
これによれば、被張架部材に緩みが発生しても、加速期間の終了時点Ｔ１ａ，Ｔ２ａという早期のうちに、補正駆動プロファイルで駆動する被駆動部材の駆動位置を基準駆動プロファイル上の目標位置に追従させることが可能となる。
【００７９】
（態様Ｆ）
前記態様Ｅにおいて、前記制御手段は、前記補正駆動プロファイルにおける加速期間中の加速度が閾値以上であるときには、前記駆動源の駆動を開始しないことを特徴とする。
これによれば、駆動装置の稼働中に駆動源のトルク不足等による不具合が発生するのを回避することができる。
【００８０】
（態様Ｇ）
前記態様Ａ〜Ｆのいずれかの態様において、前記駆動プロファイル生成手段は、所定の張力低下条件が満たされたときに、前記張力検知手段が検知した被張架部材の張力に基づいて前記所定の駆動プロファイルを生成することを特徴とする。
これによれば、所定の張力低下条件を適切な条件とすることで、生産性低下を抑制しつつ、駆動プロファイル生成手段による被張架部材の張力に基づいた所定の駆動プロファイルの生成処理の実行頻度を減らしたり、処理負荷を軽減させたりすることが可能となる。
【００８１】
（態様Ｈ）
前記態様Ｇにおいて、前記所定の張力低下条件は、当該駆動制御装置に電源が投入されたという条件を含むことを特徴とする。
これによれば、電源が投入されていない期間における何らかの原因で発生した被張架部材の緩みに対し、生産性低下を抑制しつつ適切に対応することができる。
【００８２】
（態様Ｉ）
前記態様Ｇ又はＨにおいて、前記被張架部材の累積移動距離を計測する計測手段を有し、前記所定の張力低下条件は、前記計測手段が計測した前記被張架部材の累積移動距離が規定距離以上であるという条件を含むことを特徴とする。
これによれば、経時使用による被張架部材の伸びに起因した緩みに対し、生産性低下を抑制しつつ適切に対応することができる。
【００８３】
（態様Ｊ）
前記態様Ｇ〜Ｉのいずれかの態様において、当該駆動制御装置の稼働開始からの経過時間を計測する経時手段を有し、前記所定の張力低下条件は、前記経時手段が計測した経過時間が規定時間以上であるという条件を含むことを特徴とする。
これによれば、経時使用による被張架部材の伸びに起因した緩みに対し、生産性低下を抑制しつつ適切に対応することができる。
【００８４】
（態様Ｋ）
前記態様Ｇ〜Ｊのいずれかの態様において、前記所定の駆動プロファイルに従って制御された前記駆動源における駆動結果の整定時間Ｔｓを計測する経時手段を有し、前記所定の張力低下条件は、前記整定時間が規定時間以上であるという条件を含むことを特徴とする。
これによれば、被張架部材の緩みに対し、生産性低下を抑制しつつ適切に対応することができる。
【００８５】
（態様Ｌ）
前記態様Ａ〜Ｋのいずれかの態様において、前記制御手段は、前記駆動プロファイルに従って制御された前記駆動源又は前記駆動部材の駆動量（モータエンコーダ１８９，１９２の出力値等）と該駆動プロファイル上の目標量とのズレを低減するためのフィードバック制御又はフィードフォワード制御を実施して前記駆動制御を実行することを特徴とする。
これによれば、より適切に駆動プロファイルに従った駆動制御を実現することができる。
【００８６】
（態様Ｍ）
１又は２以上の関節部１８１，１８２と、前記関節部の動作に連動する被駆動部材と駆動部材との間に張架された被張架部材を介して該駆動部材の駆動力を該被駆動部材へ伝達して該関節部を駆動させる駆動装置と、前記駆動装置の駆動源を制御して前記関節部の動作を制御する駆動制御装置とを有するマニピュレータ装置において、前記駆動制御装置として、前記態様Ａ〜Ｌのいずれかの態様に係る駆動制御装置を用いることを特徴とする。
これによれば、被張架部材に緩みが発生しても、マニピュレータ装置の制御対象箇所を適切に動作させることができる。
【００８７】
（態様Ｎ）
前記態様Ｍにおいて、前記関節部を２以上有し、前記駆動装置は関節部ごとに設けられており、前記駆動制御装置として、前記態様Ｂ又はＣに係る駆動制御装置を用い、前記駆動制御装置における前記駆動プロファイル生成手段は、当該マニピュレータ装置の制御対象箇所を前記所定の動作命令で指示された移動経路に沿って該所定の動作命令で指示された目標位置まで移動させるための各駆動装置の駆動プロファイルを生成するＣＰモード等の第１処理モードと、前記所定の動作命令で指示された目標位置に基づいて該目標位置までの移動経路を算出し、当該マニピュレータ装置の制御対象箇所を該移動経路に沿って該目標位置まで移動させるための各駆動装置の駆動プロファイルを生成するＰＴＰモード等の第２処理モードとを有しており、前記駆動プロファイル生成手段は、前記第１処理モードでは前記補正駆動プロファイルを生成するが、前記第２処理モードでは前記補正駆動プロファイルを生成せずに前記基準駆動プロファイルを生成することを特徴とする。
これによれば、第一処理モードにおいて、被張架部材に緩みが発生しても、所定の動作命令で指示された移動経路に沿って制御対象箇所を目標位置まで移動させることができる。
【符号の説明】
【００８８】
１００  マニピュレータ装置
１３１  歪みゲージ
１７３，１７４  関節軸エンコーダ
１８１  第一関節
１８２  第二関節
１８６  第一ワイヤ
１８７  第二ワイヤ
１８８  第一モータ
１８９  第一モータエンコーダ
１９１  第二モータ
１９２  第二モータエンコーダ
１９４  第一駆動プーリ
１９５  第一従動プーリ
１９６  第二駆動プーリ
１９７  中継プーリ
１９８  第二従動プーリ
２１３  駆動制御部
２１５  指令値生成部
２１５Ａ  基準指令値生成部
２１５Ｂ  演算部
２１６  コントローラ
２３５，２３７  モータドライバー
２３６，２３８  駆動情報検出部
２３９  張力検出部
３００  ホストコントローラ
【先行技術文献】
【特許文献】
【００８９】
【特許文献１】特許第３５７８３７５号公報
【図１】
0006497575.tif 000002
【図２】
0006497575.tif 000003
【図３】
0006497575.tif 000004
【図４】
0006497575.tif 000005
【図５】
0006497575.tif 000006
【図６】
0006497575.tif 000007
【図７】
0006497575.tif 000008
【図８】
0006497575.tif 000009
【図９】
0006497575.tif 000010
【図１０】
0006497575.tif 000011
【図１１】
0006497575.tif 000012
【図１２】
0006497575.tif 000013
【図１３】
0006497575.tif 000014
【図１４】
0006497575.tif 000015
【図１５】
0006497575.tif 000016
【図１６】
0006497575.tif 000017
【図１７】
0006497575.tif 000018
【図１８】
0006497575.tif 000019