JP4445038B2 - ロボット、ロボットの制御装置及び制御方法、並びに、ロボットの制御装置の制御プログラム - Google Patents

Description

本発明は、人とロボットが協働して作業（協調作業）を行う際のロボット、ロボットの制御装置及び制御方法、並びに、ロボットの制御装置の制御プログラムに関する。

近年、介護ロボット又は家事支援ロボットなどの家庭用ロボットが盛んに開発されるようになってきた。また、産業用ロボットも、今までの人と隔離されたものとは異なり、人との協働作業を行うものが増えている。これらのロボットは、人の近くで作業するため、人との物理的な接触が不可欠である。そのために、安全性の面から、ロボットが機構的に柔らかく、動きについても柔軟である必要がある。

ロボット装置の一例として、人とロボットアームが協働して物体を運搬する装置が提案されている。その中でも、ロボットアームの手先のハンドにおける回転力が零となるように手先の回転運動を制御する一方、物体が水平を維持するように手先の鉛直方向の並進運動を制御する技術が提案されている（特許文献１参照。）

この特許文献１の技術によって、従来課題であった物体の質量が不明であったり、途中で変化したりした場合の協働運搬が実現されている。

特開２０００−３４３４７０号公報

しかしながら、特許文献１のような従来の技術においては、図１３Ａに示すように、ロボットアーム１０１に作用する力とは無関係に、運搬する物体１０３Ａを水平に保つようにロボットアーム１０１が上下に移動するため、運搬する物体１０３Ａの下に他の物体１０３Ｂがあり、なおかつ、人１０２が運搬している物体１０３Ａを下げようとした場合、物体１０３Ａ，１０３Ｂ同士が接触しても、さらにロボットアーム１０１が下に移動しようとする現象が発生しうる。そのために、図１３Ａ、図１３Ｂのように、物体１０３Ａ，１０３Ｂ同士が押し付け合って破損したり、ロボットアーム１０１に過剰な負荷が発生したりするという課題があった。

本発明の目的は、このような課題に鑑みてなされたものであり、人とロボットアームが協働して物体を運搬する際に、運搬する物体の重量が不明であったり、重量が途中で変化したりしても運搬することが可能で、かつ、ロボットアーム又は運搬している物体の下に他の物体があった場合でも、物体同士を必要以上に押し付けあうことのない、安全なロボット制御を実現できる、ロボット、ロボットの制御装置及び制御方法、並びに、ロボットの制御装置の制御プログラムを提供することにある。

前記目的を達成するために、本発明は以下のように構成する。
本発明の第１態様によれば、ロボットアームと、前記ロボットアームの手先に配置されたハンドとを有して、前記人と前記ロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するロボットであって、
前記ハンドの角度を調整する駆動部と、
前記物体と前記ハンドとの間に働く力を検出して出力する力検出部と、
前記ハンドの前記角度を検出して出力する角度検出部と、
前記角度検出部の出力と前記力検出部の出力とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う制御部とを備えるロボットを提供する。
このような構成により、運搬する物体の重量にかかわらず、人の運搬力を推定して、ロボットアームと人とが協働して物体を運搬するロボットの制御が実現できる。

また、本発明の第４態様によれば、人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御装置であって、
前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報を入力する入力部と、
前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う制御部とを備えるロボットの制御装置を提供する。
本構成によって、人が物体を例えば机などに置くときに、安全に水平に、物体を置くことができるロボットの制御が実現できる。

本発明の第９態様によれば、
人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御方法であって、
前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報が入力され、
前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を制御部により行うロボットの制御方法を提供する。

本発明の第１０態様によれば、
人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御プログラムであって、
コンピュータに、
前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報が入力される機能と、
前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を制御部により行う機能とを実現させるための、ロボットの制御装置の制御プログラムを提供する。
これらのような構成により、運搬する物体の重量にかかわらず、人の運搬力を推定して、ロボットアームと人とが協働して物体を運搬するロボットの制御が実現できる。

以上述べたように、本発明のロボット、ロボットの制御装置及び制御方法、並びに、ロボットの制御装置の制御プログラムによれば、ロボットのロボットアームと人とが協働して物体を運搬する場合において、ロボットアームの手先に発生している力とロボットアームの手先のハンドの角度から、物体の重量分をアシストしつつ人が物体に対して鉛直方向にかけている力を前記制御部により推定し、その推定した力に基づき鉛直方向の力制御を前記制御部により行うため、物体の重量が不明な場合又は物体の重量が途中で変化した場合でも、物体の重量を計測することなく、ロボットアームと人とが協働して物体を運搬することができるロボット制御が実現できる。

さらに、ロボットアームの手先に発生している力と手先角度から人が運搬しようとしている力を推定するのと同時に、ロボットアームの手先に発生している力にも基づいて鉛直方向の力制御を行うため、運搬している物体が下にある他の物体と干渉したときに無理に押し付けあうことのない安全なロボット制御が可能となる。

本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、
図１は、本発明の１つの実施形態におけるロボットシステムの構成の概要を示す図である。 図２は、本発明の前記実施形態における前記ロボットシステムを構成するロボット制御装置と制御対象であるロボットアームの詳細構成を示す図である。 図３Ａは、本発明の前記実施形態における前記ロボット制御装置の制御ブロック図である。 図３Ｂは、本発明の前記実施形態における前記ロボット制御装置の別の制御ブロック図である。 図４Ａは、本発明の前記実施形態における前記ロボット制御装置により制御される前記ロボットアームの手先における記号などを説明するための説明図である。 図４Ｂは、本発明の前記実施形態における前記ロボット制御装置により制御される前記ロボットアームの手先におけるロール角、ピッチ角、ヨー角について説明するための説明図である。 図４Ｃは、本発明の前記実施形態における前記ロボット制御装置により制御される前記ロボットアームの手先におけるロール角、ピッチ角、ヨー角について説明するための説明図である。 図４Ｄは、本発明の前記実施形態における前記ロボット制御装置により制御される前記ロボットアームの手先におけるロール角、ピッチ角、ヨー角について説明するための説明図である。 図５は、本発明の前記実施形態における前記ロボットアームの手先における記号などを説明するための説明図である。 図６Ａは、本発明の前記実施形態における前記ロボットアームと人との協働による物体運搬の状態を示す図である。 図６Ｂは、本発明の前記実施形態における前記ロボットアームと人との協働による物体運搬の状態を示す図である。 図６Ｃは、本発明の前記実施形態における前記ロボットアームと人との協働による物体運搬の状態を示す図である。 図６Ｄは、本発明の前記実施形態における前記ロボットアームと人との協働による物体運搬の状態を示す図である。 図７は、本発明の前記実施形態にかかる前記ロボット制御装置における推定パラメータデータベースの一例を表形式で示す図である。 図８は、本発明の前記実施形態における前記ロボット制御装置の運動制御部の物体運搬動作制御を行なうための動作ステップを表すフローチャートである。 図９は、本発明の前記実施形態における前記ロボット制御装置の前記運動制御部の運搬物載置判定動作ステップを表すフローチャートである。 図１０は、本発明の前記実施形態における前記ロボットアームと人との協働による前記物体の載置時の状態を示す説明図である。 図１１は、本発明の前記実施形態における前記ロボット制御装置において、運搬物載置判定部により運搬物載置判定方法の一例を説明するための図（図１１の（ａ）は前記ロボットアームの手先ｚ位置と時間ｔとの関係を表すグラフ、図１１の（ｂ）は前記ロボットアームの角度ω_ｐと時間ｔとの関係を表すグラフ、図１１の（ｃ）は前記ロボットアームの鉛直方向の力Ｆ_ｚと時間ｔとの関係を表すグラフ）である。 図１２は、本発明の前記実施形態の変形例にかかるロボットシステムを示す説明図である。 図１３Ａは、従来例におけるロボット制御装置の概要を示す図である。 図１３Ｂは、従来例におけるロボット制御装置の課題を説明するための図である。

以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。

本発明の第１態様によれば、
ロボットアームと、前記ロボットアームの手先に配置されたハンドとを有して、前記人と前記ロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するロボットであって、
前記ハンドの角度を調整する駆動部と、
前記物体と前記ハンドとの間に働く力を検出して出力する力検出部と、
前記ハンドの前記角度を検出して出力する角度検出部と、
前記角度検出部の出力と前記力検出部の出力とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う制御部とを備えるロボットを提供する。

本発明の第２態様によれば、
前記制御部は、前記角度検出部の出力と前記力検出部の出力とを使用して前記人が前記ロボットに鉛直方向に作用させようとする力を推定し、推定した力で前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行なう第１態様に記載のロボットを提供する。

