ロボット制御装置、外乱判定方法およびアクチュエータ制御方法
【課題】ロボットアームに加わる外乱を高精度に検出することが可能なロボット制御装置、外乱判定方法およびロボット制御方法。
【解決手段】実施形態のロボット制御装置は、アームの駆動軸を回転駆動するアクチュエータと、前記駆動軸の回転角度をサンプリング周期ごとに検出する第１検出部と、前記アクチュエータの駆動トルクの推定値と、前記駆動トルクの真値との差分を算出する算出部と、前記アームに加わる外乱を検出する第２検出部とを備え、前記第２検出部は、前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新する更新部と、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定する判定部とを備える。
【解決手段】実施形態のロボット制御装置は、アームの駆動軸を回転駆動するアクチュエータと、前記駆動軸の回転角度をサンプリング周期ごとに検出する第１検出部と、前記アクチュエータの駆動トルクの推定値と、前記駆動トルクの真値との差分を算出する算出部と、前記アームに加わる外乱を検出する第２検出部とを備え、前記第２検出部は、前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新する更新部と、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定する判定部とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態は、ロボット制御装置、外乱判定方法およびアクチュエータ制御方法
に関する。
【背景技術】
【０００２】
ハンドリング等を行うロボットアームを動作させる際、周囲の環境に存在する障害物と
の衝突が起こり得る。衝突後においてもロボットアームを安定して動作させるためには、
ロボットアームの衝突を検出する必要がある。
【０００３】
そこで、力覚センサ等を用いずに、ロボットアームの衝突等による外乱の影響で発生す
る外部トルクを推定することで、この外部トルクがあらかじめ設定した閾値以上の場合に
ロボットの先端部が衝突したという判定を行う方法が提案されている。
【０００４】
しかしながら、ロボットアームの動力学モデルのモデル化誤差の影響により、外部トル
クには誤差が含まれることになるために、ロボットアームに加わる外乱を高精度に検出す
ることができない。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１０−１０５１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ロボットアームに加わる外乱を高精度に検出することが可能なロボット制御装置、外乱
判定方法およびロボット制御方法を提供する。
【課題を解決するための手段】
【０００７】
実施形態のロボット制御装置は、アームの駆動軸を回転駆動するアクチュエータと、前
記アーム先端の目標位置データを生成する生成部と、前記駆動軸の回転角度をサンプリン
グ周期ごとに検出する第１検出部と、前記回転角度を用いて前記アーム先端の現在位置デ
ータを算出する第１算出部と、前記目標値位置データと前記現在位置データとを用いて、
前記アクチュエータに対する指令値を算出する第２算出部と、前記駆動軸の回転角度を用
いて前記アクチュエータを駆動する駆動トルクの推定値を算出する第３算出部と、前記駆
動トルクの推定値と、前記指令値から求まる前記駆動トルクの真値との差分を算出する第
４算出部と、前記アームに加わる外乱を検出する第２検出部とを備え、
前記第２検出部は、前記差分を変数とする時系列モデルのパラメータを推定し、前記パ
ラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新する更新
部と、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去
の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定
する判定部とを備える。
【０００８】
実施形態の外乱判定方法は、算出部が、対象物に加わったと推定される外力を算出する
ステップと、更新部が、前記外力を変数とする時系列モデルのパラメータを推定し、前記
パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新するス
テップと、判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング
周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記対象物へ加わ
る外乱の有無を判定するステップとを有する。
【０００９】
実施形態のアクチュエータ制御方法は、生成部が、前記アーム先端の目標位置データを
生成するステップと、第１検出部が、前記駆動軸の回転角度をサンプリング周期ごとに検
出するステップと、第１算出部が、前記回転角度を用いて前記アーム先端の現在位置デー
タを算出するステップと、第２算出部が、前記目標値位置データと前記現在位置データと
を用いて、前記アクチュエータに対する指令値を算出するステップと、第３算出部が、前
記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算出す
るステップと、第４算出部が、前記駆動トルクの推定値と、前記指令値から求まる前記駆
動トルクの真値との差分を算出するステップと、更新部が、前記差分を変数とする時系列
モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周
期の当該時系列モデルを更新するステップと、判定部が、前記第１サンプリング周期の時
系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデ
ルとを比較して、前記アームへの外乱の有無を判定するステップとを有する。
【図面の簡単な説明】
【００１０】
【図１】第一の実施形態に係るロボット制御装置の構成図。
【図２】図１のロボット制御装置のCPUの動作を説明するブロック図。
【図３】実施例１を説明するための説明図。
【図４】実施例２を説明するための説明図。
【図５】１段予測による予測誤差の時間応答。
【図６】２段予測による予測誤差の時間応答。
【図７】３段予測による予測誤差の時間応答。
【図８】実施例３を説明するための説明図。
【図９】第一の実施形態の第一の変形例に係るロボット制御装置の構成図。
【図１０】図９のロボット制御装置のCPUの動作を説明するブロック図。
【図１１】第一の実施形態の第二の変形例に係るロボット制御装置の構成図。
【図１２】制御対象の一例であるロボットの構成図。
【図１３】図１２のロボットのアームの構成図。
【図１４】アームの衝突の様子を示す図。
【発明を実施するための形態】
【００１１】
以下、発明を実施するための実施形態について説明する。
【００１２】
（第一の実施形態）
本実施形態において制御対象の一例とするロボットは、図１２に示すように、本体３０
０と、本体３００に設けられるアーム３１０を備える。
【００１３】
図１２に示すアーム３１０は、３つのリンク３１１（リンク３１１a，リンク３１１b，
リンク３１１c）、各リンク３１１をそれぞれ回転駆動する際の回転軸（関節）となる３
つの駆動軸３１２（駆動軸３１２a，駆動軸３１２b，駆動軸３１２c）を備えている。
【００１４】
リンク３１１aは、駆動軸３１２aを介して本体３００に回転可能に接続されている。ま
た、リンク３１１bは、駆動軸３１２bを介してリンク３１１aに、リンク３１１cは、駆動
軸３１２cを介してリンク３１１bに、それぞれ回転可能に接続されている。
【００１５】
図１３に模式的に示すように、各リンク３１１は、駆動軸３１２と、各駆動軸３１２を
回転駆動するアクチュエータであるモータ３１３と、モータ３１３の回転数を落とし、各
駆動軸３１２の回転トルクを向上させる減速機３１４と、モータ３１３の回転軸の回転角
度の変化量（以下、モータ角度θM）を検出する角度検出部３１５とを備える。なお、角
度検出部３１５としては、例えばエンコーダ等のセンサを用いることができる。
【００１６】
なお、図１２において、本体３００の内部には、このロボットを制御するためのロボッ
ト制御装置が設けられる。ロボット制御装置は、通常時にはロボットのアーム３１０を所
定の軌道に沿ってアーム３１０先端（リンクcの先端位置）の位置制御を行う。
【００１７】
また、ロボット制御装置は、図１４に示すように例えばアーム３１０が障害物へ衝突す
るなどして、アーム３１０に対して外乱が加わった際には、この外乱から衝突を高精度に
検出し、この検出結果を用いてアーム３１０先端の位置制御を行う。
【００１８】
図１は、第一の実施形態に係るロボット制御装置の構成図である。
【００１９】
第一の実施形態に係るロボット制御装置は、図１に示すように、中央演算処理装置（Ｃ
ＰＵ）１００、記憶装置（メモリ）２００を備える。
【００２０】
図１のＣＰＵ１００は３つのモジュールを有する。具体的には、アーム３１０先端の位
置を制御する位置制御部１１０と、各リンク３１１における駆動軸３１２のトルクの推定
値を算出するトルク算出部１２０と、アーム３１０のいずれかのリンク３１１における障
害物への衝突をアーム３１０全体として検出する衝突検出部１３０とを備える。
【００２１】
位置制御部１１０は、後述の位置目標値生成部１１１が生成するアーム先端の３次元位
置（以下、アーム先端位置）の目標値（以下、アーム先端位置目標値）xRと、角度検出部
３１５によるモータ角度θMの検出値に基づいて得られるアーム先端位置xとの差分をゼロ
にする方向にモータ角度θMを角度制御する。また、モータ角度θMの角度制御により減速
機３１４を介してリンク角度θLを角度制御する。これにより、位置制御部１１０は、リ
ンク角度θLの角度制御を通じて、アーム先端位置目標値xRにアーム先端位置xを追従させ
る位置制御を行う。
【００２２】
トルク算出部１２０は、位置制御部１１０からリンク角度θLを得て、アーム３１０の
動力学モデルに基づいて、各リンク３１１における駆動軸３１２のトルクの推定値（以下
、トルク推定値）を算出する。なお、トルク推定値は、各モータ３１３を駆動するのに必
要と推定される駆動トルクの推定値である。
【００２３】
衝突検出部１３０は、位置制御部１１０からリンク角度θLを、トルク算出部１２０か
らトルク推定値を得て、アーム３１０に加わる外乱により各リンク３１１に生じる外部ト
ルクを算出する。そして、この外部トルクを用いてアーム３１０全体としての衝突を検出
する。
【００２４】
以下、図１および図２を参照して、それぞれのモジュールについて詳細に説明する。な
お、以下のモジュールの各処理は、サンプリング周期（制御周期）ごとに行うものとし、
サンプリングをk（ただし、k=0,1,2,…）で表す。
【００２５】
（位置制御部）
図１に示すように、位置制御部１１０は、アーム先端位置目標値xRを生成する位置目標
生成部１１１、リンク角度θLを算出する角度算出部１１２、アーム先端位置xを算出する
位置算出部１１３、各モータ３１３に対する指令値（電流値）uを算出する指令値算出部
１１４を有する。
【００２６】
ここで、各変数（モータ角度θM，リンク角度θL，アーム先端位置x）には、それぞれ
次のような関係がある。
【数１】
【００２７】
なお、nはアーム３１１の駆動軸３１２の数（本実施形態では３）を表している。また
、Λは順運動学に基づく非線形関数で、リンク角度θLからアーム先端位置xを算出する座
標変換を行う関数である。
【００２８】
位置目標生成部１１１は、例えばメモリ２００が予め記憶する、ハンドリング動作開始
から終了までのアーム先端位置目標値の履歴を得て、現在時刻における値をアーム先端位
置目標値xRとして生成する。
【００２９】
指令値算出部１１４は、位置目標生成部１１１からアーム先端位置目標値xRを、後述の
位置算出部１１３からアーム先端位置xを得て、アーム先端位置目標値xRと、角アーム先
端位置xとの差分をゼロにする方向にモータ角度θMを角度制御するための指令値uを算出
する。
【００３０】
以下、図２を参照して、指令値算出部１１４の具体的な処理について説明する。
【００３１】
指令値算出部１１４は、アーム先端位置目標値xRと、アーム先端位置xを用いて、アー
ム先端位置の微小変化Δxを次式により算出する。
【数２】
【００３２】
ここで、アーム先端位置の微小変化Δxとリンク角度の微小変化ΔθLの関係を与えるΛ
(θL)を偏微分したn×nのヤコビ行列J(θL)を次式のように定義する。
【数３】
【００３３】
このとき、上記ヤコビ行列の逆行列である逆ヤコビ行列を用いることで、アーム先端位
置の微小変化Δxとリンク角度の微小変化ΔθLには次式の関係が成り立つ。
【数４】
【００３４】
すなわち、上式により目標リンク角度の微小変化値を算出することができるので、これ
を積分することによって、
【数５】
【００３５】
としてリンク角度目標値θLRが得られる。
【００３６】
そこで、指令値算出部１１４は、後述の角度算出部１１２が算出するリンク角θLを用
いて逆ヤコビ行列を算出する。そして、アーム先端位置の微小変化Δxと、逆ヤコビ行列
を用いてリンク角度の微小変化ΔθLを次式のように算出する。
【数６】
【００３７】
また、上記リンク角度の微小変化ΔθLを積分することで、（数５）によりリンク角度
目標値θLRを算出する。
【００３８】
そして、指令値算出部１１４は、リンク角度目標値θLRを減速比NGで除算することで、
モータ角度目標値θMRを算出する。そして、モータ角度目標値θMRを入力として、公知の
速度・位置制御則を適用することで各モータ３１３を駆動するための指令値（電流値）u
を算出する。
【００３９】
指令値算出部１１４は、上記の指令値uをモータ３１３対して与えることで、モータ３
１３を駆動しモータ角度θMを角度制御する。この際、角度検出部３１５は、この駆動に
よるモータ角度θMを検出する。
【００４０】
角度算出部１１２は、角度検出部３１５が検出するモータ角度θMを用いて、次式のよ
うにリンク角θLを算出する。
【数７】
【００４１】
位置算出部１１３は、メモリ２００が記憶する非線形関数式Λを得て、角度算出部１１
２が算出するリンク角θLを用いることで、次式のように現在時刻におけるアーム先端位
置xを算出する。
【数８】
【００４２】
上記のように位置算出部１１３が算出するアーム先端位置xは、フィードバック信号と
して指令値算出部１１４に対して入力される。
【００４３】
なお、（数８）に示すように、アーム先端位置xの算出にはリンク角度θLが必要となる
ので、角度検出部３１５が直接リンク角度θLを検出することもできる。また、減速機３
１４を用いない構成も考えられる。この場合には、モータ角度θMとリンク角度θLは一致
するため、角度算出部１１２は必ずしも必要とはならないことは明らかである。
【００４４】
（トルク算出部）
図１に示すように、トルク算出部１２０は、慣性力トルクを算出する慣性力トルク算出
部１２１、摩擦力トルクを算出する摩擦力トルク算出部１２２、重力トルクを算出する重
力トルク算出部１２３、遠心力・コリオリ力トルクを算出する遠心力・コリオリ力トルク
算出部１２４、加算部１２５を有する。
【００４５】
ここで、一般にロボットアームの動力学モデルは次式の運動方程式で与えられる。
【数９】
【００４６】
したがって、リンク角度θLの検出値を１回微分したリンク角速度、２回微分したリン
ク角加速度を求めれば、各リンクの重心位置や慣性モーメント、摩擦係数等の物理パラメ
ータを用いて、重力、慣性力、摩擦力、遠心力・コリオリ力等によるトルクの推定値を算
出することができる。
【００４７】
なお、リンク角速度は、時系列データとして得られるリンク角度θLの、それぞれ異な
るサンプリング周期に得られる２つの値の差分等により算出できる（１回微分）。また、
リンク角加速度は、それぞれ異なるサンプリング周期に得られる２つのリンク角速度の値
の差分等により算出できる（２回微分）。
【００４８】
慣性力トルク算出部１２１、摩擦力トルク算出部１２２、重力トルク算出部１２３、遠
心力・コリオリ力トルク算出部１２４は、メモリ２００に予め格納されている物理パラメ
ータ及び角度算出部１１２が算出するリンク角度を用いて、慣性力トルク、摩擦力トルク
、重力トルク、遠心力・コリオリ力トルクをそれぞれ算出する。
【００４９】
加算部１２５は、次式のように各駆動軸３２０のトルク推定値（以下では、変数にハッ
トを付して推定値を表す）を算出する。
【数１０】
【００５０】
なお、本実施形態においては、慣性力トルク、摩擦力トルク、重力トルク、遠心力・コ
リオリ力トルクについて考慮するものとしたが、遠心力・コリオリ力によるトルクは他の
トルクに比べて充分に小さい（１/１０程度）ため、遠心力・コリオリ力によるトルクに
関しては考慮しないことも可能である。この場合には、加算部１２５は、各駆動軸３２０
のトルク推定値を、慣性力トルク、摩擦力トルク、重力トルクの和により算出する。
【００５１】
（衝突検出部）
図１に示すように、衝突検出部１３０は、アーム３１０に外部から加わる外部トルクを
算出する外部トルク算出部１３１、時系列モデルのパラメータを推定しモデルを更新する
モデル更新部１３２、更新されるモデルを用いて衝突判定を行う衝突判定部１３３を有す
る。
【００５２】
ここで、外部トルクτdは、トルク推定値と駆動トルクの真値との差分により次式のよ
うに与えられる。
【数１１】
【００５３】
なお、上式における駆動トルクの真値は、モータ３１３への指令値uに、モータ３１３
のカタログ値等により得られ、メモリ２００が予め記憶するトルク定数Ktを乗算すること
で得られる。
【００５４】
外部トルクは、モデル化誤差がなく、外乱等も加わっていない理想状態においては、ゼ
ロになっているはずである。したがって、外部トルクがゼロでない場合には、アーム３１
０には外乱が加わっていることが推定できるために、この外部トルクを衝突検出に用いる
ことができる。
【００５５】
しかしながら、（数９）の動力学モデルには、慣性モーメントや摩擦係数などの物理パ
ラメータ等によるモデル化誤差が含まれている。また、（数９）の動力学モデルでは考慮
されていない機械共振により生じる弾性力トルク等の影響も存在し、低速で動作している
ときには衝突検出は機能するが、高速で動作しているときには、加減速に必要なトルク変
動と衝突によるトルク変動の区別がつかないことがある。
【００５６】
そこで、本実施形態においては、このようなモデル化誤差を吸収するために、（数１１
）の外部トルクを各駆動軸３１２において１次元の時系列信号y(k)（ただし、k=0,1,2,…
）と捉えて、次式のように表されるAR（Auto Regressive：自己回帰）モデルを当てはめ
る。
【数１２】
【００５７】
上式のARモデルは、時系列信号y(k)は，過去naステップ分の時系列信号の線形結合で表
される。ここで，a1〜anaはARパラメータであり、e(k)は残差を表す。
【００５８】
また、（数１２）のARモデルは、
【数１３】
【００５９】
と定義できる。ここで、ARパラメータαとは、ARモデルを特徴付けるための係数であり、
残差e(k)を最小にするARパラメータを適切に推定することで、ARモデルを算出することが
できる。
【００６０】
ARパラメータαの推定値は、次式のような忘却係数λ（通常0.97〜0.995の範囲から選
択する）付きの逐次最小２乗法を用いれば、リアルタイムに更新しながら算出することが
できる。
【数１４】
【００６１】
ここで、ε(k)は予測誤差、kαはゲイン、P(k)は共分散行列をそれぞれ表している。
【００６２】
外部トルク算出部１３１は、トルク算出部１２０が算出するトルク推定値と、指令値算
出部１１４が算出する指令値uにトルク定数Ktを乗算することで得られる駆動トルクを用
いて、次式のように外部トルクを算出する。この外部トルクは、アーム３１０に対して外
から加わったと推定される外力と見なすことができる。
【数１５】
【００６３】
モデル更新部１３２は、外部トルク算出部１３１が算出する外部トルクを変数として、
（数１２）で表されるARモデルに当てはめる。そして、（数１４）のように、逐次最小二
乗法を用いて全てのARパラメータの推定値を逐次推定することで、ARモデルを更新する。
【００６４】
衝突判定部１３３は、詳細は後述するようにモデル更新部１３２により逐次更新される
ARモデルに基づいて、アーム３１０の衝突の有無を判定する。
【００６５】
以下、図３乃至図８を参照して衝突判定部１３３の動作について説明する。
【００６６】
（実施例１）
衝突判定部１３３が衝突を判定するための第１の方法として、逐次推定しているARパラ
メータの時間変化を用いる方法を説明する。
【００６７】
図３（a）は、アーム３１０を衝突させた時の外部トルクとモータ角速度の時間応答の
一例であり、図３（b）は、逐次更新されるARモデルのパラメータの時間応答の一例であ
る。
【００６８】
ここでは、５次のARモデルを用いており、ARパラメータは５本表示されている。図中の
凡例のa(1)〜a(5)は、それぞれa1〜a5を示している。図３（b）では、1.16[s]前後で衝突
が起こっているが、ARパラメータのうち、最低次であるa1の変化が顕著であり、次にa3の
変化が大きい。
【００６９】
それに対して、図３（a）に示した外部トルクがゆっくりした変化であるので、衝突検
出を目的とした場合、ARパラメータの変化を検出する方法の優位性は明らかである。ここ
で、衝突が起こってもモータ角速度がすぐにゼロにならないのは、関節軸がたわんでいる
ことを示している。
【００７０】
衝突判定部１３３は、図３において最も変化の大きい最低次のARパラメータa1の変化量
を、現在のサンプリング周期kにおけるパラメータの推定値と、一つ前のサンプリング周
期k-1におけるパラメータの推定値の差分により算出する。
【００７１】
そして、いずれかの駆動軸３１２において、最低次のARパラメータの変化量の絶対値が
、メモリ２００に格納されている既定の閾値（例えば0.1）を超える場合に、アーム３１
０が衝突したものと判定する。
【００７２】
なお、逐次最小２乗法において、忘却係数λを調整することによって、ARパラメータの
変化の度合いを調整することができる。忘却係数をロボットの動作加速度が小さいときに
は0.99など値を大きくしておき、加速度が大きいときには0.98など値を小さくするなど、
忘却係数を可変にすることも衝突判定の精度を上げることに有効である。
【００７３】
また、本実施形態のように最低次のARパラメータを用いるのではなく、他のARパラメー
タを用いることで衝突を判定することも可能である。
【００７４】
（実施例２）
衝突判定部１３２が衝突を判定するための第２の方法として、多段予測を用いた予測誤
差の時間変化を用いる方法を説明する。
【００７５】
（数１２）のARモデルにおいて、ARパラメータを逐次推定しながら、イタレーションk
を進めればディジタル処理に起因するサンプル時間遅れを取り戻せる上、多段の予測をす
ることができる。まず、１段先予測値は、
【数１６】
【００７６】
で与えられ、２段先予測値、３段先予測値は、それぞれ、
【数１７】
【００７７】
で与えられる。
【００７８】
よって、１段先予測誤差は、
【数１８】
【００７９】
で与えられ、２段先予測誤差、３段先予測誤差は、それぞれ、
【数１９】
【００８０】
で与えられる。
【００８１】
図４は、外部トルクと１，２，３段先予測誤差の各時間応答を比較したものである。同
図から、１段先予測値は、外部トルクからやや遅れていることがわかる。一方、２段先予
測値はやや進んでおり、３段先予測値は明らかに進んでいる。
【００８２】
また、図５は、１段先予測誤差の時間変化から衝突を検出している様子を示している。
同じく図６は２段先予測誤差、図７は３段先予測誤差の場合である。
【００８３】
図５，６，７を比較すると、それぞれ衝突検出が可能であるが、図６の予測誤差の時間
応答の波形のレンジが比較的小さく、閾値の設定が容易である。したがって、以上からア
ーム３１０の衝突検出には２段先予測誤差を用いることが好ましい。
【００８４】
衝突判定部１３３は、（数１９）のように誤差e2(k)を算出する。そして、誤差e2(k)の
変化量を、現在のサンプリング周期kにおける誤差と、一つ前のサンプリング周期k-1にお
ける誤差の差分により算出する。
【００８５】
そして、上記の誤差の変化量の絶対値が既定の閾値を超える場合に、アーム３１０が衝
突したものと判定する。
【００８６】
（実施例３）
衝突判定部１３３が衝突を検出するための第３の方法として、ARモデルから計算できる
周波数応答の時間変化を用いる方法を説明する。
【００８７】
（数１２）のARモデルをＺ変換すると、
【数２０】
【００８８】
ここで、ARモデルの伝達関数H(z-1)=1/A(z-1)の周波数応答は、（数２０）に次式を代
入することで求めることができる。
【数２１】
【００８９】
なお、（数２０）および（数２１）において、zはZ変換を表す演算子、ωは周波数、j
は虚数単位をそれぞれ表している。
【００９０】
図８は、ARモデルから計算できる周波数応答が衝突前後で変化することを用いて衝突を
検出する様子を示したものである。前述のように衝突によって摩擦係数や慣性モーメント
が変化したと考えることで、周波数応答の変化からも衝突検出ができることは明らかであ
る。
【００９１】
図８では、一点鎖線Ｂよりも高周波側の領域において、（数９）の動力学モデルでは考
慮していない機械共振特性が検出されており、衝突検出しづらくなっている。
【００９２】
そこで、あらかじめ衝突時に変化の大きい周波数領域を調べておき、例えば、図８の一
点鎖線ＡとＢの間の周波数領域でのゲインの差をサンプリング周期ごとに積分するなど、
ウィンドウ処理をすることで衝突検出のための閾値の設定を容易にすることができる。
【００９３】
衝突判定部１３２は、各サンプリング周期において周波数応答を算出する。そして、既
定のウィンドウの範囲内で、現在のサンプリング周期kにおけるゲインと、一つ前のサン
プリング周期k-1におけるゲインの差分を周波数ごとに算出し、ゲインの差分を周波数で
積分することで、図８中の面積（C）に相当する積分値を算出する。
【００９４】
そして、上記の積分値の絶対値が既定の閾値を超える場合に、アーム３１０が衝突した
ものと判定する。
【００９５】
なお、本実施例では、ARパラメータ５個を全て用いて周波数応答を計算しているので、
実施例１のようにARパラメータ個々の時間変化で衝突を検出する方法よりも計算量は増加
するが、よりロバストな衝突検出を行うことができる。
【００９６】
以上、実施例１乃至実施例３として、３通りの衝突判定の方法について説明した。この
３通りの衝突判定方法は各々適用することも可能であるが、３つ同時に適用し、多数決を
とることで、各リンク３１１におけるより正確な衝突判定が可能となる。
【００９７】
なお、位置制御部１１０の位置目標生成部１１１は、衝突判定部１３２がアーム３１０
の衝突を判定した際には、アーム先端位置目標値を新たに生成する。
【００９８】
具体的には、衝突を検出した場合は、アーム３１０を緊急停止するためのアーム先端位
置目標値を新たに生成する。また、衝突後のさらなる動作指令が必要な場合には、例えば
、反転動作が必要な場合については、衝突した方向とは逆方向に動作させるようなアーム
先端位置目標値を生成することができる。
【００９９】
なお、本実施形態では、時系列モデルとしてARモデルを用いたが、ARモデルに限定され
ることなく、各サンプリング周期における時系列信号がパラメータを用いて関係付けられ
る時系列モデルであればよい。また、パラメータの推定に関しても、逐次最小２乗法に限
定されるものではない。
【０１００】
また、本実施形態では、リンクが回転軸（関節）を中心に回転するアームを例に説明を
行ったが、並進移動するような対象に対しては、例えばリンク角度の代わりに対象の変位
を用いることで、同様の方法により対象に加わる外乱を検出することができる。
【０１０１】
（第一の変形例）
以下、図９を参照して、第一の実施形態のロボット制御装置の第一の変形例について説
明する。なお、図１と同様の構成については同様の符号を付すことで、その詳細な説明を
省略する。
【０１０２】
図９は第一の変形例に係るロボット制御装置に用いるＣＰＵ１００のシステム構成図で
ある。本変形例では、トルク算出部１２０は、（数９）よりも詳細なアーム３１０の動力
学モデルに基づいて、各駆動軸３１２のトルク推定値を算出する。
【０１０３】
（数９）は、剛体関節モデルを用いているが、減速機３１４を低剛性のバネ要素と見な
す弾性関節モデルを代わりに用いれば、動力学モデルに加減速時の機械共振を反映させる
ことができ、加減速によるトルク変動と衝突によるトルク変動の判別の分解能を向上させ
ることができる。
【０１０４】
剛体関節モデルに基づく（数９）に相当する、弾性関節モデルに基づく動力学モデルを
与える運動方程式は次式となる。
【数２２】
【０１０５】
剛体関節モデルでは、モータ角度θMを減速比倍することでリンク角度θLを算出するこ
とができたが、弾性関節モデルでは、（数２２）の動力学モデルを考慮してθMからθLを
推定しなければならない。したがってθMからθLを推定しながら、トルク推定値を算出す
る必要がある。
【０１０６】
これには、（数２２）を、
【数２３】
【０１０７】
として角加速度を求めるように変形する。（数２３）にθMと矛盾しないθLの初期値を与
えておき、実際に検出できるθMを代入して積分することを逐次繰り返すことによりθLの
推定値を得ることができる。
【０１０８】
図９のトルク算出部１２０は、リンク角度の推定値を逐次算出するリンク角度算出部１
２７、慣性力トルクを算出する慣性力トルク算出部１２１、摩擦力トルクを算出する摩擦
力トルク算出部１２２、重力トルクを算出する重力トルク算出部１２３、遠心力・コリオ
リ力トルクを算出する遠心力・コリオリ力トルク算出部１２４、弾性力トルク算出部１２
６、加算部１２５を有する。
【０１０９】
以下、ＣＰＵ１００の動作を説明する図１０を参照して、トルク算出部１２０の動作に
ついて詳細に説明する。
【０１１０】
リンク角度算出部１２７は、メモリ２００に予め格納されているリンク角度の初期値と
角度検出部３１５が検出するモータ角度θMを用いて、次式の微分方程式を解くことでリ
ンク角度の推定値を算出する。
【数２４】
【０１１１】
慣性力トルク算出部１２１、摩擦力トルク算出部１２２、重力トルク算出部１２３、遠
心力・コリオリ力トルク算出部１２４は、メモリ２００に予め格納されている物理パラメ
ータ及びリンク角度算出部１２７が算出するリンク角度の推定値を用いて、それぞれ慣性
力トルク、摩擦力トルク、重力トルク、遠心力・コリオリ力トルクを算出する。
【０１１２】
また、弾性力トルク算出部１２６は、次式のように弾性力トルクを算出する。
【数２５】
【０１１３】
加算部１２５は、慣性力トルク、摩擦力トルク、重力トルク、遠心力・コリオリ力トル
ク、弾性力トルクを用いて次式のように各駆動軸３２０のトルク推定値を算出する。
【数２６】
【０１１４】
なお、減速機３１４のバネ要素によるねじれは微小であるので、順運動学やヤコビ行列
の算出に必要なリンク角度については、第一の実施形態と同様に角度算出部１１２が算出
する値を用いることができる。
【０１１５】
（第二の変形例）
以下、図１１を参照して、第一の実施形態のロボット制御装置の第二の変形例について
説明する。なお、図１と同様の構成については同様の符号を付すことで、その詳細な説明
を省略する。
【０１１６】
図１１は第一の変形例に係るロボット制御装置に用いるＣＰＵ１００のシステム構成図
である。本変形例では、図１に示す構成に加え、アーム３１０の各リンク３１１のどこで
衝突が生じたかを推定する衝突部位推定部１４０をさらに有する。
【０１１７】
衝突部位推定部１４０は、衝突検出部１３０がアーム３１０の各駆動軸３１２において
それぞれ衝突検出を行い、衝突を検出した駆動軸３１２を備えるリンク３１１において衝
突が生じたものと推定する。
【０１１８】
以上説明した少なくとも１つの本実施形態のロボット制御装置によれば、ロボットアー
ムに加わる外乱を高精度に検出することが可能となる。
【０１１９】
これら実施形態は、例として提示したものであり、発明の範囲を限定することは意図し
ていない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明
の要旨を逸脱しない範囲で、様々の省略、置き換え、変更を行うことができる。これら実
施形態やその変形は、発明の範囲や要旨に含まれると同時に、特許請求の範囲に記載され
た発明とその均等の範囲に含まれるものである。
【符号の説明】
【０１２０】
１００・・・ＣＰＵ
１１０・・・位置制御部
１１１・・・位置目標生成部
１１２・・・角度算出部
１１３・・・位置算出部
１１４・・・指令値算出部
１２０・・・トルク算出部
１２１・・・慣性力トルク算出部
１２２・・・摩擦力トルク算出部
１２３・・・重力トルク算出部
１２４・・・遠心力・コリオリ力トルク算出部
１２５・・・加算部
１２６・・・弾性力トルク算出部
１２７・・・リンク角度算出部
１３０・・・衝突検出部
１３１・・・外部トルク算出部
１３２・・・モデル更新部
１３３・・・衝突判定部
１４０・・・衝突部位検出部
２００・・・メモリ
３００・・・本体
３１０・・・アーム
３１１・・・リンク
３１２・・・駆動軸
３１３・・・モータ
３１４・・・減速機
３１５・・・角度検出部
【技術分野】
【０００１】
本発明の実施形態は、ロボット制御装置、外乱判定方法およびアクチュエータ制御方法
に関する。
【背景技術】
【０００２】
ハンドリング等を行うロボットアームを動作させる際、周囲の環境に存在する障害物と
の衝突が起こり得る。衝突後においてもロボットアームを安定して動作させるためには、
ロボットアームの衝突を検出する必要がある。
【０００３】
そこで、力覚センサ等を用いずに、ロボットアームの衝突等による外乱の影響で発生す
る外部トルクを推定することで、この外部トルクがあらかじめ設定した閾値以上の場合に
ロボットの先端部が衝突したという判定を行う方法が提案されている。
【０００４】
しかしながら、ロボットアームの動力学モデルのモデル化誤差の影響により、外部トル
クには誤差が含まれることになるために、ロボットアームに加わる外乱を高精度に検出す
ることができない。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１０−１０５１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ロボットアームに加わる外乱を高精度に検出することが可能なロボット制御装置、外乱
判定方法およびロボット制御方法を提供する。
【課題を解決するための手段】
【０００７】
実施形態のロボット制御装置は、アームの駆動軸を回転駆動するアクチュエータと、前
記アーム先端の目標位置データを生成する生成部と、前記駆動軸の回転角度をサンプリン
グ周期ごとに検出する第１検出部と、前記回転角度を用いて前記アーム先端の現在位置デ
ータを算出する第１算出部と、前記目標値位置データと前記現在位置データとを用いて、
前記アクチュエータに対する指令値を算出する第２算出部と、前記駆動軸の回転角度を用
いて前記アクチュエータを駆動する駆動トルクの推定値を算出する第３算出部と、前記駆
動トルクの推定値と、前記指令値から求まる前記駆動トルクの真値との差分を算出する第
４算出部と、前記アームに加わる外乱を検出する第２検出部とを備え、
前記第２検出部は、前記差分を変数とする時系列モデルのパラメータを推定し、前記パ
ラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新する更新
部と、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去
の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定
する判定部とを備える。
【０００８】
実施形態の外乱判定方法は、算出部が、対象物に加わったと推定される外力を算出する
ステップと、更新部が、前記外力を変数とする時系列モデルのパラメータを推定し、前記
パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新するス
テップと、判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング
周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記対象物へ加わ
る外乱の有無を判定するステップとを有する。
【０００９】
実施形態のアクチュエータ制御方法は、生成部が、前記アーム先端の目標位置データを
生成するステップと、第１検出部が、前記駆動軸の回転角度をサンプリング周期ごとに検
出するステップと、第１算出部が、前記回転角度を用いて前記アーム先端の現在位置デー
タを算出するステップと、第２算出部が、前記目標値位置データと前記現在位置データと
を用いて、前記アクチュエータに対する指令値を算出するステップと、第３算出部が、前
記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算出す
るステップと、第４算出部が、前記駆動トルクの推定値と、前記指令値から求まる前記駆
動トルクの真値との差分を算出するステップと、更新部が、前記差分を変数とする時系列
モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周
期の当該時系列モデルを更新するステップと、判定部が、前記第１サンプリング周期の時
系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデ
ルとを比較して、前記アームへの外乱の有無を判定するステップとを有する。
【図面の簡単な説明】
【００１０】
【図１】第一の実施形態に係るロボット制御装置の構成図。
【図２】図１のロボット制御装置のCPUの動作を説明するブロック図。
【図３】実施例１を説明するための説明図。
【図４】実施例２を説明するための説明図。
【図５】１段予測による予測誤差の時間応答。
【図６】２段予測による予測誤差の時間応答。
【図７】３段予測による予測誤差の時間応答。
【図８】実施例３を説明するための説明図。
【図９】第一の実施形態の第一の変形例に係るロボット制御装置の構成図。
【図１０】図９のロボット制御装置のCPUの動作を説明するブロック図。
【図１１】第一の実施形態の第二の変形例に係るロボット制御装置の構成図。
【図１２】制御対象の一例であるロボットの構成図。
【図１３】図１２のロボットのアームの構成図。
【図１４】アームの衝突の様子を示す図。
【発明を実施するための形態】
【００１１】
以下、発明を実施するための実施形態について説明する。
【００１２】
（第一の実施形態）
本実施形態において制御対象の一例とするロボットは、図１２に示すように、本体３０
０と、本体３００に設けられるアーム３１０を備える。
【００１３】
図１２に示すアーム３１０は、３つのリンク３１１（リンク３１１a，リンク３１１b，
リンク３１１c）、各リンク３１１をそれぞれ回転駆動する際の回転軸（関節）となる３
つの駆動軸３１２（駆動軸３１２a，駆動軸３１２b，駆動軸３１２c）を備えている。
【００１４】
リンク３１１aは、駆動軸３１２aを介して本体３００に回転可能に接続されている。ま
た、リンク３１１bは、駆動軸３１２bを介してリンク３１１aに、リンク３１１cは、駆動
軸３１２cを介してリンク３１１bに、それぞれ回転可能に接続されている。
【００１５】
図１３に模式的に示すように、各リンク３１１は、駆動軸３１２と、各駆動軸３１２を
回転駆動するアクチュエータであるモータ３１３と、モータ３１３の回転数を落とし、各
駆動軸３１２の回転トルクを向上させる減速機３１４と、モータ３１３の回転軸の回転角
度の変化量（以下、モータ角度θM）を検出する角度検出部３１５とを備える。なお、角
度検出部３１５としては、例えばエンコーダ等のセンサを用いることができる。
【００１６】
なお、図１２において、本体３００の内部には、このロボットを制御するためのロボッ
ト制御装置が設けられる。ロボット制御装置は、通常時にはロボットのアーム３１０を所
定の軌道に沿ってアーム３１０先端（リンクcの先端位置）の位置制御を行う。
【００１７】
また、ロボット制御装置は、図１４に示すように例えばアーム３１０が障害物へ衝突す
るなどして、アーム３１０に対して外乱が加わった際には、この外乱から衝突を高精度に
検出し、この検出結果を用いてアーム３１０先端の位置制御を行う。
【００１８】
図１は、第一の実施形態に係るロボット制御装置の構成図である。
【００１９】
第一の実施形態に係るロボット制御装置は、図１に示すように、中央演算処理装置（Ｃ
ＰＵ）１００、記憶装置（メモリ）２００を備える。
【００２０】
図１のＣＰＵ１００は３つのモジュールを有する。具体的には、アーム３１０先端の位
置を制御する位置制御部１１０と、各リンク３１１における駆動軸３１２のトルクの推定
値を算出するトルク算出部１２０と、アーム３１０のいずれかのリンク３１１における障
害物への衝突をアーム３１０全体として検出する衝突検出部１３０とを備える。
【００２１】
位置制御部１１０は、後述の位置目標値生成部１１１が生成するアーム先端の３次元位
置（以下、アーム先端位置）の目標値（以下、アーム先端位置目標値）xRと、角度検出部
３１５によるモータ角度θMの検出値に基づいて得られるアーム先端位置xとの差分をゼロ
にする方向にモータ角度θMを角度制御する。また、モータ角度θMの角度制御により減速
機３１４を介してリンク角度θLを角度制御する。これにより、位置制御部１１０は、リ
ンク角度θLの角度制御を通じて、アーム先端位置目標値xRにアーム先端位置xを追従させ
る位置制御を行う。
【００２２】
トルク算出部１２０は、位置制御部１１０からリンク角度θLを得て、アーム３１０の
動力学モデルに基づいて、各リンク３１１における駆動軸３１２のトルクの推定値（以下
、トルク推定値）を算出する。なお、トルク推定値は、各モータ３１３を駆動するのに必
要と推定される駆動トルクの推定値である。
【００２３】
衝突検出部１３０は、位置制御部１１０からリンク角度θLを、トルク算出部１２０か
らトルク推定値を得て、アーム３１０に加わる外乱により各リンク３１１に生じる外部ト
ルクを算出する。そして、この外部トルクを用いてアーム３１０全体としての衝突を検出
する。
【００２４】
以下、図１および図２を参照して、それぞれのモジュールについて詳細に説明する。な
お、以下のモジュールの各処理は、サンプリング周期（制御周期）ごとに行うものとし、
サンプリングをk（ただし、k=0,1,2,…）で表す。
【００２５】
（位置制御部）
図１に示すように、位置制御部１１０は、アーム先端位置目標値xRを生成する位置目標
生成部１１１、リンク角度θLを算出する角度算出部１１２、アーム先端位置xを算出する
位置算出部１１３、各モータ３１３に対する指令値（電流値）uを算出する指令値算出部
１１４を有する。
【００２６】
ここで、各変数（モータ角度θM，リンク角度θL，アーム先端位置x）には、それぞれ
次のような関係がある。
【数１】
【００２７】
なお、nはアーム３１１の駆動軸３１２の数（本実施形態では３）を表している。また
、Λは順運動学に基づく非線形関数で、リンク角度θLからアーム先端位置xを算出する座
標変換を行う関数である。
【００２８】
位置目標生成部１１１は、例えばメモリ２００が予め記憶する、ハンドリング動作開始
から終了までのアーム先端位置目標値の履歴を得て、現在時刻における値をアーム先端位
置目標値xRとして生成する。
【００２９】
指令値算出部１１４は、位置目標生成部１１１からアーム先端位置目標値xRを、後述の
位置算出部１１３からアーム先端位置xを得て、アーム先端位置目標値xRと、角アーム先
端位置xとの差分をゼロにする方向にモータ角度θMを角度制御するための指令値uを算出
する。
【００３０】
以下、図２を参照して、指令値算出部１１４の具体的な処理について説明する。
【００３１】
指令値算出部１１４は、アーム先端位置目標値xRと、アーム先端位置xを用いて、アー
ム先端位置の微小変化Δxを次式により算出する。
【数２】
【００３２】
ここで、アーム先端位置の微小変化Δxとリンク角度の微小変化ΔθLの関係を与えるΛ
(θL)を偏微分したn×nのヤコビ行列J(θL)を次式のように定義する。
【数３】
【００３３】
このとき、上記ヤコビ行列の逆行列である逆ヤコビ行列を用いることで、アーム先端位
置の微小変化Δxとリンク角度の微小変化ΔθLには次式の関係が成り立つ。
【数４】
【００３４】
すなわち、上式により目標リンク角度の微小変化値を算出することができるので、これ
を積分することによって、
【数５】
【００３５】
としてリンク角度目標値θLRが得られる。
【００３６】
そこで、指令値算出部１１４は、後述の角度算出部１１２が算出するリンク角θLを用
いて逆ヤコビ行列を算出する。そして、アーム先端位置の微小変化Δxと、逆ヤコビ行列
を用いてリンク角度の微小変化ΔθLを次式のように算出する。
【数６】
【００３７】
また、上記リンク角度の微小変化ΔθLを積分することで、（数５）によりリンク角度
目標値θLRを算出する。
【００３８】
そして、指令値算出部１１４は、リンク角度目標値θLRを減速比NGで除算することで、
モータ角度目標値θMRを算出する。そして、モータ角度目標値θMRを入力として、公知の
速度・位置制御則を適用することで各モータ３１３を駆動するための指令値（電流値）u
を算出する。
【００３９】
指令値算出部１１４は、上記の指令値uをモータ３１３対して与えることで、モータ３
１３を駆動しモータ角度θMを角度制御する。この際、角度検出部３１５は、この駆動に
よるモータ角度θMを検出する。
【００４０】
角度算出部１１２は、角度検出部３１５が検出するモータ角度θMを用いて、次式のよ
うにリンク角θLを算出する。
【数７】
【００４１】
位置算出部１１３は、メモリ２００が記憶する非線形関数式Λを得て、角度算出部１１
２が算出するリンク角θLを用いることで、次式のように現在時刻におけるアーム先端位
置xを算出する。
【数８】
【００４２】
上記のように位置算出部１１３が算出するアーム先端位置xは、フィードバック信号と
して指令値算出部１１４に対して入力される。
【００４３】
なお、（数８）に示すように、アーム先端位置xの算出にはリンク角度θLが必要となる
ので、角度検出部３１５が直接リンク角度θLを検出することもできる。また、減速機３
１４を用いない構成も考えられる。この場合には、モータ角度θMとリンク角度θLは一致
するため、角度算出部１１２は必ずしも必要とはならないことは明らかである。
【００４４】
（トルク算出部）
図１に示すように、トルク算出部１２０は、慣性力トルクを算出する慣性力トルク算出
部１２１、摩擦力トルクを算出する摩擦力トルク算出部１２２、重力トルクを算出する重
力トルク算出部１２３、遠心力・コリオリ力トルクを算出する遠心力・コリオリ力トルク
算出部１２４、加算部１２５を有する。
【００４５】
ここで、一般にロボットアームの動力学モデルは次式の運動方程式で与えられる。
【数９】
【００４６】
したがって、リンク角度θLの検出値を１回微分したリンク角速度、２回微分したリン
ク角加速度を求めれば、各リンクの重心位置や慣性モーメント、摩擦係数等の物理パラメ
ータを用いて、重力、慣性力、摩擦力、遠心力・コリオリ力等によるトルクの推定値を算
出することができる。
【００４７】
なお、リンク角速度は、時系列データとして得られるリンク角度θLの、それぞれ異な
るサンプリング周期に得られる２つの値の差分等により算出できる（１回微分）。また、
リンク角加速度は、それぞれ異なるサンプリング周期に得られる２つのリンク角速度の値
の差分等により算出できる（２回微分）。
【００４８】
慣性力トルク算出部１２１、摩擦力トルク算出部１２２、重力トルク算出部１２３、遠
心力・コリオリ力トルク算出部１２４は、メモリ２００に予め格納されている物理パラメ
ータ及び角度算出部１１２が算出するリンク角度を用いて、慣性力トルク、摩擦力トルク
、重力トルク、遠心力・コリオリ力トルクをそれぞれ算出する。
【００４９】
加算部１２５は、次式のように各駆動軸３２０のトルク推定値（以下では、変数にハッ
トを付して推定値を表す）を算出する。
【数１０】
【００５０】
なお、本実施形態においては、慣性力トルク、摩擦力トルク、重力トルク、遠心力・コ
リオリ力トルクについて考慮するものとしたが、遠心力・コリオリ力によるトルクは他の
トルクに比べて充分に小さい（１/１０程度）ため、遠心力・コリオリ力によるトルクに
関しては考慮しないことも可能である。この場合には、加算部１２５は、各駆動軸３２０
のトルク推定値を、慣性力トルク、摩擦力トルク、重力トルクの和により算出する。
【００５１】
（衝突検出部）
図１に示すように、衝突検出部１３０は、アーム３１０に外部から加わる外部トルクを
算出する外部トルク算出部１３１、時系列モデルのパラメータを推定しモデルを更新する
モデル更新部１３２、更新されるモデルを用いて衝突判定を行う衝突判定部１３３を有す
る。
【００５２】
ここで、外部トルクτdは、トルク推定値と駆動トルクの真値との差分により次式のよ
うに与えられる。
【数１１】
【００５３】
なお、上式における駆動トルクの真値は、モータ３１３への指令値uに、モータ３１３
のカタログ値等により得られ、メモリ２００が予め記憶するトルク定数Ktを乗算すること
で得られる。
【００５４】
外部トルクは、モデル化誤差がなく、外乱等も加わっていない理想状態においては、ゼ
ロになっているはずである。したがって、外部トルクがゼロでない場合には、アーム３１
０には外乱が加わっていることが推定できるために、この外部トルクを衝突検出に用いる
ことができる。
【００５５】
しかしながら、（数９）の動力学モデルには、慣性モーメントや摩擦係数などの物理パ
ラメータ等によるモデル化誤差が含まれている。また、（数９）の動力学モデルでは考慮
されていない機械共振により生じる弾性力トルク等の影響も存在し、低速で動作している
ときには衝突検出は機能するが、高速で動作しているときには、加減速に必要なトルク変
動と衝突によるトルク変動の区別がつかないことがある。
【００５６】
そこで、本実施形態においては、このようなモデル化誤差を吸収するために、（数１１
）の外部トルクを各駆動軸３１２において１次元の時系列信号y(k)（ただし、k=0,1,2,…
）と捉えて、次式のように表されるAR（Auto Regressive：自己回帰）モデルを当てはめ
る。
【数１２】
【００５７】
上式のARモデルは、時系列信号y(k)は，過去naステップ分の時系列信号の線形結合で表
される。ここで，a1〜anaはARパラメータであり、e(k)は残差を表す。
【００５８】
また、（数１２）のARモデルは、
【数１３】
【００５９】
と定義できる。ここで、ARパラメータαとは、ARモデルを特徴付けるための係数であり、
残差e(k)を最小にするARパラメータを適切に推定することで、ARモデルを算出することが
できる。
【００６０】
ARパラメータαの推定値は、次式のような忘却係数λ（通常0.97〜0.995の範囲から選
択する）付きの逐次最小２乗法を用いれば、リアルタイムに更新しながら算出することが
できる。
【数１４】
【００６１】
ここで、ε(k)は予測誤差、kαはゲイン、P(k)は共分散行列をそれぞれ表している。
【００６２】
外部トルク算出部１３１は、トルク算出部１２０が算出するトルク推定値と、指令値算
出部１１４が算出する指令値uにトルク定数Ktを乗算することで得られる駆動トルクを用
いて、次式のように外部トルクを算出する。この外部トルクは、アーム３１０に対して外
から加わったと推定される外力と見なすことができる。
【数１５】
【００６３】
モデル更新部１３２は、外部トルク算出部１３１が算出する外部トルクを変数として、
（数１２）で表されるARモデルに当てはめる。そして、（数１４）のように、逐次最小二
乗法を用いて全てのARパラメータの推定値を逐次推定することで、ARモデルを更新する。
【００６４】
衝突判定部１３３は、詳細は後述するようにモデル更新部１３２により逐次更新される
ARモデルに基づいて、アーム３１０の衝突の有無を判定する。
【００６５】
以下、図３乃至図８を参照して衝突判定部１３３の動作について説明する。
【００６６】
（実施例１）
衝突判定部１３３が衝突を判定するための第１の方法として、逐次推定しているARパラ
メータの時間変化を用いる方法を説明する。
【００６７】
図３（a）は、アーム３１０を衝突させた時の外部トルクとモータ角速度の時間応答の
一例であり、図３（b）は、逐次更新されるARモデルのパラメータの時間応答の一例であ
る。
【００６８】
ここでは、５次のARモデルを用いており、ARパラメータは５本表示されている。図中の
凡例のa(1)〜a(5)は、それぞれa1〜a5を示している。図３（b）では、1.16[s]前後で衝突
が起こっているが、ARパラメータのうち、最低次であるa1の変化が顕著であり、次にa3の
変化が大きい。
【００６９】
それに対して、図３（a）に示した外部トルクがゆっくりした変化であるので、衝突検
出を目的とした場合、ARパラメータの変化を検出する方法の優位性は明らかである。ここ
で、衝突が起こってもモータ角速度がすぐにゼロにならないのは、関節軸がたわんでいる
ことを示している。
【００７０】
衝突判定部１３３は、図３において最も変化の大きい最低次のARパラメータa1の変化量
を、現在のサンプリング周期kにおけるパラメータの推定値と、一つ前のサンプリング周
期k-1におけるパラメータの推定値の差分により算出する。
【００７１】
そして、いずれかの駆動軸３１２において、最低次のARパラメータの変化量の絶対値が
、メモリ２００に格納されている既定の閾値（例えば0.1）を超える場合に、アーム３１
０が衝突したものと判定する。
【００７２】
なお、逐次最小２乗法において、忘却係数λを調整することによって、ARパラメータの
変化の度合いを調整することができる。忘却係数をロボットの動作加速度が小さいときに
は0.99など値を大きくしておき、加速度が大きいときには0.98など値を小さくするなど、
忘却係数を可変にすることも衝突判定の精度を上げることに有効である。
【００７３】
また、本実施形態のように最低次のARパラメータを用いるのではなく、他のARパラメー
タを用いることで衝突を判定することも可能である。
【００７４】
（実施例２）
衝突判定部１３２が衝突を判定するための第２の方法として、多段予測を用いた予測誤
差の時間変化を用いる方法を説明する。
【００７５】
（数１２）のARモデルにおいて、ARパラメータを逐次推定しながら、イタレーションk
を進めればディジタル処理に起因するサンプル時間遅れを取り戻せる上、多段の予測をす
ることができる。まず、１段先予測値は、
【数１６】
【００７６】
で与えられ、２段先予測値、３段先予測値は、それぞれ、
【数１７】
【００７７】
で与えられる。
【００７８】
よって、１段先予測誤差は、
【数１８】
【００７９】
で与えられ、２段先予測誤差、３段先予測誤差は、それぞれ、
【数１９】
【００８０】
で与えられる。
【００８１】
図４は、外部トルクと１，２，３段先予測誤差の各時間応答を比較したものである。同
図から、１段先予測値は、外部トルクからやや遅れていることがわかる。一方、２段先予
測値はやや進んでおり、３段先予測値は明らかに進んでいる。
【００８２】
また、図５は、１段先予測誤差の時間変化から衝突を検出している様子を示している。
同じく図６は２段先予測誤差、図７は３段先予測誤差の場合である。
【００８３】
図５，６，７を比較すると、それぞれ衝突検出が可能であるが、図６の予測誤差の時間
応答の波形のレンジが比較的小さく、閾値の設定が容易である。したがって、以上からア
ーム３１０の衝突検出には２段先予測誤差を用いることが好ましい。
【００８４】
衝突判定部１３３は、（数１９）のように誤差e2(k)を算出する。そして、誤差e2(k)の
変化量を、現在のサンプリング周期kにおける誤差と、一つ前のサンプリング周期k-1にお
ける誤差の差分により算出する。
【００８５】
そして、上記の誤差の変化量の絶対値が既定の閾値を超える場合に、アーム３１０が衝
突したものと判定する。
【００８６】
（実施例３）
衝突判定部１３３が衝突を検出するための第３の方法として、ARモデルから計算できる
周波数応答の時間変化を用いる方法を説明する。
【００８７】
（数１２）のARモデルをＺ変換すると、
【数２０】
【００８８】
ここで、ARモデルの伝達関数H(z-1)=1/A(z-1)の周波数応答は、（数２０）に次式を代
入することで求めることができる。
【数２１】
【００８９】
なお、（数２０）および（数２１）において、zはZ変換を表す演算子、ωは周波数、j
は虚数単位をそれぞれ表している。
【００９０】
図８は、ARモデルから計算できる周波数応答が衝突前後で変化することを用いて衝突を
検出する様子を示したものである。前述のように衝突によって摩擦係数や慣性モーメント
が変化したと考えることで、周波数応答の変化からも衝突検出ができることは明らかであ
る。
【００９１】
図８では、一点鎖線Ｂよりも高周波側の領域において、（数９）の動力学モデルでは考
慮していない機械共振特性が検出されており、衝突検出しづらくなっている。
【００９２】
そこで、あらかじめ衝突時に変化の大きい周波数領域を調べておき、例えば、図８の一
点鎖線ＡとＢの間の周波数領域でのゲインの差をサンプリング周期ごとに積分するなど、
ウィンドウ処理をすることで衝突検出のための閾値の設定を容易にすることができる。
【００９３】
衝突判定部１３２は、各サンプリング周期において周波数応答を算出する。そして、既
定のウィンドウの範囲内で、現在のサンプリング周期kにおけるゲインと、一つ前のサン
プリング周期k-1におけるゲインの差分を周波数ごとに算出し、ゲインの差分を周波数で
積分することで、図８中の面積（C）に相当する積分値を算出する。
【００９４】
そして、上記の積分値の絶対値が既定の閾値を超える場合に、アーム３１０が衝突した
ものと判定する。
【００９５】
なお、本実施例では、ARパラメータ５個を全て用いて周波数応答を計算しているので、
実施例１のようにARパラメータ個々の時間変化で衝突を検出する方法よりも計算量は増加
するが、よりロバストな衝突検出を行うことができる。
【００９６】
以上、実施例１乃至実施例３として、３通りの衝突判定の方法について説明した。この
３通りの衝突判定方法は各々適用することも可能であるが、３つ同時に適用し、多数決を
とることで、各リンク３１１におけるより正確な衝突判定が可能となる。
【００９７】
なお、位置制御部１１０の位置目標生成部１１１は、衝突判定部１３２がアーム３１０
の衝突を判定した際には、アーム先端位置目標値を新たに生成する。
【００９８】
具体的には、衝突を検出した場合は、アーム３１０を緊急停止するためのアーム先端位
置目標値を新たに生成する。また、衝突後のさらなる動作指令が必要な場合には、例えば
、反転動作が必要な場合については、衝突した方向とは逆方向に動作させるようなアーム
先端位置目標値を生成することができる。
【００９９】
なお、本実施形態では、時系列モデルとしてARモデルを用いたが、ARモデルに限定され
ることなく、各サンプリング周期における時系列信号がパラメータを用いて関係付けられ
る時系列モデルであればよい。また、パラメータの推定に関しても、逐次最小２乗法に限
定されるものではない。
【０１００】
また、本実施形態では、リンクが回転軸（関節）を中心に回転するアームを例に説明を
行ったが、並進移動するような対象に対しては、例えばリンク角度の代わりに対象の変位
を用いることで、同様の方法により対象に加わる外乱を検出することができる。
【０１０１】
（第一の変形例）
以下、図９を参照して、第一の実施形態のロボット制御装置の第一の変形例について説
明する。なお、図１と同様の構成については同様の符号を付すことで、その詳細な説明を
省略する。
【０１０２】
図９は第一の変形例に係るロボット制御装置に用いるＣＰＵ１００のシステム構成図で
ある。本変形例では、トルク算出部１２０は、（数９）よりも詳細なアーム３１０の動力
学モデルに基づいて、各駆動軸３１２のトルク推定値を算出する。
【０１０３】
（数９）は、剛体関節モデルを用いているが、減速機３１４を低剛性のバネ要素と見な
す弾性関節モデルを代わりに用いれば、動力学モデルに加減速時の機械共振を反映させる
ことができ、加減速によるトルク変動と衝突によるトルク変動の判別の分解能を向上させ
ることができる。
【０１０４】
剛体関節モデルに基づく（数９）に相当する、弾性関節モデルに基づく動力学モデルを
与える運動方程式は次式となる。
【数２２】
【０１０５】
剛体関節モデルでは、モータ角度θMを減速比倍することでリンク角度θLを算出するこ
とができたが、弾性関節モデルでは、（数２２）の動力学モデルを考慮してθMからθLを
推定しなければならない。したがってθMからθLを推定しながら、トルク推定値を算出す
る必要がある。
【０１０６】
これには、（数２２）を、
【数２３】
【０１０７】
として角加速度を求めるように変形する。（数２３）にθMと矛盾しないθLの初期値を与
えておき、実際に検出できるθMを代入して積分することを逐次繰り返すことによりθLの
推定値を得ることができる。
【０１０８】
図９のトルク算出部１２０は、リンク角度の推定値を逐次算出するリンク角度算出部１
２７、慣性力トルクを算出する慣性力トルク算出部１２１、摩擦力トルクを算出する摩擦
力トルク算出部１２２、重力トルクを算出する重力トルク算出部１２３、遠心力・コリオ
リ力トルクを算出する遠心力・コリオリ力トルク算出部１２４、弾性力トルク算出部１２
６、加算部１２５を有する。
【０１０９】
以下、ＣＰＵ１００の動作を説明する図１０を参照して、トルク算出部１２０の動作に
ついて詳細に説明する。
【０１１０】
リンク角度算出部１２７は、メモリ２００に予め格納されているリンク角度の初期値と
角度検出部３１５が検出するモータ角度θMを用いて、次式の微分方程式を解くことでリ
ンク角度の推定値を算出する。
【数２４】
【０１１１】
慣性力トルク算出部１２１、摩擦力トルク算出部１２２、重力トルク算出部１２３、遠
心力・コリオリ力トルク算出部１２４は、メモリ２００に予め格納されている物理パラメ
ータ及びリンク角度算出部１２７が算出するリンク角度の推定値を用いて、それぞれ慣性
力トルク、摩擦力トルク、重力トルク、遠心力・コリオリ力トルクを算出する。
【０１１２】
また、弾性力トルク算出部１２６は、次式のように弾性力トルクを算出する。
【数２５】
【０１１３】
加算部１２５は、慣性力トルク、摩擦力トルク、重力トルク、遠心力・コリオリ力トル
ク、弾性力トルクを用いて次式のように各駆動軸３２０のトルク推定値を算出する。
【数２６】
【０１１４】
なお、減速機３１４のバネ要素によるねじれは微小であるので、順運動学やヤコビ行列
の算出に必要なリンク角度については、第一の実施形態と同様に角度算出部１１２が算出
する値を用いることができる。
【０１１５】
（第二の変形例）
以下、図１１を参照して、第一の実施形態のロボット制御装置の第二の変形例について
説明する。なお、図１と同様の構成については同様の符号を付すことで、その詳細な説明
を省略する。
【０１１６】
図１１は第一の変形例に係るロボット制御装置に用いるＣＰＵ１００のシステム構成図
である。本変形例では、図１に示す構成に加え、アーム３１０の各リンク３１１のどこで
衝突が生じたかを推定する衝突部位推定部１４０をさらに有する。
【０１１７】
衝突部位推定部１４０は、衝突検出部１３０がアーム３１０の各駆動軸３１２において
それぞれ衝突検出を行い、衝突を検出した駆動軸３１２を備えるリンク３１１において衝
突が生じたものと推定する。
【０１１８】
以上説明した少なくとも１つの本実施形態のロボット制御装置によれば、ロボットアー
ムに加わる外乱を高精度に検出することが可能となる。
【０１１９】
これら実施形態は、例として提示したものであり、発明の範囲を限定することは意図し
ていない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明
の要旨を逸脱しない範囲で、様々の省略、置き換え、変更を行うことができる。これら実
施形態やその変形は、発明の範囲や要旨に含まれると同時に、特許請求の範囲に記載され
た発明とその均等の範囲に含まれるものである。
【符号の説明】
【０１２０】
１００・・・ＣＰＵ
１１０・・・位置制御部
１１１・・・位置目標生成部
１１２・・・角度算出部
１１３・・・位置算出部
１１４・・・指令値算出部
１２０・・・トルク算出部
１２１・・・慣性力トルク算出部
１２２・・・摩擦力トルク算出部
１２３・・・重力トルク算出部
１２４・・・遠心力・コリオリ力トルク算出部
１２５・・・加算部
１２６・・・弾性力トルク算出部
１２７・・・リンク角度算出部
１３０・・・衝突検出部
１３１・・・外部トルク算出部
１３２・・・モデル更新部
１３３・・・衝突判定部
１４０・・・衝突部位検出部
２００・・・メモリ
３００・・・本体
３１０・・・アーム
３１１・・・リンク
３１２・・・駆動軸
３１３・・・モータ
３１４・・・減速機
３１５・・・角度検出部
【特許請求の範囲】
【請求項１】
アームの駆動軸を回転駆動するアクチュエータと、
前記アーム先端の目標位置データを生成する生成部と、
前記駆動軸の回転角度をサンプリング周期ごとに検出する第１検出部と、
前記回転角度を用いて前記アーム先端の現在位置データを算出する第１算出部と、
前記目標値位置データと前記現在位置データとを用いて、前記アクチュエータに対する
指令値を算出する第２算出部と、
前記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算
出する第３算出部と、
前記駆動トルクの推定値と、前記指令値から求まる前記駆動トルクの真値との差分を算
出する第４算出部と、
前記アームに加わる外乱を検出する第２検出部と、
を備え、
前記第２検出部は、
前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめ
ることで、第１サンプリング周期の当該時系列モデルを更新する更新部と、
前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第
２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定する
判定部と、
を備えるロボット制御装置。
【請求項２】
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルの前記パラメータの変化量と当該閾値を比較し、前記アームへの外乱
の有無を判定する請求項１に記載のロボット制御装置。
【請求項３】
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルを用いて多段の予測誤差を算出し、当該予測誤差の変化量と前記閾値
を比較し、前記アームへの外乱の有無を判定する請求項１に記載のロボット制御装置。
【請求項４】
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルを用いて周波数応答を算出し、前記周波数応答の変化量と前記閾値を
比較し、前記アームへの外乱の有無を判定する請求項１に記載のロボット制御装置。
【請求項５】
前記生成部は、
前記第２検出部が前記外乱を検出した場合に、前記アームを緊急停止または反転動作す
る方向に前記目標位置データを更新する請求項１乃至４いずれか１項に記載のロボット制
御装置。
【請求項６】
前記アームは複数のリンクと、複数の駆動軸とを有し、
前記第２検出部は、前記駆動軸ごとに前記外乱を検出し、
前記外乱が検出された駆動軸を有するリンクに外乱が加わったものと推定する推定部を
さらに備える請求項１乃至５いずれか１項に記載のロボット制御装置。
【請求項７】
前記駆動軸は、前記アクチュータの回転軸である第１軸と、当該第１軸の回転数を減少
させる減速機を介して前記第１軸と接続する第２軸とを有し、前記第１検出部は、前記第
１軸の回転角度を検出するものであって、
前記第１軸の回転角度を用いて前記第２軸の回転角度をサンプリング周期ごとに算出す
る第５算出部を備え、
前記第１算出部は、前記第２軸の回転角度を用いて前記アーム先端の現在位置データを
算出し、
前記第３算出部は、前記第２の回転角度を用いて前記アクチュエータを駆動する駆動ト
ルクの推定値を算出する、
請求項１乃至６いずれか１項に記載のロボット制御装置。
【請求項８】
演算処理装置における外乱判定方法であって、
算出部が、対象物に加わったと推定される外力を算出するステップと、
更新部が、前記外力を変数とする時系列モデルのパラメータを推定し、前記パラメータ
を当てはめることで、第１サンプリング周期の当該時系列モデルを更新するステップと、
判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期より
も過去の第２サンプリング周期の時系列モデルとを比較して、前記対象物へ加わる外乱の
有無を判定するステップと、
を有する外乱判定方法。
【請求項９】
アームの駆動軸を回転駆動するアクチュエータを制御する方法であって、
生成部が、前記アーム先端の目標位置データを生成するステップと、
第１検出部が、前記駆動軸の回転角度をサンプリング周期ごとに検出するステップと、
第１算出部が、前記回転角度を用いて前記アーム先端の現在位置データを算出するステ
ップと、
第２算出部が、前記目標値位置データと前記現在位置データとを用いて、前記アクチュ
エータに対する指令値を算出するステップと、
第３算出部が、前記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トル
クの推定値を算出するステップと、
第４算出部が、前記駆動トルクの推定値と、前記指令値から求まる前記駆動トルクの真
値との差分を算出するステップと、
更新部が、前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータ
を当てはめることで、第１サンプリング周期の当該時系列モデルを更新するステップと、
判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期より
も過去の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無
を判定するステップと、
を有するアクチュエータ制御方法。
【請求項１】
アームの駆動軸を回転駆動するアクチュエータと、
前記アーム先端の目標位置データを生成する生成部と、
前記駆動軸の回転角度をサンプリング周期ごとに検出する第１検出部と、
前記回転角度を用いて前記アーム先端の現在位置データを算出する第１算出部と、
前記目標値位置データと前記現在位置データとを用いて、前記アクチュエータに対する
指令値を算出する第２算出部と、
前記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算
出する第３算出部と、
前記駆動トルクの推定値と、前記指令値から求まる前記駆動トルクの真値との差分を算
出する第４算出部と、
前記アームに加わる外乱を検出する第２検出部と、
を備え、
前記第２検出部は、
前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめ
ることで、第１サンプリング周期の当該時系列モデルを更新する更新部と、
前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第
２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定する
判定部と、
を備えるロボット制御装置。
【請求項２】
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルの前記パラメータの変化量と当該閾値を比較し、前記アームへの外乱
の有無を判定する請求項１に記載のロボット制御装置。
【請求項３】
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルを用いて多段の予測誤差を算出し、当該予測誤差の変化量と前記閾値
を比較し、前記アームへの外乱の有無を判定する請求項１に記載のロボット制御装置。
【請求項４】
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルを用いて周波数応答を算出し、前記周波数応答の変化量と前記閾値を
比較し、前記アームへの外乱の有無を判定する請求項１に記載のロボット制御装置。
【請求項５】
前記生成部は、
前記第２検出部が前記外乱を検出した場合に、前記アームを緊急停止または反転動作す
る方向に前記目標位置データを更新する請求項１乃至４いずれか１項に記載のロボット制
御装置。
【請求項６】
前記アームは複数のリンクと、複数の駆動軸とを有し、
前記第２検出部は、前記駆動軸ごとに前記外乱を検出し、
前記外乱が検出された駆動軸を有するリンクに外乱が加わったものと推定する推定部を
さらに備える請求項１乃至５いずれか１項に記載のロボット制御装置。
【請求項７】
前記駆動軸は、前記アクチュータの回転軸である第１軸と、当該第１軸の回転数を減少
させる減速機を介して前記第１軸と接続する第２軸とを有し、前記第１検出部は、前記第
１軸の回転角度を検出するものであって、
前記第１軸の回転角度を用いて前記第２軸の回転角度をサンプリング周期ごとに算出す
る第５算出部を備え、
前記第１算出部は、前記第２軸の回転角度を用いて前記アーム先端の現在位置データを
算出し、
前記第３算出部は、前記第２の回転角度を用いて前記アクチュエータを駆動する駆動ト
ルクの推定値を算出する、
請求項１乃至６いずれか１項に記載のロボット制御装置。
【請求項８】
演算処理装置における外乱判定方法であって、
算出部が、対象物に加わったと推定される外力を算出するステップと、
更新部が、前記外力を変数とする時系列モデルのパラメータを推定し、前記パラメータ
を当てはめることで、第１サンプリング周期の当該時系列モデルを更新するステップと、
判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期より
も過去の第２サンプリング周期の時系列モデルとを比較して、前記対象物へ加わる外乱の
有無を判定するステップと、
を有する外乱判定方法。
【請求項９】
アームの駆動軸を回転駆動するアクチュエータを制御する方法であって、
生成部が、前記アーム先端の目標位置データを生成するステップと、
第１検出部が、前記駆動軸の回転角度をサンプリング周期ごとに検出するステップと、
第１算出部が、前記回転角度を用いて前記アーム先端の現在位置データを算出するステ
ップと、
第２算出部が、前記目標値位置データと前記現在位置データとを用いて、前記アクチュ
エータに対する指令値を算出するステップと、
第３算出部が、前記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トル
クの推定値を算出するステップと、
第４算出部が、前記駆動トルクの推定値と、前記指令値から求まる前記駆動トルクの真
値との差分を算出するステップと、
更新部が、前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータ
を当てはめることで、第１サンプリング周期の当該時系列モデルを更新するステップと、
判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期より
も過去の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無
を判定するステップと、
を有するアクチュエータ制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１３−６６９６５（Ｐ２０１３−６６９６５Ａ）
【公開日】平成２５年４月１８日（２０１３．４．１８）
【国際特許分類】
【出願番号】特願２０１１−２０６６５１（Ｐ２０１１−２０６６５１）
【出願日】平成２３年９月２１日（２０１１．９．２１）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
【公開日】平成２５年４月１８日（２０１３．４．１８）
【国際特許分類】
【出願日】平成２３年９月２１日（２０１１．９．２１）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
[ Back to top ]