特開 2022-75256 座標変換用パラメータ取得方法及び装置、自己位置推定装置

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

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

(11)【公開番号】P2022075256

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

(54)【発明の名称】座標変換用パラメータ取得方法及び装置、自己位置推定装置

(51)【国際特許分類】

   G05D 1/02 20200101AFI20220511BHJP

【ＦＩ】

G05D1/02 J

【審査請求】未請求

【請求項の数】6

【出願形態】ＯＬ

(21)【出願番号】P 2020185931

(22)【出願日】2020-11-06

(71)【出願人】

【識別番号】000003218

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

(74)【代理人】

【識別番号】100088155

【弁理士】

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

(74)【代理人】

【識別番号】100113435

【弁理士】

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

(74)【代理人】

【識別番号】100124062

【弁理士】

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

(74)【代理人】

【識別番号】100148013

【弁理士】

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

(72)【発明者】

【氏名】服部 晋悟

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

5H301

【Ｆターム（参考）】

5H301AA01

5H301AA10

5H301BB07

5H301CC03

5H301CC06

5H301CC10

5H301GG08

5H301GG09

(57)【要約】

【課題】複数のセンサを使用して移動体の自己位置の推定を行う際に、移動体の自己位置の推定精度を確保することができる座標変換用パラメータ取得方法及び装置、自己位置推定装置を提供する。
【解決手段】座標変換用パラメータ取得方法は、ＧＰＳ受信機１１を使用してフォークリフト２の自己位置を推定する第１自己位置推定工程と、レーザセンサ１２を使用してフォークリフト２の自己位置を推定する第２自己位置推定工程と、第１自己位置推定工程において得られたフォークリフト２の第１自己位置と第２自己位置推定工程において得られたフォークリフト２の第２自己位置とに基づいて、ＧＰＳ受信機１１の座標系をレーザセンサ１２の座標系に変換するための座標変換用パラメータを算出するパラメータ算出工程とを有する。
【選択図】図４

【特許請求の範囲】

【請求項1】

第１センサを使用して移動体の自己位置を推定する第１自己位置推定工程と、
第２センサを使用して前記移動体の自己位置を推定する第２自己位置推定工程と、
前記第１自己位置推定工程において推定して得られた前記移動体の第１自己位置と前記第２自己位置推定工程において推定して得られた前記移動体の第２自己位置とに基づいて、前記第１センサの座標系を前記第２センサの座標系に変換するための座標変換用パラメータを算出するパラメータ算出工程とを有する座標変換用パラメータ取得方法。

【請求項2】

前記第１センサ及び前記第２センサは、２次元座標における同じ位置に上下に並んで配置されている請求項１記載の座標変換用パラメータ取得方法。

【請求項3】

前記第１自己位置は、２次元座標における前記移動体の第１座標位置及び第１座標角度を含み、
前記第２自己位置は、前記２次元座標における前記移動体の第２座標位置及び第２座標角度を含み、
前記座標変換用パラメータは、前記第１座標位置と前記第２座標位置との座標位置オフセット量と、前記第１座標角度と前記第２座標角度との座標角度差とを含む請求項１または２記載の座標変換用パラメータ取得方法。

【請求項4】

前記第１自己位置は、２次元座標における前記移動体の第１座標位置を含み、
前記第２自己位置は、前記２次元座標における前記移動体の第２座標位置を含み、
前記座標変換用パラメータは、前記第１座標位置と前記第２座標位置との座標位置オフセット量と、前記移動体の第１座標角度と前記移動体の第２座標角度との座標角度差とを含み、
前記パラメータ算出工程は、複数の地点における前記第１座標位置に基づいて前記第１座標角度を算出すると共に、前記複数の地点における前記第２座標位置に基づいて前記第２座標角度を算出した後、前記座標角度差を算出する工程を有する請求項１または２記載の座標変換用パラメータ取得方法。

【請求項5】

移動体の自己位置を推定するための第１センサを有する第１自己位置推定部と、
前記移動体の自己位置を推定するための第２センサを有する第２自己位置推定部と、
前記第１自己位置推定部により推定して得られた前記移動体の第１自己位置と前記第２自己位置推定部により推定して得られた前記移動体の第２自己位置とに基づいて、前記第１センサの座標系を前記第２センサの座標系に変換するための座標変換用パラメータを算出するパラメータ算出部とを備える座標変換用パラメータ取得装置。

