JP2024041548A - 自律航法システム - Google Patents

自律航法システム Download PDF

Info

Publication number
JP2024041548A
JP2024041548A JP2022146423A JP2022146423A JP2024041548A JP 2024041548 A JP2024041548 A JP 2024041548A JP 2022146423 A JP2022146423 A JP 2022146423A JP 2022146423 A JP2022146423 A JP 2022146423A JP 2024041548 A JP2024041548 A JP 2024041548A
Authority
JP
Japan
Prior art keywords
yaw angle
angle range
inertial sensor
detection means
yaw
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
Application number
JP2022146423A
Other languages
English (en)
Inventor
裕之 清野
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Alps Alpine Co Ltd
Original Assignee
Alps Electric Co Ltd
Alps Alpine Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Alps Electric Co Ltd, Alps Alpine Co Ltd filed Critical Alps Electric Co Ltd
Priority to JP2022146423A priority Critical patent/JP2024041548A/ja
Priority to EP23195811.7A priority patent/EP4339558A1/en
Priority to US18/367,045 priority patent/US20240085183A1/en
Priority to CN202311181877.0A priority patent/CN117705128A/zh
Publication of JP2024041548A publication Critical patent/JP2024041548A/ja
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

【課題】慣性センサの取付け角のヨー角が大きい場合にも慣性センサの取付け角の推定を良好に行う「自律航法システム」を提供する。を提供する。【解決手段】センサ取付角検出部１３において６軸慣性センサ１１のおおよその取付角のヨー角A3を検出し、センサ出力変換部１５において、６軸慣性センサ１１の座標系のXs,Ys、Zsの３軸の加速度出力（ax,ay,az)と角速度出力（ωx,ωy,ωz)を、検出した取付角A3が属する角度範囲に応じて、ヨー角が±45以内の座標系のXs',Y's、Zsの３軸の加速度（ax',ay',az)と角速度（ωx',ωy',ωz)に変換し、カルマンフィルタ１６に入力する。【選択図】図１

Description

