JP5902425B2

JP5902425B2 - ロボット制御装置、外乱判定方法およびアクチュエータ制御方法

Info

Publication number: JP5902425B2
Application number: JP2011206651A
Authority: JP
Inventors: 準治大明; 淳一郎大賀; 小川　秀樹; 秀樹小川
Original assignee: Toshiba Corp
Current assignee: Toshiba Corp
Priority date: 2011-09-21
Filing date: 2011-09-21
Publication date: 2016-04-13
Anticipated expiration: 2031-09-21
Also published as: JP2013066965A; US8694159B2; US20130073085A1

Description

本発明の実施形態は、ロボット制御装置、外乱判定方法およびアクチュエータ制御方法
に関する。

ハンドリング等を行うロボットアームを動作させる際、周囲の環境に存在する障害物と
の衝突が起こり得る。衝突後においてもロボットアームを安定して動作させるためには、
ロボットアームの衝突を検出する必要がある。

そこで、力覚センサ等を用いずに、ロボットアームの衝突等による外乱の影響で発生す
る外部トルクを推定することで、この外部トルクがあらかじめ設定した閾値以上の場合に
ロボットの先端部が衝突したという判定を行う方法が提案されている。

しかしながら、ロボットアームの動力学モデルのモデル化誤差の影響により、外部トル
クには誤差が含まれることになるために、ロボットアームに加わる外乱を高精度に検出す
ることができない。

特開２０１０−１０５１３８号公報

ロボットアームに加わる外乱を高精度に検出することが可能なロボット制御装置、外乱
判定方法およびロボット制御方法を提供する。

実施形態のロボット制御装置は、アームの駆動軸を回転駆動するアクチュエータと、前
記アーム先端の目標位置データを生成する生成部と、前記駆動軸の回転角度をサンプリン
グ周期ごとに検出する第１検出部と、前記回転角度を用いて前記アーム先端の現在位置デ
ータを算出する第１算出部と、前記目標値位置データと前記現在位置データとを用いて、
前記アクチュエータに対する指令値を算出する第２算出部と、前記駆動軸の回転角度を用
いて前記アクチュエータを駆動する駆動トルクの推定値を算出する第３算出部と、前記駆
動トルクの推定値と、前記指令値から求まる前記駆動トルクの真値との差分を算出する第
４算出部と、前記アームに加わる外乱を検出する第２検出部とを備え、
前記第２検出部は、前記差分を変数とする時系列モデルのパラメータを推定し、前記パ
ラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新する更新
部と、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去
の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定
する判定部とを備える。

実施形態の外乱判定方法は、算出部が、対象物に加わったと推定される外力を算出する
ステップと、更新部が、前記外力を変数とする時系列モデルのパラメータを推定し、前記
パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新するス
テップと、判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング
周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記対象物へ加わ
る外乱の有無を判定するステップとを有する。

実施形態のアクチュエータ制御方法は、生成部が、前記アーム先端の目標位置データを
生成するステップと、第１検出部が、前記駆動軸の回転角度をサンプリング周期ごとに検
出するステップと、第１算出部が、前記回転角度を用いて前記アーム先端の現在位置デー
タを算出するステップと、第２算出部が、前記目標値位置データと前記現在位置データと
を用いて、前記アクチュエータに対する指令値を算出するステップと、第３算出部が、前
記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算出す
るステップと、第４算出部が、前記駆動トルクの推定値と、前記指令値から求まる前記駆
動トルクの真値との差分を算出するステップと、更新部が、前記差分を変数とする時系列
モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周
期の当該時系列モデルを更新するステップと、判定部が、前記第１サンプリング周期の時
系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデ
ルとを比較して、前記アームへの外乱の有無を判定するステップとを有する。

第一の実施形態に係るロボット制御装置の構成図。図１のロボット制御装置のCPUの動作を説明するブロック図。実施例１を説明するための説明図。実施例２を説明するための説明図。１段予測による予測誤差の時間応答。２段予測による予測誤差の時間応答。３段予測による予測誤差の時間応答。実施例３を説明するための説明図。第一の実施形態の第一の変形例に係るロボット制御装置の構成図。図９のロボット制御装置のCPUの動作を説明するブロック図。第一の実施形態の第二の変形例に係るロボット制御装置の構成図。制御対象の一例であるロボットの構成図。図１２のロボットのアームの構成図。アームの衝突の様子を示す図。

