WO2023058261A1 - 位置補正装置 - Google Patents

位置補正装置 Download PDF

Info

Publication number
WO2023058261A1
WO2023058261A1 PCT/JP2022/014317 JP2022014317W WO2023058261A1 WO 2023058261 A1 WO2023058261 A1 WO 2023058261A1 JP 2022014317 W JP2022014317 W JP 2022014317W WO 2023058261 A1 WO2023058261 A1 WO 2023058261A1
Authority
WO
WIPO (PCT)
Prior art keywords
vector
gnss
calculated
acquired
calculation unit
Prior art date
Application number
PCT/JP2022/014317
Other languages
English (en)
French (fr)
Inventor
宏臣 荒金
和樹 嘉屋
Original Assignee
東京計器株式会社
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 東京計器株式会社 filed Critical 東京計器株式会社
Publication of WO2023058261A1 publication Critical patent/WO2023058261A1/ja

Links

Images

Classifications

    • 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/40—Correcting position, velocity or attitude
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Definitions

  • Embodiments of the present invention relate to a technique for correcting a position measured by GNSS (Global Navigation Satellite System).
  • GNSS Global Navigation Satellite System
  • position jump occurs in which the measured position changes significantly due to radio wave interruption from the GNSS satellite, multipath, satellite switching, etc.
  • a position output device characterized by calculating a corrected position vector for correcting the current position when it is determined that a (see Patent Document 1).
  • position jumps exceeding a preset threshold value are corrected, but smaller position errors such as position jumps that do not reach the threshold value and position deviations that gradually change from the original position are corrected. is not corrected.
  • a problem to be solved by the embodiments of the present invention is to provide a technique capable of reducing smaller positional errors.
  • a position correction device for correcting a current position of a mobile object output by a GNSS receiver, wherein the GNSS position and velocity of the mobile object are periodically acquired by the GNSS receiver output by the GNSS receiver.
  • a data acquisition unit, a predicted position calculation unit that calculates the predicted position of the moving object based on the previously acquired speed of the moving object, and a GNSS position acquired in the current period and based on the previously acquired speed of the moving object When a relative position vector indicating a relative position to the predicted position is calculated, and the rate of change from the relative position vector calculated in the previous cycle to the relative position vector calculated in the current cycle is less than a predetermined speed threshold and an error determination unit that determines that no error has occurred in the acquired GNSS position, and if it is determined that no error has occurred in the GNSS position, the relative position vector calculated in the current period and the previous calculation a correction amount calculation unit for calculating a corrected velocity vector, which is a velocity vector based on the calculated relative position vector and the time from
  • FIG. 2 is a block diagram showing the hardware configuration of the position correcting device according to the first embodiment;
  • FIG. 2 is a block diagram showing the functional configuration of the position correcting device according to the first embodiment;
  • FIG. 4 is a flow chart showing the operation of the position correction device according to the first embodiment; 4 is a flowchart showing the operation of error determination processing; 4 is a flow chart showing the operation of a corrected velocity vector calculation process; 4 is a flowchart showing initialization processing according to the first embodiment;
  • FIG. 11 is a block diagram showing the hardware configuration of a position correction device according to a second embodiment;
  • FIG. 3 is a block diagram showing the hardware configuration of the inertial measurement device;
  • FIG. FIG. 6 is a block diagram showing the functional configuration of a position correction device according to a second embodiment;
  • FIG. 9 is a flow chart showing the operation of the position correcting device according to the second embodiment; 9 is a flowchart showing operations of initialization processing according to the second embodiment;
  • FIG. 1 and 2 are block diagrams respectively showing the hardware configuration and functional configuration of the position correction device according to this embodiment.
  • a position correction device 1 is a device for correcting position information of a mobile body, and as shown in FIG. configured to In the present embodiment, the position correction device 1 is provided in a mobile object and is connected to the GNSS receiver 2 by wire. can be
  • the position correction device 1 includes a CPU (Central Processing Unit) 11, a RAM (Random Access Memory) 12, a storage device 13, and an input/output unit 14 as hardware.
  • the CPU 11 and RAM 12 cooperate to realize various functions described later.
  • the storage device 13 stores various data used by functions realized by the CPU 11 and the RAM 12 .
  • the input/output unit 14 is an interface for exchanging data with an external device such as the GNSS receiver 2 .
  • the position correction device 1 has, as functions, a GNSS data acquisition unit 101, a predicted position calculation unit 102, an error determination unit 103, a correction amount calculation unit 104, a current position calculation unit 105, and a movement determination unit 106 .
  • the GNSS data acquisition unit 101 acquires GNSS data output by the GNSS receiver 2 at a constant update period (for example, approximately 200 ms) based on the GNSS signal.
  • This GNSS data includes a GNSS position indicating the position of the mobile object and a GNSS velocity indicating the speed of the mobile object.
  • the predicted position calculator 102 calculates the predicted position of the moving object based on the GNSS data acquired by the GNSS data acquirer 101 . This predicted position is an estimated position of the moving object.
  • the error determination unit 103 determines whether or not there is an error in the GNSS position based on the rate of change of the relative position vector indicating the relative position between the GNSS position and the predicted position.
  • the correction amount calculation unit 104 calculates a correction velocity vector based on the relative position vector.
  • the current position calculation unit 105 calculates the current position of the moving object by correcting the predicted position using the integrated value of the corrected velocity vector.
  • the movement determination unit 106 determines whether or not the moving body is moving in the initialization process described later.
  • FIG. 3 is a flow chart showing the operation of the position correction device according to this embodiment. It is assumed that initialization processing is executed prior to the operation shown in FIG. 3, and for the sake of explanation, this initialization processing will be described in detail later. Moreover, the operation
  • the GNSS data acquisition unit 101 determines whether or not the GNSS data output from the GNSS receiver 2 has been acquired (S101).
  • the GNSS data acquisition unit 101 acquires the GNSS velocity and GNSS position of the moving object included in the GNSS data (S102).
  • the predicted position calculation unit 102 calculates the predicted position P PRE used in subsequent operation cycles (S103). If the predicted position P PRE has not been calculated since the position correction device 1 was activated, the GNSS position is used as the initial value of the predicted position P PRE .
  • the predicted position P PRE used in subsequent operating cycles is the predicted position used in the current operating cycle, that is, the predicted position calculated in the previous operating cycle, plus the movement vector of the moving object based on the GNSS velocity. is. Specifically, the predicted position P PRE is calculated by the following formula.
  • t is the current time indicated as the time from the activation of the position correcting device 1 to the current operation cycle, specifically, the number of operation cycles multiplied by the time of the operation cycle.
  • ⁇ t is an acquisition time, which is the time from when the GNSS data was output by the GNSS receiver 2 last time to the current operation cycle.
  • v(t) denotes the GNSS velocity at time t, and the bold letters in the above equation represent vectors.
  • P PRE (t ⁇ t) indicates the previously calculated predicted position
  • v(t ⁇ t) indicates the previously output GNSS velocity.
  • the error determination unit 103 executes error determination processing for determining whether or not an error has occurred in the GNSS position based on the predicted position calculated for the GNSS data acquired last time ( S104). Note that the error determination processing will be described in detail later.
  • the correction amount calculation unit 104 calculates the corrected velocity vector V cor when it is determined by the error determination process that no error has occurred in the GNSS position, and the reliability of the current position of the moving body is executed (S105).
  • the current position calculation unit 105 integrates the correction speed vector V cor obtained from the movement of the position correction device 1 to the current operation cycle over time t, as shown in the following equation.
  • the corrected position vector P cor is calculated (S106).
  • the current position calculation unit 105 calculates the corrected current position of the moving object by subtracting the corrected position vector P cor from the predicted position P PRE in the current operation period (S107).
  • step S101 if the GNSS data is not acquired (S101, NO), the GNSS data acquisition unit 101 determines again whether or not the GNSS data output from the GNSS receiver 2 has been acquired (S101).
  • FIG. 4 is a flow chart showing the operation of error determination processing.
  • the error determination unit 103 calculates a relative position vector P GNSS_PRE indicating the relative position between the GNSS position and the predicted position P PRE (S201).
  • This relative position vector is a position vector whose start point is the GNSS position acquired in the current operation cycle and whose end point is the predicted position P PRE calculated in the current operation cycle.
  • the error determination unit 103 converts the relative position vector P GNSS_PRE (t ⁇ T) calculated in the past operation period from the latest operation period in which the GNSS data was acquired to the relative position vector P GNSS_PRE calculated in the current operation period.
  • a change speed V GNSS_PRE to (t) is calculated by the following formula (S202).
  • ⁇ T is the time from when the relative position vector P GNSS_PRE (t ⁇ T) is calculated to when the relative position vector P GNSS_PRE (t) is calculated, and in this embodiment, ⁇ T is set to several seconds. .
  • the error determination unit 103 determines whether or not the speed of change V GNSS_PRE is less than a preset speed threshold (S203).
  • the error determination unit 103 determines that no position error has occurred in the acquired GNSS position (S204).
  • the error determination unit 103 determines that the acquired GNSS position has a position error (S205).
  • a smaller position error can be generated compared to the determination based on the distance of change of the GNSS position. can be detected.
  • FIG. 5 is a flow chart showing the operation of the correction velocity vector calculation process.
  • the correction amount calculation unit 104 determines whether or not it is determined by the error determination process that a position error has occurred in the GNSS position (S301).
  • the correction amount calculation unit 104 decreases the position error occurrence degree indicating the degree of occurrence of the position error (S302). In this embodiment, it is assumed that the position error occurrence degree is reset to 0 when the position correction device 1 is started.
  • the correction amount calculation unit 104 calculates a filter value PF GNSS_PRE based on the relative position vector P GNSS_PRE (S303), and calculates a correction velocity vector V cor based on this filter value PF GNSS_PRE (S304).
  • the filter value PF GNSS_PRE is calculated by reducing high-frequency components of the north-south position and the east-west position, which are components of the relative position vector P GNSS_PRE , by a low-pass filter.
  • the corrected velocity vector V cor is calculated by the following equation.
  • PF GNSS_PRE (t) is the filter value calculated in the current operation period
  • PF GNSS_PRE (t ⁇ t) is the filter value calculated in the operation period in which the previous GNSS data was acquired.
  • the correction amount calculation unit 104 determines whether or not the position error occurrence rate in the current operation cycle is less than a preset position error occurrence rate threshold (S305).
  • the correction amount calculation unit 104 sets the reliability of the current position calculated in the current operation cycle to "high” (S306). On the other hand, if the position error occurrence rate is not less than the position error occurrence rate threshold (S305, NO), the correction amount calculation unit 104 sets the reliability of the current position calculated in the current operation cycle to "low” (S307). ).
  • the reliability is output from the position correction device 1 in association with the current position, and used in external devices together with the current position. For example, when the reliability of the current position is "high”, the external device adopts the current position, and when it is "low”, it does not adopt the current position. It should be noted that the reliability may be indicated by at least two levels or more.
  • step S301 if a position error occurs in the GNSS position (S301, YES), the correction amount calculation unit 104 increases the position error occurrence rate (S308), It is determined whether or not the position error occurrence rate in the cycle is less than a preset position error occurrence rate threshold (S305).
  • FIG. 6 is a flowchart showing initialization processing according to this embodiment.
  • the initialization process is a process that is executed immediately after the position correction device 1 is activated, and is a process that calculates the initial value of the corrected velocity vector V cor .
  • the GNSS data acquisition unit 101 determines whether or not the GNSS data output from the GNSS receiver 2 has been acquired (S501).
  • the movement determination unit 106 determines whether or not the mobile body is moving based on the GNSS speed of the mobile body included in the GNSS data (S502). Here, the movement determination unit 106 determines that the moving object is moving when the GNSS speed is equal to or greater than a preset threshold.
  • the static initialization process is a process of calculating a corrected velocity vector from the average value of the velocity vector of the moving object based on the GNSS velocity, and this average value is calculated for each static initialization process. Also, in the static initialization process, the initialization progress rate indicating the progress rate of initialization is increased. A method of calculating the initialization progress rate will be described later.
  • the error determination unit 103 and the correction amount calculation unit 104 execute movement initialization processing (S504).
  • a corrected velocity vector is calculated by processes corresponding to steps S103, S104 and S105.
  • the initialization progress rate is increased in the same manner as in the static initialization process.
  • the initialization progress rate PR is calculated by the following formula.
  • T stop is the total time spent executing static initialization processing
  • T move is the total time spent executing movement initialization processing
  • T_S is the initial value of the corrected velocity vector obtained only by static initialization processing. This is the time required for calculation
  • T_M is the time required for calculating the initial value of the corrected velocity vector only by the movement initialization process.
  • the correction amount calculation unit 104 determines whether the initialization progress rate has reached 100% (S505).
  • the initialization progress rate is calculated as the sum of the progress rate due to the stationary initialization process and the progress rate due to the movement initialization process, as shown in the above formula. If the moving state of the moving object changes while the initialization progress rate does not reach 100%, the corrected velocity vector being calculated is taken over. Specifically, the corrected velocity vector calculated by the static initialization process is taken over by the movement initialization process, or the corrected velocity vector calculated by the movement initialization process is taken over by the static initialization process.
  • the correction amount calculation unit 104 sets the correction velocity vector being calculated by the stationary initialization process or the movement initialization process to the initial value (S506).
  • the GNSS data acquisition unit 101 again determines whether or not the GNSS data output from the GNSS receiver 2 has been acquired (S501). .
  • step S501 if GNSS data is not acquired (S501, NO), the correction amount calculation unit 104 decreases the initialization progress rate by a predetermined value (S507).
  • the predicted position and the corrected velocity vector are obtained using the GNSS velocity.
  • a predicted position and a corrected velocity vector may be obtained using the velocity of the moving object obtained.
  • FIG. 7 and 9 are block diagrams showing the hardware configuration and functional configuration of the position correction device according to this embodiment.
  • FIG. 8 is a block diagram showing the hardware configuration of the inertial measurement device.
  • the input/output unit 14 transmits and receives data not only to the GNSS receiver 2 but also to the inertial measurement device 3 as an external device. It is different from the position correction device 1 according to the first embodiment in that respect.
  • the inertial measurement device 3 is a measurement device provided in a mobile body together with the GNSS receiver 2, and includes a gyro sensor 31, an acceleration sensor 32, a magnetic direction sensor 33, and a calculation unit 34.
  • the gyro sensor 31 detects angular velocities around three axes including the X, Y and Z axes and outputs gyro signals.
  • the acceleration sensor 32 detects acceleration in three axial directions and outputs acceleration signals.
  • the magnetic azimuth sensor 33 detects a magnetic azimuth angle and outputs a magnetic azimuth signal.
  • the calculation unit 34 calculates the X-axis from the body coordinate system of the moving body to which the sensor is attached, based on the gyro signal, the acceleration signal, and the magnetic direction signal, at a constant update period shorter than the update period in the GNSS receiver 2. Calculating a coordinate transformation matrix for transformation into a local coordinate system in which the direction is north and the Z-axis direction coincides with the direction of gravity, the coordinate transformation is performed, and the acceleration is integrated to calculate the inertial velocity Calculate the inertial position by integrating .
  • the calculation unit 34 also outputs inertia data including inertial velocity and inertial position.
  • the calculation unit 34 when calculating the coordinate transformation matrix, the effect of motion acceleration is removed using the gravitational acceleration detected by the acceleration sensor 32 and the GNSS velocity of the moving object from the GNSS receiver 2.
  • the level error and the orientation error can be eliminated.
  • the removal of such level errors and azimuth errors is disclosed in Japanese Patent No. 4615287, for example.
  • the position correction device 1A differs from the position correction device 1 in that it further includes an inertia data acquisition unit 201 as a functional configuration.
  • the inertial data acquisition unit 201 acquires inertial data output by the inertial measurement device 3 at a constant update cycle.
  • the predicted position calculation unit 102 calculates the predicted position based on the GNSS data and the inertia data, and the movement determination unit 106 determines whether the moving object is moving based on the GNSS data or the inertia data. It differs from the first embodiment.
  • FIG. 10 is a flow chart showing the operation of the position correction device according to this embodiment. It is assumed that the inertial data output cycle of the inertial measurement device is shorter than the GNSS data output cycle of the GNSS receiver and longer than the operation cycle of the position correction device.
  • the inertia data acquisition unit 201 determines whether or not the inertia data output from the inertial measurement device 3 has been acquired (S601).
  • the inertial data acquisition unit 201 acquires the inertial position and inertial velocity of the moving body included in the inertial data (S602).
  • the GNSS data acquisition unit 101 determines whether or not the GNSS data output from the GNSS receiver 2 has been acquired (S101).
  • step S102 When GNSS data is acquired (S101, YES), the operation of step S102 is performed as in the first embodiment. Further, in the operations from step S103 to step S105, while performing the same operations as in the first embodiment, the predicted position is calculated based on the inertial velocity instead of the GNSS velocity (S103), When it is determined that there is no error in the GNSS position by executing miscalculation determination processing for determining whether or not there is an error in the GNSS position based on the predicted position calculated for the inertial data (S104) Another difference from the first embodiment is that a correction velocity vector is calculated based on the rate of change in the relative position between the predicted position calculated based on the inertial velocity and the GNSS position (S105). After calculating the corrected velocity vector, the operations from step S106 to step S107 are performed in the same manner as in the first embodiment.
  • the predicted position calculation unit 102 calculates the predicted position based on the inertial velocity (S103A).
  • the predicted position calculation unit 102 calculates a new predicted position by adding to the current position a vector obtained by multiplying the inertial velocity by the time from when the inertia data was obtained last time to the current time. Note that the predicted position calculation unit 102 calculates a new predicted position using the most recent predicted position among the already calculated predicted positions or the inertial position as the current position.
  • step S601 if the inertia data is not acquired (S601, NO), the inertia data acquisition unit 201 again determines whether or not the inertia data output from the inertial measurement device 3 has been acquired (S601).
  • step S106 After calculating the predicted position based on the inertial velocity, the operations from step S106 to step S107 are executed. At this time, in step S106, the corrected position vector is calculated using the corrected velocity vector based on the most recently acquired inertia data. In step S107, the current position is calculated by correcting the predicted position based on the inertial velocity with the corrected position vector calculated in step S106.
  • the predicted position can be calculated at the timing when the GNSS data is not acquired, and the predicted position can be calculated based on the most recently acquired inertial data.
  • the current position can be calculated by correcting with the corrected position vector calculated using the corrected velocity vector.
  • FIG. 11 is a flowchart showing the operation of initialization processing according to this embodiment.
  • the inertia data acquisition unit 201 determines whether or not the inertia data output from the inertial measurement device 3 has been acquired (S701).
  • the inertia data acquisition unit 201 determines again whether or not the inertia data output from the inertial measurement device 3 has been acquired (S701).
  • step S501 the operations after step S501 are executed as in the first embodiment, but the stationary initialization process and the movement initialization process are performed in the first step. Different from the embodiment.
  • the stationary initialization process when inertial data is acquired differs from the first embodiment in that the average value of the velocity vector of the moving object is calculated based on the inertial velocity instead of the GNSS velocity. Further, the movement initialization processing when the inertia data is acquired is the predicted position calculated based on the inertia speed instead of the GNSS speed when calculating the correction speed vector by the processing corresponding to steps S103, S104 and S105. is used, which is different from the first embodiment. Note that the initial value based on the inertial data is calculated separately from the initial value based on the GNSS data.
  • the initial value can be calculated at the timing when the GNSS data is not acquired.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

