WO2026014247A1

WO2026014247A1 - モバイルロボット

Info

Publication number: WO2026014247A1
Application number: PCT/JP2025/022921
Authority: WO
Inventors: 章洋鈴村; 聖也井上
Original assignee: Omron Corp; Omron Tateisi Electronics Co
Current assignee: Omron Corp
Priority date: 2024-07-11
Filing date: 2025-06-25
Publication date: 2026-01-15
Anticipated expiration: 2027-01-11
Also published as: JP2026011433A

Abstract

重心位置に関する計算負荷を小さくしながら、転倒防止以外の動作目的をも追求することが可能な制御方式を備えたモバイルロボットを提供する。　全身動作生成部は、調整モデル状態に対応するマニピュレータの重心状態を実現するための項を含む目的関数の値を最小化する最適制御であって、モバイルロボットが粗視化モデルよりも自由度の大きいマニピュレータを備えていること、又は台車が特定の機構形式を備えていることに伴う転倒防止以外の動作目的に関する事項を、目的関数の項又は最適化の制約条件として含む最適制御によって、マニピュレータの各関節を駆動するモータドライバに対する動作指令である関節指令を生成するとともに、車輪を駆動するモータドライバに対する動作指令である車輪指令を生成する。

Description

モバイルロボット

　本開示は、モバイルロボットに関する。

　工場などにおいてモバイルロボットが用いられている。モバイルロボットの中には、複数の関節を有するアーム（複数のリンク）を備えるマニピュレータと、マニピュレータを搭載する台車とを備え、台車の駆動によりマニピュレータを移動可能にされたものもある。このようなモバイルロボットにおいては、マニピュレータの転倒を防止しつつも、迅速に目的位置に移動可能とされることが求められる。

　アーム又はマニピュレータと台車とを備えたモバイルロボットとして、例えば非特許文献１、２に記載されたものが知られている。非特許文献１、２は、２自由度倒立振子モデルが適用される５自由度倒立振子ハードウェアの運動とモバイルプラットフォーム（以下、台車とよぶ）の並進速度との制御によってモバイルロボットの走行時の転倒の防止を図ることができるモバイルロボットを開示している。

D. Choi, M. Kim, and J. Oh, "Development of a rapid mobile robot with a multi-degree-of-freedom inverted pendulum using the model-based zero-moment point stabilization method," Advanced Robotics, Vol. 26, No. 5-6, pp. 515-535, 2012. D. Choi, and J. Oh, "ZMP stabilization of Rapid Mobile Manipulator", in 2012 IEEE International Conference on Robotics and Automation, St Paul, MN, USA: IEEE, May 2012, pp. 883-888. doi: 10.1109/ICRA.2012.6225369.

　非特許文献１、２のモバイルロボットでは、５自由度倒立振子ハードウェアの各関節の運動は、５自由度倒立振子の目標重心位置の指令（Ｘ_ｐｅｎ、Ｙ_ｐｅｎ）から逆運動学によって決定されるため、重心位置に関する計算負荷は小さいものの各関節の動作の目的が目標重心位置の実現に限定されるという問題がある。

　本開示は上記の課題に鑑み、重心位置に関する計算負荷を小さくしながら、転倒防止以外の動作目的をも追求することが可能な制御方式を備えたモバイルロボットを提供することを目的とする。

　本開示に係るモバイルロボットは、車輪を有する台車、複数の関節を有するアームを備えるマニピュレータ、及びロボット制御部を備えるモバイルロボットであって、前記ロボット制御部は、前記モバイルロボットを走行計画に従って走行させるための台車の目標状態である目標台車状態を取得し、合計質量が前記マニピュレータの質量である１又は複数の質点及び前記アームの関節の数よりも少ない数の自由度で前記質点を移動させる仮想機構を備えた粗視化モデルを利用して、前記モバイルロボットの転倒を回避しながら前記台車の状態を前記目標台車状態に近づけるための、前記粗視化モデルの状態である調整モデル状態及び前記台車の状態である調整台車状態を求める粗視化モデル動作計画部と、前記調整モデル状態に対応する前記マニピュレータの重心状態を実現するための項を含む目的関数の値を最小化する最適制御であって、前記モバイルロボットが前記粗視化モデルよりも自由度の大きいマニピュレータを備えていること、又は台車が特定の機構形式を備えていることに伴う転倒防止以外の動作目的に関する事項を、前記目的関数の項又は最適化の制約条件として含む最適制御によって、前記マニピュレータの各関節を駆動するモータドライバに対する動作指令である関節指令を生成するとともに、前記車輪を駆動するモータドライバに対する動作指令である車輪指令を生成する全身動作生成部と、前記粗視化モデル動作計画部及び前記全身動作生成部を周期的に実行させることにより、前記モバイルロボットを動作させる実行制御部とを備える。

　本開示に係るモバイルロボットによれば、重心位置に関する計算負荷を小さくしながら、転倒防止以外の動作目的をも追求することが可能な制御方式を備えたモバイルロボットを提供することができる。

第１の実施の形態に係るモバイルロボット１の概略構成を説明する概略図である。モバイルロボット１の動作を制御する制御部３０のハードウェア構成を説明するブロック図である。制御部３０の構成の一例を機能ブロック図である。粗視化モデル動作計画部３４について説明する概略図である。ゼロ・モーメント・ポイント（ＺＭＰ）の移動について説明する概略図である。粗視化モデル動作計画部３４における最適化問題の計算の手順について説明するフローチャートである。第２の実施の形態の動作生成部３１の構成を説明するブロック図である。第３の実施の形態の動作生成部３１の構成を説明するブロック図である。

