JP5471482B2 - パラレルリンク型ロボット及びその異常検知方法 - Google Patents

Description

本発明は、第１部材と第２部材が伸縮する複数のリンクで連結されているパラレルリンク型ロボットと、パラレルリンク型ロボットのリンク制御に異常が発生したことを検知する方法に関する。

第１部材と第２部材を複数のリンクが並列に連結しているパラレルリンク型ロボットが知られている。本明細書では、ベースとなる部材を第１部材と称し、揺動する部材を第２部材と称する。パラレルリンク型ロボットは、アクチュエータによってリンクを伸縮し、第１部材に対する第２部材の姿勢を所望の姿勢（目標姿勢）に追従させる。なお、パラレルリンク型ロボットは、リンクの長さによって第２部材の角度を決めるタイプもあれば、第２部材の角度と位置の双方を決めることができるタイプもある。第２部材の角度を決める場合、ロール、ピッチ、ヨーの３つの角度を決めるものもあれば、一つの軸周りの角度だけを決めるタイプもある。前者の例は、直交３軸方向の位置と直交３軸周りの角度の合計６自由度を制御可能なモーションライドと呼ばれる装置がその代表である。後者の例としては、非特許文献１に開示されている平面３自由度を制御する装置がある。非特許文献１に開示されているパラレルリンク型ロボットは、第２部材の平面内の位置（２自由度）と、平面の垂線周りの角度（１自由度）を決めることができる。また、パラレルリンク型ロボットを人の歩行補助装置に適用した例が特許文献１に開示されている。

本明細書では、第２部材の「位置」と「角度」を「姿勢」という言葉で総称する。この点についてまず、説明する。直交３軸方向の位置と直交３軸周りの角度の空間６自由度のうち、第２部材に許容する自由度としていずれの自由度を選択するかはパラレルリンク型ロボットの機構に依存する。パラレルリンク型ロボットに限らずロボット一般において、ロボット先端の角度と位置は位置決め制御においては同等に扱うことができる。角度と位置を総称する用語として、「空間座標」という用語が使われることがある。ただし、本明細書では、理解し易さを考慮し、「空間座標」（即ち、角度及び／又は位置）を、「姿勢」と称する。特に、本明細書では、空間６自由度のうち第２部材に許容されている自由度に対して、各自由度に対応する角度及び／又は位置を規定する値の組を「姿勢」と称する。例えば、第２部材に３自由度が許容されている場合、第２部材の姿勢は、各自由度を決める３個の数値の組で表される。そして、第２部材の制御目標値である角度及び／又は位置を「目標姿勢」と称する。

なお、以下では、第２部材に許容されている自由度の数をＮ自由度とする。例えば、第２部材の６自由度全てを決定可能なパラレルリンク型ロボットではＮ＝６である。自由度Ｎは６に限られないことに留意されたい。例えば、リンクによって第２部材が平面上を移動するように構成されているパラレルリンク型ロボット場合、第２部材の自由度Ｎは、平面内の位置２自由度と平面垂線周りの１つの回転自由度の合計３自由度である（例えば非特許文献１を参照）。それ以外の自由度は拘束されている。即ち、第２部材に許容されている自由度Ｎは３である。

特開２００８−７９８０９号公報

ヴォラポッド セリーラット、佐藤三禄、「油圧シリンダを用いた平面３自由度パラレルリンクサーボ機構の運動制御（第１報）」、日本フルードパワーシステム学会論文集、第３５巻、第２号、８−１３ページ、２００４年３月

パラレルリンク型ロボットは、リンクの長さを調整することによって第２部材の姿勢を決めるため、各リンクにはリンクの長さを計測するためのセンサが備えられている。センサが故障すると、正常なリンク制御ができなくなる。センサの故障に備えるため、各リンクに複数のセンサを設けることがある。各リンクに複数のセンサを設けるとコストが嵩む。本明細書は、パラレルリンク型ロボットの構造の特徴を利用し、各リンクに複数のセンサを備えることなく、リンクの制御に異常が発生していないか否かを検知する技術を提供する。

パラレルリンク型ロボットは、第２部材のＮ自由度の姿勢を決定するためにＮ本のリンクを備える。Ｎ本のリンクは、夫々が第１部材と第２部材を連結している。パラレルリンク型ロボットでは、第２部材の目標姿勢を各リンクの目標長さに変換し、目標長さに一致するように各リンクの長さを制御する。パラレルリンク型ロボットでは、１本のリンクでも正常に制御できなくなると、他のリンクの長さが目標長さに一致していても第２部材の姿勢が目標姿勢に一致しない。そのため、センサが故障し、そのセンサの出力データは目標長さに等しい長さを示しているが、実際のリンク長さは目標長さに等しくない状況が起こり得る。そのような場合、全てのセンサの出力データは夫々の目標長さを示すが第２部材の姿勢は目標姿勢に一致しない。そのような状況に対処するために、本明細書が開示するパラレルリンク型ロボットは、Ｎ本のリンクの他にさらに、第１部材と第２部材を連結する別のリンクを備える。「Ｎ本のリンク」と「別のリンク」区別するため、Ｎ本のリンクを主リンクと称し、別のリンクを補助リンクと称する。ただし、補助リンクは主リンクと同じ構成を有していてもよい。本明細書が開示する技術は、補助リンクの長さから、いずれかの主リンクの制御に異常が発生していることを検知する。

