JP7056496B2 - 位置検出装置 - Google Patents

Description

本発明は、位置検出装置に関する。

高低差のある場所を移動体が走行している場合、移動体が走行している路面よりも低い位置である低位置に移動体が移動すると、移動体が走行不能となる場合がある。
特許文献１には、フォークリフトがプラットホーム上の路面からプラットホームの下の平面に落下することを抑制する装置が記載されている。特許文献１では、プラットホーム上の路面にガイドワイヤを配置し、フォークリフトにはガイドワイヤを検出するためのセンサが配置されている。センサによってガイドワイヤが検出されると、フォークリフトは強制的に停止させられる。

特開２００４－１８２１００号公報

ところで、特許文献１では、低位置を検出するために事前に路面にガイドワイヤを設けるなどのインフラストラクチャーの整備が必要となる。
本発明の目的は、インフラストラクチャーの整備を行うことなく低位置を検出できる位置検出装置を提供することにある。

上記課題を解決する位置検出装置は、移動体に搭載され、前記移動体の存在する路面よりも低い位置である低位置を検出する位置検出装置であって、ステレオカメラと、前記ステレオカメラで撮像された画像から各画素に視差が対応付けられた視差画像を取得する視差画像取得部と、前記視差画像の縦方向の画素と視差との二次元配列に、前記視差画像の各画素に対応付けられた視差を投票する投票部と、前記投票部による投票数が閾値以上である視差がプロットされており、前記視差をＸ軸、前記縦方向の画素をＹ軸とする座標系から直線を抽出する直線抽出部と、前記直線抽出部によって、第１直線と、前記第１直線の終点座標のＸ座標よりも始点座標のＸ座標が大きく前記第１直線との傾きの差が許容範囲内の第２直線とが検出された場合、前記始点座標のＹ座標に対応する実空間上の位置よりも遠い位置に低位置が存在していると判断する判断部と、を備える。

ステレオカメラで撮像された画像から視差を算出すると、ステレオカメラに近いほど大きな視差が算出されることになる。縦方向の画素は、ステレオカメラからの距離を表しているため、路面が平面であれば、視差は縦方向の画素に合わせて徐々に変化し、直線抽出部は単数の直線を抽出する。これに対して、低位置が存在する場合、路面よりも低くなる分だけステレオカメラから低位置までの距離が遠くなることになる。これにより、路面に低位置が存在する場合には、路面から低位置に切り替わる画素を境にして、路面が平坦な場合に比べて視差が大きく変化することになる。視差が大きく変化することで、直線抽出部は、第１直線と第２直線とを抽出することになる。従って、これを利用することで、インフラストラクチャーの整備を行うことなく低位置が存在していることを検出することができる。

上記位置検出装置について、前記判断部は、前記第１直線の切片と前記第２直線の切片とが異なり、かつ、前記第１直線の終点座標のＸ座標と前記第２直線の始点座標のＸ座標との差が所定値以上の場合、前記低位置を段差により生じた平面であると判断してもよい。これによれば、路面よりも低い平面を検出することができる。

上記位置検出装置について、前記判断部は、第１直線の切片と前記２直線の切片との差が許容範囲内であり、前記第１直線の終点座標から延び、かつ、Ｙ座標が大きくなるほどＸ座標が小さくなる第３直線が存在し、前記第３直線の終点座標のＸ座標と前記第２直線の始点座標のＸ座標との差が所定値以上の場合、前記低位置を前記路面から凹む溝であると判断する。これによれば、溝を検出することができる。

本発明によれば、インフラストラクチャーの整備を行うことなく低位置を検出することができる。

位置検出装置が搭載されるフォークリフトの側面図。 フォークリフト、及び、位置検出装置の概略構成図。 フォークリフトの存在する路面と、路面より低い平面とを示す図。 第１実施形態の低位置探索処理を示すフローチャート。 視差画像を示す図。 二次元配列を示す図。 直線抽出により得られた直線を示す図。 車速制限処理を示すフローチャート。 フォークリフトから段差までの距離と速度規制率との関係を示す図。 フォークリフトの存在する路面と、溝とを示す図。 第２実施形態の低位置探索処理を示すフローチャート。 直線抽出により得られた直線を示す図。 領域を複数の範囲に区分した図を示す図。

（第１実施形態）
以下、位置検出装置の第１実施形態について説明する。
図１に示すように、フォークリフト１０は、車体１１と、車体１１に設けられた荷役装置１２と、を備える。なお、フォークリフト１０は、自動で走行動作及び荷役動作が行われるものであってもよいし、搭乗者による操作によって走行動作及び荷役動作が行われるものであってもよい。

図２に示すように、フォークリフト１０は、メインコントローラ２０と、走行用モータＭ１と、走行用モータＭ１を制御する走行制御装置２３と、車速センサ２４と、を備える。メインコントローラ２０は、走行動作及び荷役動作に関する制御を行う。メインコントローラ２０は、ＣＰＵ２１と、種々の制御を行うためのプログラムなどが記憶されたメモリ２２と、を備える。

メインコントローラ２０のＣＰＵ２１は、フォークリフト１０の車速が目標速度となるように走行制御装置２３に走行用モータＭ１の回転数の指令を与える。本実施形態の走行制御装置２３は、モータドライバである。本実施形態の車速センサ２４は、走行用モータＭ１の回転数を検出する回転数センサである。車速センサ２４は、走行用モータＭ１の回転数を走行制御装置２３に出力する。走行制御装置２３は、メインコントローラ２０からの指令に基づき、走行用モータＭ１の回転数が指令と一致するように走行用モータＭ１を制御する。

