JP2019089636A - 安全装置 - Google Patents

Abstract

【課題】移動体と障害物との位置関係が把握不能になることを抑止すること。【解決手段】フォークリフトには、安全装置が搭載されている。安全装置は、２つの撮像装置と、画像処理装置と、を備える。各撮像装置はステレオカメラである。撮像装置は、垂直画角の最も上側が水平面よりも上となるように配置されている。画像処理装置は、撮像装置によって撮像された画像から、障害物の位置を算出する。画像処理装置は、撮像装置によって障害物が撮像されなくなった場合、障害物の位置を推定する。【選択図】図３

Description

本発明は、安全装置に関する。

移動体が上方の障害物に衝突することを抑止するため、移動体には上方の障害物を検知するための安全装置が設けられる。
特許文献１の安全装置は、障害物までの距離を検出する距離検出装置を備える。距離検出装置は、上方を向くように配置されている。距離検出装置は、超音波やレーザなどの照射波を照射することで、障害物までの距離を検出する。距離検出装置によって検出される距離から安全装置は、障害物を検知することができる。

特許文献２の安全装置は、ステレオカメラを備える。安全装置は、ステレオカメラによって撮像される画像から、移動体が衝突する障害物を検知することができる。

特開２００４−９１１１８号公報 特開２０１３−１１４６１０号公報

ところで、特許文献１の安全装置では、移動体の移動に伴い、照射波が障害物に当たらなくなる場合がある。この場合、安全装置は、障害物を検知することができなくなる。また、特許文献２の安全装置についても、移動体の移動に伴い障害物がステレオカメラの垂直画角から外れると、障害物を検知することができない。すると、安全装置は、移動体と障害物との位置関係を把握することができず、移動体と障害物とが衝突するおそれがある。

本発明の目的は、移動体と障害物との位置関係が把握不能になることを抑止できる安全装置を提供することにある。

上記課題を解決する安全装置は、移動体に搭載される安全装置であって、垂直画角の最も上側が水平面よりも上となるように配置されたステレオカメラと、前記ステレオカメラによって撮像された画像から、障害物の位置を算出する算出手段と、前記障害物の位置情報を記憶する記憶手段と、前記移動体の移動に伴い、前記障害物が前記ステレオカメラによって撮像されなくなった場合、前記記憶手段によって記憶された前記位置情報に基づき、前記障害物の位置を推定する推定手段と、を備える。

これによれば、ステレオカメラの垂直画角の最も上側が水平面よりも上となることで、ステレオカメラは移動体の上方を撮像可能である。算出手段によって算出された障害物の位置は位置情報として記憶手段によって記憶される。ステレオカメラによって障害物が撮像されなくなると、障害物の位置が推定手段によって推定される。これにより、障害物がステレオカメラによって撮像されなくなった場合であっても、安全装置は移動体と障害物との位置関係を把握できる。したがって、移動体と障害物との位置関係が把握不能になることを抑止できる。

上記安全装置について、前記推定手段は、前記移動体の速度、及び、前記移動体の進行方向から前記障害物の位置を推定してもよい。
これによれば、推定手段は、ステレオカメラによって障害物が撮像されなくなった場合であっても、移動体の速度、及び、進行方向から障害物の位置を推定することができる。

上記安全装置について、前記推定手段は、前記障害物が前記ステレオカメラによって撮像されているときに前記障害物とともに撮像されており、かつ、前記移動体の移動に伴い前記障害物が撮像されなくなった場合にも撮像されているものを基準物とし、前記移動体の移動に伴う前記基準物と前記移動体との相対位置の変化から前記障害物の位置を推定してもよい。

これによれば、推定手段は、ステレオカメラによって障害物と基準物とが撮像されている際に障害物と基準物との位置関係を把握することで、基準物と移動体との相対位置の変化から障害物の位置を推定できる。

上記安全装置について、前記障害物の上下方向への移動速度を検出し、前記移動速度で前記障害物が移動し続けた場合に前記障害物と前記移動体とが衝突するか否かを判定する衝突判定手段を備えていてもよい。

これによれば、障害物の移動速度を加味した上で、障害物と移動体とが衝突するか否かを判定することができる。

本発明によれば、移動体と障害物との位置関係が把握不能になることを抑止できる。

