JP2022180938A - 走行経路設定装置 - Google Patents

Abstract

【課題】移動体が走行不可能な走行経路が設定されることを防止できる走行経路設定装置を提供する。【解決手段】走行経路設定装置１は、移動体２の現在の自己位置を推定するＳＬＡＭ自己位置推定部１４と、移動体２の移動情報に基づいて、移動体２の現在の自己位置を算出するオドメトリ自己位置算出部１５と、移動体２の自己位置推定値における移動体２の向きを表すベクトルａ→と、移動体２の自己位置算出値における移動体２の向きを表すベクトルｂ→とを算出するベクトル算出部１６と、ベクトルａ→とベクトルｂ→とのなす角が操舵可能角度以下であると判断されたときは、自己位置推定値を走行経路の位置として決定し、ベクトルａ→とベクトルｂ→とのなす角が操舵可能角度以下でないと判断されたときは、自己位置算出値を走行経路の位置として決定する経路決定部１９とを備える。【選択図】図１

Description

本発明は、走行経路設定装置に関する。

例えば特許文献１には、手動運転時の走行経路を記憶するティーチング（教示）モードを実施した後、記憶した走行経路に沿って自律走行するプレーバック（再生）モードを実施するという技術が記載されている。ティーチングモードでは、Ｄ－ＧＰＳ位置検出部及び推測航法位置検出部により測位した自律走行作業車の現在の自己位置と、地磁気センサの出力に基づいて検出した自律走行作業車の方位及び走行方向（前進／後退）とがメモリに記憶される。

特開２０００－２９５１７号公報

しかしながら、上記従来技術においては、以下の問題点が存在する。即ち、ティーチングモードを実施した後のプレーバックモードでは、自律走行作業車の自己位置推定値のばらつきを考慮して自律走行作業車を走行させるように、駆動機構部及び後輪操舵機構部が制御される。しかし、ティーチングモードの実施時に、自己位置推定値のばらつき具合によっては、自律走行作業車が自律走行不可能な走行経路が設定されてしまうことがある。この場合には、プレーバックモードにおいて、自律走行作業車の走行制御が不可能になる。例えば、ティーチングモードにより設定された走行経路が自律走行作業車の操舵可能角度を超過するような経路である場合には、自律走行作業車を走行経路に沿って自律走行させることができない。

本発明の目的は、移動体が走行不可能な走行経路が設定されることを防止できる走行経路設定装置を提供することである。

本発明の一態様は、移動体の自己位置を推定しながら移動体の走行を行うことにより、移動体の走行経路を設定する走行経路設定装置において、移動体の現在の自己位置を推定する自己位置推定部と、移動体の移動量及び移動方向を含む移動体の移動情報を取得する移動情報取得部と、移動情報取得部により取得された移動体の移動情報に基づいて、移動体の現在の自己位置を算出する自己位置算出部と、自己位置推定部により推定して得られた移動体の自己位置推定値における移動体の向きを表す第１向きデータと、自己位置算出部により算出して得られた移動体の自己位置算出値における移動体の向きを表す第２向きデータとを算出する向きデータ算出部と、向きデータ算出部により算出された第１向きデータと第２向きデータとの差分に相当する値が移動体の操舵に関する閾値以下であるかどうかを判断する判定部と、判定部により第１向きデータと第２向きデータとの差分に相当する値が閾値以下であると判断されたときは、移動体の自己位置推定値を移動体の走行経路の位置として決定し、判定部により第１向きデータと第２向きデータとの差分に相当する値が閾値以下でないと判断されたときは、移動体の自己位置算出値を移動体の走行経路の位置として決定する経路決定部とを備える。

