JP2020193907A - Positional deviation correction system and program - Google Patents

Abstract

To enable the absolute position of a target to be estimated at low cost with good accuracy.SOLUTION: A first target position calculation unit 214 calculates a first target position in the world coordinate system of a target from the position of the target around a vehicle in a vehicle coordinate system and the position of the vehicle in the world coordinate system, and a second target position calculation unit 216 calculates a second target position in the world coordinate system of the target from the detected position of the vehicle in the world coordinate system on the basis of changes of the target and vehicle positions. A first target group selection unit 231 selects a first target group in which a random variation in a first target position of the target in multiple sessions of traveling is less than or equal to a threshold, and a second target group position correction unit 233 corrects a second target position of the target detected in the same traveling, on the basis of the average position of first target positions with respect to each of the targets included in the first target group. A second target group average position calculation unit 234 calculates the average position of second target positions having been corrected with respect to the targets included in the first target group.SELECTED DRAWING: Figure 1

Description

The present invention relates to misalignment correction systems and programs.

Conventionally, in automatic driving and the like, a technique has been used in which a target included in a map of a driving environment is associated with a target by an in-vehicle sensor to estimate the position of the vehicle with respect to the map. In addition, there is a technique such as SLAM (simultaneous Localization And Mapping) that estimates the position of a target using a peripheral environment recognition device mounted on a moving body such as a vehicle and generates a map of the driving environment (for example, a patent document). 1).

Japanese Unexamined Patent Publication No. 2018-081252

However, although it is possible to estimate the position of a target with high relative accuracy by a technique such as SLAM, the accuracy of the absolute position is not always sufficient because it is necessary to use a target whose absolute position is known. There was a problem that the estimation accuracy of the position of the target was lowered in the absence situation.

Further, it is conceivable to secure the accuracy of the absolute position of the vehicle by using a high-precision positioning system, but there is a problem that the cost becomes high.

The present invention has been made to solve the above problems, and even in a situation where the accuracy of the absolute position of the vehicle is not always sufficient, the absolute position of the target can be estimated at low cost and with high accuracy. It is an object of the present invention to provide a misalignment correction system and a program capable of performing the above.

In order to achieve the above object, the misalignment correction system of the present invention obtains the first vehicle position, which is the position of the world coordinate system of the vehicle detected by the first positioning device mounted on the vehicle. An acquisition unit, a second acquisition unit that acquires a second vehicle position, which is a position in the world coordinate system of the vehicle, detected based on a change in the position of the vehicle by a second positioning device mounted on the vehicle. A target detection unit that acquires the position of a target around the vehicle detected by using a sensor mounted on the vehicle in the vehicle coordinate system, and the first vehicle position acquired by the first acquisition unit. The first target position, which is the position of the target in the world coordinate system, is calculated based on the position of the target acquired by the target detection unit in response to the detection of the first vehicle position. Based on the target position calculation unit, the second vehicle position acquired by the second acquisition unit, and the position of the target acquired by the target detection unit in response to the detection of the second vehicle position. , The second target position calculation unit that calculates the second target position, which is the position of the target in the world coordinate system, and each of the plurality of targets detected by the target detection unit are run a plurality of times. In, a first target group for the same target is generated based on the first target position of the target, the size of the target, etc. calculated by the first target position calculation unit, and the first target group is generated. The variation of the first target position of the target in one target group is obtained, and the first target group of the target whose variation is equal to or less than a predetermined first threshold value is selected from the plurality of targets. The average position of the first target position of the target for each of the first target group selection unit and the target included in the first target group selected by the first target group selection unit. Based on the first average position calculation unit for calculating the first average position and the first average position of each of the targets included in the first target group, the sensor is used in the same running. For each of the second target group position correction unit that corrects each of the second target positions calculated for each of the targets detected using the target, and each of the targets included in the first target group. The configuration includes a second target group average position calculation unit that calculates a second average position that is an average position of the second target position of the target corrected by the second target group position correction unit. Has been done.

Further, the program of the present invention mounts a computer on the vehicle, a first acquisition unit that acquires a first vehicle position which is a position in the world coordinate system of the vehicle detected by a first positioning device mounted on the vehicle. The second acquisition unit that acquires the position of the second vehicle, which is the position of the world coordinate system of the vehicle detected based on the change in the position of the vehicle by the second positioning device, uses the sensor mounted on the vehicle. Corresponding to the detection of the target detection unit that acquires the position of the target around the vehicle detected in the vehicle coordinate system, the first vehicle position acquired by the first acquisition unit, and the first vehicle position. The first target position calculation unit, which calculates the first target position, which is the position of the target in the world coordinate system, based on the position of the target acquired by the target detection unit, the second acquisition. At the position of the target in the world coordinate system based on the position of the second vehicle acquired by the unit and the position of the target acquired by the target detection unit in response to the detection of the second vehicle position. Each of the second target position calculation unit that calculates a certain second target position and the plurality of targets detected by the target detection unit are calculated by the first target position calculation unit in a plurality of runs. A first target group for the same target is generated based on the position of the first target of the target, the size of the target, and the like, and the first of the targets in the first target group. The first target group selection unit, the first target group selection unit, which obtains the variation of one target position and selects the first target group of the target whose variation is equal to or less than a predetermined first threshold value among the plurality of targets. For each of the targets included in the first target group selected by the target group selection unit, the first target that calculates the first average position, which is the average position of the first target position of the target. Based on the group average position calculation unit and the first average position of each of the targets included in the first target group, each of the targets detected by using the sensor in the same running was calculated. The second target group position correction unit that corrects each of the second target positions and each of the targets included in the first target group are corrected by the second target group position correction unit. This is a program for functioning as a second target group average position calculation unit for calculating the second average position, which is the average position of the second target position of the target.

