JP2012173190A - Positioning system and positioning method - Google Patents

Positioning system and positioning method Download PDF

Info

Publication number
JP2012173190A
JP2012173190A JP2011036806A JP2011036806A JP2012173190A JP 2012173190 A JP2012173190 A JP 2012173190A JP 2011036806 A JP2011036806 A JP 2011036806A JP 2011036806 A JP2011036806 A JP 2011036806A JP 2012173190 A JP2012173190 A JP 2012173190A
Authority
JP
Japan
Prior art keywords
error
data
velocity
bias
acceleration
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.)
Withdrawn
Application number
JP2011036806A
Other languages
Japanese (ja)
Inventor
Imei Cho
イメイ 丁
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.)
Seiko Epson Corp
Original Assignee
Seiko Epson Corp
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 Seiko Epson Corp filed Critical Seiko Epson Corp
Priority to JP2011036806A priority Critical patent/JP2012173190A/en
Publication of JP2012173190A publication Critical patent/JP2012173190A/en
Withdrawn legal-status Critical Current

Links

Images

Abstract

PROBLEM TO BE SOLVED: To provide a positioning method with high precision such that inertia sensor data and GPS positioning data are merged together.SOLUTION: The positioning method of positioning a moving body by merging the inertia sensor data and GPS positioning data together includes the processes: performing time update of an extended Kalman filter using a multiplication type quaternion error model based upon the inertia sensor data; performing observation update of the extended Kalman filter based upon the GPS positioning data, and estimating a position error, a speed error, and an azimuth error, and a gyro bias error and an acceleration bias error; and correcting errors in position, speed, and attitude based upon the estimated position error, speed error, azimuth error, gyro bias error, and acceleration bias error.

Description

本発明は、測位システム、及び測位方法に関する。   The present invention relates to a positioning system and a positioning method.

屋内やトンネル等、測位データが得にくい場所では、慣性航法データと測位データとを融合させることによって、屋内外を問わず、移動体の位置、速度、姿勢等の高精度な測位を行う。
慣性航法データと測位データとを融合させた測位システムや測位方法としては、慣性航法データと測位データを用いて、状態推定カルマンフィルターにより、移動体の位置、速度、姿勢の誤差補正を行うものがある。状態推定カルマンフィルターでは、要素数の少ない状態変数で特定される微小変化単位四元数を用いて、移動体の位置、速度、姿勢の誤差をモデリングし、誤差推定を行い、さらに、推定された誤差データを乗じることで、位置、速度、姿勢のデータを補正する測位システム及び測位方法が提案されている(例えば、特許文献1参照)。
In locations such as indoors and tunnels where positioning data is difficult to obtain, high-precision positioning such as the position, speed, and posture of a moving object is performed regardless of whether it is indoors or outdoors by fusing inertial navigation data and positioning data.
As a positioning system or positioning method that combines inertial navigation data and positioning data, there is one that corrects the position, velocity, and attitude errors of a moving object using a state estimation Kalman filter using inertial navigation data and positioning data. is there. In the state estimation Kalman filter, the error of the position, speed, and posture of the moving object is modeled using the minute change unit quaternion specified by the state variable with a small number of elements, and the error is estimated. There has been proposed a positioning system and a positioning method for correcting position, velocity, and attitude data by multiplying error data (see, for example, Patent Document 1).

特開2007−276507号公報JP 2007-276507 A

特許文献1では、慣性センサーデータとGPS測位データとを融合して、移動体の位置、速度、姿勢の補正を行っている。しかし、小型・軽量・ローコストの慣性センサー(例えば、MEMSセンサー等)は、バイアス誤差およびランダムドリフトが大きく、移動体の位置、速度、姿勢の高精度な測位ができないという課題を有している。   In Patent Document 1, inertial sensor data and GPS positioning data are merged to correct the position, speed, and posture of a moving body. However, a small, lightweight, and low-cost inertial sensor (for example, a MEMS sensor) has a large bias error and random drift, and has a problem that high-precision positioning of the position, speed, and posture of a moving body cannot be performed.

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。   SUMMARY An advantage of some aspects of the invention is to solve at least a part of the problems described above, and the invention can be implemented as the following forms or application examples.

[適用例1]本適用例に係る測位システムは、慣性センサーデータとGPS測位データとに基づきジャイロバイアス誤差の推定及び加速度バイアス誤差の推定と、位置誤差・速度誤差・方位誤差の推定と、を行う拡張カルマンフィルター演算部と、前記ジャイロバイアス誤差に基づき角速度データのバイアス補正を行い、かつ、前記加速度バイアス誤差に基づき加速度データのバイアス補正を行うバイアス補正部と、前記バイアス補正された角速度データに基づき姿勢データを演算する姿勢演算部と、前記バイアス補正された加速度データおよび前記姿勢データに基づきナビゲーションフレームの加速度データを出力する座標変換部と、前記ナビゲーションフレームの加速度データに基づき位置・速度を演算する位置/速度演算部と、前記位置/速度演算部が演算した位置・速度および前記姿勢データを、前記拡張カルマンフィルター演算部から出力された位置・速度・方位の推定誤差に基づいて補正するエラー補正部と、を有し、前記拡張カルマンフィルター演算部は、前記位置/速度演算部が演算した前記位置・速度と、前記姿勢演算部が演算した前記姿勢データとに基いて、乗算型クォータニオン誤差モデルを用いて前記ジャイロバイアス誤差、前記加速度バイアス誤差および前記位置誤差・速度誤差・方位誤差の推定を行うことを特徴とする。
なお、慣性センサーデータとは、ジャイロセンサー及び加速度センサーにて検出する角速度データ、加速度データを意味する。
[Application Example 1] A positioning system according to this application example includes gyro bias error estimation and acceleration bias error estimation based on inertial sensor data and GPS positioning data, and position error / speed error / azimuth error estimation. An extended Kalman filter calculation unit that performs bias correction of angular velocity data based on the gyro bias error, and a bias correction unit that performs bias correction of acceleration data based on the acceleration bias error, and the bias corrected angular velocity data A posture calculation unit that calculates posture data based on the data, a coordinate conversion unit that outputs acceleration data of the navigation frame based on the bias-corrected acceleration data and the posture data, and a position / speed calculated based on the acceleration data of the navigation frame Position / velocity calculation unit, and the position An error correction unit that corrects the position / velocity calculated by the speed calculation unit and the attitude data based on the estimation error of the position / velocity / direction output from the extended Kalman filter calculation unit, and the extension Based on the position / velocity calculated by the position / velocity calculation unit and the attitude data calculated by the attitude calculation unit, the Kalman filter calculation unit uses the multiplication type quaternion error model, The acceleration bias error and the position error / velocity error / azimuth error are estimated.
The inertial sensor data means angular velocity data and acceleration data detected by a gyro sensor and an acceleration sensor.

本適用例の測位システムによれば、乗算型クォータニオン(以降、乗算型Quaternionと表すことがある)誤差モデルを用いて姿勢誤差のモデリングを行い、拡張カルマンフィルター演算部によって移動体の位置、速度、姿勢の誤差を推定と、ジャイロセンサー及び加速度センサーの各バイアス誤差の推定と、誤差補正を行う。姿勢の乗算型クォータニオン誤差モデルは、加算型クォータニオン誤差モデルに比べ推定誤差の精度がよい。このことから、小型化、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなMEMS(Micro Electro Mechanical System)センサーを用いる場合でも、トンネルや屋内などGPS測位データが途切れる場所においても、高精度な測位が可能である。   According to the positioning system of this application example, attitude error modeling is performed using a multiplication type quaternion (hereinafter, sometimes referred to as multiplication type quaternion) error model, and the position, velocity, Posture error is estimated, gyro sensor and acceleration sensor bias errors are estimated, and error correction is performed. The attitude multiplying quaternion error model has a higher accuracy of estimation error than the additive quaternion error model. From this, it is possible to reduce the size and cost, but even when using MEMS (Micro Electro Mechanical System) sensors with large fluctuations in bias error and random drift, even in places where GPS positioning data is interrupted, such as in tunnels and indoors. High-precision positioning is possible.

