特許 7473679 ロボットに対する重力方向を推定する方法、ロボットシステム、および非一時的なコンピュータ可読媒体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-04-15

(45)【発行日】2024-04-23

(54)【発明の名称】ロボットに対する重力方向を推定する方法、ロボットシステム、および非一時的なコンピュータ可読媒体

(51)【国際特許分類】

   B25J 13/08 20060101AFI20240416BHJP

【ＦＩ】

B25J13/08 Z

【請求項の数】 18

(21)【出願番号】P 2022562961

(86)(22)【出願日】2021-04-14

(65)【公表番号】

(43)【公表日】2023-05-24

(86)【国際出願番号】 CN2021087158

(87)【国際公開番号】W WO2021208948

(87)【国際公開日】2021-10-21

【審査請求日】2022-10-14

(31)【優先権主張番号】16/849,536

(32)【優先日】2020-04-15

(33)【優先権主張国・地域又は機関】US

(73)【特許権者】

【識別番号】522403608

【氏名又は名称】上海非夕機器人科技有限公司

【氏名又は名称原語表記】ＳＨＡＮＧＨＡＩ ＦＬＥＸＩＶ ＲＯＢＯＴＩＣＳ ＴＥＣＨＮＯＬＯＧＹ ＣＯ．，ＬＴＤ．

【住所又は居所原語表記】Ｒｏｏｍ １０９６，Ｂｕｉｌｄｉｎｇ Ｃ，Ｎｏ．５５５，Ｄｏｎｇｃｈｕａｎ Ｒｏａｄ，Ｍｉｎｈａｎｇ Ｄｉｓｔｒｉｃｔ Ｓｈａｎｇｈａｉ ２００２４１ Ｃｈｉｎａ

(73)【特許権者】

【識別番号】521006211

【氏名又は名称】フレキシブ リミテッド

【氏名又は名称原語表記】ＦＬＥＸＩＶ ＬＴＤ．

(74)【代理人】

【識別番号】100121728

【弁理士】

【氏名又は名称】井関 勝守

(74)【代理人】

【識別番号】100165803

【弁理士】

【氏名又は名称】金子 修平

(74)【代理人】

【識別番号】100170900

【弁理士】

【氏名又は名称】大西 渉

(72)【発明者】

【氏名】チゥン シゥーイン

(72)【発明者】

【氏名】フゥー シィンチィー

(72)【発明者】

【氏名】チァォ ケネス

【審査官】杉山 悟史

(56)【参考文献】

【文献】国際公開第２０１８／１１３９６６（ＷＯ，Ａ１）

【文献】中国特許出願公開第１０７２３８４６８（ＣＮ，Ａ）

【文献】中国特許出願公開第１０９４８３５５５（ＣＮ，Ａ）

【文献】中国特許出願公開第１０９５７１５４９（ＣＮ，Ａ）

【文献】中国特許出願公開第１０９１２９５２５（ＣＮ，Ａ）

【文献】特開２０１９－１８８５００（ＪＰ，Ａ）

【文献】英国特許出願公開第２４８１２４９（ＧＢ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ １／００ ～ ２１／０２

(57)【特許請求の範囲】

【請求項1】

ロボットに対する重力方向を推定する方法であって、前記ロボットは、複数の関節と、複数の関節によって順次接続された複数のリンクとを含み、前記複数の関節は、第１の関節および第２の関節を含み、前記第１の関節が前記第２の関節よりもロボットの近位端に近く、
前記方法は、
前記第１の関節を回転させ、回転中の前記第１の関節の第１のトルク情報を記録するステップと、
前記第２の関節を回転させ、回転中の前記第２の関節の第２のトルク情報を記録するステップと、
前記第１のトルク情報および前記第２のトルク情報に基づいて、前記ロボットに対する重力方向を推定するステップと、を含み、
前記第１のトルク情報は、重力によって前記第１の関節にかかるトルクの正弦波部分の第１の振幅および第１の位相シフトを含み、
前記第２のトルク情報は、重力によって前記第２の関節にかかるトルクの正弦波部分の第２の振幅および第２の位相シフトを含む
ことを特徴とするロボットに対する重力方向を推定する方法。

【請求項2】

前記第１のトルク情報および前記第２のトルク情報に基づいて、前記ロボットに対する重力方向を推定することは、
前記第１の位相シフトに基づいて第１の基準角度を決定することであって、前記第１の基準角度は、重力によって引き起こされる第１の関節にかかるトルクがゼロである前記第１の関節の特定の位置における前記第１の関節の回転角に対応することと、
前記第１のトルク情報および前記第２のトルク情報に基づいて第２の基準角度を決定することであって、前記第２の基準角度は前記重力方向と前記第１の関節の回転軸との間の角度であり、前記第２の基準角度は前記第１の振幅と前記第２の振幅の比率から導出可能であることと、
前記第１の基準角度および前記第２の基準角度に基づいて、前記第１の関節の回転軸に対する重力方向を推定することと、を含む
ことを特徴とする請求項１に記載の方法。

【請求項3】

前記第２の関節を回転させる前に、前記第１の関節を特定の位置に回転させることをさらに含む
ことを特徴とする請求項２に記載の方法。

【請求項4】

前記第１の関節を回転させる前に、前記第２の関節の後に接続された複数のリンクを整列させて、前記第１の関節と前記第２の関節との間の複数のリンクのうちの１つに対して実質的に同一線上にかつ実質的に垂直に延長するようにすることをさらに含む
ことを特徴とする請求項３に記載の方法。

【請求項5】

前記第１の位相シフトに基づいて前記第１の基準角度を決定することは、以下の式に従って前記第１の基準角度を決定することを含み、

