JP7434943B2

JP7434943B2 - 自己位置制御システムおよび自己位置制御方法

Info

Publication number: JP7434943B2
Application number: JP2020013254A
Authority: JP
Inventors: 洋宍道
Original assignee: Meidensha Corp
Current assignee: Meidensha Corp
Priority date: 2020-01-30
Filing date: 2020-01-30
Publication date: 2024-02-21
Anticipated expiration: 2040-01-30
Also published as: JP2021119440A

Description

本発明は、自己位置制御システムおよび自己位置制御方法に関する。

従来より、周りの環境を探索しながら目標や障害物等を検知して移動経路を決定し、これに従って移動する自律走行車が知られている。自律走行車は、工場や医療福祉施設、家庭等において物品の搬送や清掃作業、あるいは作業台や机などにおいて所定の作業をおこなうことに利用されている。

このような自律走行車の自己位置推定には、ＬＲＦ（ＬａｓｅｒＲａｎｇｅＦｉｎｄｅｒ）等による周辺環境のセンシング結果をあらかじめ作成して保持している広域地図とマッチングして、この車両がその地図上のどこにいるかを推定するＳＬＡＭ（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎＡｎｄＭａｐｐｉｎｇ）技術が使われている。しかし、現状のＳＬＡＭ技術のおいては、位置精度が±１０ｃｍ程度であるため、充電ステーションやベルトコンベア等の設備に横付けする際に衝突する恐れがある。

特許文献１には、設備等の対象物に接近する方法として、移動台車に方向距離センサを搭載し、この方向距離センサから得られた距離の変化量が閾値以上に変化する部分を対象物の端点と推定し、端点間の距離が予め指定された対象物の外形と所定誤差範囲内であれば、この端点を対象物として認識する発明が開示されている。

特開２００６－１６３５５８号公報

引用文献１においては、図７に示すように、対象物（ランドマーク）７の端点７１、７２から等距離にあり、対象物７に垂直な距離Ｌの位置７３が目標位置として推定される。この場合、位置の推定に用いられるのは、７１、７２の２点のみであり、端点７１、７２の位置は測定誤差によって２点鎖線で示す位置までずれて認識される可能性がある。その場合、矢印７４に示す幅が目標位置の誤差として生じてしまい、目標位置に台車を精度良く移動できない恐れがある。すなわち、目標位置の推定に含まれる誤差が、自律走行車の移動制御に関して種々の問題を引き起こす恐れがある。

本発明は、上述した実情に鑑みてなされたものであり、目標位置姿勢の推定を精度良く行うことが可能な自己位置制御システムおよび自己位置制御方法を提供することを課題とする。

本発明は、上記課題を解決するため、以下の手段を採用する。
すなわち、本発明の自己位置制御システムは、２つのランドマークと、レーザレンジファインダを備えた自律走行車と、該自律走行車を制御する制御装置と、を備え、該制御装置は、前記レーザレンジファインダによって検出された前記２つのランドマークの位置データに基づいて前記自律走行車の目標位置姿勢を推定する。
このような発明によれば、推定がレーザレンジファインダによって検出した２つのランドマークの位置データに基づくので、目標位置姿勢の推定を精度良く行うことができる。

本発明の一態様においては、前記２つのランドマークは、矩形状の平面を有し、前記制御装置は、前記レーザレンジファインダが取得したデータから推定された、前記２つのランドマークの前記矩形状の平面の端部を含む直線式に基づいて前記自律走行車の目標位置姿勢を推定する。
このような構成によれば、矩形状の平面を有するランドマークの端部を含む直線式に基づいて目標位置姿勢の推定が行われるので、適切な形態のランドマークを採用し、目標位置姿勢の推定精度を向上することができる。

本発明の一態様においては、前記２つのランドマークの一方は、矩形状の平面を有し、前記２つのランドマークの他方は、円柱形状を有し、前記制御装置は、前記レーザレンジファインダが取得したデータから推定された、前記一方のランドマークの前記矩形状の平面の端部を含む直線式と、前記他方のランドマークの前記円柱形状の中心座標と距離と、に基づいて前記自律走行車の目標位置姿勢を推定する。
このような構成によれば、一方のランドマークは矩形状の平面を有するものとし、他方のランドマークを円柱形状とするので、作業台や机の脚等、作業現場に備えられた設備を円柱形状のランドマークとして採用でき、目標位置姿勢の推定精度を担保しつつ、容易にランドマークを構成することができる。

本発明の一態様においては、前記２つのランドマークは、円柱形状を有し、前記制御装置は、前記レーザレンジファインダが取得したデータから推定された、前記２つのランドマークの前記円柱形状の中心座標と距離に基づいて前記自律走行車の目標位置姿勢を推定する。
このような構成によれば、ランドマークを円柱形状のみで構成できるので、特別にランドマーク用の構造物を配置しなくても、作業台や机の脚等、作業現場に備えられた設備を円柱形状のランドマークとして採用できるので、自律走行車が使用される現場で、容易にランドマークを構成することができる。

本発明の一態様においては、前記制御装置は、前記自律走行車の目標位置姿勢の推定は拡張カルマンフィルタに基づいて行うこと、前記推定された目標位置姿勢に基づいて前記自律走行車を移動すること、前記移動後の前記自律走行車の推定目標位置姿勢が前記レーザレンジファインダの原点を目標とすること、を含む制御を行う。
このような構成によれば、目標位置姿勢の推定を拡張カルマンフィルタに基づいて行い、自律走行車の目標位置姿勢をレーザレンジファインダの原点に追い込む制御とするので、簡便なアルゴリズムで精度良く自律走行車を制御できる。

また、本発明の自己位置制御システムは、矩形状の平面を有する１つのランドマークと、レーザレンジファインダを備えた自律走行車と、該自律走行車を制御する制御装置と、を備え、該制御装置は、前記レーザレンジファインダが取得したデータから推定された、前記ランドマークの前記矩形状の平面の端部の座標に基づいて前記自律走行車の目標位置姿勢を推定すること、前記自律走行車の目標位置姿勢の推定は拡張カルマンフィルタに基づいて行うこと、前記推定された目標位置姿勢に基づいて前記自律走行車を移動すること、前記移動後の前記自律走行車の推定目標位置姿勢が前記レーザレンジファインダの原点を目標とすること、を含む制御を行う。
このような発明によれば、ランドマークを１つだけ用い、目標位置姿勢の推定を拡張カルマンフィルタに基づいて行い、自律走行車の目標位置姿勢をレーザレンジファインダの原点に追い込む制御とするので、簡便なランドマークの構成を用いることができ、かつ簡便なアルゴリズムで精度良く自律走行車を制御できる。

