JP5163227B2 - 移動ロボットの移動制御方法および移動ロボット - Google Patents

Description

本発明は、移動ロボットの移動制御方法および移動ロボットに関する。具体的には、自律的に二脚歩行または走行する移動ロボットの移動制御方法および移動ロボットに関する。

自律的に二脚で歩行する人型の脚式ロボット（二足歩行型ロボット）の開発が進められている。このような脚式ロボットは、目標ＺＭＰを姿勢の安定をはかる位置に設定しながら歩行軌道の生成を行い、この生成された軌道に基づいた歩行を行うべく各関節の動作制御を実行する。脚式ロボットはクローラ式の移動体とは異なり、二本の脚がそれぞれ着床および離床動作を繰り返すことにより移動が実現される。すなわち、二本の脚を支持脚と遊脚とに交互に交替し、移動方向に遊脚を振り出して着床させていくことでロボットの移動が実現される。

当然のことながら、歩行動作を計画するにあたっては遊脚の足先が床面に衝突しないように遊脚軌道を作成する。
しかし、ロボットが実際に歩行する床面は厳密に平坦面ではなく、傾斜を含む場合も設計値通りではない。そのため、歩行軌道の作成過程で予期していない凹凸や傾斜に遭遇する場合が常にある。予期しない凹凸や傾斜が存在する場合、支持脚の足平が想定角度よりも傾斜してしまうため、ロボットの姿勢全体も傾斜してしまう。すると、遊脚の軌道や着地位置も計画値から異なってきてしまい、当初の想定とは異なる位置に床面が存在する場合には遊脚を床面に衝突させてしまう危険がある。

そこで、特許文献１（特開平５−３０５５８０号公報）では、上記問題の対策として次の構成が開示されている。すなわち、足平の絶対座標における傾斜角度を求め、それから重心の実際位置を推定し、実際の重心位置が目標重心位置に一致するように関節角を制御する構成が開示されている（図１７参照）。これにより予期しない傾斜などで姿勢が傾いた場合でも安定な所期の姿勢に回復させて、常に安定して歩行することができる脚式移動ロボットを実現できる。

特開平５−３０５５８０号公報

しかしながら、前記特許文献１では、ロボット全体の姿勢を回復させることを目的とし、支持脚のまわりにロボット全体を回転させて重心位置を修正するものであるが、ロボット全体の姿勢の回復を図ると応答が遅いという問題がある。その結果、遊脚の着地時までに修正が間に合わず、最悪の場合、遊脚が床面に衝突してしまうことが避けられない。

重心を目標軌道に戻す場合には支持脚の足首を中心にして体全体を回転させることが考えられるが、足首を構成するアクチュエータだけでロボット全体を回動させるには十分なトルクが得られない。原理上、足裏のサイズ以上のモーメントアームが得られないため（ＺＭＰ制約）、アクチュエータの能力を上げても全体を回転させるために十分なトルクが得られない。加えて、着床時の衝撃を緩和するために足首のゲインを下げることが好ましいため、足首部分の関節によって大きなトルクを得ることが難しい。さらには、足裏を柔軟材で構成するような場合、足首部分の角度制御において高精度を求めることには限界もある。そのため、修正動作が遅れたり、修正量が足りなかったりする不都合が生じ、やはり、遊脚が床面に衝突することを避け得ない場合が生じる。そして、支持脚を中心にして姿勢全体を修正する構成では、走行のような空中相では両脚がともに離床しているので、そもそも全く効果を発揮し得えず、激しく脚を床面に衝突させてしまうことになる。姿勢を修正しきれなかった場合、遊脚が地面に衝突してしまい、激しく転倒してロボットが破損するという問題が生じる。

本発明の目的は、予定軌道からずれた場合に高速に修正動作を行って遊脚が床面と衝突することを確実に回避する移動ロボットの移動制御方法および移動ロボットを提供することにある。

本発明の移動ロボットの移動制御方法は、
遊脚と支持脚とを交互に交替する移動動作を自律的に実行する二脚式移動ロボットの移動動作を制御する移動ロボットの移動制御方法において、
移動動作の軌道を計算する軌道計算工程と、
遊脚を離床させてから移動方向に振り出して着床させる移動動作を前記軌道計算工程にて計算された軌道に基づいて実行させる移動動作実行工程と、を備え、
前記移動動作実行工程は、
体幹部および支持脚の少なくとも一方の予定軌道からの偏差を計測する偏差計測工程と、
前記偏差計測工程にて計測された前記偏差に応じて遊脚末端部の予定軌道を修正する修正軌道算出工程と、を有しており、
前記修正軌道算出工程は、支持脚の末端部を回転中心としかつ前記偏差とは逆向きに遊脚末端部を回転させる補正軌道を算出し、この補正軌道を遊脚末端部の予定軌道に加算することによって前記遊脚の予定軌道を修正する
ことを特徴とする。

