JP2017213631A - ロボットアーム制御装置、ロボットアーム制御方法、およびプログラム - Google Patents

Abstract

【課題】ロボットの仕様等を満足するロボットアームの動作軌道を生成する制御装置の提供。【解決手段】制御装置１は、関節を有するロボットアーム２０の目標状態を入力する入力部１０と、ロボットアームの関節の最大駆動角度および関節の最大駆動トルクの仕様データを記憶した仕様データ記憶部１５と、ロボットアームの関節角軌道を表す関数であって、複数の基底関数にパラメータによる重み付けをして線形結合した関節角軌道関数を記憶した関節角軌道関数記憶部１３と、関節角軌道に対して所定のコストを与えるコスト関数記憶部１６と、目標状態と関節の駆動角度が最大駆動角度以下かつ関節の駆動トルクが最大駆動トルク以下であることとを制約条件として、コスト関数を最小化する関節角軌道関数のパラメータを求める最適解計算部１１と、最適解計算部１１にて求めたパラメータに対応する制御命令をロボットアーム２０に対して出力する出力部１２とを備える。【選択図】図２

Description

本発明は、ロボットアームの動作軌道を生成する制御装置に関する。

従来から、工場においては、部品の組み立てなどを行うロボットが導入されている。こうした産業用のロボットは、同じ動作を繰り返し行うロボットである。ロボットがどのように動作すればよいかを、専門技術者があらかじめロボットの動作を手動設計することが一般的である。このような動作設計は、「ティーチング」と呼ばれており、例えば、特許文献１は、効率的にティーチングを行う方法を開示している。

近年では、例えば、会社の受付けや、介護やリハビリの支援といった生活、医療、福祉等の分野にロボットの用途が拡大している。こうしたロボットでは、専門知識のないユーザが、ティーチングによってロボットの動作を設計することは困難である。

特開２００３−２３６７８４号公報

ユーザが目標の状態を指示するだけで、目標の状態までの動きをロボットが自動決定するようにできれば、専門知識のないユーザにとって便利である。

ここで、ロボットアームの関節の動きを例として、ロボットアームの動作計画を求める従来法について説明する。まず、ロボットアームの関節角度、関節の角速度、駆動トルクの変化を離散化して微小時間区間毎に変数とする。そして、各変数が全ての時刻で運動方程式を満たすこと（ある時刻での関節角度、角速度、駆動トルクと、次の時刻での関節角度、角速度、駆動トルクとの関係が、物理法則から外れていない値になっていないか等）と、各変数がハードウェアの要件を満たすこと（関節角度は所定値以上にならないこと、所定値以上の駆動トルクは出せないこと等）を制約条件として、ロボットアームの軌道を求める。

しかし、この方法だと、時間方向に離散化して得られた膨大な変数が運動方程式を満たす必要があるため、現実的な計算量では解けない。

本発明は、ハードウェアの仕様等を満足するロボットアームの動作軌道を生成し、当該軌道にしたがって制御を行うロボットアーム制御装置を提供することを目的とする。

本発明のロボットアームの制御装置は、関節を有するロボットアームの目標状態を入力する入力部と、前記ロボットアームの関節の最大駆動角度および関節の最大駆動トルクの仕様データを記憶した仕様データ記憶部と、前記ロボットアームの関節角軌道を表す関数であって、複数の基底関数にパラメータによる重み付けをして線形結合した形式を有する関節角軌道関数を記憶した関節角軌道関数記憶部と、前記関節角軌道に対して所定の制御方針に従ってコストを与えるコスト関数を記憶したコスト関数記憶部と、前記目標状態と、関節の駆動角度が前記最大駆動角度以下であることと、関節の駆動トルクが最大駆動トルク以下であることとを制約条件として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求める最適解計算部と、前記最適解計算部にて求めたパラメータに対応する制御命令をロボットアームに対して出力する出力部とを備える。

このようにロボットアームの関節角軌道を、複数の基底関数にパラメータによる重み付けをして線形結合した形式を有する関数で表し、コストを最小化する関節角軌道のパラメータを所定の制約条件下で求める構成により、パラメータ数を大幅に削減して、現実的な計算量で関節角軌道を求めることができる。関節角軌道を表す関数は、任意の基底関数を線形結合したものなので、生成されるロボットアームの軌道の自由度が高い。

