WO2025053285A1

WO2025053285A1 - プログラム、記憶媒体、情報処理装置、システム及びロボットの動作を制御する方法

Info

Publication number: WO2025053285A1
Application number: PCT/JP2024/032211
Authority: WO
Inventors: 泰義横小路
Original assignee: Robot Industrial Basic Technology Collaborative Innovation Partnership; Kobe University NUC
Current assignee: Robot Industrial Basic Technology Collaborative Innovation Partnership; Kobe University NUC
Priority date: 2023-09-08
Filing date: 2024-09-09
Publication date: 2025-03-13
Anticipated expiration: 2026-03-08
Also published as: JPWO2025053285A1; JP7706134B1; CN121794098A

Abstract

プログラムは、第１ヤコビ行列を特異値分解して、第１行列と、第２行列と、特異値を対角要素に含む第１特異行列とを含む第２ヤコビ行列を生成することと、前記特異値と第１閾値との比較結果に基づき、特異点又は前記特異点に接近している場合の第１処理又は前記特異点から離れている場合の第２処理の実行を決定することと、前記第１処理において、前記第１特異行列内の対角要素を変換して第３ヤコビ行列を生成し、前記第３ヤコビ行列とロボット（１０）の移動部（１２）の移動指令値とを用いて、前記ロボットの複数の関節（ＪＴ）の動作指令値を決定し出力することと、前記第２処理において、前記第１ヤコビ行列又は前記第２ヤコビ行列と前記移動指令値とを用いて、前記動作指令値を決定し出力することとを、コンピュータに実行させる。

Description

プログラム、記憶媒体、情報処理装置、システム及びロボットの動作を制御する方法

　本開示は、プログラム、記憶媒体、情報処理装置、システム及びロボットの動作を制御する方法に関する。

　例えば、特許文献１は、複数のジョイントを有する運動学的リンク機構を含む多軸機械の操作中に、特異点に対処するシステムを開示する。当該システムでは、ヤコビ行列を用いた数値制御が行われる。ヤコビ行列は、特異値を含む正規直交行列、左特異ベクトル及び右特異ベクトルに特異値分解され、特異値分解結果を用いて、特異点又は特異点に近い状態のジョイントが決定され、特異値及び左特異ベクトルを用いて、決定したジョイントに関連付けられるヤコビ行列の列を変換することで、ヤコビ行列が修正される。

特許第５３９３４６１号公報

　特許文献１では、特異値を用いて左特異ベクトルを演算して得られる仮想ジョイントが、決定したジョイントに関連付けられるヤコビ行列の列と置き換えられる。このため、ヤコビ行列が本来持っていたジョイントの動作速度とリンク機構の先端の動作速度との関係性が失われ得る。本開示は、ヤコビ行列の特異値分解を用いて特異点に対応した新規なロボットの動作制御を実現するプログラム、記憶媒体、情報処理装置、システム及び方法を提供する。

　本開示の一態様に係るプログラムは、複数の関節と前記複数の関節が動作することによって移動される移動部とを含むロボットの動作を制御するプログラムであって、前記複数の関節の動作速度と前記移動部の移動速度とを関連付ける第１ヤコビ行列を特異値分解して、前記関節の動作速度空間を表す第１基底ベクトルを要素に含む第１行列と、前記移動部の移動速度空間を表す第２基底ベクトルを要素に含む第２行列と、特異値を対角要素に含む第１特異行列とを含む、第２ヤコビ行列を生成することと、前記特異値と第１閾値との比較結果に基づき、前記ロボットが特異点にある場合及び前記ロボットが前記特異点に接近している場合に実行される第１処理と、前記ロボットが前記特異点から離れている場合に実行される第２処理とのうちから、実行対象の処理を決定して実行することと、前記第１処理において、前記第１特異行列内の対角要素を変換して第３ヤコビ行列を生成することと、前記第１処理において、前記第３ヤコビ行列と、前記移動部の移動を指令する移動指令値とを用いて、前記複数の関節の動作を指令する動作指令値を決定し出力することと、前記第２処理において、前記第１ヤコビ行列又は前記第２ヤコビ行列と、前記移動指令値とを用いて、前記動作指令値を決定し出力することとを、コンピュータに実行させる。

図１は、例示的な実施の形態に係るロボットシステムの構成の一例を示す図である。図２は、図１のロボットアームの関節の構成を示す概念図である。図３は、実施の形態に係るロボットシステムの構成の一例を示すブロック図である。図４は、特異点にあるロボットアームの姿勢の一例を示す概念図である。図５は、特異点にあるロボットアームの姿勢の一例を示す概念図である。図６は、特異点にあるロボットアームの姿勢の一例を示す概念図である。図７は、特異点にあるロボットアームの姿勢の一例を示す概念図である。図８は、実施の形態に係る情報処理装置の特異点に関する処理の一例を示すフローチャートである。図９は、実施の形態に係る情報処理装置の特異点に関する処理の一例を示すフローチャートである。図１０は、実施の形態に係る情報処理装置の特異点に関する処理の一例を示すフローチャートである。図１１は、実施の形態に係る情報処理装置の特異点に関する処理の一例を示すフローチャートである。図１２は、実施の形態に係る情報処理装置の特異点に関する処理の一例を示すフローチャートである。図１３Ａは、肩特異姿勢の場合のロボットアームのセルフモーションの一例を示す概念図である。図１３Ｂは、肩特異姿勢の場合のロボットアームのセルフモーションの一例を示す概念図である。図１３Ｃは、肩特異姿勢の場合のロボットアームのセルフモーションの一例を示す概念図である。図１４は、変形例に係るロボットシステムの構成の一例を示す図である。

　以下において、本開示の例示的な実施の形態を、図面を参照しつつ説明する。以下で説明する実施の形態は、いずれも包括的又は具体的な例を示すものである。以下の実施の形態における構成要素のうち、最上位概念を示す独立請求項に記載されていない構成要素については、任意の構成要素として説明される。添付の図面における各図は、模式的な図であり、必ずしも厳密に図示されたものでない。各図において、実質的に同一の構成要素に対しては同一の符号を付しており、重複する説明は省略又は簡略化される場合がある。本明細書及び請求項では、「装置」とは、１つの装置を意味し得るだけでなく、複数の装置を含むシステムも意味し得る。

　［ロボットシステムの構成］
　実施の形態に係るロボットシステム１の構成を説明する。図１は、実施の形態に係るロボットシステム１の構成の一例を示す概略図である。図１に示すように、実施の形態に係るロボットシステム１は、ロボット１０と、制御装置２０と、ユーザインタフェース３０とを備える。限定されないが、本実施の形態では、ロボットシステム１は、ロボット１０を手動操縦するシステムとして機能し、ユーザインタフェース３０は手動操縦器として機能し、ロボット１０は手動操縦器に入力される操作に従って動作する。制御装置２０は、ロボットシステム１の全体の動作を制御する。

　ロボット１０は、リンク構造体１１と、移動部１２とを含む。リンク構造体１１は、複数のリンクＬと、複数のリンクＬを動作可能に接続する複数の関節ＪＴとを含む。移動部１２は、リンク構造体１１に取り付けられる。本実施の形態では、移動部１２は、リンク構造体１１の先端に取り付けられる。リンク構造体１１は、複数の関節ＪＴが動作することによって移動部１２を様々な位置及び姿勢に移動できる。

　限定されないが、本実施の形態では、リンク構造体１１は、多関節ロボットアームであり、例えば、産業用のロボットアームである。リンク構造体１１は、複数の関節ＪＴによって複数のリンクＬが接続される構造を有すればよく、例えば、ロボットレッグであってもよい。

　さらに、リンク構造体１１は、垂直多関節型アームの構造を有し、複数のリンクＬ及び複数の関節ＪＴとして、６つリンクＬ１からＬ６及び６つの関節ＪＴ１からＪＴ６を含む。ロボットアームとしてのリンク構造体１１は、上記に限定されず、水平多関節型等の他の型式のアーム構造を有してもよい。リンクＬの数量及び関節ＪＴの数量も、５つ以下であってよく、７つ以上であってもよい。以下において、リンク構造体１１を、ロボットアーム１１と称する場合もある。「関節」は、関節を区別する場合には「関節ＪＴ１」から「関節ＪＴ６」を用いて表され、関節を区別しない場合には「関節ＪＴ」と表される。「リンク」は、リンクを区別する場合には「リンクＬ１」から「リンクＬ６」を用いて表され、リンクを区別しない場合には「リンクＬ」と表される。

　図２は、図１のロボットアーム１１の関節の構成を示す概念図である。図１及び図２に示すように、ロボットアーム１１は、その基部で基台１３に固定される。リンクＬ１からＬ６は、ロボットアーム１１の基部から先端に向かって順に配置される。関節ＪＴ１は、リンクＬ１を基台１３に、軸Ａ１を中心に回転可能に接続する。関節ＪＴ２は、リンクＬ２をリンクＬ１に、軸Ａ２を中心に回転可能に接続する。軸Ａ２の方向は、軸Ａ１の方向と交差し、例えば、直交する。関節ＪＴ３は、リンクＬ３をリンクＬ２に、軸Ａ３を中心に回転可能に接続する。軸Ａ３の方向は、軸Ａ２の方向に沿い、例えば、平行である。関節ＪＴ４は、リンクＬ４をリンクＬ３に、軸Ａ４を中心に回転可能に接続する。軸Ａ４の方向は、軸Ａ３の方向と交差し、例えば、直交する。関節ＪＴ５は、リンクＬ５をリンクＬ４に、軸Ａ５を中心に回転可能に接続する。軸Ａ５の方向は、軸Ａ４の方向と交差し、例えば、直交する。関節ＪＴ６は、リンクＬ６をリンクＬ５に、軸Ａ６を中心に回転可能に接続する。軸Ａ６の方向は、軸Ａ５の方向と交差し、例えば、直交する。

　垂直多関節型のロボットアーム１１の関節ＪＴ１からＪＴ６はいずれも、回転関節であるが、関節ＪＴ１からＪＴ６の少なくとも１つが直動関節であってもよい。例えば、関節ＪＴ１からＪＴ３が直動関節であってもよい。この場合、後述するロボットアーム１１の特異点が異なる。

　リンクＬ６は、先端にメカニカルインタフェースを含み、メカニカルインタフェースを介して移動部１２と接続される。移動部１２は、メカニカルインタフェースを介して、信号を送受信し、電力の供給を受けることができる。移動部１２は、対象物に作用を加えることができる構造を有し、エンドエフェクタとも呼ばれる。移動部１２の作用は特に限定されないが、本実施の形態では、対象物を把持する作用である。以下において、移動部１２を、エンドエフェクタ１２と称する場合もある。

　ロボットアーム１１は、関節ＪＴ１からＪＴ６それぞれを駆動する駆動装置Ｄ１からＤ６を含む。図３に示すように、駆動装置Ｄ１からＤ６はそれぞれ、駆動源としてモータＭ１からＭ６を含み、さらに、モータＭ１からＭ６の回転駆動力を伝達する減速機を含んでもよい。図３は、実施の形態に係るロボットシステム１の構成の一例を示すブロック図である。本実施の形態では、モータＭ１からＭ６はそれぞれ、サーボモータであり、電動モータと、電動モータの回転量を検出するエンコーダ等の回転センサＥとを含む。モータＭ１からＭ６の動作は、制御装置２０によって制御される。制御装置２０は、モータＭ１からＭ６に印加された電流値とそれぞれの回転センサＥの検出結果とをフィードバック情報として用いて、モータＭ１からＭ６をフィードバック制御する。

　図１及び図２に示すように、ユーザインタフェース３０は、有線通信、無線通信又はこれらの組み合わせを介して、制御装置２０と接続される。いかなる有線通信及び無線通信が用いられてもよい。ユーザインタフェース３０は、操作者による入力を受け付け、入力内容に基づく信号を制御装置２０に出力する。例えば、ユーザインタフェース３０は、ロボットアーム１１を手動操縦するための操作の入力を受け付ける。当該入力の内容は、ロボットアーム１１によって移動されるエンドエフェクタ１２の目標速度に少なくとも関連する。当該目標速度は、エンドエフェクタ１２の目標の移動方向と、当該移動方向へのエンドエフェクタ１２の目標の速さとを含む。ユーザインタフェース３０は、手動操縦のための入力内容に従ったエンドエフェクタ１２の目標速度を指令する信号を、制御装置２０に出力する。

　例えば、ユーザインタフェース３０は、ロボット１０の諸元に関する情報の入力を受け付け、当該情報を表す信号を制御装置２０に出力する。当該情報は、リンクＬの数量及び長さと、関節ＪＴの数量及び種類と、各リンクＬに接続される関節ＪＴと、各リンクＬに接続される関節ＪＴの軸Ａの位置及び方向とを含み得る。

　リンクＬの長さは、リンクＬ自体の長さであってもよく、リンクＬに接続される２つの関節ＪＴ間の距離であってもよい。関節ＪＴの種類は、回転関節又は直動関節を表し得る。関節ＪＴの軸Ａの位置は、関節ＪＴが接続されるリンクＬに対する回転関節の回転軸の位置又は直動関節の直動軸の位置を表し得る。関節ＪＴの軸Ａの位置は、リンクＬに接続される２つの関節ＪＴの２つの軸Ａ間の相対的な位置を表してもよく、例えば、２つの軸Ａ間の距離を表してもよい。関節ＪＴの軸Ａの方向は、関節ＪＴが接続されるリンクＬに対する回転関節の回転軸の方向又は直動関節の直動軸の方向を表し得る。関節ＪＴの軸Ａの方向は、リンクＬに接続される２つの関節ＪＴの２つの軸Ａ間の相対的な方向を表してよく、例えば、２つの軸Ａ間の角度を表してもよい。

　ユーザインタフェース３０は、ロボットアーム１１に類似した構造を有する操縦用アーム、ロボットの既知の操作装置、ティーチペンダントなどのロボットの既知の教示装置、パーソナルコンピュータなどのコンピュータ、スマートフォン及びタブレットなどのスマートデバイス、ゲーム端末、その他の操作装置、その他の端末装置、これらを利用する装置、これらを改良した装置、又は、上記装置の２つ以上の組み合わせであってもよい。ユーザインタフェース３０は、入力手段として、ボタン、レバー、ダイヤル、ジョイスティック、マウス、キー、タッチパネル、モーションキャプチャ又はこれらの２つ以上の組み合わせ等の操作者の操作を介して入力される装置を含んでもよい。

　本実施の形態では、ユーザインタフェース３０は、ジョイスティックを含む。ジョイスティックの傾動方向は、エンドエフェクタ１２の目標の移動方向を表し、ジョイスティックの傾動角度は、当該移動方向へのエンドエフェクタ１２の目標の速さを表し得る。

　制御装置２０は、有線通信、無線通信又はこれらの組み合わせを介して、ロボット１０と接続される。いかなる有線通信及び無線通信が用いられてもよい。制御装置２０は、ユーザインタフェース３０から受信する信号に含まれる指令に従って、ロボット１０の動作を制御する。制御装置２０は、ユーザインタフェース３０から受信する信号に含まれるロボット１０の諸元に関する情報を用いて、ロボット１０の制御プログラムに含まれる要素を決定する。限定されないが、本実施の形態では、制御装置２０は、ロボット１０への電力供給も制御する。

　図３に示すように、制御装置２０は、情報処理装置１００と、電力制御回路２００とを含む。本実施の形態では、情報処理装置１００と電力制御回路２００とは、１つの制御装置２０に含まれるが、互いから離れた別個の装置であり、有線通信、無線通信又はこれらの組み合わせを介して互いに接続されてもよい。いかなる有線通信及び無線通信が用いられてもよい。

　電力制御回路２００は、アンプ、コンバータ及びインバータの１つ以上を含み、ロボット１０へ供給する電力を制御する。電力制御回路２００は、情報処理装置１００から受け取る指令に含まれる電流値に従って、ロボットアーム１１の駆動装置Ｄ１からＤ６のモータＭ１からＭ６に電流を供給する。

　情報処理装置１００は、回路Ｃを含み、回路ＣはプロセッサＰ及びメモリＭを含む。回路Ｃは、処理回路を含み得る。例えば、情報処理装置１００は、電子回路基板、電子制御ユニット、マイクロコンピュータ、パーソナルコンピュータ、ワークステーション、スマートフォン若しくはタブレットなどのスマートデバイス、又は、その他の電子機器等であってもよい。メモリＭは、プロセッサＰが実行するプログラム、及び各種データを記憶する。メモリＭは、ロボット１０の制御プログラムを記憶する。

　情報処理装置１００は、記憶手段として、ハードディスクドライブ（ＨＤＤ：Hard Disk Drive）又は固体ドライブ（ＳＳＤ：Solid State Drive）をさらに含んでもよい。メモリＭは、揮発性メモリであるＲＡＭ（Random Access Memory）と不揮発性メモリであるＲＯＭ（Read-Only Memory）とを含む。

　プロセッサＰは、ＲＡＭ及びＲＯＭと一緒にコンピュータシステムを形成する。コンピュータシステムは、プロセッサＰがＲＡＭをワークエリアとして用いてＲＯＭに記録されたプログラムを実行することによって、情報処理装置１００の機能を実現してもよい。情報処理装置１００の機能の一部又は全部は、上記コンピュータシステムにより実現されてもよく、電子回路又は集積回路等の専用のハードウェア回路により実現されてもよく、上記コンピュータシステム及びハードウェア回路の組み合わせにより実現されてもよい。情報処理装置１００は、単一の装置による集中制御により各処理を実行するように構成されてもよく、複数の装置の協働による分散制御により各処理を実行するように構成されてもよい。

　限定されないが、例えば、プロセッサＰは、ＣＰＵ（中央処理装置：Central Processing Unit）、ＭＰＵ（Micro Processing Unit）、ＧＰＵ（Graphics Processing Unit）、マイクロプロセッサ（microprocessor）、プロセッサコア（processor core）、マルチプロセッサ（multiprocessor）、ＡＳＩＣ（Application-Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）等を含んでもよい。プロセッサＰは、ＩＣ（集積回路）チップ、ＬＳＩ（Large Scale Integration）等に形成された論理回路又は専用回路によって各処理を実現してもよい。複数の処理は、１つ又は複数の集積回路により実現されてもよく、１つの集積回路により実現されてもよい。

