JPH0756617A - マニピュレータの最適軌道計画法 - Google Patents

Abstract

(57)【要約】 【目的】現在産業界で実際に利用されているマニピュレ
ータへの適用が可能な最適軌道計画法を提供する。 【構成】本発明は大きく２つの課題に対する解決法を示
している。まず第１に、従来の軌道計画法では空間経路
と時間軌道の計画を交互に実行するため最適解を求めら
れない場合が生ずる。この課題に対しては評価関数を、
空間経路・時間軌道に関する最適化項と各制約項を拡張
ラグランジアン関数の形式に表し、両方を同時に計画す
ることによって解決している。第２に、関節数の増加に
伴い逆ダイナミックスの計算量が多項式オーダで増加す
るため軌道計画に膨大な時間を要する。この課題に対し
ては両方の計画に対応した逆ダイナミックスと時間スケ
ーリングを組み合せたニューラルネットワークを用い、
数理モデルを使う場合より計算量を低次のオーダに抑え
る事で、計算時間の大幅な短縮を可能としている。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は各種産業用マニピュレー
タに関して任意の評価基準に対する最適軌道を計画する
装置に関するものである。

【０００２】

【従来の技術】最適軌道を計画するためには、空間経路
と時間軌道の両方について計画する必要があり、その例
としては、Ｂスプラインを用いて軌道を表現し、一般的
な評価基準の下で、空間経路と時間軌道を交互に個別に
計画することにより近似最適軌道を計画する方法（山本
元司，圓島 信也，毛利 彰：“マニピュレータの近
似最適軌道の−生成法”，日本機械学会論文集（Ｃ
編），５６−５２７，ｐｐ．１８８１−１８８６ １９
９０．）また、ニューラルネットワークを用いたものと
しては、マニピュレータの逆ダイナミックスを学習させ
経路を計画する方法、（宇野 洋二，川西 康之，鈴木
良次：“多関節腕の繰り返し制御と逆ダイナミックス
モデルの学習”信学論，Ｖｏｌ．Ｊ７３−Ｄ−ＩＩ，Ｎ
ｏ．１，ｐｐ．１４０−１４８（１９９３）．）等が知
られている。

【０００３】ここで、従来法を簡単に説明する。マニピ
ュレータの関節数をｍ，その番号をｉ＝１，…ｍ，各関
節角をθ_ｉ，空間経路をθ_（ｔ）＝［θ_１（ｔ），θ
_２（ｔ）…θ_ｍ（ｔ）］^Ｔとする。軌道のデータ点数を
Ｎ，その番号をｊ＝１，…Ｎとする。

【０００４】一般にマニピュレータの逆ダイナミックス
は次式で表される。

【数１】 ただし Ｍ（θ_（ｔ））はマニピュレータの形状姿勢な
どにより定まる慣性行例、 クを表す。以下上式で記述されたモデルを数理モデルと
呼ぶ。

【０００５】空間経路θ_（ｔ）に対して時間軸を伸縮
（時間スケール変更）することで時間軌道を計画する。
以下、スケール変更前の時間軌道を基準軌道と呼ぶ。こ
のときの

【数２】

【数３】

【数４】

【０００６】実機の逆ダイナミックスを表す数理モデル
を構築するために、初めに実機のダイナミックスの同定
に必要なデータが取れるように、マニピュレータの姿
勢、アクチュエータの制限、各関節の動作範囲を考慮し
て、任意の始点・終点を結ぶ経路を用いて実機を動作さ
せ、その時の各関節の角度、角速度、角加速度及びトル
クを計測する。次にそれを逆ダイナミックスを表す数式
（数１）に入力し、算出されるトルクが計測されたトル
クと一致するように、逆ダイナミックスを表す係数行列
を決定する。この係数行列を用いることにより、時間ス
ケール変換のダイナミックスを表す数式（数２〜４）も
構築される。

【０００７】逆ダイナミックスネットワークについて
は、始めに上記と同様にして計測したデータを入力・教
師データとしてニューラルネットワークに学習させるこ
とにより、ネットワークを構築する。