[適用例2]本適用例に係る測位方法は、慣性センサーデータとGPS測位データとを融合して移動体の測位を行う測位方法であって、前記慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、前記GPS測位データに基づき、前記拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、前記推定された位置誤差・速度誤差・方位角誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、推定されたジャイロバイアス誤差・加速度バイアス誤差に基づき、慣性センサーデータを補正する工程とを有し、前記移動体の位置・速度・姿勢のうち少なくともいずれか一つを求めることを特徴とする。
なお、拡張カルマンフィルターの時間更新とは、状態ベクトルと誤差共分散を時間毎に更新することであり、観測更新とは、外部からの補正情報を得て状態ベクトルと誤差共分散とを更新することである。
Application Example 2 A positioning method according to this application example is a positioning method in which inertial sensor data and GPS positioning data are fused to perform positioning of a moving body, and based on the inertial sensor data, a multiplication type quaternion error model The time update of the extended Kalman filter using, and the observation update of the extended Kalman filter based on the GPS positioning data to estimate the position error, velocity error, azimuth error, gyro bias error, acceleration bias error And correcting each position / velocity / attitude error based on the estimated position error / velocity error / azimuth angle error and inertial sensor data based on the estimated gyro bias error / acceleration bias error. And determining at least one of the position, speed, and posture of the moving body. It is characterized in.
Note that the time update of the extended Kalman filter is to update the state vector and the error covariance every time, and the observation update is to update the state vector and the error covariance by obtaining correction information from the outside. That is.

本適用例の測位方法によれば、乗算型クォータニオン誤差モデルを用いて姿勢誤差をモデリングし、拡張カルマンフィルターの時間更新と観測更新を行い、ジャイロバイアス誤差・加速度バイアス誤差の推定及び誤差補正と、GPS測位データ(位置、速度、方位)の誤差推定及び誤差補正とを融合させることにより、トンネルや屋内などにおいて、移動体の高精度な測位が可能である。   According to the positioning method of this application example, the attitude error is modeled using the multiplication type quaternion error model, the time update and observation update of the extended Kalman filter are performed, the gyro bias error / acceleration bias error estimation and error correction, By combining error estimation and error correction of GPS positioning data (position, speed, direction), it is possible to perform highly accurate positioning of a moving body in a tunnel or indoor.

[適用例3]上記適用例に係る測位方法は、前記方位角誤差及び前記位置誤差の少なくとも一つを、乗算型クォータニオンを用いて誤差モデリングすることが好ましい。   Application Example 3 In the positioning method according to the application example described above, it is preferable that at least one of the azimuth angle error and the position error is error-modeled using a multiplication quaternion.

このように、姿勢の誤差モデリングに加え、位置の誤差モデリングを行うことで、より高精度な移動体の測位を行うことができる。   As described above, by performing position error modeling in addition to posture error modeling, positioning of a moving body with higher accuracy can be performed.

[適用例4]上記適用例に係る測位方法は、外部から観測された距離情報及び相対速度情報を観測情報として用い、乗算型クォータニオン誤差モデルによって前記移動体の前記位置・速度・方位角の誤差、及び前記ジャイロバイアス誤差・加速度バイアス誤差の推定を行い、前記移動体の前記位置・速度・姿勢を補正することが好ましい。
ここで、外部から観測された距離情報及び相対速度情報(Measurementと表すことがある)には、擬似距離(pseudorange)やドップラー効果(doppler)を用いた相対速度情報が含まれる。
[Application Example 4] The positioning method according to the application example described above uses distance information and relative velocity information observed from the outside as observation information, and an error of the position / velocity / azimuth angle of the moving body by a multiplication quaternion error model. It is preferable to estimate the gyro bias error / acceleration bias error and correct the position / velocity / posture of the moving body.
Here, the distance information and the relative velocity information (sometimes referred to as “Measurement”) observed from the outside include relative velocity information using pseudorange and Doppler effect.

このように、Measurementデータを、乗算型クォータニオン誤差モデルを用いて誤差補正することによっても、移動体の高精度な測位を行うことができる。   As described above, it is also possible to perform highly accurate positioning of the moving body by correcting the error of the measurement data using the multiplication type quaternion error model.

[適用例5]本適用例に係る測位方法は、前記移動体が移動している場合には、請求項2ないし請求項4のいずれか一項に記載の測位方法を用いて測位し、前記移動体が停止している場合には、速度を0、ヨー角を不変として、さらに加速度センサーの検出値からロール角とピッチ角とを演算して観測値とし、前記観測値に基づき、姿勢の補正及び前記ジャイロバイアス誤差及び前記加速度バイアス誤差の補正を行うことが好ましい。   Application Example 5 In the positioning method according to this application example, when the moving body is moving, positioning is performed using the positioning method according to any one of claims 2 to 4, When the moving body is stopped, the speed is set to 0, the yaw angle is not changed, and the roll angle and the pitch angle are calculated from the detected values of the acceleration sensor to obtain the observed values. It is preferable to perform correction and correction of the gyro bias error and the acceleration bias error.

移動体が停止している場合には、GPS測位データの精度が低下する。そこで、速度0、ヨー角不変(δψ = 0)、さらに加速度センサーの検出値から算出されたロール角とピッチ角を観測値として乗算型クォータニオン誤差モデルを用いて、姿勢の補正及びジャイロバイアス誤差及び加速度バイアス誤差の補正を行うことで、停止時における移動体の位置、姿勢を検出することができる。   When the moving body is stopped, the accuracy of the GPS positioning data is lowered. Therefore, using the multiplying quaternion error model with the roll angle and pitch angle calculated from the detected value of the acceleration sensor as the speed 0, the yaw angle invariant (δψ = 0), the posture correction, the gyro bias error, and By correcting the acceleration bias error, it is possible to detect the position and orientation of the moving body when stopped.

実施形態1に係る測位システムの概要を示す構成ブロック図。1 is a configuration block diagram showing an overview of a positioning system according to Embodiment 1. FIG. 実施形態1に係る演算制御部の概要を示す構成ブロック図。FIG. 2 is a configuration block diagram illustrating an outline of an arithmetic control unit according to the first embodiment. 実施形態1に係る測位方法の1例を示すフローチャート。5 is a flowchart illustrating an example of a positioning method according to the first embodiment. シミュレーションによる移動体の移動軌跡の1例を表すグラフ。The graph showing an example of the movement locus | trajectory of the moving body by simulation. シミュレーションによる移動体の速度の1例を表すグラフ。The graph showing an example of the speed of the moving body by simulation. Z軸方向のシミュレーションによる性能評価の結果。Results of performance evaluation by simulation in the Z-axis direction. 姿勢誤差に係るシミュレーションによる性能評価の結果。Results of performance evaluation by simulation related to attitude error. 姿勢誤差に係るシミュレーションによる性能評価の結果。Results of performance evaluation by simulation related to attitude error. ジャイロバイアス誤差に係るシミュレーションによる性能評価の結果。Results of performance evaluation by simulation related to gyro bias error. 加速度バイアス誤差に係るシミュレーションによる性能評価の結果。Results of performance evaluation by simulation related to acceleration bias error.

