Position estimation device and method

Abstract

The purpose of the present invention is to provide technology that makes it possible to estimate the position of a moving body with high accuracy in various situations.　A position estimation device according to the present invention is for estimating the position of a moving body, and is characterized by comprising: a first computation unit that computes a first positional state quantity on the basis of point cloud data of an object which is measured and generated by a detection / range sensor for measuring the distance between the moving body to the object and detecting reflection intensity, and computes a second positional state quantity on the basis of acceleration data and angular velocity data which are measured by an inertial sensor; a second computation unit that computes position estimation information on the basis of the first positional state quantity, the second positional state quantity, and third positional state quantity, which is acquired on the basis of GNSS data from a positioning satellite; and a stop detection unit that ascertains the traveling state of the moving body, and outputs, to the second computation unit, a fourth positional state quantity, which is computed on the basis of the acceleration data measured by the inertial sensor upon determination of a stop.

Description

Position estimation device and method

[0001] The present invention relates to a position estimation apparatus and method.

[0002] A composite position estimation system is generally known that uses two types of sensors, a GNSS and an IMU, to measure the position of a moving object and correct the estimation error using a Kalman filter to calculate the position, velocity, and attitude. However, the gyro sensor installed in the IMU generates noise that increases over time as the object is operated for a long period of time, resulting in errors in the calculated values. Therefore, in Patent Document 1, an extended ZUPT process (Zero velocity UP daTe) is introduced into a position estimation device consisting of a GNSS / IMU when the object is stopped, thereby fixing the azimuth angle while the object is stopped, thereby preventing the influence of sensor errors.

[0003] JP 2012-193965 A

[0004] However, when the frequency of stops is low, the extended ZUPT process cannot be performed, resulting in accumulated errors. Furthermore, in areas where GNSS reception is unavailable (e.g., inside tunnels or in areas with high-rise buildings), positioning is performed using the IMU alone, which may result in further accumulated errors. Further measures are essential to improve position estimation accuracy even in various situations that could cause such errors. Therefore, the present invention aims to provide technology that enables highly accurate estimation of the position of a moving object even in various situations.

[0005] In order to solve the above problems, one representative position estimation device of the present invention is a position estimation device that estimates the position of a moving body, and is characterized by comprising: a first calculation unit that calculates a first position state quantity based on point cloud data of the object measured and generated by a detection / ranging sensor that measures the distance from the moving body to the object and detects reflection intensity, and also calculates a second position state quantity based on acceleration data and angular velocity data measured by an inertial sensor; a second calculation unit that calculates position estimation information based on the first position state quantity, the second position state quantity, and a third position state quantity obtained from GNSS data from a positioning satellite; and a stop detection unit that locates the running state of the moving body and outputs a fourth position state quantity calculated based on the acceleration data measured by the inertial sensor to the second calculation unit when a stop determination is made.

[0006] According to the present invention, it is possible to estimate the position of a moving object with high accuracy even under various circumstances. Problems, configurations, and effects other than those described above will become apparent from the description of the following embodiments.

[0007] Fig. 1 is a block diagram showing the overall configuration of a composite position estimation system. Fig. 2 is a block diagram showing an example of the functional configuration of a composite position estimation system installed in a moving body. Fig. 3 is a diagram showing the processing and data flow in a position estimation device in an embodiment. Fig. 4 is a functional configuration diagram of a strapdown navigation 150. Fig. 5 is a flowchart showing a location method of a stop detection unit 110.

[0008] Hereinafter, an embodiment of the present invention will be described with reference to the drawings. Note that the present invention is not limited to this embodiment. In addition, in the description of the drawings, the same parts are designated by the same reference numerals.