このような構成において、軌道計算工程にて求められた軌道に従った移動動作が移動動作実行工程にて実行される。すると、移動ロボットの歩行あるいは走行といった移動動作が行われる。ここで、移動動作実行中に偏差計測工程により実際の姿勢（体幹部および／または支持脚）の傾斜が予定軌道に対して有する偏差（ずれ）を計測する。そして、計測された偏差に応じて遊脚の軌道を修正する。すなわち、体幹部あるいは支持脚の偏差とは逆向きで支持脚の末端部を中心とした回転軌跡（補正軌道）を遊脚の末端部の軌道に加算した修正軌道を算出する。すると、体幹部あるいは支持脚の傾斜に応じて遊脚の軌道が当初予定から修正される。

予期しない床面の傾斜があったり、着地緩衝のために支持脚のゲインを下げたりしている場合には、体幹部あるいは支持脚が当初の予定とは異なる傾斜を示すことになる。このような場合に遊脚を当初の軌道に従って振り出すと床面に遊脚をぶつけてしまったり、逆に、空中に着地しようとするなど、移動動作（歩行または走行）を円滑に進めるのに支障となる事故が生じることが起こる。
ここで、従来は、体幹部等の姿勢のずれを計測し、この体幹部のずれを予定軌道に引き戻すようにフィードバックをかけるようにしていた。
しかしながら、ロボットの姿勢全体を立て直すには非常に時間がかかり、応答が間に合わないことが多くなる。
特に、近年にように滑らかで早い移動動作が要求されるレベルになると、姿勢立て直しの修正は間に合わず、ほとんど機能しないという問題が生じる。

この点、本発明にあっては、軌道の修正を遊脚の末端部の軌道に入れる。
軌道修正の対象を遊脚末端部とすることにより、姿勢全体を修正するよりもはるかに早い応答で軌道を修正することができる。よって、移動動作中に遊脚を床面に衝突させるなどの事故を確実に防ぐことができる。さらに、両足が浮く空中相であっても質量の大きい体幹部の慣性を利用して遊脚末端部の軌道を修正することができるため、走行動作中であっても本発明は作用効果を奏する。そして、軌道修正は遊脚に入れるとしているので、本発明を実装するにあたっては遊脚期にあるかどうかだけを判断すればよく、動作フェーズのすべてを監視して判断する（両脚支持期、片脚支持期、空中相を区別する）必要はないため、既存の制御システムに組み込む場合でも簡便である。

ここで、体幹部および／または支持脚の予定軌道からの偏差を計測する構成としては次のものが例として挙げられる。
支持脚の末端関節（足首、つま先）のゲインを下げた状態で着地させることにより足平が床面に倣って接触している場合には、支持脚末端関節のサーボ偏差に基づいて体幹部および／または支持脚の偏差を計測してもよい。または、足裏に測距センサを設けて、足裏が床面から浮いている距離を計測して姿勢の傾斜を求めてもよい。この二つを組み合わせればなおよい。さらに、体幹部に慣性センサ（加速度センサ、ジャイロスコープ）を設けて、検出値を積分することにより体幹部の姿勢を計測してもよい。

本発明では、前記補正軌道の中心となる前記支持脚の末端部は、支持脚の足首関節、支持脚のつま先関節、支持脚の足裏中心付近、および、その時点での相対ＺＭＰの位置、のうちのいずれかであることが好ましい。

本発明では、前記修正軌道算出工程は、前記補正軌道を遊脚の軌道に加算するにあたって前記補正軌道に遊脚の相に応じた重み係数を乗算することが好ましい。
また、本発明では、前記重み係数は、遊脚が離床する時点から滑らかに立ち上がって遊脚の振り出し動作中に最大になり、その後滑らかに減少して遊脚が着床する時点でゼロになるように設定されていることが好ましい。

このような構成において、遊脚の軌道修正は遊脚を振り出す際に最大になり、着地にはゼロとし、次に遊脚となる脚に修正対象を切り替えていく。
このように補正軌道をいれる重みを滑らかに変化させていくので、軌道修正の対象となる脚を交互に入れ替えても動作が滑らかに連続する。そして、本発明にあっては、遊脚を振り出す際の事故を回避することを目的としているので、軌道修正対象を遊脚から支持脚に滑らかに交替させるために重み係数が着床時にゼロになっても特に不利益はない。

