JP2016215357A - パラメータ推定装置、パラメータ推定方法、プログラム及び制御装置 - Google Patents

Abstract

【課題】ロボットのパラメータを適切に推定すること。
【解決手段】パラメータ推定装置は、複数の関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、を備えるロボットの推定パラメータを算出する。パラメータ推定装置は、Ｔ_ｂをモータドライバから制御部への通信にかかるむだ時間、Ｔ_ｆを制御部からモータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、時刻Ｔ_ｆで推定される推定パラメータが連続となるような初期値関数に基づいて離散時間で動作するパラメータ推定則を用いて推定パラメータを算出し、初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、角度センサにより検出される関節角度及び関節角速度と、目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいてパラメータ推定則を用いて推定パラメータを算出する。
【選択図】図１

Description

本発明は、複数のリンクと、各リンクを連結する複数の関節部と、を有するロボットの推定パラメータを算出するパラメータ推定装置、パラメータ推定方法、プログラム及び制御装置に関するものである。

ロボットの未知のパラメータをオンラインで推定し、該推定値に基づいてフィードフォワードトルクを算出し、算出したフィードフォワードトルクに基づいてロボットを制御する制御装置が知られている（例えば、非特許文献１参照）。

Jean−Jacques E.Slotine and Weiping Li, Composite Adaptive Control of Robot Manipulators, Automatica, vol.25, No.4, pp.509-519, 1989

上記制御装置においては、微分式を用いてパラメータを推定している。このため、運動中のロボットの状態量と、パラメータ推定則に用いられる状態量との間にむだ時間差が生じた場合に、有界が得られずパラメータを適切に推定できない虞がある。

本発明は、このような問題点を解決するためになされたものであり、ロボットのパラメータを適切に推定できるパラメータ推定装置、パラメータ推定方法、プログラム及び制御装置を提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するパラメータ推定装置であって、
Ｔ_ｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔ_ｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔ_ｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出し、前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出する、ことを特徴とするパラメータ推定装置である。
この一態様において、前記推定パラメータは、前記ロボットの各リンクの質量及び重心位置を含んでいてもよい。
この一態様において、前記初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、（１７）式、（１８）式及び（２５）式の初期値関数として与えられる、ｑ（ｔ−Ｔ_ｂ）、ｑ（ドット）（ｔ−Ｔ_ｂ）、ｑ_ｄ（ｔ−Ｔ_ｂ）、ｑ_ｄ（ドット）（ｔ−Ｔ_ｂ）、ｑ_ｄ（２ドット）（ｔ−Ｔ_ｂ）、及びη（ハット）（ｔ−Ｔ_ｂ）に基づいて、前記パラメータ推定則である（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を算出し、前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、前記角度センサからフィードバックされる関節角度ｑ（ｔ）、及び関節角速度ｑ（ドット）（ｔ）と、前記設定された目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、前記パラメータ推定則である（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を算出してもよい。
但し、上記式において、Γはパラメータ推定の速度を決定するパラメータ推定ゲイン、Ｙは、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式における外乱成分を除いたモデル化可能成分、Ｙ（アッパーバー）は、前記Ｙにローパスフィルタを掛けたもの、Λ（ｔ）は前記パラメータの推定則を安定化させるための正定値規格化行列、ｗ（ｔ）は前記パラメータの推定則のロバスト安定化を図るための修正項、τ（アッパーバー）（ｔ）は前記各関節部を動作させるための制御トルクτ（ｔ）にローパスフィルタを掛けたもの、Ｔは前記ローパスフィルタの時定数、である。
この一態様において、前記パラメータ推定装置により算出された推定パラメータに基づいて、前記モータを制御する前記制御部と、を備えていてもよい。
この一態様において、前記パラメータ推定装置により算出された推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）に基づいて、（１０）式を用いて前記モータを制御するためのフィードフォワード制御信号ｕ_ｆｆ（ｔ）を生成する前記制御部と、を備えていてもよい。
この一態様において、前記モータドライバは、前記制御部により生成されたフィードフォワード制御信号ｕ_ｆｆ（ｔ）に基づいて、（１６）式を用いて前記モータを駆動する制御トルクτ（ｔ）を算出してもよい。
上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するパラメータ推定方法であって、
Ｔ_ｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔ_ｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔ_ｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出するステップと、前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出するステップと、含む、ことを特徴とするパラメータ推定方法であってもよい。
上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するプログラムであって、Ｔ_ｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔ_ｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔ_ｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出する処理と、前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。

本発明によれば、ロボットのパラメータを適切に推定できるパラメータ推定装置、パラメータ推定方法、プログラム及び制御装置を提供することを主たる目的とする。

一実施形態に係るロボットの概略的なシステム構成を示すブロック図である。 初期値関数が繰返し現れ軌道が不連続となる状態の一例を示す図である。 むだ時間の周期で不連続点が現れる状態の一例を示す図である。 初期値関数の軌道が連続となる状態の一例を示す図である。 一実施形態に係る制御装置の制御系を示すブロック線図である。 一実施形態に係る制御装置の制御処理フローを示すフローチャートである。 むだ時間３０ｍｓの場合の、実現される関節角軌道を示す図である。 むだ時間３０ｍｓの場合の、推定される推定パラメータを示す図である。 むだ時間３０ｍｓの場合の、実現される関節角軌道を示す図である。 むだ時間３０ｍｓの場合の、推定される推定パラメータを示す図である。 外力５０［Ｎ］が手先に加わった場合、実現される関節角軌道を示す図である。 外力５０［Ｎ］が手先に加わった場合、推定される推定パラメータを示す図である。 外力５０［Ｎ］が手先に加わった場合、実現される関節角軌道を示す図である。 外力５０［Ｎ］が手先に加わった場合、推定される推定パラメータを示す図である。 むだ時間１０００ｍｓの場合の、実現される関節角軌道を示す図である。 むだ時間１０００ｍｓの場合の、推定される推定パラメータを示す図である。