　情報処理装置１００は、ユーザインタフェース３０から受信する信号に含まれるエンドエフェクタ１２の目標速度を、関節ＪＴ１からＪＴ６それぞれの目標速度に変換する。関節ＪＴ１からＪＴ６がそれぞれ当該目標速度で動作することによって、ロボットアーム１１は、エンドエフェクタ１２を目標速度で移動することができる。関節ＪＴの速度は、関節ＪＴの動作の方向と、当該方向への関節ＪＴの動作の速さとを含む。回転関節の回転角度及び直動関節の直動位置はいずれも、当該関節を基準とする位相で表され、当該関節の速度は、位相を時間微分して得られる。以下において、関節ＪＴの位相をθで表し、関節ＪＴの速度を速度θvaで表す。関節ＪＴ１からＪＴ６の速度θva1からθva6を、速度ベクトルθv（θv＝［θva1，θva2，θva3，θva4，θva5，θva6］）で表す。

　メモリＭに記憶された制御プログラムは、ヤコビ行列と呼ばれる行列Ｊを含む。目標速度の変換には、ヤコビ行列Ｊの逆行列Ｊ^-1が用いられる。ヤコビ行列Ｊは、関節ＪＴ１からＪＴ６の微小変位をエンドエフェクタ１２の位置及び姿勢の微小変位に変換する行列である。

　図１及び図２に示すように、リンクＬ６のメカニカルインタフェース面の位置及び姿勢が、エンドエフェクタ１２の位置及び姿勢として表される。メカニカルインタフェース面の位置及び姿勢は、ロボット１０を基準とするロボット座標系Σ（Ｘ，Ｙ，Ｚ）での座標で表される。限定されないが、本実施形態では、ロボット座標系Σ（Ｘ，Ｙ，Ｚ）は、基台１３と基台１３の支持面Ｓとの接続部分に原点を有し、支持面Ｓに沿って互いに直交して延びるＸ軸及びＹ軸と、Ｘ軸及びＹ軸に垂直なＺ軸とを含む。

　メカニカルインタフェース面の位置は、メカニカルインタフェース面を基準とするメカニカルインタフェース座標系Σ（Ｘm，Ｙm，Ｚm）の原点のロボット座標系Σ（Ｘ，Ｙ，Ｚ）での座標であるｘ、ｙ及びｚで表される。例えば、メカニカルインタフェース座標系Σ（Ｘm，Ｙm，Ｚm）の原点は、メカニカルインタフェース面の中心に位置する。メカニカルインタフェース面の姿勢は、メカニカルインタフェース座標系Σ（Ｘm，Ｙm，Ｚm）がロボット座標系Σ（Ｘ，Ｙ，Ｚ）に対してなす角度ｒ_O、ｒ_A及びｒ_Tで表される。角度ｒ_O、ｒ_A及びｒ_Tは、ロール角、ピッチ角及びヨー角で表されてもよく、オイラー角で表されてもよい。エンドエフェクタ１２の速度は、要素ｘ_v、ｙ_v、ｚ_v、ω_ｘ、ω_y及びω_zにより表され、例えば、速度ベクトルＶ＝［ｘ_v，ｙ_v，ｚ_v，ω_x，ω_y，ω_z］で表される。［ｘ_v，ｙ_v，ｚ_v］は、並進速度ベクトルを表し、ｘ_v、ｙ_v及びｚ_vはそれぞれ、ロボット座標系Σ（Ｘ，Ｙ，Ｚ）でのＸ軸、Ｙ軸及びＺ軸方向の速度を表す。［ω_x，ω_y，ω_z］は、角速度ベクトルを表し、ω_x、ω_y及びω_zはそれぞれ、ロボット座標系Σ（Ｘ，Ｙ，Ｚ）でのＸ軸、Ｙ軸及びＺ軸周りの角速度を表す。エンドエフェクタ１２の速度ベクトルＶと、関節ＪＴ１からＪＴ６の速度ベクトルθvと、ヤコビ行列Ｊとは、θv＝Ｊ^-1Ｖの関係式を満たす。

　情報処理装置１００は、制御プログラムに従って動作して、上記の関係式とエンドエフェクタ１２の目標速度とを用いて、関節ＪＴ１からＪＴ６それぞれ目標速度を決定する。さらに、情報処理装置１００は、目標速度で関節ＪＴ１からＪＴ６を動作させるためにモータＭ１からＭ６に印加する電流値を決定し、電力制御回路２００に出力する。情報処理装置１００は、θv＝Ｊ^-1Ｖの関係式に基づく数値制御をロボット１０に行う。つまり、情報処理装置１００は、ヤコビ行列を用いた分解速度制御を行う。

　なお、ヤコビ行列Ｊは、その要素に、関節ＪＴの位相θを変数として含む。情報処理装置１００は、フィードバック情報に含まれる回転センサＥの検出結果を用いて、関節ＪＴ１からＪＴ６の位相θ1、θ2、θ3、θ4、θ5及びθ6を算出し、算出した位相をヤコビ行列Ｊの算出に用いる。

　さらに、ヤコビ行列Ｊは、ロボット１０の諸元に依存する行列であり、ロボット１０の諸元の情報を用いて算出することができる。情報処理装置１００は、ユーザインタフェース３０等の他の装置からロボット１０の諸元に関する情報を受信すると、当該情報をメモリＭに記憶する。情報処理装置１００は、制御プログラムに従って動作して、フィードバック情報に基づく関節ＪＴ１からＪＴ６の位相θ1、θ2、θ3、θ4、θ5及びθ6と、ロボット１０の諸元に関する情報とを用いて、ヤコビ行列Ｊを算出しメモリＭに記憶する。さらに、情報処理装置１００は、ヤコビ行列Ｊの逆行列Ｊ^-1にエンドエフェクタ１２の目標速度を適用して、関節ＪＴ１からＪＴ６の目標速度を決定する。

　このため、情報処理装置１００は、制御装置２０と接続されるロボット１０が変更されても、変更後のロボット１０の諸元に関する情報を取得することによって、変更後のロボット１０に適合するヤコビ行列Ｊを算出し、変更後のロボット１０に応じた分解速度制御を行うことができる。

　また、ロボット１０のロボットアーム１１には、ヤコビ行列Ｊを用いた演算に従ってロボットアーム１１を制御できない姿勢である特異点が存在する。ロボットアーム１１の姿勢が特異点にある場合、ロボットアーム１１は、ユーザインタフェース３０から出力される目標速度に従った速度でエンドエフェクタ１２を移動できない。例えば、特異点では、ロボットアーム１１は、エンドエフェクタ１２のある方向への速度を生成できなくなる。このため、特異点の近傍では、ロボットアーム１１は、当該方向へのエンドエフェクタ１２の速度を生成するために、関節ＪＴ１からＪＴ６のいずれか又はすべてに過大な速度を必要とする。そこで、情報処理装置１００は、関節ＪＴ１からＪＴ６に要求される過大な速度をエラーとして決定し、関節ＪＴ１からＪＴ６の動作を停止する。又は、特異点の近傍では、ロボットアーム１１は、上記方向へのエンドエフェクタ１２の速度を生成するために十分な関節ＪＴ１からＪＴ６の速度を達成できず、エンドエフェクタ１２の軌道が、本来指令された軌道から逸脱する場合がある。また特異点では、ロボットアーム１１は、エンドエフェクタ１２等の一部分の移動を停止し、停止部分の位置を保った状態で動作するようなセルフモーションを行い得る。ロボットアーム１１は、特異点の近傍では、特異点に近づくに従って、目標速度に対して急激に変化する速度でエンドエフェクタ１２を移動することがある。

　図４から図７は、特異点にあるロボットアーム１１の姿勢の一例を示す概念図である。図４は、関節ＪＴ１からＪＴ６が一直線に並ぶロボットアーム１１が伸びきった姿勢を示す。図５から図７は、関節ＪＴ１からＪＴ６のうちの２つ以上が一直線上に並ぶロボットアーム１１の姿勢を示す。図５では、ロボットアーム１１の肘関節に相当する関節ＪＴ３を含んだ関節ＪＴ２からＪＴ４が一直線上に並ぶ。このようなロボットアーム１１の姿勢は、肘特異姿勢とも呼ばれる。図６では、ロボットアーム１１の手首曲げ関節に相当する関節ＪＴ５を含んだ関節ＪＴ３からＪＴ６が一直線上に並ぶ。このようなロボットアーム１１の姿勢は、手首特異姿勢とも呼ばれる。図７では、ロボットアーム１１の肩関節に相当する関節ＪＴ１及びＪＴ２と関節ＪＴ５とが一直線上に並ぶ。このようなロボットアーム１１の姿勢は、肩特異姿勢とも呼ばれる。

　特異点では、ヤコビ行列Ｊの逆行列が存在しなく、ヤコビ行列Ｊの行列式detＪが０になる。特異点に近づくに従って、行列式detＪが０に近づく。ヤコビ行列Ｊの逆行列は、１／detＪを全ての要素に含むため、行列式detＪが０に近づくに従って、関節ＪＴの速度が発散し、エンドエフェクタ１２の速度が意図しない速度になり得る。

　［情報処理装置１００の動作］
　情報処理装置１００は、特異点及びその近傍でのロボットアーム１１の意図しない動作を低減する制御を行う。図８から図１２は、実施の形態に係る情報処理装置１００の特異点に関する処理の一例を示すフローチャートである。

　以下のステップＳ１及びＳ２処理は、ユーザインタフェース３０から出力されるエンドエフェクタ１２の速度の指令に従ったロボット１０の制御の前段階の処理である。ステップＳ３以降の処理は、ロボット１０の制御に関する処理である。

　ステップＳ１において、ロボットシステム１のユーザが、ユーザインタフェース３０にロボット１０の諸元に関する情報を入力する。ユーザインタフェース３０は当該情報を送信し、制御装置２０は当該情報を受信する。

　次いで、ステップＳ２において、制御装置２０の情報処理装置１００は、ロボット１０の諸元に関する情報をメモリＭに記憶する。

　ステップＳ３において、ロボットシステム１の操作者は、ロボット１０を手動操縦するためにユーザインタフェース３０に操作を入力する。ユーザインタフェース３０は、当該操作によって指令されるエンドエフェクタ１２の速度指令値を示す信号を制御装置２０に送信する。情報処理装置１００は、当該信号からエンドエフェクタ１２の目標速度である速度指令値を取得する。本例では、速度指令値は、速度ベクトルＶで表される。速度指令値が、速度ベクトルＶでない場合、情報処理装置１００が速度指令値を速度ベクトルＶに変換してもよい。例えば、情報処理装置１００は、ユーザインタフェース３０から受信する信号から、所定のサンプリング周期毎にエンドエフェクタ１２の速度指令値を取得してもよい。

　次いで、ステップＳ４において、情報処理装置１００は、電力制御回路２００から関節ＪＴ１からＪＴ６のモータＭ１からＭ６それぞれに印加された電流値と、モータＭ１からＭ６それぞれから回転センサＥの検出結果とを、フィードバック情報として受け取る。例えば、情報処理装置１００は、所定のサンプリング周期毎にフィードバック情報を取得してもよい。

　次いで、ステップＳ５において、情報処理装置１００は、フィードバック情報に含まれる回転センサＥの検出結果を用いて、関節ＪＴ１からＪＴ６の位相θ1、θ2、θ3、θ4、θ5及びθ6を算出する。情報処理装置１００は、位相θ1、θ2、θ3、θ4、θ5及びθ6と、メモリＭに記憶されるロボット１０の諸元に関する情報とを用いてヤコビ行列Ｊを算出する。ヤコビ行列Ｊは、第１ヤコビ行列の一例である。このため、ステップＳ４とステップＳ５とは、逆の順序となってもよい。

　次いで、ステップＳ６において、情報処理装置１００は、ヤコビ行列Ｊを正規化する。

　ここで、エンドエフェクタ１２の速度ベクトルＶ＝［ｘ_v，ｙ_v，ｚ_v，ω_x，ω_y，ω_z］は、以下の式１に示すように、速度ベクトルＶ_Neに正規化できる。関節ＪＴ１からＪＴ６の速度ベクトルθ_v＝［θva1，θva2，θva3，θva4，θva5，θva6］は、以下の式２に示すように、速度ベクトルθ_vNに正規化できる。ｖi_maxは、速度ベクトルＶのｉ番目の要素ｖiに予め設定される最大値である。θvai_maxは、速度ベクトルθ_vのｉ番目の要素θvaiに予め設定される最大値である。ｖi_max及びθvai_maxは、ロボット１０の諸元に基づき設定されてもよい。ｄ_ｖ及びｄ_θvはそれぞれ、１≦ｄ_ｖ≦ｍ及び１≦ｄ_θv≦ｎを満たす補正係数である。ｍは、速度ベクトルＶの次元数であり、本例では、６である。ｎは、速度ベクトルθ_vの次元数であり、本例では、６である。ロボットアーム１１は、非冗長マニピュレータである。以降、ｍ＝ｎとし、「ｍ」及び「ｎ」をいずれも、「ｎ」で表す。速度ベクトルＶ及び速度ベクトルＶ_Neは、移動指令値の一例であり、速度ベクトルθ_v及び速度ベクトルθ_vNは、動作指令値の一例である。

　なお、速度ベクトルＶにおいて、［ｘ_v，ｙ_v，ｚ_v］は、並進ベクトルｖlと表すことができ、［ω_x，ω_y，ω_z］は、角速度ベクトルωと表すことができる。並進ベクトルｖlと角速度ベクトルωとは互いに単位系で異なる。このため、以下の式３のように、速度ベクトルＶは、並進ベクトルｖlの最大ノルム||ｖl||_maxと角速度ベクトルωの最大ノルム||ωl||_maxとを用いて、速度ベクトルＶ_Nに正規化されてもよい。本例では、補正係数ｄ_ｖは、１≦ｄ_ｖ≦２を満たす。Ｉ₃は、３行×３列の単位行列である。

　そして、正規化速度ベクトルＶ_Ne又はＶ_Nと、正規化速度ベクトルθ_vNとを用いて、ヤコビ行列Ｊを正規化できる。ヤコビ行列Ｊは、Ｖ＝Ｊθ_vの関係を満たす。上記関係と式２と式３とを用いると、以下の式４に示すように、ヤコビ行列Ｊをヤコビ行列Ｊ_Nに正規化できる。正規化ヤコビ行列Ｊ_Nは、Ｗ_vNＪＷ_θvN ^-1である。以下において、正規化速度ベクトルを、正規化速度指令値、正規化速度指令、速度指令値又は速度指令と表現する場合もある。

　エンドエフェクタ１２の速度ベクトルＶの上限値が適切に設定されている場合、正規化速度ベクトルＶ_Nのノルム||Ｖ_N||が、||Ｖ_N||≦1を満たすので、各関節の関節速度の最大値が適切に設定されていれば、正規化速度ベクトルθ_vNのノルム||θ_vN||が、||θ_vN||≦1を満たし、ロボットアーム１１に、ユーザインタフェース３０から出力されるエンドエフェクタ１２の速度指令値に従って動作させることができる。

　よって、情報処理装置１００は、ステップＳ６において、式１又は式３に従って、エンドエフェクタ１２の速度指令値Ｖを正規化して、正規化速度ベクトルＶ_Ne又はＶ_Nを算出する。さらに、情報処理装置１００は、正規化速度ベクトルＶ_Ne又はＶ_Nとヤコビ行列Ｊとを、Ｊ_N＝Ｗ_vNＪＷ_θvN ^-1のような関係に適用して、正規化ヤコビ行列Ｊ_Nを算出しメモリＭに記憶する。

　次いで、ステップＳ７において、以下の式５に示すように、情報処理装置１００は、正規化ヤコビ行列Ｊ_Nを特異値分解して、３つの行列Ｕv、Σ及びＶ_θを含む行列集合Ｍ１を生成する。行列Ｕvは、正規直交系であり、行列Ｕvの列ベクトルｕ_vi（ｉ＝１，・・・，ｎ）は、特異値分解で定義されるエンドエフェクタ１２の速度空間の基底ベクトルである。行列Ｖ_θは、正規直交系であり、行列Ｖ_θの列ベクトルである転置行列Ｖ_θ ^Ｔの行ベクトルｖ_θi（ｉ＝１，・・・，ｎ）は、特異値分解で定義される複数の関節ＪＴの速度空間の基底ベクトルである。行列Σは、対角行列であり、その対角成分が特異値σi（ｉ＝１，・・・，ｎ）である。行列Σにおいて、対角成分は、σ₁＞σ₂＞・・・＞σ_nと降順に並ぶ。列ベクトルｕ_viについての「ｎ」は、エンドエフェクタ１２の速度の次元数であり、行ベクトルｖ_θiについての「ｎ」は、複数の関節ＪＴの速度の次元数である。ロボットアーム１１の姿勢が特異点に近くなるほど、最小特異値σ_nが小さくなる。行列集合Ｍ１は、第２ヤコビ行列の一例である。行列Ｖ_θ ^Ｔは第１行列の一例であり、行列Ｕvは第２行列の一例であり、行列Σは、第１特異行列の一例である。ベクトルｖ_θiは第１基底ベクトルの一例であり、ベクトルｕ_viは第２基底ベクトルの一例である。

　なお、ロボットアーム１１の姿勢が特異点に該当しない状態である非特異状態の場合、正規化ヤコビ行列Ｊ_Nの逆行列、つまり、行列集合Ｍ1の逆行列は、以下の式６のように表される。行列集合Ｍ1の逆行列は、第２逆ヤコビ行列の一例である。行列Ｖ_θは第１変換行列の一例であり、行列Ｕ_V ^Ｔは、第２変換行列の一例であり、行列Σ^-1は、第１変換特異行列の一例である。

　エンドエフェクタ１２の速度指令値Ｖは、式１を用いて、速度ベクトルＶ_Nに正規化できる。正規化速度ベクトルＶ_Nは、そのノルム||Ｖ_N||と、正規化速度ベクトルＶ_Nの単位ベクトルｕ_Nとを用いて、以下の式７に示すように表現することができる。

　速度指令値Ｖに従ってエンドエフェクタ１２を移動するための関節ＪＴ１からＪＴ６の速度ベクトルθ_vを正規化して得られる速度ベクトルθ_vNは、式６及び式７を用いて、以下の式８のように表される。