According to the correction system and the program of the present invention, the first acquisition unit acquires the first vehicle position, which is the position of the world coordinate system of the vehicle detected by the first positioning device mounted on the vehicle, and the second The acquisition unit acquires the second vehicle position, which is the position of the world coordinate system of the vehicle, which is detected based on the change in the position of the vehicle by the second positioning device mounted on the vehicle, and the target detection unit , The position in the vehicle coordinate system of the target around the vehicle detected by using the sensor mounted on the vehicle is acquired, and the first target position calculation unit acquires the first vehicle acquired by the first acquisition unit. Based on the position and the position of the target acquired by the target detection unit in response to the detection of the first vehicle position, the first target position, which is the position of the target in the world coordinate system, is calculated. The second target position calculation unit is based on the second vehicle position acquired by the second acquisition unit and the position of the target acquired by the target detection unit in response to the detection of the second vehicle position. The second target position, which is the position of the target in the world coordinate system, is calculated.

Then, the first target group selection unit calculates the first object of the target by the first target position calculation unit in a plurality of times for each of the plurality of targets detected by the target detection unit. Based on the target position, the size of the target, etc., the first target group for the same target is generated, the variation of the first target position of the target in the first target group is obtained, and the plurality of the targets Among the targets, the first target group of the targets whose variation is equal to or less than the predetermined first threshold is selected, and the first target group average position calculation unit is selected by the first target group selection unit. For each of the targets included in one target group, the first average position, which is the average position of the first target position of the target, is calculated, and the second target group position correction unit performs the first target group. Based on the first average position of each of the targets included in, each of the second target positions calculated for each of the targets detected by the sensor in the same running is corrected, and the second The target group average position calculation unit is the average position of the second target position of the target corrected by the second target group position correction unit for each of the targets included in the first target group. Calculate the second average position.

In this way, the object is based on the position in the vehicle coordinate system of the target around the vehicle detected by using the sensor mounted on the vehicle and the position of the first vehicle which is the position in the world coordinate system of the vehicle. The first target position, which is the position of the target in the world coordinate system, is calculated, and the second vehicle, which is the position of the world coordinate system of the vehicle detected based on the change in the position of the target and the position of the vehicle. The second target position, which is the position of the target in the world coordinate system, is calculated based on the position, and for each of the plurality of detected targets, the first of the target calculated in a plurality of runs. Based on the target position, the size of the target, etc., the first target group for the same target is generated, and the variation of the first target position of the target in the first target group is obtained, and the variation is obtained. Is the average position of the first target position of the target calculated for each of the targets included in the first target group by selecting the first target group of the target whose value is equal to or less than the predetermined first threshold. Based on the first average position, each of the second target positions calculated for each of the targets detected by the sensor in the same running is corrected, and the targets included in the first target group are included. By calculating the second average position, which is the average position of the corrected second target position of the target, the absolute position of the target can be estimated at low cost and with high accuracy.

Further, the second target group position correction unit of the position deviation correction system of the present invention detects using the sensor in the same travel as the first average position of the target included in the first target group. The result of coordinate-transforming the position of the second target of each of the targets by associating the position of the second target with the position of the second target of the target and the second object of each of the targets. A first coordinate transformation matrix that minimizes the positional deviation of the target included in the first target group associated with the target position from each of the first average positions is calculated, and the first coordinate transformation matrix is used. Based on this, it is possible to correct each of the second target positions calculated for each of the targets detected by using the sensor in the same travel.

Further, the second target group position correction unit of the position deviation correction system of the present invention calculates the first coordinate conversion matrix, and based on the first coordinate conversion matrix, the second target position of the target. Is a predetermined residual, which is a positional deviation between the result of coordinate conversion of the target and the first average position of the target included in the first target group associated with the second target position of the target. Targets larger than the second threshold are excluded from the targets detected by the sensor in the same travel, and the same travel as the first average position of the targets included in the first target group. The second coordinate transformation matrix that minimizes the misalignment is calculated by associating each of the targets detected by using the sensor with the second target position, and the second coordinate transformation is performed. Based on the matrix, each of the second target positions calculated for each of the targets simultaneously detected using the sensor can be corrected.

As described above, according to the misalignment correction system and program of the present invention, it is possible to obtain the effect that the absolute position of the target can be estimated at low cost and with high accuracy.

