図面 (/)

技術 制御装置、制御方法及び制御プログラム

出願人 株式会社国際電気通信基礎技術研究所
発明者 荒川佳樹
出願日 2005年11月9日 (15年0ヶ月経過) 出願番号 2005-325278
公開日 2007年5月31日 (13年5ヶ月経過) 公開番号 2007-130714
状態 特許登録済
技術分野 マニプレータ・ロボット マニプレータ プログラマブルコントロ-ラ
主要キーワード 制御時間間隔 分数形式 暴走対策 循環小数 Y座標 位置方向データ 数値精度 ジョルダン
関連する未来課題
重要な関連分野

この項目の情報は公開日時点(2007年5月31日)のものです。
また、この項目は機械的に抽出しているため、正しく解析できていない場合があります

図面 (10)

課題

制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる制御装置を提供する。

解決手段

データ変換処理部13は、ロボットアーム21の先端部の目標データPから関節角度θを求めるデータ演算処理に使用される係数aij及び係数biを固定長数値データ形式から可変長分数データ形式へ変換し、無誤差制御処理部16は、可変長分数データ形式の係数aij及び係数biを用いてデータ演算処理を無誤差で実行して関節角度θを求め、この処理において除算を可変長分数データ形式で表現する。

概要

背景

従来のロボットアーム制御方式としては、スレーブアームと同じ軸構成等を有するマスターアーム操作者が操作してスレーブアームを制御するマスターアーム方式、操作者の腕と手の主要部(各関節)にセンサを装着して操作者の腕と手の関節の動きを検出してロボットアームを制御するモーションキャプチャー方式、操作者の手首・手の等に付けた3次元位置方向センサにより操作者の手首・手の甲の3次元位置及び方向を検出し、このデータを用いてアームリンク)の各関節角度を求める逆変換を行ってロボットアームを制御する逆変換方式等の種々の方式がある。

上記のマスターアーム方式では、操作者側には、制御されるスレーブアームだけでなく、マスターアームが必要となり、システム重装備になる。また、モーションキャプチャー方式では、操作者の腕と手の複数箇所にセンサを装着する必要があり、操作者にとって煩わしいものとなる。

一方、逆変換方式では、操作者は手の甲等に1個のセンサのみを装着するだけで済み、操作者にとって煩わしさがないが、操作者の手首の位置と方向データから各関節角度を求める逆変換が必要となる。この逆変換では、人の腕のような高い自由度(6自由度以上)を持つ軸構成における各関節角度を求める必要があるため、逆変換が非線形連立方程式となり、複雑な計算となる。

このため、本願発明者は、逆変換が非線形連立方程式とならず、単純且つ少ない演算量で厳密解を求めることができるロボットアーム・ハンド操作方法を提案した(特許文献1参照)。この操作方法では、ロボットアームの操作者の手首に付けたセンサにより検出された3次元位置及び方向からアームの各関節角度を求めるために、浮動小数点演算を用いた誤差有り演算を用いている。この浮動小数点フォーマットを用いた実数表現は、非常に小さな数から大きな数まで表現でき、計算効率がよいという優れた特徴があるため、逆変換のような実数の数値計算に適している。
特開2005−46931号公報

概要

制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる制御装置を提供する。データ変換処理部13は、ロボットアーム21の先端部の目標データPから関節角度θを求めるデータ演算処理に使用される係数aij及び係数biを固定長数値データ形式から可変長分数データ形式へ変換し、無誤差制御処理部16は、可変長分数データ形式の係数aij及び係数biを用いてデータ演算処理を無誤差で実行して関節角度θを求め、この処理において除算を可変長分数データ形式で表現する。

目的

本発明の目的は、制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる制御装置、制御方法及び制御プログラムを提供することである。

効果

実績

技術文献被引用数
1件
牽制数
0件

この技術が所属する分野

(分野番号表示ON)※整理標準化データをもとに当社作成

ライセンス契約や譲渡などの可能性がある特許掲載中! 開放特許随時追加・更新中 詳しくはこちら

請求項1

制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する変換手段と、前記変換手段により変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する無誤差演算手段とを備え、前記無誤差演算手段は、前記データ演算処理における除算を可変長分数データ形式で表現することを特徴とする制御装置

請求項2

前記無誤差演算手段は、前記データ演算処理において除算の結果に余りが発生しない場合は当該除算を実行することを特徴とする請求項1記載の制御装置。

請求項3

前記無誤差演算手段は、前記可変長分数データ形式で表現された分母データ及び分子データを約分することを特徴とする請求項1又は2記載の制御装置。

請求項4

前記変換手段は、前記データ演算処理に使用されるデータを固定長数値データ形式から可変長分数データ形式へ変換し、前記無誤差演算手段は、前記変換手段により変換された可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行することを特徴とする請求項1〜3のいずれかに記載の制御装置。

請求項5

前記制御対象は、関節を介して複数のリンクを連結したリンク機構であり、前記変換手段は、前記リンク機構の所定部位目標次元位置データ及び目標方向データからなる目標データをP、前記リンク機構の各関節角度をθとし、目標データPから関節角度θを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換することを特徴とする請求項1〜4のいずれかに記載の制御装置。

請求項6

前記無誤差演算手段は、θi=θ’i+Δθi(i=1,…,n)(ここで、θiは関節iの求める関節角度、θ’iは微小時間Δt前の関節iの関節角度、Δθiは微小時間Δt経過後の関節iの微小関節角度変化量、nは求める関節角度θの数)とし、n元連立一次方程式AΔθ=B(ここで、Aはその要素aij(i=1,…,n、j=1,…,n)がリンク機構のリンク長及び微小時間Δt前の関節iの関節角度θ’iにより決定される係数行列、Bはその要素bi(i=1,…,n)が目標データP及び微小時間Δt前の位置方向データP’により決定されるベクトル)を、可変長分数データ形式のデータを用いて、無誤差演算により解くことを特徴とする請求項5記載の制御装置。

請求項7

前記変換手段は、前記無誤差演算手段により求められた可変長分数データ形式の関節角度θを固定長数値データ形式へ変換することを特徴とする請求項5又は6記載の制御装置。

請求項8

操作者の所定部位の3次元位置及び方向を検出する検出手段をさらに備え、前記検出手段により検出された3次元位置及び方向を前記リンク機構の所定部位の目標3次元位置データ及び目標方向データとして用いて、前記リンク機構の所定部位を目標3次元位置及び目標方向に制御することを特徴とする請求項5〜7のいずれかに記載の制御装置。

請求項9

制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する第1のステップと、変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する第2のステップとを含み、前記第2のステップは、前記データ演算処理における除算を可変長分数データ形式で表現するステップを含むことを特徴とする制御方法

請求項10

制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する変換手段と、前記変換手段により変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する無誤差演算手段としてコンピュータを機能させ、前記無誤差演算手段は、前記データ演算処理における除算を可変長分数データ形式で表現することを特徴とする制御プログラム

技術分野

0001

本発明は、制御対象を制御する制御装置制御方法及び制御プログラムに関し、特に、関節を介して複数のリンクを連結したリンク機構所定部位、例えば、先端部を目標次元位置及び目標方向に制御する制御装置、制御方法及び制御プログラムに関する。

