JP2021092508A - Travel trajectory estimation method and travel trajectory estimation device - Google Patents

Abstract

To precisely estimate a travel trajectory of a vehicle even in a state in which a GPS signal cannot be stably acquired.SOLUTION: A travel trajectory estimation method according to the present invention comprises: calculating a travel trajectory where a vehicle travels on the basis of travel data; collating environmental information on a vehicle periphery with environmental information recorded in map information to detect a reference position of the vehicle; calculating a vehicle position error which is a difference between the reference position and the travel trajectory; correcting the travel trajectory on the basis of the vehicle position error at a vehicle position where the reference position is detected among vehicle positions on the travel trajectory; and correcting, at a vehicle position where the reference position is not detected, the travel trajectory on the basis of adjacent vehicle positions on front and rear sides of the vehicle position.SELECTED DRAWING: Figure 5

Description

The present invention relates to a traveling locus estimation method and a traveling locus estimation device for estimating a traveling locus in which a vehicle has traveled.

A method of updating map data has been conventionally known, and Patent Document 1 is disclosed as a method of updating map data. In Patent Document 1, the position of the vehicle's own vehicle is detected using GPS data and odometry, and the traveling locus during traveling is calculated.

Japanese Unexamined Patent Publication No. 2005-98853

However, GPS signals cannot always be obtained stably. Therefore, the above-mentioned conventional map data updating method has a problem that the traveling locus of the vehicle cannot be estimated accurately in a situation where GPS signals cannot be stably acquired.

Therefore, the present invention has been made in view of the above problems, and is a traveling locus estimation method and traveling that can accurately estimate the traveling locus of a vehicle even in a situation where GPS signals cannot be stably acquired. An object of the present invention is to provide a trajectory estimation device.

The traveling locus estimation method and its device according to one aspect of the present invention calculate the traveling locus of the vehicle based on the traveling data, and collate the environmental information around the vehicle with the environmental information recorded in the map information. Detects the reference position of the vehicle. Then, the vehicle position error, which is the difference between the reference position and the traveling locus, is calculated, and among the vehicle positions on the traveling locus, the traveling locus is corrected based on the vehicle position error at the vehicle position where the reference position is detected. On the other hand, at the vehicle position where the reference position is not detected, the traveling locus is corrected based on the vehicle positions adjacent to the front and rear of the vehicle position.

According to the present invention, it is possible to accurately estimate the traveling locus of a vehicle even in a situation where GPS signals cannot be stably acquired.

FIG. 1 is a block diagram showing a configuration of a traveling locus estimation system including a traveling locus estimation device according to an embodiment of the present invention. FIG. 2 is a diagram for explaining a method of correcting a traveling locus by the traveling locus estimation device according to the embodiment of the present invention. FIG. 3 is a diagram for explaining a method of correcting a traveling locus by the traveling locus estimation device according to the embodiment of the present invention. FIG. 4 is a diagram for explaining a method of correcting a traveling locus by the traveling locus estimation device according to the embodiment of the present invention. FIG. 5 is a flowchart showing a processing procedure of a traveling locus estimation process by the traveling locus estimation device according to an embodiment of the present invention.

Hereinafter, an embodiment of the present invention will be described with reference to the drawings. In the description of the drawings, the same parts are designated by the same reference numerals and detailed description thereof will be omitted.

[Configuration of travel locus estimation system]
The configuration of the traveling locus estimation system including the traveling locus estimation device according to the present embodiment will be described with reference to FIG. As shown in FIG. 1, the travel locus estimation system 100 includes a travel locus estimation device 1, a GPS receiver 3, an IMU (inertial measurement unit) 5, a camera 7, a laser range finder 9, and a database 11. Be prepared. In addition to this, a millimeter wave radar, a communication device, or the like may be provided. Further, in the present embodiment, the case where the traveling locus estimation system 100 is mounted on the vehicle will be described, but the traveling locus estimation device 1 may not be mounted on the vehicle.

The GPS receiver 3 detects the current location of the own vehicle on the ground by receiving radio waves from the artificial satellite, and outputs the detected data to the database 11.