現周期において取得された移動体の速度に基づいて移動体の予測位置を算出する予測位置算出部と、現周期において取得されたＧＮＳＳ位置と前回取得された速度に基づく予測位置との相対位置を示す相対位置ベクトルを算出し、前回より過去の周期において算出された相対位置ベクトルから現周期において算出された相対位置ベクトルへの変化速度が所定の速度閾値未満である場合に取得されたＧＮＳＳ位置に誤差が発生していないと判定する誤差判定部と、ＧＮＳＳ位置に誤差が発生していないと判定された場合、相対位置ベクトルと、前回算出された相対位置ベクトルと、前回に相対位置ベクトルが算出された時点から現周期までの時間とに基づく速度ベクトルである補正速度ベクトルを算出する補正量算出部と、現周期において算出された予測位置を補正速度ベクトルの積分値により補正した位置を移動体の現在位置として算出する現在位置算出部とを備える。

Description

位置補正装置
　本発明の実施形態は、ＧＮＳＳ（Ｇｌｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍ）による測定位置を補正する技術に関する。
　従来、ＧＮＳＳによる移動体の位置測定には、ＧＮＳＳ衛星からの電波遮断、マルチパス、衛星の切り替わりなどに起因して、「位置飛び」と呼ばれる、測定位置が大きく変化する現象が発生する。
　このような位置飛びによる位置誤差を低減させる技術として、衛星航法測定位置のデータに基づく現在位置と以前の現在位置に基づく予測位置との位置差から位置飛びが発生したかを判定し、位置飛びが発生したと判定された場合に、現在位置を補正するための補正位置ベクトルを算出し、予測位置を補正位置ベクトルによって補正したものを現在位置とすることを特徴とする位置出力装置、が知られている（特許文献１参照）。
特許第５１２２６７８号公報
　上述の位置出力装置によれば、予め設定された閾値以上の位置飛びは補正されるが、閾値に達しない位置飛びや、本来の位置から徐々に変化する位置ずれのような、より小さな位置誤差は補正されない、という問題がある。
　本発明の実施形態が解決しようとする課題は、より小さな位置誤差を低減することができる技術を提供することを目的とする。
　一実施形態において、ＧＮＳＳ受信機により出力される移動体の現在位置を補正する位置補正装置であって、前記ＧＮＳＳ受信機により出力されるＧＮＳＳ位置と前記移動体の速度を周期的に取得するＧＮＳＳデータ取得部と、前回取得された移動体の速度に基づいて前記移動体の予測位置を算出する予測位置算出部と、現周期において取得されたＧＮＳＳ位置と前回取得された移動体の速度に基づく予測位置との相対位置を示す相対位置ベクトルを算出し、前回より過去の周期において算出された相対位置ベクトルから現周期において算出された相対位置ベクトルへの変化速度が所定の速度閾値未満である場合に前記取得されたＧＮＳＳ位置に誤差が発生していないと判定する誤差判定部と、ＧＮＳＳ位置に誤差が発生していないと判定された場合、現周期において算出された相対位置ベクトルと、前回算出された相対位置ベクトルと、前回に相対位置ベクトルが算出された時点から現周期までの時間とに基づく速度ベクトルである補正速度ベクトルを算出する補正量算出部と、現周期において算出された予測位置を前記補正速度ベクトルの積分値により補正した位置を前記移動体の現在位置として算出する現在位置算出部とを備えた。
第１の実施形態に係る位置補正装置のハードウェア構成を示すブロック図である。 第１の実施形態に係る位置補正装置の機能構成を示すブロック図である。 第１の実施形態に係る位置補正装置の動作を示すフローチャートである。 誤差判定処理の動作を示すフローチャートである。 補正速度ベクトル算出処理の動作を示すフローチャートである。 第１の実施形態に係る初期化処理を示すフローチャートである。 第２の実施形態に係る位置補正装置のハードウェア構成を示すブロック図である。 慣性計測装置のハードウェア構成を示すブロック図である。 第２の実施形態に係る位置補正装置の機能構成を示すブロック図である。 第２の実施形態に係る位置補正装置の動作を示すフローチャートである。 第２の実施形態に係る初期化処理の動作を示すフローチャートである。
　以下、図面を参照しながら、本発明の実施形態について説明する。
＜第１の実施形態＞
（位置補正装置の構成）
　第１の実施形態に係る位置補正装置の構成について説明する。図１、図２は、それぞれ、本実施形態に係る位置補正装置のハードウェア構成、機能構成を示すブロック図である。
　本実施形態に係る位置補正装置１は、移動体の位置情報を補正する装置であり、図１に示すように、移動体に備えられてＧＮＳＳ信号を受信するＧＮＳＳ受信機２とデータを送受可能に構成される。本実施形態においては、位置補正装置１は、移動体に備えられ、ＧＮＳＳ受信機２と有線接続されるものとするが、移動体と離間した位置に設置され、ＧＮＳＳ受信機２と無線接続されても良い。
　図１に示すように、位置補正装置１は、ハードウェアとして、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）１１とＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）１２と、記憶装置１３と、入出力部１４とを備える。ＣＰＵ１１及びＲＡＭ１２は、協働して後述する各種機能を実現する。記憶装置１３は、ＣＰＵ１１及びＲＡＭ１２により実現される機能により用いられる各種データを格納する。入出力部１４は、ＧＮＳＳ受信機２のような外部機器とデータを送受するためのインターフェイスである。
　図２に示すように、位置補正装置１は、機能として、ＧＮＳＳデータ取得部１０１と、予測位置算出部１０２と、誤差判定部１０３と、補正量算出部１０４と、現在位置算出部１０５と、移動判定部１０６とを備える。
　ＧＮＳＳデータ取得部１０１は、ＧＮＳＳ信号に基づいてＧＮＳＳ受信機２が一定の更新周期（例えば、約２００ｍｓ）で出力するＧＮＳＳデータを取得する。このＧＮＳＳデータには、移動体の位置を示すＧＮＳＳ位置と、移動体の速度を示すＧＮＳＳ速度とが含まれる。予測位置算出部１０２は、ＧＮＳＳデータ取得部１０１により取得されたＧＮＳＳデータに基づいて、移動体の予測位置を算出する。この予測位置は、移動体の位置を推定したものである。誤差判定部１０３は、ＧＮＳＳ位置と予測位置との相対位置を示す相対位置ベクトルの変化速度に基づいて、ＧＮＳＳ位置に誤差が生じているか否かを判定する。
　補正量算出部１０４は、誤差判定部１０３によりＧＮＳＳ位置に誤差が生じていないと判定された場合、相対位置ベクトルに基づく補正速度ベクトルを算出する。現在位置算出部１０５は、補正速度ベクトルの積分値により予測位置を補正することによって移動体の現在位置を算出する。移動判定部１０６は、後述する初期化処理において、移動体が移動しているか否かを判定する。
（位置補正装置の動作）
　第１の実施形態に係る位置補正装置の動作について説明する。図３は、本実施形態に係る位置補正装置の動作を示すフローチャートである。なお、図３に示す動作に先立って初期化処理が実行されるものとするが、説明上、この初期化処理については後に詳述する。また、図３に示す動作は、ＧＮＳＳ受信機によるＧＮＳＳデータの出力周期よりも短い動作周期毎に実行されるものとする。
　図３に示すように、まず、ＧＮＳＳデータ取得部１０１は、ＧＮＳＳ受信機２から出力されるＧＮＳＳデータを取得したか否かを判定する（Ｓ１０１）。
　ＧＮＳＳデータが取得された場合（Ｓ１０１，ＹＥＳ）、ＧＮＳＳデータ取得部１０１は、ＧＮＳＳデータに含まれる移動体のＧＮＳＳ速度及びＧＮＳＳ位置を取得する（Ｓ１０２）。
　ＧＮＳＳ速度及びＧＮＳＳ位置の取得後、予測位置算出部１０２は、以降の動作周期において用いられる予測位置ＰＰＲＥを算出する（Ｓ１０３）。なお、位置補正装置１が起動されてから予測位置ＰＰＲＥが算出されていない場合には、予測位置ＰＰＲＥの初期値としてＧＮＳＳ位置が用いられる。
　以降の動作周期で用いられる予測位置ＰＰＲＥは、現動作周期で用いられる予測位置、即ち以前の動作周期で算出された予測位置に対して、ＧＮＳＳ速度に基づく移動体の移動ベクトルを加えたものである。具体的には、予測位置ＰＰＲＥは、下式により算出される。
Figure JPOXMLDOC01-appb-M000001
　ここで、ｔは位置補正装置１が起動されてからの現動作周期までの時間として示される現時刻であり、具体的には動作周期回数に動作周期の時間を乗じたものである。δｔは、取得時間であり、前回ＧＮＳＳ受信機２によりＧＮＳＳデータが出力された時点から現動作周期までの時間である。ｖ（ｔ）は時刻ｔにおけるＧＮＳＳ速度を示し、上式中の太字はベクトルを表す。ＰＰＲＥ（ｔ－δｔ）は前回算出された予測位置を示し、ｖ（ｔ－δｔ）は前回出力されたＧＮＳＳ速度を示す。
　予測位置の算出後、誤差判定部１０３は、前回に取得されたＧＮＳＳデータに対して算出された予測位置に基づいてＧＮＳＳ位置に誤差が生じているか否かを判定する誤差判定処理を実行する（Ｓ１０４）。なお、誤差判定処理については後に詳述する。
　誤差判定処理の実行後、補正量算出部１０４は、誤差判定処理によりＧＮＳＳ位置に誤差が生じていないと判定された場合に補正速度ベクトルＶｃｏｒを算出するとともに、移動体の現在位置に信頼度を設定する補正速度ベクトル算出処理を実行する（Ｓ１０５）。
　補正速度ベクトル算出処理の実行後、現在位置算出部１０５は、下式のように、位置補正装置１が移動されてから現動作周期までに得られた補正速度ベクトルＶｃｏｒを時間ｔで積分することにより補正位置ベクトルＰｃｏｒを算出する（Ｓ１０６）。
Figure JPOXMLDOC01-appb-M000002
　次に、現在位置算出部１０５は、現動作周期における予測位置ＰＰＲＥから補正位置ベクトルＰｃｏｒを減算することにより、補正された移動体の現在位置を算出する（Ｓ１０７）。
　また、ステップＳ１０１において、ＧＮＳＳデータが取得されない場合（Ｓ１０１，ＮＯ）、ＧＮＳＳデータ取得部１０１は、再度、ＧＮＳＳ受信機２から出力されるＧＮＳＳデータを取得したか否かを判定する（Ｓ１０１）。
（誤差判定処理）
　誤差判定処理について説明する。図４は、誤差判定処理の動作を示すフローチャートである。
　図４に示すように、まず、誤差判定部１０３は、ＧＮＳＳ位置と予測位置ＰＰＲＥとの相対位置を示す相対位置ベクトルＰＧＮＳＳ＿ＰＲＥを算出する（Ｓ２０１）。この相対位置ベクトルは、現動作周期において取得されたＧＮＳＳ位置を始点とし、現動作周期において算出された予測位置ＰＰＲＥを終点とした位置ベクトルである。
　次に、誤差判定部１０３は、ＧＮＳＳデータが取得された直近よりも過去の動作周期において算出された相対位置ベクトルＰＧＮＳＳ＿ＰＲＥ（ｔ－ΔＴ）から、現動作周期において算出された相対位置ベクトルＰＧＮＳＳ＿ＰＲＥ（ｔ）への変化速度ＶＧＮＳＳ＿ＰＲＥを、以下の式により算出する（Ｓ２０２）。
Figure JPOXMLDOC01-appb-M000003
　ここで、ΔＴは、相対位置ベクトルＰＧＮＳＳ＿ＰＲＥ（ｔ－ΔＴ）の算出時点から相対位置ベクトルＰＧＮＳＳ＿ＰＲＥ（ｔ）の算出時点までの時間であり、本実施形態においては、ΔＴは数秒に設定される。
　変化速度ＶＧＮＳＳ＿ＰＲＥの算出後、誤差判定部１０３は、変化速度ＶＧＮＳＳ＿ＰＲＥが予め設定された速度閾値未満であるか否かを判定する（Ｓ２０３）。
　変化速度ＶＧＮＳＳ＿ＰＲＥが速度閾値未満である場合（Ｓ２０３，ＹＥＳ）、誤差判定部１０３は、取得されたＧＮＳＳ位置に位置誤差が発生していないと判定する（Ｓ２０４）。
　一方、変化速度ＶＧＮＳＳ＿ＰＲＥが速度閾値未満ではない場合（Ｓ２０３，ＮＯ）、誤差判定部１０３は、取得されたＧＮＳＳ位置に位置誤差が発生していると判定する（Ｓ２０５）。
　このように、相対位置ベクトルＰＧＮＳＳ＿ＰＲＥの変化速度に基づいて、ＧＮＳＳに位置誤差が生じているか否かを判定することによって、ＧＮＳＳ位置の変化距離に基づく判定と比較して、より小さな位置誤差を検出することができる。
（補正速度ベクトル算出処理）
　速度補正量算出処理について説明する。図５は、補正速度ベクトル算出処理の動作を示すフローチャートである。
　図５に示すように、まず、補正量算出部１０４は、誤差判定処理によりＧＮＳＳ位置に位置誤差が発生したと判定されたか否かを判定する（Ｓ３０１）。
　ＧＮＳＳ位置に位置誤差が発生していない場合（Ｓ３０１，ＮＯ）、補正量算出部１０４は、位置誤差が発生した度合いを示す位置誤差発生度を減少させる（Ｓ３０２）。本実施形態において、位置誤差発生度は、位置補正装置１の起動時に０にリセットされるものとする。
　次に、補正量算出部１０４は、相対位置ベクトルＰＧＮＳＳ＿ＰＲＥに基づいてフィルタ値ＰＦＧＮＳＳ＿ＰＲＥを算出し（Ｓ３０３）、このフィルタ値ＰＦＧＮＳＳ＿ＰＲＥに基づいて補正速度ベクトルＶcorを算出する（Ｓ３０４）。ここで、フィルタ値ＰＦＧＮＳＳ＿ＰＲＥは、相対位置ベクトルＰＧＮＳＳ＿ＰＲＥを構成する成分である南北位置、東西位置のそれぞれの高周波成分がローパスフィルタにより逓減されることにより算出される。また、補正速度ベクトルＶcorは下式により算出される。
Figure JPOXMLDOC01-appb-M000004
　ここで、ＰＦＧＮＳＳ＿ＰＲＥ（ｔ）は現動作周期において算出されたフィルタ値であり、ＰＦＧＮＳＳ＿ＰＲＥ（ｔ－δｔ）は前回ＧＮＳＳデータが取得された動作周期において算出されたフィルタ値である。
　また、補正量算出部１０４は、現動作周期における位置誤差発生度が予め設定された位置誤差発生度閾値未満であるか否かを判定する（Ｓ３０５）。
　位置誤差発生度が位置誤差発生度閾値未満である場合（Ｓ３０５，ＹＥＳ）、補正量算出部１０４は、現動作周期において算出される現在位置に対する信頼度を“高”に設定する（Ｓ３０６）。一方、位置誤差発生度が位置誤差発生度閾値未満ではない場合（Ｓ３０５，ＮＯ）、補正量算出部１０４は、現動作周期において算出される現在位置に対する信頼度を“低”に設定する（Ｓ３０７）。
　信頼度は、現在位置に対応付けられて位置補正装置１から出力され、現在位置とともに外部機器において利用される。例えば、外部機器が、現在位置の信頼度が“高”である場合には現在位置を採用し、“低”である場合には現在位置を採用しないといった利用方法が挙げられる。なお、信頼度は、少なくとも２段階以上の度合いにより示されるものであれば良い。
　ステップＳ３０１において、ＧＮＳＳ位置に位置誤差が発生した場合（Ｓ３０１，ＹＥＳ）、補正量算出部１０４は、位置誤差発生度を増加させ（Ｓ３０８）、補正速度ベクトルＶcorを算出せずに、現動作周期における位置誤差発生度が予め設定された位置誤差発生度閾値未満であるか否かを判定する（Ｓ３０５）。
　このように、ＧＮＳＳ位置に位置誤差が発生していない場合のみに補正速度ベクトルＶcorを算出することによって、補正速度ベクトルＶcorに基づいて算出される補正位置ベクトルＰcorの精度を向上させることができる。
（初期化処理）
　第１の実施形態に係る初期化処理について説明する。図６は、本実施形態に係る初期化処理を示すフローチャートである。
　初期化処理は、上述したように、位置補正装置１の起動直後に実行される処理であり、補正速度ベクトルＶcorの初期値を算出する処理である。
　図６に示すように、まず、ＧＮＳＳデータ取得部１０１は、ＧＮＳＳ受信機２から出力されるＧＮＳＳデータを取得したか否かを判定する（Ｓ５０１）。
　ＧＮＳＳデータが取得された場合（Ｓ５０１，ＹＥＳ）、移動判定部１０６は、ＧＮＳＳデータに含まれる移動体のＧＮＳＳ速度に基づいて、移動体が移動中であるか否かを判定する（Ｓ５０２）。ここで、移動判定部１０６は、ＧＮＳＳ速度が予め設定された閾値以上である場合に、移動体が移動中であると判定する。
　移動体が移動中ではない、即ち、静止中である場合（Ｓ５０２，ＮＯ）、補正量算出部１０４は、静止初期化処理を実行する（Ｓ５０３）。静止初期化処理は、ＧＮＳＳ速度に基づく移動体の速度ベクトルの平均値から補正速度ベクトルを算出する処理であり、この平均値は静止初期化処理毎に算出される。また、静止初期化処理においては、初期化の進捗率を示す初期化進捗率が増加される。初期化進捗率の算出方法については後述する。
　一方、移動体が移動中である場合（Ｓ５０２，ＹＥＳ）、誤差判定部１０３及び補正量算出部１０４は、移動初期化処理を実行する（Ｓ５０４）。移動初期化処理は、ステップＳ１０３，Ｓ１０４及びＳ１０５に相当する処理によって補正速度ベクトルを算出する。また、移動初期化処理においては、静止初期化処理と同様に、初期化進捗率が増加される。初期化進捗率ＰＲは、下式によって算出される。
Figure JPOXMLDOC01-appb-M000005
ここで、Ｔｓｔｏｐは静止初期化処理を実行した時間の合計であり、Ｔｍｏｖｅは移動初期化処理を実行した時間の合計であり、Ｔ＿Ｓは静止初期化処理のみにより補正速度ベクトルの初期値の算出に掛かる時間であり、Ｔ＿Ｍは移動初期化処理のみにより補正速度ベクトルの初期値の算出に掛かる時間である。
　静止初期化処理または移動初期化処理の実行後、補正量算出部１０４は、初期化進捗率が１００％に達したか否かを判定する（Ｓ５０５）。初期化進捗率は、上式の通り、静止初期化処理による進捗率と、移動初期化処理による進捗率の合計として算出される。初期化進捗率が１００％に達しない状態において、移動体の移動状態が変化した場合には、算出中の補正速度ベクトルが引き継がれる。具体的には、静止初期化処理により算出される補正速度ベクトルが移動初期化処理に引き継がれるか、または、移動初期化処理により算出される補正速度ベクトルが静止初期化処理に引き継がれる。
　初期化進捗率が１００％に達した場合（Ｓ５０５，ＹＥＳ）、補正量算出部１０４は、静止初期化処理または移動初期化処理により算出中の補正速度ベクトルを初期値に設定する（Ｓ５０６）。
　一方、初期化進捗率が１００％に達しない場合（Ｓ５０５，ＮＯ）、ＧＮＳＳデータ取得部１０１は、再度、ＧＮＳＳ受信機２から出力されるＧＮＳＳデータを取得したか否かを判定する（Ｓ５０１）。
　ステップＳ５０１において、ＧＮＳＳデータが取得されない場合（Ｓ５０１，ＮＯ）、補正量算出部１０４は、初期化進捗率を予め定められた値だけ減少させる（Ｓ５０７）。
　このような初期化処理によれば、位置補正装置１の起動直後においても、補正速度ベクトルに基づく補正位置ベクトルを算出することができる。移動体の移動状態に応じて補正速度ベクトルを算出することにより、より適切な補正速度ベクトルの初期値を得ることができる。
　なお、本実施形態においては、ＧＮＳＳ速度を用いて予測位置及び補正速度ベクトルを求めるものとしたが、これに限らず、ＧＮＳＳ速度に代えて、例えば、車速センサのような速度センサを用いて得られた移動体の速度を用いて予測位置及び補正速度ベクトルを求めるようにしても良い。
＜第２の実施形態＞
（位置補正装置の構成）
　第２の実施形態に係る位置補正装置の構成について説明する。図７、図９は、本実施形態に係る位置補正装置のハードウェア構成、機能構成を示すブロック図である。図８は、慣性計測装置のハードウェア構成を示すブロック図である。
　図７に示すように、本実施形態に係る位置補正装置１Ａは、ハードウェア構成において、入出力部１４が、ＧＮＳＳ受信機２だけでなく、外部機器としての慣性計測装置３とデータを送受する点において、第１の実施形態に係る位置補正装置１とは異なる。
　図８に示すように、慣性計測装置３は、ＧＮＳＳ受信機２とともに移動体に備えられた計測装置であり、ジャイロセンサ３１と、加速度センサ３２と、磁気方位センサ３３と、演算部３４とを備える。ジャイロセンサ３１は、Ｘ軸、Ｙ軸及びＺ軸を含む３軸周りの角速度を検出してジャイロ信号を出力する。加速度センサ３２は、３軸方向の加速度を検出して加速度信号を出力する。磁気方位センサ３３は、磁気方位角を検出して磁気方位信号を出力する。
　演算部３４は、ＧＮＳＳ受信機２における更新周期よりも短い一定の更新周期で、ジャイロ信号、加速度信号、及び磁気方位信号に基づいて、センサが取り付けられた移動体のボディ座標系から、Ｘ軸方向が北方向で且つＺ軸方向が重力方向に一致しているローカル座標系へ変換する座標変換行列を演算し、座標変換を行って加速度を積分することで慣性速度を算出し、さらに慣性速度を積分することにより慣性位置を算出する。また、演算部３４は、慣性速度と慣性位置とを含む慣性データを出力する。
　また、この演算部３４において、座標変換行例を演算する際に、加速度センサ３２により検出された重力加速度及びＧＮＳＳ受信機２からの移動体のＧＮＳＳ速度を用いて運動加速度の影響を除去してレベル誤差を修正し、且つ、ＧＮＳＳ受信機２からのＧＮＳＳ方位及び磁気方位センサ３３からの磁気方位を用いて方位誤差を修正することで、レベル誤差及び方位誤差を除去することができる。なお、このようなレベル誤差及び方位誤差の除去については、例えば、特許第４６１５２８７号に開示される。
　図９に示すように、位置補正装置１Ａは、機能構成として、慣性データ取得部２０１を更に備える点において、位置補正装置１とは異なる。慣性データ取得部２０１は、慣性計測装置３が一定の更新周期で出力する慣性データを取得する。また、予測位置算出部１０２がＧＮＳＳデータ及び慣性データに基づいて予測位置を算出する点、移動判定部１０６がＧＮＳＳデータまたは慣性データに基づいて移動体が移動しているか否かを判定する点が第１の実施形態とは異なる。
（位置補正装置の動作）
　第２の実施形態に係る位置補正装置の動作について説明する。図１０は、本実施形態に係る位置補正装置の動作を示すフローチャートである。なお、慣性計測装置による慣性データの出力周期は、ＧＮＳＳ受信機によるＧＮＳＳデータの出力周期より短く、位置補正装置の動作周期よりも長いものとする。
　図１０に示すように、まず、慣性データ取得部２０１は、慣性計測装置３から出力される慣性データを取得したか否かを判定する（Ｓ６０１）。
　慣性データが取得された場合（Ｓ６０１，ＹＥＳ）、慣性データ取得部２０１は、慣性データに含まれる移動体の慣性位置及び慣性速度を取得する（Ｓ６０２）。
　慣性位置及び慣性速度の取得後、ＧＮＳＳデータ取得部１０１は、ＧＮＳＳ受信機２から出力されるＧＮＳＳデータを取得したか否かを判定する（Ｓ１０１）。
　ＧＮＳＳデータが取得された場合（Ｓ１０１，ＹＥＳ）、第１の実施形態と同様に、ステップＳ１０２の動作が実行される。また、ステップＳ１０３からステップＳ１０５の動作においては、第１の実施形態と同様の動作を実行しながらさらに、ＧＮＳＳ速度に代えて慣性速度に基づいて予測位置を算出し（Ｓ１０３）、前回取得された慣性データに対して算出された予測位置に基づいてＧＮＳＳ位置に誤差が生じているか否かを判定する誤算判定処理を実行して（Ｓ１０４）、ＧＮＳＳ位置に誤差が生じていないと判断された場合に、慣性速度に基づいて算出した予測位置とＧＮＳＳ位置との相対位置の変化速度に基づいて補正速度ベクトルを算出する（Ｓ１０５）点が、第１の実施形態とは異なる。補正速度ベクトルの算出後、ステップＳ１０６からステップＳ１０７の動作は、第１の実施形態と同様の動作が実行される。
　一方、ＧＮＳＳデータが取得されない場合（Ｓ１０１，ＮＯ）、予測位置算出部１０２は、慣性速度に基づいて予測位置を算出する（Ｓ１０３Ａ）。ここで、予測位置算出部１０２は、前回慣性データが取得された時点から現時点までの時間を慣性速度に乗じたベクトルを現在位置に加算することによって、新たな予測位置を算出する。なお、予測位置算出部１０２は、すでに算出された予測位置のうち直近の予測位置、または、慣性位置を現在位置として新たな予測位置を算出する。
　ステップＳ６０１において、慣性データが取得されない場合（Ｓ６０１，ＮＯ）、慣性データ取得部２０１は、再度、慣性計測装置３から出力される慣性データを取得したか否かを判定する（Ｓ６０１）。
　慣性速度に基づく予測位置の算出後、ステップＳ１０６からステップＳ１０７の動作が実行される。この際、ステップＳ１０６においては、直近で取得された慣性データに基づく補正速度ベクトルを用いて補正位置ベクトルが算出される。また、ステップＳ１０７においては、慣性速度に基づく予測位置をステップＳ１０６において算出された補正位置ベクトルにより補正することにより現在位置が算出される。
　このように、予測位置の算出にＧＮＳＳデータだけでなく慣性データも用いることによって、ＧＮＳＳデータが取得されないタイミングにおいて、予測位置を算出することができ、予測位置を直近で取得された慣性データに基づく補正速度ベクトルを用いて算出された補正位置ベクトルで補正することにより、現在位置を算出することができる。
（初期化処理）
　第２の実施形態に係る初期化処理の動作について説明する。図１１は、本実施形態に係る初期化処理の動作を示すフローチャートである。
　図１１に示すように、まず、慣性データ取得部２０１は、慣性計測装置３から出力される慣性データを取得したか否かを判定する（Ｓ７０１）。
　慣性データが取得されない場合（Ｓ７０１，ＮＯ）、慣性データ取得部２０１は、再度、慣性計測装置３から出力される慣性データを取得したか否かを判定する（Ｓ７０１）。
　一方、慣性データが取得された場合（Ｓ７０１，ＹＥＳ）、第１の実施形態と同様に、ステップＳ５０１以降の動作が実行されるが、静止初期化処理及び移動初期化処理の動作が第１の実施形態とは異なる。
　慣性データが取得された場合の静止初期化処理は、ＧＮＳＳ速度に代えて、慣性速度に基づく移動体の速度ベクトルの平均値を算出する点において第１の実施形態とは異なる。また、慣性データが取得された場合の移動初期化処理は、ステップＳ１０３，Ｓ１０４及びＳ１０５に相当する処理によって補正速度ベクトルを算出する際にＧＮＳＳ速度に代えて慣性速度に基づいて算出された予測位置が用いられる点において第１の実施形態とは異なる。なお、慣性データに基づく初期値は、ＧＮＳＳデータに基づく初期値とは別個に算出される。
　このように、初期値としての補正速度ベクトルの算出にＧＮＳＳデータだけでなく慣性データも用いることによって、ＧＮＳＳデータが取得されないタイミングにおいて、初期値を算出することができる。
　本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。この新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
１　位置補正装置
１０１　ＧＮＳＳデータ取得部
１０２　予測位置算出部
１０３　誤差判定部
１０４　補正量算出部
１０５　現在位置算出部

