JP2019095256A - 自律走行装置 - Google Patents

Abstract

【課題】磁気センサにより方位を推定可能か否かより正確に判定する。
【解決手段】自律走行装置１００は、本体１と、磁気センサ４と、方位推定部３５１と、判定部３５３と、を備える。磁気センサ４は、本体１に取り付けられる。磁気センサ４は、磁場を磁場ベクトルとして測定する。方位推定部３５１は、磁気センサ４にて測定された実測地磁気ベクトルＶ_mに基づいて、本体１の方位を推定する。判定部３５３は、第１成分Ｉ_mc（ｘｙ）と地磁気ベクトルＶ_gmの水平方向成分である第１基準成分Ｉ_s1との比較結果と、第２成分Ｉ_mc（ｚ）と地磁気ベクトルＶ_gmの鉛直方向成分である第２基準成分Ｉ_s2との比較結果と、に基づいて方位推定部３５１における方位の推定を実行するか否かを判定する。
【選択図】図２

Description

本発明は、磁場を測定するための磁気センサを備え、当該磁気センサによる磁場の測定結果に基づいて方位を推定する自律走行装置に関する。

従来、走行環境を自律的に走行する自律走行装置が知られている。この自律走行装置では、レーザレンジファインダー（ＬＲＦ）などにより得られた周囲の障害物に関する情報、及び／又は、自律走行装置を走行させる車輪の回転量に基づいて、自律走行装置の位置と方位とを推定している。

その一方で、地磁気を磁気センサにて測定したときの結果に基づいて、車両などの移動体の方位を推定する技術が知られている（例えば、特許文献１）。この方位の推定方法は、周囲に存在する障害物に関する情報や車輪の回転量といった情報を必要とすることなく、方位を推定できる。

特開平８−３１３２６０号公報

地磁気は微弱な磁場であり、磁気を発生させる物体及び／又は透磁率が高い材質にて製造された物体が存在する場合には、地磁気はその影響を受ける。磁気を発生させる物体及び／又は透磁率が高い物体としては、例えば、鉄を主成分とする材料にて製造された配管及び柱（鉄柱）などがある。

磁気センサは、磁気を発生させる物体及び／又は透磁率が高い物体の影響を受けた地磁気による磁場を測定する。そのため、磁気センサにて測定された磁場に基づいて推定された方位と実際の方位との間には誤差が存在することがある。

上記の問題を解決するため、特許文献１では、磁場（外部の影響を受けた地磁気）の鉛直方向成分を測定するセンサを設け、当該センサにて磁場の鉛直方向が検出された場合に、磁気センサによる方位の推定を中止している。この場合、地磁気の鉛直方向における外部の影響は検知できる一方、水平方向の影響は検知できない。磁気センサでは、測定された磁場の水平方向成分から方位を推定するので、地磁気に対して鉛直方向に影響がなくとも、水平方向に影響をうけた場合には、磁気センサの磁場の測定結果に基づいて正確な方位を推定できなくなる。

本発明の目的は、磁気センサによる地磁気の測定結果に基づいて方位を推定する自律走行装置において、磁気センサにより方位を推定可能か否かより正確に判定することにある。

以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
本発明の一見地に係る自律走行装置は、本体と、磁気センサと、方位推定部と、判定部と、を備える。本体は、自律走行装置の本体である。磁気センサは、本体に取り付けられる。磁気センサは、ゼロ磁場を原点とし磁場の各成分強度を表す３次元座標系において定義される磁場ベクトルを測定する。方位推定部は、磁気センサにて測定された地磁気の実測値である実測地磁気ベクトルに基づいて、本体の方位を推定する。

判定部は、第１成分と第１基準成分との比較結果と、第２成分と第２基準成分との比較結果と、に基づいて方位推定部における方位の推定を実行するか否かを判定する。
第１成分は、実測地磁気ベクトルの３次元座標系における水平方向の成分である。第１基準成分は、地磁気ベクトルの３次元座標系における水平方向の成分である。地磁気ベクトルは、３次元座標系において定義され、地磁気の大きさと方向とを表すベクトルである。
第２成分は、実測地磁気ベクトルの３次元座標系における鉛直方向の成分である。第２基準成分は、地磁気ベクトルの３次元座標系における鉛直方向の成分である。

上記の自律走行装置では、判定部が、地磁気を磁気センサにて測定したときの実測値の鉛直方向成分と地磁気の鉛直方向成分との比較結果のみでなく、地磁気の実測値の水平方向成分と地磁気の水平方向成分との比較結果も考慮して、磁気センサにて取得した地磁気の実測値を用いて自律走行装置の方位を推定するか否かを判定している。
これにより、磁気センサにて取得した地磁気の実測値を用いて自律走行装置の方位を推定するか否かを、より正確に判定できる。

判定部は、第１成分と第１基準成分との第１差分と、第２成分と第２基準成分との第２差分と、に基づいて方位推定部における方位の推定を実行するか否かを判定してもよい。
これにより、より簡単な計算により、磁気センサにて取得した地磁気の実測値を用いて自律走行装置の方位を推定するか否かを正確に判定できる。

判定部は、第１差分と第２差分との二乗和の平方根が所定の閾値以上であるか否かにより、方位推定部における方位の推定を実行するか否かを判定してもよい。
これにより、簡単な計算により、磁気センサにて取得した地磁気の実測値を用いて自律走行装置の方位を推定するか否かを正確に判定できる。

判定部は、第１差分と第２差分との二乗和の平方根が所定の閾値以上である場合に、方位推定部における方位の推定を実行しないと判定してもよい。
これにより、簡単な計算により、磁気センサにて取得した地磁気の実測値を用いて自律走行装置の方位を推定しないと正確に判定できる。

上記の自律走行装置は、補正部をさらに備えてもよい。補正部は、補正式により実測地磁気ベクトルを補正する。補正式は、本体を１回転させる間に測定された複数の実測地磁気ベクトルが３次元座標系において描く軌跡を、真円軌跡に変換する座標変換を表す式である。真円軌跡は、３次元座標系の水平方向と平行な面内において定義され、当該平行な面の原点を中心とする真円形状の軌跡である。この場合、第１基準成分は真円軌跡の半径に対応し、第２基準成分は３次元座標系の原点と真円軌跡の中心との間の距離に対応する。
これにより、実測地磁気ベクトルから自律走行装置が地磁気に及ぼす影響を排除できる。その結果、磁気センサにて取得した地磁気の実測値を用いて自律走行装置の方位を推定するか否かを正確に判定できる。

地磁気を磁気センサにて測定したときの実測値の鉛直方向成分と地磁気の鉛直方向成分との比較結果のみでなく、地磁気の実測値の水平方向成分と地磁気の水平方向成分との比較結果も考慮することで、磁気センサにて取得した地磁気の実測値を用いて自律走行装置の方位を推定するか否かを正確に判定できる。