[0009] (Explanation of Terms) The terms used in this disclosure are as follows: "Mobile body" is not particularly limited, and includes, for example, a vehicle, an airplane, a ship, a motorcycle, a bicycle, a train, etc. "Position" refers to three-dimensional coordinate values ​​(latitude, longitude, altitude). "Velocity" refers to the amount of change in the position of a mobile body per unit time. "Attitude" refers to the elevation angle (pitch angle), rotation angle (roll angle), and azimuth angle (yaw angle). "GNSS" is an abbreviation for "Global Navigation Satellite System." A GNSS receiver uses a GNSS antenna to observe positioning signals transmitted from positioning satellites using satellite communications, and determines the position, velocity, and azimuth angle based on the observation results. "IMU" is an abbreviation for "Inertial Measurement Unit." An IMU is a sensor that measures the acceleration and angular velocity of a moving object. The IMU measures acceleration and angular velocity in three axes. The IMU is an example of an inertial sensor, and in this disclosure, the inertial sensor is sometimes referred to as an IMU. "LiDAR" is an abbreviation for "Laser Imaging Detection and Ranging" (a remote sensing technology using laser light). LiDAR is a detection and ranging sensor that calculates the distance between a moving object and an object using laser light and detects the reflection intensity of the object. LiDAR can calculate the relative distance between the moving object itself and an arbitrary object based on the difference between the transmission time of the laser light it emits and the reception time of the light reflected from the arbitrary object. In addition, the LiDAR generates and transmits point cloud data, which is a collection of reflection intensity data of the arbitrary object. In this case, the object is not particularly limited and may be a structure located on the ground that is located around the moving object. For example, these include utility poles, buildings, equipment, rails, sleepers, overhead wires, etc. LiDAR is an example of a detection and ranging sensor, and in this disclosure, the detection and ranging sensor may also be referred to as LiDAR.

[0010] 1 is a block diagram showing the overall configuration of a composite position estimation system. The composite position estimation system 10 includes three types of sensors: a GNSS receiver 210 (and a GNSS antenna 211), a detection and ranging sensor 220, and an inertial sensor 230, as well as a position estimation device 100. The inertial sensor 230 includes an acceleration sensor 231 and a gyro sensor 232. Measurement values ​​from the three types of sensors are input to the position estimation device 100, which performs calculations to calculate the position, velocity, and attitude of a moving object.

[0011] Fig. 2 is a block diagram showing an example of the functional configuration of a composite position estimation system installed in a mobile body. In the example shown in Fig. 2, a GNSS receiver 210, a detection and ranging sensor 220, an inertial sensor 230, and a position estimation device 100 are installed inside the mobile body 20, and a GNSS antenna 211 is installed outside the mobile body 20. However, the installation method is not limited to this. For example, it is also possible to install the GNSS antenna 211 inside or install the sensor outside as appropriate.

[0012] <Configuration of Position Estimation Device> Next, the configuration of the position estimation device 100 according to the embodiment will be described. The position estimation device 100 includes a recording unit 190, a first calculation unit 120, a second calculation unit 130, and a stop detection unit 110. As a hardware configuration, the first calculation unit 120, the second calculation unit 130, and the stop detection unit 110 execute program processing using a computer processor such as a CPU. The recording unit 190 stores data or programs (applications) and may be a random access semiconductor memory, a storage device, or a storage medium (either volatile or non-volatile). These functional units are connected via wire or wirelessly.

[0013] (Recording Unit) The recording unit 190 records data used for calculations by the first calculation unit 120. The data includes GNSS data 191, characteristic feature data 192, orbital attitude data 193, point cloud data 194, acceleration data 195, and angular velocity data 196. The GNSS data 191 is NMEA data measured by the GNSS receiver 210. The NMEA data includes GNSS time, position information, velocity information, positioning mode status information, DOP, positioning satellite information (e.g., satellite number, elevation angle to the satellite, azimuth angle to the satellite), and azimuth angle. The characteristic feature data 192 is data including the position, attitude, and reflection intensity of the characteristic feature 300, and is stored in advance in the recording unit 190. The characteristic feature 300 is, for example, a pole with a reflector attached installed next to a railroad track, an object configured to enter the field of view of light from the LiDAR and reflect it with an appropriate intensity that can be measured. The track attitude data 193 is data related to the track attitude 310 and is stored in advance in the recording unit 190. The track attitude 310 may be, for example, existing rails, sleepers, or other existing structures, and the position and attitude of the moving object can be calculated from the reflected light when scanned with LiDAR. The point cloud data 194 is data measured and generated by LiDAR and includes the distance to the object, the angle, and the reflection intensity. The acceleration data 195 is data measured by the acceleration sensor 231 constituting the IMU and includes the acceleration in three axes corresponding to time. The angular velocity data 196 is data measured by the gyro sensor 232 constituting the IMU and includes the angular velocity in three axes corresponding to time. In other words, the angular velocity data 196 includes the angular velocity of the elevation angle (pitch angle), the angular velocity of the rotation angle (roll angle), and the angular velocity of the azimuth angle (yaw angle).