【請求項6】

移動体の自己位置を推定するための第１センサを有する第１自己位置推定部と、
前記移動体の自己位置を推定するための第２センサを有する第２自己位置推定部と、
予め決められた座標変換用パラメータを用いて、前記第１自己位置推定部により推定して得られた前記移動体の第１自己位置を前記第１センサの座標系から前記第２センサの座標系に変換する座標変換部と、
前記座標変換部により変換された前記移動体の第１自己位置と前記第２自己位置推定部により推定して得られた前記移動体の第２自己位置とに基づいて、前記移動体の自己位置を決定する自己位置決定部とを備える自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、座標変換用パラメータ取得方法及び装置、自己位置推定装置に関する。

【背景技術】

【0002】

例えば特許文献１には、自己位置推定装置が記載されている。特許文献１に記載の自己位置推定装置は、ＳＬＡＭを用いてフォークリフトの自己位置を推定する第１自己位置推定器と、ＧＮＳＳを用いてフォークリフトの自己位置を推定する第２自己位置推定器と、第１自己位置推定器及び第２自己位置推定器により得られた２つの位置推定値に基づいて、フォークリフトの自己位置を最終的に推定する最終自己位置推定部を有するコントローラとを備えている。

【先行技術文献】

【特許文献】

【0003】

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

【発明の概要】

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

【0004】

しかしながら、上記従来技術においては、以下の問題点が存在する。即ち、ＳＬＡＭとＧＮＳＳとでは、座標軸が異なることがある。ＧＮＳＳでは、南北方向がＸ軸となる。ＳＬＡＭでは、地図作成時にＸ軸が決定されるが、南北方向を正確に把握することが困難である。このため、ＳＬＡＭとＧＮＳＳとで座標合わせを行うことができない。ＳＬＡＭ及びＧＮＳＳの座標軸が一致していない場合には、フォークリフト等の移動体の自己位置の推定精度の低下につながる。

【0005】

本発明の目的は、複数のセンサを使用して移動体の自己位置の推定を行う際に、移動体の自己位置の推定精度を確保することができる座標変換用パラメータ取得方法及び装置、自己位置推定装置を提供することである。

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

【0006】

本発明の一態様に係る座標変換用パラメータ取得方法は、第１センサを使用して移動体の自己位置を推定する第１自己位置推定工程と、第２センサを使用して移動体の自己位置を推定する第２自己位置推定工程と、第１自己位置推定工程において推定して得られた移動体の第１自己位置と第２自己位置推定工程において推定して得られた移動体の第２自己位置とに基づいて、第１センサの座標系を第２センサの座標系に変換するための座標変換用パラメータを算出するパラメータ算出工程とを有する。

【0007】

このような座標変換用パラメータ取得方法においては、第１センサを使用して移動体の自己位置が推定されると共に、第２センサを使用して移動体の自己位置が推定される。そして、第１センサを使用して得られた移動体の第１自己位置と第２センサを使用して得られた移動体の第２自己位置とに基づいて、第１センサの座標系を第２センサの座標系に変換するための座標変換用パラメータが算出される。その後、実際に第１センサ及び第２センサを使用して移動体の自己位置を推定する際には、座標変換用パラメータを用いて、第１センサを使用して得られた移動体の第１自己位置が第１センサの座標系から第２センサの座標系に変換される。このため、第１センサを使用して得られた移動体の第１自己位置は、第２センサを使用して得られた移動体の第２自己位置と同様に、第２センサの座標系で表されることになる。これにより、移動体の自己位置の推定精度が確保される。

【0008】

第１センサ及び第２センサは、２次元座標における同じ位置に上下に並んで配置されていてもよい。このような構成では、移動体の自己位置が２次元座標で表される場合には、移動体の第１自己位置と移動体の第２自己位置とのオフセット量を後で補正する計算が不要となる。従って、演算処理の簡素化を図ることができる。

【0009】

第１自己位置は、２次元座標における移動体の第１座標位置及び第１座標角度を含み、第２自己位置は、２次元座標における移動体の第２座標位置及び第２座標角度を含み、座標変換用パラメータは、第１座標位置と第２座標位置との座標位置オフセット量と、第１座標角度と第２座標角度との座標角度差とを含んでもよい。このような構成では、移動体の自己位置として、２次元座標における移動体の座標位置及び座標角度が推定される。従って、移動体の位置だけでなく、移動体の姿勢も推定されることになる。