このような走行経路設定装置においては、移動体の現在の自己位置が推定されることで、移動体の自己位置推定値が得られる。また、移動体の移動量及び移動方向を含む移動体の移動情報に基づいて移動体の現在の自己位置が算出されることで、移動体の自己位置算出値が得られる。そして、自己位置推定値における移動体の向きを表す第１向きデータと、自己位置算出値における移動体の向きを表す第２向きデータとが算出される。そして、第１向きデータと第２向きデータとの差分に相当する値が移動体の操舵に関する閾値以下であるかどうかが判断される。第１向きデータと第２向きデータとの差分に相当する値が閾値以下であると判断されたときは、自己位置推定値が走行経路の位置として決定され、第１向きデータと第２向きデータとの差分に相当する値が閾値以下でないと判断されたときは、自己位置算出値が走行経路の位置として決定される。このため、第１向きデータと第２向きデータとの差分に相当する値が閾値よりも大きいときは、自己位置推定値が走行経路の位置として設定されることはなく、自己位置算出値が走行経路の位置として設定される。従って、自己位置推定部により得られる移動体の自己位置推定値のばらつきが大きくても、移動体の操舵が可能となる走行経路が設定されることとなる。これにより、移動体が走行不可能な走行経路が設定されることが防止される。

第１向きデータは、移動体の自己位置推定値と移動体の前回位置とのベクトルであり、第２向きデータは、移動体の自己位置算出値と移動体の前回位置とのベクトルであり、経路決定部は、自己位置推定値及び自己位置算出値のうち走行経路の位置として決定されたほうを次の前回位置に設定してもよい。このような構成では、第１向きデータ及び第２向きデータとしてベクトルを求めることにより、自己位置推定値及び自己位置算出値における移動体の向きが簡素な演算処理により得られる。

第１向きデータと第２向きデータとの差分に相当する値は、第１向きデータを構成するベクトルと第２向きデータを構成するベクトルとのなす角であり、閾値は、移動体の操舵が可能となる操舵可能角度であってもよい。このような構成では、第１向きデータを構成するベクトルと第２向きデータを構成するベクトルとのなす角が操舵可能角度以下であるときは、自己位置推定値が走行経路の位置として設定され、第１向きデータを構成するベクトルと第２向きデータを構成するベクトルとのなす角が操舵可能角度以下でないときは、自己位置算出値が走行経路の位置として設定される。従って、移動体の操舵角度が操舵可能角度を超過しない適切な走行経路が設定される。

移動情報取得部は、移動体の移動距離を検出するセンサと、移動体の操舵角度を検出するセンサと、移動体の向きを検出するセンサとを含んでもよい。このような構成では、移動体の移動距離、操舵角度及び向きを検出することにより、移動体の移動量及び移動方向を含む移動情報を容易に且つ安価に取得することができる。

本発明によれば、移動体が走行不可能な走行経路が設定されることを防止できる。

本発明の一実施形態に係る走行経路設定装置を概略的に示すブロック図である。 移動体の自己位置算出値、移動体の自己位置推定値と移動体の前回位置とのベクトル、及び移動体の自己位置算出値と移動体の前回位置とのベクトルを示す概念図である。 図１に示されたティーチング処理部により実行される走行経路設定処理の手順の詳細を示すフローチャートである。 比較例として、オドメトリ自己位置算出部が無い場合に設定される走行経路の一例を示す概念図である。 図１に示された走行経路設定装置により設定される走行経路の一例を示す概念図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。

図１は、本発明の一実施形態に係る走行経路設定装置を概略的に示すブロック図である。図１において、本実施形態の走行経路設定装置１は、例えばタガー（牽引台車）等の移動体２（図２参照）を自動走行させる際の走行経路を設定する装置である。このような移動体２の走行経路の設定は、いわゆるティーチングと称される。ティーチングは、移動体２の手動運転によって実施される。

移動体２は、図２に示されるように、駆動操舵輪である前輪３と、左右１対の後輪４とを有している。前輪３は、特に図示はしないが、走行モータにより回転可能であると共に、ステアリング操作または操舵モータにより転舵可能である。走行経路設定装置１は、移動体２に搭載されている。走行経路設定装置１は、移動体２の自己位置を推定しながら移動体２の走行を行うことにより、移動体２の走行経路を設定する。