[0014] (First Calculation Unit) The first calculation unit 120 performs processing for characteristic part recognition 121, track recognition 122, overhead line recognition 123, surrounding shape recognition 124, and strapdown navigation 150. The characteristic part recognition 121, track recognition 122, overhead line recognition 123, and surrounding shape recognition 124 are all performed using point cloud data acquired by the detection and ranging sensor 220, while the strapdown navigation 150 is performed using data acquired by the inertial sensor 230. Specific details of the processing will be described later.

[0015] (Second Calculation Unit) The second calculation unit 130 performs processing using a Kalman filter A131 and a Kalman filter B132. Generally, a Kalman filter is a technique that compares an estimated value of a certain state quantity of an object with a separately observed value, assigns a weight (optimal Kalman gain) to minimize the error according to the likelihood of the two, and corrects (filters) and updates the estimated value. An estimated value is calculated from a state equation at a period (estimation period) in which a state quantity that becomes an estimated value is acquired. When an observed value is updated at a period (observation period) in which a state quantity that becomes a separately observed value is acquired, the optimal Kalman gain, the observation error, and the covariance of the observation residual (hereinafter collectively referred to as the "Kalman correction value") are also updated using the observation equation, and the most recent estimated value is updated using the updated Kalman correction value. Note that, of the Kalman correction values, the following mainly focuses on the optimal Kalman gain. The Kalman filter A131 filters the state quantities acquired by the LiDAR as estimated values ​​and observed values. The Kalman filter B132 performs filtering using the state quantities acquired by the IMU as estimated values ​​and the state quantities acquired by the LiDAR (but filtered by the Kalman filter A) and the GNSS as observed values, and finally outputs position estimation information related to the position, speed, and attitude of the moving object. The specific contents of the filtering will be described later.

[0016] (Stop Detection Unit) Since the gyro sensor 230 tends to drift over time and increase errors, it is preferable to correct the position estimation information obtained by the Kalman filter B 132 by providing more accurate observation values ​​acquired in a traveling state at a predetermined timing. The stop detection unit 110 further corrects the output of the Kalman filter B using the attitude measured when the mobile object 20 is located as being in a stopped or slow-moving linear motion state as an observation value. The stop detection unit 110 has the functions of traveling state location 111, stopped state location 112, and stop determination 113. Specific details will be described later.

[0017] <Functions of the Position Estimation Apparatus> Next, a description will be given of the processing performed by the functional units of the position estimation apparatus 100 according to the embodiment. Fig. 3 is a diagram showing the processing and data flow in the position estimation apparatus according to the embodiment.

[0018] (Characteristic Part Recognition) The characteristic part recognition 121 identifies the characteristic part 300 based on the grid map and reflection intensity created from the point cloud data 194 acquired by the detection and ranging sensor 220. If the current position is within the range of the position of the characteristic part data 192 stored in the recording unit 190, the characteristic part recognition 121 identifies the position of the moving object from the characteristic part data 192. The track attitude 310 is also determined using a similar method. If the current position is within the range of the position of the track attitude data 193 stored in the recording unit 190, the position and attitude of the moving object are identified from the track attitude data 193. The characteristic part 300 can be accurately positioned by reading an attached identification label, for example. In contrast, the track attitude 310 does not require special installation costs, but is often an existing installation without an identification label, and positioning it may require computational costs. The characteristic part recognition 121 can determine the position and attitude of the moving object by appropriately combining the characteristic part 300 and the track attitude 310 according to their characteristics.