本発明の移動ロボットは、
遊脚と支持脚とを交互に交替する移動動作を自律的に実行する二脚式移動ロボットの移動動作を制御する移動ロボットであって、
移動動作の軌道を計算する軌道計算部と、
遊脚を離床させてから移動方向に振り出して着床させる移動動作を前記軌道計算部にて計算された軌道に基づいて実行させる移動動作実行部と、を備え、
前記移動動作実行部は、
体幹部および支持脚の少なくとも一方の予定軌道からの偏差を計測する偏差計測部と、
前記偏差計測部にて計測された偏差量に応じて遊脚末端部の予定軌道を修正する修正軌道算出部と、を有しており、
前記修正軌道算出部は、支持脚の末端部を回転中心としかつ前記偏差とは逆向きに遊脚末端部を回転させる補正軌道を算出し、この補正軌道を遊脚末端部の予定軌道に加算することによって前記遊脚の予定軌道を修正する
ことを特徴とする。

このような構成によれば、前記発明と同様の作用効果を奏することができる。すなわち、軌道修正の対象を遊脚末端部とすることにより、姿勢全体を修正するよりもはるかに早い応答で軌道を修正することができる。よって、移動動作中に遊脚を床面に衝突させるなどの事故を確実に防ぐことができる。

本発明によれば、軌道修正の対象を遊脚末端部とすることにより、姿勢全体を修正するよりもはるかに早い応答で軌道を修正することができる。よって、移動動作中に遊脚を床面に衝突させるなどの事故を確実に防ぐことができる。

以下に、本発明を実施するための最良の形態について図を参照して説明する。
本発明の移動ロボットの移動制御方法に係る第１実施形態について説明する。
図１は、移動ロボット１００の外観を示す図である。
移動ロボット１００は、体幹部１１０に左脚リンク２００と右脚リンク３００とが股関節部１３０を介して揺動可能に設けられている。
脚リンク２００、３００にも膝関節部４１０、足首関節部４２０、つま先関節部４３０をはじめとする関節部が設けられている。
移動ロボット１００の各関節はアクチュエータ（不図示）を備え、各アクチュエータは体幹部に内蔵された制御部１２０により駆動制御される。すなわち、制御部１２０は、後述する各工程をそれぞれ実行する機能部を格納し、本発明の移動制御方法を実現するものである。
移動ロボット１００には、撮像カメラあるいはレーザー送受信手段によって構成されるセンサ手段（不図示）が設けられている。そして、センサ手段にて取得された外部情報に基づいて周囲の環境情報（環境地図）および自己位置情報が制御部１２０にて同時認識される。
環境地図情報および自己位置情報に基づいて、制御部１２０は自律的に移動動作を計画し、各アクチュエータに制御指令を与えて歩行動作（移動動作）を実現する。

次に、フローチャートを参照して、本実施形態に係る移動ロボット１００の移動制御方法について説明する。
図２は、移動ロボット１００の移動制御方法の全体構成を示すフローチャートである。
まず、本実施形態に係る移動ロボット１００の移動制御方法は、初期設定工程（ＳＴ１００）と、遊脚を離床させてから移動方向に振り出して着床させるまでの一歩分動作を実行させる歩行動作実行工程（移動動作実行工程）（ＳＴ２００）と、歩行動作実行工程（ＳＴ２００）における一歩分の動作の後、次の歩行動作実行工程（ＳＴ２００）で遊脚の振り出しを行うまでの間に次の歩行動作実行工程（ＳＴ２００）における動作の軌道を再計算する再計算工程（ＳＴ３００）と、を備える。
以下、順に説明する。

図３は、初期設定工程（ＳＴ１００）の手順を示すフローチャートである。
初期設定工程（ＳＴ１００）では、まず、初期条件の読み込みを行う（ＳＴ１１０）。
これは、操作者によって予め入力された初期条件や初期設定を読み込む工程である。操作者が与える初期条件としては、走行あるいは歩行などの移動の動作態様、移動速度、相対ＺＭＰ軌道、終了条件（継続時間あるいは移動距離）、などが例として挙げられる。動作態様を走行とすると、両脚がともに空中に浮く空中相が存在することになる。動作態様を歩行とすると、遊脚と支持脚とが交互に右脚と左脚とで入れ替わり、その間に空中相を挟むことなく接地相の連続となる。本実施形態では歩行を例にして説明する。
移動速度は、移動ロボット１００の重心の水平方向速度として与えることができる。相対ＺＭＰ軌道は、支持脚の足平の基準点に対する目標ＺＭＰの位置の経時的データである。例えば、相対ＺＭＰ軌道を支持脚の足平の中心に固定するように設定することができる。あるいは、支持脚の足平の内部において後方から前方へ移動するような軌道を設定してもよい。