ここで、τ_１は、前記第１の関節の回転中に前記第１の関節にかかるトルクの正弦波部分であり、重力によって前記第１の関節にかかるトルクに対応し、
Ａ_１は前記第１の振幅であり、
ｑ_１は前記第１の関節の回転中の前記第１の関節の位置であり、
φ_１は前記第１の位相シフトであり、
θ_１は前記第１の基準角度である
ことを特徴とする請求項４に記載の方法。

【請求項6】

前記第１のトルク情報および前記第２のトルク情報に基づいて前記第２の基準角度を決定することは、以下の式に基づいて前記第２の基準角度を決定することを含み、

ここで、τ_２は、前記第２の関節の回転中に前記第２の関節にかかるトルクの正弦波部分であり、重力によって前記第２の関節にかかるトルクに対応し、
Ａ_２は前記第２の振幅であり、
ｑ_２は前記第２の関節の回転中の前記第２の関節の位置であり、
φ_２は前記第２の位相シフトであり、
θ_２は前記第２の基準角度である
ことを特徴とする請求項５に記載の方法。

【請求項7】

前記ロボットは、複数の関節のうちの遠位関節に接続されたエンドエフェクタをさらに含み、
前記第１のトルク情報および前記第２のトルク情報に基づいて前記第２の基準角度を決定することは、以下の式に基づいて前記第２の基準角度を決定することを含み、

ここで、τ_２は、前記第２の関節の回転中に前記第２の関節にかかるトルクの正弦波部分であり、重力によって前記第２の関節にかかるトルクに対応し、
Ａ_２は前記第２の振幅であり、
ｑ_２は前記第２の関節の回転中の前記第２の関節の位置であり、
φ_２は前記第２の位相シフトであり、
θ_２は前記第２の基準角度である
ことを特徴とする請求項５に記載の方法。

【請求項8】

前記第１の関節は前記ロボットの近位端に位置し、前記第２の関節は前記複数のリンクのうちの１つを介して前記第１の関節に接続された隣接する関節である
ことを特徴とする請求項１に記載の方法。

【請求項9】

前記第１の関節を回転させることは、実質的に一定の速度で第１の所定の範囲をカバーするように前記第１の関節を前後に回転させることを含み、
前記第２の関節を回転させることは、実質的に一定の速度で第２の所定の範囲をカバーするように前記第２の関節を前後に回転させることを含む
ことを特徴とする請求項１に記載の方法。

【請求項10】

メモリ、プロセッサ、複数の関節、および前記複数の関節によって順次接続された複数のリンクを含むロボットシステムであって、前記複数の関節は、第１の関節および第２の関節を含み、前記第１の関節は、前記第２の関節よりも前記ロボットの近位端に近く、前記メモリは、前記プロセッサによって実行されると、前記ロボットシステムに対する重力方向を推定する方法を前記プロセッサに実行させる命令を格納するように構成され、前記方法は、
前記第１の関節を回転させ、回転中の前記第１の関節の第１のトルク情報を記録するステップと、
前記第２の関節を回転させ、回転中の前記第２の関節の第２のトルク情報を記録するステップと、
前記第１のトルク情報および前記第２のトルク情報に基づいて、前記ロボットに対する重力方向を推定するステップと、を含み、
前記第１のトルク情報は、重力によって前記第１の関節にかかるトルクの正弦波部分の第１の振幅および第１の位相シフトを含み、
前記第２のトルク情報は、重力によって前記第２の関節にかかるトルクの正弦波部分の第２の振幅および第２の位相シフトを含む
ことを特徴とするロボットシステム。

【請求項11】

前記第１のトルク情報および前記第２のトルク情報に基づいて、前記ロボットに対する重力方向を推定することは、
前記第１の位相シフトに基づいて第１の基準角度を決定することであって、前記第１の基準角度は、重力によって引き起こされる第１の関節にかかるトルクがゼロである前記第１の関節の特定の位置における前記第１の関節の回転角に対応することと、
前記第１のトルク情報および前記第２のトルク情報に基づいて第２の基準角度を決定することであって、前記第２の基準角度は前記重力方向と前記第１の関節の回転軸との間の角度であり、前記第２の基準角度は前記第１の振幅と前記第２の振幅の比率から導出可能であることと、
前記第１の基準角度および前記第２の基準角度に基づいて、前記第１の関節の回転軸に対する重力方向を推定することと、を含む
ことを特徴とする請求項１０に記載のロボットシステム。

【請求項12】

前記方法は、
前記第２の関節を回転させる前に、前記第１の関節を特定の位置に回転させることをさらに含む
ことを特徴とする請求項１１に記載のロボットシステム。

【請求項13】

前記方法は、
前記第１の関節を回転させる前に、前記第２の関節の後に接続された複数のリンクを整列させて、前記第１の関節と前記第２の関節との間の複数のリンクのうちの１つに対して実質的に同一線上にかつ実質的に垂直に延長するようにすることをさらに含む
ことを特徴とする請求項１２に記載のロボットシステム。

【請求項14】

前記第１の位相シフトに基づいて前記第１の基準角度を決定することは、以下の式に従って前記第１の基準角度を決定することを含み、

ここで、τ_１は、前記第１の関節の回転中に前記第１の関節にかかるトルクの正弦波部分であり、重力によって前記第１の関節にかかるトルクに対応し、
Ａ_１は前記第１の振幅であり、
ｑ_１は前記第１の関節の回転中の前記第１の関節の位置であり、
φ_１は前記第１の位相シフトであり、
θ_１は前記第１の基準角度である
ことを特徴とする請求項１３に記載のロボットシステム。

