特開 2022-83563 速度指令値演算装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022083563

(43)【公開日】2022-06-06

(54)【発明の名称】速度指令値演算装置

(51)【国際特許分類】

   G05D 3/12 20060101AFI20220530BHJP

   B25J 19/06 20060101ALI20220530BHJP

   G05D 13/62 20060101ALI20220530BHJP

【ＦＩ】

G05D3/12 306P

B25J19/06

G05D3/12 S

G05D13/62 F

【審査請求】未請求

【請求項の数】12

【出願形態】ＯＬ

(21)【出願番号】P 2020194954

(22)【出願日】2020-11-25

(71)【出願人】

【識別番号】000237271

【氏名又は名称】株式会社ＦＵＪＩ

(74)【代理人】

【識別番号】110000604

【氏名又は名称】弁理士法人 共立特許事務所

(72)【発明者】

【氏名】名桐 啓祐

(72)【発明者】

【氏名】大野 寿彦

【テーマコード（参考）】

3C707

5H303

5H313

【Ｆターム（参考）】

3C707BS10

3C707LU01

3C707MS14

5H303AA10

5H303BB01

5H303CC10

5H303DD01

5H303EE03

5H303JJ01

5H303KK03

5H313MM01

(57)【要約】      （修正有）

【課題】移動平均処理を用いた速度指令値演算装置において、可動部が移動動作によって発生が予想される異常状態に対応可能な速度指令値演算装置を提供する。
【解決手段】速度指令値演算装置１は、可動部９４に要求される移動動作に基づいて、所定の演算周期ごとの原速度値を演算する原速度値演算部２と、通常時平均化母数を用いて前記演算周期ごとに前記原速度値に移動平均処理を施すことにより、前記可動部を駆動する駆動機構に指令する前記演算周期ごとの速度の指令値を演算する指令値演算部３と、前記可動部が前記移動動作を行うことによって障害の発生が予想される異常状態を判定する異常判定部４と、前記異常状態が判定されたときに、前記原速度値をゼロとするように前記原速度値演算部を制御するとともに、前記通常時平均化母数よりも小さな異常時平均化母数を用いて前記指令値を演算するように前記指令値演算部を制御する異常対応部５と、を備える。
【選択図】図２

【特許請求の範囲】

【請求項1】

可動部に要求される移動動作に基づいて、所定の演算周期ごとの原速度値を演算する原速度値演算部と、
通常時平均化母数を用いて前記演算周期ごとに前記原速度値に移動平均処理を施すことにより、前記可動部を駆動する駆動機構に指令する前記演算周期ごとの速度の指令値を演算する指令値演算部と、
前記可動部が前記移動動作を行うことによって障害の発生が予想される異常状態を判定する異常判定部と、
前記異常状態が判定されたときに、前記原速度値をゼロとするように前記原速度値演算部を制御するとともに、前記通常時平均化母数よりも小さな異常時平均化母数を用いて前記指令値を演算するように前記指令値演算部を制御する異常対応部と、
を備える速度指令値演算装置。

【請求項2】

前記異常対応部は、前記通常時平均化母数に予め設定された減少割合を乗算して前記異常時平均化母数を求める、請求項１に記載の速度指令値演算装置。

【請求項3】

前記減少割合は、前記可動部の減速度が前記移動動作に応じて定められた許容減速度を越えず、かつ、前記駆動機構により発生可能な最大減速度以下となるように設定される、請求項２に記載の速度指令値演算装置。

【請求項4】

可動部に要求される移動動作に基づいて、所定の演算周期ごとの原速度値を演算する原速度値演算部と、
通常時平均化母数を用いて前記演算周期ごとに前記原速度値に移動平均処理を施すことにより、前記可動部を駆動する駆動機構に指令する前記演算周期ごとの速度の指令値を演算する指令値演算部と、
前記可動部が前記移動動作を行うことによって障害物に衝突し、または移動可能範囲の境界を越えることが予想される異常状態を判定する異常判定部と、
前記異常状態が判定されたときに、前記原速度値をゼロとするように前記原速度値演算部を制御するとともに、前記可動部の現在位置と前記障害物または前記境界との離間距離に基づいて、前記通常時平均化母数よりも小さな異常時平均化母数を求め、前記異常時平均化母数を用いて前記指令値を演算するように前記指令値演算部を制御する異常対応部と、
を備える速度指令値演算装置。

【請求項5】

前記異常対応部は、前記通常時平均化母数を用いて演算された前記指令値にしたがうと仮定した場合に、前記可動部が停止するまでに要する前記現在位置からの制動距離を求め、前記離間距離および前記制動距離に基づいて前記異常時平均化母数を求める、請求項４に記載の速度指令値演算装置。

【請求項6】

前記通常時平均化母数は、前記可動部の加速度が前記移動動作に応じて定められた許容加速度を越えず、かつ、前記駆動機構により発生可能な最大加速度以下となるように設定される、請求項１～５のいずれか一項に記載の速度指令値演算装置。

【請求項7】

前記指令値演算部は、
前記通常時平均化母数に等しい個数で新旧の順に並ぶサンプルエリアに、時系列的に並んだ複数の前記原速度値を記憶するバッファと、
前記演算周期の更新に伴い、前記サンプルエリアの各々に記憶された前記原速度値を旧い側の前記サンプルエリアへ順送りに移動させるとともに、もっとも新しい側の前記サンプルエリアに現在の前記原速度値を記憶させる原速度値移動部と、
前記演算周期の各々において、前記サンプルエリアの各々に記憶された前記原速度値を加算して、前記通常時平均化母数で除算することにより前記指令値を演算する平均演算部と、を有する、
請求項１～６のいずれか一項に記載の速度指令値演算装置。

【請求項8】

前記異常対応部は、
前記異常状態が判定された前記演算周期で、前記異常時平均化母数に等しい個数の新しい側の前記サンプルエリアに現在の前記指令値を記憶させるとともに、前記平均演算部の前記通常時平均化母数を前記異常時平均化母数に変更し、さらに、
前記異常状態が判定された以降の前記演算周期で、前記通常時平均化母数から前記異常時平均化母数を減算した個数の旧い側の前記サンプルエリアにゼロを記憶させる、
請求項７に記載の速度指令値演算装置。

【請求項9】

前記異常対応部は、
前記異常状態が判定された前記演算周期で、前記異常時平均化母数に等しい個数の新しい側の前記サンプルエリアに現在の前記指令値を記憶させるとともに、前記平均演算部の前記通常時平均化母数を前記異常時平均化母数に変更し、さらに、
前記異常状態が判定された以降の前記演算周期で、前記原速度値移動部および前記平均演算部が前記異常時平均化母数に等しい個数の新しい側の前記サンプルエリアのみを用いるように制御する、
請求項７に記載の速度指令値演算装置。