以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施形態に係るロボットは、例えば、複数のリンクと、各リンクを回動可能に連結する複数の関節部と、を有するヒューマノイドロボットなどである。

図１は、本発明の一実施形態に係るロボットの概略的なシステム構成を示すブロック図である。本実施形態に係るロボット１は、ロボット１の各関節部１０に設けられ各関節部１０を駆動する複数のサーボモータ２と、ロボットの各関節部１０のサーボモータ２を制御する制御装置３と、ロボット１の各関節部１０に設けられ、各関節部１０の関節角度情報を検出する複数の角度センサ４と、各サーボモータ２を駆動するモータドライバ５と、を備えている。

制御装置３は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。なお、制御装置３は、例えば、ロボット１に搭載されているが、これに限定されない。制御装置３は、ロボット１外に配置され、無線などの通信路を介して、ロボット１を遠隔的に制御する構成であってもよい。

制御装置３は、ロボット１のパラメータを推定するパラメータ推定部３１と、パラメータ推定部３１により推定されたパラメータに基づいて、モータドライバ５を介してサーボモータ２を制御する制御部３２と、を有している。

パラメータ推定部３１は、パラメータ推定装置の一具体例である。パラメータ推定部３１は、離散時間で動作する推定パラメータ則を用いて、ロボット１の推定パラメータを推定する。

制御部３２は、モータドライバ５を介して各サーボモータ２を制御する。制御部３２は、通信路６を介してモータドライバ５に接続されている。制御部３２は、モータドライバ５に対して、フィードフォワード制御信号を送信することで、各関節部１０のサーボモータ２を制御する。

角度センサ４は、例えば、ロボット１の各関節部１０の関節角度、関節角速度など角度情報を検出する。角度センサ４は、通信路６を介して、制御装置３のパラメータ推定部３１に接続されている。角度センサ４は、例えば、エンコーダ、ポテンショメータ、レゾルバーなどで構成されている。角度センサ４は、検出した角度情報をモータドライバ５及び制御装置３のパラメータ推定部３１に送信する。

モータドライバ５は、各関節部１０のサーボモータ２に対して制御信号を送信することで、各サーボモータ２を駆動する。モータドライバ５は、例えば、１００[μｓｅｃ]周期で高速に電流制御を行なうことができる増幅器である。モータドライバ５は、ＰＤフィードバック部５１と、加算部５２と、を有している。

ＰＤフィードバック部５１は、角度センサ４からフィードバックされる角度情報に基づいて、ＰＤフィードバック制御信号を生成し、加算部５２に出力する。加算部５２は、ＰＤフィードバック部５１からのＰＤフィードバック制御信号と、制御部３２からのフィードフォワード制御信号と、を加算して、サーボモータ２の制御トルクを算出する。加算部５２は、算出した制御トルクに応じた制御信号を各関節部１０のサーボモータ２に送信する。

ここで、後述の説明で用いられるパラメータについて説明する。
ｑは、ロボット１の第ｉ関節の関節角度をθ_ｉとして、それを縦に並べたベクトルである。

Ｎはロボット１のリンクの数（又は関節の数）、ｑ_ｄはロボット１の関節角度が従うべき目標関節角軌道を縦に並べたベクトル、Ｈ（ｑ）は慣性行列を表しＮ×Ｎ正定対称行列、Ｓ（ｑ（ドット）、ｑ）は第ｉ、ｊ要素が下記式で定義される、コリオリ力と遠心力に関係するＮ×Ｎ歪対称行列である。

なお、上記のように、ｑの１階微分値をｑ（ドット）として記載し、ｑの２階微分値をｑ（２ドット）と記載する。以降、他のパラメータについても同様に表記する。
ｇ（ｑ）は各関節周りに作用する重力トルク、Ｆ_ｃｓｇｎ（ｑ（ドット））は各関節周りに生じるクーロン摩擦力、Ｆ_γｑ（ドット）は各関節周りに生じる粘性摩擦力である。

上式は各関節周りに生じる静止摩擦力である。

α_ｓは静止摩擦力が働き始めるStribeck速度、α_０は静止摩擦力の大きさＦ_ｓを推定する際に用いるStribeck速度α_ｓのノミナル値、ｎ_ｅは各関節周りに働くノイズの外乱トルク（もしくは外力）、ηは各リンクの質量と重心位置を示すベクトルを縦に並べた物理パラメータベクトル、η（ハット）は物理パラメータベクトルの推定値（以下、他のパラメータについても、パラメータに（ハット）を付記したものは推定値とする）、Ｔ_ｆは制御装置３の制御部３２からモータドライバ５への通信にかかるむだ時間、Ｔ_ｂはモータドライバ５から制御装置３の制御部３２への通信にかかるむだ時間、Ｔは後述のローパスフィルタの時定数、ｕ_ｆｆは制御装置３の制御部３２から出力されるフィードフォワード制御信号、Ｋは比例ゲイン、Ｄは微分ゲイン、Ｉは単位行列、Γ（＝γＩ）はパラメータ推定則の速度を決定するパラメータ推定ゲイン、δは微小な正定数、τはサーボモータ２の制御トルク、である。