次に、初期軌道の計算が実行され、動作開始から数歩目（例えば二歩目）の着地までの重心の相対軌道が算出される（ＳＴ１２０）。重心の相対軌道は、初期条件として与えた重心の水平方向速度のもとで実際のＺＭＰが目標ＺＭＰの軌道に一致するように算出される。

次に、ＳＴ１２０にて算出された重心の相対軌道を実現する各関節の角度を算出する（ＳＴ１３０）。
これは、重心の速度（ｑ′）と各関節の速度（θ′）とを関係づけるヤコビ方程式を解くことによって求められる。このようにして実現すべき動作（関節角度）が求められたところで初期設定工程（ＳＴ１００）を終了し、次の歩行動作実行工程（ＳＴ２００）に移行する。

図４は、歩行動作実行工程（移動動作実行工程）（ＳＴ２００）の手順を示すフローチャートである。
歩行動作実行工程（ＳＴ２００）では、まず、実現すべき動作（関節角度）を読み込んで歩行動作を開始し（ＳＴ２１０）、遊脚に設定されている脚を離床させて持ち上げる（図５参照）。
なお、片脚による立脚状態になった段階では脚の関節を構成するアクチュエータのゲインを大きくして設定条件内の最大値にする。ここでいう脚とは遊脚および立脚の両方であり、両脚の関節のゲインを最大化する。この状態で遊脚を移動方向に振り出して着地位置に向けて駆動させる（ＳＴ２２０）。

このようにＳＴ２２０において遊脚の振り出し動作に入ったところで（図６参照）、ＳＴ１２０およびＳＴ１３０にて計算された動作軌道と実際の動作軌道とが一致しているかの監視が行われる（偏差計測工程ＳＴ２２１）。
ここで特に監視対象とするのは、支持脚の関節角度および体幹部１１０の傾斜である。そして、予定軌道と実際の軌道との偏差（ずれ）が予め設定された許容範囲内であるかを判断する（ＳＴ２２２）。偏差が許容範囲内であれば（ＳＴ２２２：ＹＥＳ）、そのまま遊脚の振り出し動作を継続して歩行動作を続ける。

続いて、遊脚が着地位置に着床する直前において遊脚の関節を構成するアクチュエータのゲインを下げる（ＳＴ２３０）。
ゲインを下げる関節としては、遊脚の足首およびつま先部である末端部分の関節４２０、４３０だけでもよい。あるいは、末端部分の関節ではなく、膝関節のゲインを下げてもよい。または、末端部分および膝関節を含めた遊脚の関節すべてのゲインを下げてもよい。下げるゲインの程度は姿勢制御が保たれる範囲内において適宜設定可能である。例えば、最大値の２０％から５０％程度にすることが例として挙げられる。

このように遊脚の関節のゲインを下げた状態で遊脚を着床させる（ＳＴ２４０）（図７参照）。すると、着床時の衝撃が緩和され、滑らかな歩行動作になるとともに脚および体幹部１１０の損壊が防止される。そして、遊脚として着床した脚の足平内にＺＭＰを移し、遊脚と支持脚とを交替させる（ＳＴ２５０）。これで歩行動作の一歩が終了し、次の再計算工程（ＳＴ３００）が開始する。

図８は再計算工程（ＳＴ３００）の手順を示すフローチャートである。
再計算工程（ＳＴ３００）にあっては、まず、現状データの取得を行う（ＳＴ３１０）。すなわち、各関節の実際の角度データを取得する。
本実施形態では、前述の歩行動作実行工程（ＳＴ２００）において、遊脚を着床させる直前に遊脚の関節のゲインを下げる（ＳＴ２３０）。
これにより着床時の衝撃は緩和されるが、同時にロボット自体の重量によって遊脚が沈みこみ過ぎたり、外乱に対する頑健さが低下してしまう。
そのため目標とする関節角度に対して実際の関節角度がずれてしまうことが生じやすい。例えば、前方から後方に向けての外乱が作用した場合、図９に示すように移動ロボット１００が後ろ向きに傾斜してしまう。そこで、実際の関節角度を取得したうえで（ＳＴ３１０）、この現状データと目標関節角度とを対比して関節角度のずれを算出する（ＳＴ３２０）。

このように関節角度のずれを求めたところで（ＳＴ３２０）、この関節角度のずれに応じて目標値の変更を行う（ＳＴ３３０）。ＳＴ３２０において関節角度の目標値からの偏差（ずれ）を求めているところ、角度偏差を減少させるように関節角度の目標値を変更する（ＳＴ３３０）。