移動体としてのフォークリフト１０には、フォークリフト１０の存在している路面よりも低い位置である低位置を検出する位置検出装置３０が搭載されている。位置検出装置３０は、ステレオカメラ３１と、ステレオカメラ３１によって撮像された画像の画像処理を行う画像処理部４１と、を備える。ステレオカメラ３１は、例えば、車体１１の上部などフォークリフト１０の上方からフォークリフト１０の走行する路面を鳥瞰できるように配置されている。なお、車体１１とは、座席や走行に関する部材を備える基台や、座席の上部に設けられるヘッドガードを含む。本実施形態のステレオカメラ３１は、フォークリフト１０の後方を撮像する。従って、位置検出装置３０は、フォークリフト１０の後方に存在する低位置を検出する。

なお、フォークリフト１０の前方を撮像するステレオカメラを用いてフォークリフト１０の前方に存在する低位置を検出するようにしてもよい。また、フォークリフト１０の前方及び後方を撮像する個別のステレオカメラを用いてフォークリフト１０の前方及び後方の両側の低位置を検出するようにしてもよい。即ち、ステレオカメラ３１の配置を変更することで、任意の方向に存在する低位置を検出することが可能である。フォークリフト１０の前方を撮像するステレオカメラを設ける場合、例えば、荷役装置１２の上部や車体１１の上部などにステレオカメラは設けられる。

ステレオカメラ３１は、２つのカメラ３２，３３を備える。カメラ３２，３３としては、例えば、ＣＣＤイメージセンサや、ＣＭＯＳイメージセンサが用いられる。各カメラ３２，３３は、互いの光軸が平行となるように配置されている。本実施形態において、２つのカメラ３２，３３は、水平方向に並んで配置されている。２つのカメラ３２，３３のうち、一方を第１カメラ３２、他方を第２カメラ３３とする。第１カメラ３２によって撮像された画像を第１画像、第２カメラ３３によって撮像された画像を第２画像とすると、第１画像と第２画像では同一の物体が横方向にずれて写ることになる。詳細にいえば、同一の物体を撮像した場合、第１画像に写る物体と、第２画像に写る物体では、横方向の画素［ｐｘ］にカメラ３２，３３間の距離に応じたずれが生じることになる。第１画像及び第２画像は、画素数が同じであり、例えば、６４０×４８０［ｐｘ］＝ＶＧＡの画像が用いられる。第１画像及び第２画像は、ＲＧＢ形式の画像である。

画像処理部４１は、ＣＰＵ４２と、ＲＡＭ及びＲＯＭ等からなる記憶部４３と、を備える。記憶部４３には、ステレオカメラ３１によって撮像された画像から低位置を検出するための種々のプログラムが記憶されている。画像処理部４１は、各種処理のうち少なくとも一部の処理を実行する専用のハードウェア、例えば、特定用途向け集積回路：ＡＳＩＣを備えていてもよい。画像処理部４１は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ＡＳＩＣ等の１つ以上の専用のハードウェア回路、あるいは、それらの組み合わせを含む回路として構成し得る。プロセッサは、ＣＰＵ、並びに、ＲＡＭ及びＲＯＭ等のメモリを含む。メモリは、処理をＣＰＵに実行させるように構成されたプログラムコードまたは指令を格納している。メモリ、即ち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆるものを含む。

図３に示すように、本実施形態の位置検出装置３０は、フォークリフト１０の存在する路面Ｒ１よりも低い平面である路面Ｒ２を検出する。なお、路面Ｒ２に限られず、路面Ｒ１よりも低い平面であれば、低位置として検出される。路面Ｒ２は、段差ＳＴにより生じたものである。路面Ｒ１は、例えば、トラックなどへ荷を積む場合やトラックから荷を降ろす際に、トラックの荷台の高さに応じて設けられたプラットホーム上の路面である。路面Ｒ２は、トラックが停車する路面である。段差ＳＴとは、路面Ｒ１と路面Ｒ２との境に位置する部分であり、路面Ｒ１と路面Ｒ２に高低差を生じさせる壁部である。以下、フォークリフト１０の存在する路面Ｒ１を第１路面Ｒ１、第１路面Ｒ１よりも低い路面Ｒ２を第２路面Ｒ２と称して説明を行う。

画像処理部４１により行われる低位置探索処理について第１実施形態の作用とともに説明する。低位置探索処理は、フォークリフト１０が起動状態のときに行われる。起動状態とは、フォークリフト１０に走行動作及び荷役動作を行わせることが可能な状態である。本実施形態では、図３に示すように、ステレオカメラ３１で路面Ｒ１，Ｒ２が撮像された場合の処理を一例として説明する。

図４及び図５に示すように、ステップＳ１において、画像処理部４１は、視差画像ｄｐを取得する。視差画像ｄｐは、画素に対して視差［ｐｘ］を対応付けた画像である。視差は、第１画像と、第２画像とを比較し、各画像に写る同一特徴点について第１画像と第２画像の画素数の差を算出することで得られる。なお、特徴点とは、物体のエッジなど、境目として認識可能な部分である。特徴点は、輝度情報などから検出することができる。

画像処理部４１は、ステレオカメラ３１によって撮像されている映像から同一フレームの第１画像及び第２画像を取得する。画像処理部４１は、各画像を一時的に格納するＲＡＭを用いて、ＲＧＢからＹＣｒＣｂへの変換を行う。なお、画像処理部４１は、歪み補正、エッジ強調処理などを行ってもよい。画像処理部４１は、第１画像の各画素と第２画像の各画素との類似度を比較して視差を算出するステレオ処理を行う。なお、ステレオ処理としては、画素毎に視差を算出する手法を用いてもよいし、各画像を複数の画素を含むブロックに分割してブロック毎の視差を算出するブロックマッチング法を用いてもよい。各画素の類似度の比較には、例えば、SAD：Sum of Absolute Difference、SSD：Sum of Squared Differenceなどが用いられる。画像処理部４１は、第１画像を基準画像、第２画像を比較画像として視差画像ｄｐを取得する。画像処理部４１は、第１画像の画素毎に、最も類似する第２画像の画素を抽出し、第１画像の画素と、当該画素に最も類似する画素の横方向の画素数の差を視差として算出する。これにより、基準画像である第１画像の各画素に視差が対応付けられた視差画像ｄｐを取得することができる。なお、視差画像ｄｐとは、必ずしも表示を要するものではなく、視差画像ｄｐにおける各画素に視差が対応付けられたデータのことを示す。本実施形態では、画像処理部４１が、視差画像取得部として機能しているといえる。