【０００８】次に中間層の最適なユニット数は、汎化性
能を評価することにより決定する。まず上記と同様にし
て計測テスト用のデータとする。このときテスト用のデ
ータは学習に用いた軌道とは異なる軌道を用いて作成す
る。次にこれらのテストデータを各ネットワークに入力
しネットワークの出力とテストデータとの誤差を求め、
この誤差値が最も小さくなる場合のユニット数を最適な
ユニット数とする。

【０００９】

【発明が解決しようとする課題】これらの方法は、経路
計画および時間計画を交互に実行して最短時間軌道を計
画するため求まる軌道は近似解でしかなく、非線形最適
化法の一種である緩和法を用いて最適化問題を解くこと
に相当し、評価関数の非線形性が大きくなると、最適解
が求まらない場合が生じるという欠点がある。また、こ
れらの方法はマニピュレータのモデルとして数理モデル
を用いるため関節数の多い実機に適用する場合、計算量
が膨大になり実用上困難が多い。また、逆ダイナミック
スネットワークのみを用いた方法は時間軌道の計画はで
きない。

【００１０】本発明は空間経路、時間軌道を同時に計画
することにより、最適解が求まらない場合が生じる欠点
を回避し、数理モデルを学習ずみのニュートラルネット
ワークモデルに置き換えることにより、計算時間を大幅
に短縮した最適軌道計画法を提供することである。

【００１１】

【課題を解決するための手段】空間経路と時間軌道を同
時に計画するために、評価関数は次式に示すように、空
間経路・時間軌道に関する最適化項（Ｆ_ｐ（θ），Ｆ_ｔ
（κ））と各制約項（Ｐ_ｐ（θ），Ｐ_ｔ（κ））を拡張
Ｌａｇｒａｎｇｅａｎ関数の形式で表現し、これらのＬ
ａｇｒ−ａｎｇｅａｎ関数に係数を乗じて加算した評価
関数値を最小化する事により最適軌道を計画する。

【数５】

【００１２】ただし、７は空間経路に関するＬａｇｒａ
ｎｇｅａｎ関数に対して、時間軌道に関するＬａｇｒａ
ｎｇｅａｎ関数を重み付けをするための係数、λρ，μ
ρ，λτ，μτは空間経路・時間軌道に関するペナルテ
ィ乗数であり、これらの乗数は次により更新される。た
だしｋは反復回数を表す。

【００１３】この際、非線形最適化法を用いて評価関数
値を最小化する。収束の早さを考慮し、かつ制約条件を
満足化するために、Ｂｒｏｙｄｅｎ−Ｆｌｅｔｃｈｅｒ
−Ｇｏｌｄｆａｒｂ−Ｓｈａｎｎｏ法（ＢＦＧＳ法）と
ペナルティ乗数法を組み合わせた最適化法を用いる。こ
の最適化法を利用するためには、評価関数の各パラメー
タ（θ，κ）に関する一次偏微分値が必要となる。ここ
では、これを差分法により求めることを考えると、１変
数の微小変化に対する評価関数値の変化量が必要とな
る。通常のスプラインにより軌道を表現すると、１つの
パラメータの変化に伴い、軌道全体が変化するため、軌
道の局所的な変化に対する評価関数値の変化量を求める
ことができない。このため軌道は、１変数の変化に対し
て局所的に変化する性質（局所性）を持たなければなら
ない。また、マニピュレータの逆ダイナミックスを計算
する際、関節の角加速度を用いるため、その連続性を保
証する必要がある。従って、非線形最適化法を利用する
観点からも、Ｂスプラインが有効であると考えられる。
以降では、関節の角加速度の連続性を保証するため、４
階（３次）のＢスプラインを用いる。