また、本発明の自律走行車の制御方法は、レーザレンジファインダを備えた自律走行車の制御方法であって、矩形状の平面を有する２つのランドマークを設けること、前記レーザレンジファインダで前記２つのランドマークを検出すること、前記検出データから前記２つのランドマークの前記矩形状の平面の端部を含む直線式を推定すること、前記推定した２つの直線式に基づいて前記自律走行車の目標位置姿勢を推定すること、を含む。
このような発明によれば、推定がレーザレンジファインダによって検出した２つのランドマークの位置データに基づくので、目標位置姿勢の推定を精度良く行うことができる。また、矩形状の平面を有するランドマークの端部を含む直線式に基づいて位置の推定が行われるので、適切な形態のランドマークを採用し、目標位置姿勢の推定精度を向上することができる。

本発明の一態様においては、前記自律走行車の目標位置姿勢の推定は拡張カルマンフィルタに基づいて行うこと、前記推定された目標位置姿勢に基づいて前記自律走行車を移動すること、前記移動後の前記自律走行車の推定目標位置姿勢が前記レーザレンジファインダの原点を目標とすること、を含む制御を行う。
このような構成によれば、目標位置姿勢の推定を拡張カルマンフィルタに基づいて行い、自律走行車の目標位置姿勢をレーザレンジファインダの原点に追い込む制御とするので、簡便なアルゴリズムで精度良く自律走行車を制御できる。

本発明によれば、目標位置姿勢の推定を精度良く行うことが可能な自己位置制御システムおよび自己位置制御方法を提供することができる。

本発明の実施形態に係る自己位置制御システムの構成を示す概略図である。本発明の実施形態に係る自己位置制御システムの制御装置を含む構成を示す機能ブロック図である。本発明の実施形態に係る自己位置制御システムの制御を説明するための平面図である。本発明の実施形態に係るランドマークモードの制御を説明するためのフローチャートである。本発明の実施形態に係る自己位置制御システムのランドマーク検知を説明するための平面図である。本発明の実施形態に係る自己位置制御システムの制御を説明するための模式図である。従来例のランドマーク検知誤差を説明するための模式図である。本発明の実施形態におけるランドマーク検知精度を説明するための模式図である。本発明の実施形態に係る自己位置制御システムの制御を説明するための平面図である。本発明の実施形態に係るランドマークモードの制御を説明するためのフローチャートである。本発明の実施形態に係る自己位置制御システムのランドマーク検知を説明するための平面図である。本発明の実施形態におけるランドマーク検知精度を説明するための模式図である。本発明の実施形態に係る自己位置制御システムの制御を説明するための平面図である。本発明の実施形態に係るランドマークモードの制御を説明するためのフローチャートである。本発明の実施形態におけるランドマーク検知精度を説明するための模式図である。

以下、添付図面を参照して、本発明の実施の形態について説明する。本発明において、自律走行車の“位置”または“位置姿勢”というとき、特に言及がないときは、自律走行車の座標値とその座標系における向き（進行方向等）を含むものとする。したがって、自立走行車の“目標位置”または“目標位置姿勢”は、上記に従い、自立走行車の移動目標とする座標値と向きを含む。

（第１実施形態）
図１に、本発明の第１実施形態に係る自己位置制御システムの構成を示す概略図を示す。図２に、本実施形態に係る自己位置制御システムの制御装置を含む構成を示す機能ブロック図を示す。
図１を参照して、本実施形態に係る自己位置制御システム１は、２つのランドマーク２１、２２と、レーザレンジファインダ（ＬＲＦ）３を備えた自律走行車（ＡＧＶ）４と、自律走行車（ＡＧＶ）４を制御する制御装置５と、を備えている。本実施形態では、自律走行車４の移動の目標対象物は、充電ステーション６とされている。自律走行車４は、充電ステーション６の充電コネクタ（図示せず）に接続するために目標位置姿勢まで移動するよう制御される。また、２つのランドマーク２１、２２は、本実施形態では、矩形状の平面を有する板で構成されており、板面が床に対して鉛直となるように配置されている。レーザレンジファインダ（ＬＲＦ）３は、図１に破線Ｒで示されている範囲をレーザでスキャンし、その範囲内にある物体からのレーザの反射を検知する。レーザレンジファインダ（ＬＲＦ）３は、その検知したレーザの反射に基づいて、レーザレンジファインダ（ＬＲＦ）３の原点から物体までの距離を測定する機能を有している。

図２を参照して、本実施形態における制御装置５は、後述するＳＬＡＭ（ＳｉｍｕｌｔａｎｅｏｕｓＬｏｃａｌｉｚａｔｉｏｎＡｎｄＭａｐｐｉｎｇ）機能を有するＡＧＶコントローラ５１と、地図情報・ルートデータ部５２と、ランドマーク検出・自己位置姿勢推定部５３と、を備える構成とされ、逐次、ＬＲＦ３、およびＡＧＶ４と通信することによって、ＡＧＶ４本体を制御する。

制御装置５は、コンピュータを含むアーキテクチャにより構成され、ハードウェアリソースである、ＣＰＵ、ＲＯＭ、ＲＡＭ、入力装置、出力装置、記憶媒体であるハードディスクやＳＳＤ（ＳｏｌｉｄＳｔａｔｅＤｒｉｖｅ）、通信インターフェース等を備え、これらを記憶媒体にロードされたソフトウェアリソースであるＯＳ、アプリケーションを利用して駆動し、実装される。上述した制御装置５を構成するＡＧＶコントローラ５１、地図情報・ルートデータ部５２、およびランドマーク検出・自己位置姿勢推定部５３は、物理的な位置は、どこに置かれていてもよく、それぞれのユニットが、有線、あるいは無線によって情報伝達が行われるように通信可能に接続されていればよい。例えば、ＡＧＶコントローラ５１と地図情報・ルートデータ部５２は、ＡＧＶ４の外部のホストコンピュータ（図示せず）に実装され、ランドマーク検出・自己位置姿勢推定部５３がＡＧＶ４内部に搭載されたコンピュータに実装されていてよい。

制御装置５は、ＡＧＶ４を、任意の起点から、最初はＳＬＡＭ機能によって目標位置に移動するように制御する。ここでＳＬＡＭ機能とは、近年工場等で採用されてきている、自己位置推定と環境地図作成を同時に行う機能のことであり、位置の推定やマップ作成にレーザーレンジスキャナー(測域センサ)、カメラ、エンコーダ、マイクロフォンアレイなどが利用され、対象の移動空間の形状、及びその空間に対する自機位置の検出をすることにより、移動経路の最適化と、それによる効率化を図ることができる機能である。