The IMU5 is composed of a 3-axis gyro sensor and a 3-direction accelerometer, and detects a three-dimensional angle (or angular velocity) and acceleration. Further, the IMU 5 may be provided with a wheel speed sensor. Then, the IMU5 calculates the odometry using the detected sensor value. The odometry is the amount of movement of the vehicle per unit time, and can be calculated by using the data detected by the IMU5 or the sensor value detected by the sensor group mounted on the vehicle. The IMU 5 outputs the detected data and the calculated odometry to the database 11.

The camera 7 has an image sensor such as a CCD (Charge-Coupled Device) or a CMOS (Complementary Metal Oxide Semiconductor). The camera 7 is mounted on the own vehicle and photographs the surroundings of the own vehicle. The camera 7 has an image processing function, and detects environmental information around the vehicle from the captured image. Environmental information includes landmarks and road structures. A target is an object provided on a road or a sidewalk, for example, a traffic light, a utility pole, a traffic sign, or the like. The road structure is a component of a road, and is, for example, a road end, a curb, a median strip, or the like, in addition to a road division line or a stop line such as a white line or a yellow line. The camera 7 outputs the detected data to the database 11.

The laser range finder 9 is a sensor that detects environmental information around a vehicle by using LiDAR (Light Detection and Ranging), which is an optical sensor technology. Specifically, the laser range finder 9 scans the laser beam within a certain angle range, receives the reflected light at that time, measures the time difference between the time when the laser is emitted and the time when the reflected light is received, and measures the own vehicle. Detects the relative distance and direction of the target and the road structure (that is, the relative position with respect to the own vehicle). The laser range finder 9 outputs the detected data to the database 11. The laser range finder 9 may be a sensor other than LiDAR as long as it is a sensor that detects the relative position of a target around the vehicle with respect to the vehicle. For example, a sensor other than the laser range finder such as a laser radar or a sound wave radar is used. You may.

Further, the traveling locus estimation device 1 is connected to a sensor group (not shown) mounted on the vehicle. For example, it can be connected to an accelerator sensor, a steering sensor, a brake sensor, a vehicle speed sensor, or the like to acquire sensor values output from these sensor groups.

The database 11 stores driving data, environmental information, map information, and the like. The traveling data includes the position information of the vehicle detected by the GPS receiver 3, the angle and acceleration of the vehicle detected by the IMU5, and the odometry calculated by the IMU5. The environmental information includes information such as the position of the target and the road structure detected by the camera 7 and the laser range finder 9. The map information is information stored in a car navigation device or the like, and includes various data necessary for route guidance such as target information and facility information. Furthermore, the map information is highly accurate map information, and is the position information on the map of a three-dimensional object such as a traffic light or an electric pole provided on a road or a sidewalk, or a target such as a lane boundary line, the number of lanes of a road, or a road. Includes road structure information such as boundaries and stop lines. The database 11 outputs the stored information to the travel locus estimation device 1 in response to the request of the travel locus estimation device 1. The database 11 also stores other information necessary for estimating the traveling locus.

The database 11 periodically obtains the latest map information from the server and updates the map information it holds. In the present embodiment, a first map (high-precision map, a map used for vehicle route guidance) is provided as map information, and a second map may be provided as a supplement to the first map. This second map is created by the vehicle or an external server when the first map alone is incomplete or when more detailed map information is required.

The traveling locus estimation device 1 estimates the traveling locus in which the vehicle has traveled. The travel locus estimation device 1 acquires travel data that the vehicle has traveled and detected in advance from the database 11, and estimates the travel locus that the vehicle has traveled in the past using the acquired travel data. The estimated travel locus is used to update the map data. For example, the position of the target is calculated using the estimated travel locus, and the position of the target recorded in the map data is updated using the calculation result.

Further, the traveling locus estimation device 1 includes a control unit that processes data acquired from a GPS receiver 3, an IMU 5, a camera 7, a laser range finder 9, and a database 11, and is composed of, for example, an IC, an LSI, or the like. When this is functionally grasped, the travel locus estimation device 1 can be classified into a travel locus calculation unit 21, a map collation unit 23, a travel locus correction unit 25, and a travel locus adjustment unit 27.