It is a block diagram which shows the schematic structure of the position deviation correction apparatus in 1st Embodiment of this invention. It is an image diagram which shows the relationship between the position / posture of the vehicle in the world coordinate system and the vehicle coordinate system in the 1st Embodiment of this invention. It is an image diagram which shows the example of the position of the target in the vehicle coordinate system in 1st Embodiment of this invention. It is an image diagram which shows the example of the true value of the position of a target in the driving environment in 1st Embodiment of this invention. It is an image diagram which shows the example of the 1st target position in a plurality of traveling in 1st Embodiment of this invention. It is an image diagram which shows the example of superposition of the 1st target position of a plurality of traveling in 1st Embodiment of this invention. It is an image figure which shows the example of the average position of the 1st target position of each target in 1st Embodiment of this invention. It is an image diagram which shows the comparison of the 1st target position group of a plurality of running, the 1st average position, and the true value in 1st Embodiment of this invention. It is an image diagram which shows the example of selection of the 1st target group in 1st Embodiment of this invention. It is an image diagram which shows the example of the 1st average position in 1st Embodiment of this invention. It is an image figure which shows the comparative example of the 1st average position and the true value of the target in 1st Embodiment of this invention. It is an image figure which shows the example of the alignment of the target position group 2 with respect to the 1st average position group in 1st Embodiment of this invention. It is an image diagram which shows the example of the target position group 2 which is the set of the 2nd target position of each target of a plurality of traveling in 1st Embodiment of this invention. It is an image diagram which shows the example of superposition of the target position group 2 of a plurality of traveling in 1st Embodiment of this invention. It is an image diagram which shows the example of the 2nd average position in 1st Embodiment of this invention. It is an image figure which shows the comparative example of the 2nd average position and the true value of the target in 1st Embodiment of this invention. It is a flowchart which shows the content of the target position collection processing routine in 1st Embodiment of this invention. It is a flowchart which shows the content of the position shift correction processing routine in 1st Embodiment of this invention. It is a block diagram which shows the schematic structure of the position deviation correction system in the 2nd Embodiment of this invention. It is a block diagram which shows the schematic structure of the vehicle-mounted device in the 2nd Embodiment of this invention. It is a block diagram which shows the schematic structure of the position deviation correction apparatus in the 2nd Embodiment of this invention. It is an image diagram which shows the selection example of the 2nd target group used for the alignment in 2nd Embodiment of this invention. It is a flowchart which shows the content of the target position collection processing routine in the 2nd Embodiment of this invention. It is a flowchart which shows the content of the position shift correction processing routine in the 2nd Embodiment of this invention.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
<Structure of Positional Misalignment Correction System 1 According to First Embodiment of the Present Invention>
The misalignment correction system 1 according to the embodiment of the present invention will be described with reference to FIG. As shown in FIG. 1, the position shift correction system 1 according to the embodiment of the present invention includes a camera 10, a first positioning device 11, a second positioning device 12, and a position shift correction device 20. Will be done.

The camera 10 is mounted on a vehicle and photographs the surroundings of the vehicle while the vehicle is traveling. For example, the camera 10 is a stereo camera, which is mounted in front of the vehicle and photographs the front of the vehicle every second (FIG. 3). Then, the camera 10 passes the captured image to the position shift correction device 20.

The first positioning device 11 is mounted on a vehicle and positions the position of the first vehicle, which is the position of the world coordinate system of the vehicle. Specifically, as shown in FIG. 2, the first positioning device 11 is a vehicle represented on a world coordinate system (fixed coordinate system) using latitude, longitude, etc. obtained by using a positioning satellite or the like. Position and posture (direction) are measured. Hereinafter, the position and posture of the vehicle positioned by the first positioning device 11 will be referred to as the first vehicle position.

As shown in FIG. 2, the world coordinate system (Xw, Yw, Zw) is fixed, while the vehicle coordinate system (X _{v, t} , Y _{v, t} , Z _{v, t} ) is at each time t. It changes depending on the vehicle position and posture. Therefore, although the position of the first vehicle has an error, it can be expected to be distributed within a small error centering on the true value in a place such as a positioning satellite where the observation condition is good. On the other hand, the error becomes large in the place where the observation condition of the satellite is poor. Further, when the first vehicle position is viewed in chronological order, there is no effect due to the accumulation of errors. Then, the first positioning device 11 passes the positioned first vehicle position to the position deviation correction device 20.

The second positioning device 12 positions the second vehicle position, which is the position of the world coordinate system of the vehicle, which is mounted on the vehicle and detected based on the change in the position of the vehicle. Specifically, the second positioning device 12 calculates the relative position / posture (vehicle trajectory) by integrating changes in the relative position and posture of the vehicle acquired by using a gyro sensor or the like. By adopting the first vehicle position determined by the first positioning device 11 at a certain time as the initial value of the second vehicle position, the relative position and posture of the vehicle can be changed to obtain the vehicle on the world coordinate system. You can get the position and posture. Hereinafter, the position / posture of the vehicle on the world coordinate system calculated by the second positioning device 12 will be referred to as the second vehicle position. When viewed in chronological order, the error of the relative change in position and attitude between each time is small for the second vehicle position, but the error with respect to the true value in the world coordinate system is large due to the deviation of the initial position and the integration of the error. Become. Then, the second positioning device 12 passes the positioned second vehicle position to the position deviation correction device 20.

The misalignment correction device 20 detects a target included in the traveling environment and estimates the position information of the detected target in the world coordinate system. The target is a sign such as a three-dimensional object (sign, signal, etc.) on the road side or a road marking (lane marking, pedestrian crossing, etc.).

Specifically, the misalignment correction device 20 includes a CPU, a RAM, and a ROM that stores a program for executing a target position collection processing routine and a misalignment correction processing routine, which will be described later, and is functionally It is configured as shown below. The misalignment correction device 20 includes a target position collection unit 210, a target position storage unit 220, a misalignment correction unit 230, and an output unit 240.