【００１４】さて、マニピュレータのモデルとして数理
モデルを用いると、関節数の増加に伴い、逆ダイナミッ
クスの計算量が多項式オーダで増加する。そこで、この
計算量を削減するために、数理モデルを使わずに、図１
に示すようなネットワークを用いる。以下では、このニ
ューラルネットワークについて説明する。

【００１５】空間経路を計画するネットワークは、マニ
ピュレータの逆ダイナミックスを学習するため、関節の
角度・角速度・角加速度を入力し、これに対するトルク
を出力する構造を用いる。このネットワークを逆ダイナ
ミックスネットワークと呼ぶ。一方、時間軌道を計画す
るネットワークは、時間スケール変更の前後における角
速度とトルクの関係式（数式２〜４）を学習により獲得
するために、時間スケール変更前の関節の角度・角速度
・トルクおよび時間スケールファクタとその微分を入力
し時間スケール変更後の角速度・トルクを出力する構造
を用いる。このネットワークを時間スケーリングネット
ワークと呼ぶ。上記の２つのネットワークを個別に構築
し、２つのネットワークを組み合わせてネットワークを
構成する。以下このような構造のネットワークを軌道計
画ネットワークと呼ぶ。

【００１６】ネットワークには、学習モードと軌道計画
モードの２つの動作モードがあり、学習モードでは、ネ
ットワークが軌道計画を行うために必要な結合荷重を獲
得し、軌道計画モードでは、結合荷重を固定してネット
ワークのエネルギー値が減少するように、各ネットワー
ク入力の変数である経路と時間に関するパラメータ値を
更新する。そしてエネルギー値を最小化した後に得られ
るネットワークの出力が最適軌道となる。また、全時刻
における空間経路および時間軌道は、上記と同一の構造
および結合荷重値をもつネットワークにより生成でき
る。

【００１７】経路・時間を計画する２つのネットワーク
を学習させる際、各時刻の軌道を表すネットワークは全
て同一構造をもつため、軌道上の全時刻に対して各々１
つのネットワークのみを学習すればよい。

【００１８】時間スケーリングネットワークについて
は、初めに実機の逆ダイナミックスを表す係数行列を用
いて、時間スケール変換を表す数式（数２〜４）に、任
意の時間スケールファクタ、及び、経路を与え、それに
対する時間スケール変更後の軌道を計算する。そして時
間スケール変更前の各関節の角度、角速度、角加速度、
時間スケールファクタ、及び、その時間微分についての
データ並びに、時間スケール変更後の軌道における各関
節の角速度、トルクを入力・教師データとしてニューラ
ルネットワークに学習させることによりネットワークを
構築する。

【００１９】次に中間層のユニット数を汎化性能を評価
することにより決定する。上記と同様な手順でテスト用
のデータを作成する。この時テストデータは学習に用い
た軌道とは異なる軌道を用いて作成する。これらのテス
トデータを各ネットワークに入力し、ネットワークの出
力と教師のテストデータとの誤差を求め、この誤差が最
も小さくなる場合のユニット数を最適として選択する。

【００２０】軌道計画ネットワークが、個別に構築され
た２つのネットワークを図１のように接続した構造を持
つため、両者のモデルを１つのネットワークにより実現
するよりも、中間層のユニットが少なくて済み、学習及
び軌道計画の時間短縮に効果のあるモデルを構築でき
る。

【００２１】学習後の軌道計画ネットワークを用いた軌
道計画の手順を以下に示す。 ［１］ 経路および時間のパラメータに初期値を設定す
る。 ［２］ 経路・時間のパラメータからＢスプラインを用
いて補間し、関節の角度（空間経路）、角速度、角加速
度（時間軌道）、時間スケールファクタおよびその微分
値を求め軌道計画ネットワークに入力する。順方向計算
から、ネットワーク出力値（時間スケール変更後の速
度、トルク）を求める。 ［３］ ユニットの活性値である関節角度、角速度およ
び時間スケールファクタなどの値と、時間スケール変更
後の速度とトルクの値を用いて、経路と時間に関する評
価基準に基づき、ネットワークのもつエネルギー値を計
算する。 ［４］ エネルギー値を入力ユニットに逆伝搬し、この
値に基づいて経路と時間の各パラメータを更新し、
［２］へ戻る。 上記の［２］〜［４］を繰り返すことにより、ネットワ
ークのエネルギー値は減少し、最終的に経路と時間の各
パラメータは、制約条件を満たし、かつ各評価基準を最
小化する値に更新される。

