特開 2024-164946 ロボット制御システム、ロボット制御方法、および、ロボット制御装置

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024164946

(43)【公開日】2024-11-28

(54)【発明の名称】ロボット制御システム、ロボット制御方法、および、ロボット制御装置

(51)【国際特許分類】

   B25J 5/00 20060101AFI20241121BHJP

   B25J 13/00 20060101ALI20241121BHJP

   B25J 9/10 20060101ALI20241121BHJP

【ＦＩ】

B25J5/00 E

B25J13/00 Z

B25J9/10 A

B25J5/00 C

【審査請求】未請求

【請求項の数】13

【出願形態】ＯＬ

(21)【出願番号】P 2023080693

(22)【出願日】2023-05-16

(71)【出願人】

【識別番号】000005108

【氏名又は名称】株式会社日立製作所

(74)【代理人】

【識別番号】110000350

【氏名又は名称】ポレール弁理士法人

(72)【発明者】

【氏名】濱崎 寛太

(72)【発明者】

【氏名】伊藤 洋

(72)【発明者】

【氏名】保富 裕二 アンドレ

(72)【発明者】

【氏名】清水 自由理

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707AS13

3C707BS12

3C707BS27

3C707CS08

3C707HS27

3C707KS12

3C707KT01

3C707KT05

3C707LW08

3C707LW12

3C707WA03

3C707WA14

(57)【要約】

【課題】 目標動作を達成するための腕部状態と移動部状態の組合せが多い場合でも、適切な移動部制御則を容易に取得できるロボット制御システムを提供する。
【解決手段】 腕部と移動部を有するロボットと、前記ロボットを制御する制御装置と、を有するロボット制御システムであって、前記制御装置は、前記腕部の動作に関する目標動作および前記目標動作に応じた前記移動部の動作の許容範囲を出力する動作計画部と、前記目標動作に関する腕制御指令および腕部状態遷移を出力する腕制御部と、前記腕部状態遷移を用い、前記許容範囲に収まるように前記移動部の動作に関する移動制御指令を生成する移動制御部と、を備え、前記ロボットは、前記腕部が前記腕制御指令により制御され、前記移動部が前記移動制御指令により制御されることを特徴とするロボット制御システム。
【選択図】 図１

【特許請求の範囲】

【請求項1】

腕部と移動部を有するロボットと、
前記ロボットを制御する制御装置と、
を有するロボット制御システムであって、
前記制御装置は、
前記腕部の動作に関する目標動作および前記目標動作に応じた前記移動部の動作の許容範囲を出力する動作計画部と、
前記目標動作に関する腕制御指令および腕部状態遷移を出力する腕制御部と、
前記腕部状態遷移を用い、前記許容範囲に収まるように前記移動部の動作に関する移動制御指令を生成する移動制御部と、
を備え、
前記ロボットは、前記腕部が前記腕制御指令により制御され、前記移動部が前記移動制御指令により制御されることを特徴とするロボット制御システム。

【請求項2】

前記動作計画部は、前記許容範囲に前記移動部の動作の目標軌道を生成し、
前記移動制御部は、前記目標軌道との誤差が小さくなるように前記移動制御指令を生成することを特徴とする、請求項１に記載のロボット制御システム。

【請求項3】

前記ロボットの前記移動部は、脚部または回転翼を有し、
前記移動制御部は、前記脚部の関節または前記回転翼の状態に関する前記移動制御指令を生成することを特徴とする、請求項１に記載のロボット制御システム。

【請求項4】

前記腕制御部は、前記腕制御指令の状態遷移を用いて将来の腕制御指令を予測することにより前記腕部状態遷移を生成することを特徴とする、請求項１に記載のロボット制御システム。

【請求項5】

前記許容範囲は、前記移動部または前記腕部のエンドエフェクタの状態に対する下限と上限の少なくとも一方を設定することで生成されることを特徴とする、請求項１に記載のロボット制御システム。

【請求項6】

前記移動部または前記エンドエフェクタの状態は、位置、回転、並進速度、回転速度、並進加速度、回転加速度、並進加加速度、回転加加速度のいずれかであることを特徴とする、請求項５に記載のロボット制御システム。

【請求項7】

前記移動制御部は、前記許容範囲の中で前記移動部または前記エンドエフェクタの状態が変動する大きさを調整する重み行列に基づいて前記移動制御指令を生成することを特徴とする、請求項５に記載のロボット制御システム。

【請求項8】

前記許容範囲に関する情報を入力する許容範囲入力部と、
前記許容範囲を用いて前記ロボットが制御された結果を基に表示情報を演算する演算部と、
前記表示情報を表示する表示部と、
を備えることを特徴とする、請求項１に記載のロボット制御システム。

【請求項9】

前記表示情報は、
前記許容範囲、前記移動部又は腕部の状態、前記ロボットが消費した電力、前記腕部の前記目標動作の完了時間の予測のいずれかを含むことを特徴とする、請求項８に記載のロボット制御システム。

【請求項10】

前記許容範囲に関する情報には、許容範囲を設定する移動部または前記腕部のエンドエフェクタの状態、許容範囲の上限、許容範囲の下限のいずれかが含まれることを特徴とする、請求項８に記載のロボット制御システム。

【請求項11】

前記腕部の動作に関する腕部動作指令を入力する腕部動作指令入力部と、
前記移動部の動作の許容範囲を入力する移動部動作指令入力部と、
情報を取得するセンサと、
前記センサが取得した情報を表示する表示部と、を有し、
前記腕制御部は、前記腕部動作指令に関する腕制御指令と腕部状態遷移を生成することを特徴とする、請求項１に記載のロボット制御システム。

【請求項12】

腕部と移動部を有するロボットを制御するロボット制御方法であって、
前記腕部の動作に関する目標動作および前記目標動作に応じた前記移動部の動作の許容範囲を出力する動作計画ステップと、
前記目標動作に関する腕制御指令および腕部状態遷移を出力する腕制御ステップと、
前記腕部状態遷移を用い、前記許容範囲に収まるように前記移動部の動作に関する移動制御指令を生成する移動制御ステップと、
前記腕制御指令により前記腕部を制御し、前記移動制御指令により前記移動部を制御するロボット制御ステップと、
を備えることを特徴とするロボット制御方法。

【請求項13】

腕部と移動部を有するロボットを制御する制御装置であって、
前記腕部の動作に関する目標動作および前記目標動作に応じた前記移動部の動作の許容範囲を出力する動作計画部と、
前記目標動作に関する腕制御指令および腕部状態遷移を出力する腕制御部と、
前記腕部状態遷移を用い、前記許容範囲に収まるように前記移動部の動作に関する移動制御指令を生成する移動制御部と、
を備えることを特徴とするロボット制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ロボット制御システム、ロボット制御方法、および、ロボット制御装置に関する。

【背景技術】

【0002】

近年、少子高齢化に伴う労働力不足や、災害区域での危険作業などが社会課題となっている。それらの解決へ向け、ロボットが安全かつ効率的に作業できるように整備された工場等の環境以外（以下、「非整備環境」と呼ぶ）においても、様々な作業へのロボットの活用が期待されている。

