JP2627409B2 - 車輪付き車両航法装置および自動車両の移動制御方法 - Google Patents

Description

【発明の詳細な説明】 本発明は車輌用航法および航法装置に関するものであ
り、更に詳細に述べれば誘導線すなわち軌道を使用せず
にプログラム制御に従つて移動する自動化された車輌に
対する航法および航法装置に関するものである。

自動化された車両システムに対する航法制御技術はこ
れまでにも種々提案されている。そのうちの一つのシス
テムにおいては、該車輌に取り付けられた誘導輪、例え
ば脚輪の回転角度ならびに操向角から短い時間間隔の間
に前記車輌の線形移動ならびに角移動を計算する予測航
法システムによつて車輌の経路が決定される。この方法
では、前記システムによつてそれ自体の位置がいつでも
予測される。前記予測航法システムによつてこのように
予測された位置は、ある間隔で前記車輌に取り付けられ
た走査レーザー光源を用いて周知の位置にある反射装置
に対する方位を測定することにより検査される。そのよ
うな方位測定動作によつて検出された位置的誤差は、次
いでフイルタによる位置情報の補正に利用される。

しかしながら、この装置には以下の欠点がある。すな
わち、前記車輌が各時間間隔内で移動した直線距離なら
びにその角度の測定には、どうしても誤差が生ずること
である。これらの誤差は、例えば摩耗により生じた脚輪
半径の変化や各時間間隔中の前記脚輪の操向角度の測定
における不正確さ等の結果として、また前記車輌が「ク
ラッビング（車両が偏流される現象のことを意味す
る）」運動により移動する傾向があるために発生するも
のである。

本発明は、操向手段と、および記憶されたプログラム
に応答して前記操向手段を制御する手段とを有する車輌
に対する航法および航法装置を提供するものである。前
記航法および航法装置は、複数の連続する時間間隔の各
時間間隔中における直線走行距離と所定の回転の中心の
回りの回転量とを決定する手段と、所定の基準座標フレ
ーム内における車輌の車首方位角（Ψ）ならびに空間位
置（x,y）の更新された予測値を生ずる手段と、前記基
準フレーム内の周知の所定位置にある目標物に対する方
位および／あるいは範囲（レンジ）を生ずると共に、前
記車輌が予測された車首方位で予測された位置に実際に
ある場合測定されたであろう方位または範囲と実際に測
定された方位または範囲との差として誤差信号を発生す
る手段と、および前記車輌の車首方位角ならびに空間位
置（x,y）の補正予測値と、更に前記車輌の一つ以上の
別のパラメータの補正予測値とを前記誤差信号に応答し
て生ずるフイルタ手段とによつて構成されている。

先行技術によるシステムでは、方位測定によつて発生
された誤差信号が車首方位ならびに空間位置データの補
正にのみ利用されている。それに対し、本発明では、各
時間間隔中における車輌の直線走行距離ならびに車両の
回転量の決定に利用されるパラメータ、および／あるい
は前記車輌の直線走行距離を空間位置の移動に変換する
際利用されるパラメータ等の他のパラメータを補正する
場合にも前記誤差信号が利用される。前記直線距離およ
び前記回転が操向角度ならびに前記時間間隔における前
記脚輪の回転角度とを測定する感知器によつて決定され
た場合、前記フイルタ手段によつて補正することができ
る付加パラメータは、操向角度の測定値のオフセツト
と、各時間間隔中における脚輪回転角度の測定値のオフ
セツトと、および操向輪の半径の誤差とである。別の誤
差発生原因としては、車輌の車首方位の予測値と実際の
移動方位との系統的角度偏差がある。そのような誤差
は、車輌がどうしても車首方位の片側に移動してしまう
「クラツビング」効果により生じうるものである。この
「クラツビング」効果は、シヤーシ、車軸もしくは車輪
等の機械的不一致や車輌の駆動輪におけるトルクの不整
合等によつて生ずる。この種の誤差は、明瞭な「クラツ
ビング」運動を生じうる、目標物に対する方位または範
囲形成に利用される装置の不一致、によつても発生しう
る。これらの効果が総合された結果としての系統的な角
度誤差をここでは「クラツビング」角度と称することに
する。