【0010】

第１自己位置は、２次元座標における移動体の第１座標位置を含み、第２自己位置は、２次元座標における移動体の第２座標位置を含み、座標変換用パラメータは、第１座標位置と第２座標位置との座標位置オフセット量と、移動体の第１座標角度と移動体の第２座標角度との座標角度差とを含み、パラメータ算出工程は、複数の地点における第１座標位置に基づいて第１座標角度を算出すると共に、複数の地点における第２座標位置に基づいて第２座標角度を算出した後、座標角度差を算出する工程を有してもよい。このような構成では、複数の地点における移動体の座標位置に基づいて、移動体の座標角度が算出される。従って、精度が高い移動体の座標角度が得られる。

【0011】

本発明の他の態様に係る座標変換用パラメータ取得装置は、移動体の自己位置を推定するための第１センサを有する第１自己位置推定部と、移動体の自己位置を推定するための第２センサを有する第２自己位置推定部と、第１自己位置推定部により推定して得られた移動体の第１自己位置と第２自己位置推定部により推定して得られた移動体の第２自己位置とに基づいて、第１センサの座標系を第２センサの座標系に変換するための座標変換用パラメータを算出するパラメータ算出部とを備える。

【0012】

このような座標変換用パラメータ取得装置においては、第１自己位置推定部の第１センサを使用して移動体の自己位置が推定されると共に、第２自己位置推定部の第２センサを使用して移動体の自己位置が推定される。そして、第１センサを使用して得られた移動体の第１自己位置と第２センサを使用して得られた移動体の第２自己位置とに基づいて、第１センサの座標系を第２センサの座標系に変換するための座標変換用パラメータが算出される。その後、実際に第１センサ及び第２センサを使用して移動体の自己位置を推定する際には、座標変換用パラメータを用いて、第１センサを使用して得られた移動体の第１自己位置が第１センサの座標系から第２センサの座標系に変換される。このため、第１センサを使用して得られた移動体の第１自己位置は、第２センサを使用して得られた移動体の第２自己位置と同様に、第２センサの座標系で表されることになる。これにより、移動体の自己位置の推定精度が確保される。

【0013】

本発明の更に他の態様に係る自己位置推定装置は、移動体の自己位置を推定するための第１センサを有する第１自己位置推定部と、移動体の自己位置を推定するための第２センサを有する第２自己位置推定部と、予め決められた座標変換用パラメータを用いて、第１自己位置推定部により推定して得られた移動体の第１自己位置を第１センサの座標系から第２センサの座標系に変換する座標変換部と、座標変換部により変換された移動体の第１自己位置と第２自己位置推定部により推定して得られた移動体の第２自己位置とに基づいて、移動体の自己位置を決定する自己位置決定部とを備える。

【0014】

このような自己位置推定装置において、第１センサを有する第１自己位置推定部及び第２センサを有する第２自己位置推定部によって移動体の自己位置を推定する際には、予め決められた座標変換用パラメータを用いて、第１自己位置推定部により推定して得られた移動体の第１自己位置が第１センサの座標系から第２センサの座標系に変換される。このため、第１自己位置推定部により推定して得られた移動体の第１自己位置は、第２自己位置推定部により推定して得られた移動体の第２自己位置と同様に、第２センサの座標系で表されることになる。これにより、移動体の自己位置の推定精度が確保される。

【発明の効果】

【0015】

本発明によれば、複数のセンサを使用して移動体の自己位置の推定を行う際に、移動体の自己位置の推定精度を確保することができる。

【図面の簡単な説明】

【0016】

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

【図2】図１に示された走行制御装置が搭載されたフォークリフトが走行する様子を示す側面図である。

【図3】ＧＮＳＳ座標系及びＳＬＡＭ座標系を示す図である。

【図4】本発明の一実施形態に係る座標変換用パラメータ取得装置を概略的に示すブロック図である。

【図5】図４に示されたパラメータ算出部により実行されるパラメータ算出処理の手順を示すフローチャートである。

【図6】図４に示されたパラメータ算出部により実行されるパラメータ算出処理の手順の変形例を示すフローチャートである。

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

【0017】

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。

【0018】

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

【0019】

フォークリフト２は、荷役を行う移動体である。フォークリフト２は、車体３と、この車体３に回転可能に支持された複数の車輪４と、車体３の前側に配置された荷役装置５とを備えている。荷役装置５は、車体３の前端部に連結されたマスト６と、このマスト６に昇降可能に支持されたフォーク７とを有している。

