特開 2023-84219 位置推定装置、自律走行車両及び位置推定プログラム

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023084219

(43)【公開日】2023-06-19

(54)【発明の名称】位置推定装置、自律走行車両及び位置推定プログラム

(51)【国際特許分類】

   G05D 1/02 20200101AFI20230612BHJP

【ＦＩ】

G05D1/02 H

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2021198259

(22)【出願日】2021-12-07

(71)【出願人】

【識別番号】000002107

【氏名又は名称】住友重機械工業株式会社

(74)【代理人】

【識別番号】100090033

【弁理士】

【氏名又は名称】荒船 博司

(74)【代理人】

【識別番号】100093045

【弁理士】

【氏名又は名称】荒船 良男

(72)【発明者】

【氏名】藤本 寛和

【テーマコード（参考）】

5H301

【Ｆターム（参考）】

5H301AA02

5H301AA10

5H301BB05

5H301BB07

5H301CC03

5H301CC06

5H301CC10

5H301DD07

5H301DD17

5H301GG08

5H301GG17

(57)【要約】

【課題】好適に位置推定を行う。
【解決手段】無人搬送車１は、走行可能な車体１０に搭載され、その周囲の二次元の距離情報を取得可能な距離センサ２４と、３Ｄマップ２６１を予め記憶した記憶部２６と、制御部２７とを備える。制御部２７は、距離センサ２４により取得された二次元の距離情報と、３Ｄマップ２６１とに基づいて、車体１０の位置を推定する。
【選択図】図４

【特許請求の範囲】

【請求項1】

走行可能な車体に搭載され、その周囲の二次元の距離情報を取得可能な計測手段と、
三次元の地図情報を予め記憶した記憶手段と、
前記計測手段により取得された前記二次元の距離情報と、前記三次元の地図情報とに基づいて、前記車体の位置を推定する推定手段と、
を備える位置推定装置。

【請求項2】

前記記憶手段は、地形の傾斜情報を予め記憶し、
前記推定手段は、前記傾斜情報を補助的に用いつつ、前記二次元の距離情報と前記三次元の地図情報とに基づいて、前記車体の位置を推定する、
請求項１に記載の位置推定装置。

【請求項3】

前記記憶手段は、地形の高さ情報を予め記憶し、
前記推定手段は、前記高さ情報を補助的に用いつつ、前記二次元の距離情報と前記三次元の地図情報とに基づいて、前記車体の位置を推定する、
請求項１又は請求項２に記載の位置推定装置。

【請求項4】

前記計測手段は二次元のＬｉＤＡＲである、
請求項１から請求項３のいずれか一項に記載の位置推定装置。

【請求項5】

前記三次元の地図情報は点群データを含む、
請求項１から請求項４のいずれか一項に記載の位置推定装置。

【請求項6】

請求項１から請求項５のいずれか一項に記載の位置推定装置と、
前記車体と、
を備える自律走行車両。

【請求項7】

走行可能な車体に搭載され、その周囲の二次元の距離情報を取得可能な計測手段と、三次元の地図情報を予め記憶した記憶手段と、を備える位置推定装置のコンピュータを、
前記計測手段により取得された前記二次元の距離情報と、前記三次元の地図情報とに基づいて、前記車体の位置を推定する推定手段、
として機能させる位置推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置推定装置、自律走行車両及び位置推定プログラムに関する。

【背景技術】

【0002】

従来、自動運転を行う車両等においては、周囲の状況を取得しながら、それに基づく自己位置の推定が逐次行われる。
例えば特許文献１に記載の技術では、三次元のLiDAR（LASER Imaging Detection and Ranging）により周囲の点群情報を取得し、これを地図データと照合することで自己位置を推定している。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１９－１０９３３２号公報

【発明の概要】

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

【0004】

しかしながら、三次元の周囲情報に基づく位置推定は、６自由度（並進３、回転３）の推定が可能である反面、その分だけ計算コストが高くなる。また、三次元の周囲情報が得られる3D-LiDAR等の三次元計測器は、二次元計測器に比べて高価である。

【0005】

本発明は、上記事情に鑑みてなされたもので、好適に位置推定を行うことを目的とする。

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

【0006】

本発明に係る位置推定装置は、
走行可能な車体に搭載され、その周囲の二次元の距離情報を取得可能な計測手段と、
三次元の地図情報を予め記憶した記憶手段と、
前記計測手段により取得された前記二次元の距離情報と、前記三次元の地図情報とに基づいて、前記車体の位置を推定する推定手段と、
を備える構成とした。

【0007】

本発明に係る自律走行車両は、
上記の位置推定装置と、
前記車体と、
を備える構成とした。