前記誤差信号を連続的に利用して、方位を得る度に前
記決定手段および予測値発生手段を有効に再較正するこ
とにより、制御手段によつて駆動ならびに操向手段が必
要な補正を行ない、確実に補正ルートを追従させること
ができるように車輌の正しい位置が有効に補正されるば
かりでなく、それ自体によつて測定される車輌の位置の
精度も連続的に高められる。その理由は、前記決定手段
ならびに予測値発生手段は、連続的に方位が得られる度
に、一層、精度が上がるためである。本発明による装置
は、また、操向輪の角度および摩耗、「クラツビング」
角度のずれの測定に利用される感知器のずれをも動的に
補正することができる。

次に、添付の図面を参照しながら、ほんの一例ではあ
るが実施例を挙げて本発明を説明してゆく。

第１図は本発明による航法装置を備え付けた車輌を示
す図であり、 第２図はクラツビング角を説明する図であり、かつ 第３図は前記車輌の航法装置の動作を説明するブロツ
ク図である。

本発明によつて、記憶されたプログラムに従いそれ自
身の移動を制御し、プログラムされた経路を追従するこ
とのできる自動化された車両が提供される。前記のプロ
グラムされた制御は、車輌の最大速度ならびに利用可能
な経路幅等の、プログラムされた制約の範囲内で所要の
経路に追従するよう車輌に対する操向ならびに速度要求
が適切に行なわなければならないことが判る。本発明
は、車輌にそれ自身の位置を測定させ、既知の位置にあ
る目標物への方位、もしくは該目標物に対する範囲、或
いはまたそれらの組合せを得ることによつてそれらの予
測値を補正させる航法および航法装置に関するものであ
る。

第１図には、任意の基準デカルト座標フレーム内に位
置決めされた車輌２が図示されているが、前記座標フレ
ームは、一般に前記車輌が移動すべく制約された工場の
作業面積を表わすため工場フレームと称することにす
る。前記車輌２上の任意の点Ｐの位置は、前記工場フレ
ーム内のその座標（x,y）によつて定められる。前記任
意の点Ｐは、例えば駆動モータ（図示せず）と接続した
一対の駆動輪４を搭載する軸の中間点である。脚輪の操
向輪６は、点Ｐから距離ｄのところにある車輌の前部中
央に取り付けられている。前記脚輪６は、垂直軸の回り
に回動して、脚輪６の垂直面と車輌の進む方向を示す車
首方位ベクトル８間に所望の操向角αを与えることがで
きる。脚輪は、そのような車輌に利用できる唯一の操向
調整形式ではない。例えば、前記車輌の両側に一つずつ
ある一対の車輪を差動的に駆動することによつても操向
可能である。以下は、脚輪６による操向を仮定して説明
するものである。

車首方位ベクトル８によつて、工場フレームのＸ軸と
の角度Ψが定められている。

車輌がその位置を確認するために、多数の目標物Ｔ
（例えば反射装置）が工場フレームの周囲に配置されて
いる。前記車輌２には、該車輌上の点Ｐに具合良く位置
決めされたレーザー方位探知装置（図示せず）が設けら
れている。適切な形式の方位探知装置については、英国
特許出願番号第2 143 395号に詳しく説明されている。
ある適用例では、方位より目標物に対する範囲の測定に
適した装置を利用する場合もある。これは、例えば目標
物を車輌からかなり離れたところに設置しなければなら
ない場合に適している。この場合、範囲を測定すること
によつて更に高精度を得ることができる。その他の実施
例では、方位および範囲の双方、すなわち車輌に対する
目標物の両極座標を測定することができる。以下は、方
位を利用することを仮定して説明したものだが、付加的
に、または代替的に範囲測定値を利用する場合にも容易
に適合しうることが判る。