【0003】

災害区域等の非整備環境下で、腕部と移動部を有するロボットが自律的に作業を遂行するには、操作対象物を巧みに操作する腕制御に加え、操作対象物の近傍まで適切に移動する移動制御が要求される。それらを実現したロボットとしては、例えば、マニピュレータを装備した、２輪駆動台車、クローラ型ロボット、四脚型ロボット、人型ロボット（ヒューマノイド）、無人航空機（ＵＡＶ）などが挙げられる。

【0004】

この種のロボットの一例としては、非特許文献１の移動型マニピュレータが知られている。非特許文献１では、移動型マニピュレータ（Legged Mobile Manipulator）の腕制御器（arm controller）と移動制御器（base controller）を別途用意し、腕の挙動に基づく移動部の制御則（制御指令）を深層強化学習（Deep reinforcement learning）により取得するロボット制御方法を提案している。

【先行技術文献】

【非特許文献】

【0005】

【非特許文献1】Y. Ma, F. Farshidian, T. Miki, J. Lee and M. Hutter, "Combining Learning-based Locomotion Policy with Model-based Manipulation for Legged Mobile Manipulators," arXiv:2201.03871v1, 2022.

【発明の概要】

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

【0006】

非整備環境でロボットが行う目標動作は多様であり、例えば、操作対象物の把持・運搬・設置、扉の開閉、レバー操作などの操作対象物の位置又は姿勢を変化させる動作や、溶接・塗装・切断・組立などの操作対象物を加工する動作などがある。

【0007】

非特許文献１では、腕制御器と移動制御器が独立しているため、腕部に新たな目標動作を実行させる場合には、腕制御器の制御則を変更するだけで対応することができた。また、腕部の制御測の変更には、既に確立されている腕部の動作生成技術を活用できるため、比較的簡単に新たな目標動作を実装することができた。

【0008】

しかしながら、非特許文献１の移動部の制御方法には、移動部の制御則を学習する際に考慮した条件下でしか、移動部を安定して制御できないという問題があった。したがって、目標動作を実行する際の腕部状態と移動部状態の組合せが多い場合には、移動部の適切な制御則を取得することが困難となる。例えば、腕部の到達範囲を広げるために移動部の姿勢を変化させるような制御を実行する場合には、腕部が動作したときの慣性力の影響を含めると、考慮しなければならない状態の組合せは膨大となるため、移動部の適切な制御則を取得することが困難であった。

【0009】

そこで、本発明は、目標動作を達成するための腕部状態と移動部状態の組合せが多い場合でも、適切な移動部制御則を容易に取得できるロボット制御システム、ロボット制御方法、および、ロボット制御装置を提供することを目的とする。

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

【0010】

上記課題を解決するため、本発明のロボット制御システムは、腕部と移動部を有するロボットと、前記ロボットを制御する制御装置と、を有するロボット制御システムであって、前記制御装置は、前記腕部の動作に関する目標動作および前記目標動作に応じた前記移動部の動作の許容範囲を出力する動作計画部と、前記目標動作に関する腕制御指令および腕部状態遷移を出力する腕制御部と、前記腕部状態遷移を用い、前記許容範囲に収まるように前記移動部の動作に関する移動制御指令を生成する移動制御部と、を備え、前記ロボットは、前記腕部が前記腕制御指令により制御され、前記移動部が前記移動制御指令により制御されるロボット制御システムとした。

【発明の効果】

【0011】

本発明のロボット制御システム、ロボット制御方法、および、ロボット制御装置によれば、目標動作を達成するための腕部状態と移動部状態の組合せが多い場合でも、適切な移動部制御則を容易に取得することができる。

【図面の簡単な説明】

【0012】

【図1】実施例１に係るロボット制御システムの構成を説明する図。

【図2A】ロボットの一例である四脚型ロボットを説明する図。

【図2B】ロボットの一例である人型ロボットを説明する図。

【図2C】ロボットの一例である無人航空機を説明する図。

【図3】実施例１に係る腕部の目標動作について説明する図。

【図4】実施例１に係る制御を実施したときのロボットと周辺環境の状態を説明する図。

【図5】実施例１に係る制御を実施したときの腕部のエンドエフェクタの位置の時間変化を説明する図。

【図6】実施例１に係る制御を実施したときの脚部の関節を駆動するモータへ出力された電力量の時間変化を説明する図。

【図7】実施例２に係る腕部の目標動作について説明する図。

【図8】実施例２に係るロボット制御システムの構成を説明する図。

【図9】実施例２に係る制御を実施したときのロボットと周辺環境の状態を説明する図。

【図10】実施例２に係る制御を実施したときの基部の移動速度の時間変化を説明する図。

【図11】実施例２に係る制御を実施したときの脚部の関節を駆動するモータへ出力された電力量の時間変化を説明する図。

【図12】実施例３に係るロボット制御システムの構成を説明する図。

【図13】実施例３に係る表示部の表示例を説明する図。

【図14】実施例４に係るロボット制御システムの構成を説明する図。

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

【0013】

以下、本発明を実施するための形態の例について、添付図面を参照しながら説明する。本発明は以下の実施形態に限定されるものではなく、実施形態における種々の数値等は例示である。また、本明細書及び図面において、同一の構成要素又は実質的に同一の機能を有する構成要素には同一の符号を付することとし、重複する説明は省略する。

【実施例0014】

図１は、本発明の実施例１に係るロボット制御システム１００の構成を説明する図である。このロボット制御システム１００は、制御装置１と、ロボット２と、許容範囲入力部３１を有するシステムである。制御装置１は、動作計画部１１と腕制御部１２と移動制御部１３で構成される。この制御装置１は、具体的には、ＣＰＵ等の演算装置、半導体メモリ等の主記憶装置、ハードディスク等の補助記憶装置、および、通信装置などのハードウェアを備えたコンピュータである。そして、演算装置が所定のプログラムを実行することで、動作計画部１１等の各機能部を実現するが、以下では、このような周知技術を適宜省略しながら説明する。また、ロボット２は、操作対象物を操作するマニピュレータ（腕部２１）と、移動時に利用する移動部２２で構成される。許容範囲入力部３１は、ロボット制御に必要な情報を制御装置１に入力する際にユーザーが利用する、ヒューマンマシンインターフェースである。

【0015】

図２Ａは、ロボット２の一例である四脚型ロボット２Ａの構成を説明する図である。この四脚型ロボット２Ａは、先端にエンドエフェクタ２１ａを有する６関節の腕部２１を１つ備える。また、四脚型ロボット２Ａの移動部２２には、３関節の脚部２２ａを４つと、腕部２１と脚部２２ａを取り付けた基部２２ｂを備える。

【0016】

図２Ｂは、ロボット２の一例である人型ロボット２Ｂの構成を説明する図である。この人型ロボット２Ｂは、先端にエンドエフェクタ２１ａを有する６関節の腕部２１を左右に備える。また、人型ロボット２Ｂの移動部２２には、６関節の脚部２２ａを２つと、腕部２１と脚部２２ａを取り付けた基部２２ｂを備える。