The target position collecting unit 210 is a first target position based on the first vehicle position of the target around the vehicle and a second target based on the second vehicle position of the target around the vehicle in the running of the vehicle. Positions and are collected for multiple runs.

Specifically, the target position collection unit 210 includes an image acquisition unit 211, a target detection unit 212, a first acquisition unit 213, a first target position calculation unit 214, and a second acquisition unit 215. It is configured to include a second target position calculation unit 216.

The image acquisition unit 211 acquires the image captured by the camera 10 and passes it to the target detection unit 212.

The target detection unit 212 acquires the position of the target in the vehicle coordinate system around the vehicle detected by using the sensor mounted on the vehicle. Specifically, the target detection unit 212 detects the target in the image by image analysis processing using the image taken by the camera 10, and the vehicle is detected from the position on the image of the target. The position in the coordinate system is detected (coordinates in FIG. 3 (Xp, Yp, Zp)). Then, the target detection unit 212 passes the position of the detected target in the vehicle coordinate system to the first target position calculation unit 214 and the second target position calculation unit 216.

The first acquisition unit 213 acquires the first vehicle position of the vehicle detected by the first positioning device 11 mounted on the vehicle. Specifically, the first acquisition unit 213 acquires the position of the first vehicle at the time when the target detected by the target detection unit 212, which is positioned by the first positioning device 11, is photographed. Then, the first acquisition unit 213 passes the acquired first vehicle position to the first target position calculation unit 214.

The first target position calculation unit 214 is the position of the target acquired by the target detection unit 212 corresponding to the first vehicle position acquired by the first acquisition unit 213 and the time when the first vehicle position is positioned. Based on the above, the first target position, which is the position of the target in the world coordinate system, is calculated. Specifically, the first target position calculation unit 214 calculates the first target position by converting the position of the target in the vehicle coordinate system into the world coordinate system using the first vehicle position. Then, the first target position calculation unit 214 stores the calculated first target position in the target position storage unit 220.

The second acquisition unit 215 acquires the position of the second vehicle of the vehicle by the second positioning device 12 mounted on the vehicle. Specifically, the second acquisition unit 215 acquires the position of the second vehicle at the time when the target detected by the target detection unit 212 positioned by the second positioning device 12 is photographed. Then, the second acquisition unit 215 passes the second vehicle position of the acquired target to the second target position calculation unit 216.

The second target position calculation unit 216 is the position of the target acquired by the target detection unit 212 corresponding to the second vehicle position acquired by the second acquisition unit 215 and the time when the second vehicle position is positioned. Based on the above, the second target position, which is the position of the target in the world coordinate system, is calculated. Specifically, the second target position calculation unit 216 calculates the second target position by converting the position of the target in the vehicle coordinate system into the world coordinate system using the second vehicle position. Then, the second target position calculation unit 216 stores the calculated second target position in the target position storage unit 220.

The target position storage unit 220 stores the first target position and the second target position for each of the targets detected in the same traveling of the vehicle in a certain traveling environment for each traveling of the vehicle. .. Specifically, the target position storage unit 220 contains the position of the vehicle coordinate system of each target detected in the same running of the vehicle, and the first vehicle position and the second vehicle position at each time in the same running of the vehicle. The first target position and the second target position of each target calculated from the above are stored. When the vehicle travels in the traveling environment a plurality of times, the first target position and the second target position of each target for each of the plurality of travels are accumulated in the target position storage unit 220. ..

The misalignment correction unit 230 is based on the plurality of first target positions and the second target positions stored in the target position storage unit 220 for each target, and the world coordinates of the plurality of targets included in the traveling environment. Estimate the position in the system. FIG. 4 shows an example of the true value of the position of the target in the world coordinate system when the driving environment includes six targets. In FIG. 4, the coordinate axes represent the world coordinate system, the arrows (excluding the coordinate axes) represent the travel path on which the vehicle travels, and each point represents the true value of the position of each target. That is, the misalignment correction unit 230 estimates the true value of the position of each target in the world coordinate system using the first target position and the second target position collected by the target position collecting unit 210.

Specifically, the misalignment correction unit 230 includes a first target group selection unit 231, a first target group average position calculation unit 232, a second target group position correction unit 233, and a second target group. It is configured to include an average position calculation unit 234.

FIG. 5 shows an example of the target position group 1 which is a set of the first target positions of each target calculated in four runs (runs 1 to 4). FIG. 6 shows a superposition of the target position group 1 of each run in FIG. As shown in FIGS. 5 and 6, observation variations occur in the target position group 1 in each run. This is because there is an error in the position of the first vehicle based on the state of the positioning satellite and a difference in the position of the target detected by the sensor mounted on the vehicle. Here, the position error of the target detected by the sensor mounted on the vehicle is assumed to be relatively small as compared with the error of the position of the first vehicle. If a target including the first target position having a large error is used for estimating the true value, the estimation result also includes an error. For example, if the average position of the first target position of each target is adopted as shown in FIG. 7, if there is a target with a large variation in the first target position, the true value is set as shown in FIG. There is a target with a large position error.

