特許 7459733 自己位置推定装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-03-25

(45)【発行日】2024-04-02

(54)【発明の名称】自己位置推定装置

(51)【国際特許分類】

   G01C 21/28 20060101AFI20240326BHJP

【ＦＩ】

G01C21/28

【請求項の数】 4

(21)【出願番号】P 2020150494

(22)【出願日】2020-09-08

(65)【公開番号】P2022045026

(43)【公開日】2022-03-18

【審査請求日】2022-12-13

(73)【特許権者】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】100088155

【弁理士】

【氏名又は名称】長谷川 芳樹

(74)【代理人】

【識別番号】100113435

【弁理士】

【氏名又は名称】黒木 義樹

(74)【代理人】

【識別番号】100124062

【弁理士】

【氏名又は名称】三上 敬史

(74)【代理人】

【識別番号】100148013

【弁理士】

【氏名又は名称】中山 浩光

(72)【発明者】

【氏名】服部 晋悟

【審査官】貞光 大樹

(56)【参考文献】

【文献】特許第５９０９４８６（ＪＰ，Ｂ２）

【文献】特開２０１６－２１８０４３（ＪＰ，Ａ）

【文献】特開２０２０－８４６１（ＪＰ，Ａ）

【文献】特開２０１６－８５６８９（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ ２１／００ － ２１／３６

Ｇ０１Ｃ ２３／００ － ２５／００

Ｇ０８Ｇ １／００ － ９９／００

Ｇ０５Ｄ １／００ － １／８７

(57)【特許請求の範囲】

【請求項1】

移動体の自己位置を推定する自己位置推定器と、
前記移動体の状態に関する情報を取得する第１情報取得部と、
前記自己位置推定器による自己位置推定処理に関する情報を取得する第２情報取得部と、
前記第１情報取得部により取得された前記移動体の状態に関する情報と前記第２情報取得部により取得された前記自己位置推定器による自己位置推定処理に関する情報とに基づいて、前記自己位置推定器による前記移動体の自己位置推定の信頼性を判定する信頼性判定部とを備え、
前記第２情報取得部は、前記自己位置推定器による自己位置推定処理に関する情報として、前記自己位置推定器による推定回数及び推定時間と、前記自己位置推定器により得られた前記移動体の自己位置推定値、速度及び角速度の計算値とを取得する自己位置推定装置。

【請求項2】

前記信頼性判定部は、前記自己位置推定器による前記移動体の自己位置推定の信頼性として、前記自己位置推定器による自己位置推定誤差の予測値と前記自己位置推定器による自己位置推定の信頼度とを判定する請求項１記載の自己位置推定装置。

【請求項3】

前記第１情報取得部は、前記移動体の状態に関する情報として、前記移動体の速度及び角速度の実測値を取得する請求項１または２記載の自己位置推定装置。

【請求項4】

前記自己位置推定器は、前記移動体の周囲にレーザを照射し、前記移動体の周囲に位置する物体で反射されたレーザを受光することにより、前記移動体から前記物体までの距離を検出するレーザセンサと、前記レーザセンサにより検出された前記移動体から前記物体までの距離データと前記移動体の周囲環境の地図情報とに基づいて、前記移動体の自己位置の推定演算を行う推定演算部とを有する請求項１～３の何れか一項記載の自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

従来の自己位置推定装置としては、例えば特許文献１に記載されているような技術が知られている。特許文献１に記載の自己位置推定装置は、自律移動ロボットに設けられ、自律移動ロボットと障害物との距離を計測するレーザ距離センサと、このレーザ距離センサからの計測データと地図記憶部に記憶された地図データとを照合することで、自律移動ロボットの位置及び姿勢を推定する位置姿勢推定部と、パーティクルの分散を計算して、位置姿勢推定部の位置姿勢推定結果の信頼度の評価値を算出する位置姿勢推定評価値算出部とを備えている。

【先行技術文献】

【特許文献】

【0003】

【文献】特許第５９０９４８６号

【発明の概要】

【発明が解決しようとする課題】

【0004】

ところで、移動体の自己位置の推定精度を上げるために、３Ｄのレーザ距離センサを使用することがある。ただし、この場合には、複雑な３次元処理の計算によって自己位置推定の信頼性の評価（判定）が難しくなる。また、近年では、ディープラーニングを利用した自己位置推定の信頼性の判定が行われている。しかし、ディープラーニングを利用すると、データ量及び計算量が多くなるため、複雑なネットワークに使用されるＧＰＵ等の特別なハードウェアが必要となる。