　以下、添付図面を参照して本実施形態について説明する。添付図面では、機能的に同じ要素は同じ番号で表示される場合もある。なお、添付図面は本開示の原理に則った実施形態と実装例を示しているが、これらは本開示の理解のためのものであり、決して本開示を限定的に解釈するために用いられるものではない。本明細書の記述は典型的な例示に過ぎず、本開示の特許請求の範囲又は適用例を如何なる意味においても限定するものではない。各図面において同一又は実質的に同等な要素、部材及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

　本実施形態では、当業者が本開示を実施するのに十分詳細にその説明がなされているが、他の実装・形態も可能で、本開示の技術的思想の範囲と精神を逸脱することなく構成・構造の変更や多様な要素の置き換えが可能であることを理解する必要がある。従って、以降の記述をこれに限定して解釈してはならない。

［用語等の説明］
　実施の形態の説明の前に、実施の形態中の各種用語の意義について以下に述べる。
（１）「マニピュレータ」は、複数の関節により接続された複数リンクの先端にエンドエフェクタ、カメラなどが備えられているものであってよい。
（２）「走行計画」は、走行目的に応じて将来の必要な時間範囲について作成される経路、速度などについての計画である。「走行計画」は、走行開始前に作成される計画であってもよいし、走行中に動的に作成・修正される計画であってもよい。「走行計画」は、モバイルロボットの外部で作成されてモバイルロボットに与えられる計画でもよいし、モバイルロボット自身が作成する計画であってもよい。

（３）「モータドライバ」は、例えば入力する指令によって与えられる値を目標値として目標値に対する誤差が小さくなるようにモータを駆動する制御システムであってよい。この制御システムにおける具体的な制御方式としては、トルク制御、位置制御、速度制御、又はこれらの組み合わせであってよく、「全身動作生成部」は、モータドライバの制御方式に適合した種類の値を含む指令を作成することができる。
（４）「粗視化モデル動作計画部」は、「調整モデル状態」及び「調整台車状態」を、ＭＰＣ（Model Predictive Control）の手法によって求めるほか、ＬＱＲ（Linear Quadratic Regulator）などの手法によって求めてもよい。

（５）「前記調整モデル状態に対応する前記マニピュレータの重心状態」は、単一質点モデルの場合、質点の状態（例えば質点の位置、速度）とマニピュレータの重心状態（例えば重心の位置、速度）とが一致する場合だけでなく、例えば倒立振子モデルの質点が動く球面をマニピュレータの重心が動く高さ一定の平面に変換した場合（平面で近似した場合）に変換元の状態に対応する変換先の状態であってもよい。

（６）下記の説明に含まれる数式（イメージ）のうち、太字の表記は行列又はベクトルを表している。これらの行列又はベクトルは、スカラ量との区別のため、本文中では、太字表記ではなく「→」にて表記することがある。また、ｎ回微分を示すｎ個のドットは、数式（イメージ）中では文字の上方に表記されるが、本文中では左横に表記される。その他、数式（イメージ）中での二段階の添え字が、本文中では括弧書きで並列表記（例（ｃｍｄ、Ｃｏｇ））される場合がある。

［第１の実施の形態］
　まず、第１の実施の形態に係るモバイルロボット１を、図１の概略図を参照して説明する。このモバイルロボット１は、台車１０、マニピュレータ２０、制御部３０、及び撮像部４０を備える。台車１０は、車輪Ｗ（ＷＡ、ＷＢ）を有して並進移動又は旋回可能に構成され上部にマニピュレータ２０を搭載している。マニピュレータ２０は、後述するように、複数の関節、複数のリンクを備える。制御部３０は、走行計画に従って台車１０及びマニピュレータ２０を制御する。

　図１に示す例では、台車１０は略直方体状であるが、これに限定されるものではない。以下において、台車１０の座標系として、Ｘ軸、Ｙ軸、及びＺ軸を設定する。Ｘ軸方向は台車１０の前後方向、Ｙ軸は台車１０の左右方向、Ｚ軸は台車１０の高さ方向である。この左右方向は、台車１０の幅方向でもある。また、以下では、台車１０の高さ方向が鉛直方向と一致している場合を例として説明する。

　台車１０は、移動機構１１を備えている。移動機構１１は、２つの駆動輪ＷＡと、２つの従動輪ＷＢと、２つの駆動輪ＷＡをそれぞれ独立に駆動する２つの図示しない回転駆動源（モータ）とを有している。駆動輪ＷＡと従動輪ＷＢを総称して「車輪Ｗ」ということがある。駆動輪ＷＡ及び従動輪ＷＢは、床面ＬＦに接しており、これによってモバイルロボット１を支持し、台車１０を移動させる。駆動輪ＷＡ及び従動輪ＷＢの４つの接地点を結ぶ四角形は、支持領域ＳＡを構成する。なお、台車１０は、マニピュレータ２０を安定的に載置できるよう、３輪以上の車輪Ｗを有し、その３つ以上の車輪Ｗの各接地点を結ぶ領域が支持領域を構成していればよい。

　駆動輪ＷＡは、図示しない回転駆動源によって回転駆動される。例えば、２つの駆動輪ＷＡを一定の回転数で回転させることで、台車１０を直線（並進）運動させることができる。また、２つの駆動輪ＷＡの回転数を左右で異ならせる（差動させる）ことで、台車１０を旋回させることができる。従動輪ＷＢは、回転駆動源によって回転駆動されることはなく、駆動輪ＷＡの駆動により、床面ＬＦとの摩擦力に従って回転可能とされている。さらに、従動輪ＷＢの回転軸は、水平面内で自由に方向が変わる。すなわち、従動輪ＷＢは、自在キャスター（旋回キャスター）である。

