特開 2024-155104 姿勢算出システム及び姿勢算出方法

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

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

(11)【公開番号】P2024155104

(43)【公開日】2024-10-31

(54)【発明の名称】姿勢算出システム及び姿勢算出方法

(51)【国際特許分類】

   G01S 17/89 20200101AFI20241024BHJP

【ＦＩ】

G01S17/89

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2023069530

(22)【出願日】2023-04-20

(71)【出願人】

【識別番号】000001052

【氏名又は名称】株式会社クボタ

(74)【代理人】

【識別番号】110001818

【氏名又は名称】弁理士法人Ｒ＆Ｃ

(72)【発明者】

【氏名】玉井 拓也

(72)【発明者】

【氏名】荒川 侑摩

(72)【発明者】

【氏名】長尾 充朗

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

5J084

【Ｆターム（参考）】

5J084AA05

5J084AA13

5J084AB20

5J084AC02

5J084AC08

5J084BA03

5J084DA01

5J084DA07

5J084DA09

(57)【要約】

【課題】簡単で、信頼性の高い、点群データを用いた姿勢検出技術を提供する。
【解決手段】姿勢算出システムは、前記被姿勢算出体に設けられ、固定目標体９に対する距離測定結果としての測定点群データを取得する点群データ取得ユニット５１と、固定目標体９の形状に対応するテンプレート点群データによって構成された形状テンプレートを基準形状テンプレートとして形状テンプレート記憶部５３から読み出す基準形状テンプレート読み出し部５２と、同一座標空間において形状対応するように配置された基準形状テンプレートと測定点群データとの空間的な分布を照合する照合部５４と、照合部５４の照合結果に基づいて被姿勢算出体の固定目標体９に対する姿勢データを算出する姿勢データ算出部５５を備える。
【選択図】図１

【特許請求の範囲】

【請求項1】

固定目標体に対する被姿勢算出体の姿勢を算出する姿勢算出システムであって、
前記被姿勢算出体に設けられ、前記固定目標体に対する距離測定結果としての測定点群データを取得する点群データ取得ユニットと、
テンプレート点群データによって構成された形状テンプレートを記憶する形状テンプレート記憶部と、
前記固定目標体の形状に対応する前記形状テンプレートを基準形状テンプレートとして前記形状テンプレート記憶部から読み出す基準形状テンプレート読み出し部と、
同一座標空間において形状対応するように配置された前記基準形状テンプレートと前記測定点群データとの空間的な分布を照合する照合部と、
前記照合部の照合結果に基づいて前記被姿勢算出体の前記固定目標体に対する姿勢データを算出する姿勢データ算出部と、
を備える姿勢算出システム。

【請求項2】

前記照合部で用いられる前記測定点群データは、前記固定目標体の複数位置での横断面輪郭に対応するデータであり、前記基準形状テンプレートの形状は前記横断面輪郭に対応する形状である請求項１に記載の姿勢算出システム。

【請求項3】

前記姿勢データ算出部は、所定時間間隔で、または所定移動距離毎に取得された前記測定点群データを用いた照合結果に基づいて、前記姿勢データを算出する請求項２に記載の姿勢算出システム。

【請求項4】

前記測定点群データは前記固定目標体の三次元形状に対応し、前記基準形状テンプレートの形状は前記固定目標体の三次元輪郭形状に相当する請求項１に記載の姿勢算出システム。

【請求項5】

前記照合部は、前記測定点群データと前記基準形状テンプレートとの位置合わせのための位置合わせベクトルを算出する請求項１に記載の姿勢算出システム。

【請求項6】

前記基準形状テンプレート読み出し部は、前記固定目標体に対する画像認識によって、前記形状テンプレートを自動的に選択する請求項１から５のいずれか一項に記載の姿勢算出システム。

【請求項7】

固定目標体に対する被姿勢算出体の姿勢を算出する姿勢算出方法であって、
前記被姿勢算出体に設けられた距離センサによって、前記固定目標体の測定点群データを取得する点群データ取得ステップと、
テンプレート点群データによって構成される形状テンプレートを記憶する形状テンプレート記憶部から前記固定目標体の形状に対応する前記形状テンプレートを基準形状テンプレートとして読み出す基準形状テンプレート読み出しステップと、
同一座標空間に形状対応するように配置された前記基準形状テンプレートと前記測定点群データとの空間的な分布を照合する照合ステップと、
前記照合ステップにおける照合結果に基づいて前記被姿勢算出体の前記固定目標体に対する姿勢データを算出する姿勢データ算出ステップと、
を備える姿勢算出方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、建物、ガードレール、圃場の畦や畝などの固定目標体に対する、車両や歩行者などの被姿勢算出体の姿勢を算出する姿勢算出システム及び姿勢算出方法に関する。