ところで、従来のパラメータ推定装置は、微分式を用いてロボットの未知のパラメータを推定している。このため、例えば、制御部とモータドライバとの間の通信路においてむだ時間が生じ、運動中のロボットの状態量と、パラメータ推定則に用いられる状態量との間にむだ時間差が生じることがある。この場合に、下記のようにパラメータを適切に推定できない虞がある。なお、むだ時間とは、制御部とモータドライバとの間の通信に生じる時間のことである。例えば、制御部がモータドライバに対して遠隔にある場合、制御部から発信された信号が、モータドライバにて受信されるまでに時間がかかる。このように、信号の伝達にかかる時間がむだ時間である。このようなむだ時間は、信号の伝達手段が有線であっても無線であっても発生する。

例えば、従来のパラメータ推定装置は、下記式を用いてパラメータηを推定している。

ここで、ｅ＝ｑ（ドット）−ｑ_ｄ（ドット）＋ｋ（ｑ−ｑ_ｄ）とする。
上記式の場合、Ｙ^Ｔ（ｑ（２ドット）_ｄ−Ｋ（ｑ（ドット）−ｑ（ドット）_ｄ）、ｑ（ドット）_ｄ−Ｋ（ｑ−ｑ_ｄ）、ｑ（ドット）、ｑ））の項は、関節角度に依存するため、有界とは限らない。したがって、パラメータη（ｔ）を適切に推定できない虞がある。

また、上記推定したパラメータη（ｔ）を用いて下記式によりサーボモータの制御トルクτ（ｔ）を算出する。下記式の場合、Ｙ（ｑ（２ドット）_ｄ−Ｋ（ｑ（ドット）−ｑ（ドット）_ｄ）、ｑ（ドット）_ｄ−Ｋ（ｑ−ｑ_ｄ）、ｑ（ドット）、ｑ）の項は、観測される関節角度に依存している。このため、フィードバック系に遅延が存在する場合に、正しいフィードフォワード補償トルクを生成することが困難となる。

これに対し、本実施形態に係るパラメータ推定部３１は、下記式で示す離散時間で動作する推定パラメータ則を用いて、ロボット１の推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を算出する。

ここで、ｅ＝ｑ（ドット）−ｑ_ｄ（ドット）＋λmin（ｋ）ｋ^−１ａ（ｑ−ｑ_ｄ）とし、λmin（ｋ）は行列の最小固有値を表し、ａは飽和関数を表す。
上記式の右辺２項目のΓ^−１の項[Ｙ（ｑ（２ドット）_ｄ（ｔ−Ｔ_ｂ）、ｑ（ドット）_ｄ（ｔ−Ｔ_ｂ）、ｑ_ｄ（ｔ−Ｔ_ｂ））^Ｔ]は、後述のように目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を示し、これは有界となる。また、上記式の右辺３項目のΓ^−１の項（Γ^−１Ｙ（アッパーバー）（ｑ（ドット）（ｔ−Ｔ_ｂ）、ｑ（ｔ−Ｔ_ｂ）、ｑ_ｄ（ｔ−Ｔ_ｂ））Ｔ・・・η（ハット）（ｔ−Ｔ_ｂ））は、このΓにより規格化されているのでその係数行列は有界となる。さらに、上記式の右辺４項目のΓ^−１の項（Γ^−１ｗ（ｔ−Ｔ_ｂ）η（ハット）（ｔ−Ｔ_ｂ））は、パラメータ推定則のロバスト安定化項を示している。以上により、上記式で示す離散時間で動作する推定パラメータ則を用いれば、上記むだ時間が生じた場合でも、確実に有界を得ることができ、ロボット１の推定パラメータを適切に推定できる。

次に、上述した本実施形態に係るパラメータ推定部のパラメータ推定則の導出方法について詳細に説明する。まず、ロボット１の関節周りの摩擦を考慮したロボット１の運動方程式は、下記（１）式で表すことができる。

上記（１）式を、下記（２）式で示す未知の物理パラメータηに線形なモデル化可能成分と、下記（３）式で示すそれ以外の外乱成分ｄ（ｔ）と、に分解する。

上記（２）式の両辺に下記（４）式の伝達関数で表されるローパスフィルタを掛けると下記（５）式のように記述できる。

サーボモータ２の制御トルクτ（ｔ）及び外乱成分ｄ（ｔ）に上記（４）式のローパスフィルタを夫々かけたものを下記（６）式及び（７）式のように記述すると、上記（１）式の両辺に上記（４）式のローパスフィルタを掛けたものは、下記（８）式のように記述できる。以下、パラメータに（アッパーバー）を付記したものは、そのパラメータにローパスフィルタを掛けたものである。

以上の（１）式乃至（８）式を用いて、ロボット１のパラメータ推定則は、下記（９）式で表すことができる。パラメータ推定部３１は、下記（９）式を用いて、ロボット１の推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）の推定を行う。また、制御部３２は、パラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）に基づいて、下記（１０）式を用いて、フィードフォワード制御信号ｕ_ｆｆを算出する。