　また、マニピュレータ２０は、複数の関節Ｊと、複数のリンクＬと、エンドエフェクタＨとを備えている。リンクＬは隣接する２つの関節Ｊの間の部分である。複数の関節Ｊは、それぞれ回転軸ＡＸ（図１において、符号ＡＸは一部の関節Ｊについてのみ図示されている。）を中心に回転可能に構成される。これにより、マニピュレータ２０は、関節Ｊの回転によりエンドエフェクタＨを任意の位置に移動させることができる。エンドエフェクタＨは、対象物を保持可能に構成される装置であり、例えばロボットハンド、又は吸着パッドであり得る。なお、撮像部４０は、マニピュレータ２０に搭載され、マニピュレータ２０の周囲の画像を撮像する。

　図２のブロック図を参照して、モバイルロボット１の動作を制御する制御部３０のハードウェア構成を説明する。制御部３０は、コンピュータ１００を有する。コンピュータ１００は、プロセッサ１０２、メモリ１０４、ストレージ１０６、入力装置１０８、出力装置１１０、記憶媒体読取装置１１２及び通信Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）１１４を有する。これらの各要素は、バス１１６を介して相互に通信可能に接続されている。

　ストレージ１０６には、モバイルロボット１を制御するための制御プログラム１１８が格納されている。プロセッサ１０２は、各種のプログラムを実行したり、各要素を制御したりすることが可能である。具体的には、プロセッサ１０２は、ストレージ１０６からプログラムを読み出し、メモリ１０４を作業領域としてプログラムを実行する。すなわち、プロセッサ１０２は、ストレージ１０６に格納されているプログラムに従って、各要素の制御及び各種の演算処理を行う。

　メモリ１０４には、作業領域として、一時的にプログラム及び各種のデータを記憶可能である。

　ストレージ１０６は、たとえば、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ　Ｄｉｓｃ　Ｄｒｉｖｅ）、及びＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）等であり、各種プログラム及び各種データが格納される。このプログラムには、上記した制御プログラム等のアプリケーションプログラムだけでなく、オペレーティングシステムも含まれる。

　入力装置１０８は、コンピュータ１００に対し各種の入力を行うための装置である。入力装置１０８としては、操作スイッチや操作ボタン等が含まれる他、パーソナルコンピュータ等に使用されるキーボードやマウス等のポインティングデバイス等を含んでいてもよい。

　出力装置１１０は、コンピュータ１００からの各種の情報を出力するための装置であり、たとえば、ディスプレイや表示ランプ、スピーカー等が含まれる。出力装置１１０としてタッチパネルディスプレイを用いることも可能であり、この場合には、タッチパネルディスプレイが入力装置１０８としても機能する。

　入力装置１０８及び出力装置１１０は、モバイルロボット１を運用するときに取り外せるようになっていてもよい。入力装置１０８及び出力装置１１０を設けることなく、通信Ｉ／Ｆ１１４を介した無線通信によって、モバイルロボット１に対する動作指令その他の情報を入力し、モバイルロボット１の動作状態その他の情報を出力してもよい。

　記憶媒体読取装置１１２は、各種の記憶媒体に記憶されたデータの読み込み、及び、記憶媒体に対するデータの書き込みを行う装置である。記憶媒体としては、たとえば、ＣＤ（Ｃｏｍｐａｃｔ　Ｄｉｓｃ）－ＲＯＭ、ＤＶＤ（Ｄｉｇｉｔａｌ　Ｖｅｒｓａｔｉｌｅ　Ｄｉｓｃ）－ＲＯＭ、ブルーレイディスク、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ　Ｓｅｒｉａｌ　Ｂｕｓ）メモリ等を挙げることができる。

　通信Ｉ／Ｆ１１４は、他の機器と通信するためのインターフェースである。通信のためには、たとえば、イーサネット（登録商標）、ＦＤＤＩ（Ｆｉｂｅｒ　Ｄｉｓｔｒｉｂｕｔｅｄ　Ｄａｔａ　Ｉｎｔｅｒｆａｃｅ）等の規格が用いられる。

　上記した各関節J、リンクＬ、エンドエフェクタＨ、及び駆動輪ＷＡは、制御部３０を介して、モータドライバＭＤにより制御される。具体的には、たとえば、各関節Ｊは、図示しない駆動源が制御部３０を介してモータドライバＭＤによって制御されることにより、それぞれの回転軸ＡＸを中心として回転する。同様に、エンドエフェクタＨも、図示しない駆動源が制御部３０を介してモータドライバＭＤによって制御され、対象物に対し所定の把持、吸着、操作、移動、加工等を施す。駆動輪ＷＡは、図示しない回転駆動源が制御部３０によって制御されて回転する。

　図３の機能ブロック図を参照して、制御部３０の構成の一例を説明する。本実施の形態のモバイルロボット１の制御部３０は、制御プログラム１１８によりコンピュータ１００内に実現される、動作生成部３１、実行制御部３２、及び目標台車状態生成部３３から大略構成される。

　目標台車状態生成部３３は、台車１０の走行計画（目標台車軌道）に従って、台車１０の目標状態（目標並進速度ｖ_ｒｅｆ、角速度ω_ｒｅｆ等）を含む目標台車状態情報を生成することができる。目標台車状態情報は、モバイルロボット１の台車１０を走行計画に従って走行させるための台車１０の目標状態を示す情報である。動作生成部３１は、この目標台車状態情報を受信して、台車１０及びマニピュレータ２０を制御するための指令信号（関節指令信号、車輪指令信号）を出力する。

　モータドライバＭＤは、この指令信号を受信して、駆動信号を生成し、図示しない各種回転駆動部を駆動する。なお、モータドライバＭＤは、動作生成部３１に対し、現在の各種回転駆動部の状態を示す信号をフィードバック信号としてフィードバックする機能も備え得る。実行制御部３２は、動作生成部３１を周期的に動作させることにより、モバイルロボット１を動作させる。