[0019] (Trajectory Recognition, Catenary Recognition, Surrounding Shape Recognition) The track recognition 122 extracts point cloud data 194 near the rails from the point cloud data 194 acquired by the detection and ranging sensor 220 and creates a grid map. The extracted point cloud data 194 is divided into an arbitrary number of equal rectangular parallelepiped areas in the direction of travel. For each divided rectangular parallelepiped area, point clouds near the rails are extracted, and grid maps separated by a sampling period are compared to calculate the position of correlation values ​​where the point clouds overlap most for each rectangular parallelepiped area. The speed of the moving object is calculated by dividing the amount of movement of the correlation value position for each grid map by the sampling period. The angular velocity of the moving object is also calculated at this time. The catenary recognition 123 extracts point cloud data 194 near the catenary wires from the point cloud data 194 acquired by the detection and ranging sensor 220 and creates a grid map. The grid map created for each sampling period is then used to calculate the speed and angular velocity of the moving object using a method similar to that of the track recognition 122. Surrounding shape recognition 124 extracts point cloud data 194 near any object around the moving object from point cloud data 194 acquired by detection and ranging sensor 220, and creates a grid map. Then, using the grid map created for each sampling period, calculates the speed and angular velocity of the moving object in a manner similar to that of track recognition 122. Track recognition 122 and overhead line recognition 123 are types of surrounding shape recognition 124, and are recognition processes that are mainly intended for railway trains, so they may be applied appropriately depending on the type of moving object.

[0020] 4 is a functional configuration diagram of the strapdown navigation 150. The strapdown navigation 150 calculates the position, velocity, and attitude based on the acceleration data 195 and angular velocity data 196 acquired by the inertial sensor 230, and outputs the calculated values ​​together with the acceleration bias and angular velocity bias to the Kalman filter B 132 of the second calculation unit 130.

[0021] The coordinate transformation matrix update attitude calculation 151 acquires the three-dimensional angular velocity of the body frame, and based on the acquired angular velocity, acquires a matrix for converting the body frame to the computer frame, thereby acquiring the attitude. It also receives feedback of the angular velocity bias filtered by the Kalman filter B132. The "body frame" is a coordinate system based on the vehicle, where, for example, the forward direction is represented as x, the width direction as y, and the height direction as z. The "computer frame" is a coordinate system created within the computer, where, with the vehicle position as the origin, for example, the north direction is represented as x, the west direction as y, and the downward direction as z. The acceleration coordinate transformation 152 acquires the three-dimensional acceleration of the body frame and acquires the acceleration of the computer frame using the coordinate transformation matrix. It also receives feedback of the acceleration bias filtered by the Kalman filter B132. Velocity correction 153 calculates velocity from the gravity component, the Earth's rotational velocity, and acceleration in the Computer Frame. Relative angular velocity calculation 154 calculates the relative angular velocity in the Computer Frame from the velocity calculated by velocity correction 153 and the Earth's radius of curvature. Coordinate transformation matrix update position calculation 155 obtains a transformation matrix from the Computer Frame to the Earth-Centered Earth-Fixed Frame from the relative angular velocity, and calculates the wander angle, latitude, and longitude from the transformation matrix. The "Earth-Centered Earth-Fixed Frame" rotates with the Earth, and is represented by z on the Earth's axis, with the direction to the North Pole being positive, x in the direction of the intersection of the Greenwich meridian and the equatorial plane, and y in the direction forming a right-handed system with z and x on the equatorial plane including x. The "wander angle" is the angle with westward movement relative to true north as positive. The Earth rotation angular velocity calculation 156 calculates the Earth rotation angular velocity in the computer frame from the wander angle and latitude. The Earth gravity model calculation 157 calculates the gravity component in the computer frame from a transformation matrix from the computer frame to the Earth-Centered Earth-Fixed Frame.The earth model calculation 158 calculates the radius of curvature of the earth in the computer frame from the latitude and wander angle.