但し、上記（９）式の右辺の各係数において、σ_０を任意の正定数（通常１）、Ｍ_０を真の物理パラメータのノルム||η||の最大値とし、βを任意の正の定数とし、下記（１１）乃至（１４）で与えるものとする。

なお、上記パラメータ推定ゲインΛ（ｔ）は、上記（９）式の第４行目のη（ハット）（ｔ−Ｔ_ｂ）に係る係数（下記式）を１よりも小さくすることでパラメータ推定則（上記（９）式）を安定化させる正定値規格化行列を表している。

このパラメータ推定ゲインΛ（ｔ）は小さいほど上記（９）式のパラメータの収束性は高くなる。このため、上記（９）式を満足する範囲で出来るだけ小さな値をパラメータ推定ゲインΛ（ｔ）に設定する。また、−ｗ（ｔ）η（ハット）（ｔ）は、推定パラメータη（ハット）（ｔ）が真のパラメータの最大値Ｍ_０よりも大きくなったときに、それ以上η（ハット）（ｔ）が大きくならないように抑制をかけ、パラメータ推定則のロバスト安定化を図る修正項を表している。

制御部３２が出力する上記（１０）式のフィードフォワード制御信号ｕ_ｆｆ（ｔ）は、通信路６を介して、むだ時間Ｔ_ｆ遅れて、モータドライバ５の加算部５２に伝達される。そして、加算部５２は、このフィードフォワード制御信号ｕ_ｆｆ（ｔ）と、ＰＤフィードバック部５１が生成した下記（１５）式のＰＤフィードバック制御信号と、を加算する。

加算部５２は、下記（１６）式を用いて、フィードフォワード制御信号と、ＰＤフィードバック制御信号と、を加算して増幅を行い、制御トルクτ（ｔ）を算出する。加算部５２は、上記算出した制御トルクτ（ｔ）がロボットの各サーボモータ２に生じるように、制御トルクτ（ｔ）に応じた制御信号（制御電流）をロボットの各サーボモータ２に出力する。

なお、上記（１６）式において、Ｙ（ｑ（２ドット）_ｄ（ｔ）、ｑ（ドット）_ｄ（ｔ）、ｑ_ｄ（ｔ））η（ハット）（ｔ）の項が示すように、目標関節角軌道、目標関節角速度軌道及び目標関節角加速度軌道上でフィートフォワードトルクを求めている。このため、フィードバック系に遅延が存在する場合でも、正しいフィードフォワード補償トルクを生成することができる。

ここで、上記（９）式で構成される推定パラメータη、Γの更新則は離散時間で動作する。このため、得られる推定パラメータが時間に関して連続になるとは限らない。

例えば、従来のｄη（ハット）（ｔ）／ｄｔ（微分項）をη（ハット）（ｔ＋Ｔ_ｆ）−η（ハット）（ｔ−Ｔ_ｂ）と離散化して近似することにより、積分項が無くなり、推定パラメータが不連続となる可能性がある。ｄη（ハット）（ｔ）／ｄｔ＝０を離散化した場合、η（ハット）（ｔ＋Ｔ）＝η（ハット）（ｔ−Ｔ_ｂ）となる。初期値関数η（ハット）（ｔ′）（−Ｔ_ｂ≦ｔ′＜Ｔ_ｆ）を図２に示すように時間に関する線形軌道で与える。この場合、ｔ≧Ｔ_ｆにおいてこの初期値関数が繰返し現れることになり軌道は不連続になる。 同様に、ｄη（ハット）（ｔ）／ｄｔ＝ｆ（ｔ）を離散化した場合、η（ハット）（ｔ＋Ｔ）＝η（ハット）（ｔ−Ｔ_ｂ）＋ｆ（ｔ−Ｔ_ｂ）となる。初期値関数を０とした場合でも、図３に示す如く、むだ時間Ｔの周期で不連続点が現れる。

以上の問題を解消するために、η（ハット）（Ｔ_ｆ−０）＝η（ハット）（−Ｔ_ｂ）＋ｆ（−Ｔ_ｂ）を満たすように初期値関数：η（ハット）（ｔ′）（−Ｔ_ｂ≦ｔ′＜Ｔ_ｆ）を設定すればよいこととなる（図４）。

なお、図２乃至図４において、−Ｔ_ｂからＴ_ｆ＋２Ｔまでの太実線を初期値関数とする。図２及び図３の場合、Ｔ_ｆ以降、太実線が連続にならないが、図４の場合、Ｔ_ｆ以降、太実線が連続となる。なお、上記説明において、ｆ（ｔ）は上記（９）式の右辺第２項〜第４項においてｔ−Ｔ_ｂをｔに変更したものである。ここでは、単純な例として、ｔ＋α_０としている。上述したように、システムの内部状態が連続でない場合には、システムの安定性を保証することができない。このため、内部状態が連続になるように適切に初期値関数を設定することが必要になる。

次に、上述した本実施形態における初期値関数の設定方法について詳細に説明する。
ロボット１の初期状態を、時刻Ｔ_ｆにおいて推定パラメータが連続になるように以下のように設定する。まず、初期区間内の−Ｔ_ｂ≦ｔ′＜０におけるロボットの運動を等速度運動と仮定して、下記（１７）式を設定する。