　動作生成部３１は、一例として、粗視化モデル動作計画部３４と全身動作生成部３５とから構成され得る。粗視化モデル動作計画部３４は、マニピュレータ２０の動作を粗視化モデル（例えば倒立振子モデル）に置き換え、その粗視化モデルの動作計画を生成する部分である。粗視化モデルは、合計質量がマニピュレータ２０の質量である１又は複数の質点及びアームの関節の数よりも少ない数の自由度で前記質点を移動させる仮想機構である。具体的に粗視化モデル動作計画部３４は、目標台車状態生成部３３から受信した目標台車状態情報に従い、設定された目的関数及び制約条件に従って定められる最適化問題の目的関数の値を最小化する指令を動作計画として生成し、全身動作生成部３５に向けて出力する。ここでは、粗視化モデル動作計画部３４が出力する指令は、粗視化モデルの調整された状態量を指令する調整モデル状態指令（例えば、調整振子角度ｑ_{ｐｉｔｃｈ＿ｃ}、ｑ_{ｒｏｌｌ＿ｃ}、調整振子角速度・ｑ_{ｐｉｔｃｈ＿ｃ}、・ｑ_{ｒｏｌｌ＿ｃ}）、及び台車１０の調整された状態量を指令する調整台車状態指令（例えば、調整並進速度ｖ_ｃ、調整角速度ω_ｃ）とすることができる。調整モデル状態指令、及び調整台車状態指令が適宜変更されることにより、モバイルロボット１の転倒を回避しながら、台車１０の状態を目標台車状態に近づけることができる。粗視化モデルは、複数の質点の合計質量がマニピュレータ２０の質量と等しくされるとともに、マニピュレータ２０よりも少ない数のリンクを有し、マニピュレータ２０をより少ない自由度で移動させる仮想機構である。

　粗視化モデルの調整された状態（調整モデル状態）及び台車１０の調整された状態（調整台車状態）は、モバイルロボット１のゼロ・モーメント・ポイントの位置をモバイルロボット１の支持領域内に設定された所定範囲内で移動することを許容し、かつ、将来の一定時間範囲にわたるモバイルロボット１についての目標状態と予測状態との間の誤差量の項を含む目的関数の値を最小にする、粗視化モデルの状態及び台車１０の状態とすることができる。

　全身動作生成部３５は、粗視化モデル動作計画部３４から供給された指令に従い、モータドライバＭＤに供給するための各種指令を生成し出力する部分である。全身動作生成部３５は、一例として、重心指令生成部３６と全身指令生成部３７とを備えたものとすることができる。

　重心指令生成部３６は、調整モデル状態指令に従い、マニピュレータ２０の重心に関する動作指令であるマニピュレータ重心指令を出力する。マニピュレータ重心指令は、一例として、マニピュレータ２０の重心の位置に関するマニピュレータ重心位置指令→ｐ（ｃｍｄ、ＣｏＧ）、及び重心の独度に関するマニピュレータ重心速度指令→ｐ・（ｃｍｄ、ＣｏＧ）を含み得る。

　全身指令生成部３７は、マニピュレータ重心指令、及び調整台車状態指令に基づき、モータドライバＭＤを制御するための関節指令信号、及び車輪指令信号を出力する。

　図４を参照して、粗視化モデル動作計画部３４について、更に詳細に説明する。粗視化モデル動作計画部３４は、一例として、粗視化モデルとして倒立振子モデルを用いて、マニピュレータ２０を粗視化する。倒立振子モデルでは、ロールとピッチが独立に回転するモデルである。

　倒立振子モデルでは、状態量→ｚ、入力→ｕを下記［数１］のように定義したとき、非線形の運動方程式を、振子が直上に立って静止した状態を平衡点として、平衡点周りでテーラー展開の１次で線形化する事で、このモデルの状態空間表現を以下の［数２］のように得ることができる。

　ここでは、台車１０の車輪Ｗのトルクが、台車１０の車体の旋回と並進のトルクに損失なく伝わり、振子のピッチ軸周りの姿勢変化が、車体のヨー周りのイナーシャの変化に与える影響は小さく無視できると仮定している。以降の式では、このシステムを元に、適当なサンプリング時間で離散化し、離散の状態方程式のシステム行列→Ａd、→Ｂｄも用いる。また、離散の状態方程式は、後述の最適化問題においてダイナミクスの等式制約としても用いられる。

　次に、ロボット座標系Ｘ軸方向のゼロ・モーメント・ポイント（ＺＭＰ）をモデル化する。ＺＭＰは、例えば次に説明する方法により計算され得る。図５に示すように、モバイルロボット１全体の重心位置ＧＰ１には、重力Ｆ１及び慣性力Ｆ２が働く。慣性力Ｆ２はモバイルロボット１の前後方向の加速度走行に伴う慣性力、旋回走行に伴う遠心力、及び複数の関節Jの駆動によるリンクＬの運動に伴う慣性力の合力である。重力Ｆ１及び慣性力Ｆ２の合力Ｆ３の方向が床面ＬＦに交わる点をゼロ・モーメント・ポイント（ＺＭＰ）と呼ぶ。慣性力Ｆ２が大きくなってＺＭＰが支持領域ＳＡの端に到達すると、モバイルロボット１は車輪のいずれかが床面ＬＦから離れて転倒し始める。

　具体的に、ＺＭＰのＸ軸方向のモデル化について説明する。ＺＭＰの計算方法については後述する。Ｚ軸方向のマニピュレータの重心の変化量が小さく無視できると仮定して、重心移動量を振子角に対して平衡点付近で線形化すると、上述の状態量を用いて、ロボット中の任意の原点からのＺＭＰのＸ軸方向の変化量Ｘ（ｄ、ＺＭＰ）を、以下のように表現することができる。