安全装置が搭載されるフォークリフトの側面図。 フォークリフト、及び、安全装置の概略構成図。 第１実施形態における障害物を検知するための処理を示すフローチャート。 相対位置算出処理を示すフローチャート。 フォークリフトと、撮像装置に撮像される障害物とを示す模式図。 ワールド座標系に対象物をプロットした図。 フォークリフトと、撮像装置によって撮像されなくなった障害物とを示す模式図。 撮像装置によって障害物が撮像されなくなった場合のワールド座標系を示す図。 第２実施形態における障害物を検知するための処理を示すフローチャート。 第２実施形態における障害物の移動推定をするための処理を示すフローチャート。 変形例における障害物の検知手段を説明するための図。

（第１実施形態）
以下、安全装置の第１実施形態について説明する。
図１及び図２に示すように、移動体としてのフォークリフト１０は、車体１１と、車体１１の前部に設けられた荷役装置１２と、を備える。フォークリフト１０は、駆動輪１３と、操舵輪１４と、駆動輪１３を駆動させる走行用モータＭ１と、を備える。なお、フォークリフト１０は、自動で走行動作及び荷役動作が行われるものであってもよいし、搭乗者（オペレータ）による操作によって走行動作及び荷役動作が行われるものであってもよい。

荷役装置１２は、マスト１５を備える。マスト１５は、車体１１に対して前後に傾動可能に支持されたアウタマスト１６と、アウタマスト１６に対して昇降可能なインナマスト１７と、を備える。フォークリフト１０は、マスト１５を傾動させるティルトシリンダ１８と、インナマスト１７を昇降させるリフトシリンダ１９と、を備える。ティルトシリンダ１８及びリフトシリンダ１９は油圧シリンダである。荷役装置１２は、インナマスト１７とともに昇降する荷役具（例えば、フォークや、ロールクランプ装置などのアタッチメント）Ｌを備える。

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

メインコントローラ２０のＣＰＵ２１は、フォークリフト１０の車速が目標速度となるように走行制御装置２３に走行用モータＭ１の回転数の指令を与える。メインコントローラ２０は、走行制御装置２３に走行用モータＭ１を回転させる方向の指令を与えることで、フォークリフト１０を進行させる方向（前進又は後進）を制御する。

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

揚高センサ２５は、例えば、リール式の揚高センサである。揚高センサ２５は、一端が荷役具Ｌに接続された図示しないワイヤと、そのワイヤが巻き掛けられる図示しないリールと、リールの回転量を検出するための回転検出器（ロータリーエンコーダ）と、を備える。揚高センサ２５は、検出結果をメインコントローラ２０に出力する。

ハンドル角センサ２６は、ハンドルに配設されており、ハンドルの操作量（角度）を検出する。ハンドル角センサ２６は、検出結果をメインコントローラ２０に出力する。メインコントローラ２０は、ハンドルの操作量（角度）に応じた舵角となるように操舵輪１４の向きを制御する。

フォークリフト１０には、安全装置３０が搭載されている。安全装置３０は、２つの撮像装置３１，４１を備える。２つの撮像装置３１，４１のうちの一方は、前方撮像装置３１であり、他方は後方撮像装置４１である。前方撮像装置３１は、フォークリフト１０の前方を撮像するステレオカメラである。後方撮像装置４１は、フォークリフト１０の後方を撮影するステレオカメラである。

各撮像装置３１，４１は、車体１１の上部に配置されている。前方撮像装置３１は、２つのカメラ３２，３３を備える。２つのカメラ３２，３３のうち一方を第１カメラ３２、他方を第２カメラ３３とする。カメラ３２，３３としては、例えば、ＣＣＤイメージセンサや、ＣＭＯＳイメージセンサが用いられる。各カメラ３２，３３は、互いの光軸が平行となるように配置されている。なお、各カメラ３２，３３の光軸は、水平であってもよいし、水平でなくてもよい。後方撮像装置４１は、前方撮像装置３１と同一構成であり、第１カメラ４２と、第２カメラ４３と、を備える。以下の説明において、第１カメラ３２，４２によって撮像された画像を第１画像、第２カメラ３３，４３によって撮像された画像を第２画像として説明を行う。

前方撮像装置３１（第１カメラ３２及び第２カメラ３３）の垂直画角θ１の最も上側Ｕ１、即ち、前方撮像装置３１の撮像範囲の最も上側は水平面よりも上となっている。同様に、後方撮像装置４１（第１カメラ４２及び第２カメラ４３）の垂直画角θ２の最も上側Ｕ２は、水平面よりも上となっている。これにより、各撮像装置３１，４１は、取り付け位置よりも上方を撮像可能である。