次に、上述のような構成の自己位置制御システム１において、任意の起点に置かれたＡＧＶ４が目標位置にむけて移動する動作について説明する。図３は、自己位置制御システム１において、ランドマークモードと呼称される制御にはいる直前のＡＧＶ４とランドマーク２１、２２の平面図である。ＡＧＶ４は、図３にＧとして示される目標位置を中心とした予め定められた距離範囲内に接近するまでは、すなわち図３に示された目標位置Ｇに充分接近するまでは、広域地図とＡＧＶ４のオドメトリデータに基づいて目標位置Ｇに接近するようにＳＬＡＭ機能により移動制御される。

本実施形態では、目標位置Ｇは、図３に示すように、ランドマーク２１、２２の板面に垂直で中心を通る直線２１ａ、２２ａの交点に設定されており、ランドマーク２１からＬ１、ランドマーク２２からＬ２の距離だけはなれている。図からわかるとおり、ランドマーク２１、２２は、目標位置Ｇからみて、互いに９０度外れた位置に配置されている。

図２を参照して、ＳＬＡＭ機能による制御は以下のようにして行われる。

（１）ＡＧＶコントローラ５１は、ＬＲＦ３からＬＲＦ３がレーザでスキャンして得られた検知データ（矢印ｐ）と、地図情報・ルートデータ部５２記録されている予め作成された広域地図（矢印ｑ）と、を取得する。

（２）ＡＧＶコントローラ５１は、ＬＲＦ３から得られたデータと広域地図とを比較し、自己位置を推定する。

（３）ＡＧＶコントローラ５１は、（２）で推定した自己位置に基づいて、目標位置Ｇに接近するように、ＡＧＶ４が駆動手段（車輪等）を動作させる命令を制御情報（移動方向、移動距離等）として送信する（矢印ｒ）。

（４）ＡＧＶ４は、ＡＧＶコントローラ５１から受信した制御情報に基づいて、駆動手段を駆動する。

（５）ＡＧＶ４は、駆動終了後、制御情報に基づいて実際に動作した駆動輪の駆動角や回転角等を積算し、ＡＧＶコントローラ５１にオドメトリ情報として送信する（矢印ｓ）。

（６）ＡＧＶコントローラ５１は、ＡＧＶ４から得られたオドメトリ情報および検知データ（矢印ｐ）と広域地図（矢印ｑ）の重なり度合いの情報を用いて広域地図上の自己位置および姿勢（向き）をアップデートする。

以上の（１）～（６）のステップを、ＡＧＶ４が、目標位置Ｇから予め定められた範囲内にＡＧＶ４が移動するまで（例えば図３の状態まで）、逐次繰り返す制御が行われる。

ＡＧＶ４が目標位置Ｇを含む予め定められた距離範囲内に移動したことをＡＧＶコントローラ５１が検知すると、制御装置５は、ＳＬＡＭ機能からランドマークモードに制御を切り替える。図４は、このランドマークモードの動作のアルゴリズムを示したフローチャートである。ランドマークモードに制御が切り替わった場合は、以降の制御に利用される座標系は、ＡＧＶ４のＬＲＦ３の原点（レーザ照射源）を基準として行われる。

以下図２の機能ブロック図と図４のフローチャートを参照しながら、ランドマークモードの動作について説明する。
図４のフローチャートにおいて、まず、制御がランドマークモードに入ったかどうかが確認される（Ｓ０１）。ランドマークモードに入っていない場合（Ｓ０１：ＮＯ）、このフローチャートを終了し、引き続き前述のＳＬＡＭ機能によるＡＧＶ４の制御を継続する。制御がランドマークモードに入っている場合（Ｓ０１：ＹＥＳ）、このランドマークモードの制御が最初の実行であるかどうかが確認される（Ｓ０２）。

制御が最初のランドマークモードである場合（Ｓ０２：ＹＥＳ）、ＳＬＡＭでの推定位置姿勢結果に基づく、それぞれのランドマーク２１、２２の位置を推定する（Ｓ０３）。制御が２回目以降のランドマークモードである場合（Ｓ０２：ＮＯ）、前回のランドマークモードにおける推定位置姿勢およびオドメトリ情報からのそれぞれのランドマーク２１、２２の位置を推定する（Ｓ０４）。

Ｓ０３、Ｓ０４において具体的には、ＳＬＡＭ上での目標位置姿勢情報、ＳＬＡＭ上での現在位置姿勢、目標停止位置基準でのランドマークの両端点座標、およびオドメトリ情報が、図２において、ＡＧＶコントローラ５１からランドマーク検出・自己位置姿勢推定部５３に送られる（図２の矢印ｔ）。以降の処理は主に、ランドマーク検出・自己位置姿勢推定部５３において行われる。

次に、ランドマーク検出・自己位置姿勢推定部５３は、得られた上記の情報から、それぞれのランドマークの暫定的直線式を算出する（Ｓ０５）。ランドマークモード中にＬＲＦ３が取得した検知データは、ＬＲＦ３からランドマーク検出・自己位置姿勢推定部５３に送られており（図２の矢印ｕ）、ランドマーク検出・自己位置姿勢推定部５３は、ＬＲＦからのデータのうち、上記で求めた直線式近傍のデータを選別する（Ｓ０６）。

ここで、図５を参照して、直線式近傍のデータの選別方法を説明する。図５には、１つのランドマーク２１とＬＲＦ３搭載のＡＧＶ４の平面図が示されている。ここで図５にランドマーク２１として示されているのは、上記のランドマークの暫定的直線式から得られた暫定位置である。この図では、ランドマーク２１の１つだけについて説明するが、他方のランドマーク２２についても同様の方法がとられる。

破線Ｓは、ＬＲＦ３が、ランドマーク２１を検知するのに使用されるレーザスキャンの範囲を示している。レーザスキャン角度３ａは、ランドマーク２１の暫定位置に対して定められた余裕幅３ｂの範囲内を含んで設定される。また、ランドマーク２１の検知に使用するデータの最も遠い距離は、ランドマーク２１の暫定位置の最も遠い位置２１ｂから余裕幅３ｂを含んだＬｍに設定される。ランドマーク２１の板面に垂直な方向の選別範囲は、ランドマーク２１を真ん中に含んで紙面で上下に余裕幅３ｂ離れた幅３ｃが設定される。結果として、この図でランドマーク２１の位置検知のために選別（採用）されるデータは、楕円形２１ｃに接する破線の範囲内で検知されたものに設定されている。ＬＲＦ３に得られたデータにレーザ反射強度のばらつきがみられ、ノイズが大きい場合は、反射強度閾値を設定し、この閾値を選別に利用しても良い。