【請求項10】

前記原速度値演算部は、一定の前記原速度値を演算する、請求項１～９のいずれか一項に記載の速度指令値演算装置。

【請求項11】

前記原速度値演算部は、前記可動部の前記移動動作に応じて定められた許容速度を越えず、かつ、前記駆動機構により発生可能な速度の最大値を一定の前記原速度値とする、請求項１０に記載の速度指令値演算装置。

【請求項12】

前記可動部は、作業ロボットの可動アームであり、
前記駆動機構は、前記可動アームに前記移動動作を行わせる駆動装置、および前記可動アームの前記移動動作を制動する制動装置を含む、
請求項１～１１のいずれか一項に記載の速度指令値演算装置。

【発明の詳細な説明】

【技術分野】

【0001】

本明細書は、移動平均処理を用いて速度の指令値を演算する速度指令値演算装置に関する。

【背景技術】

【0002】

工場や事業所などの自動化、省力化を推進するために、作業ロボットが広く用いられている。多くの作業ロボットは、可動アームと、可動アームの先端に設けられて指示された作業を行うエンドエフェクタと、可動アームを駆動する駆動機構と、を備える。可動アームに要求される移動動作を実現するために、駆動機構に指令する指令値を演算する速度指令値演算装置が用いられる。速度指令値演算装置の一種に、移動平均処理を用いる方式の装置がある。この種の速度指令値演算装置に関する技術例が、特許文献１、２に開示されている。

【0003】

特許文献１の指令生成装置は、所定の平均時間における速度指令の移動平均を計算し移動平均速度指令として出力する移動平均フィルタと、平均時間を変更する手段と、平均時間の変更に伴って発生する移動距離の差分を移動距離補正量として演算する手段と、移動距離補正量に応じた速度補正値を演算する手段とを備え、移動平均速度指令に速度補正値を加算して、電動機の位置決め制御における動作指令を求める。これによれば、複数の移動平均フィルタを設けることなく、電動機の動作中に平均時間を変更することができ、かつ、正しい位置決めが行える、とされている。

【0004】

また、特許文献２の位置指令装置は、上位システムのコマンドから第１指令位置を生成する直線加減速指令生成部と、第１指令位置を設定可能な回数の移動平均をして第２指令位置を生成するＳ字加減速指令生成部と、第２指令位置を位置制御装置に送信する送信部と、上位システムの移動平均回数変更のコマンドにしたがって変更可能なときに回数変更指令を生成する回数変更判断部と、を備える。これによれば、移動平均時間の変更が可能な位置指令装置を提供できる、とされている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】国際公開第２０１１／１０１９１５号

【特許文献2】特開２００８－１７１２３０号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

ところで、特許文献１、２の技術では、移動平均の平均化母数（平均時間、移動平均回数）を可変とすることにより、機械振動の低減と動作時間の短縮の両立を図っている。つまり、円滑な移動動作が必要な時期には平均化母数を大きく変更して速度変化を制限し、動作時間の短縮が必要な時期には平均化母数を小さく変更して大きな速度変化を許容している。しかしながら、これらの技術は、通常時における可動部の移動動作を良好化するものであって、異常状態に対応するものではない。

【0007】

詳述すると、指令値（動作指令、第２指令位置）にしたがい可動部が移動動作を行うことによって、障害の発生が予想される場合がある。このような緊急停止が必要な異常状態に対して、特許文献１、２の技術は対応することができない。つまり、特許文献１、２の技術では、異常状態に陥っていても、可動部は通常時と同じ減速度で停止する。その結果、可動部が停止する以前に障害物に衝突してしまう等の問題が発生する。

【0008】

一方、異常状態に対応するために、可動部の移動目標位置を現在位置に設定したり、可動部の動作速度を直ちにゼロに設定したりする即時停止手段が考えられる。しかしながら、この手段では、駆動機構に過大な負担を掛けることになって故障の要因となるおそれが生じる。加えて、可動部の減速度が極端に大きくなるため、弊害のおそれが生じる。

【0009】

それゆえ、本明細書では、可動部が移動動作を行うことによって障害の発生が予想される異常状態に対応することにより、障害の実際の発生を抑制することができる速度指令値演算装置を提供することを解決すべき課題とする。

【課題を解決するための手段】

【0010】

本明細書は、可動部に要求される移動動作に基づいて、所定の演算周期ごとの原速度値を演算する原速度値演算部と、通常時平均化母数を用いて前記演算周期ごとに前記原速度値に移動平均処理を施すことにより、前記可動部を駆動する駆動機構に指令する前記演算周期ごとの速度の指令値を演算する指令値演算部と、前記可動部が前記移動動作を行うことによって障害の発生が予想される異常状態を判定する異常判定部と、前記異常状態が判定されたときに、前記原速度値をゼロとするように前記原速度値演算部を制御するとともに、前記通常時平均化母数よりも小さな異常時平均化母数を用いて前記指令値を演算するように前記指令値演算部を制御する異常対応部と、を備える速度指令値演算装置を開示する。

【0011】

また、本明細書は、可動部に要求される移動動作に基づいて、所定の演算周期ごとの原速度値を演算する原速度値演算部と、通常時平均化母数を用いて前記演算周期ごとに前記原速度値に移動平均処理を施すことにより、前記可動部を駆動する駆動機構に指令する前記演算周期ごとの速度の指令値を演算する指令値演算部と、前記可動部が前記移動動作を行うことによって障害物に衝突し、または移動可能範囲の境界を越えることが予想される異常状態を判定する異常判定部と、前記異常状態が判定されたときに、前記原速度値をゼロとするように前記原速度値演算部を制御するとともに、前記可動部の現在位置と前記障害物または前記境界との離間距離に基づいて、前記通常時平均化母数よりも小さな異常時平均化母数を求め、前記異常時平均化母数を用いて前記指令値を演算するように前記指令値演算部を制御する異常対応部と、を備える速度指令値演算装置を開示する。

【発明の効果】

【0012】

本明細書で開示する速度指令値演算装置において、異常対応部は、異常状態が判定されたときに、原速度値をゼロに制御するとともに、通常時平均化母数よりも小さな異常時平均化母数を用いて指令値を演算させる。これによれば、現在以降の原速度値がゼロとなるため、可動部は、直ちに減速を開始する。かつ、現在以降は小さな異常時平均化母数が用いられるため、可動部は、通常時よりも大きな減速度で停止に至る。この結果、可動部は、異常状態発生時の緊急停止動作において通常時よりも小さな制動距離で停止することができ、障害の実際の発生が抑制される。