【0017】

図２Ｃは、ロボット２の一例である無人航空機２Ｃの構成を説明する図である。この無人航空機２Ｃは、先端にエンドエフェクタ２１ａを有する６関節の腕部２１を１つ備える。また、無人航空機２Ｃの移動部２２には、揚力および推力を発生させる回転翼２２ｃを４つと、腕部２１と回転翼２２ｃを取り付けた基部２２ｂを備える。

【0018】

四脚型ロボット２Ａと人型ロボット２Ｂは、腕部２１と脚部２２ａを駆動するための駆動機構が各関節に備えられている。無人航空機２Ｃは、腕部２１を駆動するための駆動機構が各関節に備えられ、移動部２２の姿勢を制御するための駆動機構が各回転翼２２ｃに備えられている。このとき、腕部２１や脚部２２ａの駆動部が関節数より少なく、複数の関節を１つの駆動部で制御するような機構であっても良い。また、腕部２１や脚部２２ａが有する関節の数や、腕部２１、脚部２２ａ、回転翼２２ｃの数は、図２Ａ～図２Ｃの例に限定されず増減しても差し支えない。加えて、それ以外の部位（例えば、尾部など）を備えていても問題ない。また、移動部２２が備える移動を実現するための機構は脚部２２ａや回転翼２２ｃに限定されず、例えば、車輪やクローラであっても良い。加えて、図１のロボット制御システム１００は、ロボット２が実環境に存在する場合と仮想環境に存在する場合のいずれも考慮することができる。

【0019】

＜腕部２１の制御方法＞
ここで、ロボット２の腕部２１の制御方法について説明する。まず、制御装置１の動作計画部１１は、腕部２１が実施する目標動作を腕制御部１２へ指令する。目標動作には、腕部２１が実行可能なあらゆる動作が該当する。例えば、操作対象物の移動（把持、運搬、設置、操作など）、加工（塗装、切断、溶接、組立、払拭など）、観測（計器読取り、表面検査、外観検査など）が該当する。

【0020】

腕制御部１２は、動作計画部１１から指令された目標動作に基づき、それを達成するための一連動作を生成する。一連動作の生成には任意の手法を適用することができる。例えば、事前に作成したプログラムに基づくルールベースの制御手法や、学習した制御則に基づく学習型の制御手法などがある。

【0021】

腕制御部１２で生成された一連動作は、任意の制御周期Ｔ_ａで腕制御指令Ｃ_１として腕部２１に入力され、腕制御指令Ｃ_１に基づき腕部２１が動作する。腕制御指令Ｃ_１は、腕部２１の各関節を回転又は並進させる指令であり、例えば、各関節の角度、角速度、角加速度（トルク）の時系列変化を指定するものである。腕制御指令Ｃ_１と腕部２１の実際の状態との差異や、周辺環境の状態に基づいて、腕制御部１２が動作を生成する場合には、腕制御部１２はロボット２が備える各種センサの情報を利用することができる。

【0022】

＜移動部２２の制御方法＞
次に、ロボット２の移動部２２の制御方法について説明する。腕部２１と移動部２２を備えたロボット２を対象とする場合、制御装置１の移動制御部１３は、腕部２１の目標動作を達成するために、動作計画部１１から指令される目標状態（例えば、基部２２ｂの位置又は姿勢）を追従するように移動部２２を制御する。このとき、移動制御部１３は、腕部２１が動作することによる慣性力の影響を考慮して、移動部２２を制御する必要がある。

【0023】

目標状態を追従するように移動部２２を制御するためには、移動部２２の実際の状態と目標状態との差分が小さくなる制御入力を求めればよい。制御入力ができるだけ小さくなるように問題を設定すると、所定の評価期間［ｔ_０，ｔ_ｆ］ついて次のような最適制御問題が得られる。

【0024】

【数1】

【0025】

【数2】

【0026】

【数3】

【0027】

ここで、φとＬは評価関数、ｘは状態ベクトル、ｕは入力ベクトル、ｘ_ｒｅｆは目標状態ベクトル、Ｐ，Ｑ，Ｒは重み行列である。ｘとｘ_ｒｅｆに移動部２２の状態に関する変数を含めることで目標状態を追従する制御を実現できる。また、評価関数Ｌに定義される項は式２の限りではない。例えば、電力消費量を評価する項や、ロボット２が自身や周辺環境との衝突を避けるための安全性を評価する項などを追加することができる。なお、目標状態ベクトルのｘ_ｒｅｆの時系列変化の軌跡によって状態ベクトルｘの目標軌道を定義することができる。

【0028】

式１～３の最適制御問題を解くためには、状態ベクトルｘの時間発展を推定する必要がある。そこで、式１～３の最適制御問題に対して次の制約条件を定義できる。

【0029】

【数4】

【0030】

【数5】

【0031】

【数6】

【0032】

【数7】

【0033】

ここで、式４はロボット２の動力学モデル、式５は初期条件、式６は等式制約、式７は不等式制約である。

【0034】

式１～７の最適制御問題を、ロボット２の制御開始前にオフラインで解けば、最適化された一連の制御入力を事前に求めることができる。一方で、非整備環境でロボット２を制御する場合には、動力学モデルの誤差や外乱の影響により、オフラインで事前に想定した通りに状態が遷移するとは限らない。

【0035】

そこで、本実施例では、移動制御部１３が各制御周期Ｔ_ｌにおいて、その時刻で観測されたロボット状態を式５の初期条件とし、現時刻ｔ_０から有限時間先の時刻ｔ_ｆまでについて式１～７の最適制御問題を解き、得られた制御入力のうちｔ_０における結果のみを制御指令Ｃとして使用する制御手法を用いることとした。この手法は一般にモデル予測制御と呼ばれる。

【0036】

以下の実施例では、一般的なモデル予測制御を用いた場合を想定して説明するが、式１～７の最適制御問題に係る別の制御手法に置き換えることも可能である。例えば、動力学モデルの誤差や外乱の影響が限りなく小さい場合や、ロボット２をシミュレータ上で制御するような場合には、式１～７の最適制御問題をオフラインで解いた結果より、一連の制御入力を決定できる。あるいは、ロボット２の重心に生じる力及びトルクを算出した後に、各関節又は回転翼２２ｃに出力する制御入力を算出する階層的な算出方法や、式１～７の最適制御問題におけるパラメータ（重み行列、評価期間、目標状態など）の一部を機械学習により取得する方法なども採用できる。また、モデル予測制御には、微分動的計画法、Multiple shooting法などを用いた勾配ベースの手法や、モンテカルロ法や経路積分などを用いたサンプリングベースの手法があるが、いずれの手法を用いても良い。また、式１～７の最適制御問題における時刻ｔ_ｆの値は任意であり、大きな値を設定することで、より将来の状態を考慮した制御指令Ｃを算出する事ができる。一方で、移動制御部１３の制御周期Ｔ_ｌについてｔ_ｆ＝ｔ_０＋Ｔ_ｌを設定し、ロボット２に対する次の制御入力とそれによる状態変化のみを考慮して制御指令Ｃを算出することもできる。