【0020】

走行制御装置１は、ＧＰＳ受信機１１と、レーザセンサ１２と、自己位置推定用コントローラ１３と、駆動部１４とを備えている。走行制御装置１は、フォークリフト２に搭載されている。

【0021】

ＧＰＳ受信機１１及びレーザセンサ１２は、図２に示されるように、フォークリフト２の車体３の屋根に取り付けられている。ＧＰＳ受信機１１は、レーザセンサ１２上に載置されている。つまり、ＧＰＳ受信機１１及びレーザセンサ１２は、２次元座標（ＸＹ座標）における同じ位置に上下に並んで配置されている。なお、レーザセンサ１２がＧＰＳ受信機１１上に載置されていてもよい。

【0022】

ＧＰＳ受信機１１は、ＧＰＳ衛星１５から送信される電波を受信して、フォークリフト２の現在位置を測定する。ＧＰＳ受信機１１は、主として建物Ｔの外（屋外）においてフォークリフト２の自己位置を推定するための第１センサを構成している。

【0023】

レーザセンサ１２は、フォークリフト２の周囲にレーザＬを照射し、レーザＬの反射光を受光することにより、フォークリフト２の周囲の物体との距離を検出する。レーザセンサ１２としては、例えば２Ｄまたは３Ｄのレーザレンジファインダが使用される。レーザセンサ１２は、主として建物Ｔ内（屋内）においてフォークリフト２の自己位置を推定するための第２センサを構成している。

【0024】

自己位置推定用コントローラ１３は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。自己位置推定用コントローラ１３は、ＧＰＳ受信機１１及びレーザセンサ１２の検出データを入力し、フォークリフト２の自己位置の推定に関する処理を実行し、駆動部１４を制御する。

【0025】

駆動部１４は、特に図示はしないが、例えばフォークリフト２の車輪４を回転させる走行モータと、車輪４を転舵させる操舵モータとを有している。

【0026】

自己位置推定用コントローラ１３は、第１推定演算部２１と、第２推定演算部２２と、パラメータ記憶部２３と、座標変換部２４と、自己位置決定部２５と、駆動制御部２６とを有している。

【0027】

第１推定演算部２１は、ＧＰＳ受信機１１により検出された測位データに基づいて、フォークリフト２の自己位置の推定演算を行う。第１推定演算部２１は、例えばＲＴＫ－ＧＮＳＳ（realtimekinematic-global navigation satellite system）測位法を用いて、フォークリフト２の自己位置の推定演算を行う。ＲＴＫ－ＧＮＳＳ測位法は、ＧＰＳ衛星１５から送信される電波を利用して自己位置推定を行う自己位置推定技術である。

【0028】

第１推定演算部２１により得られたフォークリフト２の自己位置（第１自己位置）は、図３（ａ）に示されるように、ＧＰＳ受信機１１の座標系（ＧＮＳＳ座標系）で表される。ＧＮＳＳ座標系は、緯度及び経度で表される２次元座標系である。ＧＮＳＳ座標系では、南北方向がＸ軸方向であり、東西方向がＹ軸方向である。

【0029】

フォークリフト２の第１自己位置は、ＧＮＳＳ座標系において座標Ｐｇ（ｘｇ，ｙｇ，θｇ）で表される。（ｘｇ，ｙｇ）は、ＧＮＳＳ座標系におけるフォークリフト２のＸＹ座標位置（第１座標位置）に相当する。θｇは、ＧＮＳＳ座標系におけるフォークリフト２の座標角度（第１座標角度）に相当する。θｇは、ＧＮＳＳ座標系におけるＸ軸に対する角度である。

【0030】

ＧＰＳ受信機１１及び第１推定演算部２１は、フォークリフト２の自己位置を推定する第１自己位置推定部を構成している。

【0031】

第２推定演算部２２は、レーザセンサ１２により検出された距離データに基づいて、フォークリフト２の自己位置の推定演算を行う。第２推定演算部２２は、レーザＳＬＡＭ（simultaneous localization andmapping）手法を用いて、フォークリフト２の自己位置の推定演算を行う。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。第２推定演算部２２は、レーザセンサ１２により検出された距離データとフォークリフト２の周囲環境の地図データとをマッチングさせて、フォークリフト２の自己位置の推定演算を行う。