　ロボットアーム１１の姿勢が特異点に近づくと、正規化速度ベクトルθ_vNのノルム||θ_vN||は、||θ_vN||≦１を満たさない可能性がある。このため、式８の正規化速度ベクトルθ_vNを以下の式９のように、正規化速度ベクトルθ_vN1に変更すると、||θ_vN1||≦１が保証される。よって、ロボットアーム１１の動作の指令値を生成するために、正規化速度ベクトルθ_vNの代わりに、正規化速度ベクトルθ_vN1を用いることができ、これにより、式８に従った分解速度制御が、式９に従った分解速度制御に変更され得る。

　次いで、ステップＳ８において、情報処理装置１００は、正規化指令速度Ｖ_Nのノルム||Ｖ_N||が閾値ε_V以上であるか否かを判定する。情報処理装置１００は、ノルム||Ｖ_N||が閾値ε_V以上である場合（ステップＳ８でＹｅｓ）にステップＳ９へ進み、ノルム||Ｖ_N||が閾値ε_V未満である場合（ステップＳ８でＮｏ）にステップＳ１０へ進む。ノルム||Ｖ_N||が閾値ε_V未満であるように、正規化速度ベクトルＶ_Nの大きさが小さ過ぎる場合、式７に基づく、単位ベクトルｕ_Nを算出する正規化計算であるｕ_N＝Ｖ_N／||Ｖ_N||が数値的に不安定になる。このため、情報処理装置１００は、ノルム||Ｖ_N||が閾値ε_V未満となるような速度指令値を受け付けずに０とみなす。

　ステップＳ１０では、情報処理装置１００は、エンドエフェクタ１２の速度指令値Ｖを０に設定する、状態０に従った分解速度制御を行う。図９に示すように、ステップＳ１０は、ステップＳ１００１からＳ１００８を含み、その詳細は後述する。

　また、ステップＳ９において、情報処理装置１００は、ロボットアーム１１の姿勢が特異点又は特異点の近傍の領域にあるか否かを判定する。情報処理装置１００は、ロボットアーム１１の姿勢が特異点又は特異点の近傍の領域にある場合（ステップＳ９でＹｅｓ）にステップＳ１１へ進み、ロボットアーム１１の姿勢が特異点にも特異点の近傍の領域にもない場合（ステップＳ９でＮｏ）にステップＳ１２へ進む。

　ロボットアーム１１の姿勢が特異点に近づくに従って、式８の１/σ_nは大きくなり、やがては、ヤコビ行列の逆行列の数値計算が不可能になる。このため、ステップＳ９では、情報処理装置１００は、最小の特異値σ_nと閾値ε_σnとの比較結果に基づく処理を行う。情報処理装置１００は、σ_n＜ε_σnである場合、ロボットアーム１１の姿勢が特異点又は特異点の近傍の領域にあると決定し、σ_n≧ε_σnである場合、ロボットアーム１１の姿勢が特異点にも特異点の近傍の領域にもないと決定する。閾値ε_σnは、第１閾値の一例である。

　閾値ε_σnは、最小特異値σ_nに対して０以外の値で設定される閾値である。例えば、閾値ε_σnは、式８のように表される正規化速度ベクトルθ_vNのノルム||θ_vN||を、||θ_vN||＞１とするような最小特異値σ_nの経験則に基づき設定されてもよい。

　閾値ε_σnは、κ＝σ₁／σ_nで定義される条件数κに基づき設定されてもよい。例えば、条件数κの逆数１／κ（＝σ_n／σ₁）に閾値１／κ₀を設定することができる。閾値ε_σnは、１／κ₀を基準に設定でき、例えば、ε_σn＝σ₁／κ₀に設定できる。

　ステップＳ１２では、情報処理装置１００は、通常状態の分解速度制御である状態ＮＳ（Non Singular）に従った分解速度制御を行う。図１０に示すように、ステップＳ１２は、ステップＳ１２０１からＳ１２０９を含み、その詳細は後述する。

　ステップＳ１１において、情報処理装置１００は、式８に基づき、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのうちのエンドエフェクタ１２の速度空間の基底ベクトルｕ_vnの方向の寄与度ｗ_nを、以下の式１０に示すように算出する。ベクトルｕ_Nは、正規化速度ベクトルＶ_Nの単位ベクトルである。寄与度ｗ_nがとり得る範囲は、０≦ｗ_n≦１である。寄与度ｗ_nは、エンドエフェクタ１２の指令速度の方向が、特異点にある状態のロボットアーム１１によって実現できないエンドエフェクタ１２の速度の方向成分に寄与する程度を示す。寄与度ｗ_nが１に近づく程、寄与する程度が大きい。

　次いで、ステップＳ１３において、情報処理装置１００は、寄与度ｗ_nが閾値ε_wn以上であるか否かを判定する。情報処理装置１００は、寄与度ｗ_nが閾値ε_wn以上である場合（ステップＳ１３でＹｅｓ）にステップＳ１４に進み、寄与度ｗ_nが閾値ε_wn未満である場合（ステップＳ１３でＮｏ）にステップＳ１５に進む。閾値ε_wnは、０＜ε_wn＜１の範囲で設定される。閾値ε_wnは、第３閾値の一例である。

　ステップＳ１４では、情報処理装置１００は、状態ＳＣ（Singularity Consistent）に従った分解速度制御を行う。状態ＳＣは、特異点又は特異点の近傍の領域での寄与度が閾値以上である状態の分解速度制御である。図１１に示すように、ステップＳ１４は、ステップＳ１４０１からＳ１４２９を含み、その詳細は後述する。

　ステップＳ１５では、情報処理装置１００は、状態Ｐ（Perpendicular）に従った分解速度制御を行う。状態Ｐは、特異点又は特異点の近傍の領域での寄与度が閾値よりも小さい状態の分解速度制御である。図１２に示すように、ステップＳ１５は、ステップＳ１５０１からＳ１５１１を含み、その詳細は後述する。

　図９を使用して、状態０に関するステップＳ１０の詳細を説明する。まず、ステップＳ１００１では、情報処理装置１００は、エンドエフェクタ１２の速度指令値Ｖを０に設定し、関節ＪＴ１からＪＴ６の動作を停止する速度指令値を決定し、関節ＪＴ１からＪＴ６のモータＭ１からＭ６の回転を停止する指令値を電力制御回路２００に出力する。

　次いで、ステップＳ１００２では、電力制御回路２００は、モータＭ１からＭ６の回転を停止することで、ロボットアーム１１の動作を停止する。

　次いで、ステップＳ１００３では、情報処理装置１００は、ステップＳ３と同様に、ユーザインタフェース３０から受信する信号から、エンドエフェクタ１２の新たな速度指令値を取得する。

　次いで、ステップＳ１００４では、情報処理装置１００は、ステップＳ４と同様に、ロボット１０の新たなフィードバック情報を取得する。

　次いで、ステップＳ１００５では、情報処理装置１００は、ステップＳ５と同様に、新たな速度指令値及び新たなフィードバック情報を用いて、ヤコビ行列Ｊを算出する。

　次いで、ステップＳ１００６では、情報処理装置１００は、ステップＳ６と同様に、ヤコビ行列Ｊを正規化する。

　次いで、ステップＳ１００７では、情報処理装置１００は、ステップＳ７と同様に、正規化ヤコビ行列Ｊ_Nを特異値分解する。

　次いで、ステップＳ１００８では、情報処理装置１００は、ステップＳ８と同様に、正規化指令速度Ｖ_Nのノルム||Ｖ_N||が閾値ε_V以上であるか否かを判定する。情報処理装置１００は、ノルム||Ｖ_N||が閾値ε_V以上である場合（ステップＳ１００８でＹｅｓ）にステップＳ９へ進み、ノルム||Ｖ_N||が閾値ε_V未満である場合（ステップＳ１００８でＮｏ）にステップＳ１００１に戻り、以降の処理を繰り返す。

　図１０を使用して、状態ＮＳに関するステップＳ１２の詳細を説明する。まず、ステップＳ１２０１では、情報処理装置１００は、状態ＮＳの分解速度制御に従った処理により、関節ＪＴ１からＪＴ６の速度指令値を生成する。情報処理装置１００は、エンドエフェクタ１２の速度指令値Ｖを、式３に従って速度ベクトルＶ_Nに正規化する。さらに、情報処理装置１００は、正規化速度ベクトルＶ_Nを式９に適用して、関節ＪＴ１からＪＴ６の正規化速度ベクトルθ_vN1を算出する。情報処理装置１００は、正規化速度ベクトルθ_vN1を非正規形に戻すことで、関節ＪＴ１からＪＴ６の速度指令値θ_vを算出する。これにより、関節ＪＴ１からＪＴ６の速度が関節ＪＴ１からＪＴ６に設定される上限速度以下に保証される。

　なお、情報処理装置１００は、θ_v＝Ｊ^-1Ｖの関係式に従って、エンドエフェクタ１２の速度指令値Ｖを、ヤコビ行列Ｊの逆行列に適用することで、関節ＪＴ１からＪＴ６の速度指令値θ_vを生成してもよい。

　次いで、ステップＳ１２０２では、情報処理装置１００は、フィードバック情報に含まれる回転センサＥの検出結果から得られる関節ＪＴ１からＪＴ６の速度と速度指令値θ_vとの差分と、フィードバック情報に含まれるモータＭ１からＭ６の電流値とを用いて、速度指令値θ_vに従って関節ＪＴ１からＪＴ６に動作させるためにモータＭ１からＭ６に印加する電流値を決定する。情報処理装置１００は、決定した電流値の指令を電力制御回路２００に出力し、電力制御回路２００は、当該電流値に従ってモータＭ１からＭ６を駆動し、ロボットアーム１１に動作させる。

　次いで、ステップＳ１２０３では、情報処理装置１００は、ステップＳ１００３と同様に、ユーザインタフェース３０から受信する信号から、エンドエフェクタ１２の新たな速度指令値を取得する。

　次いで、情報処理装置１００は、ステップＳ１２０４からＳ１２０７において、新たな速度指令値を用いて、ステップ１００４からＳ１００７と同様の処理を行う。

　次いで、ステップＳ１２０８では、情報処理装置１００は、ステップＳ１００８と同様に、正規化指令速度Ｖ_Nのノルム||Ｖ_N||が閾値ε_V以上であるか否かを判定する。情報処理装置１００は、ノルム||Ｖ_N||が閾値ε_V以上である場合（ステップＳ１２０８でＹｅｓ）にステップＳ１２０９へ進み、ノルム||Ｖ_N||が閾値ε_V未満である場合（ステップＳ１２０８でＮｏ）にステップＳ１０のステップＳ１００１に進む。

　次いで、ステップＳ１２０９では、情報処理装置１００は、ステップＳ９と同様に、ロボットアーム１１の姿勢が特異点又は特異点の近傍の領域にあるか否かを判定する。情報処理装置１００は、ロボットアーム１１の姿勢が特異点又は特異点の近傍の領域にある場合（ステップＳ１２０９でＹｅｓ）にステップＳ１１へ進み、ロボットアーム１１の姿勢が特異点にも特異点の近傍の領域にもない場合（ステップＳ１２０９でＮｏ）にステップＳ１２０１へ戻り、以降の処理を繰り返す。

　図１１を使用して、状態ＳＣに関するステップＳ１４の詳細を説明する。まず、ステップＳ１４０１において、情報処理装置１００は、状態ＳＣの分解速度制御に従った処理を行う。情報処理装置１００は、式６に示される正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1を変換し、以下の式１１に示すような新たな行列Ｊ_εNを算出する。行列Ｊ_εNも正規化された行列である。具体的には、情報処理装置１００は、逆行列Ｊ_N ^-1に含まれる特異値要素１／σ₁，・・・，１／σ_n間の比率を維持するように、式６の逆行列Ｊ_N ^-1内の行列Σ^-1のすべての要素をσn／ε_σn倍する。要素１／σ_i（ｉ＝１，・・・，ｎ）は、σ_n／σ_iε_σnに変更される。さらに、σ_n／σ_iε_σnを１／σ_i ^’と表す。行列Ｊ_εNは、第３逆ヤコビ行列の一例であり、行列Ｊ_εNの逆行列は、第３ヤコビ行列の一例である。逆行列Ｊ_N ^-1から行列Ｊ_εNへの変換は、第１変換の一例であり、σn／ε_σnは、第１比率の一例である。

　ここで、例えば、情報処理装置１００は、式１１に基づき、式９と同様に、以下の式１２に示すような分解速度制御に従った処理を行うことができる。情報処理装置１００は、特異点又はその近傍領域において、エンドエフェクタ１２の速度指令値に対応する正規化速度ベクトルＶ_Nに応じて、関節ＪＴ１からＪＴ６の速度指令値に対応する正規化速度ベクトルθ_vNAを生成できる。そして、正規化速度ベクトルθ_vNAのノルム||θ_vNA||は、||θ_vNA||≦１に保証される。

　また、ロボットアーム１１の姿勢が特異点に該当する状態である特異状態の場合、式１２の行列Ｊ_εNは、以下の式１３に示すような行列となる。これは、ロボットアーム１１に特異点におけるセルフモーションを生じさせる零空間に相当する。

　式１２に従った分解速度制御において、ノルム||Ｊ_εNｕ_N||が過小になると、正規化が数値計算上不安定になり得る。これは、ロボットアーム１１が特異状態に近づくことで、式１２の１／σ_i ^’（ｉ＝１，・・・，ｎ－１）が０に近づき、寄与度ｗ_nが０に近づく状態に相当する。例えば、特異状態において寄与度ｗ_nが０である場合、ノルム||Ｊ_εNｕ_N||は０になる。

　そこで、ステップＳ１４０２において、情報処理装置１００は、正規化行列Ｊ_εNとエンドエフェクタ１２の正規化指令速度ベクトルＶ_Nとのノルム||Ｊ_εNｕ_N||が閾値κ_θ以上であるか否かを判定する。閾値κ_θは、エンドエフェクタ１２の正規化速度ベクトルＶ_Nの単位ベクトルｕ_Nと正規化行列Ｊ_εNとの積のノルムに設定される閾値である。情報処理装置１００は、ノルム||Ｊ_εNｕ_N||が閾値κ_θ以上である場合（ステップＳ１４０２でＹｅｓ）、ステップＳ１４１０へ進み、ノルム||Ｊ_εNｕ_N||が閾値κ_θ未満である場合（ステップＳ１４０２でＮｏ）、ステップＳ１４２０へ進む。閾値κ_θは、第２閾値の一例であり、ノルム||Ｊ_εNｕ_N||は、第２ノルムの一例である。

　ステップＳ１４１０において、情報処理装置１００は、状態ＳＣ－１の分解速度制御に従った処理を行う。情報処理装置１００は、行列Ｊ_εNを用いる式１２に示される分解速度制御に従って、関節ＪＴ１からＪＴ６の速度指令値を生成する。情報処理装置１００は、ステップＳ１２０１と同様に、エンドエフェクタ１２の速度指令値Ｖを速度ベクトルＶ_Nに正規化する。さらに、情報処理装置１００は、正規化速度ベクトルＶ_Nを式１２に適用して、関節ＪＴ１からＪＴ６の正規化速度ベクトルθ_vNAを算出する。式１１の行列Ｊ_εNにおける対角要素である特異値要素は、ロボットアーム１１が特異状態に近づいても発散しない。そして、速度ベクトルθ_vNAのノルム||θ_vNA||は、||θ_vNA||≦１を満たす。

　さらに、式１１の行列Ｊ_εNでは、対角要素である特異値要素間の比率が、正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1に含まれる特異値要素間の比率を維持する。このため、正規化速度ベクトルθ_vNAに従って関節ＪＴ１からＪＴ６が動作することによってロボットアーム１１がエンドエフェクタ１２を移動する方向は、状態ＮＳの分解速度制御のステップＳ１２０１で算出された正規化速度ベクトルθ_vNに従って関節ＪＴ１からＪＴ６が動作することによってロボットアーム１１がエンドエフェクタ１２を移動する方向から変化しない。よって、ロボットアーム１１は、特異点の近傍でエンドエフェクタ１２を目標とする軌道から逸脱させることなく、特異点で関節速度が上限値を超過するエラーにより急停止することなく、エンドエフェクタ１２の指令速度の方向にエンドエフェクタ１２を移動させることができる。

　次いで、ステップＳ１４１１において、情報処理装置１００は、ステップＳ１２０２と同様に、速度指令値に従って関節ＪＴ１からＪＴ６に動作させるためのモータＭ１からＭ６の印加電流値を決定する。情報処理装置１００は、決定した電流値の指令を電力制御回路２００に出力し、電力制御回路２００は、当該電流値に従ってモータＭ１からＭ６を駆動し、ロボットアーム１１に動作させる。

　次いで、ステップＳ１４１２からＳ１４１６では、情報処理装置１００は、ステップＳ１２０３からＳ１２０７と同様に、ユーザインタフェース３０から受信する信号から、エンドエフェクタ１２の新たな速度指令値を取得し、正規化ヤコビ行列Ｊ_Nを算出して特異値分解する。

　次いで、ステップＳ１４１７では、情報処理装置１００は、ステップＳ１２０８と同様に、正規化指令速度Ｖ_Nのノルム||Ｖ_N||が閾値ε_V以上であるか否かを判定する。情報処理装置１００は、ノルム||Ｖ_N||が閾値ε_V以上である場合（ステップＳ１４１７でＹｅｓ）にステップＳ１４１８へ進み、ノルム||Ｖ_N||が閾値ε_V未満である場合（ステップＳ１４１７でＮｏ）にステップＳ１０のステップＳ１００１に進む。

　ステップＳ１４１８において、情報処理装置１００は、ステップＳ１４０１と同様に、ステップＳ１４１２からＳ１４１６で算出した正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1を変換し、新たな行列Ｊ_εNを算出する。

　次いで、ステップＳ１４１９では、情報処理装置１００は、正規化行列Ｊ_εNとエンドエフェクタ１２の正規化指令速度ベクトルＶ_Nとのノルム||Ｊ_εNｕ_N||が閾値κ_θ未満であるか否かを判定する。情報処理装置１００は、ノルム||Ｊ_εNｕ_N||が閾値κ_θ未満である場合（ステップＳ１４１９でＹｅｓ）、ステップＳ１４０２に戻り、ノルム||Ｊ_εNｕ_N||が閾値κ_θ以上である場合（ステップＳ１４１９でＮｏ）、ステップＳ１４１０に戻り、以降の処理を繰り返す。