以下、本発明の実施の形態を図面を参照して説明する。
(実施形態1)
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
(Embodiment 1)

図1は、実施形態1に係る測位システムの概要を示す構成ブロック図である。測位システム1は、ジャイロセンサー20と、加速度センサー30と、GPS(Global Positioning System)信号受信部40と、ジャイロセンサー20、加速度センサー30、及びGPS信号受信部40から入力されたデータを演算処理する演算制御部10と、記憶部50を含んで構成される。演算制御部10は、移動体の位置、速度、姿勢を制御する駆動装置62、或いは、必要なデータの形態で表示するための表示部61に接続されている。   FIG. 1 is a configuration block diagram illustrating an overview of the positioning system according to the first embodiment. The positioning system 1 performs arithmetic processing on data input from the gyro sensor 20, the acceleration sensor 30, the GPS (Global Positioning System) signal receiving unit 40, the gyro sensor 20, the acceleration sensor 30, and the GPS signal receiving unit 40. The calculation control unit 10 and the storage unit 50 are included. The arithmetic control unit 10 is connected to a driving device 62 that controls the position, speed, and posture of the moving body, or a display unit 61 for displaying in the form of necessary data.

ジャイロセンサー20は、移動体に配設され、例えば、水晶の振動型センサーやMEMSセンサーを利用し、微小振動板が回転により受けるコリオリ力に基づき、回転角速度を検出するものである。本実施形態では、ジャイロセンサー20がボディーフレーム(b‐frame)の各軸まわりの回転角速度を検出するように複数個設けられ、軸まわりの回転角速度を表すデータを出力する。   The gyro sensor 20 is disposed on a moving body, and uses, for example, a quartz vibration sensor or a MEMS sensor, and detects a rotational angular velocity based on a Coriolis force that the micro diaphragm receives by rotation. In the present embodiment, a plurality of gyro sensors 20 are provided so as to detect the rotational angular velocities around each axis of the body frame (b-frame), and output data representing the rotational angular velocities around the axes.

加速度センサー30は、移動体に配設され、少なくとも一つの方向に対する移動体の加速度を測定し出力する。本実施形態では、この加速度センサー30は、水晶の振動型センサーやMEMSセンサーであり、ボディーフレームの各軸方向への移動体の移動加速度を測定し出力する。   The acceleration sensor 30 is disposed on the moving body, and measures and outputs the acceleration of the moving body in at least one direction. In the present embodiment, the acceleration sensor 30 is a quartz vibration sensor or a MEMS sensor, and measures and outputs the movement acceleration of the moving body in each axial direction of the body frame.

GPS信号受信部40は、GPS衛星からの信号を受信して、移動体の位置(緯度、経度、高度)を表すデータを出力する。本実施形態で用いられるGPS信号受信部40は広く用いられているものであるので、ここでの詳細な説明を省略する。   The GPS signal receiving unit 40 receives a signal from a GPS satellite and outputs data representing the position (latitude, longitude, altitude) of the moving body. Since the GPS signal receiver 40 used in the present embodiment is widely used, a detailed description thereof is omitted here.

演算制御部10は、CPU(Central Processing Unit)等のプログラム制御デバイスを用いて構成され、記憶部50に格納されているプログラムに従って動作する。本実施形態では、この演算制御部10が、ジャイロセンサー20、加速度センサー30やGPS信号受信部40から出力される各データに基づいて、移動体の位置、速度、姿勢、およびジャイロセンサーバイアス、加速度センサーバイアスの各誤差推定と誤差補正を行い、移動体の表示部61や駆動装置62などの出力部に出力し、移動体を制御する。
この演算制御部10の具体的な構成と作用については後に詳しく述べる。
The arithmetic control unit 10 is configured using a program control device such as a CPU (Central Processing Unit) and operates according to a program stored in the storage unit 50. In the present embodiment, the arithmetic control unit 10 determines the position, speed, posture, gyro sensor bias, and acceleration of the moving body based on each data output from the gyro sensor 20, the acceleration sensor 30, and the GPS signal receiving unit 40. Each error of the sensor bias is estimated and error correction is performed and output to an output unit such as the display unit 61 or the driving device 62 of the moving body to control the moving body.
The specific configuration and operation of the arithmetic control unit 10 will be described in detail later.

記憶部50は、RAM(Random Access Memory)等の記憶素子であり、演算制御部10によって実行されるプログラムを保持する。また、この記憶部50は、演算制御部10のワークメモリーとしても動作する。なお、記憶部50は、演算制御部10に内蔵する構成としてもよい。   The storage unit 50 is a storage element such as a RAM (Random Access Memory) and holds a program executed by the arithmetic control unit 10. The storage unit 50 also operates as a work memory for the arithmetic control unit 10. The storage unit 50 may be built in the calculation control unit 10.

以下の説明では、位置、速度、姿勢の各データは、どの座標系におけるデータであるかを識別するために、データにインデックスを付して、いずれかの座標系であるかを明確にしている。
(1)地球中心を原点として、地球の自転軸と平行になるようZ軸を定めた右手系直交座標系を「iフレーム」とする。
(2)地球中心を原点として、緯度0度、経度0度方向をX軸とし、かつ地球の自転軸と平行になるようZ軸を定めた右手系直交座標系を「eフレーム」とする。
(3)移動体を原点として、北方向をX軸、地球による重力の方向をZ軸とした右手系直交座標系を「gフレーム」とする。
(4)移動体を原点として、地球による重力方向をZ軸とした右手系直交座標系であって、gフレームにおけるZ軸を中心にgフレームをα(rad)だけ回転させたときの座標系を「nフレーム(n‐frame)」とする。
(5)移動体を原点として、移動体の進行方向をX軸、揚力方向をZ軸とした右手系直交座標系を「bフレーム(b‐frame)」とする。
In the following description, in order to identify which coordinate system each position, velocity, and orientation data is, the data is indexed to clarify which coordinate system it is. .
(1) A right-handed orthogonal coordinate system with the center of the earth as the origin and the Z-axis so as to be parallel to the rotation axis of the earth is referred to as an “i-frame”.
(2) A right-handed orthogonal coordinate system in which the center of the earth is the origin, the latitude 0 degree, longitude 0 degree direction is the X axis, and the Z axis is defined to be parallel to the rotation axis of the earth is referred to as an “e frame”.
(3) A right-handed orthogonal coordinate system with the moving body as the origin, the north direction as the X axis, and the direction of gravity by the earth as the Z axis is defined as “g frame”.
(4) A right-handed orthogonal coordinate system with the moving body as the origin and the gravity direction of the earth as the Z axis, and the coordinate system when the g frame is rotated by α (rad) about the Z axis in the g frame Is “n-frame”.
(5) A right-handed orthogonal coordinate system in which the moving body is the origin, the traveling direction of the moving body is the X axis, and the lift direction is the Z axis is “b-frame”.

なお、bフレームにおけるX軸まわりの回転を「ロール(ロール角φ)」、Y軸まわりの回転を「ピッチ(ピッチ角θ)」、Z軸まわりの回転を「ヨー(ヨー角ψ)」と称する。   Note that the rotation around the X axis in the b frame is “roll (roll angle φ)”, the rotation around the Y axis is “pitch (pitch angle θ)”, and the rotation around the Z axis is “yaw (yaw angle ψ)”. Called.

