JPH0914962A - 建設車両の位置計測装置 - Google Patents

Abstract

(57)【要約】 【目的】建設車両の位置計測を正確に行う。 【構成】推定演算を行う時点Ｔ1において、第２の位置
検出手段（ＧＰＳ）から位置検出値が未だ出力されてな
い場合には、第２の位置検出手段から位置検出値が出力
されるまでの間（Ｔ1〜Ｔ´1）、この第２の位置検出手
段の位置検出値の代わりに、当該時点Ｔ1において第１
の位置検出手段から出力されている位置検出値が使用さ
れる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、建設車両の位置を計測
する装置に関する。

【０００２】

【従来の技術および発明が解決しようとする課題】建設
車両、とりわけ無人ダンプトラックを無人走行させるに
あたっては、その位置を正確に知る必要がある。

【０００３】ここに、従来より、無人車両を所定の走行
経路に沿って目的地へ導く方法として、方向検出器と走
行長測定器にて車両の現在位置を推定し、予め数示して
おいて予定経路上の通過予定地点を通過すべく車両を自
動的に操舵する推測航法と呼ばれる方法がある。

【０００４】この推測航法の欠点は、路面の凹凸や車両
のスリップにより、車両の推定位置に誤差を生じ、正確
に通過予定地点を通過できないことである。

【０００５】この累積誤差を補正するために、走行経路
の一定区間ごとに、位置補正用の標識（例えばポール）
を設置することが考えられる。しかし、位置補正用の標
識の設置は、高コストが招来するとともに、建設車両が
走行する過酷な現場に標識を配設することは、困難な面
もある。

【０００６】また、推測航法では、初期位置との関係で
相対座標系上の位置を推定するため、初期位置データを
最初に与えなければならないという煩わしさがある。

【０００７】さらに、ティーチング・プレイバック方式
を採用した場合、走行経路のデータの大幅な変更には、
再ティーチングが必要であったり、特定の位置（例えば
排土場）以外の場所で、別の走行経路に乗り移ることが
できないといった制約もある。

【０００８】ここで、建設車両のごとく屋外を走行する
車両の位置を検出するに適した別の位置検出システムと
して、ＧＰＳ（グローバル・ポジショニング・システ
ム）がある。

【０００９】このＧＰＳでは、絶対座標系上における車
両の位置を逐次検出することができるため、推測航法を
適用した場合の上述した不都合を回避することができ
る。

【００１０】しかし、ＧＰＳは、推測航法に較べて応答
性（計算時間）がよくなかったり、定常誤差が存在する
という欠点もある。

【００１１】そこで、これら両者の利点を併せ持ち、か
つ両者の欠点を有しない位置検出センサの開発が要請さ
れる。

【００１２】しかし、乗用車に較べてその生産量がはる
かに少ない建設車両のためだけに新たなセンサを開発す
ることも、コスト上難しいことが多い。

【００１３】そこで、一つのセンサでは、その要求性能
は満足できなくても、その性能を相補うことができるよ
う、複数のセンサを組み合わせることにより、その要求
性能を達成する手段として、「センサフュージョン」と
呼ばれる手法がある。

【００１４】このセンサフュージョン」を実現するため
の最も一般的な理論としては、カルマンフィルタ（Ｋａ
ｌｍａｎ Ｆｉｌｔｅｒ）が挙げられる。

【００１５】カルマンフィルタの目的は、２つの情報
（システムの状態の変化を表す状態方程式とそのシステ
ムの状態と計測値との関係を表す観測方程式）から状態
変数（状態量ともいい、システムの状態を表す変数、量
のこと）として最も確からしい値を求めることにある。

【００１６】ここで、図２のブロック図を参照してカル
マンフィルタを簡単に説明する。

【００１７】カルマンフィルタの手法とは、要約すると
「予測」と「推定」を交互に繰り返すものと説明でき
る。

【００１８】すなわち、「予測」とは、予測部２で行わ
れる演算であり、状態方程式、 Ｘk+1＝ＦkＸk＋ＧkＷk …（１） を使用して、将来（現在の時刻ｔkに対する次の時刻ｔk
+1）の状態変数Ｘk+1の期待値やその期待値の不確かさ
ΣXkを求めることである。ここで、Ｆkは推測航法のセ
ンサ出力に基づく行列である。

【００１９】一方、「推定」とは、推定部１で行われる
演算であり、現在の時刻ｔkにおいて状態変数Ｘkに関す
る観測量Ｙkの実測値と観測方程式、 Ｙk＝ＨkＸk＋Ｖk …（２） を利用し、状態変数Ｘkに関するより確かな期待値Ｘk
(+)やその期待値の不確かさΣXk(+)を求めることをい
う。ここでＹkはＧＰＳのセンサ出力に基づく行列であ
る。このように、「推定」の場合は、その時刻ｔkにな
ってから新たに得られた観測量Ｙkの実測値を利用する
ことにより、より確かな状態変数Ｘkを求めることか
ら、「事後推定」ともいう。

【００２０】このようにして、推測航法の累積誤差等の
欠点と、ＧＰＳの応答遅れ等の欠点とが相補なわれる。

【００２１】ところで、従来のカルマンフィルタによる
センサフュージョンでは、予測部２に入力される第１の
センサからのデータの読み込みと、推定部１に入力され
る第２のセンサからのデータの読み込みとが同じタイミ
ングで行われるという前提となっている。