Claims (7)

  1. 　ＧＮＳＳ受信機により出力される移動体の現在位置を補正する位置補正装置であって、
    　前記ＧＮＳＳ受信機によるＧＮＳＳ位置と前記移動体の速度を周期的に取得するデータ取得部と、
    　前回取得された速度に基づいて前記移動体の予測位置を算出する予測位置算出部と、
    　現周期において取得されたＧＮＳＳ位置と前回取得された速度に基づく予測位置との相対位置を示す相対位置ベクトルを算出し、前回より過去の周期において算出された相対位置ベクトルから現周期において算出された相対位置ベクトルへの変化速度が所定の速度閾値未満である場合に前記取得されたＧＮＳＳ位置に誤差が発生していないと判定する誤差判定部と、
    　ＧＮＳＳ位置に誤差が発生していないと判定された場合、現周期において算出された相対位置ベクトルと、前回算出された相対位置ベクトルと、前回に相対位置ベクトルが算出された時点から現周期までの時間とに基づく速度ベクトルである補正速度ベクトルを算出する補正量算出部と、
    　現周期において算出された予測位置を前記補正速度ベクトルの積分値により補正した位置を前記移動体の現在位置として算出する現在位置算出部と
    　を備える位置補正装置。
  2. 　前記予測位置算出部は、前回算出された予測位置または現周期において取得されたＧＮＳＳ位置と、前回取得された速度とに基づいて前記移動体の予測位置を算出することを特徴とする請求項１に記載の位置補正装置。
  3. 　前記移動体の速度に基づいて前記移動体が移動しているか否かを判定する移動判定部を更に備え、
    　前記補正量算出部は、前記移動判定部による判定結果に応じて、前記補正速度ベクトルの初期値を異なる算出方法により算出することを特徴とする請求項１または請求項２に記載の位置補正装置。
  4. 　前記補正量算出部は、前記移動体が静止していると判定された場合、周期的に取得される複数の速度の平均値を前記初期値として算出することを特徴とする請求項３に記載の位置補正装置。
  5. 　前記補正量算出部は、前記移動体が移動していると判定された場合、現周期において算出された相対位置ベクトルから前回算出された相対位置ベクトルを減算した位置ベクトルを、前回に相対位置ベクトルが算出された時点から現周期までの時間により除した速度ベクトルを前記初期値として算出することを特徴とする請求項３または請求項４に記載の位置補正装置。
  6. 　前記補正量算出部は、前記誤差判定部による判定結果に基づき、周期的に取得されるＧＮＳＳ位置に位置誤差が発生する度合いを段階的に示す位置誤差発生度を算出して前記補正速度ベクトルに該位置誤差発生度を対応付け、
    　前記現在位置算出部は、前記現在位置に該現在位置が基づく補正速度ベクトルに対応付けられた位置誤差発生度を付加することを特徴とする請求項１～請求項５のいずれか一項に記載の位置補正装置。
  7. 　前記補正量算出部は、現周期において算出された相対位置ベクトルを構成する各成分についてローパスフィルタにより高周波成分を逓減したフィルタ値を算出し、該フィルタ値から前回算出されたフィルタ値を減算した位置ベクトルを、前回に相対位置ベクトルが算出された時点から現周期までの時間により除して前記補正速度ベクトルを算出することを特徴とする請求項１～請求項６のいずれか一項に記載の位置補正装置。