　なお、Ｃ_Ｚ１　、Ｃ_Ｚ２、Ｃ_Ｚ３は、幾何学的特徴から決まるスカラ量である。スカラ量Ｃ_Ｚ１、Ｃ_Ｚ２、Ｃ_Ｚ３は、同定によって定めてもよいし、設計値を用いて算出してもよい。更に［数２］を利用すれば、以下のような状態量と入力量の式に変形することができる。

　続いて、ロボット座標系Ｙ軸方向のＺＭＰをモデル化について説明する。Ｘ軸方向のモデル化との違いとしては、図１に示すような差動二輪型ロボットは、Ｙ軸方向に加速度を発生させる事ができない。ただし、並進と旋回を行う場合には、遠心力によって旋回中心から離れる方向に力を受けるため、それをＺＭＰモデルに組み込むことが必要である。わずかな時間の間、マニピュレータ２０がある旋回中心点の周りに等速円運動すると近似すれば、この時の遠心加速度ａ_Ｌは、以下の式で表すことができる。

　さらに、本手法では遠心加速度ａ_Ｌを状態量に線形になるように、旋回速度ω（実測角速度又は現在値として予測された予測角速度）と目標並進速度ｖ_ｒｅｆを用いて、以下のように近似することができる。

　これらを踏まえ、Ｘ軸と同様にして、任意の原点からのＺＭＰのＹ軸方向の変化量Ｙ（ｄ、ＺＭＰ）を、以下のように得ることができる。

　なお、Ｃ_Ｚ４、Ｃ_Ｚ５、Ｃ_Ｚ６は幾何学的特徴から決まるスカラ量である。スカラ量Ｃ_Ｚ４、Ｃ_Ｚ５、Ｃ_Ｚ６は、同定によって定めてもよいし、設計値を用いて算出してもよい。Ｘ軸方向の場合と同様に、［数７］は以下のような状態量と入力量の式に変形することができる。ｖ_ｒｅｆが時変のため、ｄ_ｂも時変となる。

　続いて、粗視化モデル動作計画部３４におけるモデル予測制御の定式化について説明する。状態量に対する指令値を以下のように定義する。

　ここで、ω_ｒｅｆ、ｖ_ｒｅｆは上位コントローラである目標台車状態生成部３３から送られてくる指令値であり、ｋは離散時間である。

ここで、→ｚと→ｚ_ｒｅｆの差→ｅ［ｋ］を以下の［数１０］のように表現したとき、有限時間内の指令の追従誤差を小さくしつつ、入力量のノルムを小さく抑えた入力量→ｕは以下の［数１１］の目的関数で表現することができる。

　Ｎｐ、Ｎｃはそれぞれ予測ホライゾンステップ、制御ホライゾンステップである（本例においてはＮｐ＝Ｎｃとしている）。

　粗視化モデルのダイナミクスに関する制約は、［数２］を離散化した状態方程式として以下のように表現される。

　モバイルロボット１の転倒回避を目的とした制約は、［数４］、［数８］のＺＭＰのモデルを用い、適当な制約範囲を設ける事で、以下のように表現することができる。

　ただし，Ｋ（ｘ，ｚｂ）、Ｋ（ｙ、ｚｂ）はモデル化誤差を緩和する目的で導入した調整パラメータである。以上の説明を一つの最適化問題にまとめると以下の［数１４］のようになる。

　ＺＭＰの位置は所定範囲内において変位することが許容される。ＺＭＰの位置が所定範囲内で移動することを許容する手段は、上記の［数１３］のような不等式制約によってでもよいし、目的関数にＺＭＰ位置の誤差項を入れることによってもよい。目的関数の誤差項による場合は、モバイルロボット１の仕様や走行加速度の制限を前提に、実験的な手法によって目的関数のパラメータを適切に設定することにより、ＺＭＰの位置が所定範囲内で移動することを許容することができる。

　ここまでに求めた入力量→ｕと［数２］を用いて、次時刻の並進速度（調整並進速度ｖ_ｃ）、旋回速度（調整角速度ω_ｃ）、仮想振子角度（調整振子角度ｑ_pitch＿ｃ）、仮想振子角速度（調整振子角速度・ｑ_pitch、・ｑ_roll＿ｃ）を生成する。更に、マニピュレータ２０の重心のＺ方向の位置は変動しないという仮定の下で、振子角度・角速度を重心の位置（重心位置指令→ｐ（ｃｍｄ、ＣｏＧ））と速度（重心速度指令→・ｐ（ｃｍｄ、ＣｏＧ））にそれぞれ変換する。これらを下段の全身指令生成部３７に出力する。

　ここで、ＺＭＰの計算方法について説明する。２質点系で近似したＺＭＰの位置は以下のように表現することができる。

　ただし、Ｍは台車１０の質量、ｍは倒立振子の質量、ｇは重力加速度、ｘはグローバル座標系における台車１０の重心位置のＸ軸成分、ｌは倒立振子の根元から倒立振子重心までの長さ、ｘ_ｊ１，ｚ_０，ｚ_１はそれぞれ，仮想振子の付け根位置のＸ軸成分，台車１０の重心高さ，仮想振子の重心高さである。また，Ｉ_ｗｈは車輪の車軸まわりの慣性モーメントである。ここで、目標とするＺＭＰの位置のＸ軸成分をＸ_ＺＭＰ０は、例えば以下のように設定することができる。

　そして、この目標ＺＭＰ位置からの誤差を、以下のように定義する。

　Ｚ軸方向のアーム重心の変化量が小さいと仮定して、sinq__pitch≒q__pitch、及び，ｚ_１を定数値とすれば，上述の状態量を用いて，モバイルロボット１中の任意の原点からのＺＭＰの変化量Ｘ（ｄ、ＺＭＰ）を、以下のように表すことができる。ただし、Ｃ_Ｚ１、Ｃ_Ｚ２、Ｃ_Ｚ３は定数値である。