背景技術

0002

従来のロボットアーム制御方式としては、スレーブアームと同じ軸構成等を有するマスターアーム操作者が操作してスレーブアームを制御するマスターアーム方式、操作者の腕と手の主要部(各関節)にセンサを装着して操作者の腕と手の関節の動きを検出してロボットアームを制御するモーションキャプチャー方式、操作者の手首・手の等に付けた3次元位置方向センサにより操作者の手首・手の甲の3次元位置及び方向を検出し、このデータを用いてアーム(リンク)の各関節角度を求める逆変換を行ってロボットアームを制御する逆変換方式等の種々の方式がある。

0003

上記のマスターアーム方式では、操作者側には、制御されるスレーブアームだけでなく、マスターアームが必要となり、システム重装備になる。また、モーションキャプチャー方式では、操作者の腕と手の複数箇所にセンサを装着する必要があり、操作者にとって煩わしいものとなる。

0004

一方、逆変換方式では、操作者は手の甲等に1個のセンサのみを装着するだけで済み、操作者にとって煩わしさがないが、操作者の手首の位置と方向データから各関節角度を求める逆変換が必要となる。この逆変換では、人の腕のような高い自由度(6自由度以上)を持つ軸構成における各関節角度を求める必要があるため、逆変換が非線形連立方程式となり、複雑な計算となる。

0005

このため、本願発明者は、逆変換が非線形連立方程式とならず、単純且つ少ない演算量で厳密解を求めることができるロボットアーム・ハンド操作方法を提案した(特許文献1参照)。この操作方法では、ロボットアームの操作者の手首に付けたセンサにより検出された3次元位置及び方向からアームの各関節角度を求めるために、浮動小数点演算を用いた誤差有り演算を用いている。この浮動小数点フォーマットを用いた実数表現は、非常に小さな数から大きな数まで表現でき、計算効率がよいという優れた特徴があるため、逆変換のような実数の数値計算に適している。
特開2005−46931号公報

発明が解決しようとする課題

0006

しかしながら、上記の浮動小数点演算を用いた数値制御では、以下のような問題がある。
(1)浮動小数点演算では、数値演算誤差が発生し、且つその蓄積が起こるため、演算結果に大きな誤差が含まれる場合があり、ロボットアームの先端位置が大幅にずれる場合がある。
(2)上記の数値演算誤差とその蓄積のために、当初想定していなかった状態が発生し、結果として制御が破綻してロボットアーム動作が暴走する状況が発生する。
(3)上記のロボットアーム動作の暴走を防止するためには、暴走対策を施す必要があり、制御が複雑化する。

0007

また、数値演算処理は、加算、減算、乗算及び除算四則演算を用いて行われるが、除算は、割り切れない場合に必ず誤差が発生し、除算を含む四則演算すべてを無誤差で実行することは困難である。この場合、除算を使用しないことも考えられるが、通常、数値制御処理には加減乗除算の四則演算が用いられるため、除算が使えないということは、適用できる範囲が狭くなり、実用性が低下する。

0008

本発明の目的は、制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる制御装置、制御方法及び制御プログラムを提供することである。

課題を解決するための手段

0009

本発明に係る制御装置は、制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する変換手段と、前記変換手段により変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する無誤差演算手段とを備え、前記無誤差演算手段は、前記データ演算処理における除算を可変長分数データ形式で表現するものである。

0010

本発明に係る制御装置では、制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータが固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換され、変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理が無誤差で実行される。ここで、データ演算処理において除算が発生した場合、当該除算が保留されて可変長分数データ形式で表現されるので、可変長分数データ形式で表現された数値に対するその後の演算を継続することができ、データ演算処理を最後まで実行することができる。また、誤差が発生する除算を実際には実行していないので、数値演算誤差が発生することがなく、その蓄積も起こらない。この結果、除算を含む四則演算すべてを無誤差演算の枠組みで処理することができるので、制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる。

0011

前記無誤差演算手段は、前記データ演算処理において除算の結果に余りが発生しない場合は当該除算を実行することが好ましい。

0012

この場合、データ演算処理において除算の結果に余りが発生しないときは当該除算が実行されるので、数値演算誤差の発生及びその蓄積を防止しながら、誤差が発生しない除算を実行して除算の結果を表すデータ量を削減することができ、データ演算処理におけるデータ量の削減及び計算効率の向上を図ることができる。

0013

前記無誤差演算手段は、前記可変長分数データ形式で表現された分母データ及び分子データを約分することが好ましい。

0014

この場合、可変長分数データ形式で表現された分母データ及び分子データが約分されるので、数値データの桁数及びデータ量を削減することができ、データ演算処理におけるデータ量の削減及び計算効率の向上を図ることができる。

0015

前記変換手段は、前記データ演算処理に使用されるデータを固定長数値データ形式から可変長分数データ形式へ変換し、前記無誤差演算手段は、前記変換手段により変換された可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行することが好ましい。

0016

この場合、可変長分数データ形式のデータを用いてデータ演算処理が無誤差で実行されるので、使用するデータ形式が固定長数値データ形式と可変長分数データ形式との2種類のみに限定され、データの管理が容易となり、また、無誤差演算手段では可変長分数データ形式のデータのみを用いることとなり、無誤差演算手段の構成も簡略化することができる。

0017

前記制御対象は、関節を介して複数のリンクを連結したリンク機構であり、前記変換手段は、前記リンク機構の所定部位の目標3次元位置データ及び目標方向データからなる目標データをP、前記リンク機構の各関節角度をθとし、目標データPから関節角度θを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換することが好ましい。

0018

この場合、目標データPから関節角度θを求めるデータ演算処理において、除算を含む四則演算すべてを無誤差演算の枠組みで処理することができるので、リンク機構の制御動作を安定化及び高信頼化することができるとともに、リンク機構を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる。

0019

前記無誤差演算手段は、θi=θ’i+Δθi(i=1,…,n)(ここで、θiは関節iの求める関節角度、θ’iは微小時間Δt前の関節iの関節角度、Δθiは微小時間Δt経過後の関節iの微小関節角度変化量、nは求める関節角度θの数)とし、n元連立一次方程式AΔθ=B(ここで、Aはその要素aij(i=1,…,n、j=1,…,n)がリンク機構のリンク長及び微小時間Δt前の関節iの関節角度θ’iにより決定される係数行列、Bはその要素bi(i=1,…,n)が目標データP及び微小時間Δt前の位置方向データP’により決定されるベクトル)を、可変長分数データ形式のデータを用いて、無誤差演算により解くことが好ましい。

0020

この場合、目標データPから関節角度θを求めるデータ演算処理を、可変長分数データ形式の係数データを用いて無誤差で解くことができるので、データ演算処理が除算を含む場合でも、データ演算処理における数値演算誤差の発生及びその蓄積を完全に排除することができる。

0021

前記変換手段は、前記無誤差演算手段により求められた可変長分数データ形式の関節角度θを固定長数値データ形式へ変換することが好ましい。

0022

この場合、データ演算処理により求められた関節角度θが可変長分数データ形式から固定長数値データ形式へ変換されるので、固定長数値データ形式の関節角度θを用いて通常の制御を行うことができ、種々の制御対象を容易に制御することができる。

