ホーム > 特許ランキング > 三菱ロジスネクスト株式会社
知財求人 - 知財ポータルサイト「IP Force」
特許7615104位置推定システム、フォークリフト、位置推定方法及びプログラム
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2025-01-07
(45)【発行日】2025-01-16
(54)【発明の名称】位置推定システム、フォークリフト、位置推定方法及びプログラム
(51)【国際特許分類】
G06T 7/70 20170101AFI20250108BHJP
B66F 9/24 20060101ALI20250108BHJP
G06T 7/62 20170101ALI20250108BHJP
【ＦＩ】
G06T7/70 A
B66F9/24 Z
G06T7/62
【請求項の数】
10
(21)【出願番号】P 2022168577
(22)【出願日】2022-10-20
(65)【公開番号】P2024060968
(43)【公開日】2024-05-07
【審査請求日】2023-10-26
(73)【特許権者】
【識別番号】000232807
【氏名又は名称】三菱ロジスネクスト株式会社
(74)【代理人】
【識別番号】110001634
【氏名又は名称】弁理士法人志賀国際特許事務所
(72)【発明者】
【氏名】北島 克将
(72)【発明者】
【氏名】木内 裕介
(72)【発明者】
【氏名】山田 昌弘
(72)【発明者】
【氏名】藤田 匠
(72)【発明者】
【氏名】明石 拓也
【審査官】長谷川 素直
(56)【参考文献】
【文献】特開２０１８－１９７４９１（ＪＰ，Ａ）
【文献】特開２０２１－０９６７８９（ＪＰ，Ａ）
【文献】特開２０１５－２３２７９４（ＪＰ，Ａ）
【文献】特開２０１８－００９９１８（ＪＰ，Ａ）
【文献】特開２０２２－０２２７１５（ＪＰ，Ａ）
【文献】国際公開第２０２０／０３２１５７（ＷＯ，Ａ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０６Ｔ ７／００
Ｂ６６Ｆ ９／２４
(57)【特許請求の範囲】
【請求項1】
カメラを備えるフォークリフトの位置を推定する位置推定システムであって、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する位置推定部と、
前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記画像において前記マストが写る範囲を示す映り込み範囲を算出し、算出した前記映り込み範囲が許容範囲を超える場合には、前記位置推定部による前記位置の推定を停止すると判定する映り込み判定部と、
を備える位置推定システム。
【請求項2】
前記映り込み範囲が前記許容範囲内の場合、前記映り込み範囲を前記画像から除外する処理を行う画像処理部、をさらに有し、
前記位置推定部は、前記除外する処理が行われた後の前記画像に基づいて、前記位置を推定する、
請求項１に記載の位置推定システム。
【請求項3】
前記フォークリフトに積載する荷物の高さ情報を取得する荷物情報取得部、をさらに備え、
前記映り込み判定部は、前記マストの高さ方向の位置と、前記荷物の高さ情報と、に基づいて、前記映り込み範囲を算出する、
請求項１又は請求項２に記載の位置推定システム。
【請求項4】
前記荷物の密度と、前記荷物を前記フォークリフトに積載するときの積載面の面積と、前記荷物の重量と、に基づいて前記荷物の高さ情報を推定する高さ推定部、をさらに備える請求項３に記載の位置推定システム。
【請求項5】
前記映り込み判定部は、前記マストの高さ方向の位置と、前記マストの傾きと、に基づいて、前記映り込み範囲を算出する、請求項１又は請求項２に記載の位置推定システム。
【請求項6】
前記映り込み判定部は、前記カメラと前記マストの高さ位置の幾何的な関係に基づいて、前記映り込み範囲を算出する、請求項１又は請求項２に記載の位置推定システム。
【請求項7】
前記フォークリフトの停止時であって前記マストが最低位置にあるときに撮影された前記画像と、前記フォークリフトの停止時であって前記マストが様々な位置にあるときに撮影された前記画像の差分から、それぞれの前記マストの高さ位置に応じた前記映り込み範囲を算出し、前記マストの高さと前記映り込み範囲の関係を定めた推定関数を算出する関数推定部、をさらに備え、前記映り込み判定部は、前記マストの高さ位置と前記推定関数に基づいて前記映り込み範囲を算出する、
請求項１又は請求項２に記載の位置推定システム。
【請求項8】
カメラと、請求項１又は請求項２に記載の位置推定システムと、
を備えるフォークリフト。
【請求項9】
コンピュータによって実行される、カメラを備えるフォークリフトの位置を推定する位置推定方法であって、前記コンピュータが、前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記カメラによって撮影された画像において前記マストが写る範囲を示す映り込み範囲を算出するステップと、
前記コンピュータが、算出した前記映り込み範囲が許容範囲を超えるか否かを判定するステップと、
前記許容範囲を超える場合には、前記コンピュータが、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する処理を停止すると判定するステップと、
を有する位置推定方法。
【請求項10】
コンピュータに、カメラを備えるフォークリフトの位置を推定する位置推定方法であって、
前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記カメラによって撮影された画像において前記マストが写る範囲を示す映り込み範囲と、を算出するステップと、
算出した前記映り込み範囲が許容範囲を超えるか否かを判定するステップと、
前記許容範囲を超える場合には、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する処理を停止すると判定するステップと、
を有する位置推定方法を実行させるプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、位置推定システム、フォークリフト、位置推定方法及びプログラムに関する。
【背景技術】
【0002】
移動体に搭載したカメラで移動体周辺の画像を撮影し、撮影した画像に写るマーカ等を検出して移動体の位置を測位する方法が多く提案されている。例えば、特許文献１には、撮影方向を天井に向けたカメラをフォークリフトに設置し、天井の画像特徴を利用してフォークリフトの位置推定を行うシステムが開示されている。このシステムでは、カメラの視野範囲には、静的構造物である天井が主に映るため、運用環境中の動的物体の影響を極力低減したロバストな測位が可能となっている。しかし、フォークリフトのような移動体は、常時、外部から給電されているわけではないため、できるだけ電力消費を抑える必要がある。また、フォークリフトの製造コストを抑えるためには、高い計算負荷に耐えられる高価な計算機を搭載することは難しい。しかし、画像は１枚あたりのデータ容量が大きく、測位のための特徴点抽出等の画像処理は、計算負荷が大きく電力消費が大きい。
【0003】
また、フォークリフトでは、フォークの上昇に応じてマストも上昇するが、その際に天井を撮影した画像には、マストが写り込むことがある。画像にマストが写った範囲は測位に必要な情報を含んでいない為、この範囲の画像処理に費やす電力消費は無駄になる。また、画像を用いた測位は、一般的に、過去に撮影した画像情報をもとに作成した地図データ（環境情報）と、取得した最新の画像を比較することで位置を推定するが、マストのように環境に無関係の物体が画像中に多く映り込むほど、地図データとの比較が難しくなり、測位の精度が悪化する可能性が高まる。
【0004】
特許文献２には、移動体に搭載したカメラで周囲環境を撮影した画像から移動体の自己位置を推定する自己位置推定装置が開示されているが、この自己位置推定装置では、推定精度の向上のために、カメラの画角内に移動体の一部が写り込むと、その領域を画像から削除し、残りの領域を使って自己位置を推定する。特許文献２に記載された技術では、異なる時刻に撮影された複数の画像を比較して、画像間で差分に基づいて移動体の一部が写り込んだ領域を特定している。
【先行技術文献】
【特許文献】
【0005】
【文献】特開２０２２－３４８６１号公報
【文献】特開２０１８－９９１８号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
画像を用いたフォークリフトの測位に関し、マスト等の映り込みの影響を低減して測位する技術が必要とされている。
【0007】
本開示は、上記課題を解決することができる位置推定システム、フォークリフト、位置推定方法及びプログラムを提供する。
【課題を解決するための手段】
【0008】
本開示の位置推定システムは、カメラを備えるフォークリフトの位置を推定する位置推定システムであって、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する位置推定部と、前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記画像において前記マストが写る範囲を示す映り込み範囲を算出し、算出した前記映り込み範囲が許容範囲を超える場合には、前記位置推定部による前記位置の推定を停止すると判定する映り込み判定部と、を備える。
【0009】
本開示のフォークリフトは、カメラと、上記の位置推定システムと、を備える。
【0010】
本開示の位置推定方法は、コンピュータによって実行される、カメラを備えるフォークリフトの位置を推定する位置推定方法であって、前記コンピュータが、前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記カメラによって撮影された画像において前記マストが写る範囲を示す映り込み範囲を算出するステップと、前記コンピュータが、算出した前記映り込み範囲が許容範囲を超えるか否かを判定するステップと、前記許容範囲を超える場合には、前記コンピュータが、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する処理を停止すると判定するステップと、を有する。
【0011】
また、本開示のプログラムは、コンピュータに、カメラを備えるフォークリフトの位置を推定する処理であって、前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記カメラによって撮影された画像において前記マストが写る範囲を示す映り込み範囲と、を算出するステップと、算出した前記映り込み範囲が許容範囲を超えるか否かを判定するステップと、前記許容範囲を超える場合には、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する処理を停止すると判定するステップと、を有する処理を実行させる。
【発明の効果】
【0012】
上述の位置推定システム、フォークリフト、位置推定方法及びプログラムによれば、画像を用いたフォークリフトの測位に関し、マスト等の映り込みの影響を低減して測位することができる。
【図面の簡単な説明】
【0013】
【図1】各実施形態に係るフォークリフトの一例を示す概略図である。
【図2】第１実施形態に係る位置推定システムの一例を示す図である。
【図3A】第１実施形態のマスト映り込み範囲算出処理を説明する第１の図である。
【図3B】第１実施形態のマスト映り込み範囲算出処理を説明する第２の図である。
【図3C】第１実施形態のマスト映り込み範囲算出処理を説明する第３の図である。
【図3D】第１実施形態のマスト映り込み範囲算出処理を説明する第４の図である。
【図4】第１実施形態に係る測位処理の一例を示すフローチャートである。
【図5】第２実施形態に係る位置推定システムの一例を示す図である。
【図6A】第２実施形態のマスク処理について説明する図である。
【図6B】第２実施形態のマスク処理について説明する図である。
【図7】第２実施形態に係る測位処理の一例を示すフローチャートである。
【図8】第３実施形態に係る位置推定システムの一例を示す図である。
【図9】第３実施形態のマスト映り込み範囲算出処理を説明する第１の図である。
【図10】第３実施形態のマスト映り込み範囲算出処理を説明する第２の図である。
【図11】第４実施形態に係る位置推定システムの一例を示す図である。
【図12】第５実施形態に係る位置推定システムの一例を示す図である。
【図13】第５実施形態のマスト映り込み範囲算出処理を説明する図である。
【図14】第６実施形態に係る位置推定システムの一例を示す図である。
【図15】第６実施形態のマスト映り込み範囲推定処理を説明する第１の図である。
【図16】第６実施形態のマスト映り込み範囲推定処理を説明する第２の図である。
【図17】各実施形態に係る位置推定システムのハードウェア構成の一例を示す図である。
【発明を実施するための形態】
【0014】
＜第１実施形態＞
以下、本開示の測位に用いる画像に対する画像処理について図面を参照して説明する。
（フォークリフトの体構成）
図１は、各実施形態に係るフォークリフトの一例を示す概略図である。
フォークリフト１は、車体２と、荷役装置３と、演算装置２０と、を備えている。車体２は、タイヤおよび不図示の駆動装置とステアリング装置を備え、フォークリフト１を走行させる。荷役装置３は、フォーク４と、フォーク４を昇降させるマスト５を備えている。車体２には、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、が設けられている。ＩＭＵ１０は、加速度センサ、ジャイロセンサを備えており、フォークリフト１の移動速度、加速度および移動方向や旋回方向を検知することができる。エンコーダ１１は、フォークリフト１のタイヤ（例えば、後輪）の回転数を検知するロータリエンコーダである。エンコーダ１１の計測値によりフォークリフト１の移動を検知することができる。カメラ１２は、車体２の上方に向けられており、フォークリフト１の稼働中は常時、作業領域の天井の画像を撮影する。荷役装置３には、揚高センサ１３と、荷重センサ１４とが設けられている。揚高センサ１３は、フォーク４の揚高位置を検出する。荷重センサ１４は、フォーク４に積載された荷物の重量を検出する。揚高センサ１３と荷重センサ１４により、荷役装置３の荷役動作やフォーク４に積載される荷物の変化などを検知することができる。
以下、本開示の測位に用いる画像に対する画像処理について図面を参照して説明する。
（フォークリフトの体構成）
図１は、各実施形態に係るフォークリフトの一例を示す概略図である。
フォークリフト１は、車体２と、荷役装置３と、演算装置２０と、を備えている。車体２は、タイヤおよび不図示の駆動装置とステアリング装置を備え、フォークリフト１を走行させる。荷役装置３は、フォーク４と、フォーク４を昇降させるマスト５を備えている。車体２には、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、が設けられている。ＩＭＵ１０は、加速度センサ、ジャイロセンサを備えており、フォークリフト１の移動速度、加速度および移動方向や旋回方向を検知することができる。エンコーダ１１は、フォークリフト１のタイヤ（例えば、後輪）の回転数を検知するロータリエンコーダである。エンコーダ１１の計測値によりフォークリフト１の移動を検知することができる。カメラ１２は、車体２の上方に向けられており、フォークリフト１の稼働中は常時、作業領域の天井の画像を撮影する。荷役装置３には、揚高センサ１３と、荷重センサ１４とが設けられている。揚高センサ１３は、フォーク４の揚高位置を検出する。荷重センサ１４は、フォーク４に積載された荷物の重量を検出する。揚高センサ１３と荷重センサ１４により、荷役装置３の荷役動作やフォーク４に積載される荷物の変化などを検知することができる。
【0015】
ＩＭＵ１０、エンコーダ１１、カメラ１２、揚高センサ１３、荷重センサ１４の各センサは演算装置２０と接続されており、各センサが計測した計測値は、演算装置２０へ送出される。演算装置２０は、プロセッサを備えるコンピュータにより構成されている。演算装置２０は、上記の各センサによる計測値を用いてフォークリフト１の位置（自己位置）を推定する。
【0016】
（位置推定システムの構成）
図２は、第１実施形態に係る位置推定システムの一例を示す図である。
図２に示すように位置推定システム１００は、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５は、演算装置２０に実装される機能である。
図２は、第１実施形態に係る位置推定システムの一例を示す図である。
図２に示すように位置推定システム１００は、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５は、演算装置２０に実装される機能である。
【0017】
第１自己位置推定処理部２０１は、カメラ１２が撮影した画像からマーカ等の地図（天井地図）上の位置を特定可能な画像特徴量（マーカ等を示す画像特徴量）を有する特徴点を抽出し、その特徴点と、作業領域のマーカ等を予め記録した地図データとを照合して、自己位置（フォークリフト１の位置）を推定する。
【0018】
第２自己位置推定処理部２０２は、第１自己位置推定処理部２０１が抽出した特徴点から地図を生成しながら、Ｖ－ＳＬＡＭ（Visual Simultaneous Localization and Mapping）の技術を用いて、画像の特徴(前時刻の画像と比較することにより相対的な移動量は算出できるが、それ単独では地図上の絶対位置は特定できない画像特徴)に基づく自己位置推定を行う。
【0019】
デッドレコニング処理部２０３は、ＩＭＵ１０、エンコーダ１１の計測値を利用して推測航法による自己位置推定を行う。
統合処理部２０４は、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３が推定した自己位置を、カルマンフィルタ等を用いて統合し、最終的な自己位置の測位結果を出力する。
統合処理部２０４は、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３が推定した自己位置を、カルマンフィルタ等を用いて統合し、最終的な自己位置の測位結果を出力する。
【0020】
マスト映り込み判定処理部２０５は、カメラ１２が撮影した画像（撮影画像）を取得し、マスト５の上昇量とカメラ１２の設置位置や画角等から、幾何的にマスト５の映り込み範囲を算出し、その算出結果に応じて、画像におけるマスト５の映り込み範囲が許容範囲を超える場合に、画像を用いた自己位置推定処理を停止する。マスト５の映り込み範囲が許容範囲であれば、マスト映り込み判定処理部２０５は、撮影画像を第１自己位置推定処理部２０１、第２自己位置推定処理部２０２へ出力し、画像を用いた自己位置推定処理の実行を指示する。
【0021】
次に図３Ａ～図３Ｄを参照して、マスト映り込み判定処理部２０５によるマスト５の映り込み範囲の算出処理について説明する。図３Ａ～図３Ｄは、それぞれ第１実施形態のマスト映り込み範囲算出処理を説明する第１～第４の図である。
図３Ａのカメラ１２から伸びる破線は、カメラ１２の撮影範囲を示しており、図３Ａの右図に示すように、フォーク４の上昇がない場合にはカメラ１２よりもマスト５の上端は下にあるものとすると、フォーク４（マスト５）の上昇により、マスト５が撮影範囲に写り込む。フォーク４の上昇がない場合のカメラ１２の高さ方向の中心とマスト５上部の鉛直方向の距離をｌｚ０とする。フォーク４の上昇量をｈとする。また、図３Ｂに示すようにθをカメラ１２の画角、αをマスト５がカメラ１２の撮影範囲に映り込む角度、ｌｘをカメラ１２とマスト５の車体前後方向の距離、ｌｚｔｈをマスト５がカメラ１２の撮影範囲に映り込み始めるカメラとマスト上部の鉛直方向の距離とする。すると、幾何的に以下の式（１）、式（２）が成り立つ。
ｌｚｔｈ＝ｌｘ・ｔａｎ（（π／２）－（θ／２））・・・（１）
α＝（θ／２）－ｔａｎ－１（ｌｘ－（ｈ／ｌｚ０））・・・（２）
マスト５が撮影範囲に写り込み始める条件は、以下の式（３）で表すことができる。
ｈ－ｌｚ０＞ｌｚｔｈ・・・（３）
また、画像端からのマスト５の映り込み範囲は、以下の式（４）で表すことができる。
（α／θ）×１００（％）・・・（４）
図３Ａのカメラ１２から伸びる破線は、カメラ１２の撮影範囲を示しており、図３Ａの右図に示すように、フォーク４の上昇がない場合にはカメラ１２よりもマスト５の上端は下にあるものとすると、フォーク４（マスト５）の上昇により、マスト５が撮影範囲に写り込む。フォーク４の上昇がない場合のカメラ１２の高さ方向の中心とマスト５上部の鉛直方向の距離をｌｚ０とする。フォーク４の上昇量をｈとする。また、図３Ｂに示すようにθをカメラ１２の画角、αをマスト５がカメラ１２の撮影範囲に映り込む角度、ｌｘをカメラ１２とマスト５の車体前後方向の距離、ｌｚｔｈをマスト５がカメラ１２の撮影範囲に映り込み始めるカメラとマスト上部の鉛直方向の距離とする。すると、幾何的に以下の式（１）、式（２）が成り立つ。
ｌｚｔｈ＝ｌｘ・ｔａｎ（（π／２）－（θ／２））・・・（１）
α＝（θ／２）－ｔａｎ－１（ｌｘ－（ｈ／ｌｚ０））・・・（２）
マスト５が撮影範囲に写り込み始める条件は、以下の式（３）で表すことができる。
ｈ－ｌｚ０＞ｌｚｔｈ・・・（３）
また、画像端からのマスト５の映り込み範囲は、以下の式（４）で表すことができる。
（α／θ）×１００（％）・・・（４）
【0022】
また、カメラ１２の高さ方向の中心と撮影範囲に写り込んだマスト５上端までの高さ方向の距離をｄとおくと、図３Ｃに示すように距離ｄにおける撮影範囲の幅と、撮影範囲の中心からマスト５までの距離はそれぞれ以下の式（５）、（６）で表すことができる。
２・ｄ・ｔａｎ（θ／２）・・・（５）
ｄ・ｔａｎ（θ／２－α）・・・（６）
従って、画像端からのマスト５の映り込み範囲は、以下の式（７）で表すことができる。
（（ｔａｎ（θ／２）－ｔａｎ（θ／２－α））／２・ｔａｎ（θ／２））×１００（％）・・・（７）
２・ｄ・ｔａｎ（θ／２）・・・（５）
ｄ・ｔａｎ（θ／２－α）・・・（６）
従って、画像端からのマスト５の映り込み範囲は、以下の式（７）で表すことができる。
（（ｔａｎ（θ／２）－ｔａｎ（θ／２－α））／２・ｔａｎ（θ／２））×１００（％）・・・（７）
【0023】
図３Ｄに示すようにマスト５の上昇量に応じて撮影画像にマスト５が写り込む範囲が変化するが、この範囲は、上記の式（４）又は式（７）で算出することができる。
【0024】
（動作）
次に図４を参照して、位置推定システム１００による測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。図４は、第１実施形態に係る測位処理の一例を示すフローチャートである。位置推定システム１００は、所定の制御周期で以下の処理を繰り返す。まず、マスト映り込み判定処理部２０５が、揚高センサ１３が計測した計測値を取得する（ステップＳ１）。次にマスト映り込み判定処理部２０５は、以下の式（３´）が成立するかどうかを判定する（ステップＳ２）。
ｈ－ｌｚ０＋ｍｈ＞ｌｚｔｈ・・・（３´）
ここで、ｍｈは映り込む高さに関する余裕度である。カメラ１２とマスト５の位置関係の測量値の誤差や、マスト５の傾き等を考慮し、ある程度のマージンｍｈを設ける。また、式（３´）のうち、ｌｚ０およびｌｚｔｈの値は、カメラ１２の取付時に計測されて既知であり、マスト映り込み判定処理部２０５にそれらの値が記録されている。ｈは、揚高センサ１３の計測値から算出可能（フォーク４の上昇前と上昇後の差分）である。式（３´）が成立すれば、マスト５がカメラ１２による撮影画像に写り込む可能性が高いことを示し、式（３´）が成立しなければ、マスト５が撮影画像に写り込む可能性が低いことを示す。つまり、式（３´）によって、マスト５が画像に映り込むかどうか、つまり映り込み範囲が０より大きいかどうか、より正確には、マスト５の撮影画像への映り込み範囲がマスト５の高さのマージンｍｈに相当する範囲未満か、又はマージンｍｈに相当する範囲以上かを判定することができる。式（３´）が成立する場合（ステップＳ２；Ｔｒｕｅ）、マスト映り込み判定処理部２０５は、画像を用いた自己位置推定処理の実行を第１自己位置推定処理部２０１および第２自己位置推定処理部２０２へ指示しない。画像を用いた自己位置推定処理とは、第１自己位置推定処理部２０１および第２自己位置推定処理部２０２による自己位置推定処理である。撮影画像にマスト５が写り込むと自己位置の推定精度が低下する。従って、式（３´）が成立する場合には、計算負荷が高い画像を用いた自己位置推定処理を停止することで推定精度の低下と電力消費を防止する。統合処理部２０４は、デッドレコニング処理部２０３の測位結果から最終的な自己位置の測位結果を算出する。一方、式（３´）が成立しない場合（ステップＳ２；Ｆａｌｓｅ）、マスト映り込み判定処理部２０５は、画像を用いた自己位置推定処理の実行を第１自己位置推定処理部２０１および第２自己位置推定処理部２０２へ指示する。第１自己位置推定処理部２０１、第２自己位置推定処理部２０２は自己位置推定処理を実行し（ステップＳ３）、推定結果を統合処理部２０４へ出力する。並行して行われているデッドレコニング処理部２０３による測位結果も統合処理部２０４へ出力される。統合処理部２０４は、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２およびデッドレコニング処理部２０３それぞれの測位結果から最終的な自己位置の測位結果を算出する。
次に図４を参照して、位置推定システム１００による測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。図４は、第１実施形態に係る測位処理の一例を示すフローチャートである。位置推定システム１００は、所定の制御周期で以下の処理を繰り返す。まず、マスト映り込み判定処理部２０５が、揚高センサ１３が計測した計測値を取得する（ステップＳ１）。次にマスト映り込み判定処理部２０５は、以下の式（３´）が成立するかどうかを判定する（ステップＳ２）。
ｈ－ｌｚ０＋ｍｈ＞ｌｚｔｈ・・・（３´）
ここで、ｍｈは映り込む高さに関する余裕度である。カメラ１２とマスト５の位置関係の測量値の誤差や、マスト５の傾き等を考慮し、ある程度のマージンｍｈを設ける。また、式（３´）のうち、ｌｚ０およびｌｚｔｈの値は、カメラ１２の取付時に計測されて既知であり、マスト映り込み判定処理部２０５にそれらの値が記録されている。ｈは、揚高センサ１３の計測値から算出可能（フォーク４の上昇前と上昇後の差分）である。式（３´）が成立すれば、マスト５がカメラ１２による撮影画像に写り込む可能性が高いことを示し、式（３´）が成立しなければ、マスト５が撮影画像に写り込む可能性が低いことを示す。つまり、式（３´）によって、マスト５が画像に映り込むかどうか、つまり映り込み範囲が０より大きいかどうか、より正確には、マスト５の撮影画像への映り込み範囲がマスト５の高さのマージンｍｈに相当する範囲未満か、又はマージンｍｈに相当する範囲以上かを判定することができる。式（３´）が成立する場合（ステップＳ２；Ｔｒｕｅ）、マスト映り込み判定処理部２０５は、画像を用いた自己位置推定処理の実行を第１自己位置推定処理部２０１および第２自己位置推定処理部２０２へ指示しない。画像を用いた自己位置推定処理とは、第１自己位置推定処理部２０１および第２自己位置推定処理部２０２による自己位置推定処理である。撮影画像にマスト５が写り込むと自己位置の推定精度が低下する。従って、式（３´）が成立する場合には、計算負荷が高い画像を用いた自己位置推定処理を停止することで推定精度の低下と電力消費を防止する。統合処理部２０４は、デッドレコニング処理部２０３の測位結果から最終的な自己位置の測位結果を算出する。一方、式（３´）が成立しない場合（ステップＳ２；Ｆａｌｓｅ）、マスト映り込み判定処理部２０５は、画像を用いた自己位置推定処理の実行を第１自己位置推定処理部２０１および第２自己位置推定処理部２０２へ指示する。第１自己位置推定処理部２０１、第２自己位置推定処理部２０２は自己位置推定処理を実行し（ステップＳ３）、推定結果を統合処理部２０４へ出力する。並行して行われているデッドレコニング処理部２０３による測位結果も統合処理部２０４へ出力される。統合処理部２０４は、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２およびデッドレコニング処理部２０３それぞれの測位結果から最終的な自己位置の測位結果を算出する。
【0025】
（効果）
有人フォークリフトの運用においては、しばしば、フォーク４を上げた状態で走行を行うことがある。従って、走行中にもかかわらず、天井向けカメラ１２の画像には、常に変化しない物体としてマスト５が映り込む。マスト５が写った範囲の画像に対する自己位置推定処理は行う必要がなく、処理を行うと不必要に電力を消費することになる。また、測位精度に関しても、例えば、実際にはフォークリフト１は移動しているにもかかわらず、推定結果では動いていない、あるいは、動きの量が少ないといった問題が生じることがある。そこで、揚高センサ１３の計測値を参照することで、カメラ１２の撮影画像にマスト５が映り込むかどうかを判定し、マスト５が映り込む場合、画像による自己位置推定処理で妥当な測位結果が算出できない可能性が高いと判断し、画像を用いた測位処理を停止する。これにより、計算リソースの節約、および、最終的な測位結果の精度悪化を防ぐことができる。
有人フォークリフトの運用においては、しばしば、フォーク４を上げた状態で走行を行うことがある。従って、走行中にもかかわらず、天井向けカメラ１２の画像には、常に変化しない物体としてマスト５が映り込む。マスト５が写った範囲の画像に対する自己位置推定処理は行う必要がなく、処理を行うと不必要に電力を消費することになる。また、測位精度に関しても、例えば、実際にはフォークリフト１は移動しているにもかかわらず、推定結果では動いていない、あるいは、動きの量が少ないといった問題が生じることがある。そこで、揚高センサ１３の計測値を参照することで、カメラ１２の撮影画像にマスト５が映り込むかどうかを判定し、マスト５が映り込む場合、画像による自己位置推定処理で妥当な測位結果が算出できない可能性が高いと判断し、画像を用いた測位処理を停止する。これにより、計算リソースの節約、および、最終的な測位結果の精度悪化を防ぐことができる。
【0026】
＜第２実施形態＞
以下、本開示の第２実施形態に係る位置推定システム１００Ａについて図５～図７を参照して説明する。第１実施形態では、マスト５が撮影画像に写り込む場合には、画像を用いた自己位置推定処理を停止した。この制御の場合、マストが映り込む場合には画像による測位を停止するため、フォーク４を上げたままの走行が長くなると、測位手段がデッドレコニング（走行距離が長いほど、誤差が累積する）のみとなり、測位精度が悪化する可能性がある。また、第２自己位置推定処理部２０２によるＶ－ＳＬＡＭを利用した測位では、最後に撮影された画像とその直前に撮影された画像の画像特徴の比較を上手く行えたほうが、位置推定処理が簡単になるため（直前の画像とのマッチングができなかった場合には、過去に登録した環境情報（地図）とのマッチングを行うことになるため、マッチング対象数が多くなり、計算負荷が上昇する。）、なるべく、連続的に（時間間隔を開けずに）処理を行うほうが、計算負荷を抑制し、測位精度を維持することができる。また、撮影画像にマスト５が映り込んでいたとしても、画像的な特徴が十分に抽出でき、妥当な測位が可能である場合や、マーカ等が画像に含まれている場合もある。従って、マスト５が写った範囲外を用いて、第１自己位置推定処理部２０１による自己位置推定処理を行うことで、測位精度の悪化回避を期待することができる。そこで、第２実施形態では、カメラ１２の取付時点で測量しておいたカメラとマストの位置関係に従って、画像中のマスト映り込み範囲を特定し、特定した範囲を除外・無効化した画像を生成して、その画像に基づいて自己位置推定を行うことにより、測位精度の悪化を回避する。
以下、本開示の第２実施形態に係る位置推定システム１００Ａについて図５～図７を参照して説明する。第１実施形態では、マスト５が撮影画像に写り込む場合には、画像を用いた自己位置推定処理を停止した。この制御の場合、マストが映り込む場合には画像による測位を停止するため、フォーク４を上げたままの走行が長くなると、測位手段がデッドレコニング（走行距離が長いほど、誤差が累積する）のみとなり、測位精度が悪化する可能性がある。また、第２自己位置推定処理部２０２によるＶ－ＳＬＡＭを利用した測位では、最後に撮影された画像とその直前に撮影された画像の画像特徴の比較を上手く行えたほうが、位置推定処理が簡単になるため（直前の画像とのマッチングができなかった場合には、過去に登録した環境情報（地図）とのマッチングを行うことになるため、マッチング対象数が多くなり、計算負荷が上昇する。）、なるべく、連続的に（時間間隔を開けずに）処理を行うほうが、計算負荷を抑制し、測位精度を維持することができる。また、撮影画像にマスト５が映り込んでいたとしても、画像的な特徴が十分に抽出でき、妥当な測位が可能である場合や、マーカ等が画像に含まれている場合もある。従って、マスト５が写った範囲外を用いて、第１自己位置推定処理部２０１による自己位置推定処理を行うことで、測位精度の悪化回避を期待することができる。そこで、第２実施形態では、カメラ１２の取付時点で測量しておいたカメラとマストの位置関係に従って、画像中のマスト映り込み範囲を特定し、特定した範囲を除外・無効化した画像を生成して、その画像に基づいて自己位置推定を行うことにより、測位精度の悪化を回避する。
【0027】
（位置推定システムの構成）
図５は、第２実施形態に係る位置推定システムの一例を示す図である。
図５に示すように位置推定システム１００Ａは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５と、マスク画像生成処理部２０６と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５、マスク画像生成処理部２０６は、演算装置２０に実装される機能である。第２実施形態に係る構成のうち、第１実施形態に係る位置推定システム１００と同様のものには同じ符号を付し、それぞれの説明を省略する。
図５は、第２実施形態に係る位置推定システムの一例を示す図である。
図５に示すように位置推定システム１００Ａは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５と、マスク画像生成処理部２０６と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５、マスク画像生成処理部２０６は、演算装置２０に実装される機能である。第２実施形態に係る構成のうち、第１実施形態に係る位置推定システム１００と同様のものには同じ符号を付し、それぞれの説明を省略する。
【0028】
マスク画像生成処理部２０６は、マスト映り込み範囲が許容できる程度である場合に、マスト映り込み判定処理部２０５によって特定されたマスト映り込み範囲をマスク化した画像を生成し、生成した画像を第１自己位置推定処理部２０１と第２自己位置推定処理部２０２へ出力し、画像を用いた自己位置推定処理の実行を指示する。
【0029】
図６Ａ、図６Ｂはそれぞれ、第２実施形態のマスク処理について説明する第１、第２の図である。図６Ａにカメラ１２によって撮影された撮影画像の一例を示す。この画像の右側にはマスト５が写り込んでいる。マスト５が写り込んだ範囲は、式（４）又は式（７）で算出することができる。式（４）、式（７）のθは既知であり、αは式（２）から算出することができる。式（２）のθ、ｌｘ、ｌｚ０は、カメラ設置時に計測されていて既知である。式（２）のｈは揚高センサ１３の計測値から算出することができる。マスト映り込み判定処理部２０５は、マスト５が写り込んだ範囲は、式（４）又は式（７）と揚高センサ１３の計測値から算出し、その算出結果にマージンｍｒを加えてマスク画像生成処理部２０６へ出力する。マスク画像生成処理部２０６は、マスト映り込み判定処理部２０５から取得したマスト映り込み範囲をマスク化した画像を生成する。マスク化した画像の一例を図６Ｂに示す。図６Ｂでは、マスト映り込み範囲を白で表示しているが、マスクする方法は任意であってよい。例えば、黒色でマスク化してもよいし、マスト映り込み範囲を削除した画像を生成してもよい。
【0030】
（動作）
次に図７を参照して、位置推定システム１００Ａによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。図７は、第２実施形態に係る測位処理の一例を示すフローチャートである。位置推定システム１００Ａは、所定の制御周期で以下の処理を繰り返す。まず、マスト映り込み判定処理部２０５が、揚高センサ１３が計測した計測値を取得する（ステップＳ１１）。次にマスト映り込み判定処理部２０５は、上記した式（３´）が成立するかどうかを判定する（ステップＳ１２）。式（３´）が成立しない場合（ステップＳ１２；Ｆａｌｓｅ）、ステップＳ１６へ進む。式（３´）が成立する場合（ステップＳ１２；Ｔｒｕｅ）、マスト映り込み判定処理部２０５は、式（２）によってα（マスト５が画像に写り込む範囲に対応する角度）を算出し、次の式（８）が成立するかどうかを判定する（ステップＳ１３）。
α＞αｔｈ ・・・（８）
ここでαｔｈは、映り込みが大きいと判断するための閾値である。式（８）が成立する場合（ステップＳ１３；Ｔｒｕｅ）、画像による自己位置の推定精度が低下すると判断し画像を用いた自己位置推定処理を実行しない。これにより自己位置推定精度の低下と電力消費を防止する。
次に図７を参照して、位置推定システム１００Ａによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。図７は、第２実施形態に係る測位処理の一例を示すフローチャートである。位置推定システム１００Ａは、所定の制御周期で以下の処理を繰り返す。まず、マスト映り込み判定処理部２０５が、揚高センサ１３が計測した計測値を取得する（ステップＳ１１）。次にマスト映り込み判定処理部２０５は、上記した式（３´）が成立するかどうかを判定する（ステップＳ１２）。式（３´）が成立しない場合（ステップＳ１２；Ｆａｌｓｅ）、ステップＳ１６へ進む。式（３´）が成立する場合（ステップＳ１２；Ｔｒｕｅ）、マスト映り込み判定処理部２０５は、式（２）によってα（マスト５が画像に写り込む範囲に対応する角度）を算出し、次の式（８）が成立するかどうかを判定する（ステップＳ１３）。
α＞αｔｈ ・・・（８）
ここでαｔｈは、映り込みが大きいと判断するための閾値である。式（８）が成立する場合（ステップＳ１３；Ｔｒｕｅ）、画像による自己位置の推定精度が低下すると判断し画像を用いた自己位置推定処理を実行しない。これにより自己位置推定精度の低下と電力消費を防止する。
【0031】
一方、式（８）が成立しない場合（ステップＳ１３；Ｆａｌｓｅ）、マスト映り込み判定処理部２０５は、マスト映り込み範囲を以下の式（９）で算出し、算出したマスト映り込み範囲の値をマスク画像生成処理部２０６へ出力する。
（α／θ）×１００＋ｍｒ（％）・・・（９）
ここでｍ ｒ は、マスク範囲に関する余裕度である。マスク画像生成処理部２０６は、画像端からマスト映り込み範囲をマスク処理した画像を生成する（ステップＳ１４）。マスク画像生成処理部２０６は、生成したマスク化画像を自己位置推定処理の実行を第１自己位置推定処理部２０１および第２自己位置推定処理部２０２へ出力し（ステップＳ１５）、画像を用いた自己位置推定処理の実行を指示する。第１自己位置推定処理部２０１、第２自己位置推定処理部２０２は、マスク画像生成処理部２０６から取得したマスク化画像のうちマスク処理されていない範囲を処理対象として自己位置推定処理を実行し（ステップＳ１６）、推定結果を統合処理部２０４へ出力する。並行して行われているデッドレコニング処理部２０３による測位結果も統合処理部２０４へ出力される。統合処理部２０４は、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２およびデッドレコニング処理部２０３それぞれの測位結果から最終的な自己位置の測位結果を算出する。
（α／θ）×１００＋ｍｒ（％）・・・（９）
ここでｍ ｒ は、マスク範囲に関する余裕度である。マスク画像生成処理部２０６は、画像端からマスト映り込み範囲をマスク処理した画像を生成する（ステップＳ１４）。マスク画像生成処理部２０６は、生成したマスク化画像を自己位置推定処理の実行を第１自己位置推定処理部２０１および第２自己位置推定処理部２０２へ出力し（ステップＳ１５）、画像を用いた自己位置推定処理の実行を指示する。第１自己位置推定処理部２０１、第２自己位置推定処理部２０２は、マスク画像生成処理部２０６から取得したマスク化画像のうちマスク処理されていない範囲を処理対象として自己位置推定処理を実行し（ステップＳ１６）、推定結果を統合処理部２０４へ出力する。並行して行われているデッドレコニング処理部２０３による測位結果も統合処理部２０４へ出力される。統合処理部２０４は、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２およびデッドレコニング処理部２０３それぞれの測位結果から最終的な自己位置の測位結果を算出する。
【0032】
以上説明したように本実施形態によれば、撮影画像にマスト５が映り込んでいても、その範囲が許容内であればマスク処理した画像による測位を行う。これにより、マスト５の映り込みによる測位精度の悪化や無駄な測位処理（マストが大きく映り込む場合等）を回避しつつ、測位精度を確保することができる。また、画像を用いた自己位置推定を行う場合であっても、マスク化した範囲に対しては画像処理を行わないことで、計算負荷および電力消費を低減することができる。
【0033】
＜第３実施形態＞
以下、本開示の第３実施形態に係る位置推定システム１００Ｂについて図８～図１０を参照して説明する。第１、第２実施形態実施例では、マスト５の映り込みのみを考慮していたが、フォーク４の上昇量が大きい、あるいは、フォーク４に積載した荷物の高さが大きい場合には、荷物も映り込む場合が考えられる。第３実施形態では、撮影画像に含まれるマスト５だけではなく、荷物を含めて映り込み判定や映り込み範囲の特定を行う。
以下、本開示の第３実施形態に係る位置推定システム１００Ｂについて図８～図１０を参照して説明する。第１、第２実施形態実施例では、マスト５の映り込みのみを考慮していたが、フォーク４の上昇量が大きい、あるいは、フォーク４に積載した荷物の高さが大きい場合には、荷物も映り込む場合が考えられる。第３実施形態では、撮影画像に含まれるマスト５だけではなく、荷物を含めて映り込み判定や映り込み範囲の特定を行う。
【0034】
（位置推定システムの構成）
図８は、第３実施形態に係る位置推定システムの一例を示す図である。
図８に示すように位置推定システム１００Ｂは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｂと、マスク画像生成処理部２０６と、荷物情報記録部２０７と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、荷重センサ１４と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｂ、マスク画像生成処理部２０６、荷物情報記録部２０７は、演算装置２０に実装される機能である。第３実施形態に係る構成のうち、第２実施形態に係る位置推定システム１００Ａと同様のものには同じ符号を付し、それぞれの説明を省略する。
図８は、第３実施形態に係る位置推定システムの一例を示す図である。
図８に示すように位置推定システム１００Ｂは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｂと、マスク画像生成処理部２０６と、荷物情報記録部２０７と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、荷重センサ１４と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｂ、マスク画像生成処理部２０６、荷物情報記録部２０７は、演算装置２０に実装される機能である。第３実施形態に係る構成のうち、第２実施形態に係る位置推定システム１００Ａと同様のものには同じ符号を付し、それぞれの説明を省略する。
【0035】
荷物情報記録部２０７は、外部から設定された荷物の高さ情報を取得し記録・記憶する。高さ情報の設定方法について特に限定は無いが、例えば、倉庫においては、取り扱う荷物の荷姿が揃っている（例えば、ＸＸＸ段済みの飲料の段ボール等）場合があるため、それら荷物の納品時等に荷物の高さ情報を荷物情報記録部２０７に設定しておいてもよい。あるいは、倉庫の管理システムと連動し、荷取時にバーコードスキャン等で荷物を確認する際に、荷の高さ情報を荷物情報記録部２０７に設定するようにしてもよい。また、荷取時の確認作業のときに作業者に大まかな荷の高さ情報を設定してもらったり、位置推定システム１００Ｂの起動時に、取り扱う荷の高さ情報を作業者に設定してもらったりといった方法（日ごとに、同じような荷姿を取り扱う場合）でもよい。
マスト映り込み判定処理部２０５Ｂは、マスト上昇量とカメラ設置位置・画角、荷物の高さ情報等から、幾何的にマスト・荷の映り込み範囲を算出し、算出結果に応じて、画像中の映り込み範囲が許容できない場合に、画像を用いた自己位置推定処理を停止する。
マスト映り込み判定処理部２０５Ｂは、マスト上昇量とカメラ設置位置・画角、荷物の高さ情報等から、幾何的にマスト・荷の映り込み範囲を算出し、算出結果に応じて、画像中の映り込み範囲が許容できない場合に、画像を用いた自己位置推定処理を停止する。
【0036】
次に図９、図１０を参照して、マスト・荷の映り込み範囲の算出方法について説明する。図９、図１０はそれぞれ、第３実施形態のマスト映り込み範囲算出処理を説明する第１、第２の図である。図９にパレット７に積んだ荷物６をフォーク４で持ち上げている状態を示す。図９に示すように、荷物６の高さが高いか又は、フォーク４の上昇量が大きい場合には、撮影画像には、マスト５だけではなく荷物６が写り込む。図１０に示すように、θをカメラ１２の画角、αｌを荷物６がカメラ１２の撮影範囲に映り込む角度、カメラ基準位置から荷背面までの距離をｌｃｌ、パレット７の上面から荷物６の上面までの距離をｈｌ、揚高センサ１３から求められるカメラ基準位置からフォーク４の上面までの距離をｈｃｆ、フォーク４の上面からパレット７の上面までの距離をｈｆｐとすると、幾何的な関係から、以下の式（１０）が成立する。
αｌ＝（θ／２）－ｔａｎ－１（ｌｃｌ／（ｈｃｆ＋ｈｆｐ＋ｈｌ））・・・（１０）
また、荷物６が画像に映り込む条件は次の式（１１）で表すことができる。
（ｈｃｆ＋ｈｆｐ＋ｈｌ）＞ｌｃｌ・ｔａｎ（（π／２）－（θ／２））・・・（１１）
ｈ ｌ は荷の高さであり、荷物情報記録部２０７に記録されている。ｌｃｌ、ｈｃｆ、ｈｆｐの各長さについても予め計測され、マスト映り込み判定処理部２０５Ｂに記録、記憶されている。マスト映り込み判定処理部２０５Ｂは、荷物６の高さｈ ｌ を荷物情報記録部２０７から取得し、式（１０）、式（１１）によって、マスト・荷の映り込み範囲と荷物６が画像に映り込む条件を算出する。
αｌ＝（θ／２）－ｔａｎ－１（ｌｃｌ／（ｈｃｆ＋ｈｆｐ＋ｈｌ））・・・（１０）
また、荷物６が画像に映り込む条件は次の式（１１）で表すことができる。
（ｈｃｆ＋ｈｆｐ＋ｈｌ）＞ｌｃｌ・ｔａｎ（（π／２）－（θ／２））・・・（１１）
ｈ ｌ は荷の高さであり、荷物情報記録部２０７に記録されている。ｌｃｌ、ｈｃｆ、ｈｆｐの各長さについても予め計測され、マスト映り込み判定処理部２０５Ｂに記録、記憶されている。マスト映り込み判定処理部２０５Ｂは、荷物６の高さｈ ｌ を荷物情報記録部２０７から取得し、式（１０）、式（１１）によって、マスト・荷の映り込み範囲と荷物６が画像に映り込む条件を算出する。
【0037】
（動作）
次に位置推定システム１００Ｂによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第３実施形態の位置推定システム１００Ｂの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。マスト映り込み判定処理部２０５Ｂが、マスト５の高さ（フォーク４の上昇量）に加えて、荷物６の高さを考慮して画像を用いた自己位置推定を行うか否かの判定を行うことや、撮影画像におけるマスク処理を施す範囲を算出する点を除けば、図４、図７で説明した処理と同様である。
次に位置推定システム１００Ｂによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第３実施形態の位置推定システム１００Ｂの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。マスト映り込み判定処理部２０５Ｂが、マスト５の高さ（フォーク４の上昇量）に加えて、荷物６の高さを考慮して画像を用いた自己位置推定を行うか否かの判定を行うことや、撮影画像におけるマスク処理を施す範囲を算出する点を除けば、図４、図７で説明した処理と同様である。
【0038】
（第１実施形態に対応する処理）
荷物６の高さを考慮した荷物６が画像に映り込む条件によって、画像を用いた自己位置推定を行うかどうかを判定する処理の流れは、第１実施形態の図４で説明したものと同様である。但し、図４のステップＳ２の式（３´）が次式（１１´）となる。
（ｈｃｆ＋ｈｆｐ＋ｈｌ）＋ｍｈ＞ｌｃｌ・ｔａｎ（（π／２）－（θ／２））・・・（１１´）
マージンのｍｈの大きさは第１実施形態と同じ値でも異なる値でも良い。
荷物６の高さを考慮した荷物６が画像に映り込む条件によって、画像を用いた自己位置推定を行うかどうかを判定する処理の流れは、第１実施形態の図４で説明したものと同様である。但し、図４のステップＳ２の式（３´）が次式（１１´）となる。
（ｈｃｆ＋ｈｆｐ＋ｈｌ）＋ｍｈ＞ｌｃｌ・ｔａｎ（（π／２）－（θ／２））・・・（１１´）
マージンのｍｈの大きさは第１実施形態と同じ値でも異なる値でも良い。
【0039】
（第２実施形態に対応する処理）
マスク化画像を用いて画像を用いた自己位置推定を行う場合の処理は、第２実施形態の図７で説明したものと同様である。但し、図７のステップＳ１２の式（３´）が上記の式（１１´）となり、ステップＳ１３の左辺がαｌとなり、ステップＳ１４のマスク処理する範囲が次式（１２）となる。
（αｌ／θ）×１００＋ｍｒ（％）・・・（１２）
マージンのｍｒの大きさは第２実施形態と同じ値でも異なる値でも良い。
マスク化画像を用いて画像を用いた自己位置推定を行う場合の処理は、第２実施形態の図７で説明したものと同様である。但し、図７のステップＳ１２の式（３´）が上記の式（１１´）となり、ステップＳ１３の左辺がαｌとなり、ステップＳ１４のマスク処理する範囲が次式（１２）となる。
（αｌ／θ）×１００＋ｍｒ（％）・・・（１２）
マージンのｍｒの大きさは第２実施形態と同じ値でも異なる値でも良い。
【0040】
（効果）
以上説明したように、本実施形態によれば、荷の高さ情報を取得することで、マスト５に加え荷物６が映り込む場合に対しても、マスト５および荷物６の映り込み範囲の特定が可能となり、第１実施形態又は第２実施形態と同様の効果を得ることができる。なお、荷物６の高さに加えて、荷物６の幅（図９における紙面奥行き方向の荷物６のサイズ）を考慮してマスク処理する範囲を算出するようにしてもよい。これにより、例えば、荷物６の幅が小さいときにはマスク処理範囲を小さくして、より多くの画像範囲を自己位置推定に用いることができる。
以上説明したように、本実施形態によれば、荷の高さ情報を取得することで、マスト５に加え荷物６が映り込む場合に対しても、マスト５および荷物６の映り込み範囲の特定が可能となり、第１実施形態又は第２実施形態と同様の効果を得ることができる。なお、荷物６の高さに加えて、荷物６の幅（図９における紙面奥行き方向の荷物６のサイズ）を考慮してマスク処理する範囲を算出するようにしてもよい。これにより、例えば、荷物６の幅が小さいときにはマスク処理範囲を小さくして、より多くの画像範囲を自己位置推定に用いることができる。
【0041】
＜第４実施形態＞
以下、本開示の第４実施形態に係る位置推定システム１００Ｃについて図１１を参照して説明する。第３実施形態は、通常運用する荷姿がほとんど同じ倉庫を除いて、荷物ごとに荷の高さ情報を設定する手間が発生する。倉庫によっては、取り扱う荷の内容（密度）は同じであるが、荷姿が異なる（積んでいる段数が異なる）場合があり、荷物はパレット（約１ｍ×１ｍ）上に積まれるため、密度情報が与えられれば、荷の高さを概ね推定することができる場合がある。そこで、第４実施形態では、事前に荷物の密度情報を設定し、荷重センサ１４の計測値と組み合わせて、荷物の高さを自動推定する機能を追加する。
以下、本開示の第４実施形態に係る位置推定システム１００Ｃについて図１１を参照して説明する。第３実施形態は、通常運用する荷姿がほとんど同じ倉庫を除いて、荷物ごとに荷の高さ情報を設定する手間が発生する。倉庫によっては、取り扱う荷の内容（密度）は同じであるが、荷姿が異なる（積んでいる段数が異なる）場合があり、荷物はパレット（約１ｍ×１ｍ）上に積まれるため、密度情報が与えられれば、荷の高さを概ね推定することができる場合がある。そこで、第４実施形態では、事前に荷物の密度情報を設定し、荷重センサ１４の計測値と組み合わせて、荷物の高さを自動推定する機能を追加する。
【0042】
（位置推定システムの構成）
図１１は、第４実施形態に係る位置推定システムの一例を示す図である。
図１１に示すように位置推定システム１００Ｃは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｂと、マスク画像生成処理部２０６と、荷物密度記録部２０８と、荷物高さ推定部２０９と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、荷重センサ１４と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｂ、マスク画像生成処理部２０６、荷物密度記録部２０８と、荷物高さ推定部２０９は、演算装置２０に実装される機能である。第４実施形態に係る構成のうち、第３実施形態に係る位置推定システム１００Ｂと同様のものには同じ符号を付し、それぞれの説明を省略する。
図１１は、第４実施形態に係る位置推定システムの一例を示す図である。
図１１に示すように位置推定システム１００Ｃは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｂと、マスク画像生成処理部２０６と、荷物密度記録部２０８と、荷物高さ推定部２０９と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、荷重センサ１４と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｂ、マスク画像生成処理部２０６、荷物密度記録部２０８と、荷物高さ推定部２０９は、演算装置２０に実装される機能である。第４実施形態に係る構成のうち、第３実施形態に係る位置推定システム１００Ｂと同様のものには同じ符号を付し、それぞれの説明を省略する。
【0043】
荷物密度記録部２０８は、外部から設定された荷物の密度情報を取得し、記録・記憶する。
荷物高さ推定部２０９は、荷重センサ１４が計測した荷物の重さと、荷物密度記録部２０８が記録した密度とを取得し、荷物の高さを推定する。例えば、荷物高さ推定部２０９は、１ｍ×１ｍ四方のパレットに、荷物密度記録部２０８が記録した密度の荷物を単位高さだけ積んだ場合の重量を算出し、荷重センサ１４が計測した荷物の重さを、算出した単位高さ当たりの重量で除算することで、図１０におけるパレット７上面から荷物６上面までの高さｈｌを推定する。
荷物高さ推定部２０９は、荷重センサ１４が計測した荷物の重さと、荷物密度記録部２０８が記録した密度とを取得し、荷物の高さを推定する。例えば、荷物高さ推定部２０９は、１ｍ×１ｍ四方のパレットに、荷物密度記録部２０８が記録した密度の荷物を単位高さだけ積んだ場合の重量を算出し、荷重センサ１４が計測した荷物の重さを、算出した単位高さ当たりの重量で除算することで、図１０におけるパレット７上面から荷物６上面までの高さｈｌを推定する。
【0044】
（動作）
次に位置推定システム１００Ｃによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第４実施形態の位置推定システム１００Ｃの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。第４実施形態の位置推定システム１００Ｃの動作は、マスト映り込み判定処理部２０５Ｂが、第３実施形態の荷物情報記録部２０７が記録した高さ情報に代えて、荷物高さ推定部２０９が推定した高さ情報を用いて、画像を用いた自己位置推定を行うか否かの判定を行うことや、撮影画像におけるマスク処理を施す範囲を算出する点を除けば、第３実施形態で説明した処理と同様である。
次に位置推定システム１００Ｃによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第４実施形態の位置推定システム１００Ｃの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。第４実施形態の位置推定システム１００Ｃの動作は、マスト映り込み判定処理部２０５Ｂが、第３実施形態の荷物情報記録部２０７が記録した高さ情報に代えて、荷物高さ推定部２０９が推定した高さ情報を用いて、画像を用いた自己位置推定を行うか否かの判定を行うことや、撮影画像におけるマスク処理を施す範囲を算出する点を除けば、第３実施形態で説明した処理と同様である。
【0045】
（効果）
以上説明したように、本実施形態によれば、荷物を積む度に高さ情報を設定する必要が無く、事前に密度情報を設定することで、荷物の高さを推定することができる。これにより、作業者の負担を軽減しつつ、第３実施形態と同様の効果を得ることができる。
以上説明したように、本実施形態によれば、荷物を積む度に高さ情報を設定する必要が無く、事前に密度情報を設定することで、荷物の高さを推定することができる。これにより、作業者の負担を軽減しつつ、第３実施形態と同様の効果を得ることができる。
【0046】
＜第５実施形態＞
以下、本開示の第５実施形態に係る位置推定システム１００Ｄについて図１２～図１３を参照して説明する。フォークリフト１の荷役中には、マスト５が、例えば、車体２側に傾くため、その傾きを考慮して、画像中の映り込み範囲の算出を行う。上記した第１～第４実施形態では、マスト５が垂直と仮定して、幾何的演算によってマスト５等の映り込み範囲の算出を行い、マスト５の傾きに関しては、他の誤差要因とまとめてマージン（ｍｈやｍｒ）として設定している。しかし、マージンを取りすぎると、自己位置推定に使える画像範囲が狭まってしまい、測位精度に影響する可能性がある。そこで、第５実施形態では、マスト５の傾き量を参照し、より厳密に映り込み範囲の算出を行う。
以下、本開示の第５実施形態に係る位置推定システム１００Ｄについて図１２～図１３を参照して説明する。フォークリフト１の荷役中には、マスト５が、例えば、車体２側に傾くため、その傾きを考慮して、画像中の映り込み範囲の算出を行う。上記した第１～第４実施形態では、マスト５が垂直と仮定して、幾何的演算によってマスト５等の映り込み範囲の算出を行い、マスト５の傾きに関しては、他の誤差要因とまとめてマージン（ｍｈやｍｒ）として設定している。しかし、マージンを取りすぎると、自己位置推定に使える画像範囲が狭まってしまい、測位精度に影響する可能性がある。そこで、第５実施形態では、マスト５の傾き量を参照し、より厳密に映り込み範囲の算出を行う。
【0047】
（位置推定システムの構成）
図１２は、第５実施形態に係る位置推定システムの一例を示す図である。
図１２に示すように位置推定システム１００Ｄは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｄと、マスク画像生成処理部２０６と、傾き推定部２１０と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、荷重センサ１４と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｄ、マスク画像生成処理部２０６、傾き推定部２１０は、演算装置２０に実装される機能である。第５実施形態に係る構成のうち、第２実施形態に係る位置推定システム１００Ａと同様のものには同じ符号を付し、それぞれの説明を省略する。
図１２は、第５実施形態に係る位置推定システムの一例を示す図である。
図１２に示すように位置推定システム１００Ｄは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｄと、マスク画像生成処理部２０６と、傾き推定部２１０と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、荷重センサ１４と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｄ、マスク画像生成処理部２０６、傾き推定部２１０は、演算装置２０に実装される機能である。第５実施形態に係る構成のうち、第２実施形態に係る位置推定システム１００Ａと同様のものには同じ符号を付し、それぞれの説明を省略する。
【0048】
傾き推定部２１０は、フォーク４に積載される荷物の重量とマスト５の傾き量（図１３に示すマスト傾き角度β）の関係を定めたテーブル等を有しており、このテーブルと、荷重センサ１４が計測した荷物の重量とに基づいて、マスト５の傾き量を推定する。推定した傾き量は、マスト映り込み判定処理部２０５Ｄへ出力される。
マスト映り込み判定処理部２０５Ｄは、マスト５の上昇量および傾き量とカメラ設置位置・画角等から、幾何的にマストの映り込み範囲を算出し、算出結果に応じて、画像中の映り込み範囲が許容できない場合に、画像を用いた自己位置推定処理を停止する。
マスト映り込み判定処理部２０５Ｄは、マスト５の上昇量および傾き量とカメラ設置位置・画角等から、幾何的にマストの映り込み範囲を算出し、算出結果に応じて、画像中の映り込み範囲が許容できない場合に、画像を用いた自己位置推定処理を停止する。
【0049】
次に図１３を参照して、本実施形態におけるマストの映り込み範囲の算出方法について説明する。図１３は、第５実施形態のマスト映り込み範囲算出処理を説明する図である。図１３に示すように、θをカメラ１２の画角、αｌをマスト５が撮影範囲に映り込む角度、マスト回転中心からカメラ基準点までの高さをｈ０、マスト５の回転中心からカメラ基準点までの水平距離をｌ０、マスト５の回転中心の座標を（ｌ０、－ｈ０）、マスト高さをｈｍ、マスト５の垂直方向からの傾き角度をβ、マスト５の回転中心から車体２側へのマスト幅をＷ
ｍ
とすると、マスト５の車体２側の上端部Ｐの座標は、(ｌ０－ｈｍ・ｓｉｎ（β）－Ｗｍ・ｃｏｓ（β）、ｈｍ・ｃｏｓ（β）－ｈ０－Ｗ
ｍ
・ｓｉｎ（β）)で表すことができる。また、マスト５の回転中心に対応する上端部Ｑの座標は、(ｌ０－ｈｍ・ｓｉｎ（β）、ｈｍ・ｃｏｓ（β）－ｈ０)で表すことができる。これらの関係性から次式（１２）が成立する。
α ｌ ＝（θ／２）－ｔａｎ－１（(ｌ０－ｈｍ・ｓｉｎ（β）－Ｗｍ・ｃｏｓ（β））／（ｈｍ・ｃｏｓ（β）－ｈ０－Ｗｍ・ｓｉｎ（β）)）・・・（１３）
ここで、θ、ｌ０、ｈｍ、Ｗｍ、ｈ０は、予め計測されて既知となった値であり、これらの値はマスト映り込み判定処理部２０５Ｄが記憶している。βは、傾き推定部２１０によって算出される。すると、マスト映り込み判定処理部２０５Ｄは、式（１３）によってαｌを算出し、例えば、以下の式（１４）によってマスト映り込み範囲を算出する。
（α ｌ ／θ）×１００＋ｍｒ´（％）・・・（１４）
本実施形態ではマスト５の傾きを考慮してαｌを算出しているので、式（１４）のマージンのｍｒ´には第２実施形態で設定される値よりも小さな値が設定される。また、マスト映り込み判定処理部２０５Ｄは、αｌ＞０が成立する場合にマスト５の映り込みがあると判定する。
α ｌ ＝（θ／２）－ｔａｎ－１（(ｌ０－ｈｍ・ｓｉｎ（β）－Ｗｍ・ｃｏｓ（β））／（ｈｍ・ｃｏｓ（β）－ｈ０－Ｗｍ・ｓｉｎ（β）)）・・・（１３）
ここで、θ、ｌ０、ｈｍ、Ｗｍ、ｈ０は、予め計測されて既知となった値であり、これらの値はマスト映り込み判定処理部２０５Ｄが記憶している。βは、傾き推定部２１０によって算出される。すると、マスト映り込み判定処理部２０５Ｄは、式（１３）によってαｌを算出し、例えば、以下の式（１４）によってマスト映り込み範囲を算出する。
（α ｌ ／θ）×１００＋ｍｒ´（％）・・・（１４）
本実施形態ではマスト５の傾きを考慮してαｌを算出しているので、式（１４）のマージンのｍｒ´には第２実施形態で設定される値よりも小さな値が設定される。また、マスト映り込み判定処理部２０５Ｄは、αｌ＞０が成立する場合にマスト５の映り込みがあると判定する。
【0050】
（動作）
次に位置推定システム１００Ｄによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第５実施形態の位置推定システム１００Ｄの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。
次に位置推定システム１００Ｄによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第５実施形態の位置推定システム１００Ｄの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。
【0051】
（第１実施形態に対応する処理）
マスト５が画像に映り込む条件によって、画像を用いた自己位置推定を行うかどうかを判定する処理の流れは、第１実施形態の図４で説明したものと同様である。但し、図４のステップＳ２の式（３´）がαｌ＞０となる。
マスト５が画像に映り込む条件によって、画像を用いた自己位置推定を行うかどうかを判定する処理の流れは、第１実施形態の図４で説明したものと同様である。但し、図４のステップＳ２の式（３´）がαｌ＞０となる。
【0052】
（第２実施形態に対応する処理）
マスク化画像を用いて画像を用いた自己位置推定を行う場合の処理は、第２実施形態の図７で説明したものと同様である。但し、図７のステップＳ１２の式（３´）がαｌ＞０となり、ステップＳ１３の左辺がαｌとなり、ステップＳ１４のマスク処理する範囲が上記の式（１４）となる。
マスク化画像を用いて画像を用いた自己位置推定を行う場合の処理は、第２実施形態の図７で説明したものと同様である。但し、図７のステップＳ１２の式（３´）がαｌ＞０となり、ステップＳ１３の左辺がαｌとなり、ステップＳ１４のマスク処理する範囲が上記の式（１４）となる。
【0053】
（効果）
以上説明したように、本実施形態によれば、マスト５の傾きを考慮することにより、映り込み範囲のマージンｍｒ´を小さくすることができ、自己位置推定処理に利用可能な画像の範囲を第２実施形態よりも広く確保することができる。これにより、自己位置推定の精度を確保することができる。なお、本実施形態と第３実施形態、第４実施形態とを組み合わせて、荷物の高さを考慮してマスク処理する範囲を算出するようにしてもよい。また、マスト５の傾きを荷物の重量から推定することとしたが、フォークリフト１にマスト５の傾きを計測できるセンサを設け、このセンサが計測するマスト５の傾き量を用いてもよい。
以上説明したように、本実施形態によれば、マスト５の傾きを考慮することにより、映り込み範囲のマージンｍｒ´を小さくすることができ、自己位置推定処理に利用可能な画像の範囲を第２実施形態よりも広く確保することができる。これにより、自己位置推定の精度を確保することができる。なお、本実施形態と第３実施形態、第４実施形態とを組み合わせて、荷物の高さを考慮してマスク処理する範囲を算出するようにしてもよい。また、マスト５の傾きを荷物の重量から推定することとしたが、フォークリフト１にマスト５の傾きを計測できるセンサを設け、このセンサが計測するマスト５の傾き量を用いてもよい。
【0054】
＜第６実施形態＞
以下、本開示の第６実施形態に係る位置推定システム１００Ｅについて図１４～図１６を参照して説明する。上記した第１～第５実施形態では、マスト５とカメラ１２の幾何的関係から、画像中のマスト映り込み範囲を算出している。この場合、カメラ１２の取付時にカメラ１２とマスト５の位置関係の計測が必須となり、導入時に手間が生じる。そこで第６実施形態では、事前にマスト５とカメラ１２の幾何的関係を計測しなくても、車体静止、且つ、フォーク４（マスト５）が最低位置まで降下した状態で天井を撮影した画像を基準として、車体静止、且つ、マスト５が上昇しているときに撮影された画像の変化から、マスト映り込み範囲を推定する。車体が止まっている場合は、原則、撮影画像の変化はほぼないため、車体の静止中でマスト５が上昇している最中に撮影された画像にマスト５や荷物６が映り込んでいる範囲は、車体静止が始まった直後の画像と差分を取ることで容易に推定できる。特定した映り込み範囲とマスト５の高さの対応関係を学習することで、マスト５の高さから映り込み範囲を推定することができる。
以下、本開示の第６実施形態に係る位置推定システム１００Ｅについて図１４～図１６を参照して説明する。上記した第１～第５実施形態では、マスト５とカメラ１２の幾何的関係から、画像中のマスト映り込み範囲を算出している。この場合、カメラ１２の取付時にカメラ１２とマスト５の位置関係の計測が必須となり、導入時に手間が生じる。そこで第６実施形態では、事前にマスト５とカメラ１２の幾何的関係を計測しなくても、車体静止、且つ、フォーク４（マスト５）が最低位置まで降下した状態で天井を撮影した画像を基準として、車体静止、且つ、マスト５が上昇しているときに撮影された画像の変化から、マスト映り込み範囲を推定する。車体が止まっている場合は、原則、撮影画像の変化はほぼないため、車体の静止中でマスト５が上昇している最中に撮影された画像にマスト５や荷物６が映り込んでいる範囲は、車体静止が始まった直後の画像と差分を取ることで容易に推定できる。特定した映り込み範囲とマスト５の高さの対応関係を学習することで、マスト５の高さから映り込み範囲を推定することができる。
【0055】
（位置推定システムの構成）
図１４は、第６実施形態に係る位置推定システムの一例を示す図である。
図１４に示すように位置推定システム１００Ｅは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｅと、マスク画像生成処理部２０６と、静止判定部２１１と、基準画像記録部２１２と、画像比較による映り込み範囲判定部２１３と、映り込み範囲とマスト上昇量記録部２１４と、映り込み範囲関数推定・更新部２１５と、映り込み範囲関数記録部２１６と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｅ、マスク画像生成処理部２０６、静止判定部２１１、基準画像記録部２１２、画像比較による映り込み範囲判定部２１３、映り込み範囲とマスト上昇量記録部２１４、映り込み範囲関数推定・更新部２１５、映り込み範囲関数記録部２１６は、演算装置２０に実装される機能である。第６実施形態に係る構成のうち、第２実施形態に係る位置推定システム１００Ａと同様のものには同じ符号を付し、それぞれの説明を省略する。
図１４は、第６実施形態に係る位置推定システムの一例を示す図である。
図１４に示すように位置推定システム１００Ｅは、第１自己位置推定処理部２０１と、第２自己位置推定処理部２０２と、デッドレコニング処理部２０３と、統合処理部２０４と、マスト映り込み判定処理部２０５Ｅと、マスク画像生成処理部２０６と、静止判定部２１１と、基準画像記録部２１２と、画像比較による映り込み範囲判定部２１３と、映り込み範囲とマスト上昇量記録部２１４と、映り込み範囲関数推定・更新部２１５と、映り込み範囲関数記録部２１６と、ＩＭＵ１０と、エンコーダ１１と、カメラ１２と、揚高センサ１３と、を備える。これらのうち、第１自己位置推定処理部２０１、第２自己位置推定処理部２０２、デッドレコニング処理部２０３、統合処理部２０４、マスト映り込み判定処理部２０５Ｅ、マスク画像生成処理部２０６、静止判定部２１１、基準画像記録部２１２、画像比較による映り込み範囲判定部２１３、映り込み範囲とマスト上昇量記録部２１４、映り込み範囲関数推定・更新部２１５、映り込み範囲関数記録部２１６は、演算装置２０に実装される機能である。第６実施形態に係る構成のうち、第２実施形態に係る位置推定システム１００Ａと同様のものには同じ符号を付し、それぞれの説明を省略する。
【0056】
静止判定部２１１は、エンコーダ１１の計測値に基づいて、車速が０付近である場合に、フォークリフト１が静止状態にあると判定する。
基準画像記録部２１２は、静止判定部２１１によって静止状態にあると判定され、且つ、揚高センサ１３が計測したフォーク４の高さ（マスト５の高さ）が最低位置にあるときにカメラ１２が撮影した画像を取得し、基準画像として記録・記憶する。
基準画像記録部２１２は、静止判定部２１１によって静止状態にあると判定され、且つ、揚高センサ１３が計測したフォーク４の高さ（マスト５の高さ）が最低位置にあるときにカメラ１２が撮影した画像を取得し、基準画像として記録・記憶する。
【0057】
画像比較による映り込み範囲判定部２１３は、静止判定部２１１によって静止状態にあると判定され、且つ、揚高センサ１３が計測したフォーク４の高さが最低位置以外にあるときにカメラ１２が撮影した画像を取得し、基準画像と取得した画像の差分を取り、差分が大きい範囲をマスト等（マスト５だけでなく荷物を含む）の映り込み範囲として判定する。画像比較による映り込み範囲判定部２１３は、取得した画像が撮影されたときの揚高センサ１３による計測値と、判定した映り込み範囲を対応付けて映り込み範囲とマスト上昇量記録部２１４へ出力する。
【0058】
映り込み範囲とマスト上昇量記録部２１４は、画像比較による映り込み範囲判定部２１３から取得した揚高センサ１３による計測値からマスト上昇量を算出し、マスト等の映り込み範囲と算出したマスト上昇量の組を記録・記憶する。また、映り込み範囲とマスト上昇量記録部２１４は、記録した組が、一定数を超えた場合には、古い順、かつ、重複のあるデータを優先して削除する。
【0059】
映り込み範囲関数推定・更新部２１５は、映り込み範囲とマスト上昇量記録部２１４にマスト等の映り込み範囲とマスト上昇量の組が十分に蓄積された場合に、両者の関係を示す関数を推定し、推定関数を映り込み範囲関数記録部２１６へ出力する。この推定関数は、マスト上昇量を入力すると、マスト等の映り込み範囲を出力するような関数である。また、映り込み範囲関数推定・更新部２１５は、推定関数の推定以降に新たに映り込み範囲とマスト上昇量記録部２１４に記録された、映り込み範囲と、新たに映り込み範囲とマスト上昇量記録部２１４に記録されたマスト上昇量と推定関数を用いて推定した映り込み範囲と、を比較して、その差が基準を超える場合、推定関数を更新すると判定する。推定関数を更新すると判定すると、映り込み範囲関数推定・更新部２１５は、新たに記録された映り込み範囲とマスト上昇量の組を用いて、両者の関係を示す関数を推定し直すことにより、推定関数を更新する。更新された推定関数は、映り込み範囲関数記録部２１６へ出力される。例えば、フォークリフト１が移動し、光源とカメラ１２の位置関係が変化すると、マスト５の上昇量が同じでもカメラ１２の撮影画像に写るマスト５によって視界が遮られる範囲が変わって見えることがあり、このような場合に推定関数の更新が行われる。
映り込み範囲関数記録部２１６は、映り込み範囲の推定関数を記録・記憶する。
映り込み範囲関数記録部２１６は、映り込み範囲の推定関数を記録・記憶する。
【0060】
マスト映り込み判定処理部２０５Ｅは、揚高センサ１３が計測した計測値を取得し、例えば、計測値からフォーク４が最低位置にあるときの揚高センサ１３の計測値を減算する等して、マスト５の上昇量を算出する。マスト映り込み判定処理部２０５Ｅは、映り込み範囲関数記録部２１６が記憶する推定関数に、算出したマスト５の上昇量を入力して、マスト等の映り込み範囲を算出する。そして、推定関数に基づくマスト等の映り込み範囲が許容できない場合に、画像を用いた自己位置推定処理を停止する。
【0061】
図１５、図１６は、それぞれ、第６実施形態のマスト映り込み範囲推定処理を説明する第１、第２の図である。
図１５に示すように、画像比較による映り込み範囲判定部２１３は、フォークリフト１の停止時に撮影された基準画像１５０と、フォークリフト１の停止時、且つ、マスト５の上昇時に撮影された画像１５１を比較して、画素値の差分が閾値を超える箇所Ｐ１を特定し、この範囲Ｐ１からマスト等の映り込み範囲（例えば、図１５の場合、画像右端からの距離Ｒ１の範囲）を判定する。そして、図１６に示すように、映り込み範囲とマスト上昇量記録部２１４は、距離Ｒ１で規定されるマスト等の映り込み範囲とマスト上昇量ｈ１の組を記録し、距離Ｒ２で規定されるマスト等の映り込み範囲とマスト上昇量ｈ２の組を記録する。
図１５に示すように、画像比較による映り込み範囲判定部２１３は、フォークリフト１の停止時に撮影された基準画像１５０と、フォークリフト１の停止時、且つ、マスト５の上昇時に撮影された画像１５１を比較して、画素値の差分が閾値を超える箇所Ｐ１を特定し、この範囲Ｐ１からマスト等の映り込み範囲（例えば、図１５の場合、画像右端からの距離Ｒ１の範囲）を判定する。そして、図１６に示すように、映り込み範囲とマスト上昇量記録部２１４は、距離Ｒ１で規定されるマスト等の映り込み範囲とマスト上昇量ｈ１の組を記録し、距離Ｒ２で規定されるマスト等の映り込み範囲とマスト上昇量ｈ２の組を記録する。
【0062】
（動作）
次に位置推定システム１００Ｅによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第６実施形態の位置推定システム１００Ｅの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。
次に位置推定システム１００Ｅによる測位処理のうちの画像を用いた自己位置推定処理の流れについて説明する。第６実施形態の位置推定システム１００Ｅの動作には、第１実施形態の測位処理、第２実施形態の測位処理の何れも適用することができる。
【0063】
（第１実施形態に対応する処理）
マスト５等が画像に映り込む条件によって画像を用いた自己位置推定を行うかどうかを判定する処理の流れは、第１実施形態の図４で説明したものと同様である。但し、図４のステップＳ２の式（３´）が、例えば、推定関数に基づいて算出したマスト等の映り込み範囲＞所定の第１閾値となる。
マスト５等が画像に映り込む条件によって画像を用いた自己位置推定を行うかどうかを判定する処理の流れは、第１実施形態の図４で説明したものと同様である。但し、図４のステップＳ２の式（３´）が、例えば、推定関数に基づいて算出したマスト等の映り込み範囲＞所定の第１閾値となる。
【0064】
（第２実施形態に対応する処理）
マスク化画像を用いて画像を用いた自己位置推定を行う場合の処理は、第２実施形態の図７で説明したものと同様である。但し、図７のステップＳ１２の式（３´）が例えば、推定関数に基づいて算出したマスト等の映り込み範囲＞所定の第１閾値となり、ステップＳ１３が例えば、推定関数に基づいて算出したマスト等の映り込み範囲＞所定の第２閾値（第２閾値＞第１閾値）となり、ステップＳ１４のマスク処理する範囲を求める式が、例えば次式（１５）となる。
（推定関数に基づいて算出したマスト等の映り込み範囲／画像の全面積）×１００＋ｍｒ´´（％）・・・（１５）
ｍｒ´´は任意のマージンである。
マスク化画像を用いて画像を用いた自己位置推定を行う場合の処理は、第２実施形態の図７で説明したものと同様である。但し、図７のステップＳ１２の式（３´）が例えば、推定関数に基づいて算出したマスト等の映り込み範囲＞所定の第１閾値となり、ステップＳ１３が例えば、推定関数に基づいて算出したマスト等の映り込み範囲＞所定の第２閾値（第２閾値＞第１閾値）となり、ステップＳ１４のマスク処理する範囲を求める式が、例えば次式（１５）となる。
（推定関数に基づいて算出したマスト等の映り込み範囲／画像の全面積）×１００＋ｍｒ´´（％）・・・（１５）
ｍｒ´´は任意のマージンである。
【0065】
（効果）
以上説明したように、本実施形態によれば、事前にマスト５とカメラ１２の幾何的関係を測定および設定する必要がなくなるため、位置推定システム１００Ｅの導入に伴う作業時間や作業コストを低減することができる。なお、第５実施形態で説明した方法でマスト５の傾き量を算出し、マスト５の傾き量とマスト５の上昇量とマスト等の映り込み範囲の関係を示す推定関数を生成して、この推定関数にマスト５の傾き量とマスト５の上昇量を入力してマスト等の映り込み範囲を算出するようにしてもよい。
以上説明したように、本実施形態によれば、事前にマスト５とカメラ１２の幾何的関係を測定および設定する必要がなくなるため、位置推定システム１００Ｅの導入に伴う作業時間や作業コストを低減することができる。なお、第５実施形態で説明した方法でマスト５の傾き量を算出し、マスト５の傾き量とマスト５の上昇量とマスト等の映り込み範囲の関係を示す推定関数を生成して、この推定関数にマスト５の傾き量とマスト５の上昇量を入力してマスト等の映り込み範囲を算出するようにしてもよい。
【0066】
図１７は、各実施形態に係る位置推定システムのハードウェア構成の一例を示す図である。
コンピュータ９００は、ＣＰＵ９０１、主記憶装置９０２、補助記憶装置９０３、入出力インタフェース９０４、通信インタフェース９０５を備える。上述の演算装置２０は、コンピュータ９００に実装される。そして、上述した各機能は、プログラムの形式で補助記憶装置９０３に記憶されている。ＣＰＵ９０１は、プログラムを補助記憶装置９０３から読み出して主記憶装置９０２に展開し、当該プログラムに従って上記処理を実行する。また、ＣＰＵ９０１は、プログラムに従って、記憶領域を主記憶装置９０２に確保する。また、ＣＰＵ９０１は、プログラムに従って、処理中のデータを記憶する記憶領域を補助記憶装置９０３に確保する。
コンピュータ９００は、ＣＰＵ９０１、主記憶装置９０２、補助記憶装置９０３、入出力インタフェース９０４、通信インタフェース９０５を備える。上述の演算装置２０は、コンピュータ９００に実装される。そして、上述した各機能は、プログラムの形式で補助記憶装置９０３に記憶されている。ＣＰＵ９０１は、プログラムを補助記憶装置９０３から読み出して主記憶装置９０２に展開し、当該プログラムに従って上記処理を実行する。また、ＣＰＵ９０１は、プログラムに従って、記憶領域を主記憶装置９０２に確保する。また、ＣＰＵ９０１は、プログラムに従って、処理中のデータを記憶する記憶領域を補助記憶装置９０３に確保する。
【0067】
なお、演算装置２０の全部または一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各機能部による処理を行ってもよい。ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、ＣＤ、ＤＶＤ、ＵＳＢ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また、このプログラムが通信回線によってコンピュータ９００に配信される場合、配信を受けたコンピュータ９００が当該プログラムを主記憶装置９０２に展開し、上記処理を実行しても良い。また、上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであってもよい。
【0068】
以上のとおり、本開示に係るいくつかの実施形態を説明したが、これら全ての実施形態は、例として提示したものであり、発明の範囲を限定することを意図していない。これらの実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で種々の省略、置き換え、変更を行うことができる。これらの実施形態及びその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【0069】
＜付記＞
各実施形態に記載の位置推定システム、フォークリフト、位置推定方法及びプログラムは、例えば以下のように把握される。
各実施形態に記載の位置推定システム、フォークリフト、位置推定方法及びプログラムは、例えば以下のように把握される。
【0070】
（１）第１の態様に係る位置推定システム１００～１００Ｅは、カメラ１２を備えるフォークリフト１の位置を推定する位置推定システムであって、前記カメラ１２によって撮影された画像に含まれる特徴に基づいて、前記フォークリフト１の位置を推定する位置推定部（第1自己位置推定処理部２０１及び第２自己位置推定処理部２０２）と、前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記画像において前記マストが写る範囲を示す映り込み範囲を算出し、算出した前記映り込み範囲が許容範囲を超える場合には、前記位置推定部による前記位置の推定を停止すると判定する（ステップＳ２、Ｓ１３）映り込み判定部（マスト映り込み判定処理部２０５、２０５Ｂ、２０５Ｅ）と、を備える。
これにより、自己位置推定処理における推定精度の確保と消費電力の低減を実現することができる。
これにより、自己位置推定処理における推定精度の確保と消費電力の低減を実現することができる。
【0071】
（２）第２の態様に係る位置推定システムは、（１）の位置推定システムであって、前記映り込み範囲が前記許容範囲内の場合、前記映り込み範囲を前記画像から除外する処理を行う画像処理部（マスク画像生成処理部２０６）をさらに有し、前記位置推定部は、前記除外する処理が行われた後の前記画像に基づいて、前記位置を推定する。
これにより、画像を用いた自己位置推定処理の処理間隔を短くすることにより、推定精度の低下を防ぐことができる。
これにより、画像を用いた自己位置推定処理の処理間隔を短くすることにより、推定精度の低下を防ぐことができる。
【0072】
（３）第３の態様に係る位置推定システムは、（１）～（２）の位置推定システムであって、前記フォークリフトに積載する荷物の高さ情報を取得する荷物情報取得部（荷物情報記録部２０７）をさらに備え、前記映り込み判定部は、前記マストの高さ方向の位置と、前記荷物の高さ情報と、に基づいて、前記映り込み範囲を算出する。
これにより、マストだけではなく荷物の映り込みを考慮して、マスト及び荷物の映り込み範囲を算出することができる。
これにより、マストだけではなく荷物の映り込みを考慮して、マスト及び荷物の映り込み範囲を算出することができる。
【0073】
（４）第４の態様に係る位置推定システムは、（３）の位置推定システムであって、前記荷物の密度と、前記荷物を前記フォークリフトに積載するときの積載面の面積と、前記荷物の重量と、に基づいて前記荷物の高さ情報を推定する高さ推定部、をさらに備える。
これにより、荷物の高さを設定する必要無く、マストと荷物の映り込み範囲を算出することができる。
これにより、荷物の高さを設定する必要無く、マストと荷物の映り込み範囲を算出することができる。
【0074】
（５）第５の態様に係る位置推定システムは、（１）～（４）の位置推定システムであって、前記映り込み判定部は、前記マストの高さ方向の位置と、前記マストの傾きと、に基づいて、前記映り込み範囲を算出する。
これにより、マストの傾きを考慮して、マストの映り込み範囲を算出することができる。
これにより、マストの傾きを考慮して、マストの映り込み範囲を算出することができる。
【0075】
（６）第６の態様に係る位置推定システムは、（１）～（５）の位置推定システムであって、前記映り込み判定部は、前記カメラと前記マストの高さ位置の幾何的な関係に基づいて、前記映り込み範囲を算出する。
これにより、カメラとマストの位置関係を設定するだけマストの映り込みは範囲を算出することができる。
これにより、カメラとマストの位置関係を設定するだけマストの映り込みは範囲を算出することができる。
【0076】
（７）第７の態様に係る位置推定システムは、（１）～（６）の位置推定システムであって、前記フォークリフトの停止時であって前記マストが最低位置にあるときに撮影された前記画像と、前記フォークリフトの停止時であって前記マストが様々な位置にあるときに撮影された前記画像の差分から、それぞれの前記マストの高さ位置に応じた前記映り込み範囲を算出し、前記マストの高さと前記映り込み範囲の関係を定めた推定関数を算出する関数推定部をさらに備え、前記映り込み判定部は、前記マストの高さ位置と前記推定関数に基づいて前記映り込み範囲を算出する。
これにより、カメラとマストの位置関係を設定する必要が無く、画像処理によってのみマストの映り込みは範囲を算出することができる。
これにより、カメラとマストの位置関係を設定する必要が無く、画像処理によってのみマストの映り込みは範囲を算出することができる。
【0077】
（８）第８の態様に係るフォークリフトは、（１）～（７）の位置推定システムと、カメラと、を備える。
【0078】
（９）第９の態様に係る位置推定方法は、カメラを備えるフォークリフトの位置を推定する位置推定方法であって、前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記カメラによって撮影された画像において前記マストが写る範囲を示す映り込み範囲と、を算出するステップと、算出した前記映り込み範囲が許容範囲を超えるか否かを判定するステップと、前記許容範囲を超える場合には、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する処理を停止すると判定するステップと、を有する。
【0079】
（１０）第１０の態様に係るプログラムは、コンピュータに、カメラを備えるフォークリフトの位置を推定する処理であって、前記フォークリフトが備えるマストの高さ方向の位置に基づいて、前記カメラによって撮影された画像において前記マストが写る範囲を示す映り込み範囲と、を算出するステップと、算出した前記映り込み範囲が許容範囲を超えるか否かを判定するステップと、前記許容範囲を超える場合には、前記カメラによって撮影された画像に含まれる特徴に基づいて、前記フォークリフトの位置を推定する処理を停止すると判定するステップと、を有する処理を実行させる。
【符号の説明】
【0080】
１・・・フォークリフト
２・・・車体
３・・・荷役装置
４・・・フォーク
５・・・マスト
６・・・荷物
７・・・パレット
１０・・・ＩＭＵ
１１・・・エンコーダ
１２・・・カメラ
１３・・・揚高センサ
１４・・・荷重センサ
１００、１００Ａ、１００Ｂ、１００Ｃ、１００Ｄ、１００Ｅ・・・位置推定システム
２０１・・・第１自己位置推定処理部
２０２・・・第２自己位置推定処理部
２０３・・・デッドレコニング処理部
２０４・・・統合処理部
２０５、２０５Ｂ、２０５Ｄ、２０５Ｅ・・・マスト映り込み判定処理部
２０６・・・マスク画像生成処理部
２０７・・・荷物情報記録部
２０８・・・荷物密度記録部
２０９・・・荷物高さ推定部
２１０・・・傾き推定部
２１１・・・静止判定部
２１２・・・基準画像記録部
２１３・・・画像比較による映り込み範囲判定部
２１４・・・映り込み範囲とマスト上昇量記録部
２１５・・・映り込み範囲関数推定・更新部
２１６・・・映り込み範囲関数記録部
９００・・・コンピュータ
９０１・・・ＣＰＵ
９０２・・・主記憶装置
９０３・・・補助記憶装置
９０４・・・入出力インタフェース
９０５・・・通信インタフェース
２・・・車体
３・・・荷役装置
４・・・フォーク
５・・・マスト
６・・・荷物
７・・・パレット
１０・・・ＩＭＵ
１１・・・エンコーダ
１２・・・カメラ
１３・・・揚高センサ
１４・・・荷重センサ
１００、１００Ａ、１００Ｂ、１００Ｃ、１００Ｄ、１００Ｅ・・・位置推定システム
２０１・・・第１自己位置推定処理部
２０２・・・第２自己位置推定処理部
２０３・・・デッドレコニング処理部
２０４・・・統合処理部
２０５、２０５Ｂ、２０５Ｄ、２０５Ｅ・・・マスト映り込み判定処理部
２０６・・・マスク画像生成処理部
２０７・・・荷物情報記録部
２０８・・・荷物密度記録部
２０９・・・荷物高さ推定部
２１０・・・傾き推定部
２１１・・・静止判定部
２１２・・・基準画像記録部
２１３・・・画像比較による映り込み範囲判定部
２１４・・・映り込み範囲とマスト上昇量記録部
２１５・・・映り込み範囲関数推定・更新部
２１６・・・映り込み範囲関数記録部
９００・・・コンピュータ
９０１・・・ＣＰＵ
９０２・・・主記憶装置
９０３・・・補助記憶装置
９０４・・・入出力インタフェース
９０５・・・通信インタフェース
【図1】
【図2】
【図3A】
【図3B】
【図3C】
【図3D】
【図4】
【図5】
【図6A】
【図6B】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】