The traveling locus estimation device 1 is composed of a general-purpose electronic circuit including a microcomputer, a microprocessor, and a CPU, and peripheral devices such as a memory. Each function of such a traveling locus estimation device 1 can be implemented by one or a plurality of processing circuits. The processing circuit includes a programmed processing device such as a processing device including an electric circuit, and is an application specific integrated circuit (ASIC) or a conventional circuit arranged to perform the functions described in the embodiments. It also includes devices such as parts.

The travel locus calculation unit 21 acquires travel data detected when the vehicle travels from the database 11, and calculates the travel locus of the vehicle based on the acquired travel data. Specifically, the travel locus calculation unit 21 calculates the travel locus of the vehicle by moving the vehicle position detected by the GPS receiver 3 by the amount of odometry, which is the amount of movement per unit time. The traveling locus calculated here is represented by a set of the vehicle position and the vehicle posture for each time. The posture of the vehicle is the angle of the vehicle, that is, the orientation of the vehicle. For example, the traveling locus is represented by a set of coordinates (x, y) at time ti (i = 1 to n) and the direction θ of the vehicle.

The map collation unit 23 acquires the environmental information around the vehicle and the map information detected when the vehicle travels from the database 11, and collates the acquired environmental information around the vehicle with the environmental information recorded in the map information. Detects the reference position of the vehicle.

Specifically, the map collation unit 23 has LiDAR data, which is the relative position of the target around the vehicle detected by the laser range finder 9 with respect to the vehicle, and the map of the target recorded as point cloud data in the map information. The reference position of the vehicle on the map is detected by collating the position with the map matching. In addition, the road structure such as white lines and curbs recorded in the map information is extracted, and the road structure such as white lines and curbs is extracted from the LiDAR data around the vehicle, collated by map matching, and the reference of the vehicle on the map. The position may be detected. Since the map collation unit 23 detects the reference position by using a point cloud data matching method such as NDT (Normal Distributions Transform), the detected reference position is obtained from the traveling locus obtained by using the odometry. Is also likely to be the correct position. Therefore, in the present embodiment, the reference position is detected at a predetermined interval, and the traveling locus is corrected so as to approach the detected reference position. In addition to map matching, the map collation unit 23 may collate the target and road structure detected by the camera 7 with the map information to detect the reference position of the vehicle.

When the reference position is detected in this way, the map collation unit 23 calculates the vehicle position error, which is the difference between the reference position and the traveling locus. For example, as shown in FIG. 2, the vehicle is located at the vehicle position X1 at the time t1, the vehicle position X2 at the time t2, and the vehicle position X3 at the time t3 on the traveling locus. Then, the reference position Y1 at the time t1 and the reference position Y3 at the time t3 are detected, respectively. In such a case, the map collation unit 23 calculates the vehicle position error d1, which is the difference between the reference position Y1 and the vehicle position X1. Similarly, the vehicle position error d3, which is the difference between the reference position Y3 and the vehicle position X3, is calculated. The vehicle position errors d1 and d3 are represented by the difference in vehicle position and the difference in vehicle orientation (angle).

The map collation unit 23 detects the reference position every predetermined time or every predetermined distance. If the interval for detecting the reference position is too wide, the estimation accuracy of the traveling locus is lowered, and if the interval is too narrow, the estimation restrictions (conditions) increase and it becomes difficult to estimate the traveling locus. Therefore, regarding the method of setting the predetermined time and the predetermined distance, the interval of the reference position capable of increasing the estimation accuracy of the traveling locus is obtained in advance by an experiment or a simulation, and the predetermined time and the predetermined distance are set so as to be the interval. Just set it.

Further, the reference position may be detected when the vehicle is in a predetermined traveling state. For example, the predetermined running state is a running state of either a stopped state, a constant speed running state, or a straight running state of the vehicle. When the vehicle is stopped, running at a constant speed, or traveling straight in this way, the matching by map matching can be performed accurately, so that the reference position can also be detected accurately. Therefore, the map collation unit 23 detects the reference position in such a predetermined traveling state.