Therefore, the first target group selection unit 231 uses the first target position calculation unit 214 to calculate each of the plurality of targets detected by the target detection unit 212 in a plurality of times. Based on the position of the first target, the size of the target, etc., the first target group for the same target is generated, and the variation of the first target position of the target in the first target group is obtained, and a plurality of targets are obtained. Select the first target group of the targets whose variation is equal to or less than the predetermined first threshold value. Specifically, as shown in FIG. 9, the first target group selection unit 231 obtains the distribution of the first target position with respect to the same target, and the variation (variance) is equal to or less than a predetermined first threshold value. Select only the target and use it as the first target group. In FIG. 9, the white circle indicates the first target position of the target whose variation in the first target position is not equal to or less than the first threshold value. Further, in FIG. 9, the black circle indicates the first target position of the target whose variation in the first target position is equal to or less than the first threshold value. As described above, for a target having a large observation error, the accuracy is lowered when the first target position is used, so that this can be excluded. Then, the first target group selection unit 231 passes the first target group to the first target group average position calculation unit 232.

The first target group average position calculation unit 232 is the average position of the first target position of the target for each of the targets included in the first target group selected by the first target group selection unit 231. The first average position is calculated. Specifically, as shown in FIG. 10, the first target group average position calculation unit 232 sets the first target position of the target (white in FIG. 10) for each of the targets included in the first target group. The first average position (the position of the black circle in FIG. 10), which is the average position of the blank circle), is calculated. The first average position may be a simple arithmetic average, or may be a weighted average when reliability is given to the result of target detection. The set of the calculated first average positions is defined as the target average position group 1. As shown in FIG. 11, it can be seen that, as compared with FIG. 8, the target having a large position error is excluded, the first average position is close to the true value, and the error is small.

The second target group position correction unit 233 uses the first average position of each of the targets included in the first target group and the second target of each of the targets detected by using the sensor in the same running. It is included in the result of coordinate conversion of each second target position of the target by associating with the position and the first target group associated with each second target position of the target. The first coordinate transformation matrix that minimizes the positional deviation from each of the first average positions of the target is calculated, and based on the first coordinate transformation matrix, the target detected by the sensor in the same run. Each of the second target positions calculated for each is corrected.

Specifically, first, as shown in FIG. 12, the second target group position correction unit 233 corresponds to the target average position group 1 calculated by the first target group average position calculation unit 232. The second target position group, which is the second target position of each of the targets included in the first target group, is associated with each other. Here, the target average position group 1 has a small error from the true value. FIG. 13 shows an example of the target position group 2 which is a set of the second target positions of each target calculated in the above-mentioned four runs (runs 1 to 4). FIG. 14 shows a superposition of the target position group 2 of each run in FIG.

Next, the second target group position correction unit 233 transforms the coordinates of each second target position of the target based on the result of the association, and the second target of each of the targets. The first coordinate transformation matrix that minimizes the positional deviation of each of the first average positions of the targets included in the first target group associated with the position is calculated. The first coordinate transformation matrix is composed of rotation and translation. The second target group position correction unit 233 calculates the corrected second target position group corrected by converting the second target position group from the calculated first coordinate conversion matrix. If a certain target is not detected by the target detection unit 212, the second target group position correction unit 233 excludes the target from the alignment. Then, the second target group position correction unit 233 passes the corrected second target group position group to the second target group average position calculation unit 234.

The second target group average position calculation unit 234 averages the second target positions of the target corrected by the second target group position correction unit 233 for each of the targets included in the first target group. The second average position, which is the position, is calculated. Specifically, the second target group average position calculation unit 234 has the first target group selection unit 231 and the second target group selection unit 231 with respect to the corrected second target position group calculated by the second target group position correction unit 233. 1 Target group As with the average position calculation unit 232, a second target group is selected, which is a set of targets whose position variation is equal to or less than a predetermined first threshold (a threshold different from the first threshold may be used). Then, for each of the targets included in the second target group, the second average position, which is the average position of the corrected second target positions of the target, is calculated (FIG. 15). FIG. 16 shows a comparison between the second average position of each target and the true value. As shown in FIG. 16, the result that the second average position of each target is substantially equal to the true value of each target is obtained.

The output unit 240 outputs the second average position of the target as the final position of the target for each of the targets included in the first target group.

<Operation of Positional Misalignment Correction System 1 According to First Embodiment of the Present Invention>
Next, the target position collection processing routine of the position shift correction system 1 of the present embodiment will be described with reference to FIG. The target position collection processing routine is repeatedly executed every time the vehicle travels.

First, in step S100, the image acquisition unit 211 acquires an image taken by the camera 10.

In step S110, the target detection unit 212 acquires the position of the target in the vehicle coordinate system around the vehicle detected by using the image acquired in step S100.

In step S120, the first acquisition unit 213 acquires the first vehicle position of the vehicle detected by the first positioning device 11 mounted on the vehicle.

In step S130, the first target position calculation unit 214 indicates the position of the target acquired in step S110 in accordance with the position of the first vehicle acquired in step S120 and the time when the position of the first vehicle is positioned. Based on the above, the first target position, which is the position of the target in the world coordinate system, is calculated.

In step S140, the first target position calculation unit 214 stores the first target position calculated in step S130 in the target position storage unit 220.

In step S150, the second acquisition unit 215 acquires the second vehicle position of the vehicle by the second positioning device 12 mounted on the vehicle.

In step S160, the second target position calculation unit 216 uses the second vehicle position acquired in step S150 and the position of the target acquired in step S110 corresponding to the time when the second vehicle position is positioned. Based on the above, the second target position, which is the position of the target in the world coordinate system, is calculated.