【請求項15】

前記第１のトルク情報および前記第２のトルク情報に基づいて前記第２の基準角度を決定することは、以下の式に基づいて前記第２の基準角度を決定することを含み、

ここで、τ_２は、前記第２の関節の回転中に前記第２の関節にかかるトルクの正弦波部分であり、重力によって前記第２の関節にかかるトルクに対応し、
Ａ_２は前記第２の振幅であり、
ｑ_２は前記第２の関節の回転中の前記第２の関節の位置であり、
φ_２は前記第２の位相シフトであり、
θ_２は前記第２の基準角度である
ことを特徴とする請求項１４に記載のロボットシステム。

【請求項16】

前記ロボットは、複数の関節のうちの遠位関節に接続されたエンドエフェクタをさらに含み、
前記第１のトルク情報および前記第２のトルク情報に基づいて前記第２の基準角度を決定することは、以下の式に基づいて前記第２の基準角度を決定することを含み、

ここで、τ_２は、前記第２の関節の回転中に前記第２の関節にかかるトルクの正弦波部分であり、重力によって前記第２の関節にかかるトルクに対応し、
Ａ_２は前記第２の振幅であり、
ｑ_２は前記第２の関節の回転中の前記第２の関節の位置であり、
φ_２は前記第２の位相シフトであり、
θ_２は前記第２の基準角度である
ことを特徴とする請求項１４に記載のロボットシステム。

【請求項17】

ロボットシステムのプロセッサによって実行されると、前記ロボットシステムに対する重力方向を推定する方法を前記プロセッサに実行させる命令を格納する非一時的なコンピュータ可読媒体であって、
前記ロボットシステムは、複数の関節と、前記複数の関節によって順次接続された複数のリンクとを含み、前記複数の関節は、第１の関節および第２の関節を含み、前記第１の関節が前記第２の関節よりもロボットの近位端に近く、前記方法は、
前記第１の関節を回転させ、回転中の前記第１の関節の第１のトルク情報を記録するステップと、
前記第２の関節を回転させ、回転中の前記第２の関節の第２のトルク情報を記録するステップと、
前記第１のトルク情報および前記第２のトルク情報に基づいて、前記ロボットに対する重力方向を推定するステップと、を含み、
前記第１のトルク情報は、重力によって前記第１の関節にかかるトルクの正弦波部分の第１の振幅および第１の位相シフトを含み、
前記第２のトルク情報は、重力によって前記第２の関節にかかるトルクの正弦波部分の第２の振幅および第２の位相シフトを含む
ことを特徴とする非一時的なコンピュータ可読媒体。

【請求項18】

前記第１のトルク情報および前記第２のトルク情報に基づいて、前記ロボットに対する重力方向を推定することは、
前記第１の位相シフトに基づいて第１の基準角度を決定することであって、前記第１の基準角度は、重力によって引き起こされる第１の関節にかかるトルクがゼロである前記第１の関節の特定の位置における前記第１の関節の回転角に対応することと、
前記第１のトルク情報および前記第２のトルク情報に基づいて第２の基準角度を決定することであって、前記第２の基準角度は前記重力方向と前記第１の関節の回転軸との間の角度であり、前記第２の基準角度は前記第１の振幅と前記第２の振幅の比率から導出可能であることと、
前記第１の基準角度および前記第２の基準角度に基づいて、前記第１の関節の回転軸に対する重力方向を推定することと、を含む
ことを特徴とする請求項１７に記載の非一時的なコンピュータ可読媒体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、一般に、ロボット技術に関し、特に、ロボットに対する重力方向を推定する方法、ロボットシステム、および非一時的なコンピュータ可読媒体に関する。

【背景技術】

【0002】

ロボットに関する重力方向の決定は、ロボットの正確な運動制御または力制御にとって非常に重要である。ロボットが取得した推定重力方向が実際の重力方向とずれている場合、動的モデルに基づいてロボットによって決定された推定重力／トルクは不正確である可能性があり、ロボットの運動制御のエラーにつながる可能性がある。

【0003】

ロボットに関する重力方向を決定するための従来の方法としては、傾斜計等の外部センサからの直接測定を用いたり、ロボットの動的モデルを利用したり、測定値と動的モデルから得られる値との関節トルク差を用いて、ロボットに対する重力の方向を推定する方法がある。

【発明の概要】

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

【0004】

本発明は、ロボットに対する重力方向を推定する方法、ロボットシステム、および非一時的なコンピュータ可読媒体を提供する。本発明によって提供される方法は、ロボットの動的モデルに依存せず、関節トルク測定における線形ゲインおよびオフセットの不正確さを許容することができる。

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

【0005】

本発明の一態様では、ロボットに対する重力方向を推定する方法を提供する。前記ロボットは、順次接続された複数の関節とリンクを含む。前記関節は、第１の関節と第２の関節を含む。前記第１の関節は、前記第２の関節よりも前記ロボットの近位端に近い。前記方法は、前記第１の関節を回転させ、回転中に前記第１の関節の第１のトルク情報を記録するステップと、前記第２の関節を回転させ、回転中に前記第２の関節の第２のトルク情報を記録するステップと、前記第１のトルク情報および前記第２のトルク情報に基づいて前記ロボットに対する前記重力方向を推定するステップと、を含む。

【0006】