　ステップＳ１４２０において、情報処理装置１００は、状態ＳＣ－２の分解速度制御に従った処理を行う。情報処理装置１００は、式１２の代わりに、以下の式１４に示される分解速度制御に従って処理を行う。式１４では、ノルム||Ｊ_εNｕ_N||が、閾値κ_θに置き換えられることで、定数に固定される。これにより、ノルム||Ｊ_εNｕ_N||が過小になった場合の関節速度の数値計算上の不安定化が抑えられる。情報処理装置１００は、行列Ｊ_εNを用い且つノルム||Ｊ_εNｕ_N||を固定した式１４に示される分解速度制御に従って、関節ＪＴ１からＪＴ６の速度指令値を生成する。

　情報処理装置１００は、ステップＳ１４１０と同様に、エンドエフェクタ１２の速度指令値Ｖの正規化速度ベクトルＶ_Nを式１４に適用して、関節ＪＴ１からＪＴ６の正規化速度ベクトルθ_vNBを算出する。正規化速度ベクトルθ_vNBの各要素は、ノルム||Ｊ_εNｕ_N||の大きさに応じた速度を示す。正規化速度ベクトルθ_vNBの各要素は、ノルム||Ｊ_εNｕ_N||が小さくなるに従って、関節ＪＴ１からＪＴ６の速度を連続的に徐々に減少する。

　そして、正規化速度ベクトルθ_vNBのノルム||θ_vNB||は、||θ_vNB||≦１を満たす。さらに、ステップＳ１４１０と同様に、正規化速度ベクトルθ_vNBに従って関節ＪＴ１からＪＴ６が動作することによってロボットアーム１１がエンドエフェクタ１２を移動する方向は、状態ＮＳの分解速度制御のステップＳ１２０１で算出された正規化速度ベクトルθ_vNに従ったエンドエフェクタ１２の移動方向から変化しない。よって、ロボットアーム１１は、特異点の近傍でエンドエフェクタ１２を目標とする軌道から逸脱させることなく、特異点で関節速度が上限値を超過するエラーにより急停止することなく、エンドエフェクタ１２の指令速度の方向にエンドエフェクタ１２を移動させることができる。

　次いで、ステップＳ１４２１において、情報処理装置１００は、ステップＳ１４１１と同様に、速度指令値に従って関節ＪＴ１からＪＴ６に動作させるためのモータＭ１からＭ６の印加電流値を決定する。情報処理装置１００は、決定した電流値の指令を電力制御回路２００に出力し、電力制御回路２００は、当該電流値に従ってモータＭ１からＭ６を駆動し、ロボットアーム１１に動作させる。

　次いで、ステップＳ１４２２からＳ１４２６では、情報処理装置１００は、ステップＳ１４１２からＳ１４１６と同様に、ユーザインタフェース３０から受信する信号から、エンドエフェクタ１２の新たな速度指令値を取得し、正規化ヤコビ行列Ｊ_Nを算出して特異値分解する。

　次いで、ステップＳ１４２７では、情報処理装置１００は、ステップＳ１４１７と同様に、正規化指令速度Ｖ_Nのノルム||Ｖ_N||が閾値ε_V以上である場合（ステップＳ１４２７でＹｅｓ）にステップＳ１４２８へ進み、ノルム||Ｖ_N||が閾値ε_V未満である場合（ステップＳ１４２７でＮｏ）にステップＳ１０のステップＳ１００１に進む。

　ステップＳ１４２８において、情報処理装置１００は、ステップＳ１４１８と同様に、ステップＳ１４２２からＳ１４２６で算出した正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1を変換し、新たな行列Ｊ_εNを算出する。

　次いで、ステップＳ１４２９では、情報処理装置１００は、正規化行列Ｊ_εNとエンドエフェクタ１２の正規化指令速度ベクトルＶ_Nとのノルム||Ｊ_εNｕ_N||が閾値κ_θ以上であるか否かを判定する。情報処理装置１００は、ノルム||Ｊ_εNｕ_N||が閾値κ_θ以上である場合（ステップＳ１４２９でＹｅｓ）、ステップＳ１４０２に戻り、ノルム||Ｊ_εNｕ_N||が閾値κ_θ未満である場合（ステップＳ１４２９でＮｏ）、ステップＳ１４２０に戻り、以降の処理を繰り返す。

　図１２を使用して、状態Ｐに関するステップＳ１５の詳細を説明する。まず、ステップＳ１５０１において、情報処理装置１００は、ステップＳ１４０１の式１１の正規化行列Ｊ_εNを変換した行列であり且つ以下の式１５に示される新たな行列Ｊ_εNAを生成する。行列Ｊ_εNAも正規化された行列である。上記変換では、正規化行列Ｊ_εNの要素１／ε_σnが、０に置き換えられる。行列Ｊ_εNAは、第４逆ヤコビ行列の一例である。

　寄与度ｗ_nが閾値ε_wn未満であり十分に小さい状態では、エンドエフェクタ１２の指令速度の方向成分の中で、特異状態により生成できないエンドエフェクタ１２の速度の方向成分は、非常に小さい。このような速度の方向成分は、特異値要素１／ε_σnに関連する速度成分である。例えば、ステップＳ１４０１の式１１の正規化行列Ｊ_εNを用いて正規化速度ベクトルθ_vNAを生成した場合、正規化速度ベクトルθ_vNAは、エンドエフェクタ１２の指令速度の方向に従ったエンドエフェクタ１２の移動を実現できるが、エンドエフェクタ１２の速度の大きさが非常に小さい状態で実現することになる。

　一方、寄与度ｗ_nが０である場合、エンドエフェクタ１２の指令速度の方向成分の中で、特異状態により生成できないエンドエフェクタ１２の速度の方向成分は、存在しないことになる。つまり、特異値要素１／ε_σnの方向の指令速度が、存在しないことになる。このため、上記変換は、寄与度ｗ_nが十分に小さい状態において、特異状態により生成できないエンドエフェクタ１２の速度の方向成分に関連する要素１／ε_σnを０とすることで、このような方向成分を切り捨て、それにより、エンドエフェクタ１２の指令速度をそのまま実現できるようにするものである。

　式１５により、特異値要素１／ε_σnの方向の指令速度がなかったものとされる。これは、エンドエフェクタ１２の指令速度に対応する正規化速度ベクトルＶ_Nを、以下の式１６に示すような新たな速度ベクトルＶ_NAに変更することに相当する。

　次いで、ステップＳ１５０２において、情報処理装置１００は、正規化行列Ｊ_εNAを使用した以下の式１７に示される分解速度制御に従って、関節ＪＴ１からＪＴ６の速度指令値を生成する。情報処理装置１００は、ステップＳ１２０１と同様に、エンドエフェクタ１２の速度指令値Ｖの正規化速度ベクトルＶ_Nを式１７に適用して、関節ＪＴ１からＪＴ６の正規化速度ベクトルθ_vNCを算出する。

　正規化速度ベクトルθ_vNCは、エンドエフェクタ１２の指令速度（手先速度）の中で特異値要素１／ε_σnの方向の速度成分をないものとみなことによって手先速度の次元数を１つ減少させ、結果的に次元が落とされて特異状態でなくなったロボットアーム１１に対する速度指令値に相当する。このため、正規化速度ベクトルθ_vNCは、ロボットアーム１１が特異状態であっても、エンドエフェクタ１２の指令速度を、ロボットアーム１１に近似的に実現させることができる。さらに、正規化速度ベクトルθ_vNCのノルム||θ_vNC||は、||θ_vNC||≦１を満たす。よって、ロボットアーム１１は、特異点の近傍でエンドエフェクタ１２を目標とする軌道から逸脱させることなく、特異点で関節速度が上限値を超過するエラーにより急停止することなく、エンドエフェクタ１２の指令速度の方向にエンドエフェクタ１２を移動させることができる。情報処理装置１００は、ステップＳ１５０２の後、ステップＳ１５０３に進む。

　ここで、操作者によるユーザインタフェース３０の操作によりエンドエフェクタ１２の指令速度の方向が変わり、寄与度ｗ_nが閾値ε_wn以上の状態から減少して閾値ε_wn未満の状態に移行する場合がある。寄与度ｗ_nが閾値ε_wn以上の状態では、例えば、ロボットアーム１１は、ステップＳ１４０１の式１２の正規化速度ベクトルθ_vNA又はステップＳ１４２０の式１４の正規化速度ベクトルθ_vNBに従った速度でエンドエフェクタ１２を移動する。この際、寄与度ｗ_nが減少するに従って、エンドエフェクタ１２の速度が低下する。さらに、寄与度ｗ_nが閾値ε_wn未満になると、ロボットアーム１１は、式１７の正規化速度ベクトルθ_vNCに従った速度でエンドエフェクタ１２を移動する。

　上述したように、正規化速度ベクトルθ_vNCは、速度の次元数が１つ減少し且つ特異状態にないロボットアーム１１に対する速度指令値に相当し、エンドエフェクタ１２の指令速度を近似的に実現できる。このため、状態ＳＣから寄与度ｗ_nが閾値ε_wn未満になったタイミングで状態Ｐに遷移し、ロボットアーム１１のエンドエフェクタ１２がある速度で突然に移動する可能性がある。しかし、図１０から図１２から分かるように、状態ＳＣから状態Ｐに遷移するには、一旦状態０に遷移するか、状態ＮＳに遷移することが必要であり、そのように突然にエンドエフェクタ１２が動き始めることがないようになっている。さらに、情報処理装置１００は、操作者に速度の切り替わりを報知して、ユーザインタフェース３０の操作の調整を促すことができる。

　ステップＳ１５０３において、情報処理装置１００は、ステップＳ１２０２と同様に、速度指令値に従って関節ＪＴ１からＪＴ６に動作させるためのモータＭ１からＭ６の印加電流値を決定する。情報処理装置１００は、決定した電流値の指令を電力制御回路２００に出力し、電力制御回路２００は、当該電流値に従ってモータＭ１からＭ６を駆動し、ロボットアーム１１に動作させる。

　次いで、ステップＳ１５０４からＳ１５０８では、情報処理装置１００は、ステップＳ１２０３からＳ１２０７と同様に、ユーザインタフェース３０から受信する信号から、エンドエフェクタ１２の新たな速度指令値を取得し、正規化ヤコビ行列Ｊ_Nを算出して特異値分解する。

　次いで、ステップＳ１５０９では、情報処理装置１００は、ステップＳ１２０８と同様に、正規化指令速度Ｖ_Nのノルム||Ｖ_N||が閾値ε_V以上である場合（ステップＳ１５０９でＹｅｓ）にステップＳ１５１０へ進み、ノルム||Ｖ_N||が閾値ε_V未満である場合（ステップＳ１５０９でＮｏ）にステップＳ１０のステップＳ１００１に進む。

　ステップＳ１５１０において、情報処理装置１００は、ステップＳ１５０４からＳ１５０８で取得したエンドエフェクタ１２の速度指令値Ｖについて、ステップＳ１１と同様に、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのうちの最小特異値σ_nに関連する基底ベクトルｕ_vnの方向の寄与度ｗ_nを算出する。

　次いで、ステップＳ１５１１において、情報処理装置１００は、ステップＳ１５０４からＳ１５０８での特異値分解により得られた最小特異値σ_nが閾値ε_σn以上であり且つ寄与度ｗ_nが閾値ε_wn未満であるという条件が満たされるか否かを判定する。情報処理装置１００は、当該条件が満たされる場合（ステップＳ１５１１でＹｅｓ）の場合にステップＳ１２に進み、当該条件が満たされない場合（ステップＳ１５１１でＮｏ）の場合にステップＳ１５０１に戻り、以降の処理を繰り返す。上記条件が満たされる場合、エンドエフェクタ１２の速度指令値Ｖの中で最小特異値σ_nに関連する方向の速度成分が小さく、最小特異値σ_nが特異状態ではない。このため、ロボットアーム１１は、状態Ｐから抜け出すことができる。

　ステップＳ３からＳ１５により、情報処理装置１００は、状態０、状態ＮＳ、状態ＳＣ及び状態Ｐの間で分解速度制御方法を遷移させつつ、特異点の近傍でエンドエフェクタ１２を目標とする軌道から逸脱させることなく、特異点で関節速度が上限値を超過するエラーにより急停止することなく且つエンドエフェクタ１２の指令速度の方向にエンドエフェクタ１２を移動させるように、ロボットアーム１１を制御できる。

　状態０は、エンドエフェクタ１２の指令速度が閾値未満であり、エンドエフェクタ１２の速度指令値を０とみなしてロボットアーム１１を停止させた状態である。状態ＮＳは、通常の分解速度制御で、関節ＪＴの速度指令値を生成可能な状態である。状態ＳＣは、ロボットアーム１１が特異状態に近い又は特異状態にある状態に対応した分解速度制御で、関節ＪＴの速度指令値を生成可能な状態である。状態Ｐは、エンドエフェクタ１２の指令速度が特異値σ_nに対応する指令方向ｕ_vnの成分を含まないようにした状態に対応した分解速度制御で、関節ＪＴの速度指令値を生成可能な状態である。

　情報処理装置１００は、状態ＮＳと状態ＳＣとの間の分解速度制御の遷移を、最小特異値σ_nと閾値ε_σnとの関係がσ_n≧ε_σnであるかσ_n＜ε_σnであるかに応じて決定する。例えば、σ_n＜ε_σnであり且つ寄与度ｗ_nがｗ_n≧ε_wnを満たせば、情報処理装置１００は、状態ＳＣの分解速度制御への遷移を決定する。よって、状態ＮＳと状態ＳＣとの間の遷移の連続性が保たれる。さらに、情報処理装置１００は、状態ＳＣにおいて、ノルム||Ｊ_εNｕ_N||と閾値κ_θとの関係が、||Ｊ_εNｕ_N||≧κ_θであるか||Ｊ_εNｕ_N||＜κ_θであるかに応じて、２つの状態間の分解速度制御の遷移を決定する。情報処理装置１００は、２つの状態のいずれにおいても、ノルム||Ｊ_εNｕ_N||が閾値κ_θを下回らないようにして分解速度制御を行うため、２つの状態間の遷移の連続性が保たれる。

　情報処理装置１００は、状態ＮＳと状態Ｐとの間の遷移を、寄与度ｗ_nと閾値ε_wnとの関係がｗ_n≧ε_wnであるかｗ_n＜ε_wnであるかに応じて決定することができる。ｗ_n＜ε_wnであり且つ最小特異値σ_nと閾値ε_σnとの関係がσ_n＜ε_σnを満たす場合に、情報処理装置１００は、状態ＮＳから状態Ｐの分解速度制御へ遷移し、エンドエフェクタ１２の指令速度から特異値σ_nに対応する指令方向ｕ_vnの成分を除去するが、閾値ε_wnを小さく設定することによって、２つの状態間の遷移の連続性が保たれる。

　状態Ｐでは、エンドエフェクタ１２の指令速度から指令方向ｕ_vnの成分が除去されるため、エンドエフェクタ１２の指令速度における寄与度ｗ_nがｗ_n≧ε_wnの関係を満たしても、ロボットアーム１１に実際に指令されるエンドエフェクタ１２の速度は、方向ｕ_vnの成分が除去されたものになる。このため、情報処理装置１００は、ｗ_n＜ε_wn且つσ_n≧ε_σnが満たされる場合に状態Ｐから状態ＮＳの分解速度制御に遷移する。この場合、２つの状態間の遷移の連続性が保たれる。

　情報処理装置１００は、状態ＳＣと状態Ｐとの間の直接の遷移は許さない。状態ＳＣから状態Ｐに遷移するには、一旦エンドエフェクタ１２の指令速度を０にして状態０に遷移させてロボットアーム１１を一旦停止させ、その後寄与度ｗ_nがｗ_n＜ε_wnの関係を満たすようなエンドエフェクタ１２の速度指令値をいきなり出すことで状態Ｐに遷移させることができる。なお、情報処理装置１００は、一旦状態ＮＳの分解速度制御に遷移した後に、状態Ｐの分解速度制御に遷移することもできる。

　逆に状態Ｐから状態ＳＣに遷移するには、一旦エンドエフェクタ１２の指令速度を０にして状態０に遷移させてロボットアーム１１を一旦停止させ、その後寄与度ｗ_nがｗ_n≧ε_wnの関係を満たすようなエンドエフェクタ１２の速度指令値を出すことで、状態ＳＣに遷移できる。なお、情報処理装置１００は、状態Ｐから一旦状態ＮＳの分解速度制御に遷移した後に、状態ＳＣの分解速度制御に遷移することもできる。

　［エンドエフェクタ１２の移動速度の上限の保証の検証］
　ステップＳ１からＳ１５では、情報処理装置１００は、関節ＪＴの速度指令値に対応する正規化速度ベクトルθ_vNのノルム||θ_vN||の大きさが||θ_vN||≦１を満たすように、正規化速度ベクトルθ_vNを処理する。しかしながら、ロボットアーム１１の姿勢によっては、正規化速度ベクトルθ_vNを非正規形にした速度指令値によって実現されるエンドエフェクタ１２の速度の大きさが、エンドエフェクタ１２の指令速度の上限を超える可能性も考えられる。そこで、式９、式１２、式１４及び式１７により得られる関節ＪＴの正規化速度ベクトルについて、エンドエフェクタ１２の移動速度の上限の保証を検証する。

　まず、状態ＳＣにおいて、式９により得られる正規化速度ベクトルθ_vN1の場合、正規化速度ベクトルθ_vN1により実現されるエンドエフェクタ１２の正規化速度ベクトルＶ_NRは、以下の式１８のように得られる。さらに、以下の式１９に示すように、正規化速度ベクトルＶ_NRのノルム||Ｖ_NR||は、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのノルム||Ｖ_N||よりも大きくなる可能性がある。

　この場合、以下の式２０によって、正規化速度ベクトルθ_vN1を補正することができる。ノルム||Ｖ_NR||は、第１ノルムの一例であり、補正された正規化速度ベクトルθ_vN1は、補正動作指令値の一例である。