【0037】

以下では、具体例として、式１～７の最適制御問題を、四脚型ロボット２Ａへ適用した場合について説明する。四脚型ロボット２Ａを制御対象とする場合、式４における動力学モデルは次のように表すことができる。

【0038】

【数8】

【0039】

【数9】

【0040】

ここで、ｐ_ｃｏｍ、ｌ_ｃｏｍは、四脚型ロボット２Ａの重心の並進運動量、及び、角運動量であり、基部２２ｂの位置ｘ_ｂ、回転θ_ｂ、並進速度ｖ_ｂ、回転速度ω_ｂ、脚部２２ａ及び腕部２１の関節角θ_ｌ及びθ_ａ、関節角速度ω_ｌ及びω_ａに依存する関数である。また、ｎ_ｌ及びｎ_ａは脚部２２ａ及び腕部２１の接触点の数、ｒ_ｌ，ｉ及びｒ_ａ，ｉは重心から脚部２２ａ及び腕部２１の接触点ｉまでの長さ、ｆ_ｌ，ｉ及びｆ_ａ，ｉは脚部２２ａ及び腕部２１の接触点ｉに生じる接触力、τ_ｌ，ｉ及びτ_ａ，ｉは脚部２２ａ及び腕部２１の接触点ｉに生じる接触トルク、ｍｇは重力により重心に生じる力である。

【0041】

なお、動力学モデルを記述する方法は式８、９に限定されない。例えば、動力学モデルをより詳細に記述するため、式８へ遠心力やコリオリ力に関する項を追加する場合や、演算量を削減するために脚部２２ａの慣性力に関する項を省略する場合などを採用できる。

【0042】

また、四脚型ロボット２Ａを制御対象とする場合、式６、７における等式制約と不等式制約は次のように与えることができる。

【0043】

【数10】

【0044】

【数11】

【0045】

【数12】

【0046】

ここで、ｖ_ｌ，ｉは脚部２２ａの接触点ｉの速度、ｆ_ｌ，ｉ ^ｘ、ｆ_ｌ，ｉ ^ｙ、ｆ_ｌ，ｉ ^ｚは接触点ｉにおいて接触面に対して接線方向ｘ、ｙ、法線方向ｚに生じる力である。式１０、１１は接触点ｉが接触面を滑らないための条件を定義している。また、式６の等式制約や式７の不等式制約は四脚型ロボット２Ａの性能や目標動作に応じて追加、削除することができる。例えば、脚部２２ａの関節角や関節角速度の上下限を不等式制約として追加することができる。

【0047】

以上に示した、四脚型ロボット２Ａを制御対象とする場合における、式８、９の動力学モデルと式１０～１２の等式制約及び不等式制約を用い、式１～７の最適制御問題を解くことで、四脚型ロボット２Ａの基部２２ｂの状態ｘ_ｂ、ｖ_ｂ、θ_ｂ、ω_ｂのいずれかが目標状態ｘ_{ｂ，ｒｅｆ}、ｖ_{ｂ，ｒｅｆ}、θ_{ｂ，ｒｅｆ}、ω_{ｂ，ｒｅｆ}を追従するような、脚部２２ａの関節角θ_ｌ（又は関節角速度ω_ｌ、関節角加速度α_ｌ）を算出し、移動制御指令Ｃ_２として移動部２２へ出力することができる。

【0048】

ここで、式１～７の最適制御問題に基づくモデル予測制御を実環境で行う場合には、動力学モデルや外乱の影響により、四脚型ロボット２Ａや周辺環境の状態は予測値と実測値とで差異が生じる。そこで、四脚型ロボット２Ａに備えられたセンサ情報（例えば、脚部２２ａの関節に備えられたエンコーダから取得される関節状態や、基部２２ｂに備えられた慣性計測ユニットから取得される姿勢情報など）を移動制御部１３へフィードバックし、制御指令Ｃの補正や式５の初期条件として利用することができる。

【0049】

さらに、式８、９の動力学モデルに現れる腕部２１の関節角θ_ａ（又は関節角速度ω_ａ、関節角加速度α_ａ）として、腕制御部１２が生成した腕部状態遷移θ_ａ ^＊（又は関節角速度遷移ω_ａ ^＊、関節角加速度遷移α_ａ ^＊）を設定するか、あるいは式１～７の最適制御問題における式６の等式制約として式１３、式１４、式１５を追加することで、腕部２１の慣性力による影響を考慮した移動制御指令Ｃ_２の生成が可能となる。

【0050】

【数13】

【0051】

【数14】

【0052】

【数15】

【0053】

ここで、式１３、式１４、式１５のうちいずれかが省略される場合も考えられる。また腕部２１のエンドエフェクタ２１aに負荷が掛かる場合には、エンドエフェクタ２１aに生じる力ｆａに関する等式制約を追加することもできる。このとき、腕制御部１２が腕制御指令Ｃ_１を算出する過程で、例えば、腕制御部１２がモデル予測制御を用いる場合や、環境状態の予測に基づく学習型制御を用いる場合のように、評価期間［ｔ_０，ｔ_ｆ］における腕部状態遷移θ_ａ ^＊を取得できる場合は、取得された腕部状態遷移θ_ａ ^＊をそのまま移動制御部１３へ出力することができる。一方で、腕制御部１２が腕制御指令Ｃ_１を算出する過程で、評価期間［ｔ_０，ｔ_ｆ］における腕部状態遷移θ_ａ ^＊を取得しない場合には、例えば、過去の制御指令Ｃ_Ｐを参照して将来の制御指令Ｃ_Ｆを外挿することで、腕部状態遷移θ_ａ ^＊を推定することができる。

【0054】

式１～７の最適制御問題において式２に現れる状態変数ｘ_ｉの重みＱ_ｉを大きく設定すると、状態変数ｘ_ｉの目標状態ｘ_{ｉ，ｒｅｆ}に対する追従性を高めることができる。一方で、設定されたパラメータ（重み行列、評価期間、目標状態など）に基づく最適制御問題では、移動部２２の状態を目標状態へ追従させるための解が算出できない場合がある。その場合、移動部２２の状態が目標状態から逸脱し、腕部２１の目標動作が失敗したり、移動部２２が安定姿勢を保てずに転倒したりする恐れがある。あるいは、移動部２２の状態が目標状態を追従できたとしても、移動部２２が目標状態を維持するために、脚部２２ａの関節へ大きな負担が生じ、過剰な電力を消費したり、駆動部の劣化を早めたりする場合がある。移動部２２の状態が目標状態を追従でき、尚且つ駆動部への負荷が小さくなるようなパラメータを事前に調整する方法も考えられる。

【0055】