本発明の第３態様によれば、
前記力検出部は、前記物体と前記ハンドとの間に働く鉛直方向の力を検出して出力し、
前記制御部は、前記角度検出部の出力と前記力検出部の出力とに基づいて前記力制御の鉛直方向の入力を決定し、前記人が前記物体を持ち上げて前記力制御の鉛直方向の入力が上向きとなるとき、前記ロボットアームを上昇させるように力制御を行うとともに、前記人が前記物体を下降させて前記力制御の鉛直方向の入力が下向きとなるとき、前記ロボットアームを下降させるように力制御を行う一方、前記力検出部で検出された前記鉛直方向の力が大きくなるとき前記ロボットアームの上向きの力を増加させるように力制御を行う一方、前記力検出部で検出された前記鉛直方向の力が小さくなるとき前記ロボットアームの上向きの力を減少させるように力制御を行う第１又は２の態様に記載のロボットを提供する。

本発明の第４態様によれば、
人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御装置であって、
前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報を入力する入力部と、
前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う制御部とを備えるロボットの制御装置を提供する。

本発明の第５態様によれば、
前記角度検出部の出力と前記力検出部の出力に基づいて前記人が前記ロボットに鉛直方向に作用させようとする力を推定する運搬力推定部を備え、
前記運搬力推定部にて推定した前記力に基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う第４態様に記載のロボットの制御装置を提供する。

本発明の第６態様によれば、
前記運搬力推定部は、前記力制御の鉛直方向の入力として、前記角度検出部の出力に係数を乗じた値と、前記力検出部の出力の鉛直成分に係数を乗じた値を加算した値を使用する第５態様に記載のロボットの制御装置を提供する。

本発明の第７態様によれば、
さらに、前記ロボットアームの手先の前記ハンドの位置を検出する手先位置検出部を備えるとともに、
前記手先位置検出部の出力と、前記角度検出部の出力と、前記力検出部の出力とから、前記運搬している物体が物体載置面に載置されたか否かを判定する運搬物載置判定部を備え、
前記運搬物載置判定部が前記物体が前記物体載置面に載置されたと判定した際には、前記ロボットアームの鉛直方向の制御を、前記力制御から位置制御に切り替える第４〜６態様のいずれか１つに記載のロボットの制御装置を提供する。

本発明の第８態様によれば、
前記運搬物載置判定部は、前記ロボットアームの前記ハンドが水平面よりも下向きで、前記手先位置検出部の出力が、下降から停止に転じ、かつ前記力検出部の出力のうち、鉛直方向下向きの力が減少に転じたときに、前記運搬している物体が前記物体載置面に載置されたと判定する第７態様に記載のロボットの制御装置を提供する。

本発明の第９態様によれば、
人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御方法であって、
前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報が入力され、
前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を制御部により行うロボットの制御方法を提供する。

本発明の第１０態様によれば、
人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御プログラムであって、
コンピュータに、
前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報が入力される機能と、
前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を制御部により行う機能とを実現させるための、ロボットの制御装置の制御プログラムを提供する。

以下、本発明の１つの実施形態にかかる、ロボット、ロボットの制御装置及び制御方法、並びに、ロボットの制御装置の制御プログラムについて、図面を用いて詳細に説明する。

まず、本発明の１つの実施形態にかかるロボットシステム１の構成について説明する。図１は、本発明の前記実施形態におけるロボットシステム１の構成の概要を示す図である。
図１に示したように、本発明の前記実施形態における、ロボットアーム５を有するロボット５Ａ及びその制御装置４を備えるロボットシステム１は、キッチン又はテーブルなどの作業台近傍の壁面９１にロボットアーム５の基端が載置され、ロボットアーム５の手先のハンド３０は運搬対象の物体３の一端３ａを保持例えば把持可能とし、物体３の一端３ａと相対する他端３ｂを、ロボットアーム５と協働する人２が把持可能とし、人２とロボット５Ａのロボットアーム５の手先のハンド３０とが、人２とロボット５Ａとの間に位置している物体３を保持した状態で人２とロボット５Ａとが協働して物体３を運搬するシステムである。具体的には、ロボットアーム５のハンド３０が物体３の一端（人２とロボット５Ａとの間に位置している物体３のうちの、ロボット５Ａに近い側の把持用部分、例えば、鍋の一方の把手３ａ）を把持しかつ人２が物体３の他端（人２とロボット５Ａとの間に位置している物体３のうちの、人２に近い側の把持用部分、例えば、鍋の他方の把手３ｂ）を把持している状態で、人２が物体３を運びたい方向に力をかけることで、ロボットシステム１のロボットアーム５が移動し、物体３をロボットアーム５と人２とが協働して運搬することができる。

本発明の前記実施形態における物体３は、水若しくは具材の入った鍋、又は、食器、又は、家具などの重量物をも含むとともに、重量が途中で変化する物体（例えば、空の鍋を運搬していて途中で水又は具材を入れる鍋）をも含む概念であり、ロボットアーム５と人２とが保持しながら協働して運搬して載置面に載置することができる、対象物体である。

また、本発明の前記実施形態において、ロボットアーム５の基端が作業台近傍の壁面９１に配置したが、壁面のないアイランドキッチンの場合は、天井面又はアイランドキッチンの作業側面など作業をするのに適した場所に載置される。

図２は、ロボットシステム１を構成する制御装置４と制御対象であるロボットアーム５の詳細構成を示す図である。さらに図４Ａにロボットアーム５の前腕リンク３２、手首部３１、ハンド３０を拡大して示す。

制御装置４は、この前記実施形態では、一例として、一般的なパーソナルコンピュータにより構成されており、制御装置４は、推定パラメータデータベース７と、動作軌道プログラム用データベース３７と、運搬制御部８とで構成される。このうち、図３Ａ及び図３Ｂに示すように、推定パラメータデータベース７と、動作軌道プログラム用データベース３７と、運搬制御部８は、制御プログラム６として構成することが可能なものである。

入出力ＩＦ（インターフェース）９は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットに接続された、例えばＤ／Ａボード、Ａ／Ｄボード、カウンタボードなどを備えるように構成されている。

制御装置４は、入力部の一例としての入出力ＩＦ９を介して、ロボットアーム５の各リンクマニピュレータを駆動するモータドライバ１０と接続され、そのモータドライバ１０に制御信号を送る。

ロボットアーム５の動作を制御する制御装置４が実行されることにより、ロボットアーム５の各関節部の後述するエンコーダ１１より出力される各関節角度情報、及び、力情報を検出しかつ後述する力検出部１３より出力されるロボットアーム５の手先（ハンド３０）に発生している力情報が、入出力ＩＦ９のカウンタボードを通じて制御装置４に取り込まれ、取り込まれた各関節角度情報及び力情報に基づき制御装置４によって各関節部の回転動作での制御指令値が算出される。算出された各制御指令値は、入出力ＩＦ９のＤ／Ａボードを通じて、ロボットアーム５の各関節部を駆動制御するためのモータドライバ１０に与えられ、モータドライバ１０から送られた各制御指令値に従って、ロボットアーム５の各関節部のモータ１２が駆動される。このモータドライバ１０とモータ１２とで駆動部の一例として機能する。また、エンコーダ１１は、角度情報を出力する角度検出部の一例として機能する。

ロボットアーム５は、一例として、６自由度の多リンクマニピュレータであり、ハンド３０と、ハンド３０が取り付けられている手首部３１を先端に有する前腕リンク３２と、前腕リンク３２の基端に先端が回転可能に連結される上腕リンク３３と、上腕リンク３３の基端が回転可能に連結支持され壁面９１に固定される台部３４とを備えている。

手首部３１は、第４関節部２７、第５関節部２８、第６関節部２９の３つの回転軸を有しており、前腕リンク３２に対するハンド３０の相対的な姿勢（向き）を変化させることができる。すなわち、図２，図４Ａにおいて、第４関節部２７は、手首部３１に対するハンド３０のΨ_ｘ軸（縦軸）周りの相対的な姿勢を変化させることができる。第５関節部２８は、手首部３１に対するハンド３０の、第４関節部２７のΨ_ｘ軸とは直交するΨ_ｙ軸（横軸）周りの相対的な姿勢を変化させる。第６関節部２９は、手首部３１に対するハンド３０の、第４関節部２７のΨ_ｘ軸及び第５関節部２８のΨ_ｙ軸とそれぞれ直交する手先先端方向の軸Ψ_ｚ（横軸）周りの相対的な姿勢を変化させることができる。すなわち、ハンド３０は、前腕リンク３２の一端３２ａに対してΨ_ｘ軸，Ψ_ｙ軸，Ψ_ｚ軸の３軸方向にそれぞれ独立して回転可能である。ここで、Ψ_ｘ軸，Ψ_ｙ軸，Ψ_ｚ軸は、互いに直交する座標軸である。また、図２における、後述する手先座標系３６とΨ_ｘ軸，Ψ_ｙ軸，Ψ_ｚ軸は必ずしも一致しない。