【図面の簡単な説明】

【0013】

【図1】第１実施形態の速度指令値演算装置を適用する作業ロボットの構成を模式的に示した斜視図である。

【図2】速度指令値演算装置の機能構成を示すブロック図である。

【図3】指令値演算部が有するバッファを模式的に説明する図である。

【図4】速度指令値演算装置の全般的な動作を説明する動作フローの図である。

【図5】速度指令値演算装置の異常時対応の動作を説明する動作フローの図である。

【図6】速度指令値演算装置が演算した通常時の原速度値および指令値を図式的に表した図である。

【図7】指令値演算部で移動平均処理により指令値を演算する方法を表した表形式の図である。

【図8】速度指令値演算装置が演算した異常状態における原速度値および指令値を図式的に表した図である。

【図9】指令値演算部で移動平均処理により異常状態における指令値を演算する方法を表した表形式の図である。

【図10】従来技術の速度指令値演算装置が演算した異常状態における原速度値および指令値を図式的に表した図である。

【図11】従来技術の指令値演算部で移動平均処理により異常状態における指令値を演算する方法を表した表形式の図である。

【図12】第２実施形態の速度指令値演算装置の機能構成を示すブロック図である。

【図13】第２実施形態で、異常状態における原速度値および指令値を図式的に表した図である。

【図14】第２実施形態で、異常状態における指令値を演算する方法を表した表形式の図である。

【発明を実施するための形態】

【0014】

１．作業ロボット９の構成例
まず、第１実施形態の速度指令値演算装置１を適用する作業ロボット９の構成例について、図１および図２を参考にして説明する。作業ロボット９は、基部９１、可動アーム９２（可動部）、エンドエフェクタ９３、駆動機構９４、および図略の作業指令部などで構成される。可動アーム９２は、基部９１に対して移動可能に設けられる。可動アーム９２は、複数の部材で構成され、複数の関節および複数の可動軸を有する。エンドエフェクタ９３は、可動アーム９２の先端に設けられており、指示された作業を実施する。

【0015】

駆動機構９４は、可動アーム９２の移動動作を駆動する。駆動機構９４は、可動軸の軸数に対応して複数組設けられる。以降の説明を簡明化するために、一つの可動軸に対応する一組の駆動機構９４について詳述する。駆動機構９４は、図２に示されるように、駆動制御部９５、駆動装置９６、および制動装置９７で構成される。

【0016】

駆動装置９６は、可動アーム９２に移動動作を行わせる。駆動装置９６は、例えば、サーボモータやパルスモータなどと呼ばれる制御性の良好なモータを用いて構成される。これに限定されず、駆動装置９６は、油圧機構や空気圧機構などを用いて可動アーム９２を駆動する装置であってもよい。

【0017】

制動装置９７は、可動アーム９２の移動動作を制動する。制動装置９７は、例えば、電磁力によって制動力を発生する電磁ブレーキを用いて構成される。これに限定されず、制動装置９７は、摩擦プレートの摩擦力によって制動力を発生する摩擦ブレーキなどを用いて構成されてもよい。

【0018】

駆動制御部９５は、速度の指令値ＣＶを受け取り、この指令値ＣＶを駆動装置９６および制動装置９７に適合する物理量に変換して、駆動装置９６および制動装置９７に指令する。適合する物理量は、モータや電磁ブレーキに流す電流の電流値、油圧機構や空気圧機構の圧力値、摩擦ブレーキにおける摩擦プレートの動作量などが相当する。

【0019】

作業指令部は、オペレータからの作業指示や予め設定された作業メニューに基づいて、指示された作業の実施を制御する。作業指令部は、作業の実施に際し、可動アーム９２に要求される移動動作、およびエンドエフェクタ９３に要求される作業動作を管理する。可動アーム９２に要求される移動動作は、可動アーム９２の現在位置からの移動方向および移動距離、あるいは移動する最終的な目標位置などで表される。

【0020】

ここで、可動アーム９２が移動するときの速度、加速度、および減速度は、二つの制約条件をもち、小さい側の制約条件によって規制される。第一制約条件は、駆動機構９４の性能上の限界である。すなわち、駆動装置９６からの駆動では、可動アーム９２の移動動作において発生可能な最大速度および最大加速度の制約がある。また、制動装置９７からの制動では、可動アーム９２の移動動作において発生可能な最大減速度の制約がある。

【0021】

第二制約条件は、可動アーム９２の移動動作に応じて定められた許容速度、許容加速度、および許容減速度の制約である。第二制約条件は、可動アーム９２が移動動作を行ったときに弊害が発生しないように設定される。例えば、エンドエフェクタ９３が部品を把持しているときに、可動アーム９２が過大な加速や減速を行うと、エンドエフェクタ９３から部品が落下するという弊害が発生し得る。また、エンドエフェクタ９３が移動しながら塗装などの作業を行う場合に、可動アーム９２が過大な速度で移動すると、作業品質が低下するという弊害が発生し得る。このような弊害を考慮して、許容速度、許容加速度、および許容減速度が適正に設定される。

【0022】

２．第１実施形態の速度指令値演算装置１の構成
次に、第１実施形態の速度指令値演算装置１の構成について、図２、図３、および図６を参考にして説明する。速度指令値演算装置１は、作業ロボット９の作業指令部からの要求指令に基づいて、速度の指令値ＣＶを演算し、駆動制御部９５に出力する。第１実施形態において、作業指令部からの要求指令は、可動アーム９２の移動動作に要求される移動距離Ｌで表される。速度指令値演算装置１は、入力部および出力部をもつコンピュータ装置を用いて構成される。図２に示されるように、速度指令値演算装置１は、四つの制御機能部、すなわち原速度値演算部２、指令値演算部３、異常判定部４、および異常対応部５で構成される。

【0023】

原速度値演算部２は、可動アーム９２に要求される移動距離Ｌに基づいて、所定の演算周期ＴＳごとの原速度値Ｖ０を演算する。原速度値演算部２は、可動アーム９２が移動動作を行うときの仮想的な一定の原速度値Ｖ０を演算する。つまり、可動アーム９２は、必ずしも一定の原速度値Ｖ０で動くわけでは無い。一定の原速度値Ｖ０は、ある期間だけ０でない原速度値Ｖ０が継続する矩形波と言い換えることができる。さらに、原速度値演算部２は、駆動装置９６により発生可能（第一制約条件を満たす）であり、かつ、第二制約条件の許容速度を越えない速度の最大値Ｖｍａｘを一定の原速度値Ｖ０とする。

【0024】

