本発明の制御システムおよびその制御対象の実施形態について図面を用いて説明する。
(ロボットの構成)
まず、本発明の一実施形態としてのロボットの構成について説明する。
図1に示されているロボットRは脚式移動ロボットであり、人間と同様に、基体B0と、基体B0の上方に配置された頭部B1と、基体B0の上部に上部両側から延設された左右の腕体B2と、左右の腕体B2のそれぞれの先端に設けられているハンドHと、基体B0の下部から下方に延設された左右の脚体B4とを備えている。
基体B0はヨー軸回りに相対的に回動しうるように上下に連結された上部および下部により構成されている。頭部B1は基体B0に対してヨー軸回りに回動する等、動くことができる。
腕体B2は第1腕体リンクB22と、第2腕体リンクB24とを備えている。基体B0と第1腕体リンクB22とは肩関節機構(第1腕関節機構)B21を介して連結され、第1腕体リンクB22と第2腕体リンクB24とは肘関節機構(第2腕関節機構)B23を介して連結され、第2腕体リンクB24とハンドHとは手首関節機構(第3腕関節機構)B25を介して連結されている。肩関節機構B21はロール、ピッチおよびヨー軸回りの回動自由度を有し、肘関節機構B23はピッチ軸回りの回動自由度を有し、手首関節機構B25はロール、ピッチ、ヨー軸回りの回動自由度を有している。
脚体B4は第1脚体リンクB42と、第2脚体リンクB44と、足部B5とを備えている。基体B0と第1脚体リンクB42とは股関節機構(第1脚関節機構)B41を介して連結され、第1脚体リンクB42と第2脚体リンクB44とは膝関節機構(第2脚関節機構)B43を介して連結され、第2脚体リンクB44と足部B5とは足関節機構(第3脚関節機構)B45を介して連結されている。
股関節機構B41はロール、ピッチおよびロール軸回りの回動自由度を有し、膝関節機構B43はピッチ軸回りの回動自由度を有し、足関節機構B45はロールおよびピッチ軸回りの回動自由度を有している。股関節機構B41、膝関節機構B43および足関節機構B45は「脚関節機構群」を構成する。なお、脚関節機構群に含まれる各関節機構の並進および回転自由度は適宜変更されてもよい。また、股関節機構B41、膝関節機構B43および足関節機構B45のうち任意の1つの関節機構が省略された上で、残りの2つの関節機構の組み合わせにより脚関節機構群が構成されていてもよい。さらに、脚体B4が膝関節とは別の第2脚関節機構を有する場合、当該第2脚関節機構が含まれるように脚関節機構群が構成されてもよい。足部B5の底には着床時の衝撃緩和のため、特開2001−129774号公報に開示されているような弾性素材B52が設けられている。
ロボットRには、ロボットRの世界座標系における位置および姿勢などの内部状態を測定するための複数の内部状態センサS1が搭載されている。ロボットRの各関節機構の屈曲角度(関節角度)に応じた信号を出力するエンコーダ(図示略)、基体B0の姿勢(方位角および仰角により特定される。)に応じた信号を出力する傾斜センサ、および、足部B5および着床および離床の別を判定するための圧力センサなどが内部状態センサS1に該当する。ロボットRの周囲の様子を撮像し、当該撮像座標に基づいて世界座標系に固定されている標識の位置を認識することにより、世界座標系におけるロボットRの位置を認識するための撮像装置が内部状態センサS1に該当する。
たとえば、頭部B1に搭載され、ロボットRの前方を撮像範囲とするCCDカメラ、赤外線カメラ等、種々の周波数帯域における光を感知しうる左右一対の頭カメラC1が撮像装置として採用されうる。また、基体B0の前側下部に搭載され、ロボットRの前方下方に向けて発せられた近赤外レーザー光の物体による反射光を検知することによりこの物体の位置や方位等を測定するための腰カメラ(アクティブセンサ)C2が撮像装置として採用されうる。
ロボットRには、その周辺における物体の位置などの外部状態を測定するための外部状態センサS2が搭載されている。前記のような撮像装置が外部状態センサS2に該当する。
ロボットRは制御システム1と、前記の複数の関節機構のそれぞれを動かすための複数のアクチュエータ2とを備えている。ロボットRの内部状態および外部状態に応じて制御システム1から出力される制御指令にしたがってアクチュエータ2のそれぞれの動作が制御されることにより、ロボットRがさまざまな態様で適応的に行動することができる。
(制御システムの構成)
図2に示されている制御システム1は、制御モジュールmod0と、第1モジュール(安定化モジュール)mod1と、第2モジュール(障害物回避モジュール)mod2と、第3モジュール(経路生成モジュール)mod3とを備えている。
(第1モジュールの構成)
第1モジュールmod1はロボットRの姿勢を安定させることを主目的(第1行動目的)として主担当する。第1モジュールmod1は第1演算周期T1ごとに入力信号を演算処理することにより出力信号の生成を繰り返すように構成されている。
(第2モジュールの構成)
第2モジュールmod2はロボットRに物体との接触を回避させることを主目的(第2行動目的)として主担当する。第2モジュールmod2は第1演算周期T1より長い第2演算周期T2(たとえばT2=2T1)ごとに入力信号を演算処理することにより出力信号の生成を繰り返すように構成されている。
(第3モジュールの構成)
第3モジュールmod3はロボットRを目標位置まで移動させることを主目的(第3行動目的)として主担当する。第3モジュールmod3は第2演算周期T2より長い第3演算周期T3(たとえばT3=2T2)ごとに入力信号を演算処理することにより出力信号の生成を繰り返すように構成されている。
(ロボットの機能)
前記構成のロボットRの機能について説明する。
(制御システムによる演算処理手順)
制御システム1により、次の手順にしたがってアクチュエータ2の動作およびロボットRの行動が制御される。
まず、制御モジュールmod0によりロボットRの現在状態が認識される。具体的には、内部状態センサS1からの出力信号に基づき、ロボットRの世界座標系における位置(着地位置およびZMP位置など)および姿勢(基体B0の姿勢)などの内部状態が認識される。また、外部状態センサS2からの出力信号に基づき、ロボットRの周囲にある物体のサイズおよび世界座標系における位置などの外部状態が認識される。なお、物体のサイズおよび位置などの外部状態を表わすデータがロボットRの外部から制御システム1に入力または送信されることにより、制御モジュールmod0が外部状態の一部または全部を認識してもよい。
ロボットRの「現在状態」は第1演算周期T1ごとに制御モジュールmod0から第1モジュールmod1に入力される(図3/矢印D1+参照)。さらに、第2行動探索処理の実行により得られる「第2探索戦略」および「第2行動指針」が第2演算周期T2ごとに第2モジュールmod2から第1モジュールmod1に入力される(図3/矢印D2-参照)。
当該入力信号に応じて、第1モジュールmod1により第1演算周期T1ごとに「第1行動探索処理」が実行される。第1行動探索処理の実行により得られる「第1行動指針」は第1演算周期T1ごとに第1モジュールmod1から制御モジュールmod0に入力される(図3/矢印D1-参照)。
なお、第1モジュールmod1の今回サイクルの開始時刻において第2モジュールmod2から入力信号がない場合、第1モジュールmod1の最近の過去サイクルの開始時刻における第2モジュールmod2からの入力信号が用いられて第1行動探索処理が実行される。
現在状態、または、当該現在状態に応じた直近未来におけるロボットRの最新予測状態が第2演算周期T2ごとに第1モジュールmod1から第2モジュールmod2に入力される(図3/矢印D2+参照)。さらに、第1行動探索処理の実行により得られる「第1探索戦略(第1行動予約を含む。)」が第2演算周期T2ごとに第1モジュールmod1から第2モジュールmod2に入力される(図3/矢印D2+参照)。また、第3行動探索処理の実行により得られる「第3探索戦略」および「第3行動指針」が第3演算周期T3ごとに第3モジュールmod3から第2モジュールmod2に入力される(図3/矢印D3-参照)。
当該入力信号に応じて、第2モジュールmod2により第2演算周期T2ごとに「第2行動探索処理」が実行される。これにより得られる「第2探索戦略」および「第2行動指針」は第2演算周期T2ごとに第2モジュールmod2から第1モジュールmod1に入力される(図3/矢印D2-参照)。また、「第2探索戦略」および「第2行動指針」は第2演算周期T2ごとに第2モジュールmod2から第3モジュールmod3にも入力される(図3/矢印D2+参照)。
なお、第2モジュールmod2の今回サイクルの開始時刻において第3モジュールmod3から入力信号がない場合、第2モジュールmod2の最近の過去サイクルの開始時刻における第3モジュールmod3からの入力信号が用いられて第2行動探索処理が実行される。
現在状態、または、当該現在状態に応じた直近未来におけるロボットRの最新予測状態が第3演算周期T3ごとに第2モジュールmod2から第3モジュールmod3に入力される(図3/矢印D3+参照)。さらに、第2行動探索処理の実行により得られる「第2探索戦略(第2行動予約を含む。)」が第3演算周期T3ごとに第2モジュールmod2から第3モジュールmod3に入力される(図3/矢印D3+参照)。
当該入力信号に応じて、第3モジュールmod3により第3演算周期T2ごとに「第3行動探索処理」が実行される。これにより得られる「第3探索戦略」および「第3行動指針」は、前記のように第3演算周期T3ごとに第3モジュールmod3から第2モジュールmod2に入力される(図3/矢印D3-参照)。
第1演算周期T1ごとの第1モジュールmod1からの入力信号(図3/矢印D1-参照)に応じて指令信号が制御モジュールmod0により逐次生成される。そして、この指令信号が制御モジュールmod0からアクチュエータ2に送信されることにより、アクチュエータ2が指令信号に応じて駆動され、その結果、ロボットRが最新の第1行動指針にしたがって行動する。
(並列モジュールの連携処理)
外乱発生直後における各モジュールによる並列処理の手順について説明する。
ロボットRの外乱の影響を受けた「現在状態」が時刻t=t0において最初に制御モジュールmod0から第1モジュールmod1に入力された場合を考える(図4/矢印D1+(t0)参照)。
まず、第1モジュールmod1により「第1行動探索処理」が実行されることにより、時刻t=t1(=t0+T1)において「第1行動指針(=第1行動予約)」が第1モジュールmod1から制御モジュールmod0に出力される(図4/矢印D1-(t1)参照)。この出力に応じてアクチュエータ2の動作が制御されることにより、外乱の影響を受けた現在状態を勘案して、ロボットRが第1行動目的(=ロボットRに姿勢を安定化させる)に適合するように行動しうる。
また、時刻t=t1において「第1探索戦略」が第1モジュールmod1から第2モジュールmod2に出力される(図4/矢印D2+(t1)参照)。さらに、第2モジュールmod2により「第2行動探索処理」が実行されることにより、時刻t=t0+T1+T2において「第2探索戦略(第2行動指針を含む。)」が第2モジュールmod2から第1モジュールmod1に出力される(図4/矢印D2-(t0+T1+T2)参照)。
そして、第1モジュールmod1により「第1行動探索処理」が実行されることにより、時刻t=t2(=t0+T2+2T1)において「第1行動指針」が第1モジュールmod1から制御モジュールmod0に出力される(図4/矢印D1-(t2)参照)。この出力に応じて、アクチュエータ2の動作が制御されることにより、外乱の影響を受けた現在状態を勘案して、ロボットRが第1行動目的に加えて、第2行動目的(=ロボットRに物体との接触を回避させる)に適合するように行動しうる。
また、時刻t=t0+T1+T2において「第2探索戦略(第2行動予約を含む。)」およびが第2モジュールmod2から第3モジュールmod3に出力される(図4/矢印D3+(t0+T1+T2)参照)。さらに、第3モジュールmod3により「第3行動探索処理」が実行されることにより、時刻t=t0+T1+T2+T3において「第3探索戦略(第3行動指針を含む。)」が第3モジュールmod3から第2モジュールmod2に出力される(図4/矢印D3-(t0+T1+T2+T3)参照)。
さらに、第2モジュールmod2により「第2行動探索処理」が実行されることにより、時刻t=t0+T1+2T2+T3において「第2探索戦略(第2行動指針を含む。)」が第2モジュールmod2から第1モジュールmod1に出力される(図4/矢印D2-(t0+T1+2T2+T3)参照)。
そして、第1モジュールmod1により「第1行動探索処理」が実行されることにより、時刻t=t3(=t0+2T1+2T2+T3)において「第1行動指針」が第1モジュールmod1から制御モジュールmod0に出力される(図4/矢印D1-(t3)参照)。この出力に応じて、アクチュエータ2の動作が制御されることにより、外乱の影響を受けた現在状態を勘案して、ロボットRが第1行動目的および第2行動目的に加えて、第3行動目的(=ロボットRを目標位置に到達させる)に適合するように行動しうる。
並列された第1モジュールmod1、第2モジュールmod2および第3モジュールmod3の前記のような連携処理により、外乱発生から2T1+2T2+T3程度の時間内に、外乱を勘案した上で第1行動目的、第2行動目的および第3行動目的のすべてに適合するようにロボットRの行動が制御されうる。たとえばT1=T,T2=2T,T3=4Tとすると、外乱発生から10T程度の時間内に、外乱を勘案した上で複数の目的のすべてに適合するようにロボットRの行動が制御されうる。
(各行動探索処理の内容)
第1モジュールmod1により実行される「第1行動探索処理」、第2モジュールmod2により実行される「第2行動探索処理」および第3モジュールmod3により実行される「第3行動探索処理」の詳細について説明する。
(第1行動探索処理の手順)
まず、現在状態に基づき、ロボットRの挙動を表わす挙動予測モデルにしたがって、ロボットRの直近未来状態が予測される(図5/STEP100)。これにより、たとえば、ロボットRが現在位置から1歩進んだときの着地位置が予測される。
また、第1探索戦略および第2探索戦略(図3/矢印D2-参照)にしたがってロボットRの複数の第1行動候補{ai1|i1=1,2,‥,n1}が探索される(図5/STEP110)。具体的には、第1総合適合度f1の評価結果および第2総合適合度f2の評価結果に応じた進化的探索の世代交代手法にしたがって第1行動候補ai1が探索される。この探索方法については後述する。
これにより、たとえば、ロボットRの1歩先の予測着地位置から、第1指定歩数q1(たとえばq1=2)にわたる歩容を定める着地位置pos(ai1;k1)(k1=1〜q1)が第1行動候補ai1として探索される。「歩容」とは、着地タイミングに着目した脚移動の繰り返しパターンを意味する。
さらに、複数の第1行動候補ai1のそれぞれに基づき、第1状態予測モデルにしたがってロボットRの複数の第1未来状態{si1|i1=1,2,‥,n1}が予測される(図5/STEP120)。これにより、たとえば、ロボットRの1歩先の予測着地位置から、第1行動候補ai1にしたがって第1指定歩数q1だけ移動した時点におけるロボットRのZMP(Zero Moment Point)が第1未来状態si1として予測される。
そして、複数の第1行動候補ai1のそれぞれについて、次の手順にしたがって第1総合適合度f1(ai1)が評価される(図5/STEP130)。
まず、第1未来状態si1に基づき、第1行動目的に鑑みた適合性を表わす第1適合度e1(ai1)が評価される。第1適合度e1(ai1)は、たとえば、図6(a)に示されている予測ZMPの目標ZMPからのずれzmpbias(ai1)と、目標ZMPから予測ZMPに向かう方位について、目標ZMPおよび足部B5の縁の間隔Lfootとに基づき、図6(b)に変化特性が示されている評価式(111)にしたがって算出される。目標ZMPの位置が第1目的に応じた状態空間において定義される基準点に相当する。予測ZMPの目標ZMPからのずれzmpbiasが当該基準点からの乖離度に相当する。たとえば、足部B5の着地面(たとえば矩形状)の中心位置が目標ZMPとして定義される。
e1(ai1)=1 (zmpbias(ai1)≦αLfoot(0<α<1)の場合),
exp(−((zmpbias(ai1)−αLfoot)/Lfoot)2) (αLfoot<zmpbias(ai1)の場合)‥(111)。
評価式(111)から明らかなように、ロボットRの予測ZMPと目標ZMPとの間隔が第1閾値αLfoot以下の範囲である場合、すなわち、ロボットRの姿勢がある程度以上の確度で安定になると予測される第1行動候補ai1であれば第1適合度e1(ai1)は一定値(=1)に評価される。その一方、ロボットRの予測ZMPが第1閾値αLfootを超えて目標ZMPから遠いほど、すなわち、ロボットRの姿勢が不安定になる可能性が高い第1行動候補ai1であるほど第1適合度e1(ai1)が低く評価される。
なお、図6(c)に変化特性が示されている評価式(112)にしたがって第1適合度e1(ai1)が算出されてもよい。
e1(ai1)=exp(−(zmpbias(ai1)/Lfoot)2) ‥(112)。
評価式(112)から明らかなように、ロボットRの予測ZMPが目標ZMPに近いほど、すなわち、ロボットRの姿勢が安定になる可能性が高い第1行動候補ai1であるほど第1適合度e1(ai1)が高く評価される。
さらに、複数の第1行動候補ai1のそれぞれについて、第2モジュールmod2から受け取った第2行動指針(図3/矢印D2-参照)に基づき、第2適合度e2(ai1)の推定値(第2推定適合度)e2^(ai1)が評価される。第2推定適合度e2^(ai1)は、たとえば、ロボットRの着地位置pos(ai1;k1)と、第2行動指針としての局所経路との間隔poserr(ai1;k1)と、ロボットRの歩幅(最大歩幅)Lstepとに基づき、評価式(120)にしたがって算出される。
e2^(ai1)=Πk1=1~q1exp(−poserr(ai1;k1)/Lstep) ‥(120)。
評価式(12)から明らかなように、ロボットRの着地位置pos(ai1;k1)が局所経路に近いほど、すなわち、ロボットRが物体との接触を回避しうると予測される第1行動候補ai1であるほど第2推定適合度e2^(ai1)が高く評価される。
なお、第2行動目的が「ロボットRの物体に対する相対姿勢を目標相対姿勢に維持しながら、ロボットRに当該物体との接触を回避させる」という目的である場合、第1行動候補ai1としての歩容により定まるロボットRの局所経路に対する相対姿勢(たとえば、局所経路に対する基体B0の相対方位により特定される。)と目標相対姿勢との偏差(相対姿勢偏差)が小さいほど、第2推定適合度e2^(ai1)が高く評価されるように定義されていてもよい(評価式(120)参照)。このような第2行動目的は、ロボットRの姿勢を物体としての人間に視認させ、この姿勢に基づいてロボットRの挙動を人間に予測させることにより、人間による偶発的な動作によるロボットRとの接触を回避させる観点から有意義である。
そして、第1適合度e1(ai1)および第2推定適合度e2^(ai1)に基づき、評価式(141)または(142)にしたがって第1総合適合度f1(ai1)が評価される。
f1(ai1)=e1(ai1)(e2^(ai1)+1) ‥(141)。
f1=w1e1(w2e2^+1) ‥(142)。
ここで「w2」および「w1(=(w2+1)-1)」は重み係数である。
第1総合適合度f1(ai1)の評価結果が、第1行動候補ai1および第2行動候補ai2のそれぞれを探索するための収束性および多様性がある「第1探索戦略」として定義される。第1探索戦略には、第1総合適合度f1(ai1)の評価が最高であった一の第1行動候補ai1としての「第1行動予約」が含まれている。第1行動予約と第1行動指針とは共通であってもよい。
評価式(141)および(142)から明らかなように第1総合適合度f1(ai1)に対する寄与度は、第1適合度e1(ai1)のほうが第2推定適合度e2^(ai1)よりも高い。たとえば、第1適合度e1(ai1)が0に近づくと、第2推定適合度e2^(ai1)の高低とは無関係に第1総合適合度f1(ai1)も0に近づく。その一方、第2推定適合度e2^(ai1)が0に近づいても、第1総合適合度f1(ai1)は0ではなく第1適合度e1(ai1)に近づき、第1適合度e1(ai1)の高低が第1総合適合度f1(ai1)に直接反映されるようになる。
ここで、第2適合度e2(ai1)ではなくその推定値である第2推定適合度e2^(ai1)が用いられるのは、第2モジュールmod2(第1モジュールmod1よりも1階位下位のモジュール)が有する第2行動目的のみならず、第3モジュールmod3(第1モジュールmod1よりも2階位下位のモジュール)が有する第3行動目的をも踏まえて第1探索戦略が定義されるようにするためである。
すなわち、前記のように第2行動指針としての局所経路を基準として、第1行動候補ai1のそれぞれの第2行動目的に対する適合度が評価される(評価式(120)参照)。しかるに、後述するように第2行動指針は第3行動指針が反映された形で第2モジュールmod2により探索される。このため、第2推定適合度e2^(ai1)が算出されることにより、第1行動候補ai1の第3行動目的に対する適合度が間接的に評価される。そして、第2推定適合度e2^(ai1)に基づいて第1総合適合度f1(ai1)が評価されることにより(評価式(141)(142)参照)、当該評価結果としての第1探索戦略には、第2行動目的が直接的に反映されるのみならず、第3行動目的も間接的に反映されうる。
(第2行動探索処理の手順)
まず、現在状態に基づき、ロボットRの挙動を表わす挙動予測モデルにしたがって、ロボットRの直近未来状態が予測される(図5/STEP200)。これにより、たとえば、ロボットRが現在位置から1歩進んだときの着地位置が予測される。なお、第1モジュールmod1により予測されたロボットRの直近未来状態が、第1モジュールmod1から第2モジュールmod2に出力されてもよい(図3/矢印D2+参照)。
また、第1探索戦略、第2探索戦略および第3探索戦略にしたがってロボットRの複数の第2行動候補{ai2|i2=1,2,‥,n2}が探索される(図5/STEP210)。具体的には、第1総合適合度f1の評価結果、第2総合適合度f2の評価結果および第3総合適合度f3の評価結果に応じた個体の世代交代方法にしたがって第2行動候補ai2が探索される。この探索方法については後述する。
これにより、たとえば、図7(a)に示されているように、第2指定歩数q2(たとえばq2=3<q1)にわたる歩容を定めるロボットRの着地位置pos(ai2;k2)(k2=1〜q2)を含む局所経路が第2行動候補ai2として探索される。たとえば、前記のように予測されたロボットRの1歩先の着地位置が局所経路の始点とされる。なお、ロボットRの1歩先の着地位置を起点として、第1行動予約(図3/矢印D2+参照)としての第1指定歩数q1にわたる歩容を経た時点におけるロボットRの予測着地位置が局所経路の始点とされてもよい。「局所経路」は、外部状態としての物体の位置等に基づいて定められる、ロボットRが当該物体との接触を回避するための経路を意味する。
また、複数の第2行動候補ai2のそれぞれに基づき、第2状態予測モデルにしたがってロボットRの複数の第2未来状態{si2|i2=1,2,‥,n2}が予測される(図5/STEP220)。これにより、たとえば、ロボットRが第2行動候補ai2としての局所経路にしたがって移動した場合の、ロボットRと物体との最短距離C(ai2)が第2未来状態si2として予測される(図7(a)参照)。
そして、複数の第2行動候補ai2のそれぞれについて、次の手順にしたがって第2総合適合度f2(ai2)が評価される(図5/STEP230)。
まず、第2未来状態si2に基づき、第2行動目的に鑑みた適合性を表わす第2適合度e2(ai2)が評価される。第2適合度e2(ai2)は、たとえば、図7(a)に示されている局所経路と物体との最短距離C(ai2)と、物体のサイズRobjectとに基づき、図7(b)に変化特性が示されている評価式(221)にしたがって算出される。物体の中心位置から無限にまたはきわめて大きく離れている局所経路(線分)が第2目的に応じた状態空間において定義される基準点に相当する。局所経路と物体の中心位置との最短距離の逆数C-1が当該基準点との乖離度に相当する。たとえば、物体の輪郭が平面上の円に近似された場合における当該円の半径が物体のサイズRobjectとして定義される。
e2(ai2)=1 (C-1(ai2)≦βRobject -1(0<β<1)の場合),
exp(−(Robject(C-1(ai2)−βRobject -1))2) (βRobject -1<C-1(ai2)の場合)‥(221)。
評価式(221)から明らかなように、ロボットRの局所経路と物体との最小間隔の逆数C-1が第2閾値βRobject -1以下の範囲である場合、すなわち、ロボットRがある程度以上の確度で物体との接触を回避しうると予測される第2行動候補ai2であれば第2適合度e2(ai2)は一定値(=1)に評価される。その一方、ロボットRの局所経路と物体との最小間隔の逆数C-1が第2閾値βRobject -1を超えて大きくなるほど、すなわち、ロボットRが物体と接触する可能性が高い第2行動候補ai2であるほど第2適合度e2(ai2)が低く評価される。
なお、図7(c)に変化特性が示されている評価式(222)にしたがって第2適合度e2(ai2)が算出されてもよい。
e2(ai2)=exp(−(Robject/C(ai2))2) ‥(222)。
評価式(222)から明らかなように、ロボットRの局所経路と物体との最小間隔の逆数C-1が小さいほど、すなわち、ロボットRが物体との接触を回避する可能性が高い第2行動候補ai2であるほど第2適合度e2(ai2)が高く評価される。
なお、第2行動目的が「ロボットRの物体に対する相対姿勢を目標相対姿勢に維持しながら、ロボットRに当該物体との接触を回避させる」という目的である場合、第2行動候補ai2としての局所経路の姿勢に対するロボットの相対姿勢(たとえば、局所経路に対する基体B0の相対方位により特定される。)と目標相対姿勢との偏差(相対姿勢偏差)が小さいほど、第2適合度e2(ai2)が高く評価されるように定義されていてもよい(評価式(221)(222)参照)。
また、複数の第2行動候補ai2のそれぞれについて、第3モジュールmod3から受け取った第3行動指針(図3/矢印D3-参照)に基づき、第3適合度e3(ai2)の推定値(第3推定適合度)e3^(ai2)が評価される。第3推定適合度e3^(ai2)は、たとえば、ロボットRの着地位置pos(ai2;k2)と、第3行動指針としての大局経路との間隔poserr(ai2;k2)と、ロボットRの歩幅(最大歩幅)Lstepとに基づき、評価式(230)にしたがって算出される。
e3^(ai2)=Πk2=2~q2exp(−poserr(ai2;k2)/Lstep) ‥(230)。
評価式(230)から明らかなように、ロボットRの着地位置pos(ai2;k2)が大局経路に近いほど、すなわち、ロボットRが目標位置に到達するための大局経路にしたがって移動すると予測される第2行動候補ai2であるほど第3推定適合度e3^(ai2)が高く評価される。
なお、第3行動目的が「ロボットRを目標姿勢で目標位置に到達させる」という目的である場合、第2行動候補ai2としての局所経路に応じて定まるロボットRの姿勢(たとえば、基体B0の方位により特定される。)と大局経路の姿勢との偏差(姿勢偏差)が小さいほど、第3推定適合度e3^(ai2)が高く評価されるように定義されていてもよい(評価式(230)参照)。このような第3行動目的は、ロボットRが目標位置に到着したときまたは到着した後、物体の把持等、その姿勢に応じて難易度が変化する作業をロボットRに実行させる観点から有意義である。
そして、第2適合度e2(ai2)および第3推定適合度e3^(ai2)に加えて、第1モジュールmod1により評価された第1適合度e1(ai2)に基づき、評価式(241)または(242)にしたがって第2総合適合度f2(ai2)が評価される。
f2(ai1)=e2(ai2)(e1(ai2)(e3^(ai2)+1)+1) ‥(241)。
f2=w2e2(w1e1(w3e3^+1)+1) ‥(242)。
ここで「w1」「w3」および「w2(={w1(w3+1)+1}-1」は重み係数である。
第2総合適合度f2(ai2)の評価結果が、第1行動候補ai1、第2行動候補ai2および第3行動候補ai3のそれぞれを探索するための収束性および多様性がある「第2探索戦略」として定義される。第2探索戦略には、第2総合適合度f2(ai2)の評価が最高であった一の第2行動候補ai2としての「第2行動予約」が含まれている。第2行動予約と第2行動指針とは共通であってもよい。
評価式(241)および(242)から明らかなように、第2総合適合度f2(ai2)に対する寄与度は、第2適合度e2(ai2)のほうが、第1適合度e1(ai2)および第3推定適合度e3^(ai2)のそれぞれよりも高い。たとえば、第2適合度e2(ai2)が0に近づくと、第1適合度e1(ai2)および第3推定適合度e3^(ai2)の高低とは無関係に第2総合適合度f2(ai2)も0に近づく。その一方、第1適合度e1(ai2)および第3推定適合度e3^(ai2)が0に近づいても、第2総合適合度f2(ai2)は0ではなく第2適合度e2(ai2)に近づき、第2適合度e2(ai2)の高低が第2総合適合度f2(ai2)に直接反映されるようになる。
さらに、第2総合適合度f2(ai2)に対する寄与度は、第1適合度e1(ai2)のほうが第3推定適合度e3^(ai2)よりも高い。たとえば、第1適合度e1(ai2)が0に近づくと、第3推定適合度e3^(ai2)の高低とは無関係に第2総合適合度f2(ai2)が小さくなる。その一方、第3推定適合度e3^(ai3)が0に近づいても第2総合適合度f2(ai2)はe2(ai2)・e1(ai1)に近づき、第1適合度e1(ai1)の高低が第3推定適合度e3^(ai2)に反映されうる。
ここで、第3適合度e3(ai2)ではなくその推定値である第3推定適合度e3^(ai2)が用いられるのは、第3モジュールmod3(第2モジュールmod2よりも1階位下位のモジュール)が有する第2行動目的のみならず、(本実施形態では存在しないものの存在する場合には)第4モジュールmod4(第2モジュールmod2よりも2階位下位のモジュール)が有する第4行動目的(たとえば、ロボットRを目標位置まで移動させた後、さらに第2の目標位置まで移動させるという目的)をも踏まえて第2探索戦略が定義されるようにするためである。
すなわち、前記のように第3行動指針としての大局経路を基準として、第2行動候補ai2のそれぞれの第3行動目的に対する適合度が評価される(評価式(230)参照)。しかるに、第3行動指針は第4行動指針が反映された形で第3モジュールmod3により探索されうる。このため、第3推定適合度e3^(ai2)が算出されることにより、第2行動候補ai2の第4行動目的に対する適合度が間接的に評価される。そして、第3推定適合度e3^(ai2)に基づいて第2総合適合度f1(ai1)が評価されることにより(評価式(241)(242)参照)、当該評価結果としての第2探索戦略には、第3行動目的が直接的に反映されるのみならず、第4行動目的も間接的に反映されうる。
(第3行動探索処理の手順)
まず、現在状態に基づき、ロボットRの挙動を表わす挙動予測モデルにしたがって、ロボットRの直近未来状態が予測される(図5/STEP300)。これにより、たとえば、ロボットRが現在位置から1歩進んだときの着地位置が予測される。なお、第1モジュールmod1または第2モジュールmod2により予測されたロボットRの直近未来状態が、第2モジュールmod2から第3モジュールmod3に出力されてもよい(図3/矢印D3+参照)。
また、第2探索戦略および第3探索戦略にしたがってロボットRの複数の第3行動候補{ai3|i3=1,2,‥,n3}が探索される(図5/STEP310)。具体的には、第2総合適合度f2の評価結果および第3総合適合度f3の評価結果に応じた個体の世代交代方法にしたがって第3行動候補ai3が探索される。この方法については後述する。
これにより、たとえば、図8(a)に示されているようにロボットRが目標位置に向かう歩容を定める大局経路が第3行動候補ai3として探索される。たとえば、前記のように予測されたロボットRの1歩先の着地位置が大局経路の始点とされる。なお、第2行動予約(図3/矢印D3+参照)としての局所経路の終点が、大局経路の始点とされる。ロボットRの目標位置はロボットRの外部から制御システムに入力または送信されるほか、制御システム1により外部状態の認識結果に基づいて計算されてもよい。
また、複数の第3行動候補ai1のそれぞれに基づき、第3状態予測モデルにしたがってロボットRの複数の第3未来状態{si3|i3=1,2,‥,n3}が予測される(図5/STEP320)。これにより、たとえば、ロボットRがその1歩先の予測着地位置から、大局経路にしたがって移動した場合の移動距離L(ai3)が第3未来状態si3として予測される。
そして、複数の第3行動候補ai3のそれぞれについて、次の手順にしたがって第3総合適合度f3(ai3)が評価される(図5/STEP330)。
まず、第3未来状態si3に基づき、第3行動目的に鑑みた適合性を表わす第3適合度e3(ai3)が評価される。第3適合度e3(ai3)は、たとえば、図8(a)に示されているロボットRの予測移動距離L(ai3)と、大局経路の始点と目標位置との直線距離Ldestとに基づき、図8(b)に変化特性が示されている評価式(331)にしたがって算出される。当該直線距離Ldestが第3目的に応じた状態空間において定義される基準点に相当する。ロボットRの予測移動距離Lと当該直線距離Ldestとの偏差が、当該基準点からの乖離度に相当する。
e3(ai3)=1 (Ldest≦L(ai3)≦γLdest(1<γ)の場合),
γLdest/L(ai3) (γLdest<L(ai3)の場合)‥(331)。
評価式(331)から明らかなように、ロボットRの予測移動距離Lが第3閾値γLdest以下の範囲である場合、すなわち、ロボットRが目標位置に到達するまでの移動距離がある程度短いと予測される第3行動候補ai3であれば第3適合度e3(ai3)は一定値(=1)に評価される。その一方、ロボットRの予測移動距離Lが第3閾値γLdestを超えて大きくなるほど、すなわち、ロボットRが目標位置に到達するまでの移動距離がある程度以上長くなる可能性が高い第3行動候補ai3であるほど第3適合度e3(ai3)が低く評価される。
なお、図8(c)に変化特性が示されている評価式(332)にしたがって第3適合度e3(ai3)が算出されてもよい。
e3(ai3)=Ldest/L(ai3) ‥(332)。
評価式(332)から明らかなように、ロボットRが直線的に目標位置に向かうほど、すなわち、ロボットRが目標位置に到達するまでの移動距離Lが短いと予測される第3行動候補ai3であるほど第3適合度e3(ai3)が高く評価される。
なお、第3行動目的が「ロボットRを目標姿勢で目標位置に到達させる」という目的である場合、第3行動候補ai3としての大局経路に応じて定まるロボットRの目標位置における姿勢(たとえば、基体B0の方位により特定される。)と目標姿勢との偏差(姿勢偏差)が小さいほど、第3適合度e3(ai3)が高く評価されるように定義されていてもよい(評価式(331)(332)参照)。
そして、第3適合度e3(ai3)に加えて、第2モジュールmod2により評価された第2適合度e2(ai3)に基づき、評価式(341)または(342)にしたがって第3総合適合度f3(ai3)が評価される。
f3(ai3)=e3(ai3)(e2(ai3)+1) ‥(341)。
f3=w3e3(w2e2+1) ‥(342)。
ここで「w2」および「w3(=(w2+1)-1)」は重み係数である。
第3総合適合度f3(ai3)の評価結果が、第2行動候補ai2および第3行動候補ai3のそれぞれを探索するための収束性および多様性がある「第3探索戦略」として定義される。
評価式(341)および(342)から明らかなように、第3総合適合度f3(ai3)に対する寄与度は、第3適合度e3(ai3)のほうが第2適合度e2(ai3)よりも高い。たとえば、第3適合度e3(ai3)が0に近づくと、第2適合度e2(ai3)の高低とは無関係に第3総合適合度f3(ai3)も0に近づく。その一方、第2適合度e2(ai3)が0に近づいても第3総合適合度f3(ai3)は0ではなく第3適合度e3(ai3)に近づき、第3適合度e3(ai3)の高低が、第3総合適合度f3(ai3)に直接反映されるようになる。
(探索戦略にしたがった行動候補の探索方法)
行動候補の探索方法について説明する。探索方法としては「収束性」および「多様性」がある方法が採用される。「収束性」がある方法により、探索範囲(行動候補により定義される空間における範囲を意味する。)のうち前回評価が高かった行動候補の近くにおいて、今回評価がより高くなるような行動候補が探索されうる。「多様性」がある当該方法により、前回評価に拘泥されすぎることなく、今回評価がよりより高くなるような行動候補が広範囲で探索されうる。
収束性および多様性がある行動候補の探索方法として、進化的探索手法が採用されうる。
具体的には、まず、図9(a)に示されている複数の親個体(行動候補aに対応している。「●」により表現されている。)のうち、円で囲まれた親個体が選択される。選択確率は適合度f(a)(実線で示されている評価曲線にしたがって評価される。)が高いほど高い。
そして、図9(b)に示されているように適合度f(a)が高い親個体ほど当該親個体に比較的小さいノイズ(ノイズは適合度f(a)が高いほど小さくなる。)が加えられた結果として子個体(「○」により表現されている。)が当該親個体の周囲の比較的狭い範囲に高確率で分布するように生成される。その一方、適合度f(a)が低い親個体ほど当該親個体に比較的大きいノイズ(ノイズは適合度f(a)が低いほど大きくなる。)が加えられた結果として子個体が当該親個体の周囲の比較的広い範囲に高確率で分布するように生成される。そのほか、図示しないが不規則に個体が生成される。すべての個体に対する不規則な個体の占有率はたとえば0.30程度に設定される。
前記のような世代交代手法が採用されることにより、世代を経るに連れて適合度f(a)が大きい領域に個体が集中していく。これにより、モジュールが主担当する目的への適合性が高い行動候補aが適応的に求められる。
さらに、探索方法の多様性について説明するため、2つのモジュールAおよびモジュールBを対象として考察する。モジュールAはその目的に鑑みた行動候補aの適合性を表わす適合度fA(a)を評価し、その評価結果に応じて探索戦略を設定してモジュールBに出力するように構成されている。モジュールBはモジュールAから探索戦略を受け取り、この探索戦略にしたがってその目的に鑑みた適合性を表わす適合度fB(a)が高い行動候補aを探索するように構成されている。
まず、図10(a)上側に示されているようにモジュールAにより適合度fA(a)が最大値を示す「1つの行動候補a1」が探索戦略として設定された場合を考える。この場合、たとえば、図10(a)下側に示されているようにモジュールBにより探索戦略としての行動候補a1を起点として、適合度fB(a)が極大値を示す行動候補a2-が探索される。しかし、この解は適合度fB(a)が最高となる最適解ではなく、局所的な解にしか過ぎない可能性がある。
そこで、図10(b)上側に示されているようにモジュールAにより適合度fA(a)が最大値(または極大値)を示す「1つの行動候補a1の周辺範囲」が探索戦略として設定される。これに応じて、たとえば、図10(b)下側に示されているようにモジュールBにより探索戦略としての範囲において適合度fB(a)が最高となるような行動候補a2+(≠a2-)が探索される。このように探索範囲が広げられた結果、最適解が得られる可能性を高くすることができる。
また、図11(a)上側に示されているようにモジュールAにより適合度fA(a)が最高値を示す「1つの行動候補a1」が探索戦略として設定された場合を考える。この場合、たとえば、図11(a)下側に示されているようにモジュールBにより探索戦略としての1つの行動候補a1の周辺の単一範囲において適合度fB(a)が極大値を示す行動候補a2-が探索される。しかし、この解は適合度fB(a)が最高となる最適解ではなく、局所的な解にしか過ぎない可能性がある。
そこで、図11(b)上側に示されているようにモジュールAにより適合度fA(a)が極大値を示す「複数の行動候補a1-およびa1+」が探索戦略として設定される。これに応じて、たとえば、図11(b)下側に示されているようにモジュールBにより探索戦略としての複数の行動候補a1-およびa1+のそれぞれの周囲範囲において適合度fB(a)が最高となるような行動候補a2+(≠a2-)が探索される。このように探索範囲が広げられた結果、最適解が得られる可能性を高くすることができる。
(実施例)
図12(a)〜(c)のそれぞれの下側に示されているロボットRのZMPおよび重心位置(「◎」により表現されている。)により表現される当該ロボットRの挙動状態は、図12(a)〜(c)のそれぞれの上側に示されている倒立振子の着地位置および重心位置(「◎」により表現されている。)により表現される当該倒立振子の挙動状態にモデル化して考えることができる。
そこで、倒立振子をその上部を変移自在に上方から吊り下げた状態で移動させて実験を行った。まず、図13(a)に示されているように倒立振子にその出発位置から移動を開始させ(時刻t=0)、その後、外乱(物体との接触により受けた力)が倒立振子に与えられた(時刻t=t0)。これに応じて倒立振子が安定化を図りながら新たな経路に沿って移動し(時刻t=t1)、物体との接触を回避しながらさらに移動し(時刻t=t2)、目標位置に到達する(時刻t=t3)。
この際、図13(b)に示されているように個体フィルタを用いた第1行動候補としての離散的な着地位置の探索のための計算が実行された。個体は升目が付された地面の上方にある雲状の塊を構成する個々の点により表現され、その地面からの高さは第1総合指数f1の大きさを表わしている。この場合、当該雲の尾根に沿って並んでいる点が地面に垂直に投影された点が、第1行動予約または第1行動指針としての着地位置を表わしている。
前記のようにロボットRの挙動状態は倒立振子の挙動状態にモデル化されることから、当該モデルを用いてロボットRの行動が制御されうる。これにより、図14に示されているようにロボットRを倒立振子と同様に行動させることができる。すなわち、まず、ロボットRにその出発位置から移動を開始させ(時刻t=0)、その後、外乱がロボットRに与えられる(時刻t=t0)。これに応じて、ロボットRは安定化を図りながら新たな経路に沿って移動し(時刻t=t1)、物体との接触を回避しながらさらに移動し(時刻t=t2)、目標位置に到達する(時刻t=t3)。
(本発明のロボットRおよびその制御システムの作用効果)
前記機能を発揮するロボットRによれば、複数のモジュールmodi(i=1,2,3)のそれぞれに自己が主担当する主目的を優先させながらも、他のモジュールが主担当する副目的をも勘案した形で、制御対象であるロボットRの行動候補が探索される(図5/STEP110,STEP210,STEP310参照)。このように、異なるモジュール間で目的が緩やかに共有された形で制御対象の行動候補が探索されうる。
また、上位モジュール(少なくとも制御モジュールmod0)により予測された制御対象の未来状態に続く、探索された行動候補に応じた制御対象の複数の未来状態が予測される(図5/STEP120,STEP220,STEP320参照)。
そして、未来状態のそれぞれに基づき、高周波の上位モジュールが主担当する上位目的が、低周波の下位モジュールが主担当する下位目的よりも優先され、自己が主担当する主目的および副目的に対する総合適合度が評価される(図5/STEP130,STEP230,STEP330参照)。これにより、高周波の上位モジュールが主担当する上位目的が、低周波の下位モジュールが主担当する下位目的よりも優先して反映された態様でロボットRの行動が制御される。
このため、外乱が発生した場合(図4/時刻t=t0参照)、当初は応答が速い上位モジュールが主担当する上位目的が制御対象の行動形態に反映され、この外乱による影響の迅速な低減が図られる(図4/時刻t=t1参照)。さらに、時間経過とともに応答が遅い下位モジュールが主担当する下位目的も制御対象の行動形態に反映され、複数の目的のすべてに適合するような制御対象の行動形態の回復が図られる(図4/時刻t=t2,t=t3参照)。これにより、任意形態の外乱に対して、ロボットRがその行動目的に鑑みて適当な形態で行動することができる。
特に、第jモジュール(前記実施形態ではj=1,2,3)により探索されたすべての第j行動候補のうち、状態空間内で第j目的に応じて定義される基準点からの乖離度が第j閾値以内である第j行動候補ajk、すなわち、第j指定行動候補群に含まれる第j行動候補ajkについては、第j適合度ejが一定値「1」に評価される(前記式(111)(221)(331)、図6(b)、図7(b)、図8(b)参照)。この一定値は「1」には限定されず、任意の値が採用されうる。その一方、第j指定行動候補群に含まれない第j行動候補ajkについては、第j適合度ejが一定値よりも低い値に評価される。すなわち、行動候補としてとりうる集合のうち、第j適合度ejが十分に高い値をとる行動候補の集合(第j解空間)において、第j適合度ejが当該一定値に評価される領域(フラット領域)と、第j適合度ejが当該一定値よりも低い領域とが形成される。当該構成は、ある程度第j目的に適合している第j行動候補ajkは、第j適合度を近似的に同等と考えてもよいという思想に基づいている。
第1行動候補を定めるZMPbiasにより定義される第1解空間については、図6(b)に示されているように0≦ZMPbias≦αLfootの範囲が第1適合度e1が一定値「1」であるフラット領域として定義され、αLfoot<ZMPbiasの範囲が第1適合度e1が当該一定値「1」から徐々に低下するスロープ領域として定義されている。
第2行動候補を定めるC-1により定義される第2空間については、図7(b)に示されているように0≦C-1≦β(Robject)-1の範囲が第2適合度e2が一定値「1」であるフラット領域として定義され、β(Robject)-1<C-1の範囲が第2適合度e2が当該一定値「1」から徐々に低下するスロープ領域として定義されている。
第3行動候補を定めるLにより定義される第3空間については、図8(b)に示されているようにLdest≦L≦γLdestの範囲が第3適合度e3が一定値「1」であるフラット領域として定義され、γLdest<Lの範囲が第3適合度e3が当該一定値「1」から徐々に低下するスロープ領域として定義されている。
フラット領域の形成により、第j解空間において総合適合度が最大または極大となるような解が探索されうる領域が広がる。このため、上位モジュールが、下位総合適合度が最大または極大となる下位行動候補を下位行動指針として下位モジュールから受け取り、自ら探索した行動候補のそれぞれについて下位行動指針との近似度を推定下位適合度として評価するように構成されている場合、解空間において当該上位モジュールにより解が探索されうる領域が広がる(図10、図11参照)。
また、第j解空間においてフラット領域に含まれている複数の第j行動候補について、第j適合度ejの高低に応じた優劣は考慮されなくてもよい。このため、第j適合度を必要以上に高めるような解が探索されること、ひいては、制御対象の動作制御の過剰な最適化が防止される。さらに、第jモジュールは、他のモジュール(第j−1モジュールおよび第j+1モジュールのうち一方または両方)が主担当する副目的に鑑みた適合度に応じて総合適合度が最大または極大となる第j行動候補を探索すれば足る。すなわち、第jモジュールを、主目的ではなく、副目的に鑑みた適合度を高くするような解の探索に集中させることができる。
このため、第j適合度ejの評価のための各モジュールmodjの演算処理負荷の軽減、ひいては、演算処理結果の算出の早期化を図りながら、第j総合適合度fjが評価または算出されうる(前記式(141)(142)(241)(242)(341)(342)参照)。これにより、任意形態の外乱に対して、制御対象としてのロボットRの行動目的に鑑みて適当な形態で、このロボットRを迅速に応答または行動させることができる。
(本発明の他の実施形態)
なお、制御対象はロボットR(図1参照)のほか、車両等、アクチュエータを備えているあらゆる装置であってもよい。
前記実施形態では、行動探索用のモジュールは3つであったが、2つであってもよく、4つ以上であってもよい。
第1モジュールmod1および第2モジュールmod2のみが制御システム1に含まれる場合、第2総合適合度f2(ai2)は第2適合度e2(ai2)および第1適合度e1(ai2)に基づき、評価式(244)にしたがって評価される。
f2(ai1)=e2(ai2)(e1(ai2)+1) ‥(244)。
また、第3モジュールmod3より上位の(演算周期が長い)第4モジュールmod4が制御システム1の構成要素としてさらに含まれている場合、第3総合適合度f3(ai3)は、第2総合適合度f2(ai2)の評価式(241)と同様の評価式(343)にしたがって評価される。
f3(ai3)=e3(ai3)(e2(ai3)(e4^(ai3)+1)+1) ‥(343)。
この場合、第4モジュールmod4により第4総合適合度f4(ai4)は、第3総合適合度f3(ai3)の評価式(34)と同様の評価式にしたがって評価される。
さらに、4つの行動探索モジュールが制御システム1の構成要素として含まれている場合、第2適合度e2(ai2)、第1適合度e1(ai2)および第3推定適合度e3^(ai2)に加えて、さらに第4推定適合度e4^(ai2)に基づき、評価式(244)にしたがって第2総合適合度f2(ai2)が評価されてもよい。
f2(ai2)=e2(ai2)
×(e1(ai2)(e3^(ai2)(e4^(ai2)+1)+1)+1) ‥(244)。
5つ以上の行動探索モジュールが構成要素として制御システム1に含まれる場合も、同様の考え方にしたがって中間モジュールの総合適合度が算出されうる。