前記方位探知装置は、反射装置Ｔに向けて１本のレー
ザー光線を放射する。前記光線が正確に前記反射装置に
当たると、前記方位探知器は、反射されたレーザー光の
エネルギーを受信する。該レーザービームが送信される
方向と車首方位ベクトル８との間の方位θが制御装置に
よつて認識される。前記車輌制御装置は、前記方位探知
器が目標物を感知すると常に反射装置Ｔに対する方位を
生ずるようプログラムされている。従つて、そのような
方位は、車輌を動作中ある間隔で利用することができ
る。前記制御装置は、それ自体の位置を連続的に計算す
ると共に、工場フレームをマツプをその記憶装置に記憶
しているため、適当な反射装置の予測方位 を計算することができる。しかしながら、一般的に、実
際の方位と計算された の値とは幾分異なる。従つて、前記方位探知装置によつ
て、 に相当する誤差信号が発生されるが、該誤差信号によつ
て、本発明の航法装置による車輌自体の位置予測値を補
正することができる。範囲探知装置を利用した場合、前
記誤差信号は測定された実際の範囲と予測された範囲と
の差となる。

必要な誤差信号を発生するために上述のレーザー装置
とは別に他の設計による方位および／或いは範囲探知装
置を利用できることが判る。

第２図では、車輌２のクラツビング角に対する二つの
要因が図示されている。機械的不一致またはトルクの不
整合、および床面の隆起による車輌の反作用等により、
車輌２は車首方位のベクトル８よりもベクトル10方向に
移動する傾向を示す。前記機械的不一致ならびにトルク
不整合により、ベクトル８および10間に、まずずれては
いるが一定の値β_１が生ずる。脚輪６ならびに床面間の
相互作用によるβ_１の変動は、ガウス分布を有している
とみなされているランダムノイズによる撹乱を生ずる。
クラツビング角の起因となるもう一つの要素β_２は、方
位探知装置の設定によつて生ずる。前記レーザーは、車
輌フレーム内で基準方向12に対する方位を測定する。こ
の基準方向12は、車首方位ベクトル８と一致するのが望
ましいが、車首方位に対して角度β_２だけ偏位してもよ
い。これから説明する航法装置において、前記補正装置
はベクトル８ではなくベクトル12に対応する車首方位角 の予測値を生ずるのに役立つ。車輌の空間位置の変化を
この 値を利用して計算する場合、誤差が入つてくるので、車
輌の新規の空間位置を正しく測定するには を用いて変換する必要がある。但し、この場合、βはβ
_１とβ_２の和である。

次に、第３図の参照しながら前記車輌航法装置につい
て説明する。

前記車輌航法装置には、連続する時間間隔Δｔが経過
した後脚輪６と関連する感知器からの入力信号手段によ
つて車輌の位置を更新する予想航法位置予測器20が備え
られている。通常のΔｔ値は60ミリ秒である。前記感知
器（図示せず）は、操向角度αと前記車輪が各時間間隔
Δｔに回転する角度γとを検出し、前記予測器20に対し
て入力22ならびに24としてこれらの値を印加する。前記
角度感知器は、例えば、脚輪６の周囲、または脚輪６の
取付け軸上に位置決めされた基準標識と共働する、固定
された磁気または光感知器でもよい。航法装置の記憶装
置には、脚輪６の半径R_cの測定値も含まれている。この
測定値は、後程更に詳しく説明するような態様で航法装
置の動作中更新される。前記予想航法位置予測器20は、
前記感知器からの入力22ならびに24を利用して車首方位
ベクトルの方向に移動する直線距離Ｖ（ｔ）と、および
夫々出力26ならびに28として時間ｔに始まる時間間隔Δ
ｔにおける前記点Ｐを介して定められた垂直軸の回りの
車輌の回転量Ｕ（ｔ）とを計算する。時間Δｔにおける
Ｖ（ｔ）およびＵ（ｔ）の値は、下記の如く得られる。
すなわち、 しかしながら、R_cおよびαはずれによる誤差を受けるこ
とがある。R_cの場合、操向輪の半径は記憶装置に記憶さ
れた値と異なることがあり、αの場合、測定値は、較正
誤差を受けることがある。よつて、誤差パラメータが導
入され、その結果脚輪６の半径はR_c（１＋l_dm）とな
り、実際の操向角度はα＋δとなる。よつて、上式
（１）は、 となる。