以下、発明を実施するための実施形態について説明する。

（第一の実施形態）
本実施形態において制御対象の一例とするロボットは、図１２に示すように、本体３０
０と、本体３００に設けられるアーム３１０を備える。

図１２に示すアーム３１０は、３つのリンク３１１（リンク３１１a，リンク３１１b，
リンク３１１c）、各リンク３１１をそれぞれ回転駆動する際の回転軸（関節）となる３
つの駆動軸３１２（駆動軸３１２a，駆動軸３１２b，駆動軸３１２c）を備えている。

リンク３１１aは、駆動軸３１２aを介して本体３００に回転可能に接続されている。ま
た、リンク３１１bは、駆動軸３１２bを介してリンク３１１aに、リンク３１１cは、駆動
軸３１２cを介してリンク３１１bに、それぞれ回転可能に接続されている。

図１３に模式的に示すように、各リンク３１１は、駆動軸３１２と、各駆動軸３１２を
回転駆動するアクチュエータであるモータ３１３と、モータ３１３の回転数を落とし、各
駆動軸３１２の回転トルクを向上させる減速機３１４と、モータ３１３の回転軸の回転角
度の変化量（以下、モータ角度θ_M）を検出する角度検出部３１５とを備える。なお、角
度検出部３１５としては、例えばエンコーダ等のセンサを用いることができる。

なお、図１２において、本体３００の内部には、このロボットを制御するためのロボッ
ト制御装置が設けられる。ロボット制御装置は、通常時にはロボットのアーム３１０を所
定の軌道に沿ってアーム３１０先端（リンクcの先端位置）の位置制御を行う。

また、ロボット制御装置は、図１４に示すように例えばアーム３１０が障害物へ衝突す
るなどして、アーム３１０に対して外乱が加わった際には、この外乱から衝突を高精度に
検出し、この検出結果を用いてアーム３１０先端の位置制御を行う。

図１は、第一の実施形態に係るロボット制御装置の構成図である。

第一の実施形態に係るロボット制御装置は、図１に示すように、中央演算処理装置（Ｃ
ＰＵ）１００、記憶装置（メモリ）２００を備える。

図１のＣＰＵ１００は３つのモジュールを有する。具体的には、アーム３１０先端の位
置を制御する位置制御部１１０と、各リンク３１１における駆動軸３１２のトルクの推定
値を算出するトルク算出部１２０と、アーム３１０のいずれかのリンク３１１における障
害物への衝突をアーム３１０全体として検出する衝突検出部１３０とを備える。

位置制御部１１０は、後述の位置目標値生成部１１１が生成するアーム先端の３次元位
置（以下、アーム先端位置）の目標値（以下、アーム先端位置目標値）x_Rと_、角度検出部
３１５によるモータ角度θ_Mの検出値に基づいて得られるアーム先端位置xとの差分をゼロ
にする方向にモータ角度θ_Mを角度制御する。また、モータ角度θ_Mの角度制御により減速
機３１４を介してリンク角度θ_Lを角度制御する。これにより、位置制御部１１０は、リ
ンク角度θ_Lの角度制御を通じて、アーム先端位置目標値x_Rにアーム先端位置xを追従させ
る位置制御を行う。

トルク算出部１２０は、位置制御部１１０からリンク角度θ_Lを得て、アーム３１０の
動力学モデルに基づいて、各リンク３１１における駆動軸３１２のトルクの推定値（以下
、トルク推定値）を算出する。なお、トルク推定値は、各モータ３１３を駆動するのに必
要と推定される駆動トルクの推定値である。

衝突検出部１３０は、位置制御部１１０からリンク角度θ_Lを、トルク算出部１２０か
らトルク推定値を得て、アーム３１０に加わる外乱により各リンク３１１に生じる外部ト
ルクを算出する。そして、この外部トルクを用いてアーム３１０全体としての衝突を検出
する。