ここで、関節角度の目標値を変更するにあたっては、角度偏差がある制限値を超えた場合に目標変更を行うこととして目標の変更を行わない偏差の制限値を設定しておく。
例えば、この偏差の制限値を２°に設定したとし、ある関節において角度の目標値が５０°であったところ実際は４８°になっており角度偏差が２°ある場合、角度偏差を減少させる目標値の設定変更は行わず、目標値は５０°のままで維持する。
一方、ある関節において関節角度の目標値が５０°であったところ実際は４６°になっており角度偏差が４°ある場合、角度偏差を減少させるように目標値の設定変更を行い、この例では目標値を４６°にする。

また、一回で変更する量の上限を設定しておく。
例えば、変更する上限量を５°に設定したとし、ある関節において角度の目標値が５０°であったところ実際は４０°になっており角度偏差が１０°ある場合、角度偏差を減少させるように目標値の設定変更を行い、この例では目標値を４５°にする。

そして、ＳＴ３４０において、支持脚のゲインを戻す。
これは、ＳＴ２３０において着床前に脚関節のゲインを下げていたところ、下げていたゲインを戻して新たに設定した関節角度の目標値に向けて駆動させる。このとき、現状と目標値との差が小さい関節のゲインについては早く戻し、現状と目標との差が大きい関節のゲインはゆっくり戻すようにする。例えば、先のＳＴ３３０にて関節角度の目標値を設定変更したところ、この変更量の大きさに応じてゲインを戻す速さを変化させる。

次に、ＳＴ３５０において、重心ずれを算出する。
すなわち、ＳＴ１２０において算出した重心の目標軌道に対して実際の重心位置がもつ偏差を求める。
関節角度としてはＳＴ３３０にて変更した関節角度の設定目標に基づいて移動ロボット１００の状態を把握し、移動ロボット１００の重心位置を求めるとともに目標軌道からのずれ（偏差）を算出する。重心のずれは、脚末端の関節（例えばつま先の関節）を中心にした回転角Δθとして求める（図１０参照）。そして、ＳＴ３６０において、実際の重心位置に基づいて重心位置を更新する。

ここで、重心位置を更新するにあたっては、目標重心位置と実際の重心位置とが一致するように目標重心位置を更新してもよく、あるいは重心位置ずれが減少するように目標重心位置を更新してもよい。たとえば、重心位置ずれが５０％減少するように目標重心位置を設定更新してもよい。ある程度重心位置ずれを解消するようにすれば累積偏差が増大することもなく、また、目標重心位置を実際の位置に合わせ過ぎると外乱に影響されすぎて却って不安定になる場合もありうる。したがって、目標重心位置を実際の位置にどの程度まで合わせるかは適宜設定しておくことが望ましい。

次に、ＳＴ３７０において、軌道計算を実行する。
すなわち、ＳＴ３６０にて新たに更新された重心位置から出発して、次の一歩の歩行を実現するための重心軌道および脚の軌道を算出する。軌道の再計算にあたって加味する点を含めて図１１を参照して説明する。

図１１は、軌道計算工程（ＳＴ３７０）の手順を示すフローチャートである。
軌道の再計算にあたっては、まず、重心ずれと更新した重心位置の読み込みを行う（ＳＴ３７１）。そして、次に、軌道計算する際の相対ＺＭＰを調整する（ＳＴ３７２）。
相対ＺＭＰは初期設定で入力しているところ、移動ロボット１００の重心が予定の軌道からずれる場合には安定化を図るために相対ＺＭＰの位置を調整する。例えば、相対ＺＭＰを足平の中心に固定したときに重心が後ろにずれる場合には、相対ＺＭＰを前にずらす（図１２中のＰ_１からＰ_２へ）。
このように重心ずれの方向とは逆方向に相対ＺＭＰをずらすことによって安定化を実現するように相対ＺＭＰを調整する。相対ＺＭＰの位置変更量としては、急な変化は避け、たとえば、重心ずれの５％から１０％を一歩の間に改善する程度の変更を加える。

次に、上体姿勢角の調整を行う（ＳＴ３７３）。
関節角度のずれに応じて関節角度の目標値を変更したところ（ＳＴ３３０）、このような変更により上体姿勢が目標値からずれることになってしまう。
脚のゲイン下げや外乱の影響で例えば重心のずれが後ろ方向に発生した場合、移動ロボット１００の上体姿勢は直立よりも後ろに反った姿勢になってしまう。そこで、前回り方向の回転運動量を軌道計算に加味することで上体姿勢を目標値に近づける（図１３参照）。すなわち、重心位置ずれに応じて、重心の位置ずれ方向とは逆向きに上体姿勢を回転させる回転運動量を軌道計算に加える。加味する回転運動量は、一回でずれを全部直すのではなく、ずれ分の２０％〜４０％を一歩の間に改善する程度を加える。移動ロボット１００の目標ＺＭＰが適切に制御させていれば、上体姿勢のずれが移動ロボット１００の歩行安定性に与える影響は小さいが、関節の可動範囲の問題や見た目の印象の観点から上体姿勢の適正化を図ることが好ましい。