　なお、式９から、以下の式２１が得られ、式２１から以下の式２２が得られる。さらに、式２２と式９とを式２０に適用することにより、以下の式２３が得られる。

　よって、式２０により補正された正規化速度ベクトルθ_vN1は、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nに対して通常の分解速度制御を適用して得られる関節ＪＴの正規化速度ベクトルに相当する。よって、ノルム||Ｖ_NR||がノルム||Ｖ_N||以下であることが確認できる。

　次に、状態ＳＣにおいて、式１２により得られる正規化速度ベクトルθ_vNAの場合、正規化速度ベクトルθ_vNAにより実現されるエンドエフェクタ１２の正規化速度ベクトルＶ_NRAは、以下の式２４のように得られる。さらに、以下の式２５に示すように、正規化速度ベクトルＶ_NRAのノルム||Ｖ_NRA||は、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのノルム||Ｖ_N||よりも大きくなる可能性がある。

　この場合、以下の式２６のように、正規化速度ベクトルθ_vNAを補正することができる。ノルム||Ｖ_NRA||は、第１ノルムの一例であり、補正された正規化速度ベクトルθ_vNAは、補正動作指令値の一例である。

　式１２の正規化行列Ｊ_εNは、正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1に対して、以下の式２７の関係を満たす。そして、式２６及び式２７から、以下の式２８が得られる。よって、式２６により補正された正規化速度ベクトルθ_vNAは、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nに対して通常の分解速度制御を適用して得られる関節ＪＴの正規化速度ベクトルに相当する。よって、ノルム||Ｖ_NRA||がノルム||Ｖ_N||以下であることが確認できる。

　次に、状態ＳＣにおいて、式１４により得られる正規化速度ベクトルθ_vNBの場合、正規化速度ベクトルθ_vNBにより実現されるエンドエフェクタ１２の正規化速度ベクトルＶ_NRBは、以下の式２９のように得られる。さらに、以下の式３０に示すように、正規化速度ベクトルＶ_NRBのノルム||Ｖ_NRB||は、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのノルム||Ｖ_N||よりも大きくなる可能性がある。

　この場合、以下の式３１のように、正規化速度ベクトルθ_vNBを補正することができる。ノルム||Ｖ_NRB||は、第１ノルムの一例であり、補正された正規化速度ベクトルθ_vNBは、補正動作指令値の一例である。

　式１４の正規化行列Ｊ_εNは、正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1に対して、式２７の関係を満たす。そして、式２７及び式３１から、以下の式３２が得られる。よって、式３１により補正された正規化速度ベクトルθ_vNBは、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nに対して通常の分解速度制御を適用して得られる関節ＪＴの正規化速度ベクトルに相当する。よって、ノルム||Ｖ_NRB||がノルム||Ｖ_N||以下であることが確認できる。

　次に、状態Ｐにおいて、式１７により得られる正規化速度ベクトルθ_vNCの場合、正規化速度ベクトルθ_vNCにより実現されるエンドエフェクタ１２の正規化速度ベクトルＶ_NRCは、以下の式３３のように得られる。さらに、以下の式３４に示すように、正規化速度ベクトルＶ_NRCのノルム||Ｖ_NRC||は、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのノルム||Ｖ_N||よりも大きくなる可能性がある。

　この場合、以下の式３５のように、正規化速度ベクトルθ_vNCを補正することができる。ノルム||Ｖ_NRC||は、第１ノルムの一例であり、補正された正規化速度ベクトルθ_vNCは、補正動作指令値の一例である。

　そして、式１６及び式１７と、式２７とから、以下の式３６が得られる。さらに、ノルム||Ｊ_Nθ_vNC||が、以下の式３７のように得られる。

　よって、式１７及び式３７を式３５に代入して、以下の式３８が得られる。

　式３８において、正規化行列Ｊ^’ _εNAは、通常の正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1において、最小特異値σ_nの要素１／σ_nを０とした、逆行列Ｊ_N ^-1の疑似逆行列である。正規化行列Ｊ^’ _εNAは、正規化行列Ｊ_εNAに類似する。式３８により補正された関節ＪＴの正規化速度ベクトルθ_vNCは、このような疑似逆行列による分解速度制御を適用して得られる関節ＪＴの正規化速度ベクトルに相当する。

　正規化行列Ｊ^’ _εNAが適用される状態Ｐは、寄与度ｗ_nが閾値ε_wn未満である場合に適用されるため、ノルム||Ｖ_NA||はノルム||Ｖ_N||に漸近的に等しい。よって、||Ｖ_N||／||Ｖ_NA||の影響は無視できる。

　疑似逆行列Ｊ^’ _εNAによる解は、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nをなるべく実現するものの中で最もノルムの小さいものとなるため、式３８によって得られる関節ＪＴの正規化速度ベクトルθ_vNCによって実現されるエンドエフェクタ１２の速度のノルム||Ｖ_NRC||の大きさが、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのノルム||Ｖ_N||の大きさを上回ることがない。

　上述より、状態ＳＣ及び状態Ｐのいずれのケースにおいても、情報処理装置１００によって出力される関節ＪＴ１からＪＴ６の速度指令値によって実現されるエンドエフェクタ１２の速度の大きさは、エンドエフェクタ１２の指令速度の大きさを超えないことが確認できる。

　［特異状態でのセルフモーションに対する処理］
　ヤコビ行列Ｊにおいて、式５に示されるような最小特異値σ_nが０になる場合がある。このような場合、ロボットアーム１１は、完全な特異状態になる。このときの正規化ヤコビ行列Ｊ_Nは、以下の式３９のように示される。

　関節ＪＴの速度空間の基底ベクトルｖ_θnは、特異状態においては、関節ＪＴの速度空間の中でエンドエフェクタ１２の速度に影響しない零空間となる。このため、特異状態において、情報処理装置１００は、エンドエフェクタ１２の速度指令値Ｖに含まれるベクトルｕ_Vnの方向への速度指令値に対して、零空間の関節ＪＴの速度指令値を出力する。このような関節ＪＴの速度指令値に従ったロボットアーム１１の動作には、セルフモーションが生じる。

　ロボットアーム１１は、図６及び図７に示す手首特異姿勢及び肩特異姿勢では、セルフモーションを継続可能である。ロボットアーム１１は、図５に示す肘特異姿勢では、セルフモーションによって特異点から抜け出してしまう場合がある。肘特異姿勢の場合、ベクトルｕ_Vnの方向へのエンドエフェクタ１２の速度指令値が与えられ続けると、ロボットアーム１１は、図１３Ａから図１３Ｂに示すように特異点に一旦到達し、その後、図１３Ｃに示すようにセルフモーションにより特異点から抜け出してエンドエフェクタ１２を目標方向と逆方向に移動する。その後、ロボットアーム１１は、図１３Ｃに示すように、再びエンドエフェクタ１２を特異点に向かうように移動し、図１３Ｂの状態に至る。その後、ロボットアーム１１は、図１３Ａの状態から図１３Ｃの状態を繰り返す。これにより、ロボットアーム１１の振動現象が誘発される。図１３Ａから図１３Ｃは、肘特異姿勢の場合のロボットアーム１１のセルフモーションの一例を示す概念図である。

　そこで、情報処理装置１００は、このような振動現象を防ぐために、ロボットアーム１１が図１３Ｂの状態に至った時点で、ロボットアーム１１の関節ＪＴ１からＪＴ６の全ての動作を一旦停止する。例えば、振動現象を防ぐために、ロボットアーム１１の関節ＪＴ１からＪＴ６のうちセルフモーションに関連する関節ＪＴのみ停止すると、ロボットアーム１１は、エンドエフェクタ１２の指令速度と異なる方向にエンドエフェクタ１２を移動する可能性がある。情報処理装置１００は、このような意図しない方向へのエンドエフェクタ１２の移動を防ぐ。そして、情報処理装置１００は、ユーザインタフェース３０からの指令に従って、又は、自律的に、肘特異姿勢から逸脱するようにロボットアーム１１を動作させる。なお、肘特異姿勢以外の特異状態において、情報処理装置１００は、ロボットアーム１１に一旦停止した後、特異状態から逸脱させてもよい。

　また、情報処理装置１００は、ヤコビ行列の計算に用いる幾何学的パラメータから、ロボットアーム１１の特異状態の種類を識別することができる。エンドエフェクタ１２の速度指令値Ｖと、関節ＪＴの速度指令値θ_vと、ヤコビ行列Ｊとに関して、Ｖ＝Ｊθ_vが成立する。この式をさらに時間微分して、以下の式４０が得られる。

　セルフモーション時において、上記の要素（dＪ/dt）θ_vのノルム||(dＪ/dt)θ_v||が零でない値を持つ場合、ロボットアーム１１は肘特異点のようにセルフモーションによってその特異状態にとどまることができずに抜け出してしまうことになる。

　ヤコビ行列Ｊにおいて、式５に示されるような最小特異値σnが閾値ε_σnに近くなり、ロボットアーム１１の姿勢が特異点に近づくとき、最小特異値σnに対応する関節速度ベクトルである基底ベクトルｖ_θnに着目することができる。関節速度ベクトルｖ_θnは、特異値σn＝０となる特異姿勢では、式３９に示すようにセルフモーションに対応する関節速度を示す。

　手首特異姿勢及び肩特異姿勢のようにセルフモーションを継続可能である特異点の場合、特異点の近傍においても、関節速度ベクトルｖ_θnは、セルフモーションに近い速度を示す。それ故、ロボットアーム１１によって移動されるエンドエフェクタ１２の加速度は、十分に小さい。このため、最小特異値σ_n及び閾値ε_σnがσ_n＜ε_σnを満たす特異点の近傍では、式４１に示すように、要素（dＪ/dt）ｖ_θnの絶対値|(dＪ/dt)ｖ_θn|が閾値ε_a未満である条件を満たす場合、当該特異点は、セルフモーションを継続可能である特異点であるとすることができる。閾値ε_aは、エンドエフェクタ１２の加速度の大きさの閾値である。dＪ/dtは、第８ヤコビ行列の一例であり、（dＪ/dt）ｖ_θnは、加速度ベクトルの一例であり、閾値ε_aは、第４閾値の一例である。

　よって、情報処理装置１００は、式４１の条件が満たされる場合に、特異点を、ロボットアーム１１のセルフモーションを継続可能である特異点に決定する。情報処理装置１００は、式４１の条件が満たされない場合に、特異点を、ロボットアーム１１のセルフモーションを継続可能でない特異点に決定する。情報処理装置１００は、式４１の条件に加えて又は代わりに、ノルム||(dＪ/dt)θ_v||が零でない値であることを条件として、特異点を、セルフモーションを継続可能である特異点に決定してもよい。情報処理装置１００は、関節速度ベクトルｖ_θnを用いることによって、現在のエンドエフェクタ１２の速度指令値に関わらずに特異点の種別を判別できる。

　情報処理装置１００は、特異点を、セルフモーションを継続可能でない特異点に決定した場合、以下のようにロボットアーム１１を停止する。情報処理装置１００は、サンプリング周期毎に取得する情報を用いて、絶対値|(dＪ/dt)ｖ_θn|を算出する。情報処理装置１００は、ロボットアーム１１が動作する過程において、絶対値|(dＪ/dt)ｖ_θn|が最大となった時点で、ロボットアーム１１を停止してもよい。例えば、情報処理装置１００は、絶対値|(dＪ/dt)ｖ_θn|の変動が増加から減少に転じたタイミングを、ロボットアーム１１の停止のタイミングとしてもよい。限定されないが、肘特異姿勢の場合に、上記タイミングは、ロボットアーム１１の停止のタイミングに特に適する。

　さらに、情報処理装置１００は、上記タイミングにおいて、エンドエフェクタ１２の指令速度の方向が反転等により変化していないかを判別してもよい。これにより、絶対値|(dＪ/dt)ｖ_θn|の増加から減少への変動が、エンドエフェクタ１２の指令速度の方向の変化を起因とするか否かの判別が可能である。情報処理装置１００は、式１０において寄与度ｗ_nの算出に用いるエンドエフェクタ１２の指令速度の単位ベクトルｕと、セルフモーションを示す関節速度ベクトルｖ_θnに対応するエンドエフェクタ１２の速度ベクトルｕ_vnとの内積ｕ_vn ^Tｕを、サンプリング周期毎に取得する情報を用いて算出しメモリＭに記憶する。情報処理装置１００は、絶対値|(dＪ/dt)ｖ_θn|の変動が増加から減少に転じたタイミング又はその周辺において、内積ｕ_vn ^Tｕの符号が反転していない場合に、ロボットアーム１１を停止する。

　情報処理装置１００は、停止したロボットアーム１１を再び動かすタイミングを、内積ｕ_vn ^Tｕの符号に基づき決定してもよい。例えば、情報処理装置１００は、ロボットアーム１１の停止を決定したときの内積ｕ_vn ^Tｕの符号から、内積ｕ_vn ^Tｕの符号が反転したタイミングを、ロボットアーム１１を再び動かすタイミングに決定してもよい。内積ｕ_vn ^Tｕがほぼ０の場合、エンドエフェクタ１２の指令速度の、速度ベクトルｕ_vnの方向の寄与度は無視できる。この場合、情報処理装置１００が、ロボットアーム１１を動かすと、ロボットアーム１１の動作領域境界に沿ってエンドエフェクタ１２を移動することになる。

　［ロボットアームが複数の特異状態に陥るケース］
　ロボットアーム１１は、複数の特異状態に陥る場合がある。この場合、２つ以上の特異値σiが閾値ε_σn未満となる。式５の正規化ヤコビ行列Ｊ_Nにおいて、特異値σ_n及びσ_n-1が、閾値ε_σn未満となる例について、情報処理装置１００の処理を説明する。本例では、特異値σ_n及びσ_n-1が、σ_n＜σ_n-1＜ε_σnの関係を満たす。特異値σ_nは、第１特異値の一例であり、特異値σ_n-1は、第２特異値の一例である。

　情報処理装置１００は、寄与度ｗ_nが閾値ε_wn以上である状態ＳＣについて、上述したステップＳ１４０１と同様に、式６に示される正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1内の行列Σ^-1のすべての要素をσ_n／ε_σn倍して、以下の式４２に示すような新たな行列Ｊ_εNを算出する。しかしながら、特異値σ_n-1が閾値ε_σnを下回るため、特異値σ_n-1を含む要素１／σ_n-1については、情報処理装置１００は、以下の式４３のように数値計算の桁落ちを防ぐ処理をする。σ_n＜σ_n-1であるため、要素１／σ_n-1 ^’は発散しない。情報処理装置１００は、行列Ｊ_εNに基づき分解速度制御を行う。寄与度ｗ_nは、第１寄与度の一例である。

　寄与度ｗ_nが閾値ε_wn未満である状態Ｐにおいては、情報処理装置１００は、特異値σ_n-1を最小特異値とみなす。情報処理装置１００は、式６に示される正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1内の行列Σ^-1のすべての要素を、σ_n-1／ε_σn倍する。要素１／σ_i（ｉ＝１，・・・，ｎ－２）は、σ_n-1／σ_iε_σnに変更される。さらに、σ_n-1／σ_iε_σnを１／σ_i”と表す。さらに、情報処理装置１００は、上述したステップＳ１５０１と同様に、特異値σ_nを含む要素を０に置き換える。これにより、情報処理装置１００は、以下の式４４に示すような新たな行列Ｊ_εNAを算出する。情報処理装置１００は、行列Ｊ_εNAに基づき分解速度制御を行う。この場合、特異値σ_nに対応する空間を除いた補空間に対して処理が行われる。行列Ｊ_εNAは、第５逆ヤコビ行列の一例であり、σ_n-1／ε_σnは、第２比率の一例である。

　２つの特異値σ_n及びσ_n-1それぞれが閾値ε_σnを下回る特異状態において、特異値σ_n及びσ_n-1それぞれに対応する寄与度ｗ_n及びｗ_n-1の状態に関する４つのケースに対応して、情報処理装置１００は、ロボットアーム１１の分解速度制御を行う。寄与度ｗ_nは、式１０の通りである。寄与度ｗ_n-1は、以下の式４５の通りであり、エンドエフェクタ１２の正規化指令速度ベクトルＶ_Nのうちの特異値σ_n-1に対応する速度空間の基底ベクトルｕ_vn-1の方向の寄与度である。さらに、寄与度ｗ_n及びｗ_n-1のいずれもに、閾値ε_wnが設定される。閾値ε_wnは、０＜ε_wn＜１の範囲で設定される。寄与度ｗ_n-1は、第２寄与度の一例である。

（１）ｗ_n≧ε_wn及びｗ_n-1≧ε_wnの第１ケース
　寄与度ｗ_nが閾値ε_wn以上であるため、情報処理装置１００は、式４２及び式１１で示される正規化行列Ｊ_εNを用いた状態ＳＣの分解速度制御を行う。例えば、特異値σ_n＜＜特異値σ_n-1ように、特異値σ_nと特異値σ_n-1との差が大きい場合、ロボットアーム１１がセルフモーションを行う際、特異値σ_nに対応するセルフモーションが支配的になる。特異値σ_nと特異値σ_n-1との差が小さくなるにしたがって、ロボットアーム１１は、特異値σ_nに対応するセルフモーションと特異値σ_n-1に対応するセルフモーションとの複合動作を行い得る。

（２）ｗ_n≧ε_wn及びｗ_n-1＜ε_wnの第２ケース
　寄与度ｗ_nが閾値ε_wn以上であるため、情報処理装置１００は、式４２及び式１１で示される正規化行列Ｊ_εNを用いた状態ＳＣの分解速度制御を行う。例えば、特異値σ_n＜＜特異値σ_n-1ように、特異値σ_nと特異値σ_n-1との差が大きい場合、ロボットアーム１１がセルフモーションを行う際、特異値σ_nに対応するセルフモーションが支配的になる。このため、寄与度ｗ_n-1が閾値ε_wnを下回ることが、ロボットアーム１１のセルフモーションに与える影響は小さい。

（３）ｗ_n＜ε_wn及びｗ_n-1≧ε_wnの第３ケース
　寄与度ｗ_nが閾値ε_wnを下回るため、情報処理装置１００は、式４４の正規化行列Ｊ_εNAを用いた状態Ｐの分解速度制御を行う。寄与度ｗ_n-1が閾値ε_wn以上であるため、ロボットアーム１１がセルフモーションを行う際、特異値σ_n-1に対応するセルフモーションが支配的になる。