安全装置３０は、各撮像装置３１，４１によって撮像された画像から障害物を検知する画像処理装置５０を備える。画像処理装置５０は、ＣＰＵ、ＲＡＭ、ＲＯＭ等から構成されている。画像処理装置５０は、上方の障害物を検知する。なお、上方の障害物とは、フォークリフト１０が走行する床よりも上方に位置しており、フォークリフト１０の上部が衝突するおそれのある障害物である。上方の障害物としては、例えば、梁や、シャッターが挙げられる。

画像処理装置５０は、図３に示す処理を所定の制御周期毎に繰り返し行う。以下、画像処理装置５０の行う処理について安全装置３０の作用とともに説明する。
図３に示すように、画像処理装置５０は、撮像装置３１，４１によって撮像された画像から、視差を算出する（ステップＳ１０）。視差は、撮像装置３１，４１のそれぞれについて第１画像と、第２画像とを比較し、各画像に写る同一の対象物について第１画像と第２画像の画像位置（ピクセル）の差を算出することで得られる。画像位置の差は、第１画像に写る対象物と、第２画像に写る対象物との画素（ピクセル）数の差［ｐｘ］である。本実施形態において、視差は視差画像として得られる。視差画像とは、撮像装置３１，４１によって撮像された画像に写る対象物と、当該対象物の視差とを対応付けた画像である。視差画像には、画素毎に視差が対応付けられている。視差画像の画素数は、第１画像（第２画像）の画素数と同一である。なお、視差画像とは、必ずしも表示を要するものではなく、視差画像における各画素（視差画像におけるＸ座標及びＹ座標）に視差が対応付けられたデータのことを示す。

なお、対象物とは、各画像に含まれる特徴部分であり、例えば、物体のエッジなど、境目として認識可能な部分である。対象物は、輝度情報などから検出することができ、画像処理装置５０は、対象物から視差を算出することができる。

画像処理装置５０は、視差からフォークリフト１０の周囲に存在する対象物の位置を算出する（ステップＳ１１）。まず、画像処理装置５０は、各撮像装置３１，４１によって得られた視差画像からカメラ座標系における対象物の位置を算出する。カメラ座標系は、光軸をＺ軸とし、光軸に直交する２つの軸のそれぞれをＸ軸、Ｙ軸とする３軸直交座標系である。カメラ座標系における対象物の位置は、カメラ座標系におけるＺ座標Ｚｃ、Ｘ座標Ｘｃ、Ｙ座標Ｙｃで表わすことができる。Ｚ座標Ｚｃ、Ｘ座標Ｘｃ、及び、Ｙ座標Ｙｃは、それぞれ、以下の（１）式〜（３）式を用いて算出することができる。

ここで、Ｂは各撮像装置３１，４１の眼間距離［ｍ］、ｆは焦点距離［ｍ］、ｄは視差［ｐｘ］である。ｘｐは視差画像中の任意のＸ座標（任意の画素の横ピクセル）、ｘ’は視差画像中の中心Ｘ座標である。ｙｐは視差画像中の任意のＹ座標（任意の画素の縦ピクセル）、ｙ’は視差画像中の中心Ｙ座標である。視差画像中の全ての対象物について、上記した（１）式〜（３）式の計算は行われる。

ここで、フォークリフト１０の前後方向に延びる軸を第１の軸としてのＹ軸、鉛直方向に延びる軸を第２の軸としてのＺ軸、Ｙ軸及びＺ軸に直交する軸を第３の軸としてのＸ軸とする３軸直交座標系（ワールド座標系）での座標を実空間上での座標（ワールド座標）とする。実空間上での対象物の位置は、実空間上におけるＸ座標Ｘｗ、Ｙ座標Ｙｗ、Ｚ座標Ｚｗで表わすことができる。

各カメラ３２，３３，４２，４３の光軸が水平（鉛直方向と直交）となるように撮像装置３１，４１が設置されている場合、Ｘｃ＝Ｘｗ、Ｙｃ＝Ｚｗ、Ｚｃ＝Ｙｗとなる。即ち、カメラ座標系における対象物の位置が実空間上での対象物の位置となる。

一方で、各カメラ３２，３３、４２，４３の光軸が水平（鉛直方向と直交）ではない場合、画像処理装置５０は、以下の（４）式を用いて座標変換（ワールド座標変換）を行う。

ここで、Ｈはワールド座標系における撮像装置３１，４１の取り付け高さであり、θはカメラ３２，３３、４２，４３の光軸と、水平面とがなす角＋９０°の角度である。本実施形態において、画像処理装置５０は、算出手段として機能する。