【００２２】しかし、第１のセンサとして推測航法のセ
ンサを使用し、第２のセンサとしてＧＰＳのセンサを使
用したとき、第１のセンサに較べて、第２のセンサの出
力間隔が著しく長くなる。

【００２３】そこで、従来は、第１のセンサの出力間隔
を、長い出力間隔の第２のセンサの出力間隔に合わせる
ようにしていた。

【００２４】しかし、本来短い出力間隔の第１のセンサ
の出力間隔を、強制的に、長い方の第２のセンサの出力
間隔に合わせた場合には、第１のセンサ出力に基づく推
測航法の精度が落ちてしまうことは明かである。

【００２５】本発明はこうした実状に鑑みてなされたも
のであり、出力間隔が大きく異なるセンサ同士であった
としても、本来、出力間隔の短い第１のセンサの出力間
隔をそのままに保持して精度を維持したままで、カルマ
ンフィルタによるセンサフュージョンを適用できるよう
にすることを第１の目的とするものである。

【００２６】さてまた、推測航法に基づいて、カルマン
フィルタにおける状態方程式を表現しようとするとき、
状態変数Ｘkは、ｘ（ｋ）、ｙ（ｋ）を位置、θ（ｋ）
を方位として、 Ｘk＝（ｘ（ｋ）、ｙ（ｋ）、θ（ｋ）） …（３） と表される。このように表現すると、状態方程式は非線
形な式、 Ｘk+1＝Ｆ´（Ｘk）＋Ｇ´（Ｘk）Ｗk …（４） で表現せざるを得ないことになる。

【００２７】この場合、カルマンフィルタもその非線形
式に対応した拡張カルマンフィルタを用いなければなら
ず、この結果、計算が複雑になるとともに、近似式を用
いるため誤差も大きくなることになる。

【００２８】そこで、本発明は、推測航法によって状態
方程式を表現した場合であっても、状態方程式を線形化
でき、もって演算の簡易化、演算精度の向上を図ること
を第２の目的とするものである。

【００２９】さて、また推測航法の初期位置は、前述し
たように未知数であり、センサフュージョンを行った結
果はじめて、演算した位置が実際の位置に徐々に近づい
ていく。

【００３０】この場合、演算位置がいつ実際の位置に収
束したかを情報として知りたいとの要求がある。

【００３１】そこで、本発明は、推測航法のセンサを一
方のセンサとして使用した場合において、カルマンフィ
ルタによる演算位置が実際の位置と収束したかを判断す
ることができるようにすることを第３の目的とするもの
である。

【００３２】さて、また、上述したように、推測航法の
センサを一方の第１のセンサとして使用した場合、初期
位置は未知数であることから、初期段階では誤差が大き
いことは明かである。したがって、かかる初期段階にお
ける誤差を考慮しつつカルマンフィルタの演算位置を実
際の位置に早期に収束せしめることが望まれる。

【００３３】そこで、本発明は、推測航法のセンサを一
方のセンサとして使用したとしても、カルマンフィルタ
による演算位置が実際の位置に収束するまでの収束期間
を短くできるようにすることを第４の目的とするもので
ある。

【００３４】

【課題を解決するための手段】そこで、上記第１の目的
を達成するために、本発明の第１発明では、建設車両の
位置を検出する第１の位置検出手段および第２の位置検
出手段と、前記第１の位置検出手段の出力と前記第２の
位置検出手段の出力とをカルマンフィルタを介して建設
車両の位置を演算出力する演算手段とを具えた建設車両
の位置計測装置であって、前記演算手段において前記第
１の位置検出手段の出力と状態方程式と推定位置とに基
づいて位置の予測演算を行い、予測位置を求めるととも
に、前記第２の位置検出手段の出力と観測方程式と前記
予測位置とに基づいて位置の推定演算を行い、推定位置
を求めるようにした建設車両の位置計測において、前記
推定演算を行う時点において、第２の位置検出手段から
位置検出値が未だ出力されてない場合には、前記第２の
位置検出手段から位置検出値が出力されるまでの間、こ
の第２の位置検出手段の位置検出値の代わりに、当該時
点において前記予測位置を使用するようにしている。

【００３５】また、上記第２の目的を達成するために、
本発明の第２発明では、建設車両の位置を推測航法によ
って検出する第１の位置検出手段および第２の位置検出
手段と、前記第１の位置検出手段の出力と前記第２の位
置検出手段の出力とをカルマンフィルタを介して建設車
両の位置を演算出力する演算手段とを具えた建設車両の
位置計測装置であって、前記演算手段において前記第１
の位置検出手段の出力と状態変数が推測航法に基づく位
置および方向θである状態方程式と推定位置とに基づい
て位置の予測演算を行い、予測位置を求めるとともに、
前記第２の位置検出手段の出力と観測方程式と前記予測
位置とに基づいて位置の推定演算を行い、推定位置を求
めるようにした建設車両の位置計測において、前記状態
方程式の方向θの状態変数の代わりに、ｓｉｎθ、ｃｏ
ｓθを使用することにより、当該状態方程式を線形化す
るようにしている。