0023

上記制御装置は、操作者の所定部位の3次元位置及び方向を検出する検出手段をさらに備え、前記検出手段により検出された3次元位置及び方向を前記リンク機構の所定部位の目標3次元位置データ及び目標方向データとして用いて、前記リンク機構の所定部位を目標3次元位置及び目標方向に制御することが好ましい。

0024

この場合、リンク機構の所定部位を操作者の所定部位の動きに応じた目標3次元位置及び目標方向に高精度に制御することができる。

0025

本発明に係る制御方法は、制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する第1のステップと、変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する第2のステップとを含み、前記第2のステップは、前記データ演算処理における除算を可変長分数データ形式で表現するステップを含むものである。

0026

本発明に係る制御プログラムは、制御対象を制御するために用いられる制御データを求めるデータ演算処理に使用されるデータを固定長数値データ形式から可変長整数データ形式、又は可変長整数データ形式の分母データ及び分子データから構成される可変長分数データ形式へ変換する変換手段と、前記変換手段により変換された可変長整数データ形式又は可変長分数データ形式のデータを用いて、前記データ演算処理を無誤差で実行する無誤差演算手段としてコンピュータを機能させ、前記無誤差演算手段は、前記データ演算処理における除算を可変長分数データ形式で表現するものである。

発明の効果

0027

本発明によれば、制御対象を制御するために用いられる制御データを求めるデータ演算処理において、除算が発生した場合、当該除算が保留されて可変長分数データ形式で表現されるので、データ演算処理を最後まで実行することができるとともに、数値演算誤差が発生することがなく、その蓄積も起こらないため、除算を含む四則演算すべてを無誤差演算の枠組みで処理することができ、制御動作を安定化及び高信頼化することができるとともに、制御対象を高精度に制御することができ、さらに、汎用性の高い且つ適用範囲の広い制御演算を実現することができる。

発明を実施するための最良の形態

0028

以下、本発明の一実施の形態による制御装置の一例であるロボットアーム制御装置を用いたロボットアーム制御システムについて図面を参照しながら説明する。

0029

図1は、本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。図1に示すロボットアーム制御システムは、ロボットアーム制御装置1、ロボットアーム部2、3次元操作グローブ3、3次元位置方向センサ4及び3次元ディスプレイ5を備える。

0030

ロボットアーム部2は、一対のロボットアーム21及びハンド22、胴体部23並びにカメラ部24を備え、人の上半身(頭部を除く)と同程度の大きさと自由度とを有し、両腕と両手の機能を実現している。

0031

ロボットアーム21は、肩、及び手首に関節を有し、これらの関節を介してリンクとなる上腕21a及び前腕21bが連結されたリンク機構(マニプレータ)であり、人の腕と同程度の大きさ及び自由度を有する。ハンド22は、人の手と同程度の大きさと自由度を有する触覚付きの5本指ロボットハンドであり、前腕21bの先端に取り付けられる。ハンド22には触覚センサ(図示省略)が設けられており、物をつかんだ時の触覚(圧力)がロボットアーム制御装置1を介して3次元操作グローブ3へ伝達される。

0032

なお、図示を省略しているが、ロボットアーム21及びハンド22は、その内部にモータ等の駆動源を有し、この駆動源により任意の角度に駆動される。また、本実施の形態では、ロボットアーム21を人の腕に模倣させ、ハンド22を人の手指に模倣させているが、ロボットアーム及びハンドの構成は、この例に特に限定されず、ハンド22の指の数を増減させる等の種々の変更が可能である。また、リンク機構の関節数及びリンク数も、上記の例に特に限定されず、種々の変更が可能である。

0033

カメラ部24は、3次元カメラステレオカメラ)等から構成され、ロボットアーム制御装置1を介して、撮影した画像を3次元ディスプレイ5へ出力する。3次元ディスプレイ5は、カメラ部24により撮影された画像を表示し、操作者Mに提示する。なお、撮影された画像の提示方法は、この例に特に限定されず、3次元ディスプレイに替えて、操作者Mに装着させたヘッドマウントディスプレイに表示させる等の種々の変更が可能である。

0034

3次元操作グローブ3は、公知の3次元操作用データグローブから構成され、操作者Mの両手にそれぞれ装着される。3次元操作グローブ3は、操作者Mの手の甲及び指の動き(関節角度)を検出し、検出した関節角度データをロボットアーム制御装置1へ出力する。3次元操作グローブ3の手首の位置には、3次元位置方向センサ4が取り付けられ、3次元位置方向センサ4は、前腕21bの先端部の目標3次元位置及び目標方向として、操作者Mの手首の3次元位置及び方向を検出し、検出した3次元位置データ及び方向データをロボットアーム制御装置1へ出力する。

0035

なお、目標3次元位置及び目標方向を検出又は入力する方法は、上記の例に特に限定されず、データグローブ又はデータスーツ等から操作者の所定部位の3次元位置及び方向を検出したり、ハプティクスデバイス等を用いて入力したりしてもよい。

0036

ロボットアーム制御装置1は、操作者Mの両手に装着された3次元操作グローブ3及び3次元位置方向センサ4からの検出結果を基に、ロボットアーム21及びハンド22の各関節角度を算出する。ハンド22に関しては、3次元操作グローブ3が手の甲の曲がり(手首の)角度及び指の関節角度を検出し、ロボットアーム制御装置1は、これらの角度データに基づいてハンド22を制御する。

0037

一方、ロボットアーム21に関しては、操作者Mの手首に装着した3次元位置方向センサ4から操作者Mの手首の三次元位置及びその方向、すなわち、ロボットアーム21の先端部の目標3次元位置及び目標方向が得られるだけである。このため、ロボットアーム21の先端部の目標3次元位置データ及び目標方向データから、ロボットアーム21の各関節角度を計算する必要がある。このように、ロボットアーム21の先端部の3次元位置及び方向からロボットアーム21の各関節角度を計算することを逆変換と呼び、これとは逆に、ロボットアーム21の各関節角度からロボットアーム21の先端部の3次元位置及び方向を計算することを順変換と呼ぶことにする。

0038

図2は、図1に示すロボットアーム制御装置1の構成を示すブロック図である。図2に示すように、ロボットアーム制御装置1は、外部インターフェース部11、固定長数値データ記憶部12、データ変換処理部13、可変長分数データ記憶部14、誤差有制御処理部15及び無誤差制御処理部16を備える。誤差有制御処理部15は、固定長数値データ演算処理部15a及び浮動小数点演算部15bを備え、無誤差制御処理部16は、可変長分数データ演算処理部16a及び可変長分数演算部16bを備える。

0039

ロボットアーム制御装置1は、CPU(中央演算処理装置)、ROM(リードオンリメモリ)、RAM(ランダムアクセスメモリ)、外部記憶装置記録媒体駆動装置入力装置表示装置外部機器インターフェース等を備えるコンピュータから構成することができる。この場合、後述するロボットアーム制御処理(位置制御処理)を実行するためのロボットアーム制御プログラム(位置制御プログラム)を、CPU等を用いて実行することにより、外部インターフェース部11、固定長数値データ記憶部12、データ変換処理部13、可変長分数データ記憶部14、誤差有制御処理部15及び無誤差制御処理部16としてコンピュータを機能させることができる。なお、ロボットアーム制御装置1の構成は、この例に特に限定されず、上記の各機能の一部又は全部を専用のハードウエアから構成する等の種々の変更が可能である。