次に、画像処理装置５０は、進行方向に障害物があるか否かの判定を行う（ステップＳ１２）。なお、進行方向は、例えば、車速センサ２４によって検出される走行用モータＭ１の回転方向、及び、ハンドル角センサ２６によって検出される角度から把握することができる。障害物の有無の判定は、ワールド座標系に対象物をプロットすることで行われる。

一例として、図５に示すように、フォークリフト１０が後進している場合で、フォークリフト１０の後方にマスト１５に衝突する高さの障害物（シャッター）Ｂが存在している場合の障害物検知について説明する。

図６に示すように、フォークリフト１０の後方の領域について、ワールド座標系に対象物の位置をプロットする。ワールド座標系にプロットされた対象物の位置を特徴点Ｐとする。特徴点Ｐとは、ワールド座標系において、視差（対象物の位置）を得られた座標といえる。なお、図６では、障害物の下端を対象物としてプロットしている。

画像処理装置５０は、対象物のうちフォークリフト１０に衝突するおそれのあるものを障害物と判断する。なお、障害物か否かの判定は、対象物のＺ座標Ｚｗから判定することができる。画像処理装置５０は、フォークリフト１０の高さ（例えば、インナマスト１７の最高到達点）と、対象物の高さとを比較することで、対象物が障害物か否かを判定する。

図３に示すように、ステップＳ１２の判定結果が肯定の場合、画像処理装置５０はステップＳ１３の処理を行う。一方で、ステップＳ１２の判定結果が否定の場合、画像処理装置５０はステップＳ１４の処理を行う。まず、ステップＳ１２の判定結果が肯定の場合について説明する。

画像処理装置５０は、障害物をクラスタ化し、クラスタ化した障害物の位置情報（座標）を保存（記憶）する（ステップＳ１３）。クラスタ化とは、障害物となる対象物をプロットした複数の特徴点Ｐの集合体を１つの障害物Ｂとみなすことである。例えば、図６のように、Ｘ軸上に密集する特徴点Ｐは、１つの障害物Ｂとして扱う。障害物Ｂの位置情報（ワールド座標）は、ＲＡＭや、ＥＥＰＲＯＭなどの書き換え可能なメモリに一時的に保存（記憶）される。なお、障害物Ｂの位置情報は、クラスタ化された障害物Ｂの座標に基づいて定められる。例えば、クラスタ化された複数の特徴点Ｐのうちから任意の特徴点Ｐの座標を障害物Ｂの位置情報としてもよいし、クラスタ化された障害物Ｂの中心座標を位置情報としてもよい。画像処理装置５０は、記憶手段として機能している。

図３に示すように、画像処理装置５０は、相対位置算出処理を行う（ステップＳ２０）。
図４に示すように、相対位置算出処理において、画像処理装置５０は、フォークリフト１０と、障害物Ｂとの相対位置を算出する（ステップＳ２１）。相対位置は、障害物ＢのＹ座標Ｙｗから算出することができる。画像処理装置５０は、算出した相対位置をメインコントローラ２０に出力する。

メインコントローラ２０は、フォークリフト１０と障害物Ｂとの相対距離が所定値以下か否かを判定する（ステップＳ２２）。所定値とは、例えば、フォークリフト１０の最高速度や、制動性能などに基づき定められる値である。

ステップＳ２２の判定結果が否定の場合、メインコントローラ２０は処理を終了する。ステップＳ２２の判定結果が肯定の場合、メインコントローラ２０は減速処理を行う（ステップＳ２３）。減速処理としては、フォークリフト１０を停止させる処理、フォークリフト１０の速度を現在速度から段階的に低下させる処理、フォークリフト１０の速度に制限を課し、制限速度を上回る速度での走行を規制する処理など、様々な処理が挙げられる。

次に、ステップＳ１２において、障害物Ｂが検知されなかった場合の処理について説明する。
図３に示すように、ステップＳ１２の判定結果が否定の場合、画像処理装置５０はステップＳ１４の処理を行う。画像処理装置５０は、１周期前の処理で、障害物Ｂが検知されていたか否かを判定する（ステップＳ１４）。

図７、及び、図８に示すように、図５の状態からフォークリフト１０が障害物Ｂに近付くと、後方撮像装置４１の垂直画角θ２から障害物Ｂが外れる。すると、後方撮像装置４１によって障害物Ｂが撮像されなくなる。後方撮像装置４１によって障害物Ｂが撮像されなくなると、障害物Ｂが撮像されていたときにプロットされていた特徴点Ｐはプロットされなくなる。即ち、一度検知した障害物Ｂが検知されなくなる（検知ロストする）。ステップＳ１４の判定結果が肯定の場合、フォークリフト１０が移動したことで、障害物Ｂが撮像装置３１，４１の垂直画角θ１，θ２から外れたと判断できる。