測定値γにも較正誤差があるが、これらの較正誤差
は、パラメータl_dmによつて効率的に考慮することがで
きる。パラメータl_dmおよびδは、後程詳しく説明する
ような態様で制御装置により常に更新される。δの現在
値は、線30を介して加算器32に送られるが、該加算器に
は線22を介してαの測定値が入力される。前記加算装置
32の出力は、前記予想航法位置予測器に送られる。l_dm
の現在値は、線34に沿つて乗算器36に送られるが、該乗
算器は、線24を介しもう一方の入力としてγの測定値を
受信する。入力線24は、加算器38の一方の入力に並列で
送られ、次いで該加算器38の出力は、予想航法位置予測
器20に送られる。この入力は、上式（２）に必要な係数
γ（１＋l_dm）を表わしている。

時間ｔ＋Δｔにおける車輌の現車首方位Ψ（ｔ＋Δ
ｔ）は、明らかに下記の如く表わされる。すなわち Ψ（ｔ＋Δｔ）＝Ψ（ｔ）＋Ｕ（ｔ） （3a） よつて、Ψ（ｔ＋Δｔ）は、加算装置40においてΨ
（ｔ）の値にＵ（ｔ）を加算することによつて発生され
る。Ψ（ｔ）の値は、遅延線42を介して加算装置（40）
の出力から発生される。

前記Ψ（ｔ）の値は、誤差信号のｋ_Ψ倍だけ補正係数
を付加することによつて目標物を観測した結果として前
記誤差信号が得られた場合、補正される。利得ｋ_Ψの発
生は、後程更に詳しく説明する。前記加算は、遅延線の
前にある加算器44で行なわれる。車輌の現在の車首方位
Ψ（ｔ＋Δｔ）は、線46に沿い加算器48を介して運動変
換処理装置50に送られる。

前記車輌の実際の移動方向は、予測車首方位角プラス
前記パラメータβで表わされたクラツビング角と考える
ことができる。従つて、車輌の基準点Ｐの位置変化は、
角度Ψ＋βにおける方向ベクトル10プラスΨの計算値な
らびに予測されるずれによる誤差パラメータβ双方に関
連する別個の白色ガウスノイズベクトルに沿つて生ず
る。βの現予測値は線52を介して加算器48の一方の入力
に送られ、その結果前記運動変換処理装置50に対する入
力は、Ψ＋βを表示する。線26上のＶ（ｔ）値は、前記
運動変換処理装置50に直接送られる。

該運動変換処理装置50の機能は、工場フレーム内の車
輌の位置（x,y）の予測値を連続的に更新して発生する
ことである。ｘ（ｔ＋Δｔ）およびｙ（ｔ＋Δｔ）の値
は、線54,56上に出力され、加算器58ならびに60に送ら
れると共に、並列で観測計算処理装置62にも送られる。

前記加算器58,60は、ある間隔で、誤差信号のk_x倍、
またはk_y倍だけ各補正係数をｘ（ｔ＋Δｔ）およびｙ
（ｔ＋Δｔ）の予測値に夫々加算する。各加算器の出力
は、各遅延線64,66を介して運動変換処理装置50に送ら
れ、ｘ（ｔ）ならびにｙ（ｔ）の補正された入力を該運
動変換処理装置50に印加する。