次に、脚末端角度の調整を行う（ＳＴ３７４）。これは、ＳＴ３７３にて上体姿勢の目標値からのずれ分を軌道計算に加えることとしたが、これとは別に上体姿勢が目標値からずれたずれ分を末端関節の目標角度に加味する。上体姿勢が目標値からずれた分だけ移動ロボット１００が床面に対して傾斜していると見積もれるため、足平も床面に対して傾斜してしまっていることになる。そこで、上体姿勢の傾き分を足平の角度に加味することで足平を床面に倣わせることができる（図１３参照）。すなわち、上体姿勢の傾き分は重心位置ずれに応じて求められるので、重心位置ずれに応じた量の角度分を脚末端関節の動きの軌道に加算することで足平の床面倣いを適切にすることができる。脚末端関節としては、足首および／またはつま先の関節４２０、４３０とすることで動きの反応が速くなり好ましい。

以上のようにＳＴ３７２からＳＴ３７４の情報を加味して、ＳＴ３７５において軌道の再計算を実行する。
これにより、新たな重心位置から次の一歩の着地までの軌道が計算される。

軌道の再計算が終了したところで（ＳＴ３７５）、次にＳＴ３８０において、算出された軌道を実現する関節角度を算出する。そして、目標関節角度が算出されたところで再計算工程（ＳＴ３００）が終了する。終了条件（ＳＴ４００）が満たされていない場合には（ＳＴ４００：ＮＯ）、歩行動作実行工程（ＳＴ２００）に戻って歩行動作が実行される。
これにより、新たに計算された軌道に基づいて歩行動作が実行され、遊脚の振り出し（ＳＴ２２０）、着床前のゲイン下げ（ＳＴ２３０）および着床（ＳＴ２４０）によって移動ロボット１００の歩行が進行される。

ここで、歩行動作実行時の偏差計測において（ＳＴ２２１）、計測された偏差が予め設定された許容範囲を超えていた場合（ＳＴ２２２：ＮＯ）、ＳＴ２２３の補正軌道算出工程に移行する。
補正軌道算出工程（ＳＴ２２３）では、体幹部１１０の傾斜と反対方向の回転軌道を遊脚の末端に与える補正軌道を算出する（図１４参照）。このときの補正軌道は、支持脚の足首関節を中心として体幹部１１０の傾斜と反対方向の回転運動を与える軌道とする。そして、前記補正軌道算出工程（ＳＴ２２３）にて算出された遊脚末端の補正軌道を遊脚の軌道に加算する（補正軌道加算工程ＳＴ２２４）。
補正軌道を加算するにあたっては、重み係数を乗算したうえで遊脚末端の軌道に加算する。
ここで、前記重み係数は予めその変化値が設定されているものである。
図１５は、補正軌道の加算にあたって補正軌道に乗算する重み係数の変化を示す一例である。
重み係数は、図１５に示されるように、遊脚が離床する時点から滑らかに立ち上がって遊脚の振り出し動作中に最大になり、その後滑らかに減少して遊脚が着床する時点でゼロになるように設定されている。
このような重み係数を乗算した補正軌道が遊脚の予定軌道に加算されることにより、遊脚末端部の修正軌道が算出される。

このようにして修正された軌道（特に遊脚の軌道）に従って遊脚の振り出し動作（ＳＴ２２０）を継続する。すると、図１４に示されるように振り出し動作中に予定軌道が修正され、遊脚末端部は支持脚の足首を中心とする回動が加算された軌道を動く。修正された新たな軌道と実際の動きとの偏差を確認しつつ（ＳＴ２２１、ＳＴ２２２）、遊脚の振り出しを行っていく（ＳＴ２２０）。その後、前述のように、着床前に遊脚の関節のゲインを下げ（ＳＴ２３０）、着床し（ＳＴ２４０）、遊脚と支持脚との交替を行って（ＳＴ２５０）、一回の歩行動作実行工程を終える。一回の歩行動作実行工程（ＳＴ２００）を終えた後は、終了条件に達するまで歩行動作実行工程（ＳＴ２００）と再計算工程（ＳＴ３００）とを繰り返し、歩行を継続する。

ここに、ＳＴ２２３（補正軌道算出工程）、ＳＴ２２４（補正軌道加算工程）により、修正軌道算出工程が構成されている。

終了条件を満たす場合には（ＳＴ４００：ＹＥＳ）、終了処理により移動ロボット１００の移動制御を終了する。

