WO2022208588A1 - 位置推定装置、位置推定プログラムおよび位置推定方法 - Google Patents

位置推定装置、位置推定プログラムおよび位置推定方法 Download PDF

Info

Publication number
WO2022208588A1
WO2022208588A1 PCT/JP2021/013221 JP2021013221W WO2022208588A1 WO 2022208588 A1 WO2022208588 A1 WO 2022208588A1 JP 2021013221 W JP2021013221 W JP 2021013221W WO 2022208588 A1 WO2022208588 A1 WO 2022208588A1
Authority
WO
WIPO (PCT)
Prior art keywords
positioning
reliability
self
unit
position estimation
Prior art date
Application number
PCT/JP2021/013221
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 三菱電機株式会社
Priority to JP2021560130A priority Critical patent/JP7046281B1/ja
Priority to PCT/JP2021/013221 priority patent/WO2022208588A1/ja
Publication of WO2022208588A1 publication Critical patent/WO2022208588A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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

  • the present disclosure relates to a position estimation device, a position estimation program and a position estimation method.
  • Patent Document 1 does not provide any specific disclosure regarding reliability determination.
  • An object of the present disclosure is to provide a method for determining the reliability of a positioning signal when switching between a positioning method that uses a positioning signal and a positioning method that does not use a positioning signal.
  • a position estimation device includes: a reliability estimation unit that estimates the reliability of the positioning position calculated from the newly received positioning signal using the statistical probability distribution of the positioning position obtained from the positioning signal transmitted by the positioning satellite; One of a first positioning method for positioning using a positioning signal and a second positioning method for positioning without using a positioning signal based on the reliability of the positioning signal estimated by the reliability estimation unit. a selection unit for estimating the position by selecting the Prepare.
  • FIG. 2 is a diagram of the first embodiment and shows a usage form of the position estimation device 100;
  • FIG. 2 is a diagram of the first embodiment and shows functional blocks of the position estimation device 100;
  • FIG. 2 is a diagram of the first embodiment and shows the hardware configuration of the position estimation device 100;
  • 4 is a diagram of the first embodiment and is a flowchart for explaining the overall operation of the position estimation device 100.
  • FIG. FIG. 10 is a flowchart showing two types of self-position estimation by a self-position estimation unit 113 in step S120 in the diagram of the first embodiment;
  • FIG. 11 is a flowchart showing two types of reliability estimation by a reliability estimation unit 114 in step S130 in the diagram of the first embodiment;
  • FIG. 7 is a diagram according to the first embodiment, showing calculation of the position distribution in Fig. 6;
  • FIG. 7 is a diagram of the first embodiment and shows the score value calculation of FIG. 6;
  • FIG. 7 is another diagram showing the score value calculation of FIG. 6 in the diagram of the first embodiment;
  • FIG. 7 is a diagram of the first embodiment showing reliability estimation using the filter operation of FIG. 6;
  • FIG. 10 is a diagram of the first embodiment and is a flow chart showing processing of the self-position switching determination unit 111 in step S140 and processing of the GNSS convergence determination unit 115 in step S180;
  • FIG. 10 is a diagram of the first embodiment and is a flowchart showing processing of a pose adjustment unit 122 in step S220;
  • FIG. 10 is a diagram of the first embodiment and shows the state before correction by a pose adjustment unit 122;
  • FIG. 10 is a diagram of the first embodiment, showing a state after correction by a pose adjustment unit 122;
  • unit may be read as “circuit”, “process”, “procedure”, “process” or “circuitry” as appropriate.
  • FIG. 1 shows how the position estimation device 100 is used.
  • the position estimation device 100 is mounted on a moving object.
  • FIG. 1 shows a vehicle 500 as an example of a mobile object.
  • the moving body may be an electric wheelchair or a low-speed vehicle called PMV.
  • PMV is an abbreviation for "Personal Mobility Vehicle".
  • the mobile object can move autonomously.
  • the vehicle 500 in FIG. 1 includes a GNSS receiver 200, an IMU 301, an INS 302, a lidar 401, an ultrasonic sensor 402, a millimeter wave radar 403, a camera 404 arranged in front of the vehicle, and a camera 404 arranged in the rear of the vehicle.
  • a camera 404 is provided.
  • GNSS Global Navigation Satellite System
  • IMU is an abbreviation for “inertial measurement unit”.
  • INS is an abbreviation for "Inertial Navigation System”.
  • a lidar 401 , an ultrasonic sensor 402 , a millimeter wave radar 403 and a camera 404 are external sensors 400 .
  • IMU 301 and INS 302 are internal sensors 300 .
  • IMU 301 is an inertial unit.
  • the GNSS receiver 200 receives positioning signals transmitted from each of the positioning satellites 610 and measures the position of the GNSS receiver 200 .
  • the position of GNSS receiver 200 is the position of vehicle 500 .
  • FIG. 2 shows functional blocks of the position estimation device 100.
  • FIG. 3 shows the hardware configuration of the position estimation device 100.
  • Position estimation device 100 includes self-position management unit 110 and sensor data management unit 120 .
  • the sensor data management unit 120 includes a self-position switching determination unit 111 , a self-position storage unit 112 , a self-position estimation unit 113 , a reliability estimation unit 114 and a GNSS convergence determination unit 115 .
  • Self-position switching determination unit 111 is a selection unit.
  • the sensor data management unit 120 has a feature amount storage unit 121 and a pose adjustment unit 122 .
  • the GNSS receiver 200, the internal sensor 300, and the external sensor 400 are connected to the self-location management unit 110, and information output from these devices is input to the self-location management unit 110.
  • Position estimation device 100 is a computer.
  • Position estimation device 100 comprises a processor 810 .
  • Position estimation device 100 includes other hardware such as main memory 820 , auxiliary memory 830 , input interface 840 , output interface 850 and communication interface 860 in addition to processor 810 .
  • Processor 810 is connected to and controls other hardware via signal line 870 . Note that the interface is hereinafter referred to as IF.
  • the position estimation device 100 includes a self-position switching determination unit 111, a self-position estimation unit 113, a reliability estimation unit 114, and a GNSS convergence determination unit 115 of the self-position management unit 110 as functional elements.
  • the position estimation device 100 also includes a pose adjustment section 122 of the sensor data management section 120 .
  • Position estimation program 101 realizes functions of self-position switching determination unit 111 , self-position estimation unit 113 , reliability estimation unit 114 , GNSS convergence determination unit 115 , and pose adjustment unit 122 .
  • Position estimation program 101 is stored in auxiliary storage device 830 .
  • Self-position storage unit 112 and feature amount storage unit 121 are realized by auxiliary storage device 830 .
  • a processor 810 is a device that executes the position estimation program 101 .
  • the position estimation program 101 is a program that implements the functions of the self-position switching determination unit 111 , the self-position estimation unit 113 , the reliability estimation unit 114 , the GNSS convergence determination unit 115 and the pose adjustment unit 122 .
  • the processor 810 is an IC (Integrated Circuit) that performs arithmetic processing. Specific examples of the processor 810 are a CPU (Central Processing Unit), a DSP (Digital Signal Processor), and a GPU (Graphics Processing Unit).
  • main memory device 820 Specific examples of the main memory device 820 are SRAM (Static Random Access Memory) and DRAM (Dynamic Random Access Memory).
  • the main memory device 820 holds the computation results of the processor 810 .
  • the auxiliary storage device 830 is a storage device that stores data in a non-volatile manner.
  • a specific example of the auxiliary storage device 830 is an HDD (Hard Disk Drive).
  • the auxiliary storage device 830 is a portable recording medium such as an SD (registered trademark) (Secure Digital) memory card, a NAND flash, a flexible disk, an optical disk, a compact disk, a Blu-ray (registered trademark) disk, or a DVD (Digital Versatile Disk). There may be.
  • Auxiliary storage device 830 stores position estimation program 101, self-position and feature amount.
  • the input IF 840 is a port through which data is input from each device.
  • the GNSS receiver 200, the internal sensor 300, and the external sensor 400 are connected to the input IF840.
  • the output IF 850 is a port to which various devices are connected and data is output from the processor 810 to the various devices.
  • Communication IF 860 is a communication port for the processor to communicate with other devices.
  • the processor 810 loads the position estimation program 101 from the auxiliary storage device 830 to the main storage device 820, reads the position estimation program 101 from the main storage device 820, and executes it.
  • the main storage device 820 stores not only the position estimation program 101 but also an OS (Operating System).
  • the processor 810 executes the position estimation program 101 while executing the OS.
  • Position estimation device 100 may include multiple processors in place of processor 810 . These multiple processors share execution of the position estimation program 101 .
  • Each processor like processor 810, is a device that executes position estimation program 101.
  • the data, information, signal values and variable values used, processed or output by the position estimation program 101 are stored in the main memory 820, the auxiliary memory 830, or the registers or cache memory within the processor 810.
  • processing is a program that causes a computer to execute each process, each procedure, or each step.
  • the position estimation method is a method performed by the position estimation device 100, which is a computer, executing the position estimation program 101.
  • the position estimation program 101 may be stored in a computer-readable recording medium and provided, or may be provided as a program product.
  • FIG. 4 is a flowchart for explaining the overall operation of the position estimation device 100. As shown in FIG. The operation of the position estimation device 100 will be described with reference to FIG.
  • step S110 the position estimation device 100 inputs each sensor data.
  • Each sensor data is the data which the GNSS receiver 200, the internal sensor 300, and the external sensor 400 output.
  • step S120 the self-position estimation unit 113 carries out self-position estimation from the outputs of various sensors.
  • self-position estimation section 113 estimates the self-position from the output of GNSS receiver 200 .
  • the self-position estimation unit 113 estimates the self-position using SLAM (FIG. 5).
  • self-position estimation section 113 estimates the self-position using IMU 301 and INS 302 (FIG. 5).
  • step S ⁇ b>130 the reliability estimation unit 114 updates the reliability of satellite positioning using positioning signals transmitted by the positioning satellites 610 . Details will be described later with reference to FIG.
  • the reliability estimation unit 114 estimates the reliability of the positioning position calculated from the newly received positioning signal using the statistical probability distribution of the positioning position obtained from the positioning signal transmitted by the positioning satellite.
  • Step S140> Based on the reliability of the positioning signal estimated by the reliability estimation unit 114, the self-location switching determination unit 111 selects a first positioning method for positioning using the positioning signal and a second positioning method for positioning without using the positioning signal. Select one of the positioning methods to estimate the position. SLAM is a second positioning method. Positioning by GPS signals is the first positioning method.
  • step S140 the self-position switching determination unit 111 starts calculating the self-position. Calculation of the self-position means that the self-position switching determination unit 111 outputs the self-position as shown in FIG. This self-position is the output of step S170 or step S210 in FIG. In other words, the self-position output in FIG.
  • the self-position calculated by SLAM is either the self-position calculated by SLAM or the position measured using the positioning signal transmitted by the positioning satellite 610 .
  • the self position calculated by SLAM is written as SLAM position
  • the position measured using the positioning signal is written as GNSS position.
  • step S15 the self-position switching determination unit 111 determines whether the switching flag is OFF.
  • the switching flag will be described with reference to FIG. 11.
  • the switching flag When the switching flag is OFF, it indicates that the satellite positioning score is equal to or higher than the threshold for the reliability of satellite positioning updated in step S130 (step S142).
  • a satellite positioning score greater than or equal to the threshold indicates that the positioning signal reception condition is good, and a satellite positioning score less than the threshold indicates that the positioning signal reception condition is not good. If the switching flag is OFF, the process proceeds to step S160, and if the switching flag is not OFF, the process proceeds to step S200.
  • step S16 the self-location switching determination unit 111 determines whether the GNSS receiver 200 is receiving positioning signals. If the positioning signal has been received, the process proceeds to step S180, and if the positioning signal has not been received, the process proceeds to step S170.
  • step S17 the self-position switching determination unit 111 outputs the self-position calculated by SLAM, and ends the process.
  • step S18 the GNSS convergence determination unit 115 performs GNSS convergence determination.
  • GNSS convergence determination will be described later with reference to FIG.
  • step S190 the self-position switching determination unit 111 determines whether the switching flag is OFF. As will be described later with reference to FIG. 11, when the process proceeds to step S183, the switching flag is turned OFF. When the switching flag is OFF, it indicates that the error is within the threshold, as will be described later with reference to FIG. If the switching flag is OFF, the process proceeds to step S220, and if the switching flag is not OFF, that is, if the error is not within the threshold, the process proceeds to step S170.
  • step S200 the self-position switching determination unit 111 refers to the feature amount storage unit 121 and checks whether or not there is a "feature amount" of a given frame.
  • This "feature amount” is the feature amount of step S122a in FIG. 5, which will be described later.
  • step S210 the self-position switching determination unit 111 outputs the GNSS position.
  • FIG. 5 shows the flow of two types of self-position estimation by the self-position estimation unit 113 in step S120.
  • the self-position estimation unit 113 may estimate the self-position from the output of the GNSS receiver 200, but this will be omitted.
  • SLAM self-position estimation by SLAM on the left side of FIG. 5 will be described.
  • the left side of FIG. 5 is a specific example of the process of step S120. This processing is executed by the self-position estimation unit 113 .
  • Step S121a the self-position estimation unit 113 acquires each sensor data.
  • Step S122a the self-position estimation unit 113 calculates each feature amount.
  • step S123a the self-position estimation unit 113 performs scan matching.
  • step S124a the self-position estimation unit 113 calculates the posture.
  • step S125a the self-position estimation unit 113 calculates the position.
  • step S126a> self-position estimation section 113 saves the feature quantity in auxiliary storage device 830 .
  • step S120 Self-position estimation by an inertial sensor such as an IMU on the right of Fig. 5 will be explained.
  • the right side of FIG. 5 is a specific example of the process of step S120. This processing is executed by the self-position estimation unit 113 .
  • Step S121b the self-position estimation unit 113 acquires each sensor data.
  • step S122b> self-position estimation section 113 calculates the speed and attitude of vehicle 500 .
  • step S ⁇ b>123 b self-position estimation section 113 adds the amount of movement of vehicle 500 .
  • step S124b> self-position estimation section 113 calculates the position of vehicle 500 .
  • FIG. 6 shows the flow of two types of reliability estimation by the reliability estimation unit 114 in step S130. First, reliability estimation using the position distribution on the left side of FIG. 6 will be described. The left side of FIG. 6 is a specific example of the process of step S130. This processing is executed by the reliability estimation unit 114 .
  • step S131a the reliability estimation unit 114 calculates the distribution of each position.
  • step S132a the reliability estimation unit 114 calculates the position distribution at the current time from the previous time data and the control value.
  • step S133a the reliability estimation unit 114 calculates the amount of information from the distributions of both the previous time and the current time.
  • FIG. 7 is a diagram showing a reliability score calculation method by the reliability estimation unit 114. As shown in FIG. The left side of FIG. 7 shows a case where the reliability estimation unit 114 uses, as the probability distribution, a probability distribution indicating a position distribution obtained from a plurality of positioning positions. The left side of FIG. 7 is based on GPS measurement positions. The right side of FIG. 7 shows a case where the reliability estimation unit 11 uses, as the probability distribution, a probability distribution representing the position distribution obtained from the positioning signal and the output data of the IMU 301 .
  • step S134a the reliability estimation unit 114 determines a reliability score from the amount of information.
  • 8 and 9 are diagrams showing calculation of score values based on the amount of information. 8 and 9 show calculation of score values using the Kullback-Leibler information content. The left side of FIG. 8 shows a case where the distribution error is small, and the right side shows a case where the distribution error is small. In FIG. 9, the smaller the difference between the distributions of the estimated values and the observed values, the smaller the amount of information, and the larger the difference, the larger the amount of information.
  • step S135a the reliability estimation unit 114 determines whether the GNSS score is equal to or greater than the threshold. If it is equal to or greater than the threshold, the process proceeds to step S136a, and if it is less than the threshold, the process ends.
  • step S136a the reliability estimation unit 114 registers the GNSS location information in the factor.
  • step S131b the reliability estimation unit 114 estimates the position of the current frame from the previous frame speed and posture.
  • the reliability estimation unit 114 determines a reliability score from the difference between the observed and estimated positions.
  • the reliability estimating unit 114 generates a probability distribution based on the state equation generated from the data obtained from the IMU 301 and the observation equation generated from the positioning position obtained from the positioning signal as a result of the filtering operation. using a probability distribution.
  • FIG. 10 shows a case where the reliability score is calculated by filtering.
  • a particle filter or a Kalman filter with IMU values as the input of the state equation and GNSS as the observed value is used.
  • particles with noise added to the input are scattered from the position of t ⁇ 1, and the distribution of the particles is expressed as a normal distribution.
  • the input from the t ⁇ 1 o'clock position is now the IMU measurement.
  • the variance can be updated at any time.
  • the amount of computation increases with the number of particles, there is a trade-off between processing speed and accuracy.
  • step S133b the reliability estimation unit 114 determines whether the GNSS score is greater than or equal to the threshold. If it is equal to or greater than the threshold, the process proceeds to step S134b, and if it is less than the threshold, the process ends.
  • step S134b the reliability estimation unit 114 registers the GNSS location information in the factor.
  • FIG. 11 shows the processing of the self-position switching determination unit 111 in step S140 and the processing of the GNSS convergence determination unit 115 in step S180. First, the processing of the self-position switching determination unit 111 on the left side of FIG. 11 will be described.
  • step S141 the self-location switching determination unit 111 determines whether the GNSS score is equal to or greater than the threshold. If the GNSS score is greater than or equal to the threshold, the process ends. If the GNSS score is less than the threshold, processing proceeds to step S142.
  • step S142 the self-position switching determination unit 111 turns ON the switching flag.
  • step S181 the GNSS convergence determination unit 115 estimates the current frame position from the previous position by SLAM or IMU.
  • step S182 the GNSS convergence determination unit 115 determines whether the error between the GNSS position of the current frame and the estimated position is within a threshold. If the error is within the threshold, the process proceeds to step S183. If the error is not within the threshold, the process ends.
  • step S183 the GNSS convergence determination unit 115 turns off the switching flag.
  • FIG. 12 shows the processing of the pose adjustment unit 122 in step S220.
  • step S221 the pose adjustment unit 122 determines whether the error between the SLAM position and the calculated position is equal to or greater than a threshold. If the error is greater than or equal to the threshold, the process proceeds to step S222. If the error is less than the threshold, the process ends.
  • step S222 the pose adjustment unit 122 determines whether or not the GNSS factor is registered within the setting frame. If there is registration, the process proceeds to step S223. If there is no registration, the process ends.
  • step S223 the pose adjustment unit 122 sets the current frame and the GNSS information of the GNSS factors registered in the past as constraint conditions.
  • step S224 the pose adjustment unit 122 performs pose adjustment.
  • the pose adjustment unit 122 corrects the multiple positions obtained by the second positioning method with the multiple positions obtained by the first positioning method.
  • FIG. 13 shows the state before correction by the pose adjustment unit 122.
  • FIG. FIG. 14 shows the state after correction by the pose adjustment unit 122.
  • black circles indicate positions measured by GPS, and white circles indicate positions estimated by SLAM.
  • the pose adjustment unit 122 uses the GPS positions at t and t+5 as constraints, and corrects the SLAM position and posture from t+1 to t+4 based on the GPS positions.
  • An object of the position estimation device 100 is to provide a determination method of the reliability of the positioning signal when the reliability estimation unit 114 switches between the positioning method using the positioning signal and the positioning method not using the positioning signal. .
  • GPS position cannot be obtained when driving under buildings or elevated tracks where GPS radio waves cannot be received, or errors due to multipath effects becomes large and an accurate position cannot be obtained.
  • the position estimation device 100 includes the reliability estimation unit 114, only the GPS positioning positions determined to have high reliability among the measured GPS positioning positions are used.
  • the absolute position can be determined by GPS.
  • the SLAM can estimate the position with high accuracy using the posture in the absolute coordinates estimated when the GPS is normally received. Position can be interpolated.
  • the position estimation device 100 can be applied not only to the vehicle 500 but also to flying objects such as drones.
  • Embodiment 1 has been described above.
  • One technical matter in Embodiment 1 may be partially implemented.
  • two or more of the plurality of technical matters included in the first embodiment may be partially combined for implementation.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