本発明のロボットアームの制御装置において、前記最適解計算部は、前記制御区間内の複数の離散点において制約条件を満たすパラメータを求めてもよい。この構成により、制約付きの最適化問題を適切に解くことができる。

本発明のロボットアームの制御装置において、前記仕様データ記憶部は、関節の角速度の最大角速度のデータを記憶しており、前記最適解計算部は、関節の角速度が前記最大角速度以下であることも制約条件として、前記パラメータを求めてもよい。

この構成により、例えば、安全性等の理由から、関節の角速度について制限がある場合に、条件を満たす適切な軌道を求めることができる。

本発明のロボットアームの制御装置において、前記入力部は、前記ロボットアームの初期状態の入力を受け付け、前記最適解計算部は、前記ロボットアームの初期状態も制約条件として、前記パラメータを求めてもよい。この構成により、前記ロボットアームの初期状態をも満たす軌道を求めることができる。

本発明のロボットアームの制御装置は、前記関節の駆動トルクを除く制約条件の下で、前記ロボットアームを前記目標状態へ最短時間で遷移させるパラメータを計算する最短動作時間計算部を備え、前記最適解計算部は、前記最短動作時間計算部にて求めたパラメータを初期値として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求めてもよい。

関節の駆動トルクに関する制約条件は非凸制約であるため、コスト関数の最適解の探索に失敗する現象が発生し得る。これに対し、上に挙げた駆動トルク以外の制約条件は凸制約なので、駆動トルク以外の制約条件の下で、ロボットアームを目標状態へ最短時間で遷移させるパラメータを求めることができる。求めたパラメータを初期値として最適解計算部による計算処理を行うことで、関節角軌道関数のパラメータの最適解が得られる可能性を高めることができる。

本発明のロボットアーム制御装置は、前記最短動作時間計算部で求めた関節角軌道が、駆動トルクの制約条件を満たすか否かを判定し、駆動トルクの制約条件を満たすと判定された場合には、前記最適解計算部による処理を行わないで前記最短動作時間計算部で求めたパラメータを最適解とし、駆動トルクの制約条件を満たさないと判定された場合には、前記最適解計算部による処理を行ってもよい。

このように最短動作時間計算部で求めたパラメータが駆動トルクの制約条件を満たすかチェックする構成により、最短動作時間計算部で求めたパラメータが駆動トルクの制約条件も満たす場合には、最適解計算部による計算処理を行わなくてもよいので、計算コストを大幅に低減できる。

本発明のロボットアーム制御方法は、関節を有するロボットアームを制御装置によって制御する方法であって、前記制御装置に、前記ロボットアームの目標状態を入力するステップと、前記制御装置が、仕様データ記憶部から、前記ロボットアームの関節の最大駆動角度および関節の最大駆動トルクの仕様データを読み出すステップと、前記制御装置が、関節角軌道関数記憶部から、前記ロボットアームの関節角軌道を表す関数であって、複数の基底関数にパラメータによる重み付けをして線形結合した形式を有する関節角軌道関数を読み出すステップと、前記制御装置が、コスト関数記憶部から、前記関節角軌道に対して所定の制御方針に従ってコストを与えるコスト関数を読み出すステップと、前記制御装置が、前記目標状態と、関節の駆動角度が前記最大駆動角度以下であることと、関節の駆動トルクが最大駆動トルク以下であることとを制約条件として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求める最適解計算ステップと、前記制御装置が、前記最適解計算ステップにて求めたパラメータに対応する制御命令をロボットアームに対して出力するステップとを備える。

