パワーアシスト装置とその適応モデル予測制御方法
【課題】 人とロボットの協調作業において、できるだけ小さい操作力で、できるだけ目標の最終位置からのずれを小さく抑えることができるパワーアシスト装置とその適応モデル予測制御方法を提供する。
【解決手段】 運搬対象物１をリジットに把持する手先部１１をもった多関節アーム型ロボット１０と、ロボットを制御する制御装置２０とを備える。ロボット１０は、多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するセンサを有する。制御装置２０は、センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する。この推定値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボット１０にアシスト力を発生させる。
【解決手段】 運搬対象物１をリジットに把持する手先部１１をもった多関節アーム型ロボット１０と、ロボットを制御する制御装置２０とを備える。ロボット１０は、多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するセンサを有する。制御装置２０は、センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する。この推定値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボット１０にアシスト力を発生させる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、パワーアシスト装置に係わり、さらに詳しくは、人とロボットの協調作業において、小さい操作力で、目標の最終位置からのずれを小さく抑えるパワーアシスト装置とその適応モデル予測制御方法に関する。
【背景技術】
【０００２】
パワーアシスト装置とは、移動または姿勢変更の対象となる物体を操作する者（操作者）の操作力に基づいてアシスト力を発生させ、操作者の力を軽減する装置である。
【０００３】
かかるパワーアシスト装置の制御方法として、インピーダンス制御が知られている。インピーダンス制御は、移動等させようとする物体を、その質量ｍ、粘性係数ｃ、ばね係数ｋを用いた運動方程式：Ｆ＝ｍ×ａ＋ｃ×ｖ＋ｋ×ｘ・・・式（１ａ）によりモデル化することにより、モータに発生するアシスト力Ｆを制御するものである。
【０００４】
しかし、インピーダンス制御のも種々の問題点があり、さらにこれを改良したパワーアシスト装置が種々創案されている（例えば特許文献１、２）。
【０００５】
特許文献１のパワーアシスト装置は、予定した位置に物体を位置決めしようとする際に、操作者がパワーアシスト装置に引っ張られるような感覚を受けるのを回避するために、粘性係数ｃを物体の移動速度ｖに基づいて定め、これにより操作者の腕のインピーダンス変化に倣ってアシスト力Ｆを求めるものである。
【０００６】
特許文献２のパワーアシスト装置は、例えば、操作者が運搬台車を所定の停止位置に停車させようとして走行中の運搬台車のハンドル等を操作すると、移動速度や駆動機構の粘性抵抗あるいは運搬するワークの軽重等によって、運搬台車に引っ張られたり逆に制止させられたりするような違和感を操作者に与えることがあるのを解決することを目的とし、
サーボ搬送制御かインピーダンス制御への移行に際し、減速制御手段により物体の移動速度を所定速度以下に減速させるものである。
【０００７】
【特許文献１】特開２００３-２５２６００号公報、「パワーアシスト装置」
【特許文献２】特開２００５-５９６４０号公報、「パワーアシスト装置、その制御方法および搬送装置」
【発明の開示】
【発明が解決しようとする課題】
【０００８】
自動車等の組立工場などにおいて、作業者の負担を軽減するために、人と協調して作業をアシストするロボットの開発が望まれている。人の作業を助け、目標のタスクを正確に実行するためには、作業者の運動状態に合わせて、作業者になるべく違和感を与えることなく、なおかつ、最終的な目標動作からのずれを最小に抑えるようにロボットの動作を調節する必要がある。
【０００９】
例えば、組立中の自動車の窓枠にフロントやリヤの窓ガラスを嵌め込む作業は、従来は二人の作業員が左右を持って協同で行っていた。この作業をロボットと人との協調作業で行なおうとする場合、従来のパワーアシスト手段では以下の問題点があった。
ロボットと協調作業する人（以下、作業者という）は、操作の主体であり、ロボットは作業者の動作をアシストするように作動する必要がある。しかし、操作主である作業者の運動状態（手先粘弾性と手先自然長位置）は、その作業状態に応じて変化するため、アシスト力Ｆを最適化することが困難であった。
そのため、アシスト力Ｆが小さすぎると作業員の負担が大きく、逆に大きすぎると作業者による正確な位置決めが困難になる問題点があった。
【００１０】
本発明は上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、人とロボットの協調作業において、できるだけ小さい操作力で、できるだけ目標の最終位置からのずれを小さく抑えることができるパワーアシスト装置とその適応モデル予測制御方法を提供することにある。
【課題を解決するための手段】
【００１１】
本発明によれば、運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットと、該ロボットを制御する制御装置とを備え、
前記ロボットは、多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するセンサを有し、
前記制御装置は、前記センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定し、
該推定値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させる、ことを特徴とするパワーアシスト装置が提供される。
【００１２】
また本発明によれば、運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットを有するパワーアシスト装置の適応モデル予測制御方法であって、
前記ロボットの多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するデータ検出ステップと、
検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する推定ステップと、
推定された各値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させるパワーアシストステップと、を有することを特徴とするパワーアシスト装置の適応モデル予測制御方法が提供される。
【００１３】
前記推定ステップにおいて、逐次最小２乗法を用いて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅを推定する。
【００１４】
また、前記パワーアシストステップにおいて、各時刻ｔごとに変化する下記の評価関数式（２４）を最小化する最適制御入力を設定する、ことが好ましい。
【数１】
【００１５】
前記パワーアシストステップにおいて、評価関数式（２４）内の終端位置決め誤差ｅ（ｔ＋Ｔ）にかける重み行列Ｆは下記の条件式（３１）を満たすように最適制御入力を設定する、ことが好ましい。
【数２】
【発明の効果】
【００１６】
上記本発明の装置及び方法によれば、リアルタイムに検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、
作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定し、推定された各値に基づき、アシスト力を最適化するので、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させることができる。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の好ましい実施形態を説明する。なお各図において、共通する部分には同一の符号を付し、重複した説明を省略する。
【００１８】
図１は、本発明のパワーアシスト装置の全体構成図である。この図において、本発明のパワーアシスト装置は、運搬対象物１をリジットに把持する手先部１１をもった多関節アーム型ロボット１０と、このロボットを制御する制御装置２０とを備える。
ロボット１０は、多関節の関節角度ベクトルｑ、手先部位置ｘを検出するセンサ（図示せず）、及びロボットと協調作業する作業者２が運搬対象物１に加える力ｆｅを検出する６軸力センサ３を有する。
制御装置２０は、センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者２の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する。
またこの推定値に基づき、制御装置２０は、ロボット１０に入力する最適制御入力をリアルタイムに設定する。
ロボット１０は、リアルタイムで設定された最適制御入力に基づいて作動し、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、アシスト力を発生する。
【００１９】
上述した多関節アーム型ロボット１０を用い、本発明の方法は、運搬対象物１をリジットに把持する手先部１１をもった多関節アーム型ロボット１０を有するパワーアシスト装置の適応モデル予測制御方法である。
図２は、本発明の方法を示すブロック線図である。この図に示すように、本発明の方法は、データ検出ステップＳ１、推定ステップＳ２、及びパワーアシストステップＳ３を有する。
データ検出ステップＳ１では、ロボット１０の多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出する。
推定ステップＳ２では、検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する。
パワーアシストステップＳ３では、推定された各値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させる。
【００２０】
推定ステップＳ２において、逐次最小２乗法を用いて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅを推定する。この推定は、後述する式に基づくのがよい。
【００２１】
また、パワーアシストステップＳ３において、各時刻ｔごとに変化する評価関数式（２４）を最小化する最適制御入力を設定する。この設定により、位置決め誤差を最小にし、かつロボットが加える操作量を最小にすることができる。
【００２２】
また、パワーアシストステップにおいて、終端時刻にかける重み行列Ｆは条件式（３１）を満たすように最適制御入力を設定する。この設定により、後述する定理１に示すように、式（２６）で示す最適制御則の安定性を保証することができる。
以下、本発明をさらに詳細に説明する。
【００２３】
この明細書において、記号上部の点「・」は時間微分を「・・」は時間に関する２回微分を、記号「＾」は推定値を意味する。本発明では、人の手先の動特性は線形の粘弾性を持ち、式（１）と書けるものとする。ここでｘは人の手先位置、添え字ｅは作業者を意味し、ｆｅは運搬対象物に加える力、Ｄｅは作業者の粘性係数、Ｋｅはその弾性係数、ｘｅは作業者の手先の自然長位置を表す。
【００２４】
一方、アシストロボットはｎ個の関節をもつアーム型ロボットで運搬対象物をリジッドに把持しているものとする。このとき、ロボットと運搬対象物、作業者を含めた全システムの運動方程式は式（２）（３）で与えられる。ここでｑは関節角度ベクトル、Ｈ（ｑ）はロボットの慣性行列、Ｃ（ｄｑ／ｄｔ，ｑ）はコリオリ力と遠心力の和、τは関節トルク、Ｊ（ｑ）は関節角度ベクトルｑからロボット手先位置ｘまでのヤコビアンをそれぞれ表す。
【００２５】
【数３】
【００２６】
以上の問題設定の下に、本発明の「適応モデル予測制御方法」では、観測データｑ，ｄｑ／ｄｔ，ｆｅに基いて、作業者の運動状態Ｋｅ，Ｄｅ，ｘｅをリアルタイムで推定し、その推定された運動状態下で可能な限り小さい操作力で、位置決め誤差を可能なかぎり小さく抑えるような最適アシスト操作力を制御インターバル１ｍｓごとに逐次的に生成する。
【００２７】
以後、各ステップにおける、制御装置の詳細な動作について説明する。
【００２８】
データ検出ステップＳ１ではロボット１０の各関節に取り付けたロータリエンコーダによって関節角度ベクトルｑ、関節角速度ベクトルｄｑ／ｄｔを、ハンドル部分に取り付けた３軸力センサにより作業者から運搬対象物に加えられる操作力の並進３自由度成分ｆｅを測定する。手先部位置ｘは高速カメラや超音波センサ、赤外線センサ等により直接計測するか、式（４）のロボットの順キネマティクスを計算することで算出する。
【００２９】
【数４】
【００３０】
データ検出ステップＳ１で関節角速度ベクトルｄｑ／ｄｔを直接計測することができない場合には、関節角度ベクトルｑから微分フィルタを用いて算出しても構わない。
【００３１】
推定ステップＳ２ではデータ検出ステップＳ１で測定した、ロボット１０の関節角度ベクトルｑ、関節角速度ベクトルｄｑ／ｄｔ、ハンドル部分に作業者から加えられる操作力ｆｅから、作業者の運動状態を推定する。作業者の手先弾性をＫｅ、手先粘性をＤｅ、作業者が運搬対象物を運ぼうとしている位置（手先自然長位置）をｘｅとすると、作業者から運搬対象物に加えられる力は式（１）で記述される。
【００３２】
ここで作業者の手先自然長位置ｘｅは窓枠はめ込み作業中一定か、若しくは、時間に依存してなめらかに変化するものと想定する。このとき、作業者の手先自然長位置ｘｅはたとえば式（５）のような２次関数で近似することができる。
この近似関数は勿論２次関数に限られるわけではなく、必要に応じて高次関数で近似しても構わない。ただし、高次関数で近似を行う場合には計算時間が多く必要になるので、１ステップの制御入力を算出するための計算時間が１ｍｓ以内に収まる範囲で近似を行わなければならない。
【００３３】
【数５】
【００３４】
このとき、作業者の運動状態は定数ベクトル（Ｋｅ，Ｄｅ，ａ０，ｂ０，ｃ０）で完全に記述され、作業者から運搬対象物に加えられる操作力ｆｅは、式（６）と記述される。
【００３５】
これは新たに式（７）の変数を導入すると、式（８）とかける。 ここでθは作業者の運動状態を表す未知パラメータ、φは観測可能なベクトルである。
【００３６】
【数６】
【００３７】
本発明、推定ステップＳ２の実施形態としてはたとえば逐次最小２乗法に基くパラメータ推定法が考えられる。逐次最小２乗法を用いる場合には、式（９）を最小にするようなパラメータ（＾付き）θとして、式（１０）を時々刻々積分することで作業者の運動状態（Ｋｅ，Ｄｅ，ａ０，ｂ０，ｃ０）を推定する。
【００３８】
【数７】
【００３９】
別の推定ステップＳ２の実施形態としてはレシーディングホライズン最小２乗法を用いる。レシーディングホライズン最小２乗法では現在の時刻ｔから一定時間Ｔｅだけ前の時刻ｔ-Ｔｅから、現在の時刻ｔまでにわたる式（１１）の推定誤差の２乗積分を最小にするようなパラメータ（＾付き）θとして、作業者の運動状態（Ｋｅ，Ｄｅ，ａ０，ｂ０，ｃ０）を推定する。
【００４０】
【数８】
【００４１】
ここで、被積分関数第２項は推定値の連続性を保つために導入されたコストで、αは対象毎に適当に選ぶ正の定数である。
この場合、最適推定値（＾付き）θは式（１２）で与えられ、これは式（１３）（１４）の逐次アルゴリズムに従って逐次的に求められる行列Γ（ｔ）、Ｙ（ｔ）を用いて、式（１５）と計算することができる。ここでＮは数値積分の刻み数である。このようにレシーディングホライズン最小２乗法では、現在の時刻から時間前までの観測データのみに基いてパラメータ推定則を構成したことで、作業者の運動状態の変化に素早く対応して、正しい運動状態の推定値を得ることができる。
【００４２】
【数９】
【００４３】
次にパワーアシストステップＳ３の詳細について述べる。
【００４４】
人との協調作業では、人（作業員）もロボットの動きに合わせて行動を変化させることが予想される。その際に、ロボットの手先が複雑な動特性を持っていると、人は、ロボットの動きを予測することができず、操作性の低下を引き起こす原因となってしまうおそれがある。
そこで、本発明ではまず、ロボットの動力学が式（１６）で与えられるように各関節に式（１７）で示すインピーダンス制御入力を加えることにする。
ここで、添字０はロボットを意味し、Ｍ０、Ｄ０、Ｋ０はそれぞれ、ロボットの手先に指令する目標の慣性、粘性、弾性係数を表し、ｘｒはロボットの手先自然長位置を表す。以後、式（１６）のインピーダンス制御された系に対して、アシスト制御則を構成する。
【００４５】
【数１０】
【００４６】
人とロボットの協調作業において、可能な限り作業員に負担をかけないように、かつ可能な限り正確に協調位置決め作業を行うためには、何らかの最適化指標を最小化するように逐次的にロボットの軌道を修正する必要がある。
しかし、人側の運動状態は未知であり、これをリアルタイムで逐次的に推定するとしているので、最適な制御ゲインを予め求めておくことはできない。そこで、最適制御ゲインも、推定された人側の運動状態に基づいて逐次的に求める必要がある。
また、時間間隔をあけて、制御入力の修正を行うと、協調作業を行う作業員に違和感を与えてしまうおそれがある。そこで、本発明では推定されたシステムのモデルに基づいて、連続時間（制御サイクル１ｍｓ）でモデル予測制御を行うことにより、人の運動状態に適応した、最適なアシスト制御を実現することを考える。
【００４７】
図３は、適応モデル予測制御の基本概念図である。通常のモデル予測制御では、この図に示すように、システムの真のモデルに基づいて一定時間後（ｔ＋Ｔ）にシステムが行き着く状態を予測し、目標の状態との差が最小になるように、最適な制御入力を時々刻々計算する。
しかし、人とロボットの協調作業においてはシステムは未知なので、真のモデルを用いた最適制御入力を構成することができない。そこで、式（１０）もしくは式（１５）の推定方法によって得られたシステムの推定モデルに基づいて、適応モデル予測制御を行う。本発明ではこれを適応モデル予測制御と呼ぶ。
【００４８】
推定された作業者と協調作業をするロボットの運動方程式は、式（１８）となる。ここで（＾付き）ｆ０は作業者運動状態の推定値（＾付き）（ａ，ｂ，ｃ）を用いて式（１９）で表されるベクトルである。
ここで、漸近的に目標のはめ込み位置ｘｄに対象物を誘導するためにロボットの手先自然長位置ｘｒを式（２０）を積分して求められる軌道に設定する。すると作業者とロボットを合わせた全システムは、式（２１）に従うことになる。ここで、ｅは位置決め誤差ｘ-ｘｄを表す。これはさらに式（２２）の変数を導入すると、式（２３）と書くことができる。
【００４９】
【数１１】
【００５０】
このシステムの式（２３）に対して、位置決め誤差を最小にかつ、ロボットが加える操作量を最小にする制御は、式（２４）で示す各時刻ｔごとに変化する評価関数を最小化する最適制御入力を構成することで実現できる。
ここで、Ｑ、Ｒ、Ｆ、は各時刻ｔで決定される重み行列で、ＱとＲは正定値、Ｆは半正定値であるとする。
最適制御入力ｕ＊（ｔ）は各時刻ｔでリッカチ微分方程式（２５）を時刻ｔまで時間を遡って積分して求めた行列Ｓ（ｔ）を用いて、式（２６）で構成される。ここで、時不変システムに対して、次の定理１が成り立つことに注意する。
【００５１】
【数１２】
【００５２】
（定理１）
線形時不変システムの式（２７）に対して、区間［ｔ，ｔ＋Ｔ］で定義される式（２８）の最適化評価関数を最小化する最適制御入力は、式（２９）を積分して求めた行列Ｓ（ｔ）を用いて式（３０）のように構成される。
このとき、もし、終端時刻にかける重み行列Ｆが条件式（３１）が満たされるならば式（２７）に最適入力式（３０）を代入したシステムは安定で、式（３２）が漸近的に成り立つ。
【００５３】
【数１３】
【００５４】
この定理１から、たとえ推定方法が収束して、真のプラントの方程式が同定された後であっても、任意に最適化評価関数を設定することはできず、安定性を保証するには式（３１）の拘束条件を満たすようにＱ、Ｒ、Ｆを設定しなければならないことになる。
しかも今の場合にはシステム自体が時間変化するので、システムの安定性を保証するには評価関数の中の重み行列を式（３１）を満たすように時々刻々修正しなければならない。そこで、本発明による第１の評価関数設定方法では、Ｑ、Ｒを予め一定値に設定し、各時刻ｔで式（３３）をＦについて解くことで、条件式（３１）を満たすような最適化評価関数式（２４）を制御サイクルごとに求める。
【００５５】
【数１４】
【００５６】
しかし、式（３３）を解くには大きな計算時間を必要とする。そのため本発明による第２の評価関数設定方法では式（３３）よりも保守的な条件式（３４）を各時刻ｔでＦについて解くことで、条件式（３１）を満たすような最適化評価関数式（２４）を制御サイクルごとに構成する。
この条件式を用いるには行列Ａが安定である必要があるが、今考えている問題では、推定された人側弾性係数Ｋｅ（推定値）、粘性係数Ｄｅ（推定値）が式（３５）を満たす限り安定であるので、推定方法の初期値が十分大きければ（３４）式の条件を用いて、重み行列を更新することが可能である。
以上よりパワーアシストステップＳ３では，推定ステップＳ２で推定されたパラメータ（＾付）Ｋｅ，（＾付）Ｄｅ，（＾付）ｆｅと式（２６）の最適制御入力ｕ＊を用いて，各関節へ指令する制御トルクτが式（３６）のように構成される．
【００５７】
【数１５】
【００５８】
以下に本発明によるパワーアシストの実施例を示す。
【実施例１】
【００５９】
本実施例では、作業者と協調して運搬対象物を正しい位置に位置決めするタスクにおいて、作業者は正しい位置よりも１．０ｍずれた位置に誤って位置決めを行おうとしているものとし、ロボットは式（３６）に従ってその操作力を調節することで、可能な限り小さい操作力で、位置決め誤差を可能な限り小さくするように作業者を補佐するものとする。
この問題に対して適応なしのモデル予測制御則を用いてアシストを行った場合のシミュレーション結果を図４に示す。ただし、このシミュレーションでは、モデル予測制御則の中の重み行列ＲとＦは固定したままで、位置決め誤差にかける重みＱだけを変化させた。
【００６０】
図４からわかるように、適応なしでモデル予測制御を行った場合には、人を含めた協調作業系のシステムとモデル予測制御に用いられるモデルが一致しないため、定常的な誤差が残ってしまう。
【００６１】
一方、創案した制御系では適応推定器を導入したことにより、人側の運動状態を漸近的に推定することができるので、図５に示すように目標位置に向かって対象物を誘導することができる。人側の運動を考慮せずに、単純に位置決め制御を行う場合には、ロボットは目標のはめ込み位置に素早く対象物を運ぼうとするが、創案した適応モデル予測制御では重み行列Ｑを小さく取ることで人の動きになるべく追従するようにロボットを動かすことができ、人に違和感を感じさせずに、運搬対象物を目標位置に誘導することができる。
【００６２】
図５は創案した適応モデル予測制御則において、重み行列Ｒを固定した状態で、位置決め誤差にかける重みＱを変化させた場合に実現される軌道を示す。
図５から、創案した制御系では位置決め誤差にかける重み行列Ｑを大きく取ればとるほど正確に、運搬対象物を目標位置へ誘導することができることが分かる。なお、予測ホライズンを５００ステップにとった時に１個の制御入力を計算するのに要する時間は２．２ＧＨｚ、Ｐｅｎｔｉｕｍ４（登録商標）、メモリ７６８ＭＢの環境下で０．９８ｍｓ／秒であった。
【実施例２】
【００６３】
図６に水平面内のｘｙ-２自由度に対して、創案した適応モデル予測制御を用いてアシスト制御を行ったシミュレーション２の結果を示す。このシミュレーションでは、正しい運搬目標位置は（ｘ，ｙ）＝（０．１，０．１）で与えられているが、作業者は誤って、（ｘ，ｙ）＝（０，０）に位置決めを行おうとしているものとし、一方、ロボットは対象物が正しい位置にくるように力を加えることで位置決め作業を補佐するものとする。
【００６４】
この際、ｙ方向の最適化評価関数に関しては重み行列を式（３７）に固定し、ｘ方向の最適化評価関数に関してはＱ１１成分以外を式（３７）と同様にとり、Ｑ１１成分だけを変化させる。Ｑ１１＝１０００のときはｘ-ｙ方向の制御則が同一になるので、実現される軌道はほぼ直線になるが、Ｑ１１を下げていくにつれ、ｙ方向は人の手先軌道ｙ＝０に追従しようとし、ｘ方向だけが正確に目標位置へ追従するようになる。
【００６５】
【数１６】
【００６６】
これにより、たとえば自動車の窓枠はめ込み作業におけるパワーアシスト装置のように、上下方向に関しては作業者に違和感を与えないようになるべく作業者の動きに追従し、水平方向に関してはなるべく目標とするはめ込み位置からのずれを小さく抑えるようにアシストすることが要求されるタスクに対しては、上下方向の自由度に対する重みＱを小さく、水平方向の自由度に対する重みＱを大きく取ることで、上下方向に関しては作業者の動きに追従し、水平方向に関しては正しい位置へと運搬対象物を誘導するような、動作方向に依存したアシスト動作を実現することが可能になる。
【００６７】
また、本発明の制御手法では、動作中にＱを変更することで、必要に応じたアシストのモードの切り替えを行うことも可能である。
【００６８】
たとえば運搬対象物を作業者と共に所定の位置まで運び、その後で正確な位置決め動作を行うような場合には、動作の開始時にはＱを小さく設定しておき、正確な位置決め動作への移行時にＱを滑らかに大きくすることで、操作力を不連続に変化させることなく、作業者に追従するモードから正確な位置決め補佐を行うモードへとスムーズに移行することが可能になる。
【００６９】
なお、本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更できることは勿論である。
【図面の簡単な説明】
【００７０】
【図１】本発明のパワーアシスト装置の全体構成図である。
【図２】本発明の方法を示すブロック線図である。
【図３】水平方向の最適制御の基本概念図である。
【図４】適応なしのモデル予測制御則を用いてアシストを行った場合のシミュレーション結果を示す図である。
【図５】創案した適応モデル予測制御則において、重み行列Ｒを固定した状態で、位置決め誤差にかける重みＱを変化させた場合に実現される軌道を示す図である。
【図６】水平面内のｘｙ-２自由度に対して、創案した適応モデル予測制御を用いてアシスト制御を行ったシミュレーション２の結果を示す図である。
【符号の説明】
【００７１】
１ 運搬対象物、２ 作業者、３ ６軸力センサ
１０ ロボット、１１ 手先部、２０ 制御装置
【技術分野】
【０００１】
本発明は、パワーアシスト装置に係わり、さらに詳しくは、人とロボットの協調作業において、小さい操作力で、目標の最終位置からのずれを小さく抑えるパワーアシスト装置とその適応モデル予測制御方法に関する。
【背景技術】
【０００２】
パワーアシスト装置とは、移動または姿勢変更の対象となる物体を操作する者（操作者）の操作力に基づいてアシスト力を発生させ、操作者の力を軽減する装置である。
【０００３】
かかるパワーアシスト装置の制御方法として、インピーダンス制御が知られている。インピーダンス制御は、移動等させようとする物体を、その質量ｍ、粘性係数ｃ、ばね係数ｋを用いた運動方程式：Ｆ＝ｍ×ａ＋ｃ×ｖ＋ｋ×ｘ・・・式（１ａ）によりモデル化することにより、モータに発生するアシスト力Ｆを制御するものである。
【０００４】
しかし、インピーダンス制御のも種々の問題点があり、さらにこれを改良したパワーアシスト装置が種々創案されている（例えば特許文献１、２）。
【０００５】
特許文献１のパワーアシスト装置は、予定した位置に物体を位置決めしようとする際に、操作者がパワーアシスト装置に引っ張られるような感覚を受けるのを回避するために、粘性係数ｃを物体の移動速度ｖに基づいて定め、これにより操作者の腕のインピーダンス変化に倣ってアシスト力Ｆを求めるものである。
【０００６】
特許文献２のパワーアシスト装置は、例えば、操作者が運搬台車を所定の停止位置に停車させようとして走行中の運搬台車のハンドル等を操作すると、移動速度や駆動機構の粘性抵抗あるいは運搬するワークの軽重等によって、運搬台車に引っ張られたり逆に制止させられたりするような違和感を操作者に与えることがあるのを解決することを目的とし、
サーボ搬送制御かインピーダンス制御への移行に際し、減速制御手段により物体の移動速度を所定速度以下に減速させるものである。
【０００７】
【特許文献１】特開２００３-２５２６００号公報、「パワーアシスト装置」
【特許文献２】特開２００５-５９６４０号公報、「パワーアシスト装置、その制御方法および搬送装置」
【発明の開示】
【発明が解決しようとする課題】
【０００８】
自動車等の組立工場などにおいて、作業者の負担を軽減するために、人と協調して作業をアシストするロボットの開発が望まれている。人の作業を助け、目標のタスクを正確に実行するためには、作業者の運動状態に合わせて、作業者になるべく違和感を与えることなく、なおかつ、最終的な目標動作からのずれを最小に抑えるようにロボットの動作を調節する必要がある。
【０００９】
例えば、組立中の自動車の窓枠にフロントやリヤの窓ガラスを嵌め込む作業は、従来は二人の作業員が左右を持って協同で行っていた。この作業をロボットと人との協調作業で行なおうとする場合、従来のパワーアシスト手段では以下の問題点があった。
ロボットと協調作業する人（以下、作業者という）は、操作の主体であり、ロボットは作業者の動作をアシストするように作動する必要がある。しかし、操作主である作業者の運動状態（手先粘弾性と手先自然長位置）は、その作業状態に応じて変化するため、アシスト力Ｆを最適化することが困難であった。
そのため、アシスト力Ｆが小さすぎると作業員の負担が大きく、逆に大きすぎると作業者による正確な位置決めが困難になる問題点があった。
【００１０】
本発明は上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、人とロボットの協調作業において、できるだけ小さい操作力で、できるだけ目標の最終位置からのずれを小さく抑えることができるパワーアシスト装置とその適応モデル予測制御方法を提供することにある。
【課題を解決するための手段】
【００１１】
本発明によれば、運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットと、該ロボットを制御する制御装置とを備え、
前記ロボットは、多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するセンサを有し、
前記制御装置は、前記センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定し、
該推定値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させる、ことを特徴とするパワーアシスト装置が提供される。
【００１２】
また本発明によれば、運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットを有するパワーアシスト装置の適応モデル予測制御方法であって、
前記ロボットの多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するデータ検出ステップと、
検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する推定ステップと、
推定された各値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させるパワーアシストステップと、を有することを特徴とするパワーアシスト装置の適応モデル予測制御方法が提供される。
【００１３】
前記推定ステップにおいて、逐次最小２乗法を用いて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅを推定する。
【００１４】
また、前記パワーアシストステップにおいて、各時刻ｔごとに変化する下記の評価関数式（２４）を最小化する最適制御入力を設定する、ことが好ましい。
【数１】
【００１５】
前記パワーアシストステップにおいて、評価関数式（２４）内の終端位置決め誤差ｅ（ｔ＋Ｔ）にかける重み行列Ｆは下記の条件式（３１）を満たすように最適制御入力を設定する、ことが好ましい。
【数２】
【発明の効果】
【００１６】
上記本発明の装置及び方法によれば、リアルタイムに検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、
作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定し、推定された各値に基づき、アシスト力を最適化するので、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させることができる。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の好ましい実施形態を説明する。なお各図において、共通する部分には同一の符号を付し、重複した説明を省略する。
【００１８】
図１は、本発明のパワーアシスト装置の全体構成図である。この図において、本発明のパワーアシスト装置は、運搬対象物１をリジットに把持する手先部１１をもった多関節アーム型ロボット１０と、このロボットを制御する制御装置２０とを備える。
ロボット１０は、多関節の関節角度ベクトルｑ、手先部位置ｘを検出するセンサ（図示せず）、及びロボットと協調作業する作業者２が運搬対象物１に加える力ｆｅを検出する６軸力センサ３を有する。
制御装置２０は、センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者２の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する。
またこの推定値に基づき、制御装置２０は、ロボット１０に入力する最適制御入力をリアルタイムに設定する。
ロボット１０は、リアルタイムで設定された最適制御入力に基づいて作動し、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、アシスト力を発生する。
【００１９】
上述した多関節アーム型ロボット１０を用い、本発明の方法は、運搬対象物１をリジットに把持する手先部１１をもった多関節アーム型ロボット１０を有するパワーアシスト装置の適応モデル予測制御方法である。
図２は、本発明の方法を示すブロック線図である。この図に示すように、本発明の方法は、データ検出ステップＳ１、推定ステップＳ２、及びパワーアシストステップＳ３を有する。
データ検出ステップＳ１では、ロボット１０の多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出する。
推定ステップＳ２では、検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する。
パワーアシストステップＳ３では、推定された各値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させる。
【００２０】
推定ステップＳ２において、逐次最小２乗法を用いて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅを推定する。この推定は、後述する式に基づくのがよい。
【００２１】
また、パワーアシストステップＳ３において、各時刻ｔごとに変化する評価関数式（２４）を最小化する最適制御入力を設定する。この設定により、位置決め誤差を最小にし、かつロボットが加える操作量を最小にすることができる。
【００２２】
また、パワーアシストステップにおいて、終端時刻にかける重み行列Ｆは条件式（３１）を満たすように最適制御入力を設定する。この設定により、後述する定理１に示すように、式（２６）で示す最適制御則の安定性を保証することができる。
以下、本発明をさらに詳細に説明する。
【００２３】
この明細書において、記号上部の点「・」は時間微分を「・・」は時間に関する２回微分を、記号「＾」は推定値を意味する。本発明では、人の手先の動特性は線形の粘弾性を持ち、式（１）と書けるものとする。ここでｘは人の手先位置、添え字ｅは作業者を意味し、ｆｅは運搬対象物に加える力、Ｄｅは作業者の粘性係数、Ｋｅはその弾性係数、ｘｅは作業者の手先の自然長位置を表す。
【００２４】
一方、アシストロボットはｎ個の関節をもつアーム型ロボットで運搬対象物をリジッドに把持しているものとする。このとき、ロボットと運搬対象物、作業者を含めた全システムの運動方程式は式（２）（３）で与えられる。ここでｑは関節角度ベクトル、Ｈ（ｑ）はロボットの慣性行列、Ｃ（ｄｑ／ｄｔ，ｑ）はコリオリ力と遠心力の和、τは関節トルク、Ｊ（ｑ）は関節角度ベクトルｑからロボット手先位置ｘまでのヤコビアンをそれぞれ表す。
【００２５】
【数３】
【００２６】
以上の問題設定の下に、本発明の「適応モデル予測制御方法」では、観測データｑ，ｄｑ／ｄｔ，ｆｅに基いて、作業者の運動状態Ｋｅ，Ｄｅ，ｘｅをリアルタイムで推定し、その推定された運動状態下で可能な限り小さい操作力で、位置決め誤差を可能なかぎり小さく抑えるような最適アシスト操作力を制御インターバル１ｍｓごとに逐次的に生成する。
【００２７】
以後、各ステップにおける、制御装置の詳細な動作について説明する。
【００２８】
データ検出ステップＳ１ではロボット１０の各関節に取り付けたロータリエンコーダによって関節角度ベクトルｑ、関節角速度ベクトルｄｑ／ｄｔを、ハンドル部分に取り付けた３軸力センサにより作業者から運搬対象物に加えられる操作力の並進３自由度成分ｆｅを測定する。手先部位置ｘは高速カメラや超音波センサ、赤外線センサ等により直接計測するか、式（４）のロボットの順キネマティクスを計算することで算出する。
【００２９】
【数４】
【００３０】
データ検出ステップＳ１で関節角速度ベクトルｄｑ／ｄｔを直接計測することができない場合には、関節角度ベクトルｑから微分フィルタを用いて算出しても構わない。
【００３１】
推定ステップＳ２ではデータ検出ステップＳ１で測定した、ロボット１０の関節角度ベクトルｑ、関節角速度ベクトルｄｑ／ｄｔ、ハンドル部分に作業者から加えられる操作力ｆｅから、作業者の運動状態を推定する。作業者の手先弾性をＫｅ、手先粘性をＤｅ、作業者が運搬対象物を運ぼうとしている位置（手先自然長位置）をｘｅとすると、作業者から運搬対象物に加えられる力は式（１）で記述される。
【００３２】
ここで作業者の手先自然長位置ｘｅは窓枠はめ込み作業中一定か、若しくは、時間に依存してなめらかに変化するものと想定する。このとき、作業者の手先自然長位置ｘｅはたとえば式（５）のような２次関数で近似することができる。
この近似関数は勿論２次関数に限られるわけではなく、必要に応じて高次関数で近似しても構わない。ただし、高次関数で近似を行う場合には計算時間が多く必要になるので、１ステップの制御入力を算出するための計算時間が１ｍｓ以内に収まる範囲で近似を行わなければならない。
【００３３】
【数５】
【００３４】
このとき、作業者の運動状態は定数ベクトル（Ｋｅ，Ｄｅ，ａ０，ｂ０，ｃ０）で完全に記述され、作業者から運搬対象物に加えられる操作力ｆｅは、式（６）と記述される。
【００３５】
これは新たに式（７）の変数を導入すると、式（８）とかける。 ここでθは作業者の運動状態を表す未知パラメータ、φは観測可能なベクトルである。
【００３６】
【数６】
【００３７】
本発明、推定ステップＳ２の実施形態としてはたとえば逐次最小２乗法に基くパラメータ推定法が考えられる。逐次最小２乗法を用いる場合には、式（９）を最小にするようなパラメータ（＾付き）θとして、式（１０）を時々刻々積分することで作業者の運動状態（Ｋｅ，Ｄｅ，ａ０，ｂ０，ｃ０）を推定する。
【００３８】
【数７】
【００３９】
別の推定ステップＳ２の実施形態としてはレシーディングホライズン最小２乗法を用いる。レシーディングホライズン最小２乗法では現在の時刻ｔから一定時間Ｔｅだけ前の時刻ｔ-Ｔｅから、現在の時刻ｔまでにわたる式（１１）の推定誤差の２乗積分を最小にするようなパラメータ（＾付き）θとして、作業者の運動状態（Ｋｅ，Ｄｅ，ａ０，ｂ０，ｃ０）を推定する。
【００４０】
【数８】
【００４１】
ここで、被積分関数第２項は推定値の連続性を保つために導入されたコストで、αは対象毎に適当に選ぶ正の定数である。
この場合、最適推定値（＾付き）θは式（１２）で与えられ、これは式（１３）（１４）の逐次アルゴリズムに従って逐次的に求められる行列Γ（ｔ）、Ｙ（ｔ）を用いて、式（１５）と計算することができる。ここでＮは数値積分の刻み数である。このようにレシーディングホライズン最小２乗法では、現在の時刻から時間前までの観測データのみに基いてパラメータ推定則を構成したことで、作業者の運動状態の変化に素早く対応して、正しい運動状態の推定値を得ることができる。
【００４２】
【数９】
【００４３】
次にパワーアシストステップＳ３の詳細について述べる。
【００４４】
人との協調作業では、人（作業員）もロボットの動きに合わせて行動を変化させることが予想される。その際に、ロボットの手先が複雑な動特性を持っていると、人は、ロボットの動きを予測することができず、操作性の低下を引き起こす原因となってしまうおそれがある。
そこで、本発明ではまず、ロボットの動力学が式（１６）で与えられるように各関節に式（１７）で示すインピーダンス制御入力を加えることにする。
ここで、添字０はロボットを意味し、Ｍ０、Ｄ０、Ｋ０はそれぞれ、ロボットの手先に指令する目標の慣性、粘性、弾性係数を表し、ｘｒはロボットの手先自然長位置を表す。以後、式（１６）のインピーダンス制御された系に対して、アシスト制御則を構成する。
【００４５】
【数１０】
【００４６】
人とロボットの協調作業において、可能な限り作業員に負担をかけないように、かつ可能な限り正確に協調位置決め作業を行うためには、何らかの最適化指標を最小化するように逐次的にロボットの軌道を修正する必要がある。
しかし、人側の運動状態は未知であり、これをリアルタイムで逐次的に推定するとしているので、最適な制御ゲインを予め求めておくことはできない。そこで、最適制御ゲインも、推定された人側の運動状態に基づいて逐次的に求める必要がある。
また、時間間隔をあけて、制御入力の修正を行うと、協調作業を行う作業員に違和感を与えてしまうおそれがある。そこで、本発明では推定されたシステムのモデルに基づいて、連続時間（制御サイクル１ｍｓ）でモデル予測制御を行うことにより、人の運動状態に適応した、最適なアシスト制御を実現することを考える。
【００４７】
図３は、適応モデル予測制御の基本概念図である。通常のモデル予測制御では、この図に示すように、システムの真のモデルに基づいて一定時間後（ｔ＋Ｔ）にシステムが行き着く状態を予測し、目標の状態との差が最小になるように、最適な制御入力を時々刻々計算する。
しかし、人とロボットの協調作業においてはシステムは未知なので、真のモデルを用いた最適制御入力を構成することができない。そこで、式（１０）もしくは式（１５）の推定方法によって得られたシステムの推定モデルに基づいて、適応モデル予測制御を行う。本発明ではこれを適応モデル予測制御と呼ぶ。
【００４８】
推定された作業者と協調作業をするロボットの運動方程式は、式（１８）となる。ここで（＾付き）ｆ０は作業者運動状態の推定値（＾付き）（ａ，ｂ，ｃ）を用いて式（１９）で表されるベクトルである。
ここで、漸近的に目標のはめ込み位置ｘｄに対象物を誘導するためにロボットの手先自然長位置ｘｒを式（２０）を積分して求められる軌道に設定する。すると作業者とロボットを合わせた全システムは、式（２１）に従うことになる。ここで、ｅは位置決め誤差ｘ-ｘｄを表す。これはさらに式（２２）の変数を導入すると、式（２３）と書くことができる。
【００４９】
【数１１】
【００５０】
このシステムの式（２３）に対して、位置決め誤差を最小にかつ、ロボットが加える操作量を最小にする制御は、式（２４）で示す各時刻ｔごとに変化する評価関数を最小化する最適制御入力を構成することで実現できる。
ここで、Ｑ、Ｒ、Ｆ、は各時刻ｔで決定される重み行列で、ＱとＲは正定値、Ｆは半正定値であるとする。
最適制御入力ｕ＊（ｔ）は各時刻ｔでリッカチ微分方程式（２５）を時刻ｔまで時間を遡って積分して求めた行列Ｓ（ｔ）を用いて、式（２６）で構成される。ここで、時不変システムに対して、次の定理１が成り立つことに注意する。
【００５１】
【数１２】
【００５２】
（定理１）
線形時不変システムの式（２７）に対して、区間［ｔ，ｔ＋Ｔ］で定義される式（２８）の最適化評価関数を最小化する最適制御入力は、式（２９）を積分して求めた行列Ｓ（ｔ）を用いて式（３０）のように構成される。
このとき、もし、終端時刻にかける重み行列Ｆが条件式（３１）が満たされるならば式（２７）に最適入力式（３０）を代入したシステムは安定で、式（３２）が漸近的に成り立つ。
【００５３】
【数１３】
【００５４】
この定理１から、たとえ推定方法が収束して、真のプラントの方程式が同定された後であっても、任意に最適化評価関数を設定することはできず、安定性を保証するには式（３１）の拘束条件を満たすようにＱ、Ｒ、Ｆを設定しなければならないことになる。
しかも今の場合にはシステム自体が時間変化するので、システムの安定性を保証するには評価関数の中の重み行列を式（３１）を満たすように時々刻々修正しなければならない。そこで、本発明による第１の評価関数設定方法では、Ｑ、Ｒを予め一定値に設定し、各時刻ｔで式（３３）をＦについて解くことで、条件式（３１）を満たすような最適化評価関数式（２４）を制御サイクルごとに求める。
【００５５】
【数１４】
【００５６】
しかし、式（３３）を解くには大きな計算時間を必要とする。そのため本発明による第２の評価関数設定方法では式（３３）よりも保守的な条件式（３４）を各時刻ｔでＦについて解くことで、条件式（３１）を満たすような最適化評価関数式（２４）を制御サイクルごとに構成する。
この条件式を用いるには行列Ａが安定である必要があるが、今考えている問題では、推定された人側弾性係数Ｋｅ（推定値）、粘性係数Ｄｅ（推定値）が式（３５）を満たす限り安定であるので、推定方法の初期値が十分大きければ（３４）式の条件を用いて、重み行列を更新することが可能である。
以上よりパワーアシストステップＳ３では，推定ステップＳ２で推定されたパラメータ（＾付）Ｋｅ，（＾付）Ｄｅ，（＾付）ｆｅと式（２６）の最適制御入力ｕ＊を用いて，各関節へ指令する制御トルクτが式（３６）のように構成される．
【００５７】
【数１５】
【００５８】
以下に本発明によるパワーアシストの実施例を示す。
【実施例１】
【００５９】
本実施例では、作業者と協調して運搬対象物を正しい位置に位置決めするタスクにおいて、作業者は正しい位置よりも１．０ｍずれた位置に誤って位置決めを行おうとしているものとし、ロボットは式（３６）に従ってその操作力を調節することで、可能な限り小さい操作力で、位置決め誤差を可能な限り小さくするように作業者を補佐するものとする。
この問題に対して適応なしのモデル予測制御則を用いてアシストを行った場合のシミュレーション結果を図４に示す。ただし、このシミュレーションでは、モデル予測制御則の中の重み行列ＲとＦは固定したままで、位置決め誤差にかける重みＱだけを変化させた。
【００６０】
図４からわかるように、適応なしでモデル予測制御を行った場合には、人を含めた協調作業系のシステムとモデル予測制御に用いられるモデルが一致しないため、定常的な誤差が残ってしまう。
【００６１】
一方、創案した制御系では適応推定器を導入したことにより、人側の運動状態を漸近的に推定することができるので、図５に示すように目標位置に向かって対象物を誘導することができる。人側の運動を考慮せずに、単純に位置決め制御を行う場合には、ロボットは目標のはめ込み位置に素早く対象物を運ぼうとするが、創案した適応モデル予測制御では重み行列Ｑを小さく取ることで人の動きになるべく追従するようにロボットを動かすことができ、人に違和感を感じさせずに、運搬対象物を目標位置に誘導することができる。
【００６２】
図５は創案した適応モデル予測制御則において、重み行列Ｒを固定した状態で、位置決め誤差にかける重みＱを変化させた場合に実現される軌道を示す。
図５から、創案した制御系では位置決め誤差にかける重み行列Ｑを大きく取ればとるほど正確に、運搬対象物を目標位置へ誘導することができることが分かる。なお、予測ホライズンを５００ステップにとった時に１個の制御入力を計算するのに要する時間は２．２ＧＨｚ、Ｐｅｎｔｉｕｍ４（登録商標）、メモリ７６８ＭＢの環境下で０．９８ｍｓ／秒であった。
【実施例２】
【００６３】
図６に水平面内のｘｙ-２自由度に対して、創案した適応モデル予測制御を用いてアシスト制御を行ったシミュレーション２の結果を示す。このシミュレーションでは、正しい運搬目標位置は（ｘ，ｙ）＝（０．１，０．１）で与えられているが、作業者は誤って、（ｘ，ｙ）＝（０，０）に位置決めを行おうとしているものとし、一方、ロボットは対象物が正しい位置にくるように力を加えることで位置決め作業を補佐するものとする。
【００６４】
この際、ｙ方向の最適化評価関数に関しては重み行列を式（３７）に固定し、ｘ方向の最適化評価関数に関してはＱ１１成分以外を式（３７）と同様にとり、Ｑ１１成分だけを変化させる。Ｑ１１＝１０００のときはｘ-ｙ方向の制御則が同一になるので、実現される軌道はほぼ直線になるが、Ｑ１１を下げていくにつれ、ｙ方向は人の手先軌道ｙ＝０に追従しようとし、ｘ方向だけが正確に目標位置へ追従するようになる。
【００６５】
【数１６】
【００６６】
これにより、たとえば自動車の窓枠はめ込み作業におけるパワーアシスト装置のように、上下方向に関しては作業者に違和感を与えないようになるべく作業者の動きに追従し、水平方向に関してはなるべく目標とするはめ込み位置からのずれを小さく抑えるようにアシストすることが要求されるタスクに対しては、上下方向の自由度に対する重みＱを小さく、水平方向の自由度に対する重みＱを大きく取ることで、上下方向に関しては作業者の動きに追従し、水平方向に関しては正しい位置へと運搬対象物を誘導するような、動作方向に依存したアシスト動作を実現することが可能になる。
【００６７】
また、本発明の制御手法では、動作中にＱを変更することで、必要に応じたアシストのモードの切り替えを行うことも可能である。
【００６８】
たとえば運搬対象物を作業者と共に所定の位置まで運び、その後で正確な位置決め動作を行うような場合には、動作の開始時にはＱを小さく設定しておき、正確な位置決め動作への移行時にＱを滑らかに大きくすることで、操作力を不連続に変化させることなく、作業者に追従するモードから正確な位置決め補佐を行うモードへとスムーズに移行することが可能になる。
【００６９】
なお、本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更できることは勿論である。
【図面の簡単な説明】
【００７０】
【図１】本発明のパワーアシスト装置の全体構成図である。
【図２】本発明の方法を示すブロック線図である。
【図３】水平方向の最適制御の基本概念図である。
【図４】適応なしのモデル予測制御則を用いてアシストを行った場合のシミュレーション結果を示す図である。
【図５】創案した適応モデル予測制御則において、重み行列Ｒを固定した状態で、位置決め誤差にかける重みＱを変化させた場合に実現される軌道を示す図である。
【図６】水平面内のｘｙ-２自由度に対して、創案した適応モデル予測制御を用いてアシスト制御を行ったシミュレーション２の結果を示す図である。
【符号の説明】
【００７１】
１ 運搬対象物、２ 作業者、３ ６軸力センサ
１０ ロボット、１１ 手先部、２０ 制御装置
【特許請求の範囲】
【請求項１】
運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットと、該ロボットを制御する制御装置とを備え、
前記ロボットは、多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するセンサを有し、
前記制御装置は、前記センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定し、
該推定値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させる、ことを特徴とするパワーアシスト装置。
【請求項２】
運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットを有するパワーアシスト装置の適応モデル予測制御方法であって、
前記ロボットの多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するデータ検出ステップと、
検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する推定ステップと、
推定された各値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させるパワーアシストステップと、を有することを特徴とするパワーアシスト装置の適応モデル予測制御方法。
【請求項３】
前記推定ステップにおいて、逐次最小２乗法を用いて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅを推定する、ことを特徴とする請求項２に記載のパワーアシスト装置の適応モデル予測制御方法。
【請求項４】
前記パワーアシストステップにおいて、各時刻ｔごとに変化する下記の評価関数式（２４）を最小化する最適制御入力を設定する、
【数１】
ことを特徴とする請求項２に記載のパワーアシスト装置の適応モデル予測制御方法。
【請求項５】
前記パワーアシストステップにおいて、評価関数式（２４）内の終端位置決め誤差ｅ（ｔ＋Ｔ）にかける重み行列Ｆは下記の条件式（３１）を満たすように最適制御入力を設定する、
【数２】
ことを特徴とする請求項４に記載のパワーアシスト装置の適応モデル予測制御方法。
【請求項１】
運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットと、該ロボットを制御する制御装置とを備え、
前記ロボットは、多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するセンサを有し、
前記制御装置は、前記センサで検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定し、
該推定値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させる、ことを特徴とするパワーアシスト装置。
【請求項２】
運搬対象物をリジットに把持する手先部をもった多関節アーム型ロボットを有するパワーアシスト装置の適応モデル予測制御方法であって、
前記ロボットの多関節の関節角度ベクトルｑ、手先部位置ｘ、及びロボットと協調作業する作業者が運搬対象物に加える力ｆｅを検出するデータ検出ステップと、
検出された関節角度ベクトルｑ、手先部位置ｘ、及び作業者の力ｆｅに基づいて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅをリアルタイムに推定する推定ステップと、
推定された各値に基づき、可能な限り小さい操作力で、位置決め誤差を可能な限り小さく抑えるように、ロボットにアシスト力を発生させるパワーアシストステップと、を有することを特徴とするパワーアシスト装置の適応モデル予測制御方法。
【請求項３】
前記推定ステップにおいて、逐次最小２乗法を用いて、作業者の手先粘性係数Ｄｅ、その弾性係数Ｋｅ、及び作業者の手先自然長位置ｘｅを推定する、ことを特徴とする請求項２に記載のパワーアシスト装置の適応モデル予測制御方法。
【請求項４】
前記パワーアシストステップにおいて、各時刻ｔごとに変化する下記の評価関数式（２４）を最小化する最適制御入力を設定する、
【数１】
ことを特徴とする請求項２に記載のパワーアシスト装置の適応モデル予測制御方法。
【請求項５】
前記パワーアシストステップにおいて、評価関数式（２４）内の終端位置決め誤差ｅ（ｔ＋Ｔ）にかける重み行列Ｆは下記の条件式（３１）を満たすように最適制御入力を設定する、
【数２】
ことを特徴とする請求項４に記載のパワーアシスト装置の適応モデル予測制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２００７−７６８０７（Ｐ２００７−７６８０７Ａ）
【公開日】平成１９年３月２９日（２００７．３．２９）
【国際特許分類】
【出願番号】特願２００５−２６６４３８（Ｐ２００５−２６６４３８）
【出願日】平成１７年９月１４日（２００５．９．１４）
【出願人】（５０３３５９８２１）独立行政法人理化学研究所 (1,056)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【公開日】平成１９年３月２９日（２００７．３．２９）
【国際特許分類】
【出願日】平成１７年９月１４日（２００５．９．１４）
【出願人】（５０３３５９８２１）独立行政法人理化学研究所 (1,056)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
[ Back to top ]