夫々の主リンクは、主リンクを伸縮させるためのアクチュエータを備えている。主リンクの長さを制御するため、夫々の主リンクにはその長さを計測するセンサも備えられている。パラレルリンク型ロボットのコントローラは、第２部材の姿勢を目標姿勢に一致させるようにアクチュエータによって主リンクの長さを制御する。

本明細書が開示するパラレルリンク型ロボットは、Ｎ本の主リンクに加えて、第１部材と第２部材を連結している補助リンクを備える。補助リンクにはその長さを計測するセンサが備えられている。補助リンクが備えるセンサと主リンクが備えるセンサを区別するために、主リンクが備えるセンサを主センサと称し、補助リンクが備えるセンサを補助センサと称することがある。ただし、主センサと補助センサは同タイプのものであってよい。主センサと補助センサを併せて「センサ」と称する場合がある。同様に、主リンクと補助リンクを併せて「リンク」と称する場合がある。

補助リンクは、外部から加えられるリンク軸方向の力によって受動的に伸縮することができるように構成されている。例えば、補助リンクは、バネによって伸展方向に付勢されている。主リンクの能動的な伸縮によって第２部材の姿勢が変化し、第２部材の姿勢変化に伴って補助リンクの長さが受動的に変化する。

本明細書が開示する技術は、第２部材の動きに伴って受動的に伸縮する補助リンクの長さから、いずれかの主リンクの制御に異常が発生しているか否かを検知する。コントローラは、補助センサによって計測された補助リンクの長さが、第２部材の姿勢が目標姿勢に一致した場合の補助リンクの予定長さと異なる場合に、いずれかの主リンクの制御に異常が発生していると判断する。異常が発生していると判断した場合、例えば、コントローラは、主リンクの制御を停止する。或いはコントローラは、異常が発生している旨を知らせる信号を外部に出力する。

本明細書が開示するパラレルリンク型ロボットは、Ｎ本の主リンクとは別に、第１部材と第２部材を主リンクと並列に連結する補助リンクを備える。許容されている自由度Ｎに対して主リンクの数がＮ本であるから、Ｎ本の主リンクによって自由度Ｎの全ての値が一意に定まる。即ち、第２部材の姿勢が一意に定まる。但し、本明細書では、特異点については言及しない。全ての主リンクが正常に制御されている場合、第２部材の姿勢は目標姿勢に一致する。本明細書が開示する技術は、Ｎ本の主リンクの長さを制御することによって目標姿勢に一致するはずの第２部材の姿勢を、補助リンクの長さに基づいて検証する。補助リンクの長さが予定長さと異なる場合、いずれかの主リンクの制御が正常に行われていないことが判明する。本明細書が開示する技術によれば、全ての主リンクに複数のセンサを備えることなく、いずれかの主リンクの制御に異常が発生していることを検知することができる。

なお、上記の異常検知は、次の状況を想定している。いずれかの主センサが故障し、実際には第２部材の姿勢が目標姿勢と一致してはいないが、故障した主センサの出力を使って算出された第２部材の姿勢が目標姿勢に一致している状況である。そのような状況では、主センサの出力だけでは異常を検知できない。本明細書が開示する技術は、そのような状況であっても主リンクの制御異常を検知することができる。

計測される補助リンクの長さを予定長さと比較することに代えて、補助リンクの長さを使って第２部材の姿勢を算出し、算出された姿勢を目標姿勢と比較することも好適である。即ち、コントローラは、次の処理を行うことも好適である。（１）Ｎ本の主リンクの中から選択したＮ未満の数の主リンクに補助リンクを加えた合計Ｎ本のリンクの長さから第２部材の姿勢を算出する。（２）算出された姿勢が目標姿勢と異なる場合に、リンクの制御に異常が発生していると判断する。そのような処理によって、第２部材の実際の姿勢が目標姿勢からどの位ずれているのかも同時に検出することができる。

