(57)【要約】【課題】多くのセンサ系統を不要とする移動体システムおよび移動体のＺＭＰ算出方法を提供することを目的とする。【解決手段】移動体に搭載された複数の撮像手段２０Ａ，２０Ｂと、複数の撮像手段２０Ａ，２０Ｂによって取得した画像データから前記移動体の位置情報を算出する位置算出手段３１１と、経時的に取得する前記位置情報に基づいて前記移動体のＺＭＰを算出するＺＭＰ算出手段３１４と、を有することを特徴とする移動体システム。【選択図】図３
2009125922.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載された複数の撮像手段と、
前記複数の撮像手段によって取得した画像データから前記移動体の位置情報を算出する位置算出手段と、
経時的に取得する前記位置情報に基づいて前記移動体のＺＭＰを算出するＺＭＰ算出手段と、
を有することを特徴とする移動体システム。
【請求項２】
さらに、前記算出したＺＭＰのデータを記憶する記憶手段を有することを特徴とする請求項１に記載の移動体システム。
【請求項３】
前記ＺＭＰ算出手段は、前記位置情報および前記移動体のオプティカルフローから求めた加速度情報に基づいて前記移動体のＺＭＰを算出することを特徴とする請求項１に記載の移動体システム。
【請求項４】
さらに、前記移動体に搭載され、前記移動体の加速度情報を取得するための加速度センサを有し、
前記ＺＭＰ算出手段は、前記位置情報および前記加速度情報に基づいて前記移動体のＺＭＰを算出することを特徴とする請求項１に記載の移動体システム。
【請求項５】
さらに、前記位置情報に基づいて前記移動体の重心を算出する重心算出手段と、
を有することを特徴とする請求項１に記載の移動体システム
【請求項６】
移動体に搭載された複数の撮像手段によって前記移動体の周辺の画像データを経時的に取得する段階と、
前記取得した画像データから位置情報を算出する段階と、
前記位置情報に基づいて前記移動体のＺＭＰを算出する段階と、
を有することを特徴とする移動体のＺＭＰ算出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体システムおよび移動体のＺＭＰ算出方法に関し、より詳細には、ＺＭＰを算出するための移動体システムおよび移動体のＺＭＰ算出方法に関する。
【背景技術】
【０００２】
二足歩行ロボットなどの移動体が二足歩行を実現するために、移動体に加わる重力、慣性力、および床反力を考慮したＺＭＰ（ゼロ・モーメント・ポイント）の軌道生成と制御が用いられることが知られている。一般に、ＺＭＰを算出する方法として、移動体の位置および加速度の情報を用いて算出する方法がある（例えば、特許文献１参照）。従来のＺＭＰを算出する移動体システムでは、特にリアルタイム制御を行う場合、移動体の位置および加速度を測定するために、位置センサおよび加速度センサを用いている。
【特許文献１】特開２００４−１１４２４３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、このような従来のＺＭＰを算出する移動体システムでは、位置センサおよび加速度センサなどのセンサ系を移動体の各質点におくことで計測処理するため、移動体全体のＺＭＰを算出するにあたって、各質点系におけるＺＭＰを算出してから移動体の重心位置へ等価計算処理するのに時間がかかってしまうという問題点があった。さらに、一般的にセンサ系が多いことはノイズに弱くなることや故障率の増加を招くという問題点があった。
【０００４】
本発明は、上記問題点を解決し、移動体に搭載された複数の撮像手段によって算出された位置情報に基づいてＺＭＰを算出することによって、多くのセンサ系統を不要とする移動体システムおよび移動体のＺＭＰ算出方法を提供することを目的とする。
【課題を解決するための手段】
【０００５】
本発明の上記目的は、下記の手段によって達成される。
【０００６】
本発明の移動体システムは、移動体に搭載された複数の撮像手段と、前記複数の撮像手段によって取得する画像データから前記移動体の位置情報を算出する位置算出手段と、経時的に取得する前記位置情報に基づいて前記移動体のＺＭＰを算出するＺＭＰ算出手段と、を有することを特徴とする。
【０００７】
本発明の移動体のＺＭＰ算出方法は、移動体に搭載された複数の撮像手段によって前記移動体の周辺の画像データを経時的に取得する段階と、前記取得した画像データから位置情報を算出する段階と、前記位置情報に基づいて前記移動体のＺＭＰを算出する段階と、を有することを特徴とする。
【発明の効果】
【０００８】
以上のように構成された本発明にかかる移動体システムおよび移動体のＺＭＰ算出方法によれば、移動体に搭載された複数の撮像手段によって算出された位置情報に基づいてＺＭＰを算出することによって、多くのセンサ系統を不要とすることができる。
【発明を実施するための最良の形態】
【０００９】
以下、図面を参照して、本発明の実施の形態を説明する。
【００１０】
本発明のそれぞれの実施の形態では、二足歩行ロボットに移動体システムを適用した場合を例にとって説明する。すなわち、本実施形態では、移動体として二足歩行ロボットを採用する。
＜第１の実施形態＞
本発明の第１実施形態の移動体システムは、たとえば、二足歩行ロボットの歩容パターンの生成・自律化を可能とするために、複数の撮像手段から取得した位置情報に基づいて加速度情報を算出し、さらに、加速度情報から二足歩行ロボットのＺＭＰを算出するものである。
【００１１】
図１は、本実施形態の移動体システムが適用される二足歩行ロボットの一例を示す図である。二足歩行ロボット１は、そのＺＭＰの軌道生成と制御を用いて、リアルタイムで歩行運動を作り出すロボットである。なお、図１における任意の対象物２は、二足歩行ロボット１の進行方向に位置するものであって、二足歩行ロボット１のＺＭＰを算出する際に対象となる物である。
【００１２】
図２は、本実施形態の移動体システムの概略構成を示すブロック図である。
【００１３】
本実施形態の移動体システム１００は、図２に示すように、ステレオカメラ２０と、ＺＭＰの算出など各種計算を行うコンピュータ３０とを備える。
【００１４】
ステレオカメラ２０は、二足歩行ロボット１に搭載された二つのカメラであって、撮像手段として機能する。具体的には、ステレオカメラ２０に用いる二つのカメラ２０Ａ，２０Ｂは、ＣＣＤ素子などの撮像素子とその他の光学系によって構成可能であり、その構成自体は、一般的なカメラと同様であるので、詳しい説明を省略する。二つのカメラ２０Ａ，２０Ｂは、位置算出を用意にするために、それぞれの光軸は互いに平行で、かつ、それぞれの画像面が同一平面状にあることが望ましい。また、ステレオカメラ２０は、たとえば、重心位置に設置すると駆動部が視界方向を遮ることや、意匠設計上などの理由から二足歩行ロボット１の頭部に相当する部分の位置に設置することができる。なお、ＺＭＰは通常全体について求めるので、二足歩行ロボット１の重心からステレオカメラ２０の設置位置にオフセットが生じるので（図１参照）、このオフセットを考慮してＺＭＰを算出する必要がある。
【００１５】
コンピュータ３０は、ステレオカメラ２０によって経時的に取得した画像データに基づいて二足歩行ロボット１のＺＭＰを算出するものであって、移動体システム１００の一部の役割を担う。コンピュータ３０は、二足歩行ロボット１に搭載することができる。なお、コンピュータ３０は、二足歩行ロボット１に搭載される場合に限らず、二足歩行ロボット１とは別の位置に設置することもでき、この場合、ステレオカメラ２０により取得した画像データは、コンピュータ３０に無線または有線などの電気通信回線を通じて送ることができる。
【００１６】
コンピュータ３０は、たとえば、図２に示すように、マイクロプロセッサからなるＣＰＵ（中央演算処理装置）３１、ＣＰＵ３１にバス結合されたＲＯＭ３２、ＲＡＭ３３、ハードディスク３４、表示部３５、入力部３６、およびインターフェイス３７などの構成要素を備えている。ＣＰＵ３１は、コンピュータの中で各装置の制御やデータの計算・加工を行なう中枢部分である。ＣＰＵ３１は、ＲＯＭ３３に記憶されたプログラムを実行する装置で、ＲＡＭ３２、ハードディスク３４、または入力部３６からデータを受け取り、演算・加工した上で、表示部３７またはハードディスク３４に出力する。ＲＯＭ３３は、一度書き込まれた情報を読み出すための記憶装置であり、たとえば、システムプログラムなどのプログラムが格納される。ＲＡＭ３３は、半導体素子を利用した記憶装置であり、たとえば、ＣＰＵ３１が実行する処理のためのデータの一時記憶などに使用される。ハードディスク３４は、外部記憶装置である。表示部３５は、たとえば、ＣＲＴディスプレイ、液晶ディスプレイ、プラズマディスプレイなどの表示装置である。入力部３６は、たとえば、キーボード、およびマウスなどのポインティングディバイスである。インターフェイス３７は、二つのものの間に立って、情報のやり取りを仲介するものである。
【００１７】
コンピュータ３０は、移動体システムの処理を実行する各部として、位置算出部３１１、速度算出部３１２、加速度算出部３１３、およびＺＭＰ算出部３１４を有し、ＣＰＵ３１がこれら各部の役割を担う。さらに、コンピュータ３０は、移動体システムの処理を実行する一部として、記憶部３３１を有し、ＲＯＭまたはハードディスクなどの記憶装置がその役割を担う。
【００１８】
位置算出部３１１は、ステレオカメラ２０によって取得した画像データから二足歩行ロボット１の位置情報を算出するものであって、位置算出手段として機能する。
【００１９】
ここで、二足歩行ロボット１の位置を算出する一般的なステレオビジョンシステムの一例を、図３を用いて簡単に説明する。図３は、第１実施形態に係る移動体システムにおける二足歩行ロボット１の距離測定原理を示す説明図である。ステレオカメラ２０Ａ，２０Ｂは、二足歩行ロボット１の進行方向に向けて設置されることにより、路面上の任意の対象物２を含む画像を撮影する。対象物２の画像は、それぞれ座標ｘ１，ｘ２として仮想スクリーン２１Ａ，２１Ｂ上に表示される。対象物２までの距離ｚは、三角測量の原理により、下記式（１）により求めることができる。
【００２０】
【数１】
2009125922.tif 000003
【００２１】
ここで、Ｂはステレオカメラ２０Ａ，２０Ｂの基線長、Ｆはステレオカメラ２０Ａ，２０Ｂから仮想スクリーンまでの距離を示す。これらのパラメータはステレオカメラの設計値を用いることができる。なお、ステレオカメラ２０Ａ，２０Ｂの光軸は互いに平行で、かつ、それぞれの画像面が同一平面状にあるようにしている。
【００２２】
速度算出部３１２は、ステレオカメラ２０によって経時的に取得した画像データに基づいて二足歩行ロボット１の位置情報から二足歩行ロボット１の速度を算出するものであって、速度算出手段として機能する。すなわち、速度算出部３１２は、いわゆるオプティカルフロー法により、時間ｔ１，ｔ２における二足歩行ロボット１の位置をそれぞれｚ１，ｚ２とすれば、その位置の変化量ｚ２−ｚ１を時間の変化量ｔ２−ｔ１で除算することによって、二足歩行ロボット１の速度を算出することができる。また、時間の変化量ｔ２−ｔ１は、たとえば、ステレオカメラ２０のシャッタースピードの間隔（たとえば、ｓ秒）である。
【００２３】
加速度算出部３１３は、速度算出部３１２で算出された二足歩行ロボット１の速度を微分することで、二足歩行ロボット１の加速度を算出するものであって、加速度算出手段として機能する。なお、加速度算出部３１３は、オプティカルフロー法を用いずに、二足歩行ロボット１と任意の対象物２までの距離の時間２階微分値を計算して加速度を算出することもできる。
【００２４】
ＺＭＰ算出部３１４は、経時的に取得した位置情報に基づいて二足歩行ロボット１のＺＭＰを算出するものであって、ＺＭＰ算出手段として機能する。すなわち、ＺＭＰ算出部３１４は、位置情報を経時的に取得することによって、上述したように二足歩行ロボット１の速度および加速度を算出することができるので、ステレオカメラ２０のみで得られた位置情報に基づいてＺＭＰを算出することができる。
【００２５】
記憶部３３１は、ＺＭＰ算出部３１４により算出したＺＭＰのデータを記憶するものであって、記憶手段として機能する。すなわち、記憶部３１４に経時的にＺＭＰを記憶し、表示部３５上にプロットすることで、二足歩行ロボット１のＺＭＰの軌道を視覚的に把握することができる。
【００２６】
ここで、ＺＭＰの算出方法について簡単に説明する。ＺＭＰとは、質量を有する物体にかかる重力と慣性力と床反力とがつりあう点であり、一般的に、その位置は式（３）の運動方程式を解くことにより求めることができる。
【００２７】
【数２】
2009125922.tif 000004
【００２８】
ここで、ｍｉはｉ番目の質点の質量、ｒｉはｉ番目の質点の位置、ｐはＺＭＰの位置、Ｇは重力加速度、Ｍｐはｐにおける床反力モーメントを示す。
【００２９】
しかし、高速な演算が必要な場合は、近似式として、式（４）を用いＺＭＰの位置を算出することが知られている。
【００３０】
【数３】
2009125922.tif 000005
【００３１】
ここで、τＺＭＰはＺＭＰに対する物体のモーメント、Ｍは物体の質量、ｇは重力加速度、ｘは物体の重心の位置、ｐはＺＭＰの位置、ｚｈは物体重心の高さを示す。
【００３２】
式（４）から明らかなように、重心の高さが既知の物体であれば、物体の位置および加速度を検知することによりＺＭＰの位置を算出することができる。
【００３３】
式（１）、式（２）におけるｚをｘに置き換えて式（４）に代入することにより、二足歩行ロボット１のＺＭＰの位置ｐを算出することができる。ここで、式（４）におけるｚｈは二足歩行ロボット１の重心の高さであり、二足歩行ロボット１の設計値を用いることができる。
【００３４】
なお、位置情報に基づいて二足歩行ロボット１の重心を算出する重心算出手段を移動体システムに設けてもよい。重心は、ＺＭＰから慣性力を除いたものであり、その算出方法は、一般的な重心を算出する方法と同様であるので、詳細な説明を省略する。
【００３５】
以上のように構成される本実施形態の移動体システムは、以下のように処理を行う。以下の処理は、ＣＰＵ３１が主として実行する。図４は、本実施形態の移動体システムの処理内容の一例を示すフローチャートである。
【００３６】
図４に示すとおり、本実施形態における移動体のＺＭＰ算出方法は、まず、ステレオカメラ２０に撮像を指示する（ステップＳ１００）。すなわち、図１に示すように、二足歩行ロボット１に取り付けられたステレオカメラ２０（２０Ａ，２０Ｂ）が、二足歩行ロボット１の進行方向に位置する任意の対象物２を撮像する。したがって、同一時間に二台のカメラ２０Ａ，２０Ｂで対象物２を撮像することで、二つの画像データを得ることができる。
【００３７】
次いで、一定の時間をおいて（たとえば、Ｓ秒後）、ステップＳ１００と同様に、ステレオカメラ２０に撮像を指示する（ステップＳ１１０）。なお、一定の時間は、二足歩行ロボット１の歩行するスピードなどの条件に合わせて、最適な時間を設定することができる。
【００３８】
次いで、同一時間に撮像された二つの画像データから同一対象物を探索する（ステップＳ１２０）。二つの画像データから同一対象物を探索する方法は、一般的な画像処理方法により求めることができるので詳細な説明は省略するが、簡単に一例を説明すれば、対象物２上の特徴点Ｐを特定し、それぞれの画像データ上に点Ｐが投影されたとすれば同一対象物と推定する方法がある。
【００３９】
次いで、二足歩行ロボット１から対象物２までの距離を時間ごとに算出する（ステップＳ１３０）。すなわち、ステップＳ１００およびステップＳ１１０における撮像時間をそれぞれｔ１，ｔ２とすれば、時間ｔ１，ｔ２における二足歩行ロボット１から対象物２までの距離ｚ１，ｚ２を算出する。二足歩行ロボット１から対象物２までの距離の測定方法は、上述したように、三角測量の原理により一般的に求めることができる。
【００４０】
ここで、ＺＭＰは通常全体について求めるので、二足歩行ロボット１の重心位置から対象物２までの距離を算出することが望ましい。したがって、本実施形態では、ステレオカメラ２０を二足歩行ロボット１の頭部の位置に設置しているので、二足歩行ロボット１の重心からステレオカメラ２０の設置位置のオフセットを考慮して補正する必要がある。また、ステレオカメラ２０は画像を利用するものであることから振動や衝撃による位置変位からジャイロセンサや歪みセンサを設置し、その位置変位分を、アクチュエータを利用して補正したり、画像処理によって補正する必要がある。図５は、二足歩行ロボット１の頭部に相当する部分を表した例を示す図であって、ステレオカメラ２０（２０Ａ，２０Ｂ）を搭載したカメラユニット４０を、アクチュエータ５０によって、Ｒｏｌｌ回転または／およびＰｉｔｃｈ回転により位置変位を調整することができる。
【００４１】
次いで、距離の差と測定間隔から速度を算出する（ステップＳ１４０）。すなわち、二足歩行ロボット１から対象物２までの位置の変化量ｚ２−ｚ１を時間の変化量ｔ２−ｔ１で除算することによって、二足歩行ロボット１の速度値を算出することができる。
【００４２】
次いで、速度値を時間微分して加速度を計算する（ステップＳ１５０）。
【００４３】
次いで、位置および加速度情報からＺＭＰを算出する（ステップＳ１６０）。すなわち、上記数式（４）により、位置および加速度情報からＺＭＰを算出する。
【００４４】
次いで、算出されたＺＭＰを記憶部３３１記憶し、表示部３５に結果をプロットする（ステップＳ１７０）。図６および図７に表示部３５上にＺＭＰを連続してプロットした場合の一例を示す。図６は、二足歩行ロボット１の下半身のみを三次元的に表した図であり、図７は、二足歩行ロボット１を上部から二次元的に表した図である。図６および図７に示すように、二足歩行ロボット１のＺＭＰの軌道を視覚的に把握することができる。この結果、シミュレーション上のＺＭＰ軌跡と実機の二足歩行ロボット１のＺＭＰ軌跡とを画面上で比較しながら、その二足歩行ロボット１の歩容パターンの生成をすることができる。
【００４５】
次いで、任意の回数Ｎ回に達するまで（ステップＳ１８０：Ｎｏ）、ステップＳ１１０〜ステップＳ１７０の処理を繰り返す。そして、任意の回数Ｎ回に達した場合（ステップＳ１８０：Ｙｅｓ）、移動体システムの処理を終了する。
【００４６】
以上のように、本実施形態の移動体システムによれば、移動体に搭載された複数の撮像手段によって算出された位置情報に基づいてＺＭＰを算出することによって、多くのセンサ系統を不要とすることができる。
＜第２の実施形態＞
次に、本発明の第２実施形態の移動体システムについて詳細に説明する。
【００４７】
第２実施形態の移動体システムは、第１実施形態とは異なり、二足歩行ロボット１の進行する加速度の情報を加速度センサから取得するものである。すなわち、第１実施形態では、ステレオカメラ２０により得られた位置情報から速度を求めて微分することにより、加速度情報を得ているが、微分することにより実際の加速度と誤差を含む場合が多くなる。そこで、加速度情報を取得する加速度センサを二足歩行ロボット１に取り付けることによって、この誤差を含む問題を解決し、より適切な二足歩行ロボット１のＺＭＰを算出することができる。なお、第１実施形態と同様の構成については、上記第１実施形態の説明を参照するものとし、以下では説明を省略する。
【００４８】
加速度センサは、物体の加速度を計測するものであって、その構造自体は一般的な加速度センサと同様であるので、詳しい説明を省略する。なお、加速度センサは、センサの数を減らすために、二足歩行ロボット１の重心に設けることが望ましいが、これに限られず、二足歩行ロボット１の二足歩行ロボット１の各質点にそれぞれ加速度センサを設けることもでき、この場合、重心位置へ等価計算処理を行う必要がある。
【００４９】
本実施形態の移動体システムは、ステレオカメラ２０により得られた位置情報と、加速度センサにより得られた加速度情報から、上記数式（４）によって、二足歩行ロボット１のＺＭＰを算出することができる。
【００５０】
また、第２実施形態の移動体システムの処理は、図４に示すステップＳ１５０の処理が加速度センサから加速度を取得するという処理に置き換わる点が第１実施形態とは異なる。第２実施形態の他の処理は、第１実施形態と同じである。
【００５１】
以上のように、第２実施形態の移動体システムによれば、ステレオカメラ２０により移動体の位置情報をえることで、各質点における位置センサを不要にするとともに、加速度センサによる正確な加速度値を用いることで、正確なＺＭＰを算出することができる。
【００５２】
以上のように本発明の好適な実施形態について説明したが、本発明は、以上の実施形態に限定されるべきものではなく、特許請求の範囲に表現された思想および範囲を逸脱することなく、種々の変形、追加、および省略が当業者によって可能である。
【００５３】
たとえば、本実施形態の移動体システムは、移動体である二足歩行ロボットにステレオカメラを搭載した場合を説明したが、これに限られず、二足歩行ロボットの位置を検出するために、ステレオカメラを移動体から切り離して設置し、任意の対象物と移動体の距離を測定する手法でも可とする。
【００５４】
また、本実施形態のように、移動体は二足歩行ロボットに限定されず、自動車などの車両にも、本発明を応用することができることはもちろんである。
【図面の簡単な説明】
【００５５】
【図１】本実施形態の移動体システムが適用される二足歩行ロボットの一例を示す図である
【図２】本実施形態の移動体システムの概略構成を示すブロック図である。
【図３】第１実施形態に係る移動体システムにおける二足歩行ロボット１の距離測定原理を示す説明図である。
【図４】本実施形態の移動体システムの処理内容の一例を示すフローチャートである。
【図５】二足歩行ロボット１の頭部に相当する部分にステレオカメラ２０を搭載した例を示す図である。
【図６】二足歩行ロボットの歩行時のＺＭＰ軌跡を示す図である。
【図７】二足歩行ロボットの歩行時のＺＭＰ軌跡を示す図である。
【符号の説明】
【００５６】
１    二足歩行ロボット、
２    任意の対象物、
２０    ステレオカメラ、
２１    仮想スクリーン、
３０    コンピュータ、
３１１    位置算出部、
３１２    速度算出部、
３１３    加速度算出部、
３１４    ＺＭＰ算出部、
３３１    記憶部。
【図１】
2009125922.tif 000006
【図２】
2009125922.tif 000007
【図３】
2009125922.tif 000008
【図４】
2009125922.tif 000009
【図５】
2009125922.tif 000010
【図６】
2009125922.tif 000011
【図７】
2009125922.tif 000012