前記運動変換処理装置は、下記の如く前記入力情報か
ら（x,y）の更新された予測値を計算する。すなわち、 ＝（ｔ）cos（Ψ＋β） ＝（ｔ）sin（Ψ＋β） これらの式を時間間隔ｔ＋Δｔに渡つて積分すると、下
記の如く示すことができる。すなわち、 〔Ψ（ｔ＋Δｔ）＋β（ｔ＋Δｔ）〕の値は、時間ｔ
＋Δｔに加算器48からの出力として運動変換処理装置50
へ送られる。

前回の値の一つが、上式3_bならびに3_cからｘ（ｔ＋Δ
ｔ）およびｙ（ｔ＋Δｔ）を計算する際Ψ（ｔ）＋β
（ｔ）として利用するために記憶される。

Ψ,x,yの値は、観測計算処理装置62に送られる。これ
らの値の予測値は、Δｔ（通常60ミリ秒）毎に更新され
る。更に長い、不規則な間隔での設定が可能であり、レ
ーザー方位探知装置は、工場フレーム内の位置（x_i,
y_i）に位置決めされた目標物の方向に方位θ_ｉを生ず
る。方位が得られると、観測計算処理装置62は、Ψ,x,y
の見積りと方位の取られた目標物の位置（x_i,y_i）につ
いての入力データとから予測方位 を計算する。前記予測方位は、加算器67の反転入力に送
られる。前記加算器67に対するもう一方の入力68は、方
位探知装置から発生された実際の方位である。加算器67
の出力は、予測方位ならびに実際の方位間の差 を表わす誤差信号であるが、該誤差信号は六つの乗算器
72から82に送られる。x_i,y_iにある目標物に対しては、
以下の如く表わすことができる。

観測された角度θ_ｉは、ランダムノイズε_θによつて
崩されるので、上式は下記の如く書き表わすことができ
る。

前記カルマンフイルタには、パラメータ｛Ψ,x,y,δ,
l_dm,β｝の各々に対するカルマン利得係数を夫々表わす
六つの出力を有する処理装置70が備えられている。各係
数は、乗算器72から82の夫々に送られ、そこで誤差信号
と乗算される。その出力積は、加算器に送られ、関連す
るパラメータの前の予測値に加算され、白色ガウスノイ
ズとみなされる誤差効果に対してそれを補正する。前記
Ψ,xおよびｙの予測値に前記補正係数を夫々加算する加
算器44,58および60は、既に説明したものである。

パラメータδ,l_dmおよびβの各々は、同じ構成から成
る遅延線84,86,88および加算器90,92,94も有し、下記の
如く表わされる。すなわち、 前記カルマンフイルタ利得は望ましくは全パラメータ
Ψ,x,y,δ,l_dmおよびβの現予測値を表わす入力バンク9
6を有する処理装置70を利用して計算される。前記カル
マンフイルタのプログラムには、ランダムまたは白色ガ
ウスノイズを受ける線形系に対する確立されたフイルタ
リング技術が利用される。これらの技術は、1967年アカ
デミツクプレス社発行のM.Aokiによる「確率系の最適
化」と題する文献（特に179〜182頁参照）に詳しく説明
されている。本明細書は、もつぱらカルマンフイルタア
ルゴリズムに関連して説明したものだが、必要な利得の
計算に適した他のフイルタリングアルゴリズムも利用で
きることが判る。

