JP2021091064A - 外乱オブザーバ及び外乱オブザーバを用いた接触点推定手段 - Google Patents

Abstract

【課題】広範なロボット作業に応用できる作業空間での、外乱の定式化を行うことができる外乱オブザーバを提供することにある。また、外力と接触する接触点位置を推定することのできる接触点推定手段を提供する。【解決手段】本発明の外乱オブザーバは、ロボットの動作が、慣性行列、速度二乗項、及び重力項が予め定義されており、且つ、ロボットの作業座標系の位置・姿勢、作業座標系内の速度、及び駆動力が測定可能であるとき、ロボットに作用する未知の外力の推定値を、外力の推定値の微分を、作業座標系のｍ次元動力学モデルを用いて、正値行列の設計パラメータと、作業座標系の位置姿勢と、ロボットの慣性行列と、遠心力とコリオリ力による速度二乗項と、重力項と、ロボットの駆動力に基づいて推定する。【選択図】図１

Description

本発明は、ロボットに作用する外力などの外乱を推定する外乱オブザーバに関する。また、外乱オブザーバを使用して、環境との接触で生じる接触力と接触点の推測を行う接触点推定手段に関する。

ロボット等の機械システムに作用する外乱には、外部環境との衝突や接触による外力が含まれる。ロボットの制御性能の向上を目的として、外乱オブザーバにより外力を推定し、制御補償する種々の試みがなされている。

特許文献１には、各軸モータへのトルク指令と各軸の速度検出値から、外乱オブザーバにより推定外乱トルクを求める技術が開示されている。特許文献２には、線形系の外乱オブザーバが開示されている。特許文献３には、外乱オブザーバの出力を非干渉化制御に利用する技術が開示されている。また、非特許文献１には、１入力非線形システムの外乱オブザーバが開示されている。非特許文献１は、漸近安定性を理論的に示している。非特許文献２には、一般的なロボットを対象としたロボット関節空間での外乱オブザーバが開示されている。非特許文献２の外乱オブザーバは、外乱が関節空間でほぼ一定であるとの条件で推定を行っている。

発明者らは、ロボットの関節に作用する摩擦力とロボットの手先に作用する外力を、関節空間内で定式化し推定することができる外乱オブザーバとロボット制御装置を発明し、特許文献４に開示した。特許文献４の外乱オブザーバは、第１補助変数と第２補助変数を用いて推定誤差の微分値を定義し、関節駆動力、関節変位及び速度、関節駆動力及び外力の作用点を入力し、ヤコビ行列に基づいて第１補助変数と前記第２補助変数を算出することにより、推定外乱を算出する。

特開２０１２−１１４０３号公報 特開２０１０−１２３９５７号公報 特開平１０−１２８６８８号公報 特開２０１８−５８１８１号公報

W.-H. Chen, D. J. Balance, P. J. Gawthrop, and J. O’Reilly, "A nonlinear disturbance observer for robotic manipulators," IEEE Transaction on Industrial Electronics, Vol. 47, No. 4, 2000, pp. 932-938. A. Mohammadi, M.Tavakoli, H.J. Marquez, and F. Hashemzadeh "Nonlinear disturbance observer design for robotic manipulators," Control Engineering Practice, Vol. 21, 2013, pp.253-267.

関節空間での動力学モデルを基礎に外乱を定式化して推定する外乱オブザーバの適用範囲は、ロボットに作用する一定外力の推定に限定され、接触点は既知としていた。このため、外乱オブザーバを適用できるロボット作業が限定されていた。ロボットの作業空間でロボットに作用する外力と接触点の推定を行う技術は、未だ確立されていない。

本発明は、上記解決すべき課題に鑑みてなされたものであって、広範なロボット作業に適用できるロボットの作業空間で、外乱の定式化を行うことができる外乱オブザーバを提供することにある。また、他の目的は、外力と接触する接触点位置を推定することのできる接触点推定手段を提供することにある。

本発明は、ロボットの推定外乱を算出する外乱オブザーバに関する。ロボットの動作は、慣性行列、速度二乗項、及び重力項が予め定義されており、且つ、前記ロボットの作業座標系の位置・姿勢、作業座標系の速度、及び駆動力が測定可能である。

本発明の外乱オブザーバは、前記式（１）の加速度の項を除外するために、
次式、

本発明はまた、接触点推定手段を提供する。本発明の接触点推定手段は、外乱オブザーバを用いて、作業座標系内のロボットの表面に外力が接触する接触点を推定する接触点推定手段であって、外乱オブザーバが出力する力とモーメントの値、および作業座標系内に定義されたロボットの外形形状のデータを入力し、ロボット表面の推定接触点の位置ベクトルと力成分との外積をモーメント成分の値に一致させることで接触点を推定することを特徴とする。