図３に示すように、ステップＳ１４の判定結果が肯定の場合、画像処理装置５０は、ステップＳ１５の処理を行う。画像処理装置５０は、位置推定フラグをオンにする（ステップＳ１５）。次に、画像処理装置５０は、相対位置算出処理を行う（ステップＳ２０）。この際の相対位置算出処理では、ステップＳ２１における相対位置の算出の際に、ステップＳ１３で記憶された位置情報、フォークリフト１０の進行方向、及び、フォークリフト１０の速度に基づいて相対位置が算出される。フォークリフト１０の進行方向は、走行用モータＭ１の回転方向及びハンドル角から把握できる。フォークリフト１０の進行方向と、速度とを把握できると、障害物Ｂを最後に検知した位置からフォークリフト１０が移動した方向と、移動した距離とを把握できる。このため、ステップＳ１３で記憶された障害物Ｂの位置にフォークリフト１０の移動した方向と移動した距離とを加味した上で、障害物Ｂとフォークリフト１０との相対位置を算出することができる。なお、１回の制御周期の間にフォークリフト１０が進む距離が僅かであることを考慮すれば、フォークリフト１０と障害物Ｂとの相対位置は、１回前の制御周期で算出されたものと同一とみなしてもよい。なお、以下の説明において、フォークリフト１０の進行方向と、フォークリフト１０の速度とを車両情報として説明を行う。

ステップＳ１４の判定結果が否定の場合、画像処理装置５０は、ステップＳ１６の処理を行う。画像処理装置５０は、位置推定フラグがオンか否かを判定する（ステップＳ１６）。ステップＳ１６の判定結果が肯定の場合、一度検出された障害物Ｂが、フォークリフト１０が移動したことで検出されなくなったと判断できる。一方で、ステップＳ１６の判定結果が否定の場合、障害物Ｂの検出が行われていないと判断できる。画像処理装置５０は、ステップＳ１６の判定結果が否定の場合、フォークリフト１０が衝突する障害物Ｂは存在しないと判断し、処理を終了する。画像処理装置５０は、ステップＳ１６の判定結果が肯定の場合、ステップＳ１７の処理を行う。

画像処理装置５０は、障害物Ｂの位置推定を行う（ステップＳ１７）。画像処理装置５０は、ステップＳ１３で記憶された障害物Ｂの位置情報、及び、車両情報から障害物Ｂの位置を推定する。

画像処理装置５０は、車両情報から、障害物Ｂを最後に検出した制御周期から、今回の制御周期までに、フォークリフト１０が移動した方向と、移動した距離を算出する。フォークリフト１０が移動すると、障害物Ｂの位置（ワールド座標）も変化する。画像処理装置５０は、ステップＳ１３で記憶された位置情報に車両情報によって得られたフォークリフト１０の移動方向と、移動した距離とを加味して、障害物Ｂの現在の位置（ワールド座標）を推定する。これにより、撮像装置３１，４１によって障害物Ｂが撮像されなくなっていても、障害物Ｂの位置が推定される。画像処理装置５０は、推定手段として機能する。

次に、画像処理装置５０は、相対位置算出処理を行う（ステップＳ２０）。この際の相対位置算出処理では、ステップＳ２１における相対位置の算出の際に、ステップＳ１７で推定された障害物Ｂの位置から、フォークリフト１０と障害物Ｂとの相対位置が算出される。

なお、本実施形態において、位置推定フラグは、検出された障害物Ｂをフォークリフト１０が通過したと推定されたとき、減速処理が行われたとき、ステップＳ１２の判定結果が肯定となったときなどにオフされる。

したがって、上記実施形態によれば以下の効果を得ることができる。
（１）画像処理装置５０は、フォークリフト１０の移動に伴い、障害物Ｂが撮像装置３１，４１によって撮像されなくなった場合、障害物Ｂの位置を推定する。このため、障害物Ｂが撮像装置３１，４１によって撮像されなくなった場合であっても、フォークリフト１０と障害物Ｂとの相対位置を把握できる。したがって、フォークリフト１０と障害物Ｂとの位置関係が把握不能になることが抑止される。

（２）画像処理装置５０は、車両情報から、障害物Ｂを最後に検知した位置からフォークリフト１０が移動した方向と、移動した距離とを把握できる。これを利用することで、画像処理装置５０は、障害物Ｂの位置を推定することができる。