第１実施形態の自律走行装置の構成を示す図。 制御部の詳細構成を示す図。 磁場ベクトルを定義する３次元座標系の一例を示す図。 地磁気ベクトルから方位を推定する方法の一例を示す図。 地磁気ベクトルから方位を推定する方法の他の一例を示す図。 地磁気ベクトルから方位を推定する方法のさらに他の一例を示す図。 真円軌跡の一例を示す図。 地磁気ベクトルの算出方法を示すフローチャート。 自律走行装置の影響を受けた地磁気を測定したときの実測地磁気ベクトルが描く軌跡の一例を示す図。 第１変換地磁気ベクトルが描く軌跡の一例を示す図。 第２変換地磁気ベクトルが描く軌跡の一例を示す図。 第３変換地磁気ベクトルが描く軌跡の一例を示す図。 自律走行中の自律走行装置の動作を示すフローチャート。

１．第１実施形態
（１）自律走行台車の全体構成
本実施形態の自律走行装置１００は、距離センサとマップマッチングを使用したＳＬＡＭによる自己位置推定と、磁気センサ４により測定した地磁気の実測値である実測地磁気ベクトルＶ_mに基づく方位推定と、を用いて、走行環境における自律走行装置１００の位置と方位（姿勢）を推定しながら走行する。

以下、図１を用いて、本発明の第１実施形態に係る自律走行装置１００の全体構成を説明する。図１は、第１実施形態の自律走行装置の構成を示す図である。自律走行装置１００は、本体１を有する。本体１は、自律走行装置１００の本体を構成する筐体である。

自律走行装置１００は、走行部２を有する。走行部２は、本体１の下部フレームＬＦに設けられ、自律走行装置１００を走行駆動する。本実施形態における走行部２は、二輪差動型の走行部である。
具体的には、走行部２は、それぞれが下部フレームＬＦの左右に設けられた２つの走行車輪２１ａ、２１ｂを有している。走行部２は、さらに、走行車輪２１ａ、２１ｂにそれぞれ対応して、一対のモータ２３ａ、２３ｂを有している。一対のモータ２３ａ、２３ｂの、出力回転軸には、走行車輪２１ａ、２１ｂが接続されている。これにより、２つの走行車輪２１ａ、２１ｂのそれぞれを独立に回転させることができる。

二輪差動型の走行部２は、走行車輪２１ａ、２１ｂを同一の回転速度にて回転することにより、自律走行装置１００（本体１）を直進させることができる。また、２つの走行車輪２１ａ、２１ｂの回転方向を同一としつつ回転速度を異ならせることにより、自律走行装置１００を直進又は後進させながらその方位（姿勢）を変化できる。さらに、２つの走行車輪２１ａ、２１ｂを互いに逆方向に同一の回転速度にて回転させることにより、自律走行装置１００をその場で回転させることができる。

モータ２３ａの出力回転軸及びモータ２３ｂの出力回転軸のそれぞれには、エンコーダ２５ａ、２５ｂ（図２）が接続されており、モータ２３ａ、２３ｂの回転位置を検出する。

自律走行装置１００は、本体１の中間フレームＭＦに配置された制御部３を有する。制御部３は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ストレージとしての記憶装置（ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）などの記憶装置により構成される）、及び各種インターフェースなどを備えるコンピュータである。後述する制御部３の各構成要素の機能の一部又は全部は、上記の記憶装置に記憶された所定のプログラムを実行することで実現されてもよい。
または、制御部３の各構成要素の機能の一部又は全部は、ＳｏＣ（Ｓｙｓｔｅｍ ｏｎ Ｃｈｉｐ）などのハードウェアにより実現されていてもよい。

自律走行装置１００は、磁気センサ４を有する。磁気センサ４は、本体１の上部フレームＵＦに取り付けられた、磁場の３次元方向の各成分強度を測定する三軸磁気センサである。三軸磁気センサである磁気センサ４は、磁場を（Ｉ_x，Ｉ_y，Ｉ_z）（Ｉ_x：磁場のｘ軸方向の強度成分、Ｉ_y：磁場のｙ軸方向の強度成分、Ｉ_z：磁場のｚ軸方向の強度成分）と３次元の座標値として測定できる。

すなわち、ゼロ磁場（すなわち、磁場が存在しない）を原点とし、本体１の幅方向の成分強度をｘ軸方向、本体１の前後方向の成分強度をｙ軸方向、本体１の高さ方向の成分強度をｚ軸方向とする、磁場の各強度成分を表す３次元座標系において、磁気センサ４にて測定される磁場は、当該３次元座標系の原点から座標値（Ｉ_x，Ｉ_y，Ｉ_z）に延びるベクトルとして定義できる。この磁場を表すベクトルを「磁場ベクトル」と呼ぶことにする。
また、磁場ベクトルは、磁場を表す３次元の座標値（Ｉ_x，Ｉ_y，Ｉ_z）として表すことにする。

このように、３次元の座標値として表される磁場ベクトルを測定することにより、磁気センサ４は、磁場の大きさと方向とを測定することができる。

自律走行装置１００は、操作部５を有する。操作部５は、ユーザによる自律走行装置１００に関する各種設定の入力を受け付ける、例えばキーボード又はタッチパネルなどの入力インターフェースである。
操作部５は、現在の走行状況に関する情報、その他の各種情報を表示するディスプレイを有してもよい。ディスプレイとしては、例えば、液晶ディスプレイ、有機ＥＬディスプレイなどを用いることができる。また、ディスプレイとタッチパネルは一体となっていてもよい。

操作部５にて受け付けたユーザによる入力は、適切な信号変換がなされて、制御部３に出力される。

（２）制御部の詳細構成
以下、自律走行装置１００に備わる制御部３の詳細構成について図２を用いて説明する。図２は、制御部の詳細構成を示す図である。
制御部３は記憶部３１を有する。記憶部３１は、制御部３の記憶装置の記憶領域の少なくとも一部に形成された記憶領域である。記憶部３１は、自律走行装置１００の各種設定値など、自律走行装置１００を走行させるために用いられるパラメータなどを記憶する。環境地図復元用データと、環境地図とを記憶する。

記憶部３１は、実測地磁気ベクトルＶ_mを用いて自律走行装置１００の方位を推定するか否かを判定する基準値を記憶する。
具体的には、記憶部３１は、３次元座標系における地磁気ベクトルの水平方向（３次元座標系のｘ−ｙ平面と平行な方向）の成分である第１基準成分と、鉛直方向（３次元座標系のｚ軸方向）の成分である第２基準成分と、を記憶する。

後述するように、本実施形態においては、所定の環境において自律走行装置１００を水平面内で１回転させる間に取得した実測地磁気ベクトルＶ_mから、地磁気ベクトル（算出地磁気ベクトルＶ_gm'）を算出している。従って、本実施形態においては、算出地磁気ベクトルＶ_gm'の水平方向の成分であるＩ_gm'（ｘｙ）と、算出地磁気ベクトルＶ_gm'の鉛直方向成分であるＩ_gm'（ｚ）と、が記憶部３１に記憶される。