本発明のプログラムは、関節を有するロボットアームを制御するためのプログラムであって、コンピュータに、前記ロボットアームの目標状態を入力するステップと、仕様データ記憶部から、前記ロボットアームの関節の最大駆動角度および関節の最大駆動トルクの仕様データを読み出すステップと、関節角軌道関数記憶部から、前記ロボットアームの関節角軌道を表す関数であって、複数の基底関数にパラメータによる重み付けをして線形結合した形式を有する関節角軌道関数を読み出すステップと、コスト関数記憶部から、前記関節角軌道に対して所定の制御方針に従ってコストを与えるコスト関数を読み出すステップと、前記目標状態と、関節の駆動角度が前記最大駆動角度以下であることと、関節の駆動トルクが最大駆動トルク以下であることとを制約条件として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求める最適解計算ステップと、前記最適解計算ステップにて求めたパラメータに対応する制御命令をロボットアームに対して出力するステップとを実行させる。

本発明によれば、現実的な計算量で、関節角や駆動トルク等の制約を満たすロボットアームの関節角軌道を求めることができる。

第１の実施の形態のロボットアームの外観を示す図である。 第１の実施の形態のロボットアーム制御装置の構成を示す図である。 第１の実施の形態のロボットアーム制御装置の動作を示す図である。 第２の実施の形態のロボットアーム制御装置の構成を示す図である。 第２の実施の形態のロボットアーム制御装置の動作を示す図である。

以下、本発明の実施の形態に係るロボットアーム制御装置について、図面を参照して説明する。
（第１の実施の形態）
図１は、本実施の形態のロボットアーム２０の外観を示す図である。ロボットアーム２０は、ロボットアーム２０を床面や机等の設置面に載置するためのベースユニット２１に、６つの関節を持つアーム２２が取り付けられている。各関節は所定の関節角の範囲で回転する。ロボットアーム２０の状態は、各関節の回転角度（ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５，ｑ_６）及び回転角速度

で規定することができる。

ロボットアーム２０のベースユニット部分には、ロボットアーム制御装置１（図２参照）が内蔵されている。ロボットアーム制御装置１は、ロボットアーム制御装置１は、ユーザからロボットアーム２０の初期状態と目標状態とが入力されると、初期状態から目標状態にロボットアーム２０を動かす最適な軌道を生成し、その軌道でロボットアーム２０を動かすための制御命令をロボットアーム２０の各関節の駆動モータに対して送信する。

図２は、ロボットアーム制御装置１の構成を示す図である。ロボットアーム制御装置１は、入力部１０と、最適解計算部１１と、出力部１２とを有している。これら各部の一部または全部は、ハードウェアで構成されてもよいし、コンピュータのプロセッサが所定のプログラムを実行することによって実現されてもよい。また、ロボットアーム制御装置１は、関節角軌道を表す関数を記憶した関節角軌道関数記憶部１３と、ロボットアーム２０の運動方程式を記憶した運動方程式記憶部１４と、ロボットアーム２０の仕様に関するデータを記憶した仕様データ記憶部１５と、ロボットアーム２０を動作させる際の制御方針に基づくコスト関数を記憶したコスト関数記憶部１６を有している。

関節角軌道関数ｑ（ｔ）は、下記の式（１）のように、基底関数ｂ（ｔ）にパラメータａ_ｉを乗じて線形結合した形式を有している。なお、基底関数ｂ（ｔ）は２階微分可能な関数を用いる。

この関節角軌道関数のパラメータａ_０，ａ_１，ａ_２，・・・ａ_ｎを求めることにより、関節角軌道関数を決定することができる。なお、ｑ（ｔ）は各関節の角度を示すベクトルであり、本実施の形態のように６つの関節を有する場合には、ｑ（ｔ）＝（ｑ_１（ｔ），ｑ_２（ｔ），ｑ_３（ｔ），ｑ_４（ｔ），ｑ_５（ｔ），ｑ_６（ｔ））である。関節角軌道関数のパラメータａ_０，ａ_１，ａ_２，・・・ａ_ｎは、６つの関節のそれぞれについて決定されるので、合計で（ｎ＋１）×６個のパラメータを決定することになる。従来は、数千ものパラメータを求めなければならなかったのに対し、本実施の形態では、パラメータ数を大幅に減らすことができる。

ロボットアーム２０の運動方程式は、τを関節のトルク、Ｍをアームの慣性行列、Ｃを遠心力・コリオリ力を表す行列、Ｇを重力の影響を表すベクトルとして、次式（２）で表される。