走行経路設定装置１は、レーザセンサ５と、ホイールエンコーダ６と、操舵ポテンショメータ７と、ジャイロセンサ８と、入力器９と、指示ボタン１０と、コントローラ１１と、記憶部１２とを備えている。

レーザセンサ５は、移動体２の周囲にレーザ光を照射し、そのレーザ光の反射光を受光することにより、移動体２から移動体２の周囲に存在する物体までの距離を検出する。物体は、壁や柱等であり、環境地図として登録されている。レーザセンサ５としては、例えばレーザレンジファインダが使用される。

ホイールエンコーダ６は、前輪３の回転角度を計測することにより、移動体２の移動距離を検出するセンサである。操舵ポテンショメータ７は、前輪３の転舵角度を計測することにより、移動体２の操舵角度を検出するセンサである。ジャイロセンサ８は、移動体２の角速度を計測することにより、移動体２の向きを検出するセンサである。ジャイロセンサ８は、環境地図のＸ座標軸を基準とした移動体２の向きを検出する（図２参照）。

ホイールエンコーダ６、操舵ポテンショメータ７及びジャイロセンサ８は、移動体２の移動量及び移動方向を含む移動体２の移動情報を取得する移動情報取得部１３を構成している。ホイールエンコーダ６は、移動体２の移動量を取得する。操舵ポテンショメータ７及びジャイロセンサ８は、移動体２の移動方向を取得する。

入力器９は、ティーチングの開始時における移動体２の初期位置を設定入力する機器である。移動体２の初期位置は、２次元座標（ＸＹ座標）で表される。指示ボタン１０は、ティーチングの開始及び終了を指示する手動操作スイッチである。

コントローラ１１は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ１１は、ＳＬＡＭ自己位置推定部１４と、オドメトリ自己位置算出部１５と、ベクトル算出部１６と、角度算出部１７と、角度判定部１８と、経路決定部１９とを有している。

ＳＬＡＭ自己位置推定部１４は、レーザセンサ５の検出データと環境地図とを用いて、移動体２の現在の自己位置を推定する。これにより、移動体２の自己位置推定値が得られる。ＳＬＡＭ自己位置推定部１４は、レーザＳＬＡＭ（simultaneous localization andmapping）手法を用いて、移動体２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。ＳＬＡＭは、センサデータを利用して、自己位置推定と環境地図の作成とを同時に行う。

具体的には、ＳＬＡＭ自己位置推定部１４は、レーザセンサ５の検出データと環境地図情報とをマッチングさせて、移動体２の自己位置の推定演算を行う。このとき、ＳＬＡＭ自己位置推定部１４は、入力器９により設定入力された初期位置に基づいて、移動体２の自己位置の推定を開始する。なお、移動体２の自己位置は、２次元座標で表される。ＳＬＡＭ自己位置推定部１４は、レーザセンサ５と協働して、移動体２の現在の自己位置を推定する自己位置推定部を構成する。

オドメトリ自己位置算出部１５は、ホイールエンコーダ６、操舵ポテンショメータ７及びジャイロセンサ８により検出された移動体２の移動情報に基づいて、移動体２の現在の自己位置を算出する自己位置算出部である。これにより、移動体２の自己位置算出値が得られる。このとき、オドメトリ自己位置算出部１５は、入力器９により設定入力された初期位置に基づいて、移動体２の自己位置の算出を開始する。