本発明の別の態様では、ロボットシステムを提供する。前記ロボットシステムは、メモリ、プロセッサ、複数の関節とリンクを含む。前記関節とリンクが順次接続される。前記関節は、第１の関節と第２の関節を含む。前記第１の関節は、前記第２の関節よりもロボットの近位端に近い。前記メモリは、プロセッサによって実行されると、ロボットシステムに対する重力方向を推定する方法をプロセッサに実行させる命令を格納するように構成される。前記方法は、前記第１の関節を回転させ、回転中に前記第１の関節の第１のトルク情報を記録するステップと、前記第２の関節を回転させ、回転中に前記第２の関節の第２トルク情報を記録するステップと、前記第１のトルク情報および前記第２のトルク情報に基づいて、前記ロボットに対する重力方向を推定するステップと、を含む。

【0007】

本発明のさらに別の態様では、ロボットシステムのプロセッサによって実行されると、ロボットシステムに対する重力方向を推定する方法をプロセッサに実行させる命令を格納する非一時的なコンピュータ可読媒体を提供する。前記ロボットシステムは、メモリ、プロセッサ、複数の関節とリンクを含む。前記関節とリンクが順次接続される。前記関節は、第１の関節と第２の関節を含む。前記第１の関節は、前記第２の関節よりもロボットの近位端に近い。前記方法は、前記第１の関節を回転させ、回転中に前記第１の関節の第１のトルク情報を記録するステップと、前記第２の関節を回転させ、回転中に前記第２の関節の第２のトルク情報を記録するステップと、前記第１のトルク情報および前記第２のトルク情報に基づいて前記ロボットに対する前記重力方向を推定するステップと、を含む。

【図面の簡単な説明】

【0008】

本発明の実施形態における技術的解決手段を明確に説明するために、前記実施形態の説明に使用される図面を以下に簡単に説明する。明らかに、以下の説明における図面は、本発明のいくつかの実施形態にすぎない。当業者であれば、創造的な作業を行うことなく、これらの図面に基づいて他の図面を取得することもできる。

【0009】

【図1】図１は、本発明の一実施形態にかかるロボットシステムの構造を示す図である。

【図2】図２は、本発明の一実施形態にかかるロボットに対する重力方向を推定する方法のフローチャートを示す図である。

【図3】図３は、本発明の一実施形態にかかるロボットシステムの第１の簡略化モデルを示す図である。

【図4】図４は、本発明の一実施形態にかかるロボットシステムの一部を示す側面図である。

【図5】図５は、本発明の一実施形態にかかる１つの例示的なポーズを示すロボットシステムの正面図である。

【図6a】図６ａは、関節の回転中に記録された例示的な関節トルク情報を示す図である。

【図6b】図６ｂは、同じ関節の逆方向への回転中に記録された別の例示的な関節トルク情報を示す図である。

【図7a】図７ａは、本発明の一実施形態にかかる第１の関節の回転中の第１の関節の第１のトルク情報を示す図である。

【図7b】図７ｂは、本発明の一実施形態にかかる第２の関節の回転中の第２の関節の第２のトルク情報を示す図である。

【図8】図８は、本発明の一実施形態にかかるロボットシステムの第２の簡略化モデルを示す図である。

【図9】図９は、点質量が第２の簡略化モデルに基づく平面ＺＯＡに位置する状況を示す図である。

【図10】図１０は、図１のロボットシステムの１つの例示的なポーズを示す図である。

【図11】図１１は、本発明の一実施形態にかかるロボットシステムの第３の簡略化モデルを示す図である。

【発明を実施するための形態】

【0010】

以下、添付の図面および実施例を参照して、本発明を詳細に説明する。明らかに、記載された実施形態は、本発明の実施形態の一部にすぎず、実施形態のすべてではない。当業者が本発明の実施形態に基づいて創造的な努力なしに得た他のすべての実施形態は、本発明の保護範囲内にあるものとする。

【0011】

図１は、本発明の一実施形態にかかるロボットシステム、例えば産業用ロボットの構造を示す図である。ロボットシステム１００は、複数のリンク（符号なし）によって順次接続された複数の関節Ｊ１～Ｊ７を含む。いくつかの実施形態では、ロボットシステム１００は、対象物体の把持または機械加工などの特定のタスクを達成するためのエンドエフェクタ１１０をさらに含んでいる。なお、ロボットシステム１００の関節の数量は、本発明では限定されない。例えば、他の実施形態では、ロボットシステム１００は、ｎ個の関節を含むｎ軸ロボットであってもよく、ｎは２以上の整数である。

【0012】

ロボットシステム１００は、メモリおよびプロセッサ（図示せず）をさらに含んでいる。メモリは、プログラム命令を格納するように構成されている。プロセッサは、ロボットシステム１００の動きを制御するためにプログラム命令を実行するように構成されている。当業者は、メモリおよびプロセッサが関節Ｊ１～Ｊ７またはリンク内に配置されてもよいこと、またはそれらがロボットシステム１００の制御ボックス内に配置されてもよいことを理解すべきである。また、いくつかの実施形態では、メモリおよびプロセッサは、外部制御装置に配置され、ネットワークを介してロボットシステム１００に伝達されてもよい。

【0013】

プロセッサは、プログラム命令を実行して、１つまたは複数の関節Ｊ１～Ｊ７の回転軸に対する重力方向などのロボットシステム１００に対する重力方向を推定する方法を実行することができる。該方法は、軸が平行でない２つの回転関節（例えば、図１に示される関節Ｊ１およびＪ２）のトルク情報を使用することを含む。ロボットシステム１００の他の関節（例えば、Ｊ３～Ｊ７）は、本発明では限定されない回転アクチュエータまたは線形アクチュエータとすることができることに留意されたい。具体的には、該方法は、図２に示すように、ブロックＳ２０１からＳ２０３に記載の動作を含む。

【0014】