運動方程式記憶部１４には、制御対象のロボットアーム２０の腕の長さや重さ等に基づいて、あらかじめ計算によって求めたＭ，Ｃ，Ｇのデータが記憶されている。

仕様データは、ロボットアーム２０のハードウェアの仕様や動作条件などのデータである。具体的に、ハードウェアの仕様としては、各関節がどの程度動くかを示す最大関節角度のデータ、各関節の最大駆動トルクのデータがある。また、安全性等の観点から設定される動作条件としては、関節角速度や関節角加速度の制限がある。また、複数のロボットアーム２０を使用する場合には、動作条件として、衝突防止等のために軌道の形状の限定を設定してもよい。

コスト関数は、ロボットアーム２０の制御方針に基づいて関節角軌道にコストを与える関数である。例えば、目標状態に早く遷移することを制御方針とするなら目標状態に向かって早く移動するほどコストを小さくする関数、低消費エネルギーを制御方針とするならトータルの駆動トルクが小さくなるほどコストを小さくする関数といったように、制御方針によってコスト関数が定められる。次式（３）は、目標状態との差の積分をコストとしており、目標状態に近づく動きをするほどコストが低くなるコスト関数の一例である。

入力部１０は、ロボットアーム２０の初期状態及び目標状態の情報を入力する機能を有する。入力部１０は、例えば、カメラであってもよく、カメラで撮影した映像を解析して、ユーザが指さす場所を初期位置、目標位置と認識してもよい。この場合には、初期速度および目標速度は０とすることで、初期状態、目標状態を規定できる。入力部１０は、キーボードやマウスであってもよく、キーボードから初期状態（初期位置および初期速度）と、目標状態（目標位置と目標速度）とを入力してもよい。

最適解計算部１１は、入力された初期状態から目標状態まで、ロボットアーム２０を動かすための最適な軌道を求める機能を有する。最適解計算部１１は、ハードウェアの仕様や動作条件などを制約として、ロボットアーム２０を初期状態から目標状態まで動かす最適な軌道を制約付き最適化問題として解く。次式（４）は、最適解計算部１１が計算を行う制約付き最適化問題を示す。次式は、「ｓ．ｔ．」で示す制約条件の下で、コスト関数Ｊを最小化するパラメータＡの最適解を求める問題である。

条件（ｉ）と条件（ｉｉ）は初期状態の制約であり、条件（ｉ）が初期状態の関節角度、条件（ｉｉ）が初期状態の関節角速度が入力された値に等しいことを条件としている。条件（ｉｉｉ）と条件（ｉｖ）は目標状態の制約であり、条件（ｉｉｉ）が目標状態の関節角度、条件（ｉｖ）が目標状態の関節角速度が入力された値に等しいことを条件としている。

条件（ｖ）は、制御区間ｔ_０からｔ_ｆにおいて、関節角度が所定の角度範囲にあることを条件としている。条件（ｖｉ）は、制御区間ｔ_０からｔ_ｆにおいて、関節角速度が所定の角速度範囲にあることを条件としている。ここでは、連続時間［ｔ_０，ｔ_ｆ］に有限個の離散点をつくり、各点で制約を満たすことを条件としている。なお、制約条件は、制御区間の全区間において満たすべきであるが、ここでは、最適解算出を行えるように、連続時間［ｔ_０，ｔ_ｆ］に有限個の離散点をつくり、各点で制約を満たすことを条件とした。これは離散化の一例であり、連続時間［ｔ_０，ｔ_ｆ］についての不等式制約を離散化する方法は、他の方法を採用してもよい。

条件（ｖｉｉ）は、駆動トルクが制御区間ｔ_０からｔ_ｆにおいて所定のトルク範囲内にあることを条件としている。ここで、関節角の軌道に従ってロボットアームを動作させるための駆動トルクは、数式（２）で示した運動方程式から求めることができる。これは、「逆動力学計算」と呼ばれる。