本発明の外乱オブザーバは、ロボットの作業空間で、外乱の定式化を行うことができる。さらに、本発明の外乱オブザーバによって、ノイズを多く含む加速度信号の値を必要とせずに、外力を定式化することができる。加速度信号を用いないため，大域的漸近安定性が保証される。

また、本発明の外乱オブザーバは、ロボットに作用する外力を作業空間で定式化している。関節空間で定式化した外乱オブザーバは、接触点を既知としており、不測の点での接触があるときには利用できないなど応用が限定されるが、作業空間で定式化した外乱オブザーバは接触点の推定を可能とし、不測の接触が生じても、推定した接触点に応じた作業ができ、広範囲に適用可能である。

本発明の外乱オブザーバを使用する接触点推定手段は、ロボットの作業空間で、ロボットが外力と接触する接触点と接触力を推定することができる。これにより、従来は、接触点と接触力の検出に必須であった触覚センサをロボットに取り付けることなく、接触後の障害物回避、高精度な接触作業、外力のモニタリングを実現することができる。

図１は、本発明の外乱オブザーバと接触点推定手段が行う処理の概要を示すブロック図である。 図２は、本発明の外乱オブザーバを搭載した枝打ちロボットの概形図である。 図３は、本発明の外乱オブザーバが処理に用いる座標系を示した図である。 図４は、枝打ちロボットの動作状態を示す図面代用写真であって、図４（ａ）が初期状態の枝打ちロボットを示しており、図４（ｂ）が枝との接触状態にあるロボットを示している。 図５は、枝打ちロボットの能動車輪１の進行方向に沿った変位と速度を示すグラフである。 図６（ａ）は、枝打ちロボットの各時刻における駆動力の総和を示し、図６（ｂ）は各時刻における４つの能動輪の中の駆動力の最大値を示しているグラフである。 図７（ａ）は、推定した外乱のｘ成分を示し、図７（ｂ）は、推定した外乱のｚ成分を示しているグラフである。 図８（ａ）は推定した外乱から抽出した接触力のｘ成分を示し、図８（ｂ）は推定した外乱から抽出した接触力のｚ成分を示し、図８（ｃ）は推定した外乱から抽出したモーメントのｘ成分を示し、図８（ｄ）は推定した外乱から抽出したモーメントのｚ成分を示しているグラフである。 図９（ａ）は、推定した接触点位置のｘ成分を示し、図９（ｂ）は、推定した接触点位置のｚ成分を示しているグラフである。

図１は、本発明の好適な実施形態である、外乱オブザーバと、接触点推定手段が行う処理の概要を示すブロック図である。

［ロボットの動作（ダイナミックス）のモデル化］

外乱オブザーバは、ロボットの推定外乱を算出する。ここで、作業座標系で表したｍ自由度のロボットの動作は、一般に次式で表される。

本発明において、ロボットの動作は、慣性行列、速度二乗項、及び重力項が予め定義されている。また、ロボットの作業座標系の位置・姿勢、作業座標系内の速度、及び駆動力が測定可能である。

このとき、作業座標で一定の大きさの外乱は、以下の数式モデルで表すことができる。

［作業座標空間でのロボット外乱オブザーバの構成］

式（８）に、式（５）、（９）の関係を代入することで、外力の推定値の微分を、以下の式（１）で表すことができる。

しかし、式（１）を適用する場合、オブザーバは作業座標での加速度を必要とする。加速度は一般にノイズが多く含まれるので、加速度を必要としないオブザーバとする事が好ましい。

このとき、補助変数の微分は、

以上の定式化により、外乱オブザーバの計算で加速度を必要としなくなる。本実施形態の外乱オブザーバは、作業空間でのロボットの動作を基礎に導いている点が、従来と異なる点である。

［安定解析］

本発明の外乱オブザーバが大域的漸近安定性を有していることを、リアプノフ安定理論により証明した。推定誤差の微分は式（９）と式（６）、（８）より、

さらに、この微分は、式（６）、（７）を代入して整理すると、

リアプノフ関数の候補を、

この時間微分は、

ここで、

上記の安定性の証明では、

［接触位置の推定］

本発明の外力外乱オブザーバを用いて、作業座標系でのロボットの表面に外力が接触する接触点を推定する接触点推定手段の実施形態について説明する。接触点推定手段は、外乱オブザーバが求めた外力の推定値を使用して、接触点を推定する。接触点推定手段は、ロボットと環境との接触が点接触で行われるとして、その位置を特定する。

環境との接触は点接触との仮定から、モーメント成分は、

ロボット表面の幾何学的形状は既知であるので、接触点はその表面にあり、次式で表される。