Further, the map collation unit 23 may detect the reference position when the vehicle shifts to the constant speed straight running after the posture of the vehicle changes. For example, when a vehicle turns left at an intersection, if the reference position can be detected in front of the intersection and the reference position can be detected even after turning left at the intersection, the reference position can be detected before and after the intersection. It can be estimated accurately. Therefore, in order to detect that the vehicle has passed an intersection or a curve, the map collation unit 23 detects that the posture of the vehicle has changed. Further, the map collation unit 23 detects the reference position when the vehicle shifts to the constant speed straight running so that the reference position is not detected in the middle of the intersection or the middle of the curve. This makes it possible to detect the reference position after completely passing through an intersection or a curve.

Further, the map collation unit 23 prevents the reference position from being detected at the vehicle position where the degree of agreement between the environmental information around the vehicle and the environmental information recorded in the map information is low. For example, when the relative positional relationship between a plurality of targets detected by the laser range finder 9 and the relative positional relationship of the plurality of targets recorded in the map information are collated, the degree of coincidence becomes a predetermined degree of coincidence. If it does not meet the requirements, there is a high possibility of false detection, so the reference position should not be detected at such a vehicle position. As a result, the reference position is detected only by the vehicle position where accurate environmental information can be detected, so that the estimation accuracy of the traveling locus can be improved.

The travel locus correction unit 25 corrects the travel locus calculated by the travel locus calculation unit 21. In particular, the travel locus correction unit 25 corrects the travel locus based on the vehicle position error calculated by the map collation unit 23 at the vehicle position where the reference position is detected among the vehicle positions on the travel locus.

For example, as shown in FIG. 2, among the vehicle positions on the traveling locus, the reference positions Y1 and Y3 are detected at the vehicle positions X1 and X3, and the vehicle position errors d1 and d3 are calculated. Therefore, the traveling locus correction unit 25 moves the vehicle position X1 by the vehicle position error d1 at the time t1. As a result, as shown in FIG. 3, the vehicle position X1 moves to the position of the reference position Y1. Similarly, the vehicle position X3 is also moved to the position of the reference position Y3 by moving the vehicle position error d3.

On the other hand, at the vehicle position where the reference position is not detected, the traveling locus correction unit 25 corrects the traveling locus based on the vehicle positions adjacent to the front and rear of the vehicle position. For example, as shown in FIG. 2, the reference position is not detected at the vehicle position X2 at time t2. As described above, the reference position is detected every predetermined time or every predetermined distance, and may be detected when the vehicle is in a predetermined traveling state. Therefore, the reference position is not detected at all the vehicle positions on the traveling track. Therefore, at the vehicle position where the reference position is not detected as in the vehicle position X2, the traveling locus correction unit 25 corrects the traveling locus based on the vehicle positions X1 and X3 adjacent to the front and rear of the vehicle position X2.

Specifically, the traveling locus correction unit 25 calculates a displacement error, which is the difference between the relative displacement indicating the amount of movement between the vehicle positions adjacent to the front and rear and the odometry at the vehicle position, and the traveling locus is based on this displacement error. To correct.

For example, as shown in FIG. 3, since the vehicle position X3 is moved by the vehicle position error d3, it deviates from the position of the tip of the odometry B2 at the vehicle position X2. In this case, the traveling locus correction unit 25 calculates the relative displacement A2 indicating the amount of movement between the vehicle positions X2 and X3 adjacent to each other in the front-rear direction, and calculates the displacement error C2 which is the difference between the relative displacement A2 and the odometry B2. calculate. Since the relative displacement and odometry are represented by the vehicle position and the vehicle orientation (angle), the displacement error is represented by the difference between the vehicle position difference and the vehicle orientation (angle).

Then, the traveling locus correction unit 25 moves the vehicle position X2 by the displacement error C2 so as to approach the position of the vehicle position X3. As a result, the vehicle position X2 moves to a position aligned with the vehicle position X3 as shown in FIG. Since the vehicle position X3 has already been moved to the position of the reference position Y3, there is a high possibility that it is an accurate position. Therefore, if the vehicle position X2 is moved so as to approach the position of the vehicle position X3, the vehicle position X2 can also be corrected to an accurate position.