Ｓ２０１：第１の関節を回転させ、回転中に第１の関節の第１のトルク情報を記録する。

【0015】

任意の対象関節の重力方向と回転軸との関係を特定したい場合、この対象関節を第１の関節として使用できる。例えば、第１の関節は、図１に示すようにロボットシステム１００の近位端に位置する関節Ｊ１であってもよい。いくつかの実施形態では、第２の関節は、それらの間のリンクを介して第１の関節Ｊ１に接続された隣接する関節Ｊ２であってもよい。

【0016】

Ｓ２０１では、ロボットシステム１００の第１の関節Ｊ１以外のすべての関節をロックし、第１の関節Ｊ１のアクチュエータ（例えば駆動モータ）により第１の関節Ｊ１を回転させる。回転中の第１の関節Ｊ１のトルクを測定または推定し、第１のトルク情報として記録する。例えば、第１の関節Ｊ１のトルクは、第１の関節Ｊ１のトルクセンサにより測定してもよいし、第１の関節Ｊ１の駆動モータの駆動電流等の他のパラメータを用いて推定してもよい。

【0017】

Ｓ２０２：第２の関節を回転させ、回転中に第２の関節の第２のトルク情報を記録する。

【0018】

第２の関節は、回転軸が第１の関節Ｊ１と平行でない関節であってもよい。例えば、第２の関節は、第１の関節Ｊ１に隣接する関節Ｊ２であってもよい。

【0019】

同様に、Ｓ２０２では、ロボットシステム１００の第２の関節Ｊ２以外のすべての関節をロックし、第２の関節Ｊ２のアクチュエータ（例えば駆動モータ）により第２の関節Ｊ２を回転させる。回転中の第２の関節Ｊ２のトルクを測定または推定し、第２のトルク情報として記録する。例えば、第２の関節Ｊ２のトルクは、第２の関節Ｊ２のトルクセンサにより測定してもよいし、第２の関節Ｊ２の駆動モータの駆動電流等の他のパラメータを用いて推定してもよい。

【0020】

図６ａおよび図６ｂは、関節の異なる方向への回転中に記録された例示的な関節トルク情報を示す図である。図６ａは、関節が一方向に回転したときに記録されたトルク曲線を示す。図６ｂは、関節が逆方向に回転したときに記録されたトルク曲線を表す。関節トルク情報は、関節トルクと関節の位置（または回転角）との関係を示すことができる。図６に示すように、トルク曲線は、正弦波部分と共通モード部分とを含んでいる。これは、関節軸が重力方向と一致していない場合である。トルク曲線の正弦波部分のみが重力によって関節にかかるトルクを表しているため、この部分を抽出して（たとえば、曲線当てはめによって）、重力方向を決定するためのさらなる計算に使用できる。

【0021】

いくつかの実施形態では、Ｓ２０１では、第１の関節Ｊ１を前後に回転させて、実質的に一定の速度で第１の所定の範囲をカバーすることができる。この実施形態では、フォワードパス及びバックワードパスのトルク記録の共通モード（図６ａおよび６ｂに示すように）を計算することができ、したがって、第１の関節Ｊ１のトルク記録の正弦波部分を取得することができる。第１の所定の範囲は、第１の所定の範囲内で取得されたトルク記録が、トルク曲線の正弦波部分の特徴（例えば、正弦波振幅および位相シフト）を説明するための十分な情報を含む限り、実際の設計要件に基づいて決定され得る。例えば、第１の所定の範囲は、９０°、１２０°、１５０°、１８０°などであってもよい。第１の関節を一定速度で前後に回転させることにより、第１の関節Ｊ１のトルク曲線の正弦波部分に寄与する可能性のある重力以外の要因（摩擦、偏心等）の影響を排除することができる。

【0022】

同様に、いくつかの実施形態では、Ｓ２０２では、第２の関節Ｊ２を前後に回転させて、実質的に一定の速度で第２の所定の範囲をカバーすることができる。この実施形態では、フォワードパス及びバックワードパスのトルク記録の共通モードを計算することができ、したがって、第２の関節Ｊ２のトルク記録の正弦波部分を取得することができる。第２の所定の範囲は、第２の所定の範囲で取得されたトルク記録が、トルク曲線の正弦波部分の特徴（例えば、正弦波振幅および位相シフト）を説明するのに十分な情報を含む限り、実際の設計要件に基づいて決定され得る。例えば、第２の所定の範囲は、９０°、１２０°、１５０°、１８０°などであってもよい。第２の関節を前後に回転させることにより、第２の関節Ｊ２のトルク曲線の正弦波部分にも寄与する可能性のある重力以外の要因の影響（例えば、摩擦、偏心）を排除することができる。

【0023】

第１のトルク情報と第２のトルク情報の一例を図７ａおよび図７ｂに示す。図示のように、第１のトルク情報は、Ｓ２０１で取得されたトルク記録（すなわち、第１の関節Ｊ１の回転中に重力によって第１の関節Ｊ１にかかるトルク）の正弦波部分の第１の振幅Ａ１および第１の位相シフトφ１またはφ１’を含んでいる。第２のトルク情報は、Ｓ２０２で取得されたトルク記録（すなわち、第２の関節の回転中に重力によって第２の関節Ｊ２にかかるトルク）の正弦波部分の第２の振幅Ａ２および第２の位相シフトφ２またはφ２’を含んでいる。

【0024】

Ｓ２０３：第１のトルク情報および第２のトルク情報に基づいて、ロボットに対する重力方向を推定する。

【0025】

