知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-01-05
(45)【発行日】2024-01-16
(54)【発明の名称】ツールヘッドの位置姿勢の調整方法、装置及び可読記憶媒体
(51)【国際特許分類】
B25J 9/16 20060101AFI20240109BHJP
G05D 1/49 20240101ALI20240109BHJP
B25J 13/00 20060101ALI20240109BHJP
【ＦＩ】
B25J9/16
G05D1/08 Z
B25J13/00 Z
【請求項の数】
8
(21)【出願番号】P 2022557115
(86)(22)【出願日】2020-12-16
(65)【公表番号】
(43)【公表日】2023-04-25
(86)【国際出願番号】 CN2020136749
(87)【国際公開番号】W WO2021184859
(87)【国際公開日】2021-09-23
【審査請求日】2022-11-10
(31)【優先権主張番号】202010194511.7
(32)【優先日】2020-03-19
(33)【優先権主張国・地域又は機関】CN
(73)【特許権者】
【識別番号】522371558
【氏名又は名称】智美康民（珠海）健康科技有限公司
(74)【代理人】
【識別番号】100112737
【弁理士】
【氏名又は名称】藤田 考晴
(74)【代理人】
【識別番号】100136168
【弁理士】
【氏名又は名称】川上 美紀
(74)【代理人】
【識別番号】100196117
【弁理士】
【氏名又は名称】河合 利恵
(72)【発明者】
【氏名】巫超
(72)【発明者】
【氏名】李愛鎮
【審査官】樋口 幸太郎
(56)【参考文献】
【文献】特開２０１１－０６２７８６（ＪＰ，Ａ）
【文献】特開２００９－２９５１０７（ＪＰ，Ａ）
【文献】国際公開第２０１６／０３１３６４（ＷＯ，Ａ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ２５Ｊ ９／１６
Ｇ０５Ｄ １／０８
Ｂ２５Ｊ １３／００
(57)【特許請求の範囲】
【請求項1】
ロボットに用いられるツールヘッドの位置姿勢の調整方法であって、レーザ距離センサのレーザスポットを、各前記レーザスポットが一直線上に存在しないように、検出すべき部位上に照射するように制御するステップであって、前記レーザ距離センサの数が２よりも大きいステップと、
各前記レーザ距離センサによって測定される、前記レーザ距離センサと対応する前記レーザスポットとの距離である距離値、各前記レーザ距離センサの初期座標及び各前記レーザ距離センサのレーザ方向を取得するステップと、
前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算し、各前記レーザスポット座標によって決定される平面の、前記レーザスポット座標によって決定される平面の平面法線ベクトルから求められる統合平面法線ベクトルを計算するステップと、
予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算するとともに、前記位置姿勢パラメータに基づき、前記ロボットのツールヘッドを前記調整すべき位置姿勢に調整するように制御するステップとを含み、
前記位置姿勢式は、オイラー角、四元数又は回転行列式を含み、
予め設定される前記位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の前記位置姿勢パラメータを計算するステップは、
求められた前記統合平面法線ベクトルをＶ（ｖ ｘ ，ｖ ｙ ，ｖ ｚ ）とし、ｖ ｚ ＞０とし、ここで、ｖ ｚ ＜０の場合、前記統合平面法線ベクトルに－１を乗算することと、
式Ｍ（α，β，γ）＊［０，０，１］ Ｔ ＝Ｖ ｎｏｒｍ Ｔ。 を予め設定することであって、ここで、Ｍ（α，β，γ）は、オイラー角回転式のうちの回転行列であり、Ｖ ｎｏｒｍ は、モジュラスが１である前記統合平面法線ベクトルを表し、α、β、γは、前記オイラー角をそれぞれ表すことと、
前記予め設定される式、前記距離値及び前記オイラー角回転式に基づき、前記位置姿勢パラメータを求めることと、を含む、ことを特徴とするツールヘッドの位置姿勢の調整方法。
【請求項2】
前記レーザ距離センサの数が３よりも大きければ、各前記レーザスポット座標によって決定される平面の統合平面法線ベクトルを計算する前記ステップは、前記レーザスポット座標によって決定される対応する平面の平面法線ベクトルを計算することと、
重み付け平均アルゴリズムを利用して各前記平面法線ベクトルの重み付け平均値を計算し、前記重み付け平均値を前記統合平面法線ベクトルとすることとを含む、ことを特徴とする請求項１に記載のツールヘッドの位置姿勢の調整方法。
【請求項3】
前記レーザ距離センサの数は、３つであり、各前記レーザ距離センサのレーザ光は、互いに平行する、ことを特徴とする請求項１に記載のツールヘッドの位置姿勢の調整方法。
【請求項4】
前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算する前記ステップは、各前記レーザ方向の単位ベクトルをそれぞれＲａ（ｒｘ１，ｒｙ１，ｒｚ１）、Ｒｂ（ｒｘ２，ｒｙ２，ｒｚ２）、Ｒｃ（ｒｘ３，ｒｙ３，ｒｚ３）とし、各前記レーザ距離センサが位置するツールヘッド座標系の座標をＰａ（ｘ１，ｙ１，ｚ１）、Ｐｂ（ｘ２，ｙ２，ｚ２）、Ｐｃ（ｘ３，ｙ３，ｚ３）とし、各前記距離値をｄ１、ｄ２、ｄ３とし、各前記レーザスポットの座標をＵａ、Ｕｂ、Ｕｃとすると、
Ｕａ．ｘ＝ｘ１＋ｒｘ１＊ｄ１、Ｕａ．ｙ＝ｙ１＋ｒｙ１＊ｄ１、Ｕａ．ｚ＝ｚ１＋ｒｚ１＊ｄ１、
Ｕｂ．ｘ＝ｘ２＋ｒｘ２＊ｄ２、Ｕｂ．ｙ＝ｙ２＋ｒｙ２＊ｄ２、Ｕｂ．ｚ＝ｚ２＋ｒｚ２＊ｄ２、
Ｕｃ．ｘ＝ｘ３＋ｒｘ３＊ｄ３、Ｕｃ．ｙ＝ｙ３＋ｒｙ３＊ｄ３、Ｕｃ．ｚ＝ｚ３＋ｒｚ３＊ｄ３であり、
すなわち、各前記レーザスポットの座標Ｕａ（Ｕａ．ｘ，Ｕａ．ｙ，Ｕａ．ｚ）、Ｕｂ（Ｕｂ．ｘ，Ｕｂ．ｙ，Ｕｂ．ｚ）、Ｕｃ（Ｕｃ．ｘ，Ｕｃ．ｙ，Ｕｃ．ｚ）を求めることを含む、ことを特徴とする請求項３に記載のツールヘッドの位置姿勢の調整方法。
【請求項5】
前記予め設定される式、前記距離値及びオイラー角回転式に基づき、前記位置姿勢パラメータを求める前記ステップは、ｒｘ＝０とし、前記予め設定される式及び前記オイラー角回転式に基づいてｒｙ、ｒｚを求めることであって、ここで、ｒｘ、ｒｙ、ｒｚは、それぞれオイラー角α、β、γであることと、
前記距離値に基づいて予め設定されるアルゴリズムを利用して統合距離値を計算し、ｄａｖｅｒとすることと、
ツールヘッド座標系の原点をｚ方向にｄａｖｅｒを加算し、前記原点が前記検出すべき部位に設置されるようにすることと、
前記原点が前記検出すべき部位に設置される時、マニピュレータベースのベース座標系に対する前記原点の位置姿勢パラメータを取得し、ｘ０、ｙ０、ｚ０、ｒｘ０、ｒｙ０、ｒｚ０とすることと、
ツールヘッドの調整すべき位置姿勢のオイラー角パラメータをＲｘ、Ｒｙ、Ｒｚとすると、
Ｒｘ＝ｒｘ０、Ｒｙ＝ｒｙ０＋ｒｙ、Ｒｚ＝ｒｚ０＋ｒｚであり、すると、
前記ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータは、ｘ０、ｙ０、ｚ０、Ｒｘ、Ｒｙ、Ｒｚであることとを含む、ことを特徴とする請求項１に記載のツールヘッドの位置姿勢の調整方法。
【請求項6】
前記予め設定されるアルゴリズムは、平均値アルゴリズム及び重み付け平均アルゴリズムを含む、ことを特徴とする請求項５に記載のツールヘッドの位置姿勢の調整方法。
【請求項7】
メモリと、プロセッサと、前記メモリに記憶され且つ前記プロセッサ上で実行可能なツールヘッドの位置姿勢の調整プログラムとを含み、前記ツールヘッドの位置姿勢の調整プログラムが前記プロセッサによって実行されると、請求項１～６のいずれか一項に記載の方法のステップを実現する、ことを特徴とするツールヘッドの位置姿勢の調整装置。
【請求項8】
ツールヘッドの位置姿勢の調整プログラムが記憶されており、前記ツールヘッドの位置姿勢の調整プログラムがプロセッサによって実行されると、請求項１～６のいずれか一項に記載のツールヘッドの位置姿勢の調整方法のステップを実現する、ことを特徴とする可読記憶媒体。【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ロボット技術分野に関し、特にツールヘッドの位置姿勢の調整方法、装置及び可読記憶媒体に関する。
【背景技術】
【0002】
ロボット技術の発展に伴い、ロボットの性能と安全性が大きく向上し、人間と機械の協働ロボットの登場は、さらに人間とロボットが協働して作業することが確実に実行可能なことになったことを示している。ここ１０年で現代工業は、飛躍的に発展し、ロボットの普及と応用により、ロボットのコストは、ここ２年で急速に消費者が負担できる水準に近づいている。ロボットの一種であるパーソナルケアロボットは、マニピュレータアームの先端に取り付けられたケア機能（マッサージ、お灸、化粧、美容等）付きのツールヘッドを含み、該ツールヘッドは、ケア対象の人体と直接接触したり、特定の距離や姿勢を保ったりしてケア作業を行う。
【0003】
現在では、ケアロボットの作業時の位置姿勢（位置と姿勢）を決定する解決案は、主に機械視覚案、接触式多軸力センサ案、レーダ（超音波又はレーザなど）センサがある。上記解決案には、いずれも不十分な点がある。例えば、機械視覚案のロボットは、高価であり、機械視覚案には、特別な光源による照明を必要とし、スキャン時間及び座標生成時間が長く、リアルタイム性が低く、カメラの体積が大きくてツールヘッドに統合されるのに不利であり、顧客のプライバシー部位を露出させる。接触式多軸力センサ案のロボットは、高価であり、ツールヘッドと人体とが接触するシナリオにのみ適用され、ツールヘッドの位置姿勢のガイドに対する先見性がない。レーダ案のロボットは、高価で精度が低く、死角が大きく、干渉を受けやすく、長時間のレーダ波の放射が人体の健康に潜在的なリスクがある。このため、どのように価格が低く、精度が高く、リアルタイム性が高く、位置姿勢の決定が正確なツールヘッドの位置姿勢決定案を提供することは、解決すべき問題となっている。
【0004】
上記内容は、本発明の技術案の理解を補助するためにのみ用いられ、上記内容が従来技術であることを認めることを意味するものではない。
【発明の概要】
【発明が解決しようとする課題】
【0005】
本発明の主な目的は、価格が低く、精度が高く、リアルタイム性が高く、位置姿勢の決定が正確なツールヘッドの位置姿勢決定案を提供することである。
【課題を解決するための手段】
【0006】
上記目的を実現するために、本発明は、ツールヘッドの位置姿勢の調整方法を提供し、前記ツールヘッドの位置姿勢の調整方法は、
数が２よりも大きいレーザ距離センサのレーザスポットを、各前記レーザスポットが一直線上に存在しないように、検出すべき部位上に照射するように制御するステップと、
各前記レーザ距離センサによって測定される、前記レーザ距離センサと対応する前記レーザスポットとの距離である距離値、各前記レーザ距離センサの初期座標及び各前記レーザ距離センサのレーザ方向を取得するステップと、
前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算し、各前記レーザスポット座標によって決定される平面の、前記レーザスポット座標によって決定される平面の平面法線ベクトルから求められる統合平面法線ベクトルを計算するステップと、
予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算するとともに、前記位置姿勢パラメータに基づき、前記ロボットのツールヘッドを前記調整すべき位置姿勢に調整するように制御するステップとを含む。
数が２よりも大きいレーザ距離センサのレーザスポットを、各前記レーザスポットが一直線上に存在しないように、検出すべき部位上に照射するように制御するステップと、
各前記レーザ距離センサによって測定される、前記レーザ距離センサと対応する前記レーザスポットとの距離である距離値、各前記レーザ距離センサの初期座標及び各前記レーザ距離センサのレーザ方向を取得するステップと、
前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算し、各前記レーザスポット座標によって決定される平面の、前記レーザスポット座標によって決定される平面の平面法線ベクトルから求められる統合平面法線ベクトルを計算するステップと、
予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算するとともに、前記位置姿勢パラメータに基づき、前記ロボットのツールヘッドを前記調整すべき位置姿勢に調整するように制御するステップとを含む。
【0007】
任意選択的に、前記レーザ距離センサの数が３よりも大きければ、各前記レーザスポット座標によって決定される平面の統合平面法線ベクトルを計算する前記ステップは、
前記レーザスポット座標によって決定される対応する平面の平面法線ベクトルを計算することと、
重み付け平均アルゴリズムを利用して各前記平面法線ベクトルの重み付け平均値を計算し、前記重み付け平均値を前記統合平面法線ベクトルとすることとを含む。
前記レーザスポット座標によって決定される対応する平面の平面法線ベクトルを計算することと、
重み付け平均アルゴリズムを利用して各前記平面法線ベクトルの重み付け平均値を計算し、前記重み付け平均値を前記統合平面法線ベクトルとすることとを含む。
【0008】
任意選択的に、前記位置姿勢式は、オイラー角、四元数又は回転行列式を含む。
【0009】
任意選択的に、前記レーザ距離センサの数は、３つであり、各前記レーザ距離センサのレーザ光は、互いに平行する。
【0010】
任意選択的に、前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算する前記ステップは、
各前記レーザ方向の単位ベクトルをそれぞれＲａ（ｒｘ１，ｒｙ１，ｒｚ１）、Ｒｂ（ｒｘ２，ｒｙ２，ｒｚ２）、Ｒｃ（ｒｘ３，ｒｙ３，ｒｚ３）とし、各前記レーザ距離センサが位置するツールヘッド座標系の座標をＰａ（ｘ１，ｙ１，ｚ１）、Ｐｂ（ｘ２，ｙ２，ｚ２）、Ｐｃ（ｘ３，ｙ３，ｚ３）とし、各前記距離値をｄ１、ｄ２、ｄ３とし、各前記レーザスポットの座標をＵａ、Ｕｂ、Ｕｃとすると、
Ｕａ．ｘ＝ｘ１＋ｒｘ１＊ｄ１、Ｕａ．ｙ＝ｙ１＋ｒｙ１＊ｄ１、Ｕａ．ｚ＝ｚ１＋ｒｚ１＊ｄ１、
Ｕｂ．ｘ＝ｘ２＋ｒｘ２＊ｄ２、Ｕｂ．ｙ＝ｙ２＋ｒｙ２＊ｄ２、Ｕｂ．ｚ＝ｚ２＋ｒｚ２＊ｄ２、
Ｕｃ．ｘ＝ｘ３＋ｒｘ３＊ｄ３、Ｕｃ．ｙ＝ｙ３＋ｒｙ３＊ｄ３、Ｕｃ．ｚ＝ｚ３＋ｒｚ３＊ｄ３であり、
すなわち、各前記レーザスポットの座標Ｕａ（Ｕａ．ｘ，Ｕａ．ｙ，Ｕａ．ｚ）、Ｕｂ（Ｕｂ．ｘ，Ｕｂ．ｙ，Ｕｂ．ｚ）、Ｕｃ（Ｕｃ．ｘ，Ｕｃ．ｙ，Ｕｃ．ｚ）を求めることを含む、ことを特徴とする。
各前記レーザ方向の単位ベクトルをそれぞれＲａ（ｒｘ１，ｒｙ１，ｒｚ１）、Ｒｂ（ｒｘ２，ｒｙ２，ｒｚ２）、Ｒｃ（ｒｘ３，ｒｙ３，ｒｚ３）とし、各前記レーザ距離センサが位置するツールヘッド座標系の座標をＰａ（ｘ１，ｙ１，ｚ１）、Ｐｂ（ｘ２，ｙ２，ｚ２）、Ｐｃ（ｘ３，ｙ３，ｚ３）とし、各前記距離値をｄ１、ｄ２、ｄ３とし、各前記レーザスポットの座標をＵａ、Ｕｂ、Ｕｃとすると、
Ｕａ．ｘ＝ｘ１＋ｒｘ１＊ｄ１、Ｕａ．ｙ＝ｙ１＋ｒｙ１＊ｄ１、Ｕａ．ｚ＝ｚ１＋ｒｚ１＊ｄ１、
Ｕｂ．ｘ＝ｘ２＋ｒｘ２＊ｄ２、Ｕｂ．ｙ＝ｙ２＋ｒｙ２＊ｄ２、Ｕｂ．ｚ＝ｚ２＋ｒｚ２＊ｄ２、
Ｕｃ．ｘ＝ｘ３＋ｒｘ３＊ｄ３、Ｕｃ．ｙ＝ｙ３＋ｒｙ３＊ｄ３、Ｕｃ．ｚ＝ｚ３＋ｒｚ３＊ｄ３であり、
すなわち、各前記レーザスポットの座標Ｕａ（Ｕａ．ｘ，Ｕａ．ｙ，Ｕａ．ｚ）、Ｕｂ（Ｕｂ．ｘ，Ｕｂ．ｙ，Ｕｂ．ｚ）、Ｕｃ（Ｕｃ．ｘ，Ｕｃ．ｙ，Ｕｃ．ｚ）を求めることを含む、ことを特徴とする。
【0011】
任意選択的に、予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算する前記ステップは、
求められた前記統合平面法線ベクトルをＶ（ｖｘ，ｖｙ，ｖｚ）とし、ｖｚ＞０とし、ここで、ｖｚ＜０の場合、前記統合平面法線ベクトルに－１を乗算することと、
式Ｍ（α，β，γ）＊［０，０，１］Ｔ＝Ｖｎｏｒｍ Ｔ。を予め設定することであって、ここで、Ｍ（α，β，γ）は、オイラー角回転式のうちの回転行列であり、Ｖｎｏｒｍは、モジュラスが１である前記統合平面法線ベクトルを表し、α、β、γは、オイラー角をそれぞれ表すことと、
前記予め設定される式、前記距離値及びオイラー角回転式に基づき、前記位置姿勢パラメータを求めることと、を含む。
求められた前記統合平面法線ベクトルをＶ（ｖｘ，ｖｙ，ｖｚ）とし、ｖｚ＞０とし、ここで、ｖｚ＜０の場合、前記統合平面法線ベクトルに－１を乗算することと、
式Ｍ（α，β，γ）＊［０，０，１］Ｔ＝Ｖｎｏｒｍ Ｔ。を予め設定することであって、ここで、Ｍ（α，β，γ）は、オイラー角回転式のうちの回転行列であり、Ｖｎｏｒｍは、モジュラスが１である前記統合平面法線ベクトルを表し、α、β、γは、オイラー角をそれぞれ表すことと、
前記予め設定される式、前記距離値及びオイラー角回転式に基づき、前記位置姿勢パラメータを求めることと、を含む。
【0012】
任意選択的に、前記予め設定される式、前記距離値及びオイラー角回転式に基づき、前記位置姿勢パラメータを求める前記ステップは、
ｒｘ＝０とし、前記予め設定される式及び前記オイラー角回転式連立方程式に基づいてｒｙ、ｒｚを求めることであって、ここで、ｒｘ、ｒｙ、ｒｚは、それぞれオイラー角α、β、γであることと、
前記距離値に基づいて予め設定されるアルゴリズムを利用して統合距離値を計算し、ｄａｖｅｒとすることと、
前記ツールヘッド座標系の原点をｚ方向にｄａｖｅｒを加算し、前記原点が前記検出すべき部位に設置されるようにすることと、
前記原点が前記検出すべき部位に設置される時、マニピュレータベースのベース座標系に対する前記原点の位置姿勢パラメータを取得し、ｘ０、ｙ０、ｚ０、ｒｘ０、ｒｙ０、ｒｚ０とすることと、
前記ツールヘッドの調整すべき位置姿勢のオイラー角パラメータをＲｘ、Ｒｙ、Ｒｚとすると、
Ｒｘ＝ｒｘ０、Ｒｙ＝ｒｙ０＋ｒｙ、Ｒｚ＝ｒｚ０＋ｒｚであり、すると、
前記ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータは、ｘ０、ｙ０、ｚ０、Ｒｘ、Ｒｙ、Ｒｚであることと、を含む。
ｒｘ＝０とし、前記予め設定される式及び前記オイラー角回転式連立方程式に基づいてｒｙ、ｒｚを求めることであって、ここで、ｒｘ、ｒｙ、ｒｚは、それぞれオイラー角α、β、γであることと、
前記距離値に基づいて予め設定されるアルゴリズムを利用して統合距離値を計算し、ｄａｖｅｒとすることと、
前記ツールヘッド座標系の原点をｚ方向にｄａｖｅｒを加算し、前記原点が前記検出すべき部位に設置されるようにすることと、
前記原点が前記検出すべき部位に設置される時、マニピュレータベースのベース座標系に対する前記原点の位置姿勢パラメータを取得し、ｘ０、ｙ０、ｚ０、ｒｘ０、ｒｙ０、ｒｚ０とすることと、
前記ツールヘッドの調整すべき位置姿勢のオイラー角パラメータをＲｘ、Ｒｙ、Ｒｚとすると、
Ｒｘ＝ｒｘ０、Ｒｙ＝ｒｙ０＋ｒｙ、Ｒｚ＝ｒｚ０＋ｒｚであり、すると、
前記ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータは、ｘ０、ｙ０、ｚ０、Ｒｘ、Ｒｙ、Ｒｚであることと、を含む。
【0013】
任意選択的に、前記予め設定されるアルゴリズムは、平均値アルゴリズム及び重み付け平均アルゴリズムを含む。
【0014】
なお、上記目的を実現するために、本発明は、メモリと、プロセッサと、前記メモリに記憶され且つ前記プロセッサ上で実行可能なツールヘッドの位置姿勢の調整プログラムとを含み、前記ツールヘッドの位置姿勢の調整プログラムが前記プロセッサによって実行されると、以上に記載のツールヘッドの位置姿勢の調整方法のステップを実現するツールヘッドの位置姿勢の調整装置をさらに提供する。
【0015】
なお、上記目的を実現するために、本発明は、ツールヘッドの位置姿勢の調整プログラムが記憶されており、前記ツールヘッドの位置姿勢の調整プログラムがプロセッサによって実行されると、以上に記載のツールヘッドの位置姿勢の調整方法のステップを実現する可読記憶媒体をさらに提供する。
【発明の効果】
【0016】
本発明は、ツールヘッドの位置姿勢の調整方法、装置及び可読記憶媒体を提供し、ロボットのツールヘッドの位置姿勢を正確に決定することにより、ロボットの作業効率を向上させることができ、本発明のレーザ距離センサは、コストが低く、体積が小さく、精度が高く、ロボットのツールヘッド上に良好に統合されることができ、それにより、ツールヘッドの位置姿勢の決定に有利であり、レーザ距離器の位置をリアルタイムに調整することができ、且つ本発明は、ツールヘッドと人体とが接触するシナリオ又は人体との距離を一定に保持するシナリオに適応することができる。
【図面の簡単な説明】
【0017】
【図1】本発明のツールヘッドの位置姿勢の調整方法の第１の実施例のフローチャートである。
【図2】お灸器ロボットが検出すべき部位で鍼灸を行う概略図である。
【図3】第１の実施例における３つのレーザ距離センサがツールヘッドに位置する概略図である。
【発明を実施するための形態】
【0018】
本発明の目的の実現、機能特徴及び利点は、実施例を結び付けて、図面を参照しながらさらに説明する。
【0019】
本出願の目的、技術的解決手段及び利点をより明確にするために、以下、図面と実施例を参照しながら、本出願についてさらに詳細に説明する。本明細書に記述される具体的な実施例は、本出願を説明するためにのみ用いられ、本出願を限定するために用いられるものではない。
【0020】
【0021】
本実施例では、該ツールヘッドの位置姿勢の調整方法の実行本体は、ツールヘッドの位置姿勢の調整システムであり、該ツールヘッドの位置姿勢の調整システムは、ツールヘッドの位置姿勢の調整装置を含み、該ツールヘッドの位置姿勢の調整装置は、ロボット機器、例えば、お灸器、マッサージ器などであってもよく、無論、ＰＣ、ハンドヘルドなどの端末機器であってもよい。本発明は、レーザ距離センサによってレーザスポットの距離値を検出し、各レーザスポットは、少なくとも一つの平面を構築することができ、それにより、検出すべき部位に対応する各平面を決定するとともに、各平面の統合法線ベクトルを計算し、位置姿勢式を利用してロボットのツールヘッドの位置姿勢パラメータを計算することにより、ツールヘッドの位置姿勢を調整する。本発明は、ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを正確に決定することができる。本実施例は、お灸器を例として説明する。該ツールヘッドの位置姿勢の調整方法は、以下のステップを含む。
【0022】
ステップＳ１０では、数が２よりも大きいレーザ距離センサのレーザスポットを、各前記レーザスポットが一直線上に存在しないように、検出すべき部位上に照射するように制御する。
本実施例では、図２を参照すると、図２は、お灸器ロボットが検出すべき部位で鍼灸を行う概略図である。該検出すべき部位は、患者が鍼灸すべき身体部位を含む。該レーザ距離センサは、鍼灸器ロボットのツールヘッドに取り付けられることができ、例えば、ロボットのツールヘッドの固定連動関係を有する面に３つ又は３つ以上のレーザ距離センサ２が取り付けられる。無論、ロボットの他の位置に取り付けられてもよい。マニピュレータ１は、ツールヘッドの位置姿勢を調整するために用いられる。検出すべき部位がレーザ距離センサ２のスパン範囲に入る時、各レーザ距離センサ２のレーザ光が検出すべき部位に照射され、検出すべき部位にレーザスポットが形成される。該レーザ距離センサ２の数は３以上である。各レーザスポットは、平面を構築できなければならないため、該レーザスポットは、同一直線上にはならない。好ましくは、該レーザ距離センサ２のレーザ光が一点に集中することを避け、該レーザ光は、互いに平行し、永遠に交点がなく、このように、レーザスポット座標を計算する時にアルゴリズムを簡略化することにより、データ処理効率を向上させ、さらにツールヘッドの位置姿勢の調整効率を加速することができる。
本実施例では、図２を参照すると、図２は、お灸器ロボットが検出すべき部位で鍼灸を行う概略図である。該検出すべき部位は、患者が鍼灸すべき身体部位を含む。該レーザ距離センサは、鍼灸器ロボットのツールヘッドに取り付けられることができ、例えば、ロボットのツールヘッドの固定連動関係を有する面に３つ又は３つ以上のレーザ距離センサ２が取り付けられる。無論、ロボットの他の位置に取り付けられてもよい。マニピュレータ１は、ツールヘッドの位置姿勢を調整するために用いられる。検出すべき部位がレーザ距離センサ２のスパン範囲に入る時、各レーザ距離センサ２のレーザ光が検出すべき部位に照射され、検出すべき部位にレーザスポットが形成される。該レーザ距離センサ２の数は３以上である。各レーザスポットは、平面を構築できなければならないため、該レーザスポットは、同一直線上にはならない。好ましくは、該レーザ距離センサ２のレーザ光が一点に集中することを避け、該レーザ光は、互いに平行し、永遠に交点がなく、このように、レーザスポット座標を計算する時にアルゴリズムを簡略化することにより、データ処理効率を向上させ、さらにツールヘッドの位置姿勢の調整効率を加速することができる。
【0023】
ステップＳ２０では、各前記レーザ距離センサによって測定される、前記レーザ距離センサと対応する前記レーザスポットとの距離である距離値、各前記レーザ距離センサの初期座標及び各前記レーザ距離センサのレーザ方向を取得する。
本実施例では、レーザ距離センサ２は、レーザスポットまでの距離を測定することができる。ツールヘッド座標系を設定し、ツールヘッド座標系におけるレーザ距離センサ２の初期座標及びレーザ距離センサ２のレーザ方向を取得する。該レーザ距離センサ２のレーザ方向は、単位ベクトルで表すことができる。
本実施例では、レーザ距離センサ２は、レーザスポットまでの距離を測定することができる。ツールヘッド座標系を設定し、ツールヘッド座標系におけるレーザ距離センサ２の初期座標及びレーザ距離センサ２のレーザ方向を取得する。該レーザ距離センサ２のレーザ方向は、単位ベクトルで表すことができる。
【0024】
ステップＳ３０では、前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算し、各前記レーザスポット座標によって決定される平面の、前記レーザスポット座標によって決定される平面の平面法線ベクトルから求められる統合平面法線ベクトルを計算する。
本実施例では、図３を参照すると、図３は、３つのレーザ距離センサ２がツールヘッドに位置する概略図である。該レーザ距離センサ２が３つであれば、該統合平面法線ベクトルは、３つのレーザスポットが位置する平面の平面法線ベクトルであり、ここで、ベクトルクロスプロダクト法によって該平面法線ベクトルを計算して得ることができる。該レーザ距離センサ２の数が３よりも大きければ、該統合法線ベクトルは、重み付け平均アルゴリズム又は他のアルゴリズムによって得られる法線ベクトルを利用することができ、例えば、各レーザスポットが４つの平面を構築することができれば、それぞれベクトルクロスプロダクト法によって４つの平面の法線ベクトルを計算し、その後に重み付け平均アルゴリズムによって４つの法線ベクトルの重み付け平均値を計算することにより、統合法線ベクトルを得る。
本実施例では、図３を参照すると、図３は、３つのレーザ距離センサ２がツールヘッドに位置する概略図である。該レーザ距離センサ２が３つであれば、該統合平面法線ベクトルは、３つのレーザスポットが位置する平面の平面法線ベクトルであり、ここで、ベクトルクロスプロダクト法によって該平面法線ベクトルを計算して得ることができる。該レーザ距離センサ２の数が３よりも大きければ、該統合法線ベクトルは、重み付け平均アルゴリズム又は他のアルゴリズムによって得られる法線ベクトルを利用することができ、例えば、各レーザスポットが４つの平面を構築することができれば、それぞれベクトルクロスプロダクト法によって４つの平面の法線ベクトルを計算し、その後に重み付け平均アルゴリズムによって４つの法線ベクトルの重み付け平均値を計算することにより、統合法線ベクトルを得る。
【0025】
ここで、該レーザ距離センサ２の数が３つであれば、各レーザ距離センサ２のレーザ光が互いに平行する。各レーザ方向の単位ベクトルをそれぞれＲａ（ｒｘ１，ｒｙ１，ｒｚ１）、Ｒｂ（ｒｘ２，ｒｙ２，ｒｚ２）、Ｒｃ（ｒｘ３，ｒｙ３，ｒｚ３）、各レーザ距離センサが位置するツールヘッド座標系の座標をＰａ（ｘ１，ｙ１，ｚ１）、Ｐｂ（ｘ２，ｙ２，ｚ２）、Ｐｃ（ｘ３，ｙ３，ｚ３）、各距離値をｄ１、ｄ２、ｄ３、各レーザスポットの座標をＵａ、Ｕｂ、Ｕｃとすると、
Ｕａ．ｘ＝ｘ１＋ｒｘ１＊ｄ１、Ｕａ．ｙ＝ｙ１＋ｒｙ１＊ｄ１、Ｕａ．ｚ＝ｚ１＋ｒｚ１＊ｄ１、
Ｕｂ．ｘ＝ｘ２＋ｒｘ２＊ｄ２、Ｕｂ．ｙ＝ｙ２＋ｒｙ２＊ｄ２、Ｕｂ．ｚ＝ｚ２＋ｒｚ２＊ｄ２、
Ｕｃ．ｘ＝ｘ３＋ｒｘ３＊ｄ３、Ｕｃ．ｙ＝ｙ３＋ｒｙ３＊ｄ３、Ｕｃ．ｚ＝ｚ３＋ｒｚ３＊ｄ３であり、
すなわち、各レーザスポットの座標Ｕａ（Ｕａ．ｘ，Ｕａ．ｙ，Ｕａ．ｚ）、Ｕｂ（Ｕｂ．ｘ，Ｕｂ．ｙ，Ｕｂ．ｚ）、Ｕｃ（Ｕｃ．ｘ，Ｕｃ．ｙ，Ｕｃ．ｚ）を求め、ベクトルクロスプロダクト法によって３つのレーザスポットの平面法線ベクトルを計算することができる。
Ｕａ．ｘ＝ｘ１＋ｒｘ１＊ｄ１、Ｕａ．ｙ＝ｙ１＋ｒｙ１＊ｄ１、Ｕａ．ｚ＝ｚ１＋ｒｚ１＊ｄ１、
Ｕｂ．ｘ＝ｘ２＋ｒｘ２＊ｄ２、Ｕｂ．ｙ＝ｙ２＋ｒｙ２＊ｄ２、Ｕｂ．ｚ＝ｚ２＋ｒｚ２＊ｄ２、
Ｕｃ．ｘ＝ｘ３＋ｒｘ３＊ｄ３、Ｕｃ．ｙ＝ｙ３＋ｒｙ３＊ｄ３、Ｕｃ．ｚ＝ｚ３＋ｒｚ３＊ｄ３であり、
すなわち、各レーザスポットの座標Ｕａ（Ｕａ．ｘ，Ｕａ．ｙ，Ｕａ．ｚ）、Ｕｂ（Ｕｂ．ｘ，Ｕｂ．ｙ，Ｕｂ．ｚ）、Ｕｃ（Ｕｃ．ｘ，Ｕｃ．ｙ，Ｕｃ．ｚ）を求め、ベクトルクロスプロダクト法によって３つのレーザスポットの平面法線ベクトルを計算することができる。
【0026】
ステップＳ４０では、予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算するとともに、前記位置姿勢パラメータに基づき、前記ロボットのツールヘッドを前記調整すべき位置姿勢に調整するように制御する。
【0027】
本実施例では、該予め設定される位置姿勢式は、オイラー角、四元数又は回転行列等の式を含む。統合平面法線ベクトル及び距離値に基づいて位置姿勢式を利用してロボットの調整すべき位置姿勢の位置姿勢パラメータを求める。
【0028】
以下では、オイラー角回転式を利用してロボットの調整すべき位置姿勢の位置姿勢パラメータを計算する。
【0029】
求められた統合平面法線ベクトルをＶ（ｖｘ，ｖｙ，ｖｚ）とし、ｖｚ＞０とし、ここで、ｖｚ＜０の場合、統合平面法線ベクトルに－１を乗算し、ここで、平面統合法線ベクトルの単位法線ベクトルを計算するために、該統合法線ベクトルの方向を決定する。
オイラー角回転式は、次の（１）式であり、式Ｍ（α，β，γ）＊［０，０，１］Ｔ＝Ｖｎｏｒｍ Ｔ。を予め設定し、ここで、Ｍ（α，β，γ）は、オイラー角回転式のうちの回転行列であり、α、β、γは、オイラー角をそれぞれ表し、Ｖｎｏｒｍは、モジュラスが１である前記統合平面法線ベクトルを表し、α、β、γは、オイラー角をそれぞれ表す。
オイラー角回転式は、次の（１）式であり、式Ｍ（α，β，γ）＊［０，０，１］Ｔ＝Ｖｎｏｒｍ Ｔ。を予め設定し、ここで、Ｍ（α，β，γ）は、オイラー角回転式のうちの回転行列であり、α、β、γは、オイラー角をそれぞれ表し、Ｖｎｏｒｍは、モジュラスが１である前記統合平面法線ベクトルを表し、α、β、γは、オイラー角をそれぞれ表す。
【0030】
【数1】
【0031】
ｒｘ＝０とし、前記予め設定される式及び前記オイラー角回転式連立方程式に基づいてｒｙ、ｒｚを求めることであって、ここで、ｒｘ、ｒｙ、ｒｚは、それぞれオイラー角α、β、γである。距離値に基づいて予め設定されるアルゴリズムを利用して統合距離値を計算し、ｄａｖｅｒとし、ツールヘッド座標系の原点をｚ方向にｄａｖｅｒを加算し、該原点が検出すべき部位に設置されるようにする。その後に、該原点が該検出すべき部位に設置される時、マニピュレータベースのベース座標系に対する該原点の位置姿勢パラメータを取得し、ｘ０、ｙ０、ｚ０、ｒｘ０、ｒｙ０、ｒｚ０とする。ツールヘッドの調整すべき位置姿勢のオイラー角パラメータをＲｘ、Ｒｙ、Ｒｚとすると、Ｒｘ＝ｒｘ０、Ｒｙ＝ｒｙ０＋ｒｙ、Ｒｚ＝ｒｚ０＋ｒｚであり、すると、ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータはｘ０、ｙ０、ｚ０、Ｒｘ、Ｒｙ、Ｒｚであり、ここで、該予め設定されるアルゴリズムは、平均値アルゴリズム及び重み付け平均アルゴリズムを含み、例えば、平均値アルゴリズムを利用してｄ１、ｄ２、ｄ３の平均値を計算し、ｄａｖｅｒを得る。
【0032】
調整すべき位置姿勢の位置姿勢パラメータに基づいてツールヘッドの位置姿勢を調整することにより、ロボットは、検出すべき部位に対して鍼灸ケアを正確に行うことができる。
【0033】
本実施例は、ツールヘッドの位置姿勢の調整方法を提供する。数が２よりも大きいレーザ距離センサのレーザスポットを、各前記レーザスポットが一直線上に存在しないように、検出すべき部位上に照射するように制御する。各前記レーザ距離センサによって測定される、前記レーザ距離センサと対応する前記レーザスポットとの距離である距離値、各前記レーザ距離センサの初期座標及び各前記レーザ距離センサのレーザ方向を取得する。前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算し、各前記レーザスポット座標によって決定される平面の、前記レーザスポット座標によって決定される平面の平面法線ベクトルから求められる統合平面法線ベクトルを計算する。予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算するとともに、前記位置姿勢パラメータに基づき、前記ロボットのツールヘッドを前記調整すべき位置姿勢に調整するように制御する。それにより、ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを正確に決定することを実現し、さらにお灸器の作業効率を向上させることができ、且つ、本発明のレーザ距離センサは、コストが低く、ロボットが作業する時、精度が高く、リアルタイム性が高く、ツールヘッドと人体とが接触するシナリオ又は人体との距離を一定に保持するシナリオに適応することができる。
【0034】
なお、本発明の実施例は、可読記憶媒体をさらに提供する。本発明の可読記憶媒体にツールヘッドの位置姿勢の調整プログラムが記憶され、前記ツールヘッドの位置姿勢の調整プログラムがプロセッサによって実行されると、
数が２よりも大きいレーザ距離センサのレーザスポットを、各前記レーザスポットが一直線上に存在しないように、検出すべき部位上に照射するように制御するステップと、
各前記レーザ距離センサによって測定される、前記レーザ距離センサと対応する前記レーザスポットとの距離である距離値、各前記レーザ距離センサの初期座標及び各前記レーザ距離センサのレーザ方向を取得するステップと、
前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算し、各前記レーザスポット座標によって決定される平面の、前記レーザスポット座標によって決定される平面の平面法線ベクトルから求められる統合平面法線ベクトルを計算するステップと、
予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算するとともに、前記位置姿勢パラメータに基づき、前記ロボットのツールヘッドを前記調整すべき位置姿勢に調整するように制御するステップとを実現する。
数が２よりも大きいレーザ距離センサのレーザスポットを、各前記レーザスポットが一直線上に存在しないように、検出すべき部位上に照射するように制御するステップと、
各前記レーザ距離センサによって測定される、前記レーザ距離センサと対応する前記レーザスポットとの距離である距離値、各前記レーザ距離センサの初期座標及び各前記レーザ距離センサのレーザ方向を取得するステップと、
前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算し、各前記レーザスポット座標によって決定される平面の、前記レーザスポット座標によって決定される平面の平面法線ベクトルから求められる統合平面法線ベクトルを計算するステップと、
予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算するとともに、前記位置姿勢パラメータに基づき、前記ロボットのツールヘッドを前記調整すべき位置姿勢に調整するように制御するステップとを実現する。
【0035】
さらに、前記レーザ距離センサの数が３よりも大きければ、各前記レーザスポット座標によって決定される平面の統合平面法線ベクトルを計算する前記ステップは、
前記レーザスポット座標によって決定される対応する平面の平面法線ベクトルを計算することと、
重み付け平均アルゴリズムを利用して各前記平面法線ベクトルの重み付け平均値を計算し、前記重み付け平均値を前記統合平面法線ベクトルとすることとを含む。
前記レーザスポット座標によって決定される対応する平面の平面法線ベクトルを計算することと、
重み付け平均アルゴリズムを利用して各前記平面法線ベクトルの重み付け平均値を計算し、前記重み付け平均値を前記統合平面法線ベクトルとすることとを含む。
【0036】
さらに、前記位置姿勢式は、オイラー角、四元数又は回転行列式を含む。
【0037】
さらに、前記レーザ距離センサの数は、３つであり、各前記レーザ距離センサのレーザ光は、互いに平行する。
【0038】
さらに、前記距離値、前記初期座標及び前記レーザ方向に基づき、前記検出すべき部位上の各レーザスポットのレーザスポット座標を計算する前記ステップは、
各前記レーザ方向の単位ベクトルをそれぞれＲａ（ｒｘ１，ｒｙ１，ｒｚ１）、Ｒｂ（ｒｘ２，ｒｙ２，ｒｚ２）、Ｒｃ（ｒｘ３，ｒｙ３，ｒｚ３）とし、各前記レーザ距離センサが位置するツールヘッド座標系の座標をＰａ（ｘ１，ｙ１，ｚ１）、Ｐｂ（ｘ２，ｙ２，ｚ２）、Ｐｃ（ｘ３，ｙ３，ｚ３）とし、各前記距離値をｄ１、ｄ２、ｄ３とし、各前記レーザスポットの座標をＵａ、Ｕｂ、Ｕｃとすると、
Ｕａ．ｘ＝ｘ１＋ｒｘ１＊ｄ１、Ｕａ．ｙ＝ｙ１＋ｒｙ１＊ｄ１、Ｕａ．ｚ＝ｚ１＋ｒｚ１＊ｄ１、
Ｕｂ．ｘ＝ｘ２＋ｒｘ２＊ｄ２、Ｕｂ．ｙ＝ｙ２＋ｒｙ２＊ｄ２、Ｕｂ．ｚ＝ｚ２＋ｒｚ２＊ｄ２、
Ｕｃ．ｘ＝ｘ３＋ｒｘ３＊ｄ３、Ｕｃ．ｙ＝ｙ３＋ｒｙ３＊ｄ３、Ｕｃ．ｚ＝ｚ３＋ｒｚ３＊ｄ３であり、
すなわち、各前記レーザスポットの座標を求めることを含み、ここで、Ｐａ、Ｐｂ、Ｐｃは、それぞれ各前記レーザ距離センサの座標Ｕａ（Ｕａ．ｘ，Ｕａ．ｙ，Ｕａ．ｚ）、Ｕｂ（Ｕｂ．ｘ，Ｕｂ．ｙ，Ｕｂ．ｚ）、Ｕｃ（Ｕｃ．ｘ，Ｕｃ．ｙ，Ｕｃ．ｚ）である。
各前記レーザ方向の単位ベクトルをそれぞれＲａ（ｒｘ１，ｒｙ１，ｒｚ１）、Ｒｂ（ｒｘ２，ｒｙ２，ｒｚ２）、Ｒｃ（ｒｘ３，ｒｙ３，ｒｚ３）とし、各前記レーザ距離センサが位置するツールヘッド座標系の座標をＰａ（ｘ１，ｙ１，ｚ１）、Ｐｂ（ｘ２，ｙ２，ｚ２）、Ｐｃ（ｘ３，ｙ３，ｚ３）とし、各前記距離値をｄ１、ｄ２、ｄ３とし、各前記レーザスポットの座標をＵａ、Ｕｂ、Ｕｃとすると、
Ｕａ．ｘ＝ｘ１＋ｒｘ１＊ｄ１、Ｕａ．ｙ＝ｙ１＋ｒｙ１＊ｄ１、Ｕａ．ｚ＝ｚ１＋ｒｚ１＊ｄ１、
Ｕｂ．ｘ＝ｘ２＋ｒｘ２＊ｄ２、Ｕｂ．ｙ＝ｙ２＋ｒｙ２＊ｄ２、Ｕｂ．ｚ＝ｚ２＋ｒｚ２＊ｄ２、
Ｕｃ．ｘ＝ｘ３＋ｒｘ３＊ｄ３、Ｕｃ．ｙ＝ｙ３＋ｒｙ３＊ｄ３、Ｕｃ．ｚ＝ｚ３＋ｒｚ３＊ｄ３であり、
すなわち、各前記レーザスポットの座標を求めることを含み、ここで、Ｐａ、Ｐｂ、Ｐｃは、それぞれ各前記レーザ距離センサの座標Ｕａ（Ｕａ．ｘ，Ｕａ．ｙ，Ｕａ．ｚ）、Ｕｂ（Ｕｂ．ｘ，Ｕｂ．ｙ，Ｕｂ．ｚ）、Ｕｃ（Ｕｃ．ｘ，Ｕｃ．ｙ，Ｕｃ．ｚ）である。
【0039】
さらに、予め設定される位置姿勢式、前記距離値及び前記統合平面法線ベクトルに基づき、前記ロボットのツールヘッドの調整すべき位置姿勢の位置姿勢パラメータを計算する前記ステップは、
求められた前記統合平面法線ベクトルをＶ（ｖｘ，ｖｙ，ｖｚ）とし、ｖｚ＞０とし、ここで、ｖｚ＜０の場合、前記統合平面法線ベクトルに－１を乗算することと、
式Ｍ（α，β，γ）＊［０，０，１］Ｔ＝Ｖｎｏｒｍ Ｔ。を予め設定することであって、ここで、Ｍ（α，β，γ）は、オイラー角回転式のうちの回転行列であることと、
前記予め設定される式、前記距離値及びオイラー角回転式に基づき、前記位置姿勢パラメータを求めることであって、ここで、Ｖｎｏｒｍは、モジュラスが１である前記統合平面法線ベクトルを表し、α、β、γは、オイラー角をそれぞれ表すことと、を含む。
求められた前記統合平面法線ベクトルをＶ（ｖｘ，ｖｙ，ｖｚ）とし、ｖｚ＞０とし、ここで、ｖｚ＜０の場合、前記統合平面法線ベクトルに－１を乗算することと、
式Ｍ（α，β，γ）＊［０，０，１］Ｔ＝Ｖｎｏｒｍ Ｔ。を予め設定することであって、ここで、Ｍ（α，β，γ）は、オイラー角回転式のうちの回転行列であることと、
前記予め設定される式、前記距離値及びオイラー角回転式に基づき、前記位置姿勢パラメータを求めることであって、ここで、Ｖｎｏｒｍは、モジュラスが１である前記統合平面法線ベクトルを表し、α、β、γは、オイラー角をそれぞれ表すことと、を含む。
【0040】
さらに、前記予め設定される式、前記距離値及びオイラー角回転式に基づき、前記位置姿勢パラメータを求める前記ステップは、
ｒｘ＝０とし、前記予め設定される式及び前記オイラー角回転式連立方程式に基づいてｒｙ、ｒｚを求めることであって、ここで、ｒｘ、ｒｙ、ｒｚは、それぞれオイラー角α、β、γであることと、
前記距離値に基づいて予め設定されるアルゴリズムを利用して統合距離値を計算し、ｄａｖｅｒとすることと、
前記ツールヘッド座標系の原点をｚ方向にｄａｖｅｒを加算し、前記原点が前記検出すべき部位に設置されるようにすることと、
前記原点が前記検出すべき部位に設置される時、マニピュレータベースのベース座標系に対する前記原点の位置姿勢パラメータを取得し、ｘ０、ｙ０、ｚ０、ｒｘ０、ｒｙ０、ｒｚ０とすることと、
前記ツールヘッドの調整すべき位置姿勢のオイラー角パラメータをＲｘ、Ｒｙ、Ｒｚとすると、
Ｒｘ＝ｒｘ０、Ｒｙ＝ｒｙ０＋ｒｙ、Ｒｚ＝ｒｚ０＋ｒｚであり、すると、
前記ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータは、ｘ０、ｙ０、ｚ０、Ｒｘ、Ｒｙ、Ｒｚであることと、を含む。
ｒｘ＝０とし、前記予め設定される式及び前記オイラー角回転式連立方程式に基づいてｒｙ、ｒｚを求めることであって、ここで、ｒｘ、ｒｙ、ｒｚは、それぞれオイラー角α、β、γであることと、
前記距離値に基づいて予め設定されるアルゴリズムを利用して統合距離値を計算し、ｄａｖｅｒとすることと、
前記ツールヘッド座標系の原点をｚ方向にｄａｖｅｒを加算し、前記原点が前記検出すべき部位に設置されるようにすることと、
前記原点が前記検出すべき部位に設置される時、マニピュレータベースのベース座標系に対する前記原点の位置姿勢パラメータを取得し、ｘ０、ｙ０、ｚ０、ｒｘ０、ｒｙ０、ｒｚ０とすることと、
前記ツールヘッドの調整すべき位置姿勢のオイラー角パラメータをＲｘ、Ｒｙ、Ｒｚとすると、
Ｒｘ＝ｒｘ０、Ｒｙ＝ｒｙ０＋ｒｙ、Ｒｚ＝ｒｚ０＋ｒｚであり、すると、
前記ツールヘッドの調整すべき位置姿勢の位置姿勢パラメータは、ｘ０、ｙ０、ｚ０、Ｒｘ、Ｒｙ、Ｒｚであることと、を含む。
【0041】
さらに、前記予め設定されるアルゴリズムは、平均値アルゴリズム及び重み付け平均アルゴリズムを含む。
【0042】
なお、本明細書では、「含む」、「包含」という用語又はその他の任意の変形は、非排他的な「含む」を意図的にカバーするものであり、それにより、一連の要素を含むプロセス、方法、物品又はシステムは、それらの要素を含むだけではなく、明確にリストアップされていない他の要素も含み、又はこのようなプロセス、方法、物品又はシステムに固有の要素も含む。それ以上の制限がない場合に、「……を１つ含む」という文章で限定された要素について、この要素を含むプロセス、方法、物品又はシステムには他の同じ要素も存在することが排除されるものではない。
【0043】
上記本発明の実施例の番号は、単に説明のためのものであり、実施例の優劣を表すものではない。
【0044】
以上の実施の形態の記述によって、当業者であればはっきりと分かるように、上記実施例の方法は、ソフトウェアと必要な汎用ハードウェアプラットフォームの形態によって実現されてもよい。無論、ハードウェアによって実現されてもよいが、多くの場合、前者は、好適な実施の形態である。このような理解を踏まえて、本発明の技術案は、実質的には又は従来の技術に寄与した部分がソフトウェア製品の形成によって具現化されてもよい。このコンピュータソフトウェア製品は、以上に記載の一つの記憶媒体（例えばＲＯＭ／ＲＡＭ、磁気ディスク、光ディスク）に記憶され、一台の端末機器（携帯電話、コンピュータ、サーバ、エアコン、又はネットワーク機器などであってもよい）に本発明の各実施例に記載の方法を実行させるための若干の指令を含む。
【0045】
以上は、本発明の好ましい実施例に過ぎず、本発明の特許範囲を限定するものではなく、本発明の明細書及び添付図面の内容を利用して行われる等価構造又は等価フロー変換、又は直接又は間接的に他の関連する技術分野に適用されることは、いずれも同様に本発明の特許の保護範囲内に含まれるものとする。
【図1】
【図2】
【図3】