JP6168158B2 - 移動ロボットの移動制御方法、および、移動ロボット - Google Patents
移動ロボットの移動制御方法、および、移動ロボット Download PDFInfo
- Publication number
- JP6168158B2 JP6168158B2 JP2015552305A JP2015552305A JP6168158B2 JP 6168158 B2 JP6168158 B2 JP 6168158B2 JP 2015552305 A JP2015552305 A JP 2015552305A JP 2015552305 A JP2015552305 A JP 2015552305A JP 6168158 B2 JP6168158 B2 JP 6168158B2
- Authority
- JP
- Japan
- Prior art keywords
- trajectory
- mobile robot
- center
- contact point
- gravity
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
- B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
- B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
Description
本発明は、移動ロボットの移動制御方法、および移動ロボットに関する。より具体的には、多点に接触しながら移動する移動ロボットが安定に移動できるような軌道を生成するための軌道生成技術に関する。
自律的に移動する機能を備えたロボットが開発されており、典型的には、二足歩行する脚式ロボットがある。二足歩行ロボットにあっては、ZMPを用いた軌道生成と動作制御により、安定な二足歩行移動が実現されている(例えば特許文献1)。
これからのロボットには様々な環境で活躍することが期待されている。例えば、サービスロボットは人と同じ住空間で活動することが期待される。住空間のような環境でロボットが移動するためには、単に脚式の二足歩行ができるだけでは不十分であると考えられる。例えば、図1のような状況が考えられる。図1において、テーブルが壁の近くにあり、そして、テーブルの奥側において壁に近い方にボトルが置かれている。ロボットがこのボトルを掴むためには、単に二足歩行で進むだけでは無理である。ロボットは、壁やテーブルに手を着きながら移動したり、体を傾けて手を伸ばしたりしなければならない。このように、サービスロボットには、複数の任意の点で環境と接触しながら運動を行える能力が必要である。
今後の説明のために、複数の任意の点で環境と接触しながら運動(移動)することを、本明細書では「多点接触移動」と称することにする。
従来の人型ロボットの安定化手法は大多数が2足歩行の安定化に関する技術であり、多点接触移動に対応するための技術はこれまであまり開発されてこなかった。2足歩行も単脚状態と両脚状態とが切り替わりながら移動していく動作であるので多点接触移動の一種と言える。しかし、2足歩行の技術と多点接触移動の技術とで大きく違う点がある。2足歩行技術は多くの場合(ZMPがその代表格)、各接触点(2足で言うと左足と右足)が同一平面内に含まれていることを前提にしている。(このことは、例えば、特許文献2の段落0012にも記載されている。)すなわち、ZMP制御では、本来的には合計6次元である力・モーメントのうち2次元の情報しか表現できておらず、極めて限定された接触状態の遷移しか扱うことができないのである。
多点接触移動に対応すべく開発された制御システムが特許文献2に開示されている。しかしながら、特許文献2に開示の技術には根本的問題として次ぎの点がある。すなわち、特許文献2に開示の技術では、接触点の計画だけではなく、目標重心位置や目標運動量を与えないと安定な軌道を作れないという問題がある。ユーザとしてはロボットに接触点の計画だけを与え、あとは、ロボットが設定された接触点の計画に基づいて自動的に安定な重心軌道を生成して自律的に移動してくれることが望ましい。
そもそも、将来の目標重心位置を予め知ることはできないのであり、制御目標値として未知であるはずの将来の重心位置をユーザが設定するというのは無理がある。(2足歩行の場合で言い換えると、目標ZMP軌道が最初にあって、この目標ZMP軌道に基づいて重心軌道を生成するというのが軌道生成の技術であり、目標重心位置を予め知ることはできない。)
また、特許文献2に開示の技術では、接触点の切替(接触状態の遷移)に伴う力の変化が急峻になってしまい、滑らかな力の遷移が実現できないという問題もある。
上記のような問題は、住環境で活躍するようなサービスロボットだけではなく、狭い空間や障害物が多い空間などで活躍する移動ロボットにも共通する問題である。
本発明の目的は、上記問題に鑑み、接触点計画に基づいて自動的に安定な軌道を生成する移動ロボットの移動制御方法を提供することにある。
本発明の目的は、上記問題に鑑み、接触点計画に基づいて自動的に安定な軌道を生成する移動ロボットの移動制御方法を提供することにある。
本発明の移動ロボットの移動制御方法は、
二以上の移動手段を交互に接地しながら移動する移動ロボットの移動制御方法であって、
前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定工程と、
前記接触点計画設定工程で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成工程と、を備え、
前記軌道生成工程は、
前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
前記評価基準には、
各接触点に対応して設定された重みに基づいて、各接触点に接触力を配分するという基準が含まれる
ことを特徴とする。
二以上の移動手段を交互に接地しながら移動する移動ロボットの移動制御方法であって、
前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定工程と、
前記接触点計画設定工程で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成工程と、を備え、
前記軌道生成工程は、
前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
前記評価基準には、
各接触点に対応して設定された重みに基づいて、各接触点に接触力を配分するという基準が含まれる
ことを特徴とする。
本発明では、
前記評価基準には、さらに、
各接触点に対応して設定された重みに基づいて、各接触点に接触力の微分値を配分するという基準が含まれる
ことが好ましい。
前記評価基準には、さらに、
各接触点に対応して設定された重みに基づいて、各接触点に接触力の微分値を配分するという基準が含まれる
ことが好ましい。
本発明では、
前記評価基準は、接触力および接触力の微分値の二乗和を含む評価関数Jを予測区間内において最小化することである
ことが好ましい。
前記評価基準は、接触力および接触力の微分値の二乗和を含む評価関数Jを予測区間内において最小化することである
ことが好ましい。
本発明では、
前記評価関数Jの最小化問題を拘束条件のもとで求解するにあたり、
前記拘束条件の式には、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを含んでおり、
前記軌道生成工程は、
予測区間内において、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを一度仮設定する工程と、
前記評価関数Jの最小化問題を拘束条件のもとで一度求解した後、その結果に応じて、予測区間内において移動ロボットの動作が安定するように重心鉛直軌道と重心回りモーメント軌道とを修正し、
修正した重心鉛直軌道と重心回りモーメント軌道とを用いて少なくとも一回以上、前記評価関数Jの最小化問題を再計算する
ことが好ましい。
前記評価関数Jの最小化問題を拘束条件のもとで求解するにあたり、
前記拘束条件の式には、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを含んでおり、
前記軌道生成工程は、
予測区間内において、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを一度仮設定する工程と、
前記評価関数Jの最小化問題を拘束条件のもとで一度求解した後、その結果に応じて、予測区間内において移動ロボットの動作が安定するように重心鉛直軌道と重心回りモーメント軌道とを修正し、
修正した重心鉛直軌道と重心回りモーメント軌道とを用いて少なくとも一回以上、前記評価関数Jの最小化問題を再計算する
ことが好ましい。
本発明では、
前記移動手段ごとに重みが設定されており、
前記評価関数Jにおいて、接触力および接触力の微分値にそれぞれ重みを乗算しておく
ことが好ましい。
前記移動手段ごとに重みが設定されており、
前記評価関数Jにおいて、接触力および接触力の微分値にそれぞれ重みを乗算しておく
ことが好ましい。
本発明では、
前記軌道生成工程において予測区間内で軌道を求め、
この予測区間の最初の一点のデータを現在の入力値として移動ロボットを駆動させ、
予測区間を微小時間だけ進めて、前記軌道生成工程を繰り返す
ことが好ましい。
前記軌道生成工程において予測区間内で軌道を求め、
この予測区間の最初の一点のデータを現在の入力値として移動ロボットを駆動させ、
予測区間を微小時間だけ進めて、前記軌道生成工程を繰り返す
ことが好ましい。
本発明の移動ロボットは、
二以上の移動手段を交互に接地しながら移動する移動ロボットであって、
前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定部と、
前記接触点計画設定部で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成部と、を備え、
前記軌道生成部は、
前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
前記評価基準には、
各接触点に対応して設定された重みに基づいて、各接触点に接触力を配分するという基準が含まれる
ことを特徴とする。
二以上の移動手段を交互に接地しながら移動する移動ロボットであって、
前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定部と、
前記接触点計画設定部で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成部と、を備え、
前記軌道生成部は、
前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
前記評価基準には、
各接触点に対応して設定された重みに基づいて、各接触点に接触力を配分するという基準が含まれる
ことを特徴とする。
本発明によれば、設定された接触点の計画に基づいて移動ロボットが自動的に安定な重心軌道を生成して自律的に移動できる。このとき、接触点の切替(接触状態の遷移)に伴う力の変化が滑らかになり、移動ロボットの安定かつ滑らかな移動が実現される。
本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。
(第1実施形態)
本実施形態は移動ロボットの移動制御方法に特徴があり、具体的には、移動ロボットの移動動作を制御するための軌道生成に特徴を有するのであるが、具体的な制御(軌道生成)を説明する前に、制御対象となる移動ロボットのハードウェア構成について予め説明しておく。
(第1実施形態)
本実施形態は移動ロボットの移動制御方法に特徴があり、具体的には、移動ロボットの移動動作を制御するための軌道生成に特徴を有するのであるが、具体的な制御(軌道生成)を説明する前に、制御対象となる移動ロボットのハードウェア構成について予め説明しておく。
図2は、移動ロボットの機械構成の一例を示した図である。
移動ロボット100は、股関節が3軸、膝関節が1軸、足首関節が2軸。さらに、肩関節が3軸(肩ピッチ、肩ロール、肩ヨー)、肘関節が1軸(肘ピッチ)、および、手首関節が3軸(手首ヨー、手首ピッチ、手首ロール)、で夫々構成されている。(移動ロボットの機械構成はこれに限定されないが、手(腕)の自由度は6以上、足(脚)の自由度も6以上は必要である。)
移動ロボット100は、各関節にエンコーダ付きモータ1、2、・・・、28を有している。各関節のモータ(関節駆動手段)1a、2a、・・・、28a(図3)は、各関節の関節角度θ1、θ2、・・・、θ28を調整できる。一方、各関節のエンコーダ(関節角度検出手段)1b、2b、・・・、28bは、各関節の関節角度θ1、θ2・・・、θ28を計測することができる。
移動ロボット100は、股関節が3軸、膝関節が1軸、足首関節が2軸。さらに、肩関節が3軸(肩ピッチ、肩ロール、肩ヨー)、肘関節が1軸(肘ピッチ)、および、手首関節が3軸(手首ヨー、手首ピッチ、手首ロール)、で夫々構成されている。(移動ロボットの機械構成はこれに限定されないが、手(腕)の自由度は6以上、足(脚)の自由度も6以上は必要である。)
移動ロボット100は、各関節にエンコーダ付きモータ1、2、・・・、28を有している。各関節のモータ(関節駆動手段)1a、2a、・・・、28a(図3)は、各関節の関節角度θ1、θ2、・・・、θ28を調整できる。一方、各関節のエンコーダ(関節角度検出手段)1b、2b、・・・、28bは、各関節の関節角度θ1、θ2・・・、θ28を計測することができる。
また、移動ロボット100は、足先部(足平部)および手先部(手の平部)に接触力センサ25を有している。ここで接触力とは6軸力であり、図4に示すように、x軸、y軸およびz軸方向の力fの組(fx、fy、fz)Tと、x軸回り、y軸回りおよびz軸回りの力τの組(τx、τy、τz)Tと、である。
この移動ロボットは、移動時に、右足、左足、右手および左手のうちの一つ以上を床、壁、あるいはテーブルなどに接触させながら移動する。そこで、本明細書の以下の説明では、右足、左足、右手および左手を接触点候補と称することがある。
また、手先、足先というのは、すなわち、移動手段のことである。
また、手先、足先というのは、すなわち、移動手段のことである。
図3は、移動ロボット100の機能ブロック図である。移動ロボット100は、各関節のモータ1a〜28a及びエンコーダ1b〜28bと、接触力センサ25と、コントローラ210と、を備えている。
コントローラ210には、各関節のエンコーダ1b〜28b及び接触力センサ25から、センサ検出値が入力される。また、コントローラ210は、各関節のモータ1a〜28aに対して駆動信号を出力する。
コントローラ210は、主要なハードウェア構成として、制御処理、演算処理等を行うCPU(Central Processing Unit)210aと、CPU210aによって実行される制御プログラム、演算プログラム等が記憶されたROM(Read Only Memory)210bと、処理データ等を一時的に記憶するRAM(Random Access Memory)210cと、を有するマイクロコンピュータにより構成されている。また、これらCPU210a、ROM210b、及びRAM210cは、データバス210dによって相互に接続されている。必要なプログラムを不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。
コントローラ210の機能ブロック図を示すと図5のようになる。
コントローラ210は、ユーザから指令される接触点計画を格納する接触点計画設定部221と、接触点計画に従った動作を安定に実行できる軌道を生成する軌道生成部222と、生成された軌道に従って移動ロボット100の全身動作を実行させる動作制御部223と、を有する。これら機能部の具体的な処理動作については後述する。
コントローラ210は、ユーザから指令される接触点計画を格納する接触点計画設定部221と、接触点計画に従った動作を安定に実行できる軌道を生成する軌道生成部222と、生成された軌道に従って移動ロボット100の全身動作を実行させる動作制御部223と、を有する。これら機能部の具体的な処理動作については後述する。
移動ロボットに多点接触移動を安定して行わせるためには、時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成する技術が必要である。このために本発明者はモデル予測制御を用いることを試みた。最初にモデル予測制御の概要を説明しておく。
(概要説明)
例えば図1に図示したような移動動作をロボットに行わせたいとする。ここでは、2本の腕と2本の脚とを有する人型ロボットに、テーブルの奥側にあるボトルを掴ませるという一連の動作を想定する。この場合、ユーザは、この一連動作(タスク)を実行できるような接触点計画を作成する。つまり、図6のように、手先および足先を、どの順番で、どこに、どのように、着くか、という計画を作成する。図6においては、床、壁およびテーブルにおいて足先および手先を接触させる箇所にマークを付けている。
例えば図1に図示したような移動動作をロボットに行わせたいとする。ここでは、2本の腕と2本の脚とを有する人型ロボットに、テーブルの奥側にあるボトルを掴ませるという一連の動作を想定する。この場合、ユーザは、この一連動作(タスク)を実行できるような接触点計画を作成する。つまり、図6のように、手先および足先を、どの順番で、どこに、どのように、着くか、という計画を作成する。図6においては、床、壁およびテーブルにおいて足先および手先を接触させる箇所にマークを付けている。
この接触点計画は、具体的には図7のようになる。
接触点計画は、左手(LH)、右手(RH)、左足(LF)および右足(RF)に関し、どの順番で、どこに、どのように、着いていくか、という時系列のデータである。
図1、図6および図7の対応関係を簡単に説明する。
当初(t0)左足1本だけで立ち、遊脚である右足を前に振り出し、そして、右足を着地させる(t1)。この動きの計画を移動ロボットに指令するには、左足が最初に着地している床上の接触点の座標PLF1、そのときの左足の姿勢rLF1、そして、右足が着地する床上の接触点の座標PRF1、そのときの右足の姿勢rRF1、を指定することが必要である。ここで、接触点の座標は、空間座標としてP=(Px、Py、Pz)の組で表わされる。また、姿勢というのは、接触点に着地したときの足の裏面の向きであり、例えばオイラー角の組としてr=(rx、ry、rz)として表わされる。(すなわち、rx、ryおよびrzは、ロール、ピッチおよびヨー角をそれぞれ表わす。)足に関する接触点の座標およびそのときの姿勢を指令する形式は今後の説明でも同様なので、以後は適宜説明を省略する。
接触点計画は、左手(LH)、右手(RH)、左足(LF)および右足(RF)に関し、どの順番で、どこに、どのように、着いていくか、という時系列のデータである。
図1、図6および図7の対応関係を簡単に説明する。
当初(t0)左足1本だけで立ち、遊脚である右足を前に振り出し、そして、右足を着地させる(t1)。この動きの計画を移動ロボットに指令するには、左足が最初に着地している床上の接触点の座標PLF1、そのときの左足の姿勢rLF1、そして、右足が着地する床上の接触点の座標PRF1、そのときの右足の姿勢rRF1、を指定することが必要である。ここで、接触点の座標は、空間座標としてP=(Px、Py、Pz)の組で表わされる。また、姿勢というのは、接触点に着地したときの足の裏面の向きであり、例えばオイラー角の組としてr=(rx、ry、rz)として表わされる。(すなわち、rx、ryおよびrzは、ロール、ピッチおよびヨー角をそれぞれ表わす。)足に関する接触点の座標およびそのときの姿勢を指令する形式は今後の説明でも同様なので、以後は適宜説明を省略する。
両足で立った後、左足を振り出し(t2)、左足を前方に着地する(t4)。その間に、左手を壁に着くようにする(t3)。ここで、左手を着く壁上の接触点の座標PLH1、および、そのときの左手の姿勢rLH1を指定する。この接触点の座標は空間座標としてP=(Px、Py、Pz)の組で表わされ、姿勢は接触点に着いたときの手の平の向きとしてオイラー角の組としてr=(rx、ry、rz)として表わされる。
これ以降の接触点計画は図1、図6および図7を対比して頂ければ自明と思われるので省略する。このようにしてユーザにより接触点計画が時系列のデータとして作成される。
このように作成された接触点計画が移動ロボットに入力されると、移動ロボットはこの接触点計画を実現するように軌道を生成して自律的に移動する。このとき、軌道生成にあたって、移動ロボットはモデル予測制御を行う。すなわち、ある時間幅を持った予測区間内で移動ロボットが安定移動できる軌道を生成し、予測区間を微小時間(Δt)ずつシフトさせながら安定動作を行える軌道を順次更新していくようにする。例えば、図8に予測区間の例を示す。現在から所定時間(例えば1.6秒)先の未来までを予測区間として設定する。
そして、この予測区間の間で発散しないように安定な軌道を生成する。この予測区間での動きをイメージしたものが図9である。このように、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。
次の軌道更新周期(Δt秒後)には予測区間をシフトさせ、新たな予測区間において同様に安定な軌道を生成する(図10参照)。
現在だけ、あるいは、現在から次ぎの制御周期(Δt秒)まで、だけを見るのではなく、上記のように、ある程度の未来までを予測区間とし、この予測区間内で発散しない軌道を生成するようにする。これを繰り返すことで移動ロボットは安定に移動することができる。
さて、ここで問題なのは、ある時間幅を持った予測区間のなかで時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成するにはどのようにすればよいか、ということである。
本発明者らは、ある予測区間における安定軌道の生成問題を凸二次計画問題(Quadratic Programming:QP)に帰着させるという着想を得た。具体的には、各接触点における接触力の二乗和と、前記6軸力の微分値の二乗和と、を含む評価関数Jを最小化するという問題を解くことで、多点接触移動の安定軌道を求める。
そこで、次に、この評価関数Jの導出およびその解法(凸二次計画問題への帰着)を説明する。この解法により、ある予測区間内で安定な多点接触移動を実現するための、重心位置、重心速度、接触力および接触力の微分値の時系列データが得られることを示す。
予測に用いる移動ロボットのモデルを改めて図11に示す。移動ロボット全体の慣性を一つの重心Gで表わす。各接触点には6軸力を定義する。
この時、重心Gの並進運動量をP、重心回りの回転運動量(角運動量)をL、接触点の数をnとすると、運動方程式は次のように書ける。
添え字iは接触点のインデックスを表す。例えば接触点の候補が左手、右手、左足、右足の4点であれば、n=4(左手:LH=1、右手:RH=2、左足:LF=3、右足:RF=4)とすればよい。ただし、床や壁に接触していない接触点候補については接触力を0にするように拘束条件を設定しておく。例えば図11の例であれば次のようにする。
(1)式の第1式、第2式を微分すると次の式が得られる。((1)式はベクトルで表現しているが、これをx、y、zに分解した上で、上から順に第1式、第2式・・・第6式と称する。)
本実施形態では、この2式をシステムとして用いる。そして、(1)式の第3から第5式を拘束条件として定式化する。
さらに、予測区間内を図12のように、N個の区間に分割し、(3)式、(4)式を離散化する。(3)式を離散化すると次のようになる。
また、サンプリング点で常に(4)式の拘束が成り立つとすると、(4)式は次のように離散化される。
ここで、パラメータを次ぎのように置く。
θiは、6軸力としての接触力を並べたベクトルである。そして、xは、重心Gのx座標、重心Gのx軸方向速度、重心Gのy座標、重心Gのy軸方向速度、および、各接触点における接触力(6軸力)、を並べたベクトルである。このxを、状態変数xと称する。さらに、uは、接触力(6軸力)の微分値を並べたベクトルである。
このようにパラメータを設定すると、(5)式を次ぎのように記述することができる。
この(8)式は、(j+1)のときの状態変数xを、その一つ前の状態で記述できることを表わしている。(8)式を用いて予測区間内の状態変数xを順に計算していくと次のようになる。
したがって、時系列的に求められる状態変数xを並べて大文字のXで表わすと、状態変数の時系列データXを次のように表わすことができる。
この(10)式は、接触力の微分値(U[k])を入力として、ある予測区間内における移動ロボットの状態遷移を表わす予測モデルとなる。
さて、ここで、本発明者らは、予測区間内において安定な軌道を生成するために次ぎのような評価関数Jを導入した。
さて、ここで、本発明者らは、予測区間内において安定な軌道を生成するために次ぎのような評価関数Jを導入した。
なお、Qi、Riは、適宜設定した重みである。重みの意味や接触点候補にどのように重みを配分するかなどの考え方については後ほど詳しく説明する。ここでは、例えば、接触点候補すべてに力を均等配分すると考えて、Qiはすべて1とし、Riはすべて1×10−6とするようなことをイメージして頂ければよい。
ここで、θiは、6軸力としての接触力の成分を並べたベクトルであった。θi(・)は、接触力成分の微分値を並べたベクトルである。(後ろにカッコで付けたドットは微分を表わすと解釈されたい。)
したがって、(11)式は、「予測区間内で、接触力(6軸力)と接触力(6軸力)微分値との2乗和を最小化する」という意味の式である。(11)式の第1項は、接触力(6軸力)の2乗和を最小化することを意味する。
この第1項には、次の作用が含まれている。
(1)各接触点への接触力を均等分配すること。これにより、重心をできる限り安定な位置に動かすという効果がある。
(2)不必要な内力を打ち消すこと。
(3)接触点の接地安定性を高めること。すなわち、接触面内の反力中心点を接触面の中心に設定するという効果がある。
したがって、(11)式は、「予測区間内で、接触力(6軸力)と接触力(6軸力)微分値との2乗和を最小化する」という意味の式である。(11)式の第1項は、接触力(6軸力)の2乗和を最小化することを意味する。
この第1項には、次の作用が含まれている。
(1)各接触点への接触力を均等分配すること。これにより、重心をできる限り安定な位置に動かすという効果がある。
(2)不必要な内力を打ち消すこと。
(3)接触点の接地安定性を高めること。すなわち、接触面内の反力中心点を接触面の中心に設定するという効果がある。
また、(11)式の第2項は、接触力(6軸力)微分値(6軸力の時間変化率)の2乗和を最小化することを意味する。
この第2項には次の作用が含まれている。
(1)重心の発散を抑制すること。
(2)滑らかに接触力を切り替えていくこと。
この第2項には次の作用が含まれている。
(1)重心の発散を抑制すること。
(2)滑らかに接触力を切り替えていくこと。
これらをQ、Rという重みによって適切に足し合わせることによって、この評価関数Jを最小化するということは、
「高い接触安定性、滑らかな接触力遷移、最低限の内力、といった条件を満たしながら、安定な重心軌道と各接触点の接触力とを出力する」
ということを意味することとなる。
「高い接触安定性、滑らかな接触力遷移、最低限の内力、といった条件を満たしながら、安定な重心軌道と各接触点の接触力とを出力する」
ということを意味することとなる。
(11)式を離散化すると次の式が得られる。
ここで、接触力(6軸力)については状態変数xに含まれているので、(7)式を変形すると次のようになる。
したがって、(12)式の一つめのシグマの計算は次ぎのようになる。
したがって、(12)式は次のようになる。
さらに、(10)式を用いることで評価関数Jを次のように定式化することができる。
次に拘束条件について考える。
拘束条件としては、
(2)式のように非接触の接触点候補に対して6軸力が0という拘束、
(6)式の鉛直方向の力の釣り合いの拘束、および、
(6)式のxy軸回りのモーメントの釣り合いの拘束、
が予測区間の全サンプリング点に渡って成り立つ必要がある。
ここで、例えば、あるサンプリング点jにおいて、i番目とi+2番目の接触点が非接触であったとする。この時、(2)式および(6)式は次のように記述できる。
拘束条件としては、
(2)式のように非接触の接触点候補に対して6軸力が0という拘束、
(6)式の鉛直方向の力の釣り合いの拘束、および、
(6)式のxy軸回りのモーメントの釣り合いの拘束、
が予測区間の全サンプリング点に渡って成り立つ必要がある。
ここで、例えば、あるサンプリング点jにおいて、i番目とi+2番目の接触点が非接触であったとする。この時、(2)式および(6)式は次のように記述できる。
ここで、係数行列C[j]、d[j]の成分はサンプリング点によって異なるが、接触点候補の接触/非接触といった情報や接触点位置は既に与えられている。
重心鉛直軌道(Gz[j])や重心回りモーメント(Lx(・)[j]、Ly(・)[j])については詳しくは後述するが、ここでは、差し当って次ぎのように理解されたい。
もしも、重心鉛直軌道や重心回りモーメントが定まっていない場合には仮の値を設定して利用する。例えば、一つ前の制御サイクルで用いた重心鉛直軌道、重心回りモーメントを仮の値として利用してもよい。もしくは、重心鉛直軌道、重心回りモーメントは予測区間内においては現時点の状態から動かない、と仮設定してもよい。そして、仮の値で一度計算したあとで、得られた結果を用いて重心鉛直軌道、重心回りモーメントを更新し、求解を繰り返す。
このようにして、重心鉛直軌道や重心回りモーメントについても既知の値として計算を進行させ、ずれについては更新計算のなかで修正するとする。
もしも、重心鉛直軌道や重心回りモーメントが定まっていない場合には仮の値を設定して利用する。例えば、一つ前の制御サイクルで用いた重心鉛直軌道、重心回りモーメントを仮の値として利用してもよい。もしくは、重心鉛直軌道、重心回りモーメントは予測区間内においては現時点の状態から動かない、と仮設定してもよい。そして、仮の値で一度計算したあとで、得られた結果を用いて重心鉛直軌道、重心回りモーメントを更新し、求解を繰り返す。
このようにして、重心鉛直軌道や重心回りモーメントについても既知の値として計算を進行させ、ずれについては更新計算のなかで修正するとする。
さて、(17)式が未来の全サンプリング点で成り立つためには、次式が成り立つ必要がある。
この式に(10)式を代入する。
最後に、接触点が安定して接触を保つ為の条件を導入する。
図13に接触点の座標系(上添え字l(エル)がついている)と、接触多角形(接触点の支持多角形)と、を示した。接触点の座標系は、接触点を原点とし、かつ、接触面の姿勢riに合わせて定義されているとする。
ここで、接触点の座標系で定義される接触力(6軸力)θi lを次のように表わす。
θi l=[fix l、fiy l、fiz l、τix l、τiy l、τiz l]T
すると、接触力(6軸力)θi lは、接触面の姿勢行列Φi=rot(ri)を用いて次ぎのように表現できる。なお、rotは、オイラー角を姿勢行列に変換する関数である。
図13に接触点の座標系(上添え字l(エル)がついている)と、接触多角形(接触点の支持多角形)と、を示した。接触点の座標系は、接触点を原点とし、かつ、接触面の姿勢riに合わせて定義されているとする。
ここで、接触点の座標系で定義される接触力(6軸力)θi lを次のように表わす。
θi l=[fix l、fiy l、fiz l、τix l、τiy l、τiz l]T
すると、接触力(6軸力)θi lは、接触面の姿勢行列Φi=rot(ri)を用いて次ぎのように表現できる。なお、rotは、オイラー角を姿勢行列に変換する関数である。
接触点が安定して接触を保つ為には、
(1)接触点が離れないこと、
(2)接触点が滑らないこと、
(3)接触点が剥がれないこと、
という3つの条件を満たす必要がある。
上記3つの要件が理解しやすいように、図14に、接触点が不安定化する場合を例示した。
(1)接触点が離れないこと、
(2)接触点が滑らないこと、
(3)接触点が剥がれないこと、
という3つの条件を満たす必要がある。
上記3つの要件が理解しやすいように、図14に、接触点が不安定化する場合を例示した。
(1)接触点が離れない為には、接触面の鉛直力が正であれば良い。
即ち、次式を満たす必要がある。
即ち、次式を満たす必要がある。
(2)接触点が滑らない為には、接触面に平行な2軸力が摩擦力以下であれば良い。
即ち次式がその条件である。
ただし接触面の摩擦係数をμiとする。
即ち次式がその条件である。
ただし接触面の摩擦係数をμiとする。
(3)接触点が剥がれない為の条件は、接触多角形のh個の頂点座標
(xi1 l, yi1 l),・・・・・(xih l, yih l)
を用いて以下のように表される。
(ただし接触多角形の頂点は反時計回りに順に与えられているとする)。
(xi1 l, yi1 l),・・・・・(xih l, yih l)
を用いて以下のように表される。
(ただし接触多角形の頂点は反時計回りに順に与えられているとする)。
以上、(21)、(22)、(23)式をまとめると次のようになる。
(24)式に(20)式を代入し、j番目のサンプリング点としてインデックスを付け加える。
(25)式は、j番目のサンプリング点におけるi番目の接触点が安定な接触を保つ為に満たすべき条件であり、安定な多点接触動作を実現する為には、全サンプリング点の全接触点において(25)式が成り立つ必要がある。
まずj番目のサンプリング点において全接触点が(25)式を満たす為の条件は次のように書ける。
まずj番目のサンプリング点において全接触点が(25)式を満たす為の条件は次のように書ける。
これが全サンプリング点で成り立つ条件は次のように書ける。
これに(10)式を代入する。
最終的に、(16)、(19)、(28)式により、多点接触時の安定軌道生成問題は次の凸2次計画問題に帰着される。
凸2次計画問題(Quadratic Programming: QP)にまで帰着できてしまえば、凸2次計画問題(QP)自体はよく知られた最適化問題の1つであり、高速な求解が可能である。(求解するとU[k]が求まる。)QPの求解後は、重心軌道と各接触点の接触力とが次ぎのようにして計算される。
このようにして、予測区間内において常に拘束条件(1)−(3)を満たし、かつ、評価関数Jを最小化するX[k+1]が求まる。
X[k+1]というのは、(7)式と(10)式とを見てわかる通り、重心のx座標、重心のx軸方向速度、重心のy座標、重心のy軸速度、および、各接触点の接触力(6軸力)の時系列データである。
X[k+1]というのは、(7)式と(10)式とを見てわかる通り、重心のx座標、重心のx軸方向速度、重心のy座標、重心のy軸速度、および、各接触点の接触力(6軸力)の時系列データである。
さらに、式の展開の途中で説明したように、重心の鉛直軌道(z座標)や重心回りモーメントについては更新計算のなかで修正されたものが得られている(詳しくは後述する)。すなわち、これで予測区間内で安定な移動を実現する軌道を得ることができたわけである。
用語が混乱しないように付言しておく。
上記の演算過程およびこの演算で求められる結果を見て分かるように、ここでいう「軌道」というのは、重心の三次元的軌道に加えて、各接触点で発生すべき接触力(6軸力)の時系列データを含めて考えると分かりやすいであろう。なお、移動ロボットを移動させるための「軌道」といえば一般的には手先、足先の軌道も含む概念であろうが、本実施形態の説明において「軌道」といった場合には、手先、足先の軌道を含んでいてもいなくても大差無い。本実施形態における安定軌道生成という観点から見ると、手先、足先の軌道は、接触点計画、重心軌道および接触力の時系列データから補間によってほぼ自動的に決定され得るものである。したがって、本実施形態の説明において「軌道」といった場合には、手先、足先の軌道を含んでいてもいなくても大差無い。
上記の演算過程およびこの演算で求められる結果を見て分かるように、ここでいう「軌道」というのは、重心の三次元的軌道に加えて、各接触点で発生すべき接触力(6軸力)の時系列データを含めて考えると分かりやすいであろう。なお、移動ロボットを移動させるための「軌道」といえば一般的には手先、足先の軌道も含む概念であろうが、本実施形態の説明において「軌道」といった場合には、手先、足先の軌道を含んでいてもいなくても大差無い。本実施形態における安定軌道生成という観点から見ると、手先、足先の軌道は、接触点計画、重心軌道および接触力の時系列データから補間によってほぼ自動的に決定され得るものである。したがって、本実施形態の説明において「軌道」といった場合には、手先、足先の軌道を含んでいてもいなくても大差無い。
(移動ロボットの制御)
移動ロボットの移動制御方法を説明する。
図15は、移動ロボットの移動制御方法の全体フローである。
移動ロボットの移動制御の工程には、大きく分けて、接触点計画の設定(ST100)と、軌道生成(ST200)と、動作指令(ST300)と、動作制御の実行(ST400)と、がある。
移動ロボットの移動制御方法を説明する。
図15は、移動ロボットの移動制御方法の全体フローである。
移動ロボットの移動制御の工程には、大きく分けて、接触点計画の設定(ST100)と、軌道生成(ST200)と、動作指令(ST300)と、動作制御の実行(ST400)と、がある。
接触点計画の設定(ST100)においては、これまで概略説明してきたように、手先および足先を、どの順番で、どこに、どのように、着くか、という多点接触移動の計画(図7)をユーザが移動ロボットに入力する。入力された接触点計画は、コントローラ210の接触点計画設定部221に記憶される。
軌道生成工程(ST200)について説明する。
図16、図17、図18に軌道生成工程(ST200)の手順を説明するための詳細なフローチャートを示す。軌道生成工程(ST200)はコントローラ210の軌道生成部222にて実行される。
まず、ST201において、軌道生成部222は現在状態x[k]を設定する。ここで、現在状態(x[k])として設定すべきは、現在の重心のx座標、重心のx軸方向速度、重心のy座標、重心のy軸速度、および、各接触点の接触力(6軸力)である。各接触点における接触力は力センサ25からの検出値によって得ることができる。重心のx座標、重心のx軸方向速度、重心のy座標および重心のy軸速度は、現在の移動ロボットの状態から推定して求める値である。
例えば、各エンコーダのセンサ値を取得して移動ロボットの全身姿勢を把握した上で、重心のx座標、重心のx軸方向速度、重心のy座標および重心のy軸速度を求めてもよい。あるいは、想定している姿勢に対して実際の移動ロボットの姿勢がどの程度傾いているか(ずれているか)を把握することによって、重心位置を修正するようにしてもよい。
図16、図17、図18に軌道生成工程(ST200)の手順を説明するための詳細なフローチャートを示す。軌道生成工程(ST200)はコントローラ210の軌道生成部222にて実行される。
まず、ST201において、軌道生成部222は現在状態x[k]を設定する。ここで、現在状態(x[k])として設定すべきは、現在の重心のx座標、重心のx軸方向速度、重心のy座標、重心のy軸速度、および、各接触点の接触力(6軸力)である。各接触点における接触力は力センサ25からの検出値によって得ることができる。重心のx座標、重心のx軸方向速度、重心のy座標および重心のy軸速度は、現在の移動ロボットの状態から推定して求める値である。
例えば、各エンコーダのセンサ値を取得して移動ロボットの全身姿勢を把握した上で、重心のx座標、重心のx軸方向速度、重心のy座標および重心のy軸速度を求めてもよい。あるいは、想定している姿勢に対して実際の移動ロボットの姿勢がどの程度傾いているか(ずれているか)を把握することによって、重心位置を修正するようにしてもよい。
次に、ST202において、重み(Qi、Ri)を設定する。
(先の式の展開でいうと、重みは、(11)式のところで評価関数Jに組み込まれたことを確認されたい。)
重み(Qi、Ri)は、ユーザが予め設定入力しているもので、軌道生成部222は、必要に応じて読み出し、更新設定する。(更新する必要がなければ、再設定の必要はない。)接触点候補すべてに力を均等配分する場合には、Qiをすべて1とし、Riをすべて1×10−6としてもよいが、もっと細かく設定することもできる。この重みを調整することで、接触力の変化をどれぐらい滑らかにするかや、手先にはあまり力を掛けないようにする、などの調整ができる。
(先の式の展開でいうと、重みは、(11)式のところで評価関数Jに組み込まれたことを確認されたい。)
重み(Qi、Ri)は、ユーザが予め設定入力しているもので、軌道生成部222は、必要に応じて読み出し、更新設定する。(更新する必要がなければ、再設定の必要はない。)接触点候補すべてに力を均等配分する場合には、Qiをすべて1とし、Riをすべて1×10−6としてもよいが、もっと細かく設定することもできる。この重みを調整することで、接触力の変化をどれぐらい滑らかにするかや、手先にはあまり力を掛けないようにする、などの調整ができる。
例えば、重みを全部同じにしたとする。すると、演算結果として、着地している手足(各接触点)に均等に力が掛かるような解が得られる。一般的に考えると、この場合が移動ロボットとしては一番安定した形(姿勢)になる。
一方、重みの配分を変えたい場合もある。例えば、足に比べて手が弱い(脆弱な)移動ロボットであって手に荷重を掛け過ぎるとよくないという場合には、手の重みを「大きく」する。重みを大きくするほど、重みに乗算されている手のθiなどが小さくなるので、手には接触力の分配が小さくなる。
あるいは、テーブルに一方の手を着いて、他方の手を前方に大きく伸ばしたいとする。この場合、手足に均等に力が配分されているだけでは無理であり、体をもっと前に傾ける必要がある。体を傾け、重心をもっと前方にシフトさせるためには、一方の手に掛かる力の分配をやや大きくする必要がある。この場合、一方の手の重みを減らし、体を傾けられるようにし、他方の手をより大きく前方に伸ばすようにする、という調整が必要になる。
一般的な動作では重みは変更の必要がないが、状況に応じて軌道更新のタイミングで毎回変更してもよい。
次に、ST203において、予測区間を設定する。
これは、図8で説明したように、現在から所定の時間幅(例えば1.6秒)を予測区間として設定する、ということである。
これは、図8で説明したように、現在から所定の時間幅(例えば1.6秒)を予測区間として設定する、ということである。
次に、ST204において、この予測区間内の接触点位置およびその姿勢(pi、ri)を設定する。これは、接触点計画(図7)のなかから予測区間内の接触点計画を切り出してくることに相当する。
次に、ST205においては、重心鉛直軌道と重心回りモーメントとを仮設定する。
本実施形態のポイントは、次ステップの演算工程(ST206)で(25)式(凸二次計画問題)を解いて、重心のxy方向軌道、各接触点の接触力および接触力微分値、を得ることにある。
しかし、この求解には、各接触点の位置、姿勢、重心鉛直軌道、および、重心回りモーメントが与えられていることが前提となる。予測区間内における各接触点の位置および姿勢は、接触点計画で与えられる。一方、この予測区間内における重心鉛直軌道および重心回りモーメントは、未知である。
そこで、演算工程(ST206)で(25)式(凸二次計画問題)が解けるように重心鉛直軌道および重心回りモーメントを一回仮設定する。
本実施形態のポイントは、次ステップの演算工程(ST206)で(25)式(凸二次計画問題)を解いて、重心のxy方向軌道、各接触点の接触力および接触力微分値、を得ることにある。
しかし、この求解には、各接触点の位置、姿勢、重心鉛直軌道、および、重心回りモーメントが与えられていることが前提となる。予測区間内における各接触点の位置および姿勢は、接触点計画で与えられる。一方、この予測区間内における重心鉛直軌道および重心回りモーメントは、未知である。
そこで、演算工程(ST206)で(25)式(凸二次計画問題)が解けるように重心鉛直軌道および重心回りモーメントを一回仮設定する。
全くの一回目の制御サイクルでは、例えば、重心鉛直軌道、重心回りモーメントは予測区間内においては現時点の状態から動かない、と仮設定してもよい。(例えば、重心高さは変化しない、と仮定してもよい。)もしくは、一つ前の制御サイクルで用いた重心鉛直軌道、重心回りモーメントを仮の値として利用してもよい。予測区間は1.6秒ほどの微小な時間であり、最終的に求まる安定軌道においてもこの間の重心高さの変化はやはり微小である。したがって、一回目の仮計算としては、重心高さが変化しないとしても、前回の制御周期と同じであるとしても計算は進められる。
なお、この後、仮の演算で求まった重心のxy方向軌道でも移動ロボットの動きが安定するように重心鉛直軌道および重心回りモーメントを修正する工程(ST209)がある。そして、この修正した重心鉛直軌道および重心回りモーメントを使って再度演算(ST206)するようにループを回すので、最終的には、重心鉛直軌道を含めて安定な軌道が適切に求まることはご理解頂けよう。
ここで、重心回りモーメントを定義しておく。
移動ロボットをリンク同士が関節でジョイントされたリンク構造体と見なすと、重心回りモーメントは、各リンクに分布した質点の運動によって重心の回りに発生するモーメントの合計値である。
移動ロボットが図19(A)、(B)に示すような多質点モデルで表されており、M個の質点がそれぞれ、
質量:mi、
位置:si=[xi、yi、zi]T
のパラメータを持っているとする。
移動ロボット全体の重心をG=(Gx、Gy、Gz)Tとすると、重心回りモーメントは次のように表わされる。
移動ロボットをリンク同士が関節でジョイントされたリンク構造体と見なすと、重心回りモーメントは、各リンクに分布した質点の運動によって重心の回りに発生するモーメントの合計値である。
移動ロボットが図19(A)、(B)に示すような多質点モデルで表されており、M個の質点がそれぞれ、
質量:mi、
位置:si=[xi、yi、zi]T
のパラメータを持っているとする。
移動ロボット全体の重心をG=(Gx、Gy、Gz)Tとすると、重心回りモーメントは次のように表わされる。
次に、ST206において演算工程を実行する。
具体的には、(29)式の凸二次計画問題を解くということである。
予測区間内において、接触点おのおのに力を適切に分散させつつ、なおかつ、この接触力を実現しながら重心(ここでの重心はxとy)が安定に遷移するような軌道が求められるわけである。具体的には、H、F、P、S、V、Wが求まり、さらに、U[k]が求まる。(このU[k]は接触力微分値のベクトルであったことを思い出されたい。)そして、(30)式によって、Xout[k+1]が求まる。(このXout[k+1]は、重心のx座標、重心のx軸方向速度、重心のy座標、重心のy軸速度、および、各接触点の接触力(6軸力)の時系列データである。)
具体的には、(29)式の凸二次計画問題を解くということである。
予測区間内において、接触点おのおのに力を適切に分散させつつ、なおかつ、この接触力を実現しながら重心(ここでの重心はxとy)が安定に遷移するような軌道が求められるわけである。具体的には、H、F、P、S、V、Wが求まり、さらに、U[k]が求まる。(このU[k]は接触力微分値のベクトルであったことを思い出されたい。)そして、(30)式によって、Xout[k+1]が求まる。(このXout[k+1]は、重心のx座標、重心のx軸方向速度、重心のy座標、重心のy軸速度、および、各接触点の接触力(6軸力)の時系列データである。)
続いて、ST208において、手先、足先の軌道を補間して求める。接触点計画においては、接触点とそのときの姿勢だけが設定されている。そこで、各接触点間を結ぶように手先、足先の補間軌道を求める。すると、図20のように、接触点計画で指示された接触点同士の間が埋まる。
なお。ST208は、次のST209の前であればどのタイミングで実行されてもよいものである。極端に言えば、接触点計画が入力された時点ですぐに手先足先の補間軌道を求めてしまってもよい。
さて、次に、ST209において、重心鉛直軌道と重心回りモーメントとを修正する作業を行う。
まず、重心鉛直軌道を修正する。これまでの工程により、重心のxy方向の軌道と、手先足先の軌道と、が得られている。(さらに、重心鉛直軌道も仮設定している。)これに基づいて、予測区間内における移動ロボットの姿勢の変化を推定することができるが、このとき、膝や腕が伸びきってしまうなど安定しない要因がある場合には安定するように重心鉛直軌道を調整する。例えば、膝が伸びきりそうになったら重心鉛直軌道を下げる、といった調整を行う。予測区間内における移動ロボットの姿勢が決定したら、それに基づいて重心回りモーメントの計算を行う。
まず、重心鉛直軌道を修正する。これまでの工程により、重心のxy方向の軌道と、手先足先の軌道と、が得られている。(さらに、重心鉛直軌道も仮設定している。)これに基づいて、予測区間内における移動ロボットの姿勢の変化を推定することができるが、このとき、膝や腕が伸びきってしまうなど安定しない要因がある場合には安定するように重心鉛直軌道を調整する。例えば、膝が伸びきりそうになったら重心鉛直軌道を下げる、といった調整を行う。予測区間内における移動ロボットの姿勢が決定したら、それに基づいて重心回りモーメントの計算を行う。
さて、次に、ST210において、ST206−ST209を1回以上繰り返したかどうかを判断し、繰り返していなければST206に戻って凸二次計画問題の求解を行う。
(ST208については必要に応じてスキップしてもよいことはもちろんである。)
この繰り返し演算(ST206)によって、修正された重心鉛直軌道および重心回りモーメントを用いて再度Xout[k+1]が求まる。
このようにして、予測区間内において移動ロボットの安定な移動を実現する、重心位置、重心速度および接触力の時系列データが求まった(ST211)。
(ST208については必要に応じてスキップしてもよいことはもちろんである。)
この繰り返し演算(ST206)によって、修正された重心鉛直軌道および重心回りモーメントを用いて再度Xout[k+1]が求まる。
このようにして、予測区間内において移動ロボットの安定な移動を実現する、重心位置、重心速度および接触力の時系列データが求まった(ST211)。
このように、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。すなわち、最初の一点だけを動作指令として動作制御部223に出力する(ST300)。
所定の終了条件を満たすまで周期を更新(ST213)すなわち現時点を微小時間進ませて、先頭(ST201)に戻り、ループを繰り返す。このように、予測区間を微小時間だけ先に進ませながら軌道生成を繰り返す。このようにして多点接触移動を安定に実現する軌道の算出が行われる。
動作指令を受けた動作制御部223が移動ロボット100をどのように制御して動かすか(関節を駆動させるか)は本実施形態のポイントではないが、簡単に説明しておく。
本実施形態においては、重心および手先足先の軌道に加えて、各接触点における接触力も動作指令として与えられる。そこで、まず、各接触点で指示された接触力が発生するように手先足先の位置(軌道)を微修正するという工程が必要になる(ST410)。例えば、手先をテーブルに着いているときに、比較的大きな接触力が指示された場合には、手先をテーブルに押し込むように手先の軌道を微修正する。
本実施形態においては、重心および手先足先の軌道に加えて、各接触点における接触力も動作指令として与えられる。そこで、まず、各接触点で指示された接触力が発生するように手先足先の位置(軌道)を微修正するという工程が必要になる(ST410)。例えば、手先をテーブルに着いているときに、比較的大きな接触力が指示された場合には、手先をテーブルに押し込むように手先の軌道を微修正する。
そして、各関節のモータを駆動して全身を動作させる(ST420)。これで移動ロボットが動作することになる。
なお、動作の実行(ST400)において、接触力制御(ST410)を行ったり、関節が指示した通りに正確に駆動しなかったり、外乱の影響を受けたりして、実機とモデルとの間には当然にずれが生じる。したがって、軌道生成(ST200)のループにあっては、毎回、現在状態xの設定を行う必要がある(ST201)。
以上、本実施形態によれば、設定された接触点の計画に基づいて移動ロボットが自動的に安定な重心軌道を生成して自律的に移動できる。そして、軌道生成のために用いた評価関数Jの意味から分かるように、接触点の切替(接触状態の遷移)に伴う力の変化が滑らかになり、移動ロボットの安定かつ滑らかな移動が実現される。
(実験例)
実験例を説明する。
上記実施形態を2足歩行と4足歩行の場合に適用した実験例を示す。
図21から図23は二足歩行の場合の例である。図21のような接触点計画を移動ロボットに与えた。(接触点は右足と左足だけであり、右手、左手は床や壁に接触しないものとした。)
実験例を説明する。
上記実施形態を2足歩行と4足歩行の場合に適用した実験例を示す。
図21から図23は二足歩行の場合の例である。図21のような接触点計画を移動ロボットに与えた。(接触点は右足と左足だけであり、右手、左手は床や壁に接触しないものとした。)
図22は、実行結果のスナップショットである。
図が多くなりすぎても冗長であるので、途中および最後を間引いたが、接触点計画と付き合わせていただければ内容はご理解頂けるであろう。
安定した2足歩行が実現されており、そして、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
図23(A)、(B)、(C)は、二足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。接触点の切り替えに伴う接触点反力が滑らかに遷移していることがわかる。
全反力中心が左右脚間で滑らかに遷移していることも確認できる。
図が多くなりすぎても冗長であるので、途中および最後を間引いたが、接触点計画と付き合わせていただければ内容はご理解頂けるであろう。
安定した2足歩行が実現されており、そして、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
図23(A)、(B)、(C)は、二足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。接触点の切り替えに伴う接触点反力が滑らかに遷移していることがわかる。
全反力中心が左右脚間で滑らかに遷移していることも確認できる。
同様に、図24から図26は4足歩行の場合である。
図24のような接触点計画を移動ロボットに与えた。
図25が、実行結果のスナップショットである。(前記と同様に途中を間引いた。)
やはり、安定した4足歩行が実現されており、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
図26(A)、(B)、(C)は、4足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
図24のような接触点計画を移動ロボットに与えた。
図25が、実行結果のスナップショットである。(前記と同様に途中を間引いた。)
やはり、安定した4足歩行が実現されており、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
図26(A)、(B)、(C)は、4足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
同様に、図27から図29は4足歩行であって、手足の着き方をランダムにしたものである。
図27のような接触点計画を移動ロボットに与えた。
図28が、実行結果のスナップショットである。
やはり、ランダム4足歩行でも安定した歩行が実現されており、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
図29(A)、(B)、(C)は、ランダム4足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
図27のような接触点計画を移動ロボットに与えた。
図28が、実行結果のスナップショットである。
やはり、ランダム4足歩行でも安定した歩行が実現されており、接触点の移り変わりに従って安定な重心軌道が生成されていることがわかる。
図29(A)、(B)、(C)は、ランダム4足歩行実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
移動ロボットとしてのハードウェア構成としては、二足歩行、4足歩行、さらには、6足でも8足でもよいのであって、手足の数に制限があるわけではない。
ただし、少なくとも一つの接触点がどこかに接地している必要がある。
(接地、というが、壁に手を着く場合も含めて解釈されたい。)
極端なケースであるが、例えば、図30のように、足が1本、手が1本で、床と壁に手足を交互に着けながら移動するような移動ロボット100であってもよい。
移動ロボットとしてのハードウェア構成としては、二足歩行、4足歩行、さらには、6足でも8足でもよいのであって、手足の数に制限があるわけではない。
ただし、少なくとも一つの接触点がどこかに接地している必要がある。
(接地、というが、壁に手を着く場合も含めて解釈されたい。)
極端なケースであるが、例えば、図30のように、足が1本、手が1本で、床と壁に手足を交互に着けながら移動するような移動ロボット100であってもよい。
この出願は、2013年12月10日に出願された日本出願特願2013−254989を基礎とする優先権を主張し、その開示の全てをここに取り込む。
1a-28a…モータ、1b-28b…エンコーダ、25…接触力センサ、100…移動ロボット、210…コントローラ、221…接触点計画設定部、222…軌道生成部、223…動作制御部。
Claims (5)
- 二以上の移動手段を交互に接地しながら移動する移動ロボットの移動制御方法であって、
前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定工程と、
前記接触点計画設定工程で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成工程と、を備え、
前記軌道生成工程は、
前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
前記評価基準には、
各接触点に対応して設定された重みに基づいて各接触点に接触力と、接触力の微分値とを配分するという基準が含まれ、
前記評価基準は、接触力および接触力の微分値の二乗和を含む評価関数Jを予測区間内において最小化することである
ことを特徴とする移動ロボットの移動制御方法。 - 請求項1に記載の移動ロボットの移動制御方法において、
前記評価関数Jの最小化問題を拘束条件のもとで求解するにあたり、
前記拘束条件の式には、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを含んでおり、
前記軌道生成工程は、
予測区間内において、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを一度仮設定する工程と、
前記評価関数Jの最小化問題を拘束条件のもとで一度求解した後、その結果に応じて、予測区間内において移動ロボットの動作が安定するように重心鉛直軌道と重心回りモーメント軌道とを修正し、
修正した重心鉛直軌道と重心回りモーメント軌道とを用いて少なくとも一回以上、前記評価関数Jの最小化問題を再計算する
ことを特徴とする移動ロボットの移動制御方法。 - 請求項1または請求項2に記載の移動ロボットの移動制御方法において、
前記移動手段ごとに重みが設定されており、
前記評価関数Jにおいて、接触力および接触力の微分値にそれぞれ重みを乗算しておく
ことを特徴とする移動ロボットの移動制御方法。 - 請求項1から請求項3のいずれかに記載の移動ロボットの移動制御方法において、
前記軌道生成工程において予測区間内で軌道を求め、
この予測区間の最初の一点のデータを現在の入力値として移動ロボットを駆動させ、
予測区間を微小時間だけ進めて、前記軌道生成工程を繰り返す
ことを特徴とする移動ロボットの移動制御方法。 - 二以上の移動手段を交互に接地しながら移動する移動ロボットであって、
前記移動手段を接地する接触点の位置と接地するときの前記移動手段の姿勢とを時系列のデータとした接触点計画を設定する接触点計画設定部と、
前記接触点計画設定部で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成部と、を備え、
前記軌道生成部は、
前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、所定の評価基準を用いて接触力の微分値の時系列データを求め、
この求まった接触力の微分値の時系列データと前記予測モデルとから、移動ロボットの重心軌道を得るものであり、
前記評価基準には、
各接触点に対応して設定された重みに基づいて各接触点に接触力と、接触力の微分値とを配分するという基準が含まれ、
前記評価基準は、接触力および接触力の微分値の二乗和を含む評価関数Jを予測区間内において最小化することである
ことを特徴とする移動ロボット。
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2013254989 | 2013-12-10 | ||
JP2013254989 | 2013-12-10 | ||
PCT/JP2014/005905 WO2015087504A1 (ja) | 2013-12-10 | 2014-11-26 | 移動ロボットの移動制御方法、および、移動ロボット |
Publications (2)
Publication Number | Publication Date |
---|---|
JPWO2015087504A1 JPWO2015087504A1 (ja) | 2017-03-16 |
JP6168158B2 true JP6168158B2 (ja) | 2017-07-26 |
Family
ID=53370835
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2015552305A Active JP6168158B2 (ja) | 2013-12-10 | 2014-11-26 | 移動ロボットの移動制御方法、および、移動ロボット |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP6168158B2 (ja) |
WO (1) | WO2015087504A1 (ja) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5910647B2 (ja) | 2014-02-19 | 2016-04-27 | トヨタ自動車株式会社 | 移動ロボットの移動制御方法 |
JP6927727B2 (ja) | 2017-03-29 | 2021-09-01 | 本田技研工業株式会社 | ロボットの制御装置 |
WO2021208917A1 (zh) * | 2020-04-14 | 2021-10-21 | 腾讯科技(深圳)有限公司 | 质心位置确定方法、装置、足式机器人及存储介质 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010137289A (ja) * | 2008-12-09 | 2010-06-24 | Sony Corp | 情報処理装置及び情報処理方法、並びにコンピューター・プログラム |
JP5398592B2 (ja) * | 2010-03-01 | 2014-01-29 | 本田技研工業株式会社 | 脚式移動ロボットの運動状態評価装置 |
US8805584B2 (en) * | 2011-11-22 | 2014-08-12 | Disney Enterprises, Inc | Kinematic and dynamic calibration methods for legged robots with force-controlled joints |
JP5661023B2 (ja) * | 2011-12-02 | 2015-01-28 | 本田技研工業株式会社 | 脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置 |
JP5807591B2 (ja) * | 2012-03-06 | 2015-11-10 | トヨタ自動車株式会社 | 脚式歩行ロボットおよびその重心軌道生成方法 |
JP5803751B2 (ja) * | 2012-03-07 | 2015-11-04 | トヨタ自動車株式会社 | 重心軌道生成装置、その生成方法及びプログラム |
-
2014
- 2014-11-26 JP JP2015552305A patent/JP6168158B2/ja active Active
- 2014-11-26 WO PCT/JP2014/005905 patent/WO2015087504A1/ja active Application Filing
Also Published As
Publication number | Publication date |
---|---|
JPWO2015087504A1 (ja) | 2017-03-16 |
WO2015087504A1 (ja) | 2015-06-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5910647B2 (ja) | 移動ロボットの移動制御方法 | |
Feng et al. | 3D walking based on online optimization | |
US7053577B2 (en) | Robot and motion control method of robot | |
JP3972854B2 (ja) | ロボットの運動制御装置 | |
JP4548135B2 (ja) | 脚式ロボットとその制御方法 | |
JP3599244B2 (ja) | ロボット装置、ロボット装置の運動制御装置並びに運動制御方法 | |
US10836040B2 (en) | Desired ZMP trajectory generating device | |
JP5034235B2 (ja) | 制御システム及び制御方法、並びにコンピュータ・プログラム | |
WO2002040224A1 (fr) | Dispositif generateur d'un modele de demarche pour robot mobile pourvu de jambes | |
JP7324932B2 (ja) | 動的計画コントローラ | |
WO2006067904A1 (ja) | 脚式移動ロボットの歩容生成装置 | |
US20120158183A1 (en) | Walking robot and control method thereof | |
JP6421683B2 (ja) | 最適制御装置、最適制御方法及び最適制御プログラム | |
WO2002040222A1 (fr) | Dispositif et procede de commande des mouvements d'un robot marcheur mobile et procede de generation d'unites de mouvement destinees a un robot marcheur mobile | |
Vladareanu et al. | Modeling and hybrid position-force control of walking modular robots | |
Shkolnik et al. | Inverse kinematics for a point-foot quadruped robot with dynamic redundancy resolution | |
JP2017109252A (ja) | 移動ロボットの制御装置 | |
US11878418B2 (en) | Controlling a robot based on constraint-consistent and sequence-optimized pose adaptation | |
US11787055B2 (en) | Controlling a robot using predictive decision making | |
JP6168158B2 (ja) | 移動ロボットの移動制御方法、および、移動ロボット | |
Agrawal et al. | Vision-aided dynamic quadrupedal locomotion on discrete terrain using motion libraries | |
Liljebäck et al. | A 3D motion planning framework for snake robots | |
Oh et al. | Humanoid whole-body remote-control framework with delayed reference generator for imitating human motion | |
Arreguit et al. | Fast multi-contact whole-body motion planning with limb dynamics | |
Hong et al. | Dynamic simulation of modifiable bipedal walking on uneven terrain with unknown height |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20170530 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20170612 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 6168158 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |