(57)【要約】  ビークル（移動体）にＧＰＳとジャイロと加速度センサを搭載して、推定加速度（Ａ＾X,Ａ＾Y，Ａ＾Z）と観測加速度（ａ〜x，ａ〜y，ａ〜z）を求め、ジャイロで測定の角速度（ｐ〜，ｑ〜，ｒ〜）からロール角φ〜、ピッチ角θ〜及びヨー角ψ〜を求め、観測加速度（ａ〜x，ａ〜y，ａ〜z）と座標変換行列Ｅ-1  によって地球座標系の加速度（Ａ〜X，Ａ〜Y，Ａ〜z）を求める。推定加速度（Ａ＾X，Ａ＾Y,Ａ＾Z）と観測加速度（Ａ〜X，Ａ〜Y，Ａ〜z）の差を推定加速度誤差（δＡ^X、δＡ^Y、δＡ^Z）として、δφ＝ｔａｎ-1｜δＡYZ｜／｜ＡYZ｜、δθ＝ｔａｎ-1｜δＡXZ｜／｜ＡXZ｜、δψ＝ｔａｎ-1｜δＡXY｜／｜ＡXY｜とし、推定加速度ＡYZと推定加速度誤差δＡYZ等の外積によりδφ、δθ、δψの極性を求めて、極性を考慮した姿勢角誤差（δφ、δθ、δψ）を用いて、推定ロール角φ＝ロール角φ〜−δφ、推定ピッチ角θ＝ピッチ角θ〜−δθ、推定ヨー角ψ＝ヨー角ψ〜−δψにより各姿勢角を求める。
2008142757.tif 000002
【特許請求の範囲】
【請求項１】
ビークル（移動体）にＧＰＳとジャイロと加速度センサを搭載してビークルの姿勢角推定方法であって、
前記ＧＰＳによってビークルの位置（ＲX，ＲY，ＲZ）、前記ジャイロによってｘ軸回りのロール角速度ｐ〜、ｙ軸回りのピッチ角速度ｑ〜、ｚ軸回りのヨー角速度ｒ〜、前記加速度センサによってｘ軸方向の縦加速度ａ〜x、ｙ軸方向の横加速度ａ〜y、ｚ軸方向の垂直加速度ａ〜zが測定され、
（１）前記ＧＰＳによるビークルの位置（ＲX，ＲY，ＲZ）から地球座標系の推定加速度（Ａ^X，Ａ^Y，Ａ^Z）を求め、
（２）前記加速度センサによるビークルの観測加速度（ａ〜x，ａ〜y，ａ〜z）を求め、
（３）前記ジャイロで測定の角速度（ｐ〜，ｑ〜，ｒ〜）から姿勢角誤差を含んだロール角φ〜、ピッチ角θ〜及びヨー角ψ〜を求め、それらの値から姿勢角誤差を含んだ座標変換行列Ｅ-1を求め、
（４）ビークル座標系の観測加速度（ａ〜x，ａ〜y，ａ〜z）から姿勢角誤差を含んだ前記座標変換行列Ｅ-1によって地球座標系の加速度（Ａ〜X，Ａ〜Y，Ａ〜z）を求め、
（５）前記ＧＰＳから求めた地球座標系の推定加速度（Ａ^X，Ａ^Y，Ａ^Z）と前記加速度センサから求めた地球座標系の観測加速度（Ａ〜X，Ａ〜Y，Ａ〜z）の差を推定加速度誤差（δＡ＾X，δＡ＾Y，δＡ＾Z）とし、
（６）ＧＰＳから求めた推定加速度ＡYZ（ベクトル要素：Ａ^Y，Ａ^Z）と推定加速度誤差δＡYZ（ベクトル要素：δＡ^Y，δＡ^Z）の関係から姿勢角誤差δφ＝ｔａｎ-1｜δＡYZ｜／｜ＡYZ｜、推定加速度ＡXZ（ベクトル要素：Ａ^X，Ａ^Z）と推定加速度誤差δＡXZ（ベクトル要素：δＡ^X，δＡ^Z）の関係から姿勢角誤差δθ＝ｔａｎ-1｜δＡXZ｜／｜ＡXZ｜、推定加速度ＡXY（ベクトル要素：Ａ^X，Ａ^Y）と推定加速度誤差δＡXY（ベクトル要素：δＡ^X，δＡ^Y）の関係から方位角誤差δψ＝ｔａｎ-1｜δＡXY｜／｜ＡXY｜をそれぞれ求めると共に、前記推定加速度ＡYZと推定加速度誤差δＡYZの外積によりδφ、推定加速度ＡXZと推定加速度誤差δＡXZの外積によりδθ、推定加速度ＡXYと推定加速度誤差δＡXYの外積によりδψの極性判定を行うか、
あるいは、ビークル座標系の推定加速度ａyz（ベクトル要素：a^y，a^z）と加速度差δayz
（ベクトル要素：δa^y，δa^z）の関係から姿勢角誤差δφ＝ｔａｎ-1｜δａyz｜／｜ａyz｜，推定加速度axz（ベクトル要素：a^x，a^z）と加速度誤差δaxz（ベクトル要素：δa^x，δa^z）の関係から姿勢角誤差δθ＝ｔａｎ-1｜δａxz｜／｜ａxz｜，推定加速度axy（ベクトル要素：a^x，a^y）と加速度誤差δaxy（ベクトル要素：δa^x，δa^y）の関係から姿勢角誤差δψ＝ｔａｎ-1｜δａxy｜／｜ａxy｜をそれぞれ求めると共に、前記推定加速度aYZと推定加速度誤差δaYZの外積によりδφ、推定加速度aXZと推定加速度誤差δaXZの外積によりδθ、推定加速度aXYと推定加速度誤差δaXYの外積によりδψの極性判定を行い、
（７）前記極性を考慮した姿勢角誤差δφ、δθ、δψにより下記の算出式で推定姿勢角（ロール角φ、ピッチ角θ、ヨー角ψ）を求める。
推定ロール角φ＝（（誤差を含んだロール角φ〜）−（ロール姿勢角誤差δφ）
推定ピッチ角θ＝（誤差を含んだピッチ角θ〜）−（ピッチ姿勢角誤差δθ）
推定ヨー角ψ  ＝（誤差を含んだヨー角ψ〜）  −（方位角誤差δψ）
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ビークルの姿勢角計測方法に関する。
【背景技術】
【０００２】
自動車を始めとするビークルの走行に関する姿勢角計測方法には、移動体上に複数の衛星信号受信アンテナを設置して測定する、特開２００１−１９４４４２号公報がある。
この公報によれば、移動体上に２つ以上の衛星信号受信アンテナを設置し、複数の衛星からの衛星信号搬送波（キャリア）位相の位相差を利用して、基準とするアンテナ（基準局）から見たその他のアンテナの相対位置をセンチメートルレベルの高精度で求め、その相対位置から移動体の姿勢角を計測する。例えば、２つのアンテナで構成した場合は、移動体の前後方向に２アンテナを設置すれば、方位角とピッチ角が計測できる。また、その２つのアンテナと直交した位置に３台目のアンテナを設置すれば、ロール角も計測することができる。【０００３】
【特許文献１】特開２００１−１９４４４２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
前記公報の方法によると、複数の衛星信号受信機を使用しての姿勢角計測は原理的に容易であが、固定局（基準局）と移動体上に最低でも３つの衛星信号受信機が必要であり、コストおよび、ビークルへの搭載占有容積がかさむことになり実用上の障害となる。
そこで、本発明では、高精度な測位ができるＧＰＳ受信機を１つと、安価（低精度）な慣性センサ（ジャイロおよび加速度センサ）を搭載し、慣性センサのみでは発生を避けられない姿勢角誤差に起因する加速度誤差を、ＧＰＳで求めた位置から推定する高精度な加速度で補正して高精度な姿勢角計測方法を提供するものである。
【課題を解決するための手段】
【０００５】
本願の発明は、ビークル（移動体）にＧＰＳとジャイロと加速度センサを搭載してビークルの姿勢角測定方法である。
そして、時刻ｔｉにおいて、ＧＰＳによってビークルの位置（ＲX，ＲY，ＲZ）が測定され、ジャイロによってｘ軸回りのロール角速度ｐ〜、ｙ軸回りのピッチ角速度ｑ〜、ｚ軸回りのヨー角速度ｒ〜が測定される。又、加速度センサによってｘ軸方向の縦加速度ａ〜x、ｙ軸方向の横加速度ａ〜y、ｚ軸方向の垂直加速度ａ〜zが測定される。【０００６】
次に、前記求めた時刻ｔｉにおける各測定値を用いて下記の演算を行う。
（１）前記ビークルの位置（ＲX，ＲY，ＲZ）から地球座標系の推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）求める。この推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）は前記ビークルの位置（ＲX，ＲY，ＲZ）を観測データとするカルマンフィルタで推定する。このカルマンフィルタは後述の式（１）の運動方程式を基準に構成する。
（２）加速度センサによるビークルの観測加速度（ａ〜x，ａ〜y，ａ〜z）を求める。
（３）前記ジャイロで測定の角速度（ｐ〜，ｑ〜，ｒ〜）から姿勢角誤差を含んだロール角φ〜、ピッチ角θ〜及びヨー角ψ〜を求め、それらの値から姿勢角誤差を含んだ座標変換行列Ｅ-1を求める。【０００７】
（４）ビークル座標系の観測加速度（ａ〜x，ａ〜y，ａ〜z）に姿勢角誤差を含んだ前記で求めた座標変換行列Ｅ-1をかけ算することによって、地球座標系加速度（Ａ〜X，Ａ〜Y，Ａ〜z）が求まる。
（５）前記ＧＰＳから求めた地球座標系の推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）と前記加速度センサ観測値から求めた地球座標系の観測加速度（Ａ〜X，Ａ〜Y，Ａ〜z）の差を推定加速度誤差（δＡ＾X，δＡ＾Y，δＡ＾Z）とする。
（６）そして、ＧＰＳから求めた推定加速度ＡYZ（ベクトル要素：Ａ＾Y，Ａ＾Z）と推定加速度誤差δＡYZ（ベクトル要素：δＡ＾Y，δＡ＾Z）の関係から姿勢角誤差δφ＝ｔａｎ-1｜δＡYZ｜／｜ＡYZ｜、また、ＧＰＳから求めた推定加速度ＡXZ（ベクトル要素：Ａ＾X，Ａ＾Z）と推定加速度誤差δＡXZ（ベクトル要素：δＡ＾X，δＡ＾Z）の関係から姿勢角誤差δθ＝ｔａｎ-1｜δＡXZ｜／｜ＡXZ｜、さらに、ＧＰＳから求めた推定加速度ＡXY（ベクトル要素：Ａ＾x，Ａ＾Y）と推定加速度誤差δＡXY（ベクトル要素：δＡ＾X，δＡ＾Y）の関係から方位角誤差δψ＝ｔａｎ-1｜δＡXY｜／｜ＡXY｜をそれぞれ求める。又、前記推定加速度ＡYZと推定加速度誤差δＡYZの外積によりδφ、推定加速度ＡXZと推定加速度誤差δＡXZの外積によりδθ、推定加速度ＡXYと推定加速度誤差δＡXYの外積によりδψの極性判定を行う。【０００８】
あるいは、前記（６）においては姿勢角誤差を地球座標系で求めたが、ビークル座標系で求めてもよい。この場合、ビークル座標系の推定加速度ａyz（ベクトル要素：a^y，a^z）と加速度誤差δayz（ベクトル要素：δa^y，δa^z）から姿勢角誤差δφ＝ｔａｎ-1｜δａyz｜／｜ａyz｜，推定加速度axz（ベクトル要素：a^x，a^z）と加速度誤差δaxz（ベクトル要素：δa^X，δa^z）の関係から姿勢角誤差δθ＝ｔａｎ-1｜δａxz｜／｜ａxz｜，推定加速度axy（ベクトル要素：a^x，a^y）と加速度誤差δaxy（ベクトル要素：δa^X，δa^y）関係から姿勢角誤差δψ＝ｔａｎ-1｜δａxy｜／｜ａxy｜をそれぞれ求める。また、前記推定加速度aYZと推定加速度誤差δaYZの外積によりδφ、推定加速度aXZと推定加速度誤差δaXZの外積によりδθ、推定加速度aXYと推定加速度誤差δaXYの外積によりδψの極性判定を行う。【０００９】
（７）そして、前記極性を考慮した姿勢角誤差（δφ、δθ、δψ）を用いて、下記の算出式で推定姿勢角（ロール角φ、ピッチ角θ、ヨー角ψ）を求める。
推定ロール角φ＝（誤差を含んだロール角φ〜）−（ロール姿勢角誤差δφ）
推定ピッチ角θ＝（誤差を含んだピッチ角θ〜）−（ピッチ姿勢角誤差δθ）
推定ヨー角ψ  ＝（誤差を含んだヨー角ψ〜）  −（方位角誤差δψ）
（８）以上から、時刻ｔｉにおける推定ロール角φ、推定ピッチ角θ、推定ヨー角ψが求まる。
以上の過程（１）〜（７）を時刻ｔｉ、ｔｉ＋δｔ、…と順次求めることによって、ビークルの移動時における姿勢角が求まる。
【発明の効果】
【００１０】
本願の発明は、ビークルに各１台のＧＰＳとジャイロと加速度センサの搭載により、ビークルの回転系と並進系の運動方程式において姿勢角誤差を含んだ座標変換行列Ｅ-1を用いて、加速度誤差を含む地球座標系加速度（Ａ〜X，Ａ〜Y，Ａ〜Z）が求まる。一方，前記ビークルのＧＰＳ測位値（RX，RY，RZ）からほとんど誤差のない正確な地球座標系の推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）が求まる。そして、地球座標系加速度（Ａ〜X，Ａ〜Y，Ａ〜Z）と推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）からカルマンフィルタで推定加速度誤差（δＡ＾X、δＡ＾Y，δＡ＾Z）を求める。この推定加速度誤差（δＡ＾X、δＡ＾Y，δＡ＾Z）と高精度な地球座標系の推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）の幾何学的関係から姿勢角誤差（δφ，δθ，δψ）を求めると共に、その姿勢角誤差（δφ，δθ，δψ）の極性を求める。そして，この姿勢角誤差を含む姿勢角から、極性を考慮した姿勢角誤差δφ，δθ，δψを差し引くことで，ロール角φ、ピッチ角θ及びヨー角ψの推定を可能にするので、簡便な設備で推定できる。
【図面の簡単な説明】
【００１１】
【図１】ＧＰＳとジャイロと加速度センサによる姿勢角推定方法の算出ブロック図である。
【図２】（Ａ）はＧＰＳから求めた推定加速度ＡYZ（ベクトル要素：Ａ＾Y，Ａ＾Z）と推定加速度誤差δＡYZ（ベクトル要素：δＡ＾Y，δＡ＾Z）による姿勢角誤差δφ＝ｔａｎ-1｜δＡYZ｜／｜ＡYZ｜の関係図である。  （Ｂ）はＧＰＳから求めた推定加速度ＡXZ（ベクトル要素：Ａ＾X,Ａ＾Z）と推定加速度誤差δＡXZ（ベクトル要素：δＡ＾X,δＡ＾Z）による姿勢角誤差δθ＝ｔａｎ-1｜δＡXZ｜／｜ＡXZ｜の関係図である。  （Ｃ）はＧＰＳから求めた推定加速度ＡXY（ベクトル要素：Ａ＾X,Ａ＾Y）と推定加速度誤差δＡXY（ベクトル要素：δＡ＾X,δＡ＾Y）による方位角誤差δψ＝ｔａｎ-1｜δＡXY｜／｜ＡXY｜の関係図である。
【発明を実施するための最良の形態】
【００１２】
本発明の実施の一形態について説明する。ビークル（移動体）には１台のＧＰＳ（Global Positioning System）と、１台のジャイロ（３軸角速度センサ）と１台の３軸加速度センサが搭載してあると共に、下記する演算の実行をするソフトウエアを格納のＣＰＵ、メモリ等を備える演算装置を搭載している。尚、以下において、誤差を含む観測値（測定値）にはサフィックス「〜」を付して表し、推定値にはサフィックス「^」を付して表す。
前記ＧＰＳによってビークルの位置（ＲX,ＲY,ＲZ）が測定され、前記ジャイロでは、x軸回りのロール角速度ｐ〜、ｙ軸回りのピッチ角速度ｑ〜、ｚ軸回りのヨー角速度ｒ〜として測定される。又、加速度センサからビークルｘ軸方向の縦加速度ａ〜x、ビークルｙ軸方向の横加速度ａ〜y、ビークルｚ軸方向の垂直加速度ａ〜zが測定データとして求まる。【００１３】
次に、姿勢角であるビークルｘ軸回りのロール角φ、ビークルｙ軸回りのピッチ角θ、ビークルｚ軸回りのヨー角ψをカルマンフィルタで推定するために必要なビークルの運動方程式（状態方程式）を回転系の式（１）と並進系の式（２）を示す。【００１４】
【数１】
2008142757.tif 000003
ここで、Ｗ〜：姿勢誤差を含む姿勢角，ω：角速度（ｐ,ｑ,ｒ），εｄω：角速度ベクトルのゆらぎ，ｗｄω：駆動雑音、α：ビークル回転系の時定数の逆数，νω：観測雑音である。【００１５】
【数２】
2008142757.tif 000004
【００１６】
ここで、ＲG：位置、Ｖ：速度、Ａ：加速度、δＡ：姿勢角誤差による加速度誤差、εＡ：加速度誤差のゆらぎ、εdA：姿勢誤差に基づく加速度のゆらぎ、WA、WdA：駆動雑音、γ：ビークル並進系の時定数の逆数，νR：GPSの観測雑音，νA：加速度計の観測雑音である。
加速度誤差δＡは白色雑音を整形フィルタを通して有色化したガウスマルコフ過程，加速度Ａのゆらぎは白色雑音で駆動される積分過程として加速度誤差δＡと加速度Ａの生成過程を明確に区別してある。【００１７】
又、観測データｙは、ｙ1＝ＲG＋νR、ｙ2＝Ａ＋δＡ＋νAである。
このｙ1は、ＧＰＳで求める位置（ＲX,ＲY,ＲZ）＋観測雑音（νR）である。又、ｙ2はＡ＋δＡ＋νAであり，加速度Ａと姿勢角誤差による加速度誤差δＡを線形結合した形で観測データを表現していることにより，加速度Ａと姿勢角誤差による加速度誤差δＡをカルマンフィルタが前記したように明確に区別した生成過程によって推定し，加速度Ａと加速度誤差δＡの分離を可能にしている。【００１８】
次に、前記式（１）（２）を解くブロック図を図１に示し、この図１を参照して、姿勢角度のロール角φ、ピッチ角θ及びヨー角ψの推定について説明する。
尚、このブロック図に基づいて演算を実行するのは演算装置であり、算出する値は時刻ｔｉにおける値であり、演算はδ時間毎に行う。【００１９】
ＧＰＳで測定される位置（ＲX，ＲY ，ＲZ）を微分すると推定速度Ｖ（Ｖ＾X，Ｖ＾Y，Ｖ＾Z）が求まる。そして、更に、この推定速度Ｖを微分して推定加速度Ａ（Ａ＾x，Ａ＾Y，Ａ＾ｚ）が求まる。この関係を状態方程式へ定式化して地球座標系カルマンフィルタを構築して加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）を推定する。
又、加速度センサからビークルｘ軸方向の縦加速度ａ〜x、ビークルｙ軸方向の横加速度ａ〜y、ビークルｚ軸方向の垂直加速度ａ〜zを測定データとして、上記の状態方程式に加速度および加速度の揺らぎの状態量を併せた形で定式化してカルマンフィルタを構築する。
又、ジャイロで測定のビークルｘ軸回りのロール角速度ｐ〜、ビークルｙ軸回りのピッチ角速度ｑ〜、ビークルｚ軸回りのヨー角速度ｒ〜を積分して、ロール角φ、ピッチ角θ及びヨー角ψを求める。この関係を状態方程式へ定式化して回転座標系カルマンフィルタを構築して(φ，θ，ψ)を推定する。【００２０】
次に、加速度計が計測した観測加速度（ａ〜x，ａ〜y，ａ〜z）を地球座標系の加速度に変換するために、座標変換行列Ｅ-1  （式（３））を用いる。
【数３】
2008142757.tif 000005
【００２１】
前記誤差を含むロール角φ〜、ピッチ角θ〜及びヨー角ψ〜を座標変換行列Ｅ-1（式（３））に代入して行列値を算出する。
そして、前記加速度計が計測した観測加速度（ａ〜x，ａ〜y，ａ〜z）と座標変換行列Ｅ-1をかけ算して、地球座標系加速度（Ａ〜X，Ａ〜Y，Ａ〜Z）を求める。
次に、前記ＧＰＳ観測データから推定した正確な地球座標系の推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）と、前記地球座標系加速度（（Ａ〜X，Ａ〜Y，Ａ〜Z）の差から、カルマンフィルタで推定加速度誤差（δＡ＾X，δ＾Y，δＡ＾Z）を求める。【００２２】
前記ＧＰＳ観測データから推定した正確な地球座標系の推定加速度（Ａ＾X，Ａ＾Y,Ａ＾Z）と推定加速度誤差（δＡ＾X，δＡ＾Y，δＡ＾Z）から、姿勢角誤差であるｘ軸回りのロール角δφ、ｙ軸回りのピッチ角δθ、ｚ軸回りのヨー角δψを求める。尚、前記ＧＰＳ観測データから推定した正確な地球座標系の推定加速度（Ａ＾X，Ａ＾Y，Ａ＾Z）と推定加速度誤差（δＡ＾X，δＡ＾Y，δＡ＾Z）は、座標変換の原理から直交するベクトルである。【００２３】
誤差モデルと誤差の分離
カルマンフィルタで推定した加速度誤差δAは、地球座標におけるもので、ビークルに搭載して計測した加速度の座標変換精度に影響された姿勢角誤差（ロール角φ，ピッチ角θ，ヨー角ψ）に依存する。【００２４】
(１)方位角誤差による加速度誤差
地球座標へ変換したビークルの運動を加速度計の計測値を基に、式（２）のＡ（Specific force）とδA(加速度誤差)加えた形で表すと、式（４）になる。
観測誤差および，重力成分を含まないビークルの運動加速度をSpecific forceと定義する。【００２５】
【数４】
2008142757.tif 000006
尚、aψはSpecificforceであり、δＡψはＡψの座標変換に基づく誤差である。【００２６】
また、式（４）の座標変換行列を変形し，δψが微小とすると式（５）を得る。
【数５】
2008142757.tif 000007
【００２７】
また、方位角誤差による加速度誤差を式（６）として表す。
【数６】
2008142757.tif 000008
【００２８】
さらに，bias誤差による加速度誤差を式（７）として表すと，式(６)と式(７)を加えた加速度誤差を改めてδＡψとおき、更にScale factor の誤差を考慮すると式(８)として得られる。
【数７】
2008142757.tif 000009
【数８】
2008142757.tif 000010
【００２９】
（２）ロール角誤差による加速度誤差
この加速度誤差は同様に式（９）として得る．この場合はロール角誤差による重力加速度成分も加算される。
【数９】
2008142757.tif 000011
【００３０】
（３）ピッチ角誤差による加速度誤差
この加速度誤差も同様に式（１０）として得る．この場合はピッチ角誤差による重力加速度成分も加算される。
【数１０】
2008142757.tif 000012
【００３１】
姿勢角誤差
式（２）からδA＾を推定した後，姿勢角誤差(δψ，δφ，δθ)を求めるには下記の要領で行う。
（１）図２（Ａ）に示すように、ＧＰＳから求めた推定加速度ＡY Z（ベクトル要素：Ａ＾Y，Ａ＾Z）と推定加速度誤差δＡY Z（ベクトル要素：δＡ＾Y，δＡ＾Z）の関係から姿勢角誤差δφ＝ｔａｎ-1｜δＡYZ｜／｜ＡYZ｜が求まる。（図２（Ａ））。
（２）又、図２(Ｂ)に示すように、ＧＰＳから求めた推定加速度ＡXZ（ベクトル要素：Ａ＾X，Ａ＾Z）と推定加速度誤差δＡXZ（ベクトル要素：δＡ＾X，δＡ＾Z）の関係から、姿勢角誤差δθ＝ｔａｎ-1｜δＡXZ｜／｜ＡXZ｜が求まる。（図２（Ｂ））。【００３２】
（３）更に、図２（Ｃ）に示すように、ＧＰＳから求めた推定加速度ＡXY（ベクトル要素：Ａ＾X，Ａ＾Y）と推定加速度誤差δＡXY（ベクトル要素：δＡ＾X，δＡ＾Y）の関係から、方位角誤差δψ＝ｔａｎ-1｜δＡXY｜／｜ＡXY｜が求まる。【００３３】
また、前記で求めた姿勢角誤差δφ、δθ、δψに替えて、ビークル座標系においては下記のように姿勢角誤差δφ、δθ、δψを求めることもできる。
ここでδψの場合について説明すると、δＡを式（１１）等によってビークル座標系に変換する。
【数１１】
2008142757.tif 000013
【００３４】
ビークル座標系の推定加速度を（a＾x，a＾y，a＾z）とし、推定加速度誤差を（δa＾x，δa＾y，δa＾z）とすると、推定加速度aY Z（ベクトル要素：ａ＾Y，ａ＾Z）と推定加速度誤差δaY Z（ベクトル要素：δａ＾Y，δａ＾Z）の関係から姿勢角誤差δφ＝ｔａｎ-1｜δaYZ｜／｜aYZ｜が求まる。
又、推定加速度aXZ（ベクトル要素：a＾X，a＾Z）と推定加速度誤差δaXZ（ベクトル要素：δa＾X，δa＾Z）の関係から、姿勢角誤差δθ＝ｔａｎ-1｜δaXZ｜／｜aXZ｜が求まる。
更に、推定加速度aXY（ベクトル要素：a＾X，a＾Y）と推定加速度誤差δaXY（ベクトル要素：δa＾X，δa＾Y）の関係から、方位角誤差δψ＝ｔａｎ-1｜δaXY｜／｜aXY｜が求まる。【００３５】
尚、加速度誤差を構成する（ａ）バイアスエラー、（ｂ）スケールファクターエラー、（ｃ）姿勢角誤差による加速度誤差、の要因が、姿勢角誤差に大きく影響する場合には、前記（ａ）〜（ｃ）を適当な方法で分離し、姿勢各誤差による加速度誤差のみを抽出する必要がある。【００３６】
（４）次に、前記で求めた姿勢角誤差δφ、δθ、δψの極性を判定する。
姿勢角誤差δφの極性は推定加速度ＡYZと推定加速度誤差δＡYZの外積により、δθの極性は推定加速度ＡXZと推定加速度誤差δＡXZの外積による。又、δψの極性は推定加速度ＡXYと推定加速度誤差δＡXYの外積による。
あるいは、前記ビークル座標系においては、姿勢角誤差δφの極性は推定加速度aY Zと推定加速度誤差δaYZの外積により、δθの極性は推定加速度aXZと推定加速度誤差δaXZの外積による。又、δψの極性は推定加速度aXYと推定加速度誤差δaXYの外積による。
（５）そして、時刻ｔｉにおけるｘ軸回りのロール角φ、ｙ軸回りのピッチ角θ、ｚ軸回りのヨー角ψは、前記極性を考慮した姿勢角誤差δφ、δθ、δψの用いて、下記の算出式で求める。
推定ロール角φ＝（誤差を含んだロール角φ〜）−（ロール姿勢角誤差δφ）
推定ピッチ角θ＝（誤差を含んだピッチ角θ〜）−（ピッチ姿勢角誤差δθ）
推定ヨー角ψ  ＝（誤差を含んだヨー角ψ〜）  −（方位角誤差δψ）【００３７】
以上のように、時刻ｔｉにおける推定ロール角φ、推定ピッチ角θ、推定ヨー角ψが求まり、順次、時刻ｔi＋δ、ｔi+２δ、・・・において姿勢角を求める。この求める方法は、１台のＧＰＳと、１台のジャイロ（３軸角速度センサ）と１台の３軸加速度センサ可能であり、且つ、精度のよい姿勢角を得ることができる。
尚、前記ではビークルに搭載の３軸加速度センサでビークルの観測加速度（ａ〜x，ａ〜y，ａ〜z）を求めているが、速度センサを搭載して、この速度から加速度を求めてもよい。
【図１】
2008142757.tif 000014
【図２】
2008142757.tif 000015
【国際調査報告】
2008142757.tif 000016
2008142757.tif 000017
2008142757.tif 000018
2008142757.tif 000019