0040

外部インターフェース部11は、3次元位置方向センサ4から3次元位置データ及び方向データを取得して固定長数値データ記憶部12に格納する。また、外部インターフェース部11は、ロボットアーム制御処理により求められた関節角度を固定長数値データ記憶部12から読み出してロボットアーム部2へ出力し、ロボットアーム21の各関節角度がロボットアーム制御処理により求められた関節角度に制御される。

0041

固定長数値データ記憶部12は、誤差有制御処理部15において処理される種々のデータを固定長数値データ形式で記憶し、可変長分数データ記憶部14は、無誤差制御処理部16において処理される種々のデータを可変長分数データ形式で記憶する。

0042

データ変換処理部13は、固定長数値データ記憶部12から浮動小数点データを読み出し、読み出した浮動小数点データを可変長分数データへ変換し、変換した可変長分数データを可変長分数データ記憶部14に格納する。また、データ変換処理部13は、可変長分数データ記憶部14から可変長分数データを読み出し、読み出した可変長分数データを浮動小数点データへ変換し、変換した浮動小数点データを固定長数値データ記憶部12に格納する。

0043

なお、本発明に用いられる固定長数値データは、上記の浮動小数点データに特に限定されず、固定長整数データを用いてもよく、この場合、浮動小数点演算部15bを固定長整数演算部に変更して固定長整数演算部により固定長整数演算を実行するようにしてもよい。例えば、3次元位置方向センサ4が出力する値(角度)が、−255〜+255等の整数値であり、ロボットアーム21の関節を動作させるモータに対する出力(回転角度)も、−1000〜+1000パルス等の整数値である場合に固定長整数データを用いることができる。

0044

誤差有制御処理部15は、ロボットアーム部2のロボットアーム21及びハンド22の制御処理のうち、固定長数値演算の一例である浮動小数点演算を用いた誤差有制御処理を実行する。固定長数値データ演算処理部15aは、固定長数値データ記憶部12から浮動小数点データを読み出し、浮動小数点演算部15bを用いて座標変換処理等(sin、cos演算)を浮動小数点演算で実行し、演算結果を固定長数値データ記憶部12に格納する。浮動小数点演算部15bは、加算器減算器乗算器及び除算器としての機能を有し、浮動小数点演算を用いて、加算、減算、乗算及び除算を実行する。

0045

無誤差制御処理部16は、ロボットアーム部2のロボットアーム21及びハンド22の制御処理のうち、可変長分数演算を用いた無誤差制御処理を実行する。可変長分数データ演算処理部16aは、可変長分数データ記憶部14から可変長分数データを読み出し、制御方程式求解等を可変長分数演算部16bを用いて可変長分数演算で実行し、演算結果を可変長分数データ記憶部14に格納する。

0046

可変長分数演算部16bは、加算器、減算器及び乗算器としての機能を有し、可変長分数演算を用いて、加算、減算及び乗算を無誤差で実行する。また、可変長分数演算部16bは、除算器としての機能をも有し、割り切れる場合は除算を行い、その結果を分母を1とする可変長分数データ形式で保持し、割り切れない場合は除算を保留し、そのまま可変長分数データ形式で保持し、割り切れないが、約分できる場合は、約分を実行し、その結果を可変長分数データ形式で保持し、以降の演算を進める。

0047

図3は、数値データの種々のフォーマットの例を示す図である。数値データは、固定長数値データと可変長数値データとに大別される。前者(固定長数値データ)の代表例には、計算機においてよく用いられる、図3の(a)に示す固定長整数データ(例えば、32ビット)と、図3の(b)に示す浮動小数点データ(例えば、64ビット)とが該当する。図3の(a)に示すように、固定長整数データは、符号部及び数値(整数)部から構成される。図3の(b)に示すように、浮動小数点データは、符号部、指数部及び仮数部から構成され、このようなデータ形式で固定長数値データ記憶部12に格納される。これらの固定長数値データは、固定長であるため、計算効率がよい反面、数値表現精度が限定され、演算誤差の発生とその蓄積とが起こる。

0048

一方、可変長数値データには、図3の(c)に示す可変長整数データと、図3の(d)に示す可変長分数データとが該当する。図3の(c)ように、可変長整数データは、語長部、符号部及び数値(整数)部から構成される。図3の(d)ように、可変長分数データは、語長部、符号部及び数値(整数)部から構成される分子データと、語長部、符号部及び数値(整数)部から構成される分母データとが一対にされたデータであり、このようなデータ形式で可変長分数データ記憶部14に格納される。これらの可変長数値データでは、数値表現精度に限界がないため、加減乗算を無誤差で演算することができ、演算誤差の発生とその蓄積とが起こらないという利点がある。

0049

しかしながら、可変長数値データでは、データ長が固定でないため、計算効率が劣る。また、除算の場合は、割り切れずに循環小数となる場合がある。この場合、無限桁数の数値となるので、可変長数値データ形式でも対応することができず、無誤差演算を行うことはできない。

0050

そこで、本実施の形態では、可変長分数データ記憶部14及び無誤差制御処理部16を用いて、可変長整数データを分子と分母とに用いた分数形式で表現して記憶及び演算を行っている。すなわち、被除数a/b(ここで、aは可変長整数データ形式の分子データ、bは可変長整数データ形式の分母データ)を除数c/d(ここで、cは可変長整数データ形式の分子データ、dは可変長整数データ形式の分母データ)で割る除算を実行する場合、除数c/dの逆数d/cを乗算してa×d(=e)及びb×c(=f)の分子部及び分母部の乗算を実行する。次に、乗算後の被除数eを除数fで割る除算において、割り切れる場合は除算を行い、その結果を、分母を1とする可変長分数データ形式で表現して記憶及び演算を行い、一方、割り切れない場合は除算を保留し、そのまま可変長分数データ形式で記憶し、以降の演算を進める。また、除算において、割り切れないが、約分できる場合は、約分を実行し、約分後の可変長分数データを記憶してその後の演算に用いている。

0051

上記の方法の利点としては、以下が挙げられる。
(1)加減乗算に加えて、除算も無誤差演算の枠組みで行うことができるため、非常に汎用性の高い且つ適用性の広い無誤差演算を実現できる。
(2)除算を分数形式データで表現するので、割り切れる場合は除算を無誤差で実行することができ、一方、割り切れない場合は、除算を分数形式データとして保留したまま無誤差演算を進めることができる。
(3)除算を分数形式データで表現するので、割り切れない場合でも、約分することができ、数値桁数を削減できる場合が多いため、データ量の削減及び計算効率の向上に大きく寄与することができる。

0052

なお、除算において割り切れる場合、上記の可変長分数データ形式で表現することなく、除算結果を可変長整数データ形式で表現するようにしてもよく、可変長整数データ記憶部、可変長整数データ演算処理部及び可変長整数演算部を付加して、可変長整数データ演算処理部及び可変長整数演算部を用いて可変長整数演算を実行するようにしてもよい。この場合、分母データを可変長整数データ記憶部に記憶する必要がなくなり、データ量をより削減することができる。