以下、図１および図２を参照して、それぞれのモジュールについて詳細に説明する。な
お、以下のモジュールの各処理は、サンプリング周期（制御周期）ごとに行うものとし、
サンプリングをk（ただし、k=0,1,2,…）で表す。

_{（位置制御部）}
図１に示すように、位置制御部１１０は、アーム先端位置目標値x_Rを生成する位置目標
生成部１１１、リンク角度θ_Lを算出する角度算出部１１２、アーム先端位置xを算出する
位置算出部１１３、各モータ３１３に対する指令値（電流値）uを算出する指令値算出部
１１４を有する。

ここで、各変数（モータ角度θ_M，リンク角度θ_L，アーム先端位置x）には、それぞれ
次のような関係がある。

なお、nはアーム３１１の駆動軸３１２の数（本実施形態では３）を表している。また
、Λは順運動学に基づく非線形関数で、リンク角度θ_Lからアーム先端位置xを算出する座
標変換を行う関数である。

位置目標生成部１１１は、例えばメモリ２００が予め記憶する、ハンドリング動作開始
から終了までのアーム先端位置目標値の履歴を得て、現在時刻における値をアーム先端位
置目標値x_Rとして生成する。

指令値算出部１１４は、位置目標生成部１１１からアーム先端位置目標値x_Rを、後述の
位置算出部１１３からアーム先端位置xを得て、アーム先端位置目標値x_Rと_、角アーム先
端位置xとの差分をゼロにする方向にモータ角度θ_Mを角度制御するための指令値uを算出
する。

以下、図２を参照して、指令値算出部１１４の具体的な処理について説明する。

指令値算出部１１４は、アーム先端位置目標値x_Rと、アーム先端位置xを用いて、アー
ム先端位置の微小変化Δxを次式により算出する。

ここで、アーム先端位置の微小変化Δxとリンク角度の微小変化Δθ_Lの関係を与えるΛ
(θ_L)を偏微分したn×nのヤコビ行列J(θ_L)を次式のように定義する。

このとき、上記ヤコビ行列の逆行列である逆ヤコビ行列を用いることで、アーム先端位
置の微小変化Δxとリンク角度の微小変化Δθ_Lには次式の関係が成り立つ。

すなわち、上式により目標リンク角度の微小変化値を算出することができるので、これ
を積分することによって、

としてリンク角度目標値θ_LRが得られる。

そこで、指令値算出部１１４は、後述の角度算出部１１２が算出するリンク角θ_Lを用
いて逆ヤコビ行列を算出する。そして、アーム先端位置の微小変化Δxと、逆ヤコビ行列
を用いてリンク角度の微小変化Δθ_Lを次式のように算出する。

また、上記リンク角度の微小変化Δθ_Lを積分することで、（数５）によりリンク角度
目標値θ_LRを算出する。

そして、指令値算出部１１４は、リンク角度目標値θ_LRを減速比N_Gで除算することで、
モータ角度目標値θ_MRを算出する。そして、モータ角度目標値θ_MRを入力として、公知の
速度・位置制御則を適用することで各モータ３１３を駆動するための指令値（電流値）u
を算出する。

指令値算出部１１４は、上記の指令値uをモータ３１３対して与えることで、モータ３
１３を駆動しモータ角度θ_Mを角度制御する。この際、角度検出部３１５は、この駆動に
よるモータ角度θ_Mを検出する。

角度算出部１１２は、角度検出部３１５が検出するモータ角度θ_Mを用いて、次式のよ
うにリンク角θ_Lを算出する。

位置算出部１１３は、メモリ２００が記憶する非線形関数式Λを得て、角度算出部１１
２が算出するリンク角θ_Lを用いることで、次式のように現在時刻におけるアーム先端位
置xを算出する。

上記のように位置算出部１１３が算出するアーム先端位置xは、フィードバック信号と
して指令値算出部１１４に対して入力される。

なお、（数８）に示すように、アーム先端位置xの算出にはリンク角度θ_Lが必要となる
ので、角度検出部３１５が直接リンク角度θLを検出することもできる。また、減速機３
１４を用いない構成も考えられる。この場合には、モータ角度θ_Mとリンク角度θ_Lは一致
するため、角度算出部１１２は必ずしも必要とはならないことは明らかである。