位置推定装置（１００）は、自己位置管理部（１１０）とセンサデータ管理部（１２０）とを備えている。自己位置管理部（１１０）は、ＧＮＳＳ受信機（２００）、内部センサ（３００）、外部センサ（４００）の出力するデータを入力する。自己位置管理部（１１０）の信頼度推定部（１１４）は、測位衛星の送信する測位信号から得られた測位位置の統計的な確率分布を用いて、新たに受信された測位信号から算出された測位位置の信頼度を推定する。自己位置切り替え判定部（１１１）は、信頼度推定部（１１４）によって推定された測位信号の信頼度に基づいて、測位信号を用いて測位する第１の測位方式である衛星測位と、測位信号を用いずに測位する第２の測位方式であるＳＬＡＭとの、いずれかを選択して自己位置を推定する。

Description

位置推定装置、位置推定プログラムおよび位置推定方法
　本開示は位置推定装置、位置推定プログラムおよび位置推定方法に関する。
　ＧＰＳ（Ｇｌｏｂａｌ　Ｐｏｓｉｔｉｏｎｉｎｇ　Ｓｙｓｔｅｍ）のような測位信号を用いた測位では、トンネルのような測位信号の遮蔽物、マルチパスなどにより位置推定が困難な状況が発生し得る。従来技術では、ＧＰＳ受信機を用いた位置推定が困難な場合に対応するため、ＧＰＳを用いる方式とＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　Ｍａｐｐｉｎｇ）を用いる方式について、位置推定方式の信頼度を求めて、信頼度の高い方式を選んで用いる技術がある（例えば、特許文献１）。
特開２０２０－１１２５５７号公報
　しかし、特許文献１には、信頼度の判定については何ら具体的な開示はない。
　本開示は、測位信号を用いる測位方式と、測位信号を用いない測位方式とのいずれかに切り替える際の、測位信号の信頼度の判定方式の提供を目的とする。
　本開示に係る位置推定装置は、
　測位衛星の送信する測位信号から得られた測位位置の統計的な確率分布を用いて、新たに受信された測位信号から算出された測位位置の信頼度を推定する信頼度推定部と、
　前記信頼度推定部によって推定された測位信号の信頼度に基づいて、測位信号を用いて測位する第１の測位方式と、測位信号を用いずに測位する第２の測位方式との、いずれかを選択して位置を推定する選択部と、
を備える。
　本開示によれば、測位信号を用いる測位方式と、測位信号を用いない測位方式とのいずれかに切り替える際の、測位信号の信頼度の判定方式を提供できる。
実施の形態１の図で、位置推定装置１００の利用形態を示す図。 実施の形態１の図で、位置推定装置１００の機能ブロックを示す図。 実施の形態１の図で、位置推定装置１００のハードウェア構成を示す図。 実施の形態１の図で、位置推定装置１００の全体動作を説明するフローチャート。 実施の形態１の図で、ステップＳ１２０の自己位置推定部１１３による２種類の自己位置推定を示すフローチャート。 実施の形態１の図で、ステップＳ１３０の信頼度推定部１１４による２種類の信頼度推定を示すフローチャート。 実施の形態１の図で、図６の位置分布の算出を示す図。 実施の形態１の図で、図６のスコア値算出を示す図。 実施の形態１の図で、図６のスコア値算出を示す別の図。 実施の形態１の図で、図６のフィルタ演算を用いる信頼度推定を示す図。 実施の形態１の図で、ステップＳ１４０の自己位置切り替え判定部１１１の処理と、ステップＳ１８０のＧＮＳＳ収束判定部１１５の処理とを示すフローチャート。 実施の形態１の図で、ステップＳ２２０のポーズ調整部１２２の処理を示すフローチャート。 実施の形態１の図で、ポーズ調整部１２２による修正前を示す図。 実施の形態１の図で、ポーズ調整部１２２による修正後を示す図。
　実施の形態の説明及び図面において、同じ要素及び対応する要素には同じ符号を付している。同じ符号が付された要素の説明は、適宜に省略又は簡略化する。以下の実施の形態では、「部」を、「回路」、「工程」、「手順」、「処理」又は「サーキットリー」に適宜読み替えてもよい。
　実施の形態１．
　図１から図１４を参照して、実施の形態１の位置推定装置１００を説明する。
　図１は、位置推定装置１００の利用形態を示す。位置推定装置１００は移動体に搭載される。図１では移動体の例として車両５００を示している。移動体は電動車いす、あるいはＰＭＶと呼ばれる低速走行車両でもよい。ＰＭＶは「Ｐｅｒｓｏｎａｌ　Ｍｏｂｉｌｉｔｙ　Ｖｅｈｉｃｌｅ」の略称である。移動体は自律移動できる。図１の車両５００は、位置推定装置１００の他に、ＧＮＳＳ受信機２００、ＩＭＵ３０１，ＩＮＳ３０２、ライダー４０１、超音波センサ４０２、ミリ波レーダ４０３、車両前方に配置されたカメラ４０４および車両後方に配置されたカメラ４０４を備えている。ＧＮＳＳは「Ｇｌｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍ」の略称である。ＩＭＵは「ｉｎｅｒｔｉａｌ　ｍｅａｓｕｒｅｍｅｎｔ　ｕｎｉｔ」の略称である。ＩＮＳは「Ｉｎｅｒｔｉａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓｙｓｔｅｍ」の略称である。ライダー４０１、超音波センサ４０２，ミリ波レーダ４０３およびカメラ４０４は外部センサ４００である。ＩＭＵ３０１およびＩＮＳ３０２は内部センサ３００である。ＩＭＵ３０１は慣性装置である。ＧＮＳＳ受信機２００は、複数の測位衛星６１０から各測位衛星の送信する測位信号を受信し、ＧＮＳＳ受信機２００の位置を測位する。ＧＮＳＳ受信機２００の位置は車両５００の位置である。
＊＊＊構成の説明＊＊＊
　図２は、位置推定装置１００の機能ブロックを示す。
　図３は、位置推定装置１００のハードウェア構成を示す。図２を参照して位置推定装置１００のブロック構成を説明する。位置推定装置１００は、自己位置管理部１１０とセンサデータ管理部１２０とを備えている。センサデータ管理部１２０は、自己位置切り替え判定部１１１、自己位置記憶部１１２、自己位置推定部１１３、信頼度推定部１１４およびＧＮＳＳ収束判定部１１５を備えている。自己位置切り替え判定部１１１は選択部である。センサデータ管理部１２０は、特徴量保存部１２１およびポーズ調整部１２２を備えている。自己位置管理部１１０には、ＧＮＳＳ受信機２００、内部センサ３００および外部センサ４００が接続しており、自己位置管理部１１０にはこれらの装置の出力する情報が入力される。
　図３を参照して位置推定装置１００のハードウェア構成を説明する。位置推定装置１００は、コンピュータである。位置推定装置１００は、プロセッサ８１０を備える。位置推定装置１００は、プロセッサ８１０の他に、主記憶装置８２０、補助記憶装置８３０、入力インタフェース８４０、出力インタフェース８５０及び通信インタフェース８６０といった、他のハードウェアを備える。プロセッサ８１０は、信号線８７０を介して、他のハードウェアと接続され、他のハードウェアを制御する。なおインタフェースは以下、ＩＦと表記する。
　位置推定装置１００は、機能要素として、自己位置管理部１１０のうちの、自己位置切り替え判定部１１１、自己位置推定部１１３、信頼度推定部１１４、ＧＮＳＳ収束判定部１１５を備えている。また、位置推定装置１００は、センサデータ管理部１２０のうちのポーズ調整部１２２を備えている。自己位置切り替え判定部１１１、自己位置推定部１１３、信頼度推定部１１４、ＧＮＳＳ収束判定部１１５およびポーズ調整部１２２の機能は位置推定プログラム１０１によって実現される。位置推定プログラム１０１は補助記憶装置８３０に格納されている。また、自己位置記憶部１１２および特徴量保存部１２１は、補助記憶装置８３０で実現される。
　プロセッサ８１０は、位置推定プログラム１０１を実行する装置である。位置推定プログラム１０１は、自己位置切り替え判定部１１１、自己位置推定部１１３、信頼度推定部１１４、ＧＮＳＳ収束判定部１１５およびポーズ調整部１２２の機能を実現するプログラムである。プロセッサ８１０は、演算処理を行うＩＣ（Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）である。プロセッサ８１０の具体例は、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）、ＤＳＰ（Ｄｉｇｉｔａｌ　Ｓｉｇｎａｌ　Ｐｒｏｃｅｓｓｏｒ）、ＧＰＵ（Ｇｒａｐｈｉｃｓ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）である。
　主記憶装置８２０の具体例は、ＳＲＡＭ（Ｓｔａｔｉｃ　Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）、ＤＲＡＭ（Ｄｙｎａｍｉｃ　Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）である。主記憶装置８２０は、プロセッサ８１０の演算結果を保持する。
　補助記憶装置８３０は、データを不揮発的に保管する記憶装置である。補助記憶装置８３０の具体例は、ＨＤＤ（Ｈａｒｄ　Ｄｉｓｋ　Ｄｒｉｖｅ）である。また、補助記憶装置８３０は、ＳＤ（登録商標）（Ｓｅｃｕｒｅ　Ｄｉｇｉｔａｌ）メモリカード、ＮＡＮＤフラッシュ、フレキシブルディスク、光ディスク、コンパクトディスク、ブルーレイ（登録商標）ディスク、ＤＶＤ（Ｄｉｇｉｔａｌ　Ｖｅｒｓａｔｉｌｅ　Ｄｉｓｋ）といった可搬記録媒体であってもよい。補助記憶装置８３０は、位置推定プログラム１０１、自己位置および特徴量を記憶している。
　入力ＩＦ８４０は、各装置からデータが入力されるポートである。位置推定装置１００では入力ＩＦ８４０には、ＧＮＳＳ受信機２００、内部センサ３００および外部センサ４００が接続されている。出力ＩＦ８５０は、各種機器が接続され、各種機器にプロセッサ８１０によりデータが出力されるポートである。通信ＩＦ８６０は、プロセッサが他の装置と通信するための通信ポートである。
　プロセッサ８１０は補助記憶装置８３０から位置推定プログラム１０１を主記憶装置８２０にロードし、主記憶装置８２０から位置推定プログラム１０１を読み込み実行する。主記憶装置８２０には、位置推定プログラム１０１だけでなく、ＯＳ（Ｏｐｅｒａｔｉｎｇ　Ｓｙｓｔｅｍ）も記憶されている。プロセッサ８１０は、ＯＳを実行しながら、位置推定プログラム１０１を実行する。位置推定装置１００は、プロセッサ８１０を代替する複数のプロセッサを備えていてもよい。これら複数のプロセッサは、位置推定プログラム１０１の実行を分担する。それぞれのプロセッサは、プロセッサ８１０と同じように、位置推定プログラム１０１を実行する装置である。位置推定プログラム１０１により利用、処理または出力されるデータ、情報、信号値及び変数値は、主記憶装置８２０、補助記憶装置８３０、または、プロセッサ８１０内のレジスタあるいはキャッシュメモリに記憶される。
　位置推定プログラム１０１は、自己位置切り替え判定部１１１、自己位置推定部１１３、信頼度推定部１１４、ＧＮＳＳ収束判定部１１５およびポーズ調整部１２２の「部」を「処理」、「手順」あるいは「工程」に読み替えた各処理、各手順あるいは各工程をコンピュータに実行させるプログラムである。
　また、位置推定方法は、コンピュータである位置推定装置１００が位置推定プログラム１０１を実行することにより行われる方法である。位置推定プログラム１０１は、コンピュータ読み取り可能な記録媒体に格納されて提供されてもよいし、プログラムプロダクトとして提供されてもよい。
＊＊＊動作の説明＊＊＊
　図４は、位置推定装置１００の全体動作を説明するフローチャートである。図４を参照して、位置推定装置１００の動作を説明する。
＜ステップＳ１１０＞
　ステップＳ１１０において、位置推定装置１００が、各センサデータを入力する。各センサデータとは、ＧＮＳＳ受信機２００，内部センサ３００および外部センサ４００の出力するデータである。
＜ステップＳ１２０＞
　ステップＳ１２０において、自己位置推定部１１３は、各種センサの出力から自己位置推定を実施する。例えば自己位置推定部１１３は、ＧＮＳＳ受信機２００の出力から自己位置を推定する。あるいは、自己位置推定部１１３はＳＬＡＭを用いて自己位置を推定する（図５）。あるいは、自己位置推定部１１３はＩＭＵ３０１およびＩＮＳ３０２を用いて自己位置を推定する（図５）。
＜ステップＳ１３０＞
　ステップＳ１３０において、信頼度推定部１１４は、測位衛星６１０の送信する測位信号を用いる衛星測位の信頼度更新を行う。詳細は図６で後述する。信頼度推定部１１４は、測位衛星の送信する測位信号から得られた測位位置の統計的な確率分布を用いて、新たに受信された測位信号から算出された測位位置の信頼度を推定する。
＜ステップＳ１４０＞
　自己位置切り替え判定部１１１は、信頼度推定部１１４によって推定された測位信号の信頼度に基づいて、測位信号を用いて測位する第１の測位方式と、測位信号を用いずに測位する第２の測位方式との、いずれかを選択して位置を推定する。ＳＬＡＭは第２の測位方式である。ＧＰＳ信号による測位は第１の測位方式である。
　ステップＳ１４０において、自己位置切り替え判定部１１１は、自己位置の算出を開始する。自己位置の算出とは、図２に示すように、自己位置切り替え判定部１１１が、自己位置を出力することを意味する。この自己位置は、図４では、ステップＳ１７０またはステップＳ２１０の出力である。つまり、図２で出力される自己位置はＳＬＡＭで計算された自己位置と、測位衛星６１０の送信する測位信号を用いて測位された位置のどちらかである。図２ではＳＬＡＭで計算された自己位置をＳＬＡＭ位置と表記し、測位信号を用いて測位された位置をＧＮＳＳ位置と表記している。
＜ステップＳ１５０＞
　ステップＳ１５において、自己位置切り替え判定部１１１は、切り替えフラグがＯＦＦかどうかを判定する。切り替えフラグについては図１１で説明するが、切り替えフラグがＯＦＦであることは、ステップＳ１３０で更新される衛星測位の信頼度について、衛星測位のスコアが閾値以上であることを示す（図１１のステップＳ１４２）。衛星測位のスコアが閾値以上とは測位信号の受信状態が良好であり、衛星測位のスコアが閾値未満とは測位信号の受信状態が良好でないことを示す。切り替えフラグがＯＦＦであれば処理はステップＳ１６０に進み、切り替えフラグがＯＦＦでなければ処理はステップＳ２００に進む。
＜ステップＳ１６０＞
　ステップＳ１６において、自己位置切り替え判定部１１１は、ＧＮＳＳ受信機２００が測位信号を受信しているかどうかを判定する。測位信号が受信されている場合処理はステップＳ１８０に進み、測位信号が受信されていない場合処理はステップＳ１７０に進む。
＜ステップＳ１７０＞
　ステップＳ１７において、自己位置切り替え判定部１１１は、ＳＬＡＭで算出した自己位置を出力し、処理を終了する。
＜ステップＳ１８０＞
　ステップＳ１８において、ＧＮＳＳ収束判定部１１５は、ＧＮＳＳ収束判定を実施する。ＧＮＳＳ収束判定は、図１１で後述する。
＜ステップＳ１９０＞
　ステップＳ１９０において、自己位置切り替え判定部１１１は、切り替えフラグがＯＦＦかどうかを判定する。図１１で後述するが、処理がステップＳ１８３に進む場合、切り替えフラグはＯＦＦになる。切り替えフラグがＯＦＦは、図１１で後述するが誤差が閾値以内に収まっていることを示す。切り替えフラグがＯＦＦの場合は、処理はステップＳ２２０に進み、切り替えフラグがＯＦＦでない場合、つまり、後述するが誤差が閾値以内に収まっていない場合は、処理はステップＳ１７０に進む。
＜ステップＳ２００＞
　ステップＳ２００において、自己位置切り替え判定部１１１は、特徴量保存部１２１を参照し、一定フレームの「特徴量」があるかどうかを確認する。この「特徴量」は後述の図５のステップＳ１２２ａの特徴量である。
＜ステップＳ２１０＞
　ステップＳ２１０において、自己位置切り替え判定部１１１は、ＧＮＳＳ位置を出力する。
　図５は、ステップＳ１２０の自己位置推定部１１３による２種類の自己位置推定のフローを示す。図５に示す２つのフローの他に自己位置推定部１１３がＧＮＳＳ受信機２００の出力から自己位置推定を行う場合もあるが、これは省略する。まず図５の左のＳＬＡＭによる自己位置推定を説明する。図５の左は、ステップＳ１２０の処理の具体例である。この処理は自己位置推定部１１３が実行する。
＜ステップＳ１２１ａ＞
　ステップＳ１２１ａにおいて、自己位置推定部１１３は、各センサデータを取得する。
＜ステップＳ１２２ａ＞
　ステップＳ１２２ａにおいて、自己位置推定部１１３は、各特徴量を算出する。
＜ステップＳ１２３ａ＞
　ステップＳ１２３ａにおいて、自己位置推定部１１３は、スキャンマッチングを実施する。
＜ステップＳ１２４ａ＞
　ステップＳ１２４ａにおいて、自己位置推定部１１３は、姿勢を算出する。
＜ステップＳ１２５ａ＞
ステップＳ１２５ａにおいて、自己位置推定部１１３は、位置を算出する。
＜ステップＳ１２６ａ＞
　ステップＳ１２６ａにおいて、自己位置推定部１１３は、特徴量を補助記憶装置８３０に保存する。
　図５の右のＩＭＵ等の慣性センサによる自己位置推定を説明する。図５の右は、ステップＳ１２０の処理の具体例である。この処理は自己位置推定部１１３が実行する。
＜ステップＳ１２１ｂ＞
　ステップＳ１２１ｂにおいて、自己位置推定部１１３は、各センサデータを取得する。
＜ステップＳ１２２ｂ＞
　ステップＳ１２２ｂにおいて、自己位置推定部１１３は、車両５００の速度および姿勢を算出する。
＜ステップＳ１２３ｂ＞
　ステップＳ１２３ｂにおいて、自己位置推定部１１３は、車両５００の移動量を加算する。
＜ステップＳ１２４ｂ＞
　ステップＳ１２４ｂにおいて、自己位置推定部１１３は、車両５００の位置を算出する。
　図６は、ステップＳ１３０の信頼度推定部１１４による２種類の信頼度推定のフローを示す。まず図６の左の、位置分布を利用する信頼度推定を説明する。図６の左は、ステップＳ１３０の処理の具体例である。この処理は信頼度推定部１１４が実行する。
＜ステップＳ１３１ａ＞
　ステップＳ１３１ａにおいて、信頼度推定部１１４は、各位置の分布を算出する。
＜ステップＳ１３２ａ＞
　ステップＳ１３２ａにおいて、信頼度推定部１１４は、前時刻データと制御値から、現時刻における位置の分布を算出する。
＜ステップＳ１３３ａ＞
　ステップＳ１３３ａにおいて、信頼度推定部１１４は、前時刻と現時刻の両方の分布から情報量を算出する。
　図７は、信頼度推定部１１４による、信頼度スコアの算出方法を示す図である。図７の左は、信頼度推定部１１４が、確率分布として、複数の測位位置から得られた位置分布を示す確率分布を使用する場合を示している。図７の左は、ＧＰＳ計測位置に基づく。図７の右は、信頼度推定部１１が、確率分布として、測位信号とＩＭＵ３０１の出力データとから得られた位置分布を示す確率分布を使用する場合を示している。
＜ステップＳ１３４ａ＞
　ステップＳ１３４ａにおいて、信頼度推定部１１４は、情報量から信頼度スコアを決定する。
　図８および図９は情報量による、スコア値の算出を示す図である。図８および図９はカルバックライブラー情報量を用いるスコア値の算出を示している。図８の左は分布誤差が小さい場合、右は分布誤差が小さい場合を示している。図９では、推定値と観測値との分布の差が小さいほど情報量は小さく、差が大きいほど情報量は大きくなる。
＜ステップＳ１３５ａ＞
　ステップＳ１３５ａにおいて、信頼度推定部１１４は、ＧＮＳＳスコアが閾値以上かどうかを判定する。閾値以上の場合、処理はステップＳ１３６ａに進み、閾値未満の場合、処理は終了する。
＜ステップＳ１３６ａ＞
　ステップＳ１３６ａにおいて、信頼度推定部１１４は、ＧＮＳＳの位置情報をファクターに登録する。
　次に図６の右の、信頼度の算出に、フィルタによる予測位置と、観測位置との距離差分から、信頼度を推定する場合を説明する。
＜ステップＳ１３１ｂ＞
　ステップＳ１３１ｂにおいて、信頼度推定部１１４は、前フレーム速度、姿勢から、現フレームの位置を推定する。
＜ステップＳ１３２ｂ＞
　ステップＳ１３２ｂにおいて、信頼度推定部１１４は、位置の観測値と推定値との差分から、信頼度スコアを決定する。信頼度推定部１１４は、確率分布として、ＩＭＵ３０１から得られたデータから生成された状態方程式と、測位信号から得られた測位位置から生成された観測方程式とに基づき、フィルタ演算の結果によって生成された確率分布を使用する。
　図１０は、フィルタ演算によって信頼度スコアを算出する場合を示す。図１０では、状態方程式の入力をＩＭＵ値、観測値をＧＮＳＳとしたパーティクルフィルタまたはカルマンフィルタを使用する。図１０では、ｔ－１時の位置から入力にノイズを載せたパーティクルをばらまき粒子の分布を正規分布として表す。ｔ－１時の位置から入力は、今回ではＩＭＵの測定値である。ノイズそのものも推定パラメータとすることで分散を随時更新可能である。ただし計算量が粒子の数に伴い増加するため、処理速度と精度とはトレードオフの関係になる。
＜ステップＳ１３３ｂ＞
　ステップＳ１３３ｂにおいて、信頼度推定部１１４は、ＧＮＳＳスコアが閾値以上かどうかを判定する。閾値以上の場合、処理はステップＳ１３４ｂに進み、閾値未満の場合、処理は終了する。
＜ステップＳ１３４ｂ＞
　ステップＳ１３４ｂにおいて、信頼度推定部１１４は、ＧＮＳＳの位置情報をファクターに登録する。
　図１１は、ステップＳ１４０の自己位置切り替え判定部１１１の処理と、ステップＳ１８０のＧＮＳＳ収束判定部１１５の処理とを示す。まず図１１の左の、自己位置切り替え判定部１１１の処理を説明する。
＜ステップＳ１４１＞
　ステップＳ１４１において、自己位置切り替え判定部１１１は、ＧＮＳＳスコアが閾値以上かどうかを判定する。ＧＮＳＳスコアが閾値以上の場合、処理は終了する。ＧＮＳＳスコアが閾値未満の場合、処理はステップＳ１４２に進む。
＜ステップＳ１４２＞
　ステップＳ１４２において、自己位置切り替え判定部１１１は、切り替えフラグをＯＮにする。
　次に図１１の右の、ＧＮＳＳ収束判定部１１５の処理を説明する。
＜ステップＳ１８１＞
　ステップＳ１８１において、ＧＮＳＳ収束判定部１１５は、前位置からＳＬＡＭあるいはＩＭＵにより、現フレーム位置を推定する。
＜ステップＳ１８２＞
　ステップＳ１８２において、ＧＮＳＳ収束判定部１１５は、現フレームのＧＮＳＳ位置と、推定位置との誤差が閾値内かどうかを判定する。誤差が閾値内の場合、処理はステップＳ１８３に進む。誤差が閾値内にない場合、処理は終了する。
＜ステップＳ１８３＞
　ステップＳ１８３において、ＧＮＳＳ収束判定部１１５は、切り替えフラグをＯＦＦにする。
　図１２は、ステップＳ２２０のポーズ調整部１２２の処理を示す。
＜ステップＳ２２１＞
　ステップＳ２２１において、ポーズ調整部１２２は、ＳＬＡＭ位置と算出位置との誤差が閾値以上かどうかを判定する。誤差が閾値以上の場合、処理はステップＳ２２２に進む。誤差が閾値未満の場合、処理は終了する。
＜ステップＳ２２２＞
　ステップＳ２２２において、ポーズ調整部１２２は、設定フレーム内にＧＮＳＳファクターの登録があるかどうかを判定すする。登録がある場合、処理はステップＳ２２３に進む。登録がない場合、処理は終了する。
＜ステップＳ２２３＞
　ステップＳ２２３において、ポーズ調整部１２２は、現フレームと過去登録されたＧＮＳＳファクターのＧＮＳＳ情報を拘束条件に設定する。
＜ステップＳ２２４＞
　ステップＳ２２４において、ポーズ調整部１２２は、ポーズ調整を実施する。ポーズ調整部１２２は、第２の測位方式で得られた複数の位置を、第１の測位方式で得られた複数の位置で修正する。
　図１３は、ポーズ調整部１２２による、修正前を示す。図１４は、ポーズ調整部１２２による、修正後を示す。図１３、図１４では黒丸はＧＰＳでの測位位置を示し、白丸はＳＬＡＭによる推定位置を示す。ポーズ調整部１２２は、ｔ，ｔ＋５のＧＰＳ位置を拘束条件にして、ｔ＋１からｔ＋４のＳＬＡＭ位置および姿勢を、ＧＰＳ位置に基づき修正する。
＊＊＊実施の形態１の効果の説明＊＊＊
　位置推定装置１００では、信頼度推定部１１４が測位信号を用いる測位方式と、測位信号を用いない測位方式とのいずれかに切り替える際の、測位信号の信頼度の判定方式の提供を目的とする。
　自動運転あるいは移動体の自律移動において位置推定の手法をＧＰＳに依存する場合、ＧＰＳの電波を受信できないビルまたは高架下を走行する際にはＧＰＳの位置が取得できない、またはマルチパスの影響により誤差が大きくなり正確な位置が取得できないといった課題があった。位置推定装置１００は、信頼度推定部１１４を備えたので、計測したＧＰＳ測位位置の中でも信頼度が高いと判定されたＧＰＳ測位位置のみを用いる。これにより、ＧＰＳ測位信号の正常受信時にはＧＰＳにより絶対位置を求めることができる。また、ＧＰＳ測位信号の信頼度が低、または受信できない場合には、ＧＰＳを正常に受信できている際に推定した絶対座標における姿勢を用いて、ＳＬＡＭにより位置を推定する事で高精度に自己位置を補間ができる。
　なお、位置推定装置１００は、車両５００に限らずドローンのような飛行体にも適用可能である。
　以上、実施の形態１について説明した。実施の形態１のうち、１つの技術事項を部分的に実施しても構わない。あるいは、実施の形態１の含む複数の技術事項のうち、２つ以上を部分的に組み合わせて実施しても構わない。
　１００　位置推定装置、１０１　位置推定プログラム、１１０　自己位置管理部、１１１　自己位置切り替え判定部、１１２　自己位置記憶部、１１３　自己位置推定部、１１４　信頼度推定部、１１５　ＧＮＳＳ収束判定部、１２０　センサデータ管理部、１２１　特徴量保存部、１２２　ポーズ調整部、２００　ＧＮＳＳ受信機、３００　内部センサ、３０１　ＩＭＵ、３０２　ＩＮＳ、４００　外部センサ、４０１　ライダー、４０２　超音波センサ、４０３　ミリ波レーダ、４０４　カメラ、５００　車両、６１０　測位衛星、８１０　プロセッサ、８２０　主記憶装置、８３０　補助記憶装置、８４０　入力ＩＦ、８５０　出力ＩＦ、８６０　通信ＩＦ、８７０　信号線。