以降では、移動距離Ｌが２００［ｍｍ］、演算周期ＴＳの周期時間ｔｓが１［ｍｓ］、最大値Ｖｍａｘが１０［ｍｍ／ｍｓ］である場合を想定して説明を進める。実際の作業ロボット９では、上記の想定と相違する動作条件等が用いられる。上記の想定による場合、可動アーム９２が１演算周期ＴＳごとに移動し得る最大の移動距離Ｌ０は、次式で求められ、１０［ｍｍ］となる。
移動距離Ｌ０＝Ｖｍａｘ・ｔｓ＝１０×１［ｍｍ］

【0025】

また、可動アーム９２が移動距離Ｌの移動動作に要する最小の周期数ｎは、次式で求められ、２０［周期］となる。
周期数ｎ＝Ｌ／Ｌ０＝２００÷１０［周期］

【0026】

原速度値演算部２は、上記の演算結果に基づいて、２０周期の演算周期ＴＳにわたり原速度値Ｖ０＝１０［ｍｍ／ｍｓ］を指令値演算部３に出力する。また、原速度値演算部２は、当該の２０周期を除いた前後の演算周期ＴＳでは、原速度値Ｖ０＝０を指令値演算部３に出力する。原速度値演算部２が出力する原速度値Ｖ０は、異常対応部５によって変更可能とされている。

【0027】

指令値演算部３は、通常時平均化母数ＮＮを用いて演算周期ＴＳごとに原速度値Ｖ０に移動平均処理を施すことにより、駆動制御部９５に指令する演算周期ＴＳごとの速度の指令値ＣＶを演算する。ここで、通常時平均化母数ＮＮは、可動アーム９２の加速度が第一制約条件の最大加速度以下となり、かつ、第二制約条件の許容加速度を越えないように設定される。

【0028】

例えば、条件を満たす最大の加速度が１（以降では単位を省略する）である場合、停止していた可動アーム９２は、演算周期ＴＳごとに１ずつ加速することができ、１０周期後に最大値Ｖｍａｘに到達する。したがって、通常時平均化母数ＮＮは、１０周期に相当する１０個に設定される。仮に、通常時平均化母数ＮＮが１０個未満であると、加速度が１を超過するため、第一および第二制約条件の少なくとも一方が満たされなくなる。また、通常時平均化母数ＮＮが１０個を超えていると、加速度が１未満となる。これにより、可動アーム９２の加速性能が低下して、移動動作が冗長に長時間化する。

【0029】

指令値演算部３は、例えばＤＳＰ（Digital Signal Processor）と呼ばれる専用の演算素子を用いて構成される。これにより、指令値演算部３は、高速の演算処理が可能となっている。指令値演算部３は、バッファ６、原速度値移動部３１、および平均演算部３２で構成される。

【0030】

バッファ６は、図３に示されるように、通常時平均化母数ＮＮに等しい１０個の第１サンプルエリアＡ１～第１０サンプルエリアＡ１０をもつ。バッファ６は、時系列的に並んだ１０個の原速度値Ｖ０を第１サンプルエリアＡ１～第１０サンプルエリアＡ１０に記憶する。第１サンプルエリアＡ１は、最も新しい原速度値Ｖ０を記憶し、第１０サンプルエリアＡ１０は、最も旧い原速度値Ｖ０を記憶する。バッファ６に記憶された原速度値Ｖ０の各々は、異常対応部５によって変更可能とされている。

【0031】

原速度値移動部３１は、バッファ６内の原速度値Ｖ０を移動させる移動処理を行う。詳述すると、原速度値移動部３１は、演算周期ＴＳの更新に伴い、第１サンプルエリアＡ１～第９サンプルエリアＡ９の各々に記憶された原速度値Ｖ０を旧い側の第２サンプルエリアＡ２～第１０サンプルエリアＡ１０へ順送りに移動させる（図３の矢印参照）。このとき、第１０サンプルエリアＡ１０に記憶されていた原速度値Ｖ０は不要となる。さらに、原速度値移動部３１は、もっとも新しい側の第１サンプルエリアＡ１に現在の原速度値Ｖ０を記憶させる。

【0032】

平均演算部３２は、演算周期ＴＳの各々において、第１サンプルエリアＡ１～第１０サンプルエリアＡ１０に記憶されている記憶内容（１０個の原速度値Ｖ０）を加算して、通常時平均化母数ＮＮ（＝１０）で除算することにより指令値ＣＶを演算する。平均演算部３２は、さらに、演算した指令値ＣＶを駆動制御部９５に出力する。平均演算部３２が１０個の記憶内容を加算する機能は、変更することができない。一方、通常時平均化母数ＮＮは、異常対応部５によって変更可能とされている。

【0033】

異常判定部４は、可動アーム９２が移動動作を行うことによって障害の発生が予想される異常状態を判定する。障害として、可動アーム９２が障害物に衝突したり、可動アーム９２が設定された移動可能範囲の境界を越えたり、エンドエフェクタ９３が作業動作を行えない作業不能領域に移動したりする場合があり、これらに限定されない。

【0034】

異常判定部４は、上記した障害物や移動可能範囲、作業不能領域の情報を予め記憶している。かつ、異常判定部４は、可動アーム９２の移動に関する情報がリアルタイムで入力される。したがって、異常判定部４は、障害の発生を予想することができる。異常判定部４は、ある演算周期ＴＳにおいて異常状態を判定した場合に、当該の演算周期ＴＳの中で異常判定信号ＡＢを異常対応部５に出力する。

【0035】

なお、異常判定部４は、別途設けられた異常検出部からの検出信号に基づいて異常状態を判定するようにしてもよい。例えば、可動アーム９２のエンドエフェクタ９３の近くに設けた監視カメラにより可動アーム９２の移動前方の障害物を検出し、検出信号を異常判定部４に送信するように構成することができる。これによれば、オペレータの手などの動き得る障害物への対応が可能となる。

【0036】

異常対応部５は、通常時には休止しており、異常判定信号ＡＢを受け取ると動作を開始する。異常対応部５は、異常状態が判定されたときに、以降の原速度値Ｖ０を強制的に０とするように原速度値演算部２を制御する。また、異常対応部５は、通常時平均化母数ＮＮよりも小さな異常時平均化母数ＮＡを用いて指令値ＣＶを演算するように指令値演算部３を制御する。

【0037】

ここで、異常対応部５は、通常時平均化母数ＮＮに予め設定された減少割合Ｒを乗算して異常時平均化母数ＮＡを求める。減少割合Ｒは、可動アーム９２の減速度が制動装置９７により発生可能な最大減速度以下となり（第一制約条件を満たす）、かつ、第二制約条件の許容減速度を越えない最小値に設定される。

【0038】