また、記憶部３１は、磁気センサ４による地磁気の測定結果（すなわち、地磁気の実測値）である実測地磁気ベクトルＶ_mの補正を行うための補正式を導出するための行列を記憶する。実測地磁気ベクトルＶ_mの補正式については、後ほど詳しく説明する。また、上記のＩ_gm'（ｘｙ）及びＩ_gm'（ｚ）は、実測地磁気ベクトルＶ_mの補正式を算出する際に算出できる。

記憶部３１は、環境地図データを記憶する。環境地図データは、自律走行装置１００が移動する移動領域を表す地図情報である。記憶部３１は、さらに、走行スケジュールを記憶している。走行スケジュールは、自律走行装置１００が自律的に走行する際の走行経路を示すものである。

制御部３は、ＳＬＡＭ処理部３２を有する。ＳＬＡＭ処理部３２は、位置推定と環境地図データの作成とを同時に実行可能なＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）処理を実行する。
具体的には、ＳＬＡＭ処理部３２は、本体１に取り付けられた距離センサ６から得られた、自律走行装置１００の周囲に存在する障害物に関する情報（この情報をローカルマップと呼ぶことにする）を用いて、位置推定と環境地図データの作成とを実行する。

より具体的には、ＳＬＡＭ処理部３２は、自律走行装置１００の各通過点にて取得したローカルマップを、対応する通過点に配置することで環境地図データを作成する。

また、ＳＬＡＭ処理部３２は、所定の時間において取得されたローカルマップと環境地図データとのマップマッチングによる位置推定結果と、エンコーダ２５ａ、２５ｂから取得した走行車輪２１ａ、２１ｂの回転量に基づいた位置推定結果と、に基づいて、自律走行装置１００の移動領域における位置と姿勢（方位）を推定する。

距離センサ６は、自律走行装置１００から周囲にある障害物までの距離を二次元的に又は三次元的に取得するためのセンサである。距離センサ６は、二次元的に距離を取得するものとして、具体的には、自律走行装置１００の走行方向前後にそれぞれ設けられた前方レーザレンジセンサ及び後方レーザレンジセンサである。

レーザレンジセンサは、例えば、レーザ発振器によりパルス発振されたレーザ光を障害物などの目標物に照射し、目標物から反射した反射光をレーザ受信機により受信することにより、目標物までの距離を算出するレーザレンジファインダ（ＬＲＦ：Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）である。これらは、照射するレーザ光を回転ミラーを用いて所定の角度で扇状にレーザ光を走査できる。
また、距離センサ６は、三次元的に距離を取得するものとして、ステレオカメラやＴＯＦ（Ｔｉｍｅ Ｏｆ Ｆｌｉｇｈｔ）カメラを採用できる。

他の実施形態において、自律走行装置１００は、例えば本体１に取り付けられたＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ：全地球航法衛星システム）受信機７を有してもよい。この場合、ＳＬＡＭ処理部３２は、ＧＮＳＳ受信機７による位置推定結果に基づいて、自律走行装置１００の位置を推定してもよい。

具体的には、ＧＮＳＳ受信機７は、ＳＬＡＭ処理部３２へ、測位により求めた現在地の絶対座標（緯度／経度）を出力する。
これにより、例えば、周囲に障害物が存在しないオープンスペースを自律走行装置１００が走行中であるか、又は、障害物は存在していてもその配置が単純であるために、上記のマップマッチングによる位置推定の精度が低下する場合に、ＧＮＳＳ受信機７による位置推定結果を用いることができる。

上記のように、ＧＮＳＳ受信機７は、現在地の絶対座標を出力できる一方、自律走行装置１００（本体１）の方位については、自律走行装置１００の移動前後の絶対座標の差分を用いても精度よく算出できない。従って、ＳＬＡＭ法による位置推定の精度が低下する箇所を自律走行装置１００が走行中の場合には、上記の磁気センサ４を用いることで、当該箇所における自律走行装置１００の方位（姿勢）を精度よく測定できる。

制御部３は、走行制御部３３を有する。走行制御部３３は、入力される走行指令に基づいて、モータ２３ａ、２３ｂの制御量を生成し、走行部２に出力する。走行制御部３３は、例えば、モーションコントローラである。
自律走行装置１００が自律的に走行するとき、上記の走行指令は、ＳＬＡＭ処理部３２により推定される環境地図上の位置及び姿勢（方位）と走行スケジュールとの比較に基づいて、生成される。

制御部３は、モータ駆動部３４ａ、３４ｂを有する。モータ駆動部３４ａ、３４ｂは、それぞれ、走行制御部３３から入力される制御量と、対応するエンコーダ２５ａ、２５ｂにより検出されるモータ２３ａ、２３ｂの回転位置とに基づいて、対応するモータ２３ａ、２３ｂをフィードバック制御する。

制御部３は、方位算出部３５を有する。方位算出部３５は、磁気センサ４にて測定された実測地磁気ベクトルＶ_mが方位の推定に適切な実測値である場合に、実測地磁気ベクトルＶｍに基づいて、自律走行装置１００の方位を算出する。

具体的には、方位算出部３５は、方位推定部３５１を有する。方位推定部３５１は、磁気センサ４にて測定された実測地磁気ベクトルＶ_mに基づいて、自律走行装置１００（本体１）の方位を推定する。実測地磁気ベクトルＶ_m（地磁気ベクトル）に基づく自律走行装置１００の方位の推定原理については、後ほど詳しく説明する。

方位算出部３５は、判定部３５３を有する。判定部３５３は、実測地磁気ベクトルＶ_mの上記の３次元座標系における水平方向の成分である第１成分と記憶部３１に記憶されている第１基準成分との比較結果と、実測地磁気ベクトルＶ_mの３次元座標系における鉛直方向の成分である第２成分と記憶部３１に記憶されている第２基準成分との比較結果と、に基づいて、方位推定部３５１における方位の推定を実行するか否かを判定する。

方位算出部３５は、補正部３５５を有する。補正部３５５は、記憶部３１に記憶されている補正式により、実測地磁気ベクトルＶ_mを補正する。

後述するように、上記の補正式は、自律走行装置１００の構成要素により影響を受けた地磁気の実測値から、自律走行装置１００の構成要素による影響を除去するための式であると言える。従って、補正部３５５を有することにより、方位算出部３５は、自律走行装置１００の構成要素による影響を除去した実測地磁気ベクトルＶ_mに基づいて、自律走行装置１００の方位を推定できる。

また、判定部３５３を有することにより、方位算出部３５は、（補正後の）実測地磁気ベクトルＶ_mが方位を推定するために適切なものであるか否かを判定できる。その結果、方位算出部３５は、例えば、方位を推定するために適切な実測地磁気ベクトルＶ_mを用いて精度よく方位を推定できる。