（４）ｗ_n＜ε_wn及びｗ_n-1＜ε_wnの第４ケース
　寄与度ｗ_nが閾値ε_wnを下回るため、情報処理装置１００は、式４４の正規化行列Ｊ_εNAに基づく状態Ｐの分解速度制御を行う。さらに、寄与度ｗ_n-1が閾値ε_wnを下回るため、最小特異値σ_nに対応する空間を除いた補空間に対して、再帰的に式１５の正規化行列Ｊ_εNAを生成するための処理を行うことになる。つまり、情報処理装置１００は、式４４の正規化行列Ｊ_εNAに対して、要素１／ε_σnを０に置き換える変換を行って、以下の式４６に示す新たな正規化行列Ｊ_εNA1を生成する。情報処理装置１００は、式４６の正規化行列Ｊ_εNA1を用いた状態Ｐの分解速度制御を行う。つまり、２つの特異値に対応する２次元空間が縮約された４次元空間での分解速度制御となる。行列Ｊ_εNA1は、第６逆ヤコビ行列及び第７逆ヤコビ行列の一例である。

　また、２つの特異値σ_n及びσ_n-1それぞれが閾値ε_σnを下回る特異状態において、ロボットアーム１１が動作した場合に、２つの特異値σ_n及びσ_n-1が閾値ε_σnを下回ったままで２つの特異値σ_n及びσ_n-1の大小関係が入れ替わることがある。このような場合においても、情報処理装置１００は、特異値σ_n及びσ_n-1それぞれに対応する寄与度ｗ_n及びｗ_n-1の状態に関する４つのケースに対応して、ロボットアーム１１の分解速度制御を行う。

（１）ｗ_n≧ε_wn及びｗ_n-1≧ε_wnの第１ケース
　情報処理装置１００は、状態ＳＣの分解速度制御を行う。例えば、情報処理装置１００は、式４２及び式１１で示される正規化行列Ｊ_εNを使用した式１２に従った分解速度制御、又は、式４２及び式１１で示される正規化行列Ｊ_εNを使用した式１４に従った分解速度制御を行い得る。しかしながら、２つの特異値σ_n及びσ_n-1の大小関係が入れ替わる直前は、特異値σ_n及びσ_n-1の差異は小さいため、入れ替わりにより寄与度ｗ_nが減少しても、ノルム||Ｊ_εNｕ_N||が閾値κ_θ未満となることはなく、それ故、情報処理装置１００は、式１４に従った分解速度制御を行うことがない。これ故、ロボットアーム１１は、特異値σ_nに対応するセルフモーションと特異値σ_n-1に対応するセルフモーションとの複合動作を行い得る。さらに、特異値σ_n及びσ_n-1の大小関係が入れ替わっても、関節ＪＴの速度指令値に変化がなく、ロボットアーム１１の動作はスムーズに遷移し得る。

（２）ｗ_n≧ε_wn及びｗ_n-1＜ε_wnの第２ケース
　寄与度ｗ_nが閾値ε_wn以上であるため、情報処理装置１００は、状態ＳＣの分解速度制御を行う。例えば、情報処理装置１００は、式４２及び式１１で示される正規化行列Ｊ_εNを使用した式１２に従った分解速度制御、又は、式４２及び式１１で示される正規化行列Ｊ_εNを使用した式１４に従った分解速度制御を行い得る。ロボットアーム１１がセルフモーションを行う際、特異値σ_nに対応するセルフモーションが支配的になる。この状態で、特異値σ_n及びσ_n-1の大小関係が入れ替わると、入れ替わり後の特異値σ_nの寄与度が閾値を下回り、それ故、情報処理装置１００は、状態Ｐの分解速度制御を行う。しかしながら、特異値σ_nに対応するセルフモーションが支配的になっている状態は、維持される。

（３）ｗ_n＜ε_wn及びｗ_n-1≧ε_wnの第３ケース
　寄与度ｗ_nが閾値ε_wnを下回るため、情報処理装置１００は、式４４の正規化行列Ｊ_εNAを用いた状態Ｐの分解速度制御を行う。ロボットアーム１１がセルフモーションを行う際、特異値σ_n-1に対応するセルフモーションが支配的になる。この状態で、特異値σ_n及びσ_n-1の大小関係が入れ替わると、エンドエフェクタ１２の指令速度において、入れ替わり後の特異値σ_n-1に対応する方向の指令速度がなかったものとされた状態は、維持される。情報処理装置１００は、ユーザインタフェース３０の指令により又は自律的に、エンドエフェクタ１２の指令速度を零とすることによって、状態Ｐを抜け出してもよい。

（４）ｗ_n＜ε_wn及びｗ_n-1＜ε_wnの第４ケース
　情報処理装置１００は、式４６の正規化行列Ｊ_εNA1を用いた状態Ｐの分解速度制御を行う。正規化行列Ｊ_εNA1では、特異値σ_n及びσ_n-1に対応する方向成分が取り除かれている。このため、特異値σ_n及びσ_n-1の大小関係が入れ替わっても、情報処理装置１００の分解速度制御は影響を受けない。情報処理装置１００は、ユーザインタフェース３０の指令により又は自律的に、エンドエフェクタ１２の指令速度を零とすることによって、状態Ｐを抜け出してもよい。

　よって、情報処理装置１００は、閾値ε_σnを下回った特異状態にある２つの特異値の大小関係が入れ替わっても、入れ替わりに対応した特別な処理をする必要がない。情報処理装置１００は、ロボット１０からのフィードバック情報が更新される毎に、ヤコビ行列を決定して特異値分解し、特異値の大小関係に基づいて上述した処理を行うことによって、各状態の分解速度制御を行うことができる。

　また、３つ以上の特異値が閾値を下回った場合や、当該特異値に対応する寄与度の中で複数の寄与度が閾値を下回った場合も、上記と同様に行列Σ^-1内の要素を適切に変更することで対応できる。このように本手法は、複数の特異状態に同時に陥った場合にも対応できるような一般性を有している。例えば、３つ以上の特異値それぞれが閾値ε_σnを下回る特異状態において、情報処理装置１００は、上述の第１ケースから第４ケースと同様の処理をすることで、各状態の分解速度制御を行ってもよい。３つ以上の特異値の間で大小関係が入れ替わる場合も、情報処理装置１００は、上述の第１ケースから第４ケースと同様の処理をすることで、各状態の分解速度制御を行ってもよい。

　例えば、第１特異値σ_nと、第２特異値σ_n-1と、第２特異値σ_n-1の次に小さい１つ以上の特異値とを含む、第１特異値σ_nから第ｋ特異値σ_n-k+1のいずれもが、閾値ε_σnを下回る特異状態において、情報処理装置１００は、第１特異値σ_nから第ｋ特異値σ_n-k+1それぞれに対応する第１寄与度ｗ_nから第ｋ寄与度ｗ_n-k+1を決定する。なお、ｋは３以上ｎ以下の自然数である。

　情報処理装置１００は、第１寄与度ｗ_nから第ｋ寄与度ｗ_n-k+1に向かって順次、寄与度が閾値ε_wn以上であるか否かを判定する。情報処理装置１００は、閾値ε_wn以上である寄与度を見つけだすことができたら、閾値ε_wn以上であると最初に判定した第ｉ寄与度ｗ_n-i+1に対応する第ｉ特異値σ_n-i+1を基準として、式６に示される正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1を変換する。ｉは、１以上ｋ以下の自然数である。第ｉ寄与度ｗ_n-i+1の序数ｉは、閾値ε_wn以上である寄与度の序数の中で最小である。

　上記変換では、情報処理装置１００は、逆行列Ｊ_N ^-1内の行列Σ^-1に含まれる特異値のうちの第１特異値σ_nから第（ｉ－１）特異値σ_n-iを０に置き換える零変換をする。さらに、情報処理装置１００は、第（ｉ＋１）特異値σ_n-iから第ｋ特異値σ_n-k+1に対応する第（ｉ＋１）寄与度ｗ_n-iから第ｋ寄与度ｗ_n-k+1それぞれについて、寄与度が閾値ε_wn以上であるか否かを判定する。情報処理装置１００は、寄与度が閾値ε_wn未満である特異値を０に置き換える零変換をする。

　さらに、情報処理装置１００は、２つの零変換後の行列Σ^-1のすべての要素を第３比率σ_n-i+1／ε_σn倍することで、逆行列Ｊ_N ^-1から変換された新たな正規化行列を算出する。第ｉ特異値σ_n-i+1は、第ｉ特異値σ_n-i+1から第ｋ特異値σ_n-k+1のうちで最小の特異値である。情報処理装置１００は、上記の第３ケースと同様に、当該正規化行列を用いた状態Ｐの分解速度制御を行う。当該正規化行列は、第７逆ヤコビ行列の一例である。

　例えば、第１寄与度ｗ_nから第ｋ寄与度ｗ_n-k+1のいずれもが閾値ε_wn以上である場合、情報処理装置１００は、第３比率として、第１比率σ_n／ε_σnを用いる。情報処理装置１００は、式６に示される正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1内の行列Σ^-1のすべての要素を第１比率σ_n／ε_σn倍して、式４２に示す正規化行列Ｊ_εNを算出する。情報処理装置１００は、上記の第１ケース又は第２ケースと同様に、正規化行列Ｊ_εNを用いた状態ＳＣの分解速度制御を行う。

　例えば、第１寄与度ｗ_nから第ｋ寄与度ｗ_n-k+1のいずれもが閾値ε_wn未満である場合、情報処理装置１００は、式６に示される正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1内の第１特異値σ_nから第ｋ特異値σ_n-k+1の全てを０に置き換える零変換をして、新たな正規化行列を算出する。さらに、情報処理装置１００は、新たな正規化行列において、零変換後の行列Σ^-1のすべての要素を第３比率σ_n-k+1／ε_σn倍してもよい。情報処理装置１００は、上記の第４ケースと同様に、当該正規化行列を用いた状態Ｐの分解速度制御を行う。当該正規化行列は、第７逆ヤコビ行列の一例である。

　例えば、３つの特異値σ_n、σ_n-1及びσ_n-2それぞれが閾値ε_σnを下回る特異状態では、それぞれの寄与度ｗ_n、ｗ_n-1及びｗ_n-2の閾値ε_wnに対する状態に関する８つのケースに対応して、情報処理装置１００は、ロボットアーム１１の分解速度制御を行ってもよい。各ケースでの処理は、第１から第４ケースの処理と同様に行うことができる。

　また、ロボットアーム１１の動作では、３つの特異値σ_n、σ_n-1及びσ_n-2が同時に閾値ε_σnを下回る特異状態に至るのではなく、２つの特異値が閾値ε_σnを下回る特異状態に至った後に、さらに、１つの特異値が閾値ε_σnを下回る状態に至ることが多い。

　例えば、図４に示すロボットアーム１１は、３つの特異値が閾値ε_σnを下回る特異状態である。図５から図７に示すロボットアーム１１は、１つの特異値が閾値ε_σnを下回る特異状態である。ロボットアーム１１は、まず、図７に示す肩特異姿勢に至り、次いで、肘を伸ばすようにして図５に示す肘特異姿勢に至り、その後又は同時に、図６に示す手首特異姿勢に至ることで、図４に示すような３つの特異値が閾値ε_σnを下回る特異姿勢に至り得る。

　また、２つの特異値σ_n及びσ_n-1それぞれが閾値ε_σnを下回る特異状態において、情報処理装置１００は、特異値σ_n及びσ_n-1の大小関係に依存しない処理をしてもよい。情報処理装置１００は、式６に示される正規化ヤコビ行列Ｊ_Nの逆行列Ｊ_N ^-1内の行列Σ^-1のすべての要素をσ_n／ε_σn倍して、式４２に示す新たな行列Ｊ_εNを算出する。さらに、情報処理装置１００は、特異値σ_n-１を含む要素を、特異値σ_nを含む要素と同じ値１／ε_σnに置き換えることで、以下の式４７に示す新たな行列Ｊ_εNDを算出する。これにより、特異値σ_n、σ_n-１を含む要素それぞれの発散を防ぐことができる。情報処理装置１００は、式４７の正規化行列Ｊ_εNDを用いた分解速度制御を行うことができる。

　［ロボットシステムの変形例］
　実施の形態に係るロボットシステム１の変形例を説明する。変形例に係るロボットシステム１Ａは、ユーザインタフェース３０とロボット１０とが通信網Ｎを介して接続される遠隔操作ロボットシステムである点で、実施の形態と異なる。以下、本変形例について、実施の形態と異なる点を中心に説明し、実施の形態と同様の点の説明を適宜省略する。

　図１４は、変形例に係るロボットシステム１Ａの構成の一例を示す図である。図１４に示すように、ロボットシステム１Ａは、１つ以上のロボットエリアＡＲそれぞれに配置されるロボット１０及び制御装置２０と、１つ以上のユーザエリアＡＵそれぞれに配置されるユーザインタフェース３０と、ロボットエリアＡＲとユーザエリアＡＵとをマッチングしてそれぞれに含まれる制御装置２０とユーザインタフェース３０とを通信網Ｎを介してデータ通信可能に接続する仲介装置３００とを含む。情報処理装置１００の機能の少なくとも一部は、ユーザインタフェース３０及び仲介装置３００の一方又は両方によって実現される。制御装置２０は、電力制御回路２００を少なくとも含む。限定されないが、本変形例では、複数のロボットエリアＡＲと、複数のユーザエリアＡＵとが、ロボットシステム１Ａの対象として存在する。

　ロボットシステム１Ａは、ロボットエリアＡＲに、撮像装置１４ａ及び１４ｂと、ロボット通信装置１５とをさらに含む。撮像装置１４ａ及び１４ｂは、カメラを含み、制御装置２０と接続される。撮像装置１４ａは、ロボット１０に配置され、例えば、ロボットアーム１１に配置され、エンドエフェクタ１２及びその処理対象物Ｗを撮像する。撮像装置１４ｂは、ロボット１０全体を撮像するように、ロボット１０以外の位置に配置される。ロボット通信装置１５は、モデム、ＯＮＵ（Optical Network Unit：光回線の終端装置）、ルータ又はモバイルデータ通信機器を含む。ロボット通信装置１５は、制御装置２０を通信網Ｎに接続する。制御装置２０は、通信網Ｎを介して、ユーザインタフェース３０及び仲介装置３００と情報、指令及びデータを送受信できる。制御装置２０は、撮像装置１４ａ及び１４ｂによって取得された画像データを送信できる。

　ロボットシステム１Ａは、ユーザエリアＡＵに、提示装置３１と、ユーザ通信装置３２とをさらに含む。提示装置３１は、ユーザインタフェース３０と接続され、操作者Ｐに情報を知覚可能に提示する。提示装置３１は、少なくともディスプレイを含み、さらに、スピーカ等を含んでもよい。ユーザ通信装置３２は、モデム、ＯＮＵ、ルータ又はモバイルデータ通信機器を含む。ユーザ通信装置３２は、ユーザインタフェース３０を通信網Ｎに接続する。ユーザインタフェース３０は、通信網Ｎを介して、制御装置２０及び仲介装置３００と情報、指令及びデータを送受信できる。提示装置３１は、ユーザインタフェース３０が受信する撮像装置１４ａ及び１４ｂの画像データを画像として提示する。ユーザインタフェース３０は、提示装置３１、ユーザ通信装置３２又はこれらの両方を含んでもよい。

　仲介装置３００は、通信網Ｎを介した通信を管理する。仲介装置３００は、コンピュータ装置を含むサーバの構成を有してもよい。仲介装置３００は、ユーザ通信装置３２の認証、ユーザ通信装置３２とロボット通信装置１５との接続及び接続解除等を管理する。例えば、仲介装置３００は、認証されたユーザ通信装置３２に接続されたユーザインタフェース３０によって指定されるロボット１０のロボット通信装置１５と当該ユーザ通信装置３２とを接続する。つまり、仲介装置３００は、ユーザ通信装置３２とロボット通信装置１５とをマッチングして接続する。仲介装置３００は、ロボット通信装置１５とユーザ通信装置３２との間のデータの送受信を管理し、当該データは仲介装置３００を経由してもよい。

　通信網Ｎは特に限定されず、例えば、ローカルエリアネットワーク（Local Area Network：ＬＡＮ）、広域ネットワーク（Wide Area Network：ＷＡＮ）、インターネット、又はこれらの２つ以上の組み合わせを含むことができる。通信網Ｎは、ブルートゥース（Bluetooth）（登録商標）及びＺｉｇＢｅｅ（登録商標）などの近距離無線通信、ネットワーク専用回線、通信事業者の専用回線、公衆交換電話網（Public Switched Telephone Network：ＰＳＴＮ）、モバイル通信網、インターネット網、衛星通信、又は、これらの２つ以上の組み合わせを用いるように構成され得る。モバイル通信網は、第４世代移動通信システム及び第５世代移動通信システム等を用いるものであってもよい。通信網Ｎは、１つ又は複数のネットワークを含むことができる。本変形例では、通信網Ｎはインターネットである。

　本変形例では、仲介装置３００は、実施の形態に係る情報処理装置１００の機能を含む。仲介装置３００は、制御装置２０又は当該制御装置２０と接続されたユーザインタフェース３０から、当該制御装置２０と接続されたロボット１０の諸元に関する情報を受信する。仲介装置３００は、制御装置２０からロボット１０のフィードバック情報を受信し、フィードバック情報とロボット１０の諸元に関する情報とを用いてヤコビ行列を生成する。仲介装置３００は、当該ヤコビ行列に基づき、実施の形態に係る情報処理装置１００が行う特異点に関する処理を行う。

　ロボットエリアＡＲの制御装置２０が、情報処理装置１００の機能を備えていなくても、仲介装置３００は、情報処理装置１００として機能して、ロボット１０に、特異点に対応した動作をさせることができる。仲介装置３００は、ロボット１０の諸元に関する情報を得ることができれば、情報処理装置１００として機能できる。仲介装置３００は、ユーザエリアＡＵの操作者Ｐが、提示装置３１によって提示される限られた領域の画像に基づいて特異点を回避するようにロボット１０を操作することを不要にする。操作者Ｐは、特異点を考慮せずにロボット１０を操作できる。仲介装置３００は、様々な操作者Ｐが様々なロボット１０を操作することを容易にする。