（トルク算出部）
図１に示すように、トルク算出部１２０は、慣性力トルクを算出する慣性力トルク算出
部１２１、摩擦力トルクを算出する摩擦力トルク算出部１２２、重力トルクを算出する重
力トルク算出部１２３、遠心力・コリオリ力トルクを算出する遠心力・コリオリ力トルク
算出部１２４、加算部１２５を有する。

ここで、一般にロボットアームの動力学モデルは次式の運動方程式で与えられる。

したがって、リンク角度θ_Lの検出値を１回微分したリンク角速度、２回微分したリン
ク角加速度を求めれば、各リンクの重心位置や慣性モーメント、摩擦係数等の物理パラメ
ータを用いて、重力、慣性力、摩擦力、遠心力・コリオリ力等によるトルクの推定値を算
出することができる。

なお、リンク角速度は、時系列データとして得られるリンク角度θ_Lの、それぞれ異な
るサンプリング周期に得られる２つの値の差分等により算出できる（１回微分）。また、
リンク角加速度は、それぞれ異なるサンプリング周期に得られる２つのリンク角速度の値
の差分等により算出できる（２回微分）。

慣性力トルク算出部１２１、摩擦力トルク算出部１２２、重力トルク算出部１２３、遠
心力・コリオリ力トルク算出部１２４は、メモリ２００に予め格納されている物理パラメ
ータ及び角度算出部１１２が算出するリンク角度を用いて、慣性力トルク、摩擦力トルク
、重力トルク、遠心力・コリオリ力トルクをそれぞれ算出する。

加算部１２５は、次式のように各駆動軸３２０のトルク推定値（以下では、変数にハッ
トを付して推定値を表す）を算出する。

なお、本実施形態においては、慣性力トルク、摩擦力トルク、重力トルク、遠心力・コ
リオリ力トルクについて考慮するものとしたが、遠心力・コリオリ力によるトルクは他の
トルクに比べて充分に小さい（１/１０程度）ため、遠心力・コリオリ力によるトルクに
関しては考慮しないことも可能である。この場合には、加算部１２５は、各駆動軸３２０
のトルク推定値を、慣性力トルク、摩擦力トルク、重力トルクの和により算出する。

（衝突検出部）
図１に示すように、衝突検出部１３０は、アーム３１０に外部から加わる外部トルクを
算出する外部トルク算出部１３１、時系列モデルのパラメータを推定しモデルを更新する
モデル更新部１３２、更新されるモデルを用いて衝突判定を行う衝突判定部１３３を有す
る。

ここで、外部トルクτ_dは、トルク推定値と駆動トルクの真値との差分により次式のよ
うに与えられる。

なお、上式における駆動トルクの真値は、モータ３１３への指令値uに、モータ３１３
のカタログ値等により得られ、メモリ２００が予め記憶するトルク定数K_tを乗算すること
で得られる。

外部トルクは、モデル化誤差がなく、外乱等も加わっていない理想状態においては、ゼ
ロになっているはずである。したがって、外部トルクがゼロでない場合には、アーム３１
０には外乱が加わっていることが推定できるために、この外部トルクを衝突検出に用いる
ことができる。

しかしながら、（数９）の動力学モデルには、慣性モーメントや摩擦係数などの物理パ
ラメータ等によるモデル化誤差が含まれている。また、（数９）の動力学モデルでは考慮
されていない機械共振により生じる弾性力トルク等の影響も存在し、低速で動作している
ときには衝突検出は機能するが、高速で動作しているときには、加減速に必要なトルク変
動と衝突によるトルク変動の区別がつかないことがある。

そこで、本実施形態においては、このようなモデル化誤差を吸収するために、（数１１
）の外部トルクを各駆動軸３１２において１次元の時系列信号y(k)（ただし、k=0,1,2,…
）と捉えて、次式のように表されるAR（Auto Regressive：自己回帰）モデルを当てはめ
る。

上式のARモデルは、時系列信号y(k)は，過去n_aステップ分の時系列信号の線形結合で表
される。ここで，a₁〜a_naはARパラメータであり、e(k)は残差を表す。

また、（数１２）のARモデルは、