【0032】

第２推定演算部２２により得られたフォークリフト２の自己位置（第２自己位置）は、図３（ｂ）に示されるように、レーザセンサ１２の座標系（ＳＬＡＭ座標系）で表される。ＳＬＡＭ座標系は、地図データに対応する２次元座標系である。ＳＬＡＭ座標系では、地図の作成時にＸ軸が決定される。また、ＳＬＡＭ座標系は、ＧＮＳＳ座標系に対してＹ軸の正負が逆となっている。

【0033】

フォークリフト２の第２自己位置は、ＳＬＡＭ座標系において座標Ｐｓ（ｘｓ，ｙｓ，θｓ）で表される。（ｘｓ，ｙｓ）は、ＳＬＡＭ座標系におけるフォークリフト２のＸＹ座標位置（第２座標位置）に相当する。θｓは、ＳＬＡＭ座標系におけるフォークリフト２の座標角度（第２座標角度）に相当する。θｓは、ＳＬＡＭ座標系におけるＸ軸に対する角度である。

【0034】

レーザセンサ１２及び第２推定演算部２２は、フォークリフト２の自己位置を推定する第２自己位置推定部を構成している。

【0035】

パラメータ記憶部２３は、ＧＮＳＳ座標系をＳＬＡＭ座標系に変換するための座標変換用パラメータを記憶する。座標変換用パラメータは、座標位置オフセット量（ｘ´，ｙ´）と座標角度差θ´とを含んでいる。座標変換用パラメータは、予め決められている。座標変換用パラメータについては、後で詳述する。

【0036】

座標変換部２４は、パラメータ記憶部２３に記憶された座標変換用パラメータを用いて、第１推定演算部２１により得られたフォークリフト２の第１自己位置をＧＮＳＳ座標系からＳＬＡＭ座標系に変換する。

【0037】

具体的には、座標変換部２４は、座標角度差θ´を用いて、ＧＮＳＳ座標系におけるフォークリフト２の座標角度θｇをＳＬＡＭ座標系におけるフォークリフト２の座標角度θに変換する。この時の座標変換は、下記式により行われる。

【数1】

【0038】

また、座標変換部２４は、座標角度差θ´及び座標位置オフセット量（ｘ´，ｙ´）を用いて、ＧＮＳＳ座標系におけるフォークリフト２の座標位置（ｘｇ，ｙｇ）をＳＬＡＭ座標系におけるフォークリフト２の座標位置（ｘ，ｙ）に変換する。この時の座標変換は、下記式により行われる。

【数2】

【0039】

自己位置決定部２５は、座標変換部２４により変換されたフォークリフト２の第１自己位置と第２推定演算部２２により得られたフォークリフト２の第２自己位置とに基づいて、フォークリフト２の自己位置を決定する。このとき、フォークリフト２の自己位置は、ＳＬＡＭ座標系で表される。

【0040】

例えば、自己位置決定部２５は、フォークリフト２が屋外を走行するときは、基本的にはフォークリフト２の第１自己位置をフォークリフト２の最終的な自己位置として決定する。自己位置決定部２５は、フォークリフト２が屋内を走行するときは、基本的にはフォークリフト２の第２自己位置をフォークリフト２の最終的な自己位置として決定する。

【0041】

駆動制御部２６は、自己位置決定部２５により決定されたフォークリフト２の自己位置に応じて、フォークリフト２を目的地に向けて自動的に走行させるように駆動部１４を制御する。

【0042】

ここで、ＧＰＳ受信機１１、レーザセンサ１２、自己位置推定用コントローラ１３の第１推定演算部２１、第２推定演算部２２、パラメータ記憶部２３、座標変換部２４及び自己位置決定部２５は、本実施形態の自己位置推定装置１０を構成している。

【0043】

次に、上述した座標変換用パラメータを取得する座標変換用パラメータ取得方法及び装置について説明する。

【0044】

図４は、本発明の一実施形態に係る座標変換用パラメータ取得装置を概略的に示すブロック図である。図４において、本実施形態の座標変換用パラメータ取得装置３０は、上述した座標変換用パラメータを取得する装置である。

【0045】

座標変換用パラメータ取得装置３０は、上記のＧＰＳ受信機１１と、上記のレーザセンサ１２と、パラメータ取得用コントローラ３１とを備えている。パラメータ取得用コントローラ３１は、ＧＰＳ受信機１１及びレーザセンサ１２と共に、フォークリフト２に搭載されている。