【００２２】

【実施例】次に数値実験例によりさらに詳細に説明す
る。

【００２３】数値実験１ 空間経路と時間軌道を交互に計画する方法（以下従来
法）と同時に計画する方法（以下同時計画法）を２関節
マニピュレータモデルを用いた軌道計画に適用し、両者
を比較する。本数値実験では、マニピュレータの手先
（第２関節の先端）により文字“ｄ”を書く軌道計画問
題を用いた。この際、各関節のトルク変化最小基準およ
び動作時間最短基準を用いて最適軌道を計画する。ここ
で、手先が４箇所の経由点を通過するように、空間経路
の拘束条件として与えた。また、関節の角速度およびト
ルクに上下限値を設定し、この範囲内で軌道を実現する
ように制約条件として与えた。この際、空間経路と時間
軌道に関する最適化項（Ｆｐ，Ｆｔ） における軌道のデータ間隔を表す。

【数６】

【数７】

【数８】

【数９】

【００２４】

【００２５】 とトルクが、各々の制限値を越えた場合の制限値からの
超過分を表す。軌道の評価関数は、数２に示した拡張Ｌ
ａｇｒａｎｇｅａｎ関数の形で表し、この値を最小化す
る最適軌道を非線形最適化法を用いて求める。

【００２６】実験結果を図２〜図４に示す。ただし、網
線・実線は、従来法・同時計画法による軌道を表す。

【００２７】図２は、マニピュレータの手先の経路を表
しており、いずれの計画法を用いても、計画された経路
は、経由点の通過という拘束条件を満足していることが
分かる。また、計画された各関節の軌道およびトルク変
化を図３、図４に示す。これらの図から、同時計画法に
より計画されたトルク変化は、従来法を用いた場合と比
較して、滑らかであることが分かる。

【００２８】これは、従来法の場合、空間経路を固定し
て時間軌道を計画する際、トルク変化を考慮せずに計画
しているため、トルクが激しく変化するものと考えられ
る。

【００２９】表１に、従来法・同時計画法における収束
後の経路と時間に関する評価関数値（最適化項のみ）を
示す。表１より、評価関数値の点から比較しても、同時
計画法は、より最適な軌道を計画していることが分か
る。

【表１】

【００３０】以上の結果から、同時計画法を用いること
により、トルク変化は滑らかになり、動作時間の短い軌
道が計画され、空間経路および時間軌道の両方の基準に
おいて、従来法より改善された軌道が得られることが分
かる。

【００３１】数値実験２ 次に、数理モデルとニューラルネットを用いた場合の軌
道計画に要する時間を比較する。ここでは、数値実験１
と同じ問題に、ニューラルネットを用いた軌道計画法を
適用する。

【００３２】ネットワークの学習・汎化テストに用いる
入出力データは、以下の手順により求める。

【００３３】初めに、軌道の始点・終点を一様乱数を用
いて生成し、２点間の最適経路を躍度最小基準に基づき
解析的に求める。そして、非線形最適化法を用いて、こ
の最適経路に対する最短時間軌道を決定することによ
り、経路および時間パラメータが求まる。次に、得られ
た経路パラメータは、軌道の代表点であるため、Ｂスプ
ラインを用いて補間し、全時刻の軌道（時間スケール変
更前の軌道）を求める。同様に、時間パラメータをＢス
プラインを用いて補間し、全時刻の時間スケールファク
タを求め、この時間スケールファクタと先に求めた時間
スケール変更前の軌道から、時間スケール変更後の軌道
を求める。最後に、時間スケール変更前の軌道とトルク
から、逆ダイナミックスネットワークの学習用データを
求め、時間スケールファクタと時間スケール変更前後の
軌道から、時間スケーリングネットワークの学習用デー
タを求める。