前腕リンク３２の他端３２ｂは、上腕リンク３３の一端３３ａに対して第３関節部２６周りに回転可能とする。前記実施形態では、第３関節部２６周りに回転可能とは、第５関節部２８のΨ_ｙ軸と平行な横軸周りに回転可能を意味する。上腕リンク３３の他端３３ｂは、台部３４に対して第２関節部２５周りに回転可能とする。前記実施形態では、第２関節部２５周りに回転可能とは、第５関節部２８のΨ_ｙ軸（第３関節部２６の横軸）と平行な横軸周りに回転可能を意味する。台部３４の上側可動部３４ａは、台部３４の下側固定部３４ｂに対して第１関節部２４周りに回転可能とする。前記実施形態では、第１関節部２４周りに回転可能とは、絶対座標系３５のｚ軸周りに回転可能を意味している。

この結果、ロボットアーム５は、第１関節部２４から第６関節部２９の合計６個の軸周りにそれぞれ独立して回転可能として、前記６自由度の多リンクマニピュレータを構成している。

各軸の回転部分を構成する各関節部には、各関節部を構成する一対の部材（例えば、回動側部材と、該回動側部材を支持する支持側部材）のうちの一方の部材に備えられ、かつ後述するモータドライバ１０により駆動制御される回転駆動装置の一例としてのモータ１２（実際には、ロボットアーム５の各関節部の内部に配設されている）と、モータ１２の回転軸の回転位相角（すなわち、関節角）を検出するエンコーダ１１（実際には、ロボットアーム５の各関節部の内部に配設されている）とを備える。よって、各関節部を構成する一方の部材に備えられたモータ１２の回転軸が、各関節部の他方の部材に連結されて前記回転軸を正逆回転させることにより、他方の部材を一方の部材に対して各軸周りに回転可能とする。

３５は、台部３４の下側固定部３４ｂに対して相対的な位置関係が固定された絶対座標系であり、３６はハンド３０に対して相対的な位置関係が固定された手先座標系である。絶対座標系３５から見た手先座標系３６の原点位置Ｏ_ｅ（ｘ，ｙ，ｚ）を、ロボットアーム５のハンド３０の位置（すなわち、手先位置）とする。そして、絶対座標系３５から見た手先座標系３６の姿勢を、ロール角とピッチ角とヨー角を使用して座標（φ，θ，ψ）で表現する。この座標（φ，θ，ψ）を、ロボットアーム５の手先姿勢（ハンド３０の姿勢）とし、手先位置及び姿勢ベクトルを、ベクトルｒ＝［ｘ、ｙ、ｚ、φ、θ、ψ］^Ｔと定義する。ロール角、ピッチ角、ヨー角について図４Ｂ〜図４Ｄを用いて説明する。まず、絶対座標系３５のＺ軸を回転軸として、座標系を角度φだけ回転させた座標系を考える（図４Ｂ参照）。このときの座標軸を［Ｘ’、Ｙ’、Ｚ］とする。次に、この座標系を、Ｙ’を回転軸として角度θだけ回転させる（図４Ｃ参照）。このときの座標軸を［Ｘ'’、Ｙ’、Ｚ'’］とする。最後に、この座標系を、Ｘ'’を回転軸として角度ψだけ回転させる（図４Ｄ参照）。このときの座標系の姿勢をロール角度φ、ピッチ角度θ、ヨー角度ψとし、このときの姿勢ベクトルは（φ，θ，ψ）となる。姿勢（φ，θ，ψ）の座標系が原点位置を手先座標系３６の原点位置Ｏ_ｅ（ｘ，ｙ，ｚ）に平行移動した座標系と、手先座標系３６が一致する場合、手先座標系の姿勢ベクトルは（φ，θ，ψ）であるとする。

ロボットアーム５の手先位置及び姿勢を制御する場合には、手先位置及び姿勢ベクトルｒを、後述する目標軌道生成部１８で生成された手先位置及び姿勢目標ベクトルｒ_ｄに運搬制御部８により追従させることになる。

力検出部１３は、ロボットアーム５のハンド３０と運搬物体３の間に発生する力を検出もしくは推定して、その情報を出力する。一例として、力検出部１３は、ハンド３０と手首部３１の間にある６軸の力センサを想定するが、物体３の運搬方向が限定される場合などは６軸以下でもかまわない。ただし、少なくとも、常に鉛直方向の力の検出ができる手段又は装置が必要となる。力検出部１３からの出力は、推定パラメータデータベース７、運搬力推定部１４、目標軌道生成部１８、及び、運搬物載置判定部（運搬物体載置判定部）１９に出力される。

また、力検出部１３の別の例として、センサを用いず、モータ電流値などから推定する力推定部（一例として、図３Ｂの１３Ａを参照）として構成するようにしてもよい。この場合、力検出部１３（力推定部１３Ａ）による外力の推定は、次の方法で推定する。モータドライバ１０の電流センサ１０ａは、ロボットアーム５の各関節部を駆動するモータ１２を流れる電流値ｉ＝［ｉ_１，ｉ_２，ｉ_３，ｉ_４，ｉ_５，ｉ_６］^Ｔを計測する。力検出部１３（力推定部１３Ａ）には、モータドライバ１０の電流センサで計測された電流値ｉ＝［ｉ_１，ｉ_２，ｉ_３，ｉ_４，ｉ_５，ｉ_６］^Ｔが、入出力ＩＦ９のＡ／Ｄボードを介して取り込まれる。また、関節角の現在値ｑが、入出力ＩＦ９のカウンタボードを介して力検出部１３（力推定部１３Ａ）に取り込まれるとともに、後述する近似逆運動学計算部２２からの関節角度誤差補償出力ｕ_ｑｅが力検出部１３（力推定部１３Ａ）に取り込まれる。

力検出部１３（力推定部１３Ａ）は、オブザーバーとして機能し、前記電流値ｉ、関節角の現在値ｑ、関節角度誤差補償出力ｕ_ｑｅの各情報から、ロボットアーム５に加わる外力によって各関節部に発生するトルクτ_ｅｘｔを算出する。そして、さらに、力検出部１３（力推定部１３Ａ）は、Ｆ_ｅｘｔ＝Ｊ_ｖ（ｑ）^−Ｔτ_ｅｘｔにより、ロボットアーム５の手先における等価手先外力Ｆ_ｅｘｔに換算して、換算後の力Ｆ_ｅｘｔを推定パラメータデータベース７と運搬力推定部１４と目標軌道生成部１８と運搬物載置判定部１９とに出力する。ここで、Ｊ_ｖ（ｑ）は、

を満たすヤコビ行列である。ただし、ｖ＝［ｖ_ｘ、ｖ_ｙ、ｖ_ｚ、λ_ｘ、λ_ｙ、λ_ｚ］^Ｔであり、（ｖ_ｘ、ｖ_ｙ、ｖ_ｚ）は手先座標系３６でのロボットアーム５の手先の並進速度、（λ_ｘ、λ_ｙ、λ_ｚ）は手先座標系３６でのロボットアーム５の手先の角速度である。

図３Ａ及び図３Ｂは運搬制御部８の詳細構成を示す図である。運搬制御部８は、運搬力推定部１４と、力制御部１５と、運搬物載置判定部１９とを備えて構成される。力制御部１５は、インピーダンス制御部１７と、位置誤差計算部４０と、位置制御部１６と、目標軌道生成部１８とを備えて構成される。さらに、位置制御部１６は、位置誤差補償部２０と、順運動学計算部２１と、近似逆運動学計算部２２とを備えて構成され、ロボットアーム５の目標軌道、並びに、ロボットアーム５の現在位置及び姿勢から、位置制御部１６によりロボットアーム５の指令値を出力する。なお、力制御系は、上記方法以外にもハイブリッド制御などのその他の力制御方法を使ってもよい。

ロボットアーム５からは、それぞれの関節部のエンコーダ１１により計測された関節角の現在値（関節角度ベクトル）ベクトルｑ＝［ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５，ｑ_６］^Ｔが出力され、入出力ＩＦ９のカウンタボードにより運搬制御部８に取り込まれる。ただし、ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５，ｑ_６は、それぞれ、エンコーダ１１から検出された、第１関節部２４、第２関節部２５、第３関節部２６、第４関節部２７、第５関節部２８、第６関節部２９の関節角度である。

目標軌道生成部１８は、手先位置及び姿勢ベクトルｒと動作軌道プログラム用データベース３７と力検出部１３からの力情報と後述する運搬物載置判定部１９からの運搬物載置判定情報を基に、目標とするロボットアーム５の動作を実現するための手先位置及び姿勢目標ベクトルｒ_ｄを出力する。事前に決められた軌道に沿って位置制御を行う場合は、目標とするロボットアーム５の動作は、目的とする作業に応じてそれぞれの時間（ｔ＝０、ｔ＝ｔ_１、ｔ＝ｔ_２、・・・）でのポイントごとの位置（ｒ_ｄ０、ｒ_ｄ１、ｒ_ｄ２、・・・）が動作軌道プログラム用データベース３７に記憶された動作軌道プログラムに事前に与えられており、目標軌道生成部１８は、動作軌道プログラム用データベース３７からの前記それぞれの時間（ｔ＝０、ｔ＝ｔ_１、ｔ＝ｔ_２、・・・）でのポイントごとの位置（ｒ_ｄ０、ｒ_ｄ１、ｒ_ｄ２、・・・）の情報と手先位置及び姿勢ベクトルｒとを基に多項式補間を使用し、各ポイント間の軌道を補完し、手先位置及び姿勢目標ベクトルｒ_ｄを生成する。また、力制御を行う場合は、力制御開始時から手先位置及び姿勢目標ベクトルｒ_ｄの更新を停止することで、後述する手先位置及び姿勢目標補正出力ｒ_ｄΔによって力制御が実現される。なお、位置及び姿勢のうち一部のみ力制御を行いたい場合は、力制御を行いたい成分のみ、更新を停止することで、実現できる。