【0046】

パラメータ取得用コントローラ３１は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。パラメータ取得用コントローラ３１は、ＧＰＳ受信機１１及びレーザセンサ１２の検出データを入力し、座標変換用パラメータの取得に関する処理を実行する。

【0047】

パラメータ取得用コントローラ３１は、第１推定演算部３２と、第２推定演算部３３と、パラメータ算出部３４と、パラメータ保存部３５とを有している。

【0048】

第１推定演算部３２は、自己位置推定用コントローラ１３の第１推定演算部２１と同じ機能を有している。第１推定演算部３２は、ＧＰＳ受信機１１を使用してフォークリフト２の自己位置を推定する第１自己位置推定工程を実施する。ＧＰＳ受信機１１及び第１推定演算部３２は、フォークリフト２の自己位置を推定する第１自己位置推定部を構成している。

【0049】

第２推定演算部３３は、自己位置推定用コントローラ１３の第２推定演算部２２と同じ機能を有している。第２推定演算部３３は、レーザセンサ１２を使用してフォークリフト２の自己位置を推定する第２自己位置推定工程を実施する。レーザセンサ１２及び第２推定演算部３３は、フォークリフト２の自己位置を推定する第２自己位置推定部を構成している。

【0050】

パラメータ算出部３４は、第１推定演算部３２により得られたフォークリフト２の第１自己位置と第２推定演算部３３により得られたフォークリフト２の第２自己位置とに基づいて、ＧＮＳＳ座標系をＳＬＡＭ座標系に変換するための座標変換用パラメータを算出するパラメータ算出工程を実施する。

【0051】

図５は、パラメータ算出部３４により実行されるパラメータ算出処理の手順を示すフローチャートである。本処理では、任意の１カ所において、ＧＰＳ受信機１１及びレーザセンサ１２を使用してフォークリフト２の自己位置が推定される。

【0052】

図５において、パラメータ算出部３４は、まず第１推定演算部３２により得られたフォークリフト２の第１自己位置データと第２推定演算部３３により得られたフォークリフト２の第２自己位置データとを取得する（手順Ｓ１０１）。

【0053】

フォークリフト２の第１自己位置は、上述したようにＧＮＳＳ座標系における第１座標位置（ｘｇ，ｙｇ）及び第１座標角度θｇを含んでいる。フォークリフト２の第２自己位置は、上述したようにＳＬＡＭ座標系における第２座標位置（ｘｓ，ｙｓ）及び第２座標角度θｓを含んでいる。パラメータ算出部３４は、フォークリフト２の第１自己位置データ及び第２自己位置データを複数ずつ取得する。

【0054】

続いて、パラメータ算出部３４は、複数の第１自己位置における第１座標位置（ｘｇ，ｙｇ）及び第１座標角度θｇをそれぞれ平均化する（手順Ｓ１０２）。また、パラメータ算出部３４は、複数の第２自己位置における第２座標位置（ｘｓ，ｙｓ）及び第２座標角度θｓをそれぞれ平均化する（手順Ｓ１０３）。

【0055】

続いて、パラメータ算出部３４は、平均化された第１座標角度θｇと平均化された第２座標角度θｓとに基づいて、第１座標角度θｇ及び第２座標角度θｓとの座標角度差θ´を算出する（手順Ｓ１０４）。座標角度差θ´は、第１座標角度θｇと第２座標角度θｓとの差分であり、下記式により算出される。

【数3】

【0056】

続いて、パラメータ算出部３４は、平均化された第１座標位置（ｘｇ，ｙｇ）と平均化された第２座標位置（ｘｓ，ｙｓ）と座標角度差θ´とに基づいて、座標位置オフセット量（ｘ´，ｙ´）を算出する（手順Ｓ１０５）。座標位置オフセット量（ｘ´，ｙ´）は、第１座標位置（ｘｇ，ｙｇ）と第２座標位置（ｘｓ，ｙｓ）との差分であり、下記式により算出される。

【数4】

【0057】

続いて、パラメータ算出部３４は、座標角度差θ´及び座標位置オフセット量（ｘ´，ｙ´）のデータをパラメータ保存部３５に出力する（手順Ｓ１０６）。

【0058】