図５には、第１画像と第２画像から得られた視差画像ｄｐの一例を示す。図５に示す視差画像ｄｐでは、視差の大小を濃淡で表現している。視差は、ステレオカメラ３１に近い位置ほど大きくなり、ステレオカメラ３１から離れるほど小さくなる。以下、視差画像ｄｐの横方向の座標をＸ座標とし、視差画像ｄｐの縦方向の座標をＹ座標とする。Ｘ座標は横方向の画素位置を示し、Ｙ座標は縦方向の画素位置を示す。視差画像ｄｐの横方向の画素のうち左の画素ほどＸ座標が小さく、縦方向の画素のうち上の画素ほどＹ座標が小さい。縦方向の画素が小さいほどステレオカメラ３１から離間しており、縦方向の画素が大きいほどステレオカメラ３１に近いといえる。視差画像ｄｐの中心座標のＸ座標は、フォークリフト１０の幅方向の中央に対応している。以下の説明において、縦方向の画素を縦画素、横方向の画素を横画素と称する。

図４及び図５に示すように、次に、ステップＳ２において、画像処理部４１は、視差画像ｄｐから低位置を検出する領域Ａを選択する。領域Ａは、横方向の画素数が視差画像ｄｐの横方向の画素数よりも少なく、縦方向の画素数が視差画像ｄｐの縦方向の画素数と同一となる範囲である。視差画像ｄｐには、領域Ａに含まれない部分が含まれることになり、この部分については低位置の検出が行われない。領域Ａの横方向の画素数は、フォークリフト１０の幅に合わせて設定されており、領域Ａはフォークリフト１０が通過すると推測される範囲であるといえる。

図４及び図６に示すように、次に、ステップＳ３において、画像処理部４１は、視差画像ｄｐの縦画素と視差との二次元配列に、領域Ａ内の各画素に対応付けられた視差を投票する。図６に示すように、二次元配列の行方向は縦画素［ｐｘ］であり、１～縦画素の最大値までの値となる。二次元配列の列方向は視差であり、１～領域Ａ内の最大視差までの値となる。二次元配列は、縦画素が同一であり、横画素が異なる画素に対応付けられた視差の頻度を表すものとなる。即ち、ステップＳ３では、領域Ａ内を、同一の縦画素毎に横方向に分割した場合に、縦画素が同一である範囲毎の視差の頻度が求められるといえる。本実施形態において、画像処理部４１が投票部として機能する。

図４及び図７に示すように、次に、ステップＳ４において、画像処理部４１は、直線Ｌ１，Ｌ２を抽出する。図７に示すように、画像処理部４１は、まず、視差をＸ軸、縦方向の画素をＹ軸とする座標系に、投票数が閾値以上である視差をプロットする。画像処理部４１は、投票数が閾値以上である視差がプロットされた座標系からハフ変換により直線Ｌ１，Ｌ２の抽出を行う。なお、直線Ｌ１，Ｌ２の抽出は、ハフ変換に限られず、LSD：Line Segment Detectorや、RANSAC：Random Sample Consensusなどで行うことができる。ここで、縦画素毎に視差の頻度を求めた場合、水平方向に延びる地面である路面Ｒ１，Ｒ２により生じる視差の頻度が最も多くなる傾向にある。これは、フォークリフト１０の上方から下方を撮像した場合に、画像中に占める路面Ｒ１，Ｒ２の面積が最も大きくなりやすいためである。上記した閾値としては、例えば、各縦画素について、路面Ｒ１，Ｒ２などの地面により生じる視差を抽出できるようにすることが好ましい。ハフ変換により得られた直線Ｌ１，Ｌ２は、縦画素の変化に対するステレオカメラ３１から路面Ｒ１，Ｒ２までの距離を表しているともいえる。なお、路面Ｒ１，Ｒ２上に配置された物体などによって複数の縦画素のうちの一部について、路面Ｒ１，Ｒ２以外の物体により生じた視差が閾値を超える場合も考えられる。この場合であっても、ハフ変換による特徴検出を行うことで、路面Ｒ１，Ｒ２により生じた視差による直線Ｌ１，Ｌ２を得ることができる。本実施形態において、画像処理部４１が直線抽出部として機能する。

図４及び図７に示すように、次に、ステップＳ５において、画像処理部４１は、ステップＳ４で抽出された直線Ｌ１，Ｌ２の傾きと、切片とを算出する。画像処理部４１は、ステップＳ４で複数の直線Ｌ１，Ｌ２が抽出されている場合には、直線Ｌ１，Ｌ２毎に個別に傾きと切片とを算出する。

ステップＳ６において、次に、画像処理部４１は、傾きが類似する直線Ｌ１，Ｌ２が複数あるか否かを判定する。ステレオカメラ３１によって撮像された画像に路面Ｒ１よりも低い低位置が存在しない場合、路面Ｒ１により生じる視差による直線が得られる。この直線は、Ｘ座標が大きいほどＹ座標が大きい単一の直線となる。なお、直線の傾きは、路面Ｒ１の傾斜を表しているといえる。