【００３６】また、上記第３の目的を達成するために、
本発明の第３発明では、上記第２発明において、（sin
θ）2＋（cosθ）2と１との差が所定のしきい値以下に
なった時点で、前記状態変数が真の値に収束したものと
判断するようにしている。

【００３７】また、上記第４の目的を達成するために、
本発明の第４発明では、建設車両の位置を検出する第１
の位置検出手段および第２の位置検出手段と、前記第１
の位置検出手段の出力と前記第２の位置検出手段の出力
とをカルマンフィルタを介して建設車両の位置を演算出
力する演算手段とを具えた建設車両の位置計測装置であ
って、前記演算手段において前記第１の位置検出手段の
出力と状態方程式と推定位置およびこの推定位置の不確
からしさとに基づいて位置の予測演算を行い、予測位置
およびこの予測位置の不確からしさを求めるとともに、
前記第２の位置検出手段の出力と観測方程式と前記予測
位置およびこの予測位置の不確からしさとに基づいて位
置の推定演算を行い、推定位置およびこの推定位置の不
確からしさを求めるようにした建設車両の位置計測にお
いて、前記状態方程式の状態変数が真の値に収束したか
否かを判断する判断手段を具え、前記判断手段によっ
て、前記状態変数が真の値に未だ収束していないと判断
されている場合には、前記予測演算の結果得られる予測
位置の不確からしさが大きくなるよう補正し、この補正
した不確からしさを使用して前記推定演算を行うように
している。

【００３８】

【作用】上記第１発明によれば、図３に示すように、推
定演算を行う時点Ｔ1において、第２の位置検出手段
（ＧＰＳ）から位置検出値が未だ出力されてない場合に
は、第２の位置検出手段から位置検出値が出力されるま
での間（Ｔ1〜Ｔ´1）、この第２の位置検出手段の位置
検出値の代わりに、当該時点Ｔ1において第１の位置検
出手段を用いた予測位置が使用される。

【００３９】この結果、出力間隔が大きく異なる位置検
出手段同士であったとしても、本来、出力間隔の短い第
１の位置検出手段の出力間隔をそのままに保持して精度
を維持したままで、カルマンフィルタによるセンサフュ
ージョンを適用できるようになる。

【００４０】また、上記第２発明によれば、状態方程式
の方向θの状態変数の代わりに、sinθ、cosθが使用さ
れ、当該状態方程式が線形化される。

【００４１】この結果、非線形式に対応した拡張カルマ
ンフィルタを用いなくてもよいので、演算が複雑となら
ず、また近似式を用いなくてもよいので、誤差が小さく
なる。このように、推測航法によって状態方程式を表現
した場合であっても、状態方程式を線形化でき、もって
演算の簡易化、演算精度の向上を図ることができる。ま
た、上記第３発明によれば、（sinθ）2＋（cosθ）2と
１との差が所定のしきい値以下になった時点で、状態変
数が真の値に収束したものと判断される。

【００４２】この結果、推測航法のセンサを一方の第１
の位置検出手段として使用した場合において、最初は演
算上の位置が実際の位置と大きくかけななれていて未だ
実際の位置に収束していなくても、いつ演算位置が実際
の位置に収束したかの情報を取得することができるよう
になる。

【００４３】また、上記第４発明によれば、状態変数が
真の値と大きくかけはなれていて未だ真の値に収束して
いないと判断されている場合には、予測演算の結果得ら
れる予測位置の不確からしさが大きくなるよう補正さ
れ、この補正された不確からしさを使用して推定演算が
行なわれる。

【００４４】すなわち、従来にあっては、第１の位置検
出手段（推測航法のセンサ）の出力の不確からしさを示
す共分散行列は、補正をかけずに固定のままとなってい
る。

【００４５】しかし、推測航法のセンサを一方の第１の
位置検出手段として使用した場合、初期位置は未知数で
あることから、初期段階では不確からしさが大きいこと
は明かである。したがって、カルマンフィルタの演算位
置を実際の位置に早期に収束せしめるためには、初期段
階において第１の検出手段の出力の不確からしさを示す
共分散行列の値に補正をかけて、収束を早めることが必
要となる。

【００４６】そこで、そのような補正をかけることで、
推測航法のセンサを一方の第１の位置検出手段として使
用した場合であっても、カルマンフィルタによる演算位
置が実際の位置に収束するまでの収束期間を短くするこ
とができるようになる。

【００４７】

【実施例】以下、図面を参照して本発明に係る建設車両
の位置計測装置の実施例について説明する。図１は、実
施例装置の構成を示すブロック図である。

【００４８】実施例では、第１の位置検出手段としてカ
ルマンフィルタの予測部に相当する推測航法のセンサが
使用され、第２の位置検出手段としてＧＰＳのセンサが
使用される場合を想定している。

【００４９】車両方位計５（例えばレートジャイロを中
心として構成されている）および車輪回転センサ６（例
えばエンコーダを中心として構成されている）は、推測
航法のセンサを構成している。

【００５０】また、車両絶対位置センサ７は、ＧＰＳの
センサを構成している。

【００５１】車両方位計５では、建設車両（以下、単に
車両という）の逐次の車両の方位角θの角速度ω(t)が
出力され、これがカルマンフィルタ予測（推測航法）演
算部４に入力される。また、車輪回転センサ６では、車
両の車輪の速度ν(t)が出力され、これがカルマンフィ
ルタ予測演算部４に入力される。