【0008】

本発明に係る位置推定プログラムは、
走行可能な車体に搭載され、その周囲の二次元の距離情報を取得可能な計測手段と、三次元の地図情報を予め記憶した記憶手段と、を備える位置推定装置のコンピュータを、
前記計測手段により取得された前記二次元の距離情報と、前記三次元の地図情報とに基づいて、前記車体の位置を推定する推定手段、
として機能させるものとした。

【発明の効果】

【0009】

本発明によれば、好適に位置推定を行うことができる。

【図面の簡単な説明】

【0010】

【図1】実施形態に係る無人搬送車の作業状態を示す図である。

【図2】実施形態に係る無人搬送車の概略の制御構成を示すブロック図である。

【図3】実施形態に係る法線マップを説明するための図である。

【図4】実施形態に係る位置推定処理の流れを示すフローチャートである。

【図5】法線マップを用いた経路探索例を示す図である。

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

【0011】

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

【0012】

［無人搬送車の構成］
図１は、本実施形態に係る無人搬送車１の作業状態を示す図である。
この図に示すように、本実施形態に係る無人搬送車１は、例えば倉庫又は工場等で無人で荷Ｌを搬送する無人搬送フォークリフト（ＡＧＦ：Automated Guided Forklift）であり、管理サーバ４０（図２参照）からの動作指令等に基づいて所定の荷役作業を行う。無人搬送車１は、本発明に係る自律走行車両の一例であり、本発明に係る位置推定装置を搭載することにより、傾斜面Ｓを含む作業エリア（走行エリア）において好適に自己位置を推定しつつ荷役作業を行う。

【0013】

具体的に、無人搬送車１の車体１０は、車両本体１１、フォーク１２、昇降体（リフト）１３、マスト１４、車輪１５を含む。マスト１４は車両本体１１の前方に設けられ、図示しない駆動源によって駆動されて車両本体１１の前後に傾斜する。昇降体１３は、図示しない駆動源によって駆動され、マスト１４に沿って昇降する。昇降体１３には、荷Ｌやパレットなどを保持する左右一対のフォーク１２が取り付けられている。一対のフォーク１２は、マスト１４及び昇降体１３の駆動により、車両本体１１に対する傾斜及び昇降が可能となっている。

【0014】

図２は、無人搬送車１の概略の制御構成を示すブロック図である。
この図に示すように、無人搬送車１は、上記構成に加え、駆動部２１、通信部２３、距離センサ２４、慣性計測装置（Inertial Measurement Unit：ＩＭＵ）２５、記憶部２６、制御部２７を備える。本発明に係る位置推定装置は、距離センサ２４、記憶部２６、制御部２７を含む。

【0015】

駆動部２１は、無人搬送車１の各種駆動源である走行モータ、操舵モータ及び荷役モータ（いずれも図示省略）を含む。走行モータは、車輪１５のうちの駆動輪を駆動する。操舵モータは、車輪１５のうちの操舵輪を回転（操舵動作）させる。荷役モータは、昇降体１３の昇降とマスト１４の傾倒との各動作を行わせる駆動源である。
通信部２３は、管理サーバ４０等と通信を行うものである。通信部２３による通信は、管理サーバ４０等の通信部と直接行うものであってもよいし、通信ネットワークを介して行うものであってもよい。
管理サーバ４０は、無人搬送車１を含む搬送システムを管理するコンピュータであり、ユーザ操作や所定のプログラムに基づいて無人搬送車１への動作指令の送信等を行う。

【0016】

距離センサ２４は、車体１０周囲の所定の二次元領域の距離情報（深度情報）を取得可能なものであり、取得した情報を制御部２７に出力する。距離センサ２４は、本発明に係る計測手段の一例であり、本実施形態では、二次元のLiDAR（LASER Imaging Detection and Ranging）である。
本実施形態の距離センサ２４は、車両本体１１の天面上に設置され（図１参照）、車両本体１１の上下方向に略直交する平面内における所定の半径領域の計測を行う。ただし、距離センサ２４の計測領域は平面領域であればよい。

【0017】

慣性計測装置２５は、無人搬送車１の三次元の加速度及び角速度を計測し、その結果を制御部２７に出力する。

【0018】

制御部２７は、例えばＣＰＵ（Central Processing Unit）を含んで構成され、無人搬送車１各部の動作を制御する。具体的に、制御部２７は、通信部２３を通じて管理サーバ４０から受信した動作指令に基づいて駆動部２１を動作させたり、記憶部２６に予め記憶されているプログラムを展開し、展開されたプログラムと協働して各種処理を実行したりする。