PCT/JP2022/014317 2021-10-08 2022-03-25 位置補正装置 WO2023058261A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2021166071 2021-10-08
JP2021-166071 2021-10-08

Publications (1)

Publication Number Publication Date
WO2023058261A1 true WO2023058261A1 (ja) 2023-04-13

Family

ID=85804064

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2022/014317 WO2023058261A1 (ja) 2021-10-08 2022-03-25 位置補正装置

Country Status (1)

Country Link
WO (1) WO2023058261A1 (ja)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08313278A (ja) * 1995-05-19 1996-11-29 Pioneer Electron Corp 測位データ補正装置及び測位データ補正方法
JP2007240532A (ja) * 2006-03-08 2007-09-20 Honeywell Internatl Inc ナビゲーション・システム内での反復拡張カルマン・フィルタを実施する方法及び装置
JP4615287B2 (ja) 2004-11-01 2011-01-19 東京計器株式会社 方位姿勢検出装置
JP5122678B1 (ja) 2011-12-09 2013-01-16 東京計器株式会社 衛星航法システムを用いた位置出力装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08313278A (ja) * 1995-05-19 1996-11-29 Pioneer Electron Corp 測位データ補正装置及び測位データ補正方法
JP4615287B2 (ja) 2004-11-01 2011-01-19 東京計器株式会社 方位姿勢検出装置
JP2007240532A (ja) * 2006-03-08 2007-09-20 Honeywell Internatl Inc ナビゲーション・システム内での反復拡張カルマン・フィルタを実施する方法及び装置
JP5122678B1 (ja) 2011-12-09 2013-01-16 東京計器株式会社 衛星航法システムを用いた位置出力装置
WO2013084987A1 (ja) * 2011-12-09 2013-06-13 東京計器株式会社 衛星航法システムを用いた位置出力装置