【００３４】ネットワークの学習に際して、収束の速さ
などの点で優れており、ニューラルネットの学習に適し
たＫｉｃｋ Ｏｕｔ 法を用いて学習させる。

【００３５】次に、中間ユニット数については、ネット
ワークの中間ユニット数を変化させ各々に対して学習用
データ３０，０００パターンを与え、学習後にテストデ
ータ１５，０００パターンを与え、１パターン当たりの
平均誤差を求め、最も誤差が小さく、汎化されている中
間ユニット数を設定する。

【００３６】まず、逆ダイナミックスネットワークと時
間スケーリングネットワークを各々単独に動作させ、空
間経路と時間軌道を計画した結果を示す。

【００３７】始めに、逆ダイナミックスネットワークに
より空間経路を計画する。ここては、学習後の逆ダイナ
ミックスネットワークの性能を調べるために、トルク変
化最小基準に従いネットワークを動作させ、経路を計画
する。この際、逆ダイナミックスネットワークのエネル
ギー関数は次式を用いた。

【数１０】

【００３８】上式はトルク変化を表す最適化項（第１
項）と経路の経由点からの誤差を表す制約項（第２，３
項）にペナルティ乗数（λ，μ）を乗じた拡張Ｌａｇｒ
ａｎｇｅａｎ関数の形式となっている。非線形最適化法
を用いてエネルギー値を減少させる に関する偏微分値が必要であり、これは、誤差逆伝搬法
を利用して次式により求まる。

【数１１】

【００３９】計画された経路を図５に示す。実線・網線
は、ニューラルネット（逆ダイナミックスネットワー
ク）・数理モデルを用いて計画した経路を表す。図５よ
り、ニューラルネットにより計画された経路は、数理モ
デルを用いた場合とほぼ同様の経路が得られていること
が分かる。

【００４０】次に時間スケーリングネットワークを用い
て時間軌道を計画する。時間スケーリングネットワーク
を用いた時間計画では、先の逆ダイナミックスネットワ
ークの場合と同様に、次式で定義されるエネルギー値を
最小化するように時間スケールファクタを決定する。

【数１２】

【００４１】 越えた場合の制限値からの超過分を表す。エネルギー関
数の時間スケールファクタに関する偏微分値は、誤差逆
伝搬法を用いて次式により求める。

【数１３】

【００４２】時間スケーリングネットワークに、図６の
網線で示す初期軌道を与えてエネルギー値を最小化した
後、得られた時間スケールファクタを図７（実線）に示
す。また、数理モデルを用いて計画した時間スケールフ
ァクタを網線で示す。ただし、両者の結果は一致してお
り、１本の曲線にしか見えない、このとき、時間スケー
リングネットワークを用いて得られた時間スケールファ
クタと時間スケール変更前の軌道から時間スケール変更
後の軌道を計算した結果を図６の実線で示す。

【００４３】図７から、計画された時間スケールファク
タの値は、データ点数が５０〜２５０の区間で変動して
いることが分かる。通常、速度、トルクに制約条件の無
い時間軌道計画問題の場合、計画された時間スケールフ
ァクタの値は、この区間内でほぼ一定値（０．１程度の
小さな数）となる。このことから図７における時間スケ
ールファクタの変動は、速度、トルクの制限条件を満た
すために生じたものと考えられる。

【００４４】本数値実験のように、制約条件のある問題
に対して、時間スケールファクタを決定することは難し
い。しかしながら、時間スケーリングネットワークを用
いて計画された結果は、数理モデルを用いた結果と同一
であることから、このネットワークは、時間スケール変
更のダイナミックスを表す関係式（数２〜４）を精度良
く近似しており、時間計画に十分利用できる性能がある
といえる。