　次に、全身指令生成部３７の動作について説明する。全身指令生成部３７では、マニピュレータ２０の関節Ｊや車輪Ｗを駆動するモータドライバＭＤへの指令値を算出するための目的関数を最適化問題として定式化し、線形ソルバーを利用して解を算出することで、指令値を生成する。具体的には、マニピュレータ２０の重心速度や手先速度を所望の値に追従させ、かつ車輪Ｗ及び台車１０の速度を所望の値に追従させるための目的関数が、最適化問題として定式化される。

　目的関数の設計と制約の設計について説明する。この例においては、台車１０の速度、マニピュレータ２０の重心速度、及びマニピュレータ２０の手先（エンドエフェクタＨ）の速度を所望の値に追従させるような目的関数が設計される。後述するように、本実施の形態の全身動作生成部３５は、調整モデル状態に対応するマニピュレータ２０の重心状態を実現するための項を含む目的関数の値を最小化する最適制御であって、モバイルロボット１が粗視化モデルよりも自由度の大きいマニピュレータ２０を備えていること、又は台車１０が特定の機構形式を備えていることに伴う転倒防止以外の動作目的に関する事項を、目的関数の項又は最適化の制約条件として含む最適制御によって、マニピュレータ２０の各関節Ｊを駆動する動作指令である関節指令を生成するとともに、車輪Ｗを駆動する動作指令である車輪指令を生成する。

　まず、粗視化モデル動作計画部３４から出力される調整並進速度ｖ_ｃ、調整角速度ω_ｃに追従するよう車輪Ｗの角速度を最適化する目的関数の設計について説明する。調整並進速度ｖ_ｃ、調整角速度ω_ｃを、次の［数１９］のように定義すると、差動二輪型移動ロボットの非ホロノミック拘束条件を用いて、左右の車輪を含む各軸速度→・θと、台車１０の調整並進速度ｖ_ｃ、調整角速度ω_ｃの関係は、以下の等式で表現することができる。

　ただし，→Ａ_ξ、→Ａ_θは台車１０の幾何構造から一意に決まる拘束条件係数行列である。より具体的には、→Ａ_ξは台車１０の姿勢及び両車輪間の距離に関する係数であり、→Ａ＿_・θは車輪半径に関する係数である。→・θは各車輪の回転速度及び各関節の回転速度を含むが、ここで制御上の有意情報は各車輪の回転速度である。

　次に、マニピュレータ２０の重心位置を所望の位置に追従させるため、マニピュレータの２０の関節Ｊの速度を最適化するための目的関数の設計について説明する。重心指令生成部３６から出力される、転倒回避のためのマニピュレータ部重心位置指令→ｐ（ｃｍｄ、ＣｏＧ）と重心速度指令→・ｐ（ｃｍｄ、ＣｏＧ）を用いて、以下のように重心速度のリファレンス値→・ｐ（ｒｅｆ、ＣｏＧ）を作成する。

　ここで、Ｋ（ＣｏＧ、ｐ）は、位置誤差に関するゲインである。このリファレンス値→・ｐ（ｒｅｆ、ＣｏＧ）とロボットの重心ヤコビアン→Ｊ^ＣｏＧを用いて、重心の追従誤差は、次のように表現することができる。ここで、各軸速度→・θの制御上の有意情報は、各関節Ｊの角速度である。

　次に、与えられたマニピュレータ２０の手先の位置（ツール・センタ・ポイントＴＣＰ）の指令を追従するようマニピュレータ２０の各軸の速度を最適化する目的関数の設計について説明する。手先の速度に所望の指令・ｐ（ｒｅｆ、ＴＣＰ）がある場合には、重心位置の場合と同様に、ヤコビアン→Ｊ^ＴＣＰを用いて，以下のように誤差を定義することができる。

　なお、手先の運動を極力抑えたい場合は，速度がロボット座標系で０となるような指令を設定すればよい。

　本実施の形態では，マニピュレータ２０の各軸の速度の最大／最小値，重心速度の最大／最小値、手先速度の最大／最小値，及び自己干渉回避を制約として導入する。４つの制約の内のはじめの３つは、ハードウェアのスペックについて適当な閾値を設定し、対応する変数を挟んで以下のように不等式制約を設定する。

　このとき，位置リミットまでの減速マージン領域に入ったら位置リミットに近づくほど速度上限値を小さくするように働く速度ダンパを導入することで，現在の位置を使って閾値を動的に変化させることで，位置次元の制約も考慮することができる。４つめの自己干渉回避については、各リンクをカプセル形状で近似し，隣り合うカプセルを除くカプセル同士の最接近距離ｖｓｅｌｆ（→・θ）が、ある閾値ｄclosestを超えないように制約する。

　以上をひとつの最適化問題にまとめると以下の［数２５］になる。全身指令生成部３７においてこの最適化問題が解かれて関節速度指令及び車輪速度指令が生成される。

　［数２５］の第２項の値を最小化することにより、調整モデル状態指令に対応して生成された重心指令からのマニピュレータ２０の重心状態の誤差を小さくし、モバイルロボット１の転倒を防止することができる。その他の項の最小化により、モバイルロボット１の転倒の防止を前提として、粗視化モデルよりも自由度の大きいマニピュレータ２０を備え又は台車１０が特定の機構形式を備えている状況において、モバイルロボット１の転倒防止以外の動作目的に関する事項を、前記目的関数の項又は最適化の制約条件として含む最適制御として実行することができる。なお、台車の各車輪Ｗの回転速度は［数２０］の関係を用いて直接算出し、［数２５］の目的関数には、マニピュレータ２０の関節Ｊの速度に関する項である2番目及び３番目の項のみを含めるようにしてもよい。また、ツール・センタ・ポイントＴＣＰの位置の安定化を図らない場合は、３番目の項は不要である。

