(57)【要約】【課題】ロボットのパラメータを適切に推定すること。【解決手段】パラメータ推定装置は、複数の関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、を備えるロボットの推定パラメータを算出する。パラメータ推定装置は、Ｔｂをモータドライバから制御部への通信にかかるむだ時間、Ｔｆを制御部からモータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、初期区間（０≦ｔ≦Ｔｂ＋Ｔｆ）において、時刻Ｔｆで推定される推定パラメータが連続となるような初期値関数に基づいて離散時間で動作するパラメータ推定則を用いて推定パラメータを算出し、初期区間後（Ｔｂ＋Ｔｆ≦ｔ）において、角度センサにより検出される関節角度及び関節角速度と、目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいてパラメータ推定則を用いて推定パラメータを算出する。【選択図】図１
2016215357.tif 000002
【特許請求の範囲】
【請求項１】
複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するパラメータ推定装置であって、
Ｔｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、
初期区間（０≦ｔ≦Ｔｂ＋Ｔｆ）において、
該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出し、
前記初期区間後（Ｔｂ＋Ｔｆ≦ｔ）において、
前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出する、
ことを特徴とするパラメータ推定装置。
【請求項２】
請求項１記載のパラメータ推定装置であって、
前記推定パラメータは、前記ロボットの各リンクの質量及び重心位置を含む、ことを特徴とするパラメータ推定装置。
【請求項３】
請求項１又は２記載のパラメータ推定装置であって、
前記初期区間（０≦ｔ≦Ｔｂ＋Ｔｆ）において、
（１７）式、（１８）式及び（２５）式の初期値関数として与えられる、ｑ（ｔ−Ｔｂ）、ｑ（ドット）（ｔ−Ｔｂ）、ｑｄ（ｔ−Ｔｂ）、ｑｄ（ドット）（ｔ−Ｔｂ）、ｑｄ（２ドット）（ｔ−Ｔｂ）、及びη（ハット）（ｔ−Ｔｂ）に基づいて、前記パラメータ推定則である（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を算出し、
前記初期区間後（Ｔｂ＋Ｔｆ≦ｔ）において、
前記角度センサからフィードバックされる関節角度ｑ（ｔ）、及び関節角速度ｑ（ドット）（ｔ）と、前記設定された目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、前記パラメータ推定則である（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を算出する、
ことを特徴とするパラメータ推定装置。
【数３２】
2016215357.tif 000034
但し、上記式において、Γはパラメータ推定の速度を決定するパラメータ推定ゲイン、Ｙは、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式における外乱成分を除いたモデル化可能成分、Ｙ（アッパーバー）は、前記Ｙにローパスフィルタを掛けたもの、Λ（ｔ）は前記パラメータの推定則を安定化させるための正定値規格化行列、ｗ（ｔ）は前記パラメータの推定則のロバスト安定化を図るための修正項、τ（アッパーバー）（ｔ）は前記各関節部を動作させるための制御トルクτ（ｔ）にローパスフィルタを掛けたもの、Ｔは前記ローパスフィルタの時定数、である。
【請求項４】
請求項１乃至３のうちいずれか１項記載のパラメータ推定装置と、
前記パラメータ推定装置により算出された推定パラメータに基づいて、前記モータを制御する前記制御部と、
を備える、ことを特徴とする制御装置。
【請求項５】
請求項１乃至３のうちいずれか１項記載のパラメータ推定装置と、
前記パラメータ推定装置により算出された推定パラメータη（ハット）（ｔ＋Ｔｆ）に基づいて、（１０）式を用いて前記モータを制御するためのフィードフォワード制御信号ｕｆｆ（ｔ）を生成する前記制御部と、
を備える、ことを特徴とする制御装置。
【数３３】
2016215357.tif 000035
【請求項６】
請求項５記載の制御装置であって、
前記モータドライバは、前記制御部により生成されたフィードフォワード制御信号ｕｆｆ（ｔ）に基づいて、（１６）式を用いて前記モータを駆動する制御トルクτ（ｔ）を算出する、ことを特徴とする制御装置。
【数３４】
2016215357.tif 000036
【請求項７】
複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するパラメータ推定方法であって、
Ｔｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、
初期区間（０≦ｔ≦Ｔｂ＋Ｔｆ）において、
該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出するステップと、
前記初期区間後（Ｔｂ＋Ｔｆ≦ｔ）において、
前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出するステップと、
含む、ことを特徴とするパラメータ推定方法。
【請求項８】
複数のリンクと、該各リンクを連結する複数の関節部と、該各関節部を駆動する複数のモータと、該各モータをモータドライバを介して制御する制御部と、前記各関節部の関節角度及び関節角速度を検出する複数の角度センサと、を備えるロボットの推定パラメータを算出するプログラムであって、
Ｔｂを前記モータドライバから前記制御部への通信にかかるむだ時間、Ｔｆを前記制御部から前記モータドライバへの通信にかかるむだ時間とし、制御を開始する時刻をｔ＝０としたとき、
初期区間（０≦ｔ≦Ｔｂ＋Ｔｆ）において、
該初期区間を前記ロボットを等速運動と仮定して前記各関節部の関節角度及び関節角速度を設定し、前記ロボットの等加速度運動と仮定して前記各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を設定したときの時刻Ｔｆで推定される前記推定パラメータが連続となるような初期値関数に基づいて、前記各関節部周りの摩擦を考慮した前記ロボットの運動方程式から導出した、離散時間で動作するパラメータ推定則を用いて、前記推定パラメータを算出する処理と、
前記初期区間後（Ｔｂ＋Ｔｆ≦ｔ）において、
前記角度センサにより検出される関節角度及び関節角速度と、前記設定された各関節部の目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道と、に基づいて、前記パラメータ推定則を用いて、前記推定パラメータを算出する処理と、
をコンピュータに実行させる、ことを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数のリンクと、各リンクを連結する複数の関節部と、を有するロボットの推定パラメータを算出するパラメータ推定装置、パラメータ推定方法、プログラム及び制御装置に関するものである。
【背景技術】
【０００２】
ロボットの未知のパラメータをオンラインで推定し、該推定値に基づいてフィードフォワードトルクを算出し、算出したフィードフォワードトルクに基づいてロボットを制御する制御装置が知られている（例えば、非特許文献１参照）。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】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）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。なお、制御装置３は、例えば、ロボット１に搭載されているが、これに限定されない。制御装置３は、ロボット１外に配置され、無線などの通信路を介して、ロボット１を遠隔的に制御する構成であってもよい。
【００１２】
制御装置３は、ロボット１のパラメータを推定するパラメータ推定部３１と、パラメータ推定部３１により推定されたパラメータに基づいて、モータドライバ５を介してサーボモータ２を制御する制御部３２と、を有している。
【００１３】
パラメータ推定部３１は、パラメータ推定装置の一具体例である。パラメータ推定部３１は、離散時間で動作する推定パラメータ則を用いて、ロボット１の推定パラメータを推定する。
【００１４】
制御部３２は、モータドライバ５を介して各サーボモータ２を制御する。制御部３２は、通信路６を介してモータドライバ５に接続されている。制御部３２は、モータドライバ５に対して、フィードフォワード制御信号を送信することで、各関節部１０のサーボモータ２を制御する。
【００１５】
角度センサ４は、例えば、ロボット１の各関節部１０の関節角度、関節角速度など角度情報を検出する。角度センサ４は、通信路６を介して、制御装置３のパラメータ推定部３１に接続されている。角度センサ４は、例えば、エンコーダ、ポテンショメータ、レゾルバーなどで構成されている。角度センサ４は、検出した角度情報をモータドライバ５及び制御装置３のパラメータ推定部３１に送信する。
【００１６】
モータドライバ５は、各関節部１０のサーボモータ２に対して制御信号を送信することで、各サーボモータ２を駆動する。モータドライバ５は、例えば、１００[μｓｅｃ]周期で高速に電流制御を行なうことができる増幅器である。モータドライバ５は、ＰＤフィードバック部５１と、加算部５２と、を有している。
【００１７】
ＰＤフィードバック部５１は、角度センサ４からフィードバックされる角度情報に基づいて、ＰＤフィードバック制御信号を生成し、加算部５２に出力する。加算部５２は、ＰＤフィードバック部５１からのＰＤフィードバック制御信号と、制御部３２からのフィードフォワード制御信号と、を加算して、サーボモータ２の制御トルクを算出する。加算部５２は、算出した制御トルクに応じた制御信号を各関節部１０のサーボモータ２に送信する。
【００１８】
ここで、後述の説明で用いられるパラメータについて説明する。
ｑは、ロボット１の第ｉ関節の関節角度をθｉとして、それを縦に並べたベクトルである。
【数１】
2016215357.tif 000003
【００１９】
Ｎはロボット１のリンクの数（又は関節の数）、ｑｄはロボット１の関節角度が従うべき目標関節角軌道を縦に並べたベクトル、Ｈ（ｑ）は慣性行列を表しＮ×Ｎ正定対称行列、Ｓ（ｑ（ドット）、ｑ）は第ｉ、ｊ要素が下記式で定義される、コリオリ力と遠心力に関係するＮ×Ｎ歪対称行列である。
【数２】
2016215357.tif 000004
【００２０】
なお、上記のように、ｑの１階微分値をｑ（ドット）として記載し、ｑの２階微分値をｑ（２ドット）と記載する。以降、他のパラメータについても同様に表記する。
ｇ（ｑ）は各関節周りに作用する重力トルク、Ｆｃｓｇｎ（ｑ（ドット））は各関節周りに生じるクーロン摩擦力、Ｆγｑ（ドット）は各関節周りに生じる粘性摩擦力である。
【００２１】
【数３】
2016215357.tif 000005
上式は各関節周りに生じる静止摩擦力である。
【００２２】
αｓは静止摩擦力が働き始めるStribeck速度、α０は静止摩擦力の大きさＦｓを推定する際に用いるStribeck速度αｓのノミナル値、ｎｅは各関節周りに働くノイズの外乱トルク（もしくは外力）、ηは各リンクの質量と重心位置を示すベクトルを縦に並べた物理パラメータベクトル、η（ハット）は物理パラメータベクトルの推定値（以下、他のパラメータについても、パラメータに（ハット）を付記したものは推定値とする）、Ｔｆは制御装置３の制御部３２からモータドライバ５への通信にかかるむだ時間、Ｔｂはモータドライバ５から制御装置３の制御部３２への通信にかかるむだ時間、Ｔは後述のローパスフィルタの時定数、ｕｆｆは制御装置３の制御部３２から出力されるフィードフォワード制御信号、Ｋは比例ゲイン、Ｄは微分ゲイン、Ｉは単位行列、Γ（＝γＩ）はパラメータ推定則の速度を決定するパラメータ推定ゲイン、δは微小な正定数、τはサーボモータ２の制御トルク、である。
【００２３】
ところで、従来のパラメータ推定装置は、微分式を用いてロボットの未知のパラメータを推定している。このため、例えば、制御部とモータドライバとの間の通信路においてむだ時間が生じ、運動中のロボットの状態量と、パラメータ推定則に用いられる状態量との間にむだ時間差が生じることがある。この場合に、下記のようにパラメータを適切に推定できない虞がある。なお、むだ時間とは、制御部とモータドライバとの間の通信に生じる時間のことである。例えば、制御部がモータドライバに対して遠隔にある場合、制御部から発信された信号が、モータドライバにて受信されるまでに時間がかかる。このように、信号の伝達にかかる時間がむだ時間である。このようなむだ時間は、信号の伝達手段が有線であっても無線であっても発生する。
【００２４】
例えば、従来のパラメータ推定装置は、下記式を用いてパラメータηを推定している。
【数４】
2016215357.tif 000006
ここで、ｅ＝ｑ（ドット）−ｑｄ（ドット）＋ｋ（ｑ−ｑｄ）とする。
上記式の場合、ＹＴ（ｑ（２ドット）ｄ−Ｋ（ｑ（ドット）−ｑ（ドット）ｄ）、ｑ（ドット）ｄ−Ｋ（ｑ−ｑｄ）、ｑ（ドット）、ｑ））の項は、関節角度に依存するため、有界とは限らない。したがって、パラメータη（ｔ）を適切に推定できない虞がある。
【００２５】
また、上記推定したパラメータη（ｔ）を用いて下記式によりサーボモータの制御トルクτ（ｔ）を算出する。下記式の場合、Ｙ（ｑ（２ドット）ｄ−Ｋ（ｑ（ドット）−ｑ（ドット）ｄ）、ｑ（ドット）ｄ−Ｋ（ｑ−ｑｄ）、ｑ（ドット）、ｑ）の項は、観測される関節角度に依存している。このため、フィードバック系に遅延が存在する場合に、正しいフィードフォワード補償トルクを生成することが困難となる。
【数５】
2016215357.tif 000007
【００２６】
これに対し、本実施形態に係るパラメータ推定部３１は、下記式で示す離散時間で動作する推定パラメータ則を用いて、ロボット１の推定パラメータη（ハット）（ｔ＋Ｔｆ）を算出する。
【数６】
2016215357.tif 000008
【００２７】
ここで、ｅ＝ｑ（ドット）−ｑｄ（ドット）＋λmin（ｋ）ｋ−１ａ（ｑ−ｑｄ）とし、λmin（ｋ）は行列の最小固有値を表し、ａは飽和関数を表す。
上記式の右辺２項目のΓ−１の項[Ｙ（ｑ（２ドット）ｄ（ｔ−Ｔｂ）、ｑ（ドット）ｄ（ｔ−Ｔｂ）、ｑｄ（ｔ−Ｔｂ））Ｔ]は、後述のように目標関節角軌道、目標関節角速度軌道、及び目標関節角加速度軌道を示し、これは有界となる。また、上記式の右辺３項目のΓ−１の項（Γ−１Ｙ（アッパーバー）（ｑ（ドット）（ｔ−Ｔｂ）、ｑ（ｔ−Ｔｂ）、ｑｄ（ｔ−Ｔｂ））Ｔ・・・η（ハット）（ｔ−Ｔｂ））は、このΓにより規格化されているのでその係数行列は有界となる。さらに、上記式の右辺４項目のΓ−１の項（Γ−１ｗ（ｔ−Ｔｂ）η（ハット）（ｔ−Ｔｂ））は、パラメータ推定則のロバスト安定化項を示している。以上により、上記式で示す離散時間で動作する推定パラメータ則を用いれば、上記むだ時間が生じた場合でも、確実に有界を得ることができ、ロボット１の推定パラメータを適切に推定できる。
【００２８】
次に、上述した本実施形態に係るパラメータ推定部のパラメータ推定則の導出方法について詳細に説明する。まず、ロボット１の関節周りの摩擦を考慮したロボット１の運動方程式は、下記（１）式で表すことができる。
【数７】
2016215357.tif 000009
【００２９】
上記（１）式を、下記（２）式で示す未知の物理パラメータηに線形なモデル化可能成分と、下記（３）式で示すそれ以外の外乱成分ｄ（ｔ）と、に分解する。
【数８】
2016215357.tif 000010
【００３０】
上記（２）式の両辺に下記（４）式の伝達関数で表されるローパスフィルタを掛けると下記（５）式のように記述できる。
【数９】
2016215357.tif 000011
【００３１】
サーボモータ２の制御トルクτ（ｔ）及び外乱成分ｄ（ｔ）に上記（４）式のローパスフィルタを夫々かけたものを下記（６）式及び（７）式のように記述すると、上記（１）式の両辺に上記（４）式のローパスフィルタを掛けたものは、下記（８）式のように記述できる。以下、パラメータに（アッパーバー）を付記したものは、そのパラメータにローパスフィルタを掛けたものである。
【数１０】
2016215357.tif 000012
【００３２】
以上の（１）式乃至（８）式を用いて、ロボット１のパラメータ推定則は、下記（９）式で表すことができる。パラメータ推定部３１は、下記（９）式を用いて、ロボット１の推定パラメータη（ハット）（ｔ＋Ｔｆ）の推定を行う。また、制御部３２は、パラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔｆ）に基づいて、下記（１０）式を用いて、フィードフォワード制御信号ｕｆｆを算出する。
【数１１】
2016215357.tif 000013
【００３３】
但し、上記（９）式の右辺の各係数において、σ０を任意の正定数（通常１）、Ｍ０を真の物理パラメータのノルム||η||の最大値とし、βを任意の正の定数とし、下記（１１）乃至（１４）で与えるものとする。
【数１２】
2016215357.tif 000014
【００３４】
なお、上記パラメータ推定ゲインΛ（ｔ）は、上記（９）式の第４行目のη（ハット）（ｔ−Ｔｂ）に係る係数（下記式）を１よりも小さくすることでパラメータ推定則（上記（９）式）を安定化させる正定値規格化行列を表している。
【数１３】
2016215357.tif 000015
【００３５】
このパラメータ推定ゲインΛ（ｔ）は小さいほど上記（９）式のパラメータの収束性は高くなる。このため、上記（９）式を満足する範囲で出来るだけ小さな値をパラメータ推定ゲインΛ（ｔ）に設定する。また、−ｗ（ｔ）η（ハット）（ｔ）は、推定パラメータη（ハット）（ｔ）が真のパラメータの最大値Ｍ０よりも大きくなったときに、それ以上η（ハット）（ｔ）が大きくならないように抑制をかけ、パラメータ推定則のロバスト安定化を図る修正項を表している。
【００３６】
制御部３２が出力する上記（１０）式のフィードフォワード制御信号ｕｆｆ（ｔ）は、通信路６を介して、むだ時間Ｔｆ遅れて、モータドライバ５の加算部５２に伝達される。そして、加算部５２は、このフィードフォワード制御信号ｕｆｆ（ｔ）と、ＰＤフィードバック部５１が生成した下記（１５）式のＰＤフィードバック制御信号と、を加算する。
【数１４】
2016215357.tif 000016
【００３７】
加算部５２は、下記（１６）式を用いて、フィードフォワード制御信号と、ＰＤフィードバック制御信号と、を加算して増幅を行い、制御トルクτ（ｔ）を算出する。加算部５２は、上記算出した制御トルクτ（ｔ）がロボットの各サーボモータ２に生じるように、制御トルクτ（ｔ）に応じた制御信号（制御電流）をロボットの各サーボモータ２に出力する。
【数１５】
2016215357.tif 000017
【００３８】
なお、上記（１６）式において、Ｙ（ｑ（２ドット）ｄ（ｔ）、ｑ（ドット）ｄ（ｔ）、ｑｄ（ｔ））η（ハット）（ｔ）の項が示すように、目標関節角軌道、目標関節角速度軌道及び目標関節角加速度軌道上でフィートフォワードトルクを求めている。このため、フィードバック系に遅延が存在する場合でも、正しいフィードフォワード補償トルクを生成することができる。
【００３９】
ここで、上記（９）式で構成される推定パラメータη、Γの更新則は離散時間で動作する。このため、得られる推定パラメータが時間に関して連続になるとは限らない。
【００４０】
例えば、従来のｄη（ハット）（ｔ）／ｄｔ（微分項）をη（ハット）（ｔ＋Ｔｆ）−η（ハット）（ｔ−Ｔｂ）と離散化して近似することにより、積分項が無くなり、推定パラメータが不連続となる可能性がある。ｄη（ハット）（ｔ）／ｄｔ＝０を離散化した場合、η（ハット）（ｔ＋Ｔ）＝η（ハット）（ｔ−Ｔｂ）となる。初期値関数η（ハット）（ｔ′）（−Ｔｂ≦ｔ′＜Ｔｆ）を図２に示すように時間に関する線形軌道で与える。この場合、ｔ≧Ｔｆにおいてこの初期値関数が繰返し現れることになり軌道は不連続になる。  同様に、ｄη（ハット）（ｔ）／ｄｔ＝ｆ（ｔ）を離散化した場合、η（ハット）（ｔ＋Ｔ）＝η（ハット）（ｔ−Ｔｂ）＋ｆ（ｔ−Ｔｂ）となる。初期値関数を０とした場合でも、図３に示す如く、むだ時間Ｔの周期で不連続点が現れる。
【００４１】
以上の問題を解消するために、η（ハット）（Ｔｆ−０）＝η（ハット）（−Ｔｂ）＋ｆ（−Ｔｂ）を満たすように初期値関数：η（ハット）（ｔ′）（−Ｔｂ≦ｔ′＜Ｔｆ）を設定すればよいこととなる（図４）。
【００４２】
なお、図２乃至図４において、−ＴｂからＴｆ＋２Ｔまでの太実線を初期値関数とする。図２及び図３の場合、Ｔｆ以降、太実線が連続にならないが、図４の場合、Ｔｆ以降、太実線が連続となる。なお、上記説明において、ｆ（ｔ）は上記（９）式の右辺第２項〜第４項においてｔ−Ｔｂをｔに変更したものである。ここでは、単純な例として、ｔ＋α０としている。上述したように、システムの内部状態が連続でない場合には、システムの安定性を保証することができない。このため、内部状態が連続になるように適切に初期値関数を設定することが必要になる。
【００４３】
次に、上述した本実施形態における初期値関数の設定方法について詳細に説明する。
ロボット１の初期状態を、時刻Ｔｆにおいて推定パラメータが連続になるように以下のように設定する。まず、初期区間内の−Ｔｂ≦ｔ′＜０におけるロボットの運動を等速度運動と仮定して、下記（１７）式を設定する。
【数１６】
2016215357.tif 000018
【００４４】
また、初期区間−Ｔｂ≦ｔ′＜Ｔｆにおけるロボットの運動を等加速度運動と仮定して、下記（１８）式を設定する。
【数１７】
2016215357.tif 000019
【００４５】
このとき、時刻Ｔｆにおいて推定パラメータが連続となる条件は、上記（９）式を用いて、下記（１９）式で表すことができる。
【数１８】
2016215357.tif 000020
【００４６】
以上のように、初期区間−Ｔｂ≦ｔ′＜Ｔｆを、ロボットの等速度運動と仮定して関節角度ｑ及び関節角速度ｑ（ドット）を設定し、ロボットの等加速度運動として目標関節角軌道ｑｄ、目標関節角速度軌道ｑ（ドット）ｄ、目標関節角加速度軌道ｑ（２ドット）ｄを設定する。そして、このときに時刻Ｔｆにて推定パラメータが連続となる条件、上記（１９）式を満たせば、推定パラメータは連続となる。
【００４７】
なお、上記（１９）式は、時刻−Ｔｂにおけるローパスフィルタの内部状態を全て０とすることで、下記（２０）式及び（２１）式が成立することを指す。
【数１９】
2016215357.tif 000021
【００４８】
仮に、Ｍ０≦||η（ハット）（−Ｔｂ）||である場合、||η（ハット）（−Ｔｂ）||を新たにＭ０にすることで、上記（１４）式より下記（２２）式が成立する。
ｗ（−Ｔｂ）＝０        ・・・（２２）
【００４９】
以上の（２０）式乃至（２２）式を上記（１９）式に代入し、η（ハット）（−Ｔｂ）について解くことにより、下記（２３）式が導出される。
【数２０】
2016215357.tif 000022
【００５０】
但し、上記（２３）式の右辺の逆行列が存在しない場合、Γを適当に大きく取り直す必要がある。実際のロボット制御においては、初期状態でロボット１を静止させておくことが多いが、その場合、上記（２３）式は下記（２４）式のように表すことができる。
【数２１】
2016215357.tif 000023
【００５１】
上記（２４）式より、初期区間−Ｔｂ≦ｔ′＜Ｔｆにおける推定パラメータを時間に関する線形関数として与える。これにより、上記（１９）式の推定パラメータの初期値関数を、下記（２５）式に示すように簡略化できる。
【数２２】
2016215357.tif 000024
【００５２】
以上により、本実施形態に係るパラメータ推定部３１は、初期区間（０≦ｔ≦Ｔｂ＋Ｔｆ）において、上記導出した（１７）式、（１８）式及び（２５）式の初期値関数に基づいて、上記パラメータ推定則の（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を算出する。なお、ｔ＝０とは、ロボット１が適応制御を開始したときを指すものとする。
さらに、パラメータ推定部３１は、初期区間後（Ｔｂ＋Ｔｆ≦ｔ）において、角度センサからフィードバックされる関節角度ｑ（ｔ）、及び関節角速度ｑ（ドット）（ｔ）と、設定された目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、上記パラメータ推定則の（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を算出する。
これにより、得られる推定パラメータを時間に関して連続にしつつ、上記むだ時間が生じた場合でも、確実に有界を得ることができ、ロボット１の推定パラメータを適切に推定できる。
【００５３】
図５は、本実施形態に係る制御装置の制御系を示すブロック線図である。本実施形態に係る制御装置３は、ダイレクト適応制御とインダイレクト適応制御と、を並列に行う。
【００５４】
制御装置３のパラメータ推定部３１は、ダイレクト適応制御において、通信遅延時間だけ遅らせた目標関節角軌道ｑｄ（ｔ−Ｔｂ）、及び目標関節角速度軌道ｑｄ（ドット）（ｔ−Ｔｂ）と、角度センサ４からフィードバックされる関節角度ｑ（ｔ−Ｔｂ）、及び関節角速度ｑ（ドット）（ｔ−Ｔｂ）と、の追従偏差ｅを算出する。このとき、上記関節角度ｑ（ｔ−Ｔｂ）、関節角速度ｑ（ドット）（ｔ−Ｔｂ）は、通信路６を介して角度センサ４からフィードバックされる。この通信路６において、むだ時間Ｔｂが生じる。そして、パラメータ推定部３１は算出された追従偏差と、目標関節角軌道ｑｄ（ｔ＋Ｔｆ）、目標関節角速度軌道ｑｄ（ドット）（ｔ＋Ｔｆ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ＋Ｔｆ）と、に基づいて、上記（９）を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を推定する。制御部３２はパラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔｆ）に基づいて、上記（１０）式を用いてフィードフォワード信号ｕｆｆ（ｔ）を生成する。制御部３２は、生成したフィードフォワード信号ｕｆｆ（ｔ）をモータドライバ５の加算部５２に、通信路６を介して送信する。この通信路６において、むだ時間が生じる。
【００５５】
制御装置３のパラメータ推定部３１は、モータドライバ５の加算部５２において、ダイレクト適応制御部で生成されるフィードフォワード信号ｕｆｆ（ｔ）と、ＰＤフィードバック部において（１５）式で生成されるフィードバック信号と、の和によって算出される制御トルクと、インダイレクト適用制御部で生成されるトルク推定値と、の差によってトルク推定誤差を算出する。このとき、制御トルクは、モータドライバ５の加算部５２から通信路６を介してフィートバックされる。また、ロボット１の各関節にトルクセンサが配置される場合には、トルクセンサ値を通信路６を介してパラメータ推定部３１にフィードバックしてもよい。この通信路６において、むだ時間が生じる。
【００５６】
パラメータ推定部３１は、算出されたトルク推定誤差と、角度センサ４からフィードバックされる関節角度ｑ（ｔ−Ｔｂ）、及び関節角速度ｑ（ドット）（ｔ−Ｔｂ）と、に基づいて、上記（９）を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を推定する。上記関節角度ｑ（ｔ−Ｔｂ）、及び関節角速度ｑ（ドット）（ｔ−Ｔｂ）は、通信路６を介して角度センサ４からフィードバックされる。この通信路６において、むだ時間が生じる。制御部３２はパラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔｆ）に基づいて、上記（１０）式を用いてフィードフォワード信号ｕｆｆ（ｔ）を生成する。制御部３２は、生成したフィードフォワード信号ｕｆｆ（ｔ）をモータドライバ５の加算部５２に、通信路６を介して送信する。この通信路６において、むだ時間が生じる。
【００５７】
次に、本実施形態に係る制御装置の制御処理フローについて説明する。図６は、本実施形態に係る制御装置の制御処理フローを示すフローチャートである。制御装置３のパラメータ推定部３１は、ロボットを時刻Ｔｆから動作させるものとし、そのときの初期姿勢ｑ（Ｔｆ）、ｑ（ドット）（Ｔｆ）をメモリなどから取得する（ステップＳ１０１）。なお、パラメータ推定部３１は、時刻Ｔｆの初期姿勢ｑ（Ｔｆ）、ｑ（ドット）（Ｔｆ）を、角度センサ４からの関節角度情報に基づいて決定してもよい。
【００５８】
パラメータ推定部３１は、動作区間ｔ≧Ｔｆにおいて追従すべき目標関節角軌道ｑｄ（ｔ）、目標関節角速度ｑｄ（ドット）（ｔ）、及び目標関節角加速度ｑｄ（２ドット）（ｔ）を設定する（ステップＳ１０２）。なお、目標関節角軌道ｑｄ（ｔ）、目標関節角速度ｑｄ（ドット）（ｔ）、及び目標関節角加速度ｑｄ（２ドット）（ｔ）は、予めメモリなどに設定されていてもよい。この場合、パラメータ推定部３１は、メモリなどから設定された目標関節角軌道ｑｄ（ｔ）、目標関節角速度ｑｄ（ドット）（ｔ）、及び目標関節角加速度ｑｄ（２ドット）（ｔ）を読み込む。
【００５９】
パラメータ推定部３１は、取得した初期姿勢ｑ（Ｔｆ）、ｑ（ドット）（Ｔｆ）と、設定した目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、上記（１７）式、（１８）式、及び（２５）式を用いて、初期区間−Ｔｂ≦ｔ≦Ｔｆにおける初期値関数ｑ（ｔ）、ｑ（ドット）（ｔ）、ｑｄ（ｔ）、ｑｄ（ドット）（ｔ）、ｑｄ（２ドット）（ｔ）、η（ハット）（ｔ）を算出する（ステップＳ１０３）。
【００６０】
パラメータ推定部３１は、上記求めた初期値関数の時刻−Ｔｂにおける値に基づいて、上記（９）式を用いて、η（ハット）（Ｔｆ）を算出する。そして、制御部３２は、パラメータ推定部３１により推定されたη（ハット）（Ｔｆ）と、設定した目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、上記（１０）式を用いて、フィードフォワード信号ｕｆｆ（０）を算出する。制御部３２は、算出したフィードフォワード制御信号ｕｆｆ（０）をモータドライバ５に送信する。
【００６１】
モータドライバ５は、フィードフォワード制御信号と、上記設定した目標関節角軌道と、設定した目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、上記（１６）式を用いて、制御トルクτ（ｔ）を算出する。モータドライバ５は、算出した制御トルクに応じた制御信号をロボットのサーボモータ２に対して出力する。ロボットのサーボモータ２は、制御信号に応じてロボットの各関節部１０を駆動する。ロボットの各関節部１０の角度センサ４は、各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）を検出する。各角度センサ４は、検出した各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）をモータドライバ５及びパラメータ推定部３１に送信する。
【００６２】
パラメータ推定部３１は、初期区間（０≦ｔ≦Ｔｂ＋Ｔｆ）の場合（ステップＳ１０４のＹＥＳ）、上記（１７）式、（１８）式及び（２５）式の初期値関数として与えられる、ｑ（ｔ−Ｔｂ）、ｑ（ドット）（ｔ−Ｔｂ）、ｑｄ（ｔ−Ｔｂ）、ｑｄ（ドット）（ｔ−Ｔｂ）、ｑｄ（２ドット）（ｔ−Ｔｂ）、η（ハット）（ｔ−Ｔｂ）に基づいて、上記（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を推定する。制御部３２は、パラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔｆ）に基づいて、上記（１０）式を用いて、フィードフォワード制御信号ｕｆｆ（ｔ）に算出する。制御部３２は、算出したフィードフォワード制御信号ｕｆｆ（ｔ）をモータドライバ５に送信する（ステップＳ１０５）。
【００６３】
モータドライバ５は、フィードフォワード制御信号と、上記設定した設定した目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、上記（１６）式を用いて、制御トルクτ（ｔ）を算出する。モータドライバ５は、算出した制御トルクτ（ｔ）に応じた制御信号をロボットのサーボモータ２に対して出力する（ステップＳ１０６）。ロボット１のサーボモータ２は、制御信号に応じてロボット１の各関節部１０を駆動する。ロボット１の各関節部１０の角度センサ４は、各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）を検出する。各角度センサ４は、検出した各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）をモータドライバ５及びパラメータ推定部３１に送信する。
【００６４】
パラメータ推定部３１は、初期区間後（Ｔｂ＋Ｔｆ≦ｔ）の場合（ステップＳ１０４のＮＯ)、ロボットの角度センサ４からモータドライバ５を介してフィードバックされる関節角度ｑ（ｔ）、及び関節角速度ｑ（ドット）（ｔ）と、設定した目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、上記（９）式を用いて、推定パラメータη（ハット）（ｔ＋Ｔｆ）を推定する。制御部３２は、パラメータ推定部３１により推定された推定パラメータη（ハット）（ｔ＋Ｔｆ）に基づいて、上記（１０）式を用いて、フィードフォワード制御信号ｕｆｆ（ｔ）に算出する。制御部３２は、算出したフィードフォワード制御信号ｕｆｆ（ｔ）をモータドライバ５に送信する（ステップＳ１０７）。
【００６５】
モータドライバ５は、フィードフォワード制御信号と、上記設定した目標関節角軌道ｑｄ（ｔ）、目標関節角速度軌道ｑｄ（ドット）（ｔ）、及び目標関節角加速度軌道ｑｄ（２ドット）（ｔ）と、に基づいて、上記（１６）式を用いて、制御トルクτ（ｔ）を算出する。モータドライバ５は、算出した制御トルクτ（ｔ）に応じた制御信号をロボット１のサーボモータ２に対して出力する（ステップＳ１０８）。ロボット１のサーボモータ２は、制御信号に応じてロボット１の各関節部１０を駆動する。ロボット１の各関節部１０の角度センサ４は、各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）を検出する。各角度センサ４は、検出した各関節部１０の関節角度ｑ（ｔ）及び関節角速度ｑ（ドット）（ｔ）をモータドライバ５及びパラメータ推定部３１に送信する。
【００６６】
次に、本実施形態に係るパラメータ推定部のパラメータ推定則の安定性について詳細に説明する。本実施形態に係る制御アルゴリズムに基づいて制御されたロボットの関節角軌道、関節角速度軌道、及び推定パラメータは、下記（２６）式を満たす。
【数２３】
2016215357.tif 000025
【００６７】
上記式において、ｃ４、ｃ５は慣性行列とクリストッフェル記号の上限に関係する定数、γＹは目標関節角軌道上におけるレグレッサ行列の最大値、を示している。上記（２６）式より、外乱ｄ（ｔ）が存在しないときの条件（下記（２７）式）が成立する場合、上記（２６）式の汎関数Ｖ（Δｑ（ドット）（ｔ）、Δｑ（ｔ）、Δηｔ）、Ｗ（Δｑ（ドット）（ｔ）、Δｑ（ｔ）、Δη（ｔ））は非負定値となる。この非負定値と解の連続性から、Barbalat′s Lemmaに基づいて、制御されたシステムは大域的に漸近安定となる。したがって、ロボットの関節角軌道が漸近的に目標軌道に正確に追従することが分かる。
【数２４】
2016215357.tif 000026
【００６８】
また、外乱が存在する場合にも、正の定数は、下記（２８）式で表すことができる。
【数２５】
2016215357.tif 000027
【００６９】
上記（２８）式を用いて、下記（２９）式に示すような領域を表すことができる。
【数２６】
2016215357.tif 000028
【００７０】
上記領域おいて、下記（３０）式が成立することから、Ｖ（Δｑ（ドット）（ｔ）、Δｑ（ｔ）、Δηｔ）は有界になり、Δｑ（ｔ）、Δｑ（ドット）（ｔ）、Δη（ｔ）は有界であることが分かる。
【数２７】
2016215357.tif 000029
【００７１】
次に、上記むだ時間が存在することによる問題点について詳細に説明する。上述の如く、通信路６の遅延によって、制御装置３の制御系と制御対象（ロボット）との間に時間差が生じる。これにより、制御システムを不安定化するエネルギーが制御系に入力される。
従来の積分型のパラメータ推定においては、通信路の遅延によって入力されるエネルギーは、下記（３１）式のように表すことができる。
【数２８】
2016215357.tif 000030
【００７２】
一方、通信路に遅延が無い場合は、下記（３２）式のように表すことができ、この式は保存エネルギーの時間微分となる。
【数２９】
2016215357.tif 000031
【００７３】
このように制御系によって印加されるエネルギーが、（保存エネルギー（非負定値関数）の時間微分）＋（散逸エネルギー（非負定値関数））で下から抑えられれば、通信遅延があっても制御システムの安定性は保たれる。しかし、通信遅延がある場合、上記のような形式で下から抑えることができない。
【００７４】
これに対し、本実施形態に係る離散型のパラメータ推定則においては、通信路６の遅延によって加わるエネルギーは、下記（３３）式のように表すことができる。なお、下記（３３）式は、保存エネルギーの時間微分と印加エネルギー（下記（３４）式）と、の差になる。
【数３０】
2016215357.tif 000032
【００７５】
上記印加エネルギーは、このままでは制御システムを不安定化させてしまう。しかし、本実施形態に係るパラメータ推定則に従がって、上記印加エネルギーは、下記（３５）式のような、遅延された状態誤差の２乗で表すことができる。
【数３１】
2016215357.tif 000033
【００７６】
上記（３５）式の係数行列は、有界である。このため、パラメータ推定ゲインΓが大きいときに推定則によって生成される散逸エネルギーによって、上記印加エネルギーは打ち消されることとなる。したがって、本実施形態に係る離散型のパラメータ推定則を用いることで、むだ時間が生じても制御システムは安定する。
【００７７】
次に、本実施形態に係るパラメータ推定部を用いたシミュレーション結果を説明する。図７及び図９は、通信路のむだ時間を３０ｍｓとした場合における、実現される関節角軌道を示す図である。図８及び図１０は、通信路のむだ時間を３０ｍｓとした場合における、推定される推定パラメータを示す図である。
【００７８】
なお、図７及び図８においては、ローパスフィルタ（上記（４）式）の時定数Ｔ＝０．０２、ＫゲインＫ＝５．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１００．０に設定している。図９及び図１０においては、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝５．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝５．０に設定している。
【００７９】
図７及び  図９に示す如く、パラメータ推定ゲインΓ＝１００．０およびΓ＝５．０のいずれの場合も、安定した関節角軌道の追従制御が実現されていることが分かる。また、図８及び図１０に示す如く、パラメータ推定ゲインΓ＝１００．０からΓ＝５．０に減少させることで、推定パラメータの収束速度が速くなることが分かる。
【００８０】
次に、ロボットの運動中に大きな外乱が加わった場合の制御システムの安定性について説明する。図１１及び図１３は、ロボットの運動中に５０［Ｎ］の外力がロボットの手先に加わった場合、実現される関節角軌道を示す図である。図１２及び図１４は、ロボットの運動中に５０［Ｎ］の外力がロボットの手先に加わった場合、推定される関節角軌道を示す図である。
【００８１】
なお、図１１及び  図１２においては、むだ時間＝３００ｍｓ、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝１０．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１０．０に設定している。図１３及び図１４においては、むだ時間＝５００ｍｓ、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝１０．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１０．０に設定している。
【００８２】
図１１及び図１３に示す如く、むだ時間を３００ｍｓから５００ｍｓに増加させた場合でも、漸近安定な関節角軌道の追従が可能であることが分かる。また、図１２及び図１４  に示す如く、ロボット１の手先に外乱が加わった瞬間に、推定パラメータが素早く変化していることから、外乱を推定することで外力のフィードフォワード補償が実現されていることが分かる。また、むだ時間を増加させた場合、実現される関節角軌道はほとんど変化しないが、推定パラメータの変動量が大きくなることが分かる。
【００８３】
次に、むだ時間を極めて大きくとった場合の制御システムの安定性について説明する。
図１５は、通信路のむだ時間を１０００ｍｓとした場合における、実現される関節角軌道を示す図である。図１６は、通信路のむだ時間を１０００ｍｓとした場合における、推定される推定パラメータを示す図である。
【００８４】
なお、図１５及び図１６においては、ローパスフィルタの時定数Ｔ＝０．０２、ＫゲインＫ＝１．０、ＤゲインＤ＝１．０、パラメータ推定ゲインΓ＝１０．０に設定している。
【００８５】
図１５に示す如く、むだ時間を極めて大きくとった場合でも、安定した関節角軌道の追従制御が実現できていることが分かる。
【００８６】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００８７】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００８８】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００８９】
１  ロボット、２  サーボモータ、３  制御装置、４  角度センサ、５  モータドライバ、３１  パラメータ推定部、３２  制御部、５１  ＰＤフィードバック部、５２  加算部
【図１】
2016215357.tif 000037
【図２】
2016215357.tif 000038
【図３】
2016215357.tif 000039
【図４】
2016215357.tif 000040
【図５】
2016215357.tif 000041
【図６】
2016215357.tif 000042
【図７】
2016215357.tif 000043
【図８】
2016215357.tif 000044
【図９】
2016215357.tif 000045
【図１０】
2016215357.tif 000046
【図１１】
2016215357.tif 000047
【図１２】
2016215357.tif 000048
【図１３】
2016215357.tif 000049
【図１４】
2016215357.tif 000050
【図１５】
2016215357.tif 000051
【図１６】
2016215357.tif 000052