【0019】

記憶部２６は、ＲＡＭ（Random Access Memory）やＲＯＭ（Read Only Memory）等により構成されるメモリであり、各種のプログラム及びデータを記憶するとともに、制御部２７の作業領域としても機能する。
本実施形態の記憶部２６は、位置推定プログラム２６０、３Ｄマップ２６１、法線マップ２６２、高低マップ２６３を予め記憶している。
位置推定プログラム２６０は、後述の位置推定処理（図４参照）を実行するためのプログラムである。

【0020】

３Ｄマップ２６１は、本実施形態では作業エリアの三次元点群データである。３Ｄマップ２６１は、特に限定はされないが、例えば三次元LiDAR（距離センサ）を搭載した無人搬送車１に作業エリアを走行させつつ三次元LiDARにより地形を計測させることで、予め取得される。この３Ｄマッピングは例えば作業エリアのレイアウトが変更された場合に都度行われ、３Ｄマップ２６１が最新のものに更新される。なお、３Ｄマップ２６１は、少なくとも作業エリアを含む領域の三次元の地図情報（地形情報）であればよい。

【0021】

法線マップ２６２は、地形表面（本実施形態では無人搬送車１の走行路面）の法線方向の情報を有する作業エリアの傾斜情報であり、予め３Ｄマップ２６１から取得される。具体的には、例えば図３（ａ）に示すような地形があった場合、まず各グリッドの法線ベクトル（ｘｙｚ座標値）が取得される。取得された法線ベクトルのｘｙｚ値がＲＧＢ値に変換されることで図３（ｂ）に示す二次元のカラーマップが生成され、それが図３（ｃ）に示すような二次元のグリッドマップに低次元化されることで、法線マップ２６２が生成される。
高低マップ２６３は、地形の高度情報を有する作業エリアの高さ情報であり、予め３Ｄマップ２６１から取得される。
なお、法線マップ２６２と高低マップ２６３は、それぞれ必要な情報（傾斜情報及び高さ情報）を含むものであれば、３Ｄマップ２６１から取得されたものでなくともよい。

【0022】

［位置推定処理］
続いて、荷役作業中の無人搬送車１が位置推定処理を実行するときの動作について説明する。図４は、位置推定処理の流れを示すフローチャートである。
位置推定処理は、無人搬送車１が荷役作業中に自己の位置を推定する処理であり、無人搬送車１の制御部２７が記憶部２６から位置推定プログラム２６０を読み出して展開することで実行される。荷役作業では、無人搬送車１が、位置推定処理により自己の位置を取得しつつ、所定のプログラム又は管理サーバ４０からの動作指令に基づいて作業エリア内で荷Ｌに対する各種動作（荷取り、荷積み、荷下ろし、運搬等）を行う。

【0023】

図４に示すように、位置推定処理が実行されると、まず制御部２７は、距離センサ２４（二次元LiDAR）により、車体１０周囲の二次元領域の距離情報を取得する（ステップＳ１）。

【0024】

次に、制御部２７は、取得した二次元の距離情報を３Ｄマップ２６１と照合する（ステップＳ２）。このとき、制御部２７は、法線マップ２６２と高低マップ２６３を補助的に使用しながら、距離情報と３Ｄマップ２６１のマッチングを行う。つまり、このマッチングでは、法線マップ２６２と高低マップ２６３の利用により、車体１０の高さを含む位置と姿勢とが推定されつつ、取得された二次元の距離情報が３Ｄマップ２６１と照合される。そして、好適に合致する３Ｄマップ２６１上の位置が自己位置として算出される。

【0025】

これにより、三次元の計測手段を用いることなく、車体１０の姿勢（傾斜）や高さ位置を好適に考慮して、自己位置を算出することができる。
すなわち、三次元の計測手段を用いた位置推定は、６自由度（並進３、回転３）の推定が可能である反面、その分だけ計算コストが高くなり、また計測器も比較的に高価である。一方、二次元の計測手段を用いた位置推定は、計測器がより安価であって計算コストも抑えられる反面、計算自由度は３自由度（並進２、回転１）に留まり、起伏のある地形への適用が難しい。この点、本実施形態では、三次元の計測手段を必要とすることなく、車体１０の姿勢や高さ位置を好適に考慮することができる。

【0026】