また、初期区間−Ｔ_ｂ≦ｔ′＜Ｔ_ｆにおけるロボットの運動を等加速度運動と仮定して、下記（１８）式を設定する。

このとき、時刻Ｔ_ｆにおいて推定パラメータが連続となる条件は、上記（９）式を用いて、下記（１９）式で表すことができる。

以上のように、初期区間−Ｔ_ｂ≦ｔ′＜Ｔ_ｆを、ロボットの等速度運動と仮定して関節角度ｑ及び関節角速度ｑ（ドット）を設定し、ロボットの等加速度運動として目標関節角軌道ｑ_ｄ、目標関節角速度軌道ｑ（ドット）_ｄ、目標関節角加速度軌道ｑ（２ドット）_ｄを設定する。そして、このときに時刻Ｔ_ｆにて推定パラメータが連続となる条件、上記（１９）式を満たせば、推定パラメータは連続となる。

なお、上記（１９）式は、時刻−Ｔ_ｂにおけるローパスフィルタの内部状態を全て０とすることで、下記（２０）式及び（２１）式が成立することを指す。

仮に、Ｍ_０≦||η（ハット）（−Ｔ_ｂ）||である場合、||η（ハット）（−Ｔ_ｂ）||を新たにＭ_０にすることで、上記（１４）式より下記（２２）式が成立する。
ｗ（−Ｔ_ｂ）＝０ ・・・（２２）

以上の（２０）式乃至（２２）式を上記（１９）式に代入し、η（ハット）（−Ｔ_ｂ）について解くことにより、下記（２３）式が導出される。

但し、上記（２３）式の右辺の逆行列が存在しない場合、Γを適当に大きく取り直す必要がある。実際のロボット制御においては、初期状態でロボット１を静止させておくことが多いが、その場合、上記（２３）式は下記（２４）式のように表すことができる。

上記（２４）式より、初期区間−Ｔ_ｂ≦ｔ′＜Ｔ_ｆにおける推定パラメータを時間に関する線形関数として与える。これにより、上記（１９）式の推定パラメータの初期値関数を、下記（２５）式に示すように簡略化できる。

以上により、本実施形態に係るパラメータ推定部３１は、初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、上記導出した（１７）式、（１８）式及び（２５）式の初期値関数に基づいて、上記パラメータ推定則の（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を算出する。なお、ｔ＝０とは、ロボット１が適応制御を開始したときを指すものとする。
さらに、パラメータ推定部３１は、初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、角度センサからフィードバックされる関節角度ｑ（ｔ）、及び関節角速度ｑ（ドット）（ｔ）と、設定された目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、上記パラメータ推定則の（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を算出する。
これにより、得られる推定パラメータを時間に関して連続にしつつ、上記むだ時間が生じた場合でも、確実に有界を得ることができ、ロボット１の推定パラメータを適切に推定できる。

図５は、本実施形態に係る制御装置の制御系を示すブロック線図である。本実施形態に係る制御装置３は、ダイレクト適応制御とインダイレクト適応制御と、を並列に行う。

制御装置３のパラメータ推定部３１は、ダイレクト適応制御において、通信遅延時間だけ遅らせた目標関節角軌道ｑ_ｄ（ｔ−Ｔ_ｂ）、及び目標関節角速度軌道ｑ_ｄ（ドット）（ｔ−Ｔ_ｂ）と、角度センサ４からフィードバックされる関節角度ｑ（ｔ−Ｔ_ｂ）、及び関節角速度ｑ（ドット）（ｔ−Ｔ_ｂ）と、の追従偏差ｅを算出する。このとき、上記関節角度ｑ（ｔ−Ｔ_ｂ）、関節角速度ｑ（ドット）（ｔ−Ｔ_ｂ）は、通信路６を介して角度センサ４からフィードバックされる。この通信路６において、むだ時間Ｔ_ｂが生じる。そして、パラメータ推定部３１は算出された追従偏差と、目標関節角軌道ｑ_ｄ（ｔ＋Ｔ_ｆ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ＋Ｔ_ｆ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ＋Ｔ_ｆ）と、に基づいて、上記（９）を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を推定する。制御部３２はパラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）に基づいて、上記（１０）式を用いてフィードフォワード信号ｕ_ｆｆ（ｔ）を生成する。制御部３２は、生成したフィードフォワード信号ｕ_ｆｆ（ｔ）をモータドライバ５の加算部５２に、通信路６を介して送信する。この通信路６において、むだ時間が生じる。

制御装置３のパラメータ推定部３１は、モータドライバ５の加算部５２において、ダイレクト適応制御部で生成されるフィードフォワード信号ｕ_ｆｆ（ｔ）と、ＰＤフィードバック部において（１５）式で生成されるフィードバック信号と、の和によって算出される制御トルクと、インダイレクト適用制御部で生成されるトルク推定値と、の差によってトルク推定誤差を算出する。このとき、制御トルクは、モータドライバ５の加算部５２から通信路６を介してフィートバックされる。また、ロボット１の各関節にトルクセンサが配置される場合には、トルクセンサ値を通信路６を介してパラメータ推定部３１にフィードバックしてもよい。この通信路６において、むだ時間が生じる。