いずれかの主リンクで制御異常が発生すると、第２部材の姿勢を目標姿勢に一致させる制御を継続できなくなる。本明細書は、いずれかの主リンクで制御異常が発生した場合であっても制御を継続することのできる技術も提供する。そのためにパラレルリンク型ロボットは、補助リンクにアクチュエータを備える。補助リンクのアクチュエータはコントローラによって制御され、補助リンクを能動的に伸縮させることができる。コントローラは、補助リンクが外部からリンク軸方向に加えられる力によって受動的に伸縮する力制御モードと、補助リンクの長さが目標長さに一致するように補助リンクを制御する位置制御モードとを切り換えて補助リンクを制御することができるように構成される。例えば、電源遮断時の伸縮摺動抵抗が小さければ、コントローラは、アクチュエータの電源を遮断することによって力制御モードを実現することができる。或いは、補助リンクに外力を計測する力センサ（例えばロードセル）を取り付け、力センサが計測する外力がゼロに近い閾値以下となるようにアクチュエータを制御することによっても力制御モードを実現することができる。さらに、コントローラは、リンクの制御に異常が発生していると判断した場合に、補助リンクを位置制御モードに切り換えて第２部材の姿勢を目標姿勢に一致させる制御に用いるように構成されている。上記の構成によって、いずれかの主リンクの制御が異常となった場合に補助リンクを使って第２部材の位置決め制御を継続することができる。なお、本明細書は、異常が発生しているリンクを特定する方法も提案する。その方法は後述する。

補助リンクがアクチュエータを備える場合、補助リンクの構造は主リンクの構造と同じであってよい。即ち、補助リンクのアクチュエータとセンサは、主リンクのアクチュエータとセンサと同じものであってよい。即ち、パラレルリンク型ロボットは、Ｎ自由度が許容されている第２部材に対してＮより多いＭ本の主リンクを備える構造であってよい。以下に、そのような構造を有するパラレルリンク型ロボットにおいてリンクの制御異常を検知する方法を説明する。

本明細書が開示する他の技術は、Ｎ＋Ｍ本のリンクが並列に第１部材と第２部材を連結しており、第２部材がＮ自由度を許容されているパラレルリンク型ロボットの異常を検知する方法である。ここで、Ｎ本のリンクは、アクチュエータによって能動的に伸縮する主リンクに相当する。Ｍ本のリンクは、主リンクの伸縮に伴って（第２部材の動きに伴って）受動的に伸縮する補助リンクに相当する。この方法は、次の４つのステップを備える。第１ステップでは、Ｎ＋Ｍ本の全リンクの中からＮ本のリンクを選択する。このとき、主リンクと補助リンクが混在する組み合わせも取り得る。第２ステップでは、選択されたリンクの長さから第１部材に対する第２部材の姿勢を算出する。第３ステップでは、算出された姿勢を第２部材の目標姿勢と比較する。第４ステップでは、上記した第１ステップから第３ステップを異なるリンクの組み合わせで繰り返す。第４ステップはさらに、算出された姿勢が目標姿勢と一致するリンク組み合わせと一致しないリンク組み合わせが存在する場合、一致しないリンク組み合わせに含まれないリンクを制御異常発生リンクとして特定する。

上記の方法で制御異常が発生しているリンクを特定できる理由は次の通りである。今、前提としているのは、異常が発生している主リンクでは、主センサの出力データは目標長さに等しい長さを示しているが、実際のリンク長さは目標長さに等しくない場合である。また、パラレルリンク構造では、１本のリンクが目標長さに一致していない場合、他のリンクが目標長さに一致していても第２部材の姿勢は目標姿勢に一致しない。そのため、異常が発生しているリンクを含むリンク組み合わせでは算出された姿勢が目標姿勢に一致するが、正常なリンクのみから構成されるリンク組み合わせでは算出された姿勢が目標姿勢に一致しない。従って、算出された姿勢が目標姿勢に一致しないリンク組み合わせを構成するリンクを除外していくことによって、異常が発生しているリンクを特定できる。

なお、前述したように、補助リンクが主リンクと同様にアクチュエータを備えていてもよい。その場合、Ｎ＋Ｍ本の主リンクを備え、コントローラは、Ｍ本のリンクを、力制御モードで制御すればよい。その場合、前述したように、異常が発生した主リンクに代えて力制御モードで制御していたリンクを位置制御モードに切り換えることで第２部材の姿勢制御を継続することができる。

本明細書は、パラレルリンク型ロボットにおいて、いずれかのリンクに制御異常が発生したことを検知する技術を提供する。

第１実施例のパラレルリンク型ロボットの模式的平面図を示す。 姿勢制御のフローチャート図を示す。 異常検知処理のフローチャート図を示す。 第２実施例のパラレルリンク型ロボットの模式的斜視図を示す。 第２実施例のロボットの異常検知処理のフローチャート図を示す。

図１に、第１実施例のパラレルリンク型ロボット１００の模式的平面図を示す。以下、簡単のため、パラレルリンク型ロボット１００を単にロボット１００と称する。ロボット１００は、第２部材１２をＸＹ平面内で位置決めする３自由度のパラレルリンク型ロボットである。第２部材１２は、ＸＹ平面内で移動可能であり、ＸＹ平面から外れることはできない。即ち、第２部材１２に許容されている自由度Ｎは、Ｎ＝３である。ロボット１００は、第２部材１２のＸＹ面内での位置と、ＸＹ平面の垂線回りの回転角の合計３自由度を制御することができる。第２部材１２のＸＹ面内での位置と、ＸＹ平面の垂直線回りの回転角の組み合わせが第２部材１２の「姿勢」に相当する。