例えば、条件を満たす最大の減速度が２である場合、最大値Ｖｍａｘ（＝１０）で移動していた可動アーム９２は、演算周期ＴＳごとに２ずつ減速して、５周期後に停止することができる。したがって、減少割合Ｒは、減速するときに要する５周期と、前述した加速するときに要する１０周期との割合で求められ、０．５に設定される（Ｒ＝０．５）。これによれば、異常時平均化母数ＮＡは、次式で求められ、５個となる。
異常時平均化母数ＮＡ＝ＮＮ・Ｒ＝１０×０．５［個］

【0039】

仮に、減少割合Ｒが最小値の０．５未満であると、異常時平均化母数ＮＡが５個未満となって減速度が２を超過するため、第一および第二制約条件の少なくとも一方が満たされなくなる。また、減少割合Ｒが最小値の０．５を超えていると、異常時平均化母数ＮＡが５個を越えて減速度が２より小さくなる。これにより、可動アーム９２の減速性能が低下し、停止に要する制動距離が冗長に長くなる。

【0040】

異常対応部５は、求めた異常時平均化母数ＮＡ（＝５）を平均演算部３２に指令して、通常時平均化母数ＮＮを異常時平均化母数ＮＡに変更する。また、異常対応部５は、異常判定信号ＡＢを受け取ると、バッファ６に対して所定の異常時セット操作を行う。異常時セット操作の詳細については、後述する異常時対応の動作の中で説明する。

【0041】

３．速度指令値演算装置１の通常時の動作
次に、速度指令値演算装置１の通常時の動作について、図４、図６、および図７を参考にして説明する。図６において（図８、図１０も同様）、横軸は演算周期ＴＳを表し、縦軸は速度を表している。また、破線のグラフは、原速度値演算部２が出力した原速度値Ｖ０（実際には演算周期ＴＳごとの離散量）を示し、黒丸および実線のグラフは、指令値演算部３が出力した指令値ＣＶ、および指令値ＣＶを結んだ変化の様子を示している。また、図７において（図９、図１１も同様）、各演算周期ＴＳにおけるバッファ６の第１サンプルエリアＡ１～第１０サンプルエリアＡ１０に記憶された原速度値Ｖ０、および指令値ＣＶが一覧表の形式で示されている。

【0042】

図４に示された動作フローのステップＳ１で、速度指令値演算装置１は、起動時に初期設定を行う。すなわち、速度指令値演算装置１は、通常時平均化母数ＮＮ＝１０個、減少割合Ｒ＝０．５、および原速度値Ｖ０＝０を初期設定する。なお、これらの量がデフォルト値として保存され、ステップＳ１の初期設定が省略されてもよい。

【0043】

次のステップＳ２で、原速度値演算部２は、移動動作の要求が有るか否かを判定する。要求が無い場合、原速度値演算部２は、動作フローの実行をステップＳ４に進める。また、要求が有る場合、原速度値演算部２は、動作フローの実行をステップＳ３に進めて、原速度値Ｖ０を演算する。以降では、第１演算周期ＴＳ１で前記した移動距離Ｌ＝２００の移動動作の要求が発生した場合を想定し、説明を進める。

【0044】

原速度値演算部２は、第１演算周期ＴＳ１のステップＳ３で、２０周期分の原速度値Ｖ０＝１０、およびその後の原速度値Ｖ０＝０を一度に演算することができる。原速度値演算部２は、図６に示されるように、第１演算周期ＴＳ１およびその後の演算周期ＴＳで、演算済みの原速度値Ｖ０を出力する。この後、動作フローはステップＳ４に合流される。

【0045】

ステップＳ４で、原速度値移動部３１は、前述したバッファ６内の移動処理を行う。移動動作の要求が無い第１演算周期ＴＳ１以前において、原速度値Ｖ０は０であり、第１サンプルエリアＡ１～第１０サンプルエリアＡ１０の各々に０が記憶された状態が継続する。原速度値移動部３１は、第１演算周期ＴＳ１において、原速度値演算部２から入力された原速度値Ｖ０＝１０を第１サンプルエリアＡ１に記憶させる。このとき、第２サンプルエリアＡ２～第１０サンプルエリアＡ１０に記憶された原速度値Ｖ０は０である。この状態は、図７の第１演算周期ＴＳ１の段に示されている。

【0046】

次のステップＳ５で、平均演算部３２は、第１サンプルエリアＡ１～第１０サンプルエリアＡ１０に記憶されている１０個の原速度値Ｖ０を加算して求めた１０を、通常時平均化母数ＮＮ（＝１０）で除算することにより指令値ＣＶ＝１を演算する。次のステップＳ６で、平均演算部３２は、演算した指令値ＣＶ＝１を駆動制御部９５に出力する。

【0047】

次のステップＳ７で、速度指令値演算装置１は、出力した指令値ＣＶが０であるか否かを判定する。ここで、指令値ＣＶが０であることは、可動アーム９２の移動動作が終了していることを意味する。第１演算周期ＴＳ１において、指令値ＣＶは０でないので、動作フローの実行はステップＳ８に進められる。ステップＳ８で、異常判定部４は、異常状態であるか否かを判定する。多くの場合に異常状態ではなく、動作フローはステップＳ４に戻され、演算周期ＴＳが更新される。

【0048】

第２演算周期ＴＳ２のステップＳ４で、原速度値移動部３１は、第１サンプルエリアＡ１の原速度値Ｖ０＝１０を第２サンプルエリアＡ２に移動するとともに、原速度値演算部２から入力された原速度値Ｖ０＝１０を第１サンプルエリアＡ１に記憶させる。このとき、第３サンプルエリアＡ３～第１０サンプルエリアＡ１０に記憶された原速度値Ｖ０は０である。この状態は、図７の第２演算周期ＴＳ２の段に示されている。

【0049】

次のステップＳ５で、平均演算部３２は、第１サンプルエリアＡ１～第１０サンプルエリアＡ１０に記憶されている１０個の原速度値Ｖ０を加算して求めた２０を、通常時平均化母数ＮＮ（＝１０）で除算することにより指令値ＣＶ＝２を演算する。次のステップＳ６で、平均演算部３２は、演算した指令値ＣＶ＝２を駆動制御部９５に出力する。この後、動作フローは、ステップＳ７およびステップＳ８を経由してステップＳ４戻され、演算周期ＴＳが更新される。

【0050】

ステップＳ４～ステップＳ８で構成された繰り返しループが１０回繰り返されると、第１０演算周期ＴＳ１０に至る。この間、指令値ＣＶは、演算周期ＴＳごとに１ずつ増加する。第１０演算周期ＴＳ１０において、第１サンプルエリアＡ１～第１０サンプルエリアＡ１０の各々には、原速度値Ｖ０＝１０が記憶されている。平均演算部３２は、ステップＳ５で指令値ＣＶ＝１０を演算し、ステップＳ６で指令値ＣＶ＝１０を駆動制御部９５に出力する。この後、第２０演算周期ＴＳ２０まで、同じ状態が維持される。