[0022] (Kalman Filter A) With regard to Kalman filter A 131, the velocity and angular velocity calculated in any of the three areas of track recognition 122, overhead line recognition 123, and surrounding shape recognition 124 are the state quantities input and output to Kalman filter A. As a specific example, estimated values ​​of velocity and angular velocity are calculated from a state equation using the velocity and angular velocity acquired by track recognition 122, and in the next cycle, the velocity and angular velocity acquired by surrounding shape recognition 124 are used as observed values ​​to compare the two and update the optimal Kalman gain (Kalman correction value). For example, if the acquisition cycle of point cloud data by LiDAR is 1 s, updating is performed every 1 s.

[0023] (Kalman Filter B) For Kalman filter B 132, data from three types of sensors, LiDAR, GNSS, and IMU, are used as state quantities. In this embodiment, the state quantities input to Kalman filter B 132 are the position, velocity, attitude, acceleration bias, and angular velocity bias calculated from IMU data by strapdown navigation 150, the position, velocity, and attitude (azimuth) acquired by GNSS, the velocity and angular velocity acquired by LiDAR (however filtered by Kalman filter A), and the position or attitude acquired from LiDAR by feature part recognition 121. As a specific example, when the state quantities acquired by the IMU are used as estimated values ​​and the state quantities acquired by the LiDAR and GNSS are used as observed values, the IMU data acquisition period is 1 s, while the LiDAR and GNSS data acquisition periods are 2 s and 3 s, respectively. Therefore, the estimated values ​​calculated using the state equations at the estimation period (1 s) are input to the observation equations using the observed values ​​acquired at the observation period (2 s or 3 s) to update the Kalman correction values ​​(optimal Kalman gain, etc.), and the position estimation information (position, velocity, and attitude) is also updated to improve the accuracy of the position estimation. In this embodiment, further improvement in accuracy can be expected by filtering the data acquired by the LiDAR using Kalman filter A and inputting it to Kalman filter B. Position estimation information including position, velocity, and attitude is then output from Kalman filter B, which becomes the final output of the position estimation device 100. At this time, Kalman filter B also outputs acceleration bias and angular velocity bias for the purpose of feedback to the strap navigation 150 (see FIG. 4).

[0024] LiDAR can recognize the position and attitude by characteristic part recognition 121 using characteristic parts 300 and track attitude 310, even in an environment where GNSS cannot be received and the vehicle position is unknown, such as inside a tunnel. Furthermore, unlike a simple speed sensor, it is possible to acquire state quantities related to position, attitude, and angular velocity in addition to speed by generating three-dimensional point cloud data. In this way, in the embodiment, by using a detection / ranging sensor capable of acquiring point cloud data such as LiDAR as the sensor, it is possible to expect improved functionality of the position estimation device even under a variety of conditions.

[0025] (Location Method) A location method in the stop detection unit 110 will be described below. FIG.