図３に示すように、第１路面Ｒ１に段差ＳＴが存在する場合、段差ＳＴを境として第１路面Ｒ１よりも低い第２路面Ｒ２が存在することになる。図３には、ステレオカメラ３１の撮像範囲ＩＡを模式的に示している。図に示す撮像範囲ＩＡは、ステレオカメラ３１の垂直画角により定まる範囲であり、縦画素に対応する範囲である。図から把握できるように、第２路面Ｒ２が存在する場合、第２路面Ｒ２は第１路面Ｒ１よりも低いため、第１路面Ｒ１よりも低くなる分だけステレオカメラ３１からの距離が遠くなることになる。また、段差ＳＴにより第２路面Ｒ２の一部が死角となることで、死角の長さｄ１に起因した距離差も生じることになる。即ち、ステレオカメラ３１の画像に段差ＳＴが含まれている場合、段差ＳＴを境としてステレオカメラ３１から第１路面Ｒ１までの距離と、ステレオカメラ３１から第２路面Ｒ２までの距離で大きな差が生じることになる。ステレオカメラ３１の画像に第２路面Ｒ２が写る場合、第２路面Ｒ２が写らない場合に比べてステレオカメラ３１からの距離が大きく変化する箇所が存在することになる。視差は、ステレオカメラ３１からの距離に応じて変化するため、画像に第２路面Ｒ２が存在すると視差にも大きな変化が現れることになる。結果として、ステレオカメラ３１によって第１路面Ｒ１と、第１路面Ｒ１よりも低い第２路面Ｒ２が撮像された場合、第１路面Ｒ１の視差に対応する直線Ｌ２と、第２路面Ｒ２の視差に対応する直線Ｌ１とが個別に抽出されることになる。なお、得られる直線の数は、段差ＳＴの数に合わせて変化することになる。

直線Ｌ１の傾きと直線Ｌ２の傾きが類似しているか否かは、直線Ｌ１の傾きと直線Ｌ２の傾きの差が許容範囲内か否かにより判定することができる。直線Ｌ１，Ｌ２の傾きの差が許容範囲内であれば、直線Ｌ１と直線Ｌ２は傾きが類似しているといえる。なお、許容範囲としては、測定誤差などを加味した上で、直線Ｌ１と直線Ｌ２との傾きが同一とみなせるように設定されている。

図４に示すように、ステップＳ６の判定結果が否定の場合、画像処理部４１は、ステップＳ７において低位置判定フラグをオフにして低位置探索処理を終了する。詳細にいえば、画像処理部４１は、低位置判定フラグがオフの場合であれば低位置判定フラグのオフを維持し、過去の制御周期で低位置判定フラグがオンになっている場合には低位置判定フラグをオフにする。

ステップＳ６の判定結果が肯定の場合、画像処理部４１は、ステップＳ８において切片が異なる直線Ｌ１，Ｌ２が複数あるか否かを判定する。段差ＳＴを境として第１路面Ｒ１と第１路面Ｒ１よりも低い第２路面Ｒ２とが分かれている場合、２つの直線Ｌ１，Ｌ２はＸ軸にずれた直線Ｌ１，Ｌ２となる。２つの直線Ｌ１，Ｌ２がＸ軸にずれると、２つの直線Ｌ１，Ｌ２の切片は異なる値となる。従って、ステップＳ８の判定結果が否定の場合には第１路面Ｒ１よりも低い第２路面Ｒ２が存在しないと判定を行うことができる。

ステップＳ８の判定結果が否定の場合、画像処理部４１は、ステップＳ７の処理を行う。ステップＳ８の判定結果が肯定の場合、画像処理部４１は、ステップＳ９において、傾きが同一であり、切片が異なる複数の直線Ｌ１，Ｌ２から切片が最小である直線Ｌ１を抽出する。次に、ステップＳ１０において、画像処理部４１は、傾きが同一であり、切片が異なる複数の直線Ｌ１，Ｌ２から切片が最大である直線Ｌ２を抽出する。直線Ｌ１，Ｌ２の切片の差は、第１路面Ｒ１と第２路面Ｒ２との高低差に依存しており、第１路面Ｒ１よりも第２路面Ｒ２が低いほど直線Ｌ１，Ｌ２の切片の差は大きくなる。

以下、切片が最大である直線Ｌ１を第１直線とし、切片が最小である直線Ｌ２を第２直線とする。なお、第１直線Ｌ１は、第２路面Ｒ２による視差で得られた直線であり、第２直線Ｌ２は第１路面Ｒ１による視差で得られた直線である。

図４及び図７に示すように、ステップＳ１１において、画像処理部４１は、切片が最小である第１直線Ｌ１の終点座標（ｘ１，ｙ１）を抽出する。終点座標（ｘ１，ｙ１）とは、第１直線Ｌ１のうち縦画素＝Ｙ座標が最も大きい座標である。終点座標（ｘ１，ｙ１）は、画像に写る第２路面Ｒ２のうちステレオカメラ３１に最も近い位置を示している。ステップＳ１２において、画像処理部４１は、切片が最大である第２直線Ｌ２の始点座標（ｘ２，ｙ２）を抽出する。始点座標（ｘ２，ｙ２）とは、第２直線Ｌ２のうち縦画素＝Ｙ座標が最も小さい座標である。始点座標（ｘ２，ｙ２）は、画像に写る第１路面Ｒ１のうちステレオカメラ３１から最も遠い位置を示している。段差ＳＴの座標は、第２直線Ｌ２の始点座標（ｘ２，ｙ２）と同一とみなすことができる。