上記のデータの選別の後、選別されたデータを使って、ランドマークの線分を検出する（Ｓ０７）。線分の検出には、各板状ランドマーク２１、２２に対して上記のように選別したＬＲＦ距離データの点群から最小二乗法や、ＲＡＮＳＡＣ（ＲａｎｄｏｍＳａｍｐｌｅＣｏｎｓｅｎｓｕｓ）等の統計手法を使用して線分を求める。

ここで、図６を参照して、ＬＲＦ３のレーザスキャンの死角にランドマークの一部が存在した場合の動作について説明する。図６は、ランドマーク２１とＬＲＦ３搭載のＡＧＶ４の平面図である。破線ＴはＬＲＦ３のレーザスキャンの範囲を示しており、紙面の左側の範囲がレーザがスキャンできる範囲であり、矢印３ｄで示された範囲の角度は、レーザがスキャンできない範囲である。図からわかるとおり、ランドマーク２１のレーザが照射可能な最端点２１ｄより紙面で右側にある部分は、ＬＲＦ３では検知できない。この場合は、見える範囲のデータ点群から直線の式を求め、板状ランドマーク２１の視野内にある片方の端点座標を算出してそこから板状ランドマーク２１の死角にある端点座標を算出する。

２つのランドマーク２１、２２の線分検出動作が終了すると、２つの線分の検出が成功して得られているかどうかが調べられる（Ｓ０８）。２つの線分検出が成功している場合（Ｓ０８：ＹＥＳ）、それぞれの線分から目標位置を通る平行な直線を求め、その交点から目標位置を算出する（Ｓ０９）。

上記の目標位置Ｇは、次のように算出される。
すなわち図３におけるランドマーク２１、２２の線分式が、それぞれ順に、

として得られたとき、これらの式について、ランドマーク２１の線分式を距離Ｌ１だけ平行移動した破線（理想的には２２ａ）、およびランドマーク２２の線分式を距離Ｌ２だけ平行移動した破線（理想的には２１ａ）の式を、計算する。これらがそれぞれ順に、

として得られたとき、目標位置Ｇ（ｘ、ｙ）の座標は、［数２］の上記方程式を解いて次の式で得られる。

ただし、ａ_１ｂ_２≠ａ_２ｂ_１である。

ここで、Ｐ_１、Ｐ_２、Ｑ_１、Ｑ_２、ａ_１、ａ_２、ｂ_１、ｂ_２、ｃ_１、ｃ_２は定数で、ｘ、ｙは、座標に係る変数である。
このとき、図３に示された目標座標Ｇの計算とは別に、ＬＲＦ３で検知されたランドマーク２１の端点２１ｅ、２１ｆの座標から、ＡＧＶ４の向きの角度である図３に示す姿勢角度θが計算される。ランドマーク２２の端点２２ｅ、２２ｆの座標からもＡＧＶ４の姿勢角度θが計算され、２つのランドマーク２１、２２から計算された姿勢角度θの平均値を現在のＡＧＶ４の姿勢角度θとして決定する。

図４のフローチャートＳ０８において、２つの線分検出が成功していない場合（Ｓ０８：ＮＯ）、ランドマーク２１かランドマーク２２のどちらかは検出されているかどうかが調べられる（Ｓ１０）。片方のランドマークが検出されている場合（Ｓ１０：ＹＥＳ）、検出された片方の線分から、目標位置を算出する（Ｓ１１）。具体的には、例えばランドマーク２２のみが検知されている場合、ランドマーク２２の２つの端点２２ｅ、２２ｆから等距離にある、ランドマーク２２の板面に垂直な距離Ｌ２離れた点が目標位置Ｇとして算出される。ランドマーク２１のみが検出された場合もランドマーク２１の端点に基づいて目標位置Ｇが算出される。
このとき、目標座標Ｇの計算とは別に、ＬＲＦ３で検知された片方のランドマーク２１または２２の端点２１ｅ、２１ｆ、または２２ｅ、２２ｆの座標から、ＡＧＶ４の向きの角度θが計算され、現在のＡＧＶ４の姿勢角度θとして決定される。

ランドマーク２１、２２のどちらも検出に成功していない場合（Ｓ１０：ＮＯ）、前回の線分検出結果とオドメトリ情報から目標位置Ｇ、および姿勢角度θが算出される（Ｓ１２）。このようにして、Ｓ０９、Ｓ１１、Ｓ１２のいずれかのステップで算出された目標位置Ｇと姿勢角度θが、ランドマーク検出・自己位置姿勢推定部５３からＡＧＶコントローラ５１に送信され（図２の矢印ｖ）（Ｓ１３）、１サイクルのランドマークモードが終了する。目標位置Ｇと姿勢角度θを受信したＡＧＶコントローラ５１は、ＡＧＶ４がそれらの値が０（ＬＲＦ３の原点と姿勢角度０）を目標として駆動するように制御情報を送信する（図２の矢印ｒ）。
このランドマークモード中は、目標座標Ｇと姿勢角度θが設定された閾値以下（ＬＲＦ３の座標系で制御しているので、理想的にはどちらも０になるまで）になるまで上述の制御がループし、目標位置、目標姿勢が許容誤差範囲内となったところで終了する。

次に図７、図８を参照して、本実施形態の効果について説明する。
図７は、従来例を説明する為の模式図で、図８は、本実施形態を説明するための模式図である。図７については、上記で説明したように、ランドマーク７の検知誤差のため２点鎖線の位置までランドマーク７の検知がずれる恐れがあり、矢印７４で示すごとく誤差が生じる様子がわかる。

図８を参照して本実施形態においては、２つのランドマーク２１、２２を用いて、それぞれのランドマークの線分式をＬＲＦ３の検知データから推定し、その２つの推定式を、ランドマーク２１の線分式については距離Ｌ１、ランドマーク２２の線分式については、距離Ｌ２の平行移動をさせた２つの直線式の交点を目標座標Ｇとして算出する。図８上においては、検知のばらつきを実線と２点鎖線で描かれたランドマークで示している。

