(57)【要約】【課題】キネマティクス問題を高速で解くことができるキネマティクス演算用ライブラリの実装選択方法を提供する。【解決手段】異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリの複数種類の実装の各々を複数種類のコンパイラでコンパイルする第１の工程と、前記第１の工程でコンパイルされたキネマティクス演算用ライブラリの複数種類の前記実装を用いて所定のキネマティクス演算を各々行う第２の工程と、前記第２の工程で行われたキネマティクス演算の各々の結果を比較し、前記キネマティクス演算用ライブラリの複数種類の実装から、前記所定のキネマティクス演算に最適な実装を選択する第３の工程とからなる、キネマティクス演算用ライブラリの実装選択方法。【選択図】図１
2019141986.tif 000002
【特許請求の範囲】
【請求項１】
異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリの複数種類の実装の各々を複数種類のコンパイラでコンパイルする第１の工程と、
前記第１の工程でコンパイルされたキネマティクス演算用ライブラリの複数種類の実装を用いて所定のキネマティクス演算を各々行う第２の工程と、
前記第２の工程で行われたキネマティクス演算の各々の結果を比較し、前記キネマティクス演算用ライブラリの複数種類の実装から、前記所定のキネマティクス演算に最適な実装を選択する第３の工程とからなる、キネマティクス演算用ライブラリの実装選択方法。
【請求項２】
異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリの複数種類の実装の各々に対し、制御対象となるプロセッサのアーキテクチャに基づいて最適化する第１の工程と、
前記第１の工程で最適化されたキネマティクス演算用ライブラリの複数種類の前記実装の各々を複数種類のコンパイラでコンパイルする第２の工程と、
前記第２の工程でコンパイルされたキネマティクス演算用ライブラリの複数種類の前記実装を用いて所定のキネマティクス演算を各々行う第３の工程と、
前記第３の工程で行われたキネマティクス演算の各々の結果を比較し、前記キネマティクス演算用ライブラリの複数種類の実装から、前記プロセッサおよび前記所定のキネマティクス演算に最適な実装を選択する第４の工程とからなる、キネマティクス演算用ライブラリの実装選択方法。
【請求項３】
前記異なる剛体変換表現形式に、４×４行列による剛体変換表現形式及び四元数による剛体変換表現形式を含む、請求項１又は２に記載のキネマティクス演算用ライブラリの実装選択方法。
【請求項４】
前記第１の工程で、前記プロセッサのCPUが持つベクトル演算機能に合わせた最適化を行う、請求項２に記載のキネマティクス演算用ライブラリの実装選択方法。
【請求項５】
請求項１又は２に記載のキネマティクス演算用ライブラリの実装選択方法で選択した実装を、制御対象となるマニピュレータにおけるプロセッサの動きを制御するキネマティクス演算のために用いる、マニピュレータの制御方法。
【請求項６】
前記キネマティクス演算は、順運動学、逆運動学、ヤコビ行列の少なくとも何れかである、請求項５に記載のマニピュレータの制御方法。
【請求項７】
複数種類の剛体変換表現形式を記憶した記憶装置と、
制御対象となるマニピュレータにおけるプロセッサの動きを制御するキネマティクス演算を行うキネマティクス演算部と、
前記記憶装置に記憶された複数種類の剛体変換表現形式の何れか１つを選択する剛体変換表現形式選択部とを有し、
前記剛体変換表現形式選択部は、前記複数種類の剛体変換表現形式の各々を、前記キネマティクス演算部にて演算し、パフォーマンスのベンチマークを取得し、取得したベンチマークを比較してパフォーマンスに最も優れる剛体変換表現形式を選択する機能を備える、キネマティクス演算用プログラム選択システム。
【請求項８】
前記剛体変換表現形式選択部で選択された最適な剛体変換表現形式の演算機能を、更に、前記プロセッサに合わせて最適化する演算機能最適化部を前記演算装置に実装する、請求項７に記載のキネマティクス演算用プログラム選択システム。
【請求項９】
制御対象となるマニピュレータと、
前記マニピュレータを制御する制御コントローラを含むプロセッサと、
複数種類の剛体変換表現形式を記憶した記憶装置と、
前記プロセッサの動きを制御するキネマティクス演算を行うキネマティクス演算部と、
前記記憶装置に記憶された複数種類の剛体変換表現形式の何れか１つを選択する剛体変換表現形式選択部とを有し、
前記剛体変換表現形式選択部は、前記複数種類の剛体変換表現形式の各々を、前記キネマティクス演算部にて演算し、パフォーマンスのベンチマークを取得し、取得したベンチマークを比較してパフォーマンスに最も優れる剛体変換表現形式を選択する機能を備える、マニピュレータの制御システム。
【請求項１０】
前記剛体変換表現形式選択部で選択された最適な剛体変換表現形式の演算機能を、更に、前記プロセッサに合わせて最適化する演算機能最適化部を更に具備する、請求項９に記載のマニピュレータの制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、キネマティクス演算用ライブラリの実装選択方法、マニピュレータの制御方法、キネマティクス演算用プログラム選択システム、及びマニピュレータの制御システムに関する。
【背景技術】
【０００２】
ロボットアーム等のマニピュレータに高速でかつ正確な作業を行わせることを目的として、オンライン・リアルタイムでキネマティクス問題の数値解を求める（以下、キネマティクス演算、ともいう）研究が行われている。
例えば、特許文献１には、冗長マニピュレータの制御方法において、関節を冗長関節と非冗長関節とに切り分けし、切り分けされた前記冗長関節の関節角をパラメータとして変化させたとき、前記パラメータとして与えられる前記冗長関節の前記関節角と、逆キネマティクス演算で解として得られた前記非冗長関節の関節角とを用いた評価関数及び拘束条件に基づいて前記関節角の最適解を求め、前記最適解が前記手先位置の目標範囲をカバーするか否かを判定し、否の判定であるときには目標手先位置の変更を行って前記最適解を求める手順を繰り返す手法が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−１９３３１１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
近年、キネマティクス演算の速度が、マニピュレータの制御システムの安定性や制御精度に大きく寄与することが明らかになり、更なる高速化が求められている。
本発明はこのような実情に鑑みてなされたものであり、本発明の課題は、キネマティクス問題を高速で解くことができるキネマティクス演算用プログラム選択システム、キネマティクス演算用ライブラリの実装選択方法、マニピュレータの制御システム及びマニピュレータの制御方法を提供することである。
【課題を解決するための手段】
【０００５】
本発明者は、上記の課題を解決するために鋭意研究した結果、制御対象となるマニピュレータを含むプロセッサのアーキテクチャごとに、それぞれ最適なマニピュレータの制御プログラムが異なることを見出した。本発明は、この知見に基づくものであり、以下の［１］〜［１０］を提供する。
本明細書において、「プロセッサ」とは、コンピュータシステムの中でソフトウェアプログラムに記述された命令セットを実行するためのハードウェア（具体的には、ロボットシステム等）を意味し、「アーキテクチャ」とは、ハードウェアの基本設計や設計思想などを意味する。また、本明細書において、「パフォーマンス」とは、プログラムの実行速度、メモリ使用量や、消費電力等を意味し、「パフォーマンスに最も優れる」とは、実行速度、メモリ使用量や、消費電力等の観点で総合的に最も優れることを意味する。また、本明細書において、「ベンチマーク」とは、性能指標とする試験的な測定結果を意味する。
＜キネマティクス演算用ライブラリの実装選択方法＞
［１］  異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリの複数種類の実装の各々を複数種類のコンパイラでコンパイルする第１の工程と、前記第１の工程でコンパイルされたキネマティクス演算用ライブラリの複数種類の実装を用いて所定のキネマティクス演算を各々行う第２の工程と、前記第２の工程で行われたキネマティクス演算の各々の結果を比較し、前記キネマティクス演算用ライブラリの複数種類の実装から、前記所定のキネマティクス演算に最適な実装を選択する第３の工程とからなる、キネマティクス演算用ライブラリの実装選択方法。
［２］  異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリの複数種類の実装の各々に対し、制御対象となるプロセッサのアーキテクチャに基づいて最適化する第１の工程と、前記第１の工程で最適化されたキネマティクス演算用ライブラリの複数種類の前記実装の各々を複数種類のコンパイラでコンパイルする第２の工程と、前記第２の工程でコンパイルされたキネマティクス演算用ライブラリの複数種類の前記実装を用いて所定のキネマティクス演算を各々行う第３の工程と、前記第３の工程で行われたキネマティクス演算の各々の結果を比較し、前記キネマティクス演算用ライブラリの複数種類の実装から、前記プロセッサおよび前記所定のキネマティクス演算に最適な実装を選択する第４の工程とからなるキネマティクス演算用ライブラリの実装選択方法。
［３］  前記異なる剛体変換表現形式に、４×４行列による剛体変換表現形式及び四元数による剛体変換表現形式を含む、［１］又は［２］に記載のキネマティクス演算用ライブラリの実装選択方法。
［４］  前記第１の工程で、前記プロセッサのCPUが持つベクトル演算機能に合わせた最適化を行う、［２］に記載のキネマティクス演算用ライブラリの実装選択方法。
＜マニピュレータの制御方法＞
［５］  ［１］又は［２］に記載のキネマティクス演算用ライブラリの実装選択方法で選択した実装を、制御対象となるマニピュレータにおけるプロセッサの動きを制御するキネマティクス演算のために用いる、マニピュレータの制御方法。
［６］  前記キネマティクス演算は、順運動学、逆運動学、ヤコビ行列の少なくとも何れかである、［５］に記載のマニピュレータの制御方法。
＜キネマティクス演算用プログラム選択システム＞
［７］  複数種類の剛体変換表現形式を記憶した記憶装置と、制御対象となるマニピュレータにおけるプロセッサの動きを制御するキネマティクス演算を行うキネマティクス演算部と、前記記憶装置に記憶された複数種類の剛体変換表現形式の何れか１つを選択する剛体変換表現形式選択部とを有し、前記剛体変換表現形式選択部は、前記複数種類の剛体変換表現形式の各々を、前記キネマティクス演算部にて演算し、パフォーマンスのベンチマークを取得し、取得したベンチマークを比較してパフォーマンスに最も優れる剛体変換表現形式を選択する機能を備える、キネマティクス演算用プログラム選択システム。
［８］  前記剛体変換表現形式選択部で選択された最適な剛体変換表現形式の演算機能を、更に、前記プロセッサに合わせて最適化する演算機能最適化部を前記演算装置に実装する、［７］に記載のキネマティクス演算用プログラム選択システム。
＜マニピュレータの制御システム＞
［９］  制御対象となるマニピュレータと、前記マニピュレータを制御する制御コントローラを含むプロセッサと、複数種類の剛体変換表現形式を記憶した記憶装置と、前記プロセッサの動きを制御するキネマティクス演算を行うキネマティクス演算部と、前記記憶装置に記憶された複数種類の剛体変換表現形式の何れか１つを選択する剛体変換表現形式選択部とを有し、前記剛体変換表現形式選択部は、前記複数種類の剛体変換表現形式の各々を、前記キネマティクス演算部にて演算し、パフォーマンスのベンチマークを取得し、取得したベンチマークを比較してパフォーマンスに最も優れる剛体変換表現形式を選択する機能を備える、マニピュレータの制御システム。
［１０］  前記剛体変換表現形式選択部で選択された最適な剛体変換表現形式の演算機能を、更に、前記プロセッサに合わせて最適化する演算機能最適化部を更に具備することを、［９］に記載のマニピュレータの制御システム。
【発明の効果】
【０００６】
本発明によれば、キネマティクス問題を高速で解くことができるキネマティクス演算用ライブラリの実装選択方法、マニピュレータの制御方法、キネマティクス演算用プログラム選択システム、及びマニピュレータの制御システムを提供することができる。
【０００７】
プロセッサのアーキテクチャごとに最適なプログラムを使用して、キネマティクス問題を高速で解くことができる本発明によれば、具体的な効果として、例えば以下の効果も得られる。
メモリ使用量が少ない実装及び/又は省電力の実装を選ぶことができる。
機械学習のためのデータ生成を高速化することができる。
マニピュレータの制御システムの安定性を高め、制御精度を上げることができる。
【図面の簡単な説明】
【０００８】
【図１】本実施形態のマニピュレータの制御システムの構成を示すブロック図である。
【図２】順運動学及び/又はヤコビアンのキネマティクス演算実行時のフロー図である。
【図３】逆運動学のキネマティクス演算実行時のフロー図である。
【図４】実施例において、キネマティクス演算用プログラムをコンパイルする際に使用したフロー図である。
【図５】本発明のマニピュレータの制御方法と従来手法での、各々のキネマティクス演算速度の結果を対比したグラフである。
【発明を実施するための形態】
【０００９】
＜キネマティクス演算用プログラム選択システム、マニピュレータの制御システム＞
本発明のキネマティクス演算用プログラム選択システムは、複数種類の剛体変換表現形式を記憶した記憶装置と、制御対象となるマニピュレータにおけるプロセッサの動きを制御するキネマティクス演算を行うキネマティクス演算部と、前記記憶装置に記憶された複数種類の剛体変換表現形式の何れか１つを選択する剛体変換表現形式選択部とを有し、前記剛体変換表現形式選択部は、前記複数種類の剛体変換表現形式の各々を、前記キネマティクス演算部にて演算し、パフォーマンスのベンチマークを取得し、取得したベンチマークを比較してパフォーマンスに最も優れる剛体変換表現形式を選択する機能を備える。
本発明のマニピュレータの制御システムは、上記のキネマティクス演算用プログラム選択システムと、制御対象となるマニピュレータを含むプロセッサを組合せて構成したものである。
【００１０】
以下、本発明のマニピュレータの制御システムの一実施形態について説明する。ただし本発明は、当実施形態に限定されるものではない。
本実施形態のマニピュレータの制御システムは、図１に示すように、制御対象となるマニピュレータ（ロボットアーム等）１と、マニピュレータの制御を行う制御コントローラ２を含むプロセッサ（本実施形態では、ロボットシステム）３と、プロセッサ３との間で制御信号を送受信するコンピュータ４から構成される。コンピュータ４は、CPU５と記憶装置６から構成され、CPU５は、制御装置７と演算装置８から構成される。
【００１１】
記憶装置６は、複数種類の剛体変換表現形式を実装している。
剛体変換表現形式の種類として、例えば、４×４行列による剛体変換表現形式、四元数による剛体変換表現形式、同次変換行列、四元数、回転行列と並進ベクトルの組み合わせ等を例示することができる。
既存のライブラリとして、提供されているものを組合せて実装することもできる。
複数種類の剛体変換表現形式の組合せは、特に限定されず、用途に応じて、適宜組み替えて使用することもできる。
【００１２】
演算装置８は、剛体変換表現形式選択部９と、演算機能最適化部１０と、キネマティクス演算部１１を実装している。
【００１３】
剛体変換表現形式選択部９は、記憶装置６に実装した複数種類の剛体変換表現形式の各々を、キネマティクス演算部１１にコンパイルしてパフォーマンスのベンチマークを取得し、取得したベンチマークを比較してパフォーマンスに最も優れる剛体変換表現形式を選択する機能を有する。
これまで、CPUが持つ固有の命令セット（SIMD最適化等）も考慮して最適な剛体変換表現形式を選ぶことは提案されていないが、仮に、当該課題が提起された場合、通常、プロセッサのアーキテクチャに基づき最適な剛体変換表現形式を予測するという手法が採用されると考えられる。しかし、近年、プロセッサのアーキテクチャが複雑化しているため、理論に基づく予測によって、制御対象のプロセッサに最適な剛体変換表現形式を決定することは困難である。本発明では、ベンチマークを取得して比較する手法で、最適な剛体変換表現形式の選択を可能とした。
【００１４】
演算機能最適化部１０は、前記剛体変換表現形式選択部９で選択された最適な剛体変換表現形式の演算機能を、更に、プロセッサに合わせて最適化する機能を有する。
最適化の手法は特に限定されないが、例えば、SIMD(CPUが持っているベクトル演算機能)最適化を実装しているライブラリ（以下、Eigen）を用いてSIMDの最適化を行う、x86系のアーキテクチャ向けに、Intel社が提供するSSE Intrinsicsを使用する等の手法を例示することができる。
プロセッサのアーキテクチャや、使用するコンパイラの組み合わせによっては、最適化の前後でパフォーマンスに大きな差が無い場合もあるが、当該工程を経ることで、多くの場合にパフォーマンスを大きく向上させることができる。
【００１５】
＜キネマティクス演算用ライブラリの実装選択方法、マニピュレータの制御方法＞
本発明のキネマティクス演算用ライブラリの実装選択方法は、上記のキネマティクス演算用プログラム選択システムで実行される方法である。本発明のキネマティクス演算用ライブラリの実装選択方法の一実施形態は、異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリの複数種類の実装の各々を複数種類のコンパイラでコンパイルする第１の工程と、前記第１の工程でコンパイルされたキネマティクス演算用ライブラリの複数種類の実装を用いて所定のキネマティクス演算を各々行う第２の工程と、前記第２の工程で行われたキネマティクス演算の各々の結果を比較し、前記キネマティクス演算用ライブラリの複数種類の実装から、前記所定のキネマティクス演算に最適な実装を選択する第３の工程を有する。本発明のキネマティクス演算用ライブラリの実装選択方法の他の実施形態は、異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリの複数種類の実装の各々に対し、制御対象となるプロセッサのアーキテクチャに基づいて最適化する第１の工程と、前記第１の工程で最適化されたキネマティクス演算用ライブラリの複数種類の前記実装の各々を複数種類のコンパイラでコンパイルする第２の工程と、前記第２の工程でコンパイルされたキネマティクス演算用ライブラリの複数種類の前記実装を用いて所定のキネマティクス演算を各々行う第３の工程と、前記第３の工程で行われたキネマティクス演算の各々の結果を比較し、前記キネマティクス演算用ライブラリの複数種類の実装から、前記プロセッサおよび前記所定のキネマティクス演算に最適な実装を選択する第４の工程を有する。
【００１６】
本発明のマニピュレータの制御方法は、上記のキネマティクス演算用ライブラリの実装選択方法で選択した実装を、制御対象となるマニピュレータにおけるプロセッサの動きを制御するキネマティクス演算のために用いる工程を有する。
【００１７】
本発明のマニピュレータの制御方法では、前記コンパイル用プログラムで、順運動学、逆運動学、ヤコビ行列の少なくとも何れかのキネマティクス演算を行うことができる。
前記コンパイル用プログラムが、順運動学及び/又はヤコビアンのキネマティクス演算用の場合は、図２のフローに従って、キネマティクス演算が実行される。
前記コンパイル用プログラムが、逆運動学のキネマティクス演算用の場合は、図３のフローに従って、キネマティクス演算が実行される。
これらのコンパイル用プログラムは、制御対象となるマニピュレータを含むプロセッサのアーキテクチャに合わせて最適化されているため、コンパイル後に実行されるキネマティクス演算の速度を高速化することができる。
【実施例】
【００１８】
以下、実施例を示して、本発明を更に具体的に説明する。
【００１９】
［演算例A］
それぞれ異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリ2種（MAT４とQUAT）について、Eigenで最適化を行った場合（表１中では、それぞれMAT４(E), QUAT(E)と記載）と、最適化を行わない場合に、４種のCPU(Cortex-M3, ARMv7, i5-7500, i7-6700K)と2種のコンパイラ（gcc, clang）の下記の組み合わせでそれぞれキネマティクス演算を行い、その演算速度を調べた。結果を表1に示す。
MAT４は、４×４行列を使用した剛体変換表現形式であり、QUATは、四元数を使用した剛体変換表現形式である。
【００２０】
【表１】
2019141986.tif 000003
【００２１】
表１から、i5-7500, i7-6700Kにおいては、MAT４とQUATの何れについても、Eigenで最適化を行うことで、キネマティクス演算速度が50％以上高速化することが確認できる。
ARMv7においては、MAT４については、Eigenで最適化を行うことで、キネマティクス演算速度が50％以上高速化することが確認できる。
Cortex-M3とMAT４、Cortex-M3とQUAT、ARMv7とQUATの組み合わせでは、Eigenでの最適化によるキネマティクス演算速度の高速化は確認できなかったが、これは、Eigenでの最適化前の剛体変換表現形式が既に、高パフォーマンスを実現可能な形式となっていることを意味する。
上記結果から、プログラムのコンパイル前に、Eigenでの最適化工程を組み込むことで、アーキテクチャによらず高パフォーマンスを実現できることが確認できる。
【００２２】
［演算例B］
それぞれ異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリ3種（MAT3、MAT４、DQ）について、Eigenで最適化を行った場合に、４種のCPU(Cortex-M3, ARMv7, i5-7500, i7-6700K)と2種のコンパイラ（gcc, clang）の下記の組み合わせでそれぞれキネマティクス演算を行い、その演算速度を調べた結果を表２に示す。
【表２】
2019141986.tif 000004
【００２３】
表２から、４種のCPU(Cortex-M3, ARMv7, i5-7500, i7-6700K)と2種のコンパイラ
（gcc, clang）の組み合わせごとに、最も高速でキネマティクス演算を実現できる剛体変換表現形式が異なることが確認できる。
上記結果から、本発明のような剛体変換表現形式の選択を行わない場合、Eigenでの最適化を行っても、安定的に高パフォーマンスを実現することはできないことが確認できる。
【００２４】
［演算例C］
それぞれ異なる剛体変換表現形式を用いたキネマティクス演算用ライブラリ3種（MAT3、MAT４、DQ）について、Eigenで最適化を行った上で、４種のCPU(Cortex-M3, ARMv7, i5-7500, i7-6700K)と2種のコンパイラ（gcc, clang）と下記組み合わせでそれぞれ、順運動、逆運動、ヤコビ行列のそれぞれについて、キネマティクス演算を行い、その演算速度を調べた結果と、既存の代表的キネマティクス演算用ライブラリOrocos KDLを用いて同様のキネマティクス演算を行い、その演算速度を調べた結果を表３に示す。
【００２５】
【表３】
2019141986.tif 000005
【００２６】
表３から、3種のキネマティクス演算用ライブラリ（MAT3、MAT４、DQ）について、Eigenで最適化を行った場合には、36例中35例（Cortex-M3での順運動以外の全て）で、KDLを用いた場合に比べて、キネマティクス演算速度が著しく速くなることが確認できるが、同一のCPUにおいても、キネマティクス演算のタイプ（順運動、逆運動、ヤコビ行列）ごとに、最適なキネマティクス演算用ライブラリが異なることが確認できる。
【００２７】
［実施例］
実施例として、16種のロボットを対象に、逆運動学のキネマティクス演算を行うキネマティクス演算用プログラムをコンパイルする際に図４のフローを適用して、キネマティクス演算を行い、演算速度を測定した。
比較例として、上記と同一対象について、逆運動学のキネマティクス演算を行うための既存プログラムであるtrac_ikを一律に適用して、キネマティクス演算を行い、演算速度を測定した。
上記実施例と比較例の測定結果を図５に示す。
【００２８】
図５から、本発明によれば、16種のロボット全てにおいて、trac ik runsを一律に適用した場合（従来の手法）に比べて、キネマティクス演算時間が８０％以下に短縮されること、すなわち、キネマティクス演算速度が２０％以上高速化することが確認できる。
【産業上の利用可能性】
【００２９】
本発明は、ロボットアーム、ロボットハンド等の剛体モデルを使用するロボット全般の、位置、速度、力の制御の何れにも用いることができる。回転関節角だけでなく、直動関節にも適用できる。
また、本発明は、３ＤＣＧ等の行列変換に対しても適用できる。
【符号の説明】
【００３０】
１    マニピュレータ
２    制御コントローラ
３    プロセッサ
４    コンピュータ
５    CPU
６    記憶装置
７    制御装置
８    演算装置
９    剛体変換表現形式選択部
１０    演算機能最適化部
１１    キネマティクス演算部
【図１】
2019141986.tif 000006
【図２】
2019141986.tif 000007
【図３】
2019141986.tif 000008
【図４】
2019141986.tif 000009
【図５】
2019141986.tif 000010