本発明は、カルマンフィルタを用いた自律航法システムに関するものである。
カルマンフィルタを用いた自律航法システムとしては、自動車に搭載された自律航法システムが知られている（たとえば、特許文献１）。
この自律航法システムでは、車速センサの出力と、３軸方向の加速度を検出する加速度センサの出力と、３軸周りの角速度を検出するジャイロの出力と、ＧＰＳ受信機の出力を反復拡張カルマンフィルタで処理して、自動車の三次元位置ベクトル等の状態を推定している。
また、本発明に関する技術として、Xs軸方向の加速度を検出する加速度センサの出力と、Xs軸と垂直なZs軸周りの角速度を検出するジャイロの出力と、ＧＰＳ受信機の出力を用いて、当該加速度センサやジャイロを含む装置の自動車への取付け角を検出する技術が知られている（たとえば、特許文献２）。
ここで、この技術では、自動車の前方方向に向かう軸をXm、自動車の上方方向に向かう軸をZmとして、Xm-Zm座標系に対する、Xs-Zs座標系のピッチ角θpitch、ヨー角θyow、ロール角θrollを、自動車への取付け角として、次のように求めている。
すなわち、車両の進行方向の加速度成分がゼロの時における加速度センサの出力する加速度Asを積算し、平均化させた値Aaveを算出する。そして、式１に示すように、値Aaveを重力加速度gで除算しsin(θpitch)を求め、sin(θpitch)の値よりピッチ角θpitchを得る。
次に、式２に示すように、加速度Asからg×sin(θpitch)を減算し値Asxとする。そして、ＧＰＳ受信機の出力に基づいて自動車のX軸方向の加速度Arを求め、式３に示すように、加速度Arの余弦値Ar×cos(θpitch)を除算することで、cos(θyaw)を求め、cos(θyaw)の値よりヨー角θyawを得る。
また、ＧＰＳ受信機の出力に基づいて自動車のZ軸周りの角速度ωrを求めると共に、式４に示すように、角速度ωrとジャイロの出力する角速度ωsとの比をとることで、ジャイロの感度Sgを求める。そして、式５に示すように、感度Sgをcos(θpitch)で除算することでcos(θroll)を求め、cos(θroll)の値よりロール角θrollを得る。
特許第５１６４６４５号公報 特開２００４-２３９６１３号公報
３軸方向の加速度を検出する加速度センサと３軸周りの角速度を検出するジャイロとを備えた６軸慣性センサと、カルマンフィルタとを用いて自律航法システムを構築する場合、自動車の三次元位置ベクトルの算出に関わる６軸慣性センサの自動車への取付け角も状態として推定する必要がある。
しかし、６軸慣性センサの自動車への取付け角が、初期値として設定した取付け角の標準値から乖離していると、取付け角の推定値が収束するまでに長時間を要したり、取付け角の推定値が誤った値に収束したりすることがあった。
また、６軸慣性センサの自動車への取付け角のヨー角が±90°を超えると、カルマンフィルタの推定能力の許容範囲を超えてしまい、取付け角の推定値が収束せずに発散してしまうことがあった。
そして、このことが、慣性センサとカルマンフィルタを用いた自律航法システムにおいて、三次元位置ベクトル等の自動車の状態を適正に算出することの妨げとなっていた。
そこで、本発明は、移動体に搭載される、慣性センサとカルマンフィルタを用いた自律航法システムにおいて、慣性センサの移動体への取付け角によらずに、移動体の状態を適正に算出することを課題とする。
前記課題達成のために、本発明は、移動体に搭載される自律航法システムに、慣性センサと、前記慣性センサの前記移動体に対する取付角のヨー角を検出するヨー角検出手段と、前記慣性センサの出力を変換し出力する変換手段と、前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを備えたものである。但し、前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、前記変換手段は、前記ヨー角検出手段が検出したヨー角に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力する。
また、前記課題達成のために、本発明は、移動体に搭載される自律航法システムに、慣性センサと、前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲をヨー角角度範囲として検出するヨー角角度範囲検出手段と、前記慣性センサの出力を変換し出力する変換手段と、前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを備えたものである。但し、前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力する。
ここで、この自律航法システムは、前記ヨー角角度範囲検出手段において、-45°と+45°の間の角度範囲、+45°と+135°の間の角度範囲、+135°と+180°の間の角度範囲、-45°と-135°の間の角度範囲、-135°と-180°の間の角度範囲のうちの、前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲を前記ヨー角角度範囲として検出し、前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±４５°以内の仮想の慣性センサの出力に変換し出力するように構成してもよい。
また、このように構成した場合、前記慣性センサを、X、Y、Zの直交する３軸方向の加速度（ax,ay,az)を出力する慣性センサとし、前記変換手段において、前記変換として加速度（ax,ay,az)の加速度（ax',ay',az')への変換を行うと共に、当該加速度（ax',ay',az')への変換を、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、（ax',ay',az')=（ax,ay,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、（ax',ay',az')=（-ay,ax,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、（ax',ay',az')=（-ax,-ay,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、（ax',ay',az')=（ay,-ax,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、（ax',ay',az')=（-ax,-ay,az)とすることにより行うようにしてもよい。
また、このように構成した場合、前記慣性センサを、X、Y、Zの直交する３軸まわりの角速度（ωx,ωy,ωz)を出力する慣性センサとし、前記変換手段において、前記変換として角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うと共に、当該角速度（ωx',ωy',ωz')への変換を、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、（ωx',ωy',ωz')=（ωx,ωy,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、（ωx',ωy',ωz')=（-ωy,ωx,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、（ωx',ωy',ωz')=（-ωx,-ωy,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、（ωx',ωy',ωz')=（ωy,-ωx,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、（ωx',ωy',ωz')=（-ωx,-ωy,ωz)とすることにより行ってもよい。
また、このように構成した場合、前記慣性センサを、加速度（ax,ay,az)と角速度（ωx,ωy,ωz)を出力するものとし、前記変換手段において、前記変換として、上述した加速度（ax,ay,az)の加速度（ax',ay',az')への変換と角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うようにしてもよい。
このように、カルマンフィルタにおいて、変換手段で慣性センサの出力を変換して取付角のヨー角が±９０°以内や±４５°以内の仮想の慣性センサの出力を生成し、慣性センサの出力に代えてカルマンフィルタに入力することにより、慣性センサの移動体への取付け角によらずに、カルマンフィルタにおいて、取付け角の推定が発散してしまうことが抑止されると共に、取付け角の推定を正しい値に速やかに収束できることが期待できる。
ここで、以上のヨー角角度範囲検出手段を備えた自律航法システムは、前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のヨー角を検出するように構成すると共に、当該自律航法システムに、前記ヨー角角度範囲検出手段が検出したヨー角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのヨー角を算定し、前記カルマンフィルタが推定する取付角のヨー角の初期値として当該カルマンフィルタに設定する初期値設定手段を備えてもよい。
また、この場合には、さらに、前記ヨー角角度範囲検出手段において、前記慣性センサの前記移動体に対する取付角のロール角とピッチ角を検出し、前記初期値設定手段において、前記ヨー角角度範囲検出手段が検出したロール角とピッチ角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのピッチ角を算定し、前記カルマンフィルタが推定する取付角のピッチ角の初期値として当該カルマンフィルタに設定してもよい。
また、ヨー角角度範囲検出手段において、-45°と+45°の間の角度範囲、+45°と+135°の間の角度範囲、+135°と+180°の間の角度範囲、-45°と-135°の間の角度範囲、-135°と-180°の間の角度範囲のうちからヨー角角度範囲を検出する自律航法システムは、前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のロール角A1とピッチ角A2とヨー角A3とを検出するように構成すると共に、当該自律航法システムに、前記カルマンフィルタが推定する取付角のピッチ角の初期値SA2とヨー角の初期値SA3を設定する初期値設定手段を備えてもよい。この場合、当該初期値設定手段は、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、SA2=A2、SA3=A3、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、SA2=A1、SA3=A3-90°、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、SA2=-A2、SA3=A3-180°、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、SA2=-A1、SA3=A3+90°、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、SA2=-A2、SA3=A3+180°により初期値SA2とヨー角の初期値SA3を算定する。
これらのように、カルマンフィルタで推定する取り付け角の初期値を設定することにより、６軸慣性センサの取り付け角の推定が、発散することなく速やかに収束することが期待できる。
以上のように、本発明によれば、移動体に搭載される、慣性センサとカルマンフィルタを用いた自律航法システムにおいて、慣性センサの移動体への取付け角によらずに、移動体の状態を適正に算出することができる。
本発明の実施形態に係る自律航法システムの構成を示すブロック図である。 本発明の実施形態においてカルマンフィルタが推定する６軸慣性センサの自動車への取付け角のヨー角を示す図である。 本発明の実施形態における６軸慣性センサの座標系から仮想６軸慣性センサの座標系の変換例を示す図である。
以下、本発明の実施形態について説明する。
図１に、本実施形態に係る自律航法システムの構成を示す。
自律航法システム１は、自動車に搭載された、自動車の位置ベクトル等の自動車の状態の算出するシステムであり、自律航法システム１が算出した自動車の位置ベクトルなどの自動車の状態は、同じく自動車に搭載されている自動運転システムやナビゲーションシステム等である状態利用システム２によって利用される。
図示するように、自律航法システム１は、６軸慣性センサ１１、衛星測位を行うＧＮＳＳ受信機１２、センサ取付角検出部１３、制御部１４、センサ出力変換部１５、カルマンフィルタ１６を備えている。
また、６軸慣性センサ１１は、６軸慣性センサ１１に対して固定された直交座標系であるセンサ座標系のXs、Ys、Zsの３軸方向の各加速度ax、ay、azを検出する加速度センサ１１１と、Xs、Ys、Zsの３軸周りの各角速度をωx、ωy、ωz検出するジャイロ１１２を備えている。
また、カルマンフィルタ１６は、予測ステップ実行部１６１とフィルタリングステップ実行部１６２を備えている。
センサ出力変換部１５は、制御部１４から設定された加速度変換関数Taに従って、加速度センサ１１１が出力する加速度ax、ay、azを、加速度ax'、ay'、az'に変換してカルマンフィルタ１６に出力すると共に、制御部１４から設定された角速度変換関数Tωに従って、ジャイロ１１２が出力する角速度ωx、ωy、ωzを、角速度ωx'、ωy'、ωz'に変換してカルマンフィルタ１６に出力する。
次に、カルマンフィルタ１６は、式６に示す状態方程式と式７に示す観測方程式に則って、各時点kの自動車の状態Xkを推定する。
推定される自動車の状態Xkには、自動車の三次元位置ベクトル（三次元移動距離）、６軸慣性センサ１１の三次元速度ベクトル、６軸慣性センサ１１のロール、ピッチ、ヨーの姿勢角、６軸慣性センサ１１の自動車への取り付け角のピッチ角とヨー角、加速度センサ１１１とジャイロ１１２の感度やバイアス等が含まれる。
式６においてukはセンサ出力変換部１５から入力する加速度ax'、ay'、az'、角速度ωx'、ωy'、ωz'を含む入力ベクトル、qkは入力ベクトルの誤差を表す。また、ｆは、誤差qkが無かったとした場合に、前時点k-1の状態Xk-1と入力ベクトルukから、状態Xkを導出する非線形関数である。
式７において、Zkは、信号処理部を介してカルマンフィルタ１６に入力するＧＮＳＳ受信器の測定値やセンサ出力変換部１５から入力する角速度ωx'、ωy'、ωz'等を含む測定値、rkは測定値の誤差を表す。また、hは、誤差rkが無かったとした場合に、状態Xkから測定値Zkを導出する非線形関数である。
カルマンフィルタ１６の予測ステップ実行部１６１とフィルタリングステップ実行部１６２は、たとえば、以下の処理を行う。
すなわち、カルマンフィルタ１６の予測ステップ実行部１６１は、時点kにおいて、式８に従って状態Xk+1の事前推定値X- k+1を算定する。ここで、式８中の、X+ kは、フィルタリングステップ実行部１６２において前時点kで算定された事後推定値である。
また、予測ステップ実行部１６１は、式９に従って、事前誤差共分散P- k+1を算定する。式９中において、Φk、Γkはそれぞれ、非線形関数fのヤコビアンで定義した行列、ΦT k、ΓT kはそれぞれ、Φk、Γkの転置行列、P+ kは、フィルタリングステップ実行部１６２で前時点kにおいて算定された事後誤差共分散であり、Qkは、誤差qkを表す共分散行列である。
次に、フィルタリングステップ実行部１６２は、時点kにおいて、測定値が得られた場合、式１０、式１１、式１２をiを０から１ずつ進めながら、１回もしくは複数回である所定回数、または、所定の停止条件が成立するまで繰り返すフィルタリングステップを実行する。
式１０は、カルマンゲインKk,iの算出式であり、式中P- kは、前時点k-1において予測ステップで算定された事前誤差共分散である。また、x+ k,0は、前時点k-1において予測ステップで算定された事前推定値x- kとする。
また、Hkは、非線形関数hのヤコビアンで定義した行列、HT kは、Hkの転置行列、Rkは、誤差rkを表す共分散行列である。
次に、式１１は、事後推定値X+ k,i+1を算定する式であり、式１２は事後誤差共分散P+ k,i+1を算定する式であり、式中のIは単位行列である。
そして、フィルタリングステップ実行部１６２は、式１０、式１１、式１２の繰り返しをｎ回で停止したならば、最終的に得られている事後推定値X+ k,ｎと、事後誤差共分散P+ k,nを、事後推定値X+ kと、事後誤差共分散P+ kとする。
そして、事後推定値X+ kを、自動車の状態Xk+1の推定値として、必要な場合には、事後誤差共分散P+ k,nと共に状態利用システム２に出力する。
なお、時点kにおいて、測定値が得られず、フィルタリングステップを実行できなかった場合は、事前推定値X- kを、自動車の状態Xkの推定値として、必要な場合には、事後誤差共分散P- k,nと共に状態利用システム２に出力する。
さて、このようなカルマンフィルタ１６の予測ステップ実行部１６１の処理は、事前推定値X- kの初期値X- 0、事前誤差共分散P- kの初期値P- 0を設定した後の時点k=1から開始される。
また、フィルタリングステップ実行部１６２の処理は、予測ステップ実行部１６１によって、最初の事前推定値x- 1と事前誤差共分散P- 1が算定された時点k=1から開始される。
事前推定値X- kの初期値X- 0に含める６軸慣性センサ１１の自動車への取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3の設定は制御部１４が行う。その他の初期値は、期待値等を設定する。
以下、制御部１４が行う、上述したセンサ出力変換部１５への加速度変換関数Taと角速度変換関数Tωの設定と、カルマンフィルタ１６への事前推定値X- kの初期値X- 0の一部としての、６軸慣性センサ１１の取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3の設定について説明する。
まず、加速度変換関数Taと角速度変換関数Tωの設定について説明する。
センサ取付角検出部１３は、自律航法システム１の初期設定時に、６軸慣性センサ１１の自動車への、おおよその取り付け角（ロール角A1、ピッチ角A2、ヨー角A3）を検出する。
ここで、自動車に対して固定された直交座標系である車両座標系が予め定義されており、車両座標系の３軸は、自動車の前方向に向かうXm軸、自動車の右方向に向かうYm軸、自動車の上方向に向かうZm軸である。
そして、６軸慣性センサ１１の自動車への取付角のロール角A1は、Xm軸回りのセンサ座標系の車両座標系に対する傾きであり、ピッチ角A2はYm軸回りのセンサ座標系の車両座標系に対する傾きであり、ヨー角A3は図２ａ、ｂに示すようにZm軸回りのセンサ座標系の車両座標系に対する傾きである。ここで、図２ａ、ｂにおいて、ヨー角A3は、時計回り方向を正方向とする。
さて、センサ取付角検出部１３の、このような６軸慣性センサ１１の自動車への、おおよそ取り付け角（ロール角A1、ピッチ角A2、ヨー角A3）の検出は、たとえば、上述した特許文献２の技術を適用し、加速度センサ１１１の出力と、ジャイロ１１２の出力と、ＧＮＳＳ受信機１２の出力から前掲した式１-式５によって推定することができる。
ただし、センサ取付角検出部１３の６軸慣性センサ１１の自動車への、おおよそ取り付け角（ロール角A1、ピッチ角A2、ヨー角A3）の検出は、他の手法により行ってもよいし、オペレータから設定されたロール角A1、ピッチ角A2、ヨー角A3を、そのままロール角A1、ピッチ角A2、ヨー角A3として推定するものであってもよい。
いずれの場合も、ロール角A1、ピッチ角A2、ヨー角A3の推定の精度は、最低でも、各角度が、-45°から+45度、+45°から+135°、-45°から-135°、+135°から+180°-135°から-180°のいずれに属するかを判別できる精度とする。
次に、制御部１４は、センサ取付角検出部１３が検出したヨー角A3が属する角度範囲に応じて、センサ出力変換部１５において加速度センサ１１１が出力する加速度ax、ay、azを、加速度ax'、ay'、az'に変換する加速度変換関数Taと、センサ出力変換部１５において、ジャイロ１１２が出力する角速度ωx、ωy、ωzを、角速度ωx'、ωy'、ωz'に変換する角速度変換関数Tωを次のように設定する。
-45°＜A3＜+45°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（ax,ay,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（ωx,ωy,ωz)
+45°≦A3＜+135°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（-ay,ax,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（-ωy,ωx,ωz)
+135°≦A3＜+180°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（-ax,-ay,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（-ωx,-ωy,ωz)
-45°≧A3＞-135°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（ay,-ax,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（ωy,-ωx,ωz)
-135°≧A3≧-180°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（-ax,-ay,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（-ωx,-ωy,ωz)
このように加速度変換関数Taと角速度変換関数Tωを設定することにより、加速度センサ１１１が出力する加速度ax、ay、azは、図５ａ-ｅにヨー角A3が属する角度範囲毎に示すXs'軸方向の加速度ax'とYs'軸方向の加速度を表すay'と、Zs軸方向の加速度を表すaz'に変換され、ジャイロ１１２が出力する角速度ωx、ωy、ωzは、Xs'軸回りの角速度を表すωx'とYs'軸回りの角速度を表すωy'とZs軸回りの角速度を表すωz'とに変換される。
すなわち、６軸慣性センサ１１の出力は、Xs'軸、Ys'軸、Zs軸を直交３軸とする仮想のセンサ座標系を持つ仮想６軸慣性センサの出力に変換される。
また、この仮想６軸慣性センサの自動車への取り付け角のヨー角A3'は、Xs'軸とys'軸が、図示するように、Xs軸とys軸をヨー方向にヨー角A3が属する角度範囲に応じて、
-45°＜A3＜+45°のとき0°、
+45°≦A3＜+135°のとき-90°、
+135°≦A3＜+180°のとき-180°
-45°≧A3＞-135°のとき+90°、
-135°≧A3≧-180°のとき+180°
回転したものとなるので、
-45°＜A3＜+45°のときA3'=A3、
+45°≦A3＜+135°のときA3'=A3-90°、
+135°≦A3＜+180°のときA3'=A3-180°
-45°≧A3＞-135°のときA3'=A3+90°、
-135°≧A3≧-180°のときA3'=A3+180°
となり、ヨー角A3によらずに、ヨー角A3'は図中に灰色で示した±45°の範囲内に収まることとなる。
よって、カルマンフィルタ１６において、６軸慣性センサ１１の出力に代えて、センサ出力変換部１５の変換によって生成した仮想６軸慣性センサの出力を用いることは、自動車への取り付け角のヨー角が±45°の範囲内のA3'である６軸慣性センサの出力を用いることと等価である。
よって、６軸慣性センサ１１の自動車への取付け角によらずに、取付け角の推定が発散してしまうことが抑止されると共に、取付け角の推定を正しい値に速やかに収束できることが期待できる。
次に、制御部１４が行う６軸慣性センサ１１の取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3の設定について説明する。
ここで、カルマンフィルタ１６は、センサ取付角検出部１３が検出したヨー角A3が属する角度範囲に応じて定まる角度θだけ、Xs軸、Ys軸、Zs軸を直交３軸とする６軸慣性センサ１１の座標系をZs軸周りに回転させた、Xs'軸、Ys'軸、Zs'軸を直交３軸とする仮想６軸慣性センサ座標系の車両座標系に対する取り付け角のピッチ角とヨー角を推定する。
そこで、制御部１４は、カルマンフィルタ１６への、取り付け角のピッチ角の初期値SA2、ヨー角の初期値SA3を、センサ取付角検出部１３が検出したピッチ角A2とヨー角A3と、ヨー角A3が属する角度範囲に応じて次のように設定する。
すなわち、
-45°＜A3＜+45°のときSA2=A2、θ=0°、SA3=A3+θ、
+135°≦A3＜+180°のときSA2=A2、θ=-180°、SA3=A3+θ
-135°≧A3≧-180°のときSA2=A2、θ=+180°、SA3=A3+θ
とする。
また、センサ取付角検出部１３が検出したヨー角A3が属する角度範囲が次の範囲にあり、センサ座標系のZs軸が車両座標系のZm軸に近似していることが期待できる場合には、取り付け角のピッチ角の初期値SA2、ヨー角の初期値SA3を、センサ取付角検出部１３が検出したロール角A1とヨー角A3と、ヨー角A3が属する角度範囲に応じて次のように設定する。
+45°≦A3＜+135°のときSA2=A1、θ=-90°、SA3=A3+θ
-45°≧A3≧-135°のときSA2=-A1、θ=+90°、SA3=A3+θ
制御部１４において、以上のように取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3を設定することにより、６軸慣性センサ１１の取り付け角のピッチ角とヨー角の推定が、発散することなく速やかに収束することが期待できる。
なお、以上の実施形態では仮想６軸慣性センサのヨー角A3'を±45°の範囲内に収めたが、ヨー角A3'を収める範囲は必ずしも±45°の範囲でなくても良く、±90°未満の任意の範囲としてもよい。
１…自律航法システム、２…状態利用システム、１１…６軸慣性センサ、１２…ＧＮＳＳ受信機、１３…センサ取付角検出部、１４…制御部、１５…センサ出力変換部、１６…カルマンフィルタ、１１１…加速度センサ、１１２…ジャイロ、１６１…予測ステップ実行部、１６２…フィルタリングステップ実行部。

