(57)【要約】【課題】レーザ光により高精度に測距しつつ、より簡易に自己位置を同定できる。【解決手段】開示の自己位置同定システム１００は、測距される領域にレーザ光を照射し、前記領域内を測距するレーザセンサ１５と、前記レーザセンサ１５により測距される領域の画像を撮影するカメラ１６と、前記画像に対する画像認識処理によって、前記画像中における複数の基準物を認識する画像認識器４１と、前記画像中における複数の前記基準物の位置から、前記領域における複数の前記基準物それぞれの方向を検出する方向検出器４２と、前記レーザセンサにより測距された前記領域中の距離データ群から、前記方向検出器によって検出された方向それぞれにおける距離を、複数の前記基準物それぞれまでの距離として求める距離検出器４３と、複数の前記基準物それぞれまでの距離に基づいて、前記システムの自己位置を同定する位置同定器４４と、を備える。【選択図】図１
2019152924.tif 000002
【特許請求の範囲】
【請求項１】
測距される領域にレーザ光を照射し、前記領域内を測距するレーザセンサと、
前記レーザセンサにより測距される領域の画像を撮影するカメラと、
前記画像に対する画像認識処理によって、前記画像中における複数の基準物を認識する画像認識器と、
前記画像中における複数の前記基準物の位置から、前記領域における複数の前記基準物それぞれの方向を検出する方向検出器と、
前記レーザセンサにより測距された前記領域中の距離データ群から、前記方向検出器によって検出された方向それぞれにおける距離を、複数の前記基準物それぞれまでの距離として求める距離検出器と、
複数の前記基準物それぞれまでの距離に基づいて、システムの自己位置を同定する位置同定器と、
を備える自己位置同定システム。
【請求項２】
前記方向検出器は、前記画像中における複数の前記基準物の位置の変化に応じて、前記領域における複数の前記基準物それぞれの方向を更新する
請求項１に記載の自己位置同定システム。
【請求項３】
前記基準物は、不定形物である
請求項１又は２に記載の自己位置同定システム。
【請求項４】
前記基準物は、樹木の幹である
請求項３に記載の自己位置同定システム。
【請求項５】
請求項１〜４のいずれか１項に記載の自己位置同定システムと、
前記自己位置に基づいて自律走行のための制御をするコントローラと、
を備える車両。
【請求項６】
複数の樹木の列に沿って自律走行する車両であって、
光検出と測距（Light Detection and Ranging（ＬｉＤＡＲ））デバイスと、
前記ＬｉＤＡＲデバイスにより測距される領域の画像を撮影するカメラと、
前記画像に対する画像認識処理によって、前記画像中における複数の樹木の幹を認識する画像認識器と、
前記画像中における複数の前記幹の位置から、前記領域における複数の前記幹それぞれの方向を検出する方向検出器と、
前記ＬｉＤＡＲデバイスにより測距された前記領域中の距離データ群から、前記方向検出器によって検出された方向それぞれにおける距離を、複数の前記幹それぞれまでの距離として求める距離検出器と、
複数の前記幹それぞれまでの距離に基づいて、前記車両の自己位置を同定する位置同定器と、
前記自己位置に基づいて、複数の前記樹木の列に沿って前記車両を自律走行させるコントローラと、
を備える車両。
【請求項７】
前記車両は、前記幹が他の物体によって隠され得る場所を走行するものであり、
前記画像認識処理は、ディープラーニングによる画像認識処理である
請求項６に記載の車両。
【請求項８】
レーザセンサにより測距される領域をカメラで撮影した画像中における１又は複数の基準物を画像認識する処理と、
前記画像中における１又は複数の前記基準物の前記画像中における位置から、前記領域における１又は複数の前記基準物の方向を検出する処理と、
前記レーザセンサにより測距された前記領域中の距離データ群から、検出された方向における距離を前記基準物までの距離として求める処理と、
を実行するよう構成された処理装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、自己位置同定システム、車両、及び処理装置に関する。
【背景技術】
【０００２】
自動運転技術等においては、Light Detection and Ranging（ＬｉＤＡＲ）によって距離が測定され、Simultaneous Localization and Mapping（ＳＬＡＭ）技術により自己位置同定がなされる。ここでは、このような自己位置同定技術をＬｉＤＡＲ  ＳＬＡＭという。
【０００３】
また、特許文献１は、ＣＣＤカメラとレーザポインタとを備える自己位置認識装置を開示している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−７１９７８号公報
【発明の概要】
【０００５】
ＬｉＤＡＲ  ＳＬＡＭでは、レーザ光により測距されるため高精度である。しかし、測距される環境によっては、測距の対象を識別することが困難なことがある。この結果、自己位置同定が困難になる。したがって、レーザ光により高精度に測距しつつ、より簡易に自己位置を同定できることが望まれる。なお、特許文献１は、ＣＣＤカメラにより撮影された画像に対する画像認識処理を開示していないし、レーザポインタ自体は測距のためのレーザセンサでもない。
【０００６】
本開示に係る自己位置同定システムは、測距される領域にレーザ光を照射し、前記領域内を測距するレーザセンサと、前記レーザセンサにより測距される領域の画像を撮影するカメラと、前記画像に対する画像認識処理によって、前記画像中における複数の基準物を認識する画像認識器と、前記画像中における複数の前記基準物の位置から、前記領域における複数の前記基準物それぞれの方向を検出する方向検出器と、前記レーザセンサにより測距された前記領域中の距離データ群から、前記方向検出器によって検出された方向それぞれにおける距離を、複数の前記基準物それぞれまでの距離として求める距離検出器と、
複数の前記基準物それぞれまでの距離に基づいて、前記システムの自己位置を同定する位置同定器と、を備える。
【０００７】
本開示に係る車両は、複数の樹木の列に沿って自律走行する車両であって、光検出と測距（Light Detection and Ranging（ＬｉＤＡＲ））デバイスと、前記ＬｉＤＡＲデバイスにより測距される領域の画像を撮影するカメラと、前記画像に対する画像認識処理によって、前記画像中における複数の樹木の幹を認識する画像認識器と、前記画像中における複数の前記幹の位置から、前記領域における複数の前記幹それぞれの方向を検出する方向検出器と、前記ＬｉＤＡＲデバイスにより測距された前記領域中の距離データ群から、前記方向検出器によって検出された方向それぞれにおける距離を、複数の前記幹それぞれまでの距離として求める距離検出器と、複数の前記幹それぞれまでの距離に基づいて、前記車両の自己位置を同定する位置同定器と、前記自己位置に基づいて、複数の前記樹木の列に沿って前記車両を自律走行させるコントローラと、を備える。
【０００８】
本開示に係る処理装置は、レーザセンサにより測距される領域をカメラで撮影した画像中における１又は複数の基準物を画像認識する処理と、前記画像中における１又は複数の前記基準物の前記画像中における位置から、前記領域における１又は複数の前記基準物の方向を検出する処理と、前記レーザセンサにより測距された前記領域中の距離データ群から、前記検出器によって検出された方向における距離を前記基準物までの距離として求める処理と、を実行するよう構成されている。この処理装置によれば、基準物までの距離をレーザ光による高精度な測距結果に基づいて、基準物までの距離を簡易に求めることができる。
【０００９】
更なる詳細は、後述の実施形態として説明される。
【図面の簡単な説明】
【００１０】
【図１】図１は、自己位置同定システムを備える車両のブロック図である。
【図２】図２は、果樹園の樹列に沿って走行する車両を示す平面図である。
【図３】図３は、自己位置同定に関する処理のフローチャートである。
【図４】図４は、カメラによって撮影された画像である。
【図５】図５は、幹の方向を計算するための幾何学的関係を示す図である。
【図６】図６は、点群の表示イメージである。
【図７】図７は、自己位置を計算するための幾何学的関係を示す図である。
【図８】図８は、カメラによって撮影された画像である。
【図９】図９は、点群の表示イメージである。
【発明を実施するための形態】
【００１１】
＜１．自己位置同定システム、車両、及び処理装置の概要＞
【００１２】
（１）実施形態に係る自己位置同定システム（以下、単に「システム」ともいう）は、レーザセンサを備える。レーザセンサは、測距される領域にレーザ光を照射し、前記領域内を測距する。測距される領域は、点ではなく、２次元的又は３次元的に広がりのある範囲を持つ。レーザセンサは、例えば、ＬｉＤＡＲデバイスである。ＬｉＤＡＲデバイスは、３次元ＬｉＤＡＲデバイスであるのが好ましい。レーザセンサは、例えば、複数のレーザレンジファインダによって、２次元的又は３次元的に広がりのある領域における距離の分布を取得するよう構成されていてもよい。
【００１３】
システムは、カメラを備えることができる。カメラは、前記レーザセンサにより測距される領域の画像を撮影する。カメラが撮影する領域は、レーザセンサにより測距される領域と完全に一致していてもよいし、一部重複しているだけでもよい。カメラが撮影する領域は、レーザセンサにより測距される領域よりも広くてもよいし、狭くてもよい。
【００１４】
システムは、画像認識器を備えることができる。画像認識器は、前記画像に対する画像認識処理によって、前記画像中における複数の基準物を認識する。基準物は、人工物であってもよいし、自然物であってもよい。画像認識処理は、例えば、ディープラーニングによる画像認識処理である。ディープラーニングによる画像認識処理では、自然物のような不定形物であっても精度よく認識することができるため有利である。
【００１５】
システムは、方向検出器を備えることができる。方向検出器は、前記画像中における複数の前記基準物の位置から、前記領域における複数の前記基準物それぞれの方向を検出する。方向検出器は、画像認識結果から基準物の方向を検出するため、基準物の方向を確実に検出することができる。
【００１６】
システムは、距離検出器を備えることができる。距離検出器は、前記レーザセンサにより測距された前記領域中の距離データ群から、前記方向検出器によって検出された方向それぞれにおける距離を、複数の前記基準物それぞれまでの距離として求める。ここで、距離データ群とは、レーザセンサにより測距された前記領域中の各位置の距離データの集合である。距離データ群は、前記領域中の各位置までの距離を示す。例えば、ＬｉＤＡＲデバイスにより測距された距離データ群は、点群とよばれる。
【００１７】
システムは、位置同定器を備えることができる。位置同定器は、複数の前記基準物それぞれまでの距離に基づいて、前記システムの自己位置を同定する。位置同定に用いられる基準物の数は２でよいが、３以上であってもよい。
【００１８】
画像認識であれば、例えば不定形物が多く存在する環境でも、基準物となる特定の不定形物を認識することができ、基準物の方向を精度よく特定することができる。しかし、画像から基準物の距離を精度よく求めるのは容易ではない。一方、レーザセンサは、測距対象の領域中の各位置までの距離を精度よく求めることができるが、レーザセンサの測距結果から特定の基準物を識別するのは容易ではない。特に、レーザセンサの測距結果から不定形物を基準物として認識するのは困難である。
【００１９】
そこで、画像認識結果から基準物の方向を検出し、検出された方向におけるレーザセンサの高精度な測距結果を用いることで、容易に、基準物までの距離を精度よく求めることができる。複数の基準物までの距離が得られると、自己位置同定を行える。
【００２０】
（２）前記方向検出器は、前記画像中における複数の前記基準物の位置の変化に応じて、前記領域における複数の前記基準物それぞれの方向を更新するのが好ましい。この場合、システムの移動に伴って、画像中における基準物の位置が変化しても、基準物の方向を確実に検出できる。この結果、精度よく自己位置を同定することができる。
【００２１】
（３）前記基準物は、不定形物であってもよい。
【００２２】
（４）前記基準物は、樹木の幹であってもよい。
【００２３】
（５）実施形態に係る車両は、上述の自己位置同定システムを備えることができる。車両は、前記自己位置に基づいて自律走行のための制御をするコントローラを備えることができる。
【００２４】
（６）他の観点において、実施形態に係る車両は、複数の樹木の列に沿って自律走行する車両である。車両は、光検出と測距（Light Detection and Ranging（ＬｉＤＡＲ））デバイスを備えることができる。車両は、前記ＬｉＤＡＲデバイスにより測距される領域の画像を撮影するカメラを備えることができる。車両は、前記画像に対する画像認識処理によって、前記画像中における複数の樹木の幹を認識する画像認識器を備えることができる。車両は、前記画像中における複数の前記幹の位置から、前記領域における複数の前記幹それぞれの方向を検出する方向検出器を備えることができる。車両は、前記ＬｉＤＡＲデバイスにより測距された前記領域中の距離データ群から、前記方向検出器によって検出された方向それぞれにおける距離を、複数の前記幹それぞれまでの距離として求める距離検出器を備えることができる。車両は、複数の前記幹それぞれまでの距離に基づいて、前記車両の自己位置を同定する位置同定器を備えることができる。車両は、前記自己位置に基づいて、複数の前記樹木の列に沿って前記車両を自律走行させるコントローラを備えることができる。
【００２５】
（７）前記車両は、前記幹が他の物体によって隠され得る場所を走行するものであってもよい。そのような場所は、例えば、樹木が植えられた農場である。樹木は、例えば、果樹である。他の物体は、例えば、樹木の枝葉又は雑草である。前記画像認識処理は、ディープラーニングによる画像認識処理であってもよい。
【００２６】
（８）実施形態に係る処理装置は、レーザセンサにより測距される領域をカメラで撮影した画像中における１又は複数の基準物を画像認識する処理を実行するように構成されていてもよい。処理装置は、前記画像中における１又は複数の前記基準物の前記画像中における位置から、前記領域における１又は複数の前記基準物の方向を検出する処理を実行するように構成されていてもよい。処理装置は、前記レーザセンサにより測距された前記領域中の距離データ群から、前記検出器によって検出された方向における距離を前記基準物までの距離として求める処理を実行するように構成されていてもよい。
【００２７】
＜２．自己位置同定システムを備える車両の例＞
【００２８】
図１及び図２は、自己位置同定システム１００を備えた車両１０を示している。図２に示す車両１０は、自律走行する無人地上車両（Unmanned Ground Vehicle）である。車両１０は、例えば、果樹園などの農場を走行する。図２に示すように、果樹園では、樹木（果樹）２０が列になって植えられていることが多い。車両１０は、樹木２０の列の間を走行することができる。
【００２９】
図２に示す車両１０は、草刈機１１を牽引する。車両１０は、樹木２０にできるだけ近づきつつも、樹木２０に接触しないように、樹木２０の列に沿って自律走行する。車両１０が、樹木２０の近傍を走行することで、草刈機１１は、樹木２０の近傍に生えている草の刈り取りをすることができる。
【００３０】
実施形態の車両１０は、樹木２０を基準物として自己位置同定をし、樹木２０の列にそって自律走行する。ここで、果樹園等の農場のように自然物の多い環境において、基準物を検出する場合には、オクルージョンが問題となる。オクルージョンは、対象物が他の物体によって隠された状況をいう。ここで、図２の画像Ｃにおいては、樹木２０，２０の幹は比較的明瞭に表れている。しかし、例えば、樹木２０の枝葉２１の成長及び樹木２０，２０付近における雑草の成長により、基準物となる幹が隠されることがある。後述のように、実施形態の車両１０は、オクルージョンが生じる環境下においても、自律走行をすることができる。
【００３１】
図１に示すように、自己位置同定システム１００は、測距のためのレーザセンサ１５として、３次元ＬｉＤＡＲデバイスを備える。ＬｉＤＡＲデバイス１５の視野１７は、車両１０の前方（図２では、走行方向に対してやや左方）に設定されている。視野１７は、上下左右に広がりのある領域である。実施形態のＬｉＤＡＲデバイス１５は、車両１０の斜め前方の領域にレーザ光を照射し、その領域内の各位置の測距をする。図２には、ＬｉＤＡＲデバイス１５によって得られた点群Ｐの表示イメージデータを示した。
【００３２】
システム１００は、カメラ１６を備える。図２に示すように、カメラ１６は、ＬｉＤＡＲデバイス１５により測距される領域を撮影できるように、ＬｉＤＡＲデバイスとほぼ同じ位置に設けられている。カメラ１６の視野も、車両１０の前方（図２では、走行方向に対してやや左方）に設定されている。実施形態のカメラ１６は、車両１０の斜め前方を撮影する。図２には、カメラ１６によって撮影された画像Ｃを示した。
【００３３】
実施形態において、ＬｉＤＡＲデバイス１５及びカメラ１６は、車両１０の斜め前方にある同じ樹木２０，２０の列が視野１７，１８に含まれるように設置されている。車両１０は、視野１７，１８に含まれる樹木２０，２０の幹を基準物として自己位置同定をし、樹木２０，２０の列に沿って自律走行する。
【００３４】
図１に示すように、システム１００は、処理装置３０を備える。処理装置３０は、プロセッサ３１と記憶装置３２とを備えるコンピュータによって構成されている。プロセッサ３１は、記憶装置３２に記憶されたコンピュータプログラム５０を実行することにより、自己位置同定に関連する処理を実行する（図３参照）。また、プロセッサ３１は、コンピュータプログラム５０を実行することにより、車両１０の自律走行のための処理を実行する。自律走行のための処理は、車両１０の操舵システム６０の制御を含む。
【００３５】
図１には、プロセッサ３１によって実行される処理４１，４２，４３，４４が示されている。これらは、自己位置同定に関連する処理である。
【００３６】
処理４１は、画像認識処理である。画像認識処理４１は、カメラ１６によって撮影された画像Ｃに含まれる特定の物体を認識する処理である。画像認識処理４１を実行するプロセッサ３１は、画像認識器として機能する。
【００３７】
実施形態においては、画像認識としては、ディープラーニングによる画像認識処理が用いられる。ディープラーニングによる画像認識処理は、樹木の幹のように、形態が一定でないが同じ種類のものを高精度で認識するのに適している。実施形態においては、画像Ｃに含まれる特定の果樹２０（例えばリンゴの木）の幹が、画像認識される。ディープラーニングによる画像認識処理では、幹が、枝葉２１や雑草で多少隠されているようなオクルージョンの環境でも、幹を検出できるため有利である。
【００３８】
処理４２は、方向検出処理である。実施形態の方向検出処理４２は、画像Ｃ中における幹の位置から、その幹の方向を検出する。幹の方向は、例えば、画像Ｃにおける幹の位置座標から計算することができる。方向検出処理４２を実行するプロセッサ３１は、方向検出器として機能する。
【００３９】
処理４３は、距離検出処理である。実施形態の距離検出処理４３は、方向検出処理４２により特定した幹の方向を使って、ＬｉＤＡＲデバイス１５により測距された点群から、幹までの距離を検出する処理である。距離検出処理４３を実行するプロセッサ３１は、距離検出器として機能する。
【００４０】
処理４４は、位置同定処理である。実施形態の位置同定処理４４は、複数の幹それぞれまでの距離に基づいて、システム１００（車両１０）の自己位置を同定する処理である。位置同定処理４４を実行するプロセッサ３１は、位置同定器として機能する。
【００４１】
プロセッサ３１は、車両１０の自律走行のための処理を実行することにより、車両１０の自律走行を制御するコントローラ４５として機能する。コントローラ４５は、経路生成処理４６を実行する。経路生成処理４６では、自己位置と幹（樹木２０）までの距離とに基づいて、樹木２０にできるだけ近づきつつも樹木２０に接触しない経路が生成される。
【００４２】
コントローラ４５は、生成された経路に沿って、車両１０を自律走行させる。自律走行のため、コントローラ４５は、車両１０の操舵システム６０に舵角などの自律走行用パラメータを与える。経路に沿った自律走行のための制御手法として、実施形態においては、Path Following制御４７が用いられる。
【００４３】
＜３．ＬＩＤＡＲと画像とを融合した自己位置同定＞
【００４４】
図３は、ＬＩＤＡＲと画像とを融合した自己位置同定の手順を示している。自己位置同定は、車両１０の走行中に行われる。車両１０の走行中においては、カメラ１６及びＬｉＤＡＲデバイス１５は常時動作する。カメラ１６により撮影された画像（動画）は、記憶装置３２に保存され、ＬｉＤＡＲデバイス１５により測距された点群のデータも、記憶装置３２に保存される。
【００４５】
自己位置同定においては、まず、ステップＳ１１において、カメラ１６で撮影された画像Ｃに対して、基準物となる幹を認識する画像認識が行われる。
【００４６】
図４は、画像Ｃ１，Ｃ２，Ｃ３，Ｃ４を示す。画像Ｃ１は、カメラ１６で撮影された画像であり、樹木の幹の根元付近が写っている。画像Ｃ１，Ｃ３，Ｃ４はすべて同じ画像であり、画像Ｃ２は、画像Ｃ１における幹の根元付近の拡大画像である。画像Ｃ２に示すように、根接ぎが行われている果樹の場合、幹の根元付近に根接ぎにより幹が肥大化したコブ２３が存在する。画像認識による検出対象を幹の根元付近とすることで、ディープラーニングなどの機械学習の際に、コブ２３が特徴量として効率的に認識され、検出精度を容易に高めることができる。また、幹の根元付近は、枝葉２１が少ないため、枝葉２１によるオクルージョンを回避する上でも有利である。
【００４７】
画像Ｃ３は、幹の根元付近の認識結果を示している。画像Ｃ３において、白枠の領域Ｓ０，Ｓ１，Ｓ２は、画像認識により幹の根元付近であると認識された範囲を示す。画像Ｃ３においては、３つの幹が認識されていることがわかる。画像Ｃ３においては、車両１０側（画像Ｃ３の左側）からみて最も近い幹に対応するのが、領域Ｓ０である。領域Ｓ１、領域Ｓ２は、車両１０からみて順次遠い位置にある。
【００４８】
続いて、ステップＳ１２において、認識された３つの幹（基準物）のうち、車両１０からみて最も近い２つの幹（領域Ｓ０，Ｓ１）が選択される。選択された二つの幹が、自己位置同定に用いられる。
【００４９】
ステップＳ１３において、選択された２つの幹（基準物）の画像座標から、２つの幹（基準物）それぞれの方向が計算される。幹２３の方向は、例えば、図４の画像Ｃ４における幹（領域Ｓ０，Ｓ１）の画像座標Ｓ０（Ｘ，Ｙ），Ｓ１（Ｘ，Ｙ）から、カメラ１６のパラメータ（焦点距離ｆなど）を用いて、計算される。幹２３の方向は、例えば、図５に示すｘ方向の角度θ、及び、ｙ方向の角度φ（図示省略）として計算される。なお、図５のｘ方向は、図４の画像Ｃ４のＸ方向に対応し、図５のｙ方向は、図４の画像Ｃ４のＹ方向に対応する。図５のｚ方向は、カメラ１６から画像Ｃ１，Ｃ２，Ｃ３，Ｃ４の画像平面に向かう方向である。なお、ｘ方向の角度θとは、図５のｘｚ平面において、ｚ軸に対してなす角度であり、ｙ方向の角度φとは、図５のｙｚ平面において、ｚ軸に対してなす角度である。
【００５０】
ステップＳ１４において、ＬｉＤＡＲ測距領域全体の点群のデータから、幹（基準物）それぞれの方向における小領域ｓ０，ｓ１の点群が取得される。図６は、画像Ｃ１，Ｃ３，Ｃ４と同じタイミングにおいて、ＬｉＤＡＲデバイスにより測距された点群Ｐ１，Ｐ２の表示イメージを示している。図６の点群Ｐ１，Ｐ２はいずれも同じものである。点群Ｐ２の表示イメージにおいては、幹の方向にある小領域ｓ０，ｓ１が示されている。小領域ｓ０は、画像Ｃ４において画像座標Ｓ０（Ｘ，Ｙ）で示される幹の方向に対応する小領域であり、小領域ｓ１は、画像Ｃ４において画像座標Ｓ１（Ｘ，Ｙ）で示される幹の方向に対応する小領域である。
【００５１】
ステップＳ１５において、小領域ｓ０内の点群（距離データ群）から、画像座標Ｓ０（Ｘ，Ｙ）で示される幹までの距離が計算され、小領域ｓ１内の点群（距離データ群）から、画像座標Ｓ１（Ｘ，Ｙ）で示される幹までの距離ｄ０，ｄ１が計算される。幹までの距離ｄ０，ｄ１は、例えば、小領域内の点群で示される各距離の平均距離として算出される。小領域ｓ０，ｓ１は、幹の方向に存在する領域であるから、小領域ｓ０，ｓ１内の点群で示される距離は、幹までの距離ｄ０，ｄ１であることが期待される。
【００５２】
ステップＳ１６において、２つの幹までの距離ｄ０，ｄ１に基づいて、車両１０の自己位置が同定される。実施形態においては、図７に示す幾何学的関係に基づいて、車両１０の自己位置が同定される。なお、以下では、ｓ０，ｓ１を、幹の位置を示す記号として用いる。
【００５３】
図７においては、２つの幹の位置ｓ０，ｓ１を結ぶ直線Ｌ１の方向をｘ方向とし、ｘ方向に直交する方向をｙ方向とする。実施形態の自己位置推定においては、最も手前にある幹の位置ｓ０を原点に設定する。図７に示す幾何学的関係から、車両１０の自己位置θ，ｙは、次の式で計算することができる。
【数１】
2019152924.tif 000003
【００５４】
車両１０の走行中において、カメラ１６による撮影とＬｉＤＡＲデバイス１５による測距は継続して行われるため、位置同定も、走行中において繰り返し行われる。車両１０が走行することで、カメラ１６で撮影した画像Ｃ中の幹（基準物）の位置が変化する。画像認識処理４１では、画像中の幹（基準物）の位置が変化した場合には、同一の幹の位置の変化に追従して、幹の領域Ｓ０，Ｓ１，Ｓ２を移動させる。
【００５５】
方向検出処理４２では、幹の位置の変化に応じて、幹の方向を更新する。距離検出処理４３では、その時点での点群Ｐのデータから、更新された方向における小領域ｓ０，ｓ１を特定し、２つの幹までの距離を再算出する。自己位置同定処理４４では、２つの幹までの距離から同定される自己位置を更新する。
【００５６】
車両１０の走行により、最も手前に見えていた幹（位置ｓ０）を見失った場合、一つ先の幹の位置ｓ１に原点を移動させ、原点となる幹を含む２つの幹の位置ｓ１，ｓ２から、自己位置を同定する。これにより、車両１０の走行中において、常に自己位置を同定することができる。
【００５７】
図８は、図４とは別の果樹園で撮影された画像Ｃ５，６を示す。画像Ｃ５，Ｃ６は同じものである。図９は、画像Ｃ５，６と同じタイミングにおいて、ＬｉＤＡＲデバイス１５により測距された点群Ｐ３，Ｐ４の表示イメージを示している。点群Ｐ３，Ｐ４も同じものである。
【００５８】
画像認識処理４１では、画像Ｃ５，Ｃ６中の２つ幹が、画像Ｃ６に示す２つの領域Ｓ０，Ｓ１内に存在していることが認識される。つまり、画像認識処理４１では、問題なく幹が認識された。
【００５９】
一方、図９の点群Ｐ３の表示イメージをみても、２つの幹の位置は判然としない。したがって、点群Ｐ３が計測された環境においては、ＬｉＤＡＲ  ＳＬＡＭで自己位置同定をするのは困難であることがわかる。
【００６０】
これに対して、実施形態の手法では、画像認識により幹の方向が特定できているため、その幹の方向に対応する領域ｓ０，ｓ１の点群を用いることにより、容易に幹までの距離を計測することができる。
【００６１】
＜４．付記＞
本発明は、上記実施形態に限定されるものではなく、様々な変形が可能である。
【符号の説明】
【００６２】
１０  車両
１１  草刈機
１５  レーザセンサ（ＬｉＤＡＲデバイス）
１６  カメラ
１７  レーザセンサ視野
１８  カメラ視野
２０  果樹（樹木）
２１  枝葉
２３  コブ（幹）
３０  処理装置
３１  プロセッサ
３２  記憶装置
４１  画像認識処理
４２  方向検出処理
４３  距離検出処理
４４  位置同定処理
４５  コントローラ
４６  経路生成
４７  Path Following制御
５０  コンピュータプログラム
６０  操舵システム
１００  自己位置同定システム
Ｃ  画像
Ｃ１，Ｃ２，Ｃ３，Ｃ４，Ｃ５，Ｃ６  画像
Ｐ  点群（距離データ群）
Ｐ１，Ｐ２，Ｐ３，Ｐ４  点群（距離データ群）
Ｓ０，Ｓ１，Ｓ２  画像における基準物領域
Ｓ０（Ｘ，Ｙ），Ｓ１（Ｘ，Ｙ）  幹（基準物）の画像座標
ｓ０，ｓ１  基準物の方向における小領域
ｓ０（ｘ，ｙ），ｓ１（ｘ，ｙ）
【図１】
2019152924.tif 000004
【図２】
2019152924.tif 000005
【図３】
2019152924.tif 000006
【図４】
2019152924.tif 000007
【図５】
2019152924.tif 000008
【図６】
2019152924.tif 000009
【図７】
2019152924.tif 000010
【図８】
2019152924.tif 000011
【図９】
2019152924.tif 000012