図８に示すように、直線式の角度のばらつきは、２直線の交点を求める場合、交点座標のばらつきに影響を与えづらいので、この方法により算出された目標位置Ｇは、ランドマーク２１、２２の角度的な検知について誤差の影響を極力減らすことができる。したがって、本実施形態においては、従来例に比較して、ランドマーク２１、２２の検知誤差に影響されずに充分高い精度で、目標位置（目標位置に対する自己位置）を推定することができる。
すなわち、目標位置姿勢の推定が精度良く行われることによって自律走行車を精度良く目標位置姿勢まで自律走行させる事が可能となる。

（第２実施形態）
次に、図９を参照して、第２実施形態を説明する。図９は、本実施形態を説明するための平面図である。本実施形態が、第１実施形態と異なっている点は、他方のランドマーク２３が、円柱形状を有する点である。一方のランドマーク２１は、矩形状の平面を有しており、第１実施形態と同じランドマーク２１である。その他の構成については、同等であるので、第１実施形態と同じ符号を付し、説明を省略する。図９には、ランドマーク２１、２３と説明のためのＬＲＦ３搭載のＡＧＶ４が描かれている。

本実施形態では、目標位置Ｇは、図９に示すように、ランドマーク２１の板面から垂直に距離Ｌ１離れたランドマーク２１の板面に平行な直線２１ｅと、ランドマーク２３の中心２３ａから距離Ｌ３の円２３ｂの交点Ｇとして設定されている。本実施形態において、ランドマークモードに入るまでのＳＬＡＭ機能によるＡＧＶ４の制御は、第１実施形態と同じであって、同様の制御によって、第１実施形態で述べたランドマークモードに入る。

図１０は、本実施形態におけるランドマークモードの動作のアルゴリズムを示したフローチャートである。ランドマークモードに制御が切り替わった場合は、以降の制御に利用される座標系は、第１実施形態と同様に、ＡＧＶ４のＬＲＦ３の原点（レーザ照射源）を基準として行われる。

図１０において、Ｓ２１からＳ２７のステップについては、第１実施形態における図４のＳ０１からＳ０７と同等であるので、ここでは、第１実施形態と異なっている円柱形状を有するランドマーク２３の検知について焦点を当てて説明する。
Ｓ２５からＳ２７は、板状ランドマーク２１の両端部を含む直線式を検知する処理であるが、その処理と平行して、Ｓ３４からＳ３６の円柱形状のランドマーク２３に関する処理が行われる。

ランドマーク２３に関する処理は、まず、ＬＲＦ３から取得してデータのうち、推定した円柱状ランドマーク２３の近傍のデータを選別する（Ｓ３４）。

ここで、図１１を参照して、直線式近傍のデータの選別方法を説明する。図１１には、１つの円柱状ランドマーク２３とＬＲＦ３搭載のＡＧＶ４の平面図が示されている。ここで図１１にランドマーク２３として示されているのは、前処理で前回のＳＬＡＭ機能による処理、またはランドマークモードによる処理で得られたデータ（ＳＬＡＭ上での目標位置姿勢情報、ＳＬＡＭ上での現在位置姿勢、目標停止位置基準でのランドマーク２３の中心座標、およびオドメトリ情報等）から暫定的に推定されたれた暫定位置である。

破線Ｕは、ＬＲＦ３が、ランドマーク２３を検知するのに使用されるレーザスキャンの範囲を示している。レーザスキャン角度３ｅは、ランドマーク２３の暫定位置に対して余裕角度分を含んで設定される。また、ランドマーク２３の検知に使用するデータのＬＲＦ３からの距離範囲は、暫定円柱状ランドマーク２３までの距離に対して、円柱状ランドマーク２３の直径も考慮し、余裕幅３ｆを含んで３ｇの範囲内に設定される。結果として、この図でランドマーク２３の位置検知のために選別（採用）されるデータは、扇形の幅３ｇの部分２３ｂの範囲内で検知されたものに設定されている。なお、ＬＲＦ３によって検知される円柱状ランドマーク２３の形状は、曲面における反射等の影響で、図１１上示す板面２３ｃとして検知されており、反射強度に閾値を設けて閾値以上のデータを採用する。

上記のデータ選別の後、選別されたデータを使って、ランドマーク２３の中心座標を検出する（Ｓ３５）。上述のように円柱状ランドマーク２３は、直線状の点群として検知されるので、これを平均化し、円柱状ランドマーク２３の半径を加えてＬＲＦ３から円柱状ランドマーク２３の中心までの距離を算出する。またＬＲＦ３からみて反射強度が最大になる角度が円柱状ランドマーク２３の中心であり、最大反射強度から角度を算出する。ここで求めた距離と角度より、円柱状ランドマーク２３の中心座標を求める。

図９に示す通り、目標位置Ｇは、円柱状ランドマーク２３の中心２３ａから距離Ｌ３の位置に設定されているので、上記で求めた円柱状ランドマーク２３の中心座標から、Ｌ３の距離を半径とした円の式を算出する（Ｓ３６）。

２つのランドマーク２１、２３の検出動作が終了すると、２つのランドマークの検出が成功しているかどうかが調べられる（Ｓ２８）。２つのランドマーク２１、２３の検出が成功している場合（Ｓ２８：ＹＥＳ）、ランドマーク２１の線分式を距離Ｌ１平行移動した直線式とランドマーク２３の円の式の交点から目標位置Ｇを算出する（Ｓ２９）。

上記の目標位置Ｇは、次のように算出される。
すなわち図９におけるランドマーク２１の線分式から、距離Ｌ１平行移動した直線２１ｅの式が、第１実施形態と同じ方法により、次のように得られる。

ここで、ａ、ｂ、ｃは定数である。また、円柱状ランドマーク２３の中心座標が（ｘｒ、ｙｒ）として求められたときの半径ｌ（Ｌ３）の円は、以下のように得られる。

ここで、［数４］と［数５］の連立方程式をとくと、交点座標Ｇ（ｘ、ｙ）は、以下のように求められる。

また、ＡＧＶ４の姿勢角度θは、板状ランドマーク２１と円柱状ランドマーク２３の位置関係から一般的な数式を解くことによって求めることができる。

図１０のフローチャートＳ２８において、２つのランドマーク検出が成功していない場合（Ｓ２８：ＮＯ）、板状ランドマーク２１のみが検出されているかどうかが調べられる（Ｓ３０）。板状ランドマーク２１が検出されている場合（Ｓ３０：ＹＥＳ）、検出された線分から、目標位置を算出する。具体的には、ランドマーク２１の２つの端点２１ｅ、２１ｆから等距離にある、ランドマーク２１の板面に垂直な距離Ｌ１離れた点が目標位置Ｇとして算出される。
このとき、目標座標Ｇの計算とは別に、ＬＲＦ３で検知されたランドマーク２１の端点２１ｅ、２１ｆの座標から、ＡＧＶ４の向きの角度θが計算され、現在のＡＧＶ４の姿勢角度θとして決定される。