第２部材１２は、３本の主リンク１３ａ、１３ｂ、及び、１３ｃと、１本の補助リンク１５を介して第１部材１１に連結されている。以下、３本の主リンクを総称する場合には「主リンク１３」と記述し、区別なく１本の主リンクを示す場合には「各主リンク１３」と記述する。３本の主リンク１３は、夫々が第１部材１１と第２部材１２を連結している。３本の主リンク１３は並列に配置されている。各主リンク１３は、一端が第１部材１１に揺動可能に連結されているとともに、他端が第２部材１２に揺動可能に連結されている。各主リンク１３は、リンク長さを調整するリニアアクチュエータ１４と、主リンクの長さを計測するためのリニアエンコーダ１７ａを備える。リニアエンコーダ１７ａが主センサに相当する。リニアアクチュエータ１４は、例えばボールネジとウォームギアを組み合わせた構造を有しており、モータによって駆動される。リニアエンコーダ１７ａのセンサデータはコントローラ２０に送られる。コントローラ２０は、リニアエンコーダ１７ａのセンサデータに基づいて、第２部材１２の姿勢を目標姿勢に一致させるように、３本の主リンク１３（リニアアクチュエータ１４）を制御する。以下、リニアアクチュエータを単に「アクチュエータ」と称し、リニアエンコーダを単に「エンコーダ」と称する。

ロボット１００はさらに、補助リンク１５を備える。補助リンク１５は、主リンク１３と並列に第１部材１１と第２部材１２を連結している。補助リンク１５の一端が第１部材１１に揺動可能に連結されているとともに、他端が第２部材１２に揺動可能に連結されている。補助リンク１５は、その長さを伸展させる方向に付勢するバネ１６を備える。補助リンク１５は、バネ１６の付勢力によって、第２部材１２の動きに応じて、即ち軸方向に加わる外力によって、受動的に伸縮する。バネ１６の付勢力は、アクチュエータ１４が出力する力に比べて無視し得るほど小さい。従って、補助リンク１５は、第２部材１２の動きに応じて「受動的に」伸縮する。また、補助リンク１５は、その長さを計測するためのエンコーダ１７ｂを備える。エンコーダ１７ｂが補助センサに相当する。エンコーダ１７ｂのセンサデータもコントローラ２０へ送られる。なお、主リンク１３が有するエンコーダ１７ａと補助リンク１５が有するエンコーダ１７ｂは同じものであるが、主リンク１３が有するエンコーダ（主センサ）と補助リンクが有するエンコーダ（補助センサ）を区別するため、符号に添え字「ａ」、「ｂ」を付している。

ロボット１００は、３本の主リンク１３の長さを調整することによって、第２部材１２の３自由度の姿勢を決定する。図２に、コントローラ２０が実行する姿勢制御のフローチャートを示す。コントローラ２０は、各エンコーダ１７ａのセンサデータ（主リンクの長さのデータ）を取得する（Ｓ２２）。次にコントローラ２０は、第２部材１２の目標姿勢から、目標姿勢を実現するための各主リンク１３の目標長さを算出する（Ｓ２３）。パラレルリンク型ロボットの第２部材の（目標）姿勢から各リンクの（目標）長さを求める変換式はロボット工学において通称「逆変換」と呼ばれている。なお、各主リンクの長さから第２部材の姿勢を求める変換式は通称「順変換」と呼ばれる。「順変換」、「逆変換」は、ロボットの機械的構造に依存する。それらの変換式についてはロボット工学の分野では良く知られているので説明は省略する。コントローラ２０は、エンコーダ１７ａ（主センサ）によって計測された各主リンク１３の長さが、目標長さに一致するように各アクチュエータ１４を駆動する（Ｓ２４）。別言すれば、コントローラ２０は、第２部材１２の姿勢を目標姿勢に一致させるようにアクチュエータ１４によって主リンク１３の長さを制御する。

コントローラ２０は、主リンクを制御している間、補助リンク１５の長さに基づいて、主リンクの制御が正常に行われているか否かをモニタする。ここで、第２部材１２に許容されている自由度Ｎは「３」であり、第２部材１２を動かすための主リンク１３の数も３本であるから、第２部材１２の姿勢は、３本の主リンクの長さによって一意に定まる。補助リンク１５は、第２部材１２の動きに合わせて受動的に伸縮する。補助リンク１５は、第２部材１２の姿勢決定には寄与しない。ロボット１００は、第２部材１２の姿勢決定には寄与しない補助リンクを使って、第２部材１２の姿勢を決定するために能動的に伸縮する主リンクに制御異常が発生していないかをモニタする。