インピーダンス制御部１７は、ロボットアーム５に、機械インピーダンス設定値への上記ロボットアーム５の機械インピーダンスの値の制御を実現する機能を果たす部分であり、後述する運搬物載置判定部１９からの運搬物載置判定情報が入力されるとともに、実験によって事前に設定されたインピーダンスパラメータである慣性Ｍ、粘性Ｄ、剛性Ｋと、運搬力推定部１４が出力した運搬力Ｆ_{ｃａｒｒｙ}とから、上記ロボットアーム５の機械インピーダンスの値の機械インピーダンス設定値への制御を実現するための手先位置及び姿勢目標補正出力ｒ_ｄΔを以下の式（２）により計算し、位置制御部１６に向けて出力する。手先位置及び姿勢目標補正出力ｒ_ｄΔは、目標軌道生成部１８の出力する手先位置及び姿勢目標ベクトルｒ_ｄに位置誤差計算部４０により加算され、手先位置及び姿勢補正目標ベクトルｒ_ｄｍが生成される。

ただし、

であり、ｓはラプラス演算子である。

順運動学計算部２１には、ロボットアーム５の各関節部のエンコーダ１１により計測された関節角の現在値ｑである関節角度ベクトルｑを、入出力ＩＦ９のカウンタボードを介して入力される。そして、順運動学計算部２１では、ロボットアーム５の関節角度ベクトルｑから手先位置及び姿勢ベクトルｒへの変換の幾何科学的計算を行う。よって、順運動学計算部２１は、ロボットアーム５の手先位置を検出する手先位置検出部の一例として機能する。順運動学計算部２１で計算された手先位置及び姿勢ベクトルｒは、目標軌道生成部１８と運搬物載置判定部１９と位置誤差補償部２０と運搬力推定部１４とに出力される。

位置誤差補償部２０は、ロボットアーム５において計測される関節角度ベクトルｑから順運動学計算部２１により計算される手先位置及び姿勢ベクトルｒと、手先位置及び姿勢補正目標ベクトルｒ_ｄｍとの誤差ｒ_ｅ（＝ｒ_ｄｍ−ｒ）が入力され、位置誤差補償出力ｕ_ｒｅが、位置誤差補償部２０から近似逆運動学計算部２２に向けて出力される。

近似逆運動学計算部２２では、位置誤差補償部２０から入力される位置誤差補償出力ｕ_ｒｅとロボットアーム５において計測される関節角度ベクトルｑとに基づき、近似式ｕ_ｏｕｔ＝Ｊ_ｒ（ｑ）^−１ｕ_ｉｎにより、逆運動学の近似計算を行う。ただし、Ｊ_ｒ（ｑ）は、

の関係を満たすヤコビ行列、ｕ_ｉｎは近似逆運動学計算部２２への入力、ｕ_ｏｕｔは近似逆運動学計算部２２からの出力であり、入力ｕ_ｉｎを関節角度誤差ｑ_ｅとすれば、ｑ_ｅ＝Ｊ_ｒ（ｑ）^−１ｒ_ｅのように手先位置及び姿勢誤差ｒ_ｅから関節角度誤差ｑ_ｅへの変換式となる。したがって、位置誤差補償部２０から位置誤差補償出力ｕ_ｒｅが近似逆運動学計算部２２に入力されると、近似逆運動学計算部２２からの出力として、関節角度誤差ｑ_ｅを補償するための関節角度誤差補償出力ｕ_ｑｅが近似逆運動学計算部２２からロボット５Ａのモータドライバ１０に向けて出力される。

関節角度誤差補償出力ｕ_ｑｅは、入出力ＩＦ９のＤ／Ａボードを介してロボット５Ａのモータドライバ１０に電圧指令値として与えられ、それぞれのモータ１２により各関節部が正逆回転駆動されロボットアーム５が動作する。

以上のように構成されるインピーダンス制御部１７に関して、ロボットアーム５のインピーダンス制御動作の原理について説明する。

インピーダンス制御動作の基本は、位置誤差補償部２０による手先位置及び姿勢誤差ｒ_ｅのフィードバック制御（位置制御）であり、図３Ａ及び図３Ｂの点線で囲まれかつ参照符号１６により示された部分が位置制御部１６になる。位置誤差補償部２０として、例えば、ＰＩＤ補償器を使用すれば、手先位置及び姿勢誤差ｒ_ｅが０に収束するように位置制御部１６による制御が働き、目標とするロボットアーム５のインピーダンス制御動作を実現することができる。

運搬力推定部１４は、推定パラメータデータベース７からの出力情報、力検出部１３からの出力情報、及び、ロボットアーム５の手先の姿勢情報ｑすなわち関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）から、ロボットアーム５と人２との協働運搬に必要な運搬力Ｆ_{ｃａｒｒｙ}を推定する。一例を、図５、図６Ａ〜６Ｂを用いて説明する。図５において参照符号２３で示すω_ｐは、ロボットアーム５の手先が水平面となす角度であって、推定パラメータデータベース７により、手先位置及び姿勢ベクトルｒから決定される角度を示している。ここでは、この角度ω_ｐにおいて、図５の矢印の向き（反時計回り）を正とする。図６Ａで示すように、人２の腕２ａとロボットアーム５が物体３の両方の把手３ａ，３ｂをそれぞれ支えているときに力検出部１３で検出する鉛直方向の力をＦ_ｚ（≦０）とする。鉛直方向の力Ｆ_ｚの向きは、図２の絶対座標系３５のｚ軸の向きを正とする。このときの力制御の鉛直方向の入力（運搬力推定部１４から力制御部１５への力制御の入力のうち、鉛直方向成分）を

とする。ここで、ｋ_ｐ及びｋ_ωは、一例として、力検出部１３で検出した鉛直方向の力Ｆ_ｚの値と、ロボットアーム５の手先が水平面となす角度ω_ｐの値の正負で決定されるパラメータであり、力検出部１３で検出した鉛直方向の力Ｆ_ｚの値と、ロボットアーム５の手先が水平面となす角度ω_ｐの値の正負で決定されるパラメータ（係数）である。なお、ｋ_ｐ及びｋ_ωは、角度ω_ｐの値の正負によることなく、角度ω_ｐの値でもっと細かくパラメータ設定することも可能であり、鉛直方向の力Ｆ_ｚと角度ω_ｐから一意に決定するパラメータと広く定義することもできる。

図６Ａで示すように、−ｋ_ｐＦ_ｚ０＋（−ｋ_ωω_ｐ０）＝０のとき、力制御の入力が０となるので、この状態が釣り合い点となり、静止する。ここで、ω_ｐ0は、人２が物体３を持っており、図６Ａに示すように、ロボットアーム５と人２との間で物体３が釣り合って保持されているときのロボットアーム５の手先の水平面に対する角度を意味する。また、Ｆ_ｚ０は、人２が物体３を持っており、図６Ａに示すように、ロボットアーム５と人２との間で物体３が釣り合って保持されているときのロボットアーム５の手先の鉛直方向の力である。

図６Ｂで示すように、図６Ａの釣り合いの状態から、人２が、物体３の一方の把手３ｂを上向きに持ち上げて、（ω_ｐ＝ω_ｐ＋の状態（水平面に対して角度ω_ｐ＋だけ、人２が物体３の把手３ｂを持ち上げた状態）となったとする。このとき、−ｋ_ｐＦ_ｚ＋ｋ_ωω_ｐ＋＞０となり、力制御の鉛直方向の入力は上向きとなるので、ロボットアーム５は上昇する。逆に、図６Ｃで示すように、図６Ａの釣り合いの状態から、人２が下向きの力を増加させ、（ω_ｐ＝ω_ｐ−の状態（水平面に対して角度ω_ｐ−だけ、人２が物体３の把手３ｂを下げた状態）となったとする。このとき、−ｋ_ｐＦ_ｚ＋ｋ_ωω_ｐ−＜０となり、力制御の鉛直方向の入力は下向きとなるので、ロボットアーム５は下降する。

このように、運搬力推定部１４において、前記式（７）を、力制御の鉛直方向の入力とすることで、人２が上下に動かしたい方向にロボットアーム５も協働して動くことができ、ロボットアーム５と人２との物体３の協働運搬が実現できる。また、この技術では、運搬する物体３の質量情報を事前に必要としない。さらに、物体３の重量が変化した場合、力検出部１３で検出する鉛直方向の力Ｆ_ｚが変化する。その結果、物体３の重量が増えた場合には、ロボットアーム５は上向きの力を増加させる一方、物体３の重量が減少した場合には、ロボットアーム５は上向きの力を減少させるので、物体３が重量変化をしても、何ら支障無く、協働運搬動作を行なうことができる。