板状ランドマーク２１の検出に成功していない場合（Ｓ３０：ＮＯ）、前回の検出結果とオドメトリ情報から目標位置Ｇ、および姿勢角度θが算出される（Ｓ３０）。このようにして、Ｓ２９、Ｓ３１、Ｓ３２のいずれかのステップで算出された目標位置Ｇと姿勢角度θが、ランドマーク検出・自己位置姿勢推定部５３からＡＧＶコントローラ５１に送信され（図２の矢印ｖ）（Ｓ３３）、１サイクルのランドマークモードが終了する。目標位置Ｇと姿勢角度θを受信したＡＧＶコントローラ５１は、ＡＧＶ４がそれらの値が０を目標（ＬＲＦ３の原点と姿勢角度０）として駆動するように制御情報を送信する（図２の矢印ｒ）。
このランドマークモード中は、目標座標Ｇと姿勢角度θが設定された閾値以下（ＬＲＦ３の座標系で制御しているので、理想的にはどちらも０になるまで）になるまで上述の制御がループし、目標位置、目標姿勢が許容誤差範囲内となったところで終了する。

次に図１２を参照して、本実施形態の効果について説明する。
本実施形態においては、２つのランドマーク２１、２３を用いて、板状ランドマーク２１の線分式と円柱状ランドマーク２３の中心座標をＬＲＦ３の検知データから推定し、ランドマーク２１の直線式を、距離Ｌ１の平行移動をさせた直線式と、円柱状ランドマーク２３の中心座標を中心とした半径Ｌ３（ｌ）の円の式を求め、その交点を目標座標Ｇとして算出する。図１２上においては、板状ランドマークの検知のばらつきを実線と２点鎖線で描かれたランドマークで示している。

図１２に示すように、直線式の角度のばらつきは、直線式と円の交点を求める場合、交点座標のばらつきに影響を与えづらいので、この方法により算出された目標位置Ｇは、ランドマーク２１の角度的な検知について誤差の影響を極力減らすことができる。したがって、本実施形態においては、従来例に比較して、ランドマーク２１の検知誤差に影響されずに充分高い精度で、目標位置（目標位置に対する自己位置）を推定することができる。
すなわち、目標位置姿勢の推定が精度良く行われることによって自律走行車を精度良く目標位置姿勢まで自律走行させる事が可能となる。

また、円柱状ランドマーク２３を使用するので、特にランドマークとして、板状のものを改めて設置しなくても、作業台や机の脚等、作業現場に備えられた設備を円柱形状のランドマークとして採用でき、位置の推定精度を担保しつつ、容易にランドマークを構成することができる。

（第３実施形態）
次に、図１３を参照して、第３実施形態を説明する。図１３は、本実施形態を説明するための平面図である。本実施形態が、第２実施形態と異なっている点は、２つのランドマーク２３、２４が、円柱形状を有する点である。その他の構成については、同等であるので、第２実施形態と同じ符号を付し、説明を省略する。図１３には、ランドマーク２３、２４と説明のためのＬＲＦ３搭載のＡＧＶ４が描かれている。

本実施形態では、目標位置Ｇは、図１３に示すように、ランドマーク２３の中心２３ａから距離Ｌ３の円２３ｂと、ランドマーク２４の中心２４ａから距離Ｌ４の円２４ｂとの交点Ｇとして設定されている。本実施形態において、ランドマークモードに入るまでのＳＬＡＭ機能によるＡＧＶ４の制御は、第１実施形態と同じであって、同様の制御によって、第１実施形態で述べたランドマークモードに入る。

図１４は、本実施形態におけるランドマークモードの動作のアルゴリズムを示したフローチャートである。ランドマークモードに制御が切り替わった場合は、以降の制御に利用される座標系は、第１実施形態と同様に、ＡＧＶ４のＬＲＦ３の原点（レーザ照射源）を基準として行われる。

図１４において、Ｓ４１からＳ４４のステップについては、第１実施形態における図４のＳ０１からＳ０４と同等である。また、図１４のＳ４５からＳ４７のステップは、第２実施形態における図１０のＳ３４からＳ３６と同等である。したがって、ここでは、第１実施形態、第２実施形態と異なっている円柱形状を有する２つのランドマーク２３、２４を利用した自己位置推定について焦点を当てて説明する。

第２実施形態のＳ３４からＳ３６の処理と同等の処理をランドマーク２３と２４に行い、ランドマーク２３と２４の中心座標を求め、図１３に示す通り、目標位置Ｇは、円柱状ランドマーク２３の中心２３ａから距離Ｌ３、および円柱状ランドマーク２４の中心２４ａから距離Ｌ４、の位置に設定されているので、上記で求めた円柱状ランドマーク２３、２４の中心座標から、それぞれＬ３、Ｌ４の距離を半径とした２つの円の式を算出する（Ｓ４５からＳ４７）。

２つのランドマーク２３、２４の検出動作が終了すると、２つのランドマークの検出が成功しているかどうかが調べられる（Ｓ４８）。２つのランドマーク２３、２４の検出が成功している場合（Ｓ４８：ＹＥＳ）、ランドマーク２３とランドマーク２４の円の式の交点から目標位置Ｇを算出する（Ｓ４９）。

上記の目標位置Ｇは、次のように算出される。
円柱状ランドマーク２３の中心座標が（ｘ_１、ｙ_１）、として求められたときの半径ｌ_１（Ｌ３）の円の式と、円柱状ランドマーク２３の中心座標が（ｘ_２、ｙ_２）、として求められたときの半径ｌ_２（Ｌ４）の円の式は、それぞれ順に以下のように得られる。

ここで、上記の連立方程式をとくと、交点座標Ｇ（ｘ、ｙ）は、以下のように求められる。

ただし、

である。
また、ＡＧＶ４の姿勢角度θは、２つの円柱状ランドマーク２３、２４の位置関係から一般的な数式を解くことによって求めることができる。