（３）撮像装置３１，４１は、垂直画角θ１，θ２の範囲に写る障害物Ｂを撮像する。このため、超音波や、レーザなどで距離を計測する距離計によって障害物までの距離を検出し、これにより障害物を検知する場合に比べて、障害物を検知できる範囲が広い。

また、距離計によって障害物を検知する場合、超音波やレーザを障害物に照射するため、フォークリフト１０の上部に距離計を配置する必要がある。これに対し、撮像装置３１，４１により障害物を検知する場合、障害物を検知できる範囲が広いため、フォークリフト１０の上部以外に撮像装置３１，４１を配置できる。即ち、配置上の制約が少ない。

（第２実施形態）
以下、安全装置の第２実施形態について説明する。なお、第２実施形態の安全装置は、障害物の検知に関する処理が第１実施形態とは異なる。以下の説明において、第１実施形態と同様な記載については、同一符号を付すことで説明を省略、あるいは、簡略する。

図９に示すように、画像処理装置５０は、第１実施形態と同様に、視差を算出し（ステップＳ１０）、視差を用いて対象物の位置を算出する（ステップＳ１１）。次に、画像処理装置５０は、マスト１５（インナマスト１７）の高さを取得する（ステップＳ３１）。マスト１５の高さは、揚高センサ２５の検出結果を取得することで、把握できる。なお、揚高センサ２５の検出結果は、揚高センサ２５から取得するようにしてもよいし、メインコントローラ２０を介して取得するようにしてもよい。

次に、画像処理装置５０は、進行方向に障害物Ｂがあるか否かを判定し（ステップＳ１２）、障害物Ｂがある場合には、クラスタ化・保存を行う（ステップＳ１３）。
次に、画像処理装置５０は、障害物Ｂの移動推定を行う（ステップＳ４０）。障害物Ｂの移動推定は、障害物Ｂが移動（上下動）しているか否かを判定し、障害物Ｂが移動している場合には移動速度を算出する処理である。フォークリフト１０などの接近を検知して、自動で開閉するシャッターなど、障害物Ｂによっては、上下方向の位置が変動するものがある。この場合、障害物Ｂの上下方向の位置によって、フォークリフト１０に衝突するか否かが異なるため、障害物Ｂが移動しているか否かを判定する。

図１０に示すように、障害物Ｂの移動推定において、画像処理装置５０は、障害物Ｂが動いているか否かを判定する（ステップＳ４１）。なお、障害物Ｂの移動推定において判定される動きは、上下方向の動きの有無であり、水平方向の動きの有無は判定されない。ステップＳ４１の判定結果が否定の場合、画像処理装置５０は障害物Ｂの移動推定の処理を終了する。

ステップＳ４１の判定結果が肯定の場合、画像処理装置５０は、障害物Ｂの移動速度と、障害物Ｂの移動方向（上又は下）とを算出する（ステップＳ４２）。障害物Ｂが移動しているか否かの判定、障害物Ｂの移動方向及び移動速度の算出は、所謂オプティカルフローによって行われる。

図９に示すように、障害物Ｂの移動推定が終了すると、画像処理装置５０は、相対位置算出処理を行う（ステップＳ２０）。相対位置算出処理は、第１実施形態と同様の処理である。

ステップＳ１２の判定結果が否定の場合、画像処理装置５０は、１周期前の処理で、障害物Ｂが検知されていたか否かを判定する（ステップＳ１４）。ステップＳ１４の判定結果が否定の場合、画像処理装置５０は、位置推定フラグがオンか否かを判定する（ステップＳ１６）。ステップＳ１６の判定結果が肯定の場合、画像処理装置５０はステップＳ３２の処理を行う。

ステップＳ３２において、画像処理装置５０は、障害物Ｂの高さを推定する。障害物Ｂの高さは、ステップＳ４０で算出された移動速度から推定できる。画像処理装置５０は、障害物Ｂが検知されなくなってから経過した時間と、ステップＳ４０算出された移動速度及び移動方向から、障害物Ｂが撮像されなくなってからの障害物Ｂの移動量を算出する。この移動量をステップＳ１３にて記憶された障害物Ｂの高さに加えることで、障害物Ｂの高さを推定することができる。なお、ステップＳ４１の判定結果が否定だった場合、即ち、障害物Ｂが移動していないと判定されていた場合、障害物Ｂの高さは、ステップＳ１３にて記憶された高さと同一と推定される。