【0005】

本発明の目的は、特別なハードウェアを不要としつつ、移動体の自己位置推定の信頼性を容易に判定することができる自己位置推定装置を提供することである。

【課題を解決するための手段】

【0006】

本発明の一態様に係る自己位置推定装置は、移動体の自己位置を推定する自己位置推定器と、移動体の状態に関する情報を取得する第１情報取得部と、自己位置推定器による自己位置推定処理に関する情報を取得する第２情報取得部と、第１情報取得部により取得された移動体の状態に関する情報と第２情報取得部により取得された自己位置推定器による自己位置推定処理に関する情報とに基づいて、自己位置推定器による移動体の自己位置推定の信頼性を判定する信頼性判定部とを備える。

【0007】

このような自己位置推定装置においては、第１情報取得部によって移動体の状態に関する情報が取得されると共に、第２情報取得部によって移動体の自己位置を推定する自己位置推定器による自己位置推定処理に関する情報が取得される。そして、信頼性判定部において、移動体の状態に関する情報と自己位置推定器による自己位置推定処理に関する情報とに基づいて、自己位置推定器による移動体の自己位置推定の信頼性が判定される。このように移動体の状態に関する情報と自己位置推定器による自己位置推定処理に関する情報とを、信頼性判定部の入力データとして使用することにより、信頼性判定部において処理されるデータ量及び計算量が少なくて済む。これにより、特別なハードウェアを不要としつつ、移動体の自己位置推定の信頼性が容易に判定される。

【0008】

信頼性判定部は、自己位置推定器による移動体の自己位置推定の信頼性として、自己位置推定器による自己位置推定誤差の予測値と自己位置推定器による自己位置推定の信頼度とを判定してもよい。このような構成では、自己位置推定器による自己位置推定誤差の予測値が自己位置推定器による自己位置推定の信頼度と共に判定されるため、移動体の自己位置推定の信頼性が高精度に判定される。

【0009】

第１情報取得部は、移動体の状態に関する情報として、移動体の速度及び角速度の実測値を取得してもよい。このような構成では、信頼性判定部の入力データとして適切な移動体の状態に関する情報が、移動体に搭載された既存のセンサによって容易に取得される。

【0010】

第２情報取得部は、自己位置推定器による自己位置推定処理に関する情報として、自己位置推定器による推定回数及び推定時間と、自己位置推定器により得られた移動体の自己位置推定値、速度及び角速度の計算値とを取得してもよい。このような構成では、信頼性判定部の入力データとして適切な自己位置推定処理に関する情報が取得される。

【0011】

自己位置推定器は、移動体の周囲にレーザを照射し、移動体の周囲に位置する物体で反射されたレーザを受光することにより、移動体から物体までの距離を検出するレーザセンサと、レーザセンサにより検出された移動体から物体までの距離データと移動体の周囲環境の地図情報とに基づいて、移動体の自己位置の推定演算を行う推定演算部とを有してもよい。このような構成では、レーザセンサの受光データを使用して移動体の自己位置が推定されても、移動体の自己位置推定の信頼性の判定処理には、レーザセンサの受光データは使用されない。従って、信頼性判定部において処理されるデータ量及び計算量が一層少なくて済む。

【発明の効果】

【0012】

本発明によれば、特別なハードウェアを不要としつつ、移動体の自己位置推定の信頼性を容易に判定することができる。

【図面の簡単な説明】

【0013】

【図1】本発明の一実施形態に係る自己位置推定装置を備えた走行制御装置の概略構成を示すブロック図である。

【図2】フォークリフトの走行路の一例を示す概念図である。

【図3】図１に示された信頼性判定部により実行される信頼性判定処理の手順を示すフローチャートである。

【発明を実施するための形態】

【0014】

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

【0015】

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

【0016】

フォークリフト２は、産業車両の一つである。フォークリフト２は、図示しない駆動輪及び操舵輪を有する走行装置３と、この走行装置３の前側に配置された荷役装置４とを備えている。荷役装置４は、図示しないパレットを保持して昇降させる１対のフォーク５を有している。

【0017】

走行制御装置１は、自己位置推定器６と、車速センサ７と、ジャイロセンサ８と、制御ユニット９と、駆動部１０とを備えている。走行制御装置１は、フォークリフト２に搭載されている。

【0018】