0053

次に、本実施の形態の制御対象となるロボットアーム21について詳細に説明する。図4は、図1に示すロボットアーム部2における右側のロボットアーム21の構成を示す模式図であり、図5は、図4に示す右側のロボットアーム21の座標系を説明するための図である。なお、左側のロボットアーム21も、図4及び図5に示す右側のロボットアーム21と同様に構成されているので、詳細な説明は省略する。

0054

図4に示すように、グローバル座標系原点Oを上腕21aの右肩(第1関節)に、グローバル座標系のX座標軸を胴体部23(図1参照)の正面方向に沿って床面と平行方向に、グローバル座標系のY座標軸を右肩から左肩方向に、グローバル座標系のZ座標軸を床面から天井への垂直方向にそれぞれ設定する。また、ロボットアーム21の上腕21aの長さをe1、前腕21bの長さをe2とし、上腕21aの肩関節のうち第1関節(Y軸回り)の関節角度θ1、第2関節(X軸回り)の関節角度θ2、第3関節(Z軸回り)の関節角度θ3とし、上腕21aと前腕21bとの間の第4関節(Y軸回り)の関節角度θ4、第5関節(Z軸回り)の関節角度θ5とし、前腕21bとハンド22との間の第6関節(Y軸回り)の関節角度θ6とする。

0055

このとき、3次元位置方向センサ4が装着されている前腕の手首のローカル座標系からグローバル座標への変換行列をHsとすると、以下の式(1)のように表現することができる。

0056

0057

ここで、図5に示すように、手首に装着された3次元位置方向センサ4の3次元位置はPs(xs,ys,zs)、X軸方向の単位ベクトルはdxs(uxs,vxs,wxs)、Y軸方向の単位ベクトルはdys(uys,vys,wys)、Z軸方向の単位ベクトルはdzs(uzs,vzs,wzs)となる。これらの値はすべて、3次元位置方向センサ4から取得されるので、既知となる。一方、Hsの各要素は、各アーム(上腕21a及び前腕21b)の軸の長さe1,e2と各関節角度θ1,θ2,θ3,θ4,θ5を用いて、以下のように表現することができる。

0058

0059

ここで、Cn=cos(θn)、Sn=sin(θn)(n=1,…,5)であり、関節角度θnは、アーム動作により刻々と変化するので、時間tの関数となり、θn’を微小時間Δt前の関節角度値直前の関節角度)とし、微小時間Δt経過後における微小関節角度変化量をΔθnとすると、以下のように表現することができる。
θn=θn’+Δθn (6)
ここで、微小時間Δt前の関節角度値θn’は既知であり、微小時間Δtは制御時間間隔である。

0060

上記の前提を基に、以下のように近似することができる。なお、下記式では、(Δθ)2は微小であるので、(Δθ)2以上の高次項は無視できるとした。
sin(θ)=sin(θ’+Δθ)≒sin(θ’)+(sin(θ’))’Δθ=sin(θ’)+cos(θ’)Δθ (7a)
cos(θ)=cos(θ’+Δθ)≒cos(θ’)+(cos(θ’))’Δθ=cos(θ’)−sin(θ’)Δθ (7b)
上記の式(7a)、(7b)を式(2)〜(5)へ代入すると、以下となる。なお、ここでも、Δθ1Δθ2等の高次のΔθは微小であり、無視できるとした。

0061

0062

0063

0064

0065

上記各式より、式(1)は、以下のように表現することができる。

0066

0067

そこで、Δθ1,Δθ2,Δθ3,Δθ4,Δθ5は、未知数が5個あるので、式(13)の中から適切な5つの式を選択し、例えば、以下の5元連立一次方程式の解として求めることができる。

0068

0069

また、上式左辺行列の各要素を係数aij(i=1,…,5、j=1,…,5)、右辺のベクトルの各要素を係数bi(i=1,…,5)で表すと、以下のように表現することができる。

0070

0071

上記の5元連立一次方程式の解法では、加減乗除算が繰り返されるために、浮動小数点演算(固定長数値演算)を用いた数値計算では、数値演算誤差とその蓄積のために解の誤差が大きくなる場合が発生する。また、ゼロで除算が行われる場合(以下、この場合を「ゼロ割」という)も発生する。結果として、これらの演算誤差、その蓄積及びゼロ割のために、ロボットアーム動作が不安定になり、暴走及び破綻が発生する。

0072

このような数値演算の誤差が大きくなること、ロボットアームの動作が暴走及び破綻することは、ロボットアーム制御処理においては許されない。ロボットアームの動作精度が低下したり、又は動作が暴走及び破綻すると、人に危害を加えたり、周辺の物を壊してしまうこととなる。また、除算があると、除算は数値演算に計算精度の低下等様々な悪影響を及ぼすことはもちろんのこと、除算では割り切れない場合が発生し(無限桁数が必要な循環小数の発生)、無誤差演算を行うことができない。

0073

このため、本願発明者は、特願2005−164567号明細書において、上記連立方程式の解法処理に関して、可変長数値(整数)データを用い且つ除算を一切使用せず、誤差を発生させない無誤差演算を行う無誤差連立一次方程式解法処理方法考案した。これにより、安定且つ高信頼性ロボット制御方法を実現した。

0074

しかしながら、除算を用いない無誤差演算方法は、適用できる範囲が非常に限定され、且つ計算を進めていくと、際限なく数値桁数が増大し、計算効率が著しく低下していく。結果として、実用的ではない場合も多々ある。このため、本願発明者は、上記の特願2005−164567号明細書に記載の方法をベースにして、この方法の欠点をなくす方法を考案した。

0075

すなわち、本実施の形態では、式(15)の連立一次方程式の解法演算において、上記した可変長整数を用いた分数形式のデータフォーマット(可変長分数データ形式)で除算を表現して無誤差演算を行う。例えば、ガウスジョルダン消去法を用いて、連立一次方程式の解を求める処理を可変長分数データ処理演算により行い、各関節角度θ1,θ2,θ3,θ4,θ5を以下のようにして求めている。

0076

まず、式(15)の係数aij(i=1,…,5、j=1,…,5)及び係数bi(i=1,…,5)を用いて、k=1,…,5として、下記の式を繰り返し演算することにより係数a6ij及び係数b6iを求める。
ak+1ij=akij (i=k) (16a)
ak+1ij=akij−akkjakik/akkk (i≠k) (16b)
bk+1i=bki (i=k) (16c)
bk+1i=bki−bkkakik/akkk (i≠k) (16d)
上記の係数a6ij及び係数b6iにより、式(15)は以下の形式となる。

0077

0078

上記のように、式(17)の左辺の係数行列は、対角要素以外が0となる対角行列となる。式(6)及び式(17)から、各関節角度θ1,θ2,θ3,θ4,θ5は、以下の式により求めることができる。なお、手首の角度θ6は、3次元操作グローブ3により直接取得することができるので、上記の無誤差連立一次方程式解法処理からは求めていない。

0079

0080

上記の解法を基に、ロボットアーム制御装置1は、上記の無誤差連立一次方程式解法処理(目標データPから関節角度θを求めるデータ演算処理)を以下のようにして実行している。

0081