【００５２】カルマンフィルタ予測演算部４では、以下
に示す演算が実行される。

【００５３】すなわち、（５）、（６）式に示すよう
に、時刻ｋにおける座標での方向θ（ｔ）に対して、時
刻ｋから時刻ｋ＋１までの車両位計５の出力ω(t)を積
分したものΔθ（ｋ＋１）を加え、時刻ｋ＋１における
方向θ（ｋ＋１）を求める。

【００５４】 一方、車両の移動距離Δｄ（ｋ）は、 となる。よって、座標位置［ｘ（ｋ＋１） ｙ（ｋ＋
１）］は、 となる。

【００５５】こうして車両の各時刻ｋにおける座標位
置、方向Ｘk（ｘ（ｋ）、ｙ（ｋ）、θ（ｋ））を、推
測航法にて検出することができる。また、カルマンフィ
ルタ予測演算部４では、上記（５）、（７）式に示す方
向変化量Δθ、距離変化量Δｄが逐次求められる。

【００５６】一方、車両絶対位置センサ７では、車両の
各時刻ｋにおける絶対座標位置Ｙk（ｘ＾（ｋ）、ｙ＾
（ｋ））が検出されており、カルマンフィルタ推定演算
部３に入力される。

【００５７】以上のように、カルマンフィルタ演算部
３、４は、図２に示す推定部１、予測部２と同様にし
て、予測部２で行われる予測演算と推定部１で行われる
推定演算とを交互に繰り返すことで、各時刻ｋ毎に、車
両の、より確かな位置、方向Ｘk(+)（ｘ（ｋ）、ｙ
（ｋ）、θ（ｋ））を演算、出力する。

【００５８】すなわち、予測部２に相当するカルマンフ
ィルタ予測演算部４では、対象としているシステムの状
態の変化を表す方程式である状態方程式、 Ｘk+1＝ＦkＸk＋ＧkＷk …（１） による演算が実行されるとともに、推定部１に相当する
カルマンフィルタ推定演算部３から入力されるΣxkに基
づき下記（１０）式に示す演算が実行される。

【００５９】 Σxk+1＝ＦkΣxk（Ｆk）T＋ＧkΣw（Ｇk）T …（１
０） ただし、上記（１）式において、Ｘkは、時刻ｔkにおけ
る車両位置、方向（状態変数）であり、カルマンフィル
タ推定演算部３より入力されるものである。また、Ｗk
は、時刻ｔkにおける車両位置、方向の誤差（システム
雑音：モデルの誤差とセンサ入力の誤差とを加算したも
の）である。また、Ｆkは、状態遷移行列で、Ｇkは、駆
動行列である。

【００６０】ここで、状態遷移行列Ｆkは、上記
（５）、（７）式により求められた方向変化量Δθ、距
離変化量Δｄに基づき、後述のごとく生成される。

【００６１】一方、上記（１）式において、Ｘk+1は、
将来（現在の時刻ｔkに対する次の時刻ｔk+1）の車両位
置、方向の期待値である。また、（１０）式において、
Σxkは、状態変数やセンサ入力の不確かさを示す共分散
行列であり、カルマンフィルタ推定演算部３より入力さ
れるものである。なお、（１０）式において、（）Tと
あるのは、転置行列を意味するものと定義する（以下、
同じ）。

【００６２】このようにして、カルマンフィルタ予測演
算部４から、将来（現在の時刻ｔkに対する次の時刻ｔk
+1）の状態変数（車両位置、方向）の期待値Ｘk+1およ
びその期待値の不確かさΣXk+1が演算、出力される。

【００６３】つぎに、時刻がｋからｋ＋１が更新される
とともに、予測演算部４から出力される状態量Ｘk+1、
不確かさΣxk+1がＸk(-)、Σxk(-)にされて推定部１に
相当するカルマンフィルタ推定演算部３に入力される。

【００６４】そして、推定演算部３では、更新された時
刻ｋにおいて、これら状態変数Ｘk(-)、誤差Σxk(-)
と、状態変数Ｘkに関する観測量である車両絶対位置セ
ンサ７の出力Ｙkとを用いた推定演算がされる。

【００６５】すなわち、観測方程式、 Ｙk＝ＨkＸk(-)＋Ｖk …（２） と下記演算式、 Ｋk＝Σxk(-)（Ｈk）T（ＨkΣxk(-)（Ｈk）T＋Σv）-1 …（１１） ｘk(+)＝ｘk(-)＋Ｋk（ＹkーＨkＸk(-)） …（１２） Σxk(+)＝Σxk(-)ーＫkＨkΣxk(-) …（１３） による演算が実行される。

【００６６】ただし、上記（２）式において、Ｙkは、
時刻ｔkにおける車両位置（観測量：センサ７の出力）
である。また、Ｖkは、時刻ｔkにおける車両位置の誤差
（観測雑音）である。また、Ｈkは、観測行列（例えば
単位行列）である。

【００６７】また、上記（１１）式において、Ｋkはカ
ルマンゲインである。なお、（１１）式において、（）
-1とあるのは、逆行列を意味するものと定義する。