図４に戻り、パラメータ保存部３５は、パラメータ算出部３４により得られた座標角度差θ´及び座標位置オフセット量（ｘ´，ｙ´）のデータを自己位置推定用コントローラ１３のパラメータ記憶部２３に保存する。

【0059】

以上のように本実施形態にあっては、ＧＰＳ受信機１１を使用してフォークリフト２の自己位置が推定されると共に、レーザセンサ１２を使用してフォークリフト２の自己位置が推定される。そして、ＧＰＳ受信機１１を使用して得られたフォークリフト２の第１自己位置とレーザセンサ１２を使用して得られたフォークリフト２の第２自己位置とに基づいて、ＧＮＳＳ座標系をＳＬＡＭ座標系に変換するための座標変換用パラメータが算出される。その後、実際にＧＰＳ受信機１１及びレーザセンサ１２を使用してフォークリフト２の自己位置を推定する際には、座標変換用パラメータを用いて、ＧＰＳ受信機１１を使用して得られたフォークリフト２の第１自己位置がＧＮＳＳ座標系からＳＬＡＭ座標系に変換される。このため、ＧＰＳ受信機１１を使用して得られたフォークリフト２の第１自己位置は、レーザセンサ１２を使用して得られたフォークリフト２の第２自己位置と同様に、ＳＬＡＭ座標系で表されることになる。これにより、フォークリフト２の自己位置の推定精度が確保される。

【0060】

また、本実施形態では、ＧＰＳ受信機１１及びレーザセンサ１２は、２次元座標における同じ位置に上下に並んで配置されている。このため、フォークリフト２の第１自己位置とフォークリフト２の第２自己位置とのオフセット量を後で補正する計算が不要となる。従って、演算処理の簡素化を図ることができる。

【0061】

また、本実施形態では、フォークリフト２の自己位置として、２次元座標におけるフォークリフト２の座標位置及び座標角度が推定される。従って、フォークリフト２の位置だけでなく、フォークリフト２の姿勢も推定されることになる。

【0062】

図６は、パラメータ算出部３４により実行されるパラメータ算出処理の手順の変形例を示すフローチャートである。本処理では、任意の２カ所（地点Ａ，Ｂとする）において、ＧＰＳ受信機１１及びレーザセンサ１２を使用してフォークリフト２の自己位置が推定される。

【0063】

図６において、パラメータ算出部３４は、まず地点Ａにおいて、第１推定演算部３２により得られたフォークリフト２の第１自己位置データと第２推定演算部３３により得られたフォークリフト２の第２自己位置データとを取得する（手順Ｓ１１１）。パラメータ算出部３４は、フォークリフト２の第１自己位置及び第２自己位置データを複数ずつ取得する。

【0064】

続いて、パラメータ算出部３４は、地点Ａにおいて取得された複数の第１自己位置における第１座標位置（ｘＡｇ，ｙＡｇ）を平均化する（手順Ｓ１１２）。また、パラメータ算出部３４は、地点Ａにおいて取得された複数の第２自己位置における第２座標位置（ｘＡｓ，ｙＡｓ）を平均化する（手順Ｓ１１３）。

【0065】

続いて、パラメータ算出部３４は、地点Ｂにおいて、第１推定演算部３２により得られたフォークリフト２の第１自己位置データと第２推定演算部３３により得られたフォークリフト２の第２自己位置データとを取得する（手順Ｓ１１４）。このとき、パラメータ算出部３４は、例えばフォークリフト２を地点Ａから地点Ｂに移動させた状態で、フォークリフト２の第１自己位置データ及び第２自己位置データを取得する。また、パラメータ算出部３４は、フォークリフト２の第１自己位置データ及び第２自己位置データを複数ずつ取得する。なお、上記の手順Ｓ１１２，Ｓ１１３は、手順Ｓ１１４の後に実行してもよい。

【0066】

続いて、パラメータ算出部３４は、地点Ｂにおいて取得された複数の第１自己位置における第１座標位置（ｘＢｇ，ｙＢｇ）を平均化する（手順Ｓ１１５）。また、パラメータ算出部３４は、地点Ｂにおいて取得された複数の第２自己位置における第２座標位置（ｘＢｓ，ｙＢｓ）を平均化する（手順Ｓ１１６）。

【0067】

続いて、パラメータ算出部３４は、下記の計算式によってフォークリフト２の第１座標角度θｇ及び第２座標角度θｓを算出する（手順Ｓ１１７）。