パラメータ推定部３１は、算出されたトルク推定誤差と、角度センサ４からフィードバックされる関節角度ｑ（ｔ−Ｔ_ｂ）、及び関節角速度ｑ（ドット）（ｔ−Ｔ_ｂ）と、に基づいて、上記（９）を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を推定する。上記関節角度ｑ（ｔ−Ｔ_ｂ）、及び関節角速度ｑ（ドット）（ｔ−Ｔ_ｂ）は、通信路６を介して角度センサ４からフィードバックされる。この通信路６において、むだ時間が生じる。制御部３２はパラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）に基づいて、上記（１０）式を用いてフィードフォワード信号ｕ_ｆｆ（ｔ）を生成する。制御部３２は、生成したフィードフォワード信号ｕ_ｆｆ（ｔ）をモータドライバ５の加算部５２に、通信路６を介して送信する。この通信路６において、むだ時間が生じる。

次に、本実施形態に係る制御装置の制御処理フローについて説明する。図６は、本実施形態に係る制御装置の制御処理フローを示すフローチャートである。制御装置３のパラメータ推定部３１は、ロボットを時刻Ｔ_ｆから動作させるものとし、そのときの初期姿勢ｑ（Ｔ_ｆ）、ｑ（ドット）（Ｔ_ｆ）をメモリなどから取得する（ステップＳ１０１）。なお、パラメータ推定部３１は、時刻Ｔ_ｆの初期姿勢ｑ（Ｔ_ｆ）、ｑ（ドット）（Ｔ_ｆ）を、角度センサ４からの関節角度情報に基づいて決定してもよい。

パラメータ推定部３１は、動作区間ｔ≧Ｔ_ｆにおいて追従すべき目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度ｑ_ｄ（２ドット）（ｔ）を設定する（ステップＳ１０２）。なお、目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度ｑ_ｄ（２ドット）（ｔ）は、予めメモリなどに設定されていてもよい。この場合、パラメータ推定部３１は、メモリなどから設定された目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度ｑ_ｄ（２ドット）（ｔ）を読み込む。

パラメータ推定部３１は、取得した初期姿勢ｑ（Ｔ_ｆ）、ｑ（ドット）（Ｔ_ｆ）と、設定した目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、上記（１７）式、（１８）式、及び（２５）式を用いて、初期区間−Ｔ_ｂ≦ｔ≦Ｔ_ｆにおける初期値関数ｑ（ｔ）、ｑ（ドット）（ｔ）、ｑ_ｄ（ｔ）、ｑ_ｄ（ドット）（ｔ）、ｑ_ｄ（２ドット）（ｔ）、η（ハット）（ｔ）を算出する（ステップＳ１０３）。

パラメータ推定部３１は、上記求めた初期値関数の時刻−Ｔ_ｂにおける値に基づいて、上記（９）式を用いて、η（ハット）（Ｔ_ｆ）を算出する。そして、制御部３２は、パラメータ推定部３１により推定されたη（ハット）（Ｔ_ｆ）と、設定した目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、上記（１０）式を用いて、フィードフォワード信号ｕ_ｆｆ（０）を算出する。制御部３２は、算出したフィードフォワード制御信号ｕ_ｆｆ（０）をモータドライバ５に送信する。

モータドライバ５は、フィードフォワード制御信号と、上記設定した目標関節角軌道と、設定した目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、上記（１６）式を用いて、制御トルクτ（ｔ）を算出する。モータドライバ５は、算出した制御トルクに応じた制御信号をロボットのサーボモータ２に対して出力する。ロボットのサーボモータ２は、制御信号に応じてロボットの各関節部１０を駆動する。ロボットの各関節部１０の角度センサ４は、各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）を検出する。各角度センサ４は、検出した各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）をモータドライバ５及びパラメータ推定部３１に送信する。

パラメータ推定部３１は、初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）の場合（ステップＳ１０４のＹＥＳ）、上記（１７）式、（１８）式及び（２５）式の初期値関数として与えられる、ｑ（ｔ−Ｔ_ｂ）、ｑ（ドット）（ｔ−Ｔ_ｂ）、ｑ_ｄ（ｔ−Ｔ_ｂ）、ｑ_ｄ（ドット）（ｔ−Ｔ_ｂ）、ｑ_ｄ（２ドット）（ｔ−Ｔ_ｂ）、η（ハット）（ｔ−Ｔ_ｂ）に基づいて、上記（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を推定する。制御部３２は、パラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）に基づいて、上記（１０）式を用いて、フィードフォワード制御信号ｕ_ｆｆ（ｔ）に算出する。制御部３２は、算出したフィードフォワード制御信号ｕ_ｆｆ（ｔ）をモータドライバ５に送信する（ステップＳ１０５）。

モータドライバ５は、フィードフォワード制御信号と、上記設定した設定した目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、上記（１６）式を用いて、制御トルクτ（ｔ）を算出する。モータドライバ５は、算出した制御トルクτ（ｔ）に応じた制御信号をロボットのサーボモータ２に対して出力する（ステップＳ１０６）。ロボット１のサーボモータ２は、制御信号に応じてロボット１の各関節部１０を駆動する。ロボット１の各関節部１０の角度センサ４は、各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）を検出する。各角度センサ４は、検出した各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）をモータドライバ５及びパラメータ推定部３１に送信する。