なお、ステップＳ２では、法線マップ２６２と高低マップ２６３を補助的に用いることとしたが、これらを用いずに二次元の距離情報と３Ｄマップ２６１のマッチングを行ってもよい。ただし、法線マップ２６２と高低マップ２６３のうち少なくともいずれか一方の利用が、特に演算回数を抑える点において好ましいのは勿論である。いずれか一方を用いる場合は法線マップ２６２を用いるのがより好ましい。
また、法線マップ２６２や高低マップ２６３に代えて、車体１０の高さや傾斜（姿勢）の情報を取得する他のセンサを用いてもよい。例えば、姿勢の情報は慣性計測装置２５から取得してもよい。

【0027】

次に、制御部２７は、位置推定処理を終了させるか否かを判定し（ステップＳ３）、終了させないと判定した場合には（ステップＳ３；Ｎｏ）、上述のステップＳ１へ処理を移行し、自己位置の推定（算出）を荷役作業中に継続して繰返し実行する。
そして、例えば荷役作業の終了等により、位置推定処理を終了させると判定した場合には（ステップＳ３；Ｙｅｓ）、制御部２７は、位置推定処理を終了させる。

【0028】

［本実施形態の技術的効果］
以上のように、本実施形態によれば、距離センサ２４により取得された二次元の距離情報と、３Ｄマップ２６１とに基づいて、車体１０の位置が推定される。
これにより、三次元の距離情報に基づく位置推定に比べ、計算コストを抑えることができる。また、二次元の距離情報を取得する距離センサ２４は、3D-LiDAR等の三次元計測器に比べて安価なもので足りる。さらに、制御部２７もより演算能力が低く安価なもので足りる。
したがって、3D-LiDAR等による三次元の距離情報に基づく位置推定に比べ、好適に位置推定を行うことができる。

【0029】

また本実施形態によれば、法線マップ２６２や高低マップ２６３を用いることにより、車体１０の姿勢（傾斜）や高さ位置を好適に考慮して、自己位置を推定することができる。したがって、高低差を含む作業エリアであっても好適に自己位置を推定できる。

【0030】

［その他］
以上、本発明の実施形態について説明したが、本発明は上記の実施形態（変形例含む）に限られない。
例えば、上記実施形態の位置推定処理では、法線マップ２６２を用い、地形の傾斜や高さを考慮した経路探索を行ってもよい。
このとき、車体１０の前後方向と幅方向とで許容される加速度に異方性がない場合には、傾斜面を走行するときのコストをＡ*アルゴリズム等の経路探索アルゴリズムに組み込むことで、法線とＺ軸のなす角度をそのままコストとして統合すればよい。これにより、例えば傾斜面のコストが大きい場合には、できるだけ傾斜面を避けた経路が得られる。あるいは、例えば図５（ａ）に示すように、許容可能な車体１０の傾斜範囲で傾斜面を走行する経路が得られる。
また、車体１０の前後方向と幅方向とで許容される加速度に異方性がある場合、法線マップ２６２をhybridＡ*アルゴリズムに組み込むことで、法線を軸とした楕円と重力ベクトルの関係から、姿勢によるコストを統合すればよい。これにより、例えば図５（ｂ）に示すように、コストの低い姿勢として車体１０の前後方向で傾斜面を走行する経路が得られる。

【0031】

また、上記実施形態では、位置推定装置が無人搬送車に搭載された場合を例示した。しかし、本発明に係る位置推定装置の搭載対象は、自己位置推定を行う移動体であれば無人搬送車に限定されず、例えば、フォークリフトや建設機械等の車両、移動ロボット、ドローン（無人航空機）等であってもよい。また、運転者が搭乗する有人機であってもよい。その利用場所も倉庫や工場、建築現場に限定されず、スロープ等の起伏（高低差）を有する場所（地形）に広く適用可能である。
さらに言えば、本発明に係る位置推定装置（位置推定プログラム）は、移動体に搭載されていなくともよく、移動体と通信可能な制御手段（例えば管理サーバ４０）に実装されていてもよい。

【0032】

また、上記実施形態では、本発明に係る自律走行車両の一例として無人搬送車を例示したが、本発明に係る自律走行車両は、自動運転（自律走行）可能な車両であればよく、搬送機能を備えていなくともよい。
その他、上記実施形態で示した細部は、発明の趣旨を逸脱しない範囲で適宜変更可能である。

【符号の説明】

【0033】

１ 無人搬送車（自律走行車両）
１０ 車体
２４ 距離センサ（計測手段）
２５ 慣性計測装置
２６ 記憶部（記憶手段）
２７ 制御部（推定手段）
２６０ 位置推定プログラム
２６１ ３Ｄマップ（三次元の地図情報）
２６２ 法線マップ（傾斜情報）
２６３ 高低マップ（高さ情報）
Ｓ 傾斜面

【図1】

【図2】

【図3】

【図4】

【図5】