【００６８】このようにして推定演算部３で、状態変数
Ｘkに関するより確かな期待値Ｘk(+)およびその期待値
の不確かさΣXk(+)が演算され、逐次の車両位置Ｘk(+)
は、カルマンフィルタ推定演算部３よりコントローラ１
０の外部に出力されるとともに（図１参照）、車両位置
Ｘk(+)およびその不確かさΣXk(+)は予測演算部４に入
力されることになる。

【００６９】なお、推定演算部３で行われる推定演算
は、その時刻ｔkになってから新たに得られた観測量Ｙk
の実測値を利用することにより、より確かな状態量Ｘk
を求めることから、「事後推定」演算と呼ばれる。

【００７０】以上のように、カルマンフィルタ推定演算
部３では、カルマンフィルタ予測演算部４に相当する推
測航法の出力が、ＧＰＳのセンサ７の出力によって事後
推定されるため、推測航法の累積誤差等の欠点と、ＧＰ
Ｓの応答遅れ等の欠点とが相補なわれ、正確な車両位
置、方向を取得することができる。

【００７１】・第１の実施例 さて、図３に示すように、推測航法のセンサを用いたカ
ルマンフィルタ予測演算部４の出力間隔に対して、ＧＰ
Ｓのセンサ７の出力間隔は著しく長い。したがって、推
定部１において推定演算を行う時点Ｔ1において、ＧＰ
Ｓのセンサ７から位置検出値が未だ出力されてない場合
には、当該時点Ｔ1におけるセンサ７のデータを使用し
た演算を行うことができない。そこで、この第１の実施
例では、このような問題を図５に示すフローチャートに
示す処理にて解決している。

【００７２】すなわち、同図５に示すように、まず、更
新後の時刻ｋにおいてＧＰＳのセンサ７からデータＹk
が入力されているか否かが判断される（ステップ１０
１）。

【００７３】ここで、図３に示す時刻Ｔ0（Ｔ0´）のご
とく、同じ時刻においてセンサ４、センサ７から共にデ
ータが出力されている場合には（ステップ１０１の判断
ＹＥＳ）、推定部１において、同時刻Ｔ0´で出力され
たセンサ７の位置データＹk、および不確かさデータΣv
を使用して、上記（２）式、（１１）〜（１３）式に示
す推定演算が実行される（ステップ１０２）。その後、
推定部１から出力された、より確かな位置、方向データ
Ｘk(+)およびその位置、方向データの不確かさデータΣ
Xk(+)に基づいて、上記（１）式、（１０）式に示す予
測演算が実行され（ステップ１０４）、時刻がｋからｋ
＋１に更新された後、再び上記ステップ１０１に移行さ
れる。

【００７４】一方、更新後の時刻ｋにおいてＧＰＳのセ
ンサ７からデータＹkが入力されていない場合、例えば
図３に示す時刻Ｔ1のように、該時刻Ｔ1においてセンサ
４からデータが出力されてはいるが、センサ７からデー
タＹkが出力されていない場合には（ステップ１０１の
判断ＮＯ）、（２）式、（１１）〜（１３）式に示す推
定演算の代わりに以下に示す代用式による演算が実行さ
れる。

【００７５】すなわち、 Ｙk＝ＨkＸk(-) …（１４） ｘk(+)*＝ｘk(-) …（１５） という演算がなされるとともに、Σxk(+)*の各要素σk
について、 という演算がなされて、誤差Σxk(+)*が求められる。

【００７６】上記（１６）式において、ｍは定数であ
り、σGは、ＧＰＳの衛星配置のパラメータであり、こ
れが小さいとＧＰＳの精度がよくなるというパラメータ
である。また、（１６）式の右辺の第２項は、前回ＧＰ
Ｓのセンサ７からデータが得られた時点ｊ（例えば図３
のＴ0´）から現時点ｋ（例えば図３のＴ1）までに車両
が進行した距離の２乗である。なお、（）2とあるの
は、２乗を意味するものと定義する。

【００７７】よって、（１６）式によって求められる不
確かさΣxk(+)*は、センサ７からデータが出力されるま
での期間が長いほど、大きくなる関係にあることがわか
る。このように、（１４）式に示すように、ＧＰＳのセ
ンサ７から位置検出値Ｙkが出力されるまでの間（Ｔ1〜
Ｔ´1）、このセンサ７の位置検出値Ｙkの代わりに、各
時点Ｔ1、Ｔ2において推測航法のセンサ４から出力され
ているデータのみに基づく状態変数Ｘk(-)が代用され
る。

【００７８】ただし、上記（１６）式に示すように、Ｇ
ＰＳのセンサ７のデータＹkの代わりに推測航法のセン
サ４のデータのみに基づく状態変数Ｘk(-)で代用をして
いる時間が長くなるほど、位置の誤差が大きくなるの
で、その誤差の見積りを表す共分散行列Σxk(+)*を、代
用している時間に応じて大きくすることで対処している
（ステップ１０３）。

【００７９】こうして推定部演算３から出力された、よ
り確かな位置、方向データＸk(+)*およびその位置、方
向データの不確かさデータΣXk(+)*に基づいて、上記
（１）式、（１０）式に示す予測演算が実行され（ステ
ップ１０４）、時刻がｋからｋ＋１に更新された後、再
び上記ステップ１０１に移行されることになる。