続いて、演算制御部10の構成及び作用について図面、及び数式を参照して説明する。
図2は、実施形態1に係る演算制御部の概要を示す構成ブロック図である。演算制御部10は、バイアス補正部11と、姿勢演算部12と、座標変換部13と、位置/速度演算部14と、エラー補正部15(error補正部15と表記)と、拡張カルマンフィルター演算部16と、を主たる構成要素として構成している。
Next, the configuration and operation of the arithmetic control unit 10 will be described with reference to the drawings and mathematical expressions.
FIG. 2 is a configuration block diagram illustrating an outline of the arithmetic control unit according to the first embodiment. The calculation control unit 10 includes a bias correction unit 11, an attitude calculation unit 12, a coordinate conversion unit 13, a position / velocity calculation unit 14, an error correction unit 15 (described as an error correction unit 15), and an extended Kalman filter calculation. The part 16 is configured as a main component.

バイアス補正部11は、拡張カルマンフィルター演算部16によって算出されたジャイロバイアス誤差、加速度バイアス誤差の推定値を利用して、角速度データ及び加速度データの補正値を数式1を用いて算出し、加速度データ(b‐frame)は座標変換部13に、角速度データは姿勢演算部12に出力する。   The bias correction unit 11 uses the estimated values of the gyro bias error and the acceleration bias error calculated by the extended Kalman filter calculation unit 16 to calculate the angular velocity data and the correction value of the acceleration data using Equation 1, and the acceleration data (B-frame) is output to the coordinate conversion unit 13, and the angular velocity data is output to the posture calculation unit 12.

Figure 2012173190
Figure 2012173190

姿勢演算部12は、移動体の姿勢に関わる運動方程式(数式2)を用いて、姿勢を求め、座標変換部13、エラー補正部15、及び拡張カルマンフィルター演算部16に入力する。   The posture calculation unit 12 obtains a posture by using an equation of motion related to the posture of the moving body (Formula 2), and inputs the posture to the coordinate conversion unit 13, the error correction unit 15, and the extended Kalman filter calculation unit 16.

Figure 2012173190
Figure 2012173190

座標変換部13は、加速度センサー30が出力するb‐frameの加速度データと、姿勢演算部12が出力する姿勢データとに基づき、数式3を用いて、n‐frameの加速度データに変換して位置/速度演算部14に出力する。   Based on the b-frame acceleration data output from the acceleration sensor 30 and the attitude data output from the attitude calculation unit 12, the coordinate conversion unit 13 converts the position data into n-frame acceleration data using Equation 3 based on the position data. / Output to speed calculation unit 14

Figure 2012173190
Figure 2012173190

位置/速度演算部14は、位置および速度に関わる運動方程式(数式4)を用いて、位置と速度を演算してエラー補正部15、及び拡張カルマンフィルター演算部16に出力する。   The position / velocity calculation unit 14 calculates the position and velocity using the equation of motion (Formula 4) related to the position and velocity, and outputs them to the error correction unit 15 and the extended Kalman filter calculation unit 16.

Figure 2012173190
Figure 2012173190

拡張カルマンフィルター演算部16は、姿勢演算部12から出力される姿勢データと、位置/速度演算部14から出力された位置データ及び速度データと、GPS測位データ(位置、速度、方位)を用いて、カルマンフィルターによって、位置、速度、方位の誤差を推定すると共に、ジャイロバイアス誤差及び加速度バイアス誤差を算出し、バイアス補正部11に入力する。本実施形態では、速度、姿勢の運動方程式が非線形のため、カルマンフィルターを拡張した誤差モデルを適用する。誤差モデルは数式5によって表すことができる。   The extended Kalman filter calculation unit 16 uses the posture data output from the posture calculation unit 12, the position data and velocity data output from the position / speed calculation unit 14, and the GPS positioning data (position, velocity, direction). Then, the Kalman filter estimates the position, velocity, and direction errors, calculates the gyro bias error and the acceleration bias error, and inputs them to the bias correction unit 11. In this embodiment, since the motion equations of speed and posture are nonlinear, an error model in which the Kalman filter is extended is applied. The error model can be expressed by Equation 5.

Figure 2012173190
Figure 2012173190

ここでは、姿勢の誤差モデルは、Quaternionの加算ではなく、より要素数の少ない微小変化単位四元数を導入し、乗算型Quaternionを用いて誤差をモデリングする。なお、微小誤差を用いた加算型Quaternionの誤差モデルは、次式で表される。   Here, the attitude error model is not an addition of Quaternion, but introduces a small change unit quaternion having a smaller number of elements, and models the error using a multiplying Quaternion. Note that an addition-type Quaternion error model using a minute error is expressed by the following equation.

Figure 2012173190
Figure 2012173190

加算型Quaternionのノルムは、次式で表される。   The norm of the addition type Quaternion is expressed by the following equation.

Figure 2012173190
Figure 2012173190

数式7に表されるように、微小変化単位四元数を用いた加算型Quaternionのノルムは、制約条件のノルム=1を満足できない。   As represented by Expression 7, the norm of the addition-type Quaternion using the minute change unit quaternion cannot satisfy the constraint norm = 1.

一方、微小誤差ベクトル要素を用いた乗算型Quaternionでは姿勢誤差を次式で表すことができる。   On the other hand, in the multiplication type Quaternion using minute error vector elements, the attitude error can be expressed by the following equation.

Figure 2012173190
Figure 2012173190

乗算型Quaternionのノルムは次式で表すことができる。   The norm of the multiplication type Quaternion can be expressed by the following equation.

Figure 2012173190
Figure 2012173190

乗算型Quaternionのノルムは、数式9から分かるように、乗算型Quaternion誤差モデルを利用することで、誤差を含んだ状態でも、ノルムが1であることが保証され、姿勢誤差ベクトルの要素数は4から3になり、推定する状態ベクトルの数を減少させることができる。   As can be seen from Equation 9, the norm of the multiplication type quaternion is guaranteed to be 1 even in a state including an error by using the multiplication type quaternion error model, and the number of elements of the posture error vector is 4 The number of state vectors to be estimated can be reduced.

ここで、推定する誤差ベクトルは、数式10で表すことができる。   Here, the error vector to be estimated can be expressed by Equation 10.

Figure 2012173190
Figure 2012173190

また、GPS測位データ(位置、速度、方位)を観測データとして、数式11のような観測方程式、及び数式12〜数式14のような誤差共分散を算出することができる。   Further, using GPS positioning data (position, velocity, direction) as observation data, an observation equation such as Equation 11 and an error covariance such as Equations 12 to 14 can be calculated.

Figure 2012173190
Figure 2012173190

推定誤差共分散Pは次式で表される。   The estimated error covariance P is expressed by the following equation.

Figure 2012173190
Figure 2012173190

システム誤差共分散Qは次式で表される。   The system error covariance Q is expressed by the following equation.

Figure 2012173190
Figure 2012173190

観測誤差共分散Rは次式で表される。   The observation error covariance R is expressed by the following equation.

Figure 2012173190
Figure 2012173190

数式12、数式13から、カルマンフィルターの時間更新は、次式で表すことができる。    From Equations 12 and 13, the time update of the Kalman filter can be expressed by the following equation.

Figure 2012173190
Figure 2012173190

また、カルマンフィルターの観測更新は、数式16、数式17で表すことができる。   Further, the observation update of the Kalman filter can be expressed by Expressions 16 and 17.

Figure 2012173190
Figure 2012173190

Figure 2012173190
Figure 2012173190

拡張カルマンフィルター演算部16によって推定された位置、速度、方位角の誤差及びジャイロバイアス誤差及び加速度バイアス誤差の推定値を用いて、移動体の位置、速度、姿勢の補正と共に、ジャイロバイアス誤差及び加速度バイアス誤差の補正をエラー補正部(Error補正部)15で行い、出力部60(表示部61、駆動装置62)に出力し、移動体を制御する。   Using the estimated values of the position, velocity, azimuth angle, and gyro bias error and acceleration bias error estimated by the extended Kalman filter calculation unit 16, the gyro bias error and acceleration are corrected together with the correction of the position, velocity, and posture of the moving object. The bias error is corrected by an error correction unit (Error correction unit) 15 and output to the output unit 60 (display unit 61, drive device 62) to control the moving body.