以上の説明から、本実施例に対する車輌の運動を説明
する完全な非線形差分方程式と、および前記ずれ補正項
の展開とが下記の如く書き表わされる。すなわち、 Ψ(ｔ＋Δｔ)＝Ψ(ｔ)＋Ｕ(ｔ)＋ε_Ψ(ｔ＋Δｔ) ｘ（ｔ＋Δｔ）＝ｘ（ｔ）＋f_x（Ψ（ｔ） ＋β（ｔ）,U（ｔ）,V（ｔ））ε_ｘ（ｔ＋Δｔ） ｙ（ｔ＋Δｔ）＝ｙ（ｔ）＋f_y（Ψ（ｔ） ＋β（ｔ）,U（ｔ）,V（ｔ））ε_ｙ（ｔ＋Δｔ） δ（ｔ＋Δｔ）＝δ（ｔ）＋ε_δ（ｔ＋Δｔ） l_dm（ｔ＋Δｔ）＝l_dm（ｔ）＋εl_dm（ｔ＋Δｔ） β（ｔ＋Δｔ）＝β（ｔ）＋ε_β（ｔ＋Δｔ） 但し、上式において、 であり、かつε_Ψ，ε_ｘおよびε_ｙは、床の起伏等によ
る車輌の運動に対するいかなる撹乱も表示する確立変数
である。ずれパラメータδ,l_dmおよびβは、ε_δ，εl
_dmおよびε_βがそれらを撹乱する確立変数である場合、
ランダムな移動としてモデル化される。

位置Ψ（ｔ＋Δｔ）,x（ｔ＋Δｔ）,y（ｔ＋Δｔ）に
おける、目標物「ｉ」に対する測定角度は、 θ_ｉ（ｔ＋Δｔ）＝tan^-1｛（y_i −ｙ（ｔ＋Δｔ））／（x_i−ｘ（ｔ＋Δｔ））｝ −Ψ（ｔ＋Δｔ）＋ε_θ（ｔ＋Δｔ） （７） となる。但し、x_i,y_iは目標物「ｉ」の座標であり、ε
_θは測定装置の誤差を表わす確率変数である。

次に、時間ｔまでに収集された全データを用いて、時
間ｔにおける車輌の位置およびずれパラメータの予測値
を示すと、 となる。

上式（５），（６），および（７）の変数から下記の
如く定義することができる。すなわち、 と、および と定義される。但し、 等は、時間ｔまでに収集された全データを利用して得ら
れた、時間（ｔ＋Δｔ）における車輌の位置パラメータ
とずれパラメータの予測値である。それらは、前記予想
航法位置予測器20および運動変換処理装置50において下
式により計算される。

すなわち、 上記予測値を利用して、時間（ｔ＋Δｔ）における目標
物「ｉ」に対する角度の最良予測値を計算することがで
きる。すなわち、 上式（５），（６）および（７）は、 （t|t）他、および 他についてテイラー級数で展開することができると共
に、更に上位の項を無視することによって以下の線形形
式で表すこともできる。すなわち、 但し、前記ベクトルは、 Δz(t)={ΔΨ(t),Δx(t),Δy(t),Δδ(t),Δl_dm(t),Δβ(t)} となり、測定された誤差信号は、 ε_ｚはランダム変数ε_Ψ，ε_x,等々のベクトルとな
る。前記行列Atは、点 （t|t）等における式（５），（６），および（７）
の関数の導関数を計算することによつて得られる。従つ
て、記号で表わすと下記の如くになる。すなわち、 但し、 であり、かつ観測ベクトルは、 である。

線形化された上式（８）を利用して、前記文献の「確
率系の最適化」に定義されたアルゴリズムを用いてカル
マン利得｛ｋ_Ψ,k_x,k_y,k_δ,k l_dm,k_β｝のベクトルを計
算することができる。前記アルゴリズムは、後程詳細に
説明する確率変数ベクトルε_ｚおよび測定誤差ε_θの統
計量を必要とする。適当な形式は、零平均値、すなわち
下記の如く定められた分散による正規分布確率変数を仮
定することである。