　なお、ユーザインタフェース３０が、実施の形態に係る情報処理装置１００の機能の一部又は全部を備えてもよい。ユーザインタフェース３０は、情報処理装置１００の機能をアプリケーションソフトとして組み入れていてもよい。例えば、ユーザインタフェース３０が、実施の形態に係る情報処理装置１００の機能の全部を備える場合、仲介装置３００は、ユーザインタフェース３０と制御装置２０とのマッチングと、ユーザインタフェース３０と制御装置２０との間の情報、指令及びデータの送受信の仲介とを行ってもよい。例えば、ユーザインタフェース３０が、実施の形態に係る情報処理装置１００の機能の一部を備え、仲介装置３００が、情報処理装置１００の機能の他部又は全部を備えてもよい。ユーザインタフェース３０が備える情報処理装置１００の機能と仲介装置３００が備える情報処理装置１００の機能とは、重複してもよく、重複していなくてもよい。例えば、仲介装置３００は、ユーザインタフェース３０が情報処理装置１００として実行できない処理を実行し、処理結果をユーザインタフェース３０に提供してもよい。

　制御装置２０が、情報処理装置１００の機能の一部を備えてもよい。この場合、仲介装置３００が、情報処理装置１００の機能の他部又は全部を備えてもよく、ユーザインタフェース３０が、情報処理装置１００の機能の他部又は全部を備えてもよい。制御装置２０、ユーザインタフェース３０及び仲介装置３００のそれぞれが、情報処理装置１００の機能の一部を備えてもよい。この場合、仲介装置３００は、情報処理装置１００の機能の全部を備えてもよい。制御装置２０が備える情報処理装置１００の機能と、ユーザインタフェース３０が備える情報処理装置１００の機能と、仲介装置３００が備える情報処理装置１００の機能とは、重複してもよく、重複していなくてもよい。

　［その他］
　以上、本開示の例示的な実施の形態及び変形例について説明したが、本開示は、上記実施の形態及び変形例に限定されない。すなわち、本開示の範囲内で種々の変形及び改良が可能である。例えば、各種変形を実施の形態又は変形例に施したもの、及び、異なる実施の形態及び変形例における構成要素を組み合わせて構築される形態も、本開示の範囲内に含まれる。

　例えば、実施の形態及び変形例では、情報処理装置１００は、ロボット１０の手動操縦の場合に、特異点に関する処理を行うが、これに限定されない。情報処理装置１００は、ロボット１０の自動操縦の場合、つまり、ロボット１０が教示データ等の予め設定されたデータに従って自律的に動作する場合に、特異点に関する処理を行ってもよい。これにより、教示データが、特異点を回避するようなロボット１０の動作のデータを含んでいなくとも、情報処理装置１００は、当該教示データに従ってロボット１０に動作させる際に、特異点に関する処理を行い、ロボット１０に特異点に対応して動作させることができる。

　本開示の技術の各態様例は、以下のように挙げられる。本開示の第１態様に係るプログラムは、複数の関節と前記複数の関節が動作することによって移動される移動部とを含むロボットの動作を制御するプログラムであって、前記複数の関節の動作速度と前記移動部の移動速度とを関連付ける第１ヤコビ行列を特異値分解して、前記関節の動作速度空間を表す第１基底ベクトルを要素に含む第１行列と、前記移動部の移動速度空間を表す第２基底ベクトルを要素に含む第２行列と、特異値を対角要素に含む第１特異行列とを含む、第２ヤコビ行列を生成することと、前記特異値と第１閾値との比較結果に基づき、前記ロボットが特異点にある場合及び前記ロボットが前記特異点に接近している場合に実行される第１処理と、前記ロボットが前記特異点から離れている場合に実行される第２処理とのうちから、実行対象の処理を決定して実行することと、前記第１処理において、前記第１特異行列内の対角要素を変換して第３ヤコビ行列を生成することと、前記第１処理において、前記第３ヤコビ行列と、前記移動部の移動を指令する移動指令値とを用いて、前記複数の関節の動作を指令する動作指令値を決定し出力することと、前記第２処理において、前記第１ヤコビ行列又は前記第２ヤコビ行列と、前記移動指令値とを用いて、前記動作指令値を決定し出力することとを、コンピュータに実行させる。

　上記態様によると、第１処理において、第１特異行列内の対角要素が変換されることにより、第２ヤコビ行列から第３ヤコビ行列が生成される。この場合、ロボットが特異点にある状態及びロボットが特異点に接近している状態に対応するように、対角要素が変換されてもよい。例えば、ロボットが特異点にある状態及びロボットが特異点に接近している状態で、第２ヤコビ行列の逆行列の要素が発散しないように、対角要素が変換されてもよい。上記の変換により、ロボットが特異点にある状態及びロボットが特異点に接近している状態で、関節の速度が急激に変化することを防ぐことができる。このように、第１特異行列内の対角要素を変換するという新規な方法によって、特異点に対応したロボットの動作制御が可能である。

　ここで、特異点近傍では、通常の分解速度制御では関節速度が過大になり得る。このような問題を解決する方法として、特異点低感度分解と特異点適合法とがある。特異点低感度分解は、関節速度が過大になることを抑制できるが、その代わりに、本来指令した軌道からエンドエフェクタが逸脱するという問題がある。特異点適合法は、関節速度が過大になることを抑制しつつ、本来指令した軌道からエンドエフェクタが逸脱もしない。ただし、特異点適合法は、ヤコビ行列の余因子行列の解析的な計算を必要とし、ある特定のアームに適用するために非常に手間を要する。

　従来の特異点適合法が、ヤコビ行列の余因子行列の計算を要していたのに対し、本願発明者は、ヤコビ行列を特異値分解し、その特異値の逆数の比が保存されるように、特異値をスケーリングすることを見出した。これにより、本願発明者は、従来の特異点適宜合法におけるヤコビ行列の余因子行列と同等の効果を持つ行列を、解析計算ではなく数値計算で求めることをできることを見出した。

　上記第１態様において、本開示の第２態様に係るプログラムは、前記動作指令値の決定に用いたヤコビ行列に前記動作指令値を適用して得られるベクトルのノルムである第１ノルムを算出することと、前記第１ノルムが前記移動指令値よりも大きい場合、前記第１ノルムを用いて前記動作指令値を補正して補正動作指令値を決定し、前記動作指令値として前記補正動作指令値を出力することと、前記第１ノルムが前記移動指令値以下である場合、前記動作指令値を出力することとを、コンピュータに実行させてもよい。

　上記態様によると、ロボットが移動部を移動する速度の上限を設定し、当該上限以下の速度で移動部が移動するように、複数の関節の動作指令値を決定することができる。よって、移動部の移動速度の過度な上昇を防ぐことができる。

　上記の第１態様又は第２態様において、本開示の第３態様に係るプログラムは、前記ロボットの諸元の情報である第１情報を受け取り、前記第１情報と、前記複数の関節の状態の情報とを用いて、前記第１ヤコビ行列を生成することを、コンピュータに実行させてもよい。

　上記態様によると、第１情報を取得し、取得した第１情報を用いて、第１ヤコビ行列を生成することができる。よって、ロボットに合わせた第１ヤコビ行列の設定が可能である。

　上記の第１態様から第３態様のいずれかにおいて、本開示の第４態様に係るプログラムは、前記特異値の最小値が前記第１閾値よりも小さい場合に前記第１処理の実行を決定し、前記特異値の最小値が前記第１閾値以上の場合に前記第２処理の実行を決定することを、コンピュータに実行させてもよい。

　上記態様によると、ロボットが特異点に接近するに従って、最小の特異値が小さくなる。最小の特異値が第１閾値よりも小さいか否かという条件は、ロボットが特異点に接近した状態であるか否かを決定するための条件、つまり、第１処理の実行の有無を決定するための条件とすることができる。よって、第１処理の実行の有無の確実な決定が可能である。

　上記の第１態様から第４態様のいずれかにおいて、本開示の第５態様に係るプログラムは、前記第２ヤコビ行列の逆行列である第２逆ヤコビ行列を生成することであって、前記第２逆ヤコビ行列は、前記第１行列、前記第２行列及び前記第１特異行列それぞれから変換された第１変換行列、第２変換行列及び第１変換特異行列を含むことと、前記第１処理において、前記第１変換特異行列内の対角要素を変換する第１変換を行うことにより、前記第３ヤコビ行列の逆行列である第３逆ヤコビ行列を生成することと、前記第１処理において、前記第３逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することと、前記第２処理において、前記第１ヤコビ行列の逆行列である第１逆ヤコビ行列又は前記第２逆ヤコビ行列に、前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させてもよい。

　上記態様によると、ヤコビ行列の逆行列である逆ヤコビ行列は、移動部の移動速度を適用されて、複数の関節の動作速度を得ることができる。逆ヤコビ行列を使用することによって、移動指令値に対する動作指令値の決定が可能である。

　上記の第５態様において、本開示の第６態様に係るプログラムは、前記第１変換では、前記第１変換特異行列内の前記特異値を含む対角要素間の比率を維持するように、前記対角要素それぞれを変換することを、コンピュータに実行させてもよい。

　上記態様によると、第１変換では、第１変換特異行列内の特異値を含む対角要素間の比率が維持される。よって、第１変換は、逆ヤコビ行列が本来持っている移動指令値と動作指令値との関係性を維持する。第１逆ヤコビ行列、第２逆ヤコビ行列及び第３逆ヤコビ行列は、同じ移動指令値を適用されると、同様の方向に移動部を移動する動作指令値を得ることができる。ロボットが特異点の近傍であるか否かに関係なく、指令される方向への移動部の動作を維持することができる。

　上記の第６態様において、本開示の第７態様に係るプログラムは、前記第１変換では、前記第１変換特異行列内の前記特異値の最小値と前記第１閾値との第１比率を、前記対角要素それぞれに乗算することを、コンピュータに実行させてもよい。

　上記態様によると、第１変換特異行列において、最小の特異値が０に近づいても、最小の特異値を含む対角要素は、発散しない。よって、特異点の近傍において、当該対角要素に対応する関節の速度が急激に変化することが防がれる。

　上記の第５態様から第７態様のいずれかにおいて、本開示の第８態様に係るプログラムは、前記第１処理において、前記移動指令値に含まれる前記移動部の指令速度の方向を表す方向ベクトルを前記第３逆ヤコビ行列に適用して得られる、ベクトルのノルムである第２ノルムを算出することと、前記第１処理において、前記第２ノルムが第２閾値以上である場合に、前記第２ノルムを用いて正規化した前記第３逆ヤコビ行列に、前記移動指令値を適用して、前記動作指令値を決定することと、前記第１処理において、前記第２ノルムが前記第２閾値未満である場合に、前記第２ノルムを用いて正規化した前記第３逆ヤコビ行列において、前記第２ノルムを前記第２閾値に変更して得られる逆ヤコビ行列に、前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させてもよい。

　上記態様によると、第２ノルムは、移動部の指令速度の方向成分に対応する移動部の移動速度の大きさを示す。第２ノルムが小さくなると、正規化した第３逆ヤコビ行列が発散し得る。第２ノルムが第２閾値未満である場合に、正規化した第３逆ヤコビ行列において、第２ノルムを第２閾値に変更することによって、正規化した第３逆ヤコビ行列の発散が防がれる。よって、特異点の近傍において、移動部の速度が急激に変化することが防がれる。

　上記の第５態様から第８態様のいずれかにおいて、本開示の第９態様に係るプログラムは、前記第１処理において、前記移動指令値に含まれる前記移動部の指令速度が、前記第２基底ベクトルの方向に寄与する割合である寄与度を決定することと、前記第１処理において、前記寄与度が第３閾値以上である場合に、前記第３逆ヤコビ行列を用いて、前記動作指令値を決定することと、前記第１処理において、前記寄与度が前記第３閾値未満である場合に、前記第１変換後の前記第１変換特異行列内の前記特異値の最小値に対応する対角要素の値を０にするように、前記第３逆ヤコビ行列を第４逆ヤコビ行列に変換することと、前記第１処理において、前記寄与度が前記第３閾値未満である場合に、前記第４逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させてもよい。

　上記態様によると、移動部の指令速度が、移動部の移動速度空間を表す第２基底ベクトルの方向に寄与しない成分を含む場合、第１変換後の第１変換特異行列内の最小の特異値に対応する対角要素の値を０にするように、第３逆ヤコビ行列が第４逆ヤコビ行列に変換される。第４逆ヤコビ行列を用いることによって、移動指令値に対する、動作指令値によって実現される移動部の速度の誤差が低減できる。

　上記の第５態様から第９態様のいずれかにおいて、本開示の第１０態様に係るプログラムは、前記第１処理において、前記移動指令値に含まれる前記移動部の指令速度が、前記第２基底ベクトルの方向に寄与する割合である寄与度のうち、前記第１変換特異行列内の前記特異値のうちで最小である第１特異値に対応する第１寄与度を決定することと、前記第１処理において、前記第１特異値と、前記第１変換特異行列内の前記特異値のうちで前記第１特異値の次に小さい第２特異値とのいずれもが、前記第１閾値よりも小さく、前記第１寄与度が第３閾値以上である場合、前記第１変換特異行列内の対角要素それぞれに、前記第１特異値と前記第１閾値との第１比率を乗算して、前記第３逆ヤコビ行列を生成し、前記第３逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することと、前記第１処理において、前記第１特異値と前記第２特異値とのいずれもが、前記第１閾値よりも小さく、前記第１特異値の前記第１寄与度が前記第３閾値未満である場合、前記第１変換特異行列内の前記第１特異値に対応する対角要素の値を０にし、前記第１変換特異行列内の対角要素それぞれに、前記第２特異値と前記第１閾値との第２比率を乗算して、第５逆ヤコビ行列を生成し、前記第５逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させてもよい。

　上記態様によると、第１特異値及び第２特異値が第１閾値よりも小さい場合、第１特異値に対応する第１寄与度と第３閾値との関係に応じた逆ヤコビ行列を決定し、決定した逆ヤコビ行列を使用した動作指令値の決定が可能である。よって、２つの特異値が第１閾値よりも小さいような、ロボットが特異点にある又は特異点に接近している状態に対応した、ロボットの動作制御が可能である。

　上記の第１０態様において、本開示の第１１態様に係るプログラムは、前記第１処理において、前記第１特異値と前記第２特異値とのいずれもが、前記第１閾値よりも小さく、前記第１寄与度及び前記第２特異値に対応する第２寄与度が前記第３閾値未満である場合、前記第１変換特異行列内の前記第１特異値及び前記第２特異値に対応する対角要素の値を０にし、前記第１変換特異行列内の対角要素それぞれに、前記第２比率を乗算して、第６逆ヤコビ行列を生成し、前記第６逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させてもよい。

　上記態様によると、移動部の指令速度が、第１特異値及び第２特異値それぞれに対応する第２基底ベクトルの方向に寄与しない成分を含む場合、第１変換特異行列内の第１特異値及び第２特異値に対応する対角要素の値を０にするように、第６逆ヤコビ行列が生成される。第６逆ヤコビ行列を用いることによって、第１特異値及び第２特異値に対応する成分が取り除かれた動作指令値が生成される。よって、移動指令値に対する、動作指令値によって実現される移動部の速度の誤差が低減できる。

　上記の第５態様から第１１態様において、本開示の第１２態様に係るプログラムは、前記第１処理において、前記第１変換特異行列内の前記特異値のうちで最小である第１特異値と、前記第１変換特異行列内の前記特異値のうちで前記第１特異値の次に小さい２つ以上の前記特異値とを含む、前記第１特異値から第ｋ特異値（ｋは３以上の自然数）のいずれもが、前記第１閾値よりも小さい場合、前記移動指令値に含まれる前記移動部の指令速度が、前記第２基底ベクトルにおける前記第１特異値から前記第ｋ特異値それぞれに対応する方向に寄与する割合である第１寄与度から第ｋ寄与度を決定することと、前記第１処理において、前記第１寄与度から前記第ｋ寄与度が第３閾値以上である場合に、前記第１変換では、前記第１変換特異行列内の前記対角要素それぞれに、前記第１特異値と前記第１閾値との第１比率を乗算して、前記第３逆ヤコビ行列を生成し、前記第３逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することと、前記第１処理において、前記第１寄与度から前記第ｋ寄与度の少なくとも１つが前記第３閾値未満である場合に、前記第１変換では、前記第１変換特異行列内の前記寄与度が前記第３閾値未満である前記特異値に対応する対角要素の値を０にする零変換をし、且つ、前記零変換後の前記第１変換特異行列内の前記寄与度が前記第３閾値未満である前記特異値を除いた中で最小の特異値と前記第１閾値との第３比率を、前記零変換後の前記第１変換特異行列内の対角要素に乗算して、第７逆ヤコビ行列に変換することと、前記第１処理において、前記第１寄与度から前記第ｋ寄与度の少なくとも１つが前記第３閾値未満である場合に、前記第７逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させてもよい。

　上記態様によると、３つ以上の特異値が第１閾値よりも小さい場合も、２つの特異値が第１閾値よりも小さい場合と同様にして、各特異値に対応する寄与度に応じた逆ヤコビ行列を決定し、決定した逆ヤコビ行列を使用した動作指令値の決定が可能である。よって、３つの特異値が第１閾値よりも小さいような、ロボットが特異点にある又は特異点に接近している状態に対応した、ロボットの動作制御が可能である。

　上記の第１態様から第１２態様のいずれかにおいて、本開示の第１３態様に係るプログラムは、前記第１ヤコビ行列を時間微分して第８ヤコビ行列を生成することと、前記第１処理中での前記複数の関節の動作速度のうち、前記ロボットのセルフモーションに対応する前記第２基底ベクトル内の速度を前記第８ヤコビ行列に適用して加速度ベクトルを生成することと、前記第１処理において、前記加速度ベクトルのノルムが第４閾値以上である場合、前記加速度ベクトルのノルムが最大値を示すか否かを決定することと、前記加速度ベクトルのノルムが最大値を示す場合に、前記複数の関節の動作を停止する指令を出力することとを、コンピュータに実行させてもよい。