[0026] The traveling state location 111 first determines that the moving object 20 is in a normal traveling state while traveling, then acquires the angular velocity bias calculated by the strapdown navigation 150 of the first calculation unit, and determines whether the angular velocity bias is less than a predetermined threshold (S410). If the result is NO, the process returns to the normal traveling state location. If the result is YES, the process determines whether the time during which the angular velocity bias is less than the threshold is equal to or greater than a predetermined linear motion duration threshold (S420). If the result is NO, the process returns to the normal traveling state location. If the result is YES, the process determines that the moving object 20 is in a linear motion state, and the process proceeds to the stopped state location 112. The stopped state location 112 determines whether the velocity calculated by the Kalman filter B 132 is less than a predetermined motion state threshold and whether the characteristic feature recognition 121 in the first calculation unit 120 has recognized a predetermined position (e.g., a characteristic feature 300 installed at a bus stop) (S430). If the result is NO, the process returns to the normal traveling state location. If the result is YES, the process proceeds to the stop determination 113. The travel state at the time of stop determination includes not only a state where the vehicle is stopped at a station or the like, but also a state where the vehicle is moving slowly in a linear motion state below a predetermined speed. The various thresholds used as the criteria for determining the travel state described above may be appropriately determined from past empirical data within a range in which the vehicle can be considered to be in a stopped or slowly moving linear motion state. The stop determination unit 113 calculates the attitude of the vehicle 20 from the acceleration data values ​​of the acceleration sensor, which has less error than a gyro sensor, and inputs the calculated attitude as an observation value into the observation equation of the Kalman filter B132 to update the Kalman correction values ​​for the attitude, acceleration bias, and angular velocity bias, thereby correcting the position estimation information, which is the final output. The updated angular velocity bias is fed back to the strapdown navigation unit 150.

[0027] The following are equations (1) to (5) that represent the attitude obtained by the stop determination 113. The above formula (1) shows the relationship between the acceleration of the computer frame and the acceleration of the body frame when the vehicle is stationary. The formula (2) shows the above rotation matrix when roll, pitch, and yaw angles are used. The above equation (2) can be summarized to obtain the following equation (3), which is solved for the roll angle and pitch angle to obtain equations (4) and (5). In this way, by inputting the attitude calculated in the stop determination 113 as an observation value to the Kalman filter B, it is possible to realize more accurate position estimation.

[0028] Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and various modifications are possible without departing from the spirit and scope of the present invention. For example, in the embodiment, the position estimation device 100 is installed on the mobile object 20, but some or all of the functional units may be installed on the ground, and data and commands may be transmitted and received via a wireless or wired network. Furthermore, in the embodiment, in the calculation of the Kalman filter B, filtering is performed using the state quantities obtained by the inertial sensor as estimated values ​​and the state quantities obtained by the detection / ranging sensor or GNSS data as observed values. However, the estimated values ​​and observed values ​​may be optimally selected depending on the measurement period and accuracy of each sensor.

