知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-12-23
(45)【発行日】2025-01-07
(54)【発明の名称】液圧駆動システム
(51)【国際特許分類】
B25J 13/00 20060101AFI20241224BHJP
【ＦＩ】
B25J13/00 Z
【請求項の数】
10
(21)【出願番号】P 2021123211
(22)【出願日】2021-07-28
(65)【公開番号】P2023018866
(43)【公開日】2023-02-09
【審査請求日】2024-02-02
(73)【特許権者】
【識別番号】507250427
【氏名又は名称】日立ＧＥニュークリア・エナジー株式会社
(74)【代理人】
【識別番号】110000350
【氏名又は名称】ポレール弁理士法人
(72)【発明者】
【氏名】ダメートリ アシュリー
(72)【発明者】
【氏名】小田井 正樹
(72)【発明者】
【氏名】小林 亮介
(72)【発明者】
【氏名】上野 克宜
(72)【発明者】
【氏名】平野 克彦
【審査官】杉山 悟史
(56)【参考文献】
【文献】特開２００９－２８１０８０（ＪＰ，Ａ）
【文献】特開２０２１－０４９５９７（ＪＰ，Ａ）
【文献】実開昭６３－０６１００６（ＪＰ，Ｕ）
【文献】特開２０２０－１５１８１２（ＪＰ，Ａ）
【文献】特開２０１９－１６７７３１（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ２５Ｊ １／００ ～ ２１／０２
(57)【特許請求の範囲】
【請求項1】
クローラーとマニピュレータからなる移動マニピュレータを備えた液圧駆動システムにおいて、
前記マニピュレータは、x-z平面で第１の連結部を回転させる少なくとも１つの関節と、別の平面で第２の連結部を回転させる少なくとも１つの関節と、エンドエフェクタとで構成され、
前記エンドエフェクタに、データを学習するためのグラウンドトゥルースを取得するためにのみ使用される6軸力トルクセンサが設置されており、
前記関節は、流体を供給することによってピストンを動かす少なくとも１つの流体アクチュエータによって駆動され、
流体リザーバから前記流体アクチュエータに流体を供給および排出する流体ドライバと、
前記流体リザーバからシステムに流体を供給するポンプと、
前記ポンプから前記システムに供給される流体の圧力を調整するリリーフ弁と、
前記システムから前記流体アクチュエータへの流体の流れを制御する１つの電磁弁と、逆方向の流れを制御するもう１つの電磁弁と、
少なくとも１つの圧力センサを用いて各関節における流体圧力を測定する圧力ユニットと、
少なくとも１つの流量測定素子を用いて関節位置または角度データを計算する姿勢ユニットと、
弁の制御信号を生成するコントローラと、
ロボットの状態を表示し、ロボットの動きを指令するインターフェースと、
3D力推定ユニットと、を有し、
前記3D力推定ユニットの入力は、圧力センサの数および姿勢データの数によって構成され、
前記3D力推定ユニットの出力は、各軸上の力の３つの出力によって構成され、
前記3D力推定ユニットの出力は、前記エンドエフェクタまたはグリッパ位置に適用され、
前記3D力推定ユニットの出力は、教師付き学習によって得られるニューラルネットワークモデルから計算される液圧駆動システムであって、
前記リリーフ弁を電気機械的に制御するモータをさらに有し、
前記リリーフ弁の出口に前記圧力センサを設置し、
前記3D力推定ユニットの入力に前記圧力センサのデータを含み、
１つの関節に設置される前記圧力ユニットは、流体アクチュエータの下部チャンバとロッド室の圧力状態を測定する２つの圧力センサを使用して取得され、
１つの関節における姿勢ユニットは、２つの流量測定素子からなり、
前記流体アクチュエータの底面にある１つの流量測定素子とロッド側にある１つの流量測定素子によって測定された流量を差し引くことによって姿勢を計算することを特徴とする液圧駆動システム。
【請求項2】
クローラーとマニピュレータからなる移動マニピュレータを備えた液圧駆動システムにおいて、
前記マニピュレータは、x-z平面で第１の連結部を回転させる少なくとも１つの関節と、別の平面で第２の連結部を回転させる少なくとも１つの関節と、エンドエフェクタとで構成され、
前記エンドエフェクタに、データを学習するためのグラウンドトゥルースを取得するためにのみ使用される6軸力トルクセンサが設置されており、
前記関節は、流体を供給することによってピストンを動かす少なくとも１つの流体アクチュエータによって駆動され、
流体リザーバから前記流体アクチュエータに流体を供給および排出する流体ドライバと、
前記流体リザーバからシステムに流体を供給するポンプと、
前記ポンプから前記システムに供給される流体の圧力を調整するリリーフ弁と、
前記システムから前記流体アクチュエータへの流体の流れを制御する１つの電磁弁と、逆方向の流れを制御するもう１つの電磁弁と、
少なくとも１つの圧力センサを用いて各関節における流体圧力を測定する圧力ユニットと、
少なくとも１つの流量測定素子を用いて関節位置または角度データを計算する姿勢ユニットと、
弁の制御信号を生成するコントローラと、
ロボットの状態を表示し、ロボットの動きを指令するインターフェースと、
3D力推定ユニットと、を有し、
前記3D力推定ユニットの入力は、圧力センサの数および姿勢データの数によって構成され、
前記3D力推定ユニットの出力は、各軸上の力の３つの出力によって構成され、
前記3D力推定ユニットの出力は、前記エンドエフェクタまたはグリッパ位置に適用され、
前記3D力推定ユニットの出力は、教師付き学習によって得られるニューラルネットワークモデルから計算される液圧駆動システムであって、
前記リリーフ弁を電気機械的に制御するモータをさらに有し、
前記リリーフ弁の出口に前記圧力センサを設置し、
前記3D力推定ユニットの入力に前記圧力センサのデータを含み、
１つの関節に設置される前記圧力ユニットは、流体アクチュエータの下部チャンバとロッド室の圧力状態を測定する２つの圧力センサを使用して取得され、
１つの関節における姿勢ユニットは、２つの流量測定素子からなり、
姿勢データからの前記3D力推定ユニットの入力は、流量測定素子の数であることを特徴とする液圧駆動システム。
【請求項3】
請求項２に記載の液圧駆動システムであって、前記3D力推定ユニットは、前記エンドエフェクタに加えられた力の発生源による１つのデータセットからのトレーニングデータを用いた連続する３段階でトレーニングされ、
前記データセットは、負荷に掛かる力、環境との接触による外乱力、負荷を保持したままでの環境との接触による外乱力の少なくともいずれかを含むことを特徴とする液圧駆動システム。
【請求項4】
請求項３に記載の液圧駆動システムであって、前記リリーフ弁の出口に温度センサをさらに有し、
前記3D力推定ユニットの入力に前記温度センサからのデータを含むことを特徴とする液圧駆動システム。
【請求項5】
請求項４に記載の液圧駆動システムであって、前6軸力トルクセンサは、前記エンドエフェクタに取り付けられた状態で較正され、
較正データの追加のグラウンドトゥルースは、前記マニピュレータが所定の姿勢に設定されたときに取得され、
前記3D力推定ユニットは、１つのデータセットからのトレーニングデータと、前記エンドエフェクタで加えられた力の発生源によるそれぞれの較正データを用いた連続する３段階でトレーニングされることを特徴とする液圧駆動システム。
【請求項6】
クローラーとマニピュレータからなる移動マニピュレータを備えた液圧駆動システムにおいて、
前記マニピュレータは、x-z平面で第１の連結部を回転させる少なくとも１つの関節と、別の平面で第２の連結部を回転させる少なくとも１つの関節と、エンドエフェクタとで構成され、
前記エンドエフェクタに、データを学習するためのグラウンドトゥルースを取得するためにのみ使用される6軸力トルクセンサが設置されており、
前記関節は、流体を供給することによってピストンを動かす少なくとも１つの流体アクチュエータによって駆動され、
流体リザーバから前記流体アクチュエータに流体を供給および排出する流体ドライバと、
前記流体リザーバからシステムに流体を供給するポンプと、
前記ポンプから前記システムに供給される流体の圧力を調整するリリーフ弁と、
前記システムから前記流体アクチュエータへの流体の流れを制御する１つの電磁弁と、逆方向の流れを制御するもう１つの電磁弁と、
少なくとも１つの圧力センサを用いて各関節における流体圧力を測定する圧力ユニットと、
少なくとも１つの流量測定素子を用いて関節位置または角度データを計算する姿勢ユニットと、
弁の制御信号を生成するコントローラと、
ロボットの状態を表示し、ロボットの動きを指令するインターフェースと、
3D力推定ユニットと、を有し、
前記3D力推定ユニットの入力は、圧力センサの数および姿勢データの数によって構成され、
前記3D力推定ユニットの出力は、各軸上の力の３つの出力によって構成され、
前記3D力推定ユニットの出力は、前記エンドエフェクタまたはグリッパ位置に適用され、
前記3D力推定ユニットの出力は、教師付き学習によって得られるニューラルネットワークモデルから計算される液圧駆動システムであって、
前記マニピュレータは、x-z平面で前記第１の連結部を回転させる少なくとも１つの関節と、x-y-z平面で前記第２の連結部を回転させる少なくとも１つの連続体関節と、前記エンドエフェクタとで構成され、
前記圧力ユニットは、少なくとも１つの圧力センサを使用することによって回転関節における流体圧力を測定し、前記連続体関節における流体圧力を少なくとも４つの圧力センサを使用することによって測定し、
前記姿勢ユニットは、少なくとも１つの流量測定素子を使用することによって前記回転関節における関節位置または角度データを計算し、前記連続体関節における関節位置または角度データを少なくとも４つの流量測定素子を使用することによって計算することを特徴とする液圧駆動システム。
【請求項7】
請求項６に記載の液圧駆動システムであって、前記リリーフ弁を電気機械的に制御するモータをさらに有し、
前記リリーフ弁の出口近傍に前記圧力センサが設置され、
前記3D力推定ユニットの入力に、前記圧力センサからのデータを含み、
前記回転関節に設置される圧力ユニットは、流体アクチュエータの下部チャンバとロッド室の圧力状態を測定する２つの圧力センサを使用して取得され、
前記連続体関節に設置される圧力ユニットは、流体アクチュエータの下部チャンバの４つのセンサとロッド室の４つのセンサで構成される８つの圧力センサを使用して取得され、
前記回転関節に設置される姿勢ユニットは、流体アクチュエータの下部チャンバとロッド室での流体の流れを測定する２つの流量測定素子で構成され、
前記連続体関節に設置される姿勢ユニットは、流体アクチュエータの下部チャンバの４つの流量測定素子とロッド室の４つの流量測定素子で構成される８つの流量測定素子で構成され、
姿勢データからの前記3D力推定ユニットの入力は、流量測定要素の数であることを特徴とする液圧駆動システム。
【請求項8】
請求項７に記載の液圧駆動システムであって、前記リリーフ弁の出口に温度センサをさらに有し、
前記3D力推定ユニットの入力に、前記温度センサからのデータを含み、
前記6軸力トルクセンサは、前記エンドエフェクタに取り付けられた状態で較正され、
前記マニピュレータが所定の姿勢に設定されると、データを学習するための追加のグラウンドトゥルースが取得され、
前記3D力推定ユニットは、１つのデータセットからのトレーニングデータと、前記エンドエフェクタで加えられた力の発生源によるそれぞれの較正データを用いた連続する３段階でトレーニングされることを特徴とする液圧駆動システム。
【請求項9】
請求項６に記載の液圧駆動システムであって、前記リリーフ弁を電気機械的に制御するモータをさらに有し、
前記リリーフ弁の出口近傍に前記圧力センサが設置され、
前記3D力推定ユニットの入力に、前記圧力センサからのデータを含み、
前記回転関節に設置される圧力ユニットは、流体アクチュエータの下部チャンバとロッド室の圧力状態を測定する２つの圧力センサを使用して取得され、
前記連続体関節に設置される圧力ユニットは、流体アクチュエータの下部チャンバの４つのセンサとロッド室の４つのセンサで構成される８つの圧力センサを使用して取得され、
前記回転関節に設置される姿勢ユニットは、流体アクチュエータの下部チャンバとロッド室での流体の流れを測定する２つの流量測定素子で構成され、
前記連続体関節に設置される姿勢ユニットは、流体アクチュエータの下部チャンバの４つの流量測定素子とロッド室の４つの流量測定素子で構成される８つの流量測定素子で構成され、
姿勢データからの前記3D力推定ユニットの入力は、流量測定要素の数であることを特徴とする液圧駆動システム。
【請求項10】
請求項９に記載の液圧駆動システムであって、前記リリーフ弁の出口に温度センサをさらに有し、
前記3D力推定ユニットの入力に、前記温度センサからのデータを含み、
前記6軸力トルクセンサは、前記エンドエフェクタに取り付けられた状態で較正され、
前記マニピュレータが所定の姿勢に設定されると、データを学習するための追加のグラウンドトゥルースが取得され、
前記3D力推定ユニットは、１つのデータセットからのトレーニングデータと、前記エンドエフェクタで加えられた力の発生源によるそれぞれの較正データを用いた連続する３段階でトレーニングされることを特徴とする液圧駆動システム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、液圧駆動マニピュレータシステムのエンドエフェクタに加えられる3D外力の推定方法に関する。より具体的には、任意の可能なマニピュレータ姿勢における力の推定方法に関する。
【背景技術】
【0002】
高放射線の建物や深海などの極限環境で重い物体や長い物体を操作するには、安全で制御可能な大型の動力機械を必要とする。液圧駆動マニピュレータは、高出力と耐久性の特性から、これらの種類の応用に適していると考えられる。
【0003】
しかしながら、これらの過酷な環境において、液圧駆動マニピュレータのより高精度な制御性を実現するために、マニピュレータのエンドエフェクタに力センサを直接設置することは不可能である。
【0004】
従って、マニピュレータのエンドエフェクタに加えられる3D外力の推定方法が必要である。
【0005】
本技術分野の背景技術として、例えば、特許文献１のような技術がある。特許文献１には、簡単な構成あるいはマニピュレータと各関節での１つのトルクセンサを用いて、負荷の力ベクトルを推定する方法が提案されている。
【0006】
特許文献１では、負荷を保持したまま、マニピュレータを所定の姿勢に設定する。その状態で、姿勢を測定せずに、各関節におけるトルクを測定する。マニピュレータの動的関係から、負荷重量と負荷の重心の水平位置を計算できる。
【0007】
また、特許文献２には、掘削機先端での大きな衝撃によるセンサ故障に起因する掘削機の自動掘削作業の信頼性向上のため、液圧駆動システムを用いてバケットの姿勢を推定する方法が提案されている。
【0008】
特許文献２では、ブーム及びアームの圧力データ及び姿勢データ、並びにバケットの圧力データを推定に用いる。特許文献２の発明は、負荷の重量を推定するために意図的に使用されてはいないが、提案されている構成により、特に記述された軸のx-y平面での2D力に対して、先端に加えられた力を推定することが可能になる。
【先行技術文献】
【特許文献】
【0009】
【文献】特開２０２０－１５１８１２号公報
【文献】特開２０１９－１６７７３１号公報
【発明の概要】
【発明が解決しようとする課題】
【0010】
しかしながら、上記特許文献１の方法は、計算部において姿勢データが使用されないため、決定されていない姿勢における外力を推定するために使用することができない。
【0011】
また、この方法は、液圧駆動マニピュレータを、システムヒステリシスによる一定のトルクまたは圧力制御を使用して等しい位置に設定することができないので、電気式の電動機システムにのみ使用できる。
【0012】
また、上記特許文献２の方法は、バケットの姿勢、ベースの姿勢及びベースの圧力またはトルクに関する情報が不足しているため、バケットの先端における3D外力を推定することは不可能である。
【0013】
そこで、本発明の目的は、マニピュレータを遠隔操作する液圧駆動システムにおいて、マニピュレータ自体にセンサを設置することなく、任意のマニピュレータ姿勢においてマニピュレータのエンドエフェクタに加わる3D外力を高精度に推定可能な信頼性の高い液圧駆動システムを提供することにある。
【課題を解決するための手段】
【0014】
上記課題を解決するために、本発明は、クローラーとマニピュレータからなる移動マニピュレータを備えた液圧駆動システムにおいて、前記マニピュレータは、x-z平面で第１の連結部を回転させる少なくとも１つの関節と、別の平面で第２の連結部を回転させる少なくとも１つの関節と、エンドエフェクタとで構成され、前記エンドエフェクタに、データを学習するためのグラウンドトゥルースを取得するためにのみ使用される6軸力トルクセンサが設置されており、前記関節は、流体を供給することによってピストンを動かす少なくとも１つの流体アクチュエータによって駆動され、流体リザーバから前記流体アクチュエータに流体を供給および排出する流体ドライバと、前記流体リザーバからシステムに流体を供給するポンプと、前記ポンプから前記システムに供給される流体の圧力を調整するリリーフ弁と、前記システムから前記流体アクチュエータへの流体の流れを制御する１つの電磁弁と、逆方向の流れを制御するもう１つの電磁弁と、少なくとも１つの圧力センサを用いて各関節における流体圧力を測定する圧力ユニットと、少なくとも１つの流量測定素子を用いて関節位置または角度データを計算する姿勢ユニットと、弁の制御信号を生成するコントローラと、ロボットの状態を表示し、ロボットの動きを指令するインターフェースと、3D力推定ユニットと、を有し、前記3D力推定ユニットの入力は、圧力センサの数および姿勢データの数によって構成され、前記3D力推定ユニットの出力は、各軸上の力の３つの出力によって構成され、前記3D力推定ユニットの出力は、前記エンドエフェクタまたはグリッパ位置に適用され、前記3D力推定ユニットの出力は、教師付き学習によって得られるニューラルネットワークモデルから計算される液圧駆動システムであって、前記リリーフ弁を電気機械的に制御するモータをさらに有し、前記リリーフ弁の出口に前記圧力センサを設置し、前記3D力推定ユニットの入力に前記圧力センサのデータを含み、１つの関節に設置される前記圧力ユニットは、流体アクチュエータの下部チャンバとロッド室の圧力状態を測定する２つの圧力センサを使用して取得され、１つの関節における姿勢ユニットは、２つの流量測定素子からなり、前記流体アクチュエータの底面にある１つの流量測定素子とロッド側にある１つの流量測定素子によって測定された流量を差し引くことによって姿勢を計算することを特徴とする。
【発明の効果】
【0015】
本発明によれば、マニピュレータを遠隔操作する液圧駆動システムにおいて、マニピュレータ自体にセンサを設置することなく、任意のマニピュレータ姿勢においてマニピュレータのエンドエフェクタに加わる3D外力を高精度に推定可能な信頼性の高い液圧駆動システムを実現することができる。
【0016】
これにより、高放射線の建物や深海などの極限環境下においても、作業中の物体やマニピュレータ自体の損傷を防ぐことができる。
【0017】
上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【0018】
【図1】本発明の実施例１に係る液圧駆動システムの概略構成を示す図である。
【図2】１つの圧力センサと１つの流量測定素子を有する１つのアクチュエータの基本構成を示す図である。
【図4】２つの圧力センサと２つの流量測定素子と、リリーフ弁における１つの圧力センサとを有する１つのアクチュエータの構成を示す図である。
【図5】外力の形態が異なる３種類のデータセットを用いて、推定量モデルを３段階で連続的に学習する方法を示す図である。
【図6】本発明の実施例２に係る液圧駆動システムの概略構成を示す図である。
【図8】様々なタイプの加えられた力の間のモーメント長の構成を示す図である。
【図9】事前情報なしでモーメント長を計算する手順を示すフローチャートである。
【図10】本発明の実施例３に係る連続体関節の構成を示す図である。
【発明を実施するための形態】
【0019】
以下、図面を用いて本発明の実施例を説明する。なお、各図面において同一の構成については同一の符号を付し、重複する部分についてはその詳細な説明は省略する。
【0020】
また、以下の説明は、提案する実施形態の例示であり、特許請求の範囲を限定するものではない。さらに、推定システムの動作原理は、当業者が本発明による方法を実施することを可能にするために提供される例示的なものであり、請求項に限定されるものではない。
【実施例1】
【0021】
【0022】
図１及び図２に、本実施例の液圧駆動システムの主要な構成を示す。本実施例の液圧駆動システムは、図１に示すように、流体ドライバ２によって制御される長いホース１０に接続する流体駆動の移動マニピュレータ１によって構成される。移動マニピュレータ１は、過酷な環境で作業されるため、ロボット（つまり、移動マニピュレータ１の本体）とホース１０のみが環境にさらされ、一方、液圧駆動システム８及びインターフェース７は、より安全な場所に保たれる。
【0023】
図２に示すように、一般に、流体は、モータポンプ（電気ポンプ）２４を使用することによって、流体リザーバ２５から供給され、シリンダアクチュエータ１２に排出される。流体の圧力は、リリーフ弁２３によって調節され、電磁弁２１を通る流体の流れを制御することによって、シリンダアクチュエータ１２の下部チャンバ１３に供給される。
【0024】
シリンダアクチュエータ１２内及びホース１０に沿って流れる流体の状態は、圧力ユニット３内部の圧力センサ３１及び姿勢ユニット４内部の流量測定素子４１によって測定される。ピストン１５を押したり引いたりすることにより、ピストン１５は機械的にマニピュレータの関節角度に変換され、移動マニピュレータ１の各関節の姿勢は、下部チャンバ１３に入る測定された流れから計算される。
【0025】
計算された移動マニピュレータ１の姿勢および圧力は、3D力推定ユニット５に入力され、Fx、Fy、Fzである３軸上のマニピュレータエンドエフェクタの力として出力される。そして、これらの３つの出力は、インターフェース７上に表示されるか、またはコントローラ６の力フィードバックパラメータとなり、流体ドライバ２を作動させて、マニピュレータの動作をさらに制御する。
【0026】
3D力推定ユニット５の入力数は、圧力ユニット３の出力ベクトルの数と姿勢ユニット4の出力ベクトルの数で定義される。マニピュレータが５つの関節で構成され、そのうちの３つの関節がx-z平面内で回転し、１つの関節がx-y平面内で回転し、他の関節がy-z平面内で回転すると仮定する。各関節が１つのシリンダアクチュエータ１２によって駆動される場合、５つの圧力センサ３１及び５つの流量測定素子４１によって状態が測定される５－シリンダアクチュエータが存在する。従って、3D力推定ユニット５の入力は１０ベクトルとなる。
【0027】
しかしながら、１つの関節におけるアクチュエータの数は、１つのシリンダアクチュエータに限らず、１つの関節を回転させるための２つ以上のシリンダアクチュエータを有することも可能である。そのため、１つの関節用に２つのシリンダアクチュエータがある場合、3D力推定ユニット５の入力数は２０ベクトルとなる。
【0028】
なお、流量測定素子４１は、従来の流量センサに限定されず、流体移動の体積または流体移動の速度のいずれかを測定することができる他の任意のセンサであってもよい。このようなセンサの例としては、ピストンに位置センサが取り付けられたシリンダが考えられ、この場合、流体の容積は、位置センサで計測したピストンの変位及びシリンダの直径から計算することができる。センサの体積変位から、シリンダアクチュエータの変位に変換し、姿勢状態として計算することができる。
【0029】
図３は、ニューラルネットワークシステムである3D力推定ユニット５の内部を示している。圧力ユニット３からの圧力状態と姿勢ユニット４からの姿勢状態の入力ベクトルは、前処理演算部５１に入力される。前処理演算部５１は、少なくとも、センサユニットの異なる周波数からのレート同期および入力の正規化から構成される。
【0030】
正規化された入力は、ニューラルネットワークの入力層５２上のノードに入力される。入力層５２は、ニューラルネットワークの隠れ層５３上のノードに接続され、ニューラルネットワークの出力層５４上のノードを通過する。隠れノード５３内のノード数は、入力層５２内のノード数と少なくとも同数以上である。
【0031】
隠れ層５３は、フルコネクト（FC）タイプのノードまたは単一層のみに限定されず、ヒステリシス特性を継承する流体駆動型アクチュエータであるため、RNNタイプ、LSTMタイプ、GRUタイプ、またはFCタイプの多層とすることができる。出力層５４上のノード数は３ノードである。出力層５４の各ノードからの出力は、後処理演算部５５で後処理される。後処理演算部５５は、少なくとも出力の非正規化からなる。
【0032】
3D力推定ユニット５に学習させるために、マニピュレータのエンドエフェクタ位置が、コントローラ６を介してインターフェース７からオペレータによって制御される。
【0033】
コントローラ６は、各関節の弁をさらに制御し、これにより、シリンダアクチュエータ１２に押す力または引く力が生成される。この押す力または引く力により、マニピュレータの姿勢状態とアクチュエータ内部の圧力状態が変化する。エンドエフェクタには、荷重重量によって、または環境との接触からの外力によって、力が加えられる。
【0034】
負荷には、3D力推定ユニット５の基礎となる重みゼロの物体も含まれる。この外力は、6軸力トルクセンサ１１を用いて測定される。マニピュレータの関節をランダムに制御し、圧力状態、姿勢状態、及び力トルクセンサデータのログを記録することにより、3D力推定ユニット５の学習データセットを取得することができる。
【0035】
各ノードの接続に対する重み付けは、機械学習法を使用してトレーニングされる。例えば、ニューラルネットワークモデルが教師付き学習法を用いてトレーニングされる場合、マニピュレータのエンドエフェクタに加えられた正しい力は、与えられた圧力と姿勢状態の入力で、6軸力トルクセンサ１１によって測定され、バックプロパゲーション法（誤差逆伝播法）などを用いて、各ノードの接続の重み付けを計算する。6軸力トルクセンサ１１は、モデルのトレーニングにのみ使用され、実際の推定には使用されない。
【0036】
マニピュレータのエンドエフェクタに加えられる3D外力を推定するために、コントローラ６は、マニピュレータに制御モーションを生成し、圧力状態および姿勢状態は、それぞれ対応する圧力ユニット３および姿勢ユニット４によって測定され、これらの状態は、入力信号として3D力推定ユニット５に入力される。
【0037】
3D力推定ユニット５に入力された入力信号は、トレーニングされたノード５２，５３，５４によってさらに処理され、これにより、6軸力トルクセンサ１１に等しいローカル座標で各軸に加えられる推定力が得られる。そして、ローカル座標センサからの出力力を目的の座標に変換し、その変換をコントローラ６内に配置することができる。
【0038】
この主要な構成により、圧力データと姿勢データを3D力推定ユニット５の入力として利用することで、流圧駆動マニピュレータのエンドエフェクタに加わる3D力を推定可能な力推定システムを実現することができる。
【0039】
また、本実施例の力推定システムは、推定精度を向上させるために、図４に示すような流体状態の付加的なデータを含んで構成されている。この図において、シリンダアクチュエータ１２，１２ａの数は、ロボットの関節の数およびその基本構成に依存する。複数のアクチュエータを有するため、調整された圧力のベースを知らずに、推定精度は、リリーフ弁２３の設定に制限される。
【0040】
本実施例では、リリーフ弁２３に電気機械式のリリーフ弁を利用しており、作動のための圧力源は、コントローラ６によって、またはインターフェース７からのコマンドによって、自動的に調節される。また、リリーフ弁２３の出力部には、圧力センサ３２が設置されており、調整された圧力を測定することができるようになっている。制御可能なリリーフ弁２３を有することによって、各シリンダのロッド室１４または１４ａにおける圧力を推定することが可能であり、その結果、推定モデルのトレーニング中に、下部チャンバ１３およびロッド室１４、または下部チャンバ１３ａおよびロッド室１４ａの両方における圧力差をカウントすることができる。
【0041】
また、推定精度をさらに向上させるために、シリンダアクチュエータ１２のロッド室１４側に圧力センサを付加すると、電磁弁２２の何らかの変化によって生じるマニピュレータの移動中に力を推定する際に、シリンダアクチュエータ１２のより正確な圧力状態が得られる。また、電磁弁２２の制御によるピストン１５の位置の変化により、マニピュレータの姿勢状態がより正確になることにより、推定精度がさらに向上する。姿勢は、下部チャンバ１３側とロッドチャンバ１４側との流体の流れの差に基づいて計算される。
【0042】
液圧駆動型アクチュエータが、ヒステリシス特性、マニピュレータの上下運動、またはピストン１５の押し引き運動を有することを知っていると、同じピストン１５の位置でも圧力差が生じる可能性があり、したがって、実際の姿勢自体は、姿勢ユニット４によって計算されるのではなく、流量測定要素の感知結果を3D力推定ユニット５への直接の入力として使用する。そのため、姿勢ユニット４に誤差を生じさせる影響のヒステリシスをネットワークに直接トレーニングすることで、推定精度を向上させている。
【0043】
一般に、推定システムにトレーニングされた任意のデータセットは、合計で集約され、モデルの学習プロセスは、前述の集約データセットのランダムな方法でトレーニングデータを取得する。ただし、負荷や外部接触力によって引き起こされる力ベクトルにはさまざまな特性がある。負荷によって引き起こされる力ベクトルの結果は、明らかに重力方向へのベクトルになるが、外部接触力は、通常は接触領域に垂直なランダム方向を向いている。全てのデータセットを直接集約してトレーニングすると、トレーニングデータの3D力ベクトルに一貫性がないため、学習プロセスをオーバーフィット（過剰適合）させることになる。
【0044】
図５は、力ベクトルの方向の不一致に関して推定システムをトレーニングする手順を示している。この図では、データセットは、負荷に対する力５０１、環境との接触力５０３、および負荷を保持しながらの環境との接触力５０５など、エンドエフェクタに加えられる外力の３つのタイプの源を有するものとして示している。
【0045】
推定モデルのトレーニングは、データセットのタイプに基づいて３つの段階で実行される。第１のトレーニング段階５０２は、負荷の重量５０１によって引き起こされる外力のデータセットのみをトレーニングする段階である。これを行うことにより、モデルは、負荷によって生じる可能性のある任意のマニピュレータ姿勢中の3D力の特性を正しく学習することができる。
【0046】
次に、第１のトレーニング段階５０２でトレーニングされたモデルは、第２のトレーニング段階５０４の初期モデルとして使用され、環境との接触力（外乱力）である第２のタイプのデータセット５０３を用いてトレーニングされる。同様に、第２のトレーニング段階５０４でトレーニングされたモデルは、第３のトレーニング段階５０６の初期モデルとして使用され、負荷の重量及び外乱力である第３のタイプのデータセット５０５を用いてトレーニングされる。
【0047】
なお、図５では、外力の発生源を３種類で示しているが、これら３種類のみに限定されるものではない。また、エンドエフェクタの静的位置を設定し、アクチュエータに姿勢制御の変更を適用することで、接触力の変動を追加することもできる。これにより、外力を引き起こす圧力状態と流量測定のダイナミックを生成することができる。また、トレーニング段階の順序は、図５に限定されず、他のバリエーションのデータセットタイプを含むデータタイプの異なる順序も可能である。
【0048】
ロボットは過酷な環境下で動作するため、厳しい温度変化が避けられない。実験室の温度条件からより広い温度変化までの推定範囲をカバーするために、追加の温度センサ（図示せず）が3D力推定ユニット５の入力として使用される。各ジョイントの温度は一般に等しいため、各ジョイントの電磁弁２１の上流側のリリーフ弁２３の出力部に１つの温度センサを追加する。流体の温度変化は圧力状態の変化にも影響するので、温度センサを付加することにより、異なる温度条件での推定能力を向上させることができる。
【0049】
異なる温度条件でのデータセットをトレーニングするためには、必ずしも全体の環境温度を変化させる必要はないが、少なくとも流体温度は、流体温度を制御する何らかのメカニズムによって変化する。温度範囲は、水、油、または気体のいずれかの流体の媒体に依存する。
【0050】
また、環境温度を変化させることによってもトレーニングを行うことができる。これを行うことにより、温度変化がセンサの測定オフセットにつながるため、6軸力トルクセンサ１１も較正する必要がある。一般に、力センサは、マニピュレータから取り外されて、地面に平らに置かれた状態で較正される。これは、トレーニング手順として、オペレータに不便さを招く。したがって、追加の較正手順が必要となる。
【0051】
本実施例では、6軸力トルクセンサ１１は、それを取り外さずにエンドエフェクタに取り付けられた状態で較正される。力センサを地面に平らに置くのではなく、マニピュレータを任意の所定の姿勢に設定しながら6軸力トルクセンサ１１を較正することができる。この方法により、較正手順を簡単化できる。その姿勢を力センサのゼロ状態として有する一方、それらの所定の姿勢での6軸力トルクセンサ１１のデータを参照してトレーニングが実行される。
【0052】
トレーニングのプロセスは、そのキャリブレーション方法で正常に実行できる。しかしながら、推定プロセス中に、3D力推定ユニット５による推定の結果を、局所的な所定の初期較正姿勢から所望の座標に変換することが重要である。また、各ステージのトレーニングデータセットには、測定システムの標準的なゼロ力を有するように、この較正データが含まれる。さらに、トレーニング段階は、力センサのこの較正データを含めながら、推定精度を向上させるために、ある範囲の温度を追加することができる。
【実施例2】
【0053】
【0054】
図６は、本実施例の液圧駆動システムの概略構成を示す図である。図６に示すように、本実施例の液圧駆動システムは、実施例１の基本構成と同様の構成を有するが、3Ｄ力推定ユニット５０は、力の３つの出力およびトルクの３つの出力を有している。この3D力推定ユニット５０の出力は、6軸力トルクセンサ１１がシステムに取り付けられているかのように等しくなる。また、3D力推定ユニット５０の入力には、作用トルクのモーメント長が加算されるのに対し、このモーメント長は、コントローラ６内部のモーメント長演算部６１により演算される。モーションコントロールユニット６０は、入力インターフェース７または自律モーション生成コントローラを電磁弁開閉指令に変換する制御部である。
【0055】
3D力推定ユニット５０の詳細プロセスを、図７に示す。トレーニングおよび推定プロセスの動作原理は、実施例１と同様であるが、前処理演算部５１１の入力は、モーメント長ｒが追加され、一方、この値は、エンドエフェクタの重心位置として設定される。そのため、入力層５２１も１つのノードで加算されるのに対し、隠れ層５３１上のノードの数は、入力層５２１内のノードの数と少なくとも同じ数以上である。そして、出力層５４１は、６つのノードを有し、ニューラルネットワーク計算の結果は、後処理演算部５５１によってさらに処理され、各軸で３つの推定力と、各軸で3つの推定トルクとが出力される。
【0056】
実施例２のこの基本構成により、加えられた力だけでなく、マニピュレータのエンドエフェクタに加えられたトルクも推定することができる。
【0057】
トレーニングプロセスの間、オペレータが負荷の重量または環境との接触位置を設定するため、モーメント長は既知である。但し、リアルタイムの推定プロセスでは、モーメント長を計算する必要がある。モーメント長演算部６１は、エンドエフェクタの重心位置６０３の値によって、デフォルトで一定の出力を与えている。さらに、ニューラルネットワークをトレーニングするためのメトリックに、6軸力トルクセンサ１１の測定値からの実際のトルクの誤差(rmse、mse、maeのいずれか)を加え、力の推定結果にエンドエフェクタの重心位置６０３のモーメント長を掛ける。
【0058】
モーメント長の計算精度を向上させるために、以下の式（１）に示すように、複数の剛体に対する重心方程式を使用することができる。エンドエフェクタの質量とエンドエフェクタの重心の積と、負荷の質量とエンドエフェクタの長さの積の合計を、エンドエフェクタの質量と負荷の質量の合計で割る。
【0059】
【数1】
【0060】
ここで、meはエンドエフェクタの質量、reはエンドエフェクタの重心、m0は推定される負荷の質量（または外力）、r0は負荷の重心（または外力）である。
【0061】
図８は、複数の剛体からの重心の位置を示す図である。図８（ａ）は、エンドエフェクタの重量のみによって生じる外力を示しており、エンドエフェクタの重心の位置（またはモーメント長）１１１およびエンドエフェクタの重量（力）の大きさは、一定であることが知られている。図８（ｂ）は、負荷の重量による外力を示しており、負荷の重心の位置を１１２、エンドエフェクタと負荷の両方の重心の位置を１１３として示している。図８（ｃ）は、環境１１４との接触による外力、図８（ｄ）は、物体（負荷）１１０を保持したまま環境１１４との接触による外力を示している。
【0062】
本実施例では、エンドエフェクタの位置に任意の種類のセンサを設置することができないため、外力の正確な位置は不明である。但し、理想的な条件を考慮することにより、外力の位置は全力のモーメント長と等しいと見なすことができる。
【0063】
トレーニング中、負荷の質量および外力は、事前に知られている。しかしながら、リアルタイムの推定では、この情報は何らかの計算から取得する必要がある。
【0064】
図９に、事前情報なしでモーメント長を計算する手順を示す。原則として、この手順はコントローラ６の一部として実行される。確認ブロック６０１は、モーメント長が計算されたかどうかを判定する。モーメント長が計算されていた場合（YES）、3D力推定ユニット５０は、計算されたモーメント長を使用することによって、推定力を通常通りに計算する。一方、モーメント長が計算されていなかった場合（NO）、コントローラ６は、後述する姿勢較正ブロック６０２に進むようにマニピュレータに指令する。
【0065】
その後、システムは、入力されたモーメント長がエンドエフェクタの重心位置６０３である3D力推定ユニット５０を使用することにより、エンドエフェクタに加えられた力を推定する。3D力推定ユニット５０の出力データ６５０である推定されたFzは、モーメント長演算部６１での計算に使用される。最後に、モーメント長演算部６１で算出されたモーメント長位置６１１は、3D力推定ユニット５０に入力され、リアルタイム推定に利用される。この3D力推定ユニットの出力データ６５１は、他の制御手順を実行するためにコントローラ６によって使用される。モーメント長演算部６１での演算は、以下の式（２）で表すことができる。
【0066】
【数2】
【0067】
ここで、Fzは、姿勢較正ブロック６０２で求めた較正姿勢を使用した3D力推定ユニットの出力データ６５０（推定結果）である。較正姿勢は、エンドエフェクタの位置が単一またはいくつかの姿勢によって一定に保たれている間の、エンドエフェクタの向きがグローバルz軸に対して垂直に上または下を向いている任意の所定の関節姿勢である。
【0068】
この姿勢では、グローバルx軸におけるエンドエフェクタの重心は、グローバルx軸における荷重の重心と等しくなる。z軸においては重心位置が異なるが、力の合計のみを測定するため、この違いは重要ではない。この所定の姿勢により、モーメント長の値をエンドエフェクタの重心位置として使用しながら、推定力Fzの精度を高めることができる。
【0069】
実施例１と同様に、本実施例では、作動のための圧力源がコントローラ６またはインターフェース７からの指令により調整できるように電気機械式のリリーフ弁２３を追加することで、推定器の精度を改善することができる。また、調整された圧力を測定することができるように、リリーフ弁２３の出力部に圧力センサ３２が追加されている。電磁弁２２の変化によって生じるマニピュレータの移動中の力を推定するときに、シリンダアクチュエータ１２のロッド室１４側に圧力センサ３３を追加することによって、推定器の精度をさらに向上することができる。また、ロッド室１４側に流量測定素子４２を追加することで、3D力推定器ユニット５０への入力に全ての流量測定素子を直接使用することになり、推定器の精度をさらに向上させることができる。
【0070】
また、実施例１と同様に、本実施例では、リリーフ弁２３の出力部に温度センサ（図示せず）を設置することで、推定器の推定可能範囲を拡大することができる。推定可能範囲をさらに向上するために、３種類以上の学習データセットを連続的にトレーニングする。また、力センサの較正は、マニピュレータに取り付けられた6軸力トルクセンサ１１を使用することによってより簡単にすることができ、較正は、マニピュレータを所定の較正姿勢に移動させることによって実行される。
【実施例3】
【0071】
図１０を参照して、本発明の実施例３に係る液圧駆動システムと3D外力推定方法について説明する。
【0072】
図１０は、本実施例の連続体関節の構成を示す図である。実施例３は、液圧駆動システム８に関して、実施例１の基本構成と同じである。本実施例では、移動マニピュレータ自体は、一般的な回転関節だけでなく、少なくとも3D空間内で柔軟な動きを有する１つの連続体関節が追加されて構成されている。
【0073】
図１０では、連続体関節の上面図および側面図を用いてその構成を示している。上から下に向かって、１ユニットの連続体関節は４対のシリンダアクチュエータ１２１，１２２，１２３，１２４で構成され、アクチュエータの各対はロッド側と下部側が互いに接続されている。１ユニットの連続体関節の頂部１５１は、４対のシリンダアクチュエータ１２１，１２２，１２３，１２４を取り付けるプレートであり、このユニットの底部１５２も、別の４対のシリンダアクチュエータ１２５，１２６，１２７，１２８（符号１２７，１２８は図示せず）を取り付けるプレートである。
【0074】
これらのアクチュエータの入力ポートは対になって接続されているので、１つの電磁弁で２つのシリンダを同時に制御することができる。また、２本のロッドの間に１つのボールジョイント（図示せず）が取り付けられており、１対のアクチュエータが伸びたり縮んだり、曲がったりすることで移動できる。
【0075】
シリンダアクチュエータ１２４とその対のシリンダアクチュエータ１２８（図示せず）の圧力状態をシリンダアクチュエータ１２１と等しくし、その対のシリンダアクチュエータ１２５の圧力状態がシリンダ対１２２，１２６とシリンダ対１２３，１２７（図示せず）の圧力状態よりも大きいとすると、連続体関節はy軸を基準に曲がる。他の組合せによっても同様のメカニズムを発生させることができる。すべての圧力状態が等しい場合、アクチュエータは、前回の圧力状態に応じて、z軸上で伸縮する。他の対のシリンダアクチュエータよりも大きな圧力状態を有する一対のシリンダアクチュエータ１２４，１２８のみであれば、連続体関節はx-y軸において屈曲する。
【0076】
また、本実施例は、１ユニットの連続体関節だけに限定されるものではなく、図１０に示すように、入力がすべてのユニットに対して並列である連続体関節を複数ユニット積層することもできる。この構成を用いて、x-y-z平面で移動できる柔軟な動きを生成でき、各軸に加えられる力も、より少ない作動部品およびより少ない圧力測定ユニットおよび姿勢測定ユニットで推定できる。
【0077】
実施例１と同様に、本実施例では、リリーフ弁２３を追加することにより、推定器の精度を向上させることができる。また、リリーフ弁２３の出力部に圧力センサ３２が追加されており、調整された圧力を測定することができる。また、シリンダアクチュエータ１２のロッド室１４側に圧力センサ３３を追加することで、推定器の精度をさらに向上させることができる。また、ロッド室１４側に流量測定素子４２を追加することで、3D力推定器ユニット５０への入力に全ての流量測定素子を直接使用することになり、推定器の精度をさらに向上させることができる。
【0078】
また、実施例１と同様に、本実施例では、リリーフ弁２３の出力部に温度センサ（図示せず）を設置することで、推定器の推定可能範囲を拡大することができる。推定可能範囲をさらに向上するために、３種類以上の学習データセットを連続的にトレーニングする。また、力センサの較正は、マニピュレータに取り付けられた6軸力トルクセンサ１１を使用することによってより簡単にすることができ、較正は、マニピュレータを所定の較正姿勢に移動させることによって実行される。
【実施例4】
【0079】
本発明の実施例４に係る液圧駆動システムと3D外力推定方法について説明する。
【0080】
実施例４の基本構成は、実施例３と同様であるが、3D力推定ユニット５０は、力の３つの出力およびトルクの３つの出力を有する。この構成により、圧力ユニットと姿勢ユニットの数が少ない連続体関節を使用した場合でも、これらの６つの出力の推定が可能になる。
【0081】
実施例３と同様に、本実施例では、リリーフ弁２３を追加することにより、推定器の精度を向上させることができる。また、リリーフ弁２３の出力部に圧力センサ３２が追加されており、調整された圧力を測定することができる。また、シリンダアクチュエータ１２のロッド室１４側に圧力センサ３３を追加することで、推定器の精度をさらに向上させることができる。また、ロッド室１４側に流量測定素子４２を追加することで、3D力推定器ユニット５０への入力に全ての流量測定素子を直接使用することになり、推定器の精度をさらに向上させることができる。
【0082】
また、実施例３と同様に、本実施例では、リリーフ弁２３の出力部に温度センサ（図示せず）を設置することで、推定器の推定可能範囲を拡大することができる。推定可能範囲をさらに向上するために、３種類以上の学習データセットを連続的にトレーニングする。また、力センサの較正は、マニピュレータに取り付けられた6軸力トルクセンサ１１を使用することによってより簡単にすることができ、較正は、マニピュレータを所定の較正姿勢に移動させることによって実行される。
【0083】
なお、本発明は上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。
【符号の説明】
【0084】
１…移動マニピュレータ
２…流体ドライバ
３…圧力ユニット
４…姿勢ユニット
５，５０…3D力推定ユニット
６…コントローラ
７…インターフェース
８…液圧駆動システム
１０，１０ａ…ホース
１１…6軸力トルクセンサ
１２，１２ａ…シリンダアクチュエータ
１３，１３ａ…下部チャンバ
１４，１４ａ…ロッド室
１５，１５ａ…ピストン
２１，２１ａ，２２，２２ａ…電磁弁
２３…リリーフ弁
２４…モータポンプ（電気ポンプ）
２５…流体リザーバ
３１，３１ａ，３２，３３，３３ａ…圧力センサ
４１，４１ａ，４２，４２ａ…流量測定素子
５１，５１１…前処理演算部
５２，５２１…ニューラルネットワークの入力層
５３，５３１…ニューラルネットワークの隠れ層
５４，５４１…ニューラルネットワークの出力層
５５，５５１…後処理演算部
６０…モーションコントロールユニット
６１…モーメント長演算部
１１０…物体（負荷）
１１１…エンドエフェクタのモーメント長
１１２…負荷の重心の位置
１１３…エンドエフェクタと負荷の両方の重心の位置
１１４…環境
１２１，１２２，１２３，１２４，１２５，１２６，１２９，１３０，１３３，１３４…（連続体関節の）シリンダアクチュエータ
１４１，１４２…シリンダのロッド室への流体入口
１４５，１４６…シリンダの下部チャンバへの流体入口
１５１，１５３…連続体関節の頂部
１５２，１５４…連続体関節の底部
５０１…負荷に対する力（負荷の重量）
５０２…推定モデルの第１のトレーニング段階
５０３…環境との接触力（外乱力）
５０４…推定モデルの第２のトレーニング段階
５０５…負荷の重量及び外乱力
５０６…推定モデルの第３のトレーニング段階
６０１…確認ブロック
６０２…姿勢較正ブロック
６０３…エンドエフェクタの重心位置
６０４…終了確認制御ブロック
６１１…モーメント長位置
６５０，６５１…3D力推定ユニットの出力データ
２…流体ドライバ
３…圧力ユニット
４…姿勢ユニット
５，５０…3D力推定ユニット
６…コントローラ
７…インターフェース
８…液圧駆動システム
１０，１０ａ…ホース
１１…6軸力トルクセンサ
１２，１２ａ…シリンダアクチュエータ
１３，１３ａ…下部チャンバ
１４，１４ａ…ロッド室
１５，１５ａ…ピストン
２１，２１ａ，２２，２２ａ…電磁弁
２３…リリーフ弁
２４…モータポンプ（電気ポンプ）
２５…流体リザーバ
３１，３１ａ，３２，３３，３３ａ…圧力センサ
４１，４１ａ，４２，４２ａ…流量測定素子
５１，５１１…前処理演算部
５２，５２１…ニューラルネットワークの入力層
５３，５３１…ニューラルネットワークの隠れ層
５４，５４１…ニューラルネットワークの出力層
５５，５５１…後処理演算部
６０…モーションコントロールユニット
６１…モーメント長演算部
１１０…物体（負荷）
１１１…エンドエフェクタのモーメント長
１１２…負荷の重心の位置
１１３…エンドエフェクタと負荷の両方の重心の位置
１１４…環境
１２１，１２２，１２３，１２４，１２５，１２６，１２９，１３０，１３３，１３４…（連続体関節の）シリンダアクチュエータ
１４１，１４２…シリンダのロッド室への流体入口
１４５，１４６…シリンダの下部チャンバへの流体入口
１５１，１５３…連続体関節の頂部
１５２，１５４…連続体関節の底部
５０１…負荷に対する力（負荷の重量）
５０２…推定モデルの第１のトレーニング段階
５０３…環境との接触力（外乱力）
５０４…推定モデルの第２のトレーニング段階
５０５…負荷の重量及び外乱力
５０６…推定モデルの第３のトレーニング段階
６０１…確認ブロック
６０２…姿勢較正ブロック
６０３…エンドエフェクタの重心位置
６０４…終了確認制御ブロック
６１１…モーメント長位置
６５０，６５１…3D力推定ユニットの出力データ
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】