（３）方位の推定原理
以下、磁気センサ４により測定した実測地磁気ベクトルＶ_mに基づいて、自律走行装置１００の方位を推定する原理について説明する。以下の説明では、外部の磁場及び物体による影響を受けていない地磁気を磁気センサ４により測定したとする。後述するように、磁気センサ４にて測定される地磁気も３次元座標系おいて各成分強度を有する、すなわち、地磁気に向きと大きさが存在することとなるので、以後、（外部環境の影響を受けていない）地磁気を「地磁気ベクトルＶ_gm」として、その向きと大きさを表すこととする。

また、以下の説明においては、磁場ベクトルを定義する３次元座標系として、図３に示すように、本体１の幅方向をｘ軸方向（左方向を正とする）とし、前後方向をｙ軸方向（前進方向を正とする）とし、高さ方向（高さが高くなる方向を正とする）をｚ軸方向とする。図３は、磁場ベクトルを定義する３次元座標系の一例を示す図である。

磁気センサ４は、本体１に取り付けられており（固定されており）、ｘ軸方向の磁場強度を測定する磁場検出素子と、ｙ軸方向の磁場強度を測定する磁場検出素子と、ｚ軸方向の磁場強度を測定する磁場検出素子と、を有している。したがって、上記の３次元座標系は、自律走行装置１００（本体）の水平方向の姿勢の変化にしたがってｚ軸周りに回転する。その一方、地磁気ベクトルＶ_gmは、常に北を向いており、一定の強度Ｉ_gmを有する。

従って、例えば、自律走行装置１００の前進方向が地磁気の南を向いた場合（図４Ａ）、地磁気ベクトルＶ_gmはｙ軸の負方向を向く。この場合、磁気センサ４は、地磁気ベクトルＶ_gmの水平方向成分として（０，−Ｉ_gm（ｘｙ））（Ｉ_gm（ｘｙ）：地磁気ベクトルＶ_gmの水平方向の強度）を測定する。すなわち、磁気センサ４のｙ軸方向の磁場強度を測定する磁場検出素子が、磁場の強度として−Ｉ_gm（ｘｙ）を検出する。

また、例えば、自律走行装置１００の前進方向が地磁気の西を向いた場合（図４Ｂ）、地磁気ベクトルＶ_gmはｘ軸の負方向を向く。この場合、磁気センサ４は、地磁気ベクトルＶ_gmの水平方向成分として（−Ｉ_gm（ｘｙ），０）を測定する。すなわち、磁気センサ４のｘ軸方向の磁場強度を測定する磁場検出素子が、磁場の強度として−Ｉ_gm（ｘｙ）を検出する。

さらに、例えば、自律走行装置１００の前進方向が、地磁気の北北東〜東北東のいずれかを向いた場合（図４Ｃ）、地磁気ベクトルＶ_gmはｘ−ｙ座標の第１象限方向に向く。この場合、磁気センサ４は、地磁気ベクトルＶ_gmの水平方向成分として（Ｉ_gm（ｘ），Ｉ_gm（ｙ））（Ｉ_gm（ｘ）：地磁気ベクトルＶ_gmのｘ軸方向の強度、Ｉ_gm（ｙ）：地磁気ベクトルＶ_gmのｙ軸方向の強度）を測定する。すなわち、磁気センサ４のｘ軸方向の磁場強度を測定する磁場検出素子がＩ_gm（ｘ）を検出し、ｙ軸方向の磁場強度を測定する磁場検出素子がＩ_gm（ｙ）を検出する。なお、Ｉ_gm（ｘｙ）＝ｓｑｒｔ（（Ｉ_gm（ｘ））²＋（Ｉ_gm（ｙ））²）（ｓｑｒｔ（）：（）内の平方根）との関係が成り立つ。さらに、Ｉ_gm＝ｓｑｒｔ（（Ｉ_gm（ｘｙ））²＋（Ｉ_gm（ｚ））²）との関係が成り立つ。
また、自律走行装置１００の具体的な方位は、前進方向が地磁気の北に向く場合を方位０°とする場合には、ｔａｎ^-1（Ｉ_gm（ｘ）／Ｉ_gm（ｙ））との式から算出できる。

図４Ａ〜図４Ｃは、地磁気ベクトルから方位を推定する方法の一例を示す図である。

このように、磁気センサ４が測定する地磁気ベクトルＶ_gmの水平方向成分の強度と、当該水平方向成分の強度がｘ−ｙ座標のどの象限に存在するかにより、自律走行装置１００の方位を具体的に推定できる。地磁気ベクトルＶ_gmの水平方向成分の強度がｘ−ｙ座標のどの象限に存在するかは、ｘ軸方向の磁場強度の正負とｙ軸方向の磁場強度の正負との関係から推定できる。

ただし、地磁気の向きと実際の方角とは一致しない。例えば、日本の場合、方位磁石が示す北（磁北）は、実際の真北よりも西に約７度ずれている。従って、磁気センサ４の出力から実際の方角を推定する際には、例えば、磁気センサ４の出力から推定された方角を、東に約７度ずらす補正を行う。

上記の場合において、本体を１回転させる間の複数の方位のそれぞれにおいて地磁気ベクトルＶ_gmを測定し３次元座標系にプロットすると、当該測定された複数の地磁気ベクトルＶ_gmは、図５に示すような真円軌跡を描く。図５は、真円軌跡の一例を示す図である。

具体的には、上記の真円軌跡は、３次元座標系の原点Ｏからｚ軸にＩ_gm（ｚ）（Ｉ_gm（ｚ）：地磁気ベクトルのｚ軸方向成分の強度）だけ負方向に移動した位置に存在する３次元座標系の水平方向と平行な面（すなわち、ｚ＝−Ｉ_gm（ｚ）との式で表されるｘ−ｙ平面に平行な平面）内に定義され、当該平行な面の原点（具体的には、（０，０，−Ｉ_gm（ｚ）））を中心とした半径Ｉ_gm（ｘｙ）の円の軌跡である。

後ほど詳しく説明するように、本実施形態において、上記の真円軌跡の半径Ｉ_gm（ｘｙ）と、３次元座標系の原点Ｏと上記の真円軌跡の中心との間の距離（Ｉ_gm（ｚ））は、磁気センサ４において測定された地磁気の実測値である実測地磁気ベクトルＶ_mに基づいて自律走行装置１００の方位を推定するか否かを判定するための基準として用いられる。従って、真円軌跡の半径Ｉ_gm（ｘｙ）を「第１基準成分Ｉ_s1」と呼び、３次元座標系の原点Ｏと上記の真円軌跡の中心との間の距離（Ｉ_gm（ｚ））を「第２基準成分Ｉ_s2」と呼ぶことにする。