【0051】

第２１演算周期ＴＳ２１のステップＳ４で、原速度値移動部３１は、原速度値演算部２から入力された原速度値Ｖ０＝０を第１サンプルエリアＡ１に記憶させる。このとき、第２サンプルエリアＡ２～第１０サンプルエリアＡ１０に記憶された原速度値Ｖ０は１０である。平均演算部３２は、ステップＳ５で指令値ＣＶ＝９を演算し、ステップＳ６で指令値ＣＶ＝９を駆動制御部９５に出力する。この後の第３０演算周期ＴＳ３０まで、指令値ＣＶは、演算周期ＴＳごとに１ずつ減少する。

【0052】

第３０演算周期ＴＳ３０において、指令値ＣＶは０であるので、動作フローの実行はステップＳ７からステップＳ２に戻され、演算周期ＴＳが更新される。これで、可動アーム９２の通常時の移動動作が終了し、次の移動動作の要求を受け付け可能な状態に戻る。図６に示されるように、可動アーム９２が減速を開始してから停止するまでの停止動作に、１０周期の停止時間を要する。また、停止動作に要する制動距離は、図６の第２０演算周期ＴＳ２０から第３０演算周期ＴＳ３０までの指令値ＣＶの面積で求められ、５０である。

【0053】

要求された移動距離Ｌが２００よりも長い場合、指令値ＣＶ＝１０を維持する周期数が増加する。この場合、停止動作に要する停止時間および制動距離は、上記の場合と変わらない。要求された移動距離Ｌが２００よりも短い場合、指令値ＣＶ＝１０を維持する周期数が減少し、あるいは、指令値ＣＶが１０に達する以前に減速が始まる。

【0054】

４．速度指令値演算装置１の異常時対応の動作
次に、速度指令値演算装置１の異常時対応の動作について、図５、図８、および図９を参考にして説明する。図４のステップＳ８で、異常判定部４が異常状態を判定していると、動作フローはステップＳ９に進められる。ステップＳ９で、速度指令値演算装置１は、異常時対応を実施する。異常時対応の動作フローの詳細は、図５に示されている。以降では、図８および図９に例示されるように、第１２演算周期ＴＳ１２で異常状態が判定された場合について説明する。

【0055】

図５のステップＳ１１で、異常対応部５は、前述したように異常時平均化母数ＮＡ（＝５）を求めて、平均演算部３２に指令する。平均演算部３２は、通常時平均化母数ＮＮを異常時平均化母数ＮＡに変更して動作を継続する。次のステップＳ１２で、異常対応部５は、以降の原速度値Ｖ０を強制的に０とするように原速度値演算部２を制御する。

【0056】

次のステップＳ１３で、異常対応部５は、バッファ６の異常時セット操作を行う。詳述すると、異常対応部５は、異常状態が判定された第１２演算周期ＴＳ１２以降の演算周期ＴＳで、第６サンプルエリアＡ６～第１０サンプルエリアＡ１０に強制的に０を記憶させる。なお、第６サンプルエリアＡ６～第１０サンプルエリアＡ１０は、通常時平均化母数ＮＮ（＝１０）から異常時平均化母数ＮＡ（＝５）を減算した５個の旧い側として決定される。これによれば、図９に示されるように、第１３演算周期ＴＳ１３以降の演算周期ＴＳにおける第６サンプルエリアＡ６～第１０サンプルエリアＡ１０の記憶内容は、すべて０となる。仮に、異常時平均化母数ＮＡが８個の場合、異常対応部５は、旧い側の２個の第９サンプルエリアＡ９および第１０サンプルエリアＡ１０に強制的に０を記憶させることになる（第２実施形態で説明する図１４参照。）

【0057】

このセット操作は、第６サンプルエリアＡ６～第１０サンプルエリアＡ１０を実質的に未使用状態とするものである。これによれば、平均演算部３２は、１０個のサンプルエリアの記憶内容を加算する機能を変更することができなくとも、異常時平均化母数ＮＡ（＝５）による５個の移動平均処理を正しく行うことができる。

【0058】

なお、原速度値移動部３１および平均演算部３２が使用するサンプルエリアの個数を変更できる構成も考えられる。この構成では、異常対応部５は、原速度値移動部３１および平均演算部３２が異常時平均化母数ＮＡに等しい５個の新しい側の第１サンプルエリアＡ１～第５サンプルエリアＡ５のみを用いるように制御すればよい。

【0059】

さらに、異常対応部５は、異常状態が判定された第１２演算周期ＴＳ１２で、異常時平均化母数ＮＡに等しい５個の新しい側の第１サンプルエリアＡ１～第５サンプルエリアＡ５に現在の指令値ＣＶ＝１０を記憶させる。仮に、第７演算周期ＴＳ７で異常状態が判定された場合、異常対応部５は、第１サンプルエリアＡ１～第５サンプルエリアＡ５に現在の指令値ＣＶ＝７を記憶させる。このセット操作は、平均化母数を変更したときに生じがちな指令値ＣＶの急峻な変化（実現不可能な変化）を回避するためのものである。

【0060】

この後、演算周期ＴＳが更新され、動作フローの実行はステップＳ１４に進められる。ステップＳ１４～ステップＳ１７における動作内容は、通常時のステップＳ４～ステップＳ７と同様であるが、異常時平均化母数ＮＡを用いる点が相違する。

【0061】

第１３演算周期ＴＳ１３のステップＳ１４で、原速度値移動部３１は、第１サンプルエリアＡ１～第４サンプルエリアＡ４の指令値ＣＶ＝１０を、第２サンプルエリアＡ２～第５サンプルエリアＡ５へ順送りに移動させる。さらに、原速度値移動部３１は、原速度値演算部２から入力された原速度値Ｖ０＝０を第１サンプルエリアＡ１に記憶させる。この状態は、図９の第１３演算周期ＴＳ１３の段に示されている。

【0062】

矢印Ｍ１に示されるように、第１２演算周期ＴＳ１２の指令値ＣＶ＝１０は、第１３演算周期ＴＳ１３の第２サンプルエリアＡ２～第５サンプルエリアＡ５に残る。これによれば、第１３演算周期ＴＳ１３の指令値ＣＶは、第１２演算周期ＴＳ１２の指令値ＣＶから大きく変化することがない。したがって、指令値ＣＶの急峻な変化が回避される。

【0063】