Similarly, as shown in FIG. 3, the traveling locus correction unit 25 calculates a relative displacement A1 indicating the amount of movement between the vehicle positions X1 and X2 adjacent to each other in the front-rear direction, and the odometry at the relative displacement A1 and the vehicle position X1. The displacement error C1 which is the difference from B1 is calculated.

Then, the traveling locus correction unit 25 moves the vehicle position X2 by the displacement error C1 so as to approach the position of the vehicle position X1. As a result, the vehicle position X2 moves to a position aligned with the vehicle position X1 as shown in FIG. Since the vehicle position X1 has already been moved to the position of the reference position Y1, there is a high possibility that it is an accurate position. Therefore, if the vehicle position X2 is moved so as to approach the position of the vehicle position X1, the vehicle position X2 can also be corrected to an accurate position.

However, when the reference position is detected as in the vehicle positions X1 and X3, the vehicle positions X1 and X3 move by the vehicle position errors d1 and d3, so that the mutation errors C1 and C2 and the vehicle position error d1 , D3 are equal. In FIG. 3, reference positions are detected for each of the vehicle positions X1 and X3 adjacent to the front and rear of the vehicle position X2, but in reality, the reference position is located between the vehicle positions where the reference position is detected. There are many undetected vehicle positions.

Therefore, in the correction of the actual traveling locus, the variation error is calculated and the vehicle position is moved in order from the vehicle position near the vehicle position where the reference position is detected, and finally the reference position is detected. Move all vehicle positions between vehicle positions to complete the trajectory correction. In this way, the traveling locus correction unit 25 first moves the vehicle position in which the reference position is detected, and then sequentially moves the vehicle position between the vehicle positions in which the reference position is detected. Correct the running locus.

The traveling locus adjusting unit 27 adjusts so that the error of the traveling locus corrected by the traveling locus correction unit 25 becomes small, and outputs the adjusted traveling locus as an estimation result of the traveling locus. Specifically, the travel locus adjustment unit 27 calculates the total error by adding all the vehicle position errors calculated by the map collation unit 23 and the displacement errors calculated by the travel locus correction unit 25, and the total error is calculated. Adjust the running locus so that it becomes smaller.

For example, the traveling locus adjustment unit 27 uses an optimization method such as the Gauss-Newton method or the LM method to gradually reduce the total error in the iterative calculation while moving the vehicle position Xi (i = 1 to n). Adjust the running locus so that it becomes. Then, when the total error becomes equal to or less than a preset predetermined value, the traveling locus adjustment unit 27 determines that the adjustment of the traveling locus has converged, and outputs the adjusted traveling locus as an estimation result of the traveling locus.

[Running trajectory estimation process]
Next, with reference to FIG. 5, the traveling locus estimation process by the traveling locus estimation device 1 according to the present embodiment will be described. FIG. 5 is a flowchart showing a traveling locus estimation process.

As shown in FIG. 5, in step S1, the traveling locus calculation unit 21 acquires the traveling data detected when the vehicle travels from the database 11. Further, the map collation unit 23 acquires the environmental information and the map information detected when the vehicle travels from the database 11.

In step S3, the travel locus calculation unit 21 calculates the travel locus in which the vehicle travels based on the travel data acquired in step S1. The travel locus calculated here is a travel locus in which the vehicle has traveled in the past, and the travel locus is calculated by moving the vehicle position by the amount of odometry. The traveling locus is represented by a set of the vehicle position and the posture of the vehicle for each time, and is specifically represented by a set of the coordinates (x, y) at the time ti (i = 1 to n) and the direction θ of the vehicle. .. For example, as shown in FIG. 2, the traveling locus is represented by a set of a vehicle position X1 at time t1, a vehicle position X2 at time t2, and a vehicle position X3 at time t3. However, the vehicle position Xn includes the direction θn of the vehicle.

In step S5, the map collation unit 23 collates the environmental information around the vehicle with the environmental information recorded in the map information to detect the reference position of the vehicle. Specifically, the map collation unit 23 is a map that is the LiDAR data around the vehicle detected by the laser range finder 9, the relative position of the target around the vehicle with respect to the own vehicle, and the point cloud data recorded in the map information. The reference position of the vehicle on the map is detected by collating with the position of the above target by map matching.