【背景技術】

【0002】

特許文献１には、圃場の畝を跨いで畝の両隣の走行面を走行する農作業車両を畝に沿って自律走行させる自律走行システムが開示されている。この自律走行システムは、農作業車両に搭載されたＬｉＤＡＲと、農作業車両を畝に沿うように走行させる制御装置とを備えている。ＬｉＤＡＲの検出領域は、畝及び当該畝の両隣の走行面を含む。制御装置は、２条の走行面に関するＬｉＤＡＲからの情報に基づいて、畝の中心線を推定するとともに、推定された畝の中心線と農作業車両の中心線との誤差に基づいて、農作業車両を畝に沿うように走行させる

【0003】

特許文献２には、点群データを用いて車両の位置を推定する車載処理装置が開示されている。この車載処理装置は、物体の一部を表す点の第１座標系（駐車場座標系）における座標が複数含まれる点群データ（駐車場点群）が格納される記憶部と、車両の周囲の情報を取得するカメラの出力を取得するセンサ入力部と、車両の移動に関する情報を取得する移動情報取得部と、第２座標系（局所座標系）における車両の位置および物体の一部を表す点の第２座標系における座標が複数含まれる局所周辺情報を生成する局所周辺情報作成部と、駐車場点群および局所周辺情報に基づき駐車場座標系と局所座標系の関係を推定し、第１座標系における車両の位置を推定する位置推定部とを備える。

【先行技術文献】

【特許文献】

【0004】

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

【特許文献2】特開２０１８－００４３４３号公報

【発明の概要】

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

【0005】

特許文献１による自律走行システムでは、畝の形状及び畝の両隣に位置する走行面を検出する必要があるので、畝と２つの走行面とを含む検出領域が大きくなり、検出系及び演算系に負担がかかるという問題点がある。特許文献２による車載処理装置では、第１座標系の点群データと第２座標系の点群データとの照合を行う前に要求される前処理が複雑であることから、より簡便な姿勢算出技術が要望されている。

【0006】

上記実情に鑑み、本発明の目的は、より簡単で、信頼性の高い、点群データを用いた姿勢検出技術を提供することである。

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

【0007】

本発明による姿勢算出システムは、固定目標体に対する被姿勢算出体の姿勢を算出するシステムであり、前記被姿勢算出体に設けられ、前記固定目標体に対する距離測定結果としての測定点群データを取得する点群データ取得ユニットと、テンプレート点群データによって構成された形状テンプレートを記憶する形状テンプレート記憶部と、前記固定目標体の形状に対応する前記形状テンプレートを基準形状テンプレートとして前記形状テンプレート記憶部から読み出す基準形状テンプレート読み出し部と、同一座標空間において形状対応するように配置された前記基準形状テンプレートと前記測定点群データとの空間的な分布を照合する照合部と、前記照合部の照合結果に基づいて前記被姿勢算出体の前記固定目標体に対する姿勢データを算出する姿勢データ算出部とを備える。

【0008】

この構成によれば、特定場所に固定配置された固定目標体（道路沿いのガードレールや塀、圃場の畦や畝など）に対する被姿勢算出体（歩行者や車両など）の姿勢が求められた場合、固定目標体の形状に対応する形状テンプレートが形状テンプレート記憶部から基準形状テンプレートとして読み出され、当該基準形状テンプレート（テンプレート点群データ）及び固定目標体に対する距離測定結果である測定点群データの、同一座標空間における空間的な分布が照合される。この照合結果に基づいて、つまり、テンプレート点群データと測定点群データとの重なり具合に基づいて、被姿勢算出体の固定目標体に対する姿勢の一致度（つまりずれ具合）が算出される。この構成では、被姿勢算出体の目標物としての固定目標体の形状に適合した形状テンプレートが選択され、この選択された形状テンプレートが基準形状テンプレートとして、測定点群データの照合相手となるので、簡単かつ信頼性の高い姿勢検出が可能となる。

【0009】

本発明では、前記照合部で用いられる前記測定点群データは、前記固定目標体の複数位置での横断面輪郭に対応するデータであり、前記基準形状テンプレートの形状は前記横断面輪郭に対応する形状であることが提案される。固定目標体に対する被姿勢算出体の遠近方向での複数位置での測定点群データとテンプレート点群データとの複数の照合結果から、姿勢データを算出することで、姿勢検出の信頼性が向上する。さらに、この構成では、固定目標体が、場所によってその形状が異なるような場合でも、固定目標体の複数箇所での測定点群データとテンプレート点群データとを用いることにより、より信頼性の高い姿勢検出が可能となる。