もし、運搬する物体３の重量の変動が比較的少ない場合には（例えば、物体３の重量の変動幅の閾値を５ｋｇと予め設定しているとき、運搬する物体の重量が５ｋｇ以下であれば）（言い換えれば、人２側で吸収できる程度の物体３の重量変動であれば）、前記式（７）において、ｋ_ｐ＝０として、ｋ_ωω_ｐのみを用いてもよい。一般に、力検出部１３の出力にはノイズがあるため、ｋ_ωω_ｐのみとすることで（すなわち、ノイズの原因をとることで）、ノイズによる振動要因を減らすことが可能となる。

図６Ｄで示すように、運搬する物体３もしくはロボットアーム５のハンド３０の下に、運搬する物体３と、干渉する物体４３とがある状況では、運搬する物体３と、干渉する物体４３との間に発生する力

によって、ロボットアーム５が受ける鉛直方向の下向きの力Ｆ_ｚは０に近くなる（例えば、０．５Ｎ程度になる）。

ここで、干渉する物体４３がない場合には、ロボットアーム５が負担する物体３の重量は、常に鉛直方向下向きの力として検出される。もし、力検出部１３で検出する鉛直方向の力Ｆ_ｚが０より大きい場合は、運搬する物体３もしくはロボットアーム５のハンド３０の下に、干渉する物体４３があると、運搬力推定部１４では判断し、安全のために、運搬力推定部１４は、力検出部１３で検出した鉛直方向の力を、そのまま出力する。これにより、運搬する物体３もしくはロボットアーム５のハンド３０と、干渉する物体４３とが押し付け合うことが防止できる。

これらのことから、力検出部１３で検出した鉛直方向の力をＦ_ｚとし、また、力検出部１３のノイズ又は誤差の影響を考え（ただし、物体３とロボットアーム５のハンド３０との間はハンド３０による物体３のチャッキングなどの把持により相対的に不動である。）、運搬力推定部１４における、ノイズ又は誤差の影響を考慮するか否かの切り替えの閾値を−εとすると、鉛直方向の運搬力

は、

と決まる。物体３の重量により変化する力は、鉛直方向の力、及び、ピッチ軸周りのトルクであるため（図４Ｂ〜図４Ｄ参照）、それ以外の力に関しては、力検出部１３で検出した力を、そのまま、運搬力推定部１４から出力する。力検出部１３で検出した６軸の力を

とすると、運搬力推定部１４の出力は、

となる。ここで、

は、ロボットアーム５の手先から、運搬する物体３の重心までの距離である。

推定パラメータデータベース７は、力検出部１３の出力情報Ｆ_ｚ及びロボットアーム５の手先の姿勢情報ω_ｐに基づいて、パラメータｋ_ｐ，ｋ_ωをそれぞれ適宜選択し、運搬力推定部１４に出力する。

推定パラメータデータベース７の一例を図７に示す。この推定パラメータデータベース７では、ロボットアーム５の手先に予め発生している力の鉛直成分Ｆ_ｚとロボットアーム５の手先が水平面となす角度ω_ｐとの組合せに対応して、パラメータｋ_ｐ，ｋ_ωの値がそれぞれ予め設定されて記憶されている。よって、前記力の鉛直成分と角度ω_ｐの情報が推定パラメータデータベース７に入力されると、推定パラメータデータベース７から、パラメータｋ_ｐ，ｋ_ωの値をそれぞれを出力する。例えば、Ｆ_ｚ＝−１０であり、また、ω_ｐ＝０．１（すなわち、ω_ｐが正）のとき、ｋ_ｐ＝０．５２，ｋ_ω＝３２を推定パラメータデータベース７から出力する。

次に、運搬物載置判定部１９の詳細について説明する。前記実施形態の場合、ノイズ、又は、力検出部１３の一例である力センサのオフセットずれなどにより、図１０のように、人２が、台９０の上面（すなわち、物体載置面９０ａ）の上に、運搬する物体３を置こうとしたにもかかわらず、ロボットアーム５のハンド３０との力の釣り合いにより、物体３が傾いた状態になってしまうことが起こりうる。具体的には、例えば、物体３の一方の把手３ａはロボットアーム５のハンド３０に把持されたままで、物体３の他方の把手３ｂを持つ人２の腕２ａを下げたことにより、物体３が人２側に傾いた状態になってしまうことが起こりうる。

そこで、運搬物載置判定部１９により、運搬中の物体３が物体載置面９０ａに載置したか否かを判定するようにしている。運搬物載置判定部１９における、運搬物載置判定方法の一例を、図１１の（ａ）〜（ｃ）のグラフを参照しながら説明する。

まず、人２が台９０の物体載置面９０ａの上に、運搬する物体３を置こうとする直前には、一般的に、運搬する物体３は、上から下へと移動し、ある位置で静止する。このような動作は、ロボットアーム５の手先位置のうちのｚ位置（手先位置及び姿勢ベクトルｒのうちのｚ成分）の変動を検出することにより、運搬物載置判定部１９で判定することができる。具体的には、図１１において、手先ｚ位置と時間ｔとの関係を表すグラフ（ａ）に示すように、ロボットアーム５の手先ｚ位置（ｚ方向の手先位置）に着目すると、物体３の載置の瞬間である時間ｔ_１以前の期間でのｚ位置は、下降し続けており、物体３の載置の瞬間である時間ｔ_１より後の期間でのｚ位置は、下降も上昇もせず、一定となる。

よって、順運動学計算部２１から入力される手先ｚ位置が下降し続けたのち、ある時間ｔ_１より後の期間でのｚ位置は、下降も上昇もせず、一定となったことを運搬物載置判定部１９が検出すると、運搬物載置判定部１９は、運搬物が載置されたと判定するようにしている。ｚ位置が一定となるとの判定は、例えば図１１の（ａ）のように、一定時間の間、ｚ位置が一定範囲Δｚ内にある場合に一定であると判定する。一定範囲Δｚは、事前の実験などにより、例えば１０ｍｍ等に予め設定する。それ以外にも、ｚの時間微分を算出し、ｚの時間微分の絶対値が閾値以下の場合、すなわちｚの速度が閾値以下の場合にｚが一定であるなどとしてもよい。

また、前記した人２が台９０の物体載置面９０ａの上に、運搬する物体３を置こうとする直前には、図１０に示すように、物体３の一方の把手３ａはロボットアーム５のハンド３０に把持されたままで、物体３の他方の把手３ｂを持つ人２の腕２ａを下げることになる。よって、物体３の他方の把手３ｂを持つ人２の腕２ａを下げたことにより、物体３が人２側に傾いたときは、ロボットアーム５の手先が水平面となす角度ω_ｐは、水平面よりも下向きとなっており、これを検出することにより、人２が運搬する物体３を物体載置面９０ａに載置しようとし始めたと判定することができる。具体的には、図１１において、角度ω_ｐと時間ｔとの関係を表すグラフ（ｂ）に示すように、時間ｔ_１以降の期間は、角度ω_ｐが負の値で一定となる。

よって、運搬物載置判定部１９内で位置及び姿勢ベクトルｒから角度ω_ｐを算出し、ある時間ｔ_１以降の期間では、角度ω_ｐが負の値で一定となったことを運搬物載置判定部１９が検出すると、運搬物載置判定部１９は、運搬物が人２側に傾いたと判定し、人２が運搬する物体３を物体載置面９０ａに載置しようとし始めたと判定することができる。

ここで言う一定とは、前述のｚ位置と同様の手法において、判定できる。また、力検出部１３から運搬物載置判定部１９に入力される、ロボットアーム５と物体３の間に発生する鉛直方向の力Ｆ_ｚは、人２が、物体３の把手３ｂを持つ腕２ａを水平面より下向きに動かしているときは、前記鉛直方向の力Ｆ_ｚは下向きの値を持ち、物体３を前記載置面９０ａに載置するときには前記鉛直方向の力Ｆ_ｚの絶対値は小さくなり、０に近づく。載置後、前記鉛直方向の力Ｆ_ｚは負の値で一定状態となる。これを検出することにより、人２が運搬する物体３を物体載置面９０ａに載置しようとし、その後、載置したと判定することができる。具体的には、図１１において、鉛直方向の力Ｆ_ｚと時間ｔとの関係を表すグラフ（ｃ）に示すように、鉛直方向の力Ｆ_ｚは、物体３の載置の瞬間である時間ｔ_１以前の期間では、前記鉛直方向の力Ｆ_ｚは下向きの値を持ち、物体３の載置の瞬間である時間ｔ_１前の期間に、前記鉛直方向の力Ｆ_ｚは上昇し、物体３の載置の瞬間である時間ｔ_１より後の期間は、前記鉛直方向の力Ｆ_ｚは負の値で一定となる。