第１の関節Ｊ１の第１のトルク情報及び第２の関節Ｊ２の第２のトルク情報を取得した後、ロボットシステム１００は、そのような情報に基づいてロボットシステム１００に対する重力方向を推定することができる。一般に、本発明ではθ１およびθ２として示されるロボットシステムに対する重力方向を説明するために、２つの基準角度（角度オフセット）が必要である。

【0026】

具体的には、第１のステップにおいて、第１の基準角度θ１は、Ｓ２０１で取得された第１の位相シフトφ１またはφ１’に基づいて決定され得る。第１の基準角度θ１は、重力によって第１の関節Ｊ１にかかるトルクが実質的にゼロとなる第１の関節Ｊ１の特定の位置に対応する。第２のステップにおいて、Ｓ２０１で取得された第１のトルク情報とＳ２０２で取得された第２のトルク情報とに基づいて、第２の基準角度θ２が決定され得る。第２の基準角度θ２は、重力方向と第１の関節Ｊ１の回転軸との間の角度である。最後に、第１基準角度θ１および第２基準角度θ２に基づいて、第１の関節Ｊ１の回転軸に対する重力方向を推定することができる。これらの２つの角度オフセットθ１およびθ２が導出されると、ロボットシステムの座標系に対する重力方向が決定される。この関係は様々な表現が可能であり、本発明では限定されない。

【0027】

図３は本発明の一実施形態にかかるロボットシステムの第１の簡略化モデルを示す図である。ＸＹＺ軸はデカルト空間のワールド座標系で、重力は－Ｚ方向である。簡単にするために、すべてのロボットの質量を点質量ｍにまとめることができると仮定し、そのため、Ｊ１とＪ２を含むすべての関節とリンクは無質量である。

【0028】

Ｊ１とＪ２の回転軸間の角度はαで表され、ロボットの運動学から知られているように、通常、シリアルチェーンキネマティックマニピュレーターでは９０°である。図４は、Ｊ１とＪ２の回転軸が実質的に垂直であるロボットシステムの一部を示す側面図である。γは、Ｊ１の回転軸と、Ｊ２と点質量ｍを接続する線との間の角度であり、第２の関節Ｊ２を回転させることによって変更できる。

【0029】

図５に示すように、いくつかの実施形態では、第２の関節Ｊ２の後に接続されたすべてのリンクを整列させて、それらを実質的に同じ線に延長することができる。第２の関節Ｊ２を回転させると、第２の関節Ｊ２の後に接続されたリンクが、第１の関節Ｊ１と第２の関節Ｊ２との間のリンクに対して垂直に延長し、すなわち、第１の関節Ｊ１の回転軸に対して垂直である。この実施形態において、ロボットシステムは、図３に示される第１の簡略化モデルに非常に近く、また、Ｓ２０１における第１の関節の回転中に、角度γは、９０°に等しい。

【0030】

Ｓ２０１では、他の関節位置が固定されている間に、第１の関節Ｊ１を回転させる。関節運動中、第１の関節Ｊ１にかかるトルクを記録する。摩擦トルクを除去した後、関節トルクτ_１の正弦波部分を抽出でき、これには次の関係がある。

【数1】

ここで、τ_１は重力によって第１の関節Ｊ１にかかるトルクに対応する。
Ａ_１は第１の振幅である。
ｑ_１は回転中の第１の関節Ｊ１の位置である。
φ_１は第１の位相シフトである。

【0031】

【数2】

ここで、ｍはシステム全体の質量である。
ｇは重力加速度である。
Ｌ_１はＪ２から点質量ｍまでの長さである。
θ_２は、図３に示すように、重力方向とＪ１の回転軸との間の角度オフセットである。

【0032】

第１の振幅Ａ１がゼロに非常に近い場合、重力方向が第１の関節Ｊ１の回転軸にほぼ平行であると結論付けることができることを理解する必要がある。したがって、実際の応用に応じて閾値を事前に定義する。第１の振幅Ａ１が事前定義された閾値未満である場合、重力方向を直接取得することができる。それ以外の場合、重力方向を決定するために、次の操作を実行する。

【0033】

図７ａに示すように、正弦曲線とｘ軸との交点はφ_１およびφ_１’であり、これらは重力がＪ１に及ぼす影響がゼロである位置である。したがって、第１の基準角度θ１（水平軸に対する角度オフセット）は、次の関係を使用して決定することができる。

【数3】

【0034】

上述したように、第１の基準角度θ１は、重力によって第１の関節Ｊ１にかかるトルクが実質的にゼロとなる第１の関節Ｊ１の特定の位置に対応する。すなわち、第１の関節Ｊ１がこの特定位置θ１まで回転したとき、質量点ｍは、Ｊ１の回転軸と重力ベクトルによって定義される平面ＺＯＡ上に位置する。いくつかの実施形態では、ステップＳ２０２の前に、第１の関節Ｊ１がこの特定の位置に回転する。

【0035】

Ｓ２０２では、第２の関節Ｊ２以外のすべての関節（第１の関節Ｊ１を含む）をロックし、第２の関節Ｊ２を回転させる。関節運動中、第２の関節Ｊ２にかかるトルクを記録する。摩擦トルクを除去した後、関節トルクτ_２の正弦波部分を抽出でき、これには次の関係がある。

【数4】

ここで、τ_２は重力によって第２の関節Ｊ２にかかるトルクに対応する。
Ａ_２は第２の振幅である。
ｑ_２は第２の関節Ｊ２の回転中の第２の関節の位置である。
φ_２は第２の位相シフトである。

【0036】

図３に示すような所定の構成により、第２の振幅Ａ_２、またはＪ２の最大重力トルク（Ｊ１の角度位置はθ１に等しい）は、次のように導き出すことができる。