（４）算出地磁気ベクトルの算出方法
以下、磁気センサ４にて測定した（外部環境の影響を受けた）地磁気の実測値である実測地磁気ベクトルＶ_mから、外部環境の影響を受けていない地磁気ベクトルを算出する方法について、図６を用いて説明する。図６は、地磁気ベクトルの算出方法を示すフローチャートである。以下においては、実測地磁気ベクトルＶ_mから算出される地磁気ベクトルを「算出地磁気ベクトルＶ_gm’」と定義する。

まず、磁気センサ４を取り付けた本体１（自律走行装置１００）を水平面内で１回転させる。本体１を１回転させる間の複数の方位のそれぞれにおいて、磁気センサ４にて実測地磁気ベクトルＶ_mを取得する（ステップＳ１１）。具体的には、各方位において、磁気センサ４が有する３つの磁気検出素子により、実測地磁気ベクトルＶ_mの各強度成分を測定する。
どの程度の数の実測地磁気ベクトルＶ_mを取得するかについては、計算精度と（制御部３の）処理能力などを考慮して、適宜決定できる。

ステップＳ１１における実測地磁気ベクトルＶ_mの取得は、地磁気に対して、自律走行装置１００による影響以外の影響がないか、あっても微弱な環境で行うことが好ましい。なぜなら、実測地磁気ベクトルＶ_mから地磁気ベクトルＶ_gmを算出する過程にて算出された座標の変換式は、実際に方位を推定する際に実測地磁気ベクトルＶ_mを補正する際に用いるからである。

ステップＳ１１において得られた複数の実測地磁気ベクトルＶ_mは、自律走行装置１００の構成要素の影響を受けて、３次元座標系において、例えば図７Ａに示すような軌跡を描く。図７Ａは、自律走行装置の影響を受けた地磁気を測定したときの実測地磁気ベクトルが描く軌跡の一例を示す図である。

次に、図７Ａに示す複数の実測地磁気ベクトルＶ_mが描く軌跡を回転して、当該軌跡を水平方向（ｘ−ｙ平面）と平行にする（ステップＳ１２）。複数の実測地磁気ベクトルＶ_mが描く軌跡を水平方向と平行にするにあたり、まず、当該複数の実測地磁気ベクトルＶ_mが含まれる３次元座標系における平面を求める。
例えば、複数の実測地磁気ベクトルＶ_mに対して主成分分析を行い、当該主成分分析にて算出された第３主成分を、複数の実測地磁気ベクトルＶ_mが含まれる３次元座標系における平面の法線ベクトルとする。

その後、複数の実測地磁気ベクトルＶ_mが含まれる平面の法線ベクトルをｚ軸に平行とするような回転を表す行列（行列Ｍ１とする）を算出し、当該行列Ｍ１と複数の実測地磁気ベクトルＶ_mとを掛け合わせて、複数の実測地磁気ベクトルＶ_mを座標変換（回転）する。算出した行列Ｍ１は、記憶部３１に記憶される。行列Ｍ１により座標変換後の複数の実測地磁気ベクトルＶ_mを、「第１変換地磁気ベクトルＶ_m’」と呼ぶことにする。
第１変換地磁気ベクトルＶ_m’は、例えば図７Ｂに示すように、水平方向に平行な（ｘ−ｙ平面に平行な）楕円軌跡を描く。図７Ｂは、第１変換地磁気ベクトルが描く軌跡の一例を示す図である。

また、第１変換地磁気ベクトルＶ_m’のｚ軸座標値を、算出地磁気ベクトルＶ_gm’が描く真円軌跡の原点と３次元座標系の原点Ｏとの距離Ｉ_gm’（ｚ）として、記憶部３１に記憶する。当該Ｉ_gm’（ｚ）は、上記の第２基準成分Ｉ_s2と所定のオフセット成分とを含む。

上記の座標変換後、以下に説明する演算は、ｘ−ｙ座標系にて実行される。上記の座標変換後、第１変換地磁気ベクトルＶ_m’が描く楕円軌道を表す式を、例えば、楕円の一般式（長軸と短軸がｘ軸及びｙ軸と平行でなく、中心が原点でない楕円を表し、変数としてｘとｙを含む式）と第１変換地磁気ベクトルＶ_m’とのフィッティングにより算出する。

第１変換地磁気ベクトルＶ_m’が描く楕円を表す式を算出後、当該楕円の中心を、ｘ−ｙ座標系の原点、すなわち、座標値（０，０）に平行移動させる座標変換を表す行列（行列Ｍ２とする）を算出する。当該行列Ｍ２と第１変換地磁気ベクトルＶ_m’とを掛け合わせて、第１変換地磁気ベクトルＶ_m’を座標変換（平行移動）する（ステップＳ１３）。当該座標変換（平行移動）後の第１変換地磁気ベクトルＶ_m’を、「第２変換地磁気ベクトルＶ_m’’」と呼ぶことにする。算出された行列Ｍ２は、記憶部３１に記憶される。
その結果、第２変換地磁気ベクトルＶ_m’’は、例えば図７Ｃに示すような、ｘ−ｙ座標系における原点を中心とした楕円軌道を描く。図７Ｃは、第２変換地磁気ベクトルが描く軌跡の一例を示す図である。

ｘ−ｙ座標系における原点を中心とする楕円軌跡を描く第２変換地磁気ベクトルＶ_m’’を算出後、第２変換地磁気ベクトルＶ_m’’が描く楕円軌道を歪み補正して、半径がＩ_gm’（ｘｙ）である真円軌道に変換する行列（行列Ｍ３とする）を算出し、当該行列Ｍ３と第２変換地磁気ベクトルＶ_m’’とを掛け合わせて、第２変換地磁気ベクトルＶ_m’’を座標変換する（ステップＳ１４）。当該座標変換後の第２変換地磁気ベクトルＶ_m’’を、「第３変換地磁気ベクトルＶ_m’’’」と呼ぶことにする。
その結果、第３変換地磁気ベクトルＶ_m’’’は、例えば図７Ｄに示すような、ｘ−ｙ座標系における原点を中心とした真円軌道を描く。図７Ｄは、第３変換地磁気ベクトルが描く軌跡の一例を示す図である。

算出地磁気ベクトルＶ_gm’の水平方向成分Ｉ_gm’（ｘｙ）である第３変換地磁気ベクトルＶ_m’’’が描く真円軌道の半径は、ｓｑｒｔ（（Ｉ_m’’’（ｘ））²＋（Ｉ_m’’’（ｙ））²）（Ｉ_m’’’（ｘ）：第３変換地磁気ベクトルＶ_m’’’のｘ軸方向成分、Ｉ_m’’’（ｙ）：第３変換地磁気ベクトルＶ_m’’’のｙ軸方向成分）と算出できる。このようにして算出されたＩ_gm’（ｘｙ）は、記憶部３１に記憶される。