と定義できる。ここで、ARパラメータαとは、ARモデルを特徴付けるための係数であり、
残差e(k)を最小にするARパラメータを適切に推定することで、ARモデルを算出することが
できる。

ARパラメータαの推定値は、次式のような忘却係数λ（通常0.97〜0.995の範囲から選
択する）付きの逐次最小２乗法を用いれば、リアルタイムに更新しながら算出することが
できる。

ここで、ε(k)は予測誤差、kαはゲイン、P(k)は共分散行列をそれぞれ表している。

外部トルク算出部１３１は、トルク算出部１２０が算出するトルク推定値と、指令値算
出部１１４が算出する指令値uにトルク定数K_tを乗算することで得られる駆動トルクを用
いて、次式のように外部トルクを算出する。この外部トルクは、アーム３１０に対して外
から加わったと推定される外力と見なすことができる。

モデル更新部１３２は、外部トルク算出部１３１が算出する外部トルクを変数として、
（数１２）で表されるARモデルに当てはめる。そして、（数１４）のように、逐次最小二
乗法を用いて全てのARパラメータの推定値を逐次推定することで、ARモデルを更新する。

衝突判定部１３３は、詳細は後述するようにモデル更新部１３２により逐次更新される
ARモデルに基づいて、アーム３１０の衝突の有無を判定する。

以下、図３乃至図８を参照して衝突判定部１３３の動作について説明する。

（実施例１）
衝突判定部１３３が衝突を判定するための第１の方法として、逐次推定しているARパラ
メータの時間変化を用いる方法を説明する。

図３（a）は、アーム３１０を衝突させた時の外部トルクとモータ角速度の時間応答の
一例であり、図３（b）は、逐次更新されるARモデルのパラメータの時間応答の一例であ
る。

ここでは、５次のARモデルを用いており、ARパラメータは５本表示されている。図中の
凡例のa(1)〜a(5)は、それぞれa₁〜a₅を示している。図３（b）では、1.16[s]前後で衝突
が起こっているが、ARパラメータのうち、最低次であるa₁の変化が顕著であり、次にa₃の
変化が大きい。

それに対して、図３（a）に示した外部トルクがゆっくりした変化であるので、衝突検
出を目的とした場合、ARパラメータの変化を検出する方法の優位性は明らかである。ここ
で、衝突が起こってもモータ角速度がすぐにゼロにならないのは、関節軸がたわんでいる
ことを示している。

衝突判定部１３３は、図３において最も変化の大きい最低次のARパラメータa₁の変化量
を、現在のサンプリング周期kにおけるパラメータの推定値と、一つ前のサンプリング周
期k-1におけるパラメータの推定値の差分により算出する。

そして、いずれかの駆動軸３１２において、最低次のARパラメータの変化量の絶対値が
、メモリ２００に格納されている既定の閾値（例えば0.1）を超える場合に、アーム３１
０が衝突したものと判定する。

なお、逐次最小２乗法において、忘却係数λを調整することによって、ARパラメータの
変化の度合いを調整することができる。忘却係数をロボットの動作加速度が小さいときに
は0.99など値を大きくしておき、加速度が大きいときには0.98など値を小さくするなど、
忘却係数を可変にすることも衝突判定の精度を上げることに有効である。

また、本実施形態のように最低次のARパラメータを用いるのではなく、他のARパラメー
タを用いることで衝突を判定することも可能である。

（実施例２）
衝突判定部１３２が衝突を判定するための第２の方法として、多段予測を用いた予測誤
差の時間変化を用いる方法を説明する。

（数１２）のARモデルにおいて、ARパラメータを逐次推定しながら、イタレーションk
を進めればディジタル処理に起因するサンプル時間遅れを取り戻せる上、多段の予測をす
ることができる。まず、１段先予測値は、

で与えられ、２段先予測値、３段先予測値は、それぞれ、

で与えられる。

よって、１段先予測誤差は、

で与えられ、２段先予測誤差、３段先予測誤差は、それぞれ、

で与えられる。

図４は、外部トルクと１，２，３段先予測誤差の各時間応答を比較したものである。同
図から、１段先予測値は、外部トルクからやや遅れていることがわかる。一方、２段先予
測値はやや進んでおり、３段先予測値は明らかに進んでいる。