【0010】

固定目標体に沿った移動中における異なる時点で取得された測定点群データを用いることで、複数の位置での固定目標体の横断面輪郭を取得することが可能である。このことから、本発明の別な形態では、前記姿勢データ算出部は、所定時間間隔で、または所定移動距離毎に取得された前記測定点群データを用いた照合結果に基づいて、前記姿勢データを算出することも提案される。

【0011】

固定目標体が同じ横断面で延びているような形状ではなく、表面が平坦でないような形状の場合、固定目標体の三次元形状に対応する測定点群データと、その固定目標体の三次元形状に対応する基準形状テンプレートとを用いることで、良好な姿勢検出が可能となる。このことから、本発明では、前記測定点群データは前記固定目標体の三次元形状に対応し、前記基準形状テンプレートの形状は前記固定目標体の三次元輪郭形状に相当することが提案される。

【0012】

本発明では、前記照合部は、前記測定点群データと前記基準形状テンプレート（テンプレート点群データ）との位置合わせのための位置合わせベクトルを算出するように構成されることが提案される。点群データ同士のマッチングは、ベクトル演算アルゴリズムを用いて簡単に行うことができ、演算結果としての位置合わせベクトルは、測定点群データと基準形状テンプレートとの位置ずれ、つまり固定目標体に対する非姿勢算出体の位置ずれを示すことになるので、好都合である。そのようなベクトル演算アルゴリズムとして、ＩＣＰ(Iterative Closest Point)アルゴリズム、あるいはＩＣＰの改善版が適している。

【0013】

基準形状テンプレート読み出し部は、固定目標体の形状が単純で一定の横断面形状で延びているような場合、適正な形状テンプレートを人為的に選択することが可能である。しかしながら、固定目標体の外形や横断面形状の視認が困難な場合、あるいは人為的な選択操作が煩わしい場合、基準形状テンプレート読み出し部が自動的に適正な形状テンプレートを選択することが要望される。このような適正な形状テンプレートの選択は、固定目標体に対する画像認識結果に基づいて行われると、好都合である。このことから、本発明では、前記基準形状テンプレート読み出し部は、前記固定目標体に対する画像認識によって、前記形状テンプレートを自動的に選択することが提案される。画像認識のためのソース画像データとして、測定点群データの利用が可能であるが、カメラの撮影画像が用いられてもよい。

【0014】

本願は、上述した姿勢算出システムだけでなく、同様の目的と作用・効果を有する姿勢算出方法も発明対象としている。例えば、そのような姿勢算出方法は、固定目標体に対する被姿勢算出体の姿勢を算出する方法であり、前記被姿勢算出体に設けられた距離センサによって、前記固定目標体の測定点群データを取得する点群データ取得ステップと、テンプレート点群データによって構成される形状テンプレートを記憶する形状テンプレート記憶部から前記固定目標体の形状に対応する前記形状テンプレートを基準形状テンプレートとして読み出す基準形状テンプレート読み出しステップと、同一座標空間に形状対応するように配置された前記基準形状テンプレートと前記測定点群データとの空間的な分布を照合する照合ステップと、前記照合ステップにおける照合結果に基づいて前記被姿勢算出体の前記固定目標体に対する姿勢データを算出する姿勢データ算出ステップとを備える。姿勢算出システムにおいて上述した種々の提案形態もこの姿勢算出方法に適用可能である。さらには、上述した姿勢算出システムや姿勢算出方法が適用された車両やナビゲータ装置も本願の発明対象である。

【図面の簡単な説明】

【0015】

【図1】姿勢算出システムの基本構成を示す機能ブロック図である。

【図2】畝成形機を装備したトラクタの側面図である。

【図3】姿勢算出システムを搭載したトラクタの機能ブロック図である。

【図4】畝に対する距離測定結果としての測定点群データを示す模式図である。

【図5】畝の測定点群データと基準形状テンプレートとを示す模式図である。

【図6】照合開始時の測定点群データと基準形状テンプレートとの関係を示す模式図である。

【図7】照合完了時の測定点群データと基準形状テンプレートとの関係を示す模式図である。

【図8】台形畝を成形する畝成型機を装備したトラクタの背面図である。

【図9】台形畝に対する姿勢算出処理における測定点群データと基準形状テンプレートとの関係を示す模式図である。