In step S7, the map collation unit 23 determines whether or not the reference position is detected in the vicinity of the vehicle position on the traveling locus, and if the reference position is detected, proceeds to step S9 and proceeds to the reference position. If is not detected, the process proceeds to step S13. For example, as shown in FIG. 2, since the reference positions Y1 and Y3 are detected at the vehicle positions X1 and X3, the process proceeds to step S9, and since the reference position is not detected at the vehicle position X2, the process proceeds to step S13.

The map collation unit 23 first determines whether or not a reference position is detected in the vicinity of the vehicle position X1 at time t1, performs the processes of steps S9 to 13, and then sequentially from the vehicle position X2 to the vehicle position Xn. The processes of steps S9 to S13 are performed.

In step S9, the map collation unit 23 calculates the vehicle position error, which is the difference between the reference position and the vehicle position on the traveling locus. For example, in the vehicle position X1 of FIG. 2, the difference d1 between the reference position Y1 and the vehicle position X1 is output as the vehicle position error. Similarly, at time t3, the difference d3 between the reference position Y3 and the vehicle position X3 is output as the vehicle position error.

In step S11, the traveling locus correction unit 25 corrects the traveling locus of the vehicle position in which the reference position is detected based on the vehicle position error calculated in step S9. For example, as shown in FIG. 2, the traveling locus correction unit 25 moves the vehicle positions X1 and X3 by the vehicle position errors d1 and d3 with respect to the vehicle positions X1 and X3 in which the reference positions Y1 and Y3 are detected. To correct.

In step S13, the map collation unit 23 determines whether or not the processing of steps S7 to 13 has been performed for all the vehicle positions on the traveling locus. Then, if there is a vehicle position in which the processes of steps S7 to 13 have not been performed, the process returns to step S7, and if the processes of steps S7 to 13 have been performed for all the vehicle positions, the process proceeds to step S15. In the case of proceeding to step S15, at the vehicle positions such as X1 and X3 where the reference position was detected, all the vehicle positions were moved as shown in FIG. 3, and the reference position was not detected like X2. At the vehicle position, as shown in FIG. 3, the vehicle position remains unchanged.

In step S15, the traveling locus correction unit 25 determines the displacement error, which is the difference between the relative displacement indicating the amount of movement between the front and rear adjacent vehicle positions and the odometry at the vehicle position, with respect to the vehicle position where the reference position is not detected. calculate. For example, as shown in FIG. 3, for the vehicle position X2 for which the reference position was not detected, the relative displacement A2 indicating the amount of movement between the vehicle positions X2 and X3 adjacent to the front and rear is calculated, and the relative displacement A2 and the vehicle are calculated. The displacement error C2, which is the difference from the odometry B2 at the position X2, is calculated. Similarly, the traveling locus correction unit 25 calculates the displacement error C1.

In step S17, the traveling locus correction unit 25 corrects the traveling locus of the vehicle position for which the reference position was not detected based on the displacement error calculated in step S15. As shown in FIG. 3, the traveling locus correction unit 25 moves the vehicle position X2 by the displacement errors C1 and C2 with respect to the vehicle position X2 for which the reference position is not detected, so that the traveling locus is corrected as shown in FIG. to correct. When the reference position is detected at the vehicle positions adjacent to the front and rear, the displacement error is equal to the vehicle position error.

In step S19, the traveling locus correction unit 25 determines whether or not the correction of the vehicle position has been completed for the vehicle position for which the reference position has not been detected. For all the vehicle positions where the reference position is not detected as in the vehicle position X2 of FIG. 3, the traveling locus correction unit 25 determines whether or not the correction of the vehicle position is completed. Then, if the correction of the vehicle position is not completed, the process returns to step S15, and if it is completed, the process proceeds to step S21.

In step S21, the traveling locus adjustment unit 27 adds all the vehicle position errors calculated in step S9 and the displacement errors calculated in step S15 to calculate the total error. This total error is the sum of the vehicle position error and the displacement error of all the vehicle positions from the vehicle position X1 to the vehicle position Xn.

