(57)【要約】【課題】移動体の加速時に、加速方向に存在する障害物をより早く検知する移動体の衝突防止システムを提供する。【解決手段】移動体１１０に設置された複数のカメラ１０１により障害物１３０を含む画像をそれぞれ撮影する撮影手段１００と、移動体のＺＭＰ１５０を算出する算出手段１００と、画像における障害物の位置とＺＭＰの位置とを照合する照合手段１００と、を有することを特徴とする。【選択図】図１
2009126499.tif 000002
【特許請求の範囲】
【請求項１】
移動体に設置された複数のカメラにより障害物を含む画像をそれぞれ撮影する撮影手段と、
前記移動体のＺＭＰを算出する算出手段と、
前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、前記画像における障害物の位置と、を照合する照合手段と、
を有することを特徴とする移動体の衝突防止システム。
【請求項２】
前記算出手段は、前記複数のカメラによりそれぞれ経時的に取得した画像データに含まれる前記障害物の位置情報に基づき前記移動体のＺＭＰを算出することを特徴とする請求項１に記載の移動体の衝突防止システム。
【請求項３】
前記算出手段は、
前記障害物の位置情報から算出される、前記障害物の前記移動体に対する相対位置と、
前記相対位置の時間２階微分値と、
から前記移動体のＺＭＰを算出することを特徴とする請求項２に記載の移動体の衝突防止システム。
【請求項４】
複数のカメラにより障害物および移動体の双方を含む画像をそれぞれ撮影する撮影手段と、
前記移動体のＺＭＰを算出する算出手段と、
前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、前記画像における障害物の位置と、を照合する照合手段と、
を有することを特徴とする移動体の衝突防止システム。
【請求項５】
前記算出手段は、前記複数のカメラによりそれぞれ経時的に取得した画像データに含まれる前記障害物および前記移動体の位置情報に基づき前記移動体のＺＭＰを算出することを特徴とする請求項４に記載の移動体の衝突防止システム。
【請求項６】
前記算出手段は、
前記障害物および前記移動体の位置情報から算出される、前記障害物の前記移動体に対する相対位置と、
前記相対位置の時間２階微分値と、
から前記移動体のＺＭＰを算出することを特徴とする請求項５に記載の移動体の衝突防止システム。
【請求項７】
前記照合手段による照合の結果に基づき、前記障害物の位置と前記ＺＭＰの位置および前記対象位置の少なくとも一方との間隔が一定の閾値以下になったときは、衝突を回避するように前記移動体を制御する制御手段、または、前記移動体の操作者を警告する警告手段をさらに有することを特徴とする請求項１〜６のいずれかに記載の移動体の衝突防止システム。
【請求項８】
移動体に設置された複数のカメラにより障害物を含む画像をそれぞれ撮影する撮影段階と、
前記移動体のＺＭＰを算出する算出段階と、
前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、前記画像における障害物の位置と、を照合する照合段階と、
を有することを特徴とする移動体の衝突防止方法。
【請求項９】
複数のカメラにより障害物および移動体の双方を含む画像をそれぞれ撮影する撮影段階と、
前記移動体のＺＭＰを算出する算出段階と、
前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、前記画像における障害物の位置と、を照合する照合段階と、
を有することを特徴とする移動体の衝突防止方法。
【請求項１０】
前記照合手段による照合の結果に基づき、前記障害物の位置と前記ＺＭＰの位置および前記対象位置の少なくとも一方との間隔が一定の閾値以下になったときは、衝突を回避するように前記移動体を制御する制御段階、または、前記移動体の操作者を警告する警告段階をさらに有することを特徴とする請求項８または９のいずれかに記載の移動体の衝突防止方法。
【請求項１１】
前記算出手段は、加速度センサにより前記移動体の加速度情報を検知し、前記移動体の加速度情報を用いて前記移動体のＺＭＰを算出することを特徴とする請求項１、４、７のいずれかに記載の移動体の衝突防止システム。
【請求項１２】
前記算出手段は、前記複数のカメラによりそれぞれ経時的に取得した画像データに含まれる前記障害物の位置情報と、オプティカルフローから求めた加速度情報と、から前記移動体のＺＭＰを算出することを特徴とする請求項１、４、７のいずれかに記載の移動体の衝突防止システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、衝突防止システムに関し、具体的には、移動体のＺＭＰと障害物の位置を照合することにより、移動体が障害物と衝突することを防止する衝突防止システムに関する。
【背景技術】
【０００２】
従来は、カメラにより障害物を認識し、超音波センサにより障害物までの距離を検知し、移動体が障害物と一定の距離以内に近づかないように移動体を制御することにより、移動体と障害物が衝突することを防止していた。
【０００３】
しかし、自動車のように高加速で移動する移動体は、現在障害物が遠距離にあっても、より早く減速の判断をしなければ衝突の危険性が高い。したがって、高加速で移動する移動体にあっては、加速度および加速方向を障害物との衝突回避の判断材料として考慮し、加速方向に存在する障害物をより早く検知する必要がある。
【特許文献１】特開２００１−６３５００号公報
【特許文献２】特許第３８３４６２９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
本発明によれば、移動体の加速時に、加速方向に存在する障害物をより早く検知することを可能とする。
【課題を解決するための手段】
【０００５】
上記課題を解決するために、本発明に係る移動体の衝突防止システムは、移動体に設置された複数のカメラにより障害物を含む画像をそれぞれ撮影する撮影手段と、移動体のＺＭＰを算出する算出手段と、前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、画像における障害物の位置と、を照合する照合手段と、を有する。
【０００６】
算出手段は、複数のカメラによりそれぞれ経時的に取得した画像データに含まれる障害物の位置情報に基づき移動体のＺＭＰを算出しうる。
【０００７】
算出手段は、障害物の位置情報から算出される、障害物の移動体に対する相対位置と、相対位置の時間２階微分値と、から移動体のＺＭＰを算出しうる。
【０００８】
また、他の側面における本発明に係る移動体の衝突防止システムは、複数のカメラにより障害物および移動体の双方を含む画像をそれぞれ撮影する撮影手段と、移動体のＺＭＰを算出する算出手段と、前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、画像における障害物の位置と、を照合する照合手段と、を有する。
【０００９】
算出手段は、複数のカメラによりそれぞれ経時的に取得した画像データに含まれる障害物および移動体の位置情報に基づき移動体のＺＭＰを算出しうる。
【００１０】
算出手段は、障害物および移動体の位置情報から算出される、障害物の移動体に対する相対位置と、相対位置の時間２階微分値と、から移動体のＺＭＰを算出しうる。
【００１１】
照合手段による照合の結果に基づき、障害物の位置とＺＭＰの位置および前記対象位置の少なくとも一方との間隔が一定の閾値以下になったときは、衝突を回避するように移動体を制御する制御手段、または、移動体の操作者を警告する警告手段をさらに有しうる。
【００１２】
本発明に係る移動体の衝突防止方法は、移動体に設置された複数のカメラにより障害物を含む画像をそれぞれ撮影する撮影段階と、移動体のＺＭＰを算出する算出段階と、前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、画像における障害物の位置と、を照合する照合段階と、を有する。
【００１３】
また、他の側面における本発明に係る移動体の衝突防止方法は、複数のカメラにより障害物および移動体の双方を含む画像をそれぞれ撮影する撮影段階と、移動体のＺＭＰを算出する算出段階と、前記ＺＭＰの位置および前記ＺＭＰの位置の前記移動体の重心に対する対称位置の少なくとも一方と、画像における障害物の位置と、を照合する照合段階と、を有する。
【００１４】
照合手段による照合の結果に基づき、障害物の位置とＺＭＰの位置および前記対象位置の少なくとも一方との間隔が一定の閾値以下になったときは、衝突を回避するように前記移動体を制御する制御段階、または、移動体の操作者を警告する警告段階をさらに有しうる。
【００１５】
算出手段は、加速度センサにより移動体の加速度情報を検知し、移動体の加速度情報を用いて移動体のＺＭＰを算出しうる。
【００１６】
算出手段は、複数のカメラによりそれぞれ経時的に取得した画像データに含まれる障害物の位置情報と、オプティカルフローから求めた加速度情報と、から移動体のＺＭＰを算出しうる。
【発明の効果】
【００１７】
本発明に係る衝突防止システムによれば、移動体の加速時に、加速方向に存在する障害物を、より早く検知することができる。
【００１８】
ＺＭＰを複数のカメラの画像データに基づいて算出することにより、障害物の認識と、ＺＭＰの算出を統一的に行うことができるため、小規模のシステムで、高速かつ客観的に障害物を検知することができる。
【００１９】
ＺＭＰを複数のカメラの画像データに基づいて算出することにより、障害物の認識と、ＺＭＰの算出を統一的に行うことができるため、障害物の位置と、ＺＭＰの位置の整合性が高く、高精度に障害物を検知することができる。
【発明を実施するための最良の形態】
【００２０】
以下、図面を用いて、本発明の実施形態に係る移動体の衝突防止システムおよび衝突防止方法について詳細に説明する。
【００２１】
本発明の実施形態に係る衝突防止システムは、障害物を検知するために、障害物との衝突の危険性を判断する上で重要度が高い移動体の加速度を考慮する。すなわち、加速度を用いて算出する移動体のＺＭＰ（Ｚｅｒｏ  Ｍｏｍｅｎｔ  Ｐｏｉｎｔ）を利用し、ＺＭＰと障害物の距離が一定の閾値以下になったときに警告信号を発生させて移動体が障害物と衝突することを防止する。
【００２２】
ここで、ＺＭＰについて説明する。ＺＭＰとは、質量を有する物体にかかる重力と慣性力と床反力とがつりあう点であり、一般的に、その位置は式（１）の運動方程式を解くことにより求めることができる。
【００２３】
【数１】
2009126499.tif 000003
【００２４】
ここで、ｍｉはｉ番目の質点の質量、ｒｉはｉ番目の質点の位置、ｐはＺＭＰの位置、Ｇは重力加速度、Ｍｐはｐにおける床反力モーメントを示す。
【００２５】
しかし、高速な演算が必要な場合は、近似式として、式（２）を用いＺＭＰの位置を算出することが知られている。
【００２６】
【数２】
2009126499.tif 000004
【００２７】
ここで、τＺＭＰはＺＭＰに対する物体のモーメント、Ｍは物体の質量、ｇは重力加速度、ｘは物体の重心の位置、ｐはＺＭＰの位置、ｚｋは物体重心の高さを示す。
【００２８】
式（２）から明らかなように、重心の高さが既知の物体であれば、物体の位置および加速度を検知することによりＺＭＰの位置を算出することができる。
[第１実施形態]
図１は、本発明の第１実施形態に係る移動体の衝突防止システムを示した図である。図２は、本発明の第１実施形態に係る移動体の衝突防止システムのブロック図を示した図である。
【００２９】
図１に示すように、本発明の第１実施形態に係る移動体の衝突防止システム（以下、「衝突防止システム」と称する）１００は、移動体である自動車１１０に設置される。衝突防止システム１００は２つのカメラからなるステレオカメラ１０１を備える。ステレオカメラ１０１は、自動車１１０の進行方向１２０の路面１４０上に存在する障害物１３０を含む画像を撮影できるように進行方向１２０に向けて設置される。
【００３０】
自動車から障害物までの距離と、該距離の時間２階微分とから算出されるＺＭＰ１５０は、自動車１１０の加速状況により自動車に対し前後に移動する。自動車が加速しているときは、ＺＭＰ１５０は路面１４０上で自動車１１０の進行方向１２０後方に位置する。そこで、本実施形態においては、後述するように、ＺＭＰ１５０が自動車１１０の進行方向１２０後方に位置する場合であっても、ＺＭＰ１５０の位置の他、ＺＭＰ１５０の位置の自動車１１０の重心に対する対称位置も利用することにより、より早く障害物を検知することを可能とする。
【００３１】
図２に示すように、本発明の第１実施形態に係る移動体の衝突防止システムは、ステレオカメラ２００と、ＺＭＰの算出など各種計算を行うコンピュータ２０とを備える。コンピュータ２０は、ＣＰＵ２２０、ＣＰＵ２２０にバス結合されたＲＡＭ２４０、ＲＯＭ２５０、ハードディスク２６０、入力部２１０、表示部２３０、インタフェース２７０を含んで構成されうる。
【００３２】
ステレオカメラ２００は、２つのカメラを含んで構成され、撮影手段として機能する。具体的には、ステレオカメラ２００に用いる２つのカメラは、ＣＣＤ素子などの撮像素子とその他の光学系によって構成可能であり、その構成自体は、一般的なカメラと同様であるので、詳しい説明は省略する。
【００３３】
コンピュータ２０は、算出手段および照合手段として機能するほか、制御手段および警告手段としても機能する。ＣＰＵ２２０は、ＲＯＭ２５０に記憶されたプログラムを実行する装置で、ＲＡＭ２４０、ハードディスク２６０、または入力部２１０からデータを受け取り、演算・加工した上で、表示部２３０またはハードディスク２６０に出力する。ステレオカメラ２００により撮影された画像のデータは、インタフェース２７０を介してＲＡＭ２４０に記憶され、ＣＰＵ２２０にて演算処理される。ＣＰＵ２２０は、ＲＡＭ２４０と協働して、ＳＡＤ（Ｓｕｍ  ｏｆ  Ａｂｓｏｌｕｔｅ  Ｄｉｆｆｅｒｅｎｃｅ）やＰＯＣ（Ｐｈａｓｅ  Ｏｎｌｙ  Ｃｏｒｒｅｌａｔｉｏｎ）といった特徴点を検出する一般的な画像処理を行う。このような画像処理により、ステレオカメラ２００が備える２つのカメラにより撮影された画像上の障害物を同一の障害物として認識しうる。ＣＰＵ２２０は、後述するように、自動車と障害物間の距離を計算し、該距離の時間２階微分値を計算することにより、自動車のＺＭＰを算出しうる。また、ＣＰＵ２２０は、障害物と自動車のＺＭＰのデータを照合し、表示部２３０において同一画像内に両者を表示させうるとともに、ＺＭＰおよびＺＭＰの自動車の重心に対する対称位置の少なくとも一方と障害物との距離が閾値以下となったときは、表示部２３０に警告を表示することにより自動車の運転手に対し衝突の危険性を知らせうる。これにより、ＺＭＰが自動車の進行方向に対し後方に位置する場合であっても、ＺＭＰの自動車の重心に対する対称位置（ＺＭＰが自動車の進行方向に対し後方に位置するとき自動車の進行方向に対し前方に位置する）が考慮されるため、加速方向に存在する障害物をより早く検知することができる。
【００３４】
ＲＡＭ２４０は、半導体素子を利用した記憶装置であり、たとえば、ＣＰＵ２２０が実行する処理のためのデータの一時記憶などに使用される。
【００３５】
ハードディスク２６０は、外部記憶装置であり、前記閾値のほか、ステレオカメラで撮影した画像データや、ＣＰＵ２２０による演算結果を必要に応じて記憶させうる。
【００３６】
ＲＯＭ２５０は一度書き込まれた情報を読み出すための記憶装置であり、システムプログラムやアプリケーションプログラムを記憶させうる。
【００３７】
入力部２１０は、たとえば、キーボード、およびマウスなどのポインティングデバイスであり、入力部２１０からは、ステレオカメラの方向を制御するデータや、表示部２３０に警告を表示させるときのＺＭＰと障害物との距離の閾値を設定するデータを入力しうる。
【００３８】
表示部２１０は、たとえば、ＣＲＴディスプレイ、液晶ディスプレイ、プラズマディスプレイなどの表示装置である。
【００３９】
インタフェース２７０は、二つのものの間に立って、情報のやり取りを仲介するものである。
【００４０】
図３は、本発明の第１実施形態に係る衝突防止システムにおける自動車のＺＭＰの算出の原理を示す説明図である。ステレオカメラ３００Ａ、３００Ｂは、自動車に進行方向に向けて設置されることにより、路面上の障害物３２０を含む画像を撮影する。障害物３２０の画像は、それぞれ座標ｘ１、ｘ２として仮想スクリーン３１０Ａ、３１０Ｂ上に表示される。ステレオカメラから障害物までの距離ｚおよびその時間２階微分は、三角測量の原理に基づき、それぞれ下記式（３）および式（４）により求めることができる。
【００４１】
【数３】
2009126499.tif 000005
【００４２】
ここで、Ｂはステレオカメラ３００Ａ、３００Ｂの基線長、Ｆはステレオカメラ３００Ａ、３００Ｂから仮想スクリーンまでの距離を示す。これらのパラメータはステレオカメラの設計値を用いることができる。
【００４３】
式（３）、式（４）におけるｚをｘに置き換えて式（２）に代入することにより、ＺＭＰの位置ｐを算出することができる。ここで、式（２）におけるｚｋは自動車の重心の高さであり、自動車の設計値を用いることができる。
【００４４】
なお、実際は、自動車の重心から障害物までの距離（障害物の移動体に対する相対位置）と、その時間２階微分値を計算してＺＭＰを算出する。ステレオカメラから障害物までの距離ｚが判れば、自動車の設計情報を用いて、自動車から障害物までの距離は容易に計算できる。
【００４５】
このように、本発明の第１実施形態に係る衝突防止システムによれば、ＺＭＰを複数のカメラの画像データに基づいて算出することにより、障害物の認識と、ＺＭＰの算出を統一的に行うことができるため、小規模のシステムで、高速かつ客観的に障害物を検知することができる。
【００４６】
また、ＺＭＰを複数のカメラの画像データに基づいて算出することにより、障害物の認識と、ＺＭＰの算出を統一的に行うことができるため、障害物の位置と、ＺＭＰの位置の整合性が高く、高精度に障害物を検知することができる。
【００４７】
なお、自動車のＺＭＰは、加速度センサまたはオプティカルフローを用いて自動車の加速度を求めることにより算出してもよい。すなわち、加速度センサにより自動車の加速度を検知し、検出された自動車の加速度を用いて自動車のＺＭＰを算出してもよいし、ステレオカメラによりそれぞれ経時的に取得した画像データに含まれる障害物の位置情報と、オプティカルフローから求めた加速度情報と、から自動車のＺＭＰを算出してもよい。
【００４８】
図４は、本発明の第１実施形態に係る移動体の衝突防止方法のフローチャートを示す図である。フローチャートの各ステップの左には、それぞれ対応するステップにおいて、表示部（図２参照）に表示されうる画像を示した。以下、各ステップにつき説明する。
【００４９】
〔Ｓ４００〕
自動車に設置されたステレオカメラにより自動車の進行方向の路面４２０上を継続的に撮影する。これにより、路面４２０上の障害物４００の位置情報を含む画像データを経時的に取得する。
【００５０】
〔Ｓ４１０〕
ステレオカメラにより撮影された画像上の路面４２０および全ての物体を認識する。このとき、図４においては、障害物４００、路面近傍の木４１０Ａ、４１０Ｂを認識している。すなわち、自動車の障害物となりうる物体の候補として、障害物４００、路面近傍の木４１０Ａ、４１０Ｂを認識している。
【００５１】
〔Ｓ４２０〕
ＺＭＰ４３０を算出する。ＺＭＰ４３０はその性質上、床面、すなわち自動車が走行している路面４２０上に存在する。ＺＭＰ４３０は、上述したように、ステレオカメラによって撮影された画像データに基づいて算出されうる。すなわち、画像データに含まれる障害物４００の位置情報に基づき、障害物４００の自動車に対する相対位置と、該相対位置の時間の２階微分値とから自動車のＺＭＰが算出されうる。
【００５２】
〔Ｓ４３０〕
障害物４００の位置とＺＭＰ４３０を照合する。ＺＭＰ４３０はステレオカメラによって撮影された画像上の路面４２０上の点として観念しうる。したがって、路面４２０上の障害物４００の位置とＺＭＰの位置を照合することができる。ここで、障害物４００の位置とＺＭＰ４３０の照合は、ＺＭＰ４３０およびＺＭＰの自動車の重心に対する対称位置の少なくとも一方と照合することを含む。
【００５３】
〔Ｓ４３０〕
ＺＭＰおよびＺＭＰの自動車の重心に対する対称位置の少なくとも一方が障害物４００の閾値範囲内に存在するかどうか判断する。すなわち、障害物４００およびＺＭＰの自動車の重心に対する対称位置の少なくとも一方とＺＭＰとの距離が一定の閾値以下にあるかどうかを判断する。自動車から障害物４００までの距離は、先述したように、ステレオカメラにより、三角測量の原理に基づいて算出できる。閾値は、自動車の通常の運転速度や加速度を考慮して決定されうる。
【００５４】
ここで、注意すべきことは、路面上にない木４１０Ａ、４１０Ｂを「障害物」として誤検出しないようにすることである。路面上にない木４１０Ａ、４１０Ｂも路面上にある障害物４００もＺＭＰからはほぼ等距離にあると考えられるため、何ら対策をしない場合は、路面上にない木４１０Ａ、４１０Ｂも「障害物」として誤検出されうる。このような誤検出を回避するため、本発明の第１実施形態に係る衝突防止システムにおいては、ＺＭＰが障害物の閾値範囲内に存在するかどうか判断するときに、物体がＺＭＰの延長線上にあるかどうかも併せて判断し、物体がＺＭＰの延長線上にないときは、その物体については衝突の危険性が低いため、「障害物」ではないと判断する。物体がＺＭＰの延長線上にあるかどうかは、実際には、自動車の車幅を考慮し、延長線に自動車の車幅に相当する適切な幅を有するものとして判断しうる。
【００５５】
より早く障害物を検知するために、後述する予見制御法に基づいて算出した未来の推定ＺＭＰを用いて障害物を検知してもよい。また、予見制御法のほか、過去のＺＭＰの軌跡を記憶させておき、将来もその軌跡をリピートするものと推定して算出したＺＭＰを用いて障害物を検知してもよい。
【００５６】
〔Ｓ４４０〕
ＺＭＰおよびＺＭＰの自動車の重心に対する対称位置の少なくとも一方が障害物４００の閾値範囲内に存在すると判断したときは警告を出す。警告は、図４に示すように、「Ｗａｒｎｉｎｇ」の表示を、表示部に表示させることによって行ってもよく、警告音を発生させることによってもよい。
【００５７】
一方、本発明の第１実施形態に係る衝突防止システムにおいては、警告に替えて、もしくは警告とともに、衝突を回避するように自動車を制御してもよい。このとき、障害物が存在しない方向へＺＭＰが行くように自動車を制御してもよい。
【００５８】
ここで、前述した予見制御法について説明する。
【００５９】
例えば、２足歩行ロボットの走行パターン生成は、式（２）を離散化して得られる３項方程式を解くことで高速に目標ＺＭＰを実現する計算手法がある。
【００６０】
目標ＺＭＰを出力とし、次のようなシステムとして実現する。
【００６１】
【数４】
2009126499.tif 000006
【００６２】
これをサンプリングタイムＴとして離散化すると、下式（５）のようになる。
【００６３】
【数５】
2009126499.tif 000007
【００６４】
ここで、
【００６５】
【数６】
2009126499.tif 000008
【００６６】
となる。
【００６７】
目標ＺＭＰ軌道ｐｋｒｅｆにできるだけ追従するようにするために、下式（６）の評価関数を最小化する。
【００６８】
【数７】
2009126499.tif 000009
【００６９】
Ｑ、Ｒは適当な正の数とする。
【００７０】
予見制御理論によれば、式（６）を最小化する制御入力は、下式（７）のように与えられる。
【００７１】
【数８】
2009126499.tif 000010
【００７２】
式（７）は、ある瞬間における重心の駆動量を、その瞬間の重心の運動状態のフィードバック（式（７）右辺第１項）とＮステップ先の未来までの目標値ｐｋ＋１ｒｅｆ，・・・，ｐｋ＋Ｎｒｅｆに基づいて決定することを意味する。
【００７３】
また、予見制御に必要なフィードバックゲインは、下式（８）のように計算できる。
【００７４】
【数９】
2009126499.tif 000011
【００７５】
式（８）内のＰはＲｉｃｃａｔｉ方程式の解（下式（９））である。
【００７６】
【数１０】
2009126499.tif 000012
【００７７】
これを移動系の移動制御システムとして用いる場合、例えば図５のようになる。図５は、予見制御法を用いた制御システムの図である。
【００７８】
以上を用いたシステムの場合、システムの入力信号、つまりセンサとしては位置センサと加速度センサ（または位置センサの２階微分）が、特にリアルタイム制御の場合必要となる。これは、２足歩行ロボットの場合よく用いられる手法でＺＭＰから歩容パターンを生成する（逆ＺＭＰ問題）。２足歩行系の場合、これらの演算をリアルタイムで行うことが望ましい。
【００７９】
以上、本発明の第１実施形態に係る衝突防止システムおよび衝突防止方法について説明したが、本発明の第１実施形態に係る衝突防止システムおよび衝突防止方法によれば、移動体の加速時に、加速方向に存在する障害物を、より早く検知することができる。
[第２実施形態]
図６は、本発明の第２実施形態に係る移動体の衝突防止システムを示した図である。本発明の第２実施形態に係る移動体の衝突防止システムは、ロボット６１０に設置され、ロボット６１０が加速して歩行しているときに、より早く路面上の障害物６３０を検知し、障害物６３０との衝突による転倒を防止するものである。本発明の第２実施形態に係る衝突防止システムは、ロボットに設置されることを除き、第１実施形態と同様であるため、第１実施形態における説明と重複する説明は省略する。
【００８０】
図６に示すように、本発明の第２実施形態に係る移動体の衝突防止システム６００は、移動体であるロボット６１０に設置される。衝突防止システム６００は２つのカメラからなるステレオカメラ６０１を備える。ステレオカメラ６０１はロボット６１０の進行方向６２０の路面６４０上に存在する障害物６３０を含む画像を撮影するために進行方向６２０に向けて設置される。ロボット６１０のＺＭＰ６６０は、ロボット６１０の歩行が加速されているときは、路面６４０上でロボット６１０の進行方向６２０前方に位置する。
【００８１】
本発明の第２実施形態に係る移動体の衝突防止システム６００においても、第１実施形態と同様に、ロボット６１０のＺＭＰ６６０と障害物６３０との距離が閾値範囲内に存在するかどうか判断する。そして、ＺＭＰと障害物６３０との距離が閾値範囲内に存在すると判断したときは、障害物６３０が存在しない方向へＺＭＰが行くようにロボット６１０を制御して、ロボット６１０が障害物と衝突することを回避する。
【００８２】
ここで、路面上にない木６５０Ａ、６５０Ｂを「障害物」として誤検出しないようにするために、ＺＭＰと障害物６３０との距離が閾値範囲内に存在するかどうか判断するときに、障害物６３０がＺＭＰの延長線上にあるかどうかも併せて判断し、障害物６３０がＺＭＰの延長線上にないときは、障害物６３０については衝突の危険性が低いため「障害物」ではないと判断する。障害物６３０がＺＭＰの延長線上にあるかどうかは、実際には、延長線がロボット６１０が走行するために必要な幅に相当する適切な幅を有するものとして判断しうる。
【００８３】
以上、本発明の第２実施形態に係る衝突防止システムおよび衝突防止方法について説明したが、本発明の第２実施形態に係る衝突防止システムおよび衝突防止方法は第１実施形態と同様な効果を奏する。
[第３実施形態]
図７は、本発明の第３実施形態に係る衝突防止システムを示した図である。本発明の第３実施形態に係る移動体の衝突防止システム７００Ａ、７００Ｂは、ステレオカメラ７０１を除く一部７００Ａがロボット７１０に設置され、ステレオカメラ７０１を含む他の一部７００Ｂはロボット７１０および障害物７３０の双方を含む画像を撮影しうる場所に設置される。本第３実施形態に係る衝突防止システムは、ロボット７１０が加速して歩行しているときに、より早く路面上の障害物７３０との接近を検知し、障害物７３０との衝突による転倒を防止する。本発明の第３実施形態に係る衝突防止システムは、ステレオカメラ７０１を除く一部７００Ａがロボット７１０に設置され、ステレオカメラ７０１を含む他の一部７００Ｂはロボット７１０および障害物７３０の双方を含む画像を撮影しうる場所に設置される点を除き、第２実施形態と同様であるため、第２実施形態における説明と重複する説明は省略する。
【００８４】
本第３実施形態においては、ロボット７１０から障害物７３０までの距離ｚは、次のように算出する。すなわち、第１実施形態において図３を用いて説明した方法と同様に、ステレオカメラ７０１の２つの仮想スクリーン上に現れた障害物７３０のそれぞれの座標から、三角測量の原理により、ステレオカメラ７０１から障害物７３０までの距離を算出する。そして、同様に、ステレオカメラ７０１の２つの仮想スクリーン上に現れたロボット７１０の重心のそれぞれの座標から、ステレオカメラ７０１からロボット７１０の重心までの距離を算出する。ステレオカメラ７０１から障害物７３０までの距離と、ステレオカメラ７０１からロボット７１０の重心までの距離とからロボット７１０から障害物７３０までの距離ｚを算出する。距離ｚおよび距離ｚの時間２階微分値から式（２）によりロボット７１０のＺＭＰ７６０を算出することができる。
【００８５】
本発明の第３実施形態に係る移動体の衝突防止システムの一部７００Ａは、ロボット７１０に設置され、ＣＰＵ、ＲＡＭ、ＲＯＭ、ハードディスク、入力部、表示部、インタフェース、受信器（図示せず）を含んで構成されうる。また、他の一部６００Ｂは、ステレオカメラ７０１および送信器（図示せず）を含んで構成されうる。ステレオカメラ７０１により撮影された画像データは、衝突防止システムの他の一部７００Ｂを構成する送信器からロボット７１０に設置された衝突防止システムの一部７００Ａを構成する受信器に対し送信される。
【００８６】
以上、本発明の第３実施形態に係る衝突防止システムおよび衝突防止方法について説明したが、本発明の第３実施形態に係る衝突防止システムおよび衝突防止方法は第１実施形態および第２実施形態と同様な効果を奏する。
【００８７】
以上、本願発明を実施形態に基づき説明してきたが、本発明の範囲は実施形態に限定されるものではない。
【００８８】
例えば、上述の実施形態では、衝突防止システムの構成要素である複数のカメラとして、２つのカメラを有するステレオカメラを用いているが、３つ以上のカメラにより複数のカメラを構成してもよい。
【図面の簡単な説明】
【００８９】
【図１】本発明の第１実施形態に係る移動体の衝突防止システムを示した図である。
【図２】本発明の第１実施形態に係る移動体の衝突防止システムのブロック図である。
【図３】本発明の第１実施形態に係る衝突防止システムにおける自動車のＺＭＰの算出の原理を示す説明図である。
【図４】本発明の第１実施形態に係る移動体の衝突防止方法のフローチャートを示す図である。
【図５】予見制御法を用いた制御システムの図である。
【図６】本発明の第２実施形態に係る衝突防止システムを示した図である。
【図７】本発明の第３実施形態に係る衝突防止システムを示した図である。
【符号の説明】
【００９０】
２０    コンピュータ、
１００、６００    衝突防止システム、
１０１、６０１、７０１    ステレオカメラ、
１１０    自動車、
１２０    自動車の進行方向、
１３０、３２０、４００、６３０、７３０    障害物、
１４０、４２０、６４０    路面、
１５０、４３０、６６０、７６０    ＺＭＰ、
３００Ａ、３００Ｂ    カメラ、
３１０Ａ、３１０Ｂ    仮想スクリーン、
４１０Ａ、４１０Ｂ、６５０Ａ、６５０Ｂ    木、
６１０、７１０    ロボット、
６２０    ロボットの進行方向、
７００Ａ    衝突防止システムの一部、
７００Ｂ    衝突防止システムの他の一部。
【図１】
2009126499.tif 000013
【図２】
2009126499.tif 000014
【図３】
2009126499.tif 000015
【図４】
2009126499.tif 000016
【図５】
2009126499.tif 000017
【図６】
2009126499.tif 000018
【図７】
2009126499.tif 000019