自己位置推定器６は、フォークリフト２の現在の自己位置を推定する。自己位置推定器は、レーザセンサ１１と、推定演算ユニット１２とを有している。

【0019】

レーザセンサ１１は、図２に示されるように、フォークリフト２の周囲にレーザＬを照射し、フォークリフト２の周囲に位置する物体１３で反射するレーザＬを受光することにより、フォークリフト２から物体１３までの距離を検出する。物体１３は、例えばフォークリフト２が走行する走行路Ｓの左右両側に設置された柱または壁等であり、地図情報（後述）として登録されている。

【0020】

レーザセンサ１１は、例えばフォークリフト２の前方直進を中心とした所定の角度範囲（例えば２７０度）でレーザＬを照射する。レーザセンサ１１としては、例えば３次元にレーザＬを照射する３Ｄのレーザレンジファインダが使用される。

【0021】

推定演算ユニット１２は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。推定演算ユニット１２は、レーザセンサ１１により検出されたフォークリフト２から物体１３までの距離データとフォークリフト２の周囲環境の地図情報とに基づいて、フォークリフト２の自己位置の推定演算を行う推定演算部である。

【0022】

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

【0023】

また、推定演算ユニット１２は、フォークリフト２の現在及び過去の自己位置推定値を用いて、フォークリフト２の速度及び角速度を算出する。これにより、フォークリフト２の速度及び角速度の計算値が得られる。フォークリフト２の速度の計算値は、フォークリフト２の現在及び過去の自己位置推定値から得られた移動距離を時間で除することで求まる。フォークリフト２の角速度の計算値は、フォークリフト２の現在及び過去の自己位置推定値から得られた移動角度を時間で除することで求まる。

【0024】

推定演算ユニット１２は、自己位置推定器６による推定回数（イテレーション）及び推定時間と、自己位置推定器６により得られたフォークリフト２の自己位置推定値、速度及び角速度の計算値を、制御ユニット９に出力する。これらの推定回数、推定時間、フォークリフト２の自己位置推定値、速度及び角速度の計算値は、自己位置推定器６による自己位置推定処理に関する情報である。

【0025】

車速センサ７は、フォークリフト２の速度を検出するセンサである。車速センサ７によりフォークリフト２の速度の実測値が得られる。ジャイロセンサ８は、フォークリフト２の角速度を検出するセンサである。ジャイロセンサ８によりフォークリフト２の角速度の実測値が得られる。フォークリフト２の速度及び角速度の実測値は、フォークリフト２の状態に関する情報である。

【0026】

制御ユニット９は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。制御ユニット９は、自己位置推定器６の出力データと車速センサ７及びジャイロセンサ８の出力データとを入力し、所定の処理を実行し、フォークリフト２を目的地まで走行させるように駆動部１０を制御する。

【0027】

駆動部１０は、特に図示はしないが、フォークリフト２の駆動輪（前述）を回転させる走行モータと、フォークリフト２の操舵輪（前述）を転舵させる操舵モータとを有している。

【0028】

制御ユニット９は、車両情報取得部１４と、自己位置情報取得部１５と、信頼性判定部１６と、駆動制御部１７とを有している。

【0029】

車両情報取得部１４は、車速センサ７により得られたフォークリフト２の速度の実測値と、ジャイロセンサ８により得られたフォークリフト２の角速度の実測値とを取得する。車両情報取得部１４は、車速センサ７及びジャイロセンサ８と協働して、フォークリフト２の状態に関する情報を取得する第１情報取得部を構成している。

【0030】

自己位置情報取得部１５は、自己位置推定器６による推定回数及び推定時間と、自己位置推定器６により得られたフォークリフト２の自己位置推定値、速度及び角速度の計算値とを取得する。自己位置情報取得部１５は、自己位置推定器６による自己位置推定処理に関する情報を取得する第２情報取得部を構成している。

【0031】

信頼性判定部１６は、車両情報取得部１４により取得されたフォークリフト２の状態に関する情報と自己位置情報取得部１５により取得された自己位置推定器６による自己位置推定処理に関する情報とに基づいて、自己位置推定器６によるフォークリフト２の自己位置推定の信頼性を判定する。

【0032】

信頼性判定部１６は、自己位置推定器６によるフォークリフト２の自己位置推定の信頼性として、自己位置推定器６による自己位置推定誤差の予測値と、自己位置推定器６による自己位置推定の信頼度とを判定する。

【0033】