パラメータ推定部３１は、初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）の場合（ステップＳ１０４のＮＯ)、ロボットの角度センサ４からモータドライバ５を介してフィードバックされる関節角度ｑ（ｔ）、及び関節角速度ｑ（ドット）（ｔ）と、設定した目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、上記（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を推定する。制御部３２は、パラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）に基づいて、上記（１０）式を用いて、フィードフォワード制御信号ｕ_ｆｆ（ｔ）に算出する。制御部３２は、算出したフィードフォワード制御信号ｕ_ｆｆ（ｔ）をモータドライバ５に送信する（ステップＳ１０７）。

モータドライバ５は、フィードフォワード制御信号と、上記設定した目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、上記（１６）式を用いて、制御トルクτ（ｔ）を算出する。モータドライバ５は、算出した制御トルクτ（ｔ）に応じた制御信号をロボット１のサーボモータ２に対して出力する（ステップＳ１０８）。ロボット１のサーボモータ２は、制御信号に応じてロボット１の各関節部１０を駆動する。ロボット１の各関節部１０の角度センサ４は、各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）を検出する。各角度センサ４は、検出した各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）をモータドライバ５及びパラメータ推定部３１に送信する。

次に、本実施形態に係るパラメータ推定部のパラメータ推定則の安定性について詳細に説明する。本実施形態に係る制御アルゴリズムに基づいて制御されたロボットの関節角軌道、関節角速度軌道、及び推定パラメータは、下記（２６）式を満たす。

上記式において、ｃ_４、ｃ_５は慣性行列とクリストッフェル記号の上限に関係する定数、γ_Ｙは目標関節角軌道上におけるレグレッサ行列の最大値、を示している。上記（２６）式より、外乱ｄ（ｔ）が存在しないときの条件（下記（２７）式）が成立する場合、上記（２６）式の汎関数Ｖ（Δｑ（ドット）（ｔ）、Δｑ（ｔ）、Δηｔ）、Ｗ（Δｑ（ドット）（ｔ）、Δｑ（ｔ）、Δη（ｔ））は非負定値となる。この非負定値と解の連続性から、Barbalat′s Lemmaに基づいて、制御されたシステムは大域的に漸近安定となる。したがって、ロボットの関節角軌道が漸近的に目標軌道に正確に追従することが分かる。

また、外乱が存在する場合にも、正の定数は、下記（２８）式で表すことができる。

上記（２８）式を用いて、下記（２９）式に示すような領域を表すことができる。

上記領域おいて、下記（３０）式が成立することから、Ｖ（Δｑ（ドット）（ｔ）、Δｑ（ｔ）、Δηｔ）は有界になり、Δｑ（ｔ）、Δｑ（ドット）（ｔ）、Δη（ｔ）は有界であることが分かる。

次に、上記むだ時間が存在することによる問題点について詳細に説明する。上述の如く、通信路６の遅延によって、制御装置３の制御系と制御対象（ロボット）との間に時間差が生じる。これにより、制御システムを不安定化するエネルギーが制御系に入力される。
従来の積分型のパラメータ推定においては、通信路の遅延によって入力されるエネルギーは、下記（３１）式のように表すことができる。

一方、通信路に遅延が無い場合は、下記（３２）式のように表すことができ、この式は保存エネルギーの時間微分となる。

このように制御系によって印加されるエネルギーが、（保存エネルギー（非負定値関数）の時間微分）＋（散逸エネルギー（非負定値関数））で下から抑えられれば、通信遅延があっても制御システムの安定性は保たれる。しかし、通信遅延がある場合、上記のような形式で下から抑えることができない。

これに対し、本実施形態に係る離散型のパラメータ推定則においては、通信路６の遅延によって加わるエネルギーは、下記（３３）式のように表すことができる。なお、下記（３３）式は、保存エネルギーの時間微分と印加エネルギー（下記（３４）式）と、の差になる。

上記印加エネルギーは、このままでは制御システムを不安定化させてしまう。しかし、本実施形態に係るパラメータ推定則に従がって、上記印加エネルギーは、下記（３５）式のような、遅延された状態誤差の２乗で表すことができる。

上記（３５）式の係数行列は、有界である。このため、パラメータ推定ゲインΓが大きいときに推定則によって生成される散逸エネルギーによって、上記印加エネルギーは打ち消されることとなる。したがって、本実施形態に係る離散型のパラメータ推定則を用いることで、むだ時間が生じても制御システムは安定する。

次に、本実施形態に係るパラメータ推定部を用いたシミュレーション結果を説明する。図７及び図９は、通信路のむだ時間を３０ｍｓとした場合における、実現される関節角軌道を示す図である。図８及び図１０は、通信路のむだ時間を３０ｍｓとした場合における、推定される推定パラメータを示す図である。

なお、図７及び図８においては、ローパスフィルタ（上記（４）式）の時定数Ｔ＝０．０２、ＫゲインＫ＝５．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１００．０に設定している。図９及び図１０においては、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝５．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝５．０に設定している。

図７及び 図９に示す如く、パラメータ推定ゲインΓ＝１００．０およびΓ＝５．０のいずれの場合も、安定した関節角軌道の追従制御が実現されていることが分かる。また、図８及び図１０に示す如く、パラメータ推定ゲインΓ＝１００．０からΓ＝５．０に減少させることで、推定パラメータの収束速度が速くなることが分かる。

次に、ロボットの運動中に大きな外乱が加わった場合の制御システムの安定性について説明する。図１１及び図１３は、ロボットの運動中に５０［Ｎ］の外力がロボットの手先に加わった場合、実現される関節角軌道を示す図である。図１２及び図１４は、ロボットの運動中に５０［Ｎ］の外力がロボットの手先に加わった場合、推定される関節角軌道を示す図である。