Claims (7)

  1. 　測位衛星の送信する測位信号から得られた測位位置の統計的な確率分布を用いて、新たに受信された測位信号から算出された測位位置の信頼度を推定する信頼度推定部と、
    　前記信頼度推定部によって推定された測位信号の信頼度に基づいて、測位信号を用いて測位する第１の測位方式と、測位信号を用いずに測位する第２の測位方式との、いずれかを選択して位置を推定する選択部と、
    を備える位置推定装置。
  2. 　前記信頼度推定部は、
    　前記確率分布として、複数の測位位置から得られた位置分布を示す確率分布を使用する請求項１に記載の位置推定装置。
  3. 　前記信頼度推定部は、
    　前記確率分布として、測位信号と慣性装置の出力データとから得られた位置分布を示す確率分布を使用する請求項１に記載の位置推定装置。
  4. 　前記信頼度推定部は、
    　前記確率分布として、慣性装置から得られたデータから生成された状態方程式と、測位信号から得られた測位位置から生成された観測方程式とに基づき、フィルタ演算の結果によって生成された確率分布を使用する請求項１に記載の位置推定装置。
  5. 　前記位置推定装置は、さらに、
    　前記第２の測位方式で得られた複数の位置を、前記第１の測位方式で得られた複数の位置で修正するポーズ調整部を備える請求項１から請求項４のいずれか１項に記載の位置推定装置。
  6. 　コンピュータに、
    　測位衛星の送信する測位信号から得られた測位位置の統計的な確率分布を用いて、新たに受信された測位信号から算出された測位位置の信頼度を推定する信頼度推定処理と、
    　前記信頼度推定処理によって推定された測位信号の信頼度に基づいて、測位信号を用いて測位する第１の測位方式と、測位信号を用いずに測位する第２の測位方式との、いずれかを選択して位置を推定する選択処理と、
    を実行させる位置推定プログラム。
  7. 　コンピュータが、
    　測位衛星の送信する測位信号から得られた測位位置の統計的な確率分布を用いて、新たに受信された測位信号から算出された測位位置の信頼度を推定し、
    　推定された測位信号の信頼度に基づいて、測位信号を用いて測位する第１の測位方式と、測位信号を用いずに測位する第２の測位方式との、いずれかを選択して位置を推定する、
    位置推定方法。