次に、測位方法についてフローチャートを参照して説明する。
図3は、本実施形態に係る測位方法の1例を示すフローチャートである。図1、図2も参照する。まず、ジャイロセンサー20によって角速度データ、加速度センサー30によって加速度データを検出する(ST1)。これら検出した角速度データ及び加速度データを後述する工程(ST7)において推定されたジャイロバイアス誤差、加速度バイアス誤差に基づき、バイアス補正部11にてバイアス補正を行う(ST2)。
Next, the positioning method will be described with reference to a flowchart.
FIG. 3 is a flowchart showing an example of the positioning method according to the present embodiment. Reference is also made to FIGS. First, angular velocity data is detected by the gyro sensor 20, and acceleration data is detected by the acceleration sensor 30 (ST1). Based on the detected angular velocity data and acceleration data, a bias correction unit 11 performs bias correction based on the gyro bias error and acceleration bias error estimated in step (ST7) described later (ST2).

続いて、バイアス補正された角速度データに基づき姿勢演算部12で移動体の姿勢を演算する(ST3)。バイアス補正された加速度データ(b‐frame)は座標変換部13でn‐frameデータに変換され、位置/速度演算部14に入力される。位置/速度演算部14では、移動体の現時点の位置及び速度を演算する(ST4)。   Subsequently, the posture of the moving body is calculated by the posture calculation unit 12 based on the bias-corrected angular velocity data (ST3). The bias-corrected acceleration data (b-frame) is converted into n-frame data by the coordinate converter 13 and input to the position / velocity calculator 14. The position / speed calculation unit 14 calculates the current position and speed of the moving body (ST4).

次に、乗算型Quaternion誤差モデルを用いる拡張カルマンフィルターの時間更新を行う(ST5)。拡張カルマンフィルターは、時間更新と観測更新の2ステップ交代で構成される。ここで、時間更新とは、状態方程式を用いて、状態ベクトルと誤差共分散を時間毎に更新することであり、外部からの補正情報がなければ、誤差がどんどん蓄積されていく。また、観測更新とは、外部からの補正情報を得て、蓄積された誤差を補正して、状態ベクトルと誤差共分散とを更新することである。   Next, the time update of the extended Kalman filter using the multiplication-type Quaternion error model is performed (ST5). The extended Kalman filter is composed of two steps of time update and observation update. Here, the time update is to update the state vector and the error covariance for each time by using the state equation. If there is no correction information from the outside, errors are accumulated more and more. The observation update is to obtain correction information from the outside, correct the accumulated error, and update the state vector and the error covariance.

次に、GPS観測信号(GPS測位データ)の検出の有無を判定する(ST6)。GPS観測信号が検出されない(No)場合、ST1〜ST5のステップを繰り返す。GPS観測信号を検出(Yes)した場合、拡張カルマンフィルター演算部16において、乗算型Quaternion誤差モデルを用いる拡張カルマンフィルターの観測更新を行い、位置・速度・姿勢、及びジャイロバイアス・加速度バイアスの各誤差を推定する(ST7)。   Next, it is determined whether or not a GPS observation signal (GPS positioning data) is detected (ST6). When the GPS observation signal is not detected (No), the steps ST1 to ST5 are repeated. When a GPS observation signal is detected (Yes), the extended Kalman filter calculation unit 16 updates the observation of the extended Kalman filter using the multiplication-type Quaternion error model, and each error of position / velocity / attitude, and gyro bias / acceleration bias Is estimated (ST7).

次に、ジャイロバイアス値及び加速度バイアス値の更新、位置・速度・姿勢の誤差補正を行い(ST8)、誤差が補正された位置・速度・姿勢データを移動体の出力部60に出力する(ST9)。なお、ST8において、更新されたジャイロバイアス値及び加速度バイアス値は、バイアス補正部11に入力され、更新された姿勢は姿勢演算部12に入力され、更新された位置・速度情報は位置/速度演算部14に入力され、図示するようなループで繰り返される。
(実施例)
Next, the gyro bias value and acceleration bias value are updated and position / velocity / posture error correction is performed (ST8), and the position / velocity / posture data with the corrected error is output to the output unit 60 of the moving body (ST9). ). In ST8, the updated gyro bias value and acceleration bias value are input to the bias correction unit 11, the updated attitude is input to the attitude calculation unit 12, and the updated position / velocity information is the position / speed calculation. Input to the unit 14 and repeated in a loop as shown.
(Example)

次に、本実施形態の具体的な実施例について、乗算型Quaternionと加算型Quaternionの各誤差モデルの誤差推定の比較結果を図面を参照して説明する。まず、比較基礎となる移動体の移動軌跡、速度について説明する。
図4、図5は、移動体のシミュレーションによる移動軌跡と速度の1例を表すグラフである。図4(a)は移動体の移動軌跡(横軸は経度、縦軸は緯度)を表しており、図4(b)は高度の移動(横軸は時間[time:sec]、縦軸は高度[m])を表している。図5(a)は南北方向の移動、図5(b)は東西方向の移動を表している。共に、横軸は時間(time[sec])、縦軸は速度[m/s]を表している。図5(c)は水平面に対する垂直方向(Z軸方向)の速度[m/s]を表している。つまり、本シミュレーションは、移動体は水平方向の移動をしている場合である。
Next, a specific example of the present embodiment will be described with reference to the drawings for comparison results of error estimation of each error model of the multiplication type quaternion and the addition type quaternion. First, the movement trajectory and speed of a moving body that is a comparison basis will be described.
4 and 5 are graphs showing an example of the movement trajectory and speed by simulation of the moving body. 4A shows the movement trajectory of the moving body (horizontal axis is longitude, vertical axis is latitude), FIG. 4B shows altitude movement (horizontal axis is time [time: sec], and vertical axis is Altitude [m]). FIG. 5A shows the movement in the north-south direction, and FIG. 5B shows the movement in the east-west direction. In both cases, the horizontal axis represents time (time [sec]), and the vertical axis represents speed [m / s]. FIG. 5C shows the velocity [m / s] in the direction perpendicular to the horizontal plane (Z-axis direction). That is, this simulation is a case where the moving body is moving in the horizontal direction.

続いて、図4、図5に示した移動体の移動軌跡に基づき、乗算型Quaternion誤差モデルと加算型Quaternion誤差モデルの性能比較結果について説明する。   Next, based on the movement trajectory of the moving body shown in FIGS. 4 and 5, the performance comparison result between the multiplication type quaternion error model and the addition type quaternion error model will be described.

図6は、Z軸方向のシミュレーションによる性能評価の結果を表し、図6(a)は高度の変化、図6(b)はZ軸方向の速度変化[m/s]を表している。なお、図4(b)で示したように、Z軸方向では高度684mで水平移動をした場合であって、加算型Quaternion誤差モデルに比べ、乗算型Quaternion誤差モデルの方が精度がよいことが分かる。   FIG. 6 shows the results of performance evaluation by simulation in the Z-axis direction, FIG. 6A shows the change in altitude, and FIG. 6B shows the change in speed [m / s] in the Z-axis direction. Note that, as shown in FIG. 4B, in the case of horizontal movement at an altitude of 684 m in the Z-axis direction, the multiplication-type Quaternion error model has better accuracy than the addition-type Quaternion error model. I understand.