[0029] The present invention is not limited to the following embodiments, which may be included in the content of the present invention: (Aspect 1) A position estimation device for estimating the position of a moving body, comprising: a first calculation unit that calculates a first position state quantity based on point cloud data of the object measured and generated by a detection / ranging sensor that measures the distance from the moving body to the object and detects reflection intensity, and that calculates a second position state quantity based on acceleration data and angular velocity data measured by an inertial sensor, a second calculation unit that calculates position estimation information based on the first position state quantity, the second position state quantity, and a third position state quantity acquired from GNSS data from a positioning satellite, and a stop detection unit that locates the traveling state of the moving body, and that outputs a fourth position state quantity calculated based on the acceleration data measured by the inertial sensor to the second calculation unit when a stop is determined. (Aspect 2) The position estimation device according to Aspect 1, characterized in that the first position state quantity includes the speed of the moving body and the position of the moving body calculated from a characteristic part, and the second position state quantity, the third position state quantity, and the position estimation information include the position, speed, and attitude of the moving body. (Aspect 3) The position estimation device according to Aspect 1 or 2, characterized in that the second calculation unit includes a Kalman filter B that sets the first position state quantity or the third position state quantity as an observation value, sets the second position state quantity as an estimate value, calculates a Kalman correction value, filters the estimate value, and outputs the position estimation information. (Aspect 4) The position estimation device according to Aspect 3, characterized in that the first position state quantity is a state quantity filtered by Kalman filter A. (Aspect 5) The position estimation device according to any one of Aspects 1 to 4, characterized in that the second calculation unit corrects the position estimation information based on the fourth position state quantity. (Aspect 6) The position estimation device according to any one of Aspects 1 to 5, characterized in that the fourth position state quantity is the attitude of the moving body.(Aspect 7) A position estimation device according to any one of Aspects 1 to 6, characterized in that, in the location of the running state of the stop detection unit, the stop determination is made when it is determined that the value of the angular velocity bias calculated by the first calculation unit based on the angular velocity data measured by the inertial sensor is less than a predetermined threshold, the state in which the value of the angular velocity bias is less than the predetermined threshold continues for a linear motion duration value or more, the velocity output by the Kalman filter B is less than a motion state threshold, and the first calculation unit has recognized a predetermined characteristic part. (Aspect 8) A position estimation method for estimating the position of a moving body, characterized in that a first calculation unit calculates a first position state quantity based on point cloud data of the object measured and generated by a detection / ranging sensor that measures the distance from the moving body to the object and detects reflection intensity, and calculates a second position state quantity based on acceleration data and angular velocity data measured by an inertial sensor, a second calculation unit calculates position estimation information based on the first position state quantity, the second position state quantity, and a third position state quantity acquired from GNSS data from a positioning satellite, and a stop detection unit locates the traveling state of the moving body, and when a stop is determined, outputs a fourth position state quantity calculated based on the acceleration data measured by the inertial sensor to the second calculation unit. (Aspect 9) The position estimation method according to Aspect 8, characterized in that the first position state quantity includes the speed of the moving body and the position of the moving body calculated from characteristic parts, and the second position state quantity, the third position state quantity, and the position estimation information include the position, speed, and attitude of the moving body. (Aspect 10) A position estimation method according to Aspect 8 or 9, wherein Kalman filter B of the second calculation unit uses the first position state quantity or the third position state quantity as an observation value, uses the second position state quantity as an estimate value, calculates a Kalman correction value, filters the estimate value, and outputs the position estimation information. (Aspect 11) A position estimation method according to Aspect 10, characterized in that the first position state quantity is a state quantity filtered by Kalman filter A. (Aspect 12) A position estimation method according to any one of Aspects 8 to 11, characterized in that the second calculation unit corrects the position estimation information based on the fourth position state quantity.(Aspect 13) The position estimation method according to any one of Aspects 8 to 12, characterized in that the fourth position state quantity is the attitude of the moving body. (Aspect 14) The position estimation method according to any one of Aspects 8 to 13, characterized in that, in locating the running state of the stop detection unit, the stop determination is made when it is determined that a value of an angular velocity bias calculated by a first calculation unit based on angular velocity data measured by the inertial sensor is less than a predetermined threshold, a state in which the value of the angular velocity bias is less than the predetermined threshold continues for a linear motion duration value or more, the velocity output by the Kalman filter B is less than a motion state threshold, and the first calculation unit has recognized a predetermined characteristic portion.

[0030] 10... Composite position estimation system, 20... Mobile body, 100... Position estimation device, 110... Stop detection unit, 111... Running state location, 112... Stop state location, 113... Stop judgment, 120... First calculation unit, 121... Characteristic part recognition, 122... Track recognition, 123... Overhead line recognition, 124... Surrounding shape recognition, 130... Second calculation unit, 131... Kalman filter A, 132... Kalman filter B, 150... Strapdown navigation, 151... Coordinate transformation matrix update attitude calculation, 152... Acceleration coordinate transformation, 153... Velocity correction, 154... Relative angular velocity Calculation, 155... Coordinate transformation matrix update position calculation, 156... Earth rotation angular velocity calculation, 157... Earth gravity model calculation, 158... Earth model calculation, 190... Recording unit, 191... GNSS data, 192... Characteristic part data, 193... Orbital attitude data, 194... Point cloud data, 195... Acceleration data, 196... Angular velocity data, 210... GNSS receiver, 211... GNSS antenna, 220... Detection and ranging sensor, 230... Inertial sensor, 231... Acceleration sensor, 232... Gyro sensor, 300... Characteristic part, 310... Orbital attitude

Claims