【００４５】最後に軌道計画ネットワークにより最適軌
道を計画する。逆ダイナミックスネットワークと、時間
スケーリングネットワークを図１に従い結合し、経路に
関してトルク変化最小、時間に関しては動作時間最小と
いう基準を用いて軌道を計画した。実験結果を図８〜図
１０に示す。

【００４６】図８より、得られた軌道は、経路に関する
制約を満足していることが分かる。また、図９，１０よ
り、ニューラルネットワークを用いて軌道を計画した結
果、数理モデルを用いた場合と同様に、トルク変化を抑
える軌道が得られていることが分かる。

【００４７】軌道計画に要した時間は、数理モデルを用
いた場合、約２６時間ニューラルネットワークを用いた
場合、約３０分であった。（計算機：Ｓｕｎ Ｓｐａｒ
ｃＳｒａｔｉｏｎ １）．これより、ニューラルネット
ワークを用いた軌道計画では、数理モデルを用いた軌道
計画と比較して、約５０分の１の時間で軌道を計画でき
ることが分かる。

【００４８】

【発明の効果】この発明は従来法に比べて次のような利
点を有している。 １）本軌道計画法を用いると、従来法を用いて計画され
た軌道と比較して、空間経路、及び、時間軌道に関して
任意に与えられた評価基準の点で、より最適化された軌
道を計画することができる。 ２）従来法の１／５０という計算時間で計画ができる。

【図面の簡単な説明】

【図１】 軌道計画ネットワーク

【図２】 計画された手先の経路

【図３】 計画された最適軌道

【図４】 計画されたトルク

【図５】 逆ダイナミックスネットワークにより計画さ
れた経路

【図６】 時間スケール変更前後の軌道

【図７】 計画された時間スケールファクタ

【図８】 関節軌道から実現される手先の経路

【図９】 計画された最適軌道

【図１０】 計画されたトルク

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号 庁内整理番号 ＦＩ 技術表示箇所 Ｇ０５Ｂ 13/02 Ｋ 9131−3Ｈ

Claims (2)

【特許請求の範囲】
1. 【請求項１】マニピュレータの最適軌道を計画するにあ
   たって （イ） 実機のマニピュレータに関する逆ダイナミック
   ス及び時間スケール変換を表すダイナミックスを同定し
   た数理モデルを構築しそれを用いること （ロ） 空間経路・時間軌道を表現する変数を含み、か
   つ、上記の数理モデルを元に構成される評価関数を用い
   ること（ただし、空間経路とは各間節の通過位置−関節
   角度−であり、時間軌道とは、各位置での通過速度−角
   速度−及び加速度−角加速度−である） （ハ） 評価関数は任意の評価基準から構成される項−
   最適化項−と任意の制約条件から構成される項−制約項
   −からなり、空間経路・時間軌道に関する各々の項に係
   数を乗じて和の形式で表現されていること （ニ） 評価関数を数理モデルに適用し、その値を非線
   形最適化法を用いて最大化又は最小化することにより、
   空間経路・時間軌道の両方について同時に解を求める最
   適軌道計画法。
2. 【請求項２】マニピュレータの軌道計画にニューラルネ
   ットワークを用いるにあたって （イ） 実機のマニピュレータに関する逆ダイナミック
   スと時間スケール変換を表すダイナミックスを学習した
   ２つの個別のニューラルネットワークモデルを構築する
   こと。 （ロ） 空間経路・時間軌道を表現する変数を含み、か
   つ、上記の２つのニューラルネットワークを元に構成さ
   れる評価関数を用いること。 （ハ） 評価関数は任意の評価基準から構成される項
   （最適化項）と任意の制約条件から構成される項（制約
   項）からなり、空間経路・時間軌道に関する各々の項に
   係数を乗じて和の形式で表現されていること。 （ニ） 評価関数を上記２つのネットワークを結合させ
   たニューラルネットワークに適用しその値を非線形最適
   化法を用いて最大化又は最小化することにより、空間経
   路・時間軌道の両方について同時に解を求める最適軌道
   計画法。