【００８０】このように、この第１の実施例によれば、
出力間隔が大きく異なる位置センサ４、７同士を組み合
わせて使用したとしても、本来、出力間隔の短かいセン
サ４の出力間隔をそのままに保持して精度を維持したま
まで、カルマンフィルタによるセンサフュージョンを適
用できるようになる。

【００８１】・第２の実施例さて、前述したように、推
測航法に基づいて、カルマンフィルタにおける状態方程
式を生成しようとするとき、状態変数Ｘkを、 Ｘk＝（ｘ（ｋ）、ｙ（ｋ）、θ（ｋ）） …（３） と表すのが、一般的である。しかし、このように表現す
ると、本実施例における状態方程式は非線形な式、 Ｘk+1＝Ｆ´（Ｘk）＋Ｇ´（Ｘk）Ｗk …（４） で表現せざるを得ないことになる。

【００８２】この場合、Ｆ´（Ｘk）は、前述の（６）
式（ θ（ｋ＋１）＝θ（ｋ）＋Δθ（ｋ＋１） ）、
（８）式（ ｘ（Ｋ＋１）＝ｘ（ｋ）＋Δｄ（ｋ＋１）
cos｛θ（ｋ）＋Δθ（ｋ＋１）｝ ）、（９）式（
ｙ（ｋ＋１）＝ｙ（ｋ）＋Δｄ（ｋ＋１）sin｛θ
（ｋ）＋Δθ（ｋ＋１）｝ ）そのものである。

【００８３】しかし、前述したように、演算の簡易化、
演算精度の向上のためには、状態方程式を線形化するこ
とが望まれる。

【００８４】そこで、この第２実施例では、方向θの代
わりに、ｓｉｎθ、ｃｏｓθを使用し１自由度冗長にす
る工夫を施すことにより、状態方程式を線形化してい
る。

【００８５】すなわち、上記（６）式と三角関数の加法
定理より、 cos｛θ（ｋ＋１）｝＝cos｛θ（ｋ）＋Δθ（ｋ＋１）｝＝cosθ（ｋ）cosΔ θ（ｋ＋１）ーsinθ（ｋ）sinΔθ（ｋ＋１） …（１７） sin｛θ（ｋ＋１）｝＝sin｛θ（ｋ）＋Δθ（ｋ＋１）｝＝cosθ（ｋ）sinΔ θ（ｋ＋１）＋sinθ（ｋ）cosΔθ（ｋ＋１） …（１８） が得られる。

【００８６】よって、上記（８）、（９）、（１７）、
（１８）式を行列で表現すると、 …（１）と、状態方程式（１）式が取得される。ただ
し、η（ｋ）＝cosθ（ｋ）であり、ξ（ｋ）＝sinθ
（ｋ）である。

【００８７】このように、状態方程式を、Ｘk+1＝ＦkＸ
k(+)＋ＧkＷk といった形式で線形化することができ
る。

【００８８】この結果、非線形式に対応した拡張カルマ
ンフィルタを用いなくてもよいので、演算が複雑となら
ず、また拡張カルマンフィルタにおける近似式を用いな
くてもよいので、誤差が小さくなる。このように、この
第２の実施例によれば、推測航法によって状態方程式を
表現しようとするときでも、状態方程式を線形化でき、
もって演算の簡易化、演算精度の向上を図ることができ
る。

【００８９】・第３の実施例 さて、前述したように、カルマンフィルタ予測演算部４
に相当する推測航法の初期位置は、未知数であり、上述
した第１の実施例のごとくＧＰＳとの間でセンサフュー
ジョンを行った結果はじめて、演算した位置が実際の位
置に徐々に近づいていく。この場合、演算位置がいつ実
際の位置に収束したかを情報として知りたいとの要求が
ある。そこで、この第３の実施例では、推測航法のセン
サを一方のセンサとして使用した場合、いつカルマンフ
ィルタによる演算位置が実際の位置に収束したかを判断
することができるようにしている。

【００９０】そこで、上述した第２の実施例において、
状態方程式（（１）式）の状態変数（位置、方向）が実
際の位置、方向に収束しているならば、その状態変数を
構成するsinθ、cosθの両者間に一定の関係が成立して
いるはずである、という点に着目した。

【００９１】つまり、これらsinθ、cosθの間には、本
来 （sinθ）2＋（sinθ）2＝１ …（１９） という関係が成立するので、上記（１９）式が成立して
いるか否かによって、状態変数が真の値に収束している
か否かを、つまりカルマンフィルタによる演算位置が実
際の位置に収束したか否かを判断することができる。

【００９２】具体的には、しきい値１ーα、１＋α（α
は例えば０．２）を設定して、 １ーα＜（sinθ）2＋（sinθ）2＜１＋α …（２０） が成立した時点で、カルマンフィルタによる演算位置が
実際の位置に収束したものと判断し、上記（２０）式が
成立していない場合には、カルマンフィルタによる演算
位置が実際の位置に未だ収束していないと判断する。

【００９３】このように、推測航法のセンサを一方のセ
ンサとして使用した場合であって、最初は演算上の位置
が実際の位置に収束していなくても、いつ演算位置が実
際の位置に収束したかの情報を取得することができるよ
うになる。