ステップＳ１３において、画像処理部４１は、第１直線Ｌ１の終点座標（ｘ１，ｙ１）のＸ座標と第２直線Ｌ２の始点座標（ｘ２，ｙ２）のＸ座標との差である｜ｘ２－ｘ１｜の値が所定値以上となるか否かを判定する。｜ｘ２－ｘ１｜は、画像に写る第１路面Ｒ１のうち最もステレオカメラ３１から離れた位置と、画像に写る第２路面Ｒ２のうち最もステレオカメラ３１に近い位置との差が大きいほど大きくなる。｜ｘ２－ｘ１｜は、第１路面Ｒ１と第２路面Ｒ２との高低差が大きいほど大きくなるといえる。従って、｜ｘ２－ｘ１｜は第１路面Ｒ１と第２路面Ｒ２の高低差を表しているといえる。所定値は、第１路面Ｒ１と第２路面Ｒ２の高低差をフォークリフト１０が乗り越え可能か否かを判定するための閾値である。例えば、第１路面Ｒ１と第２路面Ｒ２の高低差がフォークリフト１０の車輪の半径よりも小さければ進行可能であり、半径以上であれば進行不能である。第１路面Ｒ１と第２路面Ｒ２の高低差が車輪の半径と同一であるときの｜ｘ２－ｘ１｜に基づき所定値を設定することで、フォークリフト１０が進行可能かどうかを判定することができる。ステップＳ１３の判定結果が否定の場合、第１路面Ｒ１には進行可能な段差ＳＴが存在しているといえる。ステップＳ１３の判定結果が肯定の場合、第１路面Ｒ１には進行不可能な段差ＳＴが存在しているといえる。本実施形態において、画像処理部４１は、判断部として機能する。

ステップＳ１３の判定結果が否定の場合、画像処理部４１は、ステップＳ７の処理を行う。一方で、ステップＳ１３の判定結果が肯定の場合、画像処理部４１は、ステップＳ１４の処理を行う。ステップＳ１４において、画像処理部４１は、低位置判定フラグをオンにする。詳細にいえば、画像処理部４１は、低位置判定フラグがオフの場合にはオンにし、過去の制御周期で低位置判定フラグがオンにされている場合にはオンを維持し、低位置探索処理を終了する。

低位置判定フラグがオンにされると、車速制限処理が行われる。車速制限処理は、低位置判定フラグがオンの間には継続して行われ、低位置判定フラグがオフになると中止される。以下、車速制限処理について説明する。

図８に示すように、画像処理部４１は、ステップＳ２１において、ステレオカメラ３１から段差ＳＴまでの距離を算出する。画像処理部４１は、カメラ座標系における段差ＳＴの位置を算出する。カメラ座標系は、光軸をＺ軸とし、光軸に直交する２つの軸のそれぞれをＸ軸、Ｙ軸とする３軸直交座標系である。カメラ座標系における段差ＳＴの位置は、カメラ座標系におけるＺ座標Ｚｃ、Ｘ座標Ｘｃ、Ｙ座標Ｙｃで表わすことができる。Ｚ座標Ｚｃ、Ｘ座標Ｘｃ及びＹ座標Ｙｃは、それぞれ、以下の（１）式～（３）式を用いて算出することができる。

ここで、Ｂはステレオカメラ３１の眼間距離＝基線長［ｍ］、ｆは焦点距離［ｍ］、ｄは視差［ｐｘ］である。ｘｐは視差画像ｄｐ中の任意のＸ座標、ｘ’は視差画像ｄｐの中心Ｘ座標である。ｙｐは視差画像ｄｐ中の任意のＹ座標、ｙ’は視差画像ｄｐの中心Ｙ座標である。段差ＳＴの座標に対応する視差を把握することで、（１）～（３）式からカメラ座標系における段差ＳＴの位置を把握することができる。

ここで、フォークリフト１０の進行方向に延びる軸をＹ軸、鉛直方向に延びる軸をＺ軸、Ｙ軸及びＺ軸に直交する軸をＸ軸とする３軸直交座標系での座標を実空間上での座標であるワールド座標とする。実空間上での物体の位置は、実空間上におけるＸ座標Ｘｗ、Ｙ座標Ｙｗ、Ｚ座標Ｚｗで表わすことができる。

画像処理部４１は、以下の（４）式を用いてカメラ座標系をワールド座標系に変換する。

ここで、Ｈはワールド座標系におけるステレオカメラ３１の取り付け高さであり、θはカメラ３２，３３の光軸と、水平面とがなす角＋９０°の角度である。

前述したように、第２直線Ｌ２の始点座標（ｘ２，ｙ２）は、段差ＳＴの座標、言い換えれば、第１路面Ｒ１の終了位置を表している。このため、ｄ＝始点座標（ｘ２，ｙ２）のＸ座標ｘ２、ｘｐ＝ｘ’、ｙｐ＝始点座標（ｘ２，ｙ２）のｙ２とし、（１）～（３）式を用いてカメラ座標系での段差ＳＴの位置を算出することができる。そして、カメラ座標系での段差ＳＴの位置を（４）式を用いてワールド座標に変換することで、実空間上での段差ＳＴの位置を演算することができる。なお、段差ＳＴは、視差画像ｄｐ中で横方向に延びる傾向があることを考えれば、ｘｐの値は、領域Ａ内のＸ座標のうち任意の値としてもよい。

段差ＳＴは、第２直線Ｌ２の始点座標（ｘ２，ｙ２）のＹ座標であるｙ２に対応する実空間上の座標である。従って、画像処理部４１は、第２直線Ｌ２の始点座標（ｘ２，ｙ２）のＹ座標に対応する実空間上の位置よりも遠い位置に第２路面Ｒ２が存在していると判断することができる。

ワールド座標のうちＹ座標Ｙｗは、フォークリフト１０の進行方向に対するフォークリフト１０から段差ＳＴまでの距離、即ち、フォークリフト１０と段差ＳＴとの相対距離を表しているといえる。画像処理部４１は、実空間上での段差ＳＴの座標をメインコントローラ２０に出力する。