PCT/JP2021/013221 2021-03-29 2021-03-29 位置推定装置、位置推定プログラムおよび位置推定方法 WO2022208588A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP2021560130A JP7046281B1 (ja) 2021-03-29 2021-03-29 位置推定装置、位置推定プログラムおよび位置推定方法
PCT/JP2021/013221 WO2022208588A1 (ja) 2021-03-29 2021-03-29 位置推定装置、位置推定プログラムおよび位置推定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2021/013221 WO2022208588A1 (ja) 2021-03-29 2021-03-29 位置推定装置、位置推定プログラムおよび位置推定方法

Publications (1)

Publication Number Publication Date
WO2022208588A1 true WO2022208588A1 (ja) 2022-10-06

Family

ID=81255857

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/013221 WO2022208588A1 (ja) 2021-03-29 2021-03-29 位置推定装置、位置推定プログラムおよび位置推定方法

Country Status (2)

Country Link
JP (1) JP7046281B1 (ja)
WO (1) WO2022208588A1 (ja)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6408245B1 (en) * 2000-08-03 2002-06-18 American Gnc Corporation Filtering mechanization method of integrating global positioning system receiver with inertial measurement unit
JP2013108961A (ja) * 2011-11-24 2013-06-06 Toyota Central R&D Labs Inc 測位装置及びプログラム
WO2016059930A1 (ja) * 2014-10-17 2016-04-21 ソニー株式会社 装置、方法及びプログラム
JP2019074505A (ja) * 2017-10-16 2019-05-16 三星電子株式会社Ｓａｍｓｕｎｇ Ｅｌｅｃｔｒｏｎｉｃｓ Ｃｏ．，Ｌｔｄ． 位置推定方法、装置及びコンピュータプログラム
WO2020202522A1 (ja) * 2019-04-04 2020-10-08 三菱電機株式会社 車両測位装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6408245B1 (en) * 2000-08-03 2002-06-18 American Gnc Corporation Filtering mechanization method of integrating global positioning system receiver with inertial measurement unit
JP2013108961A (ja) * 2011-11-24 2013-06-06 Toyota Central R&D Labs Inc 測位装置及びプログラム
WO2016059930A1 (ja) * 2014-10-17 2016-04-21 ソニー株式会社 装置、方法及びプログラム
JP2019074505A (ja) * 2017-10-16 2019-05-16 三星電子株式会社Ｓａｍｓｕｎｇ Ｅｌｅｃｔｒｏｎｉｃｓ Ｃｏ．，Ｌｔｄ． 位置推定方法、装置及びコンピュータプログラム
WO2020202522A1 (ja) * 2019-04-04 2020-10-08 三菱電機株式会社 車両測位装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
IWASE, TATSUYA ET AL.: "The Reliability of Positioning After Removing GPS Multipath Error Based on INS", TRANSACTIONS OF SOCIETY OF AUTOMOTIVE ENGINEERS OF JAPAN, vol. 44, no. 1, 2013, pages 119 - 124, XP055977204, Retrieved from the Internet <URL:http://doi.org/10.11351/jsaeronbun.44.119> *

