JP2022051191A - 自己位置推定装置 - Google Patents

Abstract

【課題】移動体の自己位置の推定精度を向上させることができる自己位置推定装置を提供する。
【解決手段】自己位置推定装置１５は、移動体２から移動体２の周囲に存在する物体６までの距離を検出するレーザセンサ３と、レーザセンサ３により検出された移動体２から物体６までの距離データと移動体２の周囲環境の地図情報とに基づいて、移動体２の自己位置を推定する自己位置推定部１３と、地図情報に登録されていない障害物７を検知する障害物検知部１１とを備え、自己位置推定部１３は、障害物検知部１１により障害物７が検知されたときは、移動体２から物体６までの距離データのうち障害物７を除いた物体６までの距離データと地図情報とに基づいて、移動体２の自己位置を推定する。
【選択図】図１

Description

本発明は、自己位置推定装置に関する。

従来の自己位置推定装置としては、例えば特許文献１に記載されているような技術が知られている。特許文献１に記載の自己位置推定装置は、ロボットと周囲にある物体との距離を計測する距離センサであるレーザ測長器と、このレーザ測長器により計測された距離データとコントローラの地図データ記憶部に記憶された地図データとを照合することで、ロボットの位置を同定する同定装置とを備えている。

特開２０１２－２５６３４４号公報

しかしながら、上記従来技術においては、以下の問題点が存在する。即ち、ロボット等の移動体を使用して作業を行う環境では、移動体と作業者とが共存する。そのような環境下において、移動体の自己位置の推定を行いながら移動体を走行させる際、移動体の周辺に存在する作業者は地図データには無い障害物である。このとき、作業者は地図データ上の特徴物と誤認識されるため、移動体の自己位置の推定精度が低下してしまう。

本発明の目的は、移動体の自己位置の推定精度を向上させることができる自己位置推定装置を提供することである。

本発明の一態様に係る自己位置推定装置は、移動体から移動体の周囲に存在する物体までの距離を検出する距離検出部と、距離検出部により検出された移動体から物体までの距離データと移動体の周囲環境の地図情報とに基づいて、移動体の自己位置を推定する自己位置推定部と、地図情報に登録されていない障害物を検知する障害物検知部とを備え、自己位置推定部は、障害物検知部により障害物が検知されたときは、移動体から物体までの距離データのうち障害物を除いた物体までの距離データと地図情報とに基づいて、移動体の自己位置を推定する。

このような自己位置推定装置においては、距離検出部により移動体から移動体の周囲に存在する物体までの距離が検出され、移動体から物体までの距離データと移動体の周囲環境の地図情報とに基づいて、移動体の自己位置が推定される。このとき、地図情報に登録されていない障害物が検知されると、移動体から物体までの距離データのうち障害物を除いた物体までの距離データと地図情報とに基づいて、移動体の自己位置が推定される。従って、移動体の自己位置の推定処理において、移動体から障害物までの距離データは使用されないため、自己位置の推定精度の悪化要因が除去されることになる。これにより、移動体の自己位置の推定精度が向上する。

障害物検知部は、距離検出部により検出された移動体から物体までの距離データに基づいて、障害物を検知してもよい。このような構成では、移動体の自己位置を推定するための距離検出部が障害物の検知処理にも利用されることになる。従って、障害物を検知するための専用のセンサ等が不要となる。これにより、低コスト化が図られる。

自己位置推定装置は、障害物検知部により障害物が検知されたときに、移動体に対する障害物の位置を算出する位置算出部を更に備え、自己位置推定部は、障害物検知部により障害物が検知されたときは、移動体から物体までの距離データのうち位置算出部により算出された障害物の位置に相当する距離データを削除して、移動体の自己位置を推定してもよい。このような構成では、移動体に対する障害物の位置が算出され、移動体から物体までの距離データのうち障害物の位置に相当する距離データが削除されるため、移動体から障害物までの距離データが容易に除かれる。

本発明によれば、移動体の自己位置の推定精度を向上させることができる。