次に、画像処理装置５０は、障害物Ｂの位置を推定する（ステップＳ１７）。次に、画像処理装置５０は、ステップＳ３２で推定された障害物Ｂの高さが、マスト１５の高さ以下か否かを判定する（ステップＳ３４）。

ステップＳ３４の判定結果が否定の場合、障害物Ｂが上昇することで、障害物Ｂがマスト１５に衝突しなくなったと判断できる。一方で、ステップＳ３４の判定結果が肯定の場合、障害物Ｂとマスト１５とが衝突すると判断できる。画像処理装置５０は、ステップＳ３４の判定結果が否定の場合、位置推定フラグをオフにし（ステップＳ３５）、処理を終了する。一方で、ステップＳ３４の判定結果が肯定の場合、画像処理装置５０は、相対位置算出処理（ステップＳ２０）を行う。画像処理装置５０は、衝突判定手段として機能している。

したがって、上記実施形態によれば、第１実施形態の各効果に加えて、以下の効果を得ることができる。
（２−１）障害物Ｂが上下方向に移動するか否かを判定し、移動する場合には移動速度を検出している。障害物Ｂが撮像装置３１，４１によって撮像されなくなった場合には、障害物Ｂの移動速度から障害物Ｂの高さを推定できる。

障害物Ｂが上下方向に移動する動体の場合、撮像装置３１，４１によって撮像されている間はフォークリフト１０に衝突しない位置にあった下降途中の障害物Ｂが、撮像装置３１，４１によって撮像されなくなった後に、フォークリフト１０に衝突する位置まで下降している場合がある。この場合、障害物Ｂの位置を推定したにも関わらず、フォークリフト１０と障害物Ｂとが衝突するおそれがある。更に、撮像装置３１，４１によって撮像されている間はフォークリフト１０に衝突する位置にあった上昇途中の障害物Ｂが、撮像装置３１，４１によって撮像されなくなった後に、フォークリフト１０に衝突しない位置まで上昇している場合がある。この場合、フォークリフト１０が障害物Ｂに衝突しないにも関わらず減速処理が行われ、作業効率が低下する場合がある。

これに対して、障害物Ｂの移動速度を加味して障害物Ｂの高さを推定することで、フォークリフト１０と障害物Ｂが衝突することを抑制でき、障害物Ｂと衝突しないにも関わらず減速処理が行われることも抑制できる。

なお、実施形態は以下のように変更してもよい。
○図１１に示すように、画像処理装置５０は、基準物Ｎと障害物Ｂとの相関関係から障害物Ｂの位置を推定してもよい。基準物Ｎは、撮像装置３１，４１によって障害物Ｂが撮像されているときに障害物Ｂとともに撮像されており、かつ、フォークリフト１０の移動に伴い障害物Ｂが撮像されなくなった場合にも撮像されているものである。基準物Ｎは、白線や標識などの移動しないもの、即ち、障害物Ｂとの位置関係が変化しないものが好ましい。

画像処理装置５０は、障害物Ｂ及び基準物Ｎが撮像されているときに、両者の位置を算出する。そして、障害物Ｂの位置と、基準物Ｎの位置とを対応付ける。撮像装置３１，４１によって障害物Ｂが撮像されなくなった場合も、画像処理装置５０は、基準物Ｎの位置（フォークリフト１０との相対位置）を算出し続ける。また、画像処理装置５０は、基準物Ｎの位置と、障害物Ｂとの位置関係から障害物Ｂの位置を推定する。障害物Ｂが撮像されなくなった場合であっても、フォークリフト１０と基準物Ｎとの相対位置の変化を監視し続けることで、障害物Ｂの位置を推定することができる。

なお、基準物Ｎによる障害物Ｂの位置推定と、車両情報による障害物Ｂの位置推定とを組み合わせて障害物Ｂの位置を推定してもよい。例えば、基準物Ｎによる障害物Ｂの位置推定と、車両情報による位置推定の両方を行い、両者の一致性などを確認してもよい。

○各実施形態において、フォークリフト１０と障害物Ｂとの相対距離が所定値以下となった場合、メインコントローラ２０は、その旨を表示部に表示したり、ブザー音を鳴らすことで報知を行ってもよい。

○各実施形態において、撮像装置３１，４１として、３つ以上のカメラを備えるものを用いてもよい。
○各実施形態において、移動体としては、乗用車、建設機械、自動搬送車、トラックなどの車両や、二足歩行式のロボットであってもよい。なお、移動体としては、屋内を移動するものであることが好ましい。