主リンクの制御異常検知処理のフローチャートを図３に示す。コントローラ２０は、まず、補助リンク１５が備えるエンコーダ１７ｂ（補助センサ）のデータを取得する（Ｓ３２）。即ちコントローラ２０は、補助リンク１５の長さを取得する。次にコントローラ２０は、第２部材１２の姿勢が目標姿勢に一致したと仮定した場合に補助リンク１５が実現すべき予定長さを算出する（Ｓ３３）。予定長さは、目標姿勢を逆変換することによって得られる。

次にコントローラ２０は、ステップＳ３３で算出された予定長さがステップＳ３２で取得した補助リンク１５の長さ（計測長さ）と一致しているか否かを判断する（Ｓ３４）。予定長さが計測長さに一致している場合は（Ｓ３４：ＹＥＳ）、３本の主リンク１３によって目標姿勢が実現されていることを意味する。この場合は、コントローラ２０は異常検知処理を終了する。他方、予定長さが計測長さに一致していない場合（Ｓ３４：ＮＯ）、第２部材１２の姿勢が目標姿勢に一致していないことを意味する。Ｓ３４の分岐判断が「ＮＯ」の場合、コントローラ２０は、いずれかの主リンクの制御に異常が発生していると判断する。この場合、コントローラ２０は、アクチュエータ１４の制御を停止するとともに（Ｓ３６）、異常発生を知らせる信号を出力する（Ｓ３７）。例えばコントローラ２０は、異常発生を通知するランプを点灯させる信号を出力する。

図３のステップＳ３４において、補助リンク１５の計測長さが予定長さと一致しない場合、即ち異常発生と判断する場合について説明を加える。先に述べたように、この場合は、第２部材１２の姿勢が目標姿勢に一致していないことを意味する。図２に示したフローチャートとともに先に説明したように、各主リンク１３の長さは、第２部材１２の姿勢が目標姿勢に一致するように制御されている。全ての主センサ（エンコーダ１７ａ）が正常に機能している場合であって、何らかの理由で第２部材１２の姿勢が目標姿勢に一致していない場合、いずれかの主リンクにおいて、目標長さと計測長さの偏差が残る。アクチュエータを制御しても偏差が残ることから、コントローラ２０は第２部材１２が目標姿勢に一致していないことを検知できる。しかしながら、いずれかの主センサ（エンコーダ１７ａ）が故障しており、実際には主リンク長さが目標長さに一致してはいないが、主センサの出力が、主リンク長さが目標長さに等しいことを示している場合、主センサの出力からは故障を検知することができない。そこで、実施例のロボット１００では、補助リンク１５の補助センサ（エンコーダ１７ｂ）のセンサデータを使って第２部材１２の姿勢を算出し、主センサ（エンコーダ１７ａ）の出力が正しいか否かをモニタしている。ロボット１００は、補助リンク１５を備えることによって、主センサだけでは検知不可能なタイプの制御異常を検知することができる。

図３に示した異常検知処理の代替処理を説明する。コントローラ２０は、ステップＳ３３で補助リンクの予定長さを算出する代わりに、３本の主リンク１３の中から選択した２本の主リンクに補助リンクを加えた合計３本のリンク長さから第２部材１２の姿勢を算出してもよい。その場合、ステップＳ３４では、算出された姿勢が目標姿勢と異なる場合に、リンクの制御異常が発生したと判断する。そのような手法は、補助リンクの計測長さに基づいて主センサの異常を検知するという観点において、図３のフローチャートによる異常検知の手法と基本的に同等である。ただし、この手法は、目標姿勢と実際の姿勢の偏差がどの程度であるかを同時に検出することができる利点がある。コントローラ２０は、目標姿勢と実際の姿勢との偏差が予め定められた閾値を超えた場合に、異常が発生したと判断するように構成されることも好適である。

上記説明した手法を自由度Ｎについて表現すると以下の通りである。コントローラ２０は、Ｎ本の主リンクの中から選択したＮ以下の数の主リンクに補助リンクを加えた合計Ｎ本のリンク長さから第２部材の姿勢を算出し、算出された姿勢が目標姿勢と異なる場合に、異常が発生していると判断する。補助リンクを加えた合計Ｎ本のリンクの長さを用いるので、Ｎ自由度の第２部材の姿勢を決定することができる。上記説明した異常検知の方法は、複数の補助リンクを備える場合であっても適用できることに留意されたい。

次に第２実施例のパラレルリンク型ロボット２００を説明する。以下では、パラレルリンク型ロボット２００を単にロボット２００と称する。ロボット２００の模式的斜視図を図４に示す。ロボット２００は、第２部材２１２の直交３軸各軸周りの回転角を変更することができる３自由度パラレルリンク型ロボットである。ここで、図４に示すＸ軸、Ｙ軸、Ｚ軸が、直交３軸に相当する。各軸周りの回転角は、いわゆる、ピッチ角、ロール角、及びヨー角に相当する。