本発明の一実施形態に係る自己位置推定装置を備えた走行制御装置の概略構成を示すブロック図である。 レーザセンサからレーザが照射される様子を示す概念図である。 図１に示された障害物検知部により実行される障害物検知処理の手順を示すフローチャートである。 図１に示された位置算出部により実行される位置算出処理の手順を示すフローチャートである。 図１に示された自己位置推定部により実行される自己位置推定処理の手順を示すフローチャートである。 移動体の周辺に存在する作業者が地図上の特徴物と誤認識される様子を示す概念図である。 障害物の位置に相当する距離データが削除された残りの距離データを使用して、移動体の自己位置を推定する様子を示す概念図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。

図１は、本発明の一実施形態に係る自己位置推定装置を備えた走行制御装置の概略構成を示すブロック図である。図１において、走行制御装置１は、例えば倉庫内においてフォークリフト等の移動体２（図２参照）を自動的に走行させるように制御する装置である。

走行制御装置１は、移動体２に搭載されている。走行制御装置１は、レーザセンサ３と、コントローラ４と、駆動部５とを備えている。

レーザセンサ３は、図２に示されるように、移動体２から移動体２の周囲に位置する物体６までの距離を検出する距離センサ（距離検出部）である。物体６は、壁６Ａ（図２参照）または柱６Ｂ（図７参照）等の特徴物であり、地図情報（後述）として登録されている。

レーザセンサ３としては、例えばレーザレンジファインダ等が使用される。レーザセンサ３は、移動体２の周囲にレーザＬを照射し、物体６で反射されるレーザＬを受光することにより、移動体２から物体６までの距離を検出する。

レーザセンサ３は、例えば移動体２の前方直進を中心とした所定の角度範囲（例えば２７０度）でレーザＬを照射する。レーザセンサ３のタイプとしては、例えばレーザＬを２次元に照射する２Ｄタイプでもよいし、レーザＬを３次元に照射する３Ｄタイプでもよい。

コントローラ４は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ４は、レーザセンサ３により検出された移動体２から物体６までの距離データを入力し、所定の処理を実行し、移動体２を自動走行させるように駆動部５を制御する。駆動部５は、例えば移動体２を走行させる走行モータと、移動体２を操舵させる操舵モータとを有している。

コントローラ４は、地図情報記憶部１０と、障害物検知部１１と、位置算出部１２と、自己位置推定部１３と、駆動制御部１４とを有している。地図情報記憶部１０は、移動体２の周囲環境の地図情報を記憶する。

障害物検知部１１は、レーザセンサ３により検出された移動体２から物体６までの距離データに基づいて、地図情報記憶部１０に記憶された地図情報に登録されていない障害物７（図７参照）を検知する。障害物７は、物体６の一つである。代表的な障害物７としては、作業者が挙げられる。

位置算出部１２は、障害物検知部１１により障害物７が検知されたときに、移動体２に対する障害物７の位置を算出する。

自己位置推定部１３は、レーザセンサ３により検出された移動体２から物体６までの距離データと地図情報記憶部１０に記憶された移動体２の周囲環境の地図情報とに基づいて、移動体２の自己位置を推定する。自己位置推定部１３は、障害物検知部１１により障害物７が検知されたときは、移動体２から物体６までの距離データのうち障害物７を除いた物体６までの距離データと地図情報とに基づいて、移動体２の自己位置を推定する。

駆動制御部１４は、自己位置推定部１３により推定された移動体２の自己位置に基づいて、移動体２を目的地まで走行させるように駆動部５を制御する。

ここで、レーザセンサ３、コントローラ４の地図情報記憶部１０、障害物検知部１１、位置算出部１２及び自己位置推定部１３は、本実施形態の自己位置推定装置１５を構成している。

図３は、障害物検知部１１により実行される障害物検知処理の手順を示すフローチャートである。図３において、障害物検知部１１は、まず障害物フラグを０にセットする（手順Ｓ１０１）。続いて、障害物検知部１１は、レーザセンサ３により検出された移動体２から物体６までの距離データを取得する（手順Ｓ１０２）。

続いて、障害物検知部１１は、移動体２から物体６までの距離データに基づいて、移動体２の周囲に地図情報に登録されていない障害物７が存在するかどうかを判断する（手順Ｓ１０３）。このとき、障害物検知部１１は、例えば物体６における複数のレーザＬの反射点（レーザ点）の位置に基づいて、物体６の形状及び寸法と物体６が移動しているか否かとを推定し、物体６が障害物７であるかどうかを判定する。