○各実施形態において、前方撮像装置３１と、後方撮像装置４１とは同一構成でなくてもよい。
○各実施形態において、フォークリフト１０の進行方向は、車速センサ２４の検出結果、及び、操舵輪１４の操舵角などから判定されてもよい。

○各実施形態において、撮像装置３１，４１によって撮像された画像の画像処理を行う装置として、画像処理装置５０を設けたが、画像処理をメインコントローラ２０などに行わせてもよい。即ち、走行動作や荷役動作に関する制御を行いメインコントローラ２０の１つの機能として画像処理が行われるようにしてもよい。

○各実施形態において、画像処理装置５０が算出手段、記憶手段、推定手段、及び、衝突判定手段として機能するようにしたが、それぞれの手段として機能する個別の装置を設けてもよい。

○各実施形態において、走行駆動装置として、エンジンが用いられてもよい。すなわち、フォークリフト１０は、エンジンの駆動によって走行してもよい。この場合、走行制御装置は、エンジンへの燃料噴射量などを制御する装置となる。

○各実施形態において、前進のみしか行えない移動体の場合、後方撮像装置４１を設けなくてもよい。
○各実施形態において、搭乗者が搭乗可能な移動体に安全装置３０が搭載される場合、搭乗者による視認が行われにくい後方の障害物のみを検出してもよい。この場合、前方撮像装置３１を設けなくてもよい。

○各実施形態において、前方撮像装置３１は、フォークリフト１０の前方を撮像できれば、どのような位置に配置されていてもよい。後方撮像装置４１は、フォークリフト１０の後方を撮像できれば、どのような位置に配置されていてもよい。例えば、前方撮像装置３１は、マスト１５に配置されていてもよいし、後方撮像装置４１は、車体１１の後方に設けられるカウンタウェイトに配置されていてもよい。前方撮像装置３１がマスト１５に配置される場合、マスト１５が最も前傾した状態で、前方撮像装置３１の垂直画角θ１の上側が水平面よりも上側となるようにする。

○各実施形態において、座標変換は、（４）式に代えて、テーブルデータによって行われてもよい。テーブルデータは、Ｙ座標ＹｃとＺ座標Ｚｃの組み合わせにＹ座標Ｙｗを対応させたテーブルデータと、Ｙ座標ＹｃとＺ座標Ｚｃとの組み合わせにＺ座標Ｚｗを対応させたテーブルデータである。これらのテーブルデータを画像処理装置５０のＲＯＭなどに記憶しておくことで、カメラ座標系におけるＹ座標ＹｃとＺ座標Ｚｃから、ワールド座標系におけるＹ座標Ｙｗ及びＺ座標Ｚｗを求めることができる。なお、実施形態では、カメラ座標系におけるＸ座標Ｘｃと、ワールド座標系におけるＸ座標Ｘｗとは一致するため、Ｘ座標Ｘｗを求めるためのテーブルデータは記憶されない。

Ｎ…基準物、θ１，θ２…垂直画角、１０…フォークリフト（移動体）、３０…安全装置、３１，４１…撮像装置（ステレオカメラ）、５０…画像処理装置（算出手段、記憶手段、推定手段、及び、衝突判定手段）。

Claims (4)

1. 移動体に搭載される安全装置であって、
   垂直画角の最も上側が水平面よりも上となるように配置されたステレオカメラと、
   前記ステレオカメラによって撮像された画像から、障害物の位置を算出する算出手段と、
   前記障害物の位置情報を記憶する記憶手段と、
   前記移動体の移動に伴い、前記障害物が前記ステレオカメラによって撮像されなくなった場合、前記記憶手段によって記憶された前記位置情報に基づき、前記障害物の位置を推定する推定手段と、を備える安全装置。
2. 前記推定手段は、前記移動体の速度、及び、前記移動体の進行方向から前記障害物の位置を推定する請求項１に記載の安全装置。
3. 前記推定手段は、前記障害物が前記ステレオカメラによって撮像されているときに前記障害物とともに撮像されており、かつ、前記移動体の移動に伴い前記障害物が撮像されなくなった場合にも撮像されているものを基準物とし、
   前記移動体の移動に伴う前記基準物と前記移動体との相対位置の変化から前記障害物の位置を推定する請求項１又は請求項２に記載の安全装置。
4. 前記障害物の上下方向への移動速度を検出し、前記移動速度で前記障害物が移動し続けた場合に前記障害物と前記移動体とが衝突するか否かを判定する衝突判定手段を備える請求項１〜請求項３のうちいずれか一項に記載の安全装置。