具体的には、図２（ａ）に示されるように、移動体２の移動距離をｄ、移動体２の操舵角度をΦ、移動体２の向きをαとし、移動体２の前回位置Ｑを（Ｘ_ｎ－１，Ｙ_ｎ－１）とし、移動体２の自己位置算出値Ｂを（Ｘ_ｎ＿ｏｄ，Ｙ_ｎ＿ｏｄ）とすると、自己位置算出値Ｂは下記式で表される。
Ｘ_ｎ＿ｏｄ＝Ｘ_ｎ－１＋ｄ＊ｃｏｓ（α＋Φ）
Ｙ_ｎ＿ｏｄ＝Ｙ_ｎ－１＋ｄ＊ｓｉｎ（α＋Φ）

ベクトル算出部１６は、ＳＬＡＭ自己位置推定部１４により得られた移動体２の自己位置推定値と移動体２の前回位置とのベクトルと、オドメトリ自己位置算出部１５により得られた移動体２の自己位置算出値と移動体２の前回位置とのベクトルとを算出する向きデータ算出部である。自己位置推定値と前回位置とのベクトルは、自己位置推定値における移動体２の向きを表す第１向きデータを構成する。自己位置算出値と前回位置とのベクトルは、自己位置算出値における移動体２の向きを表す第２向きデータを構成する。

具体的には、図２（ｂ）に示されるように、移動体２の自己位置推定値Ａを（Ｘ_ｎ＿ｌｏ，Ｙ_ｎ＿ｌｏ）とすると、自己位置推定値Ａと前回位置Ｑとのベクトルａ^→及び自己位置算出値Ｂと前回位置Ｑとのベクトルｂ^→は、下記の通りである。
ａ^→＝（Ｘ_ｎ＿ｌｏ－Ｘ_ｎ－１，Ｙ_ｎ＿ｌｏ－Ｙ_ｎ－１）
ｂ^→＝（Ｘ_ｎ＿ｏｄ－Ｘ_ｎ－１，Ｙ_ｎ＿ｏｄ－Ｙ_ｎ－１）

角度算出部１７は、自己位置推定値Ａと前回位置Ｑとのベクトルａ^→と、自己位置算出値Ｂと前回位置Ｑとのベクトルｂ^→とのなす角θを、下記式により算出する。ベクトルａ^→とベクトルｂ^→とのなす角θは、第１向きデータと第２向きデータとの差分に相当する値である。
ｃｏｓθ＝（ａ^→・ｂ^→）／（｜ａ^→｜・｜ｂ^→｜）

角度判定部１８は、角度算出部１７により算出されたベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下であるかどうかを判断する。操舵可能角度は、移動体２の操舵が可能となる角度であり、移動体２の仕様等によって予め決まっている。操舵可能角度は、移動体２の操舵に関する閾値に相当する。

角度判定部１８は、角度算出部１７と協働して、ベクトル算出部１６により算出されたベクトルａ^→（第１向きデータ）とベクトルｂ^→（第２向きデータ）との差分に相当する値が移動体２の操舵に関する閾値以下であるかどうかを判断する判定部を構成する。

経路決定部１９は、角度判定部１８の判定結果に応じて、移動体２の走行経路を決定する。経路決定部１９は、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下であると判断されたときは、移動体２の自己位置推定値Ａを移動体２の走行経路の位置として決定し、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下でないと判断されたときは、移動体２の自己位置算出値Ｂを移動体２の走行経路の位置として決定する。

また、経路決定部１９は、移動体２の自己位置推定値Ａ及び自己位置算出値Ｂのうち走行経路の位置として採用されたほうを次の前回位置に設定する。次の前回位置は、ベクトル算出部１６により次回にベクトルａ^→及びベクトルｂ^→を求める際に使用される前回位置である。

ここで、ベクトル算出部１６、角度算出部１７、角度判定部１８及び経路決定部１９は、ＳＬＡＭ自己位置推定部１４により得られた移動体２の自己位置推定値とオドメトリ自己位置算出部１５により得られた移動体２の自己位置算出値とを用いて、移動体２の走行経路を設定するティーチング処理を行うティーチング処理部２０を構成している。