第２部材２１２は、第１部材２１１から伸びる支柱２３０の先端にボールジョイントを介して揺動可能に連結されている。第２部材２１２は、ボールジョイントによって支柱２３０の先端に連結されているので、３軸各軸の周りに揺動することができる。しかし第２部材２１２は、支柱２３０によって、並進方向の移動は拘束されている。即ち、第２部材２１２に許容されている自由度Ｎは、Ｎ＝３である。第２部材２１２の各軸周りの回転角（ピッチ角、ロール角、及び、ヨー角）の組み合わせが、第２部材２１２の「姿勢」に相当する。なお、Ｚ軸は、支柱２３０の長手方向に伸びる軸であり、Ｘ軸とＹ軸は、Ｚ軸に直交する軸である。

ロボット２００では、４本の主リンク２１３ａ、２１３ｂ、２１３ｃ、及び、２１４ｄが、夫々第１部材２１１と第２部材２１２を連結している。各主リンクとも、一端がボールジョイントを介して第１部材２１１に揺動可能に連結されており、他端がボールジョイントを介して第２部材２１２に揺動可能に連結されている。各主リンクは、リンクを伸縮させるアクチュエータ（リニアアクチュエータ）２１４と、リンク長を計測するエンコーダ（リニアエンコーダ）２１７を有する。エンコーダ２１７はアクチュエータ２１４に内蔵されており、図示を省略している。各主リンクの長さは、コントローラ２２０によって制御される。

図４に示す機械的構造を有するロボット２００は、全ての主リンクを同時に伸ばすと（同時に縮めると）、第２部材２１２がＺ軸の周りに回転する。いくつかの主リンクを伸ばし、他の主リンクを縮めることによって、第２部材２１２はＸ軸或いはＹ軸の周りに回転する。そのようにして、ロボット２００は、第２部材２１２の姿勢（各軸周りの角度）を目標姿勢に一致させることができる。即ちロボット２００は、第２部材２１２の３自由度の姿勢を目標姿勢に一致させることができる。

１本の主リンク２１３ａは、力センサ２１８を備えている。力センサ２１８は、主リンク２１３ａの軸方向に加わる力（外力）を計測する。コントローラ２２０は、力センサ２１８によって計測される外力がほぼゼロとなるように主リンク２１３ａの長さを制御する。即ち、主リンク２１３ａは、軸方向に作用する外力に応じて受動的に伸縮するように力制御される。このことを、コントローラ２２０が主リンク２１３ａを「力制御モードで制御する」と表現する。他方、コントローラ２２０は、他の３本の主リンク２１３ｂ、２１３ｃ、及び、２１３ｄの長さを適宜に調整することによって、第２部材２１２の姿勢を目標姿勢に一致させることができる。コントローラ２２０は、先に説明した逆変換によって目標姿勢から主リンク２１３ｂ、２１３ｃ、及び、２１３ｄの目標長さを算出し、夫々の主リンクが目標長さに一致するように制御する。このことを、コントローラが主リンク２１３ｂ、２１３ｃ、及び、２１３ｄを「位置制御モードで制御する」と表現する。なお、第２部材２１２の自由度ＮはＮ＝３であるから、３本の主リンク２１３ｂ、２１３ｃ、及び、２１３ｄの長さによって第２部材２１２の姿勢が一意に定まる。前述したように、主リンク２１３ａは、外力に応じて受動的に伸縮するように制御されるので、３本の主リンク２１３ｂ、２１３ｃ、及び、２１３ｄの長さ変化によって第２部材２１２の姿勢が変化し、第２部材２１２の姿勢変化に伴って主リンク２１３ａの長さが受動的に変化する。

力制御モードで制御される主リンク２１３ａを位置制御モードで制御される他の３本の主リンクと区別するために、主リンク２１３ａを「補助リンク２１３ａ」と称する場合がある。同様に、主リンク２１３ａが備えるセンサ（エンコーダ２１７）を補助センサと称することがある。これに対して位置制御モードで制御される他の３本の主リンクが有するセンサ（エンコーダ２１７）を主センサと称することがある。また、以下では、主リンク２１３ｂ、２１３ｃ、及び２１３ｄと補助リンク２１３ａを総称する場合は単に「リンク」と称する。また、主リンクのセンサと補助リンクのセンサを総称する場合は単に「センサ」と称する。