図１４のフローチャートＳ４８において、２つのランドマーク検出が成功していない場合（Ｓ４８：ＮＯ）、前回の検出結果とオドメトリ情報から目標位置Ｇ、および姿勢角度θが算出される（Ｓ５０）。このようにして、Ｓ４９、Ｓ５０のいずれかのステップで算出された目標位置Ｇと姿勢角度θが、ランドマーク検出・自己位置姿勢推定部５３からＡＧＶコントローラ５１に送信され（図２の矢印ｖ）（Ｓ５１）、１サイクルのランドマークモードが終了する。目標位置Ｇと姿勢角度θを受信したＡＧＶコントローラ５１は、ＡＧＶ４がそれらの値が０を目標（ＬＲＦ３の原点と姿勢角度０）として駆動するように制御情報を送信する（図２の矢印ｒ）。
このランドマークモード中は、目標座標Ｇと姿勢角度θが設定された閾値以下（ＬＲＦ３の座標系で制御しているので、理想的にはどちらも０になるまで）になるまで上述の制御がループし、目標位置、目標姿勢が許容誤差範囲内となったところで終了する。

次に図１５を参照して、本実施形態の効果について説明する。
本実施形態においては、２つのランドマーク２３、２４を用いて、２つの円柱状ランドマーク２３、２４の中心座標をＬＲＦ３の検知データから推定し、円柱状ランドマーク２３の中心座標を中心とした半径Ｌ３（ｌ_１）の円の式と、円柱状ランドマーク２４の中心座標を中心とした半径Ｌ４（ｌ_２）の円の式と、を求め、その交点を目標座標Ｇとして算出する。

図１５に示すように、本実施形態では、２つのランドマークから求めた２つの円の交点によって目標位置Ｇを算出する。したがって、本実施形態においては、従来例に比較して、ランドマーク２３、２４を２系統の独立した手順で検知するので、検知誤差の影響を極力押さえて高い精度で、目標位置（目標位置に対する自己位置）を推定することができる。
すなわち、目標位置姿勢の推定が精度良く行われることによって自律走行車を精度良く目標位置姿勢まで自律走行させる事が可能となる。

また、ランドマーク２３、２４として、円柱形状のものだけを使用するので、特別にランドマーク用の構造物を配置しなくても、作業台や机の脚等、作業現場に備えられた設備を円柱形状のランドマークとして採用でき、自律走行車が使用される現場で、容易にランドマークを構成することができる。

（第４実施形態）
次に、第４実施形態について、説明する。本実施形態が、上記の第１から第３実施形態と異なる点は、ランドマークモードにおける、目標位置Ｇと姿勢角度θの推定に拡張カルマンフィルタを用いる点である。（以降、目標位置Ｇと姿勢角度Ｇをあわせて、目標位置姿勢と呼称する。）したがって、ランドマークの種類の組合せや、目標位置Ｇや姿勢角度θの基本的な算出方法は、上述のそれぞれの実施形態と同じである。

ここにおいて、ＡＧＶコントローラ５１に最終段階で送信する目標位置姿勢は、目標停止位置と目標姿勢角度までの残差であり、これはＬＲＦ３の座標系においては、目標位置姿勢（ｘ_ｋ、ｙ_ｋ、θ_ｋ）^Ｔそのものである。また、制御入力はオドメトリ変化量（ｄｘ_ｋｄｙ_ｋｄθ_ｋ）^Ｔと記載し、ランドマーク検出結果を幾何計算して得られるＬＲＦ３座標系の目標位置姿勢の観測値を（ｚ_ｋ ^（ｘ）、ｚ_ｋ ^（ｙ）、ｚ_ｋ ^（θ））^Ｔとすると、拡張カルマンフィルタにおける状態モデルと観測モデルは、以下のようになる。

ここで、ｗ_ｋは、制御信号に対するノイズ成分を表し、ｖ_ｋは、観測センサに対するノイズ成分を表しており、期待値は０で、ある幅をもって変動している。
これから得られるヤコビアンは、以下のようになる。

ここでＨ_ｋは単位行列となり、式を単純化できる。
続いて予測を行う。状態の予測は、状態モデルのノイズを除いた式で以下のように計算する。

誤差共分散行列は制御ノイズの共分散行列Ｑ_ｋを用いて以下の通りとなる。

ここで出てくる制御ノイズの共分散行列は、本来のＡＧＶ４の制御誤差を計測してばらつきを求めて設定する。また誤差共分散行列の初期値Ｐ_０は、０としている。
続いて誤差共分散行列の更新を行う。実際の観測と状態から予測される観測との差ｅ_ｋを以下のように求める。

上記の値を使って観測誤差の共分散行列Ｓ_ｋを求める。Ｈｋは上述したように単位行列であるので、以下のように式を単純化できる。

ここで観測ノイズの共分散行列Ｒ_ｋは、ランドマーク検出結果のばらつきを計測して設定する。
続いてカルマンゲインＫ_ｋを以下のように求める。

最後に、状態と誤差共分散行列を更新する。

以上で状態の更新が完了する。ＡＧＶコントローラ５１へは、目標位置姿勢の残差として、

を送信する。

本実施形態においては、目標位置姿勢の推定に、拡張カルマンフィルタを使用するので、ＬＲＦ３の観測ノイズ、及びＡＧＶ４への制御ノイズを最小とするように最適化した予測値を使用でき、実施形態１から３の作用効果に加え、さらに高精度な制御が可能となる。すなわち、目標位置姿勢の推定がさらに高精精度に行われることによって自律走行車を精度良く目標位置姿勢まで自律走行させる事が可能となる。また、拡張カルマンフィルタの制御については、ＬＲＦ３の座標系でおこなわれ、目標位置姿勢は、ＬＲＦ３の座標系における原点であり、姿勢角度０が目標として制御される。したがって、制御アルゴリズムの構成を簡便に実装可能となる。

（第５実施形態）
次に、第５実施形態を説明する。本実施形態が、第１実施形態と異なる点は、ランドマークとして、板状ランドマーク２１の一つだけを用い、目標位置姿勢の推定に第４実施形態と同様の拡張カルマンフィルタを用いる点である。すなわち、第１実施形態における図５に示したのと同等の構成を採用する。１つの板状ランドマーク２１から目標位置姿勢を算出する動作は、第１実施形態の図４のフローチャートＳ１１の動作と同等である。その結果に対して第４実施形態で説明した拡張カルマンフィルタを用いる。

この構成により、本実施形態では、板状ランドマーク２１を１つだけ用い、位置の推定を拡張カルマンフィルタに基づいて行うので、簡便なランドマークの構成を用いて高い精度の目標位置姿勢の推定が可能となる。すなわち、目標位置姿勢の推定が精度良く行われることによって自律走行車を精度良く目標位置姿勢まで自律走行させる事が可能となる。また、拡張カルマンフィルタの制御については、ＬＲＦ３の座標系でおこなわれ、目標位置姿勢は、ＬＲＦ３の座標系における原点であり、姿勢角度０が目標として制御される。したがって、第４実施形態と同様に、制御アルゴリズムの構成を簡便に実装可能となる。