次のステップＳ１５で、平均演算部３２は、指令値ＣＶ＝８を演算する。次のステップＳ１６で、平均演算部３２は、演算した指令値ＣＶ＝８を駆動制御部９５に出力する。次のステップ１７で、指令値ＣＶが０でないことから、動作フローはステップＳ１４に戻されて、演算周期ＴＳが更新される。この後、ステップＳ１４～ステップＳ１７で構成された繰り返しループが繰り返される。指令値ＣＶは、第１２演算周期ＴＳ１２において１０であり、演算周期ＴＳが更新されるたびに２ずつ減少して、第１７演算周期において０となる。

【0064】

これで異常時対応が終了し、動作フローはステップＳ１８に進められる。ステップＳ１８で、異常対応部５は、平均演算部３２の異常時平均化母数ＮＡを通常時平均化母数ＮＮに戻すとともに、異常時セット操作を解消する。この後、動作フローの実行は、図４のステップＳ９を経由してステップＳ２に戻され、次の移動動作の要求を受け付け可能な状態に戻る。

【0065】

異常時対応において、可動アーム９２が減速を開始してから停止するまでの緊急停止動作に５周期の停止時間を要する。また、緊急停止動作に要する制動距離は、図８の第１２演算周期ＴＳ１２から第１７演算周期ＴＳ１７までの指令値ＣＶの面積で求められ、２５である。

【0066】

５．従来技術の速度指令値演算装置との対比
次に、従来技術の速度指令値演算装置の構成および動作について説明する、従来技術の速度指令値演算装置は、実施形態と同じ原速度値演算部２、指令値演算部３、および異常判定部４を備え、異常対応部の機能のみが相違する。従来技術の異常対応部は、異常状態が判定されたとき、実施形態と同様に以降の原速度値Ｖ０を強制的に０とするが、通常時平均化母数ＮＮの変更やバッファ６の異常時セット操作は行わない。

【0067】

したがって、従来技術の速度指令値演算装置において、可動アーム９２の緊急停止動作は、通常時と同様に１０周期を要する。この結果、図１０および図１１に示されるように、第１２演算周期ＴＳ１２で異常状態が判定された場合、可動アーム９２の停止は、第２２演算周期ＴＳ２２となる。このため、従来技術の速度指令値演算装置は、異常時対応の緊急停止動作であっても、通常時と同じ制動距離（＝５０）が必要となる。これに対比して、実施形態の速度指令値演算装置１では、緊急停止動作の停止時間（＝５周期）および制動距離（＝２５）が半分で済む。

【0068】

第１実施形態の速度指令値演算装置１において、異常対応部５は、異常状態が判定されたときに、原速度値Ｖ０をゼロに制御するとともに、通常時平均化母数ＮＮよりも小さな異常時平均化母数ＮＡを用いて指令値ＣＶを演算させる。これによれば、現在以降の原速度値Ｖ０がゼロとなるため、可動アーム９２（可動部）は、直ちに減速を開始する。かつ、現在以降は小さな異常時平均化母数ＮＡが用いられるため、可動アーム９２は、通常時よりも大きな減速度で停止に至る。この結果、可動アーム９２は、異常状態発生時の緊急停止動作において通常時よりも小さな制動距離で停止することができ、障害の実際の発生が抑制される

【0069】

また、異常時平均化母数ＮＡは第一制約条件が許す範囲内で設定されるので、駆動装置９６および制動装置９７に過大な負担をかけることが無く、故障要因を低減することができる。さらに、異常時平均化母数ＮＡは、第二制約条件が許す範囲内で設定されるので、異常時対応において可動アーム９２の移動動作に弊害が発生しない。

【0070】

６．第２実施形態の速度指令値演算装置１Ａ
次に、第２実施形態の速度指令値演算装置１Ａの構成および動作について、図１２～図１４を参考にして、第１実施形態と異なる点を主に説明する。第２実施形態において、異常判定部４が異常検出部９８を有する点が第１実施形態と異なり、かつ、異常対応部５の動作が第１実施形態と異なる。

【0071】

異常検出部９８は、異常状態の判定に有効な可動アーム９２の現在状態を検出する。異常検出部９８は、例えば、可動アーム９２のエンドエフェクタ９３の近くに設けられた監視カメラや近接センサなどによって構成され、可動アーム９２の移動前方の障害物を検出する。異常検出部９８は、障害物の有無に関する信号、および、可動アーム９２の現在位置と障害物との離間距離ＤＸに関する信号を異常判定部４に出力する。

【0072】

異常判定部４は、異常検出部９８から受け取った信号に基づいて、異常状態を判定するとともに、前記した離間距離ＤＸを演算する。離間距離ＤＸの演算方法として、監視カメラが取得した画像データの画像処理や、近接センサの出力信号の距離換算などを例示することができる。異常判定部４は、第１実施形態と同様の異常判定信号ＡＢに加えて、離間距離ＤＸを表す信号を異常対応部５に出力する。

【0073】

異常対応部５は、第１実施形態と同様、異常判定信号ＡＢを受け取ると動作を開始する。しかしながら、第１実施形態と異なり、減少割合Ｒは、予め設定されていない可変量である。異常対応部５は、離間距離ＤＸを用いた演算により減少割合Ｒを求める。さらに、異常対応部５は、求めた減少割合Ｒと通常時平均化母数ＮＮを乗算して、異常時平均化母数ＮＢを求める。第２実施形態において、減少割合Ｒの値は、１を超過したり、第１実施形態で説明した最小値の０．５より小さくなったりする場合が生じ得る。

【0074】

演算方法について詳述すると、異常対応部５は、まず、通常時平均化母数ＮＮを用いて演算された指令値ＣＶにしたがうと仮定した場合に、可動アーム９２が停止するまでに要する現在位置からの制動距離を求める。可動アーム９２が最大値Ｖｍａｘ（＝１０）で移動しているときの制動距離は、第１実施形態で説明した５０である。可動アーム９２が最大値Ｖｍａｘよりも小さな速度で移動している場合、制動距離は、５０よりも小さくなるが、演算によって求めることが可能である。次に、異常対応部５は、次式を用いて減少割合Ｒを演算する。
減少割合Ｒ＝ＤＸ／ＤＳ

【0075】

さらに、異常対応部５は、次式を用いて異常時平均化母数ＮＢを演算する。なお、異常時平均化母数ＮＢに１未満の端数が生じた場合、切り捨てによって整数の異常時平均化母数ＮＢを求める。
異常時平均化母数ＮＢ＝ＮＮ・Ｒ
以降では、求められた異常時平均化母数ＮＢの大きさに基づき、１）～３）に場合分けして説明する。

【0076】