【数5】

【0037】

したがって、第１の振幅Ａ１と第２の振幅Ａ２に基づいて、垂直軸（重力方向）とＪ１の回転軸との間の角度オフセットを表す第２の基準角度θ２は、次のように決定できる。

【数6】

【数7】

【0038】

上記の実施形態では、角度γは、第１の関節Ｊ１の回転中、９０°に等しい。この方法は、第１の関節Ｊ１の回転中に角度γが９０°に等しくない場合にも適用可能であることを理解されたい。具体的には、この状況では、第１の基準角度θ１は、次の式を使用して決定できる。

【数8】

【数9】

【数10】

第２の基準角度θ２は、次のように決定することができる。

【数11】

【数12】

【数13】

【0039】

このようにして、第１の基準角度θ１および第２の基準角度θ２の両方が取得され、ロボットシステムに対する重力方向は、第１基準角度θ１および第２の基準角度θ２に基づいて決定され得る。

【0040】

θ２の値は、Ａ１とＡ２の比率に関連するため、Ａ１とＡ２はそれぞれ第１のトルク情報と第２のトルク情報の正弦波部分の振幅であり、計算結果は線形ゲイン、トルク測定／推定のオフセット誤差及び第１の関節Ｊ１と第２の関節Ｊ２の関節摩擦に敏感ではない。したがって、本発明によって提供されるロボットに対する重力方向を推定する方法は、従来の方法と比較して、より信頼性が高く、正確である。

【0041】

図８および図９は、本発明の一実施形態にかかるロボットシステムの第２の簡略化モデルを示す図である。同様に、ＸＹＺ軸はデカルト空間のワールド座標系であり、重力は－Ｚ方向である。すべてのロボットの質量は点質量ｍにまとめることができると仮定しているため、Ｊ１とＪ２を含むすべての関節とリンクは質量が無質量である。

【0042】

図８に示される第２の簡略化モデルは、第１の簡略化モデル（図３）に類似している。相違点は、変位Ｌ２（Ｌ１およびＪ２の回転軸に垂直）の存在であり、図９に示すように、Ｌ１と、第２の関節Ｊ２を質量点ｍに接続する線との間に角度オフセットβが導入され、図１０に示されているように、ロボットシステム１００は、第２の簡略化モデルに近い。具体的には、ロボットシステム１００は、遠位端に配置された大きな質量、例えばエンドエフェクタ、を有することができ、第２の関節Ｊ２とエンドエフェクタとの間に接続されたリンクは、実質的に同一線上に整列される。

【0043】

Ｓ２０１では、第１の関節Ｊ１を回転させ、他の関節位置を全て固定する（Ｊ２角度位置γ＝９０°）。関節運動中、第１の関節Ｊ１にかかるトルクを記録する。摩擦トルクを除去した後、関節トルクτ_１の正弦波部分を抽出でき、これには次の関係がある。

【数14】

ここで、τ_１は重力によって第１の関節Ｊ１にかかるトルクに対応する。
Ａ_１は第１の振幅である。
ｑ_１は第１の関節Ｊ１の回転中の第１の関節の位置である。
φ_１は第１の位相シフトである。

【0044】

図８に示すような所定の構成により、第１の振幅、またはＪ１の最大重力トルク（γは９０°に等しい）は、次のように導き出すことができる。

【数15】

【0045】

第１の基準角度θ１も同様に決定することができる。

【数16】

【0046】

Ｓ２０２では、第２の関節Ｊ２以外のすべての関節（第１の関節Ｊ１を含む）をロックし（Ｊ１の角度位置＝θ１）、第２の関節Ｊ２を等速回転させる。関節運動中に、第２の関節Ｊ２にかかるトルクを記録する。摩擦トルクを除去した後、関節トルクτ_２の正弦波部分を抽出でき、これには次の関係がある。

【数17】

ここで、τ_２は重力によって第２の関節Ｊ２にかかるトルクに対応する。
Ａ_２は第２の振幅である。
ｑ_２は第２の関節Ｊ２の回転中の第２の関節Ｊ２の位置である。
φ_２は第２の位相シフトである。

【0047】

図８に示すような所定の構成により、第２の振幅、またはＪ２の最大重力トルク（Ｊ１の角度位置はθ１に等しい）は、次のように導き出すことができる。

【数18】

【0048】

第２の簡略化モデルの第２の位相シフトはθ_２＋βに等しく、幾何学（図９）から、次の関係を導き出すことができる。

【数19】

【0049】

上記の式と積和の恒等式に基づいて、第２の基準角度θ_２を取得できる。

【数20】

【0050】

このようにして、第１の基準角度θ１および第２の基準角度θ２の両方が取得され、ロボットシステムに対する重力方向は、第１の基準角度θ１および第２の基準角度θ２に基づいて決定され得る。

【0051】

上記の説明では、角度γは、第１の関節Ｊ１の回転中、９０°に等しい。この方法は、第１の関節Ｊ１の回転中に角度γが９０°に等しくない場合にも適用可能であることを理解されたい。具体的には、この状況では、第１の基準角度θ_１は、次の式を使用して決定できる。

【数21】

【数22】

【数23】

第２の基準角度θ_２は、次のように決定することができる。

【数24】

【数25】

【数26】

【0052】

このようにして、第１の基準角度θ１および第２の基準角度θ２の両方が取得され、ロボットシステムに対する重力方向は、第１の基準角度θ１および第２の基準角度θ２に基づいて決定され得る。

【0053】