力検出部１３から入力される前記鉛直方向の力Ｆ_ｚが下向きの値を持ち、ある時間ｔ_１前の期間に、前記鉛直方向の力Ｆ_ｚが減少し、前記ある時間ｔ_１より後の期間は、前記鉛直方向の力Ｆ_ｚが負の値で一定となったことを運搬物載置判定部１９により検出したときには、運搬物載置判定部１９は、運搬物３が載置されたと判定するようにしている。前記鉛直方向の力Ｆ_ｚが減少したとの運搬物載置判定部１９による判定は、前記鉛直方向の力Ｆ_ｚを時間微分した値が、ある閾値以上のときに、減少したと、運搬物載置判定部１９により判定する。閾値は、事前の実験などにより、例えば５Ｎ/ｓ等に予め設定する。また、一定となったとの判定は、前記ｚ方向の位置と同様の手法において、判定できる。

以上のことから、運搬物載置判定部１９は、ロボットアーム５の手先ｚ位置の情報と、ロボットアーム５の手先が水平面となす角度ω_ｐの情報と、力検出部１３の出力のうちの鉛直方向の力情報Ｆ_ｚとから、前記角度検出部の一例であるエンコーダ１１の出力に基づき前記ロボットアーム５の手先の前記ハンド３０が水平面よりも下向きに傾斜していると判定し、かつ、前記手先位置検出部の一例である前記順運動学計算部２１の出力に基づき手先ｚ位置の変動が下降から一定に転じていると判定し、かつ、前記力検出部１３の出力に基づき、鉛直方向下向きの力が減少に転じたと判定したとき（図１１では、「物体３が載置する瞬間である時間ｔ_１から一定時間Δｔ経過した後の時間ｔ_２のとき）、物体３は載置面９０ａに載置された」と、運搬物載置判定部１９により、みなす。一定時間Δｔは、事前の実験などにより、例えば１秒等に予め設定する。

次に、運搬物載置判定部１９が、「物体３の物体載置面９０ａへの載置が解除された」とみなすときについて述べる。一般に、物体３を載置する場所は水平面であることが想定されるため、ロボットアーム５の手先角度は、水平方向よりも下向きから水平方向までの間にあると考えられる。このため、運搬物載置判定部１９により、「物体３が物体載置面９０ａへ載置中である」と判定している間、運搬物載置を意味する信号を出力し続けるとともにロボットアーム５の手先角度をエンコーダ１１から取得し続けて、ロボットアーム５の手先角度が、水平面よりも上向きになった時点で、「物体３の載置が解除された」と運搬物載置判定部１９がみなし、運搬物載置判定部１９は、運搬物載置を意味する信号の出力を停止する。また、ロボットアーム５と物体３の間に発生する鉛直方向の力Ｆ_ｚは、物体３が載置面９０ａに載置したとしても、一般に、下向きの力から０までの間にある。よって、この範囲以外の前記鉛直方向の力Ｆ_ｚを力検出部１３により検出し（すなわち、前記鉛直方向の力Ｆ_ｚが上向きの力となるのを力検出部１３により検出し）、その検出情報が運搬物載置判定部１９に入力されたときには、「ロボットアーム５のハンド３０が載置面９０ａに物体３を押し付けている状態である」か、又は、「人２が物体３を上方向に持ち上げている状態である」と、運搬物載置判定部１９は、みなすことができるため、このときも「物体３の載置が解除された」と、運搬物載置判定部１９では、みなす。

以上の運搬制御部８の動作ステップについて、図８、図９のフローチャートに基づいて説明する。これらの動作ステップの機能は、制御プログラム６（図２参照）としてコンピュータにそれぞれ実行させることが可能なものである。

初めに、運搬物載置判定部１９が動作していない場合について図８のフローチャートに基づいて説明する。

ロボットアーム５の関節部のそれぞれのエンコーダ１１により計測された関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）が、エンコーダ１１から制御装置４に取り込まれる（ステップＳ１）。

次いで、制御装置４に取り込まれた関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）に基づいて、近似逆運動学計算部２２にて、ロボットアーム５の運動学計算に必要なヤコビ行列Ｊ_ｒ等の計算を行う（ステップＳ２）。

次いで、順運動学計算部２１にて、ロボットアーム５のそれぞれのエンコーダ１１からの関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）から、ロボットアーム５の現在の手先位置及び姿勢ベクトルｒを計算して、推定パラメータデータベース７と位置誤差計算部４０と目標軌道生成部１８などに出力する（ステップＳ３）。

次いで、力検出部１３は、ロボットアーム５に取り付けられた力センサの出力を元に、手先に発生している力Ｆ_ｅｘｔを計算して、推定パラメータデータベース７と運搬力推定部１４と目標軌道生成部１８などに出力する（ステップＳ４）。

次いで、運搬力推定部１４は、ステップＳ３で順運動学計算部２１により計算して求められた手先位置及び姿勢ベクトルｒから、角度ω_ｐを決定する。図４Ｂ〜図４Ｄのように手先姿勢を定義した場合、角度ω_ｐ＝θである。さらに、運搬力推定部１４は推定パラメータデータベース７に角度ω_ｐを出力する。（ステップＳ５）。

次いで、力検出部１３の出力である力Ｆ_ｅｘｔ及び前記角度ω_ｐから、パラメータｋ_ｐ及びｋ_ωの値を推定パラメータデータベース７によりそれぞれ決定して、推定パラメータデータベース７から運搬力推定部１４へそれぞれ出力する（ステップＳ６）。

次いで、運搬力推定部１４は、パラメータｋ_ｐ及びｋ_ωと、手先に発生している力Ｆ_ｅｘｔと、角度ω_ｐと、関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）とから、運搬力Ｆ_{ｃａｒｒｙ}を計算して、インピーダンス制御部１７へ出力する（ステップＳ７）。

次いで、予め記憶されていたロボットアーム５の動作軌道プログラム用データベース３７の動作軌道プログラムとステップＳ３で順運動学計算部２１により計算して求められた手先位置及び姿勢ベクトルｒとに基づき、目標軌道生成部１８は、ロボットアーム５の手先位置及び姿勢目標ベクトルｒ_ｄを計算する（ステップＳ８）。

次いで、インピーダンス制御部１７は、実験によって予め設定された機械インピーダンスパラメータの慣性Ｍ、粘性Ｄ、剛性Ｋと、運搬力推定部１４により計算されたロボットアーム５に加わる等価手先外力Ｆ_{ｃａｒｒｙ}とから、手先位置及び姿勢目標補正出力ｒ_ｄΔを、インピーダンス制御部１７により計算して、位置誤差計算部４０に出力する（ステップＳ９）。

次いで、位置誤差計算部４０は、目標軌道生成部１８からの手先位置及び姿勢目標ベクトルｒ_ｄとインピーダンス制御部１７からの手先位置及び姿勢目標補正出力ｒ_ｄΔとの和（ｒ_ｄ＋ｒ_ｄΔ）である、手先位置及び姿勢補正目標ベクトルｒ_ｄｍを計算する。次いで位置誤差計算部４０は、手先位置及び姿勢補正目標ベクトルｒ_ｄｍと順運動学計算部２１からの現在の手先位置及び姿勢ベクトルｒとの差（ｒ_ｄｍ−ｒ）である、手先位置及び姿勢の誤差ｒ_ｅを、計算し位置誤差補償部２０に出力する。最後に、位置誤差補償部２０は手先位置及び姿勢の誤差ｒ_ｅが０に収束するようにロボットアーム５を制御するための制御入力となる、位置誤差補償出力ｕ_ｒｅを計算して、位置誤差補償部２０から近似逆運動学計算部２２に出力する（ステップＳ１０）。位置誤差補償部２０の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、位置誤差が０に収束するように位置誤差補償部２０の制御が働く。

次いで、近似逆運動学計算部２２は、ステップＳ２で計算したヤコビ行列Ｊ_ｒの逆行列を位置誤差補償出力ｕ_ｒｅに乗算することにより、位置誤差補償出力ｕ_ｒｅを、手先位置及び姿勢の誤差に関する値から関節角度の誤差に関する値である関節角度誤差補償出力ｕ_ｑｅに変換して、ロボット５Ａのモータドライバ１０に出力する（ステップＳ１１）。

次いで、関節角度誤差補償出力ｕ_ｑｅは、近似逆運動学計算部２２からの情報を入出力ＩＦ９を通じ、モータドライバ１０に与えられ、モータドライバ１０は、関節角度誤差補償出力ｕ_ｑｅに基づき、関節部のそれぞれのモータ１２を流れる電流量を変化させる。この電流量の変化により、ロボットアーム５のそれぞれの関節部の回転運動が発生して、ロボットアーム５が動作を行なう（ステップＳ１２）。

次いで、前記運搬制御動作のステップＳ１〜ステップＳ１２を続ける場合にはステップＳ１に戻る一方、それ以外の場合（電源を切る又はロボットを停止させるのではなく、前記運搬制御動作のみを終了させる場合）には前記運搬制御動作のステップＳ１〜ステップＳ１２を終了する（ステップＳ１３）。