Similar Documents

Publication Publication Date Title
US7860651B2 (en) Enhanced inertial system performance
CN108957495B (zh) Gnss与mimu组合导航方法、装置及计算机设备
WO2013084987A1 (ja) 衛星航法システムを用いた位置出力装置
WO2014022664A2 (en) Method and apparatus for data fusion of a three axis magnetometer and three axis accelerometer
EP1593931A4 (en) DIFFERENCE CORRECTION METHOD FOR POSTURE DETERMINATION INSTRUMENT AND DISPLACEMENT MEASUREMENT INSTRUMENT
JP2007240532A (ja) ナビゲーション・システム内での反復拡張カルマン・フィルタを実施する方法及び装置
US20200286245A1 (en) Posture estimation method, posture estimation apparatus and computer readable storage medium
WO2011088245A1 (en) Motion state detection for mobile device
JPH09318382A (ja) 姿勢角検出装置及び方法
CN107228674B (zh) 一种针对星敏感器和陀螺联合滤波的改进方法
JP2015094630A (ja) センサー用ｉｃ、センサーデバイス、電子機器及び移動体
WO2014001320A1 (en) Sequential estimation in a real-time positioning or navigation system using historical states
KR20150082382A (ko) 적응형 스케일 및/또는 중력 추정
US20160025496A1 (en) Positioning Device, Method for Controlling Same, and Program
WO2023058261A1 (ja) 位置補正装置
JP2019082328A (ja) 位置推定装置
TWI472953B (zh) 慣性感測輸入裝置、系統及其方法
US20150134294A1 (en) Attitude calculation apparatus and attitude calculation method
KR101752724B1 (ko) 관성항법장치의 정렬 방법
JP6384194B2 (ja) 情報処理装置、情報処理方法及び情報処理プログラム
JP7400922B2 (ja) 測位装置、測位方法及び測位プログラム
CN112461258A (zh) 一种参数修正的方法及装置
KR20240074763A (ko) 위치 보정 장치
JP2016125988A (ja) 携帯端末装置及びキャリブレーション方法
JP2015102330A (ja) 移動情報算出装置、移動情報算出方法、移動情報算出プログラム、および移動体

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22878134

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2022878134

Country of ref document: EP

Effective date: 20240312

ENP Entry into the national phase

Ref document number: 2023552687

Country of ref document: JP

Kind code of ref document: A