行列Ｍ３は、例えば、第２変換地磁気ベクトルＶ_m’’が描く楕円の長軸がｘ軸方向、短軸がｙ軸方向となるよう第２変換地磁気ベクトルＶ_m’’を回転（当該回転を表す行列をＲ１とする）し、当該回転後の第２変換地磁気ベクトルＶ_m’’に対してｘ座標及びｙ座標の拡大／縮小（当該拡大縮小を表す行列をＥとする）を行う。さらに、当該拡大／縮小後の第２変換地磁気ベクトルＶ_m’’を最初の回転とは逆方向に回転（当該回転を表す行列をＲ２とする）する行列として算出される。具体的には、行列Ｍ３は、Ｒ２・Ｅ・Ｒ１（「・」は行列の積を表す）として算出できる。算出された行列Ｍ３は、記憶部３１に記憶される。

上記のようにして、本体１を１回転させる間に測定された複数の実測地磁気ベクトルＶ_mが３次元座標系において描く楕円軌跡を、３次元座標系の水平方向と平行な面内において定義され当該平行な面の原点を中心とする真円軌跡に変換する座標変換を表す行列を算出できる。すなわち、上記のようにして、実測地磁気ベクトルＶ_mから自律走行装置１００が地磁気に及ぼす影響を除去するための補正式に用いる行列を算出できる。

（５）自律走行装置の動作
以下、本実施形態に係る自律走行装置の動作について説明する。以下では、記憶部３１に記憶された走行スケジュールに従って自律的に走行する際の自律走行装置１００の動作を例にとって説明する。自律走行中の自律走行装置１００は、図８に示すフローチャートに従って動作する。

具体的には、まず、ＳＬＡＭ処理部３２が、距離センサ６から自律走行装置１００の周囲に存在する障害物に関する情報を取得する。また、エンコーダ２５ａ、２５ｂから走行車輪２１ａ、２１ｂの回転量を取得する（ステップＳ１）。

自律走行装置１００がＧＮＳＳ受信機７を有する他の実施形態においては、ＳＬＡＭ処理部３２は、ＧＮＳＳ受信機７から測位により求めた現在地の絶対座標を取得してもよい。

次に、ＳＬＡＭ処理部３２は、障害物に関する情報及び／又は走行車輪２１ａ、２１ｂの回転量に基づいて、自律走行装置１００の位置及び姿勢（方位）を推定する（ステップＳ２）。

自律走行装置１００がＧＮＳＳ受信機７を有する他の実施形態においては、ＳＬＡＭ処理部３２は、障害物に関する情報、走行車輪２１ａ、２１ｂの回転量、及び／又は、測位により求めた現在地の絶対座標に基づいて、自律走行装置１００の位置と姿勢を推定してもよい。

ＳＬＡＭ処理部３２による位置及び姿勢の推定後、方位算出部３５は、磁気センサ４から実測地磁気ベクトルＶ_mを取得する（ステップＳ３）。具体的には、ｘ軸方向の磁場強度を測定する磁場検出素子から実測地磁気ベクトルＶ_mのｘ軸方向の強度成分Ｉ_m（ｘ）を取得する。ｙ軸方向の磁場強度を測定する磁場検出素子からｙ軸方向の強度成分Ｉ_m（ｙ）を取得する。ｚ軸方向の磁場強度を測定する磁場検出素子からｚ軸方向の強度成分Ｉ_m（ｚ）を取得する。

これにより、実測地磁気ベクトルＶ_mを、（Ｉ_m（ｘ），Ｉ_m（ｙ），Ｉ_m（ｚ））と３次元座標系の原点から延びるベクトルとして表現できる。

実測地磁気ベクトルＶ_mを取得後、補正部３５５が、取得した実測地磁気ベクトルＶ_mの自律走行装置１００による磁場への影響を除去する補正を実行する（ステップＳ４）。具体的には、まず、Ｍ１・Ｖ_mとの行列の演算式（「・」は行列の積を表す）により、補正後の実測地磁気ベクトルＶ_mcを算出する。補正後の実測地磁気ベクトルＶ_mcは、（Ｉ_mc（ｘ），Ｉ_mc（ｙ），Ｉ_mc（ｚ））と表される。このとき算出される補正後の実測地磁気ベクトルＶ_mcの鉛直方向（ｚ軸方向）の成分（I_mc（ｚ））が第２成分に対応する。

その後、補正後の実測地磁気ベクトルＶ_mcの水平方向成分（Ｖ_mc（ｘｙ）と呼ぶことにする）、すなわち、（Ｉ_mc（ｘ），Ｉ_mc（ｙ））に対して、Ｍ３・Ｍ２・Ｖ_mc（ｘｙ）との行列の演算を実行する。Ｍ３・Ｍ２・Ｖ_mc（ｘｙ）との演算により算出されるベクトルが第１成分Ｉ_mc（ｘｙ）に対応する。

上記の行列の演算式において、行列Ｍ１、及び、行列の演算式Ｍ３・Ｍ２から算出される行列が、実測地磁気ベクトルＶ_mの自律走行装置１００による磁場への影響を除去する補正式に対応する。すなわち、取得した実測地磁気ベクトルＶ_mが自律走行装置１００の構成要素による影響のみを受けているか、又は、外部の磁場及び／又は物体による影響が小さい場合には、行列Ｍ１、及び、Ｍ３・Ｍ２（補正式）は、取得した実測地磁気ベクトルＶ_mが描く軌跡を上記の真円軌跡に変換する行列に対応する。

さらに言い換えると、取得した実測地磁気ベクトルＶ_mが自律走行装置１００の構成要素による影響のみを受けているか、又は、外部の磁場及び／又は物体による影響が小さい場合には、上記の補正式と実測地磁気ベクトルＶ_mとを用いて算出されるベクトルは、上記の真円軌跡上又はその近傍に配置される。

その後、判定部３５３は、補正後の実測地磁気ベクトルＶ_mcの水平方向（ｘ−ｙ平面に平行な方向）の成分である第１成分Ｉ_mc（ｘｙ）（＝ｓｑｒｔ（（Ｉ_mc（ｘ））²＋（Ｉ_mc（ｙ））²））と第１基準成分Ｉ_s1である真円軌跡の半径Ｉ_gm’（ｘｙ）との比較結果と、補正後の実測地磁気ベクトルＶ_mcの鉛直方向（ｚ軸方向）の成分である第２成分Ｉ_mc（ｚ）と記憶部３１に記憶されているＩ_gm’（ｚ）との比較結果と、に基づいて、磁気センサ４にて取得した実測地磁気ベクトルＶ_mが、自律走行装置１００以外の外部の磁界及び／又は外部の物体による影響を受けているか否かを判定する。

具体的には、第１成分Ｉ_mc（ｘｙ）と真円軌跡の半径Ｉ_gm’（ｘｙ）との第１差分ΔＲと、第２成分Ｉ_mc（ｚ）と記憶部３１に記憶されているＩ_gm’（ｚ）との第２差分Δｚとの二乗和の平方根（ｓｑｒｔ（（ΔＲ）²＋（Δｚ）²））が所定の閾値以上であるか否かを判定する。