以上のステップＳ１〜ステップＳ１２が制御の計算ループとして繰り返し実行されることにより、ロボットアーム５の動作の制御、すなわち、人２とロボットアーム５が協働して物体３を運搬する運搬動作を実現することができる。

次に、運搬物載置判定部１９が運搬物の載置判定動作を行なう場合について、図９のフローチャートに基づいて説明する。

ロボットアーム５の関節部それぞれのエンコーダ１１により計測された関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）が、エンコーダ１１から制御装置４に取り込まれる（ステップＳ２１）。

次いで、制御装置４に取り込まれた関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）に基づいて、近似逆運動学計算部２２にて、ロボットアーム５の運動学計算に必要なヤコビ行列Ｊ_ｒ等の計算を行う（ステップＳ２２）。

次いで、順運動学計算部２１にて、ロボットアーム５のそれぞれのエンコーダ１１からの関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）から、ロボットアーム５の現在の手先位置及び姿勢ベクトルｒを計算して、推定パラメータデータベース７と位置誤差補償部２０と目標軌道生成部１８と運搬物載置判定部１９とに出力する（ステップＳ２３）。

次いで、力検出部１３は、ロボットアーム５に取り付けられた力センサの出力を元に、手先に発生している力Ｆ_ｅｘｔを計算して、推定パラメータデータベース７と運搬力推定部１４と目標軌道生成部１８と運搬物載置判定部１９とに出力する。（ステップＳ２４）。

次いで、運搬力推定部１４及び運搬物載置判定部１９は、ステップＳ３で順運動学計算部２１により計算して求められた手先位置及び姿勢ベクトルｒから、角度ω_ｐを決定する。図４Ｂ〜図４Ｄのように手先姿勢を定義した場合、角度ω_ｐ＝θである。さらに、運搬力推定部１４は推定パラメータデータベース７に角度ω_ｐを出力する。（ステップＳ２５）。

次いで、運搬物載置判定部１９は、力検出部１３の出力のうちの鉛直方向の力情報Ｆ_ｚが入力される（ステップＳ２６）。さらに、ステップＳ３で順運動学計算部２１により計算して求められた手先位置及び姿勢ベクトルｒから手先ｚ位置（手先位置及び姿勢ベクトルｒのうちのｚ成分）を決定する。

次いで、入力された情報を基に、運搬物載置判定部１９は、運搬している物体３が机などの台９０の物体載置面９０ａに載置したか否かを検出して、その判定を行う（ステップＳ２７）。具体的には、先に説明したように、運搬物載置判定部１９は、前記順運動学計算部２１からのロボットアーム５の手先ｚ位置の情報と手先が水平面となす角度ω_ｐの情報と、力検出部１３からの出力のうちの鉛直方向の力情報Ｆ_ｚとから、「物体３は載置面９０ａに載置された」か否かを判定する。

よって、例えば、運搬物載置判定部１９が、前記順運動学計算部２１により計算して求められた手先位置及び姿勢ベクトルｒから算出した手先が水平面となす角度ω_ｐに基づき前記ロボットアーム５の手先の前記ハンド３０が水平面よりも下向きに傾斜していると判定し、かつ、前記順運動学計算部２１の出力に基づき手先ｚ位置の変動が下降から一定に転じていると判定し、かつ、前記力検出部１３の出力に基づき、鉛直方向下向きの力が減少に転じたと判定したとき（図１１では、「物体３が載置する瞬間である時間ｔ_１から一定時間Δｔ経過した後の時間ｔ_２のとき）、物体３は載置面９０ａに載置された」と、運搬物載置判定部１９により、みなす。ここで、一定時間Δｔは、事前の実験などにより、例えば１秒等に予め設定する。

逆に、これ以外の場合には、「物体３は載置面９０ａに載置されていない」と、運搬物載置判定部１９により判定する。

以下、ステップＳ２７にて、「物体３は載置面９０ａに載置されていない」と運搬物載置判定部１９により判定された場合に、力制御を行なうことについて説明する。

推定パラメータデータベース７は力検出部１３の出力である力Ｆ_ｅｘｔ及び運搬力推定部１４から入力された角度ω_ｐから、パラメータｋ_ｐ及びｋ_ωの値を推定パラメータデータベース７によりそれぞれ決定して、推定パラメータデータベース７から運搬力推定部１４へそれぞれ出力する（ステップＳ２８）。

次いで、運搬力推定部１４は、パラメータｋ_ｐ及びｋ_ωと、手先に発生している力Ｆ_ｅｘｔと、角度ω_ｐと、関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）とから、運搬力Ｆ_{ｃａｒｒｙ}を計算して、インピーダンス制御部１７へ出力する（ステップＳ２９）。

次いで、予め記憶されていたロボットアーム５の動作軌道プログラム用データベース３７の動作軌道プログラムとステップＳ２３で順運動学計算部２１により計算して求められた手先位置及び姿勢ベクトルｒと力検出部１３からの力情報とに基づき、目標軌道生成部１８は、ロボットアーム５の手先位置及び姿勢目標ベクトルｒ_ｄを計算して、位置誤差計算部４０に出力する（ステップＳ３０）。

次いで、インピーダンス制御部１７は、実験によって予め設定された機械インピーダンスパラメータの慣性Ｍ、粘性Ｄ、剛性Ｋと、運搬力推定部１４により計算されたロボットアーム５に加わる等価手先外力Ｆ_{ｃａｒｒｙ}とから、手先位置及び姿勢目標補正出力ｒ_ｄΔを、インピーダンス制御部１７により計算して、位置誤差計算部４０に出力する（ステップＳ３１）。

次いで、位置誤差計算部４０は、目標軌道生成部１８からの手先位置及び姿勢目標ベクトルｒ_ｄとインピーダンス制御部１７からの手先位置及び姿勢目標補正出力ｒ_ｄΔとの和（ｒ_ｄ＋ｒ_ｄΔ）である、手先位置及び姿勢補正目標ベクトルｒ_ｄｍを計算する。次いで位置誤差計算部４０は、手先位置及び姿勢補正目標ベクトルｒ_ｄｍと順運動学計算部２１からの現在の手先位置及び姿勢ベクトルｒとの差（ｒ_ｄｍ−ｒ）である、手先位置及び姿勢の誤差ｒ_ｅを、計算し位置誤差補償部２０に出力する。最後に、位置誤差補償部２０は手先位置及び姿勢の誤差ｒ_ｅが０に収束するようにロボットアーム５を制御するための制御入力となる、位置誤差補償出力ｕ_ｒｅを計算して、位置誤差補償部２０から近似逆運動学計算部２２に出力する（ステップＳ３２）。位置誤差補償部２０の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、位置誤差が０に収束するように位置誤差補償部２０の制御が働く。

次いで、近似逆運動学計算部２２は、ステップＳ２２で計算したヤコビ行列Ｊ_ｒの逆行列を位置誤差補償出力ｕ_ｒｅに乗算することにより、位置誤差補償出力ｕ_ｒｅを、手先位置及び姿勢の誤差に関する値から関節角度の誤差に関する値である関節角度誤差補償出力ｕ_ｑｅに変換して、ロボット５Ａのモータドライバ１０に出力する（ステップＳ３３）。

次いで、関節角度誤差補償出力ｕ_ｑｅは、近似逆運動学計算部２２からの情報を入出力ＩＦ９を通じ、モータドライバ１０に与えられ、モータドライバ１０は、関節角度誤差補償出力ｕ_ｑｅに基づき、関節部のそれぞれのモータ１２を流れる電流量を変化させる。この電流量の変化により、ロボットアーム５のそれぞれの関節部の回転運動が発生して、ロボットアーム５が動作を行なう（ステップＳ３４）。

次いで、前記運搬制御動作のステップＳ２１〜ステップＳ３６を続ける場合にはステップＳ２１に戻る一方、それ以外の場合（電源を切る又はロボットを停止させるのではなく、前記運搬制御動作のみを終了させる場合）には前記運搬制御動作のステップＳ２１〜ステップＳ３６を終了する（ステップＳ３７）。

次に、ステップＳ２７にて、「物体３は載置面９０ａに載置された」と運搬物載置判定部１９により判定された場合に、位置制御を行なうことについて図９を用いて説明する。

動作軌道プログラム用データベース３７には「物体３は載置面９０ａに載置された」と判定された場合にロボットアーム５が動作する軌道を予め記憶しておく。物体３の傾きを修正するための軌道として、ここでは一例として、毎秒１０mmの速度で鉛直方向下向きに移動するとする。運搬物載置判定部１９により「物体３は載置面９０ａに載置された」と判定された場合、予め記憶されている載置時の動作軌道プログラム用データベース３７の動作軌道プログラムが目標軌道生成部１８に入力される。

