JP3681081B2 - 地磁気センサの補正装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、地磁気センサの方位検出誤差を補正するためのキャリブレーションを行う地磁気センサの補正装置に関する。
【０００２】
【従来の技術】
周知のように、車両に地磁気センサを搭載して進行方位を正確に検出するためには、予め地磁気センサのキャリブレーションを行い、車体や周囲の磁界の影響を補正する必要がある。
【０００３】
この地磁気センサのキャリブレーションは、図７に示すように、車体に地磁気センサを固定した状態で水平且つ平らな路面上で１回転させたときに観測される各Ｘ，Ｙ軸の磁気検知コイルの円状の出力軌跡の中心とゲイン（各軸方向の直径）を求めることであり、特開平４−１１０７１７号公報や特開平４−３４６０２０号公報等に開示されているように、地磁気センサを連続的に回すことができない場合、三点以上のデータから出力軌跡の中心とゲインを推定する、地磁気センサを連続的に回すことができる場合、Ｘ，Ｙコイルの各最大値と各最小値とから出力軌跡の中心とゲインを求める等の手法が採用されている。
【０００４】
【発明が解決しようとする課題】
しかしながら、従来の地磁気センサのキャリブレーションでは、情報が少ないため精度に限界があり、周囲の外部磁界の状況、車体の帯磁状況、地磁気センサの振動等により出力軌跡の中心点が本来の中心点からずれ、誤差を含んだ方位が検出される虞がある。
【０００５】
本発明は上記事情に鑑みてなされたもので、地磁気センサのキャリブレーションを振動や周囲の外部磁界の影響を受け難いものとし、地磁気センサによる正確な方位検出を可能とする地磁気センサの補正装置を提供することを目的としている。
【０００６】
【課題を解決するための手段】
上記目的を達成するため、請求項１記載の発明は、図１の基本構成図に示すように、地磁気センサを搭載した車両を一定方向に設定時間旋回させ、上記地磁気センサの出力軌跡の中心位置を仮のキャリブレーション値として求める粗キャリブレーション手段と、上記粗キャリブレーション手段で求めた仮のキャリブレーション値を用いて方位演算することにより車両を一定速度で１周旋回させ、１周の旋回で得られた上記地磁気センサの出力データを用いて上記地磁気センサの出力を三角関数で近似することにより上記地磁気センサの出力軌跡の中心位置を求め、上記地磁気センサの方位検出誤差を補正するための精密キャリブレーション値とする精密キャリブレーション手段とを備えたことを特徴とする。
【０００８】
請求項２記載の発明は、請求項１記載の発明において、上記精密キャリブレーション手段は、上記地磁気センサの出力を最小二乗法により三角関数で近似することを特徴とする。
【０００９】
すなわち、請求項１記載の発明では、まず、粗キャリブレーションにより、地磁気センサを搭載した車両を一定方向に設定時間旋回させて地磁気センサの出力軌跡の中心位置を仮のキャリブレーション値として求め、次に、この仮キャリブレーション値を用いて車両を一定速度で正確に１周旋回させ、１周の旋回で得られた地磁気センサの出力データを用いて、地磁気センサの出力を三角関数で近似することにより地磁気センサの出力軌跡の中心位置を精密キャリブレーション値として求める。
【００１０】
この際、請求項２記載の発明では、三角関数への近似を最小二乗法により行う。
【００１１】
【発明の実施の形態】
以下、図面を参照して本発明の実施の形態を説明する。図２〜図６は本発明の実施の一形態を示し、図２及び図３は地磁気センサのキャリブレーション処理ルーチンのフローチャート、図４は地磁気センサのＸコイル及びＹコイルの出力を示す説明図、図５は自律走行制御系のブロック図、図６は地磁気センサを搭載した自律走行車の外観図である。
【００１２】
図６において、符号１は無人で自走可能な自律走行作業車を示し、本形態においては、ゴルフ場等の草・芝刈作業を行なう芝刈作業車である。この芝刈作業車１は、エンジン駆動で走行し、前後輪の操舵角が独立して制御されるようになっており、車両本体下部に、草・芝刈作業を行うためのモーア等の刈刃機構部２を備えるとともに、基準位置からの走行履歴に基づいて現在位置を測定するための推測航法用センサ、走行障害物を検出するための障害物検出用センサ等を備え、さらに、走行及び操舵制御のための各種センサ類・アクチュエータ類を備えている。
【００１３】
上記推測航法用センサとしては、上記芝刈作業車１の後部に備えられた地磁気センサ３、及び、左右の後輪に備えられた車輪エンコーダ４ａ，４ｂを備え、上記障害物検出用センサとしては、超音波センサあるいは光センサ等からなる無接触型センサ５ａ，５ｂが上記芝刈作業車１の前後部に取り付けられるとともに、スイッチ式の接触型センサ６ａ，６ｂが上記芝刈作業車１の前後端に取り付けられている。
【００１４】
上記地磁気センサ３は、地磁気ベクトルの水平成分を検出するため、励磁コイルを巻回したリング状コアの磁心に、互いに直交する南北検出用のＸコイルと東西検出用のＹコイルとを巻回、配置した周知の方位検出センサであり、正確な方位を検出するためには、予めキャリブレーションを行い、地磁気の乱れや周囲の外部磁界の影響による交流的な誤差を除去する必要がある。
【００１５】
上記地磁気センサ３のキャリブレーションは、図５に示す車載の制御装置２０によって行われる。この制御装置２０は、上記地磁気センサ３によって検出した芝刈作業車１の進行方位を目標方位との誤差に応じて修正し、自律走行を制御するものであり、自律走行制御開始に先立ち、図示しないキャリブレーションスイッチからＯＮ信号により、あるいは、図示しないハンディーターミナルからのコマンド入力により、上記地磁気センサ３のキャリブレーションを実行できるようになっている。すなわち、上記制御装置２０に上述の各センサ類や後述するアクチュエータ類が接続されて本発明に係わる粗キャリブレーション手段、精密キャリブレーション手段、及び、その他の制御手段の機能を実現する。
【００１６】
以下、上記制御装置２０の詳細について説明する。上記制御装置２０は、例えば複数のマイクロコンピュータからなり、上記地磁気センサ３及び上記車輪エンコーダ４ａ，４ｂが接続される推測航法位置検出部２１、上記無接触型センサ５ａ，５ｂ及び上記接触型センサ６ａ，６ｂが接続される障害物検出部２２、各検出部２１，２２からの信号に基づいて芝刈作業車１の自律走行を制御する走行制御部２３、この走行制御部２３によって参照される作業データ・マップ等が格納されている作業データ蓄積部２４、上記走行制御部２３からの指示によって車両制御を行なう車両制御部２５が備えられ、さらに、この車両制御部２５からの出力に基づいて芝刈作業車１の各機構部を駆動するため、駆動制御部２６、操舵制御部２７、刈刃制御部２８等が備えられている。
【００１７】
上記推測航法位置検出部２１では、上記車輪エンコーダ４ａ，４ｂからの出力パルスを積算して走行距離を求め、この走行距離を上記地磁気センサ３で検出した走行方向の変化に対応させて累積することにより、基準地点からの走行履歴を算出して自車両の現在位置を測定する。また、上記障害物検出部２２は、無接触型センサ５ａ，５ｂ及び接触型センサ６ａ，６ｂによって予測できない障害物を検出し、検出信号を上記走行制御部２３に出力する。
【００１８】
上記走行制御部２３では、上記推測航法位置検出部２１からの情報に基づいて、作業データ蓄積部２４のマップ（作業内容及び草・芝刈り作業を行う作業領域の地形データ等の走行用地図）を参照して自己の現在位置と目標位置との誤差量を算出し、この誤差量分を補正すべく走行経路指示や車両制御指示を決定し、上記車両制御部２５に出力する。また、上記障害物検出部２２からの信号により、障害物が検出されたときには、障害物回避あるいは車両停止を指示する。
【００１９】
上記作業データ蓄積部２４は、草・芝刈作業を行なう作業領域の地形データ等が記憶されるエリアと、各センサからの信号を処理したデータ、推測航法による測位データ等の制御実行中のワークデータが記憶されるエリアから構成されている。
【００２０】
上記車両制御部２５では、上記走行制御部２３からの指示を具体的な制御指示量に変換し、この制御指示量を上記駆動制御部２６、操舵制御部２７、刈刃制御部２８に出力する。これにより、上記駆動制御部２６では、スロットル開度を調整してエンジン出力を制御するためのスロットルアクチュエータ、変速アクチュエータ、前後進切換アクチュエータ、ブレーキアクチュエータ等の走行制御アクチュエータ８を駆動し、また、油圧ポンプ９を制御して各機能部を駆動するための油圧を発生させる。
【００２１】
上記操舵制御部２７では、操舵系に備えられた前輪舵角センサ１０ａ、後輪舵角センサ１０ｂからの入力に基づいて前輪操舵用油圧制御弁１１ａ、後輪操舵用油圧制御弁１１ｂを介して操舵制御（操舵量フィードバック制御）を行ない、刈刃制御部２８では、刈刃制御用油圧制御弁１２を介して刈刃機構部２のサーボ制御を行なう。
【００２２】
以下、上記制御装置２０において、図示しないキャリブレーションスイッチからＯＮ信号により、あるいは、図示しないハンディーターミナルからのコマンド入力により実行される地磁気センサ３のキャリブレーション処理について図２及び図３のフローチャートに従って説明する。尚、この地磁気センサ３のキャリブレーションは平坦地での実行を前提とする。
【００２３】
このキャリブレーション処理ルーチンでは、まず、ステップS1〜S4で粗キャリブレーションを行って地磁気センサ３の仮キャリブレーション値を求め、その後、ステップS5〜S12で仮キャリブレーション値を使った精密キャリブレーションを行う。
【００２４】
すなわち、ステップS1で、芝刈作業車１の走行制御アクチュエータ８の１つを構成するスロットルアクチュエータを介してエンジン出力を制御するとともに操舵系を制御し、一定方向への旋回走行を開始させると、ステップS2で、地磁気センサ３のＸコイル出力及びＹコイル出力を記録しながら、Ｘコイル出力の最大値ＸMAX、最小値ＸMIN、及び、Ｙコイル出力の最大値ＹMAX、最小値ＹMINをそれぞれ更新し、ステップS3で、予め設定した設定時間が経過したか否かを調べる。
【００２５】
上記設定時間は、予め車両が１周旋回するに要する時間を設定しておくものであり、上記ステップS3で設定時間が経過していないときには上記ステップS2へ戻ってＸ，Ｙコイル出力のサンプリングを続行しながら最大値及び最小値を更新し、設定時間が経過したとき、上記ステップS3からステップS4へ進む。
【００２６】
ステップS4では、上記設定時間における左右の車輪エンコーダ４ａ，４ｂの出力パルス差から実際の車両の旋回角を算出し、車両を１周旋回させたときのＸコイル出力をＸ軸、Ｙコイル出力をＹ軸とするＸ，Ｙ座標平面上の地磁気センサ３の円状の出力軌跡の中心値（ＸO，ＹO）を求めるとともに、Ｘ軸方向の直径（ゲイン）ＸD及びＹ軸方向の直径（ゲイン）ＹDを求め、以下の(1)〜(4)式により、仮のキャリブレーション値として求める。
【００２７】
ＸO＝（ＸMAX＋ＸMIN）／２ …(1)
ＹO＝（ＹMAX＋ＹMIN）／２ …(2)
ＸD＝ ＸMAX−ＸMIN …(3)
ＹD＝ ＹMAX−ＹMIN …(4)
尚、上記仮のキャリブレーション値、及び、後述する精密キャリブレーション値は、通常、平坦地でキャリブレーションを実行し、地磁気センサ３のＸ，Ｙコイルの出力ゲインが同じになるよう調整されているとき、地磁気センサ３のＸ，Ｙ座標平面上の軌跡を円とみなすことができる値となる。
【００２８】
その後、精密キャリブレーション実行のためステップS5へ進み、引き続き車両を制御して一定速度での旋回走行を開始する。そして、ステップS6で地磁気センサ３のＸコイル出力Ｖx及びＹコイル出力Ｖyから仮のキャリブレーション値を使って方位演算し、走行開始方位を記録する。すなわち、地磁気センサ３の出力軌跡円から方位角θは以下の(5)式で算出することができる。
【００２９】
θ＝ａｒｃｔａｎ{(Ｖx−ＸO)／(Ｖy−ＹO)} …(5)
次に、ステップS7へ進み、地磁気センサ３のＸコイル出力Ｖx及びＹコイル出力Ｖyを読み込んで記憶すると、ステップS8で再び仮のキャリブレーション値を使って方位演算し、ステップS9で現在方位が走行開始方位に達したか否かを調べる。その結果、現在方位が走行開始方位に達しておらず、車両の旋回が１周に達していないときには、上記ステップS9からステップS7へ戻って地磁気センサ３のＸ，Ｙコイル出力のサンプリングを続け、仮のキャリブレーション値を使ってステップS8で演算した方位が走行開始方位に達したとき、すなわち、車両が１周旋回したとき、上記ステップS9からステップS10へ進んで車両を停止させる。
【００３０】
その後、ステップS11へ進み、１周の旋回でサンプリングしたＸ，Ｙコイルの出力データを用い、最小二乗法によって各Ｘ，Ｙ出力を三角関数で近似する。すなわち、精密キャリブレーションでは、車速を一定にして１周車両を旋回させているため、Ｘ，Ｙコイル出力は、図４に示すような三角関数の曲線となり、周期Ｔが既知の三角関数における振幅及びオフセット値を最小二乗法によって求めることでＸ，Ｙコイル出力を近似することができる。従って、１周分の測定データを全て使ってＸコイル出力Ｖx及びＹコイル出力Ｖyを最小二乗法で求めると、以下の(6),(7)式となる。
【００３１】
Ｖx＝Ａ1・ｓｉｎ(Ｔ・ｔ＋Φ1)＋Ｂ1 …(6)
Ｖy＝Ａ2・ｓｉｎ(Ｔ・ｔ＋Φ2)＋Ｂ2 …(7)
但し、 Ａ1,Ａ2 … 振幅
Ｂ1,Ｂ2 … オフセット値
Φ1,Φ2 … 初位相
そして、上記ステップS11からステップS12へ進み、Ｘ，Ｙコイル出力の三角関数から、Ｘ，Ｙ座標平面上の出力軌跡の中心値（Ｂ1，Ｂ2）、出力軌跡のＸ軸方向の直径Ａ1及びＹ軸方向の直径Ａ2を、作業データ蓄積部２４に地磁気センサ３の精密キャリブレーション値としてストアし、ルーチンを終了する。
【００３２】
この場合、Ｘ，Ｙコイル出力の位相差（｜Φ1−Φ2｜）は９０度であるため、地磁気センサ３のＸ，Ｙコイル出力のゲインが同じとき（Ａ1＝Ａ2）、Ｘ，Ｙ座標平面上の出力軌跡は中心位置（Ｂ1，Ｂ2）の円となり、方位角θを以下の（8)式で求めることができる。
【００３３】
θ＝ａｒｃｔａｎ{(Ｖx−Ｂ1)／(Ｖy−Ｂ2)} …(8)
すなわち、粗キャリブレーションと精密キャリブレーションとを２段階行うことにより、地磁気センサ３の出力軌跡の中心位置及びゲインを正確に求めることができ、しかも、精密キャリブレーションにおいて、１周旋回したときに得られる地磁気センサ３の出力データを全て使って誤差を最小にするよう三角関数で近似しているため、精度良く、地磁気センサ３の出力軌跡の中心値を求めることができる。これにより、限られたデータ数から出力軌跡の中心値を求める従来のキャリブレーションに対し、より精密なキャリブレーションで振動や外部磁界の影響を最小限として地磁気センサ３による方位検出精度を高めることができる。
【００３４】
尚、地磁気センサ３のＸ，Ｙコイル出力のゲインが同じでないとき、あるいは傾斜地においては、Ａ1≠Ａ2となり、地磁気センサ３のＸ，Ｙ座標平面上の軌跡は楕円となるが、このような場合でも、上記精密キャリブレーション値を用いてゲイン比による補正や傾斜補正を行うことにより正確な方位角を求めることができる。
【００３５】
【発明の効果】
以上説明したように本発明によれば、まず、粗キャリブレーションにより、地磁気センサを搭載した車両を一定方向に設定時間旋回させて地磁気センサの出力軌跡の中心位置を仮のキャリブレーション値として求め、次に、この仮キャリブレーション値を用いて車両を一定速度で正確に１周旋回させ、１周の旋回で得られた地磁気センサの出力データを用いて地磁気センサの出力軌跡の中心位置を精密キャリブレーション値として求めるため、限られたデータ数から出力軌跡の中心値を求める従来のキャリブレーションに対し、より精密なキャリブレーションで振動や外部磁界の影響を最小限として地磁気センサによる方位検出精度を高めることができる。
【図面の簡単な説明】
【図１】本発明の基本構成図
【図２】地磁気センサのキャリブレーション処理ルーチンのフローチャート（その１）
【図３】地磁気センサのキャリブレーション処理ルーチンのフローチャート（その２）
【図４】地磁気センサのＸコイル及びＹコイルの出力を示す説明図
【図５】自律走行制御系のブロック図
【図６】地磁気センサを搭載した自律走行車の外観図
【図７】地磁気センサのキャリブレーションの説明図
【符号の説明】
１ …芝刈作業車（車両）
３ …地磁気センサ
２０…制御装置（粗キャリブレーション手段、精密キャリブレーション手段）
代理人 弁理士 伊 藤 進

Claims (2)

1. 地磁気センサを搭載した車両を一定方向に設定時間旋回させ、上記地磁気センサの出力軌跡の中心位置を仮のキャリブレーション値として求める粗キャリブレーション手段と、
   上記粗キャリブレーション手段で求めた仮のキャリブレーション値を用いて方位演算することにより車両を一定速度で１周旋回させ、１周の旋回で得られた上記地磁気センサの出力データを用いて上記地磁気センサの出力を三角関数で近似することにより上記地磁気センサの出力軌跡の中心位置を求め、上記地磁気センサの方位検出誤差を補正するための精密キャリブレーション値とする精密キャリブレーション手段とを備えたことを特徴とする地磁気センサの補正装置。
2. 上記精密キャリブレーション手段は、上記地磁気センサの出力を最小二乗法により三角関数で近似することを特徴とする請求項１記載の地磁気センサの補正装置。

Images