１）異常時平均化母数ＮＢが通常時平均化母数ＮＮよりも大きい場合
（ＮＢ＞ＮＮ＝１０）
この場合、減少割合Ｒは１を超過しており、仮定から求めた制動距離は、離間距離ＤＸよりも小さい。つまり、可動アーム９２は、通常時平均化母数ＮＮを用いて演算された指令値ＣＶにしたがって移動しても、障害物に衝突しない。したがって、異常対応部５は、以降の原速度値Ｖ０を強制的に０とするように原速度値演算部２を制御し、指令値演算部３を制御しない。指令値演算部３は、通常時平均化母数ＮＮを用いた移動平均処理を継続する。

【0077】

２）異常時平均化母数ＮＢが通常時平均化母数ＮＮ以下で、かつ第１実施形態で説明した第一および第二制約条件を満たす場合（ＮＮ＝１０≧ＮＢ≧５）
この場合、減少割合Ｒは、概ね０．５以上で１未満の範囲内にある。つまり、可動アーム９２は、通常時平均化母数ＮＮを用いて演算された指令値ＣＶにしたがうと障害物に衝突するおそれがある。それでも、指令値演算部３が異常時平均化母数ＮＢを用いることにより、可動アーム９２は、衝突を回避することができる。したがって、異常対応部５は、以降の原速度値Ｖ０を強制的に０とするように原速度値演算部２を制御するとともに、異常時平均化母数ＮＢを平均演算部３２に指令し、バッファ６に対して所定の異常時セット操作を行う。

【0078】

例えば、第１実施形態と同じく第１２演算周期ＴＳ１２で異常状態が判定され、離間距離ＤＸ＝４２、制動距離＝５０の場合、減少割合Ｒ＝０．８４となり、異常時平均化母数ＮＢは８個となる。この場合の速度指令値演算装置１Ａの動作は、図１３および図１４に示されている。

【0079】

図１４に示されるように、第１３演算周期ＴＳ１３以降の演算周期ＴＳにおける第９サンプルエリアＡ９および第１０サンプルエリアＡ１０の記憶内容は、すべて０となる。また、矢印Ｍ２に示されるように、第１２演算周期ＴＳ１２の指令値ＣＶ＝１０は、第１３演算周期ＴＳ１３の第２サンプルエリアＡ２～第８サンプルエリアＡ８に残る。したがって、第１３演算周期ＴＳ１３の指令値ＣＶは、第１２演算周期ＴＳ１２の指令値ＣＶから大きく変化しない。かつ、第１３演算周期ＴＳ１３の指令値ＣＶは８．７５であり、通常時よりも大きな減速度となる。指令値ＣＶは、第１２演算周期ＴＳ１２において１０であり、演算周期ＴＳが更新されるたびに１．２５ずつ減少して、第２０演算周期において０となる。

【0080】

可動アーム９２が減速を開始してから停止するまでに要する制動距離は、図１３の第１２演算周期ＴＳ１２から第２０演算周期ＴＳ２０までの指令値ＣＶの面積で求められ、４０である。つまり、可動アーム９２は、離間距離ＤＸ（＝４２）よりもわずかに小さな制動距離（＝４０）で障害物の少し手前に停止し、障害物への衝突を回避することができる。加えて、減速度を必要以上に大きくしないので、許容される最大の減速度で常に緊急停止動作を行う第１実施形態と比較して、可動アーム９２や制動装置９７に大きな負担をかけない優しい緊急停止動作を実現することができる。

【0081】

３）異常時平均化母数ＮＢが第１実施形態で説明した第一および第二制約条件を満たさない場合（５＞ＮＢ）
この場合、減少割合Ｒは、０．５よりも小さい。つまり、可動アーム９２は、第一および第二制約条件を満たす異常時平均化母数ＮＢを用いて演算された指令値ＣＶにしたがう限り、障害物への衝突を回避することができない。異常対応部５は、以降の原速度値Ｖ０を強制的に０とするように原速度値演算部２を制御するとともに、第一および第二制約条件を満たす最小の異常時平均化母数ＮＢ（＝５）を平均演算部３２に指令し、バッファ６に対して所定の異常時セット操作を行う。

【0082】

この異常時対応によれば、可動アーム９２は、第１実施形態で説明したように制動距離ＤＳ＝２５で停止する。したがって、可動アーム９２が障害物に衝突する瞬間の速度は、第一および第二制約条件が許す範囲内で最小となり、衝突時のショックが最小に緩和される。

【0083】

第２実施形態の速度指令値演算装置１Ａでは、第１実施形態と同様、可動アーム９２は、異常状態発生時の緊急停止動作において通常時よりも小さな制動距離で停止することができ、障害の実際の発生が抑制される。加えて、離間距離ＤＸの大小に合わせて異常時平均化母数ＮＢを可変にかつ適正に設定することができるので、可動アーム９２や制動装置９７に大きな負担をかけない優しい緊急停止動作を実現することができる。

【0084】

７．実施形態の応用および変形
なお、第１実施形態で例示した演算周期ＴＳの周期時間ｔｓ（＝１ｍｓ）は、実機ではさらに小さく設定され、通常時平均化母数ＮＮ（＝１０個）は実機ではさらに大きく設定されて、精細な移動制御が行われる。また、第１実施形態において、指令値演算部３は、専用の演算素子を用いて構成されているが、速度指令値演算装置１を構成するコンピュータ装置のメモリおよびプログラムを用いて構成されてもよい。さらに、第２実施形態は、可動アーム９２の移動可能範囲が物理的な境界で区画された構成に適用することができる。この構成において、異常検出部９８は、可動アーム９２の移動前方の物理的な境界を検出する。第１および第２実施形態は、その他にも様々な応用や変形が可能である。

【産業上の利用可能性】

【0085】

第１および第２実施形態の速度指令値演算装置（１、１Ａ）は、作業ロボット９に限定されず、移動平均処理を用いて速度の指令値を演算する各種の機器に利用することが可能である。

【符号の説明】

【0086】

１、１Ａ：速度指令値演算装置 ２：原速度値演算部 ３：指令値演算部 ３１：原速度値移動部 ３２：平均演算部 ４：異常判定部 ５：異常対応部 ６：バッファ ９：作業ロボット ９１：基部 ９２：可動アーム ９３：エンドエフェクタ ９４：駆動機構 ９５：駆動制御部 ９６：駆動装置 ９７：制動装置 Ａ１～Ａ１０：第１～第１０サンプルエリア Ｌ：移動距離 ＮＮ：通常時平均化母数 ＮＡ：異常時平均化母数 Ｒ：減少割合 Ｖ０：原速度値 Ｖｍａｘ：（速度の）最大値 ＣＶ：（速度の）指令値 ＴＳ：演算周期

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】