Claims (9)

  1. 移動体に搭載される自律航法システムであって、
    慣性センサと、
    前記慣性センサの前記移動体に対する取付角のヨー角を検出するヨー角検出手段と、
    前記慣性センサの出力を変換し出力する変換手段と、
    前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを有し、
    前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、
    前記変換手段は、前記ヨー角検出手段が検出したヨー角に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力することを特徴とする自律航法システム。
  2. 移動体に搭載される自律航法システムであって、
    慣性センサと、
    前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲をヨー角角度範囲として検出するヨー角角度範囲検出手段と、
    前記慣性センサの出力を変換し出力する変換手段と、
    前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを有し、
    前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、
    前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力することを特徴とする自律航法システム。
  3. 請求項２記載の自律航法システムであって、
    前記ヨー角角度範囲検出手段は、-45°と+45°の間の角度範囲、+45°と+135°の間の角度範囲、+135°と+180°の間の角度範囲、-45°と-135°の間の角度範囲、-135°と-180°の間の角度範囲のうちの、前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲を前記ヨー角角度範囲として検出し、
    前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±４５°以内の仮想の慣性センサの出力に変換し出力することを特徴とする自律航法システム。
  4. 請求項３記載の自律航法システムであって、
    前記慣性センサは、X、Y、Zの直交する３軸方向の加速度（ax,ay,az)を出力する慣性センサであり、
    前記変換手段は、前記変換として加速度（ax,ay,az)の加速度（ax',ay',az')への変換を行うと共に、当該加速度（ax',ay',az')への変換を、
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
    （ax',ay',az')=（ax,ay,az)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
    （ax',ay',az')=（-ay,ax,az)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
    （ax',ay',az')=（-ax,-ay,az)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
    （ax',ay',az')=（ay,-ax,az)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
    （ax',ay',az')=（-ax,-ay,az)
    とすることにより行うことを特徴とする自律航法システム。
  5. 請求項３記載の自律航法システムであって、
    前記慣性センサは、X、Y、Zの直交する３軸まわりの角速度（ωx,ωy,ωz)を出力する慣性センサであり、
    前記変換手段は、前記変換として角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うと共に、当該角速度（ωx',ωy',ωz')への変換を、
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（ωx,ωy,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（-ωy,ωx,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（-ωx,-ωy,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（ωy,-ωx,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（-ωx,-ωy,ωz)
    とすることにより行うことを特徴とする自律航法システム。
  6. 請求項４記載の自律航法システムであって、
    前記慣性センサは、加速度（ax,ay,az)に加え、前記X、Y、Zの３軸まわりの角速度（ωx,ωy,ωz)を出力し、
    前記変換手段は、前記変換として、さらに、角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うと共に、当該角速度（ωx',ωy',ωz')への変換を、
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（ωx,ωy,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（-ωy,ωx,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（-ωx,-ωy,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（ωy,-ωx,ωz)
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
    （ωx',ωy',ωz')=（-ωx,-ωy,ωz)
    とすることにより行うことを特徴とする自律航法システム。
  7. 請求項２、３、４、５または６記載の自律航法システムであって、
    前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のヨー角を検出し、
    当該自律航法システムは、
    前記ヨー角角度範囲検出手段が検出したヨー角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのヨー角を算定し、前記カルマンフィルタが推定する取付角のヨー角の初期値として当該カルマンフィルタに設定する初期値設定手段を有することを特徴とする自律航法システム。
  8. 請求項７記載の自律航法システムであって、
    前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のロール角とピッチ角を検出し、
    前記初期値設定手段は、前記ヨー角角度範囲検出手段が検出したロール角とピッチ角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのピッチ角を算定し、前記カルマンフィルタが推定する取付角のピッチ角の初期値として当該カルマンフィルタに設定することを特徴とする自律航法システム。
  9. 請求項３、４、５または６記載の自律航法システムであって、
    前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のロール角A1とピッチ角A2とヨー角A3とを検出し、
    当該自律航法システムは、
    前記カルマンフィルタが推定する取付角のピッチ角の初期値SA2とヨー角の初期値SA3を設定する初期値設定手段を有し、
    当該初期値設定手段は、
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
    SA2=A2
    SA3=A3
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
    SA2=A1
    SA3=A3-90°
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
    SA2=-A2
    SA3=A3-180°
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
    SA2=-A1
    SA3=A3+90°
    ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
    SA2=-A2
    SA3=A3+180°
    により初期値SA2とヨー角の初期値SA3を算定することを特徴とする自律航法システム。