以上、このような制御構成を備える第１実施形態によれば、次の効果を奏することができる。
（１）歩行動作実行工程（ＳＴ２００）中において、偏差計測工程ＳＴ２２１により体幹部１１０および／または支持脚の偏差を計測し、計測された偏差に応じて遊脚末端部の軌道を修正する。このように、軌道修正の対象を遊脚末端部とすることにより、姿勢全体を修正するよりもはるかに早い応答で軌道を修正することができる。よって、移動動作中に遊脚を床面に衝突させるなどの事故を確実に防ぐことができる。

（２）軌道の修正を姿勢全体ではなく遊脚末端部としているので、例えば両足が浮く空中相であっても遊脚末端部の軌道を修正して発明の作用効果を奏することができる（図１６参照）。すなわち、両脚が空中に浮いていても質量の大きい体幹部１１０の慣性を利用して遊脚末端部の軌道を修正することができる。動作が速くて転倒時の損傷が大きい走行中であっても適用可能であるため本発明の効果は大きい。

（３）補正軌道を遊脚の軌道に加算するにあたって前記補正軌道に遊脚の相に応じた重み係数を設定し、この重み係数は、遊脚が離床する時点から滑らかに立ち上がって遊脚の振り出し動作中に最大になり、その後滑らかに減少して遊脚が着床する時点でゼロになるように設定している。このように重み係数を設定することにより、遊脚の軌道修正は遊脚を振り出す際に最大になり、着地にはゼロとし、次に遊脚となる脚に修正対象が滑らかに切り替えられる。その結果、本発明を適用した場合でも歩行動作が滑らかに連続する。

（４）歩行動作実行工程と再計算工程とを交互に繰り返し、次の一歩動作は、再計算された軌道に基づいて実行される。このように一歩動作の後に次の動作を無理なく円滑に進行可能な軌道計算を行うので、姿勢が傾いたりして動作が目標からずれてしまうことがあっても、常に安定した歩行動作を継続させることができる。

（５）関節角度ずれ算出工程にて目標からの角度ずれを算出し、目標関節角度変更工程にて関節角度のずれをある程度受け入れて目標値を変更するので、着地位置のずれを補正して次の動作に円滑に移行させることができる。

（６）遊脚を着床させる前に遊脚の関節のゲインを下げる遊脚関節ゲイン下げ工程を備えることにより、着床時の衝撃を緩和することができる。遊脚のゲインを下げて着床させてしまうと自分の重みによって沈み込み過ぎたり、外乱の影響を受けやすくなってしまうという問題が生じやすくなるが、本発明では再計算工程により再計算された軌道に基づいて次の一歩動作を行い、かつ、遊脚軌道を修正するので、自重による沈みや外乱の影響があっても円滑に移動動作を継続することができる。

（７）重心位置ずれと逆方向に相対ＺＭＰを設定更新する相対ＺＭＰ更新工程を備えるので、移動動作時における目標軌道からのずれを少なくすることができる。これにより、移動動作のより一層の安定化を図ることができる。

（８）重心の位置ずれ方向とは逆向きに上体姿勢を回転させる回転運動量を軌道計算に加える上体姿勢調整工程を備えることにより、適正な姿勢で移動動作を継続することができる。

（９）重心の位置ずれ方向とは逆向きに足平を回動させる動きを軌道計算に加える足平角度調整工程を備えるので、足平を床面に倣わせることができ、ロボット１００をより一層安定化させることができる。

なお、本発明は上記実施形態に限定されず、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれる。
ＳＴ２２１における偏差計測で体幹部および／または支持脚の予定軌道からの偏差を計測する構成については特に説明しなかったが、例えば次の構成を採用することができる。
支持脚の末端関節（足首、つま先）のゲインを下げた状態で着地させることにより足平が床面に倣って接触している場合には、支持脚末端関節のサーボ偏差に基づいて体幹部および／または支持脚の偏差を計測してもよい。または、足裏に測距センサを設けて、足裏が床面から浮いている距離を計測して姿勢の傾斜を求めてもよい。この二つを組み合わせればなおよい。
さらに、体幹部に慣性センサ（加速度センサ、ジャイロスコープ）を設けて、検出値を積分することにより体幹部の姿勢を計測してもよい。

上記実施形態では、ＳＴ２２２において偏差が許容範囲内であるか否かを判定して、許容範囲を超えた場合に修正軌道算出工程（ＳＴ２２３、ＳＴ２２４）を行うとしたが、このような判断工程を入れずに、常に偏差分を修正するとしてもよい。遊脚末端部の応答は早いので、発振等の問題が生じることなく良好な制御性能を維持できる。あるいは、偏差量を修正量に反映させる手前でローパスフィルタまたは周波数整形したフィードバックコントローラを入れてもよい。
上記実施形態においては説明の都合上、歩行動作実行工程ＳＴ２００と再計算工程ＳＴ３００との手順をシリアルに一つずつ説明したが、歩行動作を円滑に進行させるため、歩行動作実行工程ＳＴ２００と再計算工程ＳＴ３００とはパラレルに進行するものである。すなわち、現在の歩行動作実行工程ＳＴ２００を実行中に次の歩行動作実行工程における軌道を再計算工程ＳＴ３００にて計算する。

