(57)【要約】【課題】被験者の姿勢に応じた最適なリハビリテーション訓練を行うこと。【解決手段】リハビリテーション装置は、被験者によって操作される操作手段と、操作手段の操作量を検出する操作量検出手段と、操作手段を駆動する駆動手段と、駆動手段の駆動を制御する制御手段と、被験者の姿勢を検出する姿勢検出手段と、操作手段に作用する外力を検出する外力検出手段と、操作手段の操作目標位置を表示する表示手段と、を備える。制御手段は、姿勢検出手段により検出された姿勢に基づいて、被験者の身体の可操作性を示す可操作楕円を算出し、可操作楕円に基づいて操作目標位置を算出すると共に、外力検出手段により検出された外力と、操作目標位置と、に基づいて、操作手段に対する操作量の目標値を算出し、該操作量の目標値に操作量検出手段により検出された操作量が追従するように駆動手段を制御する。【選択図】図３
2015221073.tif 000002
【特許請求の範囲】
【請求項１】
リハビリテーションの被験者によって操作される操作手段と、
前記操作手段の操作量を検出する操作量検出手段と、
前記操作手段を駆動する駆動手段と、
前記駆動手段の駆動を制御する制御手段と、
被験者の姿勢を検出する姿勢検出手段と、
前記操作手段に作用する外力を検出する外力検出手段と、
前記操作手段の操作目標位置を表示する表示手段と、
を備えるリハビリテーション装置であって、
前記制御手段は、
前記姿勢検出手段により検出された被験者の姿勢に基づいて、被験者の身体の可操作性を示す可操作楕円を算出し、該算出した可操作楕円に基づいて前記操作目標位置を算出すると共に、
前記外力検出手段により検出された外力と、前記算出した操作目標位置と、に基づいて、前記操作手段に対する操作量の目標値を算出し、該算出した操作量の目標値に前記操作量検出手段により検出された操作量が追従するように前記駆動手段を制御する、
ことを特徴とするリハビリテーション装置。
【請求項２】
請求項１記載のリハビリテーション装置であって、
前記制御手段は、前記可操作楕円の長径及び短径の比率及び傾きを含む振幅の正弦波関数に基づいて前記表示手段の表示画面上における２次元座標系の前記操作目標位置を算出する、ことを特徴とするリハビリテーション装置。
【請求項３】
請求項２記載のリハビリテーション装置であって、
前記制御手段は、下記式を用いて前記表示手段における２次元座標系の操作目標位置ｐｆｉｎを算出する、ことを特徴とするリハビリテーション装置。
【数１１】
2015221073.tif 000013
但し、上記式において、θｒｏｔ＿ｋは前記表示手段における２次元座標系の原点と前記操作目標位置とを結ぶ線である仮想ガイドラインの回転角度、λｅｌｌｉｐは前記可操作楕円の長径及びと短径の比率、θｅｌｌｉｐは前記可操作楕円の傾き、Ｒｅｎは前記可操作楕円のノミナル半径である。
【請求項４】
請求項１乃至３のうちいずれか１項記載のリハビリテーション装置であって、
前記制御手段は、前記表示手段における２次元座標系の原点と前記操作目標位置とを結ぶ仮想ガイドラインを生成し、該仮想ガイドラインの方向に対して低負荷となり、該仮想ガイドラインと垂直方向に対して高負荷となるアドミタンス制御を前記駆動手段に対して行う、ことを特徴とするリハビリテーション装置。
【請求項５】
請求項４記載のリハビリテーション装置であって、
前記制御手段は、前記仮想ガイドラインの方向に対してスティッフネス定数及びダンピング定数のうち少なくとも一方を低く設定し、前記垂直方向に対してスティッフネス定数及びダンピング定数のうち少なくとも一方を高く設定したアドミタンス行列を用いて、前記操作手段に対する操作量の目標値を算出する、ことを特徴とするリハビリテーション装置。
【請求項６】
請求項４又は５記載のリハビリテーション装置であって、
前記制御手段は、前記外力検出手段により検出された外力をグローバル座標系の外力に変換し、該変換した外力を前記仮想ガイドラインの回転角度に基づいて回転変換し、該回転変換した値を前記アドミタンス行列を用いた変換を行い、該変換した値を前記仮想ガイドラインの回転角度に基づいて逆回転変換を行い、該逆回転変換した値を逆キネマティクスを用いて変換を行い、前記操作手段に対する操作量の目標値を算出する、ことを特徴とするリハビリテーション装置。
【請求項７】
請求項４又は５記載のリハビリテーション装置であって、
前記制御手段は、下記式を用いて前記操作手段に対する操作量の目標値θｒｅｆを算出する、ことを特徴とするリハビリテーション装置。
【数１２】
2015221073.tif 000014
但し、上記式において、θｒｏｔ＿ｋは前記表示手段における２次元座標系の原点と前記操作目標位置とを結ぶ線である仮想ガイドラインの回転角度、回転変換Ｒ（θｒｏｔ＿ｋ）は回転変換行列、Ｇａｄｍ（ｓ）はアドミタンス行列、ＲＴ（θｒｏｔ＿ｋ）は逆回転変換行列、ｆｅｘｔはグローバル座標系における前記外力、Ｄａｄｍ１は前記仮想ガイドラインの方向に対するダンピング定数、Ｋａｄｍ１は前記仮想ガイドラインの方向に対するスティッフネス定数、Ｄａｄｍ２は前記垂直方向に対するダンピング定数、Ｋａｄｍ２は前記垂直方向に対するスティッフネス定数である。
【請求項８】
請求項５記載のリハビリテーション装置であって、
前記制御手段は、前記操作量検出手段により検出された操作量に対応する前記表示手段上の現在位置が前記仮想ガイドラインから前記垂直方向に外れるに従がって、前記ダンピング定数及び／又はスティッフネス定数を徐々高く設定する、ことを特徴とするリハビリテーション装置。
【請求項９】
請求項５記載のリハビリテーション装置であって、
前記制御手段は、前記操作量検出手段により検出された操作量に対応する前記表示手段上の現在位置が前記可操作楕円の外側に出たと判断すると、ダンピング定数及び／又はスティッフネス定数を所定量増加させる、ことを特徴とするリハビリテーション装置。
【請求項１０】
請求項１乃至９のうちいずれか１項記載のリハビリテーション装置であって、
前記姿勢検出手段は、前記被験者の身体の各関節角度を検出し、
前記制御手段は、前記姿勢検出手段により検出された各関節角度と、前記身体のヤコビアン行列と、に基づいて前記可操作楕円を算出する、ことを特徴とするリハビリテーション装置。
【請求項１１】
リハビリテーションの被験者によって操作される操作手段の操作量を検出するステップと、
被験者の姿勢を検出するステップと、
前記操作手段に作用する外力を検出するステップと、
前記操作手段の操作目標位置を表示するステップと、
を含む制御方法であって、
前記検出された被験者の姿勢に基づいて、被験者の身体の可操作性を示す可操作楕円を算出するステップと、
該算出した可操作楕円に基づいて前記操作目標位置を算出するステップと、
前記検出された外力と、前記算出した操作目標位置と、に基づいて、前記操作手段に対する操作量の目標値を算出するステップと、
該算出した操作量の目標値に前記検出された操作量が追従するように前記操作手段を駆動する駆動手段を制御するステップと、
を含む、ことを特徴とする制御方法。
【請求項１２】
リハビリテーションの被験者の姿勢に基づいて、被験者の身体の可操作性を示す可操作楕円を算出する処理と、
該算出した可操作楕円に基づいて操作目標位置を算出する処理と、
被験者によって操作される操作手段の前記操作目標位置を表示する処理と、
前記操作手段に作用する外力と、前記算出した操作目標位置と、に基づいて、前記操作手段に対する操作量の目標値を算出する処理と、
該算出した操作量の目標値に前記操作手段の操作量が追従するように前記操作手段を駆動する駆動手段を制御する処理と、
をコンピュータに実行させる、ことを特徴とする制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、被験者の運動機能を回復させるリハビリテーションを行うためのリハビリテーション装置、制御方法及び制御プログラムに関するものである。
【背景技術】
【０００２】
運動機能が損なわれた人等は、リハビリテーションをすることにより運動機能の回復に努めることが行われ、そのようなリハビリテーションを適切に行うための様々な装置が開発されている。例えば、流体クラッチを介して駆動手段により負荷を被験者の上肢に与え、そのリハビリテーションを行う装置が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００６−２４７２８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、一般に被験者の身体運動にはその姿勢に応じて動作し難い方向が存在する。上記リハビリテーション装置においては、そのような被験者の姿勢を考慮していないため、その姿勢によっては、被験者に無理な負荷を強いるという問題が生じ得る。
【０００５】
本発明は、このような問題点に鑑みてなされたものであり、被験者の姿勢に応じた最適なリハビリテーション訓練を行うことができるリハビリテーション装置、制御方法及び制御プログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、リハビリテーションの被験者によって操作される操作手段と、前記操作手段の操作量を検出する操作量検出手段と、前記操作手段を駆動する駆動手段と、前記駆動手段の駆動を制御する制御手段と、被験者の姿勢を検出する姿勢検出手段と、前記操作手段に作用する外力を検出する外力検出手段と、前記操作手段の操作目標位置を表示する表示手段と、を備えるリハビリテーション装置であって、  前記制御手段は、前記姿勢検出手段により検出された被験者の姿勢に基づいて、被験者の身体の可操作性を示す可操作楕円を算出し、該算出した可操作楕円に基づいて前記操作目標位置を算出すると共に、前記外力検出手段により検出された外力と、前記算出した操作目標位置と、に基づいて、前記操作手段に対する操作量の目標値を算出し、該算出した操作量の目標値に前記操作量検出手段により検出された操作量が追従するように前記駆動手段を制御する、ことを特徴とするリハビリテーション装置である。なお、この一態様において、前記可操作楕円は、可操作楕円体を含むものとする。
この一態様において、前記制御手段は、前記可操作楕円の長径及び短径の比率及び傾きを含む振幅の正弦波関数に基づいて前記表示手段の表示画面上における２次元座標系の前記操作目標位置を算出してもよい。
この一態様において、前記制御手段は、下記式を用いて前記表示手段における２次元座標系の操作目標位置ｐｆｉｎを算出してもよい。
この一態様において、前記制御手段は、前記表示手段における２次元座標系の原点と前記操作目標位置とを結ぶ仮想ガイドラインを生成し、該仮想ガイドラインの方向に対して低負荷となり、該仮想ガイドラインと垂直方向に対して高負荷となるアドミタンス制御を前記駆動手段に対して行ってもよい。
この一態様において、前記制御手段は、前記仮想ガイドラインの方向に対してスティッフネス定数及びダンピング定数のうち少なくとも一方を低く設定し、前記垂直方向に対してスティッフネス定数及びダンピング定数のうち少なくとも一方を高く設定したアドミタンス行列を用いて、前記操作手段に対する操作量の目標値を算出してもよい。
この一態様において、前記制御手段は、前記外力検出手段により検出された外力をグローバル座標系の外力に変換し、該変換した外力を前記仮想ガイドラインの回転角度に基づいて回転変換し、該回転変換した値を前記アドミタンス行列を用いた変換を行い、該変換した値を前記仮想ガイドラインの回転角度に基づいて逆回転変換を行い、該逆回転変換した値を逆キネマティクスを用いて変換を行い、前記操作手段に対する操作量の目標値を算出してもよい。
この一態様において、前記制御手段は、下記式を用いて前記操作手段に対する操作量の目標値θｒｅｆを算出してもよい。
この一態様において、前記制御手段は、前記操作量検出手段により検出された操作量に対応する前記表示手段上の現在位置が前記仮想ガイドラインから前記垂直方向に外れるに従がって、前記ダンピング定数及び／又はスティッフネス定数を徐々高く設定してもよい。
この一態様において、前記制御手段は、前記操作量検出手段により検出された操作量に対応する前記表示手段上の現在位置が前記可操作楕円の外側に出たと判断すると、ダンピング定数及び／又はスティッフネス定数を所定量増加させてもよい。
この一態様において、前記姿勢検出手段は、前記被験者の身体の各関節角度を検出し、  前記制御手段は、前記姿勢検出手段により検出された各関節角度と、前記身体のヤコビアン行列と、に基づいて前記可操作楕円を算出してもよい。
上記目的を達成するための本発明の一態様は、リハビリテーションの被験者によって操作される操作手段の操作量を検出するステップと、被験者の姿勢を検出するステップと、  前記操作手段に作用する外力を検出するステップと、前記操作手段の操作目標位置を表示するステップと、を含むリハビリテーション装置の制御方法であって、前記検出された被験者の姿勢に基づいて、被験者の身体の可操作性を示す可操作楕円を算出するステップと、該算出した可操作楕円に基づいて前記操作目標位置を算出するステップと、前記検出された外力と、前記算出した操作目標位置と、に基づいて、前記操作手段に対する操作量の目標値を算出するステップと、該算出した操作量の目標値に前記検出された操作量が追従するように前記操作手段を駆動する駆動手段を制御するステップと、を含む、ことを特徴とするリハビリテーション装置の制御方法であってもよい。
上記目的を達成するための本発明の一態様は、リハビリテーションの被験者の姿勢に基づいて、被験者の身体の可操作性を示す可操作楕円を算出する処理と、該算出した可操作楕円に基づいて操作目標位置を算出する処理と、被験者によって操作される操作手段の前記操作目標位置を表示する処理と、前記操作手段に作用する外力と、前記算出した操作目標位置と、に基づいて、前記操作手段に対する操作量の目標値を算出する処理と、該算出した操作量の目標値に前記操作手段の操作量が追従するように前記操作手段を駆動する駆動手段を制御する処理と、をコンピュータに実行させる、ことを特徴とするリハビリテーション装置の制御プログラムであってもよい。
【発明の効果】
【０００７】
本発明によれば、被験者の姿勢に応じた最適なリハビリテーション訓練を行うことができるリハビリテーション装置、制御方法及び制御プログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施の形態に係るリハビリテーション装置の概略的なシステム構成を示すブロック図である。
【図２】グリップレバー部の操作を示す図である。
【図３】本発明の一実施の形態に係る制御装置のアシスト制御系の構成を示すブロック図である。
【図４】本発明の一実施の形態に係るアドミタンス制御による効果を示す図である。
【図５】被験者の上肢を水平面内で横方向に回転動作させたときの、可操作楕円の形状を示す図である。
【図６】図５左端における腕姿勢（ａ）のときの仮想ガイドラインを示す図である。
【図７】図５右端における腕姿勢（ｂ）のときの仮想ガイドラインを示す図である。
【図８】本発明の一実施の形態に係るリハビリテーション装置の制御処理フローを示すフローチャートである。
【発明を実施するための形態】
【０００９】
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の一実施の形態に係るリハビリテーション装置の概略的なシステム構成を示すブロック図である。本実施の形態に係るリハビリテーション装置１は、被験者によって操作されるグリップレバー部２と、被験者の上肢の姿勢を撮影するカメラ３と、グリップレバー部２に操作トルクを付与する第１及び第２サーボモータ４、５と、グリップレバー部２の操作量を検出する第１及び第２回転センサ６、７と、グリップレバー部２に作用する外力を検出する力センサ８と、第１及び第２サーボモータ４、５を制御する制御装置９と、各種の操作情報を表示する表示装置１０と、を備えている。
【００１０】
グリップレバー部２は、操作手段の一具体例であり、被験者が上肢のリハビリテーションを行うために操作を行うものである（図２）。グリップレバー部２は、複数の関節部を有する多関節型のロボットアームとして構成されている。グリップレバー部２は、例えば、筺体２１と筺体２１に回転可能に設けられた第１関節軸２２と、第１関節軸２２に連結された第１リンク２３と、第１リンク２３に連結された第２関節軸２４と、第２関節軸２４に連結された第２リンク２５と、第２リンク２５の先端に連結され被験者が把持するハンドル２６と、を有している。
【００１１】
第１リンク２３は第１関節軸２２を中心にして回転し、第２リンク２５は第２関節軸２４を中心にして回転する。この構成により、ハンドル２６は、２次元平面上の任意の位置に移動することができる。なお、上記グリップレバー部２の構成は一例であり、これに限らない。例えば、グリップレバー部２は３つ以上の関節部を有し、ハンドル２６が２次元平面あるいは３次元空間上の任意の位置に移動する構成であってもよい。被験者は、ハンドル２６を把持し、ハンドル２６を指示された方向に操作することで、リハビリテーション訓練を行う。
【００１２】
第１及び第２回転センサ６、７は、操作量検出手段の一具体例であり、グリップレバー部２の操作量を検出する。第１回転センサ６は、グリップレバー部２の第１関節軸２２の回転角を検出する。第２回転センサ７は、グリップレバー部２の第２関節軸２４の回転角を検出する。第１及び第２回転センサ６、７は、例えば、ポテンショメータ、ロータリーエンコーダなどで構成されている。
【００１３】
第１及び第２回転センサ６、７はＡ／Ｄ（アナログ／デジタル）変換器１１を介して制御装置９に接続されている。第１及び第２回転センサ６、７は、検出したグリップレバー部２の第１及び第２関節軸２２、２４の回転角に応じた回転角信号を制御装置９に出力する。
【００１４】
第１及び第２サーボモータ４、５は、駆動手段の一具体例であり、グリップレバー部２のハンドル２６に操作トルクを付与する機能を有している。第１サーボモータ４の駆動軸は、グリップレバー部２の第１関節軸２２に連結されている。第２サーボモータ５の駆動軸は、グリップレバー部２の第２関節軸２４に連結されている。第１及び第２サーボモータ４、５は、例えば、ＡＣ（交流式）サーボモータであり、減速機構を内蔵している。
【００１５】
第１及び第２サーボモータ４、５は、サーボアンプ１２及びＤ／Ａ（デジタル／アナログ）変換器１３を介して制御装置９に接続されている。第１及び第２サーボモータ４、５は制御装置９から送信される制御信号に応じて、第１及び第２関節軸２２、２４を介して、グリップレバー部２のハンドル２６にトルクを付与する。
【００１６】
力センサ８は、外力検出手段の一具体例であり、被験者がグリップレバー部２を操作した際のハンドル２６に作用する外力を検出する。力センサ８は、例えば、グリップレバー部２のハンドル２６の根元に設けられている。力センサ８は、Ａ／Ｄ変換器１１を介して制御装置９に接続されている。力センサ８は、検出した外力に応じた力値信号を制御装置９に出力する。
【００１７】
カメラ３は、姿勢検出手段の一具体例であり、被験者がグリップレバー部２を操作する際の被験者の上肢の姿勢を撮影する。例えば、被験者の腕（手首関節、肘関節、肩関節など）には、複数の画像マーカＭが取付られている。カメラ３は、被験者の腕の各画像マーカＭを撮影する。カメラ３は、撮影した撮影画像を制御装置９に出力する。
【００１８】
制御装置９は、制御手段の一具体例であり、第１及び第２サーボモータ４、５を制御する。制御装置９は、力センサ８から出力された力値信号と、第１及び第２回転センサ６、７から出力される回転角信号と、に基づいて、第１及び第２サーボモータ４、５に対するトルク指令値（操作量の目標値）を算出する。制御装置９は、算出したトルク指令値に応じた制御信号を生成し、第１及び第２サーボモータ４、５に対して出力する。第１及び第２サーボモータ４、５は、制御装置９からの制御信号に応じてグリップレバー部２にトルクを付与する。
【００１９】
なお、制御装置９は、例えば、演算処理、制御処理等と行うＣＰＵ（Central Processing Unit）９ａ、ＣＰＵ９ａによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ９ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）９ｃ、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ９ａ、メモリ９ｂ、及びインターフェイス部９ｃは、データバス９ｄなどを介して相互に接続されている。
【００２０】
表示装置１０は、表示手段の一具体例であり、被験者の操作に関する各種の操作情報を表示する。表示装置１０は制御装置９に接続されており、制御装置９から出力される情報に基づいて各種の操作情報を表示する。表示装置１０は、例えば、液晶ディスプレイ装置、有機ＥＬディスプレイ装置などで構成されている。
【００２１】
表示装置１０は、例えば、制御装置９から出力されるグリップレバー部２のハンドルの現在位置に対応するターゲットマーク□と、被験者が操作目標とするターゲットマーク○と、を同時に表示画面上に２次元座標系で表示させる。このターゲットマーク○は、被験者の上肢のリハビリテーション訓練を行う上でのハンドル操作の目標位置となる。被験者は、表示装置１０の表示画面上に表示されたターゲットマーク□を、そのトラッキング課題であるターゲットマーク○に追従させるように、グリップレバー部２のハンドル２６を操作する。
これにより、所望の関節運動の回復を可能とするリハビリテーション訓練が行われる。なお、上述したリハビリテーション訓練の方法は一例であり、これに限定されない。
【００２２】
ところで、一般に被験者の上肢などの身体運動にはその姿勢に応じて動作し難い方向が存在する。従来のリハビリテーション装置においては、そのような被験者の姿勢を考慮していないため、その姿勢によっては、被験者に無理な負荷を強いるという問題が生じ得る。
【００２３】
これに対し、本実施の形態に係るリハビリテーション装置１において、制御装置９は、カメラ３により撮影された被験者の姿勢の画像に基づいて、被験者の身体の可操作性を示す可操作楕円を算出し、該算出した可操作楕円に基づいてグリップレバー部２のハンドル２６の操作目標位置を算出し、表示装置１０に表示させる。さらに、制御装置９は、力センサ８により検出された外力と、算出した操作目標位置と、に基づいて、アドミタンス制御を行い、グリップレバー部２の第１及び第２関節軸２２、２４の制御目標値を算出し、該算出した制御目標値に第１及び第２回転センサ６、７により検出された第１及び第２関節軸２２、２４の回転角が追従するように第１及び第２サーボモータ４、５を制御する。
【００２４】
これにより、被験者の姿勢に基づいた可操作楕円を考慮して操作目標位置が設定される。このため、、被験者に無理な負荷を強いることがない。さらに、被験者がグリップレバー部２のハンドル２６を操作目標位置の方向に操作し易いようにガイドするアドミタンス制御を行う。これにより、被験者に掛かる負荷を適度に軽減できる。すなわち、被験者の上肢の姿勢に応じた最適なリハビリテーション訓練を行うことができる。
【００２５】
上記制御を実現するために、制御装置９は、第１及び第２回転センサ６、７により検出されたグリップレバー部２の第１及び第２関節軸２２、２４の回転角と、力センサ８により検出されたグリップレバー部２のハンドル２６に作用する外力と、カメラ３により撮影された被験者の上肢の姿勢の画像と、に基づいて、被験者によるグリップレバー部２のハンドル２６の操作を補助するアシスト制御を行う。表示装置１０の表示画面上で仮想のガイドラインに沿って動く操作目標値（ターゲットマーク）の振幅は、可操作楕円の大きさで制限される。
制御装置９は、上記アシスト制御を実行する上で、後述する、上位制御系及び下位制御系を実行する。
【００２６】
制御装置９は、上位制御系において、カメラ３により撮影された被験者の上肢の姿勢の画像に基づいて、可操作楕円を算出する。さらに、制御装置９は、上位制御系において、力センサ８により検出されたグリップレバー部２のハンドル２６に作用する外力に基づいて、２自由度アドミタンス制御を行い、ハンドル２６を操作目標位置へガイドする仮想のガイドラインを算出する。
【００２７】
制御装置９は、下位制御系において、上位制御系で算出した仮想ガイドラインに沿った第１及び第２関節軸２２、２４の制御目標値にグリップレバー部２の第１及び第２関節軸２２、２４の回転角を追従させる位置制御を行う。制御装置９は、この位置制御において、グリップレバー部２の第１及び第２関節軸２２、２４の回転角をフィードバックさせたＰＩＤ制御によるフィードバック制御と、慣性補償及び摩擦補償によるフィードフォワード制御を行い、第１及び第２サーボモータ４、５に対するトルク指令値を算出する。
【００２８】
次に、上述した上位制御系について、詳細に説明する。
図３は、本実施の形態に係る制御装置の概略的なシステム構成を示すブロック図である。
本実施の形態に係る制御装置９は、上位制御系の円ハフ変換部９１と、関節角度変換部９２と、可操作楕円算出部９３と、目標値算出部９４と、回転変換部９５と、アドミタンス制御部９６と、逆キネマティクス部９７と、を有している。
【００２９】
円ハフ変換部９１は、カメラ３から出力された各画像マーカの撮影画像を、円のハフ変換を行い、各画像マーカの中心位置ｐｃｊを算出する。
ｐｃｊ＝（ｉｍｇｒａｙ、Ｒｃｉｒ）
ここで、ｉｍｇｒａｙはグレイスケールイメージであり、Ｒｃｉｒは画像マーカの半径である。ｐｃｊ＝[ｕｊ、ｖｊ、１]Ｔは、画像座標系における画像マーカの中心位置である。
【００３０】
関節角度変換部９２は、各画像マーカの中心位置ｐｃｊを、周知の変換行列Ｍｐｅｒｓによりワールド座標系（ＸＹ座標系）の中心位置ｐｊに変換する。
【数１】
2015221073.tif 000003
【００３１】
上記式において、ｐｊ＝[ｐｊｘ、ｐｊｙ]Ｔはワールド座標系における各画像マーカの中心位置であり、ｊ＝１、２、３は、被験者の上肢の肩関節、肘関節、及び手首関節を夫々示している。但し、上肢の肩関節、肘関節、及び手首関節は、同一の高さ平面内で運動しているものとし、その高さ情報は省略する。
【００３２】
ここで、幾何学的関係により上肢の肩関節角度ｑ１及び肘関節角度ｑ２は、下記式で表すことができる。
【数２】
2015221073.tif 000004
【００３３】
したがって、上肢のヤコビアン行列は下記式で表すことができる。なお、下記式において、Ｌ１は上肢の上腕の長さであり、Ｌ２は上肢の前腕の長さである。
【数３】
2015221073.tif 000005
【００３４】
関節角度変換部９２は、上記式を用いて、上肢のヤコビアン行列を算出する。
可操作楕円算出部９３は、下記式を用いてヤコビアン行列を特異値分解し、被験者の手先の可操作性を示す可操作楕円の長軸の大きさσｌｏｎｇ、短軸の大きさσｓｈｏｒｔ、および方向（Ｕｈの第１列ベクトル）を算出する。
【数４】
2015221073.tif 000006
【００３５】
可操作楕円算出部９３は、可操作楕円の半径比率λｅｌｌｉｐ及び傾きθｅｌｌｉｐは、下記式を用いて算出する。
【数５】
2015221073.tif 000007
【００３６】
目標値算出部９４は、可操作楕円算出部９３により算出された可操作楕円の半径比率λｅｌｌｉｐ及び傾きθｅｌｌｉｐに基づいて、下記式を用いてトラッキング課題の最終目標値ｐｆｉｎ＝[ｐｒｘ、ｐｒｙ]Ｔを算出する。
ｐｆｉｎ＝ｆ（λｅｌｌｉｐ、θｅｌｌｉｐ）
【００３７】
但し、上記最終目標値ｐｆｉｎは、トラッキング課題において、被験者が表示装置１０の表示画面上で目視により操作目標とする操作目標位置（例えば、ターゲットマークの２次元座標位置）である。
【００３８】
目標値算出部９４は、算出した最終目標値ｐｆｉｎを表示装置１０に対して出力する。表示装置１０は、表示画面の２次元座標系において、目標値算出部９４から出力された最終目標値ｐｆｉｎを操作目標のターゲットマークとして、グリップレバー部２のハンドル２６の現在位置を示すターゲットマークと共に表示する。
【００３９】
なお、目標値算出部９４は、例えば、第１及び第２回転センサ６、７から出力されるグリップレバー部２の第１及び第２関節軸２２、２４の回転角に基づいて順キネマティクスを用いてグリップレバー部２のハンドル２６の現在位置に対応するターゲットマークの位置を算出する。
【００４０】
上記最終目標値ｐｆｉｎを下記式で表すことができる。下記式において、Ｒ（θｒｏｔ＿ｋ）は回転行列である。θｒｏｔ＿ｋ＝０＋π（ｋ−１）／８（ｋ＝１、・・・、８）は、最終目標値ｐｆｉｎの主軸（２次元座標系の原点とｐｆｉｎとを結ぶ線であり、仮想のガイドライン）の回転角度である。
【数６】
2015221073.tif 000008
【００４１】
上記式において、ｐｒｘ０は、最終目標値の信号であり、下記正弦波で表すことができる。
ｐｒｘ０＝Ａｒｓｉｎ（２πｆｔ）
上記正弦波の振幅Ａｒは、最終目標値ｐｆｉｎの主軸の回転角度θｒｏｔ＿ｋだけでなく、可操作楕円の半径に応じて変化する。
【００４２】
すなわち、正弦波の振幅Ａｒは、可操作楕円の長軸方向（被験者の上肢が動作し易い方向）には大きく変化する。このため、表示装置１０の表示画面上において可操作楕円の長軸方向に最終目標値ｐｆｉｎは大きく変化する。一方、正弦波の振幅Ａｒは、可操作楕円の短軸方向（被験者の上肢が動作し難い方向）には小さく変化する。このため、表示装置１０の表示画面上において可操作楕円の短軸方向に最終目標値ｐｆｉｎは小さく変化する。このように、被験者に無理のないトラッキング課題となるような最終目標値ｐｆｉｎが生成される。
【００４３】
但し、上記式において、正弦波の周波数を、例えばｆ＝０．３（Ｈｚ）とし、振幅Ａｒを下記式で表すことができる。
【数７】
2015221073.tif 000009
【００４４】
上記式において、可操作楕円の長径Ｒｅｌ及び短径Ｒｅｓは、下記式を用いて算出できる。下記式において、Ｒｅｎは可操作楕円のノミナル半径であり、予め設計時に設定される設計パラメータである。
【数８】
2015221073.tif 000010
【００４５】
なお、回転角度θｒｏｔ＿ｋ＝０＋π（ｋ−１）／８（ｋ＝１、・・・、８）としているが、これに限らない。例えば、回転角度θｒｏｔ＿ｋ＝０＋π（ｋ−１）／ｎ（ｋ＝１、・・・、ｎ）としもよい。ｎを増加させることで、グリップレバー部２のハンドル２６をより滑らかにガイドすることができる。
目標値算出部９４は、算出した最終目標値ｐｆｉｎの主軸の回転角度θｒｏｔ＿ｋを回転変換部９５に出力する。
【００４６】
続いて、グリップレバー部２の操作をガイドするための、仮想のガイドライン（以下、仮想ガイドライン）を作成する２自由度アドミタンス制御について詳細に説明する。
回転変換部９５は、第１回転センサ６から出力される第１関節軸２２の回転角θ１と第２回転センサ７から出力される第２関節軸２４の回転角θ２と、に基づいて、力センサ８から出力される力値信号を、下記式を用いて回転変換することでワールド座標系の力値信号に変換する。ここで、上記回転変換前の力値信号（力ベクトル）は、ｆｅｘｔ０＝[ｆｅｘ０  ｆｅｙ０]Ｔであり、上記回転変換後の力値信号（力ベクトル）は、ｆｅｘｔ＝[ｆｅｘ  ｆｅｙ]Ｔである。
ｆｅｘｔ＝Ｒ（θｘ）ｆｅｘｔ０
θｘ＝θ１＋θ２−π／２
【００４７】
アドミタンス制御部９６は、回転変換部９５により変換された力信号ｆｅｘｔに基づいて、下記式を用いて、作業空間におけるグリップレバー部２のハンドル２６の操作目標位置である制御目標値ｐｒｅｆを算出する。
【００４８】
アドミタンス制御部９６は、下記式に示す如く、回転変換部９５からの力値信号ｆｅｘｔに対して、最終目標値ｐｆｉｎの主軸の回転角度θｒｏｔ＿ｋ＝０＋π（ｋ−１）／８を用いて回転変換Ｒ（θｒｏｔ＿ｋ）を行う。そして、アドミタンス制御部９６は、回転変換Ｒ（θｒｏｔ＿ｋ）後、アドミタンス行列Ｇａｄｍ（ｓ）による変換（アドミタンス制御）を行い、逆回転変換ＲＴ（θｒｏｔ＿ｋ）を行うことで、制御目標値ｐｒｅｆを算出する。
【数９】
2015221073.tif 000011
【００４９】
上記式において、Ｄａｄｍ１は主軸方向（仮想ガイドラインの方向）に対するダンピング定数であり、Ｋａｄｍ１は主軸方向のスティッフネス定数である。Ｄａｄｍ２は副軸方向（仮想ガイドラインの方向に対して垂直方向）に対するダンピング定数であり、Ｋａｄｍ２は副軸方向のスティッフネス定数である。
【００５０】
例えば、主軸方向のダンピング定数Ｄａｄｍ１及び／又はスティッフネス定数Ｋａｄｍ１を低く設定し、副軸方向のダンピング定数Ｄａｄｍ２及び／又はスティッフネス定数Ｋａｄｍ２を高く設定する。これにより、グリップレバー部２のハンドル２６を主軸方向に操作したとき低負荷となり、副軸方向に操作したとき高負荷となる。したがって、被験者はグリップレバー部２のハンドル２６を仮想ガイドラインの方向に操作しとき低負荷を感じ、グリップレバー部２のハンドル２６を仮想ガイドラインから外れた方向に操作を行ったとき、高負荷を感じる。このようにして、グリップレバー部２のハンドル２６の操作を仮想ガイドラインに沿って操作目標位置（制御目標値ｐｒｅｆ）の方向に自然とガイドすることができる。したがって、被験者は、グリップレバー部２のハンドル２６を操作目標位置に向けて容易に操作できるため、その負荷を適度に軽減できる。
【００５１】
逆キネマティクス部９７は、下記逆キネマティクス関数ＩＫ（）を用いて、アドミタンス制御部９６から出力される作業空間における制御目標値ｐｒｅｆを関節空間における制御目標値θｒｅｆに変換する。
θｒｅｆ＝ＩＫ（ｐｒｅｆ）
【００５２】
次に、上述した下位制御系について、詳細に説明する。
制御装置９は、下位制御系において、上位制御系で算出した関節空間の制御目標値θｒｅｆ＝[θｒ１  θｒ２]をグリップレバー部２の第１及び第２関節軸２２、２４の回転角θ１、θ２を追従させる位置制御を行う。下位制御系は、ＰＩＤ制御からなるフィードバック部分と慣性補償及び摩擦補償からなるフィードフォワード部分とを含む。
【００５３】
制御装置９は、上位制御系の逆キネマティクス部９７から出力された制御目標値θｒｅｆに、第１及び第２回転センサ６、７により検出されたグリップレバー部２の第１及び第２関節軸２２、２４の回転角θ１、θ２を追従させるように、第１及び第２サーボモータ４、５に対するトルク指令値τ１、τ２を下記式を用いて夫々算出する。制御装置９は、算出したトルク指令値τ１、τ２に応じた制御信号を生成し第１及び第２サーボモータ４、５に対して出力することで、第１及び第２サーボモータ４、５を夫々制御する。
【数１０】
2015221073.tif 000012
【００５４】
上記式において、ｉ＝１、２であり、ｉ＝１は第１関節軸２２を表し、ｉ＝２は第２関節軸２４を表している。Ｉｍハットはグリップレバー部２のハンドル２６の慣性モーメントの推定値、Ｂｍハットは粘性摩擦項係数の推定値、Ｄｍハットは動摩擦係数の推定値、を夫々示している。Ｉｍハット、Ｂｍハット、Ｄｍハットは、慣性補償および摩擦補償のため最小２乗法によりオフライン同定されている。Ｋｐ、Ｋｉ、Ｋｄは、ＰＩＤ制御の比例ゲイン、積分ゲイン、微分ゲインを夫々示している。
【００５５】
制御装置９は、上記式に基づいて、慣性補償部、摩擦補償部、及び、ＰＩＤ制御によるフィードバック部を含む、下位制御を実行することができる。この下位制御系において、慣性補償部及び、特に、摩擦補償部を含むことで、安価なサーボモータを用いることができるため、コスト低減に繋がる。
【００５６】
制御装置９は、グリップレバー部２の第１及び第２関節軸２２、２４の回転角θ＝[θ１  θ２]Ｔから下記式の順キネマティクスを用いて、作業空間におけるグリップレバー部２のハンドル２６の現在位置ｐ＝[ｐｘ  ｐｙ]Ｔを算出し、表示装置１０に出力する。表示装置１０は、表示画面の２次元座標系で、グリップレバー部２のハンドル２６の現在位置ｐ＝[ｐｘ  ｐｙ]Ｔに対応した位置にターゲットマークを表示させる。
ｐ＝ＦＫ（θ）
【００５７】
図４は、本実施の形態に係るアドミタンス制御による効果を示す図である。但し、本アドミタンス制御において、可操作楕円は考慮されていない。図４に示す如く、作業空間において、仮想ガイドラインにより円周方向に２２．５（ｄｅｇ）毎に等分割されており、その仮想ガイドラインに沿って、グリップレバー部２のハンドルをスムーズに操作可能であることが確認できる。
【００５８】
図５は、被験者の上肢を水平面内で横方向に回転動作させたときの、可操作楕円の形状を示す図である。図５に示す如く、被験者の手先の円弧運動に応じて、その円弧の円周方向に可操作楕円の長軸が配置されている。したがって、算出された可操作楕円の形状が良好であることが分かる。また、円弧の半径方向に可操作楕円の短軸が配置されており、その半径方向が動かし難い方向であることが分かる。本実施の形態に係るリハビリテーション装置１は、このような上肢の姿勢の場合、手先の円弧運動の円周方向に大きく半径方向に小さくグリップレバー部２を操作するトラッキング課題を設定する。
【００５９】
図６は、図５左端における腕姿勢（ａ）のときの仮想ガイドラインを示す図である。図７は、図５右端における腕姿勢（ｂ）のときの仮想ガイドラインを示す図である。
図６及び図７に示す如く、仮想ガイドラインに沿った制御目標値ｐｆｉｎが可操作楕円の大きさにより制限されて、腕の動作し易い方向（可操作楕円の長軸方向）には長く、腕の動作し難い方向（可操作楕円の短軸方向）には短く、グリップレバー部２の操作が誘導されていることが分かる。よって、被験者は無理の無いリハビリテーションを行うことができることが分かる。
【００６０】
次に、本実施の形態に係るリハビリテーション装置の制御方法について詳細に説明する。図８は、本実施の形態に係るリハビリテーション装置の制御処理フローを示すフローチャートである。なお、図８に示す制御処理は所定時間毎に繰り返し実行される。
【００６１】
カメラ３は、被験者の腕の各画像マーカＭを撮影し、撮影した撮影画像を制御装置９に出力する。円ハフ変換部９１は、カメラ３から出力された各画像マーカＭの撮影画像を円ハフ変換して各画像マーカの中心位置ｐｃｊを算出し、関節角度変換部９２に出力する（ステップＳ１０１）。
【００６２】
関節角度変換部９２は、円ハフ変換部９１から出力された各画像マーカの中心位置ｐｃｊを変換行列によりワールド座標系の中心位置ｐｊに変換し、上肢のヤコビアン行列を算出し、可操作楕円算出部９３に出力する（ステップＳ１０２）。
【００６３】
可操作楕円算出部９３は、関節角度変換部９２から出力されたヤコビアン行列を特異値分解し、可操作楕円の長軸の長さσｌｏｎｇ、短軸の長さσｓｈｏｒｔ、および方向（Ｕｈの第１列ベクトル）を算出し、可操作楕円の半径比率λｅｌｌｉｐ及び傾きθｅｌｌｉｐを算出する（ステップＳ１０３）。可操作楕円算出部９３は、算出した可操作楕円の半径比率λｅｌｌｉｐ及び傾きθｅｌｌｉｐを目標値算出部９４に出力する。
【００６４】
目標値算出部９４は、可操作楕円算出部９３から出力された可操作楕円の半径比率λｅｌｌｉｐ及び傾きθｅｌｌｉｐに基づいて、トラッキング課題の最終目標値ｐｆｉｎ＝[ｐｒｘ、ｐｒｙ]Ｔを算出し、表示装置１０に対して出力する（ステップＳ１０４）。
【００６５】
表示装置１０は、表示画面の２次元座標系において、目標値算出部９４から出力された最終目標値ｐｆｉｎを操作目標のターゲットマークとして、グリップレバー部２のハンドル２６の現在位置を示すターゲットマークと共に表示する（ステップＳ１０５）。
【００６６】
回転変換部９５は、第１回転センサ６から出力される第１関節軸２２の回転角θ１と第２回転センサ７から出力される第２関節軸２４の回転角θ２と、に基づいて、力センサ８から出力される力値信号ｆｅｘｔ０を、回転変換することでワールド座標系の力値信号ｆｅｘｔに変換し、アドミタンス制御部９６に出力する（ステップＳ１０６）。
【００６７】
アドミタンス制御部９６は、回転変換部９５から出力されたワールド座標系の力値信号ｆｅｘｔに基づいて、作業空間におけるグリップレバー部２のハンドル２６の位置である制御目標値ｐｒｅｆを算出し、逆キネマティクス部９７に出力する（ステップＳ１０７）。
【００６８】
逆キネマティクス部９７は、逆キネマティクス関数ＩＫ（）を用いて、アドミタンス制御部９６から出力される作業空間における制御目標値ｐｒｅｆを関節空間における制御目標値θｒｅｆに変換する（ステップＳ１０８）。
【００６９】
制御装置９は、上位制御系の逆キネマティクス部９７から出力された制御目標値θｒｅｆに、第１及び第２回転センサ６、７により検出されたグリップレバー部２の第１及び第２関節軸２２、２４の回転角θ１、θ２を追従させるように、第１及び第２サーボモータ４、５に対するトルク指令値τ１、τ２を夫々算出する（ステップＳ１０９）。制御装置９は、算出したトルク指令値τ１、τ２に応じた制御信号を生成し第１及び第２サーボモータ４、５に対して出力することで、第１及び第２サーボモータ４、５を夫々制御する。
【００７０】
以上、本実施の形態に係るリハビリテーション装置１において、被験者の手先の可操作性を示す可操作楕円を算出し、該算出した可操作楕円に基づいてグリップレバー部２のハンドル２６の操作目標位置を算出し、表示装置１０に表示させる。さらに、リハビリテーション装置１において、力センサ８により検出された外力と、算出した操作目標位置と、に基づいて、アドミタンス制御を行い、グリップレバー部２の第１及び第２関節軸２２、２４の操作量の目標値を算出し、該算出した操作量の目標値に第１及び第２回転センサ６、７により検出された第１及び第２関節軸２２、２４の回転角が追従するように第１及び第２サーボモータ４、５を制御する。
【００７１】
これにより、被験者の上肢の姿勢に基づいた可操作楕円を考慮して操作目標位置が設定される。このため、被験者に無理な負荷を強いることがない。さらに、被験者がグリップレバー部２のハンドル２６を操作目標位置の方向に操作し易いようにガイドするアドミタンス制御を行う。これにより、被験者に掛かる負荷を適度に軽減できる。すなわち、被験者の姿勢に応じた最適なリハビリテーション訓練を行うことができる。
【００７２】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【００７３】
上記一実施の形態において、制御装置９は、カメラ３から出力される被験者の上肢の撮影画像に基づいて、上記可操作楕円を算出しているが、これに限らず、任意のセンサを用いて上記可操作楕円を算出することができる。制御装置９は、例えば、被験者の上肢に装着された慣性センサから出力される信号に基づいて、上記可操作楕円を算出してもよい。制御装置９は、例えば、慣性センサから出力される信号と、予め設定された関節角度の運動モデルと、に基づいて上肢の関節角度を算出し、該算出した関節角度と、上肢のヤコビアン行列とに基づいて上記可操作楕円を算出する。
【００７４】
また、制御装置９は、例えば、デプスセンサ（キネクト（登録商標））から出力される信号に基づいて、上記可操作楕円を算出してもよい。制御装置９は、例えば、デプスセンサから出力される深度情報と、予め設定された関節角度の運動モデルと、に基づいて上肢の関節角度を算出し、該算出した関節角度と、上肢のヤコビアン行列とに基づいて上記可操作楕円を算出する。
【００７５】
制御装置９は、上位制御系において、力センサ８により検出されたグリップレバー部２のハンドルに作用する外力に基づいて、２自由度アドミタンス制御を行って仮想ガイドラインを算出しているが、これに限らない。制御装置９は、上位制御系において、力センサ８により検出されたグリップレバー部２のハンドルに作用する外力に基づいて、２自由度インピーダンス制御を行って仮想ガイドラインを算出してもよい。
【００７６】
上記実施の形態において、グリップレバー部２は、例えば、３つの関節軸を有し、ハンドル２６が３次元空間上の任意の位置に移動する構成であってもよい。この場合、可操作楕円算出部９３は、上肢のヤコビアン行列を特異値分解し、被験者の手先の可操作性を示す可操作楕円体の長軸の大きさ、短軸の大きさ、および方向を算出し、可操作楕円体の半径比率λｅｌｌｉｐ及び傾きθｅｌｌｉｐを算出する。目標値算出部９４は、可操作楕円算出部９３により算出された可操作楕円体の半径比率λｅｌｌｉｐ及び傾きθｅｌｌｉｐに基づいて、トラッキング課題の最終目標値ｐｆｉｎを算出する。一方、アドミタンス制御部９６は、回転変換Ｒ（θｒｏｔ＿ｋ）後、３行３列のアドミタンス行列による変換（アドミタンス制御）を行い、逆回転変換ＲＴ（θｒｏｔ＿ｋ）を行うことで、制御目標値ｐｒｅｆを算出する。
【００７７】
上記実施の形態において、制御装置９のアドミタンス制御部９６は、グリップレバー部２のハンドル２６の現在位置が仮想ガイドラインから副軸方向に外れるに従がって、ダンピング定数Ｄａｄｍ２及び／又はスティッフネス定数Ｋａｄｍ２を徐々高く設定してもよい。これにより、グリップレバー部２のハンドル２６を仮想ガイドライン上にゆるやかに導くことができる。
【００７８】
上記実施の形態において、制御装置９のアドミタンス制御部９６は、グリップレバー部２のハンドル２６の現在位置が可操作楕円の外側に出たと判断すると、ダンピング定数Ｄａｄｍ１、Ｄａｄｍ２及び／又はスティッフネス定数Ｋａｄｍ１、Ｋａｄｍ２を所定量だけ急激に増加させてもよい。これにより、グリップレバー部２のハンドル２６の位置が可操作楕円の外側に外れるとハンドル２６の動作が急激に固くなる。したがって、ハンドル２６が可操作楕円の外側に外れるのを抑制でき、被験者に無理な姿勢を強制的に抑制できる。
【００７９】
上記実施の形態において、リハビリテーション装置１は、被験者の上肢のリハビリテーション訓練を行っているが、これに限らず、例えば、被験者の下肢のリハビリテーション訓練を行ってもよい。制御装置９は、上位制御系において、カメラ３により撮影された被験者の下肢の姿勢の画像に基づいて、可操作楕円を算出する。制御装置９は、上位制御系において、力センサ８により検出された下肢の外力に基づいてアドミタンス制御を行い、操作目標位置の仮想ガイドラインを算出する。
【００８０】
本発明は、例えば、図８に示す処理を、ＣＰＵ９ａにコンピュータプログラムを実行させることにより実現することも可能である。
【００８１】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００８２】
プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００８３】
１  リハビリテーション装置、２  グリップレバー部、３  カメラ、４、５  第１及び第２サーボモータ、６、７  第１及び第２回転センサ、８  力センサ、９  制御装置、１０  表示装置、９１  円ハフ変換部、９２  関節角度変換部、９３  可操作楕円算出部、９４  目標値算出部、９５  回転変換部、９６  アドミタンス制御部、９７  逆キネマティクス部
【図１】
2015221073.tif 000015
【図２】
2015221073.tif 000016
【図３】
2015221073.tif 000017
【図４】
2015221073.tif 000018
【図５】
2015221073.tif 000019
【図６】
2015221073.tif 000020
【図７】
2015221073.tif 000021
【図８】
2015221073.tif 000022