【図10】姿勢算出システムを搭載した野菜移植機の側面図である。

【図11】野菜移植機の後面図である。

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

【0016】

図１は、固定目標体９に対する被姿勢算出体の姿勢を算出する姿勢算出システムの基本構成を示す機能ブロック図である。ここでの固定目標体９とは延設された構造体であり、例えば、道路沿いのガードレールや塀、あるいは圃場の畦や畝などである。被姿勢算出体とは、固定目標体９に沿って移動する歩行者や車両などである。被姿勢算出体が歩行者の場合、姿勢算出システムはナビゲータ等のモバイル機器に組み込まれ、歩行者に装着される。被姿勢算出体が車両の場合、姿勢算出システムは車両に搭載され、自動操舵の制御量算出のために、あるいは操縦者を支援する操舵ガイダンスのために利用される。

【0017】

姿勢算出システムの基本構成要素は、点群データ取得ユニット５１、形状テンプレート記憶部５３、基準形状テンプレート読み出し部５２、照合部５４、姿勢データ算出部５５である。

【0018】

点群データ取得ユニット５１は、ＴｏＦカメラ、ステレオカメラ、ＬｉＤＡＲなどから構成される距離センサ３から出力される固定目標体９に対する測定点群データを取得する。ビームを用いた距離センサ３は、固定目標体９にビームを照射し、物体から反射した反射ビームを受信し、反射物体までの距離を算出するセンサである。ビームには、超音波、レーザ光、電磁波が用いられる。いずれにせよ、距離センサ３は、照準先の空間における反射物体としての固定目標体９の表面の形状に対応する点群データを生成し、点群データ取得ユニット５１に与える。取得された点群データは、測定点群データとして照合部５４に送られる。

【0019】

形状テンプレート記憶部５３は、固定目標体９の形状に対応するようにテンプレート点群データによって構成された形状テンプレートを記憶する。形状テンプレートは、別な装置によって作成され、形状テンプレート記憶部５３に格納されてもよいし、入力データに基づいて所望の形状テンプレートを生成する形状テンプレート生成部が備えられてもよい。

【0020】

基準形状テンプレート読み出し部５２は、照合部５４における測定点群データの照合相手となる基準形状テンプレートを形状テンプレート記憶部５３から選択して読み出す。基準形状テンプレート読み出し部５２は、距離センサ３によって捕捉されている固定目標体９の形状（例えば横断面輪郭形状）に対応する形状を有する形状テンプレートを選択し、基準形状テンプレートとして読み出す。この選択は、人為的に入力された指令によって行われてもよいが、入力情報処理部４１を介して送られてくる固定目標体９に関する形状や識別符号を含む固定目標情報に基づいて、自動的に行われてもよい。あるいは、測定点群データまたは固定目標体９の撮影画像に対する画像認識に基づいて固定目標体９の形状が推定され、その推定に基づいて形状テンプレートが選択されてもよい。

【0021】

照合部５４は、測定点群データと基準形状テンプレート（テンプレート点群データ）とを、固定目標体９と被姿勢算出体との存在位置を表す座標空間（例えば、距離センサ３の照射点を原点とする座標空間）に配置する。照合部５４は、同一座標空間において形状対応するように配置された基準形状テンプレートと測定点群データとの空間的な分布を照合する。その際、基準形状テンプレートは、実際の固定目標体９の位置に対応するように配置される。照合開始時の固定目標体９の位置（出発位置）は、予め人為的に、または撮影画像を用いて自動的に設定することが好ましい。照合部５４は、測定点群データと基準形状テンプレートの空間的な分布を照合する処理として、基準形状テンプレートに測定点群データを重ね合わせる処理を行う。この重ね合わせ処理プログラムには、ＩＣＰ(Iterative Closest Point)アルゴリズムが好適に用いられる。この重ね合わせ処理の結果（照合結果）として、重ね合わせのための位置合わせベクトルが算出される。この位置合わせベクトルは姿勢データ算出部５５に送られる。

【0022】

位置合わせベクトルは距離センサ３を装備した被姿勢算出体の固定目標体９に対する姿勢ずれを示す。位置合わせベクトルとして、横ずれの解消に必要な平行移動を示す並進ベクトルと、方位ずれの解消に必要な回転を示す回転ベクトルとが用いることができる。姿勢データ算出部５５は、照合部５４によって生成される位置合わせベクトルに基づいて、被姿勢算出体の固定目標体９に対する、横ずれや方位ずれを示す姿勢データを算出する。固定目標体９がガードレールや塀などの道路に沿って延設されている構造物である場合、方位ずれは、固定目標体９の延設方向線に対する角度を示し、横ずれは、固定目標体９の延設方向線から当該延設方向線の垂直線方向の距離を示す。