　モータドライバＭＤが速度指令を入力する仕様である場合は、全身指令生成部３７は、算出したマニピュレータ２０の各軸の速度指令及び車輪Ｗの速度指令を、関節指令及び車輪指令としてモータドライバＭＤに出力することができる。

　モータドライバＭＤが位置指令を入力する仕様である場合は、全身指令生成部３７は、算出したマニピュレータ２０の各軸の速度指令及び車輪Ｗの速度指令を積分して角度指令に変換して関節指令及び車輪指令としてモータドライバＭＤに出力することができる。

　モータドライバＭＤがトルク指令を入力する仕様である場合は、全身指令生成部３７は、算出した各軸の速度指令を計算トルク法（動力学モデル）を用いてトルク指令に変換して関節指令及び車輪指令としてモータドライバＭＤに渡す。モータドライバＭＤにより駆動される回転駆動部（モータ）内では電流ループの制御コントローラを備えたものとすることができ、別途チューニングすることができる。

　図６のフローチャートを参照して、粗視化モデル動作計画部３４における最適化問題の計算の手順について説明する。まず、ステップＳ１１において、図示しないセンサ等からの情報に基づいて、モバイルロボット１の状態量→ｚを計算する。現在の状態量は状態オブザーバ等による推定値として取得されてもよい。

　次に、ステップＳ１２において、実行可能解を設定する。実行可能解は、等式制約及び不等式制約を充足する必要がある。ｋ＝０の時は現在の状態量が初期実行可能解として使用され、ｋ＝１以降は、従前のステップ（ステップｋ－１）で得られた最適解が実行可能解として使用される。

　続いて、ステップＳ１３において、設定された実行可能解に基づいて目的関数（［数１１］）の値を得て、前回の目的関数の値よりも小さくするような指令値系列を求められる。実行可能解の更新は、ニュートン法、準ニュートン法、逐次二次計画法等を用いた最適化アルゴリズムによって計算すればよく、目的関数の値を前回の目的関数の値よりも小さくするような指令値系列を求める。求められた指令値系列が最適解探索の終了条件を満たしているか否かが判定される（ステップＳ１４）。ここで、最適解探索の終了条件の例としては、例えば（１）目的関数の値が所定値以下になったこと、（２）目的関数の値の前回からの変化の大きさが所定値以下になったこと、（３）ループ処理の回数が所定回数に達したことなどが挙げられる。

　ステップＳ１４の判定がＹｅｓであればステップＳ１５に移動し、そのときの指令値系列が後段の全身動作生成部３５へ送信される。Ｎｏであれば、Ｙｅｓ判定がなされるまでステップＳ１３が繰り返される。以上の処理をステップｋ、ｋ＋１、ｋ＋２、と繰り返し（ステップＳ１６）、最終的な制御目的が達成されるまで処理が繰り返される。

　以上説明したように、この第１の実施の形態のモバイルロボット１によれば、粗視化モデルの状態である調整モデル状態指令及び調整台車状態指令を、粗視化モデル動作計画部３４から出力し、全身動作生成部３５において、調整モデル状態指令に対応するマニピュレータ２０の重心状態を実現するよう目的関数の値を最小化する最適制御を行いながら、転倒防止以外の動作目的に関する事項も目的関数の項又は最適化の制約条件として含ませている。これにより、重心位置に関する計算負荷を小さくしながら、転倒防止以外の動作目的をも追求することが可能になる。

［第２の実施の形態］
　次に、第２の実施の形態に係るモバイルロボット１を、図７を参照して説明する。このモバイルロボット１の基本構成は第１の実施の形態と同一であるので、重複する説明は省略する。この第２の実施の形態では、粗視化モデルとして、図７に示すような２つの質点Ｍ_Ａ、Ｍ_Ｌによりマニピュレータ２０を表現する２質点モデルを採用する。この点が第１の実施の形態とは異なっている。

　粗視化モデル動作計画部３４は、ＺＭＰの位置がモバイルロボット１の支持領域ＳＡ内に設定された所定範囲内で移動することを許容しるように２質点モデルの動作計画を行い、全身動作生成部３５にて２質点モデルの状態から全関節の状態を決定することができる。例えば、図７の構造のモバイルロボット１について、関節Ｊ３よりも下側のリンク群Ａを質点Ｍ_Ａで表し、関節Ｊ３よりもエンドエフェクタＨ側のリンク群Ｂを質点Ｍ_Ｂで表した２質点モデルを考える。２つの質点Ｍ_Ａ、Ｍ_Ｂは、その合計質量がマニピュレータ２０の質量と等しくなるように設定される。この２質点モデルによれば、マニピュレータ２０の姿勢変化に伴う重心位置の高さの変化を表現することができる。

　粗視化モデル動作計画部３５では、目的関数に質点Ｍ_Ｂの高さについての目標値との誤差を表す項を追加することにより、質点Ｍ_Ｂの高さを目標高さに追従させることができる。全身動作生成部３５では、質点Ｍ_Ａ、質点Ｍ_Ｂにそれぞれ対応する重心速度のリファレンス値→・ｐ（ｒｅｆ、Ａ）、→・ｐ（ｒｅｆ、Ｂ）、及び全関節Ｊの速度・θをリンク群Ａの重心速度→・ｐ（ＣｏＧ、Ａ）へ射影する重心ヤコビアン→Ｊ（ＣｏＧ、Ａ）とリンク群Ｂの重心速度・ｐ（ＣｏＧ、Ｂ）へ射影する重心ヤコビアン→Ｊ（ＣｏＧ、Ｂ）を用いて、［数２５］の→Ｊ^Ｃ０Ｇ・θの項を２つの項に分けることにより、全身動作生成部３５の最適化問題を解くことができる。質点の数が３以上の粗視化モデルも同様にして利用することができる。