移動ロボットの外観を示す図。 移動ロボットの移動制御方法の全体構成を示すフローチャート。 初期設定工程の手順を示すフローチャート。 歩行動作実行工程の手順を示すフローチャート。 歩行動作において遊脚を離床させた様子を示す図。 歩行動作において遊脚を振り出している様子を示す図。 歩行動作において遊脚を着床させた様子を示す図。 再計算工程の手順を示すフローチャートである。 着床時に外乱の影響で後傾した様子を示す図。 重心ずれの算出を説明するための図。 軌道計算工程の手順を示すフローチャート。 相対ＺＭＰをずらして歩行姿勢を安定させる様子を説明するための図。 上体姿勢および足平角度を調整する様子を説明するための図。 遊脚の軌道を修正する補正軌道を説明するための図。 重み係数の変化を示す一例を表す図。 空中相において遊脚末端の軌道を修正する様子を示す図。 背景技術において重心位置を修正する様子を説明するための図。

符号の説明

１００…移動ロボット、１１０…体幹部、１２０…制御部、１３０…股関節部、２００…左脚リンク、３００…右脚リンク、４１０…膝関節部、４２０…足首関節部、４３０…つま先関節部。

Claims (5)

1. 遊脚と支持脚とを交互に交替する移動動作を自律的に実行する二脚式移動ロボットの移動動作を制御する移動ロボットの移動制御方法において、
   移動動作の軌道を計算する軌道計算工程と、
   遊脚を離床させてから移動方向に振り出して着床させる移動動作を前記軌道計算工程にて計算された軌道に基づいて実行させる移動動作実行工程と、を備え、
   前記移動動作実行工程は、
   体幹部および支持脚の少なくとも一方の予定軌道からの偏差を計測する偏差計測工程と、
   前記偏差計測工程にて計測された前記偏差に応じて遊脚末端部の予定軌道を修正する修正軌道算出工程と、を有しており、
   前記修正軌道算出工程は、支持脚の末端部を回転中心としかつ前記偏差とは逆向きに遊脚末端部を回転させる補正軌道を算出し、この補正軌道を遊脚末端部の予定軌道に加算することによって前記遊脚の予定軌道を修正する
   ことを特徴とする移動ロボットの移動制御方法。
2. 請求項１に記載の移動ロボットの移動制御方法において、
   前記補正軌道の中心となる前記支持脚の末端部は、支持脚の足首関節、支持脚のつま先関節、支持脚の足裏中心付近、および、その時点での相対ＺＭＰの位置、のうちのいずれかである
   ことを特徴とする移動ロボットの移動制御方法。
3. 請求項１または請求項２に記載の移動ロボットの移動制御方法において、
   前記修正軌道算出工程は、前記補正軌道を遊脚の軌道に加算するにあたって前記補正軌道に遊脚の相に応じた重み係数を乗算する
   ことを特徴とする移動ロボットの移動制御方法。
4. 請求項３に記載の移動ロボットの移動制御方法において、
   前記重み係数は、
   遊脚が離床する時点から滑らかに立ち上がって遊脚の振り出し動作中に最大になり、その後滑らかに減少して遊脚が着床する時点でゼロになるように設定されている
   ことを特徴とする移動ロボットの移動制御方法。
5. 遊脚と支持脚とを交互に交替する移動動作を自律的に実行する二脚式移動ロボットの移動動作を制御する移動ロボットであって、
   移動動作の軌道を計算する軌道計算部と、
   遊脚を離床させてから移動方向に振り出して着床させる移動動作を前記軌道計算部にて計算された軌道に基づいて実行させる移動動作実行部と、を備え、
   前記移動動作実行部は、
   体幹部および支持脚の少なくとも一方の予定軌道からの偏差を計測する偏差計測部と、
   前記偏差計測部にて計測された偏差量に応じて遊脚末端部の予定軌道を修正する修正軌道算出部と、を有しており、
   前記修正軌道算出部は、支持脚の末端部を回転中心としかつ前記偏差とは逆向きに遊脚末端部を回転させる補正軌道を算出し、この補正軌道を遊脚末端部の予定軌道に加算することによって前記遊脚の予定軌道を修正する
   ことを特徴とする移動ロボット。

Images