JP2010096647A - 航法装置及び推定方法 - Google Patents
航法装置及び推定方法 Download PDFInfo
- Publication number
- JP2010096647A JP2010096647A JP2008268150A JP2008268150A JP2010096647A JP 2010096647 A JP2010096647 A JP 2010096647A JP 2008268150 A JP2008268150 A JP 2008268150A JP 2008268150 A JP2008268150 A JP 2008268150A JP 2010096647 A JP2010096647 A JP 2010096647A
- Authority
- JP
- Japan
- Prior art keywords
- speed
- unit
- error
- calculation unit
- calculated
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
Abstract
【課題】航法装置の推定の精度を劣化させることなく、計算量を少なくする。
【解決手段】姿勢角外挿計算部122は、角速度センサ111が計測した移動体座標系における移動体800の角速度ωbに基づいて、慣性座標系における移動体800の姿勢角qnを計算する。速度増分計算部126は、加速度センサ112が計測した移動体座標系における移動体800に加わる加速度abと姿勢角qnとに基づいて、慣性座標系における移動体800の速度の増分Δvnを計算する。速度外挿計算部127は、速度の増分Δvnに基づいて、慣性座標系における移動体800の速度vnを計算する。速度減算部130は、速度vnと、GPS受信機113が計測した慣性座標系における移動体800の速度vgとの差dvを計算する。第一推定部140は、姿勢角qnと、速度の増分Δvnと、速度の差dvとに基づいて、角速度の誤差bgと、姿勢角の誤差εとを推定する。
【選択図】図2
【解決手段】姿勢角外挿計算部122は、角速度センサ111が計測した移動体座標系における移動体800の角速度ωbに基づいて、慣性座標系における移動体800の姿勢角qnを計算する。速度増分計算部126は、加速度センサ112が計測した移動体座標系における移動体800に加わる加速度abと姿勢角qnとに基づいて、慣性座標系における移動体800の速度の増分Δvnを計算する。速度外挿計算部127は、速度の増分Δvnに基づいて、慣性座標系における移動体800の速度vnを計算する。速度減算部130は、速度vnと、GPS受信機113が計測した慣性座標系における移動体800の速度vgとの差dvを計算する。第一推定部140は、姿勢角qnと、速度の増分Δvnと、速度の差dvとに基づいて、角速度の誤差bgと、姿勢角の誤差εとを推定する。
【選択図】図2
Description
この発明は、航空機等の移動体に搭載され、移動体の3次元姿勢角や速度の推定を行う航法装置及びその推定方法に関する。
航法装置は、慣性センサである角速度センサおよび加速度センサの出力を積分することにより、移動体の姿勢角および速度を得る。特に、安定度の低い廉価な慣性センサを用いた廉価な航法装置の場合、出力に含まれるバイアス誤差等の誤差要因により、時間と共に急激に精度が劣化するため、GPS受信機の位置または速度出力により、誤差を補正する機能を有している場合が多い。この誤差の補正手段としては、カルマンフィルタ等の推定器が用いられる。
廣川類、佐藤幸一、中久喜健司「Design and Evaluation of A Tightly Coupled GPS/INS using Low Cost MEMS IMU」GPS/GNSS国際シンポジウム2003、2003年。 Jay A.Farrell、Matthew Barth「The Global Positioning System and Inertial Navigation」McGraw−Hill、1999年。
廣川類、佐藤幸一、中久喜健司「Design and Evaluation of A Tightly Coupled GPS/INS using Low Cost MEMS IMU」GPS/GNSS国際シンポジウム2003、2003年。 Jay A.Farrell、Matthew Barth「The Global Positioning System and Inertial Navigation」McGraw−Hill、1999年。
航空機等、三次元空間で加速度運動をする移動体の場合、カルマンフィルタで高次の行列計算を多く行う必要があるため、計算負荷が高い。そのため、高い処理能力が必要となり、航法装置の製造コストが高くなるという課題がある。
この発明は、例えば、上記のような課題を解決するためになされたものであり、推定の精度を劣化させることなく、航法装置における計算負荷を低減することにより、航法装置の製造コストを低くすることを目的とする。
この発明は、例えば、上記のような課題を解決するためになされたものであり、推定の精度を劣化させることなく、航法装置における計算負荷を低減することにより、航法装置の製造コストを低くすることを目的とする。
この発明にかかる航法装置は、
移動体に搭載される航法装置において、
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有し、
上記角速度計測部は、上記移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部は、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部は、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部は、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部は、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部は、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部は、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする。
移動体に搭載される航法装置において、
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有し、
上記角速度計測部は、上記移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部は、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部は、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部は、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部は、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部は、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部は、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする。
この発明にかかる航法装置によれば、姿勢角外挿計算部が計算した姿勢角qnと、速度増分計算部が計算した速度の増分Δvnと、速度減算部が計算した速度の差dvとに基づいて、第一推定部が角速度の誤差bgと、姿勢角の誤差εとを推定するので、推定にかかる計算量が少なく、航法装置100の製造コストを低く抑えることができる。
実施の形態1.
実施の形態1について、図1〜図8を用いて説明する。
実施の形態1について、図1〜図8を用いて説明する。
図1は、この実施の形態における航法装置100の機能ブロックの構成の一例を示すブロック構成図である。
航法装置100は、航空機や車両などの移動体800に搭載され、移動体800の位置、速度、姿勢角などを推定する装置である。
航法装置100は、角速度センサ111、加速度センサ112、GPS受信機113、慣性航法部120、速度減算部130、第一推定部140、第二推定部150、姿勢角出力部191、速度出力部192、位置出力部193を有する。
航法装置100は、航空機や車両などの移動体800に搭載され、移動体800の位置、速度、姿勢角などを推定する装置である。
航法装置100は、角速度センサ111、加速度センサ112、GPS受信機113、慣性航法部120、速度減算部130、第一推定部140、第二推定部150、姿勢角出力部191、速度出力部192、位置出力部193を有する。
なお、以下の説明において、移動体800を基準とする移動体座標系(第一座標系)の直交軸を、大文字でX軸・Y軸・Z軸、地球を基準とする慣性座標系(第二座標系)の直交軸を小文字でx軸・y軸・z軸と表記する。
移動体座標系は、例えば、移動体800の重心を原点とし、移動体800の前方向をX軸、移動体800の右方向をY軸、移動体800の下方向をZ軸とする。
慣性座標系は、例えば、地球上の所定の点を原点とし、北方向をx軸、東方向をy軸、下方向をz軸とする。あるいは、慣性座標系は、地球の中心を原点とし、春分点方向をx軸、赤道面内で春分点方向に対して垂直な方向をy軸、地軸方向をz軸としてもよい。
移動体座標系は、例えば、移動体800の重心を原点とし、移動体800の前方向をX軸、移動体800の右方向をY軸、移動体800の下方向をZ軸とする。
慣性座標系は、例えば、地球上の所定の点を原点とし、北方向をx軸、東方向をy軸、下方向をz軸とする。あるいは、慣性座標系は、地球の中心を原点とし、春分点方向をx軸、赤道面内で春分点方向に対して垂直な方向をy軸、地軸方向をz軸としてもよい。
角速度センサ111(角速度計測部)は、例えば、ジャイロスコープなど、移動体800の角速度を計測するセンサである。角速度センサ111は、X軸を中心とする回転の角速度ωXと、Y軸を中心とする回転の角速度ωYと、Z軸を中心とする回転の角速度ωZとの三つの角速度を計測する。以下、角速度センサ111が計測した移動体座標系における三軸の角速度ωX,ωY,ωZを成分とする三次の縦ベクトルを「ωb」(=[ωX ωY ωZ]T)と表記し、角速度センサ111が時刻tkに計測した角速度ωbを「ωb k」と表記する。角速度センサ111は、計測した角速度ωbを表わすデータを出力する。角速度センサ111が計測した角速度ωbを表わすデータを出力する頻度は、例えば、1秒間に100回程度である。
加速度センサ112(加速度計測部)は、移動体800に加わる加速度を計測するセンサである。加速度センサ112は、X軸方向の加速度aXと、Y軸方向の加速度aYと、Z軸方向の加速度aZとの三つの加速度を計測する。以下、加速度センサ112が計測した移動対座標系における三軸の加速度aX,aY,aZを成分とする三次の縦ベクトルを「ab」(=[aX aY aZ]T)と表記し、加速度センサ112が時刻tkに計測した加速度abを「ab k」と表記する。加速度センサ112は、計測した加速度abを表わすデータを出力する。加速度センサ112が計測した加速度abを表わすデータを出力する頻度は、角速度センサ111と同様、例えば、1秒間に100回程度である。
GPS受信機113(速度計測部・位置計測部・速度計測精度算出部)は、GPS(Global Positioning System)衛星が送信した電波を受信することにより、移動体800の位置pgや速度vgなどを計測する。GPS受信機113が計測する位置pgや速度vgは、慣性座標系におけるものであり、それぞれxyz軸方向の成分を有する。以下の説明において、GPS受信機113が計測した位置pgや速度vgは、三次の縦ベクトルとして扱う。また、GPS受信機113が時刻tkに計測した位置pg及び速度vgをそれぞれ「pg k」「vg k」と表記する。GPS受信機113は、計測した位置pgや速度vgを表わすデータを出力する。GPS受信機113が計測した位置pgや速度vgを表わすデータを出力する頻度は、例えば、1秒間に1〜4回程度である。
また、GPS受信機113は、受信した電波の数や、GPS衛星の配置などに基づいて、計測した位置pgや速度pgの精度を推定する。GPS受信機113は、推定した精度を表わすデータを出力する。
また、GPS受信機113は、受信した電波の数や、GPS衛星の配置などに基づいて、計測した位置pgや速度pgの精度を推定する。GPS受信機113は、推定した精度を表わすデータを出力する。
慣性航法部120は、角速度センサ111が計測した角速度ωbや加速度センサ112が計測した加速度abに基づいて、慣性座標系における移動体800の姿勢角qnや速度vnを推定する。推定の詳細については、後述する。
慣性航法部120は、例えば、データを処理するCPU(Central Processing Unit)などの処理装置が、プログラムを実行することにより実現してもよいし、ハードウェアにより実現してもよい。あるいは、ハードウェアとソフトウェアとの組み合わせにより実現してもよい。速度減算部130、第一推定部140、第二推定部150なども同様である。
慣性航法部120は、例えば、データを処理するCPU(Central Processing Unit)などの処理装置が、プログラムを実行することにより実現してもよいし、ハードウェアにより実現してもよい。あるいは、ハードウェアとソフトウェアとの組み合わせにより実現してもよい。速度減算部130、第一推定部140、第二推定部150なども同様である。
速度減算部130は、GPS受信機113が計測した速度vgと、慣性航法部120が推定した速度vnとの差dv=vg−vnを計算する。ここで、速度vg及び速度vnは、ともに三次の縦ベクトルであるから、速度減算部130が計算する速度の差dvも三次の縦ベクトルである。速度減算部130は、計算した速度の差dvを表わすデータを出力する。
第一推定部140は、慣性航法部120が推定した姿勢角qnや速度vnと、速度減算部130が計算した速度の差dvとに基づいて、角速度センサ111が計測した角速度ωbのバイアス誤差bgや慣性航法部120が推定した姿勢角qnの誤差εを推定する。なお、第一推定部140が推定する角速度ωbのバイアス誤差bgは、例えば、角速度ωbの三つの成分に対応して、移動体座標系のXYZ三軸それぞれについてのバイアス誤差を成分とする三次の縦ベクトルである。また、第一推定部140が推定する姿勢角qnの誤差εは、例えば、慣性座標系のxyz三軸それぞれを中心とする回転誤差を成分とする三次の縦ベクトルである。なお、推定の詳細については、後述する。第一推定部140は、推定した角速度ωbのバイアス誤差bgや姿勢角qnの誤差εを表わすデータを出力する。
第二推定部150は、速度減算部130が計算した速度の差dvと、GPS受信機113が推定した精度とに基づいて、慣性航法部120が推定した速度vnの誤差δvを推定する。なお、第二推定部150が推定する速度vnの誤差δvは、例えば、慣性座標系のxyz三軸それぞれについての誤差を成分とする三次の縦ベクトルである。推定の詳細ついては、後述する。第二推定部150は、推定した速度の誤差δvを表わすデータを出力する。
慣性航法部120は、第一推定部140や第二推定部150が推定した誤差に基づいて、誤差の補正を行い、誤差を補正した姿勢角qや速度vを算出する。慣性航法部120は、算出した慣性座標系における姿勢角qや速度vを表わすデータを出力する。
姿勢角出力部191は、慣性航法部120が出力した姿勢角qを表わすデータを入力する。姿勢角出力部191は、入力したデータが表わす姿勢角qを、例えば、表示装置を用いて表示したり、送信装置を用いて送信したりするなどして、外部に出力する。
速度出力部192は、慣性航法部120が出力した速度vを表わすデータを入力する。速度出力部192は、姿勢角出力部191と同様、入力したデータが表わす速度vを、外部に出力する。
位置出力部193は、GPS受信機113が出力した位置pgを表わすデータを入力する。位置出力部193は、姿勢角出力部191と同様、入力したデータが表わす位置pgを、外部に出力する。
図2は、この実施の形態における慣性航法部120の機能ブロックの構成の一例を示す詳細ブロック図である。
慣性航法部120は、角速度誤差補正部121、姿勢角外挿計算部122、姿勢角誤差補正部123、姿勢角記憶部124、方向余弦行列計算部125、速度増分計算部126、速度外挿計算部127、速度誤差補正部128、速度記憶部129を有する。
慣性航法部120は、角速度誤差補正部121、姿勢角外挿計算部122、姿勢角誤差補正部123、姿勢角記憶部124、方向余弦行列計算部125、速度増分計算部126、速度外挿計算部127、速度誤差補正部128、速度記憶部129を有する。
角速度誤差補正部121は、角速度センサ111が出力した角速度ωbを表わすデータと、第一推定部140が出力した角速度ωbのバイアス誤差bgを表わすデータとを入力する。角速度誤差補正部121は、入力したデータが表わす角速度ωbと、バイアス誤差bgとに基づいて、角速度ωbの誤差を補正して、誤差を補正した角速度ωb+を算出する。角速度誤差補正部121が算出する角速度ωb+は、角速度ωbと同様、移動体座標系におけるXYZ三軸それぞれを中心とする角速度を成分とする三次の縦ベクトルである。角速度誤差補正部121は、算出した角速度ωb+を表わすデータを出力する。
角速度誤差補正部121は、例えば、次の計算式を用いて、角速度ωb+を算出する。
ここで、ωb+ kは、角速度誤差補正部121が算出する時刻kにおける角速度を表わす三次の縦ベクトル。ωb kは、角速度センサ111が計測した時刻kにおける角速度を表わす三次の縦ベクトル。bgは、第一推定部140が推定した角速度ωbのバイアス誤差を表わす三次の縦ベクトル。
角速度誤差補正部121は、例えば、次の計算式を用いて、角速度ωb+を算出する。
姿勢角記憶部124は、慣性座標系における移動体800の姿勢角qを表わすデータを記憶している。姿勢角記憶部124は、例えば、姿勢角qを表わすデータとして、コータニオン(quaternion)表示形式による四つの実数q0,q1,q2,q3を表わすデータを記憶する。
姿勢角記憶部124は、あらかじめ、例えば、移動体800が水平状態で静止しているときに、図示していないコンパスなどの方位センサが計測した移動体800の向きに基づいて算出した姿勢角を用いて、記憶した姿勢角qを表わすデータを初期化しておく。
姿勢角記憶部124は、あらかじめ、例えば、移動体800が水平状態で静止しているときに、図示していないコンパスなどの方位センサが計測した移動体800の向きに基づいて算出した姿勢角を用いて、記憶した姿勢角qを表わすデータを初期化しておく。
速度記憶部129は、慣性座標系における移動体800の速度vを表わすデータを記憶している。速度記憶部129は、例えば、速度vを表わすデータとして、慣性座標系におけるxyz三軸それぞれの方向の速度を表わす三つの実数vx,vy,vzを表わすデータを記憶する。
速度記憶部129は、あらかじめ、例えば、移動体800は静止しているときに求めた移動体800の速度(すなわち「0」)を用いて、記憶した速度vを表わすデータを初期化しておく。
速度記憶部129は、あらかじめ、例えば、移動体800は静止しているときに求めた移動体800の速度(すなわち「0」)を用いて、記憶した速度vを表わすデータを初期化しておく。
姿勢角記憶部124および速度記憶部129が記憶した姿勢角qおよび速度vは、その後、計測のたびに更新される。以下、時刻tkにおける計測により更新された姿勢角qおよび速度vを、それぞれ「qk」「vk」と表記する。
姿勢角外挿計算部122は、姿勢角記憶部124が記憶した慣性座標系における姿勢角qを表わすデータと、角速度誤差補正部121が出力した補正した移動体座標系における角速度ωb+を表わすデータとを入力する。姿勢角外挿計算部122は、入力したデータが表わす姿勢角qと角速度ωb+とに基づいて、外挿計算をし、慣性座標系における新しい姿勢角qnを算出する。姿勢角外挿計算部122は、算出した姿勢角qnを表わすデータを出力する。
姿勢角外挿計算部122は、例えば、次の計算式を用いて、新しい姿勢角qnを算出する。
ここで、qn kは、姿勢角外挿計算部122が算出する時刻tkにおける姿勢角のコータニオン表示を成分とする四次の縦ベクトル。qk−1は、姿勢角記憶部124が記憶した時刻tk−1における姿勢角のコータニオン表示を成分とする四次の縦ベクトル。Tsは、角速度センサ111が角速度ωbを計測した周期を表わすスカラー値であり、Ts=tk−tk−1。Ωb kは、四行四列の正方行列。ωX,ωY,ωZは、角速度誤差補正部121が補正した時刻tkにおける角速度ωb+ kのXYZ三軸それぞれを中心すとする成分。
姿勢角外挿計算部122は、例えば、次の計算式を用いて、新しい姿勢角qnを算出する。
姿勢角誤差補正部123は、姿勢角外挿計算部122が出力した姿勢角qnを表わすデータと、第一推定部140が出力した姿勢角qnの誤差εを表わすデータとを入力する。姿勢角誤差補正部123は、入力したデータが表わす姿勢角qnと、誤差εとに基づいて、姿勢角qnの誤差を補正して、補正した姿勢角qn+を算出する。姿勢角誤差補正部123は、補正した姿勢角qn+を表わすデータを出力する。
姿勢角誤差補正部123は、例えば、次の計算式を用いて、補正した姿勢角qn+を算出する。
ここで、qn+ kは、姿勢角誤差補正部123が算出する時刻tkにおける補正した姿勢角のコータニオン表示を成分とする四次の縦ベクトル。qn kは、姿勢角外挿計算部122が算出した時刻tkにおける姿勢角のコータニオン表示を成分とする四次の縦ベクトル。○で囲まれた×は、コータニオンの乗算。δqは、コータニオンを表わす四次の縦ベクトル。εは、第一推定部140が算出した姿勢角の誤差を表わす三次の縦ベクトル。‖ε‖は、ベクトルεのノルムであるスカラー値。
姿勢角誤差補正部123は、例えば、次の計算式を用いて、補正した姿勢角qn+を算出する。
姿勢角記憶部124は、姿勢角誤差補正部123が出力した姿勢角qn+を表わすデータを入力する。姿勢角記憶部124は、入力したデータを、姿勢角qを表わすデータとして記憶する。姿勢角記憶部124が記憶した姿勢角qを表わすデータは、姿勢角出力部191が入力して、外部に出力する。
方向余弦行列計算部125は、姿勢角誤差補正部123が出力した補正した姿勢角qn+を表わすデータを入力する。方向余弦行列計算部125は、入力したデータが表わす姿勢角qn+に基づいて、方向余弦行列Cn bを算出する。方向余弦行列Cn bは、移動体座標系から慣性座標系への座標変換を表わす行列である。方向余弦行列計算部125は、算出した方向余弦行列Cn bを表わすデータを出力する。方向余弦行列計算部125が出力した方向余弦行列Cn bを表わすデータは、第一推定部140が入力し、誤差の推定に使用する。
方向余弦行列計算部125は、例えば、次の計算式を用いて、方向余弦行列Cn bを算出する。
ここで、Cn bは、方向余弦行列計算部125が算出する時刻tkにおける方向余弦行列である三行三列の正方行列。q0,q1,q2,q3は、姿勢角誤差補正部123が補正した時刻tkにおける姿勢角qn+のコータニオン表示における各成分。
方向余弦行列計算部125は、例えば、次の計算式を用いて、方向余弦行列Cn bを算出する。
速度増分計算部126は、加速度センサ112が出力した移動体座標系における加速度abを表わすデータと、方向余弦行列計算部125が出力した方向余弦行列Cn bを表わすデータとを入力する。速度増分計算部126は、入力したデータが表わす加速度abと方向余弦行列Cn bとに基づいて、移動体座標系における加速度abを慣性座標系における加速度に変換し、変換した加速度に基づいて、慣性座標系における移動体800の速度の増分Δvnを算出する。速度増分計算部126は、算出した速度の増分Δvnを表わすデータを出力する。速度増分計算部126が出力した速度の増分Δvnを表わすデータは、第一推定部140が入力し、誤差の推定に使用する。
速度増分計算部126は、例えば、次の計算式により、速度の増分Δvnを算出する。
ここで、Δvn kは、速度増分計算部126が算出する時刻tk−1から時刻tkまでの間の速度の増分を表わす三次の縦ベクトル。Tsは、加速度センサ112が加速度を計測した周期を表わすスカラー値で、数12におけるTsと同じく、Ts=tk−tk−1。Cn bは、方向余弦行列計算部125が算出した方向余弦行列である三行三列の正方行列。ab kは、加速度センサ112が計測した時刻tkにおける加速度を表わす三次の縦ベクトル。gnは、慣性座標系における重力加速度を表わす三次の縦ベクトル。
速度増分計算部126は、例えば、次の計算式により、速度の増分Δvnを算出する。
速度外挿計算部127は、速度記憶部129が記憶した慣性座標系における速度vを表わすデータと、速度増分計算部126が出力した慣性座標系における速度の増分Δvnを表わすデータとを入力する。速度外挿計算部127は、入力したデータが表わす速度vと速度の増分Δvnとに基づいて、外挿計算をし、慣性座標系における新しい速度vnを算出する。速度外挿計算部127は、算出した速度vnを表わすデータを出力する。速度外挿計算部127が出力した速度vnを表わすデータは、速度減算部130が入力し、速度の差dvの算出に使用する。
速度外挿計算部127は、例えば、次の計算式を用いて、速度vnを算出する。
ここで、vn kは、速度外挿計算部127が算出する時刻tkにおける速度を表わす三次の縦ベクトル。vk−1は、速度記憶部129が記憶した時刻tk−1における速度を表わす三次の縦ベクトル。Δvn kは、速度増分計算部126が算出した時刻tk−1から時刻tkまでの間の速度の増分を表わす三次の縦ベクトル。
速度外挿計算部127は、例えば、次の計算式を用いて、速度vnを算出する。
速度誤差補正部128は、速度外挿計算部127が出力した慣性座標系における速度vnを表わすデータと、第二推定部150が出力した速度の誤差δvを表わすデータとを入力する。速度誤差補正部128は、入力したデータが表わす速度vnと速度の誤差δvとに基づいて、速度vnの誤差を補正して、補正した速度vn+を算出する。速度誤差補正部128は、算出した速度vn+を表わすデータを出力する。
速度誤差補正部128は、例えば、次の計算式を用いて、速度vn+を算出する。
ここで、vn+ kは、速度誤差補正部128が算出する時刻tkにおける補正した速度を表わす三次の縦ベクトル。vn kは、速度外挿計算部127が算出した時刻tkにおける速度を表わす三次の縦ベクトル。δvkは、第二推定部150が推定した時刻tkにおける速度の誤差を表わす三次の縦ベクトル。
速度誤差補正部128は、例えば、次の計算式を用いて、速度vn+を算出する。
なお、GPS受信機113が移動体800の位置・速度を計測していない場合、速度誤差補正部128は、速度誤差の補正をせず、入力したデータが表わす速度vnを、そのまま速度vn+とする。
速度記憶部129は、速度誤差補正部128が出力した慣性座標系における速度vn+を表わすデータを入力する。速度記憶部129は、入力したデータを、速度vを表わすデータとして記憶する。速度記憶部129が記憶した速度vを表わすデータは、速度出力部192が入力して、外部に出力する。
図3は、この実施の形態における第一推定部140の機能ブロックの構成の一例を示す詳細ブロック図である。
第一推定部140は、カルマンフィルタを用いて、角速度センサ111が計測する角速度ωbのバイアス誤差bgと、慣性航法部120が推定する姿勢角の誤差εとを推定する。
第一推定部140が使用するカルマンフィルタは、例えば、次の式で表わされる六次の縦ベクトルxを状態量とし、三次の縦ベクトルΔzを観測量とする。
第一推定部140は、カルマンフィルタを用いて、角速度センサ111が計測する角速度ωbのバイアス誤差bgと、慣性航法部120が推定する姿勢角の誤差εとを推定する。
第一推定部140が使用するカルマンフィルタは、例えば、次の式で表わされる六次の縦ベクトルxを状態量とし、三次の縦ベクトルΔzを観測量とする。
第一推定部140は、観測行列計算部141、推移行列計算部142、誤差共分散外挿計算部143、カルマンゲイン計算部144、誤差共分散観測更新計算部145、誤差共分散記憶部146、状態量外挿計算部147、状態量観測更新計算部148、状態量記憶部149を有する。
観測行列計算部141は、慣性航法部120の速度増分計算部126が出力した速度の増分Δvnを表わすデータを入力する。観測行列計算部141は、入力したデータが表わす速度の増分Δvnに基づいて、カルマンフィルタで使用する観測行列Hを算出する。観測行列計算部141は、算出した観測行列Hを表わすデータを出力する。
観測行列計算部141は、例えば、次の計算式を用いて、観測行列Hを算出する。
ここで、Hは、観測行列計算部141が算出する観測行列である三行六列の行列。03×3は、三行三列の零行列。Ωvは、三行三列の正方行列。Δvn x,Δvn y,Δvn zは、速度増分計算部126が算出した時刻tk−1から時刻tkまでの間の速度の増分Δvn kのxyz三軸それぞれの方向成分。
観測行列計算部141は、例えば、次の計算式を用いて、観測行列Hを算出する。
なお、観測行列計算部141が計算する観測行列Hは、GPS受信機113が移動体800の位置pgや速度vgを計測し、速度減算部130が速度の差dvを算出した場合のみ使用される。このため、観測行列計算部141は、GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測したか否かを判定し、計測したと判定した場合は観測行列Hを算出し、計測していないと判定した場合は観測行列Hを算出しない構成としてもよい。
推移行列計算部142は、慣性航法部120の方向余弦行列計算部125が出力した方向余弦行列Cn bを表わすデータを入力する。推移行列計算部142は、入力したデータが表わす方向余弦行列Cn bに基づいて、カルマンフィルタで使用する推移行列Φを算出する。推移行列計算部142は、算出した推移行列Φを表わすデータを出力する。
推移行列計算部142は、例えば、次の計算式を用いて、推移行列Φを算出する。
ここで、Φは、推移行列計算部142が算出する推移行列である六行六列の正方行列。I3×3は、三行三列の単位行列。03×3は、三行三列の零行列。Tsは、角速度センサ111及び加速度センサ112が角速度や加速度を計測する周期を表わすスカラー値で、数12や数15におけるTsと同じく、Ts=tk−tk−1。
推移行列計算部142は、例えば、次の計算式を用いて、推移行列Φを算出する。
誤差共分散記憶部146は、カルマンフィルタにおける誤差共分散行列Pを表わすデータを記憶している。誤差共分散記憶部146は、例えば、六行六列の行列である誤差共分散行列Pを表わすデータとして、6×6=36個の実数を表わすデータを記憶する。誤差共分散記憶部146は、例えば、あらかじめ、誤差共分散行列Pの初期値として設定した所定の行列を用いて、記憶した誤差共分散行列Pを表わすデータを初期化しておく。
状態量記憶部149は、カルマンフィルタにおける状態量xを表わすデータを記憶している。状態量記憶部149は、例えば、六次の縦ベクトルである状態量xを表わすデータとして、六個の実数を表わすデータを記憶する。状態量記憶部149は、例えば、あらかじめ、状態量xの初期値として設定した所定のベクトルを用いて、記憶した状態量xを表わすデータを初期化しておく。
誤差共分散記憶部146および状態量記憶部149が記憶した誤差共分散行列Pおよび状態量xは、その後、計測のたびに更新される。以下、時刻tkにおける計測により更新された誤差共分散行列Pおよび状態量xを、それぞれ「Pk」「xk」と表記する。
誤差共分散外挿計算部143は、推移行列計算部142が出力した推移行列Φを表わすデータと、誤差共分散記憶部146が記憶した誤差共分散行列Pを表わすデータとを入力する。誤差共分散外挿計算部143は、入力したデータが表わす推移行列Φと、誤差共分散行列Pとに基づいて、誤差共分散行列の外挿計算をして、外挿した誤差共分散行列P−を算出する。誤差共分散外挿計算部143は、算出した誤差共分散行列P−を表わすデータを出力する。
誤差共分散外挿計算部143は、例えば、次の計算式を用いて、誤差共分散行列P−を算出する。
ここで、P− kは、誤差共分散外挿計算部143が算出する時刻tkにおける外挿した誤差共分散行列である六行六列の正方行列。Pk−1は、誤差共分散記憶部146が記憶した時刻tk−1における誤差共分散行列である六行六列の正方行列。Φは、推移行列計算部142が算出した推移行列である六行六列の正方行列。ΦTは、行列Φの転置行列である六行六列の行列。Qは、あらかじめ設定されたプロセスノイズ行列である六行六列の正方行列。
誤差共分散外挿計算部143は、プロセスノイズ行列Qとして、あらかじめ、設定された値を記憶している。
誤差共分散外挿計算部143は、例えば、次の計算式を用いて、誤差共分散行列P−を算出する。
誤差共分散外挿計算部143は、プロセスノイズ行列Qとして、あらかじめ、設定された値を記憶している。
状態量外挿計算部147は、推移行列計算部142が出力した推移行列Φを表わすデータと、状態量記憶部149が記憶した状態量xを表わすデータとを入力する。状態量外挿計算部147は、入力したデータが表わす推移行列Φと状態量xとに基づいて、状態量の外挿計算をして、外挿した状態量x−を算出する。状態量外挿計算部147は、算出した外挿した状態量x−を表わすデータを出力する。
状態量外挿計算部147は、例えば、次の計算式を用いて、外挿した状態量x−を算出する。
ここで、x− kは、状態量外挿計算部147が算出する時刻tkにおける外挿した状態量である六次の縦ベクトル。Φは、推移行列計算部142が算出した推移行列である六行六列の正方行列。xk−1は、状態量記憶部149が記憶した時刻tk−1における状態量である六次の縦ベクトル。
状態量外挿計算部147は、例えば、次の計算式を用いて、外挿した状態量x−を算出する。
カルマンゲイン計算部144は、観測行列計算部141が出力した観測行列Hを表わすデータと、誤差共分散外挿計算部143が出力した誤差共分散行列P−を表わすデータとを入力する。カルマンゲイン計算部144は、入力したデータが表わす観測行列Hと誤差共分散行列P−とに基づいて、カルマンフィルタにおけるカルマンゲインKを算出する。カルマンゲイン計算部144は、算出したカルマンゲインKを表わすデータを出力する。
カルマンゲイン計算部144は、例えば、次の計算式を用いて、カルマンゲインKを算出する。
ここで、Kは、カルマンゲイン計算部144が算出する時刻tkにおけるカルマンゲインである六行三列の行列。P− kは、誤差共分散外挿計算部143が算出した時刻tkにおける外挿した誤差共分散行列である六行六列の正方行列。Hは、観測行列計算部141が算出した時刻tkにおける観測行列である三行六列の行列。HTは、観測行列Hの転置行列である六行三列の行列。Rは、あらかじめ設定された観測誤差の共分散行列である三行三列の行列。(〜)−1は、逆行列。
カルマンゲイン計算部144は、観測誤差の共分散行列Rとして、あらかじめ、設定された値を記憶している。
カルマンゲイン計算部144は、例えば、次の計算式を用いて、カルマンゲインKを算出する。
カルマンゲイン計算部144は、観測誤差の共分散行列Rとして、あらかじめ、設定された値を記憶している。
なお、カルマンゲイン計算部144が計算するカルマンゲインKは、GPS受信機113が移動体800の位置pgや速度vgを計測し、速度減算部130が速度の差dvを算出した場合のみ使用される。このため、カルマンゲイン計算部144は、GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測したか否かを判定し、計測したと判定した場合はカルマンゲインKを算出し、計測していないと判定した場合はカルマンゲインKを算出しない構成としてもよい。
誤差共分散観測更新計算部145は、観測行列計算部141が出力した観測行列Hを表わすデータと、誤差共分散外挿計算部143が出力した外挿した誤差共分散行列P−を表わすデータと、カルマンゲイン計算部144が出力したカルマンゲインKを表わすデータとを入力する。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測したと判定した場合、誤差共分散観測更新計算部145は、入力したデータが表わす観測行列Hと外挿した誤差共分散行列P−とカルマンゲインKとに基づいて、誤差共分散行列を観測更新し、観測更新した誤差共分散行列P+を算出する。誤差共分散観測更新計算部145は、算出した観測更新した誤差共分散行列P+を表わすデータを出力する。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測していないと判定した場合、誤差共分散観測更新計算部145は、入力したデータが表わす外挿した誤差共分散行列P−をそのまま誤差共分散行列P+とし、誤差共分散行列P+を表わすデータを出力する。
誤差共分散観測更新計算部145は、例えば、次の計算式を用いて、観測更新した誤差共分散行列P+を算出する。
ここで、P+ kは、誤差共分散観測更新計算部145が算出する時刻tkにおける観測更新した誤差共分散行列である六行六列の行列。I6×6は、六行六列の単位行列。Kは、カルマンゲイン計算部144が算出したカルマンゲインである六行三列の行列。Hは、観測行列計算部141が算出した観測行列である三行六列の行列。P− kは、誤差共分散外挿計算部143が算出した外挿した誤差共分散行列である六行六列の行列。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測したと判定した場合、誤差共分散観測更新計算部145は、入力したデータが表わす観測行列Hと外挿した誤差共分散行列P−とカルマンゲインKとに基づいて、誤差共分散行列を観測更新し、観測更新した誤差共分散行列P+を算出する。誤差共分散観測更新計算部145は、算出した観測更新した誤差共分散行列P+を表わすデータを出力する。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測していないと判定した場合、誤差共分散観測更新計算部145は、入力したデータが表わす外挿した誤差共分散行列P−をそのまま誤差共分散行列P+とし、誤差共分散行列P+を表わすデータを出力する。
誤差共分散観測更新計算部145は、例えば、次の計算式を用いて、観測更新した誤差共分散行列P+を算出する。
誤差共分散記憶部146は、誤差共分散観測更新計算部145が出力した誤差共分散行列P+を表わすデータを入力する。誤差共分散記憶部146は、入力したデータを、誤差共分散行列Pを表わすデータとして記憶する。
状態量観測更新計算部148は、状態量外挿計算部147が出力した外挿した状態量x−を表わすデータと、カルマンゲイン計算部144が出力したカルマンゲインKを表わすデータと、速度減算部130が出力した速度の差dvを表わすデータとを入力する。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測したと判定した場合、状態量観測更新計算部148は、入力したデータが表わす外挿した状態量x−とカルマンゲインKと速度の差dvとに基づいて、状態量を観測更新し、観測更新した状態量x+を算出する。状態量観測更新計算部148は、算出した観測更新した状態量x+を表わすデータを出力する。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測していないと判定した場合、状態量観測更新計算部148は、入力したデータが表わす外挿した状態量x−をそのまま状態量x+とし、状態量x+を表わすデータを出力する。
状態量観測更新計算部148は、例えば、次の計算式を用いて、観測更新した状態量x+を算出する。
ここで、x+ kは、状態量観測更新計算部148が算出する観測更新した状態量である六次の縦ベクトル。x− kは、状態量外挿計算部147が算出した外挿した状態量である六次の縦ベクトル。Kは、カルマンゲイン計算部144が算出したカルマンゲインである六行三列の行列。Δzは、観測量である三次の縦ベクトル。dvは、速度減算部130が算出した速度の差である三次の縦ベクトル。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測したと判定した場合、状態量観測更新計算部148は、入力したデータが表わす外挿した状態量x−とカルマンゲインKと速度の差dvとに基づいて、状態量を観測更新し、観測更新した状態量x+を算出する。状態量観測更新計算部148は、算出した観測更新した状態量x+を表わすデータを出力する。
GPS受信機113が時刻tkにおける移動体800の位置pgや速度vgを計測していないと判定した場合、状態量観測更新計算部148は、入力したデータが表わす外挿した状態量x−をそのまま状態量x+とし、状態量x+を表わすデータを出力する。
状態量観測更新計算部148は、例えば、次の計算式を用いて、観測更新した状態量x+を算出する。
状態量記憶部149は、状態量観測更新計算部148が出力した状態量x+を表わすデータを入力する。状態量記憶部149は、入力したデータを、状態量xを表わすデータとして記憶する。状態量記憶部149が記憶した状態量x(角速度ωbのバイアス誤差bgおよび姿勢角qの誤差ε)を表わすデータは、慣性航法部120が入力し、誤差の補正に使用する。
図4は、この実施の形態における第二推定部150の機能ブロックの構成の一例を示す詳細ブロック図である。
第二推定部150は、GPS受信機113が推定したGPS受信機113の計測精度に基づいて、慣性航法部120が推定した速度vの誤差δvを推定する。
第二推定部150は、ゲイン算出部151、速度誤差推定部152を有する。
第二推定部150は、GPS受信機113が推定したGPS受信機113の計測精度に基づいて、慣性航法部120が推定した速度vの誤差δvを推定する。
第二推定部150は、ゲイン算出部151、速度誤差推定部152を有する。
ゲイン算出部151は、GPS受信機113が出力した計測の精度を表わすデータを入力する。ゲイン算出部151は、入力したデータが表わす精度に基づいて、速度誤差のゲインKvを算出する。ゲイン算出部151は、算出したゲインKvを表わすデータを出力する。
ゲイン算出部151が算出するゲインKvは、例えば、0以上1以下の値をとるスカラー値である。この場合、例えば、GPS受信機113の計測精度が高く、GPS受信機113が計測した速度vgが完全に信頼できる場合、ゲイン算出部151は、ゲインKvとして「1」を算出する。逆に、GPS受信機113の計測精度が低く、GPS受信機113が計測した速度vgが全く信頼できない場合、ゲイン算出部151は、ゲインKvとして「0」を算出する。通常は、GPS受信機113が計測した速度vgが完全に信頼できることも全く信頼できないこともないので、ゲイン算出部151は、GPS受信機113が推定した精度に基づいて、ゲインKVとして「0」と「1」との値を算出する。ゲイン算出部151は、GPS受信機113の計測精度が高いほど、算出するゲインKvの値を大きくする。
ゲイン算出部151は、例えば、あらかじめ、GPS受信機113の計測精度をゲインKvに変換する変換テーブルを表わすデータを記憶しておく。ゲイン算出部151は、記憶したデータが表わす変換テーブルを参照し、入力したデータが表わす計測精度に対応するゲインKvを算出する。
ゲイン算出部151が算出するゲインKvは、例えば、0以上1以下の値をとるスカラー値である。この場合、例えば、GPS受信機113の計測精度が高く、GPS受信機113が計測した速度vgが完全に信頼できる場合、ゲイン算出部151は、ゲインKvとして「1」を算出する。逆に、GPS受信機113の計測精度が低く、GPS受信機113が計測した速度vgが全く信頼できない場合、ゲイン算出部151は、ゲインKvとして「0」を算出する。通常は、GPS受信機113が計測した速度vgが完全に信頼できることも全く信頼できないこともないので、ゲイン算出部151は、GPS受信機113が推定した精度に基づいて、ゲインKVとして「0」と「1」との値を算出する。ゲイン算出部151は、GPS受信機113の計測精度が高いほど、算出するゲインKvの値を大きくする。
ゲイン算出部151は、例えば、あらかじめ、GPS受信機113の計測精度をゲインKvに変換する変換テーブルを表わすデータを記憶しておく。ゲイン算出部151は、記憶したデータが表わす変換テーブルを参照し、入力したデータが表わす計測精度に対応するゲインKvを算出する。
なお、ゲイン算出部151が算出するゲインKvは、例えば、三行三列の行列であってもよい。例えば、GPS受信機113が、水平方向の精度と垂直方向の精度というように、方向によって異なる複数の精度を算出する場合、ゲイン算出部151は、慣性座標系におけるxyz三軸の各軸方向によって異なるゲインを算出して、行列形式で表わされるゲインKvとする構成であってもよい。
また、ゲイン算出部151は、GPS受信機113が推定した計測の精度にかかわらず、あらかじめ設定した一定の値(例えば「0.5」)をゲインとして算出する構成であってもよい。その場合、GPS受信機113は、計測の精度を推定する機能のないものであってもよい。
速度誤差推定部152は、速度減算部130が出力した速度の差dvを表わすデータと、ゲイン算出部151が出力したゲインKvを表わすデータとを入力する。速度誤差推定部152は、入力したデータが表わす速度の差dvとゲインKvとに基づいて、慣性航法部120が推定した速度の誤差δvを算出する。速度誤差推定部152は、算出した速度の誤差δvを表わすデータを出力する。速度誤差推定部152が出力した速度の誤差δvを表わすデータは、慣性航法部120が入力し、誤差の補正に使用する。
速度誤差推定部152は、例えば、次の計算式を用いて、速度の誤差δvを算出する。
ここで、δvは、速度誤差推定部152が推定する速度の誤差を表わす三次の縦ベクトル。Kvは、ゲイン算出部151が算出した速度誤差のゲインを表わすスカラー値もしくは三行三列の行列。dvは、速度減算部130が算出した速度の差を表わす三次の縦ベクトル。
速度誤差推定部152は、例えば、次の計算式を用いて、速度の誤差δvを算出する。
次に、動作について説明する。
図5は、この実施の形態における航法装置100が移動体800の速度・姿勢角を推定する航法処理の全体的な流れの一例を示すフローチャート図である。
航法処理は、初期化処理S510、外挿計算処理S520、GPS計測判定工程S540、観測更新処理S550を有する。
航法処理は、初期化処理S510、外挿計算処理S520、GPS計測判定工程S540、観測更新処理S550を有する。
初期化処理S510において、航法装置100は、記憶したデータを初期化する。
外挿計算処理S520において、航法装置100は、角速度センサ111や加速度センサ112が計測した角速度・加速度に基づいて、推定した速度・姿勢角などを外挿計算する。
外挿計算処理S520において、航法装置100は、角速度センサ111や加速度センサ112が計測した角速度・加速度に基づいて、推定した速度・姿勢角などを外挿計算する。
GPS計測判定工程S540において、航法装置100は、GPS受信機113が移動体800の位置・速度を計測したか否かを判定する。
GPS受信機113が移動体800の位置・速度を計測したと判定した場合、航法装置100は、観測更新処理S550へ進む。
GPS受信機113が移動体800の位置・速度を計測していないと判定した場合、航法装置100は、外挿計算処理S520に戻る。
GPS受信機113が移動体800の位置・速度を計測したと判定した場合、航法装置100は、観測更新処理S550へ進む。
GPS受信機113が移動体800の位置・速度を計測していないと判定した場合、航法装置100は、外挿計算処理S520に戻る。
観測更新処理S550において、航法装置100は、GPS受信機113が計測した移動体800の速度に基づいて、推定した速度・姿勢角などを観測更新する。その後、航法装置100は、外挿計算処理S520に戻る。
図6は、この実施の形態における航法装置100がデータを初期化する初期化処理S510の詳細な流れの一例を示すフローチャート図である。
初期化処理S510は、姿勢角初期化工程S511、速度初期化工程S512、誤差共分散初期化工程S514、状態量初期化工程S515を有する。
初期化処理S510は、姿勢角初期化工程S511、速度初期化工程S512、誤差共分散初期化工程S514、状態量初期化工程S515を有する。
姿勢角初期化工程S511において、姿勢角記憶部124は、記憶した姿勢角qを初期化する。
速度初期化工程S512において、速度記憶部129は、記憶した速度vを初期化する。
誤差共分散初期化工程S514において、誤差共分散記憶部146は、記憶した誤差共分散行列Pを初期化する。
状態量初期化工程S515において、状態量記憶部149は、記憶した状態量xを初期化する。
速度初期化工程S512において、速度記憶部129は、記憶した速度vを初期化する。
誤差共分散初期化工程S514において、誤差共分散記憶部146は、記憶した誤差共分散行列Pを初期化する。
状態量初期化工程S515において、状態量記憶部149は、記憶した状態量xを初期化する。
図7は、この実施の形態における航法装置100が外挿計算をする外挿計算処理S520の詳細な流れの一例を示すフローチャート図である。
外挿計算処理S520は、角速度誤差補正工程S521、姿勢角外挿計算工程S522、姿勢角誤差補正工程S523、方向余弦行列計算工程S524、速度増分計算工程S525、速度外挿計算工程S526、推移行列計算工程S531、誤差共分散外挿計算工程S532、状態量外挿計算工程S533を有する。
外挿計算処理S520は、角速度誤差補正工程S521、姿勢角外挿計算工程S522、姿勢角誤差補正工程S523、方向余弦行列計算工程S524、速度増分計算工程S525、速度外挿計算工程S526、推移行列計算工程S531、誤差共分散外挿計算工程S532、状態量外挿計算工程S533を有する。
角速度誤差補正工程S521において、角速度誤差補正部121は、状態量記憶部149が記憶した状態量xのうち角速度のバイアス誤差bgに基づいて、角速度センサ111が計測した角速度ωbを補正し、補正した角速度ωb+を算出する。
姿勢角外挿計算工程S522において、姿勢角外挿計算部122は、角速度誤差補正工程S521で角速度誤差補正部121が補正した角速度ωb+に基づいて、姿勢角記憶部124が記憶した姿勢角qを外挿し、外挿した姿勢角qnを算出する。
姿勢角誤差補正工程S523において、姿勢角誤差補正部123は、状態量記憶部149が記憶した状態量xのうち姿勢角の誤差εに基づいて、姿勢角外挿計算工程S522で姿勢角外挿計算部122が外挿した姿勢角qnを補正し、補正した姿勢角qn+を算出する。姿勢角記憶部124は、姿勢角誤差補正部123が補正した姿勢角qn+を、新しい姿勢角qとして記憶する。
方向余弦行列計算工程S524において、方向余弦行列計算部125は、姿勢角誤差補正工程S523で姿勢角誤差補正部123が補正した姿勢角qn+に基づいて、方向余弦行列Cn bを算出する。
速度増分計算工程S525において、速度増分計算部126は、方向余弦行列計算工程S524で方向余弦行列計算部125が算出した方向余弦行列Cn bに基づいて、加速度センサ112が計測した移動体座標系における加速度abを、慣性座標系における加速度に座標変換し、座標変換した慣性座標系における加速度に基づいて、速度の増分Δvnを算出する。
速度外挿計算工程S526において、速度外挿計算部127は、速度増分計算工程S525で速度増分計算部126が算出した速度の増分Δvnに基づいて、速度記憶部129が記憶した速度vを外挿し、外挿した速度vnを算出する。速度記憶部129は、速度外挿計算部127が外挿した速度vnを、暫定的な速度vとして記憶する。
姿勢角外挿計算工程S522において、姿勢角外挿計算部122は、角速度誤差補正工程S521で角速度誤差補正部121が補正した角速度ωb+に基づいて、姿勢角記憶部124が記憶した姿勢角qを外挿し、外挿した姿勢角qnを算出する。
姿勢角誤差補正工程S523において、姿勢角誤差補正部123は、状態量記憶部149が記憶した状態量xのうち姿勢角の誤差εに基づいて、姿勢角外挿計算工程S522で姿勢角外挿計算部122が外挿した姿勢角qnを補正し、補正した姿勢角qn+を算出する。姿勢角記憶部124は、姿勢角誤差補正部123が補正した姿勢角qn+を、新しい姿勢角qとして記憶する。
方向余弦行列計算工程S524において、方向余弦行列計算部125は、姿勢角誤差補正工程S523で姿勢角誤差補正部123が補正した姿勢角qn+に基づいて、方向余弦行列Cn bを算出する。
速度増分計算工程S525において、速度増分計算部126は、方向余弦行列計算工程S524で方向余弦行列計算部125が算出した方向余弦行列Cn bに基づいて、加速度センサ112が計測した移動体座標系における加速度abを、慣性座標系における加速度に座標変換し、座標変換した慣性座標系における加速度に基づいて、速度の増分Δvnを算出する。
速度外挿計算工程S526において、速度外挿計算部127は、速度増分計算工程S525で速度増分計算部126が算出した速度の増分Δvnに基づいて、速度記憶部129が記憶した速度vを外挿し、外挿した速度vnを算出する。速度記憶部129は、速度外挿計算部127が外挿した速度vnを、暫定的な速度vとして記憶する。
推移行列計算工程S531において、推移行列計算部142は、方向余弦行列計算工程S524で方向余弦行列計算部125が算出した方向余弦行列Cn bに基づいて、推移行列Φを算出する。
誤差共分散外挿計算工程S532において、誤差共分散外挿計算部143は、推移行列計算工程S531で推移行列計算部142が算出した推移行列Φに基づいて、誤差共分散記憶部146が記憶した誤差共分散行列Pを外挿し、外挿した誤差共分散行列P−を算出する。GPS受信機113が移動体800の位置や速度を計測していない場合、誤差共分散外挿計算部143が算出した誤差共分散行列P−が、そのまま誤差共分散記憶部146が記憶する誤差共分散行列Pとなるので、誤差共分散記憶部146は、誤差共分散外挿計算部143が外挿した誤差共分散行列P−を、暫定的に、誤差共分散行列Pとして記憶する。
状態量外挿計算工程S533において、状態量外挿計算部147は、推移行列計算工程S531で推移行列計算部142が算出した推移行列Φに基づいて、状態量記憶部149が記憶した状態量xを外挿し、外挿した状態量x−を算出する。GPS受信機113が移動体800の位置や速度を計測していない場合、状態量外挿計算部147が算出した外挿した状態量x−が、そのまま状態量記憶部149が記憶する状態量xとなるので、状態量記憶部149は、状態量外挿計算部147が外挿した状態量x−を、暫定的に、状態量xとして記憶する。
誤差共分散外挿計算工程S532において、誤差共分散外挿計算部143は、推移行列計算工程S531で推移行列計算部142が算出した推移行列Φに基づいて、誤差共分散記憶部146が記憶した誤差共分散行列Pを外挿し、外挿した誤差共分散行列P−を算出する。GPS受信機113が移動体800の位置や速度を計測していない場合、誤差共分散外挿計算部143が算出した誤差共分散行列P−が、そのまま誤差共分散記憶部146が記憶する誤差共分散行列Pとなるので、誤差共分散記憶部146は、誤差共分散外挿計算部143が外挿した誤差共分散行列P−を、暫定的に、誤差共分散行列Pとして記憶する。
状態量外挿計算工程S533において、状態量外挿計算部147は、推移行列計算工程S531で推移行列計算部142が算出した推移行列Φに基づいて、状態量記憶部149が記憶した状態量xを外挿し、外挿した状態量x−を算出する。GPS受信機113が移動体800の位置や速度を計測していない場合、状態量外挿計算部147が算出した外挿した状態量x−が、そのまま状態量記憶部149が記憶する状態量xとなるので、状態量記憶部149は、状態量外挿計算部147が外挿した状態量x−を、暫定的に、状態量xとして記憶する。
図8は、この実施の形態における航法装置100が観測更新をする観測更新処理S550の詳細な流れの一例を示すフローチャート図である。
観測更新処理S550は、速度減算工程S551、観測行列計算工程S552、カルマンゲイン計算工程S553、誤差共分散観測更新計算工程S554、状態量観測更新計算工程S555、ゲイン算出工程S561、速度誤差推定工程S562、速度誤差補正工程S563を有する。
観測更新処理S550は、速度減算工程S551、観測行列計算工程S552、カルマンゲイン計算工程S553、誤差共分散観測更新計算工程S554、状態量観測更新計算工程S555、ゲイン算出工程S561、速度誤差推定工程S562、速度誤差補正工程S563を有する。
速度減算工程S551において、速度減算部130は、GPS受信機113が計測した速度vgと、速度外挿計算工程S526で速度外挿計算部127が外挿した速度vnとに基づいて、速度の差dvを算出する。
観測行列計算工程S552において、観測行列計算部141は、速度増分計算工程S525で速度増分計算部126が算出した速度の増分Δvnに基づいて、観測行列Hを算出する。
カルマンゲイン計算工程S553において、カルマンゲイン計算部144は、誤差共分散外挿計算工程S532で誤差共分散外挿計算部143が外挿した誤差共分散行列P−と、観測行列計算工程S552で観測行列計算部141が算出した観測行列Hとに基づいて、カルマンゲインKを算出する。
誤差共分散観測更新計算工程S554において、誤差共分散観測更新計算部145は、観測行列計算工程S552で観測行列計算部141が算出した観測行列Hと、カルマンゲイン計算工程S553でカルマンゲイン計算部144が計算したカルマンゲインKとに基づいて、誤差共分散外挿計算工程S532で誤差共分散外挿計算部143が外挿した誤差共分散行列P−を観測更新し、観測更新した誤差共分散行列P+を算出する。誤差共分散記憶部146は、誤差共分散観測更新計算部145が観測更新した誤差共分散行列P+を、誤差共分散行列Pとして記憶する。
状態量観測更新計算工程S555において、状態量観測更新計算部148は、カルマンゲイン計算工程S553でカルマンゲイン計算部144が計算したカルマンゲインKに基づいて、状態量外挿計算工程S533で状態量外挿計算部147が外挿した状態量x−を観測更新し、観測更新した状態量x+を算出する。状態量記憶部149は、状態量観測更新計算部148が観測更新した状態量x+を、状態量xとして記憶する。
なお、GPS受信機113が移動体800の位置や速度を計測していない場合、観測更新処理S550を実行しないので、誤差共分散記憶部146は、誤差共分散外挿計算工程S532で暫定的に記憶した誤差共分散行列P(すなわち、誤差共分散外挿計算部143が外挿した誤差共分散行列P−)をそのまま記憶し続け、状態量記憶部149は、状態量外挿計算工程S533で暫定的に記憶した状態量x(すなわち、状態量外挿計算部147が外挿した状態量x−)をそのまま記憶し続けることとなる。
観測行列計算工程S552において、観測行列計算部141は、速度増分計算工程S525で速度増分計算部126が算出した速度の増分Δvnに基づいて、観測行列Hを算出する。
カルマンゲイン計算工程S553において、カルマンゲイン計算部144は、誤差共分散外挿計算工程S532で誤差共分散外挿計算部143が外挿した誤差共分散行列P−と、観測行列計算工程S552で観測行列計算部141が算出した観測行列Hとに基づいて、カルマンゲインKを算出する。
誤差共分散観測更新計算工程S554において、誤差共分散観測更新計算部145は、観測行列計算工程S552で観測行列計算部141が算出した観測行列Hと、カルマンゲイン計算工程S553でカルマンゲイン計算部144が計算したカルマンゲインKとに基づいて、誤差共分散外挿計算工程S532で誤差共分散外挿計算部143が外挿した誤差共分散行列P−を観測更新し、観測更新した誤差共分散行列P+を算出する。誤差共分散記憶部146は、誤差共分散観測更新計算部145が観測更新した誤差共分散行列P+を、誤差共分散行列Pとして記憶する。
状態量観測更新計算工程S555において、状態量観測更新計算部148は、カルマンゲイン計算工程S553でカルマンゲイン計算部144が計算したカルマンゲインKに基づいて、状態量外挿計算工程S533で状態量外挿計算部147が外挿した状態量x−を観測更新し、観測更新した状態量x+を算出する。状態量記憶部149は、状態量観測更新計算部148が観測更新した状態量x+を、状態量xとして記憶する。
なお、GPS受信機113が移動体800の位置や速度を計測していない場合、観測更新処理S550を実行しないので、誤差共分散記憶部146は、誤差共分散外挿計算工程S532で暫定的に記憶した誤差共分散行列P(すなわち、誤差共分散外挿計算部143が外挿した誤差共分散行列P−)をそのまま記憶し続け、状態量記憶部149は、状態量外挿計算工程S533で暫定的に記憶した状態量x(すなわち、状態量外挿計算部147が外挿した状態量x−)をそのまま記憶し続けることとなる。
ゲイン算出工程S561において、ゲイン算出部151は、GPS受信機113が推定した計測の精度に基づいて、速度誤差のゲインKvを算出する。
速度誤差推定工程S562において、速度誤差推定部152は、速度減算工程S551で速度減算部130が算出した速度の差dvと、ゲイン算出工程S561でゲイン算出部151が算出した速度誤差のゲインKvとに基づいて、速度の誤差δvを算出する。
速度誤差補正工程S563において、速度誤差補正部128は、速度誤差推定工程S562で速度誤差推定部152が算出した速度の誤差δvに基づいて、速度外挿計算工程S526で速度外挿計算部127が外挿した速度vnを補正し、補正した速度vn+を算出する。速度記憶部129は、速度誤差補正部128が補正した速度vn+を、速度vとして記憶する。
速度誤差推定工程S562において、速度誤差推定部152は、速度減算工程S551で速度減算部130が算出した速度の差dvと、ゲイン算出工程S561でゲイン算出部151が算出した速度誤差のゲインKvとに基づいて、速度の誤差δvを算出する。
速度誤差補正工程S563において、速度誤差補正部128は、速度誤差推定工程S562で速度誤差推定部152が算出した速度の誤差δvに基づいて、速度外挿計算工程S526で速度外挿計算部127が外挿した速度vnを補正し、補正した速度vn+を算出する。速度記憶部129は、速度誤差補正部128が補正した速度vn+を、速度vとして記憶する。
慣性航法部120が推定した姿勢角qが誤差εを含んでいる場合、その誤差εは、方向余弦行列Cn bを介して、速度増分計算部126が座標変換した加速度に伝播し、速度増分計算部126が算出する速度の増分Δvnに反映する。誤差εが反映された速度の増分Δvnに基づいて観測行列計算部141が算出した観測行列Hを用いて、誤差εと、観測量Δz(速度の差dv)とを結びつけることにより、カルマンフィルタを用いて、状態量x(バイアス誤差bg及び誤差ε)を推定することができる。
ここで、加速度センサ112が計測する加速度abの誤差が大きい場合には、その誤差も、速度増分計算部126が算出する速度の増分Δvnに反映する。そのため、例えば、位置の誤差、速度の誤差、姿勢角の誤差、角速度センサの誤差、加速度センサの誤差を状態量とし、角速度センサや加速度センサに基づいて推定した位置及び速度と、GPS受信機が計測した位置及び速度との差を観測量とする必要が生じる。
これに対し、加速度センサ112が計測する加速度abの誤差が無視できるほど小さい場合には、バイアス誤差bgと誤差εとのみを状態量としても、精度よく推定をすることができる。
これに対し、加速度センサ112が計測する加速度abの誤差が無視できるほど小さい場合には、バイアス誤差bgと誤差εとのみを状態量としても、精度よく推定をすることができる。
航法装置100の製造コストを低く抑えるため、角速度センサ111や加速度センサ112として、安価なセンサを使用する場合、角速度センサは、バイアス誤差が比較的大きいが、加速度センサには、バイアス誤差が比較的小さく、適切な補償をすることにより、バイアス誤差を無視できるものが存在する。
カルマンフィルタを用いた推定において、状態量ベクトルの次数は、推定に必要な計算量に大きな影響を与える。いま、状態量ベクトルの次数をn、プロセスノイズ行列の次数をm、観測量ベクトルの次数をrとすると、カルマンフィルタによる1回の観測更新における乗算の回数は、以下の式で与えられる。
なお、加算の計算負荷は、乗算の計算負荷に比べてはるかに小さいので、推定に必要な計算量は、乗算の回数により見積もることができる。
この実施の形態における航法装置100において、n=m=6、r=3であるから、Sm=786である。
比較例として、位置の誤差(三次)、速度の誤差(三次)、姿勢角の誤差ε(三次)、角速度センサの誤差bg(三次)、加速度センサの誤差ba(三次)を状態量(15次)とし、位置の差(三次)、速度の差(三次)を観測量(六次)とした場合を考えると、n=m=15、r=6であるから、Sm=9855である。
このように、この実施の形態における航法装置100は、比較例に対して、乗算の回数を10分の1以下に抑えることができる。
比較例として、位置の誤差(三次)、速度の誤差(三次)、姿勢角の誤差ε(三次)、角速度センサの誤差bg(三次)、加速度センサの誤差ba(三次)を状態量(15次)とし、位置の差(三次)、速度の差(三次)を観測量(六次)とした場合を考えると、n=m=15、r=6であるから、Sm=9855である。
このように、この実施の形態における航法装置100は、比較例に対して、乗算の回数を10分の1以下に抑えることができる。
GPS受信機113が移動体800の位置や速度を計測する頻度は、1秒間に1〜4回程度であるから、実時間で航法処理をするには、1秒間に1〜4回の観測更新処理をする必要がある。
この実施の形態における航法装置100は、1回の処理に必要な計算量が少ないので、比較的処理能力の低いマイクロコンピュータなどを使用して、慣性航法部120や第一推定部140などを実現しても、実時間で処理を行うことができる。
このため、航法装置100の製造コストを抑えることができる。
このため、航法装置100の製造コストを抑えることができる。
この実施の形態における航法装置100の構成により、推定の精度が劣化しないことを確認するため、以下のような実験を行った。
(1)シミュレーションによる実験
実機での実験に先立ち、以下の条件のもと、シミュレーションによる模擬実験を行った。
角速度センサ111の誤差モデル:ノイズ誤差0.054度/秒/√Hz、バイアス誤差0.19度/秒/√h、スケールファクタ1.0%、初期化誤差0.1%。
加速度センサ112の誤差モデル:ノイズ誤差41.7μg/√Hz、バイアス誤差0.028m/s2/√h、スケールファクタ1.0%、初期化誤差0.1%。
GPSデータ:2007年7月20日のデータ。平均利用可能衛星数7。
実機での実験に先立ち、以下の条件のもと、シミュレーションによる模擬実験を行った。
角速度センサ111の誤差モデル:ノイズ誤差0.054度/秒/√Hz、バイアス誤差0.19度/秒/√h、スケールファクタ1.0%、初期化誤差0.1%。
加速度センサ112の誤差モデル:ノイズ誤差41.7μg/√Hz、バイアス誤差0.028m/s2/√h、スケールファクタ1.0%、初期化誤差0.1%。
GPSデータ:2007年7月20日のデータ。平均利用可能衛星数7。
比較対象として、位置誤差、速度誤差、姿勢角誤差、角速度バイアス誤差、加速度バイアス誤差を状態量(15次)とする従来の疎結合GPS/INS(比較例1)、更に、状態量にクロックバイアス誤差とクロックドリフト誤差を加えた(17次)従来の密結合GPS/INS(比較例2)についても、同じ条件でシミュレーションを行った。
その結果、この実施の形態における航法装置100における姿勢角の推定誤差の二乗平均平方根(RMS)は、ロール方向で0.31度、ピッチ方向で0.42度、ヨー方向で1.55度であった。
これに対し、比較例1は、ロール方向で0.33度、ピッチ方向で0.41度、ヨー方向で1.34度であった。比較例2は、ロール方向で0.28度、ピッチ方向で0.37度、ヨー方向で1.22度であった。
これに対し、比較例1は、ロール方向で0.33度、ピッチ方向で0.41度、ヨー方向で1.34度であった。比較例2は、ロール方向で0.28度、ピッチ方向で0.37度、ヨー方向で1.22度であった。
この模擬実験の結果によれば、この実施の形態における航法装置100の構成による推定の精度は、従来の構成とほとんど変わらない。
(2)実機による実験
模擬実験により、一応の確認が取れたので、更に、以下の条件のもと、実機による実験を行った。
センサ:自動車用低価格MEMS慣性センサ。計測周波数64Hz。
GPS受信機:計測周波数4Hz。
参照データの計測:リングレーザージャイロを用いたGPS/INS(姿勢角の推定誤差の二乗平均平方根0.1度)。
比較対象:模擬実験と同じ。
模擬実験により、一応の確認が取れたので、更に、以下の条件のもと、実機による実験を行った。
センサ:自動車用低価格MEMS慣性センサ。計測周波数64Hz。
GPS受信機:計測周波数4Hz。
参照データの計測:リングレーザージャイロを用いたGPS/INS(姿勢角の推定誤差の二乗平均平方根0.1度)。
比較対象:模擬実験と同じ。
実験の結果、この実施の形態における航法装置100における姿勢角の推定誤差の二乗平均平方根は、ロール方向で0.24度、ピッチ方向で0.17度、ヨー方向で1.41度であった。
これに対し、比較例1は、ロール方向で0.25度、ピッチ方向で0.20度、ヨー方向で1.78度であった。比較例2は、ロール方向で0.24度、ピッチ方向で0.25度、ヨー方向で1.31度であった。
これに対し、比較例1は、ロール方向で0.25度、ピッチ方向で0.20度、ヨー方向で1.78度であった。比較例2は、ロール方向で0.24度、ピッチ方向で0.25度、ヨー方向で1.31度であった。
この実機による実験の結果によれば、この実施の形態における航法装置100の構成による推定の精度は、従来の構成とほとんど変わらない。
以上のように、この実施の形態における航法装置100によれば、推定の精度を劣化させることなく、カルマンフィルタの計算量を減らすことができる。
この実施の形態における航法装置100は、移動体800に搭載される航法装置である。
上記航法装置100は、角速度計測部(角速度センサ111)と、加速度計測部(加速度センサ112)と、速度計測部(GPS受信機113)と、姿勢角外挿計算部122と、速度増分計算部126と、速度外挿計算部127と、速度減算部130と、第一推定部140とを有する。
上記角速度計測部(角速度センサ111)は、上記移動体800を基準とした第一座標系(移動体座標系)における上記移動体800の角速度ωbを計測する。
上記加速度計測部(加速度センサ112)は、上記第一座標系における上記移動体800に加わる加速度abを計測する。
上記速度計測部(GPS受信機113)は、上記第一座標系と異なる第二座標系(慣性座標系)における上記移動体800の速度vgを計測する。
上記姿勢角外挿計算部122は、上記角速度計測部(角速度センサ111)が計測した角速度ωbに基づいて、上記第二座標系における上記移動体800の姿勢角qnを計算する。
上記速度増分計算部126は、上記加速度計測部(加速度センサ112)が計測した加速度abと、上記姿勢角外挿計算部122が計算した姿勢角qnとに基づいて、上記第二座標系における上記移動体800の速度の増分Δvnを計算する。
上記速度外挿計算部127は、上記速度増分計算部126が計算した速度の増分Δvnに基づいて、上記第二座標系における上記移動体800の速度vnを計算する。
上記速度減算部130は、上記速度外挿計算部127が計算した速度vnと、上記速度計測部(GPS受信機113)が計測した速度vgとの差dvを計算する。
上記第一推定部140は、上記姿勢角外挿計算部122が計算した姿勢角qnと、上記速度増分計算部126が計算した速度の増分Δvnと、上記速度減算部130が計算した速度の差dvとに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度の誤差bgと、上記姿勢角外挿計算部122が計算した姿勢角の誤差εとを推定する。
上記航法装置100は、角速度計測部(角速度センサ111)と、加速度計測部(加速度センサ112)と、速度計測部(GPS受信機113)と、姿勢角外挿計算部122と、速度増分計算部126と、速度外挿計算部127と、速度減算部130と、第一推定部140とを有する。
上記角速度計測部(角速度センサ111)は、上記移動体800を基準とした第一座標系(移動体座標系)における上記移動体800の角速度ωbを計測する。
上記加速度計測部(加速度センサ112)は、上記第一座標系における上記移動体800に加わる加速度abを計測する。
上記速度計測部(GPS受信機113)は、上記第一座標系と異なる第二座標系(慣性座標系)における上記移動体800の速度vgを計測する。
上記姿勢角外挿計算部122は、上記角速度計測部(角速度センサ111)が計測した角速度ωbに基づいて、上記第二座標系における上記移動体800の姿勢角qnを計算する。
上記速度増分計算部126は、上記加速度計測部(加速度センサ112)が計測した加速度abと、上記姿勢角外挿計算部122が計算した姿勢角qnとに基づいて、上記第二座標系における上記移動体800の速度の増分Δvnを計算する。
上記速度外挿計算部127は、上記速度増分計算部126が計算した速度の増分Δvnに基づいて、上記第二座標系における上記移動体800の速度vnを計算する。
上記速度減算部130は、上記速度外挿計算部127が計算した速度vnと、上記速度計測部(GPS受信機113)が計測した速度vgとの差dvを計算する。
上記第一推定部140は、上記姿勢角外挿計算部122が計算した姿勢角qnと、上記速度増分計算部126が計算した速度の増分Δvnと、上記速度減算部130が計算した速度の差dvとに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度の誤差bgと、上記姿勢角外挿計算部122が計算した姿勢角の誤差εとを推定する。
この実施の形態における航法装置100によれば、姿勢角外挿計算部122が計算した姿勢角qnと、速度増分計算部126が計算した速度の増分Δvnと、速度減算部130が計算した速度の差dvとに基づいて、第一推定部140が角速度の誤差bgと、姿勢角の誤差εとを推定するので、推定にかかる計算量が少なく、航法装置100の製造コストを低く抑えることができる。
この実施の形態における航法装置100は、更に、角速度誤差補正部121と、姿勢角誤差補正部123とを有する。
上記角速度誤差補正部121は、上記第一推定部140が推定した角速度の誤差bgに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度ωbを補正する。
上記姿勢角外挿計算部122は、上記角速度誤差補正部121が補正した角速度ωb+に基づいて、上記第二座標系における上記移動体800の姿勢角qnを計算する。
上記姿勢角誤差補正部123は、上記第一推定部140が推定した姿勢角の誤差εに基づいて、上記姿勢角外挿計算部122が計算した姿勢角qnを補正する。
上記速度増分計算部126は、上記加速度計測部(加速度センサ112)が計測した加速度abと、上記姿勢角誤差補正部123が補正した姿勢角qn+とに基づいて、上記第二座標系における上記移動体800の速度の増分Δvnを計算する。
上記第一推定部140は、上記姿勢角誤差補正部123が補正した姿勢角qn+と、上記速度増分計算部126が計算した速度の増分Δvnと、上記速度減算部130が計算した速度の差dvとに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度の誤差bgと、上記姿勢角外挿計算部122が計算した姿勢角の誤差εとを推定する。
上記角速度誤差補正部121は、上記第一推定部140が推定した角速度の誤差bgに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度ωbを補正する。
上記姿勢角外挿計算部122は、上記角速度誤差補正部121が補正した角速度ωb+に基づいて、上記第二座標系における上記移動体800の姿勢角qnを計算する。
上記姿勢角誤差補正部123は、上記第一推定部140が推定した姿勢角の誤差εに基づいて、上記姿勢角外挿計算部122が計算した姿勢角qnを補正する。
上記速度増分計算部126は、上記加速度計測部(加速度センサ112)が計測した加速度abと、上記姿勢角誤差補正部123が補正した姿勢角qn+とに基づいて、上記第二座標系における上記移動体800の速度の増分Δvnを計算する。
上記第一推定部140は、上記姿勢角誤差補正部123が補正した姿勢角qn+と、上記速度増分計算部126が計算した速度の増分Δvnと、上記速度減算部130が計算した速度の差dvとに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度の誤差bgと、上記姿勢角外挿計算部122が計算した姿勢角の誤差εとを推定する。
この実施の形態における航法装置100によれば、第一推定部140が推定した角速度の誤差bgと姿勢角の誤差εとに基づいて、角速度計測部(角速度センサ111)が計測した角速度ωbを角速度誤差補正部121が補正し、姿勢角外挿計算部122が計算した姿勢角qnを姿勢角誤差補正部123が補正するので、移動体800の状態を精度よく推定することができる。
この実施の形態における航法装置100において、上記第一推定部140は、上記角速度計測部(角速度センサ111)が計測した角速度の誤差bgと、上記姿勢角外挿計算部122が計算した姿勢角の誤差εとを状態量とするカルマンフィルタにより、上記状態量を推定する。
この実施の形態における航法装置100によれば、第一推定部140がカルマンフィルタにより状態量を推定するので、状態量の成分が少ないことにより計算量が大幅に削減できる。
この実施の形態における航法装置100において、上記第一推定部140は、以下の式で表される観測量ベクトルΔzと、状態量ベクトルxと、推移行列Φと、観測行列Hとを用いたカルマンフィルタにより上記状態量を推定する。
ただし、dvは、上記速度減算部130が計算した速度の差を表わす三次の縦ベクトル。εは、上記第一推定部140が推定する角速度の誤差を表わす三次の縦ベクトル。bgは、上記第一推定部140が推定する姿勢角の誤差を表わす三次の縦ベクトル。I3×3は、三行三列の単位行列。03×3は、三行三列の零行列。Tsは、上記速度計測部(GPS受信機113)が速度を計測する周期を表わすスカラー値。Cn bは、上記姿勢角外挿計算部122が計算した姿勢角qnに基づいて、上記第一座標系を、上記第二座標系へ変換する座標変換を表わす三行三列の方向余弦行列。Δvn=[Δvn x Δvn y Δvn z]は、上記速度増分計算部126が計算した速度の増分を表わす三次のベクトル。
この実施の形態における航法装置100によれば、加速度計測部(加速度センサ112)が計測した第一座標系(移動体座標系)における加速度abを、姿勢角外挿計算部122が計算した姿勢角qnに基づいて、速度増分計算部126が座標変換し、座標変換した加速度に基づいて速度の増分Δvnを算出し、算出した速度の増分Δvnの成分を、観測行列Hの成分とするので、推定すべき状態量である姿勢角の誤差εと、観測量である速度の差dvとを結びつけることができ、速度の誤差などを状態量として推定する必要がないので、カルマンフィルタで使用する各行列の次数を小さくすることができ、カルマンフィルタの計算に必要な計算量を少なくすることができる。、
この実施の形態における航法装置100は、更に、速度誤差推定部152と、速度誤差補正部128とを有する。
上記速度誤差推定部152は、上記速度減算部130が計算した速度の差dvに基づいて、上記速度外挿計算部127が計算した速度の誤差δvを推定する。
上記速度誤差補正部128は、上記速度誤差推定部152が推定した誤差δvに基づいて、上記速度外挿計算部127が計算した速度vnを補正する。
上記速度誤差推定部152は、上記速度減算部130が計算した速度の差dvに基づいて、上記速度外挿計算部127が計算した速度の誤差δvを推定する。
上記速度誤差補正部128は、上記速度誤差推定部152が推定した誤差δvに基づいて、上記速度外挿計算部127が計算した速度vnを補正する。
この実施の形態における航法装置100によれば、速度減算部130が計算した速度の差dvに基づいて、速度誤差推定部152が速度の誤差δvを推定するので、カルマンフィルタのような計算量の多い推定方式を用いる必要がなく、推定に必要な計算の計算量を少なくすることができる。
この実施の形態における航法装置100において、上記速度誤差推定部152は、上記速度減算部130が計算した速度の差dvに、所定のゲインKvを乗じ、上記速度外挿計算部127が計算した速度の誤差δvとする。
この実施の形態における航法装置100によれば、速度減算部130が計算した速度の差dvに、速度誤差推定部152が所定のゲインKvを乗じることにより、速度の誤差δvを推定するので、極めて少ない計算量で、速度の誤差δvを推定することができる。
この実施の形態における航法装置100は、更に、速度計測精度算出部(GPS受信機113)と、ゲイン算出部151とを有する。
上記速度計測精度算出部(GPS受信機113)は、上記速度計測部(GPS受信機113)が計測した速度の精度を算出する。
上記ゲイン算出部151は、上記速度計測精度算出部(GPS受信機113)が算出した精度に基づいて、ゲインKvを算出する。
上記速度誤差推定部152は、上記速度減算部130が計算した速度の差dvに、上記ゲイン算出部151が算出したゲインKvを乗じ、上記速度外挿計算部127が計算した速度の誤差δvとする。
上記速度計測精度算出部(GPS受信機113)は、上記速度計測部(GPS受信機113)が計測した速度の精度を算出する。
上記ゲイン算出部151は、上記速度計測精度算出部(GPS受信機113)が算出した精度に基づいて、ゲインKvを算出する。
上記速度誤差推定部152は、上記速度減算部130が計算した速度の差dvに、上記ゲイン算出部151が算出したゲインKvを乗じ、上記速度外挿計算部127が計算した速度の誤差δvとする。
この実施の形態における航法装置100によれば、速度計測精度算出部(GPS受信機113)が算出した精度に基づいて、ゲイン算出部151がゲインKvを算出するので、速度計測部(GPS受信機113)の計測精度に応じて、ゲインKvを調整することができ、速度の誤差δvの推定の妥当性を高くすることができる。
この実施の形態における航法装置100が、推定をする推定方法は、以下の工程を有する。
上記角速度計測部(角速度センサ111)が、上記航法装置100が搭載された移動体800を基準とした第一座標系(移動体座標系)における上記移動体800の角速度ωbを計測する。
上記加速度計測部(加速度センサ112)が、上記第一座標系における上記移動体800に加わる加速度abを計測する。
上記速度計測部(GPS受信機113)が、上記第一座標系と異なる第二座標系(慣性座標系)における上記移動体800の速度vgを計測する。
上記姿勢角外挿計算部122が、上記角速度計測部(角速度センサ111)が計測した角速度ωbに基づいて、上記第二座標系における上記移動体800の姿勢角qnを計算する。
上記速度増分計算部126が、上記加速度計測部(加速度センサ112)が計測した加速度abと、上記姿勢角外挿計算部122が計算した姿勢角qnとに基づいて、上記第二座標系における上記移動体800の速度の増分Δvnを計算する。
上記速度外挿計算部127が、上記速度増分計算部126が計算した速度の増分Δvnに基づいて、上記第二座標系における上記移動体800の速度vnを計算する。
上記速度減算部130が、上記速度外挿計算部127が計算した速度vnと、上記速度計測部(GPS受信機113)が計測した速度vgとの差dvを計算する。
上記第一推定部140が、上記姿勢角外挿計算部122が計算した姿勢角qnと、上記速度増分計算部126が計算した速度の増分Δvnと、上記速度減算部130が計算した速度の差dvとに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度の誤差bgと、上記姿勢角外挿計算部122が計算した姿勢角の誤差εとを推定する。
上記角速度計測部(角速度センサ111)が、上記航法装置100が搭載された移動体800を基準とした第一座標系(移動体座標系)における上記移動体800の角速度ωbを計測する。
上記加速度計測部(加速度センサ112)が、上記第一座標系における上記移動体800に加わる加速度abを計測する。
上記速度計測部(GPS受信機113)が、上記第一座標系と異なる第二座標系(慣性座標系)における上記移動体800の速度vgを計測する。
上記姿勢角外挿計算部122が、上記角速度計測部(角速度センサ111)が計測した角速度ωbに基づいて、上記第二座標系における上記移動体800の姿勢角qnを計算する。
上記速度増分計算部126が、上記加速度計測部(加速度センサ112)が計測した加速度abと、上記姿勢角外挿計算部122が計算した姿勢角qnとに基づいて、上記第二座標系における上記移動体800の速度の増分Δvnを計算する。
上記速度外挿計算部127が、上記速度増分計算部126が計算した速度の増分Δvnに基づいて、上記第二座標系における上記移動体800の速度vnを計算する。
上記速度減算部130が、上記速度外挿計算部127が計算した速度vnと、上記速度計測部(GPS受信機113)が計測した速度vgとの差dvを計算する。
上記第一推定部140が、上記姿勢角外挿計算部122が計算した姿勢角qnと、上記速度増分計算部126が計算した速度の増分Δvnと、上記速度減算部130が計算した速度の差dvとに基づいて、上記角速度計測部(角速度センサ111)が計測した角速度の誤差bgと、上記姿勢角外挿計算部122が計算した姿勢角の誤差εとを推定する。
この実施の形態における推定方法によれば、姿勢角外挿計算部122が計算した姿勢角qnと、速度増分計算部126が計算した速度の増分Δvnと、速度減算部130が計算した速度の差dvとに基づいて、第一推定部140が角速度の誤差bgと、姿勢角の誤差εとを推定するので、推定にかかる計算量を少なくすることができる。
以上説明した航法装置100は、航空機等の加速度運動を伴う移動体800に搭載され、移動体800の三次元姿勢角q及び速度vの推定を行う。航法装置100は、GPS速度出力vgと慣性航法計算に基づく速度vnとの差dvを観測量Δzとし、姿勢角誤差εおよび角速度バイアス値bgを推定・補正する推定フィルタ(第一推定部140)と、同じく観測量(速度の差dv)に基づき、速度誤差δvを推定・補正する第2の推定フィルタ(第二推定部150)により、推定フィルタを構成する。これにより、従来方式に比べて著しく推定精度を劣化させることなく、計算負荷を低減することができる。
航法装置100は、カルマンフィルタにおける行列計算において、行列の次数を少なくすることにより、計算負荷を低くし、廉価な計算機により航法装置100を構成できる。これにより、例えば、組込製品等で使用される廉価なマイクロコントローラに実装することができる。
IMU(Inertial Measurement Unit)(角速度計測部・加速度計測部)は、角速度センサと加速度センサをそれぞれ3軸搭載する。IMUは、直交座標系として定義された移動体の機体座標系(第一座標系)における角速度ベクトルωb、加速度ベクトルabを計測し、慣性航法部120に出力する。
慣性航法部120は、適当なアライメント処理およびGPS等により、速度、姿勢角の初期化を行う。その後、慣性航法部120は、ストラップダウン航法計算(外挿計算処理)に基づき、速度vn、姿勢角qnの更新を行い、出力する。なお、姿勢角の表現形式として、コータニオンに基づく表現形式を例として用いたが、オイラー角、方向余弦行列等、他の表現形式を用いる構成であってもよい。また、ストラップダウン航法計算の計算方式は、上述した計算式によるものに限らず、他の計算方式を用いる構成であってもよい。
慣性航法部120は、適当なアライメント処理およびGPS等により、速度、姿勢角の初期化を行う。その後、慣性航法部120は、ストラップダウン航法計算(外挿計算処理)に基づき、速度vn、姿勢角qnの更新を行い、出力する。なお、姿勢角の表現形式として、コータニオンに基づく表現形式を例として用いたが、オイラー角、方向余弦行列等、他の表現形式を用いる構成であってもよい。また、ストラップダウン航法計算の計算方式は、上述した計算式によるものに限らず、他の計算方式を用いる構成であってもよい。
慣性航法部120が計算する速度vn、姿勢角qnは、それぞれの初期化誤差や、IMUが計測した角速度ベクトルωb、加速度ベクトルabに含まれるバイアス誤差等の要因により、誤差を発生する。IMUの計測誤差は積分され、速度や姿勢角の誤差は、時間と共に増加する傾向がある。
GPS受信機113は、GPS測位に基づく速度vgを出力する。減算器(速度減算部130)は、GPS受信機113の速度出力vgから慣性航法部120の速度出力vnを差し引いた差dvを計算する。
第一推定部140は、速度差dvと速度出力vn、姿勢角出力qnから6次元のカルマンフィルタ計算に基づき姿勢角qnの補正εおよび角速度バイアス補正量bgを推定する。
第二推定部150は、速度差dvと航法計算部(慣性航法部120)の速度出力vnから速度出力vnの誤差を算出し、速度誤差補正部128は、速度出力vnの補正を行う。
第一推定部140は、速度差dvと速度出力vn、姿勢角出力qnから6次元のカルマンフィルタ計算に基づき姿勢角qnの補正εおよび角速度バイアス補正量bgを推定する。
第二推定部150は、速度差dvと航法計算部(慣性航法部120)の速度出力vnから速度出力vnの誤差を算出し、速度誤差補正部128は、速度出力vnの補正を行う。
第一推定部140におけるカルマンフィルタ計算は、状態量として、位置pn、速度vn、加速度バイアス補正量baを含んでいない。また、観測量として、位置の差分dpを含んでいない。
航法装置100は、従来よりも状態量の数を少なくしているが、推定精度の劣化は小さく、実用的に問題がない精度を確保することができる。なぜなら、姿勢角誤差εを推定する基本的な原理は不変であり、廉価な加速度センサにおいても比較的バイアス安定度は高く、姿勢角決定精度は角速度センサの性能に基づくため、加速度センサのバイアス補正量baを省略し、温度補正のみを行った場合でも、性能の劣化を小さく抑えることができるからである。
航法装置100は、従来よりも状態量の数を少なくしているが、推定精度の劣化は小さく、実用的に問題がない精度を確保することができる。なぜなら、姿勢角誤差εを推定する基本的な原理は不変であり、廉価な加速度センサにおいても比較的バイアス安定度は高く、姿勢角決定精度は角速度センサの性能に基づくため、加速度センサのバイアス補正量baを省略し、温度補正のみを行った場合でも、性能の劣化を小さく抑えることができるからである。
第二推定部150は、速度の誤差δvを推定し、速度誤差補正部128が速度vnを補正する処理を行う。この計算において、第二推定部150は、ゲインKvを、例えば、固定、または、GPS測位条件に基づいてテーブルにより設定する。これにより、第二推定部150における計算負荷は、第一推定部140における計算負荷と比べて無視できるレベルとすることができる。
カルマンフィルタにおける計算負荷は、状態量の次元nと観測量の次元rに依存する。例えば、状態量の次元nを5分の2(15→6)、観測量の次元rを2分の1(6→3)とすれば、1ステップあたりの乗算の数は、10%以下と著しく減少する。このため、実装において、性能が比較的低いマイクロコンピュータを使用することができ、廉価なシステムを構成することができる。
角速度計測部(角速度センサ111)が計測した角速度ωbの誤差は、外挿計算により蓄積し、姿勢角外挿計算部122が算出する姿勢角qnの誤差に反映する。姿勢角外挿計算部122が算出する姿勢角qnの誤差は、外挿計算により蓄積し、速度外挿計算部127が算出する速度vnの誤差に反映する。
したがって、角速度ωbの誤差が姿勢角qnの誤差に反映するのは比較的速いのに対し、姿勢角qnを介する分、角速度ωbの誤差が速度vnの誤差に反映するのは比較的遅い。このように、誤差の反映する速さが異なる観測量に基づいて、一つのカルマンフィルタで推測しようとすることは、効率が悪い。
以上説明した航法装置100は、誤差が反映するのが比較的速い部分に絞って、カルマンフィルタによる推測を行うことにより、効率を高くし、必要な計算量を少なくする。
したがって、角速度ωbの誤差が姿勢角qnの誤差に反映するのは比較的速いのに対し、姿勢角qnを介する分、角速度ωbの誤差が速度vnの誤差に反映するのは比較的遅い。このように、誤差の反映する速さが異なる観測量に基づいて、一つのカルマンフィルタで推測しようとすることは、効率が悪い。
以上説明した航法装置100は、誤差が反映するのが比較的速い部分に絞って、カルマンフィルタによる推測を行うことにより、効率を高くし、必要な計算量を少なくする。
実施の形態2.
実施の形態2について、図9〜図14を用いて説明する。
なお、実施の形態1で説明した部分と共通する部分については、同一の符号を付し、説明を省略する場合がある。
実施の形態2について、図9〜図14を用いて説明する。
なお、実施の形態1で説明した部分と共通する部分については、同一の符号を付し、説明を省略する場合がある。
図9は、この実施の形態における航法装置100の機能ブロックの構成の一例を示すブロック構成図である。
航法装置100は、実施の形態1で説明した機能ブロックに加えて、更に、位置減算部160を有する。
航法装置100は、実施の形態1で説明した機能ブロックに加えて、更に、位置減算部160を有する。
慣性航法部120は、更に、推定した速度vnに基づいて、慣性座標系における移動体800の位置pnを推定する。
位置減算部160は、GPS受信機113が計測した位置pgと、慣性航法部120が推定した位置pnとの差dp=pg−pnを計算する。ここで、位置pg及び位置pnは、ともに三次の縦ベクトルであるから、位置減算部160が計算する位置の差dpも三次の縦ベクトルである。位置減算部160は、計算した速度の差dpを表わすデータを出力する。
位置減算部160は、GPS受信機113が計測した位置pgと、慣性航法部120が推定した位置pnとの差dp=pg−pnを計算する。ここで、位置pg及び位置pnは、ともに三次の縦ベクトルであるから、位置減算部160が計算する位置の差dpも三次の縦ベクトルである。位置減算部160は、計算した速度の差dpを表わすデータを出力する。
第二推定部150は、更に、位置減算部160が計算した位置の差dpと、GPS受信機113が推定した精度とに基づいて、慣性航法部120が推定した位置pnの誤差δpを推定する。なお、第二推定部150が推定する位置pnの誤差δpは、例えば、慣性座標系のxyz三軸それぞれについての誤差を成分とする三次の縦ベクトルである。第二推定部150は、更に、推定した位置の誤差δpを表わすデータを出力する。
慣性航法部120は、更に、第二推定部150が推定した位置の誤差δpに基づいて、推定した位置pnを補正し、誤差を補正した位置pを算出する。慣性航法部120は、更に、算出した慣性座標系における位置pを表わすデータを出力する。
位置出力部193は、慣性航法部120が出力した位置pを表わすデータを入力する。位置出力部193は、入力したデータが表わす位置pを、外部に出力する。
図10は、この実施の形態における慣性航法部120の機能ブロックの構成の一例を示す詳細ブロック図である。
慣性航法部120は、実施の形態1で説明した機能ブロックに加えて、更に、位置外挿計算部171、位置誤差補正部172、位置記憶部173を有する。
慣性航法部120は、実施の形態1で説明した機能ブロックに加えて、更に、位置外挿計算部171、位置誤差補正部172、位置記憶部173を有する。
位置記憶部173は、慣性座標系における移動体800の位置pを表わすデータを記憶している。位置記憶部173は、例えば、位置pを表わすデータとして、慣性座標系におけるxyz三軸それぞれの座標を表わす三つの実数px,py,pzを表わすデータを記憶する。
位置記憶部173は、あらかじめ、例えば、位置が既知のマーカー上で移動体800が静止しているときに、マーカーの位置を用いて、記憶した位置pを表わすデータを初期化しておく。
位置記憶部173は、あらかじめ、例えば、位置が既知のマーカー上で移動体800が静止しているときに、マーカーの位置を用いて、記憶した位置pを表わすデータを初期化しておく。
位置外挿計算部171は、位置記憶部173が記憶した慣性座標系における位置pを表わすデータと、速度誤差補正部128が出力した補正した速度vn+を表わすデータとを入力する。位置外挿計算部171は、入力したデータが表わす位置pと速度vn+とに基づいて、外挿計算をし、慣性座標系における新しい位置pnを算出する。位置外挿計算部171は、算出した位置pnを表わすデータを出力する。位置外挿計算部171が出力した位置pnを表わすデータは、位置減算部160が入力し、位置の差dpの算出に使用する。
位置外挿計算部171は、例えば、次の計算式を用いて、位置pnを算出する。
ここで、pn kは、位置外挿計算部171が算出する時刻tkにおける位置を表わす三次の縦ベクトル。pk−1は、位置記憶部173が記憶した時刻tk−1における位置を表わす三次の縦ベクトル。Tsは、角速度センサ111及び加速度センサ112が角速度や加速度を計測する周期を表わすスカラー値で、数12や数15や数20におけるTsと同じく、Ts=tk−tk−1。vn+ kは、速度誤差補正部128が補正した時刻kにおける速度を表わす三次の縦ベクトル。
位置外挿計算部171は、例えば、次の計算式を用いて、位置pnを算出する。
位置誤差補正部172は、位置外挿計算部171が出力した慣性座標系における位置pnを表わすデータと、第二推定部150が出力した位置の誤差δpを表わすデータとを入力する。位置誤差補正部172は、入力したデータが表わす位置pnと位置の誤差δpとに基づいて、位置pnの誤差を補正して、補正した位置pn+を算出する。位置誤差補正部172は、算出した位置pn+を表わすデータを出力する。
位置誤差補正部172は、例えば、次の計算式を用いて、補正した位置pn+を算出する。
ここで、pn+ kは、位置誤差補正部172が算出する時刻tkにおける補正した位置を表わす三次の縦ベクトル。pn kは、位置外挿計算部171が算出した時刻tkにおける位置を表わす三次の縦ベクトル。δpは、第二推定部150が推定した位置の誤差を表わす三次の縦ベクトル。
位置誤差補正部172は、例えば、次の計算式を用いて、補正した位置pn+を算出する。
なお、GPS受信機113が移動体800の位置・速度を計測していない場合、位置誤差補正部172は、位置誤差の補正をせず、入力したデータが表わす位置pnを、そのまま位置pn+とする。
位置記憶部173は、位置誤差補正部172が出力した補正した位置pn+を表わすデータを入力する。位置記憶部173は、入力したデータを、位置pを表わすデータとして記憶する。位置記憶部173が記憶した位置pを表わすデータは、位置出力部193が入力して、外部に出力する。
図11は、この実施の形態における第二推定部150の機能ブロックの構成の一例を示す詳細ブロック図である。
第二推定部150は、実施の形態1で説明した機能ブロックに加えて、更に、位置誤差推定部153を有する。
第二推定部150は、実施の形態1で説明した機能ブロックに加えて、更に、位置誤差推定部153を有する。
ゲイン算出部151は、更に、GPS受信機113が推定した位置の計測の精度に基づいて、位置誤差のゲインKpを算出する。ゲイン算出部151は、算出したゲインKpを表わすデータを出力する。
ゲイン算出部151が算出するゲインKpは、ゲインKvと同様の値であり、ゲインKvと同じであってもよいし、異なっていてもよい。
ゲイン算出部151が算出するゲインKpは、ゲインKvと同様の値であり、ゲインKvと同じであってもよいし、異なっていてもよい。
位置誤差推定部153は、位置減算部160が出力した位置の差dpを表わすデータと、ゲイン算出部151が出力したゲインKpを表わすデータとを入力する。位置誤差推定部153は、入力したデータが表わす位置の差dpとゲインKpとに基づいて、慣性航法部120が推定した位置の誤差δpを算出する。位置誤差推定部153は、算出した位置の誤差δpを表わすデータを出力する。位置誤差推定部153が出力した位置の誤差δpを表わすデータは、慣性航法部120が入力し、誤差の補正に使用する。
位置誤差推定部153は、例えば、次の計算式を用いて、位置の誤差δpを算出する。
ここで、δpは、位置誤差推定部153が算出する位置の誤差を表わす三次の縦ベクトル。Kpは、ゲイン算出部151が算出した位置誤差のゲインを表わすスカラー値もしくは三行三列の行列。dpは、位置減算部160が算出した位置の差を表わす三次の縦ベクトル。
位置誤差推定部153は、例えば、次の計算式を用いて、位置の誤差δpを算出する。
次に、動作について説明する。
この実施の形態における航法処理の全体的な流れは、実施の形態1で説明したものと同様である。
この実施の形態における航法処理の全体的な流れは、実施の形態1で説明したものと同様である。
図12は、この実施の形態における航法装置100がデータを初期化する初期化処理S510の詳細な流れの一例を示すフローチャート図である。
初期化処理S510は、実施の形態1で説明した工程に加えて、更に、位置初期化工程S513を有する。
初期化処理S510は、実施の形態1で説明した工程に加えて、更に、位置初期化工程S513を有する。
位置初期化工程S513において、位置記憶部173は、記憶した位置pを初期化する。
図13は、この実施の形態における航法装置100が外挿計算をする外挿計算処理S520の詳細な流れの一例を示すフローチャート図である。
外挿計算処理S520は、実施の形態1で説明した工程に加えて、更に、位置外挿計算工程S527を有する。
外挿計算処理S520は、実施の形態1で説明した工程に加えて、更に、位置外挿計算工程S527を有する。
位置外挿計算工程S527において、位置外挿計算部171は、速度外挿計算工程S526で速度外挿計算部127が算出した速度vnに基づいて、位置記憶部173が記憶した位置pを外挿し、外挿した位置pnを算出する。GPS受信機113が移動体800の位置や速度を計測していない場合、位置外挿計算部171が算出した外挿した位置pnが、そのまま位置記憶部173が記憶する位置pとなるので、位置記憶部173は、位置外挿計算部171が外挿した位置pnを、暫定的に、位置pとして記憶する。
図14は、この実施の形態における航法装置100が観測更新をする観測更新処理S550の詳細な流れの一例を示すフローチャート図である。
観測更新処理S550は、実施の形態1で説明した工程に加えて、更に、位置減算工程S564、位置誤差推定工程S565、位置誤差補正工程S566を有する。
観測更新処理S550は、実施の形態1で説明した工程に加えて、更に、位置減算工程S564、位置誤差推定工程S565、位置誤差補正工程S566を有する。
ゲイン算出工程S561において、ゲイン算出部151は、GPS受信機113が推定した計測の精度に基づいて、速度誤差のゲインKvと、位置誤差のゲインKpとを算出する。
位置減算工程S564において、位置減算部160は、GPS受信機113が計測した位置pgと、位置外挿計算工程S527で位置外挿計算部171が外挿した位置pnとに基づいて、位置の差dpを算出する。
位置誤差推定工程S565において、位置誤差推定部153は、ゲイン算出工程S561でゲイン算出部151が算出した位置誤差のゲインKpと、位置減算工程S564で位置減算部160が算出した位置の差dpとに基づいて、位置の誤差δpを算出する。
位置誤差補正工程S566において、位置誤差補正部172は、位置誤差推定工程S565で位置誤差推定部153が算出した位置の誤差δpに基づいて、位置外挿計算工程S527で位置外挿計算部171が外挿した位置pnを補正し、補正した位置pn+を算出する。位置記憶部173は、位置誤差補正部172が補正した位置pn+を、位置pとして記憶する。
なお、GPS受信機113が移動体800の位置や速度を計測していない場合、観測更新処理S550を実行しないので、位置記憶部173は、位置外挿計算工程S527で暫定的に記憶した位置p(すなわち、位置外挿計算部171が外挿した位置pn)を、そのまま記憶し続けることになる。
位置誤差推定工程S565において、位置誤差推定部153は、ゲイン算出工程S561でゲイン算出部151が算出した位置誤差のゲインKpと、位置減算工程S564で位置減算部160が算出した位置の差dpとに基づいて、位置の誤差δpを算出する。
位置誤差補正工程S566において、位置誤差補正部172は、位置誤差推定工程S565で位置誤差推定部153が算出した位置の誤差δpに基づいて、位置外挿計算工程S527で位置外挿計算部171が外挿した位置pnを補正し、補正した位置pn+を算出する。位置記憶部173は、位置誤差補正部172が補正した位置pn+を、位置pとして記憶する。
なお、GPS受信機113が移動体800の位置や速度を計測していない場合、観測更新処理S550を実行しないので、位置記憶部173は、位置外挿計算工程S527で暫定的に記憶した位置p(すなわち、位置外挿計算部171が外挿した位置pn)を、そのまま記憶し続けることになる。
この実施の形態における航法装置100は、更に、位置計測部(GPS受信機113)と、位置外挿計算部171と、位置減算部160と、位置誤差推定部153と、位置誤差補正部172とを有する。
上記位置計測部(GPS受信機113)は、上記第二座標系(慣性座標系)における移動体800の位置pgを計測する。
上記位置外挿計算部171は、上記速度外挿計算部127が計算した速度vnに基づいて、上記第二座標系における移動体800の位置pnを計算する。
上記位置減算部160は、上記位置外挿計算部171が計算した位置pnと、上記位置計測部(GPS受信機113)が計測した位置pgとの差を計算する。
上記位置誤差推定部153は、上記位置減算部160が計算した位置の差dpに基づいて、上記位置外挿計算部171が計算した位置の誤差δpを推定する。
上記位置誤差補正部172は、上記位置誤差推定部153が推定した位置の誤差δpに基づいて、上記位置外挿計算部171が計算した位置pnを補正する。
上記位置計測部(GPS受信機113)は、上記第二座標系(慣性座標系)における移動体800の位置pgを計測する。
上記位置外挿計算部171は、上記速度外挿計算部127が計算した速度vnに基づいて、上記第二座標系における移動体800の位置pnを計算する。
上記位置減算部160は、上記位置外挿計算部171が計算した位置pnと、上記位置計測部(GPS受信機113)が計測した位置pgとの差を計算する。
上記位置誤差推定部153は、上記位置減算部160が計算した位置の差dpに基づいて、上記位置外挿計算部171が計算した位置の誤差δpを推定する。
上記位置誤差補正部172は、上記位置誤差推定部153が推定した位置の誤差δpに基づいて、上記位置外挿計算部171が計算した位置pnを補正する。
この実施の形態における航法装置100によれば、速度外挿計算部127が算出した移動体800の速度vnに基づいて、位置外挿計算部171が移動体800の位置pnを算出するので、なんらかの原因で、位置計測部(GPS受信機113)が移動体800の位置pgを計測できない場合であっても、移動体800の位置を推定することができる。
以上説明した航法装置100は、慣性航法部120において位置外挿計算を実施し、第二推定部150において位置に関する更新計算も行う。航法装置100は、慣性航法部120が、位置外挿計算を実施し、減算器(速度減算部130・位置減算部160)が、位置・速度の差dp、dvを計算し、第二推定部150が、位置に関する更新計算も行う。なお、位置誤差のゲインKpは、速度誤差のゲインKvと同様に固定または簡単な計算により設定することができる。
これにより、GPS出力が一時中断した場合やGPS出力の遅延が大きいことが問題となる場合など、慣性装置に基づく位置出力pnを使用することができる。
これにより、GPS出力が一時中断した場合やGPS出力の遅延が大きいことが問題となる場合など、慣性装置に基づく位置出力pnを使用することができる。
100 航法装置、111 角速度センサ、112 加速度センサ、113 GPS受信機、120 慣性航法部、121 角速度誤差補正部、122 姿勢角外挿計算部、123 姿勢角誤差補正部、124 姿勢角記憶部、125 方向余弦行列計算部、126 速度増分計算部、127 速度外挿計算部、128 速度誤差補正部、129 速度記憶部、130 速度減算部、140 第一推定部、141 観測行列計算部、142 推移行列計算部、143 誤差共分散外挿計算部、144 カルマンゲイン計算部、145 誤差共分散観測更新計算部、146 誤差共分散記憶部、147 状態量外挿計算部、148 状態量観測更新計算部、149 状態量記憶部、150 第二推定部、151 ゲイン算出部、152 速度誤差推定部、153 位置誤差推定部、160 位置減算部、171 位置外挿計算部、172 位置誤差補正部、173 位置記憶部、191 姿勢角出力部、192 速度出力部、193 位置出力部、800 移動体。
Claims (9)
- 移動体に搭載される航法装置において、
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有し、
上記角速度計測部は、上記移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部は、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部は、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部は、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部は、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部は、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部は、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする航法装置。 - 上記航法装置は、更に、角速度誤差補正部と、姿勢角誤差補正部とを有し、
上記角速度誤差補正部は、上記第一推定部が推定した角速度の誤差に基づいて、上記角速度計測部が計測した角速度を補正し、
上記姿勢角外挿計算部は、上記角速度誤差補正部が補正した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記姿勢角誤差補正部は、上記第一推定部が推定した姿勢角の誤差に基づいて、上記姿勢角外挿計算部が計算した姿勢角を補正し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角誤差補正部が補正した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記第一推定部は、上記姿勢角誤差補正部が補正した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする請求項1に記載の航法装置。 - 上記第一推定部は、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを状態量とするカルマンフィルタにより、上記状態量を推定することを特徴とする請求項1または請求項2に記載の航法装置。
- 上記第一推定部は、以下の式で表される観測量ベクトルΔzと、状態量ベクトルxと、推移行列Φと、観測行列Hとを用いたカルマンフィルタにより上記状態量を推定することを特徴とする請求項3に記載の航法装置。
- 上記航法装置は、更に、速度誤差推定部と、速度誤差補正部とを有し、
上記速度誤差推定部は、上記速度減算部が計算した速度の差に基づいて、上記速度外挿計算部が計算した速度の誤差を推定し、
上記速度誤差補正部は、上記速度誤差推定部が推定した誤差に基づいて、上記速度外挿計算部が計算した速度を補正することを特徴とする請求項1乃至請求項4のいずれかに記載の航法装置。 - 上記速度誤差推定部は、上記速度減算部が計算した速度の差に、所定のゲインを乗じ、上記速度外挿計算部が計算した速度の誤差とすることを特徴とする請求項5に記載の航法装置。
- 上記航法装置は、更に、速度計測精度算出部と、ゲイン算出部とを有し、
上記速度計測精度算出部は、上記速度計測部が計測した速度の精度を算出し、
上記ゲイン算出部は、上記速度計測精度算出部が算出した精度に基づいて、ゲインを算出し、
上記速度誤差推定部は、上記速度減算部が計算した速度の差に、上記ゲイン算出部が算出したゲインを乗じ、上記速度外挿計算部が計算した速度の誤差とすることを特徴とする請求項5または請求項6に記載の航法装置。 - 上記航法装置は、更に、位置計測部と、位置外挿計算部と、位置減算部と、位置誤差推定部と、位置誤差補正部とを有し、
上記位置計測部は、上記第二座標系における移動体の位置を計測し、
上記位置外挿計算部は、上記速度外挿計算部が計算した速度に基づいて、上記第二座標系における移動体の位置を計算し、
上記位置減算部は、上記位置外挿計算部が計算した位置と、上記位置計測部が計測した位置との差を計算し、
上記位置誤差推定部は、上記位置減算部が計算した位置の差に基づいて、上記位置外挿計算部が計算した位置の誤差を推定し、
上記位置誤差補正部は、上記位置誤差推定部が推定した位置の誤差に基づいて、上記位置外挿計算部が計算した位置を補正することを特徴とする請求項1乃至請求項7のいずれかに記載の航法装置。 - 角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有する航法装置が、推定をする推定方法において、
上記角速度計測部が、上記航法装置が搭載された移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部が、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部が、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部が、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部が、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部が、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部が、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部が、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする推定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2008268150A JP2010096647A (ja) | 2008-10-17 | 2008-10-17 | 航法装置及び推定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2008268150A JP2010096647A (ja) | 2008-10-17 | 2008-10-17 | 航法装置及び推定方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2010096647A true JP2010096647A (ja) | 2010-04-30 |
Family
ID=42258435
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2008268150A Pending JP2010096647A (ja) | 2008-10-17 | 2008-10-17 | 航法装置及び推定方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2010096647A (ja) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2013170904A (ja) * | 2012-02-20 | 2013-09-02 | Ono Sokki Co Ltd | 計測装置及び計測方法 |
WO2015146048A1 (ja) * | 2014-03-25 | 2015-10-01 | セイコーエプソン株式会社 | 誤差推定方法、運動解析方法、誤差推定装置、及びプログラム |
CN105486305A (zh) * | 2014-09-17 | 2016-04-13 | 上海新跃仪表厂 | 一种估计加速度计漂移的近程相对导航滤波方法 |
CN111027137A (zh) * | 2019-12-05 | 2020-04-17 | 中国人民解放军63620部队 | 基于遥测数据的航天器动力学模型高精度动态构建方法 |
CN113503882A (zh) * | 2021-06-03 | 2021-10-15 | 北京自动化控制设备研究所 | 一种车载惯性/地磁组合导航方法及装置 |
JP2022500617A (ja) * | 2019-10-29 | 2022-01-04 | 広東工業大学Guangdong University Of Technology | IMUに基づくuwb測位異常値処理方法 |
-
2008
- 2008-10-17 JP JP2008268150A patent/JP2010096647A/ja active Pending
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2013170904A (ja) * | 2012-02-20 | 2013-09-02 | Ono Sokki Co Ltd | 計測装置及び計測方法 |
WO2015146048A1 (ja) * | 2014-03-25 | 2015-10-01 | セイコーエプソン株式会社 | 誤差推定方法、運動解析方法、誤差推定装置、及びプログラム |
CN105486305A (zh) * | 2014-09-17 | 2016-04-13 | 上海新跃仪表厂 | 一种估计加速度计漂移的近程相对导航滤波方法 |
JP2022500617A (ja) * | 2019-10-29 | 2022-01-04 | 広東工業大学Guangdong University Of Technology | IMUに基づくuwb測位異常値処理方法 |
JP7055484B2 (ja) | 2019-10-29 | 2022-04-18 | 広東工業大学 | IMUに基づくuwb測位異常値処理方法 |
CN111027137A (zh) * | 2019-12-05 | 2020-04-17 | 中国人民解放军63620部队 | 基于遥测数据的航天器动力学模型高精度动态构建方法 |
CN111027137B (zh) * | 2019-12-05 | 2023-07-14 | 中国人民解放军63620部队 | 基于遥测数据的航天器动力学模型高精度动态构建方法 |
CN113503882A (zh) * | 2021-06-03 | 2021-10-15 | 北京自动化控制设备研究所 | 一种车载惯性/地磁组合导航方法及装置 |
CN113503882B (zh) * | 2021-06-03 | 2023-09-12 | 北京自动化控制设备研究所 | 一种车载惯性/地磁组合导航方法及装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6094026B2 (ja) | 姿勢判定方法、位置算出方法及び姿勢判定装置 | |
JP4199553B2 (ja) | ハイブリッド航法装置 | |
JP5237723B2 (ja) | 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法 | |
CN110702143B (zh) | 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法 | |
CN101608920B (zh) | 一种组合式空间位姿精密动态测量装置及方法 | |
CN111380514A (zh) | 机器人位姿估计方法、装置、终端及计算机存储介质 | |
CN112945271B (zh) | 磁力计信息辅助的mems陀螺仪标定方法及标定系统 | |
KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
CN101949703A (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
JP2010096647A (ja) | 航法装置及び推定方法 | |
CN109931955A (zh) | 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 | |
CN107036598A (zh) | 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
JPH095104A (ja) | 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置 | |
CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
CN113551669B (zh) | 基于短基线的组合导航定位方法及装置 | |
CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
JP4527171B2 (ja) | 単一gpsと慣性データ(加速度,角速度)によるビークル姿勢角の計測方法 | |
CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
JP2012202749A (ja) | 方位測定装置 | |
JP4343581B2 (ja) | 移動体の姿勢検出装置 | |
CN116086488A (zh) | 统一建模原理的极区动基座对准方法、设备及介质 |