次に、姿勢誤差のシミュレーション結果について説明する。
図7、図8は、姿勢誤差に係るシミュレーションによる性能評価の結果を表し、図7(a)はロール角誤差(φ[deg])、図7(b)はピッチ角誤差(θ[deg])、図8はヨー角誤差(ψ[deg])を表している。横軸は時間(time[sec])、縦軸は誤差[deg]である。
また、表1に、上記シミュレーション結果から演算して求めた各姿勢の分散値を表している。
Next, a simulation result of the posture error will be described.
7 and 8 show the results of performance evaluation by simulation related to the posture error, FIG. 7A shows the roll angle error (φ [deg]), and FIG. 7B shows the pitch angle error (θ [deg]). 8 shows the yaw angle error (ψ [deg]). The horizontal axis represents time (time [sec]), and the vertical axis represents error [deg].
Table 1 shows the dispersion value of each posture calculated from the simulation result.

Figure 2012173190
Figure 2012173190

図7、図8及び表1からも分かるように、移動体の姿勢誤差、つまり、ロール角誤差、ピッチ角誤差、及びヨー角誤差においても、加算型Quaternionに比べ、乗算型Quaternionの方が誤差が小さくなる。   As can be seen from FIGS. 7 and 8 and Table 1, the error in the attitude of the moving body, that is, the roll angle error, the pitch angle error, and the yaw angle error is more in the multiplication type Quaternion than in the addition type Quaternion. Becomes smaller.

次に、バイアス誤差のシミュレーション結果について説明する。
図9は、バイアス誤差に係るシミュレーションによる性能評価の結果を表し、図9はZ軸のジャイロバイアス誤差(Gyro Bias[rad/S])、図10(a)はX軸の加速度バイアス誤差(Acc Bias[m/s/s])、図10(b)は、Y軸の加速度バイアス誤差(Acc Bias[m/s/s])を表している。共に横軸は時間(time[sec])である。
Next, a simulation result of the bias error will be described.
FIG. 9 shows the results of performance evaluation by simulation related to the bias error, FIG. 9 shows the Z-axis gyro bias error (Gyro Bias [rad / S]), and FIG. 10A shows the X-axis acceleration bias error (Acc). Bias [m / s / s]), FIG. 10B shows the Y-axis acceleration bias error (Acc Bias [m / s / s]). In both cases, the horizontal axis represents time (time [sec]).

図9、図10から分かるように、ジャイロバイアス誤差及び加速度バイアス誤差の推定値は、加算型Quaternionに比べ、乗算型Quaternionの方が誤差が小さくなる。   As can be seen from FIGS. 9 and 10, the estimated values of the gyro bias error and the acceleration bias error are smaller in the multiplication type Quaternion than in the addition type Quaternion.

以上説明した本実施形態の測位システム及び測位方法によれば、慣性センサーデータ(加速度データ及び角速度データ)とGPS測位データを融合して、拡張カルマンフィルター(拡張カルマンフィルター演算部16)によって移動体の位置、速度、姿勢の誤差を推定すると共に、ジャイロセンサー20及び加速度センサー30の各バイアス誤差の推定を行う。また、拡張カルマンフィルターでは、乗算型Quaternion誤差モデルを用いて姿勢誤差を正確にモデリングできることから、位置、速度、姿勢およびセンサーバイアスの誤差を精度よく補正することができる。   According to the positioning system and the positioning method of the present embodiment described above, inertial sensor data (acceleration data and angular velocity data) and GPS positioning data are fused, and an extended Kalman filter (extended Kalman filter calculation unit 16) In addition to estimating position, speed, and posture errors, each bias error of the gyro sensor 20 and the acceleration sensor 30 is estimated. Further, since the extended Kalman filter can accurately model the posture error using the multiplication-type Quaternion error model, the error of the position, velocity, posture, and sensor bias can be corrected with high accuracy.

従って、位置、速度、姿勢及びジャイロバイアス、加速度バイアスの各誤差を精度よく補正することができ、慣性センサーとして、小型、低コスト化が可能ではあるが、バイアス誤差及びランダムドリフトの変動が大きなMEMSセンサーを用いても、高精度、高安定なセンサー出力を得ることができることから、トンネルや屋内などGPS測位データが途切れる場所においても、高精度な測位が可能である。
(実施形態2)
Therefore, the position, velocity, attitude, gyro bias, and acceleration bias errors can be accurately corrected, and the inertial sensor can be reduced in size and cost, but the bias error and random drift fluctuations are large. Even if a sensor is used, highly accurate and highly stable sensor output can be obtained. Therefore, highly accurate positioning is possible even in places where GPS positioning data is interrupted, such as in tunnels and indoors.
(Embodiment 2)

続いて、実施形態2について説明する。前述した実施形態1が、要素数の少ない微小変化単位四元数を導入し、乗算型Quaternionを用いて姿勢誤差をモデリングしていることに対して、実施形態2は、姿勢誤差のモデルだけでなく、位置誤差モデルでも、乗算型Quaternionを用いて誤差をモデリングすることに特徴を有する。具体的には、数式18に示すように誤差のモデリングを行う。   Next, Embodiment 2 will be described. While the first embodiment described above introduces a small change unit quaternion having a small number of elements and models the posture error using the multiplication-type Quaternion, the second embodiment has only the posture error model. In addition, the position error model is characterized in that the error is modeled using the multiplication type Quaternion. Specifically, error modeling is performed as shown in Equation 18.

Figure 2012173190
Figure 2012173190

このように、姿勢の誤差モデリングに加え、位置の誤差モデリングを行うことで、乗算型クォータニオンを用いて正確に誤差のモデリングを行い、位置、速度、及びセンサーバイアスの誤差をより高精度に補正することができる。
(実施形態3)
In this way, in addition to attitude error modeling, position error modeling is performed to accurately perform error modeling using a multiplying quaternion, and errors in position, velocity, and sensor bias are corrected with higher accuracy. be able to.
(Embodiment 3)

続いて、実施形態3について説明する。前述した実施形態1は、GPS信号受信部40からの測位データ(位置、速度、方位角)を観測データとして利用し、拡張カルマンフィルターにより移動体の位置、速度、姿勢の誤差及びジャイロセンサー20、加速度センサー30のバイアス誤差を推定している。この実施形態1に対して、実施形態3では、外部から観測された少なくとも距離情報及び相対速度情報(measurement)を観測情報として用い、乗算型クォータニオン誤差モデルによって移動体の位置・速度・姿勢の誤差、及びジャイロバイアス誤差・加速度バイアス誤差の推定を行い、移動体の位置・速度・姿勢を補正することを特徴とする。   Subsequently, Embodiment 3 will be described. In the first embodiment described above, the positioning data (position, velocity, azimuth) from the GPS signal receiver 40 is used as observation data, and the position, velocity, orientation error of the moving object and the gyro sensor 20 by the extended Kalman filter, The bias error of the acceleration sensor 30 is estimated. In contrast to the first embodiment, in the third embodiment, at least distance information and relative velocity information (measurement) observed from the outside are used as observation information, and the position / velocity / posture error of the moving object is determined by a multiplication type quaternion error model. And gyro bias error / acceleration bias error are estimated, and the position / velocity / posture of the moving body is corrected.

measuamentとしては、擬似距離(psudorange)やドップラー効果(doppler)を用いた相対速度情報を用いることができ、このような手法を用いても、移動体の高精度な測位を行うことができる。
(実施形態4)
As the measurement, relative velocity information using a pseudorange and a Doppler effect can be used, and even using such a method, it is possible to perform highly accurate positioning of the moving object.
(Embodiment 4)