主リンクの制御異常を検知する処理のフローチャートを図５に示す。コントローラ２２０はまず、フラグに「ＯＦＦ」を設定する（Ｓ６１）。このフラグは、異常が発生しているリンクが存在する場合に「ＯＮ」に再設定される（Ｓ６６、後述）。このフラグは、処理を円滑に進めるための技巧的なフラグであることに留意されたい。コントローラ２２０は、４本のリンクのセンサデータ（即ち各リンクの長さ）を取得する（Ｓ６２）。次にコントローラ２２０は、４本のリンクの中から３本を選択する（Ｓ６３）。選択されるリンクに補助リンク２１３ａが含まれていてもよい。４本のリンクの中から３本を選択する組み合わせ（リンク組み合わせ）は４通りである。後述するように、コントローラ２２０は、４通りの全てのリンク組み合わせについて、ステップＳ６３からＳ６７までの処理を繰り返す（Ｓ６７：ＹＥＳ、Ｓ６３）。コントローラ２２０は、選択した３本のリンクの長さから第２部材２１２の姿勢を算出する（Ｓ６４）。次にコントローラ２２０は、算出した姿勢が第２部材２１２の目標姿勢に一致しているか否かを判断する（Ｓ６５）。算出した姿勢が目標姿勢に一致していない場合は（Ｓ６５：ＮＯ）、いずれかのリンクで制御異常が発生していることを意味する。そこでコントローラ２２０は、フラグに「ＯＮ」を設定する（Ｓ６６）。なお、コントローラ２２０は、夫々のリンク組み合わせを構成するリンクと、そのリンク組み合わせで算出された姿勢が目標姿勢に一致したか否かの情報を記憶しておく。

上記Ｓ６３からＳ６７までの処理が全てのリンク組み合わせについて行われた後、フラグにＯＮが設定されている場合はステップＳ６９とＳ７０を実行し（Ｓ６８：ＹＥＳ）、ＯＮが設定されていない場合は処理を終了する（Ｓ６８：ＮＯ）。即ち、フラグにＯＮが設定されている場合は、いずれかの主リンクで制御異常が発生していることを示し、フラグにＯＦＦが設定されたままの場合は、制御異常が発生していないことを示す。

ステップＳ６９ではコントローラ２２０は、制御異常が発生しているリンクを特定する。先に述べたように、コントローラ２２０は、夫々のリンク組み合わせを構成するリンクと、そのリンク組み合わせによって算出された姿勢が目標姿勢に一致しているか否かを記憶している。コントローラ２２０は、全てのリンクから、算出された姿勢が目標姿勢に一致しなかったリンク組み合わせを構成するリンクを除外していき、残ったリンクを異常発生リンクとして特定する。このようにして異常発生リンクを特定できるのは次の理由による。

今、前提としているのは、制御異常が発生しているリンクでは、センサの出力データは目標長さに等しい長さを示しているが、実際のリンク長さは目標長さに等しくない場合である。また、Ｎ自由度に対してＮ本の主リンク（長さが能動的に調整されるリンク）を有するパラレルリンク型ロボットでは、１本のリンクが目標長さに一致していない場合、他のリンクが目標長さに一致していても第２部材２１２の姿勢は目標姿勢に一致しない。そのため、制御異常が発生しているリンクを含むリンク組み合わせでは算出された姿勢が目標姿勢に一致するが、正常なリンクのみから構成されるリンク組み合わせでは、算出された姿勢が目標姿勢に一致しない。算出された姿勢が目標姿勢に一致するリンク組み合わせを構成するリンクであっても正常なリンクは、他のリンクと組み合わされたときに、算出される姿勢が目標姿勢に一致しない。そのため、算出された姿勢が目標姿勢に一致するリンク組み合わせを構成するリンクであっても正常なリンクは除外されていく。こうして、算出された姿勢が目標姿勢に一致しないリンク組み合わせを構成するリンクを除外していく処理を続けていき、除外されずに残ったリンクが、制御異常が発生しているリンクとして特定される。

コントローラ２２０は、制御異常が発生しているリンクの電源を遮断し（パワーオフ）、そのリンクを、受動的に伸縮する状態にする（Ｓ６９）。そしてコントローラ２２０は、補助リンクを力制御モードから位置制御モードに切り換える（Ｓ７０）。こうして、制御異常が発生しているリンクを位置制御（第２部材２１２を目標姿勢に一致させるための制御）から外し、補助リンクを位置制御に切り換えることによって、第２部材２１２の姿勢を目標姿勢に一致させる制御を継続することができる。