外部インターフェース部11は、ロボットアームの21の先端部の目標3次元位置(3次元位置方向センサ4の3次元位置Ps)及び目標方向(X、Y、Z軸方向の単位ベクトルdxs、dys、dzs)からなる目標データPを3次元位置方向センサ4から取得し、固定長数値データ記憶部12に浮動小数点データ形式で記憶させる。このとき、固定長数値データ記憶部12には、微小時間Δt前の位置方向データP’及び微小時間Δt前の関節角度データθ’iが記憶されており、また、上腕21aの長さe1、前腕21bの長さe2等の初期値は予め記憶されているものとする。

0082

ここで、上記の微小時間Δt前の位置方向データP’は、微小時間Δt前の目標データではなく、微小時間Δt前の実際の3次元位置データ及び方向データであり、微小時間Δt前の関節角度データθ’iもアームの実際の関節角度である。これは、後述するように、P’、θ’及びPからΔθを求め(線形近似後、連立一次方程式を無誤差で解く)、そして、θ=θ’+Δθからθを求めており、ここで(線形)近似しているため、このθから求めた実際の位置方向データは目標データと一致しない。そこで、次の時刻の計算では、微小時間Δt前の目標データ等ではなく、微小時間Δt前の実際の3次元位置データ及び方向データ、並びに微小時間Δt前の実際の関節角度データを用いている。

0083

固定長数値データ演算処理部15aは、固定長数値データ記憶部12から微小時間Δt前の関節角度θ’i、上腕21aの長さe1、前腕21bの長さe2を読み出し、浮動小数点演算部15bを用いて浮動小数点演算を実行することにより式(15)の係数データaijを算出し、固定長数値データ記憶部12に記憶させる。また、固定長数値データ演算処理部15aは、固定長数値データ記憶部12から目標データP及び微小時間Δt前の位置方向データP’を読み出し、浮動小数点演算部15bを用いて浮動小数点演算を実行することにより式(15)の係数データbiを算出し、固定長数値データ記憶部12に記憶させる。

0084

データ変換処理部13は、固定長数値データ記憶部12から係数aij及び係数biを順次読み出し、浮動小数点データ形式から可変長分数データ形式に変換した係数aij及び係数biを、係数a1ij及び係数b1iとして可変長分数データ記憶部14に格納する。

0085

可変長分数データ演算処理部16aは、可変長分数データ記憶部14から係数a1ij及び係数b1iを読み出し、可変長分数演算部16bを用いて可変長分数演算(無誤差演算)を実行することにより、式(16a)乃至(16d)を用いて係数ak+1ij及びbk+1iを順次算出し、最終的に係数a6ij及びb6iを可変長分数データ記憶部14に格納する。

0086

また、可変長分数データ演算処理部16aは、可変長分数データ記憶部14から係数a6ii及び係数b6iを読み出し、可変長分数演算部16bを用いて可変長分数演算(無誤差演算)を実行することにより、式(17)を基に微小関節角度変化量Δθi=b6i/a6iiを算出し、さらに、式(18)に従って関節角度θi=θi’+Δθiを算出し、可変長分数データ記憶部14に格納する。

0087

これらの可変長分数演算において、可変長分数データを用いて、加算、減算及び乗算はすべて実行され、除算は割り切れる場合に実行され、割り切れない場合には保留され、さらに、約分ができる場合は約分が実行される。

0088

データ変換処理部13は、可変長分数データ記憶部14から関節角度θiを順次読み出して可変長分数データ形式から浮動小数点データ形式に変換し、浮動小数点データ形式の関節角度θiを固定長数値データ記憶部12に格納する。

0089

外部インターフェース部11は、固定長数値データ記憶部12から浮動小数点データ形式の関節角度θiを読み出してロボットアーム部2へ出力する。ロボットアーム部2は、ロボットアーム21の各関節角度が関節角度θiとなるようにロボットアーム21を動作させる。

0090

上記の制御動作により、操作者Mは、3次元ディスプレイ5を用いて、カメラ部24により撮影された画像を見ながら、両手に装着した3次元操作グローブ3を用いて、ロボットアーム21及びハンド22を操作し、操作者Mの腕と手の動きとがロボットアーム21及びハンド22により忠実再現される。

0091

本実施の形態では、データ変換処理部13が変換手段の一例に相当し、無誤差制御処理部16(可変長分数データ演算処理部16a及び可変長分数演算部16b)が無誤差演算手段の一例に相当し、3次元位置方向センサ4が検出手段の一例に相当する。

0092

次に、上記のように構成されたロボットアーム制御装置1によるロボットアーム制御処理について説明する。図6は、図1に示すロボットアーム制御装置1によるロボットアーム制御処理を説明するためのフローチャートである。

0093

図6に示すように、まず、ステップS11において、外部インターフェース部11は、3次元位置方向センサ4から目標となる3次元位置方向データHsを取得して固定長数値データ記憶部12に格納する。

0094

次に、ステップS12において、誤差有制御処理部15の固定長数値データ演算処理部15aは、固定長数値データ記憶部12から3次元位置方向データHs等を読み出し、浮動小数点演算部15bを用いて、式(15)の係数aij(i=1,…,5、j=1,…,5)及び係数bi(i=1,…,5)を求め、固定長数値データ記憶部12に格納する。

0095

次に、ステップS13において、データ変換処理部13は、固定長数値データ記憶部12から係数aij及び係数biを順次読み出して浮動小数点データ形式から可変長分数データ形式へ変換し、可変長分数データ形式の係数a1ij及び係数b1iを可変長分数データ記憶部14に格納する。

0096

なお、上記のステップS12及びS13では、浮動小数点演算を用いて3次元位置方向データHs等から係数aij及び係数biを求めた後に、可変長分数データへフォーマット変換したが、この例に特に限定されず、データ変換処理部13により3次元位置方向データHsを浮動小数点データから可変長分数データに変換し、その後、無誤差制御処理部16により可変長分数データの3次元位置方向データHs等から可変長分数データの係数a1ij及び係数b1iを求めるようにしてもよい。

0097

ここで、図6に示すステップS13における固定長数値データ(浮動小数点数値データ)から可変長分数データへの変換処理を詳細に説明する。図7は、図6に示すステップS13における浮動小数点数値データから可変長分数データへの変換処理を説明するためのフローチャートである。

0098

図7に示すように、まず、ステップS31において、データ変換処理部13は、固定長数値データ記憶部12から浮動小数点数値データdを読み出し、浮動小数点数値データdを上位から(n+1)桁目四捨五入をする(ステップS51)。ここで、桁数nは要求される数値精度に応じて任意に設定することができる。例えば、d=12345.6789、n=7とすると、四捨五入した結果はd=12345.68となる。

0099

次に、ステップS32において、データ変換処理部13は、四捨五入後の数値dが整数か小数点以下の数値を持つかどうかを判定し、整数である場合は、ステップS33に処理を移行して可変長分数データの分子a=d、分母b=lに設定し、この可変長分数データa/bを、可変長分数データ記憶部14に格納した後、図6に示すステップS14へ処理を戻す。

0100