In step S170, the second target position calculation unit 216 stores the second target position calculated in step S160 in the target position storage unit 220.

In step S180, the target position collecting unit 210 determines whether or not the running has been completed.

When the running is not completed (NO in step S180), in step S190, the process waits for a predetermined time, returns to step S100, and repeats the processes of steps S100 to S170 again.

On the other hand, when the running is completed (YES in step S180), the target position collection processing routine is terminated. The first target position stored in the target position storage unit 220 in step S140 is stored in the target position storage unit 220 as the first target position in the same travel. Further, the second target position stored in the target position storage unit 220 in step S170 is stored in the target position storage unit 220 as the second target position in the same travel.

Next, the position shift correction processing routine of the position shift correction system 1 of the present embodiment will be described with reference to FIG. The misalignment correction processing routine is executed every time the target position collection processing routine is executed a predetermined number of times.

First, in step S200, the first target group selection unit 231 sets the target detection unit 212 for each of the plurality of targets detected by the target position collection processing routine. For each of the plurality of targets detected by the above, the first target with respect to the same target is based on the first target position of the target, the size of the target, etc. calculated in step S140 in the plurality of runs. A target group is generated, the variation of the first target position of the target in the first target group is obtained, and the variation of the plurality of targets is equal to or less than the predetermined first threshold value. Select a group.

In step S210, the first target group average position calculation unit 232 sets the average position of the first target position of the target for each of the targets included in the first target group selected in step S200. Calculate a certain first average position.

In step S220, the second target group position correction unit 233 uses the sensor to detect the first average position of the targets included in the first target group in the same travel as the first average position of the target. The result of coordinate conversion of each second target position of the target by associating with the two target positions and the first target group associated with each second target position of the target. The first coordinate transformation matrix that minimizes the positional deviation from each of the first average positions of the target included in is calculated, and the said one detected by using a sensor in the same running based on the first coordinate transformation matrix. Each of the second target positions calculated for each of the targets is corrected.

In step S230, the second target group average position calculation unit 234 uses the average position of the second target position of the target corrected in step S220 for each of the targets included in the first target group. Calculate a second average position.

In step S240, the output unit 240 outputs the second average position of the target as the final position of the target for each of the targets included in the first target group.

As described above, according to the position deviation correction device according to the first embodiment of the present invention, the position of the target around the vehicle detected by using the sensor mounted on the vehicle in the vehicle coordinate system , The first target position, which is the position of the target in the world coordinate system, is calculated based on the first vehicle position, which is the position of the vehicle's world coordinate system, and the position of the target and the position of the vehicle A plurality of detected second target positions are calculated based on the second vehicle position, which is the position of the vehicle in the world coordinate system detected based on the change, and the position of the target in the world coordinate system. For each of the targets, a first target group for the same target is generated based on the first target position of the target calculated in a plurality of runs, the size of the target, and the like, and the first target group is generated. The variation of the first target position of the target in the target group is obtained, the first target group of the target whose variation is equal to or less than the predetermined first threshold is selected, and the object included in the first target group. Based on the first average position, which is the average position of the first target position of the target calculated for each of the targets, the second calculated for each of the targets detected by using the sensor in the same running. By correcting each of the two target positions and calculating the second average position, which is the average position of the corrected second target position of the target, for each of the targets included in the first target group. The absolute position of the target can be estimated at low cost and with high accuracy.

<Structure of Positional Misalignment Correction System 2 According to Second Embodiment of the Present Invention>
The configuration of the position shift correction system 2 according to the second embodiment of the present invention will be described with reference to FIG. The same configuration as that of the misalignment correction system 1 according to the first embodiment is designated by the same reference numerals, and detailed description thereof will be omitted.

In the position deviation correction system 1 according to the first embodiment, the first target position and the second target position are collected by traveling in the traveling environment a plurality of times by one vehicle. In the misalignment correction system 2 according to the second embodiment, the first target position and the second target position are collected by traveling in the driving environment by a plurality of vehicles, and the server is positioned via the network. A configuration for performing deviation correction will be described as an example.

The misalignment correction system 2 includes a plurality of on-board units 30 equipped with a camera 10, a first positioning device 11, and a second positioning device 12, a misalignment correction device 40, a base station 50, and a network 60. It is configured.

The on-board unit 30 includes a CPU, a RAM, and a ROM that stores a program for executing a target position acquisition processing routine described later, and is functionally configured as shown below.

As shown in FIG. 20, the vehicle-mounted device 30 according to the embodiment of the present invention includes a target position collecting unit 210 and a transmitting unit 300.

In the present embodiment, the first target position calculation unit 214 and the second target position calculation unit 216 pass the calculated first target position and second target position to the transmission unit 300.

The transmission unit 300 transmits the first target position and the second target position to the position shift correction device 40 via the base station 50 and the network 60. The base station 50 is a radio base station connected to the network 60, and the network 60 is a public network.

The misalignment correction device 40 includes a CPU, a RAM, and a ROM that stores a program for executing a misalignment correction processing routine described later, and is functionally configured as shown below.

As shown in FIG. 21, the position shift correction device 40 according to the embodiment of the present invention includes a receiving unit 400, a first target group selection unit 231, a first target group average position calculation unit 232, and a first target group average position calculation unit 232. The two target group position correction unit 433, the second target group average position calculation unit 234, and the output unit 240 are provided.

