移動環境認識装置及び方法
【課題】高速、且つ高精度に測定対象の形状認識情報を得ることができる移動環境認識装置を提供する。
【解決手段】測定対象７に対して円錐走査を行う円錐走査手段１を備えている。円錐走査手段は、移動環境検出器３と、この移動環境検出器で計測した情報を加工する外界認識装置９を備えている。この外界認識装置は、円錐走査手段の測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出手段と、この移動環境検出手段が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断手段と、この面形状判断手段が水平面と判断した所定の測定対象の形状を認識する形状認識手段とを備えている。
【解決手段】測定対象７に対して円錐走査を行う円錐走査手段１を備えている。円錐走査手段は、移動環境検出器３と、この移動環境検出器で計測した情報を加工する外界認識装置９を備えている。この外界認識装置は、円錐走査手段の測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出手段と、この移動環境検出手段が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断手段と、この面形状判断手段が水平面と判断した所定の測定対象の形状を認識する形状認識手段とを備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は移動環境認識装置及び方法に関し、例えば脚型ロボットや脚車輪型ロボットのような脚動作型移動体が移動する際に、必要とする移動環境の有無を認識する場合に適用して好適な、移動ロボット用ビジョンシステムに関するものである。
【背景技術】
【０００２】
脚型ロボットや、脚着地部材として車輪を有する脚車輪型ロボットのような脚動作型移動体は、段差がある移動環境を脚により歩行動作をする際には、例えば階段の踏み板のように、一歩移動するごとに脚によって自立できる場所（これを踏み場と呼ぶ）を踏みしめている必要があり、当該踏み場が存在することを歩行動作を開始する前に認識しておく必要がある。
従来、この種の認識手法として、特許文献１〜４の手法が提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平９−６１１１７号公報
【特許文献２】特開２００２−１４４２７８公報
【特許文献３】特開２００８−２２４３８０公報
【特許文献４】特開２００９−２１２０８６公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１に記載の手法は、円錐状の広がりをもつリング光を対象物に対して斜め方向から照射し、対象物に対して垂直軸上にあるカメラで画像を取得し、画像座標検出手段により対象物までの距離を算出する三次元座標計算手段と、予め画像メモリに記録した画像の一部又は全部との比較によって、距離を測定する三次元位置検出手法である。この手法は、いわゆる三角測量を基本原理としたリング照明を用いたステレオカメラの機能をもっている。
しかしながら、この特許文献１の手法では、平面の傾きを検出することができず、また傾きの変化を検出することもできない問題がある。
【０００５】
また、特許文献２の手法は、腰部に取り付けられた外部センサが、床面までの距離や床面形状の計測を行うために、スリット光を照射する発光手段と、光切断線を撮像する視準値の異なる２つの撮像手段と、光切断方向から撮像された画像を基に計測対象までの距離を算出する手段と、周囲を監視する手段と、を設け、当該２つの撮像手段により同一物体の対応付け及び視差を利用して距離の算出を行う二眼立体視構成を有する。
この特許文献２の手法は、腰部に取り付けられることにより検出感度が向上することや、ロボットの動作中も安定に床面の計測を行うことができるところに特徴があり、基本となる技術は従来技術そのままを用いている点において環境の認識が未だ不十分である。
【０００６】
また、特許文献３の手法は、三次元レーザレーダによって得られた垂直走査面内の距離データのデータ配列から認識対象の垂直方向及び水平方向の特徴を抽出するようにしたことにより、脚動作型移動体が認識対象を歩行できるような移動環境情報を得るようになされているが、レーザの走査による切断面の特徴量から三次元形状を再構築するために、処理すべき情報量が多く、また手順も複雑であるため、これらを軽減することが望ましい。
さらに、特許文献４の手法は、円錐形状をした三次元距離測定器を用いて測定範囲の形状を認識し、位置姿勢を算出する手法であるが、測定する物体の形状を認識することが難しい。
本発明は上記事情に鑑みてなされたものであり、高速、且つ高精度に測定対象の形状認識情報を得ることができる移動環境認識装置及び方法を提案するものである。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、本発明に係る請求項１記載の移動環境認識装置は、測定対象に対して円錐走査を行う円錐走査手段と、この円錐走査手段の測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出手段と、この移動環境検出手段が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断手段と、この面形状判断手段が水平面と判断した所定の測定対象の形状を認識する形状認識手段と、を備えている。
【０００８】
また、請求項２記載の発明は、請求項１記載の移動環境認識装置において、前記円錐走査手段は、多数の測定セグメントを同時に検出する距離画像センサを備えており、当該距離画像センサが検出した各測定セグメントに対して円錐走査を行なうようにした。
また、請求項３記載の発明は、請求項１又は２記載の移動環境認識装置において、形状認識手段は、前記面形状判断手段で水平面と判断した前記所定の測定対象の連結成分抽出処理を行い、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出する。
【０００９】
さらに、請求項４記載の発明は、請求項１乃至３の何れか１項に記載の移動環境認識装置において、前記円錐走査手段の傾斜角を測定可能な姿勢センサと、前記円錐走査手段の測定データに、前記姿勢センサが測定した前記傾斜角に基づいた座標変換を行なうことで、前記移動環境検出手段が算出した前記測定セグメントの測定データを水平面基準に補正する測定データ補正手段とを備えている。
【００１０】
一方、請求項５記載の移動環境認識方法は、測定対象に対して円錐走査を行う円錐走査工程と、この円錐走査工程で測定した測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出工程と、この移動環境検出工程が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断工程と、この面形状判断工程で水平面と判断した所定の測定対象の形状を認識する形状認識工程とを備えている。
【００１１】
また、請求項６記載の発明は、請求項５記載の移動環境認識方法において、前記円錐走査工程は、多数の測定セグメントを同時に検出し、これら各測定セグメントに対して円錐走査を行なう。
さらに、請求項７記載の発明は、請求項５又は６記載の移動環境認識方法において、形状認識工程は、前記面形状判断工程で水平面と判断した前記所定の測定対象の連結成分抽出処理を行う工程と、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出する工程とを備えている。
【発明の効果】
【００１２】
本発明に係る移動環境認識装置及び方法によれば、測定対象に対して円錐走査を行うことで、測定距離及び円錐走査回転角度に対応するデータ、傾斜方向、センサ原点から測定対象中心までの距離、傾斜角を蓄積し、水平面形状判定を行なうことで得られた水平面の測定対象の形状を算出することができるので、高精度に測定対象の形状認識情報を得ることができる。
【図面の簡単な説明】
【００１３】
【図１】移動体の構成を示す概略図である。
【図２】外界認識装置を示すブロック図である。
【図３】移動環境検出器の構成を示す図である。
【図４】距離測定器を用いた円錐走査法を示す略線図である。
【図５】円錐走査法を示す別の略線図である。
【図６】円錐走査法を示す略線図である。
【図７】距離画像センサを用いる場合の多数の測定対象（セグメント）を示す図である。
【図８】円錐走査法を示す略線図である。
【図９】階段の凸部を測定した場合のα−Ｌ線図を示すものである。
【図１０】階段の凹部を測定した場合のα−Ｌ線図を示すものである。
【図１１】階段の踏み面を測定した場合のα−Ｌ線図を示すものである。
【図１２】階段の垂直面を測定した場合のα−Ｌ線図を示すものである。
【図１３】測定対象である階段の認識情報部位を示す図である。
【図１４】測定対象の認識処理を示すフローチャートである。
【図１５】ロボット中心地面の投影座標系を示す図である。
【図１６】センサ座標系変換図である。
【図１７】移動環境検出器に姿勢センサを装着した状態を示す図である。
【図１８】移動体本体に姿勢センサを装着した状態を示す図である。
【発明を実施するための形態】
【００１４】
以下、本発明を実施するための形態（以下、実施形態という。）を、図面を参照しながら詳細に説明する。
（１）脚動作型移動体の構成と動作
図１において、符号１は全体として移動体を示し、直方体形状の移動体本体２の前面上端中央部に前方に突出するように移動環境検出器３が設けられている。
移動体本体２の前面下部の左右両端位置には左前脚４Ｌ及び右前脚４Ｒが装着されていると共に、移動体本体２の後面下部の左右両端位置には左後脚５Ｌ及び右後脚５Ｒが装着されている。
左前脚４Ｌ及び右前脚４Ｒ並びに左後脚５Ｌ及び右後脚５Ｒは、それぞれ支軸周りの関節Ｊ０、股関節Ｊ１及び膝関節Ｊ２を介して脚車輪Ｊ３を歩行動作ができるように保持している。
【００１５】
そして、左前脚４Ｌ及び右前脚４Ｒ並びに左後脚５Ｌ及び右後脚５Ｒが平らな床面６上に自立しているときには、脚車輪Ｊ３を回転駆動することにより、移動体本体２を車輪の回転動作を利用して移動できるようになされていると共に、移動体本体２が測定対象としての階段７を上り下りする際には、左前脚４Ｌ若しくは右前脚４Ｒ又は左後脚５Ｌ若しくは右後脚５Ｒのいずれか１つ、または２つ以上を階段７の段差を上り下りするように脚車輪Ｊ３を回転駆動させずに、もしくは階段７の上の段の存在する側へ進むよう回転駆動しながら歩行動作させ、これにより脚車輪Ｊ３を階段７の踏み面８を踏む手段として用いて階段７を１歩ずつ上ったり下ったりできるようになされている。
このような移動体１の移動動作は、移動体本体２内に設けられている外界認識装置９が移動環境検出器３の検出結果を用いて移動環境を認識して左前脚４Ｌ若しくは右前脚４Ｒ又は左後脚５Ｌ若しくは右後脚５Ｒを駆動制御することによって行われる。
【００１６】
外界認識装置９は、図２に示すように、移動環境検出器３として円錐走査式距離測定部３１あるいは距離画像測定部３６を装備し、情報処理を行うパーソナルコンピュータによって構成され、データ入出力手段１１からユーザが入力した指令データをバス１２を介してシステム制御用中央処理ユニット（ＣＰＵ）１３が受け取ったとき、これに応じてＣＰＵ１３がプログラムＲＯＭ・動作メモリＲＡＭ１４を用いて移動環境検出器３の検出結果に応じた環境認識演算処理を実行し、データ入出力手段１１を通じて本体駆動部１５に当該環境認識結果を通知することにより、左前脚４Ｌ若しくは右前脚４Ｒ又は左後脚５Ｌ若しくは右後脚５Ｒを移動動作させる。
【００１７】
（２）円錐走査による測定
円錐走査式距離測定部３１は、図３に示すように、水平走査機構２１を有し、この水平走査機構２１が移動体本体２の前面から前方に突出する突出支持部２２（図１参照）に支持されており、これにより水平走査機構２１が垂直方向の中心軸ｚを中心として水平ターンテーブル２３を矢印ａで示すように水平面内において回動動作させる。
水平ターンテーブル２３の下面には、取付部材２４を介して垂直走査機構２５が取り付けられており、これにより垂直走査機構２５が垂直ターンテーブル２６を、矢印ｂで示すように、一方の水平軸ｙを中心として垂直面内において回動動作させる。
【００１８】
垂直ターンテーブル２６には距離測定器２７が取り付けられ、これにより距離測定器２７の距離測定方向が他方の水平軸ｘを基準にして垂直面内（従って仰角が変更する方向）において変更制御される。
この実施形態の場合、距離測定器２７は、赤外線パルスビームを所定の測定方向に放出すると共に、当該赤外線パルスビームが測定対象において反射して得られる戻り光を受光し、当該赤外線パルスビームの発射時刻と戻り光の到達時刻との時間差に基づいて、距離測定器２７から測定対象までの距離Ｌを測定する。
【００１９】
また、距離測定器２７は、赤外線ビームを所定の測定方向に放出すると共に、当該赤外線ビームが測定対象において反射して得られる戻り光を受光し、当該赤外線ビームと戻り光との三角形状を利用して三角測量の原理に基づいて、距離測定器２７から測定対象までの距離Ｌを測定するようにしてもよい。
この実施形態の場合、水平ターンテーブル２３に取り付けられた取付部材２４、垂直走査機構２５、垂直ターンテーブル２６及び距離測定器２７は保護カバー２８によって覆われている。
【００２０】
当該距離測定器２７に対する水平走査機構２１及び垂直走査機構２５の走査は、外界認識装置９（図２参照）のデータ入出力手段１１から入力されるユーザの動作命令によって、距離測定器２７から放出される円錐走査検出光ＤＥＴ（図４参照）の放出方向（これを視準方向と呼ぶ）を、外界認識装置９（図２参照）のＣＰＵ１３が制御することにより行われる。
【００２１】
そして、移動環境検出器３は、水平面内のｘｙ座標を順次指定して行くことにより距離測定器２７を水平面内のすべての角度方向に走査できると共に、当該水平面内のすべての角度位置について、垂直面内の角度位置を走査することができ、これにより移動体１が移動すべき環境に存在する認識対象と距離測定器２７との間の距離を、図４に示す円錐走査式距離測定法によって、測定することができる。
【００２２】
図４において、距離測定器２７が配設されている位置Ｏを原点として（これをセンサ原点と呼ぶ）、距離測定器２７の視準方向をＺ軸で表わすと共に、当該視準方向軸Ｚと直交する平面の座標を直交座標軸Ｘ及びＹによって表わす。
図４の場合、視準方向軸Ｚのセンサ原点Ｏから基準距離ｄＳＴの位置に、視準方向軸Ｚ（以下これを視準方向と呼ぶ）を中心として描いた半径ｒＳＴの円形平面を水平垂直走査面Ｓ１として定義し、センサ原点Ｏからの赤外線パルス検出ビームを水平垂直走査面Ｓ１の円形軌道に沿うように走査させることにより、センサ原点Ｏを頂点としかつ水平垂直走査面Ｓ１を円錐底面とする円錐走査立体を想定する。
【００２３】
このセンサ原点Ｏを頂点としかつ水平垂直走査面Ｓ１を円錐底面とする円錐体については、図５（Ａ）に示すように、ＸＺ平面についてセンサ原点Ｏから水平垂直走査面Ｓ１の中心位置ＣＳＴ（水平垂直走査面Ｓ１とＺ軸との交点位置）から半径ｒＳＴだけＸ軸と平行に移動した点と、センサ原点Ｏとを結ぶ線は、センサ原点Ｏから発射された検出パルス光Ｌｐ（すなわち赤外線パルスビーム）が通る円錐面を表している。
従って、当該円錐面の円錐底面の半径ｒＳＴは、円錐半頂角βと、センサ原点Ｏから水平垂直走査面Ｓ１までの基準距離ｄＳＴとから、次式
【００２４】
【数１】
によって表すことができる。
【００２５】
水平垂直走査面Ｓ１の各走査位置について、水平垂直走査面Ｓ１の直交座標軸ＸＹについて、Ｘ軸と平行なｘ軸との交点を起点として、円錐走査回転角度α（＝０〜３６０〔°〕）によって円錐底面の外周面に沿った走査点Ｐ１を順次走査すると考えると、この走査点Ｐ１を通る検出パルス光は水平垂直走査面Ｓ１より前方位置にある測定対象面Ｓ２の測定点Ｐ２に到達するから、測定対象面Ｓ２上に水平垂直走査面Ｓ１の走査点Ｐ１の走査に対応して移動する測定点Ｐ２が考えられる。
【００２６】
ここで走査対象面Ｓ２は、測定対象の外観形状が任意であれば、視準方向軸Ｚに対して直交する面をもつとは限らず、傾きをもっているものもあるが、直交するものと考えると、測定対象面Ｓ２は水平垂直走査面Ｓ１と相似の円を描くことになる。
かくして水平垂直走査面Ｓ１上の走査点Ｐ１のｘｙ平面のｘ座標点Ｐ１ｘは、図５（Ｂ）に示すように、水平走査角θによって、次式
【００２７】
【数２】
によって表すことができると共に、ｙｚ平面のｙ座標点Ｐ１ｙは、図５（Ｂ）に示すように、垂直走査角φによって
【００２８】
【数３】
によって表すことができる。
水平垂直走査面Ｓ１上の走査点Ｐ１のｘ軸上の座標ｘは、図５（Ｃ）に示すように、円錐走査回転角度αが時間の経過と共にωｔで変化すると考えれば、次式、
【００２９】
【数４】
のように表すことができると共に、ｙ軸上の座標ｙは次式、
【００３０】
【数５】
のように表すことができる。
【００３１】
以上の動作原理に基づいて、外界認識装置９（図２参照）のＣＰＵ１３は、円錐走査式距離測定部３１を起動することにより、円錐走査式距離測定部３１の円錐走査制御部３２によって水平垂直走査面Ｓ１上の走査点座標ｘ及びｙを、（４）式及び（５）式に基づいて、円錐走査回転角度αを角速度ωで変更制御することにより、センサ原点Ｏにある距離測定器２７から放出されるパルスビームを円錐底面の円軌道に沿って走査するために必要な水平走査角θ及び垂直走査角φを表す駆動角度データを生成する。
【００３２】
この水平走査角θ及び垂直走査角φの駆動角度データは水平垂直走査部３３に与えられ、当該水平垂直走査部３３が移動環境検出器３に対して、水平走査機構２１及び垂直走査機構２５の駆動出力を送出する。
これにより測定対象面Ｓ２上にパルスビームがα＝０〔°〕からα＝３６０〔°〕まで走査するように投射される。
【００３３】
ここで、測定対象面Ｓ２が、図６に示すように、視準方向Ｚの軸に対して直交する平面を有するとすれば、距離測定器２７から放出された検出パルス光Ｌｐは走査点Ｐ１を通って測定点Ｐ２に当たって反射される。
この結果、測定点Ｐ２は、測定対象面Ｓ２上に、水平垂直走査面Ｓ１と相似の円形の交円軌跡ＴＲ（以下これを交円と呼ぶ）を描き、これに対して測定対象面Ｓ２が視準方向Ｚの軸に対して直交せずに傾いているときは楕円形の交円ＴＲを描く。
【００３４】
かくして、測定対象面Ｓ２の交円ＴＲから反射された検出パルス光Ｌｐが移動環境検出器３の距離測定器２７に戻ってきたとき、その検出データがバス１２を介して円錐走査式距離測定部３１の距離検出部３４に取り込まれてセンサ原点Ｏから測定点Ｐ２までの距離を検出できることになり、当該測定距離Ｌが、円錐走査回転角度α（＝０〜３６０〔°〕）に対応するデータ（α、Ｌ）として、円錐走査距離データメモリ３５Ａに蓄積される。
このデータ（α、Ｌ）は、円錐走査回転角度αと測定距離Ｌの関係が得られるα−Ｌ線図と呼ばれ、測定距離の最大値Ｌｍａｘ及び最大値Ｌｍａｘのときの円錐走査回転角度αから傾斜方向α（Ｌｍａｘ）を決定することができる。
【００３５】
一方、距離画像測定部３６は、多点の距離を同時に高速に測定することが可能な距離画像センサを使用しており、距離画像センサの測定領域内において、複数の円形セグメント上の距離情報に対応する画素位置のデータとして距離画像データメモリ３６Ｂに蓄積される。
すなわち、図７に示すように、円形セグメントの個数をｍ（ｘ方向），ｎ（ｙ方向），間隔をΔx，Δｙ，センサの横方向ピクセル数ｗｃ，センサの縦方向ピクセル数ｈｃとして任意のｉ番，ｊ番目の円の中心点は、
【００３６】
【数６】
で表され、この点を中心に、円錐走査角αに応じた各抽出点を算出する。そして、距離画像センサからは各画素における測定距離Ｌが得られるので、円錐走査回転角度αが０〜３６０°回転する場合に相当する距離画像上の座標を算出し、その座標に対応する画素位置の測定距離Ｌを取得することで、データ（α、Ｌ）が得られる。なお、画素の中間位置については、バイリニア法による測定データ平滑補間処理を行う。
【００３７】
なお、円錐走査距離データメモリ３５Ａの複数領域の走査データ結果を距離画像データメモリ３６Ｂに蓄積することで、以降の処理を同様の手順で行なうことができる。
このデータ（α、Ｌ）もα−Ｌ線図と呼ばれ、測定距離の最大値Ｌｍａｘ及び最大値Ｌｍａｘのときの円錐走査回転角度αから傾斜方向α（Ｌｍａｘ）を決定することができる。
【００３８】
そして、距離画像データメモリ３６Ｂに蓄積されたα−Ｌ線図は、傾斜方向α（Ｌｍａｘ）の幾何学的な位置関係と関係式から、センサ原点から円形セグメント中心までの距離ｄｚ、傾斜角γを算出する。
すなわち、図８のｚは、センサ原点Ｏから視準方向Ｚへとった距離を表し、主軸ｍは、ｚに対して直交する方向の距離を表す。
【００３９】
また、βは円錐走査回転角度αによって形成される円錐走査検出光ＤＥＴの半頂角で、センサ原点Ｏから極小点ＭＮまでの距離ｄｍｉｎは、次式、
【００４０】
【数７】
によって求められると共に、センサ原点Ｏから極大点ＭＸまでの距離ｄｍａｘは、次式、
【００４１】
【数８】
によって求めることができ、その差分ｈは、次式、
【００４２】
【数９】
になる。
ここで、極大点ＭＸと極小点ＭＮとを結ぶ線は、平面上の成分を表しており、差分ｈは当該平面の傾斜の高さを表している。
【００４３】
一方、極小点ＭＮ及び極大点ＭＸからそれぞれｚ軸に下ろした脚の長さｒｍｉｎ及びｒｍａｘは、
【００４４】
【数１０】
【００４５】
【数１１】
のように、半頂角βによって表すことができるから、両者の和ｋは、
【００４６】
【数１２】
になる。
この結果、（９）式の差分ｈとの間には、
【００４７】
【数１３】
の関係がある。
【００４８】
（１３）式のγは、極大点ＭＸと極小点ＭＮとを結ぶ傾斜面（すなわち測定対象である平面）の、円錐走査検出光ＤＥＴの円錐底面からの傾斜角を表しており、当該傾斜角γを（１３）式から求めることができる。
かくして求めた傾斜角γによって、センサ原点から円形セグメント中心までの距離ｄｚは、次式、
【００４９】
【数１４】
【００５０】
【数１５】
によって求めることができる。
したがって、距離画像データメモリ３６Ｂには、多数の円形セグメントのα−Ｌ線図、傾斜方向α（Ｌｍａｘ）、センサ原点から円形セグメント中心までの距離ｄｚ、傾斜角γが蓄積される。
【００５１】
（３）測定対象（階段）の水平面形状判定機能
また、ＣＰＵ１３は、距離画像データメモリ３６Ｂに蓄積されたデータに基づいて測定対象の水平面形状判定を行なう。
図９から図１２の（ａ）は、移動環境検出器３を斜め下向きに配置し、測定対象である図１の階段７の凹凸部、平面部を示したものである。これらの図の（ｂ）で示すα−Ｌ線図の実線は計測値である。また、測定対象が１つの平面であれば、α−Ｌ線図が一次の三角関数で表現できることに着目し、一次のフーリエ級数計算を行い、以下の（１６）式の通りフィッティング値を算出することとした。なお、α−Ｌ線図の破線がフィッティング値であり、傾斜方向α(Ｌｍａｘ)は、フィッティング値から求めている。
【００５２】
【数１６】
【００５３】
【数１７】
【００５４】
図９の階段７の凸部を示すα−Ｌ線図は、水平面（踏み面Ｆ）及び垂直面（蹴込面Ｋ）の両者が存在するので１次曲線にはならずフィッティング値と測定値との残差が大きい。また、図１０の階段７の凹部を示すα−Ｌ線図も、水平面（床面６）及び垂直面（蹴込面Ｋ）が存在するので１次曲線にはならずフィッティング値と測定値との残差が大きい。
これに対して、図１１の水平面（踏み面Ｆ）を示すα−Ｌ線図と、図１２の垂直面（蹴込面Ｋ）を示すα−Ｌ線図の両者は、範囲内に１つの面のみが存在し、１次曲線近くなり、フィッティング値と測定値との残差が小さい。そして、フィッティング値と測定値との残差が小さく、平面の傾斜方向α(Ｌｍａｘ)の値が大きい図１１のα−Ｌ線図を、水平面（踏み面Ｆ）と判定（水平面判定）することができる。
【００５５】
なお、残差の評価値としては、以下の（１８）式を用いる。
【００５６】
【数１８】
【００５７】
（４）測定対象（階段）の認識処理手順
測定対象である階段の認識情報は、図１３に示すように階段の位置（Ｓｘ，Ｓｙ），方向γ，階段幅Ｓｗである。なお、階段幅Ｓｗは測定レンジ内の有効幅とし、階段の位置は有効幅の中心点とする。
ＣＰＵ１３は、図１４のフローチャートで示す手順で階段の認識処理を行なう。
図１４の階段の認識処理は、先ず、ステップＳＴ１において、上述した水平面形状判定機能で示したように距離画像データメモリ３６Ｂのデータから水平面判定を受けた円形セグメントを読込み、そのセグメント（円形セグメント）の中心座標（ｘＣ,yＣ）をプロットした画像を作成する。
【００５８】
次いでステップＳＴ２に移行し、作成した画像に対して連結成分抽出処理（ラベリング）を実行し、単なる点の集合であった画像から、複数のエリアが抽出される。
次いでステップＳＴ３に移行し、以下の（１９）式〜（２２）式を用いてセグメントの中心座標（ｘＣ,yＣ）からロボット座標系のＸ座標、Ｙ座標、Ｚ座標を算出する（図１５のロボット中心地面投影座標系及び図１６のセンサ座標系変換図を参照）。
【００５９】
【数１９】
【００６０】
次いでステップＳＴ４に移行し、抽出エリア毎に高さＺ座標の平均値Ｚａｖｅと、高さＺ座標のばらつき（最大値Ｚｍａｘと最小値Ｚｍｉｎの差で評価）を算出する。
次いでステップＳＴ５に移行し、所定の抽出エリアの高さＺ座標の平均値Ｚａｖｅが階段の一段目高さであり（第１の条件）、且つ、高さＺ座標のばらつき（Ｚｍａｘ − Ｚｍｉｎ ）が十分に小さい場合には（第２の条件）、ステップＳＴ６に移行し、上記の第１及び第２の条件の少なくとも一方を満たしていない場合には、ステップＳＴ７に移行し、次の抽出エリアを読み込む。ステップＳＴ７からステップＳＴ５に移行し、上述した判定処理を行なう。
ステップＳＴ６では、高さＺ座標の平均値Ｚａｖｅが階段の一段目高さであり、且つ、高さＺ座標のばらつき（Ｚｍａｘ − Ｚｍｉｎ ）が十分に小さい抽出エリアに対して最小自乗法による直線フィッティングを行う。
【００６１】
次いでステップＳＴ８に移行し、近似直線上の左右端点（ｘｌｅｆｔ，ｙｌｅｆｔ）,（ｘrｉｇｈｔ，ｙrｉｇｈｔ）を抽出する。
次いでステップＳＴ９に移行し、ロボット座標系の左右端点（Ｘｌｅｆｔ，Ｙｌｅｆｔ，Ｚｌｅｆｔ）,（Ｘrｉｇｈｔ，Ｙrｉｇｈｔ，Ｚrｉｇｈｔ）に変換する。
次いでステップＳＴ１０に移行し、変換されたロボット座標系の左右端点（Ｘｌｅｆｔ，Ｙｌｅｆｔ，Ｚｌｅｆｔ）,（Ｘrｉｇｈｔ，Ｙrｉｇｈｔ，Ｚrｉｇｈｔ）から、以下の（２７）式〜（３０）式を用いて階段の位置（Ｓｘ，Ｓｙ），方向γ，階段幅Ｓｗを算出する。
【００６２】
【数２０】
【００６３】
なお、ステップＳＴ６において階段の２段目以降に該当する高さを条件に設定し同様に処理し、複数の段についての段の方向、位置、幅の情報を取得することにより、階段の段数についての認識や、階段が続いていく方向の認識も可能となる。
上記構成の外界認識装置９によると、移動環境検出器３が測定対象に対して円錐走査を行うことで、測定距離Ｌ及び円錐走査回転角度α（＝０〜３６０〔°〕）に対応するデータ（α、Ｌ）（α−Ｌ線図）、傾斜方向α（Ｌｍａｘ）、センサ原点から円形セグメント中心までの距離ｄｚ、傾斜角γを蓄積し、測定対象（階段）水平面形状判定機能を行なうことで、フィッティング値と測定値との残差が小さく平面の傾斜方向α(Ｌｍａｘ)の値が大きいデータを水平面と判定し、測定対象（階段）の認識処理を行なうことで、水平面と判断した測定対象の連結成分抽出処理を行い、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記測定対象の形状を算出することができるので、高精度に測定対象の形状認識情報を得ることができる。
【００６４】
また、多点の距離を同時に高速に測定することが可能な距離画像センサを使用し、この距離画像センサの測定領域内において、複数の円形セグメント上の距離情報に対応する画素位置のデータとして、測定距離Ｌ及び円錐走査回転角度αに対応するデータ（α、Ｌ）を蓄積すると、高速、且つ高精度に測定対象の形状認識情報を得ることができる。
なお、図１７及び図１８に示すように、移動体本体２に装着された移動環境検出器３の傾斜角を測定することが可能な姿勢センサ５０を、移動環境検出器３、或いは移動体本体２に備え、図２に示す外界認識装置９に備えた測定データ補正部５１が、移動環境検出器３が測定した測定データに対して姿勢センサ５０が測定した傾斜角に基づいて座標変換を行なうようにすると、さらに高精度に測定対象の形状認識情報を得ることができる。
【符号の説明】
【００６５】
１…移動体、２…移動体本体、３…移動環境検出器、４Ｌ…左前脚、４Ｒ…右前脚、５Ｌ…左後脚、５Ｒ…右後脚、６…床面、７…階段、８，Ｆ…踏み面、９…外界認識装置、１１…データ入出力手段、１２…バス、１５…本体駆動部、２１…水平走査機構、２２…突出支持部、２３…水平ターンテーブル、２４…取付部材、２５…垂直走査機構、２６…垂直ターンテーブル、２７…距離測定器、２８…保護カバー、３１…円錐走査式距離測定部、３２…円錐走査制御部、３３…水平垂直走査部、３４…距離検出部、３５Ａ…円錐走査距離データメモリ、３６…距離画像測定部、３６Ｂ…距離画像データメモリ、５０…姿勢センサ、５１…測定データ補正部
【技術分野】
【０００１】
本発明は移動環境認識装置及び方法に関し、例えば脚型ロボットや脚車輪型ロボットのような脚動作型移動体が移動する際に、必要とする移動環境の有無を認識する場合に適用して好適な、移動ロボット用ビジョンシステムに関するものである。
【背景技術】
【０００２】
脚型ロボットや、脚着地部材として車輪を有する脚車輪型ロボットのような脚動作型移動体は、段差がある移動環境を脚により歩行動作をする際には、例えば階段の踏み板のように、一歩移動するごとに脚によって自立できる場所（これを踏み場と呼ぶ）を踏みしめている必要があり、当該踏み場が存在することを歩行動作を開始する前に認識しておく必要がある。
従来、この種の認識手法として、特許文献１〜４の手法が提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平９−６１１１７号公報
【特許文献２】特開２００２−１４４２７８公報
【特許文献３】特開２００８−２２４３８０公報
【特許文献４】特開２００９−２１２０８６公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１に記載の手法は、円錐状の広がりをもつリング光を対象物に対して斜め方向から照射し、対象物に対して垂直軸上にあるカメラで画像を取得し、画像座標検出手段により対象物までの距離を算出する三次元座標計算手段と、予め画像メモリに記録した画像の一部又は全部との比較によって、距離を測定する三次元位置検出手法である。この手法は、いわゆる三角測量を基本原理としたリング照明を用いたステレオカメラの機能をもっている。
しかしながら、この特許文献１の手法では、平面の傾きを検出することができず、また傾きの変化を検出することもできない問題がある。
【０００５】
また、特許文献２の手法は、腰部に取り付けられた外部センサが、床面までの距離や床面形状の計測を行うために、スリット光を照射する発光手段と、光切断線を撮像する視準値の異なる２つの撮像手段と、光切断方向から撮像された画像を基に計測対象までの距離を算出する手段と、周囲を監視する手段と、を設け、当該２つの撮像手段により同一物体の対応付け及び視差を利用して距離の算出を行う二眼立体視構成を有する。
この特許文献２の手法は、腰部に取り付けられることにより検出感度が向上することや、ロボットの動作中も安定に床面の計測を行うことができるところに特徴があり、基本となる技術は従来技術そのままを用いている点において環境の認識が未だ不十分である。
【０００６】
また、特許文献３の手法は、三次元レーザレーダによって得られた垂直走査面内の距離データのデータ配列から認識対象の垂直方向及び水平方向の特徴を抽出するようにしたことにより、脚動作型移動体が認識対象を歩行できるような移動環境情報を得るようになされているが、レーザの走査による切断面の特徴量から三次元形状を再構築するために、処理すべき情報量が多く、また手順も複雑であるため、これらを軽減することが望ましい。
さらに、特許文献４の手法は、円錐形状をした三次元距離測定器を用いて測定範囲の形状を認識し、位置姿勢を算出する手法であるが、測定する物体の形状を認識することが難しい。
本発明は上記事情に鑑みてなされたものであり、高速、且つ高精度に測定対象の形状認識情報を得ることができる移動環境認識装置及び方法を提案するものである。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、本発明に係る請求項１記載の移動環境認識装置は、測定対象に対して円錐走査を行う円錐走査手段と、この円錐走査手段の測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出手段と、この移動環境検出手段が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断手段と、この面形状判断手段が水平面と判断した所定の測定対象の形状を認識する形状認識手段と、を備えている。
【０００８】
また、請求項２記載の発明は、請求項１記載の移動環境認識装置において、前記円錐走査手段は、多数の測定セグメントを同時に検出する距離画像センサを備えており、当該距離画像センサが検出した各測定セグメントに対して円錐走査を行なうようにした。
また、請求項３記載の発明は、請求項１又は２記載の移動環境認識装置において、形状認識手段は、前記面形状判断手段で水平面と判断した前記所定の測定対象の連結成分抽出処理を行い、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出する。
【０００９】
さらに、請求項４記載の発明は、請求項１乃至３の何れか１項に記載の移動環境認識装置において、前記円錐走査手段の傾斜角を測定可能な姿勢センサと、前記円錐走査手段の測定データに、前記姿勢センサが測定した前記傾斜角に基づいた座標変換を行なうことで、前記移動環境検出手段が算出した前記測定セグメントの測定データを水平面基準に補正する測定データ補正手段とを備えている。
【００１０】
一方、請求項５記載の移動環境認識方法は、測定対象に対して円錐走査を行う円錐走査工程と、この円錐走査工程で測定した測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出工程と、この移動環境検出工程が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断工程と、この面形状判断工程で水平面と判断した所定の測定対象の形状を認識する形状認識工程とを備えている。
【００１１】
また、請求項６記載の発明は、請求項５記載の移動環境認識方法において、前記円錐走査工程は、多数の測定セグメントを同時に検出し、これら各測定セグメントに対して円錐走査を行なう。
さらに、請求項７記載の発明は、請求項５又は６記載の移動環境認識方法において、形状認識工程は、前記面形状判断工程で水平面と判断した前記所定の測定対象の連結成分抽出処理を行う工程と、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出する工程とを備えている。
【発明の効果】
【００１２】
本発明に係る移動環境認識装置及び方法によれば、測定対象に対して円錐走査を行うことで、測定距離及び円錐走査回転角度に対応するデータ、傾斜方向、センサ原点から測定対象中心までの距離、傾斜角を蓄積し、水平面形状判定を行なうことで得られた水平面の測定対象の形状を算出することができるので、高精度に測定対象の形状認識情報を得ることができる。
【図面の簡単な説明】
【００１３】
【図１】移動体の構成を示す概略図である。
【図２】外界認識装置を示すブロック図である。
【図３】移動環境検出器の構成を示す図である。
【図４】距離測定器を用いた円錐走査法を示す略線図である。
【図５】円錐走査法を示す別の略線図である。
【図６】円錐走査法を示す略線図である。
【図７】距離画像センサを用いる場合の多数の測定対象（セグメント）を示す図である。
【図８】円錐走査法を示す略線図である。
【図９】階段の凸部を測定した場合のα−Ｌ線図を示すものである。
【図１０】階段の凹部を測定した場合のα−Ｌ線図を示すものである。
【図１１】階段の踏み面を測定した場合のα−Ｌ線図を示すものである。
【図１２】階段の垂直面を測定した場合のα−Ｌ線図を示すものである。
【図１３】測定対象である階段の認識情報部位を示す図である。
【図１４】測定対象の認識処理を示すフローチャートである。
【図１５】ロボット中心地面の投影座標系を示す図である。
【図１６】センサ座標系変換図である。
【図１７】移動環境検出器に姿勢センサを装着した状態を示す図である。
【図１８】移動体本体に姿勢センサを装着した状態を示す図である。
【発明を実施するための形態】
【００１４】
以下、本発明を実施するための形態（以下、実施形態という。）を、図面を参照しながら詳細に説明する。
（１）脚動作型移動体の構成と動作
図１において、符号１は全体として移動体を示し、直方体形状の移動体本体２の前面上端中央部に前方に突出するように移動環境検出器３が設けられている。
移動体本体２の前面下部の左右両端位置には左前脚４Ｌ及び右前脚４Ｒが装着されていると共に、移動体本体２の後面下部の左右両端位置には左後脚５Ｌ及び右後脚５Ｒが装着されている。
左前脚４Ｌ及び右前脚４Ｒ並びに左後脚５Ｌ及び右後脚５Ｒは、それぞれ支軸周りの関節Ｊ０、股関節Ｊ１及び膝関節Ｊ２を介して脚車輪Ｊ３を歩行動作ができるように保持している。
【００１５】
そして、左前脚４Ｌ及び右前脚４Ｒ並びに左後脚５Ｌ及び右後脚５Ｒが平らな床面６上に自立しているときには、脚車輪Ｊ３を回転駆動することにより、移動体本体２を車輪の回転動作を利用して移動できるようになされていると共に、移動体本体２が測定対象としての階段７を上り下りする際には、左前脚４Ｌ若しくは右前脚４Ｒ又は左後脚５Ｌ若しくは右後脚５Ｒのいずれか１つ、または２つ以上を階段７の段差を上り下りするように脚車輪Ｊ３を回転駆動させずに、もしくは階段７の上の段の存在する側へ進むよう回転駆動しながら歩行動作させ、これにより脚車輪Ｊ３を階段７の踏み面８を踏む手段として用いて階段７を１歩ずつ上ったり下ったりできるようになされている。
このような移動体１の移動動作は、移動体本体２内に設けられている外界認識装置９が移動環境検出器３の検出結果を用いて移動環境を認識して左前脚４Ｌ若しくは右前脚４Ｒ又は左後脚５Ｌ若しくは右後脚５Ｒを駆動制御することによって行われる。
【００１６】
外界認識装置９は、図２に示すように、移動環境検出器３として円錐走査式距離測定部３１あるいは距離画像測定部３６を装備し、情報処理を行うパーソナルコンピュータによって構成され、データ入出力手段１１からユーザが入力した指令データをバス１２を介してシステム制御用中央処理ユニット（ＣＰＵ）１３が受け取ったとき、これに応じてＣＰＵ１３がプログラムＲＯＭ・動作メモリＲＡＭ１４を用いて移動環境検出器３の検出結果に応じた環境認識演算処理を実行し、データ入出力手段１１を通じて本体駆動部１５に当該環境認識結果を通知することにより、左前脚４Ｌ若しくは右前脚４Ｒ又は左後脚５Ｌ若しくは右後脚５Ｒを移動動作させる。
【００１７】
（２）円錐走査による測定
円錐走査式距離測定部３１は、図３に示すように、水平走査機構２１を有し、この水平走査機構２１が移動体本体２の前面から前方に突出する突出支持部２２（図１参照）に支持されており、これにより水平走査機構２１が垂直方向の中心軸ｚを中心として水平ターンテーブル２３を矢印ａで示すように水平面内において回動動作させる。
水平ターンテーブル２３の下面には、取付部材２４を介して垂直走査機構２５が取り付けられており、これにより垂直走査機構２５が垂直ターンテーブル２６を、矢印ｂで示すように、一方の水平軸ｙを中心として垂直面内において回動動作させる。
【００１８】
垂直ターンテーブル２６には距離測定器２７が取り付けられ、これにより距離測定器２７の距離測定方向が他方の水平軸ｘを基準にして垂直面内（従って仰角が変更する方向）において変更制御される。
この実施形態の場合、距離測定器２７は、赤外線パルスビームを所定の測定方向に放出すると共に、当該赤外線パルスビームが測定対象において反射して得られる戻り光を受光し、当該赤外線パルスビームの発射時刻と戻り光の到達時刻との時間差に基づいて、距離測定器２７から測定対象までの距離Ｌを測定する。
【００１９】
また、距離測定器２７は、赤外線ビームを所定の測定方向に放出すると共に、当該赤外線ビームが測定対象において反射して得られる戻り光を受光し、当該赤外線ビームと戻り光との三角形状を利用して三角測量の原理に基づいて、距離測定器２７から測定対象までの距離Ｌを測定するようにしてもよい。
この実施形態の場合、水平ターンテーブル２３に取り付けられた取付部材２４、垂直走査機構２５、垂直ターンテーブル２６及び距離測定器２７は保護カバー２８によって覆われている。
【００２０】
当該距離測定器２７に対する水平走査機構２１及び垂直走査機構２５の走査は、外界認識装置９（図２参照）のデータ入出力手段１１から入力されるユーザの動作命令によって、距離測定器２７から放出される円錐走査検出光ＤＥＴ（図４参照）の放出方向（これを視準方向と呼ぶ）を、外界認識装置９（図２参照）のＣＰＵ１３が制御することにより行われる。
【００２１】
そして、移動環境検出器３は、水平面内のｘｙ座標を順次指定して行くことにより距離測定器２７を水平面内のすべての角度方向に走査できると共に、当該水平面内のすべての角度位置について、垂直面内の角度位置を走査することができ、これにより移動体１が移動すべき環境に存在する認識対象と距離測定器２７との間の距離を、図４に示す円錐走査式距離測定法によって、測定することができる。
【００２２】
図４において、距離測定器２７が配設されている位置Ｏを原点として（これをセンサ原点と呼ぶ）、距離測定器２７の視準方向をＺ軸で表わすと共に、当該視準方向軸Ｚと直交する平面の座標を直交座標軸Ｘ及びＹによって表わす。
図４の場合、視準方向軸Ｚのセンサ原点Ｏから基準距離ｄＳＴの位置に、視準方向軸Ｚ（以下これを視準方向と呼ぶ）を中心として描いた半径ｒＳＴの円形平面を水平垂直走査面Ｓ１として定義し、センサ原点Ｏからの赤外線パルス検出ビームを水平垂直走査面Ｓ１の円形軌道に沿うように走査させることにより、センサ原点Ｏを頂点としかつ水平垂直走査面Ｓ１を円錐底面とする円錐走査立体を想定する。
【００２３】
このセンサ原点Ｏを頂点としかつ水平垂直走査面Ｓ１を円錐底面とする円錐体については、図５（Ａ）に示すように、ＸＺ平面についてセンサ原点Ｏから水平垂直走査面Ｓ１の中心位置ＣＳＴ（水平垂直走査面Ｓ１とＺ軸との交点位置）から半径ｒＳＴだけＸ軸と平行に移動した点と、センサ原点Ｏとを結ぶ線は、センサ原点Ｏから発射された検出パルス光Ｌｐ（すなわち赤外線パルスビーム）が通る円錐面を表している。
従って、当該円錐面の円錐底面の半径ｒＳＴは、円錐半頂角βと、センサ原点Ｏから水平垂直走査面Ｓ１までの基準距離ｄＳＴとから、次式
【００２４】
【数１】
によって表すことができる。
【００２５】
水平垂直走査面Ｓ１の各走査位置について、水平垂直走査面Ｓ１の直交座標軸ＸＹについて、Ｘ軸と平行なｘ軸との交点を起点として、円錐走査回転角度α（＝０〜３６０〔°〕）によって円錐底面の外周面に沿った走査点Ｐ１を順次走査すると考えると、この走査点Ｐ１を通る検出パルス光は水平垂直走査面Ｓ１より前方位置にある測定対象面Ｓ２の測定点Ｐ２に到達するから、測定対象面Ｓ２上に水平垂直走査面Ｓ１の走査点Ｐ１の走査に対応して移動する測定点Ｐ２が考えられる。
【００２６】
ここで走査対象面Ｓ２は、測定対象の外観形状が任意であれば、視準方向軸Ｚに対して直交する面をもつとは限らず、傾きをもっているものもあるが、直交するものと考えると、測定対象面Ｓ２は水平垂直走査面Ｓ１と相似の円を描くことになる。
かくして水平垂直走査面Ｓ１上の走査点Ｐ１のｘｙ平面のｘ座標点Ｐ１ｘは、図５（Ｂ）に示すように、水平走査角θによって、次式
【００２７】
【数２】
によって表すことができると共に、ｙｚ平面のｙ座標点Ｐ１ｙは、図５（Ｂ）に示すように、垂直走査角φによって
【００２８】
【数３】
によって表すことができる。
水平垂直走査面Ｓ１上の走査点Ｐ１のｘ軸上の座標ｘは、図５（Ｃ）に示すように、円錐走査回転角度αが時間の経過と共にωｔで変化すると考えれば、次式、
【００２９】
【数４】
のように表すことができると共に、ｙ軸上の座標ｙは次式、
【００３０】
【数５】
のように表すことができる。
【００３１】
以上の動作原理に基づいて、外界認識装置９（図２参照）のＣＰＵ１３は、円錐走査式距離測定部３１を起動することにより、円錐走査式距離測定部３１の円錐走査制御部３２によって水平垂直走査面Ｓ１上の走査点座標ｘ及びｙを、（４）式及び（５）式に基づいて、円錐走査回転角度αを角速度ωで変更制御することにより、センサ原点Ｏにある距離測定器２７から放出されるパルスビームを円錐底面の円軌道に沿って走査するために必要な水平走査角θ及び垂直走査角φを表す駆動角度データを生成する。
【００３２】
この水平走査角θ及び垂直走査角φの駆動角度データは水平垂直走査部３３に与えられ、当該水平垂直走査部３３が移動環境検出器３に対して、水平走査機構２１及び垂直走査機構２５の駆動出力を送出する。
これにより測定対象面Ｓ２上にパルスビームがα＝０〔°〕からα＝３６０〔°〕まで走査するように投射される。
【００３３】
ここで、測定対象面Ｓ２が、図６に示すように、視準方向Ｚの軸に対して直交する平面を有するとすれば、距離測定器２７から放出された検出パルス光Ｌｐは走査点Ｐ１を通って測定点Ｐ２に当たって反射される。
この結果、測定点Ｐ２は、測定対象面Ｓ２上に、水平垂直走査面Ｓ１と相似の円形の交円軌跡ＴＲ（以下これを交円と呼ぶ）を描き、これに対して測定対象面Ｓ２が視準方向Ｚの軸に対して直交せずに傾いているときは楕円形の交円ＴＲを描く。
【００３４】
かくして、測定対象面Ｓ２の交円ＴＲから反射された検出パルス光Ｌｐが移動環境検出器３の距離測定器２７に戻ってきたとき、その検出データがバス１２を介して円錐走査式距離測定部３１の距離検出部３４に取り込まれてセンサ原点Ｏから測定点Ｐ２までの距離を検出できることになり、当該測定距離Ｌが、円錐走査回転角度α（＝０〜３６０〔°〕）に対応するデータ（α、Ｌ）として、円錐走査距離データメモリ３５Ａに蓄積される。
このデータ（α、Ｌ）は、円錐走査回転角度αと測定距離Ｌの関係が得られるα−Ｌ線図と呼ばれ、測定距離の最大値Ｌｍａｘ及び最大値Ｌｍａｘのときの円錐走査回転角度αから傾斜方向α（Ｌｍａｘ）を決定することができる。
【００３５】
一方、距離画像測定部３６は、多点の距離を同時に高速に測定することが可能な距離画像センサを使用しており、距離画像センサの測定領域内において、複数の円形セグメント上の距離情報に対応する画素位置のデータとして距離画像データメモリ３６Ｂに蓄積される。
すなわち、図７に示すように、円形セグメントの個数をｍ（ｘ方向），ｎ（ｙ方向），間隔をΔx，Δｙ，センサの横方向ピクセル数ｗｃ，センサの縦方向ピクセル数ｈｃとして任意のｉ番，ｊ番目の円の中心点は、
【００３６】
【数６】
で表され、この点を中心に、円錐走査角αに応じた各抽出点を算出する。そして、距離画像センサからは各画素における測定距離Ｌが得られるので、円錐走査回転角度αが０〜３６０°回転する場合に相当する距離画像上の座標を算出し、その座標に対応する画素位置の測定距離Ｌを取得することで、データ（α、Ｌ）が得られる。なお、画素の中間位置については、バイリニア法による測定データ平滑補間処理を行う。
【００３７】
なお、円錐走査距離データメモリ３５Ａの複数領域の走査データ結果を距離画像データメモリ３６Ｂに蓄積することで、以降の処理を同様の手順で行なうことができる。
このデータ（α、Ｌ）もα−Ｌ線図と呼ばれ、測定距離の最大値Ｌｍａｘ及び最大値Ｌｍａｘのときの円錐走査回転角度αから傾斜方向α（Ｌｍａｘ）を決定することができる。
【００３８】
そして、距離画像データメモリ３６Ｂに蓄積されたα−Ｌ線図は、傾斜方向α（Ｌｍａｘ）の幾何学的な位置関係と関係式から、センサ原点から円形セグメント中心までの距離ｄｚ、傾斜角γを算出する。
すなわち、図８のｚは、センサ原点Ｏから視準方向Ｚへとった距離を表し、主軸ｍは、ｚに対して直交する方向の距離を表す。
【００３９】
また、βは円錐走査回転角度αによって形成される円錐走査検出光ＤＥＴの半頂角で、センサ原点Ｏから極小点ＭＮまでの距離ｄｍｉｎは、次式、
【００４０】
【数７】
によって求められると共に、センサ原点Ｏから極大点ＭＸまでの距離ｄｍａｘは、次式、
【００４１】
【数８】
によって求めることができ、その差分ｈは、次式、
【００４２】
【数９】
になる。
ここで、極大点ＭＸと極小点ＭＮとを結ぶ線は、平面上の成分を表しており、差分ｈは当該平面の傾斜の高さを表している。
【００４３】
一方、極小点ＭＮ及び極大点ＭＸからそれぞれｚ軸に下ろした脚の長さｒｍｉｎ及びｒｍａｘは、
【００４４】
【数１０】
【００４５】
【数１１】
のように、半頂角βによって表すことができるから、両者の和ｋは、
【００４６】
【数１２】
になる。
この結果、（９）式の差分ｈとの間には、
【００４７】
【数１３】
の関係がある。
【００４８】
（１３）式のγは、極大点ＭＸと極小点ＭＮとを結ぶ傾斜面（すなわち測定対象である平面）の、円錐走査検出光ＤＥＴの円錐底面からの傾斜角を表しており、当該傾斜角γを（１３）式から求めることができる。
かくして求めた傾斜角γによって、センサ原点から円形セグメント中心までの距離ｄｚは、次式、
【００４９】
【数１４】
【００５０】
【数１５】
によって求めることができる。
したがって、距離画像データメモリ３６Ｂには、多数の円形セグメントのα−Ｌ線図、傾斜方向α（Ｌｍａｘ）、センサ原点から円形セグメント中心までの距離ｄｚ、傾斜角γが蓄積される。
【００５１】
（３）測定対象（階段）の水平面形状判定機能
また、ＣＰＵ１３は、距離画像データメモリ３６Ｂに蓄積されたデータに基づいて測定対象の水平面形状判定を行なう。
図９から図１２の（ａ）は、移動環境検出器３を斜め下向きに配置し、測定対象である図１の階段７の凹凸部、平面部を示したものである。これらの図の（ｂ）で示すα−Ｌ線図の実線は計測値である。また、測定対象が１つの平面であれば、α−Ｌ線図が一次の三角関数で表現できることに着目し、一次のフーリエ級数計算を行い、以下の（１６）式の通りフィッティング値を算出することとした。なお、α−Ｌ線図の破線がフィッティング値であり、傾斜方向α(Ｌｍａｘ)は、フィッティング値から求めている。
【００５２】
【数１６】
【００５３】
【数１７】
【００５４】
図９の階段７の凸部を示すα−Ｌ線図は、水平面（踏み面Ｆ）及び垂直面（蹴込面Ｋ）の両者が存在するので１次曲線にはならずフィッティング値と測定値との残差が大きい。また、図１０の階段７の凹部を示すα−Ｌ線図も、水平面（床面６）及び垂直面（蹴込面Ｋ）が存在するので１次曲線にはならずフィッティング値と測定値との残差が大きい。
これに対して、図１１の水平面（踏み面Ｆ）を示すα−Ｌ線図と、図１２の垂直面（蹴込面Ｋ）を示すα−Ｌ線図の両者は、範囲内に１つの面のみが存在し、１次曲線近くなり、フィッティング値と測定値との残差が小さい。そして、フィッティング値と測定値との残差が小さく、平面の傾斜方向α(Ｌｍａｘ)の値が大きい図１１のα−Ｌ線図を、水平面（踏み面Ｆ）と判定（水平面判定）することができる。
【００５５】
なお、残差の評価値としては、以下の（１８）式を用いる。
【００５６】
【数１８】
【００５７】
（４）測定対象（階段）の認識処理手順
測定対象である階段の認識情報は、図１３に示すように階段の位置（Ｓｘ，Ｓｙ），方向γ，階段幅Ｓｗである。なお、階段幅Ｓｗは測定レンジ内の有効幅とし、階段の位置は有効幅の中心点とする。
ＣＰＵ１３は、図１４のフローチャートで示す手順で階段の認識処理を行なう。
図１４の階段の認識処理は、先ず、ステップＳＴ１において、上述した水平面形状判定機能で示したように距離画像データメモリ３６Ｂのデータから水平面判定を受けた円形セグメントを読込み、そのセグメント（円形セグメント）の中心座標（ｘＣ,yＣ）をプロットした画像を作成する。
【００５８】
次いでステップＳＴ２に移行し、作成した画像に対して連結成分抽出処理（ラベリング）を実行し、単なる点の集合であった画像から、複数のエリアが抽出される。
次いでステップＳＴ３に移行し、以下の（１９）式〜（２２）式を用いてセグメントの中心座標（ｘＣ,yＣ）からロボット座標系のＸ座標、Ｙ座標、Ｚ座標を算出する（図１５のロボット中心地面投影座標系及び図１６のセンサ座標系変換図を参照）。
【００５９】
【数１９】
【００６０】
次いでステップＳＴ４に移行し、抽出エリア毎に高さＺ座標の平均値Ｚａｖｅと、高さＺ座標のばらつき（最大値Ｚｍａｘと最小値Ｚｍｉｎの差で評価）を算出する。
次いでステップＳＴ５に移行し、所定の抽出エリアの高さＺ座標の平均値Ｚａｖｅが階段の一段目高さであり（第１の条件）、且つ、高さＺ座標のばらつき（Ｚｍａｘ − Ｚｍｉｎ ）が十分に小さい場合には（第２の条件）、ステップＳＴ６に移行し、上記の第１及び第２の条件の少なくとも一方を満たしていない場合には、ステップＳＴ７に移行し、次の抽出エリアを読み込む。ステップＳＴ７からステップＳＴ５に移行し、上述した判定処理を行なう。
ステップＳＴ６では、高さＺ座標の平均値Ｚａｖｅが階段の一段目高さであり、且つ、高さＺ座標のばらつき（Ｚｍａｘ − Ｚｍｉｎ ）が十分に小さい抽出エリアに対して最小自乗法による直線フィッティングを行う。
【００６１】
次いでステップＳＴ８に移行し、近似直線上の左右端点（ｘｌｅｆｔ，ｙｌｅｆｔ）,（ｘrｉｇｈｔ，ｙrｉｇｈｔ）を抽出する。
次いでステップＳＴ９に移行し、ロボット座標系の左右端点（Ｘｌｅｆｔ，Ｙｌｅｆｔ，Ｚｌｅｆｔ）,（Ｘrｉｇｈｔ，Ｙrｉｇｈｔ，Ｚrｉｇｈｔ）に変換する。
次いでステップＳＴ１０に移行し、変換されたロボット座標系の左右端点（Ｘｌｅｆｔ，Ｙｌｅｆｔ，Ｚｌｅｆｔ）,（Ｘrｉｇｈｔ，Ｙrｉｇｈｔ，Ｚrｉｇｈｔ）から、以下の（２７）式〜（３０）式を用いて階段の位置（Ｓｘ，Ｓｙ），方向γ，階段幅Ｓｗを算出する。
【００６２】
【数２０】
【００６３】
なお、ステップＳＴ６において階段の２段目以降に該当する高さを条件に設定し同様に処理し、複数の段についての段の方向、位置、幅の情報を取得することにより、階段の段数についての認識や、階段が続いていく方向の認識も可能となる。
上記構成の外界認識装置９によると、移動環境検出器３が測定対象に対して円錐走査を行うことで、測定距離Ｌ及び円錐走査回転角度α（＝０〜３６０〔°〕）に対応するデータ（α、Ｌ）（α−Ｌ線図）、傾斜方向α（Ｌｍａｘ）、センサ原点から円形セグメント中心までの距離ｄｚ、傾斜角γを蓄積し、測定対象（階段）水平面形状判定機能を行なうことで、フィッティング値と測定値との残差が小さく平面の傾斜方向α(Ｌｍａｘ)の値が大きいデータを水平面と判定し、測定対象（階段）の認識処理を行なうことで、水平面と判断した測定対象の連結成分抽出処理を行い、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記測定対象の形状を算出することができるので、高精度に測定対象の形状認識情報を得ることができる。
【００６４】
また、多点の距離を同時に高速に測定することが可能な距離画像センサを使用し、この距離画像センサの測定領域内において、複数の円形セグメント上の距離情報に対応する画素位置のデータとして、測定距離Ｌ及び円錐走査回転角度αに対応するデータ（α、Ｌ）を蓄積すると、高速、且つ高精度に測定対象の形状認識情報を得ることができる。
なお、図１７及び図１８に示すように、移動体本体２に装着された移動環境検出器３の傾斜角を測定することが可能な姿勢センサ５０を、移動環境検出器３、或いは移動体本体２に備え、図２に示す外界認識装置９に備えた測定データ補正部５１が、移動環境検出器３が測定した測定データに対して姿勢センサ５０が測定した傾斜角に基づいて座標変換を行なうようにすると、さらに高精度に測定対象の形状認識情報を得ることができる。
【符号の説明】
【００６５】
１…移動体、２…移動体本体、３…移動環境検出器、４Ｌ…左前脚、４Ｒ…右前脚、５Ｌ…左後脚、５Ｒ…右後脚、６…床面、７…階段、８，Ｆ…踏み面、９…外界認識装置、１１…データ入出力手段、１２…バス、１５…本体駆動部、２１…水平走査機構、２２…突出支持部、２３…水平ターンテーブル、２４…取付部材、２５…垂直走査機構、２６…垂直ターンテーブル、２７…距離測定器、２８…保護カバー、３１…円錐走査式距離測定部、３２…円錐走査制御部、３３…水平垂直走査部、３４…距離検出部、３５Ａ…円錐走査距離データメモリ、３６…距離画像測定部、３６Ｂ…距離画像データメモリ、５０…姿勢センサ、５１…測定データ補正部
【特許請求の範囲】
【請求項１】
測定対象に対して円錐走査を行う円錐走査手段と、
この円錐走査手段の測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出手段と、
この移動環境検出手段が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断手段と、
この面形状判断手段が水平面と判断した所定の測定対象の形状を認識する形状認識手段と、
を備えていることを特徴とする移動環境認識装置。
【請求項２】
前記円錐走査手段は、多数の測定セグメントを同時に検出する距離画像センサを備えており、当該距離画像センサが検出した各測定セグメントに対して円錐走査を行なうことを特徴とする請求項１記載の移動環境認識装置。
【請求項３】
形状認識手段は、前記面形状判断手段で水平面と判断した前記所定の測定対象の連結成分抽出処理を行い、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出することを特徴とする請求項１又は２記載の移動環境認識装置。
【請求項４】
前記円錐走査手段の傾斜角を測定可能な姿勢センサと、
前記円錐走査手段の測定データに、前記姿勢センサが測定した前記傾斜角に基づいた座標変換を行なうことで、前記移動環境検出手段が算出した前記測定セグメントの測定データを水平面基準に補正する測定データ補正手段と、を備えていることを特徴とする請求項１乃至３の何れか１項に記載の移動環境認識装置。
【請求項５】
測定対象に対して円錐走査を行う円錐走査工程と、
この円錐走査工程で測定した測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出工程と、
この移動環境検出工程が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断工程と、
この面形状判断工程で水平面と判断した所定の測定対象の形状を認識する形状認識工程と、
を備えていることを特徴とする移動環境認識方法。
【請求項６】
前記円錐走査工程は、多数の測定セグメントを同時に検出し、これら各測定セグメントに対して円錐走査を行なうことを特徴とする請求項５記載の移動環境認識方法。
【請求項７】
形状認識工程は、前記面形状判断工程で水平面と判断した前記所定の測定対象の連結成分抽出処理を行う工程と、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出する工程とを備えていること特徴とする請求項５又は６記載の移動環境認識方法。
【請求項１】
測定対象に対して円錐走査を行う円錐走査手段と、
この円錐走査手段の測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出手段と、
この移動環境検出手段が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断手段と、
この面形状判断手段が水平面と判断した所定の測定対象の形状を認識する形状認識手段と、
を備えていることを特徴とする移動環境認識装置。
【請求項２】
前記円錐走査手段は、多数の測定セグメントを同時に検出する距離画像センサを備えており、当該距離画像センサが検出した各測定セグメントに対して円錐走査を行なうことを特徴とする請求項１記載の移動環境認識装置。
【請求項３】
形状認識手段は、前記面形状判断手段で水平面と判断した前記所定の測定対象の連結成分抽出処理を行い、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出することを特徴とする請求項１又は２記載の移動環境認識装置。
【請求項４】
前記円錐走査手段の傾斜角を測定可能な姿勢センサと、
前記円錐走査手段の測定データに、前記姿勢センサが測定した前記傾斜角に基づいた座標変換を行なうことで、前記移動環境検出手段が算出した前記測定セグメントの測定データを水平面基準に補正する測定データ補正手段と、を備えていることを特徴とする請求項１乃至３の何れか１項に記載の移動環境認識装置。
【請求項５】
測定対象に対して円錐走査を行う円錐走査工程と、
この円錐走査工程で測定した測定データに基づいて前記測定対象の傾斜角、傾斜方向及び測定対象中心までの距離を算出する移動環境検出工程と、
この移動環境検出工程が算出した前記測定対象の算出データに基づき、各測定対象の面形状を判断する面形状判断工程と、
この面形状判断工程で水平面と判断した所定の測定対象の形状を認識する形状認識工程と、
を備えていることを特徴とする移動環境認識方法。
【請求項６】
前記円錐走査工程は、多数の測定セグメントを同時に検出し、これら各測定セグメントに対して円錐走査を行なうことを特徴とする請求項５記載の移動環境認識方法。
【請求項７】
形状認識工程は、前記面形状判断工程で水平面と判断した前記所定の測定対象の連結成分抽出処理を行う工程と、抽出領域ごとに高さ情報が特定の条件を満たしているか否かを判断し、特定の抽出領域の高さ情報が特定の条件を満たしているときに、前記所定の測定対象の形状を算出する工程とを備えていること特徴とする請求項５又は６記載の移動環境認識方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【公開番号】特開２０１２−２４２３６８（Ｐ２０１２−２４２３６８Ａ）
【公開日】平成２４年１２月１０日（２０１２．１２．１０）
【国際特許分類】
【出願番号】特願２０１１−１１６２６９（Ｐ２０１１−１１６２６９）
【出願日】平成２３年５月２４日（２０１１．５．２４）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【出願人】（５０４１３３１１０）国立大学法人電気通信大学 (383)
【Ｆターム（参考）】
【公開日】平成２４年１２月１０日（２０１２．１２．１０）
【国際特許分類】
【出願日】平成２３年５月２４日（２０１１．５．２４）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【出願人】（５０４１３３１１０）国立大学法人電気通信大学 (383)
【Ｆターム（参考）】
[ Back to top ]