【００９４】・第４の実施例 さて、一般的なカルマンフィルタを用いた演算では、一
方の位置センサである推測航法のセンサを用いた予測演
算部４の出力の不確かさを示す共分散行列Σxk(-)は、
補正をかけずに、（１０）式および（１３）式に基づき
求められる。

【００９５】しかし、推測航法では、初期位置は未知数
であることから、初期段階では誤差が著しく大きいこと
は明かである。したがって、カルマンフィルタの演算位
置を実際の位置に早期に収束せしめるためには、初期段
階において推測航法の位置センサ４の出力の誤差を示す
共分散行列Σxkの値に補正をかけて、収束を早めること
が必要となる。

【００９６】そこで、そのような補正をかけることで、
推測航法の位置センサを一方の位置センサとして使用し
た場合であっても、カルマンフィルタによる演算位置が
実際の位置に収束するまでの収束期間を短くすることが
できるようにするのがこの第４の実施例の目的である。

【００９７】以下、図６に示すフローチャートを参照し
て、この第４の実施例の処理の手順を説明する。

【００９８】ここで、図４は、推測航法のみを使用した
場合の車両の走行経路の計算値Ｌ1（Ｑ0は初期位置で、
Ｑ1、Ｑ2…は各推定位置を示す）、ＧＰＳのみを使用し
た場合の車両の走行経路の計算値（点列Ｐ1Ｐ2…Ｐn…
として示す）、実施例のカルマンフィルタによるセンサ
フュージョンの結果得られる車両の走行経路の計算値
Ｌ、車両の実際の走行経路ＬRをそれぞれ示している。

【００９９】センサフュージョンの結果得られる走行経
路Ｌは、初期段階Ｒでは、実際の走行経路ＬRに一致し
ていないが、やがてＳ点において実際の走行経路ＬRに
一致することになる。

【０１００】以下、この図４を併せ参照して説明する
と、まず、状態方程式（１）式の状態変数が真の値に収
束しているか否かが判断される。つまり、演算上の位置
（走行経路Ｌ）が実際の位置（走行経路ＬR）に収束し
ているか否かが判断される。

【０１０１】具体的には、上記第３の実施例で述べたよ
うに、状態変数sinθ、cosθに関して、上記（２０）
式、 １ーα＜（sinθ）2＋（sinθ）2＜１＋α が成立したか否かによって判断する（ステップ２０
１）。

【０１０２】ここで、上記（２０）式が成立していない
場合には、未だカルマンフィルタによる演算位置が実際
の位置と未だ収束していない区間Ｒ（図４参照）にある
ものと判断し、ステップ２０２に移行される。

【０１０３】ここでは、上記（１）式、（１０）式に示
す予測演算の結果得られる予測位置Ｘkの不確かさΣxk
が大きくなるよう補正される。

【０１０４】たとえば、 Σxk(-)＝ＣΣxk(-) …（２１） （ただし、Ｃは定数）といった補正演算がなされ、不確
かさΣxk(-)が大きくされる（ステップ２０２）。

【０１０５】そして、この（２１）式によって補正され
た不確かさΣxkを使用して、上記（２）式、（１１）〜
（１３）式に示す推定演算が行なわれる（ステップ２０
３）。ついで、予測演算が実行されて（ステップ２０
４）、手順は再び上記ステップ２０１に移行される。

【０１０６】ステップ２０１において、上記（２０）式
が成立した場合には、カルマンフィルタによる演算位置
が実際の位置に収束した、つまりＳ点（図４参照）に到
達したものと判断し、手順は上記ステップ２０２を経ず
に、直接ステップ２０３に移行され、上記（２１）式に
より不確かさΣxk(-)に補正をかけることなく推定演算
が実行されることになる。以後、予測演算（ステップ２
０４）が実行され、手順はステップ２０１に移行されて
同様な処理が繰り返し実行される。

【０１０７】なお、この実施例では、（２１）式に示す
ように、推測航法のセンサ４の出力の不確かさΣxk(-)
に補正係数Ｃをかけて、当該不確かさを大きくしている
が、結果的に不確かさΣxk(-)を大きくできるのであれ
ば、いかなる補正演算であってもよい。

【０１０８】ＧＰＳのセンサ７の出力の不確かさΣvを
小さくすることで相対的に不確かさΣxkを大きくしても
よく、また不確かさΣxkを大きくするとともに、不確か
さΣvを小さくしてもよい。

【０１０９】このように、この第４の実施例によれば、
演算の初期段階において推測航法の位置センサを用いた
予測演算部４の出力の不確かさを示す共分散行列Σxkの
値が大きくなるように補正をかけるようにしたので、カ
ルマンフィルタの演算が正確に実行され、演算位置を実
際の位置に早期に収束せしめることができ、不正確な区
間Ｒを短くすることができる。

【０１１０】

【発明の効果】以上説明したように本発明によれば、出
力間隔が大きく異なる位置検出手段同士であったとして
も、本来、出力間隔の短い第１の位置検出手段の出力間
隔をそのままに保持したまま、精度を維持したままで、
カルマンフィルタによるセンサフュージョンを適用でき
るようになる。

【０１１１】また、本発明によれば、状態方程式の方向
θの状態変数の代わりに、ｓｉｎθ、ｃｏｓθが使用さ
れ、当該状態方程式が線形化されるので、非線形式に対
応した拡張カルマンフィルタを用いなくてもよくなる。
このため、演算が複雑とならず、誤差が小さくでき、演
算の簡易化、演算精度の向上を図ることができる。