障害物検知部１１は、移動体２の周囲に障害物７が存在すると判断したときは、障害物フラグを１にセットする（手順Ｓ１０４）。障害物検知部１１は、移動体２の周囲に障害物７が存在しないと判断したときは、手順１０４を実行しない。そして、障害物検知部１１は、障害物フラグデータを位置算出部１２及び自己位置推定部１３に出力し（手順Ｓ１０５）、上記の手順Ｓ１０１を再度実行する。

図４は、位置算出部１２により実行される位置算出処理の手順を示すフローチャートである。図４において、位置算出部１２は、障害物検知部１１により設定された障害物フラグが１であるかどうかを判断する（手順Ｓ１１１）。

位置算出部１２は、障害物フラグが１であると判断したときは、移動体２に対する障害物７の位置を算出する（手順Ｓ１１２）。移動体２に対する障害物７の位置は、２次元座標（ＸＹ座標）及び向き（角度）で表される。そして、位置算出部１２は、移動体２に対する障害物７の位置データを自己位置推定部１３に出力し（手順Ｓ１１３）、上記の手順Ｓ１１１を再度実行する。

位置算出部１２は、障害物フラグが１でなく０であると判断したときは、手順Ｓ１１２，Ｓ１１３を実行せずに、上記の手順Ｓ１１１を再度実行する。

図５は、自己位置推定部１３により実行される自己位置推定処理の手順を示すフローチャートである。図５において、自己位置推定部１３は、まずレーザセンサ３により検出された移動体２から物体６までの距離データを取得する（手順Ｓ１２１）。

続いて、自己位置推定部１３は、障害物検知部１１により設定された障害物フラグが１であるかどうかを判断する（手順Ｓ１２２）。自己位置推定部１３は、障害物フラグが１であると判断したときは、移動体２から物体６までの距離データのうち位置算出部１２により算出された障害物７の位置に相当する距離データを削除する（手順Ｓ１２３）。自己位置推定部１３は、障害物フラグが１でなく０であると判断したときは、手順Ｓ１２３を実行しない。

自己位置推定部１３は、地図情報記憶部１０に記憶された移動体２の周囲環境の地図情報を取得する（手順Ｓ１２４）。そして、自己位置推定部１３は、移動体２から物体６までの距離データと移動体２の周囲環境の地図情報とに基づいて、移動体２の現在の自己位置の推定演算を行う（手順Ｓ１２５）。

このとき、自己位置推定部１３は、レーザＳＬＡＭ（simultaneous localization andmapping）手法を用いて、移動体２の自己位置の推定演算を行う。ＳＬＡＭは、センサデータ及び地図データを用いて自己位置推定を行う自己位置推定技術である。自己位置推定部１３は、移動体２から物体６までの距離と移動体２の周囲環境の地図とをマッチングさせて、移動体２の自己位置の推定演算を行う。移動体２の自己位置は、移動体２の２次元座標（ＸＹ座標）及び向き（角度）で表される。

続いて、自己位置推定部１３は、移動体２の自己位置データを駆動制御部１４に出力し（手順Ｓ１２６）、上記の手順Ｓ１２１を再度実行する。

ところで、図６（ａ）に示されるように、移動体２の周辺に作業者Ｈが存在する場合には、以下の不具合が発生する。即ち、作業者Ｈは、地図情報記憶部１０に記憶された地図情報に無い障害物７である。このため、作業者Ｈでのレーザ点が自己位置の推定処理に反映されると、作業者Ｈが地図上の特徴物である壁６Ａと誤認識される（２点鎖線Ｐ参照）。その結果、図６（ｂ）に示されるように、移動体２の自己位置推定値は実際の位置（１点鎖線Ｓ参照）から大きくずれてしまう。