【0023】

報知部４２は、姿勢データ算出部５５から送られた姿勢データ（横ずれや方位ずれ）に基づいて、横ずれや方位ずれをユーザに報知する。報知部４２は、横ずれや方位ずれを視覚的に報知するためのグラフィック報知データを生成して、ディスプレイ４２ａに送る。また、報知部４２は、横ずれや方位ずれを音声で報知するための音声報知データを生成して、スピーカ４２ｂに送る。

【0024】

次に、本発明による姿勢算出システムの具体的な適用例の１つを説明する。この適用例では、姿勢算出システムは、農用作業車であるトラクタに装備されている。図２は、トラクタの側面図である。なお、以下の説明においては、特に断りがない限り、図２に示す矢印Ｆの方向を「前」、矢印Ｂの方向を「後」とする。また、矢印Ｆと矢印Ｕとで規定される面に垂直な方向を「横」とする。このトラクタは、前輪１ａと後輪１ｂとを備えた車体１０を備えている。車体１０の前領域には原動機１１が搭載され、車体１０の後領域には運転部１３が配置され、車体１０の後端領域には、作業装置を連結するためのリンク機構１２が設けられている。運転部１３には、運転座席１４、ステアリングホイール１５、フロントパネル１６が配置されている。フロントパネル１６の上面には、運転パネル１７が設けられ、フロントパネル１６の内部には、コントローラボックス１８が配置されている。リンク機構１２には、畝成形機２が連結されている。このトラクタは、圃場を走行しながら、畝９０の形成や畝９０の管理を行うことができる。つまり、この適用例では、トラクタが被姿勢算出体であり、固定目標体９が畝９０である。

【0025】

畝成形機２には、距離センサ３として機能するＬｉＤＡＲ３０が設けられている。このＬｉＤＡＲ３０は、畝９０を照準としている。コントローラボックス１８には、姿勢算出システムの主要な機能部とトラクタの主要な制御機能部とを含むトラクタ制御装置が収納されている。

【0026】

図３は、トラクタ制御装置の機能ブロック図である。トラクタ制御装置には、図１で示された姿勢算出システムの構成要素が含まれている。図３の機能ブロック図の説明では、先に説明した機能部の繰り返しとなる説明は省略される。

【0027】

運転パネル１７には、トラクタに対する運転指令や運転情報を入力するための操作入力部、運転者にトラクタの運転状態を表示するメータパネル部、などが含まれている。運転情報には、畝９０に関する畝情報や圃場地図などの圃場情報が含まれている。入力情報処理部４１は、運転パネル１７を通じて入力された畝情報を基準形状テンプレート読み出し部５２に与える。基準形状テンプレート読み出し部５２は、畝情報に基づいて、形成しようとする畝形状、または管理しようとする畝形状に適合する形状テンプレートを選択し、基準形状テンプレートとして照合部５４に与える。基準形状テンプレート読み出し部５２は、適切な畝情報が与えられない場合には、点群データ取得ユニット５１から出力される測定点群データに基づいて、畝形状を推定し、推定された畝形状に適合する形状テンプレートを選択することも可能である。

【0028】

図３には、姿勢算出システムに関係するトラクタの主要な制御機能部として、操舵制御部４３と走行距離算出部４４とだけが示されている。操舵制御部４３は、姿勢データ算出部５５から出力された横ずれ及び方位ずれを解消するように車体１０を自動操舵する。走行距離算出部４４は、図示されていないが、トラクタに装備されている衛星航法機能及び慣性航法機能を用いて、車体１０の位置検知、さらには車体１０の移動距離や車速を算出する。操舵制御部４３は、衛星航法機能及び慣性航法機能を用いた自動操舵も可能である。

【0029】

上述のように構成されたトラクタで畝形成や畝管理を行う際の、車体１０の畝９０に対する簡易的な姿勢算出処理の流れ（制御ステップ）を、図４から図７を用いて説明する。ここでは、畝９０の横断面形状は、略三角形である。

【0030】