目標軌道生成部１８は、ロボットアーム５の手先に発生している力のうち、鉛直方向の力Ｆｚが下向きの力のときは、ロボットアーム５の動作軌道プログラム用データベース３７の動作軌道プログラムとステップＳ２３で順運動学計算部２１により計算して求められた手先位置及び姿勢ベクトルｒに基づき、ロボットアーム５の手先位置及び姿勢目標ベクトルｒｄを計算し、位置誤差計算部４０に出力する。また、ロボットアーム５の手先に発生している力のうち、鉛直方向の力Ｆｚが０または上向きになったときは、目標軌道生成部１８は、ロボットアーム５の手先位置及び姿勢目標ベクトルｒｄを更新せず、位置誤差計算部４０に同じ値を出力し続ける。（ステップＳ３５）。これにより、ロボットアーム５は物体３が載置面９０ａに傾きなく載置されるまで下がり続け、物体３が載置面９０ａに傾きなく載置されロボットに下向きの力を与えなくなった所で停止する。

次いで、位置誤差計算部４０は、目標軌道生成部１８からの手先位置及び姿勢目標ベクトルｒ_ｄとインピーダンス制御部１７からの手先位置及び姿勢目標補正出力ｒ_ｄΔとの和（ｒ_ｄ＋ｒ_ｄΔ）である、手先位置及び姿勢補正目標ベクトルｒ_ｄｍを計算するが、インピーダンス制御部１７からの手先位置及び姿勢目標補正出力ｒ_ｄΔが０であるため、ｒ_ｄ＝ｒ_ｄｍとなる。次いで、手先位置及び姿勢補正目標ベクトルｒ_ｄｍと順運動学計算部２１からの現在の手先位置及び姿勢ベクトルｒとの差（ｒ_ｄｍ−ｒ）である、手先位置及び姿勢の誤差ｒ_ｅを、位置誤差計算部４０により計算し位置誤差補償部２０に出力する。最後に、手先位置及び姿勢の誤差ｒ_ｅが０に収束するようにロボットアーム５を制御するための制御入力となる、位置誤差補償出力ｕ_ｒｅを位置誤差補償部２０により計算して、位置誤差補償部２０から近似逆運動学計算部２２に出力する（ステップＳ３６）。位置誤差補償部２０の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、位置誤差が０に収束するように位置誤差補償部２０の制御が働く。

次いで、前述のステップＳ３４が行われる。

以上のステップＳ２１〜ステップＳ３６が制御の計算ループとして繰り返し実行されることにより、ロボットアーム５の運搬動作及び運搬物の載置動作の制御、すなわち、人２とロボットアーム５が協働して物体３を運搬し、物体３の載置時には物体３が水平面に対して傾かずに載置できる、ロボットアーム５の動作を実現することができる。

前記実施形態では、運搬物載置判定部１９が物体３の載置を判定した後、載置判定結果に基づき、運搬物載置判定部１９での判定結果に基づき、運搬物載置動作を行うようにロボットアーム５の力制御と位置制御とを自動で切り替えるようにしたが、安全のため、例えば、ボタンなどを設けて、運搬物載置判定部１９が物体３の載置を判定していて、かつ人２がボタンを押したときだけ、ロボットアーム５の力制御と位置制御とを切り替えるようにしてもよい。これにより、人２が意図したときのみロボットアーム５の力制御と位置制御とを切り替えることができ、人２が意図しない場合にはロボットアーム５の力制御と位置制御との切り替えが行なわれないため、より安全性を高めることができる。

なお、前記実施形態では、６軸すべての方向に対して力制御を行うとしたが、鉛直方向の移動及びピッチ量以外については力制御ではなく、位置制御によって動かしてもよい。

以上のように、本発明の前記実施形態にかかる制御装置４によれば、人２とロボットアーム５で協働して物体３を運搬する際に、物体３の重量がわからない場合又は物体３の重量が途中で変化した場合にも、物体３を円滑に運搬でき、さらに干渉物体４３に物体３を押し付けることがない、安全なロボット制御を実現できる制御装置を提供することができる。

また、図１２に示すように、ロボットアーム５の基端が壁面９に固定されるものに限らず、壁面９に固定された一対のレール３８に、レール３８に沿って移動可能な、スライドユニットである移動体３９にロボットアーム５の基端が支持され、レール３８上をロボットアーム５がレール３８沿いに横方向例えば水平方向に移動可能とするようにしてもよい。移動体３９は、制御装置４の制御の下に自走可能なモータなどが内蔵されていてもよいし、又は、レール３８沿いに移動体３９が移動自在として、人２の力により、運搬方向に人２と共に移動自在としてもよい。

なお、前記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。

本発明は、家庭用ロボットなど人と協働作業を行うロボット、ロボットのロボットアームの動作の制御を行なうロボット（アーム）の制御装置及び制御方法、並びに、ロボットの制御装置の制御プログラムとして有用である。また、家庭用ロボットに限らず、人と協働作業を行う産業用ロボット、又は、生産設備等における可動機構のロボットアームの制御装置及び制御方法、ロボットアームの制御装置を有するロボット、及びロボットアームの制御プログラムとしても適用が可能である。

本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形又は修正は明白である。そのような変形又は修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

Claims (6)

1. ロボットアームと、前記ロボットアームの手先に配置されたハンドとを有して、人とロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するロボットであって、
   前記ハンドの角度を調整する駆動部と、
   前記物体と前記ハンドとの間に働く力を検出して出力する力検出部と、
   前記ハンドの前記角度を検出して出力する角度検出部と、
   前記角度検出部の出力と前記力検出部の出力とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う制御部とを備え、
   前記力検出部は、前記物体と前記ハンドとの間に働く鉛直方向の力を検出して出力し、
   前記制御部は、前記角度検出部の出力と前記力検出部の出力とに基づいて前記力制御の鉛直方向の入力を決定し、前記人が前記物体を持ち上げて前記力制御の鉛直方向の入力が上向きとなるとき、前記ロボットアームを上昇させるように力制御を行うとともに、前記人が前記物体を下降させて前記力制御の鉛直方向の入力が下向きとなるとき、前記ロボットアームを下降させるように力制御を行うロボット。
2. 人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御装置であって、
   前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報を入力する入力部と、
   前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う制御部と、
   前記力制御の鉛直方向の入力として、前記角度検出部の出力に係数を乗じた値と、前記力検出部の出力の鉛直成分に係数を乗じた値を加算した値を使用して、前記人が前記ロボットに鉛直方向に作用させようとする力を推定する運搬力推定部と、
   前記運搬力推定部にて推定した前記力に基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行うロボットの制御装置。
3. 人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御装置であって、
   前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報を入力する入力部と、
   前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行う制御部と、
   前記ロボットアームの手先の前記ハンドの位置を検出する手先位置検出部と、
   前記手先位置検出部の出力と、前記角度検出部の出力と、前記力検出部の出力とから、前記運搬している物体が物体載置面に載置されたか否かを判定する運搬物載置判定部とを備え、
   前記運搬物載置判定部が、前記物体が前記物体載置面に載置されたと判定した際には、前記ロボットアームの鉛直方向の制御を、前記力制御から位置制御に切り替えるロボットの制御装置。
4. 前記運搬物載置判定部は、前記角度検出部の出力によりロボットアームの前記ハンドが水平面よりも下向きであり、かつ、前記手先位置検出部の出力により前記手先位置の変動が下降から一定に転じ、かつ前記力検出部の出力のうち、鉛直方向下向きの力が減少に転じたときに、前記運搬している物体が前記物体載置面に載置されたと判定する請求項３に記載のロボットの制御装置。
5. 人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御方法であって、
   前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報が、入力部から入力され、
   前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を制御部により行い、
   前記力制御の鉛直方向の入力として、前記角度検出部の出力に係数を乗じた値と、前記力検出部の出力の鉛直成分に係数を乗じた値を加算した値を使用して、前記人が前記ロボットに鉛直方向に作用させようとする力を、運搬力推定部により推定し、
   前記運搬力推定部にて推定した前記力に基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を行うロボットの制御方法。
6. 人と、ロボットアームと前記ロボットアームの手先に配置されたハンドとを有するロボットとの間に位置した物体を前記人と前記ハンドとで保持した状態で協働して前記物体を運搬するように前記ロボットを制御する、ロボットの制御プログラムであって、
   コンピュータに、
   前記物体と前記ハンドとの間に働く力を検出する力検出部から検出された力情報及び駆動部により角度調整される前記ハンドの角度を検出する角度検出部から検出された角度情報を、入力部から入力する機能と、
   前記力検出部の前記力情報と前記角度検出部の前記角度情報とに基づいて前記ロボットアームの鉛直方向の力が所定の力となるよう制御する力制御を制御部により行う機能と、
   前記ロボットアームの手先の前記ハンドの位置を手先位置検出部により検出する機能と、
   前記手先位置検出部の出力と、前記角度検出部の出力と、前記力検出部の出力とから、前記運搬している物体が物体載置面に載置されたか否かを運搬物載置判定部により判定する機能と、
   前記運搬物載置判定部が、前記物体が前記物体載置面に載置されたと判定した際には、前記ロボットアームの鉛直方向の制御を、前記力制御から位置制御に切り替える機能とを実現させるための、ロボットの制御プログラム。

Images