続いて、実施形態4について説明する。実施形態4は、移動体が移動している間には、前述した実施形態1、実施形態2、実施形態3に記載の測位方法を用いてGPS信号受信部40からの測位データ(位置、速度、方位角)、または距離情報、相対速度情報を観測データとして利用して、移動体の位置、速度、姿勢の誤差及びジャイロセンサー20、加速度センサー30の各バイアス誤差を推定し、補正する。
ここで、移動体が静止している間はGPS信号受信部40からの測位データの精度がよくないことから、実施形態4では、以下の観測データを利用して、位置、速度、姿勢の誤差、及び慣性センサーのバイアス誤差を推定する。
(1)速度をゼロ(V=0)とする。
(2)ヨー角は不変(Δψ=0)とする。
(3)移動体が静止しているため、加速度センサー30の出力から次に示す数式も用いてロール(Roll)角とピッチ(Pitch)角を求める。
Next, Embodiment 4 will be described. In the fourth embodiment, while the moving body is moving, the positioning data (position, speed) from the GPS signal receiving unit 40 using the positioning method described in the first, second, and third embodiments described above. , Azimuth), or distance information and relative speed information as observation data, the position, speed, and attitude errors of the moving body and the bias errors of the gyro sensor 20 and acceleration sensor 30 are estimated and corrected.
Here, since the accuracy of the positioning data from the GPS signal receiving unit 40 is not good while the moving body is stationary, in the fourth embodiment, the following observation data is used to detect position, speed, and posture errors. , And estimate the inertial sensor bias error.
(1) The speed is set to zero (V = 0).
(2) The yaw angle is unchanged (Δψ = 0).
(3) Since the moving body is stationary, the roll angle and the pitch angle are obtained from the output of the acceleration sensor 30 using the following mathematical formula.

Figure 2012173190
Figure 2012173190

このようにすれば、数式19によって算出されたロール角とピッチ角と、ゼロ速度、ヨー角不変を観測値として、拡張カルマンフィルターに入れて、移動体が停止している場合においても、姿勢の補正、及びセンサーバイアスの補正ができる。   In this way, the roll angle and pitch angle calculated by Equation 19 and the zero speed and yaw angle invariant are used as observation values in the extended Kalman filter, and even when the moving body is stopped, Correction and sensor bias can be corrected.

1…測位システム、10…演算制御部、11…バイアス補正部、12…姿勢演算部、13…座標変換部、14…位置/速度演算部、15…エラー(Error)補正部、16…拡張カルマンフィルター演算部、20…ジャイロセンサー、30…加速度センサー、40…GPS信号受信部。   DESCRIPTION OF SYMBOLS 1 ... Positioning system, 10 ... Calculation control part, 11 ... Bias correction part, 12 ... Attitude calculation part, 13 ... Coordinate conversion part, 14 ... Position / speed calculation part, 15 ... Error (Error) correction part, 16 ... Extended Kalman Filter operation unit, 20 ... gyro sensor, 30 ... acceleration sensor, 40 ... GPS signal receiving unit.

Claims (5)

慣性センサーデータとGPS測位データとに基づきジャイロバイアス誤差の推定及び加速度バイアス誤差の推定と、位置誤差・速度誤差・方位誤差の推定と、を行う拡張カルマンフィルター演算部と、
前記ジャイロバイアス誤差に基づき角速度データのバイアス補正を行い、かつ、前記加速度バイアス誤差に基づき加速度データのバイアス補正を行うバイアス補正部と、
前記バイアス補正された角速度データに基づき姿勢データを演算する姿勢演算部と、
前記バイアス補正された加速度データおよび前記姿勢データに基づきナビゲーションフレームの加速度データを出力する座標変換部と、
前記ナビゲーションフレームの加速度データに基づき位置・速度を演算する位置/速度演算部と、
前記位置/速度演算部が演算した位置・速度および前記姿勢データを、前記拡張カルマンフィルター演算部から出力された位置・速度・方位の推定誤差に基づいて補正するエラー補正部と、
を有し、
前記拡張カルマンフィルター演算部は、前記位置/速度演算部が演算した前記位置・速度と、前記姿勢演算部が演算した前記姿勢データとに基いて、乗算型クォータニオン誤差モデルを用いて前記ジャイロバイアス誤差、前記加速度バイアス誤差および前記位置誤差・速度誤差・方位誤差の推定を行うことを特徴とする測位システム。
An extended Kalman filter calculation unit for performing gyro bias error estimation and acceleration bias error estimation, position error / velocity error / azimuth error estimation based on inertial sensor data and GPS positioning data;
A bias correction unit that performs bias correction of angular velocity data based on the gyro bias error, and performs bias correction of acceleration data based on the acceleration bias error;
An attitude calculation unit for calculating attitude data based on the bias-corrected angular velocity data;
A coordinate converter that outputs acceleration data of a navigation frame based on the bias-corrected acceleration data and the attitude data;
A position / velocity calculation unit for calculating a position / velocity based on the acceleration data of the navigation frame;
An error correction unit that corrects the position / velocity and the posture data calculated by the position / velocity calculation unit based on the estimation error of the position / velocity / azimuth output from the extended Kalman filter calculation unit;
Have
The extended Kalman filter calculation unit uses the multiplication quaternion error model based on the position / velocity calculated by the position / velocity calculation unit and the attitude data calculated by the attitude calculation unit. A positioning system that estimates the acceleration bias error and the position error / velocity error / azimuth error.
慣性センサーデータとGPS測位データとを融合して移動体の測位を行う測位方法であって、
前記慣性センサーデータに基づき、乗算型クォータニオン誤差モデルを用いる拡張カルマンフィルターの時間更新を行う工程と、
前記GPS測位データに基づき、前記拡張カルマンフィルターの観測更新を行い、位置誤差・速度誤差・方位角誤差、及びジャイロバイアス誤差・加速度バイアス誤差を推定する工程と、
前記推定された位置誤差・速度誤差・方位角誤差に基づき、位置・速度・姿勢の各誤差を補正する工程と、
推定されたジャイロバイアス誤差・加速度バイアス誤差に基づき、慣性センサーデータを補正する工程と
を有し、
前記移動体の位置・速度・姿勢のうち少なくともいずれか一つを求めることを特徴とする測位方法。
A positioning method in which inertial sensor data and GPS positioning data are fused to perform positioning of a moving object,
Performing a time update of an extended Kalman filter using a multiplying quaternion error model based on the inertial sensor data;
Updating the extended Kalman filter based on the GPS positioning data, estimating a position error, velocity error, azimuth error, and gyro bias error / acceleration bias error;
Correcting each position / velocity / attitude error based on the estimated position error / velocity error / azimuth angle error;
Correcting inertial sensor data based on the estimated gyro bias error / acceleration bias error, and
A positioning method characterized in that at least one of position, speed, and posture of the moving body is obtained.
請求項2に記載の測位方法において、
前記方位角誤差及び前記位置誤差の少なくとも一つを、乗算型クォータニオンを用いて誤差モデリングすることを特徴とする測位方法。
The positioning method according to claim 2,
A positioning method, wherein at least one of the azimuth angle error and the position error is error-modeled using a multiplication quaternion.
請求項2に記載の測位方法において、
外部から観測された距離情報及び相対速度情報を観測情報として用い、乗算型クォータニオン誤差モデルによって前記移動体の前記位置・速度・方位角の誤差、及び前記ジャイロバイアス誤差・加速度バイアス誤差の推定を行い、前記移動体の前記位置・速度・姿勢を補正することを特徴とする測位方法。
The positioning method according to claim 2,
Using the distance information and relative velocity information observed from the outside as observation information, the multiplication type quaternion error model is used to estimate the position / velocity / azimuth angle error of the moving body, and the gyro bias error / acceleration bias error. A positioning method, wherein the position, speed, and posture of the moving body are corrected.
前記移動体が移動している場合には、請求項2ないし請求項4のいずれか一項に記載の測位方法を用いて測位し、
前記移動体が停止している場合には、速度を0、ヨー角を不変として、さらに加速度センサーの検出値からロール角とピッチ角とを演算して観測値とし、前記観測値に基づき、姿勢の補正及び前記ジャイロバイアス誤差及び前記加速度バイアス誤差の補正を行うことを特徴とする測位方法。
When the moving body is moving, positioning is performed using the positioning method according to any one of claims 2 to 4,
When the moving body is stopped, the velocity is set to 0, the yaw angle is not changed, the roll angle and the pitch angle are calculated from the detected values of the acceleration sensor to obtain the observed values, and the posture is determined based on the observed values. And a gyro bias error and an acceleration bias error are corrected.
JP2011036806A 2011-02-23 2011-02-23 Positioning system and positioning method Withdrawn JP2012173190A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2011036806A JP2012173190A (en) 2011-02-23 2011-02-23 Positioning system and positioning method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2011036806A JP2012173190A (en) 2011-02-23 2011-02-23 Positioning system and positioning method