逆動力学計算は、関節数が多い場合には、計算が非常に重くなるので、本実施の形態では、再帰計算式を用いた逆動力学計算アルゴリズムを用いて、計算量の低減を図る。この計算アルゴリズムは、例えば、Park, Frank C., James E. Bobrow, and Scott R. Ploen. "A Lie group formulation of robot dynamics." The International Journal of Robotics Research14.6 (1995): 609-618.に記載されている方法を採用することができる。

最適解計算部１１は、式（４）に示す制約付き最適化問題を、ＳＱＰ（Sequential Quadratic Programming：逐次２次計画法）、内点法などによって解く。最適解計算部１１は、制約条件（ｉ）〜（ｖｉｉ）の下で、コスト関数を最小化するパラメータを求めるとともに、このパラメータに対応する各関節の駆動トルクを求める。

出力部１２は、最適解計算部１１にて求めた駆動トルクのデータを制御命令としてロボットアーム２０に送信する。

図３は、第１の実施の形態のロボットアーム制御装置１の動作を示す図である。ロボットアーム制御装置１は、ユーザから初期状態および目標状態の入力を受け付ける（Ｓ１０）。初期状態は、ロボットアーム２０の最初の位置および関節角速度であり、目標状態は、目標とするロボットアーム２０の位置および関節角速度である。ロボットアーム２０を静止した状態から静止した状態に動かす場合には、初期状態および目標状態の関節角速度は０である。

ロボットアーム制御装置１は、関節角軌道関数記憶部１３から関節角軌道のデータを読み出し（Ｓ１１）、運動方程式記憶部１４から運動方程式のデータを読み出し（Ｓ１２）、仕様データ記憶部１５からハードウェアの仕様や動作条件等のデータを読み出し（Ｓ１３）、コスト関数記憶部１６からコスト関数を読み出す（Ｓ１４）。各データの読み出しの順序は、上述した順に限られず、任意に順番を入れ替えてもよい。

ロボットアーム制御装置１は、初期状態および目標状態、関節角度、関節角速度、駆動トルクの制約条件の下で、コスト関数を最小にする関節角軌道のパラメータの最適解を求める（Ｓ１５）。この際、最適解のパラメータに対応して、各関節の駆動トルクの時間的変化が、運動方程式により求まる。ロボットアーム制御装置１は、制御区間の各時刻における駆動トルクのデータにしたがって、ロボットアーム２０に制御命令を送信する（Ｓ１６）。これにより、ロボットアーム２０が、制約条件を満たす軌道で初期状態から目標状態に動く。

第１の実施の形態のロボットアーム制御装置１は、関節角軌道関数のパラメータを、制約付きのコスト関数最適化問題として解くことにより、パラメータ数を大幅に削減して、現実的な計算量で関節角軌道を求めることができる。

また、この構成により、任意の基底関数を線形結合した形式の関節角軌道関数の最適解を求めることができるので、生成されるロボットアーム２０の軌道の自由度が高く、任意の軌道を描くことができる。

また、制約条件を満たす最適解を計算することにより、例えば、遠回りを含む動作のように、人間では思いつきにくい動作を発見することができる。本出願人らは、本実施の形態のロボットアーム制御装置を用いて、１リンク、３リンク、６リンク等の様々なタイプのロボットアームの軌道をシミュレーションしたところ、制約を満たす適切な軌道を現実的な計算時間で生成できた。一例を挙げると、低駆動トルクの１リンクのアームにおいて、アームを振り上げた目標状態にするような軌道を計算したところ、アームを持ち上げることができない軌道ではなく、アームをスウィングバックして勢いをつけることによって、低トルクでアームを振り上げる軌道が求められた。

（第２の実施の形態）
図４は、第２の実施の形態のロボットアーム制御装置２の構成を示す図である。第２の実施の形態のロボットアーム制御装置２の基本的な構成は、第１の実施の形態のロボットアーム制御装置１と同じであるが、第２の実施の形態のロボットアーム制御装置２は、最短動作時間計算部１７を備えている。