次に、ステップＳ２２において、メインコントローラ２０は、画像処理部４１から出力された段差ＳＴの座標からフォークリフト１０から段差ＳＴまでの距離を把握し、当該距離が制限開始距離以下か否かを判定する。制限開始距離とは、フォークリフト１０の最高速度やフォークリフト１０の使用される環境を考慮した上で、フォークリフト１０が段差ＳＴに進入することを抑制できるように設定された距離である。メインコントローラ２０は、ステップＳ２２の判定結果が否定の場合、処理を終了する。一方で、メインコントローラ２０は、ステップＳ２２の判定結果が肯定の場合、ステップＳ２３において速度制限を課す。

図９示すように、速度制限は、フォークリフト１０から段差ＳＴまでの距離が短くなるほど厳しい制限が課される。本実施形態では、フォークリフト１０の到達し得る最高速度に対する速度規制率として速度制限が課される。メインコントローラ２０は、制限開始距離から、速度規制率が最大となる最短距離に向けて徐々に速度規制率を大きくしていく。なお、速度規制率の最大は１００％＝フォークリフト１０の停止であってもよいし、１００％未満の値であってもよい。メインコントローラ２０は、速度規制率に応じた速度を上限として走行制御装置２３に指令を与えることで、フォークリフト１０の速度制限を実施する。これにより、フォークリフト１０が段差ＳＴに進入することで、フォークリフト１０が走行不能になることを抑制できる。

第１実施形態の効果について説明する。
（１－１）画像処理部４１は、ステレオカメラ３１によって得られた画像から視差画像ｄｐを取得し、視差を用いて段差ＳＴを検出している。そして、段差ＳＴにより生じる視差から、フォークリフト１０から段差ＳＴまでの距離を演算することができる。従って、インフラストラクチャーの整備を行うことなく低位置を検出できる。

（１－２）画像処理部４１は、切片の異なる複数の直線Ｌ１，Ｌ２を抽出し、第１直線Ｌ１の終点座標（ｘ１，ｙ１）と第２直線Ｌ２の始点座標（ｘ２，ｙ２）とを抽出している。そして、第１直線Ｌ１の終点座標（ｘ１，ｙ１）のＸ座標と第２直線Ｌ２の始点座標（ｘ２，ｙ２）のＸ座標とを比較することで、段差ＳＴにより生じた第２路面Ｒ２を検出することができる。

（第２実施形態）
以下、位置検出装置の第２実施形態について説明する。なお、以下の説明において、第１実施形態と同様の部分については記載を省略する。

図１０に示すように、第２実施形態では、低位置としてフォークリフト１０の存在する路面Ｒ１１から鉛直方向に凹んだ溝Ｇを検出する。なお、溝Ｇとは、溝Ｇを挟んだ両側の部分Ｒ１１Ａ，Ｒ１１Ｂを仮想的に延長した場合に、両部分Ｒ１１Ａ，Ｒ１１Ｂが起伏なく重なり合う位置をいう。路面Ｒ１１が傾斜していない平面であれば、溝Ｇを挟んだ両側の部分Ｒ１１Ａ，Ｒ１１Ｂには、高低差がない、あるいは、フォークリフト１０を走行させるのに無視できる程度の高低差が存在することになる。

図１１に示すように、画像処理部４１は、ステップＳ１～ステップＳ７までは第１実施形態と同様の処理を行う。以下、第２実施形態の低位置探索処理について第２実施形態の作用とともに説明する。

図１１及び図１２に示すように、画像処理部４１は、ステップＳ１～Ｓ６の処理の次に、ステップＳ３１の処理を行う。ステップＳ３１において、画像処理部４１は、傾きが類似する複数の直線Ｌ１，Ｌ２のうち切片が類似する直線Ｌ１，Ｌ２が複数あるか否かを判定する。切片が類似するか否かは、複数の直線Ｌ１，Ｌ２同士の切片同士の差が許容範囲内か否かにより判定することができる。許容範囲は、測定誤差などを加味した上で、直線Ｌ１，Ｌ２同士の切片が同一とみなせる値に設定される。複数の直線Ｌ１，Ｌ２のうちの１つを第１直線Ｌ１とし、第１直線Ｌ１の終点座標（ｘ１，ｙ１）のＸ座標よりも始点座標（ｘ２，ｙ２）のＸ座標が大きい直線Ｌ２を第２直線Ｌ２とする。傾き及び切片の両方が類似する直線Ｌ１，Ｌ２が抽出されることになる。なお、傾き及び切片の両方が類似する直線が３つ以上存在する場合、３つ以上の直線のうちの任意の２組の直線を第１直線及び第２直線とし、第１直線と第２直線の各組み合わせについて以下の処理を行ってもよい。また、傾き及び切片の両方が類似する直線が３つ以上存在する場合、各直線の始点座標を比較し、最もＹ座標の大きい直線を第２直線、第２直線の次にＹ座標の大きい直線を第１直線としてもよい。Ｙ座標が大きいほど、ステレオカメラ３１に近いため、Ｙ座標の最も大きい直線を用いることで、ステレオカメラ３１に最も近い溝Ｇの検出を行うことができる。

ステップＳ３１の判定結果が否定の場合、画像処理部４１は、ステップＳ７において、低位置判定フラグをオフにする。一方で、ステップＳ３１の判定結果が肯定の場合、画像処理部４１は、ステップＳ３２の処理を行う。

ステップＳ３２において、画像処理部４１は、第３直線Ｌ３が存在するか否かを判定する。第３直線Ｌ３は、第１直線Ｌ１の終点座標（ｘ１，ｙ１）から延び、かつ、Ｙ座標が大きくなるほどＸ座標が小さくなる直線である。第３直線Ｌ３の傾きの正負は、第１直線Ｌ１の傾きの正負とは逆になる。