ε｛ε_ｚ・ε_ｚ′｝＝C_t・Ｑ・C_t′；ε｛ε_θ ^２｝＝Ｒ 但し、Ｑは共分散の対角行列であり、C_tは回転行列で
ある。通常、前記行列C_tは、車輌の車首方位と整合すべ
くｘおよびｙ座標内の無作為な撹乱を回転する。すなわ
ち、 カルマン利得を計算するアルゴリズムは、性質上回帰的
であり（前記文献を参照のこと）、予測値に対する最初
の共分散行列を必要とする。すなわち、 ε｛Δｚ（ｏ）．Δｚ′（ｏ）｝＝G_o 車両の位置およびドリフト補正項の位置については最初
は判っていないことを表すようこの行列を最初に適当に
選択すると、対角要素が充分に大きい対角行列となる。
前記処理装置70によつて実行されるプログラムは、観
測が行なわれる度毎にマトリツクスA_tおよびC_tが計算さ
れるよう設計されている。その時、「演繹的」共分散行
列Ｐが下記の如く計算される。すなわち、 Ｐ＝A_tGA_t′＋C_tQC_t′ 但し、Ｇは、前の観測からの帰納的共分散行列である。
これが最初の観測であつた場合、G_oが用いられる。

前記観測計算処理装置62は、予測角度 を生じ、かつ加算器67は、前述の乗算器に対して誤差信
号を出力する。該誤差信号が所定の閾値以下の場合、観
測行列Ｈ_ｔ＋Δｔが計算される。次いで、カルマン利得
係数が以下の関係式から計算される。すなわち、 これらのカルマン利得係数値は、前記乗算器72〜82、と
および加算器44,58,60,90,92ならびに94とによつて、既
に述べた如く δ（ｔ＋Δｔ），_dm（ｔ＋Δｔ）ならびに の予測値を補正するのに夫々利用される。

前記帰納的共分散行列Ｇは、以下の如く更新され、次
の観測が行なわれる時に利用される。すなわち、 Ｇ＝Ｐ−Ｋ_ｔ＋ΔｔＨ_ｔ＋ΔｔＰ 前記処理装置62からの誤差信号が閾値以上の場合、カル
マン利得係数は処理装置70によつて計算されない。Ｇの
新規の値は、Ｐの最後の値となる。よつて、補正された
予測値は、前記予想航法位置予測器および運動変換処理
装置によつて発生されたもとの予測値のままとなり、パ
ラメータδ,l_dmならびにβの値は、そのままとなる。前
記閾値は、車輌が走行していた時間等の要因によりある
間隔で再計算され、可能性としてどの位誤差信号が大き
いのかを表示する。閾値を利用することによつて、例え
ば予測方位の計算されていた目標物とは異なる目標物か
らの方位により発生された明らかに違う誤差信号や、目
標物以外のものからの擬似レーザー反射等が廃棄され、
必要性のない大幅な針路補正が防止される。

処理装置70での実行に必要な行列操作を簡素化するた
め、種々の技術を利用することができる。ある対称を利
用することによつて、行列の多くはいくつかの零項を有
し、かつ行列Ａを恒等行列と非対角項だけを有する行列
との和として書き込むことによつて、乗算ならびに加算
数を処理しやすい値へ低減し、前記処理が実時間内に実
行できるようにする。

上記x,yデカルト座標系の代わりにいずれの適当な座
標系を利用してもよいことが判る。このことによつて、
車輌の運動を表わす式が影響を受けることはあつても基
本的原則が変更されることはない。同様に、車輌の操行
調整、または各時間間隔中の運動測定に対する設計変更
によつて、前記運動式の項が改変される。いずれの適当
な車輌の設計およびパラメータの定義に対しても、上記
の如く線形化してフイルタリングアルゴリズムに利用す
ることのできる運動の方程式系が提供されることが判
る。

Claims (5)