最短動作時間計算部１７は、複数の制約条件のうちの凸制約の下で、最短時間で、ロボットアーム２０を初期状態から目標状態に動かす関節角軌道関数のパラメータを求める。第１の実施の形態において、初期状態（条件（ｉ）（ｉｉ）、目標状態（条件（ｉｉｉ）（ｉｖ））、関節角度（条件（ｖ））、関節角速度（条件（ｖｉ））、駆動トルク（条件（ｖｉｉ））の制約条件について説明したが、このうち駆動トルクの制約条件が非凸制約であり、その他は凸制約である。

制約条件が凸制約の場合には、パラメータの初期値の設定によらず、解が一意に求まることが知られている（線形計画問題）。最短動作時間計算部１７は、凸制約のみを制約条件として、最適解計算処理を行って、関節角軌道のパラメータとその動作時間を求める。次式（５）は、最短動作時間計算部１７が計算を行う制約付き最適化問題を示す。次式は、「ｓ．ｔ．」で示す制約条件の下で、コスト関数Ｊを最小化するパラメータＡと初期状態から目標状態になるまでの時間ｔ_ｆの最適解を求める問題である。次式（５）は、上記した式（４）を、制御区間［ｔ_０，ｔ_ｆ］を［０，１］に正規化して式変形することで得られる。なお、最短動作時間計算部１７のコスト関数は、初期状態から目標状態になるまでの時間ｔ_ｆである。

条件（ｉ）と条件（ｉｉ）は初期状態の制約であり、条件（ｉ）が初期状態の関節角度、条件（ｉｉ）が初期状態の関節角速度が入力された値に等しいことを条件としている。条件（ｉｉｉ）と条件（ｉｖ）は目標状態の制約であり、条件（ｉｉｉ）が目標状態の関節角度、条件（ｉｖ）が目標状態の関節角速度が入力された値に等しいことを条件としている。

条件（ｖ）は、正規化された区間０から１において、関節角度が所定の角度範囲にあることを条件としている。条件（ｖｉ）は、正規化された区間０から１において、関節角速度が所定の角速度範囲にあることを条件としている。条件（ｖｉｉ）は、初期状態から目標状態に至る時間に関する制約であり、目標時刻の探索範囲制限を追加するものである。

最短動作時間計算部１７は、式（５）に示す凸制約のみの制約付き最適化問題を解いて、関節角軌道のパラメータとその動作時間を求める。式（５）に示すように、初期状態から目標状態までの時間ｔ_ｆをコスト関数としているので、最短動作時間計算部１７による計算で、初期状態から目標状態までの動作時間が最短となる軌道が求められる。ただし、駆動トルクの制約をつけていないので、駆動トルクに関しては無制限に大きくできるという前提である。以上が、最短動作時間計算部１７が行う計算処理である。

図５は、第２の実施の形態のロボットアーム制御装置２の動作を示す図である。第２の実施の形態のロボットアーム制御装置２が、初期状態及び目標状態の入力を受け付けて（Ｓ２０）、その後、関節角軌道関数、運動方程式、仕様データ、コスト関数を記憶部から読み出すまでの処理は（Ｓ２１〜Ｓ２４）、第１の実施の形態のロボットアーム制御装置１の動作と同じである（Ｓ１０〜Ｓ１４）。第２の実施の形態においては、ロボットアーム制御装置２は、凸制約の制約条件、具体的には、式（５）に示す条件（ｉ）〜（ｖ）及び（ｖｉｉ）を満たす最短動作時間の軌道を計算する（Ｓ２５）。

続いて、最適解計算部１１は、最短動作時間計算部１７によって求めた関節角軌道から、逆動力学計算により、各関節の駆動トルクを求め、駆動トルクが制約条件（ハードウェア仕様）を満たしているかどうか判定する（Ｓ２６）。この結果、駆動トルクが制約条件を満たしている場合には（Ｓ２６でＹＥＳ）、最短動作時間計算部１７で求めたパラメータが最適解であり、これに対応する駆動トルクに基づいて、ロボットアーム２０に制御命令を出す（Ｓ２７）。