しかし、操作対象物や周辺環境の状態に関する完全な情報を事前に取得できなかったり、それらの状態が動的に変化したりするような状況では、全ての状況に対して有効なパラメータを事前に用意することは困難である。また、そのようなパラメータを、ロボット２の制御中にオンラインでリアルタイム演算することも現実的でない。

【0056】

この課題を解決するため、本実施例では、腕部２１の目標動作の特性に基づいて、式１～７の最適制御問題における状態変数に対し、許容範囲を予め設定する以下の方法を提案する。

【0057】

そもそも、腕部２１の目標動作を実行するうえで、移動部２２の状態が目標状態を厳密に追従することは必須ではない。例えば、目標動作が操作対象物の加工であり、加工結果がある範囲内に収まっていれば要求品質を満たすような場合には、その範囲内で加工が行えるように移動部２２を制御できれば十分である。また、腕制御部１２がロボット２の備えるセンサの情報のフィードバックに基づいて腕部２１を制御する場合も、移動部２２の状態が所定範囲内に収まっていれば、移動部２２の実際の状態と目標状態の差分に基づいて腕制御指令Ｃ_１を補正し、目標動作を達成すれば良い。

【0058】

したがって、腕部２１の目標動作の特性を考慮することにより、腕部２１の目標動作を達成するための許容範囲を状態変数に対して予め設定することができる。これにより、許容範囲を満たす範囲で移動部２２の状態を選択できるようになり、腕部２１の目標動作を達成するための解を算出できる確率が大きくなる。更には、移動部２２を制御する際の消費電力量や駆動部への負荷低減を期待することができる。

【0059】

腕部２１の目標動作を達成するための、状態変数ｘの許容範囲設定方法は、例えば次の２つである。

【0060】

第１の許容範囲設定方法は、式１～７の最適制御問題における式７の不等式制約として、次式を追加する方法である。

【0061】

【数16】

【0062】

ここで、ｘ_ｉは許容範囲を設定する状態変数であり、ｘ_ｍｉｎ及びｘ_ｍａｘは腕部２１の動作目標を達成するために要求されるｘ_ｉの下限値と上限値である。このとき、状態変数ｘ_ｉは移動部２２の状態以外の変数でも良い。例えば、状態変数ｘ_ｉとして腕部２１の接触点の位置又は姿勢を設定することもできる。また、複数の状態変数に対して式１３の許容範囲を設定しても良い。なお、この場合の下限値ｘ_ｍｉｎと上限値ｘ_ｍａｘは、許容範囲入力部３１を介してユーザーが入力したものであっても良いし、機械学習などを利用して動作計画部１１が自動生成したものであっても良い。

【0063】

第２の許容範囲設定方法は、式１～７の最適制御問題において、式２の評価関数における重み行列を調整する方法である。例えば、状態変数ｘ_ｉに関する重みＱ_ｉを小さくすることで、目標状態ｘ_{ｉ，ｒｅｆ}への追従性を小さくできるので、式１～７の最適制御問題を解いたときに式１３が満たされるようにＱ_ｉを設定することで、腕部２１の目標動作を達成できる移動制御指令Ｃ_２を算出することができる。なお、この場合の重みＱ_ｉは許容範囲入力部３１を介してユーザーが入力したものであっても良いし、機械学習などを利用して動作計画部１１が自動生成したものであっても良い。

【0064】

また、これら２つの許容範囲設定方法を組合わせることも可能である。例えば、式１３の許容範囲をｘ_ｉに対して明示的に設定した後に、設定された許容範囲内でのｘ_ｉの変化のし易さを、Ｑ_ｉを用いて調整することが可能である。

【0065】

本実施例では四脚型ロボット２Ａを対象として具体例を説明したが、式４の動力学モデルや式６、７の等式制約及び不等式制約をロボット２の形態に応じて置き換えることで、ロボット２が人型ロボット２Ｂや無人航空機２Ｃである場合へも拡張することができる。

【0066】

例えば、人型ロボット２Ｂを制御する場合には、脚部２２ａ及び腕部２１の接点数をｎ_ｌ及びｎ_ａへ設定し、基部２２ｂと頭部を纏めて胴部として扱えば、式８、式９の動力学モデルや、式１０、式１１、式１２の制約条件をそのまま利用することができる。

【0067】

また、無人航空機２Ｃを制御する場合には、式４の動力学モデルを、慣性座標系における並進運動方程（式１７）と、無人航空機２Ｃの重心を原点とした機体座標系における回転運動方程（式１８）で表せば良い。

【0068】

【数17】

【0069】

【数18】

【0070】

ここで、ｐ_ｃｏｍ、ｌ_ｃｏｍは、無人航空機２Ｃの重心の並進運動量、及び、角運動量であり、基部２２ｂの位置ｘ_ｂ、回転角θ_ｂ、並進速度ｖ_ｂ、回転速度ω_ｂ、腕部２１の関節角θ_ａ、関節角速度ω_ａに依存する関数である。また、ｎ_ａは基部２２ｂの接触点の数、ｒ_ａ，ｉは重心から腕部２１の接触点ｉまでの長さ、ｆ_ａ，ｉは腕部２１の接触点ｉに生じる接触力、τ_ａ，ｉは腕部２１の接触点ｉに生じる接触トルク、ｍｇは重力により重心に生じる力である。また、Ｒは機体座標系から慣性座標系に変換するための回転行列、Ｆ_ｗ及びτ_ｗは回転翼２２ｃの回転により無人航空機２Ｃへ発生する推力及びトルクである。推力Ｆ_ｗ及びトルクτ_ｗは回転翼２２ｃの回転数ω_ｗの二乗に比例する。

【0071】

なお、無人航空機２Ｃの動力学モデルの記述方法は、式１７、式１８に限定されず、例えば、式１７の並進運動方程式へ空気抵抗に関する項を追加したり、式１８の回転運動方程式へ空気の粘性抵抗に関する項を追加したりできる。また、式６、式７における制約条件は必ずしも与える必要はないが、例えば、回転翼２２ｃの回転数ω_ｗの上限を式７の不等式制約として与えることができる。

【0072】

以上により、式１７、式１８の動力学モデルを用いて式１～７の最適制御問題を解くことで、基部２２ｂの状態ｘ_ｂ、ｖ_ｂ、θ_ｂ、ω_ｂのいずれかが目標状態ｘ_{ｂ，ｒｅｆ}、ｖ_{ｂ，ｒｅｆ}、θ_{ｂ，ｒｅｆ}、ω_{ｂ，ｒｅｆ}を追従するような、無人航空機２Ｃの回転翼２２ｃの回転数ω_ｗ（又は各回転翼２２ｃの推力、各回転翼２２ｃの角速度など）を算出し、移動部制御指令として移動部２２へ出力することができる。

【0073】

＜本実施例のロボット制御システムの具体的な運用＞
次に、図３から図６を用いて、ロボット２が四脚型ロボット２Ａである場合の、ロボット制御システム１００の具体的な運用を説明する。

【0074】