図１１は、本発明の一実施形態にかかるロボットシステムの第３の簡略化モデルを示す図である。同様に、ＸＹＺ軸はデカルト空間のワールド座標系であり、重力は－Ｚ方向である。すべてのロボットの質量は点質量ｍにまとめることができると仮定しているため、Ｊ１とＪ２を含むすべての関節とリンクは質量が無質量である。

【0054】

第３の簡略化モデルは、図８に示した第２の簡略化モデルと同様である。異なる点は、図１１に示すように、Ｌ_１とＬ_２によって決定される平面に垂直な別のオフセットＬ３の存在である。第３の簡略化モデルは、ロボットシステムが任意の姿勢であることができる非常に一般的な状況を表している。

【0055】

第１のステップＳ２０１では、γを９０°とし、他のすべての関節位置を固定し、第１の関節Ｊ１を回転させる。関節運動中、第１の関節Ｊ１にかかるトルクを記録する。摩擦トルクを除去した後、関節トルクτ_１の正弦波部分を抽出でき、これには次の関係がある。

【数27】

次に、第１の基準角度を決定できる。

【数28】

【0056】

第２のステップＳ２０２では、図１１に示すように、質量点ｍが平面ＺＯＡ内に位置するように、第１の関節Ｊ１を特定の位置θ_１に固定する。次に、他のすべての関節位置を固定し、第２の関節Ｊ２を回転させる。関節運動中に、第２の関節Ｊ２にかかるトルクを記録する。摩擦トルクを除去した後、関節トルクτ_２の正弦波部分を抽出でき、これには次の関係がある。

【数29】

ここで、

【数30】

第２の簡略化モデルφ_２＝θ_２＋β、

【数31】

と同様に、Ａ_２は次のようにも表すことができる。

【数32】

Ｊ１の位置θ_１＝－φ_１（または１８０°－φ_１）とＪ２の位置γ＝９０°のとき、Ｊ２の関節トルクτ_２’を次のように記録および表現できる。

【数33】

τ_２’とＡ_２の比率は次のように表すことができる。

【数34】

上記の式と積和の恒等式に基づいて、第２の基準角度θ_２を取得できる。

【数35】

【0057】

このようにして、第１の基準角度θ１および第２の基準角度θ２の両方が取得され、ロボットシステムに対する重力方向は、第１の基準角度θ１および第２の基準角度θ２に基づいて決定され得る。

【0058】

前述の式の例は、本発明の範囲を限定することを意図していないことを理解されたい。当業者は、上記の式に基づいて様々な同等の修正または変更を行うことができ、これらの修正または変更も本発明の範囲内にあるものとする。

【0059】

上記の方法は、互いに平行でない少なくとも２つの公転回転軸を持つさまざまなタイプのロボットに適用できる。例えば、いくつかの実施形態では、ロボットシステムは、ロボットアーム用の回転支持体を備えた可動ベースをさらに含んでもよい。この実施形態では、回転支持体は第１の関節Ｊ１と見なすことができ、回転支持体の回転軸と平行でない回転軸を有するロボットアームの関節の１つを第２の関節Ｊ２と見なすことができる。

【0060】

当業者は、本発明に記載の開示された方法および手順のそれぞれが、１つまたは複数のコンピュータプログラムまたはコンポーネントを使用して実施され得ることを理解する。これらのコンポーネントは、ＲＡＭ、ＲＯＭ、フラッシュメモリ、磁気または光ディスク、光メモリ、または他の記憶媒体などの揮発性および不揮発性メモリを含む、任意の従来のコンピュータ可読媒体または機械可読媒体上の一連のコンピュータ命令として提供され得る。命令は、ソフトウェアまたはファームウェアとして提供される場合があり、ＡＳＩＣ、ＦＰＧＡ、ＤＳＰ、または他の同様のデバイスなどのハードウェアコンポーネントで全体または一部が実施される場合がある。命令は、一連のコンピュータ命令を実行するときに、開示された方法および手順のすべてまたは一部の実行を促進する、１つまたは複数のプロセッサによって実行されるように構成することができる。

【0061】

ソフトウェアの機能ユニットが実現され、製品として使用される場合、それはコンピュータ内の読み取り可能な記憶媒体に格納することができる。この理解に基づいて、本発明によって提案される技術案は、本質的または部分的にソフトウェア製品として実現され得る。他の場合には、従来の技術にとって有益な上述のシステムおよび方法の１つまたは複数の部分は、ソフトウェア製品の形態として実現され得る。ソフトウェア製品は、実施形態によって開示されたステップのすべてまたは一部を実行するための計算デバイス（プロセッサ、パーソナルコンピュータ、サーバ、またはネットワークデバイスなど）１つまたは複数の命令を含む記憶媒体に格納されてもよい。記憶媒体は、ＵＳＢ（登録商標）ディスク、モバイルハードディスク、読み取り専用メモリ（ＲＯＭ）、ランダムアクセスメモリ（ＲＡＭ）、およびフレキシブルディスクの１つまたは複数などの、プログラムコードを格納できる１つまたは複数のメディアを含んでいる。

【0062】

ここに記載された例に対する様々な変更および修正は、当業者には明らかであることを理解されたい。このような変更および修正は、本発明の精神および範囲から逸脱することなく、また意図した利点を損なうことなく行うことができる。したがって、そのような変更および修正は、添付の特許請求の範囲によってカバーされることが意図されている。他の点では、構成要素間の説明または記載された結合（例えば、直接結合または通信結合）は、１つまたは複数の有線または無線インターフェース（例えば、電気的及び／又は機械的インターフェース）を介して得られる。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6a】

【図6b】

【図7a】

【図7b】

【図8】

【図9】

【図10】

【図11】