そのような課題に対し、本実施形態では、図７（ａ）に示されるように、レーザセンサ３により検出された移動体２からの距離データに基づいて、作業者Ｈ（障害物７）が検知される。そして、移動体２に対する作業者Ｈの位置が算出される。そして、図７（ｂ）に示されるように、レーザセンサ３により検出された移動体２からの距離データのうち作業者Ｈの位置に相当する距離データが削除された残りの距離データを使用して、移動体２の自己位置が推定される。

以上のように本実施形態によれば、レーザセンサ３により移動体２から移動体２の周囲に存在する物体６までの距離が検出され、移動体２から物体６までの距離データと移動体２の周囲環境の地図情報とに基づいて、移動体２の自己位置が推定される。このとき、地図情報に登録されていない障害物７が検知されると、移動体２から物体６までの距離データのうち障害物７を除いた物体６までの距離データと地図情報とに基づいて、移動体２の自己位置が推定される。従って、移動体２の自己位置の推定処理において、移動体２から障害物７までの距離データは使用されないため、自己位置の推定精度の悪化要因が除去されることになる。これにより、移動体２の自己位置の推定精度が向上する。その結果、移動体２の自己位置推定値と移動体２の実際の位置（真値）とのずれを低減することができる。

また、本実施形態では、レーザセンサ３により検出された移動体２から物体６までの距離データに基づいて、障害物７が検知される。よって、移動体２の自己位置を推定するためのレーザセンサ３が障害物７の検知処理にも利用されることになる。従って、障害物７を検知するための専用のセンサ等が不要となる。これにより、低コスト化が図られる。

また、本実施形態では、移動体２に対する障害物７の位置が算出され、移動体２から物体６までの距離データのうち障害物７の位置に相当する距離データが削除されるため、移動体２から障害物７までの距離データが容易に除かれる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、レーザセンサ３により検出された移動体２から物体６までの距離データに基づいて、障害物７が検知されているが、特にその形態には限られず、例えばカメラの撮像画像データに基づいて、障害物７を検知してもよい。

また、上記実施形態では、レーザセンサ３の受光データを利用したレーザＳＬＡＭ手法を用いて、移動体２の自己位置が推定されているが、特にその形態には限られず、カメラの撮像画像データを利用したＶｉｓｕａｌ ＳＬＡＭ手法等を用いて、移動体２の自己位置を推定してもよい。この場合には、同じカメラの撮像画像データに基づいて障害物７を検知してもよいし、或いはレーザセンサの受光データに基づいて障害物７を検知してもよい。

また、障害物７を検知するためのセンサとしては、レーザセンサ及びカメラ以外にも、超音波センサや赤外線センサ等であってもよい。

また、上記実施形態の自己位置推定装置１５は、移動体２としてのフォークリフトの自己位置を推定しているが、本発明は、特にフォークリフト以外の車両や移動ロボット等の自己位置の推定にも適用可能である。

２…移動体、３…レーザセンサ（距離検出部）、６…物体、７…障害物、１１…障害物検知部、１２…位置算出部、１３…自己位置推定部、１５…自己位置推定装置。

Claims (3)

1. 移動体から前記移動体の周囲に存在する物体までの距離を検出する距離検出部と、
   前記距離検出部により検出された前記移動体から前記物体までの距離データと前記移動体の周囲環境の地図情報とに基づいて、前記移動体の自己位置を推定する自己位置推定部と、
   前記地図情報に登録されていない障害物を検知する障害物検知部とを備え、
   前記自己位置推定部は、前記障害物検知部により前記障害物が検知されたときは、前記移動体から前記物体までの距離データのうち前記障害物を除いた物体までの距離データと前記地図情報とに基づいて、前記移動体の自己位置を推定する自己位置推定装置。
2. 前記障害物検知部は、前記距離検出部により検出された前記移動体から前記物体までの距離データに基づいて、前記障害物を検知する請求項１記載の自己位置推定装置。
3. 前記障害物検知部により前記障害物が検知されたときに、前記移動体に対する前記障害物の位置を算出する位置算出部を更に備え、
   前記自己位置推定部は、前記障害物検知部により前記障害物が検知されたときは、前記移動体から前記物体までの距離データのうち前記位置算出部により算出された前記障害物の位置に相当する距離データを削除して、前記移動体の自己位置を推定する請求項１または２記載の自己位置推定装置。

Images