【数5】

【0068】

続いて、パラメータ算出部３４は、図５に示される処理手順と同様に、手順Ｓ１０４～手順Ｓ１０６を実行する。

【0069】

本変形例においては、２つの地点におけるフォークリフト２の座標位置に基づいて、フォークリフト２の座標角度が算出される。従って、精度が高いフォークリフト２の座標角度が得られる。

【0070】

なお、本変形例では、２つの地点におけるフォークリフト２の座標位置に基づいて、フォークリフト２の座標角度が算出されているが、特にその形態には限られず、３つ以上の地点におけるフォークリフト２の座標位置に基づいて、フォークリフト２の座標角度を算出してもよい。例えば、任意の３カ所（地点Ａ～Ｃとする）でフォークリフト２の自己位置を推定する場合は、地点Ａ，Ｂの座標位置から得られた座標角度と地点Ｂ，Ｃの座標位置から得られた座標角度と地点Ｃ，Ａの座標位置から得られた座標角度との平均をとってもよい。

【0071】

また、本変形例では、複数の地点におけるフォークリフト２の座標位置に基づいて、フォークリフト２の座標角度が算出される。このため、第１推定演算部３２及び第２推定演算部３３は、フォークリフト２の自己位置として、フォークリフト２の座標位置のみを推定し、フォークリフト２の座標角度を推定しなくてもよい。

【0072】

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、ＧＰＳ受信機１１及びレーザセンサ１２は、２次元座標における同じ位置に上下に並んで配置されているが、特にそのような形態には限られない。ＧＰＳ受信機１１及びレーザセンサ１２は、フォークリフト２の車輪４の前後方向または左右方向に並んで配置されていてもよい。この場合には、フォークリフト２の第１自己位置とフォークリフト２の第２自己位置とのオフセット量を後で補正する計算を行う。

【0073】

また、上記実施形態では、座標変換用パラメータ取得装置３０のＧＰＳ受信機１１及びレーザセンサ１２は、実際に使用されるフォークリフト２に搭載されているが、特にそのような形態には限られない。座標変換用パラメータ取得装置３０のＧＰＳ受信機１１及びレーザセンサ１２は、実際に使用されるフォークリフト２とは異なる専用の車両等に搭載されていてもよい。

【0074】

また、上記実施形態では、座標変換用パラメータ取得装置３０のパラメータ取得用コントローラ３１は、フォークリフト２に搭載されているが、特にそのような形態には限られない。例えば、第１推定演算部、第２推定演算部３３及びパラメータ保存部３５を有するコントローラは、車両に搭載され、パラメータ算出部３４を有するコントローラは、パソコン（ＰＣ）等であってもよい。

【0075】

また、上記実施形態では、座標変換用パラメータは、ＧＮＳＳ座標系をＳＬＡＭ座標系に変換するためのパラメータであるが、座標変換用パラメータとしては、特にその形態には限られず、ＳＬＡＭ座標系をＧＮＳＳ座標系に変換するためのパラメータであってもよい。

【0076】

また、上記実施形態では、ＧＰＳ受信機１１を使用してフォークリフト２の自己位置が推定されると共に、レーザセンサ１２を使用してフォークリフト２の自己位置が推定されているが、フォークリフト２の自己位置を推定するための２つのセンサとしては、特にそのような形態には限られない。フォークリフト２の自己位置を推定するためのセンサとしては、例えば車両の周囲を撮像するカメラ、路面に設置された磁気テープを検出する磁気センサ、或いは地上からの電波を受信するトランスポンダ等を使用してもよい。

【0077】

また、上記実施形態では、フォークリフト２の自己位置が推定されているが、本発明は、特にフォークリフト２には限られず、例えば搬送台車や搬送ロボット等の移動体にも適用可能である。

【符号の説明】

【0078】

２…フォークリフト（移動体）、１０…自己位置推定装置、１１…ＧＰＳ受信機（第１センサ、第１自己位置推定部）、１２…レーザセンサ（第２センサ、第２自己位置推定部）、２１…第１推定演算部（第１自己位置推定部）、２２…第２推定演算部（第２自己位置推定部）、２４…座標変換部、２５…自己位置決定部、３０…座標変換用パラメータ取得装置、３２…第１推定演算部（第１自己位置推定部）、３３…第２推定演算部（第２自己位置推定部）、３４…パラメータ算出部。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】