上記のように、Ｉ_gm’（ｚ）は第２基準成分Ｉ_s2を含んでいる。従って、第２成分Ｉ_mc（ｚ）とＩ_gm’（ｚ）との差分として定義される第２差分Δｚは、第２成分Ｉ_mc（ｚ）と第２基準成分Ｉ_s2との差分（比較結果）を含んでいる。

第１差分ΔＲと第２差分Δｚとの二乗和の平方根が所定の閾値以上である場合、すなわち、補正後の実測地磁気ベクトルＶ_mが真円軌道から離れた位置に存在すると判定した場合（ステップＳ５において「Ｙｅｓ」の場合）、判定部３５３は、実測地磁気ベクトルＶ_mが外部の磁界及び／又は物体による影響を受けていると判定する。この場合、判定部３５３は、方位推定部３５１にて補正後の実測地磁気ベクトルＶ_mcに基づいた方位の推定を実行しないと判定する（ステップＳ６）。

ステップＳ６において、判定部３５３は、例えば、ＳＬＡＭ処理部３２にて推定された自律走行装置１００の姿勢を、自律走行装置１００の方位とする。

一方、第１差分ΔＲと第２差分Δｚとの二乗和の平方根が所定の閾値よりも小さい場合、すなわち、補正後の実測地磁気ベクトルＶ_mcが真円軌道上又はその近傍に存在すると判定した場合（ステップＳ５において「Ｎｏ」の場合）、判定部３５３は、実測地磁気ベクトルＶ_mが外部の磁界及び／又は物体による影響を受けていないかその影響が小さいと判定する。この場合、判定部３５３は、補正後の実測地磁気ベクトルＶ_mcに基づいて方位を推定すると判定する（ステップＳ７）。

ステップＳ７において、方位推定部３５１は、例えば、上記の「（３）方位の推定原理」にて説明したように、ｔａｎ^-1（Ｉ_mc（ｘ）／Ｉ_mc（ｙ））の値と、補正後の実測地磁気ベクトルＶ_mcの水平方向成分（Ｉ_mc（ｘ），Ｉ_mc（ｙ））がｘ−ｙ座標系において存在する象限と、に基づいて推定された方位にさらに所定の補正（例えば、推定された方位を東に約７度ずらす補正）を行い、自律走行装置１００の実際の方位を推定する。

上記のようにして自律走行装置１００の現在の位置及び方位（姿勢）を推定後、走行制御部３３が、当該現在の推定位置及び方位（姿勢）と、走行スケジュールに示された自律走行装置１００の目標到達位置及び方位（姿勢）と、の差分に基づいて走行指令を算出する。走行制御部３３は、算出した走行指令をモータ駆動部３４ａ、３４ｂに出力する。

これにより、走行部２は、現在の推定位置及び方位（姿勢）と、自律走行装置１００の目標到達位置及び方位（姿勢）と、に基づいて制御される（ステップＳ８）。この結果、自律走行装置１００は、走行スケジュールに従って自律的に走行できる。

走行部２を制御後、制御部３は、走行スケジュールに示された全ての通過点を通過したか否かを判定する。全ての通過点を通過していない場合（ステップＳ９において「Ｎｏ」の場合）には、上記のステップＳ１〜Ｓ８を繰り返し実行する。

一方、全ての通過点を通過した場合（ステップＳ９において「Ｙｅｓ」の場合）には、制御部３は、自律走行のための制御を終了する。

上記のように、本実施形態の自律走行装置１００では、判定部３５３が、地磁気を磁気センサ４にて測定したときの実測値（実測地磁気ベクトルＶ_m）の鉛直方向成分Ｉ_m（ｚ）と地磁気（算出地磁気ベクトルＶ_gm'）の鉛直方向成分Ｉ_gm'（ｚ）との比較結果のみでなく、実測地磁気ベクトルＶ_mの水平方向成分Ｉ_m（ｘｙ）と算出地磁気ベクトルＶ_gm'の水平方向成分Ｉ_gm'（ｘｙ）との比較結果も考慮して、磁気センサ４にて取得した地磁気の実測値を用いて自律走行装置１００の方位を推定するか否かを判定している。

これにより、磁気センサ４にて取得した地磁気の実測値を用いて自律走行装置１００の方位を推定するか否かを、より正確に判定できる。なぜなら、本実施形態においては、地磁気の実測値により方位を推定するか否かを判定するときに、自律走行装置１００の方位を決定する実測地磁気ベクトルＶ_mの水平方向成分Ｉ_m（ｘｙ）が外部の磁場及び／又は物体の影響を受けているか否かを考慮しているからである。

また、本実施形態においては、判定部３５３は、第１成分Ｉ_mc（ｘｙ）と算出地磁気ベクトルＶ_gm'の水平方向成分Ｉ_gm’（ｘｙ）（第１基準成分Ｉ_s1に対応）との第１差分ΔＲと、第２成分Ｉ_mc（ｚ）と算出地磁気ベクトルＶ_gm'の鉛直方向成分Ｉ_gm'（ｚ）との第２差分Δｚ（第２成分Ｉ_mc（ｚ）と第２基準成分Ｉ_s2との差分を含む）と、の二乗和の平方根ｓｑｒｔ（（ΔＲ）²＋（Δｚ）²）が所定の閾値以上であるか否かにより、方位推定部３５１における方位の推定を実行するか否かを判定している。

これにより、簡単な計算により、磁気センサ４にて取得した地磁気の実測値を用いて自律走行装置１００の方位を推定するか否かを正確に判定できる。

自律走行装置１００の外部において存在する磁場の向き及び大きさ、及び、自律走行装置１００の周囲に存在する物体と当該物体との距離は、自律走行装置１００の移動に伴って変化する。すなわち、自律走行装置１００の周囲における外部の磁場及び／又は物体の地磁気への影響は、自律走行装置１００の移動に伴って変化する。従って、所定の環境において算出された上記の補正式を用いて実測地磁気ベクトルＶ_mを補正しても、外部の磁場及び／又は物体の地磁気への影響を除去することはできない。

このため、本実施形態において、判定部３５３は、第１差分ΔＲと第２差分Δｚとの二乗和の平方根が所定の閾値以上である場合、すなわち、外部の磁場及び／又は物体の地磁気への影響が大きいと判定される場合に、方位推定部３５１における方位の推定を実行しないと判定している。

また、第１差分ΔＲと第２差分Δｚとの二乗和の平方根が所定の閾値以上である場合に方位推定部３５１における方位の推定を実行しないと判定することにより、簡単な計算により、磁気センサ４にて取得した実測地磁気ベクトルＶ_mを用いて自律走行装置１００の方位を推定しないと正確に判定できる。