In step S23, the traveling locus adjustment unit 27 adjusts the traveling locus so that the total error calculated in step S21 becomes small, and determines whether or not the traveling locus has converged. The traveling locus adjustment unit 27 uses an optimization method such as the Gauss-Newton method or the LM method to gradually reduce the total error in the iterative calculation while moving the vehicle position Xi (i = 1 to n). Adjust the running trajectory to. Then, if the total error does not fall below a preset predetermined value, it is determined that the adjustment of the traveling locus does not converge, and the process returns to step S3. On the other hand, when the total error becomes less than or equal to a preset predetermined value, it is determined that the adjustment of the traveling locus has converged, and the adjusted traveling locus is output as the estimation result of the traveling locus to obtain the traveling locus according to the present embodiment. End the estimation process.

[Effect of Embodiment]
As described in detail above, the travel locus estimation device 1 according to the present embodiment calculates the travel locus of the vehicle based on the travel data, and includes the environmental information around the vehicle and the environmental information recorded in the map information. Is collated to detect the reference position of the vehicle. Then, the vehicle position error, which is the difference between the reference position and the traveling locus, is calculated, and among the vehicle positions on the traveling locus, the traveling locus is corrected based on the vehicle position error at the vehicle position where the reference position is detected. On the other hand, at the vehicle position where the reference position is not detected, the traveling locus is corrected based on the vehicle positions adjacent to the front and rear of the vehicle position. As a result, even in a situation where GPS signals cannot be stably acquired, the reference position can be detected from the environmental information around the vehicle and the traveling locus can be corrected, so that the traveling locus of the vehicle can be estimated accurately.

In particular, in the traveling locus estimation device 1 according to the present embodiment, the reference position is detected by a point cloud data matching method such as NDT (Normal Distributions Transform) using the LiDAR data acquired by the laser range finder 9. The reference position detected in this way is likely to be a more accurate position than the traveling locus obtained using the odometry. Therefore, since the traveling locus estimation device 1 according to the present embodiment corrects the traveling locus based on the reference position detected at the accurate position, the traveling locus of the vehicle can be estimated accurately.

Further, the traveling locus estimation device 1 according to the present embodiment detects a reference position at predetermined time intervals or predetermined distance intervals. As a result, since the reference position is detected at a predetermined interval set in advance so that the estimation accuracy is high and the traveling locus is corrected, the traveling locus of the vehicle can be estimated with high accuracy.

Further, the traveling locus estimation device 1 according to the present embodiment detects a reference position when the vehicle is in a predetermined traveling state. As a result, the reference position can be detected in the traveling state where the matching by map matching can be performed accurately. Therefore, since the traveling locus can be corrected by using the reference position detected at the accurate position, the traveling locus of the vehicle can be estimated accurately.

Further, the traveling locus estimation device 1 according to the present embodiment detects a reference position when the vehicle is in a stopped state, a constant speed traveling state, or a straight running state as a predetermined traveling state. As a result, the reference position can be detected in the stopped state, the constant speed running state, and the straight running state where the matching by map matching can be performed accurately. Therefore, since the traveling locus can be corrected by using the reference position detected at the accurate position, the traveling locus of the vehicle can be estimated accurately.

Further, the traveling locus estimation device 1 according to the present embodiment detects a reference position when the vehicle shifts to constant speed straight-ahead traveling after the posture of the vehicle changes. As a result, the reference position can be detected after the point where the posture of the vehicle has changed, such as a curve or an intersection, so that the traveling locus can be estimated more accurately.

Further, in the traveling locus estimation device 1 according to the present embodiment, the reference position is not detected at the vehicle position where the degree of agreement between the environmental information around the vehicle and the environmental information recorded in the map information is low. As a result, the reference position is detected only by the vehicle position where accurate environmental information can be detected, so that the estimation accuracy of the traveling locus can be improved.

Further, in the traveling locus estimation device 1 according to the present embodiment, the traveling locus is represented by a set of the vehicle position and the posture of the vehicle for each time. As a result, the traveling locus can be accurately represented for each time, so that the traveling locus of the vehicle can be estimated accurately.