図３は、ロボット２（四脚型ロボット２Ａ）が壁面Ｗに対して水平方向（ｘ方向）に移動しながら、腕部２１が保持する塗装スプレーＳで、壁面Ｗの塗装領域Ｗａを塗装する目標動作について説明する図である。以下では、図３の目標動作を例に、目標動作を達成するための許容範囲の定め方や、許容範囲を式１３の不等式として式１～７の最適制御問題へ設定する方法について説明する。

【0075】

図３に示す目標動作を達成するため、制御装置１の腕制御部１２は、まず、ロボット２の腕部２１に対し、腕部２１の関節Ｊａ～Ｊｄ、Ｊｆが図３の初期状態を維持するような制御指令を与える。残りの関節Ｊｅについては、センサ情報（基部２２ｂの姿勢情報、視覚センサ情報など）のフィードバックに基づき、塗装スプレーＳの噴射方向が壁面Ｗに対して常に垂直方向（ｙ軸方向）を向くように制御指令を与える。本目標動作は、これらの制御指令を統合した腕制御指令Ｃ_１のもとで、移動部２２が壁面Ｗに対して水平方向（ｘ方向）へ移動する事で達成できる。ここで、塗装スプレーＳの噴射口から壁面Ｗまでの距離が所定の範囲に収まっていれば、噴射された塗料が拡散せずに、本目標動作の要求品質を満たせるものとする。

【0076】

本目標動作の要求品質を満たすため、ロボット２の基部２２ｂの姿勢（ｘ_ｂｘ，ｘ_ｂｙ，ｘ_ｂｚ，θ_ｂｘ，θ_ｂｙ，θ_ｂｚ）に対し、（ｘ_{ｂｘ，ｒｅｆ}，ｘ_{ｂｙ，ｒｅｆ}，ｘ_{ｂｚ，ｒｅｆ}，θ_{ｂｘ，ｒｅｆ}，θ_{ｂｙ，ｒｅｆ}，θ_{ｂｚ，ｒｅｆ}）＝（ｖ_ｘ，０ｔ、ｘ_ｂｙ，０，ｘ_ｂｚ，０，θ_ｂｘ，０，θ_ｂｙ，０，θ_ｂｚ，０）のように目標姿勢を設定できる。このとき、ｘ_{ｂｘ，ｒｅｆ}は基部２２ｂが一定速度ｖ_ｘ０でｘ方向へ移動するような目標姿勢、それ以外は初期値を維持するような目標姿勢を表している。

【0077】

加えて、塗装という目標動作の要求品質を満たすため、腕部２１のエンドエフェクタ２１ａのｙ方向の位置ｘ_ａｙに対して、式１３の不等式制約としてｘ_{ａｙ，ｍｉｎ}≦ｘ_ａｙ≦ｘ_{ａｙ，ｍａｘ}を設定できる。ここで、要求品質を満たせれば、基部２２ｂの位置又は姿勢などの他の状態変数に対して式１３の不等式制約を設定しても良い。また、壁面Ｗの鉛直方向（ｚ方向）に対する塗装のズレが許容できる場合には、腕部２１のエンドエフェクタ２１ａのｚ方向の位置ｘ_ａｚに対して不等式制約ｘ_{ａｚ，ｍｉｎ}≦ｘ_ａｚ≦ｘ_{ａｚ，ｍａｘ}を設定することもできる。あるいは、ｘ_ａｙの変化による塗料の拡散も、壁面Ｗの鉛直方向に対する塗装のズレに影響するとして、不等式制約としてｘ_{ａｙ－ｚ，ｍｉｎ}≦Ａｘ_ａｙ＋Ｂｘ_ａｚ≦ｘ_{ａｙ－ｚ，ｍａｘ}を設定することもできる。ここでＡ、Ｂは比例定数である。

【0078】

図１のロボット制御システム１００の構成において、本目標動作を達成する手順は次の通りである。

【0079】

まず、動作計画部１１が腕制御部１２に対して本目標動作の実行を指令する。同時に、動作計画部１１は移動制御部１３に対し、基部２２ｂに対する前記目標姿勢と、腕部２１のエンドエフェクタ２１ａの位置に対する前記許容範囲を出力する。

【0080】

続いて、腕制御部１２は、本目標動作を達成するための腕制御指令Ｃ_１を制御周期Ｔ_ａで腕部２１へ出力する。それと同時に、腕制御部１２は、移動制御部１３に対して、腕部状態遷移θ_ａ ^＊の情報を出力する。本目標動作の場合には、時間で変化しない腕部２１の関節Ｊの状態に関する情報を腕部状態遷移θ_ａ ^＊の情報として出力できる。このとき、実際はセンサ情報のフィードバックに基づいて、腕部２１の関節Ｊｅの角度が変化するように制御を行っているが、関節Ｊｅの角度の変化による慣性力の変化は小さく、移動部２２のダイナミクスに対する影響は小さいと仮定している。

【0081】

そして、移動制御部１３は、式１～７の最適制御問題に基づくモデル予測制御を用いて、腕部状態遷移θ_ａ ^＊の情報を考慮しつつ、基部２２ｂの目標姿勢を追従するような移動制御指令Ｃ_２を算出し、制御周期Ｔ_ｂで移動部２２へ出力する。これにより、移動部２２は、腕部状態遷移θ_ａ ^＊の情報を踏まえて適切に制御されることになる。

【0082】

図４～図６は、図３の目標動作を実行するようにロボット２を制御したときの制御結果を説明する図である。図４は、ロボット２と周辺環境の概略図、図５は、腕部２１のエンドエフェクタ２１ａの位置ｘ_ａｙの時間変化、図６は、脚部２２ａの関節を駆動するモータへ出力した電力量の時間変化を示す。

【0083】

＜＜本発明を適用しない場合（許容範囲を考慮しない場合）＞＞
まず、制御装置１の移動制御部１３が、エンドエフェクタ２１ａの位置ｘ_ａｙに対する許容範囲を考慮しない場合のロボット制御結果について説明する。この場合、ロボット２の基部２２ｂの姿勢が目標姿勢を追従することで、図４左図のように、基部２２ｂが初期姿勢を維持し続ける。これにより、図５における許容範囲を考慮しない場合の結果５ａのように、エンドエフェクタ２１ａの位置ｘ_ａｙを許容範囲５ｃに収めることができる。

【0084】

ただし、図４左図の状態では、ロボット２の腕部２１の重心は、移動部２２の重心よりｙ軸の正方向に位置しており、その状態の腕部２１に起因して移動部２２をｘ軸周りに回転させるモーメントが発生している。そのため、このモーメントに抗いつつ基部２２ｂの姿勢を目標姿勢のまま維持すると、図６における許容範囲を考慮しない場合の結果６ａのように、脚部２２ａの関節を駆動するモータでの消費電力量が大きくなってしまう。このように、エンドエフェクタ位置の許容範囲を考慮せずにロボット２を制御すると、ロボット２で過剰な電力を消費したり、脚部２２ａの関節劣化が早まったりする恐れがある。

【0085】