自己位置推定器６による自己位置推定誤差の予測値は、自己位置推定器６によるフォークリフト２の自己位置推定値とフォークリフト２の真位置との差の予測値である。自己位置推定器６による自己位置推定の信頼度は、自己位置推定器６による自己位置推定の誤差の予測値に応じて、高いか低いかの二値で表される。自己位置推定誤差の予測値が予め決められた規定値以下であるときは、自己位置推定の信頼度が高いとされる。自己位置推定誤差の予測値が規定値よりも大きいときは、自己位置推定の信頼度が低いとされる。

【0034】

信頼性判定部１６は、例えばサポートベクターマシン（ＳＶＭ）またはニューラルネットワーク等の機械学習法を用いて、自己位置推定器６によるフォークリフト２の自己位置推定の信頼性を判定する。

【0035】

ここで、機械学習により得られる正解データであるラベルは、シミュレーションを用いて生成される。具体的には、シミュレーションによって、自己位置推定器６によるフォークリフト２の自己位置推定値とフォークリフト２の真位置との差が得られる。そして、自己位置推定器６によるフォークリフト２の自己位置推定値とフォークリフト２の真位置との差に基づいて、自己位置推定器６による自己位置推定誤差の予測値と自己位置推定器６による自己位置推定の信頼度とが機械学習の出力データ（ラベル）として生成される。

【0036】

そして、ラベルの生成時に車速センサ７及びジャイロセンサ８からそれぞれ出力されたフォークリフト２の速度及び角速度の実測値と、ラベルの生成時に自己位置推定器６から出力された推定回数、推定時間、フォークリフト２の自己位置推定値、速度及び角速度の計算値とが、機械学習の入力データとなる。

【0037】

このように機械学習の入力データとしてカメラの画像データ及びレーザの受光データが使用されないため、機械学習の入力データの量が少なくなり、全結合層において層数が少ない単純なネットワークの生成が可能となる。

【0038】

図３は、信頼性判定部１６により実行される信頼性判定処理の手順を示すフローチャートである。図３において、信頼性判定部１６は、まず車両情報取得部１４により取得されたフォークリフト２の速度及び角速度の実測値を入力する（手順Ｓ１０１）。また、信頼性判定部１６は、自己位置情報取得部１５により取得された自己位置推定器６の推定回数及び推定時間とフォークリフト２の自己位置推定値、速度及び角速度の計算値とを入力する（手順Ｓ１０２）。

【0039】

続いて、信頼性判定部１６は、フォークリフト２の速度及び加速度の実測値と、自己位置推定器６の推定回数、推定時間、フォークリフト２の自己位置推定値、速度及び角速度の計算値とに基づいて機械学習を行い、自己位置推定器６による自己位置推定誤差の予測値及び自己位置推定器６による自己位置推定の信頼度を判定する（手順Ｓ１０３）。

【0040】

そして、信頼性判定部１６は、自己位置推定器６による自己位置推定誤差の予測値及び自己位置推定器６による自己位置推定の信頼度のデータを駆動制御部１７に出力する（手順Ｓ１０４）。なお、信頼性判定部１６は、自己位置推定器６による自己位置推定の信頼度のデータのみを駆動制御部１７に出力してもよい。

【0041】

駆動制御部１７は、信頼性判定部１６からの出力データに基づいて、駆動部１０を制御する。具体的には、駆動制御部１７は、信頼性判定部１６により自己位置推定の信頼度が高いと判定されたときは、自己位置推定器６により得られたフォークリフト２の自己位置推定値に基づいて、フォークリフト２を目的地まで走行させるように駆動部１０を制御する。駆動制御部１７は、信頼性判定部１６により自己位置推定の信頼度が低いと判定されたときは、フォークリフト２を停止させるように駆動部１０を制御する。

【0042】

ここで、自己位置推定器６、車速センサ７、ジャイロセンサ８、制御ユニット９の車両情報取得部１４、自己位置情報取得部１５及び信頼性判定部１６は、本実施形態の自己位置推定装置１８を構成している。

【0043】

ところで、ディープラーニングを用いて、自己位置推定器６による自己位置推定の信頼性の判定を行う場合には、例えば畳み込み層を利用した複雑なニューラルネットワークに使用されるＧＰＵ（Graphics Processing Unit）等の特別なハードウェアが必要となる。特に３Ｄのレーザセンサ１１が使用されると、処理されるデータ量及び計算量が増加するため、高性能かつ高価なＧＰＵが必要となる。