The receiving unit 400 receives the first target position and the second target position, and holds them using, for example, a buffer (not shown). When the position shift correction process is started, the receiving unit 400 passes all the held first target position and second target position to the first target group selection unit 231.

The second target group position correction unit 433 first, like the second target group position correction unit 233, first detects the first average position of each of the targets included in the first target group in the same running. The result of coordinate-transforming each second target position of the target by associating it with the second target position of each of the targets detected using the above, and each second object of the target. A first coordinate transformation matrix that minimizes the positional deviation of each of the first average positions of the targets included in the first target group associated with the target position is calculated, and based on the first coordinate transformation matrix, Each of the second target positions calculated for each of the targets detected by using the sensor in the same running is coordinate-transformed.

Next, the second target group position correction unit 433 performs coordinate conversion of the second target position of the target based on the calculated first coordinate conversion matrix, and the second target of the target. A target whose residual is larger than a predetermined second threshold value, which is a positional deviation of the target included in the first target group associated with the position from the first average position, is used by the camera 10 in the same running. Exclude from the same running object group consisting of detected objects. Specifically, as shown in FIG. 21, the second target group position correction unit 433 performs position correction by the first coordinate transformation matrix to perform a target whose residual is larger than the second threshold value in the same running target group. Exclude from.

Next, the second target group position correction unit 433 runs in the same manner, excluding the first average position of each of the targets included in the first target group and the target whose residual is larger than the second threshold. The second coordinate conversion matrix that minimizes the misalignment is calculated by associating with the second target position of each of the targets included in the target group, and the same running object is calculated based on the second coordinate conversion matrix. A corrected second target position group is calculated by correcting each of the second target positions calculated for each of the targets included in the target group. Compared with the position correction by the first coordinate transformation matrix, the absolute position accuracy of the target average position group 1 is improved because the targets whose residuals are larger than the second threshold are excluded from the same running object group. .. Therefore, the absolute position of the target average position group 2 is also improved. Then, the second target group position correction unit 433 passes the corrected second target group position group to the second target group average position calculation unit 234.

<Operation of Positional Misalignment Correction System 2 According to Second Embodiment of the Present Invention>
FIG. 23 is a flowchart showing a target position collection processing routine according to the second embodiment of the present invention. The same processing as the target position collection processing routine according to the first embodiment is designated by the same reference numerals, and detailed description thereof will be omitted. The target position collection processing routine is repeatedly executed in each of the vehicles each time the vehicle travels.

In step S370, the transmission unit 300 transmits the first target position and the second target position to the position shift correction device 40 via the base station 50 and the network 60.

FIG. 24 is a flowchart showing a misalignment correction processing routine according to the second embodiment of the present invention. The same processing as that of the misalignment correction processing routine according to the first embodiment is designated by the same reference numerals, and detailed description thereof will be omitted.

In step S470, the receiving unit 400 receives the first target position and the second target position.

In step S480, the receiving unit 400 determines whether or not to start the misalignment correction.

If the misalignment correction is not started, the process returns to step S470, and the receiving unit 400 repeats receiving the first target position and the second target position. On the other hand, when starting the misalignment correction, the process proceeds to step S200.

In step S510, the second target group position correction unit 433 performs coordinate conversion of the second target position of the target based on the first coordinate conversion matrix, and the second target position of the target. A target whose residual is larger than a predetermined second threshold value, which is a positional deviation of the target included in the first target group associated with the first average position, is detected by using a sensor in the same running. Exclude from the same running object group consisting of the coordinates.

In step S520, the second target group position correction unit 433 is included in the first average position of each of the targets included in the first target group and in the same traveling target group excluded in step S510. An object included in the same running object group is calculated by associating each second target position of the target with the position of the second target, calculating the second coordinate transformation matrix that minimizes the misalignment, and based on the second coordinate transformation matrix. Each of the second target positions calculated for each of the marks is corrected.

As described above, according to the position shift correction device according to the second embodiment of the present invention, the result of coordinate conversion of the second target position of the target based on the first coordinate transformation matrix and the result of coordinate conversion. A target whose residual is larger than a predetermined second threshold value, which is a positional deviation of the target from the first average position of the target included in the first target group associated with the second target position of the target. Exclude from the target detected by the sensor in the same run, the first average position of the target included in the first target group and the target detected by the sensor in the same run The target is associated with each second target position, the second coordinate transformation matrix that minimizes the misalignment is calculated, and the target detected at the same time using the sensor based on the second coordinate transformation matrix. By correcting each of the second target positions calculated for each of the above, the position of the target can be estimated more accurately.

The present invention is not limited to the above-described embodiment, and various modifications and applications are possible without departing from the gist of the present invention.

For example, in the above-described embodiment, the vehicle has been described as an example, but the present invention is not limited to this, and other moving bodies may be used. Further, a vehicle and a moving body other than the vehicle may be used.

Further, when the position of the target is detected by a plurality of vehicles, the position of the target may be associated in advance with the one whose density of the target is not high.

Further, in the above-described embodiment, the target detection unit 212 detects a target in the traveling environment using an image taken by the camera 10, but the present invention is not limited to this. For example, another sensor such as Lidar may be used to detect a target in the driving environment.

Further, although described as an embodiment in which the program is pre-installed in the specification of the present application, it is also possible to provide the program by storing it in a computer-readable recording medium.