＜＜本発明を適用した場合（許容範囲を考慮する場合）＞＞
次に、制御装置１の移動制御部１３が、エンドエフェクタ２１ａの位置ｘ_ａｙに対する許容範囲を考慮する場合の制御結果について説明する。この場合、図４右図のように、ロボット２の基部２２ｂの姿勢がθ_ｂｘ方向に回転しているが、図５における許容範囲を考慮する場合の結果５ｂのように、エンドエフェクタ２１ａの位置ｘ_ａｙを許容範囲５ｃに収めることができている。

【0086】

このとき、図４右図のように、ロボット２の基部２２ｂの姿勢をθ_ｂｘ方向に回転させることで、ｙ軸上での腕部２１の重心位置を移動部２２の重心位置と略一致させることができる。これにより、腕部２１に起因するｘ軸周りのモーメントを抑制できるため、図６における許容範囲を考慮する場合の結果６ｂのように、脚部２２ａの関節を駆動するモータでの消費電力量を小さくすることができる。その結果、本目標動作を達成するための電力消費量や脚部２２ａの関節への負荷を低減することができる。

【0087】

以上の図３から図６では、スプレー塗装を目標動作として挙げたが、目標動作をローラーや刷毛を使った塗装、研磨、溶接、払拭などに変更しても、同様な手順で許容範囲を設定することができる。このとき、研磨や払拭の場合には、腕部２１のエンドエフェクタ２１ａが壁面Ｗへ接する必要があるため、壁面Ｗの法線方向（ｙ方向）の位置ｘ_ａｙに対する許容範囲を小さく設定するなどの調整が必要である。また、壁面Ｗとの接触により、腕部２１のエンドエフェクタ２１ａへ外力が生じる場合には、腕部２１に備えられた力覚センサなどにより外力を取得して、式９の動力学モデルに現れるｆ_ａへ設定することで、腕部２１に生じる外力を考慮した移動部制御が可能である。

【0088】

以上で説明したように、本実施例のロボット制御システムによれば、状態変数の許容範囲を予め設定することで、目標動作を達成するための腕部状態と移動部状態の組合せが多い場合でも、適切な移動部制御則を容易に取得することができる。

【実施例0089】

次に、図７から図１１を用いて、本発明の実施例２に係るロボット制御システム１００を説明する。なお、実施例１との共通点は重複説明を省略する。

【0090】

図７は、ロボット２（四脚型ロボット２Ａ）が位置ｘ_ｓからｘ軸に沿って移動し、操作対象物Ｏｂの近傍を通過する際に、位置ｘ_ｍで腕部２１が操作対象物Ｏｂを把持する目標動作を説明する図である。本実施例では、本目標動作を具体例に、腕部２１の状態に基づいて許容範囲が変化する場合について、本目標動作を達成するための許容範囲の定め方や、許容範囲を式１３の不等式として式１～７の最適制御問題への設定する方法について説明する。

【0091】

本目標動作を達成するために、腕制御部１２は、まず、ロボット２に備えられたカメラ２３が撮像した周辺環境の画像情報Ｉ_１より、操作対象物Ｏｂの状態を認識する。続いて、操作対象物Ｏｂに関する時系列情報を基に、操作対象物Ｏｂの将来の軌道を予測し、操作対象物Ｏｂを把持するための腕部２１の目標軌道を生成する。これは、ベルトコンベアなどの上を移動する操作対象物Ｏｂを把持するための腕部２１の制御方法と等価である。そのため、操作対象物Ｏｂが移動する代わりに、腕部２１と接続された移動部２２を移動させる事で、腕部２１に対する既存の制御手法を腕制御部１２における制御方法としてそのまま利用することができる。

【0092】

ここで、腕制御部１２は、操作対象物Ｏｂを捉えたカメラ２３の時系列の画像情報Ｉ_１から、移動部２２が現在位置からある速度で移動した場合に、腕部２１が操作対象物Ｏｂを把持できるかを評価できるものとする。その場合、腕部２１が操作対象物Ｏｂを把持するための、移動部２２の速度の上限ｖ_{ｂｘ，ｍａｘ}を決定することができる。このとき、腕部２１のエンドエフェクタ２１ａと操作対象物Ｏｂとの間の距離ｒ_ａ－ｏが近づくほど、操作対象物Ｏｂの予測軌道に応じた、腕部２１のエンドエフェクタ２１ａの先回りができなくなるため、ｖ_{ｂｘ，ｍａｘ}は小さくなる。このように、ｖ_{ｂｘ，ｍａｘ}はｒ_ａ－ｏに依存して変化するため、本目標動作を達成するために設定する許容範囲もｒ_ａ－ｏに基づき変化することになる。

【0093】

以上により、基部２２ｂの姿勢（ｘ_ｂｘ，ｘ_ｂｙ，ｘ_ｂｚ，θ_ｂｘ，θ_ｂｙ，θ_ｂｚ）に対する時刻ｔ＋Δｔでの目標姿勢は、（ｘ_{ｂｘ，ｒｅｆ}，ｘ_{ｂｙ，ｒｅｆ}，ｘ_{ｂｚ，ｒｅｆ}，θ_{ｂｘ，ｒｅｆ}，θ_{ｂｙ，ｒｅｆ}，θ_{ｂｚ，ｒｅｆ}）＝（ｘ_ｂｘ，ｔ＋ｖ_{ｂｘ，ｍａｘ}（ｔ）Δｔ、ｘ_ｂｙ，０，ｘ_ｂｚ，０，θ_ｂｘ，０，θ_ｂｙ，０，θ_ｂｚ，０）のように設定できる。ここで、ｘ_ｂｘ，ｔは時刻ｔにおける基部２２ｂのｘ軸上の位置、ｖ_{ｂｘ，ｍａｘ}（ｔ）は時刻ｔにおける移動部２２の速度の上限である。加えて、腕部２１が操作対象物Ｏｂを把持するための許容範囲は、基部２２ｂの並進速度ｖ_ｂｘに対して、式１３の不等式制約としてｖ_{ｂｘ，ｍｉｎ}≦ｖ_ｂｘ≦ｖ_{ｂｘ，ｍａｘ}のように設定できる。ここで、ｖ_{ｂｘ，ｍｉｎ}はｖ_{ｂｘ，ｍａｘ}より小さな任意の速度を設定すれば良い。ここでは、最小限の移動速度の保証として、ｖ_{ｂｘ，ｍｉｎ}＝ｖ_{ｂｘ，ｍａｘ}／２を設定する。

【0094】

図８は、本目標動作を達成するためのロボット制御システム１００の構成を説明する図である。図８のロボット制御システム１００の構成において、本目標動作を達成する手順は次の通りである。

【0095】