（６）他の実施形態
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。
（Ａ）第１実施形態において説明した図６及び／又は図８のフローチャートに示した各処理の順番及び／又は処理は、適宜変更してもよい。例えば、図８に示すフローチャートにおいて、障害物に関する情報と実測地磁気ベクトルＶ_mとを取得後に、自律走行装置１００の位置と方位の推定を実行してもよい。すなわち、ステップＳ１とＳ３とを最初に実行し、その後、ステップＳ２とＳ４を実行してもよい。

（Ｂ）または、図８に示すフローチャートにおいて、最初にステップＳ３〜Ｓ７を実行して実測地磁気ベクトルＶ_mを用いて方位を推定するか否かを決定し、その後、ステップＳ１及びＳ２を実行してＳＬＡＭ処理部３２による位置と方位の推定を実行してもよい。

（Ｃ）判定部３５３は、第１差分ΔＲと第２差分Δｚとの二乗和の平方根以外を用いて、実測地磁気ベクトルＶ_mに基づいて方位を推定するか否かを判定してもよい。例えば、判定部３５３は、第１差分ΔＲの絶対値と第２差分Δｚの絶対値との和に基づいて、実測地磁気ベクトルＶ_mに基づいて方位を推定するか否かを判定できる。

（Ｄ）判定部３５３は、第１成分Ｉ_mc（ｘｙ）と第２成分Ｉ_mc（ｚ）との比（Ｉ_mc（ｘｙ）／Ｉ_mc（ｚ））と、第１成分Ｉ_mc（ｘｙ）又は第２成分Ｉ_mc（ｚ）と、に基づいて、実測地磁気ベクトルＶ_mに基づいて方位を推定するか否かを判定してもよい。

例えば、上記の比Ｉ_mc（ｘｙ）／Ｉ_mc（ｚ）が、第１基準成分Ｉ_s1と第２基準成分Ｉ_s2との比（Ｉ_s1／Ｉ_s2）を中心とした所定の範囲内の値であり、かつ、第１成分Ｉ_mc（ｘｙ）が第１基準成分Ｉ_s1を中心とした所定の範囲内の値であるときに、実測地磁気ベクトルＶ_mに基づいて方位を推定すると判定できる。

（Ｅ）自律走行装置１００は、操作者の操作により走行可能であってもよい。また、操作者の操作を走行スケジュールとして教示可能であってもよい。この場合、自律走行装置１００は、例えば、操作部５を操作するか、又は、ジョイスティックなどの外部のコントローラを用いて操作可能となる。

走行スケジュールの教示において、制御部３は、判定部３５３の判定結果に基づいて、磁気センサ４にて測定した実測地磁気ベクトルＶ_mに基づいた方位の推定値を走行スケジュールに記憶させるか、又は、ＳＬＡＭ処理部３２にて推定された姿勢を走行スケジュールに記憶させるかを判断してもよい。

（Ｆ）第１実施形態において説明した実測地磁気ベクトルＶ_mに基づいて方位を推定するか否かの判定方法は、自律的に走行する自律走行装置１００以外にも、例えば、３軸の磁気センサにより方位を推定する移動体（車両、船舶など）にも適用できる。

本発明は、磁場を測定するための磁気センサを備え、当該磁気センサによる磁場の測定結果に基づいて方位を推定する移動体に広く適用できる。

１００ 自律走行装置
１ 本体
ＬＦ 下部フレーム
ＭＦ 中間フレーム
ＵＦ 上部フレーム
２ 走行部
２１ａ、２１ｂ 走行車輪
２３ａ、２３ｂ モータ
２５ａ、２５ｂ エンコーダ
３ 制御部
３１ 記憶部
３２ ＳＬＡＭ処理部
３３ 走行制御部
３４ａ、３４ｂ モータ駆動部
３５ 方位算出部
３５１ 方位推定部
３５３ 判定部
３５５ 補正部
４ 磁気センサ
５ 操作部
６ 距離センサ
７ ＧＮＳＳ受信機
Ｖ_gm 地磁気ベクトル
Ｖ_gm' 算出地磁気ベクトル
Ｉ_gm（ｘｙ） 地磁気ベクトルの水平方向成分
Ｉ_gm（ｚ） 地磁気ベクトルの鉛直方向成分
Ｖ_m 実測地磁気ベクトル
Ｉ_m（ｘｙ） 実測地磁気ベクトルの水平方向成分
Ｉ_m（ｚ） 実測地磁気ベクトルの鉛直方向成分
Ｖ_mc 補正後の実測地磁気ベクトル
Ｉ_mc（ｘｙ） 第１成分（補正後の実測地磁気ベクトルの水平方向成分）
Ｉ_mc（ｚ） 第２成分（補正後の実測地磁気ベクトルの鉛直方向成分）
ΔＲ 第１差分
Δｚ 第２差分

Claims (5)

1. 本体と、
   前記本体に取り付けられ、ゼロ磁場を原点とし磁場の各成分強度を表す３次元座標系において定義される磁場ベクトルを測定する磁気センサと、
   前記磁気センサにて測定された地磁気の実測値である実測地磁気ベクトルに基づいて前記本体の方位を推定する方位推定部と、
   前記実測地磁気ベクトルの前記３次元座標系における水平方向の成分である第１成分と前記３次元座標系において定義され地磁気の大きさと方向とを表す地磁気ベクトルの前記水平方向の成分である第１基準成分との比較結果と、前記実測地磁気ベクトルの前記３次元座標系における鉛直方向の成分である第２成分と前記地磁気ベクトルの前記鉛直方向の成分である第２基準成分との比較結果と、に基づいて、前記方位推定部における方位の推定を実行するか否かを判定する判定部と、
   を備える自律走行装置。
2. 前記判定部は、前記第１成分と前記第１基準成分との第１差分と、前記第２成分と前記第２基準成分との第２差分と、に基づいて、前記方位推定部における方位の推定を実行するか否かを判定する、請求項１に記載の自律走行装置。
3. 前記判定部は、前記第１差分と前記第２差分との二乗和の平方根が所定の閾値以上であるか否かにより、前記方位推定部における方位の推定を実行するか否かを判定する、請求項２に記載の自律走行装置。
4. 前記判定部は、前記第１差分と前記第２差分との二乗和の平方根が前記所定の閾値以上である場合に、前記方位推定部における方位の推定を実行しないと判定する、請求項３に記載の自律走行装置。
5. 前記本体を１回転させる間に測定された複数の前記実測地磁気ベクトルが前記３次元座標系において描く軌跡を、前記３次元座標系の水平方向と平行な面内において定義され当該平行な面の原点を中心とする真円軌跡に変換する座標変換を表す補正式により前記実測地磁気ベクトルを補正する補正部をさらに備え、
   前記第１基準成分は前記真円軌跡の半径に対応し、前記第２基準成分は前記３次元座標系の原点と前記真円軌跡の中心との間の距離に対応する、
   請求項１〜４のいずれかに記載の自律走行装置。

Images