1, 2 Positioning correction system 10 Camera 11 First positioning device 12 Second positioning device 20 Positioning correction device 30 On-board unit 40 Positioning correction device 50 Base station 60 Network 210 Target position collection unit 211 Image acquisition unit 212 Target Detection unit 213 1st acquisition unit 214 1st target position calculation unit 215 2nd acquisition unit 216 2nd target position calculation unit 220 Target position storage unit 230 Position deviation correction unit 231 1st target group selection unit 232 1st Target group average position calculation unit 233 Second target group position correction unit 234 Second target group average position calculation unit 240 Output unit 300 Transmission unit 400 Reception unit 433 Second target group position correction unit

Claims (4)

A first acquisition unit that acquires the position of the first vehicle, which is the position of the world coordinate system of the vehicle detected by the first positioning device mounted on the vehicle.
A second acquisition unit that acquires a second vehicle position, which is a position in the world coordinate system of the vehicle, detected based on a change in the position of the vehicle by a second positioning device mounted on the vehicle.
A target detection unit that acquires the position of a target around the vehicle in the vehicle coordinate system detected by using a sensor mounted on the vehicle.
World coordinates of the target based on the position of the first vehicle acquired by the first acquisition unit and the position of the target acquired by the target detection unit in response to the detection of the first vehicle position. The first target position calculation unit that calculates the first target position, which is the position in the system,
World coordinates of the target based on the position of the second vehicle acquired by the second acquisition unit and the position of the target acquired by the target detection unit in response to the detection of the second vehicle position. The second target position calculation unit that calculates the second target position, which is the position in the system,
For each of the plurality of targets detected by the target detection unit, the same object is obtained based on the first target position of the target calculated by the first target position calculation unit in a plurality of runs. A first target group with respect to a target is generated, a variation in the position of the first target of the target in the first target group is obtained, and the variation among the plurality of targets is equal to or less than a predetermined first threshold value. The first target group selection unit that selects the first target group of the target that is
For each of the targets included in the first target group selected by the first target group selection unit, the first average position, which is the average position of the first target position of the target, is calculated. 1 target group average position calculation unit and
The second target position calculated for each of the targets detected by using the sensor in the same running based on the first average position of each of the targets included in the first target group. The second target group position correction unit that corrects each of
For each of the targets included in the first target group, the second average position, which is the average position of the second target position of the target corrected by the second target group position correction unit, is calculated. 2nd target group average position calculation unit and
Positional deviation correction system including. The second target group position correction unit is the first of the targets detected by using the sensor in the same running as the first average position of the targets included in the first target group. The result of coordinate conversion of the second target position of each of the targets by associating with the two target positions and the first target associated with the second target position of each of the targets. A first coordinate transformation matrix that minimizes the positional deviation of the targets included in the target group from each of the first average positions is calculated, and the sensor is used in the same run based on the first coordinate transformation matrix. The misalignment correction system according to claim 1, wherein each of the second target positions calculated for each of the targets detected using the device is corrected. The second target group position correction unit calculates the first coordinate conversion matrix, performs coordinate conversion of the second target position of the target based on the first coordinate conversion matrix, and the target. A target whose residual is larger than a predetermined second threshold value, which is a positional deviation of the target included in the first target group associated with the second target position from the first average position, is the same. Exclude from the target detected by using the sensor in the running of
The first average position of the target included in the first target group is associated with the second target position of each of the targets detected by using the sensor in the same running. The second coordinate transformation matrix that minimizes the misalignment is calculated, and based on the second coordinate transformation matrix, the second target position calculated for each of the targets simultaneously detected by the sensor. The misalignment correction system according to claim 2, wherein each of the above is corrected. Computer,
The first acquisition unit, which acquires the position of the first vehicle, which is the position of the world coordinate system of the vehicle detected by the first positioning device mounted on the vehicle.
A second acquisition unit that acquires a second vehicle position, which is a position in the world coordinate system of the vehicle, detected by a second positioning device mounted on the vehicle based on a change in the position of the vehicle.
A target detection unit that acquires the position of a target around the vehicle in the vehicle coordinate system detected by using a sensor mounted on the vehicle.
World coordinates of the target based on the position of the first vehicle acquired by the first acquisition unit and the position of the target acquired by the target detection unit in response to the detection of the first vehicle position. The first target position calculation unit, which calculates the first target position, which is the position in the system,
World coordinates of the target based on the position of the second vehicle acquired by the second acquisition unit and the position of the target acquired by the target detection unit in response to the detection of the second vehicle position. The second target position calculation unit, which calculates the second target position, which is the position in the system,
For each of the plurality of targets detected by the target detection unit, the same object is obtained based on the first target position of the target calculated by the first target position calculation unit in a plurality of runs. A first target group with respect to a target is generated, a variation in the position of the first target of the target in the first target group is obtained, and the variation among the plurality of targets is equal to or less than a predetermined first threshold value. The first target group selection unit for selecting the first target group of the target, which is
For each of the targets included in the first target group selected by the first target group selection unit, the first average position, which is the average position of the first target position of the target, is calculated. 1 target group average position calculation unit,
The second target position calculated for each of the targets detected by the sensor in the same running based on the first average position of each of the targets included in the first target group. The second target group position correction unit that corrects each of the above, and the second target group that is corrected by the second target group position correction unit for each of the targets included in the first target group. A program for functioning as a second target group average position calculation unit that calculates the second average position, which is the average position of the target positions.