まず、動作計画部１１が腕制御部１２に対して本目標動作の実行を指令する。次に、腕制御部１２が、カメラ２３で撮像した画像情報Ｉ_１に基づいて腕制御指令Ｃ_１を生成し、制御周期Ｔ_ａで腕部２１へ出力する。それと同時に、腕制御部１２は腕部状態遷移θ_ａ ^＊と、それに基づく許容範囲ｖ_{ｂｘ，ｍａｘ}の遷移を生成し、腕部状態遷移θ_ａ ^＊を移動制御部１３、許容範囲遷移を動作計画部１１へ出力する。この腕部状態遷移θ_ａ ^＊は、例えば、操作対象物Ｏｂが一定速度で移動すると仮定して、腕部２１が将来取るべき目標軌道を予測することにより生成できる。

【0096】

続いて、動作計画部１１は、許容状態遷移の情報に基づいて、基部２２ｂの姿勢に対する目標姿勢と、ｖ_ｂｘに対する許容範囲を決定し、それらを移動制御部１３へ出力する。そして、移動制御部１３は式１～７の最適制御問題に基づくモデル予測制御を用いて、腕部状態遷移θ_ａ ^＊の情報を考慮し、基部２２ｂの目標姿勢を追従するような移動制御指令Ｃ_２を算出し、制御周期Ｔ_ｂで移動部２２へ出力する。このとき、移動制御指令Ｃ_２が変化すると、カメラ２３が捉える操作対象物Ｏｂの位置も変化するため、腕制御部１２が生成する腕制御指令Ｃ_１が修正される。したがって、移動制御部１３が式１～７の最適制御問題に基づいて移動制御指令Ｃ_２を算出する過程で、腕部状態遷移θ_ａ ^＊が変化してしまう。

【0097】

正確な腕部状態遷移θ_ａ ^＊を考慮するために、式１～７の最適制御問題で最適化計算を行う過程で、腕部状態遷移θ_ａ ^＊を逐次修正することもできる。例えば、腕部状態を腕部２１のエンドエフェクタ２１ａと操作対象物Ｏｂとの間の距離ｒ_ａ－ｏに関する関数として近似することで、低い計算コストで腕部状態遷移θ_ａ ^＊を修正できる。一方で、モデル予測制御を用いる場合、更新された腕部状態遷移θ_ａ ^＊に基づく移動制御指令Ｃ_２の算出が制御周期Ｔ_ｂで実行されるため、腕部状態遷移θ_ａ ^＊がある程度正確でなくとも、最適解から大きくは外れない移動制御指令Ｃ_２を算出することができる。

【0098】

ここで、腕制御部１２の制御周期Ｔ_ａと移動制御部１３の制御周期Ｔ_ｂが異なる場合、腕部状態遷移θ_ａ ^＊と許容範囲遷移の更新周期はＴ_ａ、Ｔ_ｂのいずれであっても良い。動作計画部１１が移動制御部１３へ目標姿勢と許容範囲を出力する周期については、Ｔ_ａ又はＴ_ｂと同じか、あるいはそれ以外の周期で合って良い。

【0099】

また、ロボット２と操作対象物Ｏｂとの間の距離が大きく、腕制御部１２が目標動作を実行できない場合が考えられる。その場合、動作計画部１１に対してカメラ２３の画像情報Ｉ_１をフィードバックし、ロボット２と操作対象物Ｏｂとの間の距離がある基準値より小さくなったタイミングで、腕制御部１２に対する目標動作の指令を開始するような方法を採用することができる。このとき、腕制御部１２に対する目標動作の指令を開始するまでは、操作対象物Ｏｂへ向かってある速度で移動するような目標位置又は目標姿勢を移動制御部１３に対して指令する事ができる。

【0100】

図９、図１０、図１１は本目標動作に対してロボット２を制御したときの制御結果を説明する図である。図９はロボット２と周辺環境の概略図、図１０は基部２２ｂの並進速度ｖ_ｂｘの遷移、図１１は脚部２２ａの関節を駆動するモータへ出力された電力量の遷移を示す。

【0101】

＜＜本発明を適用しない場合（許容範囲を考慮しない場合）＞＞
まず、制御装置１の移動制御部１３が、基部２２ｂの並進速度ｖ_ｂｘに対する許容範囲を考慮しない場合の制御結果１０ａについて説明する。図９左図のようにロボット２の基部２２ｂの姿勢が目標姿勢を追従することで、位置ｘ_ｍで腕部２１が操作対象物を把持できる。一方で、ロボット２の腕部２１が操作対象物Ｏｂへ近づくと、ロボット２の重心位置がｙ軸に対して負の方向へ移動する。これにより、基部２２ｂの姿勢を目標姿勢に追従させるために、図１１における許容範囲を考慮しない場合の結果１１ａのように、脚部２２ａの関節を駆動するモータへ出力される電力量が大きくなる。したがって、過剰に電力が消費されたり、脚部２２ａの関節の劣化が早まったりする恐れがある。

【0102】

＜＜本発明を適用した場合（許容範囲を考慮する場合）＞＞
次に、制御装置１の移動制御部１３が、基部２２ｂの並進速度ｖ_ｂｘに対する許容範囲を考慮する場合の制御結果１０ｂについて説明する。このとき、基部２２ｂの移動速度ｖ_ｂｘに対する許容範囲に加え、基部２２ｂのｚ方向の位置ｘ_ｂｚに対する許容範囲としてｘ_{ｂｚ，ｍｉｎ}≦ｘ_ｂｚ≦ｘ_{ｂｚ，ｍａｘ}を与えることとする。図９左図の許容範囲を考慮しない場合と比較して、図９右図の許容範囲を考慮する場合の方が、基部２２ｂの姿勢がｚ軸に対して負の方向へ移動する。これにより、腕部２１の重心位置がｙ軸に対して正の方向へ移動するため、図１１における許容範囲を考慮する場合の結果１１ｂのように、脚部２２ａの関節を駆動するモータへ出力される電力量が小さくなる。その結果、電力消費量や脚部２２ａの関節への負荷を低減することができる。このとき、基部２２ｂのｚ方向の位置ｘ_ｂｚを低く維持した状態で歩行するために、図１０における許容範囲を考慮する場合の結果１０ｂのように、移動速度ｖ_ｂｘが小さくなっているが、許容範囲１０ｃへ収めることができている。

【0103】

本実施例では、目標動作として静止物体の把持を例に挙げたが、目標動作として動的物体の把持や把持物の設置、スイッチの押下などに変更しても、同様な手順で許容範囲を設定することができる。また、操作対象物Ｏｂの把持や周辺環境との接触により、腕部２１のエンドエフェクタ２１ａへ外力が生じる場合には、腕部２１に備えられた力覚センサにより外力を測定するなどして、式９の動力学モデルに現れるｆ_ａへ設定することで、腕部２１に生じる外力を考慮した移動部制御が可能である。あるいは、操作対象物Ｏｂや周辺環境の状態遷移を式９の動力学モデルへ記述することにより、それらの挙動をより正確に考慮することができる。

【0104】

また、カメラ２３を備えた本実施例のシステムにおいては、式８～１２を利用する場合に、カメラ２３の情報を移動制御部１３へフィードバックし、周辺環境にある障害物や地形を認識することで、基部２２ｂの目標状態や脚部２２ａの接触点の目標位置を決定することができる。