また、図５は、１段先予測誤差の時間変化から衝突を検出している様子を示している。
同じく図６は２段先予測誤差、図７は３段先予測誤差の場合である。

図５，６，７を比較すると、それぞれ衝突検出が可能であるが、図６の予測誤差の時間
応答の波形のレンジが比較的小さく、閾値の設定が容易である。したがって、以上からア
ーム３１０の衝突検出には２段先予測誤差を用いることが好ましい。

衝突判定部１３３は、（数１９）のように誤差e₂(k)を算出する。そして、誤差e₂(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を用いて、次式の微分方程式を解くことでリ
ンク角度の推定値を算出する。

慣性力トルク算出部１２１、摩擦力トルク算出部１２２、重力トルク算出部１２３、遠
心力・コリオリ力トルク算出部１２４は、メモリ２００に予め格納されている物理パラメ
ータ及びリンク角度算出部１２７が算出するリンク角度の推定値を用いて、それぞれ慣性
力トルク、摩擦力トルク、重力トルク、遠心力・コリオリ力トルクを算出する。

また、弾性力トルク算出部１２６は、次式のように弾性力トルクを算出する。

加算部１２５は、慣性力トルク、摩擦力トルク、重力トルク、遠心力・コリオリ力トル
ク、弾性力トルクを用いて次式のように各駆動軸３２０のトルク推定値を算出する。

なお、減速機３１４のバネ要素によるねじれは微小であるので、順運動学やヤコビ行列
の算出に必要なリンク角度については、第一の実施形態と同様に角度算出部１１２が算出
する値を用いることができる。

（第二の変形例）
以下、図１１を参照して、第一の実施形態のロボット制御装置の第二の変形例について
説明する。なお、図１と同様の構成については同様の符号を付すことで、その詳細な説明
を省略する。

図１１は第一の変形例に係るロボット制御装置に用いるＣＰＵ１００のシステム構成図
である。本変形例では、図１に示す構成に加え、アーム３１０の各リンク３１１のどこで
衝突が生じたかを推定する衝突部位推定部１４０をさらに有する。

衝突部位推定部１４０は、衝突検出部１３０がアーム３１０の各駆動軸３１２において
それぞれ衝突検出を行い、衝突を検出した駆動軸３１２を備えるリンク３１１において衝
突が生じたものと推定する。

以上説明した少なくとも１つの本実施形態のロボット制御装置によれば、ロボットアー
ムに加わる外乱を高精度に検出することが可能となる。

これら実施形態は、例として提示したものであり、発明の範囲を限定することは意図し
ていない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明
の要旨を逸脱しない範囲で、様々の省略、置き換え、変更を行うことができる。これら実
施形態やその変形は、発明の範囲や要旨に含まれると同時に、特許請求の範囲に記載され
た発明とその均等の範囲に含まれるものである。

１００・・・ＣＰＵ
１１０・・・位置制御部
１１１・・・位置目標生成部
１１２・・・角度算出部
１１３・・・位置算出部
１１４・・・指令値算出部
１２０・・・トルク算出部
１２１・・・慣性力トルク算出部
１２２・・・摩擦力トルク算出部
１２３・・・重力トルク算出部
１２４・・・遠心力・コリオリ力トルク算出部
１２５・・・加算部
１２６・・・弾性力トルク算出部
１２７・・・リンク角度算出部
１３０・・・衝突検出部
１３１・・・外部トルク算出部
１３２・・・モデル更新部
１３３・・・衝突判定部
１４０・・・衝突部位検出部
２００・・・メモリ
３００・・・本体
３１０・・・アーム
３１１・・・リンク
３１２・・・駆動軸
３１３・・・モータ
３１４・・・減速機
３１５・・・角度検出部

Claims

アームの駆動軸を回転駆動するアクチュエータと、
前記アーム先端の目標位置データを生成する生成部と、
前記駆動軸の回転角度をサンプリング周期ごとに検出する第１検出部と、
前記回転角度を用いて前記アーム先端の現在位置データを算出する第１算出部と、
前記目標値位置データと前記現在位置データとを用いて、前記アクチュエータに対する指令値を算出する第２算出部と、
前記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算出する第３算出部と、
前記駆動トルクの推定値と、前記指令値から求まる前記駆動トルクの真値との差分を算出する第４算出部と、
前記アームに加わる外乱を検出する第２検出部と、
を備え、
前記第２検出部は、
前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新する更新部と、
前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定する判定部と、
を備えるロボット制御装置。
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルの前記パラメータの変化量と当該閾値を比較し、前記アームへの外乱の有無を判定する請求項１に記載のロボット制御装置。
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルを用いて多段の予測誤差を算出し、当該予測誤差の変化量と前記閾値を比較し、前記アームへの外乱の有無を判定する請求項１に記載のロボット制御装置。
前記外乱の検出のための閾値を記憶する記憶部を備え、
前記判定部は、
前記時系列モデルを用いて周波数応答を算出し、前記周波数応答の変化量と前記閾値を比較し、前記アームへの外乱の有無を判定する請求項１に記載のロボット制御装置。
前記生成部は、
前記第２検出部が前記外乱を検出した場合に、前記アームを緊急停止または反転動作する方向に前記目標位置データを更新する請求項１乃至４いずれか１項に記載のロボット制御装置。
前記アームは複数のリンクと、複数の駆動軸とを有し、
前記第２検出部は、前記駆動軸ごとに前記外乱を検出し、
前記外乱が検出された駆動軸を有するリンクに外乱が加わったものと推定する推定部をさらに備える請求項１乃至５いずれか１項に記載のロボット制御装置。
前記駆動軸は、前記アクチュータの回転軸である第１軸と、当該第１軸の回転数を減少させる減速機を介して前記第１軸と接続する第２軸とを有し、前記第１検出部は、前記第１軸の回転角度を検出するものであって、
前記第１軸の回転角度を用いて前記第２軸の回転角度をサンプリング周期ごとに算出する第５算出部を備え、
前記第１算出部は、前記第２軸の回転角度を用いて前記アーム先端の現在位置データを算出し、
前記第３算出部は、前記第２の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算出する、
請求項１乃至６いずれか１項に記載のロボット制御装置。
演算処理装置における外乱判定方法であって、
算出部が、対象物の駆動軸の回転角度を用いて算出される、前記駆動軸を回転駆動するアクチュエータの駆動トルクの推定値と、前記アクチュエータに対する指令値から求まる前記駆動トルクの真値との差分を算出するステップと、
更新部が、前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新するステップと、
判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記対象物へ加わる外乱の有無を判定するステップと、
を有する外乱判定方法。
アームの駆動軸を回転駆動するアクチュエータを制御する方法であって、
生成部が、前記アーム先端の目標位置データを生成するステップと、
第１検出部が、前記駆動軸の回転角度をサンプリング周期ごとに検出するステップと、
第１算出部が、前記回転角度を用いて前記アーム先端の現在位置データを算出するステップと、
第２算出部が、前記目標値位置データと前記現在位置データとを用いて、前記アクチュエータに対する指令値を算出するステップと、
第３算出部が、前記駆動軸の回転角度を用いて前記アクチュエータを駆動する駆動トルクの推定値を算出するステップと、
第４算出部が、前記駆動トルクの推定値と、前記指令値から求まる前記駆動トルクの真値との差分を算出するステップと、
更新部が、前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新するステップと、
判定部が、前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記アームへの外乱の有無を判定するステップと、
を有するアクチュエータ制御方法。
対象物の駆動軸の回転角度を用いて算出される、前記駆動軸を回転駆動するアクチュエータの駆動トルクの推定値と、前記アクチュエータに対する指令値から求まる前記駆動トルクの真値との差分を算出する算出部と、
前記差分を変数とする時系列モデルのパラメータを推定し、前記パラメータを当てはめることで、第１サンプリング周期の当該時系列モデルを更新する更新部と、
前記第１サンプリング周期の時系列モデルと前記第１サンプリング周期よりも過去の第２サンプリング周期の時系列モデルとを比較して、前記対象物へ加わる外乱の有無を判定する判定部と、
を有するロボット制御装置。