（１）距離画像から３か所の畝横断面の抽出
図４は、畝９０に対して斜め上方からレーザ光を照射するＬｉＤＡＲ３０から出力された測定点群データに基づく距離画像と、当該距離画像における特定位置である複数箇所（ここでは３カ所）の畝横断面の外形輪郭を示す横断面グラフとが示されている。横断面グラフでは、測定点群データは、大きな点（黒丸）を用いて模式的に描かれている。図４の距離画像において、y[pix]軸は畝９０の延び方向を示し、x[pix]軸は畝９０を横断する方向を示し、距離画像の濃淡は、ＬｉＤＡＲ３０から畝９０の反射点までの距離を示している。この点群データから畝９０の横断面が三角形で、少なくとも所定距離延びていることが推定できる。図４の横断面グラフにおいて、横軸であるx[pix]軸は畝９０を横断する方向を示し、縦軸は畝９０の高さを示し、符号ＣＬで示された点線は、予め設定されている畝９０の基準面ＣＬ(畝９０の延び方向中心線を通る縦断面)を示す線である。

【0031】

なお、距離画像における特定位置である複数箇所の畝横断面は、１つの測定点群データから求められてもよいし、所定時間間隔で、または所定移動距離毎に取得された測定点群データを用いて求められてもよい。

【0032】

（２）基準形状テンプレートとして三角形の形状テンプレートを選択
図５では、模式的な測定点群データの座標系に、基準形状テンプレート読み出し部５２によって選択された基準形状テンプレートが配置されている。その際、基準形状テンプレートは、底辺がx[pix]軸と平行で（つまり圃場の水平面に平行で）、上側頂点が基準面ＣＬを示す線上に位置するように配置される。基準形状テンプレートは実際にはテンプレート点状データであるが、ここでは、薄く塗られた三角形で示されている。

【0033】

（３）測定点群データに対する基準形状テンプレートの位置合わせ
図６は、測定点群データが作り出す三角形に基準形状テンプレートを合わせる照合過程を示している。ここでは、薄く塗られた三角形で示された基準形状テンプレートが照合過程において濃く塗られた三角形の方に移動している様子が示されている。この照合過程では、測定点群データが作り出す三角形の上側頂点に、基準形状テンプレートが作り出す三角形の上側頂点を一致させる移動が行われる。この照合過程は、３カ所の畝横断面に対して行われる。

【0034】

（４）基準形状テンプレートの３つの頂点位置に基づく姿勢評価直線の生成
図７では、照合過程終了時の各横断面グラフが示され、測定点群データが作り出す三角形の上側頂点に基準形状テンプレートが作り出す三角形の上側頂点（白丸で示されている）が実質的に一致している。また、距離画像においても、基準形状テンプレートが作り出す三角形の上側頂点が大きな白丸で示されている。これらの３つの上側頂点を近似的に結ぶ直線が姿勢評価直線ＡＬとして生成される。この姿勢評価直線ＡＬは、トラクタの走行方向線（車体前後方向線）を示している。

【0035】

（５）姿勢評価直線を用いたトラクタの横ずれと方位ずれの算出
基準面ＣＬを示す線に対する姿勢評価直線ＡＬのずれは、トラクタの走行ずれを表す。俯瞰視において、基準面ＣＬを示す線に対する姿勢評価直線ＡＬの横ずれ（畝９０の延び方向の横断線方向でのずれ）は、トラクタの畝９０に対する横ずれとなり、基準面ＣＬに対する姿勢評価直線ＡＬの角度ずれ（基準面ＣＬを示す線と姿勢評価直線ＡＬとの交差角度）は、トラクタの畝９０に対する方位ずれとなる。

【0036】

上述した姿勢算出処理の流れでは、測定点群データが作り出す三角形の上側頂点に、基準形状テンプレートが作り出す三角形の上側頂点（白丸で示されている）を一致させるだけの移動（照合）を行っていたので、３カ所（２カ所でもよいし、４カ所以上でもよい）の畝横断面で照合を行い、得られた上側頂点を近似的に結ぶ直線である姿勢評価直線ＡＬと基準面ＣＬを示す線との交差角度からトラクタの畝９０に対する方位ずれが算出された。これに代えて、三次元空間で、測定点群データが作り出す三角形に基準形状テンプレートを一致させていく三次元移動（照合）を1カ所の畝横断面だけで行い、この照合のために算出された位置合わせベクトルの平行移動ベクトルからトラクタの畝９０に対する横ずれを求めて、回転ベクトルからトラクタの畝９０に対する方位ずれを求めてもよい。

【0037】