一方、数値dが小数点以下の数値を持つ場合は、データ変換処理部13は、ステップS34において、数値dの小数点以下の桁数n’を求め、ステップS35において、分子a=d×10n’、分母b=10n’とし、この可変長分数データa/bを可変長分数データ記憶部14に格納した後、図6に示すステップS14へ処理を戻す。上記の例では、小数点以下の数値を持ち、n’=2となるので、分子a=12345.68×102=1234568、分母b=102=100となり、これらのデータが一対のデータとして可変長分数データ記憶部14に記憶される。

0101

再び、図6を参照して、上記のステップS13の処理が終了した後、ステップS14、S15及びS16において、可変長分数データ演算処理部16aは、初期設定として、k=1、i=1、j=1に設定する。次に、ステップS17において、可変長分数データ演算処理部16aは、可変長分数データ記憶部14から係数akij,akkk,akkj,akikを読み出し、可変長分数演算部16bを用いて、i=kの場合はak+1ij=akijに設定し、i≠kの場合はak+1ij=akij−akkjakik/akkkを算出し、係数ak+1ijを可変長分数データ記憶部14に格納する。

0102

次に、ステップS18において、可変長分数データ演算処理部16aは、j<5であるか否かを判断し、j<5の場合、ステップS27においてjを1だけインクリメントしてステップS17以降の処理を継続し、j<5でない(j=5)場合、ステップS19へ処理を移行する。

0103

j<5でない場合、ステップS19において、可変長分数データ演算処理部16aは、可変長分数データ記憶部14から係数bki,bkk,akik,akkkを読み出し、可変長分数演算部16bを用いて、i=kの場合はbk+1i=bkiに設定し、i≠kの場合はbk+1i=bki−bkkakik/akkkを算出し、係数bk+1iを可変長分数データ記憶部14に格納する。

0104

次に、ステップS20において、可変長分数データ演算処理部16aは、i<5であるか否かを判断し、i<5の場合、ステップS26においてiを1だけインクリメントしてステップS16以降の処理を継続し、i<5でない(i=5)場合、ステップS21へ処理を移行する。

0105

i<5でない場合、ステップS21において、可変長分数データ演算処理部16aは、k<5であるか否かを判断し、k<5の場合、ステップS25においてkを1だけインクリメントしてステップS15以降の処理を継続し、k<5でない(k=5)場合、ステップS22へ処理を移行する。

0106

上記の処理により、可変長分数データ記憶部14からakij及びbkiが読み出され、無誤差制御処理部16により、可変長分数演算部16bを用いて式(16a)乃至(16d)の演算処理が行われ、演算結果であるak+1ij及びbk+1iが、再び可変長分数データ記憶部14に記憶される。ここでの演算の除算において、割り切れる場合は除算が実行され(分母が1となる)、割り切れない場合は、除算は保留される(分数形式のままとなる)。また、割り切れない場合で、約分ができる場合は約分が行われ、分子データ及び分母データの数値桁数の削減が行われる。これらの点に関して以下のステップS22における除算も同様である。

0107

次に、ステップS22において、可変長分数データ演算処理部16aは、可変長分数データ記憶部14から係数a6ii及び係数b6i(i=1,…,5)を順次読み出し、可変長分数演算部16bを用いて、微小関節角度変化量Δθi=b6i/a6ii(i=1,…,5)を算出し、可変長分数データ記憶部14に格納する。

0108

ここで、図6に示すステップS17、S19及びS22の除算における約分処理を詳細に説明する。本実施の形態では、ロボットアーム制御処理における除算において、例えば、可変長整数データa及びbに対する除算a÷bが発生すると、可変長分数データa/bとして表現され、分子データaと分母データbとが1つのペアデータとして可変長分数データ記憶部14に記憶され、割り切れる場合は除算が実行され、さらに、約分ができる場合は約分が行われる。

0109

図8は、図6に示すステップS17、S19及びS22の除算における約分処理を説明するためのフローチャートである。図8に示すように、約分処理では、まず、ステップS41において、可変長分数データ演算処理部16aは、可変長分数データa/bを可変長分数データ記憶部14から読み込む。

0110

次に、ステップS42において、可変長分数データ演算処理部16aは、可変長分数演算部16bを用いて、分子データa及び分母データbの下位ビットにおいて「0」が連続するビット数na及びnbを求める。例えば、a=12=1100(2進数)、b=6=110(2進数)であるとすると、na=2、nb=1となる。

0111

次に、ステップS43において、可変長分数データ演算処理部16aは、可変長分数演算部16bを用いて、ビット数naとnbとを比較し、大きくない方をnとする。上記の例では、n=nb=1となる。

0112

次に、ステップS44において、可変長分数データ演算処理部16aは、可変長分数演算部16bを用いて、分子データa及び分母データbのビットを下位方向にnビットシフトさせ、それぞれa’及びb’とする。上記の例では、n=1であるため、分子データa及び分母データbのビットを下位方向に1ビットシフトし、それぞれa’=6=110(2進数)及びb’=3=11(2進数)となる。

0113

次に、可変長分数データ演算処理部16aは、可変長分数演算部16bを用いて、ステップS45において、a’とb’とを比較し、小さくない方をA、大きくない方をBとし、ステップS46において、両者の差C=A−Bを求める。上記の例では、A=6、B=3となり、C=6−3=3となる。

0114

次に、可変長分数データ演算処理部16aは、可変長分数演算部16bを用いて、ステップS47において、C=0であるか否かを判断し、C=0の場合は、ステップS51において、Aが最大公約数となり、分子データa’及び分母データb’をAで除算し、約分された分数データa/bを再び可変長分数データ記憶部14に記憶させた後、図6に示すステップS17、S19及びS22へ処理を戻す。

0115

一方、C=0でない場合、可変長分数データ演算処理部16aは、可変長分数演算部16bを用いて、ステップS48において、A=B、B=Cに設定し、ステップS49において、A<Bであるか否かを判断し、A<Bでない場合は、ステップS46に戻って以降の処理を継続し、A<Bである場合は、ステップS50において、AとBとを入れ替えた後、ステップ46に戻って以降の処理を継続する。この処理では、BとCとを比較し、小さくない方をA、大きくない方をBとし、C=0となるまで、処理を繰り返す。上記の例では、C=0となるのは、A=3、B=3となるときであり、最大公約数は3となり、a/bを約分すると、a/b=12/6=a’/b’=6/3=(6÷3)/(3÷3)=2/1となる。

0116

再び、図6を参照して、上記のステップS22の処理が終了した後、ステップS23において、可変長分数データ演算処理部16aは、可変長分数データ記憶部14から直前の関節角度値θ’i及び微小関節角度変化量Δθi(i=1,…,5)を順次読み出し、可変長分数演算部16bを用いて、式(18)に従って関節角度θi=θi’+Δθi(i=1,…,5)を算出し、可変長分数データ記憶部14に格納する。このように、ステップS14〜S24及びS25〜S27までの処理が無誤差演算を用いて実行される。

0117

次に、ステップS24において、データ変換処理部13は、可変長分数データ記憶部14から関節角度θi(i=1,…,5)を順次読み出して可変長分数データ形式から浮動小数点データ形式に変換し、浮動小数点データ形式の関節角度θiを固定長数値データ記憶部12に格納する。この処理では、分数データを浮動小数点数値に変換するために除算すなわち分子を分母で割る処理が行われる。この演算により誤差が発生する場合があるが、この処理は1回の除算を行うだけであるので、除算に十分な精度が確保されていれば、誤差が問題になることはなく、誤差の管理が可能であり、誤差の蓄積も起こらない。