最短動作時間計算部１７で求めた関節角軌道が、駆動トルクの制約条件を満たしていない場合には（Ｓ２６でＮＯ）、最適解計算部１１が凸制約の制約条件に加えて、非凸制約である駆動トルクの制約条件を加えて、最適解計算処理を行う（Ｓ２７）。このとき、最適解計算部１１は、最短動作時間計算部１７にて求めたパラメータを初期値として用いて、最適化計算を行う。この計算は、第１の実施の形態と同様に、ＳＱＰや内点法を用いて行う。なお、最短動作時間計算部１７にて求められた動作時間は、駆動トルクの制約をつけずに求めた動作時間なので、トルク制約をつけた場合の最適解の制御時間ｔは、最短動作時間計算部１７にて求まった時間ｔ_ｆ以上になる。したがって、最適解計算部１１は、制御時間ｔ_ｆ以上の範囲で最適解を求めればよい。

最適解計算部１１が最適解のパラメータを求めると、これに対応する駆動トルクに基づいて、ロボットアーム２０に制御命令を出す（Ｓ２８）。

以上、第２の実施の形態のロボットアーム制御装置２について説明した。第２の実施の形態のロボットアーム制御装置２は、制約条件を満たす最適解を求める際に、まず、最短動作時間計算部１７にて、凸制約の制約条件の下で、ロボットアーム２０を初期状態から目標状態へと最短時間で動かす関節角軌道関数のパラメータを求める。その上で、その関節角軌道のパラメータが非凸制約である駆動トルクの制約をも満たしている場合には、最短動作時間計算部１７にて求めたパラメータをそのまま最適解として用いることができ、非凸制約の制約条件化での計算を行わなくてもよいので、計算処理を大幅に減らすことができる。

非凸制約を有する制約付き最適化問題においては、初期値に依存して、解が得られたり、得られなかったりすることがある。本実施の形態においては、最適解計算部１１がコスト関数を最小にする制約付き最適化を行う場合には、最短動作時間計算部１７で求めたパラメータを初期値として用いることにより、最適解が得られる可能性が高くなる。

また、目標状態の時刻ｔ_ｆの指定が短すぎると、解が存在しない問題になってしまうことがあるが、本実施の形態では、凸制約の制約条件の下で最短動作時間を計算しておくことにより、最短動作時間かそれ以上の範囲で解を探索すれば、解が存在しないという事態を回避することができる。

以上、本発明のロボットアーム制御装置およびロボットアーム制御方法について、実施の形態を挙げて詳細に説明したが、本発明は上記した実施の形態に限定されるものではない。

上記した実施の形態では、初期状態、目標状態、関節角度、関節角速度、駆動トルクを制約条件として用いる例を挙げたが、これらのすべてを制約条件としなくてもよい。関節角度や駆動トルクはロボットアームの固有のハードウェア仕様なので、制約条件から省くことは困難であるが、例えば、動作条件に関係する関節角速度は、必ずしも制約条件として用いなくてもよい。逆に、上記に例示した制約条件以外を制約条件として用いてもよい。

上記した実施の形態では、入力部から初期状態を入力する例を挙げたが、ロボットアームの現在状態を初期状態として用いる場合には、入力部から初期状態を入力する必要はない。

本発明のロボットアーム制御装置は、初期状態から目標状態に至るロボットアームの軌道を自動生成するので、近年、その用途が拡大しているロボットの制御技術として有用である。特に、低トルク出力アームの性能を引き出す用途や、カメラ等のセンサと連携した多関節ロボットアーム制御の高効率化に有用である。

１，２ ロボットアーム制御装置
１０ 入力部
１１ 最適解計算部
１２ 出力部
１３ 関節角軌道関数記憶部
１４ 運動方程式記憶部
１５ 仕様データ記憶部
１６ コスト関数記憶部
１７ 第１計算処理部
１８ 第２計算処理部
２０ ロボットアーム

Claims (8)