Also Published As

Publication number Publication date
JPWO2022208588A1 (ja) 2022-10-06
JP7046281B1 (ja) 2022-04-01

Similar Documents

Publication Publication Date Title
US8860609B2 (en) Loosely-coupled integration of global navigation satellite system and inertial navigation system
US8412456B2 (en) Loosely-coupled integration of global navigation satellite system and inertial navigation system: speed scale-factor and heading bias calibration
CN107710016B (zh) 用于在实时运动模式与精确定位模式之间切换的卫星导航接收器及方法
CN108027443B (zh) 用于在实时运动模式与精确定位模式之间切换的卫星导航接收器及方法
JP5270184B2 (ja) 衛星航法／推測航法統合測位装置
US8560234B2 (en) System and method of navigation based on state estimation using a stepped filter
CN109313272B (zh) 使用速度积分的改进gnss接收器
US20100036613A1 (en) Methods and systems for implementing an iterated extended kalman filter within a navigation system
US9423486B2 (en) Position calculating method and position calculating device
KR20140034043A (ko) 내비게이션 수신기를 추적하기 위한 장치 및 방법
CN108029092B (zh) 用于移动终端的定位方法、装置及移动终端
US11243311B2 (en) Method and device for determining a position of a mobile object
JP2001264076A (ja) カーナビゲーション装置
WO2019180963A1 (ja) 走行支援システム、走行支援方法、および走行支援プログラム
US9952330B2 (en) Automotive dead reckoning with dynamic calibration and/or dynamic weighting
WO2022208588A1 (ja) 位置推定装置、位置推定プログラムおよび位置推定方法
JP2011163817A (ja) 測位装置及びプログラム
WO2020149014A1 (ja) 衛星選択装置、及びプログラム
JP2010145178A (ja) 移動体位置特定装置
US11762104B2 (en) Method and device for locating a vehicle
JP2023548513A (ja) 多義性の解明によって少なくとも１つのｇｎｓｓ衛星信号を評価する方法
CN113777639A (zh) 基于多普勒观测值约束的强跟踪卡尔曼滤波单点定位计算方法、设备、介质、产品
JP2012052905A (ja) 位置測定装置、および位置測定方法
JP6735696B2 (ja) 複合測位装置、複合測位方法および複合測位プログラム
JP4657050B2 (ja) 速度ベクトル決定システム

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2021560130

Country of ref document: JP

Kind code of ref document: A

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

Ref document number: 21934764

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21934764

Country of ref document: EP

Kind code of ref document: A1