知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2025-03-10
(45)【発行日】2025-03-18
(54)【発明の名称】移動体、制御ユニット、および移動体の動作を制御する方法
(51)【国際特許分類】
G05D 1/43 20240101AFI20250311BHJP
A01B 69/00 20060101ALI20250311BHJP
【ＦＩ】
G05D1/43
A01B69/00 303D
【請求項の数】
24
(21)【出願番号】P 2022563677
(86)(22)【出願日】2021-11-01
(86)【国際出願番号】 JP2021040298
(87)【国際公開番号】W WO2022107586
(87)【国際公開日】2022-05-27
【審査請求日】2023-12-20
(31)【優先権主張番号】P 2020192013
(32)【優先日】2020-11-18
(33)【優先権主張国・地域又は機関】JP
(73)【特許権者】
【識別番号】000001052
【氏名又は名称】株式会社クボタ
(74)【代理人】
【識別番号】100101683
【弁理士】
【氏名又は名称】奥田 誠司
(74)【代理人】
【識別番号】100155000
【弁理士】
【氏名又は名称】喜多 修市
(74)【代理人】
【識別番号】100139930
【弁理士】
【氏名又は名称】山下 亮司
(74)【代理人】
【識別番号】100188813
【弁理士】
【氏名又は名称】川喜田 徹
(74)【代理人】
【識別番号】100202197
【弁理士】
【氏名又は名称】村瀬 成康
(74)【代理人】
【識別番号】100202142
【弁理士】
【氏名又は名称】北 倫子
(72)【発明者】
【氏名】湯浅 純一
(72)【発明者】
【氏名】宮下 隼輔
(72)【発明者】
【氏名】玉井 拓也
(72)【発明者】
【氏名】江戸 俊介
(72)【発明者】
【氏名】倉田 諒
【審査官】田中 成彦
(56)【参考文献】
【文献】特開２０１４－１２２０１９（ＪＰ，Ａ）
【文献】特表２０２０－５１１６３９（ＪＰ，Ａ）
【文献】米国特許出願公開第２０１９／０１７６８４１（ＵＳ，Ａ１）
【文献】特開２０１０－０９６７５２（ＪＰ，Ａ）
【文献】特表２０１５－５３５４００（ＪＰ，Ａ）
【文献】米国特許第０９３１５１９２（ＵＳ，Ｂ１）
【文献】特表２０１８－５１３３６５（ＪＰ，Ａ）
【文献】特開２０１８－０５９７４２（ＪＰ，Ａ）
【文献】特開２００８－１３４７４３（ＪＰ，Ａ）
【文献】国際公開第２０１９／０４４４９８（ＷＯ，Ａ１）
【文献】国際公開第２０１３／１５０６３０（ＷＯ，Ａ１）
【文献】米国特許出願公開第２０１７／００２３６７８（ＵＳ，Ａ１）
【文献】米国特許出願公開第２０１９／０３１７２１９（ＵＳ，Ａ１）
【文献】米国特許出願公開第２０１３／００８００５７（ＵＳ，Ａ１）
【文献】米国特許第０６７２８６０８（ＵＳ，Ｂ２）
【文献】独国特許出願公開第１０２０１６２１４４７２（ＤＥ，Ａ１）
【文献】中国特許出願公開第１０９７８２７７１（ＣＮ，Ａ）
【文献】韓国登録特許第１０－２１１６０３１（ＫＲ，Ｂ１）
【文献】特開２０１９－１５２９２４（ＪＰ，Ａ）
【文献】特表２０１８－５０５４２６（ＪＰ，Ａ）
【文献】特開平０４－１７６７８６（ＪＰ，Ａ）
【文献】特開２０１９－１７０３１２（ＪＰ，Ａ）
【文献】特開２０１６－１８９１７２（ＪＰ，Ａ）
【文献】特開２０１６－０６６２０６（ＪＰ，Ａ）
【文献】特開２００９－２１３４００（ＪＰ，Ａ）
【文献】特開昭６１－１８７７０６（ＪＰ，Ａ）
【文献】米国特許出願公開第２０１８／０１６５５４４（ＵＳ，Ａ１）
【文献】中国特許出願公開第１０９４７１４３４（ＣＮ，Ａ）
【文献】韓国登録特許第１０－２０７７２１９（ＫＲ，Ｂ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／００ － １／８７
Ａ０１Ｂ ６９／００
Ｇ０１Ｓ １７／４２
Ｇ０１Ｓ １７／９３１
(57)【特許請求の範囲】
【請求項1】
複数の樹木列の間を移動する移動体であって、前記移動体の周囲の環境における物体の分布を示す３次元の点群データであるセンサデータを出力するＬｉＤＡＲセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記ＬｉＤＡＲセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備え、
前記複数の樹木列のうちの隣り合う２つの樹木列の一方について、前記２つの樹木列の他方よりも少ない数の幹部が検出された場合、前記制御装置は、検出された前記幹部の分布に基づいて、前記２つの樹木列の前記一方における隠れた幹部の分布を推定し、推定した前記分布に基づいて、前記移動体の操舵制御を行う、
移動体。
【請求項2】
前記ＬｉＤＡＲセンサは、前記樹木列の幹部の平均高さよりも低い位置に搭載され、
前記ＬｉＤＡＲセンサは、複数のレーザ光源を有し、
前記複数のレーザ光源は、それぞれ、異なる仰角でレーザパルスを出射し、
前記自己位置推定装置は、前記複数のレーザ光源のうち、前記仰角が所定の範囲に含まれる一部のレーザ光源から出射された前記レーザパルスの反射点に基づいて前記幹部を検出する、
請求項１に記載の移動体。
【請求項3】
前記自己位置推定装置は、前記複数のレーザ光源から出射された前記レーザパルスの反射点のうち、前記幹部の平均高さよりも低い位置にある反射点に基づいて前記幹部を検出する、請求項２に記載の移動体。
【請求項4】
前記複数のレーザ光源の一部は、前記移動体の移動方向に対して下向きに前記レーザパルスを出射し、前記複数のレーザ光源の残りは、前記移動体の移動方向に対して上向きに前記レーザパルスを出射し、
前記自己位置推定装置は、前記複数のレーザ光源のうち、前記移動体の移動方向に対して下向きに前記レーザパルスを出射する一部のレーザ光源からの前記レーザパルスの反射点に基づいて前記幹部を検出する、
請求項２または３に記載の移動体。
【請求項5】
前記ＬｉＤＡＲセンサは、地上から１５ｃｍ以上１００ｃｍ以下の高さに配置されている、請求項１から４のいずれかに記載の移動体。
【請求項6】
前記自己位置推定装置は、前記ＬｉＤＡＲセンサから繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む複数のスキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、請求項１から５のいずれかに記載の移動体。
【請求項7】
前記自己位置推定装置は、前記所定の範囲を、ユーザが端末装置を用いて行った設定の操作に基づいて決定する、請求項２から４のいずれかに記載の移動体。
【請求項8】
前記自己位置推定装置は、前記ＬｉＤＡＲセンサから前記レーザパルスの反射点までの距離の、前記ＬｉＤＡＲセンサによるスキャン中における変化に基づいて前記幹部を検出する、請求項２から４、および７のいずれかに記載の移動体。
【請求項9】
前記ＬｉＤＡＲセンサは、予め設定された周期で前記点群データを繰り返し出力し、前記自己位置推定装置は、１周期以上の期間中に出力された前記点群データにおける各点の位置、または各点の前記移動体からの距離もしくは角度に基づいて、前記幹部の表面を表す点を抽出し、抽出した前記点と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、請求項１から８のいずれかに記載の移動体。
【請求項10】
慣性計測装置をさらに備え、前記自己位置推定装置は、前記慣性計測装置から出力された信号を利用して前記移動体の位置を推定する、
請求項１から９のいずれかに記載の移動体。
【請求項11】
前記自己位置推定装置は、前記慣性計測装置から出力された前記信号に基づいて、前記ＬｉＤＡＲセンサの傾斜角を決定し、
前記傾斜角に応じて、前記所定の範囲を適応的に選択する、
請求項１０に記載の移動体。
【請求項12】
前記自己位置推定装置は、前記センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部と、前記環境地図データから抽出した前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部とのマッチングにより、前記移動体の位置を推定する、請求項１から１１のいずれかに記載の移動体。
【請求項13】
前記制御装置は、前記環境地図データおよび前記センサデータに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、請求項１から１２のいずれかに記載の移動体。
【請求項14】
障害物センサをさらに備え、前記制御装置は、前記障害物センサによって前記移動体の経路上に障害物が検出されたとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、請求項１から１３のいずれかに記載の移動体。
【請求項15】
前記センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、前記各幹の太さを記憶媒体に記録する記録装置をさらに備える、請求項１から１４のいずれかに記載の移動体。
【請求項16】
前記自己位置推定装置は、前記センサデータと、記憶媒体に記録された前記複数の樹木列の配置関係を示す情報とに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出する、請求項１から１５のいずれかに記載の移動体。
【請求項17】
前記移動体は自律的に移動可能な作業車両である、請求項１から１６のいずれかに記載の移動体。
【請求項18】
前照灯をさらに備え、前記ＬｉＤＡＲセンサは、前記前照灯よりも低い位置に配置されている、
請求項１から１７のいずれかに記載の移動体。
【請求項19】
前記自己位置推定装置は、前記センサデータに基づいて検出した前記樹木列の幹部の分布を示すデータを生成し、
前記幹部の分布を示すデータを用いて前記環境地図データを更新する、または、前記幹部の分布を示すデータを、前記環境地図データを更新する外部の装置に送信する、
請求項１から１８のいずれかに記載の移動体。
【請求項20】
前記自己位置推定装置は、前記センサデータが示す点群から、（ａ）下に凸の円弧状の点の集まり、または
（ｂ）周囲の点に比べて前記ＬｉＤＡＲセンサからの距離が局所的に近い点の集まり、または
（ｃ）反射強度が所定の範囲内にあり、かつ位置が近接する点の集まり、
を抽出することにより、前記幹部を検出する、
請求項１から１９のいずれかに記載の移動体。
【請求項21】
前記自己位置推定装置は、前記センサデータが示す点群から、地面からの高さがある範囲に含まれる点群のみを抽出し、抽出した点群に基づいて前記幹部を検出する、請求項１から２０のいずれかに記載の移動体。
【請求項22】
前記環境地図データは、前記複数の樹木列の周囲に位置する複数の支柱の分布をさらに示し、前記自己位置推定装置は、前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部および支柱を検出し、検出した前記樹木列の幹部および前記支柱と、前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、請求項１から２１のいずれかに記載の移動体。
【請求項23】
複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示す３次元の点群データであるセンサデータを出力するＬｉＤＡＲセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記１つ以上のＬｉＤＡＲセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備え、
前記複数の樹木列のうちの隣り合う２つの樹木列の一方について、前記２つの樹木列の他方よりも少ない数の幹部が検出された場合、前記制御装置は、検出された前記幹部の分布に基づいて、前記２つの樹木列の前記一方における隠れた幹部の分布を推定し、推定した前記分布に基づいて、前記移動体の操舵制御を行う、
制御ユニット。
【請求項24】
複数の樹木列の間を移動する移動体の動作を制御する方法であって、前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示す３次元の点群データであるセンサデータを、前記移動体に搭載されたＬｉＤＡＲセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
前記複数の樹木列のうちの隣り合う２つの樹木列の一方について、前記２つの樹木列の他方よりも少ない数の幹部が検出された場合、検出された前記幹部の分布に基づいて、前記２つの樹木列の前記一方における隠れた幹部の分布を推定することと、
推定した前記分布に基づいて、前記移動体の操舵制御を行うことと、
を含む方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、移動体、制御ユニット、および移動体の動作を制御する方法に関する。
【背景技術】
【0002】
次世代農業として、ＩＣＴ（Information and Communication Technology）およびＩｏＴ（Internet of Things）を活用したスマート農業の研究開発が進められている。圃場で使用されるトラクタなどの作業車両の自動化および無人化に向けた研究開発も進められている。例えば、精密な測位が可能なＧＮＳＳ（Global Navigation Satellite System）などの測位システムを利用して自動操舵で走行する作業車両が実用化されてきた。特許文献１から３は、ＧＮＳＳを利用して行った測位の結果に基づいて自動操舵を行う作業車両の例を開示している。
【0003】
一方、ＬｉＤＡＲ（Light Detection and Ranging）などの距離センサを利用して自律的に移動する移動体の開発も進められている。例えば特許文献４は、ＬｉＤＡＲを利用して圃場における作物列の間を自動走行する作業車両の例を開示している。
【先行技術文献】
【特許文献】
【0004】
【文献】国際公開第２０１７／２０８３０６号
【文献】特開２０２０－１０４６１７号公報
【文献】特開２０２０－１２６８０号公報
【文献】特開２０１９－１５４３７９号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
ぶどう園（vineyard）などの果樹園、または森林などの、樹木が高い密度で分布する環境では、樹木の上部で生い茂る葉が樹冠（canopy）を形成し、衛星からの電波にとって障害物または多重反射体として働く。このような環境では、ＧＮＳＳによる正確な測位が困難である。ＧＮＳＳを利用できない環境においては、位置推定と地図作成とを同時に実行するＳＬＡＭ（Simultaneous Localization and Mapping）を利用することが考えられる。しかし、ＳＬＡＭを利用して多数の樹木が存在する環境内を自律的に、あるいは自動操舵で移動する移動体の実用化には様々な課題が存在する。例えば、季節の変化によって樹木の葉部の分布が大きく変化するため、過去に作成された地図を継続して使用することができないといった課題がある。
【課題を解決するための手段】
【0006】
本開示の例示的な実施形態による移動体は、複数の樹木列の間を移動する。前記移動体は、前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、前記自己位置推定装置によって推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、を備える。前記自己位置推定装置は、前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する。
【0007】
本開示の包括的または具体的な態様は、装置、システム、方法、集積回路、コンピュータプログラム、もしくはコンピュータが読み取り可能な記録媒体、またはこれらの任意の組み合わせによって実現され得る。コンピュータが読み取り可能な記録媒体は、揮発性の記録媒体を含んでいてもよいし、不揮発性の記録媒体を含んでいてもよい。装置は、複数の装置で構成されていてもよい。装置が２つ以上の装置で構成される場合、当該２つ以上の装置は、１つの機器内に配置されてもよいし、分離した２つ以上の機器内に分かれて配置されていてもよい。
【発明の効果】
【0008】
本開示のある実施形態によれば、ＧＮＳＳによる測位が困難な環境においても、移動体の自動操舵あるいは自律移動を実現することができる。樹木列の幹部の分布は、葉部の分布と比較して季節による変化が少ない。幹部に着目して環境地図を作成することにより、比較的長い期間にわたって同一の環境地図を継続して使用することが可能になる。
【図面の簡単な説明】
【0009】
【図1】時刻ｔ１から時刻ｔ２までの間に平面上を移動する移動体の例を模式的に示す図である。
【図2A】時刻ｔ１における移動体と複数のランドマークとを示す図である。
【図2B】時刻ｔ２における移動体と複数のランドマークとを示す図である。
【図3】移動体座標系ΣＶとワールド座標系ΣＷとの配置関係を示す図である。
【図4】マッチングの一例を説明するための図である。
【図5】マッチングの一例を説明するための図である。
【図6】実施形態１におけるトラクタの外観を示す斜視図である。
【図7】トラクタを側面方向から見た模式図である。
【図8】トラクタおよび作業機の概略的な構成の例を示すブロック図である。
【図9A】ＬｉＤＡＲセンサをトラクタの側面方向から見た場合の模式図である。
【図9B】ＬｉＤＡＲセンサを鉛直上方から見た場合の模式図である。
【図10】ＬｉＤＡＲセンサの概略的な構成の例を示すブロック図である。
【図11】トラクタが走行する環境の一例を模式的に示す図である。
【図12】トラクタの周囲の環境の例を模式的に示す斜視図である。
【図13A】トラクタの走行経路の一例を模式的に示す図である。
【図13B】トラクタの走行経路の他の例を模式的に示す図である。
【図14A】ＬｉＤＡＲセンサから出射されたレーザビームで樹木および地面が照射される様子を模式的に示す図である。
【図14B】ＬｉＤＡＲセンサから出射されたレーザビームで地面が照射される様子を模式的に示す図である。
【図15A】ある時点におけるトラクタと、ＬｉＤＡＲセンサから放射されたレーザパルスの反射点の分布の例を模式的に示す図である。
【図16】複数のレイヤのレーザビームの１スキャンによって得られる幹部の点群の例を模式的に示す図である。
【図17A】レイヤＬｋのレーザビームのスキャンによって幹部の点群が得られる様子を模式的に示す図である。
【図17B】レイヤＬｋ＋１のレーザビームのスキャンによって幹部の点群が得られる様子を模式的に示す図である。
【図17C】レイヤＬｋ＋２のレーザビームのスキャンによって幹部の点群が得られる様子を模式的に示す図である。
【図18】複数のレイヤのレーザビームによって１スキャンで計測される反射点までの距離と反射点の方位角との関係の例を示すグラフである。
【図19】ある位置におけるトラクタで取得されたスキャンデータから、幹部の表面に位置する反射点を抽出して作成した局所地図の例を示す図である。
【図20A】局所地図を、構築済みの環境地図とマッチングさせることにより、移動体の位置を推定するプロセスを模式的に示す平面図である。
【図20B】局所地図を、構築済みの環境地図とマッチングさせることにより、移動体の位置を推定するプロセスを模式的に示す平面図である。
【図21A】トラクタの車両本体の前方下部にＬｉＤＡＲセンサが取り付けられた例を示す図である。
【図21B】トラクタのキャビンの前方上部にＬｉＤＡＲセンサが取り付けられた例を示す図である。
【図22】自己位置推定モジュールの機能構成の例を示す図である。
【図23】環境地図データの形式の一例を模式的に示す図である。
【図24】ＩＭＵデータとスキャンデータの時間的な関係の一例を示す図である。
【図25】自己位置推定動作を示すフローチャートである。
【図26】スキャンデータのフィルタリング処理を説明するための図である。
【図27】トラクタのポーズが推定された後に実行される制御の例を示すフローチャートである。
【図28A】走行予定経路Ｐに沿って走行するトラクタの例を示す図である。
【図28B】走行予定経路Ｐから右にシフトした位置にあるトラクタの例を示す図である。
【図28C】走行予定経路Ｐから左にシフトした位置にあるトラクタの例を示す図である。
【図28D】走行予定経路Ｐに対して傾斜した方向を向いているトラクタの例を示す図である。
【図29A】環境地図に基づいて生成される、樹木列の幹分布を示す２次元データの例を示す図である。
【図29B】幹分布データに基づく走行予定経路の決定方法の一例を説明するための図である。
【図30】経路設定用の幹分布データの他の例を示す図である。
【図31A】樹木列が曲線的に配置されている環境の一例を模式的に示す図である。
【図31C】隠れた幹部の位置を推定することによって走行経路を決定する動作を説明するための図である。
【図31D】樹木列が曲線的に配置されている場合における幹部の２次元分布を示す格子状データの一例を示す図である。
【図32】トラクタの走行環境に障害物および人が存在する状況の例を模式的に示す図である。
【図33A】トラクタによって検出される樹木列の幹部の配置の例を示す図である。
【図33B】環境地図データが示す幹分布の例を示す図である。
【図34】実施形態２におけるトラクタを側面方向から見た模式図である。
【図35】実施形態２におけるトラクタの概略的な構成の例を示すブロック図である。
【図36A】トラクタが樹木の近くを走行する様子を模式的に示す図である。
【図36B】トラクタが樹木の近くを走行する様子を模式的に示す他の図である。
【図36C】トラクタが樹木の近くを走行する様子を模式的に示すさらに他の図である。
【図37】実施形態３のシステムの構成例を模式的に示す図である。
【図38】実施形態３のシステムの構成例を示すブロック図である。
【図39】ドローンの外観の例を示す斜視図である。
【図40】複数のドローンによるデータ収集作業の様子の例を模式的に示す斜視図である。
【図41】ドローンに搭載されたＬｉＤＡＲセンサから出射されたレーザビームによって樹木が照射されている様子を模式的に示す図である。
【図42】ドローンから出力される局所地図データの形式の一例を示す図である。
【発明を実施するための形態】
【0010】
［１．用語の定義］
本明細書において用いられる主な用語の定義を以下に記載する。
本明細書において用いられる主な用語の定義を以下に記載する。
【0011】
「移動体」は、移動のための駆動力（traction）を発生させる車輪、クローラ、二足または多足歩行装置、またはプロペラなどの駆動装置を備える装置である。本開示における「移動体」は、トラクタなどの作業車両、搬送車、モバイルロボット、およびマルチコプタなどの無人航空機（Unmanned aerial vehicle：ＵＡＶ、いわゆるドローン）を含む。移動体は、無人・有人の別を問わない。
【0012】
「作業車両」は、圃場（例えば、果樹園、畑、水田、または牧草地など）、山林、または建設現場などの作業地において特定の作業を行いながら走行することが可能な車両を意味する。作業車両の例は、トラクタ、田植機、コンバイン、乗用管理機、および乗用草刈機などの農業機械に加えて、建設作業車および除雪車などの、農業以外の用途で使用される車両も含む。
【0013】
「ＳＬＡＭ」は、移動体の自己位置推定と地図作成とを同時に行う技術の総称である。
【0014】
「自己位置推定」は、地図上における移動体の位置（例えば移動体の重心位置）を推定することである。ＳＬＡＭによる自己位置推定では、通常、移動体のポーズ（pose）が求められる。
【0015】
「ポーズ」は、物（オブジェクト）の「位置および向き」である。２次元空間におけるポーズは、例えば（ｘ，ｙ，θ）の３個の座標値によって規定される。ここで、（ｘ，ｙ）は、地球に固定されたワールド座標系であるＸＹ座標系における座標値であり、θは基準方向に対する角度である。３次元空間におけるポーズは、例えば（ｘ，ｙ，ｚ，θＲ，θＰ，θＹ）の６個の座標値によって規定される。ここで（ｘ，ｙ，ｚ）は、ワールド座標系であるＸＹＺ座標系における座標値であり、（θＲ，θＰ，θＹ）は、それぞれの基準方向に対するロール、ピッチ、およびヨーの角度である。移動体の姿勢は、（θＲ，θＰ，θＹ）によって表される。ロール角θＲは、移動体の前後方向の軸の周りの回転量を表す。ピッチ角θＰは、移動体の左右方向の軸の周りの回転量を表す。ヨー角θＹは、移動体の上下方向の軸の周りの回転量を表す。姿勢は、オイラー角などの他の角度、またはクォータニオン（四元数）によっても規定され得る。
【0016】
「環境地図データ」は、移動体が移動する環境に存在する物の位置または領域を所定の座標系によって表現したデータである。環境地図データを単に「環境地図」と称することがある。環境地図を規定する座標系の例は、地球に対して固定された地理座標系などのワールド座標系だけでなく、オドメトリ情報に基づくポーズを表示するオドメトリ座標系などを含む。環境地図データは、環境に存在する物について、位置以外の情報（例えば、属性情報その他の情報）を含んでいてもよい。環境地図は、点群地図または格子地図など、さまざまな形式の地図を含む。以下、環境地図を「地図データ」と呼んだり、単に「地図」と呼んだりすることがある。また、環境地図を構築する過程で生成または処理される局所地図または部分地図のデータについても、「地図データ」または単に「地図」と呼ぶことがある。
【0017】
「自動操舵」は、手動によらず、制御装置の働きによって移動体の操舵を行うことである。制御装置の一部または全部が移動体の外部にあってもよい。移動体の外部にある制御装置と移動体との間では、制御信号、コマンド、またはデータなどの通信が行われ得る。自動操舵中において、速度制御など、その他の動作は手動で行われてもよい。
【0018】
「自律移動」は、人が移動の制御に関与することなく、周囲の環境をセンシングしながら、制御装置の働きによって移動体の移動を行うことを意味する。自律移動は、自律走行および自律飛行を含む。制御装置は、移動体の移動に必要な操舵、速度制御、走行の開始および停止、飛行中の上昇・降下・ホバリングなどの動作を制御し得る。自律移動は、障害物の検知および障害物の回避動作を含み得る。
【0019】
「自動走行」は、移動体が備える制御装置による自律走行、および、運行管理システムにおけるコンピュータからの指令に基づく走行を含む。自律走行には、移動体が所定の経路に沿って目的地に向かう移動のみならず、追尾目標に追従する移動も含まれる。また、一時的に作業者の指示に基づいて移動してもよい。
【0020】
「自己位置推定装置」は、ＬｉＤＡＲ（Light Detection and Ranging）センサなどの外界センサによって取得されたセンサデータに基づいて環境地図上における自己位置を推定する装置である。
【0021】
「外界センサ」は、移動体の外部の状態をセンシングするセンサである。外界センサの例は、例えば、レーザレンジファインダ（「測域センサ」ともいう）、カメラ（またはイメージセンサ）、ＬｉＤＡＲセンサ、ミリ波レーダ、および磁気センサを含む。
【0022】
「内界センサ」は、移動体の状態をセンシングするセンサである。内界センサは、車輪の回転速度を計測するホイールエンコーダ、加速度センサ、および角加速度センサ（例えばジャイロスコープ）を含む。慣性計測装置（ＩＭＵ）は、加速度センサおよび角加速度センサを備え、移動体の移動量および姿勢を示す信号を出力することができる。内界センサによって取得される移動体のポーズ変化量を示す情報を「オドメトリ情報」と称する。
【0023】
「樹木の幹部」は、木本植物の木質化した茎であり、地上に直立して枝を生ずる主軸部分である。樹木の枝、葉および根は含まない。
【0024】
［２．ＳＬＡＭの基本原理］
次に、本開示の実施形態において用いられるＳＬＡＭ技術を利用した自己位置推定の基本原理を説明する。ここでは、簡単のため、移動体が２次元空間（すなわち平面）内を移動するものとする。
次に、本開示の実施形態において用いられるＳＬＡＭ技術を利用した自己位置推定の基本原理を説明する。ここでは、簡単のため、移動体が２次元空間（すなわち平面）内を移動するものとする。
【0025】
まず、図１を参照する。図１は、時刻ｔ１から時刻ｔ２までの間に平面上を移動する移動体１００の例を模式的に示す図である。この例における移動体１００は、車輪によって走行する作業車両（例えばトラクタ）である。移動体１００は、トラクタ以外の車両、または歩行ロボットもしくはドローンなどの他の種類の移動体であってもよい。図１には、平面上を移動する移動体１００の位置および向きが模式的に示されている。移動体１００の位置座標は、ＸＹ座標系によって示される。ＸＹ座標系は、地球に固定されたワールド座標系Σｗである。図１には、移動体１００に固定された移動体座標系ΣＶも示されている。この例における移動体座標系ΣＶは、移動体１００の正面が向く方向をｕ軸方向、ｕ軸を反時計回りに９０度回転させた方向をｖ軸方向とするｕｖ座標系である。移動体座標系ΣＶの原点の位置は、移動体１００の移動に伴って変化する。移動体座標系ΣＶの向きは、移動体１００の向きの変化、すなわち回転に伴って変化する。
【0026】
時刻ｔ１における移動体１００のポーズ（すなわち位置および向き）をｒ１、時刻ｔ２における移動体１００のポーズをｒ２とする。ポーズｒ１は、例えば、座標（ｘ１，ｙ１）で示される位置と、角度θ１で示される向きとによって規定される。ここでは、移動体１００の向きは移動体１００の正面が向く方向であると定義する。また、Ｘ軸正方向を角度の基準方向とし、反時計回りを角度の正方向とする。ポーズｒ２は、座標（ｘ２，ｙ２）で示される位置と、角度θ２で示される向きとによって規定される。
【0027】
図１の例において、時刻ｔ１から時刻ｔ２までの間に、移動体１００の位置は、角度θ１の方位にΔｄ１だけ移動（並進）し、移動体１００の向きは、Δφ１（＝θ２－θ１）だけ反時計回りに回転している。このように、移動体１００の運動は、「並進」と「回転」の合成である。
【0028】
この例において、距離Δｄ１が十分に短いとき、移動体１００の進行方向は移動体座標系ΣＶのｕ軸に平行であると近似できる。このため、以下の式１が成立する。
【数1】
【0029】
移動体１００が車輪の回転数センサおよび／または慣性計測装置（ＩＭＵ）などの内界センサを備えている場合、その内界センサから、Δｄ１およびΔφ１の推定値、すなわちオドメトリ情報を取得することができる。時刻ｔ１から時刻ｔ２までの時間差が例えば１０ミリ秒のように短い場合、距離Δｄ１は十分に短く、式１が成立する。時刻がｔ１、t２、ｔ３、・・・と進む過程で、Δｄ１およびΔφ１の推定値を周期的に更新していくことにより、移動体１００の位置および向き（すなわちポーズ）の変化を推定することが可能である。言い換えると、初期のポーズ、例えば（ｘ１，ｙ１，θ１）が既知である場合、その後の移動体１００のポーズの推定値をオドメトリ情報に基づいて周期的に更新することができる。しかし、オドメトリ情報に基づくポーズの推定には、誤差が累積するという問題がある。このため、多くの場合、衛星測位システムまたはＳＬＡＭ技術を利用して、移動体１００の位置の正確度の高い推定値を取得することが必要である。
【0030】
【0031】
＜環境地図データが存在する場合＞
図２Ａおよび図２Ｂに示される例において、時刻ｔ１から時刻ｔ２までの間に移動体１００が移動する環境には、位置が固定されたランドマークｍ１、ｍ２、ｍ３、ｍ４が存在する。この例では、ランドマークの位置を示す環境地図データがすでに存在する。このような環境地図データは、例えば、個々のランドマークを特定する識別子（例えばランドマーク番号）と、各識別子に対応付けられたランドマークのワールド座標系Σｗにおける位置座標とを含む。ワールド座標系Σｗにおけるランドマークｍ１、ｍ２、ｍ３、ｍ４の座標を、それぞれ、（ｘｍ１，ｙｍ１）、（ｘｍ２，ｙｍ２）、（ｘｍ３，ｙｍ３）、（ｘｍ４，ｙｍ４）とする。ＬｉＤＡＲによって取得される点群データにおける個々の点がランドマークとして機能してもよい。点群データによって構成される環境地図を、点群地図と称する。ランドマークの位置は格子地図上のセルによって表示されていてもよい。
図２Ａおよび図２Ｂに示される例において、時刻ｔ１から時刻ｔ２までの間に移動体１００が移動する環境には、位置が固定されたランドマークｍ１、ｍ２、ｍ３、ｍ４が存在する。この例では、ランドマークの位置を示す環境地図データがすでに存在する。このような環境地図データは、例えば、個々のランドマークを特定する識別子（例えばランドマーク番号）と、各識別子に対応付けられたランドマークのワールド座標系Σｗにおける位置座標とを含む。ワールド座標系Σｗにおけるランドマークｍ１、ｍ２、ｍ３、ｍ４の座標を、それぞれ、（ｘｍ１，ｙｍ１）、（ｘｍ２，ｙｍ２）、（ｘｍ３，ｙｍ３）、（ｘｍ４，ｙｍ４）とする。ＬｉＤＡＲによって取得される点群データにおける個々の点がランドマークとして機能してもよい。点群データによって構成される環境地図を、点群地図と称する。ランドマークの位置は格子地図上のセルによって表示されていてもよい。
【0032】
図２Ａの例において、ポーズｒ１における移動体１００が取得したランドマークｍ１、ｍ２、ｍ３、ｍ４の観測値（または計測値）を、それぞれ、ｚ１、ｚ２、ｚ３、ｚ４とする。観測値は、例えば、外界センサによって計測される各ランドマークまでの距離および方向のそれぞれを示す値である。観測値は、外界センサによって取得されるデータであるため、本開示において「センサデータ」と称することがある。
【0033】
図２Ｂに示されるように、移動体１００が移動し、ポーズｒ２における移動体１００が取得したランドマークｍ１、ｍ２、ｍ３の観測値を、それぞれ、ｚ５、ｚ６、ｚ７とする。
【0034】
これらの観測値に基づいて、移動体座標系ΣＶ上のランドマークの位置座標を取得することができる。前述したように、移動体座標系ΣＶは移動体１００に固定された座標系であるため、同一のランドマーク（例えばランドマークｍ１）の移動体座標系ΣＶ上における位置座標は、移動体１００のポーズの変化に応じて変化していく。
【0035】
観測値に基づいて取得されるランドマークの位置は、外界センサの位置および向きによって定まるセンサ座標系における座標を有する。厳密には、センサ座標系は移動体座標系ΣＶとは異なり得るが、以下の説明では、センサ座標系と移動体座標系ΣＶとは同一であるものとする。センサ座標系と移動体座標系ΣＶとの関係は既知であるため、一方の座標系を既知の角度だけ回転させ、かつ、既知の距離だけ並進させることにより、他方の座標系に一致させることができる。
【0036】
環境内を移動する移動体１００は、複数のランドマークを観測することにより、移動体座標系ΣＶにおける各ランドマークの位置座標を取得する。そして、ワールド座標系ΣＷにおける各ランドマークの位置座標が環境地図データに含まれている場合、観測値ｚ１～ｚ７などに基づいてポーズｒ１、ｒ２を推定することができる。このような推定は、例えば、観測値から求められるランドマークの位置座標と、環境地図データに含まれるランドマークの位置座標とのマッチングを行うことにより可能になる。
【0037】
次に、図３を参照して、移動体座標系ΣＶ上の座標（ｕ１，ｖ１）をワールド座標系ΣＷ上の座標（ｕ１’、ｖ１’）に変換する方法を説明する。図３は、時刻ｔ２における移動体座標系ΣＶとワールド座標系ΣＷとの配置関係を示している。図３の例では、簡単のため、移動体座標系ΣＶの原点をセンサ座標系の原点（センサ中心）と一致させ、両座標系が同一であるものとする。移動体座標系ΣＶ上の座標（ｕ１，ｖ１）は、時刻ｔ２のポーズｒ２における移動体１００からのランドマークｍ１に対する観測値ｚ５に基づいて算出される。観測値ｚ５が、距離ｒｍ１とｕ軸に対する角度φｍ１とによって規定されるとすると、ｕ１およびｖ１は、ｕ１＝ｒｍ１ｃｏｓφｍ１、ｖ１＝ｒｍ１ｓｉｎφｍ１によって計算される。移動体座標系ΣＶ上の座標からワールド座標系ΣＷ上の座標への変換（座標変換）は、移動体座標系ΣＶを時計回りに角度θ２だけ回転させること、および、移動体座標系ΣＶの原点をワールド座標系ΣＷの原点まで並進させることによって行われる。このとき、以下の式２および式３が成立する。
【数2】
【数3】
【0038】
ここで、Ｒは、移動体１００の向きによって決まる回転行列であり、Ｔはワールド座標系Σｗにおける移動体１００の位置ベクトルである。回転行列Ｒおよび位置ベクトルＴの中身は、移動体１００のポーズから求められる。
【0039】
座標変換によって得られる（ｕ１’、ｖ１’）は、ワールド座標系Σｗにおけるランドマークｍ１の座標である（ｘｍ１，ｙｍ１）に一致すべきである。しかし、移動体１００のポーズの推定値が真値からずれていると、（ｕ１’，ｖ１’）と（ｘｍ１，ｙｍ１）との間に誤差（距離）が生じる。自己位置推定を行うことは、（ｕ１’，ｖ１’）と（ｘｍ１，ｙｍ１）との間の誤差が小さくなるように回転行列Ｒおよび位置ベクトルＴの中身を決定することである。
【0040】
次に、図４および図５を参照してマッチングの一例をより詳細に説明する。図４は、時刻ｔ２における移動体座標系ΣＶであるｕｖ座標系上のランドマークｍ１、ｍ２、ｍ３の座標を模式的に示している。これらの座標は、観測値ｚ５、ｚ６、ｚ７から算出され得る。この例において、観測値ｚ５、ｚ６、ｚ７から算出される座標を、それぞれ、（ｕ５，ｖ５）、（ｕ６，ｖ６）、（ｕ７，ｖ７）とする。移動体座標系ΣＶからワールド座標系ΣＷへの座標変換を行うことにより、移動体座標系ΣＶ上の（ｕ５，ｖ５）、（ｕ６，ｖ６）、（ｕ７，ｖ７）は、それぞれ、図５に示されるように、ワールド座標系ΣＷ上の（ｕ５’，ｖ５’）、（ｕ６’，ｖ６’）、（ｕ７’，ｖ７’）に変換される。図５は、変換された座標（ｕ５’，ｖ５’）、（ｕ６’，ｖ６’）、（ｕ７’，ｖ７’）と、対応するランドマークの座標（ｘｍ２，ｙｍ２）、（ｘｍ３，ｙｍ３）、（ｘｍ４，ｙｍ４）との間に差異が生じている状態の例を示している。座標変換は、回転行列Ｒによる回転、および位置ベクトルＴによる並進によって規定される。ワールド座標系ΣＷ上において、（ｕ５’，ｖ５’）、（ｕ６’，ｖ６’）、（ｕ７’，ｖ７’）が、それぞれ、対応するランドマークの座標である（ｘｍ２，ｙｍ２）、（ｘｍ３，ｙｍ３）、（ｘｍ４，ｙｍ４）に近づくように（すなわちマッチするように）座標変換（Ｒ、Ｔ）の中身が決定される。
【0041】
前述した式２の等式において、未知数はｘ２、ｙ２、θ２である。未知数の個数は３であるため、式２に相当する等式が３個以上あれば、計算によって、ｘ２、ｙ２、θ２を求めることができる。図５に示されるように同一のポーズｒ２における移動体１００から３個以上のランドマークが観測される場合、式２に相当する等式が３個得られるため、連立方程式を解くことにより、ポーズｒ２（ｘ２、ｙ２、θ２）の計算値を取得できる。現実の観測には誤差が加わるため、３個を超える多数の式からなる連立方程式を用い、最小二乗法などによる最適化が行われる。
【0042】
このようなマッチングによる最適化を行って自己位置を推定するアルゴリズムの例は、ＩＣＰマッチング法およびＮＤＴマッチング法を含む。これらのいずれのマッチング法、または他の方法を用いてもよい。
【0043】
なお、観測値と環境地図データから移動体１００の位置を推定するとき、必ずしも移動体１００の向きを推定する必要はない。例えば図２Ａおよび図２Ｂに示されるように、位置が既知であるランドマークまでの距離が観測値から取得できる場合、三角測量の原理に基づいて、移動体１００の位置（正確には測距センサの位置）を算出することが可能である。このような原理に基づいて位置を推定することも、「自己位置推定」に含まれる。
【0044】
＜環境地図データが存在しない場合＞
環境地図データが存在しない場合、図２Ａおよび図２Ｂに示されるように移動体１００が移動中に取得した観測値に基づいて、ランドマークｍ１、ｍ２、ｍ３、ｍ４、・・・の座標を決定（すなわち推定）しながら、移動体１００のポーズを推定する処理が必要になる。ランドマークｍ１、ｍ２、ｍ３、ｍ４、・・・のワールド座標系ΣＷ上における座標は、環境地図データを構成する。したがって、地図を構築するには、ランドマークとして機能する物体のワールド座標系ΣＷ上における位置座標を決定する必要がある。
環境地図データが存在しない場合、図２Ａおよび図２Ｂに示されるように移動体１００が移動中に取得した観測値に基づいて、ランドマークｍ１、ｍ２、ｍ３、ｍ４、・・・の座標を決定（すなわち推定）しながら、移動体１００のポーズを推定する処理が必要になる。ランドマークｍ１、ｍ２、ｍ３、ｍ４、・・・のワールド座標系ΣＷ上における座標は、環境地図データを構成する。したがって、地図を構築するには、ランドマークとして機能する物体のワールド座標系ΣＷ上における位置座標を決定する必要がある。
【0045】
図２Ａおよび図２Ｂの例では、移動しつつある移動体１００がランドマークｍ２、ｍ３、ｍ４のそれぞれから複数の観測値を取得している。より多くの観測を行うことにより、移動体１００のポーズおよびランドマークの座標に含まれる未知数の総数よりも多くの連立方程式を得ることができる。それにより、移動体１００のポーズおよびランドマークの座標の推定値を算出することが可能である。
【0046】
ＳＬＡＭによって自己位置推定と環境地図の作成を行うためのアルゴリズムには、多数の種類が存在する。ＳＬＡＭアルゴリズムの例は、ＬｉＤＡＲセンサを利用するアルゴリズムだけではなく、カメラなどの他の外界センサを利用するアルゴリズムも含む。また、自己位置推定にパーティクルフィルタなどのベイズフィルタを利用したり、グラフベース手法によってポーズ推定の精度を高めたりしてもよい。本開示の実施形態では、ＳＬＡＭアルゴリズムの種類は特に限定されない。
【0047】
なお、ＬｉＤＡＲセンサには、レーザビームのスキャンによって空間中の物体の距離分布の情報を取得するスキャン型のセンサと、広範囲に拡散する光を利用して空間中の物体の距離分布の情報を取得するフラッシュ型のセンサとがある。スキャン型のＬｉＤＡＲセンサは、フラッシュ型のＬｉＤＡＲセンサよりも高い強度の光を使用するため、より遠方の距離情報を取得することができる。一方、フラッシュ型のＬｉＤＡＲセンサは、構造が簡単であり、低いコストで製造できるため、強い光を必要としない用途に適している。本開示では、主にスキャン型のＬｉＤＡＲセンサが用いられる例を説明するが、用途によってはフラッシュ型のＬｉＤＡＲセンサも使用され得る。
【0048】
スキャン型の典型的なＬｉＤＡＲセンサによって環境内の物体を観測する場合、パルス状のレーザビーム（すなわちレーザパルス）を出射し、周囲の環境に存在する物体によって反射されたレーザパルスがＬｉＤＡＲセンサに帰還するまでの時間を計測することにより、物体の表面に位置する反射点までの距離と方向とを知ることができる。反射点までの距離および方向がわかれば、移動体座標系ΣＶにおける「反射点」の座標を取得することができる。スキャン型のＬｉＤＡＲセンサは、２次元ＬｉＤＡＲセンサと、３次元ＬｉＤＡＲセンサとに分類することができる。２次元ＬｉＤＡＲセンサによれば、レーザビームが１つの平面内を回転するように環境のスキャンが行われ得る。これに対して、３次元ＬｉＤＡＲセンサでは、複数のレーザビームがそれぞれ異なる円錐面に沿って回転するように環境のスキャンが行われ得る。これらのＬｉＤＡＲセンサによって取得される個々の反射点の座標（２次元または３次元の座標値）は、移動体座標系ΣＶで表される。個々の反射点の座標を移動体座標系ΣＶからワールド座標系ΣＷに変換することにより、個々の反射点のワールド座標系ΣＷ上における座標を得ることができ、その結果、点群地図を構築することができる。移動体座標系ΣＶからワールド座標系ΣＷに変換するためには、上記のように、移動体１００のポーズの情報が必要である。
【0049】
ＳＬＡＭ技術では、移動体１００のポーズの推定と、環境地図の構築とを同時に実行することができる。しかしながら、環境地図を構築するときにＳＬＡＭ技術以外の技術を利用して移動体１００のポーズを推定または測定してもよい。移動体１００のポーズは、移動体１００の位置を測定する衛星測位システムおよび慣性誘導装置を利用しても決定することができるからである。しかしながら、衛星測位システムのような測位システムを利用できない状況では、環境地図を構築するためにＳＬＡＭ技術を利用して移動体１００のポーズを推定することが必要になる。なお、移動体１００の位置についてはＳＬＡＭ技術を利用して推定し、移動体１００の向きまたは姿勢については、慣性計測装置などの他のセンサによって推定してもよい。
【0050】
［３．実施形態］
以下、本開示の実施形態を説明する。ただし、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明および実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になることを避け、当業者の理解を容易にするためである。なお、発明者は、当業者が本開示を十分に理解するために添付図面および以下の説明を提供するのであって、これらによって特許請求の範囲に記載の主題を限定することを意図するものではない。以下の説明において、同一または類似の機能を有する構成要素については、同じ参照符号を付している。
以下、本開示の実施形態を説明する。ただし、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明および実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になることを避け、当業者の理解を容易にするためである。なお、発明者は、当業者が本開示を十分に理解するために添付図面および以下の説明を提供するのであって、これらによって特許請求の範囲に記載の主題を限定することを意図するものではない。以下の説明において、同一または類似の機能を有する構成要素については、同じ参照符号を付している。
【0051】
［３－１．実施形態１］
本開示の第１の実施形態による移動体を説明する。
本開示の第１の実施形態による移動体を説明する。
【0052】
本実施形態における移動体は、複数の樹木が生い茂り、複数の樹木列が形成されている環境で使用される。移動体は、周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、記憶装置と、自己位置推定装置と、移動体の移動を制御する制御装置とを備える。記憶装置は、複数の樹木列の幹部の分布を示す環境地図データを記憶する。自己位置推定装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。制御装置は、推定された移動体の位置に応じて移動体の移動を制御する。本実施形態における移動体は、環境地図データ、または環境地図データを生成するための局所地図データを生成するデータ生成装置をさらに備えている。データ生成装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。データ生成装置は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成し、記憶装置に記録することもできる。このように、本実施形態における移動体は、複数の樹木列の間を移動しながら環境地図を作成する機能と、作成した環境地図を利用して移動体の位置を推定しながら樹木列間を自律的に移動する機能とを備える。
【0053】
１つ以上のセンサは、例えば２次元または３次元の点群データをセンサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含み得る。本明細書において「点群データ」は、ＬｉＤＡＲセンサによって観測される複数の反射点の分布を示すデータを広く意味する。点群データは、例えば、２次元空間または３次元空間における各反射点の座標値、および／または各反射点の距離および方向を示す情報を含み得る。
【0054】
ＬｉＤＡＲセンサは、例えば予め設定された周期で点群データを繰り返し出力する。データ生成装置は、１周期以上の期間中に出力された点群データにおける各点の位置、または各点の移動体からの距離もしくは角度に基づいて、幹部を検出することができる。
【0055】
以下、移動体が、主にぶどう園などの果樹園における作業に用いられるトラクタなどの作業車両であり、センサがスキャン型のＬｉＤＡＲセンサである場合を例に、本実施形態の構成および動作を説明する。
【0056】
＜構成＞
図６は、本実施形態におけるトラクタ１００Ａの外観を示す斜視図である。図７は、トラクタ１００Ａを側面方向から見た模式図である。図７には、トラクタ１００Ａに連結されて使用される作業機（implement）３００も示されている。
図６は、本実施形態におけるトラクタ１００Ａの外観を示す斜視図である。図７は、トラクタ１００Ａを側面方向から見た模式図である。図７には、トラクタ１００Ａに連結されて使用される作業機（implement）３００も示されている。
【0057】
図６に示すように、本実施形態におけるトラクタ１００Ａは、ＬｉＤＡＲセンサ１１０と、ＧＮＳＳユニット１２０と、１つ以上の障害物センサ１３０とを備える。図６の例では１つの障害物センサ１３０が示されているが、障害物センサ１３０は、トラクタ１００Ａの複数の箇所に設けられ得る。
【0058】
図７に示すように、トラクタ１００Ａは、車両本体１０１と、原動機（エンジン）１０２と、変速装置（トランスミッション）１０３とを備える。車両本体１０１には、車輪（タイヤ）１０４と、キャビン１０５とが設けられている。車輪１０４は、前輪１０４Ｆと後輪１０４Ｒとを含む。キャビン１０５の内部に運転席１０７、操舵装置１０６、および操作端末２００が設けられている。前輪１０４Ｆおよび後輪１０４Ｒの一方または両方は、タイヤではなくクローラであってもよい。
【0059】
本実施形態におけるＬｉＤＡＲセンサ１１０は、車両本体１０１の前面下部に配置されている。ＬｉＤＡＲセンサ１１０は、トラクタ１００Ａが走行する環境に存在する樹木列の幹部の平均高さよりも低い位置、例えば、地上から１５ｃｍ以上１００ｃｍ以下の高さに配置され得る。ＬｉＤＡＲセンサ１１０は、例えば、トラクタ１００Ａの高さの半分よりも低い位置に配置され得る。トラクタ１００Ａの高さは、地面からキャビン１０５の最上部までの高さであり、例えば２メートル以上であり得る。ＬｉＤＡＲセンサ１１０は、トラクタ１００Ａの高さの１／３よりも低い位置に配置されていてもよい。図７の例では、ＬｉＤＡＲセンサ１１０は、車輪１０４の上部および前照灯１０９よりも低い位置に配置されている。ＬｉＤＡＲセンサ１１０をこのような低い位置に配置することによって得られる効果については後述する。
【0060】
ＬｉＤＡＲセンサ１１０は、トラクタ１００Ａが移動している間、周囲の環境に存在する物体の距離および方向、または２次元もしくは３次元の座標値を示すセンサデータを繰り返し出力する。ＬｉＤＡＲセンサ１１０から出力されたセンサデータは、トラクタ１００Ａが備えるＥＣＵ（Electronic Control Unit）などの制御装置によって処理される。制御装置は、前述のＳＬＡＭアルゴリズムを利用して、センサデータに基づく環境地図データの生成、および環境地図を用いた自己位置推定などの処理を実行することができる。なお、制御装置は、環境地図データを完成する代わりに、環境地図データを生成するための局所地図データを作成してもよい。その場合、局所地図データを統合して環境地図データを構築する処理は、トラクタ１００Ａの外部にあるコンピュータ、例えばクラウドサーバで実行されてもよい。
【0061】
図７の例では、車両本体１０１の後部に障害物センサ１３０が設けられている。障害物センサ１３０は、車両本体１０１の後部以外の部位にも配置され得る。例えば、車両本体１０１の側部、前部、およびキャビン１０５のいずれかの箇所に、１つまたは複数の障害物センサ１３０が設けられ得る。障害物センサ１３０は、トラクタ１００Ａの比較的近くに存在する物体を検出する。障害物センサ１３０は、例えばレーザスキャナまたは超音波ソナーを備え得る。障害物センサ１３０は、障害物センサ１３０から所定の距離よりも近くに物体が存在する場合に、障害物が存在することを示す信号を出力する。複数の障害物センサ１３０がトラクタ１００Ａの車体の異なる位置に設けられていてもよい。例えば、複数のレーザスキャナと、複数の超音波ソナーとが、車体の異なる位置に配置されていてもよい。そのような多くの障害物センサ１３０を備えることにより、トラクタ１００Ａの周囲の障害物の監視における死角を減らすことができる。ＬｉＤＡＲセンサ１１０が障害物センサとして機能してもよい。
【0062】
ＧＮＳＳユニット１２０は、キャビン１０５の上部に設けられている。ＧＮＳＳユニット１２０は、ＧＮＳＳ衛星からの信号を受信するアンテナと、処理回路とを備えるＧＮＳＳ受信機である。ＧＮＳＳユニット１２０は、例えばＧＰＳ（Global Positioning System）、ＧＬＯＮＡＳ、Ｇａｌｉｌｅｏ、ＢｅｉＤｏｕ、または準天頂衛星（Quasi-Zenith Satellite System：ＱＺＳＳ、例えばみちびき）などのＧＮＳＳ衛星から送信されるＧＮＳＳ信号を受信し、当該信号に基づいて測位を行う。本実施形態におけるトラクタ１００Ａは、主にぶどう園のように、複数の樹木が生い茂り、ＧＮＳＳを利用しにくい環境で使用されるため、ＬｉＤＡＲセンサ１１０を利用して測位が行われる。しかし、ＧＮＳＳ信号を受信可能な環境ではＧＮＳＳユニット１２０を利用して測位を行ってもよい。ＬｉＤＡＲセンサ１１０による測位とＧＮＳＳユニット１２０による測位とを組み合わせることにより、測位の安定性または精度を向上させることができる。
【0063】
原動機１０２は、例えばディーゼルエンジンである。ディーゼルエンジンに代えて電動モータが使用されてもよい。変速装置１０３は、変速によってトラクタ１００Ａの推進力を切り換えることができる。変速装置１０３は、トラクタ１００Ａの前進と後進とを切り換えることもできる。
【0064】
操舵装置１０６は、ステアリングホイールと、ステアリングホイールに接続された操舵軸と、ステアリングホイールによる操舵を補助するパワーステアリング装置とを含む。パワーステアリング装置は、操舵軸を回転させるための補助力を供給する油圧装置または電動モータを含む。前輪１０４Ｆは操舵輪であり、その操舵角を変化させることにより、トラクタ１００Ａの走行方向を変化させることができる。前輪１０４Ｆの操舵角は、ステアリングホイールを操作することによって変化させることができる。自動操舵が行われるときには、トラクタ１００Ａ内に配置された電子制御ユニット（ＥＣＵ）からの制御により、油圧装置または電動モータの力によって操舵角が自動で調整される。
【0065】
車両本体１０１の後部には、連結装置１０８が設けられている。連結装置１０８は、例えば３点リンク（ヒッチ）装置、ＰＴＯ（Power Take Off）軸、およびユニバーサルジョイントを含む。連結装置１０８によって作業機３００をトラクタ１００Ａに着脱することができる。連結装置１０８は、例えば油圧装置によって３点リンク装置を昇降させ、作業機３００の位置または姿勢を制御することができる。また、ユニバーサルジョイントを介してトラクタ１００Ａから作業機３００に動力を送ることができる。トラクタ１００Ａは、作業機３００を引きながら、作業機３００に所定の作業を実行させることができる。連結装置は、車両本体１０１の前方に設けられていてもよい。その場合、トラクタ１００Ａの前方に作業機を接続することができる。なお、作業機がトラクタ１００Ａの前方に接続された状態でＬｉＤＡＲセンサ１１０が使用される場合、ＬｉＤＡＲセンサ１１０は、レーザビームが作業機によって遮られない位置に配置される。
【0066】
図７に示す作業機３００は、作物に薬剤を噴霧するスプレイヤであるが、作業機３００はスプレイヤに限定されない。例えば、モーア（草刈機）、シーダ（播種機）、スプレッダ（施肥機）、レーキ、ベーラ（集草機）、ハーベスタ（収穫機）、プラウ、ハロー、またはロータリなどの、任意の作業機をトラクタ１００Ａに接続して使用することができる。
【0067】
以下の説明において、図７に示すｕｖｗ座標系を用いる。ｕｖｗ座標系は、トラクタ１００ＡおよびＬｉＤＡＲセンサ１１０に固定された座標系である。トラクタ１００Ａの進行方向をｕ軸方向、左方向をｖ軸方向、上方向をｗ軸方向とする。図面では、わかりやすさのため、ｕｖｗ座標系の原点がトラクタ１００Ａから離れた位置に存在するように記載されているが、現実には、トラクタ１００Ａに設けられるＬｉＤＡＲセンサ１１０の中心にｕｖｗ座標系の原点が一致している。
【0068】
図８は、トラクタ１００Ａおよび作業機３００の概略的な構成の例を示すブロック図である。この例におけるトラクタ１００Ａは、ＬｉＤＡＲセンサ１１０、ＧＮＳＳユニット１２０、障害物センサ１３０、操作端末２００に加え、慣性計測装置（ＩＭＵ）１２５、駆動装置１４０、記憶装置１５０、複数の電子制御ユニット（ＥＣＵ）１６０、１７０、１８０、および通信インタフェース（ＩＦ）１９０を備える。作業機３００は、駆動装置３４０と、制御装置３８０と、通信インタフェース３９０とを備える。なお、図８には、本実施形態における自己位置推定動作、自動走行動作、および地図生成動作との関連性が相対的に高い構成要素が示されており、それ以外の構成要素の図示は省略されている。
【0069】
本実施形態におけるＩＭＵ１２５は、３軸加速度センサおよび３軸ジャイロスコープを備える。ＩＭＵ１２５は、モーションセンサとして機能し、トラクタ１００Ａの加速度、速度、変位、および姿勢などの諸量を示す信号を出力することができる。ＩＭＵ１２５は、例えば１秒間に数十回から数千回程度の頻度で、当該信号を出力する。ＩＭＵ１２５に代えて、３軸加速度センサおよび３軸ジャイロスコープを別々に設けてもよい。
【0070】
駆動装置１４０は、例えば前述の原動機１０２、変速装置１０３、車輪１０４、操舵装置１０６、および連結装置１０８などの、トラクタ１００Ａの走行および作業機３００の駆動に必要な各種の装置を含む。原動機１０２は、例えばディーゼル機関などの内燃機関を備える。駆動装置１４０は、内燃機関に代えて、あるいは内燃機関とともに、トラクション用の電動モータを備えていてもよい。
【0071】
記憶装置１５０は、フラッシュメモリまたは磁気ディスクなどの記憶媒体を含み、環境地図データを初めとするＥＣＵ１６０、１７０、１８０が生成する各種のデータを記憶する。記憶装置１５０は、ＥＣＵ１６０、１７０、１８０に、後述する各種の動作を実行させるコンピュータプログラムも記憶する。そのようなコンピュータプログラムは、記憶媒体（例えば半導体メモリまたは光ディスク等）または電気通信回線（例えばインターネット）を介してトラクタ１００Ａに提供され得る。そのようなコンピュータプログラムが、商用ソフトウェアとして販売されてもよい。
【0072】
ＥＣＵ１６０は、前述のＳＬＡＭ技術に基づく処理を実行する電子回路である。ＥＣＵ１６０は、自己位置推定モジュール１６２および地図データ生成モジュール１６４を含む。ＥＣＵ１６０は、前述の自己位置推定装置およびデータ生成装置の一例である。ＥＣＵ１６０は、トラクタ１００Ａが走行している間にＬｉＤＡＲセンサ１１０およびＩＭＵ１２５から繰り返し出力されたデータまたは信号に基づき、トラクタ１００Ａの位置および向き（すなわちポーズ）の推定を行いながら地図データを生成することができる。ＥＣＵ１６０はまた、環境地図データが生成された後の自動走行時において、ＬｉＤＡＲセンサ１１０から出力されたセンサデータと環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置および向きを推定することができる。
【0073】
自己位置推定モジュール１６２は、トラクタ１００Ａの位置および向きの推定、すなわち自己位置推定を行うための演算を実行する。地図データ生成モジュール１６４は、地図データを生成する処理を実行する。地図データ生成モジュール１６４が生成する「地図データ」には、自動走行時のマッチングに使用される環境地図データと、環境地図データを構築するために生成される部分的なデータである局所地図データとが含まれる。地図データ生成モジュール１６４は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成し、記憶装置１５０に記録することができる。自己位置推定モジュール１６２および地図データ生成モジュール１６４は、１つの回路で実現されていてもよいし、複数の回路に分かれていてもよい。
【0074】
環境地図データがまだ生成されていない段階において、ＥＣＵ１６０は、自己位置推定を行いながら、トラクタ１００Ａの周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを繰り返し生成して記憶装置１５０に記録することができる。ＥＣＵ１６０は、さらに、局所地図データを繋ぎ合わせることにより、圃場（例えばぶどう園）の全体、または圃場の一区画についての環境地図データを生成することができる。圃場の区画ごとに環境地図データが生成されてもよい。ＥＣＵ１６０は、局所地図データを生成する段階では樹木列の幹部を検出せず、最終的な環境地図データを生成する段階で幹部を検出し、幹部を他の物体と区別できる形式で記録してもよい。環境地図データが生成された後の自動走行時において、ＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０から出力されるセンサデータと環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置および向きを推定する。なお、ＥＣＵ１６０は、トラクタ１００Ａの位置のみをマッチングで決定し、トラクタ１００Ａの向きはＩＭＵ１２５からの信号を利用して決定してもよい。
【0075】
ＥＣＵ１７０は、トラクタ１００Ａの経路を決定する処理を行う回路である。環境地図データがまだ生成されていない段階において、ＥＣＵ１７０は、ＬｉＤＡＲセンサ１１０および障害物センサ１３０から出力されたデータまたは信号に基づいて、トラクタ１００Ａが走行すべき経路を決定する。例えば、ＬｉＤＡＲセンサ１１０から出力されるセンサデータに基づいて検出される複数の樹木列の間を通り、かつ障害物を避ける経路が目標経路として決定される。環境地図データが生成された後の自動走行時において、ＥＣＵ１７０は、環境地図データに基づいて、またはユーザからの指示に基づいて、目標経路（以下、「走行予定経路」とも称する。）を決定する。
【0076】
ＥＣＵ１８０は、駆動装置１４０を制御する回路である。ＥＣＵ１８０は、ＥＣＵ１６０によって推定されたトラクタ１００Ａの位置および向きと、ＥＣＵ１７０によって決定された走行予定経路とに基づいて、駆動装置１４０を制御する。ＥＣＵ１８０はまた、作業機３００の動作を制御する信号を生成し、その信号を通信ＩＦ１９０から作業機３００に送信する動作も実行する。
【0077】
ＥＣＵ１６０、１７０、１８０は、例えばＣＡＮ（Controller Area Network）などのビークルバス規格に従って、相互に通信することができる。図８において、ＥＣＵ１６０、１７０、１８０のそれぞれは、個別のブロックとして示されているが、これらのそれぞれの機能が、複数のＥＣＵによって実現されていてもよい。あるいは、ＥＣＵ１６０、１７０、１８０の機能を統合した１つの車載コンピュータが設けられていてもよい。トラクタ１００Ａは、ＥＣＵ１６０、１７０、１８０以外のＥＣＵを備えていてもよく、機能に応じて任意の個数のＥＣＵが設けられ得る。
【0078】
通信ＩＦ１９０は、作業機３００の通信ＩＦ３９０との間で通信を行う回路である。通信ＩＦ１９０は、例えばＩＳＯ １１７８３に基づくＩＳＯＢＵＳなどの通信制御規格に準拠した信号の送受信を、作業機３００の通信ＩＦ３９０との間で実行する。これにより、作業機３００に所望の動作を実行させたり、作業機３００から情報を取得したりすることができる。通信ＩＦ１９０はまた、有線または無線のネットワークを介して外部のコンピュータと通信することもできる。例えば、作物の生育状況、トラクタ１００Ａの稼働状況、および作業記録等を管理する営農管理システムにおけるサーバなどのコンピュータと通信することができる。
【0079】
操作端末２００は、トラクタ１００Ａの自動走行または自動操舵に関する操作をユーザが実行するための端末であり、バーチャルターミナル（ＶＴ）と称することもある。操作端末２００は、タッチスクリーンなどの表示装置、および／または１つ以上のボタンを備え得る。ユーザは、操作端末２００を操作することにより、例えば自動走行モードまたは自動操舵モードのオン／オフの切り替え、トラクタ１００Ａの初期位置の設定、経路の設定、環境地図の記録または編集などの種々の操作を実行することができる。
【0080】
作業機３００における駆動装置３４０は、作業機３００が所定の作業を実行するために必要な動作を行う。駆動装置３４０は、例えばポンプ、油圧装置、または電気モータなどの、作業機３００の用途に応じた装置を含む。
【0081】
制御装置３８０は、駆動装置３４０の動作を制御する。制御装置３８０は、通信ＩＦ３９０を介してトラクタ１００Ａから送信された信号に応答して、駆動装置３４０に各種の動作を実行させる。
【0082】
次に、図９Ａおよび図９Ｂを参照して、ＬｉＤＡＲセンサ１１０の構成例を説明する。本実施形態におけるＬｉＤＡＲセンサ１１０は、レーザビームのスキャンによって空間中の物体の距離分布の情報を取得可能なスキャン型のセンサである。図９Ａは、ＬｉＤＡＲセンサ１１０をトラクタ１００Ａの側面方向から見た場合の模式図である。図９Ｂは、ＬｉＤＡＲセンサ１１０を鉛直上方から見た場合の模式図である。図９Ａおよび図９Ｂにおいて放射状に延びる複数の直線は、ＬｉＤＡＲセンサ１１０から出射されるレーザビームの中心軸（あるいは進行方向）を模式的に表している。各レーザビームは、平行光にコリメートされているが、数ミリラジアン（例えば０．１～０．２度）の拡がり角度を有している。このため、各レーザビームの断面サイズ（スポット径）は、ＬｉＤＡＲセンサ１１０からの距離に比例して拡大する。例えば、ＬｉＤＡＲセンサ１１０から２０メートル先で直径が数センチメートルの光スポットが形成され得る。図では、簡単のため、レーザビームの広がりは無視し、レーザビームの中心軸のみが記載されている。
【0083】
図９Ａに示す例におけるＬｉＤＡＲセンサ１１０は、縦方向に配列された複数のレーザ光源から、それぞれ、異なる仰角でレーザビームを出射することができる。仰角は、ｕｖ面に対する角度によって定義される。この例において、ｕｖ面は水平面にほぼ平行である。なお、地面（地表）が水平面に対して傾斜している場合は、ｕｖ面と水平面とが交差する。図９Ａには、Ｎ本のレーザビームＬ１、・・・、ＬＮが出射されている様子が示されている。ここで「Ｎ」は、１以上の整数であり、例えば１０以上、高性能な機種では６４、あるいは１００以上であり得る。複数のレーザビームのうち、下からｋ番目のレーザビームの仰角をθｋとする。図９Ａには、一例として、Ｎ－１番目のレーザビームの仰角θＮ－１が示されている。ｕｖ面よりも上方に向かうレーザビームの仰角を「正の仰角」とし、ｕｖ面よりも下方に向かうレーザビームの仰角を「負の仰角」とする。
【0084】
Ｎが１であるＬｉＤＡＲセンサを「２次元ＬｉＤＡＲ」と称し、Ｎが２以上であるＬｉＤＡＲセンサを「３次元ＬｉＤＡＲ」と称する場合がある。Ｎが２以上の場合、１番目のレーザビームとＮ番目のレーザビームとのなす角度を、「垂直視野角」と称する。垂直視野角は、例えば２０°から６０°程度の範囲内に設定され得る。
【0085】
ＬｉＤＡＲセンサ１１０は、図９Ｂに示すように、レーザビームの出射方向（方位角）を変化させることができる。図９Ｂには、図９Ａに示す複数のレーザビームの出射方向が、ｗ軸に平行な回転軸の周りに回転している様子が示されている。レーザビームの出射方向（方位角）の範囲は３６０°であってもよいし、３６０°よりも小さい角度の範囲（例えば２１０°または２７０°等）であってもよい。レーザビームの出射方向の方位角の範囲を「水平視野角」と称する。水平視野角は、例えば９０°から３６０°程度の範囲内に設定され得る。ＬｉＤＡＲセンサ１１０は、レーザビームの出射方向をｗ軸に平行な回転軸の周りに回転させながら、異なる方位角の方向にパルス状のレーザ光（レーザパルス）を順次出射する。こうして、異なる仰角および異なる方位角に出射されるパルス状のレーザ光により、各反射点までの距離を計測することが可能になる。各反射点は、点群データに含まれる個々の点に相当する。レーザビームの方位角が回転軸の周りに一回転する間に反射点までの距離を測定する動作を１スキャンと称する。１スキャンによって得られるセンサデータは、図９Ａに示す特定の仰角に対応付けられたレイヤごとに測定されるデータを含む。従って、レイヤの数が増加するほど、同一環境に対する１スキャンによって得られる点群中の点の個数は増加する。ＬｉＤＡＲセンサ１１０は、例えば１秒あたり１回から２０回程度の頻度でスキャン動作を繰り返す。１スキャンの動作中に例えば１０万パルス以上のレーザ光が異なる方向に出射され得る。
【0086】
図１０は、ＬｉＤＡＲセンサ１１０の概略的な構成の例を示すブロック図である。図１０に示すように、ＬｉＤＡＲセンサ１１０は、複数のレーザユニット１１１と、電気モータ１１４と、制御回路１１５と、信号処理回路１１６と、メモリ１１７とを備える。各レーザユニット１１１は、レーザ光源１１２と、光検出器１１３とを備える。各レーザユニット１１１は、レンズおよびミラーなどの光学系を含み得るが、それらの図示は省略されている。モータ１１４は、例えば、各レーザ光源１１２から出射されるレーザビームの光路上に配置されたミラーを回転させることにより、各レーザ光源１１２から出射されるレーザビームの方向を変化させる。
【0087】
レーザ光源１１２は、レーザダイオードを含み、制御回路１１５からの指令に応答して所定の波長のパルス状のレーザビームを出射する。レーザビームの波長は、例えば近赤外線の波長域（およそ７００ｎｍから２．５μｍ）に含まれる波長であり得る。使用される波長は、光検出器１１３に使用される光電変換素子の材料に依存する。例えばシリコン（Ｓｉ）を光電変換素子の材料として用いる場合、９００ｎｍ前後の波長が主に使用され得る。インジウムガリウム砒素（ＩｎＧａＡｓ）を光電変換素子の材料として用いる場合、例えば１０００ｎｍ以上１６５０ｎｍ以下の波長が使用され得る。なお、レーザビームの波長は、近赤外線の波長域に限定されない。環境光の影響が問題にならない用途（例えば夜間用等）においては、可視域（およそ４００ｎｍから７００ｎｍ）に含まれる波長を使用してもよい。用途によっては紫外線の波長域を使用することも可能である。本明細書において、紫外線、可視光、および赤外線の波長域に含まれる放射全般を「光」と称する。
【0088】
光検出器１１３は、レーザ光源１１２から出射され、物体で反射または散乱されたレーザパルスを検出する装置である。光検出器１１３は、例えばアバランシェフォトダイオード（ＡＰＤ）などの光電変換素子を備える。光検出器１１３は、受光量に応じた電気信号を出力する。
【0089】
モータ１１４は、制御回路１１５からの指令に応答して各レーザ光源１１２から出射されるレーザビームの光路上に配置されたミラーを回転させる。これにより、レーザビームの出射方向を変化させるスキャン動作が実現される。
【0090】
制御回路１１５は、レーザ光源１１２によるレーザパルスの出射、光検出器１１３による反射パルスの検出、およびモータ１１４による回転動作を制御する。制御回路１１５は、例えばマイクロコントローラユニット（ＭＣＵ）などの、プロセッサを備える回路によって実現され得る。
【0091】
信号処理回路１１６は、光検出器１１３から出力された信号に基づく演算を行う回路である。信号処理回路１１６は、例えばＴｏＦ（Time of Flight）法によって各レーザ光源１１２から出射されたレーザパルスを反射した物体までの距離を計算する。ＴｏＦ法には、直接ＴｏＦ法と間接ＴｏＦ法とがある。直接ＴｏＦ法においては、レーザ光源１１２からレーザパルスが出射されてから光検出器１１３によって反射光が受光されるまでの時間を直接計測することにより、反射点までの距離を計算する。間接ＴｏＦ法においては、光検出器１１３に複数の露光期間が設定され、各露光期間で検出された光量の比に基づいて各反射点までの距離が計算される。直接ＴｏＦ法および間接ＴｏＦ法のいずれの方法も用いられ得る。信号処理回路１１６は、例えば、各反射点までの距離と、その反射点の方向とを示すセンサデータを生成して出力する。信号処理回路１１６は、さらに、各反射点までの距離と、その反射点の方向とに基づいて、センサ座標系における座標（ｕ，ｖ）または（ｕ，ｖ，ｗ）を計算し、センサデータに含めて出力してもよい。
【0092】
制御回路１１５および信号処理回路１１６は、図１０の例では２つの回路に分かれているが、１つの回路によって実現されていてもよい。
【0093】
メモリ１１７は、制御回路１１５および信号処理回路１１６によって生成されるデータを記憶する記憶媒体である。メモリ１１７は、例えば、各レーザユニット１１１から出射されるレーザパルスの出射タイミングと、出射方向と、反射光強度と、反射点までの距離と、センサ座標系における座標（ｕ，ｖ）または（ｕ，ｖ，ｗ）とを関連付けたデータを記憶する。そのようなデータが、レーザパルスが出射される度に生成され、メモリ１１７に記録される。制御回路１１５は、所定の周期（例えば、所定回数のパルス出射に要する時間、半スキャン周期、または１スキャン周期等）で当該データを出力する。出力されたデータは、トラクタ１００Ａの記憶装置１５０に記録される。
【0094】
なお、距離計測の方法はＴｏＦ法に限らず、ＦＭＣＷ（Frequency Modulated Continuous Wave）法などの他の方法を用いてもよい。ＦＭＣＷ法においては、周波数を線形的に変化させた光が出射され、出射光と反射光との干渉で生じるビートの周波数に基づいて距離が計算される。
【0095】
＜動作＞
次に、トラクタ１００Ａの動作を説明する。
次に、トラクタ１００Ａの動作を説明する。
【0096】
図１１は、トラクタ１００Ａが走行する環境の一例を模式的に示す図である。この例では、トラクタ１００Ａは、ぶどう園における複数の樹木列２０の間を走行しながら、作業機３００を用いて所定の作業（例えば、草刈り、防除、その他）を行う。ぶどう園では、上空が枝および葉で遮られ、ＧＮＳＳを用いた自動走行が難しい。また、ぶどう園では、樹木の葉または垣根の外形が季節によって大きく変化する。そのため、従来のＳＬＡＭ技術によって作成された点群地図を通期で使用することが難しい。
【0097】
そこで、本開示の実施形態では、ぶどう園などの果樹園に適した環境地図を用意し、その環境地図に基づいてトラクタ１００Ａの自動走行を行う。例えばぶどう園における樹木は、一度植えられると長期間植え替えられることがなく、幹部は葉部に比べて季節変化による外形の変化が少ない。幹部をＳＬＡＭのランドマークとすることで、通期で使用可能な環境地図を作成し、年間を通じて環境地図を作成し直すことなく自動走行作業を行うことができる。
【0098】
以下、トラクタ１００Ａの動作を具体的に説明する。ＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０から出力されるセンサデータを利用して、ぶどう園全体またはぶどう園の一区画における樹木列２０の幹部の分布を示す環境地図データをまず生成し、記憶装置１５０に記録する。環境地図データの生成は、自己位置推定モジュール１６２による自己位置推定と、地図データ生成モジュール１６４による局所地図データの生成とを繰り返すことによって実行される。地図データ生成モジュール１６４は、トラクタ１００Ａが移動している間にＬｉＤＡＲセンサ１１０から繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、トラクタ１００Ａの周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを生成して記憶装置１５０に記録する動作を繰り返す。地図データ生成モジュール１６４は、トラクタ１００Ａがぶどう園全体またはぶどう園の一区画を走行する間に生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成する。この環境地図データは、環境中の樹木列２０の幹部の分布を、他の物体と区別可能な形式で記録したデータである。このように、本実施形態では、樹木列２０の幹部がＳＬＡＭのランドマークとして使用される。
【0099】
複数の樹木列が配置されたエリアには１つ以上の支柱が設けられることがある。例えば、ぶどう園においては、一般に、垣根仕立てのための複数の支柱が樹木の近傍に設けられる。支柱は、樹木の幹部と同様、季節による外形の変化が少ないため、ランドマークとして適している。そこで、自己位置推定モジュール１６２は、センサデータに基づいて、トラクタ１００Ａの周囲の環境における支柱をさらに検出し、地図データ生成モジュール１６４は、幹部の分布に加えて支柱の分布も示す環境地図データを生成してもよい。
【0100】
環境地図データが生成されると、トラクタ１００Ａによる自動走行または自律走行が可能になる。ＥＣＵ１６０の自己位置推定モジュール１６２は、トラクタ１００Ａの走行中、ＬｉＤＡＲセンサ１１０から繰り返し出力されるセンサデータに基づいて、周囲の環境における樹木列２０の幹部を検出し、検出した樹木列２０の幹部と環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置を推定する。環境地図データに支柱の分布情報が含まれる場合は、自己位置推定モジュール１６２は、センサデータに基づいて検出した樹木列の幹部および支柱と、環境地図データとのマッチングを行うことにより、トラクタ１００Ａの位置を推定してもよい。駆動制御用のＥＣＵ１８０は、ＥＣＵ１６０によって推定されたトラクタ１００Ａの位置に応じてトラクタ１００Ａの移動を制御する制御装置である。例えば、ＥＣＵ１８０は、ＥＣＵ１７０によって決定された走行予定経路から逸脱している場合に、トラクタ１００Ａのステアリングを調整することにより、走行予定経路に近付ける制御を行う。このような操舵制御は、トラクタ１００Ａの位置だけでなく向きに基づいて行われてもよい。
【0101】
樹木列２０の幹部の分布を示す環境地図は、任意の時期に生成することができる。例えば、冬などの樹木の葉部が少ない季節に環境地図を生成することができる。その場合、夏などの樹木の葉部が多い季節に環境地図を生成する場合と比較して、幹部の分布をより正確に反映した環境地図を生成し易い。冬に環境地図が生成される場合、冬だけでなくそれ以外の季節においても、ＥＣＵ１６０は、その環境地図に基づいて自己位置推定を行ってもよい。その場合、ＥＣＵ１６０は、冬の間に生成された環境地図データと、ＬｉＤＡＲセンサ１１０から出力されたセンサデータから幹部以外の部分を除去したデータとのマッチングを行うことにより、自己位置推定を行う。
【0102】
ＥＣＵ１６０は、トラクタ１００Ａの走行中に繰り返し取得されたセンサデータに基づいて検出した樹木列の幹部の分布を示すデータを生成し、当該データを用いて環境地図データを更新してもよい。例えば、ＥＣＵ１６０は、新たに取得されたセンサデータから検出された幹部の情報を前回生成した環境地図データに加えることによって環境地図データを更新してもよい。あるいは、検出した樹木列の幹部の分布を示すデータを、環境地図データを更新する外部の装置に送信してもよい。その場合、ＥＣＵ１６０は、推定されたトラクタ１００Ａの位置を示す情報を当該幹部の分布を示すデータ（例えば局所地図データ）に含めて出力する。外部の装置は、取得したデータを用いて環境地図データを更新し、更新後の環境地図データをトラクタ１００Ａに送信することができる。そのような動作により、樹木の生長などに起因して、環境地図を作成した時点から走行環境が変化した場合であっても、その変化を反映した環境地図を新たに作り直すことができる。
【0103】
図１２は、トラクタ１００Ａの周囲の環境の例を模式的に示す斜視図である。トラクタ１００Ａは、ぶどう園内の隣り合う２つの樹木列の間を走行する。トラクタ１００Ａは、走行しながら、ＬｉＤＡＲセンサ１１０を用いて周囲の環境をレーザビームでスキャンする。これにより、環境内に存在する物体の距離分布を示すデータを取得する。距離分布を示すデータは、２次元または３次元の点群データに変換され、記録される。トラクタ１００Ａは、ぶどう園内を走行しながら繰り返し取得した点群データを統合することにより、ぶどう園全体、またはぶどう園の一区画における樹木列の幹部の分布を示す環境地図データを生成する。点群データの統合は、異なるタイミングで取得された複数の反射点の座標を、地球に固定されたワールド座標系における座標に変換し、繋ぎ合わせる処理を含む。本実施形態では、トラクタ１００ＡのＥＣＵ１６０が環境地図データを生成するが、トラクタ１００Ａの外部に設けられたコンピュータが環境地図データを生成してもよい。
【0104】
図１３Ａは、環境地図生成時、および自動走行時のトラクタ１００Ａの走行経路の一例を模式的に示す図である。トラクタ１００Ａは、例えば図１３Ａに矢印で示す経路３０に沿ってぶどう園内の複数の樹木列２０の間を走行する。図では、経路３０に含まれる線分が直線状に記載されているが、トラクタ１００Ａが実際に走行する経路は、蛇行部分を含んでいてもよい。ここで、複数の樹木列２０を、端から順に第１の樹木列２０Ａ、第２の樹木列２０Ｂ、第３の樹木列２０Ｃ、第４の樹木列２０Ｄ、・・・と順序付けする。図１３Ａの例では、トラクタ１００Ａは、第１の樹木列２０Ａと第２の樹木列２０Ｂとの間をまず走行し、その走行が完了すると、旋回して第２の樹木列２０Ｂと第３の樹木列２０Ｃとの間を逆向きに走行する。第２の樹木列２０Ｂと第３の樹木列２０Ｃとの間の走行が完了すると、さらに旋回して第３の樹木列２０Ｃと第４の樹木列２０Ｄとの間を走行する。以後、同様の動作を繰り返すことにより、最後の２つの樹木列の間の経路３０の終端まで走行する。なお、隣り合う樹木列の間の距離が短い場合は、図１３Ｂに示すように、１列おきに走行してもよい。この場合、最後の２つの樹木列の間の走行が完了した後、まだ走行していない樹木列間を１列おきに走行する動作が実行され得る。このような走行は、トラクタ１００ＡがＬｉＤＡＲセンサ１１０を利用して自己位置推定を行いながら、自動で行う。なお、ＧＮＳＳユニット１２０がＧＮＳＳ信号を受信できるタイミングにおいては、ＧＮＳＳ信号に基づいて測位を行ってもよい。例えば、図１３Ａおよび図１３Ｂに示す経路３０において方向転換するタイミングでは、ＧＮＳＳ信号を遮る葉が存在しないことから、ＧＮＳＳ信号に基づく測位が可能である。
【0105】
＜センサデータ取得の例＞
次に、図１４Ａから図１７Ｃを参照して、トラクタ１００Ａによるセンサデータの取得の例を説明する。センサデータは、環境地図が既に構築済みの場合は、環境地図とのマッチングを行うことによってトラクタ１００Ａの位置を推定するために利用される。一方、環境地図が構築されていない場合、あるいは環境地図の少なくとも一部を更新する場合には、センサデータに基づいて環境地図の構築または更新が行われる。環境地図の構築および更新は、トラクタ１００Ａの移動中に行われる必要はなく、トラクタ１００Ａが移動中に取得したセンサデータをトラクタ１００Ａの内部または外部のコンピュータによって処理することで実行され得る。
次に、図１４Ａから図１７Ｃを参照して、トラクタ１００Ａによるセンサデータの取得の例を説明する。センサデータは、環境地図が既に構築済みの場合は、環境地図とのマッチングを行うことによってトラクタ１００Ａの位置を推定するために利用される。一方、環境地図が構築されていない場合、あるいは環境地図の少なくとも一部を更新する場合には、センサデータに基づいて環境地図の構築または更新が行われる。環境地図の構築および更新は、トラクタ１００Ａの移動中に行われる必要はなく、トラクタ１００Ａが移動中に取得したセンサデータをトラクタ１００Ａの内部または外部のコンピュータによって処理することで実行され得る。
【0106】
図１４Ａは、トラクタ１００ＡのＬｉＤＡＲセンサ１１０から出射されたレーザビームで樹木２１および地面が照射される様子を模式的に示す図である。図１４Ｂは、ＬｉＤＡＲセンサ１１０から出射されたレーザビームで地面が照射される様子を模式的に示す図である。これらの図において、ＬｉＤＡＲセンサ１１０から負の仰角の方向に出射されたレーザビームは太い破線で表され、ＬｉＤＡＲセンサ１１０から正の仰角の方向に出射されたレーザビームは細い破線で表されている。ＬｉＤＡＲセンサ１１０から異なる仰角で出射される複数のレーザビームのうち、負の仰角の方向に出射されたレーザビームは、樹木２１に当たらない場合、地面で反射され得る。一方、正の仰角の方向に出射されたレーザビームは、樹木２１に当たらない場合、反射点を形成せず、樹木２１に当たった場合の反射点は、葉や枝の表面に位置することが多い。前述したように、葉や枝の表面に位置する反射点は、環境地図データとのマッチングに適さない。このため、本実施形態では、幹部での反射を利用して自己位置推定および環境地図の構築を行う。具体的には、ＬｉＤＡＲセンサ１１０から異なる仰角で出射される複数のレーザビームのうち、仰角が所定の範囲に含まれるレーザ光源から出射されたレーザパルスの反射点に基づいて幹部を検出してもよい。また、反射点の高さが幹部の平均的な高さよりも低い反射点に基づいて幹部を検出してもよい。これらの場合、幹部の検出後に、地面に位置する反射点をセンサデータの点群から選択的にカットしてもよい。
【0107】
図１５Ａおよび図１５Ｂは、トラクタ１００ＡのＬｉＤＡＲセンサ１１０による１スキャン動作で取得されるセンサデータの点群の例を模式的に示す平面図である。図１５Ａは、ある時点におけるトラクタ１００Ａと、ＬｉＤＡＲセンサ１１０から放射されたレーザパルスの反射点の分布の例を模式的に示している。この例では、地面は平坦な面であると仮定している。地面で反射されたレーザビームは、地面で円弧上に反射点を形成する。このような地面上の反射点は、仰角が負の方向に出射されたレーザビームによって形成される。図１５Ａにおいて、半径が異なる７本の円弧（７つのレイヤに相当）上に反射点が並んでいる。それぞれのレイヤは、異なる負の仰角で出射されたレーザビームが方位角方向に回転するスキャンによって形成される。このスキャンが行われているとき、水平方向または正の仰角の方向にレーザビームによるスキャンが行われていてもよい。なお、同一レイヤ上に並ぶ反射点は、同一のレーザ光源から同一の仰角で出射されたレーザビームが回転しながら物体表面にパルス状に入射して形成される。図１５Ｂは、図１５Ａの時点からわずかに時間が経過した時点におけるトラクタ１００Ａと、ＬｉＤＡＲセンサ１１０から放射されたレーザパルスの反射点の分布の例を模式的に示している。
【0108】
ＬｉＤＡＲセンサ１１０から放射されたレーザパルスの一部は、樹木の幹部２２の表面で反射される。地面、樹木の幹部２２、または他の物体で反射されたレーザパルスの一部は、反射点が測距可能距離（例えば５０ｍ、１００ｍ、または２００ｍ等）を超える遠方に位置する場合を除き、ＬｉＤＡＲセンサ１１０によって検出され、反射点までの距離が計測される。ＬｉＤＡＲセンサ１１０は、例えば、反射点ごとに、反射点までの距離と、反射点の方向と、反射光強度と、反射点を形成したレーザビームを出射したレーザ光源の識別番号とを関連付けたセンサデータを生成して出力する。このセンサデータには、計測時刻の情報も含まれ得る。計測時刻の情報は、例えば反射点ごと、または一定の時間内に計測された反射点のまとまりごとに記録され得る。ＥＣＵ１６０の自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたセンサデータを点群データに変換する。点群データは、ＬｉＤＡＲセンサ１１０に固定されたセンサ座標系で表現された各反射点の３次元座標（ｕ，ｖ，ｗ）の情報を含むデータである。なお、ＬｉＤＡＲセンサ１１０が各反射点の距離および方向のデータを点群データに変換した上で出力する場合には、自己位置推定モジュール１６２による点群データへの変換は省略される。
【0109】
環境地図の構築時において、トラクタ１００Ａが走行している間、地図データ生成モジュール１６４は、点群データから、幹部２２を他の部分と区別可能な形式で記録した局所地図データを生成する。トラクタ１００Ａの走行中、地図データ生成モジュール１６４は、それまでに生成した局所地図データに、新たに取得したセンサデータに基づく局所地図データを追加して更新する動作を繰り返す。これにより、最終的な環境地図データを生成することができる。なお、地図データ生成モジュール１６４は、トラクタ１００Ａの走行中には局所地図データを生成して記録する動作のみを行い、最終的な環境地図データの生成は、走行完了後に行ってもよい。その場合、最終的な環境地図データは、外部のコンピュータが生成してもよい。
【0110】
本実施形態では、ＬｉＤＡＲセンサ１１０がトラクタ１００Ａの車体の前部に取り付けられている。したがって、図１５Ａおよび図１５Ｂに示すように、レーザパルスは、トラクタ１００Ａの前方に向けて放射される。トラクタ１００Ａの後部にもＬｉＤＡＲセンサ１１０が取り付けられていてもよい。その場合には、トラクタ１００Ａの前方と後方の両方の点群データを一度に取得できるため、より点群密度の高い地図データを生成することができる。また、環境地図が生成された後の自動走行におけるマッチングによる自己位置推定の精度を向上させることができる。さらに、個々の幹部２２の表面における広い範囲から点群データが得られるため、その点群データから幹部２２の太さを高い精度で計測することもできる。このように、ＥＣＵ１６０は、センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、各幹部の太さを記憶媒体に記録する記録装置としても機能する。記憶媒体は、記憶装置１５０内の記憶媒体でもよいし、他の記憶媒体でもよい。幹部の太さは、各ランドマークの属性情報のひとつとして環境地図データに加えられてもよいし、他のベクタ形式の地図データに記録されてもよい。
【0111】
図１６は、複数のレイヤ（Ｌ１、・・・、Ｌｋ＋３）のレーザビームの１スキャンによって得られる幹部２２の点群の例を模式的に示す図である。図１７Ａから図１７Ｃは、それぞれ、図１６に示す複数のレイヤのうちの３つのレイヤ（Ｌｋ、Ｌｋ＋１、Ｌｋ＋２）のレーザビームのスキャンによって幹部２２の点群が得られる様子を模式的に示す図である。この図に示すように、スキャン毎に、計測可能な範囲に存在する幹部２２の分布を示す点群データを得ることができる。より詳細には、１スキャンによって個々の反射点までの距離を計測するとき、それぞれのレイヤにおいて、距離が相対的に長い反射点が連続する領域と、距離が相対的に短い反射点が局所的に表れる領域とが含まれ得る。
【0112】
図１８は、複数のレイヤ（Ｌｋ、Ｌｋ＋１、Ｌｋ＋２、・・・）のレーザビームによって１スキャンで計測される反射点までの距離と反射点の方位角との関係の例を示すグラフである。図１８に示される方位角の範囲は狭く、この範囲では２本の幹部からの反射が示されている。この例において、各レイヤの主たる反射点は地面に位置している。個々のレイヤに属する反射点のうち、相対的に距離が短い反射点は、幹部の表面に対応する曲線上に並んでいる。２つの幹部のうち、距離が相対的に短い位置にある幹部は、レイヤＬｋ、Ｌｋ＋１、Ｌｋ＋２、・・・のレーザビームで照射される。これに対し、距離が相対的に長い位置にある幹部は、より少ないレイヤＬｋ＋１、Ｌｋ＋２、・・・のレーザビームで照射される。このため、３次元空間中の幹部の表面における点群を水平面に平行な平面に投影した場合、距離が相対的に短い位置にある幹部の点群の密度が相対的に高い。図１８に示される計測結果に基づいて、幹部を検出することができる。実際には地面には起伏が存在したり、雑草が存在したりするため、距離の計測値は図示されるほど単純ではない。
【0113】
トラクタ１００Ａが移動しながら上記のスキャンを繰り返すことにより、トラクタ１００Ａの周囲に位置する各幹部から密度の高い点群のデータを取得することが可能である。したがって、移動するトラクタ１００Ａが取得する大量のセンサデータを利用することにより、幹部の分布を示す精度の高い環境地図の構築に必要な高密度の点群データを取得することができる。
【0114】
なお、反射点までの距離ではなく、反射光の強度と反射点の方位角との関係から幹部を検出することもできる。同一レイヤのレーザビームによる反射光の強度は、反射点までの距離が短いほど大きくなるからである。
【0115】
上述したように、幹部の分布を示す環境地図と比較されるべきセンサデータ（以下、「スキャンデータ」とも称する。）には、幹部の表面に位置する反射点以外の多数の反射点が含まれ得る。このため、取得されたスキャンデータから、幹部の表面に位置する可能性が高い反射点を選択することが有効である。また、スキャンデータは、１スキャンによって得られた反射点だけではなく、連続的に取得された複数のスキャンによって得られた反射点を統合して密度の高い点群データを生成してもよい。
【0116】
なお、図１７Ａから図１７Ｃでは省略されているが、例えば正の仰角で出射されるレーザビームは、幹部以外の葉部や枝部を照射して多数の反射点を形成し得る。本実施形態におけるＥＣＵ１６０は、スキャンデータに基づいて生成した点群データから、幹部２２の特徴を有する一部の点を抽出することにより、幹部２２の分布を示すデータを生成する。
【0117】
図１９は、ある位置におけるトラクタ１００Ａで取得されたスキャンデータから、幹部の表面に位置する反射点を抽出して作成した局所地図の例を示している。この例では、幹部の２次元分布を示す２次元の局所地図が作成されている。このような局所地図では、その位置におけるトラクタ１００Ａの周囲に位置する幹部の座標が、トラクタ１００Ａに固定されたｕｖ座標系で表現され得る。
【0118】
図２０Ａおよび図２０Ｂは、図１９の局所地図を、構築済みの環境地図とマッチングさせることにより、トラクタ１００Ａの位置を推定するプロセスを模式的に示す平面図である。局所地図における各座標はセンサ座標系で表現され、環境地図における各座標はワールド座標系で表現されている。マッチングは、センサ座標系からワールド座標系への座標変換を最適化することである。そのような座標変換の最適化により、ワールド座標系におけるセンサの位置座標および向きが決定される。図２０Ａおよび図２０Ｂにおいて、環境地図における幹部は黒塗りで表現されている。図２０Ａは、マッチングの途中の状態を示している。この状態では、局所地図上にある幹部と環境地図上にある対応する幹部との間には位置ずれ（誤差）が存在する。図２０Ｂは、マッチングが完了した状態を示している。この状態では、局所地図上にある幹部と環境地図上にある対応する幹部との間の位置ずれ（誤差）は最小化されている。
【0119】
このように幹部を抽出したスキャンデータと幹部の分布を示す環境地図を利用することにより、精度の高い位置推定が実現する。なお、上記の例では、２次元の環境地図に基づくマッチングが行われるが、３次元の環境地図に基づいてマッチングを行ってもよい。
【0120】
【0121】
図２１Ａは、本実施形態のように、トラクタ１００Ａの車両本体の前方下部にＬｉＤＡＲセンサ１１０が取り付けられた例を示している。図２１Ｂは、トラクタ１００Ａのキャビンの前方上部にＬｉＤＡＲセンサ１１０が取り付けられた例を示している。これらの図において、太い破線は樹木２１の幹部に照射されるレーザビームを示しており、細い破線は樹木の葉部またはその上空に向けて出射されるレーザビームを示している。
【0122】
図２１Ａの例において、ＬｉＤＡＲセンサ１１０は、樹木列の幹部の平均高さよりも低い位置に搭載されている。樹木列の幹部の平均高さは、トラクタ１００Ａの走行予定経路に存在する複数の樹木列に含まれる樹木の幹部の高さの平均値である。樹木の幹部の高さは、地面から、その樹木において最も低い枝を生ずる部分までの距離である。ＬｉＤＡＲセンサ１１０は、例えば地上から１０ｃｍ以上１５０ｃｍ以下、ある例では１５ｃｍ以上１００ｃｍ以下の高さに配置され得る。このような低い位置にＬｉＤＡＲセンサ１１０を配置することにより、葉や枝によって遮られることなく、より多くのレーザビームで幹部を照射することができる。このため、センサデータから幹部の位置を高い精度で決定することが可能になる。
【0123】
一方、図２１Ｂの例では、ＬｉＤＡＲセンサ１１０が高い位置（例えば地上から約２ｍの位置）に配置されていることから、レーザビームの多くは、幹部ではなく葉部を照射する。このため、幹部の点群データを取得する用途においては、図２１Ｂのような配置は適さない。しかし、このような配置は、樹木列の葉部の分布情報を効率的に取得できるため、葉部の分布情報を積極的に取得する用途においては有効である。例えば、樹木の生育状況を管理したり、樹木の葉部を避ける経路を決定したりする目的のために図２１Ｂのような配置が採用されてもよい。なお、図２１Ｂのような配置においても、ＬｉＤＡＲセンサ１１０を下向きに取り付けることにより、より多くのレーザビームで幹部を照射することができる。このため、幹部の分布データを取得する用途においても図２１Ｂに示すような配置が採用される場合もある。ＬｉＤＡＲセンサ１１０は、車体の前方下部およびキャビンの前方上部に限らず、他の位置に配置されていてもよい。例えば、ＬｉＤＡＲセンサ１１０は、キャビンの高さよりも低い位置に配置され得る。
【0124】
なお、本実施形態では１つのＬｉＤＡＲセンサ１１０が使用されるが、トラクタ１００Ａは複数のＬｉＤＡＲセンサを備えていてもよい。複数のＬｉＤＡＲセンサから出力されるセンサデータを組み合わせることにより、さらに効率的に幹部の分布データを取得することができる。複数のＬｉＤＡＲセンサは、例えばトラクタ１００Ａの左右に設けられていてもよいし、前部および後部に設けられていてもよい。複数のＬｉＤＡＲセンサを備えた移動体の実施形態の詳細については後述する。
【0125】
ＥＣＵ１６０における自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０における複数のレーザ光源のうち、仰角が所定の範囲に含まれるレーザ光源から出射されたレーザパルスの反射点に基づいて幹部を検出してもよい。例えば、負の仰角の方向に出射されるレーザパルスの反射点のみに基づいて幹部を検出してもよい。幹検出に利用されるレーザパルスの仰角の範囲をユーザが設定できるようにしてもよい。例えば、ユーザが操作端末２００を操作することによって幹検出に利用されるレーザパルスの仰角の範囲を設定できるようにしてもよい。ＬｉＤＡＲセンサ１１０から出射されるレーザパルスのうち、幹部を照射する可能性の高い特定の仰角の範囲に出射されるレーザパルスのみを利用することにより、幹部をより効率的に検出することができる。また、地面に起伏があり、センサ座標系におけるｕｖ面が水平面から大きく傾斜するときは、センサ座標系の傾斜角に応じて、幹部で反射されたレーザパルスを適切に選択するように、レーザビームの仰角範囲を適応的に選択して反射点を抽出してもよい。ＬｉＤＡＲセンサ１１０の傾斜角は、ＩＭＵ１２５からの信号を利用して求めることができる。
【0126】
＜環境地図構築後の自動走行動作の例＞
次に、環境地図が作成された後の自動走行動作の例を説明する。
次に、環境地図が作成された後の自動走行動作の例を説明する。
【0127】
図２２は、ＥＣＵ１６０における自己位置推定モジュール１６２の機能構成の例を示す図である。自己位置推定モジュール１６２は、記憶装置１５０に格納されたコンピュータプログラム（ソフトウェア）を実行することにより、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータと地図データとのマッチングを行い、トラクタ１００Ａの位置および向きを推定する。
【0128】
自己位置推定モジュール１６２は、スキャンデータ取得１６２ａ、地図データ取得１６２ｂ、ＩＭＵデータ取得１６２ｃ、スキャンデータフィルタリング１６２ｄ、マッチング１６２ｅ、および車両位置・向き決定１６２ｆの各処理を実行する。以下、これらの処理の詳細を説明する。
【0129】
（スキャンデータ取得１６２ａ）
自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータを取得する。ＬｉＤＡＲセンサ１１０は、例えば毎秒１回から２０回程度の頻度で、スキャンデータを出力する。このスキャンデータは、センサ座標系で表現された複数の点の座標と、タイムスタンプの情報を含み得る。なお、スキャンデータが各点までの距離および方向の情報を含み、座標情報を含まない場合は、自己位置推定モジュール１６２が距離および方向の情報から座標情報への変換を行う。
自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータを取得する。ＬｉＤＡＲセンサ１１０は、例えば毎秒１回から２０回程度の頻度で、スキャンデータを出力する。このスキャンデータは、センサ座標系で表現された複数の点の座標と、タイムスタンプの情報を含み得る。なお、スキャンデータが各点までの距離および方向の情報を含み、座標情報を含まない場合は、自己位置推定モジュール１６２が距離および方向の情報から座標情報への変換を行う。
【0130】
（地図データ取得１６２ｂ）
自己位置推定モジュール１６２は、記憶装置１５０に格納された環境地図データを取得する。環境地図データは、トラクタ１００Ａが走行する環境内に含まれる樹木列の幹部の分布を示す。環境地図データは、例えば以下の（１）～（３）のいずれかの形式のデータを含む。
（１）幹部と幹部以外の物体とが識別可能な形式で記録されたデータ
例えば、幹部であると判断された点に数値「１」が付与され、幹部以外の物体であると判断された点に数値「０」が付与されたデータが該当する。個々の幹部を識別するための幹ＩＤが環境地図データに含まれていてもよい。
（２）幹部に相対的に大きい重みが付与され、幹部以外の物体に相対的に小さい重みが付与されたデータ
例えば、幹部の表面上の点であると推定される確率が高い点ほど大きい数値が付与されるデータが該当する。
（３）検出された幹部の分布の情報を含み、幹部以外の物体の一部または全部の分布の情報を含まないデータ
自己位置推定モジュール１６２は、記憶装置１５０に格納された環境地図データを取得する。環境地図データは、トラクタ１００Ａが走行する環境内に含まれる樹木列の幹部の分布を示す。環境地図データは、例えば以下の（１）～（３）のいずれかの形式のデータを含む。
（１）幹部と幹部以外の物体とが識別可能な形式で記録されたデータ
例えば、幹部であると判断された点に数値「１」が付与され、幹部以外の物体であると判断された点に数値「０」が付与されたデータが該当する。個々の幹部を識別するための幹ＩＤが環境地図データに含まれていてもよい。
（２）幹部に相対的に大きい重みが付与され、幹部以外の物体に相対的に小さい重みが付与されたデータ
例えば、幹部の表面上の点であると推定される確率が高い点ほど大きい数値が付与されるデータが該当する。
（３）検出された幹部の分布の情報を含み、幹部以外の物体の一部または全部の分布の情報を含まないデータ
【0131】
例えば、環境地図を表す点群から、幹部の表面であると判断された点のみを残して他の点を削除したデータが該当する。幹部ではないと判断された点の全てが削除されるのではなく、一部の点が残されていてもよい。例えば、ぶどう園では一般に、垣根仕立てのための支柱が幹部の周囲に設けられるため、そのような支柱を表す点の情報が環境地図に含まれていてもよい。
【0132】
点群中のある点が幹部または支柱に該当するか否かは、例えば、その点およびその点の周囲の複数の点の分布が、幹部または支柱の表面形状を反映した分布（例えば下に凸の円弧状の分布等）であるか否かに基づいて判断され得る。あるいは、各回のスキャンで取得された曲線的に分布する点群のデータから、近傍の点に比べてＬｉＤＡＲセンサ１１０からの距離が局所的に近い点の集まりを抽出し、それらの点を幹部または支柱を表す点であると判断してもよい。各回のスキャンで取得された曲線的に分布する点群のデータを、ＬｉＤＡＲセンサ１１０からの距離に応じて複数のクラスに分類し、クラスごとに幹部または支柱に該当するか否かを判断してもよい。また、距離だけでなく反射強度の情報にも基づいて点群を分類してもよい。樹木の幹とその周囲の他の部分とでは、反射強度が明確に異なるため、反射強度の類似性と位置の類似性とに基づいて点群を分類することが有効である。例えば、反射強度が所定の範囲内にあり、かつ位置が近接する複数の点を幹部の表面を表す点の候補としてもよい。ＬｉＤＡＲセンサ１１０から複数の異なる波長のレーザビームを出射し、波長ごとの反射強度の比に基づいて点群を分類し、幹部を検出してもよい。
【0133】
幹部の検出には、機械学習が利用されてもよい。深層学習などのニューラルネットワークに基づく機械学習アルゴリズムを利用することにより、点群データから樹木の幹の表面に該当する点を高い精度で検出することができる。機械学習アルゴリズムが用いられる場合、点群データから幹部を検出するための学習済みモデルの作成（すなわち学習）が事前に行われる。
【0134】
樹木の幹部に限らず、それ以外の物体を認識してもよい。例えばパターン認識または機械学習によって点群データから地面、雑草、および樹木の葉部等を認識してそれらの点を除去することにより、樹木の幹部に該当する点を主に含む点群データを生成してもよい。幹部の検出に先立って、地面からの高さが所定の範囲（例えば、０．５ｍから１．５ｍ等）に含まれる点群のみを抽出する処理を行ってもよい。そのような特定の座標範囲に含まれる点群のみを幹部検出の対象とすることにより、検出に要する時間を短縮することができる。地面からの高さは、各点のＺ座標から地面のＺ座標を減じることによって計算される。地面のＺ座標は、例えば、数値標高モデル（Digital Elevation Model：ＤＥＭ）を参照して決定され得る。地面のＺ座標は、地面を示す点群から決定されてもよい。
【0135】
図２３は、環境地図データの形式の一例を模式的に示す図である。この例における環境地図データは、点の個数の情報と、点ごとのＩＤ、分類、幹ＩＤ、Ｘ座標、Ｙ座標、Ｚ座標、および反射強度の情報とを含む。点の個数をｎとすると、点ＩＤ、分類、幹ＩＤ、Ｘ座標、Ｙ座標、Ｚ座標、および反射強度のデータの組がｎ組分記録される。分類は、その点が何を表すかを示し、例えば樹木の幹、垣根の支柱、地面、雑草、樹木の葉などを示す識別子を含む。分類は、樹木の幹であるか否かを示す２値の数値、または、樹木の幹である確率を示す数値であってもよい。幹ＩＤは、その点が幹を表す場合に、その幹を識別するための番号である。幹ＩＤは環境地図データに含まれていなくてもよい。また、幹ＩＤに加えて、他の物体（例えば、葉、枝、地面、雑草等）の識別子が含まれていてもよい。Ｘ座標、Ｙ座標、およびＺ座標は、ワールド座標系におけるその点の座標である。反射強度は、その点からの反射光強度を表す。反射強度の情報は環境地図データに含まれていなくてもよい。環境地図データは、図２３に示す情報以外にも、データ形式を示すヘッダなどの情報を含み得る。トラクタ１００Ａが走行するぶどう園が広い場合には、ぶどう園の区画ごと（例えば、１００ｍ×１００ｍの区画ごと）に環境地図データが生成され得る。その場合には、環境地図データは、区画を識別するためのＩＤ情報も含み得る。なお、環境地図データは、各点の３次元座標を記録する代わりに、２次元座標を記録してもよい。例えば、水平面に平行な特定の高さの平面上の点群の座標を含むデータが環境地図データとして生成されてもよい。その場合、スキャンデータと環境地図データとのマッチングは、その２次元座標に関して行われる。
【0136】
（ＩＭＵデータ取得１６２ｃ）
自己位置推定モジュール１６２は、ＩＭＵ１２５から出力されたＩＭＵデータを取得する。ＩＭＵデータは、トラクタ１００Ａの加速度、速度、変位、姿勢、計測時刻（タイムスタンプ）などの情報を含み得る。ＩＭＵデータは、例えば毎秒数十回から数千回程度の頻度で出力される。この出力周期は、一般に、ＬｉＤＡＲセンサ１１０によるスキャンデータの出力周期よりも短い。
自己位置推定モジュール１６２は、ＩＭＵ１２５から出力されたＩＭＵデータを取得する。ＩＭＵデータは、トラクタ１００Ａの加速度、速度、変位、姿勢、計測時刻（タイムスタンプ）などの情報を含み得る。ＩＭＵデータは、例えば毎秒数十回から数千回程度の頻度で出力される。この出力周期は、一般に、ＬｉＤＡＲセンサ１１０によるスキャンデータの出力周期よりも短い。
【0137】
図２４は、ＩＭＵデータとスキャンデータの時間的な関係の一例を示す図である。この例では、ＩＭＵデータは１ミリ秒（ｍｓ）の周期で出力され、スキャンデータは１０ｍｓの周期で出力される。１スキャンに要する時間が例えば１００ｍｓであるとすると、図２４の例では、１０分の１スキャンごとにスキャンデータが出力される。スキャンデータには、計測された各点の距離および方向、または座標と、計測時刻（タイムスタンプ）の情報が含まれる。
【0138】
自己位置推定モジュール１６２は、取得したスキャンデータのタイムスタンプを参照して、対応する時間中に生成されたＩＭＵデータを取得する。
【0139】
（スキャンデータフィルタリング１６２ｄ）
自己位置推定モジュール１６２は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、マッチングに不要な部分を除去する。例えば、地面、雑草、樹木の葉部、および障害物などの、樹木の幹部または垣根の支柱に該当しないと判断される点を除去することができる。
自己位置推定モジュール１６２は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、マッチングに不要な部分を除去する。例えば、地面、雑草、樹木の葉部、および障害物などの、樹木の幹部または垣根の支柱に該当しないと判断される点を除去することができる。
【0140】
（マッチング１６２ｅ）
自己位置推定モジュール１６２は、フィルタされたスキャンデータと地図データとのマッチングを行う。マッチングは、例えばＮＤＴ（Normal Distribution Transform）またはＩＣＰ（Iterative Closest Point）などの任意のマッチングアルゴリズムを用いて実行され得る。マッチングにより、ＬｉＤＡＲセンサ１１０の位置および向きが決定される。
自己位置推定モジュール１６２は、フィルタされたスキャンデータと地図データとのマッチングを行う。マッチングは、例えばＮＤＴ（Normal Distribution Transform）またはＩＣＰ（Iterative Closest Point）などの任意のマッチングアルゴリズムを用いて実行され得る。マッチングにより、ＬｉＤＡＲセンサ１１０の位置および向きが決定される。
【0141】
（車両位置・向き決定１６２ｆ）
自己位置推定モジュール１６２は、マッチングの結果に基づき、トラクタ１００Ａの位置および向きを決定し、その位置および向きを示すデータを出力する。当該データは、駆動制御用のＥＣＵ１８０に送られ、駆動装置１４０の制御に利用される。
自己位置推定モジュール１６２は、マッチングの結果に基づき、トラクタ１００Ａの位置および向きを決定し、その位置および向きを示すデータを出力する。当該データは、駆動制御用のＥＣＵ１８０に送られ、駆動装置１４０の制御に利用される。
【0142】
次に、図２５を参照しながら、本実施形態における自己位置推定動作をより詳細に説明する。
【0143】
図２５は、自己位置推定モジュール１６２による自己位置推定動作を示すフローチャートである。自己位置推定モジュール１６２は、図２５に示すステップＳ１０１からＳ１０９の動作を実行することにより、トラクタ１００Ａの位置および向き、すなわちポーズを推定する。以下、各ステップの動作を説明する。図２５に示す動作は、例えばユーザによる操作端末２００を用いた操作によって開始され得る。
【0144】
（ステップＳ１０１）
自己位置推定モジュール１６２は、記憶装置１５０から環境地図データを読み込む。走行対象のぶどう園の区画ごとに異なる環境地図データが記録されている場合は、現在の地点に対応する環境地図データを読み込む。現在の地点に対応する環境地図データは、例えばユーザが操作端末２００を操作することによって指定され得る。あるいは、その地点でＧＮＳＳ信号を受信できる場合には、ＧＮＳＳユニット１２０が受信したＧＮＳＳ信号に基づいて現在の地点を特定し、対応する環境地図データを選択して読み込んでもよい。この例のように、トラクタ１００Ａの位置に対応する一部の環境地図データのみを読み込むことにより、処理を高速化することができる。なお、本ステップにおいて全ての環境地図データを一括して読み込んでもよい。
自己位置推定モジュール１６２は、記憶装置１５０から環境地図データを読み込む。走行対象のぶどう園の区画ごとに異なる環境地図データが記録されている場合は、現在の地点に対応する環境地図データを読み込む。現在の地点に対応する環境地図データは、例えばユーザが操作端末２００を操作することによって指定され得る。あるいは、その地点でＧＮＳＳ信号を受信できる場合には、ＧＮＳＳユニット１２０が受信したＧＮＳＳ信号に基づいて現在の地点を特定し、対応する環境地図データを選択して読み込んでもよい。この例のように、トラクタ１００Ａの位置に対応する一部の環境地図データのみを読み込むことにより、処理を高速化することができる。なお、本ステップにおいて全ての環境地図データを一括して読み込んでもよい。
【0145】
（ステップＳ１０２）
自己位置推定モジュール１６２は、位置推定の開始地点を設定する。位置推定の開始地点は、その時点でのトラクタ１００Ａの現在位置であり、例えばユーザが操作端末２００に表示された地図から特定の地点を指定することによって設定され得る。あるいは、その地点でＧＮＳＳ信号を受信できる場合には、ＧＮＳＳユニット１２０が受信したＧＮＳＳ信号に基づいて開始地点を設定してもよい。
自己位置推定モジュール１６２は、位置推定の開始地点を設定する。位置推定の開始地点は、その時点でのトラクタ１００Ａの現在位置であり、例えばユーザが操作端末２００に表示された地図から特定の地点を指定することによって設定され得る。あるいは、その地点でＧＮＳＳ信号を受信できる場合には、ＧＮＳＳユニット１２０が受信したＧＮＳＳ信号に基づいて開始地点を設定してもよい。
【0146】
（ステップＳ１０３）
開始位置が設定されると、ＬｉＤＡＲセンサ１１０の動作が開始される。自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータを読み込む。ＬｉＤＡＲセンサ１１０は、所定の周期（例えば５ミリ秒以上１秒以下）でスキャンデータを出力する。スキャンデータには、レイヤごとに、例えば数度から３６０度の範囲内の点群データが含まれる。自己位置推定モジュール１６２は、例えばＬｉＤＡＲセンサ１１０からスキャンデータが出力される度に、そのスキャンデータを読み込む。あるいは、自己位置推定モジュール１６２は、スキャンデータが所定回数分出力される度に、それらのスキャンデータをまとめて読み込んでもよい。
開始位置が設定されると、ＬｉＤＡＲセンサ１１０の動作が開始される。自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０から出力されたスキャンデータを読み込む。ＬｉＤＡＲセンサ１１０は、所定の周期（例えば５ミリ秒以上１秒以下）でスキャンデータを出力する。スキャンデータには、レイヤごとに、例えば数度から３６０度の範囲内の点群データが含まれる。自己位置推定モジュール１６２は、例えばＬｉＤＡＲセンサ１１０からスキャンデータが出力される度に、そのスキャンデータを読み込む。あるいは、自己位置推定モジュール１６２は、スキャンデータが所定回数分出力される度に、それらのスキャンデータをまとめて読み込んでもよい。
【0147】
（ステップＳ１０４）
自己位置推定モジュール１６２は、スキャンデータに含まれるタイムスタンプを参照して、そのスキャンデータに対応するＩＭＵデータを読み込む。
自己位置推定モジュール１６２は、スキャンデータに含まれるタイムスタンプを参照して、そのスキャンデータに対応するＩＭＵデータを読み込む。
【0148】
（ステップＳ１０５）
自己位置推定モジュール１６２は、読み込んだＩＭＵデータに基づき、マッチングの初期位置を設定する。マッチングの初期位置は、ＩＭＵデータが示す現時点でのトラクタ１００Ａの推定位置である。この初期位置からマッチングを開始することにより、収束までの時間を短くすることができる。ＩＭＵデータを用いる代わりに、例えば、過去２スキャン分の位置および向きの推定値の差分に基づいて、線形補間によってマッチングの初期値を決定してもよい。
自己位置推定モジュール１６２は、読み込んだＩＭＵデータに基づき、マッチングの初期位置を設定する。マッチングの初期位置は、ＩＭＵデータが示す現時点でのトラクタ１００Ａの推定位置である。この初期位置からマッチングを開始することにより、収束までの時間を短くすることができる。ＩＭＵデータを用いる代わりに、例えば、過去２スキャン分の位置および向きの推定値の差分に基づいて、線形補間によってマッチングの初期値を決定してもよい。
【0149】
（ステップＳ１０６）
自己位置推定モジュール１６２は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、前述の方法により、幹部を検出して、幹部以外の不要な部分の少なくとも一部を点群から除去する。
自己位置推定モジュール１６２は、取得したスキャンデータをフィルタリングし、マッチングに用いる点の個数を削減する。さらに、前述の方法により、幹部を検出して、幹部以外の不要な部分の少なくとも一部を点群から除去する。
【0150】
ここで、図２６を参照して、スキャンデータのフィルタリングの動作の例を説明する。図２６は、スキャンデータに含まれる点群の一部を模式的に示している。図２６において、白丸が点群に含まれる点である。自己位置推定モジュール１６２は、センサ座標系における３次元空間を、一定の大きさの複数のボクセルに分割する。そして、個々のボクセルに複数の点が含まれる場合、それらの点を、重心に位置する１つの点に置き換える。図２６において、置き換えられた重心の点が黒丸で表現されている。このようなダウンサンプリングを行うことにより、スキャンデータの点の個数を削減して、マッチングを高速化することができる。本実施形態では、このようなダウンサンプリングが行われた後、幹部の検出が行われるが、ダウンサンプリングの前に幹部の検出を行ってもよい。点群のデータサイズが問題にならない場合には、ダウンサンプリング処理を省略してもよい。
【0151】
（ステップＳ１０７）
自己位置推定モジュール１６２は、フィルタリングされたスキャンデータと、環境地図データとのマッチングを行い、ＬｉＤＡＲセンサ１１０のポーズを推定する。具体的には、ＮＤＴ法またはＩＣＰ法などの方法で、センサ座標系からワールド座標系への座標変換を決定することにより、ＬｉＤＡＲセンサ１１０のポーズを決定する。
自己位置推定モジュール１６２は、フィルタリングされたスキャンデータと、環境地図データとのマッチングを行い、ＬｉＤＡＲセンサ１１０のポーズを推定する。具体的には、ＮＤＴ法またはＩＣＰ法などの方法で、センサ座標系からワールド座標系への座標変換を決定することにより、ＬｉＤＡＲセンサ１１０のポーズを決定する。
【0152】
（ステップＳ１０８）
自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０のポーズに基づき、トラクタ１００Ａのポーズを計算し、結果を出力する。トラクタ１００Ａのポーズは、ワールド座標系で表現されたトラクタ１００Ａの代表点（原点）の座標（ｘ，ｙ，ｚ）および姿勢（θＲ，θＰ，θＹ）のデータであり得る。なお、環境地図およびスキャンデータが２次元空間（平面）における点群を表し、その２次元空間においてマッチングが行われる場合には、出力されるポーズデータは、２次元座標（ｘ，ｙ）および向き（θ）の値を含み得る。位置のみをマッチングで推定し、姿勢についてはＩＭＵデータが示す姿勢の情報をそのまま利用してもよい。トラクタ１００Ａの座標系がセンサ座標系と一致している場合は、ステップＳ１０８は省略され得る。
自己位置推定モジュール１６２は、ＬｉＤＡＲセンサ１１０のポーズに基づき、トラクタ１００Ａのポーズを計算し、結果を出力する。トラクタ１００Ａのポーズは、ワールド座標系で表現されたトラクタ１００Ａの代表点（原点）の座標（ｘ，ｙ，ｚ）および姿勢（θＲ，θＰ，θＹ）のデータであり得る。なお、環境地図およびスキャンデータが２次元空間（平面）における点群を表し、その２次元空間においてマッチングが行われる場合には、出力されるポーズデータは、２次元座標（ｘ，ｙ）および向き（θ）の値を含み得る。位置のみをマッチングで推定し、姿勢についてはＩＭＵデータが示す姿勢の情報をそのまま利用してもよい。トラクタ１００Ａの座標系がセンサ座標系と一致している場合は、ステップＳ１０８は省略され得る。
【0153】
（ステップＳ１０９）
自己位置推定モジュール１６２は、動作終了の指令が出されているか否かを判断する。動作終了の指令は、例えばユーザが操作端末２００を用いて自動走行モードの停止を指示したり、トラクタ１００Ａが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップＳ１０３に戻り、次のスキャンデータについて同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
自己位置推定モジュール１６２は、動作終了の指令が出されているか否かを判断する。動作終了の指令は、例えばユーザが操作端末２００を用いて自動走行モードの停止を指示したり、トラクタ１００Ａが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップＳ１０３に戻り、次のスキャンデータについて同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
【0154】
次に、駆動制御用のＥＣＵ１８０の動作の例を説明する。
【0155】
図２７は、トラクタ１００Ａのポーズが推定された後に実行されるＥＣＵ１８０の動作の例を示すフローチャートである。図２５に示すステップＳ１０８においてトラクタ１００Ａのポーズが出力されると、ＥＣＵ１８０は、そのポーズのデータを取得する（ステップＳ１２１）。次に、ＥＣＵ１８０は、当該ポーズデータが示すトラクタ１００Ａの位置と、ＥＣＵ１７０によって予め決定された走行予定経路との偏差を算出する（ステップＳ１２２）。走行予定経路は、例えばユーザが操作端末２００を操作することによって設定され得る。偏差は、その時点におけるトラクタ１００Ａの推定された位置と、走行予定経路との距離を表す。ＥＣＵ１８０は、算出した位置の偏差が予め設定された閾値を超えるか否かを判断する（ステップＳ１２３）。偏差が閾値を超える場合、ＥＣＵ１８０は、偏差が小さくなるように、駆動装置１４０の駆動値（例えば操舵角）を変更する。操舵角に加えて速度を変更してもよい。ステップＳ１２３において偏差が閾値を超えない場合、ステップＳ１２４は省略される。続くステップＳ１２５において、ＥＣＵ１８０は、動作終了の指令を受けたか否かを判断する。動作終了の指令は、前述のように、例えばユーザが操作端末２００を用いて自動走行モードの停止を指示したり、トラクタ１００Ａが目的地に到達したりした場合に出される。動作終了の指令が出されていない場合、ステップＳ１２１に戻り、次のスキャンデータに基づいて推定されたトラクタ１００Ａのポーズに基づいて、同様の動作を実行する。動作終了の指令が出されている場合、処理を終了する。
【0156】
図２７の例では、ＥＣＵ１８０は、推定された位置と走行予定経路との偏差のみに基づいて駆動装置１４０を制御するが、方位の偏差もさらに考慮して制御してもよい。例えば、ＥＣＵ１８０は、取得したポーズデータが示すトラクタ１００Ａの推定された向きと、走行予定経路の方向との角度差である方位偏差が、予め設定された閾値を超える場合に、その偏差に応じて駆動装置１４０の駆動値（例えば操舵角）を変更してもよい。
【0157】
【0158】
図２８Ａは、走行予定経路Ｐに沿って走行するトラクタ１００Ａの例を示す図である。図２８Ｂは、走行予定経路Ｐから右にシフトした位置にあるトラクタ１００Ａの例を示す図である。図２８Ｃは、走行予定経路Ｐから左にシフトした位置にあるトラクタ１００Ａの例を示す図である。図２８Ｄは、走行予定経路Ｐに対して傾斜した方向を向いているトラクタ１００Ａの例を示す図である。
【0159】
図２８Ａに示すように、トラクタ１００Ａの位置および向きが走行予定経路Ｐから外れていない場合には、ＥＣＵ１８０は、トラクタ１００Ａの操舵角および速度を変更せずに維持する。
【0160】
図２８Ｂに示すように、トラクタ１００Ａの位置が走行予定経路Ｐから右側にシフトしている場合には、ＥＣＵ１８０は、トラクタ１００Ａの走行方向が左寄りになるように、駆動装置１４０に含まれるステアリングモータの回転角を変更して操舵角を変更する。このとき、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δｘの大きさに応じて調整され得る。
【0161】
図２８Ｃに示すように、トラクタ１００Ａの位置が走行予定経路Ｐから左側にシフトしている場合には、ＥＣＵ１８０は、トラクタ１００Ａの走行方向が右寄りになるように、ステアリングモータの回転角を変更して操舵角を変更する。この場合も、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δｘの大きさに応じて調整され得る。
【0162】
図２８Ｄに示すように、トラクタ１００Ａの位置は走行予定経路Ｐから大きく外れていないが、向きが走行予定経路Ｐの方向とは異なる場合は、ＥＣＵ１８０は、方位偏差Δθが小さくなるように操舵角を変更する。この場合も、操舵角に加えて速度も併せて変更してもよい。操舵角の変化量は、例えば位置偏差Δｘおよび方位偏差Δθの大きさに応じて調整され得る。例えば、位置偏差Δｘの絶対値が小さいほど方位偏差Δθに応じた操舵角の変化量を大きくしてもよい。位置偏差Δｘの絶対値が大きい場合には、走行予定経路Ｐに戻るために操舵角を大きく変化させることになるため、必然的に方位偏差Δθの絶対値が大きくなる。逆に、位置偏差Δｘの絶対値が小さい場合には、方位偏差Δθをゼロに近づけることが必要であるため、操舵角の変化量を決定する際の方位偏差Δθの重みを相対的に大きくすることが妥当である。
【0163】
トラクタ１００Ａの操舵制御および速度制御には、ＰＩＤ制御またはＭＰＣ制御（モデル予測制御）などの制御技術が適用され得る。これらの制御技術を適用することにより、トラクタ１００Ａを走行予定経路Ｐに近付ける制御を滑らかにすることができる。
【0164】
なお、走行中に１つ以上の障害物センサ１３０によって障害物が検出された場合には、ＥＣＵ１８０は、障害物を回避するように駆動装置１４０を制御する。障害物を回避できない場合には、ＥＣＵ１８０は、トラクタ１００Ａを停止させる。なお、ＥＣＵ１８０は、障害物が検出された場合に、障害物を回避できるか否かに関わらず、トラクタ１００Ａを停止させてもよい。
【0165】
＜走行予定経路の決定方法の例＞
続いて、ＥＣＵ１７０によるトラクタ１００Ａの走行予定経路の決定方法の例を説明する。
続いて、ＥＣＵ１７０によるトラクタ１００Ａの走行予定経路の決定方法の例を説明する。
【0166】
環境地図が作成された後、ＥＣＵ１７０は、トラクタ１００Ａの走行予定経路を決定する。走行予定経路は、ＥＣＵ１７０が環境地図に基づいて自動で決定してもよいし、ユーザが操作端末２００を操作して設定してもよい。
【0167】
図２９Ａは、環境地図に基づいて生成される、樹木列の幹分布を示す２次元データの例を示す図である。ＥＣＵ１６０における地図データ生成モジュール１６４は、マッチング用の環境地図とは別に、図２９Ａに示すような、幹部２２の分布を大まかに示す２次元データを生成してもよい。この２次元データは、個々の樹木の幹部２２が一定の大きさの円で表現されている。このような幹分布を示す２次元データを「幹分布データ」と称することがある。幹分布データは、例えば個々の幹の中心の位置座標（ｘ，ｙ）の情報を含む。幹分布データは、位置座標に加えて、個々の幹の識別番号（幹ＩＤ）および幹の太さなどの情報を含んでいてもよい。そのような情報を含めることにより、幹の生育状況を管理することもできる。樹木の幹または葉などの生育状況は、例えば営農管理システム内のコンピュータによって管理され得る。この例のように、樹木の生育状況は、環境地図とは異なるデータで管理され得る。ユーザが操作端末２００などのヒューマンマシンインタフェース（ＨＭＩ）を用いて、樹間距離または幹部の位置座標などの、複数の樹木列の配置関係を示す情報を入力できるようにしてもよい。入力された情報は、記憶装置１５０または他の記憶媒体に記録される。
【0168】
図２９Ｂは、幹分布データに基づく走行予定経路３０の決定方法の一例を説明するための図である。この例におけるＥＣＵ１７０は、各樹木列の幹部２２を繋いだ曲線を決定し、隣り合う２つの樹木列の中央部を通る曲線または折れ線を走行予定経路３０として決定する。隣り合う２つの樹木の中央部をトラクタ１００Ａの中心が必ず位置する必要はなく、走行予定経路３０は、隣り合う２つの樹木間の中央部からシフトしていてもよい。駆動制御用のＥＣＵ１８０は、決定された走行予定経路３０を基準に、トラクタ１００Ａの操舵制御を行う。
【0169】
図３０は、経路設定用の幹分布データの他の例を示す図である。この例では、トラクタ１００Ａの走行環境が複数のセルに分割されて管理される。各セルは、例えば幹部２２の直径の平均値に近い辺の長さをもつ正方形であり得る。セルは、以下の（１）～（４）の４種類に分類される。
（１）幹部２２が含まれるセル
（２）幹部２２が含まれるセルに隣接するセル
（３）隣り合う２つの樹木列の間に位置し、（１）および（２）に該当しないセル
（４）それ以外のセル
（１）幹部２２が含まれるセル
（２）幹部２２が含まれるセルに隣接するセル
（３）隣り合う２つの樹木列の間に位置し、（１）および（２）に該当しないセル
（４）それ以外のセル
【0170】
図３０において、（１）～（４）のセルが、異なる濃さで表現されている。幹部２２が含まれるセルに隣接するセル（２）の範囲は、ＬｉＤＡＲセンサ１１０によって取得された樹木列の葉部を示す点群データに基づいて決定されてもよい。その場合、ＥＣＵ１６０の地図データ生成モジュール１６４は、マッチング用の環境地図データとは別に、樹木列の幹部および葉部のそれぞれの２次元分布を示す格子状データを生成する。この例におけるＥＣＵ１７０は、図３０に示すような格子状データに基づいてトラクタ１００Ａの経路を決定する。例えば、ＥＣＵ１７０は、隣り合う２つの樹木列の間に位置する上記（３）のセルを走行可能領域として設定する。ＥＣＵ１８０は、走行可能領域の中央部付近を通るようにトラクタ１００Ａの走行を制御する。
【0171】
＜地図データ生成時の操舵制御＞
前述のように、本実施形態におけるトラクタ１００Ａは、自動走行によって環境地図を作成するとき、自己位置推定と操舵制御とを行いながら、樹木列の幹部の分布を示す局所地図データを収集する。このとき、ＥＣＵ１６０は、トラクタ１００Ａが移動している間にＬｉＤＡＲセンサ１１０から繰り返し出力されるセンサデータに基づいて、トラクタ１００Ａの周囲の環境における樹木列の葉部をさらに検出し、検出した樹木列の葉部の分布を示すデータをさらに生成してもよい。その場合、ＥＣＵ１８０は、検出された樹木列の幹部および葉部のそれぞれの分布に基づいて、トラクタ１００Ａの操舵制御を行うことができる。ＥＣＵ１８０は、例えば、検出された樹木列のうちの隣り合う２つの樹木列の間を通り、かつ樹木列の葉部との接触が低減される経路に沿ってトラクタ１００Ａを移動させるようにトラクタ１００Ａの操舵制御を行う。
前述のように、本実施形態におけるトラクタ１００Ａは、自動走行によって環境地図を作成するとき、自己位置推定と操舵制御とを行いながら、樹木列の幹部の分布を示す局所地図データを収集する。このとき、ＥＣＵ１６０は、トラクタ１００Ａが移動している間にＬｉＤＡＲセンサ１１０から繰り返し出力されるセンサデータに基づいて、トラクタ１００Ａの周囲の環境における樹木列の葉部をさらに検出し、検出した樹木列の葉部の分布を示すデータをさらに生成してもよい。その場合、ＥＣＵ１８０は、検出された樹木列の幹部および葉部のそれぞれの分布に基づいて、トラクタ１００Ａの操舵制御を行うことができる。ＥＣＵ１８０は、例えば、検出された樹木列のうちの隣り合う２つの樹木列の間を通り、かつ樹木列の葉部との接触が低減される経路に沿ってトラクタ１００Ａを移動させるようにトラクタ１００Ａの操舵制御を行う。
【0172】
この動作において、ＥＣＵ１７０は、ＬｉＤＡＲセンサ１１０からのセンサデータに基づいて検出された樹木列の幹部の分布から、隣り合う２つの樹木列の幹部の間を通る経路を決定する。例えば、隣り合う２つの樹木列の幹部の対の位置の中央部を通る経路を決定する。ＥＣＵ１８０は、決定された経路に沿ってトラクタ１００Ａを移動させるように、トラクタ１００Ａの操舵制御を行う。これにより、局所地図データを収集しながら、自動走行を行うことができる。
【0173】
トラクタ１００Ａが走行する環境において、樹木列は必ずしも直線的に配置されているとは限らない。樹木列が曲線的に配置される場合もある。そのような場合、地図データを収集しながら自動走行するときに、左右の片側にしか樹木列が検出されない場合がある。そのような場合には、左右の樹木列の対を特定できないため、上記の方法では自動走行を継続することが難しい。
【0174】
そこで、樹木列が曲線的に配置されている場合には、以下の制御が行われ得る。樹木列が曲線的に配置される場合、隣り合う２つの樹木列の一方について、２つの樹木列の他方よりも少ない数の幹部が検出される。そのような場合には、ＥＣＵ１７０は、検出された幹部の分布に基づいて、当該２つの樹木列の一方における隠れた幹部の分布を推定し、推定した分布に基づいて、トラクタ１００Ａの経路を決定する。ＥＣＵ１８０は、決定された経路に沿ってトラクタ１００Ａが走行するようにトラクタ１００Ａの操舵制御を行う。
【0175】
図３１Ａは、樹木列が曲線的に配置されている環境の一例を模式的に示す図である。図中の円柱は樹木の幹部を表し、太い破線は樹木列の概略的な配列を表し、点線はＬｉＤＡＲセンサ１１０から出射される複数のレイヤのレーザビームによるスキャンを模式的に表している。図３１Ｂは、図３１Ａに示す環境において１回のスキャンで観測され得る点群を模式的に示す図である。この例においては、隣り合う２つの樹木列のうち、右側の樹木列については３本の幹部２２Ｒ１、２２Ｒ２、２２Ｒ３が検出され、左側の樹木列については１本の幹部２２Ｌ１のみが検出されている。左側の樹木列については、前方の樹木の幹部２２Ｌ１によってレーザビームが遮られ、後方の樹木の幹部を検出することができない。
【0176】
このような場合、ＥＣＵ１７０は、相対的に多くの樹木が検出される右側の樹木列の配列から左側の隠れた樹木の幹部の位置を推定し、推定した幹部の位置に基づいて走行経路を決定する。例えば、スキャンデータに基づいて位置が特定される前方の隣り合う２つの幹部２２Ｒ１および２２Ｌ１の相対的な位置関係を、後方の隣り合う２つの幹部にも適用することにより、隠れた幹部の位置を推定する。
【0177】
図３１Ｃは、隠れた幹部の位置を推定することによって走行経路を決定する動作を説明するための図である。この例では、スキャンデータに基づいて取得された樹木列の幹部の２次元分布を示す幹分布データに基づいて、隠れた幹部の位置が推定される。図３１Ｃには、スキャンデータに基づいて検出された樹木の幹部の位置が実線の円で示され、まだ検出されていない樹木の幹部の位置が破線の円で示されている。この例において、ＥＣＵ１７０は、まず幹部２２Ｒ１の位置から幹部２２Ｌ１の位置までのベクトルＶ１を決定する。次に、後方の幹部２２Ｒ２の位置を起点として、その位置からベクトルＶ１の分だけ変位した位置を、隠れた幹部の推定位置として決定する。図３１Ｃにおいて、推定された隠れた幹部の位置が×印で示されている。ＥＣＵ１７０は、例えば、当該位置と幹部２２Ｒ２の位置との中点Ｃ１を通る経路を、トラクタ１００Ａの走行経路として決定する。
【0178】
このような方法で決定された経路に沿ってトラクタ１００Ａが走行すると、やがて幹部２２Ｌ１によって隠れていた後方の幹部２２Ｌ２がスキャンデータから検出される。さらに後方の幹部２２Ｌ３はまだ検出されていないとする。幹部２２Ｌ２の位置が推定位置（×印）とは異なる場合、ＥＣＵ１７０は、幹部２２Ｌ２と対を成す右側の幹部（図３１Ｃの例では幹部２２Ｒ３）を特定し、その幹部２２Ｒ３から幹部２２Ｌ２までのベクトルＶ２を決定する。次に、さらに後方の幹部２２Ｒ４の位置を起点として、その位置からベクトルＶ２の分だけ変位した位置を、隠れた幹部２２Ｌ３の位置として決定する。以上のような動作を繰り返すことにより、隠れた幹部の位置を推定して適切な経路を設定し、その経路に沿って自動操舵を行うことができる。
【0179】
図３１Ｄは、樹木列が曲線的に配置されている場合における幹部の２次元分布を示す格子状データの一例を示す図である。ＥＣＵ１６０は、環境地図を構築するためのデータを収集する過程で、図３１Ｄに示すような格子状データを生成してもよい。その場合、ＥＣＵ１７０は、このような格子状データに基づいて経路設定を行ってもよい。この例において、隠れた幹の位置は、セルの大きさの単位で推定され得る。
【0180】
以上のような動作により、環境地図を作成しながら、比較的なめらかな操舵制御を行うことができる。
【0181】
＜障害物回避動作＞
次に、本実施形態における障害物回避動作を説明する。
次に、本実施形態における障害物回避動作を説明する。
【0182】
図３２は、トラクタ１００Ａの走行環境に障害物４０および人５０が存在する状況の例を模式的に示す図である。トラクタ１００Ａは、１つ以上の障害物センサ１３０を備えている。走行中、障害物センサ１３０によって障害物４０（例えばカゴ）、または人５０を検出した場合、ＥＣＵ１８０は、障害物４０または人５０を回避するように操舵制御を行う。回避しても障害物４０、人５０、枝部、または幹部２２等との衝突を避けられない場合には、ＥＣＵ１８０は、トラクタ１００Ａを停止させる。なお、ＥＣＵ１８０は、障害物が検出された場合に、障害物を回避できる場合であってもトラクタ１００Ａを停止させてもよい。障害物センサ１３０によらず、ＬｉＤＡＲセンサ１１０によって同様の検出を行ってもよい。
【0183】
図３２に示す例では、人５０が樹木の幹部２２の近くに存在する。樹木の葉部も含めてマッチングが行われる場合には、葉部に人５０またはその他の障害物が隠れてしまい、葉部か他の物体かの判別が難しいことがある。特に、人５０が幹部２２の近くの葉の中に立っている場合、判別が難しい。本実施形態では、幹部２２の分布を示す環境地図を用いてマッチングが行われるので、幹部と他の物体との判別を容易にすることができる。スキャンデータと環境地図とのマッチングによって検出された幹部以外の物体を、人または障害物として容易に検出することができる。このように、ＬｉＤＡＲセンサ１１０を障害物センサとして利用することもできる。トラクタ１００Ａが複数のＬｉＤＡＲセンサ１１０を、トラクタ１００Ａの異なる箇所に備えている場合、障害物センサ１３０を省略してもよい。
【0184】
＜固有の幹間隔の組み合わせに基づく自己位置推定＞
以上の実施形態では、樹木の幹部の間隔がおおよそ一定であるものとしているが、現実には、樹木の幹部の間隔は場所によって異なり得る。そこで、ＥＣＵ１６０は、自己位置推定を行うとき、センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、当該固有の幹間隔の組み合わせを有する複数の樹木の幹部と、環境地図データから抽出した当該固有の幹間隔の組み合わせを有する複数の樹木の幹部とのマッチングにより、移動体の位置を推定してもよい。以下、図３３Ａおよび図３３Ｂを参照して、この動作を説明する。
以上の実施形態では、樹木の幹部の間隔がおおよそ一定であるものとしているが、現実には、樹木の幹部の間隔は場所によって異なり得る。そこで、ＥＣＵ１６０は、自己位置推定を行うとき、センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、当該固有の幹間隔の組み合わせを有する複数の樹木の幹部と、環境地図データから抽出した当該固有の幹間隔の組み合わせを有する複数の樹木の幹部とのマッチングにより、移動体の位置を推定してもよい。以下、図３３Ａおよび図３３Ｂを参照して、この動作を説明する。
【0185】
図３３Ａは、トラクタ１００Ａによって検出される樹木列の幹部２２の配置の例を示す図である。図３３Ｂは、環境地図データが示す幹分布の例を示す図である。この例において、点線枠で囲まれた３つの幹部は、１．２ｍと１．５ｍという固有の幹間隔の組み合わせを有する。このような場合、ＥＣＵ１６０は、自己位置推定を行うとき、取得したスキャンデータから検出された幹部２２のそれぞれの間隔を計算し、固有の幹間隔の組み合わせを有する部分を決定する。そして、図３３Ｂに示す環境地図から、決定した幹間隔の組み合わせを有する部分を検索して特定する。ＥＣＵ１６０は、その固有の幹間隔を有する部分同士のマッチングにより、トラクタ１００Ａの現在の位置および向きを推定することができる。このような動作により、樹木列が固有の幹間隔の組み合わせを有する部分を含む場合に、マッチングの高速化および位置推定の精度の向上を図ることができる。樹木列における幹間隔の組み合わせを示す情報が予め記憶媒体に記録されていてもよい。そのような情報は、例えばＥＣＵ１６０が環境地図データに基づいて自動で記録してもよいし、ユーザが操作端末２００を操作することによって記録してもよい。
【0186】
本実施形態において、トラクタ１００Ａが生成した環境地図データは、同一の環境内を走行する他のトラクタまたはトラクタ以外の移動体に提供することもできる。例えば広い果樹園において、複数の移動体によって作業を行う場合、環境地図を一台の移動体またはコンピュータが作成すれば十分であり、環境地図を複数の移動体で共有することが効率的である。１つの環境内の環境地図を構築するための局所地図を、複数のトラクタまたはその他の移動体によって分担して生成してもよい。その場合、複数の移動体によって生成された局所地図をまとめて１つの環境地図を生成するコンピュータがシステム内に設けられる。当該コンピュータは、生成した環境地図を、複数の移動体に、有線もしくは無線のネットワーク、または記憶媒体を介して配信してもよい。
【0187】
同一の環境内を複数の移動体が移動しながら作業するシステムにおいては、移動体ごとにセンサ座標系と移動体座標系との位置関係が異なり得る。同一の機種であっても、センサの取り付け位置の誤差に起因して、センサ座標系から移動体座標系への変換に誤差が生じ得る。そのような誤差の影響を低減するために、各移動体は、通常の運行を開始する前に、試運転によってセンサ座標系から移動体座標系への座標変換のためのパラメータを決定するキャリブレーションを実行する。
【0188】
本実施形態の技術は、ぶどう園などの果樹園を走行するトラクタに限らず、森林などの、複数の樹木列が存在する環境で使用される任意の移動体（例えば、移動ロボット、ドローン等）に適用することができる。この点については、以降の実施形態でも同様である。
【0189】
［３－２．実施形態２］
次に、本開示の第２の実施形態による移動体を説明する。
次に、本開示の第２の実施形態による移動体を説明する。
【0190】
本実施形態における移動体は、少なくとも２つのＬｉＤＡＲセンサを備え、それらのＬｉＤＡＲセンサから出力されるセンサデータに基づいて、自己位置推定および地図データの生成を行う。少なくとも２つのＬｉＤＡＲセンサの各々は、移動体の周囲の環境における物体の分布を示す２次元または３次元の点群データ、または距離分布データを出力する。移動体は、実施形態１と同様、複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、自己位置推定装置によって推定された移動体の位置に応じて移動体の移動を制御する制御装置とを備える。自己位置推定装置は、移動体が移動している間に少なくとも２つのセンサから繰り返し出力される点群データに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。移動体はさらに、環境地図データ、または環境地図データを構築するための局所地図データを生成するデータ生成装置も備える。データ生成装置は、移動体が移動している間に少なくとも２つのセンサから繰り返し出力される点群データに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。データ生成装置は、移動体が移動している間に繰り返し生成した局所地図データを統合することにより、ワールド座標系における環境地図データを生成して記憶装置に記録することもできる。
【0191】
以下、移動体がぶどう園などの果樹園内を走行するトラクタである場合を例に、本実施形態の構成および動作を説明する。以下の説明において、実施形態１とは異なる点を中心に説明し、重複する事項についての説明は省略する。
【0192】
図３４は、本実施形態におけるトラクタ１００Ｂを側面方向から見た模式図である。このトラクタ１００Ｂは、車体前方下部に第１のＬｉＤＡＲセンサ１１０Ａを備え、キャビンの上部前方に第２のＬｉＤＡＲセンサ１１０Ｂを備えている。それ以外の点は、図７に示す構成と同様である。図３４には、図７に示されている連結装置１０８、および作業機３００が示されていないが、本実施形態においてもこれらは使用され得る。
【0193】
本実施形態における第１のＬｉＤＡＲセンサ１１０Ａおよび第２のＬｉＤＡＲセンサ１１０Ｂの各々は、２次元ＬｉＤＡＲセンサである。言い換えれば、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの各々は、図９Ａに示す例において、Ｎ＝１の場合、すなわち１つの仰角の方向にのみレーザビームを出射するＬｉＤＡＲセンサ１１０と同等である。ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの各々は、各反射点までの距離と方向とを示すデータ、または、２次元の点群データを出力する。２次元の点群データは、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂのそれぞれのセンサ座標系で表現された複数の反射点の２次元座標の情報を含む。２つの２次元ＬｉＤＡＲを用いることにより、３次元ＬｉＤＡＲを用いる場合よりもコストを低減させることができる。
【0194】
第１のＬｉＤＡＲセンサ１１０Ａは、トラクタ１００Ｂが走行する環境における樹木列の幹部の平均高さよりも低い位置に搭載されている。第２のＬｉＤＡＲセンサ１１０Ｂは、当該樹木列の幹部の平均高さよりも高い位置に搭載されている。第１のＬｉＤＡＲセンサ１１０Ａは、例えば地上から１０ｃｍ以上１５０ｃｍ以下、ある例では１５ｃｍ以上１００ｃｍ以下の高さに配置され得る。第２のＬｉＤＡＲセンサ１１０Ｂは、例えば地上から１５０ｃｍよりも高い位置（例えば地上から約２ｍ）に配置され得る。
【0195】
第１のＬｉＤＡＲセンサ１１０Ａは、実施形態１におけるＬｉＤＡＲセンサ１１０と同様、トラクタ１００Ｂの前方に向けてレーザパルスを出射するように配置される。図３４の例における第１のＬｉＤＡＲセンサ１１０Ａは、地面にほぼ平行にレーザパルスを出射する。トラクタ１００Ｂが平坦な地面に位置しているとき、第１のＬｉＤＡＲセンサ１１０Ａから出射されるレーザパルスの出射方向と地面とのなす角度は、例えば±２０度の範囲内、ある例では±１０度の範囲内に設定され得る。
【0196】
一方、第２のＬｉＤＡＲセンサ１１０Ｂは、トラクタ１００Ｂの前方斜め下に向けてレーザパルスを出射するように配置される。図３４の例における第２のＬｉＤＡＲセンサ１１０Ｂは、地面から約２５度傾いた方向にレーザパルスを出射する。トラクタ１００Ｂが平坦な地面に位置しているとき、第２のＬｉＤＡＲセンサ１１０Ｂから出射されるレーザパルスの出射方向と地面とのなす角度は、例えば１０度以上４５度以下の範囲に設定され得る。
【0197】
図３５は、本実施形態におけるトラクタ１００Ｂの概略的な構成の例を示すブロック図である。図３５に示す構成例では、トラクタ１００Ｂが２つのＬｉＤＡＲセンサ１１０Ａ、１１０Ｂを備えている点で、図８に示す構成とは異なる。
【0198】
ＥＣＵ１６０は、上記の自己位置推定装置およびデータ生成装置として機能する。ＥＣＵ１６０は、２つのＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから繰り返し出力される点群データから、最新スキャンを含む複数スキャンの入力点群を取得し、複数スキャンの入力点群と環境地図データとのマッチングを行う。これにより、走行中のトラクタ１００Ｂの位置および向きを推定することができる。なお、ＥＣＵ１６０は、マッチングによってトラクタ１００Ｂの位置のみを推定し、向きについてはＩＭＵ１２５から出力された信号に基づいて決定してもよい。
【0199】
環境地図を構築する過程において、ＥＣＵ１６０は、トラクタ１００Ｂが移動している間に２つのＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから繰り返し出力されるスキャンデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置１５０に記録する。この動作が行われている間、ＥＣＵ１７０は、スキャンデータに基づいて検出された樹木列の幹部の間を通る目標経路を設定する。ＥＣＵ１８０は、設定された目標経路に従って駆動装置１４０を制御することにより、トラクタ１００Ｂを目標経路に沿って走行させる。ＥＣＵ１７０は、障害物センサ１３０から出力される信号、またはＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力されるスキャンデータに基づいて障害物を検出し、障害物との接触を回避するように目標経路を設定してもよい。ＥＣＵ１６０は、繰り返し生成した局所地図データを繋ぎ合わせることにより、環境地図データを生成して記憶装置１５０に記録する。なお、樹木列の幹部の検出は、局所地図データを生成する段階ではなく、環境地図データを生成する段階で行ってもよい。また、局所地図データに基づいて環境地図データを生成する処理は、外部のコンピュータが実行してもよい。
【0200】
環境地図が構築された後の自動走行または自動操舵の動作中、ＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから繰り返し出力されるスキャンデータに基づいて樹木列の幹部を検出し、検出した幹部と環境地図データが示す幹部とのマッチングを行うことにより、自己位置推定を行う。ＥＣＵ１８０は、ＥＣＵ１７０によって予め決定された走行予定経路に沿ってトラクタ１００Ｂを走行させる。この場合も、ＥＣＵ１７０は、トラクタ１００Ｂの走行中に障害物センサ１３０から出力される信号、またはＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力されるスキャンデータに基づいて障害物を検出し、障害物との接触を回避するように走行予定経路を変更してもよい。
【0201】
図３６Ａから図３６Ｃは、トラクタ１００Ｂが樹木２１の近くを走行する様子を模式的に示す図である。図３６Ａ、図３６Ｂ、図３６Ｃの順に、トラクタ１００Ｂが樹木２１に近付いている。これらの図において、破線は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂから出射されるレーザビームを表している。図示されるように、トラクタ１００Ｂと樹木２１との距離に応じて、第２のＬｉＤＡＲセンサ１１０Ｂから出射されるレーザビームによって照射される樹木２１の幹部２２の照射点の位置が、高さ方向に変化する。一方、第１のＬｉＤＡＲセンサ１１０Ａから出射されるレーザビームによって照射される樹木２１の幹部２２の照射点の位置の変化は相対的に小さい。これらのレーザビームの複数回のスキャンによって取得される点群データを組み合わせることにより、３次元ＬｉＤＡＲを用いた場合と同様に、幹部２２の表面における反射点の３次元分布を示す点群データを取得することができる。
【0202】
本実施形態におけるＥＣＵ１６０は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂのそれぞれから出力された所定のスキャン回数分（例えば３回分または５回分等）のスキャンデータを蓄積し、蓄積されたスキャンデータと、環境地図データとのマッチングを行うことにより、自己位置推定を行ってもよい。このようにすることで、実質的に３次元ＬｉＤＡＲを用いた場合と同様の高い精度の自己位置推定が可能になる。
【0203】
地図データを取得するためにトラクタ１００Ｂが走行している間、ＥＣＵ１７０は、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力された数スキャン分のスキャンデータに基づいて、トラクタ１００Ｂの局所的な目標経路を決定する。例えば、ＬｉＤＡＲセンサ１１０Ａ、１１０Ｂの一方または両方から出力された数スキャン分のスキャンデータが示す点群の配置から、隣り合う２つの樹木間の中心位置を決定し、その中心位置を通るようにトラクタ１００Ｂの局所的な目標経路を決定することができる。このような経路設定により、トラクタ１００Ｂは、左右の樹木列との距離が概ね等しい状態を保ちながら自律的に走行することができる。
【0204】
幹部の検出は、第１のＬｉＤＡＲセンサ１１０Ａから出力されるスキャンデータのみに基づいて行われてもよい。相対的に低い位置にある第１のＬｉＤＡＲセンサ１１０Ａから出力されるスキャンデータは、幹部の表面を表す点群の情報を多く含むことが多い。このため、第１のＬｉＤＡＲセンサ１１０Ａからのスキャンデータのみを用いた場合でも幹部の分布情報を取得することができる。
【0205】
本実施形態における第２のＬｉＤＡＲセンサ１１０Ｂは、相対的に高い位置に配置されているため、樹木列の葉部の検出に適している。そこで、ＥＣＵ１６０は、第２のＬｉＤＡＲセンサ１１０Ｂによって取得された点群データに基づいて、樹木列の葉部の分布を示すデータを生成し、記憶装置１５０または他の記憶媒体に記録してもよい。そのようなデータは、樹木の生育状況の管理（例えば、キャノピーマネジメント）のために使用され得る。
【0206】
トラクタ１００Ｂの走行中に、第２のＬｉＤＡＲセンサ１１０Ｂによって取得された点群データに基づいて、樹木列の葉部などの、比較的高い位置に存在する障害物を検出し、回避または停止する動作を行ってもよい。トラクタ１００Ｂの前方上部に障害物センサ１３０が設けられていない場合、第２のＬｉＤＡＲセンサ１１０Ｂが設けられていなければ、樹木列間の経路の上方にはみだした樹木の葉または蔓を検出して避けることが難しい。第２のＬｉＤＡＲセンサ１１０Ｂを設けることにより、それらの葉または蔓を検出することが容易になる。
【0207】
本実施形態によれば、２つの２次元ＬｉＤＡＲを用いることにより、低いコストで実施形態１と同様の動作を実現することができる。なお、２つに限らず、３つ以上の２次元ＬｉＤＡＲを異なる位置に設けてもよい。また、１つ以上の２次元ＬｉＤＡＲと１つ以上の３次元ＬｉＤＡＲとを組み合わせて使用してもよい。このように、本実施形態の構成には多様な変形が可能である。なお、実施形態１について説明した種々の技術は、本実施形態においてもそのまま適用できる。
【0208】
［３－３．実施形態３］
次に、本開示の第３の実施形態を説明する。
次に、本開示の第３の実施形態を説明する。
【0209】
本実施形態は、１台または複数台の移動体（例えばドローン）を用いて、環境地図データを構築するための局所地図データを収集し、収集された局所地図データに基づいて環境地図データを生成するシステムに関する。
【0210】
図３７は、本実施形態のシステムの構成例を模式的に示す図である。本システムは、１台以上のドローン４００と、１台以上のトラクタ１００Ｃと、サーバコンピュータ５００（以下、「サーバ５００」と称する。）とを備える。これらの構成要素は、有線または無線のネットワーク６０を介して、互いに通信可能に接続されている。図３７には、２台のドローン４００および３台のトラクタ１００Ｃが示されているが、ドローン４００およびトラクタ１００Ｃのそれぞれの台数は任意である。各ドローン４００は、各トラクタ１００Ｃが走行する環境内を飛行しながら環境地図データを構築するための局所地図データを生成し、サーバ５００に送信する。サーバ５００は、１台以上のドローン４００から受信した局所地図データに基づいて、統一されたワールド座標系で表現された環境地図データを生成し、各トラクタ１００Ｃに配信する。各トラクタ１００Ｃは、配信された環境地図データに基づいて果樹園内を自律的に走行する。
【0211】
図３８は、本システムの構成例を示すブロック図である。この例におけるトラクタ１００Ｃは、図８に示す実施形態１のトラクタ１００Ａと同様の構成要素を備える。ただし、本実施形態におけるＥＣＵ１６０は、地図データ生成モジュール１６４を備えていない。通信ＩＦ１９０は、ネットワーク６０を介してサーバ５００と通信を行う。記憶装置１５０は、サーバ５００から配信された環境地図データを記憶する。なお、図３８では、トラクタ１００Ｃに連結されて使用される作業機の図示は省略されている。通信ＩＦ１９０は、実施形態１と同様、トラクタ１００Ｃに接続された作業機との通信を行ってもよい。
【0212】
ドローン４００は、データ生成ユニット４５０と、駆動装置４４０と、制御装置４８０と、通信ＩＦ４９０とを備える。駆動装置４４０は、例えば駆動用の電気モータ、および複数のプロペラなどの、ドローン４００の飛行に必要な種々の装置を含む。制御装置４８０は、データ生成ユニット４５０および駆動装置４４０の動作を制御する。通信ＩＦ４９０は、ネットワーク６０を介してサーバ５００と通信するための回路である。データ生成ユニット４５０は、ＬｉＤＡＲセンサ４１０と、データ生成装置４６０とを備える。ＬｉＤＡＲセンサ４１０は、実施形態１におけるＬｉＤＡＲセンサ１１０と同様の機能を備える。データ生成装置４６０は、実施形態１におけるＥＣＵ１６０と同様の機能を備える。すなわち、データ生成装置４６０は、自己位置推定と地図生成とを同時に行う機能を備える。ただし、データ生成装置４６０は、最終的な環境地図データの生成を行わない。データ生成装置４６０は、プロセッサとメモリ等の記憶媒体とを備える。データ生成装置４６０は、ドローン４００の飛行中にＬｉＤＡＲセンサ４１０から繰り返し出力されたスキャンデータに基づいて、複数の樹木列の幹部の分布を示す局所地図データを繰り返し生成して記憶媒体に蓄積する。蓄積された局所地図データは、例えばユーザによる操作により、通信ＩＦ４９０からサーバ５００に送信される。
【0213】
サーバ５００は、例えばクラウドサーバまたはエッジサーバなどの、トラクタ１００Ｃおよびドローン４００から離れた場所に設置されたコンピュータであり得る。サーバ５００は、記憶装置５５０と、処理装置５６０と、通信ＩＦ５９０とを備える。通信ＩＦ５９０は、ネットワーク６０を介してトラクタ１００Ｃおよびドローン４００と通信するための回路である。処理装置５６０は、複数のドローン４００から取得した局所地図データを、座標系を統一して統合することにより、環境地図データを生成し、記憶装置５５０に記録する。処理装置５６０は、生成した環境地図データを、通信ＩＦ５９０から複数のトラクタ１００Ｃに配信する。処理装置５６０は、環境地図データをトラクタ１００Ｃ以外の移動体に配信してもよい。例えば、ドローン４００が、地図データの収集だけでなく、播種、施肥、または防除などの作業も実行する場合には、ドローン４００に環境地図データが配信されてもよい。その場合、ドローン４００は、ＬｉＤＡＲセンサ４１０から出力されたスキャンデータと、環境地図データとのマッチングによって自己位置推定を行いながら自律的に飛行し、所定の作業を実行することができる。
【0214】
図３９は、ドローン４００の外観の例を示す斜視図である。このドローン４００は、本体の前面にデータ生成ユニット４５０を備える。データ生成ユニット４５０は、ＬｉＤＡＲセンサ４１０を備えており、飛行しながらレーザビームで周囲の環境をスキャンすることができる。
【0215】
図４０は、複数のドローン４００によるデータ収集作業の様子の例を模式的に示す斜視図である。図４０には、複数の樹木列２０の間を飛行する２台のドローン４００が例示されている。これらのドローン４００は、樹木列２０の間を低空飛行で自律的に移動しながら、樹木列２０の幹部をレーザビームでスキャンすることにより、幹部の分布を示す局所地図データを生成する動作を繰り返す。局所地図データの生成方法は、実施形態１における方法と同様である。
【0216】
図４１は、ドローン４００に搭載されたＬｉＤＡＲセンサ４１０から出射されたレーザビームによって樹木２１が照射されている様子を模式的に示す図である。本実施形態におけるドローン４００は、ＬｉＤＡＲセンサ４１０が、樹木列の幹部の平均高さよりも低い位置を保つ高度で飛行するようにプログラミングされている。ＬｉＤＡＲセンサ４１０が、例えば地上から１５ｃｍ以上１００ｃｍ以下の高さに位置するように、ドローン４００の高度が制御される。データ生成装置４６０は、このような低い高度に位置するＬｉＤＡＲセンサ４１０から繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、ドローン４００の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す局所地図データを生成して記憶媒体に記録する。データ生成装置４６０は、ＬｉＤＡＲセンサ４１０から繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む少なくとも１つのスキャンの入力点群と、前回以前に生成された局所地図データとのマッチングを行うことにより、自己位置推定を行う。
【0217】
図４２は、各ドローン４００から出力される局所地図データの形式の一例を示す図である。この例における局所地図データは、ドローン４００の識別番号である移動体ＩＤと、ワールド座標系におけるドローン４００の位置（ｘ，ｙ，ｚ）および姿勢（θＲ，θＰ，θＹ）と、反射点ごとに記録される情報とを含む。反射点ごとに記録される情報は、図２３に示す情報と同様であるが、各反射点の座標は、ドローン４００のＬｉＤＡＲセンサ４１０に固定されたセンサ座標系における座標（ｕ，ｖ，ｗ）である。このような局所地図データが、各ドローン４００からサーバ５００に送信され得る。なお、分類および幹ＩＤなどの、その点が幹であるか否かに関する情報は、局所地図データに含まれていなくてもよい。サーバ５００における処理装置５６０が、複数のドローン４００から局所地図データを取得した後、点群の分布の特徴に基づいて幹部に該当する点群を検出してもよい。
【0218】
以上の構成により、例えば広い果樹園においても、環境地図を構築するためのデータを効率的に収集することができる。
【0219】
なお、本実施形態では、ドローン４００だけでなく、トラクタ１００Ｃなどの他の移動体も、環境地図データを構築するための局所地図データを取得する動作を行ってもよい。また、局所地図データを生成する移動体は、必ずしも自律的に移動可能な移動体でなくてもよい。例えば、樹木列の幹部の平均高さよりも低い位置に搭載された１つ以上のＬｉＤＡＲセンサを備えたトラクタまたはドローンなどの移動体を、ユーザが運転または操縦することによって局所地図データを生成してもよい。
【0220】
サーバ５００を設けることなく、ドローン４００またはトラクタ１００Ｃなどの移動体が最終的な環境地図データを生成し、他の移動体に環境地図データを提供してもよい。その場合には、移動体間の通信によって環境地図データが直接的に送受信され得る。
【0221】
［３－４．他の実施形態］
以上の実施形態において、各トラクタは、無人トラクタであってもよい。その場合には、キャビン、運転席、ステアリングホイール、操作端末などの、有人運転にのみ必要な構成要素は、トラクタに設けられていなくてもよい。無人トラクタは、自律走行、またはトラクタのユーザによる遠隔操作によって、前述の各実施形態における動作と同様の動作を実行してもよい。
以上の実施形態において、各トラクタは、無人トラクタであってもよい。その場合には、キャビン、運転席、ステアリングホイール、操作端末などの、有人運転にのみ必要な構成要素は、トラクタに設けられていなくてもよい。無人トラクタは、自律走行、またはトラクタのユーザによる遠隔操作によって、前述の各実施形態における動作と同様の動作を実行してもよい。
【0222】
以上の実施形態において、移動体が備える１つ以上のセンサは、レーザビームのスキャンによって２次元または３次元の点群データまたは距離分布データをセンサデータとして出力するＬｉＤＡＲセンサである。しかし、センサはそのようなＬｉＤＡＲセンサに限定されない。例えば、フラッシュ型のＬｉＤＡＲセンサ、またはイメージセンサなどの他の種類のセンサを利用してもよい。そのような他の種類のセンサを、スキャン型のＬｉＤＡＲセンサと組み合せて利用してもよい。
【0223】
以上の実施形態における自己位置推定および自律移動（もしくは自動操舵）、または地図データ生成に必要な処理を実行する装置を、それらの機能を有しない移動体に後から取り付けることもできる。例えば、複数の樹木列の間を移動する移動体の動作を制御する制御ユニットを、当該移動体に取り付けて使用することができる。そのような制御ユニットは、移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、自己位置推定装置と、自己位置推定装置によって推定された移動体の位置に応じて移動体の移動を制御する制御装置とを備える。自己位置推定装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部と環境地図データとのマッチングを行うことにより、移動体の位置を推定する。また、地図データを生成するデータ生成ユニットを、複数の樹木列の間を移動する移動体に取り付けて使用することもできる。そのようなデータ生成ユニットは、移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、データ生成装置とを備える。データ生成装置は、移動体が移動している間に１つ以上のセンサから繰り返し出力されるセンサデータに基づいて、自己位置推定を行いながら、移動体の周囲の環境における樹木列の幹部を検出し、検出した樹木列の幹部の分布を示す環境地図データを生成するための局所地図データを生成して記憶装置に記録する。
【0224】
以上のように、本開示は、以下の項目に記載の移動体、制御ユニット、方法、およびコンピュータプログラムを含む。
【0225】
［項目１］
複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える移動体。
複数の樹木列の間を移動する移動体であって、
前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える移動体。
【0226】
［項目２］
前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含む、項目１に記載の移動体。
前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含む、項目１に記載の移動体。
【0227】
［項目３］（ＬｉＤＡＲセンサの位置）
前記ＬｉＤＡＲセンサは、前記樹木列の幹部の平均高さよりも低い位置に搭載されている、項目２に記載の移動体。
前記ＬｉＤＡＲセンサは、前記樹木列の幹部の平均高さよりも低い位置に搭載されている、項目２に記載の移動体。
【0228】
［項目４］
前記ＬｉＤＡＲセンサは、地上から１５ｃｍ以上１００ｃｍ以下の高さに配置されている、項目３に記載の移動体。
前記ＬｉＤＡＲセンサは、地上から１５ｃｍ以上１００ｃｍ以下の高さに配置されている、項目３に記載の移動体。
【0229】
［項目５］
前記自己位置推定装置は、前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサから繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む少なくとも１つのスキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目３または４に記載の移動体。
前記自己位置推定装置は、前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサから繰り返し出力されるセンサデータから複数スキャンの入力点群を取得し、最新スキャンを含む少なくとも１つのスキャンの入力点群と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目３または４に記載の移動体。
【0230】
［項目６］
前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサは、複数のレーザ光源を有しており、前記複数のレーザ光源は、それぞれ、異なる仰角でレーザパルスを出射し、
前記自己位置推定装置は、前記複数のレーザ光源のうち、前記仰角が所定の範囲に含まれるレーザ光源から出射された前記レーザパルスの反射点に基づいて前記幹部を検出する、項目３から５のいずれかに記載の移動体。
前記樹木列の幹部の平均高さよりも低い位置に搭載されている前記ＬｉＤＡＲセンサは、複数のレーザ光源を有しており、前記複数のレーザ光源は、それぞれ、異なる仰角でレーザパルスを出射し、
前記自己位置推定装置は、前記複数のレーザ光源のうち、前記仰角が所定の範囲に含まれるレーザ光源から出射された前記レーザパルスの反射点に基づいて前記幹部を検出する、項目３から５のいずれかに記載の移動体。
【0231】
［項目７］
前記自己位置推定装置は、前記ＬｉＤＡＲセンサから前記レーザパルスの反射点までの距離の、前記ＬｉＤＡＲセンサによるスキャン中における変化に基づいて前記幹部を検出する、項目６に記載の移動体。
前記自己位置推定装置は、前記ＬｉＤＡＲセンサから前記レーザパルスの反射点までの距離の、前記ＬｉＤＡＲセンサによるスキャン中における変化に基づいて前記幹部を検出する、項目６に記載の移動体。
【0232】
［項目８］
前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含み、
前記ＬｉＤＡＲセンサは、予め設定された周期で前記点群データを繰り返し出力し、
前記自己位置推定装置は、１周期以上の期間中に出力された前記点群データにおける各点の位置、または各点の前記移動体からの距離もしくは角度に基づいて、前記幹部の表面を表す点を抽出し、抽出した前記点と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目１に記載の移動体。
前記１つ以上のセンサは、２次元または３次元の点群データを前記センサデータとして出力する少なくとも１つのＬｉＤＡＲセンサを含み、
前記ＬｉＤＡＲセンサは、予め設定された周期で前記点群データを繰り返し出力し、
前記自己位置推定装置は、１周期以上の期間中に出力された前記点群データにおける各点の位置、または各点の前記移動体からの距離もしくは角度に基づいて、前記幹部の表面を表す点を抽出し、抽出した前記点と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、項目１に記載の移動体。
【0233】
［項目９］
慣性計測装置をさらに備え、
前記自己位置推定装置は、前記慣性計測装置から出力された信号を利用して前記移動体の位置を推定する、
項目１から８のいずれかに記載の移動体。
慣性計測装置をさらに備え、
前記自己位置推定装置は、前記慣性計測装置から出力された信号を利用して前記移動体の位置を推定する、
項目１から８のいずれかに記載の移動体。
【0234】
［項目１０］
前記自己位置推定装置は、前記センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部と、前記環境地図データから抽出した前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部とのマッチングにより、前記移動体の位置を推定する、
項目１から９のいずれかに記載の移動体。
前記自己位置推定装置は、前記センサデータから、固有の幹間隔の組み合わせを有する複数の樹木の幹部を検出し、前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部と、前記環境地図データから抽出した前記固有の幹間隔の組み合わせを有する前記複数の樹木の幹部とのマッチングにより、前記移動体の位置を推定する、
項目１から９のいずれかに記載の移動体。
【0235】
［項目１１］
前記制御装置は、前記環境地図データおよび前記センサデータに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目１から１０のいずれかに記載の移動体。
前記制御装置は、前記環境地図データおよび前記センサデータに基づいて、前記移動体の経路上に障害物が存在するか否かを判断し、前記障害物が存在すると判断したとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目１から１０のいずれかに記載の移動体。
【0236】
［項目１２］
障害物センサをさらに備え、
前記制御装置は、前記障害物センサによって前記移動体の経路上に障害物が検出されたとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目１から１１のいずれかに記載の移動体。
障害物センサをさらに備え、
前記制御装置は、前記障害物センサによって前記移動体の経路上に障害物が検出されたとき、前記移動体に、前記障害物に衝突することを回避する動作を実行させる、項目１から１１のいずれかに記載の移動体。
【0237】
［項目１３］
前記センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、前記各幹の太さを記憶媒体に記録する記録装置をさらに備える、項目１から１２のいずれかに記載の移動体。
前記センサデータに基づいて、検出された樹木列の各幹部の太さを計測し、前記各幹の太さを記憶媒体に記録する記録装置をさらに備える、項目１から１２のいずれかに記載の移動体。
【0238】
［項目１４］
前記複数の樹木列が配置されたエリアには１つ以上の支柱が設けられ、
前記環境地図データは、前記幹部の分布に加えて前記支柱の分布も示し、
前記自己位置推定装置は、前記センサデータに基づいて、前記移動体の周囲の環境における前記支柱をさらに検出し、検出した前記樹木列の幹部および前記支柱と、前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、
項目１から１３のいずれかに記載の移動体。
前記複数の樹木列が配置されたエリアには１つ以上の支柱が設けられ、
前記環境地図データは、前記幹部の分布に加えて前記支柱の分布も示し、
前記自己位置推定装置は、前記センサデータに基づいて、前記移動体の周囲の環境における前記支柱をさらに検出し、検出した前記樹木列の幹部および前記支柱と、前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する、
項目１から１３のいずれかに記載の移動体。
【0239】
［項目１５］
前記自己位置推定装置は、前記センサデータと、記憶媒体に記録された前記複数の樹木列の配置関係を示す情報とに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出する、項目１から１４のいずれかに記載の移動体。
前記自己位置推定装置は、前記センサデータと、記憶媒体に記録された前記複数の樹木列の配置関係を示す情報とに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出する、項目１から１４のいずれかに記載の移動体。
【0240】
［項目１６］
前記複数の樹木列の配置関係を示す情報は、ＨＭＩ（Human Machine Interface）を介して入力されることにより、前記記憶媒体に記録される、項目１５に記載の移動体。
前記複数の樹木列の配置関係を示す情報は、ＨＭＩ（Human Machine Interface）を介して入力されることにより、前記記憶媒体に記録される、項目１５に記載の移動体。
【0241】
［項目１７］
前記移動体は自律的に移動可能な作業車両である、項目１から１６のいずれかに記載の移動体。
前記移動体は自律的に移動可能な作業車両である、項目１から１６のいずれかに記載の移動体。
【0242】
［項目１８］
前記自己位置推定装置は、
前記センサデータに基づいて検出した前記樹木列の幹部の分布を示すデータを生成し、
前記幹部の分布を示すデータを用いて前記環境地図データを更新する、または、前記幹部の分布を示すデータを、前記環境地図データを更新する外部の装置に送信する、
項目１から１７のいずれかに記載の移動体。
前記自己位置推定装置は、
前記センサデータに基づいて検出した前記樹木列の幹部の分布を示すデータを生成し、
前記幹部の分布を示すデータを用いて前記環境地図データを更新する、または、前記幹部の分布を示すデータを、前記環境地図データを更新する外部の装置に送信する、
項目１から１７のいずれかに記載の移動体。
【0243】
［項目１９］
複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える制御ユニット。
複数の樹木列の間を移動する移動体の動作を制御する制御ユニットであって、
前記移動体に取り付けられて使用され、前記移動体の周囲の環境における物体の分布を示すセンサデータを出力する１つ以上のセンサと、
前記複数の樹木列の幹部の分布を示す環境地図データを記憶する記憶装置と、
前記移動体が移動している間に前記１つ以上のセンサから繰り返し出力される前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出し、検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定する自己位置推定装置と、
推定された前記移動体の位置に応じて前記移動体の移動を制御する制御装置と、
を備える制御ユニット。
【0244】
［項目２０］
複数の樹木列の間を移動する移動体の動作を制御する方法であって、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された１つ以上のセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を含む方法。
複数の樹木列の間を移動する移動体の動作を制御する方法であって、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された１つ以上のセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を含む方法。
【0245】
［項目２１］
複数の樹木列の間を移動する移動体の動作を制御するコンピュータによって実行されるコンピュータプログラムであって、
前記コンピュータに、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された１つ以上のセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を実行させるコンピュータプログラム。
複数の樹木列の間を移動する移動体の動作を制御するコンピュータによって実行されるコンピュータプログラムであって、
前記コンピュータに、
前記複数の樹木列の幹部の分布を示す環境地図データを取得することと、
前記移動体が移動している間に、前記移動体の周囲の環境における物体の分布を示すセンサデータを、前記移動体に搭載された１つ以上のセンサから取得することと、
取得した前記センサデータに基づいて、前記移動体の周囲の環境における前記樹木列の幹部を検出することと、
検出した前記樹木列の幹部と前記環境地図データとのマッチングを行うことにより、前記移動体の位置を推定することと、
推定された前記移動体の位置に応じて前記移動体の移動を制御することと、
を実行させるコンピュータプログラム。
【産業上の利用可能性】
【0246】
本開示の技術は、例えばぶどう園などの果樹園のように、複数の樹木列が存在する環境を移動するトラクタ、ドローン、歩行ロボットなどの移動体に適用することができる。
【符号の説明】
【0247】
２０ 樹木列
２２ 樹木の幹部
３０ 走行経路
４０ 障害物
５０ 人
６０ ネットワーク
１００ 移動体
１００Ａ、１００Ｂ トラクタ
１０１ 車両本体
１０２ 原動機（エンジン）
１０３ 変速装置（トランスミッション）
１０４ 車輪
１０５ キャビン
１０６ 操舵装置
１０７ 運転席
１０８ 連結装置
１１０、１１０Ａ、１１０Ｂ ＬｉＤＡＲセンサ
１１１ レーザユニット
１１２ レーザ光源
１１３ 光検出器
１１４ モータ
１１５ 制御回路
１１６ 信号処理回路
１１７ メモリ
１２０ ＧＮＳＳユニット
１２５ 慣性計測装置（ＩＭＵ）
１３０ 障害物センサ
１４０ 駆動装置
１５０ 記憶装置
１６０ 電子制御ユニット（ＳＬＡＭ）
１６２ 自己位置推定モジュール
１６４ 地図データ生成モジュール
１７０ 電子制御ユニット（経路決定）
１８０ 電子制御ユニット（駆動制御）
１９０ 通信インタフェース（ＩＦ）
２００ 操作端末
３００ 作業機（インプルメント）
３４０ 駆動装置
３８０ 制御装置
３９０ 通信インタフェース（ＩＦ）
４００ ドローン
４１０ ＬｉＤＡＲセンサ
４４０ 駆動装置
４５０ データ生成ユニット
４６０ データ生成装置
４８０ 制御装置
４９０ 通信ＩＦ
５００ サーバ
５５０ 記憶装置
５６０ 処理装置
５９０ 通信ＩＦ
２２ 樹木の幹部
３０ 走行経路
４０ 障害物
５０ 人
６０ ネットワーク
１００ 移動体
１００Ａ、１００Ｂ トラクタ
１０１ 車両本体
１０２ 原動機（エンジン）
１０３ 変速装置（トランスミッション）
１０４ 車輪
１０５ キャビン
１０６ 操舵装置
１０７ 運転席
１０８ 連結装置
１１０、１１０Ａ、１１０Ｂ ＬｉＤＡＲセンサ
１１１ レーザユニット
１１２ レーザ光源
１１３ 光検出器
１１４ モータ
１１５ 制御回路
１１６ 信号処理回路
１１７ メモリ
１２０ ＧＮＳＳユニット
１２５ 慣性計測装置（ＩＭＵ）
１３０ 障害物センサ
１４０ 駆動装置
１５０ 記憶装置
１６０ 電子制御ユニット（ＳＬＡＭ）
１６２ 自己位置推定モジュール
１６４ 地図データ生成モジュール
１７０ 電子制御ユニット（経路決定）
１８０ 電子制御ユニット（駆動制御）
１９０ 通信インタフェース（ＩＦ）
２００ 操作端末
３００ 作業機（インプルメント）
３４０ 駆動装置
３８０ 制御装置
３９０ 通信インタフェース（ＩＦ）
４００ ドローン
４１０ ＬｉＤＡＲセンサ
４４０ 駆動装置
４５０ データ生成ユニット
４６０ データ生成装置
４８０ 制御装置
４９０ 通信ＩＦ
５００ サーバ
５５０ 記憶装置
５６０ 処理装置
５９０ 通信ＩＦ
【図1】
【図2A】
【図2B】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9A】
【図9B】
【図10】
【図11】
【図12】
【図13A】
【図13B】
【図14A】
【図14B】
【図15A】
【図15B】
【図16】
【図17A】
【図17B】
【図17C】
【図18】
【図19】
【図20A】
【図20B】
【図21A】
【図21B】
【図22】
【図23】
【図24】
【図25】
【図26】
【図27】
【図28A】
【図28B】
【図28C】
【図28D】
【図29A】
【図29B】
【図30】
【図31A】
【図31B】
【図31C】
【図31D】
【図32】
【図33A】
【図33B】
【図34】
【図35】
【図36A】
【図36B】
【図36C】
【図37】
【図38】
【図39】
【図40】
【図41】
【図42】