記憶部１２は、ティーチング処理部２０により設定された移動体２の走行経路のデータを記憶する。このとき、記憶部１２は、移動体２の走行経路を形成する各位置座標データを記憶する。

図３は、ティーチング処理部２０により実行される走行経路設定処理の手順の詳細を示すフローチャートである。図３において、ティーチング処理部２０は、まず入力器９により移動体２の初期位置が入力されたかどうかを判断する（手順Ｓ１０１）。

ティーチング処理部２０は、移動体２の初期位置が入力されたと判断したときは、指示ボタン１０によりティーチングの開始が指示されたかどうかを判断する（手順Ｓ１０２）。ティーチング処理部２０は、ティーチングの開始が指示されていないと判断したときは、手順Ｓ１０１を再度実行する。

ティーチング処理部２０は、ティーチングの開始が指示されたと判断したときは、ＳＬＡＭ自己位置推定部１４により得られた移動体２の自己位置推定値Ａを取得する（手順Ｓ１０３）。また、ティーチング処理部２０は、オドメトリ自己位置算出部１５により得られた移動体２の自己位置算出値Ｂを取得する（手順Ｓ１０４）。

そして、ティーチング処理部２０は、自己位置推定値Ａと前回位置Ｑとのベクトルａ^→と、自己位置算出値Ｂと前回位置Ｑとのベクトルｂ^→とを算出する（手順Ｓ１０５）。続いて、ティーチング処理部２０は、ベクトルａ^→とベクトルｂ^→とのなす角θを算出する（手順Ｓ１０６）。

続いて、ティーチング処理部２０は、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下であるかどうかを判断する（手順Ｓ１０７）。ティーチング処理部２０は、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下であると判断したときは、走行経路の位置として自己位置推定値Ａを採用する（手順Ｓ１０８）。

ティーチング処理部２０は、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下でないと判断したときは、走行経路の位置として自己位置算出値Ｂを採用する（手順Ｓ１０９）。なお、移動体２の移動情報に基づいたオドメトリにより算出される自己位置は、短周期では誤差が少ない。このため、自己位置算出値Ｂは、走行経路の位置として適切に使用可能である。

ティーチング処理部２０は、手順Ｓ１０８または手順Ｓ１０９を実行した後、自己位置推定値Ａ及び自己位置算出値Ｂのうち走行経路の位置として選択されたほうを次の前回位置に設定する（手順Ｓ１１０）。

続いて、ティーチング処理部２０は、指示ボタン１０によりティーチングの終了が指示されたかどうかを判断する（手順Ｓ１１１）。ティーチング処理部２０は、ティーチングの終了が指示されていないと判断したときは、手順Ｓ１０３以降を再度実行する。ティーチング処理部２０は、ティーチングの終了が指示されたと判断したときは、走行経路の各位置データを記憶部１２に保存し（手順Ｓ１１２）、本処理を終了する。

ここで、ベクトル算出部１６は、上記の手順Ｓ１０１～Ｓ１０５，Ｓ１１１を実行する。角度算出部１７は、上記の手順Ｓ１０６を実行する。角度判定部１８は、上記の手順Ｓ１０７を実行する。経路決定部１９は、上記の手順Ｓ１０８～Ｓ１１０，Ｓ１１２を実行する。

以上のようなティーチングが実施された後、走行経路に沿った移動体２の自動走行を実施することが可能となる。移動体２の自動走行時には、記憶部１２に記憶された走行経路に沿って移動体２が走行するように、図示しない走行モータ及び操舵モータ（前述）が制御される。

図４は、比較例として、上記のオドメトリ自己位置算出部１５が無い場合に設定される走行経路の一例を示す概念図である。図４において、ティーチングの実施時には、ＳＬＡＭ自己位置推定部１４により得られた自己位置推定値に基づいて、移動体２の走行経路が設定される。なお、点Ｐは移動体２の現在の自己位置推定値であり、点Ｑは移動体２の前回位置であり、点Ｑ_０は移動体２の前々回位置である。また、点Ｓは、移動体２の現在の正しい位置（真値）であり、ここでは移動体２が前回位置から真っ直ぐ進んだ位置である。