JP2022146423A 2022-09-14 2022-09-14 自律航法システム Pending JP2024041548A (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2022146423A JP2024041548A (ja) 2022-09-14 2022-09-14 自律航法システム
EP23195811.7A EP4339558A1 (en) 2022-09-14 2023-09-06 Autonomous navigation system
US18/367,045 US20240085183A1 (en) 2022-09-14 2023-09-12 Autonomous navigation system
CN202311181877.0A CN117705128A (zh) 2022-09-14 2023-09-13 自主导航系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2022146423A JP2024041548A (ja) 2022-09-14 2022-09-14 自律航法システム

Publications (1)

Publication Number Publication Date
JP2024041548A true JP2024041548A (ja) 2024-03-27

Family

ID=87933679

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2022146423A Pending JP2024041548A (ja) 2022-09-14 2022-09-14 自律航法システム

Country Status (4)

Country Link
US (1) US20240085183A1 (ja)
EP (1) EP4339558A1 (ja)
JP (1) JP2024041548A (ja)
CN (1) CN117705128A (ja)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4191499B2 (ja) 2003-02-03 2008-12-03 パイオニア株式会社 車載装置
CN101583844B (zh) * 2007-01-18 2011-11-09 三菱电机株式会社 车载导航装置
JP5164645B2 (ja) 2008-04-07 2013-03-21 アルパイン株式会社 カルマンフィルタ処理における繰り返し演算制御方法及び装置
JP5328252B2 (ja) * 2008-07-30 2013-10-30 アルパイン株式会社 ナビゲーションシステムの位置検出装置および位置検出方法
JP6094026B2 (ja) * 2011-03-02 2017-03-15 セイコーエプソン株式会社 姿勢判定方法、位置算出方法及び姿勢判定装置
JP6257865B2 (ja) * 2015-10-15 2018-01-10 三菱電機株式会社 測位装置および測位方法

Also Published As

Publication number Publication date
US20240085183A1 (en) 2024-03-14
EP4339558A1 (en) 2024-03-20
CN117705128A (zh) 2024-03-15

Similar Documents

Publication Publication Date Title
US8571795B2 (en) Navigation apparatus and navigation information calculation method
JP5328252B2 (ja) ナビゲーションシステムの位置検出装置および位置検出方法
JP7036080B2 (ja) 慣性航法装置
JP5586994B2 (ja) 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
JP6473684B2 (ja) 車輪の滑り角推定装置及びその方法
EP3388787B1 (en) Polar region operating attitude and heading reference system
CN108627152B (zh) 一种微型无人机基于多传感器数据融合的导航方法
JP2012193965A (ja) 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
JP5419268B2 (ja) 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム
Emami et al. A low complexity integrated navigation system for underwater vehicles
JP5022747B2 (ja) 移動体の姿勢及び方位検出装置
JPH095104A (ja) 移動物体の三次元姿勢角測定法および三次元姿勢角計測装置
US11947364B2 (en) Vessel anti-rolling control apparatus and anti-rolling control method
CN112985384A (zh) 一种抗干扰磁航向角优化系统
JP2024041548A (ja) 自律航法システム
JP2896407B1 (ja) 慣性航法装置
JP4517298B2 (ja) 方位センサの補正係数演算装置及び演算プログラム
WO2014171227A1 (ja) 姿勢角推定装置およびそれを備える移動状態検出装置
Damerius et al. A generic inertial navigation system
JP6981459B2 (ja) センサ誤差補正装置
Onunka et al. USV attitude estimation: an approach using quaternion in direction cosine matrix
JP6463551B2 (ja) 進行方向推定装置
JP6409625B2 (ja) 車両位置算出装置
JP2019199238A (ja) 航走制御方法および航走制御装置
JP2756554B2 (ja) 慣性装置