【０１１２】また、本発明によれば、（ｓｉｎθ）2＋
（ｃｏｓθ）2と１との差が所定のしきい値以下になっ
た時点で、状態変数が真の値に収束したものと判断され
るので、推測航法のセンサを一方の第１の位置検出手段
として使用した場合において、最初は演算上の位置が実
際の位置に収束していなくても、いつ演算位置が実際の
位置に収束したかの情報を取得することができるように
なる。

【０１１３】また、本発明によれば、状態変数が真の値
に未だ収束していないと判断されている場合には、予測
演算の結果得られる予測位置の不確かさが大きくなるよ
う補正され、この補正した不確かさを使用して推定演算
が行なわれるので、推測航法のセンサを一方の第１の位
置検出手段として使用した場合であっても、カルマンフ
ィルタによる演算位置が実際の位置に収束するまでの収
束期間を短くすることができるようになる。

【図面の簡単な説明】

【図１】図１は本発明に係る建設車両の位置計測装置の
実施例の構成を示すブロック図である。

【図２】図２は図１に示すカルマンフィルタ演算部の構
成を示すブロック図である。

【図３】図３は出力間隔が異なる２つのセンサの出力間
隔を比較して示す図である。

【図４】図４は実施例の車両位置の走行経路を示す図で
ある。

【図５】図５は図１に示す装置で実行される処理手順を
示すフローチャートである。

【図６】図６は図１に示す装置で実行される処理手順を
示すフローチャートである。

【符号の説明】

１ 推定部 ２ 予測部 ３ カルマンフィルタ推定演算部 ４ カルマンフィルタ予測演算部 ５ 車両方位計 ６ 車輪回転センサ ７ 車両絶対位置センサ

Claims (4)

【特許請求の範囲】
1. 【請求項１】 建設車両の位置を検出する第１の位
   置検出手段および第２の位置検出手段と、前記第１の位
   置検出手段の出力と前記第２の位置検出手段の出力とを
   カルマンフィルタを介して建設車両の位置を演算出力す
   る演算手段とを具えた建設車両の位置計測装置であっ
   て、前記演算手段において前記第１の位置検出手段の出
   力と状態方程式と推定位置とに基づいて位置の予測演算
   を行い、予測位置を求めるとともに、前記第２の位置検
   出手段の出力と観測方程式と前記予測位置とに基づいて
   位置の推定演算を行い、推定位置を求めるようにした建
   設車両の位置計測において、 前記推定演算を行う時点において、第２の位置検出手段
   から位置検出値が未だ出力されてない場合には、前記第
   ２の位置検出手段から位置検出値が出力されるまでの
   間、この第２の位置検出手段の位置検出値の代わりに、
   当該時点において前記予測位置を使用するようにした建
   設車両の位置計測装置。
2. 【請求項２】 建設車両の位置を推測航法によって
   検出する第１の位置検出手段および第２の位置検出手段
   と、前記第１の位置検出手段の出力と前記第２の位置検
   出手段の出力とをカルマンフィルタを介して建設車両の
   位置を演算出力する演算手段とを具えた建設車両の位置
   計測装置であって、前記演算手段において前記第１の位
   置検出手段の出力と状態変数が推測航法に基づく位置お
   よび方向θである状態方程式と推定位置とに基づいて位
   置の予測演算を行い、予測位置を求めるとともに、前記
   第２の位置検出手段の出力と観測方程式と前記予測位置
   とに基づいて位置の推定演算を行い、推定位置を求める
   ようにした建設車両の位置計測において、 前記状態方程式の方向θの状態変数の代わりに、ｓｉｎ
   θ、ｃｏｓθを使用することにより、当該状態方程式を
   線形化するようにした建設車両の位置計測装置。
3. 【請求項３】 （sinθ）2＋（cosθ）2と１との差
   が所定のしきい値以下になった時点で、前記状態変数が
   真の値に収束したものと判断するようにした請求項２記
   載の建設車両の位置計測装置。
4. 【請求項４】 建設車両の位置を検出する第１の位
   置検出手段および第２の位置検出手段と、前記第１の位
   置検出手段の出力と前記第２の位置検出手段の出力とを
   カルマンフィルタを介して建設車両の位置を演算出力す
   る演算手段とを具えた建設車両の位置計測装置であっ
   て、前記演算手段において前記第１の位置検出手段の出
   力と状態方程式と推定位置およびこの推定位置の不確か
   らしさとに基づいて位置の予測演算を行い、予測位置お
   よびこの予測位置の不確からしさを求めるとともに、前
   記第２の位置検出手段の出力と観測方程式と前記予測位
   置およびこの予測位置の不確からしさとに基づいて位置
   の推定演算を行い、推定位置およびこの推定位置の不確
   からしさを求めるようにした建設車両の位置計測におい
   て、 前記状態方程式の状態変数が真の値に収束したか否かを
   判断する判断手段を具え、 前記判断手段によって、前記状態変数が真の値に未だ収
   束していないと判断されている場合には、前記予測演算
   の結果得られる予測位置の不確からしさが大きくなるよ
   う補正し、この補正した不確からしさを使用して前記推
   定演算を行うようにした建設車両の位置計測装置。