1. A position estimation device that estimates the position of a moving body, comprising: a first calculation unit that calculates a first position state quantity based on point cloud data of the object measured and generated by a detection and ranging sensor that measures the distance from the moving body to the object and detects reflection intensity, and that calculates a second position state quantity based on acceleration data and angular velocity data measured by an inertial sensor; a second calculation unit that calculates position estimation information based on the first position state quantity, the second position state quantity, and a third position state quantity acquired from GNSS data from a positioning satellite; and a stop detection unit that locates the traveling state of the moving body and, when a stop is determined, outputs a fourth position state quantity calculated based on the acceleration data measured by the inertial sensor to the second calculation unit.

2. The position estimation device described in claim 1, characterized in that the first position state quantity includes the speed of the moving body and the position of the moving body calculated from characteristic parts, and the second position state quantity, the third position state quantity, and the position estimation information include the position, speed, and attitude of the moving body.

3. A position estimation device according to claim 1 or 2, characterized in that the second calculation unit is provided with a Kalman filter B that uses the first position state quantity or the third position state quantity as an observed value, the second position state quantity as an estimated value, calculates a Kalman correction value, filters the estimated value, and outputs the position estimation information.

4. A position estimation device according to claim 3, wherein the first position state quantity is a state quantity filtered by a Kalman filter A.

5. A position estimation device according to claim 1 or 2, characterized in that the second calculation unit corrects the position estimation information based on the fourth position state quantity.

6. A position estimation device according to claim 1 or 2, wherein the fourth position state quantity is the attitude of the moving body.

7. A position estimation device as described in claim 1 or 2, characterized in that, in the location of the running state of the stop detection unit, the stop determination is made when it is determined that the value of the angular velocity bias calculated by the first calculation unit based on the angular velocity data measured by the inertial sensor is less than a predetermined threshold, the state in which the value of the angular velocity bias is less than the predetermined threshold continues for more than the linear motion duration value, the velocity output by the Kalman filter B is less than the motion state threshold, and the first calculation unit has recognized a predetermined characteristic part.

8. A position estimation method for estimating the position of a moving body, characterized in that a first calculation unit calculates a first position state quantity based on point cloud data of the object measured and generated by a detection and ranging sensor that measures the distance from the moving body to the object and detects reflection intensity, and also calculates a second position state quantity based on acceleration data and angular velocity data measured by an inertial sensor; a second calculation unit calculates position estimation information based on the first position state quantity, the second position state quantity, and a third position state quantity acquired from GNSS data from a positioning satellite; and a stop detection unit locates the running state of the moving body, and when a stop is determined, outputs a fourth position state quantity calculated based on the acceleration data measured by the inertial sensor to the second calculation unit.

9. A position estimation method as described in claim 8, characterized in that the first position state quantity includes the speed of the moving body and the position of the moving body calculated from characteristic parts, and the second position state quantity, the third position state quantity, and the position estimation information include the position, speed, and attitude of the moving body.

10. A position estimation method according to claim 8 or 9, wherein a Kalman filter B of the second calculation unit uses the first position state quantity or the third position state quantity as an observed value, uses the second position state quantity as an estimated value, calculates a Kalman correction value, filters the estimated value, and outputs the position estimation information.

11. A position estimation method according to claim 10, wherein the first position state quantity is a state quantity filtered by a Kalman filter A.

12. A position estimation method according to claim 8 or 9, characterized in that the second calculation unit corrects the position estimation information based on the fourth position state quantity.

13. A position estimation method according to claim 8 or 9, wherein the fourth position state quantity is the attitude of the moving body.

14. A position estimation method as described in claim 8 or 9, characterized in that, in the location of the running state of the stop detection unit, the stop determination is made when it is determined that the value of the angular velocity bias calculated by the first calculation unit based on the angular velocity data measured by the inertial sensor is less than a predetermined threshold, the state in which the value of the angular velocity bias is less than the predetermined threshold continues for more than the linear motion duration value, the velocity output by the Kalman filter B is less than the motion state threshold, and the first calculation unit has recognized a predetermined characteristic part.

Images