ここで、図１０に示すように、路面Ｒ１１に溝Ｇが存在している場合、溝Ｇよりフォークリフト１０側の部分Ｒ１１Ａにより生じる視差により、視差はステレオカメラ３１に近付くにつれて徐々に大きくなっていく。ステレオカメラ３１によって溝Ｇが撮像されている場合、各画像には、溝Ｇを区画している壁面のうち溝Ｇよりフォークリフト１０から離れた部分Ｒ１１Ｂに交わる壁面Ｗが写る部分と、部分Ｒ１１Ａが写る部分との境となる画素が存在する。この部分では、路面Ｒ１１が平坦である場合に比べて、ステレオカメラ３１からの距離が大きく変化することになる。従って、溝Ｇよりもフォークリフト１０側の部分Ｒ１１Ａにより１つの直線Ｌ２が得られることになる。この直線Ｌ２が第２直線Ｌ２である。

壁面Ｗは、鉛直方向に延びる面であるため、縦画素が小さくなるにつれてステレオカメラ３１に近付いていく。壁面Ｗが平坦面であれば、視差画像ｄｐの縦画素が小さくなるにつれて、徐々に視差は大きくなっていく。従って、溝Ｇを区画する壁面Ｗにより１つの直線Ｌ３が得られることになる。この直線Ｌ３が第３直線Ｌ３となる。

壁面Ｗと部分Ｒ１１Ｂは、交わっており、両者の交点は画像に写る。画像には、壁面Ｗが写る部分と部分Ｒ１１Ｂが写る部分との境となる画素が存在することになる。部分Ｒ１１Ｂでは縦画素が大きくなるほど視差が大きくなる。また、溝Ｇを挟んだ両部分Ｒ１１Ａ，Ｒ１１Ｂは、溝Ｇがない場合には同一平面となるため、溝Ｇがある場合には単一の直線から溝Ｇの分だけ視差が欠落した直線となる。従って、溝Ｇの分の視差が欠落した部分を除けば、溝Ｇを挟んだ両部分Ｒ１１Ｂ，Ｒ１１Ａにより得られる直線Ｌ１，Ｌ２は同一切片であり同一傾きの直線Ｌ１，Ｌ２となる。従って、第２直線Ｌ２と切片が同一である直線Ｌ１が第１直線Ｌ１となる。第１直線Ｌ１と第３直線Ｌ３の交点とが画像に写ることで、第３直線Ｌ３は第１直線Ｌ１の終点座標（ｘ１，ｙ１）から延びる直線となる。第３直線Ｌ３の始点座標は第１直線Ｌ１の終点座標（ｘ１，ｙ１）と同一座標とみなすことができる。第１直線Ｌ１と第３直線Ｌ３とのなす角は、壁面Ｗと部分Ｒ１１Ｂとのなす角により変化し、壁面Ｗと部分Ｒ１１Ｂとが直角に交わっていれば第１直線Ｌ１と第３直線Ｌ３は直角に交わることになる。

ステップＳ３２の判定結果が否定の場合、画像処理部４１は、ステップＳ７の処理を行う。ステップＳ３２の判定結果が肯定の場合、画像処理部４１は、ステップＳ３３の処理を行う。

ステップＳ３３において、画像処理部４１は、第３直線Ｌ３の終点座標（ｘ３，ｙ３）のＸ座標と第２直線Ｌ２の始点座標（ｘ２，ｙ２）のＸ座標との差である｜ｘ２－ｘ３｜の値が所定値以上か否かを判定する。第３直線Ｌ３の終点座標（ｘ３，ｙ３）のＸ座標と第２直線Ｌ２の始点座標（ｘ２，ｙ２）のＸ座標との差｜ｘ２－ｘ３｜は、両部分Ｒ１１Ａ，Ｒ１１Ｂ間の溝Ｇの長さによって変化する。従って、所定値をフォークリフト１０の進行可能な溝Ｇの長さに基づいて設定することで、溝Ｇが進行可能か否かを判定することができる。ステップＳ３３の判定結果が否定の場合、ステップＳ７において、画像処理部４１は低位置判定フラグをオフにする。一方で、ステップＳ３３の判定結果が肯定の場合、ステップＳ１４において、画像処理部４１は低位置判定フラグをオンにする。

第２実施形態の効果について説明する。第２実施形態では、第１実施形態の効果（１－１）に加えて以下の効果を得ることができる。
（２－１）画像処理部４１は、第１直線Ｌ１及び第２直線Ｌ２に加えて第３直線Ｌ３を抽出することで、低位置は溝Ｇであると判断することができる。

各実施形態は、以下のように変更して実施することができる。各実施形態及び以下の変形例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
○各実施形態において、第１直線Ｌ１と第２直線Ｌ２が抽出され、第１直線Ｌ１の傾きと第２直線Ｌ２の傾きとが許容範囲内であれば、低位置が存在していると判定してもよい。即ち、低位置が第１路面Ｒ１より低い第２路面Ｒ２なのか溝Ｇなのかの判定や、フォークリフト１０による進行が可能か否かの判定を行わなくてもよい。

○第１実施形態と第２実施形態とを組み合わせて、低位置の検出とともに、当該低位置が路面Ｒ１より低い第２路面Ｒ２か溝Ｇかの判定を行ってもよい。詳細にいえば、第１実施形態におけるステップＳ８の判定結果が否定になった場合に、第３直線Ｌ３が存在するかの判定を行い、この判定結果が肯定であれば低位置は溝Ｇであると判断すればよい。このようにすることで、低位置が第１路面Ｒ１より低い第２路面Ｒ２であっても、溝Ｇであっても検出することができる。