このようなティーチングによって、移動体２の操舵角度Φが移動体２の操舵可能角度を超過するような走行経路（実線Ｒ１，Ｒ２参照）が設定されるときは、走行経路の設定不良であり、移動体２を走行経路に沿って自律走行させることができない。

一方、本実施形態では、移動体２の現在の自己位置が推定されることで、移動体２の自己位置推定値が得られる。また、移動体２の移動量及び移動方向を含む移動体２の移動情報に基づいて移動体２の現在の自己位置が算出されることで、移動体２の自己位置算出値が得られる。そして、自己位置推定値における移動体２の向きを表すベクトルａ^→と、自己位置算出値における移動体２の向きを表すベクトルｂ^→とが算出され、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下であるかどうかが判断される。ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下であると判断されたときは、移動体２の自己位置推定値が移動体２の走行経路の位置として決定され、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下でないと判断されたときは、移動体２の自己位置算出値が移動体２の走行経路の位置として決定される。

このようにベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度よりも大きいときは、図５に示されるように、移動体２の自己位置推定値Ａが移動体２の走行経路Ｒａの位置として設定されることはなく、移動体２の自己位置算出値Ｂが移動体２の走行経路Ｒｂの位置として設定される。

従って、本実施形態によれば、ＳＬＡＭ自己位置推定部１４により得られる移動体２の自己位置推定値のばらつきが大きくても、走行経路の設定不良が抑えられ、移動体２の操舵が可能となる走行経路が設定されることとなる。これにより、移動体２が走行不可能な走行経路が設定されることが防止される。その結果、移動体２を走行経路に沿って自律走行させることが可能となる。

また、本実施形態では、ベクトルａ^→及びベクトルｂ^→を求めることにより、自己位置推定値及び自己位置算出値における移動体２の向きが簡素な演算処理により得られる。

また、本実施形態では、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度と比較されるので、移動体２の操舵角度Φが操舵可能角度を超過することがない適切な走行経路が設定される。

また、本実施形態では、移動体２の移動距離、操舵角度及び向きを検出することにより、移動体２の移動量及び移動方向を含む移動情報を容易に且つ安価に取得することができる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下であるときは、自己位置推定値が走行経路の位置として使用され、ベクトルａ^→とベクトルｂ^→とのなす角θが操舵可能角度以下でないときは、自己位置算出値が走行経路の位置として使用されているが、特にそのような形態には限られない。例えば、ベクトルａ^→とベクトルｂ^→とのなす角θの微分値を操舵応答性（操舵速度）の限界値と比較してもよい。

この場合、ベクトルａ^→とベクトルｂ^→とのなす角θの微分値が限界値以下であるときは、自己位置推定値が走行経路の位置として使用され、ベクトルａ^→とベクトルｂ^→とのなす角θの微分値が限界値以下でないときは、自己位置算出値が走行経路の位置として使用される。なお、ベクトルａ^→とベクトルｂ^→とのなす角θの微分値は、第１向きデータと第２向きデータとの差分に相当する値である。操舵応答性の限界値は、移動体２の操舵に関する閾値である。

また、上記実施形態では、レーザセンサ５の検出データを使用したレーザＳＬＡＭ手法によって、移動体２の自己位置が推定されているが、特にレーザＳＬＡＭ手法には限られず、移動体２の周囲を撮像するカメラの画像データを使用した画像ＳＬＡＭによって、移動体２の自己位置を推定してもよい。また、自己位置推定技術としては、特にＳＬＡＭ手法には限られず、例えばＲＴＫ－ＧＮＳＳ（realtimekinematic-global navigation satellite system）測位法等を用いて、移動体２の自己位置を推定してもよい。