上記の実施形態では、自律走行車として、ＡＧＶ（無人搬送車）を例示したが、これに限定されず、自律走行で制御されるものであれば、例えば掃除ロボットや、ロボットアームを備えて、作業台で作業するようなロボットであってよく、本発明は、ＬＲＦを備えて自律走行する機能が備わっているすべてのシステムに適用可能である。

１自己位置制御システム
２１、２２、２３、２４ランドマーク
３レーザレンジファインダ（ＬＲＦ）
４自律走行車（ＡＧＶ）
５制御装置

Claims

２つのランドマークと、
レーザレンジファインダを備えた自律走行車と、
該自律走行車を制御する制御装置と、を備え、
該制御装置は、前記レーザレンジファインダによって検出された前記２つのランドマークの位置データに基づいて前記自律走行車の目標位置姿勢を推定し、
前記２つのランドマークは、矩形状の平面を有し、前記制御装置は、前記レーザレンジファインダが取得したデータから推定された、前記２つのランドマークそれぞれの、前記矩形状の平面の２つの端部を含む直線式に基づいて前記自律走行車の目標位置姿勢を推定し、
前記目標位置姿勢の推定は、
前記２つのランドマークの前記直線式から予め定められたそれぞれの距離だけ移動した目標位置を通る平行な直線を求め、その交点から目標位置を算出し、
いずれかまたは双方のランドマークの２つの端点の座標から、前記２つのランドマークに対して予め定められた基準方向を０度とするときの前記自律走行車の進行方向の角度である姿勢角度を算出する、自己位置制御システム。
２つのランドマークと、
レーザレンジファインダを備えた自律走行車と、
該自律走行車を制御する制御装置と、を備え、
該制御装置は、前記レーザレンジファインダによって検出された前記２つのランドマークの位置データに基づいて前記自律走行車の目標位置姿勢を推定し、
前記２つのランドマークの一方は、矩形状の平面を有し、前記２つのランドマークの他方は、円柱形状を有し、前記制御装置は、前記レーザレンジファインダが取得したデータから推定された、前記一方のランドマークの前記矩形状の平面の２つの端部を含む直線式と、前記他方のランドマークの前記円柱形状の中心座標と距離と、に基づいて前記自律走行車の目標位置姿勢を推定し、
前記目標位置姿勢の推定は、
前記一方のランドマークの前記直線式から予め定められた第１の距離だけ平行移動した直線式と、前記他方のランドマークの前記中心座標から予め定められた第２の距離を半径とする円との交点から目標位置を算出し、
前記一方のランドマークの２つの端点の座標から、前記２つのランドマークに対して予め定められた基準方向を０度とするときの前記自律走行車の進行方向の角度である姿勢角度を算出する、自己位置制御システム。
２つのランドマークと、
レーザレンジファインダを備えた自律走行車と、
該自律走行車を制御する制御装置と、を備え、
該制御装置は、前記レーザレンジファインダによって検出された前記２つのランドマークの位置データに基づいて前記自律走行車の目標位置姿勢を推定し、
前記２つのランドマークは、円柱形状を有し、前記制御装置は、前記レーザレンジファインダが取得したデータから推定された、前記２つのランドマークの前記円柱形状の中心座標と距離に基づいて前記自律走行車の目標位置姿勢を推定し、
前記目標位置姿勢の推定は、
前記２つのランドマークのそれぞれの中心座標から予めそれぞれ定められた距離を半径とする２つの円の交点から目標位置を算出し、
前記２つのランドマークのそれぞれの中心座標から、前記２つのランドマークに対して予め定められた基準方向を０度とするときの前記自律走行車の進行方向の角度である姿勢角度を算出する、自己位置制御システム。
前記制御装置は、
前記自律走行車の目標位置姿勢の推定は拡張カルマンフィルタに基づいて行うこと、
前記推定された目標位置姿勢に基づいて前記自律走行車を移動すること、
前記移動後の前記自律走行車の推定目標位置姿勢が前記レーザレンジファインダの原点を目標とすること、
を含む制御を行う、請求項１から３のいずれか１項に記載の自己位置制御システム。
矩形状の平面を有する１つのランドマークと、
レーザレンジファインダを備えた自律走行車と、
該自律走行車を制御する制御装置と、を備え、
該制御装置は、
前記レーザレンジファインダが取得したデータから推定された、前記ランドマークの前記矩形状の平面の端部の座標に基づいて前記自律走行車の目標位置姿勢を推定すること、
前記目標位置姿勢の推定は、
前記ランドマークの２つの端点から等距離にある、前記ランドマークの板面に垂直な予め定められた距離だけ離れた点を目標位置として算出し、
前記ランドマークの２つの端点の座標から、前記ランドマークに対して予め定められた基準方向を０度とするときの前記自律走行車の進行方向の角度である姿勢角度を算出し、
前記自律走行車の目標位置姿勢の推定は拡張カルマンフィルタに基づいて行うこと、
前記推定された目標位置姿勢に基づいて前記自律走行車を移動すること、
前記移動後の前記自律走行車の推定目標位置姿勢が前記レーザレンジファインダの原点を目標とすること、
を含む制御を行う、自己位置制御システム。
レーザレンジファインダを備えた自律走行車の制御方法であって、
矩形状の平面を有する２つのランドマークを設けること、
前記レーザレンジファインダで前記２つのランドマークを検出すること、
前記検出データから前記２つのランドマークそれぞれの、前記矩形状の平面の２つの端部を含む直線式を推定すること、
前記推定した２つの直線式に基づいて前記自律走行車の目標位置姿勢を推定すること、
を含み、
前記目標位置姿勢を推定することは、
前記２つのランドマークの前記直線式から予め定められたそれぞれの距離だけ移動した目標位置を通る平行な直線を求め、その交点から目標位置を算出すること、
いずれかまたは双方のランドマークの２つの端点の座標から、前記２つのランドマークに対して予め定められた基準方向を０度とするときの前記自律走行車の進行方向の角度である姿勢角度を算出すること、を含む自律走行車の自己位置制御方法。
前記自律走行車の目標位置姿勢の推定は拡張カルマンフィルタに基づいて行うこと、
前記推定された目標位置姿勢に基づいて前記自律走行車を移動すること、
前記移動後の前記自律走行車の推定目標位置姿勢が前記レーザレンジファインダの原点を目標とすること、
を含む制御を行う、請求項６に記載の自律走行車の自己位置制御方法。