なお、図１１及び 図１２においては、むだ時間＝３００ｍｓ、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝１０．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１０．０に設定している。図１３及び図１４においては、むだ時間＝５００ｍｓ、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝１０．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１０．０に設定している。

図１１及び図１３に示す如く、むだ時間を３００ｍｓから５００ｍｓに増加させた場合でも、漸近安定な関節角軌道の追従が可能であることが分かる。また、図１２及び図１４ に示す如く、ロボット１の手先に外乱が加わった瞬間に、推定パラメータが素早く変化していることから、外乱を推定することで外力のフィードフォワード補償が実現されていることが分かる。また、むだ時間を増加させた場合、実現される関節角軌道はほとんど変化しないが、推定パラメータの変動量が大きくなることが分かる。

次に、むだ時間を極めて大きくとった場合の制御システムの安定性について説明する。
図１５は、通信路のむだ時間を１０００ｍｓとした場合における、実現される関節角軌道を示す図である。図１６は、通信路のむだ時間を１０００ｍｓとした場合における、推定される推定パラメータを示す図である。

なお、図１５及び図１６においては、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝１．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１０．０に設定している。

図１５に示す如く、むだ時間を極めて大きくとった場合でも、安定した関節角軌道の追従制御が実現できていることが分かる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ ロボット、２ サーボモータ、３ 制御装置、４ 角度センサ、５ モータドライバ、３１ パラメータ推定部、３２ 制御部、５１ ＰＤフィードバック部、５２ 加算部

Claims (8)

1. 複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するパラメータ推定装置であって、
   Ｔ_ｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔ_ｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、
   初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、
   該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔ_ｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出し、
   前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、
   前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出する、
   ことを特徴とするパラメータ推定装置。
2. 請求項１記載のパラメータ推定装置であって、
   前記推定パラメータは、前記ロボットの各リンクの質量及び重心位置を含む、ことを特徴とするパラメータ推定装置。
3. 請求項１又は２記載のパラメータ推定装置であって、
   前記初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、
   （１７）式、（１８）式及び（２５）式の初期値関数として与えられる、ｑ（ｔ−Ｔ_ｂ）、ｑ（ドット）（ｔ−Ｔ_ｂ）、ｑ_ｄ（ｔ−Ｔ_ｂ）、ｑ_ｄ（ドット）（ｔ−Ｔ_ｂ）、ｑ_ｄ（２ドット）（ｔ−Ｔ_ｂ）、及びη（ハット）（ｔ−Ｔ_ｂ）に基づいて、前記パラメータ推定則である（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を算出し、
   前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、
   前記角度センサからフィードバックされる関節角度ｑ（ｔ）、及び関節角速度ｑ（ドット）（ｔ）と、前記設定された目標関節角軌道ｑ_ｄ（ｔ）、目標関節角速度軌道ｑ_ｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑ_ｄ（２ドット）（ｔ）と、に基づいて、前記パラメータ推定則である（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）を算出する、
   ことを特徴とするパラメータ推定装置。
   

   

   但し、上記式において、Γはパラメータ推定の速度を決定するパラメータ推定ゲイン、Ｙは、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式における外乱成分を除いたモデル化可能成分、Ｙ（アッパーバー）は、前記Ｙにローパスフィルタを掛けたもの、Λ（ｔ）は前記パラメータの推定則を安定化させるための正定値規格化行列、ｗ（ｔ）は前記パラメータの推定則のロバスト安定化を図るための修正項、τ（アッパーバー）（ｔ）は前記各関節部を動作させるための制御トルクτ（ｔ）にローパスフィルタを掛けたもの、Ｔは前記ローパスフィルタの時定数、である。
4. 請求項１乃至３のうちいずれか１項記載のパラメータ推定装置と、
   前記パラメータ推定装置により算出された推定パラメータに基づいて、前記モータを制御する前記制御部と、
   を備える、ことを特徴とする制御装置。
5. 請求項１乃至３のうちいずれか１項記載のパラメータ推定装置と、
   前記パラメータ推定装置により算出された推定パラメータη（ハット）（ｔ＋Ｔ_ｆ）に基づいて、（１０）式を用いて前記モータを制御するためのフィードフォワード制御信号ｕ_ｆｆ（ｔ）を生成する前記制御部と、
   を備える、ことを特徴とする制御装置。
   

   
6. 請求項５記載の制御装置であって、
   前記モータドライバは、前記制御部により生成されたフィードフォワード制御信号ｕ_ｆｆ（ｔ）に基づいて、（１６）式を用いて前記モータを駆動する制御トルクτ（ｔ）を算出する、ことを特徴とする制御装置。
   

   
7. 複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するパラメータ推定方法であって、
   Ｔ_ｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔ_ｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、
   初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、
   該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔ_ｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出するステップと、
   前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、
   前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出するステップと、
   含む、ことを特徴とするパラメータ推定方法。
8. 複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するプログラムであって、
   Ｔ_ｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔ_ｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、
   初期区間（０≦ｔ≦Ｔ_ｂ＋Ｔ_ｆ）において、
   該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔ_ｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出する処理と、
   前記初期区間後（Ｔ_ｂ＋Ｔ_ｆ≦ｔ）において、
   前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出する処理と、
   をコンピュータに実行させる、ことを特徴とするプログラム。

Images