0118

また、ステップS24において、外部インターフェース部11は、固定長数値データ記憶部12から関節角度θ1,θ2,θ3,θ4,θ5を読み出してロボットアーム部2へ出力する。ロボットアーム部2は、ロボットアーム21の各関節角度が関節角度θ1,θ2,θ3,θ4,θ5となるようにロボットアーム21を動作させる。

0119

なお、手首の関節角度θ6は、3次元操作グローブ3から直接取得することができるので、3次元操作グローブ3により検出された関節角度θ6がロボットアーム制御装置1を介してロボットアーム部2へ出力され、ロボットアーム部2は、ロボットアーム21の手首の関節角度が関節角度θ6となるようにロボットアーム21を動作させる。上記の処理が制御時間間隔ごとに繰り返され、ロボットアーム21の先端位置が操作者Mの手首の位置に応じて正確に制御される。

0120

ここで、図6に示すステップS24における可変長分数データから浮動小数点数値データへの変換処理を詳細に説明する。図9は、図6に示すステップS24における可変長分数データから浮動小数点数値データへの変換処理を説明するためのフローチャートである。

0121

図9に示すように、データ変換処理部13は、まず、ステップS61において、可変長分数データ記憶部14から可変長分数データa/bを読み出して、分子データaの上位から(n+1)桁目で四捨五入をし、これを仮数部a’とし、ステップS62において、分子データaの指数部iを求める。ここで、桁数nは要求される数値精度に応じて任意に設定することができる。例えば、a/b=123456/789、n=3とすると、四捨五入した結果は、a’=1.23となり、i=5となる。

0122

次に、データ変換処理部13は、ステップS63において、分母データbの上位から(n+1)桁目で四捨五入をし、これを仮数部b’とし、ステップS64において、分母データの指数部jを求める。上記の例では、b’=7.89、j=2となる。

0123

次に、データ変換処理部13は、ステップS65において、a’をb’で割った仮数部cを求め、ステップS66において、その指数部k=i−jを求める。上記の例では、c=a’/b’=1.23/7.89=0.l56、k=i−j=5−2=3となる。

0124

最後に、ステップS67において、データ変換処理部13は、求めた仮数部と指数部とで構成される浮動小数点数値dを正規化して、固定長数値データ記憶部12に格納して処理を終了する。上記の例では、正規化は、d=0.156×103=1.56×102となる。なお、この例では、説明を分かりやすくするために、10進数を用いて説明してきたが、実際の実装は2進数を用いて行われる。

0125

上記の処理により、本実施の形態では、可変長分数データ演算を用いて無誤差演算制御を実現しているので、以下の効果を奏することができる。
(1)除算を含む四則演算すべてを用いて、無誤差演算で制御処理を行うことができ、汎用性、実用性が高い。
(2)可変長分数データ処理では、約分をすることができる。これにより、無誤差演算の欠点であった数値桁数の際限のない増大を抑えることができ、データ量を削減し、演算処理を効率化及び高速化することができる。
(3)制御演算誤差及びその蓄積をなくすことにより、制御処理の破綻をなくし、高精度な位置制御を実現することができる。
(4)リンク機構(ロボットアーム)動作の暴走等をなくすことができ、ロボット動作を安定化及び高信頼化することができる。
(5)上記との関連で、ロボットアームの暴走防止対策等が不要となり、制御プログラム及び制御システムを単純化することができる。

0126

なお、本実施の形態では、浮動小数点データを可変長分数データへ変換して無誤差演算を実行したが、この例に特に限定されず、浮動小数点データを可変長整数データへ変換し、除算が発生した場合に可変長整数データを可変長分数データへ変換して表現する等の種々の変更が可能である。

0127

また、操作者の手首の位置及び方向を検出し、これらを目標位置及び目標方向として制御したが、本発明による制御例は、この例に特に限定されず、3次元操作グローブ3、3次元位置方向センサ4及び3次元ディスプレイ5を省略し、テーチングを行うことにより固定長数値データ記憶部12に記憶した目標位置及び目標方向となるようにロボットアーム21を制御する等の他の制御例にも適用可能である。また、ロボットアーム21を制御対象としたが、この例に特に限定されず、ハンド22を制御対象にする等の種々の変更が可能である。

図面の簡単な説明

0128

本発明の一実施の形態によるロボットアーム制御装置を用いたロボットアーム制御システムの構成を示すブロック図である。
図1に示すロボットアーム制御装置の構成を示すブロック図である。
数値データの種々のフォーマットの例を示す図である。
図1に示すロボットアーム部における右側のロボットアームの構成を示す模式図である。
図4に示す右側のロボットアームの座標系を説明するための図である。
図1に示すロボットアーム制御装置によるロボットアーム制御処理を説明するためのフローチャートである。
図6に示すステップS13における浮動小数点数値データから可変長分数データへの変換処理を説明するためのフローチャートである。
図6に示すステップS17、S19及びS22の除算における約分処理を説明するためのフローチャートである。
図6に示すステップS24における可変長分数データから浮動小数点数値データへの変換処理を説明するためのフローチャートである。

符号の説明

0129

1ロボットアーム制御装置
2ロボットアーム部
3 3次元操作グローブ
4 3次元位置方向センサ
5 3次元ディスプレイ
11外部インターフェース部
12固定長数値データ記憶部
13データ変換処理部
14可変長分数データ記憶部
15誤差有制御処理部
15a 固定長数値データ演算処理部
15b浮動小数点演算部
16 無誤差制御処理部
16a 可変長分数データ演算処理部
16b可変長分数演算部
21 ロボットアーム
21a上腕
21b前腕
22ハンド
23胴体部
24カメラ部

ページトップへ

この技術を出願した法人

この技術を発明した人物

ページトップへ

関連する挑戦したい社会課題

関連する公募課題

該当するデータがありません

ページトップへ

技術視点だけで見ていませんか?

この技術の活用可能性がある分野

分野別動向を把握したい方- 事業化視点で見る -

(分野番号表示ON)※整理標準化データをもとに当社作成

ページトップへ

おススメ サービス

おススメ astavisionコンテンツ

新着 最近 公開された関連が強い技術

この 技術と関連性が強い人物

関連性が強い人物一覧

この 技術と関連する社会課題

関連する挑戦したい社会課題一覧

この 技術と関連する公募課題

該当するデータがありません

astavision 新着記事

サイト情報について

本サービスは、国が公開している情報(公開特許公報、特許整理標準化データ等)を元に構成されています。出典元のデータには一部間違いやノイズがあり、情報の正確さについては保証致しかねます。また一時的に、各データの収録範囲や更新周期によって、一部の情報が正しく表示されないことがございます。当サイトの情報を元にした諸問題、不利益等について当方は何ら責任を負いかねることを予めご承知おきのほど宜しくお願い申し上げます。

主たる情報の出典

特許情報…特許整理標準化データ(XML編)、公開特許公報、特許公報、審決公報、Patent Map Guidance System データ