【0044】

そのような課題に対し、本実施形態では、車両情報取得部１４によってフォークリフト２の状態に関する情報が取得されると共に、自己位置情報取得部１５によってフォークリフト２の自己位置を推定する自己位置推定器６による自己位置推定処理に関する情報が取得される。そして、信頼性判定部１６において、フォークリフト２の状態に関する情報と自己位置推定器６による自己位置推定処理に関する情報とに基づいて、自己位置推定器６によるフォークリフト２の自己位置推定の信頼性が判定される。このようにフォークリフト２の状態に関する情報と自己位置推定器６による自己位置推定処理に関する情報とを、信頼性判定部１６の入力データとして使用することにより、信頼性判定部１６において処理されるデータ量及び計算量が少なくて済む。これにより、ＧＰＵ等の特別なハードウェアを不要としつつ、フォークリフト２の自己位置推定の信頼性が容易に判定される。

【0045】

また、本実施形態では、自己位置推定器６によるフォークリフト２の自己位置推定の信頼性として、自己位置推定器６による自己位置推定誤差の予測値が自己位置推定器６による自己位置推定の信頼度と共に判定されるため、フォークリフト２の自己位置推定の信頼性が高精度に判定される。

【0046】

また、本実施形態では、フォークリフト２の状態に関する情報として、フォークリフト２の速度及び角速度の実測値が取得される。このため、信頼性判定部１６の入力データとして適切なフォークリフト２の状態に関する情報が、フォークリフト２に搭載された既存の車速センサ７及びジャイロセンサ８によって容易に取得される。

【0047】

また、本実施形態では、自己位置推定器６による自己位置推定処理に関する情報として、自己位置推定器６による推定回数及び推定時間と、自己位置推定器６により得られたフォークリフト２の自己位置推定値、速度及び角速度の計算値とが取得される。このため、信頼性判定部１６の入力データとして適切な自己位置推定処理に関する情報が取得される。

【0048】

また、本実施形態では、レーザセンサ１１の受光データを使用してフォークリフト２の自己位置が推定されても、フォークリフト２の自己位置推定の信頼性の判定処理には、レーザセンサ１１の受光データは使用されない。従って、信頼性判定部１６において処理されるデータ量及び計算量が一層少なくて済む。

【0049】

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、自己位置推定器６によるフォークリフト２の自己位置推定の信頼性として、自己位置推定器６による自己位置推定誤差の予測値と自己位置推定器６による自己位置推定の信頼度とが判定されているが、特にその形態には限られず、自己位置推定器６による自己位置推定の信頼度のみを判定してもよい。

【0050】

また、上記実施形態では、フォークリフト２の状態に関する情報として、車速センサ７及びジャイロセンサ８により検出されたフォークリフト２の速度及び角速度の実測値が取得されているが、特にその形態には限られず、例えばフォークリフト２の加速度及び重量等の実測値も取得してもよい。

【0051】

また、上記実施形態では、車両情報取得部１４、自己位置情報取得部１５、信頼性判定部１６及び駆動制御部１７は、１つの制御ユニット９に含まれているが、特にそのような形態には限られない。信頼性判定部１６は、車両情報取得部１４、自己位置情報取得部１５及び駆動制御部１７とは別の制御ユニットで構成されていてもよい。

【0052】

また、上記実施形態では、３次元にレーザＬを照射する３Ｄのレーザセンサ１１が使用されているが、特にその形態には限られず、２次元にレーザＬを照射する２Ｄのレーザセンサ１１を使用してもよい。

【0053】

また、上記実施形態では、レーザＳＬＡＭ手法を用いて、フォークリフト２の自己位置の推定演算が行われているが、特にその形態には限られず、カメラの画像データを利用したＶｉｓｕａｌ ＳＬＡＭ手法等を用いて、フォークリフト２の自己位置の推定演算を行ってもよい。

【0054】

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

【符号の説明】

【0055】

２…フォークリフト（移動体）、６…自己位置推定器、７…車速センサ（第１情報取得部）、８…ジャイロセンサ（第１情報取得部）、１１…レーザセンサ、１２…推定演算ユニット（推定演算部）、１３…物体、１４…車両情報取得部（第１情報取得部）、１５…自己位置情報取得部（第２情報取得部）、１６…信頼性判定部、１８…自己位置推定装置、Ｌ…レーザ。

【図1】

【図2】

【図3】