　上記態様によると、加速度ベクトルのノルムが第４閾値未満である場合、ロボットは、セルフモーションを継続可能な小さい加速度を発生する。加速度ベクトルのノルムが第４閾値以上である場合、ロボットは、セルフモーションにより特異点の近傍にある状態から抜け出すことができるような加速度を発生する。加速度ベクトルのノルムが第４閾値以上であり、且つ加速度ベクトルのノルムが最大値を示す場合、ロボットは、特異点に到達し、セルフモーションにより特異点からの逸脱と特異点への到達とを繰り返す振動現象を発生し得る。このような場合に、複数の関節の動作を一旦停止し、その後に動作することによって、振動現象を防ぐことができる。

　本開示の第１４態様に係る記憶媒体は、上記の第１態様から第１３態様のいずれかのプログラムを記憶する。上記態様によると、本開示の各態様に係るプログラムと同様の効果が得られる。記憶媒体は、非一時的で有形なコンピュータ読み取り可能な媒体であってもよい。記憶媒体は、非一時的で有形なコンピュータ読み取り可能な記録媒体であってもよい。記憶媒体の例は、１つ又は複数の半導体ベースの若しくは他の集積回路（ＩＣ）、ＲＯＭ、ＥＥＰＲＯＭ（Electrically Erasable and Programmable Read Only Memory ）、ＥＰＲＯＭ、フラッシュメモリ、ＣＤ－ＲＯＭ（Compact Disc Read Only Memory ）、ＣＤ－ＲＷ（Compact Disc Rewritable ）、ＤＶＤ（Digital Versatile Disk）、ハードディスクドライブ（ＨＤＤ）、ハイブリッドハードドライブ（ＨＨＤ）、光ディスク、光ディスクドライブ（ＯＤＤ）、光磁気ディスク、光磁気ドライブ、フロッピィ・ディスク、フロッピィ・ディスク・ドライブ（ＦＤＤ）、磁気テープ、固体ドライブ（ＳＳＤ）、ＲＡＭドライブ、セキュア・デジタル・カード、セキュア・デジタル・ドライブ、任意の他の適切な記憶媒体、又はこれらの２つ以上の組合せを含むことができる。集積回路の例は、フィールド・プログラマブル・ゲート・アレイ（ＦＰＧＡ）、特定用途向けＩＣ（ＡＳＩＣ）などを含み得る。記憶媒体は、揮発性、不揮発性、又は、揮発性と不揮発性との組合せであってもよい。

　本開示の第１５態様に係る情報処理装置は、上記の第１態様から第１３態様のいずれかのプログラムを記憶するメモリと、前記メモリに記憶された前記プログラムを実行するプロセッサとを含む。上記態様によると、本開示の各態様に係るプログラムと同様の効果が得られる。

　本開示の第１６態様に係るシステムは、上記の第１態様から第１３態様のいずれかのプログラムを実行するコンピュータを含む情報処理装置と、前記ロボットとを含み、前記情報処理装置は、ロボット操作が入力されるユーザインタフェース及び前記ロボットと接続され、前記情報処理装置は、前記ユーザインタフェースから、前記移動部を移動する指令を受信し、前記移動部を移動する指令に基づいた前記移動指令値を用いて、前記動作指令値を決定し、前記動作指令値を前記ロボットに送信し、前記情報処理装置は、前記ロボットの諸元の情報である第１情報を入力され、前記第１情報を用いて、前記第１ヤコビ行列を生成する。上記態様によると、本開示の各態様に係るプログラムと同様の効果が得られる。

　本開示の第１７態様に係るロボットの動作を制御する方法は、複数の関節と前記複数の関節が動作することによって移動される移動部とを含むロボットの動作を制御する方法であって、前記複数の関節の動作速度と前記移動部の移動速度とを関連付ける第１ヤコビ行列を特異値分解して、前記関節の動作速度空間を表す第１基底ベクトルを要素に含む第１行列と、前記移動部の移動速度空間を表す第２基底ベクトルを要素に含む第２行列と、特異値を対角要素に含む第１特異行列とを含む、第２ヤコビ行列を生成することと、前記特異値と第１閾値との比較結果に基づき、前記ロボットが特異点にある場合及び前記ロボットが前記特異点に接近している場合に実行される第１処理と、前記ロボットが前記特異点から離れている場合に実行される第２処理とのうちから、実行対象の処理を決定して実行することと、前記第１処理において、前記第１特異行列内の対角要素を変換して第３ヤコビ行列を生成することと、前記第１処理において、前記第３ヤコビ行列と、前記移動部の移動を指令する移動指令値とを用いて、前記複数の関節の動作を指令する動作指令値を決定し出力することと、前記第２処理において、前記第１ヤコビ行列又は前記第２ヤコビ行列と、前記移動指令値とを用いて、前記動作指令値を決定し出力することとを、含む。上記態様によると、本開示の各態様に係るプログラムと同様の効果が得られる。

　本開示のプログラムは、例えば、非一時的で有形なコンピュータ読み取り可能な記録媒体に記録されたプログラムであってもよく、記録媒体のドライブ装置を用いて当該記録媒体から読み出されコンピュータにインストールされるように構成されてもよい。当該プログラムは、例えば、インターネット等の伝送媒体を介して流通させることができるプログラムであってもよく、コンピュータにダウンロードされインストールされるように構成されてもよい。

　本開示の方法の一部及び全部は、例えば、ＣＰＵ、ＬＳＩなどの回路、ＩＣカード又は単体のモジュール等によって、実現されてもよい。本開示の方法に含まれる複数の要素は、１つの装置によって実現されてもよく、２つ以上の装置によって分担して実現されてもよい。

　本明細書で開示する要素の機能は、開示された機能を実行するよう構成又はプログラムされた汎用プロセッサ、専用プロセッサ、集積回路、ＡＳＩＣ、従来の回路、及び／又は、それらの組み合わせ、を含む回路又は処理回路を使用して実行できる。プロセッサは、トランジスタやその他の回路を含むため、処理回路又は回路と見なされる。本開示において、回路、ユニット、又は手段は、列挙された機能を実行するハードウェアであるか、又は、列挙された機能を実行するようにプログラムされたハードウェアである。ハードウェアは、本明細書に開示されているハードウェアであってもよいし、あるいは、列挙された機能を実行するようにプログラム又は構成されているその他の既知のハードウェアであってもよい。ハードウェアが回路の一種と考えられるプロセッサである場合、回路、手段、又はユニットはハードウェアとソフトウェアの組み合わせであり、ソフトウェアはハードウェア及び／又はプロセッサの構成に使用される。

　本明細書で用いた序数、数量等の数字は、全て本開示の技術を具体的に説明するために例示するものであり、本開示は例示された数字に制限されない。構成要素間の接続関係は、本開示の技術を具体的に説明するために例示するものであり、本開示の機能を実現する接続関係はこれに限定されない。

　本開示は、その本質的な特徴の範囲から逸脱することなく、様々なかたちで実施され得るように、本開示の範囲は、明細書の記載よりも添付の請求項によって定義されるため、例示的な実施の形態及び変形例は、例示的なものであって限定的なものではない。請求項及びその範囲内にあるすべての変更、又は、請求項及びその範囲の均等物は、請求項によって包含されることが意図されている。

Claims

　複数の関節と前記複数の関節が動作することによって移動される移動部とを含むロボットの動作を制御するプログラムであって、
　前記複数の関節の動作速度と前記移動部の移動速度とを関連付ける第１ヤコビ行列を特異値分解して、前記関節の動作速度空間を表す第１基底ベクトルを要素に含む第１行列と、前記移動部の移動速度空間を表す第２基底ベクトルを要素に含む第２行列と、特異値を対角要素に含む第１特異行列とを含む、第２ヤコビ行列を生成することと、
　前記特異値と第１閾値との比較結果に基づき、前記ロボットが特異点にある場合及び前記ロボットが前記特異点に接近している場合に実行される第１処理と、前記ロボットが前記特異点から離れている場合に実行される第２処理とのうちから、実行対象の処理を決定して実行することと、
　前記第１処理において、前記第１特異行列内の対角要素を変換して第３ヤコビ行列を生成することと、
　前記第１処理において、前記第３ヤコビ行列と、前記移動部の移動を指令する移動指令値とを用いて、前記複数の関節の動作を指令する動作指令値を決定し出力することと、
　前記第２処理において、前記第１ヤコビ行列又は前記第２ヤコビ行列と、前記移動指令値とを用いて、前記動作指令値を決定し出力することとを、コンピュータに実行させる
　プログラム。
　前記動作指令値の決定に用いたヤコビ行列に前記動作指令値を適用して得られるベクトルのノルムである第１ノルムを算出することと、
　前記第１ノルムが前記移動指令値よりも大きい場合、前記第１ノルムを用いて前記動作指令値を補正して補正動作指令値を決定し、前記動作指令値として前記補正動作指令値を出力することと、
　前記第１ノルムが前記移動指令値以下である場合、前記動作指令値を出力することとを、コンピュータに実行させる
　請求項１に記載のプログラム。
　前記ロボットの諸元の情報である第１情報を受け取り、前記第１情報と、前記複数の関節の状態の情報とを用いて、前記第１ヤコビ行列を生成することを、コンピュータに実行させる
　請求項１に記載のプログラム。
　前記特異値の最小値が前記第１閾値よりも小さい場合に前記第１処理の実行を決定し、前記特異値の最小値が前記第１閾値以上の場合に前記第２処理の実行を決定することを、コンピュータに実行させる
　請求項１に記載のプログラム。
　前記第２ヤコビ行列の逆行列である第２逆ヤコビ行列を生成することであって、前記第２逆ヤコビ行列は、前記第１行列、前記第２行列及び前記第１特異行列それぞれから変換された第１変換行列、第２変換行列及び第１変換特異行列を含むことと、
　前記第１処理において、前記第１変換特異行列内の対角要素を変換する第１変換を行うことにより、前記第３ヤコビ行列の逆行列である第３逆ヤコビ行列を生成することと、
　前記第１処理において、前記第３逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することと、
　前記第２処理において、前記第１ヤコビ行列の逆行列である第１逆ヤコビ行列又は前記第２逆ヤコビ行列に、前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させる
　請求項１に記載のプログラム。
　前記第１変換では、前記第１変換特異行列内の前記特異値を含む対角要素間の比率を維持するように、前記対角要素それぞれを変換することを、コンピュータに実行させる
　請求項５に記載のプログラム。
　前記第１変換では、前記第１変換特異行列内の前記特異値の最小値と前記第１閾値との第１比率を、前記対角要素それぞれに乗算することを、コンピュータに実行させる
　請求項６に記載のプログラム。
　前記第１処理において、前記移動指令値に含まれる前記移動部の指令速度の方向を表す方向ベクトルを前記第３逆ヤコビ行列に適用して得られる、ベクトルのノルムである第２ノルムを算出することと、
　前記第１処理において、前記第２ノルムが第２閾値以上である場合に、前記第２ノルムを用いて正規化した前記第３逆ヤコビ行列に、前記移動指令値を適用して、前記動作指令値を決定することと、
　前記第１処理において、前記第２ノルムが前記第２閾値未満である場合に、前記第２ノルムを用いて正規化した前記第３逆ヤコビ行列において、前記第２ノルムを前記第２閾値に変更して得られる逆ヤコビ行列に、前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させる
　請求項５に記載のプログラム。
　前記第１処理において、前記移動指令値に含まれる前記移動部の指令速度が、前記第２基底ベクトルの方向に寄与する割合である寄与度を決定することと、
　前記第１処理において、前記寄与度が第３閾値以上である場合に、前記第３逆ヤコビ行列を用いて、前記動作指令値を決定することと、
　前記第１処理において、前記寄与度が前記第３閾値未満である場合に、前記第１変換後の前記第１変換特異行列内の前記特異値の最小値に対応する対角要素の値を０にするように、前記第３逆ヤコビ行列を第４逆ヤコビ行列に変換することと、
　前記第１処理において、前記寄与度が前記第３閾値未満である場合に、前記第４逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させる
　請求項５に記載のプログラム。
　前記第１処理において、前記移動指令値に含まれる前記移動部の指令速度が、前記第２基底ベクトルの方向に寄与する割合である寄与度のうち、前記第１変換特異行列内の前記特異値のうちで最小である第１特異値に対応する第１寄与度を決定することと、
　前記第１処理において、前記第１特異値と、前記第１変換特異行列内の前記特異値のうちで前記第１特異値の次に小さい第２特異値とのいずれもが、前記第１閾値よりも小さく、前記第１寄与度が第３閾値以上である場合、前記第１変換特異行列内の対角要素それぞれに、前記第１特異値と前記第１閾値との第１比率を乗算して、前記第３逆ヤコビ行列を生成し、前記第３逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することと、
　前記第１処理において、前記第１特異値と前記第２特異値とのいずれもが、前記第１閾値よりも小さく、前記第１特異値の前記第１寄与度が前記第３閾値未満である場合、前記第１変換特異行列内の前記第１特異値に対応する対角要素の値を０にし、前記第１変換特異行列内の対角要素それぞれに、前記第２特異値と前記第１閾値との第２比率を乗算して、第５逆ヤコビ行列を生成し、前記第５逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させる
　請求項５に記載のプログラム。
　前記第１処理において、前記第１特異値と前記第２特異値とのいずれもが、前記第１閾値よりも小さく、前記第１寄与度及び前記第２特異値に対応する第２寄与度が前記第３閾値未満である場合、前記第１変換特異行列内の前記第１特異値及び前記第２特異値に対応する対角要素の値を０にし、前記第１変換特異行列内の対角要素それぞれに、前記第２比率を乗算して、第６逆ヤコビ行列を生成し、前記第６逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させる
　請求項１０に記載のプログラム。
　前記第１処理において、前記第１変換特異行列内の前記特異値のうちで最小である第１特異値と、前記第１変換特異行列内の前記特異値のうちで前記第１特異値の次に小さい２つ以上の前記特異値とを含む、前記第１特異値から第ｋ特異値（ｋは３以上の自然数）のいずれもが、前記第１閾値よりも小さい場合、前記移動指令値に含まれる前記移動部の指令速度が、前記第２基底ベクトルにおける前記第１特異値から前記第ｋ特異値それぞれに対応する方向に寄与する割合である第１寄与度から第ｋ寄与度を決定することと、
　前記第１処理において、前記第１寄与度から前記第ｋ寄与度が第３閾値以上である場合に、前記第１変換では、前記第１変換特異行列内の前記対角要素それぞれに、前記第１特異値と前記第１閾値との第１比率を乗算して、前記第３逆ヤコビ行列を生成し、前記第３逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することと、
　前記第１処理において、前記第１寄与度から前記第ｋ寄与度の少なくとも１つが前記第３閾値未満である場合に、前記第１変換では、前記第１変換特異行列内の前記寄与度が前記第３閾値未満である前記特異値に対応する対角要素の値を０にする零変換をし、且つ、前記零変換後の前記第１変換特異行列内の前記寄与度が前記第３閾値未満である前記特異値を除いた中で最小の特異値と前記第１閾値との第３比率を、前記零変換後の前記第１変換特異行列内の対角要素に乗算して、第７逆ヤコビ行列に変換することと、
　前記第１処理において、前記第１寄与度から前記第ｋ寄与度の少なくとも１つが前記第３閾値未満である場合に、前記第７逆ヤコビ行列に前記移動指令値を適用して、前記動作指令値を決定することとを、コンピュータに実行させる
　請求項５に記載のプログラム。
　前記第１ヤコビ行列を時間微分して第８ヤコビ行列を生成することと、
　前記第１処理中での前記複数の関節の動作速度のうち、前記ロボットのセルフモーションに対応する前記第２基底ベクトル内の速度を前記第８ヤコビ行列に適用して加速度ベクトルを生成することと、
　前記第１処理において、前記加速度ベクトルのノルムが第４閾値以上である場合、前記加速度ベクトルのノルムが最大値を示すか否かを決定することと、
　前記加速度ベクトルのノルムが最大値を示す場合に、前記複数の関節の動作を停止する指令を出力することとを、コンピュータに実行させる
　請求項１に記載のプログラム。
　請求項１から１３のいずれか一項に記載のプログラムを記憶する記憶媒体。
　請求項１から１３のいずれか一項に記載のプログラムを記憶するメモリと、前記メモリに記憶された前記プログラムを実行するプロセッサとを含む情報処理装置。
　請求項１から１３のいずれか一項に記載のプログラムを実行するコンピュータを含む情報処理装置と、
　前記ロボットとを含み、
　前記情報処理装置は、ロボット操作が入力されるユーザインタフェース及び前記ロボットと接続され、
　前記情報処理装置は、前記ユーザインタフェースから、前記移動部を移動する指令を受信し、前記移動部を移動する指令に基づいた前記移動指令値を用いて、前記動作指令値を決定し、前記動作指令値を前記ロボットに送信し、
　前記情報処理装置は、前記ロボットの諸元の情報である第１情報を入力され、前記第１情報を用いて、前記第１ヤコビ行列を生成する
　システム。
　複数の関節と前記複数の関節が動作することによって移動される移動部とを含むロボットの動作を制御する方法であって、
　前記複数の関節の動作速度と前記移動部の移動速度とを関連付ける第１ヤコビ行列を特異値分解して、前記関節の動作速度空間を表す第１基底ベクトルを要素に含む第１行列と、前記移動部の移動速度空間を表す第２基底ベクトルを要素に含む第２行列と、特異値を対角要素に含む第１特異行列とを含む、第２ヤコビ行列を生成することと、
　前記特異値と第１閾値との比較結果に基づき、前記ロボットが特異点にある場合及び前記ロボットが前記特異点に接近している場合に実行される第１処理と、前記ロボットが前記特異点から離れている場合に実行される第２処理とのうちから、実行対象の処理を決定して実行することと、
　前記第１処理において、前記第１特異行列内の対角要素を変換して第３ヤコビ行列を生成することと、
　前記第１処理において、前記第３ヤコビ行列と、前記移動部の移動を指令する移動指令値とを用いて、前記複数の関節の動作を指令する動作指令値を決定し出力することと、
　前記第２処理において、前記第１ヤコビ行列又は前記第２ヤコビ行列と、前記移動指令値とを用いて、前記動作指令値を決定し出力することとを、含む方法。