1. 関節を有するロボットアームの制御装置であって、
   前記ロボットアームの目標状態を入力する入力部と、
   前記ロボットアームの関節の最大駆動角度および関節の最大駆動トルクの仕様データを記憶した仕様データ記憶部と、
   前記ロボットアームの関節角軌道を表す関数であって、複数の基底関数にパラメータによる重み付けをして線形結合した形式を有する関節角軌道関数を記憶した関節角軌道関数記憶部と、
   前記関節角軌道に対して、所定の制御方針に従ってコストを与えるコスト関数を記憶したコスト関数記憶部と、
   前記目標状態と、関節の駆動角度が前記最大駆動角度以下であることと、関節の駆動トルクが最大駆動トルク以下であることとを制約条件として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求める最適解計算部と、
   前記最適解計算部にて求めたパラメータに対応する制御命令をロボットアームに対して出力する出力部と、
   を備えるロボットアーム制御装置。
2. 前記最適解計算部は、前記制御区間内の複数の離散点において制約条件を満たすパラメータを求める請求項１に記載のロボットアーム制御装置。
3. 前記仕様データ記憶部は、関節の角速度の最大角速度のデータを記憶しており、
   前記最適解計算部は、関節の角速度が前記最大角速度以下であることも制約条件として、前記パラメータを求める請求項１または２に記載のロボットアーム制御装置。
4. 前記入力部は、前記ロボットアームの初期状態の入力を受け付け、
   前記最適解計算部は、前記ロボットアームの初期状態も制約条件として、前記パラメータを求める請求項１乃至３のいずれかに記載のロボットアーム制御装置。
5. 前記関節の駆動トルクを除く制約条件の下で、前記ロボットアームを前記目標状態へ最短時間で遷移させるパラメータを計算する最短動作時間計算部を備え、
   前記最適解計算部は、前記最短動作時間計算部にて求めたパラメータを初期値として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求める請求項１乃至４のいずれかに記載のロボットアーム制御装置。
6. 前記最短動作時間計算部で求めた関節角軌道が、駆動トルクの制約条件を満たすか否かを判定し、駆動トルクの制約条件を満たすと判定された場合には、前記最適解計算部による処理を行わないで前記最短動作時間計算部で求めたパラメータを最適解とし、駆動トルクの制約条件を満たさないと判定された場合には、前記最適解計算部による処理を行う請求項５に記載のロボットアーム制御装置。
7. 関節を有するロボットアームを制御装置によって制御する方法であって、
   前記制御装置に、前記ロボットアームの目標状態を入力するステップと、
   前記制御装置が、仕様データ記憶部から、前記ロボットアームの関節の最大駆動角度および関節の最大駆動トルクの仕様データを読み出すステップと、
   前記制御装置が、関節角軌道関数記憶部から、前記ロボットアームの関節角軌道を表す関数であって、複数の基底関数にパラメータによる重み付けをして線形結合した形式を有する関節角軌道関数を読み出すステップと、
   前記制御装置が、コスト関数記憶部から、前記関節角軌道に対して所定の制御方針に従ってコストを与えるコスト関数を読み出すステップと、
   前記制御装置が、前記目標状態と、関節の駆動角度が前記最大駆動角度以下であることと、関節の駆動トルクが最大駆動トルク以下であることとを制約条件として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求める最適解計算ステップと、
   前記制御装置が、前記最適解計算ステップにて求めたパラメータに対応する制御命令をロボットアームに対して出力するステップと、
   を備えるロボットアーム制御方法。
8. 関節を有するロボットアームを制御するためのプログラムであって、コンピュータに、
   前記ロボットアームの目標状態を入力するステップと、
   仕様データ記憶部から、前記ロボットアームの関節の最大駆動角度および関節の最大駆動トルクの仕様データを読み出すステップと、
   関節角軌道関数記憶部から、前記ロボットアームの関節角軌道を表す関数であって、複数の基底関数にパラメータによる重み付けをして線形結合した形式を有する関節角軌道関数を読み出すステップと、
   コスト関数記憶部から、前記関節角軌道に対して所定の制御方針に従ってコストを与えるコスト関数を読み出すステップと、
   前記目標状態と、関節の駆動角度が前記最大駆動角度以下であることと、関節の駆動トルクが最大駆動トルク以下であることとを制約条件として、前記コスト関数を最小化する前記関節角軌道関数のパラメータを求める最適解計算ステップと、
   前記最適解計算ステップにて求めたパラメータに対応する制御命令をロボットアームに対して出力するステップと、
   を実行させるプログラム。

Images