(57)【特許請求の範囲】
1. 【請求項１】車両の移動した距離の測定値を生じる車輪
   の車輪回転角（γ）および操向手段の操向角（α）を検
   出するための検出手段（22、24）と、 前記検出手段と結合され、複数の連続した時間間隔の各
   々の間における車両の走行した直線距離（）および所
   定の回転中心の回りでの車両の回転量（）を計算する
   ための予想航法位置予測手段（20）と、 該予想航法位置予測手段の出力と結合され、所定の基準
   座標フレーム内における車両の車首方位角 および空間位置（，）の更新された予想値を発生す
   るための更新予想手段（40、42、50）と、 該更新予想手段の出力および基準フレーム内の所定の目
   標（Ｔ）の位置（xi,yi）に関する入力データに応答
   し、前記目標に対する車両の予想方位角 または／およびレンジを計算するための計算手段（62）
   と、 前記目標（Ｔ）に対する車両の方位角（θｉ）または／
   およびレンジを測定するための手段（68）と、 前記測定された方位角（θｉ）またはレンジと前記予想
   方位角 またはレンジとの差を表わす誤差信号を発生するための
   手段（67）と、 前記誤差信号に応答して、車首方位角、該車首方位角と
   車両の実際の移動方向とのオフセット値、および空間位
   置、を含むパラメータの補正された予想値を発生するた
   めのフィルタ手段（70、72〜82）とを備えた操向手段と
   を有し、 前記フィルタ手段は、距離測定値を生じる前記車輪の径
   （Rc）の変化（ldm）および／または操向手段の操向角
   のオフセット値（δ）および／または一つ以上の前記パ
   ラメータの補正された予想値と、さらに前記一つ以上の
   パラメータの更新値を発生するよう誤差信号に応答自在
   であり、 前記予想航法位置予測手段（20）および／または前記更
   新予想手段（40、42、50）および／または前記誤差信号
   発生手段（67）のそれぞれの測定値は、前記フィルタ手
   段が出力する前記更新値により連続的に更新され、該更
   新された測定値に基づき予め記憶されたプログラムに応
   答して前記操向手段を制御することを特徴とする車輪付
   き車両航法装置。
2. 【請求項２】フィルタ（70、72〜87）は、誤差信号が所
   定値を越えない限り前記パラメータの補正値を発生する
   よう作動することを特徴とする請求の範囲第１項記載の
   車輪付き車両航法装置。
3. 【請求項３】フィルタ手段（70、72〜87）は、カルマン
   フィルタアルゴリズムを用いることを特徴とする請求の
   範囲第１項または第２項記載の車輪付き車両航法装置。
4. 【請求項４】車両の移動した距離の測定値を表す車輪の
   回転角（γ）および車両の操向手段（６）の操向角
   （α）を検出すると共に、複数の連続する時間間隔の各
   々の内で前記検出から車両の角度変位量（）および線
   形変位量（）を決定し、 所定の基準座標フレーム内の車両の存在の車首方位角 および位置（，）の予想値を更新し、 前記基準座標フレーム内における所定の目標に対する車
   両の予想方位角 および／またはレンジを計算し、 前記目標に対する車両の方位角（θｉ）および／または
   レンジを測定し、 該測定された方位角またはレンジと、予想方位角または
   レンジとの間の誤差を表す誤差信号を発生し、 所定のフィルタリングを用いて前記誤差信号に応答して
   車両の車首方位角および位置の補正予想値を発生し、 前記フィルタリングが、距離の測定値を生じる車輪の径
   （Rc）、操向手段の操向角のオフセット値（δ）および
   車両の実際の方向と車両の車首方位角とのオフセット値
   を含む一つ以上のパラメータの更新値を発生することに
   より、予め記憶されたプログラムに応答して自動車両の
   移動を制御することを特徴とする自動車両の移動制御方
   法。
5. 【請求項５】前記フィルタリングは、カルマンフィルタ
   アルゴリズムを用いて各パラメータに対する補正係数を
   発生し、該補正係数に前記誤差信号を乗算し、該乗算し
   た積を前記パラメータの先の予想値に加算して補正され
   た予想値を発生することを特徴とする請求の範囲第４項
   記載の自動車両の移動制御方法。