次に、図８と図９とを用いて、トラクタを用いた具体的な畝形成作業における、トラクタの畝９０に対する姿勢算出処理を説明する。図８は、トラクタの後面図であり、畝成形機２がリンク機構１２に連結されている。この畝成形機２は３対の畝立板２ｂと４つのガイド輪２ａとを備えており、トラクタの走行に伴って等脚台形タイプの３列の畝９０を同時に形成する。ＬｉＤＡＲ３０は、中央の畝９０と当該畝９０の両側に位置するガイド輪２ａの走行面とを照準として、斜め上方からレーザビームを照射している。この畝成形機２によって形成される畝９０の横断面は等脚台形であり、この形状と寸法に合致する基準形状テンプレートが選択される。

【0038】

図９の左側には、畝形成時に得られる１列の畝９０とガイド輪２ａの走行面の計測点群データに基づく距離画像が示されている。この距離画像は、ＬｉＤＡＲ３０のビーム照射点から見た畝９０の測定点群データの俯瞰画像であり、ビーム照射点から畝９０の反射点までの距離が濃淡で示されている。この距離画像には、所定間隔で設定された畝９０の３カ所での畝横断面の位置を示す、一点鎖線で描かれた横断面線が示されている。

【0039】

図９の右側の横断面グラフでは、３カ所での畝横断面輪郭（測定点群データ）が薄い実線で示されている。さらに、照合過程における基準形状テンプレートの移動を模式的に示す２つの等脚台形が示されている。この照合過程では、測定点群データによって規定される畝横断面輪郭（台形）に、基準形状テンプレートが合わせられる。つまり、測定点群データと基準形状テンプレートを規定しているテンプレート点群データとの空間的な分布が照合される。この照合過程の終了時における基準形状テンプレートの上辺中点が、図９の右側の距離画像及び図９の左側の３つの畝横断面において、白点で示されている。距離画像における、この３つの白点を近似的に結ぶ直線が姿勢評価直線ＡＬとなるので、基準面ＣＬを示す線に対する姿勢評価直線ＡＬの横ずれが、トラクタの畝９０に対する横ずれとして算出され、基準面ＣＬに対する姿勢評価直線ＡＬの角度ずれが、トラクタの畝９０に対する方位ずれとして算出される。算出された横ずれ及び方位ずれは、操舵制御部４３に与えられ、操舵制御に用いられる。

【0040】

上述した実施形態では、姿勢算出システムは、農用作業車であるトラクタに装備されていたが、次に、姿勢算出システムが農用作業車である移植機に装備された実施形態を説明する。図１０は、移植機の一例としての野菜移植機の側面図であり、図１１は、野菜移植機と畝９０との関係を示す、野菜移植機の後面図である。

【0041】

この野菜移植機の機体要素は、形態は異なっても、実質的に上述したトラクタと同様な機体要素を備えており、そのような機体要素には同じ符号または類似の符号を与えている。また、図１０に示す矢印Ｆの方向を「前」、矢印Ｂの方向を「後」とし、図１０に示す矢印Ｕの方向を「上」、矢印Ｄの方向を「下」とし、図１１に示す矢印Ｌの方向を「左」、矢印Ｒの方向を「右」とする。

【0042】

図１０と図１１とに示されているように、野菜移植機は、前輪１ｃと後輪１ｄとを備えた車体１０の前領域には原動機１１が搭載され、車体１０の中間領域には運転部１３が配置され、車体１０の後端領域には、作業装置を連結するためのリンク機構１２が設けられている。運転部１３には、運転座席１４、ステアリングホイール１５、フロントパネル１６が配置されている。フロントパネル１６の上面には、運転パネル１７が設けられ、フロントパネル１６の内部には、コントローラボックス１８が配置されている。

【0043】

リンク機構１２には、苗植付装置２０が連結されている。苗植付装置２０は、例えば、日本国特開２０２０－２０２８０２号公報に記載されているように、植え付けフレームユニット２０ａ、苗載せ台２０ｂ、植付体２０ｃ、覆土輪２０ｄなどを備えている。野菜移植機は、圃場の畝９０を跨ぎながら、畝９０に沿って走行する際に、苗植付装置２０が畝９０に苗（野菜苗）を植え付ける。ここでは、野菜移植機が被姿勢算出体であり、固定目標体９が畝９０である。

【0044】

苗載せ台２０ｂ、植付体２０ｃ、覆土輪２０ｄは、苗植付装置２０の主構造体である植え付けフレームユニット２０ａによって支持されている。苗載せ台２０ｂから排出された苗は、植付体２０ｃによって畝９０の上面に植え付けされる。植え付けられた苗の左側及び右側の領域は、転動する覆土輪２０ｄによって土寄せされるとともに、押し固められる。

【0045】