また、上記実施形態では、移動情報取得部１３は、移動体２の移動距離を検出するホイールエンコーダ６と、移動体２の操舵角度を検出する操舵ポテンショメータ７と、移動体２の向きを検出するジャイロセンサ８とを含んでいるが、特にそのような形態には限られない。例えば、移動情報取得部１３は、移動体２の角速度及び加速度を計測する慣性計測ユニット（ＩＭＵ）等を含んでもよい。

また、上記実施形態の走行経路設定装置１は、タガーまたはフォークリフト等の産業車両に搭載されているが、本発明は、走行経路に沿って自動走行する操舵可能な移動体であれば、適用可能である。

１…走行経路設定装置、２…移動体、５…レーザセンサ（自己位置推定部）、６…ホイールエンコーダ、７…操舵ポテンショメータ、８…ジャイロセンサ、１３…移動情報取得部、１４…ＳＬＡＭ自己位置推定部（自己位置推定部）、１５…オドメトリ自己位置算出部（自己位置算出部）、１６…ベクトル算出部（向きデータ算出部）、１７…角度算出部（判定部）、１８…角度判定部（判定部）、１９…経路決定部、Ａ…自己位置推定値、Ｂ…自己位置算出値、Ｑ…前回位置、ａ^→…ベクトル（第１向きデータ）、ｂ^→…ベクトル（第２向きデータ）、θ…なす角、Ｒｂ…走行経路。

Claims (4)

1. 移動体の自己位置を推定しながら前記移動体の走行を行うことにより、前記移動体の走行経路を設定する走行経路設定装置において、
   前記移動体の現在の自己位置を推定する自己位置推定部と、
   前記移動体の移動量及び移動方向を含む前記移動体の移動情報を取得する移動情報取得部と、
   前記移動情報取得部により取得された前記移動体の移動情報に基づいて、前記移動体の現在の自己位置を算出する自己位置算出部と、
   前記自己位置推定部により推定して得られた前記移動体の自己位置推定値における前記移動体の向きを表す第１向きデータと、前記自己位置算出部により算出して得られた前記移動体の自己位置算出値における前記移動体の向きを表す第２向きデータとを算出する向きデータ算出部と、
   前記向きデータ算出部により算出された前記第１向きデータと前記第２向きデータとの差分に相当する値が前記移動体の操舵に関する閾値以下であるかどうかを判断する判定部と、
   前記判定部により前記第１向きデータと前記第２向きデータとの差分に相当する値が前記閾値以下であると判断されたときは、前記移動体の自己位置推定値を前記移動体の走行経路の位置として決定し、前記判定部により前記第１向きデータと前記第２向きデータとの差分に相当する値が前記閾値以下でないと判断されたときは、前記移動体の自己位置算出値を前記移動体の走行経路の位置として決定する経路決定部とを備える走行経路設定装置。
2. 前記第１向きデータは、前記移動体の自己位置推定値と前記移動体の前回位置とのベクトルであり、
   前記第２向きデータは、前記移動体の自己位置算出値と前記移動体の前回位置とのベクトルであり、
   前記経路決定部は、前記自己位置推定値及び前記自己位置算出値のうち前記走行経路の位置として決定されたほうを次の前記前回位置に設定する請求項１記載の走行経路設定装置。
3. 前記第１向きデータと前記第２向きデータとの差分に相当する値は、前記第１向きデータを構成するベクトルと前記第２向きデータを構成するベクトルとのなす角であり、
   前記閾値は、前記移動体の操舵が可能となる操舵可能角度である請求項２記載の走行経路設定装置。
4. 前記移動情報取得部は、前記移動体の移動距離を検出するセンサと、前記移動体の操舵角度を検出するセンサと、前記移動体の向きを検出するセンサとを含む請求項１～３の何れか一項記載の走行経路設定装置。

Images