本発明の、外乱オブザーバと接触点推定手段を、枝打ちロボットに具現化した実施例を、以下に説明する。図２に、枝打ちロボットの概形図を示す。

枝打ちロボットは、能動輪１、能動輪２、能動輪３、能動輪４の計４つの能動輪で駆動する。上側に能動輪１，２、下側に能動輪３，４がある。能動輪は、鉛直軸まわりに均等配置されている。各能動輪はセルフロック機能のあるウオーム減速機構を介してモータ駆動される。さらに、操舵機構により直動と螺旋の昇降ができる。また、枝打ちロボット質量中心を円筒の外部になるように機構設計している。これにより、枝打ちロボットは動力を用いることなく円筒に静止することが可能である。また、上側の二つの能動輪１，２は第一アームにより、下側二つの能動輪は第二アームにより、円筒径に応じて円筒軸中心に向かって移動できる。これにより、枝打ちを行う対称の樹木に、円筒形状の変化があっても、ロボットの姿勢は一定に保たれる。

図３は、枝打ちロボットの座標系を示した図である。

枝打ちロボットでは、枝との想定外の接触に対し、接触位置に対応した後退動作と前進動作を組み合わせたリトライが可能となる。

枝打ちロボットの動力学モデルと外乱推定

本ロボットは低速昇降であるために速度２乗項（加速度）を無視できる。また、z軸周りの姿勢センサがないため、式（５）をロボットのベース座標系で表すと、

本実施例の枝打ちロボットは、以下のとおり動作を近似することができる。
（１）タイヤの接触点の変形は無視できる。
（２）外力の接触時には接触点の移動はない。
（３）上下のアームの駆動力は、それぞれ連結している二つの能動輪に均等に伝達される。
（４）能動車輪には滑りはなく、４つの車輪によりその平均速度で昇降する。

接触点位置の推定

枝打ちロボット動作時の外乱オブザーバと接触点推定手段による計算結果

図４に、本実施例の枝打ちロボットが、円筒状の木を直動昇降している状態を示す。図４（ａ）は、動作前の初期状態、図４（ｂ）は、上昇して枝に接触した状態を示す。木は、上昇距離１ｍにつき外径が０．０１ｍ細くなっている。枝打ちロボットは、昇降速度０．１ｍ／ｓで上昇した。そして、直動上昇により、枝が、管径０．０３２ｍ、中心径０．３２ｍの環状フレームに衝突した。その接触点座標は、およそ、

図５（ａ）に能動輪１の進行方向に沿った変位と時間の関係を示す。図５（ｂ）に能動輪１の進行方向に沿った速度と時間の関係を示す。４つの能動輪はそれぞれ速度のＰＩ制御により、ほぼ同じ波形で運動した。

図７（ａ）に推定した外乱のｘ成分を示し、図７（ｂ）に推定した外乱のｚ成分を示す。ｙ成分はほぼゼロのため省略している。直動昇降のため、ｚ軸成分が大きな値を示している。接触タイミングは−１００Ｎから−１８０Ｎに変化したときである。これより、推定した外力からも接触タイミングが読み取れることが明らかとなった。

図８の外力のデータを、式（１９）、（２０）に代入して接触位置を求めた結果を、図９に示す。過渡状態のときを除き、接触点のｘ成分とｙ成分はほぼ一定であり、その平均値は、

本発明によって、環境との接触で生じる接触力と接触点の推測を可能とする作業座標空間での外乱オブザーバと接触点推定手段を提供することができた。本発明の外乱オブザーバは、加速度信号を用いておらず、大域的漸近安定性が保証されている。また、オブザーバによる推定値は、重力項や機構の摩擦力を含むため、これらの影響を取り除くことで、接触位置の推定が可能である。本発明を適用した、枝打ちロボットによって、外乱オブザーバと接触点推定手段の有効性が確認された。

Claims (3)

1. ロボットの推定外乱を算出する外乱オブザーバであって、
   前記ロボットの動作は、慣性行列、速度二乗項、及び重力項が予め定義されており、且つ、前記ロボットの作業座標系の位置・姿勢、作業座標系内の速度、及び駆動力が測定可能であり、
   

   
2. 前記式（６）の加速度の項を除外するために、
   次式、
   

   
3. 請求項１記載の外乱オブザーバを用いて、作業座標系内のロボットの表面に外力が接触する接触点を推定する接触点推定手段であって、
   前記外乱オブザーバが出力する力とモーメントの値、および作業座標系内に定義されたロボットの外形形状のデータを入力し、
   前記ロボット表面の推定接触点の位置ベクトルと力成分との外積をモーメント成分の値に一致させることで接触点を推定することを特徴とする、接触点推定手段。
   

Images