野菜移植機の車体１０の前端フレームに、距離センサ３として機能するＬｉＤＡＲ３０が取り付けられている。このＬｉＤＡＲ３０は、車体走行方向前方における畝９０を照準としている。ＬｉＤＡＲ３０の取付は、車体１０の前端フレームに限定されるわけではなく、例えば、車体１０の前部に立設されている門型の予備苗台２０ｅの天井部材に取り付けられてもよい。図１１では、ＬｉＤＡＲ３０は上記２カ所に取り付けられているが、もちろん、どちらか一方でよい。

【0046】

コントローラボックス１８には、姿勢算出システムの主要な機能部と野菜移植機の主要な制御機能部とを含む制御装置が収納されている。姿勢算出システムの主要な機能部と野菜移植機の走行に関する制御機能部は、図３を用いて説明されたトラクタ制御装置での機能部と実質的に同一であるので、その説明は、繰り返しを避けるために省略される。この野菜移植機では、車体走行方向前方における畝９０の測定点群データが取得され、畝９０に対する車体１０の姿勢を示す姿勢データが算出される。野菜移植機が自動操舵される場合には、算出された姿勢データに基づいて、車体１０が自動操舵される。野菜移植機が手動操舵される場合には、算出された姿勢データに基づいて、操縦ガイダンスが作成され、報知されることで、運転者の操縦が支援される。

【0047】

〔別実施の形態〕
（１）上述した実施形態では、被姿勢算出体がトラクタや野菜移植機であり、固定目標体９が畝９０であったが、固定目標体９が畦や圃場に設置された案内帯などであってもよい。被姿勢算出体が自動車や電動車いすであり、固定目標体９が道路に敷設されたブロック帯や歩行案内帯（視覚障害者誘導用ブロック）などであってもよい。

【0048】

（２）上述したトラクタや野菜移植機を被姿勢算出体とした実施形態では、測定点群データが作り出す三次元空間から切り取られた複数の横断面における基準多角形を用いた照合が行われたが、これに代えて、被姿勢算出体の所定距離の移動毎に（または経時的に）取得された測定点群データに基づく複数の横断面での照合がおこなわれてもよい。その際、取り扱われる測定点群データは固定目標体９の三次元形状に対応し、その固定目標体９の三次元輪郭形状に対応する基準形状テンプレートが用いられる。

【0049】

（３）上述したトラクタや野菜移植機を被姿勢算出体とした実施形態では、照合部５４における照合結果として、位置合わせ時の並進ベクトルが用いられていたが、並進ベクトルと回転ベクトルとが用いられてもよい。その場合、並進ベクトルが横ずれを示し、回転ベクトルが方位ずれを示す。

【0050】

（４）上述したトラクタや野菜移植機を被姿勢算出体とした実施形態では、取り扱われる基準形状テンプレートは多角形などの二次元形状体であったが、多面体などの三次元形状体であってもよい。

【0051】

（５）図１及び図３で示された機能ブロック図は、説明目的のものであり、各機能部は、他の機能部と統合されてもよいし、複数の機能部に分割されてもよい。

【0052】

なお、上記実施形態（別実施形態を含む、以下同じ）で開示される構成は、矛盾が生じない限り、他の実施形態で開示される構成と組み合わせて適用することが可能であり、また、本明細書において開示された実施形態は例示であって、本発明の実施形態はこれに限定されず、本発明の目的を逸脱しない範囲内で適宜改変することが可能である。

【産業上の利用可能性】

【0053】

本発明は、固定目標体に対する被姿勢算出体の姿勢を算出する姿勢算出技術に適用可能である。

【符号の説明】

【0054】

２ ：畝成形機
２ａ ：ガイド輪
２ｂ ：畝立板
２０ ：苗植付装置
２０ａ ：植え付けフレームユニット
２０ｂ ：苗載せ台
２０ｃ ：植付体
２０ｄ ：覆土輪
３ ：距離センサ
３０ ：ＬｉＤＡＲ、ＴｏＦカメラ
９ ：固定目標体
９０ ：畝
１０ ：車体
１７ ：運転パネル
１８ ：コントローラボックス
４１ ：入力情報処理部
４２ ：報知部
４２ａ ：ディスプレイ
４２ｂ ：スピーカ
４３ ：操舵制御部
４４ ：走行距離算出部
５１ ：点群データ取得ユニット
５２ ：基準形状テンプレート読み出し部
５３ ：形状テンプレート記憶部
５４ ：照合部
５５ ：姿勢データ算出部
ＡＬ ：姿勢評価直線
ＣＬ ：基準面

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】