第２実施例ではＮ＝３の場合を想定した。本実施例の技術は、Ｎ＝３以外の場合にも適用できる。また、補助リンクが複数本の場合にも適用できる。図５のフローチャートの処理を自由度を記号Ｎで表し、補助リンクの数を記号Ｍで表すと、次の通り表現することができる。このリンクの制御異常検知方法は、アクチュエータによって能動的に伸縮するＮ本のリンクが並列に第１部材と第２部材を連結しており、第２部材がＮ自由度の動きを許容されているパラレルリンク型ロボットのリンク制御異常を検知する方法である。その方法は、次の４つのステップを含む。第１ステップ：Ｎ＋Ｍ本のリンクの中からＮ本のリンクを選択する。このステップは、図５のステップＳ６３に相当する。第２ステップ：選択されたリンクの長さから第１部材に対する第２部材の姿勢を算出する。このステップは、図５のステップＳ６４に相当する。第３ステップ：算出された姿勢を第２部材の目標姿勢と比較する。このステップは、図５のステップＳ６５に相当する。第４ステップ：第１ステップから第３ステップをリンクの異なる組み合わせで繰り返し（Ｓ６７）、算出された姿勢が目標姿勢と一致するリンク組み合わせと一致しないリンク組み合わせが存在する場合（Ｓ６６、フラグ＝ＯＮ）、一致しないことを示したリンク組み合わせに含まれないリンクを制御異常発生リンクとして特定する（Ｓ６９）。

また、第２実施例において、異常発生リンクを除外し、補助リンクによって第２部材２１２の姿勢制御を継続する機能に着目すると、第２実施例のロボット２００の特徴は次の通り表すことができる。コントローラ２２０は、補助リンクが外部から加えられる力によって受動的に伸縮する力制御モードと、補助リンクの長さが目標長さに一致するように補助リンクを制御する位置制御モードとを切り換えて補助リンク２１３ａを制御することが可能である。コントローラ２２０は、いずれかのリンクの制御異常を検知すると、補助リンク２１３ａを位置制御モードに切り換えて第２部材の姿勢を目標姿勢に一致させる制御に用いる。なお、制御異常が発見されたリンクは、位置制御モードから力制御モードに切り換えられる。

本発明の好適な実施例を説明した。本明細書が開示する技術の留意点について述べる。第１実施例、第２実施例共に、第２部材に許容されている自由度Ｎは「３」であった。本実施例が開示する技術は、Ｎ＝３以外の場合にも適用し得る。但し、当然、１≦Ｎ≦６である。また、実施例のロボットでは、リンクの夫々の端部がボールジョイントによって第１部材と第２部材に連結されている。ボールジョイントに代えていわゆる自在継手（ユニバーサルジョイント）を用いてもよい。

第２実施例のロボットでは、力センサを用いて、補助リンク２１３ａが外力に応じて伸縮する力制御モードを実現した。補助リンクのメカニカルな伸縮摺動抵抗が小さい場合、補助リンクのアクチュエータへの電力供給を遮断することによっても、補助リンクを外力に応じて伸縮させることができる。本明細書における「力制御モード」は、そのように電力遮断によってリンクを受動的に伸縮させ得る場合を含む。例えば、リンクを伸縮させるアクチュエータがエアシリンダの場合、エアを供給するためのポンプへの電力供給を遮断するだけで、「力制御モード」が達成できる。

以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

１１、２１１：第１部材
１２、２１２：第２部材
１３ａ、１３ｂ、１３ｃ：主リンク
１４、２１４：リニアアクチュエータ
１５：補助リンク
１６：バネ
１７ａ、１７ｂ、２１７：リニアエンコーダ
２０、２２０：コントローラ
１００、２００：パラレルリンク型ロボット
２１３ａ、２１３ｂ、２１３ｃ、２１３ｄ：リンク
２１８：力センサ
２３０：支柱

Claims (2)

1. 第１部材に連結されている第２部材のＮ自由度の姿勢を制御することができるパラレルリンク型ロボットであり、
   夫々が第１部材と第２部材を連結しており、アクチュエータによって伸縮するＮ本の主リンクと、
   第１部材と第２部材を連結しており、外部から加えられる力によって受動的に伸縮する補助リンクと、
   補助リンクの長さを計測するセンサと、
   第２部材の姿勢を目標姿勢に一致させるようにアクチュエータによって主リンクの長さを制御するコントローラと、を備えており、
   コントローラが、センサによって計測された補助リンクの長さが、第２部材の姿勢が目標姿勢に一致した場合の補助リンクの予定長さと異なる場合に、リンクの制御に異常が発生していると判断し、
   補助リンクが、補助リンクを伸縮させるアクチュエータを備えており、
   コントローラは、
   補助リンクが外部から加えられる力によって受動的に伸縮する力制御モードと、補助リンクの長さが目標長さに一致するように補助リンクを制御する位置制御モードとを切り換えて補助リンクを制御することが可能であり、
   リンクの制御に異常が発生していると判断した場合に、補助リンクを位置制御モードに切り換えて第２部材の姿勢を目標姿勢に一致させる制御に用いることを特徴とするパラレルリンク型ロボット。
2. コントローラは、
   Ｎ本の主リンクの中から選択したＮ未満の数の主リンクに補助リンクを加えた合計Ｎ本のリンクの長さから第２部材の姿勢を算出し、
   算出された姿勢が目標姿勢と異なる場合にリンクの制御に異常が発生していると判断することを特徴とする請求項１に記載のパラレルリンク型ロボット。

Images