特許ウォッチ

公開番号2024064664
公報種別公開特許公報(A)
公開日2024-05-14
出願番号2022173422
出願日2022-10-28
発明の名称自己位置推定装置
出願人株式会社豊田自動織機
代理人個人,個人
主分類G01C 21/28 20060101AFI20240507BHJP(測定;試験)
要約【課題】初期位置の精度を向上させること。
【解決手段】制御装置は、移動体の自己位置を表すグローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得する。制御装置は、地図データ上で、水平方向座標を中心とする立体柱であってZ軸に延びる立体柱を生成する。制御装置は、Z座標の変化と、立体柱に収容される点の密度であって水平面に分布する点の密度の変化との対応関係を算出する。制御装置は、点の密度が極大値となるZ座標に基づいて、自己位置を表すグローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出する。制御装置は、水平方向座標と鉛直方向座標で表されるグローバル座標系での三次元座標を初期位置として設定する。
【選択図】図4
特許請求の範囲【請求項１】
移動体に搭載される自己位置推定装置であって、
グローバル座標系での三次元座標を表す点によって環境地図を表した地図データを記憶する記憶装置と、
前記移動体の周辺環境をローカル座標系での三次元座標として検出するセンサと、
制御装置と、を備え、
前記制御装置は、
前記移動体の自己位置を表す前記グローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得し、
前記グローバル座標系の座標軸のうち上下方向を表す座標軸をＺ軸とすると、前記地図データ上で、前記水平方向座標を中心とする立体柱であって前記Ｚ軸に延びる立体柱を生成し、
前記Ｚ軸の座標であるＺ座標の変化と、前記立体柱に収容される前記点の密度であって水平面に分布する前記点の密度の変化との対応関係を算出し、
前記点の密度が極大値となる前記Ｚ座標に基づいて、前記自己位置を表す前記グローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出し、
前記水平方向座標と前記鉛直方向座標で表される前記グローバル座標系での三次元座標を初期位置として設定してスキャンマッチングを開始する、自己位置推定装置。
続きを表示（約 170 文字）【請求項２】
前記制御装置は、
前記点の密度が前記極大値となる前記Ｚ座標が複数存在する場合、前記点の密度が前記極大値となる前記Ｚ座標のそれぞれに基づいて算出された前記鉛直方向座標毎に信頼度を算出し、
前記水平方向座標と最も前記信頼度が高い前記鉛直方向座標を、前記初期位置として設定する、請求項１に記載の自己位置推定装置。

発明の詳細な説明【技術分野】
【０００１】
本開示は、自己位置推定装置に関する。
続きを表示（約 1,900 文字）【背景技術】
【０００２】
特許文献１に開示の自己位置推定方法は、移動体の自己位置を推定する。自己位置推定方法は、センサから得られたローカル座標系での点と、地図データによって表されるグローバル座標系での点とのマッチングを行うことで、グローバル座標系での自己位置を推定する。
【先行技術文献】
【特許文献】
【０００３】
特開２０２２－４２６３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
グローバル座標系での全域に対してマッチングを行うと処理負荷が大きいため、マッチングを開始する前には、グローバル座標系での初期位置を設定する必要がある。この際、初期位置のうちグローバル座標系での上下方向の座標であるＺ座標の精度が低くなる場合がある。例えば、初期位置は、GPS(Global Positioning System)を用いて設定されるが、GPSを用いる場合にはＺ座標のばらつきが大きくなりやすい。
【課題を解決するための手段】
【０００５】
上記課題を解決する自己位置推定装置は、移動体に搭載される自己位置推定装置であって、グローバル座標系での三次元座標を表す点によって環境地図を表した地図データを記憶する記憶装置と、前記移動体の周辺環境をローカル座標系での三次元座標として検出するセンサと、制御装置と、を備え、前記制御装置は、前記移動体の自己位置を表す前記グローバル座標系での三次元座標のうち水平方向の座標を表す水平方向座標を取得し、前記グローバル座標系の座標軸のうち上下方向を表す座標軸をＺ軸とすると、前記地図データ上で、前記水平方向座標を中心とする立体柱であって前記Ｚ軸に延びる立体柱を生成し、前記Ｚ軸の座標であるＺ座標の変化と、前記立体柱に収容される前記点の密度であって水平面に分布する前記点の密度の変化との対応関係を算出し、前記点の密度が極大値となる前記Ｚ座標に基づいて、前記自己位置を表す前記グローバル座標系での三次元座標のうち鉛直方向の座標を表す鉛直方向座標を算出し、前記水平方向座標と前記鉛直方向座標で表される前記グローバル座標系での三次元座標を初期位置として設定してスキャンマッチングを開始する。
【０００６】
Ｚ座標の変化と、立体柱に収容される点の密度の変化を算出すると、立体柱に収容される点の密度には極大値が生じる。地図データ上では、路面に対応するＺ座標には、点が密に存在する一方で、路面に対応するＺ座標より下方向には点が存在しない。このため、Ｚ座標を変化させると、路面に対応するＺ座標を境にして立体柱に収容される点の密度に極大値が生じることになる。制御装置は、点の密度が極大値となるＺ座標に基づいて鉛直方向座標を算出する。これにより、初期位置の精度を向上させることができる。
【０００７】
上記自己位置推定装置について、前記制御装置は、前記点の密度が前記極大値となる前記Ｚ座標が複数存在する場合、前記点の密度が前記極大値となる前記Ｚ座標のそれぞれに基づいて算出された前記鉛直方向座標毎に信頼度を算出し、前記水平方向座標と最も前記信頼度が高い前記鉛直方向座標を、前記初期位置として設定してもよい。
【発明の効果】
【０００８】
本発明によれば、初期位置の精度を向上させることができる。
【図面の簡単な説明】
【０００９】
移動体が用いられる環境を示す模式図である。
図１の移動体が備える自己位置推定装置の概略構成図である。
図２の補助記憶装置に記憶された地図データを示す図である。
図２の制御装置が実行する自己位置推定を示すフローチャートである。
Ｚ座標の変化に対する点の密度の変化を示す図である。
【発明を実施するための形態】
【００１０】
自己位置推定装置の一実施形態について説明する。
＜環境＞
図１に示すように、移動体Ｖ１が用いられる環境Ａ１には種々の構造物Ｏ１，Ｏ２，Ｏ３が含まれる。構造物Ｏ１，Ｏ２，Ｏ３は、例えば、建物を含む。詳細にいえば、構造物Ｏ１，Ｏ２，Ｏ３は、壁、柱、及び屋根など、建物を構成する部材を含む。環境Ａ１は、路面Ｒ１を含む。移動体Ｖ１は、例えば、車両、飛行体、及び多足歩行ロボットを含む。車両は、例えば、乗用車、及び産業車両を含む。産業車両は、例えば、フォークリフト、トーイングトラクタ、及び搬送車を含む。
（【００１１】以降は省略されています）

関連特許