ロボット、及びその制御方法
【課題】被誘導者を適切に誘導することができるロボット、及びその制御方法を提供すること。
【解決手段】本発明の一態様にかかるロボットは、ハンドを有する腕部と、脚部と、胴体部と、を有し、人２０１を誘導して移動するロボットであって、所定のロボット位置におけるハンドの目標位置とハンドの測定位置との偏差に応じて、ロボットに対する被誘導者の追従状態を推定する追従状態推定部１３６と、追従状態の推定結果に応じて、ロボットの移動速度を調整する歩行調整部１３７と、歩行調整部１３７によって調整された移動速度に応じて前記ロボットを移動させるよう、脚部を駆動する駆動制御部１３８と、を備えたものである。
【解決手段】本発明の一態様にかかるロボットは、ハンドを有する腕部と、脚部と、胴体部と、を有し、人２０１を誘導して移動するロボットであって、所定のロボット位置におけるハンドの目標位置とハンドの測定位置との偏差に応じて、ロボットに対する被誘導者の追従状態を推定する追従状態推定部１３６と、追従状態の推定結果に応じて、ロボットの移動速度を調整する歩行調整部１３７と、歩行調整部１３７によって調整された移動速度に応じて前記ロボットを移動させるよう、脚部を駆動する駆動制御部１３８と、を備えたものである。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット、及びその制御方法に関し、特に詳しくは、被誘導者を誘導するロボット、及びその制御方法に関する。
【背景技術】
【０００２】
脚式移動ロボットにおいて、人と手をつないで歩行する技術が開示されている（特許文献１）。特許文献１の移動ロボットでは、ハンドを介して人と接触したときに、人から作用する外力を検出し、その検出された外力によって歩容を生成している。また、外力の外乱分を吸収するようにコンプライアンス制御を行っている。さらに、人の身長に応じて、重力方向におけるハンド位置を決定している。そして、ロボットが人と手を繋いだ状態で、ロボットが歩行する。このようにすることで、ロボットが人を案内する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−１８５７６３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、ヒューマノイドロボットの介護用途機能として、視覚障害者や高齢者の手を引いて、目的地まで誘導することができれば、好ましい。この場合、誘導される人の安全を考慮して、人の追従具合に応じて、制御する必要がある。例えば、人を誘導する場合、誘導方向に人を引っ張る必要がある。さらに、人に応じた、歩行速度が変わる場合もある。上記の特許文献１では、このような観点から制御を行う方法について何ら開示していないため、適切に誘導することができないという問題点がある。
【０００５】
本発明は、上記の問題点に鑑みてなされたものであり、適切に誘導することができるロボット、及びその制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明の第１の態様にかかるロボットは、ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットであって、移動始点から移動終点までの移動経路に応じて、歩容データを生成する歩容データ生成部と、前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するハンド位置算出部と、前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定する追従状態推定部と、前記追従状態推定部の推定結果に応じて、ロボットの移動速度を調整する調整部と、調整部によって調整された移動速度で前記ロボットを移動させるよう、前記脚部を駆動する駆動制御部と、を備えたものである。
【０００７】
本発明の第２の態様にかかるロボットは、上記のロボットであって、前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させるものである。
【０００８】
本発明の第３の態様にかかるロボットは、上記のロボットであって、前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されているものである。
【０００９】
本発明の第４の態様にかかるロボットは、上記のロボットであって、遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定されるものである。
【００１０】
本発明の第５の態様にかかるロボットは、上記のロボットであって、前記歩幅が、前記偏差に応じて線形に変化することを特徴とするものである。
【００１１】
本発明の第６の態様にかかるロボットは、上記のロボットであって、前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整するものである。
【００１２】
本発明の第７の態様にかかるロボットは、上記のロボットであって、前記ハンド位置算出部が、前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出するものである。
【００１３】
本発明の第８の態様にかかるロボットは、上記のロボットであって、前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するインピーダンス制御部と、をさらに備えるものである。
【００１４】
本発明の第９の態様にかかるロボットは、上記のロボットであって、前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とするものである。
【００１５】
本発明の第１０の態様にかかるロボットは、上記のロボットであって、前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止するものである。
【００１６】
本発明の第１１の態様にかかるロボットは、上記のロボットであって、前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とするものである。
【００１７】
本発明の第１２の態様にかかるロボットの制御方法は、ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットの制御方法であって、移動始点から移動終点までの移動経路に応じて、歩容データを生成するステップと、前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するステップと、前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定するステップと、前記追従状態の推定結果に応じて、ロボットの移動速度を調整するステップと、調整された前記移動速度で前記ロボットを移動させるよう、前記脚部を駆動するステップと、を備えたものである。
【００１８】
本発明の第１３の態様にかかるロボットの制御方法は、上記の制御方法であって、前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させるものである。
【００１９】
本発明の第１４の態様にかかるロボットの制御方法は、上記の制御方法であって、前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されているものである。
【００２０】
本発明の第１５の態様にかかるロボットの制御方法は、上記の制御方法であって、遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定されるものである。
【００２１】
本発明の第１６の態様にかかるロボットの制御方法は、上記の制御方法であって、前記歩幅が、前記偏差に応じて線形に変化することを特徴とするものである。
【００２２】
本発明の第１７の態様にかかるロボットの制御方法は、上記の制御方法であって、前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整するものである。
【００２３】
本発明の第１８の態様にかかるロボットの制御方法は、上記の制御方法であって、前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出するものである。
【００２４】
本発明の第１９の態様にかかるロボットの制御方法は、上記の制御方法であって、前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するものである。
【００２５】
本発明の第２０の態様にかかるロボットの制御方法は、上記の制御方法であって、前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とするものである。
【００２６】
本発明の第２１の態様にかかるロボットの制御方法は、上記の制御方法であって、前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止するものである。
【００２７】
本発明の第２２の態様にかかるロボットの制御方法は、上記の制御方法であって、前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とするものである。
【発明の効果】
【００２８】
本発明によれば、適切に誘導することができるロボット、及びその制御方法を提供するこができる。
【図面の簡単な説明】
【００２９】
【図１】本発明にかかるロボットの全体構成を模式的に示す図である。
【図２】本発明にかかるロボットの制御部の構成を示すブロック図である。
【図３】本発明にかかるロボットの制御方法を示すフローチャートである。
【図４】本発明にかかるロボットの移動経路を示す図である。
【図５】本発明にかかるロボットの移動経路と人の移動経路を示す図である。
【図６】本発明にかかるロボットの移動経路と手先位置軌道を示す図である。
【図７】本発明にかかるロボットの手先位置を中心とする座標系を示す上面図である。
【図８】本発明にかかるロボットの手先位置を中心とする座標系を示す斜視図である。
【図９】本発明にかかるロボットにおいて、追従状態に異常が発生した様子を示す図である。
【図１０】本発明にかかるロボットにおいて、追従状態に異常が発生した様子を示す図である。
【図１１】本発明の実施形態１にかかるロボットの歩幅調整処理を説明するための図である
【図１２】本発明の実施形態にかかるロボットにおいて、歩幅を調整する処理を示すフローチャートである。
【図１３】制御周期を考慮した誘導動作を示すフローチャートである。
【図１４】線形に歩幅を調整する様子を示す図である。
【図１５】実施の形態２にかかるロボットの歩幅調整処理を説明するための図である。
【図１６】実施の形態２にかかるロボットの制御方法で調整された歩幅を示す上面図である。
【発明を実施するための形態】
【００３０】
以下、本発明に係る移動体の実施形態を、図面に基づいて詳細に説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００３１】
実施の形態１．
以下に、図１を参照しつつ本発明の実施の形態１にかかる脚式移動型ロボット（以下、単にロボットという）について説明する。図１は、ロボットの全体構成を模式的に示す正面図である。本実施の形態にかかるロボットは、人の手を引いて誘導するヒューマノイド型ロボットである。すなわち、ロボット１が人を引っ張って歩行することで、人がロボットの後を追従する。
【００３２】
図１は、ロボット１を正面から見た様子を概略的に表す概略図であり、床面Ｆ上をロボット１が歩行する様子を表しているなお、図１においては、説明の便宜上、ロボット１が進行する向き（前後方向）をｘ軸、ロボット１が進行する方向について水平方向に直交する向き（左右方向）をｙ軸、移動体の移動する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これらの３軸からなる座標系を用いて説明する。すなわち、図１中において、前記ｘ軸は紙面の奥行方向、ｙ軸は紙面に向かって左右方向、ｚ軸は紙面中の上下方向を示す。、
【００３３】
図１に示すように、ロボット１は、頭部２と、体幹３と、体幹３に結合された腰部４と、体幹３に接続された右腕５、左腕６と、腰部４に対して回動自在に固定される脚部１０と、を備えた２脚歩行型のロボットである。以下、詳細に説明する。
【００３４】
頭部２は、ロボット１の周囲の環境を視覚的に撮像するための左右一対の撮像部（図示せず）を備えているとともに、体幹３に対して頭部２を鉛直方向に平行な軸周りに回動させることで、周囲の環境を広く撮像する。撮像した周囲の環境を示す画像データは、後述する制御部１３０に送信され、ロボット１の動作を決定するための情報として用いられる。
【００３５】
体幹３は、その内部にロボット１の動作を制御する制御部１３０や、脚部や腕部のアクチュエータ（例えば、モータ）に電力を供給するためのバッテリー（図示せず）等を収容するものである。制御部１３０は、後述するように、脚部１０を駆動し、ロボット１を動かすための歩容データを記憶する記憶領域と、この記憶領域に記憶された歩容データを読み出す演算処理部と、脚部１０に含まれるモータを駆動する駆動制御部と、を備えている。これらの各構成要素は、体幹３の内部に設けられたバッテリー（図示せず）から電力を供給されることで動作する。
【００３６】
また、演算処理部は、記憶領域に記憶された歩容データを読み出すとともに、読み出した歩容データによって特定されるロボット１の姿勢を実現するために必要な脚部１０の関節角を算出する。そして、このように算出した関節角に基づく信号を駆動制御部に送信する。歩容データの生成、及び歩容データに基づく歩行の基本的制御については公知の技術を用いることができる。
【００３７】
駆動制御部は、演算処理部より送信された信号に基づいて、脚部を駆動するための各モータの駆動量を特定し、これらの駆動量でモータを駆動させるためのモータ駆動信号を各モータに送信する。これによって脚部１０の各関節における駆動量が変更され、ロボット１の動きが制御される。
【００３８】
また、演算処理部は、読み出した歩容データに基づいてモータの駆動を行うように指令するほか、ロボット１に組み込まれたジャイロや加速度計などセンサ（図示せず）からの信号を受けて、モータの駆動量を調整する。また、レーザセンサなどを設けて、床面Ｆまでの距離や障害物までの距離を検出してもよい。ジャイロセンサや加速度計やレーザセンサなどの各種センサは、例えば、体幹３や腰部４に設けられる。このように、センサにより検出したロボット１に作用する外力や、ロボット１の姿勢などに応じて脚部１０の関節角を調整することで、ロボット１が安定した状態を維持することができる。
【００３９】
右腕５および左腕６は、体幹３に対して回動自在に接続されており、肩部分、肘部分および手首部分に設けられた関節部分を駆動することにより、人間の腕部と同様の動きを行うことができる。また、手首部分の先端に接続された手先部は、図示を省略するが物体を把持するためのハンド構造を備えており、ハンド構造に組み込まれた複数の指関節を駆動することで、様々な形状の物体を把持することが可能となる。例えば、指関節を駆動することで、誘導する人の手を握ることができる。なお、人を誘導する際には、ロボット１が人の手を握ってもよく、人がロボット１のハンドを握ってもよい。さらには、ロボット１と人が互いに手とハンドを握り合っていても良い。
【００４０】
腰部４は、体幹３に対して回動するように接続されており、歩行動作を行う際に腰部４の回動動作を組み合わせることで、脚部１０を駆動するために必要な駆動エネルギーを低減させることができる。体幹３と腰部４が胴体部分を構成する。
【００４１】
２足歩行を行うための脚部１０（右脚２０、左脚３０）は、右脚２０と左脚３０とから構成されている。詳細には、図２に示すように、右脚２０は右股関節２１、右上腿２２、右膝関節２３、右下腿２４、右足首関節２５、右足平２６を備え、同様に、左脚３０は左股関節３１、左上腿３２、左膝関節３３、左下腿３４、左足首関節３５、左足平３６を備えている。
【００４２】
そして、右脚２０および左脚３０とは、図示しないモータからの駆動力が、同じく図示しないプーリおよびベルトを介して伝達されることで、各関節部が所望の角度に駆動され、その結果、脚部に所望の動きをさせることができる。
【００４３】
なお、本実施形態においては、脚部１０（右脚２０および左脚３０）は、下腿を膝関節回りに前方側に持ち上げると、人間の脚部のように、上腿と下腿が後方側に向かって開いた状態（上腿の延長線よりも後方側に、下腿が膝関節回りに回転した状態）となる。
【００４４】
記憶領域に記憶された歩容データは、脚部１０の移動量に対応づけて、脚部１０の足平（右足平２６、左足平３６）の先端（足先）の位置と、移動体本体の位置とを、ロボット１の移動する空間を定める座標系（例えばｘｙｚ座標系）において経時的に指示するものである。
【００４５】
次に、人の手を引くために設けられた腕部について具体的に説明する。右腕５と左腕６は、例えば、複数の自由度を持つアーム機構であり、ハンド（手先）を所望の位置に駆動することができる。具体的には７、又は８自由度のアーム機構を用いることができる。もちろん、これ以外の自由度を有するアーム機構を用いてもよい。図示しないモータからの駆動力が、同じく図示しないプーリおよびベルトを介して伝達されることで、各関節部が所望の角度に駆動され、その結果、脚部に所望の動きをさせることができる。
【００４６】
右腕５は、右ハンド５１、右手首関節５２、右前腕５３、右肘関節５４、右上腕５５、右肩関節５６を有している。右手首関節５２、右肘関節、右肩関節５６は、回転関節機構などを有している。これらの関節が制御部１３０によって制御されることで、右腕５が右ハンド５１の手先位置を所望の位置にすることができる。さらに、右ハンド５１には、右ハンドセンサ部５７が設けられている。
【００４７】
同様に、左腕６は、左ハンド６１、左手首関節６２、左前腕６３、左肘関節６４、左上腕６５、左肩関節６６を有している。左手首関節６２、左肘関節６４、左肩関節６６は、回転関節機構などを有している。これらの関節が制御部によって制御されることで、左腕６が左ハンド６１の手先位置を所望の位置にすることができる。さらに、左ハンド６１には、左ハンドセンサ部６７が設けられている。なお、常時同じ腕によって誘導する場合は、一方の腕のみにセンサ部が設けられていればよい。以下、右ハンドセンサ部５７、及び左ハンドセンサ部６７のうちの一方をハンドセンサ部と称し、特に言及がない場合、人の手を引っ張っている方のハンドに設けられた方のハンドセンサ部を指す。
【００４８】
制御部１３０は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、制御部１３０は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。もちろん、制御部１３０は、物理的に一つの構成に限られるものではない。制御部１３０は、脚部１０、右腕５、左腕６を制御するための演算処理を行う。すなわち、各関節のアクチュエータ（例えば、モータ）に対して指令値を出力する。
【００４９】
右ハンドセンサ部５７、左ハンドセンサ部６７は、ユーザの追従状態を推定するための一つ又は複数のセンサを有している。例えば、右ハンドセンサ部５７、左ハンドセンサ部６７は、タッチセンサ、力センサ、接触センサ等を有している。例えば、ロボットハンドの手の平部分にタッチセンサや接触センサを設けることで、ユーザの手を引いているか否かを判断することができる。さらに、力センサは、人によって与えられる力を検出する。力センサは、自重の影響を差分してもよい。
【００５０】
ハンドセンサ部から出力されるセンサ信号を用いた制御に付いて、図２、及び図３を用いて説明する。図２は、制御部１３０の構成を示すブロック図である。図３は、制御部１３０による制御方法を示すフローチャートである。ロボット１がハンドによって、人（被誘導者）を誘導している。従って、制御部１３０は一方のハンドに設けられたハンドセンサ部から出力されるセンサ信号に応じて制御を行っている。以下の説明では、左ハンド６１が人の手を握った状態で、誘導しているとして説明する。
【００５１】
制御部１３０は、経路算出部１３１、歩容データ算出部１３２、手先位置算出部１３３、インピーダンス制御部１３４、接触状態判別部１３５、追従状態推定部１３６、歩行調整部１３７、駆動制御部１３８を備えている。さらに、制御部１３０には、左ハンドセンサ部６７からのセンサ信号が入力されている。なお、図２では、関節モータのエンコーダの出力値を省略している。
【００５２】
まず、人を誘導するためのロボット経路を計画する（ステップＳ１０１）。まず、経路算出部１３１は、移動始点から移動終点までの移動経路を算出する。経路算出部１３１は、左ハンドセンサ部６７からのセンサ信号によって、左ハンド６１が誘導される人の手を握ったことを検知する。具体的には、接触センサやタッチセンサ等からのセンサ信号によって、接触していることを検出する。これにより、誘導可能状態になったことが認識される。そして、現在位置（移動始点）から入力された目的位置（移動終点）までの移動経路を算出する。さらに、レーザセンサやカメラなどによって、障害物の位置を測定して、疎障害物を避けるように移動経路を算出する。なお、レーザセンサやカメラ等の測定結果に応じて、適宜移動経路を変更するようにしてもよい。すなわち、障害物が移動した場合、その障害物を避けるように新たな移動経路を算出してもよい。
【００５３】
さらに、障害物２００の位置の変化に応じて、移動経路を更新しても良い。例えば、ロボット１に設けられたカメラやレーザセンサで障害物を検出する。また、移動環境中に設置されたカメラによって、障害物を検出しても良い。そして、障害物の位置が変化した場合は、移動経路を変更しても良い。すなわち、障害物が移動経路の近傍に移動して、ロボット１、及び人２０１の移動の妨げとなるおそれが生じた場合、移動経路を変更する。
【００５４】
歩容データ算出部１３２は、移動経路における歩容データを算出する。歩容データとは、脚部１０の移動量に対応づけて、脚部１０の足平（右足平２６、左足平３６）の先端（足先）の位置と、ロボット１本体の位置とを、ロボット１の移動する空間を定める座標系（例えばｘｙｚ座標系）において経時的に指示するものである。なお、歩容データの生成については、従来から広く用いられている方法を用いることができるため、説明を省略する。右足平２６と左足平３６が交互に着地することで、ロボット１が歩行する。このため、歩容データには、足平が着地している位置（足付き位置）が含まれている。このように、経路算出部１３１と歩容データ算出部１３２によって、移動終点までの経路計画が実行される。すなわち、両脚で歩行するためのフットプランが計画される。なお、歩容データは、移動経路全体の歩容データを算出しなくてもよい。例えば、移動始点に近い一部の経路だけ歩容データを算出して、歩行中に随時歩容データを算出するようにしてもよい。
【００５５】
次に、歩行中の手先位置（ハンドの位置）を拘束制御する（ステップＳ１０２）。そのため、手先位置算出部１３３は、その歩容データに基づいて、目標手先位置を算出する。ここでは、ユーザの手を把持している左ハンド６１の手先位置を算出する。ロボット１の本体がある位置にあるとき、腕（右腕５、左腕６）の関節角度によって手先位置が決まる。例えば、歩行中の手の振り幅等によって、移動経路上のあるロボット位置における腕関節角度が決まる。手先位置は、各関節の角度や、関節を結ぶリンクの長さに基づいて、算出される。このようにして、歩容データに基づいて、移動経路に沿った手先位置の軌道が算出される。そして、歩行中の手先位置を拘束制御する。
【００５６】
そして、進行方向に一定の力で人の手を引くように、手先をインピーダンス制御する（ステップＳ１０３）。例えば、インピーダンス制御部１３４は、目標手先位置と力センサ等からのセンサ信号に応じて、手先位置のインピーダンス制御を行う。具体的には、左ハンドセンサ部６７に設けられた力センサによって、人から左ハンド６１が受ける力が検出される。この力に応じてインピーダンス制御する。人を把持している左腕６を制御するためのアーム指令値を出力する。このアーム指令値が左腕６の関節モータに入力されることで、手先の位置が制御される。これにより、左手首関節６２、左肘関節６４、左上腕６５の関節モータが所望の回転トルク、回転速度で駆動する。よって、適切な力で人の手を引くことができる。
【００５７】
次に、手先の接触状態と人からの反力を検出する（ステップＳ１０４）。左ハンドセンサ部６７の力センサが反力を検出する。さらに、左ハンドセンサ部６７の接触センサやタッチセンサが、人がロボット１に左ハンド６１と接触しているか否かを検出する。接触状態判別部１３５は、これらのセンサ信号に応じて、人とロボット１の左ハンド６１との接触状態を判別する。これにより、非接触状態か、接触状態かを判別することができる。そして、追従状態推定部１３６は、手先のインピーダンス制御結果と接触状態判別部１３５の判別結果等に基づいて、人の追従状態を推定する。
【００５８】
まず、追従状態に異常があるか否かが判定される（ステップＳ１０５）。追従状態推定部１３６は、例えば、人の手がハンドから離れていて誘導されていない状態を異常状態と推定する。さらには、追従状態推定部１３６は、人からロボット１が受ける力がしきい値を越えた場合を異常状態と推定する。このような場合（ステップＳ１０５のＹＥＳ）、移動を停止する（ステップＳ１０６）。例えば、支持脚の駆動を停止して、かつ遊脚が着地した時点で遊脚の駆動を停止する。
【００５９】
異常状態でない場合、追従状態推定部１３６は、追従速度が速い状態か、追従速度が遅い状態かを推定する。換言すると、追従状態推定部１３６は、目標手先位置とインピーダンス制御結果に応じて、人の歩行速度と、ロボットの歩行速度の差を算出する。そして、追従速度が速い状態や遅い状態の場合、手先の基準位置との偏差に応じて、歩幅や歩行周期を調整して次の一歩を算出する（ステップＳ１０６）。歩行調整部１３７は、追従状態の推定結果に応じて、歩行調整を行う。例えば、人の追従速度が速い場合は、ロボット１の歩行速度を速くし、人の追従速度が遅い場合は、ロボット１の歩行速度を遅くする。そのため、歩行調整部１３７は、歩幅や歩行周期を調整する。
【００６０】
そして、駆動制御部１３８は、歩行調整部１３７で調整された結果に応じて、脚部の駆動を制御する。すなわち、脚部の関節モータに対して、歩行指令値を出力する。これにより、右股関節２１、左股関節３１、右膝関節２３、左膝関節３３、右足首関節２５、及び左足首関節３５の関節モータが、所望のトルク、回転速度で駆動する。よって、ロボット１が、適切な速度で、移動経路に沿った所望の方向に歩行する。
【００６１】
目標位置、すなわち移動終点に到達したか否かを判定する（ステップＳ１０８）。目標位置に到達した場合は、歩行を停止する（ステップＳ１０９）。すなわち、脚部のモータの駆動を停止する。さらに、左ハンドによって人を誘導する力を０にする。一方、目標位置に到達していない場合は、ステップＳ１０２からの処理を繰り返す。そして、目標位置に到達するまで、あるいは、異常状態と推定されるまで、歩行を継続する。このようにして、歩行制御が行われる。以下に、上記の処理について詳細に説明する。
【００６２】
（経路計画）
経路計画について、図４、及び図５を用いて説明する。図４は、移動経路に付いて模式的に示す図である。図５は、障害物を回避する移動経路を算出する方法について説明する図である。以下、経路算出部１３１による処理に付いて説明する。
【００６３】
ここでは、既に知られている各種の方法を利用して、移動経路を算出することができる。図４に示すように、障害物２００を回避するため、移動経路の移動始点Ｓから移動終点Ｇまでの移動経路Ｒを算出する。例えば、確率的探索手法であるＲＲＴ（Ｒａｐｉｄｌｙ Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）やＡ＊アルゴリズムにより、離散的に経路を計画する。そして、各離散点をサブゴールＳＧとして、サブゴール間をスプライン曲線、ペジエ曲線、線形等で補完する。これにより、移動経路の軌跡が生成される。さらに、移動経路の生成には、ロボット１が予め記憶している移動環境の地図情報を利用しても良い。さらには、カメラやレーザセンサで取得された障害物の情報を利用してもよい。
【００６４】
また、経路計画時には、図５に示すように、人の幅を考慮したマージンを持って移動経路を算出する。ロボット１の中心点から、人２０１を引く側のロボット１の手先位置までの距離（Ｌｒ）と、人２０１の幅（Ｌｈ）を考慮する。そして、距離Ｌｒと幅Ｌｈの和Ｌａｌｌの幅だけ余裕を持って、移動経路を算出する。すなわち、移動経路から障害物２００までの距離がＬａｌｌよりも長くなるようにする。これにより、人２０１が障害物と干渉しないように歩くことができる。もちろん、人２０１と反対側の手については、幅Ｌｈを考慮する必要はない。このように、誘導されている人２０１が、障害物２００を回避できるようにマージンを持たせて、移動経路を算出する。これにより、人が障害物２００を意識せずに歩くことができるようになる。なお、人の幅Ｌｈは予め設定しておくことが好ましい。もちろん、誘導される人に２０１応じて、人の幅Ｌｈを変えてもよい。ロボット１と人２０１を合わせた幅は、２Ｌｒ＋Ｌｈとなり、この幅が通過できるような移動経路Ｒが算出される。
【００６５】
そして、この移動経路Ｒに沿って歩くように、歩容データ算出部１３２が歩容データを算出する。ここでは、両足の足付き位置が算出される。このとき、理想的な、ロボット１の手先位置の軌道は、Ｒｄとなり、人の手（ロボット１と反対側の手）の軌道は、Ｒｈとなる。
【００６６】
（拘束制御）
手先位置の拘束制御について図６を用いて説明する。図６を移動経路に従って移動するロボット１と人２０１を示す上面図である。さらに、図６では、右足平２６の足付き位置Ｔｒと左足平３６の足付き位置Ｔｌの軌跡が示されている。足付き位置とは、それぞれの足平が接地する位置を示し、歩容データに応じて算出することができるものである。右足平２６の足付き位置Ｔｒは移動経路Ｒの右側にあり、左足平３６の足付き位置Ｔｌは移動経路Ｒの左側にある。まず、拘束制御しない場合の問題点について説明する。
【００６７】
２足歩行ロボットで安定して歩行するためには、支持脚が形成する支持多角形内に静歩行なら重心、動歩行ならＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）を収めるように動作を行う必要がある。一般的に、２足歩行ロボットは、上記の制約にしたがって歩行する。このため、腰中心は、移動経路の進行方向に対して、横方向の変動を伴う軌道Ｒｂとなる。例えば、右足平２６が接地しているときは、腰中心軌道Ｒｂが移動経路Ｒの右側になり、左足平３６が設置しているときは、腰中心軌道Ｒｂが移動経路の左側となる。このように、歩行中の腰中心軌道Ｒｂは支持脚よりになるため、腰中心軌道Ｒｂは移動経路Ｒに対して横方向変動量を伴う。
【００６８】
拘束制御を行わない場合、腰中心軌道Ｒｂが横方向変動につれて、手先位置も変動してしまう。すなわち、ロボット１の歩行にしたがって、ロボット１の腰中心と、手先位置が変化することになる。すると、移動中に、人２０１の手が左右に揺れることになってしまう。すなわち、腰中心の横方向変動に合わせて、人２０１がロボット１から力を受けてしまう。このような力を人が受けるのは好ましいことではない。例えば、人が横方向の力を受けると、移動方向を誤って認識してしまう。
【００６９】
そこで、本実施形態では、移動中における、手先位置の軌道Ｒｄの横方向変動量が、腰中心軌道Ｒｂの横方向変動量よりも小さくなるように、拘束制御している。ここで、横方向変動量とは、各軌道と移動経路Ｒの距離のばらつきを示すものである。したがって、手先位置軌道Ｒｄが移動経路Ｒと平行になれば、手先位置軌道Ｒｄと移動経路との距離は一定になる。手先位置軌道Ｒｄが移動経路と平行な場合、横方向変動量は０となる。一方、腰中心軌道Ｒｂは移動経路Ｒをまたぐように左右に揺れている。このため、移動経路上の位置に応じて腰中心軌道Ｒｂと移動経路の距離が変化して、距離のばらつきが大きくなる。このように、手先位置軌道Ｒｄが移動経路Ｒとほぼ平行になるように、ロボット１の手先位置を拘束制御する。
【００７０】
腰中心位置が横方向変動を伴っても、誘導する手先は横方向変動がないように手先位置を拘束制御する。制御部１３０は、この拘束制御をリアルタイムに行う。実際には、上記の重心やＺＭＰの制約を満たしつつ、手先位置の軌道Ｒｄが移動経路と平行になるようにする。すなわち、制御部１３０は、手先位置が移動経路Ｒと一定距離を保って移動するように制御する。この制御を行うため、制御部１３０は、例えば、逆運動学を解くことで、全身の関節角度を求める。逆運動学は解析解で解いてもよく、ヤコビアンを用いた数値処理で解いてもよい。具体的には、ロボット１のリンク構成に応じて、精度と計算負荷を考慮して、全身の関節角度を求めればよい。これによって、手先位置が移動経路Ｒに沿って移動するようになる。手先位置の軌道Ｒｄが移動経路Ｒと平行になり、横方向変動量を減少することができる。よって、人が誘導方向について、誤って認識するのを避けることができる。
【００７１】
（インピーダンス制御）
手先位置のインピーダンス制御に付いて、図７及び図８を用いて説明する。図７は、手先位置のインピーダンス制御の説明を明確にするための座標系を示す上面図である。図８は、手先位置のインピーダンス制御を示す斜視図である。以下、インピーダンス制御部１３４による処理に付いて説明する。
【００７２】
まず、図７、及び図８に示すように、右手系のローカル座標系を定義する。すなわち、ロボット１の移動方向をＸ方向とし、水平面内において、そのＸ方向と垂直な方向をＹ方向とする。さらに、Ｘ方向及びＹ方向と垂直な方向、つまり鉛直方向をＺ方向とする。そして、手先位置軌道上の点を原点とする。なお、Ｘ方向は、その地点における移動経路Ｒと平行になる。そして、以下のようなポリシーでインピーダンス制御する。なお、上記の通り、インピーダンス制御に用いる手先位置は、手先位置算出部１３３によって算出されており、左ハンド６１に加わる力は左ハンドセンサ部６７によって測定されている。
【００７３】
（ａ）Ｘ軸（進行方向）
進行方向に対して、人２０１を引っ張る力Ｆｒを提示できるようにインピーダンス制御する。具体的には、一定の軽い力（Ｆｒ：５〜１０Ｎ程度）で人２０１を引っ張るような力を生じさせる。さらに、人２０１が逆向きに反力（５〜２０Ｎ程度）を加えた場合は、逆向きにも追従できる程度の柔らかさで制御する。
【００７４】
（ｂ）Ｙ軸（横方向）
人の反力に対して、ある程度倣う硬さにインピーダンス制御する。例えば、人２０１が反力（２０Ｎ程度の荷重）を左ハンドに与えた場合、１〜２ｃｍ程度倣うようにインピーダンス制御する。すなわち、人２０１が２０Ｎ程度の力で左ハンド６１を引っ張った場合、左ハンド６１が１〜２ｃｍ程度引っ張られる。
【００７５】
（ｃ）Ｚ軸（鉛直方向）
人の反力に対して、ある程度倣う硬さにインピーダンス制御する。例えば、人２０１が反力（１０Ｎ程度の荷重）を左ハンドに与えた場合、３〜４ｃｍ程度倣うようにインピーダンス制御する。すなわち、人２０１が１０Ｎ程度の力で左ハンド６１を引っ張った場合、左ハンド６１が３〜４ｃｍ程度引っ張られる。
【００７６】
（ｄ）Ｒｚ軸（Ｚ軸周りの回転：ヨー方向回転）
ロボット１が旋回して、進行方向が変わった時にも、ある程度人の手に倣うような硬さにインピーダンス制御する。
【００７７】
（ｅ）Ｒｘ軸、（Ｘ軸周りの回転：ロール方向回転）、Ｒｙ軸（Ｙ軸周りの回転：ピッチ方向回転）
非常に硬く制御して、ほとんど外力に倣わなくても良い。従って、Ｒｘ軸、及びＲｙ軸については、インピーダンス制御しなくても良い。
【００７８】
このように、Ｒｘ軸、Ｒｙ軸はインピーダンス制御しないようにする。さらに、Ｘ軸を最も柔らかく、かつ一定の力で引くように制御する。さらに、Ｙ軸をＸ軸、Ｚ軸、Ｒｚ軸よりを硬く制御する。よって、インピーダンス制御の柔らかさは以下の順となる。Ｘ軸｛最も柔らかい｝→Ｒｚ軸、Ｚ軸→Ｙ軸→（Ｒｘ軸、Ｒｙ軸）
【００７９】
上記のインピーダンス制御を実現する適用例として、以下の式（１）のような制御則を利用できる。
【００８０】
【数１】
【００８１】
なお、式（１）において、ｐは現在の手先位置（ｘ,ｙ,ｚ,Ｒｘ，Ｒｙ,Ｒｚ）のベクトル、ｐｄは手先位置指令（目標手先位置）、Ｍは実慣性行列、Ｂはダンパ行列、Ｋは仮想バネ行列、Ｆは力計測値、Ｆｄは力指令値、Ｓはスイッチ行列、Ｋｆは力ゲイン、Ｅは単位ベクトルである。なお、手先位置指令ｐｄは、拘束制御において算出された目標手先位置に相当する。
【００８２】
上記の（１）式中のスイッチ行列を操作することによって、コンプライアントにハンドを制御するだけでなく、任意の力指令値に応じた力を発生させることができる。そこで、移動方向であるＸ方向のみ力指令値を有効として、その他の方向は、コンプライアント制御となるようにスイッチ行列を設定する。また、各方向の制御の硬さに関しては、上記の（ａ）〜（ｅ）で示した所望の硬さを実現するように、慣性行列、ダンパ行列、バネ行列のパラメータを設定する。
【００８３】
拘束制御によって目標手先位置を算出する。そして、目標手先位置から、マス要素、ダンパ要素、バネ要素を組み合わせて、力指令値を算出することで、適切な力での誘導が可能になる。そして、インピーダンス制御結果に応じて、インピーダンス制御部１３４がアーム指令値を出力する。これにより、インピーダンス制御結果に従って手先位置を駆動することができる。なお、腕部の可動範囲やロボットの自己干渉を考慮して、それ以上動けない場合には、直前の腕位置を保つように制御する。このように、インピーダンス制御部１３４は、移動経路に沿った進行方向に人を引き、かつ、Ｘ方向がＸ方向と直交するＹ方向、Ｚ方向よりも柔らかくなるようにインピーダンス制御する。
【００８４】
（異常状態制御）
追従状態が異常状態と判定された時の制御について、説明する。図９、及び図１０は、追従異常となっている例を説明するための図である。図９に示すように、人２０１の手が左ハンド６１から離れた場合、左ハンドセンサ部６７のタッチセンサ、接触センサ、力センサでの反応がなくなる。従って、左ハンドセンサ部６７からのセンサ信号によって、人２０１の手が離れたか否かを判定することができる。以下、接触状態判別部１３５、追従状態推定部１３６による処理に付いて説明する。
【００８５】
なお、力センサ使用時には、自重の影響による出力値は差分して考慮することが好ましい。また、ＯＮ／ＯＦＦではなく、アナログ値が出力されるセンサを用いる場合、センサ信号の値とあるしきい値を比較することで、接触しているか否かを判定するようにしてもよい。センサのチャタリング防止のため、数ｍｓｅｃ以上反応がない状態が続いた場合に、人が離れたと判定することが好ましい。さらに、タッチセンサや接触センサでは、人２０１がロボット１の左ハンド６１を握っているか否かを判定するために、左ハンド６１の手の甲側と、手の平側の両方にセンサを設置することが好ましい。この場合、両方の反応を取ることによって、判定精度を高めることができる。例えば、いずれか一方のセンサの反応がなくなった場合に、人の手がロボットハンドから離れたと判定する。もちろん、両方の反応がなくなった場合に、人の手がロボットハンドから離れたと推定しても良い。なお、各種センサは、人の手がロボット１のハンドを握る際に、通常接触する位置に設けられる。
【００８６】
また、図１０に示すように、人が転倒しそうになると、ロボットハンドに過大な反力が加わる。この場合、以下に示す手法を用いることで、異常状態か否かを推定することができる。左ハンドセンサ部６７の力センサからのセンサ信号の値を、しきい値と比較する。例えば、センサ信号の値が、しきい値を越えた場合、異常状態と推定する。また、力センサの周波数応答が高域で、しきい値を越えた場合、異常状態と推定する。この場合、衝突等のインパルス的な反力を検知することができる。インピーダンス制御適用後の、手先位置が、基準の手先位置に対してしきい値以上の偏差を伴っている場合、異常状態と推定しても良い。この場合、Ｘ軸、Ｙ軸、Ｚ軸、Ｒｘ軸、Ｒｙ軸、Ｒｚ軸に対して、それぞれ別のしきい値を設定する。このように、センサ信号の値や、手先位置の偏差をしきい値と比較することによって、過大な力が加わった異常状態であることを検知することができる。
【００８７】
異常状態と推定された場合、以下の手順で移動を停止する。現在が、両脚支持期なら、次の歩行を行わずに静止する。すなわち、両脚を離地させる前に、脚部モータの駆動を停止する。また、現在が片脚支持期なら、次に遊脚が着地したところで、静止する。すなわち、遊脚が着地するまでは、脚部モータを駆動する。そして、遊脚が着地した後に、脚部モータを停止する。さらに、安全性をより向上するために、左腕のインピーダンス制御を全方向に関してコンプライアントな状態とするようにしてもよい。これにより、反力によってロボット１のバランスが崩れることを防止することができる。例えば、式（１）中のスイッチ行列を単位行列にして、コンプライアンス性を高めるように、各パラメータを変更する。そして、変更されたパラメータで制御を継続する。これにより、駆動制御部１３８から、脚部モータを適切に停止する指令値が出力される。よって、ロボット１がバランスを崩して、転倒を防ぐことができる。
【００８８】
（歩行調整）
次に、推定された追従状態に応じて、歩行速度を調整する処理に付いて説明する。歩行調整部１３７では、追従状態推定部１３６で推定された追従状態によって、歩行速度を調整している。すなわち、ステップ１０７によって、歩幅や歩行周期を調整して、次の一歩を算出する。以下の処理では、歩行調整部において、歩幅を調整する処理について、図１１を用いて説明する。
【００８９】
図１１は、調整された歩幅を示す上面図である。ここで、Ｔｒは右足平２６の足付き位置、Ｔｌは左足平３６の足付き位置を示している。さらに、この説明では、左脚３０が支持脚であり、右脚２０が遊脚である状態において、右脚２０の次の一歩を算した例に付いて説明する。すなわち、右足平の２６の次の足付き位置を決定する。ここで、ロボット１の基準の歩幅をＳｒとすると、その歩幅で歩いた時の右足平２６の足付き位置は、Ｔｒｎとなる。なお、基準の歩幅Ｓｒは、ロボットの構成やリンク長や基準の歩行速度に応じて決めることができる。
【００９０】
図１１に示すように、支持脚である左脚３０を基準に、両足を仮想的にそろえた時の右足平２６の仮想位置をＴｒ'とする。なお、右足平２６の仮想位置Ｔｒ'の中心と、左足平３６の中心とを結ぶ直線は、移動経路Ｒに対して垂直となっている。ここで、仮想位置Ｔｒ'から見た次の足付き位置Ｔｒｎを（ｘ方向歩幅、ｙ方向歩幅、旋回角度）で表すと、（Ｓｒｘ，Ｓｒｙ，θｒ）と表すことができる。ここで、次の足付き位置Ｔｒｎは、仮想位置Ｔｒ'から基準の歩幅Ｓｒで歩いた時の位置である。基準の歩幅Ｓｒを、ＳｒｘとＳｒｙを用いて表すと、Ｓｒ＝（Ｓｒｘ２＋Ｓｒｙ２）１／２なる。
【００９１】
上記の通り、歩行中の手先位置は、インピーダンス制御されている。移動経路上のある位置を基準（基準ロボット位置）とした目標手先位置と、インピーダンス制御後の手先位置との偏差（以下手先位置偏差）に応じて歩幅を調整する。すなわち、基準ロボット位置における目標手先位置と実測の手先位置の手先位置偏差を、前の一歩で算出しておく。なお、基準ロボット位置における目標手先位置は、上述の通り、手先位置算出部１３３が算出する。基準ロボット位置における実際の手先位置は、移動経路上のある所定のロボット位置における腕部の関節角度によって算出することができる。基準ロボット位置の目標手先位置と、実際の手先位置との差を手先位置偏差として、次の歩幅を決定する。このように、手先位置偏差は、ロボット１の本体がある所定位置にあるとした時の目標手先位置と実測の手先位置の偏差となる。
【００９２】
インピーダンス制御後の手先位置偏差が進行方向側ならば、人２０１がロボット１に十分追従していると推定する。すなわち、手先が目標手先位置よりも進んでいるため、人２０１がロボット１よりも速く歩いていると推定する。計画よりもロボット１を速く歩かせるため、歩幅を基準の歩幅よりも大きくする。これにより、ロボット１が加速して、歩行速度が速くなる。
【００９３】
インピーダンス制御後の手先位置偏差が進行方向と反対側ならば、人２０１がロボット１に十分追従していないと推定する。すなわち、手先が目標手先位置よりも遅れているため、人２０１がロボット１に対して遅れていると推定する。この場合、計画よりもロボット１を遅く歩かせるため、歩幅を基準の歩幅よりも小さくする。これにより、ロボット１が減速して、歩行速度が遅くなる。
【００９４】
上記の歩幅調整の一例を説明する。インピーダンス制御適用後の手先位置偏差（ｐ−ｐｄ）について考える。ここで（１）式中のｘ成分のみを取り出す。すなわち、ベクトルｐをｘ成分のみに付いて考えると、以下の（２）式が得られる。
【００９５】
【数２】
【００９６】
このように、進行方向における手先位置偏差は（ｘ−ｘｄ）で表される。なお、ｘは実測の手先位置、ｘｄは目標手先位置である。この偏差（ｘ−ｘｄ）の値によって、以下のポリシーで歩幅を変更する。
【００９７】
例えば、進行方向における手先位置偏差（ｘ−ｘｄ）が正ならば、歩幅を基準の歩幅よりも大きくする。すなわち、人がロボット１の歩行速度よりも速く歩いている場合、左ハンド６１が人から進行方向に力を受ける。このため、人が把持している左ハンド６１が基準ロボット位置での目標手先位置よりも先に進む。よって、ロボット１の歩行速度を速くするために、歩幅を大きくする。ロボット１の一歩の時間が略同じであるとすると、ロボット１が基準速度よりも速く歩く。一方、進行方向における手先位置偏差（ｘ−ｘｄ）が負ならば、歩幅を小さくする。すなわち、人がロボット１の歩行速度よりも遅く歩いている場合、左ハンド６１が人から進行方向と逆方向に力を受ける。このため、人が把持している左ハンド６１が遅れることになり、基準ロボット位置での目標手先位置よりも後ろになる。よって、ロボット１の歩行速度を遅くするために、歩幅を小さくする。ロボット１の一歩の時間が略同じであるとすると、ロボット１が基準速度よりもゆっくり歩く。
【００９８】
また、偏差（ｘ−ｘｄ）がしきい値Ｘｍｉｎよりも小さければ歩幅を０とする。なお、しきい値は負の値とすることが好ましい。例えば、しきい値Ｘｍｉｎを−０．３ｍとすることができる。従って、調整後の歩幅を（Ｓｘ,Ｓｙ,θ）とすると、歩幅調整は以下の式（３）〜式（６）で示すことができる
【００９９】
（ｘ−ｘｄ）がＸｍｉｎ以上の時
Ｓｘ＝（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×Ｓｒｘ ・・・（３）
Ｓｙ＝（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×Ｓｒｘ ・・・（４）
θ＝（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×θｒ ・・・（５）
【０１００】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
（Ｓｘ,Ｓｙ,θ）＝（０,０,０） ・・・（６）
【０１０１】
上記の計算方法では、（ｘ−ｘｄ）が正の場合、誘導される人の手先が計画よりも早めに動いているということができる。よって、ロボット１の歩幅を大きくして、歩行を加速する。これは、人が誘導させるだけでなく、正しい進行方向にならば、人側からロボット１を押して、移動を早めることができることを意味する。よって、直感的な誘導動作が実現される。この場合、ロボット１が人の歩行速度に合わせて移動することになる。よって、適切な誘導を行うことができる。よって、様々な人に対して適切な誘導動作を実現することができる。また、進行方向への偏差（ｘ−ｘｄ）に応じて、歩行周期を変化させてもよい。すなわち、歩幅を一定とした状態で、一歩の時間、すなわち、離地してから着地するまでの時間を変更することで歩行速度を調整しても良い。もちろん、歩幅と歩行周期の両方を変化させて、歩行速度を調整してもよい。なお、一般に、二足歩行において、歩行周期が長くなりすぎると、安定性を欠くことがある。よって、歩幅を変えることで歩行調整を行うことが好ましい。
【０１０２】
本実施の形態では、進行方向における手先位置偏差（ｘ−ｘｄ）に応じて、歩幅を線形で調整している。従って、支持脚を基準として両足を揃えた時の、仮想的な足付き位置から、次の足付き位置が線形に変化する。すなわち、どのような手先位置偏差を取った場合でも、次の足付き位置は、ある直線上にあることになる。この直線は、支持脚を基準に両足を揃えた時の遊脚の仮想的な足付き位置から、基準歩幅で着地した時の仮想的な着地位置を結ぶ方位にある。すなわち、仮想位置Ｔｒ'と基準歩幅での足付き位置Ｔｒｎとを結ぶ直線上に次の足付き位置が変化する。換言すると、遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る１直線上に、歩幅を変更した後の足付き位置足付き位置が存在する。
【０１０３】
また、手先位置偏差がしきい値以上の場合、歩幅は手先位置偏差と線形関係となっている。すなわち、手先位置偏差に基準歩幅を乗じた値に応じて、次の足付き位置までの距離を決定することができる。これにより、進行方向における手先位置偏差（ｘ−ｘｄ）が大きい程、歩幅が大きくなる。線形に歩幅調整することで、調整を簡便に行うことができる。さらに、負のしきい値を下回った場合は、両足を揃えるように制御する。すなわち、人の歩行速度が非常に遅い場合は、両足が左右に揃った状態となる。これにより、人の歩行速度に合わせて、適切に誘導することができる。
【０１０４】
なお、上記の説明では、Ｘ軸のみに着目して、歩幅を調整したが、Ｙ軸、Ｚ軸に基づいて歩幅を調整しても良い。例えば、Ｙ軸方向の手先位置偏差としきい値との比較結果、又は、Ｚ軸方向の手先位置偏差としきい値の比較結果によって、両足を揃えるようにする。これにより、Ｙ軸、Ｚ軸において、ハンドに過大な力が加わった場合は、歩行速度を遅らせることができる。
【０１０５】
上記のように、歩幅を調整した場合、歩容データにおける足付き位置が変化することになる。よって、歩容データの更新が必要になる。以下に、歩容データを更新する処理に付いて、図１２を用いて説明する。
【０１０６】
まず、最初に経路計画を行う（ステップＳ２０１）。ここではステップＳ１０１で示した処理と同様に、移動経路が算出される。すなわち、障害物２００と衝突しないように、移動始点Ｓから移動経路Ｇまでの経路を算出する。さらに、移動経路に従った歩容データが算出される。これにより、移動経路に沿った足付き位置の座標が算出される。
【０１０７】
そして、歩行中の手先の位置拘束制御を行う（ステップＳ２０２）。ここでは、ステップＳ１０２と同様に、目標手先位置が算出される。そして、センサ信号に基づいて、追従状態に異常が発生したか否かを判定する（ステップＳ２０３）。ここでは、上記の通り、接触状態に異常が発生したか否か、又は反力に異常が発生したか否かが判定される。人２０１の手が左ハンド６１に接触しており、かつ、人から受ける反力が所定の範囲内にある場合、異常が発生していない正常状態となる。
【０１０８】
異常が発生していない場合（ステップＳ２０３のＮＯ）、手先のインピーダンス制御を行う（ステップＳ２０４）。これにより、左ハンド６１が人２０１の手に適切な力を与える。これにより、人２０１が所定の力でＸ方向に引っ張られる。そして、手先位置偏差に応じた歩幅を決定する（ステップＳ２０５）。すなわち、上記の通り、手先位置が目標手先位置よりも進んでいる場合は歩幅を大きくし、手先位置が目標手先位置よりも遅れている場合は、歩幅を小さくする。そして、歩行を継続する（ステップＳ２０６）。この処理を繰り返すためステップＳ２０１に戻る。ここで、歩幅が基準歩幅Ｓｒから変更した場合、経路が再計画され、調整された歩幅に応じて、歩容データが調整される。すなわち、調整された歩幅では足付き位置が当初の位置から変わることになる。従って、新たな足付き位置に応じた歩容データを算出する。このようにして、歩容データが更新される。そして、このようにして処理を繰り返し行って、移動始点まで移動する。なお、歩容データは、移動経路全体に対して算出しなくてもよい。すなわち、歩幅が調整されると、足付き位置が変化してしまうことになるため、ロボットの現在位置近傍のみ歩容データを算出すればよい。
【０１０９】
なお、上記の処理は、手先位置の拘束制御、インピーダンス制御、歩行調整、歩容データの更新は、それぞれ、所望の制御周期で、繰り返し実行すればよい。一般に、インピーダンス制御は、数ｍｓｅｃ程度の速い制御周期で行った方が、応答がよくなる。一方、歩幅調整は、歩行周期（例えば、０．５ｓｅｃ〜２ｓｅｃ)に合わせて次の一歩が必要になる際に行えばよい。例えば、着地する前までに、次の一歩の歩幅が決定できていればよい。移動経路の更新は、環境を認識するセンサの計測周期や計算速度を考慮した制御周期でよい。例えば、移動経路の更新周期は、１ｓｅｃ〜数ｓｅｃ等の最も遅い周期でよい。それらの制御周期を考慮すると、図１３に示すようなフローとなる。
【０１１０】
まず、移動環境中の障害物を検知するセンサの情報を取得する（ステップＳ３０１）。例えば、カメラやレーザセンサによって障害物の位置を検出する。そして、経路計画の更新が必要か否かを判定する（ステップＳ３０２）。なお、ステップＳ３０２の判定は、１〜数ｓｅｃ程度の周期で行えばよい。例えば、カメラやレーザセンサなどからのセンサ情報が十分に取得されていない場合は、経路計画を更新しなくても良い。障害物の位置が変わって、移動の妨げとなる場合は、経路計画の更新が必要となる(ステップＳ３０２のＹＥＳ)。
【０１１１】
経路計画の更新が必要な場合（ステップＳ３０２のＹＥＳ）、経路計画と基準の歩行プランを更新する(ステップＳ３０３)。これにより、移動経路とその移動経路に沿った足付き位置が算出される。その後、手先のインピーダンス制御を適用する（ステップＳ３０４）。経路計画の更新が不要な場合（ステップＳ３０２のＮＯ）、手先のインピーダンス制御を適用する（ステップＳ３０４）。ここで、インピーダンス制御の制御周期は、数ｍｓｅｃ程度である。これにより、手先によって所望の力を発生させることができる。
【０１１２】
インピーダンスの制御結果に基づいて、次の一歩が必要か否かを判定する（ステップＳ３０５）。ここでは、次の一歩を更新できるタイミングか否かを判定する。ここでの制御周期は、歩行周期に応じたものとなっており、例えば、０．５ｓｅｃ〜２ｓｅｃとなっている。例えば、片足支持期のあるタイミングにおいて、次の一方の歩幅を決定する。よって、そのタイミングまでは、あるいはそのタイミングを過ぎたら、次の一方の歩幅の決定は不要となる。換言すると、一歩ごとに、次の歩幅を決定すれば良い。次の一歩が不要の場合（ステップＳ３０５のＮＯ）、全身の関節角度を決定する（ステップＳ３０７）。これにより、脚部と腕部の関節角度を求めることができる。また、次の一歩が必要な場合、最新の手先位置偏差に応じて、次の一歩を更新する（ステップＳ３０６）。そして、全身の関節角度を決定する（ステップＳ３０７）関節角度が決定したら、その関節角度に対応するアクチュエータへ指令値を出力する（ステップＳ３０８）。
【０１１３】
このように、次の一歩を更新する制御周期をインピーダンス制御の制御周期よりも遅くする。たとえば、一歩ごとに次の歩の歩幅を決定すれば良い。これにより、計算時間が長くなるのを防ぐことができる。なお、手先位置偏差の算出は、最新のロボット位置から求めればよい。あるいは、複数のロボット位置における手先位置偏差の平均によって、次の歩幅を決定してもよい。
【０１１４】
実施の形態２．
本実施の形態は、歩幅調整の方法が実施の形態１と異なっている。なお、それ以外の処理とロボット構成については、実施の形態１と同様であるため、説明を省略する。実施の形態１では、手先位置偏差に応じて歩幅を線形に調整している。しかしながら、図１４に示すように、歩幅の調整量や移動経路の曲率に応じて、移動経路から大きくずれてしまうことになる。なお、図１３では、現在の足付き位置Ｔｒと基準となる歩幅の足付き位置Ｔｒｎに対して線形に調整した足付き位置を点線で示している。足付き位置が移動経路から大きくずれると、例えば、移動経路が長くなったり、他方の脚と干渉するおそれも生じたりしてしまう。そこで、本実施の形態に係る方法で、歩幅を調整すると、より厳密に歩幅を調整することができるようになる。
【０１１５】
図１５に示すように、移動経路上において、現在の両足中心位置を原点する。すなわち、調整開始位置の右足平２６の中心位置と、左足平３６の中心位置の中間位置を原点（０,０,０）する。さらに、経路長をｎとし、経路上の点の２次元座標と傾きを（ｆｘ（ｎ）,ｆｙ（ｎ）、φ（ｎ））とする。その点に対応する右足平の中心位置を（ｘｒ（ｎ）,ｙｒ（ｎ）,φ（ｎ））とし、左足平３６の中心位置を（ｘｌ（ｎ）,ｙｌ（ｎ）,φ（ｎ））とする。なお、φ（ｎ）は、ｘ軸方向から反時計回りを静とした時の回転角度である。このφ（ｎ）は、両足中心と右足平中心と左足平中心で同じ角度を取る。両足中心から、左足平中心までの距離をｗとすると、右足開始位置は（０,−ｗ,０）となり、左足開始位置は（０,ｗ,０）となる。ｗを用いて、右足平２６の中心位置Ｔｒｎと左足平３６の中心位置Ｔｌｎを表すと、以下の式（７）〜式（９）のようになる。
【０１１６】
φ（ｎ）＝ａｔａｎ２（ｙ'（ｎ）,ｘ'（ｎ）） ・・・（７）
ｘｒ（ｎ）＝ｆｘ（ｎ）＋ｗｓｉｎ（φ（ｎ））、ｙｒ（ｎ）＝ｆｙ（ｎ）−ｗｃｏｓ（φ（ｎ）） ・・・（８）
ｘｌ（ｎ）＝ｆｘ（ｎ）−ｗｓｉｎ（φ（ｎ））、ｙｌ（ｎ）＝ｆｙ（ｎ）＋ｗｃｏｓ（φ（ｎ）） ・・・（９）
【０１１７】
そして、歩幅調整を行う場合、以下の条件を満たすように調整する。右足平２６の位置は以下の式（１０）〜（１３）に示すようになる。
【０１１８】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
Ｓｘ＝ｘｒ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１０）
Ｓｙ＝ｙｒ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'）＋ｗ ・・・（１１）
θ＝φ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１２）
【０１１９】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
（Ｓｘ,Ｓｙ,θ）＝（０,０,０） ・・・（１３）
【０１２０】
また、左足平３６の位置は以下の式（１４）〜（１７）に示すようになる。
【０１２１】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
Ｓｘ＝ｘｌ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１４）
Ｓｙ＝ｙｌ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'）−ｗ ・・・（１５）
θ＝φ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１６）
【０１２２】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
（Ｓｘ,Ｓｙ,θ）＝（０,０,０） ・・・（１７）
【０１２３】
上記の式（１０）〜（１７）において、ｎ'は基準歩幅の時の経路長である。なお、右足平２６と右足平３６のθは同じである。このように、歩幅を調整する際、移動経路に沿って、足付き位置を変更する。すなわち、インピーダンス制御結果の手先位置偏差から、歩幅を調整する際、計画した移動経路に沿って次の一歩を調整することができる。これにより、図１６に示すように、両足の干渉を回避することが可能になる。なお、図１６では、様々な歩幅で調整した時の右足平２６の足付き位置を示している。本実施の形態にかかる歩幅調整方法を用いることで、移動経路に沿った足付き位置の変更が可能になる。さらに、移動経路からのずれが小さくなるため、ロボット１が大回りするのを防ぐことができる。よって、移動終点まで短い距離で誘導することができる。よって、人の負担を軽減することができる。
【０１２４】
人の手を引くロボット１においてインピーダンス制御を行うことによって、人に与える力により歩く方向を直感的に提示することができる。よって、目的となる移動終点まで適切に誘導することができる。さらに、直感的に誘導方向が把握されるため、目の不自由な人を適切に誘導することに好適である。さらに、人の追従状態に合わせて歩行速度を決定できる。これにより、被誘導者側の負担を軽減することができる。また、人の手が離れた場合、過大な反力が発生した場合、歩行を停止する。これにより、安全性を向上することができる。
【０１２５】
上記の説明では、左ハンドによって人の手を把持したが、右ハンドによって人の手を把持した場合でも、同様に制御することができる。また、上記説明ではロボット１が移動経路を算出したが、移動経路Ｒは予め与えられていても良い。すなわち、他のコンピュータ等によって、算出された移動経路を、ロボット１に供給しても良い。もちろん、歩幅調整は毎歩行わなくてもよく、例えば、複数歩に１回の割合で歩幅を調整しても良い。
【０１２６】
なお、上記の実施の形態１、２は適宜組み合わせてもよい。例えば、ある区間では実施の形態１の手法によって歩幅を調整し、他の区間では実施の形態２の手法で歩幅を調整しても良い。ロボット１の歩行制御は、静歩行、動歩行のいずれでもよい。さらには、全脚が床から離地する走行によって、人を誘導しても良い。また、本実施形態にかかるロボット１は、２脚型以外の脚式ロボットでもよい。すなわち、３脚以上の脚式ロボットであっても良い。
【符号の説明】
【０１２７】
１ ロボット
２ 頭部
３ 体幹
４ 腰部
５ 右腕
６ 左腕
１０ 脚部
２０ 右脚
２１ 右股関節
２２ 右上腿
２３ 右膝関節、
２４ 右下腿
２５ 右足首関節
２６ 右足平
３０ 左脚
３１ 左股関節
３２ 左上腿
３３ 左膝関節
３４ 左下腿
３５ 左足首関節
３６ 左足平
５１ 右ハンド
５２ 右手首関節
５３ 右前腕
５４ 右肘関節
５５ 右上腕
５６ 右肩関節
５７ 右ハンドセンサ
６１ 左ハンド
６２ 左手首関節
６３ 左前腕
６４ 左肘関節
６５ 左上腕
６６ 左肩関節
６７ 左ハンドセンサ
１３１ 経路算出部
１３２ 歩容データ算出部
１３３ 手先位置算出部
１３４ インピーダンス制御部
１３５ 接触状態判別部
１３６ 追従状態推定部
１３７ 歩行調整部
１３８ 駆動制御部
【技術分野】
【０００１】
本発明は、ロボット、及びその制御方法に関し、特に詳しくは、被誘導者を誘導するロボット、及びその制御方法に関する。
【背景技術】
【０００２】
脚式移動ロボットにおいて、人と手をつないで歩行する技術が開示されている（特許文献１）。特許文献１の移動ロボットでは、ハンドを介して人と接触したときに、人から作用する外力を検出し、その検出された外力によって歩容を生成している。また、外力の外乱分を吸収するようにコンプライアンス制御を行っている。さらに、人の身長に応じて、重力方向におけるハンド位置を決定している。そして、ロボットが人と手を繋いだ状態で、ロボットが歩行する。このようにすることで、ロボットが人を案内する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−１８５７６３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、ヒューマノイドロボットの介護用途機能として、視覚障害者や高齢者の手を引いて、目的地まで誘導することができれば、好ましい。この場合、誘導される人の安全を考慮して、人の追従具合に応じて、制御する必要がある。例えば、人を誘導する場合、誘導方向に人を引っ張る必要がある。さらに、人に応じた、歩行速度が変わる場合もある。上記の特許文献１では、このような観点から制御を行う方法について何ら開示していないため、適切に誘導することができないという問題点がある。
【０００５】
本発明は、上記の問題点に鑑みてなされたものであり、適切に誘導することができるロボット、及びその制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明の第１の態様にかかるロボットは、ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットであって、移動始点から移動終点までの移動経路に応じて、歩容データを生成する歩容データ生成部と、前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するハンド位置算出部と、前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定する追従状態推定部と、前記追従状態推定部の推定結果に応じて、ロボットの移動速度を調整する調整部と、調整部によって調整された移動速度で前記ロボットを移動させるよう、前記脚部を駆動する駆動制御部と、を備えたものである。
【０００７】
本発明の第２の態様にかかるロボットは、上記のロボットであって、前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させるものである。
【０００８】
本発明の第３の態様にかかるロボットは、上記のロボットであって、前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されているものである。
【０００９】
本発明の第４の態様にかかるロボットは、上記のロボットであって、遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定されるものである。
【００１０】
本発明の第５の態様にかかるロボットは、上記のロボットであって、前記歩幅が、前記偏差に応じて線形に変化することを特徴とするものである。
【００１１】
本発明の第６の態様にかかるロボットは、上記のロボットであって、前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整するものである。
【００１２】
本発明の第７の態様にかかるロボットは、上記のロボットであって、前記ハンド位置算出部が、前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出するものである。
【００１３】
本発明の第８の態様にかかるロボットは、上記のロボットであって、前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するインピーダンス制御部と、をさらに備えるものである。
【００１４】
本発明の第９の態様にかかるロボットは、上記のロボットであって、前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とするものである。
【００１５】
本発明の第１０の態様にかかるロボットは、上記のロボットであって、前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止するものである。
【００１６】
本発明の第１１の態様にかかるロボットは、上記のロボットであって、前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とするものである。
【００１７】
本発明の第１２の態様にかかるロボットの制御方法は、ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットの制御方法であって、移動始点から移動終点までの移動経路に応じて、歩容データを生成するステップと、前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するステップと、前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定するステップと、前記追従状態の推定結果に応じて、ロボットの移動速度を調整するステップと、調整された前記移動速度で前記ロボットを移動させるよう、前記脚部を駆動するステップと、を備えたものである。
【００１８】
本発明の第１３の態様にかかるロボットの制御方法は、上記の制御方法であって、前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させるものである。
【００１９】
本発明の第１４の態様にかかるロボットの制御方法は、上記の制御方法であって、前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されているものである。
【００２０】
本発明の第１５の態様にかかるロボットの制御方法は、上記の制御方法であって、遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定されるものである。
【００２１】
本発明の第１６の態様にかかるロボットの制御方法は、上記の制御方法であって、前記歩幅が、前記偏差に応じて線形に変化することを特徴とするものである。
【００２２】
本発明の第１７の態様にかかるロボットの制御方法は、上記の制御方法であって、前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整するものである。
【００２３】
本発明の第１８の態様にかかるロボットの制御方法は、上記の制御方法であって、前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出するものである。
【００２４】
本発明の第１９の態様にかかるロボットの制御方法は、上記の制御方法であって、前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するものである。
【００２５】
本発明の第２０の態様にかかるロボットの制御方法は、上記の制御方法であって、前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とするものである。
【００２６】
本発明の第２１の態様にかかるロボットの制御方法は、上記の制御方法であって、前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止するものである。
【００２７】
本発明の第２２の態様にかかるロボットの制御方法は、上記の制御方法であって、前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とするものである。
【発明の効果】
【００２８】
本発明によれば、適切に誘導することができるロボット、及びその制御方法を提供するこができる。
【図面の簡単な説明】
【００２９】
【図１】本発明にかかるロボットの全体構成を模式的に示す図である。
【図２】本発明にかかるロボットの制御部の構成を示すブロック図である。
【図３】本発明にかかるロボットの制御方法を示すフローチャートである。
【図４】本発明にかかるロボットの移動経路を示す図である。
【図５】本発明にかかるロボットの移動経路と人の移動経路を示す図である。
【図６】本発明にかかるロボットの移動経路と手先位置軌道を示す図である。
【図７】本発明にかかるロボットの手先位置を中心とする座標系を示す上面図である。
【図８】本発明にかかるロボットの手先位置を中心とする座標系を示す斜視図である。
【図９】本発明にかかるロボットにおいて、追従状態に異常が発生した様子を示す図である。
【図１０】本発明にかかるロボットにおいて、追従状態に異常が発生した様子を示す図である。
【図１１】本発明の実施形態１にかかるロボットの歩幅調整処理を説明するための図である
【図１２】本発明の実施形態にかかるロボットにおいて、歩幅を調整する処理を示すフローチャートである。
【図１３】制御周期を考慮した誘導動作を示すフローチャートである。
【図１４】線形に歩幅を調整する様子を示す図である。
【図１５】実施の形態２にかかるロボットの歩幅調整処理を説明するための図である。
【図１６】実施の形態２にかかるロボットの制御方法で調整された歩幅を示す上面図である。
【発明を実施するための形態】
【００３０】
以下、本発明に係る移動体の実施形態を、図面に基づいて詳細に説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００３１】
実施の形態１．
以下に、図１を参照しつつ本発明の実施の形態１にかかる脚式移動型ロボット（以下、単にロボットという）について説明する。図１は、ロボットの全体構成を模式的に示す正面図である。本実施の形態にかかるロボットは、人の手を引いて誘導するヒューマノイド型ロボットである。すなわち、ロボット１が人を引っ張って歩行することで、人がロボットの後を追従する。
【００３２】
図１は、ロボット１を正面から見た様子を概略的に表す概略図であり、床面Ｆ上をロボット１が歩行する様子を表しているなお、図１においては、説明の便宜上、ロボット１が進行する向き（前後方向）をｘ軸、ロボット１が進行する方向について水平方向に直交する向き（左右方向）をｙ軸、移動体の移動する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これらの３軸からなる座標系を用いて説明する。すなわち、図１中において、前記ｘ軸は紙面の奥行方向、ｙ軸は紙面に向かって左右方向、ｚ軸は紙面中の上下方向を示す。、
【００３３】
図１に示すように、ロボット１は、頭部２と、体幹３と、体幹３に結合された腰部４と、体幹３に接続された右腕５、左腕６と、腰部４に対して回動自在に固定される脚部１０と、を備えた２脚歩行型のロボットである。以下、詳細に説明する。
【００３４】
頭部２は、ロボット１の周囲の環境を視覚的に撮像するための左右一対の撮像部（図示せず）を備えているとともに、体幹３に対して頭部２を鉛直方向に平行な軸周りに回動させることで、周囲の環境を広く撮像する。撮像した周囲の環境を示す画像データは、後述する制御部１３０に送信され、ロボット１の動作を決定するための情報として用いられる。
【００３５】
体幹３は、その内部にロボット１の動作を制御する制御部１３０や、脚部や腕部のアクチュエータ（例えば、モータ）に電力を供給するためのバッテリー（図示せず）等を収容するものである。制御部１３０は、後述するように、脚部１０を駆動し、ロボット１を動かすための歩容データを記憶する記憶領域と、この記憶領域に記憶された歩容データを読み出す演算処理部と、脚部１０に含まれるモータを駆動する駆動制御部と、を備えている。これらの各構成要素は、体幹３の内部に設けられたバッテリー（図示せず）から電力を供給されることで動作する。
【００３６】
また、演算処理部は、記憶領域に記憶された歩容データを読み出すとともに、読み出した歩容データによって特定されるロボット１の姿勢を実現するために必要な脚部１０の関節角を算出する。そして、このように算出した関節角に基づく信号を駆動制御部に送信する。歩容データの生成、及び歩容データに基づく歩行の基本的制御については公知の技術を用いることができる。
【００３７】
駆動制御部は、演算処理部より送信された信号に基づいて、脚部を駆動するための各モータの駆動量を特定し、これらの駆動量でモータを駆動させるためのモータ駆動信号を各モータに送信する。これによって脚部１０の各関節における駆動量が変更され、ロボット１の動きが制御される。
【００３８】
また、演算処理部は、読み出した歩容データに基づいてモータの駆動を行うように指令するほか、ロボット１に組み込まれたジャイロや加速度計などセンサ（図示せず）からの信号を受けて、モータの駆動量を調整する。また、レーザセンサなどを設けて、床面Ｆまでの距離や障害物までの距離を検出してもよい。ジャイロセンサや加速度計やレーザセンサなどの各種センサは、例えば、体幹３や腰部４に設けられる。このように、センサにより検出したロボット１に作用する外力や、ロボット１の姿勢などに応じて脚部１０の関節角を調整することで、ロボット１が安定した状態を維持することができる。
【００３９】
右腕５および左腕６は、体幹３に対して回動自在に接続されており、肩部分、肘部分および手首部分に設けられた関節部分を駆動することにより、人間の腕部と同様の動きを行うことができる。また、手首部分の先端に接続された手先部は、図示を省略するが物体を把持するためのハンド構造を備えており、ハンド構造に組み込まれた複数の指関節を駆動することで、様々な形状の物体を把持することが可能となる。例えば、指関節を駆動することで、誘導する人の手を握ることができる。なお、人を誘導する際には、ロボット１が人の手を握ってもよく、人がロボット１のハンドを握ってもよい。さらには、ロボット１と人が互いに手とハンドを握り合っていても良い。
【００４０】
腰部４は、体幹３に対して回動するように接続されており、歩行動作を行う際に腰部４の回動動作を組み合わせることで、脚部１０を駆動するために必要な駆動エネルギーを低減させることができる。体幹３と腰部４が胴体部分を構成する。
【００４１】
２足歩行を行うための脚部１０（右脚２０、左脚３０）は、右脚２０と左脚３０とから構成されている。詳細には、図２に示すように、右脚２０は右股関節２１、右上腿２２、右膝関節２３、右下腿２４、右足首関節２５、右足平２６を備え、同様に、左脚３０は左股関節３１、左上腿３２、左膝関節３３、左下腿３４、左足首関節３５、左足平３６を備えている。
【００４２】
そして、右脚２０および左脚３０とは、図示しないモータからの駆動力が、同じく図示しないプーリおよびベルトを介して伝達されることで、各関節部が所望の角度に駆動され、その結果、脚部に所望の動きをさせることができる。
【００４３】
なお、本実施形態においては、脚部１０（右脚２０および左脚３０）は、下腿を膝関節回りに前方側に持ち上げると、人間の脚部のように、上腿と下腿が後方側に向かって開いた状態（上腿の延長線よりも後方側に、下腿が膝関節回りに回転した状態）となる。
【００４４】
記憶領域に記憶された歩容データは、脚部１０の移動量に対応づけて、脚部１０の足平（右足平２６、左足平３６）の先端（足先）の位置と、移動体本体の位置とを、ロボット１の移動する空間を定める座標系（例えばｘｙｚ座標系）において経時的に指示するものである。
【００４５】
次に、人の手を引くために設けられた腕部について具体的に説明する。右腕５と左腕６は、例えば、複数の自由度を持つアーム機構であり、ハンド（手先）を所望の位置に駆動することができる。具体的には７、又は８自由度のアーム機構を用いることができる。もちろん、これ以外の自由度を有するアーム機構を用いてもよい。図示しないモータからの駆動力が、同じく図示しないプーリおよびベルトを介して伝達されることで、各関節部が所望の角度に駆動され、その結果、脚部に所望の動きをさせることができる。
【００４６】
右腕５は、右ハンド５１、右手首関節５２、右前腕５３、右肘関節５４、右上腕５５、右肩関節５６を有している。右手首関節５２、右肘関節、右肩関節５６は、回転関節機構などを有している。これらの関節が制御部１３０によって制御されることで、右腕５が右ハンド５１の手先位置を所望の位置にすることができる。さらに、右ハンド５１には、右ハンドセンサ部５７が設けられている。
【００４７】
同様に、左腕６は、左ハンド６１、左手首関節６２、左前腕６３、左肘関節６４、左上腕６５、左肩関節６６を有している。左手首関節６２、左肘関節６４、左肩関節６６は、回転関節機構などを有している。これらの関節が制御部によって制御されることで、左腕６が左ハンド６１の手先位置を所望の位置にすることができる。さらに、左ハンド６１には、左ハンドセンサ部６７が設けられている。なお、常時同じ腕によって誘導する場合は、一方の腕のみにセンサ部が設けられていればよい。以下、右ハンドセンサ部５７、及び左ハンドセンサ部６７のうちの一方をハンドセンサ部と称し、特に言及がない場合、人の手を引っ張っている方のハンドに設けられた方のハンドセンサ部を指す。
【００４８】
制御部１３０は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、制御部１３０は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。もちろん、制御部１３０は、物理的に一つの構成に限られるものではない。制御部１３０は、脚部１０、右腕５、左腕６を制御するための演算処理を行う。すなわち、各関節のアクチュエータ（例えば、モータ）に対して指令値を出力する。
【００４９】
右ハンドセンサ部５７、左ハンドセンサ部６７は、ユーザの追従状態を推定するための一つ又は複数のセンサを有している。例えば、右ハンドセンサ部５７、左ハンドセンサ部６７は、タッチセンサ、力センサ、接触センサ等を有している。例えば、ロボットハンドの手の平部分にタッチセンサや接触センサを設けることで、ユーザの手を引いているか否かを判断することができる。さらに、力センサは、人によって与えられる力を検出する。力センサは、自重の影響を差分してもよい。
【００５０】
ハンドセンサ部から出力されるセンサ信号を用いた制御に付いて、図２、及び図３を用いて説明する。図２は、制御部１３０の構成を示すブロック図である。図３は、制御部１３０による制御方法を示すフローチャートである。ロボット１がハンドによって、人（被誘導者）を誘導している。従って、制御部１３０は一方のハンドに設けられたハンドセンサ部から出力されるセンサ信号に応じて制御を行っている。以下の説明では、左ハンド６１が人の手を握った状態で、誘導しているとして説明する。
【００５１】
制御部１３０は、経路算出部１３１、歩容データ算出部１３２、手先位置算出部１３３、インピーダンス制御部１３４、接触状態判別部１３５、追従状態推定部１３６、歩行調整部１３７、駆動制御部１３８を備えている。さらに、制御部１３０には、左ハンドセンサ部６７からのセンサ信号が入力されている。なお、図２では、関節モータのエンコーダの出力値を省略している。
【００５２】
まず、人を誘導するためのロボット経路を計画する（ステップＳ１０１）。まず、経路算出部１３１は、移動始点から移動終点までの移動経路を算出する。経路算出部１３１は、左ハンドセンサ部６７からのセンサ信号によって、左ハンド６１が誘導される人の手を握ったことを検知する。具体的には、接触センサやタッチセンサ等からのセンサ信号によって、接触していることを検出する。これにより、誘導可能状態になったことが認識される。そして、現在位置（移動始点）から入力された目的位置（移動終点）までの移動経路を算出する。さらに、レーザセンサやカメラなどによって、障害物の位置を測定して、疎障害物を避けるように移動経路を算出する。なお、レーザセンサやカメラ等の測定結果に応じて、適宜移動経路を変更するようにしてもよい。すなわち、障害物が移動した場合、その障害物を避けるように新たな移動経路を算出してもよい。
【００５３】
さらに、障害物２００の位置の変化に応じて、移動経路を更新しても良い。例えば、ロボット１に設けられたカメラやレーザセンサで障害物を検出する。また、移動環境中に設置されたカメラによって、障害物を検出しても良い。そして、障害物の位置が変化した場合は、移動経路を変更しても良い。すなわち、障害物が移動経路の近傍に移動して、ロボット１、及び人２０１の移動の妨げとなるおそれが生じた場合、移動経路を変更する。
【００５４】
歩容データ算出部１３２は、移動経路における歩容データを算出する。歩容データとは、脚部１０の移動量に対応づけて、脚部１０の足平（右足平２６、左足平３６）の先端（足先）の位置と、ロボット１本体の位置とを、ロボット１の移動する空間を定める座標系（例えばｘｙｚ座標系）において経時的に指示するものである。なお、歩容データの生成については、従来から広く用いられている方法を用いることができるため、説明を省略する。右足平２６と左足平３６が交互に着地することで、ロボット１が歩行する。このため、歩容データには、足平が着地している位置（足付き位置）が含まれている。このように、経路算出部１３１と歩容データ算出部１３２によって、移動終点までの経路計画が実行される。すなわち、両脚で歩行するためのフットプランが計画される。なお、歩容データは、移動経路全体の歩容データを算出しなくてもよい。例えば、移動始点に近い一部の経路だけ歩容データを算出して、歩行中に随時歩容データを算出するようにしてもよい。
【００５５】
次に、歩行中の手先位置（ハンドの位置）を拘束制御する（ステップＳ１０２）。そのため、手先位置算出部１３３は、その歩容データに基づいて、目標手先位置を算出する。ここでは、ユーザの手を把持している左ハンド６１の手先位置を算出する。ロボット１の本体がある位置にあるとき、腕（右腕５、左腕６）の関節角度によって手先位置が決まる。例えば、歩行中の手の振り幅等によって、移動経路上のあるロボット位置における腕関節角度が決まる。手先位置は、各関節の角度や、関節を結ぶリンクの長さに基づいて、算出される。このようにして、歩容データに基づいて、移動経路に沿った手先位置の軌道が算出される。そして、歩行中の手先位置を拘束制御する。
【００５６】
そして、進行方向に一定の力で人の手を引くように、手先をインピーダンス制御する（ステップＳ１０３）。例えば、インピーダンス制御部１３４は、目標手先位置と力センサ等からのセンサ信号に応じて、手先位置のインピーダンス制御を行う。具体的には、左ハンドセンサ部６７に設けられた力センサによって、人から左ハンド６１が受ける力が検出される。この力に応じてインピーダンス制御する。人を把持している左腕６を制御するためのアーム指令値を出力する。このアーム指令値が左腕６の関節モータに入力されることで、手先の位置が制御される。これにより、左手首関節６２、左肘関節６４、左上腕６５の関節モータが所望の回転トルク、回転速度で駆動する。よって、適切な力で人の手を引くことができる。
【００５７】
次に、手先の接触状態と人からの反力を検出する（ステップＳ１０４）。左ハンドセンサ部６７の力センサが反力を検出する。さらに、左ハンドセンサ部６７の接触センサやタッチセンサが、人がロボット１に左ハンド６１と接触しているか否かを検出する。接触状態判別部１３５は、これらのセンサ信号に応じて、人とロボット１の左ハンド６１との接触状態を判別する。これにより、非接触状態か、接触状態かを判別することができる。そして、追従状態推定部１３６は、手先のインピーダンス制御結果と接触状態判別部１３５の判別結果等に基づいて、人の追従状態を推定する。
【００５８】
まず、追従状態に異常があるか否かが判定される（ステップＳ１０５）。追従状態推定部１３６は、例えば、人の手がハンドから離れていて誘導されていない状態を異常状態と推定する。さらには、追従状態推定部１３６は、人からロボット１が受ける力がしきい値を越えた場合を異常状態と推定する。このような場合（ステップＳ１０５のＹＥＳ）、移動を停止する（ステップＳ１０６）。例えば、支持脚の駆動を停止して、かつ遊脚が着地した時点で遊脚の駆動を停止する。
【００５９】
異常状態でない場合、追従状態推定部１３６は、追従速度が速い状態か、追従速度が遅い状態かを推定する。換言すると、追従状態推定部１３６は、目標手先位置とインピーダンス制御結果に応じて、人の歩行速度と、ロボットの歩行速度の差を算出する。そして、追従速度が速い状態や遅い状態の場合、手先の基準位置との偏差に応じて、歩幅や歩行周期を調整して次の一歩を算出する（ステップＳ１０６）。歩行調整部１３７は、追従状態の推定結果に応じて、歩行調整を行う。例えば、人の追従速度が速い場合は、ロボット１の歩行速度を速くし、人の追従速度が遅い場合は、ロボット１の歩行速度を遅くする。そのため、歩行調整部１３７は、歩幅や歩行周期を調整する。
【００６０】
そして、駆動制御部１３８は、歩行調整部１３７で調整された結果に応じて、脚部の駆動を制御する。すなわち、脚部の関節モータに対して、歩行指令値を出力する。これにより、右股関節２１、左股関節３１、右膝関節２３、左膝関節３３、右足首関節２５、及び左足首関節３５の関節モータが、所望のトルク、回転速度で駆動する。よって、ロボット１が、適切な速度で、移動経路に沿った所望の方向に歩行する。
【００６１】
目標位置、すなわち移動終点に到達したか否かを判定する（ステップＳ１０８）。目標位置に到達した場合は、歩行を停止する（ステップＳ１０９）。すなわち、脚部のモータの駆動を停止する。さらに、左ハンドによって人を誘導する力を０にする。一方、目標位置に到達していない場合は、ステップＳ１０２からの処理を繰り返す。そして、目標位置に到達するまで、あるいは、異常状態と推定されるまで、歩行を継続する。このようにして、歩行制御が行われる。以下に、上記の処理について詳細に説明する。
【００６２】
（経路計画）
経路計画について、図４、及び図５を用いて説明する。図４は、移動経路に付いて模式的に示す図である。図５は、障害物を回避する移動経路を算出する方法について説明する図である。以下、経路算出部１３１による処理に付いて説明する。
【００６３】
ここでは、既に知られている各種の方法を利用して、移動経路を算出することができる。図４に示すように、障害物２００を回避するため、移動経路の移動始点Ｓから移動終点Ｇまでの移動経路Ｒを算出する。例えば、確率的探索手法であるＲＲＴ（Ｒａｐｉｄｌｙ Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）やＡ＊アルゴリズムにより、離散的に経路を計画する。そして、各離散点をサブゴールＳＧとして、サブゴール間をスプライン曲線、ペジエ曲線、線形等で補完する。これにより、移動経路の軌跡が生成される。さらに、移動経路の生成には、ロボット１が予め記憶している移動環境の地図情報を利用しても良い。さらには、カメラやレーザセンサで取得された障害物の情報を利用してもよい。
【００６４】
また、経路計画時には、図５に示すように、人の幅を考慮したマージンを持って移動経路を算出する。ロボット１の中心点から、人２０１を引く側のロボット１の手先位置までの距離（Ｌｒ）と、人２０１の幅（Ｌｈ）を考慮する。そして、距離Ｌｒと幅Ｌｈの和Ｌａｌｌの幅だけ余裕を持って、移動経路を算出する。すなわち、移動経路から障害物２００までの距離がＬａｌｌよりも長くなるようにする。これにより、人２０１が障害物と干渉しないように歩くことができる。もちろん、人２０１と反対側の手については、幅Ｌｈを考慮する必要はない。このように、誘導されている人２０１が、障害物２００を回避できるようにマージンを持たせて、移動経路を算出する。これにより、人が障害物２００を意識せずに歩くことができるようになる。なお、人の幅Ｌｈは予め設定しておくことが好ましい。もちろん、誘導される人に２０１応じて、人の幅Ｌｈを変えてもよい。ロボット１と人２０１を合わせた幅は、２Ｌｒ＋Ｌｈとなり、この幅が通過できるような移動経路Ｒが算出される。
【００６５】
そして、この移動経路Ｒに沿って歩くように、歩容データ算出部１３２が歩容データを算出する。ここでは、両足の足付き位置が算出される。このとき、理想的な、ロボット１の手先位置の軌道は、Ｒｄとなり、人の手（ロボット１と反対側の手）の軌道は、Ｒｈとなる。
【００６６】
（拘束制御）
手先位置の拘束制御について図６を用いて説明する。図６を移動経路に従って移動するロボット１と人２０１を示す上面図である。さらに、図６では、右足平２６の足付き位置Ｔｒと左足平３６の足付き位置Ｔｌの軌跡が示されている。足付き位置とは、それぞれの足平が接地する位置を示し、歩容データに応じて算出することができるものである。右足平２６の足付き位置Ｔｒは移動経路Ｒの右側にあり、左足平３６の足付き位置Ｔｌは移動経路Ｒの左側にある。まず、拘束制御しない場合の問題点について説明する。
【００６７】
２足歩行ロボットで安定して歩行するためには、支持脚が形成する支持多角形内に静歩行なら重心、動歩行ならＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）を収めるように動作を行う必要がある。一般的に、２足歩行ロボットは、上記の制約にしたがって歩行する。このため、腰中心は、移動経路の進行方向に対して、横方向の変動を伴う軌道Ｒｂとなる。例えば、右足平２６が接地しているときは、腰中心軌道Ｒｂが移動経路Ｒの右側になり、左足平３６が設置しているときは、腰中心軌道Ｒｂが移動経路の左側となる。このように、歩行中の腰中心軌道Ｒｂは支持脚よりになるため、腰中心軌道Ｒｂは移動経路Ｒに対して横方向変動量を伴う。
【００６８】
拘束制御を行わない場合、腰中心軌道Ｒｂが横方向変動につれて、手先位置も変動してしまう。すなわち、ロボット１の歩行にしたがって、ロボット１の腰中心と、手先位置が変化することになる。すると、移動中に、人２０１の手が左右に揺れることになってしまう。すなわち、腰中心の横方向変動に合わせて、人２０１がロボット１から力を受けてしまう。このような力を人が受けるのは好ましいことではない。例えば、人が横方向の力を受けると、移動方向を誤って認識してしまう。
【００６９】
そこで、本実施形態では、移動中における、手先位置の軌道Ｒｄの横方向変動量が、腰中心軌道Ｒｂの横方向変動量よりも小さくなるように、拘束制御している。ここで、横方向変動量とは、各軌道と移動経路Ｒの距離のばらつきを示すものである。したがって、手先位置軌道Ｒｄが移動経路Ｒと平行になれば、手先位置軌道Ｒｄと移動経路との距離は一定になる。手先位置軌道Ｒｄが移動経路と平行な場合、横方向変動量は０となる。一方、腰中心軌道Ｒｂは移動経路Ｒをまたぐように左右に揺れている。このため、移動経路上の位置に応じて腰中心軌道Ｒｂと移動経路の距離が変化して、距離のばらつきが大きくなる。このように、手先位置軌道Ｒｄが移動経路Ｒとほぼ平行になるように、ロボット１の手先位置を拘束制御する。
【００７０】
腰中心位置が横方向変動を伴っても、誘導する手先は横方向変動がないように手先位置を拘束制御する。制御部１３０は、この拘束制御をリアルタイムに行う。実際には、上記の重心やＺＭＰの制約を満たしつつ、手先位置の軌道Ｒｄが移動経路と平行になるようにする。すなわち、制御部１３０は、手先位置が移動経路Ｒと一定距離を保って移動するように制御する。この制御を行うため、制御部１３０は、例えば、逆運動学を解くことで、全身の関節角度を求める。逆運動学は解析解で解いてもよく、ヤコビアンを用いた数値処理で解いてもよい。具体的には、ロボット１のリンク構成に応じて、精度と計算負荷を考慮して、全身の関節角度を求めればよい。これによって、手先位置が移動経路Ｒに沿って移動するようになる。手先位置の軌道Ｒｄが移動経路Ｒと平行になり、横方向変動量を減少することができる。よって、人が誘導方向について、誤って認識するのを避けることができる。
【００７１】
（インピーダンス制御）
手先位置のインピーダンス制御に付いて、図７及び図８を用いて説明する。図７は、手先位置のインピーダンス制御の説明を明確にするための座標系を示す上面図である。図８は、手先位置のインピーダンス制御を示す斜視図である。以下、インピーダンス制御部１３４による処理に付いて説明する。
【００７２】
まず、図７、及び図８に示すように、右手系のローカル座標系を定義する。すなわち、ロボット１の移動方向をＸ方向とし、水平面内において、そのＸ方向と垂直な方向をＹ方向とする。さらに、Ｘ方向及びＹ方向と垂直な方向、つまり鉛直方向をＺ方向とする。そして、手先位置軌道上の点を原点とする。なお、Ｘ方向は、その地点における移動経路Ｒと平行になる。そして、以下のようなポリシーでインピーダンス制御する。なお、上記の通り、インピーダンス制御に用いる手先位置は、手先位置算出部１３３によって算出されており、左ハンド６１に加わる力は左ハンドセンサ部６７によって測定されている。
【００７３】
（ａ）Ｘ軸（進行方向）
進行方向に対して、人２０１を引っ張る力Ｆｒを提示できるようにインピーダンス制御する。具体的には、一定の軽い力（Ｆｒ：５〜１０Ｎ程度）で人２０１を引っ張るような力を生じさせる。さらに、人２０１が逆向きに反力（５〜２０Ｎ程度）を加えた場合は、逆向きにも追従できる程度の柔らかさで制御する。
【００７４】
（ｂ）Ｙ軸（横方向）
人の反力に対して、ある程度倣う硬さにインピーダンス制御する。例えば、人２０１が反力（２０Ｎ程度の荷重）を左ハンドに与えた場合、１〜２ｃｍ程度倣うようにインピーダンス制御する。すなわち、人２０１が２０Ｎ程度の力で左ハンド６１を引っ張った場合、左ハンド６１が１〜２ｃｍ程度引っ張られる。
【００７５】
（ｃ）Ｚ軸（鉛直方向）
人の反力に対して、ある程度倣う硬さにインピーダンス制御する。例えば、人２０１が反力（１０Ｎ程度の荷重）を左ハンドに与えた場合、３〜４ｃｍ程度倣うようにインピーダンス制御する。すなわち、人２０１が１０Ｎ程度の力で左ハンド６１を引っ張った場合、左ハンド６１が３〜４ｃｍ程度引っ張られる。
【００７６】
（ｄ）Ｒｚ軸（Ｚ軸周りの回転：ヨー方向回転）
ロボット１が旋回して、進行方向が変わった時にも、ある程度人の手に倣うような硬さにインピーダンス制御する。
【００７７】
（ｅ）Ｒｘ軸、（Ｘ軸周りの回転：ロール方向回転）、Ｒｙ軸（Ｙ軸周りの回転：ピッチ方向回転）
非常に硬く制御して、ほとんど外力に倣わなくても良い。従って、Ｒｘ軸、及びＲｙ軸については、インピーダンス制御しなくても良い。
【００７８】
このように、Ｒｘ軸、Ｒｙ軸はインピーダンス制御しないようにする。さらに、Ｘ軸を最も柔らかく、かつ一定の力で引くように制御する。さらに、Ｙ軸をＸ軸、Ｚ軸、Ｒｚ軸よりを硬く制御する。よって、インピーダンス制御の柔らかさは以下の順となる。Ｘ軸｛最も柔らかい｝→Ｒｚ軸、Ｚ軸→Ｙ軸→（Ｒｘ軸、Ｒｙ軸）
【００７９】
上記のインピーダンス制御を実現する適用例として、以下の式（１）のような制御則を利用できる。
【００８０】
【数１】
【００８１】
なお、式（１）において、ｐは現在の手先位置（ｘ,ｙ,ｚ,Ｒｘ，Ｒｙ,Ｒｚ）のベクトル、ｐｄは手先位置指令（目標手先位置）、Ｍは実慣性行列、Ｂはダンパ行列、Ｋは仮想バネ行列、Ｆは力計測値、Ｆｄは力指令値、Ｓはスイッチ行列、Ｋｆは力ゲイン、Ｅは単位ベクトルである。なお、手先位置指令ｐｄは、拘束制御において算出された目標手先位置に相当する。
【００８２】
上記の（１）式中のスイッチ行列を操作することによって、コンプライアントにハンドを制御するだけでなく、任意の力指令値に応じた力を発生させることができる。そこで、移動方向であるＸ方向のみ力指令値を有効として、その他の方向は、コンプライアント制御となるようにスイッチ行列を設定する。また、各方向の制御の硬さに関しては、上記の（ａ）〜（ｅ）で示した所望の硬さを実現するように、慣性行列、ダンパ行列、バネ行列のパラメータを設定する。
【００８３】
拘束制御によって目標手先位置を算出する。そして、目標手先位置から、マス要素、ダンパ要素、バネ要素を組み合わせて、力指令値を算出することで、適切な力での誘導が可能になる。そして、インピーダンス制御結果に応じて、インピーダンス制御部１３４がアーム指令値を出力する。これにより、インピーダンス制御結果に従って手先位置を駆動することができる。なお、腕部の可動範囲やロボットの自己干渉を考慮して、それ以上動けない場合には、直前の腕位置を保つように制御する。このように、インピーダンス制御部１３４は、移動経路に沿った進行方向に人を引き、かつ、Ｘ方向がＸ方向と直交するＹ方向、Ｚ方向よりも柔らかくなるようにインピーダンス制御する。
【００８４】
（異常状態制御）
追従状態が異常状態と判定された時の制御について、説明する。図９、及び図１０は、追従異常となっている例を説明するための図である。図９に示すように、人２０１の手が左ハンド６１から離れた場合、左ハンドセンサ部６７のタッチセンサ、接触センサ、力センサでの反応がなくなる。従って、左ハンドセンサ部６７からのセンサ信号によって、人２０１の手が離れたか否かを判定することができる。以下、接触状態判別部１３５、追従状態推定部１３６による処理に付いて説明する。
【００８５】
なお、力センサ使用時には、自重の影響による出力値は差分して考慮することが好ましい。また、ＯＮ／ＯＦＦではなく、アナログ値が出力されるセンサを用いる場合、センサ信号の値とあるしきい値を比較することで、接触しているか否かを判定するようにしてもよい。センサのチャタリング防止のため、数ｍｓｅｃ以上反応がない状態が続いた場合に、人が離れたと判定することが好ましい。さらに、タッチセンサや接触センサでは、人２０１がロボット１の左ハンド６１を握っているか否かを判定するために、左ハンド６１の手の甲側と、手の平側の両方にセンサを設置することが好ましい。この場合、両方の反応を取ることによって、判定精度を高めることができる。例えば、いずれか一方のセンサの反応がなくなった場合に、人の手がロボットハンドから離れたと判定する。もちろん、両方の反応がなくなった場合に、人の手がロボットハンドから離れたと推定しても良い。なお、各種センサは、人の手がロボット１のハンドを握る際に、通常接触する位置に設けられる。
【００８６】
また、図１０に示すように、人が転倒しそうになると、ロボットハンドに過大な反力が加わる。この場合、以下に示す手法を用いることで、異常状態か否かを推定することができる。左ハンドセンサ部６７の力センサからのセンサ信号の値を、しきい値と比較する。例えば、センサ信号の値が、しきい値を越えた場合、異常状態と推定する。また、力センサの周波数応答が高域で、しきい値を越えた場合、異常状態と推定する。この場合、衝突等のインパルス的な反力を検知することができる。インピーダンス制御適用後の、手先位置が、基準の手先位置に対してしきい値以上の偏差を伴っている場合、異常状態と推定しても良い。この場合、Ｘ軸、Ｙ軸、Ｚ軸、Ｒｘ軸、Ｒｙ軸、Ｒｚ軸に対して、それぞれ別のしきい値を設定する。このように、センサ信号の値や、手先位置の偏差をしきい値と比較することによって、過大な力が加わった異常状態であることを検知することができる。
【００８７】
異常状態と推定された場合、以下の手順で移動を停止する。現在が、両脚支持期なら、次の歩行を行わずに静止する。すなわち、両脚を離地させる前に、脚部モータの駆動を停止する。また、現在が片脚支持期なら、次に遊脚が着地したところで、静止する。すなわち、遊脚が着地するまでは、脚部モータを駆動する。そして、遊脚が着地した後に、脚部モータを停止する。さらに、安全性をより向上するために、左腕のインピーダンス制御を全方向に関してコンプライアントな状態とするようにしてもよい。これにより、反力によってロボット１のバランスが崩れることを防止することができる。例えば、式（１）中のスイッチ行列を単位行列にして、コンプライアンス性を高めるように、各パラメータを変更する。そして、変更されたパラメータで制御を継続する。これにより、駆動制御部１３８から、脚部モータを適切に停止する指令値が出力される。よって、ロボット１がバランスを崩して、転倒を防ぐことができる。
【００８８】
（歩行調整）
次に、推定された追従状態に応じて、歩行速度を調整する処理に付いて説明する。歩行調整部１３７では、追従状態推定部１３６で推定された追従状態によって、歩行速度を調整している。すなわち、ステップ１０７によって、歩幅や歩行周期を調整して、次の一歩を算出する。以下の処理では、歩行調整部において、歩幅を調整する処理について、図１１を用いて説明する。
【００８９】
図１１は、調整された歩幅を示す上面図である。ここで、Ｔｒは右足平２６の足付き位置、Ｔｌは左足平３６の足付き位置を示している。さらに、この説明では、左脚３０が支持脚であり、右脚２０が遊脚である状態において、右脚２０の次の一歩を算した例に付いて説明する。すなわち、右足平の２６の次の足付き位置を決定する。ここで、ロボット１の基準の歩幅をＳｒとすると、その歩幅で歩いた時の右足平２６の足付き位置は、Ｔｒｎとなる。なお、基準の歩幅Ｓｒは、ロボットの構成やリンク長や基準の歩行速度に応じて決めることができる。
【００９０】
図１１に示すように、支持脚である左脚３０を基準に、両足を仮想的にそろえた時の右足平２６の仮想位置をＴｒ'とする。なお、右足平２６の仮想位置Ｔｒ'の中心と、左足平３６の中心とを結ぶ直線は、移動経路Ｒに対して垂直となっている。ここで、仮想位置Ｔｒ'から見た次の足付き位置Ｔｒｎを（ｘ方向歩幅、ｙ方向歩幅、旋回角度）で表すと、（Ｓｒｘ，Ｓｒｙ，θｒ）と表すことができる。ここで、次の足付き位置Ｔｒｎは、仮想位置Ｔｒ'から基準の歩幅Ｓｒで歩いた時の位置である。基準の歩幅Ｓｒを、ＳｒｘとＳｒｙを用いて表すと、Ｓｒ＝（Ｓｒｘ２＋Ｓｒｙ２）１／２なる。
【００９１】
上記の通り、歩行中の手先位置は、インピーダンス制御されている。移動経路上のある位置を基準（基準ロボット位置）とした目標手先位置と、インピーダンス制御後の手先位置との偏差（以下手先位置偏差）に応じて歩幅を調整する。すなわち、基準ロボット位置における目標手先位置と実測の手先位置の手先位置偏差を、前の一歩で算出しておく。なお、基準ロボット位置における目標手先位置は、上述の通り、手先位置算出部１３３が算出する。基準ロボット位置における実際の手先位置は、移動経路上のある所定のロボット位置における腕部の関節角度によって算出することができる。基準ロボット位置の目標手先位置と、実際の手先位置との差を手先位置偏差として、次の歩幅を決定する。このように、手先位置偏差は、ロボット１の本体がある所定位置にあるとした時の目標手先位置と実測の手先位置の偏差となる。
【００９２】
インピーダンス制御後の手先位置偏差が進行方向側ならば、人２０１がロボット１に十分追従していると推定する。すなわち、手先が目標手先位置よりも進んでいるため、人２０１がロボット１よりも速く歩いていると推定する。計画よりもロボット１を速く歩かせるため、歩幅を基準の歩幅よりも大きくする。これにより、ロボット１が加速して、歩行速度が速くなる。
【００９３】
インピーダンス制御後の手先位置偏差が進行方向と反対側ならば、人２０１がロボット１に十分追従していないと推定する。すなわち、手先が目標手先位置よりも遅れているため、人２０１がロボット１に対して遅れていると推定する。この場合、計画よりもロボット１を遅く歩かせるため、歩幅を基準の歩幅よりも小さくする。これにより、ロボット１が減速して、歩行速度が遅くなる。
【００９４】
上記の歩幅調整の一例を説明する。インピーダンス制御適用後の手先位置偏差（ｐ−ｐｄ）について考える。ここで（１）式中のｘ成分のみを取り出す。すなわち、ベクトルｐをｘ成分のみに付いて考えると、以下の（２）式が得られる。
【００９５】
【数２】
【００９６】
このように、進行方向における手先位置偏差は（ｘ−ｘｄ）で表される。なお、ｘは実測の手先位置、ｘｄは目標手先位置である。この偏差（ｘ−ｘｄ）の値によって、以下のポリシーで歩幅を変更する。
【００９７】
例えば、進行方向における手先位置偏差（ｘ−ｘｄ）が正ならば、歩幅を基準の歩幅よりも大きくする。すなわち、人がロボット１の歩行速度よりも速く歩いている場合、左ハンド６１が人から進行方向に力を受ける。このため、人が把持している左ハンド６１が基準ロボット位置での目標手先位置よりも先に進む。よって、ロボット１の歩行速度を速くするために、歩幅を大きくする。ロボット１の一歩の時間が略同じであるとすると、ロボット１が基準速度よりも速く歩く。一方、進行方向における手先位置偏差（ｘ−ｘｄ）が負ならば、歩幅を小さくする。すなわち、人がロボット１の歩行速度よりも遅く歩いている場合、左ハンド６１が人から進行方向と逆方向に力を受ける。このため、人が把持している左ハンド６１が遅れることになり、基準ロボット位置での目標手先位置よりも後ろになる。よって、ロボット１の歩行速度を遅くするために、歩幅を小さくする。ロボット１の一歩の時間が略同じであるとすると、ロボット１が基準速度よりもゆっくり歩く。
【００９８】
また、偏差（ｘ−ｘｄ）がしきい値Ｘｍｉｎよりも小さければ歩幅を０とする。なお、しきい値は負の値とすることが好ましい。例えば、しきい値Ｘｍｉｎを−０．３ｍとすることができる。従って、調整後の歩幅を（Ｓｘ,Ｓｙ,θ）とすると、歩幅調整は以下の式（３）〜式（６）で示すことができる
【００９９】
（ｘ−ｘｄ）がＸｍｉｎ以上の時
Ｓｘ＝（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×Ｓｒｘ ・・・（３）
Ｓｙ＝（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×Ｓｒｘ ・・・（４）
θ＝（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×θｒ ・・・（５）
【０１００】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
（Ｓｘ,Ｓｙ,θ）＝（０,０,０） ・・・（６）
【０１０１】
上記の計算方法では、（ｘ−ｘｄ）が正の場合、誘導される人の手先が計画よりも早めに動いているということができる。よって、ロボット１の歩幅を大きくして、歩行を加速する。これは、人が誘導させるだけでなく、正しい進行方向にならば、人側からロボット１を押して、移動を早めることができることを意味する。よって、直感的な誘導動作が実現される。この場合、ロボット１が人の歩行速度に合わせて移動することになる。よって、適切な誘導を行うことができる。よって、様々な人に対して適切な誘導動作を実現することができる。また、進行方向への偏差（ｘ−ｘｄ）に応じて、歩行周期を変化させてもよい。すなわち、歩幅を一定とした状態で、一歩の時間、すなわち、離地してから着地するまでの時間を変更することで歩行速度を調整しても良い。もちろん、歩幅と歩行周期の両方を変化させて、歩行速度を調整してもよい。なお、一般に、二足歩行において、歩行周期が長くなりすぎると、安定性を欠くことがある。よって、歩幅を変えることで歩行調整を行うことが好ましい。
【０１０２】
本実施の形態では、進行方向における手先位置偏差（ｘ−ｘｄ）に応じて、歩幅を線形で調整している。従って、支持脚を基準として両足を揃えた時の、仮想的な足付き位置から、次の足付き位置が線形に変化する。すなわち、どのような手先位置偏差を取った場合でも、次の足付き位置は、ある直線上にあることになる。この直線は、支持脚を基準に両足を揃えた時の遊脚の仮想的な足付き位置から、基準歩幅で着地した時の仮想的な着地位置を結ぶ方位にある。すなわち、仮想位置Ｔｒ'と基準歩幅での足付き位置Ｔｒｎとを結ぶ直線上に次の足付き位置が変化する。換言すると、遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る１直線上に、歩幅を変更した後の足付き位置足付き位置が存在する。
【０１０３】
また、手先位置偏差がしきい値以上の場合、歩幅は手先位置偏差と線形関係となっている。すなわち、手先位置偏差に基準歩幅を乗じた値に応じて、次の足付き位置までの距離を決定することができる。これにより、進行方向における手先位置偏差（ｘ−ｘｄ）が大きい程、歩幅が大きくなる。線形に歩幅調整することで、調整を簡便に行うことができる。さらに、負のしきい値を下回った場合は、両足を揃えるように制御する。すなわち、人の歩行速度が非常に遅い場合は、両足が左右に揃った状態となる。これにより、人の歩行速度に合わせて、適切に誘導することができる。
【０１０４】
なお、上記の説明では、Ｘ軸のみに着目して、歩幅を調整したが、Ｙ軸、Ｚ軸に基づいて歩幅を調整しても良い。例えば、Ｙ軸方向の手先位置偏差としきい値との比較結果、又は、Ｚ軸方向の手先位置偏差としきい値の比較結果によって、両足を揃えるようにする。これにより、Ｙ軸、Ｚ軸において、ハンドに過大な力が加わった場合は、歩行速度を遅らせることができる。
【０１０５】
上記のように、歩幅を調整した場合、歩容データにおける足付き位置が変化することになる。よって、歩容データの更新が必要になる。以下に、歩容データを更新する処理に付いて、図１２を用いて説明する。
【０１０６】
まず、最初に経路計画を行う（ステップＳ２０１）。ここではステップＳ１０１で示した処理と同様に、移動経路が算出される。すなわち、障害物２００と衝突しないように、移動始点Ｓから移動経路Ｇまでの経路を算出する。さらに、移動経路に従った歩容データが算出される。これにより、移動経路に沿った足付き位置の座標が算出される。
【０１０７】
そして、歩行中の手先の位置拘束制御を行う（ステップＳ２０２）。ここでは、ステップＳ１０２と同様に、目標手先位置が算出される。そして、センサ信号に基づいて、追従状態に異常が発生したか否かを判定する（ステップＳ２０３）。ここでは、上記の通り、接触状態に異常が発生したか否か、又は反力に異常が発生したか否かが判定される。人２０１の手が左ハンド６１に接触しており、かつ、人から受ける反力が所定の範囲内にある場合、異常が発生していない正常状態となる。
【０１０８】
異常が発生していない場合（ステップＳ２０３のＮＯ）、手先のインピーダンス制御を行う（ステップＳ２０４）。これにより、左ハンド６１が人２０１の手に適切な力を与える。これにより、人２０１が所定の力でＸ方向に引っ張られる。そして、手先位置偏差に応じた歩幅を決定する（ステップＳ２０５）。すなわち、上記の通り、手先位置が目標手先位置よりも進んでいる場合は歩幅を大きくし、手先位置が目標手先位置よりも遅れている場合は、歩幅を小さくする。そして、歩行を継続する（ステップＳ２０６）。この処理を繰り返すためステップＳ２０１に戻る。ここで、歩幅が基準歩幅Ｓｒから変更した場合、経路が再計画され、調整された歩幅に応じて、歩容データが調整される。すなわち、調整された歩幅では足付き位置が当初の位置から変わることになる。従って、新たな足付き位置に応じた歩容データを算出する。このようにして、歩容データが更新される。そして、このようにして処理を繰り返し行って、移動始点まで移動する。なお、歩容データは、移動経路全体に対して算出しなくてもよい。すなわち、歩幅が調整されると、足付き位置が変化してしまうことになるため、ロボットの現在位置近傍のみ歩容データを算出すればよい。
【０１０９】
なお、上記の処理は、手先位置の拘束制御、インピーダンス制御、歩行調整、歩容データの更新は、それぞれ、所望の制御周期で、繰り返し実行すればよい。一般に、インピーダンス制御は、数ｍｓｅｃ程度の速い制御周期で行った方が、応答がよくなる。一方、歩幅調整は、歩行周期（例えば、０．５ｓｅｃ〜２ｓｅｃ)に合わせて次の一歩が必要になる際に行えばよい。例えば、着地する前までに、次の一歩の歩幅が決定できていればよい。移動経路の更新は、環境を認識するセンサの計測周期や計算速度を考慮した制御周期でよい。例えば、移動経路の更新周期は、１ｓｅｃ〜数ｓｅｃ等の最も遅い周期でよい。それらの制御周期を考慮すると、図１３に示すようなフローとなる。
【０１１０】
まず、移動環境中の障害物を検知するセンサの情報を取得する（ステップＳ３０１）。例えば、カメラやレーザセンサによって障害物の位置を検出する。そして、経路計画の更新が必要か否かを判定する（ステップＳ３０２）。なお、ステップＳ３０２の判定は、１〜数ｓｅｃ程度の周期で行えばよい。例えば、カメラやレーザセンサなどからのセンサ情報が十分に取得されていない場合は、経路計画を更新しなくても良い。障害物の位置が変わって、移動の妨げとなる場合は、経路計画の更新が必要となる(ステップＳ３０２のＹＥＳ)。
【０１１１】
経路計画の更新が必要な場合（ステップＳ３０２のＹＥＳ）、経路計画と基準の歩行プランを更新する(ステップＳ３０３)。これにより、移動経路とその移動経路に沿った足付き位置が算出される。その後、手先のインピーダンス制御を適用する（ステップＳ３０４）。経路計画の更新が不要な場合（ステップＳ３０２のＮＯ）、手先のインピーダンス制御を適用する（ステップＳ３０４）。ここで、インピーダンス制御の制御周期は、数ｍｓｅｃ程度である。これにより、手先によって所望の力を発生させることができる。
【０１１２】
インピーダンスの制御結果に基づいて、次の一歩が必要か否かを判定する（ステップＳ３０５）。ここでは、次の一歩を更新できるタイミングか否かを判定する。ここでの制御周期は、歩行周期に応じたものとなっており、例えば、０．５ｓｅｃ〜２ｓｅｃとなっている。例えば、片足支持期のあるタイミングにおいて、次の一方の歩幅を決定する。よって、そのタイミングまでは、あるいはそのタイミングを過ぎたら、次の一方の歩幅の決定は不要となる。換言すると、一歩ごとに、次の歩幅を決定すれば良い。次の一歩が不要の場合（ステップＳ３０５のＮＯ）、全身の関節角度を決定する（ステップＳ３０７）。これにより、脚部と腕部の関節角度を求めることができる。また、次の一歩が必要な場合、最新の手先位置偏差に応じて、次の一歩を更新する（ステップＳ３０６）。そして、全身の関節角度を決定する（ステップＳ３０７）関節角度が決定したら、その関節角度に対応するアクチュエータへ指令値を出力する（ステップＳ３０８）。
【０１１３】
このように、次の一歩を更新する制御周期をインピーダンス制御の制御周期よりも遅くする。たとえば、一歩ごとに次の歩の歩幅を決定すれば良い。これにより、計算時間が長くなるのを防ぐことができる。なお、手先位置偏差の算出は、最新のロボット位置から求めればよい。あるいは、複数のロボット位置における手先位置偏差の平均によって、次の歩幅を決定してもよい。
【０１１４】
実施の形態２．
本実施の形態は、歩幅調整の方法が実施の形態１と異なっている。なお、それ以外の処理とロボット構成については、実施の形態１と同様であるため、説明を省略する。実施の形態１では、手先位置偏差に応じて歩幅を線形に調整している。しかしながら、図１４に示すように、歩幅の調整量や移動経路の曲率に応じて、移動経路から大きくずれてしまうことになる。なお、図１３では、現在の足付き位置Ｔｒと基準となる歩幅の足付き位置Ｔｒｎに対して線形に調整した足付き位置を点線で示している。足付き位置が移動経路から大きくずれると、例えば、移動経路が長くなったり、他方の脚と干渉するおそれも生じたりしてしまう。そこで、本実施の形態に係る方法で、歩幅を調整すると、より厳密に歩幅を調整することができるようになる。
【０１１５】
図１５に示すように、移動経路上において、現在の両足中心位置を原点する。すなわち、調整開始位置の右足平２６の中心位置と、左足平３６の中心位置の中間位置を原点（０,０,０）する。さらに、経路長をｎとし、経路上の点の２次元座標と傾きを（ｆｘ（ｎ）,ｆｙ（ｎ）、φ（ｎ））とする。その点に対応する右足平の中心位置を（ｘｒ（ｎ）,ｙｒ（ｎ）,φ（ｎ））とし、左足平３６の中心位置を（ｘｌ（ｎ）,ｙｌ（ｎ）,φ（ｎ））とする。なお、φ（ｎ）は、ｘ軸方向から反時計回りを静とした時の回転角度である。このφ（ｎ）は、両足中心と右足平中心と左足平中心で同じ角度を取る。両足中心から、左足平中心までの距離をｗとすると、右足開始位置は（０,−ｗ,０）となり、左足開始位置は（０,ｗ,０）となる。ｗを用いて、右足平２６の中心位置Ｔｒｎと左足平３６の中心位置Ｔｌｎを表すと、以下の式（７）〜式（９）のようになる。
【０１１６】
φ（ｎ）＝ａｔａｎ２（ｙ'（ｎ）,ｘ'（ｎ）） ・・・（７）
ｘｒ（ｎ）＝ｆｘ（ｎ）＋ｗｓｉｎ（φ（ｎ））、ｙｒ（ｎ）＝ｆｙ（ｎ）−ｗｃｏｓ（φ（ｎ）） ・・・（８）
ｘｌ（ｎ）＝ｆｘ（ｎ）−ｗｓｉｎ（φ（ｎ））、ｙｌ（ｎ）＝ｆｙ（ｎ）＋ｗｃｏｓ（φ（ｎ）） ・・・（９）
【０１１７】
そして、歩幅調整を行う場合、以下の条件を満たすように調整する。右足平２６の位置は以下の式（１０）〜（１３）に示すようになる。
【０１１８】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
Ｓｘ＝ｘｒ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１０）
Ｓｙ＝ｙｒ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'）＋ｗ ・・・（１１）
θ＝φ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１２）
【０１１９】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
（Ｓｘ,Ｓｙ,θ）＝（０,０,０） ・・・（１３）
【０１２０】
また、左足平３６の位置は以下の式（１４）〜（１７）に示すようになる。
【０１２１】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
Ｓｘ＝ｘｌ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１４）
Ｓｙ＝ｙｌ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'）−ｗ ・・・（１５）
θ＝φ（（１−（ｘ−ｘｄ）／Ｘｍｉｎ）×ｎ'） ・・・（１６）
【０１２２】
（ｘ−ｘｄ）がＸｍｉｎよりも小さい時
（Ｓｘ,Ｓｙ,θ）＝（０,０,０） ・・・（１７）
【０１２３】
上記の式（１０）〜（１７）において、ｎ'は基準歩幅の時の経路長である。なお、右足平２６と右足平３６のθは同じである。このように、歩幅を調整する際、移動経路に沿って、足付き位置を変更する。すなわち、インピーダンス制御結果の手先位置偏差から、歩幅を調整する際、計画した移動経路に沿って次の一歩を調整することができる。これにより、図１６に示すように、両足の干渉を回避することが可能になる。なお、図１６では、様々な歩幅で調整した時の右足平２６の足付き位置を示している。本実施の形態にかかる歩幅調整方法を用いることで、移動経路に沿った足付き位置の変更が可能になる。さらに、移動経路からのずれが小さくなるため、ロボット１が大回りするのを防ぐことができる。よって、移動終点まで短い距離で誘導することができる。よって、人の負担を軽減することができる。
【０１２４】
人の手を引くロボット１においてインピーダンス制御を行うことによって、人に与える力により歩く方向を直感的に提示することができる。よって、目的となる移動終点まで適切に誘導することができる。さらに、直感的に誘導方向が把握されるため、目の不自由な人を適切に誘導することに好適である。さらに、人の追従状態に合わせて歩行速度を決定できる。これにより、被誘導者側の負担を軽減することができる。また、人の手が離れた場合、過大な反力が発生した場合、歩行を停止する。これにより、安全性を向上することができる。
【０１２５】
上記の説明では、左ハンドによって人の手を把持したが、右ハンドによって人の手を把持した場合でも、同様に制御することができる。また、上記説明ではロボット１が移動経路を算出したが、移動経路Ｒは予め与えられていても良い。すなわち、他のコンピュータ等によって、算出された移動経路を、ロボット１に供給しても良い。もちろん、歩幅調整は毎歩行わなくてもよく、例えば、複数歩に１回の割合で歩幅を調整しても良い。
【０１２６】
なお、上記の実施の形態１、２は適宜組み合わせてもよい。例えば、ある区間では実施の形態１の手法によって歩幅を調整し、他の区間では実施の形態２の手法で歩幅を調整しても良い。ロボット１の歩行制御は、静歩行、動歩行のいずれでもよい。さらには、全脚が床から離地する走行によって、人を誘導しても良い。また、本実施形態にかかるロボット１は、２脚型以外の脚式ロボットでもよい。すなわち、３脚以上の脚式ロボットであっても良い。
【符号の説明】
【０１２７】
１ ロボット
２ 頭部
３ 体幹
４ 腰部
５ 右腕
６ 左腕
１０ 脚部
２０ 右脚
２１ 右股関節
２２ 右上腿
２３ 右膝関節、
２４ 右下腿
２５ 右足首関節
２６ 右足平
３０ 左脚
３１ 左股関節
３２ 左上腿
３３ 左膝関節
３４ 左下腿
３５ 左足首関節
３６ 左足平
５１ 右ハンド
５２ 右手首関節
５３ 右前腕
５４ 右肘関節
５５ 右上腕
５６ 右肩関節
５７ 右ハンドセンサ
６１ 左ハンド
６２ 左手首関節
６３ 左前腕
６４ 左肘関節
６５ 左上腕
６６ 左肩関節
６７ 左ハンドセンサ
１３１ 経路算出部
１３２ 歩容データ算出部
１３３ 手先位置算出部
１３４ インピーダンス制御部
１３５ 接触状態判別部
１３６ 追従状態推定部
１３７ 歩行調整部
１３８ 駆動制御部
【特許請求の範囲】
【請求項１】
ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットであって、
移動始点から移動終点までの移動経路に応じて、歩容データを生成する歩容データ生成部と、
前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するハンド位置算出部と、
前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定する追従状態推定部と、
前記追従状態推定部の推定結果に応じて、ロボットの移動速度を調整する調整部と、
調整部によって調整された移動速度で前記ロボットを移動させるよう、前記脚部を駆動する駆動制御部と、を備えたロボット。
【請求項２】
前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させる請求項１に記載のロボット。
【請求項３】
前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されている請求項は２に記載のロボット。
【請求項４】
遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定される請求項２、又は３に記載のロボット。
【請求項５】
前記歩幅が、前記偏差に応じて線形に変化することを特徴とする請求項４に記載のロボット。
【請求項６】
前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整する請求項２乃至５のいずれか１項に記載のロボット。
【請求項７】
前記ハンド位置算出部が、前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出する請求項１乃至６のいずれか１項に記載のロボット。
【請求項８】
前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するインピーダンス制御部と、をさらに備える請求項７にロボット。
【請求項９】
前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とする請求項８に記載のロボット。
【請求項１０】
前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止する請求項１乃至９のいずれか１項に記載のロボット。
【請求項１１】
前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とする請求項１乃至１０のいずれか１項に記載のロボット。
【請求項１２】
ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットの制御部であって、
移動始点から移動終点までの移動経路に応じて、歩容データを生成するステップと、
前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するステップと、
前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定するステップと、
前記追従状態の推定結果に応じて、ロボットの移動速度を調整するステップと、
調整された前記移動速度で前記ロボットを移動させるよう、前記脚部を駆動するステップと、を備えたロボットの制御方法。
【請求項１３】
前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させる請求項１２に記載のロボットの制御方法。
【請求項１４】
前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されている請求項１３に記載のロボットの制御方法。
【請求項１５】
遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定される請求項１３、又は１４に記載のロボットの制御方法。
【請求項１６】
前記歩幅が、前記偏差に応じて線形に変化することを特徴とする請求項１５に記載のロボットの制御方法。
【請求項１７】
前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整する請求項１３乃至１６のいずれか１項に記載のロボットの制御方法。
【請求項１８】
前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出する請求項１２乃至１７のいずれか１項に記載のロボットの制御方法。
【請求項１９】
前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するインピーダンス制御部と、をさらに備える請求項１８にロボットの制御方法。
【請求項２０】
前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とする請求項１９に記載のロボットの制御方法。
【請求項２１】
前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止する請求項１２乃至２０のいずれか１項に記載のロボットの制御方法。
【請求項２２】
前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とする請求項１２乃至２１のいずれか１項に記載のロボットの制御方法。
【請求項１】
ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットであって、
移動始点から移動終点までの移動経路に応じて、歩容データを生成する歩容データ生成部と、
前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するハンド位置算出部と、
前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定する追従状態推定部と、
前記追従状態推定部の推定結果に応じて、ロボットの移動速度を調整する調整部と、
調整部によって調整された移動速度で前記ロボットを移動させるよう、前記脚部を駆動する駆動制御部と、を備えたロボット。
【請求項２】
前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させる請求項１に記載のロボット。
【請求項３】
前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されている請求項は２に記載のロボット。
【請求項４】
遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定される請求項２、又は３に記載のロボット。
【請求項５】
前記歩幅が、前記偏差に応じて線形に変化することを特徴とする請求項４に記載のロボット。
【請求項６】
前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整する請求項２乃至５のいずれか１項に記載のロボット。
【請求項７】
前記ハンド位置算出部が、前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出する請求項１乃至６のいずれか１項に記載のロボット。
【請求項８】
前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するインピーダンス制御部と、をさらに備える請求項７にロボット。
【請求項９】
前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とする請求項８に記載のロボット。
【請求項１０】
前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止する請求項１乃至９のいずれか１項に記載のロボット。
【請求項１１】
前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とする請求項１乃至１０のいずれか１項に記載のロボット。
【請求項１２】
ハンドを有する腕部と、脚部と、胴体部と、を有し、被誘導者を誘導して移動するロボットの制御部であって、
移動始点から移動終点までの移動経路に応じて、歩容データを生成するステップと、
前記歩容データに基づいて、所定のロボット位置における前記ハンドの目標位置を算出するステップと、
前記所定のロボット位置における前記ハンドの前記目標位置と前記ハンドの測定位置との偏差に応じて、前記ロボットに対する前記被誘導者の追従状態を推定するステップと、
前記追従状態の推定結果に応じて、ロボットの移動速度を調整するステップと、
調整された前記移動速度で前記ロボットを移動させるよう、前記脚部を駆動するステップと、を備えたロボットの制御方法。
【請求項１３】
前記ハンドの前記目標位置と前記測定位置との偏差に応じて、前記脚部の歩幅を変更することで、ロボットの移動速度を変化させる請求項１２に記載のロボットの制御方法。
【請求項１４】
前記歩幅を変更した後の足付き位置が、前記移動経路に沿って算出されている請求項１３に記載のロボットの制御方法。
【請求項１５】
遊脚が基準歩幅で着地する時の仮想的な足付き位置を通る直線上に、前記歩幅を変更した後の足付き位置が決定される請求項１３、又は１４に記載のロボットの制御方法。
【請求項１６】
前記歩幅が、前記偏差に応じて線形に変化することを特徴とする請求項１５に記載のロボットの制御方法。
【請求項１７】
前記手先位置偏差がしきい値より小さい場合に、前記歩幅を変更した後の足付き位置が、支持脚の足付き位置と揃うように歩幅を調整する請求項１３乃至１６のいずれか１項に記載のロボットの制御方法。
【請求項１８】
前記移動経路に対する前記ハンドの目標位置の横方向変動量が、前記移動経路に対する前記胴体部位置の横方向変動量よりも小さくなるように、前記ハンドの目標位置を算出する請求項１２乃至１７のいずれか１項に記載のロボットの制御方法。
【請求項１９】
前記ハンドに設けられたセンサ部からのセンサ信号と前記ハンドの目標位置とに応じて、前記ハンドの位置をインピーダンス制御するインピーダンス制御部と、をさらに備える請求項１８にロボットの制御方法。
【請求項２０】
前記インピーダンス制御が、前記移動経路に沿った進行方向に前記被誘導者を引き、かつ、前記進行方向が前記進行方向と直交する方向よりも柔らかくなるようにインピーダンス制御することを特徴とする請求項１９に記載のロボットの制御方法。
【請求項２１】
前記ハンドに設けられたセンサ部からのセンサ信号によって、前記誘導者とロボットとが非接触状態となっていることが検知された場合に、前記脚部による移動と停止する請求項１２乃至２０のいずれか１項に記載のロボットの制御方法。
【請求項２２】
前記移動軌跡が、前記ロボットと前記被誘導者の幅の和を考慮して、決定されていることを特徴とする請求項１２乃至２１のいずれか１項に記載のロボットの制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【公開番号】特開２０１１−１９４５３９（Ｐ２０１１−１９４５３９Ａ）
【公開日】平成２３年１０月６日（２０１１．１０．６）
【国際特許分類】
【出願番号】特願２０１０−６６１４３（Ｐ２０１０−６６１４３）
【出願日】平成２２年３月２３日（２０１０．３．２３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２３年１０月６日（２０１１．１０．６）
【国際特許分類】
【出願日】平成２２年３月２３日（２０１０．３．２３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]