Further, in the traveling locus estimation device 1 according to the present embodiment, it is the difference between the relative displacement indicating the amount of movement between the front and rear adjacent vehicle positions and the odometry at the vehicle position at the vehicle position where the reference position is not detected. Calculate the displacement error. Then, the traveling locus is corrected based on this displacement error. As a result, even in the vehicle position where the reference position is not detected, the traveling locus can be corrected based on the vehicle positions adjacent to the front and rear, so that the traveling locus of the vehicle can be estimated accurately.

Further, in the traveling locus estimation device 1 according to the present embodiment, the total error is calculated by adding all the vehicle position errors and the displacement errors, and the corrected traveling locus is adjusted so that the total error is small. As a result, the consistency of the entire traveling locus can be obtained, so that the accuracy of the estimated traveling locus can be improved.

The above embodiment is an example of the present invention. Therefore, the present invention is not limited to the above-described embodiment, and even if the embodiment is other than this embodiment, as long as it does not deviate from the technical idea of the present invention, it may be designed according to the design or the like. Of course, various changes are possible.

1 Travel locus estimation device 3 GPS receiver 5 IMU (Inertial measurement unit)
7 Camera 9 Laser range finder 11 Database 21 Travel locus calculation unit 23 Map collation unit 25 Travel locus correction unit 27 Travel locus adjustment unit 100 Travel locus estimation system

Claims (10)

It is a traveling locus estimation method that estimates the traveling locus of a vehicle.
The driving data detected when the vehicle travels and the environmental information around the vehicle detected when the vehicle travels are acquired.
Based on the travel data, the travel locus of the vehicle traveled is calculated.
The reference position of the vehicle is detected by collating the environmental information around the vehicle with the environmental information recorded in the map information.
The vehicle position error, which is the difference between the reference position and the traveling locus, is calculated.
Among the vehicle positions on the traveling locus, at the vehicle position where the reference position is detected, the traveling locus is corrected based on the vehicle position error.
A traveling locus estimation method, characterized in that, at a vehicle position where the reference position is not detected, the traveling locus is corrected based on the vehicle positions adjacent to the front and rear of the vehicle position. The traveling locus estimation method according to claim 1, wherein the reference position is detected every predetermined time or every predetermined distance. The traveling locus estimation method according to claim 1 or 2, wherein the reference position is detected when the vehicle is in a predetermined traveling state. The traveling locus estimation method according to claim 3, wherein the predetermined traveling state is either a stopped state, a constant speed traveling state, or a straight running state of the vehicle. The traveling locus estimation method according to claim 1 or 2, wherein the reference position is detected when the vehicle shifts to constant speed straight-ahead traveling after the posture of the vehicle changes. The reference position according to any one of claims 1 to 5, wherein the reference position is not detected at a vehicle position where the degree of agreement between the environmental information around the vehicle and the environmental information recorded in the map information is low. Travel locus estimation method. The traveling locus estimation method according to any one of claims 1 to 6, wherein the traveling locus is represented by a set of a vehicle position and a vehicle posture for each time. At the vehicle position where the reference position was not detected, a displacement error, which is the difference between the relative displacement indicating the amount of movement between the front and rear adjacent vehicle positions and the odometry at the vehicle position, is calculated, and the displacement error is calculated based on the displacement error. The traveling locus estimation method according to any one of claims 1 to 7, wherein the traveling locus is corrected. The travel locus estimation according to claim 8, wherein the total error is calculated by adding all the vehicle position errors and the displacement errors, and the corrected travel locus is adjusted so that the total error is small. Method. It is a traveling locus estimation device equipped with a control unit that estimates the traveling locus of the vehicle.
The control unit
The driving data detected when the vehicle travels and the environmental information around the vehicle detected when the vehicle travels are acquired.
Based on the travel data, the travel locus of the vehicle traveled is calculated.
The reference position of the vehicle is detected by collating the environmental information around the vehicle with the environmental information recorded in the map information.
The vehicle position error, which is the difference between the reference position and the traveling locus, is calculated.
Among the vehicle positions on the traveling locus, at the vehicle position where the reference position is detected, the traveling locus is corrected based on the vehicle position error.
A traveling locus estimation device characterized in that, at a vehicle position where the reference position is not detected, the traveling locus is corrected based on the vehicle positions adjacent to the front and rear of the vehicle position.

Images