○図１３に示すように、視差画像ｄｐの領域Ａを複数の範囲に区分してもよい。図１３に示す例では、領域Ａは、３つの範囲Ａ１～Ａ３に区分されている。３つの範囲Ａ１～Ａ３のうち中央の範囲Ａ２は、フォークリフト１０が進行した場合にフォークリフト１０が通過する範囲である。３つの範囲Ａ１～Ａ３のうち中央以外の範囲Ａ１，Ａ３は、フォークリフト１０が進行した場合にフォークリフト１０の通過する範囲の側方部分となる。この場合、中央の範囲Ａ２に低位置が検出された場合には速度制限、左右の範囲Ａ１，Ａ３に低位置が検出された場合には搭乗者に対する警報を行うなど範囲に合わせて異なる制御を行ってもよい。

○第２実施形態において、画像処理部４１は、溝Ｇの深さを推定してもよい。画像処理部４１は、第３直線Ｌ３の始点座標（ｘ１，ｙ１）のＹ座標と第３直線Ｌ３の終点座標（ｘ３，ｙ３）のＹ座標とを用いて深さを算出する。詳細にいえば、画像処理部４１は第３直線Ｌ３の始点座標（ｘ１，ｙ１）のＹ座標に対応する実空間上の座標と、第３直線Ｌ３の終点座標（ｘ３，ｙ３）のＹ座標に対応する実空間上の座標との差から溝Ｇの深さを推定する。

○各実施形態において、画像処理部４１は、低位置の検出をすることに加えて、フォークリフト１０の走行の妨げとなる障害物や、壁を検出するようにしてもよい。障害物や壁は、視差から検出することができる。

○各実施形態において、速度制限は、段差ＳＴまでの距離に関わらず一定の制限が課されるようにしてもよい。
○各実施形態において、カメラ座標系からワールド座標系への座標変換は、（４）式に代えて、テーブルデータによって行われてもよい。テーブルデータは、Ｙ座標ＹｃとＺ座標Ｚｃの組み合わせにＹ座標Ｙｗを対応させたテーブルデータと、Ｙ座標ＹｃとＺ座標Ｚｃとの組み合わせにＺ座標Ｚｗを対応させたテーブルデータである。これらのテーブルデータを画像処理部４１のＲＯＭなどに記憶しておくことで、カメラ座標系におけるＹ座標ＹｃとＺ座標Ｚｃから、ワールド座標系におけるＹ座標Ｙｗ及びＺ座標Ｚｗを求めることができる。

○各実施形態において、フォークリフト１０から低位置までの距離が制限開始距離以下のときに速度制限に代えて搭乗者への警報が行われるようにしてもよいし、速度制限と警報の両方が行われるようにしてもよい。警報は、例えば、搭乗者の視認可能な表示器への表示や、音によって行われる。

○各実施形態において、第１画像の画素数と第２画像の画素数とは異なっていてもよい。例えば、比較画像である第２画像の画素数を視差画像ｄｐの画素数と同一とし、基準画像である第１画像の画素数を第２画像の画素数よりも多くしてもよい。

○各実施形態において、視差画像取得部、投票部、直線抽出部及び判断部は、それぞれ、個別の制御装置によって構成されていてもよい。
○各実施形態において、第１カメラ３２と第２カメラ３３は、鉛直方向に並んで配置されていてもよい。

○各実施形態において、ステレオカメラ３１は、３つ以上のカメラを備えていてもよい。
○各実施形態において、フォークリフト１０は、エンジンの駆動によって走行するものでもよい。この場合、走行制御装置は、エンジンへの燃料噴射量などを制御する装置となる。

○各実施形態において、位置検出装置３０は、建設機械、自動搬送車、トラックなどフォークリフト１０以外の産業車両や乗用車などの移動体に搭載されていてもよい。

Ｌ１…第１直線、Ｌ２…第２直線、Ｌ３…第３直線、Ｒ１，Ｒ２…路面、Ｇ…溝、１０…フォークリフト（移動体）、３０…位置検出装置、３１…ステレオカメラ、４１…画像処理部（視差画像取得部、投票部、直線抽出部及び判断部）。

Claims (3)

1. 移動体に搭載され、前記移動体の存在する路面よりも低い位置である低位置を検出する位置検出装置であって、
   ステレオカメラと、
   前記ステレオカメラで撮像された画像から各画素に視差が対応付けられた視差画像を取得する視差画像取得部と、
   前記視差画像の縦方向の画素と視差との二次元配列に、前記視差画像の各画素に対応付けられた視差を投票する投票部と、
   前記投票部による投票数が閾値以上である視差がプロットされており、前記視差をＸ軸、前記縦方向の画素をＹ軸とする座標系から直線を抽出する直線抽出部と、
   前記直線抽出部によって、第１直線と、前記第１直線の終点座標のＸ座標よりも始点座標のＸ座標が大きく前記第１直線との傾きの差が許容範囲内の第２直線とが検出された場合、前記始点座標のＹ座標に対応する実空間上の位置よりも遠い位置に低位置が存在していると判断する判断部と、を備える位置検出装置。
2. 前記判断部は、前記第１直線の切片と前記第２直線の切片とが異なり、かつ、前記第１直線の終点座標のＸ座標と前記第２直線の始点座標のＸ座標との差が所定値以上の場合、前記低位置を段差により生じた平面であると判断する請求項１に記載の位置検出装置。
3. 前記判断部は、前記第１直線の切片と前記第２直線の切片との差が許容範囲内であり、
   前記第１直線の終点座標から延び、かつ、Ｙ座標が大きくなるほどＸ座標が小さくなる第３直線が存在し、前記第３直線の終点座標のＸ座標と前記第２直線の始点座標のＸ座標との差が所定値以上の場合、前記低位置を前記路面から凹む溝であると判断する請求項１に記載の位置検出装置。

Images