Publications (1)

Publication Number Publication Date
JP2012173190A true JP2012173190A (en) 2012-09-10

Family

ID=46976220

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2011036806A Withdrawn JP2012173190A (en) 2011-02-23 2011-02-23 Positioning system and positioning method

Country Status (1)

Country Link
JP (1) JP2012173190A (en)

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457757A (en) * 2014-12-08 2015-03-25 中国矿业大学 Underground moving target positioning method based on front feedback correction
JP2015169582A (en) * 2014-03-07 2015-09-28 リコーエレメックス株式会社 Attitude detector
CN105203098A (en) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor
JP2016090247A (en) * 2014-10-30 2016-05-23 株式会社小野測器 Speed measuring apparatus
CN106324285A (en) * 2016-11-11 2017-01-11 苏州工讯科技有限公司 Testing device and method for smooth level in industrial product logistic process
CN107942364A (en) * 2016-10-13 2018-04-20 阿里巴巴集团控股有限公司 Vehicle positioning method and vehicle positioning system
US10022070B2 (en) 2013-11-11 2018-07-17 Seiko Epson Corporation Integrated circuit including a detection unit for detecting an angular velocity signal of a moving object based on a signal from a sensor
JP2018179573A (en) * 2017-04-05 2018-11-15 日鉄住金レールウェイテクノス株式会社 Travel running speed and position detection method of railway vehicle and stop duration determination method
JP2019002718A (en) * 2017-06-12 2019-01-10 アルパイン株式会社 Present position calculation device, navigation system, and method for calculating pitch angle error
CN109916399A (en) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 A kind of attitude of carrier estimation method under shade
CN110967006A (en) * 2018-09-28 2020-04-07 巴拿拿科技(香港)有限公司 Navigation positioning method and device based on tunnel map, storage medium and terminal equipment
JP2021038989A (en) * 2019-09-03 2021-03-11 株式会社ゼンリンデータコム Vehicle-mounted device, state estimation method, and program
CN114440881A (en) * 2022-01-29 2022-05-06 海南大学 Unmanned vehicle positioning method integrating multi-source sensor information
US11408735B2 (en) 2018-01-05 2022-08-09 Rohm Co., Ltd. Positioning system and positioning method

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10022070B2 (en) 2013-11-11 2018-07-17 Seiko Epson Corporation Integrated circuit including a detection unit for detecting an angular velocity signal of a moving object based on a signal from a sensor
JP2015169582A (en) * 2014-03-07 2015-09-28 リコーエレメックス株式会社 Attitude detector
JP2016090247A (en) * 2014-10-30 2016-05-23 株式会社小野測器 Speed measuring apparatus
CN104457757A (en) * 2014-12-08 2015-03-25 中国矿业大学 Underground moving target positioning method based on front feedback correction
CN105203098A (en) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor
CN107942364A (en) * 2016-10-13 2018-04-20 阿里巴巴集团控股有限公司 Vehicle positioning method and vehicle positioning system
CN106324285A (en) * 2016-11-11 2017-01-11 苏州工讯科技有限公司 Testing device and method for smooth level in industrial product logistic process
JP2018179573A (en) * 2017-04-05 2018-11-15 日鉄住金レールウェイテクノス株式会社 Travel running speed and position detection method of railway vehicle and stop duration determination method
JP2019002718A (en) * 2017-06-12 2019-01-10 アルパイン株式会社 Present position calculation device, navigation system, and method for calculating pitch angle error
US11408735B2 (en) 2018-01-05 2022-08-09 Rohm Co., Ltd. Positioning system and positioning method
CN110967006A (en) * 2018-09-28 2020-04-07 巴拿拿科技(香港)有限公司 Navigation positioning method and device based on tunnel map, storage medium and terminal equipment
CN109916399A (en) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 A kind of attitude of carrier estimation method under shade
CN109916399B (en) * 2019-04-04 2019-12-24 中国人民解放军火箭军工程大学 Carrier attitude estimation method under shadow
JP2021038989A (en) * 2019-09-03 2021-03-11 株式会社ゼンリンデータコム Vehicle-mounted device, state estimation method, and program
JP7272910B2 (en) 2019-09-03 2023-05-12 株式会社ゼンリンデータコム In-vehicle device, state estimation method and program
CN114440881A (en) * 2022-01-29 2022-05-06 海南大学 Unmanned vehicle positioning method integrating multi-source sensor information
CN114440881B (en) * 2022-01-29 2023-05-30 海南大学 Unmanned vehicle positioning method integrating multi-source sensor information

Similar Documents

Publication Publication Date Title
JP2012173190A (en) Positioning system and positioning method
CN111207774B (en) Method and system for laser-IMU external reference calibration
JP6094026B2 (en) Posture determination method, position calculation method, and posture determination apparatus
CN106959110B (en) Cloud deck attitude detection method and device
JP7299261B2 (en) Vehicle dead reckoning method, apparatus, device, storage medium, and program
JP5328252B2 (en) Position detection apparatus and position detection method for navigation system
JP4876204B2 (en) Small attitude sensor
JP4199553B2 (en) Hybrid navigation device
JP2012185111A (en) Positioning device and positioning method
WO2019071916A1 (en) Antenna beam attitude control method and system
US11408735B2 (en) Positioning system and positioning method
CN110325822B (en) Cradle head pose correction method and cradle head pose correction device
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
JP2007276507A (en) Mobile element controller and mobile element control method
JP2007232443A (en) Inertia navigation system and its error correction method
JP2012193965A (en) Position estimating device, and position estimating method and position estimating position program for position estimating device
JP2016033473A (en) Position calculation method and position calculation device
JP2019078560A (en) Gyro sensor offset correcting device, offset correction program, and pedestrian autonomous navigation device
CN108627152A (en) A kind of air navigation aid of the miniature drone based on Fusion
JP5511088B2 (en) Portable device, program and method for correcting gravity vector used for autonomous positioning
CN111307114B (en) Water surface ship horizontal attitude measurement method based on motion reference unit
JP2009250778A (en) Repeated calculation control method and device in kalman filter processing
Kao et al. Design and analysis of an orientation estimation system using coplanar gyro-free inertial measurement unit and magnetic sensors
JP2015017858A (en) Reference value generation device and reference value generation method
CN114252073B (en) Robot attitude data fusion method

Legal Events

Date Code Title Description
A300 Withdrawal of application because of no request for examination

Free format text: JAPANESE INTERMEDIATE CODE: A300

Effective date: 20140513