［第３の実施の形態］
　次に、第３の実施の形態に係るモバイルロボット１を、図８を参照して説明する。このモバイルロボット１の基本構成は第１の実施の形態と同一であるので、重複する説明は省略する。ただし、この第３の実施の形態は、全身動作生成部３５の構成が前述の実施の形態とは異なっている。

　図８を参照して、この第３の実施の形態の動作生成部３１の構成を説明する。この動作生成部３１では、粗視化モデル動作計画部３４において、粗視化モデルとして単質点モデルを生成し、この単質点モデルを用いて、重心指令を介せずにモータドライバＭＤに出力する指令を生成する。

　具体的には、粗視化モデル動作計画部３４で生成された単質点モデルの状態から、第１の実施の形態のような重心の位置や速度に係る指令は生成せず、マニピュレータ２０の関節の状態に係る指令を直接求める。一例として、粗視化モデル動作計画部３４では、単質点モデルの仮想振子トルクτ_{ｐｉｔｃｈ}を決定するようことができる。さらに、振子長さｌを用いると、重心に加えたい力→ｆを以下のように決定することができる。簡単にＸ方向のみについて考えると、次のように表現することができる。

　この力→ｆを使って、全身動作生成部３５では、例えば以下の［数２７］のように、力→ｆと単質点モデルの全軸のトルク→τとの間の関係を示す等式制約の下で全軸のトルクを最小化する最適化問題を解くことで、モータドライバＭＤへの指令を生成することができる。

　ここで、→Ｊτ、comは、関節トルクに対する重心にかかる力のヤコビアンであり、ロボットの動力学モデルと重心ヤコビアンを用いて算出する。さらに、目的関数に台車の速度追従に関する項を追加したり、台車の機構によって決まる拘束、モータのスペックに関する制約などを追加したりしてもよい。

　本開示は上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本開示を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。

１…モバイルロボット、　１０…台車、　１１…移動機構、　２０…マニピュレータ、　３０…制御部、　ＷＡ…駆動輪、　ＷＢ…従動輪、　Ｊ…関節、　Ｌ…リンク、　Ｈ…エンドエフェクタ、ＡＸ…回転軸、　３１…動作生成部、　３２…実行制御部、　３３…目標台車状態生成部、　３４…粗視化モデル動作計画部、　３５…全身動作生成部、　３６…重心指令生成部、　３７…全身指令生成部、　４０…撮像部、　１００…コンピュータ。　１０２…プロセッサ、　１０４…メモリ、　１０６…ストレージ、　１０８…入力装置、　１１０…出力装置、　１１２…記憶媒体読取装置、　１１６…バス、　１１８…制御プログラム、　ＭＤ…モータドライバ、　ＳＡ…支持領域。

Claims

　車輪を有する台車、複数の関節を有するアームを備えるマニピュレータ、及びロボット制御部を備えるモバイルロボットであって、
　前記ロボット制御部は、
　前記モバイルロボットを走行計画に従って走行させるための台車の目標状態である目標台車状態を取得し、合計質量が前記マニピュレータの質量である１又は複数の質点及び前記アームの関節の数よりも少ない数の自由度で前記質点を移動させる仮想機構を備えた粗視化モデルを利用して、前記モバイルロボットの転倒を回避しながら前記台車の状態を前記目標台車状態に近づけるための、前記粗視化モデルの状態である調整モデル状態及び前記台車の状態である調整台車状態を求める粗視化モデル動作計画部と、
　前記調整モデル状態に対応する前記マニピュレータの重心状態を実現するための項を含む目的関数の値を最小化する最適制御であって、前記モバイルロボットが前記粗視化モデルよりも自由度の大きいマニピュレータを備えていること、又は台車が特定の機構形式を備えていることに伴う転倒防止以外の動作目的に関する事項を、前記目的関数の項又は最適化の制約条件として含む最適制御によって、前記マニピュレータの各関節を駆動するモータドライバに対する動作指令である関節指令を生成するとともに、前記車輪を駆動するモータドライバに対する動作指令である車輪指令を生成する全身動作生成部と、
　　前記粗視化モデル動作計画部及び前記全身動作生成部を周期的に実行させることにより、前記モバイルロボットを動作させる実行制御部と
を備える、モバイルロボット。
　前記最適制御における転倒防止以外の動作目的に関する事項は、
　前記目的関数が前記台車の状態を前記調整台車状態に追従させるための項を含むこと、
　前記目的関数が前記マニピュレータの先端位置を目標先端位置に追従させるための項を含むこと、
　少なくとも１の前記関節の回転速度が不等式制約に従うこと、
　前記車輪の回転速度が前記台車の機構上の拘束条件に従うこと、
　前記マニピュレータの重心の移動速度が不等式制約に従うこと、
　前記マニピュレータのツール・センタ・ポイントの速度が不等式制約に従うこと、及び
　前記関節の角度の組み合わせが前記マニピュレータの自己干渉回避の制約に従うこと
のうちの少なくとも１つを含む、請求項１に記載のモバイルロボット。
　前記調整モデル状態及び前記調整台車状態は、前記モバイルロボットのゼロ・モーメント・ポイントの位置を前記モバイルロボットの支持領域内に設定された所定範囲内で移動することを許容し、かつ、将来の一定時間範囲にわたる前記モバイルロボットについての目標状態と予測状態との間の誤差量の項を含む目的関数の値を最小にする前記粗視化モデルの状態及び前記台車の状態である、請求項１に記載のモバイルロボット。
　前記全身動作生成部は、
　前記調整モデル状態の指令に従い、前記マニピュレータの重心に関する重心指令を生成する重心指令生成部と、
　前記重心指令、及び前記調整台車状態の指令に従い、前記マニピュレータ及び前記台車の動作に関する指令を生成する全身指令生成部と
を更に備える、請求項３に記載のモバイルロボット。